本文在前文已经搭建好的规划控制验证平台中进行LQR算法的复现：
1.车辆动力学建模
汽车轨迹跟踪误差模型示意图如下：
摆角 ;
和 
分别为车辆前 、 后轮的侧偏角。并设车辆前后轴侧向合力为 
和 
,假设轮胎侧偏角较小,则汽车二自由度模型的微分方程为:
由于轮胎的侧偏角 α 很小 , 且车辆的后轴不作为转向轴 ,设前轮和后轮的侧偏刚度分别为
和
2.Frenet坐标系下的误差方程
本节采用两种方式推导Frenet坐标系下的误差模型
2.1 Frenet坐标系与自然坐标系转换
因此，Frenet坐标系到笛卡尔坐标系之间的转换，就是如下过程：
横向速度可以记作：
则：
此外，由于：
则对于，本文直接给出公式：
2.2 方式一：
由2.1中推导得到的坐标转换公式，由于将章节一中的笛卡尔坐标系中的变量替换为自然坐标系中的变量，记:,将
展开并忽略路径变化率得到：
对其求导，忽略的变化率得到：
带入到章节一中推导得到的动力学方程中有：
2.3 方式二
对于航向偏差有（注意，此处不是航向偏差，为笔误，应当是):
设车辆行驶在半径为 r 的路径上 , 可求出车辆在目标路径上的理想加速度为 :
由 Frenet 公式可得汽车的实际侧向加速度为 :
由此可得 :
将结果带入到动力学方程中得到：
写作状态空间方程：
3. 离散轨迹规划的误差计算
由于LQR最终输出的量为：
此处的误差详细计算为：
其中
为切向速度，8中的对应关系由2.2中的公式给出。
4.LQR算法中K矩阵的计算
Apllo中采用离线LQR算法查表的方式，先离线计算出不同对应下的K的值，能极大的缩短计算时间：
1.构建=1,2......5000，共五千个点作为离散的速度输入:
,(
=1,2......5000)
2.构造拉格朗日乘子函数：
3.构造指标函数，计算出最优的控制输入,使得下列性能指标函数达到最小值：
4.对指标函数求导求极值，得到：
5.最终得到LQR控制器的控制率，即得到K矩阵：
5. 前馈的计算
为了使轨迹跟踪控制系统更快地响应参考信号和减小稳态误差 , 引入前馈控制 。
可以看出无论 
取何值 ,
和 
都不可能为 0。 因此引入前馈控制量前轮转角 δ f 使得横
向误差为 0, 控制率可表示为 :
带有前馈的 LQR 控制器最终会导致为 0, 不为 0，则有：
则计算得到的前馈为：
其中：kr 为路径曲率，
6.最终的控制输出
结合上述求解的过程，可以得到最终输出的横向控制角度为：
