机械臂环境三维重建与避障算法研究

陈新度，徐 学，高 萌，孔德良

（1.广东工业大学机电工程学院，广东 广州 510006；2.佛山智能装备技术研究院，广东 佛山 528234 3.广东工业大学省部共建精密电子制造技术与装备国家重点实验室，广东 广州 510006）

1 前言

工业机械臂作为仿人手的自动装置，具有通用性强、运动灵活等优点［1］。随着机器人应用场景的复杂、非结构化，对其提出了实时感知和避障的要求。常规可预先建立障碍物空间模型或通过示教方法，以形成无碰撞轨迹。但此类方法只能针对静态环境，对动态环境不具有泛化性。因此，在未知环境中的三维重建和避障规划成为亟需解决的实际问题。

在目标识别方面，主要分为基于模板匹配、三维局部特征、学习的方法。文献［2］利用快速形状匹配算法将三维CAD模型在多视角的位姿映射作为模板，与位姿完成匹配。文献［3］通过提取二维图像和点云图像的多模态信息，实现物体的识别定位。文献［4］以各视点下的RGB-D图像融合CNN网络，完成待测物体的目标识别。

在三维重建方面，主要以基于传统多视角几何、深度学习算法为主。如文献［5］提出的Kinect Fusion，开启了RGB实时三维重建的序幕，此后有Elastic Fusion［6］、Bundle Fusion［7］等。文献［8］提出Pixel2Mesh，用端到端的神经网络实现了从单张彩色图直接生成用网格表示的物体三维信息。

在机器人避障规划方面，文献［9］将障碍物映射到构形空间，利用A∗算法搜索避障路径，但将空间障碍物投影到二维平面中，无法充分发挥高自由度工业机械臂在避障方面的优势。文献［10］提出RRT-connect，通过双树搜索大幅提升规划速度，但由于随机采样，不具备渐进最优。

综上分析可看出，虽然识别定位、三维重建和避障规划方面研究成果较丰富，但三者的结合，即目标物识别后对于未知环境空间的重建及效果、避障算法驱动机械臂避开重建的障碍的效果等研究尚存在不足。据此，本研究提出一种基于视觉的三维重建与避障规划方法。

通过搭建基于机器人操作系统（ROS）的实验平台，使用深度相机获取ArUco标记上的特征点坐标，求解目标物的姿态；通过改进重建方法，融合机械臂末端位姿和点云数据，对未知环境空间进行三维重建，作为后续规划的障碍空间；应用RRT的改进算法进行机械臂的避障运动规划，并进行了真机验证。

2 原理与方法

2.1 相机模型与标定

物体的三维空间坐标与图像坐标存在线性关系。世界坐标系、相机坐标系以及图像坐标系之间的关系，如图1所示。而坐标点间的转换关系，如式（1）所示。

图1 各坐标系转换模型Fig.1 Transformation Model of Each Coordinate System

式中：Puv—标记在像素坐标系下的像素坐标；Pw—标记在世界坐标系中的坐标；K—相机内参；T—相机外参。这里采用ArUco 自带的标定算法［11］对相机进行标定，确定摄像头的内参矩阵K以及畸变参数D，标定结果为：

研究构建了眼在手上的手眼系统，如图2所示。A为末端与机械臂基坐标系的变换关系，可通过机械臂正解获得；C为相机与标定板的变换，即已求的相机外参；B为相机与机械臂末端的变换关系，该关系是最终要求解的手眼矩阵。

图2 手眼标定示意图Fig.2 Schematic Diagram of Hand-eye Calibration

为求解B，需要多次变换机械臂姿态拍摄标定板，获得不同角度下的标定图像，从而推导出相机相对于末端的变换矩阵。

建立不同空间转换回路，如式（2）所示：

式（4）、式（5）代入式（3）得：

式（6）就转换成了经典方程AX=XB的求解问题［12］。通过两步法求解手眼矩阵为：

2.2 目标识别与位姿估计

位姿估计即找到现实世界和图像投影之间的对应点，求得相机与目标之间的相对位姿。本研究选用基于二进制标记的方法，优点在于单个标记即可提供足够多的对应点，且具有错误检测与校正机制［13］。选用的ArUco标记由一个四边全黑的边框以及内部的二进制矩阵组成，黑色代表0，白色代表1，每一个标记都有一个独有的ID，标记大小为（5×5），标记尺寸为(40×40)mm，ID为1，标记的中心点为原点，如图3所示。

表1 集成的插件及参数设置Tab.1 Integrated Plug-in and Parameter Settings

图3 ArUco标记图Fig.3 Marked Diagram of Aruco

ArUco标记的识别步骤，如图4所示。将获取的RGB图像经过灰度化、自适应阈值分割、轮廓滤波、透视变换得到正视图；并利用Otsu 分离出白黑色位，分析标记ID；将角点亚像素级细化后，根据小孔成像模型，通过ArUco标记的4个角点像素坐标，使用solvePnP函数［14］即可求得标记与相机之间的变换矩阵。

图4 ArUco位姿估计流程图Fig.4 Flow Chart of Aruco Pose Estimation

ArUco标记、相机、末端和基座关系，如图5所示。

图5 各坐标系变换关系图Fig.5 Transformation Relation Diagram of Each Coordinate System

转换关系，如式（7）所示。通过求得的标记与相机之间的变换矩阵、手眼矩阵和末端位姿，可得标记与机械臂基坐标系之间的变换矩阵，即目标物空间三维姿态。

2.3 融合末端位姿的三维重建

ICP算法是用来解决点云之间的配准问题，计算出最优匹配变换矩阵，使得误差最小［15］。但在两点云相差较大的情况下，ICP容易陷入局部最优解，所以需要在精配准之前进行粗配准［16］，并以粗配准得到的转换矩阵作为其初始转换矩阵。由于采用的是手眼系统，而末端相对于机械臂基座的位姿关系容易获得，可通过在配准之前，获取机器人末端位姿，将对应时刻的点云数据统一到同一坐标系下，再进行后续的配准融合工作。

假定相机在位置1、2 处分别获取点云，且获得对应处的位姿，有：

由式（8）、式（9）可得：

化简式（10）得：

式中：n∈N+，n≥2。

得到各点云刚体变换矩阵后，再将其融合FPFH_ICP配准算法，得到基于基坐标系的全局点云模型。

利用八叉树数据结构描述作业空间（全局点云），将各象限定义为八叉树的叶子节点，并不断进行迭代，直到满足终止条件［17］。假设t=1，…，T时刻，观测的数据为z1，…，zT，则第n个叶子节点的状态信息为：

式中：P(n|z1：T)—空间节点n在1到t时刻被占有的概率；

P(n|z1：T-1)—空间节点n在1到t-1时刻被占有的概率；

P(n|zT)—空间节点n在t时刻被占有的概率；

P(n)—先验概率。

2.4 RRT＊改进算法

RRT-Connect算法通过双向搜索和贪婪策略大大降低了计算量，提高了搜索速度，但由于随机采样，路径较长，无法达到渐进最优。

RRT*通过引入路径代价的概念，不断迭代找到渐进最优路径，但降低了计算效率。

针对上述缺陷，提出了改进算法CAN-RRT*（RRT* with Caucy Sample、Attractive Force and NodeReject），通过引入柯西采样函数取代随机采样，使采样点更大概率落在障碍物附近，降低采样的盲目性；通过引入目标引力和动态步长法，提高局部搜索速度；利用节点拒绝策略来剔除不必要的采样节点，使算法能够以高效率生成最终路径。

柯西分布概率密度函数为f(x，x0，g)，其中，x0—峰值的位置参数；γ—最大值一半处的一半宽度的尺度参数。

令l=|x-x0|，表示采样点与障碍物的距离，l越小，函数值越大，l越大，函数值越小。

引入目标引力，引力势能为：

式中：k—引力系数。可得qgoal对qnear的目标引力为：

扩展后新节点位置，如式（18）所示。

式中：ρ1—随机方向步长；ρ2—目标方向步长。

在采样过程选择一个节点之后，在节点扩展前使用节点剔除策略，如式（19）所示，即如果当前的最低成本低于直接从初始节点到该节点以及从该节点到目标节点的总成本，则剔除该节点。

式中：qstart—起点位姿；qgoal—目标位姿；q—候选节点；σbest—当前最低路径成本。

伪代码，如图6所示。

图6 CAN-RRT*伪代码Fig.6 Pseudocode of CAN-RRT*

其中，（1）CaucySample（）：柯西采样函数，在C-space得到随机节点xrand；（2）Neares（t）：最近邻点函数，得到最接近qrand的节点qnearest；（3）AttractiveForce（（）：目标引力与动态步长函数，产生新节点qnew；（4）NodeRejec（t（）：节点拒绝函数；（5）GetSortedLis（t（）：集合排序函数，返回按成本函数升序排列的列表；（6）ChooseBest-Paren（t（）：最优父节点函数；（7）RewireVertices（（）：重布线函数，找到随机树Tb中距离xnew最近的节点xconn；（8）connec（t）：返回连接xstart和xgoal的无碰撞路径；（9）SwapTrees（）：交换双树。

在三维、随机障碍的环境下，对CAN-RRT*算法进行避障规划，如图7所示。

图7 CAN-RRT*运动规划Fig.7 Motion Planning of CAN-RRT*

3 实验验证与分析

3.1 实验平台搭建

3.1.1 真实平台搭建

所搭建的实验平台，如图8所示。该平台包含机器人、深度相机和末端执行器。采用的机器人由Universal Robots 制造，型号为UR5，负载5kg，工作范围为850mm，理论重复定位精度为±0.03mm；深度相机为INTEL 公司的RealSense D435i，其具有彩色、红外及IMU模块，测量距离为（0.105～10）m；末端执行器为因时机器人的EG2-4B2电动夹爪，重复定位精度±0.5mm，最大速度97mm/s。

图8 UR5机器人平台Fig.8 UR5 Robot Platform

视觉系统获取工件及作业环境的信息，机械臂经规划，运动到工件位置，末端执行器对工件进行夹持，并将工件放到货架指定位置处。

3.1.2 仿真及控制搭建

基于ROS开发了仿真及控制平台，集成碰撞检测、正逆运动学求解、轨迹平滑等功能，部分插件选择及参数设置，如表1 所示。RVIZ界面的部署，如图9所示。系统的避障规划流程，如图10所示。

图9 机器人仿真与控制平台Fig.9 Robot Simulation and Control Platform

图10 机器人仿真及控制系统Fig.10 Robot Simulation and Control System

视觉定位系统获取工件姿态，三维重建系统生成障碍模型，信息汇总到规划场景中，提供给CAN-RRT*规划器生成无碰撞路径，并通过Movelt驱动机械臂，运动到目标位置成夹持任务。

3.2 识别定位分析

为了验证视觉定位精度，开展了精度评估实验，如图13 所示。使用4点法将机器人末端TCP移到顶针末端，使用Halcon手眼标定获得相机与机器人坐标系之间的变换矩阵，ArUco标记张贴在平面上。将顶针末端移动至与ArUco原点重合，从RVIZ中监听该点在机器人基座坐标系下的位姿，作为真实值，即1.2节式（7）中的，通过ArUco 的姿态估计矩阵、机器人手眼标定矩阵和机械臂末端位姿矩阵，得到ArUco标记原点的计算值。通过比较两者之间的偏差可以评估视觉定位精度。使用ID字典为5的ArUco标记，相机距离平面350mm，垂直向下拍摄，统计实验数据，如表2、图11所示。

表2 同一位置目标点识别位姿Tab.2 Recognized Pose of the Same Target Point

图11 同一位置定位数据偏差图Fig.11 Location Data Deviation Diagram of the Same Location

重复测量十次，实验结果表明，X、Y、Z方向基本稳定在8mm以内，姿态角可以稳定在2°以内，验证了目标识别精度的可行性。

3.3 三维建模分析

对机械臂作业的环境进行重建，重建效果，如图12所示。可以看出，融合末端位姿的重建方法能够有效的将环境中的障碍物重建到规划环境中，但某些地方还存在空洞现象。

图12 全局点云效果图Fig.12 Rendering of Global Point Cloud

这里采用定位精度和尺寸精度来衡量重建的效果。如图13中的长方体物体，实际尺寸为（277mm，200mm，126mm），进行多次重建，取其中8次结果对比。

图13 作业空间障碍物及标记点Fig.13 Obstacles and Markers in Working Space

分析表3，可知在物体长宽高方向上误差均值分别为1.37mm、1.78mm、1.34mm，误差控制在5mm以内，基本上达到重建的要求，表3中单位为mm。

表3 三维重建尺寸数据Tab.3 Dimensional Data of 3D Reconstruction

为了验证三维建模的定位精度，在障碍物表面贴上黑白标定板，如图13所示。选取10个不同位置处的角点，采用类似上文的方案，通过尖端触点得到各角点真实位姿，将模型转换到机械臂基坐标系下，比较对应角点的偏差来评估定位精度。实验结果，如表4、图14所示。

表4 各角点真实值与扫描值数据Tab.4 Real Value and Scanning Value of Each Corner

图14 角点定位数据偏差图Fig.14 Data Deviation Diagram of the Corner Location

实验结果表明，角点在X、Y方向误差稳定在8mm以内，深度方向误差保持在5mm左右，姿态角误差稳定在3°以内，验证了三维建模的定位精度。

3.4 避障运动规划分析

将1.3节中的尺度参数γ设为1，最小距离ε设为0.5，步长ρ1设为1.6°，步长ρ2设为1.8°，引力系数设为1.1，搜索半径设为3，最大迭代次数设为1000，在三维空间、确定障碍物的环境场景下进行1000次重复实验，相关数据经均值处理，统计，如表5所示。

表5 各规划算法规划对比Tab.5 Comparison of Planning Algorithms

由表5 可看出，相比RRT，改进后的算法路径长度缩短了22.2%，搜索时间降低了55.9%，采样节点数下降了54.3%，有效性得到验证。

将CAN-RRT*算法添加到开源运动规划库OMPL 中，通过Movelt控制机械臂。分别设置规划时间上限为0.1s，0.2s，0.5s，初始状态位置不变，目标状态为通过ArUco 码识别的姿态（0.152，0.536，0.437，-0.287，-1.48645，0.0902），障碍空间为三维重建获得的栅格地图，每种情况进行1000次重复实验，统计结果，如表6所示。可看出该算法在时间上限为0.1s时，成功率达到96.5%，表明该算法有效且可靠。

表6 CAN-RRT＊规划成功率Tab.6 Planning Success Rate of CAN-RRT＊

3.5 避障运动失败原因分析

为了给后期的研究工作提供改进思路，对规划失败原因进行分析统计，如表7所示。主要由以下原因造成：（1）在当前目标状态及障碍物的情况下，逆解求解错误；（2）规划超时；（3）碰撞检测失败；（4）识别误差及机械臂运动误差。

表7 失败原因统计Tab.7 Statistics on the Cause of Failure

由表7可知，识别误差与运动误差占总体5.7%，说明对于目标物的姿态定位比较准确，而规划超时仅有1 次，说明CANRRT*算法在规划速度方面满足需求。

逆解求解失败和路径段碰撞检测错误占总体88%，前者主要是因为在当前目标状态及障碍物位置下，机械臂本身不存在到达该点的构型；而后者失败原因在于碰撞检测分辨率设置略大，导致部分情况下碰撞检测跳过了障碍物。可考虑设置更小的碰撞检测分辨率来解决这个问题，但同时会导致规划时间增长。

4 结束语

鉴于当前工业机器人在复杂、未知环境中的避障规划方面的不足，本研究提出了基于视觉的三维重建与避障规划的方法和流程。首先使用ArUco码对工件识别定位，然后利用机械臂末端位姿和FPFH_ICP算法对环境点云进行融合，重建了复杂环境下的工作空间，最后应用改进的CAN-RRT*算法，实现了未知环境下的避障运动。

研究通过实验验证了方法的可行性和稳定性，不仅适用于关节臂机器人，而且也适用于其他构型的机器人。