何慰，方勇純,*，梁瀟，張鵬

1. 南開大學(xué) 機器人與信息自動化研究所，天津 300350 2. 天津市智能機器人技術(shù)重點實驗室，天津 300350

旋翼無人機在應(yīng)用領(lǐng)域受到越來越多的關(guān)注。由于其結(jié)構(gòu)簡單、成本低廉的優(yōu)點，被廣泛應(yīng)用于航拍、電力巡檢、巡邏等應(yīng)用領(lǐng)域[1]。在這些應(yīng)用場景中，無人機主要用于環(huán)境感知，缺乏和環(huán)境的主動交互。飛行機械臂系統(tǒng)是在旋翼無人機上安裝帶有末端執(zhí)行器的機械臂，增強了無人機系統(tǒng)和環(huán)境的交互能力，從而使其在貨物抓取與運輸、裝配、樣品采集、救災(zāi)等領(lǐng)域發(fā)揮極大的作用[2-7]。

旋翼無人機本身存在欠驅(qū)動性和狀態(tài)變量之間的耦合特性[8]，在增加機械臂后不僅增加了系統(tǒng)的耦合性，還增加了系統(tǒng)自由度數(shù)量，使得系統(tǒng)模型極為復(fù)雜。在考慮無人機自身受到干擾的情況下，需要7自由度機械臂才能使得末端執(zhí)行器以任意姿態(tài)到達工作空間內(nèi)任意工作點且抑制擾動的影響[5]。因此飛行機械臂系統(tǒng)一般為高冗余系統(tǒng)，這使得該系統(tǒng)的運動規(guī)劃問題非常困難。無人機帶臂飛行過程中，機械臂產(chǎn)生的質(zhì)心偏移會使無人機受到更大的離心力和科氏力影響；而在機械臂動態(tài)運動過程中，也會對無人機產(chǎn)生反作用力，這些擾動會顯著影響無人機的穩(wěn)定性。綜上所述，飛行機械臂系統(tǒng)在建模、規(guī)劃和控制等方面都十分有挑戰(zhàn)性。

在建模方面，主要有牛頓-歐拉法和歐拉-拉格朗日法[9]。前者在實際應(yīng)用中易于實現(xiàn)，后者由于顯式地給出系統(tǒng)質(zhì)量矩陣、廣義科氏力矩陣和重力矩陣，便于進行控制器的設(shè)計和分析。機械臂對無人機系統(tǒng)的影響既可以被視為擾動[10]，也可以與無人機作為一個整體精確建模[9,11-12]，同時還可以將機械臂的部分影響建模到整體模型中而忽略次要影響[13]。Zhang等[13]將機械臂運動過程中機械臂的質(zhì)心偏移建模到系統(tǒng)中，在控制器設(shè)計中忽略了難以獲得和計算的機械臂慣性張量矩陣。文獻[14]對飛行機械臂系統(tǒng)的模型在平衡點附近做了局部線性化。在控制器設(shè)計方面，處理機械臂對無人機影響的方式有多種：① 將 機械臂及負載的影響當作外部擾動，使用抗擾控制或擾動觀測器估計并補償；② 將機械臂對無人機的影響建模，直接計算出其對無人機的擾動并補償；③ 將機械臂與無人機進行整體建模和控制。文獻[15]對系統(tǒng)進行了整體建模，使用了反饋線性化和一定的模型簡化對內(nèi)外環(huán)解耦，并使用自適應(yīng)控制處理模型誤差和外界擾動。Kim等使用了飛行機械臂整體模型，設(shè)計了滑模自適應(yīng)控制器[11]。文獻[16]采用了擾動觀測器和魯棒控制來處理機械臂及負載擾動。處理飛行機械臂系統(tǒng)與外界環(huán)境之間的交互可以采用阻抗控制的方法[12]。此外，可以綜合多種方法抑制機械臂以及外界擾動：對飛行機械臂的部分擾動建模，如質(zhì)心偏移等，并使用電池的位置進行補償，然后將其他部分使用擾動觀測器來估計并補償[10]。

本文設(shè)計了一種2自由度飛行機械臂系統(tǒng)，對系統(tǒng)整體建立了精確的模型，并在一定條件下對模型進行簡化。對簡化模型設(shè)計了基于內(nèi)外環(huán)的控制器，并通過仿真驗證了控制方法的可行性和有效性。本文其余部分組織如下：第1節(jié)對設(shè)計的飛行機械臂系統(tǒng)進行介紹；第2節(jié)介紹系統(tǒng)模型的構(gòu)建；第3節(jié)實現(xiàn)了控制器設(shè)計；第4節(jié)介紹仿真實驗并分析實驗結(jié)果；第5節(jié)是總結(jié)和展望。

1 系統(tǒng)設(shè)計

本文設(shè)計的飛行機械臂系統(tǒng)如圖1所示。無人機采用軸距450 mm的四旋翼無人機，搭載Pixhawk開源飛控，使用Raspberry Pi 3作為機載電腦。無人機的動力系使用T-MOTOR MN3508無刷電機、HOBBYWING XRotor Micro 40A BLHeli_32 DShot1200電調(diào)。

圖1 飛行機械臂系統(tǒng)Fig.1 Aerial manipulation system

為了能夠?qū)崿F(xiàn)抓取任務(wù)，避免機構(gòu)過于復(fù)雜，確保系統(tǒng)重量適中，采用2自由度機械臂的pitch-pitch配置。使用ROBOTISE公司的Dynamixel XH430-W350R總線舵機直接驅(qū)動。機械臂的各個連接部件采用3D打印的方式加工。機械臂的硬件驅(qū)動系統(tǒng)如圖2所示。

圖2 機械臂硬件Fig.2 Hardware of manipulator

為了適應(yīng)抓取任務(wù)，機械臂末端的夾爪應(yīng)該具有較快的張合速度，因此使用了一種舵機(KST DS145MG)驅(qū)動的平行四邊形四連桿機構(gòu)(如圖3所示)。由于環(huán)境和系統(tǒng)本身的限制，不能保證以最合適的角度接近被抓取物體，并且無人機在飛行和懸停過程中存在抖動，因此采用了基于鰭條效應(yīng)(Fin ray?effect)的夾爪手指[17]。這種仿生學(xué)設(shè)計可以自動貼合被抓取物體，對于抓取角度誤差和不規(guī)則物體有很強的包容性。

圖3 機械臂末端執(zhí)行器Fig.3 End effector of manipulator

2 系統(tǒng)模型

本節(jié)將首先介紹飛行機械臂系統(tǒng)的運動學(xué)模型，在此基礎(chǔ)上根據(jù)歐拉-拉格朗日方程構(gòu)建系統(tǒng)完整的動力學(xué)模型，最后根據(jù)特定假設(shè)對動力學(xué)模型進行簡化。

2.1 運動學(xué)模型

系統(tǒng)坐標系和動力學(xué)參數(shù)如圖4所示。Σb、ΣC1、ΣC2分別代表位于無人機質(zhì)心以及2個機械臂連桿質(zhì)心處的坐標系；O0、O1、O2、Oe為分別固連在無人機、連桿1、連桿2、末端執(zhí)行器上的坐標系原點[18]。L1、L2分別表示2個機械臂連桿的長度。

圖4 飛行機械臂系統(tǒng)示意圖Fig.4 Illustration of aerial manipulation system

(1)

記機體坐標系到慣性坐標系的旋轉(zhuǎn)矩陣為Rb，其表達式為

(2)

式中：s和c分別代表正弦和余弦函數(shù)。則有

(3)

(4)

式中：[·]×表示對向量取反對稱矩陣。將式(4)展開后可得[18]

(5)

(6)

表1 坐標系相互關(guān)系Table 1 Relationships between coordinates

機械臂各個連桿的質(zhì)心(Ci)位置在慣性坐標系下的表示(pCi)為

(7)

經(jīng)過計算得到連桿1和連桿2的運動學(xué)方程為

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

于是可以得到各個連桿相對于慣性坐標系的運動學(xué)方程為

(16)

(17)

(18)

2.2 動力學(xué)模型

下面使用歐拉-拉格朗日法建立系統(tǒng)完整的動力學(xué)模型:

(19)

L=K-U

(20)

(21)

于是有

(22)

無人機旋翼產(chǎn)生的升力大小為F，則有

(23)

(24)

(25)

式中:M11(q)∈R3×3，M22(q)∈R3×3，M33(q)∈R2×2為正定對稱矩陣；M12(q)=M21(q)T∈R3×3，M13(q)=M31(q)T∈R3×2，M23(q)=M32(q)T∈R3×2。

(26)

i,j= 1,2,…,8

(27)

G(q)的計算方式為

(28)

2.3 模型簡化

將式(24)寫成分塊矩陣的形式:

(29)

(30)

因而可以得到無人機子系統(tǒng)模型

(31)

3 控制器設(shè)計

飛行機械臂系統(tǒng)是一個高度非線性的系統(tǒng)，機械臂的構(gòu)型和運動增加了整個系統(tǒng)的復(fù)雜度，使得對于系統(tǒng)的整體控制非常困難。由于無人機內(nèi)環(huán)動態(tài)顯著快于外環(huán)，因此可以針對無人機內(nèi)環(huán)與外環(huán)分別設(shè)計控制器。

(32)

(33)

從而可得新的系統(tǒng)為

(34)

由式(33)可以看出，在設(shè)計外環(huán)控制器輸入τp時需要使用內(nèi)環(huán)控制器輔助輸入w2，然而此時w2是未知的。為了消除設(shè)計控制器時內(nèi)環(huán)和外環(huán)之間的耦合，可以將M12置為0。記新的質(zhì)量矩陣為

(35)

文獻[20]通過仿真結(jié)果指出，由于飛行機械臂系統(tǒng)中機械臂的質(zhì)量一般顯著低于無人機，因此相比于M11，M12可以忽略不計，因而式(35)是合理的。于是新的控制輸入為

(36)

由此外環(huán)引入了建模誤差，將外環(huán)的建模誤差和空氣阻力擾動、負載變化等統(tǒng)一表示成未知擾動d∈R3。最終得到內(nèi)外環(huán)2個子系統(tǒng)

(37)

反饋線性化后的精確模型為

(38)

(39)

3.1 外環(huán)滑?？刂破?/h3>

對外環(huán)子系統(tǒng)設(shè)計滑?？刂破鱗21]。無人機質(zhì)心的位置期望軌跡為pd，用戶給定偏航角為ψr，則外環(huán)位置和速度跟蹤誤差為

(40)

定義滑模面為

(41)

式中：Α=diag(α)，α∈R3，α>0。則可以得到輔助控制輸入為

(42)

其中：K=diag(k)，k∈R3且滿足：

k≥|d|

(43)

其中：|·|代表對矢量各元素求絕對值。于是可以得到外環(huán)的控制輸入為

(44)

由此，可以得到期望升力為

(45)

由于升力的輸入和無人機姿態(tài)耦合，造成了無人機的欠驅(qū)動性質(zhì)。具體而言，無人機升力的方向與機體坐標系z軸平行，因此機體坐標系期望姿態(tài)下的z軸單位向量為

(46)

由給定期望偏航角ψr，可以得到期望姿態(tài)下機體坐標系x軸單位向量為

(47)

無人機機體坐標系的x軸和z軸不能任意配置，因此只能保證機體坐標系x軸和期望方向在同一垂直平面內(nèi)[22]，所以期望姿態(tài)下的機體坐標系可以定義為

(48)

由此可以得到期望姿態(tài)的旋轉(zhuǎn)矩陣為

(49)

進而可以得到期望姿態(tài)對應(yīng)的歐拉角為

(50)

其中:

(51)

式中：Rb,d(i,j)表示矩陣Rb,d第i行第j列的元素。

此外，由于存在sgn(s)這一不連續(xù)函數(shù)，閉環(huán)系統(tǒng)會出現(xiàn)抖振現(xiàn)象。為了消除系統(tǒng)的抖振現(xiàn)象，并且使得內(nèi)環(huán)控制器的期望連續(xù)可微，可以使用連續(xù)的函數(shù)來近似不連續(xù)的符號函數(shù)，本文使用的近似函數(shù)為雙曲正切函數(shù)tanh(10s)。

3.2 內(nèi)環(huán)控制器

由于內(nèi)環(huán)的期望ψd、θd、φd由外環(huán)得到，因此很難解析獲得它們的高階導(dǎo)數(shù)，而需要使用數(shù)值微分得到。數(shù)值微分會嚴重受到噪聲影響，破壞系統(tǒng)的穩(wěn)定性。根據(jù)時標分離原則，考慮內(nèi)環(huán)是一個快變系統(tǒng)，且其暫態(tài)過程對外環(huán)可以忽略，則可以將內(nèi)環(huán)的跟蹤當作一個鎮(zhèn)定問題，這樣一來，可以認為期望姿態(tài)歐拉角的二階及以上導(dǎo)數(shù)為0，由此可以得到誤差

(52)

設(shè)計PID控制器為

(53)

式中:Kp∈R3×3、Ki∈R3×3、Kd∈R3×3為非負對角增益矩陣。由此可得內(nèi)環(huán)控制輸入為

(54)

該控制輸入為歐拉角對應(yīng)的廣義力矩，需要轉(zhuǎn)換為施加到系統(tǒng)上的真實力矩。根據(jù)式(22)，可以得到機體坐標系下的力矩輸入為

(55)

4 仿真實驗

本節(jié)介紹物理仿真實驗結(jié)果。通過機器人物理仿真軟件v-rep[23]和MATLAB進行仿真，和開源飛控Pixhawk中采用的基于四元數(shù)的PID算法進行對比。本文設(shè)計了3種仿真實驗場景來測試所提方法在機械臂靜態(tài)和動態(tài)擾動環(huán)境下的鎮(zhèn)定和軌跡跟蹤性能。為了解決不連續(xù)滑?？刂茙淼亩墩駟栴}，在實現(xiàn)控制器時將符號函數(shù)sgn(s)替換為雙曲正切函數(shù)tanh(10s)。

實驗1機械臂靜態(tài)偏置下的鎮(zhèn)定實驗。設(shè)置機械臂靜態(tài)偏置為η1=60°,η2=30°。由于機械臂的偏置，系統(tǒng)的質(zhì)心偏離機體坐標系原點。實驗結(jié)果如圖5所示。

圖5 實驗1無人機位置Fig.5 Positions of UAV in experiment 1

實驗2機械臂周期性擺動下的鎮(zhèn)定實驗。機械臂在2個關(guān)節(jié)位形之間(η1=60°,η2=30°和η1=-60°,η2=-30°)做周期性擺動。實驗結(jié)果如圖6～圖8所示。

圖6 實驗2無人機位置Fig.6 Positions of UAV in experiment 2

圖7 實驗2無人機姿態(tài)(Pixhawk方法結(jié)果)Fig.7 Orientations of UAV in experiment 2 (results of Pixhawk method)

圖8 實驗2無人機姿態(tài)(本文方法結(jié)果)Fig.8 Orientations of UAV in experiment 2 (results of presented method)

實驗3機械臂周期性擺動下的直線軌跡跟蹤實驗。機械臂在2個關(guān)節(jié)位形之間(η1=60°,η2= 30°和η1=-60°,η2=-30°)做周期性擺動，同時無人機跟蹤三次樣條速度曲線。實驗結(jié)果如圖9～圖11所示。

圖9 實驗3無人機位置Fig.9 Positions of UAV in experiment 3

圖10 實驗3無人機姿態(tài)(Pixhawk方法結(jié)果)Fig.10 Orientations of UAV in experiment 3 (results of Pixhawk method)

圖11 實驗3無人機姿態(tài)(本文方法結(jié)果)Fig.11 Orientations of UAV in experiment 3 (results of presented method)

實驗4機械臂周期性擺動下的S形曲線軌跡跟蹤實驗。機械臂在2個關(guān)節(jié)位形之間(η1=60°,η2=30°和η1=-60°,η2=-30°)做周期性擺動，同時無人機跟蹤S形曲線。實驗結(jié)果如圖12～圖15所示。

圖12 實驗4無人機位置Fig.12 Positions of UAV in experiment 4

圖13 實驗4無人機位置誤差Fig.13 Position errors of UAV in experiment 4

圖14 實驗4無人機姿態(tài)(Pixhawk方法結(jié)果)Fig.14 Orientations of UAV in experiment 4 (results of Pixhawk method)

圖15 實驗4無人機姿態(tài)(本文方法結(jié)果)Fig.15 Orientations of UAV in experiment 4 (results of presented method)

由圖5可以看出，相比于Pixhawk采用的基于四元數(shù)的串級PID算法，由于對機械臂質(zhì)心偏移進行了補償，所提方法在3個坐標軸方向上位置誤差均有明顯改善。通過對圖6、圖9、圖12、圖13分析可知，存在機械臂擺動時，所提方法的無人機位置誤差均低于Pixhawk的算法。通過對圖7、圖8、圖10、圖11、圖14、圖15的對比可以觀察到，在所有實驗中，所提方法的姿態(tài)誤差均小于Pixhawk的算法。因此所提控制方法能夠明顯抑制機械臂靜態(tài)偏置以及動態(tài)擺動對鎮(zhèn)定及跟蹤性能的影響，其位置誤差和超調(diào)以及姿態(tài)誤差均明顯低于Pixhawk基于四元數(shù)的串級PID算法。實驗證明了所提控制方法對于飛行機械臂系統(tǒng)的有效性。

5 結(jié) 論

本文設(shè)計了一種2自由度飛行機械臂系統(tǒng)，并搭建了飛行機械臂實物平臺。使用歐拉-拉格朗日法建立了系統(tǒng)模型。針對機械臂緩慢運動的情況，對系統(tǒng)模型進行了有效簡化，降低模型的復(fù)雜度，基于此設(shè)計了內(nèi)外環(huán)分離的控制器。仿真實驗證明了所提方法對于帶有機械臂擾動的無人機鎮(zhèn)定和軌跡跟蹤任務(wù)均具有較好效果。在接下來的工作中，將會考慮使用范圍更廣的模型假設(shè)。由于歐拉角的奇異性和不連續(xù)性問題，還將考慮內(nèi)環(huán)使用基于旋轉(zhuǎn)矩陣或者四元數(shù)的姿態(tài)控制方案。

[21] KHALIL H K. Nonlinear systems[M]. Upper Saddle River: Prentice Hall, 2002: 552-579.