MPU6050加速度的解算
先根據兩張圖建立各個角度的關系
| \(Roll\)(橫滾角) | 以\(x\)軸為旋轉軸的旋轉角度 |
|---|---|
| \(Yaw\)(偏航角) | 以\(z\)軸為旋轉軸的旋轉角度 |
| \(Pitch\)(俯仰角) | 以\(y\)軸為旋轉軸的旋轉角度 |
| 我們先了解加速度的單位及量程方便進行解算 | |
| 在MPU初始化函數中找到初始化加速度傳感器的函數 |
//設置MPU6050加速度傳感器滿量程范圍
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,設置成功
// 其他,設置失敗
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//設置加速度傳感器滿量程范圍
}
通過寄存器找到數據手冊中的說明
比如我們設置量程為\(\pm2g\)
mpu是通過ADC采集電壓的值來輸出加速度值，
而數據寄存器是16位且最高一位為符號位，
即ADC讀取的值位\(\pm32768\)，對應\(\pm2g\),
\(1g\)對應的ADC值為\(+32768/2\),即16384\(LSB/g\)為數據分辨率
同樣可得\(\pm4g\),數據分辨率為\(+32768/4=8192LSB/g\)
通過上面的說明我們可以把mpu讀取到的原始數據解算為加速度
即：\(Gx=aacx/16384\)單位為\(m/s^2\)
我們選擇\(Pitch\)解算
水平狀態時只有\(z\)軸有加速度且加速度為\(1g\)
繞\(y\)軸旋轉后：
\(x^、\)為繞\(y\)軸旋轉后的\(z\)軸加速度方向
\(z^、\)為繞\(y\)軸旋轉后的\(z\)軸加速度方向
由圖\(（4）\)可得\(\rho=arctan\frac{Gx}{Gz}\),
\(\frac{Gx}{Gz}=\frac{aacx}{aacz}\)（分辨率計算后抵消）
\(\rho\)的結果為弧度制，我們將其轉化為角度制
\(Pitch=\frac{\rho*2π}{360}\),
最終的公式為：
\(Pitch=arctan\frac{aacx}{aacz}*\frac{2π}{360}\),
\(\frac{2π}{360}=57.2974\)
\(Pitch=arctan\frac{aacx}{aacz}*57.2974\),
代碼如下：
mpu_dmp_get_data(&pitch,&roll,&yaw); //得到姿態角即歐拉角
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度傳感器數據
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺儀數據
Acc_Pitch=atan((float)aacx/(float)aacz)*57.2974;
printf("%f %f\r\n",pitch,Acc_Pitch);
我們與DMP處理后的\(Pitch\)進行比較
數據相差一個負號
將公式變為： \(Pitch=-arctan\frac{aacx}{aacz}*57.2974\)即可
代碼也對應的進行修改：
mpu_dmp_get_data(&pitch,&roll,&yaw); //得到姿態角即歐拉角
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度傳感器數據
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺儀數據
Acc_Pitch=-atan((float)aacx/(float)aacz)*57.2974;
printf("%f %f\r\n",pitch,Acc_Pitch);
輸出數據波形:
紅色為加速度解算
藍色為\(DMP\)解算
加速度解算后的 \(Pitch\)相對於\(DMP\)的輸出值包含大量高頻噪聲,但是靈敏度大大提高，基本可以實時輸出角度值。
（個人感覺還是\(DMP\)比較穩定）
同理可得
\(Roll=arctan\frac{aacy}{aacz}*57.2974\)
\(Yaw=arctan\frac{aacx}{aacy}*57.2974\)
由於\(yaw\)角為繞\(z\)軸旋轉的角，陀螺儀平放時，accz近似為\(1g\)
而\(y\)和\(z\)上少有加速度分量，所以解算的\(yaw\)誤差較大。
\(yaw\)的解算值與\(DMP\)輸出波形的比較，通過輸出曲線圖可以看到解算后誤差較大，且有大量高頻噪聲，一般不采用。
MPU6050角速度的解算
先看mpu角速度初始化的函數
//設置MPU6050陀螺儀傳感器滿量程范圍
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,設置成功
// 其他,設置失敗
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//設置陀螺儀滿量程范圍
}
根據寄存器在數據手冊中找到相應的說明，
這里的\(LSB/(°/s)\)仍然是數據分辨率，
還是ADC讀數數據
數據寄存器為16位，最高一位為符號位
即ADC輸出范圍為\(\pm32768\)
比如我們選中測量范圍為\(\pm2000°/s\),
ADC讀取的原始值為300
數據分辨率就是\(32768/2000=16.384 LSB/(°/s)\)
那么轉換為角速度就是\(\frac{300}{16.384}=18.3105(°/s)\)
公式為：\(角速度=\frac{角速度原始值}{數據分辨率}\) 單位：\((°/s)\)
角速度已經推算出來了，
角度就可以通過對角速度的積分來計算（選取\(Pitch\)計算）
\(Pitch=\int \frac{gyrox}{角度分辨率}*dt\)
dt為我們讀取mpu數據的間隔時間，
因為我們讀取的數據是離散的，所以直接進行累加就可以計算角度
\(Pitch=\sum_1^n\frac{gyrox}{角度分辨率}*\Delta t\)
另外兩個角的推算方法相同。
代碼如下：
mpu_dmp_get_data(&pitch,&roll,&yaw); //得到姿態角即歐拉角
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度傳感器數據
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺儀數據
Acc_Pitch=atan((float)aacx/(float)aacy)*57.297;
Gyro_Pitch+=-(gyrox/16.4)*0.01; //讀取間隔為10ms轉換為0.01s計算，讀取數值與實際值相反加負號
printf("%f %f\r\n",pitch,Gyro_Pitch);
與\(DMP\)的輸出波形相比，角速度積分的數據漂移比較明顯，但數據的平滑性較好，可以有效抑制高頻干擾。
互補濾波使用（數據融合）
| 對比項 | 加速度 | 陀螺儀(角速度) |
|---|---|---|
| 高頻振動噪聲 | 十分敏感 | 基本無感 |
| 高頻低頻姿態漂移 | 基本無感 | 飄移嚴重 |
| 實時性 | 實時性強，基本不會隨時間飄移 | 隨時間飄移嚴重 |
| 加速度數據，通過受力分能夠顯示角度變化趨勢，在長期變化來看是可以利用的，實時性強。 |
角速度數據，加速度積分得到角度，但是由於傳感器誤差，積分作用會造成累計誤差，所以角速度數據在短期是可以使用的，長期來看誤差會很大是不可使用的。
數字低通濾波器
可以讓長期變化通過(低頻)，濾波消除短期變化(高頻)，用於對加速度計的濾波。
數字高通濾波器
可以讓短時信號通過(低頻)，過濾出變化緩慢的信號(高頻)，消除漂移。
我們截取陀螺儀解算數據的高頻部分和加速度解算數據的低頻部分進行融合，形成一個類似與模電的帶通濾波器。
簡單點就是陀螺儀低頻不彳亍，我只要你的高頻(短期)信號；
加速度的高頻太抖了不彳亍，我就拿你的變化做個修正就\(OK\)了；
代碼部分：
mpu_dmp_get_data(&pitch,&roll,&yaw); //得到姿態角即歐拉角
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度傳感器數據
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺儀數據
Acc_Pitch=atan((float)aacx/(float)aacz)*57.297;
Gyro_Pitch=(gyrox/16.4)*0.01; //讀取間隔為10ms轉換為0.01s計算，讀取數值與實際值相反加負號
Merge_Pitch=(Last_data+Gyro_Pitch)*(1-a)+a*Acc_Pitch;
Last_data=Merge_Pitch;
printf("%f %f\r\n",pitch,-Merge_Pitch);
\(MergePitch=(Lastdata+GyroPitch)*(1-a)+a*AccPitch\)
\(融合結果=(上次融合值+角速度一個采樣周期的積分值)*(1-a)+a*加速度解算值\)
從代碼中看出，角速度長時間容易飄移，我們就去一個周期的變化(高頻)
加速度太抖，我們就拿你當成數據的修正(抑制高頻)，對數據進行更新。
藍色為\(DMP\)處理數據
紅色為互補濾波融合后的數據
對比\(DMP\)，互補濾波的平滑性更好，同時也具備靈敏度與實時性。