摘 要： 針對(duì)無(wú)人飛行器行業(yè)的快速發(fā)展和導(dǎo)航系統(tǒng)對(duì)飛行器的重要性，提出了組合導(dǎo)航系統(tǒng)的融合方案,。介紹了捷聯(lián)慣導(dǎo)系統(tǒng)的原理,、姿態(tài)算法,。通過(guò)對(duì)慣性傳感器進(jìn)行誤差標(biāo)定和補(bǔ)償，利用擴(kuò)展卡爾曼濾波器建立了INS/GPS組合導(dǎo)航系統(tǒng),。仿真實(shí)驗(yàn)表明,，組合導(dǎo)航系統(tǒng)的工作性能要優(yōu)于純慣性導(dǎo)航系統(tǒng)，能夠?yàn)轱w行器提供較高的導(dǎo)航精度,。最后,，將這種組合導(dǎo)航系統(tǒng)在四旋翼無(wú)人飛行器上進(jìn)行了實(shí)現(xiàn)。

關(guān)鍵詞： 無(wú)人機(jī),；慣性導(dǎo)航系統(tǒng),；組合導(dǎo)航系統(tǒng)；卡爾曼濾波器

0 引言

近年來(lái),，隨著智能化產(chǎn)業(yè)的迅速發(fā)展,，無(wú)人機(jī)行業(yè)逐漸走進(jìn)人們的視線。由于四旋翼無(wú)人機(jī)的結(jié)構(gòu)簡(jiǎn)單可靠,，具有垂直起降,、定點(diǎn)懸停、穩(wěn)定低速飛行和在小空間內(nèi)飛行的能力^[1],，所以受到廣泛的關(guān)注并在多個(gè)領(lǐng)域得到了廣泛的應(yīng)用^[2],。

在四旋翼無(wú)人機(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)航、視覺(jué)導(dǎo)航以及它們的組合導(dǎo)航等,。由于單一導(dǎo)航系統(tǒng)難以滿足無(wú)人飛行器的發(fā)展要求^[3],，于是產(chǎn)生了把兩種或兩種以上的不同導(dǎo)航系統(tǒng)以適當(dāng)?shù)姆绞浇M合在一起的組合導(dǎo)航，比單獨(dú)使用任一導(dǎo)航系統(tǒng)時(shí)具有更高的系統(tǒng)性能^[4],。本文是利用擴(kuò)展卡爾曼濾波器將慣性導(dǎo)航系統(tǒng)和GPS導(dǎo)航系統(tǒng)的組合來(lái)完成導(dǎo)航任務(wù),，建立了系統(tǒng)模型并將其在四旋翼無(wú)人飛行器上應(yīng)用實(shí)現(xiàn)。

1 慣性導(dǎo)航系統(tǒng)

1.1 慣性傳感器誤差分析

影響導(dǎo)航系統(tǒng)工作精度的主要因素之一是慣性傳感器誤差的存在,，為了減少傳感器誤差，需要先對(duì)傳感器進(jìn)行標(biāo)定和誤差補(bǔ)償來(lái)保證導(dǎo)航系統(tǒng)的高精度和高性能,。

1.2 慣性導(dǎo)航系統(tǒng)姿態(tài)矩陣計(jì)算

在導(dǎo)航系統(tǒng)的解算過(guò)程中,，首先要處理的是坐標(biāo)系的標(biāo)定，通常有慣性坐標(biāo)系(通常表示為i系) ,、地理坐標(biāo)系(通常表示為g系),、導(dǎo)航坐標(biāo)系(通常表示為n系),、載體坐標(biāo)系(通常表示為b系)等。本文在分析導(dǎo)航系統(tǒng)問(wèn)題時(shí)是將導(dǎo)航坐標(biāo)系設(shè)定為地理坐標(biāo)系,，地理坐標(biāo)系采用x指東向,、y指北向、z指天向的形式,，慣導(dǎo)系統(tǒng)的原理方程可以用圖1表示,。

在導(dǎo)航系統(tǒng)中，最重要的計(jì)算就是姿態(tài)矩陣的求解,，而常見(jiàn)的姿態(tài)矩陣算法主要有四元數(shù)法,、歐拉角法、方向余弦法3種^[5],。歐拉角法計(jì)算過(guò)程中方程式出現(xiàn)退化現(xiàn)象,，方向余弦法通常計(jì)算量很大，因此經(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時(shí)刻和t時(shí)刻的姿態(tài)四元數(shù),，則：

1.2.3 旋轉(zhuǎn)矢量的求解

通常，對(duì)于旋轉(zhuǎn)矢量采用泰勒級(jí)數(shù)展開法來(lái)求解,。若采用直線擬合角速度,，等效旋轉(zhuǎn)矢量的二子樣算法為：

2 組合導(dǎo)航系統(tǒng)

雖然慣性導(dǎo)航系統(tǒng)能夠連續(xù)工作并有效地提供姿態(tài)信息、位置信息和速度信息,，但由于慣性傳感器誤差的積累,，慣性導(dǎo)航系統(tǒng)的工作精度會(huì)隨時(shí)間下降。而GPS雖然可以提供長(zhǎng)時(shí)間的誤差為米級(jí)的高精度位置輸出且用戶設(shè)備成本較低,，但由于GPS信號(hào)會(huì)被遮擋或干擾,，因此不能僅依賴GPS提供連續(xù)導(dǎo)航參數(shù)^[8]。

鑒于INS和GPS系統(tǒng)的優(yōu)缺點(diǎn)互補(bǔ),，將二者組合在一起,，綜合兩系統(tǒng)的優(yōu)勢(shì)，能提供有效,、長(zhǎng)時(shí),、高精度、完整的導(dǎo)航參數(shù),。組合導(dǎo)航的一般結(jié)構(gòu)如圖2所示,。

2.1 卡爾曼濾波器

Kalman濾波是一種估計(jì)算法^[9]，是導(dǎo)航系統(tǒng)中大多數(shù)狀態(tài)估計(jì)算法的基礎(chǔ)，如衛(wèi)星導(dǎo)航結(jié)果的平滑,、慣性導(dǎo)航系統(tǒng)的對(duì)準(zhǔn)和標(biāo)定,、慣性導(dǎo)航系統(tǒng)與衛(wèi)星或其他導(dǎo)航傳感器間的組合導(dǎo)航等^[10]，并已經(jīng)成為從導(dǎo)航系統(tǒng)各種測(cè)量數(shù)據(jù)中獲得最優(yōu)估計(jì)結(jié)果的關(guān)鍵技術(shù),。

實(shí)際工程問(wèn)題中,，多數(shù)系統(tǒng)都是非線性的，因此采用擴(kuò)展卡爾曼濾波器（EKF）,。擴(kuò)展卡爾曼濾波（EKF）為卡爾曼濾波的非線性形式^[11],。

EKF的系統(tǒng)動(dòng)態(tài)模型和觀測(cè)模型分別為：

函數(shù)f(·)和h(·)不能直接應(yīng)用在協(xié)方差中，取而代之的是可以計(jì)算其雅可比矩陣^[12],，這個(gè)過(guò)程實(shí)質(zhì)上將非線性的函數(shù)在當(dāng)前估計(jì)值處線性化了,。

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)模型,。若卡爾曼濾波器估計(jì)的是相對(duì)于地球并投影到當(dāng)?shù)貙?dǎo)航坐標(biāo)系的姿態(tài)和速度誤差,，且估計(jì)的位置誤差以緯度、經(jīng)度和高度來(lái)表示,，則狀態(tài)向量變?yōu)椋?/p>

式中,，上標(biāo)n表示投影到當(dāng)?shù)貙?dǎo)航坐標(biāo)系。

除了地球旋轉(zhuǎn)角速率和陀螺測(cè)量值之外,，姿態(tài)傳播方程還引入了一個(gè)轉(zhuǎn)移速率項(xiàng),，當(dāng)?shù)貙?dǎo)航坐標(biāo)系下的姿態(tài)誤差為：

慣性導(dǎo)航系統(tǒng)的主要噪聲源是由加速度計(jì)比力測(cè)量噪聲導(dǎo)致的速度誤差隨機(jī)游走以及由陀螺角速度率測(cè)量噪聲帶來(lái)的姿態(tài)誤差隨機(jī)游走。如果單獨(dú)估計(jì)加速度計(jì)和陀螺的動(dòng)態(tài)零偏,，則加速度計(jì)和陀螺零偏在運(yùn)行時(shí)的變化可以近似為白噪聲,。

在INS/GPS組合中，使用GPS用戶設(shè)備的測(cè)量輸出與根據(jù)慣性導(dǎo)航參數(shù)預(yù)測(cè)的測(cè)量值之間的差來(lái)更新狀態(tài)向量,，采用哪些測(cè)量則依賴于組合結(jié)構(gòu),。

3 仿真與實(shí)驗(yàn)結(jié)果

本次實(shí)驗(yàn)選用Pixhawk飛控單元作為飛行器主控板，MATLAB作為仿真軟件,，分別對(duì)純慣導(dǎo)系統(tǒng)與組合導(dǎo)航系統(tǒng)進(jìn)行仿真,，四旋翼無(wú)人飛行器搭建環(huán)境描述如下：

選用材料為尼龍加纖維的機(jī)架搭建飛行器，機(jī)架的對(duì)角軸距為35 cm,；選取無(wú)刷電機(jī)型號(hào)為MT2312-960KV用于多旋翼飛行器中,，提供動(dòng)力輸出；選取電池容量為5 000 mA,，最大放電電流為30 A,。該飛行器遙控器型號(hào)為樂(lè)迪AT9，對(duì)應(yīng)的接收器型號(hào)為2.4 G,、9通道的R9D,；地面站軟件采用3DR推薦的專為PX4/PIXHAWK設(shè)計(jì)的新的QGroundControl,，在該環(huán)境下對(duì)飛行器進(jìn)行校準(zhǔn)和調(diào)試,。

3.1 理想狀態(tài)下的飛行軌跡

為了更好地理解導(dǎo)航系統(tǒng)的工作性能,，在四旋翼無(wú)人飛行器飛行過(guò)程中，設(shè)定飛行順序如圖3所示,。

具體過(guò)程可以描述為懸停時(shí)間為15 s,；加速過(guò)程中，時(shí)間為10 s,，加速度大小為0.5g(g為重力加速度),；爬升過(guò)程中，時(shí)間為25 s,，仰角速度為2 °/s,；俯沖過(guò)程中，時(shí)間為25 s,，俯角速度為2 °/s,；減速過(guò)程中，時(shí)間為5 s,，加速度大小為1g,；飛行周期為140 s，循環(huán)飛行3次,，則無(wú)人飛行器理想狀態(tài)下的飛行姿態(tài)參數(shù)如圖4所示,。

3.2 實(shí)驗(yàn)結(jié)果

(1)純慣導(dǎo)系統(tǒng)導(dǎo)航系統(tǒng)姿態(tài)誤差曲線如圖5所示。

(2)組合導(dǎo)航系統(tǒng)的姿態(tài)誤差曲線如圖6所示,。

由實(shí)驗(yàn)結(jié)果可知,，理想飛行狀態(tài)下，飛行器的橫滾角和偏航角數(shù)值均為誤差值（接近于0）,，只有俯仰角發(fā)生變化,，如圖4；若采用純慣性導(dǎo)航,，飛行器的姿態(tài)誤差隨著時(shí)間而增大,，在飛行結(jié)束時(shí)，東向姿態(tài)誤差（俯仰角誤差）會(huì)達(dá)到1.1°左右,，北向姿態(tài)誤差（橫滾角誤差）達(dá)到約1.6°,，天向姿態(tài)誤差（偏航角誤差）達(dá)到約1.5°，如圖5,；若采用INS/GPS組合導(dǎo)航系統(tǒng),，飛行器東向（俯仰）和北向（橫滾）的姿態(tài)誤差均小于0.5°，而且在長(zhǎng)時(shí)間內(nèi)可以小于0.2°,，而天向（偏航）姿態(tài)角誤差雖然會(huì)在短時(shí)間內(nèi)達(dá)到1°左右,，但在其他時(shí)間段內(nèi)都能保持在0.5°左右，如圖6。因此,，在相同的條件下,，組合導(dǎo)航系統(tǒng)的性能優(yōu)于純慣導(dǎo)系統(tǒng)，誤差較小,，能夠?yàn)樗男頍o(wú)人飛行器提供較為精確的導(dǎo)航信息,。

4 結(jié)論

本文主要是分析了導(dǎo)航系統(tǒng)的原理和算法，在對(duì)傳感器誤差進(jìn)行標(biāo)定和補(bǔ)償之后,，采用了擴(kuò)展卡爾曼濾波器將捷聯(lián)慣性導(dǎo)航系統(tǒng)與GPS導(dǎo)航系統(tǒng)融合,，并成功應(yīng)用于四旋翼無(wú)人飛行器上。仿真實(shí)驗(yàn)結(jié)果表明了系統(tǒng)方案在四旋翼無(wú)人飛行器上實(shí)現(xiàn)的可行性,，且在長(zhǎng)時(shí)間內(nèi)可以提供較為準(zhǔn)確的導(dǎo)航信息,，誤差較小，確保了無(wú)人飛行器的飛行效果,。