邱宏波，司徒仕忠，高 萌，孔德良，冯 桑
(1.广东工业大学机电工程学院，广州 510006；
2.佛山智能装备技术研究院，佛山 528234)
在我国实施制造强国的战略背景下，市场发展对6自由度机械臂技术提出更高的要求[1]。现阶段机械臂对周围环境的感知识别能力较弱，无法避开出现在工作空间中的人或障碍物，安全性、智能化程度有待提高。
围绕机械臂避障规划的研究中冗余机械臂的研究成果较为丰富，HAN等[2]提出了一种基于距离计算和离散碰撞检测的机器臂动态避障方法，该方法将机械臂的连杆简化为3个圆柱体，将深度相机捕捉到的障碍物点云实时替换为凸包模型。CHEN等[3]将动态环境下避障规划由碰撞检测和避障路径规划两个阶段完成，使用包围盒算法和改进的D*算法来完成动态避障。LUO等[4]对SRS型7自由度冗余机械臂进行研究，将机械臂的肩部、肘部、腕部3点组成手臂平面，以手臂角度为冗余参数，实现主动全臂避碰，但是这类避障方法对6自由度机械臂并不适用。LIU等[5]提出了一种基于位置采样和危险指数的人工势场的方法，用于人机协作和交互中的避碰，在避障对象上粘贴AruCo码，不符合工作空间中人或物体可能随机出现在工作空间的实际工程情况。LIU等[6]利用Kinect深度相机检测工作空间出现的人体骨架，并利用快速扩展随机数和排斥矢量相结合的方法来躲避人体，该研究的避障对象仅为人体且仅能实现机械臂末端避障，具有一定的局限性。
本文采用眼在手外的深度相机获取机械臂工作空间的动态情况，通过点云处理提取障碍物点云；
接着，将简化后的机械臂模型与障碍物点云进行距离计算作为碰撞检测的判断条件；
最后引入改进的人工势场法完成避障过程，并通过实验进行验证。该方法实现了连续、实时的动态避障过程，提高了机械臂的自主避障能力和安全性。
1.1 深度相机点云获取原理
深度相机，又称RGB-D相机，可以利用红外结构光或time-of-flight(ToF)原理，主动向探测目标发射光并接收返回的光，通过返回的结构光图案或光束飞行时间来确定物体离相机的距离[7]。
(a) 小孔成像 (b) 相似三角形原理
利用深度相机可以获得RGB图像和深度图像，深度图像的像素值代表传感器到所拍摄物体的距离值，根据图1针孔相机模型结合仿射变换等图形学知识，可列出公式：
(1)
(2)
联立式(1)和式(2)，可得：
(3)
式中，u、v为像素坐标系Ouv下的像素坐标；
x、y为物理坐标系Oxy下的物理坐标；
XC、YC、ZC为相机坐标系Ccam下生成点云的坐标位置；
f为相机中心到图像平面的距离为焦距；
dx、dy为每像素在x、y方向上的物理尺寸；
fx=f/dx、fy=f/dy、cx、cy为相机的内部参数。通过上式可以把深度图像转换为点云，直接反映物体可见表面的几何形状。
1.2 点云数据处理
为了获取障碍物点云，须先滤除机械臂本体的点云数据，本文采用开源的realtime_urdf_filter机器人实时过滤包进行过滤，然后利用式(3)将深度图转化为点云图。该方法首先是实时提取深度图中的机械臂URDF模型作为掩膜，再将原深度图与掩膜进行运算操作，从而过滤机械臂。
但是在实际情况下，深度相机在进行眼在手外标定时存在误差，真实机械臂上会有夹爪或吸盘等线束布置影响，导致机械臂点云滤除不干净、过滤后有残留点云的现象。在ROS平台的gazebo物理仿真器中添加机械臂模型、障碍物物块，深度相机采用眼在手外(eye-to-hand)的方式布置，在Rviz可视化界面图2b，可以看到，机械臂上面存在未被滤除的深度点云。
(a) gazebo仿真环境 (b) Rviz可视化界面
针对上述问题，对实时提取机械臂URDF模型的掩膜图像进行膨胀处理，增大运算操作的区域，达到完全滤除机械臂的效果。图3为膨胀处理前后的机械臂掩膜深度图对比，利用全景深度图减去膨胀后的掩模即可完全滤除机械臂模型。
(a) 原机械臂掩模 (b) 膨胀后机械臂掩模
图4 滤波处理后的障碍物点云
为了降低点云的数量级，满足实时性的要求，必须借助PCL(point could library)库对源点云图像进行滤波处理[8]。在环境及障碍物点云方面，对于超过机械臂工作空间的点云，使用PCL库的直通滤波器进行某一个或几个维度的截取，只保留一定范围内的点云。深度相机在使用时容易产生噪声，引入的离群点，可使用PCL库的统计滤波器或半径滤波器来滤除。最后使用PCL库的体素滤波器对障碍物点云进行降采样处理。体素滤波器能够减少点云的数量级，大大降低计算量，同时保留障碍物点云的形状特征，相比与对障碍物进行包围盒处理能同时兼顾碰撞检测的速度和精度。
2.1 机械臂模型简化
碰撞检测是避障规划的前验步骤。在3D碰撞检测中，为了加快碰撞检测的效率，常使用包围盒来近似地代替复杂的几何对象，常见的包围盒类型有AABB包围盒、OBB包围盒、包围球等[9]。包围球适合经常处于转动的物体，使用时主要是比较两球间半径和与球心距离的大小。
考虑到机械臂在运动过程中关节处总是相对转动，本文以关节link1～link6为机械臂为球心、以设定的安全距离为半径画虚拟包围球，如图5所示。然而，当连杆较长时，计算出来的距离dcal与实际距离dreal存在较大的偏差，降低了最近距离计算的精度。
基于此，为了提高虚拟包围球组成的模型与实际机械臂模型的紧密性以及最近距离计算的精度，同时使虚拟包围球较好地覆盖机械臂模型，在研究中对较长的连杆插补虚拟关节，以此来增加虚拟包围球的数量。
如图6所示，结合华数HSR-Co605机械臂的DH(denavi-hartenberg)参数，对于长度大于15 cm的连杆，每增加15 cm即增设一个虚拟关节点(virtual_link)，虚拟关节点同样会用于距离计算，每个虚拟包围球对应一个虚拟关节。
图5 机械臂关节的虚拟包围球 图6 虚拟关节插补
2.2 实时最近距离计算
综合前面的分析，滤波后障碍物点云已经转换到机械臂基坐标系下的x、y、z坐标，虚拟包围球球心的坐标也可以通过计算转换得到。进一步的，通过计算障碍物各点云和各关节的欧氏距离，得到表达如式(4)所示。
(4)
式中，(Pballxi,Pballyi,Pballzi)为机械臂模型第i个虚拟包围球球心在机械臂基坐标系下的坐标；
(Pobsxj,Pobsyj,Pobszj)为第j个障碍物点云在机械臂基坐标系下的坐标；
Dij为机械臂关节与障碍物点云各个点的距离。
图7 标记最近距离
经过排序比较，即可得出机械臂模型与障碍物的实时最近距离Dmin，在Rivz可视化中利用Marker工具箱可以对该最近距离进行实时的标记显示，如图7绿色线段所示。
将该最近距离与设定的安全距离阈值作比较，完成碰撞检测过程，同时为后续避障时全局和局部规划器的切换提供判断前提。
3.1 吸引速度和排斥速度
人工势场法[10]是一种高效的局部路径规划算法，其基本思想是在机器人运动的空间中，目标点定义为引力场产生“引力”，障碍物定义为斥力场会产生“斥力”，将这两者的“合力”作为“推动力”。
考虑到障碍物和机械臂“作用力”的描述难度，采用在笛卡尔空间改进人工势场法进行避障规划，速度场产生“吸引”和“排斥”作用。在全局规划中，吸引速度Vatt作用于机械臂末端，表达式如下：
(5)
(6)
在局部规划中，排斥速度作用于各个关节点，当机械臂与障碍物点云的最近距离Dmin小于设定的安全距离阈值时，障碍物点云的最近点产生排斥速度Vrep。为了更好躲避动态障碍物，在斥力函数中引入Pivot算法[11]，如下所示。
(7)
(8)
图8 吸引速度大小与距离的关系曲线 图9 排斥速度大小与最近距离的关系曲线
3.2 合速度
为了使机械臂及时躲避障碍物，排斥速度的数值通常会大于吸引速度，这时候会导致合速度V1偏离目标位置，如图10所示。本文对人工势场法进行改进，优化排斥速度和吸引速度的合速度方向。
图10 改进人工势场法的合速度
当排斥速度Vrep和吸引速度Vatt的方向大于90°时，首先根据右手定则，将向量Vatt叉乘向量Vrep，得到一个垂直于纸面向外的向量，将该向量与向量Vatt再进行叉乘得到向量Vver；
接着，计算向量Vrep在向量Vver上的投影，通过倍数关系可求得向量Vver1；
最后，通过向量的加减运算即可得到目标合速度向量V2，如式(9)～式(12)所示。所生成的合速度V2在与障碍物保持安全距离的基础上，比V1方向更优，能够实现避障的同时更快地到达目标位置。
Vver=Vatt×Vrep×Vatt
(9)
(10)
(11)
V2=2Vver1-Vrep
(12)
上述得到的吸引速度和排斥速度的矢量合成需要在关节空间进行[12]，在机器人学中，关节空间到笛卡尔空间的映射通常通过雅可比矩阵实现，虚拟关节点所受的排斥速度可转化到上一个关节点进行计算。
(13)
(14)
(15)
VAFPJ=VattJ+VrepJ
(16)
为了验证改进人工势场法的有效性，在Ubuntu 18.04系统的ROS环境中搭建仿真平台，硬件配置为CPU Intel i7-9700八核、RAM 16 G、GPU NVIDIA GeForce GTX 1660Ti；
仿真深度相机的位置如图2a所示，此时的外参为[0.5 1 2.5 0.91 0 0 -0.42]。
图11 机械臂末端避障轨迹对比
实验中障碍物的中心位置为(0.25，0.20，1.52)，机械臂从初始关节角(0，-1.57，3.14，0，1.57，0)运动到目标关节角(-1.57，-1.57，3.14，0，1.57，0)，安全距离阈值Ds为0.15。人工势场法改进前后的机械臂末端避障轨迹如图11所示。
改进后的人工势场法使机械臂贴近障碍物实现避障，分别对改进前后的人工势场法进行30组避障实验，记录下表1数据，可以看到，改进后的方法运行时间更短，避障成功率相比改进前有明显的提升，达到96.7%。
表1 人工势场法改进前后的仿真结果对比
采用华数HSR-Co605六轴机械臂进行真机实验，深度相机的型号为Kinect V2，手眼标定的外参为[-1.1 0.53 0.75 -0.43 0.65 -0.53 0.34]，机械臂运行步长为0.04 rad。
实验中用泡沫板模拟障碍物多次阻挡机械臂的前进方向。所拍摄的机械臂避障过程如图12所示。可以看到，图12c中机械臂完成了第一次避障，图12d中进行了第二次阻挡，图12f、图12g中机械臂往后躲避并进行绕开，机械臂的响应时间为20 ms，整个过程实现了连续的、实时的动态避障。
图12 机械臂动态避障过程
本文基于ROS搭建机械臂仿真平台，通过眼在手外的深度相机实时监控机械臂的工作空间，利用OpenCV、PCL库对图像、点云信息进行处理，成功滤除机械臂本体点云，得到障碍物点云。包围球的扩展应用实现了对机械臂模型的简化，通过增设虚拟关节提高距离计算的精度，以此来完成碰撞检测。在笛卡尔空间中，对传统人工势场法进行改进，最后通过雅可比矩阵将合速度转化到关节空间。
在实验中验证了改进人工势场法的优化效果，实现了6轴机械臂连续、实时的动态避障，避障成功率高。实验证明，该方法能检测进入机械臂工作空间的人或其它障碍物，无需对障碍物等进行标记，符合实际工程应用场景，为工业应用提供了一种6轴机器人安全避障思路。
猜你喜欢 势场碰撞检测障碍物 基于动力学补偿的机器人电机力矩误差碰撞检测汽车实用技术(2022年11期)2022-06-20全新预测碰撞检测系统汽车工程师(2021年12期)2022-01-17基于Frenet和改进人工势场的在轨规避路径自主规划北京航空航天大学学报(2021年4期)2021-11-24基于改进人工势场法的维修分队机动路线规划方法*指挥控制与仿真(2021年3期)2021-06-15融合前车轨迹预测的改进人工势场轨迹规划研究汽车工程(2021年12期)2021-03-08基于SPH方法的流体粒子与软体碰撞检测①计算机系统应用(2020年5期)2020-05-22高低翻越动漫界·幼教365(中班)(2020年3期)2020-04-20赶飞机创新作文(1-2年级)(2019年4期)2019-10-15基于势场搜索的无人车动态避障路径规划算法研究北京汽车(2019年4期)2019-09-17月亮为什么会有圆缺好孩子画报(2019年10期)2019-01-10