一种基于改进DWA和TEB算法的四足机器人局部路径规划方法

本发明涉及路径规划，更具体涉及一种基于改进dwa和teb算法的四足机器人局部路径规划方法。

背景技术：

1、路径规划作为运动规划的重要组成部分，其发展现状呈现出多元化、智能化和跨领域融合的趋势。在机器人领域，路径规划是实现机器人自主导航和避障的关键技术。无论是工业机器人还是服务机器人，都需要通过路径规划来规划出安全、高效的行驶路线。例如，在仓库、工厂、灾害疏导等环境中，无人搬运车等需要根据实际情况规划路径以实现高效、安全的运行。类似的现有技术有公开号为cn118274847b的中国专利，提出一种基于路面不平度的农业巡检机器人导航决策规划方法，包括控制农业巡检机器人构建二维栅格地图，根据二维栅格地图进行全局路径规划，得到全局最优路径。测量高度数据点的高度值，根据高度值计算激光评定值。根据激光评定值和视觉评定值计算路面不平度，根据路面不平度进行局部路径规划，得到局部最优路径。控制农业巡检机器人运动，更新全局最优路径和局部最优路径，直到农业巡检机器人到达目标终点。结合激光评定值和视觉评定值计算路面不平度，可以提前对周围环境的路面不平度进行识别与预测，提高了路面不平度评估的准确性和鲁棒性。根据场景实际情况实时更新全局最优路径和局部最优路径，可以保证巡检作业的顺利进行。此外类似的现有技术还有公开号为us20210402599a1的美国专利，提出一种移动机器人控制装置，包括在线3d建模器和路径规划器。在线3d建模器被配置为从移动机器人接收图像序列并基于图像序列生成第一地图和不同于第一地图的第二地图。路径规划器被配置为基于第一映射生成全局路径，基于第二映射提取目标表面，并基于全局生成具有小于全局路径的移动单位的移动单位的局部检查路径。上述两篇专利都解决了路径规划的问题，但是都没有考虑到移动障碍物速度变化的问题及静态障碍物避开过程中路径优化的问题。

技术实现思路

1、为了更好的解决上述问题，本发明提供一种基于改进dwa和teb算法的四足机器人局部路径规划方法，所述方法包括如下步骤：

2、步骤s1：获取所述机器人的起始位置和目标位置，还获取所述起始位置到所述目标位置之间的静态障碍物信息，并基于所述起始位置、所述目标位置及所述静态障碍物信息获取第一路径；

3、步骤s2：通过平滑算法对所述第一路径进行平滑处理，获取第二路径；

4、步骤s3：所述机器人沿着所述第二路径行驶过程中，通过监测单元实时监测所述第二路径中的移动障碍物的当前位置，并获取所述移动障碍物的最大速度，基于所述移动障碍物当前位置、所述最大速度及所述机器人的当前位置和预估平均速度计算最大避障距离；

5、步骤s4：在所述机器人与所述移动障碍物之间的第一距离小于所述最大避障距离，且所述移动障碍物的实时速度小于最大速度时，周期性获取平均避障距离及碰撞速度窗口，并基于所述机器人与所述移动障碍物之间的第一距离与安全避障距离之间的差值及所述碰撞速度窗口获取避障速度；

6、步骤s5：基于所述避障速度和所述第二路径获取第三路径，并基于所述第三路径到达所述目标位置。

7、作为本发明一种优选技术方案，所述步骤s1包括：

8、将所述起始位置作为所述第一路径的第一节点，将所述机器人的所述起始位置和所述目标位置用直线连接，并获取所述直线上的所述静态障碍物信息，距离所述第一路径上第i节点最近的所述静态障碍物作为避障目标，获取所述避障目标的第一近似圆，并以所述第i节点为起始点作所述避障目标对应所述第一近似圆的两侧切线，并将所述两侧切线上的目标点作为第i+1节点，其中，所述目标点为所述两侧切线上与所述目标位置连线能够避开所述避障目标且距离所述直线最近的点，重复本步骤，获取所述第一路径上的全部节点，i的取值为大于等于2的正整数，所述静态障碍物包括所述静态障碍物的位置及所述第一近似圆。

9、作为本发明一种优选技术方案，所述步骤s2包括：

10、将所述第一路径上任意两个不连续节点相连接，获取第一连接线，在所述第一连接线能够避开所述两个不连续节点中间的所述静态障碍物时，将所述两个不连续节点中间的其他节点删除，获取新的所述第一路径，获取新的所述第一路径中每一节点的两条边的外切圆及两个切点，将所述两个切点之间圆弧作为平滑路径，基于新的所述第一路径和所述平滑路径获取所述第二路径，其中通过设置所述圆弧的半径，使得所述圆弧能够满足所述机器人平稳行驶。

11、作为本发明一种优选技术方案，所述步骤s3包括：

12、步骤s31：在所述机器人沿着所述第二路径进行行驶的过程中，通过所述监测单元实时监测所述移动障碍物的当前位置，在所述机器人的当前位置与所述移动障碍物当前位置之间的第一距离大于预设距离时，获取所述移动障碍物的实时速度；

13、步骤s32：在所述第一距离小于等于所述预设距离时，根据之前获取的所述移动障碍物的所述实时速度获取所述移动障碍物的所述最大速度；

14、步骤s33：基于所述机器人的机器算法获取所述机器人在与所述移动障碍物交汇之前的预估平均速度，基于所述移动障碍物的所述最大速度、所述机器人的所述预估平均速度和所述移动障碍物的第二近似圆半径通过避障距离公式计算所述最大避障距离，所述避障距离公式为：

15、

16、其中，dmax为所述最大避障距离，vmax为所述移动障碍物的最大速度，v0为所述机器人的预估平均速度，r0为所述第二近似圆的半径。

17、作为本发明一种优选技术方案，所述步骤s4包括如下步骤：

18、步骤s41：在所述机器人与所述移动障碍物之间的第一距离小于所述最大避障距离，且所述移动障碍物的实时速度大于最小速度时，周期性的获取所述移动障碍物的第二平均速度和所述机器人的第一平均速度，并基于所述第一平均速度、所述第二平均速度及所述移动障碍物的第二近似圆半径周期性的计算平均避障距离及所述平均避障距离对应的碰撞速度窗口；

19、步骤s42：设置所述机器人与所述移动障碍物的安全避障距离为所述平均避障距离与余量距离之和，在所述机器人与所述移动障碍物之间的第一距离与所述安全避障距离之间的差值大于设定差值时，所述机器人按照预定速度进行行驶，在所述第一距离与所述安全避障距离之间的差值小于设定差值时，基于所述碰撞速度窗口生成所述机器人的避障速度，其中所述余量距离d的计算公式为：

20、

21、作为本发明一种优选技术方案，所述平均避障距离公式为：

22、

23、其中，d1为避障距离，v1为所述机器人的第一平均速度，v2为所述移动障碍物的第二平均速度，r为所述移动障碍物的所述第二近似圆的半径。

24、作为本发明一种优选技术方案，所述步骤s5包括：

25、基于所述机器人的所述避障速度获取与所述第二路径距离最短的避障路径，并将避障节点与所述目标位置连接，并重复步骤s1和所述步骤s2获取第三路径，所述机器人基于所述第三路径行驶。

26、作为本发明一种优选技术方案，所述第二近似圆为所述移动障碍物的外接圆。

27、本发明还提供一种基于改进dwa和teb算法的四足机器人局部路径规划系统，所述系统用于实现上述的方法，所述系统包括：

28、路径规划单元，用于获取所述机器人的起始位置和目标位置，还获取所述起始位置到所述目标位置之间的静态障碍物信息，并基于所述起始位置、所述目标位置及所述静态障碍物信息获取第一路径；

29、平滑单元，用于通过平滑算法对所述第一路径进行平滑处理，获取第二路径；

30、通过监测单元，用于在所述机器人沿着所述第二路径行驶过程中，实时监测所述第二路径中的移动障碍物的当前位置，并获取所述移动障碍物的最大速度，基于所述移动障碍物当前位置、所述最大速度及所述机器人的当前位置和预估平均速度计算最大避障距离；

31、计算单元，用于在所述机器人与所述移动障碍物之间的第一距离小于所述最大避障距离，且所述移动障碍物的实时速度小于最大速度时，周期性获取平均避障距离及碰撞速度窗口，并基于所述机器人与所述移动障碍物之间的第一距离与安全避障距离之间的差值获取避障速度；

32、路径规划单元，还用于基于所述避障速度和所述第二路径获取第三路径，并基于所述第三路径到达所述目标位置。

33、本发明还提供一种计算机存储介质，所述存储介质存储有程序指令，其中在所述程序指令运行时控制所述存储介质所在设备执行上述的方法。

34、与现有技术相比，本发明的有益效果至少如下所述：

35、本发明通过将上述机器人的起始位置通过直线连接上述目标位置，上述直线穿过的上述静态障碍物为上述机器人需要避开的静态障碍物，并以上述第i节点为点作距离最近的上述静态障碍物外接圆的两侧切线，并将上述两侧切线上与上述目标位置的连线能够避开上述避障目标且距离上述直线最短的上述目标点作为上述第i+1节点，通过上述方法获取上述第一路径上的全部节点，从而获取最短避障的第一路径，还将上述第一路径经过平滑处理获取第二路径，在上述机器人沿着上述第二路径行驶的过程中，获取并基于上述机器人的上述平均速度、上述移动障碍物的第二近似圆半径及上述最大速度计算上述最大避障距离，为精确避开上述移动障碍物提供依据，还在上述机器人与上述移动障碍物之间的第一距离小于所述最大避障距离且上述移动障碍物的实时速度小于最大速度的情况下，通过周期性的计算平均避障距离及上述平均避障距离对应的速度碰撞窗口，并根据速度碰撞窗口及与上述第二路径的距离调整机器人的避障速度，在检测到上述机器人与上述移动障碍物之间的上述第一距离大于上述安全避障距离时，或者两者之间的差值小于上述设定差值，继续按照预定速度行驶，在第一距离小于上述安全避障距离且两者之间的差值连续n次大于等于上述设定差值，并且第一距离大于上述平均避障距离，即上述机器人与上述移动障碍物之间的第一距离能够满足要求，但是距离上述平均避障距离较近，并且连续出现n次，及时通过上述碰撞速度窗口生成所述机器人的避障速度，通过上述技术方案的相互配合，能够最大限度保证上述机器人稳定行驶的状态下，准确避障，还能够精确避障还能够提高机器人整体行驶效率。