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

概述：本文主要学习车辆轨迹追踪的优化控制——LQR

---

前言

本文将继续深入研究自动驾驶车辆轨迹追踪优化控制——LQR。

---

线性二次调节器（LQR，Linear quadratic regulator）

LQR概念：

1. 原理

系统从状态A到状态B，给每一个路径赋予一个代价，并且从中选取出最小路径的最优解，决定最优解的过程就是设计每一个代价的权重。将其思想延申到控制系统，那么优化这个控制器的过程就是选择什么（系统表现、花费等）对我们更重要的过程。

2. 研究对象
   LQR的研究对象基于状态空间模型给出的线性系统。
   

3. 目标函数（损失函数）
   LQR目标函数是对象状态、控制输入的二次型函数。
   
   （1）二次型函数的积分面积为正，更加准确的表现出响应回归的快慢；
   （2）二次型函数是凸函数，一定会存在局部最小值，这样就可以保证损失函数会有一个最小值；
   （3）二次型函数平方之后会加速惩罚。

4. 任务
   LQR的任务是当系统的状态出于任何原因偏离了平衡状态时，在不消耗多余的能量情况下，让系统的状态和输入回归到平衡状态。一般来说，把0作为平衡状态。

一般用响应的面积来代替代价的大小，当面积越小时，代价就越小，能够更快的回归到平衡状态。

所以，LQR的目的就是把初始状态不为0的系统的所有状态和输入调节到0，并且最小化代价函数。

目标函数解析

其中，Q和R是指对于状态和输入权重之间的比例，一般来说，Q和R矩阵可以写成对角矩阵的形式：



对角阵上的元素代表了状态量和控制量的权重大小，权重越大，则对量的重视程度越大，对这个量的惩罚也就越大。如果对每个量的重视程度一样时，可以将对角矩阵取成单位矩阵。

基于一维标量系统的LQR解析（a scalar example）

假设一个系统，控制输入尽可能少，并且在回归过程中尽可能靠近于0：

其中，A和B都等于1，初始状态不为0。

所以，目标函数J尽可能要小：

其中，q>=0，状态误差度量为正；r>0，控制输入量误差度量为正。

要想使目标函数尽可能小，需要进行如下步骤：

1. 假设
   
   其中，令r为1，则只需要调节q的比例即可。

2.将上述公式带入系统及目标函数：

3. 求解线性微分方程：
   
   当k>1时，函数收敛，系统才是稳定的。

4. 将x(t)带回目标函数：

5. 在固定q和x(0)的条件下，求目标函数最小值，则需对K求一阶偏导等于0，二阶偏导大于等于0。

则目标函数求一阶导：

求二次偏导：

则：
由于 K>=1，q>=0，所以上述求得的K的解中，只有K1是满足条件的，验证了系统稳定性的K取值要求。

则目标函数最小值：

综上所示：
系统问题就转变成调节q的值，当q取值较大，则K值较大，对于状态响应（趋近于0）的速度加快，此时看重的是对于误差的矫正；当q取值较小，相当于间接增大控制输入的权重r，此时更看重的是输入的耗费（数量）情况。

基于n维系统的LQR解析（raccati equation）

假设一个系统：

其中，x是n×n维的矩阵，u是n维矩阵。

1. 假设存在K值，使得系统是稳定的。

2. 将上述公式带入系统及目标函数
   
   

3. 引入P矩阵，N×N的对称矩阵，使其x和u最终趋近于0。
   

4. 可以将上式带入目标函数中
   
   这样就将求解目标函数J的过程转换成求解P的过程。
   

（1）由于K项是未知的，将K项去掉，先将上述等式展开：

（2）将系统函数带入上述公式：


（3）对于二次型函数等于0，则中间项必须为0。

（4）系统函数反代回上式：

（5）令K为下式：


（6）黎卡提方程（algebraic Riccati equation ，CARE）

其中，A、B是已知的，Q是可以调节的，只有P是未知的可以求出P，进而求出目标函数。

总结：
对于线性系统，

总是存在这样的控制率：

使闭环系统：

使损失函数最小：

引入P，使其系统趋近于0:

通过raccati 方程求得P：

LQR调节（LQR tuning）

Q固定

1. 假设Q固定：

2. 选择不同的R：
   （1）当R选择10时，对于输入u的影响会更大，而状态x的影响会更小，就会更少使用控制量，使得控制输入回归响应变快，使得系统回归响应（位置误差回归响应）变慢。
   （2）当R选择1时，对于输入u的影响会更小， 而状态x的影响会更大，就会更多使用控制量，使得控制输入回归响应变慢，使得系统回归响应（位置误差回归响应）变快。

R固定

1. 假设R固定：
   
2. 选择不同的Q：
   由于Q和状态量的调节有关，所以此时x1是位置，x2是速度：
   

（1）
当选择q1=10，q2=1，则其位置误差上的惩罚影响最大，速度上的惩罚影响最小，但是整体比输入的惩罚大，系统会有一个快速的回归响应，但是需要在输入上花费更多的精力去达到这个快速响应。

（2）
当选择q1=1，q2=1，则其位置误差上的惩罚影响等于速度上的惩罚影响，系统的回归响应相对较慢，但控制输入的能量消耗相对较小。

总的来说，LQR调节是对于Q和R的比例调节，注重的是Q和R的相对比例关系，也就是Q的增大和R的减小是一样的影响，比例和权重不会改变。
LQR调节视情况而定：
（1）当选择更快的系统响应而不在意控制输入时，可以使Q调节大一些，最先优化Q。
（2）当很在意控制输入（控制器的能量消耗）时，可以使R调节大一些，最先优化R。

---

总结

本文主要是对于自动驾驶规划控制中轨迹追踪的优化控制的学习，主要介绍了LQR的概念、解析及调节等知识，这篇文章希望可以对想要学习自动驾驶规划控制方向的同学们有一定的帮助。
喜欢的朋友们动动小手点个关注，我会定期分享我的一些知识总结和心得体会，感谢大家!