一种基于带挂车模型的最优轨迹规划方法与流程

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

背景技术：

1、自动驾驶带挂车的运动规划，在结构化道路环境中驾驶是车辆的常见情况，其运动规划需要更高阶的平滑路径，同时需要满足避障的需求，优化算法可以很好的保证这两点需求，所以是该场景轨迹规划中最合适的方法。

2、对于这种复杂的道路环境和障碍物避让情况，带挂车辆的自动驾驶，相比非带挂的，要更具挑战。由于挂车以铰接的方式连接到牵引车，这相当于增加了一个被动自由度，而系统的输入数量保持不变，这使得系统更加复杂，将此系统用于优化算法中会增加求解成功的难度。

3、现有技术，在带挂车辆的运动规划中，通常只使用了质点模型，由于模型没有考虑到挂车的影响，为了避免挂车与障碍物的碰撞，只能通过类似增加安全距离的方式进行运动规划。但该规划方式较粗糙，对于一些狭窄场景无法精细化操作，要么安全距离给的太大导致无解，要么安全距离给的太小，虽然有解，但可能会产生碰撞。这使得车辆无法很好的处理多种多样的障碍物环境。

4、还有些现有技术虽然使用了带挂车运动学模型，在障碍物处理的时候使用增加碰撞势能或者采用多级走廊算法(multi stage corridors)虽然能满足避障要求，但在处理障碍物数量、种类变多后的复杂场景，就显得捉襟见肘，比如不同障碍物需要保持不同的安全距离。有些边界是绝对禁止跨越的，而有些边界在特殊情况可以跨越，比如车道线。传统方法在算力高效性和可靠性上都难以保证。

技术实现思路

1、针对上述问题，本发明的主要目的在于设计一种基于带挂车模型的最优轨迹规划方法，解决带挂车在极端狭窄空间和复杂障碍物避让情况下，路径规划和控制困难，以及性能差的技术问题，满足带挂车在结构化道路环境中的驾驶需求。

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

3、一种基于带挂车模型的最优轨迹规划方法，包括如下步骤：

4、步骤1：以车道的中心线为参考线，根据车道上障碍物的情况，计算车辆在车道上的可行驶横向距离并生成横向边界；

5、步骤2：基于步骤1生成的横向边界构建最优控制问题，所述最优控制问题的构建包括：

6、s21：构建带挂车运动学模型；

7、s22：计算检测点相对于参考线的横向距离，所述检测点包括牵引车后轴中心、牵引车前端点、挂车后端点；

8、s23：构建微分方程，计算检测点与参考线上对应的弧长；

9、s24：构建回调函数，根据步骤s23计算出不同检测点的弧长值，再根据步骤1中解析的横向边界，查询对应的横向边界值；

10、s25：针对横向边界构建约束；

11、s26：构建目标函数；

12、步骤3：求解步骤2中的最优控制问题，得到横向轨迹。

13、作为本发明进一步的描述，所述步骤1中，横向边界的生成包括如下步骤：

14、s101：将参考线分成连续若干段；

15、s102：根据车道边界，计算每段参考线所对应的左右边界[llmin，llmax]；

16、s103：根据车道上的障碍物位置更新左右边界；

17、s104：根据不同的绕行需求和障碍物安全距离，将每段的左右边界分为硬边界[llmin，llmax]和软边界[sllmin，sllmax]。

18、作为本发明进一步的描述，所述步骤s21中，构建带挂车运动学模型，表示为：

19、

20、

21、

22、

23、

24、其中，x，y是牵引车后轴中心在世界坐标系的平面坐标位置，κ是曲率，dκ是曲率变化率，α是挂车与牵引车的角度，li是挂车铰接点距离挂车后轴的距离，θ是牵引车的航向角。

25、作为本发明进一步的描述，所述步骤s22中，计算三个检测点相对于参考线的横向距离，包括如下步骤：

26、s2201：计算牵引车后轴中心相对于参考线的横向距离l，表示为：

27、l＝-sinθr(x-xr)+cosθr(y-yr)，

28、其中，xr，yr是牵引车后轴中心在参考线上对应点的坐标值，θr是对应的航线角；

29、s2202：计算牵引车前端点相对于参考线的横向距离lc，表示为：

30、lc＝-sinθcr(xc-xcr)+cosθcr(yc-ycr)，

31、其中，xcr，ycr是牵引车前端点在参考线上对应点的坐标值，xc，yc是牵引车前端点的世界坐标值，θcr是对应的航线角；

32、s2203：计算挂车后端点相对于参考线的横向距离li，表示为：

33、li＝-sinθir(xi-xir)+cosθir(yi-yir)，

34、其中，xir，yir是挂车后端点在参考线上对应点的坐标值，xi，yi是挂车后端点的世界坐标值，θr是对应的航线角。

35、作为本发明进一步的描述，所述步骤s23中，构建微分方程，计算带挂车运动模型上检测点与参考线上对应的弧长(s)，表示为：

36、

37、其中，κr是对应点的曲率。

38、作为本发明进一步的描述，所述步骤s24中，构建回调函数，根据不同检测点的s值，查询并更新步骤1中的横向边界；

39、得到牵引车后轴中心的横向硬边界[lmin，lmax]和横向软边界[slmin，slmax]；

40、得到牵引车前端点的横向硬边界[lcmin，lcmax]和横向软边界[slcmin，slcmax]；

41、得到挂车后端点的横向硬边界[limin，limax]和横向软边界[slimin，slimax]。

42、作为本发明进一步的描述，所述步骤s25中，构建的约束包括横向硬约束、横向软约束与其他约束，具体包括如下：

43、s2501：构建横向硬约束，表示为：

44、lmin≤l≤lmax，

45、lcmin≤lc≤lcmax，

46、limin≤li≤limax，

47、s2502：构建横向软约束，表示为：

48、slmin-slack≤l≤slmax+slack，

49、slcmin-slackc≤lc≤slcmax+slackc，

50、slimin-slacki≤li≤slimax+slacki，

51、其中，slack，slackc，slacki分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点对应性横向约束松弛变量；

52、s2503：其他约束包括曲率κ，曲率变化率dκ，挂车与牵引车的角度α的约束，表示为：

53、-κmax≤κ≤κmax，

54、-dκmax≤dκ≤dκmax，

55、-αmax≤α≤αmax.

56、其中，κmax是曲率最大值，dκmax是曲率变化率最大值，αmax是挂车转角最大值。

57、作为本发明进一步的描述，所述步骤s26中，构建目标函数包括状态目标函数、输入目标函数、松弛目标函数，表示为：

58、j＝jx+ju+js，

59、s2601：构建状态目标函数，表示为：

60、

61、其中，wl是横向距离权重，wκ是曲率权重；

62、s2602：构建输入目标函数，表示为：

63、

64、其中，wdκ是曲率变化权重；

65、s2603：构建松弛目标函数，表示为：

66、

67、其中，wslack，wslackc，wslacki分别是牵引车后轴中心、牵引车前端点、挂车后端点三个检测点的松弛变量权重。

68、作为本发明进一步的描述，所述步骤3中，通过求解器进行最优控制问题的求解，包括如下步骤：

69、s301：设置求解器时间点个数和时间间隔；

70、s302：求解步骤2中的最优控制问题，将最优解中所有时间点的x，y，θ，κ组合成一条横向轨迹。

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

72、本发明提供了一种基于带挂车模型的最优轨迹规划方法，创新的边界解析方法，能够便捷、高效处理结构化道路的复杂障碍物信息，结合考虑了带挂车运动学模型的最优控制问题的构建、求解，极大程度地提高轨迹规划在极端狭窄空间和复杂障碍物避让情况下的性能，使带挂车在复杂场景下的算力高效性和可靠性上得到保证。