前幾天做了6050原始數據的串口輸出和上位機波形的查看。這篇博客我們來看一下對原始數據的處理。
任務：利用STC89C52RC對MPU6050原始數據進行濾波與姿態融合。
首先我摘抄了一段別人在昨晚這個實驗的寫的最后總結。
1.盡量不要用MPU6050內置的LPF濾波。雖然相比於原始加速度計輸出，該LPF可以平滑輸出，但是在FFT頻譜上的表現相當差勁。
2.廣泛使用的窗口平均滑動濾波無論在FFT還是RMSE表現上都有不錯的表現，所以一般基礎應用（低速運動或四軸初學者）采用窗口平均濾波是比較明智的選擇。
3.想要達到更好的濾波效果，FIR或者IIR濾波器是更好的選擇。筆者測試2階30Hz的Butterworth濾波器雖然在平滑性RMSE只比窗口平均滑動差了一點（但是比LPF要好），但是數據實時性性能指標上比前者響應速度提高了近一倍。因此在制作四軸的進階階段，可以考慮將窗口平均滑動換成Butterworth濾波器。
開發日記，不想看的可以跳過，僅僅是為了記錄自己的過程。
//----------------------------------------------------------------------------------------------------------------------------------------
2017.12.5 晚
剛剛寫完6050的零偏矯正和滑動窗口濾波，就已經超過51單片機內部最大RAM允許范圍了。具體表現為：
USERS\MPU6050.C(241): error C249: 'DATA': SEGMENT TOO LARGE
定義的數據段大於單片機內部RAM,
查了資料，原來這段程序並沒有把stc89c52內部的512字節的RAM全部用完！而是用完了開頭的128字節的數據（0X00-0X7F）后面還有128*3的字節量沒有被使用。
另外，當我們程序中總變量的字節數超過128字節時，必須對程序中所有變量進行初始化，否則沒有被初始化的變量默認值不在是0，而是不確定的隨機數。這時我們必須在keil中設置存儲器的存儲模式。但是當你的變量總字節在128字節以內的話，並且存儲模式為small模式時，沒有初始化的變量的默認值才是0.
1.Small模式
所有缺省變量參數均裝入內部 128字節 RAM，優點是訪問速度快，缺點是空間有限，只適用於小程序。
2. Compact 模式
所有缺省變量均位於外部RAM區的一頁(256Byte)。和small模式中使用pdata定義變量的效果相同。如：uchar pdata a[100]; 該模式下，總變量不得超過256字節。
3. Large 模式
所有缺省變量可放在多達 64KB 的外部RAM 區，優點是空間大，可存變量多，缺點是速度較慢。在large模式下，和在small模式中使用關鍵字xdata 定義變量的效果一樣。如：char xdata a[100];這樣顯式的規定了變量的存放區域，以定義為准，不受存儲模式的影響。在此模式下編寫程序時，定義的變量不要超過單片機規定的內部最大RAM數，因為即使超過了，編譯器也不會報錯，但是程序運行一定出錯。
注意： 為了程序安全，當程序中定義超過128字節的數據時，最好申明為xdata
型，為其在擴展RAM中分配內存。
2017.12.6 下午
一般我們最好配置為small模式，然后必要的情況下，使用xdata對變量進行說明。
/************
注意看編譯后的這個，他們的單位時字節B。而1k = 1024B
Program Size: data=16.1 xdata=247 code=3424
/************
由於在6050數據處理函數中多次使用靜態局部變量，所以加深一下對他的理解。
對靜態局部變量的說明：
(1) 靜態局部變量在靜態存儲區內分配存儲單元。在程序整個運行期間都不釋放。而自動變量（即動態局部變量）屬於動態存儲類別，存儲在動態存儲區空間(而不是靜態存儲區空間)，函數調用結束后即釋放。
(2) 為靜態局部變量賦初值是在編譯時進行值的，即只賦初值一次，在程序運行時它已有初值。以后每次調用函數時不再重新賦初值而只是保留上次函數調用結束時的值。而為自動變量賦初值，不是在編譯時進行的，而是在函數調用時進行，每調用一次函數重新給一次初值，相當於執行一次賦值語句。
(3) 如果在定義局部變量時不賦初值的話，對靜態局部變量來說，編譯時自動賦初值0(對數值型變量)或空字符(對字符型變量)。而對自動變量來說，如果不賦初值，則它的值是一個不確定的值。這是由於每次函數調用結束后存儲單元已釋放，下次調用時又重新另分配存儲單元，而所分配的單元中的值是不確定的。
(4) 雖然靜態局部變量在函數調用結束后仍然存在，但其他函數是不能引用它的，也就是說，在其他函數中它是“不可見”的。
不管怎么修改量程，輸出的波形震動范圍總是在+-20000波動，但是當我把加速度計的量程設置為+-16G的時候，理論上其LSB應該為2048左右，但是發現輸出的波形也是震動范圍很大。而且陀螺儀的數據比一開始不處理的時候震動更加明顯。
內存覆蓋：
以覆蓋的（某個函數的局部變量空間在退出該函數是就釋放,由別的函數的局部變量覆蓋）,可以提高內存利用率。當然靜態局部變量除外,其內存使用方式與全局變量相同;
2017.12.7 晚
今天發現我的程序確實存在問題，就是運算時的數據溢出。在進行零偏數據的計算時，需要將200個原始數據相加，但是一開始我的總和定義的還是一個int型的數據，這時200個數據的和可能已經超過了int型數據的取值范圍，所以產生了溢出。導致數據的不准確。那么后面計算的數據也是不准確的。所以我晃動傳感器曲線都沒有什么變化。唉，學單片機都得過數據類型、內存管理這道坎啊。
2017.12.11 下午
如果全是自己開發的話，沒有人指導的情況下，每一個小小的進步，都是非常不容易的！！
2017.12.12 晚
目前的問題就是靜止狀態下陀螺儀輸出的數據不為零。還有就是由於單片機運算能力的限制，輸出的波形延遲較大。
//---------------------------------------------------------------------------------------------------------------
經過大概一個星期的嘗試，終於用51單片機對6050進行了正確的讀取與姿態融合。其實現在想想自己這幾天所做的所有事情，並不多。程序前兩天就寫完了，后面的大部分時間都是在對程序進行調整，對波形進行分析。從剛開始凌亂的波形，到后面使用一階互補濾波和卡爾曼濾波得出一個姿態角的流暢波形。這中間的苦惱和困難只有自己知道。也行對於一個老手來說，我這幾天所做的所有事情，他能在一上午或者幾個小時內完成，但是這幾天所有的思考與疑惑都對我有很大的幫助。慢慢的養成了分析思考問題的方法。遇到問題不要慌張，一點點的去分析。不要放過任何一個細節。其實我們常說，一個經驗豐富的老手能很快的解決一個棘手的問題。他們靠的是什么？所有人都知道是經驗。但是經驗又是什么？說的通俗一點，經驗就是他們所有踩過的坑，進行過的所有思考。
想說一個我在做這個實驗小小的插曲，有一天的時間，波形出來的都是方波，稍微動一下就滿量程，波形很難看。想了很久，檢查了不知多少遍代碼，邏輯沒問題啊，也沒有報錯，怎么波形就不對呢？突然一瞬間，發現了一點小問題。一個計數變量，定義了一個unsigned char i;來進行零偏數據的計數，每500個數據進行一次平均，剛開始愣是沒發現不對。unsigned char 變量的最大計數是256！！！
一個計數變量 ，平時根本就不會去注意他的，但是關鍵時候就是他攪的局。改了這個錯誤之后，波形看起來好了很多。
首先在上次博客的基礎上，先進行一次滑動平均濾波，濾波窗口大致為6個數據。
/* **2017.12.5 **對6050的原始數據進行處理，濾波，角度轉換。 **滑動平均濾波 */ void mpu6050_data_process(void) { unsigned char i = 0, j = 0; static unsigned char acc_filter_cnt = 0;//濾波次數計數 static unsigned char gyro_filter_cnt = 0; long int xdata acc_temp[3] = {0};//三軸加速度,注意，這個不能定義為靜態 long int xdata gyro_temp[3] = {0}; static int acc_x_buf[ACC_FILTER_NUM] = {0}, //滑動窗口緩存，每調用一次，里面數據多一個。 acc_y_buf[ACC_FILTER_NUM] = {0}, acc_z_buf[ACC_FILTER_NUM] = {0}; static int gyro_x_buf[GYRO_FILTER_NUM] = {0}, //滑動窗口緩存，每調用一次，里面數據多一個。 gyro_y_buf[GYRO_FILTER_NUM] = {0}, gyro_z_buf[GYRO_FILTER_NUM] = {0}; float xdata init_ax = 0,init_ay = 0,init_az = 0; float xdata acc_angle_x = 0,gyro_angle_x = 0; //一階互補的參數傳遞 mpu6050_read_acc(); //獲取原始數據 mpu6050_read_gyro(); init_ax = (float)mpu_data.acc_data.x / ACC_G; //計算單位為G的各軸重力加速度分量 init_ay = (float)mpu_data.acc_data.y / ACC_G; init_az = (float)mpu_data.acc_data.z / ACC_G; acc_angle_x = atan(init_ax/init_az) * 180 / PI ; //加速度計x軸角度 gyro_angle_x = -(float)mpu_data.gyro_data.y / 7510.0; //陀螺儀計算x軸角度,7150?? mpu_data.acc_angle.x = (int)one_filter(acc_angle_x,gyro_angle_x); //一階互補濾波，獲取偏x軸角度 mpu_data.acc_angle.y = (int)kalman_filter(acc_angle_x,gyro_angle_x); //獲取卡爾曼濾波 //加速度計數據處理 //----------------------------------------------------- acc_x_buf[acc_filter_cnt] = mpu_data.acc_data.x;//平均濾波 ,更新數據，一開始這里寫成了加，數據一直不對 acc_y_buf[acc_filter_cnt] = mpu_data.acc_data.y; acc_z_buf[acc_filter_cnt] = mpu_data.acc_data.z; for(i=0;i<ACC_FILTER_NUM;i++) //滑動濾波,取平均值 { acc_temp[0]+= acc_x_buf[i]; acc_temp[1]+= acc_y_buf[i]; acc_temp[2]+= acc_z_buf[i]; } //在此進行 六面校准 //--------------------------- mpu_data.acc_filter.x = (acc_temp[0] / ACC_FILTER_NUM); //得出沿x軸方向的重力加速度， 單位G mpu_data.acc_filter.y = (acc_temp[1] / ACC_FILTER_NUM); mpu_data.acc_filter.z = (acc_temp[2] / ACC_FILTER_NUM); //-------------------------- acc_filter_cnt++; if(acc_filter_cnt == ACC_FILTER_NUM) { acc_filter_cnt = 0; } //陀螺儀數據處理 //----------------------------------------------------------- gyro_x_buf[gyro_filter_cnt] = mpu_data.gyro_data.x; gyro_y_buf[gyro_filter_cnt] = mpu_data.gyro_data.y; gyro_z_buf[gyro_filter_cnt] = mpu_data.gyro_data.z; for(j=0;j<GYRO_FILTER_NUM;j++) //滑動濾波,取平均值 { gyro_temp[0]+= gyro_x_buf[j]; gyro_temp[1]+= gyro_y_buf[j]; gyro_temp[2]+= gyro_z_buf[j]; } //在此進行 六面校准 //----------------------------- mpu_data.gyro_filter.x = gyro_temp[0]/GYRO_FILTER_NUM; mpu_data.gyro_filter.y = gyro_temp[1]/GYRO_FILTER_NUM; mpu_data.gyro_filter.z = gyro_temp[2]/GYRO_FILTER_NUM; //-------------------------- gyro_filter_cnt++; if(gyro_filter_cnt == GYRO_FILTER_NUM) { gyro_filter_cnt = 0; } //化為度制 // mpu_data.gyro_dps.x = (float)mpu_data.gyro_filter.x * GYRO_A; // mpu_data.gyro_dps.y = (float)mpu_data.gyro_filter.y * GYRO_A; // mpu_data.gyro_dps.z = (float)mpu_data.gyro_filter.z * GYRO_A; // // //化為弧度制 // mpu_data.gyro_rad.x = (float)mpu_data.gyro_filter.x * GYRO_R; // mpu_data.gyro_rad.y = (float)mpu_data.gyro_filter.y * GYRO_R; // mpu_data.gyro_rad.z = (float)mpu_data.gyro_filter.z * GYRO_R; }
數據波形為：
這些僅僅是對原始數據的濾波處理，因為加速度計包含了太多的噪聲。陀螺儀一般不需要濾波，原始值就很穩定，就是陀螺儀剛開始可能會有零偏，需要我們手動減去這個值。MPU6050 可以輸出三軸的加速度和角速度。通過加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是說有兩組 Pitch、Roll 角，到底應該選哪組呢？別急，先分析一下。MPU6050 的加速度計和陀螺儀各有優缺點，三軸的加速度值沒有累積誤差，且通過算 tan() 可以得到傾角，但是它包含的噪聲太多（因為待測物運動時會產生加速度，電機運行時振動會產生加速度等），不能直接使用；陀螺儀對外界振動影響小，精度高，通過對角速度積分可以得到傾角，但是會產生累積誤差。所以，不能單獨使用 MPU6050 的加速度計或陀螺儀來得到傾角，需要互補。一階互補算法的思想就是給加速度和陀螺儀不同的權值，把它們結合到一起，進行修正。
然后就可以利用這個加速度計和陀螺儀的數據進行姿態融合。姿態融合一方面對數據進行了濾波，一方面計算出了角度值。所以也可以利用原始數據進行姿態融合。因為我使用的是51系列的單片機，運算速率很慢，在此我直接用原始數據除精度后進行一階互補濾波，得出了x軸的偏移角度。然后再用卡爾曼濾波對姿態進行融合，對比兩種融合方式的波形。
loat xdata one_filter_angle = 0; float xdata kalman_filter_angle = 0, kalman_filter_angle_dot = 0; //---------------- //在進行濾波之前，需要獲取原始數據 float one_filter(float angle_m,float gyro_m) { float xdata K1 =0.1; // 對加速度計取值的權重 float xdata dt=0.005;//注意：dt的取值為濾波器采樣時間 one_filter_angle = K1 * angle_m+ (1-K1) * (one_filter_angle + gyro_m * dt); return one_filter_angle; } //------------------ float kalman_filter(float angle_m,float gyro_m) { //濾波參數 float xdata dt = 0.005; //卡爾曼采樣時間 float xdata P[2][2] = {{1,0},{0,1}}; float xdata Pdot[4] = {0,0,0,0}; float xdata Q_angle = 0.001;//角度數據置信度,陀螺儀協方差 float xdata Q_gyro = 0.005; //角速度數據置信度，陀螺儀飄移噪聲協方差 float xdata R_angle = 0.5; //加速度計協方差 char xdata C_0 = 1; float xdata q_bias = 0,angle_err = 0; //q_bias為陀螺儀飄移 float xdata PCt_0 = 0,PCt_1 = 0,E = 0; float xdata K_0 = 0, K_1 = 0, t_0 = 0, t_1 = 0; kalman_filter_angle+= (gyro_m - q_bias) * dt; //卡爾曼預測方程，認為每次飄移相同， Pdot[0]=Q_angle - P[0][1] - P[1][0]; Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; PCt_0 = C_0 * P[0][0]; //矩陣乘法中間變量 PCt_1 = C_0 * P[1][0]; E = R_angle + C_0 * PCt_0; //分母 K_0 = PCt_0 / E; //增益值 K_1 = PCt_1 / E; angle_err = angle_m - kalman_filter_angle; kalman_filter_angle += K_0 * angle_err; //對狀態的卡爾曼估計，最優角度 q_bias += K_1 * angle_err; kalman_filter_angle_dot = gyro_m-q_bias;//最優角速度 t_0 = PCt_0; //矩陣計算中間變量 t_1 = C_0 * P[0][1]; P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; return kalman_filter_angle; }
融合之后的波形：
