楊淑潔，曾慶雙，伊國興

（哈爾濱工業(yè)大學空間控制與慣性技術研究中心，黑龍江哈爾濱 150001）

0 引言

四旋翼微小飛行器的能量和帶載荷能力有限，要求姿態(tài)測量系統(tǒng)體積小、質量輕、功耗低。隨著MEMS慣性傳感器和嵌入式系統(tǒng)的發(fā)展，使得研制滿足這種要求的姿態(tài)測量系統(tǒng)成為可能［1］。但由于MEMS陀螺的噪聲和零漂相對于傳統(tǒng)的高精度傳感器要大很多，單獨使用無法滿足控制系統(tǒng)的精度要求。本文采用磁場、重力場測量的觀測量修正陀螺姿態(tài)解算的誤差，利用擴展Kalman濾波技術，對不同類型傳感器的數(shù)據(jù)進行數(shù)據(jù)融合，得到無人機動態(tài)的姿態(tài)信息。該算法綜合了陀螺的短時精度高、加速度/磁強計系統(tǒng)靜態(tài)精度高的優(yōu)點，實現(xiàn)低代價較高精度的姿態(tài)確定系統(tǒng)。設計的姿態(tài)測量精度要求:橫滾角與俯仰角的靜態(tài)輸出誤差小于1°;航向角的靜態(tài)輸出誤差小于2°;航向角的動態(tài)輸出誤差小于3°。

1 捷聯(lián)慣導系統(tǒng)中載體姿態(tài)的確定

捷聯(lián)慣導系統(tǒng)中，陀螺和加表直接固聯(lián)在運載體上，設有載體的機體軸確定的坐標系為b，慣導系統(tǒng)采用的導航坐標系為n，b系和n系都是直角坐標系，載體的姿態(tài)則是b系和n系直接的方位關系。姿態(tài)信息的捷聯(lián)解算在n系中完成，因此，首先要將固聯(lián)在載體上的傳感器測得的姿態(tài)參考向量轉換為n系下，這個轉換由b系到n系的坐標變換矩陣完成，就是捷聯(lián)矩陣，根據(jù)矩陣中的元素可以求取載體的姿態(tài)角［2］。

求取姿態(tài)角之前要定義機體坐標系和導航坐標系。本文的導航坐標系定義為東北天坐標系，原點選在無人機重心;機體坐標系原點選在無人機重心，xb沿機體橫軸指向右，yb沿機體縱軸指向前，zb沿機體的豎軸指向上，滿足右手定則。ψ，θ，φ分別為無人機的航向角，俯仰角和橫滾角，則由載體坐標系到導航坐標系轉換的姿態(tài)矩陣［3］為

在捷聯(lián)慣性導航系統(tǒng)中常用四元數(shù)法完成姿態(tài)解算。四元數(shù)法算法簡單、易于操作，而且可以避免歐拉角的奇異問題，是較實用的工程方法。設Q=q0+q1i+q2j+q3k為n系到b系的旋轉四元數(shù)，則姿態(tài)矩陣與姿態(tài)四元數(shù)的關系為

2 無人機姿態(tài)更新算法

由上文的分析可以看出:表征n系到b系的旋轉四元數(shù)Q包含了所有的姿態(tài)信息，由Q計算出姿態(tài)矩陣Cnb，從而確定載體的3個姿態(tài)角，因此，姿態(tài)解算實質上是計算四元數(shù)Q。

根據(jù)捷聯(lián)慣導系統(tǒng)的四元數(shù)理論

求解式（3）的微分方程即是對四元數(shù)Q進行更新。求解微分方程需要Q的初始值，開機后系統(tǒng)需要靜止幾秒鐘，利用重力場和磁場進行初始對準。

本文的姿態(tài)解算系統(tǒng)有2種工作模式。當載體靜止或勻速運動的時候，利用擴展Kalman濾波算法來求取姿態(tài)信息和更新姿態(tài)矩陣［4］，同時每周期檢測載體的運動狀態(tài)，當系統(tǒng)檢測到載體在進行加速或減速運動時，就在上周期加速度計解算值的基礎上，利用陀螺繼續(xù)解算，如果系統(tǒng)恢復勻速，就重新利用擴展Kalman濾波算法解算。測量系統(tǒng)的框圖見圖1。

圖1 測量系統(tǒng)框圖Fig 1 Block diagram of measuring system

判斷載體是否處于勻速運動狀態(tài)，利用加速度計三軸分量與重力加速度的關系來判定。在靜止或勻速運動的情況下，加速度計測得的值的平方和應該等于重力加速度的平方值

但在實際系統(tǒng)中由于加速度計的精度和噪聲問題，在判斷時不可能嚴格滿足式（5），而需要設置一個門限值σ，只要加速度計三軸測得的值的平方和與重力加速度平方值的差小于該門限值，則可視為滿足式（5）。

2.1 陀螺動態(tài)姿態(tài)解算

本文中使用的MEMS陀螺誤差較大，并不能敏感到地球的旋轉角速度，在算法設計中僅將陀螺儀的輸出誤差看成常值零偏與白噪聲之和，不考慮地球自轉。常值零偏在數(shù)據(jù)初始化時補償，式（3）中的直接由MEMS陀螺測量輸出去除零偏后獲得

此處采用四階龍格庫塔法直接求解微分方程˙Q=ΩbQ

2.2 擴展Kalman濾波姿態(tài)解算

采用擴展Kalman濾波器進行多傳感器信息融合，就是利用無時間累加誤差的重力場和磁場觀測量修正陀螺給出的姿態(tài)角［5］。

當載體在導航坐標系中靜止時，并忽略磁偏角，重力場和磁場在導航坐標系中的投影分別是an=［0 0g］'和mn=［mx0my0mz0］'為已知常量，其中，g為重力加速度。當載體處于任意姿態(tài)時，重力場和磁場在集團坐標系中的投影為ab=［axayaz］'和mb=［mxmymz］。

首先構造出一種適合本系統(tǒng)使用的Kalman濾波器模型，采集陀螺儀的數(shù)據(jù)通過四元數(shù)算法得到一組姿態(tài)角，將四元數(shù)的4個參數(shù)做為狀態(tài)變量

由式（3）得

由式（8）建立Kalman濾波的狀態(tài)方程

W（t）=［δxδyδz］'近似為零均值高斯白噪聲過程，其協(xié)方差矩陣Q（t）為非負定常值對角陣。

將方程（9）線性近似離散化，同時忽略高階小量得到

其中

接著建立觀測方程，取觀測量為

其中，θ1與φ1為通過一步預測后得到的俯仰角與橫滾角。

根據(jù)坐標變換關系得到

得到觀測方程為

其中，V為量測噪聲。

線性近似離散化后得到

忽略高階小量，得到系統(tǒng)量測矩陣H（k）

通過上述的分析，已經(jīng)得到針對本系統(tǒng)的狀態(tài)方程與觀測方程。根據(jù)擴展 Kalman濾波遞推方程［7］，建立Kalman濾波的時間傳播方程。

狀態(tài)一步預測

均方誤差一步預測

狀態(tài)估計

測量修正方程

3 仿真和實驗結果

本文討論的姿態(tài)測量系統(tǒng)的硬件組成包括:MIMU（包括三軸MEMS陀螺和三軸MEMS加速度計）、三軸磁強計和導航計算機，利用MIMU和磁強計感知無人機的運動，傳感器信號送至導航計算機，進入姿態(tài)解算工作程序。為了驗證姿態(tài)解算算法的有效性，本文進行了3個實驗。在利用實驗室現(xiàn)有的三軸轉臺實驗時，由于磁強計易受金屬臺體的磁場影響，在轉臺上進行實驗時，采用轉臺的姿態(tài)角信號進行信息融合，橫滾角和俯仰角保持為零，只能利用偏航角進行比較。

圖2的結果是將姿態(tài)測量系統(tǒng)置于零位，未采用擴展Kalman濾波器，只利用MEMS陀螺輸出計算出的3個姿態(tài)角，經(jīng)過80 s的測試，由于陀螺漂移的存在，可以看出:3個姿態(tài)角是發(fā)散的。圖3的結果是將橫滾角和俯仰角保持為0°，將偏航角轉至3.5°，并處于靜止狀態(tài)測試60s，3 個姿態(tài)角保持在初始值，3個姿態(tài)角的誤差均小于0.35°。圖4的結果是將橫滾角和俯仰角保持為0°，將轉臺偏航角設置為0.25 Hz的正弦信號，擴展Kalman濾波器解算出的偏航角能夠很好地跟隨轉臺信號，動態(tài)誤差小于3°，滿足設計要求。

4 結論

本文介紹了一種適合微小型無人機使用的姿態(tài)測量系統(tǒng)，將MIMU和磁強計的傳感器信號采用擴展Kalman濾波進行信息融合，很好地抑制了MEMS陀螺的漂移。實驗表明:3個姿態(tài)角的靜態(tài)誤差小于0.5°，航向角的動態(tài)誤差小于3°，靜態(tài)、動態(tài)精度都能滿足無人機對姿態(tài)精度的要求。該系統(tǒng)具有成本低、質量輕、體積小的優(yōu)點。

圖2 陀螺姿態(tài)解算Fig 2 Gyro attitude solution

圖3 Kalman濾波靜態(tài)姿態(tài)角輸出Fig 3 Attitude angle static output with Kalman filter

圖4 Kalman濾波動態(tài)姿態(tài)角輸出Fig 4 Attitude angle dynamic output with Kalman filter

［1］朱 榮.周兆英.基于MENS的姿態(tài)測量系統(tǒng)［J］.測控技術，2002，21（10）:6 －8.

［2］陳 哲.捷聯(lián)慣導系統(tǒng)原理［M］.北京:宇航出版社，1996:82－85.

［3］秦永元.慣性導航［M］.北京:科學出版社，2006:297.

［4］黃麗斌，周百令.低成本MINS/GPS組合導航中卡爾曼濾波算法的綜合應用研究［J］.中國慣性技術學報，2005，13（5）:16－19.

［5］Gao Z Y，Niu X J，Guo M F.Quaternion-based Kalman filter for micro-machined strapdown attitude heading reference system［J］.Chinese Journal of Aeronautics，2002，15（3）:171 －175.

［6］Caruso M J.Applications of magnetoresistive sensors in navigation system［J］.Transactions of SAE，1997，106（1）:1092 －1098.

［7］秦永元，張洪錢，汪叔華.卡爾曼濾波與組合導航原理［M］.西安:西北工業(yè)大學出版社，1998:283－286.