无人驾驶-控制-LQR（运动学）

无人驾驶-控制-LQR（运动学）

一、车辆建模

二、参考轨迹

• 利用泰勒展开，进行线性化：
  
• 离散化处理
  

对离散后的式子进行处理，得到X(k+1)的表达式

综上：

由于系统矩阵A和输入矩阵B的元素随时间变化，所以上述系统是一个线性时变系统。

三、LQR控制律设计

(1)设计代价函数
其中，Q、R就是需要设计的半正定矩阵和正定矩阵。

为什么这么设计代价函数？

代价函数J 需要达到最小值，那么在t 趋近于无穷时，状态向量x(t)肯定趋近于0，即是达到了系统稳态；同理，t 趋近于无穷时，控制向量u(t)也会趋近于0，意味着，随着时间的推移，需要对系统施加的控制量会越来越小，意味着使用最小的控制量使得系统达到了最终控制目标。

（2）Q，R矩阵的意义

一般来说，选取Q、R矩阵的时候，为了方便观察各个系统状态量而选取对角阵，增加Q的一个值，意味着这个值作用的系统状态量，将以更快的速度衰减到0，这时候，举个栗子还是很必要的，比如，Q ​
选取较大的值，会让x 很快的衰减到0；另外一方面，加大R的值，会使得对应的控制量减小，控制器执行更少的动作，意味着系统的状态衰减将变慢。所以，Q ，R的选取，要综合看具体的实际应用场景来调节

四、公式推导

4.1 将u带入cost函数

4.2 假定存在矩阵P

4.3 使能量函数值最小

4.4 对P进行微分展开

整理（8）式，可得到如下：

4.5 获取A,B,Q,P,K,R的等式

【补充公式（3）源头】

此步骤可以表示出，LQR控制的控制量是由状态量获取的，且控制量 u = - Kx


将（3）代入到（9）

4.6 配凑K与R,B,P关系

4.7 由A,B,Q,R已知量求得P，再求出K

（1）由A,B,Q,R已知量求得P

（2）由P求得K

（3）由K求得u控制量

五、总结

六、黎卡提方程求解

黎卡提方程求解.

参考链接

1. https://blog.csdn.net/qq_24649627/article/details/104690279
2. https://blog.csdn.net/zhouyy858/article/details/107606500