二. 踩坑分享

在進(jìn)行姿態(tài)解算分享之前，先分享一個(gè)踩坑經(jīng)歷。一般來(lái)說(shuō)MPU6050的ID讀出為0x68，淘寶上買(mǎi)到的模塊，基本上都是這個(gè)。但是我使用的是自己畫(huà)的PCB，手動(dòng)焊接的，在讀取ID的時(shí)候，一直為0x98，但是認(rèn)知中要為0x68才是對(duì)的，這個(gè)時(shí)候就會(huì)懷疑是不是自己的程序或者焊接的問(wèn)題了。但好在后面讀取六軸數(shù)據(jù)，姿態(tài)解算后得到的角度基本是正確的(折騰了一天了，才發(fā)現(xiàn))。這是個(gè)坑，大家可以注意一下。

三. 姿態(tài)解算

所謂姿態(tài)解算就是通過(guò)六軸的數(shù)據(jù)，來(lái)求解物體的三個(gè)角度: roll , pitch , yaw。

1. 通過(guò)加速度求解

先看一下加速度求解角度的表達(dá)式(通過(guò)加速度是無(wú)法求解yaw的)。atan(acc_y / acc_x)和sqrt(acc_y*acc_y + acc_z * acc_z)不就是上篇中Cordic算法求解的值嗎,都不需要使用的其他的計(jì)算。

roll = atan(acc_y / acc_x);
pitch = atan(acc_x / (sqrt(acc_y*acc_y + acc_z * acc_z)));

2. 通過(guò)角速度求解

通過(guò)角速度的求解就更簡(jiǎn)單了，只需要將當(dāng)前角度加上(角速度×dt)就可以。角速度求解的時(shí)候會(huì)有些問(wèn)題，在靜態(tài)的時(shí)候，角速度會(huì)有零漂，這個(gè)時(shí)候角度誤差會(huì)越來(lái)越大。

3. 融合

可以看到有上面的兩種方法求解角度，可以單獨(dú)使用，但是可能會(huì)不太準(zhǔn)確，精度要求不高的場(chǎng)合可以只使用加速度求解。在精度要求比較高的場(chǎng)合下，需要使用這兩種方法求解，然后再將求得的結(jié)果進(jìn)行融合。常用的方法有: 卡爾曼濾波、一階互補(bǔ)濾波、二階互補(bǔ)濾波。

一階互補(bǔ)濾波，如下，簡(jiǎn)單粗暴。要想濾波效果好的話(huà)，可以試試卡爾曼濾波。

roll = a * acc_roll + (1 - a) *gyro_roll;

以上只是一種比較常規(guī)的求解方法，追求高精度的話(huà)，可以使用四元數(shù)的方法進(jìn)行求解(復(fù)雜度大大增加)。

四. 代碼實(shí)現(xiàn)

代碼都是現(xiàn)成的，在之前的文章中已經(jīng)寫(xiě)好了，這里做的工作就是將這些模塊組合在一起。

1. 模塊接口

輸入請(qǐng)求，輸出應(yīng)答和三個(gè)角度，角度值擴(kuò)大了2^16倍。

module IMU(
  input            clk,    //27M
  input            rst_n,


  input            imu_req,
  output           imu_ack,


  output signed[31:0]     roll,
  output signed[31:0]     pitch,
  output signed[31:0]     yaw,


  output           IICSCL,       /*IIC 時(shí)鐘輸出*/
  inout            IICSDA       /*IIC 數(shù)據(jù)線(xiàn)*/
);

2. 狀態(tài)機(jī)

這里使用到了兩個(gè)Cordic模塊，第一個(gè)模塊先計(jì)算出roll和sqrt(acc_y*acc_y + acc_z * acc_z)的值，然后第二個(gè)模塊通過(guò)acc_x和sqrt(acc_y*acc_y + acc_z * acc_z)的值 計(jì)算出 pitch的角度。最后對(duì)數(shù)據(jù)經(jīng)過(guò)了一個(gè)簡(jiǎn)單的FIR濾波。

always@(*) begin
  case(state)
  S_IDLE:
    if( imu_req == 1'b1)
      next_state <= S_READ_MPU6050;
 ? ? ? else
 ? ? ? ? ? next_state <= S_IDLE;
 ? S_READ_MPU6050:
 ? ? ? if( mpu6050_ack == 1'b1 )
 ? ? ? ? ? next_state <= S_Cordic;
 ? ? ? else
 ? ? ? ? ? next_state <= S_READ_MPU6050;
 ? S_Cordic:
 ? ? ? if( cordic_ack == 1'b1)
 ? ? ? ? ? next_state <= S_Cordic2;
 ? ? ? else
 ? ? ? ? ? next_state <= S_Cordic;
 ? S_Cordic2:
 ? ? ? if( cordic2_ack == 1'b1)
 ? ? ? ? ? next_state <= S_FILTER;
 ? ? ? else
 ? ? ? ? ? next_state <= S_Cordic2;
 ? S_FILTER:
 ? ? ? if( fir_filter_ack == 1'b1)
 ? ? ? ? ? next_state <= S_ACK;
 ? ? ? else
 ? ? ? ? ? next_state <= S_FILTER;
 ? S_ACK:
 ? ? ? next_state <= S_IDLE;
 ? default: ? ? next_state <= S_IDLE;
 ? endcase
end

3. 融合

這里的融合，暫時(shí)沒(méi)有做，只對(duì)加速度求解的角度進(jìn)行了一個(gè)濾波處理，后面會(huì)根據(jù)需要再進(jìn)行更新。

FIR_Filter FIR_Filter_HP(


  .clk            (      clk       ),
  .rst_n           (      rst_n      ),


  .fir_filter_req       (      fir_filter_req ),
  .fir_filter_ack       (      fir_filter_ack ),


  .filter_data_in       (      theta      ),
  .filter_data_out      (      acc_roll    )
);




FIR_Filter FIR_Filter_HP2(


  .clk            (      clk       ),
  .rst_n           (      rst_n      ),


  .fir_filter_req       (      fir_filter_req ),
  .fir_filter_ack       (              ),


  .filter_data_in       (      theta2      ),
  .filter_data_out      (      acc_pitch    )
);

模塊已上板測(cè)試，解算出來(lái)的角度沒(méi)有問(wèn)題(可能精度不是那么完美，濾波與融合那里需要下點(diǎn)功夫)。

審核編輯：劉清