四旋翼飛行器懸停算法設(shè)計(jì)與實(shí)現(xiàn)

鄧博文，竇 強(qiáng)，龔國(guó)輝，鄧 林

(國(guó)防科技大學(xué) 計(jì)算機(jī)學(xué)院，湖南 長(zhǎng)沙 410073)

四旋翼飛行器與常規(guī)飛行器相比，具有機(jī)動(dòng)性高、控制靈活、可垂直起降、自由懸停等特點(diǎn)，近年來(lái)成為航空領(lǐng)域的研究熱點(diǎn)。早些年，國(guó)際上已有諸多科學(xué)機(jī)構(gòu)致力于四旋翼飛行器控制技術(shù)的研究，并取得良好的成果，具體代表有Draganflyer 公司、賓夕法尼亞大學(xué)的HMX4 實(shí)驗(yàn)室等。而國(guó)內(nèi)在這方面起步相對(duì)較晚［1］。

懸停功能作為四旋翼飛行器獨(dú)特的動(dòng)力特性之一，一直是研究的熱點(diǎn)及難點(diǎn)。本文通過(guò)分析四旋翼飛行器的受力，建立了其在懸停狀態(tài)下的動(dòng)力學(xué)模型，針對(duì)飛行器易受干擾和靈敏度高的特點(diǎn)利用經(jīng)典PID算法設(shè)計(jì)了位置控制回路。在Matlab/Simulink 平臺(tái)上，對(duì)模型的懸停模式進(jìn)行控制仿真，并做了大量現(xiàn)場(chǎng)飛行測(cè)試，測(cè)試效果良好。

1 數(shù)學(xué)建模

在對(duì)無(wú)人機(jī)飛行姿態(tài)進(jìn)行控制之前，建立一個(gè)合理、準(zhǔn)確的數(shù)學(xué)模型是必不可少的。該模型具有如下約束條件［2］:(1)地面坐標(biāo)系為慣性坐標(biāo)系，地球表面視為平面。(2)飛行器為剛體并且質(zhì)量分布均勻，重心位于機(jī)身中央。

人們已習(xí)慣將地面坐標(biāo)系作為衡量物體運(yùn)動(dòng)的標(biāo)準(zhǔn)，而飛行器受力分析大多是在其自身坐標(biāo)系中討論，因此必須建立機(jī)體坐標(biāo)系與地面坐標(biāo)系的轉(zhuǎn)換關(guān)系，將飛行器的狀態(tài)轉(zhuǎn)換到地面坐標(biāo)系中進(jìn)行分析。如圖1所示，建立機(jī)體坐標(biāo)系B(oxyz)及地面坐標(biāo)系A(chǔ)(OXYZ)。

圖1 地面坐標(biāo)系A(chǔ) 與機(jī)體坐標(biāo)系B

1.1 轉(zhuǎn)換矩陣

坐標(biāo)系B 到A 之間的變換矩陣R 可寫為如下形式［3］

其中，α，β，γ 分別是飛行器在地面坐標(biāo)系A(chǔ) 中沿OX(俯仰)，OY(橫滾)，OZ 軸(偏航)旋轉(zhuǎn)的角度。

1.2 外力平衡方程

在機(jī)體坐標(biāo)系B 中，單個(gè)螺旋槳的升力T 和水平轉(zhuǎn)矩τ 與轉(zhuǎn)速存在如下關(guān)系［4］

式中，KM為旋翼升力系數(shù);Kτ為旋翼反扭矩系數(shù);ω為旋翼轉(zhuǎn)速。

在機(jī)體坐標(biāo)系B 中，飛行器的升力方向永遠(yuǎn)豎直向上，結(jié)合式(2)，文中將其升力寫為

根據(jù)轉(zhuǎn)換矩陣R，將飛行器在地面坐標(biāo)系A(chǔ) 中的升力寫成

1.3 外力矩平衡方程

式(8)中，U2=T1+T4－T2－T3，U3=T1+T2－T3－T4，U4=τ1+τ3－τ2－τ4，lx，ly為飛行器繞x，y 軸旋轉(zhuǎn)的力臂長(zhǎng)度。根據(jù)飛行器結(jié)構(gòu)的對(duì)稱性，L 為飛行器單個(gè)動(dòng)力軸軸長(zhǎng)。

機(jī)體坐標(biāo)系下的角速度［p q r］T與地面坐標(biāo)系下的角速度［之間有如下關(guān)系［6］

由于當(dāng)角度足夠小時(shí)，正弦函數(shù)趨近與0，余弦函數(shù)趨近于1，因此將式(8)簡(jiǎn)化，可近似地將認(rèn)為

將式(9)的結(jié)論代入式(7)，再結(jié)合式(5)，得到四旋翼飛行器最終的運(yùn)動(dòng)學(xué)方程

2 控制器設(shè)計(jì)及仿真

2.1 控制過(guò)程

本文針對(duì)四旋翼飛行器的特殊動(dòng)力學(xué)結(jié)構(gòu)，分別將姿態(tài)、高度和水平位置分開來(lái)控制。

如圖2 所示，本文將期望得到的坐標(biāo)值(Ex，Ey，Ez)分別輸入到不同的運(yùn)算單元，計(jì)算出期望得到的三軸加速度與姿態(tài)角，輸入動(dòng)力單元后分別計(jì)算出各個(gè)電機(jī)的控制量，最后輸出飛行器模型，達(dá)到控制器飛行狀態(tài)的目的。

圖2 總體控制框架

2.2 PID 控制器

與其他控制方法相比，PID 控制具有結(jié)構(gòu)簡(jiǎn)單、易于實(shí)現(xiàn)、穩(wěn)定性好等特點(diǎn)。其控制器通過(guò)調(diào)節(jié)比例(P)、積分(I)、微分(D)系數(shù)可在滿足系統(tǒng)穩(wěn)定性的前提下達(dá)到快速、精確的控制效果［7－9］。鑒于PID 控制和跟蹤控制方式的優(yōu)勢(shì)，選用PID 控制器可以有效地對(duì)懸停這一過(guò)程進(jìn)行控制。

圖3 PID 控制器結(jié)構(gòu)

針對(duì)圖2 中的水平控制器模塊和高度控制模塊，利用圖3 所示結(jié)構(gòu)，通過(guò)目標(biāo)位置與當(dāng)前位置作差再進(jìn)行PID 運(yùn)算［10］，得到3 個(gè)運(yùn)動(dòng)方向的期望加速度，具體操作如下

式中，(xd，yd，zd)是目標(biāo)位置的三維坐標(biāo);(x，y，z)是當(dāng)前位置的三維坐標(biāo)。

2.3 仿真

本文重點(diǎn)是懸停功能的實(shí)現(xiàn)，而飛行器只能通過(guò)改變姿態(tài)角來(lái)產(chǎn)生水平方向的加速度。因此，這里重點(diǎn)取姿態(tài)控制模塊進(jìn)行分析。

圖5 ～圖7 分別是俯仰、橫滾、偏航的仿真圖，實(shí)線為期望值，虛線為實(shí)際值。當(dāng)給定輸入俯仰和橫滾期望角度時(shí)(5°)，可看出實(shí)際偏轉(zhuǎn)角度存在小幅抖動(dòng)。但當(dāng)滾轉(zhuǎn)角和俯仰角趨于穩(wěn)定時(shí)，偏航角迅速收斂回到原點(diǎn)，輸出波形的跟隨性較好。因此，基于簡(jiǎn)化模型設(shè)計(jì)的PID 控制器能夠達(dá)到較為滿意的結(jié)果。

圖4 Simulink 中建立的姿態(tài)控制模塊

圖5 俯仰角仿真波形

圖6 橫滾角仿真波形

圖7 偏航角仿真波形

3 現(xiàn)場(chǎng)飛行測(cè)試與結(jié)果分析

測(cè)試采用機(jī)型為軸長(zhǎng)0.204 m，中心對(duì)稱的X 型機(jī)架。整機(jī)重量為1.34 kg，繞x，y，z 三軸的轉(zhuǎn)動(dòng)慣量IxIyIz分別為1.968×10－3N·m·s2，1.968×10－3N·m·s2，3.245×10－3N·m·s2。升力系數(shù)KM=3.179×10－5N·s2·rad－2，反 扭 矩 系 數(shù) Kτ= 7.432 ×10－7N·s2·rad－2。測(cè)試采用激光測(cè)距儀記錄飛行數(shù)據(jù)，每次測(cè)試時(shí)長(zhǎng)設(shè)定為100 s，每隔1 s 記錄一次飛行器數(shù)據(jù)。其中一組數(shù)據(jù)如圖8 所示。

圖8 高度與水平測(cè)量結(jié)果

在現(xiàn)場(chǎng)飛行過(guò)程中，當(dāng)飛行器起飛至半空，開啟懸停功能后，其會(huì)在約2 m 的范圍內(nèi)漂移。造成該現(xiàn)象的原因是由于GPS 的精度較低，是無(wú)法控制算法彌補(bǔ)的。但觀察整個(gè)飛行過(guò)程，可明顯看出，飛行器在漂移達(dá)到一定距離時(shí)，有明顯的回向動(dòng)作來(lái)糾正這一誤差，使其飛行位置保持在一個(gè)有限的范圍內(nèi)。圖8(a)中，飛行器位置落在半徑2 m 圓范圍內(nèi)的概率分布達(dá)到91%，高度的漂移范圍維持在±1 m 以內(nèi)。

4 結(jié)束語(yǔ)

本文以四旋翼飛行器的懸停功能作為研究對(duì)象，分析四旋翼飛行器的整體受力情況，建立了近似的數(shù)學(xué)模型。利用PID 控制方法對(duì)飛行器懸停狀態(tài)下的姿態(tài)進(jìn)行控制，并做了大量現(xiàn)場(chǎng)飛行測(cè)試，測(cè)試結(jié)果為飛行器在水平范圍±2 m、垂直范圍±1 m 的空間范圍內(nèi)漂移，實(shí)現(xiàn)了懸停功能。

［1］ 陳國(guó)棟，賈培發(fā)，劉艷.微型飛行器的研究與發(fā)展［J］.機(jī)器人技術(shù)與應(yīng)用，2006(2):34－37.

［2］ 謝木生，王恒升，羅濤.捷聯(lián)式慣性導(dǎo)航仿真研究［M］.計(jì)算機(jī)仿真，2013(4):84－88.

［3］ 陳龍.小型四旋翼無(wú)人機(jī)的自動(dòng)控制系統(tǒng)設(shè)計(jì)［D］.成都:電子科技大學(xué)，2014.

［4］ 柴衛(wèi)華，侯蕓.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)算法研究［J］.現(xiàn)代防御技術(shù)，2001，29(4):43－45.

［5］ 周衍柏.理論力學(xué)教程［M］.北京:高等教育出版社，2009.

［6］ 方振華.傾轉(zhuǎn)四旋翼飛行器控制系統(tǒng)設(shè)計(jì)［D］.哈爾濱:哈爾濱工業(yè)大學(xué)，2012.

［7］ Karl J Astrom，Tore Hagglund.PID controllers－theory design and tuning［M］.2nd Edition.New York:Instrument Society of America，1995.

［8］ 李波波，賈秋玲.基于滑?？刂频乃男盹w行器控制器設(shè)計(jì)［J］.電子設(shè)計(jì)工程，2013，21(16):76－78，82.

［9］ 黃軍友.基于PID 控制的X 模式四旋翼飛行器研究［J］.電子設(shè)計(jì)工程，2014，22(2):127－129.

［10］鄢景華.自動(dòng)控制原理［M］.哈爾濱:哈爾濱工業(yè)大學(xué)出版社，2006.