機械臂力跟蹤阻抗和避碰控制仿真研究

汪 雙 ，劉滿祿 ，2，張 華

（1.西南科技大學 特殊環(huán)境機器人技術四川省重點實驗室，綿陽 621010；2.中國科學技術大學 信息科學技術學院，合肥 230026）

隨著機器人技術的不斷提高以及應用領域的不斷擴展，作為機器人重要代表之一的機械臂也趨于適應更多的應用場合，當前機械臂更是迫切需要應用于非結構化的作用環(huán)境[1]，打磨、拋光、組裝等接觸性任務不但要求機械臂末端工具能以適當力接觸工件表面，同時要求機械臂能夠沿著工件表面相對移動[2]，這對機械臂的柔順性和安全性提出了更高的要求。機械臂柔順控制和占據機械臂安全問題主導地位的避碰控制技術一直是國內外的研究熱點。

文獻[3]提出的阻抗控制是機械臂主動柔順控制的重要部分；文獻[4]基于位置控制內環(huán)的阻抗控制更是確保了非接觸狀態(tài)下的位置跟蹤精度；文獻[5]基于力/位混合控制實現了機械臂末端工具在對象弧形表面的柔性接觸，但其控制結構較之阻抗控制更為復雜；文獻[6-7]基于自適應混合阻抗控制實現了機械臂在環(huán)境表面的穩(wěn)定接觸力運動；文獻[8]在未知環(huán)境信息下實現力跟蹤阻抗控制則更為簡單；文獻[9-11]利用廣義動量觀測器進行碰撞力估計，但其準確度容易受到較大加速度的影響。

綜合以上方法優(yōu)缺點，為了解決機械臂力跟蹤阻抗控制和安全避碰問題，本文基于圖1所示的二自由度平面機械臂，建立其運動學動力學模型，設計操作空間線性解耦位置控制器，結合位置控制內環(huán)以及力跟蹤阻抗控制，實現機械臂在操作空間阻抗控制方向上作用力的穩(wěn)定；同時在位置控制方向上，基于作用合力方向的直線運動控制，確保位置控制的安全避碰；文章最后基于Simulink-Adams聯(lián)合仿真，驗證了力跟蹤阻抗控制和避碰控制的有效性。

圖1 二自由度平面機械臂Fig.1 2 DOF plane robot manipulator

1 機械臂運動學動力學分析

1.1 運動學分析

圖2所示為二自由度平面機械臂運動學模型，對其第一、二關節(jié)以及末端工具處建立參考坐標系，基坐標系（x，0，y）與第一關節(jié)參考坐標系（x0，0，y0）初始位置重合。

圖2 機械臂運動學模型Fig.2 Kinematics model of robot manipulator

圖 中 ：m1、m2、l1、l2、a1、a2、θ1、θ2分 別 為 連 桿 1、2的質量、長度、重心距離前一關節(jié)長度以及關節(jié)角度；r為末端工具到基坐標系原點距離；（x，y）為末段工具相對基坐標系的位置，其正運動學為

由式（1）～式（3）以及余弦定理可知其逆運動學為

式中，±分別對應機械臂同一末端位置的2種構型。

1.2 動力學分析

式中：M，V，G分別為慣性、科氏力向心力以及重力矩矩陣；u為作用在機械臂關節(jié)上的廣義力矩；f為環(huán)境作用在末端工具上的廣義力；J為雅克比矩陣。假設為末端工具笛卡爾速度，則：

2 位置內環(huán)阻抗控制器

2.1 環(huán)境模型

環(huán)境施加在機械臂上的作用力f通常用彈簧模型來表示：

式中：f為環(huán)境作用力，當機械臂未與環(huán)境接觸時f為零；x，xe分別為機械臂位置以及環(huán)境靜態(tài)位置；Ke為環(huán)境剛度。

2.2 操作空間線性解耦位置控制器

關節(jié)空間基于非線性補償的線性解耦控制律[13]為

式中，α為需要設計的控制輸入，結合式（8）且機械臂未與環(huán)境接觸（f=0）：

假設J非奇異，由式（10）可得：

式中，Kd，Kp為對稱正定的增益矩陣，結合式（12）～式（15）得操作空間線性解耦控制器為

若 f=0 且 e=xd-x，將式（16）代入式（8）得閉環(huán)控制律：

因Kd，Kp均正定，可知誤差均為0。

2.3 位置內環(huán)阻抗控制器

機械臂與環(huán)境接觸時，如果只對機械臂施加位置控制，由式（11）可知作用力會逐漸增大，進而損壞機械臂末端工具或環(huán)境。阻抗控制則能實時依據作用力并調整參考軌跡，以使機械臂與環(huán)境之間呈現如下動態(tài)關系：

式中：A，B，K分別為慣性、阻尼和剛度系數對角矩陣，其定義了機械臂與環(huán)境接觸時的動態(tài)行為特性；xd，xc分別為參考位置和阻抗調整后位置。因A，B，K為對角矩陣則式（18）關系解耦，則單一操作空間自由度阻抗動態(tài)特性如圖3所示。

圖3 阻抗動態(tài)特性Fig.3 Dynamic characteristics of impedance

圖中：ai，bi，ki為 A，B，K 對角元素；c，c，xc分別為調整后的操作空間參考加速度、速度和位置，再結合式（16）位置控制器形成位置內環(huán)阻抗控制器。

結合圖3可知機械臂位置輸出X（s）與參考位置 Xd（s）之間特征方程為

可知阻尼比為

由于環(huán)境剛度 ke遠大于 ai，bi，ki，系統(tǒng)很容易處于欠阻尼狀態(tài)，由式（22）可知應適當增大阻尼系數bi，同時減小質量、剛度系數 ai，ki，以獲得期望的系統(tǒng)阻尼。

3 力跟蹤阻抗控制器

通過圖3可以看出環(huán)境靜態(tài)位置和剛度決定了反饋作用力f。理論上說，如果能夠確切獲悉環(huán)境位置和剛度，通過軌跡規(guī)劃和阻抗控制也可以實現期望作用力，然而實際上很難做到這一點。

假設xr為依據環(huán)境位置設定的初始參考軌跡，xr確保機械臂能與環(huán)境接觸即可；fr為參考作用力，ri為軌跡調整系數。通過Δxr調整xr獲得阻抗控制參考位置：

結合位置控制器，參考位置xd驅使作用力f趨于參考作用力 fr。 忽略力跟蹤阻抗控制結構如圖4所示。

圖4 力跟蹤阻抗控制結構Fig.4 Structure of force tracking impedance control

分別定義作用力誤差和二次誤差函數：

穩(wěn)態(tài)時由式（18）、（19）可知：

結合式（20）的假設和式（11）可知穩(wěn)態(tài)時[4]：

根據梯度下降法可知，誤差函數ε沿著：

迭代收斂到最小值，λ為迭代步長或采樣周期，此時 f→ fr，同時 xr沿著 Δxr變化方向使 xr→xd：

令 ri=λ，對式（31）拉氏變換：

依據式（20）的假設，由圖4可知機械臂位置輸出與參考作用力fr（s）的閉環(huán)特征方程為

由勞斯判據可知系統(tǒng)穩(wěn)定的條件為

由于式（34）中系數均大于0，故任意ri＞0式（34）肯定成立，系統(tǒng)穩(wěn)定。實際可以取較小的ri，使得作用力跟蹤性能更好[10]。

4 避碰控制器

機械臂操作空間位置控制自由度方向上，末段工具沿著環(huán)境表面運動甚至與障礙碰撞時，當作用力大于一定閾值，則可能損壞末端工具或環(huán)境。若能夠實時檢測作用力，且在作用力超過一定閾值時控制機械臂向作用力瞬速衰減方向運動，則機械臂和環(huán)境的安全性將得到很大的保障。圖5為機械臂與環(huán)境接觸示意。

圖5 機械臂與環(huán)境接觸示意Fig.5 Sketch map of the contact between robot manipulator and environment

由圖5可見，當機械臂沿著環(huán)境表面運動時，此時fxy1為作用力合力；當機械臂與障礙碰撞時，此時fxy2為作用力合力。可以發(fā)現，作用力合力方向正好是避免機械臂與環(huán)境過度接觸的最佳撤退方向。假設fx，fy分別為環(huán)境對機械臂沿著x，y軸方向作用力，則撤退方向單位矢量為

若機械臂當前位置為x，期望撤退位置為xf，d為撤退距離系數，則：

將式（15）控制律稍作修改[14]：

此時式（16）的控制律將使機械臂以笛卡爾速度Vmax直線運動到期望撤退位置xf。

設定作用力閾值為 Fg=（Fgx，Fgy），若條件判斷（fx＞Fgx）‖（fy＞Fgy）成立，則機械臂由力跟蹤阻抗控制切換到避碰控制。

5 仿真驗證

在Adams中建立機械臂仿真模型如圖6所示，右邊為曲形表面環(huán)境模型。

圖6 Adams仿真模型Fig.6 Adams simulation model

圖7為機械臂參考軌跡圖。其中，連桿1、2的質量分別為m1=4.85 kg，m2=1.22 kg；長度分別為l1=0.350 m，l2=0.355 m；重心距離分別為 a1=0.250 m，a2=0.160 m；連桿 1的轉動慣量為 I1x=0.07807 kg·m2，I1y=0.07302 kg·m2，I1z=0.01056 kg·m2； 連桿 2 的轉動慣量為 I2x=0.02034 kg·m2，I2y=0.02018 kg·m2，I2z=0.00044 kg·m2；環(huán)境剛度 Ke=105N/m；重力沿 y軸負方向，G=9.81 N/kg。

圖7 機械臂參考軌跡Fig.7 Reference trajectory of robot manipulator

Simulink仿真控制結構如圖8所示，軌跡調整部分實時調整參考軌跡，生成中間參考軌跡提供給位置內環(huán)阻抗控制器；作用力未超過閾值時阻抗控制器接管控制，作用力超過閾值時，避碰控制器Switch有效并接管控制。其中參考力fr=10 N；軌跡調整系數ri=0.06；阻抗控制器慣性、阻尼以及剛度系數矩陣分別為 A=［5，5］，B=［3000，3000］，K=［50，50］；內環(huán) 位 置 控 制 器 參 數 分 別 為 Kd=［500，500］，Kp=［2000，2000］；避碰控制器參數 Kd=［500，500］；撤退距離系數d=0.1；撤退速度Vmax=0.03 m/s；接觸力閾值 Fg=［30，30］N。

圖8 Simulink仿真控制結構Fig.8 Control structure of Simulink simulation

圖9為Adams仿真動態(tài)圖，當時t=9.1 s，機械臂末端工具與環(huán)境開始接觸，然后沿著環(huán)境表面向下運動；當t=19.5 s時，機械臂末端工具與障礙發(fā)生碰撞；當t=20.3 s時，作用力超過閾值而觸發(fā)避碰控制器，機械臂開始遠離環(huán)境；當t=24.0 s時，機械臂撤退到安全位置。

圖9 Adams仿真動態(tài)圖Fig.9 Dynamic graphs of Adams simulation

機械臂期望軌跡和實際軌跡如圖10所示，可見位置內環(huán)阻抗控制確保了非接觸時的位置跟蹤，同時由于末端工具參考點位于半徑r=0.02 m的圓形接觸體中心，因此接觸時實際參考軌跡相比環(huán)境接觸表面偏左0.02 m。

圖10 期望軌跡和實際軌跡Fig.10 Desire trajectory and actual trajectory

機械臂x軸方向上的作用力以及誤差如圖11所示，可見機械臂與環(huán)境接觸時，力跟蹤阻抗控制能夠迅速維持作用力f=10 N；但在t=15 s左右時，作用力出現波動，此時末段工具處于接觸面最凹處，接觸面對機械臂作用力變化比較大，這一點通過圖12機械臂y軸方向作用力在t=15 s時的快速變化可以看出；t=19.5 s時，機械臂與障礙發(fā)生碰撞，x軸作用力出現波動，y軸作用力迅速上升；直至t=20.3 s時，y軸作用力達到閾值30 N，避碰控制器觸發(fā)，x、y軸作用力迅速降為0，機械臂撤退到安全位置；避碰觸發(fā)瞬間可以發(fā)現x軸作用力出現波動，這是由于控制切換瞬間，x軸阻抗特性瞬間丟失，同時位置控制需要一定的時間去收斂。

圖11 x軸作用力和誤差Fig.11 Reaction force in x-axis and force deviation

圖12 y軸作用力Fig.12 Reaction force in y-axis

6 結語

本文在位置內環(huán)阻抗控制的基礎上，基于作用力誤差實時調整參考軌跡的力跟蹤方式，實現了機械臂在操作空間x軸方向的作用力恒定；同時基于操作空間y軸方向的位置控制，依據作用合力方向直線位置運動的避碰控制原理，實現了機械臂在非阻抗控制方向上的安全保障。本文基于比較簡單的二自由度機械臂進行了柔順性和安全性分析，然而實際生產中廣泛應用的六自由度機械臂，涉及姿態(tài)以及扭矩等更復雜的柔順相關問題以及機械臂本體避碰問題；其次，接觸面的摩擦力以及實際環(huán)境模型可能涉及到基于剛度系數的指數級變化，甚至包括阻尼特性，這些問題將是接下來的研究深入點。

[1]A.De Luca，R.Mattone.Sensorless robot collision detection and hybrid force/motion control[C]//Proceeding of the 2005 IEEE International Conference on Robotics and Automation，2005：18-22.

[2]李二超，李戰(zhàn)明.基于力/力矩信息的面相位控機器人的阻抗控制[J].控制與決策，2016，31（5）：957-956.

[3]N.Hogan.Impedance control：an approach to manipulation[J].ASME J.Dyna.Syst.，Measure.，Control，1985（107）：1-24.

[4]Bruno Siciliano，Luigi Villani.Robot Force Control[M].New York：Kluwer Academic Publishers，1999：31-43.

[5]L.Hu，J.Zhan.Study on the orthomogonalization for hybrid motion/force control and its application in aspheric surface polishing[J].The InternationalJournalofAdvanced Manufacturing Technology，2015，77（5）：1259-1268.

[6]L.F.Baptista，JMGSD Costa.Adaptive hybrid impedance control of robot manipulators：a comparative study[C]//Proceedings of the 1997 OE/IFIP/IEEE International Conference on Integrated and Sustainable Industrial Production，Lisbon，1997：177-187.

[7]S.Jung，T.C.Hsia，RG Bonitz.Force tracking impedance control of robot manipulators under unknow environment[J].IEEE Transactions on Control Systems Technology，2004，12（3）：474-483.

[8]T.A.Lasky，T.C.Hsia.On force-tracking impedance control of robot manipulators[C]//Proceedings of the 1991 IEEE International Conference on Robotics and Automation，1991（1）：274-280.

[9]E.Magrini，F.Flacco，A.De Luca.Estimation of contact forces using a virtual force sensor[C]//Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems，2014：2126-2133.

[10]E.Magrini，F.Flacco，A.De Luca.Control of generalized contact motion and force in physical human-robot interaction[C]//Proceedings of the 2015 IEEE International Conference on Robotics and Automation，2015：2298-2304.

[11]A.De Luca，R.Mattone.Sensorless robot collision detection and hybrid force/motion control[C]//Proceedings of the 2005 IEEE International Conference on Robotics and Automation，2005：999-1004.

[12]Frank L.Lewis，Darren M.Dawson，Chaouki T.Abdallah.Robot Manipulator Control Theory and Practice[M].New York：Marcel Dekker，Inc.，2004：107-126.

[13]Bruno Siciliano，Lorenzo Sciavicco，Luigi Villani，Giuseppe Oriolo.Robotics：Modelling，Planning and Control[M].London：Springer Science and Business Media，2010：327-348.

[14]Oussama Khatib.A unified approach for motion and force control of robot manipulators：the operational space formulation[J].IEEE Journal on Robotics and Automation，1987，3（1）：43-53.