局部路径规划方法、装置、四轮差速机器人及存储介质

本发明涉及移动机器人路径规划领域，尤其是涉及一种基于改进teb的四轮差速机器人局部路径规划方法。

背景技术：

1、随着科技的发展与社会的进步，移动机器人成为了一个新的研究热点。而在移动机器人作业过程中，路径规划技术保证了机器人的安全行驶，起着不可替代的作用。路径规划算法可分为全局路径规划算法和局部路径规划算法。全局路径规划是在已知的环境中，为机器人规划出一条最优或近似最优的完整路径；局部路径规划是指机器人在全局路径的基础上，基于实时的感知信息，动态地规划出一条能实时安全通过的路径。

2、teb(timed elasitc band)算法是在eb算法的基础上发展而来的。teb算法引入时间信息参量，增加了速度、加速度等约束，解决了早期局部路径规划算法普遍存在的问题，因此被广泛应用于移动机器人与自动驾驶领域。然而在实际应用中，teb算法还是存在速度和角速度波动较大、稳定性不高等问题，不利于轨迹跟踪。因此，需要对传统teb算法做出改进，解决当前存在的问题。

3、目前已有一些对基于teb的局部路径规划的研究。申请号为202011006826.0的专利，根据障碍物相对局部路径的位置来判断局部路径是否震荡，通过预设自适应避障距离对路径进行优化，减少了路径震荡的范围，但未考虑局部路径与移动机器人运动速度之间的关系，具有一定的局限性；申请号为202010877790.7的专利，使用改进的teb算法用于agv的局部路径规划，考虑了agv的速度和运动学约束，平滑了agv的运动速度，但没有考虑机器人模型带来的线速度和角速度之间的约束关系。

技术实现思路

1、本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于改进teb的四轮差速机器人局部路径规划方法。

2、本发明的目的可以通过以下技术方案来实现：

3、作为本发明的第一方面，提供一种局部路径规划方法，所述方法用于对四轮差速机器人进行局部路径规划，步骤包括：

4、基于四轮差速底盘运动学模型，建立约束方程；

5、构建改进teb算法模型，所述改进teb算法模型设置有速度和角速度的联合约束以及相邻位姿时间间隔的限制；

6、基于g2o图优化框架，求解改进teb算法模型；

7、设定改进teb算法模型参数，进行局部路径规划。

8、进一步的，所述基于四轮差速底盘运动学模型，建立约束方程，包括以下步骤：

9、建立四轮差速机器人简化模型，其左右轮距为d，前后轮距为l；

10、建立四轮差速机器人运动模型；

11、小车质心前进线速度vt为：

12、

13、小车质心处的角速度ωt与左右轮速之间的关系为：

14、

15、式中，vot为外侧轮线速度；vit为内侧轮线速度；rt为转弯半径。

16、进一步的，所述构建改进teb算法模型，包括以下步骤：

17、将全局路径离散化为一系列相对于世界坐标系的位姿；

18、构建速度与加速度的约束；

19、构建速度和角速度的联合约束；

20、构建速度与加速度的约束；

21、限制teb相邻位姿的时间间隔。

22、进一步的，所述速度与加速度的约束具体如下：

23、相邻位姿的速度表示为：

24、

25、相邻位姿的角速度表示为：

26、

27、式中，xi,yi为机器人的位置；θi为机器人的朝向；δti为邻两个位姿之间的时间间隔。

28、进一步的，所述速度和角速度的联合约束表示如下：

29、

30、式中，vmax为机器人最大线速度；δ和α是需要调整的参数，且均大于0；k为曲率，表示如下

31、

32、式中，r为机器人转弯半径，vi与ωi分别为相邻位姿的速度与角速度。

33、进一步的，所述限制teb相邻位姿的时间间隔δti具体如下：

34、定义参数dt_ref，用于限制teb相邻位姿的时间间隔δti；定义参数dt_hysteresis作为浮动时间；对一段给定的路径，遍历目标序列，选取路径点的规则如下：

35、若相邻位姿的时间间隔δti大于dt_ref与dt_hysteresis之和，则在位姿si到si+1之间插入一个新的状态，且插入状态与两个相邻位姿的时间间隔均为

36、若相邻位姿的时间间隔δti小于dt_ref与dt_hysteresis之差，则删除位姿si+1，此时新的时间间隔为δti加上δti+1；

37、根据新的时间间隔，调整目标序列，继续判断是否满足期望，符合则输出平滑路径，不符合则再次进行循环。

38、进一步的，所述基于g2o图优化框架，求解teb算法模型，具体如下：

39、将机器人的位姿和相邻位姿的时间作为待优化的节点；将各个状态间的约束，如速度与加速度约束、障碍物约束、非完整运动学约束、时间最优约束等作为优化的边，建立n个优化函数后进行加权并求和，定义总体优化函数如下：

40、

41、式中，εk为fk(q,δτ)的权重系数；q为机器人在世界坐标系下的位姿序列；δτ为位姿间隔的时间序列；

42、通过g2o图优化框架，对上述优化函数进行求解，得到当前时刻最优的局部路径。

43、作为本发明的第二方面，提供一种局部路径规划设备，包括：

44、一个或多个处理器；

45、存储器，用于存储一个或多个程序；

46、当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上所述的局部路径规划方法。

47、作为本发明的第三方面，提供一种四轮差速机器人，所述四轮差速机器人采用有如上所述的局部路径规划设备进行局部路径规划。

48、作为本发明的第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的局部路径规划的步骤。

49、与现有技术相比，本发明具有以下有益效果：

50、本发明在传统teb算法进行局部路径规划的基础上，增加了速度和角速度的联合约束，进一步限制了最大线速度，减小了速度和角速度的波动，提高了算法的稳定性；除此之外，增加了对teb相邻位姿时间间隔的限制，从而限制了局部路径规划的解析度，使规划出的局部路径更为平滑，同时降低了机器人速度和角速度变化的频率，更易于路径跟踪的实现。本发明对提升四轮差速机器人在路径规划与导航时的稳定性具有着重要的意义。

技术特征：

1.一种局部路径规划方法，其特征在于，所述方法用于对四轮差速机器人进行局部路径规划，步骤包括：

2.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述基于四轮差速底盘运动学模型，建立约束方程，包括以下步骤：

3.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述构建改进teb算法模型，包括以下步骤：

4.根据权利要求3所述的一种局部路径规划方法，其特征在于，所述速度与加速度的约束具体如下：

5.根据权利要求4所述的一种局部路径规划方法，其特征在于，所述速度和角速度的联合约束表示如下：

6.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述限制teb相邻位姿的时间间隔δti具体如下：

7.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述基于g2o图优化框架，求解teb算法模型具体如下：

8.一种局部路径规划设备，其特征在于，包括：

9.一种四轮差速机器人，其特征在于，所述四轮差速机器人采用有如权利要求8所述的局部路径规划设备进行局部路径规划。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-7任一项所述的局部路径规划的步骤。

技术总结
本发明涉及一种局部路径规划方法、装置、四轮差速机器人及存储介质。通过该规划方法对四轮差速机器人进行局部路径规划，步骤包括：基于四轮差速底盘运动学模型，建立约束方程；构建改进TEB算法模型，所述改进TEB算法模型设置有速度和角速度的联合约束以及相邻位姿时间间隔的限制；基于g2o图优化框架，求解改进TEB算法模型；设定改进TEB算法模型参数，进行局部路径规划。本发明能够解决传统TEB算法进行局部路径规划时存在稳定性不高的问题，使局部路径更为平滑，更有利于机器人进行路径跟踪。

技术研发人员：苗中华,王舜略,李楠,李云辉,胡博,何创新
受保护的技术使用者：上海大学
技术研发日：
技术公布日：2024/1/13