楊 帥 ，薛 嵐

（1.江蘇電子產品裝備制造工程技術研究開發(fā)中心，江蘇淮安223003；2.淮安信息職業(yè)技術學院自動化學院，江蘇淮安223003）

從機器人誕生至今，機器人技術對工業(yè)的發(fā)展作出了巨大貢獻，不僅可以替代人類進行重復性工作，而且還可以在危險環(huán)境中執(zhí)行任務。隨著社會的發(fā)展，機器人也在不斷地演化，從單臂機器人發(fā)展到雙臂機器人。相比單臂機器人，雙臂機器人具有以下優(yōu)點[1-2]：①可以搬運更大的物體；②可以相互取長補短；③能夠更好地與人類交互。但是，在實際應用中，很難設計出可靠的雙臂機器人控制系統(tǒng)，主要是由于雙臂機器人控制系統(tǒng)的高復雜度和環(huán)境的不確定性，對控制系統(tǒng)的要求也越來越高。因此，深入研究雙臂機器人控制系統(tǒng)，對促進機器人與環(huán)境交互發(fā)展具有重要意義，也能為多臂機器人領域的發(fā)展提供參考價值。

當前，雙臂機器人技術在不斷發(fā)展，其應用領域也越來越廣泛，許多科研工作者從不同角度對其進行了研究，從而產生了大量理論。例如：文獻[3-4]研究了雙臂機器人協(xié)調控制方法，推導了雙臂機器人動力學方程式，提出了統(tǒng)一的力學模型，設計了雙臂機器人協(xié)調控制器，通過仿真驗證機械臂位置跟蹤精度，從而提高了機械臂控制精度。文獻[5-6]研究了雙臂機器人神經網(wǎng)絡控制方法，分析了雙臂機器人結構，建立了連桿坐標系，推導出機械臂末端位姿方程式，設計了神經網(wǎng)絡滑?？刂品椒?，通過仿真驗證路徑跟蹤效果，避免了控制的不穩(wěn)定性。文獻[7-8]研究了雙臂機器人滑?？刂品椒ǎ治隽藱C器人運動學模型，根據(jù)關節(jié)角位移誤差，設計機器人滑?？刂葡到y(tǒng)，采用實驗驗證滑?？刂菩Ч瑸闄C器人控制算法的研究提供借鑒意義。但是，以往研究的雙臂機器人，只能執(zhí)行較為簡單的軌跡追蹤任務，在復雜軌跡追蹤過程中，就會在某個時間段，脫離期望運動軌跡追蹤。對此，本文建立了雙臂機器人模型簡圖，通過雅克比矩陣推導機器人逆運動學模型。為了避免任務沖突，采用分層優(yōu)先級的任務體系結構，將優(yōu)先級低的任務投射到優(yōu)先級高的任務零空間上，給出了冗余機械臂關節(jié)限制策略，設計了阻抗控制方法，并對控制效果進行仿真驗證，為深入研究雙臂機器人控制技術提供了理論參考數(shù)據(jù)。

1 雙臂機器人運動學

1.1 雅克比矩陣

雙臂機器人具有兩個相同的機械臂，即A和B，如圖1所示。

圖1 雙臂機器人Fig.1 The dual-arm robot

末端執(zhí)行器B的坐標系記為Be，末端執(zhí)行器A的坐標系記為Ae，用矢量PR和ΦR表示。其中，矢量PR通過笛卡爾坐標定義末端執(zhí)行器的相對位置，矢量ΦR通過最小方向描述末端執(zhí)行器的相對方向。對這兩個矢量關于時間的微分，可以定義一個唯一的矢量xRk，它包含末端執(zhí)行器在時刻k的相對速度分量，如下所示：

式中：xRk為rab維列向量（三維空間中rab≤6）。

通過定義兩個機械臂之間的運動關系，可以得到一個關節(jié)總數(shù)為nab=2nj的等效機械臂，以及由矢量xRk定義的相對末端執(zhí)行器速度分量。根據(jù)機械臂等效操縱器相關的雅克比矩陣[9]可以得到

1.2 逆運動學

式（2）中采用的相對雅可比矩陣JR（qAk，qBk）相對末端效應器運動矢量xRk，與兩個操縱器的關節(jié)速度矢量之間的運動關系如下所示：

采用最小化范數(shù)可以得到JR的偽逆解為

因此，式（4）變換為

式中：eRk為末端執(zhí)行器位置誤差；KRk為增益矩陣。

式（6）是基于閉環(huán)逆運動學的一階運動學算法，具有反饋校正項。

2 關節(jié)規(guī)避極限策略

2.1 分層優(yōu)先級任務體系結構

采用冗余執(zhí)行一個或多個次要任務，次要任務相對于主任務優(yōu)先級較低。冗余雙臂機器人能夠處理的任務數(shù)取決于關節(jié)自由度和每個任務相關的雅可比矩陣的秩。為了避免任務沖突，采用分層優(yōu)先級的任務體系結構，將優(yōu)先級低的任務投射到優(yōu)先級高的任務零空間上。這樣，優(yōu)先級低的任務不會影響優(yōu)先級高的任務性能，而優(yōu)先級高的任務性能得到保證。因此，給定三個通用的優(yōu)先級任務x1，x2和x3（下標越大，優(yōu)先級越高），就有可能獲得聯(lián)合速度矢量為

式中：Ji（i=1，2，3）為每個任務的偽逆雅克比矩陣；P2，P3為零空間Nk上的正交投影矩陣。

根據(jù)式（7）可知，如果系統(tǒng)具有冗余運動，可以增加第三個任務，以滿足進一步的約束（比如增加系統(tǒng)的可操縱性或避免關節(jié)位置限制）。由于本文研究關節(jié)極限規(guī)避問題，這些額外的任務可以直接在關節(jié)空間坐標中表示。因此，可將雙臂系統(tǒng)的運動關系[10]寫為

式中：Kk為末端執(zhí)行器（A）位置誤差的反饋增益；PA和PB為零空間的正交投影矩陣。

2.2 關節(jié)規(guī)避極限策略

為了優(yōu)化關節(jié)極限規(guī)避任務中使用的冗余運動的數(shù)量，采用一個排斥關節(jié)速度，該速度可以避開極限。對于雙臂系統(tǒng)，機械臂的關節(jié)速度矢量分別為

式中：qTA，qTB為nj維列向量；HA，HB為控制律的增益。

為了引入監(jiān)控系統(tǒng)，雙臂運動方程修改如下：

式中：CAB和BAB矩陣的維數(shù)為（nab×nab）。

CAB和BAB矩陣可以定義為

如果通用協(xié)同操縱器冗余度等于1，則采用冗余來管理q1，因為n=rank（PA）=1，而BAB矩陣等于單位矩陣，CAB為零矩陣。相反，當?shù)诙€關節(jié)為主導時，q2沒有冗余度，通過增加相對關節(jié)極限規(guī)避任務的優(yōu)先級來管理q2。

2.3 阻抗控制

冗余機械臂動力學阻抗控制律[11]采用如下形式：

式中：M（q）為慣性矩陣；aq和af為控制量輸入；C（q，q'）為哥氏力矢量；g（q）為重力矢量；JT為雅克比轉置矩陣。

冗余機械臂運動位置和速度方程式[12]定義為

式中：J（q）為雅克比矩陣。

對式（15）求導后可得

假設輸入量aq滿足如下關系式：

式中：ax為輸入量aq對應的加速度。

因此，可以推導出

式中：D（q）=J（q）M-1（q）JT（q）。

設af=F，則

在運動空間中，阻抗?jié)M足關系式為

式中：Md，Bd，Kd分別為慣性、阻尼和剛度矩陣；αF為力的比例因子。

由式（20）和式（21）可得

將式（22）代入式（16）可得

式中：J+為雅克比矩陣；H為加權矩陣；Λ為偽動能矩陣。

因此，可以得到通用的阻抗控制律為

3 仿真及分析

為了比較阻抗控制冗余機械臂關節(jié)限制和軌跡跟蹤誤差，采用Matlab軟件對阻抗控制效果進行仿真驗證。仿真參數(shù)設置如下：αF=9，Kd=diag｛100，100，100｝，Bd=diag｛20，20，20｝，m1=m2=m3=2.0 kg，l1=l2=l3=0.5 m，運動軌跡為q=cos2πt，仿真時間4 s，無阻抗控制的冗余機械臂角位移跟蹤仿真結果如圖2所示，有阻抗控制的冗余機械臂角位移跟蹤仿真結果如圖3所示。

圖2 角位移跟蹤仿真結果（無阻抗控制）Fig.2 Simulation results of tracking angular displacement（non impedance control）

圖3 角位移跟蹤仿真結果（有阻抗控制）Fig.3 Simulation results of tracking angular displacement（impedance control）

由圖2可知：在無阻尼控制條件下，機器人手臂運動關節(jié)受到限制，在某個時間段內，機械臂關節(jié)不能按照指定的運動軌跡進行跟蹤，導致輸出誤差較大。由圖3可知：在有阻尼控制條件下，機器人手臂運動關節(jié)沒有受到限制，在整個仿真時間段內，機械臂關節(jié)都能按照指定的運動軌跡進行跟蹤，輸出誤差較小。因此，阻抗控制可以避開關節(jié)的限制條件，更好地實現(xiàn)機器人手臂關節(jié)角度的跟蹤。

4 結論

本文對雙臂機器人冗余運動進行研究，并設計了阻抗控制方法，通過Matlab軟件對控制誤差進行仿真驗證，主要結論如下：①雙臂機器人比單臂機器人更加復雜，執(zhí)行任務過程中，當冗余運動次數(shù)不再足夠時，則關節(jié)避限任務獲得與后續(xù)路徑任務相同的優(yōu)先級；②雙臂機器人需要手臂關節(jié)相互配合，受到限制，采用阻抗控制，能夠避免機械臂受到關節(jié)限制，其軌跡追蹤精度較高；③采用Matlab軟件能夠對控制系統(tǒng)進行仿真，對驅動軌跡進行預測，為實際運動軌跡提供參考數(shù)據(jù)。