0 引言

　　近年來，隨著智能化產業的迅速發展，無人機行業逐漸走進人們的視線。由于四旋翼無人機的結構簡單可靠，具有垂直起降、定點懸停、穩定低速飛行和在小空間內飛行的能力［1］，所以受到廣泛的關注并在多個領域得到了廣泛的應用［2］。

　　在四旋翼無人機系統發展所面臨的關鍵技術中，導航系統是其重要的組成部分之一，它承擔著提供給飛行器位置、速度、姿態等參數狀態數據的任務。目前常用的導航方法有：慣性導航、衛星導航、視覺導航以及它們的組合導航等。由于單一導航系統難以滿足無人飛行器的發展要求［3］，于是產生了把兩種或兩種以上的不同導航系統以適當的方式組合在一起的組合導航，比單獨使用任一導航系統時具有更高的系統性能［4］。本文是利用擴展卡爾曼濾波器將慣性導航系統和GPS導航系統的組合來完成導航任務，建立了系統模型并將其在四旋翼無人飛行器上應用實現。

　　1 慣性導航系統

　　1.1 慣性傳感器誤差分析

　　影響導航系統工作精度的主要因素之一是慣性傳感器誤差的存在，為了減少傳感器誤差，需要先對傳感器進行標定和誤差補償來保證導航系統的高精度和高性能。

　　1.2 慣性導航系統姿態矩陣計算

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

　　在導航系統中，最重要的計算就是姿態矩陣的求解，而常見的姿態矩陣算法主要有四元數法、歐拉角法、方向余弦法3種［5］。歐拉角法計算過程中方程式出現退化現象，方向余弦法通常計算量很大，因此經常采用的是四元數法求解姿態矩陣。但四元數法存在不可交換誤差，為減小這種誤差，本文采用的是Bortz在1971年提出的等效旋轉矢量算法［6］。

　　1.2.1 旋轉矢量與姿態四元數的關系

　　設Q（t+h）和Q（t）分別為飛行器載體在t+h時刻和t時刻的姿態四元數，則：

　　1.2.3 旋轉矢量的求解

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

　　2 組合導航系統

　　雖然慣性導航系統能夠連續工作并有效地提供姿態信息、位置信息和速度信息，但由于慣性傳感器誤差的積累，慣性導航系統的工作精度會隨時間下降。而GPS雖然可以提供長時間的誤差為米級的高精度位置輸出且用戶設備成本較低，但由于GPS信號會被遮擋或干擾，因此不能僅依賴GPS提供連續導航參數［8］。

　　鑒于INS和GPS系統的優缺點互補，將二者組合在一起，綜合兩系統的優勢，能提供有效、長時、高精度、完整的導航參數。組合導航的一般結構如圖2所示。

　　2.1 卡爾曼濾波器

　　Kalman濾波是一種估計算法［9］，是導航系統中大多數狀態估計算法的基礎，如衛星導航結果的平滑、慣性導航系統的對準和標定、慣性導航系統與衛星或其他導航傳感器間的組合導航等［10］，并已經成為從導航系統各種測量數據中獲得最優估計結果的關鍵技術。

　　實際工程問題中，多數系統都是非線性的，因此采用擴展卡爾曼濾波器（EKF）。擴展卡爾曼濾波（EKF）為卡爾曼濾波的非線性形式［11］。

　　EKF的系統動態模型和觀測模型分別為：

　　函數f（·）和h（·）不能直接應用在協方差中，取而代之的是可以計算其雅可比矩陣［12］，這個過程實質上將非線性的函數在當前估計值處線性化了。

　　2.2 系統模型與狀態選擇

　　本文將慣導系統和GPS導航系統進行融合，在當地導航坐標系下建立系統模型。若卡爾曼濾波器估計的是相對于地球并投影到當地導航坐標系的姿態和速度誤差，且估計的位置誤差以緯度、經度和高度來表示，則狀態向量變為：

　　式中，上標n表示投影到當地導航坐標系。

　　除了地球旋轉角速率和陀螺測量值之外，姿態傳播方程還引入了一個轉移速率項，當地導航坐標系下的姿態誤差為：

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

　　在INS/GPS組合中，使用GPS用戶設備的測量輸出與根據慣性導航參數預測的測量值之間的差來更新狀態向量，采用哪些測量則依賴于組合結構。

　　3 仿真與實驗結果

　　本次實驗選用Pixhawk飛控單元作為飛行器主控板，MATLAB作為仿真軟件，分別對純慣導系統與組合導航系統進行仿真，四旋翼無人飛行器搭建環境描述如下：

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

　　3.1 理想狀態下的飛行軌跡

　　為了更好地理解導航系統的工作性能，在四旋翼無人飛行器飛行過程中，設定飛行順序如圖3所示。

　　具體過程可以描述為懸停時間為15 s；加速過程中，時間為10 s，加速度大小為0.5g（g為重力加速度）；爬升過程中，時間為25 s，仰角速度為2 °/s；俯沖過程中，時間為25 s，俯角速度為2 °/s；減速過程中，時間為5 s，加速度大小為1g；飛行周期為140 s，循環飛行3次，則無人飛行器理想狀態下的飛行姿態參數如圖4所示。

　　3.2 實驗結果

　　（1）純慣導系統導航系統姿態誤差曲線如圖5所示。

　　（2）組合導航系統的姿態誤差曲線如圖6所示。

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

　　4 結論

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

　　參考文獻

　　［1］ 馬遠超。四旋翼飛行器導航及控制技術研究［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］ 李曉峰。捷聯慣導和組合導航的仿真研究［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］ 秦永元，張士邈。捷聯慣導姿態更新的四子樣旋轉矢量優化算法研究［J］。中國慣性技術學報，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.