四軸飛行器 是微型飛行器的其中一種，相對于固定翼飛行器，它的方向控制靈活、抗幹擾能力強、飛行穩定，能夠攜帶一定的負載和有懸停功能，是以能夠很好地進行空中拍攝、監視、偵查等功能，在軍事和民用上具備廣泛的運用前景。
四軸飛行器關鍵技術在于控制政策。由于智能控制算法在運作複雜的浮點型運算以及矩陣運算時，微處理器計算能力受限，難以達到飛行控制實時性的要求；而PID控制簡單，易于實作，且技術成熟，是以目前主流的控制政策主要是圍繞傳統的PID控制展開。
1 四軸飛行器的結構與基本飛行原理
四軸飛行器結構主要由主要闆和呈十字交叉結構的4個電子調速器、電機、旋漿組成，電機由電子調速器控制，主要闆主要負責解算目前飛行姿态、控制電調等功能。
以十字飛行模式為例，l号旋翼為頭，1、3号旋翼逆時針旋轉，2、4号旋翼順時針旋轉，如圖1所示。
圖1 四軸飛行器結構圖
參照飛行狀态表1變化電機轉速，由于四個電機轉速不同，使其與水準面傾斜一定角度，如圖l所示。四個電機産生的合力分解為向上的升力與前向分力。當重力與升力相等時，前向分力驅動四軸飛行器向傾斜角度的方向水準飛行。空間三軸角度歐拉角分為仰俯角、橫滾角、航向角：傾斜角是仰俯角時，向前、向後飛行；傾斜角是橫滾角時，向左、向右飛行；而傾斜航向角時，向左、右旋轉運動，左(右)旋轉是由于順時針兩電機産生的反扭矩之和與逆時針兩電機産生的反扭矩之和不等，即不能互相抵消，機身便在反扭矩作用下繞z軸自旋轉。
2 姿态解算
四軸飛行器運用姿态解算計算出空間三軸歐拉角。結構架構如圖2所示，陀螺儀采樣三軸角速度值，加速度傳感器采樣三軸加速度值，而磁力傳感器采樣得到三軸地磁場值，将陀螺儀、加速度傳感器、磁力傳感器采樣後的資料進行标定、濾波、校正後得到三軸歐拉角度，其中陀螺儀和加速度傳感器選用MPU6050晶片，磁力傳感器選用HMC5883L晶片，采用IIC總線與主要闆通信。
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量矢量，mx、my、mz為磁場的測量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場的參考矢量，參考矢量是通過實時四元數值與本次測量值計算出來。
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量矢量，mx、my、mz為磁場的測量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場的參考矢量，參考矢量是通過實時四元數值與本次測量值計算出來。
再将叉積修正角速度漂移值：
式(2)中ωx(t)、ωy(t)、ωz(t)為角速度，kpex(t)為比例項修正，
為積分修正項。
将校正後的角速度通過二階畢卡算法轉化為四元數，公式如式(3)。
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量矢量，mx、my、mz為磁場的測量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場的參考矢量，參考矢量是通過實時四元數值與本次測量值計算出來。
3 高度計算
高度計算是通過氣壓傳感器采集的大氣壓值計算出來，将氣壓傳感器采集值進行校正後，在通過溫度二階補償，得到準确的大氣壓值，最後經過氣壓轉換為高度公式
式(6)中Altitude為計算出來的實際高度，CurrentPressure為目前氣壓值，StartPressure為起飛之前氣壓值。氣壓傳感器選用MS5611晶片，其中內建了溫度傳感器和氣壓傳感器，采用IIC總線與主要闆通信。
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量矢量，mx、my、mz為磁場的測量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場的參考矢量，參考矢量是通過實時四元數值與本次測量值計算出來。
4 PID控制
4．1 雙閉環PID控制
當四軸飛行器正常飛行時，突遇外力(風等)或磁場幹擾，使加速度傳感器或磁力傳感器采集資料失真，造成姿态解算出來的歐拉角錯誤，隻用角度單環情況下，使系統很難穩定運作，是以可以加入角速度作為内環，角速度由陀螺儀采集資料輸出，采集值一般不存在受外界影響情況，抗幹擾能力強，并且角速度變化靈敏，當受外界幹擾時，回複迅速；同理，高度環中氣壓傳感器同樣也會受到外界幹擾，引入z軸加速度環可有效避免外界幹擾造成的影響，增強了系統的魯棒性。
四軸飛行器雙閉環PID控制，如圖3、圖4所示。角度作為外環，角速度作為内環，進行姿态PID控制；當需要定高時，高度作為外環，z軸加速度作為内環，進行高度PID控制。其中，PID輸出為油門值，油門給定電子調速器值，電子調速器控制電機使空間三軸歐拉角和高度變化
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量矢量，mx、my、mz為磁場的測量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場的參考矢量，參考矢量是通過實時四元數值與本次測量值計算出來。
圖3姿态PID控制總體流程圖
圖4高度PID控制總體流程圖
PID控制算法采用位置式數字PID控制：
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量矢量，mx、my、mz為磁場的測量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場的參考矢量，參考矢量是通過實時四元數值與本次測量值計算出來。
圖3姿态PID控制總體流程圖
圖4高度PID控制總體流程圖
PID控制算法采用位置式數字PID控制：
式(7)中u(t)為PID輸出值，e(t)為期望值與實際值之差，
為積分量，
為微分量，kp，、ki、kd。為比例、積分、微分系數。
在将積分量，微分量離散化得到PID計算公式
式(8)中T為更新時間。
基于公式(8)，姿态PID控制算法
圖2 姿态解算結構圖
由于傳感器存在器件誤差，是以在使用前需要标定。陀螺儀在靜止時，角速度為0；但實際情況由于器件誤差并不為0，是以可在靜止時采樣500次資料，再求平均，得出偏移量，标定陀螺儀資料；加速度傳感器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的标定方程，利用最小二乘法求出标定偏移值和誤內插補點進行标定。而磁力傳感器校正，可将器件靜置于桌面旋轉一周找出最小值和最大值，通過電子羅盤校正計算公式計算出标定偏移值和誤內插補點。
由于陀螺儀長時間采集角速度會有飄移，是以需要使用加速度傳感器的值與磁力傳感器的值進行校正。将加速度的測量矢量和磁場的測量矢量與參考矢量做叉積後相加
式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量矢量，mx、my、mz為磁場的測量矢量，axref、ayref、azref為加速度的參考矢量，mxref、myref、mzref為磁場的參考矢量，參考矢量是通過實時四元數值與本次測量值計算出來。
圖3姿态PID控制總體流程圖
圖4高度PID控制總體流程圖
PID控制算法采用位置式數字PID控制：
式(7)中u(t)為PID輸出值，e(t)為期望值與實際值之差，
為積分量，
為微分量，kp，、ki、kd。為比例、積分、微分系數。
在将積分量，微分量離散化得到PID計算公式
式(8)中T為更新時間。
基于公式(8)，姿态PID控制算法