空間雙臂機(jī)器人抓捕非合作目標(biāo)后的協(xié)調(diào)穩(wěn)定控制

夏鵬程,羅建軍,王明明,*,譚龍玉

1.西北工業(yè)大學(xué) 深圳研究院,深圳 518057

2.西北工業(yè)大學(xué) 航天動(dòng)力學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室,西安 710072 3.上海航天控制技術(shù)研究所,上海 201109

近年來(lái),隨著空間技術(shù)與應(yīng)用的發(fā)展,利用空間機(jī)器人維修失效衛(wèi)星、操控非合作目標(biāo)等在軌服務(wù)任務(wù)日益引人關(guān)注?？臻g多臂機(jī)器人相較于空間單臂機(jī)器人,具有更強(qiáng)的操作能力和更廣泛的應(yīng)用,但操作更為復(fù)雜。因此,空間多臂機(jī)器人捕獲非合作目標(biāo)的操控技術(shù)成為當(dāng)前研究的熱點(diǎn)和完成任務(wù)的關(guān)鍵。空間多臂機(jī)器人捕獲非合作目標(biāo)的過(guò)程可分為抓捕前、抓捕中和抓捕后等三個(gè)階段。本文研究空間雙臂機(jī)器人抓捕非合作目標(biāo)后的安全協(xié)調(diào)穩(wěn)定控制問(wèn)題。

針對(duì)空間機(jī)器人抓捕非合作目標(biāo)后的穩(wěn)定控制,相關(guān)研究一般將該階段的控制問(wèn)題拆分為兩個(gè)子問(wèn)題：期望軌跡規(guī)劃問(wèn)題和跟蹤控制問(wèn)題。為了規(guī)劃空間機(jī)器人穩(wěn)定目標(biāo)運(yùn)動(dòng)的期望軌跡,研究者們通常將抓捕后階段的穩(wěn)定軌跡規(guī)劃問(wèn)題轉(zhuǎn)化為優(yōu)化問(wèn)題進(jìn)行求解。對(duì)于慣性參數(shù)無(wú)法準(zhǔn)確獲得的非合作目標(biāo),通常利用其慣性參數(shù)的估計(jì)值規(guī)劃參考期望運(yùn)動(dòng)軌跡。Yoshida等從能量分配的角度規(guī)劃了空間機(jī)器人的運(yùn)動(dòng)軌跡。Zappa等則從動(dòng)量守恒的角度,規(guī)劃自由飛行機(jī)器人的期望運(yùn)動(dòng)軌跡。Wang等利用樣條曲線對(duì)空間機(jī)器人的關(guān)節(jié)運(yùn)動(dòng)進(jìn)行參數(shù)化處理,提出了針對(duì)冗余機(jī)械臂的軌跡規(guī)劃方法。Yu等利用機(jī)械臂的冗余度,提出了一種考慮優(yōu)先級(jí)的空間多臂機(jī)器人期望軌跡規(guī)劃方法。在抓捕后階段,由于目標(biāo)慣性參數(shù)無(wú)法準(zhǔn)確地獲得,因而無(wú)法有效地控制空間機(jī)器人跟蹤期望運(yùn)動(dòng)軌跡從而實(shí)現(xiàn)目標(biāo)的穩(wěn)定。為了避免目標(biāo)不確定性對(duì)基座運(yùn)動(dòng)控制的影響,Sharf等提出了基座無(wú)擾的穩(wěn)定控制策略。Abiko等則考慮目標(biāo)不確定性對(duì)期望軌跡跟蹤的影響,根據(jù)目標(biāo)的不確定性設(shè)計(jì)了自適應(yīng)穩(wěn)定控制器。由于非合作目標(biāo)的慣性參數(shù)具有不確定性,加之上述控制方法未考慮控制過(guò)程中機(jī)械臂末端受到的接觸力與力矩,空間機(jī)器人無(wú)法在規(guī)劃的接觸力范圍內(nèi)跟蹤參考期望運(yùn)動(dòng),無(wú)法保證空間機(jī)器人與目標(biāo)交互的安全。

為了實(shí)現(xiàn)抓捕后階段機(jī)器人與目標(biāo)與交互的柔順和安全,降低機(jī)械臂末端與目標(biāo)之間的接觸力/力矩,研究者們將柔順控制方法引入到空間機(jī)器人抓捕目標(biāo)的相關(guān)研究中。柔順控制方法通過(guò)設(shè)計(jì)控制律,將機(jī)器人與目標(biāo)的交互過(guò)程等效為一個(gè)質(zhì)量-彈簧-阻尼系統(tǒng),繼而實(shí)現(xiàn)交互過(guò)程可控的柔順接觸。描述該質(zhì)量-彈簧-阻尼系統(tǒng)的等式被稱為柔順等式。為了保證機(jī)械臂末端的接觸安全,Yoshida等根據(jù)柔順等式提出了空間機(jī)器人的阻抗控制方法,設(shè)計(jì)了調(diào)整柔順等式參數(shù)的自適應(yīng)律,根據(jù)接觸力與力矩調(diào)整跟蹤控制律,實(shí)現(xiàn)交互過(guò)程中的柔順接觸。Wang等利用柔順等式提出了期望運(yùn)動(dòng)軌跡的調(diào)整方法,通過(guò)調(diào)整期望軌跡實(shí)現(xiàn)了目標(biāo)穩(wěn)定過(guò)程中的接觸柔順,使得跟蹤控制器的控制性能不受接觸力與力矩的影響。Stolfi等將一般的位置跟蹤控制與柔順控制相結(jié)合,提出了力/位混合控制方法,僅在需要實(shí)現(xiàn)柔順接觸的方向進(jìn)行柔順控制。然而上述研究?jī)H針對(duì)單臂空間機(jī)器人,而且只考慮了目標(biāo)慣性參數(shù)不確定性對(duì)接觸力與力矩的影響。當(dāng)雙臂同時(shí)操控被抓捕目標(biāo)時(shí),雙臂對(duì)目標(biāo)會(huì)產(chǎn)生拉扯/擠壓作用,可能對(duì)目標(biāo)會(huì)造成損傷,機(jī)械臂末端與目標(biāo)之間可能產(chǎn)生過(guò)大的接觸力與力矩,因此,利用空間雙臂機(jī)器人穩(wěn)定和操控非合作目標(biāo)時(shí),除了需要考慮目標(biāo)不確定性對(duì)接觸力與力矩的影響,還需要進(jìn)一步地考慮雙臂拉扯/擠壓目標(biāo)產(chǎn)生的接觸力與力矩影響。

針對(duì)空間雙臂機(jī)器人抓捕非合作目標(biāo)后的安全穩(wěn)定問(wèn)題,同時(shí)考慮非合作目標(biāo)慣性參數(shù)的不確定性以及雙臂操控目標(biāo)的拉扯/擠壓作用,提出了一種協(xié)調(diào)穩(wěn)定控制方法,通過(guò)協(xié)調(diào)地控制雙臂的運(yùn)動(dòng)實(shí)現(xiàn)雙臂末端與目標(biāo)接觸柔順,降低穩(wěn)定過(guò)程中產(chǎn)生的接觸力與力矩。首先,利用目標(biāo)慣性參數(shù)的估值規(guī)劃穩(wěn)定目標(biāo)的運(yùn)動(dòng)軌跡。然后,針對(duì)目標(biāo)慣性參數(shù)不確定性,對(duì)規(guī)劃的目標(biāo)穩(wěn)定軌跡進(jìn)行調(diào)整；同時(shí),基于抓捕矩陣零空間內(nèi)存在不影響目標(biāo)運(yùn)動(dòng)的無(wú)效運(yùn)動(dòng)旋量反應(yīng)了雙臂對(duì)目標(biāo)的拉扯/擠壓,對(duì)雙臂末端的期望運(yùn)動(dòng)進(jìn)行調(diào)整；根據(jù)運(yùn)動(dòng)學(xué)約束求解逆運(yùn)動(dòng)學(xué)得到雙臂運(yùn)動(dòng)協(xié)調(diào)的安全穩(wěn)定運(yùn)動(dòng)軌跡；在設(shè)計(jì)的控制性能范圍內(nèi)對(duì)安全穩(wěn)定運(yùn)動(dòng)軌跡進(jìn)行有效跟蹤,協(xié)調(diào)地控制雙臂運(yùn)動(dòng)實(shí)現(xiàn)接觸柔順的安全穩(wěn)定控制。最后,通過(guò)仿真算例驗(yàn)證了本文協(xié)調(diào)穩(wěn)定控制方法的有效性。

1 動(dòng)力學(xué)建模

研究對(duì)象空間雙臂機(jī)器人如圖1所示。該空間機(jī)器人由兩個(gè)對(duì)稱安裝的七關(guān)節(jié)空間機(jī)械臂和可以六自由度運(yùn)動(dòng)的基座組成。在空間雙臂機(jī)器人捕獲非合作目標(biāo)的抓捕后階段,空間機(jī)械臂的末端與目標(biāo)固連形成組合體。

圖1 空間雙臂機(jī)器人抓捕目標(biāo)示意圖Fig.1 Snapshot of a dual-arm space robot capturing a target

用表示向量∈R對(duì)應(yīng)的反對(duì)稱矩陣。為了便于定義目標(biāo)的六自由度運(yùn)動(dòng),根據(jù)向量,∈R定義運(yùn)算：

式中：為目標(biāo)的慣性矩陣；為目標(biāo)質(zhì)心到抓捕點(diǎn)的雅可比矩陣即抓捕矩陣。由于抓捕后階段目標(biāo)與空間機(jī)器人雙臂的末端在抓捕點(diǎn)處固定連接,因此目標(biāo)與空間機(jī)器人的運(yùn)動(dòng)滿足運(yùn)動(dòng)學(xué)約束：

2 安全穩(wěn)定控制問(wèn)題分析

當(dāng)空間機(jī)器人的雙臂同時(shí)作用于非合作目標(biāo)時(shí),描述末端接觸力與力矩的旋量和作用于目標(biāo)質(zhì)心的運(yùn)動(dòng)旋量滿足：

式中：運(yùn)動(dòng)旋量∈R被稱為控制目標(biāo)運(yùn)動(dòng)的有效運(yùn)動(dòng)旋量。與空間單臂機(jī)器人不同,當(dāng)兩個(gè)機(jī)械臂同時(shí)作用于目標(biāo)時(shí),在雅可比矩陣的零空間內(nèi)會(huì)產(chǎn)生不影響目標(biāo)運(yùn)動(dòng)的無(wú)效運(yùn)動(dòng)旋量,其表達(dá)式為

式中：?表示矩陣的廣義逆；∈R為單位矩陣；∈R為反應(yīng)拉扯、擠壓程度的旋量。無(wú)效運(yùn)動(dòng)旋量反映了兩個(gè)機(jī)械臂與目標(biāo)之間的拉扯/擠壓,會(huì)對(duì)機(jī)械臂末端的接觸力/力矩產(chǎn)生影響。因此,為了機(jī)械臂末端與目標(biāo)接觸的安全,需要在控制空間機(jī)器人的過(guò)程中考慮無(wú)效運(yùn)動(dòng)旋量的影響。

除此之外,由于非合作目標(biāo)的慣性參數(shù)無(wú)法準(zhǔn)確獲得,通常利用目標(biāo)慣性參數(shù)的估值規(guī)劃穩(wěn)定其運(yùn)動(dòng)的參考期望軌跡。根據(jù)式(3)中的目標(biāo)動(dòng)力學(xué)方程可知,參考期望軌跡滿足動(dòng)力學(xué)約束為

由此關(guān)系不難看出,因?yàn)閰?shù)估計(jì)值與真值之間存在誤差,跟蹤參考期望軌跡并不一定能夠滿足規(guī)劃的接觸力與力矩約束,因此跟蹤參考期望軌跡存在威脅機(jī)械臂末端接觸安全的風(fēng)險(xiǎn)。

根據(jù)上述分析可知,為了保證空間雙臂機(jī)器人穩(wěn)定非合作目標(biāo)過(guò)程中的末端接觸安全,需要同時(shí)考慮無(wú)效運(yùn)動(dòng)旋量和目標(biāo)慣性參數(shù)不確定性對(duì)接觸力/力矩產(chǎn)生的影響,協(xié)調(diào)地控制雙臂的運(yùn)動(dòng),降低控制過(guò)程中產(chǎn)生的接觸力與力矩。

3 安全協(xié)調(diào)穩(wěn)定控制

為了同時(shí)處理無(wú)效運(yùn)動(dòng)旋量以及目標(biāo)不確定性對(duì)機(jī)械臂末端與目標(biāo)接觸的影響,基于柔順等式對(duì)規(guī)劃的期望穩(wěn)定運(yùn)動(dòng)軌跡進(jìn)行調(diào)整,求解能夠?qū)崿F(xiàn)目標(biāo)與雙臂末端接觸柔順的安全穩(wěn)定運(yùn)動(dòng)軌跡；通過(guò)在設(shè)計(jì)的控制性能約束范圍內(nèi)有效地跟蹤安全穩(wěn)定運(yùn)動(dòng)軌跡,保障控制的效率,協(xié)調(diào)地控制雙臂運(yùn)動(dòng)實(shí)現(xiàn)接觸的柔順,使得空間雙臂機(jī)器人能夠安全地穩(wěn)定非合作目標(biāo)的運(yùn)動(dòng)。

3.1 目標(biāo)運(yùn)動(dòng)軌跡規(guī)劃

為了實(shí)現(xiàn)空間雙臂機(jī)器人穩(wěn)定非合作目標(biāo)的運(yùn)動(dòng),首先,利用目標(biāo)慣性參數(shù)的估值規(guī)劃穩(wěn)定控制過(guò)程中目標(biāo)的參考期望運(yùn)動(dòng)軌跡。利用貝塞爾曲線對(duì)目標(biāo)的六自由度運(yùn)動(dòng)進(jìn)行參數(shù)化處理,并設(shè)計(jì)穩(wěn)定過(guò)程中目標(biāo)的優(yōu)化運(yùn)動(dòng)軌跡。

選取貝塞爾曲線：

式中：下標(biāo)代表運(yùn)動(dòng)的第個(gè)自由度；4為貝塞爾曲線的階數(shù)；為構(gòu)造二項(xiàng)式系數(shù)的變量；P是構(gòu)建貝塞爾曲線的控制點(diǎn)；是歸一化時(shí)間。根據(jù)目標(biāo)運(yùn)動(dòng)穩(wěn)定時(shí)間,時(shí)間可定義為。考慮到空間非合作目標(biāo)運(yùn)動(dòng)的不確定性、組合體操控的安全性和時(shí)效最優(yōu)性,本文在參考期望軌跡規(guī)劃的過(guò)程中采用了時(shí)間最優(yōu)與接觸力/力矩盡量小和安全的聯(lián)合優(yōu)化方式。根據(jù)式(3)中的目標(biāo)動(dòng)力學(xué)方程,可將穩(wěn)定目標(biāo)的運(yùn)動(dòng)規(guī)劃問(wèn)題轉(zhuǎn)化為優(yōu)化問(wèn)題：

式中：和分別是最優(yōu)時(shí)間和接觸力與力矩的權(quán)重因子；和為目標(biāo)運(yùn)動(dòng)約束的下界與上界；為接觸力與力矩的安全約束。由于該優(yōu)化問(wèn)題同時(shí)考慮了最優(yōu)穩(wěn)定時(shí)間和接觸力/力矩,不會(huì)規(guī)劃過(guò)大的接觸力/力矩,且可以通過(guò)調(diào)節(jié)權(quán)重因子控制最優(yōu)穩(wěn)定時(shí)間的長(zhǎng)短,因此可以根據(jù)任務(wù)需求靈活地規(guī)劃參考期望運(yùn)動(dòng)軌跡。

通過(guò)求解上述的優(yōu)化問(wèn)題,可以規(guī)劃穩(wěn)定過(guò)程中目標(biāo)的參考期望運(yùn)動(dòng)軌跡。

3.2 安全穩(wěn)定運(yùn)動(dòng)軌跡求解

鑒于目標(biāo)運(yùn)動(dòng)軌跡是利用目標(biāo)慣性參數(shù)估值規(guī)劃的,由3.1節(jié)的分析可知,直接跟蹤對(duì)應(yīng)的關(guān)節(jié)與基座運(yùn)動(dòng)軌跡具有一定的風(fēng)險(xiǎn)。提出一種根據(jù)接觸力與力矩調(diào)整穩(wěn)定運(yùn)動(dòng)軌跡的內(nèi)外雙環(huán)式結(jié)構(gòu),利用內(nèi)環(huán)與外環(huán)分別針對(duì)無(wú)效運(yùn)動(dòng)旋量和目標(biāo)不確定性對(duì)接觸力/力矩的影響,對(duì)目標(biāo)參考期望運(yùn)動(dòng)軌跡進(jìn)行柔順化調(diào)整。最終,求解得到安全穩(wěn)定目標(biāo)的空間機(jī)器人關(guān)節(jié)與基座期望運(yùn)動(dòng)軌跡。

綜上,安全穩(wěn)定目標(biāo)的空間機(jī)器人關(guān)節(jié)與基座的期望運(yùn)動(dòng)軌跡生成框圖如圖2所示。經(jīng)過(guò)式(14)和式(16)中柔順等式的調(diào)整,最終得到的期望運(yùn)動(dòng)考慮了無(wú)效運(yùn)動(dòng)旋量和目標(biāo)不確定性對(duì)接觸力/力矩的影響。跟蹤時(shí),空間雙臂機(jī)器人能夠同時(shí)考慮無(wú)效運(yùn)動(dòng)旋量和目標(biāo)不確定性對(duì)機(jī)械臂末端與目標(biāo)接觸產(chǎn)生的影響,在機(jī)械臂末端與目標(biāo)交互過(guò)程中實(shí)現(xiàn)柔順接觸。

圖2 安全穩(wěn)定運(yùn)動(dòng)軌跡求解框圖Fig.2 Schematic diagram of safe stabilization trajectory generation

3.3 約束跟蹤控制器設(shè)計(jì)

為了保證空間機(jī)器人能夠有效地跟蹤安全穩(wěn)定目標(biāo)運(yùn)動(dòng)的期望運(yùn)動(dòng),實(shí)現(xiàn)目標(biāo)與雙臂末端交互的柔順,基于障礙李雅普諾夫函數(shù)設(shè)計(jì)了控制性能可約束的跟蹤控制器。

式中：∈R為由正數(shù)組成的控制性能約束向量。為了使得機(jī)器人跟蹤過(guò)程中滿足設(shè)計(jì)的性能約束,根據(jù)動(dòng)力學(xué)方程式(1),設(shè)計(jì)跟蹤控制律：

其中,障礙李雅普諾夫函數(shù)為

對(duì)式(25)中的求導(dǎo)可得

障礙李雅普諾夫函數(shù)導(dǎo)數(shù)為

利用不等式：

將式(19)中的控制律代入李雅普諾夫函數(shù)的導(dǎo)數(shù)中,可以推得≤0。由此可以推導(dǎo)得到,在跟蹤控制的過(guò)程中,0時(shí)刻和后續(xù)時(shí)刻的李雅普諾夫函數(shù)滿足：

對(duì)不等式兩邊進(jìn)行指數(shù)化處理,整理后可以推得,后續(xù)時(shí)刻的跟蹤誤差滿足約束關(guān)系：

式中： 和 為誤差收斂時(shí)的約束下界與上界。根據(jù)跟蹤誤差的定義以及式(23)和式(24)可知,該跟蹤控制器能夠滿足式(18)中設(shè)計(jì)的控制性能。對(duì)式(30)求導(dǎo)可知,該約束跟蹤控制器能夠約束跟蹤誤差的收斂速率。利用此約束跟蹤控制器,空間雙臂機(jī)器人可以在控制性能約束范圍內(nèi)有效地跟蹤安全穩(wěn)定目標(biāo)的期望運(yùn)動(dòng)軌跡,實(shí)現(xiàn)空間雙臂機(jī)器人抓捕非合作目標(biāo)后的接觸柔順。

綜上,空間雙臂機(jī)器人抓捕非合作目標(biāo)后的安全協(xié)調(diào)穩(wěn)定控制框圖如圖3所示。

圖3 協(xié)調(diào)穩(wěn)定控制結(jié)構(gòu)圖Fig.3 Schematic diagram of coordinated stabilization control

4 仿真校驗(yàn)

設(shè)計(jì)了利用所提方法穩(wěn)定非合作目標(biāo)的仿真算例。空間機(jī)器人的雙臂安裝位置對(duì)稱,空間機(jī)器人的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)參數(shù)如表1所示。目標(biāo)慣性參數(shù)的估值為真實(shí)值的80%。

表1 空間機(jī)器人的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)參數(shù)Table 1 Kinematics and dynamics parameters of space robot

非合作目標(biāo)被抓捕后的相對(duì)速度為零,角速度為[-0.174 5 0 0]rad/s,目標(biāo)上兩個(gè)抓捕點(diǎn)的位置分別為[0 0.5 0]m 和[0 -0.5 0]m。設(shè)定目標(biāo)運(yùn)動(dòng)軌跡的接觸力/力矩約束上界分別為20 N 與20 N·m,約束下界為-20 N與-20 N·m,目標(biāo)運(yùn)動(dòng)軌跡的時(shí)長(zhǎng)為3 s?？臻g雙臂機(jī)器人穩(wěn)定非合作目標(biāo)時(shí),為了避免基座運(yùn)動(dòng)對(duì)機(jī)械臂末端與目標(biāo)接觸產(chǎn)生影響,令基座的位姿固定,期望運(yùn)動(dòng)速度為零。

根據(jù)式(4)中的運(yùn)動(dòng)學(xué)約束與目標(biāo)運(yùn)動(dòng)軌跡,通過(guò)逆運(yùn)動(dòng)學(xué)計(jì)算可以得到未經(jīng)內(nèi)外雙環(huán)結(jié)構(gòu)調(diào)整的穩(wěn)定運(yùn)動(dòng)軌跡。對(duì)于控制性能可設(shè)計(jì)的跟蹤控制器,選擇控制參數(shù)：

為了展示所提協(xié)調(diào)穩(wěn)定控制方法的有效性,根據(jù)已有研究中的穩(wěn)定控制方法設(shè)計(jì)了兩個(gè)對(duì)照組。在對(duì)照組1中,穩(wěn)定過(guò)程中不考慮空間機(jī)器人與目標(biāo)交互的安全,利用PD 控制器直接跟蹤未經(jīng)調(diào)整的穩(wěn)定運(yùn)動(dòng)軌跡。由空間雙臂機(jī)器人的動(dòng)力學(xué)方程,可以設(shè)計(jì)PD 控制器：

在對(duì)照組2中,穩(wěn)定過(guò)程中采取已有研究中的阻抗控制方法,根據(jù)接觸力與力矩對(duì)跟蹤控制律進(jìn)行調(diào)整實(shí)現(xiàn)接觸柔順。仿真中利用阻抗控制器跟蹤未經(jīng)調(diào)整的穩(wěn)定運(yùn)動(dòng)軌跡：

式中：、和為對(duì)稱的正定矩陣。仿真過(guò)程中,對(duì)照組2中實(shí)現(xiàn)阻抗控制的柔順等式參數(shù)以及所提方法求解安全穩(wěn)定運(yùn)動(dòng)軌跡所需的柔順等式參數(shù)如表2所示。

表2 柔順等式參數(shù)Table 2 Parameters of compliant equations

圖4～圖6給出了所提方法與對(duì)照組方法在穩(wěn)定目標(biāo)過(guò)程時(shí),空間雙臂機(jī)器人末端的接觸力/力 矩情況。圖中、、和、、分別為和的 三 軸 分 量；、、和、、分別為和的三軸分量。由于抓捕點(diǎn)處目標(biāo)的加速度與機(jī)械臂末端的加速度之間存在初始偏差,因此在抓捕后開始階段,目標(biāo)的運(yùn)動(dòng)會(huì)受到初始接觸的沖擊影響,抓捕點(diǎn)處會(huì)產(chǎn)生較大的初始接觸力/力矩。

圖4 協(xié)調(diào)穩(wěn)定控制機(jī)械臂末端的接觸力與力矩Fig.4 Contact wrenches of robot end-effector under coordinated stabilization control

圖6 阻抗控制機(jī)械臂末端的接觸力與力矩Fig.6 Contact wrenches of robot end-effector under impedance control

對(duì)比圖4與圖5、圖6可以發(fā)現(xiàn),由于未考慮目標(biāo)與機(jī)械臂末端交互的安全,PD 控制器跟蹤未經(jīng)調(diào)整的期望軌跡時(shí)產(chǎn)生了三組中最大的接觸力/力矩,無(wú)法保證目標(biāo)與機(jī)械臂末端的安全交互。相較于PD 控制,考慮了末端接觸力/力矩的阻抗控制方法與協(xié)調(diào)控制方法有效地降低抓捕后階段的接觸力。而協(xié)調(diào)控制方法同時(shí)考慮了目標(biāo)的不確定性與無(wú)效運(yùn)動(dòng)旋量對(duì)接觸力/力矩的影響,故相較于僅考慮接觸力/力矩大小的阻抗控制,協(xié)調(diào)控制進(jìn)一步地降低了機(jī)械臂末端的接觸力矩。鑒于協(xié)調(diào)控制需要根據(jù)接觸力/力矩生成安全運(yùn)動(dòng)軌跡,受到安全運(yùn)動(dòng)軌跡的影響,協(xié)調(diào)控制穩(wěn)定目標(biāo)時(shí)產(chǎn)生的接觸力/力矩收斂速度較PD控制慢；而與阻抗控制方法相比,接觸力/力矩能夠根據(jù)安全軌跡快速收斂,因而收斂速度較阻抗控制快。因此,所提方法通過(guò)協(xié)調(diào)雙臂期望運(yùn)動(dòng)和約束跟蹤控制性能,有效地控制了目標(biāo)不確定性與無(wú)效運(yùn)動(dòng)旋量對(duì)接觸力/力矩造成的影響,實(shí)現(xiàn)了雙臂末端與目標(biāo)接觸的柔順,降低了穩(wěn)定過(guò)程中產(chǎn)生的接觸力/力矩,保證了穩(wěn)定過(guò)程中雙臂末端與目標(biāo)交互的安全。

圖5 PD 控制機(jī)械臂末端的接觸力與力矩Fig.5 Contact wrenches of robot end-effector under PD control

圖7～圖9給出了利用所提協(xié)調(diào)控制方法穩(wěn)定目標(biāo)和使用對(duì)照方法跟蹤軌跡穩(wěn)定目標(biāo)時(shí)的跟蹤誤差。由仿真結(jié)果可知,所提協(xié)調(diào)控制與PD控制均可以快速地跟蹤上期望運(yùn)動(dòng)軌跡。從圖7的仿真結(jié)果可以看出,約束跟蹤控制器能夠在設(shè)計(jì)的跟蹤誤差約束范圍內(nèi)有效地跟蹤安全穩(wěn)定運(yùn)動(dòng)軌跡。對(duì)比圖7和圖8可知,由于安全穩(wěn)定運(yùn)動(dòng)軌跡受到接觸力/力矩的影響,協(xié)調(diào)控制的跟蹤誤差收斂速率低于PD 控制。此外,由于阻抗控制的控制性能受到接觸力/力矩的影響,阻抗控制的跟蹤誤差收斂速率為三組中最低。相較于阻抗控制,本文所提協(xié)調(diào)控制方法的跟蹤控制性能不受接觸力/力矩影響且可以設(shè)計(jì),能夠有效地跟蹤安全穩(wěn)定軌跡,保障穩(wěn)定控制的效率。

圖7 協(xié)調(diào)穩(wěn)定控制關(guān)節(jié)運(yùn)動(dòng)跟蹤誤差Fig.7 Tracking errors of joints under coordinated stabilization control

圖8 PD 控制關(guān)節(jié)運(yùn)動(dòng)跟蹤誤差Fig.8 Tracking errors of joints under PD control

圖9 阻抗控制關(guān)節(jié)運(yùn)動(dòng)跟蹤誤差Fig.9 Tracking errors of joints under impedance control

圖10～圖12給出了所提協(xié)調(diào)控制方法與兩種對(duì)照方法穩(wěn)定目標(biāo)運(yùn)動(dòng)時(shí)目標(biāo)的運(yùn)動(dòng)狀態(tài)。由于初始時(shí)刻的沖擊影響,目標(biāo)在初始時(shí)刻產(chǎn)生了速度。圖中 、 和 為的三軸分量； 、 和 為的三軸分量。從仿真結(jié)果可知,三種方法均可以有效地穩(wěn)定目標(biāo)運(yùn)動(dòng)。由于PD 控制未考慮接觸力/力矩的影響,目標(biāo)運(yùn)動(dòng)參照未經(jīng)調(diào)整的參考期望運(yùn)動(dòng)軌跡,因而PD 控制的穩(wěn)定速度最快。相較于PD 控制,由于協(xié)調(diào)控制的安全穩(wěn)定運(yùn)動(dòng)軌跡受到接觸力/力矩的影響,因而穩(wěn)定速率低于PD 控制。而阻抗控制的控制性能受到接觸力/力矩影響,當(dāng)控制律中的接觸力與力矩收斂時(shí)跟蹤誤差才能保證收斂,因此阻抗控制穩(wěn)定目標(biāo)運(yùn)動(dòng)的速率為三組中最低。

圖10 協(xié)調(diào)穩(wěn)定控制目標(biāo)運(yùn)動(dòng)速度Fig.10 Velocities of target under coordinated stabilization control

圖12 阻抗控制目標(biāo)運(yùn)動(dòng)速度Fig.12 Velocities of target under impedance control

圖13～圖15給出了穩(wěn)定控制過(guò)程中,機(jī)械臂由于耦合作用對(duì)基座產(chǎn)生的反作用力/力矩。圖中 、 和 為的三軸分量； 、 和 為的三軸分量。由仿真結(jié)果可知,PD控制產(chǎn)生的反作用力/力矩最大；而阻抗控制與所提協(xié)調(diào)控制均有效地降低了穩(wěn)定過(guò)程中因耦合作用對(duì)基座產(chǎn)生的反作用力/力矩,保證了基座安全。由于阻抗控制根據(jù)接觸力/力矩調(diào)整了基座和機(jī)械臂關(guān)節(jié)的跟蹤控制律,相比協(xié)調(diào)控制方法基座受到的最大反作用力/力矩小,但跟蹤期望運(yùn)動(dòng)的時(shí)間較長(zhǎng)。而協(xié)調(diào)控制穩(wěn)定目標(biāo)運(yùn)動(dòng)時(shí),約束跟蹤控制器為了保障跟蹤控制性能基座受到的最大反作用力/力矩大于阻抗控制,但仍在安全范圍內(nèi),且穩(wěn)定控制的速率和反作用力/力矩的收斂速率比阻抗控制高,這說(shuō)明本方法在保證安全的前提下協(xié)調(diào)控制效率更高。

圖11 PD 控制目標(biāo)運(yùn)動(dòng)速度Fig.11 Velocities of target under PD control

圖13 協(xié)調(diào)穩(wěn)定控制對(duì)基座的反作用力與力矩Fig.13 Coupling wrenches for base under coordinated stabilization control

圖15 阻抗控制對(duì)基座的反作用力與力矩Fig.15 Coupling wrenches for base under impedance control

圖14 PD 控制對(duì)基座的反作用力與力矩Fig.14 Coupling wrenches for base under PD control

為了方便對(duì)照,表3對(duì)3種方法穩(wěn)定目標(biāo)的時(shí)間、穩(wěn)定目標(biāo)過(guò)程中的最大接觸力/力矩、接觸力/力矩收斂時(shí)間、對(duì)基座產(chǎn)生的反作用力/力矩及其收斂時(shí)間等進(jìn)行了匯總和對(duì)比。相較于未考慮交互安全的PD 控制,所提協(xié)調(diào)控制方法實(shí)現(xiàn)穩(wěn)定控制過(guò)程中的最大接觸力降低了74.2%,最大接觸力矩降低了84.1%,基座受到的最大反作用力與力矩分別降低了66.7%和66.4%。由于接觸力/力矩對(duì)安全穩(wěn)定運(yùn)動(dòng)軌跡的影響,協(xié)調(diào)控制的穩(wěn)定時(shí)間相較PD 控制增加了21.6%,接觸力/力矩收斂至數(shù)量級(jí)10內(nèi)的時(shí)間增加了9.1%。此外,由于PD 控制并未根據(jù)接觸力/力矩協(xié)調(diào)雙臂的運(yùn)動(dòng),相較于協(xié)調(diào)控制,反作用力/力矩收斂至數(shù)量級(jí)10內(nèi)的時(shí)間短。與考慮了交互安全的阻抗控制相對(duì)比,由于本文方法同時(shí)考慮了目標(biāo)不確定性和無(wú)效運(yùn)動(dòng)旋量對(duì)接觸力/力矩的影響,雖然最大接觸力提高了10.9%,但是最大接觸力矩相較阻抗控制降低了81.2%,仍然保證了交互的安全。此外,由于所提方法的跟蹤控制性能不受接觸力/力矩影響,相較于阻抗控制方法,接觸力/力矩收斂至數(shù)量級(jí)10內(nèi)的時(shí)間減少了68.2%,反作用力/力矩收斂至數(shù)量級(jí)10內(nèi)的時(shí)間降低了69.6%,穩(wěn)定目標(biāo)運(yùn)動(dòng)的時(shí)間縮短了56.3%,所提方法在穩(wěn)定目標(biāo)的過(guò)程中更高效地實(shí)現(xiàn)了雙臂末端與非合作目標(biāo)的安全交互。

表3 3種控制方法的性能結(jié)果對(duì)比Table 3 List of simulation results of three control methods

綜上,相較于無(wú)法保證目標(biāo)與機(jī)械臂末端交互安全的PD 控制,本文所提協(xié)調(diào)控制能夠在實(shí)現(xiàn)交互安全的同時(shí)保障跟蹤控制性能,便于設(shè)計(jì)且安全性更高。與阻抗控制相比,協(xié)調(diào)控制進(jìn)一步地降低了機(jī)械臂末端的接觸力矩；跟蹤控制性能可設(shè)計(jì)且不受接觸力與力矩的影響,減弱了接觸力/力矩對(duì)穩(wěn)定速率的影響。所提方法通過(guò)協(xié)調(diào)雙臂運(yùn)動(dòng)實(shí)現(xiàn)了雙臂末端與目標(biāo)交互柔順的穩(wěn)定控制,降低了穩(wěn)定過(guò)程中產(chǎn)生的接觸力與力矩,保證了穩(wěn)定控制過(guò)程中目標(biāo)與機(jī)械臂末端交互的安全,提高了柔順交互時(shí)穩(wěn)定控制的效率。

5 結(jié) 論

1)所提方法通過(guò)跟蹤由內(nèi)外雙環(huán)求解的安全穩(wěn)定運(yùn)動(dòng)軌跡,協(xié)調(diào)地控制了雙臂的運(yùn)動(dòng),使得雙臂末端與目標(biāo)交互柔順,降低了穩(wěn)定過(guò)程中產(chǎn)生的接觸力/力矩,實(shí)現(xiàn)了機(jī)械臂末端與目標(biāo)交互柔順的安全穩(wěn)定控制。

2)與已有的阻抗控制方法相比,所提方法同時(shí)考慮了雙臂末端對(duì)目標(biāo)拉扯/擠壓以及目標(biāo)不確定性對(duì)接觸力/力矩的影響,進(jìn)一步地降低了機(jī)械臂末端的接觸力矩；跟蹤控制性能可設(shè)計(jì),減弱了接觸力/力矩對(duì)穩(wěn)定速率的影響,提高了雙臂末端與目標(biāo)柔順交互時(shí)穩(wěn)定控制的效率。