CN109571466B - 一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法 - Google Patents

Abstract

本发明公开了一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法，包括离线规划和在线规划，离线规划利用冗余机械臂逆运动学的解析解法确定最优目标状态，来作为目标节点以构建搜索树，在线规划在所得到的搜索树上依据当前环境进行树的扩展和重新布线，实时得到一条从目标节点到根节点的路径，且当机械臂运动时树的根节点也随之变化，若目标节点被障碍物阻挡时切换目标节点，搜索新的路径从而避开动态障碍物。本发明通过离线和在线两步规划，解决了因RRT*计算复杂度过高导致无法用于冗余机械臂实时避障的问题，并通过实时更新搜索树的根节点和目标节点，解决了动态环境下目标节点不可达问题，保证实时为机械臂规划一条无碰路径。

Description

一种基于快速随机搜索树的七自由度冗余机械臂动态避障路 径规划方法

技术领域

本发明涉及多自由度机械臂路径规划领域，尤其涉及一种基于快速随机搜索树的七自由 度冗余机械臂动态避障路径规划方法。

背景技术

机器人路径规划是机器人研究领域的基本问题，旨在为机器人找到一条从起始状态到目 标状态的路径，并安全避开运动空间中的所有障碍物。国内外学者针对移动机器人已经提出 了许多有效的路径规划方法，例如A*算法、Dijkstra算法等，此时常将移动机器人简化成一 个带有运动学约束的质点。但机械臂是一个多输入多输出、高度非线性、强耦合的复杂系统， 在笛卡尔空间中无法直接作为质点看待，因此大多数用于移动机器人的路径规划方法无法直 接扩展到机械臂。

RRT*(快速随机搜索树)是一种典型的树状结构搜索算法，是其在对最优化控制、非完 整线性规划和随机采样算法等理论进行深度研究后提出的一种基于随机采样的增量式运动规 划算法。其基本思想是以初始状态作为根节点，通过在状态空间中进行随机采样不断扩展， 逐步覆盖状态空间的自由区域并最终覆盖整个状态空间，从而获得一条从初始状态到目标状 态的路径。RRT*方法实现简单，不需要对障碍物进行精确建模，在高维空间中依然可以实现 快速搜索且没有人工势场法易出现的局部极小值问题。

发明内容

本发明的目的在于针对现有技术的不足，提供一种基于RRT*的七自由度机械臂动态避障 路径规划方法。

本发明的目的是通过以下技术方案来实现的：一种基于改进快速随机搜索树的七自由度 冗余机械臂动态避障规划方法，该方法包括以下步骤：

步骤一：根据目标点的位置，利用冗余机械臂逆运动学的解析解法确定机械臂的最优目 标状态；

步骤二：以最优目标状态为目标节点，在关节空间构建搜索树，规划一条无碰路径；

步骤三：从步骤二规划好的路径中选取前k个节点作为当前路径输入给机械臂，然后将第 k个节点更新为根节点；

步骤四：根据障碍物的位置变化，实时更新环境地图，并对搜索树重新布线；

步骤五：若关节空间上的目标节点被障碍物遮挡，则重新计算当前环境下的最优目标状 态作为新的目标节点。

进一步地，步骤一中利用冗余机械臂逆运动学的解析解法确定机械臂的最优目标状态， 具体如下：

首先定义评价函数

用于评价机械臂所处状态的优劣，其值越小 表示机械臂所处状态越优；

然后对该评价函数进行优化求解可求得唯一θ值；

其中θ为自运动变量，当机械臂末端位姿确定且不考虑翻肩现象时，给定θ值便可唯一确 定机械臂状态，n为障碍物个数，F_i(θ)为第i个障碍物对机械臂产生的排斥力，F_i(θ)计算公式 如下：

其中，D_i(θ)为第i个障碍物与机械臂的最近距离，ρ为障碍物排斥势场作用范围，α为形 变因子。

进一步地，步骤二中以最优目标状态为目标节点构建搜索树，规划一条无碰路径，具体 如下：

设置当前机械臂所处状态q_init＝[q₁，q₂，…，q₇]^T为搜索树根节点q_root，求解得到的最优目 标状态q_best为目标节点q_goal，其中q₁，q₂，…，q₇分别为机械臂的7个关节角；

然后在机械臂关节角空间中随机采样一个点x_rand，连接搜索树中与该随机点距离最近的 节点x_nearest，然后在x_rand与x_nearest的连线上以一定的步长Δt截取得到新的节点x_new；判断 节点x_nearest与x_new的连线是否与障碍物碰撞，如果碰撞则重新采样，否则遍历x_new的近邻点 集X_near，找到使离线代价函数cost(x_new)最小且连线不与障碍物碰撞的节点作为x_new的父节 点，将x_new添加到搜索树中；

当达到最大设定迭代次数后结束搜索树的构建，从目标节点的近邻点集中选择代价函数 cost(q_goal)最小的节点，向上回溯到根节点，得到一条无碰路径(x₀，x₁，…，x_goal)；

其中，

(1)x_rand按如下公式计算：

Uniform(X_goal)＝f(θ_r) θ_r∈[0，2π]

其中，Uniform(·)表示均匀采样，P_r∈[0，1]表示随机数，α为自定义常数，X_goal表示目 标解空间，X_free表示机械臂无障碍空间，θ_r为在[0，2π]范围内的随机数，f(θ_r)表示给定θ_r角 后求解机械臂每个关节角的过程；

(2)x_new按如下公式计算：

其中，x_nearest为搜索树中离x_rand距离最近点，Δt为自定义步长；

(3)X_near按如下公式计算：

X_near＝{x_i∈X_tree：||x_i-x_new||＜D_near}

其中，X_tree表示搜索树中的节点集合，x_i表示搜索树中的第i个节点，D_near表示自定义 近邻距离；

(4)cost(x_i)按如下公式计算：

其中，x_parent表示节点x_i的父节点，root表示根节点的序号。

进一步地，步骤三中从规划好的路径中选取前k个节点作为当前路径输入给机械臂，然后 将第k个节点更新为根节点，具体为：

将规划好的路径(x₀，x₁，…，x_goal)作为当前路径，从该路径中选取出前k个点输入给机械 臂，当机械臂当前所处的状态x_a离规划路径的第一个节点x₀的距离到达设定阈值时，令机械 臂沿着路径(x₀，x₁，…，x_k)运动，并同时将原来的根节点去除，将第k个节点x_k更新为搜索树 新的根节点x₀。

进一步地，步骤四中，根据障碍物的位置变化，实时更新环境地图，并对搜索树重新布 线，具体为：

首先更新环境地图信息，然后从当前根节点x₀出发，在一定时间T_rewire内更新满足||x_i-x₀||＜D_rewire的所有节点，其中T_rewire为自定义更新时间，D_rewire为自定义更新范围； 如果一定时间内未更新完所有节点则放弃更新剩余未更新节点，如果范围内所有节点全部更新完毕而时间还有剩余，则再次更新环境地图，重新从根节点x0出发，在剩余时间内更新满足||x_i-x₀||＜D_rewire的节点；

其中，更新节点的实现方法如下：

遍历待更新节点x_wait的近邻点集X_near，找到使在线代价函数cost′(x_wait)最小的节点作 为x_wait的父节点，然后更新x_wait的父节点x_parent；

其中，cost′(x_wait)按如下公式计算：

其中，δ_d、δ_f、δ_a分别表示距离、排斥速度和排斥速度变化率的权重，用来选择不同偏 向的路径，d_min为机械臂与障碍物之间的最近距离，ρ₀为障碍物影响范围，F为当前时刻障碍 物对机械臂产生的排斥力，F′为上一个时刻障碍物对机械臂产生的排斥力，a_max表示排斥力 容许最大变化率。

进一步地，步骤五中若目标节点被障碍物遮挡，则重新计算当前环境下的最优目标状态 作为新的目标节点，具体为：判断当前目标节点与障碍物的距离是否小于设定值D_goal，如果 小于则依据当前环境重新执行步骤一生成新的目标节点，然后重复执行步骤步骤二到步骤五。

与现有技术相比，本发明的有益效果如下：本发明提出的基于RRT*的七自由度冗余机械 臂动态避障规划方法，是在传统快速搜索随机树算法的基础上进行的改进。利用解析解求解 机械臂逆运动学然后通过优化目标函数确定目标节点的方法，避免了冗余机械臂目标状态不 确定性的问题；将规划过程分为离线规划和在线规划两个过程，解决了因RRT*计算复杂度过 高导致无法用于冗余机械臂实时避障的问题；在在线规划阶段，利用搜索树的扩展和重新布 线达到避开动态障碍物的目的，当原目标节点被动态障碍物遮挡导致目标不可达时，能够切 换目标节点重新搜索一条新的路径。

附图说明

图1是本发明的RRT*的七自由度冗余机械臂动态避障路径规划方法的流程图；

图2是机械臂四连杆结构简图；

图3是重新布线示意图；

图4为机械臂与障碍物最近距离随时间变化的曲线图；

图5为机械臂末端与目标距离随时间变化的曲线图。

具体实施方式

下面根据附图和优选实施例详细描述本发明，本发明的目的和效果将变得更加明白，以 下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例 仅仅用以解释本发明，并不用于限定本发明。

本发明基于RRT*的七自由度冗余机械臂动态避障规划方法，用于多自由度机械臂在动态 环境下的路径规划。该方法包括离线规划和在线规划两部分，离线规划部分利用冗余机械臂 逆运动学的解析解法确定最优目标状态，然后以该目标为目标节点构建搜索树，在线规划部 分在离线规划得到的搜索树上依据当前环境进行树的扩展和重新布线，实时得到一条从目标 节点到根节点的路径，且当机械臂运动时树的根节点也随之变化，当离线规划选定的目标节 点被障碍物阻挡时切换目标节点，搜索新的路径达到避开空间动态障碍物的目的。整个系统 流程图如图1所示。

步骤一：根据目标点的位置，利用冗余机械臂逆运动学的解析解法确定机械臂的最优目 标状态。

如图2所示为机械臂四连杆结构简图，其中A、B、C和D点分别表示机械臂关节2、4、6和7。A点固连于机械臂底座，当机械臂在世界坐标系中的位置确定时，A点位置也确定， 姿态随关节角度变化。机械臂末端坐标系固连于D点，则可通过齐次变换关系求得C点位置，但无法确定其姿态。因此，当给定机械臂末端目标位姿后即可唯一求解A、C和D点的位置，此时无论B点如何运动都不影响机械臂末端位姿。从图2中可看出，当固定A、C点后，B 点只能以AC为轴，B点到AC的垂线为半径作圆周运动，即冗余机械臂的自运动。为便于 描述其运动轨迹，在轨迹圆心E处建立坐标系，其中z轴由A点指向C点，y轴垂直于平面 OAC向外，x轴可由右手法则确定，定义机械臂的自运动变量，即B点在坐标系E运动的转 角为θ，如图2右所示。当机械臂末端位姿确定且不考虑翻肩现象时，给定θ值便可唯一确定 机械臂状态。

为确定最优的目标状态，定义评价函数如下：

其中，θ为自运动变量，n为障碍物个数，F_i(θ)为第i个障碍物对机械臂产生的排斥力。 对该评价函数进行优化求解便可求得唯一θ值，D_i(θ)为第i个障碍物与机械臂的最近距离，ρ 为障碍物排斥势场作用范围，α为形变因子。

步骤二：以最优目标状态为目标节点，在关节空间构建搜索树，规划一条无碰路径。

设置当前机械臂所处状态q_init＝[q₁，q₂，…，q₇]^T为搜索树根节点q_root，求解得到的最优目 标状态q_best为目标节点q_goal，其中q₁，q₂，…，q₇分别为机械臂的7个关节角；

然后在机械臂关节角空间中随机采样一个点x_rand，连接搜索树中与该随机点距离最近的 节点x_nearest，然后在x_rand与x_nearest的连线上以一定的步长Δt截取得到新的节点x_new；判断 节点x_nearest与x_new的连线是否与障碍物碰撞，如果碰撞则重新采样，否则遍历x_new的近邻点 集X_near，找到使离线代价函数cost(x_new)最小且连线不与障碍物碰撞的节点作为x_new的父节 点，将x_new添加到搜索树中；

当达到最大设定迭代次数后结束搜索树的构建，从目标节点的近邻点集中选择代价函数 cost(q_goal)最小的节点，向上回溯到根节点，得到一条无碰路径(x₀，x₁，…，x_goal)；

其中，

(1)x_rand按如下公式计算：

Uniform(X_goal)＝f(θ_r) θ_r∈[0，2π]

其中，Uniform(·)表示均匀采样，P_r∈[0，1]表示随机数，α为自定义常数，X_goal表示目 标解空间，X_free表示机械臂无障碍空间，θ_r为在[0，2π]范围内的随机数，f(θ_r)表示给定θ_r角 后求解机械臂每个关节角的过程；

(2)x_new按如下公式计算：

其中，x_nearest为搜索树中离x_rand距离最近点，Δt为自定义步长；

(3)X_near按如下公式计算：

X_near＝{x_i∈X_tree：||x_i-x_new||＜D_near} (4)

其中，X_tree表示搜索树中的节点集合，x_i表示搜索树中的第i个节点，D_near表示自定义 近邻距离；

(4)cost(x_i)按如下公式计算：

其中，x_parent表示节点x_i的父节点，root表示根节点的序号。

步骤三：从步骤二规划好的路径中选取前k个节点作为当前路径输入给机械臂，然后将第 k个节点更新为根节点，具体为：

将规划好的路径(x₀，x₁，…，x_goal)作为当前路径，从该路径中选取出前k个点输入给机械 臂，当机械臂当前所处的状态x_a离规划路径的第一个节点x₀的距离到达设定阈值时，令机械 臂沿着路径(x₀，x₁，…，x_k)运动，并同时将原来的根节点去除，将第k个节点x_k更新为搜索树 新的根节点x₀。

步骤四：根据障碍物的位置变化，实时更新环境地图，并对搜索树重新布线。

首先更新环境地图信息，然后从当前根节点x₀出发，在一定时间T_rewire内更新满足||x_i-x₀||＜D_rewire的所有节点，其中，T_rewire为自定义更新时间，D_rewire为自定义更新范围。如果一定时间内未更新完所有节点则放弃更新剩余未更新节点，如果范围内所有节点全部更 新完毕而时间还有剩余，则再次更新环境地图，重新从根节点x₀出发，在剩余时间内更新满 足||x_i-x₀||＜D_rewire的节点；

其中，更新节点的实现方法如下：

遍历待更新节点x_wait的近邻点集X_near，找到使在线评价函数cost′(x_wait)最小的节点作 为x_wait的父节点，然后更新x_wait的父节点x_parent。

其中，cost′(x_wait)按如下公式计算：

其中，δ_d、δ_f、δ_a分别表示距离、排斥速度和排斥速度变化率的权重，用来选择不同偏 向的路径，d_min为机械臂与障碍物之间的最近距离，ρ₀为障碍物影响范围，F为当前时刻障碍 物对机械臂产生的排斥力，F′为上一个时刻障碍物对机械臂产生的排斥力，a_max表示排斥力 容许最大变化率。

图3给出了更新节点的示意图，图中为某一时刻搜索树和障碍物的相对状态，其中节点x₁和x₂被障碍物遮挡，则障碍物对两节点产生的排斥力趋于无穷，因此利用式(6)计算两节点的 代价也趋于无穷。当对搜索树重新布线时，x₂的子节点x₃将更新其父节点为x₄，如图3(b)所 示，期间根节点也发生变化。当障碍物朝着节点x₄的方向高速运动，对节点x₄产生的排斥速 度变化率a＞a_max时，此时节点x₄的花费也趋于无穷，则节点x₃将连接周围其他花费较小的节 点，如图3(c)所示。

步骤五：若关节空间上的目标节点被障碍物遮挡，则重新计算当前环境下的最优目标状 态作为新的目标节点，具体为：判断当前目标节点与障碍物的距离是否小于设定值D_goal，如 果小于则依据当前环境重新执行步骤一生成新的目标节点，然后重复执行步骤步骤二到步骤 五。

图4和图5给出了本发明的方法的运行结果，给定条件为：两个球形动态障碍物，半径 分别为0.05m和0.06m，位置分别为(-0.23,-0.20,0.40)和(-0.03,-0.22,0.13)，运动加速度分别为 0.017m/s²和0.08m/s²，运动方向分别为(-1,0,0)和(0,0,1)；一个球形静态障碍物，半径为0.1m， 位置为(-0.45,-0.36,0.13)。目标初始位置为(-0.15,-0.40,0.27)，以0.0018m/s²的加速度向(1,0.1,0) 方向运动。一个七自由度机械臂，总长0.94m，初始位姿为(0,0,0,π/2,0,π/2,0)，初始运动速度 为(0,0,0,0,0,0,0)，末端初始位置为(-0.32,0,0.34)，末端最大可运行速度为0.1m/s，则机械臂与 障碍物最近距离随时间的变化曲线如图4所示，机械臂末端与目标距离随时间的变化曲线如 图5所示。从结果可以看出，本发明的方法能够很好实现机械臂避开环境中动静态障碍物并 追踪动态目标的功能。

本技术领域的人员根据本发明所提供的文字描述、附图以及权利要求书能够很容易在不 脱离权利要求书所限定的本发明的思想和范围条件下，可以做出多种变化和改动。凡是依据 本发明的技术思想和实质对上述实施例进行的任何修改、等同变化，均属于本发明的权利要 求所限定的保护范围之内。

Claims (6)

1.一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法，其特征在于，该方法包括以下步骤：

步骤一：根据目标点的位置，利用冗余机械臂逆运动学的解析解法确定机械臂的最优目标状态；

步骤二：以最优目标状态为目标节点，在关节空间构建搜索树，规划一条无碰路径；

步骤三：从步骤二规划好的路径中选取前k个节点作为当前路径输入给机械臂，然后将第k个节点更新为根节点；

步骤四：根据障碍物的位置变化，实时更新环境地图，并在新的根节点附近，自定义更新时间和更新半径，对搜索树重新布线；

步骤五：若关节空间上的目标节点被障碍物遮挡，则重新计算当前环境下的最优目标状态作为新的目标节点。

2.根据权利要求1所述的方法，其特征在于，步骤一中利用冗余机械臂逆运动学的解析解法确定机械臂的最优目标状态，具体如下：

首先定义评价函数

用于评价机械臂所处状态的优劣，其值越小表示机械臂所处状态越优；

然后对该评价函数进行优化求解可求得唯一θ值；

其中θ为自运动变量，当机械臂末端位姿确定且不考虑翻肩现象时，给定θ值便可唯一确定机械臂状态，n为障碍物个数，F_i(θ)为第i个障碍物对机械臂产生的排斥力，F_i(θ)计算公式如下：

其中，D_i(θ)为第i个障碍物与机械臂的最近距离，ρ为障碍物排斥势场作用范围，α为形变因子。

3.根据权利要求1所述的方法，其特征在于，步骤二中以最优目标状态为目标节点构建搜索树，规划一条无碰路径，具体如下：

设置当前机械臂所处状态q_init＝[q₁，q₂，…，q₇]^T为搜索树根节点q_root，求解得到的最优目标状态q_best为目标节点q_goal，其中q₁，q₂，…，q₇分别为机械臂的7个关节角；

然后在机械臂关节角空间中随机采样一个点x_rand，连接搜索树中与该随机点距离最近的节点x_nearest，然后在x_rand与x_nearest的连线上以一定的步长Δt截取得到新的节点x_new；判断节点x_nearest与x_new的连线是否与障碍物碰撞，如果碰撞则重新采样，否则遍历x_new的近邻点集X_near，找到使离线代价函数cost(x_new)最小且连线不与障碍物碰撞的节点作为x_new的父节点，将x_new添加到搜索树中；

当达到最大设定迭代次数后结束搜索树的构建，从目标节点的近邻点集中选择代价函数cost(q_goal)最小的节点，向上回溯到根节点，得到一条无碰路径(x₀，x₁，…，x_goal)；

其中，

(1)x_rand按如下公式计算：

Uniform(X_goal)＝f(θ_r)θ_r∈[0，2π]

其中，Uniform(·)表示均匀采样，P_r∈[0，1]表示随机数，α为自定义常数，X_goal表示目标解空间，X_free表示机械臂无障碍空间，θ_r为在[0，2π]范围内的随机数，f(θ_r)表示给定θ_r角后求解机械臂每个关节角的过程；

(2)x_new按如下公式计算：

其中，x_nearest为搜索树中离x_rand距离最近点，Δt为自定义步长；

(3)X_near按如下公式计算：

X_near＝{x_i∈X_tree：||x_i-x_new||＜D_near}

其中，X_tree表示搜索树中的节点集合，x_i表示搜索树中的第i个节点，D_near表示自定义近邻距离；

(4)cost(x_i)按如下公式计算：

其中，x_parent表示节点x_i的父节点，root表示根节点的序号。

4.根据权利要求1所述的方法，其特征在于，步骤三中从规划好的路径中选取前k个节点作为当前路径输入给机械臂，然后将第k个节点更新为根节点，具体为：

将规划好的路径(x₀，x₁，…，x_goal)作为当前路径，从该路径中选取出前k个点输入给机械臂，当机械臂当前所处的状态x_a离规划路径的第一个节点x₀的距离到达设定阈值时，令机械臂沿着路径(x₀，x₁，…，x_k)运动，并同时将原来的根节点去除，将第k个节点x_k更新为搜索树新的根节点x₀。

5.根据权利要求1所述的方法，其特征在于，步骤四中，根据障碍物的位置变化，实时更新环境地图，并对搜索树重新布线，具体为：

首先更新环境地图信息，然后从当前根节点x₀出发，在一定时间T_rewire内更新满足||x_i-x₀||＜D_rewire的所有节点，其中T_rewire为自定义更新时间，D_rewire为自定义更新范围；如果一定时间内未更新完所有节点则放弃更新剩余未更新节点，如果范围内所有节点全部更新完毕而时间还有剩余，则再次更新环境地图，重新从根节点x₀出发，在剩余时间内更新满足||x_i-x₀||＜D_rewire的节点；

其中，更新节点的实现方法如下：

遍历待更新节点x_wait的近邻点集X_near，找到使在线代价函数cost′(x_wait)最小的节点作为x_wait的父节点，然后更新x_wait的父节点x_parent；

其中，cost′(x_wait)按如下公式计算：

其中，δ_d、δ_f、δ_a分别表示距离、排斥速度和排斥速度变化率的权重，用来选择不同偏向的路径，d_min为机械臂与障碍物之间的最近距离，ρ₀为障碍物影响范围，F为当前时刻障碍物对机械臂产生的排斥力，F′为上一个时刻障碍物对机械臂产生的排斥力，a_max表示排斥力容许最大变化率。

6.根据权利要求1所述的方法，其特征在于，步骤五中若目标节点被障碍物遮挡，则重新计算当前环境下的最优目标状态作为新的目标节点，具体为：判断当前目标节点与障碍物的距离是否小于设定值D_goal，如果小于则依据当前环境重新执行步骤一生成新的目标节点，然后重复执行步骤步骤二到步骤五。

Images