一種角度融合算法在姿態(tài)角解算中的應(yīng)用

張振濤,王 娟

(1.吉林化工學院 信息與控制工程學院，吉林 吉林 132022；2.吉林工業(yè)職業(yè)技術(shù)學院 電氣與信息技術(shù)學院，吉林 吉林 132013)

水準儀從過去的氣泡式到現(xiàn)在的電子式經(jīng)歷了幾代的改進[1]，在其姿態(tài)角數(shù)據(jù)測量中，傳感器測量的數(shù)據(jù)需要進行標度轉(zhuǎn)換成實時的角度顯示.而角度融合算法在工程上的實現(xiàn)有不同的方法，采用歐拉角微分方程式需要進行三角函數(shù)的運算，且方程會出現(xiàn)“奇點”，方程式退化，所以不能全姿態(tài)工作[2]；用方向余弦法計算姿態(tài)矩陣，沒有方程退化問題，可以滿足全姿態(tài)工作，但是需要求解六個微分方程，計算量較大，影響嵌入式系統(tǒng)的響應(yīng)時間[3]；Rodrigue參數(shù)法計算效率較高，且結(jié)構(gòu)簡單直觀，但是存在旋轉(zhuǎn)角有奇異值的缺陷[4].采用四元數(shù)微分方程式，通過建立水平儀達到嵌入式系統(tǒng)，驗證該方法在該系統(tǒng)的應(yīng)用.

1 測量方法

四元數(shù)微分方程式是慣性導航技術(shù)中常用的數(shù)學工具[5].四元數(shù)是簡單的超復數(shù)，由一個實部加上單個虛部構(gòu)成，可以表示為a+bi+cj+dk，其中a、b、c、d都是實數(shù).在慣性系統(tǒng)中，在一個三維空間內(nèi)用四元數(shù)描述目標相對一個固定坐標系的方向矢量，其中標量部分代表了轉(zhuǎn)角一半的余弦值，也就是轉(zhuǎn)角的大小，而矢量部分表示相對于坐標軸的轉(zhuǎn)動方向，也就是瞬時轉(zhuǎn)動軸和坐標系之間的方向余弦值.

可以通過四元數(shù)求出一個物體相對于參考坐標系旋轉(zhuǎn)之后的坐標信息，那么參考坐標系和旋轉(zhuǎn)后的坐標系的轉(zhuǎn)換關(guān)系用變換矩陣式(1)來解算.

(1)

其中q1是四元數(shù)的實部，q2、q3、q4是四元數(shù)的虛部.

物體旋轉(zhuǎn)過后相對參考坐標系的角度變化信息可以通過復合繞X軸旋轉(zhuǎn)稱物體的翻滾角(θ)、繞Y軸旋轉(zhuǎn)稱物體的俯仰角(γ)、繞Z軸旋轉(zhuǎn)稱物體的航向角(ψ)得到三個姿態(tài)角的變換矩陣復合得到物體相對于參考坐標系的角度信息，姿態(tài)角變換矩陣如式(2)所示.

(2)

(3)

式(1)物體轉(zhuǎn)動的姿態(tài)變換矩陣和式(3)物體繞三軸旋轉(zhuǎn)復合的姿態(tài)角矩均為參考坐標系變換到物體坐標系的姿態(tài)矩陣，所以兩變換矩陣相等，也就是兩個變換矩陣元素一一對應(yīng)相等[6].取第3行3個元素分別為g1、g2、g3和第1列第2個元素為g4，第1列第1個元素為g5則有：

根據(jù)以上方程組，知道了q1、q2、q3、q4的具體數(shù)值，就能求出需要的姿態(tài)角.

2 測量電路

圖1 測量電路圖

MPU6050使用I2C或者SPI接口和芯片連接，并且總是作為從設(shè)備.本系統(tǒng)采用I2C接口，而且使用模擬I2C協(xié)議，因此單片機引腳的驅(qū)動能力一般不夠，所以SDA和SCL信號線通常需要接上拉電阻到VDD來增加驅(qū)動能力，如圖1所示.讀取芯片的數(shù)據(jù)先要進行初始化，需要配置電源管理、陀螺儀采樣頻率、低通濾波頻率、陀螺儀自檢及測量范圍和加速計自檢、測量范圍及高通濾波頻率.初始化之后就可以讀取各軸的數(shù)據(jù)，每軸的數(shù)據(jù)是16位，分高8位和低8位，高字節(jié)地址在前，低字節(jié)地址在后.

從MPU6050讀出的數(shù)據(jù)要進行卡爾曼濾波之后將數(shù)據(jù)進行矩陣運算得到四元數(shù)[7]，再將四元數(shù)通過反三角函數(shù)計算得到當期那的角度.

3 軟件實現(xiàn)

如果現(xiàn)在已知MPU6050測得的角速度和加速度，那么需要求解一個四元數(shù)微分方程，通過求解微分方程，就可以得到我們需要的四元數(shù)參數(shù).求解過程要對角速度進行積分，積分過程中角速度誤差會加大，所以我們需要用加速度信息來校正角度信息，從而減小誤差.

在MCU也很容易實現(xiàn)，首先定義四個浮點數(shù)q1、q2、q3、q4，將經(jīng)過濾波后的數(shù)據(jù)單位化，接下來估計重力方向，同時將把四元數(shù)換算成方向余弦矩陣中第3列的3個元素，同樣需要定義浮點數(shù)類型的vx、vy、vz、ex、ey、ez.

vx=2*(q1q3+q0q2)；

vy=2*(q0q1+q2q3)；

vz=q0q0-q1q1-q2q2+q3q3；

ex=(ay*vz-az*vy)；

ey=(az*vx-ax*vz)；

ez=(ax*vy-ay*vx)；

其中ax、ay、az是MPU6050讀取的數(shù).ax、ay、az是固定坐標參照系上加速度計測出來的重力方向的向量，vx、vy、vz是陀螺儀對角速度積分后的重力方向的向量，它們之間存在的誤差可以用向量叉積來表示，ex、ey、ez就是兩個重力向量的叉積，這個叉積向量仍舊是位于坐標系上的，陀螺積分誤差也是基于這個坐標系，叉積的大小與陀螺積分誤差正好成正比，故拿來糾正陀螺[8].根據(jù)MPU6050的讀取速度跟單片機的運算速度需要再定義delta_2和FACTOR，用來校正陀螺儀測量值，同時用叉積誤差來做PI修正陀螺零偏，最后整合四元數(shù)率，利用四元數(shù)微分方程，使用的是二階畢卡法.

delta_2=(2*halfT*gx)(2*halfT*gx)+(2*halfT*gy)(2*halfT*gy)+(2*halfT*gz)(2*halfT*gz)；

q0=(1-delta_2/8)q0+(-q1*gx-q2*gy-q3*gz)halfT；

q1=(1-delta_2/8)q1+(q0*gx+q2*gz-q3*gy)halfT；

q2=(1-delta_2/8)q2+(q0*gy-q1*gz+q3*gx)halfT；

q3=(1-delta_2/8)q3+(q0*gz+q1*gy-q2*gx)halfT；

其中g(shù)x、gy、gz是MPU6050讀取的數(shù).正?；脑獢?shù)后，換成當前的歐拉角.

Q_ANGLE.Y=asin(-2*q1*q3+2*q0*q2)*57.3；

Q_ANGLE.X=atan2(2*q1*q2+2*q0*q3,-2*q1*q1-2*q2*q2+1)*57.3；

Q_ANGLE.Z=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1)*57.3；

到此就完成了從加速度計和陀螺儀的數(shù)據(jù)向角度的轉(zhuǎn)換，本實驗裝置只用到了其中兩個角度X和Y，只需要計算其中兩個.

4 調(diào)試與實驗

實驗由STM32單片機做主控芯片，配合MPU6050傳感器、無線收發(fā)模塊、液晶顯示模塊、上位機通訊電路和電源完成對數(shù)據(jù)的采集、處理、無線通訊以及顯示，實物如圖2所示.

圖2 測量實物圖

下位機通過串口將數(shù)據(jù)傳送到上位機，本系統(tǒng)上位機編程環(huán)境是VC++2010[9]，并且使用OpenGL設(shè)計3D模型.上位機對數(shù)據(jù)進行處理在界面顯示.程序運行時，首先點擊連接設(shè)備，成功后會有彈窗提示，串口號和波特率都是規(guī)定好的，當然在程序里可以改，連接成功后X和Y的數(shù)值會隨著測量模塊的變化而不斷變化，點擊立體圖就會彈出另一個界面，如圖3所示，它是將當前的角度用3D模型展現(xiàn)出來.

圖3 模型顯示界面

5 結(jié) 論

結(jié)合實際驗證了四元數(shù)微分方程這種角度融合算法在姿態(tài)角解算中的應(yīng)用，通過設(shè)計水準儀的測量裝置，測量了平面的X軸和Y軸的角度，并且給出了四元數(shù)微分方程具體應(yīng)用過程，減少了工程設(shè)計中工作量，簡化了編程，而且提高了MCU的代碼運行效率，以及水準儀的精度和穩(wěn)定性，優(yōu)化了顯示界面，使顯示不再固定，整個界面顯得更加美觀.