底盘解算

  \  /       1号(A轮)  2号(B轮)
  /  \       3号(B轮)  4号(A轮)

  /  \       1号(B轮)  2号(A轮)
  \  /       3号(A轮)  4号(B轮)

float s = mecanum_ctrl_handle.roller_sign; /* X型: +1, O型: -1 */
speed_wheel[0] = (1.0f / r) * (speedx - s * speedy + s * k * target_speedw);
speed_wheel[1] = (1.0f / r) * (speedx + s * speedy - s * k * target_speedw);
speed_wheel[2] = (1.0f / r) * (speedx + s * speedy + s * k * target_speedw);
speed_wheel[3] = (1.0f / r) * (speedx - s * speedy - s * k * target_speedw);

float s = mecanum_ctrl_handle.roller_sign;

*vx = (r / 4.0f) * (wheel_speed[0] + wheel_speed[1] +
                     wheel_speed[2] + wheel_speed[3]);
*vy = s * (r / 4.0f) * (-wheel_speed[0] + wheel_speed[1] +
                          wheel_speed[2] - wheel_speed[3]);
*vw = s * (r / (4.0f * k)) * (wheel_speed[0] - wheel_speed[1] +
                               wheel_speed[2] - wheel_speed[3]);

if (mecanum_ctrl_handle.world_coordinate) {
    speedx = COS_F32(world_angle_rad) * target_speedx +
             SIN_F32(world_angle_rad) * target_speedy;
    speedy = -SIN_F32(world_angle_rad) * target_speedx +
             COS_F32(world_angle_rad) * target_speedy;
} else {
    speedx = target_speedx;
    speedy = target_speedy;
}

#define MECANUM_WHEEL_NUM 4 /* 麦轮底盘固定为四轮 */

/* 装配模式枚举 */
typedef enum {
    MECANUM_X_TYPE = 0, /* X型装配: 对角线辊子同向 */
    MECANUM_O_TYPE = 1, /* O型装配: 同侧辊子同向 */
} mecanum_assembly_t;

/* 电机控制函数指针 */
typedef void (*mecanum_wheel_ctrl_t)(float /* speed */[]);

#ifdef USE_ARM_MATH
#include "arm_math.h"
#define SIN_F32(x)  arm_sin_f32(x)
#define COS_F32(x)  arm_cos_f32(x)
#else
#include <math.h>
#define SIN_F32(x)  sinf(x)
#define COS_F32(x)  cosf(x)
#endif

/**
 * @brief 麦轮底盘初始化
 *
 * @param wheel_radius    轮子半径, 单位 m
 * @param half_wheelbase  半轴距 (轮子中心到底盘中心纵向距离), 单位 m
 * @param half_track      半轮距 (轮子中心到底盘中心横向距离), 单位 m
 * @param max_wheel_speed 单轮最大转速, 用于轮速归一化
 * @param wheel_ctrl      电机控制函数指针
 * @param yaw_angle       世界坐标系下偏航角指针, 范围 [-180, +180] DEG
 * @param coordinate      坐标系选择: 世界坐标[true], 局部坐标[false]
 * @param assembly        装配模式: MECANUM_X_TYPE 或 MECANUM_O_TYPE
 */
void mecanum_wheel_init(float wheel_radius, float half_wheelbase,
                        float half_track, float max_wheel_speed,
                        mecanum_wheel_ctrl_t wheel_ctrl,
                        float *yaw_angle, bool coordinate,
                        mecanum_assembly_t assembly);

/**
 * @brief 麦轮底盘速度控制（逆运动学）
 *
 * @param target_speedx 目标速度x（前进方向）
 * @param target_speedy 目标速度y（左侧方向）
 * @param target_speedw 目标自转速度w（逆时针为正）
 */
void mecanum_wheel_ctrl(float target_speedx, float target_speedy,
                        float target_speedw);

/**
 * @brief 麦轮底盘正运动学解算
 *
 * @param wheel_speed 四个轮子的转速数组
 * @param vx 输出: x 方向速度 (m/s)
 * @param vy 输出: y 方向速度 (m/s)
 * @param vw 输出: 自转角速度 (rad/s)
 */
void mecanum_forward_kinematics(const float wheel_speed[], float *vx,
                                float *vy, float *vw);

void mecanum_set_world_coordinate(void);   /* 切换到世界坐标系 */
void mecanum_set_self_coordinate(void);    /* 切换到自身坐标系 */
bool mecanum_is_world_coordinate(void);    /* 查询当前坐标系模式 */

void mecanum_set_assembly(mecanum_assembly_t assembly); /* 切换装配模式 */
mecanum_assembly_t mecanum_get_assembly(void);          /* 查询当前装配模式 */

void mecanum_set_halt(bool halt);  /* 设置驻停 */
bool mecanum_get_halt(void);       /* 查询驻停状态 */

              (1)
            /     \
          /         \
        /             \
      /                 \
    /                     \
  (2)----------------------(3)

坐标系：
      y
      |
      |       z轴逆时针正
      |
      |
     (0)——————————>x

#if (3 == OMNI_WHEEL_NUM)
    speed_wheel[0] = -speedx + speedw;
    speed_wheel[1] = speedx * ONE_2 - speedy * SQRT3_2 + speedw;
    speed_wheel[2] = speedx * ONE_2 + speedy * SQRT3_2 + speedw;
#endif

      (1)--------------------(2)
       |                      |
       |                      |
       |         (O)          |
       |                      |
       |                      |
      (3)--------------------(4)

坐标系：
      y
      |
      |       z轴逆时针正
      |
      |
     (0)——————————>x

#elif (4 == OMNI_WHEEL_NUM)
    speed_wheel[0] = -speedx * SQRT2_2 - speedy * SQRT2_2 + speedw;
    speed_wheel[1] = -speedx * SQRT2_2 + speedy * SQRT2_2 + speedw;
    speed_wheel[2] =  speedx * SQRT2_2 - speedy * SQRT2_2 + speedw;
    speed_wheel[3] =  speedx * SQRT2_2 + speedy * SQRT2_2 + speedw;
#endif

if (world_coordinate) {
    /* 如果开启世界坐标 */
    speedx = COS_F32(world_angle_rad) * target_speedx +
             SIN_F32(world_angle_rad) * target_speedy;
    speedy = -SIN_F32(world_angle_rad) * target_speedx +
             COS_F32(world_angle_rad) * target_speedy;
    speedw = target_speedw * OMNI_RADIU;
} else {
    /* 如果关闭世界坐标 */
    speedx = target_speedx;
    speedy = target_speedy;
    speedw = target_speedw;
}

#define OMNI_WHEEL_NUM 4        /* 轮子数量：3 或 4，编译时选择 */
#define OMNI_RADIU     1.0f     /* 轮子到中心距离（单位：m），需根据实际硬件修改 */

/* 全向轮底盘电机控制函数指针 */
typedef void (*omni_single_wheel_ctrl_t)(float /* speed */[]);

#ifdef USE_ARM_MATH
#include "arm_math.h"
#define SIN_F32(x)  arm_sin_f32(x)
#define COS_F32(x)  arm_cos_f32(x)
#else
#include <math.h>
#define SIN_F32(x)  sinf(x)
#define COS_F32(x)  cosf(x)
#endif

/**
 * @brief 全向轮底盘初始化
 *
 * @param wheel_ctrl  电机控制函数指针，接收各轮转速数组
 * @param yaw_angle   世界坐标系下的偏航角指针，范围 [-π, +π]
 * @param coordinate  坐标系选择：true=世界坐标，false=自身坐标
 */
void omni_wheel_init(omni_single_wheel_ctrl_t wheel_ctrl,
                     float *yaw_angle, bool coordinate);

/**
 * @brief 全向轮底盘速度控制
 *
 * @param target_speedx 目标速度x（前进方向）
 * @param target_speedy 目标速度y（左侧方向）
 * @param target_speedw 目标自转速度w（逆时针为正）
 */
void omni_wheel_ctrl(float target_speedx, float target_speedy,
                     float target_speedw);

void omni_set_world_coordinate(void);   /* 切换到世界坐标系 */
void omni_set_self_coordinate(void);    /* 切换到自身坐标系 */
bool omni_is_world_coordinate(void);    /* 查询当前坐标系模式 */

[图1占位符：舵轮模块结构示意图]
![](Pictures/底盘解算/swerve_module_structure.png)

[图2占位符：舵轮速度矢量图]
![](Pictures/底盘解算/swerve_velocity_vector.png)

static struct {
    float radius;                           /* 轮子到中心的距离 */
    float *world_angle;                     /* 世界坐标的角度指针 */
    bool halt;                              /* 是否驻停 */
    float set_wheel_angle[WHEEL_NUM];       /* 设置的轮子角度 */
    float set_wheel_speed[WHEEL_NUM];       /* 设置的轮子速度 */
    float *steering_wheel_angle[WHEEL_NUM]; /* 每个舵向电机当前的角度 */
    steering_direction_ctrl_t steering_direction_ctrl; /* 舵向电机控制函数 */
    steering_speed_ctrl_t steering_speed_ctrl;         /* 航向电机控制函数 */
} steering_ctrl_handle;

/**
 * @param chassis_radius 轮子到中心的距离, 单位 m
 * @param world_angle    车身相对于世界坐标的角度指针 (yaw), 范围 [-180, +180] DEG
 * @param direction_ctrl 舵向电机控制函数指针
 * @param speed_ctrl     航向电机控制函数指针
 * @param steering_motor_real_angle 舵向电机角度指针, 范围 [0, 2π] RAD
 */
void steering_wheel_init(float chassis_radius, float *world_angle,
                         const steering_direction_ctrl_t direction_ctrl,
                         const steering_speed_ctrl_t speed_ctrl,
                         float *steering_motor_real_angle[]);

#define WHEEL_NUM 3  /* 或 4，与底盘实际配置保持一致 */

布局示意图（俯视图）：
       (1)------------------(2)
        |                    |
        |       底盘         |
        |     (中心点)       |
        |                    |
       (3)------------------(4)

编号规则：
- 1号轮：前左（Z字形起点）
- 2号轮：前右
- 3号轮：后左
- 4号轮：后右
- 轮子位置：距中心距离为 radius

/* 舵向电机角度解算 (极坐标映射) */
wheel_angle[i] = mix_x[i] < 0
                     ? ACOS_F32(mix_y[i] / wheel_speed[i])
                     : -ACOS_F32(mix_y[i] / wheel_speed[i]);

/**
 * @brief 角度重映射函数，将角度映射到定义域 [-pi, pi]
 * @param input 需要映射角度
 * @return 映射后的角度
 */
static float remap(float input) {
    input = fmod(input, 2 * PI);
    if (my_fabs(input) > PI) {
        input = input > 0 ? (input - 2 * PI) : (input + 2 * PI);
    }
    return input;
}

[图1占位符：逆运动学解算流程图]
![](Pictures/底盘解算/swerve_algorithm_flow.png)

[图2占位符：最优转角选择示意图]
![](Pictures/底盘解算/swerve_angle_optimization.png)

[图3占位符：底盘几何参数标注图]
![](Pictures/底盘解算/swerve_geometry_params.png)

[图4占位符：矢量叠加原理示意图]
![](Pictures/底盘解算/swerve_vector_superposition.png)

void steering_wheel_ctrl(float speedx, float speedy, float speedw);

static float mix_x[WHEEL_NUM] = {0};    /* 自身坐标系下x方向上的合速度 */
static float mix_y[WHEEL_NUM] = {0};    /* 自身坐标系下y方向上的合速度 */
static float speed_wx[WHEEL_NUM] = {0}; /* 自转速度分解到自身坐标系x轴上 */
static float speed_wy[WHEEL_NUM] = {0}; /* 自转速度分解到自身坐标系y轴上 */

static float wheel_angle[WHEEL_NUM] = {0}; /* 轮子的角度 */
static float wheel_speed[WHEEL_NUM] = {0}; /* 轮子的速度 */

static float Vx = 0.0f; /* 底盘世界坐标系x方向合速度 */
static float Vy = 0.0f; /* 底盘世界坐标系y方向合速度 */

/* 偏转角：世界坐标系角度取反后转为弧度 */
float steering_select_yaw_angle = DEG2RAD(
    -(*steering_ctrl_handle.world_angle));

/* 注意航向轮是反着装的, 根据车辆装载可能需要修改此处 */
Vx = speedx;
Vy = -speedy;

/* 将自转速度分解到每个舵轮的X,Y方向上，
 * 后面与平动矢量叠加，逆时针方向为自旋的正方向。 */
speed_wx[0] = speedw * COS_F32(PI / 4);
speed_wx[1] = speedw * COS_F32(PI / 4);
speed_wx[2] = speedw * -COS_F32(PI / 4);
speed_wx[3] = speedw * -COS_F32(PI / 4);

speed_wy[0] = speedw * -SIN_F32(PI / 4);
speed_wy[1] = speedw * SIN_F32(PI / 4);
speed_wy[2] = speedw * -SIN_F32(PI / 4);
speed_wy[3] = speedw * SIN_F32(PI / 4);

speed_wx[0] = speedw * COS_F32(0);
speed_wx[1] = speedw * -COS_F32(PI / 3);
speed_wx[2] = speedw * -COS_F32(-PI / 3);

speed_wy[0] = speedw * -SIN_F32(0);
speed_wy[1] = speedw * -SIN_F32(PI / 3);
speed_wy[2] = speedw * -SIN_F32(-PI / 3);

/* 分支1: 驻停锁死 */
if (steering_ctrl_handle.halt) {
    /* 轮子转到X形锁定位置，速度清零（详见4.2.5节） */
}
/* 分支2: 有速度输入时执行完整解算 */
else if ((math_compare_float(speedx, 0.0f) != MATH_FP_EQUATION) ||
         (math_compare_float(speedy, 0.0f) != MATH_FP_EQUATION) ||
         (math_compare_float(speedw, 0.0f) != MATH_FP_EQUATION)) {

    for (uint32_t i = 0; i < WHEEL_NUM; i++) {
        /* 速度分解并合成：平动坐标变换 + 自转分解 */
        mix_x[i] = Vx * COS_F32(steering_select_yaw_angle) +
                   Vy * SIN_F32(steering_select_yaw_angle) + speed_wx[i];
        mix_y[i] = Vy * COS_F32(steering_select_yaw_angle) -
                   Vx * SIN_F32(steering_select_yaw_angle) + speed_wy[i];

        /* 航向电机转速解算 */
        wheel_speed[i] = sqrt(pow(mix_x[i], 2) + pow(mix_y[i], 2));

        /* 舵向电机角度解算 (极坐标映射) */
        wheel_angle[i] = mix_x[i] < 0
                             ? ACOS_F32(mix_y[i] / wheel_speed[i])
                             : -ACOS_F32(mix_y[i] / wheel_speed[i]);
    }
}
/* 分支3: 速度为零但未驻停，仅停止驱动，保持当前角度 */
else {
    for (uint32_t i = 0; i < WHEEL_NUM; i++) {
        wheel_speed[i] = 0.0f;
    }
}

/* 对每个轮子执行最小转角优化（详见4.2.4节） */
for (uint32_t i = 0; i < WHEEL_NUM; i++) {
    steering_wheel_single_ctrl(i, wheel_speed[i], wheel_angle[i]);
}

/* 统一发送控制指令 */
steering_ctrl_handle.steering_direction_ctrl(
    steering_ctrl_handle.set_wheel_angle);
steering_ctrl_handle.steering_speed_ctrl(
    steering_ctrl_handle.set_wheel_speed);

/* 矢量模长即为轮速 */
wheel_speed[i] = sqrt(pow(mix_x[i], 2) + pow(mix_y[i], 2));

/*
 * 这里的所映射到的角度空间是一个和全场定位陀螺仪一样的角度空间,
 * 逆时针从 0 开始一直增大到 PI, 经过一个跳变点到 -PI, 随后逐渐增大到 0
 * 这样处理后一圈内所有角度都会有一个值与之一一对应
 * 各轮子的角度空间也是这样的
 */
wheel_angle[i] = mix_x[i] < 0
                     ? ACOS_F32(mix_y[i] / wheel_speed[i])
                     : -ACOS_F32(mix_y[i] / wheel_speed[i]);

/**
 * @brief 单个舵轮最小转角解算, 求解航向轮旋转角度, 以及航向轮的实际速度
 *
 * @param num   轮子编号
 * @param V     航向轮的转速
 * @param theta 舵向轮在世界坐标系下需要偏转的角度
 */
void steering_wheel_single_ctrl(uint8_t num, float V, float theta) {
    /* 轮子的角度, 作为中间量 */
    static float wheel_dir[WHEEL_NUM] = {0};

    /**
     * @var re_theta        与theta相反方向对应的角度
     * @var diff_theta      舵向轮转到theta还需要转动的角度
     * @var diff_re_theta   舵向轮转到re_theta需要转动的角度
     */
    float re_theta = 0, diff_theta = 0, diff_re_theta = 0;

    /* 获取当前舵向电机的实际角度 */
    wheel_dir[num] = *(steering_ctrl_handle.steering_wheel_angle[num]);

    /* 获得相反方向对应的角度（相差180°） */
    re_theta = theta > 0 ? theta - PI : theta + PI;

    /* 计算两种选择的转角差 */
    diff_theta = theta - wheel_dir[num];
    diff_re_theta = re_theta - wheel_dir[num];

    /* 超过定义域重新映射到 [-π, π] */
    diff_theta = remap(diff_theta);
    diff_re_theta = remap(diff_re_theta);

    /* 选择较小角度 */
    if (fabsf(diff_theta) > fabsf(diff_re_theta)) {
        /*
         * 选择相反方向：在当前位置基础上转所需角度,
         * 实际转动角度是 diff_re_theta 而不是 theta,
         * theta 只是一个绝对角度, 用于控制电机.
         */
        theta = wheel_dir[num] + diff_re_theta;
        V = -V;  /* 速度反向 */
    } else {
        /* 选择直接方向：在当前位置基础上转所需角度
         * (相当于重新投射到局部坐标系) */
        theta = wheel_dir[num] + diff_theta;
    }

    /* 写入控制句柄 */
    steering_ctrl_handle.set_wheel_speed[num] = V;
    steering_ctrl_handle.set_wheel_angle[num] = remap(theta);
}

if (steering_ctrl_handle.halt) {
    wheel_angle[0] = -PI / 4;   /* 1号轮（前左）-45° */
    wheel_angle[1] = PI / 4;    /* 2号轮（前右）+45° */
    wheel_angle[2] = PI / 4;    /* 3号轮（后左）+45° */
    wheel_angle[3] = -PI / 4;   /* 4号轮（后右）-45° */

    wheel_speed[0] = 0.0f;
    wheel_speed[1] = 0.0f;
    wheel_speed[2] = 0.0f;
    wheel_speed[3] = 0.0f;
}

if (steering_ctrl_handle.halt) {
    wheel_angle[0] = 0;         /* 1号轮朝前 */
    wheel_angle[1] = PI / 3;    /* 2号轮 +60° */
    wheel_angle[2] = -PI / 3;   /* 3号轮 -60° */

    wheel_speed[0] = 0.0f;
    wheel_speed[1] = 0.0f;
    wheel_speed[2] = 0.0f;
}

else {
    /* 仅停止, 保持角度 */
    wheel_speed[0] = 0.0f;
    wheel_speed[1] = 0.0f;
    wheel_speed[2] = 0.0f;
    wheel_speed[3] = 0.0f;  /* 四轮时 */
}

// 第i个轮子的速度分量
V_ix = V_i * cos(θ_i)   // X方向分量（前进方向）
V_iy = V_i * sin(θ_i)   // Y方向分量（左侧方向）

// 平动速度（平均各轮速度）
Vx = average(V_ix[i])   // i = 0, 1, 2, 3
Vy = average(V_iy[i])

// 自转角速度（根据各轮切向速度计算）
// 对于第i个轮子，切向速度 = V_i · sin(θ_i - α_i)
// 其中α_i为轮子安装角度
ω = sum(V_i * sin(θ_i - α_i)) / (4 * radius)

[图1占位符：舵轮矢量协调示意图]
![](Pictures/底盘解算/swerve_vector_coordination.png)

[图2占位符：同步转向时序图]
![](Pictures/底盘解算/swerve_sync_timing.png)

[图3占位符：零半径旋转模式示意图]
![](Pictures/底盘解算/swerve_point_turn.png)

// 找出最大速度
max_speed = max(|wheel_speed[i]|)

// 如果超过限制，进行归一化
if (max_speed > MAX_SPEED_LIMIT) {
    scale = MAX_SPEED_LIMIT / max_speed
    for (i = 0; i < WHEEL_NUM; i++) {
        wheel_speed[i] *= scale
    }
}

// 各轮都指向切线方向（垂直于半径）
wheel_angle[0] = -π/4   // 1号轮指向右前方（逆时针切线方向）
wheel_angle[1] = π/4    // 2号轮指向左前方（逆时针切线方向）
wheel_angle[2] = 3π/4   // 3号轮指向左后方（逆时针切线方向）
wheel_angle[3] = -3π/4  // 4号轮指向右后方（逆时针切线方向）

// 各轮速度大小与到中心距离成正比
wheel_speed[i] = speedw * radius

// 绕点 (x, y) 以速度 v 旋转
// 计算各轮到旋转点的距离和角度
for (i = 0; i < WHEEL_NUM; i++) {
    dx = wheel_x[i] - x
    dy = wheel_y[i] - y
    distance = sqrt(dx² + dy²)

    // 轮子指向垂直于半径的切线方向
    wheel_angle[i] = atan2(dy, dx) + π/2

    // 速度与到旋转点距离成正比
    wheel_speed[i] = v * distance / R_center
}

[图1占位符：底盘控制软件架构图]
![](Pictures/底盘解算/swerve_software_architecture.png)

[图2占位符：舵轮控制数据流图]
![](Pictures/底盘解算/swerve_data_flow.png)

/* 核心解算函数 */
void steering_wheel_ctrl(float speedx, float speedy, float speedw);
```c
- 坐标变换（世界坐标系 ↔ 机器人坐标系）
- 自转速度分解到各轮
- 速度矢量合成（平动+自转）
- 轮速和转角计算
- 最小转角优化
- 输出各轮目标角度和速度

**下层：电机控制层**
```c
/* 控制函数指针类型定义 */
typedef void (*steering_direction_ctrl_t)(float * /* angle */);
typedef void (*steering_speed_ctrl_t)(float * /* speed */);
```c
- 舵向电机角度控制（接收 `set_wheel_angle` 数组）
- 航向电机速度控制（接收 `set_wheel_speed` 数组）
- 编码器反馈读取（舵向电机角度，范围 [0, 2π] rad）

#### 5.1.2 实时性考虑

**解算周期**
- 推荐周期：1-10ms
- 舵轮解算计算量适中，可在主循环中执行

**通信延迟优化**
- 主控只发送目标角度和速度
- 各轮PID控制由轮载芯片独立完成
- 减少主控与电机驱动器通信负担

**控制时序**
```c
时间线：
|--- 读取编码器 ---|--- 运动学解算 ---|--- 发送控制指令 ---|--- PID控制(轮载) ---|
    (1ms)              (2ms)               (1ms)              (轮载芯片执行)

Pictures/
└── 底盘解算/
    ├── coordinate_system.png           # 图1-1
    ├── velocity_decomposition.png      # 图1-2
    ├── wheel_position.png              # 图1-3
    ├── mecanum_roller_arrangement.png  # 图2-1
    ├── mecanum_single_wheel.png        # 图2-2
    ├── mecanum_4wheel_layout.png       # 图2-3
    ├── mecanum_vector_synthesis.png    # 图2-4
    ├── mecanum_motion_modes.png        # 图2-5
    ├── omni_wheel_structure.png        # 图3-1
    ├── omni_3wheel_layout.png          # 图3-2
    ├── omni_vector_decomposition.png   # 图3-3
    ├── omni_synthesis.png              # 图3-4
    ├── omni_4wheel_layout.png          # 图3-5
    ├── omni_3vs4_comparison.png        # 图3-6
    ├── swerve_module_structure.png     # 图4-1
    ├── swerve_velocity_vector.png      # 图4-2
    ├── swerve_4wheel_layout.png        # 图4-3
    ├── swerve_3wheel_layout.png        # 图4-4
    ├── swerve_algorithm_flow.png       # 图4-5
    ├── swerve_angle_optimization.png   # 图4-6
    ├── swerve_geometry_params.png      # 图4-7
    ├── swerve_vector_superposition.png # 图4-8
    ├── swerve_forward_kinematics_fusion.png  # 图4-9
    ├── swerve_forward_kinematics_block.png   # 图4-10
    ├── swerve_vector_coordination.png  # 图4-11
    ├── swerve_sync_timing.png          # 图4-12
    ├── swerve_point_turn.png           # 图4-13
    ├── swerve_software_architecture.png      # 图5-1
    ├── swerve_data_flow.png            # 图5-2
    └── swerve_pid_control.png          # 图5-3