裴國棟，溫亞楠，朱莉莉

（青島市光電工程技術研究院，山東青島 266019）

0 引言

利用微機械結構來測量載體相對于空間運動數(shù)據(jù)的傳感器成為慣性傳感器[1]，通過空間距離的變化將運動數(shù)據(jù)轉(zhuǎn)化為模擬電子數(shù)據(jù)，然后通過相關算法計算后最終得到導航數(shù)據(jù)。慣性導航近年來被廣泛應用于航海、航空、各種平臺的穩(wěn)定與控制以及無人機的的姿態(tài)以及導航控制、軍工產(chǎn)業(yè)等各個領域。由于該技術應用廣泛，所以對國民經(jīng)濟的發(fā)展有著突出的貢獻。

慣性導航主要是通過慣性傳感器來實現(xiàn)的,現(xiàn)如今慣性傳感器主要包括加速度計與陀螺儀，加速度計可以測量沿某個軸向的加速度值，陀螺儀可以測量沿某個軸向的角速度值，因為這兩種傳感器的工作原理的不同，所以它們數(shù)據(jù)的應用方式也不同[2]。由于陀螺儀數(shù)據(jù)得到姿態(tài)角需要做積分運算，所以陀螺儀數(shù)據(jù)中的誤差會隨時間的增加而逐漸增大；加速度計由于內(nèi)部微機械結構的限制較適合靜態(tài)的姿態(tài)檢測，動態(tài)時數(shù)據(jù)不穩(wěn)定。因此需要一種合適的算法將兩種數(shù)據(jù)融合在一起，使融合后的數(shù)據(jù)更接近真實姿態(tài)數(shù)據(jù)[3]。

1 系統(tǒng)硬件設計

根據(jù)系統(tǒng)需求以及各方面綜合考量本系統(tǒng)選用 ST的增強型處理器 STM32f103C8T6芯片作為主處理器芯片，負責從慣性傳感器采集、處理、合成數(shù)據(jù)，芯片內(nèi)置 64KB 的閃存存儲器，最高時鐘頻率可72 MHz。慣性傳感器選擇MPU6050，該傳感器是 InvenSense 公司生產(chǎn)的全球首列整合性六軸運動處理組件，整合了三軸陀螺儀、三軸加速度計，內(nèi)置的數(shù)字運動處理(DMP: Digital Motion Processor)。使用非常方便[4]。其使用原理圖如圖1所示。

圖1 MPU6050原理圖

2 數(shù)據(jù)融合與姿態(tài)解算

2.1 四元數(shù)與歐拉角

四元數(shù)的組成是實數(shù)加上三個復數(shù)元素i、j、k，各個復數(shù)之間的關系如下：

每個四元數(shù)都是1、i、j、k的線性組合，所以四元數(shù)可表示如下：

歐拉角由章動角θ、進動角ψ和自轉(zhuǎn)角φ組成，是用來表示定點轉(zhuǎn)動的物體位置的三個參數(shù)。歐拉角最先由L.歐拉首先提出，一個繞點轉(zhuǎn)動的物體歐拉角有多種取法，以下是其中常用的一種表示法[5]。

圖2

圖2所示，為歐拉角按照z-y-x旋轉(zhuǎn)次序的一種表示法，ψ、θ、φ分別為物體繞 z軸、y軸以及x軸的角度，若用Tait-bryan angle來表示為：Yaw（偏航角），Roll（橫滾角），Pitch（俯仰角）。

2.2 數(shù)據(jù)的采集處理

采集的數(shù)據(jù)在進行 Kalman濾波時，需要采用陀螺儀以及加速度計的輸出數(shù)據(jù)來構造合適的狀態(tài)方程以及測量方程。Kalman是一種遞歸的估計濾波算法，只需要記錄上一時刻狀態(tài)的估計值以及解算出當前狀態(tài)的觀測值就可以解算出當前狀態(tài)的估計值。公式（3）是卡爾曼濾波器的一個核心公式，計算得到的K(n)是估計誤差占總誤差的權重，H(n)為觀測矢量[6]。

2.3 姿態(tài)融合算法

第一步、首先對加速度計數(shù)值進行歸一化：(注：Aac_x、Aac_y、Aac_z分別為 X、y、z軸加速度計數(shù)值;)

第二步、根據(jù)四元數(shù)計算法計算出重力在當前三個軸上的加速度分量：

第三步、根據(jù)向量積計算測得的重力與計算的重力間的誤差：

第四步、對誤差進行積分計算：

第五步、將積累的誤差經(jīng)過PI算法后補償?shù)酵勇輧x，即補償零點漂移，由于z軸沒有觀測者進行矯正會產(chǎn)生持續(xù)漂移[7][8]：

第六步、采用一介畢卡解法解算四元數(shù)，并保存歷史值待用（q0temp為q0歷史值，其他類似）：

第七部、單位化四元數(shù)，使其在空間旋轉(zhuǎn)時不會被拉伸變換：

第八步、四元數(shù)轉(zhuǎn)換到歐拉角完成數(shù)據(jù)融合：

3 結論

融合后的角度曲線上傳至上位機顯示如圖 3，其中包含9個數(shù)據(jù)，分別是三軸角速度值，三軸角加速度值以及融合后的三軸角度，其中曲線1為X軸角度曲線，曲線2為X軸陀螺儀曲線，由于加速度計數(shù)據(jù)太小圖中無法展示出來，在周期性搖晃X軸的過程中，角度曲線緊跟靈敏度最高的角速度曲線，合成后的角度曲線無過沖現(xiàn)象，能緊跟平臺的角度變化。根據(jù)角度曲線所示水下姿態(tài)檢測系統(tǒng)已基本實現(xiàn)。

圖3 數(shù)據(jù)融合角度曲線

本課題所研制的水下 ROV姿態(tài)檢測系統(tǒng)，系統(tǒng)簡單實用，可以達到水下 ROV姿態(tài)控制對相關數(shù)據(jù)的要求。

參考文獻：

[1]陳哲. 捷聯(lián)慣導系統(tǒng)原理[M]. 北京: 宇航出版社,1996: 20-43.

[2]陳永冰, 鐘斌. 慣性導航原理[M]. 北京: 國防工業(yè)出版社, 1990: 14-21.

[3]于波, 陳云相, 郭秀中. 慣性技術[M]. 北京: 北京航空航天大學出版社, 1994: 33-52

[4]楊艷娟. 捷聯(lián)慣導系統(tǒng)關鍵技術研究[D]. 哈爾濱:哈爾濱工程大學, 2001: 19-54.

[5]盧大偉. 多傳感器數(shù)據(jù)融合方法在新型組合導航系統(tǒng)中的應用[D]. 南京: 東南大學, 2001.

[6]曾慶華, 張為華. IMEMS 速率陀螺芯片在MAV飛行控制系統(tǒng)中應用研究[J]. 測控技術, 2004, 23(2).

[7]張靜, 金志華. 空中平臺航姿參考系統(tǒng)的設計[J].中國慣性技術學報, 2004, 12(2): 47-52.

[8]秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導航原理[M]. 西安: 西北工業(yè)大學出版社, 1999, 7(3):17-21.