平行泊车路径规划方法及系统

本发明涉及汽车智能化，特别涉及一种平行泊车路径规划方法及系统。

背景技术：

1、自动泊车路径规划就是在满足各种泊车约束条件并结合周围泊车环境，从泊车起始点与终止点之间规划出一条安全合理容易跟踪的路径曲线。路径规划作为连接自动泊车停车位检测与跟踪控制的中间环节，对自动泊车系统能否快速安全的停入车位起到关键作用。自动泊车通过各种传感器获取车辆自身的位姿，车位信息以及障碍物的信息，并将所得到的信息进行路径规划，使车辆能够根据规划好的路径能够安全高效、无碰撞的泊车。可靠性且高效率的自动泊车系统能够保证驾驶员安全，减少停车堵塞，有效的提高驾驶舒适性。

2、cn115384518a公开了一种基于超声波雷达的侧边车位定位方法和装置，包括：在当前车辆前进过程中通过超声波雷达获取当前车辆侧面的障碍物的轮廓点集；根据所述轮廓点集对所述当前车辆侧面的障碍物轮廓进行分段拟合，得到当前车辆侧面的障碍物的方向；根据所述当前车辆侧面的障碍物的方向，确定当前车辆侧面的车位方向。

3、jp2022173839a公开了一种自动泊车系统及自动泊车系统控制方法，其具有多个停车区域，在该停车区域中，多个停车位至少在平行方向上平行。所述自动停车系统为所述多个停车区域中的每一个获取停车位的数量，所述停车位的数量是沿平行方向排列的停车位的数量。对于多个停车场中的每一个，获取空位的数量，即空闲停车位的数量。基于垂直数量，计算空位阈值，该空位阈值是空位数量的阈值，用于计算自动停车的车辆停放在其中的停车区域的优先级。计算优先级，使得空位数目大于或等于空位数目阈值的停车区域的优先级高于空位数目小于空位阈值的停车区域的优先级。将优先考虑在高优先级停车场的停车位中停放自动泊车的车辆。

4、cn115303263a公开了一种基于神经网络的自动泊车方法及系统，所述方法包括：通过摄像头获取车辆周围全景环视图像；基于全景环视图像通过深度神经网络算法获取泊车位置；基于泊车位置和车辆状态，规划出泊车路径；启动车辆控制系统执行泊车，每隔一定时间阈值判断是否泊车结束，若结束则检测结束，否则继续执行泊车。

5、现有自动泊车系统的泊车路径规划中，往往采用几何法的方式进行路径规划，包括两段圆弧、圆弧直线、回旋曲线、b样条曲线、多项式曲线、正弦函数曲线往往难以完成高精度的自动泊车。

技术实现思路

1、经过长期实践发现，圆弧与直线的方法结构简单，但其路径曲率不连续，不易跟踪，会额外增加转向系统和轮胎的压力，有损乘员的舒适性；回旋曲线和b样条曲线虽然曲率连续易于跟踪，但结构复杂计算量大，且多段路径会增加转向系统和轮胎的损伤和实际泊车路径的误差；多项式曲线泊车方法存在泊车路径曲率过大、曲线偏长。现有技术的泊车路径规划方法中，存在的路径曲率不连续、曲率过大不易跟踪、拟合路径误差较大，从而影响自动泊车效果，进而造成泊车失败等问题。

2、有鉴于此，本发明旨在提出一种平行泊车路径规划方法，所述平行泊车路径规划方法包括，

3、步骤s1，通过车辆传感器感知待泊车车位数据、车辆初始位置姿态数据，根据车辆平行泊车路径建立多项式函数关系，

4、y(x)＝a5x5+a4x4+a3x3+a2x2+a1x+a0

5、由车辆起始点坐标与泊车终止点坐标建立第一约束条件，

6、

7、

8、其中，起始点坐标数值通过定位系统得到，终止点坐标数值通过泊车结束时车辆与车位的安全约束得到；a0、a1、a2、a3、a4、a5分别为多项式系数；

9、步骤s2，根据路径的起始点及终止点的位置姿态约束条件，得到路径曲线在起始点及终止点处的一阶导数为0的第二约束条件，即为，

10、

11、

12、步骤s3，根据泊车结束时，其转向轮回正，此时泊车路径中斜率不发生变化，得到路径曲线在终止点处的二阶导数为0的第三约束条件，即为，

13、

14、步骤s4，根据车辆安全无碰撞驶出车位约束条件，后轴中心点正好处于点的第四约束条件，即为，

15、

16、其中，r1为车辆最小转弯半径，δ为车辆转弯角度；

17、根据第一约束条件、第二约束条件、第三约束条件、第四约束条件获得a0、a1、a2、a3、a4、a5多项式系数数值，并获得多项式函数路径规划曲线；

18、步骤s5，基于双曲正切tanh函数获得双曲正切路径规划曲线，

19、

20、其中，a、b、c、d分别函数系数；

21、步骤s6，融合多项式函数路径规划曲线和双曲正切路径规划曲线，获得平行泊车路径规划曲线，即为，

22、f(x)＝kg(x)+(1-k)y(x)

23、其中，k为比例系数，范围0-1；

24、车辆根据平行泊车路径规划曲线进行跟踪控制，完成车辆泊车操作。

25、优选地，在步骤s5中，函数系数a、b、c、d通过第一约束条件、第二约束条件、第三约束条件、第四约束条件的车辆泊车过程中位置姿态约束关系获得。

26、优选地，在步骤s6中，通过迭代优化方法确定比例系数k。

27、优选地，通过车辆传感器获取车辆与车位相对位置，在平行泊车路径规划曲线中，以车辆后轴中心点行驶轨迹作为跟踪控制依据。

28、本发明还公开了一种用于执行上述平行泊车路径规划方法的系统，所述系统包括，

29、第一约束单元，包括传感器模块，传感器模块至少包括视觉传感器、超声波传感器；用于通过车辆传感器感知待泊车车位数据、车辆初始位置姿态数据，根据车辆平行泊车路径建立多项式函数关系，

30、y(x)＝a5x5+a4x4+a3x3+a2x2+a1x+a0

31、由车辆起始点坐标与泊车终止点坐标建立第一约束条件，

32、

33、

34、其中，起始点坐标数值通过定位系统得到，终止点坐标数值通过泊车结束时车辆与车位的安全约束得到；a0、a1、a2、a3、a4、a5分别为多项式系数；

35、第二约束单元，用于根据路径的起始点及终止点的位置姿态约束条件，得到路径曲线在起始点及终止点处的一阶导数为0的第二约束条件，即为，

36、

37、

38、第三约束单元，用于根据泊车结束时，其转向轮回正，此时泊车路径中斜率不发生变化，得到路径曲线在终止点处的二阶导数为0的第三约束条件，即为，

39、

40、第四约束单元，包括计算模块，用于根据车辆安全无碰撞驶出车位约束条件，后轴中心点正好处于点的第四约束条件，即为，

41、

42、其中，r1为车辆最小转弯半径，δ为车辆转弯角度；

43、所述计算模块用于根据第一约束条件、第二约束条件、第三约束条件、第四约束条件获得a0、a1、a2、a3、a4、a5多项式系数数值，并获得多项式函数路径规划曲线；

44、构建单元，用于基于双曲正切tanh函数获得双曲正切路径规划曲线，

45、

46、其中，a、b、c、d分别函数系数；

47、融合单元，包括跟踪控制模块，用于融合多项式函数路径规划曲线和双曲正切路径规划曲线，获得平行泊车路径规划曲线，即为，

48、f(x)＝kg(x)+(1-k)y(x)

49、其中，k为比例系数，范围0-1；

50、跟踪控制模块用于车辆根据平行泊车路径规划曲线进行跟踪控制，完成车辆泊车操作。

51、优选地，所述第一约束单元还包括定位模块，所述定位模块包括slam定位，用于识别车辆和待泊车位的位置与相对位置，获取待泊车位边界障碍物信息与安全距离数据。

52、优选地，所述融合单元还包括迭代优化模块，用于通过迭代优化方法确定比例系数k。

53、本发明还公开了一种车辆，所述车辆装载有包括上述的系统，或能够实现如上述平行泊车路径规划方法。

54、本发明公开了一种电子设备，包括存储器和处理器：所述存储器，用于存储计算机程序；所述处理器，用于当执行计算机程序时，实现上述的平行泊车路径规划方法。

55、本发明还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现本发明提供的方法。

56、相对于现有技术，本发明提供的平行泊车路径规划方法包括步骤s1-s6，即通过车辆传感器感知待泊车车位数据、车辆初始位置姿态数据，根据车辆平行泊车路径建立多项式函数关系，由车辆起始点坐标与泊车终止点坐标建立第一约束条件；根据路径的起始点及终止点的位置姿态约束条件，得到路径曲线在起始点及终止点处的一阶导数为0的第二约束条件；根据泊车结束时位置姿态得到第三约束条件以及根据车辆安全能够无碰撞驶出车位约束条件，后轴中心点所在位置建立第四约束条件；最终根据第一约束条件、第二约束条件、第三约束条件、第四约束条件获得多项式系数数值，并获得多项式函数路径规划曲线；基于双曲正切tanh函数获得双曲正切路径规划曲线，再通过融合多项式函数路径规划曲线和双曲正切路径规划曲线，获得平行泊车路径规划曲线，车辆根据平行泊车路径规划曲线进行跟踪控制，完成车辆泊车操作。本发明还公开了一种用于执行上述方法的系统，该方法和系统通过融合多项式函数路径规划曲线与基于双曲正切tanh函数获得双曲正切路径规划曲线，其路径曲率连续，易于跟踪，双曲正切函数弥补了泊车常用的五次多项式方法曲率过大的缺陷，缓解了车辆转向压力，能够解决泊车路径曲率不连续、不易跟踪、路径结构组成复杂，节点过多，拟合路径误差较大的缺点，保障泊车路径平滑，提高泊车效率，改善泊车效果。

57、本发明的其它特征和优点将在随后的具体实施方式部分予以详细说明。