基于预测模型的自动驾驶汽车稳定性路径跟踪方法

1.本发明涉及车辆自动驾驶领域，特别是涉及一种基于预测模型的自动驾驶汽车稳定性路径跟踪方法。


背景技术：

2.随着汽车电动化、信息化、智能化的发展，自动驾驶汽车已经成为全球汽车行业研究的热点。与传统汽车相比，自动驾驶汽车在减少交通事故、提升出行效率、提高行驶安全性等方面具有较大的优势。随着自动驾驶技术的发展，自动驾驶汽车已经基本能够在简单的道路环境中低速行驶。如何提高自动驾驶汽车行驶速度和复杂环境的适应能力，使其能够在复杂工况下安全行驶，是未来自动驾驶汽车技术发展的重要目标。
3.当自动驾驶车辆高速行驶时，一旦进入失控或侧倾等失稳状态，很难通过制动减速来规避危险，即使是车辆本身的稳定性控制系统发挥作用也难以恢复车辆的行驶能力。与低速工况不同，自动驾驶汽车在高速工况等复杂工况下安全行驶不仅要考虑车辆非完整性约束及可行驶道路约束，还要考虑操纵稳定性相关的非线性动力学约束，这使得自动驾驶汽车在复杂工况下稳定性路径跟踪控制方法极具挑战性。


技术实现要素：

4.基于此，有必要针对自动驾驶汽车在复杂工况下行驶稳定性较差的问题，提供一种基于预测模型的自动驾驶汽车稳定性路径跟踪方法。
5.一种基于预测模型的自动驾驶汽车稳定性路径跟踪方法，所述方法包括：
6.建立包括车辆动力学模型与跟踪误差模型的自动驾驶汽车的车辆模型与预测模型；
7.自动驾驶汽车执行路径规划，并基于贝塞尔曲线对规划出的参考路径分段拟合，实时计算道路曲率；
8.基于所述预测模型构建表达控制输出预测值与控制输出参考值之间的误差和控制增量的目标函数；
9.求解所述目标函数，获得最优控制量，并输入所述车辆模型。
10.进一步的，所述车辆动力学模型表示为：
[0011][0012][0013]
[0014][0015]
式中，m为车辆质量，v
x
、vy分别为车体质心处的纵向速度和横向速度，为横摆角速度，β为质心侧偏角，iz为车身绕z轴的转动惯量，lf、lr分别为车辆质心到前轴和后轴的距离，c
αf
、c
αf
为轮胎侧偏刚度，c
lf
、c
lr
为轮胎纵向刚度，αf、αr为前轮和后轮轮胎侧偏角，sf、sr为前轮和后轮轮胎滑移率，δf为前轮转角。
[0016]
进一步的，所述车辆跟踪误差模型表示为：
[0017][0018][0019]
式中，横向位置误差ey为车辆后轴中心在道路中心线上投影点之间的距离，航向误差为道路中心线切向与道路地面坐标系x的夹角，κ为参考路径的道路曲率。
[0020]
进一步的，所述建立预测模型，包括：
[0021]
对车辆动力学模型和车辆跟踪误差模型进行线性化处理，获得车辆跟踪控制模型：
[0022][0023]
式中，状态向量为输入向量为u1＝δf，附加输入向量为u2＝κ；a,b1,b2为雅可比矩阵，其中，
[0024][0025][0026]
b2(t)＝[0 0 0 0 0
ꢀ‑vx
]
t
；
[0027]
对车辆跟踪控制模型采用一阶差商的方法进行离散化处理，得到离散的状态空间表达式：
[0028]
ξ(k 1)＝akξ(k) bku1(k) cku2(k)，
[0029]
式中，ak＝i ta(t),bk＝tb1(t),ck＝tb2(t)，i为六阶单位矩阵；
[0030]
构建新的状态向量ξ(k|t)＝[ξ(k) u1(k-1)]
t
，根据离散的状态空间表达式得到新
的状态空间方程：
[0031][0032]
式中，
[0033]
根据新的状态空间方程进行状态预测，获得未来时刻的所述预测模型：
[0034]
y＝ψξ(k) θ
△
u1 υu2，
[0035]
其中，
[0036]
y＝[ξ(k 1) ξ(k 2)
ꢀ…ꢀ
ξ(k nc)
ꢀ…ꢀ
ξ(k n
p
)]
t
,
[0037][0038][0039][0040]
△
u1＝[
△
u1(k)
ꢀ△
u1(k 1)
ꢀ…ꢀ△
u1(k nc)]
t
,
[0041]
u2＝[u2(k) u2(k 1)
ꢀ…ꢀ
u2(k nc)]
t
,
[0042]
式中，n
p
为预测时域，nc为控制时域，
△
u1为控制增量。
[0043]
进一步的，所述参考路径由一系列离散路点构成，采用三阶贝塞尔曲线分段拟合表示为：
[0044]
q(τi)＝(1-τi)3p0 3τi(1-τi)2p1 3τ
i2
(1-τi)2p2 τ
i3
p3，
[0045]
式中，q(τi)为参数τi处的插值点，pk为第k个控制点，通过对参数τi在[0,1]内取值，可以在第一个控制点和最后一个控制点之间生成任意个插值点；
[0046]
根据每个拟合路段的插值点计算道路曲率：
[0047][0048]
式中，x
′
＝x
c-xa，y
′
＝y
c-ya，x
″
＝xc x
a-2xb，y
″
＝yc y
a-2yb，η＝(x
′
)2 (y
′
)2，(xa,
ya),(xb,yb),(xc,yc)为每个拟合路段的插值点。
[0049]
进一步的，所述目标函数为：
[0050][0051]
式中，q
ey
,r为权重矩阵，ρ为权重系数，ε2为权重因子。
[0052]
进一步的，还包括构建目标函数的约束条件的步骤：
[0053]
建立由横摆角速度、质心侧偏角构成的稳定性包络边界，对横摆角速度和质心侧偏角进行稳定性约束；
[0054]
建立可行道路区域边界，对横向位置误差进行约束；
[0055]
对控制输入量和轮胎滑移率进行约束。
[0056]
进一步的，所述横摆角速度的稳定性控制边界为：
[0057][0058]
式中，后轮侧偏角α
r,ss
阈值为[-α
r,lim
,α
r,lim
]；
[0059]
所述质心侧偏角的稳定性控制边界为：
[0060][0061]
所述稳定性包络边界为：
[0062]henv
ξ(k)≤g
env
，
[0063]
式中，ξ(k)为k时刻时的车辆状态量。
[0064]
进一步的，所述可行道路区域边界表示为：
[0065]hroad
ξ(k)≤g
road
，
[0066]
式中，d为车体宽度，rd为路面宽度；
[0067]
所述控制输入量和轮胎滑移率的约束条件为：
[0068]
0.1≤s≤0.3，
[0069][0070]
式中，δ
f,max
,
△
δ
f,max
分别为控制量阈值和控制增量阈值。
[0071]
进一步的，还包括建立反馈修正模块，降低外部环境、建模误差对控制系统的干扰的步骤，所述反馈修正模块的构建为：
[0072]
通过建立状态量和控制量控制输出参考值(ξ
p
(k),u
p
(k))与实际测量值(ξm(k),um(k))之间的线性关系，达到参数修正的目的，最后，修正后的参数(ξc(k),uc(k))返回到控制系统，反馈修正方程为：
[0073][0074]
式中，k1，k2为修正系数。
[0075]
上述基于预测模型的自动驾驶汽车稳定性路径跟踪方法，采用贝塞尔曲线等距分段拟合参考路径的方法，实现实时提取道路曲率，有效的抑制了曲率对路径跟踪性能和稳定性的影响。实现了自动驾驶车辆行驶时，特别是在高速、冰雪路面等复杂道路环境中行驶时能保持良好的路径跟踪效果，保证车辆稳定行驶。
附图说明
[0076]
图1为一个实施例的基于预测模型的自动驾驶汽车稳定性路径跟踪方法流程图；
[0077]
图2为车辆动力学模型图；
[0078]
图3为车辆跟踪误差模型图；
[0079]
图4为贝塞尔曲线示意图；
[0080]
图5为稳定性路径跟踪控制整体策略图；
[0081]
图6为稳定性包络边界图；
[0082]
图7为可行道路区域包络边界图。
具体实施方式
[0083]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0084]
如图1所示，在一个实施例中，一种基于预测模型的自动驾驶汽车稳定性路径跟踪方法，包括以下步骤：
[0085]
步骤s110，建立包括车辆动力学模型与跟踪误差模型的自动驾驶汽车的车辆模型。
[0086]
首先，建立车辆模型：包括车辆动力学模型和车辆跟踪误差模型。
[0087]
在车辆重心处建立车身坐标系xyz，坐标系原点与车辆质心重合，x轴平行于地面与车辆纵向行驶方向重合，y轴平行于地面与车辆横向行驶方向重合，z轴垂直于地面。车辆
动力学模型如图2所示，车辆动力学模型的横摆动力学方程表示为：
[0088][0089][0090][0091][0092]
式中，m为车辆质量，v
x
、vy分别为车体质心处的纵向速度和横向速度，f
yf
、f
yr
为前轮和后轮的侧向力，f
xf
、f
xr
为前轮和后轮的纵向力，为横摆角速度，为侧偏角速率，iz为车身绕z轴的转动惯量，lf、lr分别为车辆质心到前轴和后轴的距离。
[0093]
轮胎侧向力和纵向力可表示为：
[0094]fyf
＝c
αf
αf[0095]fyr
＝c
αr
αr，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0096]fxf
＝c
lf
sf[0097]fxr
＝c
lr
sr，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0098]
式中，c
αf
、c
αf
为轮胎侧偏刚度，c
lf
、c
lr
为轮胎纵向刚度，αf、αr为前轮和后轮轮胎侧偏角，sf、sr为前轮和后轮轮胎滑移率。
[0099]
轮胎侧偏角可表示为：
[0100][0101][0102]
式中，δf为前轮转角。
[0103]
轮胎滑移率可表示为：
[0104][0105]
式中，r为车轮滚动半径，ω
t
为车轮角速度，v为车辆行驶速度。
[0106]
质心侧偏角β可表示为：
[0107][0108]
将式(1)～(4)的车辆动力学模型的横摆动力学方程转换成如下车辆动力学模型：
[0109][0110][0111][0112][0113]
式中，m为车辆质量，v
x
、vy分别为车体质心处的纵向速度和横向速度，为横摆角速度，β为质心侧偏角，iz为车身绕z轴的转动惯量，lf、lr分别为车辆质心到前轴和后轴的距离，c
αf
、c
αf
为轮胎侧偏刚度，c
lf
、c
lr
为轮胎纵向刚度，αf、αr为前轮和后轮轮胎侧偏角，sf、sr为前轮和后轮轮胎滑移率，δf为前轮转角。
[0114]
车辆跟踪误差模型如图3所示，采用车辆后轴中心与参考道路中心线(road centerline)之间的误差表征跟踪误差，所建立的车辆跟踪误差模型表示为：
[0115][0116][0117]
式中，横向位置误差ey为车辆后轴中心在道路中心线上投影点之间的距离，航向误差为道路中心线切向与道路地面坐标系x的夹角，κ为参考路径的道路曲率。
[0118]
其次，建立预测模型。
[0119]
联立式(10)～(14)，对车辆动力学模型和车辆跟踪误差模型进行线性化处理，获得车辆跟踪控制模型：
[0120][0121]
式中，状态向量为输入向量为u1＝δf，附加输入向量为u2＝κ；a,b1,b2为雅可比矩阵，其中，
[0122]
[0123][0124]
b2(t)＝[0 0 0 0 0
ꢀ‑vx
]
t
；
[0125]
对车辆跟踪控制模型采用一阶差商的方法进行离散化处理，得到离散的状态空间表达式：
[0126]
ξ(k 1)＝akξ(k) bku1(k) cku2(k)，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0127]
式中，ak＝i ta(t),bk＝tb1(t),ck＝tb2(t)，i为六阶单位矩阵；
[0128]
构建新的状态向量ξ(k|t)＝[ξ(k) u1(k-1)]
t
，根据离散的状态空间表达式得到新的状态空间方程：
[0129][0130]
式中，
[0131]
根据新的状态空间方程进行状态预测，获得未来时刻的所述预测模型：
[0132]
y＝ψξ(k) θ
△
u1 γu2，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0133]
其中，
[0134]
y＝[ξ(k 1) ξ(k 2)
ꢀ…ꢀ
ξ(k nc)
ꢀ…ꢀ
ξ(k n
p
)]
t
,
[0135][0136][0137][0138]
△
u1＝[
△
u1(k)
ꢀ△
u1(k 1)
ꢀ…ꢀ△
u1(k nc)]
t
,
[0139]
u2＝[u2(k) u2(k 1)
ꢀ…ꢀ
u2(k nc)]
t
,
[0140]
式中，n
p
为预测时域，nc为控制时域，
△
u1为控制增量。
[0141]
步骤s120，自动驾驶汽车执行路径规划，并基于贝塞尔曲线对规划出的参考路径
分段拟合，实时计算道路曲率。
[0142]
贝塞尔曲线如图4所示，由自动驾驶汽车中的环境感知系统采集环境信息，执行路径规划，规划出参考路径，此参考路径为一系列离散路点构成，并采用三阶贝塞尔曲线分段拟合，表示为：
[0143]
q(τi)＝(1-τi)3p0 3τi(1-τi)2p1 3τ
i2
(1-τi)2p2 τ
i3
p3,
ꢀꢀꢀ
(19)
[0144]
式中，q(τi)为参数τi处的插值点，pk为第k个控制点，通过对参数τi在[0,1]内取值，可以在第一个控制点和最后一个控制点之间生成任意个插值点。中间控制点p1和p2的计算方程为：
[0145][0146][0147]
式中，
[0148]
把参考路径的起止点分别作为三次贝塞尔曲线的第一个和最后一个控制点，在每次曲线拟合的迭代过程中，首先求解各段贝塞尔曲线中间控制点的位置，根据式(19)可以得到与原路径点对应的插值点，然后根据控制时域从起点到终点等距分段，取n段，最后，根据每个拟合路段的插值点计算道路曲率：
[0149][0150]
式中，x
′
＝x
c-xa，y
′
＝y
c-ya，x
″
＝xc x
a-2xb，y
″
＝yc y
a-2yb，η＝(x
′
)2 (y
′
)2，(xa,ya),(xb,yb),(xc,yc)为每个拟合路段的插值点。
[0151]
采用贝塞尔曲线等距分段拟合参考路径的方法，实现实时提取道路曲率，有效的抑制了曲率对路径跟踪性能和稳定性的影响。
[0152]
步骤s130，基于预测模型构建表达控制输出预测值与控制输出参考值之间的误差和控制增量的目标函数。
[0153]
自动驾驶汽车稳定性路径跟踪方法的整体控制策略如图5所示，首先，基于预测模型得到的输出预测值与输出参考值之间的误差和控制增量构建目标函数。其次，构建目标函数的约束条件，包括：建立由横摆角速度、质心侧偏角构成的稳定性包络边界，对横摆角速度和质心侧偏角进行稳定性约束；建立可行道路区域边界，对横向位置误差进行约束；对控制输入量和轮胎滑移率进行约束。再次，建立反馈修正模块，降低外部环境、建模误差对控制系统的干扰。最后，目标函数求解出最优控制量传递给被控车辆执行，实现车辆在复杂
工况下满足自身横向稳定性的同时能够安全的跟踪参考路径。
[0154]
车载系统采集与处理得到状态量vy,v
x
,β,ey,并作为控制系统的输入量，作为控制系统的输出量，基于预测模型得到的输出预测值与输出参考值之间的误差和控制增量构建目标函数：
[0155][0156]
式中，q
ey
,r为权重矩阵，ρ为权重系数，ε2为权重因子。
[0157]
构建目标函数的约束条件。
[0158]
首先，建立由横摆角速度、质心侧偏角构成的稳定性包络边界，对横摆角速度和质心侧偏角进行稳定性约束。稳定性包络边界如图6所示，
①
和
③
为横摆角速度控制边界，
②
和
④
为质心侧偏角控制边界，自动驾驶汽车在执行路径跟踪时应尽可能把质心侧偏角的值和横摆角速度的值约束于包络边界内，以保证车辆的稳定性。
[0159]
根据后轮最大侧偏力约束，由式(4)可得到横摆角速度稳定性控制边界：
[0160][0161]
式中，后轮侧偏角α
r,ss
阈值为[-α
r,lim
,α
r,lim
]。
[0162]
根据式(7)和式(9)可得到质心侧偏角稳定性控制边界：
[0163][0164]
根据式(23)和式(24)建立横摆稳定性包络边界：
[0165]henv
ξ(k)≤g
env
，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)
[0166]
式中，ξ(k)为k时刻时的车辆状态量。该稳定性包络边界使车辆在行驶时尽可能处在包络边界内，保证车辆的横摆稳定性。
[0167]
其次，建立可行道路区域边界，对横向位置误差进行约束。
[0168]
可行道路区域如图7所示，假设不考虑障碍物的出现，根据车辆纵向速度和车辆动力学模型的离散化步长，在道路纵向上生成一系列参考点，由车辆行驶轨迹与道路环境之间的横向距离偏差阈值e
y,max
和e
y,min
作为可行道路区域边界，保证车辆不驶出道路边界，可行道路区域边界表示为：
[0169]hroad
ξ(k)≤g
road
，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0170]
式中，d为车体宽度，rd为路面宽度。
[0171]
再次，对控制输入量和轮胎滑移率进行约束。
[0172]
对轮胎滑移率进行约束，使轮胎附着力在良好范围内，有效防止制动时自动驾驶车辆甩尾、侧滑等失稳现象的发生，对控制输入量施加约束，保证车辆能够满足驱动能力。
[0173]
控制输入量和轮胎滑移率约束条件的构建具体为：
[0174]
0.1≤s≤0.3，
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(27)
[0175][0176]
式中，δ
f,max
,
△
δ
f,max
分别为控制量阈值和控制增量阈值。
[0177]
考虑轮胎滑移率对制动稳定性能的影响，对滑移率施加约束，保持轮胎附着力在良好范围内，有效防止制动时自动驾驶车辆甩尾、侧滑等失稳现象的发生。
[0178]
接下来，建立反馈修正模块，降低外部环境、建模误差对控制系统的干扰。
[0179]
通过建立状态量和控制量控制输出参考值(ξ
p
(k),u
p
(k))与实际测量值(ξm(k),um(k))之间的线性关系，达到参数修正的目的，最后，修正后的参数(ξc(k),uc(k))返回到控制系统，反馈修正方程为：
[0180][0181]
式中，k1，k2为修正系数。
[0182]
反馈修正模块，降低外部环境、建模误差对控制系统的干扰，有助于实时提高车辆路径跟踪能力和车辆稳定性的控制效果。
[0183]
步骤s140，求解目标函数，获得最优控制量，并输入所述车辆模型。
[0184]
根据式(22)、式(25)、式(26)、式(28)构建带有约束条件的目标函数：
[0185][0186]
采用二次规划问题求解目标函数，把目标函数转化成形如下式的求解函数：
[0187][0188]
式中，h＝θ
′
qθ r，f＝2((ψξ(k))
t
(γu2)
t
)qθ，a,b为边界条件和约束条件构成的矩阵，lb,ub为控制增量阈值；通过求解把向量z中的第一个参数作为最优控制量传递给被控车辆执行，实现车辆在满足自身横向稳定性的同时能够安全的跟踪参考路径。
[0189]
本发明实现自动驾驶车辆行驶时，特别是在高速、冰雪路面等复杂道路环境中行驶时能保持良好的路径跟踪效果，保证车辆稳定行驶。
[0190]
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。