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

(重慶大學 機械傳動國家重點實驗室，重慶 400044)

機器人的冗余度使機器人能夠在非結(jié)構(gòu)化的復(fù)雜環(huán)境中運行，能夠同時完成多個任務(wù)。經(jīng)典的利用冗余度的方法有梯度投影法[1-2]、增廣雅克比方法[3-4]和加權(quán)最小范數(shù)法[5-9]。多任務(wù)優(yōu)先級算法使低優(yōu)先級任務(wù)在高優(yōu)先級任務(wù)的零空間中運行，保證首先完成高優(yōu)先級任務(wù)，利用冗余度完成低優(yōu)先級任務(wù)。Nakamura等[10]最先提出了任務(wù)優(yōu)先級概念，采用零空間投影技術(shù)，實現(xiàn)了迭代梯度投影，保證低優(yōu)先級任務(wù)在不影響高優(yōu)先級任務(wù)的前提下執(zhí)行，并成功用于避障[2]。在之前研究的基礎(chǔ)上，Siciliano等[11]給出了高冗余度系統(tǒng)多任務(wù)優(yōu)先級通用算法，能夠迭代求解關(guān)節(jié)速度。

復(fù)雜的非結(jié)構(gòu)化環(huán)境中，在不同的時間和位置，機器人需要完成任務(wù)不同，需要進行任務(wù)的插入或移除，這會引起雅克比矩陣秩的變化，出現(xiàn)關(guān)節(jié)速度的不連續(xù)現(xiàn)象[12]。解決任務(wù)優(yōu)先級方法中關(guān)節(jié)速度的不連續(xù)問題是非常關(guān)鍵的。Chiaverini[13]提出奇異魯棒方法，通過引入阻尼因子，以跟蹤誤差為代價，解決雅克比矩陣秩的變化引起的奇異問題。在文獻[14]中，提出了一種基于連續(xù)零空間投影算子的平滑控制方法，以較低的計算成本解決了任務(wù)狀態(tài)切換時的不連續(xù)問題，但即使所有任務(wù)都處于激活狀態(tài)或失活狀態(tài)，該方法系統(tǒng)的穩(wěn)定性仍然存在問題。Mansard等[15]提出了一種連續(xù)逆，可以保證在任務(wù)切換過程中的關(guān)節(jié)速度連續(xù)性，并且當任務(wù)完全激活或失效時，其連續(xù)逆等于偽逆，之后應(yīng)用于處理多個優(yōu)先級任務(wù)[16]。但Mansard等提出的連續(xù)逆隨著任務(wù)數(shù)量的增加，計算時間也呈指數(shù)增長。之后，Jiang等[17]和黃水華[18]通過連續(xù)迭代投影每個任務(wù)的零空間構(gòu)建了連續(xù)迭代投影算子，有限次迭代能夠保證任務(wù)切換過程中的連續(xù)性，當?shù)螖?shù)接近無限次時，連續(xù)迭代投影算子逐漸接近經(jīng)典偽逆投影算子。和Mansard等提出的方法相比，此方法可以節(jié)省計算時間。

綜上所述，需要針對任務(wù)優(yōu)先級算法中任務(wù)插入與移除引起的關(guān)節(jié)速度不連續(xù)問題，進行深入研究和分析。筆者基于文獻[17]的連續(xù)迭代投影原理，提出連續(xù)迭代投影多任務(wù)優(yōu)先級方法，依據(jù)李雅普諾夫穩(wěn)定性原理證明所提出方法的穩(wěn)定性。通過仿真驗證該方法的有效性和穩(wěn)定性，并與增廣投影多任務(wù)優(yōu)先級方法進行對比。

1 機器人運動學控制

1.1 機器人運動學控制

冗余串聯(lián)機器人系統(tǒng)的自由度大于其完成期望任務(wù)的自由度，即關(guān)節(jié)空間的維數(shù)n超過任務(wù)空間的維數(shù)m,n與m的差值為冗余自由度。定義關(guān)節(jié)空間關(guān)節(jié)位置向量為q∈Rn，任務(wù)空間末端位置向量為x∈Rm，則機器人正向運動學方程為：

x=f(q)，

(1)

式中：f為任務(wù)函數(shù)，為從關(guān)節(jié)空間向量到任務(wù)空間向量的映射。對式(1)求導可以得到關(guān)節(jié)速度和末端執(zhí)行器速度的映射關(guān)系，如下：

(2)

式中：J∈Rm×n為雅克比矩陣。對于冗余機器人，式(2)的一般解可表示為：

(3)

P=In-J?J，

(4)

式中In為n維的單位方陣。

冗余機器人可以同時執(zhí)行多個任務(wù)。如跟蹤末端軌跡的同時，進行避障、保證關(guān)節(jié)限位等。但需要對多個任務(wù)的優(yōu)先級進行排序，來保證首要任務(wù)在高優(yōu)先級層次被執(zhí)行，利用冗余的自由度來完成次要任務(wù)。為了簡單起見，先考慮T1和T2兩個任務(wù)，這兩個任務(wù)定義為：

x1=f1(q)，

(5)

x2=f2(q)。

(6)

假設(shè)T1是首要任務(wù)，任務(wù)T2在任務(wù)T1的零空間中執(zhí)行。任務(wù)T1和T2對應(yīng)的雅克比矩陣分別為J1和J2。根據(jù)式(3)，則關(guān)節(jié)速度的解為：

(7)

(8)

(9)

低優(yōu)先級任務(wù)的關(guān)節(jié)速度投影在高優(yōu)先級任務(wù)的增廣零空間中，這種方法為增廣投影法。

在不同的時間點以及當機器人關(guān)節(jié)移動到不同位置時，需要進行任務(wù)的插入與移除。如：在進行末端期望軌跡跟蹤的同時，當機器人靠近障礙物時，需要考慮避障任務(wù)，且需要將避障任務(wù)插入優(yōu)先級層次中；當機器人遠離障礙物時，則不需要考慮避障任務(wù)，可以將避障任務(wù)移除，將冗余度用于完成其他任務(wù)。

式(8)在任務(wù)插入與移除時，會出現(xiàn)關(guān)節(jié)速度不連續(xù)的問題，不能滿足需求。因此，將根據(jù)連續(xù)迭代投影原理，構(gòu)建一種新型連續(xù)迭代任務(wù)優(yōu)先級的控制方法。

2 連續(xù)迭代投影任務(wù)優(yōu)先級方程

2.1 連續(xù)迭代投影算子

引入激活因子，由激活因子構(gòu)成的激活矩陣，來對任務(wù)的插入以及移除進行控制。激活矩陣定義如下：

(10)

式中：hi∈[0,1](i=1,2,…,k)是第i個任務(wù)的激活因子，用于表示第i個任務(wù)的激活狀態(tài)。當hi=0，第i個任務(wù)完全失活，被移除；當hi=1，第i個任務(wù)完全激活，被執(zhí)行；當hi∈(0,1)，任務(wù)i被部分激活。

基于激活因子和激活矩陣，在文獻[16]中，構(gòu)建了連續(xù)迭代投影算子如下：

(11)

連續(xù)迭代投影算子PNH的連續(xù)性與激活因子hi和迭代次數(shù)N有關(guān)。激活因子hi在[0,1]之間，hi平滑連續(xù)變化，直接影響連續(xù)迭代投影算子PNH的連續(xù)性。此外，迭代次數(shù)N也會對連續(xù)迭代投影算子PNH的連續(xù)性產(chǎn)生影響。PNH可以寫作關(guān)于hi的多項式形式，對于有限的迭代次數(shù)N，PNH相對于激活因子的連續(xù)性可以得到保證。當?shù)螖?shù)趨于無窮時，PNH相對于激活因子是不連續(xù)的。

由(In-(HJ)?HJ)矩陣的對稱性質(zhì)可知：

(In-(HJ)?HJ)=(In-(HJ)?HJ)T=

(12)

(13)

(14)

(15)

根據(jù)式(13)，又由于(HJ)?HJ(HJ)?=(HJ)?，可以得到：

(16)

綜上所述，連續(xù)迭代投影算子PNH的連續(xù)性和激活因子hi和迭代次數(shù)N有關(guān)。迭代次數(shù)N可以保證連續(xù)迭代投影算子的連續(xù)性。然而，當?shù)螖?shù)趨于無窮大時，逐漸接近(I-(HJ)?HJ)，連續(xù)迭代投影算子出現(xiàn)不連續(xù)現(xiàn)象。

2.2 多任務(wù)連續(xù)迭代投影優(yōu)先級方程

基于連續(xù)迭代投影算子，類似于式(8)，構(gòu)建連續(xù)迭代投影多任務(wù)優(yōu)先級方程如下：

(17)

當任務(wù)i的激活因子hi=1時，根據(jù)式(16)可知：

(18)

對于雅克比矩陣Ji有Ji(In-(HJ)?HJ)=0，式(18)可以變換為：

(19)

因此，

(20)

(21)

(22)

(23)

由式(22)(23)可得，對于處于完全激活狀態(tài)的任務(wù)，即hi=1(i=1,2,…,k)，迭代次數(shù)N趨于無窮時，任務(wù)優(yōu)先級層次有效。

3 李雅普諾夫穩(wěn)定性分析

(24)

(25)

同樣對于任務(wù)T2有：

(26)

對于任務(wù)誤差

(27)

可以得到：

(28)

(29)

V(e)對時間求導可得：

(30)

將式(17)帶入式(30)可得：

(31)

可以整合為：

(32)

對于M的子矩陣M11有：

(33)

M11=J1(J1)?K1=K1。

(34)

K1是對角正定矩陣，因此，子矩陣M11也是正定的。當連續(xù)迭代次數(shù)N趨于無窮時，對于子矩陣Μ12有：

(35)

對于子矩陣M22有：

(36)

(37)

K2是對角正定矩陣，因此，子矩陣M22也是正定的。

4 仿真分析

4.1 仿真案例

通過對六自由度平面機械臂進行MATLAB仿真，完成有障礙物的軌跡跟蹤任務(wù)，從而驗證上一節(jié)提出的控制方法的有效性。在仿真過程中，設(shè)置目標點和障礙物，通過高次多項式對連桿末端軌跡進行規(guī)劃，并完成對軌跡的跟蹤，當連桿靠近障礙物時，激活避障任務(wù)。通過仿真，來驗證所提出的控制算法的優(yōu)點及有效性。

六自由度平面機械臂的每個桿長都為10 cm，連桿初始角度設(shè)置為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與障礙物不發(fā)生碰撞，則連桿i與障礙物的距離di大于0。為了避免碰撞，連桿i的避障任務(wù)可用公式表示為：

(39)

(40)

vi=hivo。

(41)

仿真過程中，采用連續(xù)迭代投影多任務(wù)優(yōu)先級方法式(17)，以避障任務(wù)為首要任務(wù)，末端軌跡跟蹤任務(wù)在避障任務(wù)的零空間中運行，可以表示為：

(42)

式中六個連桿的避障任務(wù)的雅克比Jo為：

(43)

(44)

(45)

避障任務(wù)為首要任務(wù)，末端軌跡跟蹤任務(wù)為次要任務(wù)，引入激活矩陣，根據(jù)式(8)采用增廣雅克比零空間投影多任務(wù)優(yōu)先級方法可以表示為：

(46)

式中：六個連桿避障任務(wù)的增廣零空間P1=In-(HoJo)?HoJo。

對于仿真中可能出現(xiàn)的奇異問題，引入了阻尼因子來避免奇異帶來的關(guān)節(jié)速度不連續(xù)。對于矩陣A，引入阻尼因子λ時，其偽逆的形式如下：

(47)

式中阻尼因子λ的設(shè)置如下：

(48)

在仿真過程中，迭代次數(shù)N=10，末端跟蹤反饋增益矩陣為Ke=10In，控制周期T=5 ms。

4.2 仿真結(jié)果

連續(xù)迭代投影多任務(wù)優(yōu)先級方法仿真結(jié)果如圖1所示。圖1為連續(xù)迭代投影多任務(wù)優(yōu)先級方法式(42)仿真結(jié)果。圖1(a)展示了連續(xù)迭代投影任務(wù)優(yōu)先級算法仿真的運動過程。在初始運動階段t=[0,2.9)s 時，所有連桿遠離柱形障礙物，避障任務(wù)的激活因子都為零，自由度用于執(zhí)行軌跡跟蹤任務(wù)，連桿末端以直線形式朝著目標點靠近。之后連桿靠近障礙物，連桿6、5、3的避障任務(wù)依次激活，激活因子從零連續(xù)平滑增大，避障任務(wù)具有較高的優(yōu)先級，得到保證，關(guān)節(jié)速度的變化也為連續(xù)的。由于末端軌跡任務(wù)處于次優(yōu)先級，跟蹤任務(wù)出現(xiàn)了誤差，跟蹤任務(wù)的誤差在運動結(jié)束時收斂到了零。

圖1 連續(xù)迭代投影多任務(wù)優(yōu)先級方法仿真結(jié)果Fig. 1 The simulation results of iteratively successive projection multi-task priority method

圖2 增廣投影多任務(wù)優(yōu)先級方法仿真結(jié)果Fig. 2 The simulation results of augmented projection multi-task priority method

圖2為增廣投影多任務(wù)優(yōu)先級方法式(46)的仿真結(jié)果。在t=2.9 s之前，連桿末端以直線形式朝著目標點前進，逐漸靠近圓柱形障礙物。在t=2.9 s之后，避障任務(wù)逐漸激活，關(guān)節(jié)速度發(fā)生突變震蕩，由于阻尼因子的作用，關(guān)節(jié)速度沒有出現(xiàn)不連續(xù)。由于避障任務(wù)處于較高優(yōu)先級，在運行過程中避障任務(wù)始終得到了保證。但需要注意到的是，增廣投影多任務(wù)優(yōu)先級方法采用阻尼因子來保證關(guān)節(jié)速度和投影算子的連續(xù)性，在任務(wù)結(jié)束時，避障任務(wù)不需要執(zhí)行之后，高優(yōu)先級的自由度沒有得到釋放，跟蹤任務(wù)處于次優(yōu)先級，跟蹤任務(wù)誤差沒有收斂到零。

對比兩種方法的仿真結(jié)果，可知相比于增廣投影多任務(wù)優(yōu)先級方法，連續(xù)迭代投影多任務(wù)優(yōu)先級方法能夠很好地保證關(guān)節(jié)速度的連續(xù)性。且可以在任務(wù)移除后，釋放自由度，在冗余度的范圍內(nèi)盡可能地保證低優(yōu)先級任務(wù)。

5 結(jié) 語

1)基于連續(xù)迭代投影原理，提出了連續(xù)迭代投影多任務(wù)優(yōu)先級方法。當任務(wù)處于完全激活狀態(tài)，迭代次數(shù)趨于無窮大時，可以保證任務(wù)優(yōu)先級層次的有效性。

2)根據(jù)李雅普諾夫穩(wěn)定性理論，當連續(xù)迭代投影算子的迭代次數(shù)趨于無窮大時，證明了所提出的連續(xù)迭代投影多任務(wù)優(yōu)先級方法的穩(wěn)定性。

3)設(shè)置柱形避障物，以避障任務(wù)為首要任務(wù)，末端軌跡跟蹤任務(wù)為次要任務(wù)。對比增廣投影多任務(wù)優(yōu)先級方法，在六連桿的平面機械臂上仿真驗證了所提出方法的有效性和穩(wěn)定性。在仿真過程中，當避障任務(wù)不需要考慮時，自由度用于完成末端軌跡跟蹤任務(wù)；當連桿靠近障礙物時，避障任務(wù)分別被激活，避障任務(wù)得到保證；在運動結(jié)束時，末端軌跡跟蹤任務(wù)的誤差收斂到了零。

4)在推導過程中多以連續(xù)迭代投影算子的迭代次數(shù)趨于無窮大為條件。然而，迭代次數(shù)較小時，可以保證關(guān)節(jié)速度的連續(xù)性；迭代次數(shù)增大時，連續(xù)迭代投影多任務(wù)優(yōu)先級方法閉環(huán)系統(tǒng)的穩(wěn)定性更好，關(guān)節(jié)速度的連續(xù)性難以得到保證。迭代次數(shù)的折中選取仍是需要深入探討的問題。