姿態(tài)角（Euler角）pitch yaw roll

　　飛行器的姿態(tài)角并不是指哪個(gè)角度，是三個(gè)角度的統(tǒng)稱(chēng)。

　　它們是：俯仰、滾轉(zhuǎn)、偏航。你可以想象是飛機(jī)圍繞XYZ三個(gè)軸分別轉(zhuǎn)動(dòng)形成的夾角。

　　地面坐標(biāo)系（earth-surface inertial reference frame）Sg--------OXgYgZg

　　①在地面上選一點(diǎn)Og

　　②使Xg軸在水平面內(nèi)并指向某一方向

　　③Zg軸垂直于地面并指向地心（重力方向）

　　④Yg軸在水平面內(nèi)垂直于Xg軸，其指向按右手定則確定

　　機(jī)體坐標(biāo)系（Aircraft-body coordinate frame）Sb-------OXYZ

　　①原點(diǎn)O取在飛機(jī)質(zhì)心處，坐標(biāo)系與飛機(jī)固連

　　②x軸在飛機(jī)對(duì)稱(chēng)平面內(nèi)并平行于飛機(jī)的設(shè)計(jì)軸線(xiàn)指向機(jī)頭

　　③y軸垂直于飛機(jī)對(duì)稱(chēng)平面指向機(jī)身右方

　　④z軸在飛機(jī)對(duì)稱(chēng)平面內(nèi)，與x軸垂直并指向機(jī)身下方

　　歐拉角/姿態(tài)角（Euler Angle）

　　機(jī)體坐標(biāo)系與地面坐標(biāo)系的關(guān)系是三個(gè)Euler角，反應(yīng)了飛機(jī)相對(duì)地面的姿態(tài)。

　　俯仰角θ（pitch）：機(jī)體坐標(biāo)系X軸與水平面的夾角。當(dāng)X軸的正半軸位于過(guò)坐標(biāo)原點(diǎn)的水平面之上（抬頭）時(shí)，俯仰角為正，否則為負(fù)。

　　偏航角ψ（yaw）：

　　機(jī)體坐標(biāo)系xb軸在水平面上投影與地面坐標(biāo)系xg軸（在水平面上，指向目標(biāo)為正）之間的夾角，由xg軸逆時(shí)針轉(zhuǎn)至機(jī)體xb的投影線(xiàn)時(shí)，偏航角為正，即機(jī)頭右偏航為正，反之為負(fù)。

　　滾轉(zhuǎn)角Φ（roll）：機(jī)體坐標(biāo)系z(mì)b軸與通過(guò)機(jī)體xb軸的鉛垂面間的夾角，機(jī)體向右滾為正，反之為負(fù)。

　　首先要明確，MPU6050是一款姿態(tài)傳感器，使用它就是為了得到待測(cè)物體（如四軸、平衡小車(chē)）x、y、z軸的傾角（俯仰角Pitch、滾轉(zhuǎn)角Roll、偏航角Yaw）。我們通過(guò)I2C讀取到MPU6050的六個(gè)數(shù)據(jù)（三軸加速度AD值、三軸角速度AD值）經(jīng)過(guò)姿態(tài)融合后就可以得到Pitch、Roll、Yaw角。

　　主要介紹三種姿態(tài)融合算法：四元數(shù)法、一階互補(bǔ)算法和卡爾曼濾波算法。

　　一、四元數(shù)法

　　關(guān)于四元數(shù)的一些概念和計(jì)算就不寫(xiě)上來(lái)了，我也不懂。我能告訴你的是：通過(guò)下面的算法，可以把六個(gè)數(shù)據(jù)轉(zhuǎn)化成四元數(shù)（q0、q1、q2、q3），然后四元數(shù)轉(zhuǎn)化成歐拉角（P、R、Y角）。

　　雖然MPU6050自帶的DMP庫(kù)可以直接輸出四元數(shù)，減輕STM32的運(yùn)算負(fù)擔(dān)，這里在此沒(méi)有使用，因?yàn)槲沂怯肧TM32的硬件I2C讀取MPU6050數(shù)據(jù)的，DMP庫(kù)需要對(duì)I2C函數(shù)進(jìn)行修改，如DMP庫(kù)中的I2C寫(xiě)：i2c_write（st.hw-》addr，st.reg-》pwr_mgmt_1，1，&（data［0］））；有4個(gè)輸入變量，而STM32硬件I2C的I2C寫(xiě)為：voidMPU6050_I2C_ByteWrite（u8slaveAddr，u8pBuffer，u8writeAddr），只有3個(gè)輸入量（這之間的差異好像是由于MPU6050的DMP庫(kù)是針對(duì)MSP430單片機(jī)寫(xiě)的），所以必須進(jìn)行修改，但是改固件庫(kù)是一件很痛苦的事，你們應(yīng)該都懂。當(dāng)然，如果你用模擬I2C的話(huà)，是容易實(shí)現(xiàn)的，網(wǎng)上的DMP移植幾乎都是基于模擬I2C的。

　　要注意的的是，四元數(shù)算法輸出的是三個(gè)量Pitch、Roll和Yaw，運(yùn)算量很大。而像平衡小車(chē)這樣的例子只需要一個(gè)角（Pitch或Roll）就可以滿(mǎn)足工作要求，個(gè)人覺(jué)得做平衡小車(chē)最好不用四元數(shù)法。

　　#include《math.h》

　　#include“stm32f10x.h”

　　//------------------------

　　//變量定義

　　#defineKp100.0f//比例增益支配率收斂到加速度計(jì)/磁強(qiáng)計(jì)

　　#defineKi0.002f//積分增益支配率的陀螺儀偏見(jiàn)的銜接

　　#definehalfT0.001f//采樣周期的一半

　　floatq0=1，q1=0，q2=0，q3=0;//四元數(shù)的元素，代表估計(jì)方向

　　floatexInt=0，eyInt=0，ezInt=0;//按比例縮小積分誤差

　　floatYaw，Pitch，Roll;//偏航角，俯仰角，翻滾角

　　voidIMUupdate（floatgx，floatgy，floatgz，floatax，floatay，floataz）

　　{

　　floatnorm;

　　floatvx，vy，vz;

　　floatex，ey，ez;

　　//測(cè)量正常化

　　norm=sqrt（ax*ax+ay*ay+az*az）;

　　ax=ax/norm;//單位化

　　ay=ay/norm;

　　az=az/norm;

　　//估計(jì)方向的重力

　　vx=2*（q1*q3-q0*q2）;

　　vy=2*（q0*q1+q2*q3）;

　　vz=q0*q0-q1*q1-q2*q2+q3*q3;

　　//錯(cuò)誤的領(lǐng)域和方向傳感器測(cè)量參考方向之間的交叉乘積的總和

　　ex=（ay*vz-az*vy）;

　　ey=（az*vx-ax*vz）;

　　ez=（ax*vy-ay*vx）;

　　//積分誤差比例積分增益

　　exInt=exInt+ex*Ki;

　　eyInt=eyInt+ey*Ki;

　　ezInt=ezInt+ez*Ki;

　　//調(diào)整后的陀螺儀測(cè)量

　　gx=gx+Kp*ex+exInt;

　　gy=gy+Kp*ey+eyInt;

　　gz=gz+Kp*ez+ezInt;

　　//整合四元數(shù)率和正常化

　　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;

　　//正常化四元

　　norm=sqrt（q0*q0+q1*q1+q2*q2+q3*q3）;

　　q0=q0/norm;

　　q1=q1/norm;

　　q2=q2/norm;

　　q3=q3/norm;

　　Pitch=asin（-2*q1*q3+2*q0*q2）*57.3;//pitch，轉(zhuǎn)換為度數(shù)

　　Roll=atan2（2*q2*q3+2*q0*q1，-2*q1*q1-2*q2*q2+1）*57.3;//rollv

　　//Yaw=atan2（2*（q1*q2+q0*q3），q0*q0+q1*q1-q2*q2-q3*q3）*57.3;//此處沒(méi)有價(jià)值，注掉

　　}

　　二、一階互補(bǔ)算法

　　MPU6050可以輸出三軸的加速度和角速度。通過(guò)加速度和角速度都可以得到Pitch和Roll角（加速度不能得到Y(jié)aw角），就是說(shuō)有兩組Pitch、Roll角，到底應(yīng)該選哪組呢？別急，先分析一下。MPU6050的加速度計(jì)和陀螺儀各有優(yōu)缺點(diǎn)，三軸的加速度值沒(méi)有累積誤差，且通過(guò)算tan（）可以得到傾角，但是它包含的噪聲太多（因?yàn)榇郎y(cè)物運(yùn)動(dòng)時(shí)會(huì)產(chǎn)生加速度，電機(jī)運(yùn)行時(shí)振動(dòng)會(huì)產(chǎn)生加速度等），不能直接使用；陀螺儀對(duì)外界振動(dòng)影響小，精度高，通過(guò)對(duì)角速度積分可以得到傾角，但是會(huì)產(chǎn)生累積誤差。所以，不能單獨(dú)使用MPU6050的加速度計(jì)或陀螺儀來(lái)得到傾角，需要互補(bǔ)。一階互補(bǔ)算法的思想就是給加速度和陀螺儀不同的權(quán)值，把它們結(jié)合到一起，進(jìn)行修正。得到Pitch角的程序如下：

　　//一階互補(bǔ)濾波

　　floatK1=0.1;//對(duì)加速度計(jì)取值的權(quán)重

　　floatdt=0.001;//注意：dt的取值為濾波器采樣時(shí)間

　　floatangle;

　　angle_ax=atan（ax/az）*57.3;//加速度得到的角度

　　gy=（float）gyo［1］/7510.0;//陀螺儀得到的角速度

　　Pitch=yijiehubu（angle_ax，gy）;

　　floatyijiehubu（floatangle_m，floatgyro_m）//采集后計(jì)算的角度和角加速度

　　{

　　angle=K1*angle_m+（1-K1）*（angle+gyro_m*dt）;

　　returnangle;

　　}

　　互補(bǔ)算法只能得到一個(gè)傾角，這在平衡車(chē)項(xiàng)目中夠用了，而在四軸飛行器設(shè)計(jì)中還需要Roll和Yaw，就需要兩個(gè)互補(bǔ)算法，我是這樣寫(xiě)的，注意變量不要搞混：

　　//一階互補(bǔ)濾波

　　floatK1=0.1;//對(duì)加速度計(jì)取值的權(quán)重

　　floatdt=0.001;//注意：dt的取值為濾波器采樣時(shí)間

　　floatangle_P，angle_R;

　　floatyijiehubu_P（floatangle_m，floatgyro_m）//采集后計(jì)算的角度和角加速度

　　{

　　angle_P=K1*angle_m+（1-K1）*（angle_P+gyro_m*dt）;

　　returnangle_P;

　　}

　　floatyijiehubu_R（floatangle_m，floatgyro_m）//采集后計(jì)算的角度和角加速度

　　{

　　angle_R=K1*angle_m+（1-K1）*（angle_R+gyro_m*dt）;

　　returnangle_R;

　　}

　　單靠MPU6050無(wú)法準(zhǔn)確得到Y(jié)aw角，需要和地磁傳感器結(jié)合使用。

　　三、卡爾曼濾波

　　其實(shí)卡爾曼濾波和一階互補(bǔ)有些相似，輸入也是一樣的。卡爾曼原理以及什么5個(gè)公式等等的，我也不太懂，就不寫(xiě)了，感興趣的話(huà)可以上網(wǎng)查。在此給出具體程序，和一階互補(bǔ)算法一樣，每次卡爾曼濾波只能得到一個(gè)方向的角度。

　　#include《math.h》

　　#include“stm32f10x.h”

　　#include“Kalman_Filter.h”

　　//卡爾曼濾波參數(shù)與函數(shù)

　　floatdt=0.001;//注意：dt的取值為kalman濾波器采樣時(shí)間

　　floatangle，angle_dot;//角度和角速度

　　floatP［2］［2］={{1，0}，

　　{0，1}};

　　floatPdot［4］={0，0，0，0};

　　floatQ_angle=0.001，Q_gyro=0.005;//角度數(shù)據(jù)置信度，角速度數(shù)據(jù)置信度

　　floatR_angle=0.5，C_0=1;

　　floatq_bias，angle_err，PCt_0，PCt_1，E，K_0，K_1，t_0，t_1;

　　//卡爾曼濾波

　　floatKalman_Filter（floatangle_m，floatgyro_m）//angleAx和gyroGy

　　{

　　angle+=（gyro_m-q_bias）*dt;

　　angle_err=angle_m-angle;

　　Pdot［0］=Q_angle-P［0］［1］-P［1］［0］;

　　Pdot［1］=-P［1］［1］;

　　Pdot［2］=-P［1］［1］;

　　Pdot［3］=Q_gyro;

　　P［0］［0］+=Pdot［0］*dt;

　　P［0］［1］+=Pdot［1］*dt;

　　P［1］［0］+=Pdot［2］*dt;

　　P［1］［1］+=Pdot［3］*dt;

　　PCt_0=C_0*P［0］［0］;

　　PCt_1=C_0*P［1］［0］;

　　E=R_angle+C_0*PCt_0;

　　K_0=PCt_0/E;

　　K_1=PCt_1/E;

　　t_0=PCt_0;

　　t_1=C_0*P［0］［1］;

　　P［0］［0］-=K_0*t_0;

　　P［0］［1］-=K_0*t_1;

　　P［1］［0］-=K_1*t_0;

　　P［1］［1］-=K_1*t_1;

　　angle+=K_0*angle_err;//最優(yōu)角度

　　q_bias+=K_1*angle_err;

　　angle_dot=gyro_m-q_bias;//最優(yōu)角速度

　　returnangle；

　　}

　總結(jié)：三種融合算法都能夠輸出姿態(tài)角（Pitch和Roll），一次四元數(shù)法可以輸出P、R、Y三個(gè)傾角，計(jì)算量比較大。一階互補(bǔ)和卡爾曼濾波每次只能輸出一個(gè)軸的姿態(tài)角。