CN104155974A - 一种用于机器人快速避碰的路径规划方法及设备 - Google Patents

Abstract

本发明实施例提供一种用于机器人快速避碰的路径规划方法，本发明利用BBRrt算法和基于凸集理论的碰撞检测算法，同时从初始位形和目标位形交替向对方位形扩展，构成两棵平衡RRT树，能够迅速地在位形空间寻找到一条连接起始位形和目标位形的无碰撞运动路径，有效通过狭窄通道，且有效地避免与环境中的障碍物发生碰撞，并充分利用多步扩展方法和带有启发性目的的平衡扩展方式，从而有效地改善RRT算法的收敛性和重复性；基于凸集理论，提出一种基于线性不等式方式组所构成的凸包快速碰撞检测算法，可以有效改进避碰路径规划方法中碰撞检测模块的效率和性能。本发明提出的方法可以推广到高维机器人多树RRT路径规划。

Description

一种用于机器人快速避碰的路径规划方法及设备

技术领域

本发明涉及高维机器人领域，尤其涉及到一种用于机器人快速避碰的路径规划方法及设备。

背景技术

高维机器人深度融合了人工智能、智能控制、信息处理、先进制造和计算软件等方面的技术，其发展将对社会、经济、科学和国防等产生重大影响，已成为各国高新技术竞相研究的战略性目标。避碰路径规划是高维机器人研究的核心关键技术，受到国内外学者的普遍关注。

典型的路径规划方法包括：（1）传统方法，如，栅格法、拓扑降维法、人工势场法、多边形拟合法等；（2）基于图搜索的方法，如Dijkstra算法、Voronoi图、A*算法和D*算法等；（3）基于智能算法的方法，如，模糊规则法、神经网络算法、遗传算法、蚁群优化算法、模拟退火算法、粒子群优化等；（4）基于随机采样的方法，例如，概率地图法（PRM）和快速探索随机树法(RRT)。

RRT方法采用随机采样，仅需对C-空间采样点进行碰撞检测，避免了在C-空间中对机器人和障碍物的复杂建模，且采用树型结构存储基于随机采样扩展生成的机器人构型节点，其相邻节点之间生成的轨迹符合机器人运动学、动力学和避碰要求，适合解决含有运动动力学约束的非完整冗余高维复杂环境下的路径规划问题，因而得到研究者普遍关注。但因RRT基于随机采样法，该算法存在费时、收敛速度慢、实时性较差、路径规划可重复性差、经常远离最短路径或最优路径、碰撞检测频繁且代价较大等方面的问题。

发明内容

本发明实施例提供了一种用于机器人快速避碰的路径规划方法，旨在解决如何使用简单的算法使得机器人快速避碰的问题。

一种用于机器人快速避碰的路径规划方法，所述方法包括：

确定起始位形θ_start、目标位形θ_goal、和单步扩展步长λ、当前迭代次数；以θ_start为快速随机搜索树RRTT_a的根节点、以θ_goal为T_a的目标位形点，初始化树T_a；以θ_goal为T_b的根节点、以T_a树扩展生成的新位形θ_new为T_b的目标位形点，初始化树T_b；

在位形空间随机采样选取θ_rand，在T_a搜索离θ_rand距离最近的位形节点T_a从向θ_rand扩展一个步长λ；在T_b中搜索离θ_rand距离最近的位形节点T_b从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand之间距离小于所述步长λ；

当T_a和T_b最近节点之间的距离小于或等于λ时，则获取机器人避碰的路径。

所述方法还包括：

当T_a和T_b最近节点之间的距离大于λ时，则交换T_a和T_b，继续在位形空间随机采样选择θ_rand，在T_b搜索离θ_rand距离最近的位形节点T_b从向θ_rand扩展一个步长λ；在T_a中搜索离θ_rand距离最近的位形节点T_a从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand之间距离小于所述步长λ，并将所述当前迭代次数增加1。

所述方法在当T_a和T_b最近节点之间的距离小于或等于λ时，则获取机器人避碰的路径之前，还包括：

判断当前迭代次数是否大于最大迭代次数K；

所述方法还包括：

若当前迭代次数大于预先设置的迭代最大次数K时，则无法确定所述机器人避碰的路径。

所述T_b向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长λ，包括：

在T_b中获取离θ_rand最近的一个位形节点θ_nearest；

θ_nearest向θ_rand移动一个步长λ获得新的位形节点θ_new；

检测所述新的位形节点θ_new是否与所述障碍物碰撞；

若所述新的位形节点θ_new与所述障碍物碰撞，则所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new与θ_rand的距离小于或者等于步长λ，则θ_new已到达θ_rand，所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new未与所述障碍物碰撞或者未到达θ_rand,则θ_new再向θ_rand扩展一个步长λ。

所述检测所述新的位形节点θ_new是否与所述障碍物碰撞，包括：

将所述机器人在位形空间中的每个位形点表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

将在所述位形空间中的障碍物表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

若所述机器人与所述障碍物发生碰撞，则所述机器人的多个凸包和所述障碍物的多个凸包之间有碰撞。

所述设备包括：

初始化单元，用于确定起始位形θ_start、目标位形θ_goal、和单步扩展步长λ；以θ_start为快速随机搜索树RRTT_a的根节点、以θ_goal为T_a的目标位形点，初始化树T_a；以θ_goal为T_b的根节点、以T_a树扩展生成的新位形θ_new为T_b的目标位形点，初始化树T_b；

随机采样单元，用于从位形空间随机采样选取θ_rand；

搜索单元，用于在T_a搜索离θ_rand距离最近的位形节点在T_b中搜索离θ_rand距离最近的位形节点

扩展单元，用于T_a从向θ_rand扩展一个步长λ，T_b从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长λ；

获取路径解单元，用于当T_a和T_b最近节点之间的距离小于或等于λ时，则获取机器人避碰的路径。

所述设备还包括交换单元，所述交换单元具体用于：

当T_a和T_b最近节点之间的距离大于λ时，重新返回继续执行随机采样单元以随机选取θ_rand，交换T_a和T_b；然后执行搜索单元，在T_b搜索离θ_rand最近的位形节点T_b从向θ_rand扩展一个步长λ；在T_a中搜索离θ_rand最近的位形节点T_a从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长，并且将所述当前迭代次数增加1。

所述设备还包括最大迭代次数单元和路径规划结束单元，所述最大迭代次

数单元具体用于判断所述当前迭代次数是否大于最大迭代次数K，当所述当

前迭代次数大于最大迭代次数K时，则所述路径规划结束单元用于结束所

述机器人避碰的路径的获取。

所述扩展单元，具体用于：

在T_b中获取离θ_rand最近的一个位形节点θ_nearest；

θ_nearest向θ_rand移动一个步长λ获得新的位形节点θ_new；

检测所述新的位形节点θ_new是否与所述障碍物碰撞；

若所述新的位形节点θ_new与所述障碍物碰撞，则所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new与θ_rand的距离小于或者等于步长λ，则θ_new已到达θ_rand，所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new未与所述障碍物碰撞或者未到达θ_rand,则θ_new再向θ_rand扩展一个步长λ。

所述扩展单元中包括碰撞检测单元，所述碰撞检测单元具体用于：

将所述机器人在位形空间中的每个位形点表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

将在所述位形空间中的障碍物表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

若所述机器人与所述障碍物发生碰撞，则所述机器人的多个凸包和所述障碍物的多个凸包之间有碰撞。

本发明实施例提供一种用于机器人快速避碰的路径规划方法，本发明利用BBRrt算法和基于凸集理论的碰撞检测算法，同时从初始位形和目标位形交替向对方扩展，构成两棵平衡RRT，能够迅速地在位形空间寻找到一条连接起始位形到目标位形的无碰撞运动路径，有效通过狭窄通道，并充分利用多步扩展方法和带有启发性目的的平衡扩展方式，可以有效地改善RRT算法的收敛性和重复性，并从理论和仿真实验上证明了本发明提出算法的完备性；用凸集理论，提出一种基于凸包的快速碰撞检测算法，可以有效改进避碰路径规划的效率和性能。仿真结果表明，该算法在复杂障碍物环境下，既提高了算法的执行效率，同时保证了运动路径的相对稳定性和重复性。本发明所提出的算法可以推广到高维机器人多树RRT路径规划。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种用于机器人快速避碰的方法示意图；

图2是本发明实施例提供的一种用于机器人快速避碰的方法流程图；

图3是本发明实施例提供的一种机器人初始位形、目标位形和规划中间位形所展示的虚拟仿真环境示意图；

图4是本发明实施例提供的一种双向平衡RRT树；

图5是三次样条法优化后的动态规划路径示意图；

图6是局部放大观察双向平衡RRT树效果图；

图7是BBRrt规划算法获得的原始可行路径显示的机器人关节角时序图；

图8是三次样条法优化后获得的优化路径所表示的关节角时序图；

图9是本发明实施例提供的一种用于机器人快速避碰的设备结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参考图1，图1是本发明实施例提供的一种用于机器人快速避碰的方法示意图。如图1所示，BBRrt算法是正向RRT搜索和后向RRT搜索的组合。一棵树从初始位形开始生长，而另一棵树从目标位形开始生长。相对于单树RRT路径规划来说，利用双向RRT进行路径规划将更快捷、更有效。这是因为双向搜索树不只从初始点开始，而且还要以目标点为根，分别交替构造两棵快速扩展的搜索树，并通过判断两棵树之间是否存在交叉点，来确定可行路径的存在与否。对于双向平衡RRT来说，不仅要保证其快速扩展的特性，还必须考虑两棵生成树的交叉连接问题。另外，还要确保两棵生成树的平衡生长问题，只有这样才能保证两棵树在构造时间和空间上的一致性。

在基本的单树RRT算法中，当位形空间C中包含大量障碍物和狭窄通道约束，算法的效率会大幅下降，有必要在RRT算法的连接和采样等方面，进行改进，实现BBRrt算法，其原理图如图1所示。θ_start、θ_goal和θ_rand分别表示初始位形点、目标位形点和随机采样位形点，λ表示扩展步长，θ_nearest是RRT树中离θ_rand最近的位形节点，每棵RRT树都是一个拓扑图T=G(V,E)，图顶点V∈F表示随机采样的自由位形点，图中的边E表示连接相邻位形点的可行局部路径。T_a和T_b分别表示以θ_start和θ_goal为根节点的随机树。在路径规划的每次迭代过程中，都会有一个顶点V=θ_new和一条边E=(θ_nearest,θ_new)加入到拓扑图G中，构成新的随机树RRT。设为拓扑图G中所有能达到位形点的集合，因每个e_i∈E是一条路径，则 $\mathrm{Path}=\underset{e_i\∈E}{U}{e}_i\left(\right[\mathrm{0,1}\left]\right).$

参考图2，图2是本发明实施例提供的一种用于机器人快速避碰的方法流程图。如图2所示，所述方法包括以下关键步骤：

步骤201，确定起始位置θ_start、目标位置θ_goal、和单步扩展步长λ、当前迭代次数；

步骤202，以θ_start为快速随机搜索树RRTT_a的根节点、以θ_goal为目标位形点，初始化树T_a；以θ_goal为T_b的根节点、以T_a树扩展生成的新位形θ_new为T_b的目标位形点，初始化树T_b；

步骤203，在位形空间随机采样选取θ_rand，在T_a搜索离θ_rand距离最近的位形节点T_a从向θ_rand扩展一个步长λ；在T_b中搜索离θ_rand距离最近的位形节点T_b从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand之间距离小于所述步长λ；

具体的，平衡双向RRT算法的伪代码如下所示。

函数Result Expansion（）的功能主要负责完成双向RRT树交替迭代的每次扩展。首先调用RandConfiguration（）函数在位形空间C中均匀随机采样获得位形点θ_rand；然后调用Result Extend（）函数使T_a向θ_rand扩展一个步长λ；再调用Result Connect（）函数使T_b尽可能向θ_rand扩展多个步长λ。

使用函数Result Extend()使得T_a向θ_rand扩展一个步长λ，函数Result Extend()的功能主要负责在位形空间C中使T向θ_rand扩展一个步长λ。调用

NearestNeighbor()在T中寻找距离θ_rand最近的一个位形节点θ_nearest。调用

NewConfiguration()使θ_nearest向θ_rand移动一个步长λ获得θ_new：

${\mathrm{\θ}}_{\mathrm{New}}={\mathrm{\θ}}_{\mathrm{nearest}}+\frac{{\mathrm{\θ}}_{\mathrm{Rand}}-{\mathrm{\θ}}_{\mathrm{nearest}}}{\left|\right|{\mathrm{\θ}}_{\mathrm{Rand}}-{\mathrm{\θ}}_{\mathrm{nearest}}\left|\right|}\·\mathrm{\λ}$

函数Result Extend（）算法可以表示如下：

使用函数Result Connect（）使得T_b向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长λ。

函数Result Connect（）主要功能使T向θ_rand扩展多个步长λ，直到遇到障碍、到达θ_rand或与θ_rand的距离小于扩展步长λ。

函数Result Connect（）算法可以表示如下：

具体的，所述T_b向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长λ，包括：

在T_b中获取离θ_rand最近的一个位形节点θ_nearest；

θ_nearest向θ_rand移动一个步长λ获得新的位形节点θ_new；

检测所述新的位形节点θ_new是否与所述障碍物碰撞；

若所述新的位形节点θ_new与所述障碍物碰撞，则所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new与θ_rand的距离小于或者步长λ，则θ_new已到达θ_rand，所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new未与所述障碍物碰撞或者未到达θ_rand,则θ_new再向θ_rand扩展一个步长λ。

具体的，所述检测所述新的位形节点θ_new是否与所述障碍物碰撞，包括：

将所述机器人在位形空间中的每个位形点表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

将在所述位形空间中的障碍物表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

若所述机器人与所述障碍物发生碰撞，则所述机器人的多个凸包和所述障碍物的多个凸包之间有碰撞。

使用isCollision（(θ_new),n）检测新位形节点θ_new是否与障碍C_obs碰撞，若发生碰撞返回true,否则返回false。当θ_new与C_obs发生碰撞时，Extend（）返回Trapped，表示当前树节点θ_nearest不能再往前扩展一个步长λ，出现这种情况表明θ_nearest可能位于障碍C_obs或约束区域附近。当θ_new∈F时，则将其作为新位形节点其添加到T中，并将直线段（θ_nearest，θ_new）作为边添加到T中。若θ_new与θ_rand的距离测度小于步长λ，表明θ_new已达到θ_rand，则Extend（）返回Reached；若θ_new尚未到达θ_rand,则Extend（）返回Advanced，表明θ_new还可能向θ_rand扩展一个以上步长λ。

若令高维机器人空间中任意两个凸包分别命名为Δ_A和Δ_B,则可以用线性不等式方程组描述凸包，进而将任意两个凸包Δ_A和Δ_B是否发生碰撞的问题可归结为判定构成的集合H是否为空的问题。考虑到该类问题的普遍性和通用性，本专利针对n个变量的情况，将两个任意两个凸包Δ_A和Δ_B不等式组构成的集合可抽象成：从而可将判定笛卡尔空间中任意两个凸包Δ_A和Δ_B是否发生碰撞的问题，归结为判定所构成的凸集是否非空的问题。

令表示由任意两个凸包表示的不等式式构成的集合，n表示不等式组集合中的变量的个数(取值为大于等于零的整数),S_I表示自然数集合的子集，则碰撞检测方法isCollision的算法可以如下：

步骤204，当T_a和T_b最近节点之间的距离小于或等于λ时，则获取机器人避碰的路径。

BBRrt（）算法的输入参数包括：起始位形点θ_start、目标位形点θ_goal、随机搜索最大迭代次数K和单步扩展步长。首先，进行初始化T_a、T_b和path，分别以θ_start和θ_goal为根节点构造两棵树T_a和T_b。然后进入循环双向扩展Expansion（）交换Swap（）迭代过程：先调用RandConfiguration在位形空间C随机选取θ_rand；其次调用Extend使T_a向θ_rand扩展一个步长λ；再调用Connect（）函数使T_b尽可能向θ_rand扩展多个步长λ。如果T_a和T_b能够完全连接，则获得可行路径解Path(T_a,T_b)；否则，调用Swap()函数交换两棵树T_a和T_b，重新执行下次迭代，直至获得可行路径解，或者到达迭代最大次数K，搜索失败。

该算法引人的提高算法速度的机制包括：

（1）Connect()函数在每次迭代中比Extend()扩展更多个步长，从而树的扩展速度更快。

（2）通过构造两棵RRT树，使每棵树朝向对方交替扩展，因而不再是随机的扩展方式，而是带有启发性目的的扩展方式。

因此，BBRrt()算法的综合性能比传统单树RRT算法更高。尤其是当输入的一对起始和目标位形点θ_start和θ_goal均位于约束区域中时，两棵树可以通过朝向对方快速扩展而逃离这些区域，因而比单树RRT算法更具优势。

作为一种可选的实施例，所述方法还包括：

当T_a和T_b最近节点之间的距离大于λ时，则以θ_start为快速随机搜索树RRTT_b的根节点、以θ_goal为目标位形点，以θ_goal为T_a的根节点、以θ_start为目标位形点；

返回执行随机选取θ_rand，T_a向θ_rand扩展一个步长λ，T_b向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长，并将所述当前迭代次数增加1。

作为另一种可选的实施例，所述方法在当T_a和T_b最近节点之间的距离小于或等于λ时，则获取机器人避碰的路径之前，还包括：

判断当前迭代次数是否大于最大迭代次数K；

所述方法还包括：

若当前迭代次数大于最大迭代次数K，则无法确定所述机器人避碰的路径。

本发明实施例提供一种用于机器人快速避碰的路径规划方法，本发明利用BBRrt算法和基于凸集理论的碰撞检测算法，同时从初始位形和目标位形交替向对方扩展，构成两棵平衡RRT，能够迅速地在位形空间寻找到一条连接起始位形到目标位形的无碰撞运动路径，有效通过狭窄通道，并充分利用多步扩展函数和带有启发性目的的平衡扩展方式，可以有效地改善RRT算法的收敛性和重复性，并从理论和仿真实验上证明了本发明所提出算法的完备性；基于凸集理论，提出一种基于凸包的快速碰撞检测算法，可以有效改进避碰路径规划的效率和性能。仿真结果表明，该算法在复杂障碍物环境下，既提高了算法的执行效率，同时保证了运动路径的相对稳定性和重复性。本发明所提出的算法可以推广到高维机器人多树RRT路径规划。

设BBRrt算法的随机扩展采样步长λ为0.04,最大采样数K=50000,位形点随机采样概率Prob＝0.1；KUKA机器人起始位形θ_start=[83.62，-57.27，90.64，-11.48，-33.90，-80.43]T,目标位形θ_goal=[-84.73，-41.67，91.55，0.00，40.12，264.73]T，从起始位形θ_start到目标位形θ_goal，避免与存在多个障碍物和狭窄通道的复杂环境发生碰撞，采用本发明提出避碰BBRrt规划方法的仿真结果如图3-8所示。图3主要表现机器人初始位形、目标位形和规划中间位形所展示的虚拟仿真环境。图4显示利用本发明提出算法所生成的双向平衡RRT树。图5是采用三次样条法优化后的动态规划过程。图6是局部放大观察双向平衡RRT树效果图。图7是采用BBRrt规划算法获得的原始可行路径显示的机器人关节角时序历程。图8是采用三次样条法优化后获得的优化路径所表示的关节角时序历程。本发明提出算法与基本RRT算法、RRT-Connect算法进行了对比，结果如表1所示。根据规划算法的不同问题规模对本发明提出的碰撞检测算法所测试的结果如表2所示。

表1

表2

从大量的计算仿真结果和算法关键性能指标(运行时间、总节点数和碰撞检测次数)来看,本发明提出的BBRrt算法与现有的典型算法相比,收敛速度(运行时间)和稳定性得到显著提高,同时验证了基于凸集理论的碰撞检测算法的可靠性和高效性。

参考图9，图9是本发明实施例提供的一种用于机器人快速避碰的设备结构图。如图9所示，所述设备包括：

初始化单元901，用于确定起始位形θ_start、目标位形θ_goal、和单步扩展步长λ、当前迭代次数；

以θ_start为快速随机搜索树RRTT_a的根节点、以θ_goal为目标位形点，以θ_goal为T_b的根节点、以θ_start为目标位形点；

随机采样单元902，用于从位形空间随机采样选取θ_rand；

搜索单元903，用于在T_a搜索离θ_rand距离最近的位形节点在T_b中搜索离θ_rand距离最近的位形节点

扩展单元904，用于T_a从向θ_rand扩展一个步长λ，T_b从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长λ；

其中，平衡双向RRT算法、函数Result Expansion（）、函数Result Extend()、函数Result Connect（）的算法描述可以参考方法实施例的描述，在此不再赘述。

具体的，所述扩展单元904具体用于：

在T_b中获取离θ_rand最近的一个位形节点θ_nearest；

θ_nearest向θ_rand移动一个步长λ获得新的位形节点θ_new；

检测所述新的位形节点θ_new是否与所述障碍物碰撞；

若所述新的位形节点θ_new与所述障碍物碰撞，则所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new与θ_rand的距离小于或者步长λ，则θ_new已到达θ_rand，所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new未与所述障碍物碰撞或者未到达θ_rand,则θ_new再向θ_rand扩展一个步长λ。

具体的，所述扩展单元904中包括碰撞检测单元904a，所述碰撞检测单元904a具体用于：

将所述机器人在位形空间中的每个位形点表示为笛卡尔空间的由线性不等式方式组构成的多个凸包集合；

将在所述位形空间中的障碍物表示为笛卡尔空间的由线性不等式方式组构成的多个凸包集合；

若所述机器人与所述障碍物发生碰撞，则所述机器人的多个凸包和所述障碍物的多个凸包之间有碰撞。

获取路径解单元905，用于当T_a和T_b最近节点之间的距离小于或等于λ并且当前迭代次数小于或者等于最大迭代次数K时，则获取机器人避碰的路径。

BBRrt（）算法的输入参数包括：起始位形点θ_start、目标位形点θ_goal、随机搜索最大迭代次数K和单步扩展步长。首先，进行初始化T_a、T_b和path，分别以θ_start和θ_goal为根节点构造两棵树T_a和T_b。然后进入循环双向扩展Expansion（）交换Swap（）迭代过程：先调用RandConfiguration在位形空间C随机选取θ_rand；其次调用Extend使T_a向θ_rand扩展一个步长λ；再调用Connect（）函数使T_b尽可能向θ_rand扩展多个步长λ。如果T_a和T_b能够完全连接，则获得可行路径解Path(T_a,T_b)；否则，调用Swap()函数交换两棵树T_a和T_b，重新执行下次迭代，直至获得可行路径解，或者到达迭代最大次数K，搜索失败。

该算法引人的提高算法速度的机制包括：

（1）Connect()函数在每次迭代中比Extend()扩展更多个步长，从而树的扩展速度更快。

（2）通过构造两棵RRT树，使每棵树朝向对方交替扩展，因而不再是随机的扩展方式，而是带有启发性目的的扩展方式。

因此，BBRrt()算法的综合性能比传统单树RRT算法更高。尤其是当输入的一对起始和目标位形点θ_start和θ_goal均位于约束区域中时，两棵树可以通过朝向对方快速扩展而逃离这些区域，因而比单树RRT算法更具优势。

作为一种可选的实施例，所述设备还包括交换单元906，所述交换单元906具体用于：

当T_a和T_b最近节点之间的距离大于λ时，则以θ_start为快速随机搜索树RRTT_b的根节点、以θ_goal为目标位形点，以θ_goal为T_a的根节点、以θ_start为目标位形点；

返回执行随机选取θ_rand，T_a向θ_rand扩展一个步长λ，T_b向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长，并且将所述当前迭代次数增加1。

作为另一种可选的实施例，所述设备还包括最大迭代次数单元907和路径规划结束单元908，所述最大迭代次数单元907具体用于判断所述当前迭代次数是否大于最大迭代次数K，当所述当前迭代次数大于最大迭代次数K时，则所述路径规划结束单元908用于结束所述机器人避碰的路径的获取。

本发明实施例提供一种用于机器人快速避碰的路径规划设备，本发明利用BBRrt算法和基于凸集理论的碰撞检测算法，同时从初始位形和目标位形交替向对方扩展，构成两棵平衡RRT，能够迅速地在位形空间寻找到一条连接起始位形到目标位形的无碰撞运动路径，有效通过狭窄通道，并充分利用多步扩展函数和带有启发性目的的平衡扩展方式，可以有效地改善RRT算法的收敛性和重复性，并从理论和仿真实验上证明了本发明所提出算法的完备性；基于凸集理论，提出一种基于凸包的快速碰撞检测算法，可以有效改进避碰路径规划的效率和性能。仿真结果表明，该算法在复杂障碍物环境下，既提高了算法的执行效率，同时保证了运动路径的相对稳定性和重复性。本发明所提出的算法可以推广到高维机器人多树RRT路径规划。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (10)

1.一种用于机器人快速避碰的路径规划方法，其特征在于，所述方法包括：

确定起始位形θ_start、目标位形θ_goal、和单步扩展步长λ、当前迭代次数；

以θ_start为快速随机搜索树RRTT_a的根节点、以θ_goal为T_a的目标位形点，初始化树T_a；以θ_goal为T_b的根节点、以T_a树扩展生成的新位形θ_new为T_b的目标位形点，初始化树T_b；

在位形空间随机采样选取θ_rand，在T_a搜索离θ_rand距离最近的位形节点T_a从向θ_rand扩展一个步长λ；在T_b中搜索离θ_rand距离最近的位形节点T_b从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand之间距离小于所述步长λ；

当T_a和T_b最近节点之间的距离小于或等于λ时，则获取机器人避碰的路径。

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

当T_a和T_b最近节点之间的距离大于λ时，则交换T_a和T_b，继续在位形空间随机采样选择θ_rand，在T_b搜索离θ_rand距离最近的位形节点T_b从向θ_rand扩展一个步长λ；在T_a中搜索离θ_rand距离最近的位形节点T_a从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand之间距离小于所述步长λ，并将所述当前迭代次数增加1。

3.根据权利要求2所述的方法，其特征在于，所述方法在当T_a和T_b最近节点之间的距离小于或等于λ时，则获取机器人避碰的路径之前，还包括：

判断当前迭代次数是否大于最大迭代次数K；

所述方法还包括：

若当前迭代次数大于最大迭代次数K，则无法确定所述机器人避碰的路径。

4.根据权利要求3所述的方法，其特征在于，所述T_b向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长λ，包括：

在T_b中获取离θ_rand最近的一个位形节点θ_nearest；

θ_nearest向θ_rand移动一个步长λ获得新的位形节点θ_new；

检测所述新的位形节点θ_new是否与所述障碍物碰撞；

若所述新的位形节点θ_new与所述障碍物碰撞，则所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new与θ_rand的距离小于或等于步长λ，则θ_new已到达θ_rand，所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new未与所述障碍物碰撞或者未到达θ_rand,则θ_new再向θ_rand扩展一个步长λ。

5.根据权利要求4所述的方法，其特征在于，所述检测所述新的位形节点θ_new是否与所述障碍物碰撞，包括：

将所述机器人在位形空间中的每个位形点表示为笛卡尔空间的由线性不等式方式组所构成多个凸包集合；

将在所述位形空间中的障碍物表示为笛卡尔空间的由线性不等式方式组所构成多个凸包集合；

若所述机器人与所述障碍物发生碰撞，则所述机器人的多个凸包和所述障碍物的多个凸包之间有碰撞。

6.一种用于机器人快速避碰的设备，其特征在于，所述设备包括：

初始化单元，用于初始化起始位形θ_start、目标位形θ_goal和单步扩展步长λ和当前迭代次数；以θ_start为快速随机搜索树RRTT_a的根节点、以θ_goal为目标位形点，初始化树T_a；以θ_goal为T_b的根节点、以T_a树扩展生成的新位形θ_new为T_b的目标位形点，初始化树T_b；

随机采样单元，用于从位形空间随机采样选取θ_rand；

搜索单元，用于在T_a搜索离θ_rand距离最近的位形节点在T_b中搜索离θ_rand距离最近的位形节点

扩展单元，用于T_a从向θ_rand扩展一个步长λ，T_b从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长λ；

获取路径解单元，用于当T_a和T_b最近节点之间的距离小于或等于λ并且当前迭代次数小于或者等于最大迭代次数K时，则获取机器人避碰的路径。

7.根据权利要求6所述的设备，其特征在于，所述设备还包括交换单元，所述交换单元具体用于：

当T_a和T_b最近节点之间的距离大于λ时，重新返回继续执行随机采样单元以随机选取θ_rand，交换T_a和T_b；然后执行搜索单元，在T_b搜索离θ_rand最近的位形节点T_b从向θ_rand扩展一个步长λ；在T_a中搜索离θ_rand最近的位形节点T_a从向θ_rand扩展多个步长λ直至遇到障碍物或者到达θ_rand或者与θ_rand小于所述步长，并且将所述当前迭代次数增加1。

8.根据权利要求6或7所述的设备，其特征在于，所述设备还包括最大迭代次数单元和路径规划结束单元，所述最大迭代次数单元具体用于判断所述当前迭代次数是否大于最大迭代次数K，当所述当前迭代次数大于最大迭代次数K时，则所述路径规划结束单元用于结束所述机器人避碰的路径的获取。

9.根据权利要求8所述的设备，其特征在于，所述扩展单元，具体用于：

在T_b中获取离θ_rand最近的一个位形节点θ_nearest；

θ_nearest向θ_rand移动一个步长λ获得新的位形节点θ_new；

检测所述新的位形节点θ_new是否与所述障碍物碰撞；

若所述新的位形节点θ_new与所述障碍物碰撞，则所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new与θ_rand的距离小于或者等于步长λ，则θ_new已到达θ_rand，所述新的位形节点θ_new不能向θ_rand再扩展一个步长λ；或者，

若θ_new未与所述障碍物碰撞或者未到达θ_rand,则θ_new再向θ_rand扩展一个步长λ。

10.根据权利要求9所述的设备，其特征在于，所述扩展单元中包括碰撞检测单元，所述碰撞检测单元具体用于：

将所述机器人在位形空间中的每个位形点表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

将在所述位形空间中的障碍物表示为笛卡尔空间的由线性不等式方式组所构成的多个凸包集合；

若所述机器人与所述障碍物发生碰撞，则所述机器人的多个凸包和所述障碍物的多个凸包之间有碰撞。