Apollo纵横向控制算法学习

横向控制算法之LQR(二次线性调节)

算法简单流程概述如下：

二自由度车辆动力学模型建立 -> 状态空间方程建立 -> 离散化（双线性变换） -> 标准二次型目标函数建立 -> 黎卡提方程求解矩阵P，求解反馈举证增益K。

这样还不够，需要求解稳定性。

将控制量带入状态方程，发现有稳态误差，需要加一个前馈环节，目的使稳态误差变为0，如何做到呢？在控制量中加上前馈量，再次代入到状态方程，并作拉氏变换，再根据终值定理，计算出稳态误差的公式，其中，为了使状态量趋向0，直接令前馈量的值使得横向稳态误差为0，由此可得到前馈+反馈的控制结果。

链接：车辆控制算法之LQR控制算法解读

---

接下来，将作具体的推导和代码实现。