張寶譯

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

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

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

1 無(wú)人機(jī)姿態(tài)更新概述

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

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

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

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

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

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

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

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

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

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

2.1 無(wú)人機(jī)航向角修正技術(shù)思路及難點(diǎn)

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

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

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

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

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

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

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

假設(shè)四元數(shù)Qb里包含的航向角度為βb，差分定向技術(shù)測(cè)量的無(wú)人機(jī)航向角度為β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ù)乘法運(yùn)算，實(shí)現(xiàn)修正航向角度的目的，即QC=QrQb（符號(hào)代表四元數(shù)乘法）。

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

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

3 方案實(shí)施過(guò)程中遇到的問(wèn)題及解決方法

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

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

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

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

4 實(shí)驗(yàn)及數(shù)據(jù)分析

4.1 靜態(tài)實(shí)驗(yàn)

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

4.2 動(dòng)態(tài)實(shí)驗(yàn)

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

圖3 動(dòng)態(tài)實(shí)驗(yàn)航向角度數(shù)據(jù)

4.3 實(shí)驗(yàn)數(shù)據(jù)分析

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

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

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

5 結(jié)束語(yǔ)

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