1.算法思想

对一个受控系统，从一类允许的控制方案中找出一个最优的方案，使系统由初始状态转移到目标状态的同时某个特定的性能指标为最优。

在运动方程和允许控制范围的约束下，对以控制函数和运动状态为变量的性能指标函数求极值。

2.算法原理

车辆运动学方程

参考轨迹时间/距离最近点𝑥𝑟,𝑦𝑟,𝜑𝑟,𝛿𝑟泰勒展开线性化 + 离散化处理

性能指标函数

最优控制律

 式中，P为黎卡提方程的解，可以循环迭代求解（补充：迭代初值取Q，可同时设置最大迭代次数）。

3.算法调参

1.状态加权系数矩阵Q影响状态偏差累计大小，半正定，常取对角阵；

2.控制加权系数矩阵R影响控制能量损耗，正定且对称，常取对角阵；

3.Q阵横摆角误差系数为横向位置误差系数的阻尼项。

4.个人理解

1.优点：有显示解，工程应用简单；

2.稳定性分析：被控对象状态完全可观可控、Q可分解为Q=𝑆𝑇S，𝐴,𝑆可观，则最优控制存在，闭环系统渐进稳定；

3.纵向位置误差为快变量，横向位置误差为慢变量，故Q阵中Q(1,1)、Q(2,2)的值应分别设计，并随车辆航向角自适应调整实现全向运动的控制品质最优化。

5.实现

https://github.com/mulanchaoxiongda/TrackingAlgorithm/tree/main/LQR_FeedforwardControl

6.视频分享

…

7.相关论文

…