基于RT-Thread的RoboMaster电控框架（四）

背景

使用的开发板为大疆的 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的零飘依然存在；
之后考虑通过融合磁力计数据，解决零飘问题；