一種兩自由度飛行機械臂系統的設計與實現

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

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

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

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

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

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

1 系統設計

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

圖1 飛行機械臂系統Fig.1 Aerial manipulation system

為了能夠實現抓取任務，避免機構過于復雜，確保系統重量適中，采用2自由度機械臂的pitch-pitch配置。使用ROBOTISE公司的Dynamixel XH430-W350R總線舵機直接驅動。機械臂的各個連接部件采用3D打印的方式加工。機械臂的硬件驅動系統如圖2所示。

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

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

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

2 系統模型

本節將首先介紹飛行機械臂系統的運動學模型，在此基礎上根據歐拉-拉格朗日方程構建系統完整的動力學模型，最后根據特定假設對動力學模型進行簡化。

2.1 運動學模型

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

圖4 飛行機械臂系統示意圖Fig.4 Illustration of aerial manipulation system

(1)

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

(2)

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

(3)

(4)

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

(5)

(6)

表1 坐標系相互關系Table 1 Relationships between coordinates

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

(7)

經過計算得到連桿1和連桿2的運動學方程為

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

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

(16)

(17)

(18)

2.2 動力學模型

下面使用歐拉-拉格朗日法建立系統完整的動力學模型:

(19)

L=K-U

(20)

(21)

于是有

(22)

無人機旋翼產生的升力大小為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)

因而可以得到無人機子系統模型

(31)

3 控制器設計

飛行機械臂系統是一個高度非線性的系統，機械臂的構型和運動增加了整個系統的復雜度，使得對于系統的整體控制非常困難。由于無人機內環動態顯著快于外環，因此可以針對無人機內環與外環分別設計控制器。

(32)

(33)

從而可得新的系統為

(34)

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

(35)

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

(36)

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

(37)

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

(38)

(39)

3.1 外環滑模控制器

對外環子系統設計滑模控制器[21]。無人機質心的位置期望軌跡為pd，用戶給定偏航角為ψr，則外環位置和速度跟蹤誤差為

(40)

定義滑模面為

(41)

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

(42)

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

k≥|d|

(43)

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

(44)

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

(45)

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

(46)

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

(47)

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

(48)

由此可以得到期望姿態的旋轉矩陣為

(49)

進而可以得到期望姿態對應的歐拉角為

(50)

其中:

(51)

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

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

3.2 內環控制器

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

(52)

設計PID控制器為

(53)

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

(54)

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

(55)

4 仿真實驗

本節介紹物理仿真實驗結果。通過機器人物理仿真軟件v-rep[23]和MATLAB進行仿真，和開源飛控Pixhawk中采用的基于四元數的PID算法進行對比。本文設計了3種仿真實驗場景來測試所提方法在機械臂靜態和動態擾動環境下的鎮定和軌跡跟蹤性能。為了解決不連續滑模控制帶來的抖振問題，在實現控制器時將符號函數sgn(s)替換為雙曲正切函數tanh(10s)。

實驗1機械臂靜態偏置下的鎮定實驗。設置機械臂靜態偏置為η1=60°,η2=30°。由于機械臂的偏置，系統的質心偏離機體坐標系原點。實驗結果如圖5所示。

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

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

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

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

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

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

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

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

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

實驗4機械臂周期性擺動下的S形曲線軌跡跟蹤實驗。機械臂在2個關節位形之間(η1=60°,η2=30°和η1=-60°,η2=-30°)做周期性擺動，同時無人機跟蹤S形曲線。實驗結果如圖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無人機姿態(Pixhawk方法結果)Fig.14 Orientations of UAV in experiment 4 (results of Pixhawk method)

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

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

5 結 論

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

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