馬 樂，劉 杰，馬志麗，閆一鳴，徐東甫

（1. 東北電力大學 自動化工程學院，吉林 132000; 2. 渤海船舶職業(yè)學院 電氣工程系 葫蘆島 125105）

目前建筑物外觀清潔、輸電線裝配、光伏設備維護等絕大部分空中作業(yè)仍依賴人工，其風險性、艱巨性與低效性顯然。行業(yè)需要、人本訴求，使研制有效的空中自動化作業(yè)設備成為產業(yè)升級的必然趨勢。

旋翼飛行平臺與機械臂結合的空中作業(yè)系統(tǒng)（Aerial Manipulation System, AMS）彌補了傳統(tǒng)旋翼無人機執(zhí)行能力不足，同時繼承了傳統(tǒng)旋翼靈活機動、操作簡單、對復雜環(huán)境的高度適應等優(yōu)點[1,2]，能靈活地實現(xiàn)廣泛的主動空中作業(yè)任務（如圖1）。有效的AMS能顯著提升空中作業(yè)自動化水平，為開辟新產業(yè)空間和經濟鏈提供良好契機。因此近年AMS研究受到廣泛關注[3]。

圖1 空中作業(yè)機器人操作示意圖Fig.1 Diagram of application scenarios of aerial manipulation

現(xiàn)有研究幾乎涵蓋了各類傳統(tǒng)飛行平臺與不同類型操作機構的結合方式。多旋翼與串聯(lián)機械臂的結合方式在靈活性、可控性等方面較其他結合方式有顯著優(yōu)勢[4,5]。飛行平臺運動相對局限，因此Euler角被廣泛采用以分析飛行平臺運動學，機械臂運動則以Denavit-Hartenberg（D-H）方法為主。動力學建模則繼續(xù)沿用旋翼平臺與機械臂領域的Newton-Euler和Lagrangian分析方法[6,7]。

雖然AMS研究取得了一系列成果，但因其系統(tǒng)復雜性導致現(xiàn)有研究成果不足以支撐實際作業(yè)任務[8]。其中飛行平臺控制是關鍵問題之一[9]，其難點在于：1）AMS屬一類樹形無根多剛體系統(tǒng)，其強耦合與非線性增加了控制難度；2）飛行平臺與機械臂間存在動態(tài)約束，機械臂的位形與速度影響平臺控制，平臺運動又決定機械臂末端執(zhí)行器位姿；3）目前采用飛行平臺多為非完整結構，位置與姿態(tài)控制需分部控制，在控制中勢必犧牲位置或姿態(tài)精度；4）姿態(tài)控制是飛行平臺控制的核心，采用慣性測量技術的傳感器在測量時不可避免會產生噪聲，對控制的精度產生一定程度的影響；5）由于機構限制平臺無法對機械臂運動提供足夠的補償，進而影響系統(tǒng)穩(wěn)定性。因此AMS飛行平臺控制問題亟待解決。

為增強穩(wěn)定性，文獻[10]采用減小機械臂體積以此減小因后者運動造成的影響，但該方式導致了系統(tǒng)負載能力較低。文獻[11]集成了大功率機械臂以提升負載能力，但該方案將研究重心放在機械臂的規(guī)劃，增加了復雜度，同時因設備龐大不適用于常規(guī)空中作業(yè)任務。文獻[12]采用多個旋翼平臺共同搭載同一機械臂以提升執(zhí)行能力并降低機械臂對單個旋翼平臺影響，然而該方式增大了系統(tǒng)的運行空間并提升了系統(tǒng)協(xié)同難度，影響穩(wěn)定性。

除機械參數(shù)外，子系統(tǒng)間作用對平臺穩(wěn)定性也至關重要。文獻[13]發(fā)展了上述研究，視AMS為整體時變剛體，通過估計機械臂運動時AMS整體質心與慣性張量設計控制器。但該方法在動量定理中的理論依據與精度有待討論，且從算法過程看出計算動態(tài)慣性張量與質心位置本身難度巨大，僅當機械臂低速且空載運動條件下才近似接近實際狀態(tài)。

文獻[11-13]等視子系統(tǒng)間作用力/矩為擾動并依此設計控制器，但當作用力/矩持續(xù)變化時該方法控制效果欠佳。文獻[14]采用模型預測控制以調和上述擾動，但控制性能受目標函數(shù)設計影響，同時解算相對復雜影響實時性。文獻[15]建立了系統(tǒng)間作用力/矩解析表示，由此可分析并設計動態(tài)負載補償項。但現(xiàn)有飛行平臺因機構局限，無法對任意方向的動態(tài)負載力/矩給予充分補償。

同時，由于慣性傳感器的信號中存在隨機的非高斯噪聲[16]，不準確的噪聲信息可能導致位置和速度漂移。并且噪聲的大小可能會在不同的應用和環(huán)境中發(fā)生變化。這意味著在實際應用過程中噪聲具有時變性，因此傳感器的噪聲問題是飛行平臺控制的重要問題。文獻[17]中針對飛行平臺能力的限制，無法使用大體積、重質量的慣性測量單元，提出了一種變距離多普勒質心的誤差估計方法，以提高慣性系統(tǒng)的精度。文獻[18]針對磁干擾影響多旋翼航向出現(xiàn)偏差問題，提出基于擴展Kalman濾波的姿態(tài)信息融合方法，通過建立可調節(jié)測量噪聲方差矩陣，實現(xiàn)自適應修正多旋翼飛行器航向功能。

上述分析得出有效的AMS飛行平臺設計與控制仍為亟待解決問題。文獻[19]打破了傳統(tǒng)多旋翼結構，通過調整旋翼位置與方向等參數(shù)設計出具有全向驅動能力的飛行平臺?；谌蝌寗釉O計思路，作者前期設計了一種基于全向驅動旋翼平臺的空中作業(yè)系統(tǒng)及其PID控制方法[20]。

為進一步提升控制性能，本文在前期工作[20]基礎上提出了一種基于自抗擾、反演與動態(tài)補償?shù)娜尶罩凶鳂I(yè)飛行平臺控制方法，記為ABC（Adrc,Backstepping, Compensation）控制。針對空中作業(yè)機器人動力學模型較為復雜問題，采用Carig與遞歸Newton-Euler方程推導了有效的動力學模型?？紤]機械臂對飛行平臺產生的擾動影響，建立了飛行平臺全向驅動控制補償項，并結合自抗擾控制技術、反演控制技術以及動態(tài)補償策略，有效地解決了強噪聲、干擾和不確定性問題，提升了控制性能。

1 全驅AMS機械結構

圖2 為全驅空中作業(yè)系統(tǒng)SolidWorks設計圖。與傳統(tǒng)平臺不同，本文旋翼結構為大傾角( βi＜ 60°)，因此旋翼能夠提供機體坐標系x與y方向的推進力以及力矩，進而能實現(xiàn)飛行平臺的全向驅動。

圖2 全向驅動AM系統(tǒng)SolidWorks結構圖Fig.2 Structure chart fully-actuated AM system in SolidWorks

系統(tǒng)機械臂設計為4關節(jié)以兼顧靈活性與負載能力。采用Dynamixel AX-12A伺服舵機為關節(jié)驅動，該舵機具有位置、速度等反饋功能，結合電壓、電流、負載等反饋信息可估計關節(jié)角加速度，并能以位置、速度作為指令控制關節(jié)運動。

2 系統(tǒng)建模

設慣性參考坐標系、旋翼平臺坐標系和機械臂第i關節(jié)坐標系分別為｛W ｝、｛A｝、｛ Mi｝，｛A｝相對｛W｝位姿矩陣為：

其中0PA與0RA分別為｛A｝相對｛W ｝的原點向量與旋轉矩陣，0RA可由 x,y,z向歐拉角[φ,ψ,γ]得到。

機械臂關節(jié)坐標系i-1與i間變換矩陣表示為（規(guī)定i=1為關節(jié)1與｛A｝變換矩陣）：

其中i-1Ri為i系到i-1系旋轉矩陣，i-1Pi為i系原點在i-1系坐標。采用Craig參數(shù)計算i-1Ai。其中為常數(shù)，θ為關節(jié)角度變量。表1為本文機械臂Craig配置參數(shù)表（為方便表示末端執(zhí)行器位姿，加設虛擬關節(jié)5）。

表1 機械臂Craig參數(shù)表Tab.1 Craig parameters list of manipulator

由坐標系傳遞關系，可得各關節(jié)坐標系相對｛W ｝系位姿矩陣：

飛行平臺角速度0ωA與[φ,ψ,γ]導數(shù)關系為：

各關節(jié)坐標系角速度由0ωA與關節(jié)角速度決定，并有遞推關系：

Z0=［ 0,0,1］T。

對式(4)求導得關節(jié)坐標系間線速度關系如下：

式(1)-(7)建立了飛行平臺與機械臂各連桿坐標系位置、姿態(tài)與其角速度關系。

采用遞歸Newton-Euler方程建立系統(tǒng)動力學模型，變量均為｛W ｝系下表示。旋翼平臺的動力學方程如下：

其中0Uf、0Uτ分別為旋翼產生的驅動力/矩，0f1與0τ1分別為旋翼平臺對連桿1的力/矩，作用在連桿1的力臂向量，mA與0IA分別為旋翼平臺質量和相對｛W｝的慣性張量，GA為旋翼平臺重力向量。

其中0fi、0τi分別為連桿 i- 1對連桿i的力/矩，mi與0Ii分別為連桿質量和相對｛W ｝的慣性張量，為連桿i重力向量。分別為連桿i-1、i對連桿i、 i+1的力臂向量：

對式(6)(7)求導得：

聯(lián)立式(8)-(11)構成二階代數(shù)微分方程組，并視二階項為變量，其他參數(shù)為常量，求解方程。

3 自抗擾-反演動態(tài)補償控制

從前節(jié)分析看出旋翼平臺動力學模型呈多輸入多輸出（Multiple Input Multiple Output, MIMO）二階非線性，且與機械臂系統(tǒng)存在強耦合。相比傳統(tǒng)多旋翼平臺，AMS旋翼平臺穩(wěn)定性受機械臂嚴重影響。為此本文利用動力學模型特性建立動態(tài)補償項以抵消機械臂對平臺影響，并分別基于自抗擾與反演控制設計平臺位置與姿態(tài)控制器。

3.1 動態(tài)負載補償

分析式(8)得出，平臺質心加速度與角加速度除受驅動力/矩控制外還與角速度、機械臂作用力/矩有關。視機械臂作用力/矩為平臺負載項，并依此建立補償項udc以抵消其作用：

通過求解(8)-(11)聯(lián)立的方程組得到補償項所需參數(shù)，詳見作者前期工作[20]。控制輸入引入該補償項能化簡平臺動力學模型，從而將AMS平臺控制問題轉化為傳統(tǒng)多旋翼控制問題?？刂破鬏斎胄问綖椋?/p>

其中uap為旋翼平臺控制器輸入?？紤]對平臺位置的擾動與姿態(tài)穩(wěn)定性，本文分別采用自抗擾與反演控制分別設計平臺位置與姿態(tài)控制器。

3.2 平臺位置自抗擾控制

設飛行平臺位置 PA與速度為系統(tǒng)狀態(tài)消除補償項后式(8)位置部分化簡為：

式(14)為MIMO二階系統(tǒng)，下面設計其ADRC控制器。ADRC控制器由基于微分跟蹤器（Tacking Differentiator, TD）的過渡過程、擴張狀態(tài)觀測器（Extended State Observer, ESO）與非線性反饋控制律組成。

二階非線性最速TD的離散形式表示為：

其中h為采樣時間，yd為控制期望，1v與v2分別為過渡過程輸入及其跟蹤微分信號，fhan為特殊函數(shù)：

ESO以系統(tǒng)輸出觀測量yh估計系統(tǒng)狀態(tài)，二階離散形式為：

其中β、α、δ為可調參數(shù)。

以非線性狀態(tài)誤差反饋作為ADRC控制輸出，本文采用形式為：

其中b0、與 δu為可調參數(shù)，二階系統(tǒng)中N=2。

3.3 平臺姿態(tài)反演控制

考慮二階MIMO系統(tǒng)：

設計其反演控制器，首先定義誤差系統(tǒng)：

選擇Lyapunov函數(shù)：

式(22)(24)對時間求導并將式(23)代入得：

取

則式(31)為：

利用Yong不等式：

其中J由式(5)得到，I3×3為3維單位矩陣，則式(8)姿態(tài)部分化為式(21)。因此可由式(32)得到旋翼平臺姿態(tài)控制律。

4 仿真分析

4.1 仿真系統(tǒng)描述

為提高仿真運行效率并避免版權問題，本文仿真系統(tǒng)從前期的Matlab實現(xiàn)轉為基于Python的開源項目集，涉及主要模塊如表2。（注：因項目處于過渡時期，本文仿真程序與數(shù)據處理部分依賴Matlab）

表2 仿真系統(tǒng)主要模塊說明Tab.2 Main module of the simulation system

4.2 全驅控制測試

本節(jié)測試所提出的方法在全驅控制問題中的性能。選擇四種具有代表性的機械臂靜態(tài)位形分別為[-90°, -90°, 0°, 0°]、[- 90°, 0°, 0°,0°]、[ 0 °,0 °, -90°,0°]與[- 90 °, 0 °, - 90 °, 0°]。首先測試AMS的全驅位置控制，令初值位置為 [9m, 9m, 9m]，期望位置為[10m, 10m, 10m]，初值與期望 Euler角均為[0 °,0°,0°]。仿真參考模型參數(shù)不確定性為±10%，相對前期工作仿真增大噪聲幅值分別為± 30 mm與± 3 °范圍的高斯分布，施加± 10 N與± 80N·S范圍的高斯分布擾動，擾動間隔時間為1 s，控制頻率為100 Hz。

圖3 為基于本文方法的四種機械臂位形下旋翼平臺全驅位置控制誤差曲線圖，從中分析出：1）旋翼平臺在四種位形并伴隨噪、擾條件下均能在X,Y,Z三方向約2秒內無超調收斂至期望，同時位移誤差曲線相對平滑，其原因為ADRC中TD起了去噪作用；2）在控制過程中平臺三個姿態(tài)角誤差始終在± 2 °內，該誤差小于仿真?zhèn)鞲衅髟肼暎虼嗽O計的反演姿態(tài)控制器能在位移控制中以很小誤差保持姿態(tài)穩(wěn)定。綜上，在不同靜態(tài)機械臂位形與噪、擾條件下，本文方法在不犧牲姿態(tài)前提下同時實現(xiàn)三維位置全向驅動控制。

圖3 四種機械臂位形全驅位置控制誤差曲線Fig.3 Errors of control in 4 types of configurations of arm

設機械臂位形為[-90 °, -90 °, 0 °, 0 °]，以上述仿真擾動為單位施加1-5倍擾動，測試本文方法在有傾角懸?？刂浦行阅?，初始與期望位置均為 [9m, 9m, 9m]，初始Euler角為[0 °, 0 °, 0 °]，期望為[20 °, 20 °, 0 °]，其余仿真參數(shù)同上。

圖4 為上述條件下有傾角懸?？刂莆蛔隧憫€，從中分析出：1）φ與ψ姿態(tài)角均能快速收斂（調節(jié)時間＜1 s）至期望；2）不同擾動等級下Euler角均能以較小誤差（±1.5 °）保持穩(wěn)態(tài)；3）X與Y方向均能以小誤差穩(wěn)定在期望位置，但由于階躍響應、隨機擾動沖擊、系統(tǒng)旋翼動力參數(shù)限制與重力約束，Z方向在初始時刻產生向下震蕩，最大幅度為48 mm，然而能在1.5秒內實現(xiàn)穩(wěn)定調節(jié)，并保持低誤差穩(wěn)定。綜上得，本文方法在有傾角懸停全驅控制中響應速度快、誤差低，對噪聲與擾動具有較好抑制能力。

圖4 不同等級擾動下傾斜懸?？刂祈憫€Fig.4 Response curves of tilted hover control under different levels of disturbance

4.3 空中作業(yè)任務仿真

實際空中作業(yè)勢必需要機械臂的運動，為此本文模擬兩類空中作業(yè)任務，并與前期工作[20]中方法比較，分析本文方法在伴隨機械臂運動的空中作業(yè)任務中性能。相對文獻[20]中設置提高擾動范圍至± 40 N與± 300 N·S。

任務1為機械臂運動下的懸?？刂?，機械臂初始關節(jié)角度為[-90 °, 0 °, -45 °, 0 °]，初始與期望Euler角均為0 °，關節(jié)3以=0.5sint 速度運動，噪聲與模型參數(shù)不確定范圍同上。

圖5 為本文方法（ABC）與文獻[20]方法（DC-PID）任務1的位置與姿態(tài)誤差曲線，從中分析出：1）X與Y方向兩方法控制效果基本相同，均能以較小誤差（±15 mm內）保持穩(wěn)定；2）由于系統(tǒng)功率限制、初始擾動與重力約束兩方法在-Z方向均產生震蕩，文獻[20]方法隨后出現(xiàn)了一定程度穩(wěn)態(tài)誤差（約為20 mm），但本文方法仍能在1秒內完成調節(jié)并以小誤差穩(wěn)定，該結論與前節(jié)仿真結果一致；3）兩方法對于γ角均能以± 1.5 °誤差范圍保持穩(wěn)定，但文獻[20]方法因機械臂運動與較大擾動因素在初始時刻在φ與ψ角較本文方法出現(xiàn)明顯震蕩。綜上得出，兩方法針對任務1問題均能實現(xiàn)穩(wěn)定控制，但本文方法在大擾動條件下具有更好的抗擾能力與動態(tài)性能。

圖5 任務1位姿誤差曲線Fig.5 Errors of positions and attitudes for task 1

在上述條件基礎上，任務2加入平臺的全驅運動，設初始與期望位置分別為[9 m, 9 m, 9 m]與[10 m,10 m,10 m]，其余參數(shù)同任務1。

圖6 為兩種方法任務2條件下位置與姿態(tài)誤差曲線，從中分析出：1）本文方法在X與Y方向的調節(jié)時間較快，在Z方向的響應速度本文方法優(yōu)勢更加明顯；2）與任務1相似，本文方法在φ與ψ角調節(jié)初期震蕩明顯小于文獻[20]方法；3）由于比任務1增加了條件及系統(tǒng)性能限制，本方法γ角調節(jié)初期存在相對前一任務較大震蕩，但能在較短時間實現(xiàn)校正，其穩(wěn)態(tài)誤差與文獻[20]方法相當。綜上得出，本文提出方法在任務2條件下綜合性能優(yōu)于文獻[20]方法。

圖6 任務2位姿誤差曲線Fig.6 Errors of positions and attitudes for task 2

上述仿真分析得出，本文方法較前期提出方法具有更好的擾動抑制能力與穩(wěn)定性，從而在更大的噪聲與擾動條件下具有更好的控制性能。

4.4 CoppeliaSim平臺仿真實驗

利用Coppeliasim物理仿真系統(tǒng)搭建了虛擬實驗平臺，如圖7所示。

圖7 CoppeliaSim虛擬實驗仿真平臺Fig.7 The demonstration of virtual experiment in CoppeliaSim

本文的仿真控制周期為0.01 s。物理引擎選擇Bullet 2.78。將精度設置為最高精度。在前面數(shù)值模擬的基礎上，著重在實際問題中驗證該方法的有效性。初始位置為 [2m, 2m, 2m]。期望位置為 [3m, 3m, 3m]。初始值和期望歐拉角為[0 °, 0 °, 0 °]。第三關節(jié)以θ3= 0.5sint的速度從-π3向π3周期性擺動。噪聲和模型參數(shù)不確定性的范圍與上述相同。

圖8 為ABC控制方法和文獻[20]所述的DC-PID方法在相同條件下產生的位置和姿態(tài)響應曲線。從圖8可以看出，兩種方法都能在X,Y,Z方向收斂到期望信號，而ABC方法在3秒左右收斂到期望信號。另一方面，DC-PID收斂到需要的信號需要4-6秒。由此可以看出，ABC方法具有更快的響應速度。在這兩種方法中，由于在本節(jié)中測試的機械臂負載，φ的角度出現(xiàn)了一定程度的震蕩。由于關節(jié)3的連續(xù)運動，力會不斷地傳遞到飛行平臺上。在這種連續(xù)的強擾動作用下，ABC法的振蕩范圍更小。圖9與圖10為兩種方法的位置跟蹤和姿態(tài)跟蹤平均跟蹤誤差絕對值對比柱狀圖，從圖中可看出，無論是位置跟蹤還是姿態(tài)跟蹤，本文方法的平均跟蹤誤差絕對值都小于DC-PID方法。相比于DC-PID方法，本文方法的平均跟蹤誤差絕對值在位置X,Y,Z方向的跟蹤控制中分別降低了37.9%、45.9%、64.2%，在姿態(tài)φ,ψ,γEuler角跟蹤控制中分別降低了73.2%、84.6%、78.8%。即本文方法相比DC-PID方法，在位置與姿態(tài)跟蹤控制中，平均跟蹤誤差絕對值降低37.9%~84.6%。綜上所述，兩種方法均能實現(xiàn)穩(wěn)定控制，但ABC法具有較好的抗擾動能力和較強擾動條件下的動態(tài)性能。

圖8 CoppeliaSim實驗位姿響應曲線Fig.8 Responses of positions and attitudes for CoppeliaSim

圖9 位置系統(tǒng)跟蹤誤差Fig.9 The position system tracking error

圖10 姿態(tài)系統(tǒng)跟蹤誤差Fig.10 The attitude system tracking error

5 結 論

通過上述論述與分析得出如下結論：1）提出的基于模型動力學的補償項能夠化簡系統(tǒng)并抵消機械臂對系統(tǒng)作用影響；2）相比前期DC-PID方法，本文提出的自抗擾反演控制在強噪聲、擾動條件下具有更好的控制性能。

未來將在優(yōu)化系統(tǒng)機構設計、建立柔性學習控制、建立實時解耦方法等方面開展進一步工作。