一种基于改进混合A*算法的车头泊入路径重规划方法

本发明涉及自动泊车路径规划领域领域，尤其涉及一种基于改进混合a*算法的车头泊入路径重规划方法。

背景技术：

1、汽车保有量持续增多，随之而来的是交通环境拥挤，城市停车位资源紧张，停车位空间小等问题。

2、自动泊车技术的出现和发展为解决泊车问题提供了新思路，让泊车变得更智能、更安全、更便捷，将有效解决驾驶员找车位难、停车难、找车难等痛点。

3、自主泊车系统的路径规划功能主要是根据车辆周围环境信息和目标停车位位置规划出车辆在不与周围物体发生碰撞的情况下，从当前位置驶入目标停车位的路径，规划得到的路径尽可能短而且路径曲率比较平滑。

4、现有的路径规划方法大多考虑车辆起始位置在停车位附近，而自动泊车过程中车辆周围可能出现障碍物导致泊车过程中断，发生这种情况后车辆将无法沿着原有的泊车路径行驶，需要重新规划泊车路径，即路径规划模块的重规划功能。

5、混合a*算法是路径规划常用的规划算法，混合a*算法引用了启发信息搜索路径效率较高，根据车辆动力学模型搜索得到的泊车路径可以满足车辆的动力学约束，通过设计合理的启发函数可以提高混合a*算法的搜索效率。

6、由于车辆在泊车过程中触发重规划的位置是随机的，而且停车位周围的障碍物位置也会发生变化，当泊车场景发生变化后，传统的混合a*算法无法保证搜索效率，容易在启发值接近的区域反复搜索，搜索得到的路径存在曲率不连续等问题。

7、因此，如何提高混合a*算法的搜索效率，避免在启发值接近的区域反复搜索为改进混合a*算法提供了新的思路。

技术实现思路

1、本发明的目的在于克服传统混合a*算法的缺点和不足，提供一种基于改进混合a*算法的车头泊入路径重规划方法。本发明解决了现有混合a*算法搜索时间长，搜索路径曲率不连续的问题，可以实现实时高效规划泊车路径，提高了混合a*算法搜索路径的适应性，能够在多种重规划环境下保证规划得到平滑的泊车路径。

2、本发明设计相切圆规划方法得到车辆从停车位驶出的参考路径，相切圆规划方法能适应泊车场景变化。

3、本发明设计评价函数选取相切圆路径点作为混合a*算法搜索的目标点，提高混合a*算法搜索效率。

4、本发明设计曲率平滑方法将改进混合a*搜索路径输出为平滑路径，解决了混合a*算法搜索路径曲率不连续问题。

5、本发明通过下述技术方案实现：

6、一种基于改进混合a*算法的车头泊入路径重规划方法，包括以下步骤：

7、s1、输入车辆起点位置姿态，初始化探索节点列表openlist和封闭节点列表closelist，将车辆起点作为初始节点放入openlist中；

8、s2、根据车位坐标位置获取目标停车位置，利用相切圆规划方法得到车辆从车位内驶出的参考路径，相切圆规划方法的具体步骤如下：

9、s2.1、计算车辆与车位框线相切泊出的圆弧半径r，根据圆弧公式计算车辆倒车驶出停车位的位置，作为下一步圆弧规划的起点；

10、s2.2、重复进行如下步骤：

11、s2.2.1、切换车辆档位d，利用圆弧公式计算车辆以最小转弯半径r从当前位置行驶至车身姿态与车位平行的位置的圆弧路径；

12、s2.2.2、遍历圆弧路径点判断是否与障碍物发生碰撞，并记录发生碰撞的路径点；

13、s2.2.3、保留圆弧起点到不碰撞的圆弧路径点之间的圆弧路径作为相切圆路径，并将不发生碰撞的圆弧路径点更新为下一步圆弧规划起点；

14、s2.3、直到车辆在圆弧路径终点的车身姿态与车位平行，返回相切圆路径；

15、s3、根据评价函数j(i)得到参考路径中与车辆起点位置接近的路径点，作为混合a*算法搜索的目标点target；

16、s4、利用混合a*算法，并根据周围障碍物位置信息，搜索得到一条无碰撞泊车路径。具体包括如下步骤：

17、s4.1、重复进行如下步骤：

18、s4.1.1、在openlist中选取代价值f(n)最小的节点作为父节点tnode，将tnode放入closelist中；

19、s4.1.2、判断tnode是否为搜索的目标点target，如果是目标点则输出搜索路径，如果不是则进入下一步；

20、s4.1.3、判断是否可以利用reeds-shepp路径规划得到从当前节点tnode到达目标点target的路径，如果可以则输出混合a*算法搜索路径和rs路径，如果不可以则进入下一步；

21、s4.1.4、利用车辆动力学公式，给车辆固定速度，不同的轮胎转角计算得到tnode能到达的下一节点nextnode，并将不发生碰撞的nextnode放入openlist中；

22、s4.2、直指搜索得到目标节点target，退出循环；

23、s5、拼接混合a*算法搜索路径和相切圆路径，如果搜索路径中包含rs路径，则拼接混合a*算法搜索路径、rs路径和相切圆路径。

24、s6、将拼接路径利用曲率平滑方法得到从车辆起点位置姿态行驶至目标停车位置姿态的一条完整平滑的路径。

25、上述步骤s1中，起点位置姿态包括车辆位置的x0，y0坐标，车辆航向角θ0，车轮转角δ0和车辆档位d0，采用的坐标系为大地坐标系。

26、上述步骤s2中，车辆与车位框线相切泊出的圆弧半径r的计算公式如下：

27、δs＝carparking.w/2-threshold；

28、r＝(vehicle.lf2+vehicle.w2/4-δs2)/(2*(δs-vehicle.w/2))；

29、其中carparking.w为停车位宽度，threshold为安全距离阈值，vehicle.lf为车辆前悬加上车辆轴距的长度，vehicle.w为车身宽度。

30、上述步骤s2中，相切圆规划方法中采用的圆弧计算公式如下：

31、xi＝x0+r(cosα0-cosαi)；

32、yi＝y0+r(sinαi-sinα0)；

33、

34、

35、其中(x0，y0，θ0)为圆弧规划起点，(xi，yi，θi)为圆弧规划终点。α0，α1为圆弧起点和终点的圆心角。圆弧计算公式采用的坐标系为大地坐标系。

36、上述步骤s3中，评价函数为j(i)＝k1(xi-x0)+k2(yi-y0)+k3(θi-θ0)。

37、其中，(xi，yi，θi)为相切圆参考路径中的路径点姿态，(x0，y0，θ0)为车辆当前姿态。k1，k2，k3为对应的权重系数。

38、上述步骤s4.1.1中，混合a*算法代价函数f(n)公式如下：

39、

40、其中，(xi，yi，θi)为混合a*算法当前节点姿态；

41、(xn,yn，θn)为混合a*算法目标节点姿态；

42、w1，w2为对应的代价权重。

43、上述步骤s4.1.4中，车辆动力学公式如下：

44、xi＝xi-1+vdi-1cosθi-1；

45、vi＝yi-1+vdi-1sinθi-1；

46、

47、其中，(xi，yi，θi)为车辆下一位置姿态；

48、(xi-1，yi-1，θi-1)为车辆上一位置姿态；

49、v为车辆速度；

50、di-1为车辆上一位置档位，其中，前进挡为1，倒车档为-1，δi-1为车辆上一位置轮胎转角，vehicle.l为车辆车身长度。

51、上述步骤s6中，曲率平滑方法计算公式如下：

52、

53、

54、δ′i＝δlast+(i+1)*δδ′，i＝0，1，2...np-1；

55、其中，np为当前路径的点数，δavg为当前路径的平均曲率值，δlast为下一段路径起点的曲率，δδ′为曲率变化率，δ′i为当前路径点的曲率序列，引入δlast计算得到δavg可以避免当前路径终点与下一段路径起点之间的曲率发生突变。

56、本发明相对于现有技术，具有如下的优点及效果：

57、由于采用相切圆规划方法生成参考路径，可以选择更接近车辆当前位置姿态的路径点作为混合a*算法搜索的目标点，有助于提升混合a*算法的实时性和效率性。本发明与传统的混合a*算法相比，有搜索效率高，搜索时间短，能适应环境变化的优点，适合在实车上进行在线应用。