用于水下ROV控制的姿態(tài)融合技術(shù)研究

裴國棟，溫亞楠，朱莉莉

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

0 引言

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

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

1 系統(tǒng)硬件設(shè)計(jì)

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

圖1 MPU6050原理圖

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

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

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

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

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

圖2

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

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

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

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

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

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

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

第四步、對誤差進(jìn)行積分計(jì)算：

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

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

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

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

3 結(jié)論

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

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

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

參考文獻(xiàn)：

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

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

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

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

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

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

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

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