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

汪 雙 ，劉滿祿 ，2，張 華

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

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

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

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

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

1 機(jī)械臂運(yùn)動學(xué)動力學(xué)分析

1.1 運(yùn)動學(xué)分析

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

圖2 機(jī)械臂運(yùn)動學(xué)模型Fig.2 Kinematics model of robot manipulator

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

由式（1）～式（3）以及余弦定理可知其逆運(yùn)動學(xué)為

式中，±分別對應(yīng)機(jī)械臂同一末端位置的2種構(gòu)型。

1.2 動力學(xué)分析

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

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

2.1 環(huán)境模型

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

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

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

關(guān)節(jié)空間基于非線性補(bǔ)償?shù)木€性解耦控制律[13]為

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

假設(shè)J非奇異，由式（10）可得：

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

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

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

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

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

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

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

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

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

可知阻尼比為

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

3 力跟蹤阻抗控制器

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

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

結(jié)合位置控制器，參考位置xd驅(qū)使作用力f趨于參考作用力 fr。 忽略力跟蹤阻抗控制結(jié)構(gòu)如圖4所示。

圖4 力跟蹤阻抗控制結(jié)構(gòu)Fig.4 Structure of force tracking impedance control

分別定義作用力誤差和二次誤差函數(shù)：

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

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

根據(jù)梯度下降法可知，誤差函數(shù)ε沿著：

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

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

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

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

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

4 避碰控制器

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

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

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

若機(jī)械臂當(dāng)前位置為x，期望撤退位置為xf，d為撤退距離系數(shù)，則：

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

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

設(shè)定作用力閾值為 Fg=（Fgx，F(xiàn)gy），若條件判斷（fx＞Fgx）‖（fy＞Fgy）成立，則機(jī)械臂由力跟蹤阻抗控制切換到避碰控制。

5 仿真驗(yàn)證

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

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

圖7為機(jī)械臂參考軌跡圖。其中，連桿1、2的質(zhì)量分別為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的轉(zhuǎn)動慣量為 I1x=0.07807 kg·m2，I1y=0.07302 kg·m2，I1z=0.01056 kg·m2； 連桿 2 的轉(zhuǎn)動慣量為 I2x=0.02034 kg·m2，I2y=0.02018 kg·m2，I2z=0.00044 kg·m2；環(huán)境剛度 Ke=105N/m；重力沿 y軸負(fù)方向，G=9.81 N/kg。

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

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

圖8 Simulink仿真控制結(jié)構(gòu)Fig.8 Control structure of Simulink simulation

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

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

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

圖10 期望軌跡和實(shí)際軌跡Fig.10 Desire trajectory and actual trajectory

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

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

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

6 結(jié)語

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