本文在前文已经搭建好的规划控制验证平台中进行LQR算法的复现：

1.车辆动力学建模

汽车轨迹跟踪误差模型示意图如下：

2.Frenet坐标系下的误差方程

本节采用两种方式推导Frenet坐标系下的误差模型

2.1 Frenet坐标系与自然坐标系转换

因此，Frenet坐标系到笛卡尔坐标系之间的转换，就是如下过程：

横向速度可以记作：

则：

此外，由于：

则对于，本文直接给出公式：

2.2 方式一：

由2.1中推导得到的坐标转换公式，由于将章节一中的笛卡尔坐标系中的变量替换为自然坐标系中的变量，记:,将展开并忽略路径变化率得到：

对其求导，忽略的变化率得到：

                                                 

带入到章节一中推导得到的动力学方程中有：

2.3 方式二

对于航向偏差有（注意，此处不是航向偏差，为笔误，应当是):

写作状态空间方程：

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. 前馈的计算

6.最终的控制输出