一种基于反向RRT算法的远距离自动泊车路径规划方法及系统

本发明涉及数字水印，更具体的，涉及一种基于反向rrt算法的远距离自动泊车路径规划方法及系统。

背景技术：

1、随着机动车拥有量的急速增长，城市内停车空间日趋紧张，泊车空间变得日益狭小，部分泊车位的视野存在很大限制，泊车过程中非常容易出现车辆剐蹭、碰撞等交通事故或难以停进泊位等现象，自动泊车技术受到广泛关注。泊车路径规划是自动泊车技术的重要组成部分，其目的是根据待停靠车辆和车位信息快速生成一条连续光滑且无碰撞的参考路径。泊车路径规划方法主要包括经典路径搜索方法、基于数值优化的方法、基于深度学习的方法、基于预定义几何集的方法和基于随机搜索的方法。

2、基于预定义几何集方法需要针对不同泊车场景设计算法，应用中存在一定局限性。基于随机搜索的泊车路径规划方法是通过随机采样和一系列判断来找到起点和终点间可行路线的一种方法，其中，快速搜索随机树算法(rrt)利用增量式方法构建搜索树，目前较多的应用于机器人路径规划领域，但在自动泊车路径规划中的研究较少。

3、现有技术有一种基于改进rrt算法的无人汽车路径规划算法。初始化地图空间，起始点和目标点的位置和航向角，汽车基本参数，算法参数以及环境中障碍物信息。引入目标概率采样策略获取随机采样点xrand的位置。使用多信息融合的评价方法定义最临近节点xnear。使用目标偏置扩展的方式对随机树进行扩展，并在节点连接过程中使用贝塞尔曲线进行光滑连接。重复上述步骤，直到随机树扩展至目标点的可连接范围，使用reeds-shepp曲线直接将随机树和目标点进行连接。

4、然而现有技术存在没有考虑泊入泊位前初始状态，导致泊车效率低的问题，因此如何发明一种考虑泊入泊位前初始状态的远距离自动泊车路径规划方法，是本技术领域亟需解决的技术问题。

技术实现思路

1、本发明为了解决现有技术没有考虑泊入泊位前初始状态，导致泊车效率低的问题，提供了一种基于反向rrt算法的远距离自动泊车路径规划方法及系统，其具有将自动泊车介入时间点提前，泊车成功率高的特点。

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

3、一种基于反向rrt算法的远距离自动泊车路径规划方法，包括以下具体步骤：

4、s1：构建考虑前后轮速度、前后轮关系、车辆横摆角度的车辆运动学模型；

5、s2：获取泊车环境地图，将泊车环境地图进行栅格化处理；

6、s3：在远距离靠近泊位阶段，采用反向rrt算法，根据车辆运动学模型、栅格化泊车环境地图，生成第一阶段泊车路径；

7、s4:对第一阶段泊车路径进行一次平滑；

8、s5：构建车辆调整控制模型；通过车辆调整控制模型考虑车辆形式和环境障碍物，调整一次平滑后的第一阶段泊车路径；

9、s6：在泊入泊位阶段，采用reeds-shepp曲线改进反向rrt算法，根据车辆运动学模型、栅格化泊车环境地图，生成泊入车位路径作为第二阶段泊车路径；

10、s7：对第二阶段泊车路径进行二次平滑，结合调整后的第一阶段泊车路径，得到完整泊车路径规划，完成自动泊车路径规划。

11、优选的，所述的步骤s1中，所述的车辆动力学模型，具体为：

12、s101：设前轮方向的速度为vf，设后轮轴心的位置为(xb,yb)的速度为vb：

13、vb＝vfcosα

14、其中，α为车辆当前运动方向；

15、s102：设前后轮的位移趋势与各自速度角度的正弦值相同，将前后轮运动约束表示为：

16、δyfcos(α+β)＝δxfcos(α+β)

17、δybcosα＝δxbsinα

18、其中，β为车辆的航向角，前轮轴心的位置为(xf,yf)；

19、s103：得到前后轮关系：

20、

21、其中，l为前后轴的轴距；

22、s104：得到车辆运动学模型：

23、

24、车辆运动遵循车辆运动学模型的限制。

25、进一步的，还构建了车辆边界计算模型：定义车辆边界左前角的坐标为(xlf,ylf)、右前角的坐标为(xrf,yrf)、左后角的坐标为(xlb,ylb)、右后角的坐标为(xrb,yrb)，其中：

26、xlf＝xb+l×cosβ+h/2×cos(β+π/2)+fs×cosβ

27、ylf＝yb+l×sinβ+h/2×sin(β+π/2)+fs×sinβ

28、xrf＝xb+l×cosβ+h/2×cos(β-π/2)+fs×cosβ

29、yrf＝yb+l×sinβ+h/2×sin(β-π/2)+fs×sinβ

30、xlb＝xb+h/2×cos(β+π/2)-ro×cosβ

31、ylb＝yb+h/2×sin(β+π/2)-ro×sinβ

32、xrb＝xb+h/2×cos(β-π/2)-ro×cosβ

33、yrb＝yb+h/2×sin(β-π/2)-ro×sinβ

34、其中，h为车辆宽度，fs为前悬长度，ro为后悬长度；根据车辆边界计算模型，构建车辆碰撞检测模型：将车辆边界离散化为若干个点；用集合c表示车辆的所有边界点的坐标，若存在m(c)＝1，则表示该车辆边界点的集合c中有被地图模型标记为1的点，也就是说车辆与地图中障碍物已发生碰撞，应取消该路径段。

35、更进一步的，所述的步骤s2中，将泊车环境地图进行栅格化处理，具体为：将泊车环境地图分割为大小相同的若干个方块；将车辆边界离散化为若干个点，并将障碍物进行膨胀处理；在泊车环境地图上的每个方块上赋权，权值表示该方块处是否存在障碍物。

36、更进一步的，所述的步骤s3中，采用反向rrt算法，根据车辆运动学模型、栅格化泊车环境地图，生成第一阶段泊车路径，具体步骤为：

37、s301：根据格栅化处理后的图，初始化起点p_star作为车辆当前位置的后轮胎轴心坐标和运动方向、终点p_goal作为车辆从待泊车位附近停入待泊车位的后轮胎轴心坐标和运动方向、车辆信息；

38、s302：倒转起点和终点，将p_goal作为起点加入随机探索树；

39、s303：以一定的概率生成随机点p_rand或终点；随机生成数rand，若rand大于设定的概率阈值p，则生成目标点为随机点p_rand，若rand不大于概率阈值p，则生成目标点为终点；

40、s304：判断树中所有节点与目标点的距离，并取近距离的点为p_near；

41、s305：判断p_near与p_rand的距离是否大于步长s，若是，则以p_near为原点、以p_near-p_rand为方向计算长度为s的坐标为p_new；若否，则以p_rand为p_new；

42、s306：判断p_near与p_new的连线是否与障碍物碰撞，若是，则转到step3，若否，则将p_new作为p_near的子节点加入随机探索树中；

43、s307：通过车辆运动学模型和车辆碰撞检测模型判断树中最末端节点是否可达终点并无碰撞，若是，则转至s308，若否，则转到s303；

44、s308：生成第一阶段泊车路径。

45、更进一步的，所述的步骤s4中，对第一阶段泊车路径进行一次平滑，具体步骤为：

46、s401：导入第一阶段泊车路径rrt_path；

47、s402：令平滑路径为smooth_path，节点smooth_path(1)＝rrt_path(1)；

48、s403：令i为rrt_path的最大节点数；

49、s404：判断rrt_path(i)与smooth_path(end)连线是否碰撞障碍物或连线长度是否大于平滑长度d，若是，则令i＝i-1，重复执行s404，否则，转至s405；

50、s405：将rrt_path(i)加入smooth_path(end)的末尾；

51、s406：判断smooth_path(end)与rrt_path(end)的连线是否碰撞障碍物或连线长度是否大于平滑长度d，若是，则将rrt_path的节点1～i删除，转至s403；否则转至s407；

52、s407：将rrt_path(end)加入smooth_path(end)的末尾。

53、更进一步的，所述的步骤s5中，构建车辆调整控制模型，通过车辆调整控制模型考虑车辆形式和环境障碍物，调整一次平滑后的第一阶段泊车路径，具体为：

54、设置前进和倒车、直行前进和直行倒车、反向前进和反向倒车6种运动形式，使车辆在遇到不可直达区域或遇到障碍物时可以灵活调整车辆位置并继续前行；以前轮最大转向角±maxα和前轮轴线为界将栅格化泊车环境地图划分为6个子域，车辆正前和正后方±maxα的子域为车辆前轮直接转向可达区域，余下区域为前轮转向不可直达区域；当目标位置在可直达区域时，直接旋转对应的角度即可到达；当目标位置在不可直达区域时，通过匀速转动方向盘使车辆平滑地转向目标位置直至前轮角度转至最大值或当目标位置进入车辆的前轮转向可达区域时转为相应的控制；设置可前进和倒车两种控制模式，在前进模式中采取向前行驶策略，在倒车模式中采取倒车行驶策略，反向前进和倒车是对前轮转角进行取反来实现，直行前行和直行倒车则通过将前轮转向角固定为0来实现，完成车辆调整控制模型的构建；

55、通过车辆调整控制模型考虑车辆形式和环境障碍物，调整一次平滑后的第一阶段泊车路径：

56、s501：初始化车辆数据并输入车辆调整控制模型，令i＝1；

57、s502：以平滑后的第一阶段泊车路径的第i个点为导向点，若i大于总导向路径节点数量，则转至s501；

58、s503：将导向点作为车辆目标点，规划车辆前进路线＝1；

59、s504：改变车辆控制策略为前进，检测车辆是否碰撞障碍物，若是，转至s505，若否，则转至s510；

60、s505：变车辆控制策略为倒车，检测是否可获得有效移动，若是，转至s504，若否，则转至s506；

61、s506：改变车辆控制策略为直行后退，检测是否可获得有效移动，若是，转至s504，若否，则转至s508；

62、s507：改变车辆控制策略为直行后退，检测是否可获得有效移动，若是，转至s504，若否，则转至s508；

63、s508：改变车辆控制策略为反向前进，检测是否可获得有效移动，若是，转至s504，若否，转至s509；

64、s509：改变车辆控制策略为反向倒车，检测是否可获得有效移动，若是，转至s504，若否，车辆进入无法脱困状态，重新计算导向路线；

65、s510：检测车辆是否到达目标点附近，若是，令i＝i+1并转至s502，若否，则转至s504；

66、s511：输出调整后的第一阶段泊车路径。

67、更进一步的，所述的步骤s6中，采用reeds-shepp曲线改进反向rrt算法，根据车辆运动学模型、栅格化泊车环境地图，生成泊入车位路径作为第二阶段泊车路径，具体为：

68、将反向rrt算法随机生成的目标点替换为目标车辆位姿(x,y,α)；在选择p_near节点时，计算树中所有节点到目标位姿的reeds-shepp曲线长度，选择与障碍物无碰撞且reeds-shepp曲线长度最短的节点为父节点；

69、根据车辆运动学模型、栅格化泊车环境地图，生成泊入车位路径作为第二阶段泊车路径：

70、s601：导入栅格化泊车环境地图，将调整后的第一阶段泊车路径的终点车辆位姿作为车辆起始位姿，设定终点泊位位姿；

71、s602：随机生成数rand2，若rand2大于概率阈值p，则生成目标位姿为随机位姿，若rand2小于概率阈值p，则生成目标位姿作为终点泊位位姿；

72、s603：判断目标点是否合法，若合法，转至s604；若不合法，则转至s602；

73、s604：计算树中所有节点到目标位姿的reeds-shepp曲线长度，选择与障碍物无碰撞且reeds-shepp曲线长度最短的节点为父节点；

74、s605：判断随机树中所有节点位姿是否已到达终点泊位位姿，若否，转至s602，若是，则转至s606；

75、s606：根据车辆运动学模型，通过随机树中父节点关系生成第二阶段泊车路径。

76、更进一步的，所述的步骤s7中，对第二阶段泊车路径进行二次平滑，结合调整后的第一阶段泊车路径，得到完整泊车路径规划，完成自动泊车路径规划，具体为：以第一阶段泊车路径节点与平滑路径终点间的reeds-shepp曲线为依据来判断路径是否会碰撞障碍物，对第二阶段泊车路径进行一次平滑；在一次平滑之后，将路径整体翻转后再进行二次平滑，最终在任意起始点位姿及任意泊位类型下生成一条无碰撞的泊入路径。

77、一种基于反向rrt算法的远距离自动泊车路径规划系统，包括运动模型构建模块、栅格化模块、第一阶段路径生成模块、一次平滑模块、调整控制模型、第二阶段路径生成模块、二次平滑模块：

78、所述的运动模型构建模块用于构建考虑前后轮速度、前后轮关系、车辆横摆角度的车辆运动学模型；

79、所述的栅格化模块用于获取泊车环境地图，将泊车环境地图进行栅格化处理；

80、所述的第一阶段路径生成模块用于在远距离靠近泊位阶段，采用反向rrt算法，根据车辆运动学模型、栅格化泊车环境地图，生成第一阶段泊车路径；

81、所述的一次平滑模块用于对第一阶段泊车路径进行一次平滑；

82、所述的调整控制模型用于构建车辆调整控制模型；通过车辆调整控制模型考虑车辆形式和环境障碍物，调整一次平滑后的第一阶段泊车路径；

83、所述的第二阶段路径生成模块用于在泊入泊位阶段，采用reeds-shepp曲线改进反向rrt算法，根据车辆运动学模型、栅格化泊车环境地图，生成泊入车位路径作为第二阶段泊车路径；

84、所述的二次平滑模块用于对第二阶段泊车路径进行二次平滑，结合调整后的第一阶段泊车路径，得到完整泊车路径规划，完成自动泊车路径规划。

85、本发明的有益效果如下：

86、本发明公开了一种基于反向rrt算法的远距离自动泊车路径规划方法及系统，通过构建考虑前后轮速度、前后轮关系、车辆横摆角度的车辆运动学模型、栅格化处理地图、生成第一阶段泊车路径并进行一次平滑、生成第二阶段泊车路径并进行二次平滑；本发明中的路径基于反向rrt算法分别设计路径规划算法，使自动泊车系统介入泊车过程的时间提前，减轻驾驶员泊车压力；使车辆在泊入泊位前处于较好的初始状态，有利于提高泊车效率和成功率。由此本发明解决现有技术没有考虑泊入泊位前初始状态，导致泊车效率低的问题。