一种基于多层可通行空间的轨迹规划方法与流程

本发明属于汽车轨迹规划，具体涉及到一种基于多层可通行空间的轨迹规划方法。

背景技术：

1、自动驾驶作为推动港口发展的关键，其应用和普及能显著提高港口运营效率并降低成本。然而，港口众多的狭窄场景，如过锁扭站、过换电站也给自动驾驶轨迹规划带来了诸多挑战。

2、对于障碍物，传统的轨迹规划只考虑障碍物的二维平面信息，而忽略了z轴高度信息。具体来说，将三维立体直接投影到二维平面。由于这种方案损失了障碍物的高度信息，因此投影时会保守处理，得到所有高度上最大的二维投影面积，这会造成通行空间的浪费，使得一些极限狭窄场景无法通过。

3、以车辆穿过两侧锁扭站的场景进行说明，通常两侧锁扭站之间的宽度从高到低是不一致的，下半部分较窄(3200mm)，上半部分较宽(3700mm)，整车的宽度也不是统一的，如承装平台最宽，底盘次之，集装箱最窄。按传统方法，通行空间、车辆宽度按同一宽度处理，出于安全考虑，通行宽度取最窄处(3200mm)，车辆宽度按最宽处取(3500mm)，显然车辆宽度大于通行宽度，最终得出了车辆无法通行的结论，而没有对障碍物、车辆做精细化的考虑，忽略了不同高度的宽度不一致这个关健信息，在实际上存在通行空间的情况下得出了无法通行的错误结论。

4、除了锁扭站外，港口典型的窄道场景还包括：换电站、两侧低矮栅栏等，这些都是自动驾驶车辆在港口作业过程中必不可少的通行环节。因此，顺利、高效通过窄道空间对自动驾驶车辆，特别是宽体自动驾驶车辆，具有重要意义，有利于提升全场景通过性。

技术实现思路

1、针对上述问题，本发明的主要目的在于设计一种基于多层可通行空间的轨迹规划方法，融合障碍物三维信息的多层可行域解析，得到精细化的轨迹，解决狭窄场景下自动驾驶的通行问题。

2、为了实现上述目的本发明采用如下技术方案：

3、一种基于多层可通行空间的轨迹规划方法，包括如下步骤：

4、步骤1：将车辆与障碍物进行分层，并使用线条表征障碍物的外轮廓，得到障碍物集合；

5、步骤2：针对多层可通行域解析，计算车辆的可行驶范围，包括如下过程：

6、步骤2.1：根据地图参考线进行采样，得到采样序列；

7、步骤2.2：计算每个采样点到两侧线条的距离；

8、步骤2.3：遍历完采样序列的所有采样点，得到每一层的可行域；

9、步骤3：建立轨迹规划模型，并求解优化轨迹。

10、作为本发明进一步的描述，所述步骤1中，每个线条line包括的信息为：编号，线条起点及终点的x、y、z轴坐标信息，线条起点及终点在frenet坐标下的sl信息，线条的绕行方向。

11、作为本发明进一步的描述，根据车辆的外部结构从低到高进行高度分层，障碍物与车辆的高度分层一致；

12、障碍物每层包含若干线条，即layer0＝l{ine 0l,ine l1i,ne 2...}，障碍物的线条组成障碍物集合mdb＝{layer 0l,ayer l1a,yer 2…}。

13、作为本发明进一步的描述，所述步骤2.1中，沿着参考线按一定分辨率delta_s＝0.2m采样，得到采样序列s＝{s0,s1,s2...}。

14、作为本发明进一步的描述，所述步骤2.3中，综合每一层的可行域，得到车辆的最终可行域，最终可行域包括硬边界、软边界，表达式为：

15、barrier_set＝{s0,l_lower0,l_upper0,s1,l_lower1,l_upper1...}；

16、soft_set＝{s0,l_lower0,l_upper0,s1,l_lower1,l_upper1...}；

17、其中，上述barrier为硬边界，soft为软边界，下标的数字代表采样点的排序，l_lower0,l_upper0为第0个采样点的可行域的右边界和左边界；

18、对于任意的采样点，通过对其s进行插值，可得到对应s的软边界与硬边界。

19、作为本发明进一步的描述，所述步骤3中，建立轨迹规划模型包括如下步骤：

20、步骤3.1：根据车辆运动学特性，通过状态量x＝[x,y,theta,phif,phir]，构建状态转移方程，表达式为：

21、

22、其中，x,y表示车体中心坐标，theta表示车体朝向，phif,phir分别表示车体前后轮转角；phi＝atan((tan(phif)+tan(phir))/2)，表示车辆实际行驶方向与车头朝向的角度，kappa＝(tan(phif)-tan(phir))/wheelbase，表示由车辆前后轮转角计算得到的曲率，wheelbase表示车辆轴距；

23、步骤3.2：设计代价函数；

24、根据参考线上的轨迹点对应信息(xr,yr,thetar)，得到在frenet下的横向偏移为：l＝-sin(thetar)*(x-xr)+cos(thetar)*(y-yr)；

25、其中，xr,yr表示车体中心坐标的参考值，thetar表示车体朝向的参考值；

26、偏离参考线代价：0.5*ref_weight*l*l+0.5*theta_weight*(theta-thetar)*(theta-thetar)；

27、其中，weight表示对应变量的权重；ref_weight表示为参考线权重，theta_weight表示为车体朝向权重；

28、曲率：

29、0.5*kappa_weight*kappa*kappa；

30、其中，kappa_weight表示为曲率权重；

31、曲率变化率：

32、0.5*dphif_weight*dphif*dphif+dphir_weight*dphir*dphir；

33、其中，dphif_weight表示为前轮转角变化率的权重，dphir_weight表示为后轮转角变化率的权重，dphif表示为前轮转角变化率，dphir表示为后轮转角变化率；

34、步骤3.3：约束设计；

35、l_lower≤l≤l_upper；

36、sl_lower≤sl≤sl_upper；

37、phif_lower≤phif≤phif_upper；

38、phir_lower≤phir≤phir_upper；

39、dphif_lower≤dphif≤dphif_upper；

40、dphir_lower≤dphir≤dphir_upper；

41、其中，l_lower、l_upper分别表示采样点可行域的右边界和左边界，sl、sl_lower、sl_upper分别表示采样点可行域软边界、软边下界、软边上界，phif_lower、phif_upper分别表示车体前轮转角的下界和上界，phir_lower、phir_upper分别表示车体后轮转角的右边界和左边界，dphif_lower、dphif_upper分别表示前轮转角的下界和上界，dphir_lower、dphir_upper分别表示前轮转角变化率的下界和上界。

42、作为本发明进一步的描述，所述步骤3中，通过求解器求解轨迹规划模型，得到可通行空间的轨迹。

43、相对于现有技术，本发明的技术效果为：

44、本发明提供了一种基于多层可通行空间的轨迹规划方法，通过将障碍物与车辆进行分层，并对分层求解后的可行域信息进行融合，得到最终的可通行空间，结合使用mpc进行规划问题建模、求解，得到规划轨迹；该方法融合三维信息的障碍物表征方法，实现可通行空间的分层精细化建模；对带有高度信息的多层障碍物进行分层通行空间解析，得到精细化的多层可通行域，提升狭窄场景下的通过性。