一種兩自由度飛行機(jī)械臂系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)

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

1. 南開(kāi)大學(xué) 機(jī)器人與信息自動(dòng)化研究所，天津 300350 2. 天津市智能機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，天津 300350

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

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

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

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

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

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

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

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

圖2 機(jī)械臂硬件Fig.2 Hardware of manipulator

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

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

2 系統(tǒng)模型

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

2.1 運(yùn)動(dòng)學(xué)模型

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

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

(1)

記機(jī)體坐標(biāo)系到慣性坐標(biāo)系的旋轉(zhuǎn)矩陣為Rb，其表達(dá)式為

(2)

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

(3)

(4)

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

(5)

(6)

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

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

(7)

經(jīng)過(guò)計(jì)算得到連桿1和連桿2的運(yùn)動(dòng)學(xué)方程為

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

于是可以得到各個(gè)連桿相對(duì)于慣性坐標(biāo)系的運(yùn)動(dòng)學(xué)方程為

(16)

(17)

(18)

2.2 動(dòng)力學(xué)模型

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

(19)

L=K-U

(20)

(21)

于是有

(22)

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

(23)

(24)

(25)

式中:M11(q)∈R3×3，M22(q)∈R3×3，M33(q)∈R2×2為正定對(duì)稱矩陣；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)的計(jì)算方式為

(28)

2.3 模型簡(jiǎn)化

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

(29)

(30)

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

(31)

3 控制器設(shè)計(jì)

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

(32)

(33)

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

(34)

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

(35)

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

(36)

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

(37)

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

(38)

(39)

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

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

(40)

定義滑模面為

(41)

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

(42)

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

k≥|d|

(43)

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

(44)

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

(45)

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

(46)

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

(47)

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

(48)

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

(49)

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

(50)

其中:

(51)

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

此外，由于存在sgn(s)這一不連續(xù)函數(shù)，閉環(huán)系統(tǒng)會(huì)出現(xiàn)抖振現(xiàn)象。為了消除系統(tǒng)的抖振現(xiàn)象，并且使得內(nèi)環(huán)控制器的期望連續(xù)可微，可以使用連續(xù)的函數(shù)來(lái)近似不連續(xù)的符號(hào)函數(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ù)值微分會(huì)嚴(yán)重受到噪聲影響，破壞系統(tǒng)的穩(wěn)定性。根據(jù)時(shí)標(biāo)分離原則，考慮內(nèi)環(huán)是一個(gè)快變系統(tǒng)，且其暫態(tài)過(guò)程對(duì)外環(huán)可以忽略，則可以將內(nèi)環(huán)的跟蹤當(dāng)作一個(gè)鎮(zhèn)定問(wèn)題，這樣一來(lái)，可以認(rèn)為期望姿態(tài)歐拉角的二階及以上導(dǎo)數(shù)為0，由此可以得到誤差

(52)

設(shè)計(jì)PID控制器為

(53)

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

(54)

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

(55)

4 仿真實(shí)驗(yàn)

本節(jié)介紹物理仿真實(shí)驗(yàn)結(jié)果。通過(guò)機(jī)器人物理仿真軟件v-rep[23]和MATLAB進(jìn)行仿真，和開(kāi)源飛控Pixhawk中采用的基于四元數(shù)的PID算法進(jìn)行對(duì)比。本文設(shè)計(jì)了3種仿真實(shí)驗(yàn)場(chǎng)景來(lái)測(cè)試所提方法在機(jī)械臂靜態(tài)和動(dòng)態(tài)擾動(dòng)環(huán)境下的鎮(zhèn)定和軌跡跟蹤性能。為了解決不連續(xù)滑模控制帶來(lái)的抖振問(wèn)題，在實(shí)現(xiàn)控制器時(shí)將符號(hào)函數(shù)sgn(s)替換為雙曲正切函數(shù)tanh(10s)。

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

圖5 實(shí)驗(yàn)1無(wú)人機(jī)位置Fig.5 Positions of UAV in experiment 1

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

圖6 實(shí)驗(yàn)2無(wú)人機(jī)位置Fig.6 Positions of UAV in experiment 2

圖7 實(shí)驗(yàn)2無(wú)人機(jī)姿態(tài)(Pixhawk方法結(jié)果)Fig.7 Orientations of UAV in experiment 2 (results of Pixhawk method)

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

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

圖9 實(shí)驗(yàn)3無(wú)人機(jī)位置Fig.9 Positions of UAV in experiment 3

圖10 實(shí)驗(yàn)3無(wú)人機(jī)姿態(tài)(Pixhawk方法結(jié)果)Fig.10 Orientations of UAV in experiment 3 (results of Pixhawk method)

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

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

圖12 實(shí)驗(yàn)4無(wú)人機(jī)位置Fig.12 Positions of UAV in experiment 4

圖13 實(shí)驗(yàn)4無(wú)人機(jī)位置誤差Fig.13 Position errors of UAV in experiment 4

圖14 實(shí)驗(yàn)4無(wú)人機(jī)姿態(tài)(Pixhawk方法結(jié)果)Fig.14 Orientations of UAV in experiment 4 (results of Pixhawk method)

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

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

5 結(jié) 論

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

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