利用Cordic算法來進(jìn)行姿態(tài)解算

二. 踩坑分享

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

三. 姿態(tài)解算

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

1. 通過加速度求解

先看一下加速度求解角度的表達(dá)式(通過加速度是無法求解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. 通過角速度求解

通過角速度的求解就更簡單了，只需要將當(dāng)前角度加上(角速度×dt)就可以。角速度求解的時候會有些問題，在靜態(tài)的時候，角速度會有零漂，這個時候角度誤差會越來越大。

3. 融合

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

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

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

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

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

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

1. 模塊接口

輸入請求，輸出應(yīng)答和三個角度，角度值擴(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 時鐘輸出*/
  inout            IICSDA       /*IIC 數(shù)據(jù)線*/
);

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

這里使用到了兩個Cordic模塊，第一個模塊先計(jì)算出roll和sqrt(acc_y*acc_y + acc_z * acc_z)的值，然后第二個模塊通過acc_x和sqrt(acc_y*acc_y + acc_z * acc_z)的值 計(jì)算出 pitch的角度。最后對數(shù)據(jù)經(jīng)過了一個簡單的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. 融合

這里的融合，暫時沒有做，只對加速度求解的角度進(jìn)行了一個濾波處理，后面會根據(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    )
);

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

審核編輯：劉清