一种自动驾驶半挂卡车横向轨迹跟踪自适应控制方法与流程

1.本发明属于自动驾驶车辆运动控制技术领域，具体涉及到一种自动驾驶半挂卡车横向轨迹跟踪自适应控制方法。


背景技术：

2.半挂卡车由牵引车和挂车两部分组成，相对于乘用车具有更复杂的动力学特性，尤其是，对于高速的重载半挂车而言，当挂车载荷变化时，牵引车和挂车的质心位置和横摆转动惯量都会明显改变。同时，半挂车车辆的轮胎侧偏刚度，也会随着各轴垂向载荷的变化而不断变化，另外，定位跳变和信号噪声会严重影响控制输出的平滑性和车辆的稳定性，导致车辆在巡线和换道过程中，出现震荡或超调。
3.在自动驾驶跟踪控制领域，常见的横向轨迹跟踪控制算法普遍存在一个或多个问题，具体为：(1)基于运动学模型设计控制器，无法考虑车辆在高速时的复杂动力学特性；(2)基于二自由度车辆动力学模型设计半挂卡车的横向控制器，无法准确表征牵引车和挂车之间的耦合关系；(3)利用固定的轮胎侧偏刚度与车辆动力学模型结合，无法精确反应轮胎动力学特性对车辆系统的影响；(4)未对反馈控制输出进行约束处理，难以保证车辆控制的平滑性和稳定性。


技术实现要素：

4.针对上述问题，本发明设计了一种自动驾驶半挂卡车横向轨迹跟踪自适应控制方法，以解决半挂车的精准性问题，并保证跟踪控制的适应性和稳定性。
5.为了实现上述目的本发明采用如下技术方案：
6.一种自动驾驶半挂卡车横向轨迹跟踪自适应控制方法，该方法的系统包括车辆参数自适应计算模块、垂向载荷分配模块、轮胎侧偏度刚度计算模块、权重系数自适应调整模块、lqr控制器求解模块；
7.该方法包括以下步骤：
8.步骤1：根据挂车的载重，通过车辆参数自适应计算模块计算牵引车和挂车的质心位置和横摆转动惯量；
9.步骤2：根据牵引车和挂车的质心位置，通过垂向载荷分配模块计算各轴处的垂向载荷；
10.步骤3：根据各轴处的垂向载荷和轮胎数量，通过轮胎侧偏刚度计算模块，实时更新各轮胎的侧偏刚度；
11.步骤4：根据车速，通过权重系数自适应调整模块，实时调整各状态变量的权重系数；
12.步骤5：根据车辆各状态变量以及对应的权重系数，通过lqr控制器求解模块计算方向盘的转角。
13.作为本发明进一步的描述，该方法的系统还包括用于对状态误差计算和测量信号
进行约束和滤波处理的控制输出约束模块，所述的控制输出约束模块包括反馈状态约束和整体约束；
14.反馈状态约束为非线性调整和低通滤波；
15.整体约束为基于静态侧倾理论计算，得出的前轮转角极限约束。
16.作为本发明进一步的描述，步骤1中，在车辆参数自适应计算模块输入挂车的载重质量，根据力矩平衡和平行移轴定理计算，得出牵引车和挂车整体质心至前后轴的距离和各自整体的横摆转动惯量。
17.作为本发明进一步的描述，步骤2中，在垂向载荷分配模块输入牵引车和挂车整体质心至前后轴的距离、整体质量和纵向加速度，根据达朗贝尔原理计算，得出的牵引车和挂车各轴处的垂向载荷。
18.作为本发明进一步的描述，步骤3中，在轮胎侧偏刚度计算模块输入各轴处的垂向载荷和轮胎数量，输出各轴处的轮胎侧偏刚度，即各轮胎侧偏刚度之和。
19.作为本发明进一步的描述，步骤4中，在权重系数自适应调整模块输入车速，输出各状态变量的权重系数，包括横向位置偏差、航向角偏差、侧向速度、横摆角速度、折叠角速度和折叠角等的权重系数。
20.作为本发明进一步的描述，步骤5中，所述的lqr控制器求解模块包括基于更新后车辆参数和轮胎侧偏刚度建立的半挂车辆动力学模型，设置为前馈 反馈的控制形式。
21.作为本发明进一步的描述，建立的半挂车车辆动力学模型包括侧向速度、横摆角速度和挂车相对于牵引车的折叠角三个自由度；
22.其中，前馈由基于半挂车辆动力学模型的传递函数、终值定理和轨迹半径计算得到。
23.作为本发明进一步的描述，步骤5中，方向盘的转角由lqr控制器求解模块的前轮转角查表得到，所述的前轮转角为前馈前轮转角与反馈前轮转角之和。
24.作为本发明进一步的描述，所述的前馈前轮转角由半挂车跟踪模型的状态方程传递函数和中值定理结合得到，所述的反馈前轮转角通过基于黎卡提方程迭代求解的反馈增益矩阵和状态变量进行计算得到。
25.相对于现有技术，本发明的技术效果为：
26.本发明提供了一种自动驾驶半挂卡车横向轨迹跟踪自适应控制方法，通过质心位置、横摆转动惯量和轮胎侧偏刚度等的变化，提高了半挂车辆动力学模型的准确性；对反馈控制输出进行约束处理，解决定位跳变和信号噪声导致的震荡或超调，保证车辆控制的平滑性和稳定性，本发明为自动驾驶半挂卡车的运动控制提供了新思路，对自动驾驶平台整体架构的设计与优化具有重要价值。
附图说明
27.图1为本发明的控制方法的整体架构示意图；
28.图2为本发明中半挂卡车的结构示意图；
29.图3为本发明半挂卡车的动力学模型示意图；
30.图4为本发明半挂卡车的轨迹跟踪模型示意图。
31.图5为本发明轮胎侧偏刚度随垂向载荷的变化曲线。
32.图中，1.牵引车，2.挂车。
具体实施方式
33.下面结合附图对本发明进行详细描述：
34.一种自动驾驶半挂卡车横向轨迹跟踪自适应控制方法，参考图1-5所示，该方法的系统包括车辆参数自适应计算模块、垂向载荷分配模块、轮胎侧偏度刚度计算模块、权重系数自适应调整模块、lqr控制器求解模块和控制输出约束模块；
35.该方法包括以下步骤：
36.步骤1：根据挂车的载重，通过车辆参数自适应计算模块计算牵引车和挂车的质心位置和横摆转动惯量；
37.步骤2：根据牵引车和挂车的质心位置，通过垂向载荷分配模块计算各轴处的垂向载荷；
38.步骤3：根据各轴处的垂向载荷和轮胎数量，通过轮胎侧偏刚度计算模块，实时更新各轮胎的侧偏刚度；
39.步骤4：根据车速，通过权重系数自适应调整模块，实时调整各状态变量的权重系数；
40.步骤5：根据车辆各状态变量以及对应的权重系数，通过lqr控制器求解模块计算方向盘的转角。
41.所述的控制输出约束模块包括反馈状态约束和整体约束；所述反馈状态约束是通过非线性调整和低通滤波，避免定位跳变和信号噪声引起的输出波动；所述整体输出约束是利用基于静态侧倾理论计算得出的前轮转角极限约束前轮转角的整体输出，保证车辆的侧倾安全性。
42.具体的，本实施例针对各模块进行详细分析，具体实施内容公开如下：
43.1.车辆参数自适应计算模块
44.所述的车辆参数自适应计算模块，输入为挂车的载重质量，输出为根据力矩平衡和平行移轴定理计算，得出的牵引车和挂车整体质心至前后轴的距离和各自整体的横摆转动惯量，其中，固定参数为牵引车和挂车的簧在质量、非簧在质量、簧在质量转动惯量以及簧在质量中心至各轴的距离。
45.具体的，定义牵引车的簧在质量为m
s1
，挂车的簧在质量为m
s2
，牵引车前轴的非簧在质量为m
uf
，牵引车后轴的非簧在质量为m
ur
，挂车的非簧在质量为m
ut
，挂车的载重质量为m
t
，牵引车簧在质量的横摆转动惯量为i
s1
，挂车的簧在质量的横摆转动惯量为i
s2
，牵引车前轴至簧在质心的距离为a
s1
，牵引车后轴至簧在质心的距离为b
s1
，牵引车的轴距为l1，铰接点至挂车簧在质心的距离为a
s2
，挂车后轴至簧在质心的距离为b
s2
，牵引车质心至铰接点的距离为c，牵引车簧载质量的质心高度为h1，挂车簧载质量的质心高度为h2。定义挂车载重质量的长为l
t
，挂车载重质量的宽为w
t
，挂车载重质量前侧至铰接点的距离为e。
46.因此，牵引车总的质量为m1＝m
s1
m
u1
m
u2
，牵引车的非簧在质量之和为m
u1
＝m
uf
m
ur
，挂车总的载重质量为m
ss2
＝m
s2
m
t
，挂车总的质量为m2＝m
ss2
m
ut
。
47.(1)计算牵引车的质心位置和整体的横摆转动惯量
48.确定牵引车前轴非簧在质量距离质量中心的距离为：
[0049][0050]
牵引车非簧在质量中心至簧在质量中心的距离为：
[0051]
l
su1
＝a
u1-a
s1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0052]
牵引车前轴至整体质量中心的距离为：
[0053][0054]
牵引车后轴至整体质量中心的距离为：
[0055][0056]
根据平行移轴定理，牵引车整体的横摆转动惯量为：
[0057][0058]
(2)计算挂车的质心位置和整体的横摆转动惯量
[0059]
挂车载重质量中心至铰接点的距离为：
[0060]ass2
＝l
t
/2-e
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0061]
挂车载重质量中心与簧在质量中心的距离为：
[0062]
l
ss2
＝a
ss2-a
s2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0063]
根据新的质心位置计算铰接点至挂车质心的位置：
[0064][0065]
根据新的质心位置计算挂车后轴至挂车质心的位置：
[0066][0067]
挂车新质心与原质心的距离为：
[0068][0069]
根据平行移轴定理，挂车新的簧在质量中心处的转动惯量为：
[0070][0071]
铰接点至挂车整体质量中心的距离为：
[0072][0073]
挂车非簧质量至簧载质量中心的距离为：
[0074][0075]
挂车后轴至整体质量中心的距离为：
[0076][0077]
根据平行移轴定理，挂车整体的横摆转动惯量为：
[0078][0079]
2.垂向载荷分配模块
[0080]
所述的垂向载荷分配模块，输入为车辆参数自适应计算模块得到的牵引车和挂车整体质心至前后轴的距离、整体质量和纵向加速度，输出为根据达朗贝尔原理计算，得出的各轴处的垂向载荷，其中，固定参数为牵引车和挂车簧在质量中心的高度。
[0081]
具体的，基于达朗贝尔原理计算各轴载荷，其中，铰接点处的垂向载荷为：
[0082][0083]
牵引车前轴处的簧在质量载荷为：
[0084][0085]
牵引车后轴处的簧在质量载荷为：
[0086][0087]
挂车后轴处的簧在质量载荷为：
[0088][0089]
因此，牵引车前轴处的垂向载荷为：
[0090]fzf
＝f
zfs
m
ufgꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)
[0091]
牵引车后轴处的垂向载荷为：
[0092]fzr
＝f
zrs
m
urgꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0093]
挂车后轴处的垂向载荷为：
[0094]fzt
＝f
zts
m
utgꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0095]
其中，式中，a
x
为纵向加速度，g为重力加速度。
[0096]
3.轮胎侧偏刚度计算模块
[0097]
所述的轮胎侧偏刚度计算模块，输入为各轴处的垂向载荷和轮胎数量，输出为各轴处的轮胎侧偏刚度，即为各轮胎侧偏刚度之和，假设各轮胎侧偏刚度为垂向载荷的三次多项式函数。
[0098]
当轮胎型号确定时，轮胎侧偏刚度主要受垂向载荷和路面附着系数的影响。对于半挂车辆，车轮的垂向载荷会因挂车载重质量的变化而明显不同。因此，本发明假设轮胎侧偏刚度为垂向载荷的三次多项式函数，即：
[0099][0100]
式中，k表示轮胎侧偏刚度，fz表示相应车轮的垂向载荷，p0、p1、p2、和p3为三次多项式的拟合参数。
[0101]
不同垂向载荷下的标定值和拟合值如图5所示。因此，本发明所述三次多项式可以准确反应轮胎侧偏刚度与垂向载荷之间的映射关系。另外，本发明轮胎侧偏刚度计算模块所述思想也可以基于二维表格插值实现。
[0102]
4.权重系数自适应调整模块
[0103]
所述的权重系数自适应调整模块，输入为车速，输出为横向位置偏差、航向角偏差、侧向速度、横摆角速度、折叠角速度和折叠角等状态变量的权重系数，映射关系为二维表格或非线性函数，利用不同速度下的标定参数插值或拟合得到。
[0104]
由于不同车速下，半挂车动力学特性不同，本实施例基于拟合函数或二维表格确定车辆位置偏差、航向角偏差、侧向速度、横摆角速度、折叠角速度和折叠角等状态变量所对应的权重系数；例如：当关注5km/h至80km/h工况下的控制性能时，在确定控制变量权重系数r后，可以优选标定出5km/h、10km/h、20km/h、30km/h、40km/h、50km/h、60km/h、70km/h、80km/h等9个车速下最佳的状态变量权重系数q1至q6，然后离线得出拟合函数的系数，进一步利用拟合公式在线插值计算或基于离线表格插值得到。
[0105]
5.lqr控制器求解模块
[0106]
所述的lqr控制器求解模块，包括基于更新后车辆参数和轮胎侧偏刚度建立的半挂车辆动力学模型，设置为前馈 反馈的控制形式；建立的半挂车车辆动力学模型包括侧向速度、横摆角速度和挂车相对于牵引车的折叠角三个自由度；其中，前馈为基于半挂车辆动力学模型的传递函数、终值定理和轨迹半径计算得到。
[0107]
所述的lqr控制器求解模块，输入为实时的车辆位置偏差、航向角偏差、侧向速度、横摆角速度、折叠角速度和折叠角等状态变量以及所对应的权重系数，输出为方向盘转角。其中，所述方向盘转角由控制求解的前轮转角查表得到，所述前轮转角为前馈前轮转角与反馈前轮转角之和，所述反馈前轮转角由基于黎卡提方程迭代求解的反馈增益矩阵和状态变量计算得到，具体计算求得公开如下：
[0108]
在考虑半挂车辆的横向运动、横摆运动和折叠运动，建立如下3自由度动力学模型：
[0109]
m1a
y1
＝f
yf
f
yr-f
hy
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(24)
[0110][0111]
m2a
y2
＝f
yt
f
hy
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)
[0112][0113]
式中，m1为牵引车质量，m2为挂车质量，i
z1
为牵引车的横摆转动惯量，i
z2
为挂车的横摆转动惯量，a1为牵引车前轴至其质心的距离，b1为牵引车后轴至其质心的距离，c为铰接点至牵引车质心的距离，a2为铰接点至挂车质心的距离，b2为挂车后轴至其质心的距离，a
y1
为牵引车的侧向加速度，a
y2
为挂车的侧向加速度，γ1为牵引车的横摆角速度，γ2为挂车的横摆角速度，f
yf
为牵引车前轴的侧向力，f
yr
为牵引车后轴的侧向力，f
yt
为挂车后轴的侧向力，f
hy
为铰接点处的相互作用力。
[0114]
当折叠角较小时，可以认为：
[0115]vx1
＝v
x2
＝v
x
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0116]
式中，v
x1
为牵引车的纵向速度，v
x2
为挂车的纵向速度，v
x
为纵向速度。
[0117]
根据几何和运动关系(本实施例折叠角为挂车相对于牵引车的夹角)，挂车横摆角速度与牵引车横摆角速度的关系可表示为：
[0118][0119]
式中，θ为牵引车与挂车的相对转动角度，即折叠角。
[0120]
在铰接点处有
[0121][0122]
牵引车和挂车的侧向加速度分别为：
[0123][0124][0125]
根据加速度公式，挂车的侧向速度微分可以表示为：
[0126][0127]
左右同时积分可得：
[0128][0129]
式中，v
y1
为牵引车的侧向速度，v
y2
为挂车的侧向速度。
[0130]
牵引车运动特性与两轴车辆类似，都是以前轴为转向轴，后轴为非转向轴。根据坐标系的规定，牵引车前后轮胎侧偏角：
[0131][0132][0133]
式中，δf为牵引车的前轮转角。
[0134]
挂车后轮胎侧偏角的表达式为：
[0135][0136]
根据式(24)可得：
[0137]fhy
＝f
yf
f
yr-m1a
y1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(38)
[0138]
将上式带入到式(25)至式(27)可得：
[0139][0140]
m2a
y2
＝f
yt
f
yf
f
yr-m1a
y1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(40)
[0141][0142]
整理后可得：
[0143][0144]
m1a
y1
m2a
y2
＝f
yf
f
yr
f
yt
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(43)
[0145][0146]
其中，
[0147][0148]
式中，kf、kr和k
t
分别为各轮胎的侧偏刚度。
[0149]
整理式(35)至式(45)，可以得到半挂卡车动力学模型的状态方程形式为：
[0150][0151]
其中，
[0152][0153][0154]
经过整理可得：
[0155][0156]
其中，a-m-1
g，b＝m-1
f。
[0157]
为了表达半挂车辆与参考轨迹之间的关系，建立轨迹跟踪模型如图4所示。其中，ey为牵引车质心到期望轨迹上预瞄点d
p
的横向距离；e
yo
为牵引车质心到期望轨迹最近点d的距离；l为预瞄距离；ψ为牵引车的实际航向角；ψd为期望航向角。
[0158]
根据车辆与参考轨迹间的相对位置关系，基于单点预瞄的轨迹跟踪模型为：
[0159][0160]
式中，ey为横向位置偏差；e
ψ
为航向角偏差；κd为期望轨迹的道路曲率。
[0161]
为了保证预瞄控制的适应性，车辆行驶速度越高，则需要关注越远距离的道路信息。因此，定义自适应预瞄距离为：
[0162]
l＝v
x
δt,l
min
≤l≤l
max
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(49)
[0163]
式中，δt为预瞄时间；l
min
和l
max
分别为预瞄距离的下限和上限。
[0164]
结合式(47)和式(48)，定义系统的状态变量为控制变量为u＝δf，扰动变量为w＝-κdv
x
，输出变量为y＝[ey,e
ψ
]
t
。此时，可以得到半挂车辆跟踪模型的状态方程形式为：
[0165][0166]
式中，
[0167][0168][0169]
其中，a
ij
＝a(ij)，bi＝b(i)；
[0170]
在本发明中，式(50)中的控制变量为所述反馈前轮转角，由黎卡提方程迭代求解得到反馈增益矩阵k和状态变量x计算得到，即：
[0171]
δf＝-kx
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(51)
[0172]
所述前馈前轮转角δ
ff
由式(50)的传递函数形式和中值定理结合得到，即在最优控制量δf的基础上增加一个前馈环节，使系统趋于稳定时的横向偏差量稳态值为0。因此，本发明实际前轮转角的控制量为：
[0173]
δ＝δf δ
ff
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(52)
[0174]
根据所述实际前轮转角可以查表得到方向盘转角，用半挂卡车的横向轨迹跟踪控制。
[0175]
6.控制输出约束模块
[0176]
基于本实施例的发明，考虑到定位跳变和信号噪声等引起的控制输出波动，本发明对状态误差计算和测量信号进行了约束和滤波处理，优选的，为了避免车辆在轨迹跟踪过程中在轨迹线附近频繁波动，本发明对横向位置偏差项，即反馈前轮转角中的第一项进行非线性约束：
[0177][0178]
式中，δ
f1
为反馈前轮转角中的第一项，k1为反馈增益矩阵的第一项，x1为状态矩阵的第一项，tanh为双曲正切函数，λ1为用于减小横向位置偏差波动引入的过渡系数。需要说明的是，可以实现本发明中式(53)思想的其他技术手段，同样属于本发明的保护范围之内。
[0179]
为了降低横摆角速度信号噪声的影响程度，本发明优选使用一阶低通滤波进行约束处理，即：
[0180][0181]
式中，δ
f4
为反馈前轮转角中的第四项，k4反馈增益矩阵的第四项，x4为状态矩阵的第四项，k为增益系数，τ为时间系数，s为拉普拉斯算子。
[0182]
为了保证半挂卡车的侧倾安全性，本发明中对实际前轮转角进行约束。根据静态侧倾理论，定义横向载荷转移率为：
[0183][0184]
式中，ltr为横向载荷转移率，h1为牵引车的质心高度，b1为牵引车的轮距。
[0185]
由于忽略了车辆悬架系统、道路横坡和质心高度估算精度等的影响，可以选取侧翻预警ltr值取为0.8，此时侧向加速度阈值为：
[0186][0187]
稳态侧向加速度可以近似表示为a
y1
＝v
x
γ1，且稳态的横摆角速度γ可以表示为：
[0188]
γ1＝g
γ
δ
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(57)
[0189]
式中，g
γ
为根据式(46)得到的横摆角速度稳态增益。
[0190]
因此，考虑侧倾安全的实际前轮转角约束为：
[0191][0192]
式中，sign为符号函数。
[0193]
需要说明的是，如果挂车的质心高度可以精确估算时，应当基于牵引车的横向载荷转移率和挂车的横向载荷转移率共同进行约束，选取二者中的最小值作为整体的输出约束。另外，如果除横向位置偏差项和横摆角速度项以外的其他项受到类似干扰，可以应用本发明的逻辑进行约束处理。
[0194]
以上实施例仅用以说明本发明的技术方案而非限制，本领域普通技术人员对本发明的技术方案所做的其他修改或者等同替换，只要不脱离本发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。