邊宇艦 焦君圣

（聲納技術重點實驗室 第七一五研究所，杭州，310023）

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

邊宇艦 焦君圣

（聲納技術重點實驗室 第七一五研究所，杭州，310023）

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

三分量；磁傳感器；姿態(tài)；歐拉角；坐標旋轉

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

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

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

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

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

（2）橫滾角：載體縱向對稱面與縱向鉛垂平面之間的夾角。橫滾角從鉛垂平面算起，右傾為正，左傾為負，定義域為?180°～180°。

（3）航向角：載體縱軸在水平面上的投影與地理子午線之間的夾角。航向角的數值以地理北向為起點沿逆時針方向計算，定義域為0～360°[8]。

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

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

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

即坐標也是可以逆變換的[12]，則有

此處坐標系旋轉變換時建議代入普通坐標系點進行驗算。

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

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

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

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

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

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

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

3 利用磁場計算航向角

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

圖2 X-Y-Z坐標系內方位角定義

方位傳感器方位角數學模型為：

式中，X、Y是磁傳感器探測到地磁場H在兩個互相垂直方向的分量。γ*角為傳感器坐標系X軸正向與磁場水平分量之間的夾角，即X軸正向與磁北之間的夾角。γ*角減去或加上當地的磁偏角ε即得X軸正向與正北之間的夾角，即方位角（航向角）。

當傳感器不平行水平面時，傳感器坐標系O-XYZ與坐標系O-XHYHZH之間的關系用歐拉角和表示，其數學模型為：

則：

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

4 實驗驗證

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

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

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

5 結論

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

[1]岳志杰,王婷.AUV水下對接的導航和定位系統(tǒng)設計[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].測控技術,2002,(10):6-8.

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

[5]高鋒.應用磁阻傳感器的數字航向儀設計[D].南京理工大學,2004.

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

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

[8]崔璐璐.基于 MEMS器件的姿態(tài)測量系統(tǒng)研究與實現[D].大連理工大學,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]張振友,李明,田應仲.物體空間姿態(tài)的實施測量方法研究[J].機械設計與研究，2008,(5):83-87.

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

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