MPU6050姿态融合（转载）
姿態(tài)角（Euler角）pitch yaw roll
飛行器的姿態(tài)角并不是指哪個(gè)角度，是三個(gè)角度的統(tǒng)稱。
它們是：俯仰、滾轉(zhuǎn)、偏航。你可以想象是飛機(jī)圍繞XYZ三個(gè)軸分別轉(zhuǎn)動(dòng)形成的夾角。
地面坐標(biāo)系（earth-surface inertial reference frame）Sg--------OXgYgZg
<ignore_js_op>
①在地面上選一點(diǎn)Og
②使Xg軸在水平面內(nèi)并指向某一方向
③Zg軸垂直于地面并指向地心(重力方向)
④Yg軸在水平面內(nèi)垂直于Xg軸，其指向按右手定則確定
機(jī)體坐標(biāo)系(Aircraft-body coordinate frame)Sb-------OXYZ
<ignore_js_op>
①原點(diǎn)O取在飛機(jī)質(zhì)心處，坐標(biāo)系與飛機(jī)固連
②x軸在飛機(jī)對(duì)稱平面內(nèi)并平行于飛機(jī)的設(shè)計(jì)軸線指向機(jī)頭
③y軸垂直于飛機(jī)對(duì)稱平面指向機(jī)身右方
④z軸在飛機(jī)對(duì)稱平面內(nèi)，與x軸垂直并指向機(jī)身下方
歐拉角/姿態(tài)角（Euler Angle）
<ignore_js_op>
<ignore_js_op>
機(jī)體坐標(biāo)系與地面坐標(biāo)系的關(guān)系是三個(gè)Euler角，反應(yīng)了飛機(jī)相對(duì)地面的姿態(tài)。
俯仰角θ（pitch）：機(jī)體坐標(biāo)系X軸與水平面的夾角。當(dāng)X軸的正半軸位于過坐標(biāo)原點(diǎn)的水平面之上（抬頭）時(shí)，俯仰角為正，否則為負(fù)。
<ignore_js_op>
偏航角ψ（yaw）：
機(jī)體坐標(biāo)系xb軸在水平面上投影與地面坐標(biāo)系xg軸（在水平面上，指向目標(biāo)為正）之間的夾角，由xg軸逆時(shí)針轉(zhuǎn)至機(jī)體xb的投影線時(shí)，偏航角為正，即機(jī)頭右偏航為正，反之為負(fù)。
<ignore_js_op>
滾轉(zhuǎn)角Φ（roll）：機(jī)體坐標(biāo)系z(mì)b軸與通過機(jī)體xb軸的鉛垂面間的夾角，機(jī)體向右滾為正，反之為負(fù)。
<ignore_js_op>
首先要明確，MPU6050 是一款姿態(tài)傳感器，使用它就是為了得到待測(cè)物體（如四軸、平衡小車） x、y、z 軸的傾角（俯仰角 Pitch、滾轉(zhuǎn)角 Roll、偏航角 Yaw） 。我們通過 I2C 讀取到 MPU6050 的六個(gè)數(shù)據(jù)（三軸加速度 AD 值、三軸角速度 AD 值）經(jīng)過姿態(tài)融合后就可以得到 Pitch、Roll、Yaw 角。
本帖主要介紹三種姿態(tài)融合算法：四元數(shù)法 、一階互補(bǔ)算法和卡爾曼濾波算法。
一、四元數(shù)法
關(guān)于四元數(shù)的一些概念和計(jì)算就不寫上來了，我也不懂。我能告訴你的是：通過下面的算法，可以把六個(gè)數(shù)據(jù)轉(zhuǎn)化成四元數(shù)（q0、q1、q2、q3），然后四元數(shù)轉(zhuǎn)化成歐拉角（P、R、Y 角）。
雖然 MPU6050 自帶的 DMP庫可以直接輸出四元數(shù)，減輕 STM32 的運(yùn)算負(fù)擔(dān)， 這里在此沒有使用，因?yàn)槲沂怯?STM32 的硬件 I2C 讀取 MPU6050 數(shù)據(jù)的（http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735），DMP庫需要對(duì) I2C 函數(shù)進(jìn)行修改，如 DMP 庫中的 I2C 寫：i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]))；有4個(gè)輸入變量，而 STM32 硬件 I2C 的 I2C 寫為：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 個(gè)輸入量（這之間的差異好像是由于 MPU6050 的 DMP 庫是針對(duì) MSP430單片機(jī)寫的），所以必須進(jìn)行修改，但是改固件庫是一件很痛苦的事，你們應(yīng)該都懂。當(dāng)然，如果你用模擬 I2C 的話，是容易實(shí)現(xiàn)的，網(wǎng)上的 DMP 移植幾乎都是基于模擬 I2C 的。
復(fù)制代碼
#include<math.h>
#include "stm32f10x.h"
//---------------------------------------------------------------------------------------------------
// 變量定義
#define Kp 100.0f // 比例增益支配率收斂到加速度計(jì)/磁強(qiáng)計(jì)
#define Ki 0.002f // 積分增益支配率的陀螺儀偏見的銜接
#define halfT 0.001f // 采樣周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元數(shù)的元素，代表估計(jì)方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例縮小積分誤差
float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滾角
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, 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; //此處沒有價(jià)值，注掉
}
要注意的的是，四元數(shù)算法輸出的是三個(gè)量 Pitch、Roll 和 Yaw，運(yùn)算量很大。而像平衡小車這樣的例子只需要一個(gè)角（Pitch 或 Roll ）就可以滿足工作要求，個(gè)人覺得做平衡小車最好不用四元數(shù)法。
二、一階互補(bǔ)算法
MPU6050 可以輸出三軸的加速度和角速度。通過加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是說有兩組 Pitch、Roll 角，到底應(yīng)該選哪組呢？別急，先分析一下。MPU6050 的加速度計(jì)和陀螺儀各有優(yōu)缺點(diǎn)，三軸的加速度值沒有累積誤差，且通過算 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)影響小，精度高，通過對(duì)角速度積分可以得到傾角，但是會(huì)產(chǎn)生累積誤差。所以，不能單獨(dú)使用 MPU6050 的加速度計(jì)或陀螺儀來得到傾角，需要互補(bǔ)。一階互補(bǔ)算法的思想就是給加速度和陀螺儀不同的權(quán)值，把它們結(jié)合到一起，進(jìn)行修正。得到 Pitch 角的程序如下：
//一階互補(bǔ)濾波
float K1 =0.1; // 對(duì)加速度計(jì)取值的權(quán)重
float dt=0.001;//注意：dt的取值為濾波器采樣時(shí)間
float angle;
angle_ax=atan(ax/az)*57.3; //加速度得到的角度
gy=(float)gyo[1]/7510.0; //陀螺儀得到的角速度
Pitch = yijiehubu(angle_ax,gy);
float yijiehubu(float angle_m, float gyro_m)//采集后計(jì)算的角度和角加速度
{
angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
return angle;
}
互補(bǔ)算法只能得到一個(gè)傾角，這在平衡車項(xiàng)目中夠用了，而在四軸飛行器設(shè)計(jì)中還需要 Roll 和 Yaw，就需要兩個(gè) 互補(bǔ)算法，我是這樣寫的，注意變量不要搞混：
復(fù)制代碼
//一階互補(bǔ)濾波
float K1 =0.1; // 對(duì)加速度計(jì)取值的權(quán)重
float dt=0.001;//注意：dt的取值為濾波器采樣時(shí)間
float angle_P,angle_R;
float yijiehubu_P(float angle_m, float gyro_m)//采集后計(jì)算的角度和角加速度
{
angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
return angle_P;
}
float yijiehubu_R(float angle_m, float gyro_m)//采集后計(jì)算的角度和角加速度
{
angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
return angle_R;
}
單靠 MPU6050 無法準(zhǔn)確得到 Yaw 角，需要和地磁傳感器結(jié)合使用。
三、卡爾曼濾波
其實(shí)卡爾曼濾波和一階互補(bǔ)有些相似，輸入也是一樣的。卡爾曼原理以及什么5個(gè)公式等等的，我也不太懂，就不寫了，感興趣的話可以上網(wǎng)查。在此給出具體程序，和一階互補(bǔ)算法一樣，每次卡爾曼濾波只能得到一個(gè)方向的角度。
#include<math.h>
#include "stm32f10x.h"
#include "Kalman_Filter.h"
//卡爾曼濾波參數(shù)與函數(shù)
float dt=0.001;//注意：dt的取值為kalman濾波器采樣時(shí)間
float angle, angle_dot;//角度和角速度
float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度數(shù)據(jù)置信度,角速度數(shù)據(jù)置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//卡爾曼濾波
float Kalman_Filter(float angle_m, float gyro_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)角速度
return angle；
}
作個(gè)總結(jié)：三種融合算法都能夠輸出姿態(tài)角（Pitch 和 Roll ），一次四元數(shù)法可以輸出 P、R、Y 三個(gè)傾角，計(jì)算量比較大。一階互補(bǔ)和卡爾曼濾波每次只能輸出一個(gè)軸的姿態(tài)角。
《新程序員》：云原生和全面數(shù)字化實(shí)踐50位技術(shù)專家共同創(chuàng)作，文字、視頻、音頻交互閱讀總結(jié)
以上是生活随笔為你收集整理的MPU6050姿态融合（转载）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 使用 MWC V2.5 中的 MPU60
- 下一篇： stm32F105的can2问题