林天宇，齊俊桐，姚　俊

(1.中國科學院沈陽自動化研究所 機器人學國家重點實驗室，沈陽 110016;2.沈陽理工大學 信息科學與工程學院，沈陽 110159)

?

液壓旋翼飛行機械臂系統(tǒng)建模與控制方法

林天宇1,2，齊俊桐1，姚俊2

(1.中國科學院沈陽自動化研究所 機器人學國家重點實驗室，沈陽 110016;2.沈陽理工大學 信息科學與工程學院，沈陽 110159)

摘要：在旋翼飛行機器人上加裝機械臂而得到一種新的飛行機器人系統(tǒng)，擴展了旋翼飛行機器人的應用范圍。本研究將小型液壓機械臂系統(tǒng)應用到已有的旋翼飛行機器人上，通過分析無人機與機械臂的運動耦合，得到懸停模式下液壓旋翼飛行機械臂系統(tǒng)的整體動力學模型；用LQR控制器控制機械臂完成平面內擺動動作；將機械臂運動對機器人本體的影響作為擾動進行考慮，并采用魯棒控制器來穩(wěn)定旋翼飛行機器人懸停時的位姿。通過仿真驗證了本文所述模型結構與控制策略的有效性。

關鍵詞：旋翼飛行機器人；液壓機械臂；動力學建模；LQR控制；魯棒控制

地面移動機器人結合機械臂組成的作業(yè)型地面移動機器人系統(tǒng)，在反恐防暴、救災救援等多種場合已經開始應用并得到廣泛的認可。隨著機器人應用領域的擴展，人們期望飛行機器人對其所處環(huán)境能夠施加主動的影響。實際上，大部分關于飛行機器人的研究通常都是盡量躲避障礙而不與環(huán)境“接觸”。旋翼飛行機器人(Rotary-wing Unmanned Aerial Vehicle，RUAV)因其三維空間機動性強、具備懸停能力等特點，可以代替人完成高危環(huán)境下的信息獲取與作業(yè)等任務。因此，人們希望旋翼機能夠與環(huán)境進行作用來完成一些“接觸”任務。懸停狀態(tài)下的旋翼飛行機器人通過加載機械臂可將工作區(qū)域從二維擴展到三維，極大的拓展了操作型旋翼飛行機器人的應用。

文獻[1]開發(fā)的耶魯空中作業(yè)平臺可以完成實物抓取，借助安裝在平臺上的柔順抓取手臂系統(tǒng)實現落地或懸停時抓取的動作，還研究了載荷質量與偏離重心位置構成的系統(tǒng)穩(wěn)定域。文獻[2]采用參數估計法，對旋翼機抓取重物時的不確定特性參數進行估計。文獻[3]研究的無人空中系統(tǒng)加載7自由度工業(yè)機械臂，可抓取直立在地面上的長桿，并研究了抓取時的重心變化與相應的控制問題。文獻[4-5]是關于單機或多機協(xié)同運送被吊裝的重物。

上述研究都是把抓手固定在旋翼飛行機器人機身上以抓取質量輕或形狀確定的物體，且機械臂和抓手都由電機驅動。而實際中有很多需求不僅僅是簡單的抓取動作就能滿足，如在維修高壓輸電線路時需要用很大的力或力矩對損毀的高壓線路進行剪斷作業(yè)，這種作業(yè)要求末端執(zhí)行器具有巨大的力或力矩，并在較短時間內完成剪切任務。而上述研究都不能完成此任務，因為其機械臂抓手由電機驅動，末端安裝電機會不可避免的引入附加載荷，且電機振動問題也會帶來附加擾動。

為解決上述問題，本文提出一個新的旋翼飛行機器人系統(tǒng)，即由飛行機器人和多自由度液壓機械臂組成的移動操作型旋翼飛行機械臂系統(tǒng)。這里選擇液壓作為動力源，是因為液壓系統(tǒng)有很多優(yōu)于電動或氣動系統(tǒng)的特點。液壓傳動裝置依靠油液的連續(xù)流動進行傳動，傳動十分平穩(wěn)、吸振能力強且振動??；輸出同樣功率，液壓傳動的體積和質量可減少很多，因而慣性??；更重要的是，液壓傳動易于獲得很大的力和轉矩，同時又可簡化機械結構，減少機械零部件數目并降低故障幾率。

因此，將小型液壓機械臂安裝在旋翼飛行機器人上而得到一種新的液壓旋翼飛行機械臂系統(tǒng)，可獲得更好的性能，完成更多的任務。本研究已經完成了機械結構的設計，構建了動力學模型并設計了控制器，以便用于模型仿真試驗。本文將旋翼飛行機器人與液壓機械臂之間的相對動力學看成內部擾動，并通過力學分析進行建模。首先根據牛頓-歐拉方程和旋翼系統(tǒng)的空氣動力學特性，對液壓旋翼飛行機械臂系統(tǒng)進行整體動力學建模。然后，在旋翼無人機懸停模式下使用LQR控制器操作液壓機械臂，同時將機械臂的動作看成旋翼飛行機器人的擾動，并用魯棒控制器調整飛行機器人的姿態(tài)。最后，對液壓機械臂在空間進行畫弧線仿真，并通過仿真結果來衡量旋翼飛行機器人LQR與魯棒控制器的控制性能。

1液壓旋翼飛行機械臂系統(tǒng)建模

為實現對液壓旋翼飛行機械臂系統(tǒng)的控制，需要對其運動學和動力學模型進行分析和研究[6-7]。實際應用中，機械臂的動作一般都在旋翼飛行機器人空中懸停狀態(tài)下進行?；谶@種工作狀態(tài)，在建立所設計機械臂的運動學模型時，可依據機器人機械臂的傳統(tǒng)理論來進行。而對于系統(tǒng)整體動力學模型，即飛行機器人的空間6自由度剛體運動力學表示，可以考慮不同來源的力和力矩的組合。

1.1機械臂連桿坐標系的建立

將旋翼飛行機器人機體看成是液壓機械臂的基座，稱之為連桿0。四自由度機械臂旋轉底盤為連桿1，并通過關節(jié)1與機體相連接。連桿2與連桿1通過關節(jié)2相連接，直到末端連桿。這里的坐標系規(guī)定與機器人機械臂傳統(tǒng)描述方法：Denavit-Hartenberg (D-H)法類似。坐標系{i}的Z軸Zi與關節(jié)軸i共線，其指向可任選。坐標系{i}的X軸與Xi連桿公垂線重合，指向從關節(jié)i到關節(jié)i+1。利用右手定則確定坐標系的Y軸。機械臂模型的具體D-H坐標系規(guī)定如圖1所示。

按照上述方法，確定四自由度機械臂的各個連桿參數并代入到相對應變換矩陣中，可得到相鄰連桿坐標系之間的轉換矩陣。四自由度機械臂的具體連桿參數見表1。

圖1　機械臂抽象模型坐標系

連桿i桿長ai-1/mm扭角αi-1/(°)偏距di/mm關節(jié)角θi/(°)100°0θ12l1-90°0θ23l20°0θ34090°l3θ4

(1)

計算得到從末端到基座坐標系之間的轉換關系：

(2)

式(2)中四自由度機械臂的底盤不需要轉動，即θ1=0,cosθ1=1；sinθ1=0;始終保持末端液壓剪刀的刀口姿態(tài)不變，即θ4=0，cosθ4=1,sinθ4=0,則式(2)簡化為

(3)

1.2旋翼飛行機器人系統(tǒng)建模

對于帶有機械臂的旋翼飛行機器人，傳統(tǒng)的旋翼飛行機器人模型無法描述機械臂對旋翼機本體的影響。因此，需要對旋翼飛行機器人的模型[6-8]加以改進，增加機械臂對旋翼機的作用，滿足本文情況下對旋翼飛行機器人的控制要求。下面給出旋翼飛行機器人模型的主要部分，在此基礎上加入機械臂對旋翼飛行機器人的影響。

通常情況下，旋翼飛行機器人模型建立在本體坐標系下，即以機頭方向為x軸，右側為y軸，z軸指向機體下方，3個坐標軸滿足右手定則。本研究中，需要對旋翼飛行機器人進行位置控制。無人機的位置是在世界坐標系中定義的，兩個坐標系之間可以通過旋轉矩陣進行轉換。由本體坐標系到世界坐標系的變換矩陣為

(4)式中：φ、θ、ψ分別為旋翼飛行機器人的橫滾角、俯仰角和航向角。本體速度與世界坐標系下的位置可由式(5)描述：

(5)

式中：P=[x,y,z]表示飛行機器人的位置；Vb=[ub,vb,wb]表示旋翼飛行機器人的前向、側向及垂向的速度；下標w表示當前變量是在世界坐標系中定義的；下標b表示當前變量是在飛行機器人本體坐標系下定義的。

在本體坐標系下對旋翼飛行機器人的模型進行簡要介紹。首先考慮旋轉運動：

(6)

(7)

式中：Fb為空氣動力矢量；Mb為空氣動力矩矢量；Fg為重力矢量；Ib為轉動慣量；m為旋翼飛行機器人的質量。通常情況下，空氣動力矢量和空氣動力矩可由下列非線性函數描述：

(8)

式中：ff、fm為非線性函數；a1s、b1s分別為主旋翼縱向和橫向揮舞角；δcol為主旋翼總距角；δped為尾旋翼總距角?？紤]主旋翼揮舞動力學，有

(9)

式中：τmr為主旋翼時間常數；Abs、Bas為耦合系數；δlon和δlat分別為前向和側向控制量。

公式(4)～(9)簡要描述了通用旋翼飛行機器人模型。下面將考慮機械臂對旋翼飛行機器人的影響，繼而建立包含機械臂影響的液壓旋翼飛行機械臂系統(tǒng)模型。假設機械臂與旋翼飛行機器人機身固定連接，則可將它們看作一個剛體，其6自由度剛體動力學模型可由牛頓-歐拉方程描述：

(10)

(11)

在機械臂運動過程中產生的力和力矩可以從末端連桿向基座進行迭代求解

(12)

根據牛頓第三定律(作用力與反作用力)，易得出下式描述的施加在旋翼飛行機器人本體上的力和力矩

(13)

2液壓旋翼飛行機械臂系統(tǒng)控制

液壓機械臂的操作只在旋翼飛行機器人懸停模式下進行，將機械臂對旋翼飛行機器人本體的影響作為擾動，使用魯棒控制器加以抑制。

2.1液壓機械臂系統(tǒng)控制

根據文獻[9-10]可知，液壓執(zhí)行機構的開環(huán)傳遞函數為:

(14)

將上述開環(huán)傳遞函數寫為狀態(tài)空間形式可得

(15)

為實現對液壓執(zhí)行機構的控制，設計如下狀態(tài)反饋控制器：

um(t)=Kmxm(t)+Kmrrefm

(16)

式中：refm為參考輸入；Km為反饋矩陣，用于保證系統(tǒng)的穩(wěn)定性，可用線性二次型調節(jié)器(LQR)方法計算得到；Kmr為前饋矩陣，用于實現對參考輸入的跟蹤。

(17)

式中[·]+表示偽逆。液壓執(zhí)行機構的狀態(tài)xm無法直接測得，因此需要設計如下狀態(tài)觀測器對其進行觀測：

(18)

式中觀測器矩陣L可以通過極點配置的方法求得，則相應的狀態(tài)反饋控制器為

(19)

2.2旋翼飛行機器人系統(tǒng)控制

為抑制機械臂運動對旋翼飛行機器人的影響，將Fm與Mm看作是作用在旋翼飛行機器人本體上的擾動，通過控制算法對其進行抑制。

選取旋翼飛行機器人懸停點附近的一個穩(wěn)定狀態(tài)作為平衡點，對2.1所述包含機械臂影響的旋翼飛行機器人模型進行線性化，可以得到如下線性模型。

yp(t)=C1xp(t)

(20)

zp(t)=C2xp(t)

為實現旋翼飛行機器人的跟蹤控制，定義誤差向量e(t)=∫ref-z(t)dt，則式(20)可以表示為

yp(t)=Ce1xe(t)

(21)

zp(t)=Ce2xe(t)

令Erω(t)=Eeωp(t)+Reref，則

(22)

為減小擾動對系統(tǒng)的影響并保證一定的跟蹤精度，需要設計控制器對上述系統(tǒng)進行控制。由于系統(tǒng)狀態(tài)中的a1s、b1s無法通過傳感器直接測量，故設計如下動態(tài)輸出反饋控制器

(23)

式中：xc為控制器狀態(tài)向量，其維數與xp相同；Ac、Bc、Cc、Dc為待定的控制器矩陣，則閉環(huán)系統(tǒng)可以表示為

(24)

為盡可能減小擾動ω(t)對系統(tǒng)的影響，考慮H∞性能，‖e‖2=‖ref-zp(t)‖2≤γ‖ω‖2，與存在X>0使下列矩陣不等式存在等價[11]：

(25)

值得注意的是，式(25)并不是線性矩陣不等式(LMI)，因為A矩陣中存在待定矩陣，而X同樣也是待定的。因此，無法直接對該矩陣不等式求解?？捎孟ɑ蜃兞刻鎿Q法[12]，將上述矩陣不等式轉化為線性矩陣不等式進行求解，從而得到動態(tài)輸出反饋控制器的參數矩陣Ac、Bc、Cc、Dc。

這樣，機械臂對于旋翼飛行機器人的影響可以被有效的抑制，為機械臂的準確操作提供了較為穩(wěn)定的平臺。

3系統(tǒng)仿真及驗證

為驗證機械臂控制方法的有效性，基于現有的液壓執(zhí)行機構開環(huán)模型，對機械臂執(zhí)行機構閉環(huán)控制系統(tǒng)進行仿真，仿真結果如圖2所示。虛線為期望角度，實線為機械臂實際運動角度。由仿真結果可知，機械臂可基本按照給定期望運動，滿足控制要求。

為驗證旋翼飛行機器人對機械臂擾動抑制的情況，對3種擾動情況進行仿真驗證，仿真結果如圖3所示。仿真過程中，設定旋翼飛行機器人與機械臂總重35kg，在第10s至20s，在旋翼飛行機器人機體坐標x軸方向施加頻率為0.2Hz幅值為10N的力，用于模擬機械臂在x軸方向對旋翼飛行機器人產生的力擾動；第30s至40s，在y軸方向施加頻率為0.2Hz幅值為10N的力；第50s至60s，在x軸上施加頻率為0.2Hz幅值為5Nm的力矩擾動。圖3顯示不同的擾動對系統(tǒng)穩(wěn)定性的影響。

圖2　機械臂控制響應曲線

圖3　旋翼飛行機器人對機械臂擾動的抑制情況

由圖3可知，本文所用的H∞魯棒控制方法對機械臂施加于旋翼飛行機器人的力擾動有很好的抑制效果，其中，姿態(tài)變化小于0.6rad，位置變化小于0.05m。對于力矩擾動的抑制效果相對較差，其中，姿態(tài)變化小于1rad，位置變化小于0.1m。

基于液壓機械臂模型、旋翼飛行機器人模型及控制方法，對液壓旋翼飛行機械臂系統(tǒng)進行綜合仿真驗證。為驗證旋翼飛行機器人懸停狀態(tài)下，安裝于旋翼飛行機器人上的機械臂運動時其末端刀口的運動情況，令機械臂關節(jié)在一定角度內緩慢擺動，對刀口的運動軌跡進行繪制，仿真結果如圖4所示。其中虛線為期望運動軌跡，實線為安裝于旋翼飛行機器人上的液壓機械臂末端刀口的運動軌跡。由仿真結果可知，由機械臂運動導致的旋翼飛行機器人的晃動對機械臂末端刀口運動軌跡的影響得到了較好的抑制，驗證了本文建模方法與控制方法的可行性。

圖4　刀口的運動軌跡仿真

4結束語

分析了液壓旋翼飛行機械臂系統(tǒng)動力學模型，并基于模型設計不同的控制器。采用LQR控制器對機械臂進行控制，仿真驗證了機械臂控制器的有效性。對旋翼飛行機器人與空中機械臂之間的相對擾動，使用魯棒控制器進行抑制；對所施加的不同的擾動進行仿真，驗證了控制器的魯棒性，確保在旋翼機器人本體穩(wěn)定的情況下進行機械臂操作作業(yè)的有效性。

下一步，將搭建物理實驗仿真平臺，實際測試本文提出的控制策略的性能，為未來的空中操作型機器人的建立做好準備工作。

參考文獻：

[1]E I Paul,D R Bersak,A M Dollar.The Yale Aerial Manipulator:Grasping in Flight [C]//IEEE International Conference on Robotics and Automation.NY,USA:IEEE，2011:2974-2975.

[2]D Mellinger,Q Lindsey,M Shomin,et al.Design,modeling,estimation and control for aerial grasping and manipulation [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.NY,USA:IEEE,2011:2668-2673.

[3]F Huber,K Kondak,K Krieger,et al.First analysis and experiments in aerial manipulation using fully actuated redundant robot arm [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems.NY,USA:IEEE,2013:3452-3457.

[4]M Bisgaard,J Bendtsen,A la Cour-Harbo.Modeling of Generic Slung Load System [J].Journal of Guidance,Control and Dynamics,2009,32(2):573-585.

[5]M Bernard,K Kondak.Generic Slung Load Transportation System Using small Size Helicopters [C]//IEEE International Conference on Robotics and Automation.NY,USA:IEEE,2009:1278-1284.

[6]D Song,J Qi,L Dai,et al.Modeling a small-size unmanned helicopter using optimal estimation in the frequency domain [J].International Journal of Intelligent Systems Technologies and Applications,2010,8(1-4):70-85.

[7]Cai G,Chen B,Lee T.Unmanned Rotorcraft System [M].London:Springer,2011.

[8]Shim H.Hierarchical fight control system synthesis for rotorcraft-based unmanned aerial vehicles [D].Berkeley:University of California,2000.

[9]Daqing Zhang,Qinghua He,Xuan Wu,et al.Adaptive control of hydraulic excavator manipulator [C]//IEEE International Conference on Mechatronics and Automation.Chengdu,China:IEEE,2012.

[10]程濤.液壓挖掘機的軌跡規(guī)劃與控制[D].沈陽：東北大學，2007.

[11]Gahinet P,Apkarian P.A linear matrix inequality approach toH∞control [J].International Journal of Robust and Nonlinear Control,1994,5(4):421-448.

[12]俞立.魯棒控制——線性矩陣不等式處理方法 [M].北京：清華大學出版社，2002.

(責任編輯：趙麗琴)

The Modeling and Control Method for a Hydraulic Rotorcraft Aerial Manipulator

LIN Tianyu1,2，QI Juntong1，YAO Jun2

(1.State Key Laboratory of Robotics,Shenyang Institute of Automation,Chinese Academy of Sciences,Shenyang 110016,China；2.Shenyang Ligong University,Shenyang 110159，China)

Abstract：A Rotorcraft Aerial Manipulator (RAM) system is obtained through installing mechanical manipulator on RUAV,which expands the application scope of RUAVs.The research is done on installing miniature hydraulic manipulator system on the existing RUAV.The overall dynamic model of the hydraulic RAM systems is obtained through the analysis of the coupled motion of the RUAV and the manipulator.The planar swinging motion of the manipulator is controlled by LQR controller.The motion of the manipulator is considered as a disturbance to the RAM system and a robust controller is utilized to eliminate the disturbance and stabilize the R-UAV′s hovering pose.The model structure and the control strategy proposed are verified through simulation.

Key words：RUAV；hydraulic manipulator；dynamic modeling；LQR control；robust control

中圖分類號：TP241.3

文獻標志碼：A

文章編號：1003-1251(2016)02-0054-06

作者簡介：林天宇(1988—)，男，碩士研究生；通訊作者：齊俊桐(1981—)，男，研究員，研究方向：飛機機器人系統(tǒng)理論及技術、機器人自主控制等。

收稿日期：2014-12-10