航向傾角是什麼意思啊

王勇軍1，2，李 智1，2，李 翔2

（1。桂林航天工業學院 無人遙測重點實驗室，廣西 桂林541004；

2。桂林電子科技大學 電子工程與自動化學院，廣西 桂林541004）

為了解決低成本小型無人機航姿精密測量的問題，設計了一種基於MARG感測器的航向測量系統方案。該系統由MEMS IMU、電子羅盤和STM32F407微處理器組成，採用運算量較小的梯度下降演算法和改進型二階互補濾波演算法將具有互補特性的電子羅盤和IMU進行資料組合濾波， 並基於四元數進行座標轉換，解算出飛行器航向資訊。透過對航向測量系統的實驗測試及其在旋翼飛行器上的驗證分析，結果表明，在沒有外界資訊輔助的情況下，該系統較好地解決了噪聲干擾與航向測量問題，可以滿足小型旋翼無人機對航向資訊的要求。

中圖分類號：

V249

文獻標識碼：

A

DOI：

10。16157/j。issn。0258-7998。173121

中文引用格式：

王勇軍，李智，李翔。 採用組合濾波演算法的無人機航向測量系統研究［J］。電子技術應用，2018，44（2）：39-43。

英文引用格式：

Wang Yongjun，Li Zhi，Li Xiang。 Research of heading measurement system for UAV based on combined filter［J］。 Application of Electronic Technique，2018，44（2）：39-43。

0 引言

航向是無人飛行器的重要飛行引數，在飛行過程中，實時準確地獲得飛行器的航向資訊決定了飛行器的可靠性和穩定性。如今隨著微電子技術的發展，小型低成本旋翼無人飛行器普遍採用基於微機電系統（MEMS）的三軸陀螺儀、三軸加速度計和三軸磁強計來測量航向

［1］

。由於MEMS感測器自身存在非正交誤差，以及資料積分產生的發散誤差和外部干擾產生的隨機誤差，因此需要對多感測器進行誤差處理並尋求最佳資料融合演算法，才能得到精確的航向資訊。

目前多數由陀螺儀和磁強計組成的航向測量系統主要採用羅差修正法和卡爾曼濾波演算法來計算航向資訊

［2］

。採用羅差修正的方法對磁強計進行校正，雖然能提高一定的精度，但僅針對磁感測器校正仍然無法提高系統在受到長時間低頻干擾時的航向精度；而卡爾曼及其擴充套件演算法可對磁強計和陀螺儀進行資訊融合，能有效平滑曲線，使高頻誤差得到抑制，但無法消除磁強計受到的軟硬磁干擾誤差，而且應用時需要為其建立可靠穩定的狀態方程，確定合適的量測噪聲和過程噪聲協方差矩陣，需要較大的時間開銷。

為此，本文設計一種基於MEMS慣性測量單元（IMU）和電子羅盤的航向測量系統，採用梯度下降演算法來對磁強計和陀螺儀進行誤差修正，而且在姿態解算過程只需進行乘法和加法運算，普通的微控制器即能滿足演算法要求

［3］

。另外，在MAHONY R提出的補償濾波器［4］的基礎上進行最佳化改進，結合二階互補濾波演算法來進行資料融合，以期得到精密航向輸出。

1 無人機航向角描述與系統方案設計

1.1 座標系定義及姿態矩陣

無人機的航姿資訊主要包括航向角（也稱航偏角）、俯仰角θ、橫滾角γ。選取“東北天”為地理座標系（記為ENU系），“右前上”為機體座標系（記為NED系），由於NED系和ENU系均是直角座標系，且各個軸之間始終保持垂直，因此，NED系和ENU系的空間位置關係即可理解為剛體的定點轉動

［5］

。地理座標系與機體座標系之間的角度轉換關係如圖1所示。

1.2 系統方案設計

該航向測量系統硬體主要由MEMS-IMU、MEMS電子羅盤和主控制器組成，如圖2所示。其中MEMS-IMU採用的是InvenSense公司的MPU-6500，它由一個三軸陀螺儀和三軸加速度計組成，可以透過SPI口輸出飛行器的角速度、加速度等姿態資訊，用來計算俯仰角及橫滾角；所採用的MEMS電子羅盤是ST（意法半導體）公司的LSM303D，它內部的三軸磁強計可透過SPI匯流排口輸出測得的磁場資料，另外，它內部的三軸加速度可用來測量重力向量，獲得載體的傾角資訊，並透過運算補償航向資訊的輸出。主控器採用ST公司CM4核心的32位ARM微控制器STM32F407，主要透過獲取MEMS感測器的輸出資料進行姿態解算，完成飛行器航向的精密測量。

由於IMU中的陀螺儀、加速度計和電子羅盤中的加速度感測器都可以測量加速度資訊，因此該系統的感測器配置也是具有一定冗餘度，主控制器的另一任務就是將各感測器的資料取長補短進行資料融合，提高姿態解算精度。主控制器讀取出感測器採集到的原始資料，利用梯度下降法最佳化電子羅盤中加速度計和磁強計的輸出資料，獲得靜態性較好的姿態四元數，然後利用改進型互補濾波器將其與IMU獲得的動態性較好的姿態資訊進行融合濾波，最後透過四元數的座標換算解算出航向角。

2 航向資料組合濾波演算法設計

2.1 梯度下降法

梯度下降演算法是一種迭代求極值的演算法，具體實現過程是按目標函式斜率的負方向來搜尋尋優。電子羅盤中的加速度計和磁強計具有良好的靜態特性，長時間使用不會引入積分誤差，但是加速度計受載體振動和運動影響較大，動態環境下，瞬時誤差大，而且磁強計容易受到周邊環境中軟硬磁干擾［7］，因此本文首先使用梯度下降演算法加對速度計和磁強計輸出資料進行最佳化，獲得一個靜態特性較好的姿態四元數，然後再進行下一步航向解算。

顯然f（q）≥0，進而對姿態四元數的最佳化就可以轉換為對f（q）求最小值。

梯度下降法在姿態解算過程中只有簡單的加法和乘法運算，此時四元數更新方程可變為

［8］

：

2.2 改進型互補濾波演算法

利用電子羅盤中三軸磁強計和加速度計測得的資料透過梯度下降法對姿態四元數進行修正轉換後得到姿態值記為。根據加速度計和磁強計的特性，不會引入累積誤差，但是在動態情況下，會引入較大瞬時誤差。相反IMU中的陀螺儀可測得瞬時精度高的姿態資料，但是有累積誤差。設IMU中陀螺儀和加速度計直接積分解算出來的姿態值為，根據和在頻域上的互補特性，透過傳統的互補濾波方法融合兩者，可得到新的姿態值，雖然能同時消除低頻和高頻的干擾，實現姿態資料的融合，然而，互補濾波器

［9］

的低通阻帶衰減緩慢，誤差較大時，振動大，濾波效果差。因此在傳統互補濾波器的基礎上添加了PI（比例積分）環節，構成改進型二階互補濾波器，如圖3所示。

由圖3可知：

2.3 航向角組合解算

系統基於四元數的姿態計算即對式（1）中的姿態四元數的微分方程進行求解，四元數微分方程常用的解算方法有

［10］

：單子樣旋轉適量法、多子樣旋轉向量法、龍格庫塔法和泰勒展開法。旋轉向量法多用於輸出是角增量的陀螺儀， 而MEMS陀螺儀的輸出形式是角速率。考慮到運算精度和速度， 本系統採用四階-龍格庫塔法求解微分方程：

式中，h為姿態資料取樣的週期，即四元數更新週期；ω

b

（t）、、ω

b

（t+h）分別為在更新週期h時間內陀螺的取樣值。每個取樣週期內提取陀螺儀的資料，對式（10）進行迭代運算，便能夠實現對四元數的實時更新，從而得出航向角。

3 實驗驗證分析

將設計的航向測量系統安裝在電子轉檯上，為避免鐵磁干擾，加工一個60 cm高的木架做固定支撐，並校正好零位，透過串列埠連線至上位機，進行感測器資料採集，如圖4所示。

透過電子轉檯水平轉動，每次回到零位時利用上位機分別採集電子磁羅盤的資料和經過梯度下降與互補濾波演算法融合後的資料。透過對比分析可知，沒有使用濾波演算法的磁強計測量的航偏角相對零位誤差較大，達到了±3°左右，磁強計受外界干擾嚴重。經過組合濾波處理後得到的偏航角消除了部分干擾訊號，誤差可穩定保持在±1。0°以內。

在電子轉檯上轉動測試可看作是靜態測試，為進一步驗證航向測量系統在多旋翼飛行器實際飛行過程中的效能，將該系統固定在PIX4飛控模組遮蔽罩上，並一同安裝在六旋翼飛行器上，機架底部水平固定在萬向節一端，萬向節另外一端固定在水平的桌面上，如圖5所示。

啟動旋翼飛行器電機，將飛行器油門通道值保持在空中懸停狀態，操作飛行器方向舵，讓其原地旋轉。然後，透過2。4G串列埠數傳電臺接收飛控航向資料和航向測量系統傳出的磁強計、陀螺儀原始資料以及組合解算後的資料，並用MATLAB進行資料分析，如圖6所示。

圖6中採集的是飛行器水平懸停轉動（以飛控為基準）時的資料，其中的電子羅盤和IMU輸出航向分別代表的是磁強計和陀螺儀資料處理的航向角誤差曲線，組合濾波輸出航向表示經梯度下降和二階互補濾波演算法融合後解算的航向角誤差曲線。旋翼飛行器運動過程中受電機工作引起的機架振動、載體重力分佈不均、機械連線處阻尼問題等因素影響，導致航向角誤差比水平放置在轉檯上時的靜態零點誤差偏差相對較大且波動相對厲害，經過組合濾波處理後，航向角誤差在±1。5°以內，可以滿足小型旋翼飛行器對航向資料的要求。

4 結論

航向資訊作為無人飛行器導航定位的重要測量引數，要求具有較高的精度。在小型旋翼無人飛行器系統上，利用MEMS IMU和電子磁羅盤各自的優點及其互補性，配合ARM主控制器組成低成本航向測量系統，採用梯度下降和二階互補濾波演算法，使三軸陀螺儀、三軸加速度計、三軸磁強計經過資料融合後，完成座標變換和姿態解算，從而輸出較高精度的航向角。實驗結果表明：該航向測量系統較好地解決了噪聲干擾與航向最優估計問題，並在實驗平臺上得到了驗證，航偏角誤差保持在±1。5°以內，在沒有外界資訊輔助的情況下，可穩定地輸出準確姿態資料，滿足了低成本旋翼無人機對航向資訊的要求。

參考文獻

［1］ 何川，李翔，王勇軍。基於STM32的四旋翼飛行器的姿態最優估計研究［J］。電子技術應用，2015，41（12）：61-64。

［2］ 任愛愛，孫永榮，胡應東，等。無人機航向測量抗差補償濾波技術研究［J］。感測器與微系統，2009，28（5）：34-36。

［3］ MADGWICK S O H，HARRISON A J L，VAIDYANATHAN R，et al。Estimation of IMU and MARG orientation using a gradient descent algorithm［C］。IEEE International Conference on Rehabilitation Robotics，2011：1-7。

［4］ MAHONY R，HAMEL T，PFLIMLIN J M。Nonlinear complementary filters on the special orthogonal group［C］。IEEE Transactions on Automatic Control，2008，53（5）：1203-1218。

［5］ 秦永元。慣性導航［M］。北京：科學出版社，2014。

［6］ 曾憲陽，楊紅莉，鬱漢琪。基於STM32微控制器四旋翼飛行器建模分析與設計［J］。電子技術應用，2016，12（42）：65-68。

［7］ 聶鵬，李佩華，李正強，等。基於卡爾曼濾波的小型無人機姿態估計算法研究［J］。瀋陽航空航天大學學報，2013，30（6）：53-57。

［8］ 譚廣超。四旋翼飛行器姿態控制系統的設計與實現［D］。大連：大連理工大學，2013。

［9］ 廖懿華，張鐵民，廖貽泳。基於模糊-比例積分偏差修正的多旋翼飛行器姿態測算系統［J］。農業工程學報，2014，30（20）：19-27。

［10］ 王彤，馬建倉，秦濤，等。基於旋轉四元數的姿態解算演算法［J］。彈箭與制導學報，2014，34（3）：15-17。