強磁干擾環境下無人機航向的修正方法研究

張寶譯

（山東師范大學信息科學與工程學院，山東 濟南 250358）

現今，無人機被應用在測繪、安防、巡檢等多個領域。姿態信息（俯仰角、橫滾角、航向角）是決定無人機飛行穩定的重要因素之一。姿態信息中，航向角十分重要，其多采用磁力計測量所在位置的磁場信息，與當地地球磁場信息進行數據融合處理來確定，以此獲得無人機的初始航向角以及修正陀螺儀的時間累計誤差，維持無人機航向角的正確與穩定。但是在強磁干擾環境下，無人機上的磁力計傳感器測量的數據不再是所在位置的地磁信息，不能夠確定無人機的航向角，而無人機航向角度不準確會直接影響無人機的位置信息的計算，極易發生飛行事故。因而，強磁干擾環境下如何準確得到符合無人機高動態控制要求的航向角度的研究關系到無人機的正常控制和飛行安全，具有十分重要的工程應用價值，對其研究具有非常重要的意義。

在強磁干擾環境下，磁力計無法使用，可以使用其他手段獲得角度。比如，使用差分定向技術測量航向角度信息，但是差分定向技術輸出測量角度的頻率慢（5～10Hz）、精度低（與兩天線之間的距離有關），無法滿足無人機的高動態控制要求，不能直接使用，因此必須將差分航向角度與無人機姿態信息進行融合處理，從而得到動態性能好、準確度高的無人機航向角數據。文章對差分定向測量的航向角與無人機姿態信息如何進行數據融合處理進行研究，給出了一種實時、便捷的數據融合方法。

1 無人機姿態更新概述

1.1 姿態更新方法

用于無人機姿態更新的算法有許多種，如方向余弦法、歐拉角法、四元數法等。

四元數是定義由4個元構成的數：Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k，其中，q0、q1、q2、q3是實數，i、j、k既是互相正交的單位向量又是虛單位。

三維空間的任意一次旋轉，都能夠用三維空間內的某一個軸旋轉一定的角度來表示。這種方法中，某一個軸可以用三維向量(x,y,z)表示，一定的角度可以用θ表示。簡單地說，三維向量(x,y,z)和角度θ組成的四維向量(θ,x,y,z)可以表示出三維空間內的任意旋轉。其中，三維向量(x,y,z)用來表示旋轉軸的方向，因此可以用單位向量來表示此旋轉軸的方向。

單位向量(x,y,z)旋轉θ角度的四元數為。相較于歐拉角法和旋轉矩陣法，四元數法表示更加緊湊，計算量也小一些，是一種比較常用的方法。文章姿態更新采用四元數更新算法。

1.2 強磁干擾環境下航向角測量概況

在強磁干擾環境下，磁力計無法使用，因此需要尋找一種能夠不受強磁環境干擾穩定輸出航向角度的傳感器或設備。

差分GPS設備可以利用RTK雙天線技術，將2根天線固定在載體的特定位置，根據載波相位差分原理，計算出載體在大地坐標系的航向角。因此，可以在無人機上的固定位置放置2根GPS天線，通過差分定向技術獲得無人機的航向角。

差分定向獲取的航向角度，更新頻率慢、精度低，不能滿足無人機高動態控制要求，無法直接使用。為了滿足無人機飛行控制對于航向角度高動態、實時性和準確性的要求，文章提出一種以差分定向測量航向角為標定量，實時計算（修正）無人機航向角的方法。既解決了強磁干擾下無航向糾正信息導致的航向角度錯誤問題，也解決了強磁干擾下無航向糾正信息導致的在航向角度計算上的陀螺儀時間累計誤差，使得無人機可以在強磁干擾下，依然可以獲取穩定的航向角度信息。

2 強磁干擾環境下無人機航向的修正技術

2.1 無人機航向角修正技術思路及難點

無人機采集自身陀螺儀、加速度計等傳感器數據，采用相關算法實時計算其姿態，得到包含姿態信息的數據，如四元數。無人機更新姿態的簡要流程圖如圖1所示。

在強磁干擾環境下，磁力計無法使用，可根據圖1計算出的四元數Qb，不能保證包含正確的航向角姿態信息。為此，設計一種方法，不再使用易受強磁干擾的磁力計，使用不受強磁干擾影響的差分定向技術獲取無人機的航向角度原始值，修正無磁力計傳感器數據參與而計算出的包含姿態信息的四元數Qb，經過數據融合處理，最終得到經過修正的四元數Qc。該方法的難點，也是文章研究的重點，是如何將差分定向獲取的航向角度原始值與正常姿態更新計算出的姿態數據進行數據融合，進而得到經過修正的姿態數據Qc。該方法的流程圖如圖2所示。

圖1 姿態更新簡要流程圖

圖2 文章所用方法的基本流程圖

圖2中，利用差分定向測量的航向角度數據與新計算出的姿態數據進行數據融合處理是本方法的難點與創新點，其原理是將差分定向測量的航向角作為標定量，新計算出的姿態數據中的航向角作為待修正量，兩者之間的差值即為航向角度修正量。將待修正量修正到標定量上，可以等效為一次三維空間內的旋轉。假設無人機使用的導航坐標系方向為（NED-北東地），那么此次修正的旋轉軸為(0,0,1)，旋轉的角度即為修正量。

2.2 無人機航向角修正技術實現方式

無人機姿態更新系統使用不含磁力計數據的傳感器數據、上一次四元數數據Qa，按照正常流程進行姿態計算，迭代計算出新的四元數Qb。

假設四元數Qb里包含的航向角度為βb，差分定向技術測量的無人機航向角度為βrtk，則兩者之差為航向角度初始修正量βb=(βrtk-βb)。

航向角度初始修正量乘比例因子k，得到航向角度修正量βdiffer=(k·βd)。

構建旋轉四元數Qr=[cos(βdiffer/2),0,0,sin(βdiffer/2)]

根據四元數與旋轉之間的關系，利用Qr與Qb的四元數乘法運算，實現修正航向角度的目的，即QC=QrQb（符號代表四元數乘法）。

假設四元數QC=(qc0,qc1,qc2,qc3)，四元數Qb=(qb0,qb1,qb2,qb3),則：

由此可得，四元數QC為經過差分航向角度修正的四元數。可以根據無人機系統控制需要直接利用四元數進行飛行控制，也可以將QC轉換為姿態角用于無人機的控制。

3 方案實施過程中遇到的問題及解決方法

在現階段調研的產品中，差分定向測量航向角的頻率，最高為10Hz，一般為5Hz。而無人機姿態計算的頻率一般為200Hz或更高。因此，以陀螺儀傳感器為主的無人機自主姿態計算為主，當差分定向航向角度輸出時，再對姿態四元數進行修正。

使用差分定向技術獲得的航向角，其精度為0.5/L（L為2根天線之間的距離），單位為°。由于無人機大小的限制，2根天線的距離一般在40～80cm，其精度為0.625～1.25°。

由于差分定向測量角度的精度偏低，因此將其作為標定量，在差分定向標志位指示差分定向數據可用時，以差分定向測量角度與此時解算角度求差，然后乘比例系數，得到需要修正的角度量。

比例系數k可根據工程實際需要進行調節，差分定向測量精度不高，比例系數過大，每次得到的航向角度修正量就會偏大，造成航向角度修正過于“猛烈”，不利于電機的響應及無人機的控制。如果比例系數過小，得到的航向角度修正量就會偏小，雖然在修正航向角度的“平滑性”上會好，但是會造成修正時間過長、延時過大、實時性下降，不符合無人機控制的高動態性要求。

4 實驗及數據分析

4.1 靜態實驗

安裝好雙天線（天線距離約0.5m）的無人機，靜止放置在戶外空曠處。采集同一時間段內（20min），經過差分定向測量航向角修正的無人機航向角度和差分定向測量的航向角度，并分析2組數據的均值和方差。

4.2 動態實驗

無人機在空中做快速轉動的過程中，動態實驗航向角度數據對比圖如圖3所示。其中，采集差分定向測量的航向角度如圖3中虛線所示，經過差分定向數據修正的航向角度如圖3中實線所示。

圖3 動態實驗航向角度數據

4.3 實驗數據分析

（1）靜態實驗數據。差分定向測量的航向角度方差為0.0633，均值為205.2386。經過修正的航向角度，其方差為0.0273，均值為205.2704。經過修正的航向角相較于差分定向測量航向角，方差更小，曲線更為平滑，且無角度漂移現象發生，始終被差分定向測量航向角修正，維持在正確的航向角度上。

（2）動態實驗數據。圖3中的的數據顯示，經過修正的航向角度不僅維持了實時性好的特點，而且始終被差分測量航向角度修正，且瞬時波動值更小。從圖3的兩條曲線可以看出，經過修正的航向角度的平滑性明顯優于差分定向原始測量角度。

（3）實驗結果總結。通過對無人機在靜止和運動狀態下采集的數據進行分析可以得出，利用文章所提方法進行修正的航向角度不僅維持了高實時性、高平滑性，而且始終被差分航向角度準確修正，沒有發生時間累積誤差，完全滿足了研究要求，能夠解決無人機在強磁干擾環境下的航向角度測量問題。

5 結束語

普通無人機通常使用磁力計傳感器來確定航向角度，在強磁干擾情況下（如鋼鐵結構大橋附近、高壓電線附近或地下鐵礦附近等）不能正確測量航向角，容易導致無人機控制異常，影響飛行器自身及周邊人員、環境的安全。為此，文章設計了一種方案，利用差分定向技術獲取航向角度，并提出了一種利用差分航向角與無人機自身姿態更新數據進行融合處理的方法，該方法實用、快速，能夠在強磁干擾環境下準確獲得無人機航向角度，保證了無人機在強磁環境下的飛行安全，具有十分重要的實用價值。