自动驾驶-控制算法（二）

前言

本文主要记录线性二次型调节器（Linear quadratic regulator，LQR）原理和推导过程，并使用基于运动学模型的LQR实现车辆路径跟踪。

提示：以下是本篇文章正文内容，下面案例可供参考

一、全状态反馈控制系统

假设有一个线性系统用状态向量表示：
$$\begin{array}{r}\dot{x}=Ax+Bu\\ \dot{y}=Cx+Du\end{array}$$
其中，$x(t)\in R^n$，$u(t)\in R^m$。

设计状态反馈控制器
$$\begin{array}{r}u=-Kx\end{array}$$

将式（3）带入系统状态方程中，可得：
$$\begin{array}{r}\dot{x}=(A-BK)x=A_{c}x\end{array}$$

设定系统中的各个状态量都可知，式（1）所示的开环系统，传递函数的极点就是系统矩阵A的特征值。式（4）所示的闭环系统，通过配置反馈矩阵$K$，可以使得闭环系统达到所期望的系统状态。

二、LQR控制器

LQR（Linear Quadratic Regulator，线性二次型调节器）是一种用于解决最优控制问题的数学方法，特别是在系统可以表示为线性方程，且性能指标可以表示为二次型的情况下。LQR通常用于设计线性系统的控制器，使得系统从一个初始状态转移到期望的最终状态，同时最小化一个给定的性能指标。

2.1 连续时间

LQR目标是找到一组控制量$u_0,u_1,...$，使得$x_0,x_1,...$能够快速、稳定地趋近于零，并保持平衡（系统达到稳定状态），同时控制量$u_0,u_1,...$尽可能小。
这是一个典型的多目标优化最优控制稳定，选取目标函数为
$$\begin{array}{r}J=\frac{1}{2}{\int }_0^{\infty }(x^{T}Qx+u^{T}Ru)dt\end{array}$$
其中，Q、R分别是需要设计的半正定矩阵和正定矩阵。Q矩阵元素变大意味着希望状态量能够快速趋近于零，R矩阵元素变大意味着希望控制输入能够尽可能小，系统的状态衰减将变慢。Q、R的设置需结合具体的实际应用场景来调节。

将$u=-Kx$带入代价函数，可得：
$$\begin{array}{r}J=\frac{1}{2}{\int }_0^{\infty }{x}^T(Q+K^{T}RK)xdt\end{array}$$
假设存在一个常量矩阵$P$，使得：
$$\begin{array}{r}\frac{d}{dt}{x}^{T}Px=-x^T(Q+K^{T}RK)x\end{array}$$
将式（7）带入式（6）可得：
$$\begin{array}{r}J=-\frac{1}{2}{\int }_0^{\infty }\frac{d}{dt}(x^{T}Px)dt=-\frac{1}{2}{x}^{T}Px{|}_0^{\infty }=\frac{1}{2}{x}^T(0)Px(0)\end{array}$$
上式当$t\to \infty$时，系统状态向量$x(t)$趋近于0。
将式（7）左边微分展开，可得：
$$\begin{array}{r}{\dot{x}}^{T}Px+x^{T}P\dot{x}+x^T(Q+K^{T}RK)x=0\end{array}$$
将$\dot{x}=(A-BK)x$带入上式并整理可得：
$$\begin{array}{r}{x}^T(A^{T}P-K^T{B}^{T}P+PA-PBK+Q+K^{T}RK)x=0\end{array}$$
若上式有解，则括号中的部分必须为零，即：
$$\begin{array}{r}{A}^{T}P-K^T{B}^{T}P+PA-PBK+Q+K^{T}RK=0\end{array}$$
令$K=R^{-1}{B}^{T}P$，上式可化简为：
$$\begin{array}{r}{A}^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q=0\end{array}$$
式（12）中，$A,B,Q,R$都是已知量，通过上式可求解出$P$，式（12）就是著名的连续时间代数Riccati方程。
连续时间下的LQR算法步骤如下：
1.选择参数矩阵$Q,R$（分别满足半正定和正定）
2.根据公式（12）求解Riccati方程得到矩阵P
$$\begin{array}{r}{A}^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q=0\end{array}$$
3. 根据$P$计算增益$K=R^{-1}{B}^{T}P$
4. 计算控制量$u^{\ast }=-Kx$

2.2 离散时间

假设一个离散系统表示为
$$\begin{array}{r}{\mathrm{x}}_{k+1}=A{\mathrm{x}}_{k+1}+B{\mathrm{u}}_k\end{array}$$
离散LQR代价函数为：
$$\begin{array}{r}J=\sum _{k=0}^{N-1}({\mathrm{x}}_k^{T}Q{\mathrm{x}}_k+{\mathrm{u}}_k^{T}R{\mathrm{u}}_k)+{\mathrm{x}}_N^T{Q}_f{\mathrm{x}}_N\end{array}$$
其中，$Q$为状态代价矩阵，$Q_f$为最终状态代价矩阵，$R$为输入代价矩阵，$N$为时间范围。
求解离散LQR的方法有最小二乘法、动态规划算法等，在此不做详细介绍，详情参考基础算法 - LQR - 离散时间有限边界

根据参考链接中动态规划算法的求解结果，给出离散LQR的求解步骤：

1. 确定迭代范围$N$，设置迭代初始值$P_N=Q_f$；
2. 循环迭代，$k=N,...,1$
   $$\begin{array}{r}{P}_{k-1}=Q+A^T{P}_{k}A-A^T{P}_{k}B(R+B^T{P}_{k}B{)}^{-1}{B}^T{P}_{k}A\end{array}$$
3. 反馈系数$K_k=-(R+B^T{P}_{k+1}B{)}^{-1}{B}^T{P}_{k+1}A$
4. 优化的控制量${\mathrm{u}}_k=K_k{\mathrm{x}}_t$

三、基于车辆运动学模型的LQR优化

采用以车辆后轴中心为原点的运动学模型

图中的部分符号和定义如下：
$A、B$：车辆前轴、后轴中心点；
$P$：规划路径中距后轴中心最近的点；
$L$：车辆轴距；
$v$：车辆后轴中心处的速度；
$\dot{s}$：目标点处的速度；
$\psi$：车辆航向角；
${\psi }_t$：$P$点处的航向角；
${\delta }_f$：车辆的前轮转角；
${\psi }_e$：$P$点处航向角与车辆航向角之间的偏差；
$e_y$：前轴中线点到$P$点切线方向的偏差；
$d$：后轴中心点到目标点的距离；

对应的运动学模型如下：
$$\begin{array}{r}\dot{x}=v\cdot \cos \psi \\ \dot{y}=v\cdot \sin \psi \\ \dot{\psi }=\frac{v\cdot \tan {\delta }_f}{L}\end{array}$$
状态量为：$X=[x,y,\psi {]}^T$，控制量为：$\mathrm{u}=[v,\delta {]}^T$。
将上述模型在参考点$X_r=[x_r,y_r,{\psi }_r]$处利用泰勒级数展开线性化：
$$\begin{array}{c}\dot{X}=f(X,u)\\ \dot{X}={\dot{X}}_r+f_X^{{}^{\prime }}(X_r,u_r)[X-X_r]+f_u^{{}^{\prime }}(X_r,u_r)[u-u_r]\end{array}$$
可得状态误差方程：

$$\begin{array}{r}\dot{\tilde{X}}=A\tilde{X}+B\tilde{\mathrm{u}}\end{array}$$
其中：
$\tilde{X}=\left[\begin{array}{c}x-x_r\\ y-y_r\\ \psi -{\psi }_r\end{array}\right]$

$\tilde{\mathrm{u}}=\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]$

$A=\left[\begin{array}{ccc}0& 0& -v_r\sin {\psi }_r\\ 0& 0& v_r\cos {\psi }_r\\ 0& 0& 0\end{array}\right]$

$B=\left[\begin{array}{cc}\cos {\psi }_r& 0\\ \sin {\psi }_r& 0\\ \frac{\tan {\delta }_r}{L}& \frac{v_r}{L\cdot {\cos }^2{\delta }_r}\end{array}\right]$

将上述方程利用前向欧拉法进行离散化可得：
$$\begin{array}{r}\tilde{X}(k+1)=\tilde{A}\tilde{X}(k)+\tilde{B}\tilde{\mathrm{u}}(k)\end{array}$$
其中：
$\tilde{X}(k)=\left[\begin{array}{c}x(k)-x_r\\ y(k)-y_r\\ \psi (k)-{\psi }_r\end{array}\right]$

$\tilde{\mathrm{u}}(k)=\left[\begin{array}{c}v(k)-v_r\\ \delta (k)-{\delta }_r\end{array}\right]$
$\tilde{A}=A\cdot T+I=\left[\begin{array}{ccc}1& 0& -v_r\cdot T\cdot \sin {\psi }_r\\ 0& 1& v_r\cdot T\cdot \cos {\psi }_r\\ 0& 0& 1\end{array}\right]$

$\tilde{B}=B\cdot T=\left[\begin{array}{cc}T\cos {\psi }_r& 0\\ T\sin {\psi }_r& 0\\ \frac{T\cdot \tan {\delta }_r}{L}& \frac{v_r\cdot T}{L\cdot {\cos }^2{\delta }_r}\end{array}\right]$

式中，$T$为采样步长，$I$为单位矩阵，维度与矩阵$A$一致。

采用LQR进行控制量求解的步骤包括：

1. 确定迭代范围$N$，预设精度$eps$；
2. 设置$Q、R$矩阵，设置迭代初始值为$P_N=Q$；
3. 根据车辆状态信息、目标点计算偏差方程中的$A、B$矩阵(对应上述偏差方程中$\tilde{A}、\tilde{B}$)；
4. 循环迭代，$k=N,...,1$
   $$\begin{array}{r}{P}_{k-1}=Q+A^T{P}_{k}A-A^T{P}_{k}B(R+B^T{P}_{k}B{)}^{-1}{B}^T{P}_{k}A\end{array}$$
   若$||P_{k-1}-P_k||<eps$，结束循环；
5. 计算反馈系数$K=-(R+B^T{P}_{k}B{)}^{-1}{B}^T{P}_{k}A$
6. 优化的控制偏差量为$\tilde{\mathrm{u}}=K\tilde{X}$
7. 进一步计算得控制量$\mathrm{u}$。

车辆前进和后退控制的唯一区别是：前进速度为正，后退速度为负；
车辆方向信息始终采用车辆朝向，即车头对应的方向。

参考文献

1、【控制理论】离散及连续的LQR控制算法原理推导
2、基础算法 - LQR - 离散时间有限边界
3、基于车辆运动学模型的LQR轨迹跟踪控制器