劉慶鵬，于 雨，惠 力，魯成杰

(1.齊魯工業(yè)大學(xué)(山東省科學(xué)院)，濟(jì)南 250353；2.齊魯工業(yè)大學(xué)(山東省科學(xué)院)，山東省科學(xué)院海洋儀器儀表研究所，青島 266100)

0 引言

機(jī)械式海流儀經(jīng)歷了幾代發(fā)展[1]，在其姿態(tài)角數(shù)據(jù)測(cè)量中，傳感器測(cè)量的數(shù)據(jù)經(jīng)算法處理后，變?yōu)閷?shí)時(shí)的角度顯示。而角度融合算法在工程上有幾種不同的方法[2]，采用歐拉角微分方程式需要進(jìn)行三角函數(shù)的運(yùn)算，且方程會(huì)出現(xiàn)“奇點(diǎn)”，方程式退化；用方向余弦法計(jì)算姿態(tài)矩陣，沒(méi)有方程退化問(wèn)題，但需要求解6個(gè)微分方程，計(jì)算量較大，影響嵌入式系統(tǒng)的響應(yīng)時(shí)間。文章采用四元數(shù)微分方程式，計(jì)算量不大且沒(méi)有“奇點(diǎn)”問(wèn)題，并在實(shí)驗(yàn)基礎(chǔ)上對(duì)算法可行性進(jìn)行了驗(yàn)證。

1 姿態(tài)解算原理

1.1 常用的坐標(biāo)系

為了求解機(jī)械海流計(jì)的姿態(tài)角信息，引入北東地坐標(biāo)系作為地理坐標(biāo)系[3]。具體來(lái)說(shuō)，OXg指向北方，OYg指向東方，OZg垂直指向地面。由機(jī)械海流計(jì)的縱軸OYb、橫軸OXb和豎軸OZb所構(gòu)成的右手系作為載體坐標(biāo)系，將兩個(gè)坐標(biāo)系的原點(diǎn)定在機(jī)械海流計(jì)的質(zhì)心，解算出地理坐標(biāo)系和載體坐標(biāo)系之間的角度關(guān)系即可求得機(jī)械海流計(jì)的姿態(tài)信息。

1.2 四元數(shù)解算原理

四元數(shù)是一種超復(fù)數(shù)，其形式可以表示為a+bi+cj+dk，有1個(gè)實(shí)部和3個(gè)虛部，其中a，b，c，d都是實(shí)數(shù)[4]。四元數(shù)可以用來(lái)描述機(jī)械海流計(jì)相對(duì)于地理坐標(biāo)系的方向矢量，其中實(shí)部表示轉(zhuǎn)角大小，虛部表示轉(zhuǎn)動(dòng)方向。通過(guò)四元數(shù)可以求出一個(gè)物體相對(duì)參考坐標(biāo)系旋轉(zhuǎn)之后的坐標(biāo)信息，參考坐標(biāo)系和旋轉(zhuǎn)后的坐標(biāo)系的轉(zhuǎn)換關(guān)系用變換矩陣式(1)來(lái)解算。

(1)

其中，q0代表實(shí)部；q1、q2、q3代表虛部。

物體旋轉(zhuǎn)后相對(duì)參考坐標(biāo)系的角度變化信息也可以通過(guò)繞X軸旋轉(zhuǎn)一個(gè)角度(橫滾角)θ，繞Y軸旋轉(zhuǎn)一個(gè)角度(俯仰角)γ和繞Z軸旋轉(zhuǎn)一個(gè)角度(偏航角)φ來(lái)表示。3次旋轉(zhuǎn)過(guò)程可由姿態(tài)角變換矩陣來(lái)表示，姿態(tài)變換矩陣如式(2)所示。

(2)

式(1)和式(2)均為物體從參考坐標(biāo)系變換到載體坐標(biāo)系，所以?xún)蓚€(gè)變換矩陣相等。由此可建立起四元數(shù)組成的矩陣與方向余弦矩陣之間的關(guān)系，可以通過(guò)反三角函數(shù)求取3個(gè)歐拉角[5]，公式如下：

橫滾角：

(3)

俯仰角：

γ=-arctan(2(q1q3+q0q2))

(4)

偏航角：

(5)

只需要知道四元數(shù)的4個(gè)參數(shù)，就能解算出物體的姿態(tài)角。

海流計(jì)處于動(dòng)態(tài)之中，故四元數(shù)是一個(gè)隨時(shí)間變化的函數(shù)。建立四元數(shù)微分方程如下：

(6)

式(6)中，d0、d1、d2、d3表示更新后的四元數(shù)；ωx、ωy、ωz是由陀螺儀測(cè)量得到的機(jī)械海流計(jì)繞載體坐標(biāo)系三軸旋轉(zhuǎn)的角速率。實(shí)際計(jì)算中將四元數(shù)微分方程離散化，使用一階龍格-庫(kù)塔法得到更新后的四元數(shù)，則有：

(7)

式中，T表示姿態(tài)解算當(dāng)前時(shí)刻，T+ΔT表示姿態(tài)解算下一時(shí)刻。從式(7)可看出，理論上只需要得到3個(gè)繞軸角速率便可以通過(guò)四元數(shù)法求出物體的歐拉角。而MEMS陀螺儀根據(jù)角動(dòng)量守恒原理可以精確地測(cè)量角速率，但由于溫度變化、摩擦力、不穩(wěn)定力矩等因素，陀螺儀會(huì)產(chǎn)生漂移誤差，通過(guò)積分會(huì)使誤差變大。該誤差可以通過(guò)加速度計(jì)進(jìn)行矯正，當(dāng)加速度計(jì)在沒(méi)有加速度的狀態(tài)下，加速度計(jì)的輸出是一個(gè)幅度為1 G地球表面的法向量。通過(guò)該法向量，可以計(jì)算出羅盤(pán)相對(duì)于地球表面的俯仰角和橫滾角，利用這兩個(gè)角可以在機(jī)械海流計(jì)近乎勻速的情況下修正陀螺儀產(chǎn)生的累計(jì)誤差。文章采用六軸的處理組件MPU6050(包括三軸加速度計(jì)和三軸陀螺儀)，通過(guò)四元數(shù)算法對(duì)HMC5883L得到的流向進(jìn)行傾角誤差補(bǔ)償。

2 測(cè)量電路

測(cè)量電路采用InvenSence公司的MPU-6050六軸傳感器芯片和Honeywell公司的HMC5883L磁阻傳感器芯片[6]，將二者組合相連用以測(cè)量加速度數(shù)據(jù)A、陀螺儀數(shù)據(jù)G和地磁場(chǎng)數(shù)據(jù)M。姿態(tài)解算電路如圖1所示。

圖1 姿態(tài)解算電路

陀螺儀和加速度計(jì)都集成在MPU6050傳感器芯片上，其測(cè)量范圍達(dá)到±16 g。芯片正常工作時(shí)，MPU6050采用I2C接口與控制芯片連接，SDA和SCL信號(hào)線需要接入4.7 kΩ的上拉電阻以增加驅(qū)動(dòng)能力。讀取芯片數(shù)據(jù)首先要進(jìn)行初始化，需要配置電源管理，陀螺儀采樣頻率、低通濾波頻率、陀螺儀自檢及測(cè)量范圍和加速度計(jì)自檢、測(cè)量范圍及高通濾波頻率。初始化后，陀螺儀采集科里奧利力在3個(gè)軸上形成的電壓值，加速度計(jì)采集1 G的地球表面法向量在各軸的分量形成的電壓值，經(jīng)過(guò)16位A/D轉(zhuǎn)換器轉(zhuǎn)換為數(shù)字信號(hào)，利用預(yù)設(shè)的比例關(guān)系將其換算成實(shí)際的角速度值和角度值。

HMC5883L三軸磁場(chǎng)強(qiáng)度敏感芯片內(nèi)置ASIC放大器，工作頻率達(dá)160 Hz，羅盤(pán)航行精度精確至1°～2°，同樣使用I2C總線輸出。芯片的SCL和SDA分別接到MPU6050的AUX_CL和AUX_DA，同時(shí)接入4.7 kΩ的電阻來(lái)增加驅(qū)動(dòng)能力。磁阻傳感器主要用于測(cè)量地磁場(chǎng)矢量，定位海流計(jì)的方向(定位流向)。

3 具體姿態(tài)解算過(guò)程

3.1 三軸磁羅盤(pán)組件HMC5883L測(cè)流向

帶有磁阻傳感器的線路板封裝在機(jī)械海流計(jì)旋槳?dú)んw內(nèi)，安裝時(shí)將X軸指向旋槳方向，Y軸指向旋槳右側(cè)，Z軸垂直于其他兩軸指向海底。海流計(jì)尾翼隨海流方向的變化而變化，旋槳相應(yīng)調(diào)整方向并指向海流方向。磁阻傳感器感應(yīng)地磁場(chǎng)變化，采集各軸方向上的磁場(chǎng)強(qiáng)度。

磁場(chǎng)水平分量HN與磁場(chǎng)垂直分量Hz的矢量和表征地球磁場(chǎng)強(qiáng)度[7]。磁場(chǎng)水平分量HN總是指向地球磁場(chǎng)北極，利用該特性，若想測(cè)出流向，只需要測(cè)出旋槳方向(載體坐標(biāo)系X軸和磁場(chǎng)水平分量之間的夾角)即可。磁阻傳感器感應(yīng)到X軸的磁場(chǎng)強(qiáng)度為Hx，Y軸的磁場(chǎng)強(qiáng)度為Hy，其矢量和為地磁場(chǎng)水平分量HN。地磁場(chǎng)水平分量HN與X軸之間的夾角即為方位角α，已知方位角即可知流向[8](圖2)。

圖2 磁阻傳感器處于水平狀態(tài)的坐標(biāo)系

根據(jù)圖2，由三角函數(shù)可求得方位角為[9]：

(8)

3.2 MPU6050修正流向

在實(shí)際應(yīng)用中，磁阻傳感器坐標(biāo)系和海流計(jì)載體坐標(biāo)系重合，假設(shè)載體坐標(biāo)系和地理坐標(biāo)系重合，則海流計(jì)測(cè)得的磁場(chǎng)強(qiáng)度HB為：

(9)

式(9)中，HN為地磁場(chǎng)水平分量，其方向與載體坐標(biāo)系X軸重合，在Y軸上的分量為0；Hz為地磁場(chǎng)垂直分量，其方向與Z軸重合。

(10)

(11)

式(11)中，q0、q1、q2、q3是MPU6050所測(cè)數(shù)據(jù)更新后的四元數(shù)(標(biāo)量)；Hbx、Hby和Hbz分別為當(dāng)?shù)氐卮艔?qiáng)度在3個(gè)軸上的磁場(chǎng)分量。通過(guò)式(3)和式(4)可得X軸與水平面之間的俯仰角γ，Z軸與過(guò)X軸的鉛垂平面之間的橫滾角θ，結(jié)合磁阻傳感器測(cè)出的地磁場(chǎng)在三軸的方向分量，重新計(jì)算得到水平方向分量Hx2和Hy2計(jì)算其方位角。由坐標(biāo)關(guān)系得水平方向磁分量：

(12)

將Hx2，Hy2帶入式(8)計(jì)算即可得到方位角。

由于磁力線在地球表面分布的大小和方向存在差異，所以地球南北極和地磁南北極之間的磁偏角也是因地而異。要得到真方位角，只需在計(jì)算出的方位角基礎(chǔ)上加上或減去當(dāng)?shù)氐拇牌恰２槌鏊诤Ｓ虻拇牌铅?，則真方位角ρ為：

ρ=α+β

(13)

3.3 軟件實(shí)現(xiàn)

文章利用C語(yǔ)言編寫(xiě)軟件程序，且采用調(diào)試更為方便的模塊化設(shè)計(jì)，以便快速獲取海流流向信息。軟件功能主要包括初始化系統(tǒng)參數(shù)、磁場(chǎng)數(shù)據(jù)和姿態(tài)角數(shù)據(jù)的采集、方位角的解算、磁偏角矯正以及真北顯示輸出等，軟件處理流程如圖3所示。

圖3 軟件處理流程

4 調(diào)試與實(shí)驗(yàn)

為驗(yàn)證文章所采用的姿態(tài)解算算法的有效性，對(duì)以姿態(tài)算法為原理搭建的機(jī)械海流計(jì)進(jìn)行測(cè)試。實(shí)驗(yàn)的主控制器選用STM32F103RCT6，姿態(tài)檢測(cè)單元選用MPU6050和磁阻傳感器HMC5883L，配合CAN總線收發(fā)模塊及最小系統(tǒng)模塊完成對(duì)姿態(tài)數(shù)據(jù)的采集、處理、通訊以及顯示。

實(shí)驗(yàn)步驟如下：

1)在保持海流計(jì)水平狀態(tài)下，使其做原地旋轉(zhuǎn)運(yùn)動(dòng)，取定點(diǎn)測(cè)量，并進(jìn)行數(shù)據(jù)的采集和解算；

2)對(duì)海流計(jì)進(jìn)行人為擾動(dòng)，在海流計(jì)不能保持水平狀態(tài)的情況下，使其做原地旋轉(zhuǎn)運(yùn)動(dòng)，取定點(diǎn)測(cè)量，并進(jìn)行數(shù)據(jù)的采集和解算。

經(jīng)多次測(cè)量，發(fā)現(xiàn)在定點(diǎn)處測(cè)量的數(shù)據(jù)在兩種狀態(tài)下得到的流向基本一致。說(shuō)明使用該算法能夠很好地處理海流計(jì)在不平衡狀態(tài)下的傾角補(bǔ)償問(wèn)題，能夠測(cè)出準(zhǔn)確的流向數(shù)據(jù)。

5 結(jié)束語(yǔ)

文章利用磁阻傳感器HMC5883L測(cè)出海流計(jì)的流向，分析了方向余弦與四元數(shù)之間一一對(duì)應(yīng)的關(guān)系，并用MPU6050結(jié)合四元數(shù)算法解算出機(jī)械海流計(jì)的姿態(tài)角，用姿態(tài)角對(duì)海流計(jì)流向進(jìn)行修正。文章結(jié)合實(shí)驗(yàn)驗(yàn)證了四元數(shù)法在姿態(tài)解算中的應(yīng)用，通過(guò)設(shè)計(jì)海流計(jì)的姿態(tài)解算模塊，給出了四元數(shù)具體的應(yīng)用過(guò)程。為海流計(jì)在任意姿態(tài)下進(jìn)行流向測(cè)量提供了保障。