evil0knight's Blog

底盘解算程序

7分钟阅读

底盘解算程序

← 返回 MOC | ← 主页

代码来源工程：Intelligent_Agricultural_Equipment_Innovation_Competition（Mycode/mike.c）

正解（给定 Vx/Vy/ω 求四轮转速）

平台无关纯算法。lw 为底盘中心到轮子的纵向半距离 与横向半距离 之和，单位与速度单位一致。

/**
 * @brief 麦轮底盘正解：给定底盘速度，计算四轮转速
 *
 * @param vx    纵向速度（向前为正）
 * @param vy    横向速度（向左为正）
 * @param omega 旋转角速度（逆时针为正）
 * @param lw    l+w（底盘中心到轮子纵向距离与横向距离之和）
 * @param v_fl  输出：左前轮速度
 * @param v_fr  输出：右前轮速度
 * @param v_rl  输出：左后轮速度
 * @param v_rr  输出：右后轮速度
 */
void mecanum_forward(float vx, float vy, float omega, float lw,
                     float *v_fl, float *v_fr, float *v_rl, float *v_rr)
{
    float rot = lw * omega;
 
    *v_fl = vx - vy - rot;
    *v_fr = vx + vy + rot;
    *v_rl = vx + vy - rot;
    *v_rr = vx - vy + rot;
}
 
/* 使用示例 */
/*
float v_fl, v_fr, v_rl, v_rr;
float lw = 200.0f;  // 按实际底盘尺寸填写，单位 mm
 
mecanum_forward(500.0f, 0.0f, 0.0f, lw,
                &v_fl, &v_fr, &v_rl, &v_rr);
// 纯前进：四轮速度均为 500
*/

逆解（给定四轮转速求底盘速度）

常用于里程计：读取编码器实际转速，反推底盘当前运动状态。

/**
 * @brief 麦轮底盘逆解：给定四轮转速，计算底盘速度
 *
 * @param v_fl  左前轮实际速度
 * @param v_fr  右前轮实际速度
 * @param v_rl  左后轮实际速度
 * @param v_rr  右后轮实际速度
 * @param lw    l+w
 * @param vx    输出：纵向速度
 * @param vy    输出：横向速度
 * @param omega 输出：旋转角速度
 */
void mecanum_inverse(float v_fl, float v_fr, float v_rl, float v_rr,
                     float lw,
                     float *vx, float *vy, float *omega)
{
    *vx    = (v_fl + v_fr + v_rl + v_rr) / 4.0f;
    *vy    = (-v_fl + v_fr + v_rl - v_rr) / 4.0f;
    *omega = (-v_fl + v_fr - v_rl + v_rr) / (4.0f * lw);
}
 
/* 使用示例 */
/*
float vx, vy, omega;
float lw = 200.0f;
 
mecanum_inverse(encoder_fl, encoder_fr, encoder_rl, encoder_rr,
                lw, &vx, &vy, &omega);
// omega 接近 0 说明底盘无旋转，可用于跑偏检测
*/

里程计（编码器位移 → 世界坐标位姿）

逆解的典型应用。每个控制周期把四轮位移增量代入逆解得到底盘坐标系下的 ，再通过旋转变换累积到世界坐标系。

用本周期中点角度 做旋转变换，精度优于直接用起点或终点角度（相当于二阶近似）。

#include <math.h>
 
typedef struct {
    float x;      // 世界坐标系 x
    float y;      // 世界坐标系 y
    float theta;  // 朝向角（rad）
} Odom_t;
 
/**
 * @brief 麦轮里程计累积更新
 *
 * @param odom   里程计状态（需在外部持久化）
 * @param ds_fl  本周期左前轮位移（编码器脉冲 × 系数）
 * @param ds_fr  本周期右前轮位移
 * @param ds_rl  本周期左后轮位移
 * @param ds_rr  本周期右后轮位移
 * @param lw     l+w
 */
void mecanum_odom_update(Odom_t *odom,
                         float ds_fl, float ds_fr,
                         float ds_rl, float ds_rr,
                         float lw)
{
    /* 1. 逆解：底盘坐标系下的位移与角度增量 */
    float dx     = (ds_fl + ds_fr + ds_rl + ds_rr) / 4.0f;
    float dy     = (-ds_fl + ds_fr + ds_rl - ds_rr) / 4.0f;
    float dtheta = (-ds_fl + ds_fr - ds_rl + ds_rr) / (4.0f * lw);
 
    /* 2. 用中点角度做旋转变换到世界坐标系 */
    float mid = odom->theta + dtheta * 0.5f;
    float c   = cosf(mid);
    float s   = sinf(mid);
 
    odom->x     += dx * c - dy * s;
    odom->y     += dx * s + dy * c;
    odom->theta += dtheta;
}
 
/* 使用示例 */
/*
static Odom_t odom = {0};
static int32_t last_fl, last_fr, last_rl, last_rr;
float lw = 200.0f;
 
// 每个脉冲对应的距离，按实测标定（四轮可能略有差异）
const float COEF_FL = 0.04047f;
const float COEF_FR = 0.04018f;
const float COEF_RL = 0.04020f;
const float COEF_RR = 0.04034f;
 
// 控制周期内调用：
int32_t now_fl = encoder_read(FL);
int32_t now_fr = encoder_read(FR);
int32_t now_rl = encoder_read(RL);
int32_t now_rr = encoder_read(RR);
 
float ds_fl = (now_fl - last_fl) * COEF_FL;
float ds_fr = (now_fr - last_fr) * COEF_FR;
float ds_rl = (now_rl - last_rl) * COEF_RL;
float ds_rr = (now_rr - last_rr) * COEF_RR;
 
last_fl = now_fl;  last_fr = now_fr;
last_rl = now_rl;  last_rr = now_rr;
 
mecanum_odom_update(&odom, ds_fl, ds_fr, ds_rl, ds_rr, lw);
// odom.x / odom.y / odom.theta 即为底盘在世界坐标系下的位姿
*/

带跑偏补偿的正解（陀螺仪 PID 叠加）

在正解基础上，将陀螺仪 PID 输出叠加到旋转分量，补偿平动时的非期望偏转。纯平动时 target_omega 传 0。

/* 平台无关 PID 状态结构体 */
typedef struct {
    float kp;
    float ki;
    float kd;
    float integral;
    float last_error;
} PID_t;
 
static float pid_update(PID_t *pid, float error)
{
    pid->integral   += error;
    float derivative = error - pid->last_error;
    pid->last_error  = error;
    return pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
}
 
/**
 * @brief 带跑偏补偿的麦轮正解
 *
 * @param vx           纵向速度
 * @param vy           横向速度
 * @param target_omega 期望角速度（纯平动时为 0）
 * @param gyro_omega   陀螺仪实测角速度
 * @param lw           l+w
 * @param pid          跑偏补偿 PID 状态（需在外部持久化）
 * @param v_fl/fr/rl/rr 输出四轮速度
 */
void mecanum_forward_compensated(float vx, float vy,
                                 float target_omega, float gyro_omega,
                                 float lw, PID_t *pid,
                                 float *v_fl, float *v_fr,
                                 float *v_rl, float *v_rr)
{
    float error        = target_omega - gyro_omega;
    float compensation = pid_update(pid, error);
    float omega_cmd    = target_omega + compensation;
 
    mecanum_forward(vx, vy, omega_cmd, lw, v_fl, v_fr, v_rl, v_rr);
}
 
/* 使用示例 */
/*
PID_t drift_pid = {
    .kp = 0.5f,
    .ki = 0.0f,    // 先置 0，调稳后再加
    .kd = 0.05f,
    .integral   = 0.0f,
    .last_error = 0.0f
};
 
float v_fl, v_fr, v_rl, v_rr;
float lw = 200.0f;
 
// 在控制周期内循环调用：
mecanum_forward_compensated(
    rc_vx, rc_vy,
    0.0f,           // 纯平动，期望角速度为 0
    gyro_get_z(),   // 替换为实际陀螺仪读取函数
    lw, &drift_pid,
    &v_fl, &v_fr, &v_rl, &v_rr
);
*/

关系图谱

反向链接