CN104154917B - 一种机器人避碰路径的规划方法、装置 - Google Patents

Abstract

本发明适用于机器人避碰和操控领域，提供了一种机器人避碰路径的规划方法、装置，所述方法包括：先根据机器人初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；再根据q_s和q_g构造双向快速密集搜索树，并设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；然后根据q_s、q_g生成一条机器人规划路径；最后对机器人规划路径上的离散采样位形点分别进行碰撞检测，如果有离散位形点与障碍物发生碰撞，则更新r_col的值为r_col/2；并根据该更新后的r_col生成新的机器人规划路径，直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。本发明，若机器人与障碍物发生碰撞会自适应将碰撞检测距离参数r_col减半，以便于成功通过狭窄通道或障碍物密集空间。

Description

一种机器人避碰路径的规划方法、装置

技术领域

本发明属于机器人避碰和操控领域，尤其涉及一种机器人避碰路径的规划方法、装置。

背景技术

目前，基于采样的规划技术已被广泛应用于高自由度智能机器人避碰和操控等领域，成为国内外普遍关注的研究热点。该方法的显著优点是基于随机采样只需近似构造自由位形空间，从而避免了采用骨架网格和栅格分解等方法需完整和精确构造全部有效C-空间模型，以寻找精确解析所带来的PSPACE难题，规避了高自由度所引起的存储空间和计算量呈指数爆炸增长的NP问题。最典型的两类基于采样的运动规划方法为：快速随机搜索树法和概率地图法。它们均利用采样技术在自由C-空间中构造树或图，然后迭代细化，直到找到连接初始位形和目标位形的无碰撞路径解。然而，该类方法的有效性，尤其在处理高维空间和复杂障碍环境等复杂问题时，严重依赖于所选的采样参数和离散碰撞检测距离参数。

发明内容

本发明实施例提供了一种机器人避碰路径的规划方法、装置，旨在解决现有技术在处理高维空间和复杂障碍环境等复杂问题时，严重依赖于所选的采样参数和离散碰撞检测距离参数的问题。

一方面，提供一种机器人避碰路径的规划方法，所述方法包括：

步骤1、根据机器人的初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；

步骤2、根据所述初始位形q_s、目标位形q_g构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树；

步骤3、设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；

步骤4、根据所述初始位形q_s、目标位形q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；

步骤5、对所述机器人规划路径上的离散采样位形点分别进行碰撞检测，如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则执行步骤6；

步骤6、更新r_col的值为r_col/2；

步骤7、循环执行步骤3至6，直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

进一步地，所述步骤4包括：

根据所述初始位形q_s、目标位形q_g计算得到扩展步长的初始值r_ext；

根据所述初始位形q_s、目标位形q_g和所述扩展步长的初始值r_ext配置所述双向快速密集搜索树；

局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径。

进一步地，所述局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径包括：

从位形空间随机采样获得一个随机采样位形点q_rand；

搜索所述正向快速密集搜索树上与所述随机采样位形q_rand最近邻的边e₁；

在边e₁上搜索与所述随机采样位形q_rand最近邻的位形点

根据所述q_rand、以及所述扩展步长计算得到不与障碍物发生碰撞的新位形点q_new；

根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则搜索所述逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂；

在边e₂上搜索与所述新位形点q_new最近邻的位形点

根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

若连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞时，将所述边e₂、所述最近邻的位形点和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长加倍；

若连接所述最近邻的位形点和所述新位形点q_new之间的路径与障碍物发生碰撞时，计算机器人从向q_new移动而不与障碍物发生碰撞的最合适的位形点，将所述最适合的位形点、所述边e₂和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长减半，直至连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞。

进一步地，利用加权采样方法，从位形空间随机采样获得一个随机采样位形点q_rand。

另一方面，提供一种机器人避碰路径的规划装置，所述装置包括：

碰撞检测距离初始化单元，用于根据机器人的初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；

搜索树构建单元，用于根据所述初始位形q_s、目标位形q_g构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树；

碰撞检测距离设置单元，用于设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；

规划路径生成单元，用于根据所述初始位形q_s、目标位形q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；

碰撞检测单元，用于对所述机器人规划路径上的离散采样位形点分别进行碰撞检测；

碰撞检测距离更新单元，用于如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则更新r_col的值为r_col/2；

规划路径更新单元，用于依次循环调用规划路径生成单元、规划路径生成单元、碰撞检测单元和碰撞检测距离更新单元，直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

进一步地，所述规划路径生成单元包括：

扩展步长初始化模块，用于根据所述初始位形q_s、目标位形q_g计算得到扩展步长的初始值r_ext；

搜索树配置模块，用于根据所述初始位形q_s、目标位形q_g和所述扩展步长的初始值r_ext配置所述双向快速密集搜索树；

扩展步长调整模块，用于局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径。

进一步地，所述扩展步长调整模块包括：

随机采样位形获取子模块，用于从位形空间随机采样获得一个随机采样位形点q_rand；

第一最近邻边获取子模块，用于搜索所述正向快速密集搜索树上与所述随机采样位形q_rand最近邻的边e₁；

第一最近邻位形点获取子模块，用于在边e₁上搜索与所述随机采样位形q_rand最近邻的位形点

新位形点获取子模块，用于根据所述q_rand、以及所述扩展步长计算得到不与障碍物发生碰撞的新位形点q_new；

第一碰撞检测子模块，用于根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

第二最近邻边获取子模块，用于如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则搜索所述逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂；

第二最近邻位形点获取子模块，用于在边e₂上搜索与所述新位形点q_new最近邻的位形点

第二碰撞检测子模块，用于根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

扩展步长加倍子模块，用于若连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞时，将所述边e₂、所述最近邻的位形点和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长加倍；

扩展步长减半子模块，用于若连接所述最近邻的位形点和所述新位形点q_new之间的路径与障碍物发生碰撞时，计算机器人从向q_new移动而不与障碍物发生碰撞的最合适的位形点，将所述最适合的位形点、所述边e₂和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长减半，直至连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞。

进一步地，所述随机采样位形获取子模块利用加权采样方法，从位形空间随机采样获得一个随机采样位形点q_rand。

在本发明实施例，先根据机器人初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；再根据q_s和q_g构造双向快速密集搜索树，并设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；然后根据q_s、q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；最后对所述机器人规划路径上的离散采样位形点分别进行碰撞检测，如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则更新r_col的值为r_col/2；并根据该更新后的r_col生成新的机器人规划路径，直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。实现了若机器人与障碍物发生碰撞会自适应将碰撞检测距离参数r_col减半，以便于成功通过狭窄通道或障碍物密集空间。

附图说明

图1是本发明实施例一提供的机器人避碰路径的规划方法的实现流程图；

图2是本发明实施例一提供的通过BiRDT对象调用自适应双向快速密集搜索树动态扩展方法ABiRDTDExtend(q_s,q_g)来生成一条机器人规划路径的实现流程图；

图3是本发明实施例一提供的通过局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径的实现流程图；

图4是本发明实施例一提供的快速密集搜索树从随机采样位形q_rand向目标位形q_g扩展，获得不与障碍物发生碰撞的新位形q_new的过程示意图；

图5是本发明实施例一提供的RDTExtend(T,q_rand，q_new)的实现流程图；

图6是本发明实施例一提供的构建快速密集搜索树（RDT）的新边时，扩展步长减半的过程示意图；

图7是本发明实施例一提供的RDTConnect(T,q)算法的实现流程图；

图8是本发明实施例二提供的机器人避碰路径的规划装置的结构框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

在本发明实施例中，先根据机器人初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；再根据q_s和q_g构造双向快速密集搜索树，并设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；然后根据q_s、q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；最后对所述机器人规划路径上的离散采样位形点分别进行碰撞检测，如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则更新r_col的值为r_col/2；并根据该更新后的r_col生成新的机器人规划路径，直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

以下结合具体实施例对本发明的实现进行详细描述：

实施例一

图1示出了本发明实施例一提供的机器人避碰路径的规划方法的实现流程，详述如下：

在步骤S101中，根据机器人的初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col。

本发明实施例中，先初始化机器人的初始位形q_s和目标位形q_g，再根据初始位形q_s和目标位形q_g计算得到碰撞检测距离的初始值r_col，其中，计算得到的r_col满足：

r_col=||q_g-q_s||。

在步骤S102中，根据所述初始位形q_s、目标位形q_g构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树。

在本发明实施例中，构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树T_a和逆向快速密集搜索树T_b，T_a是从初始位形q_s开始，递增地构造的正向快速密集搜索树，T_b是从目标位形q_g开始，递增地构造的逆向快速密集搜索树。

其中，具体实现时，双向快速密集搜索树以CBiRDT(q_s,q_g)对象实例BiRDT对象表示，后续在介绍具体实现时，都以BiRDT对象来进行说明，不再赘述。

在步骤S103中，设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col。

在本发明实施例中，BiRDT对象调用SetColPara(r_col)方法设置双向快速密集搜索树的碰撞检测距离参数的值为r_col。

在步骤S104中，根据所述初始位形q_s、目标位形q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径。

本发明实施例中，获得机器人的初始位形q_s、目标位形q_g后，可以通过BiRDT对象调用自适应双向快速密集搜索树动态扩展方法ABiRDTDExtend(q_s,q_g)来生成一条机器人规划路径，具体实现包括的步骤如图2所示，详述如下：

步骤1、根据机器人初始位形q_s、目标位形q_g计算得到扩展步长的初始值r_ext。

本步骤中，根据目标位形q_g和初始位形q_s，计算后续将要构造的双向快速密集搜索树的扩展步长的初始值r_ext，其中r_ext满足：

r_ext=||q_g-q_s||。

扩展步长初始值的计算采用位形空间距离测度方法，即是： $r_{\mathrm{ext}}=\left|\right|q_g-q_s\left|\right|=\sqrt{{\mathrm{\Σ}}_{i=1}^n{\left(q_{g_i-q_{\mathrm{si}}}\right)}^2},$ 其中，n为位形空间的维度。

步骤2、根据初始位形q_s、目标位形q_g和扩展步长的初始值r_ext配置所述双向快速密集搜索树。

本步骤中，可以根据Config函数配置正向快速密集搜索树和逆向快速密集搜索树的参数，具体通过Ta.Config(q_s,r_ext)和Tb.Config(q_g,r_ext)函数配置正向快速密集搜索树和逆向快速密集搜索树的参数，其中双向快速密集搜索树的每个节点中均存储有扩展步长的初始值r_ext。

步骤3、局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径。

本步骤中，BiRDT对象通过调用自适应双向快速密集树动态扩展方法ABiRDTDExtend(q_s,q_g)中的RDTExtend(T,q_rand,q_new)和RDTConnect(T,q_new)方法扩展双向搜索树，通过局部自适应调整存储在双向搜索树每个节点中存储的扩展步长的初始值r_ext，能够处理狭窄通道运动规划问题。通过调用RDTExtend(T_a,q_rand,q_new)和RDTConnect(T_a,q_new)方法处理完狭窄通道运动规划问题后，BiRDT对象可以调用Execute()方法，执行自适应双向快速密集树动态扩展方法，获得一条局部可行路径Path。

其中，步骤3的实现流程如图3所示，具体包括以下步骤：

步骤11、利用加权采样方法，从位形空间随机采样获得一个随机采样位形点q_rand。

本步骤中，通过调用函数q_rand=RandConfig()从位形空间随机采样获得一个随机采样位形点q_rand。

其中，RandConfig()法主要处理C-空间随机采样，本发明实施例采用加权采样方法，生成分辨率均匀的采样，以消除C-空间不同维度在工作空间对机器人影响的差异性。为了在C-空间路径两个位形点q₁和q₂之间进行加权均匀采样，其单位步长矢量计算公式如(1)式：

v_step(q₁,q₂)=(q₂-q₁)/ρ_ws(q₂-q₁) (1)

其中，ρ_ws(q_c)计算C-空间路径的最大工作空间位移量，其公式如（2）式；

${\mathrm{\ρ}}_{\mathrm{ws}}\left(q_c\right)={\mathrm{\Σ}}_{i=0}^{n-1}{w}_i{q}_{c_i}---\left(2\right)$

在(2)式中，权重系数w_i可通过求解从机器人第i个关节开始的所有运动链K的最大距离。对运动链K，属于K所有关节k∈K在工作空间的最大扩展是通过查找在相应表面S_k上两点和之间最大距离和获得，其计算公式如(3)式：

$w_i=\underset{K}{\max }\left({\mathrm{\Σ}}_{k\∈K}\underset{q_{c_i},q_{c_j}\∈S_k}{\max }\right|q_{c_i}-q_{c_j}\left|\right)---\left(3\right)$

因此，在两个位形q₁和q₂之间的路径上可均匀产生m=[ρ_ws/r_ext]-1个中间采样位形q_t，其计算公式如(4)式：

q_t=q₁+kr_extv_step(q₁,q₂),t=(1,...,m) (4)

若将w_i设置为机器人工作空间位移的最大上限，则简化计算，从而避免依赖人工设置归一化C-空间维度参数，提高C-空间采样的自适应性。

步骤12、搜索所述正向快速密集搜索树上与所述随机采样位形q_rand最近邻的边e1。

本步骤中，通过NNeighborEdge(T,q)函数来搜索正向快速密集搜索树上与所述随机采样位形q_rand最近邻的边e₁。

步骤13、在边e₁上搜索与所述随机采样位形q_rand最近邻的位形点

步骤14、根据所述q_rand、以及所述扩展步长的初始值r_ext计算得到不与障碍物发生碰撞的新位形点q_new。

步骤15、根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测。

步骤16、如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则搜索所述逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂。

步骤17、在边e₂上搜索与所述新位形点q_new最近邻的位形点

步骤18、根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测，如果发生碰撞，执行步骤19，否则，执行步骤20。

步骤19、如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则将所述边e₂、所述最近邻的位形点和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长加倍。

步骤20、否则，计算机器人从向q_new移动而不与障碍物发生碰撞的最合适的位形点，将所述最适合的位形点、所述边e₂和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长减半，直至连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞。

其中，RDTExtend(T,q_rand,q_new)可以使得正向快速密集搜索树T_a从随机采样位形q_rand向目标位形q_g扩展，获得不与障碍物发生碰撞的新位形q_new，也可以使得逆向快速密集搜索树T_b从随机采样位形q_rand向目标位形q_s扩展，获得不与障碍物发生碰撞的新位形q_new并返回扩展状态Res_E，若扩展成功，则Res_E返回TURE，否则返回FALSE；RDTConnect(T,q_new)可用于搜索逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂，其中，也可用于搜索正向快速密集搜索树上与所述新位形点q_new最近邻的边e₁并返回扩展状态Res_C，若扩展成功，则Res_C返回TURE，否则返回FALSE；如果Res_E和Res_C均扩展成功，则获得一条局部可行路径Path；否则，为了保证扩展的平衡性，通过函数Swap（T_a，T_b）交换正向快速密集搜索树和逆向快速密集搜索树，然后再调用RDTExtend(T,q_rand,q_new)和RDTConnect(T,q_new)，直至Res_E和Res_C均扩展成功为止。

具体的，RDTExtend(T,q_rand,q_new)的基本思路是：确定在边e上与随机采样位形q_rand的最近邻位形q_NN，然后通过考虑边e的局部扩展步长r_ext，将扩展生成的新位形q_new，限制在r_ext以内(如图4所示)。图中黑色的q_rand情况表示，当随机采样位形q_rand位于扩展步长r_ext以内，则q_new=q_rand；灰色q_rand情况表示，当随机采样位形q_rand位于扩展步长r_ext以外，则需要裁减为：q_new=q_NN+(q_rand-q_NN)r_ext|q_rand-q_NN|。具体的实现流程如图5所示，详述如下：

步骤21：搜索正向快速密集搜索树T中与位形点q_rand最近邻的边e=NNeighborEdge(T,q_rand)，即是遍历搜索RDT树T中的每条边，求每条边与位形点q_rand的距离最短的作为最近邻边e。

步骤22：在RDT树的边e上寻找与位形点q_rand最近邻的位形：q_NN=NNeighborOnEdge(T,e,q_rand)，即是求位形点q_rand与最近邻边e的垂足。

步骤23:当r_ext≥|q_rand-q_NN|时，q_new=q_rand；当r_ext<|q_rand-q_NN|时，q_new=q_NN+(q_rand-q_NN)r_ext|q_rand-q_NN|。即是说：计算新的位形点，当r_ext≥|q_rand-q_NN|(最近q_NN与随机采样位形q_rand之间的距离小于等于r_ext邻的位形)时，q_new=q_rand；其他，即是当r_ext<|q_rand-q_NN|（最近q_NN与随机采样位形q_rand之间的距离大于r_ext的位形），q_new=q_NN+(q_rand-q_NN)r_ext|q_rand-q_NN|（裁剪超过的部分，使其约束在边界上，详见图4）。

步骤24：调用isCollision(q_NN,q_new)做碰撞检测，判断连接位形点q_NN和q_new之间的路径是否与障碍物发生碰撞；若发生碰撞，则输出false；否则调用RDTConnect(T,q_new)搜索逆向快速密集搜索树上的新边。

具体的，RDTConnect(T,q)主要用于构建快速密集搜索树(RDT)的新边。如果在位形q_NN和q之间的直线段路径是避碰的，则新边被添加到RDT中，且与边e对应的扩展步长r_ext加倍。否则，在从q_NN到q路径上最大有效位形的边将被添加到树中，且将扩展步长r_ext减半。该方法的扩展步长减半的示意图如图6所示。

RDTConnect(T,q)算法的流程如图7所示，具体步骤描述如下：

步骤31：搜索RDT上与位形q最近邻的边e=NNeighborEdge(T,q)；

步骤32:在RDT树的边e上寻找与位形点q最近邻的位形：q_NN=NNeighborOnEdge(T,e,q);

步骤33:调用isCollision(q_NN,q)判定从q_NN和q的路径是否与障碍物发生碰撞；如发生碰撞，则转步骤34；若不发生碰撞，则转步骤37；

步骤34：增加最有效位形点:q_new=GetMValidConfig(q_NN,q)；

步骤35：增加新的扩展边：e_new=AddEdge(T,e,q_NN,q_new)

步骤36：扩展步长减半：e.q₁.r_ext=e.q₁.r_ext/2；e.q₂.r_ext=e.q₂.r_ext/2；e_new.q₁.r_ext=r_ext/2；e_new.q₂.r_ext=r_ext/2，输出false，转步骤39；

步骤37：向RDT树增加新边和位形点e_new=AddEdge(T,e,q_NN,q)；

步骤38：扩展步长加倍：e.q₁.r_ext=2*e.q₁.r_ext；e.q₂.r_ext=2*e.q₂.r_ext；e_new.q₁.r_ext=2*r_ext；e_new.q₂.r_ext=2*r_ext；输出true。

步骤39：RDTConnect(T,q)结束。

具体的，函数isCollision(q_NN,q)用于判定从q_NN和q的路径是否与障碍物发生碰撞，isCollision(q_NN,q)利用方向包围盒（OBB）的紧密性和球状包围盒计算简单的优点来构造机器人和环境的混合包围体层次树结构，快速排除不相交的对象，并利用OpenMP模型并行遍历混合包围体层次树，从而加快碰撞检测速度。该方法的主要实现步骤如下：

步骤41：预处理，构建基于方向包围盒和球状包围盒的混合包围盒层次树。

步骤42：先采用基于OpenMP的混合包围盒并行相交检测进行粗略相交检测。

步骤43：判断混合包围盒间存在相交，若不相交，则表明机器人和障碍物之间不发生碰撞；输出false，转步骤45；若相交，则转步骤44。

步骤44：精确相交检测，基于OpenMP相交包围盒所对应的三角形间的相交检测，若相交，则判定机器人与障碍物发生碰撞，输出true;否则，判定机器人不与障碍物发生碰撞，输出false;转步骤45。

步骤45：碰撞检测算法结束。

在步骤S105中，对所述机器人规划路径上的离散采样位形点分别进行碰撞检测，如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则执行步骤S106。

在步骤S106中，更新r_col的值为r_col/2。

在步骤S107中，循环执行步骤S103至S106，直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

本实施例，先根据机器人初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；再根据q_s和q_g构造双向快速密集搜索树，并设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；然后根据q_s、q_g采用自适应双向快速密集搜索树扩展方法生成一条机器人规划路径；最后对所述机器人规划路径上的离散采样位形点分别进行碰撞检测，如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则更新r_col的值为r_col/2；并根据该更新后的r_col生成新的机器人规划路径，直至生成的新的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。实现了若机器人与障碍物发生碰撞会自适应将碰撞检测距离参数r_col减半，以便于成功通过狭窄通道或障碍物密集空间。

另外，在生成机器人规划路径的过程中，可以扩展搜索树，通过局部自适应调整存储在搜索树每个节点的扩展参数r_ext，能够处理狭窄通道运动规划问题。比如，若扩展搜索树失败，则该r_ext减半；若扩展成功，则该r_ext加倍，从而对环境中的障碍物做出条件反射，自适应调整扩大或减小扩展步长，在障碍物较少的空间加快扩展速度，在狭窄通道或障碍物较多的地方，采用较小扩展步长，确保不与障碍物发生碰撞。

本领域普通技术人员可以理解实现上述各实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，相应的程序可以存储于一计算机可读取存储介质中，所述的存储介质，如ROM/RAM、磁盘或光盘等。

实施例二

图8示出了本发明实施例二提供的机器人避碰路径的规划装置的具体结构框图，为了便于说明，仅示出了与本发明实施例相关的部分。该装置8包括：碰撞检测距离初始化单元41、搜索树构建单元42、碰撞检测距离设置单元43、规划路径生成单元44、碰撞检测单元45、碰撞检测距离更新单元46和规划路径更新单元47。

其中，碰撞检测距离初始化单元41，用于根据机器人的初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；

搜索树构建单元42，用于根据所述初始位形q_s、目标位形q_g构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树；

碰撞检测距离设置单元43，用于设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；

规划路径生成单元44，用于根据所述初始位形q_s、目标位形q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；

碰撞检测单元45，用于对所述机器人规划路径上的离散采样位形点分别进行碰撞检测；

碰撞检测距离更新单元46，用于如果所有离散位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散位形点与障碍物发生碰撞，则更新r_col的值为r_col/2；

规划路径更新单元47，用于依次循环调用规划路径生成单元、规划路径生成单元、碰撞检测单元和碰撞检测距离更新单元，直至生成的机器人规划路径上的所有离散位形点均没有与障碍物发生碰撞为止。

进一步地，所述规划路径生成单元44包括：

扩展步长初始化模块，用于根据所述初始位形q_s、目标位形q_g计算得到扩展步长的初始值r_ext；

搜索树配置模块，用于根据所述初始位形q_s、目标位形q_g和所述扩展步长的初始值r_ext配置所述双向快速密集搜索树；

扩展步长调整模块，用于局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径。

进一步地，所述扩展步长调整模块包括：

随机采样位形获取子模块，用于从位形空间随机采样获得一个随机采样位形点q_rand；

第一最近邻边获取子模块，用于搜索所述正向快速密集搜索树上与所述随机采样位形q_rand最近邻的边e₁；

第一最近邻位形点获取子模块，用于在边e₁上搜索与所述随机采样位形q_rand最近邻的位形点

新位形点获取子模块，用于根据所述q_rand、以及所述扩展步长计算得到不与障碍物发生碰撞的新位形点q_new；

第一碰撞检测子模块，用于根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

第二最近邻边获取子模块，用于如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则搜索所述逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂；

第二最近邻位形点获取子模块，用于在边e₂上搜索与所述新位形点q_new最近邻的位形点

第二碰撞检测子模块，用于根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

扩展步长加倍子模块，用于若连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞时，将所述边e₂、所述最近邻的位形点和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长加倍；

扩展步长减半子模块，用于若连接所述最近邻的位形点和所述新位形点q_new之间的路径与障碍物发生碰撞时，计算机器人从向q_new移动而不与障碍物发生碰撞的最合适的位形点，将所述最适合的位形点、所述边e₂和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长减半，直至连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞。

进一步地，所述随机采样位形获取子模块利用加权采样方法，从位形空间随机采样获得一个随机采样位形点q_rand。

本发明实施例提供的机器人避碰路径的规划装置可以应用在前述对应的方法实施例一中，详情参见上述实施例一的描述，在此不再赘述。

值得注意的是，上述系统实施例中，所包括的各个单元只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

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

步骤1、根据机器人的初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；

步骤2、根据所述初始位形q_s、目标位形q_g构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树；

步骤3、设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；

步骤4、根据所述初始位形q_s、目标位形q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；

步骤5、对所述机器人规划路径上的离散采样位形点分别进行碰撞检测，如果所有离散采样位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散采样位形点与障碍物发生碰撞，则执行步骤6；

步骤6、更新r_col的值为r_col/2；

步骤7、循环执行步骤3至6，直至生成的机器人规划路径上的所有离散采样位形点均没有与障碍物发生碰撞为止；

所述步骤4包括：

根据所述初始位形q_s、目标位形q_g计算得到扩展步长的初始值r_ext；

根据所述初始位形q_s、目标位形q_g和所述扩展步长的初始值r_ext配置所述双向快速密集搜索树；

局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径；

所述局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径包括：

从位形空间随机采样获得一个随机采样位形点q_rand；

搜索所述正向快速密集搜索树上与所述随机采样位形点q_rand最近邻的边e₁；

在边e₁上搜索与所述随机采样位形点q_rand最近邻的位形点

根据所述q_rand、以及所述扩展步长r_ext计算得到不与障碍物发生碰撞的新位形点q_new；

根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则搜索所述逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂；

在边e₂上搜索与所述新位形点q_new最近邻的位形点

根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

若连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞时，将所述边e₂、所述最近邻的位形点和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长r_ext加倍；

若连接所述最近邻的位形点和所述新位形点q_new之间的路径与障碍物发生碰撞时，计算机器人从向q_new移动而不与障碍物发生碰撞的最合适的位形点，将所述最合适的位形点、所述边e₂和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长r_ext减半，直至连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞。

2.如权利要求1所述的方法，其特征在于，利用加权采样方法，从位形空间随机采样获得一个随机采样位形点q_rand。

3.一种机器人避碰路径的规划装置，其特征在于，所述装置包括：

碰撞检测距离初始化单元，用于根据机器人的初始位形q_s和目标位形q_g计算得到碰撞检测距离参数的初始值r_col；

搜索树构建单元，用于根据所述初始位形q_s、目标位形q_g构造双向快速密集搜索树，所述双向快速密集搜索树包括正向快速密集搜索树和逆向快速密集搜索树；

碰撞检测距离设置单元，用于设置所述双向快速密集搜索树的碰撞检测距离参数的值为r_col；

规划路径生成单元，用于根据所述初始位形q_s、目标位形q_g采用自适应双向快速密集搜索树动态扩展方法生成一条机器人规划路径；

碰撞检测单元，用于对所述机器人规划路径上的离散采样位形点分别进行碰撞检测；

碰撞检测距离更新单元，用于如果所有离散采样位形点均没有与障碍物发生碰撞，则将由所有离散采样位形点组成的机器人规划路径作为有效路径，如果有离散采样位形点与障碍物发生碰撞，则更新r_col的值为r_col/2；

规划路径更新单元，用于依次循环调用规划路径生成单元、规划路径生成单元、碰撞检测单元和碰撞检测距离更新单元，直至生成的机器人规划路径上的所有离散采样位形点均没有与障碍物发生碰撞为止；

所述规划路径生成单元包括：

扩展步长初始化模块，用于根据所述初始位形q_s、目标位形q_g计算得到扩展步长的初始值r_ext；

搜索树配置模块，用于根据所述初始位形q_s、目标位形q_g和所述扩展步长的初始值r_ext配置所述双向快速密集搜索树；

扩展步长调整模块，用于局部调整所述扩展步长的初始值r_ext，获得一条机器人规划路径；

所述扩展步长调整模块包括：

随机采样位形获取子模块，用于从位形空间随机采样获得一个随机采样位形点q_rand；

第一最近邻边获取子模块，用于搜索所述正向快速密集搜索树上与所述随机采样位形点q_rand最近邻的边e₁；

第一最近邻位形点获取子模块，用于在边e₁上搜索与所述随机采样位形点q_rand最近邻的位形点

新位形点获取子模块，用于根据所述q_rand、以及所述扩展步长r_ext计算得到不与障碍物发生碰撞的新位形点q_new；

第一碰撞检测子模块，用于根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

第二最近邻边获取子模块，用于如果连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞，则搜索所述逆向快速密集搜索树上与所述新位形点q_new最近邻的边e₂；

第二最近邻位形点获取子模块，用于在边e₂上搜索与所述新位形点q_new最近邻的位形点

第二碰撞检测子模块，用于根据所述最近邻的位形点以及所述新位形点q_new做碰撞检测；

扩展步长加倍子模块，用于若连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞时，将所述边e₂、所述最近邻的位形点和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长加倍；

扩展步长减半子模块，用于若连接所述最近邻的位形点和所述新位形点q_new之间的路径与障碍物发生碰撞时，计算机器人从向q_new移动而不与障碍物发生碰撞的最合适的位形点，将所述最合适的位形点、所述边e₂和所述新位形点q_new添加至所述逆向快速密集搜索树中，并获得新边e_new，同时分别使与所述边e₂和所述新边e_new对应的扩展步长减半，直至连接所述最近邻的位形点和所述新位形点q_new之间的路径没有与障碍物发生碰撞。

4.如权利要求3所述的装置，其特征在于，所述随机采样位形获取子模块利用加权采样方法，从位形空间随机采样获得一个随机采样位形点q_rand。