張振濤,王 娟

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

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

1 測量方法

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

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

(1)

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

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

(2)

(3)

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

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

2 測量電路

圖1 測量電路圖

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

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

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

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

在MCU也很容易實(shí)現(xiàn)，首先定義四個(gè)浮點(diǎn)數(shù)q1、q2、q3、q4，將經(jīng)過濾波后的數(shù)據(jù)單位化，接下來估計(jì)重力方向，同時(shí)將把四元數(shù)換算成方向余弦矩陣中第3列的3個(gè)元素，同樣需要定義浮點(diǎn)數(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是固定坐標(biāo)參照系上加速度計(jì)測出來的重力方向的向量，vx、vy、vz是陀螺儀對角速度積分后的重力方向的向量，它們之間存在的誤差可以用向量叉積來表示，ex、ey、ez就是兩個(gè)重力向量的叉積，這個(gè)叉積向量仍舊是位于坐標(biāo)系上的，陀螺積分誤差也是基于這個(gè)坐標(biāo)系，叉積的大小與陀螺積分誤差正好成正比，故拿來糾正陀螺[8].根據(jù)MPU6050的讀取速度跟單片機(jī)的運(yùn)算速度需要再定義delta_2和FACTOR，用來校正陀螺儀測量值，同時(shí)用叉積誤差來做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ù)后，換成當(dāng)前的歐拉角.

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；

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

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

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

圖2 測量實(shí)物圖

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

圖3 模型顯示界面

5 結(jié) 論

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