机械臂目标区域跟踪防撞控制

杨沁林, 张 劲

(1. 四川大学电气工程学院, 成都 610065;2. 四川大学生物医学工程学院, 成都 610065)

1 引 言

医护人员短缺已成为全球医疗行业共同面临的严峻问题[1]，在此情况下,因为机械臂具有工作时间长，工作性能稳定，对疫情免疫等优势，所以使用机械臂代替医护人员完成常见的，重复性高的医护工作，可以在很大程度上减少医护人员的工作强度以及感染传染性疾病的风险.为此研究设计具有医护能力的机器人受到了研究人员的广泛关注.Jayaswal等[2]在疫情期间设计了鲁棒柔性机械臂控制方法，并对该方法进行了仿真实验验证.Ahamed等[3]设计实现可提醒患者或老年人及时服药，并在患者需要的时候随时提供饮用水的移动机器人.

机械臂在医护过程中涉及与患者的交互，例如当机械臂在进行护理过程中，机械臂末端执行器停留在患者附近时，患者可以根据个人需要，将机械臂末端执行器推离或拉近.在实际设置实验装置之前，需要依据文献[4]才能保证机械臂的控制过程涉及人机交互任务.文献[5-9]将机械臂末端在任务空间中的目标移动路径设置为一系列的点，其中所提出的控制方案均能较好实现目标轨迹的跟踪任务，但是在涉及机械臂操作过程中存在人机交互协作的情况，将机械臂末端的目标设置为一个区域更加合理，并且机械臂末端在目标区域内表现出柔顺性，以确保人机交互过程的安全，顺利.

在护理病房内，因为机械臂执行护理任务的特殊性，需要保证在护理过程中患者的安全，防止机械臂末端执行器误触患者重要部位，以及防止误触医护用品，避免在护理过程中发生医疗事故，所以机械臂末端在跟随目标区域移动过程中，需要规避患者及医护用品等障碍物.文献[10]提出了双向时效A*算法，提高了A*算法在规避障碍物过程中的执行效率，但每次迭代的路径选取未考虑障碍的位置，易陷入障碍密集区域;文献[11]使用RRT(Rapidly Exploring Random Tree)算法，提出了一种基子目标搜索策略，该算法主要是针对二维平面上的路径规划及避障，对于三维空间路径规划该算法搜索缓慢;文献[12]运用向量场直方图方法VFH(Vector Field Histogram)，提出一种波谷宽度计算方法，使得逼近目标位置过程中轨迹更加平滑.因为人工势场法规避障碍物具有结构清晰，运算量小的优点，由此引入人工势场[13].在人工势能场中包括两种势场：引力势能场和斥力势能场.利用人工势场规避障碍物也被广泛使用在无人机[14]、车辆[15]和船舶[15]等领域.在本文中，引力势能场作用在机械臂末端执行器，起到引导机械臂末端执行器实现目标区域跟踪的作用，斥力势能场同样作用在机械臂末端执行器，起到规避障碍物的作用.

本文提出了一种机械臂目标区域跟踪防撞控制器；实现机械臂末端执行器在跟踪目标区域过程中规避障碍物，并在目标区域内具有柔顺的人机交互性能；借鉴船舶领域的最近会遇距离和最小会遇时间，提出了一种机械臂碰撞预测模型；通过机械臂与障碍物的实时状态，得到防撞安全指数；将障碍物作用半径与防撞安全指数相联系，以此改变障碍物作用半径，实现机械臂在跟踪目标区域的同时，规避障碍物，保护患者，提高了医护机械臂的安全性.

2 问题描述

2.1 机械臂动力学模型

根据拉格朗日定理，机械臂在关节空间的动力学模型可以表示为

(1)

其中q∈n表示n自由度机械臂的广义关节角坐标向量；M(q)∈n×n为对称正定惯性矩阵；n×n为哥氏力与离心力矩阵；g(q)∈n为重力项；τ∈n表示控制输出力矩.

设x∈n表示机械臂末端执行器在任务空间中的位置向量

x=h(q)

(2)

其中h(·)表示从关节空间的关节角度q映射到任务空间末端执行器位置x的正向运动学变化矩阵.对式(2)两边同时求导，可得

(3)

(4)

结合关节空间动力学模型式(1)和文献[16]，可以推导出任务空间动力学模型为

(5)

其中

Mx(x)=J-T(q)M(q)J-1(q)

J-T(q)M(q)J-1(q))J-1(q)

g(x)=J-T(q)g(q)

F=J-T(q)τ

(6)

在本文中，因为实现控制任务都指定在任务空间中进行，所以将关节空间转换为任务空间，在任务空间中设计控制器更加方便，合理.

2.2 控制目标

本文的控制目标为：设计一种基于机械臂目标区域跟踪防撞控制器，将机械臂末端执行器的位置收敛到目标区域并对移动的目标区域进行跟踪，在目标区域跟踪过程中防止碰撞障碍物.障碍物中心定义为点xob∈3，障碍物作用半径为λ0，选择运动目标区域Τ为球形，其中心定义为点x0∈3，半径定义为r0.并且假设目标区域中心点x0的轨迹为二阶可导.

3 控制器设计

3.1 人工势能场

人工势能场是由引力势能场和斥力势能场共同组成.机械臂末端会受到目标区域的引力作用，同时也会受到障碍物的斥力作用.机械臂会在引力和斥力的共同作用下，规避障碍物向目标区域移动.

在三维空间中，目标区域将使用一个集合表示

(7)

其中Δx=x-x0,x0是目标区域的中心；f(Δx):3→为目标函数.目标函数关于Δx连续，且可微.f(Δx)的一阶偏导数∂f(Δx)/∂Δx和二阶偏导数∂2f(Δx)/∂2Δx有界.定义引力势能函数，斥力势能函数分别表示为[17]

(8)

(9)

其中c为引力增益常量；η为斥力增益常量；λ0为障碍物作用半径；λ为机械臂末端执行器位置x到障碍物中心x0的距离λ=|xobx|；m为大于0的常数.机械臂势能函数为

(10)

势能函数P(Δx)关于Δx求偏导数表示为

(11)

其中：

其中∂Uat(Δx)/∂Δx为引力势能对Δx求偏导，方向为从机械臂末端指向目标区域中心，将机械臂末端引导到目标区域.斥力势能对Δx求偏导由两部分组成，其中∂Ure1(Δx)/∂Δx的方向为障碍物指向机械臂末端，作用为将机械臂末端引导到障碍物作用范围以外，∂Ure2(Δx)/∂Δx的方向为从机械臂末端指向目标点，引导机械臂末端收敛到目标区域，其表达式分别为

(12)

当机械臂末端位于目标区域外，或在障碍物附近时，∂P(Δx)/∂Δx将会驱使机械臂末端离开障碍物作用范围，向目标区域f(Δx)≤0的区域移动，当机械臂末端位置在目标区域内，且不在障碍物附近时，f(Δx)≤0，∂P(Δx)/∂Δx=0.

3.2 碰撞预测模型

在使用斥力场规避障碍物的过程中，障碍物的作用范围λ0为主要参数.通常λ0为固定值，λ0设置过大造成机械臂避障效率降低，λ0设置过小造成规避不及时，存在碰撞危险.由此，本文提出一种机械臂的碰撞预测模型.碰撞预测模型已在船舶导航[18]，自动驾驶汽车[19,20]等领域得到广泛的应用，本文借鉴船舶领域中最近会遇距离DCPA(Distance of Closest Point of Approach)与最小会遇时间TCPA(Time of Closest Point of Approach)概念[21]，通过对发生碰撞在空间和时间上的危险度分别进行分析，得到空间防撞安全度与时间防撞安全度，通过对两个安全度的联立计算得到防撞安全指数，并由防撞安全指数确定障碍物作用范围.

图1 机械臂与障碍物会遇示意图

DCPA=λsin (αT)

(13)

(14)

其中λ=|xobx|为机械臂末端点到障碍物中心的距离，αT∈(0，π)为机械臂末端点速度方向与机械臂末端点到障碍物中心连线夹角.

首先设空间防撞安全度udT定义为

(15)

其中k为正常数.设d1为最小避让距离，d2为机械臂安全避让距离取d2=2d1.最小避让距离d1的确定，当机械臂末端安装执行器，末端执行器侧边面对障碍物中心时，存在碰撞危险，设机械臂末端执行器长度为Lend=0.1 m，将d1设置为d1=1.5Lend，以此保证末端执行器在规避障碍物过程中不会触碰障碍物.

在机械臂末端与障碍物有碰撞可能的情况下，时间防撞安全度utT定义为

(16)

其中t1和t2分别表示为

式中，t1表示机械臂末端速度方向上距离障碍物为d1的位置，达到最近会与点的时间；t2表示机械臂末端速度方向上距离障碍物为d2的位置，达到最近会与点的时间.

由式(15)和式(16)分别得到空间防撞安全度udT与时间防撞安全度utT后，防撞安全指数uT，由utT和udT量化为

(17)

将机械臂末端与障碍物的状态信息代入碰撞预测模型中，得到防撞安全指数，根据以下公式确定障碍物作用范围λ0.

(18)

式中，μ>1，由此可得到随着空间防撞安全度与时间防撞安全度变化的障碍物作用范围.

3.3 区域跟踪防撞控制器设计

本文中在任务空间的区域跟踪防撞控制器设计为

(19)

(20)

假设Lyapunov-like泛函数被设置为

(21)

其中P(Δx)在式(10)中被定义，V为正定，对式(21)两边求导得

(22)

将(20)代入(22)中，得到

(23)

证明 为了证明以上定理，先证明

(24)

由于dV/dt为负，V对t是递减的，可设

(25)

如果ε≠0,则对于任意t≥t0有，又由V连续，正定，并且在平衡点处t=t0时，V=0，z2=0，P(Δx)=0，故存在λ1>0或λ2>0， 使得‖z2‖>λ1或P(Δx)>λ2，所以如果取：

(26)

由dV/dt为负定，故m<0，于是

(27)

4 实验结果及分析

本文通过数值仿真以及实验，验证了所设计的目标区域跟踪防撞控制器的有效性.本文实验过程使用UR10机械臂.UR10有6个转动关节，负载能力为10 kg，操作范围1300 mm.因为在医疗护理机械臂工作过程中，完成任务空间中的目标区域跟踪，使用前3个关节即可实现，后3个关节被视作一个整体，医护人员在机械臂末端进入目标区域后单独控制，完成更加具体的医护工作.在本文中，仿真及实验通过控制前3个关节角，实现机械臂末端的在任务空间的位置控制，后3个关节作为末端执行器看作一个整体，并将关节角均设置为固定值.基于UR10机械臂建立动力学模型，UR10各关节坐标系如图2所示，机械臂DH参数如表1所示，前3个连杆的质量如表2所示.

图2 UR10各关节坐标Fig.2 Coordinates for each joint of the UR10

表1 UR10DH参数

表2 UR10物理参数

4.1 仿真跟踪移动目标区域

首先使用Matlab2020a数值仿真对目标区域跟踪防撞控制器的有效性进行验证.采用时间步长0.005 s的固定步长ODE4 (RungeKutta)求解器.仿真过程中，各参数设置为μ=1.1，k=1，kα=1，kν=3，c=5，m=1.05，η=0.1.机械臂前3个关节初始角度分别设置为：[2.37,-π/3,π/3]Trad，后3个关节的角度保持为固定值[0,-π/3,0]Trad.控制目标为UR10机器臂的末端执行器跟踪运动的目标区域，该区域定义为一个半径为r0=0.1 m的球体.目标区域中心x0沿着期望轨迹运动.

x0=[0.3cos(0.05t)，-0.8+0.3sin(0.05)t，0.2]

机械臂末端执行器轨迹三维及二维视图如图3和图4所示，机械臂末端执行器收敛到目标区域后，跟随目标区域移动.

图4 移动区域轨迹跟踪: UR10:末端执行器二维轨迹;REG:目标区域;START:开始点;END:停止点

势能函数如图5所示，机器人末端执行器能够快速收敛到目标区域.关节角度和控制输入力矩随时间变化如图6所示.仿真结果证明了目标区域跟踪防撞控制器对完成运动轨迹跟踪任务的有效性.

图5 移动区域轨迹跟踪:势能函数

图6 移动区域轨迹跟踪：关节角和控制输入(a) 关节角; (b) 控制输入Fig.6 Moving region trajectory tracking: joint angles and control inputs (a) Joint angles ; (b) control inputs

4.2 仿真安全防撞

医疗护理机械臂在完成不同医护任务移动过程中，存在机械臂末端执行器碰撞患者或者医护用品的风险，为此在已知患者与医护用品位置的前提下，对控制器的防撞安全性进行验证.各参数不变，所考虑的轨迹与上一个仿真实验相同目标区域球体半径r0=0.2 m，分别在xob1=[0.25,-0.55,0.43]T,xob2=[-0.12,-0.64,0.52]T,xob3=[0.25,-0.90,0.33]T3个位置放置障碍物代表护理病房空间中的三个医护用品，障碍物最小作用距离λ0=0.1 m.机械臂末端执行器轨迹的三维视图如图7所示，机械臂在规避障碍物后，继续保持稳定，并跟踪运动的目标区域.势能函数如图8所示，关节角度和控制力矩输入如图9所示.仿真结果验证了目标区域跟踪防撞控制器在机械臂运行过程中出现障碍物时，防止机械臂末端执行器碰撞障碍物的有效性.

图7 规避碰撞:UR10:3D:末端执行器三维轨迹;UR10:2D:末端执行器二维轨迹;REG:目标区域二维视图;OBS-REG:障碍物最小影响范围;OSB:障碍物；START:开始点;END:停止点

图8 规避碰撞：势能函数

图9 规避碰撞:关节角和控制输入

4.3 人机交互实验

人机交互实验，在UR10机械臂平台上完成，该平台由UR10机械臂本体、控制柜CB3、上位机控制系统三部分组成.上位机控制系统使用URScript编写控制算法，负责在线运行控制算法，向控制柜发送各关节角速度控制指令，控制柜负责接收控制指令，控制移动机械臂，并获取机械臂信息，将机械臂状态反馈到上位机控制系统，实现对机械臂的控制，实验平台如图10所示.

图10 实验平台Fig.10 Experimental setup

人机交互实验如图11，机械臂末端执行器收敛到目标区域后，可在目标区域内通过手持移动机械臂末端执行器位置，在目标区域内实现柔顺人机交互.在实验过程中，各参数设置为μ=1.1，k=1,kα=1，kν=1，c=2，m=1.05，η=0.1.目标区域中心点坐标设置为x0=[0,-0.3918,0.274]T，目标区域球体半径r0=0.2 m,以及机械臂前三个关节初始角度分别设置为：[1.5,-1.5,2.41]Trad，后三个关节的角度保持固定值[0,-π/3,0]Trad.在完成人机交互实验过程中，机械臂关节角度和势能变化如图12所示，机械臂位置变化如图13所示，实验结果显示机械臂末端执行器在目标区域内，势能为0，关节角度不改变，t=2 s通过移动机械臂末端执行器位置到目标区域外，势能大于0，控制器控制机械臂改变关节角度，将机械臂末端执行器收敛至目标区域，保持稳定.t=7.9 s，在目标区域内移动末端执行器位置，势能为0，机械臂保持稳定，不改变各关节角度.根据实验结果，验证了目标区域跟踪防撞控制器在机械臂操作过程中人机交互有效性.

图11 人机交互实验

图12 人机交互:关节角和势能函数

图13 人机交互:REG:目标区域;UR10:简化UR10

4.4 安全防撞实验

安全防撞实验，机械臂末端执行器在跟踪移动区域过程中，规避障碍物，实现机械臂末端执行器对障碍物的安全防撞.实验在UR10机械臂平台上完成，各参数不变，各关节初始角度不变，与前一个实验相同，目标区域中心x0期望轨迹由以下公式表示，目标区域球体半径r0=0.2 m,x0=[0,-0.3918-0.02t,0.274]T分别在xob1=[-0.08,-0.55,0.24]T,xob2=[0.15,-0.83,0.24]T两个位置放置障碍物代表护理病房空间中的两个个医护用品，设置实验如图14所示.

本文使用防撞预测模型，通过计算防撞安全指数实时改变障碍物作用半径，与不改变障碍物作用半径的无差别避障进行试验对比，因在护理病房内如水杯，体温枪，药瓶，最大边长约0.1 m，为保证机械臂末端安全避障，预留安全距离，所以将无差别避障中障碍物作用半径设置为λ0=0.17 m，通过实验数据绘制机械臂末端执行器运动轨迹三维视图及俯视图对比如图15所示.

图14 规避碰撞实验

(a)

(b)

(c)

各关节角度变化对比如图16所示，根据实验结果可得到以下结论，在规避障碍物过程中应用防撞预测模型改变障碍物作用范围，减少了各关节角的变化幅度，使得障碍物规避过程中机械臂末端执行器运动轨迹更加平滑，并与障碍物保持安全距离，验证了目标区域跟踪防撞控制器在防止机械臂末端执行器碰撞障碍物过程中的有效性.

图16 规避碰撞:CAP:防撞预测; IOA:无差别避障

5 结 论

本文针对医疗护理机械臂在控制过程中存在的人机交互、轨迹跟踪，以及在护理过程防止碰撞的实际问题，提出目标区域跟踪防撞控制器.该方法结合人工势能函数使用了区域到达控制，实现医疗护理机械臂有效跟踪目标区域，并在目标区域实现柔顺人机交互.本文提出的防撞预测模型通过改变障碍物斥力场作用半径实现机械臂对障碍物的规避，既保留了人工势场法简明的数学结构，又提高了机械臂的护理安全性，以及避障轨迹的平滑稳定.