基于虚拟空间三维体素—语义地图的机械臂任务规划

王立鹏，王小晨，刘梦杰，孟 浩

（哈尔滨工程大学智能科学与工程学院，哈尔滨 150001）

0 引言

服务类机械臂是当前机器人领域的研究热点，未来的服务类机械臂会进入每个家庭并从事重要的自动化和智能化任务，对于服务类机器人的研究存在一些挑战，包括自主性建模和估计、机器人环境建图、语义驱动等问题［1-2］。本文围绕机械臂多样化信息的三维地图构建、语义驱动条件下的机械臂路径规划和优化等开展研究，考虑到虚拟仿真技术具有低成本和耗时少等优点［3-4］，将基于虚拟仿真空间开展以上内容的研究工作。

文献［5］中介绍了视觉同步定位与建图（Visual Simultaneous Localization and Mapping，VSLAM）经典框架，从间接和直接两种方法综述了VSLAM 的关键技术和最新研究进展，并对VSLAM 的发展趋势进行了展望。文献［6］中提出一种基于三维激光雷达数据的低漂移SLAM算法，针对激光雷达扫描采样策略，设计了一种模型匹配框架，实验验证了新方法的有效性。Zhou 等［7］提出一种使用高效通道注意力插入压缩MobileNet V2（Efficient Channel Attention to insert into Compressed MobileNet V2，ECMobileNet）的策略，减少操作数同时保持精度，基于ECMobileNet 特征向量的平均分布设计了相应的环路检测方法。文献［8］中提出了3D动态场景图，完成可操作空间感知的统一表示，并且提供了算法来获得室内环境及其关系的分层表示。Yu等［9］中提出一种自底向上结构化三维场景图生成框架，以结构化的表示方式高效描述三维室内环境的对象、关系和属性，采用视觉感知捕获语义信息，从场景先验中进行推理，计算出最优解析图。近年来，在提供语义丰富且几何精确的空间模型，三维重建已成为一项重要且具有挑战性的任务［10］。语义丰富且几何准确的空间模型为工作空间内导航服务提供了关键信息［11］，室内环境建模有不同的表示方法，包括网格模型、计算机辅助设计几何模型以及参数模型［12］。常用的工作空间建图方法是规划前建图，为保证机器人在空间中的任务规划，可对场景中部分或者全部障碍物进行操作。Sinha等［13］通过视觉获取环境信息，通过移除障碍物使得抓取目标不被障碍物遮挡。

根据采集场景数据的完整性，把路径规划分为：基于先验完全信息的全局路径规划和基于传感器信息的局部路径规划［14］。现阶段，机器人轨迹规划算法较为成熟，对未知非结构化环境内的任务规划和机器人协调规划是研究的热点。基于全局3D 传感器（深度相机，雷达等）或机器人与传感器结合等完成非结构环境建图，Deeb 等［15］提出分段确定性准静态同步定位与建图（Piecewise-deterministic Quasi-static Simultaneous Localization and Mapping，PDQS-SLAM），解决了地标长期微位移对机器人定位与建图的影响。Wan等［16］提出一种基于被动立体视觉的高精度位姿测量系统，以实现非结构化环境下的精确目标定位和位姿估计；为解决在障碍物环境中轨迹规划问题，分别为避障规划和与障碍物的交互［17］。在任务与运动规划融合方面，强化学习等方法开展端到端的任务规划，采用传统基于任务的运动规划或两者根据实际情况考虑优先级，与此同时，轨迹优化也是有着许多优秀的算法，根据目标不同可分为最小能量、最优时间、速度连续和凸优化等［18-19］。

目前机械臂任务规划存在如下问题：机器人地图更多关注地图精度与效率，很少将建图与机器人任务相结合，地图模型不能完全跟随环境信息的变化而更新，需要消耗资源，扩展性受到限制；其次，规划算法很多，但需根据实际使用情况进行调整匹配，不具有速度信息的路径需经过后续轨迹优化得到安全、可行的路径，优化过程也会因机器人、优化目标不同而不同，同时，直接在速度、加速度空间上开展运动规划则会存在规划时间长或者规划后依然需要优化的情况。

为解决以上问题，提出基于虚拟空间的机械臂建图与规划方法，技术路线如图1 所示。

图1 技术路线

由图1 可见，利用虚拟空间，构建一种三维体素-语义混合地图，训练Mask RCNN 神经网络模型，得到物体图像掩膜结合深度图像分割、拼接单个较完整的物体点云，利用迭代最近点（Iterative Closest Point，ICP）算法估计物体位姿，通过优化A*算法和位姿插值方法开展机械臂末端执行器路径规划，通过贝塞尔曲线插值进行轨迹优化，使得关节轨迹光滑连续。设计语义驱动下的典型几何体抓取位姿和任务分解策略，通过虚拟场景和实际物理场景开展实验仿真。本文研究成果可提供其他算法的验证性环境，也可接入实际物理环境提供的采集数据，在虚拟场景中建图，形成体素-语义地图，虚拟场景完成规划和优化、分析和分解任务，驱动实际机械臂执行。

1 三维体素-语义融合地图构建

机械臂任务规划需要描述场景中的障碍物以及与任务相关的物体信息，本文构建三维体素-语义融合地图，采用Gazebo构建三维虚拟场景，场景中含有利用SoildWorks软件设计的平台、机械臂和待抓取物体。

1.1 基于Mask RCNN目标识别、分割及位姿估计

利用基于掩码区域的卷积神经网络（Mask Region-based Convolutional Neural Network，Mask RCNN）完成彩色图像中的目标物体识别与分割，数据集来源于虚拟场景中相机的拍摄，数据集类别有：碗、可乐罐、啤酒罐、圆柱、立方体、球体以及机械臂本身。为丰富数据集，避免过拟合，在仿真中选择了不同机器人的工作背景。考虑到目标物体种类较少，本文采集标定400 张图片作为数据集，部分样本如图2 所示。

图2 部分数据集

在Mask RCNN训练时选用ResNet101 模块，先训练heads模块40 epochs，学习率0.001，训练总体40 epochs，学习率是heads模块学习率的1/10，ROIS设置为100，每个epoch设置100 步，validation_steps是50，训练结束后的loss值为83%，训练结果较好。部分样本图片的识别结果如图3 所示。

图3 Mask RCNN训练模型检测结果

每个RGB 图像经Mask RCNN 可输出物体种类，为同一种物体分配不同编号和对应的掩膜，将掩膜图像保存为二值灰白图像，将三维点云数据对应掩膜图像，如掩膜图像中值为255，则将该点加入点云，按照该方式多幅图像经掩膜筛选后的点云拼接在一起，即可组合成一个相对完整的物体目标点云，分割物体、掩膜图像以及拼接后的目标点云如图4 所示。

图4 分割后物体点云图像

利用ICP算法可得目标点云到源点云的平移和旋转矩阵，将目标点云的点在世界坐标系下表示，且源点云坐标系与世界坐标系重合，即源点云中心点与世界坐标系原点重合，则得转换矩阵是目标点云在世界坐标下位姿的逆矩阵，即可估计出目标物体的位姿。

1.2 体素-语义地图融合

在虚拟场景，采集获得的RGBD 数据转化为点云数据，可将三维点云按照膨胀半径扩展为体素地图，经过Mask RCNN识别、分割后的目标物体并通过位姿估计可得物体语义信息，包括物体名称、编号和位姿信息等。将该语义信息中包含的物体与上述体素地图分离标注，再融合处理，得到体素-语义地图，如图5 所示。

图5 三维体素-语义地图

通过Mask RCNN 和ICP 算法可获得目标点云位姿，在执行机械臂抓取任务后，可在体素-语义地图中直接更新目标点云，不需要重新扫描建图、识别和分割，这是本文虚拟空间的优势。

2 基于体素-语义混合地图的机械臂运动规划

将机械臂运动规划分为无碰撞轨迹规划以及基于速度约束的轨迹优化，通过改进A*算法完成机械臂末端轨迹规划，利用贝塞尔曲线优化关节轨迹获得平滑的轨迹。

2.1 基于A*算法机械臂轨迹规划

优化A*算法的实现流程，为每个节点添加一个状态标签state，state=0 表示节点未遍历，state=1 表示节点已扩展，state=-1 表示节点已移除同时代价值计算并未修改。代价数值均用欧拉距离计算，各节点代价函数的总代价值

式中：g（n）为节点n到起点的代价值；h（n）为节点n到终点的预计代价值。

选择set红黑树容器和无序set 容器，其中set 红黑树容器保存state=1，用以存储已经扩展但未遍历的节点，记为open_list_set；无序set 容器，用以存储所有扩展遍历的节点，记为had_unordered_set。在体素-语义地图中，已知起始节点和目标节点，计算两个节点的g（·）、h（·）和f（·）值，把起始和目标节点放入容器中。从open_list_set中取出f（·）值最小的节点X，并从open_list_set 中移除，令其状态值为-1，遍历节点X所有非障碍邻居节点Y，如果Y在open_list_set中且Y的state值为1，是则判断euler（X，Y）+g（X）＜g（Y）是否成立，成立则更新节点Y的g（·）和f（·）值，Y的父节点为X，更新open_list_set中Y的值；否则计算节点Y的g和f值，Y的父节点记为X，放入open_list_set和had_unordered_set。

2.2 机械臂关节轨迹优化

在末端执行路径初步规划结果的基础上，在起点和终点之间将位姿插值处理，机械臂末端姿态角用四元数表示，采用四元数球面线性插值方法，设A*算法规划路径长度为l，则需要进行位姿插值个数为l-2，起点姿态为qst，终点姿态为qend，则相邻两个点的姿态差

第i个点的机械臂模块位姿

与起点和终点相关的末端执行器坐标系到世界坐标系的齐次转换矩阵分别为，末端执行器坐标系和世界坐标系的Z轴单位向量表示为Pzst和Pzend，则在世界坐标系中起点位姿：

若两个向量均在与坐标面平行的平面上时，计算两向量夹角

通过式（2）、（3）计算第2 点的姿态角，转换到世界坐标系得到向量Pw2，计算Pw2和Pwend两个向量的夹角θ2end，若θ2end＞θstend，则认为Δq取值错误并将qend修改为其共轭四元数并进行插值，反之则直接进行插值。两个向量不在与坐标面平行的平面上，则投影在任意两坐标轴形成的平面上，方法同上。

这里用m表示关节编号，用i表示第i个轨迹点。设机械臂的关节速度最大限制为ωT，在插值过程中对6 个关节角的变化值求累加和，用Δθ 表示，当累加和Δθ 加到某关节角度相对于前一关节角变化值Δθm，i时，如Δθm，i∈［0.95ωT，ωT］，则将该点作为最后一个控制点来进行贝塞尔曲线插值，如Δθm，i＞ωT，则额外添加一组控制点，保证该关节变量添加值θ=θm，i-1+0.95wT，其余关节角选择与最后的关节角一致。

在插值完成后，根据关节轨迹按顺序向机械臂发送期望关节角和期望关节速度，时间步长T=0.02 s，期望关节速度为

终点速度为0。

3 基于语义驱动的机器人空间任务规划

体素-语义混合地图已附带语义信息，利用语义为机械臂指定运动规划目标，驱动机械臂完成抓取任务，使机械臂任务规划灵活性更高，交互性更强且具有智能性。

3.1 机械臂末端抓取位姿

根据不同物体的几何形状，给出一个合适的抓取位姿，位姿的限定需满足3 个基本条件：抓取位置以稳定为主，避免物体脱落；抓取位姿选择靠近机械臂基底的位姿；抓取位置没有障碍物。在本文虚拟空间建立的模型主要有以下几类：

（1）圆柱体。机械臂抓取圆柱体高度一半的位置，末端执行器姿态角要求Z轴平行于圆柱底面且垂直于圆柱中心线。

（2）长方体。机械臂抓取两个对立面的中心点，姿态角选择要求Z轴平行于长方体底面且垂直于长方体中心线。

（3）碗状物体。以碗中心与机械臂基底连线，来判断最靠近机械臂的中部位置，抓取位置选择碗沿，姿态角选择需要判断碗的倾斜程度，保持Z轴与斜边平行朝向碗底面；斜边倾斜角需要根据物体的CAD模型进行测量计算。

以上模型的抓取效果如图6 所示。

图6 虚拟场景中目标抓取

3.2 抓取任务规划

机械臂任务分解主要包括：移动、抓取和放置。本文利用任务分解的方法，把复杂任务分解成子任务，具体如下：

（1）移动任务。以机械臂的运动规划为基础，是机械臂的空间任务规划的基础任务。

（2）抓取任务。分为预抓取、可抓取、抓取和后退4 个步骤。预抓取，选择靠近物体的一个位姿，距离能抓取物体有一段微调整；可抓取，机械臂贴近目标物体，物体已经在机械爪中间；抓取，机械爪闭合抓取，更新机械爪和物体在地图中的状态；后退，机械臂末端与物体为一个整体，体积增大，调整地图的膨胀半径，向后移动一定距离。

（3）放置任务。分为机械爪占用状态判断、可放置、放置和远离物体等步骤。机械爪占用状态判断，避免不合理的任务指令进行无效放置任务；可放置，机械爪移动到目标位置；放置，张开机械爪，与物体分离；远离物体，机械臂末端向远离物体方向移动，仍朝向目标物体，更新地图数据。

4 机械臂任务规划实验仿真

虚拟空间中基于语义驱动的任务规划

针对语义驱动的机械臂任务规划方法，本文设计如下仿真：

（1）机械臂把小球放入2 号盘子中（执行流程：抓取小球并放入2 号盘子）。

（2）机械臂移动到长方体旁（执行流程：从不同起始位置开始，方便于下一步抓取长方体，多次仿真统计平均时间和路程）。

任务1把小球放在2 号盘子。

语义内容：拿起小球放置到2 号盘子。

仿真初始条件：机械臂起始关节位置为随机初始位置。

仿真预期效果：机械臂末端抓取小球，并放置到2号盘子中。

在虚拟场景下机械臂执行抓取和放置过程如图7所示。

图7 任务1仿真过程

本次任务仿真路径长度为215 cm，规划所用时间为4.5 s，执行时间为36 s。

任务2机械臂移动到长方体旁。

语义内容：移动到长方体旁。

仿真初始条件：机械臂起始关节位置为随机初始位置。

仿真预期效果：机械臂多次从不同的初始位置出发，移动到长方体旁边。

在虚拟场景下机械臂移动过程如图8 所示。

图8 任务2仿真过程

本次任务仿真路径长度为78 cm，规划所用时间为2 s，执行时间为10.2 s。

综上所述，本文实现语义驱动机器人空间任务，规划速度快，占用时间较短，运行时间由于末端轨迹的限制，但符合实际情况。

5 结语

本文提出机器人在未知环境下三维体素-语义建图以及机械臂运动规划方法，构建一种三维体素-语义混合地图，由点云、语义信息和物体三维模型组成，该地图支持避障运动规划，附带语义信息支持语义驱动任务，支持实时更新物体在地图中的位姿，具有广泛适用性；利用优化A*算法实现机械臂末端笛卡尔空间轨迹规划，达到末端执行器路径最短，用贝塞尔曲线完成关节轨迹平滑，使运动过程速度连续，减小能量损耗，实现完整机械臂运动规划，并且以语义驱动机械臂任务规划，智能完成多种机械臂的空间任务，利用机械臂抓取位姿来分解复杂任务。本文尚未对机械臂的关节轨迹做碰撞检测，后续重点研究碰撞检测和重规划。