趙世榮，孫運(yùn)強(qiáng)，石喜玲

（中北大學(xué)（山西）儀器與電子學(xué)院，儀器科學(xué)與動(dòng)態(tài)測(cè)試教育部重點(diǎn)實(shí)驗(yàn)室，太原 030051）

近年來(lái)由于新材料（碳纖維等）、新型微機(jī)電（MEMS）傳感器、微型慣性導(dǎo)航控制器（MIMU）以及計(jì)算機(jī)自動(dòng)控制算法的不斷發(fā)展和進(jìn)步，以四旋翼飛行器為代表的多旋翼飛行器得以快速發(fā)展。當(dāng)前世界上四旋翼飛行器的研究熱點(diǎn)是飛行器的自主飛行器及執(zhí)行任務(wù)，這對(duì)飛行器的穩(wěn)定起降及懸停提出很高要求。

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

無(wú)跡卡爾曼濾波簡(jiǎn)寫(xiě)為UKF（Unscented Kalman Filter）是一種新型的濾波估計(jì)算法。UKF以UT變換為基礎(chǔ)，摒棄了對(duì)非線(xiàn)性函數(shù)進(jìn)行線(xiàn)性化的傳統(tǒng)做法，采用卡爾曼線(xiàn)性濾波框架，對(duì)于一步預(yù)測(cè)方程，使用無(wú)跡（UT）變換來(lái)處理均值和協(xié)方差的非線(xiàn)性傳遞，就成為UKF算法。UKF是對(duì)非線(xiàn)性函數(shù)的概率密度分布進(jìn)行近似，用一系列確定樣本來(lái)逼近狀態(tài)的后驗(yàn)概率密度，而不是對(duì)非線(xiàn)性函數(shù)進(jìn)行近似，不需要求導(dǎo)計(jì)算Jacobian矩陣。UKF沒(méi)有線(xiàn)性化忽略高階項(xiàng)，因此非線(xiàn)性分布統(tǒng)計(jì)量的計(jì)算精度較高。基于上述優(yōu)點(diǎn)，UKF被廣泛應(yīng)用于導(dǎo)航、目標(biāo)跟蹤、信號(hào)處理和神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)等多個(gè)領(lǐng)域［4］。

將三維空間中的目標(biāo)和場(chǎng)景對(duì)應(yīng)于二維圖像平面運(yùn)動(dòng)時(shí)，他們?cè)诙S圖像平面的投影就形成了運(yùn)動(dòng)，這種運(yùn)動(dòng)以圖像平面亮度模式表現(xiàn)出來(lái)的流動(dòng)就稱(chēng)為光流。光流法是對(duì)運(yùn)動(dòng)序列圖像進(jìn)行分析的一個(gè)重要方法，光流不僅包含圖像中目標(biāo)的運(yùn)動(dòng)信息，而且包含了三維物理結(jié)構(gòu)的豐富信息，因此可用來(lái)確定目標(biāo)的運(yùn)動(dòng)情況以及反映圖像其他等信息。目前，已經(jīng)研制出了光流傳感器，具有高感光度、高像素、高數(shù)據(jù)更新速度，并且能夠通過(guò)地面站軟件實(shí)時(shí)獲取數(shù)據(jù)等優(yōu)點(diǎn)，在低空下可以通過(guò)它較準(zhǔn)確的測(cè)得飛行器相對(duì)地面的運(yùn)動(dòng)速度。

圖1 四旋翼飛行器的動(dòng)力學(xué)模型

1 四旋翼飛行器的力學(xué)模型及其簡(jiǎn)化

四旋翼飛行器的空間自由度為6（沿3個(gè)坐標(biāo)軸作平移和旋轉(zhuǎn)動(dòng)作），這6個(gè)自由度的控制是通過(guò)調(diào)節(jié)不同電機(jī)的轉(zhuǎn)速進(jìn)而改變4個(gè)旋翼的升力而實(shí)現(xiàn)的，其基本的運(yùn)動(dòng)狀態(tài)包括垂直運(yùn)動(dòng)、前后運(yùn)動(dòng)、側(cè)向運(yùn)動(dòng)、滾轉(zhuǎn)運(yùn)動(dòng)、俯仰運(yùn)動(dòng)、偏航運(yùn)動(dòng)。

其動(dòng)力學(xué)公式為如下：

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

2 多傳感器的布置及其簡(jiǎn)化模型

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

圖2 光流傳感器高度不同

飛行器的側(cè)傾和俯仰的變化也將導(dǎo)致傳感器返回的X和Y值的變化。與橫向運(yùn)動(dòng)的計(jì)算不同，這些都不依賴(lài)于可見(jiàn)物體的距離。在下面的圖片中，可以看到的四軸已經(jīng)側(cè)傾了10°，但第1張圖中視角中心的兩個(gè)花都移到了第2張圖片的視角邊緣。如圖3所示。

圖3 光流傳感器傾斜

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

我們得到的x，y軸的運(yùn)動(dòng)后，就可以與當(dāng)前的偏航相結(jié)合結(jié)合，用于估計(jì)位置。

需要注意的是，傳感器只能在光線(xiàn)好的環(huán)境下工作；因?yàn)槭褂枚ń圭R頭，所以不能對(duì)近于30 cm的物體對(duì)焦，同時(shí)旋轉(zhuǎn)傳感器將使傳感器混亂［6］。

超聲波發(fā)送端發(fā)射超聲波，當(dāng)超聲波遇到物體后被反射回來(lái)，接收傳感器接收超聲波。渡越時(shí)間法就是通過(guò)測(cè)量超聲波從發(fā)射到接收的時(shí)間差t，從而求出待測(cè)距離L，計(jì)算公式為：

式中，L為待測(cè)距離，v為聲速，t是測(cè)得的時(shí)間。

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

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

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

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

圖4 穩(wěn)定懸停時(shí)濾波效果

圖5 高度變換時(shí)濾波效果

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

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

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

圖7 X，Y運(yùn)動(dòng)加速度濾波效果

3.3 基于無(wú)跡卡爾曼的姿態(tài)解算

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

通過(guò)模擬仿真得到，經(jīng)過(guò)融合濾波后的姿態(tài)角與理想值比較接近，偏差在0.2°以?xún)?nèi)，滿(mǎn)足飛行器的穩(wěn)定性要求。

圖8 三軸姿態(tài)角UKF濾波效果

4 研究結(jié)果

本次實(shí)驗(yàn)搭建了一個(gè)基于STM32F427的四旋翼飛行器，通過(guò)加入上述改進(jìn)算法實(shí)現(xiàn)了平穩(wěn)性飛行。

但是實(shí)際進(jìn)行懸停及起降時(shí)發(fā)現(xiàn)它的穩(wěn)定性沒(méi)有達(dá)到理想效果，考慮可能問(wèn)題是多傳感器信息融合在硬件平臺(tái)實(shí)際應(yīng)用中沒(méi)有達(dá)到應(yīng)有效果；同時(shí)控制算法的不完善也影響飛行器的穩(wěn)定性。因此，改進(jìn)信息融合算法在硬件平臺(tái)上的運(yùn)行效率及控制算法是下一步的重點(diǎn)工作。

［1］Di L，F(xiàn)romm 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］郝曉靜，李國(guó)新，李明珠，等.無(wú)跡卡爾曼濾波算法在目標(biāo)跟蹤中的研究［J］.電子設(shè)計(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］劉小明，陳萬(wàn)春，邢曉嵐，等.光流/慣導(dǎo)多傳感器信息融合方法［J］.北京航空航天大學(xué)學(xué)報(bào)，2012（5）：620-624.

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

［8］呂強(qiáng)，倪佩佩，王國(guó)勝，等.基于光流傳感器的四旋翼飛行器懸停校正［J］.裝甲兵工程學(xué)院學(xué)報(bào)，2014（3）：68-72.

趙世榮（1988-）男，漢族，山西孝義人，碩士研究生，研究方向?yàn)槎嘈盹w行器設(shè)計(jì)，550158357@qq.com；

孫運(yùn)強(qiáng)（1963-），男，漢族，中北大學(xué)教授，syq@nuc.edu.cn。