路径跟踪之LQR控制算法

（一）车辆建模

两自由度车辆模型为：

考虑恒定不变，则上述模型可以看作一个输入为、状态变量为的控制系统，可以表示为：

对于参考轨迹，可以表示为：

将（2）式在处泰勒展开，并忽略高次项，有：

记、，式（4）-（3）可得：

对上式进行离散化处理，可得：

所以，

记：

则（7）式可以写为：

由于系统矩阵A和输入矩阵B的元素随时间变化，所以上述系统是一个线性时变系统。

（二） LQR控制算法仿真

根据上一篇博文的分析，LQR设计步骤如下：

1）根据工程经验和性能分析，确定矩阵Qf、Q和R；

2）倒序求解P；

3）计算反馈矩阵K；

4）正序求解u(k)。

基于第一部分的车辆运动学模型，运用python对LQR控制算法进行仿真，跟踪一个圆形轨迹的仿真结果如下图所示。

具体仿真总结如下：

1）R矩阵的选取

如果不对控制量（前轮转角）进行限制，R需要选择较大的值，比如这里选择86能达到较好的跟踪效果（见上图）；如果R选择较小的值，比如取1，跟踪效果就会很差，如下图所示。

此时，如果对控制量（前轮转角）进行限幅处理，可以提升跟踪效果，如下图所示。

所以，选取较大的R可以减小控制量。

2）Q矩阵的选取

有资料指出：较大的Q可以得到较快的调节速度。

目前仿真下来还没有摸清楚Q选取的规律，只能说Q不能太大、也不能太小。

3）Qf矩阵的选取

将Qf取为0，也能达到较好的跟踪效果，如下图所示。如果对终端误差不作限制，Qf可以取为0。