背景

使用的開發板為大疆的 RoboMaster-C 型開發板，基礎工程為 rt-thread>bsp>stm32f407-robomaster-c

IMU姿態解算

使用 BMI088 robomaster-c 開發板上集成的6軸 imu 進行姿態解算，使用的方案其實是RM中比較普遍成熟的一套，主要使用了四元數和卡爾曼濾波進行融合解算，解算頻率為 1Khz。

dt = dwt_get_delta(&ins_dwt);
    imu_ops.gyro_read(ins.gyro);
    imu_ops.accel_read(ins.accel);
    // 核心函數,EKF更新四元數
    IMU_QuaternionEKF_Update(ins.gyro[X], ins.gyro[Y], ins.gyro[Z], ins.accel[X], ins.accel[Y], ins.accel[Z], dt);
    memcpy(ins.q, QEKF_INS.q, sizeof(QEKF_INS.q));
    // 機體系基向量轉換到導航坐標系，本例選取慣性系為導航系
    BodyFrameToEarthFrame(xb, ins.xn, ins.q);
    BodyFrameToEarthFrame(yb, ins.yn, ins.q);
    BodyFrameToEarthFrame(zb, ins.zn, ins.q);
    // 將重力從導航坐標系n轉換到機體系b,隨后根據加速度計數據計算運動加速度
    float gravity_b[3];
    EarthFrameToBodyFrame(gravity, gravity_b, ins.q);
    for (uint8_t i = 0; i < 3; ++i) // 同樣過一個低通濾波
    {
        ins.motion_accel_b[i] = (ins.accel[i] - gravity_b[i]) * dt / (ins.accel_lpf + dt) + ins.motion_accel_b[i] * ins.accel_lpf / (ins.accel_lpf + dt);
    }
    BodyFrameToEarthFrame(ins.motion_accel_b, ins.motion_accel_b, ins.q); // 轉換回導航系n
    ins.yaw = QEKF_INS.Yaw;
    ins.pitch = QEKF_INS.Pitch;
    ins.roll = QEKF_INS.Roll;
    ins.yaw_total_angle = QEKF_INS.YawTotalAngle;

恒溫控制

C型開發板上的 BMI088 周圍是又一圈加熱電阻的，可以通過 PWM 控制加熱功率，從而實現恒溫控制，有助于抑制陀螺儀漂移，根據手冊提示恒溫控制在40攝氏度較好，

static pid_obj_t *imu_temp_pid;
static pid_config_t imu_temp_config = {
.Kp = 50000,
.Ki = 8000,
.Kd = 0,
.IntegralLimit = 50000,
.Improve = PID_Integral_Limit,
.MaxOut = 250000,
};
static rt_err_t temp_pwm_init(rt_uint32_t period, rt_uint32_t pulse)
{
temp_pwm_dev = (struct rt_device_pwm )rt_device_find(TEMP_PWM_DEV_NAME);
if (temp_pwm_dev == RT_NULL)
{
LOG_E("Can't find %s device!", TEMP_PWM_DEV_NAME);
return -RT_ERROR;
}
/ 設置PWM周期和脈沖寬度默認值 /
rt_pwm_set(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL, period, pulse);
/ 使能設備 */
rt_pwm_enable(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL);
}

在 ins_task 中以 500hz 的頻率進行恒溫控制，需要注意，使用加熱電阻外圍電路需要給C板額外供電。

void ins_thread_entry(void argument)
{
static uint32_t count;
temp_pwm_init(period, pulse);
/ 注冊 PID 實例 */
imu_temp_pid = pid_register(&imu_temp_config);
imu_ops.imu_init();
LOG_I("Example Task Start");
for (;;)
{
example_start = dwt_get_time_ms();
imu_ops.gyro_read(gyro);
imu_ops.accel_read(acc);
if(count % 2 == 0){
temp = imu_ops.temp_read();
pulse = pid_calculate(imu_temp_pid, temp, IMU_TARGET_TEMP);
rt_pwm_set_pulse(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL, pulse);
}
count++;
rt_thread_delay(1);
}
}

實際效果測試如下：

不加恒溫控制，室溫下（23攝氏度左右）十分鐘內，yaw 軸偏移近20度；
加入恒溫控制后（40攝氏度），十分鐘內，yaw 軸偏移10度
arm_math庫使用
有一個小插曲就是arm_math庫的移植使用，需要通過修改 Scons 文件，將 arm_math 庫鏈接到工程中，并添加需要的宏定義：

使用 arm_math.h 需要添加相關內核定義

CPPDEFINES = ['ARM_MATH_CM4']
LIBPATH = [cwd + '/arm_math']
LIBS = ['libarm_cortexM4lf_math.a']
path += [cwd + '/arm_math']
group = DefineGroup('RM_Algorithms', src, depend = [''], CPPPATH = path, CPPDEFINES = CPPDEFINES, LIBS = LIBS, LIBPATH=LIBPATH)

到此就可以使用解算得到的歐拉角等數據去對云臺等進行閉環控制啦。

存在問題及優化方向

雖然通過陀螺儀校準和恒溫控制等有效抑制了零漂，但yaw的零飄依然存在；
之后考慮通過融合磁力計數據，解決零飄問題；