仿人機器人協(xié)調(diào)阻抗控制算法

齊春麗， 伍錫如

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

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

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

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

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

1 仿人機器人運動學模型

1.1 正逆運動學分析

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

(1)

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

(2)

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

1.2 雙臂運動學模型

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

(3)

圖1 雙臂機器人模型

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

(4)

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

(5)

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

(6)

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

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

(7)

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

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

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

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

(8)

由牛頓-歐拉方程，操作物動力學方程為：

(9)

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

(10)

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

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

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

(11)

(12)

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

(13)

2 帶有擾動觀測器的雙臂阻抗控制

2.1 雙臂阻抗控制

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

(14)

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

(15)

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

(16)

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

Kx(xd-xo)]。

(17)

(18)

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

(19)

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

(20)

2.2 基于廣義動量的擾動觀測器

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

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

(21)

則

(22)

令殘余向量r作為觀測器指標，其表達式為

(23)

則

(24)

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

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

Kx(xd-xo)]。

(25)

(26)

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

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

(27)

機械臂在緊協(xié)調(diào)搬運時的總驅(qū)動能量為

ξ=0.5μTμ。

(28)

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

3 實驗結果與分析

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

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

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

圖3為無觀測器時操作物體的位置誤差。從圖3可看出，當無觀測器時，操作物體的位置誤差比較大。圖4為有觀測器時操作物體的位置誤差。比較圖4、圖3可看出，引入觀測器后，操作物體的位置誤差遠遠小于無觀測器的位置誤差，表明操作物體位置精度有很明顯的提高。

圖3 無觀測器時操作物體的位置誤差

圖4 有觀測器時操作物體的位置誤差

圖5 機械臂的實際關節(jié)力矩

圖6 觀測器觀測的關節(jié)力矩

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

4 結束語

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