7.LQR算法解析及实现

秋日的动物园

已于 2023-02-03 18:53:56 修改

阅读量2.3k

点赞数 3

分类专栏： Apollo星火计划PNC专项笔记文章标签：自动驾驶

于 2023-02-03 16:48:10 首次发布

本文链接：https://blog.csdn.net/weixin_56492465/article/details/128871314

版权

Apollo星火计划PNC专项笔记专栏收录该内容

7 篇文章 19 订阅

订阅专栏

星火计划2.0基础课：https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课：https://apollo.baidu.com/community/online-course/12

apollo代码中以状态偏差的形式作为状态量存在，即状态量x表述为横向的偏差以及横向偏差的变化率；航向的偏差以及航向的变化率。公式中描述的是状态量的变化情况，即当知道描述自身属性的矩阵A,B就能在当前的状态下根据控制量推测出下一时刻大概能到的位置(状态).

A矩阵中红色项赋值给matrix_a_，其为常量项；A矩阵中的绿色项为和当前车辆状态(速度)挂钩的项，先放入到另一个矩阵matrix_a_coeff_，后面会一起赋值给A.

代码右侧为给B矩阵赋值。

以上模型是在车辆二自由度动力学模型上进行了frenet坐标转换，转换为上述状态跟踪偏差模型。最终形式为状态量由横向偏差、横向偏差变化率、航向偏差、航向偏差变化率表示，通过该方法对车辆的横向控制进行描述，在控制上更为直观，也能更好的表述控制跟踪planning的能力。

以上为车辆前进和倒车时的两个动力学模型，Apollo倒车部分，动力学模型作了一些修改(红色)。其实低速倒车应用运动学模型也完全足够。该模型的差异表述为，在前进的时候，横向偏差变化率等于原本的横向偏差变化率；倒车的时候横向偏差变化率等于速度乘以航向偏差变化率，该部分为模型假设部分的一个小角度假设。

apollo中侧偏刚度的定义，在配置文件中侧偏刚度为正数，而车辆专业中车辆侧偏刚度为负数。

注意为自行车模型，所以侧偏刚度为两倍。

Q和R需要设计为半正定矩阵和正定矩阵，由于x和u都是平方项，所以可以转化为最小二乘问题。

J中的红色框通过累计求和使得代价函数J达到最小值，当N趋近于无穷的时候，状态向量x就会趋近于0，，即系统达到稳定的状态，而此时控制量u也会趋近于0，因为状态量达到稳定的时候，其不存在偏差，也就意味着控制量不需要做出调整。

最终的目标是求解出最优的状态反馈控制器，其中最重要的是反馈的增益矩阵k，通过k就可以求出在当前状态下给的控制量。该部分内容可以通过构建黎卡提方程进行求解，其通过不断迭代求解。在配置文件重可以设计迭代的停止条件，比如迭代150次，或P矩阵每一项值的变化小于一个阈值，就认定为P达到收敛的状态，就用其计算K矩阵。