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

5.1、车辆横向动力学模型

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

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

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

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

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

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

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

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

简化后的车辆运动学模型如上图所示。我们选择车辆的质心为参考点，在车身坐标系yyy轴上，根据其受力平衡和力矩平衡可以得到
{Fyf,+Fyr,=may=m(v˙y+vxψ˙)lfFyf,−lrFyr,=Izψ¨\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} {Fyf,+Fyr,=may=m(v˙y+vxψ˙)lfFyf,−lrFyr,=Izψ¨
其中

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

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

mmm为车辆质量；

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

vxv_xvx和vyv_yvy为车身坐标系下质心的纵向速度和侧向速度；

IzI_zIz为车辆绕zzz轴的转动惯量；

lfl_flf和lrl_rlr分别为车辆质心到前、后轴的距离；

ψ\psiψ为车辆横摆角；

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

αf\alpha_fαf为轮胎侧偏角；

δ\deltaδ为前轮转角；

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

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

cfc_fcf、crc_rcr分别为前轮与后轮的侧偏刚度；

分别将前后轮速度分解到车身坐标系上，有
vfx=vf×cos⁡(θvf)=vxvfy=vf×sin⁡(θvf)=vy+lf⋅ψ˙vrx=vr×cos⁡(θvr)=vxvry=vr×sin⁡(θvr)=vy−lr⋅ψ˙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} vfx=vf×cos(θvf)=vxvfy=vf×sin(θvf)=vy+lf⋅ψ˙vrx=vr×cos(θvr)=vxvry=vr×sin(θvr)=vy−lr⋅ψ˙
由此可以得到前后轮速度偏角的表达式
θvf=tan⁡−1(vfyvfx)=tan⁡−1(vy+lfψ˙vx)θvr=tan⁡−1(vryvrx)=tan⁡−1(vy−lrψ˙vx)\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}) θvf=tan−1(vfxvfy)=tan−1(vxvy+lfψ˙)θvr=tan−1(vrxvry)=tan−1(vxvy−lrψ˙)
将轮胎侧向力分解到车身坐标系yyy轴上，可得
(Fyfcos⁡(δ)−Fxfsin⁡(δ))+Fyr=m(v˙y+vxr)lf(Fyfcos⁡(δ)−Fxfsin⁡(δ))−lrFyr=Izψ¨(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} (Fyfcos(δ)−Fxfsin(δ))+Fyr=m(v˙y+vxr)lf(Fyfcos(δ)−Fxfsin(δ))−lrFyr=Izψ¨
解得
v˙y=cf[δ−tan⁡−1(vy+lfψ˙vx)]cos⁡(δ)−crtan⁡−1(vy−lrψ˙vx)−Fxfsin⁡(δ)m−vxψ˙ψ¨=lfcf[δ−tan⁡−1(vy+lfψ˙vx)]cos⁡(δ)+lrcrtan⁡−1(vy−lrψ˙vx)+lfFxfsin⁡(δ)Iz\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} v˙y=mcf[δ−tan−1(vxvy+lfψ˙)]cos(δ)−crtan−1(vxvy−lrψ˙)−Fxfsin(δ)−vxψ˙ψ¨=Izlfcf[δ−tan−1(vxvy+lfψ˙)]cos(δ)+lrcrtan−1(vxvy−lrψ˙)+lfFxfsin(δ)
对转向角做小角度假设，即cos⁡(δ)≈1\cos(\delta)\approx1cos(δ)≈1，sin⁡(δ)≈0\sin(\delta)\approx0sin(δ)≈0，tan⁡−1(θ)≈θ\tan^{-1}(\theta)\approx \thetatan−1(θ)≈θ，则有
v˙y=−cfvy−cflfψ˙mvx+cfδm+−crvy+crlrψ˙mvx−vxrψ¨=−lfcfvy−lf2cfψ˙Izvx+lfcfδIz+lrcrvy−lr2crψ˙Izvx\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=mvx−cfvy−cflfψ˙+mcfδ+mvx−crvy+crlrψ˙−vxrψ¨=Izvx−lfcfvy−lf2cfψ˙+Izlfcfδ+Izvxlrcrvy−lr2crψ˙
将系统状态量侧向速度vyv_yvy和横摆角速度ψ˙\dot{\psi}ψ˙以及系统控制量前轮转角δ\deltaδ提取，则有
v˙y=−(cf+cr)mvxvy+[(lrcr−lfcf)mvx−vx]ψ˙+cfmδψ¨=lrcr−lfcfIzvxvy+−(lf2cf+lr2cr)Izvxψ˙+lfcfIzδ\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 v˙y=mvx−(cf+cr)vy+[mvx(lrcr−lfcf)−vx]ψ˙+mcfδψ¨=Izvxlrcr−lfcfvy+Izvx−(lf2cf+lr2cr)ψ˙+Izlfcfδ
结合上式，可以得到基于前轮偏角小角度假设和线性化轮胎模型的车辆横摆动力学模型。

令ξ=[vyψ˙]T\xi=[v_y\quad\dot{\psi}]^Tξ=[vyψ˙]T为状态矢量，u1=δu_1=\deltau1=δ为输入矢量，线性化的车辆横摆动力学模型可以写成状态空间方程的形式：
ξ˙=Aξ+B1u1\dot{\xi}=A\xi+B_1u_1 ξ˙=Aξ+B1u1
其中A=[−(cf+cr)mvx(lrcr−lfcf)mvx−vxlrcr−lfcfIzvx−(lf2cf+lr2cr)Izvx]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]A=[mvx−(cf+cr)Izvxlrcr−lfcfmvx(lrcr−lfcf)−vxIzvx−(lf2cf+lr2cr)]，B1=[cfmlfcfIz]B_1=\left[\begin{array}{ccc|c}\frac{c_f}{m}\\\frac{l_fc_f}{I_z}\end{array}\right]B1=[mcfIzlfcf]；

令X=[yy˙ψψ˙]TX=[y\quad\dot{y}\quad\psi\quad\dot{\psi}]^TX=[yy˙ψψ˙]T为状态矢量，u2=δu_2=\deltau2=δ为输入矢量，线性化的车辆动力学模型可以写成状态空间方程式的形式：
X˙=AX+B1u2\dot{X}=AX+B_1u_2 X˙=AX+B1u2
其中，A=[01000−(cf+cr)mvx0(lrcr−lfcf)mvx−vx00010lrcr−lfcfIzvx0−(lf2cf+lr2cr)Izvx]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]A=00001mvx−(cf+cr)0Izvxlrcr−lfcf00000mvx(lrcr−lfcf)−vx1Izvx−(lf2cf+lr2cr)，B1=[0cfm0lfcfIz]B_1=\left[\begin{array}{c}0\\\frac{c_f}{m}\\0\\\frac{l_fc_f}{I_z}\end{array}\right]B1=0mcf0Izlfcf；

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

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

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

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

现在定义一种代价函数JJJ：
J=∫0∞xTQx+uTRudtJ=\int^{\infty}_0x^TQx+u^TRu\, dt J=∫0∞xTQx+uTRudt
其中，QQQ和RRR是两个对角参数矩阵，分别决定了状态向量xxx和输入向量uuu的重要性。显然，代价函数JJJ是一个二次型函数。

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

下面分析一下代价函数的意义。考虑一个双变量系统，即x=[x1x2]x=\left[ \begin{array}{c} x_1\\x_2\end{array}\right]x=[x1x2]，我们希望设计的控制器可以表示为u=−[k1k2][x1x2]=−k1x1−k2x2u=-\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_2u=−[k1k2][x1x2]=−k1x1−k2x2.

此时代价函数可以写为：
J=∫0∞[x1x2]Q[x1x2]+(−[k1k2][x1x2])TR(−[k1k2][x1x2])dtJ=\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 J=∫0∞[x1x2]Q[x1x2]+(−[k1k2][x1x2])TR(−[k1k2][x1x2])dt
令QQQ和RRR分别为
Q=[q1q2]Q=\left[ \begin{array}{cc} q_1&\\&q_2\end{array}\right] Q=[q1q2]

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

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

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

那么，如何求解KKK，从而让代价函数JJJ最小呢？

为此，我们定义一个辅助常量矩阵PPP，使得
ddtxTPx=−(xTQx+uTRu)\frac{d}{dt}x^TPx=-(x^TQx+u^TRu) dtdxTPx=−(xTQx+uTRu)
则代价函数JJJ可化为
J=−∫0∞ddtxTPxdt=−(xTPx∣∞−xTPx∣0)=−(0−xTPx∣0)=xT(0)Px(0)\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} J=−∫0∞dtdxTPxdt=−(xTPx∣∞−xTPx∣0)=−(0−xTPx∣0)=xT(0)Px(0)
注：由于我们假设系统稳定，当t→∞t\to\inftyt→∞，x(t)→0x(t)\to0x(t)→0。

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

将定义PPP式左边的微分项展开可得：
x˙TPx+xTPx˙+xTQx+xTKTRKx=0xTAclTPx+xTPAclx+xTQx+xTKTRKx=0xT(AclTP+PAcl+Q+KTRK)x=0\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˙TPx+xTPx˙+xTQx+xTKTRKx=0xTAclTPx+xTPAclx+xTQx+xTKTRKx=0xT(AclTP+PAcl+Q+KTRK)x=0
由于上式对于所以的x(t)x(t)x(t)来说都满足，因此括号中的项要恒等于零。代入Acl=A−BKA_{cl}=A-BKAcl=A−BK可以得到
AclTP+PAcl+Q+KTRK=0(A−BK)TP+P(A−BK)+Q+KTRK=0ATP+PA+Q+KTRK−KTBTP−PBK=0A_{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 AclTP+PAcl+Q+KTRK=0(A−BK)TP+P(A−BK)+Q+KTRK=0ATP+PA+Q+KTRK−KTBTP−PBK=0
假设K=R−1BTPK=R^{-1}B^TPK=R−1BTP，则上式可改写为
ATP+PA+Q+(R−1BTP)TR(R−1BTP)−(R−1BTP)TBTP−PB(R−1BTP)=0ATP+PA+Q−PBR−1BTP=0A^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 ATP+PA+Q+(R−1BTP)TR(R−1BTP)−(R−1BTP)TBTP−PB(R−1BTP)=0ATP+PA+Q−PBR−1BTP=0
上式在现代控制理论中非常重要，也被称为黎卡提方程AlgebraicRiccatiEquation(ARE)Algebraic Riccati Equation(ARE)AlgebraicRiccatiEquation(ARE)。AREAREARE是一个矩阵二次方程，对于给定的(A,B,Q,R)(A,B,Q,R)(A,B,Q,R)可以求出辅助矩阵PPP。之后，优化反馈控制器的KKK以及代价函数的最小值JminJ_{min}Jmin就可以通过辅助矩阵PPP求出。

综上，求解LQRLQRLQR反馈控制器参数KKK的过程为：

设计参数矩阵QQQ、RRR；
求解AREAREARE方程以得到辅助矩阵PPP；
求解反馈控制器参数K=R−1BTPK=R^{-1}B^TPK=R−1BTP；

目前已有求解AREAREARE很完善的数值程序，例如MATLABMATLABMATLAB把它封装进了lqr(A,B,Q,R)lqr(A,B,Q,R)lqr(A,B,Q,R)函数中。

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

离散系统状态空间表达式为：
x(k+1)=Adx(k)+Bdu(k)y(k)=Cdx(k)x(k+1)=A_dx(k)+B_du(k)\\ y(k)=C_dx(k) x(k+1)=Adx(k)+Bdu(k)y(k)=Cdx(k)
其中，Ad=eAΔtA_d=e^{A\Delta t}Ad=eAΔt，Bd=(∫0ΔteAτdτ)BB_d=(\int^{\Delta t}_0e^{A\tau}d\tau)BBd=(∫0ΔteAτdτ)B，eAt=L−1(sI−A)−1e^{At}=L^{-1}(sI-A)^{-1}eAt=L−1(sI−A)−1.

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

其中K=(R+BdTPBd)−1BdTPAdK=(R+B_d^TPB_d)^{-1}B_d^TPA_dK=(R+BdTPBd)−1BdTPAd；

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

辅助常量矩阵P=AdTPAd−AdTPBd(R+BdTPBd)−1BdTPAd+QP=A_d^TPA_d-A_d^TPB_d(R+B_d^TPB_d)^{-1}B_d^TPA_d+QP=AdTPAd−AdTPBd(R+BdTPBd)−1BdTPAd+Q；

5.3、基于LQR的轨迹追踪

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

我们选择系统控制量x=(ecge˙cgeθe˙θ)Tx=(e_{cg}\quad\dot{e}_{cg}\quad e_{\theta}\quad \dot{e}_{\theta})^Tx=(ecge˙cgeθe˙θ)T，系统控制量u=δu=\deltau=δ；

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

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

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

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

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

根据上图，可以推导出
e˙cg=vy+vxsin⁡(θ(s)−θP(s))=vy+vxsin⁡(eθ)\begin{aligned}\dot{e}_{cg}&=v_y+v_x\sin(\theta(s)-\theta_P(s))\\&=v_y+v_x\sin(e_{\theta})\end{aligned} e˙cg=vy+vxsin(θ(s)−θP(s))=vy+vxsin(eθ)
车辆横向误差的加速度e¨cg\ddot{e}_{cg}e¨cg也可以得到
e˙cg=(v˙y+vxθ˙(s))−v˙y(s)=v˙y+vx(θ˙(s)−θ˙p(s))=v˙y+vxe˙θ\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} e˙cg=(v˙y+vxθ˙(s))−v˙y(s)=v˙y+vx(θ˙(s)−θ˙p(s))=v˙y+vxe˙θ
其中，向心加速度部分推导如下
acen=vx2⋅k(s)=vx⋅θ˙(s)\begin{aligned} a_{cen}&=v_x^2\cdot k(s)\\&=v_x\cdot\dot{\theta}(s)\end{aligned} acen=vx2⋅k(s)=vx⋅θ˙(s)
k(s)k(s)k(s)为当前路径点的曲率，θ˙(s)=vx⋅k(s)\dot{\theta}(s)=v_x\cdot k(s)θ˙(s)=vx⋅k(s)。

则车辆侧向动力学模型和误差模型可以表示成如下形式
vy=e˙cg−vxsin⁡(eθ)v˙y=e¨cg−vxe˙θθ(s)=eθ+θp(s)θ˙(s)=e˙θ+θ˙p(s)θ¨=e¨θ+θ¨p(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) vy=e˙cg−vxsin(eθ)v˙y=e¨cg−vxe˙θθ(s)=eθ+θp(s)θ˙(s)=e˙θ+θ˙p(s)θ¨=e¨θ+θ¨p(s)
将以上模型代入第一小节建立的车辆横向动力学模型中
v˙y=−(cf+cr)mvxvy+[(lrcr−lfcf)mvx−vx]ψ˙+cfmδψ¨=lrcr−lfcfIzvxvy+−(lf2cf+lr2cr)Izvxψ˙+lfcfIzδ\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 v˙y=mvx−(cf+cr)vy+[mvx(lrcr−lfcf)−vx]ψ˙+mcfδψ¨=Izvxlrcr−lfcfvy+Izvx−(lf2cf+lr2cr)ψ˙+Izlfcfδ
就可以得到车辆行驶误差模型
e¨cg=−(cf+cr)mvxe˙cg+cf+crmeθ+(lrcr−lfcf)mvxe˙θ+[(lrcr−lfcf)mvx−vx]θ˙p(s)+cfmδe¨θ=lrcr−lfcfIzvxe˙cg+lrcr−lfcfIzeθ+−(lf2cf+lr2cr)Izvx(e˙θ+θ˙p(s))+lfcfIzδ−θ¨p(s)\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) e¨cg=mvx−(cf+cr)e˙cg+mcf+creθ+mvx(lrcr−lfcf)e˙θ+[mvx(lrcr−lfcf)−vx]θ˙p(s)+mcfδe¨θ=Izvxlrcr−lfcfe˙cg+Izlrcr−lfcfeθ+Izvx−(lf2cf+lr2cr)(e˙θ+θ˙p(s))+Izlfcfδ−θ¨p(s)
由于车辆行驶路径一般来说比较平滑，θ¨p(s)≈0\ddot{\theta}_p(s)\approx0θ¨p(s)≈0，这样就得到了以x=(ecge˙cgeθe˙θ)Tx=(e_{cg}\quad\dot{e}_{cg}\quad e_{\theta}\quad \dot{e}_{\theta})^Tx=(ecge˙cgeθe˙θ)T状态量，u=δu=\deltau=δ为控制量，以θp(s)\theta_p(s)θp(s)为前馈量的线性状态空间方程。
x˙=Ax+B1δ+B2rdes\dot{x}=Ax+B_1\delta+B_2r_{des} x˙=Ax+B1δ+B2rdes
A=[01000−cf+crmvcf+crmlrcr−lfcfmv00010lrcr−lfcfIzvlrcr−lfcfIz−lf2cf+lr2crIzv]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]A=00001−mvcf+cr0Izvlrcr−lfcf0mcf+cr0Izlrcr−lfcf0mvlrcr−lfcf1−Izvlf2cf+lr2cr；B1=[0cfm0lfcfIz]B_1=\left [\begin{array} {c} 0\\\frac{c_f}{m}\\0\\\frac{l_fc_f}{I_z} \end{array}\right]B1=0mcf0Izlfcf;B2=[0lrcr−lfcfmv−v0−lf2cf+lr2crIzv]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]B2=0mvlrcr−lfcf−v0−Izvlf2cf+lr2cr；

首先需要检查这个系统是否是可控的，即能控性矩阵[B1,AB1,A2B1,A3B1][B_1,AB_1,A^2B_1,A^3B_1][B1,AB1,A2B1,A3B1]是否是满秩的。

其次，需要用第二小节提到的离散化方法，将连续系统变为离散化的系统。

最后，利用套用第二小节中方法，求得反馈控制器参数KKK，则车辆转角控制信号δ∗(k)=−Kx(k)\delta^*(k)=-Kx(k)δ∗(k)=−Kx(k)。