姿態(tài)解算

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

一、歐拉角

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

如下圖所示：

橫滾角（Roll）：繞運動坐標(biāo)系的Y軸旋轉(zhuǎn)

偏航角（Yaw）：繞運動坐標(biāo)系的Z軸旋轉(zhuǎn)

俯仰角（Roll）：繞運動坐標(biāo)系的X軸旋轉(zhuǎn)

二、姿態(tài)解算

2.1利用加速度計解算姿態(tài)（僅能解算Roll和Pitch）

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

2.1.1位姿矩陣：

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

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

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

繞IMU的Z軸旋轉(zhuǎn)：航向角yaw， 轉(zhuǎn)動y角度

繞IMU的Y軸旋轉(zhuǎn)：俯仰角pitch，轉(zhuǎn)動p角度

繞IMU的X軸旋轉(zhuǎn)：橫滾角row， 轉(zhuǎn)動r角度

當(dāng)IMU繞Z軸旋轉(zhuǎn)y度，再繞Y軸旋轉(zhuǎn)P度，再繞X軸旋轉(zhuǎn)r度，其終止位姿可以表示為：

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

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

因為MPU6050是繞運動坐標(biāo)系旋轉(zhuǎn)，所以對應(yīng)的矩陣變換是左乘：

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

當(dāng)IMU繞Z軸旋轉(zhuǎn)y度，再繞Y軸旋轉(zhuǎn)P度，再繞X軸旋轉(zhuǎn)r度，其終止位姿可以表示為：

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

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

因為MPU6050是繞運動坐標(biāo)系旋轉(zhuǎn)，所以對應(yīng)的矩陣變換是左乘：

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

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

對應(yīng)C語言代碼：

//（atan2返回弧度，需轉(zhuǎn)成度） 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利用陀螺儀解算姿態(tài)

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

yaw角解算對應(yīng)代碼：

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

pitch和roll角對應(yīng)解算公式：

pitch和roll角對應(yīng)解算代碼（此代碼在卡爾曼濾波器中，根據(jù)傳入?yún)?shù)pitch和roll來決定解算什么角）：

//此代碼在卡爾曼濾波器中，根據(jù)傳入?yún)?shù)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;           // 積分得到角度變化