星火计划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矩阵。
上述代码为黎卡提迭代及反馈矩阵k的实现
如果想要保证车辆在车道保持中的侧向控制，除了LQR计算出转向角之后，车辆横向上还是存在一个稳态误差。因为在模型建立重，从笛卡尔坐标系转化到了frenet坐标系，此时还会存在一个曲率的变化特征影响到了线性系统产生稳态误差，因此需要加入前馈计算。
倒车时认为在低速状态下，车辆前后轮的侧偏角是相同的，稳态时转向角可以直接使用L/R. 即此时前馈方向盘的结果是通过该公式求解的。
以下为LQR代码解析
调试过程中如果控制模块没有达到期望的姿态，最期望关注当前底盘方向盘状态、方向盘转角二者是否吻合；如果二者吻合，说明控制层面的输出值不够，可以根据横向偏差、横向偏差的变化率去调整Q矩阵中对应的权重；如果横向方向盘的输出值不够，可以关注K矩阵中的四项；如果还是不行，应当关注前馈控制的输出
该函数为加载当前的控制参数，控制参数比较关注ts_（执行频率）、preview_window_能改变A矩阵维度，一般默认维度为4，而apollo使用为0.
初始化滤波器，初始化偏差均值滤波
矩阵初始化
加入滞后超前控制器
加载横向增益
倒车动力学矩阵A进行了修改
判断车辆驾驶方向，同时给控制结果的输出加一个负号
计算横向偏差，通过获取当前车辆的位置xy，车辆是前进和后退，车速，角速度，线性加速度，Planning的轨迹得到。
该计算函数通过绝对时间查找最近的匹配点或根据当前车辆所处位置查找最近匹配点，得到匹配点后计算笛卡尔坐标系下的x，y偏差，然后根据当前planning给出的航向heading参考值，求解出一个车身坐标系下横向的偏差，可以理解为如果车辆处在planning右侧，为负的，反之为正。
矩阵状态更新(涉及实时更新速度)
前馈结果计算，分为倒车和前进(运动学模型)
超前滞后补偿加入
组成最终方向盘角度输出
计算反馈控制矩阵k在不同维度，不同偏差的贡献值
将debug的状态量打印到log中
7982
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
