一、LQR控制
假设有一个线性系统能用状态向量的形式表示成:
初始条件是x(0),并且假设这个系统的所有状态变量都是可测量到的。在介绍LQR前,先简单回顾一下现代控制理论中最基本的控制器--全状态反馈控制。全状态反馈控制系统图形如下:
我们要设计一个状态反馈控制器:
使得闭环系统能够满足我们期望的性能。我们把这种控制代入之前的系统状态方程得到:
由现代控制理论我们知道开环传递函数的极点就是系统矩阵A的特征值,现在变成了如上的闭环形式,状态变换矩阵A变成了(A-BK)。因此通过配置反馈矩阵K,可以使得闭环系统的极点达到我们期望的状态。注意,这种控制器的设计与输出矩阵C,D没有关系。那么,什么样的极点会使得系统性能很棒呢?并且,当系统变量很多的时候,即使设计好了极点,矩阵K也不好计算。于是,LQR为我们设计最优控制器提供了一种思路。在设计LQR控制器前,我们得设计一个能量函数,最优的控制轨迹应该使得该能量函数最小。一般选取如下形式的能量函数。
其中Q是你自己设计的半正定矩阵,R为正定矩阵
可是,为什么能量函数(或称系统的目标函数)得设计成这个样子呢?首先假设状态向量x(t)是1维的,那么xTQx其实就是一个平方项 Qx^2 >= 0,uTRu同理。能量函数J要最小,那么状态向量x(t),u(t)都得小。J最小,那肯定是个有界的函数,我们能推断当t趋于无穷时,状态向量x(t)将趋于0,这也保证了闭环系统的稳定性。那输入u(t)要小是什么意思呢?它意味着我们用最小的控制代价得到最优的控制。譬如控制电机,输入PWM小,将节省能量。
再来看看矩阵Q,R的选取,一般来说,Q值选得大意味着,要使得J小,那x(t)需要更小,也就是意味着闭环系统的矩阵(A-BK)的特征值处于S平面左边更远的地方,这样状态x(t)就以更快的速度衰减到0。另一方面,大的R表示更加关注输入变量u(t),u(t)的减小,意味着状态衰减将变慢。同时,Q为半正定矩阵意味着他的特征值非负,R为正定矩阵意味着它的特征值为正数。如果你选择Q,R都是对角矩阵的话,那么Q的对角元素为正数,允许出现几个0。R的对角元素只能是正数。
下面来聊聊Q、R值的选取,一般来说,选取Q、R矩阵的时候,为了方便观察各个系统状态量而选取对角阵,增加Q的一个值,意味着这个值作用的系统状态量,将以更快的速度衰减到0,这时候,举个栗子还是很必要的,比如, Q选取较大的值,会让 x很快的衰减到0;另外一方面,加大R的值,会使得对应的控制量减小,控制器执行更少的动作,意味着系统的状态衰减将变慢。所以, Q 、R的选取,要综合看具体的实际应用场景来调节,俗话说,鱼和熊掌不可兼得,这个矛盾就像做的轨迹跟踪MPC中的设置不同的权重一样,期望车辆后轴心cte值越小,那么到了转弯处必须要很大幅度的刹车,以及打方向盘,速度以及yaw都会大幅度变化,而如果期望执行器动作越小,那么就无法保证cte的准确度一样。
注意LQR调节器是将状态调节到0,这与轨迹跟踪不同,轨迹跟踪是使得系统误差为0。知道了背景后,那如何设计反馈矩阵K使得能量函数J最小呢?很多地方都是从最大值原理,Hamilton函数推导出来。这里用另外一种更容易接受的方式推导。
将u = -Kx 代入之前的能量函数得到:
为了找到K,我们先不防假设存在一个常量矩阵P使得:
代入得:
注意,我们已经假设闭环系统是稳定的,也就是t趋于无穷时,x(t)趋于0。
现在把上式左边的微分展开,并把状态变量x的微分替代得到:
这个式子要始终成立的话,括号里的项必须恒等于0.
这是一个关于K的二次型等式,当然这个二次型是我们不愿看到的,因为计算复杂。现在只要这个等式成立,我们何必不选择K使得两个二次项正好约掉了呢?这样既符合了要求,又简化了计算。取:
代入上式得:
K的二次项没有了,可K的取值和P有关,而P是我们假设的一个量,P只要使得的上式成立就行了。而上式在现代控制理论中极其重要,它就是有名的Riccati 方程。
现在回过头总结下LQR控制器是怎么计算反馈矩阵K的:
1.选择参数矩阵Q,R
2.求解Riccati 方程得到矩阵P
3.计算
再看看LQR的结构图:
利用迭代法求解离散时间的黎卡提方程。根据迭代法求解P,P为Riccati方程的解,然而用LQR需要计算K,再将K算出。离散时间黎卡提方程:
伪代码表示为:
P=Q
while(err > tolerance && iteration_num < max_num_iteration)
{
iteration_num++;
P_1 = Q + A^T*P*A - A^T*P*B(R+B^T*P*B)^(-1)*B^T*P*A;
err = fabs(P_1 - P);
P = P_1;
}
if(iteration_num < max_num_iteration)
{
K = -(R + B^T * P *B)^(-1)*B^T*P*A
}
参考链接:
(3)迭代法求黎卡提(Riccati)方程的解_迭代法解黎卡提方程-CSDN博客
(4)【数理知识】Riccati 黎卡提 system_黎卡提方程-CSDN博客
(5)Apollo代码学习(五)—横纵向控制_apollo纵向控制 论文-CSDN博客