CN104977933A - 一种自主驾驶车辆的区域型路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种自主驾驶车辆的区域型路径跟踪控制方法，以克服跟踪控制方法未考虑车辆和道路的形状与大小进而不能保证车辆不与道路边界或周围障碍物发生碰撞的问题，步骤为：建立二维车辆道路模型；建立车辆路径跟踪问题的数学模型；计算车辆前方一段距离内的可行道路区域边界线；建立车辆系统模型；进行区域型路径跟踪控制模型的设计并选取控制量即当前时刻最佳的前轮转角；根据最佳的前轮转角控制转向执行机构动作从而使被控车辆在车辆感知系统给出的车辆前方一段距离内可行道路区域内行驶。本方法在建立的二维车辆道路模型时，考虑了车辆和道路的形状与大小，降低车辆与道路边界发生碰撞的可能，提高了自主驾驶车辆的安全性。

Description

一种自主驾驶车辆的区域型路径跟踪控制方法

技术领域

本发明属于自主驾驶技术领域的控制方法，涉及一种自主驾驶车辆的路径跟踪控制方法。

背景技术

自主驾驶技术的广泛应用前景使其日益受到人们的关注。典型的自主驾驶系统包括感知系统和驾驶控制系统两大功能模块，感知系统用以获取车辆周围环境信息和车辆自身的运行状态信息，驾驶控制系统则是代替驾驶员控制车辆行驶。对自主驾驶技术而言，路径跟踪控制是驾驶控制系统需要解决的一个最为关键的控制问题。现有的自主驾驶车辆的路径跟踪控制大都是参照了机器人的运动控制方法，首先根据感知系统扫描到的道路交通信息规划出一条可行的轨迹或路径线，然后控制车辆跟踪这条可行的轨迹或路径线。这些控制方法大都是基于点线式的车辆道路模型设计的，其中一些典型的控制方法有纯点追踪法、预瞄PID和Stanly方法等。它们都能取得较好的路径跟踪控制效果，但由于在设计过程中忽略掉了车辆和道路的形状与大小，所以并不能保证车辆不会与道路边界或其周围的障碍物发生碰撞。

发明内容

本发明要解决的问题是克服现有自主驾驶车辆的路径跟踪控制方法中存在的未考虑车辆和道路的形状与大小进而不能保证车辆不与道路边界或其周围障碍物发生碰撞的问题，提供一种自主驾驶车辆的区域型路径跟踪控制方法。

本发明提出的一种自主驾驶车辆的区域型路径跟踪控制方法是采用如下技术方案实现的：

一种自主驾驶车辆的区域型路径跟踪控制方法，自主驾驶车辆中的驾驶控制系统首先根据感知系统扫描处理后给出的可行道路区域信息以及车辆自身的运行状态信息优化出当前时刻最佳的前轮转角，然后根据该前轮转角控制车辆的转向执行机构动作，以使车辆在可行的道路区域内行驶，其特征在于步骤如下：

步骤一、建立二维车辆道路模型：

建立二维车辆道路模型，设刚性杆RF为车辆模型，它过车辆的质心o，长度等于车身长度l，期望路径则由期望道路区域左边界线f′_l(x)、期望道路区域右边界线f′_r(x)和期望道路区域中心线f(x)组成的期望道路区域来表示，且满足：

$\left\{\begin{array}{c}{f}_l^{\′}\left(x\right)=f_l\left(x\right)-\frac{w}{2}\\ f_r^{\′}\left(x\right)=f_r\left(x\right)+\frac{w}{2}\\ f\left(x\right)=\frac{f_l\left(x\right)+f_r\left(x\right)}{2}\end{array}\right.---\left(1\right)$

式中，f_l(x)为通过自主驾驶车辆中的感知系统扫描处理得到的前方一段距离内可行道路区域的左边界；f_r(x)为通过自主驾驶车辆中的感知系统扫描处理得到的前方一段距离内可行道路区域的右边界；f′_l(x)为期望道路区域左边界线，f_r'(x)为期望道路区域右边界线，w为车辆宽度，单位，m；

步骤二、建立车辆的区域型路径跟踪问题的数学模型：

基于步骤一建立的二维车辆道路模型，保证刚性杆RF始终处于由期望道路区域左边界线f′_l(x)、期望道路区域右边界线f_r'(x)和期望道路区域中心线f(x)组成的期望道路区域内，结合刚性杆的几何和物理特性，建立车辆的区域型路径跟踪问题的数学模型如下：

$\{\begin{array}{c}{f}_r^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\\ f_r^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}---\left(6\right)$

式中，y_o为车辆质心o的侧向位置，单位，m；l_f为车辆质心o到车辆前端点F的距离，单位，m；l_r为车辆质心o到车辆后端点R的距离，单位，m；ψ为车辆横摆角，单位，rad；β为车辆质心侧偏角，单位，rad；

步骤三、计算车辆前方一段距离内可行道路区域边界线f_l(x)和f_r(x)：

假定自主驾驶车辆的感知系统可实时获取车辆附近的可行道路区域边界的点序列信息(x_r,y_r,x_l,y_l)，基于二次搜索算法采用三次拉格朗日插值公式得到车辆前方一段距离内可行道路区域的边界线函数为：

$\left\{\begin{array}{c}{f}_r\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_r(i\left)\right)}{\left(x_r\right(p)-x_r(i\left)\right)}{y}_r\left(p\right)\\ f_l\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_l(i\left)\right)}{\left(x_l\right(p)-x_l(i\left)\right)}{y}_l\left(p\right)\end{array}\right.,\begin{array}{c}p=j,n,m,k;\\ i=j,n,m,k;\end{array}---\left(7\right)$

式中，(x_r(i),y_r(i),x_l(i),y_l(i)),i＝j,n,m,k为可行道路区域边界的点序列(x_r,y_r,x_l,y_l)中的四组坐标点；

步骤四、建立自主驾驶车辆系统模型并将其整理为状态空间形式：

$\left\{\begin{array}{c}\stackrel{\·}{x}=Ax+{\mathrm{B\δ}}_f\\ y_o=Cx\end{array}\right.---\left(19\right)$

式中，

$A=\left[\begin{array}{cccc}0& v& v& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{2(C_f+C_r)}{mv}& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1\\ 0& 0& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}& \frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ -\frac{2C_f}{mv}\\ -\frac{2{\mathrm{aC}}_f}{I_z}\end{array}\right],C=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right];$

式中，x为系统状态向量，且x＝[y_o ψ β r]^T；δ_f为车辆前轮转角，也为系统控制量，单位，rad；y_o为系统输出；A为系统矩阵；B为输入矩阵；C为输出矩阵；v为车辆质心处的速度，单位，m/s；r为车辆的横摆角速度，单位，rad/s；C_f为车辆前轮轮胎的侧偏刚度，单位，N/rad；C_r为车辆后轮轮胎的侧偏刚度，单位，N/rad；m为车辆的质量，单位，kg；I_z为车辆绕z轴的转动惯量，单位，kg·m²；a为车辆质心o到车辆前轴的距离，单位，m；b为车辆质心o到车辆后轴的距离，单位，m；

步骤五、采用模型预测控制方法设计车辆的区域型路径跟踪控制模型为：

$\left\{\begin{array}{c}\underset{{\mathrm{\δ}}_f(k+i)}{\min }J\\ J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)\end{array}\right.---\left(32\right)$

满足： $\left\{\begin{array}{c}x(k+i+1)=A_{c}x(k+i)+B_c{\mathrm{\δ}}_f(k+i)\\ y_o(k+i)=C_{c}x(k+i)\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\\ \left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ f_r^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ f_r^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ \left|C_{\mathrm{\β}}x(k+i)\right|\≤\left|{\mathrm{\β}}_{rollover}\right|\end{array}\right.$

式中：

$Y(k+1|k)=\left[\begin{array}{c}{y}_o(k+1)\\ y_o(k+2)\\ .\\ .\\ .\\ y_o(k+P)\end{array}\right],R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right],U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right];$

$A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C;$

Δx_d(k+i)＝v(k)·T_s；

Δy_d(k+i)＝y_o(k+i)-y_o(k+i-1)；

Δδ_f(k+i)＝δ_f(k+i)-δ_f(k+i-1)；

C_ψ＝[0 1 0 0],C_β＝[0 0 1 0]；

并选取控制量即当前时刻最佳的前轮转角为：

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right)$

式中δ_f(k+i)为k+i时刻的系统控制量，即为车辆的前轮转角，单位，rad；

x(k+i)为k+i时刻的系统状态向量；

y_o(k+i)为k+i时刻的系统输出，即车辆质心的侧向位置，单位，m；

P为预测时域，N为控制时域；

Γ_y和Γ_u为加权矩阵；

Γ_d,i为权重因子；

y_r(k+i),i＝1,…,P为期望道路区域中心线f(x)的离散量，离散间隔为v(k)·T_s，单位，m；

Δx_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的纵向位移，单位，m；

Δy_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的侧向位移，单位，m；

Δδ_f(k+i)为k+i时刻的控制增量，单位，rad；

δ_fsat为转向执行机构所能实现的最大的前轮转角，单位，rad；

Δδ_fsat转向执行机构所能实现的最大的前轮转角增量，单位，rad；

f′_l(k+i)为期望道路区域左边界线f′_l(x)在时刻k+i的采样值，单位，m；

f_r'(k+i)则为期望道路区域右边界线f_r'(x)在时刻k+i的采样值，单位，m；

β_rollover为车辆可能发生侧翻的临界量，单位，rad；

T_s为采样时间，单位s；

U^*为通过优化求解得到的最优控制序列；

步骤六、根据步骤五中给出的当前时刻最佳的前轮转角控制转向执行机构动作，使得被控车辆的前轮转角等于当前时刻最佳的前轮转角从而使被控车辆在车辆感知系统给出的车辆前方一段距离内可行道路区域内行驶，实现区域路径跟踪的控制目标。

进一步的技术方案为：

步骤二的具体过程为：

为保证刚性杆RF始终处于由期望道路区域左边界线f′_l(x)、期望道路区域右边界线f_r'(x)和期望道路区域中心线f(x)组成的期望道路区域内，需保证下述的式(2)中描述的关系成立：

$\left\{\begin{array}{c}{f}_r^{\′}\left(x\right)\≤y_F\≤f_l^{\′}\left(x\right)\\ f_r^{\′}\left(x\right)\≤y_R\≤f_l^{\′}\left(x\right)\end{array}\right.---\left(2\right)$

式中，y_F为刚性杆RF前端点F的侧向位置，单位，m；y_R为刚性杆RF后端点R的侧向位置，单位，m；

刚性杆RF的前端点F和后端点R与质心o存在如下几何关系：

$\left\{\begin{array}{c}{y}_F=y_o+l_f\sin (\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r\sin (\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(3\right)$

式中，y_o为车辆质心o的侧向位置，单位，m；l_f为车辆质心o到车辆前端点F的距离，单位，m；l_r为车辆质心o到车辆后端点R的距离，单位，m；ψ为车辆横摆角，单位，弧度(rad)；β为车辆质心侧偏角，单位，rad；

考虑到自主驾驶车辆的感知系统每次可观测到的距离大约为50m，而道路的曲率也大都是比较小的，所以认为车辆在这段区域内行驶时的横摆角ψ是很小的，又考虑到车辆的质心侧偏角β很小，本发明采用如下近似关系：

$\left\{\begin{array}{c}sin(\mathrm{\ψ}+\mathrm{\β})\≈\mathrm{\ψ}+\mathrm{\β}\\ cos(\mathrm{\ψ}+\mathrm{\β})\≈1\end{array}\right.---\left(4\right)$

进而可将公式(3)简化为：

$\left\{\begin{array}{c}{y}_F=y_o+l_f(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(5\right)$

将公式(5)代入到公式(2)中，整理即可得到步骤二中所述的车辆的区域型路径跟踪问题的数学模型式(6)。

步骤三的具体过程为：

步骤三中的式(7)中，(x_r(i),y_r(i),x_l(i),y_l(i)),i＝j,n,m,k为道路点序列(x_r,y_r,x_l,y_l)中的四组坐标点，这四组坐标点的选取则是基于二次搜索算法进行的，二次搜索的目的是为了获取车辆前方一段距离内可行道路区域的起点与终点二次搜索算法的具体推导过程如下：

在不考虑倒车的情况下，假设被控车辆当前所在的位置坐标为(x_o,y_o)，取作为搜索的起点，则点的横坐标x_r(0)和点的横坐标x_l(0)必为负值，此次搜索的目的是寻找位于车辆质心o后方且在X轴方向上距离点o最近的一组点则点的横坐标x_r(j)和点的横坐标x_l(j)必满足公式(8)：

$\left\{\begin{array}{c}{x}_r\left(j\right)\·x_r(j+1)\≤0\\ x_l\left(j\right)\·x_l(j+1)\≤0\end{array}\right.---\left(8\right)$

式中，x_r(j+1)为位于点前方且距离它最近的点的横坐标；x_l(j+1)为位于点前方且距离它最近的点的横坐标；

寻找到满足式(8)的一组点后，将它们的信息存储起来，然后在进行第二次搜索时，以它们作为搜索的起始点，考虑到作用到自主车上的控制信号，其有效作用期一般为1s左右，只考虑车辆前方长度为v的一段道路区域，式中v为车辆质心处的速度，因此，本次搜索的目标点的横坐标必定满足式(9)中的不等式关系：

$\left\{\begin{array}{c}\left(x_r\right(k)-v)\·\left(x_r\right(k+1)-v)\≤0\\ \left(x_l\right(k)-v)\·\left(x_l\right(k+1)-v)\≤0\end{array}\right.---\left(9\right)$

式中，x_r(k)为点的横坐标；x_l(k)为点的横坐标；x_r(k+1)为位于点前方且距离它最近的点的横坐标；x_l(k+1)为位于点前方且距离它最近的点的横坐标；

将搜索到的点和作为式(7)的两组插值点，同时选取点和作为式(7)的另外两组插值点，令j、k、n和m分别代表上述四组插值点在已知道路点序列(x_r,y_r,x_l,y_l)的位置，j、k、n和m之间的关系如下：

步骤四的具体过程为：

(1)建立车辆运动学模型

假定车辆是一个刚性体，其中装置着四个不会发生形变的车轮，并以车辆前轮作为转向轮，根据运动学方程以及几何关系得到车辆的运动学模型如公式(11)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=vcos(\mathrm{\ψ}+\mathrm{\β})\\ {\stackrel{\·}{y}}_o=vsin(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(11\right)$

式中，x_o为车辆质心o的纵向位置，单位，m；y_o为车辆质心o的侧向位置，单位，m；v为车辆质心处的速度，单位，m/s；r为车辆的横摆角速度，单位，rad/s，将式(4)代入到式(11)中，则可以得到简化的车辆运动学模型，如式(12)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=v\\ {\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(12\right)$

(2)建立车辆动力学模型

设车辆动力学模型中车辆质心o为坐标原点，沿着车身向前的方向为横轴x的正方向，垂直于横轴向上的方向为纵轴y的正方向，本方法是通过控制车辆的前轮转角来实现路径跟踪的目的的，所以忽略车辆的纵向动力学，只考虑车辆的侧向动力学及横摆方向的动力学，根据牛顿第二定律与力矩平衡关系，可得到如式(13)所示的车辆动力学模型：

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f+F_{yr}\\ I_z\stackrel{\·}{r}=a(F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f)-{\mathrm{bF}}_{yr}\end{array}\right.---\left(13\right)$

式中，v_x为车辆质心处的纵向速度，单位，m/s；F_yf为车辆前轮侧向力，单位，N；F_yr为车辆后轮侧向力，单位，N；m为车辆的质量，单位，kg；I_z为车辆绕z轴的转动惯量，单位，kg·m²；a为车辆质心o到车辆前轴的距离，单位，m；b为车辆质心o到车辆后轴的距离，单位，m；δ_f为车辆前轮转角，单位，rad，车辆的前轮转角δ_f很小，可将公式(13)简化，简化后的车辆动力学模型如式(14)所示：

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{yf}+F_{yr}\\ I_z\stackrel{\·}{r}={\mathrm{aF}}_{yf}-{\mathrm{bF}}_{yr}\end{array}\right.---\left(14\right)$

假定车辆侧向轮胎力未达到饱和，此时侧向力F_y与轮胎侧偏角α基本呈线性关系，如式(15)所示：

$\left\{\begin{array}{c}{F}_{yf}=2C_f{\mathrm{\α}}_f\\ F_{yr}=2C_r{\mathrm{\α}}_r\end{array}\right.---\left(15\right)$

式中，C_f为车辆前轮的轮胎侧偏刚度，单位，N·rad；C_r为车辆后轮的轮胎侧偏刚度，单位，N·rad；α_f为车辆前轮的轮胎侧偏角，单位，rad；α_r为车辆后轮的轮胎侧偏角，单位，rad，根据坐标系的规定，前轮的轮胎侧偏角α_f和后轮的轮胎侧偏角α_r分别为：

$\left\{\begin{array}{c}{\mathrm{\α}}_f=\mathrm{\β}+\frac{ar}{v_x}-{\mathrm{\δ}}_f\\ {\mathrm{\α}}_r=\mathrm{\β}-\frac{br}{v_x}\end{array}\right.---\left(16\right)$

结合式(14)、(15)和(16)，整理可得二自由度的车辆动力学模型，如式(17)所示：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{{\mathrm{mv}}_x}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}_x^2}-1)r-\frac{2C_f}{{\mathrm{mv}}_x}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_z{v}_x}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(17\right)$

(3)建立车辆系统的状态空间模型

结合式(12)和式(17)，同时考虑到则可得车辆系统运动和动力学的微分方程式，具体如式(18)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\\ \stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{mv}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1)r-\frac{2C_f}{mv}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(18\right)$

通过控制车辆的前轮转角进而保证车辆的侧向位置满足式(6)中的不等式约束，选取车辆质心o的侧向位置y_o作为系统输出，同时选取前轮转角δ_f作为系统控制量，系统状态向量选取为x＝[y_o ψ β r]，车辆系统模型可被描述成步骤四中式(19)所示的状态空间模型。

步骤五的具体过程为：

假设自主驾驶车辆在一个预测时域内保持恒速行驶，步骤四中的式(19)为车辆系统的微分模型，为了用于车辆的区域型路径跟踪控制模型的设计，需要将式(19)离散化，得到离散时间的车辆系统模型，如式(20)所示：

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ y_o\left(k\right)=C_{c}x\left(k\right)\end{array}\right.---\left(20\right)$

式中， $A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C,$ 式中T_s为采样时间；

假定预测时域为P，控制时域为N，且满足N≤P，同时假定控制时域之外的控制量保持不变，即δ_f(k+N-1)＝δ_f(k+N)＝…＝δ_f(k+P-1)，则基于式(20)中离散时间的车辆系统模型推导出P步的状态预测方程，具体如(21)所示：

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ x(k+2)=A_{c}x(k+1)+B_c{\mathrm{\δ}}_f(k+1)\\ =A_c^{2}x\left(k\right)+A_c{B}_c{\mathrm{\δ}}_f\left(k\right)+B_c{\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ x(k+N)=A_c^{N}x\left(k\right)+A_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+B_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ x(k+P)=A_c^{P}x\left(k\right)+A_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(21\right)$

同时推导出P步的预测输出，如式(22)所示：

$\left\{\begin{array}{c}{y}_o(k+1)=C_c{A}_{c}x\left(k\right)+C_c{B}_c{\mathrm{\δ}}_f\left(k\right)\\ .\\ .\\ .\\ y_o(k+N)=C_c{A}_c^{N}x\left(k\right)+C_c{A}_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+C_c{B}_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ y_o(k+P)=C_c{A}_c^{P}x\left(k\right)+C_c{A}_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{C}_c{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(22\right)$

定义控制输入序列U(k)和控制预测输出序列Y(k+1|k)分别为：

$\left\{\begin{array}{c}U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right]\\ Y(k+1|k)=\left[\begin{array}{c}{y}_o(k+1)\\ y_o(k+2)\\ .\\ .\\ .\\ y_o(k+P)\end{array}\right]\end{array}\right.---\left(23\right)$

为了使自主驾驶车辆尽可能沿着期望道路区域的中心线行驶，定义如式(24)所示的参考输入序列R(k)：

$R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right]---\left(24\right)$

式中，y_r(k+i),i＝1,…,P为期望道路区域中心线f(x)的离散量，离散间隔为v(k)·T_s，为了控制车辆尽可能沿着期望道路区域中心线行驶，可通过最小化式(25)中的目标函数来实现：

J₁＝‖Y(k+1|k)-R(k)‖²       (25)

为了使区域型路径跟踪控制模型具有最短化被控车辆行驶路线的功能，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时，可通过最小化由车辆行驶位移构成的目标函数来实现，如式(26)所示：

$J_2=\underset{i=1}{\overset{P}{\Σ}}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(26\right)$

式中，

Δx_d(k+i)＝v(k)·T_s,i＝1,…,P

Δy_d(k+i)＝y_o(k+i)-y_o(k+i-1),i＝1,…,P；

式中，Δx_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的纵向位移，单位，m；Δy_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的侧向位移，单位，m；

为了对控制器的控制动作加以控制，使其不会过大，以保证被控车辆的转向平顺性，将由控制输入序列U(k)构成的式(27)作为控制模型的一个优化目标：

J₃＝‖U(k)‖²       (27)

引入权重系数对J₁、J₂和J₃三个优化目标的需求进行权衡处理，设计的基于模型预测控制的区域型路径跟踪控制模型的优化目标为：

$J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(28\right)$

式中，Γ_y和Γ_u为加权矩阵；Γ_d,i为权重因子；

为了保证被控车辆始终行驶在可行道路区域内，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时对控制系统输出加以约束，结合步骤二中式(6)中的不等式关系，该输出约束可被写成如式(29)所示的形式：

$\{\begin{array}{c}{f}_r^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\\ f_r^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\end{array},i=1,...,P---\left(29\right)$

式中，ψ(k+i)＝C_ψx(k+i)，而C_ψ＝[0 1 0 0]；β(k+i)＝C_βx(k+i)，而C_β＝[0 0 0 1]；f′_l(k+i)为期望道路区域左边界线f′_l(x)在时刻k+i的采样值，单位，m；f_r'(k+i)则为期望道路区域右边界线f_r'(x)在时刻k+i的采样值，单位，m。

为使车辆前轮转角及其变化率不高于转向机构的饱和值，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时考虑如下式所示的控制约束和控制增量约束：

$\left\{\begin{array}{c}\left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\end{array}\right.,i=1,...,N---\left(30\right)$

式中，δ_f(k+i)为k+i时刻的车辆前轮转角，单位，rad；δ_fsat为转向执行机构所能实现的最大的前轮转角，单位，rad；Δδ_f(k+i)＝δ_f(k+i)-δ_f(k+i-1)为k+i时刻的前轮转角增量，单位，rad；Δδ_fsat转向执行机构所能实现的最大的前轮转角增量，单位，rad；

为提高车辆的侧向稳定性，降低其发生侧翻的风险，设计的车辆的区域型路径跟踪控制模型应尽可能使车辆质心侧偏角β不大于车辆发生侧翻的临界值β_rollover，在因此，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时时考虑如下的状态约束：

|β(k+i)|≤β_rollover,i＝1,…,P       (31)

通过整理公式(25)～(31),得到步骤五中的区域路径跟踪控制模型的式(32)，求解式(32)中的优化问题即可获得一个最优的控制序列U^*，结合模型预测控制的基本原理，选取控制量即当前时刻最佳的前轮转角即为步骤五中所述的式(33)：

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right).$

与现有技术相比，本发明具有以下有益效果：

1.本发明建立的二维车辆道路模型时，考虑了车辆和道路的形状与大小，降低车辆与道路边界发生碰撞的可能，提高了自主驾驶车辆的安全性。

2.本发明的控制模型是基于车辆动力学和运动学关系设计的，在低速和高速时同样具有较好的路径跟踪性能。

3.本发明在设计车辆的区域型路径跟踪控制模型时，将车辆稳定性及车辆能耗问题也考虑在内，在保证自主驾驶车辆路径跟踪性能的同时，确保车辆的行驶稳定性，降低其油耗，提高自主车的经济性。

附图说明

图1是本发明所述的一种自主驾驶车辆的区域型路径跟踪控制方法的流程图；

图2是本发明所述的一种自主驾驶车辆的区域型路径跟踪控制方法中建立的二维车辆道路模型的示意图；

图3是本发明所述的一种自主驾驶车辆的区域型路径跟踪控制方法中的车身前后端点与车辆质心的几何关系图；

图4是本发明所述的一种自主驾驶车辆的区域型路径跟踪控制方法中的获取车辆前方一段距离内可行道路区域边界线的原理示意图；

图5是本发明所述的一种自主驾驶车辆的区域型路径跟踪控制方法中的车辆运动学模型示意图；

图6是本发明所述的一种自主驾驶车辆的区域型路径跟踪控制方法中的车辆动力学模型示意图；

图7是本发明实施方式中自主驾驶车辆的区域型路径跟踪控制系统框图；

图8是本发明实施方式中离线仿真实验的道路工况图；

图9a至图9d是本发明实施方式中第一组离线仿真实验的仿真结果，其中图9a为被控车辆的行驶路径，图9b为控制器优化出的控制量，即车辆的前轮转角，图9c为车辆的质心侧偏角，图9d为车辆的横摆角速度；

图10a至图10d是本发明实施方式中第二组离线仿真实验的仿真结果，其中图10a为被控车辆的行驶路径，图10b为控制器优化出的控制量，即车辆的前轮转角，图10c为车辆的质心侧偏角，图10d为车辆的横摆角速度。

具体实施方式

下面结合附图对本发明进行详细说明：

本发明提出一种自主驾驶车辆的区域型路径跟踪控制方法，其具体实施步骤如下：

步骤一、建立二维车辆道路模型：

本发明建立一种新型的二维车辆道路模型，如附图2所示。忽略车辆左右两边的宽度，用过车辆质心o的刚性杆RF表示车辆，刚性杆RF的长度等于车身长度l。期望路径则由期望道路区域左边界线f′_l(x)、期望道路区域右边界线f_r'(x)和期望道路区域中心线f(x)组成的期望道路区域来表示，其中期望道路区域左边界线f′_l(x)和期望道路区域右边界线f_r'(x)可由下式计算得到：

$\left\{\begin{array}{c}{f}_l^{\′}\left(x\right)=f_l\left(x\right)-\frac{w}{2}\\ f_r^{\′}\left(x\right)=f_r\left(x\right)+\frac{w}{2}\\ f\left(x\right)=\frac{f_l\left(x\right)+f_r\left(x\right)}{2}\end{array}\right.---\left(1\right)$

其中，f_l(x)为通过感知系统扫描，然后处理得到的前方一段距离内可行道路区域的左边界；f_r(x)为通过感知系统扫描，然后处理得到的前方一段距离内可行道路区域的右边界；w为车辆宽度，单位，m。

步骤二、建立车辆的区域型路径跟踪控制问题的数学模型：

路径跟踪的目标是使得自主驾驶汽车沿着期望路径行驶。基于本发明建立的二维车辆道路模型，可知本发明中路径跟踪的主要目标是保证刚性杆RF始终处于由期望道路区域左边界线f′_l(x)、期望道路区域右边界线f_r'(x)和期望道路区域中心线f(x)组成的期望道路区域内，所以本发明的路径跟踪问题又名为区域型路径跟踪。

刚性杆RF具有在运动和受力情况下大小和形状不变的特性，所以只要保证刚性杆RF的前端点F和后端点R处于给定的期望道路区域内，整个刚性杆RF就处于该期望道路区域内。因此，本发明所提出的区域型路径跟踪问题，其主要目标是保证式(2)中所描述的关系成立：

$\left\{\begin{array}{c}{f}_r^{\′}\left(x\right)\≤y_F\≤f_l^{\′}\left(x\right)\\ f_r^{\′}\left(x\right)\≤y_R\≤f_l^{\′}\left(x\right)\end{array}\right.---\left(2\right)$

其中，y_F为刚性杆RF前端点F的侧向位置，单位，m；y_R为刚性杆RF后端点R的侧向位置，单位，m。

如附图3所示，刚性杆RF的前端点F和后端点R与质心o存在如下几何关系：

$\left\{\begin{array}{c}{y}_F=y_o+l_f\sin (\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r\sin (\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(3\right)$

其中，y_o为车辆质心o的侧向位置，单位，m；l_f为车辆质心o到车辆前端点F的距离，单位，m；l_r为车辆质心o到车辆后端点R的距离，单位，m；ψ为车辆横摆角，单位，弧度(rad)；β为车辆质心侧偏角，单位，rad。

考虑到自主驾驶车辆的感知系统，其每次可观测到的距离大约为50m，而道路的曲率也大都是比较小的，所以认为车辆在这段区域内行驶时的横摆角ψ是很小的。又考虑到车辆的质心侧偏角β很小，本发明采用如下近似关系：

$\left\{\begin{array}{c}sin(\mathrm{\ψ}+\mathrm{\β})\≈\mathrm{\ψ}+\mathrm{\β}\\ cos(\mathrm{\ψ}+\mathrm{\β})\≈1\end{array}\right.---\left(4\right)$

进而可将公式(3)简化为：

$\left\{\begin{array}{c}{y}_F=y_o+l_f(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(5\right)$

将公式(5)代入到公式(2)中，整理可得本发明提出的车辆的区域型路径跟踪问题的数学模型，如下式所示：

$\{\begin{array}{c}{f}_r^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\\ f_r^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}---\left(6\right)$

步骤三、计算前方一段距离内可行道路区域边界线函数f_l(x)和f_r(x)

本发明假定车辆感知系统可实时获取车身坐标系下的车辆周围道可行道路区域边界的点序列(x_r,y_r,x_l,y_l)。基于此，采用三次拉格朗日插值公式对获得的道路点序列(x_r,y_r,x_l,y_l)进行插值处理，获得车辆前方一段距离内可行道路区域的左边界线f_l(x)和右边界线f_r(x)，具体如式(7)所示：

$\left\{\begin{array}{c}{f}_r\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_r(i\left)\right)}{\left(x_r\right(p)-x_r(i\left)\right)}{y}_r\left(p\right)\\ f_l\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_l(i\left)\right)}{\left(x_l\right(p)-x_l(i\left)\right)}{y}_l\left(p\right)\end{array}\right.,\begin{array}{c}p=j,n,m,k;\\ i=j,n,m,k;\end{array}---\left(7\right)$

式中，(x_r(i),y_r(i),x_l(i),y_l(i)),i＝j,n,m,k为可行道路区域边界的点序列(x_r,y_r,x_l,y_l)中的四组坐标点。这四组坐标点的选取则是基于二次搜索算法进行的，具体如附图4所示。二次搜索的目的是为了获取车辆前方一段距离内可行道路区域的起点与终点二次搜索算法的具体推导过程如下：

在不考虑倒车的情况下，假设被控车辆当前所在的位置坐标为(x_o,y_o)，取作为搜索的起点，则点的横坐标x_r(0)和点的横坐标x_l(0)必为负值。此次搜索的目的是寻找位于车辆质心o后方且在X轴方向上距离点o最近的一组点则点的横坐标x_r(j)和点的横坐标x_l(j)必满足式(8)：

$\left\{\begin{array}{c}{x}_r\left(j\right)\·x_r(j+1)\≤0\\ x_l\left(j\right)\·x_l(j+1)\≤0\end{array}\right.---\left(8\right)$

其中，x_r(j+1)为位于点前方且距离它最近的点的横坐标；x_l(j+1)为位于点前方且距离它最近的点的横坐标。

寻找到满足式(8)的一组点后，将它们的信息存储起来，然后在进行第二次搜索时，以它们作为搜索的起始点。考虑到作用到自主车上的控制信号，其有效作用期一般为1s左右，所以本发明只考虑车辆前方长度为v的一段道路区域，其中v为车辆质心处的速度。因此，本次搜索的目标点的横坐标必定满足式(9)中的不等式关系：

$\left\{\begin{array}{c}\left(x_r\right(k)-v)\·\left(x_r\right(k+1)-v)\≤0\\ \left(x_l\right(k)-v)\·\left(x_l\right(k+1)-v)\≤0\end{array}\right.---\left(9\right)$

其中，x_r(k)为点的横坐标；x_l(k)为点的横坐标；x_r(k+1)为位于点前方且距离它最近的点的横坐标；x_l(k+1)为位于点前方且距离它最近的点的横坐标。

本发明将搜索到的点和作为式(7)的两组插值点，同时选取点和作为另外两组插值点。令j、k、n和m分别代表上述四组插值点在已知道路点序列(x_r,y_r,x_l,y_l)的位置，则存在如下关系：

步骤四、建立车辆系统模型并整理成状态空间形式：

考虑到车辆的运动学和动力学关系分别对低速和高速行驶时的自主驾驶车辆有着重大的影响，本发明在建立车辆系统模型时，同时将车辆的运动学和动力学关系考虑在内。

(1)建立车辆运动学模型

车辆运动学模型的示意图如附图5所示，这里假定车辆是一个刚性体，其中装置着四个不会发生形变的车轮，并以前轮作为转向轮。根据运动学方程以及附图5中所示的几何关系可得车辆的运动学模型如式(11)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=vcos(\mathrm{\ψ}+\mathrm{\β})\\ {\stackrel{\·}{y}}_o=vsin(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(11\right)$

式中，x_o为车辆质心o的纵向位置，单位，m；y_o为车辆质心o的侧向位置，单位，m；v为车辆质心处的速度，单位，m/s；r为车辆的横摆角速度，单位，rad/s。将式(4)代入到式(11)中，则可以得到简化的车辆运动学模型，如式(12)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=v\\ {\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(12\right)$

(2)建立车辆动力学模型

车辆动力学模型的示意图如附图6所示，其中车辆质心o为坐标原点，沿着车身向前的方向为横轴x的正方向，垂直于横轴向上的方向为纵轴y的正方向。由于本发明是通过控制车辆的前轮转角来实现路径跟踪的目的的，所以忽略车辆的纵向动力学，而只考虑车辆的侧向动力学及横摆方向的动力学。根据牛顿第二定律与力矩平衡关系，可得到如式(13)所示的车辆动力学模型：

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f+F_{yr}\\ I_z\stackrel{\·}{r}=a(F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f)-{\mathrm{bF}}_{yr}\end{array}\right.---\left(13\right)$

式中，v_x为车辆质心处的纵向速度，单位，m/s；F_yf为车辆前轮侧向力，单位，N；F_yr为车辆后轮侧向力，单位，N；m为车辆的质量，单位，kg；I_z为车辆绕z轴的转动惯量，单位，kg·m²；a为车辆质心o到车辆前轴的距离，单位，m；b为车辆质心o到车辆后轴的距离，单位，m；δ_f为车辆前轮转角，单位，rad。车辆的前轮转角δ_f很小，所以可将公式(13)简化，简化后的车辆动力学模型如式(14)所示：

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{yf}+F_{yr}\\ I_z\stackrel{\·}{r}={\mathrm{aF}}_{yf}-{\mathrm{bF}}_{yr}\end{array}\right.---\left(14\right)$

假定车辆侧向轮胎力未达到饱和，此时侧向力F_y与轮胎侧偏角α基本呈线性关系，如式(15)所示：

$\left\{\begin{array}{c}{F}_{yf}=2C_f{\mathrm{\α}}_f\\ F_{yr}=2C_r{\mathrm{\α}}_r\end{array}\right.---\left(15\right)$

式中，C_f车辆前轮的轮胎侧偏刚度，单位，N·rad；C_r为车辆后轮的轮胎侧偏刚度，单位，N·rad；α_f为车辆前轮的轮胎侧偏角，单位，rad；α_r为车辆后轮的轮胎侧偏角，单位，rad。根据坐标系的规定，前、后轮的轮胎侧偏角分别为：

$\left\{\begin{array}{c}{\mathrm{\α}}_f=\mathrm{\β}+\frac{ar}{v_x}-{\mathrm{\δ}}_f\\ {\mathrm{\α}}_r=\mathrm{\β}-\frac{br}{v_x}\end{array}\right.---\left(16\right)$

结合式(14)、(15)和(16)，整理可得二自由度的车辆动力学模型，如式(17)所示：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{{\mathrm{mv}}_x}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}_x^2}-1)r-\frac{2C_f}{{\mathrm{mv}}_x}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_z{v}_x}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(17\right)$

(3)建立车辆系统的状态空间模型

结合式(12)和式(17)，同时考虑到则可得车辆系统运动和动力学的微分方程式，具体如式(18)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\\ \stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{mv}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1)r-\frac{2C_f}{mv}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(18\right)$

本发明提出的车辆的区域型路径跟踪问题，其主要目标是通过控制车辆的前轮转角进而保证车辆的侧向位置满足式(6)中的不等式约束。所以，选取车辆质心o的侧向位置y_o作为系统输出，同时选取前轮转角δ_f作为系统控制量，选取系统状态向量x＝[y_o ψ β r]^T。基于此，车辆系统模型可被描述成式(19)所示的状态空间模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}=Ax+{\mathrm{B\δ}}_f\\ y_o=Cx\end{array}\right.---\left(19\right)$

式中，

$A=\left[\begin{array}{cccc}0& v& v& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{2(C_f+C_r)}{mv}& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1\\ 0& 0& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}& \frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ -\frac{2C_f}{mv}\\ -\frac{2{\mathrm{aC}}_f}{I_z}\end{array}\right],C=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right];$

其中，A为系统矩阵；B为输入矩阵；C为输出矩阵。

步骤五、采用模型预测控制方法进行区域型路径跟踪控制模型的设计，得到当前时刻最佳的前轮转角

本发明设计的区域型路径跟踪控制系统的系统框图如附图7所示，表征车辆运行状态的车辆质心的位置(x_o,y_o)、车辆横摆角ψ、车辆的速度v、车辆的横摆角速度r以及车辆的质心侧偏角β都可通过感知系统中装置的高精度GPS传感器RT3002测量得到。考虑到模型预测控制方法在处理约束上的优越性，本发明基于模型预测控制方法进行区域型路径跟踪控制模型的设计。

首先，根据前面建立的区域型路径跟踪问题的数学模型可知，在设计车辆的区域型路径跟踪控制模型时必须对车辆质心的侧向位置进行约束，使其满足式(6)中的不等式关系。其次，考虑到安全性是车辆在行驶过程中必须要关注的一个重要问题，而针对进行区域型路径跟踪的自主车来讲，沿着其前方可行道路区域的中心线行驶明显是最安全的行驶方案，所以设计的车辆的区域型路径跟踪控制模型必须能保证自主车尽可能地行驶在区域中心线上。车辆发生侧向翻转也严重影响着车辆的行驶安全。由经验可知，当车辆质心侧偏角β大于某一量(我们称为侧翻临界点β_rollover)时，车辆就会有发生侧翻的危险，所以在设计车辆的区域型路径跟踪控制模型时必须对车辆质心侧偏角加以约束。再次，考虑到现今广受国内外关注的油耗问题，本发明提出通过最短化车辆的行驶路线来降低能耗进而实现减少油耗的目的。最后，考虑到自主车的行驶平顺性，还需对控制动作的大小进行限制，以避免出现过大的控制动作。车辆的转向机构是一个机械系统，会存在机械饱和的问题，所以若要保证控制器输出的控制量能有效地发挥控制作用，在设计车辆的区域型路径跟踪控制模型时还必须将转向执行机构的饱和问题考虑在内。综上分析，本发明设计的车辆的区域型路径跟踪控制模型需要实现以下一些目标：

目标1)使车辆质心的侧向位置满足式(6)中的约束；

目标2)使车辆尽可能地行驶在期望道路区域的中心线上，以降低车辆与道路边缘或障碍物发生碰撞的危险性；

目标3)使车辆质心侧偏角β不大于车辆发生侧翻的临界值β_rollover；

目标4)使车辆行驶的路径尽可能短，以降低车辆的油耗；

目标5)保证控制器输出的控制量即车辆的前轮转角始终平稳，避免出现过大的控制动作。

目标6)使前轮转角及其变化率不高于转向机构的饱和值；

根据上述控制目标，采用模型预测控制方法进行车辆的区域型路径跟踪控制模型的设计，具体过程如下：

可知，车速是一个缓慢变化的连续量，所以本发明做出如下假设：假设自主驾驶车辆在一个预测时域内保持恒速行驶。

式(19)为车辆系统的微分模型，为用于车辆的区域型路径跟踪控制模型的设计，需要将式(19)离散化，得到离散时间的车辆系统模型，如式(20)所示：

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ y_o\left(k\right)=C_{c}x\left(k\right)\end{array}\right.---\left(20\right)$

式中， $A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C,$ 其中T_s为采样时间。

假定预测时域为P，控制时域为N，且满足N≤P。同时假定控制时域之外的控制量保持不变，即δ_f(k+N-1)＝δ_f(k+N)＝…＝δ_f(k+P-1)，则基于式(20)中离散时间的车辆系统模型，可推导出P步的状态预测方程，具体如(21)所示：

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ x(k+2)=A_{c}x(k+1)+B_c{\mathrm{\δ}}_f(k+1)\\ =A_c^{2}x\left(k\right)+A_c{B}_c{\mathrm{\δ}}_f\left(k\right)+B_c{\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ x(k+N)=A_c^{N}x\left(k\right)+A_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+B_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ x(k+P)=A_c^{P}x\left(k\right)+A_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(21\right)$

同时推导出P步的预测输出，如式(22)所示：

$\left\{\begin{array}{c}{y}_o(k+1)=C_c{A}_{c}x\left(k\right)+C_c{B}_c{\mathrm{\δ}}_f\left(k\right)\\ .\\ .\\ .\\ y_o(k+N)=C_c{A}_c^{N}x\left(k\right)+C_c{A}_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+C_c{B}_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ y_o(k+P)=C_c{A}_c^{P}x\left(k\right)+C_c{A}_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{C}_c{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(22\right)$

定义控制输入序列U(k)和控制预测输出序列Y(k+1|k)分别为：

$\left\{\begin{array}{c}U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right]\\ Y(k+1|k)=\left[\begin{array}{c}{y}_o(k+1)\\ y_o(k+2)\\ .\\ .\\ .\\ y_o(k+P)\end{array}\right]\end{array}\right.---\left(23\right)$

根据上述关于控制目标的分析，可知其中一个控制目标是使自主驾驶车辆尽可能沿着期望道路区域的中心线行驶，所以本发明定义了如式(24)所示的参考输入序列R(k)：

$R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right]---\left(24\right)$

式中，y_r(k+i),i＝1,…,P为期望道路区域中心线f(x)的离散量，离散间隔为v(k)·T_s。那么，控制车辆尽可能沿着期望道路区域中心线行驶的这一控制目标可通过最小化式(25)中的目标函数来实现：

J₁＝‖Y(k+1|k)-R(k)‖²      (25)

目标4)指出：区域型路径跟踪控制模型应具有最短化被控车辆行驶路线的功能，以此来实现节能减排的目的。

在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时，这一控制需求可通过最小化由车辆行驶位移构成的目标函数来实现，如式(26)所示：

$J_2=\underset{i=1}{\overset{P}{\Σ}}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(26\right)$

式中，

Δx_d(k+i)＝v(k)·T_s,i＝1,…,P

Δy_d(k+i)＝y_o(k+i)-y_o(k+i-1),i＝1,…,P；

其中，Δx_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的纵向位移，单位，m；Δy_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的侧向位移，单位，m。同时目标5)指出：应对控制器输出的控制量即车辆前轮转角的大小加以控制，使其不会过大，以保证被控车辆的转向平顺性。因此，本发明将由控制输入序列U(k)构成的式(27)作为控制模型的一个优化目标：

J₃＝‖U(k)‖²         (27)

对于这样一个具有J₁、J₂和J₃三个优化目标的优化问题，需引入权重系数来对各个优化目标的需求冲突进行衡量和处理，以获得一个最合适的优化结果。因此，本发明设计的基于模型预测控制的区域型路径跟踪控制模型的优化目标为：

$J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(28\right)$

式中，Γ_y和Γ_u为加权矩阵；Γ_d,i为权重因子。

又知，区域型路径跟踪控制需要实现的最重要的一个控制目标是保证被控车辆始终行驶在可行道路区域内。为此，本发明在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时对系统输出加以约束。结合式(6)中的不等式关系，该输出约束可被写成如式(29)所示的形式：

$\{\begin{array}{c}{f}_r^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\\ f_r^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\end{array},i=1,...,P---\left(29\right)$

式中，ψ(k+i)＝C_ψx(k+i)，而C_ψ＝[0 1 0 0]；β(k+i)＝C_βx(k+i)，而C_β＝[0 0 0 1]；f′_l(k+i)为期望道路区域左边界线f′_l(x)在时刻k+i的采样值，单位，m；f_r'(k+i)则为期望道路区域右边界线f_r'(x)在时刻k+i的采样值，单位，m。

车辆的转向执行机构存在机械饱和的情况，过大或过快的控制量不能被有效地作用到被控车辆上。为避免控制器给出无效的控制量，本发明提出了目标6)：使前轮转角及其变化率不高于转向机构的饱和值，这则需要在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时考虑如下式所示的控制约束和控制增量约束：

$\left\{\begin{array}{c}\left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\end{array}\right.,i=1,...,N---\left(30\right)$

式中，δ_f(k+i)为k+i时刻车辆前轮转角，单位，rad；δ_fsat为转向执行机构所能实现的最大的前轮转角，单位，rad；Δδ_f(k+i)＝δ_f(k+i)-δ_f(k+i-1)为k+i时刻的前轮转角增量，单位，rad；Δδ_fsat转向执行机构所能实现的最大的前轮转角增量，单位，rad。同时为提高车辆的侧向稳定性，降低其发生侧翻的风险，本发明提出设计的区域型路径跟踪控制模型应尽可能使车辆质心侧偏角β不大于车辆发生侧翻的临界值β_rollover。因此，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时考虑如下的状态约束：

|β(k+i)|≤β_rollover,i＝1,…,P     (31)

综上所述，本发明采用模型预测控制方法设计的车辆的区域型路径跟踪控制模型可被整理成以下形式：

$\left\{\begin{array}{c}\underset{{\mathrm{\δ}}_f(k+i)}{\min }J\\ J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)\end{array}\right.---\left(32\right)$

满足: $\left\{\begin{array}{c}x(k+i+1)=A_{c}x(k+i)+B_c{\mathrm{\δ}}_f(k+i)\\ y_o(k+i)=C_{c}x(k+i)\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\\ \left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ f_r^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ f_r^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ \left|C_{\mathrm{\β}}x(k+i)\right|\≤\left|{\mathrm{\β}}_{rollover}\right|\end{array}\right.$

式中：

$A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C;$

Δx_d(k+i)＝v·T_s；

Δy_d(k+i)＝y_o(k+i)-y_o(k+i-1)；

Δδ_f(k+i)＝δ_f(k+i)-δ_f(k+i-1)；

C_ψ＝[0 1 0 0],C_β＝[0 0 1 0]；

求解式(32)中的优化问题即可获得一个最优的控制序列U^*，结合模型预测控制的基本原理，本发明选取控制量即当前时刻最佳的前轮转角为：

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right)$

即选取最优控制序列U^*的第一个量作为控制量作用到被控车辆上。到下一时刻，基于模型预测控制的区域型路径跟踪控制模型将根据当前车辆状态重新计算一个最优控制量，以此往复，即实现了滚动优化控制。

步骤六、根据步骤五中给出的当前时刻最佳的前轮转角控制转向执行机构动作，使得被控车辆的前轮转角等于当前时刻最佳的前轮转角从而使被控车辆在车辆感知系统给出的车辆前方某一段距离内可行道路区域内行驶，实现区域路径跟踪的控制目标。

为验证所设计的路径跟踪控制模型的有效性，在Matlab/Simulink环境下搭建基于模型预测控制的区域型路径跟踪控制模型，采用高精度的车辆动力学仿真软件veDYNA作为被控对象，进行离线仿真并分析仿真结果。

表1HQ430车辆模型参数

仿真实验中所采用的车辆系统模型为红旗HQ430模型，其主要参数如表1所示。实验道路工况如附图8所示，为沥青道路。为充分验证所设计的路径跟踪控制模型的性能，本发明共进行了两组仿真实验；

一组是在干燥的沥青路面工况下进行的，路面与轮胎的摩擦系数μ取为0.9。在实验过程中，车辆首先进行启动加速，然后分别以60km/h和85km/h的纵向速度进行恒速行驶。

另一组则是在潮湿的沥青路面工况下进行的，路面与轮胎的摩擦系数μ取为0.6。在实验过程中，车辆首先进行启动加速，然后分别以60km/h和82km/h的纵向速度进行恒速行驶。

两组仿真结果分别如附图9和附图10所示：附图9为第一组仿真的仿真结果，即在干燥沥青道路工况(μ＝0.9)下的仿真结果，其中图9a为被控车辆的行驶路径，图9b为控制器优化出的控制量，即车辆的前轮转角，图9c为车辆的质心侧偏角，图9d为车辆的横摆角速度；附图10为第二组的在潮湿沥青路面工况(μ＝0.6)下的仿真结果，其中图10a为被控车辆的行驶路径，图10b为控制器优化出的控制量，即车辆的前轮转角，图10c为车辆的质心侧偏角，图10d为车辆的横摆角速度。

经过离线仿真，可以看出本发明设计的基于模型预测控制的区域型路径跟踪控制模型可以控制被控车辆始终行驶在给定的道路区域内，优化出的前轮转角低于转向机构的饱和值，同时可以保证被控车辆的行驶安全性和侧向稳定性，且对路面摩擦系数的变化具有一定的鲁棒性。

Claims (5)

1.一种自主驾驶车辆的区域型路径跟踪控制方法，自主驾驶车辆中的驾驶控制系统首先根据感知系统扫描处理后给出的可行道路区域信息以及车辆自身的运行状态信息优化出当前时刻最佳的前轮转角，然后根据该前轮转角控制车辆的转向执行机构动作，以使车辆在可行的道路区域内行驶，其特征在于步骤如下：

步骤一、建立二维车辆道路模型：

建立二维车辆道路模型，设刚性杆RF为车辆模型，它过车辆的质心o，长度等于车身长度l，期望道路区域则由期望道路区域左边界线f_l'(x)、期望道路区域右边界线f_r'(x)和期望道路区域中心线f(x)组成的期望道路区域来表示，且满足：

$\left\{\begin{array}{c}{f}_l^{\′}\left(x\right)=f_l\left(x\right)-\frac{w}{2}\\ f_r^{\′}\left(x\right)=f_r\left(x\right)+\frac{w}{2}\\ f\left(x\right)=\frac{f_l\left(x\right)+f_r\left(x\right)}{2}\end{array}\right.---\left(1\right)$

式中，f_l(x)为通过自主驾驶车辆中的感知系统扫描处理得到的前方一段距离内可行道路区域的左边界；f_r(x)为通过自主驾驶车辆中的感知系统扫描处理得到的前方一段距离内可行道路区域的右边界；f_l'(x)为期望道路区域左边界线，f_r'(x)为期望道路区域右边界线，w为车辆宽度，单位，m；

步骤二、建立车辆的区域型路径跟踪问题的数学模型：

基于步骤一建立的二维车辆道路模型，保证刚性杆RF始终处于由期望道路区域左边界线f_l'(x)、期望道路区域右边界线f_r'(x)和期望道路区域中心线f(x)组成的期望道路区域内，结合刚性杆的几何和物理特性，建立车辆的区域型路径跟踪问题的数学模型如下：

$\{\begin{array}{c}{f}_r^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\\ f_r^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}---\left(6\right)$

式中，y_o为车辆质心o的侧向位置，单位，m；l_f为车辆质心o到车辆前端点F的距离，单位，m；l_r为车辆质心o到车辆后端点R的距离，单位，m；ψ为车辆横摆角，单位，rad；β为车辆质心侧偏角，单位，rad；

步骤三、计算车辆前方一段距离内可行道路区域边界线f_l(x)和f_r(x)：

假定自主驾驶车辆的感知系统可实时获取车辆附近的可行道路区域边界的点序列信息(x_r,y_r,x_l,y_l)，基于二次搜索算法采用三次拉格朗日插值公式得到车辆前方一段距离内可行道路区域的边界线函数为：

$\left\{\begin{array}{c}{f}_r\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_r(i\left)\right)}{\left(x_r\right(p)-x_r(i\left)\right)}{y}_r\left(p\right)\\ f_l\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_l(i\left)\right)}{\left(x_l\right(p)-x_l(i\left)\right)}{y}_l\left(p\right)\end{array}\right.,\begin{array}{c}p=j,n,m,k;\\ i=j,n,m,k;\end{array}---\left(7\right)$

式中，(x_r(i),y_r(i),x_l(i),y_l(i)),i＝j,n,m,k为可行道路区域边界的点序列(x_r,y_r,x_l,y_l)中的四组坐标点；

步骤四、建立自主驾驶车辆系统模型并将其整理为状态空间形式：

$\{\begin{array}{c}\stackrel{\·}{x}=Ax+{\mathrm{B\δ}}_f\\ y_o=Cx\end{array}---\left(19\right)$

式中，

$A=\left[\begin{array}{cccc}0& v& v& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{2(C_f+C_r)}{mv}& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1\\ 0& 0& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}& \frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ -\frac{2C_f}{mv}\\ -\frac{2{\mathrm{aC}}_f}{I_z}\end{array}\right],C=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right];$

式中，x为系统状态向量，且x＝[y_o ψ β r]^T；δ_f为车辆前轮转角，也为系统控制量，单位，rad；y_o为系统输出；A为系统矩阵；B为输入矩阵；C为输出矩阵；v为车辆质心处的速度，单位，m/s；r为车辆的横摆角速度，单位，rad/s；C_f为车辆前轮轮胎的侧偏刚度，单位，N/rad；C_r分别为车辆后轮轮胎的侧偏刚度，单位，N/rad；m为车辆的质量，单位，kg；I_z为车辆绕z轴的转动惯量，单位，kg·m²；a为车辆质心o到车辆前轴的距离，单位，m；b为车辆质心o到车辆后轴的距离，单位，m；

步骤五、采用模型预测控制方法设计车辆的区域型路径跟踪控制模型为：

$\left\{\begin{array}{c}\underset{{\mathrm{\δ}}_f(k+i)}{\min J}\\ J=\left|\right|{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)|{|}^2+|\left|{\mathrm{\Γ}}_{u}U\left(k\right)\right|{|}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}\left(\right|\left|{\mathrm{\Δx}}_d\right(k+i\left)\right|{|}^2+\left|\right|{\mathrm{\Δy}}_d\left(k+i\right)\left|{|}^2\right)\end{array}\right.---\left(32\right)$

满足: $\left\{\begin{array}{c}x(k+i+1)=A_{c}x(k+i)+B_c{\mathrm{\δ}}_f(k+i)\\ y_o(k+i)=C_{c}x(k+i)\\ \left|\mathrm{\Δ}{\mathrm{\δ}}_f\right(k+i\left)\right|\≤\mathrm{\Δ}{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ f_r^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ f_r^{\′}(k+i)+l_r(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_r(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ \left|C_{\mathrm{\β}}x\right(k+i\left)\right|\≤\left|{\mathrm{\β}}_{rollover}\right|\end{array}\right.$

式中：

$Y(k+1|k)=\left[\begin{array}{c}{y}_0(k+1)\\ y_0(k+2)\\ .\\ .\\ .\\ y_0(k+P)\end{array}\right],R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right],U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right];$

$A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C;$

Δx_d(k+i)＝v(k)·T_s；

Δy_d(k+i)＝y_o(k+i)-y_o(k+i-1)；

Δδ_f(k+i)＝δ_f(k+i)-δ_f(k+i-1)；

C_ψ＝[0 1 0 0],C_β＝[0 0 1 0]；

并选取控制量即当前时刻最佳的前轮转角为：

${\mathrm{\δ}}_f^{*}=U*\left(1\right)---\left(33\right)$

式中δ_f(k+i)为k+i时刻的系统控制量，即为车辆的前轮转角，单位，rad；

x(k+i)为k+i时刻的系统状态向量；

y_o(k+i)为k+i时刻的系统输出，即车辆质心的侧向位置，单位，m；

P为预测时域，N为控制时域；

Γ_y和Γ_u为加权矩阵；

Γ_d,i为权重因子；

y_r(k+i),i＝1,…,P为期望道路区域中心线f(x)的离散量，离散间隔为v(k)·T_s，单位，m；

Δx_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的纵向位移，单位，m；

Δy_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的侧向位移，单位，m；

Δδ_f(k+i)为k+i时刻的控制增量，单位，rad；

δ_fsat为转向执行机构所能实现的最大的前轮转角，单位，rad；

Δδ_fsat转向执行机构所能实现的最大的前轮转角增量，单位，rad；

f_l'(k+i)为期望道路区域左边界线f_l'(x)在时刻k+i的采样值，单位，m；

f_r'(k+i)则为期望道路区域右边界线f_r'(x)在时刻k+i的采样值，单位，m；

β_rollover为车辆可能发生侧翻的临界量，单位，rad；

T_s为采样时间，单位s；

U^*为通过优化求解得到的最优控制序列；

步骤六、根据步骤五中给出的当前时刻最佳的前轮转角控制转向执行机构动作，使得被控车辆的前轮转角等于当前时刻最佳的前轮转角从而使被控车辆在车辆感知系统给出的车辆前方一段距离内可行道路区域内行驶，实现区域路径跟踪的控制目标。

2.按照权利要求1所述的一种自主驾驶车辆的区域型路径跟踪控制方法，其特征在于，步骤二的具体过程为：

为保证刚性杆RF始终处于由期望道路区域左边界线f_l'(x)、期望道路区域右边界线f_r'(x)和期望道路区域中心线f(x)组成的期望道路区域内，需保证下述的式(2)中描述的关系成立：

$\left\{\begin{array}{c}{f}_r^{\′}\left(x\right)\≤y_F\≤f_l^{\′}\left(x\right)\\ f_r^{\′}\left(x\right)\≤y_R\≤f_l^{\′}\left(x\right)\end{array}\right.---\left(2\right)$

式中，y_F为刚性杆RF前端点F的侧向位置，单位，m；y_R为刚性杆RF后端点R的侧向位置，单位，m；

刚性杆RF的前端点F和后端点R与质心o存在如下几何关系：

$\left\{\begin{array}{c}{y}_F=y_o+l_{f}sin(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r\sin (\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(3\right)$

式中，y_o为车辆质心o的侧向位置，单位，m；l_f为车辆质心o到车辆前端点F的距离，单位，m；l_r为车辆质心o到车辆后端点R的距离，单位，m；ψ为车辆横摆角，单位，弧度(rad)；β为车辆质心侧偏角，单位，rad；

考虑到自主驾驶车辆的感知系统每次可观测到的距离大约为50m，而道路的曲率也大都是比较小的，所以认为车辆在这段区域内行驶时的横摆角ψ是很小的，又考虑到车辆的质心侧偏角β很小，本发明采用如下近似关系：

$\left\{\begin{array}{c}\sin (\mathrm{\ψ}+\mathrm{\β})\≈\mathrm{\ψ}+\mathrm{\β}\\ \cos (\mathrm{\ψ}+\mathrm{\β})\≈1\end{array}\right.---\left(4\right)$

进而可将公式(3)简化为：

$\left\{\begin{array}{c}{y}_F=y_o+l_f(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(5\right)$

将公式(5)代入到公式(2)中，整理即可得到步骤二中所述的车辆的区域型路径跟踪问题的数学模型式(6)。

3.按照权利要求1所述的一种自主驾驶车辆的区域型路径跟踪控制方法，其特征在于，步骤三的具体过程为：

步骤三中的式(7)中，(x_r(i),y_r(i),x_l(i),y_l(i)),i＝j,n,m,k为可行道路区域边界的点序列(x_r,y_r,x_l,y_l)中的四组坐标点，这四组坐标点的选取则是基于二次搜索算法进行的，二次搜索的目的是为了获取车辆前方一段距离内的可行道路区域的起点与终点二次搜索算法的具体推导过程如下：

在不考虑倒车的情况下，假设被控车辆当前所在的位置坐标为(x_o,y_o)，取作为搜索的起点，则点的横坐标x_r(0)和点的横坐标x_l(0)必为负值，此次搜索的目的是寻找位于车辆质心o后方且在X轴方向上距离点o最近的一组点则点的横坐标x_r(j)和点的横坐标x_l(j)必满足公式(8)：

$\left\{\begin{array}{c}{x}_r\left(j\right)\·x_r(j+1)\≤0\\ x_l\left(j\right)\·x_l(j+1)\≤0\end{array}\right.---\left(8\right)$

式中，x_r(j+1)为位于点前方且距离它最近的点的横坐标；x_l(j+1)为位于点前方且距离它最近的点的横坐标；

寻找到满足式(8)的一组点后，将它们的信息存储起来，然后在进行第二次搜索时，以它们作为搜索的起始点，考虑到作用到自主车上的控制信号，其有效作用期一般为1s左右，只考虑车辆前方长度为v的一段道路区域，式中v为车辆质心处的速度，因此，本次搜索的目标点的横坐标必定满足式(9)中的不等式关系：

$\left\{\begin{array}{c}\left(x_r\right(k)-v)\·\left(x_r\right(k+1)-v)\≤0\\ \left(x_l\right(k)-v)\·\left(x_l\right(k+1)-v)\≤0\end{array}\right.---\left(9\right)$

式中，x_r(k)为点的横坐标；x_l(k)为点的横坐标；x_r(k+1)为位于点前方且距离它最近的点的横坐标；x_l(k+1)为位于点前方且距离它最近的点的横坐标；

将搜索到的点和作为式(7)的两组插值点，同时选取点和作为式(7)的另外两组插值点，令j、k、n和m分别代表上述四组插值点在已知道路点序列(x_r,y_r,x_l,y_l)的位置，j、k、n和m之间的关系如下：

4.按照权利要求1所述的一种自主驾驶车辆的区域型路径跟踪控制方法，其特征在于，步骤四的具体过程为：

(1)建立车辆运动学模型

假定车辆是一个刚性体，其中装置着四个不会发生形变的车轮，并以车辆前轮作为转向轮，根据运动学方程以及几何关系得到车辆的运动学模型如公式(11)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=vcos(\mathrm{\ψ}+\mathrm{\β})\\ {\stackrel{\·}{y}}_o=vsin(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(11\right)$

式中，x_o为车辆质心o的纵向位置，单位，m；y_o为车辆质心o的侧向位置，单位，m；v为车辆质心处的速度，单位，m/s；r为车辆的横摆角速度，单位，rad/s，将式(4)代入到式(11)中，则可以得到简化的车辆运动学模型，如式(12)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=v\\ {\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(12\right)$

(2)建立车辆动力学模型

设车辆动力学模型中车辆质心o为坐标原点，沿着车身向前的方向为横轴x的正方向，垂直于横轴向上的方向为纵轴y的正方向，本方法是通过控制车辆的正方向来实现路径跟踪的目的的，所以忽略车辆的纵向动力学，只考虑车辆的侧向动力学及横摆方向的动力学，根据牛顿第二定律与力矩平衡关系，可得到如式(13)所示的车辆动力学模型：

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{xf}{\mathrm{sin\β}}_f+F_{yf}{\mathrm{cos\β}}_f+F_{yr}\\ I_z\stackrel{\·}{r}=a(F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f)-{\mathrm{bF}}_{yr}\end{array}\right.---\left(13\right)$

式中，v_x为车辆质心处的纵向速度，单位，m/s；F_yf为车辆前轮侧向力，单位，N；F_yr为车辆后轮侧向力，单位，N；m为车辆的质量，单位，kg；I_z为车辆绕z轴的转动惯量，单位，kg·m²；a为车辆质心o到车辆前轴的距离，单位，m；b为车辆质心o到车辆后轴的距离，单位，m；δ_f为车辆前轮转角，单位，rad，车辆的前轮转角δ_f很小，可将公式(13)简化，简化后的车辆动力学模型如式(14)所示：

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{yf}+F_{yr}\\ I_z\stackrel{\·}{r}={\mathrm{aF}}_{yf}-{\mathrm{bF}}_{yr}\end{array}\right.---\left(14\right)$

假定车辆侧向轮胎力未达到饱和，此时侧向力F_y与轮胎侧偏角α基本呈线性关系，如式(15)所示：

$\left\{\begin{array}{c}{F}_{yf}=2C_f{\mathrm{\α}}_f\\ F_{yr}=2C_r{\mathrm{\α}}_r\end{array}\right.---\left(15\right)$

式中，C_f为车辆前轮的轮胎侧偏刚度，单位，N·rad；C_r为车辆后轮的轮胎侧偏刚度，单位，N·rad；α_f为车辆前轮的轮胎侧偏角，单位，rad；α_r为车辆后轮的轮胎侧偏角，单位，rad，根据坐标系的规定，前轮的轮胎侧偏角α_f和后轮的轮胎侧偏角α_r分别为：

$\left\{\begin{array}{c}{\mathrm{\α}}_f=\mathrm{\β}+\frac{ar}{v_x}-{\mathrm{\δ}}_f\\ {\mathrm{\α}}_f=\mathrm{\β}+\frac{br}{v_x}\end{array}\right.---\left(16\right)$

结合式(14)、(15)和(16)，整理可得二自由度的车辆动力学模型，如式(17)所示：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{{\mathrm{mv}}_x}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}_x^2}-1)r-\frac{2C_f}{{\mathrm{mv}}_x}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f+{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_z{v}_x}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(17\right)$

(3)建立车辆系统的状态空间模型

结合式(12)和式(17)，同时考虑到则可得车辆系统运动和动力学的微分方程式，具体如式(18)所示：

$\left\{\begin{array}{c}{\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\\ \stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{mv}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1)r-\frac{2C_f}{mv}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(18\right)$

通过控制车辆的前轮转角进而保证车辆的侧向位置满足式(6)中的不等式约束，选取车辆质心o的侧向位置y_o作为系统输出，同时选取前轮转角δ_f作为系统控制量，系统状态向量选取为x＝[y_o ψ β r]，车辆系统模型可被描述成步骤四中式(19)所示的状态空间模型。

5.按照权利要求1所述的一种自主驾驶车辆的区域型路径跟踪控制方法，其特征在于，步骤五的具体过程为：

假设自主驾驶车辆在一个预测时域内保持恒速行驶，步骤四中的式(19)为车辆系统的微分模型，为了用于车辆的区域型路径跟踪控制模型的设计，需要将式(19)离散化，得到离散时间的车辆系统模型，如式(20)所示：

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ y_o\left(k\right)=C_{c}x\left(k\right)\end{array}\right.---\left(20\right)$

式中， $A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C,$ 式中T_s为采样时间；

假定预测时域为P，控制时域为N，且满足N≤P，同时假定控制时域之外的控制量保持不变，即δ_f(k+N-1)＝δ_f(k+N)＝…＝δ_f(k+P-1)，则基于式(20)中离散时间的车辆系统模型推导出P步的状态预测方程，具体如(21)所示：

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ x(k+2)=A_{c}x(k+1)+B_c{\mathrm{\δ}}_f(k+1)\\ =A_c^{2}x\left(k\right)+A_c{B}_c{\mathrm{\δ}}_f\left(k\right)+B_c{\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ x(k+N)=A_c^{N}x\left(k\right)+A_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+B_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ x(k+P)=A_c^{P}x\left(k\right)+A_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(21\right)$

同时推导出P步的预测输出，如式(22)所示：

$\{\begin{array}{c}{y}_0(k+1)=C_c{A}_{c}x\left(k\right)+C_c{B}_c{\mathrm{\δ}}_f\left(k\right)\\ .\\ .\\ .\\ y_0(k+N)=C_c{A}_c^{N}x\left(k\right)+C_c{A}_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+C_c{B}_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ y_0(k+P)=C_c{A}_c^{P}x\left(k\right)+C_c{A}_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{C}_c{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}---\left(22\right)$

定义控制输入序列U(k)和控制预测输出序列Y(k+1|k)分别为：

$\left\{\begin{array}{c}U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right]\\ Y(k+1|k)=\left[\begin{array}{c}{y}_0(k+1)\\ y_0(k+2)\\ .\\ .\\ .\\ y_0(k+P)\end{array}\right]\end{array}\right.---\left(23\right)$

为了使自主驾驶车辆尽可能沿着期望道路区域的中心线行驶，定义如式(24)所示的参考输入序列R(k)：

$R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right]---\left(24\right)$

式中，y_r(k+i),i＝1,…,P为期望道路区域中心线f(x)的离散量，离散间隔为v(k)·T_s，为了控制车辆尽可能沿着期望道路区域中心线，可通过最小化式(25)中的目标函数来实现：

J₁＝‖Y(k+1|k)-R(k)‖²                 (25)

为了使区域型路径跟踪控制模型具有最短化被控车辆行驶路线的功能，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时，可通过最小化由车辆行驶位移构成的目标函数来实现，如式(26)所示：

$J_2=\underset{i=1}{\overset{P}{\Σ}}\left(\right|\left|{\mathrm{\Δx}}_d\right(k+i)|{|}^2+|\left|{\mathrm{\Δy}}_d(k+i)\right|{|}^2)---\left(26\right)$

式中，

Δx_d(k+i)＝v(k)·T_s,i＝1,…,P

Δy_d(k+i)＝y_o(k+i)-y_o(k+i-1),i＝1,…,P；

式中，Δx_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的纵向位移，单位，m；Δy_d(k+i)为车辆在(k+i-1)～(k+i)这一段时间内行驶的侧向位移，单位，m；

为了对控制器的控制动作加以控制，使其不会过大，以保证被控车辆的转向平顺性，将由控制输入序列U(k)构成的式(27)作为控制模型的一个优化目标：

J₃＝‖U(k)‖²                    (27)

引入权重系数对J₁、J₂和J₃三个优化目标的需求进行权衡处理，设计的车辆的区域型路径跟踪控制模型的优化目标为：

$J=\left|\right|{\mathrm{\Γ}}_y\left(Y\right(k+1\left|k\right)-R\left(k\right)\left)\right|{|}^2+\left|\right|{\mathrm{\Γ}}_{u}U\left(k\right)|{|}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}\left(\right|\left|{\mathrm{\Δx}}_d\right(k+i)|{|}^2+\left|\right|{\mathrm{\Δy}}_d(k+i)\left|{|}^2\right)---\left(28\right)$

式中，Γ_y和Γ_u为加权矩阵；Γ_d,i为权重因子；

为了保证被控车辆始终行驶在可行道路区域内，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时对系统输出加以约束，结合步骤二中式(6)中的不等式关系，该输出约束可被写成如式(29)所示的形式：

$\left\{\begin{array}{c}{f}_r^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\\ f_r^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\end{array}\right.,i=1,...,P---\left(29\right)$

式中，ψ(k+i)＝C_ψx(k+i)，而C_ψ＝[0 1 0 0]；β(k+i)＝C_βx(k+i)，而C_β＝[0 0 0 1]；f_l'(k+i)为期望道路区域左边界线f_l'(x)在时刻k+i的采样值，单位，m；f_r'(k+i)则为期望道路区域右边界线f_r'(x)在时刻k+i的采样值，单位，m；

为使车辆前轮转角及其变化率不高于转向机构的饱和值，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时考虑如下式所示的控制约束和控制增量约束：

$\left\{\begin{array}{c}\left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\end{array}\right.,i=1,...,N---\left(30\right)$

式中，δ_f(k+i)为k+i时刻车辆前轮转角，单位，rad；δ_fsat为转向执行机构所能实现的最大的前轮转角，单位，rad；Δδ_f(k+i)＝δ_f(k+i)-δ_f(k+i-1)为k+i时刻的前轮转角增量，单位，rad；Δδ_fsat转向执行机构所能实现的最大的前轮转角增量，单位，rad；

为提高车辆的侧向稳定性，降低其发生侧翻的风险，设计的区域型路径跟踪控制模型应尽可能使车辆质心侧偏角β不大于车辆发生侧翻的临界值β_rollover，在因此，在采用模型预测控制方法设计车辆的区域型路径跟踪控制模型时考虑如下的状态约束：

|β(k+i)|≤β_rollover,i＝1,…,P           (31)

通过整理公式(25)～(31),得到步骤五中的区域路径跟踪控制模型的式(32)，求解式(32)中的优化问题即可获得一个最优的控制序列U^*，结合模型预测控制的基本原理，选取控制量即当前时刻最佳的前轮转角即为步骤五中所述的式(33)：

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right).$