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

背景

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

IMU姿態(tài)解算

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

dt = dwt_get_delta(&ins_dwt);
    imu_ops.gyro_read(ins.gyro);
    imu_ops.accel_read(ins.accel);
    // 核心函數(shù),EKF更新四元數(shù)
    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));
    // 機(jī)體系基向量轉(zhuǎn)換到導(dǎo)航坐標(biāo)系，本例選取慣性系為導(dǎo)航系
    BodyFrameToEarthFrame(xb, ins.xn, ins.q);
    BodyFrameToEarthFrame(yb, ins.yn, ins.q);
    BodyFrameToEarthFrame(zb, ins.zn, ins.q);
    // 將重力從導(dǎo)航坐標(biāo)系n轉(zhuǎn)換到機(jī)體系b,隨后根據(jù)加速度計(jì)數(shù)據(jù)計(jì)算運(yùn)動加速度
    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); // 轉(zhuǎn)換回導(dǎo)航系n
    ins.yaw = QEKF_INS.Yaw;
    ins.pitch = QEKF_INS.Pitch;
    ins.roll = QEKF_INS.Roll;
    ins.yaw_total_angle = QEKF_INS.YawTotalAngle;

恒溫控制

C型開發(fā)板上的 BMI088 周圍是又一圈加熱電阻的，可以通過 PWM 控制加熱功率，從而實(shí)現(xiàn)恒溫控制，有助于抑制陀螺儀漂移，根據(jù)手冊提示恒溫控制在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;
}
/ 設(shè)置PWM周期和脈沖寬度默認(rèn)值 /
rt_pwm_set(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL, period, pulse);
/ 使能設(shè)備 */
rt_pwm_enable(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL);
}

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

void ins_thread_entry(void argument)
{
static uint32_t count;
temp_pwm_init(period, pulse);
/ 注冊 PID 實(shí)例 */
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);
}
}

實(shí)際效果測試如下：

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

使用 arm_math.h 需要添加相關(guān)內(nèi)核定義

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)

到此就可以使用解算得到的歐拉角等數(shù)據(jù)去對云臺等進(jìn)行閉環(huán)控制啦。

存在問題及優(yōu)化方向

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