邹宇星， 李立君， 高自成

(中南林业科技大学 机电工程学院，湖南 长沙 410000)

0 引 言

由于在工作空间中进行避障路径规划存在局部最小点和振荡等问题[1]，机械臂避障路径规划通常在构形空间[2，3]内进行。国内外学者基于构形空间对避障路径规划算法开展了大量研究[4～8]，提出了包括概率地图(probabilistic roadmap,PRM)算法[9]、快速扩展随机树(rapidly-exploring random tree,RRT)算法[10,11]和快速扩展树(fast marching tree，FMT)算法[12]等在内的路径规划算法。研究过程中传统算法不断获得改进，文献[13]提出了一种改进的bi-RRT算法，算法能够自动选择合适的采样点作为目标节点，提高了算法搜索效率；文献[14]基于关节空间提出一种改进的A*算法对空间机械臂进行避障路径规划，验证了A*算法应用于高自由度机械臂的可行性；文献[15]利用改进PRM算法对机械臂进行避障路径规划，算法只要通过采样获得部分机械臂关节构形空间无撞点，连接无撞点即可获得机械臂避障路径。

本文针对PRM算法需要大量耗时以构建无撞点网络图的问题，引进一种快速构形空间构建算法，避免PRM算法在构建网络图时所需要的复杂碰撞检测。实验结果表明，改进算法能够快速、有效地为平面2R机械臂和三维空间高自由度机械臂进行避障路径规划。

1 PRM算法的融合改进

根据PRM算法的基本原理不难发现，该算法路标图的质量取决于随机采样点的个数。若将PRM算法应用于高自由度机械臂避障路径规划，那么在构建路标图的过程中，对于任意随机采样点都需要检测该采样点所对应的机械臂位姿与工作空间障碍物的碰撞情况。而空间三维模型的碰撞检测问题十分复杂，且计算量巨大。为保证PRM算法的性能，本文引入构形空间构建算法对其进行改进，通过构形空间构建算法快速建立明确的构形空间，使PRM算法采样点可行性检测由复杂三维模型碰撞检测转化为简单的查表操作和布尔运算。

1.1 快速构形空间构建算法

1.1.1 碰撞查询数据库的构建

在机械臂路径规划研究中，机械臂实际运动所处的空间一般称为工作空间W，以机械臂各关节角度值为坐标所建立的空间通常称为机械臂的构形空间C。因此，工作空间W中的障碍物可在空间C中映射为一个不可达区域，通常称为构形空间障碍物Cobs。空间障碍物Cobs在空间C中的补集通常称为自由区域Cfree。若空间C中点cs和点cg分别表示机械臂的初始位姿qs和目标位姿qg，那么，机械臂避障路径规划就转化成在空间C中搜索一条路径Cpath=[cs,c1,…,cn,cg]，且满足Cpath⊂Cfree。ci表示构形空间C中某点的第i轴坐标值。

若n自由度(degree of freedom,DOF)机械臂的第i个关节角度值为qi(mm/(°))，那么该机械臂的某位姿可以表示为q=[q1,q2,…,qn]。

该机械臂构形空间C中与q唯一对应的点可以表示为c=[c1,c2,…,cn]，且满足qi=ci。

将机械臂工作空间W分割成各个方向尺寸均为dw(mm)的离散化单元wi，那么分割后的工作空间可以表示为W，其中单元序号i为

i=i1+i2+…+in-(n-1)+(hn-1)·(in-1-1)+

(hn·hn-1-1)·(in-2-1)+…+

(hn·hn-1·…·h2-1)·(i1-1)

(1)

式中in为离散单元在空间中的坐标值,hn为空间最大坐标值。

对于工作空间任意离散单元wi，通过计算获得所有与该单元相交的机械臂位姿qwi={q|wi∩Aq≠0}。qwi为工作空间某点状障碍物处于离散单元wi中时，构形空间C中所对应的障碍物模型,Aq为机械臂A处于位姿q状态。

如图1所示为x，y轴范围均为[-100,100]mm的二维工作空间W，该空间被分割成400个尺寸一致的正方形单元，图中灰色正方形为离散单元w125。图2表示单元w125在机械臂构形空间C中的映射模型qw125。

图1 离散化的二维空间和机械臂模型

图2 离散单元在构形空间中的映射模型

若能够计算获得任意离散单元wi所对应的qwi，则qwi的集合就构成了碰撞查询数据库(collision query database,CQD)，即

CQD={q|Aq∩W≠0}={q|Aq∩(∪wi)≠0}

=∪{q|Aq∩wi≠0}=∪(qwi)

(2)

为构建CQD，以dc为关节步长遍历机械臂位姿，若某位姿q对应的机械臂三维模型与单元wi发生干涉时，则将该位姿数据q=[q1,q2,…,qn]保存至离散单元序号所对应的数据空间di中。对工作空间W中的所有离散化单元完成上述遍历操作后，数据空间di的集合∪di就构成了CQD。

在离线阶段建立CQD，该数据库中存储了机械臂工作空间中任意离散单元在构形空间C中的映射模型，在线阶段只需要根据空间障碍物所占据的微小单元对应的序号检索CQD即可快速构建障碍物在空间C中的映射模型。

1.1.2 机械臂碰撞检测模型及碰撞检测算法

在建立CQD的过程中需要对机械臂与工作空间离散单元wi之间的碰撞关系进行检测。由于采摘机械臂曲面造型较为复杂，若以精确的曲面模型为基础进行碰撞检测，CQD建立速度过慢，难以满足使用性能要求，因此,需要对机械臂模型进行一定简化，以提高碰撞检测速度。

方向包围盒(oriented bounding box，OBB)作为机械臂常见的简化模型，其模型拟合程度较高，碰撞检测算法成熟。因此，本文采用OBB模型拟合机械臂精确三维模型。

为了在基础坐标系Tbase中明确描述OBB模型的尺寸和空间位姿，在OBB模型上以相互平行的两个面的中心o1和o2为原点分别建立局部坐标系Ta和Tb，且局部坐标系Ta和Tb的各坐标轴与OBB模型对应棱边平行。

在建立了局部坐标系的基础上，通过齐次变换矩阵可以将表示在局部坐标系中的点的坐标转换为在基础坐标系中表示的点的坐标。

齐次变换矩阵的一般形式为

(3)

式中T为坐标系TA与坐标系TB的齐次变换矩阵,R为姿态变换矩阵,P为位置变换矩阵。

如图3所示为简化后机械臂碰撞检测模型。

图3 采摘机器人机械臂碰撞检测三维模型

由于机械臂精确三维模型被简化成OBB模型，而且工作空间被分割成各边长尺寸一致的离散立方体单元，因此，机械臂与工作空间单元之间的碰撞检测问题可以等效为OBB-OBB碰撞检测问题。

Gottschalk在文献[16]中系统地阐述了采用轴向投影方法确定空间OBB模型相交性的分离轴算法(separating axis theorem，SAT)。如图4所示为一对平面OBB模型。

图4 SAT算法在平面OBB模型中的应用

对于三维空间中任一对OBB模型，只需对15条分离轴进行测试，即可确定两者之间的碰撞关系。这15条分离轴中有6条为两OBB模型不同方向面的法线，另外9条为两OBB模型棱边的叉乘组合。若两OBB模型在15条潜在轴线上的投影都相交，则可以确定该对OBB模型必然相交。

1.1.3 索引CQD数据库建立构形空间

在线阶段机器人双目视觉系统所获得的障碍物点云模型为避障路径规划提供障碍物信息。障碍物点云模型中任一点obsi所占据的工作空间离散单元wi在x轴方向序号sx为

sx=(x-xmin)/dw

(4)

式中x为该点在x轴方向的坐标值，xmin为工作空间在x轴方向的最小坐标值，同理可以求取离散单元wi在y和z轴方向上的序号。

之后，通过式(1)求取障碍物点云占据的工作空间单元序号，并通过序号索引CQD可以快速建立与当前障碍物模型对应的明确构形空间。

1.2 融合构形空间的PRM算法

为了避免PRM算法复杂的碰撞检测，提高避障路径规划效率，先快速建立当前场景构形空间， PRM算法在判断随机采样点是否可行的过程中，只需要进行简单的布尔运算。融合了构形空间构建算法的PRM算法主要步骤包括：

1)在离线阶段构建CQD。

2)将工作空间障碍物Wobs分割成离散单元集合wobs。

3)根据离散单元集合wobs中的单元序号i检索CQD，建立与当前场景对应的明确构形空间C。

4)在构形空间C中随机生成采样点c，通过计算求得该采样点所占据的构形空间单元ck及其序号k。

5)根据序号k检索构形空间C，确定单元ck是否满足条件ck∈Cfree：若不满足该条件，则重复步骤(4)、步骤(5),重新生成采样点并判断其可行性；若满足条件，则将采样点c加入到概率地图路标点集合V中。

6)通过欧拉公式计算获得集合V中与采样点c邻近的节点集合M。

7)点c与M中任意点m的连线按一定步长分割成若干节点，利用节点占据的单元序号来检索构形空间C以确定采样点c与点m之间连线e是否满足条件e∈Cfree。

8)以步骤(7)的方法遍历集合M中的所有点，并在与采样点c相关联的数据空间中存储M中所有满足条件e∈Cfree的节点序号。

9)当集合V中点的个数达到设置的阈值时，将机械臂的初始构形qs和目标构形qg在构形空间C中所对应的点cs和点cg通过最短欧拉距离连接到集合V中。

10)最后通过Dijkstra算法从集合V中搜寻一条连接点cs和点cg的距离最短路径，完成避障路径规划。

2 仿真结果与分析

2.1 平面2R机械臂避障路径规划

为验证本文所提算法的有效性和可行性，首先将算法应用于一种平面2R机械臂的避障路径规划，再将算法扩展应用至高自由度采摘机器人机械臂避障路径规划问题。图5为平面2R机械臂及其避障路径规划场景俯视图。

图5 平面2R机械臂避障路径规场景俯视

根据表1中的工作空间单元尺寸参数，可将实验场景中的工作空间划分成400个离散工作空间单元。

表1 平面2R机械臂避障路径规划CQD参数

通过式(1)可以获得工作空间障碍物所占据的5个单元序号为115，147，235，333，348。

同样，可将图6所示的构形空间划分成5 198个离散构形空间单元。利用遍历的方法建立CQD数据库后通过工作空间障碍物序号对其进行索引，即可获得所有构形空间障碍物单元序号以及单元总个数，其中单元总数为1 713。

图6 利用PRM算法在构形空间搜寻避障路径

利用MATLAB软件在构形空间中绘制障碍物单元，即可构建如图7所示的平面2R机械臂的明确构形空间。

图7 平面2R机械臂的避障运动

图中字母S和G表示的构形空间单元分别对应工作空间中平面2R机械臂起始位姿和目标位姿。

在此基础上，通过PRM算法在构形空间中成功搜寻到一条连接代表机械臂起始位姿和目标位姿的无碰撞路径，该路径中各节点的坐标可以构成一组关节角度序列。PRM算法主要控制参数为:算法迭代次数阈值为500，路标地图节点数为100，节点连接距离阈值为80，节点连线分割距离为50。

利用无碰撞路径节点的坐标驱动2R机械臂各关节，可以获得如图7所示的机械臂运动过程。可以观察到平面2R机械臂从起始位姿运动到了目标位姿，且在此过程中未与工作空间中的障碍物发生碰撞。

2.2 6自由度采摘机器人机械臂避障路径规划

为了进一步验证本文所提出的算法在高自由度(degree of freedom,DOF)机械臂避障路径规划中的有效性，将改进算法应用至6 DOF机械臂避障路径规划场景中。

由于6 DOF采摘机器人机械臂对应的构形空间达到六维，因此，要绘制六维构形空间以描述路径搜索过程具有相当大的困难，这里仅对避障路径规划完成后机器人的运动过程进行可视化操作。表2为6DOF机械臂避障路径规划相关实验参数。

表2 6 DOF机械臂避障路径规划CQD参数

如图8所示，为利用本文算法所获得的6自由度机械臂避障运动状态。可以看出，本文算法所求得的关节角度序列安全，无碰撞地将机械臂末端执行器从起始位姿移动到目标采摘位姿。

分别利用传统PRM算法和本文所提出的改进算法在当前场景下进行50次避障路径规划，算法路径规划平均耗时分别为0.81 s和 0.63 s，相比传统PRM算法,改进算法的耗时降低了约22.2 %，可见改进算法规划速度提升较为明显。值得指出的是，改进算法在构建CQD的过程中耗时约为5.36 h，但是由于CQD的建立是在离线阶段完成的，不会对在线避障路径规划耗时造成不利影响。

图8 6 DOF采摘机器人机械臂避障运动状态

3 结 论

针对概率地图法在避障路径规划中存在的不足，本文通过融入快速构形空间构建算法对其进行了改进。结果表明，本文的改进算法相比传统PRM算法速度提高22.2 %，能够满足复杂环境下机械臂的实时避障路径规划要求。