四元數(shù)姿態(tài)解析x

1、這個程序得到了四元數(shù)，怎么進(jìn)一步計算姿態(tài)YAW,ROLL,PITCH?/Headerfiles#include"AHRS.h"#include<math.h>/Definitions#defineKp2.0f/比例增益支配收斂率accelerometer/magnetometer#defineKi0.005f/積分增益執(zhí)政速率陀螺儀的銜接gyroscopeases#definehalfT0.5f/采樣周期的一半/Variabledefinitionsfloatq0=1,q1=0,q2=0,q3=0;/四元數(shù)的元素，代表估計方向floatexInt=0,eyInt

2、=0,ezInt=0;/按比例縮小積分誤差/=/Function/=voidAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz)floatnorm;floathx,hy,hz,bx,bz;floatvx,vy,vz,wx,wy,wz;floatex,ey,ez;/輔助變量，以減少重復(fù)操作數(shù)floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;fl

3、oatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=q3*q3;/測量正?；痭orm=sqrt(ax*ax+ay*ay+az*az);ax=ax/norm;ay=ay/norm;az=az/norm;norm=sqrt(mx*mx+my*my+mz*mz);mx=mx/norm;my=my/norm;mz=mz/norm;/計算參考磁通方向hx=2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);hy=2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+

4、2*mz*(q2q3-q0q1);hz=2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2);bx=sqrt(hx*hx)+(hy*hy);bz=hz;/估計方向的重力和磁通(V和W)vx=2*(q1q3-q0q2);vy=2*(q0q1+q2q3);vz=q0q0-q1q1-q2q2+q3q3;wx=2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2);wy=2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);wz=2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2);/錯誤是

5、跨產(chǎn)品的總和之間的參考方向的領(lǐng)域和方向測量傳感器ex=(ay*vz-az*vy)+(my*wz-mz*wy);ey=(az*vx-ax*vz)+(mz*wx-mx*wz);ez=(ax*vy-ay*vx)+(mx*wy-my*wx);/積分誤差比例積分增益exInt=exInt+ex*Ki;eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;/調(diào)整后的陀螺儀測量gx=gx+Kp*ex+exInt;gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt;/整合四元數(shù)率和正?；痲0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;q1=q1+(q0*

6、gx+q2*gz-q3*gy)*halfT;q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;/正?；脑猲orm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0=q0/norm;q1=q1/norm;q2=q2/norm;q3=q3/norm;/=/ENDOFCODE/=一種常見的四軸飛行器姿態(tài)解算方法分析作者：讓四軸飛時間：2014-06-04來源：電子產(chǎn)品世界全國各地已經(jīng)陸續(xù)開放低空管制，北京也將在2015年全面開放低空領(lǐng)域，這對低空飛行器將是一個十分重大的好消息!低空飛行器也將迎來一個新的發(fā)

7、展春天。實際上，近年四軸飛行器發(fā)展相當(dāng)迅速，國內(nèi)的航拍水平越來越高，順豐及亞馬遜已在嘗試將無人機(jī)用于快遞行業(yè)。越來越多的人開始關(guān)注并研究四軸飛行器。本文引用地址：本文將分析一種常見的四軸飛行器姿態(tài)解算方法，Mahony的互補(bǔ)濾波法。此法簡單有效，希望能給學(xué)習(xí)四軸飛行器的朋友們帶來幫助。關(guān)于姿態(tài)解算和濾波的理論知識，推薦秦永元的兩本書，一是慣性導(dǎo)航，目前已出到第二版了;二是卡爾曼濾波與組合導(dǎo)航原理。程序中的理論基礎(chǔ)，可在書中尋找。同時歡迎到論壇發(fā)帖交流：下面開始進(jìn)入正題：先定義Kp，Ki，以及halfT。Kp，Ki，控制加速度計修正陀螺儀積分姿態(tài)的速度halfT，姿態(tài)解算時間的一半。此處解算姿態(tài)

8、速度為500HZ，因此halfT為0.001#defineKp2.0f#defineKi0.002f#definehalfT0.001f初始化四元數(shù)floatq0=1,q1=0,q2=0,q3=0;定義姿態(tài)解算誤差的積分floatexInt=0,eyInt=0,ezInt=0;以下為姿態(tài)解算函數(shù)。參數(shù)gx，gy，gz分別對應(yīng)三個軸的角速度，單位是弧度/秒;參數(shù)ax，ay，az分別對應(yīng)三個軸的加速度原始數(shù)據(jù)由于加速度的噪聲較大，此處應(yīng)采用濾波后的數(shù)據(jù)voidIMUupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz)floatnorm;fl

9、oatvx,vy,vz;floatex,ey,ez;將加速度的原始數(shù)據(jù)，歸一化，得到單位加速度norm=sqrt(ax*ax+ay*ay+az*az);ax=ax/norm;ay=ay/norm;az=az/norm;把四元數(shù)換算成“方向余弦矩陣”中的第三列的三個元素。根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量，轉(zhuǎn)到機(jī)體坐標(biāo)系，正好是這三個元素。所以這里的vx、vy、vz，其實就是當(dāng)前的機(jī)體坐標(biāo)參照系上，換算出來的重力單位向量。(用表示機(jī)體姿態(tài)的四元數(shù)進(jìn)行換算)vx=2*(q1*q3-q0*q2);vy=2*(q0*q1+q2*q3);vz=q0*q0-q1*q1-q2*q2+q3*q3

10、;這里說明一點(diǎn)，加速度計由于噪聲比較大，而且在飛行過程中，受機(jī)體振動影響比陀螺儀明顯，短時間內(nèi)的可靠性不高。陀螺儀噪聲小，但是由于積分是離散的，長時間的積分會出現(xiàn)漂移的情況，因此需要將用加速度計求得的姿態(tài)來矯正陀螺儀積分姿態(tài)的漂移。在機(jī)體坐標(biāo)參照系上，加速度計測出來的重力向量是ax、ay、az;陀螺積分后的姿態(tài)來推算出的重力向量是vx、vy、vz;它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加速度計測出來的姿態(tài)之間的誤差。向量間的誤差，可以用向量積(也叫外積、叉乘)來表示，ex、ey、ez就是兩個重力向量的叉積。這個叉積向量仍舊是位于機(jī)體坐標(biāo)系上的，而陀螺積分誤差也是在機(jī)體坐標(biāo)系，而且叉積的大小

11、與陀螺積分誤差成正比，正好拿來糾正陀螺。由于陀螺是對機(jī)體直接積分，所以對陀螺的糾正量會直接體現(xiàn)在對機(jī)體坐標(biāo)系的糾正。叉乘是數(shù)學(xué)基礎(chǔ)，百度百科里有詳細(xì)解釋。ex=(ay*vz-az*vy);ey=(az*vx-ax*vz);ez=(ax*vy-ay*vx);將叉乘誤差進(jìn)行積分exInt=exInt+ex*Ki;eyInt=eyInt+ey*Ki;ezInt=ezInt+ez*Ki;用叉乘誤差來做PI修正陀螺零偏，通過調(diào)節(jié)Kp,Ki兩個參數(shù)，可以控制加速度計修正陀螺儀積分姿態(tài)的速度gx=gx+Kp*ex+exInt;gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt;四元數(shù)微分

12、方程，沒啥好說的了，看上面推薦的書吧，都是理論的東西，自個琢磨琢磨實在琢磨不明白，那就把指定的參數(shù)傳進(jìn)這個函數(shù)，再得到相應(yīng)的四元數(shù)，最后轉(zhuǎn)化成歐拉角即可了。不過建議還是把理論弄清楚一點(diǎn)。q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;四元數(shù)單位化norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0=q0/norm;q1=q1/norm;q2=q2/norm;q3=q3/norm;姿態(tài)解算后，就得到了表示姿態(tài)的四元數(shù)。但四元數(shù)不夠直觀，一般將其轉(zhuǎn)化為歐拉角。轉(zhuǎn)化時根據(jù)旋轉(zhuǎn)軸的次序不同，公式也不同