[发明专利]基于TEB的路径规划方法及装置

权利要求书

1.一种基于时间弹力带TEB的路径规划方法，所述方法适用于移动机器人的局部路径规划，其特征在于，所述方法，包括：

根据预设局部初始路径选择条件，确定本次局部路径规划的局部初始路径；

根据所述局部初始路径，按照预设自适应避障距离对所述局部初始路径进行TEB优化以得到优化成功的局部优化路径；

所述根据预设局部初始路径选择条件，确定本次局部路径规划的局部初始路径，包括：

若TEB优化连续失败次数大于预设次数；或，

若发生路径震荡，且路径震荡持续的时间超过预设时间阈值；或，

若本次局部路径规划的起点和终点相较于上次局部路径规划的起点和终点的偏移量大于预设偏移阈值时，

按照本次的局部代价地图进行全局路径规划，得到本次局部路径规划的局部初始路径；其中，所述本次局部路径规划的起点为所述移动机器人的位置点，终点为全局路径落在所述局部代价地图边缘处的点；所述局部代价地图是以所述移动机器人为中心、面积为预设面积的区域所对应的地图；

否则，将上次得到的局部优化路径作为本次局部路径规划的局部初始路径。

2.根据权利要求1所述的方法，其特征在于，所述按照本次的局部代价地图进行全局路径规划，包括：

将本次的局部代价地图作为全局代价地图，采用Dijkstra算法进行全局路径规划，将得到的全局路径作为局部初始路径。

3.根据权利要求1所述的方法，其特征在于，所述方法，还包括：

确定本次的局部代价地图中的障碍物是否存在于上次的局部代价地图中；

若存在，则判断所述障碍物是否分别位于本次局部初始路径左侧、且位于上次局部初始路径的右侧，或位于本次局部初始路径右侧、且位于上次局部初始路径的左侧，若是，则确定发生路径振荡。

4.根据权利要求1所述的方法，其特征在于，所述按照预设自适应避障距离对所述局部初始路径进行TEB优化，包括：

对局部初始路径上每个位姿执行如下操作：

确定离当前位姿最近的左右两边障碍物的距离，得到第一距离和第二距离；

计算所述第一距离和第二距离的平均值，并对所述位姿进行调整直至所述平均值大于等于预设最小避障距离且小于等于预设最大避障距离。

5.一种基于时间弹力带TEB的路径规划装置，所述装置适用于移动机器人的局部路径规划，其特征在于，包括：确定单元以及优化单元；其中，

所述确定单元，用于根据预设局部初始路径选择条件，确定本次局部路径规划的局部初始路径；

所述优化单元，用于根据所述局部初始路径，按照预设自适应避障距离对所述局部初始路径进行TEB优化以得到优化成功的局部优化路径；

所述确定单元，根据预设局部初始路径选择条件，确定本次局部路径规划的局部初始路径，具体用于：

若TEB优化连续失败次数大于预设次数；或，

若发生路径震荡，且路径震荡持续的时间超过预设时间阈值；或，

若本次局部路径规划的起点和终点相较于上次局部路径规划的起点和终点的偏移量大于预设偏移阈值时，

按照本次的局部代价地图进行全局路径规划，得到本次局部路径规划的局部初始路径；其中，所述本次局部路径规划的起点为所述移动机器人的位置点，终点为全局路径落在所述局部代价地图边缘处的点；所述局部代价地图是以所述移动机器人为中心、面积为预设面积的区域所对应的地图；

否则，将上次得到的局部优化路径作为本次局部路径规划的局部初始路径。

6.根据权利要求5所述的装置，其特征在于，所述确定单元，按照本次的局部代价地图进行全局路径规划，具体用于：

将本次的局部代价地图作为全局代价地图，采用Dijkstra算法进行全局路径规划，将得到的全局路径作为局部初始路径。