3. 全向轮底盘运动学模型：三轮全向底盘的运动学方程推导
好，咱们今天来啃一块硬骨头——三轮全向底盘的运动学方程推导。
说实话，我刚入行那会儿，看到这些公式也头疼。总觉得搞机器人嘛，能跑起来不就行了？直到有一次，我做的底盘在实验室里转圈，怎么调PID都抖得跟筛糠似的。后来才发现，是运动学模型里一个符号搞反了。嗯，从那以后，我再也不敢小看这一步了。
3.1 为什么需要运动学模型？
说白了，运动学模型就是告诉你：你想让机器人怎么走，轮子该怎么转。
你想想看，你给底盘发一个指令：“以0.5m/s的速度向右平移”。底盘本身可听不懂人话。它只知道三个轮子的电机转速。运动学模型，就是连接“机器人速度”和“轮子转速”的那座桥。
核心思想： 正向运动学 = 轮子转速 → 机器人速度；逆向运动学 = 机器人速度 → 轮子转速。我们做控制，主要用逆向运动学。
3.2 三轮全向底盘的布局
我习惯用最常见的布局：三个全向轮呈120°均匀分布。每个轮子的轴线指向底盘中心。
先定义几个关键参数：
- R：底盘中心到每个轮子的距离（也就是底盘的半径）
- r：轮子本身的半径
- ω₁、ω₂、ω₃：三个轮子的角速度（rad/s）
- vₓ、vᵧ：底盘在X轴和Y轴方向的速度
- ω：底盘绕自身中心旋转的角速度
坐标系怎么定？我一般这样：
- X轴：指向1号轮子的方向
- Y轴：从X轴逆时针旋转90°
- 三个轮子编号：1号在X轴正方向，2号在120°方向，3号在240°方向
我的小习惯： 轮子编号和坐标系方向一定要在代码里写死。我见过太多人换了个项目，坐标系方向变了，结果运动学算出来底盘横着走。血的教训。
3.3 单个轮子的速度分解
每个全向轮有两个速度分量：
- 主动方向：轮子电机驱动的方向，速度 = r × ωᵢ
- 被动方向：轮子上的小辊子自由滚动的方向，这个方向我们控制不了
对于1号轮子（在X轴正方向）：
- 主动方向：沿Y轴方向（垂直于轮子轴线）
- 被动方向：沿X轴方向（平行于轮子轴线）
所以，1号轮子在地面上的速度可以写成：
v₁_active = r × ω₁ (沿Y方向)
v₁_passive = ? (沿X方向，自由滚动，我们不管)3.4 从底盘速度到轮子速度
现在，我们把底盘的运动分解到每个轮子上。
底盘中心的速度是 (vₓ, vᵧ)，再加上旋转带来的切向速度。对于1号轮子，它距离中心R，方向是X轴正方向，所以旋转带来的速度是：
- 切向速度大小：R × ω
- 方向：沿Y轴正方向（逆时针旋转时）
因此，1号轮子位置处的总速度（在底盘坐标系下）为：
V₁x = vₓ + 0 (旋转不贡献X方向速度，因为轮子在X轴上)
V₁y = vᵧ + R × ω (旋转贡献Y方向速度)注意，这个总速度要投影到轮子的主动方向上。1号轮子的主动方向是Y轴，所以：
r × ω₁ = V₁y = vᵧ + R × ω同理，对于2号轮子（在120°方向）：
- 轮子位置角度：120°
- 主动方向：垂直于轮子轴线，即120° + 90° = 210°方向
经过一番几何投影（我这里直接给结果，推导过程我当年在草稿纸上画了半小时），得到：
r × ω₂ = -sin(120°)×vₓ + cos(120°)×vᵧ + R × ω
= -0.866×vₓ - 0.5×vᵧ + R × ω对于3号轮子（在240°方向）：
r × ω₃ = -sin(240°)×vₓ + cos(240°)×vᵧ + R × ω
= 0.866×vₓ - 0.5×vᵧ + R × ω3.5 完整的运动学方程
把三个方程写在一起，就是三轮全向底盘的运动学模型：
ω₁ = (1/r) × (vᵧ + R × ω)
ω₂ = (1/r) × (-0.866×vₓ - 0.5×vᵧ + R × ω)
ω₃ = (1/r) × (0.866×vₓ - 0.5×vᵧ + R × ω)写成矩阵形式更清爽：
[ω₁] [ 0 1 R ] [vₓ]
[ω₂] = [ -0.866 -0.5 R ] [vᵧ]
[ω₃] [ 0.866 -0.5 R ] [ω ]别忘了，前面还有个系数 1/r。
这就是逆向运动学方程。 你给它 (vₓ, vᵧ, ω)，它给你三个轮子的转速。控制程序里，这就是核心计算模块。
3.6 正向运动学：从轮子转速推算底盘速度
有时候我们需要反过来算——比如用编码器数据估算底盘实际速度。那就需要正向运动学。
其实就是把上面的矩阵求逆。我直接给结果：
vₓ = (r/3) × ( -ω₂ + ω₃ ) × √3
vᵧ = (r/3) × ( 2×ω₁ - ω₂ - ω₃ )
ω = (r/(3×R)) × ( ω₁ + ω₂ + ω₃ )注意看，ω的公式很有意思——三个轮子转速之和，决定了底盘的自转速度。如果三个轮子转得一样快，底盘就原地转圈。如果想让底盘平移，三个轮子的转速必须满足一定关系。
我曾经踩过的坑： 正向运动学里，vₓ的公式有个√3因子。我第一次推导时漏掉了，结果底盘明明在往前走，里程计显示它在斜着飘。查了两天才发现是这里的问题。所以，推导完一定要做单位测试——给一组简单的输入，手算验证输出是否合理。
3.7 验证方法：给个具体例子
咱们来验证一下。假设底盘想纯向右平移（vₓ = 0.5 m/s, vᵧ = 0, ω = 0），轮子半径 r = 0.05m，底盘半径 R = 0.2m。
代入逆向运动学方程：
ω₁ = (1/0.05) × (0 + 0) = 0
ω₂ = (1/0.05) × (-0.866×0.5 - 0 + 0) = -8.66 rad/s
ω₃ = (1/0.05) × (0.866×0.5 - 0 + 0) = 8.66 rad/s结果：1号轮不动，2号轮反转，3号轮正转。你想想看，是不是这个道理？底盘向右平移，右边的轮子（1号）不需要动，左边的两个轮子一个正转一个反转，产生向右的合力。
我的调试技巧： 每次写完运动学代码，我都会做三个基本测试：纯平移X、纯平移Y、纯旋转。每个测试用手算一遍预期结果，再对比实际输出。三个测试都过了，基本就不会有问题。
3.8 本章小结
运动学方程，说白了就是一组线性变换。它不复杂，但极其重要。我见过太多项目，硬件选型、电机选型都对了，最后栽在运动学模型上——要么符号反了，要么系数错了。
记住：推导花一小时，调试省三天。这个账，咱们做工程师的都会算。
公众号:蓝海资料掘金营，微信deep3321