基于速度修正的协作机械臂动态避碰控制方法

王 欢，门 涛，苟维东，张子平，姜 勇

（1.鞍钢集团矿业有限公司能源管控中心，鞍山110168；2.中国科学院沈阳自动化研究所 机器人国家重点实验室，沈阳110016；3.中国科学院机器人与智能制造创新研究院，沈阳110016）

随着柔性制造及人机协作概念的提出，机器人的工作环境由传统的结构化封闭环境更多地转变为与人共享工作空间的非结构化环境。在这样的环境中，协作型机器人因其轻质、灵活易用，能与人协同作业等特性而具有了更加广阔的应用前景[1-2]。人机协作过程中，首要考虑的是安全问题。目前，为了保证人机安全，在协作型机器人的设计中常采用轻量化和柔顺性设计，并且在控制策略中加入功率和力限制，以保证机器人在与人发生碰撞时能及时停止，避免对人身造成更大的伤害[3]。当触碰到人体时，机器人将暂停工作进程，如果人体障碍一直存在，那么机器人将一直保持准静止状态，其工作效率会受到极大的影响。因此，实时检测机器人与人之间的距离，当距离过近时，机器人将人视为障碍进行躲避，避免人机碰撞的发生，这样既可保证安全性，又不会对机器人的工作进程产出太大影响。但这个动态的过程中，外界环境是不可预知的，需要依赖可靠的检测和避障控制方法[4-5]。

避障控制一直是机器人领域的研究热点，常见的避障控制方法主要分为两类：全局避障和局部避障。其中，全局避障方法主要包括构型空间法、C 空间法、A* 算法、单元分解法等[6-8]。全局避障方法需要全局建模或搜索最优解，需要全局信息，计算量较大，并且不能应对突然出现的动态障碍物，因此不适合人机协作场景下的避障控制。局部避障方法最具代表性的是人工势场法，即将机器人视为在虚拟的人工受力场中运动，障碍物对机器人产生斥力，目标点对机器人产生引力，引力与斥力的合力控制机器人的运动。人工势场法具有计算量小、实时性高等优点，但传统的人工势场法依然无法解决动态避障问题。为此，学者们提出了排斥向量法等改进算法[9-12]，但算法复杂度较高，实时性差。另外，此类局部避障方法由于缺乏全局信息，因此控制过程中容易陷入局部最小值点，造成机器人震荡或停滞不前。文献[7]提出了一种基于障碍物与机器人位置图像寻找最小距离的方法，该方法通过遍历图像中与机器人有关的凸包，直至找到障碍物，获得机器人与障碍物的最小距离。该方法虽然可以避免陷入局部最小值点，但使用图像获得的是两点之间的距离信息，该距离信息仅能使机器人以停止运动的方式避障，避障之后机器人不能继续运动到目标点。文献[8]使用Kinect 深度传感器获取障碍物与机器人的深度信息来估计机器人与障碍物的距离，然后将该距离引入人工势场法中，作为机器人的排斥指令进行避障。该方法始终是在笛卡尔空间控制机器人避障，在避障过程中机器人有可能因到达奇异点无法运动，而导致避障失败。

针对上述避障控制问题，本文提出一种基于速度修正的协作机械臂动态避碰控制方法。该方法将Kinect 检测的机器人与人体障碍的距离转化为虚拟力，作为机器人导纳控制的输入，控制机器人下一时刻的速度进行避障。人体障碍运动速度过大时，通过速度修正控制机器人的运动方向来进行紧急避障。最后，利用人机协作测试平台对机械臂的动态避碰控制方法进行了实验验证。

1 Kinect 标定与人体障碍检测

Kinect 传感器由IR 红外元件、RGB 图像元件及Mic 麦克风矩阵构成，可采集深度（红外）、色彩和音频信息。其中色彩帧分辨率为1920×1080，深度帧分辨率为512×424，同时提供了C++语言环境的音频数据处理、身体数据索引、坐标系转换（Camera/Color/Depth）、色彩及深度信息获取等基本例程。

Kinect 标定是求解相机模型参数的过程，也是人体障碍检测中的重要环节。其中，相机模型可近似为典型的针孔成像模型，如图1 所示。

图1 针孔相机模型Fig.1 Pinhole camera model

设笛卡尔空间中任一点P 在世界坐标系中的坐标为Pw=（xwywzw）T，经过旋转和平移变换后，可得到相机坐标系下的对应点Pc=（xcyczc）T，变换关系如下：

式中：R3×3和t3×1分别是旋转变换矩阵和平移变换矩阵。

P 点映射在相机坐标系和像素坐标系中的点Pc和Pu具有如下变换关系：

式中：fx=f/dx，fy=f/dy。dx和dy分别表示每个像素在x′和y′轴上的长度。

据式（1）和式（2），可得笛卡尔空间中的点P 在像素坐标系中的投影Pu=（pupv）T为

仅用像素坐标Pu无法获知点P 的深度信息，因此引入三维深度矩阵Dep=（pupvpd）T，其中pu和pv是P 点的像素坐标，pd是P 点到相机平面的距离，即：

利用深度矩阵不仅可以得到笛卡尔空间点的像素信息，也可以获得其深度信息，实现笛卡尔空间点数据与Kinect 数据之间的转换。

为了获取机械臂与人体障碍之间的距离，需要获知在同一坐标系下两者的位置关系，为此将相机世界坐标系上的人体障碍位置转换到机械臂基坐标系中。选取距离机械臂最近的人体部位作为障碍物，并将其位置通过转换矩阵（R｜t）转换到机械臂基坐标系中得到障碍点位置O=（xo，yo，zo）。

机械臂末端执行器的位置设为P=（xp，yp，zp）。通过空间向量法，计算出机械臂末端执行器与人体障碍之间的距离如下：

如图2 所示，基于Kinect 的图像和深度信息可以识别出人体框架和机械臂位置，计算出机械臂末端执行器与人体障碍的距离。

图2 基于Kinect 的人机距离获取Fig.2 Kinect-based human-machine distance acquisition

2 基于速度修正的避障控制

2.1 距离估计

控制点P 与障碍点O 之间的相对运动信息是成功避障的一个充分条件。将Kinect 捕获到的障碍点O 用深度矩阵表示为：OD=（ouovod）T，控制点P表示为P=（pupvpd）T，使用如下方法估计障碍点与控制点在笛卡尔空间中的距离：

式中：Δx 为控制点P 与障碍物点O 之间的距离。在避障过程中，过远的障碍点对控制点P 当前运动状态的影响非常小，可忽略不计。因此划定一个以控制点P 为中心，半径为ρ 的球形区域，去掉过远的障碍点。在该区域内，基于最短距离进行避障，以像素点（pu，pv）为中心，向外进行遍历，来获取新的局部最小值。

2.2 导纳控制

采用基于位置的阻抗控制，即导纳控制来控制机械臂避障。在导纳控制中，末端执行器位置与环境作用力之间的动态导纳关系如下所示：

式中：Fe为外力项；xd，为机械臂期望的运动信息；x0，为规划的运动信息；Mn×n，Bn×n，Kn×n分别为常数惯性矩阵、阻尼矩阵与刚度矩阵，这些矩阵决定了外力Fe与机械臂运动信息之间的动态特性。

在拉氏域中，式（7）的导纳控制关系可描述如下：

式中：

常规导纳控制器中输入的是传感器测量的外力，但是当传感器精度较低时，无法在碰撞瞬间测量出准确的实际受力值，加之导纳控制器存在的时延，导致人机碰撞时机器人不能及时停止而对人身造成伤害。因此本质上，基于实际外力的导纳控制无法满足人机协作安全性要求。而通过Kinect 实时检测控制点与障碍点的相对运动信息，并将其转化为虚拟力代替实际的外力，输入到导纳控制器中，控制机器人通过避障来避免实际碰撞的发生，可以从根本上保障人员的安全[10]。

虚拟力的定义如下：

其方向与Δx 相同，幅值定义为

式中：当‖Δx‖＝0 时，虚拟力的最大幅值为Vmax。需要注意的是必须满足‖Δx‖＜ρ 的条件，因为当‖Δx‖＝ρ 时，幅值为0。式（10）中虚拟力的幅值与距离的关系如图3 所示，当Vmax=2 m/s，ρ＝0.4 时，虚拟力的幅值随着距离的增大而减小。

图3 距离与虚拟力幅值关系图Fig.3 Admittance control block diagram

导纳控制中，虚拟力代替实际力作为控制系统的输入，经过导纳控制得到的是控制点与障碍点之间的相对速度，即：

另设，

由于障碍点的位置信息与Δx 有关，因此使用Δx 求导所得的速度估计障碍点的运动速度，即：

则避障过程如图4 所示。

图4 导纳控制的在线避障Fig.4 Admittance control online obstacle avoidance

2.3 速度修正

由虚拟导纳控制所得的控制量是控制点P 避开障碍点O 的速度向量V（P），当障碍点O 向控制点P 的移动速度大于控制点的避障速度时，会存在避障不及时的问题。因此，当无法及时调整控制点的速度大小时，通过调整速度的方向来解决该问题。文献[8]中通过在控制点运动的方向上施加与当前方向近似垂直的方向向量来克服此类情况，施加的方向向量与控制点和障碍点之间的角度有关。

向量n 与Γ（P）和V（P）所属的平面Π 垂直，向量v 与n 和V（P）正交。向量（α，n，v）（α，n 指定的正交基）用来修正平面Π 上V（P）的方向。速度向量V（P）的幅值变化函数（由正标量c 所形成）由平面Π 上的速度向量的角度γ 决定，当速度向量的幅值变化量为0 时，函数值为β；当速度向量的变化量趋于最大允许变量值Vmax（O）时，该函数值收敛于π/2。

图5 避障方向修正过程Fig.5 Obstacle avoidance direction correction process

由此可知，使用方向修正量Vpivot（P）对控制点P的避障方向进行在线修正，使其在遇到高速运动的障碍物时，通过向Vpivot（P）的方向运动进行避障，可以明显提高机械臂避障的成功率。

3 实验

3.1 实验设计

机械臂的控制和轨迹规划均在笛卡尔空间进行，虚拟导纳控制系统的控制对象为机械臂的末端执行器。障碍物为实验者的左手。障碍点与控制点之间的距离由一台Kinect V2 传感器实时测量，其捕获像素为640×480，深度图像的频率为30 Hz。实验中球形避障区域的半径为0.35 m。障碍物与目标点距离小于0.35 m 时，虚拟导纳控制系统有虚拟力输入，输出给末端执行器的为下一时刻的避障速度指令，直至目标点远离障碍点的距离超过0.35 m。以下实验中，虚拟力幅值参数为Vmax=4 m/s，ρ＝0.35 m。

3.2 实验分析

实验分为2 组，第一组为静态障碍物避障实验，第二组为动态障碍物避障实验。

第一组实验过程如图6 所示，整个过程中障碍物处于静止状态。机械臂运动初始时刻，障碍物与目标点之间的距离为0.435 m，机械臂无需避障；而当机械臂沿规划轨迹运动遇到处于静止的障碍物时，障碍物进入球形避障区域，机械臂开始避障。

图6 静态障碍物避障Fig.6 Static obstacle avoidance

机械臂的初始轨迹和遇到静态障碍物时的轨迹如图7 所示。

图7 机械臂的静态障碍物避障轨迹Fig.7 Static obstacle avoidance trajectory of robotic arm

第二组实验过程如图8 所示，机器人沿规划轨迹运动遇到侧边快速运动的障碍物时（障碍点的移动速度明显大于控制点的避障速度），控制点沿速度修正量修正后的方向运动，避开障碍点后重新回到规划轨迹上。

图8 动态障碍物避障Fig.8 Dynamic obstacle avoidance

图9 为机械臂在遇到动态障碍物时的运行轨迹。

图9 机械臂的动态障碍物避障轨迹Fig.9 Dynamic obstacle avoidance trajectory of robotic arm

由以上实验可以看出，使用基于速度修正的虚拟导纳控制避障方法，机械臂遇到静止和高速运动的障碍物均可以顺利完成避障过程。同时，与文献[8]相比，控制点和障碍点的距离超过球形避障区域的半径时，机械臂可以重新运动到规划轨迹上继续工作进程。

4 结语

针对人机协作环境下机械臂的动态轨迹控制问题，提出了一种基于速度修正的协作机械臂动态避碰控制方法。其核心思想是，通过捕捉人体障碍与机械臂之间的距离信息，利用虚拟导纳控制器，分别对静态和动态障碍物进行避障控制。当障碍物为静止状态时，直接采用等效虚拟力的导纳控制完成避障；当障碍物为运动状态时，在导纳控制的基础上引入机械臂速度空间的修正算子，利用速度修正控制机械臂紧急避障。最后通过在Kinect V2 和UR5 机械臂平台上开展的验证实验，证明了该方法的实现可行性与有效性。