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

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

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

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

1 复杂环境物体建模技术

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

1.1 凸多面体表示

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

$V=\{\{{{P}_{1}},{{P}_{2}},\ldots ,{{P}_{m}}\};\{{{F}_{1}},{{F}_{2}},\ldots ,{{F}_{n}}\};\{{{L}_{1}},{{L}_{2}},\ldots ,{{L}_{k}}\};\{{{O}_{v}}\}\}$

(1)

式中：V表示凸多面体、P表示顶点、F表示面、L表示棱边，m、n、k分别为凸多面体点、面、棱的个数，凸多面体的点、面、棱的位置坐标均相对于其本体坐标系O_v定义，O_v由该凸多面体的六个自由度参数决定，表示了该凸多面体的整体空间位姿。

1.2 刚体表示

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

$G=\left\{ \left\{ {{V}_{1}}, \right. \right.{{V}_{2}},...\left. {{V}_{h}} \right\};\left\{ O \right.\left. \left. _{g} \right\} \right\}$

(2)

每个刚体是h个凸多面体的集合，对于集合中的任意凸多面体V_i，其对应的坐标系O_vi定义为该凸多面体在本刚体坐标系O_g下的局部坐标表示，而O_g则用来表示整个刚体相对于更上一层物体的坐标表示，采用这样的级联坐标表示，既能保证物体各部件及子凸多面体间具有清晰的相对位置关系，也能够通过坐标系间的级联变换，实时获得任意凸多面体相对于世界系的位姿。如图 1，以机械臂某关节为例，利用多个凸多面体的组合来逼近刚体形状。

1.3 树型结构物体表示

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

$W=\{\{{{W}_{{{S}_{1}}}},{{W}_{{{S}_{2}}}}...{{W}_{{{S}_{N}}}}\};Dof\left( \Gamma ,\eta \right);\{{{O}_{w}}\}\}$

(3)

每个物体为多个子物体的集合，对于某个子物体W_Si，N_g如果不是叶子节点，则仍可包含若干个子物体，如果为叶子节点，则W_Si退化为刚体G_i，不论W_Si或G_i，其对应的坐标系O_wi或O_gi均定义为该刚体或子物体在上一级物体坐标系O_w下的局部坐标表示，直到最终的根节点，其坐标系O_wR定义为整个物体相对于世界坐标系O_E的坐标表示。

式(3)中Dof(Γ,η)为该子物体相对于上一级物体的自由度表示，代表了一个转动自由度或平动自由度，对于转动自由度，η取值代表转动角度θ，对于平动自由度，η取值代表平移量τ，其中Γ的取值代表自由度类型，取1、2、3时分别代表绕3个坐标轴转动，取4、5、6时分别代表沿3个坐标轴平动，此处的坐标轴均指该运动子物体的坐标系中的坐标轴，虽然这种表示将子物体的运动自由度限制在某个坐标轴上，但由于每个子物体的O_wi的存在，使得子物体局部坐标系可相对上一级物体任意设定，一般总能把物体间的运动方向或转轴变换至沿着运动子物体的某坐标轴方向，因此，该物体表示模型能够适应多轴机械臂及各类复杂物体，并且还能根据任务的变化，实现两个物体的自动附加和分离。

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

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

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

采用1.1至1.3节描述的树型物体表示方法，对Reinovo型机械臂进行描述，其子物体结构图如图 2所示，其中G₀表示基座，G₁~G₆表示6个关节，W₁~W₆是为了实现机械臂各关节间运动连接而引入的虚拟子物体，W₁表示关节1至关节6构成的整体系统，W₂表示关节2至关节6构成的整体系统，依次类推。各子物体W_i以及G_i所包含的坐标系表示O_wi以及O_gi分别用来描述每一级子物体相对于上一级子物体的坐标系偏移。

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

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

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

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

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

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

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

设世界系为O_E，某树型物体根节点为{O_wRoot}，并设其内部共包含N_W个子物体，N_S个子刚体，N_V个凸多面体，设物体根节点变换至世界系的变换阵_R^ET，子物体集中第i个子物体相对其上一级物体的变换阵位_W^WTⁱ，刚体集中第i个子刚体相对于其上级物体的变换阵位_g^WTⁱ，凸多面体集中第i个子凸多面体相对于上一级刚体的变化阵为_v^gTⁱ。对于_v^gTⁱ，可直接根据该凸多面体相对于上级刚体的坐标系{O_vi}的六自由度参数写出，而对于_W^WTⁱ以及_g^WTⁱ，则需要同时考虑运动自由度的取值，以_g^WTⁱ为例，求解：

${{^{W}}_{g}}{{T}^{i}}{{=}^{W}}_{g}{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{T}}^{i}}{{^{g}}_{Dof}}{{T}^{i}}$

式中：${{^{W}}_{g}}{{{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{T}}}^{i}}$为坐标系{O_gi}所确定的变换阵，${{^{g}}_{Dof}}{{T}^{i}}$由本刚体相对上级物体的自由度参数Dof(Г_gi，η_gi)确定，根据上文定义的自由度取值含义以及坐标变换理论可直接写出${{^{g}}_{Dof}}{{T}^{i}}$(略)。

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

1) 建立临时首节点和临时坐标变换阵T，并将首节点设为物体根节点，坐标变换阵设为物体根节点至世界系的变换阵_R^ET；

2) 对于当前临时首节点，如已为叶子节点，则该节点为子刚体，遍历该子刚体中所有的子凸多面体，访问每个凸多面体V_i时，将当前的临时变换阵T右乘当前凸多面体至上级刚体的变换阵gvTⁱ后，作为本凸多面体的新的临时变换阵T_Vi，此时T_Vi中恰记录了本凸多面体到世界系的变换阵，将凸多面体中所有的点、面、棱坐标均左乘变换阵T_Vi后即可其变换至世界系；

3) 如当前临时首节点不为叶子节点，则说明该节点仍链接了若干个下层子物体(子刚体)，则遍历其所有子物体(子刚体)，对于每个子物体或子刚体，将当前临时变换阵T右乘该子物体或子刚体相对上级物体的变换阵_W^WTⁱ或_g^WTⁱ，并将求得变换阵作为新的临时变换阵入口，将当前子物体或子刚体作为新的首节点，转2)；

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

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

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

2 机械臂的碰撞检测算法 2.1 树型结构物体的层次包围盒

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

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

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

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

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

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

本文利用得到的所有扫掠体的并集来逼近凸多面体V_A沿曲线Г_A运动形成的轨迹区域，将具有较好的真实度。另外，观察图 6可知，由于凸多面体的姿态变化，利用扫掠体取并集后的区域仍存在小的缝隙(如V′_A(t_k)与V_A(t_k+1)上端的缝隙)，而且Г_A运动区域覆盖的空间会有微小部分超出掠体并集空间，当采样间隔不大时，这样缝隙及空间越界都将变得很小，此外，还可在求取扫掠体并集时将凸多面体尺寸整体微小膨胀(膨胀系数可根据碰撞检测的保守性需求而定)，这样不仅可填满缝隙，同时也将覆盖空间越界区域。对于凸多面体沿直线运动后形成的直线扫掠体，其区域外轮廓仍为一个凸多面体，这样仍可通过凸多面体碰撞检测算法实现两个扫掠体间的碰撞检测。本文凸多面体的碰撞检测采用GJK算法^[10]，最终连续运动状态下凸多面体碰撞检测的碰撞检测流程图如图 7所示。

3 机械臂遗传避碰路径规划 3.1 路径编码设计

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

(θ₁¹,θ₂¹,θ₃¹,θ₄¹,θ₅¹,θ₆¹,···,θ₁^N,θ₂^N,θ₃^N,θ₄^N,θ₅^N,θ₆^N)

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

编码过程中，各关节角度的搜索区间选择也比较重要，在满足避碰需求的情况下应尽量控制每个中间点的分布范围，设给定的机械臂起点和终点对应的关节角度序列分别为{θ_j^START}、{θ_j^END}(如果起点及终点位姿在直角坐标空间给定，需用机械臂运动学逆解方法^[11]解出关节角度)，那么各中间点分布中心可沿着{θ_j^START}、{θ_j^END}在六维角度空间连线上排列：

θ_jⁱ=θ_j^START+(θ_j^END－θ_j^START)×search_jⁱ+δⁱ×p_j/p_MAX

式中：θ_jⁱ为第i个路径点第j个角度；θ_j^START、θ_j^END为起始点及终止点对应的第j个角度，searchⁱ为路径点分布系数，当其取值在0~1等间距变化时，表示中间点的分布中心在六维空间连线上分布，δ_jⁱ为θ_jⁱ的角度搜索范围，决定了机械臂避碰规划时能够绕行的空间区域大小；p_j为θ_jⁱ所对应二进制串代表的十进制数；p_MAX为θ_jⁱ所对应二进制串能代表的最大十进制数，若θ_jⁱ由K位二进制串(q₀,q₁,…,q_K) 表示，则有

$\left\{ \begin{matrix} {{p}_{j}}={{({{q}_{0}},{{q}_{1}},\cdot \cdot \cdot ,{{q}_{K}})}_{2}}={{(\sum\limits_{k=0}^{K}{{{q}_{jk}}}\cdot {{2}^{k}})}_{10}} \\ {{p}_{MAX}}={{2}^{K}}-1 \\ \end{matrix} \right.$

编码时每个关节对应的角度变化范围δ_jⁱ及二进制串长度K可以不相同，对于Reinovo型机械臂，前3关节变化时对机械臂末端位置变化影响较大，因此本文对前3关节取δ_jⁱ∈[－30°30°]，每个角度占2个字节(16 bit)，后3个关节取δ_jⁱ∈[－20°20°]，每个角度占1个字节(8 bit)。

3.2 路径解码设计

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

本文采用关节空间线性插值方法，采用随时间连续变化的关节变量θ(t)描述机械臂的运动路径，θ(t)={θ_j^i,i+1(t)|1≤i≤N,1≤j≤6}，其中θ_j^i,i+1(t)为机械臂从第i个路径点到第i+1个路径点连续路径。若已知两路径点关节角度序列θ_jⁱ和θ_j^i,i+1，θ_j^i,i+1(t)计算方法可表示为

θ_j^i,i+1(t)=θ_jⁱ(t)+(θ_jⁱ⁺¹(t)－θ_jⁱ(t))t/t_f^i,i+1

式中：t_f^i,i+1代表从第i个路径点到第i+1个路径点运动最大时间，为使机械臂各关节能同步运动，t_f^i,i+1采用下式求取：

${{t}^{i,i+1}}_{f}=max\{\frac{|{{\theta }^{i}}_{j}-{{\theta }^{i+1}}_{j}|}{{{w}_{j.}}_{max}}\},1\le j\le 6$

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

3.3 适应度函数设计

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

F(θ)=w₁f₁(θ)+w₂f₂(θ)+w₃f₃(θ)+w₄f₄(θ)

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

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

f₁(θ)=t₁+t₂+···+t_N

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

${{f}_{2}}\left( \theta \right)=\sum\limits_{i=0}^{N}{\sum\limits_{j=1}^{6}{{{\alpha }_{j}}}}{{[{{\theta }^{i+1}}_{j}-{{\theta }^{i}}_{j}]}^{2}}$

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

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

f₃(θ)=collsionTimes

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

${{f}_{4}}\left( \theta \right)=\sum\limits_{i=0}^{N}{\sum\limits_{j=1}^{6}{{{\beta }_{j}}}}{{[{{\theta }^{i}}_{j}-{{\theta }_{j.m}}]}^{2}},{{\theta }^{i}}_{j}\notin \left[ {{\theta }_{j.min}}{{\theta }_{j.max}} \right]$

式中：θ_j.min、θ_j.max代表关节最小和最大限位，fabs(θ_j.m－θ_j.max) ＜fabs(θ_j.m－θ_j.min)时,θ_j.m=θ_j.max,反之取θ_j.m=θ_j.min。

3.4 算法步骤

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

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

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

利用仿真系统对给定实物机械臂避碰任务进行仿真，仿真机械臂的机构尺寸，装配关系与实物机械臂一致，场景中包含一个机械臂、2个大小不同的圆柱模具、2个四棱锥、一个工作平台、一个乒乓球拍。本实验要求机械臂抓取物件(乒乓球拍)后越过障碍物达到安全区域，算法设定参数如下：设定中间路径点个数为N=3，路径点分布系数search⁰=0.25,search¹=0.5, search²=0.75；机械臂末端起始，结束关节角度序列{θ_j^START}、{θ_j^END};适应度函数各项权值w₁=0.04，w₂=0.03，w₃=0.90，w₄=0.03；遗传算法最大迭代次数k_MAX=30，并且选取初始种群规模Pop=50，交叉概率f_c=0.8，变异概率为f_m=0.05。机械臂沿避碰路径运动时的仿真过程如图 11所示。

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

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

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

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

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

3 结论

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

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

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

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