齊春麗， 伍錫如

(桂林電子科技大學(xué) 電子工程與自動(dòng)化學(xué)院，廣西 桂林 541004)

近年來，仿人雙臂機(jī)器人在操作空間內(nèi)具有優(yōu)越的協(xié)調(diào)操作能力，可以完成重型物體搬運(yùn)、零件裝配、焊接等復(fù)雜任務(wù)，已成為機(jī)器人領(lǐng)域的研究熱點(diǎn)。自20世紀(jì)80年代起，國內(nèi)外許多研究者開始了對(duì)雙臂協(xié)作的研究[1-2]。

基于機(jī)械臂運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型，雙機(jī)械臂協(xié)作的諸多控制算法被提出，主要包括位置控制、力/位混合控制、阻抗控制等[3]。李賀立等[4]提出了一種新型阻抗控制算法，將雙臂看作主從臂，引入雙臂間相對(duì)誤差和虛擬恢復(fù)力概念，合理分配載荷，實(shí)現(xiàn)了雙臂用于搬運(yùn)柔性物體的需求，同時(shí)減小了雙臂間誤差，提高了系統(tǒng)的穩(wěn)定性。Caccavale等[5]提出基于阻抗內(nèi)環(huán)和阻抗外環(huán)相統(tǒng)一的整體控制算法，解決雙臂的協(xié)調(diào)阻抗控制問題，并采用工業(yè)機(jī)器人進(jìn)行實(shí)驗(yàn)驗(yàn)證，證明算法既能減小內(nèi)應(yīng)力，又能防止滑環(huán)現(xiàn)象的發(fā)生，克服了單一阻抗控制存在的缺點(diǎn)。敖天翔[6]將阻抗模型拓展為多點(diǎn)阻抗模型，并利用有限元理論對(duì)其求解，解決了雙臂協(xié)調(diào)操作過程中運(yùn)動(dòng)與力協(xié)調(diào)的分配問題，實(shí)現(xiàn)操作系統(tǒng)的可靠性。Tao等[7]提出了位置控制模型和力控制模型2種控制方案，將二者應(yīng)用于協(xié)作機(jī)器人同一相互作用方向上，并驗(yàn)證了控制方案的優(yōu)越性。華磊等[8]提出一種擴(kuò)展的冗余機(jī)械臂阻抗控制算法，該算法通過對(duì)操作空間和零空間共組的增廣操作空間解耦，得出相應(yīng)驅(qū)動(dòng)力矩，并引入避關(guān)節(jié)極限性能指標(biāo)函數(shù)來優(yōu)化零空間運(yùn)動(dòng)，確保冗余關(guān)節(jié)運(yùn)動(dòng)角度的合理性。陳鋼等[9]在雙臂協(xié)調(diào)操作運(yùn)動(dòng)約束關(guān)系的基礎(chǔ)上，提出了適用于協(xié)調(diào)操作的力/位混合控制方法，并通過實(shí)驗(yàn)與傳統(tǒng)控制方法比較，驗(yàn)證了該方法的正確性。Duan Jinjun等[10]針對(duì)雙臂協(xié)同時(shí)外部干擾和校準(zhǔn)誤差會(huì)引起未知軌跡偏差的問題，提出了一種用于力/位跟蹤的對(duì)稱自適應(yīng)變導(dǎo)納控制方法，該方法能夠補(bǔ)償偏差，減小其對(duì)系統(tǒng)的影響。

考慮到操作過程中干涉力會(huì)對(duì)雙臂產(chǎn)生影響，許多研究者通過引入擾動(dòng)觀測(cè)器，以控制干涉力對(duì)雙臂的影響。司彥娜等[11]采用非線性干擾觀測(cè)器的自適應(yīng)反演滑模控制方法，實(shí)現(xiàn)對(duì)機(jī)械臂軌跡跟蹤問題的控制，保證系統(tǒng)穩(wěn)定性。Sadeghian等[12]基于任務(wù)空間誤差和機(jī)器人的廣義動(dòng)量，設(shè)計(jì)了2種不同的擾動(dòng)觀測(cè)器，以觀測(cè)估計(jì)和補(bǔ)償任務(wù)變量上的廣義力，確保任務(wù)空間誤差的漸近穩(wěn)定性，并在七自由度機(jī)械臂上進(jìn)行了實(shí)驗(yàn)驗(yàn)證。錢琮瑋等[13]基于動(dòng)量守恒設(shè)計(jì)了一種外部擾動(dòng)觀測(cè)器對(duì)機(jī)械臂碰撞的作用力進(jìn)行預(yù)估，根據(jù)觀測(cè)器觀測(cè)結(jié)果的不同設(shè)計(jì)相應(yīng)的響應(yīng)控制方法，避免機(jī)械臂與機(jī)械臂和機(jī)械臂與外部干擾的碰撞。

為了實(shí)現(xiàn)既能觀測(cè)對(duì)外部擾動(dòng)，又可達(dá)到合理分配載荷的目的，保證物體搬運(yùn)過程中的控制精度，考慮到操作物的運(yùn)動(dòng)特性，構(gòu)建統(tǒng)一動(dòng)力學(xué)模型，結(jié)合基于廣義動(dòng)量的擾動(dòng)觀測(cè)器觀測(cè)外部擾動(dòng)力矩，設(shè)計(jì)了適用于雙臂協(xié)調(diào)的阻抗控制方法，同時(shí)引入變加權(quán)矩陣以求得最優(yōu)抓取矩陣，達(dá)到協(xié)調(diào)操作過程中合理分配載荷的目的，提高操作系統(tǒng)的性能。

1 仿人機(jī)器人運(yùn)動(dòng)學(xué)模型

1.1 正逆運(yùn)動(dòng)學(xué)分析

多關(guān)節(jié)機(jī)器人是由多個(gè)連桿組成，各連桿之間通過相鄰坐標(biāo)的齊次變換(平移變換和旋轉(zhuǎn)變換)實(shí)現(xiàn)機(jī)械臂末端位姿變換，完成預(yù)定工作。相鄰坐標(biāo)之間的變換通式為

(1)

其中qi、ai-1、αi-1、di為機(jī)械臂連桿相對(duì)應(yīng)的D-H參數(shù)，分別表示各關(guān)節(jié)轉(zhuǎn)角、連桿長度、相鄰關(guān)節(jié)的扭轉(zhuǎn)角、連桿偏移。根據(jù)D-H參數(shù)法可得2個(gè)七自由度機(jī)械臂末端相對(duì)于基座坐標(biāo)系的位姿變換矩陣為

(2)

若已知機(jī)械臂末端位姿，求解機(jī)械臂各關(guān)節(jié)的轉(zhuǎn)角變量的過程稱為機(jī)械臂的逆運(yùn)動(dòng)學(xué)分析，逆運(yùn)動(dòng)學(xué)是正運(yùn)動(dòng)學(xué)的逆運(yùn)算。

1.2 雙臂運(yùn)動(dòng)學(xué)模型

采用n(n=7)自由度的Baxter仿人雙臂機(jī)器人，其模型如圖1所示，相應(yīng)的關(guān)節(jié)空間動(dòng)力學(xué)模型為

(3)

圖1 雙臂機(jī)器人模型

由于外力矩由機(jī)械臂末端受外力所產(chǎn)生的操作力矩分量μo和其他位置的擾動(dòng)力矩分量μd組成，式(3)可改寫為

(4)

通常，機(jī)械臂末端操作空間和關(guān)節(jié)空間的轉(zhuǎn)換關(guān)系為

(5)

其中：x為機(jī)械臂末端位姿；J為雅可比矩陣。式(5)的另一種形式為

(6)

其中：J+為雅可比矩陣的廣義逆矩陣；δ為任意n維向量。式(6)中前后兩部分分別表示機(jī)械臂操作空間和零空間的運(yùn)動(dòng)動(dòng)力學(xué)模型。

設(shè)零空間速度向量為v，雅可比矩陣為JN，兩變量約束條件為

(7)

1.3 雙臂與操作物統(tǒng)一動(dòng)力學(xué)模型

仿人機(jī)器人緊協(xié)調(diào)時(shí)雙臂與操作物之間的約束及受力關(guān)系如圖2所示。其中：Ol和Or分別為左右臂在操作物上的抓取點(diǎn)；Oc為操作物質(zhì)心；Of為仿人機(jī)器人基座坐標(biāo)原點(diǎn)；ftl、ftr、τtl和τtr分別為雙臂末端輸出力和左右臂輸出力矩；m0g表示操作物體的重力。

圖2 仿人機(jī)器人緊協(xié)調(diào)操作約束及受力關(guān)系

根據(jù)緊協(xié)調(diào)操作特性，當(dāng)忽略操作物形變時(shí)，O、Ol和Or處的速度、加速度均相等。操作物質(zhì)心處所受合力表達(dá)式為

(8)

由牛頓-歐拉方程，操作物動(dòng)力學(xué)方程為：

(9)

其中：Io為操作物慣性矩陣；ωo為操作物角速度。由式(8)、(9)可變換為

(10)

其中：W為雙臂協(xié)調(diào)操作是的抓取矩陣；Mo為常數(shù)矩陣；Co為反對(duì)稱矩陣。

根據(jù)式(10)，雙臂作用在操作物質(zhì)心的操作力為

Ft=W+(Fo-Fto)+(I12-W+W)Ωd=

(11)

(12)

用Ft代替式(7)中的J+Tμo，在滿足雙臂緊協(xié)調(diào)操作特性的基礎(chǔ)上，仿人雙臂機(jī)器人與操作物的統(tǒng)一動(dòng)力學(xué)模型為

(13)

2 帶有擾動(dòng)觀測(cè)器的雙臂阻抗控制

2.1 雙臂阻抗控制

阻抗控制可以很好地控制機(jī)械臂和環(huán)境之間的相互作用，能夠?qū)崿F(xiàn)機(jī)械臂協(xié)調(diào)操作的穩(wěn)定性。冗余雙臂關(guān)節(jié)空間阻抗方程式為

(14)

其中：qd為機(jī)械臂的期望軌跡；Md、Bd和Kd分別為關(guān)節(jié)期望慣性、期望阻尼和期望剛度。式(14)的對(duì)應(yīng)控制輸入為

(15)

采用期望內(nèi)力Fid和調(diào)節(jié)系數(shù)L實(shí)現(xiàn)操作物內(nèi)力平滑變化。為控制操作物內(nèi)力，設(shè)操作物期望位姿為xd，根據(jù)式(13)，機(jī)械臂操作空間阻抗方程為：

(16)

其中：Mx、Bx和Kx分別為操作物期望慣性、期望阻尼和期望剛度。式(16)相應(yīng)的控制輸入為

Kx(xd-xo)]。

(17)

(18)

根據(jù)式(18)，可知零空間的阻抗方程為

(19)

其中Mv、Bv和Kv分別為零空間中期望慣性、期望阻尼和期望剛度。由于零空間位置誤差不易定義，由文獻(xiàn)[14]可知，經(jīng)投影的關(guān)節(jié)空間誤差能被用來定義零空間所要求的輸入加速度，

(20)

2.2 基于廣義動(dòng)量的擾動(dòng)觀測(cè)器

未知環(huán)境對(duì)機(jī)械臂產(chǎn)生的外部擾動(dòng)干涉力對(duì)機(jī)械臂運(yùn)行的軌跡精度有很大的影響，為了提高機(jī)械臂運(yùn)動(dòng)精度，增加機(jī)械臂穩(wěn)定性，設(shè)計(jì)了基于廣義動(dòng)量的能觀測(cè)擾動(dòng)干涉力矩μd的控制器。

根據(jù)統(tǒng)一動(dòng)力學(xué)模型(13)，定義機(jī)械臂的廣義動(dòng)量為

(21)

則

(22)

令殘余向量r作為觀測(cè)器指標(biāo)，其表達(dá)式為

(23)

則

(24)

其中Kr為正定對(duì)角線矩陣。特別地，P(0)=0，r(0)=0。

當(dāng)存在干涉力時(shí)，觀測(cè)器指標(biāo)會(huì)快速增加到-μd，根據(jù)外力矩的變化而變化，實(shí)現(xiàn)對(duì)干涉力矩的觀測(cè)。結(jié)合式(23)和式(17)，用殘余向量代替擾動(dòng)力矩μd，式(17)可改寫為

Kx(xd-xo)]。

(25)

(26)

該控制律能控制操作物運(yùn)動(dòng)和內(nèi)力分配，解決因干涉力導(dǎo)致的機(jī)械臂運(yùn)行精度下降問題，保證緊協(xié)調(diào)操作過程中機(jī)械臂及操作物的穩(wěn)定運(yùn)行。

設(shè)計(jì)合理的抓取矩陣，可直接控制分配載荷及內(nèi)應(yīng)力的大小。定義W+為

(27)

機(jī)械臂在緊協(xié)調(diào)搬運(yùn)時(shí)的總驅(qū)動(dòng)能量為

ξ=0.5μTμ。

(28)

若要合理分配抓取載荷，則需要對(duì)式(28)進(jìn)行最小優(yōu)化處理。采用KKT條件對(duì)其進(jìn)行最優(yōu)化[15]，引入變加權(quán)矩陣實(shí)現(xiàn)了抓取載荷的合理分配，且能控制操作物內(nèi)應(yīng)力的大小，使操作系統(tǒng)性能達(dá)到最優(yōu)。

3 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證本算法的正確性，采用七自由度的雙臂機(jī)器人為模型，利用Matlab軟件平臺(tái)進(jìn)行仿真實(shí)驗(yàn)。表1為其中一個(gè)機(jī)械臂的D-H參數(shù)。

表1 雙臂D-H參數(shù)表

仿真時(shí)將2個(gè)機(jī)械臂的初始關(guān)節(jié)角分別設(shè)置為ql=[-π/20π/6π/2-π/1.4π0]和qr=-ql，機(jī)械臂的終止關(guān)節(jié)角度為ql=[-ππ/2π/2π/3.5-π/60π/8]，操作物體的質(zhì)量為4 kg，操作時(shí)間為40 s，時(shí)間間隔為0.05 s，阻抗控制參數(shù)分別為Bx=16E6，Kx=0.2E6，Bv=1.5E14，Kv=16E14。

圖3為無觀測(cè)器時(shí)操作物體的位置誤差。從圖3可看出，當(dāng)無觀測(cè)器時(shí)，操作物體的位置誤差比較大。圖4為有觀測(cè)器時(shí)操作物體的位置誤差。比較圖4、圖3可看出，引入觀測(cè)器后，操作物體的位置誤差遠(yuǎn)遠(yuǎn)小于無觀測(cè)器的位置誤差，表明操作物體位置精度有很明顯的提高。

圖3 無觀測(cè)器時(shí)操作物體的位置誤差

圖4 有觀測(cè)器時(shí)操作物體的位置誤差

圖5 機(jī)械臂的實(shí)際關(guān)節(jié)力矩

圖6 觀測(cè)器觀測(cè)的關(guān)節(jié)力矩

圖5為機(jī)械臂的實(shí)際關(guān)節(jié)力矩，圖6為觀測(cè)器觀測(cè)出的關(guān)節(jié)力矩。對(duì)比圖5、6可看出，10 s時(shí)存在外干涉力條件下，觀測(cè)器觀測(cè)的關(guān)節(jié)力矩與實(shí)際關(guān)節(jié)力矩基本一致，說明了本觀測(cè)器是有效的。

4 結(jié)束語

針對(duì)仿人雙臂機(jī)器人在未知環(huán)境下協(xié)調(diào)操作時(shí)操作物和機(jī)械臂軌跡跟蹤精度低的問題，在阻抗控制方法的基礎(chǔ)上，提出了雙臂緊協(xié)調(diào)的控制算法。通過分析緊協(xié)調(diào)操作時(shí)物體與雙臂存在的約束關(guān)系，建立兩者統(tǒng)一動(dòng)力學(xué)模型。同時(shí)，為了使機(jī)械臂達(dá)到節(jié)約工作資源、優(yōu)化分配載荷的目的，引入變加權(quán)矩陣。為了能實(shí)現(xiàn)對(duì)未知環(huán)境干涉力的直接控制，采用能觀測(cè)外部干涉力的擾動(dòng)觀測(cè)器，使其與阻抗控制模型結(jié)合，設(shè)計(jì)了仿人雙臂機(jī)器人的協(xié)調(diào)阻抗控制算法，提高了操作物的位置跟蹤精度，極大地改善了協(xié)調(diào)操作的性能。