趙世榮,孫運(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é)模型
四旋翼飛行器的空間自由度為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為偏航控制量。
光流技術(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ì)。
圖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í)濾波效果
在低空且光線(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)加速度濾波效果
設(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濾波效果
本次實(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。