线控转向之基于 LQR 的横向控制
线控转向之基于 LQR 的横向控制
LQR(Linear Quadratic Regulator)即线性二次调节器，是一种最优控制器，在轨迹跟踪控制中，LQR 将控制车辆沿规划路径行驶的任务，转化成先建立一个关于车辆实际位姿和规划期望位姿之间的误差微分方程，再设计一个控制器使得该误差方程在约束下取得极小值，从而达到最优控制的目的。如此，使用 LQR 完成车辆的横向控制就可以分成两步，首先建立误差微分方程，其次求解误差方程在车辆动力学模型约束下的极小值。由于连续 LQR 涉及泛函分析与变分法且规划模块求解得
出的路径实质上是一系列离散的轨迹点，故本文采用离散 LQR。
1.离散约束方程的建立
轨迹控制中的约束为车辆动力学约束，对车辆动力学模型方程两边积分可得：
其中 t 为当前状态，t+dt 为下一状态，由积分中值定理可得：
连续方程的离散化一般使用欧拉法，此处对 
使用中点欧拉法，对
使用向前欧拉法，可得：
使用中点欧拉法，对
使用向前欧拉法，可得：
化简可得：
设
，可得离散约束方程：
，可得离散约束方程： 
其中
，即为横向控制的控制输入量。
，即为横向控制的控制输入量。 2．误差微分方程的建立
误差微分方程用于描述车辆实际状态与规划期望状态之间的误差，本文设计的误差向量为：
其中
为横向误差，
为航向误差。下面直接给出
和
的关系式：
为横向误差，
为航向误差。下面直接给出
和
的关系式：
其中， r 为期望轨迹点的航向角，将
和
扩展后写成矩阵形式，则有：
和
扩展后写成矩阵形式，则有： 
由式（6）设定的误差向量，可得误差微分方程：
3．代价函数的定义和极小值的求解
基于上述的误差微分方程和离散约束方程可以建立代价函数：
由于当前时刻的控制量会影响下一时刻的状态量，故式（10）状态量比控制量多一项，约束关系如式（5）所示。下面给出求解代价函数取极小值时的控制量的一般步骤：
1）Riccati 黎卡提方程如下：
黎卡提方程具有收敛性，取
代入式（11）后，迭代几十次 会收敛，即
代入式（11）后，迭代几十次 会收敛，即
2）根据A,B,P,R计算得到uk
至此我们使用 LQR 控制器通过控制量uk实现了横向最优控制。
