复杂环境建模与机器人避障规划研究

张智, 邹盛涛, 董然, 张乐乐, 郭文县

(哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001)



复杂环境建模与机器人避障规划研究

张智, 邹盛涛, 董然, 张乐乐, 郭文县

(哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001)

针对复杂环境下的机械臂作业问题，提出了一种基于空间凸多面体集模型的避碰路径规划方法。首先，给出一种利用空间凸多面体模型的复杂物体建模方法，该方法具有一套树型递归结构框架，能精确逼近任意形状的物体，并支持子物体之间的自由度连接，从而能恰当表示如多关节机械臂和电脑桌等带有运动自由度(关节转动、柜门开合、抽屉拉出等)的物体；然后，设计了一种基于直线扫掠体序列的碰撞检测方法，它能快速、准确地进行连续运动的机械臂与空间物体间的碰撞检测；最后，采用遗传算法实现了机械臂的避碰路径规划。在桌面上摆放多个复杂形状物体后，机械臂顺利完成了避障作业规划。仿真结果验证了算法的有效性，表明了其实用价值。

机械臂;凸多面体;碰撞检测;遗传算法;避碰路径规划

机器人是当代科技发展的重要成果，代表了机电一体化和自动化技术的最高成就，其中工业机器人(机械臂)已经发展为现代高端制造业的支撑技术，在材料焊接、喷涂、物品搬运、物料切割等多个行业得到了广泛的应用[1-2]。

当机器人应用于社会化的环境时，其面临的环境理解及作业规划问题要远比工业机器人复杂[3-4]，真实环境下机器人和环境物体往往形状复杂并且位姿随时间动态变化，没有固定规律，而且场景中物体数量极多，物体的摆放相对自由。因此，如何对复杂环境物体进行建模，并针对性地设计机器人避障作业规划算法，就成为了机器人社会化过程的一个关键问题。Hasan[5]将机械臂用轴线段表示，而将障碍物用圆柱包围盒和椭球包围盒表示，在避碰规划方面利用人工神经网络算法实现避碰，但是该方法需要对神经网络进行大量训练，难以适应动态环境；祁若龙等[6]将机械臂的关节简化为轴线段，将障碍物简化为球体，通过增加球体的数量来提高模型的精度，在避碰规划方法将路径分段描述，避开一个障碍需要增加一个中间点，将未知的中间点利用遗传算法优化实现避碰。贾庆轩等[7]将环境障碍物利用球体包围盒进行近似建模，通过障碍物与机械臂的碰撞条件建立自由空间，进而采用A*算法在自由空间中对避碰路径进行搜索。

总的来说，国内外的相关研究主要集中在机械臂避碰路径规划方法的研究：遗传算法、神经网络法、A*算法等，而对于环境建模主要采用简单规则物体对形状进行近似，但是这种方法极大浪费了机器人避碰规划的可用空间。

本文算法力求适用于任意复杂环境，为了准确的对复杂环境物体进行建模，本文提出一种基于树型凸多面体集的物体描述方法，该方法将多个凸多面体组合近似为单个刚体，然后利用多个刚体组合成单个子物体，再利用多个子物体递归组合成为树型结构物体，该方法支持各级之间(物体与子物体、子物体(或刚体)与凸多面体)的相对自由度连接。为了有效实现机械臂连续运动情况下的避碰运动规划，提出对机械臂的连续路径进行采样，并在相邻采样点之间构造近似轨迹包围盒，利用遗传算法建立了机械臂避碰规划问题的适应度函数，采用关节空间内若干关键点对避碰路径进行编码，最后通过仿真和实验对本文提出的复杂环境建模方法以及避碰方法进行了验证。

1 复杂环境物体建模技术

环境建模应该满足以下两点：一是环境应该由形状紧密的包围盒构建；二是需要采用一种有效的数据结构对其进行管理，以便描述物体间的逻辑关系，并且支持自由度变换，方便物体的坐标变换。本节将提出一种基于树型凸多面体集的物体描述方法，并将采用这种方法描述的物体称为树型结构物体。树型结构物体分为物体、子物体和凸多面体三级，下面分别从这三个层次依次介绍。

1.1 凸多面体表示

将凸多面体表示为点、面、棱3种元素的集合：

(1)

1.2 刚体表示

刚体用来表示机械臂或其他物体的子部件，内部不带有任何相对运动自由度，如机械臂的底座或某个关节、桌子的抽屉、桌体框架等，每个刚体由于形状复杂，难以用一个凸多面体精确逼近，因此采用凸多面体集合表示：

(2)

图1 刚体的凸多面体集表示Fig.1 Polytopes-representation of rigid body

1.3 树型结构物体表示

物体的表示采用树型结构，即每个物体可由若干个子物体构成，每个子物体又可包含若干子物体，依次递归表示，直到叶子节点，而各叶子节点均为刚体，物体表示如下：

(3)

1.4 机械臂工作环境物体的树型结构表示

基于1.3节给出的物体表示模型，本节将举例给出机械臂及电脑桌的表示方法。

1.4.1 机械臂的凸多面体集表示

图2 机械臂的树型结构物体表示Fig.2 Tree object representation of the manipulator

同样，W1～W6子物体所包含的Dof(Γi,ηi)分别用来描述各关节的转动关系，由表1中的D-H参数确定，例如W1，取Γ1=3，表示绕z轴旋转(见1.3节定义)，η1动态跟随关节1转角θ1变化。

在完成各层子物体和刚体的分解后，还需按照1.2节的描述，将每一级刚体(即每个关节)形状分解为若干个凸多面体的组合，分解完成后，即实现了对Reinovo型机械臂的空间凸多面体集描述，最终结果如图3所示。

表1 机械臂的D-H参数表

图3 机械臂的凸多面体集表示Fig.3 Polytopes-representation of the manipulator

1.4.2 电脑桌的凸多面体集表示

采用1.1～1.3节描述的树型物体表示方法，对作业环境物体中典型的物体电脑桌进行描述，子物体结构图如图4所示。图中的左柜门系统W3、左抽屉系统W4、右抽屉系统W5是相对于上级物体具有运动自由度的，分别代表了柜门的旋转和抽屉的拉出。

图4 桌子的树型物体表示Fig.4 Tree object representation of the desk

将桌子的子刚体G0～G6中各子刚体继续分解为若干个凸多面体(分解过程略)后，即可得到桌子的凸多面体集表示，最终结果如图5所示。

图5 桌子的凸多面体集表示Fig.5 Polytopes-representation of the desk

1.4.3 树型结构物体中的凸多面体坐标变换

前面各节给出了基于空间凸多面体集的物体形状描述，以及基于树型结构的多自由度物体表示方法，为了实现任意物体间的碰撞检测，在此需给出坐标变换方法，将某树型物体中所有子刚体中的所有子凸多面体变换至世界系表示。

算法1 物体的子凸多面体坐标变换

3)在完成当前首节点所有子物体遍历后，算法结束。

算法1构成了递归调用模式，算法启动后会自动遍历树型结构的物体，并对叶子节点的子刚体，自动遍历其下属的子凸多面体，直到完成所有凸多面体的变换。

需说明的是，建模过程树型结构的组织，凸多面体划分等过程目前是人工完成的，也可将本文算法与凸多面体自动分割等算法(本文不讨论)相结合，来进一步提高建模效率。

2 机械臂的碰撞检测算法

2.1 树型结构物体的层次包围盒

为了提高碰撞检测的执行效率，快速排除不可能发生碰撞的物体，一种有效的方法就是层次包围体法[8]。第1章介绍树状结构物体的凸多面体坐标变化，然后利用变化后的凸多面体分别构造包含

物体层、刚体层和凸多面体层的层次包围盒。本文采用自底向上的方法构造层次包围盒。

1)底层包围盒：本文利用树型结构物体的描述方法对机械臂工作场景中所有的物体进行描述，由于树状结构物体均由凸多面体集构成，因此底层凸多面体即是底层包围盒。

2)中层包围盒：包围体层次树的中层对应树型结构物体中的刚体，对于机械臂而言，中层刚体代表关节，对于桌子而言，中层刚体代表抽屉、柜门等部分。刚体由多个凸多面体组成(如关节由多个凸多面体近似)，依次遍历刚体中的每一个凸多面体，将得到实时的世界坐标系下凸多面体顶点数据，采用AABB构造中层包围盒。

3)顶层包围盒：包围体层次树的顶层对应树型结构物体中的物体，对于机械臂而言，顶层物体对应机械臂本身，对于桌子而言，顶层物体对应桌子，遍历物体中刚体，并遍历每一个刚体的AABB包围盒，可得到所有AABB在世界坐标系下的顶点坐标，采用AABB包围盒构造顶层包围盒。

2.2 机械臂连续运动碰撞检测方法

针对机械臂离散时间碰撞检测容易导致检测遗漏等不精确问题，本文的解决方法提出对机械臂的连续路径进行采样，并在相邻采样点之间构造近似轨迹包围盒[9]，其中树型结构物体中单个凸多面体的轨迹包围盒构造示意图如图6所示。

图6 凸多面体的轨迹包围盒Fig.6 Path swept volumes of the polytopes

图7 机械臂的碰撞检测算法Fig.7 Collision detection of the manipulator

3 机械臂遗传避碰路径规划

3.1 路径编码设计

由于关节空间不存在机械臂奇异性问题，因此，本文机械臂路径编码将在关节空间进行，具体方法是在关节空间中选取N组关节角度序列作为机械臂连续路径运动的中间点，每个路径点将由六个关节转角构成的六维矢量表示，最终在遗传算法中采用的染色体的编码方式为

如图8，染色体编码中每一组角度矢量，均对应着一种机械臂空间位姿(中间点)，将若干个空间位姿点通过插值算法连接，便可形成空间连续运动路径，当搜索过程不断调整中间点的位置时，即可得到有效的避碰路径。中间点个数N应选取适当，N过大会导致搜索空间太大，算法收敛速度变慢，N太小将无法找到可行解，一般应根据规划任务的复杂性及规划实时性要求折中选取，本文取N=3，插值方法为关节空间线性插值，由于每个中间点均在6维空间变化，这样编码后仍能保证机械臂具有较强的避碰规划能力。

图8 机械臂路径点示意图Fig.8 Path points of the manipulator

3.2 路径解码设计

路径解码就是根据中间点还原一条连续运动路径，并且保证路径的唯一。路径设定的开始点、结束点以及N个中间点构成N+2组六维位姿矢量，我们需要依次连接相邻的两个矢量构成子路径片段，最终连接各子路径还原整个路径。针对任意给定的两个位姿，本文将推导出路径片段的还原方法。

式中wj.max为每个关节允许转动的最大角速度。

3.3 适应度函数设计

适应度函数综合考虑机械臂运动过程的时间、能量、碰撞、限位等约束，对机械臂定义适应度函数表示为

适应度函数是充分考虑机械臂运动过程中的各种指标后加权得到的，wi(i=1,2,3,4)是各项指标对应的加权系数。

f1(θ)是机械臂沿路径θ(t)运动的时间，对于N个中间路径点的机械臂，其运动时间为

f2(θ)表征了机械臂沿路径θ(t)由起点经过中间点到终点所消耗的能量，对于N个中间路径点的六自由度机械臂，其能量[6]为

式中：αj是机械臂各关节变化的权重，反映了机械臂各关节运动代价的比例关系。

f3(θ)表示机械臂沿路径θ(t)运动时与自身或环境障碍发生碰撞程度。具体碰撞检测时需对机械臂连续路径进行离散采样，对每个采样时刻对应的关节状态分别作碰撞检测，并以发生碰撞的采样时刻数量collsionTimes来表示碰撞程度：

f3(θ)=collsionTimes

f4(θ)代表机械臂沿路径θ(t)运动时关节角位移限制，对于N个中间路径点的六自由度机械臂，其代价为

式中：θj.min、θj.max代表关节最小和最大限位，fabs(θj.m-θj.max)

3.4 算法步骤

综合以上算法，总结机械臂遗传避碰路径规划算法的流程图如图9所示。

图9 机械臂路径规划方法流程图Fig.9 The path planning procedure of the manipulator

4 机械臂的避碰路径规划仿真

为了考察本文算法在实际生活中的应用情况，利用VC++开发机械臂避碰作业仿真系统，模拟实物机械臂工作情况，本文仿真实验均在IntelXeonCPU2.4GHz, 2GB内存计算机上完成。为了验证本文相关算法，本文设计了机械臂抓取物件时的避碰实验，任务描述如图10所示。

进化过程中，根据本文的适应度函数对机械臂的避碰路径进行评价，在遗传算子的作用下种群逐渐趋于稳定，算法迭代30次，适应度函数的收敛曲线如图12所示。

图10 机械臂避障任务描述Fig.10 Task description of the manipulator

图11 抓取物件避碰作业任务时仿真过程Fig.11 The path planning results of the simulating manipulator

图12 最优解收敛曲线Fig.12 The convergent curve of optimal solution

从图11和图12可以看出，利用本文基于遗传算法的机械臂避碰路径规划方法顺利地实现了机械臂的避碰路径规划，保证了机械臂顺利地避开复杂障碍并到达期望位姿。

本文将仿真系统生成的角度序列(一般在70组左右)发送到实物机械臂，实现了实物机械臂抓取物件时的避碰路径规划，实物机械臂避碰过程如图13所示。

图13 避碰作业任务时实物运行过程Fig.13 The path planning results of the real manipulator

为验证算法的统计性能，在仿真系统中进行多次规划仿真试验，试验分6组，任务设定均与前文相同，但每组对应不同的物体摆放位置，对每组各进行20次仿真，各组仿真的成功率、迭代次数及平均时间如表2所示。

表2 机械臂的数据统计

通过实验观测可知，实物机械臂依据算法规划的轨迹，成功地完成了抓取物件的避碰作业任务。需说明的是，目前实物场景中物体的位置为人工测量并输入到仿真系统，如要提高系统自主规划能力，需将本文方法与视觉目标识别和定位等技术进一步结合方可。另外，本文对所述方法进行连续20次运行，实验过程中仿真机械臂全部避开障碍，稳定可靠且计算效率较高。

3 结论

本文针对机械臂在复杂环境中的作业问题，从环境建模、碰撞检测和避碰规划方法等方面进行了深入、系统地研究，得到如下结论：

1)基于树型凸多面体集的物体建模方法，是一种适用性较强并且颇具研究潜力的空间环境建模方法；

2)基于该方法，构造了树型结构物体的层次包围盒模型，实现了机械臂连续运动情况下的碰撞检测算法，有效地实现了在放有各类复杂形状物体的桌面上的机械臂作业路径规划；

3)随着后续研究工作深入，本文环境建模方法将在机械臂的复杂环境作业方面发挥更大的作用。

[1]蔡自兴. 机器人学基础[M]. 北京: 机械工业出版社, 2009: 163-166.

CAI Zixing. Fundamentals of Robotics[M]. Beijing: China Machine Press, 2009: 163-166.

[2]王田苗, 陶永. 我国工业机器人技术现状与产业化发展战略[J]. 机械工程学报, 2014, 50(9): 1-13. WANG Tianmiao, TAO Yong. Research status and industrialization development strategy of Chinese industrial robot[J]. Journal of mechanical engineering, 2014, 50(9): 1-13.

[3]SEKMEN A, CHALLA P. Assessment of adaptive human-robot interactions[J]. Knowledge-based systems, 2013, 42: 49-59.

[5]HASAN A T, ISMAIL N, HAMOUDA A M S, et al. Artificial neural network-based kinematics jacobian solution for serial manipulator passing through singular configurations[J]. Advances in engineering software, 2010, 41(2): 359-367.

[6]祁若龙, 周维佳, 王铁军. 一种基于遗传算法的空间机械臂避障轨迹规划方法[J]. 机器人, 2014, 36(3): 263-270. QI Ruolong, ZHOU Weijia, WANG Tiejun. An obatacle avoidance trajectory planning scheme for space manipulators based on genetic algorithm[J]. Robot, 2014, 36(3): 263-270.

[7]贾庆轩, 陈钢, 孙汉旭, 等. 基于A*算法的空间机械臂避障路径规划[J]. 机械工程学报, 2010, 46(13): 109-115.

JIA Qingxuan, CHEN Gang, SUN Hanxu, et al. Path planning for space manipulator to avoid obstacle based on A*algorithm[J]. Journal of mechanical engineering, 2010, 46(13): 109-115.

[8]马登武, 叶文, 李瑛. 基于包围盒的碰撞检测算法综述[J]. 系统仿真学报, 2006, 18(4): 1058-1061, 1064.

MA Dengwu, YE Wen, LI Ying. Survey of box-based algorithms for collision detection[J]. Journal of system simulation, 2006, 18(4): 1058-1061, 1064.

[9]ERICSON C. Real-time Collision Detection[M]. Singapore: CRC Press, 2004: 36-48.

[10]GILBERT E G, JOHNSON D W, KEERTHI S S. A fast procedure for computing the distance between complex objects in three-dimensional space[J]. IEEE journal on robotics and automation, 1988, 4(2): 193-203.

[11]张智, 邹盛涛, 李佳桐, 等. 六自由度机械手三维可视化仿真研究[J]. 计算机仿真, 2015, 32(2): 374-377, 382.

ZHANG Zhi, ZOU Shengtao, LI Jiatong, et al. Three-dimensional visual simulation research on six degrees of freedom manipulator[J]. Computer simulation, 2015, 32(2): 374-377, 382.

Research on complex environmental modeling and robot operation planning

ZHANG Zhi, ZOU Shengtao, DONG Ran, ZHANG Lele, GUO Wenxian

(College of Automation, Harbin Engineering University, Harbin 150001, China)

For the operation of a manipulator in a complex environment, a collision avoidance path planning method on the basis of spatial convex polytope model was developed. First, a complex object modeling method utilizing spatial polytope model was proposed. The approach was characterized by a framework with a tree structure. It can precisely approximate the outline of an arbitrary object and support DOF linking among the affiliated bodies. Therefore, a moving objective with certain DOF (joint rotation, opening/closing of cabinet door, pull-out of drawer, etc.), such as a multi-joint robot arm and a computer table, can always be represented appropriately. Then, a collision detection method featuring a sequence of linear-swept volumes was designed. It permits efficient and accurate collision detections between the continuously moving manipulator and the environmental objectives involved. Finally, a collision avoidance path planning was realized by using the genetic algorithm. In simulations, the operational planning was implemented successfully for the case where some tasks are required to be finished on a desktop with multiple complex-shaped objects settled. Experimental results demonstrate the effectiveness and utility value of the proposed method.

manipulator; epolytopes; collision detection; genetic algorithm; collision avoidance path planning

2015-05-15.

日期：2016-09-14.

国家自然科学基金项目(61104037, 61304060); 中央高校基础业务费项目(HEUCF041307, HEUCFX41304); 国家科技合作专项项目(2013DFR10030).

张智(1981-), 男, 副教授.

张智, E-mail： neverbadzz@163. com.

10.11990/jheu.201505048

网络出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20160914.1426.004.html

TP24

A

1006-7043(2016)10-1373-08

张智, 邹盛涛, 董然，等. 复杂环境建模与机器人避障规划研究[J]. 哈尔滨工程大学学报， 2016, 37(10): 1373-1380.

ZHANG Zhi, ZOU Shengtao, DONG Ran, et al. Research on complex environmental modeling and robot operation planning[J]. Journal of Harbin Engineering University, 2016, 37(10): 1373-1380.