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

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


背景技术：

2.随着科技的发展与社会的进步，移动机器人成为了一个新的研究热点。而在移动机器人作业过程中，路径规划技术保证了机器人的安全行驶，起着不可替代的作用。路径规划算法可分为全局路径规划算法和局部路径规划算法。全局路径规划是在已知的环境中，为机器人规划出一条最优或近似最优的完整路径；局部路径规划是指机器人在全局路径的基础上，基于实时的感知信息，动态地规划出一条能实时安全通过的路径。
3.teb(timed elasitc band)算法是在eb算法的基础上发展而来的。teb算法引入时间信息参量，增加了速度、加速度等约束，解决了早期局部路径规划算法普遍存在的问题，因此被广泛应用于移动机器人与自动驾驶领域。然而在实际应用中，teb算法还是存在速度和角速度波动较大、稳定性不高等问题，不利于轨迹跟踪。因此，需要对传统teb算法做出改进，解决当前存在的问题。
4.目前已有一些对基于teb的局部路径规划的研究。申请号为202011006826.0的专利，根据障碍物相对局部路径的位置来判断局部路径是否震荡，通过预设自适应避障距离对路径进行优化，减少了路径震荡的范围，但未考虑局部路径与移动机器人运动速度之间的关系，具有一定的局限性；申请号为202010877790.7的专利，使用改进的teb算法用于agv的局部路径规划，考虑了agv的速度和运动学约束，平滑了agv的运动速度，但没有考虑机器人模型带来的线速度和角速度之间的约束关系。


技术实现要素：

5.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于改进teb的四轮差速机器人局部路径规划方法。
6.本发明的目的可以通过以下技术方案来实现：
7.作为本发明的第一方面，提供一种局部路径规划方法，所述方法用于对四轮差速机器人进行局部路径规划，步骤包括：
8.基于四轮差速底盘运动学模型，建立约束方程；
9.构建改进teb算法模型，所述改进teb算法模型设置有速度和角速度的联合约束以及相邻位姿时间间隔的限制；
10.基于g2o图优化框架，求解改进teb算法模型；
11.设定改进teb算法模型参数，进行局部路径规划。
12.进一步的，所述基于四轮差速底盘运动学模型，建立约束方程，包括以下步骤：
13.建立四轮差速机器人简化模型，其左右轮距为d，前后轮距为l；
14.建立四轮差速机器人运动模型；
15.小车质心前进线速度v
t
为：
[0016][0017]
小车质心处的角速度ω
t
与左右轮速之间的关系为：
[0018][0019]
式中，v
ot
为外侧轮线速度；v
it
为内侧轮线速度；r
t
为转弯半径。
[0020]
进一步的，所述构建改进teb算法模型，包括以下步骤：
[0021]
将全局路径离散化为一系列相对于世界坐标系的位姿；
[0022]
构建速度与加速度的约束；
[0023]
构建速度和角速度的联合约束；
[0024]
构建速度与加速度的约束；
[0025]
限制teb相邻位姿的时间间隔。
[0026]
进一步的，所述速度与加速度的约束具体如下：
[0027]
相邻位姿的速度表示为：
[0028][0029]
相邻位姿的角速度表示为：
[0030][0031]
式中，xi,yi为机器人的位置；θi为机器人的朝向；δti为邻两个位姿之间的时间间隔。
[0032]
进一步的，所述速度和角速度的联合约束表示如下：
[0033][0034]
式中，v
max
为机器人最大线速度；δ和α是需要调整的参数，且均大于0；k为曲率，表示如下
[0035][0036]
式中，r为机器人转弯半径，vi与ωi分别为相邻位姿的速度与角速度。
[0037]
进一步的，所述限制teb相邻位姿的时间间隔δti具体如下：
[0038]
定义参数dt_ref，用于限制teb相邻位姿的时间间隔δti；定义参数dt_hysteresis作为浮动时间；对一段给定的路径，遍历目标序列，选取路径点的规则如下：
[0039]
若相邻位姿的时间间隔δti大于dt_ref与dt_hysteresis之和，则在位姿si到s
i+1
之间插入一个新的状态，且插入状态与两个相邻位姿的时间间隔均为
[0040]
若相邻位姿的时间间隔δti小于dt_ref与dt_hysteresis之差，则删除位姿s
i+1
，此时新的时间间隔为δti加上δt
i+1
；
[0041]
根据新的时间间隔，调整目标序列，继续判断是否满足期望，符合则输出平滑路径，不符合则再次进行循环。
[0042]
进一步的，所述基于g2o图优化框架，求解teb算法模型，具体如下：
[0043]
将机器人的位姿和相邻位姿的时间作为待优化的节点；将各个状态间的约束，如速度与加速度约束、障碍物约束、非完整运动学约束、时间最优约束等作为优化的边，建立n个优化函数后进行加权并求和，定义总体优化函数如下：
[0044][0045]
式中，εk为fk(q,δτ)的权重系数；q为机器人在世界坐标系下的位姿序列；δτ为位姿间隔的时间序列；
[0046]
通过g2o图优化框架，对上述优化函数进行求解，得到当前时刻最优的局部路径。
[0047]
作为本发明的第二方面，提供一种局部路径规划设备，包括：
[0048]
一个或多个处理器；
[0049]
存储器，用于存储一个或多个程序；
[0050]
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上所述的局部路径规划方法。
[0051]
作为本发明的第三方面，提供一种四轮差速机器人，所述四轮差速机器人采用有如上所述的局部路径规划设备进行局部路径规划。
[0052]
作为本发明的第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的局部路径规划的步骤。
[0053]
与现有技术相比，本发明具有以下有益效果：
[0054]
本发明在传统teb算法进行局部路径规划的基础上，增加了速度和角速度的联合约束，进一步限制了最大线速度，减小了速度和角速度的波动，提高了算法的稳定性；除此之外，增加了对teb相邻位姿时间间隔的限制，从而限制了局部路径规划的解析度，使规划出的局部路径更为平滑，同时降低了机器人速度和角速度变化的频率，更易于路径跟踪的实现。本发明对提升四轮差速机器人在路径规划与导航时的稳定性具有着重要的意义。
附图说明
[0055]
图1是本发明一种基于改进teb的四轮差速机器人局部路径规划方法的流程图；
[0056]
图2是四轮差速机器人的简化模型示意图；
[0057]
图3是仿真实验流程图；
[0058]
图4是四轮差速机器人进行局部路径规划的示意图。
具体实施方式
[0059]
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
[0060]
实施例1
[0061]
本发明为基于改进teb的四轮差速机器人局部路径规划方法，该方法的流程图如图1所示，包含以下步骤：
[0062]
步骤1：分析四轮差速底盘运动学模型，建立约束方程；
[0063]
步骤2：构建teb算法模型，提出改进方法；
[0064]
步骤3：基于g2o图优化框架，求解teb算法模型；
[0065]
步骤4：设定改进后的teb参数，对四轮差速机器人进行局部路径规划。
[0066]
具体的，如图2所示，在步骤1中，分析四轮差速底盘运动学模型，建立约束方程，包括以下步骤：
[0067]
步骤11：建立四轮差速机器人简化模型，其左右轮距为d，前后轮距为l；
[0068]
步骤12：建立四轮差速机器人运动模型。在四轮差速模型中，同侧前后轮速度同步，设外侧轮线速度为v
ot
,内侧轮线速度为v
it
，整车质心处理论线速度为v
t
，角速度为ω
t
。设机器人在转向时，绕圆心o
t
做圆周运动，且转弯半径为r
t
。小车前进线速度为：
[0069][0070]
绕圆心做圆周运动的物体，其线速度v、角速度ω和圆周半径r满足：
[0071][0072]
由相似理论可得，小车绕旋转中心o
t
点的角速度：
[0073][0074]
因此，整车质心处的角速度与左右轮速之间的关系为：
[0075][0076]
具体的，在步骤2中，构建teb算法模型，提出改进方法，包括以下步骤：
[0077]
步骤21：将全局路径离散化为一系列相对于世界坐标系的位姿，机器人的第i个位姿状态可以表示为：
[0078]
si＝(xi,yi,θi)
t (5)
[0079]
其中xi,yi是机器人的位置，θi是机器人的朝向。机器人在世界坐标系下的位姿序列记为：
[0080]
q＝{si},i＝0,1,...,n,n∈n (6)
[0081]
记相邻两个位姿之间的时间间隔为δti，n个位姿间隔的时间序列记为：
[0082]
δτ＝{δti},i＝0,1,...,n-1,n∈n (7)
[0083]
将位姿序列与时间序列合并，则teb的轨迹信息表示为：
[0084]
t:＝{q,δτ}＝{s1,δt1,s2,δt2,...,s
n-1
,δt
n-1
,sn},n∈n(8)
[0085]
步骤22：原始的teb算法存在速度与加速度的约束，其作用是使优化出的轨迹能满足机器人的运动学约束。相邻位姿si、s
i+1
的速度和角速度可以近似为：
[0086][0087]
[0088]
原始算法中，只考虑到了机器人最大速度和最大加速度的约束，而机器人本身的速度和角速度也存在着约束关系。在实际情况下，机器人可能无法同时满足最大线速度和最大角速度。因此，对算法增加了速度和角速度的联合约束。定义r为机器人转弯半径，k为曲率，即
[0089][0090]
增加速度和角速度的联合约束如下：
[0091][0092]
式中，v
max
为机器人最大线速度，δ和α是需要调整的参数，且均大于0。增加上述联合约束后，进一步限制了机器人线速度与角速度的关系，提高了稳定性。
[0093]
步骤23：定义参数dt_ref，限制teb相邻位姿的时间间隔δti；同时定义了参数dt_hysteresis作为浮动时间，即dt_ref可以在设定的基础上进行浮动。对一段给定的路径，遍历目标序列，选取路径点的规则如下：
[0094]
(1)若δti》dt_ref+dt_hysteresis,则在位姿si到s
i+1
之间插入一个新的状态s
new
，且s
new
与si、s
i+1
的时间间隔均为
[0095]
(2)若δti《dt_ref-dt_hysteresis，则删除状态s
i+1
，此时新的时间间隔为δti+δt
i+1
；
[0096]
(3)根据新的时间间隔，调整目标序列，继续判断是否满足期望，符合则输出平滑路径，不符合则再次进行循环。
[0097]
当解析度过高时，相邻位姿的时间间隔过小，意味着机器人会在极短的时间内不断通过状态搜索树寻找最优路径，导致了局部路径的震荡。加入对解析度的限制后，在可接受的范围内牺牲一定的精度，使得到的路径更为平滑。
[0098]
具体的，步骤3，基于g2o图优化框架，求解teb算法模型，包括：
[0099]
将机器人的位姿和相邻位姿的时间作为待优化的节点；将各个状态间的约束，如速度与加速度约束、障碍物约束、非完整运动学约束、时间最优约束等作为优化的边，建立n个优化函数后进行加权并求和，定义总体优化函数如下：
[0100]
f(q,δτ)＝∑kε
kfk
(q,δτ)，k＝1,...,n (13)
[0101]
其中，εk为fk(q,δτ)的权重系数。通过g2o图优化框架，对上述优化函数进行求解，得到当前时刻最优的局部路径。
[0102]
具体的，步骤4，设定改进后teb算法模型参数，采用模型对四轮差速机器人进行局部路径规划。
[0103]
方法验证：
[0104]
设定改进后的teb参数，本发明所提供的四轮差速机器人进行局部路径规划方法进行局部路径规划仿真实验，实验流程如图3所示，包括以下步骤：
[0105]
步骤51：建立四轮差速机器人urdf模型，同时添加激光雷达模型，用于仿真中小车的定位。
[0106]
步骤52：将urdf模型简化为xacro模型，并添加gazebo标签和差速控制器，在gazebo物理引擎中实现对四轮差速小车的控制。
[0107]
步骤53：在gazebo中建立仿真实验场景，添加障碍物。
[0108]
步骤54：设定速度和角速度联合约束参数，δ＝0.5，α＝2；设定局部路径规划解析度参数dt_ref＝0.35s，dt_hysteresis＝0.1s。启动改进teb的算法节点，实现局部路径规划，并在rviz中显示。自主导航效果如图4所示，图中绿色轨迹为全局路径，红色轨迹为局部路径。
[0109]
本实例公开一种基于改进teb的四轮差速机器人局部路径规划方法，使用四轮差速机器人，并对提出的改进规划算法进行了仿真验证。通过增加速度和角速度联合约束、限制teb相邻位姿之间的时间间隔，使局部路径更为平滑，减少了速度与角速度的大幅度波动，提升了机器人在路径跟踪时的稳定性与可靠性。
[0110]
实施例2
[0111]
作为本发明的第二方面，本技术还提供一种局部路径规划设备，包括：一个或多个处理器11；存储器12，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器11执行，使得所述一个或多个处理器11实现如上述局部路径规划方法。除了所述的处理器、存储器以及接口之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
[0112]
实施例3
[0113]
作为本发明的第二方面，本技术还提供一种四轮差速机器人，该差四轮差速机器人搭载有如上实施例所述的局部路径规划设备，四轮差速机器人采用该设备进行局部路径规划。
[0114]
实施例4
[0115]
作为本发明的第四方面，本技术还提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如上述局部路径规划方法。所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡(smart media card，smc)、sd卡、闪存卡(flash card)等。进一步的，所述计算机可读存储介还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
[0116]
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

技术特征：
1.一种局部路径规划方法，其特征在于，所述方法用于对四轮差速机器人进行局部路径规划，步骤包括：基于四轮差速底盘运动学模型，建立约束方程；构建改进teb算法模型，所述改进teb算法模型设置有速度和角速度的联合约束以及相邻位姿时间间隔的限制；基于g2o图优化框架，求解改进teb算法模型；设定改进teb算法模型参数，进行局部路径规划。2.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述基于四轮差速底盘运动学模型，建立约束方程，包括以下步骤：建立四轮差速机器人简化模型，其左右轮距为d，前后轮距为l；建立四轮差速机器人运动模型；小车质心前进线速度v
t
为：小车质心处的角速度ω
t
与左右轮速之间的关系为：式中，v
ot
为外侧轮线速度；v
it
为内侧轮线速度；r
t
为转弯半径。3.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述构建改进teb算法模型，包括以下步骤：将全局路径离散化为一系列相对于世界坐标系的位姿；构建速度与加速度的约束；构建速度和角速度的联合约束；构建速度与加速度的约束；限制teb相邻位姿的时间间隔。4.根据权利要求3所述的一种局部路径规划方法，其特征在于，所述速度与加速度的约束具体如下：相邻位姿的速度v
i
表示为：相邻位姿的角速度ω
i
表示为：式中，x
i
,y
i
为机器人的位置；θ
i
为机器人的朝向；δt
i
为邻两个位姿之间的时间间隔。5.根据权利要求4所述的一种局部路径规划方法，其特征在于，所述速度和角速度的联合约束表示如下：
式中，v
max
为机器人最大线速度；δ和α是需要调整的参数，且均大于0；k为曲率，表示如下：式中，r为机器人转弯半径，v
i
与ω
i
分别为相邻位姿的速度与角速度。6.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述限制teb相邻位姿的时间间隔δt
i
具体如下：定义参数dt_ref，用于限制teb相邻位姿的时间间隔δt
i
；定义参数dt_hysteresis作为浮动时间；对一段给定的路径，遍历目标序列，选取路径点的规则如下：若相邻位姿的时间间隔δt
i
大于dt_ref与dt_hysteresis之和，则在位姿s
i
到s
i+1
之间插入一个新的状态，且插入状态与两个相邻位姿的时间间隔均为若相邻位姿的时间间隔δt
i
小于dt_ref与dt_hysteresis之差，则删除位姿s
i+1
，此时新的时间间隔为δt
i
加上δt
i+1
；根据新的时间间隔，调整目标序列，继续判断是否满足期望，符合则输出平滑路径，不符合则再次进行循环。7.根据权利要求1所述的一种局部路径规划方法，其特征在于，所述基于g2o图优化框架，求解teb算法模型具体如下：将机器人的位姿和相邻位姿的时间作为待优化的节点；将各个状态间的约束，如速度与加速度约束、障碍物约束、非完整运动学约束、时间最优约束等作为优化的边，建立n个优化函数后进行加权并求和，定义总体优化函数如下：式中，ε
k
为f
k
(q,δτ)的权重系数；q为机器人在世界坐标系下的位姿序列；δτ为位姿间隔的时间序列；通过g2o图优化框架，对上述优化函数进行求解，得到当前时刻最优的局部路径。8.一种局部路径规划设备，其特征在于，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-7任一项所述的局部路径规划方法。9.一种四轮差速机器人，其特征在于，所述四轮差速机器人采用有如权利要求8所述的局部路径规划设备进行局部路径规划。10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-7任一项所述的局部路径规划的步骤。

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


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