一種角度融合算法在姿態角解算中的應用

張振濤,王 娟

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

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

1 測量方法

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

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

(1)

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

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

(2)

(3)

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

根據以上方程組，知道了q1、q2、q3、q4的具體數值，就能求出需要的姿態角.

2 測量電路

圖1 測量電路圖

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

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

3 軟件實現

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

在MCU也很容易實現，首先定義四個浮點數q1、q2、q3、q4，將經過濾波后的數據單位化，接下來估計重力方向，同時將把四元數換算成方向余弦矩陣中第3列的3個元素，同樣需要定義浮點數類型的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讀取的數.ax、ay、az是固定坐標參照系上加速度計測出來的重力方向的向量，vx、vy、vz是陀螺儀對角速度積分后的重力方向的向量，它們之間存在的誤差可以用向量叉積來表示，ex、ey、ez就是兩個重力向量的叉積，這個叉積向量仍舊是位于坐標系上的，陀螺積分誤差也是基于這個坐標系，叉積的大小與陀螺積分誤差正好成正比，故拿來糾正陀螺[8].根據MPU6050的讀取速度跟單片機的運算速度需要再定義delta_2和FACTOR，用來校正陀螺儀測量值，同時用叉積誤差來做PI修正陀螺零偏，最后整合四元數率，利用四元數微分方程，使用的是二階畢卡法.

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；

其中gx、gy、gz是MPU6050讀取的數.正常化四元數后，換成當前的歐拉角.

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；

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

4 調試與實驗

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

圖2 測量實物圖

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

圖3 模型顯示界面

5 結 論

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