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

王娖蕓，唐先智，江 沛，王 彥，李華強(qiáng)

(重慶大學(xué) 機(jī)械傳動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，重慶 400044)

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

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

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

1 機(jī)器人運(yùn)動(dòng)學(xué)控制

1.1 機(jī)器人運(yùn)動(dòng)學(xué)控制

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

x=f(q)，

(1)

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

(2)

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

(3)

P=In-J?J，

(4)

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

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

x1=f1(q)，

(5)

x2=f2(q)。

(6)

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

(7)

(8)

(9)

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

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

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

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

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

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

(10)

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

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

(11)

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

由(In-(HJ)?HJ)矩陣的對(duì)稱性質(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ù)性。然而，當(dāng)?shù)螖?shù)趨于無(wú)窮大時(shí)，逐漸接近(I-(HJ)?HJ)，連續(xù)迭代投影算子出現(xiàn)不連續(xù)現(xiàn)象。

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

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

(17)

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

(18)

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

(19)

因此，

(20)

(21)

(22)

(23)

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

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

(24)

(25)

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

(26)

對(duì)于任務(wù)誤差

(27)

可以得到：

(28)

(29)

V(e)對(duì)時(shí)間求導(dǎo)可得：

(30)

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

(31)

可以整合為：

(32)

對(duì)于M的子矩陣M11有：

(33)

M11=J1(J1)?K1=K1。

(34)

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

(35)

對(duì)于子矩陣M22有：

(36)

(37)

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

4 仿真分析

4.1 仿真案例

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

六自由度平面機(jī)械臂的每個(gè)桿長(zhǎng)都為10 cm，連桿初始角度設(shè)置為q0=[0° 20° 30° 30° 30° 30°]T。連桿末端終點(diǎn)在笛卡爾空間的坐標(biāo)為xf=[3.5 2.0]Tcm。柱形障礙物的中心點(diǎn)O的坐標(biāo)為(10,10) cm，半徑為ro=6.5 cm。在連桿末端對(duì)軌跡跟蹤的過(guò)程中，所有連桿都有與障礙物碰撞的危險(xiǎn)，第i個(gè)連桿與障礙物的最近距離為：

(38)

式中:Cix和Ciy為連桿i上距離障礙物最近點(diǎn)Ci的坐標(biāo)，Ox和Oy為障礙物中心點(diǎn)O的坐標(biāo)。為了保證連桿i與障礙物不發(fā)生碰撞，則連桿i與障礙物的距離di大于0。為了避免碰撞，連桿i的避障任務(wù)可用公式表示為：

(39)

(40)

vi=hivo。

(41)

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

(42)

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

(43)

(44)

(45)

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

(46)

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

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

(47)

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

(48)

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

4.2 仿真結(jié)果

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

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

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

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

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

5 結(jié) 語(yǔ)

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

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

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

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