無人機捷聯(lián)航姿系統(tǒng)誤差分析與補償*

朱 燕， 崔智軍，2

(1.安康學(xué)院 電子與信息工程學(xué)院，陜西 安康 725000；2.西北工業(yè)大學(xué) 電子信息學(xué)院，陜西 西安 710129)

0 引 言

隨著無人機的不斷發(fā)展，對航向姿態(tài)系統(tǒng)的要求越來越高，不僅要實時、準(zhǔn)確獲取包括俯仰角、傾斜角、航向角等信息，為控制裝置提供關(guān)鍵的參數(shù)，而且要求其尺寸不斷減小、功耗不斷降低[1～3]。另外，由于飛行環(huán)境的電磁條件復(fù)雜，無人機若要提高系統(tǒng)控制精度，要求傳感器采集的數(shù)據(jù)具有很好的精度，亦要求傳感器抗干擾能力強、適應(yīng)性好[4]，因此設(shè)計一種微型化、低功耗、高精度的航向姿態(tài)參考系統(tǒng)顯得尤為重要。

近年來，利用三軸微機電系統(tǒng)(micro-electro-mechanical system,MEMS)陀螺、三軸MEMS加速度計以及三軸磁傳感器進(jìn)行姿態(tài)測量成為捷聯(lián)式低成本全固態(tài)姿態(tài)測量系統(tǒng)研究的熱點之一[5,6]。慣導(dǎo)系統(tǒng)的工作精度在很大程度上取決于慣性器件的工作精度[7]，而低成本MEMS陀螺的性能較低，只能在很短的時間內(nèi)保證測量精度，而利用加速度計和磁傳感器對地球重力場和地磁場的測量值所計算的航向姿態(tài)值在動態(tài)情況下也不準(zhǔn)確，因此如何應(yīng)用這些器件使系統(tǒng)產(chǎn)生高精度的測量姿態(tài)信號，是無人機捷聯(lián)航姿系統(tǒng)設(shè)計取得成功的一項關(guān)鍵技術(shù)。

本文的研究目標(biāo)是在由三軸MEMS加速度計、三軸MEMS角速率陀螺、三軸微型磁通門傳感器以及ARM處理器為數(shù)據(jù)處理核心構(gòu)建的微型捷聯(lián)航姿系統(tǒng)硬件平臺上，分析各傳感器件的誤差特點，建立誤差模型，進(jìn)行適當(dāng)?shù)男盘枮V波、元件補償以及算法優(yōu)化以提高器件的測量精度，進(jìn)而提升航向姿態(tài)測量的精度與穩(wěn)定性，使之成為一種低成本、微型化、高精度的航向和姿態(tài)測量裝置。

1 傳感器誤差建模與誤差補償算法

1.1 磁傳感器誤差建模與誤差補償

磁傳感器的誤差來源主要分為以下幾類：零位誤差、標(biāo)度因素誤差、非正交誤差、安裝誤差和羅差。其中標(biāo)度因素誤差、零位誤差和不正交誤差可以歸為制造誤差。

結(jié)合常見磁羅盤的泊松數(shù)學(xué)模型，對三軸磁傳感器的數(shù)學(xué)模型描述為[8]

hm=KhNh(Shh+Hh)+Bh+εh

(1)

式中hm為磁傳感器的實際輸出；Kh為磁傳感器的標(biāo)度因素誤差矩陣；Nh為磁傳感器的非正交誤差矩陣；Sh為磁傳感器的軟磁矢量誤差；h為磁場的矢量實際值；Hh為磁傳感器的硬磁矢量誤差；Bh為磁傳感器的零位誤差；εh為磁傳感器的噪聲誤差，可以通過平均值測量。

磁傳感器的羅差可以等效為制造誤差，因此磁傳感器的誤差補償分為制造誤差校準(zhǔn)和安裝誤差(可以通過傳感器的精準(zhǔn)安裝補償)校準(zhǔn)兩部分。本文只分析制造誤差。

磁傳感器的制造誤差校準(zhǔn)有很多方法，而不借助外界信息的自標(biāo)定方法被廣泛研究，如文獻(xiàn)[9～14]，但各方法均有不足，本文研究針對三軸磁傳感器的智能誤差補償方法，采用橢球面假設(shè)法，其補償原理討論如下：

通常，某一地點的地磁場大小和方向h0保持不變，理論上磁傳感器的輸出軌跡為球形，但實際中由于誤差的存在，真實的輸出hf是橢球，結(jié)合羅差的Possion數(shù)學(xué)模型[8]，則有h0=M-1(hf-Bh)，M為標(biāo)度因數(shù)誤差、非正交誤差和軟磁效應(yīng)引起的誤差的綜合影響。此時

(2)

式中L=M-1;A=LTL。

1.2 加速度計誤差建模與誤差補償

結(jié)合該MEMS加速度計的基本工作原理及性能指標(biāo)的分析，將對輸出形成影響的誤差源主要分為以下幾類：零偏誤差、標(biāo)度因素誤差、安裝誤差、非線性誤差和隨機噪聲。對于一般的MEMS加速度計，在外部環(huán)境保持不變的情況下，傳感器刻度因數(shù)的非線性度較小，因此在工作環(huán)境變化不大的應(yīng)用中，可以不考慮非線性造成的影響，交叉耦合誤差、振蕩誤差均可忽略。以X軸為例,簡化后的加速度計誤差模型為[16]

am=SaNaa+Ba+εa=Ka+Ba+εa

(3)

式中a為加速度計理論值；am為加速度計實際輸出值；Sa為標(biāo)度因素誤差；Ba為零偏誤差；Na為X軸安裝誤差；εa為隨機誤差。

由于MEMS加速度計輸出數(shù)據(jù)噪聲較大，會導(dǎo)致姿態(tài)計算結(jié)果不穩(wěn)定，故需預(yù)先進(jìn)行濾波處理。而Kalman濾波迭代速度快，存儲空間小，非常適合系統(tǒng)頻率不高、存儲空間寶貴的嵌入式系統(tǒng)，本文在加速度計輸出補償之前先進(jìn)行Kalman濾波[17],之后，進(jìn)行加速度的校準(zhǔn)。本文在常見的6位置法基礎(chǔ)上，增加10個采樣點。設(shè)計了一種16位置法，利用已知的加速度計的輸出投影計算加速度計的誤差補償系數(shù)k和Ba。

式(3)加速度計的誤差模型可以寫成矩陣形式

(4)

本文設(shè)計的16位置求解誤差補償系數(shù)的具體步驟為：1)調(diào)整好實驗室的三軸轉(zhuǎn)臺與水平坐標(biāo)精確對準(zhǔn);2)將所要補償?shù)募铀俣扔嫲惭b在調(diào)整好的三軸轉(zhuǎn)臺上;3)將轉(zhuǎn)臺旋轉(zhuǎn)到指定的16個位置，并記錄實驗數(shù)據(jù)，計算誤差補償系數(shù)，補償完畢。

已知重力加速度計在指定的16個位置的重力投影矩陣可以用矩陣V表示為

(5)

以式(5)為基準(zhǔn)，加速度計在上述16位置的真實輸出值可以用3×16矩陣U表示

(6)

由最小二乘法計算系數(shù)矩陣P=UVT(VVT)-1，求得誤差補償系數(shù)k和Ba，完成對加速度計的補償。

1.3 速率陀螺誤差建模與誤差補償

由于在造成MEMS速率陀螺測量不精確的諸多誤差當(dāng)中，零偏誤差和標(biāo)度因素誤差影響最大，而通常見到的MEMS速率陀螺每小時可以達(dá)到十幾度的漂移，以敏感軸X為例,在不考慮加速度偏差的情況下，MEMS速率陀螺的誤差數(shù)學(xué)模型為[18]

ωm=SNTω+Bω+ε=Mω+Bω+ε

(7)

式中ωm為速率陀螺實際輸出；S為標(biāo)度因素誤差；Bω為零偏誤差；ε為隨機誤差；N為非正交誤差；T為X軸安裝誤差；ω為速率陀螺實際的輸入值。

本文針對九軸傳感器姿態(tài)測量系統(tǒng)中的速率陀螺特點，采用分步補償?shù)乃惴?，動態(tài)補償中利用磁傳感器的輸出補償速率陀螺，無需速率轉(zhuǎn)臺就可以完成對速率陀螺的補償。本文對三軸速率陀螺誤差分為靜態(tài)補償和動態(tài)補償：

1)靜態(tài)補償:在速率陀螺靜止的情況下，陀螺自身的分辨率是無法辨別地球的自轉(zhuǎn)，此時速率陀螺理論上無輸出，可以近似認(rèn)為零偏誤差Bω是造成陀螺輸出的主要原因。則式(7)簡化為ωm=Bω+ε,可以近似地將陀螺靜態(tài)時的輸出ωm看作是關(guān)于Bω的線性方程式，本文采用遞推最小二乘法求解。

2)動態(tài)誤差補償:速率陀螺的動態(tài)補償是指在陀螺有一定的角度輸入的情況下對陀螺進(jìn)行誤差補償，且動態(tài)誤差補償必須建立在靜態(tài)誤差補償?shù)幕A(chǔ)上。靜態(tài)補償之后，從式(7)知，只要求出M就可以完成陀螺的補償。

在九軸傳感器的姿態(tài)測量系統(tǒng)中，完成對磁傳感器和加速度計的誤差補償后的九軸傳感器的3個敏感軸和轉(zhuǎn)臺的對應(yīng)敏感軸已經(jīng)對準(zhǔn)，那么在外界因素相對穩(wěn)定的情況下，磁傳感器可以較為準(zhǔn)確的提供地磁場信息，利用磁傳感器的輸出作為基準(zhǔn)，在已知時間間隔的情況下就可以完成對速率陀螺的補償。

以水平旋轉(zhuǎn)Z軸輸出為例，當(dāng)將九軸傳感器姿態(tài)測量系統(tǒng)固定在轉(zhuǎn)臺上施加一定的外界加速度水平旋轉(zhuǎn)時，俯仰角θ、傾斜角γ不會發(fā)生變換，只有航向角φ發(fā)生變化，此時針對磁傳感器而言，只有X軸和Y軸的輸出發(fā)生改變，Z軸不發(fā)生改變。X軸和Y軸的輸出坐標(biāo)關(guān)系如圖1所示。

圖1 X軸、Y軸磁傳感器數(shù)據(jù)分量隨Δφ的變化示意

由圖1可知

(8)

由于實際中磁傳感器的更新頻率遠(yuǎn)小于速率陀螺，因此將磁傳感的更新頻率作為旋轉(zhuǎn)時間差Th，設(shè)Δt=Th,則在Th時間內(nèi)，磁傳感器繞Z軸的基準(zhǔn)速率為ωz=Δφ/Th,此時，式(7)速率陀螺誤差模型可以改寫為

(9)

式中ωmx1,ωmy1,ωmz1為傳感器系統(tǒng)繞Z軸旋轉(zhuǎn)時陀螺的輸出值。

同理，單獨圍繞Y軸旋轉(zhuǎn)時，磁傳感器Y軸輸出幾乎保持不變，只有X軸和Z軸的輸出變化；單獨圍繞X軸旋轉(zhuǎn)時，磁傳感器X軸輸出幾乎保持不變，只有Y軸和Z軸的輸出變化；在這2種情況下可以得到類似式(9)的矩陣式。通過計算3種情況下的矩陣式，利用遞推最小二乘法可以求出動態(tài)誤差補償?shù)?個系數(shù)，陀螺補償完畢。

2 實驗驗證

為了驗證上述誤差補償效果，首先制作捷聯(lián)航姿系統(tǒng)，其中磁傳感器為自制的三軸磁通門傳感器，三軸MEMS加速度和三軸MEMS速率陀螺分別采用意法半導(dǎo)體公司生產(chǎn)的LIS3DSH和L3GD20；采用C語言分別實現(xiàn)三軸磁傳感器、三軸加速度計和三軸速率陀螺的誤差補償算法。

2.1 磁傳感器誤差補償效果測試

為了驗證磁傳感器誤差補償效果，首先將磁傳感器精準(zhǔn)安裝在三軸無磁轉(zhuǎn)臺上(中國船舶工業(yè)集團(tuán)公司第6354研究所研制的3SK—150型，分辨率為0.02°)，以避免產(chǎn)生安裝誤差。精準(zhǔn)安裝后認(rèn)為三軸磁通門傳感器三軸和三軸轉(zhuǎn)臺重合，在給定三軸磁通門傳感器的俯仰角θ和傾斜角γ(三軸無磁轉(zhuǎn)臺提供)情況下，通過測試4種姿態(tài)下補償前后航向角φ的變化，驗證誤差補償效果。誤差補償前后對比如表1所示。

表1 固定俯仰角θ和傾斜角γ測得航向角φ的變化 (°)

由表1中對4種不同位置計算的補償前后航向值可知，補償后的磁傳感器達(dá)到了較好的水平。誤差總體控制在-1.5°～0.8°范圍之內(nèi)，達(dá)到較好的效果。

2.2 加速度計誤差補償效果測試

MEMS加速度計誤差補償之前，下位機發(fā)送的加速度計數(shù)據(jù)經(jīng)過補償程序的采集且已經(jīng)完成Kalman濾波處理，利用16位置法對加速度計進(jìn)行標(biāo)定。經(jīng)查西安地區(qū)的重力加速度約為9.774 m/s2，采集的16位置數(shù)據(jù)如表2所示。

表2 加速度計16位置采集數(shù)據(jù)

在加速度計經(jīng)過16位置法補償之后，驗證誤差補償效果。此時航姿系統(tǒng)已經(jīng)安裝在無磁三軸轉(zhuǎn)臺上，初始狀態(tài)下令初始俯仰角、傾斜角、航向角都為0°。分別繞X,Y軸旋轉(zhuǎn)無磁轉(zhuǎn)臺，使得俯仰角、傾斜角從-85°～85°變化，每5°采樣1次，記錄每個位置補償后的加速度計輸出值。補償后的數(shù)據(jù)顯示，當(dāng)俯仰角、傾斜角從-85°～85°變化時，俯仰角誤差總體控制在-0.25°～0.05°范圍內(nèi)，傾斜角誤差總體控制在-0.25°～0.35°范圍內(nèi)，達(dá)到較好水平。

2.3 速率陀螺誤差補償效果測試

速率陀螺誤差補償開始前將三軸傳感器旋轉(zhuǎn)到俯仰角、傾斜角、航向角均為0°，為了盡量準(zhǔn)確，誤差補償設(shè)定采集時間為10 min。待靜態(tài)誤差補償完以后，繼續(xù)保持靜止測量準(zhǔn)確性，整個過程持續(xù)3.5 min，采集該段時間內(nèi)補償后的速率陀螺的輸出，圖2為速率陀螺在靜止的情況下補償前后的航向角誤差對比。

圖2 補償前后航向角誤差對比

從圖2可知，航向角誤差在補償前隨著時間的增加逐漸增加，陀螺也存在累積誤差，航向角誤差最高可達(dá)到11°左右，零偏誤差補償后可以減少到±0.3°之內(nèi)。從整個補償過程看，傾斜角誤差補償后控制在±0.4°之內(nèi)，俯仰角誤差補償后控制在±0.4°之內(nèi)，達(dá)到較好的補償效果。

靜態(tài)誤差補償后，有前述方法，分別繞X,Y,Z3軸轉(zhuǎn)動計算動態(tài)誤差補償系數(shù)，得到的動態(tài)誤差補償系數(shù)M為

為了測試補償后的機動性能效果，固定俯仰角、傾斜角為0°，航向角從0°～360°變化，圍繞Z軸旋轉(zhuǎn)，期間速率變化不定，不定時加速、勻速、靜止。速率變化期間的最大角速度約為15°/s，最小角速度約為0.6°/s，整個過程持續(xù)3.5 min，從誤差補償效果看，當(dāng)載體急加速或減速時，誤差會明顯增大，但整個過程誤差控制在±1°以內(nèi)，當(dāng)載體趨于勻速或者靜止時誤差趨于穩(wěn)定，大小控制在±0.5°之內(nèi)，達(dá)到較好的補償效果。

3 結(jié)束語

本文針對由三軸磁傳感器、三軸MEMS加速度計和三軸MEMS速率陀螺構(gòu)成的無人機捷聯(lián)航姿系統(tǒng)，對傳感器的3種誤差來源進(jìn)行了詳細(xì)的分析，并針對誤差來源建立了相應(yīng)的誤差數(shù)學(xué)模型同時提出了對應(yīng)的誤差補償算法。結(jié)果顯示，誤差補償均達(dá)到了較好的效果。