基于信息融合四旋翼飛行器穩定性改進設計

趙世榮，孫運強，石喜玲

（中北大學（山西）儀器與電子學院，儀器科學與動態測試教育部重點實驗室，太原 030051）

近年來由于新材料（碳纖維等）、新型微機電（MEMS）傳感器、微型慣性導航控制器（MIMU）以及計算機自動控制算法的不斷發展和進步，以四旋翼飛行器為代表的多旋翼飛行器得以快速發展。當前世界上四旋翼飛行器的研究熱點是飛行器的自主飛行器及執行任務，這對飛行器的穩定起降及懸停提出很高要求。

多傳感器信息融合是充分利用不同時間與空間的多傳感器數據資源，采用計算機技術對按時間序列獲得的多傳感器觀測數據，在一定準則下進行分析、綜合、支配和使用，獲得對被測對象的一致性解釋與描述，進而實現相應的決策和估計，使系統獲得比它的各組成部分更充分的信息［1］。由于四旋翼飛行器采用的MEMS傳感器的精度、漂移及環境等的影響，單個傳感器測得的值可靠性差，影響飛行器的穩定性起降及懸停，所以需要飛行器多傳感器采集到的數據的實時融合［2-3］。

無跡卡爾曼濾波簡寫為UKF（Unscented Kalman Filter）是一種新型的濾波估計算法。UKF以UT變換為基礎，摒棄了對非線性函數進行線性化的傳統做法，采用卡爾曼線性濾波框架，對于一步預測方程，使用無跡（UT）變換來處理均值和協方差的非線性傳遞，就成為UKF算法。UKF是對非線性函數的概率密度分布進行近似，用一系列確定樣本來逼近狀態的后驗概率密度，而不是對非線性函數進行近似，不需要求導計算Jacobian矩陣。UKF沒有線性化忽略高階項，因此非線性分布統計量的計算精度較高。基于上述優點，UKF被廣泛應用于導航、目標跟蹤、信號處理和神經網絡學習等多個領域［4］。

將三維空間中的目標和場景對應于二維圖像平面運動時，他們在二維圖像平面的投影就形成了運動，這種運動以圖像平面亮度模式表現出來的流動就稱為光流。光流法是對運動序列圖像進行分析的一個重要方法，光流不僅包含圖像中目標的運動信息，而且包含了三維物理結構的豐富信息，因此可用來確定目標的運動情況以及反映圖像其他等信息。目前，已經研制出了光流傳感器，具有高感光度、高像素、高數據更新速度，并且能夠通過地面站軟件實時獲取數據等優點，在低空下可以通過它較準確的測得飛行器相對地面的運動速度。

圖1 四旋翼飛行器的動力學模型

1 四旋翼飛行器的力學模型及其簡化

四旋翼飛行器的空間自由度為6（沿3個坐標軸作平移和旋轉動作），這6個自由度的控制是通過調節不同電機的轉速進而改變4個旋翼的升力而實現的，其基本的運動狀態包括垂直運動、前后運動、側向運動、滾轉運動、俯仰運動、偏航運動。

其動力學公式為如下：

其中，U1為垂直速度控制量，U2為翻滾輸入控制量，U3為俯仰控制量，U4為偏航控制量。

2 多傳感器的布置及其簡化模型

光流技術是一種廣泛用于視覺處理而不需要精確地跟蹤特征點就能實現運動檢測的仿生技術。這里選用了PX4FLOW光流傳感器，PX4Flow是一款智能光學流動傳感器。傳感器擁有原生752×480像素分辨率，計算光學流的過程中采用了4倍分級和剪裁算法，計算速度達到250 Hz（白天，室外），具備非常高的感光度［5］。與其他滑鼠傳感器不同，它可以以120 Hz（黑暗，室內）的計算速度在室內或者室外暗光環境下工作，而無需照明LED。也可以對它重新編程，用于執行其他基礎的，高效率的低等級機器視覺任務。這里將它安裝在四旋翼飛行器的下方，用以檢測飛行器相對地面的運動狀況。為了把傳感器輸出值轉換成實際運動的距離，我們需要考慮高度。從下面的兩張圖中可以看到，如果兩個四軸運動了同樣距離，但是一個低，一個高，低的一個會看到表面特征運動得更遠，所以光流數值會更大，測得更遠，所以光流數值會更大。如圖2所示。

圖2 光流傳感器高度不同

飛行器的側傾和俯仰的變化也將導致傳感器返回的X和Y值的變化。與橫向運動的計算不同，這些都不依賴于可見物體的距離。在下面的圖片中，可以看到的四軸已經側傾了10°，但第1張圖中視角中心的兩個花都移到了第2張圖片的視角邊緣。如圖3所示。

圖3 光流傳感器傾斜

傳感器值預期的變化可以根據下面的公式用側傾和俯仰的變化算出。我們把這些預期的變化從傳感器返回的實際值中減去。

我們得到的x，y軸的運動后，就可以與當前的偏航相結合結合，用于估計位置。

需要注意的是，傳感器只能在光線好的環境下工作；因為使用定焦鏡頭，所以不能對近于30 cm的物體對焦，同時旋轉傳感器將使傳感器混亂［6］。

超聲波發送端發射超聲波，當超聲波遇到物體后被反射回來，接收傳感器接收超聲波。渡越時間法就是通過測量超聲波從發射到接收的時間差t，從而求出待測距離L，計算公式為：

式中，L為待測距離，v為聲速，t是測得的時間。

飛行器的高度由氣壓計和超聲波進行檢測。MS5611-01BA氣壓傳感器是由MEAS（瑞士）推出的一款SPI和I2C總線接口的新一代高分辨率氣壓傳感器，分辨率可達到10 cm。該傳感器模塊包括1個高線性度的壓力傳感器和一個超低功耗的24 bit Σ模數轉換器。MS5611提供了一個精確的24 bit數字壓力值和溫度值以及不同的操作模式，可以提高轉換速度并優化電流消耗。通過搭建測試平臺可得到它的靜態測量波動在±0.1 m以內，動態測量誤差為大約0.2 m～0.3 m，可以滿足飛行器在高空中飛行的需求，但穩定起降及低空飛行精度相對差不滿足要求。超聲波是一種頻率在40 kHz左右的聲波，其在大氣中的傳播速度與大氣溫度、濕度、壓力有密切的關系。從超聲波測距原理可知，超聲波傳播速度的準確性直接影響到超聲波測距模塊的距離測量精度，因此對超聲波測距模塊進行溫度補償、濕度補償以及壓力補償，可以有效地提高超聲波測距模塊測距精度。利用超聲波傳感器搭建的測量高度系統在10 m內的測量偏差在0.01 m內，精度遠高于氣壓計。

3 基于卡爾曼濾波的信息融合

3.1 基于卡爾曼的飛行器高度

圖4、圖5分別為飛行器在懸停和運動時超聲波和氣壓計測量值的濾波效果；可以看到，由于氣壓計的測量偏差相對超聲波較大，經過卡爾曼濾波處理后偏離理想值仍然相對較大；而超聲波測量值經過濾波處理后與理想值很接近，誤差很小而且不隨時間推移明顯增大，實時性很高。

圖4 穩定懸停時濾波效果

圖5 高度變換時濾波效果

3.2 基于卡爾曼的飛行器速度

在低空且光線滿足的情況下，利用光流法得到的運動位移偏移在0.06 m內，運動速度偏移在0.03 m/s內。圖6為X，Y軸光流傳感器測得位移的濾波效果，可以看出濾波后的位移和理想值的接近程度很好，誤差較小同時沒有明顯累積，相比加速度計測量效果好；圖7為X，Y軸的速度的濾波效果。

圖6 X，Y軸速度濾波效果

圖7 X，Y運動加速度濾波效果

3.3 基于無跡卡爾曼的姿態解算

設飛行器的初始姿態角分別為1°，2°，3°，陀螺儀的白噪聲為0.5°/h，陀螺儀的輸出采樣頻率為100 Hz，初始姿態角加速度都為1°，通過UKF濾波仿真，效果如圖8所示。

通過模擬仿真得到，經過融合濾波后的姿態角與理想值比較接近，偏差在0.2°以內，滿足飛行器的穩定性要求。

圖8 三軸姿態角UKF濾波效果

4 研究結果

本次實驗搭建了一個基于STM32F427的四旋翼飛行器，通過加入上述改進算法實現了平穩性飛行。

但是實際進行懸停及起降時發現它的穩定性沒有達到理想效果，考慮可能問題是多傳感器信息融合在硬件平臺實際應用中沒有達到應有效果；同時控制算法的不完善也影響飛行器的穩定性。因此，改進信息融合算法在硬件平臺上的運行效率及控制算法是下一步的重點工作。

［1］Di L，Fromm T，Chen Y Q，et al.A Data Fusion System for Attitude Estimation of Low-Cost Miniature UAVs［J］.Journal of Intelligent and Robotic Systems，201265（1-4）：621-635.

［2］Schwarz S，Sjostrom M，Olsson R，et al.A Weighted Optimization Approach to Time-of-Flight Sensor Fusion［J］.IEEE Transactions on Image Processing，2014，23（1）：214-225.

［3］Xi C.The Application of Measuring Robot Based on the Measure?ment Dynamic Measurement Technology at High-Pressure Line Pa?trol.2012，IEEE：492-495.

［4］郝曉靜，李國新，李明珠，等.無跡卡爾曼濾波算法在目標跟蹤中的研究［J］.電子設計工程，2012（13）：161-164.

［5］Baker S，Szeliski R，Scharstein D，et al，A Database and Evalua?tion Methodology for Optical Flow［J］.International Journal of Computer Vision，2011，92（1）：1-31.

［6］劉小明，陳萬春，邢曉嵐，等.光流/慣導多傳感器信息融合方法［J］.北京航空航天大學學報，2012（5）：620-624.

［7］陳航科，張東升，盛曉超，等.四旋翼飛行器懸停狀態姿態控制建模與仿真［J］.計算機仿真，2013（11）：41-45.

［8］呂強，倪佩佩，王國勝，等.基于光流傳感器的四旋翼飛行器懸停校正［J］.裝甲兵工程學院學報，2014（3）：68-72.

趙世榮（1988-）男，漢族，山西孝義人，碩士研究生，研究方向為多旋翼飛行器設計，550158357@qq.com；

孫運強（1963-），男，漢族，中北大學教授，syq@nuc.edu.cn。