本发明涉及智能交通技术，尤其涉及一种基于层次迭代搜索的半挂牵引车轨迹规划方法。
背景技术：
1、半挂牵引车作为公路运输的主要工具，其自动驾驶技术的完善对提高交通安全等级，推动物流产业升级，打造智慧城市具有重大意义。美国汽车工程师学会(sae)根据车辆智能程度，将智能车辆划分为六个等级：无自动化，驾驶支持，部分自动化，有条件自动化，高度自动化，完全自动化。社会各界普遍认为自动驾驶技术将在未来几十年快速发展，且相较于行驶环境更加复杂的乘用车，以干线物流和园区调度为主要应用场景的自动驾驶半挂牵引车具备更加明确的应用场景和更低的落地成本，因此被认为是最适合应用自动驾驶技术的对象之一。
2、现有技术中，多数自动驾驶轨迹规划方法以普通乘用车为研究对象，无法直接应用于自动驾驶半挂牵引车轨迹规划当中。此外，相较于乘用车，自动驾驶半挂牵引车重心高，载重大，行驶工况复杂，由于铰接结构和非线性约束的存在，其局部路径轨迹规划问题更加复杂。现有自动驾驶半挂牵引车轨迹规划理论与算法存在计算时间过长，约束理想化等问题，难以实现实时动态轨迹求解与控制。
技术实现思路
1、本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于层次迭代搜索的半挂牵引车轨迹规划方法。
2、本发明解决其技术问题所采用的技术方案是：一种基于层次迭代搜索的半挂牵引车轨迹规划方法，包括以下步骤：
3、1)通过车载设备实时采集半挂牵引车的交通信息数据；所述交通信息数据包括：实时车辆的速度、车辆的位置及姿态信息，车辆周围的rgb图像信息和点云图像信息；
4、2)车道级高精地图构建全局道路参考线，建立道路坐标系，获取车辆在道路坐标系的位置；
5、3)通过实时采集的前车速度和前车与车辆车头间距，确定车辆的目标速度；
6、4)通过获取的道路交通信息和车辆的目标速度，计算横向采样间隔和纵向采样间隔、采样时域以及样条采样数量；
7、以横纵向采样间隔、采样时域以及样条采样数量构建典型采样空间，并以全局道路参考线为采样中心，在典型的采样空间进行横向和纵向的状态采样，包括横向和纵向的位置，速度，加速度；通过构建采样轨迹先验估计损失函数，计算采样的始末状态与目标状态的偏差，对采样空间内所有轨迹的损失值大小进行估计；
8、横向采样间隔dgap,纵向采样间隔vgap，采样时域数量nt和样条采样数量nsample可以通过下式进行计算：
9、
10、vgap＝v>30？3∶5
11、
12、式中，ωd和ωt为调整系数；wr为道路宽度；wr为车辆宽度；vnow为车辆当前行驶速度；vtarget为车辆目标行驶速度；vlimit为道路限速；vgap为速度采样间隔，通常为5；tmax为最大采样时域；tmin为最小采样时域；ws为可行使区域宽度；vgap为速度采样间隔。
13、5)通过统计每一个采样平面内所有采样轨迹估计损失总和，确定采样空间内的优选采样平面；
14、通过估计采样平面内轨迹损失大小，选取每个采样平面损失最小的前n条轨迹，建立优选采样平面内的初始猜测轨迹解集合；
15、通过猜测解的始末采样状态，求解猜测解的牵引车完整运行轨迹；
16、6)建立半挂牵引车耦合运动学模型，通过牵引车的运行轨迹，求解拖挂车的运行轨迹，实现半挂牵引车整体(牵引车和拖挂车)的完整运行轨迹；
17、7)通过建立完整损失函数，评估猜测最优轨迹的完整损失；通过对猜测解周围的完整轨迹进行求解并计算完整轨迹损失，评估最优轨迹的最优性，如果猜测解为最优解，则接受猜测解为最优解，反之则通过梯度法计算采样更新方向，更新猜测解集合至收敛；
18、8)通过建立运动学约束条件和安全性约束条件，检查猜测解集合内所有轨迹的可行性，然后从猜测解集合内取出损失最小的可行猜测解为最优可行解，并根据最优可行解完成局部轨迹规划过程。
19、按上述方案，所述步骤4)中，
20、先验损失估计为cest，具体如下：
21、cest＝cterminal+ccontinue
22、式中，ccontinue是轨迹连续性损失；cterminal是终端偏移损失；
23、其中，ccontinue和cterminal具体可通过下式进行计算
24、
25、按上述方案，所述步骤5)中，
26、采样平面损失总和的计算方法具体如下：
27、
28、式中，ch是采样平面损失，
29、cs,d为采样状态是(s,d)时的估计损失。
30、按上述方案，所述步骤5)中，
31、通过猜测解的始末采样状态，分别利用五次多项式和四次多项式轨迹连接横向和纵向的始末状态，求解猜测解的牵引车完整运行轨迹；
32、所述纵向四次多项式轨迹可以用下式进行描述：
33、s(t)＝cs0+cs1t+cs2t2+cs3t3+cs4t4
34、式中，cs0是多项式系数，
35、t是时间，
36、s是纵向运动距离。
37、步骤4中所述横向五次多项式轨迹可以用下式进行描述：
38、d(t)＝cd0+cd1t+cd2t2+cd3t3+cd4t4+cd5t5
39、式中，c为多项式系数；t为时间；d为纵向运动距离。
40、按上述方案，所述步骤5)中，完整损失函数通过下式进行描述：
41、ctotal＝cprocess+cterminal
42、
43、式中，cprocess是过程损失；cterminal是终端损失，w为权重系数，jlat,jlon代表横、纵向加加速度，alat,alat代表横纵向加速度，t为采样时域。
44、按上述方案，所述步骤6)中，半挂牵引车运动学模型可以通过下式进行描述：
45、
46、式中，δf为牵引车前轴转向角，lhead是牵引车的轴距，dt为采样时间间隔，是t时刻牵引车和拖挂车的航向角变化率，分别是t时刻牵引车和拖挂车的航向角，(xthead,ythead)为t时刻牵引车的后轴中心的横纵坐标，为t时刻拖挂车后轴中心的横纵坐标变化率。
47、按上述方案，所述步骤8)中，运动学约束条件为：
48、
49、式中，v是车辆速度；vlimit是道路限速；a是加速度；amin、amax分别是最小、最大加速度；θhead、θtrailer分别是牵引车和拖挂车的航向角；δθmax是最大铰接角。
50、按上述方案，所述步骤8)中，安全性约束条件采用多圆包络法的安全约束条件，具体分为包络圆心计算和碰撞约束条件两部分，包络圆心通过下式进行计算：
51、
52、碰撞约束条件通过下式进行描述：
53、
54、式中，是牵引车第i个包络圆的圆心，是拖挂车第i个包络圆的圆心；(xh,yh)是牵引车后轴中心坐标；(xt,yt)是拖挂车的后轴中心位置；rveh、robs分别是车辆包络圆和障碍物的半径；θh是牵引车的航向角；θt是拖挂车的航向角。
55、本发明产生的有益效果是：
56、本发明提供了一种实时性高的半挂牵引车轨迹规划方法，降低由于轨迹规划不及时造成的交通事故发生率，能有效提升车辆行驶过程中的安全性和稳定性。
1.一种基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，所述步骤4)中，
3.根据权利要求1所述的基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，所述步骤5)中，
4.根据权利要求1所述的基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，所述步骤5)中，
5.根据权利要求1所述的基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，所述步骤5)中，
6.根据权利要求1所述的基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，所述步骤6)中，半挂牵引车运动学模型通过下式进行描述：
7.根据权利要求1所述的基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，所述步骤8)中，运动学约束条件为：
8.根据权利要求1所述的基于层次迭代搜索的半挂牵引车轨迹规划方法，其特征在于，所述步骤8)中，安全性约束条件采用多圆包络法的安全约束条件，具体分为包络圆心计算和碰撞约束条件两部分，包络圆心通过下式进行计算：
9.一种电子设备，其特征在于，
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至8任一项所述的方法。
