張寶譯

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

現(xiàn)今，無人機被應(yīng)用在測繪、安防、巡檢等多個領(lǐng)域。姿態(tài)信息（俯仰角、橫滾角、航向角）是決定無人機飛行穩(wěn)定的重要因素之一。姿態(tài)信息中，航向角十分重要，其多采用磁力計測量所在位置的磁場信息，與當(dāng)?shù)氐厍虼艌鲂畔⑦M行數(shù)據(jù)融合處理來確定，以此獲得無人機的初始航向角以及修正陀螺儀的時間累計誤差，維持無人機航向角的正確與穩(wěn)定。但是在強磁干擾環(huán)境下，無人機上的磁力計傳感器測量的數(shù)據(jù)不再是所在位置的地磁信息，不能夠確定無人機的航向角，而無人機航向角度不準(zhǔn)確會直接影響無人機的位置信息的計算，極易發(fā)生飛行事故。因而，強磁干擾環(huán)境下如何準(zhǔn)確得到符合無人機高動態(tài)控制要求的航向角度的研究關(guān)系到無人機的正?？刂坪惋w行安全，具有十分重要的工程應(yīng)用價值，對其研究具有非常重要的意義。

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

1 無人機姿態(tài)更新概述

1.1 姿態(tài)更新方法

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

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

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

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

1.2 強磁干擾環(huán)境下航向角測量概況

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

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

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

2 強磁干擾環(huán)境下無人機航向的修正技術(shù)

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

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

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

圖1 姿態(tài)更新簡要流程圖

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

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

2.2 無人機航向角修正技術(shù)實現(xiàn)方式

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

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

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

構(gòu)建旋轉(zhuǎn)四元數(shù)Qr=[cos(βdiffer/2),0,0,sin(βdiffer/2)]

根據(jù)四元數(shù)與旋轉(zhuǎn)之間的關(guān)系，利用Qr與Qb的四元數(shù)乘法運算，實現(xiàn)修正航向角度的目的，即QC=QrQb（符號代表四元數(shù)乘法）。

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

由此可得，四元數(shù)QC為經(jīng)過差分航向角度修正的四元數(shù)?？梢愿鶕?jù)無人機系統(tǒng)控制需要直接利用四元數(shù)進行飛行控制，也可以將QC轉(zhuǎn)換為姿態(tài)角用于無人機的控制。

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

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

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

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

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

4 實驗及數(shù)據(jù)分析

4.1 靜態(tài)實驗

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

4.2 動態(tài)實驗

無人機在空中做快速轉(zhuǎn)動的過程中，動態(tài)實驗航向角度數(shù)據(jù)對比圖如圖3所示。其中，采集差分定向測量的航向角度如圖3中虛線所示，經(jīng)過差分定向數(shù)據(jù)修正的航向角度如圖3中實線所示。

圖3 動態(tài)實驗航向角度數(shù)據(jù)

4.3 實驗數(shù)據(jù)分析

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

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

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

5 結(jié)束語

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