底盘正解算
1. 舵轮底盘
先看代码：
/**
* @brief 舵轮速度正运动学解算
*
* 将四个舵轮的实际速度和角度转换为车身坐标系下的运动速度
*
* @param wheel_speed 四个轮子的实际速度（mm/s）
* @param wheel_angle 四个轮子的转向角度（度）
* @param speedx 输出的车身x方向速度
* @param speedy 输出的车身y方向速度
* @param speedw 输出的自转角速度
*/
void steering_wheel_forward_calc(float wheel_speed[WHEEL_NUM],
float wheel_angle[WHEEL_NUM],
float *speedx, float *speedy,
float *speedw) {
float out_x = 0.0f, out_y = 0.0f, out_w = 0.0f;
#if WHEEL_NUM == 4
float wheel_x[WHEEL_NUM] = {0.0f};
float wheel_y[WHEEL_NUM] = {0.0f};
/* 第一步：将每个轮子的速度分解到固定坐标系下 */
for (uint32_t i = 0; i < WHEEL_NUM; i++) {
wheel_x[i] = -wheel_speed[i] * SIN_F32(wheel_angle[i]);
wheel_y[i] = wheel_speed[i] * COS_F32(wheel_angle[i]);
}
/* 第二步：计算车身质心在固定坐标系下的运动（平均） */
float body_x =
(wheel_x[0] + wheel_x[1] + wheel_x[2] + wheel_x[3]) * 0.25f;
float body_y =
(wheel_y[0] + wheel_y[1] + wheel_y[2] + wheel_y[3]) * 0.25f;
/* 第三步：计算车身的自转角速度 */
/* rot_from_x: 由左右轮速度差计算旋转 */
float rot_from_x =
(wheel_x[0] + wheel_x[1] - wheel_x[2] - wheel_x[3]) /
(4.0f * CHASSIS_ROT_KX);
/* rot_from_y: 由前后轮速度差计算旋转 */
float rot_from_y =
(wheel_y[1] + wheel_y[3] - wheel_y[0] - wheel_y[2]) /
(4.0f * CHASSIS_ROT_KY);
float rot_speed = (rot_from_x + rot_from_y) * 0.5f;
/* 第四步：将固定坐标系转换到车身坐标系 */
/* world_angle 是车头朝向，转换需要取反 */
float yaw = DEG2RAD(-(*steering_ctrl_handle.world_angle));
float cos_yaw = COS_F32(yaw);
float sin_yaw = SIN_F32(yaw);
float vy_internal = body_x * sin_yaw + body_y * cos_yaw;
out_x = body_x * cos_yaw - body_y * sin_yaw;
out_y = -vy_internal;
/* 第五步：计算输出的旋转角速度 */
/* radius 为 0 时表示纯平移运动，不计算角速度 */
if (steering_ctrl_handle.radius > 0.0f) {
out_w = rot_speed / steering_ctrl_handle.radius;
}
#endif /* WHEEL_NUM == 4 */
*speedx = out_x;
*speedy = out_y;
*speedw = out_w;
}
有一个很让人困惑的点，就是：“我只是把四个轮子的速度拆成了 x、y 分量，为什么居然就能反推出整个车身的平移速度和角速度？” 答案很简单：
因为底盘是刚体。
刚体上任意一点的速度，不是互相独立的，而是都必须满足同一个刚体运动规律。
平移速度是所有轮子共享的公共部分，先拿矩形底盘举例：
- 左前：\(v1x=vx−ωb\)
- 右前：\(v2x=vx+ωb\)
- 左后：\(v3x=vx−ωb\)
- 右后：\(v4x=vx+ωb\)
把四个加起来：
\(v1x+v2x+v3x+v4x=4vx\)
因为旋转项一正一负，抵消了。
所以：
\[vx=\frac{v_{1x}+v_{2x}+v_{3x}+v_{4x}}{4}\]
同理：
$\(vy=\frac{v_{1y}+v_{2y}+v_{3y}+v_{4y}}{4}vy\)$
然后进行旋转正解算
旋转和前面的平移刚好反过来：
平移看四轮的“共同部分”，旋转看四轮的“差分部分”。
因为纯平移时，四个轮子的 x/y 分量都差不多； 而纯旋转时，对称位置的轮子速度方向相反。
所以代码里先用 x 方向差分算一次旋转：
这个式子的意思是：
\[rot_x=\frac{v_{0x}+v_{1x}-v_{2x}-v_{3x}}{4K_x}\]
前面两个轮子和后面两个轮子相减，公共的平移速度会被抵消，剩下的就是旋转造成的速度。
再用 y 方向差分算一次：
也就是：
\[rot_y=\frac{v_{1y}+v_{3y}-v_{0y}-v_{2y}}{4K_y}\]
这两个值理论上应该一样，但实际会有编码器误差、打滑、舵向角误差，所以最后取平均：
这里的 rot_speed 还不是角速度，而是自转对应的切向线速度。
因为：
\[v=\omega r\]
所以：
\[\omega=\frac{v}{r}\]
对应代码：
最后再把前面算出来的 body_x/body_y 按 world_angle 做一次坐标旋转：
float yaw = DEG2RAD(-(*steering_ctrl_handle.world_angle));
out_x = body_x * cos_yaw - body_y * sin_yaw;
out_y = -(body_x * sin_yaw + body_y * cos_yaw);
这里 -world_angle 是把世界系转回车体系的逆变换；out_y 前面的负号是当前工程里的 y 轴方向适配。
所以整个正解算可以简单理解成：