低成本無人機姿態測量系統研究

楊淑潔，曾慶雙，伊國興

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

0 引言

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

1 捷聯慣導系統中載體姿態的確定

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

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

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

2 無人機姿態更新算法

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

根據捷聯慣導系統的四元數理論

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

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

圖1 測量系統框圖Fig 1 Block diagram of measuring system

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

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

2.1 陀螺動態姿態解算

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

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

2.2 擴展Kalman濾波姿態解算

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

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

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

由式（3）得

由式（8）建立Kalman濾波的狀態方程

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

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

其中

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

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

根據坐標變換關系得到

得到觀測方程為

其中，V為量測噪聲。

線性近似離散化后得到

忽略高階小量，得到系統量測矩陣H（k）

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

狀態一步預測

均方誤差一步預測

狀態估計

測量修正方程

3 仿真和實驗結果

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

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

4 結論

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

圖2 陀螺姿態解算Fig 2 Gyro attitude solution

圖3 Kalman濾波靜態姿態角輸出Fig 3 Attitude angle static output with Kalman filter

圖4 Kalman濾波動態姿態角輸出Fig 4 Attitude angle dynamic output with Kalman filter

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

［2］陳 哲.捷聯慣導系統原理［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］.西安:西北工業大學出版社，1998:283－286.