邊宇艦 焦君圣

（聲納技術(shù)重點(diǎn)實(shí)驗(yàn)室 第七一五研究所，杭州，310023）

利用三分量磁傳感器解算姿態(tài)問題研究

邊宇艦 焦君圣

（聲納技術(shù)重點(diǎn)實(shí)驗(yàn)室 第七一五研究所，杭州，310023）

為精準(zhǔn)測量載體姿態(tài)，在利用磁傳感器測量地磁場來計算載體航向的基礎(chǔ)上，從原理上推導(dǎo)如何利用Terrella6磁傳感器來測量載體姿態(tài)角。通過數(shù)據(jù)比對，誤差＜1°，驗(yàn)證了文章推導(dǎo)出的姿態(tài)角解算公式正確。

三分量；磁傳感器；姿態(tài)；歐拉角；坐標(biāo)旋轉(zhuǎn)

了解載體姿態(tài)對于載體導(dǎo)航、定位等工作具有重大幫助[1]。載體姿態(tài)的測量方式有多種，根據(jù)對象以及任務(wù)要求的不同，姿態(tài)傳感器可選用磁感應(yīng)式、慣性式等。磁感應(yīng)傳感器由于利用了地磁場，結(jié)構(gòu)中沒有可動部件，所以具有較好的抗沖擊和抗干擾性，而且其結(jié)構(gòu)簡單、重量輕、體積較小、啟動快、成本低，成為許多載體姿態(tài)測量的首選器件[2]。三分量磁傳感器由三軸微加速度計和三軸微磁強(qiáng)計組成，利用大地磁場和重力場在地理坐標(biāo)系和載體坐標(biāo)系之間的方向余弦轉(zhuǎn)換進(jìn)行絕對角度解算，克服了傳統(tǒng)的（如陀螺測量）需要積分得到姿態(tài)角，避免了累積誤差，保證了長期的測角精度[3]。

1 姿態(tài)測量原理

目前，已有文獻(xiàn)對姿態(tài)角的定義都不盡相同：有的定義俯仰角為載體縱軸與縱向水平軸之間的夾角；有的甚至直接定義姿態(tài)角等于歐拉角。看似類似，實(shí)則卻有偏差并且是片面的。因沒有姿態(tài)角的權(quán)威定義，學(xué)者參考時常會混淆[4]。在計算姿態(tài)角時，方法更是層出不窮[5]。使用歐拉角解算姿態(tài)角時，對于夾角的順序和標(biāo)記、夾角的兩個軸的指定，沒有任何標(biāo)準(zhǔn)規(guī)定[6]，科學(xué)家對此也未達(dá)成共識。而在實(shí)際中用到歐拉角時，必須明確的表示出夾角的順序，指定其參考軸。實(shí)際上，有許多方法可以設(shè)定兩個坐標(biāo)系的相對取向，歐拉角方法只是其中的一種[7]。此外，不同的作者會用不同組合的歐拉角來描述，或用不同的名字表示歐拉角。因此，使用歐拉角前，必須先做好明確的定義。

姿態(tài)測量系統(tǒng)需要給出的載體姿態(tài)信息是載體相對于地平面的三個姿態(tài)角：俯仰角、橫滾角、航向角，這實(shí)際上是載體坐標(biāo)系與地理坐標(biāo)系三軸間的夾角，具體定義如下：

（1）俯仰角：載體縱軸和水平面之間的夾角，向上為正，向下為負(fù)，定義域?yàn)?90°～90°。

（2）橫滾角：載體縱向?qū)ΨQ面與縱向鉛垂平面之間的夾角。橫滾角從鉛垂平面算起，右傾為正，左傾為負(fù)，定義域?yàn)?180°～180°。

（3）航向角：載體縱軸在水平面上的投影與地理子午線之間的夾角。航向角的數(shù)值以地理北向?yàn)槠瘘c(diǎn)沿逆時針方向計算，定義域?yàn)?～360°[8]。

對于載體姿態(tài)的解算方法，比較常用的是余弦法、矩陣法和角度給定法。角度給定法具有簡單易懂、便于理解的特點(diǎn)。角度給定法可以分為 RPY（Roll,Pitch,Yaw）和歐拉角方位表示法兩種[9]。RPY法起源于描繪船舶的姿態(tài)，一個是繞船前進(jìn)的方向即x軸方向的Roll轉(zhuǎn)角，第二個是繞水平Y(jié)軸方向的Pitch角，第三個是繞鉛垂z方向的Yaw角。RPY法中的x、y、z三軸是原坐標(biāo)系的三軸，如圖1所示。

圖1 RPY角姿態(tài)描述方法

歐拉角計算載體姿態(tài)有很長的歷史了，該方法不僅計算方便有效，最主要的是用它表示載體的姿態(tài)十分簡潔明了[10]。歐拉角表示法與 RPY法是不同的，首先是繞著O-xyz的z軸旋轉(zhuǎn)一個角度γ，得到新的坐標(biāo)系O-x1y1z1；之后繞著新坐標(biāo)系O-x1y1z1的xl軸旋轉(zhuǎn)φ角，得到新的坐標(biāo)系O-x2y2z2；再繞著O-x2y2z2的y2軸旋轉(zhuǎn)θ角，得到最終的坐標(biāo)系O-NED。每次的旋轉(zhuǎn)就相當(dāng)于進(jìn)行一次坐標(biāo)變換，這個過程可以用對應(yīng)的變換矩陣來表示。各矩陣排列順序是旋轉(zhuǎn)順序的逆序，這樣可以確保在后面與坐標(biāo)矩陣相乘時，首先旋轉(zhuǎn)的角度矩陣能先與坐標(biāo)矩陣相乘。

即坐標(biāo)也是可以逆變換的[12]，則有

此處坐標(biāo)系旋轉(zhuǎn)變換時建議代入普通坐標(biāo)系點(diǎn)進(jìn)行驗(yàn)算。

歐拉角是坐標(biāo)軸旋轉(zhuǎn)時兩個對應(yīng)坐標(biāo)軸之間的夾角，而俯仰角是載體縱軸和水平面之間的夾角，所以當(dāng)且僅當(dāng)先繞y產(chǎn)生俯仰角，再繞x產(chǎn)生橫滾角時，姿態(tài)角與歐拉角相等。當(dāng)先存在橫滾角，再產(chǎn)生俯仰角時，姿態(tài)角與歐拉角不再相等。例如，當(dāng)先繞y軸旋轉(zhuǎn)30°，即存在俯仰角30°的情況下，再繞x軸旋轉(zhuǎn)30°，即產(chǎn)生橫滾角30°。此時載體的姿態(tài)與先產(chǎn)生30°橫滾角，再產(chǎn)生30°俯仰角時的姿態(tài)是不相同的。因?yàn)闅W拉角與 RPY法不同，每次坐標(biāo)軸旋轉(zhuǎn)都是繞上一次旋轉(zhuǎn)完后新的坐標(biāo)系軸進(jìn)行的，因此旋轉(zhuǎn)順序很重要，須定義清楚。

2 利用加速度計算俯仰和橫滾角

本文中定義的參考坐標(biāo)系O-XHYHZH和傳感器坐標(biāo)系O-XYZ均是笛卡兒坐標(biāo)系。默認(rèn)參考坐標(biāo)系為XH軸指向地球北極，YH軸與緯線平行指向東，ZH軸垂直水平面向下。傳感器坐標(biāo)系是傳感器自身所在的坐標(biāo)系。Terrella6傳感器是通過感測地球地磁場和重力場所獲得的信息來確定方位的。測量過程分為兩步：首先由測得的重力加速度在傳感器坐標(biāo)系三個軸上的分量通過坐標(biāo)旋轉(zhuǎn)矩陣來確定其歐拉角；然后通過歐拉角將由磁傳感器測得的傳感器坐標(biāo)下的三軸磁分量投影到參考坐標(biāo)系，得到參考坐標(biāo)系下的三軸磁分量，由該參數(shù)可以計算得到方位角。在傳感器坐標(biāo)系X、Y、Z軸上可分別測得重力加速度g在其上的分量Ax、Ay、Az，三者與重力加速度g存在一定的旋轉(zhuǎn)對應(yīng)關(guān)系。根據(jù)不同的旋轉(zhuǎn)順序，求出的歐拉角是不同的，由于投影三軸磁分量到參考坐標(biāo)系只需用到歐拉角φ和θ，所以只解兩個旋轉(zhuǎn)矩陣即可。如果先繞X軸轉(zhuǎn)再繞Y軸轉(zhuǎn)，則根據(jù)旋轉(zhuǎn)關(guān)系有

由此可得俯仰角和橫滾角分別為

如果先繞Y軸轉(zhuǎn)再繞X軸轉(zhuǎn)，則旋轉(zhuǎn)變換關(guān)系變?yōu)?/p>

由此可得俯仰角和橫滾角分別為

兩種結(jié)果相似。根據(jù)上文的分析，只有先繞y軸旋轉(zhuǎn)再繞x軸旋轉(zhuǎn)時歐拉角才會與姿態(tài)角相等。因此只有第二種情況才是正確的。

3 利用磁場計算航向角

當(dāng)傳感器處在水平面上時，俯仰角α和橫滾角β均為零，如圖2所示。

圖2 X-Y-Z坐標(biāo)系內(nèi)方位角定義

方位傳感器方位角數(shù)學(xué)模型為：

式中，X、Y是磁傳感器探測到地磁場H在兩個互相垂直方向的分量。γ*角為傳感器坐標(biāo)系X軸正向與磁場水平分量之間的夾角，即X軸正向與磁北之間的夾角。γ*角減去或加上當(dāng)?shù)氐拇牌铅偶吹肵軸正向與正北之間的夾角，即方位角（航向角）。

當(dāng)傳感器不平行水平面時，傳感器坐標(biāo)系O-XYZ與坐標(biāo)系O-XHYHZH之間的關(guān)系用歐拉角和表示，其數(shù)學(xué)模型為：

則：

式中，X、Y和Z是三維磁傳感器探測得到地磁場H在傳感器三個互相垂直方向上的分量。YH和XH為利用歐拉角將測得的三軸磁分量投影到參考坐標(biāo)系下的磁分量。根據(jù)式（9）、（10）、（15），我們就可以利用測量得到的數(shù)進(jìn)行載體的三維姿態(tài)估計。

4 實(shí)驗(yàn)驗(yàn)證

Terrella6三分量磁傳感器能夠輸出航向、俯仰、橫滾角度值以及加速度和磁場強(qiáng)度分別在三軸上的分量。利用磁傳感器輸出的加速度和磁場分量計算其航向、俯仰和橫滾，并與其自身測量所得姿態(tài)角度進(jìn)行對比，以此來驗(yàn)證姿態(tài)角計算公式的正確性。通過串口將磁傳感器的測量結(jié)果發(fā)送到上位機(jī)，調(diào)整傳感器的姿態(tài)，使其輸出不同結(jié)果，便于后期對比，測量結(jié)果如表1所示。表1中Mx、My、Mz分別表示載體坐標(biāo)x、y、z軸上的地磁場分量。

表1 用磁傳感器實(shí)測數(shù)據(jù)與姿態(tài)解算公式計算姿態(tài)數(shù)據(jù)對比

通過實(shí)測數(shù)據(jù)，利用推導(dǎo)的姿態(tài)解算公式計算結(jié)果和傳感器實(shí)測結(jié)果進(jìn)行比較，航向、橫滾、俯仰的計算誤差都比較小，最大在1°誤差左右，在可接受范圍內(nèi)，證明了推導(dǎo)出的姿態(tài)解算公式的正確性。表中有些航向角數(shù)據(jù)計算出來需要用 360°或180°相減才能得到傳感器直接輸出的數(shù)據(jù)，因?yàn)閭鞲衅鲀?nèi)部已經(jīng)處理過數(shù)據(jù)。

5 結(jié)論

在正確得到姿態(tài)解算公式之后，通過分析解算公式就能方便地進(jìn)行誤差分析，較為容易地得知影響姿態(tài)角的因素具體有哪些，對其精度影響有多大，從而進(jìn)一步提高測量精度。

[1]岳志杰,王婷.AUV水下對接的導(dǎo)航和定位系統(tǒng)設(shè)計[C].2016 IEEE/OES China Ocean Acoustics Symposium，2016.

[2] LIU SHENG WU,ZHUANG JI DONG,HUNG J C,et al.Compass deviation analysis and compensation for a three-axis strapdown magnetic heading system[C].IFAC Symposia Series-Proceedings of a Triennial World Congress,1990.

[3]朱榮,周兆英.基于MEMS的姿態(tài)測量系統(tǒng)[J].測控技術(shù),2002,(10):6-8.

[4]臧愛云,原魁,嚴(yán)志剛,等.磁傳感器在導(dǎo)航系統(tǒng)中的應(yīng)用[J].高技術(shù)通訊,2004,(10):53-57.

[5]高鋒.應(yīng)用磁阻傳感器的數(shù)字航向儀設(shè)計[D].南京理工大學(xué),2004.

[6]劉敬彪,鄭玉冰,章雪挺.三軸磁羅盤的設(shè)計與誤差校正[J].自動化儀表,2008,(9):10-12.

[7]劉武發(fā)，蔣秦，龔振邦.基于磁阻和MEMS加速度傳感器的電子羅盤設(shè)計及應(yīng)用[J].兵工學(xué)報,2008,(2):244-248.

[8]崔璐璐.基于 MEMS器件的姿態(tài)測量系統(tǒng)研究與實(shí)現(xiàn)[D].大連理工大學(xué),2009.

[9]LIN Y,ZHANG W J,KOUBEK R J.On integration of interface design methods:can debates be resolved[J].Interacting with Computers,2006,18(4):709-722.

[10]張振友,李明,田應(yīng)仲.物體空間姿態(tài)的實(shí)施測量方法研究[J].機(jī)械設(shè)計與研究，2008,(5):83-87.

[11]崔中興.慣性導(dǎo)航系統(tǒng)[M].北京:國防工業(yè)出版社,1982．

[12]羅武勝,徐濤,杜列波.基于加速度計和磁強(qiáng)計的定向鉆進(jìn)姿態(tài)測量及方位校正[J].國防科技大學(xué)學(xué)報,2007,(1)：106-110.