MPU6050姿態結算（採用硬體I2C實現資料融合演算法）

姿態角（Euler角）pitch yaw roll
飛行器的姿態角並不是指哪個角度，是三個角度的統稱。
它們是：俯仰、滾轉、偏航。你可以想象是飛機圍繞XYZ三個軸分別轉動形成的夾角。

地面座標系（earth-surface inertial reference frame）Sg--------OXgYgZg
<ignore_js_op> 
①在地面上選一點Og
②使Xg軸在水平面內並指向某一方向
③Zg軸垂直於地面並指向地心(重力方向)
④Yg軸在水平面內垂直於Xg軸，其指向按右手定則確定

機體座標系(Aircraft-body coordinate frame)Sb-------OXYZ
<ignore_js_op> 

①原點O取在飛機質心處，座標系與飛機固連
②x軸在飛機對稱平面內並平行於飛機的設計軸線指向機頭
③y軸垂直於飛機對稱平面指向機身右方
④z軸在飛機對稱平面內，與x軸垂直並指向機身下方

尤拉角/姿態角（Euler Angle）
<ignore_js_op> 
<ignore_js_op> 

機體座標系與地面座標系的關係是三個Euler角，反應了飛機相對地面的姿態。
俯仰角θ（pitch）：機體座標系X軸與水平面的夾角。當X軸的正半軸位於過座標原點的水平面之上（抬頭）時，俯仰角為正，否則為負。
<ignore_js_op> 

偏航角ψ（yaw）：
機體座標系xb軸在水平面上投影與地面座標系xg軸（在水平面上，指向目標為正）之間的夾角，由xg軸逆時針轉至機體xb的投影線時，偏航角為正，即機頭右偏航為正，反之為負。
<ignore_js_op> 

滾轉角Φ（roll）：機體座標系zb軸與通過機體xb軸的鉛垂面間的夾角，機體向右滾為正，反之為負。
<ignore_js_op>

 

 

首先要明確，MPU6050 是一款姿態感測器，使用它就是為了得到待測物體（如四軸、平衡小車） x、y、z 軸的傾角（俯仰角 Pitch、滾轉角 Roll、偏航角 Yaw） 。我們通過 I2C 讀取到 MPU6050 的六個資料（三軸加速度 AD 值、三軸角速度 AD 值）經過姿態融合後就可以得到 Pitch、Roll、Yaw 角。

本帖主要介紹三種姿態融合演算法：四元數法 、一階互補演算法和卡爾曼濾波演算法。

一、四元數法

關於四元數的一些概念和計算就不寫上來了，我也不懂。我能告訴你的是：通過下面的演算法，可以把六個資料轉化成四元數（q0、q1、q2、q3），然後四元數轉化成尤拉角（P、R、Y 角）。

        雖然 MPU6050 自帶的 DMP庫可以直接輸出四元數，減輕 STM32 的運算負擔， 這裡在此沒有使用，因為我是用 STM32 的硬體 I2C 讀取 MPU6050 資料的（http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735），DMP庫需要對 I2C 函式進行修改，如 DMP 庫中的 I2C 寫：i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]))；有4個輸入變數，而 STM32 硬體 I2C 的 I2C 寫為：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 個輸入量（這之間的差異好像是由於 MPU6050 的 DMP 庫是針對 MSP430 微控制器寫的），所以必須進行修改，但是改韌體庫是一件很痛苦的事，你們應該都懂。當然，如果你用模擬 I2C 的話，是容易實現的，網上的 DMP 移植幾乎都是基於模擬 I2C 的。