力矩优化的扩展雅可比方法机械手避障研究*

彭 芳，黎 萍，吴泽帆，刘春铭

(电子科技大学中山学院 机电工程学院，广东 中山 528402)

0 引言

机械手已广泛应用于自动化生产中，代替人类完成重复繁重的劳动。当机械手放置空间有限时，障碍物在工作空间形成了几何约束，必须进行避障控制。机械手避障控制是确保机械手末端在完成轨迹跟踪主任务时机械手连杆能避开障碍物，机械手的正确姿态需要进行无碰撞路径规划作为期望关节轨迹。

人们已经在机械手避障这一领域做了很多研究，其中，主任务雅克比矩阵伪逆的零空间投影是实现机械手避障的一种重要方法，对于避障机械手，其工作任务包括末端执行机构的路径规划任务(主任务)和连杆的避障任务(次级任务)，在不影响主任务的情况下，次级任务可以通过伪逆零空间投影实现[1-8]，这样，避障问题转化为优化目标函数问题。尽管文献[1-8]所述控制器利用零空间投影实现了机械手避障，但在实际应用中依然遇到问题。一个有效的运动/动力学控制方法必须考虑执行器饱和问题，关节力矩需要限定在工作范围内，否则将很难用于实践[9]。

本文提出机械手在实现末端轨迹跟踪的前提下，利用雅克比矩阵扩展方法，引入关节位移能量优化与避障约束自运动，同时采用峰值力矩减小法，实现在力矩约束下的避障控制。

1 机械手逆运动学

假设一个机械手有n个关节，操作空间为m维，有n>m。若Q为n维关节空间，W为m维任务空间，则正向运动函数表示为f：Q→W。设q=(q1,q2,…qn)T为关节角度，x=(x1,x2,…,xm)T为机械手在操作空间的位置，则机械手的运动方程有:

x=f(q)

(1)

(2)

这里J(q)∈Rm×n称为机械手的雅克比矩阵，由于n>m，J(q)是一个长方形矩阵。应用零空间投影法，式(2)可改写为：

(3)

假设h：Q→R是一个优化每一位置的实数函数，则利用梯度投影法:

z=±α▽h

系统能够渐进趋向最优，这里α是任意正定增益常数。虽然式(3)仅需要计算▽h，但增益α的选择很困难，α太小，自运动项会导致系统反应迟钝，α太大，控制系统变得不稳定。Baillieul[ 10]提出的扩展雅可比矩阵方法解决了这一问题：

假设ni(i=1,…,n-m)是张成J零空间的一组正交向量，N=(n1,…,nn-m)，在满足末端执行器位置的情况下最小化标量函数h，有:

NT▽h=0

(4)

N∈Rn×(n-m)，它的梯度可用于扩展原来的雅克比矩阵。

(5)

扩展后的雅克比矩阵成为方阵，可直接求逆，而不需要再用伪逆求解。

2 扩展雅克比矩阵避障控制设计

2.1 带避障约束的扩展雅克比矩阵

应用扩展雅克比矩阵方法，避障约束任务可以被定义为：

(6)

机械手的雅克比矩阵可通过自运动向量扩展，结合式(2)与式(4)，有：

(7)

(8)

式(8)中函数h(q)必然包含障碍物限制的影响，通常以障碍物到机械手每根连杆间的欧几里得距离di为基础，构造防撞约束Di。这样，由避障相关约束形成的标量函数定义为：

(9)

(10)

由此，式(9)标量函数可以写成：

(11)

根据式(11)，可得h(q)的梯度表示为：

其中，

2.2 避障约束下的末端轨迹跟踪控制律设计

已知n关节机械手动力学方程有：

(12)

由式(7)和式(12)，扩展雅克比矩阵避障控制律设计为[11]：

(13)

式中，

末端轨迹跟踪采用PD控制，xd为末端期望轨迹，x为末端实际轨迹，Kp、Kd分别为比例与微分系数。

3 关节力矩优化策略

力矩优化是机械手动力学主要的优化目标之一，但在许多情况下，例如跟踪控制问题，限制峰值转矩比转矩最小化更重要，由于大的需求转矩可以导致转矩饱和，从而导致跟踪精度的降低。本文提出利用机械手末端执行器运动的局部信息得到一个力矩局部优化方法，以减小峰值力矩。

最小化性能指标定义为：

(14)

满足条件:

(15)

和

(16)

在这里W是斜对称正定权重矩阵，

(1)根据当前状态i的信息计算下一状态i+1的信息：

(17)

(18)

(4)计算下一个状态的速度 。计算如下：

(19)

(5)在i+1状态重复上述操作。

具有峰值力矩优化的机械手避障控制框图如图1所示。在给定末端轨迹的情况下，通过扩展雅克比矩阵方法设计避障自运动向量，同时在满足阈值条件进行力矩优化，阈值条件设置为各连杆距离障碍物的最小距离(安全距离)，安全距离大于所设阈值则采用力矩优化算法，否则依然采用避障优先控制。

图1 机械手避障控制结构框图

4 避障算法求解与仿真

算法仿真以平面3连杆机械手为例，亦可看成是常用6轴机械手在垂直平面的规划，此时θ1、θ4和θ6为锁紧状态，只有另外三个关节是自由的，即:

(20)

平面3连杆机械手如图2所示，机械手的末端自由度是2(m=2)，冗余度则为1(r=n-m=1)。障碍物被包裹在以圆心为(x1b,x2b)、半径为rb的圆内。

图2 平面3连杆机械手避障

4.1 算法求解

(1)求解标量函数h(q)

现以连杆1为例计算防撞约束D1。首先定义两个向量v1=OA和v2=OC，连杆1至障碍物圆心的欧式距离为d1，则有：

(21)

由向量计算公式，有:

v1·v2=v1v2cos(v1,v2)

(22)

由式(22)有：

(23)

利用公式(21)～式(23)，可以计算出欧式距离d1：

这样，防撞约束D1为：

(24)

这里ds为防撞安全余量。

用相同方法，可求得连杆2和连杆3的防撞约束D2和D3：

(25)

(26)

将式(24)～式(26)带入式(11)即可求得h(q)。

(27)

(3)求解扩展雅克比矩阵Je

求解Je的关键在于Ja的求解，对于式(8)，Klein等提出了一种更易于数值化处理的Ja计算公式[19]：

Ja=NT▽(▽h)-W

在这里，W是一个向量，它的元素wi计算方法如下：

此处，▽(▽h)是hessian矩阵，即：

4.2 仿真结果

以图3所示的实验室LT950-A-6六轴机械手为例，θ2、θ3和θ5关节所对应的连杆长度分别为L1=0.45m，L2=0.45 m，L3=0.109 m；初始条件q0=[1.5;-1.08;-0.6]；障碍物是半径为0.03m的圆，其圆心位于(0.4,0.44)。机械手末端轨迹为一条由上而下的直线：

图3 六轴机械手与障碍物位置演示

(1)无避障算法的机械手末端轨迹跟踪控制如图4、图5所示，末端轨迹跟踪采用PD控制，参数选择Kp=diag[2600,4500]，Kd=diag[30,21]，t=4s。由图可见，采用PD控制算法基本能够实现末端轨迹跟踪控制要求，但是机械手连杆穿越障碍物而过，如果障碍物存在，必定直接损坏机械手结构。

(2)采用雅克比扩展避障算法的机械手轨迹跟踪控制如图6～图8所示，末端轨迹跟踪仍采用PD控制，参数选择Kp=diag[1855,1840,1450]，Kd=diag[30,35,30]，避障参数设置为ρ1=0.1，ρ2=0.1，ρ3=0.4；机械手连杆距离障碍物的安全距离设置为ds=0.01。由图8可见，雅克比扩展避障算法在实现末端轨迹跟踪的同时实现了机械手的避障。但输出的峰值力矩较大，如不加以限制，可能超出电机实际转矩。

(3)在雅克比扩展避障算法基础上，加入峰值力矩减小算法限定机械手关节出现的峰值力矩。PD参数与避障参数设置同上，机械手的实际关节力矩极限值分别为±60Nm、±40Nm、±20Nm。设置安全距离为0.02m，即只有在连杆距离障碍物大于0.02m时，采用力矩优化算法。图9～图11所示为本文所提算法优化结果。由图可见，在实现避障同时，关节控制力矩峰值有效的限定在了极限值内。

从仿真结果看，在扩展雅克比算法中引入避障约束自运动，能在完成末端轨迹跟踪的同时有效实现障碍物的回避，只要机械手关节力矩Kp和Kd参数设置合适，采用PD控制算法，末端轨迹能够得到较好跟踪，但是关节转矩过大时，末端跟踪误差将随之增加，由图(7)和图(10)的结果比较可见，峰值力矩减小算法的加入提高了避障时的轨迹跟踪精度，同时将关节力矩限定在了工作范围内。如果需要进一步提高跟踪精度，还可采用自适应神经网络控制[1]、自适应模糊控制[8]等算法实现。

图4 无避障的末端轨迹误差

图5 无避障机械手运动轨迹

图6 避障控制下的关节控制力矩

图7 避障控制下的末端轨迹跟踪误差

图8 避障控制下的机械手轨迹

图9 峰值力矩减小法应用后的关节控制力矩

图10 峰值力矩减小法应用后的末端轨迹跟踪误差

图11 峰值力矩减小法应用后的机械手轨迹

5 结论

本文利用机械手冗余度解决避障控制问题，在机械手逆运动学中利用扩展雅克比矩阵方法，引入实现避障任务的自运动向量，通过设计能量优化与防撞约束的标量函数计算自运动向量，从而在完成轨迹跟踪主任务的同时实现避障控制。为了达到实际控制需求，在完成避障控制的前提下，提出了峰值减小的力矩优化算法，避免饱和转矩对轨迹跟踪的不良影响。本文算法在平面3连杆机械手上进行了仿真实验，仿真结果很好的验证了算法的有效性，该算法可直接扩展应用于六轴机械手的避障控制中。

[1] Daachi B, Madani T, Benallegue A. Adaptive neural controller for redundant robot manipulators and collision avoidance with mobile obstacles[J]. Neurocomputing, 2012, 79(3):50-60.

[2] Ding H, Chan S P. A Real-Time Planning Algorithm for Obstacle Avoidance of Redundant Robots[J]. Journal of Intelligent & Robotic Systems, 1996, 16(3):229-243.

[3] 封岸松, 戴炬. 冗余自由度机械手的避障控制[J]. 机器人, 2002, 24(3):213-216.

[4] 谢宗武, 孙奎, 刘宏. 扩展雅克比方法的冗余度机器人逆运动学应用[J]. 哈尔滨工业大学学报, 2009,41(5):34-36.

[5] Guo D, Zhang Y. A New Inequality-Based Obstacle-Avoidance MVN Scheme and Its Application to Redundant Robot Manipulators[J]. IEEE Transactions on Systems Man & Cybernetics Part C, 2012, 42(6):1326-1340.

[6] Yen-Chen Liu, Nikhil Chopra. Control of semi-autonomous teleoperation system with time delays [J]. Automatica, 2013, 49(6):1553-1565.

[7] Liu Y C, Chopra N. Semi-autonomous teleoperation in task space with redundant slave robot under communication delays[J]. 2011, 32(14):679-684.

[8] Benzaoui M, Chekireb H, Tadjine M, et al. Trajectory tracking with obstacle avoidance of redundant manipulator based on fuzzy inference systems[J]. Neurocomputing, 2016, 196: 23-30.

[9] Salinas A, Moreno-Valenzuela J, Kelly R. A family of nonlinear PID-like regulators for a class of torque-driven robot manipulators equipped with torque-constrained actuators[J]. Behavior modification,1997,21(4):470.

[10] Baillieul J. Kinematic programming alternatives for redundant manipulators[C]//Robotics and Automation. Proceedings. 1985 IEEE International Conference on. IEEE, 1985, 2: 722-728.

[11] Benzaoui M, Chekireb H, Tadjine M. Redundant robot manipulator control with obstacle avoidance using extended Jacobian method[C]// Control & Automation (MED), 2010 18th Mediterranean Conference on,2010:371-376.