文獻(xiàn)標(biāo)識碼: A
文章編號: 0258-7998(2015)05-0167-04
中文引用格式:黃建,胡越黎,楊文榮,冉峰.組合導(dǎo)航系統(tǒng)在四旋翼無人機(jī)上的實現(xiàn)[J].電子技術(shù)應(yīng)用,2015,41(05):167-170.
0 引言
近年來,隨著智能化產(chǎn)業(yè)的迅速發(fā)展,無人機(jī)行業(yè)逐漸走進(jìn)人們的視線。由于四旋翼無人機(jī)的結(jié)構(gòu)簡單可靠,具有垂直起降、定點懸停、穩(wěn)定低速飛行和在小空間內(nèi)飛行的能力[1],所以受到廣泛的關(guān)注并在多個領(lǐng)域得到了廣泛的應(yīng)用[2]。
在四旋翼無人機(jī)系統(tǒng)發(fā)展所面臨的關(guān)鍵技術(shù)中,導(dǎo)航系統(tǒng)是其重要的組成部分之一,它承擔(dān)著提供給飛行器位置、速度、姿態(tài)等參數(shù)狀態(tài)數(shù)據(jù)的任務(wù)。目前常用的導(dǎo)航方法有:慣性導(dǎo)航、衛(wèi)星導(dǎo)航、視覺導(dǎo)航以及它們的組合導(dǎo)航等。由于單一導(dǎo)航系統(tǒng)難以滿足無人飛行器的發(fā)展要求[3],于是產(chǎn)生了把兩種或兩種以上的不同導(dǎo)航系統(tǒng)以適當(dāng)?shù)姆绞浇M合在一起的組合導(dǎo)航,比單獨使用任一導(dǎo)航系統(tǒng)時具有更高的系統(tǒng)性能[4]。本文是利用擴(kuò)展卡爾曼濾波器將慣性導(dǎo)航系統(tǒng)和GPS導(dǎo)航系統(tǒng)的組合來完成導(dǎo)航任務(wù),建立了系統(tǒng)模型并將其在四旋翼無人飛行器上應(yīng)用實現(xiàn)。
1 慣性導(dǎo)航系統(tǒng)
1.1 慣性傳感器誤差分析
影響導(dǎo)航系統(tǒng)工作精度的主要因素之一是慣性傳感器誤差的存在,為了減少傳感器誤差,需要先對傳感器進(jìn)行標(biāo)定和誤差補(bǔ)償來保證導(dǎo)航系統(tǒng)的高精度和高性能。
1.2 慣性導(dǎo)航系統(tǒng)姿態(tài)矩陣計算
在導(dǎo)航系統(tǒng)的解算過程中,首先要處理的是坐標(biāo)系的標(biāo)定,通常有慣性坐標(biāo)系(通常表示為i系) 、地理坐標(biāo)系(通常表示為g系)、導(dǎo)航坐標(biāo)系(通常表示為n系)、載體坐標(biāo)系(通常表示為b系)等。本文在分析導(dǎo)航系統(tǒng)問題時是將導(dǎo)航坐標(biāo)系設(shè)定為地理坐標(biāo)系,地理坐標(biāo)系采用x指東向、y指北向、z指天向的形式,慣導(dǎo)系統(tǒng)的原理方程可以用圖1表示。
在導(dǎo)航系統(tǒng)中,最重要的計算就是姿態(tài)矩陣的求解,而常見的姿態(tài)矩陣算法主要有四元數(shù)法、歐拉角法、方向余弦法3種[5]。歐拉角法計算過程中方程式出現(xiàn)退化現(xiàn)象,方向余弦法通常計算量很大,因此經(jīng)常采用的是四元數(shù)法求解姿態(tài)矩陣。但四元數(shù)法存在不可交換誤差,為減小這種誤差,本文采用的是Bortz在1971年提出的等效旋轉(zhuǎn)矢量算法[6]。
1.2.1 旋轉(zhuǎn)矢量與姿態(tài)四元數(shù)的關(guān)系
設(shè)Q(t+h)和Q(t)分別為飛行器載體在t+h時刻和t時刻的姿態(tài)四元數(shù),則:
1.2.3 旋轉(zhuǎn)矢量的求解
通常,對于旋轉(zhuǎn)矢量采用泰勒級數(shù)展開法來求解。若采用直線擬合角速度,等效旋轉(zhuǎn)矢量的二子樣算法為:
2 組合導(dǎo)航系統(tǒng)
雖然慣性導(dǎo)航系統(tǒng)能夠連續(xù)工作并有效地提供姿態(tài)信息、位置信息和速度信息,但由于慣性傳感器誤差的積累,慣性導(dǎo)航系統(tǒng)的工作精度會隨時間下降。而GPS雖然可以提供長時間的誤差為米級的高精度位置輸出且用戶設(shè)備成本較低,但由于GPS信號會被遮擋或干擾,因此不能僅依賴GPS提供連續(xù)導(dǎo)航參數(shù)[8]。
鑒于INS和GPS系統(tǒng)的優(yōu)缺點互補(bǔ),將二者組合在一起,綜合兩系統(tǒng)的優(yōu)勢,能提供有效、長時、高精度、完整的導(dǎo)航參數(shù)。組合導(dǎo)航的一般結(jié)構(gòu)如圖2所示。
2.1 卡爾曼濾波器
Kalman濾波是一種估計算法[9],是導(dǎo)航系統(tǒng)中大多數(shù)狀態(tài)估計算法的基礎(chǔ),如衛(wèi)星導(dǎo)航結(jié)果的平滑、慣性導(dǎo)航系統(tǒng)的對準(zhǔn)和標(biāo)定、慣性導(dǎo)航系統(tǒng)與衛(wèi)星或其他導(dǎo)航傳感器間的組合導(dǎo)航等[10],并已經(jīng)成為從導(dǎo)航系統(tǒng)各種測量數(shù)據(jù)中獲得最優(yōu)估計結(jié)果的關(guān)鍵技術(shù)。
實際工程問題中,多數(shù)系統(tǒng)都是非線性的,因此采用擴(kuò)展卡爾曼濾波器(EKF)。擴(kuò)展卡爾曼濾波(EKF)為卡爾曼濾波的非線性形式[11]。
EKF的系統(tǒng)動態(tài)模型和觀測模型分別為:
函數(shù)f(·)和h(·)不能直接應(yīng)用在協(xié)方差中,取而代之的是可以計算其雅可比矩陣[12],這個過程實質(zhì)上將非線性的函數(shù)在當(dāng)前估計值處線性化了。
2.2 系統(tǒng)模型與狀態(tài)選擇
本文將慣導(dǎo)系統(tǒng)和GPS導(dǎo)航系統(tǒng)進(jìn)行融合,在當(dāng)?shù)貙?dǎo)航坐標(biāo)系下建立系統(tǒng)模型。若卡爾曼濾波器估計的是相對于地球并投影到當(dāng)?shù)貙?dǎo)航坐標(biāo)系的姿態(tài)和速度誤差,且估計的位置誤差以緯度、經(jīng)度和高度來表示,則狀態(tài)向量變?yōu)椋?/p>
式中,上標(biāo)n表示投影到當(dāng)?shù)貙?dǎo)航坐標(biāo)系。
除了地球旋轉(zhuǎn)角速率和陀螺測量值之外,姿態(tài)傳播方程還引入了一個轉(zhuǎn)移速率項,當(dāng)?shù)貙?dǎo)航坐標(biāo)系下的姿態(tài)誤差為:
慣性導(dǎo)航系統(tǒng)的主要噪聲源是由加速度計比力測量噪聲導(dǎo)致的速度誤差隨機(jī)游走以及由陀螺角速度率測量噪聲帶來的姿態(tài)誤差隨機(jī)游走。如果單獨估計加速度計和陀螺的動態(tài)零偏,則加速度計和陀螺零偏在運(yùn)行時的變化可以近似為白噪聲。
在INS/GPS組合中,使用GPS用戶設(shè)備的測量輸出與根據(jù)慣性導(dǎo)航參數(shù)預(yù)測的測量值之間的差來更新狀態(tài)向量,采用哪些測量則依賴于組合結(jié)構(gòu)。
3 仿真與實驗結(jié)果
本次實驗選用Pixhawk飛控單元作為飛行器主控板,MATLAB作為仿真軟件,分別對純慣導(dǎo)系統(tǒng)與組合導(dǎo)航系統(tǒng)進(jìn)行仿真,四旋翼無人飛行器搭建環(huán)境描述如下:
選用材料為尼龍加纖維的機(jī)架搭建飛行器,機(jī)架的對角軸距為35 cm;選取無刷電機(jī)型號為MT2312-960KV用于多旋翼飛行器中,提供動力輸出;選取電池容量為5 000 mA,最大放電電流為30 A。該飛行器遙控器型號為樂迪AT9,對應(yīng)的接收器型號為2.4 G、9通道的R9D;地面站軟件采用3DR推薦的專為PX4/PIXHAWK設(shè)計的新的QGroundControl,在該環(huán)境下對飛行器進(jìn)行校準(zhǔn)和調(diào)試。
3.1 理想狀態(tài)下的飛行軌跡
為了更好地理解導(dǎo)航系統(tǒng)的工作性能,在四旋翼無人飛行器飛行過程中,設(shè)定飛行順序如圖3所示。
具體過程可以描述為懸停時間為15 s;加速過程中,時間為10 s,加速度大小為0.5g(g為重力加速度);爬升過程中,時間為25 s,仰角速度為2 °/s;俯沖過程中,時間為25 s,俯角速度為2 °/s;減速過程中,時間為5 s,加速度大小為1g;飛行周期為140 s,循環(huán)飛行3次,則無人飛行器理想狀態(tài)下的飛行姿態(tài)參數(shù)如圖4所示。
3.2 實驗結(jié)果
(1)純慣導(dǎo)系統(tǒng)導(dǎo)航系統(tǒng)姿態(tài)誤差曲線如圖5所示。
(2)組合導(dǎo)航系統(tǒng)的姿態(tài)誤差曲線如圖6所示。
由實驗結(jié)果可知,理想飛行狀態(tài)下,飛行器的橫滾角和偏航角數(shù)值均為誤差值(接近于0),只有俯仰角發(fā)生變化,如圖4;若采用純慣性導(dǎo)航,飛行器的姿態(tài)誤差隨著時間而增大,在飛行結(jié)束時,東向姿態(tài)誤差(俯仰角誤差)會達(dá)到1.1°左右,北向姿態(tài)誤差(橫滾角誤差)達(dá)到約1.6°,天向姿態(tài)誤差(偏航角誤差)達(dá)到約1.5°,如圖5;若采用INS/GPS組合導(dǎo)航系統(tǒng),飛行器東向(俯仰)和北向(橫滾)的姿態(tài)誤差均小于0.5°,而且在長時間內(nèi)可以小于0.2°,而天向(偏航)姿態(tài)角誤差雖然會在短時間內(nèi)達(dá)到1°左右,但在其他時間段內(nèi)都能保持在0.5°左右,如圖6。因此,在相同的條件下,組合導(dǎo)航系統(tǒng)的性能優(yōu)于純慣導(dǎo)系統(tǒng),誤差較小,能夠為四旋翼無人飛行器提供較為精確的導(dǎo)航信息。
4 結(jié)論
本文主要是分析了導(dǎo)航系統(tǒng)的原理和算法,在對傳感器誤差進(jìn)行標(biāo)定和補(bǔ)償之后,采用了擴(kuò)展卡爾曼濾波器將捷聯(lián)慣性導(dǎo)航系統(tǒng)與GPS導(dǎo)航系統(tǒng)融合,并成功應(yīng)用于四旋翼無人飛行器上。仿真實驗結(jié)果表明了系統(tǒng)方案在四旋翼無人飛行器上實現(xiàn)的可行性,且在長時間內(nèi)可以提供較為準(zhǔn)確的導(dǎo)航信息,誤差較小,確保了無人飛行器的飛行效果。
參考文獻(xiàn)
[1] 馬遠(yuǎn)超.四旋翼飛行器導(dǎo)航及控制技術(shù)研究[D].哈爾濱:哈爾濱工程大學(xué),2013.
[2] MILHIM A B,ZHANG Y M,RABBATH C A.Gain scheduling based PID controller for fault tolerant control of a quad-rotor UAV[C].AIAA Infotech@Aerospace,2010:1-13.
[3] SALYCHEV O S.Applied inertial navigation:problems and solutions[M].Moscow,Russia:BMSTU Press,2004.
[4] MERWE R V D,WAN E A.Sigma-point Kalman filters for integrated navigation[C].Proceedings of the 60th Annual Meeting of the Institute of Navigation(ION),2004:641-654.
[5] 李曉峰.捷聯(lián)慣導(dǎo)和組合導(dǎo)航的仿真研究[D].西安:西安電子科技大學(xué),2010.
[6] BORTZ J E.A new mathematical formulation for strapdown inertial navigation[J].Aerospace and Electronic Systems,IEEE Transactions on,1971(1):61-66.
[7] 秦永元,張士邈.捷聯(lián)慣導(dǎo)姿態(tài)更新的四子樣旋轉(zhuǎn)矢量優(yōu)化算法研究[J].中國慣性技術(shù)學(xué)報,2001,9(4):1-7.
[8] GROVES P D.Principles of GNSS,inertial, and multisensor integrated navigation systems[M].Artech House,2013.
[9] MOURIKIS A I,ROUMELIOTIS S I.A multi-state constraint Kalman filter for vision-aided inertial navigation[C].Robotics and Automation,2007 IEEE International Conference on.IEEE,2007:3565-3572.
[10] HELAL A,MOORE S E,RAMACHANDRAN B.Drishti:An integrated navigation system for visually impaired and disabled[C].Wearable Computers,2001.Proceedings.Fifth International Symposium on.IEEE,2001:149-156.
[11] HAYKIN S S,HAYKIN S S.Kalman filtering and neural networks[M].Wiley Interscience,2001.
[12] JULIER S J,UHLMANN J K,DURRANT-WHYTE H F.A new approach for filtering nonlinear systems[C].American Control Conference,Proceedings of the 1995.IEEE,1995,3:1628-1632.