连续迭代投影多任务优先级方法

王娖芸，唐先智，江 沛，王 彦，李华强

(重庆大学 机械传动国家重点实验室，重庆 400044)

机器人的冗余度使机器人能够在非结构化的复杂环境中运行，能够同时完成多个任务。经典的利用冗余度的方法有梯度投影法[1-2]、增广雅克比方法[3-4]和加权最小范数法[5-9]。多任务优先级算法使低优先级任务在高优先级任务的零空间中运行，保证首先完成高优先级任务，利用冗余度完成低优先级任务。Nakamura等[10]最先提出了任务优先级概念，采用零空间投影技术，实现了迭代梯度投影，保证低优先级任务在不影响高优先级任务的前提下执行，并成功用于避障[2]。在之前研究的基础上，Siciliano等[11]给出了高冗余度系统多任务优先级通用算法，能够迭代求解关节速度。

复杂的非结构化环境中，在不同的时间和位置，机器人需要完成任务不同，需要进行任务的插入或移除，这会引起雅克比矩阵秩的变化，出现关节速度的不连续现象[12]。解决任务优先级方法中关节速度的不连续问题是非常关键的。Chiaverini[13]提出奇异鲁棒方法，通过引入阻尼因子，以跟踪误差为代价，解决雅克比矩阵秩的变化引起的奇异问题。在文献[14]中，提出了一种基于连续零空间投影算子的平滑控制方法，以较低的计算成本解决了任务状态切换时的不连续问题，但即使所有任务都处于激活状态或失活状态，该方法系统的稳定性仍然存在问题。Mansard等[15]提出了一种连续逆，可以保证在任务切换过程中的关节速度连续性，并且当任务完全激活或失效时，其连续逆等于伪逆，之后应用于处理多个优先级任务[16]。但Mansard等提出的连续逆随着任务数量的增加，计算时间也呈指数增长。之后，Jiang等[17]和黄水华[18]通过连续迭代投影每个任务的零空间构建了连续迭代投影算子，有限次迭代能够保证任务切换过程中的连续性，当迭代次数接近无限次时，连续迭代投影算子逐渐接近经典伪逆投影算子。和Mansard等提出的方法相比，此方法可以节省计算时间。

综上所述，需要针对任务优先级算法中任务插入与移除引起的关节速度不连续问题，进行深入研究和分析。笔者基于文献[17]的连续迭代投影原理，提出连续迭代投影多任务优先级方法，依据李雅普诺夫稳定性原理证明所提出方法的稳定性。通过仿真验证该方法的有效性和稳定性，并与增广投影多任务优先级方法进行对比。

1 机器人运动学控制

1.1 机器人运动学控制

冗余串联机器人系统的自由度大于其完成期望任务的自由度，即关节空间的维数n超过任务空间的维数m,n与m的差值为冗余自由度。定义关节空间关节位置向量为q∈Rn，任务空间末端位置向量为x∈Rm，则机器人正向运动学方程为：

x=f(q)，

(1)

式中：f为任务函数，为从关节空间向量到任务空间向量的映射。对式(1)求导可以得到关节速度和末端执行器速度的映射关系，如下：

(2)

式中：J∈Rm×n为雅克比矩阵。对于冗余机器人，式(2)的一般解可表示为：

(3)

P=In-J†J，

(4)

式中In为n维的单位方阵。

冗余机器人可以同时执行多个任务。如跟踪末端轨迹的同时，进行避障、保证关节限位等。但需要对多个任务的优先级进行排序，来保证首要任务在高优先级层次被执行，利用冗余的自由度来完成次要任务。为了简单起见，先考虑T1和T2两个任务，这两个任务定义为：

x1=f1(q)，

(5)

x2=f2(q)。

(6)

假设T1是首要任务，任务T2在任务T1的零空间中执行。任务T1和T2对应的雅克比矩阵分别为J1和J2。根据式(3)，则关节速度的解为：

(7)

(8)

(9)

低优先级任务的关节速度投影在高优先级任务的增广零空间中，这种方法为增广投影法。

在不同的时间点以及当机器人关节移动到不同位置时，需要进行任务的插入与移除。如：在进行末端期望轨迹跟踪的同时，当机器人靠近障碍物时，需要考虑避障任务，且需要将避障任务插入优先级层次中；当机器人远离障碍物时，则不需要考虑避障任务，可以将避障任务移除，将冗余度用于完成其他任务。

式(8)在任务插入与移除时，会出现关节速度不连续的问题，不能满足需求。因此，将根据连续迭代投影原理，构建一种新型连续迭代任务优先级的控制方法。

2 连续迭代投影任务优先级方程

2.1 连续迭代投影算子

引入激活因子，由激活因子构成的激活矩阵，来对任务的插入以及移除进行控制。激活矩阵定义如下：

(10)

式中：hi∈[0,1](i=1,2,…,k)是第i个任务的激活因子，用于表示第i个任务的激活状态。当hi=0，第i个任务完全失活，被移除；当hi=1，第i个任务完全激活，被执行；当hi∈(0,1)，任务i被部分激活。

基于激活因子和激活矩阵，在文献[16]中，构建了连续迭代投影算子如下：

(11)

连续迭代投影算子PNH的连续性与激活因子hi和迭代次数N有关。激活因子hi在[0,1]之间，hi平滑连续变化，直接影响连续迭代投影算子PNH的连续性。此外，迭代次数N也会对连续迭代投影算子PNH的连续性产生影响。PNH可以写作关于hi的多项式形式，对于有限的迭代次数N，PNH相对于激活因子的连续性可以得到保证。当迭代次数趋于无穷时，PNH相对于激活因子是不连续的。

由(In-(HJ)†HJ)矩阵的对称性质可知：

(In-(HJ)†HJ)=(In-(HJ)†HJ)T=

(12)

(13)

(14)

(15)

根据式(13)，又由于(HJ)†HJ(HJ)†=(HJ)†，可以得到：

(16)

综上所述，连续迭代投影算子PNH的连续性和激活因子hi和迭代次数N有关。迭代次数N可以保证连续迭代投影算子的连续性。然而，当迭代次数趋于无穷大时，逐渐接近(I-(HJ)†HJ)，连续迭代投影算子出现不连续现象。

2.2 多任务连续迭代投影优先级方程

基于连续迭代投影算子，类似于式(8)，构建连续迭代投影多任务优先级方程如下：

(17)

当任务i的激活因子hi=1时，根据式(16)可知：

(18)

对于雅克比矩阵Ji有Ji(In-(HJ)†HJ)=0，式(18)可以变换为：

(19)

因此，

(20)

(21)

(22)

(23)

由式(22)(23)可得，对于处于完全激活状态的任务，即hi=1(i=1,2,…,k)，迭代次数N趋于无穷时，任务优先级层次有效。

3 李雅普诺夫稳定性分析

(24)

(25)

同样对于任务T2有：

(26)

对于任务误差

(27)

可以得到：

(28)

(29)

V(e)对时间求导可得：

(30)

将式(17)带入式(30)可得：

(31)

可以整合为：

(32)

对于M的子矩阵M11有：

(33)

M11=J1(J1)†K1=K1。

(34)

K1是对角正定矩阵，因此，子矩阵M11也是正定的。当连续迭代次数N趋于无穷时，对于子矩阵Μ12有：

(35)

对于子矩阵M22有：

(36)

(37)

K2是对角正定矩阵，因此，子矩阵M22也是正定的。

4 仿真分析

4.1 仿真案例

通过对六自由度平面机械臂进行MATLAB仿真，完成有障碍物的轨迹跟踪任务，从而验证上一节提出的控制方法的有效性。在仿真过程中，设置目标点和障碍物，通过高次多项式对连杆末端轨迹进行规划，并完成对轨迹的跟踪，当连杆靠近障碍物时，激活避障任务。通过仿真，来验证所提出的控制算法的优点及有效性。

六自由度平面机械臂的每个杆长都为10 cm，连杆初始角度设置为q0=[0° 20° 30° 30° 30° 30°]T。连杆末端终点在笛卡尔空间的坐标为xf=[3.5 2.0]Tcm。柱形障碍物的中心点O的坐标为(10,10) cm，半径为ro=6.5 cm。在连杆末端对轨迹跟踪的过程中，所有连杆都有与障碍物碰撞的危险，第i个连杆与障碍物的最近距离为：

(38)

式中:Cix和Ciy为连杆i上距离障碍物最近点Ci的坐标，Ox和Oy为障碍物中心点O的坐标。为了保证连杆i与障碍物不发生碰撞，则连杆i与障碍物的距离di大于0。为了避免碰撞，连杆i的避障任务可用公式表示为：

(39)

(40)

vi=hivo。

(41)

仿真过程中，采用连续迭代投影多任务优先级方法式(17)，以避障任务为首要任务，末端轨迹跟踪任务在避障任务的零空间中运行，可以表示为：

(42)

式中六个连杆的避障任务的雅克比Jo为：

(43)

(44)

(45)

避障任务为首要任务，末端轨迹跟踪任务为次要任务，引入激活矩阵，根据式(8)采用增广雅克比零空间投影多任务优先级方法可以表示为：

(46)

式中：六个连杆避障任务的增广零空间P1=In-(HoJo)†HoJo。

对于仿真中可能出现的奇异问题，引入了阻尼因子来避免奇异带来的关节速度不连续。对于矩阵A，引入阻尼因子λ时，其伪逆的形式如下：

(47)

式中阻尼因子λ的设置如下：

(48)

在仿真过程中，迭代次数N=10，末端跟踪反馈增益矩阵为Ke=10In，控制周期T=5 ms。

4.2 仿真结果

连续迭代投影多任务优先级方法仿真结果如图1所示。图1为连续迭代投影多任务优先级方法式(42)仿真结果。图1(a)展示了连续迭代投影任务优先级算法仿真的运动过程。在初始运动阶段t=[0,2.9)s 时，所有连杆远离柱形障碍物，避障任务的激活因子都为零，自由度用于执行轨迹跟踪任务，连杆末端以直线形式朝着目标点靠近。之后连杆靠近障碍物，连杆6、5、3的避障任务依次激活，激活因子从零连续平滑增大，避障任务具有较高的优先级，得到保证，关节速度的变化也为连续的。由于末端轨迹任务处于次优先级，跟踪任务出现了误差，跟踪任务的误差在运动结束时收敛到了零。

图1 连续迭代投影多任务优先级方法仿真结果Fig. 1 The simulation results of iteratively successive projection multi-task priority method

图2 增广投影多任务优先级方法仿真结果Fig. 2 The simulation results of augmented projection multi-task priority method

图2为增广投影多任务优先级方法式(46)的仿真结果。在t=2.9 s之前，连杆末端以直线形式朝着目标点前进，逐渐靠近圆柱形障碍物。在t=2.9 s之后，避障任务逐渐激活，关节速度发生突变震荡，由于阻尼因子的作用，关节速度没有出现不连续。由于避障任务处于较高优先级，在运行过程中避障任务始终得到了保证。但需要注意到的是，增广投影多任务优先级方法采用阻尼因子来保证关节速度和投影算子的连续性，在任务结束时，避障任务不需要执行之后，高优先级的自由度没有得到释放，跟踪任务处于次优先级，跟踪任务误差没有收敛到零。

对比两种方法的仿真结果，可知相比于增广投影多任务优先级方法，连续迭代投影多任务优先级方法能够很好地保证关节速度的连续性。且可以在任务移除后，释放自由度，在冗余度的范围内尽可能地保证低优先级任务。

5 结 语

1)基于连续迭代投影原理，提出了连续迭代投影多任务优先级方法。当任务处于完全激活状态，迭代次数趋于无穷大时，可以保证任务优先级层次的有效性。

2)根据李雅普诺夫稳定性理论，当连续迭代投影算子的迭代次数趋于无穷大时，证明了所提出的连续迭代投影多任务优先级方法的稳定性。

3)设置柱形避障物，以避障任务为首要任务，末端轨迹跟踪任务为次要任务。对比增广投影多任务优先级方法，在六连杆的平面机械臂上仿真验证了所提出方法的有效性和稳定性。在仿真过程中，当避障任务不需要考虑时，自由度用于完成末端轨迹跟踪任务；当连杆靠近障碍物时，避障任务分别被激活，避障任务得到保证；在运动结束时，末端轨迹跟踪任务的误差收敛到了零。

4)在推导过程中多以连续迭代投影算子的迭代次数趋于无穷大为条件。然而，迭代次数较小时，可以保证关节速度的连续性；迭代次数增大时，连续迭代投影多任务优先级方法闭环系统的稳定性更好，关节速度的连续性难以得到保证。迭代次数的折中选取仍是需要深入探讨的问题。