1 前言
在前面的系列博客文章中为读者阐述了很多规划相关的知识(可参考下面专栏),本篇博客带领读者朋友们了解控制相关的知识,后续仍会撰写规控相关文档。
在控制理论的发展过程中,人们逐渐认识到对于线性动态系统的控制,可以通过优化某种性能指标来达到更好的控制效果。而LQR控制就是基于这种思想发展而来的。它通过将系统状态与控制信号之间的关系表示成一种二次型,然后通过最小化这个二次型来求解最优的控制策略。
目前的工业领域三大控制器占据着主要位置:PID,LQR以及MPC,本篇博客为读者介绍LQR控制器。
2 LQR建模求解
LQR(Linear quadratic regulator,线性二次型调节器)是一种基于状态反馈的最优控制策略,其核心在于设计一个状态反馈控制器,使得系统的性能指标达到最优。下面将对LQR控制器进行详细介绍:
2.1 原理与步骤
- 确定状态方程模型:首先需要确定一个描述系统状态的动力学模型,该模型通常以状态空间的形式给出,用于描述系统在不同状态下的动态行为。
- 线性化处理:对状态方程进行线性化处理,将其转化为线性系统模型。这是为了简化问题,使得控制器设计更加便捷。
- 定义目标函数:目标函数通常是系统状态和控制输入的二次型函数,用于评估控制性能的好坏。这个函数的选择会直接影响到控制器的设计效果。
- 优化目标函数:通过设计状态反馈控制器,使得目标函数取最小值。这个过程中,需要求解一个最优控制问题,通常涉及到Riccati方程的求解。
2.2 详细内容
2.2.1 一般推导
对于一个闭环系统,状态方程为:
其中 为状态量, 为控制量, 分别为状态矩阵,控制矩阵。假设,那么上式可以整理为一个新的状态方程:
为了能够选出最优化的特征值,引入了一个代价函数,考虑两个方面的内容:
- 希望系统达到稳定状态使得偏差最小:惩罚状态量
- 希望控制量较小的情况下也能达到稳定状态:惩罚控制量
因此代价函数可以写成如下形式:
其中, 为状态量, 为控制量, 分别为状态权重矩阵,控制权重矩阵。在量产中的调参也主要集中在矩阵上。
基于上面两个方面的考虑,这里希望 为半正定矩阵来保证;希望 为正定矩阵来保证,该项表示控制所付出的代价。
将 代入到代价函数种可得,
为了得到 矩阵,假设存在一个常量矩阵 使得,
将上式左边微分展开得到,并代入 ,
由二次型的性质可得,如果保证上式为0,则括号内的值必须为0,即:
上式中含有矩阵的二次型,不利于计算,因此取 消掉二次项得,
从上面的公式中,可以看到形式正好是控制理论中的Riccati方程。
2.2.2 离散域推导
网上看到很多文章的推导结果不太一致,在此处说明,另一种形式的Riccati方程是基于离散时域的推导结果:
记 时刻代价函数为 ,价值函数为 ,其中
这里同样假设存在一个矩阵 ()使得下式成立,
那么基于离散状态方程 , 可以写为:
对于价值函数 取极小值,即
这里插一个矩阵求导的知识(注意区分:分子布局还是分母布局)
整理可得,
由于 都是对称矩阵,可得,
由 可得,
将 代入到 中可得,
最后得到的离散Riccati方程如下:
在控制理论中,Riccati方程扮演着至关重要的角色,它通常用于描述线性时不变系统的最优控制问题。通过求解Riccati方程,可以找到使得系统状态达到最优的反馈控制律。对于离散时间系统和连续时间系统,Riccati方程的形式和求解方法会有所不同。
求解连续时间Riccati方程通常涉及数值方法,因为解析解可能不存在或难以找到。常用的数值方法包括迭代法和直接法。迭代法如牛顿法或不动点迭代法,通过迭代更新矩阵 P的值,直到满足收敛条件。直接法则试图通过矩阵分解或变换直接求解方程。
此部分的代码主要如下:
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>
#include <iostream>using Eigen::Matrix;
using Eigen::Dynamic;typedef Matrix<double, Dynamic, Dynamic> Mtx;/** @brief Computes the LQR gain matrix (usually denoted K) for a discrete time* infinite horizon problem.** @param A State matrix of the underlying system* @param B Input matrix of the underlying system* @param Q Weight matrix penalizing the state* @param R Weight matrix penalizing the controls* @param N Weight matrix penalizing state / control pairs* @param K Pointer to the generated matrix (has to be a double/dynamic size* matrix!)* @param eps Delta between iterations that determines when convergence is* reached*/
bool SolveLQRMatrix(const Mtx &A, const Mtx &B, const Mtx &Q, const Mtx &R,const Mtx &N, Mtx *K, double eps = 1e-15) {// check if dimensions are compatibleif (A.rows() != A.cols() || B.rows() != A.rows() || Q.rows() != Q.cols() ||Q.rows() != A.rows() || R.rows() != R.cols() || R.rows() != B.cols() ||N.rows() != A.rows() || N.cols() != B.cols()) {std::cout << "One or more matrices have incompatible dimensions. Aborting."<< std::endl;return false;}// precompute as much as possibleMtx B_T = B.transpose();Mtx Acal = A - B * R.inverse() * N.transpose();Mtx Acal_T = Acal.transpose();Mtx Qcal = Q - N * R.inverse() * N.transpose();// initialize P with QMtx P = Q;// iterate until P convergesunsigned int numIterations = 0;Mtx Pold = P;while (true) {numIterations++;// compute new PP = Acal_T * P * Acal -Acal_T * P * B * (R + B_T * P * B).inverse() * B_T * P * Acal + Qcal;// update deltaMtx delta = P - Pold;if (fabs(delta.maxCoeff()) < eps) {std::cout << "Number of iterations until convergence: " << numIterations<< std::endl;break;}Pold = P;}// compute K from P*K = (R + B_T * P * B).inverse() * (B_T * P * A + N.transpose());return true;
}
3 车辆LQR控制
本小节以汽车运动学模型为例,车辆的运动模型如下图所示,本小节理论部分主要参考龚建伟老师著作《无人驾驶车辆模型预测控制》,基于汽车动力学模型的推导可参考Rajamani的著作《Vehicle Dynamics And Control》。
3.1 运动学模型
可得车辆的运动学模型为:
可将整个系统看作一个输入为 和 的控制系统。其一般形式为:
在自动驾驶轨迹跟踪中,通常以跟踪偏差作为输入,因此可得,
为了能够将该模型应用于控制器的设计,最终进行离散化处理,最终可以写成如下形式:
建立运动学模型的代码如下(只保留重要部分):
void Model::GetModel(float curvature, Eigen::Matrix<double, 3, 1> state_actual){//target delta is variable because of curve path.float target_delta = atan(wheel_base*curvature); //dphi/dt = V*tan(delta)/L; w = V*tan(delta)/L/*****************************************************/// AMatrix_->A(0, 0) = 1;Matri>A(1, 0) = 0;Matrix_->A(1, 1) = 1;Matrix_->Ax_->A(0, 1) = 0;Matrix_->A(0, 2) = -target_velocity*sin(state_actual(2,0))*dt_;Matrix_-(1, 2) = target_velocity*cos(state_actual(2,0))*dt_;Matrix_->A(2, 0) = 0;Matrix_->A(2, 1) = 0;Matrix_->A(2, 2) = 1;/*****************************************************/// BMatrix_->B(0, 0) = cos(state_actual(2,0))*dt_;Matrix_->B(0, 1) = 0;Matrix_->B(1, 0) = sin(state_actual(2,0))*dt_;Matrix_->B(1, 1) = 0;Matrix_->B(2, 0) = tan(target_delta)*dt_/wheel_base;Matrix_->B(2, 1) = target_velocity*dt_/(wheel_base*(cos(target_delta)*cos(target_delta)));
}
3.2 求解
再结合上面一节LQR求解代码如下(非完整):
void LQRControl::LQRController(){//near pointint index = FindMatchedPoint(actual_pos.x, actual_pos.y);x_ref_(0, 0) = planning_path[index].position.x;x_ref_(1, 0) = planning_path[index].position.y;x_ref_(2, 0) = planning_path[index].position.yaw;x_act_(0, 0) = actual_pos.x;x_act_(1, 0) = actual_pos.y;x_act_(2, 0) = actual_pos.yaw;/***********compute curvature of target point********************************/curvature = ComputeCurvature(index);// get model matrix A and Bmodel_.GetModel(curvature, x_act_);matrix_c.A = model_.Matrix_->A;matrix_c.B = model_.Matrix_->B;// Q(3x3): positive semi-definite matrix, penalizing the statematrix_c.Q(0, 0) = 1;matrix_c.Q(0, 0) = 10;matrix_c.Q(0, 0) = 1;// R(2x2): positive definite matrix, penalizing the controlsmatrix_c.R(0, 0) = 100;matrix_c.R(1, 1) = 100;// N(3x2): penalizing the state/control pairsmatrix_c.N = Eigen::Matrix<double, 3, 2>::Zero();/****************************************************************************/// Solve the Riccati Equationif (!SolveLQRMatrix(matrix_c.A, matrix_c.B, matrix_c.Q, matrix_c.R, matrix_c.N, &matrix_k)){std::cout << "Compute matrix K failed!" << std::endl;}else{std::cout << matrix_k << std::endl;}
}
4 特点与应用
(1) 特点
- 最优性:LQR控制器通过最小化目标函数,使得系统性能达到最优。这种最优性是基于给定的性能指标进行衡量的,因此可以根据实际需求进行灵活调整。
- 适应性:LQR控制器可以适应不同的系统模型和性能指标,通过调整目标函数和控制器参数,可以实现对不同系统的有效控制。
- 稳定性:通过设计合适的控制器参数,LQR控制器可以保证系统的稳定性,避免出现过大的状态波动或控制误差。
(2)应用领域
LQR控制器广泛应用于控制工程、机器人学、飞行器控制以及其他控制领域。在这些领域中,LQR控制器能够有效地处理线性系统的控制问题,实现精确、稳定的控制效果。
5 总结
综上所述,LQR控制器是一种基于状态反馈的最优控制策略,具有最优性、适应性和稳定性等特点。它在多个领域都有广泛的应用,并且与其他控制器相比具有优势。在实际应用中,可以根据具体需求选择和设计合适的LQR控制器,以实现精确、稳定的控制效果。