一种基于HybridAstar与空间走廊的路径规划方法

本发明涉及路径规划，特别涉及一种基于hybrid astar与空间走廊的路径规划方法。

背景技术：

1、无人车普遍使用于农业、运输业、军工或是日常交通，在实际使用过程中，多障碍环境是不可避免的。无人车经典的路径规划算法包括基于采样的rrt系列，基于搜索的如a*系列，基于多项式的以及基于最优控制的。在实际运用中，路径要求除了安全无障碍物，还需要满足平缓顺滑舒适性高，所以一般在得到初始规划结果后会采用多项式或样条插值等方法去平滑初始结果路径。但在多障碍环境中，即使初始结果能保证不碰到障碍物，平滑后的曲线也可能会因为波动在角落或狭窄处碰到障碍物。或是在设计避障情境下的初始路径时，为了得到路径代价较低的结果规划出了比较贴近障碍物的路径，此时用bezier去平滑初始路径，如果产生波动较大的结果，会加大碰障的可能。而且对于bezier而言选取新的路点，拼接前后段优化路径也是十分困难的。

技术实现思路

1、本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种基于hybridastar与空间走廊的路径规划方法，以解决上述背景技术中提出的问题。

2、一种基于hybrid astar与空间走廊的路径规划方法，包括：

3、构建多障碍环境下的全局占据栅格地图，并设定任务起始点和终点的坐标；

4、根据构建的全局占据栅格地图基于hybrid astar进行路径规划，得到规划的初始路径；

5、根据得到的初始路径膨胀路点至障碍物，得到多个矩形空间；

6、对得到的多个矩形空间进行数量简化，并形成空间走廊；

7、通过最优控制模型在空间走廊内对初始路径进行二次规划，得到优化后的路径。

8、作为本发明的进一步的方案：所述构建多障碍环境下的全局占据栅格地图，并设定任务起始点和终点的坐标的具体步骤包括：

9、利用定位设备gps进行自我定位得到起始点坐标，并获取任务终点gps坐标；

10、同时通过自主无人车的环境感知系统获取车辆当前位置周围的障碍物，得到局部占据栅格地图，并不断更新；

11、通过坐标转换将局部占据栅格地图映射到构建的全局占据栅格地图中。

12、作为本发明的进一步的方案：所述根据构建的全局占据栅格地图基于hybridastar进行路径规划，得到规划的初始路径的具体步骤包括：

13、获取任务的起始点、终点，以及构建的全局占据栅格地图；

14、基于hybrid astar进行路径规划：

15、当扩展结点与终点距离最近时，认定到达终点；

16、当利用reeds_sheep去命中终点时，使得reeds_sheep路点包含在全局占据栅格地图内，且不能越界不能触碰障碍物，否则继续使用hybrid astar进行路径搜索。

17、作为本发明的进一步的方案：所述根据得到的初始路径膨胀路点至障碍物，得到多个矩形空间的具体步骤包括：

18、依次遍历整个初始路径的路点，根据每个路点的坐标初始化矩形空间box_now的四个角点p1、p2、p3，以及p4；

19、判断当前路点坐标上是否有障碍物，若有则跳过这个点，且初始化最大膨胀次数，在膨胀次数内不断扩展p2与p3横纵坐标至障碍物边界，得到p2与p3横纵坐标；

20、将得到的p2与p3横纵坐标对应的赋值给p1与p4，形成当前路点对应的矩形空间。

21、作为本发明的进一步的方案：所述对得到的多个矩形空间进行数量简化，并形成空间走廊的具体步骤包括：

22、预设四个角点为(0，0)的矩形空间box_last；

23、当矩形空间box_last与初始化矩形空间box_now在交集部分有相同的路点，而非交集部分没有特有的路点，则删除被覆盖的部分。

24、作为本发明的进一步的方案：所述通过最优控制模型在空间走廊内对初始路径进行二次规划，得到优化后的路径的具体步骤包括：

25、步骤一、建立运动学模型：

26、首先基于自行车模型对车辆运动学进行建模：

27、

28、其中，车辆后轴终点以p0(x0,y0)，θ0表示航向角，v0表示p0的速度，a0表示线性加速度，为前轮旋转角度，w0为前轮角速度，lw0表示车辆后轴长度；

29、当t∈[0,tf]时刻的状态变量边界为：

30、|a0(t)|≤amax；

31、|v0(t)|≤vmax；

32、

33、|w0(t)|≤ωmax；

34、其中，amax、vmax、ωmax分别表示每个变量的上界；

35、步骤二、设定边界限制：

36、根据车辆的感知系统获取在初始状态t＝0时刻，x0(0)、y0(0)、v0(0)、θ0(0)、a0(0)，以及w0(0)每个变量的信息；

37、在终止时刻t＝tf，整个车辆系统稳定停止，则得到公式：

38、[v0(tf),a0(tf),w0(tf)]＝[0,0,0]；

39、步骤三、设定空间走廊限制：

40、初始路径包含nfe个路点，则每一个路点pc(xc(t)，yc(t))应限制在空间走廊内，公式为：

41、

42、其中，其中表示路点对应矩形空间p3点的横坐标，表示路点对应矩形空间p2点横坐标，表示路点对应矩形空间p3点纵坐标，表示路点对应矩形空间p2点纵坐标；

43、步骤四、获取代价函数：

44、所述代价函数为：

45、其中，w1,w2≥0为代价函数，tf表示从始点到终点车行进的时间，第二项表示车辆加速度的变化，第三项表示角速度的变化；

46、步骤五、根据步骤一至四组合得到最优控制模型；

47、再基于一阶显式runge-kutta和内点法来求解模型，得到限制在空间走廊内的安全无障碍的最优路径。

48、与现有技术相比，本发明存在以下技术效果：

49、采用上述的技术方案，通过在多障碍物情景下进行路径规划，在对初始路径优化时引入空间走廊，并建立最优控制模型得到优化后处于无障碍的空间走廊内的最优路径，满足车辆运动学约束的同时最大程度保证车辆的安全，有效地避免优化路径波动大导致与近处障碍物碰撞的危险。

技术特征：

1.一种基于hybrid astar与空间走廊的路径规划方法，其特征在于，其具体步骤包括：

2.根据权利要求1所述一种基于hybrid astar与空间走廊的路径规划方法，其特征在于，所述构建多障碍环境下的全局占据栅格地图，并设定任务起始点和终点的坐标的具体步骤包括：

3.根据权利要求1所述一种基于hybrid astar与空间走廊的路径规划方法，其特征在于，所述根据构建的全局占据栅格地图基于hybrid astar进行路径规划，得到规划的初始路径的具体步骤包括：

4.根据权利要求1所述一种基于hybrid astar与空间走廊的路径规划方法，其特征在于，所述根据得到的初始路径膨胀路点至障碍物，得到多个矩形空间的具体步骤包括：

5.根据权利要求4所述一种基于hybrid astar与空间走廊的路径规划方法，其特征在于，所述对得到的多个矩形空间进行数量简化，并形成空间走廊的具体步骤包括：

6.根据权利要求1所述一种基于hybrid astar与空间走廊的路径规划方法，其特征在于，所述通过最优控制模型在空间走廊内对初始路径进行二次规划，得到优化后的路径的具体步骤包括：

技术总结
本发明公开了一种基于Hybrid Astar与空间走廊的路径规划方法，包括构建多障碍环境下的全局占据栅格地图，并设定任务起始点和终点的坐标；根据构建的全局占据栅格地图基于Hybrid Astar进行路径规划，得到规划的初始路径；根据得到的初始路径膨胀路点至障碍物，得到多个矩形空间；对得到的多个矩形空间进行数量简化，并形成空间走廊；通过最优控制模型在空间走廊内对初始路径进行二次规划，得到优化后的路径。本发明在多障碍物情景下进行路径规划，在对初始路径优化时引入空间走廊，并建立最优控制模型得到优化后处于无障碍的空间走廊内的最优路径，满足车辆运动学约束的同时最大程度保证车辆的安全，有效地避免优化路径波动大导致与近处障碍物碰撞的危险。

技术研发人员：梁华为,常燚,李志远,王健
受保护的技术使用者：中国科学院合肥物质科学研究院
技术研发日：
技术公布日：2024/1/12