四、规划控制——车辆轨迹追踪优化控制（3）

概述：本文主要学习基于LQR的车辆轨迹追踪

---

前言

本文将继续深入研究基于LQR的自动驾驶车辆轨迹追踪。

---

一、控制系统离散化

控制系统中，各个变量都是时间的连续函数，随着计算机引入控制系统，使得控制器中的一些信号不是系统的连续函数，而是一组离散的脉冲序列或者是数字序列，所以需要对系统进行离散化的处理。
控制器中被控对象的离散化是实现计算机控制的一个重要内容，被控对象经常使用零阶保持器离散化的方法。

1.1 离散化

连续系统中可以表达为：

使用零阶保持器离散化：

其中，


连续形式：

在实际应用中，无论是传感器的反馈还是数字化的计算中，不能完全得到每个点的值，只能得到采样点的值，所以，需要选择一个定期的周期Δt进行采样，这样就会有采样的开关和零阶的保持。
这样就会把连续的系统转换为离散的系统。

1.2 LQR离散化

1. 存在uk
   
   
2. 损失函数（状态误差和输入量进行平衡）

3. 离散化的raccati quation（DARE）

离散的情况下，P和K的公式和连续的是不一样的。

二、Path cordinates Model（Error dynamics）

2.1 参数分析

在LQR应用中，需要构建一个以误差作为状态的系统，因为LQR的目标就是把状态误差更快并且消耗更小的输入消除掉。
对于轨迹追踪的任务：

（1）可以选取侧向误差、侧向误差变化率、角度误差、角度误差的变化率作为状态量X：
（2）选择转向角作为输入量u：

（3）路径是s的函数，路径上任何点的方向可以用：

（4）路径点的角度变化率可以用：

其中变量解析：
（1）ecg：沿车辆纵轴质心点到平移到路径的路径点之间的距离；
（2）ecg’：车辆质心点和路径的相对速度；
（3）eθ：车辆横摆角和路径点沿路径切线夹角的差值：

（4）eθ’：车辆质心点和路径的相对角度变化率（路径 yaw rate）：

2.2 模型建立

模型建立假设：纵向速度是一个常量 Vx：
（1）车辆质心到路径的速度变化率：

（2）车辆质心到路径的加速度变化率：
等于Vy的速度加上向心加速度-路径的向心加速度（eθ’）

也可以用（1）式直接求导，把sineθ近似成θ，求得上式。

这样我们就建立起横向动力学模型与误差模型之间的关系：

回顾：横向动力学模型

将关系式带入模型中可得误差模型:


这里：

1. 将ecg、ecg’、eθ、eθ’作为状态量；
2. 将δ作为输入量；
3. 将r(s)（路径的角度变化量）作为前馈量，r(s)’因为路径相对平滑可以忽略。

三、LQR轨迹跟踪

3.1 LQR控制系统：

（1）其中状态量为：

（2）输入量为转向角；
（3）多了一项路径的转动速度rdes。也就是路径转角变换量，可以看成有理想目标的前馈值。

系数矩阵可以写成：

3.2 系统条件

我们需要寻找一个最优的转向角来使我们的状态量快速回归到0。

1. 首先判断系统是否可控（为满秩行矩阵）：
   

2. 连续系统离散化
   

3. 使用反馈控制律，使得K与状态误差量联系在一起：
   
   这样就可以找到合理的输入控制量。

3.3 输入解析

通过上述条件验证验证了系统的可控合理性。

1. 构建LQR系统并离散化：
   
   

2. 损失函数：
   
   其中，Q为对角阵，其对角元素分别是每个误差状态量的权重。

3. P为离散的raccati equation：
   

四、LQR总结

4.1 LQR意义

LQR构造了系统状态未来时间变化和执行性控制的二次型代价函数，通过对权状态的反馈将不同的状态量以及输入控制量加权求和得到总成本最低的增益矩阵，达到一个最优的控制，从而达到控制的目的。
一般来说，Q、R的设计代表了对于状态量和输入控制量重视程度的大小，Q或者R越大说明在系统设计时状态量或者控制输入量保持比较小的值，或者说对于状态量或者控制输入量的惩罚越大，则会让Q或者R变得更大，所以，LQR就是通过构造了状态误差和控制量的二次型代价函数，通过调节惩罚力度来达到想要的控制效果。

4.2 LQR特点

（1）LQR的控制规律是一个状态变量的线性函数，因此可以通过状态的反馈得到闭环最优解，
（2）LQR提出了相对简单K值求解方法：

（3）LQR的调节相对直观，可以只需确定对状态误差的容忍度高还是对于执行器付出代价的惩罚度高就可以得到最终设计的最优控制量。
（4）LQR提供较好的鲁棒性（phase marge 大于60°）

总而言之，LQR设计便利，并且适用于多输入多输出系统，通过几个矩阵就可以控制稳态性能和控制花费，拥有极好的鲁棒性能。

4.3 优缺点

优点

1. 合理构建出以误差作为状态量的系统，直接可以套用LQR方法的结论，就可以得到相对较优化的控制器（输入量）；
2. 权重和实际状态量相关联，就可以在得到最优解的基础上根据系统实时状态表现来进行权重的进一步的调节和系统优化，比如对于车辆相对于轨迹的侧向误差ecg收敛很慢，我们就可以增大q1权重比例，如果车辆相对于轨迹的角度变化eθ过于激进，可以减小q3权重比例；

缺点

1. LQR是线性优化控制器，需要对系统进行线性优化处理，这就限制了转向角取值较小的工况，如果转向角过大或者路径曲率过大，模型就会出现偏差变得不准确，则线性控制的优化也就不准确。所以，LQR对于大曲率的追踪性能不是特别好。
2. LQR状态误差量只考虑了当前点的距离或者角度的误差，没有考虑未来路径的曲率等状态情况。所以，比较短视，也会导致系统不是很稳定，追踪性能不是特别好。

---

总结

本文主要是对于自动驾驶规划控制学习中LQR轨迹优化控制的知识进行学习，主要介绍了LQR离散化以及误差模型的建立，最后对LQR进行总结。这篇文章希望可以对想要学习自动驾驶规划控制方向的同学们有一定的帮助。
喜欢的朋友们动动小手点个关注，我会定期分享我的一些知识总结和心得体会，感谢大家!