一种基于带挂车模型的B样条轨迹规划方法与流程

本发明属于自动驾驶，涉及到车辆的轨迹规划，具体涉及到一种基于带挂车模型的b样条轨迹规划方法。

背景技术：

1、在自动驾驶中，绕行障碍物是一个典型的常见场景需求。在该场景中，轨迹规划承担的角色是获取地图、感知、定位等信息，并在收到目标点位置信息后，规划出一条自身位置出发到达目标点的，能够绕开所有障碍物，且平滑的，可供控制系统跟踪的轨迹曲线。

2、轨迹规划所针对的车辆中，带挂车的需求逐步提高。带挂车量常见于大型货车，一般由车头和挂车两个部分组成。车头和挂车之间由铰链链接，可以产生平面转动，铰链的角度在车辆转弯的时候会发生变化。

3、在一些复杂的场景，比如障碍物数量众多，绕行距离相对较远，给轨迹规划算法提出了更严苛的需求，一些更为复杂的轨迹规划算法应运而生，这其中就包括基于b样条的优化算法，在b样条的优化算法过程中，需要构建碰撞模型，而碰撞模型是在轨迹规划中，用几何线条近似表达出的自车轮廓，在传统方法中，碰撞模型一般是以自车长为长边、自车宽为宽边的矩形，基准点在后轴中心，基准点走过的轨迹即行车轨迹，矩形长边方向为车头方向，也是轨迹点切向。

4、如果沿用传统的碰撞模型到带挂车的轨迹规划中，就会出现一些弊端，模型基准点在车头后轴中心，长边长度为整车长，宽边宽度参考最小转弯半径下的车尾最远点的位置，此为了保证碰撞模型在任何情况下的有效性，以下称该模型为传统碰撞模型，在建立限制条件时，传统碰撞模型会用来做碰撞检测，以此确定横向移动范围。可以看到传统碰撞模型比自车的轮廓大了很多，在保证轨迹安全的同时，也极大的压缩了轨迹规划的求解空间。特别是在一些弯道大的地方，由于求解空间的压缩，很容易产生没有解的情况。此外，该算法只能保证车头轨迹的平滑平顺性。由于车头和挂车之间没有刚性连接，挂车的实际运行轨迹质量会在一定程度上遭受损失。在带挂车中，挂车不仅是从体积还是质量上在整车中所占比重都很高，因此从经济型和安全性上讲，针对挂车的轨迹质量的优化都存在需求和空间。

技术实现思路

1、针对上述问题，本发明的主要目的在于设计一种基于带挂车模型的b样条轨迹规划方法,解决弯道较大的地方，碰撞模型比自车的轮廓大较多，极大压缩了轨迹规划的求解空间，不利于优化挂车轨迹质量的技术问题。

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

3、一种基于带挂车模型的b样条轨迹规划方法，包括：

4、步骤1：根据起始点和障碍物信息，用搜索的方法得到初始轨迹；

5、步骤2：用b样条曲线对初始轨迹进行插值，得到控制点作为优化变量；

6、步骤3：在b样条曲线上进行均匀采样得到采样点；

7、步骤4：以挂车后轴中心为基准，建立挂车碰撞模型，并根据障碍物信息计算采样点的横向移动范围，将横向移动范围作为限制条件；

8、步骤5：构建起始点的限制条件；

9、步骤6：建立目标函数；

10、步骤7：将限制条件和目标函数放入优化求解器求解，得到优化后的b样条曲线，在优化后的b样条曲线上采样，得到采样点组成的轨迹即为挂车后轴中心的轨迹；

11、步骤8：根据车头与挂车的几何关系，推算车头的后轴中心轨迹。

12、作为本发明的进一步描述，步骤1中，所述的初始轨迹以轨迹点的形式进行保存pti，i＝1…np；其中np为轨迹点的个数；

13、所述初始轨迹为连接起点和终点，且将挂车碰撞模型放在起点和终点都不与障碍物发生碰撞的曲线，表示为：

14、rt＝[x y θ]t

15、其中，x，y，θ，为一个轨迹点中包含的三个状态，θ是航向角，为曲线在轨迹点的切线方向。

16、作为本发明的进一步描述，步骤2中，对初始轨迹pti进行插值，得到b样条曲线：bs(u)＝∑inipi；

17、其中，ｕ是节点向量，pi是控制点，ni是转化矩阵。

18、进一步的，在插值时，将每一个轨迹点pti所在弧长si等比映射到节点空间ui，可以得到一系列的方程，即：

19、

20、通过求解上述方程，即可得到所有的控制点。

21、作为本发明的进一步描述，步骤3中，对b样条曲线进行采样，得到采样点pbi，i＝1…nb，nb为采样点的个数；

22、获取所有在所述b样条曲线左边的障碍物和道路边界，以线段的形式保存sli，i＝1...nl，nl为左边线段的个数；

23、获取所有在所述b样条曲线右边的障碍物和道路边界，以线段的形式保存sri，i＝1…nr，nr为左边线段的个数；

24、每条线段都包含一个起点pstart,一个终点pend，sl/sr＝[pstart pend]t。

25、作为本发明的进一步描述，步骤4中，所述的挂车碰撞模型为用几何线条表达出的挂车轮廓；所述的挂车轮廓为以挂车后轴中心为基准点，整车长作为边长，整车宽作为边宽；

26、将挂车碰撞模型放在采样点上，在采样点处的法向上左右平移挂车碰撞模型，针对挂车碰撞模型与所有sl/sr求交，得出向左最大不碰撞的距离lleft和向右最大不碰撞的距离lright；

27、则，限制条件的公式为lright≤l≤lleft；其中，l为采样点处的横向距离。

28、作为本发明的进一步描述，步骤5中，设置起点和终点的限制条件：

29、起点posei＝[xi yi θi]t和终点posef＝[xf yf θf]t，则f[bs(ui)}＝posei，f[bs(uf)]＝posef；

30、其中，ui是b样条曲线起点所对应的节点向量，uf是b样条曲线终点所对应的节点向量。

31、作为本发明的进一步描述，步骤6中，设置代价函数

32、其中，bs′为b样条曲线的一阶导数，bs″为b样条曲线的二阶导数，bs[3]为b样条曲线的三阶导数，c1为b样条曲线一阶导数的系数，c2为b样条曲线二阶导数的系数，c3为b样条曲线三阶导数的系数。

33、作为本发明的进一步描述，步骤7中，将步骤4、步骤5的限制条件和步骤6的代价函数代入求解器，得到优化后的b样条曲线函数

34、在优化后的b样条曲线函数上进行采样，得到一系列的位姿点所有位姿点组成的轨迹即是挂车后轴中心的轨迹。

35、作为本发明的进一步描述，步骤8中，根据车头和挂车的几何关系，推算车头的后轴中心轨迹，包括如下步骤：

36、步骤8.1：针对挂车后轴中心轨迹的轨迹点，计算对应的曲率半径；

37、步骤8.2：曲率半径小于设定阈值，计算小曲率半径下的车头后轴中心轨迹点的坐标；

38、步骤8.3：曲率半径大于设定阈值，计算大曲率半径下的车头后轴中心轨迹点的坐标；

39、步骤8.4：综合步骤8.2和步骤8.3的轨迹点，得到车头后轴中心的整条轨迹。

40、一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信，其特征在于：所述存储器，用于存储计算机程序；

41、所述处理器，用于通过运行所述存储器上所存储的所述计算机程序，执行上述基于带挂车模型的b样条轨迹规划方法的步骤。

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

43、本发明提供了一种基于带挂车模型的b样条轨迹规划方法，将挂车碰撞模型与b样条曲线优化算法结合，使挂车碰撞模型检测更加精确，避免优化求解空间被过度压缩，提高优化求解成功率；同时，使挂车后轴中心轨迹更加平滑，由于挂车的质量和体积占整车的比重较大，因此整车的安全性和节能减排效果得到提升。