CN108582073B - 一种基于改进的随机路标地图法的机械臂快速避障方法 - Google Patents

Abstract

本发明实施例提供了一种基于改进随机路标地图法的机械臂快速避障路径规划方法，实现了机械臂处于复杂环境下的高效无碰撞路径搜索，包括：以关节空间向量坐标的方式建立机械臂状态模型，通过一种“边界采样”方法和人工势场法，优化传统随机路标地图法的地图采样点选取策略，选定满足机械臂结构与精度要求的地图采样点；针对选定的地图采样点集，设计一种三层分步地图采样点连通方法，保证路标地图连通性能；针对机械臂始末节点与路标地图连通时的避障问题，设计高级局部规划器，优化非地图采样点位置的规划策略。根据本发明实施例提供的技术方案，可有效提升机械臂避障路径规划的精度和执行效率。

Description

一种基于改进的随机路标地图法的机械臂快速避障方法

【技术领域】

本发明涉及一种基于改进的随机路标地图法(以下简称PRM算法)的机械臂快速避障方法，属于机械臂运动避障规划技术领域。

【背景技术】

随着科学技术的快速发展，具有操作灵活、精准度高等优点的高自由度机械臂已被广泛应用于工业生产、航空航天等领域。机械臂实际工况下，环境因素复杂，机械臂所处环境甚至机械臂自身设备会成为限制机械臂运动规划的因素，所以机械臂执行工作任务时，需考虑限制机械臂运动的环境或自身约束，规划其无碰撞路径。由于机械臂运动规划是实现各种机械臂功能的基本要素，而且规划其避障路径的速率会大大影响机械臂的工作能力，因此，开展机械臂快速避障方法的相关研究，提高机械臂避障路径规划效率，对机械臂在人类工业生产、太空探索等领域的应用有着不可估量的理论价值和现实意义。

现有机械臂避障路径规划方法主要采用传统的即时规划即时避障的思路，即在已知机械臂目标点后，结合环境因素对机械臂进行整段路径的规划方法。典型方法如传统启发式搜索算法，在机械臂避障路径规划的过程中需对机械臂执行路径进行实时的碰撞检测，大大增加了规划机械臂避障路径的耗时。在环境约束复杂的情况下，该方法难以满足机械臂工作过程中的避障路径规划效率问题，规划能力相对较差。

PRM算法是一种典型的基于图搜索的方法，相比于传统规划方法可大幅提升规划效率。此算法主要分学习阶段和搜索阶段两部分。其中，学习阶段为选取地图采样点、构建路标地图过程，搜索阶段为搜索已构建好的路标地图，规划整条搜索路径。由于机械臂结构复杂，单纯的末端位置不能表征机械臂构型状态，原有选取地图采样点策略也不能满足机械臂在工作空间内避障运动规划精度需求，所以传统PRM算法不能直接应用于机械臂避障路径搜索中。

【发明内容】

有鉴于此，本发明提供了一种基于改进随机路标地图法的机械臂快速避障方法。所提方法可解决该算法不能直接应用于机械臂的问题，并高质量、高效率规划出机械臂避障路径，满足机械臂在规划过程中的效率要求。

上述机械臂快速避障路径规划过程中，用到的方法包括如下：

1.依据人工势场法与一种“边界采样”策略，设计一种地图采样点选取方法，提高障碍空间附近地图采样点的密度并提高地图采样点均匀程度；

2.依据所得采样点与局部规划器，设计一种三层分步路标地图连通策略，满足路标地图的对连通性能的要求；

3.依据机械臂路径规划方法与启发式算法，设计一种“高级局部规划器”，优化机械臂始末位置至路标地图内的联通策略。

上述求解过程中，实现选取地图采样点的过程包括：

第一步：构建机械臂位置状态的数学模型，提出一种基于“边界采样”的地图采样点移动策略，最大化利用落入障碍空间内的地图采样点，提高障碍周围的地图采样点密度；

机械臂状态的表征与其学习阶段内地图采样点的选取为构建路标地图的基础。由于机械臂构型复杂，与传统意义上的小车或机器人不同，采用机械臂关节空间向量的方式可以更好地描述机械臂在工作空间内的状态，所以，在表征机械臂在实际工况内的状态时，应采用关节空间坐标的方式建立数学模型，将机械臂在工作空间内的构型表述为一个k维向量v＝(θ₁,θ₂,...,θ_k)，其中k为机械臂转动关节数，θ_i为第i个关节的关节角度。

首先通过传统地图采样点选取策略选取地图采样点集，并将其按是否与障碍空间重合分类，具体方式如下：

在机械臂构型空间内随机选取地图采样点，构成点集c。在c中对每个地图采样点进行遍历，检测其是否与障碍对应构型空间发生重合，将点集c分为自由空间内的地图采样点集合c₀和与障碍空间发生重合的地图采样点集合 c₁。

传统PRM算法在学习阶段选取采样点时，如果发生了采样点恰好处在障碍空间内的情况，会直接舍弃此地图采样点，此行为会降低后面用来构建路标地图的地图采样点个数。“边界采样”策略是一种针对复杂空间内无效采样点过多情况下的优化方式，其思想为将落入障碍内的采样点给一个随机的初速度使其运动至工作空间内，目的为增加障碍周围的地图采样点密度，增加机械臂操作精度。具体方式如下：

针对c₁内的每一地图采样点v_i，分别给定一随机速度d_i，使采样点沿速度方向运动，直至地图采样点运动至自由工作空间内。将所有能够成功移动至自由工作空间内的新地图采样点添加至点集c₀中。

第二步：基于人工势场法调整障碍周围的地图采样点“边界采样”策略所利用地图采样点，使地图采样点密度均匀；

由于传统地图采样点选取方式为随机采样，且上述“边界采样”的地图采样策略中，对障碍物内不可用的地图采样点的处理方式为给定随机速度使其运动，没有考虑运动方向与障碍空间的关系，所以极容易出现地图采样点位置不均匀的情况。针对此类现象，本发明设计了一种基于人工势场法的调整地图采样点的方法，假定地图采样点在落在自由构型空间内后，产生一排斥势场，对接下来生成的地图采样点产生排斥力，使接下来的地图采样点沿着受力方向进行移动，增加障碍周围地图采样点的均匀程度。具体方式如下：

1.已存在的点v_i＝(θ₁,θ₂,…,θ_k)对于新生成的地图采样点v产生的势场力定义如下：

式中，

表示两采样点间的距离，D为产生势场力的最小距离，H为表征受力大小的常量。此式表示两个地图采样点如果距离越近，产生的势场排斥力越大，如果两个地图采样点距离大于最小距离D，就可以认为此点位置合适，不产生势场力。采样点受到的势场力如下：

式中m为已存在的自由空间内的地图采样点数量。

2.因为人工势场法局部的最小值问题，会使得地图采样点v在受力达到平衡时并没有逃离其他地图采样点的势力作用范围，本发明通过如下公式来检测这一情况。

min(dist(v_i,v))≤D&&F＝0 (3)

如果式(3)成立，则说明采样点v仍然受到其他地图采样点对它产生的力的作用，此时，本发明对采样点v人为的施加一个任意方向的附加力F，使其运动一段时间，之后再重新利用人工势场法决定采样点的受力大小和方向，直到其满足：

min(dist(v_i,v))＞D&&F＝0 (4)

则说明采样点v达到采样要求，将v加入到图G中。其实在实际操作中，有可能经过多次迭代后采样点v还是无法满足上述要求，此时说明采样点v附近的采样密度已经足够了，可直接将采样点v舍去。

由此步骤，可以得到在自由空间内的地图采样点集合c₀。此集合在障碍物周围的地图采样点密度增大且均匀。相较于传统PRM算法的随机均匀采样策略，此点集c₀更适合构建路标搜索地图。

根据上述已得的地图采样点集，路标搜索地图连通过程包括：

针对已确定的地图采样点集c₀，需使用上述局部规划器对点集进行连通，构建路标搜索地图。但是，如果用局部规划器对点集c₀内每两个点都进行规划检测，会大大增加算法在学习阶段的时间，因此，本发明设计了一种三层分步连通策略，在保证路标地图连通率与点连通度的情况下尽可能减少学习时间，具体方式如下：

1.将已存在的点集c₀按是否进行过移动分为c₁和c₂，其中c₁为自由空间内未经移动过的点集，c₂为按“边界采样”策略和人工势场法调整过的障碍周围的重利用点集。c₁的每一点v_1i，利用局部规划器连接与其距离最近的k_n个点，并记录此k_n个点内连接成功的点数d_1i。点v_1i的连通成功率可表示如下：

由于被调整过的点集c₂点密度相对较大，如果只按传统连通策略进行连通，就会出现连通范围过小，边的数量不足的问题，所以针对此类地图采样点v_2i，应使用局部规划器对其周围一定距离范围k_d内所有采样点进行检测，由此减小点密度差对地图连通度的影响。此类点v_2i连通度可表示如下：

其中，d_2i表示节点v_2i与周围节点的连通个数，

表示所有边界节点的平均连通个数。此时可得到一初始路标搜索地图G_ini。

2.针对已经求得的初始路标搜索地图G_ini中，可能会存在的度数低的地图采样点v_low，继续进行扩展。如果点的连通度小于一阈值ω₀，则给定更大的一检测点数k_n′或连通范围k_d′进行连通，增加图中部分未有效利用的地图采样点的度数(点上连接的边的数目)、增大图的连通度，由此得到一新的路标搜索地图G_mid。

3.对上一步骤生成的路标搜索地图G_mid进行检验，如果此图分支数大于 1，则应对每两个独立分支进行连通，即保证最后生成的路标地图为连通图。假设图G_mid存在n个分支，将之从小到大排列，针对分支V_i，遍历所有更大的分支，找出一个最接近的分支V_i+1，将V_i和V_i+1相连通。重复这个过程，直到所有分支合并为一个分支。由此可得到最终的路标搜索地图G_end。

依据机械臂规划算法与启发式算法，“局部规划器”包括：

第一种：连接图内节点的“传统局部规划器”：

在已得待连通的点集c₀后，为构建路标地图，需对两点进行规划，为判断路标地图内两点间是否存在一条边(如果两地图采样点间存在一条边，则证明存在一条可行路径从一点规划至另一点)，本发明采用局部规划器对两点间路径进行检测，如果可以规划得无碰撞可行解，那么就认为可在两地图采样点间构建一条边。

由于应用于机械臂的地图采样点为关节空间位置，所以应在关节空间内对两采样点间进行直线规划。规划过程中如发生机械臂与障碍碰撞的情况，则规划失败，不能在此两被规划点间建立边；反之，若能够成功完成关节空间内的直线规划，则在两点间添加一条边。

第二种：将机械臂初始位置和目标位置与路标地图连通的“高级局部规划器”：

根据PRM算法特点，路标搜索地图内采样点位置只是自由空间内的有限构型。为了让搜索图能够应用于任意的机械臂起始位置和目标位置，需要在PRM算法规划阶段规划起始位置到路标地图最近的采样点，并从距目标位置最近的采样点规划至机械臂运动目标位置。因为在构造搜索地图时没有考虑此两段路径的可行性，所以本发明采用启发式算法设计一种“高级局部规划器”对此两段路径进行避障路径规划，步骤如下：

1.在地图采样点集c₀中分别搜索距实际初始运动起点v_begin与距实际目标位置v_end最近的地图采样点v_b-near与v_e-near。分别对两路径进行关节空间内的直线路径规划，如果成功，则可在v_begin和节点v_b-near间添加一条边用以直接搜索机械臂从初始状态和目标状态的路径。

2.如果在上一步骤对两节点间规划失败，发生两节点间的直线规划发生与障碍物发生碰撞情况，基于启发式算法在八维的关节空间内规划其无碰撞路径，栅格化机械臂关节空间构型，并设计估价函数如下：

f(x)＝g(x)+h(x) (7)

其中，h(x)为启发项，设置为当前机械臂关节空间内构型与目标构型间的距离，表征如下式：

在给定步长u之后，便可在每一步启发式规划时对机械臂进行碰撞检测，规划其得到其无碰撞路径。

【附图说明】

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

图1是本发明实施例中机械臂快速避障路径规划方法的流程示意图；

图2是本发明实施例中“边界采样”采样点选取策略与人工势场法地图采样点调整策略对比示意图；

图3是本发明实施例中的连通效果示意图；

图4是本发明实施例中高级局部规划器在二维空间内的栅格化搜索示意图。

【具体实施例】

根据本发明所述的机械臂快速避障方法，以八自由度模块化机械臂为研究对象展开验证，研究对象DH参数表如下所示。

本实施例采用二维平面的方式展示改进PRM算法的优越性，如上述图2 所示。示意图中第一排为传统PRM算法地图采样点选取策略的采样效果，此方式会产生狭窄区域内地图采样点数目过小、地图采样点密度不均匀的问题；第二排为应用“边界采样”策略的采样结果，可看出狭窄空间内采样点密度有明显提高，但还未解决采样点密度不均匀的问题；第三排为应用“边界采样”策略并通过人工势场法调整地图采样点，此点集在狭窄空间内密度高、并且地图采样点密度均匀，达到了预期优化效果。

本实施例所选取采样点为八维机械臂关节空间向量v＝(θ₁,θ₂,...,θ₈)，θ_i为第i个关节的关节角度。

依上述发明内容介绍的地图采样点选取策略进行采样。通过“边界采样”策略增大障碍空间周围的地图采样点密度，通过人工势场法调整地图采样点的均匀程度。此地图采样点选取策略已在上述二维空间内验证过执行效果，同理也可推广至八维关节空间。

将所得地图采样点集c₀按是否进行过移动分为c₁和c₂，其中c₁为自由空间内未经移动过的点集，c₂为按“边界采样”策略和人工势场法调整过的障碍周围的重利用点集。

按改进后的三层分步连通策略连通路标搜索地图，连通过程具体如下：

第一步：将上述点集c₁的每一点v_1i，利用局部规划器连接与其距离最近的k_n个点，并记录此k_n个点内连接成功的点数d_1i，按式7计算器连通度ω(v_1i)。

由于被调整过的点集c₂中地图采样点v_2i密度相对较大，所以应使用局部规划器对其周围一定距离范围k_d内所有采样点进行检测，按式8计算器连通度ω(v_2i)。

第二步：将连通度不满足的采样点进行补充连通，增加图中部分未有效利用的地图采样点的度数(点上连接的边的数目)、增大图的连通度。

第三步：检验上步产生的路标搜索地图是否为连通图，保证所得地图连通性好。如果上步所得路标地图为非连通图，连通其所有分支，使其成为连通图，得到最终的路标地图G_end。

选定如下两构型v₁、v₂为本实施例起始与目标构型，并在机械臂运动空间内添加影响机械臂工作的障碍。

构型v₁：[163°,178°,-72°,-7°,8°,7°,86°,-81°]；

构型v₂：[-102°,72°,-165°,-2°,-15°,17°,-112°,-81°]；

本实施例选取初始采样点个数为2000个，实验运行环境为处理器是 Intel(R)Core(TM)i5-4590CPU@3.30GHz型号的个人计算机，二维构型空间环境采用c++语言，Qt开发框架进行仿真实验验证，用如下三种方式进行机械臂的无碰撞路径规划，所得实验结果如下表所示。

在该实验条件下，应用高级局部规划器后，非地图采样点位置连通图至地图内的成功率从96％左右提高至了100％，证明了该方法的有效性。

由上述实验结果可得，本发明所提出的基于改进PRM算法的快速避障方法搜索时间有了大幅提高，而同时路径代价值也与传统路径规划算法相近且远优于传统PRM算法，所以本发明在机械臂实际工况下能够大幅提高机械臂工作能力，实现机械臂的快速避障规划。

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

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (6)

1.一种基于路标地图法的机械臂快速避障路径规划算法，其特征在于该方法包括以下步骤：

(1)依据机械臂构型特点，以关节空间坐标方式表征机械臂构型，提出一种“边界采样”地图采样点选取策略与一种基于人工势场法的地图采样点调整策略，用以选定地图采样点集，增大障碍周围地图采样点密度并提高地图采样点均匀程度；

(2)依据所得地图采样点集特点，设计一种三层连通策略构建路标搜索地图策略，分别依次进行初始路标地图构建、地图采样点连通度补充构建、路标地图连通性检验，保证所构建路标地图的连通性能；

(3)依据启发式算法设计高级局部规划器，优化非地图采样点位置至路标地图内的规划策略。

2.根据权利要求1所述的方法，其特征在于，选定地图采样点集的过程至少包括：

(1)建立机械臂位置状态模型，将k自由度机械臂在工作空间内的某一状态表述为一个k维向量v＝(θ₁,θ₂,...,θ_k)，θ_i为第i个关节的关节角度，并依此随机生成初始地图采样点集c_ini；

(2)基于一种“边界采样”地图采样点选取策略，增大障碍周围地图采样点密度；

(3)基于人工势场法，使边界采样点密度均匀化，得到应用于构建路标地图的地图采样点集c。

3.根据权利要求2所述的方法，其特征在于，“边界采样”地图采样点选取策略的过程至少包括：

(1)将已存在的点集c按是否与障碍空间发生重合分为集合c₁和c₂，对障碍内点集c₁内的每一地图采样点v_i，分别给定一随机速度d_i，使采样点沿速度方向运动，直至其落入自由构型空间内；

(2)将所有能够成功移动至自由工作空间内的新地图采样点添加至点集c₀中，如不能运动至自由构型空间内，则舍弃该地图采样点。

4.根据权利要求2所述的方法，其特征在于，基于人工势场法调整采样点策略的过程至少包括：

(1)已存在的点v_i＝(θ₁,θ₂,...,θ_k)对于新生成的地图采样点v产生的势场力定义如下：

式中，

表示两采样点间的距离，D为产生势场力的最小距离，H为表征受力大小的常量，由此，采样点受到势场力如下：

式中m为已存在的自由空间内的地图采样点数量；

(2)本发明通过如下公式来检测地图采样点v这人工势场法的局部最小值：

min(dist(v_i,v))≤D&&F＝0

&&表示按位取与。如果上式成立，则再给定一随机势场力跳出局部最小点；若多次迭代失败，则去掉该采样点。

5.根据权利要求1所述的方法，其特征在于，连通构建路标地图的过程至少包括：

(1)将已存在的点集c₀按是否进行过移动分为c₁和c₂，其中c₁为自由空间内未经移动过的点集，c₂为按“边界采样”策略和人工势场法调整过的障碍周围的重利用点集，其中c₁的每一点v_1i连通率表示如下：

其中，k_n为尝试连接的节点个数，d_1i为连接成功的点数，c₂内点v_2i连通度可表示如下：

其中，d_2i表示节点v_2i与周围节点的连通个数，

表示所有边界节点的平均连通个数，此时可得到初始路标搜索地图G_ini；

(2)如果点的连通度小于阈值ω₀，则给定更大的检测点数k_n′或连通范围k_d′进行连通，增加图中部分未有效利用的地图采样点的度数、增大图的连通度，由此得到一新的路标搜索地图G_mid，其中地图采样点的度数指与节点连接的边的数目；

(3)对上一步骤生成的路标搜索地图G_mid进行检验，如果此图分支数大于1，则应对每两个独立分支进行连通，可得到最终的路标搜索地图G_end。

6.根据权利要求1所述的方法，其特征在于，规划始末位置与地图采样点间路径的过程至少包括：

连通始末位置与最近的距离采样点间距离时，若出现非地图采样点位置不能连通至地图内的情况，基于启发式算法在关节空间内规划其无碰撞路径，栅格化机械臂关节空间构型，并设计估价函数如下：

f(x)＝g(x)+h(x)

其中，g(x)为已规划代价，h(x)为启发项，设置为当前机械臂关节空间内位置v_i与目标位置v_des间距离，表征如下式：

其中，

为当前关节空间内位置机械臂构型，

为目标位置机械臂构型，在给定步长u之后，便可在每一步启发式规划时对机械臂进行碰撞检测，规划其得到其无碰撞路径，减小非地图采样点位置的规划失败概率。

Images