姿態解算

我們僅僅獲取了MPU6050的三軸加速度和角速度，要想得到姿態角，需要利用讀取的數據進行姿態解算。

一、歐拉角

歐拉角是一種用于描述三維剛體相對參考坐標系（通常為水平面的直角坐標系）姿態的參數，通過依次繞三個互相垂直的坐標軸（對應姿態檢測中常用的翻滾角 Roll、俯仰角 Pitch、偏航角 Yaw，分別繞 X、Y、Z 軸旋轉）的旋轉角度來表征設備的傾斜、轉向狀態。

如下圖所示：

橫滾角（Roll）：繞運動坐標系的Y軸旋轉

偏航角（Yaw）：繞運動坐標系的Z軸旋轉

俯仰角（Roll）：繞運動坐標系的X軸旋轉

二、姿態解算

2.1利用加速度計解算姿態（僅能解算Roll和Pitch）

歐拉角與旋轉矩陣來對陀螺儀與加速度計的原始數據進行姿態求解，并將兩種姿態進行互補融合，最終得到IMU的實時姿態。

2.1.1位姿矩陣：

位姿矩陣是用于在三維笛卡爾參考坐標系中，統一描述剛體的位置（平移狀態）與姿態 的 4×4 階齊次變換矩陣，其前 3×3 的子矩陣為旋轉矩陣，對應由歐拉角等姿態參數轉換而來的剛體旋轉信息，用來表征設備相對參考坐標系的傾斜、轉向姿態（即 MPU6050 檢測的 Roll、Pitch、Yaw 對應的姿態狀態），矩陣最后一列的前 3 個元素為平移向量，用來表征設備在參考坐標系中的三維位置坐標，最后一行固定為 [0,0,0,1] 以滿足齊次坐標的運算規范，它可以將剛體的旋轉與平移變換整合為一次矩陣運算，在嵌入式姿態檢測與空間定位的場景中，能簡化不同坐標系間的姿態、位置轉換計算，常作為姿態解算、多傳感器融合的基礎數學工具。

當加速度計水平放置，即Z軸豎直向上時，Z軸可以讀到1g的數值（g為重力加速度），X軸和Y軸兩個方向讀到0，初始位姿可以記作：

本篇的姿態解算選用的旋轉順序為ZYX，即IMU坐標系初始時刻與大地坐標系重合，然后依次繞自己的Z、Y、X軸進行旋轉，這里先自定義一下每次的旋轉名稱和符號：

繞IMU的Z軸旋轉：航向角yaw， 轉動y角度

繞IMU的Y軸旋轉：俯仰角pitch，轉動p角度

繞IMU的X軸旋轉：橫滾角row， 轉動r角度

當IMU繞Z軸旋轉y度，再繞Y軸旋轉P度，再繞X軸旋轉r度，其終止位姿可以表示為：

根據機器人運動學：對于一個傳感器的末端姿態，我們可以將其看作分別繞z、y，x軸旋轉得到，其中正向運動學的繞各軸的旋轉矩陣如下：

現在我們已知了初始位姿，終止位姿、繞三軸的旋轉矩陣，對其進行機器人逆運動學的求解，可求得繞三個軸的旋轉角度：

因為MPU6050是繞運動坐標系旋轉，所以對應的矩陣變換是左乘：

解這個方程，可以得到roll和pitch角（由于繞Z旋轉時，感受到的重力加速度是不變的，因此加速度計無法計算****yaw角）

當IMU繞Z軸旋轉y度，再繞Y軸旋轉P度，再繞X軸旋轉r度，其終止位姿可以表示為：

根據機器人運動學：對于一個傳感器的末端姿態，我們可以將其看作分別繞z、y，x軸旋轉得到，其中正向運動學的繞各軸的旋轉矩陣如下：

現在我們已知了初始位姿，終止位姿、繞三軸的旋轉矩陣，對其進行機器人逆運動學的求解，可求得繞三個軸的旋轉角度：

因為MPU6050是繞運動坐標系旋轉，所以對應的矩陣變換是左乘：

解這個方程，可以得到roll和pitch角（由于繞Z旋轉時，感受到的重力加速度是不變的，因此加速度計無法計算yaw角）

解這個方程，可以得到roll和pitch角（由于繞Z旋轉時，感受到的重力加速度是不變的，因此加速度計無法計算yaw角）

對應C語言代碼：

//（atan2返回弧度，需轉成度） Pitch角公式：atan2(AX, sqrt(AY2 + AZ2)) × (180/π)
        accAnglePitch = atan2(ax, sqrt(ay*ay + az*az)) * (180.0f / M_PI);
        //  （若算roll角：atan2(AY, sqrt(AX2 + AZ2)) × (180/π)）
        accAngleRoll = atan2(ay, sqrt(ax*ax + az*az)) * (180.0f / M_PI);

2.2利用陀螺儀解算姿態

我們知道陀螺儀輸出的是角速度，我們對其進行積分，可以得到角度：

yaw角解算對應代碼：

float gyroRateZ = -((gz / GYRO_SENSITIVITY) - gyroBiasZ);  // 扣除零偏后的角速度后取反
// 步驟3：角速度積分計算Yaw角（核心：角度 = 角速度 × 時間）
// 復用IMU_Process_Kalman中已計算的dt（采樣時間，單位s）
 Cacl_yawAngle += gyroRateZ * dt;

pitch和roll角對應解算公式：

pitch和roll角對應解算代碼（此代碼在卡爾曼濾波器中，根據傳入參數pitch和roll來決定解算什么角）：

//此代碼在卡爾曼濾波器中，根據傳入參數pitch和roll來決定解算什么角
float KalmanFilter(float newAngle, float newRate, float dt,
                   float *kalmanAngle, float *kalmanBias, float kalmanP[2][2]) 
float rate = newRate - *kalmanBias;  // 減去偏置             
*kalmanAngle += dt * rate;           // 積分得到角度變化