基于安全裕度的导航路径优化方法及相关设备与流程

摘要: 本技术涉及机器人路径规划,尤其涉及一种基于安全裕度的导航路径优化方法及相关设备。背景技术:1、四足机器人因具有较好的地形适应性和机动性,在室内配送、巡检等场景中...
基于安全裕度的导航路径优化方法及相关设备与流程

本技术涉及机器人路径规划,尤其涉及一种基于安全裕度的导航路径优化方法及相关设备。

背景技术:

1、四足机器人因具有较好的地形适应性和机动性,在室内配送、巡检等场景中的应用日益广泛。相关技术中,四足机器人在室内固定地图环境下的自主导航通常基于占据栅格地图实现,具体为:将室内环境划分为规则栅格,标记障碍区域和自由空间,以机器人当前位置为起点、以目标点为终点进行路径搜索,得到可供机器人通行的导航路径。

2、然而,相关技术中的路径搜索通常以路径长度最短为目标,得到的路径容易贴近障碍物边缘。四足机器人在实际行走过程中,机身具有一定的宽度,步态摆动和定位误差会使机器人与障碍物之间的实际距离小于规划距离,导致机器人与墙体、桌椅、门框等障碍物发生擦碰,导航的安全性不足,导致室内四足机器人的导航路径规划效果差。

技术实现思路

1、本技术的主要目的在于提供一种基于安全裕度的导航路径优化方法及相关设备,旨在解决相关技术中室内四足机器人导航路径规划效果差的技术问题。

2、为实现上述目的,本技术提出一种基于安全裕度的导航路径优化方法,所述的方法包括:

3、获取目标区域的第一像素坐标和机器人当前位置的第二像素坐标;

4、基于预存的室内场景地图识别障碍区域,对所述障碍区域进行膨胀处理,并计算自由空间中各点到最近的膨胀后的障碍区域的欧式清距;

5、基于所述欧式清距及预设的安全裕度,以所述第二像素坐标为起点、以所述第一像素坐标为终点进行带清距惩罚的路径搜索,得到安全路径;

6、基于预设的稀疏优化规则对所述安全路径进行路径点优化,得到目标导航路径。

7、在一实施例中,所述基于所述欧式清距及预设的安全裕度,以所述第二像素坐标为起点、以所述第一像素坐标为终点进行带清距惩罚的路径搜索,得到安全路径的步骤包括:

8、将所述欧式清距与预设的安全清距阈值进行比较;

9、当所述欧式清距小于所述安全清距阈值时,根据所述欧式清距与所述安全清距阈值的差值确定惩罚代价,并将所述惩罚代价叠加至路径搜索的基础移动代价,得到路径搜索代价;

10、当所述欧式清距不小于所述安全清距阈值时,以所述基础移动代价作为所述路径搜索代价;

11、根据所述路径搜索代价,以所述第二像素坐标为起点、以所述第一像素坐标为终点执行路径搜索,得到安全路径。

12、在一实施例中,所述对所述障碍区域进行膨胀处理的步骤包括:

13、获取所述机器人的机身尺寸、步态摆动范围和定位误差;

14、根据所述机身尺寸确定基础膨胀半径;

15、根据所述步态摆动范围确定摆动余量,并根据所述定位误差确定误差余量;

16、根据所述基础膨胀半径、所述摆动余量和所述误差余量确定膨胀半径;

17、以所述障碍区域的边界为基准,按所述膨胀半径向外扩展,得到膨胀后的障碍区域。

18、在一实施例中,所述基于预设的稀疏优化规则对所述安全路径进行路径点优化,得到目标导航路径的步骤包括:

19、对所述安全路径中的路径点进行直线可视检测,判断两个路径点之间的连线是否穿越所述障碍区域;

20、当所述连线未穿越所述障碍区域时,删除所述两个路径点之间的冗余路径点;

21、计算删除所述冗余路径点后剩余的相邻路径点之间的段长;

22、当所述段长超过预设的最大段长时,沿所述相邻路径点之间的线段方向插入中间点;

23、当所述中间点与所述第一像素坐标之间的距离小于预设的近邻阈值时,以所述相邻路径点之间的中点替代所述中间点;

24、根据插入或替代后的路径点,得到目标导航路径。

25、在一实施例中,所述获取目标区域的第一像素坐标和机器人当前位置的第二像素坐标的步骤包括:

26、读取所述目标区域预存的第一像素坐标;

27、获取所述机器人当前的地图坐标;

28、获取预设的地图原点偏移、地图分辨率和栅格高度;

29、根据所述地图原点偏移、所述地图分辨率和所述栅格高度,将所述当前的地图坐标转换为第二像素坐标。

30、在一实施例中,所述基于预设的稀疏优化规则对所述安全路径进行路径点优化,得到目标导航路径的步骤之后,还包括:

31、根据所述目标导航路径中相邻路径点确定目标航向角;

32、获取所述机器人当前的航向角;

33、计算所述目标航向角与所述当前的航向角之间的航向差;

34、当所述航向差小于预设的角度阈值时,保持所述机器人当前的航向角;

35、当所述航向差不小于所述角度阈值时,将所述机器人的航向角调整至所述目标航向角。

36、此外,为实现上述目的,本技术还提出一种基于安全裕度的导航路径优化装置,所述基于安全裕度的导航路径优化装置包括:

37、获取模块,用于获取目标区域的第一像素坐标和机器人当前位置的第二像素坐标;

38、计算模块,用于基于预存的室内场景地图识别障碍区域,对所述障碍区域进行膨胀处理,并计算自由空间中各点到最近的膨胀后的障碍区域的欧式清距;

39、规划模块,用于基于所述欧式清距及预设的安全裕度,以所述第二像素坐标为起点、以所述第一像素坐标为终点进行带清距惩罚的路径搜索,得到安全路径;

40、优化模块,用于基于预设的稀疏优化规则对所述安全路径进行路径点优化,得到目标导航路径。

41、此外,为实现上述目的,本技术还提出一种基于安全裕度的导航路径优化设备,所述设备包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述计算机程序配置为实现如上文所述的基于安全裕度的导航路径优化方法的步骤。

42、此外,为实现上述目的,本技术还提出一种存储介质,所述存储介质为计算机可读存储介质,所述存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如上文所述的基于安全裕度的导航路径优化方法的步骤。

43、此外,为实现上述目的,本技术还提供一种计算机程序产品,所述计算机程序产品包括计算机程序,所述计算机程序被处理器执行时实现如上文所述的基于安全裕度的导航路径优化方法的步骤。

44、本技术提出的一个或多个技术方案,至少具有以下技术效果:

45、与相关技术相比,本技术通过获取目标区域的第一像素坐标和机器人当前位置的第二像素坐标,基于预存的室内场景地图识别障碍区域并对障碍区域进行膨胀处理,计算自由空间中各点到最近的膨胀后的障碍区域的欧式清距,基于欧式清距及预设的安全裕度进行带清距惩罚的路径搜索得到安全路径,基于预设的稀疏优化规则对安全路径进行路径点优化得到目标导航路径。可理解的是,本技术通过计算自由空间中各点到最近的膨胀后的障碍区域的欧式清距,并将欧式清距作为路径搜索的惩罚依据,使路径在避开障碍区域的同时主动远离障碍物边缘,从而提升路径的安全性;通过基于预设的稀疏优化规则对安全路径进行路径点优化,使路径点密度与机器人导航协议的执行接口相匹配,从而提升导航执行的稳定性,进而提升室内四足机器人的导航路径规划效果。