連續迭代投影多任務優先級方法

王娖蕓，唐先智，江 沛，王 彥，李華強

(重慶大學 機械傳動國家重點實驗室，重慶 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)在推導過程中多以連續迭代投影算子的迭代次數趨于無窮大為條件。然而，迭代次數較小時，可以保證關節速度的連續性；迭代次數增大時，連續迭代投影多任務優先級方法閉環系統的穩定性更好，關節速度的連續性難以得到保證。迭代次數的折中選取仍是需要深入探討的問題。