一种考虑避障稳定性的多轴特种车辆安全轨迹规划方法

1.本发明属于车辆工程技术领域，具体地涉及一种考虑避障稳定性的多轴特种车辆安全轨迹规划方法。


背景技术：

2.多轴特种车辆由于其重型承载和广域机动的特点，在交通运输领域得到广泛应用。相比于两轴轻型车辆，多轴特种车质心高、质量大，在高速机动时更易失稳侧翻。传统意义上，多轴特种车辆的安全稳定控制主要依赖于驾驶员经验和车辆运动控制器。通过设计合理的车辆运动控制器，跟踪车辆期望状态，可以一定程度上实现多轴车辆横摆稳定性和防倾翻控制。但是，多轴特种车辆大惯量的特点造成其执行器响应速度和输出范围难以适应复杂多变的行驶环境，仅通过车辆运动控制器进行稳定控制往往难以到达预期的效果。
3.为实现多轴特种车辆稳定高速行驶，设计一种考虑轨迹跟踪时车辆避障稳定性的安全轨迹规划方法，在进行轨迹规划时考虑车辆的动力学性能，采用路径-速度协同规划的方式，避免多轴特种车的车辆控制器进行轨迹跟踪时超出动力学约束边界，引发失稳侧翻。


技术实现要素：

4.针对以上问题，本发明以五轴特种车辆为研究对象，考虑周围车辆运动状态及本车高速机动和避障稳定性，基于动力学模型实现局部路径-速度协同规划，实现五轴特种车安全稳定行驶。首先，考虑五轴车辆纵向、侧向、横摆以及侧倾四个自由度建立五轴特种车辆动力学模型，并基于车辆模型的动力学响应验证模型的正确性；其次，考虑周围车辆和特种车的相对运动状态，基于恒加速度假设，建立车辆行驶风险场模型，同时考虑车辆避障行驶时的动力学稳定域，基于质心侧偏角-横摆角速度相平面建立车辆行驶约束条件；再次，设计mpc轨迹规划器，以风险场模型为软约束，车辆稳定性为硬约束，同时考虑轨迹平滑性，生成可行行驶轨迹；最后，通过仿真验证所提出方法的正确性。
5.为了实现上述目的，本发明所采用的技术方案如下：
6.一种考虑避障稳定性的多轴特种车辆安全轨迹规划方法，包括：
7.步骤1：基于五轴特种车辆纵向、侧向、横摆以及侧倾四个自由度建立五轴特种车辆动力学模型，并对五轴特种车辆动力学模型的正确性进行验证；
8.步骤2：基于步骤1建立的五轴特种车辆动力学模型，参考周围车辆和特种车的相对运动状态，基于恒加速度假设，建立五轴特种车辆行驶风险场模型，同时基于车辆横摆稳定性和侧倾安全性，建立五轴特种车辆动力学约束模型；
9.步骤3：以步骤2建立的五轴特种车辆行驶风险场模型为软约束，五轴特种车辆动力学约束模型为硬约束，设计mpc轨迹规划器，生成安全行驶轨迹。
10.优选的，所述步骤1包括：
11.步骤1.1：在车辆坐标系下建立四自由度车体动力学模型，在大地坐标系下，建立五轴车辆运动学模型；
12.步骤1.2：基于dugoff模型建立轮胎模型；
13.步骤1.3：建立五轴特种车辆的转向模型；
14.步骤1.4：利用matlab/simulink软件建立四自由度整车模型，同时利用trucksim软件建立五轴车辆整车模型，通过比较两种模型在相同给定输入情形下的动力学响应，来验证四自由度整车模型的准确性。
15.优选的，所述步骤1.1包括：
16.步骤1.1.1：在车辆坐标系下建立五轴特种车辆单轨模型，四自由度车体动力学方程表示为：
17.纵向运动微分方程：
[0018][0019]
侧向运动微分方程：
[0020][0021]
横摆运动微分方程：
[0022][0023]
侧倾运动微分方程：
[0024][0025]
其中，m为整车质量；v
x
、vy分别表示车辆坐标系下车辆沿x轴、y轴的纵向速度和侧向速度；表示车辆的横摆角速度；iz为车辆横摆转动惯量；i
x
为车辆绕侧倾中心转动惯量，f
xi
和f
yi
分别表示车辆坐标系下地面通过第i个(i＝1，2，...，5)轮胎作用于整车的纵向力和侧向力；li(i＝1，2，...，5)表示车辆各轴到质心的距离，θ表示车体侧倾角，h为车辆质心距离地面垂直距离，ay为车辆质心处横向加速度，表示为由于悬架的存在，车体同时受到抗侧倾力矩，用mr表示，其大小为k、d分别表示车体侧倾的刚度系数和阻尼系数；
[0026]
步骤1.1.2：在大地坐标系下，建立五轴车辆运动学模型：
[0027][0028]
上式，表示车辆航向角，x、y分别为车辆在大地坐标系下横向位置和纵向位置。
[0029]
优选的，所述步骤1.2包括：
[0030]
步骤1.2.1：车辆坐标系下和轮胎坐标系下轮胎力映射关系为：
[0031][0032]
其中，δi表示第i轴的转向角度，f
li
、f
ci
表示第i轴轮胎在轮胎坐标系下受到的纵向
力和侧向力；
[0033]
步骤1.2.2：采用dugoff轮胎模型，建立起轮胎力与车体动力学的联系：
[0034][0035]
其中，c
li
、c
ci
分别表示第i轴轮胎的纵向刚度和侧向刚度，αi为第i轴轮胎的轮胎侧偏角，si为第i轴轮胎的轮胎滑移率，μ为地面附着系数，f
zi
为第i轴轮胎的轮胎纵向力，λi为轮胎力修正系数；
[0036]
基于式(7)的轮胎力计算，以轮胎侧偏角和轮胎滑移率为输入，计算方式如下：
[0037]
轮胎侧偏角：
[0038][0039]
轮胎滑移率：
[0040][0041]
其中，正负号约定为，前两轴取正，后三轴取负；ωi表示车轮转速，rw为轮胎滚动半径，v
li
为轮心纵向速度，表示为：
[0042][0043]
优选的，步骤1.3具体为：
[0044]
五轴特种车辆转向角基于阿克曼转向模型，五轴车第1、2、4、5轴为转向轴，瞬心位于第三轴延长线上，对单轨模型，建立如下关系：
[0045][0046]
进而：
[0047][0048]
优选地，所述步骤2的五轴特种车辆行驶风险场模型具体为：
[0049]
步骤2.1.1：基于二元高斯分布函数，建立五轴特种车辆预测风险场：
[0050][0051]
其中，x
ret
、y
ret
为本车和周围车辆间相对位置；z为碰撞调节系数，σ
x
、σy为风险场调
节因子；
[0052]
对本车周围第i台车辆，由于车辆相对行驶方向，相对坐标按如下方式定义：
[0053][0054]
表示本车与周围第i台车的航向夹角，(x0，y0)为本车在大地坐标系下的坐标，(xi，yi)为第i台车辆在大地坐标系下位置；
[0055]
步骤2.1.2：定义系数σ为风险场调节因子，本车某时刻运动状态为分别表示车辆位置、速度、加速度以及航向角，周围车辆运动状态为
[0056]
风险场调节因子σ计算公式为：
[0057][0058]
式中，为常数系数，δx和δy分别表示本车与周围车辆x方向和y方向差值，a
x0
、a
y0
为本车横向和纵向加速度，v
x0
、v
y0
为本车横向和纵向速度，c
x
、cy根据实际车速确定，采用如下计算公式：
[0059][0060]
上式中，表示道路最高限速和最低限速；
[0061]
对道路边界定义为：
[0062][0063]
式中，z为常数系数，y1、y2分别为车辆到车道边界两边的最近距离，σy定义方式与式(15)相同；
[0064]
步骤2.1.3：为建立未来n个时刻的风险场模型，基于恒加速度假设，跟据当前对他车运动状态的估计量，在下一时刻他车运动状态为：
[0065][0066]
其中t为两个运动时刻的时间间隔，xi、yi表示周围第i辆车在大地坐标系下横向位置和纵向位置；
[0067]
若短时间内加速度为定值a
x
，ay，在t时刻预测未来n个时刻运动状态为：
[0068][0069]
优选的，所述步骤2的五轴特种车辆动力学约束模型包括车辆稳定性约束和车辆侧倾约束；
[0070]
车辆稳定性约束具体为：
[0071]
步骤2.2.1：车体动力学方程简化为二自由度：
[0072][0073]
式中，β为质心侧偏角，其大小为vy/v
x
，车辆稳定状态下，质心侧偏角变化率为0，同时地面提供的最大附着力，将最大稳定横摆角速度表示为：
[0074][0075]
μ为地面附着系数，
[0076]
对质心侧偏角约束结合非转向轴轮胎力饱和情况，结合式(8)得：
[0077][0078]
其中，轮胎饱和侧偏角近似由下式计算：
[0079][0080]
其中，c
α
为轮胎侧向刚度，fz为轮胎垂向力，μ为地面附着系数，由于五轴车辆车身与轮胎悬架构成超静定系统，将第三轴轮胎垂向力表示为：
[0081][0082]
式中，mb为车辆簧载质量，mw为车辆簧下质量，正负号约定与步骤1.2.3相同；
[0083]
步骤2.2.2：车辆侧倾约束
[0084]
对车辆侧倾约束基于零力矩点位置偏移，若地面存在一点相对车辆质心偏移y
zm
，车辆重力、惯性力和地面对车体作用力相对此点产生车辆侧倾力矩之和为零，则得如下关系式：
[0085][0086]
由于车身侧倾角较小，基于小角度假设，进而：
[0087][0088]
通过限制车辆质心偏移y
zm
，约束车辆侧倾：
[0089]-y
zm，max
≤y
zm
≤y
zm，max
(27)
[0090]
最终，对车辆行驶稳定性约束可表示为：
[0091][0092]
优选的，所述步骤3的mpc轨迹规划器设计具体为：
[0093]
步骤3.1.1：将步骤1建立的车辆动力学模型记载为：
[0094][0095]
式中ξ表示此轨迹规划器系统的状态量，为u表示系统控制量，为
[0096]
步骤3.1.2：步骤3.1.1的轨迹规划器系统为非线性系统，将其在当前运行时刻线性化，并定义系统输出量为则轨迹规划器系统表示为：
[0097][0098]
式中，矩阵矩阵为雅各比矩阵，d
t
为线性化误差；
[0099]
步骤3.1.3：基于当前时刻t，生成未来n个时刻行驶轨迹，将以上系统采用前向欧拉法离散，进而将系统表示为：
[0100][0101]
式中，t表示离散时长，离散后的线性化误差表示为：
[0102][0103]
步骤3.1.4：采用控制增量δu(t)代替控制量u(t)，通过直接对控制增量进行约束可实现车辆平滑轨迹规划，若控制增量δu(t)＝u(t)-u(t-1)，则将式(30)化为：
[0104][0105]
将式(34)表示为：
[0106]
[0107]
若预测时域为h
p
，控制时域为hc，且h
p
＞hc，假定在当前时刻t，给定控制时域[t,t+h
c-1]一系列控制量δu1，δu2，...，δum；在控制时域外[t+hc，t+h
p-1]，给定控制量为0，则在预测时域内的预测输出表示为：
[0108][0109]
根据式(36)，以当前被优化系列输入量为未知量计算最优输出轨迹。
[0110]
优选的，所述步骤3的最优行驶轨迹生成具体为：
[0111]
步骤3.2.1：以步骤2中的行驶风险场模型及动力学约束模型为约束，规划器在求解时长t内循环求解预测时域内的最优问题j，定义最优化问题j为：
[0112][0113]
式中，tra为行驶轨迹，tra
ref
为参考行驶轨迹输入，v
x
为车辆纵向速度，v
xref
为参考车速，p为预测时域内跟据他车运动状态观测结果和道路边界生成的运动风险场，通过式(13)-(19)计算；
[0114]
步骤3.2.2：κ为预测时域内某时刻车辆行驶轨迹的曲率，计算方式为：
[0115][0116]
式中，x、y为规划器输出轨迹点的三次样条拟合曲线，其边界条件通过约束起始点和终止点车辆横摆角状态给定，则在时刻t，规划器输出位置为：
[0117][0118]
其中，公式中的参数为三次样条曲线的拟合系数，无物理意义；
[0119]
步骤3.2.3：若车辆容许最大侧向加速度为|a
lat
|
max
，则对车辆速度约束：
[0120][0121]
式中，v
sign
为车道限速，涉及车辆行驶平顺性和乘车舒适性，并结合式(27)安全性约束，最优化问题表示为：
[0122][0123]
在预测时域h
p
内，规划器通过求解最优化问题(42)，生成车辆连续行驶轨迹，下层控制器通过实时跟踪行驶轨迹，实现车辆结构化道路的自动行驶。
[0124]
与现有技术相比，本发明的有益效果是：
[0125]
(1)传统路径-速度解耦的轨迹规划方法不能保证车辆高速巡航行驶时的避障稳定性，本发明采用路径-速度协同规划方法，考虑自身车辆动力学约束和周围车辆行驶状态，生成实现稳定避障的车辆行驶轨迹，实现多轴特种车辆安全稳定地高速行驶。
[0126]
(2)传统轨迹规划方法，建立风险场时不考虑周围车辆动态特性，无法保证车辆行驶安全性，本发明基于二元高斯分布，考虑周围车辆行驶时地运动学特性，建立风险场模型，生成未来n个时刻的风险场，并以此为约束，生成安全的车辆行驶轨迹。
附图说明
[0127]
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
[0128]
在附图中：
[0129]
图1为五轴车辆单轨模型；
[0130]
图2为车辆模型对比：a.方向盘转角输入；b.不同激励下模型质心侧偏角响应；c.不同激励下横摆角速度响应；
[0131]
图3为障碍风险场：a.障碍风险场；b.道路边界风险场；c.车辆运动风险场；
[0132]
图4为稳定性约束示意图；
[0133]
图5为轨迹规划时序图；
[0134]
图6为规划验证流程；
[0135]
图7为直行道路周边车辆位置分布；
[0136]
图8为速度18m/s时车辆行驶轨迹；
[0137]
图9为速度25m/s时车辆行驶轨迹；
[0138]
图10为不同车速下轨迹规划时空图：a.18m/s车速下时空图；b.25m/s车速下时空图；
[0139]
图11为不同车速下车辆行驶时相轨迹及侧倾角：a.车速18m/s时车辆相轨迹，b.车速18m/s时车辆侧倾角；c.车速25m/s时车辆相轨迹，d.车速25m/s时车辆侧倾角；
[0140]
图12为弯路工况车辆行驶轨迹；
[0141]
图13为弯道行驶工况车辆时空图；
[0142]
图14(a)为车辆弯路行驶时相轨迹,图14(b)为车辆弯路行驶时侧倾角；
[0143]
图15为车辆不同约束下轨迹对比；
[0144]
图16(a)为无约束下车辆行驶时相轨迹，图16(b)为无约束下车辆行驶时侧倾角；
[0145]
图17为本发明的方法流程图。
具体实施方式
[0146]
以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
[0147]
实施例：
[0148]
参照附图1-16所示，一种考虑避障稳定性的多轴特种车辆安全轨迹规划方法，包括：
[0149]
步骤1：基于五轴特种车辆纵向、侧向、横摆以及侧倾四个自由度建立五轴特种车
辆动力学模型，并对五轴特种车辆动力学模型的正确性进行验证。具体为：
[0150]
步骤1.1：在车辆坐标系下建立四自由度车体动力学模型，在大地坐标系下，建立五轴车辆运动学模型；
[0151]
步骤1.1.1：考虑五轴车辆纵向、侧向、横摆以及侧倾四个自由度，在车辆坐标系下建立五轴特种车辆单轨模型，如图1所示。四自由度车体动力学方程表示为：
[0152]
纵向运动微分方程：
[0153][0154]
侧向运动微分方程：
[0155][0156]
横摆运动微分方程：
[0157][0158]
侧倾运动微分方程：
[0159][0160]
其中，m为整车质量；v
x
、vy分别表示车辆坐标系下车辆沿x轴、y轴的纵向速度和侧向速度；表示车辆的横摆角速度；iz为车辆横摆转动惯量；i
x
为车辆绕侧倾中心转动惯量，f
xi
和f
yi
分别表示车辆坐标系下地面通过第i个(i＝1，2，...，5)轮胎作用于整车的纵向力和侧向力；li(i＝1，2，...，5)表示车辆各轴到质心的距离，θ表示车体侧倾角，h为车辆质心距离地面垂直距离，ay为车辆质心处横向加速度，表示为由于悬架的存在，车体同时受到抗侧倾力矩，用mr表示，其大小为k、d分别表示车体侧倾的刚度系数和阻尼系数；
[0161]
步骤1.1.2：在大地坐标系下，建立五轴车辆运动学模型：
[0162][0163]
上式，表示车辆航向角，x、y分别为车辆在大地坐标系下横向位置和纵向位置。
[0164]
步骤1.2：基于dugoff模型建立轮胎模型；
[0165]
步骤1.2.1：车辆坐标系下和轮胎坐标系下轮胎力映射关系为：
[0166][0167]
其中，δi表示第i轴的转向角度，f
li
、f
ci
表示第i轴轮胎在轮胎坐标系下受到的纵向力和侧向力；
[0168]
步骤1.2.2：采用dugoff轮胎模型，建立起轮胎力与车体动力学的联系：
[0169][0170]
其中，c
li
、c
ci
分别表示第i轴轮胎的纵向刚度和侧向刚度，αi为第i轴轮胎的轮胎侧偏角，si为第i轴轮胎的轮胎滑移率。μ为地面附着系数，f
zi
为第i轴轮胎的轮胎纵向力，λi为轮胎力修正系数；
[0171]
基于式(7)的轮胎力计算，以轮胎侧偏角和轮胎滑移率为输入，计算方式如下：
[0172]
轮胎侧偏角：
[0173][0174]
轮胎滑移率：
[0175][0176]
其中，正负号约定为，前两轴取正，后三轴取负；ωi表示车轮转速，rw为轮胎滚动半径，v
li
为轮心纵向速度，表示为：
[0177][0178]
步骤1.3：建立五轴特种车辆的转向模型，具体为：
[0179]
五轴特种车转向角基于阿克曼转向模型，即转向时所有车轮具有同一瞬时转向中心，以使车轮近似处于纯滚动状态。所述五轴车第1、2、4、5轴为转向轴，瞬心位于第三轴延长线上(图1)。此时，对单轨模型，存在如下关系：
[0180][0181]
进而：
[0182][0183]
步骤1.4：利用matlab/simulink软件建立四自由度整车模型，同时利用trucksim软件建立五轴车辆整车模型，通过比较两种模型在相同给定输入情形下的动力学响应，来验证四自由度整车模型的准确性。车辆模型部分参数如表1所示。
[0184]
表1车辆模型参数
[0185][0186]
前期结合实车路测,利用speedbox-ins采集了实车在转向激励下的姿态数据，通过对比采用的trucksim模型与实车动力学响应，验证了trucksim模型的准确性。
[0187]
对两种模型在方向盘转角阶跃输入和正弦输入激励下进行仿真验证，车速设置为72km/h，仿真结果如图2所示。
[0188]
可以看出，在相同的给定输入激励下，简化的四自由度动力学模型在质心侧偏角响应和横摆角速度响应上与trucksim模型具有良好的一致性，验证了本文所提模型的正确性。下一步本发明将结合所建立的简化动力学模型，考虑车辆行驶时自身及周围车辆动力学状态，生成五轴特种车安全稳定的行驶轨迹。
[0189]
步骤2：基于步骤1建立的五轴特种车辆动力学模型，参考周围车辆和特种车的相对运动状态，基于恒加速度假设，建立五轴特种车辆行驶风险场模型，同时基于车辆横摆稳定性和侧倾安全性，建立五轴特种车辆动力学约束模型。具体为：
[0190]
步骤2.1.1：基于二元高斯分布函数，建立五轴特种车辆预测风险场：
[0191][0192]
其中，x
ret
、y
ret
为本车和周围车辆间相对位置；z为碰撞调节系数。σ
x
、σy为风险场调节因子；
[0193]
对本车周围第i台车辆，由于车辆相对行驶方向，相对坐标按如下方式定义：
[0194][0195]
表示本车与周围第i台车的航向夹角，(x0，y0)为本车在大地坐标系下的坐标，(xi，yi)为第i台车辆在大地坐标系下位置；
[0196]
步骤2.1.2：考虑到由于障碍车和本车运动状态差异会导致碰撞风险不同，车辆间
相对运动速度或加速度的正负决定了车辆未来时刻是否发生碰撞(表2)，定义系数σ为风险场调节因子，本车某时刻运动状态为
[0197]
分别表示车辆位置、速度、加速度以及航向角，周围车辆运动状态为则按照运动状态不同，车辆间的碰撞风险度存在表2所示关系。
[0198]
表2车辆碰撞风险与车辆运动状态关系
[0199][0200]
综合考虑车辆间的相对运动状态对碰撞风险影响，风险场调节因子σ计算公式为：
[0201][0202]
式中，为常数系数，δx和δy分别表示本车与周围车辆x方向和y方向差值，a
x0
、a
y0
为本车横向和纵向加速度，v
x0
、v
y0
为本车横向和纵向速度，c
x
、cy根据实际车速确定，采用如下计算公式：
[0203][0204]
上式中，表示道路最高限速和最低限速；
[0205]
对于结构化道路，除周围车辆等动态障碍外，还包括道路边界及道路中央掉落物等静态障碍。考虑五轴特种车自身运动状态，对静态障碍调节因子定义方式与上式相同，其中速度和加速度值均为0。对道路边界定义为：
[0206][0207]
式中，z为常数系数，y1、y2分别为车辆到车道边界两边的最近距离，σy定义方式与式(15)相同；生成的风险场如图3所示。
[0208]
图3(c)为五轴车辆在以20m/s速度匀速行驶时，周围不同运动状态的车辆风险场分布示意图，可以看出，周围车辆的速度和加速度差异，导致碰撞风险场的分布差异。对b车，车辆行驶速度大于五轴车，因此五轴车在b车后方行驶时无碰撞风险，而在b车前方行驶时，碰撞风险则随着距离减小而增大，对c车则相反。
[0209]
步骤2.1.3：为建立未来n个时刻的风险场模型，基于恒加速度假设，跟据当前对他车运动状态的估计量，在下一时刻他车运动状态为：
[0210][0211]
其中t为两个运动时刻的时间间隔，xi、yi表示周围第i辆车在大地坐标系下横向位置和纵向位置
[0212]
若短时间内加速度为定值a
x
，ay，在t时刻预测未来n个时刻运动状态为：
[0213][0214]
步骤2的五轴特种车辆动力学约束模型包括车辆稳定性约束和车辆侧倾约束；
[0215]
车辆稳定性约束具体为：
[0216]
步骤2.2.1：对车辆稳定性约束主要考虑轮胎失稳边界。如图4所示，本文基于横摆角速度-质心侧偏角相平面，生成车辆稳定性包络面。
[0217]
考虑到车辆巡航状态下车速波动较小，假设车辆匀速行驶，忽略侧倾影响，车体动力学方程简化为二自由度：
[0218][0219]
式中，β为质心侧偏角，其大小为vy/vx，车辆稳定状态下，质心侧偏角变化率为0，同时地面提供的最大附着力，若地面附着系数为μ，将最大稳定横摆角速度表示为：
[0220][0221]
为表征车辆稳定状态同时简化约束模型，对质心侧偏角约束结合非转向轴轮胎力饱和情况，结合式(8)得：
[0222][0223]
其中，轮胎饱和侧偏角近似由下式计算：
[0224][0225]
其中，c
α
为轮胎侧向刚度，fz为轮胎垂向力，由于五轴车辆车身与轮胎悬架构成超静定系统，考虑变形协调条件，将第三轴轮胎垂向力表示为：
[0226][0227]
式中，mb为车辆簧载质量，mw为车辆簧下质量，正负号约定与步骤1.2.3相同；
[0228]
步骤2.2.2：车辆侧倾约束
[0229]
对车辆侧倾约束基于零力矩点位置偏移，若地面存在一点相对车辆质心偏移y
zm
，车辆重力、惯性力和地面对车体作用力相对此点产生车辆侧倾力矩之和为零，则得如下关系式：
[0230][0231]
由于车身侧倾角较小，基于小角度假设，进而：
[0232][0233]
通过限制车辆质心偏移y
zm
，约束车辆侧倾：
[0234]-y
zm，max
≤y
zm
≤y
zm，max
(27)
[0235]
最终，对车辆行驶稳定性约束可表示为：
[0236][0237]
优选的，所述步骤3的mpc轨迹规划器设计具体为：
[0238]
步骤3：以步骤2建立的五轴特种车辆行驶风险场模型为软约束，五轴特种车辆动力学约束模型为硬约束，设计mpc轨迹规划器，生成安全行驶轨迹。
[0239]
步骤3.1.1：将步骤1建立的车辆动力学模型记载为：
[0240][0241]
式中，ξ表示此轨迹规划器系统的状态量，为u表示系统控制量，为[δ1，ωi]。
[0242]
步骤3.1.2：步骤3.1.1的系统为非线性系统，将其在当前运行时刻线性化，并定义系统输出量为则系统表示为：
[0243][0244]
式中，矩阵矩阵为雅各比矩阵，d
t
为线性化误差；
[0245]
步骤3.1.3：基于当前时刻t，生成未来n个时刻行驶轨迹，将以上系统采用前向欧拉法离散，进而将系统表示为：
[0246]
[0247]
式中，t表示离散时长，离散后的线性化误差表示为：
[0248][0249]
步骤3.1.4：采用控制增量δu(t)代替控制量u(t)，通过直接对控制增量进行约束可实现车辆平滑轨迹规划，若控制增量δu(t)＝u(t)-u(t-1)，则将式(30)化为：
[0250][0251]
将式(34)表示为：
[0252][0253]
若预测时域为h
p
，控制时域为hc，且h
p
＞hc，假定在当前时刻t，给定控制时域[t,t+h
c-1]一系列控制量δu1，δu2，...，δum；在控制时域外[t+hc，t+h
p-1]，给定控制量为0，则在预测时域内的预测输出表示为：
[0254][0255]
根据式(36)，以当前被优化系列输入量为未知量计算最优输出轨迹。
[0256]
步骤3的最优行驶轨迹生成具体为：
[0257]
五轴特种车保持车道行驶时，所设计的mpc规划器获取车辆运动状态，跟据预定的时间间隔，循环求解符合动力学约束的最优轨迹，流程如图5所示。
[0258]
步骤3.2.1：以步骤2中的行驶风险场模型及动力学约束模型为约束，规划器在求解时长t内循环求解预测时域内的最优问题j，定义最优化问题j为：
[0259][0260]
式中，tra为行驶轨迹，tra
ref
为参考行驶轨迹输入，v
x
为车辆纵向速度，v
xref
为参考车速，p为预测时域内跟据他车运动状态观测结果和道路边界生成的运动风险场，通过式(13)-(19)计算；
[0261]
步骤3.2.2：κ为预测时域内某时刻车辆行驶轨迹的曲率，计算方式为：
[0262][0263]
式中，x、y为规划器输出轨迹点的三次样条拟合曲线，其边界条件通过约束起始点和终止点车辆横摆角状态给定，则在时刻t，规划器输出位置为：
[0264][0265]
步骤3.2.3：若车辆容许最大侧向加速度为|a
lat
|
max
，则对车辆速度约束：
[0266][0267]
式中，v
sign
为车道限速，涉及车辆行驶平顺性和乘车舒适性，并结合式(27)安全性约束，最优化问题表示为：
[0268][0269]
在预测时域h
p
内，规划器通过求解最优化问题(42)，生成车辆连续行驶轨迹，下层控制器通过实时跟踪行驶轨迹，实现车辆结构化道路的自动行驶。
[0270]
仿真实验验证
[0271]
为验证方法的正确性，基于matlab/simulink和trucksim软件搭建联合仿真平台，分别在直行道路和连续弯道场景测试轨迹生成方法。所设定的仿真场景包含道路边界线，周围行驶车辆以及道路中央静态障碍。上层规划器采用本文方法实现，下层采用pid控制器实现轨迹跟踪。规划验证原理如图6所示。
[0272]
轨迹规划器相关参数如表3所示。车辆模型参数与表1相同。
[0273]
表3规划器部分参数
[0274][0275]
直线道路行驶工况
[0276]
若五轴特种车在直行道路处于巡航行驶状态，在t时刻，设置周边车辆数目为3，其相对位置如图7所示，且道路前方存在静态障碍a_obs。
[0277]
五轴特种车辆参考车速分别设定为18m/s和20m/s，在两种巡航速度下对车辆进行测试，各车辆运动状态如表4所示。其中，表中数据格式为[pi,,vi,,ai],i＝a,b,c。不同车速下的规划轨迹如图8、图9所示。
[0278]
表4直行工况下车辆状态
[0279][0280]
在图8和图9中，上图表示采用本文方法规划出的车辆行驶轨迹和规划结束后的车辆最终位置状态，下图表示本车行驶的实际车速。
[0281]
图8中，当车辆以18m/s速度行驶时，由于前方存在障碍aobs,车辆开始减速以降低
碰撞风险。车辆绕过障碍后(位置100m处)，前方b车以10m/s速度匀速向前行驶，同时更远处a车以相同的速度匀速行驶，为避免与a、b两车碰撞，本车在保证自身行驶稳定性的前提下，首先远离b车。当本车接近位于左前方a车后，与a车保持一定的纵向距离，同时加速超过b车(100m-220m处)。之后，本车在270m处持续加速，远离a车和b车，避免产生碰撞。由于c车始终以20m/s速度保持前向行驶，因此对本车未产生影响。
[0282]
对图9，本车以25m/s速度向前匀速行驶。此工况下，由于a车距离本车较远，且处于加速状态，c车速度大于本车车速，因此两车对本车行驶轨迹无影响。本车在经过减速降低碰撞风险绕过障碍aobs后，在250m处超过b车，并开始加速直至参考车速。不同车速下车辆行驶时的时空图如图10所示，行驶过程中的质心侧偏角-横摆角速度相轨迹和侧倾角如图11所示。
[0283]
图10中的x-y轴表示车辆的平面位置，z轴表示时间，此三维空间表明了在不同时刻，本车和周围车辆所处的空间位置。跟据图10所示轨迹时空图，本车按照规划轨迹行驶时，通过路径-速度协同规划能够实现动态避障，同时图11表明，在对车辆转角和轮速约束的基础上规划车辆轨迹可保证其行驶稳定性和安全性。
[0284]
弯路行驶工况
[0285]
为进一步验证行驶轨迹生成方法的正确性，考虑弯道行驶工况进行测试，测试设定五轴特种车周围存在三辆行驶车辆，本车行驶速度设定为20m/s，周围车辆初始车速分别设定为a车15m/s，b车10m/s，c车13m/s，三辆车在道路行驶时变速行驶，其加速度变化范围为0-2m/s2。车辆相对位置和行驶轨迹如图12所示，车辆行驶时空图如图13所示。
[0286]
结合图13车辆行驶时空图，本车在曲线道路行驶时，于100m处开始转弯，此时为保证本车避障稳定性和安全性，车速降低。车辆入弯后，本车首先实现对车速较低的b车避障，由于b车当前正处于加速行驶阶段，本车在120m处逐渐加速直至超越b车，同时由于c车与本车横向距离较远，本车接近c车后对c车进行轻微规避并超越c车。之后，车辆逐渐减速至参考车速，在380m附近开始跟随a车行驶。本车行驶过程中的相平面轨迹和侧倾角变化如图14所示。
[0287]
图14(a)表明，在弯路行驶工况下，本车进行轨迹规划时，其相轨迹始终处于稳定性区间，极限情况下，车辆能够主动降速以保证避障时的稳定性，同时有效控制车身侧倾(图14(b))。
[0288]
为进一步阐明本文方法的效果，在相同条件下生成不考虑避障稳定性的驾驶轨迹，如图15所示。同时，生成车辆无约束轨迹行驶时的相轨迹及侧倾角变化，如图16所示。
[0289]
图15中，蓝色轨迹为考虑避障稳定性约束下生成的行驶轨迹，红色轨迹为不考虑避障稳定性生成的行驶轨迹。由图15可以看出，本车为实现稳定避障，规划的轨迹较平滑。相对于考虑避障稳定性的轨迹，红色轨迹局部存在较大曲率区域，意味着车辆控制器进行轨迹跟踪时存在较大的转向角变化率。同时对比分析图16，此时本车横摆角速度变化率也超出了稳定性区域，造成车辆失稳。
[0290]
综上所述：
[0291]
本发明以五轴特种车为例，针对多轴特种车辆高速机动时的稳定安全性问题，提出一种考虑避障稳定性的多轴特种车辆安全轨迹规划方法。
[0292]
(1)以五轴特种车为例，建立考虑纵向、侧向、横摆和侧倾四自由度的动力学模型，
并通过比较trucksim模型和所建立模型的动力学响应，验证模型的正确性。
[0293]
(2)针对多轴特种车辆的避障稳定性问题，基于质心侧偏角-横摆角速度相平面，建立多轴特种车辆稳定性约束；基于车辆运动状态，建立多轴特种车辆障碍风险场。
[0294]
(3)为生成多轴特种车辆安全行驶轨迹，设计了基于模型预测控制的轨迹规划器，仿真实验证明，所设计的规划器能够满足多轴特种车辆避障稳定性和行驶安全性需求。
[0295]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。