姿態(tài)解算

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

一、歐拉角

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

如下圖所示：

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

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

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

二、姿態(tài)解算

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

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

2.1.1位姿矩陣：

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

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

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

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

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

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

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

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

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

因?yàn)镸PU6050是繞運(yùn)動(dòng)坐標(biāo)系旋轉(zhuǎn)，所以對(duì)應(yīng)的矩陣變換是左乘：

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

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

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

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

因?yàn)镸PU6050是繞運(yùn)動(dòng)坐標(biāo)系旋轉(zhuǎn)，所以對(duì)應(yīng)的矩陣變換是左乘：

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

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

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

//（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)

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

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

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

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

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

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