黃 建，胡越黎，，楊文榮，冉 峰

（1.上海大學 機電工程與自動化學院，上海200072；2.上海大學 微電子研究與開發(fā)中心，上海200072）

組合導(dǎo)航系統(tǒng)在四旋翼無人機上的實現(xiàn)*

黃 建1，胡越黎1，2，楊文榮2，冉 峰2

（1.上海大學 機電工程與自動化學院，上海200072；2.上海大學 微電子研究與開發(fā)中心，上海200072）

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

無人機；慣性導(dǎo)航系統(tǒng)；組合導(dǎo)航系統(tǒng)；卡爾曼濾波器

0 引言

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

在四旋翼無人機系統(tǒng)發(fā)展所面臨的關(guān)鍵技術(shù)中，導(dǎo)航系統(tǒng)是其重要的組成部分之一，它承擔著提供給飛行器位置、速度、姿態(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)以適當?shù)姆绞浇M合在一起的組合導(dǎo)航，比單獨使用任一導(dǎo)航系統(tǒng)時具有更高的系統(tǒng)性能[4]。本文是利用擴展卡爾曼濾波器將慣性導(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)工作精度的主要因素之一是慣性傳感器誤差的存在，為了減少傳感器誤差，需要先對傳感器進行標定和誤差補償來保證導(dǎo)航系統(tǒng)的高精度和高

性能。

如果 εa（g）、Ma（g）以及與 Gg都可以估計出來，則經(jīng)過補償之后的比力和角速度為：

式中，符號^表示估計值。

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

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

圖1 導(dǎo)航系統(tǒng)方程模塊圖

在導(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ù)，則：

其中，?表示四元數(shù)乘法，h是時間間隔，q（h）是由旋轉(zhuǎn)矢量Φ構(gòu)造的四元數(shù)：

其中，Φ0=（ΦT·Φ）1/2。

1.2.2 等效旋轉(zhuǎn)矢量的微分方程

假設(shè)旋轉(zhuǎn)矢量表示為：Φ（t）=[ΦxΦyΦz]T，用旋轉(zhuǎn)矢量來描述載體姿態(tài)運動時，等效旋轉(zhuǎn)矢量微分方程可以寫為：

其中，符號×表示叉乘，Φ為等效旋轉(zhuǎn)矢量，它表征載體從t時刻到t+h時刻的等效旋轉(zhuǎn)。

工程上，通常因為姿態(tài)更新周期很短，將Φ的高次項可略去不計，得到常用的近似方程：

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

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

若采用拋物線擬合角速度，等效旋轉(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)缺點互補，將二者組合在一起，綜合兩系統(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)的對準和標定、慣性導(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ù)。

圖2 INS/GNSS組合導(dǎo)航一般結(jié)構(gòu)

通?？柭鼮V波算法可以描述為：若以K-1時刻的最優(yōu)估計 XK-1為準，預(yù)測 K時刻的狀態(tài)變量X?K/K-1，同時又對該狀態(tài)進行觀測，得到觀測變量ZK，在預(yù)測與觀測之間進行分析（或者說以觀測量對預(yù)測量進行修正），可以得到K時刻的最優(yōu)狀態(tài)估計XK。

式中，XK是K時刻的系統(tǒng)狀態(tài)；ZK是K時刻的測量值；ΦK，K-1和 ΓK，K-1是 K-1時刻到 K時刻的狀態(tài)轉(zhuǎn)移矩陣；HK是測量系統(tǒng)的參數(shù)；WK和 VK分別表示過程和測量噪聲，它們被假設(shè)成高斯白噪聲，滿足一定條件[9]。

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

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

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

2.2 系統(tǒng)模型與狀態(tài)選擇

本文將慣導(dǎo)系統(tǒng)和GPS導(dǎo)航系統(tǒng)進行融合，在當?shù)貙?dǎo)航坐標系下建立系統(tǒng)模型。若卡爾曼濾波器估計的是相對于地球并投影到當?shù)貙?dǎo)航坐標系的姿態(tài)和速度誤差，且估計的位置誤差以緯度、經(jīng)度和高度來表示，則狀態(tài)向量變?yōu)椋?/p>

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

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

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

在 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)進行仿真，四旋翼無人飛行器搭建環(huán)境描述如下：

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

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

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

圖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所示。

圖4 理想狀態(tài)下飛行器的姿態(tài)角參數(shù)

圖5 純慣導(dǎo)系統(tǒng)模式下的姿態(tài)誤差

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

圖6 組合導(dǎo)航系統(tǒng)模式下的姿態(tài)誤差

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

4 結(jié)論

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

[1]馬遠超.四旋翼飛行器導(dǎo)航及控制技術(shù)研究[D].哈爾濱：哈爾濱工程大學，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].西安：西安電子科技大學，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ù)學報，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.

Integrated navigation system implemented on the quad-rotor UAV

Huang Jian1，Hu Yueli1，2，Yang Wenrong2，Ran Feng2
（1.College of Mechatronic Engineering and Automation，Shanghai University，Shanghai 200072，China；2.Microelectronic Research and Development Center，Shanghai University，Shanghai 200072，China）

An information fusion scheme based on the inertial navigation system（INS）/GPS integrated navigation system is presented according to the rapid development of unmanned aerial vehicle（UAV）industry and the importance of navigation system on UAV.The principle of strapdown inertial navigation and the algorithm of attitude matrix are carried out in this paper.Then，the extended Kalman filter of INS/GPS integrated navigation system is built up after calculating and compensating the sensor error.Simulation results show that the working performance of the integrated navigation system is better than that of the pure inertial navigation system and can provide high precision of navigation for UAV.Finally，the integrated navigation system is realized on the quad-rotor UAV.

unmanned aerial vehicle；inertial navigation system；integrated navigation；Kalman filter

V249.32

A

0258-7998（2015）05-0167-04

10.16157/j.issn.0258-7998.2015.05.042

2015-03-16）

黃建（1990-），男，碩士研究生，主要研究方向：嵌入式系統(tǒng)、機器視覺。

國家自然科學基金項目（61376028）

胡越黎（1959-），通信作者，男，博士，教授，博士生導(dǎo)師，主要研究方向：圖像處理、機器視覺、片上系統(tǒng)、芯片設(shè)計，E-mail：huyueli@shu.edu.cn。