当移动机械臂在室内环境中执行抓取任务时，主要挑战之一是如何高效地避开障碍并且成功抓取目标物。在室内环境较为复杂、障碍物较多的情况下，移动机械臂的抓取规划成功率会受到障碍物的影响，并且抓取规划成功率低也会导致RRT的规划时间增加。因此，移动机械臂首先要预处理环境信息，然后生成并到达优化后的停靠位置，再进行抓取。

随着机器人技术的不断发展，固定位置操作的机械臂不能满足多方面需求，而且机械臂需要安全高效地在动态环境中工作。移动机械臂所面临的环境更加复杂，若移动机械臂搜寻路径效率不高，则会严重影响其工作质量。因此如何快速地规划出一条有效的安全路径极具研究意义与实际应用价值^[1-3]。在较狭窄或较拥挤场所中，移动机械臂需要根据环境中的信息快速获取目标的位置，然后结合机械臂抓取空间的评价标准，得到优化后的底盘停靠位置，最后结合RRT算法避开障碍物，完成目标点抓取任务。

对于移动机械臂避障抓取问题，早期大多数研究都是对移动机械臂进行分散控制，即先控制移动底盘到达目标位置，然后再进行机械臂的运动规划避障抓取。例如，先进行移动底盘导航定位及地图创建^[4-5]，然后进行机械臂运动规划及轨迹生成^[6]；现在有部分研究涉及移动底盘与机械臂的全身控制，通过协调控制移动机械臂缩短控制时间，并且能够实现在狭小空间调整位姿抓取^[7-8]。其中都未提及到该如何结合环境信息、机械臂位置信息和各约束条件，生成物体抓取的最优停靠位置等相关问题。

对于机械臂抓取过程中的避障问题，文献[9]首次提出应用于机器人路径规划的虚拟力场方法——人工势场法，文献[10-11]提出一种基于A*算法的移动机械臂无碰抓取方法，文献[12]在传统蚁群算法的基础上，引入带方向信息的全局启发因子来提高最优路径的搜索效率，能够在较短时间内规划出满足条件的最优路径。文献[13]提出一种改进的快速扩展随机树算法(Rapidly-exploring Random trees, RRT)。文献[14]提出低振荡人工势场−自适应快速扩展随机树(Adaptive Rapidly-exploring Random Trees, ARRT)混合算法进行路径规划。文献[15]设计基于双向RRT算法的运动规划方法，有效地解决仿人机器人的抓取操作问题。上述算法都未考虑将机械臂连杆与障碍物碰撞条件加入避障。

本文对环境中目标物及障碍物的识别、定位^[16-17]采用的是aruco码^[18]标记以及在动作捕捉系统下粘贴感应点的方法。在不同的约束条件下，如何找到移动机械臂优化后的停靠位置，提高移动机械臂抓取物体的规划成功率是主要挑战。本文重点研究如何在更短时间内提高移动机械臂在多障碍物环境下的避障抓取规划成功率，主要贡献如下：(1) 分析了机械臂结合夹爪碰撞约束条件且无障碍物的抓取空间；(2) 分析了移动底盘、机械臂末端以及机械臂连杆与障碍物之间碰撞的多约束条件；(3) 提出了移动机器人在有障碍物的环境下优化停靠位置的计算方法，有效地提高了移动机器人进行抓取任务规划的效率。

1 硬件规格

图1为研究中使用的移动机械臂，包括加拿大Clearpath公司生产的ridgeback移动底盘、优傲机器人公司生产的UR5协作式机械臂和Robotiq公司生产的三指夹爪。其中ridgeback移动底盘装有4个麦克纳姆轮，为全方位移动小车；UR5为六关节机械臂，末端负载为5 kg；Robotiq夹爪为自适应三指夹爪，可抓取多种不同形状物体。Realsense相机安装在机械臂末端，其还使用了动作捕捉系统，目的是建立环境中各物体的位置信息。电脑通过网络端口连接控制移动机械臂，通过局域网连接动作捕捉系统传输数据。

2 方法 2.1 碰撞约束条件

根据任务要求，碰撞约束条件主要包括在对工作空间进行分析时的夹爪与机械臂自身连杆的碰撞和在进行RRT避障规划时的机械臂末端、连杆与障碍物的碰撞。此处将夹爪和障碍物简化为球体，则碰撞约束问题简化为求空间中一点到一条线段的最短距离是否满足条件。

如图2所示，问题描述为 $ P $ 是夹爪中心点或障碍物中心点， $ {X}_{\mathrm{H}} $ ， $ {Y}_{\mathrm{H}} $ 分别为连杆的2个端点， $ r $ 为球体半径， $ {R}_{\mathrm{H}} $ 为连杆半径。当参数 $ t\in (0,1) $ 时，O_project为 $ {X}_{\mathrm{H}}{Y}_{\mathrm{H}} $ 线段上的点；当 $ t\leqslant 0 $ 时，其为 $ {Y}_{{\rm{H}}}{X}_{{\rm{H}}} $ 延长线上的点；当 $ t\geqslant 1 $ 时，其为 $ {X}_{\mathrm{H}}{Y}_{\mathrm{H}} $ 延长线上的点。 $ L $ 为 $ O_{{\rm{project}}}$ 与P的距离，比较 $ L $ 与 $ r+{R}_{\mathrm{H}} $ 的大小判断是否满足碰撞约束条件。

$ {O}_{\text{project}}={X}_{\mathrm{H}}+t \left({Y}_{\mathrm{H}}-{X}_{\mathrm{H}}\right) $

(1)

$ t=\frac{\begin{array}{c}\left({x}_{1}-{x}_{\mathrm{w}}\right)\left({x}_{1}-{x}_{2}\right)+\left({y}_{1}-{y}_{\mathrm{w}}\right)\left({y}_{1}-{y}_{2}\right)+\\ \left({z}_{1}-{z}_{\mathrm{w}}\right)\left({z}_{1}-{z}_{2}\right)\end{array}}{({x}_{1}-{x}_{2}{)}^{2}+({y}_{1}-{y}_{2}{)}^{2}+({z}_{1}-{z}_{2}{)}^{2}}\quad\qquad $

(2)

$ L=\left\{\begin{array}{l}\sqrt{\begin{array}{l}{\left[\left({x}_{1}-{x}_{\mathrm{w}}\right)+t \left({x}_{2}-{x}_{1}\right)\right]}^{2}+\\ {\left[\left({y}_{1}-{y}_{\mathrm{w}}\right)+t \left({y}_{2}-{y}_{1}\right)\right]}^{2}+\\ {\left[\left({z}_{1}-{z}_{\mathrm{w}}\right)+t \left({z}_{2}-{z}_{1}\right)\right]}^{2}\end{array}}\;\;\begin{array}{l}\;\\,\;\;1>t>0\end{array}\\ \sqrt{\begin{array}{l}({x}_{1}-{x}_{\mathrm{w}}{)}^{2}+({y}_{1}-{y}_{\mathrm{w}}{)}^{2}+\\ ({z}_{1}-{z}_{\mathrm{w}}{)}^{2}\end{array}}\;\;\;\;\;\;\;\;\;\;\begin{array}{l}\;\\,\;\;t \leqslant 0\end{array}\\ \sqrt{\begin{array}{l}({x}_{2}-{x}_{\mathrm{w}}{)}^{2}+({y}_{2}-{y}_{\mathrm{w}}{)}^{2}+\\ ({z}_{2}-{z}_{\mathrm{w}}{)}^{2}\end{array}}\;\;\;\;\;\;\;\;\;\;\begin{array}{l}\;\\,\;\;t \geqslant 1\end{array}\end{array}\right. $

(3)

根据式(2)计算 $ t $ 的取值，便可得到空间中点到直线的最短距离 $ L $ ，根据式(4)可知是否满足碰撞约束条件。

$ L>(r+{R}_{\mathrm{H}}) $

(4)

2.2 机械臂抓取工作空间分析

为移动底盘提供停靠策略之前，需要在无障碍物环境下，先对结合夹爪碰撞约束的机械臂进行工作空间的分析。

根据上述碰撞约束的判断和机械臂抓取情况，物体在机械臂基座上方和前方，机械臂工作空间区域为(x：0~900 mm，y：−900~900 mm，z：0~900 mm)，依次按比例划分为 $ 3\times 6\times 3 $ 个区域，随机给定6组关节角，检测并记录满足碰撞约束的末端点，绘制空间网格图，根据每个区域中包含末端点数目的不同绘制不同颜色。平抓方式与下抓方式满足碰撞约束的末端点数目不同，具体划分规则见表1，m和n分别代表平抓与下抓区域中满足点的数目。

实验得到了平抓、下抓抓取方式满足碰撞约束条件的工作空间网格分布，见图3。

将机械臂工作空间可达到的高度0~1000 mm依次划分为0~20个区域，平抓与下抓抓取方式在机械臂空间高度上满足点数目的对比见图4。

根据图3~4可知在机械臂的工作区域(x：0~300 mm，y：−300~300 mm，z：0~300 mm)中，选取合适抓取姿态会有较高的抓取规划成功率。

2.3 机械臂抓取难易评价标准分析

根据上面对机械臂工作空间的分析，可知机械臂的可抓取性会随物体在抓取空间内的位置变化而变化，为综合评价机械臂抓取质量，下文使用上述实验数据来综合评价抓取质量 $ {Q}_{f} $ ，其中 $ a $ 为被抓取物体宽度， $ b $ 为夹爪最大张开宽度， $ d $ 为被抓取物体与移动机械臂底盘的距离， $ h $ 为被抓取物体相对移动机械臂底盘的高度，抓取方式此处定为平抓与下抓方式。

定义 $a < b$ ， $ 0\leqslant \left|d\right|<960\;\mathrm{m}\mathrm{m} $ ， $ 0\leqslant \left|h\right|<960\;\mathrm{m}\mathrm{m} $ ，平抓、下抓抓取方式为一次可执行的抓取操作，对其中每个元素取值范围设置评分标准为

$ \begin{aligned} & {Q}_{1}=\left\{\begin{array}{l}1,\;\;\;\; a <b\\ 0, \;\;\;\; a\geqslant b\end{array}\right. \\& {Q}_{2}=\left\{\begin{array}{l}3,\;\;\;\; 0\leqslant \left|d\right|<320\;\mathrm{mm}\\ 2, \;\;\;\;320\leqslant \left|d\right|<640\;\mathrm{mm} \\1, \;\;\;\;640\leqslant \left|d\right|<960\;\mathrm{mm}\\ 0,\;\;\;\; {\text{其他}}\end{array}\right. \\& {Q}_{3}=\left\{\begin{array}{l}3,\;\;\;\; 0\leqslant \left|h\right|<320\;\mathrm{mm}\\ 2{, }\;\;\;\;{320 \leqslant }\left|h\right|<640\;\mathrm{mm}\\ 1, \;\;\;\;640\leqslant \left|h\right|<960\;\mathrm{mm}\\ 0, \;\;\;\;{\text{其他}}\end{array}\right.\\& {Q}_{4}=\left\{\begin{array}{l}3,\;\;\;\; {\text{平抓}}\\ 1,\;\;\;\; {\text{下抓}}\end{array}\right. \end{aligned} $

(5)

最终抓取质量评分为 $ {Q}_{f}={Q}_{1}{Q}_{2}{Q}_{3}{Q}_{4} $ ，27分为最高分，0分为最低分。由式(5)可知当被抓取物体宽度大于夹爪最大张开宽度时，直接导致抓取得分为0。对于评分过低的抓取操作需要以此为初始值进行搜索，以得到可靠性更高的抓取。

2.4 障碍物环境下移动机械臂停靠策略

针对移动机械臂工作环境中存在障碍物等情况，结合2.2和2.3节，本文提出了移动底盘停靠位置优化算法，并且加入碰撞约束的RRT算法，使机械臂末端在空间中进行避障抓取。

成功抓取目标物体的关键是移动底盘的优化位置搜索，针对在桌面上抓取物体，移动底盘停靠位置优化算法流程图如图5所示。

其中机械臂初始六关节角为 $ {\theta }_{0}(1\times 6) $ ，移动基座起始位置为 $ p\_\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{r}\mathrm{t} $ ，目标物体位置为 $ p\_\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l} $ ，障碍物位置为 $ p\_\mathrm{o}\mathrm{b}\mathrm{s} $ ；圆心为 $ p\_\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l} $ ，半径为r，定义圆心与基座起始位置连线为分割线 $ l $ ，圆上与分割线相交的解通过极坐标方程求解，定义为 $ s $ 。

$ {\mathrm{C}}_{1} $ ：障碍物简化为点后与分割线的最短距离判断依据为2.1中碰撞约束条件的求解方式。

$ {\mathrm{C}}_{2} $ ：交点 $ s $ 与目标 $ p\_\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l} $ 是否满足在同一侧的判断依据为 $p\_\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}\left(1\right) ,s\left(1\right)\leqslant p\_\mathrm{c}\mathrm{e}\mathrm{n}\left(1\right) 或 p\_\mathrm{g}\mathrm{o}\mathrm{a}\mathrm{l}\left(1\right) , s\left(1\right)> $ $ p\_\mathrm{c}\mathrm{e}\mathrm{n}\left(1\right) $ ，其中 $ p\_\mathrm{c}\mathrm{e}\mathrm{n} $ 为桌面的中心位置。

$ {\mathrm{C}}_{3} $ ：机械臂抓取难易评价标准 $ {Q}_{2} $ 。

3 仿真和实验

为了评估移动底盘停靠位置优化方法的有效性，结合上述对机械臂抓取工作空间的分析，限定物体的高度及抓取方式之后，抓取过程只涉及到物体与移动机械臂底盘的距离，使用提出的算法进行仿真，并在实体机器人上进行实验。该算法在Intel Core i5-8300HQ的PC上运行(2.3 GHz) MATLAB版本2016b实验。对比移动底盘不移动与加入移动底盘停靠位置优化算法在抓取物体上的区别，结果表明加入移动底盘停靠位置优化方法后，计算时间更短，抓取成功率更高。

3.1 仿真

未加入移动底盘停靠位置优化算法的移动机械臂保持位置不变，直接运行RRT避障规划算法，生成机械臂末端轨迹点。加入移动底盘停靠位置优化算法，移动机械臂会移动到优化位置，然后机械臂旋转朝向目标物体，再采用RRT避障规划算法规划末端点轨迹。

下文在几组不同条件下验证加入移动底盘停靠位置优化算法的可行性。考虑机械臂平抓方式比下抓方式抓取成功率更高，并且实际环境中物体摆放位置有一定高度，验证中采用平抓方式。其中障碍物及夹爪简化为球体，半径r设置为40 mm；连杆近似为圆柱体，半径R_H设置为40 mm，移动机械臂底盘起点均为坐标系中的(0, 0, 0)位置。选取可执行率均值、路径点完全执行次数、总规划路径点数和RRT规划时间均值作为评价指标，可执行率为一组测试中机械臂可执行路径点数目与总规划路径点数目的比值。

3.1.1 空间单障碍物靠近目标物体避障仿真实验

此实验中，障碍物位置为(400 mm, 400 mm, 400 mm)，目标点位置为(500 mm, 500 mm, 400 mm)，机械臂起始六关节角为(30°, −75°, −104°, −29°, 91°, 231°)。加入移动底盘位置控制前后的对比见图6。

图7为20次测试下移动底盘是否移动在可执行率均值及规划时间均值上的对比。表2为移动底盘是否移动的评价指标数据对比。

3.1.2 空间三障碍物避障仿真实验

此实验中，障碍物位置分别为(200 mm, 200 mm, 200 mm)，(300 mm, 0 mm, 500 mm)，(200 mm, 400 mm, 300 mm)，目标点位置为(550 mm, 500 mm, 500 mm)，机械臂起始六关节角为(−179°, −52°, 152°, −290°, 98°, −180°)。

加入移动底盘停靠位置优化算法仿真步骤如图8所示，移动机械臂的底盘先移动到计算得到的优化停靠位置，然后调整机械臂初始朝向，最后进行RRT算法避障。

图9为20次测试下移动底盘是否移动在可执行率均值和规划时间均值上的对比。表3为移动底盘是否移动的评价指标数据对比。

3.1.3 空间三障碍物靠近目标物体避障仿真实验

此实验中，障碍物位置分别为(400 mm, 700 mm, 400 mm)，(500 mm, 300 mm, 500 mm)，(300 mm, 550 mm, 400 mm)，目标点位置为(500 mm, 500 mm, 400 mm)，机械臂起始六关节角(−179°, −52°, 152°, −290°, 98°, −180°)。图10为加入移动底盘位置控制前后的对比。图11为20次测试下移动底盘是否移动在可执行率均值和规划时间均值上的对比。表4为移动底盘是否移动的评价指标数据对比。

3.1.4 空间六障碍物避障仿真实验

此仿真实验中，障碍物位置分别为(300 mm, 200 mm, 400 mm)，(300 mm, 0 mm, 500 mm)，(200 mm, 400 mm, 300 mm)，(600 mm, 0 mm, 400 mm)，(500 mm, 700 mm, 500 mm)，(600 mm, 700 mm, 300 mm)，目标点位置为(500 mm, 500 mm, 400 mm)，机械臂起始六关节角设置为(−179°, −52°, 152°, −290°, 98°, −180°)；图12为加入移动底盘位置控制前后的对比。图13为20次测试下移动底盘是否移动在可执行率均值和规划时间均值上的对比。表5为移动底盘是否移动的评价指标数据对比。

3.1.5 空间桌面避障仿真实验

在仿真环境中添加桌面条件，使其更符合实际机械臂抓取环境。3个障碍物的位置采用随机生成的方式，障碍物在桌面上方。目标位置为(500 mm, 500 mm, 400 mm)，桌面以(850 mm, 800 mm, 210 mm)为角点，长、宽、高分别为600 mm、400 mm、80 mm，机械臂起始六关节角为(44, −42°, −104°, −29°, 91°, 231°)。图14为根据随机障碍物的位置生成移动底盘停靠优化位置后抓取的效果图。

图15为20次测试下移动底盘是否移动在可执行率均值和规划时间均值上的对比。表6为在有桌面和障碍物的情况下，考虑机械臂连杆与其之间的碰撞约束条件后，移动底盘是否移动的评价指标对比。

3.1.6 样机仿真实验

在仿真环境中添加桌面条件，使其更加符合实际环境，分别针对正方体、长方体桌面进行了仿真。

正方体桌面仿真实验条件：长宽为 $ 500\;\mathrm{m}\mathrm{m}\times $ $ 500\;\mathrm{m}\mathrm{m} $ ，桌面位置为(x：1250~1750 mm，y：1250~1750 mm，z：210~290 mm），移动机械臂初始位置为(0 mm,0 mm,0 mm)，障碍物位置分别为(1550 mm, 1500 mm, 350 mm)，(1400 mm, 1350 mm, 350 mm)，(1450 mm, 1650 mm, 350 mm)，目标位置为(1300 mm, 1500 mm, 400 mm)；移动机械臂移动抓取过程如图16所示。

长方体桌面实验条件：长宽为 $ 1\;800\;\mathrm{m}\mathrm{m}\times $ $ 600\;\mathrm{m}\mathrm{m} $ ，桌面的位置为（x：1250~1850 mm, y：400~2200 mm, z：210~290 mm），移动机械臂初始位置为(0 mm, 0 mm, 0 mm)，障碍物位置分别为(1500 mm, 500 mm, 350 mm)，(1400 mm, 1200 mm, 350 mm)，(1700 mm, 700 mm, 350 mm)，目标物位置为(1700 mm, 500 mm, 400 mm)；仿真实验结果如图17所示。

3.2 样机实验

在正方体和长方体桌面上对目标物体及障碍物分别进行了实验，实际环境均在动作捕捉系统下搭建，桌面长宽根据仿真距离搭建，障碍物用方盒代替，目标物体用水瓶代替。

图18显示的是移动机械臂抓取正方体桌面上的目标物体，正方体桌面长宽为 $ 500\;\mathrm{m}\mathrm{m}\times 500\;\mathrm{m}\mathrm{m} $ ，在动作捕捉系统下，正方体桌面的位置为(x：1250~1750 mm, y：1250~1750 mm, z：600 mm)，移动机械臂初始位置为(0 mm, 0 mm, 300 mm)，障碍物及目标物位置根据仿真条件设定，移动机械臂最终停靠位置为(945 mm, 1080 mm, 300 mm)。

图19显示的是移动机械臂抓取长方体桌面上的目标物体。长方体桌面长宽为 $ 1\;800\;{\rm{mm}}\times 600\;{\rm{mm}} $ ，在动作捕捉系统下，长方体桌面的位置为(x：1250~1 850 mm, y：400~2200 mm, z：600 mm)，移动机械臂初始位置为(0 mm, 0 mm, 300 mm)，障碍物及目标物位置根据仿真条件设定，移动机械臂最终停靠位置为(2276 mm, 330 mm, 300 mm)。

图18和图19中，(a)和(b)为移动机械臂底盘移动至优化后的停靠位置；(c)和(d)为机械臂末端移动至目标物体；(e)和(f)为夹爪闭合夹取目标物体并举起。

4 讨论

上述仿真实验数据显示，不同条件下加入移动底盘停靠位置优化算法后可提高可执行率及增加路径点完全执行次数。

仿真实验3.1.1显示当单个障碍物较大且靠近目标物体时，移动机械臂不移动的情况下不存在能完全执行路径点到达目标点的情况；而底盘的移动能有效地提高目标物体的抓取规划成功率，并缩短规划时间。

样机实验实现了目标物体的成功抓取，证明了仿真计算的移动底盘停靠位置优化算法及RRT避障算法在实际应用中的可行性。

与之前的方法相比，本文所提出的加入连杆碰撞约束的RRT避障方法增强了机械臂的可操作性与避障能力；移动底盘停靠位置优化算法不仅提高了规划速度，而且提高了物体的抓取规划成功率。未来的研究将集中在动态方面，根据实时环境监测提出移动底盘实时搜寻移动位置的控制策略。