一种集装箱卡车自主泊车运动规划方法、装置、设备及介质与流程

本发明涉及自动驾驶泊车规划领域，尤其是涉及一种适用于复杂场景的集装箱卡车自主泊车运动规划方法、装置、设备及介质。

背景技术：

1、随着经济社会的快速发展，经济结构调整步伐的加快，货物运输业得到了前所未有的发展。《智能网联汽车技术路线图2.0》提出：2025年左右，ca级自动驾驶货运车辆开始进入市场。2035年，fa级智能网联货运车辆开始应用于日常生活。标志着商用车辆的无人驾驶正式提上日程。铰接式车辆由于载货量大、运输成本低等优势，成为了交通运输智慧物流体系的重要部分，但铰接的形式也为运动规划带来一定难度，即在港口码头和物流园区等非结构化场景中，即使驾驶经验非常丰富的驾驶员，也很难一次性将车辆无碰撞的泊入库中。

2、于非结构化场景中任意指定合理泊车起始位姿状态的高效半挂车自主泊车规划是一个关键且尚未解决的问题，现对该问题难点总结如下：(1)挂车的大尺寸、欠驱动约束和非完整约束组成的运动学模型对于规划中轨迹的求解；(2)对下一时刻半挂车状态计算的运动学模型用于预测；(3)输出形式为路径，后期配速(高度耦合的状态变量)；(4)计算速度和求解质量并未兼顾，规划成功率等问题；(5)建立完美的非线性优化模型难度大(要的是求解一整条完整的状态变量序列，比如每一步的挂车航向角是要根据前车状态去推算的，计算量大，众多的约束可能导致算法无法收敛)。

3、现有的泊车算法多以乘用车为主且较为成熟，而对于半挂车的自主泊车系统，一般只能处理相对固定化的简单场景，例如在设定好的一块区域中结合人工经验指定泊车起始位姿状态的前提下实现半挂车的自主泊车规划。因此，缺乏一种能够保证与障碍物不碰撞，且生成的轨迹安全、可行，易于下层控制器实施的集装箱卡车的自主轨迹规划方法。

技术实现思路

1、本发明的目的是为了提供一种适用于复杂场景的、能够保证与障碍物不碰撞，且生成的轨迹安全、可行，易于下层控制器实施的集装箱卡车自主泊车运动规划方法、装置、设备及介质。

2、本发明的目的可以通过以下技术方案来实现：

3、一种适用于复杂场景的集装箱卡车自主泊车运动规划方法，包括以下步骤：

4、步骤1：基于泊车环境构建泊车地图欧氏距离代价矩阵；

5、步骤2：确定泊车任务，设置车辆的初始位姿与目标位姿；

6、步骤3：确定车辆的几何尺寸和物理参数，建立车辆的包围盒模型；

7、步骤4：基于集装箱卡车逆向运动学模型和车辆非完整约束建立半挂车未来时刻位姿状态预测模型；

8、步骤5：针对具体的泊车场景和泊车任务建立半挂车在场景中的位姿状态评价函数，所述位姿状态评价函数包括起步代价和启发式预计代价，其中，所述启发式预计代价由欧式距离代价、航向偏差、挂车reedsheep曲线距离代价、换向惩罚以及转向惩罚的线性组合构成；

9、步骤6：基于面向直方和侧方库位的融合改进hybrid a*算法和reed sheep曲线扩展的泊车路径规划方法进行路径规划，在路径规划过程中基于车辆的包围盒模型进行碰撞检测，并基于位姿状态评价函数评价路径节点的好坏；

10、步骤7：构建非线性最优化控制模型用于实现轨迹规划，同时，从路径规划结果中提取粗路径节点作为轨迹规划中非线性最优化控制模型的参考初始解；

11、步骤8：设计非线性最优化控制模型包含过程约束和始末端约束的限制条件；

12、步骤9：以系统唯二的控制输入序列前轮转角和纵向车速的连续性为优化对象，构建非线性最优化控制模型目标函数，并基于开源非线性求解器在满足限制条件的前提下进行求解，输出泊车最优轨迹状态序列。

13、所述步骤3中的车辆包围盒模型，以单列或多列圆形集对车辆矩形轮廓进行包络，基于圆心距的设定决定安全碰撞距离的大小：

14、

15、

16、其中，n表示沿车辆纵向分布的单列圆形集中圆形个数；pr为车辆后轴中心点；lt为牵引车车身长度；w0为牵引车车身宽度；rs为安全碰撞距离；d表示包络圆的圆心距。

17、所述步骤4中的集装箱卡车逆向运动学模型用于路径规划阶段的节点扩展策略，即以挂车为基准，使用虚拟挂车前轮转角和挂车行驶速度来实现挂车位姿的计算，通过车辆非完整约束的限制，推导得出牵引车的位姿信息，所述集装箱卡车逆向运动学模型表示为：

18、

19、

20、

21、

22、其中，表示挂车的虚拟前轮转角，v1表示挂车的纵向车速，θ0、θ1表示牵引车和挂车的航向角，lh和l1分别表示牵引车后轴到铰接点的距离和铰接点到挂车后轴的距离。

23、所述半挂车未来时刻位姿状态预测模型中，假设车辆以低速状态行驶，忽略轮胎侧滑效应，以传统单车模型为基础，使用如下微分方程描述牵引-半挂车运动状态：

24、

25、

26、

27、

28、

29、γ＝θ0-θ1

30、其中，(x0,y0)、(x1,y1)表示牵引车和挂车后轴中心坐标；l0、lh和l1分别表示牵引车轴距、牵引车后轴到铰接点的距离和铰接点到挂车后轴的距离；θ0、θ1表示牵引车和挂车的航向角；δf表示牵引车等效前轮转角；γ表示铰接角；v0、v1表示牵引车和挂车的纵向车速。

31、所述位姿状态评价函数表示为：

32、f(n)＝g(n)+h(n)＝pathdist+[σ1·eurodistdevi+σ2·yawdevi+σ3·trailerreedsheep dist+σ4·switchpenalty+σ5·steeringpenalty]

33、其中，f(n)表示位姿状态评价函数；g(n)表示起步代价；h(n)表示启发式预计代价；pathdist表示当前节点距离起点位姿的距离；eurodistdevi表示与终点位置的欧氏距离偏差，由步骤1的欧式距离代价矩阵确定；yawdevi表示与终点姿态的航向角偏差；trailerreedsheepdist表示以单挂车为规划对象时以reedsheep曲线连接当前节点到终点位姿的rs曲线距离；switchpenalty表示泊车过程中换向的惩罚值；steeringpenalty表示泊车过程中转向角度的惩罚值；σ1,σ2,σ3,σ4,σ5表示5个惩罚值各自的权重。

34、所述步骤8中，过程约束条件包括运动学模型约束、节点状态约束、碰撞检测约束、场景边界约束；始末端约束条件包括始端约束、末端停车框约束。

35、所述非线性最优化控制模型的目标函数表示如下：

36、

37、其中，μ1,μ2表示权重；[xref(t),yref(t)]表示从路径规划结果中提取的粗路径解；(x0,y0)表示牵引车后轴中心坐标；t∈[t0,tf]，表示规划从起始到结束的时间段；δf表示牵引车等效前轮转角；σ0表示牵引车的纵向车速。

38、一种适用于复杂场景的集装箱卡车自主泊车运动规划装置，包括：

39、初始化模块，用于执行以下步骤：基于泊车环境构建泊车地图欧氏距离代价矩阵；确定泊车任务，设置车辆的初始位姿与目标位姿；确定车辆的几何尺寸和物理参数，建立车辆的包围盒模型；

40、位姿状态预测模块，用于基于集装箱卡车逆向运动学模型和车辆非完整约束建立半挂车未来时刻位姿状态预测模型；

41、位姿状态评价函数构建模块，用于针对具体的泊车场景和泊车任务建立半挂车在场景中的位姿状态评价函数，所述位姿状态评价函数包括起步代价和启发式预计代价，其中，所述启发式预计代价由欧式距离代价、航向偏差、挂车reedsheep曲线距离代价、换向惩罚以及转向惩罚的线性组合构成；

42、路径规划模块，用于基于面向直方和侧方库位的融合改进hybrid a*算法和reedsheep曲线扩展的泊车路径规划方法进行路径规划，在路径规划过程中基于车辆的包围盒模型进行碰撞检测，并基于位姿状态评价函数评价路径节点的好坏；

43、轨迹规划模块，用于执行以下步骤：构建非线性最优化控制模型用于实现轨迹规划，同时，从路径规划结果中提取粗路径节点作为轨迹规划中非线性最优化控制模型的参考初始解；设计非线性最优化控制模型包含过程约束和始末端约束的限制条件；以系统唯二的控制输入序列前轮转角和纵向车速的连续性为优化对象，构建非线性最优化控制模型目标函数，并基于开源非线性求解器在满足限制条件的前提下进行求解，输出泊车最优轨迹状态序列。

44、一种电子设备，包括存储器和处理器，所述存储器上存储有计算机程序，所述处理器执行所述程序时实现如上述所述的方法。

45、一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现如上述所述的方法。

46、与现有技术相比，本发明具有以下有益效果：

47、1、本发明生成具有速度和前轮转角信息的泊车轨迹，方便后续控制器设计。

48、2、本发明的方法设置了合理车辆位姿状态评价函数，使得算法具有较强泛化性，对于泊车场景没有具体要求，可广泛适用于存在多障碍物的复杂直方位、侧方位停车等泊车场景。

49、3、本发明的方法在hybrid a*算法的基础上加入reed sheep曲线扩展策略，对于泊车起点没有具体要求，可广泛适用于各种极端的起始位姿。

50、4、本发明基于路径规划中的碰撞检测和轨迹规划中的约束条件对路径规划中的碰撞进行双重约束，能够生成不与障碍物碰撞，更为安全、可行的控制轨迹。