五、车辆轨迹追踪的优化控制

5.1、车辆横向动力学模型

车辆动力学模型一般包括用于分析车辆平顺性的质量-弹簧-阻尼模型和分析车辆操纵稳定性的车辆-轮胎模型。两者研究的侧重点不同，平顺性分析的重点是车辆的悬架特性，而车辆的操纵稳定性分析的重点是车辆纵向和侧向力学特性。车辆横向控制的目标是使车辆快速而稳定地跟踪给定的期望路径，属于车辆操纵稳定性的范畴，因此对悬架特性不做深入研究。

由于建立的动力学模型主要是作为控制器的预测模型使用，需要在较为准确地描述车辆动力学过程的基础上尽可能地简化，以减少控制算法的计算量。因此，在车辆动力学建模时，要进行以下理想化假设：

（1）假设无人驾驶车辆在平坦路面上行驶，忽略车辆垂向运动；

（2）假设悬架系统及车辆是刚性的，忽略悬架运动及其对耦合关系的影响；

（3）只考虑纯侧偏轮胎特性，忽略横向、纵向轮胎力的耦合关系；

（4）不考虑轮胎的横向载荷转移；

（5）认为轮距相对于转弯半径可忽略不计，使用自行车模型来描述车辆的运动；

（6）忽略横向和纵向空气动力学对车辆横摆运动的影响；

在这里插入图片描述

简化后的车辆运动学模型如上图所示。我们选择车辆的质心为参考点，在车身坐标系 $y$ 轴上，根据其受力平衡和力矩平衡可以得到
$\begin{cases} F_{yf}^,+F_{yr}^,=ma_y=m(\dot{v}_y+v_x\dot{\psi})\\ l_fF_{yf}^,-l_rF_{yr}^,=I_z\ddot{\psi} \end{cases}$
其中

$F_{yf}^,$ 是车辆前轴上的侧向力的合力；

$F_{yr}^,$ 是车辆后轴上的侧向力的合力；

$m$ 为车辆质量；

$a_y$ 为车身坐标系下的侧向加速度；

$v_x$ 和 $v_y$ 为车身坐标系下质心的纵向速度和侧向速度；

$I_z$ 为车辆绕 $z$ 轴的转动惯量；

$l_f$ 和 $l_r$ 分别为车辆质心到前、后轴的距离；

$\psi$ 为车辆横摆角；

在这里插入图片描述

车辆运动学模型与车辆动力学模型最大的区别就是车辆动力学认为轮胎的朝向与轮胎的速度方向不相同相同。在车辆动力学模型中，考虑到汽车在道路上行驶时，轮胎不仅受到纵向力，还会受到侧向力，这就导致轮胎的朝向与轮胎的速度方向不一致，即产生一个侧偏角 $\alpha_f$ 。
$\alpha_f=\delta-\theta_{vf}$
其中

$\alpha_f$ 为轮胎侧偏角；

$\delta$ 为前轮转角；

$\theta_{vf}$ 为车辆纵轴与车辆速度方向的夹角；

在这里插入图片描述

轮胎侧向力与轮胎侧偏角的关系如图所示。由曲线可以看出，当轮胎侧偏角较小时，轮胎侧向力可以近似表示为轮胎侧偏角的线性函数，即
$F_{yf}=2c_f\alpha_f=2c_f(\delta-\theta_{vf})\\ F_{yr}=2c_r\alpha_r=2c_r(-\theta_{vr})$
其中

$c_f$ 、 $c_r$ 分别为前轮与后轮的侧偏刚度；

在这里插入图片描述

分别将前后轮速度分解到车身坐标系上，有
$v_{fx}=v_f\times \cos(\theta_{vf})=v_x\\ v_{fy}=v_f\times \sin(\theta_{vf})=v_y+l_f\cdot\dot{\psi}\\ v_{rx}=v_r\times \cos(\theta_{vr})=v_x\\ v_{ry}=v_r\times \sin(\theta_{vr})=v_y-l_r\cdot \dot{\psi}$
由此可以得到前后轮速度偏角的表达式
$\theta_{vf}=\tan^{-1}(\frac{v_{fy}}{v_{fx}})=\tan^{-1}(\frac{v_y+l_f\dot{\psi}}{v_x})\\ \theta_{vr}=\tan^{-1}(\frac{v_{ry}}{v_{rx}})=\tan^{-1}(\frac{v_y-l_r\dot{\psi}}{v_x})$
将轮胎侧向力分解到车身坐标系 $y$ 轴上，可得
$(F_{yf}\cos(\delta)-F_{xf}\sin(\delta))+F_{yr}=m(\dot{v}_y+v_xr)\\ l_f(F_{yf}\cos(\delta)-F_{xf}\sin(\delta))-l_rF_{yr}=I_z\ddot{\psi}$
解得
$\dot{v}_y=\frac{c_f[\delta-\tan^{-1}(\frac{v_y+l_f\dot{\psi}}{v_x})]\cos(\delta)-c_r\tan^{-1}(\frac{v_y-l_r\dot{\psi}}{v_x})-F_{xf}\sin(\delta)}{m}-v_x\dot{\psi}\\ \ddot{\psi}=\frac{l_fc_f[\delta-\tan^{-1}(\frac{v_y+l_f\dot{\psi}}{v_x})]\cos(\delta)+l_rc_r\tan^{-1}(\frac{v_y-l_r\dot{\psi}}{v_x})+l_fF_{xf}\sin(\delta)}{I_z}$
对转向角做小角度假设，即 $\cos(\delta)\approx1$ ， $\sin(\delta)\approx0$ ， $\tan^{-1}(\theta)\approx \theta$ ，则有
$\dot{v}_y=\frac{-c_fv_y-c_fl_f\dot{\psi}}{mv_x}+\frac{c_f\delta}{m}+\frac{-c_rv_y+c_rl_r\dot{\psi}}{mv_x}-v_xr\\ \ddot{\psi}=\frac{-l_fc_fv_y-l_f^2c_f\dot{\psi}}{I_zv_x}+\frac{l_fc_f\delta}{I_z}+\frac{l_rc_rv_y-l_r^2c_r\dot{\psi}}{I_zv_x}$
将系统状态量侧向速度 $v_y$ 和横摆角速度 $\dot{\psi}$ 以及系统控制量前轮转角 $\delta$ 提取，则有
$\dot{v}_y=\frac{-(c_f+c_r)}{mv_x}v_y+[\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x]\dot{\psi}+\frac{c_f}{m}\delta\\ \ddot{\psi}=\frac{l_rc_r-l_fc_f}{I_zv_x}v_y+\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\dot{\psi}+\frac{l_fc_f}{I_z}\delta$
结合上式，可以得到基于前轮偏角小角度假设和线性化轮胎模型的车辆横摆动力学模型。

令 $\xi=[v_y\quad\dot{\psi}]^T$ 为状态矢量， $u_1=\delta$ 为输入矢量，线性化的车辆横摆动力学模型可以写成状态空间方程的形式：
$\dot{\xi}=A\xi+B_1u_1$
其中 $A=\left[\begin{array}{ccc|c}\frac{-(c_f+c_r)}{mv_x}&\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x\\\frac{l_rc_r-l_fc_f}{I_zv_x}&\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\end{array}\right]$ ， $B_1=\left[\begin{array}{ccc|c}\frac{c_f}{m}\\\frac{l_fc_f}{I_z}\end{array}\right]$ ；

令 $X=[y\quad\dot{y}\quad\psi\quad\dot{\psi}]^T$ 为状态矢量， $u_2=\delta$ 为输入矢量，线性化的车辆动力学模型可以写成状态空间方程式的形式：
$\dot{X}=AX+B_1u_2$
其中， $A=\left[\begin{array}{cccc}0&1&0&0\\0&\frac{-(c_f+c_r)}{mv_x}&0&\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x\\0&0&0&1\\0&\frac{l_rc_r-l_fc_f}{I_zv_x}&0&\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\end{array}\right]$ ， $B_1=\left[\begin{array}{c}0\\\frac{c_f}{m}\\0\\\frac{l_fc_f}{I_z}\end{array}\right]$ ；

5.2、线性二次调节器（LQR）

LQR（Linaer Quadratic Regulator），即线性二次型调节器，是一种现代控制理论中设计状态反馈控制器（State Variable Feedback,SVFB）的方法。

假设有一个线性系统能用状态向量的形式表示成：
$\dot{x}=Ax+Bu\\ y=Cx+Du$
其中 $x(t)\in R^n$ ， $u(t)\in R^n$ ，初始条件是 $x (0)$ 。并且假设这个系统的所有状态变量都是可以测量到的。

我们设计一个线性反馈控制器 $u = - K x$ ，使闭环系统能够满足我们期望的性能。则此时的状态方程可以写为
$\dot{x}=Ax-BKX=(A-BK)x=A_{cl}x$
由于让系统稳定的条件是矩阵 $A_{cl}$ 的特征值得实部均为负数，因此可以手动选择几个满足上述条件的特征值，然后反解出 $K$ ，从而得出控制器。

现在定义一种代价函数 $J$ ：
$J=\int^{\infty}_0x^TQx+u^TRu\, dt$
其中， $Q$ 和 $R$ 是两个对角参数矩阵，分别决定了状态向量 $x$ 和输入向量 $u$ 的重要性。显然，代价函数 $J$ 是一个二次型函数。

我们希望的是在满足系统稳定的前提下，通过设计合适的 $K$ ，让代价函数 $J$ 最小。

下面分析一下代价函数的意义。考虑一个双变量系统，即 $x=\left[ \begin{array}{c} x_1\\x_2\end{array}\right]$ ，我们希望设计的控制器可以表示为 $u=-\left[\begin{array}{cc} k_1&k_2\end{array}\right]\left[ \begin{array}{c} x_1\\x_2\end{array}\right]=-k_1x_1-k_2x_2$ .

此时代价函数可以写为：
$J=\int^\infty_0\left[\begin{array}{cc} x_1&x_2\end{array}\right]Q\left[ \begin{array}{c} x_1\\x_2\end{array}\right]+(-\left[\begin{array}{cc} k_1&k_2\end{array}\right]\left[ \begin{array}{c} x_1\\x_2\end{array}\right])^TR(-\left[\begin{array}{cc} k_1&k_2\end{array}\right]\left[ \begin{array}{c} x_1\\x_2\end{array}\right])dt$
令 $Q$ 和 $R$ 分别为
$Q=\left[ \begin{array}{cc} q_1&\\&q_2\end{array}\right]$

$R=r\quad(u是一维向量)$

则代价函数可以写为：
$J=\int^{\infty}_{0}q_1x_1^2+q_2x_2^2+ru^2dt$
显然，如果令 $q_1>q_2>r$ ，则状态变量 $x_1$ 在代价函数中的占比就更大，这意味着如希望代价函数最小， $x_1$ 必须更小。又因为 $Q$ 越大意味着闭环系统矩阵 $A_{cl}$ 的极点在 $s$ 平面中跟偏左，因此 $x_1$ 收敛得更快。若 $r>q_1>q_2$ ，则希望输入量收敛得更快，也就是以更小的代价实现系统稳定，通常意味着更节省能量。

因为对象是线性的，并且代价函数是二次型，因此这种选择 $K$ 设计状态反馈控制器以最小化代价函数 $J$ 的方法被称为“线性二次型调节器（ $L QR$ ）”。

那么，如何求解 $K$ ，从而让代价函数 $J$ 最小呢？

为此，我们定义一个辅助常量矩阵 $P$ ，使得
$\frac{d}{dt}x^TPx=-(x^TQx+u^TRu)$
则代价函数 $J$ 可化为
$\begin{aligned} J&=-\int^{\infty}_0\frac{d}{dt}x^TPx\,dt\\ &=-(x^TPx|_{\infty}-x^TPx|_0)\\ &=-(0-x^TPx|_0)\\ &=x^T(0)Px(0) \end{aligned}$
注：由于我们假设系统稳定，当 $t\to\infty$ ， $x(t)\to0$ 。

显然，代价函数 $J$ 只和参数矩阵 $P$ 以及系统的初始状态有关，让 $P$ 最小也就是让代价函数最小，所以我们需要找到满足条件的参数矩阵 $P$ 。

将定义 $P$ 式左边的微分项展开可得：
$\dot{x}^TPx+x^TP\dot{x}+x^TQx+x^TK^TRKx=0\\ x^TA_{cl}^TPx+x^TPA_{cl}x+x^TQx+x^TK^TRKx=0\\ x^T(A_{cl}^TP+PA_{cl}+Q+K^TRK)x=0$
由于上式对于所以的 $x (t)$ 来说都满足，因此括号中的项要恒等于零。代入 $A_{cl}=A-BK$ 可以得到
$A_{cl}^TP+PA_{cl}+Q+K^TRK=0\\ (A-BK)^TP+P(A-BK)+Q+K^TRK=0\\ A^TP+PA+Q+K^TRK-K^TB^TP-PBK=0$
假设 $K=R^{-1}B^TP$ ，则上式可改写为
$A^TP+PA+Q+(R^{-1}B^TP)^TR(R^{-1}B^TP)-(R^{-1}B^TP)^TB^TP-PB(R^{-1}B^TP)=0\\ A^TP+PA+Q-PBR^{-1}B^TP=0$
上式在现代控制理论中非常重要，也被称为黎卡提方程 $A l g e b r ai c R i cc a t i Eq u a t i o n (A RE)$ 。 $A RE$ 是一个矩阵二次方程，对于给定的 $(A, B, Q, R)$ 可以求出辅助矩阵 $P$ 。之后，优化反馈控制器的 $K$ 以及代价函数的最小值 $J_{min}$ 就可以通过辅助矩阵 $P$ 求出。

综上，求解 $L QR$ 反馈控制器参数 $K$ 的过程为：

设计参数矩阵 $Q$ 、 $R$ ；
求解 $A RE$ 方程以得到辅助矩阵 $P$ ；
求解反馈控制器参数 $K=R^{-1}B^TP$ ；

目前已有求解 $A RE$ 很完善的数值程序，例如 $M A T L A B$ 把它封装进了 $lq r (A, B, Q, R)$ 函数中。

由于计算机处理的是离散的信号，所以在实际运用中往往需要将 $L QR$ 方法离散化，这里直接给出结论。

离散系统状态空间表达式为：
$x(k+1)=A_dx(k)+B_du(k)\\ y(k)=C_dx(k)$
其中， $A_d=e^{A\Delta t}$ ， $B_d=(\int^{\Delta t}_0e^{A\tau}d\tau)B$ ， $e^{At}=L^{-1}(sI-A)^{-1}$ .

假设状态反馈控制器 $u^*(k)=-Kx(k)$ ；

其中 $K=(R+B_d^TPB_d)^{-1}B_d^TPA_d$ ；

代价函数 $J=\sum^{\infty}_{k=0}x^T(k)Qx(k)Qx(k)+u^T(k)Ru(k)$ ；

辅助常量矩阵 $P=A_d^TPA_d-A_d^TPB_d(R+B_d^TPB_d)^{-1}B_d^TPA_d+Q$ ；

5.3、基于LQR的轨迹追踪

为将 $L QR$ 应用于车辆轨迹追踪中，需要基于车辆运动学模型，构建一个以误差作为状态量，转角作为控制量的系统状态方程。

在这里插入图片描述

我们选择系统控制量 $x=(e_{cg}\quad\dot{e}_{cg}\quad e_{\theta}\quad \dot{e}_{\theta})^T$ ，系统控制量 $u=\delta$ ；

其中， $e_{cg}$ 为车辆横向误差，即重心 $C . G$ 到路径点 $c_x,c_y)$ 的垂直距离；

$\dot{e}_{cg}$ 为车辆横向误差的变化率；

$e_{\theta}$ 为车辆角度误差，即车辆纵轴与路径上点 $c_x,c_y)$ 的切线间的夹角，也即车辆纵轴与绝对坐标系下 $x$ 轴的夹角 $\theta$ 与路径上点 $c_x,c_y)$ 的切线与绝对坐标系下 $x$ 轴的夹角 $\theta(s)$ 之差；

$\dot{e}_{\theta}$ 为车辆角度误差的变化率， $\dot{e}_{\theta}=\dot{\theta}(s)-\dot{\theta}_p(s)$ ；

在这里插入图片描述

在建立模型之前，假设车辆纵向速度 $v_x$ 为常量。

根据上图，可以推导出
$\begin{aligned}\dot{e}_{cg}&=v_y+v_x\sin(\theta(s)-\theta_P(s))\\&=v_y+v_x\sin(e_{\theta})\end{aligned}$
车辆横向误差的加速度 $\ddot{e}_{cg}$ 也可以得到
$\begin{aligned} \dot{e}_{cg}&=(\dot{v}_y+v_x\dot{\theta}(s))-\dot{v}_y(s)\\ &=\dot{v}_y+v_x(\dot{\theta}(s)-\dot{\theta}_p(s))\\ &=\dot{v}_y+v_x\dot{e}_{\theta} \end{aligned}$
其中，向心加速度部分推导如下
$\begin{aligned} a_{cen}&=v_x^2\cdot k(s)\\&=v_x\cdot\dot{\theta}(s)\end{aligned}$
$k (s)$ 为当前路径点的曲率， $\dot{\theta}(s)=v_x\cdot k(s)$ 。

则车辆侧向动力学模型和误差模型可以表示成如下形式
$v_y=\dot{e}_{cg}-v_x\sin(e_{\theta})\\ \dot{v}_y=\ddot{e}_{cg}-v_x\dot{e}_{\theta}\\ \theta(s)=e_{\theta}+\theta_p(s)\\ \dot{\theta}(s)=\dot{e}_{\theta}+\dot{\theta}_p(s)\\ \ddot{\theta}=\ddot{e}_{\theta}+\ddot{\theta}_p(s)$
将以上模型代入第一小节建立的车辆横向动力学模型中
$\dot{v}_y=\frac{-(c_f+c_r)}{mv_x}v_y+[\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x]\dot{\psi}+\frac{c_f}{m}\delta\\ \ddot{\psi}=\frac{l_rc_r-l_fc_f}{I_zv_x}v_y+\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\dot{\psi}+\frac{l_fc_f}{I_z}\delta$
就可以得到车辆行驶误差模型
$\ddot{e}_{cg}=\frac{-(c_f+c_r)}{mv_x}\dot{e}_{cg}+\frac{c_f+c_r}{m}e_{\theta}+\frac{(l_rc_r-l_fc_f)}{mv_x}\dot{e}_{\theta}+[\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x]\dot{\theta}_p(s)+\frac{c_f}{m}\delta\\ \ddot{e}_{\theta}=\frac{l_rc_r-l_fc_f}{I_zv_x}\dot{e}_{cg}+\frac{l_rc_r-l_fc_f}{I_z}e_{\theta}+\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}(\dot{e}_{\theta}+\dot{\theta}_p(s))+\frac{l_fc_f}{I_z}\delta-\ddot{\theta}_p(s)$
由于车辆行驶路径一般来说比较平滑， $\ddot{\theta}_p(s)\approx0$ ，这样就得到了以 $x=(e_{cg}\quad\dot{e}_{cg}\quad e_{\theta}\quad \dot{e}_{\theta})^T$ 状态量， $u=\delta$ 为控制量，以 $\theta_p(s)$ 为前馈量的线性状态空间方程。
$\dot{x}=Ax+B_1\delta+B_2r_{des}$
$A=\left [\begin{array} {cccc} 0&1&0&0\\0& -\frac{c_f+c_r}{mv}&\frac{c_f+c_r}{m}&\frac{l_rc_r-l_fc_f}{mv}\\0&0&0&1\\0&\frac{l_rc_r-l_fc_f}{I_zv}&\frac{l_rc_r-l_fc_f}{I_z}&-\frac{l_f^2c_f+l_r^2c_r}{I_zv} \end{array}\right]$ ； $B_1=\left [\begin{array} {c} 0\\\frac{c_f}{m}\\0\\\frac{l_fc_f}{I_z} \end{array}\right]$ ; $B_2=\left [\begin{array} {c} 0\\\frac{l_rc_r-l_fc_f}{mv}-v\\0\\-\frac{l_f^2c_f+l_r^2c_r}{I_zv} \end{array}\right]$ ；