四軸飛行器是微型飛行器的其中一種，相對(duì)于固定翼飛行器，它的方向控制靈活、抗干擾能力強(qiáng)、飛行穩(wěn)定，能夠攜帶一定的負(fù)載和有懸停功能，因此能夠很好地進(jìn)行空中拍攝、監(jiān)視、偵查等功能，在軍事和民用上具備廣泛的運(yùn)用前景。

四軸飛行器關(guān)鍵技術(shù)在于控制策略。由于智能控制算法在運(yùn)行復(fù)雜的浮點(diǎn)型運(yùn)算以及矩陣運(yùn)算時(shí)，微處理器計(jì)算能力受限，難以達(dá)到飛行控制實(shí)時(shí)性的要求;而PID控制簡(jiǎn)單，易于實(shí)現(xiàn)，且技術(shù)成熟，因此目前主流的控制策略主要是圍繞傳統(tǒng)的PID控制展開(kāi)。

1 四軸飛行器的結(jié)構(gòu)與基本飛行原理

四軸飛行器結(jié)構(gòu)主要由主控板和呈十字交叉結(jié)構(gòu)的4個(gè)電子調(diào)速器、電機(jī)、旋漿組成，電機(jī)由電子調(diào)速器控制，主控板主要負(fù)責(zé)解算當(dāng)前飛行姿態(tài)、控制電調(diào)等功能。

以十字飛行模式為例，l號(hào)旋翼為頭，1、3號(hào)旋翼逆時(shí)針旋轉(zhuǎn)，2、4號(hào)旋翼順時(shí)針旋轉(zhuǎn)，如圖1所示。

圖1 四軸飛行器結(jié)構(gòu)圖

參照飛行狀態(tài)表1變化電機(jī)轉(zhuǎn)速，由于四個(gè)電機(jī)轉(zhuǎn)速不同，使其與水平面傾斜一定角度，如圖l所示。四個(gè)電機(jī)產(chǎn)生的合力分解為向上的升力與前向分力。當(dāng)重力與升力相等時(shí)，前向分力驅(qū)動(dòng)四軸飛行器向傾斜角度的方向水平飛行?？臻g三軸角度歐拉角分為仰俯角、橫滾角、航向角：傾斜角是仰俯角時(shí)，向前、向后飛行;傾斜角是橫滾角時(shí)，向左、向右飛行;而傾斜航向角時(shí)，向左、右旋轉(zhuǎn)運(yùn)動(dòng)，左(右)旋轉(zhuǎn)是由于順時(shí)針兩電機(jī)產(chǎn)生的反扭矩之和與逆時(shí)針兩電機(jī)產(chǎn)生的反扭矩之和不等，即不能相互抵消，機(jī)身便在反扭矩作用下繞z軸自旋轉(zhuǎn)。

2 姿態(tài)解算

四軸飛行器運(yùn)用姿態(tài)解算計(jì)算出空間三軸歐拉角。結(jié)構(gòu)框架如圖2所示，陀螺儀采樣三軸角速度值，加速度傳感器采樣三軸加速度值，而磁力傳感器采樣得到三軸地磁場(chǎng)值，將陀螺儀、加速度傳感器、磁力傳感器采樣后的數(shù)據(jù)進(jìn)行標(biāo)定、濾波、校正后得到三軸歐拉角度，其中陀螺儀和加速度傳感器選用MPU6050芯片，磁力傳感器選用HMC5883L芯片，采用IIC總線(xiàn)與主控板通信。

圖2 姿態(tài)解算結(jié)構(gòu)圖

由于傳感器存在器件誤差，因此在使用前需要標(biāo)定。陀螺儀在靜止時(shí)，角速度為0;但實(shí)際情況由于器件誤差并不為0，因此可在靜止時(shí)采樣500次數(shù)據(jù)，再求平均，得出偏移量，標(biāo)定陀螺儀數(shù)據(jù);加速度傳感器可依據(jù)在靜止時(shí)，三軸重力加速度平方和的開(kāi)方為重力加速度的標(biāo)定方程，利用最小二乘法求出標(biāo)定偏移值和誤差值進(jìn)行標(biāo)定。而磁力傳感器校正，可將器件靜置于桌面旋轉(zhuǎn)一周找出最小值和最大值，通過(guò)電子羅盤(pán)校正計(jì)算公式計(jì)算出標(biāo)定偏移值和誤差值。

由于陀螺儀長(zhǎng)時(shí)間采集角速度會(huì)有飄移，因此需要使用加速度傳感器的值與磁力傳感器的值進(jìn)行校正。將加速度的測(cè)量矢量和磁場(chǎng)的測(cè)量矢量與參考矢量做叉積后相加

式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測(cè)量矢量，mx、my、mz為磁場(chǎng)的測(cè)量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場(chǎng)的參考矢量，參考矢量是通過(guò)實(shí)時(shí)四元數(shù)值與本次測(cè)量值計(jì)算出來(lái)。

再將叉積修正角速度漂移值：

式(2)中ωx(t)、ωy(t)、ωz(t)為角速度，kpex(t)為比例項(xiàng)修正，

為積分修正項(xiàng)。

將校正后的角速度通過(guò)二階畢卡算法轉(zhuǎn)化為四元數(shù)，公式如式(3)。

3 高度計(jì)算

高度計(jì)算是通過(guò)氣壓傳感器采集的大氣壓值計(jì)算出來(lái)，將氣壓傳感器采集值進(jìn)行校正后，在通過(guò)溫度二階補(bǔ)償，得到準(zhǔn)確的大氣壓值，最后經(jīng)過(guò)氣壓轉(zhuǎn)換為高度公式

式(6)中Altitude為計(jì)算出來(lái)的實(shí)際高度，CurrentPressure為當(dāng)前氣壓值，StartPressure為起飛之前氣壓值。氣壓傳感器選用MS5611芯片，其中集成了溫度傳感器和氣壓傳感器，采用IIC總線(xiàn)與主控板通信。

4 PID控制

4.1 雙閉環(huán)PID控制

當(dāng)四軸飛行器正常飛行時(shí)，突遇外力(風(fēng)等)或磁場(chǎng)干擾，使加速度傳感器或磁力傳感器采集數(shù)據(jù)失真，造成姿態(tài)解算出來(lái)的歐拉角錯(cuò)誤，只用角度單環(huán)情況下，使系統(tǒng)很難穩(wěn)定運(yùn)行，因此可以加入角速度作為內(nèi)環(huán)，角速度由陀螺儀采集數(shù)據(jù)輸出，采集值一般不存在受外界影響情況，抗干擾能力強(qiáng)，并且角速度變化靈敏，當(dāng)受外界干擾時(shí)，回復(fù)迅速;同理，高度環(huán)中氣壓傳感器同樣也會(huì)受到外界干擾，引入z軸加速度環(huán)可有效避免外界干擾造成的影響，增強(qiáng)了系統(tǒng)的魯棒性。

四軸飛行器雙閉環(huán)PID控制，如圖3、圖4所示。角度作為外環(huán)，角速度作為內(nèi)環(huán)，進(jìn)行姿態(tài)PID控制;當(dāng)需要定高時(shí)，高度作為外環(huán)，z軸加速度作為內(nèi)環(huán)，進(jìn)行高度PID控制。其中，PID輸出為油門(mén)值，油門(mén)給定電子調(diào)速器值，電子調(diào)速器控制電機(jī)使空間三軸歐拉角和高度變化。

圖3姿態(tài)PID控制總體流程圖

圖4高度PID控制總體流程圖

PID控制算法采用位置式數(shù)字PID控制：

式(7)中u(t)為PID輸出值，e(t)為期望值與實(shí)際值之差，

為積分量，

為微分量，kp，、ki、kd。為比例、積分、微分系數(shù)。

在將積分量，微分量離散化得到PID計(jì)算公式

式(8)中T為更新時(shí)間。

基于公式(8)，姿態(tài)PID控制算法

式(9)為角度環(huán)PID計(jì)算公式，(10)為角速度環(huán)PID計(jì)算公式。AngelPIDOut(t)為角度環(huán)PID輸出，AngelRatePIDOut(t)為角速度環(huán)PID輸出，e(t)=期望角度一實(shí)際角度，e'(t)=AngelPIDOut(t) - 實(shí)際角速度。

同理高度PID控制算法：

式(11)為高度環(huán)PID計(jì)算公式，公式(12)為加速度環(huán)PID計(jì)算公式，AltitudePIDOut(t)為高度環(huán)PID輸出，AcceleratePIDOut(t)為加速度環(huán)PID輸出。e(t)=期望高度一實(shí)際高度，e'(t)=AltitudePIDOut(t) - (z軸加速度 - 重力加速度值)。

4.2 油門(mén)輸出計(jì)算

PID輸出值先經(jīng)過(guò)限幅處理，再輸出給油門(mén)，防止某些時(shí)刻輸出油門(mén)值過(guò)大，造成過(guò)沖，使系統(tǒng)難以穩(wěn)定，公式如下

AngelOut=AngelPIDOut(t)(限制范圍 - 100到100) (13)

AltitudeOut=AcceleratePIDOut(t)(限制范圍 - 10到10) (14)

式中AngelOut。為歐拉角輸出值，AltitudeOut為高度輸出值。

最后經(jīng)過(guò)十字飛行模式油門(mén)輸出公式，計(jì)算出4個(gè)電機(jī)輸出油門(mén)

式(15)中throttlelOut到throttle4Out為油門(mén)1到油門(mén)4輸出值，依據(jù)公式(13)輸出的AngelOut分為三軸角度：pitchOut為仰俯角輸出值，rollOut為橫滾角輸出值，yawOut為航向角輸出值。

4.3 油門(mén)補(bǔ)償

以十字飛行模式為例，PID控制條件為線(xiàn)性環(huán)境，而給出的油門(mén)值與轉(zhuǎn)速的關(guān)系為非線(xiàn)性，且兩對(duì)向電機(jī)再同一油門(mén)下轉(zhuǎn)速存在差異。運(yùn)用光電傳感器測(cè)出不同油門(mén)量對(duì)應(yīng)的轉(zhuǎn)速，通過(guò)matlab軟件繪制出該曲線(xiàn)，其中一對(duì)向電機(jī)油門(mén)與轉(zhuǎn)速的關(guān)系如圖5所示，兩電機(jī)在相同油門(mén)下轉(zhuǎn)速存在差異且不平行，因此將油門(mén)曲線(xiàn)分段，并通過(guò)計(jì)算公式

補(bǔ)償油門(mén)值 =α(實(shí)際油門(mén)值 - β) (16)

進(jìn)行油門(mén)補(bǔ)償，式(16)中α、β為補(bǔ)償偏移系數(shù)，得出曲線(xiàn)如圖5(b)所示。

圖5 油門(mén) - 轉(zhuǎn)速曲線(xiàn)圖

5 測(cè)試

本次測(cè)試姿態(tài)解算更新周期T ≈ 4 mS，PID控制更新周期T ≈ 10 ms，期望值為水平0度。將四軸飛行器控制仰俯角的一對(duì)電機(jī)固定住，另一對(duì)邊能自由旋轉(zhuǎn)，即能改變橫滾角度，如圖6所示。將橫滾角傾斜到 - 70°，遙控器油門(mén)迅速推到1400，平衡后如圖6所示。

圖6 姿態(tài)PID控制測(cè)試

將四軸飛行器采集的歐拉角值通過(guò)無(wú)線(xiàn)模塊NRF24L01發(fā)送到PC機(jī)上，接收的數(shù)據(jù)顯示到上位機(jī)進(jìn)行分析。上位機(jī)顯示橫滾角波形如圖7所示，波形圖橫坐標(biāo)單位為10 ms，縱坐標(biāo)單位為度。

圖 7雙閉環(huán)PID控制橫滾角波形圖

由圖7可知，橫滾角波形經(jīng)過(guò)一大波一小波震蕩后近似歸為期望值零點(diǎn)，且超調(diào)量較小，系統(tǒng)很快進(jìn)入穩(wěn)定狀態(tài)。

其他歐拉角測(cè)試結(jié)果類(lèi)似。

在加入雙閉環(huán)高度PID控制，四軸飛行器在室外飛行效果如圖8所示，可以看出飛行器飛行穩(wěn)定，達(dá)到設(shè)計(jì)要求。

圖8 室外飛行測(cè)試

6 結(jié)論

主要研究了基于四軸飛行器的雙閉環(huán)PID控制算法。在姿態(tài)PID控制和高度PID控制中分別增加了內(nèi)環(huán)角速度環(huán)和加速度環(huán)，不僅抗干擾能力強(qiáng)，并且回復(fù)迅速，增強(qiáng)了系統(tǒng)的魯棒性。