4、四轮全向底盘运动学：逆运动学与正运动学
各位同学，今天我们来啃一块硬骨头——四轮全向底盘的运动学。说实话，我刚入行那会儿，看到一堆公式就头大。但后来我发现，搞懂运动学就像学骑自行车，一旦掌握了平衡感，后面就顺了。
四轮全向底盘，说白了就是四个轮子各自独立驱动，能让车体朝任意方向平移，还能原地转圈。这背后的数学工具，就是运动学。它分两块：逆运动学和正运动学。一个告诉你怎么控制轮子达到目标姿态，另一个告诉你轮子转起来后车体会怎么动。
核心概念速览：
- 逆运动学：已知车体速度（vx, vy, ω），求四个轮子的转速（ω1, ω2, ω3, ω4）
- 正运动学：已知四个轮子的转速，求车体实际的速度
4.1 四轮全向底盘的布局与坐标系
先说说常见的轮子布局。我见过两种主流方案：一种是轮子呈矩形布置，另一种是正方形。我个人习惯用正方形布局，因为对称性好，控制起来更直观。
坐标系怎么定？通常这样：
- 车体中心为原点O
- X轴指向车头方向
- Y轴指向左侧
- Z轴垂直向上（旋转用）
每个轮子都有自己的安装角度。对于全向轮，轮子的滚动方向与安装方向有一个固定夹角。嗯，这里要注意：不同厂家的轮子，这个角度可能不一样。我在项目中遇到过，买了一批轮子回来，发现安装角度差了5度，结果跑出来的轨迹全是歪的。
4.2 逆运动学公式推导
逆运动学，说白了就是解一个线性方程组。我们假设四个轮子编号为1、2、3、4，分别对应左前、右前、左后、右后。
每个轮子的转速由三部分贡献叠加而成：
- 车体沿X轴平移产生的分量
- 车体沿Y轴平移产生的分量
- 车体绕Z轴旋转产生的分量
公式长这样（以轮子1为例）：
ω1 = (1/R) * [ -vx * sin(θ) + vy * cos(θ) + L * ω ]
其中：
- R：轮子半径
- θ：轮子安装角度（相对于车体坐标系）
- L：轮子到车体中心的距离
- vx, vy：车体平移速度
- ω：车体旋转角速度
四个轮子合起来，写成矩阵形式：
[ω1] [ -sinθ1 cosθ1 L ] [vx]
[ω2] = [ -sinθ2 cosθ2 L ] [vy]
[ω3] [ -sinθ3 cosθ3 L ] [ω ]
[ω4] [ -sinθ4 cosθ4 L ]
我的小技巧： 实际写代码时，我习惯先把这四个矩阵系数算好存起来，运行时直接做矩阵乘法。这样每帧计算量很小，适合嵌入式平台。
4.3 正运动学公式推导
正运动学是逆运动学的逆过程。你想想看，如果四个轮子都在转，车体到底会怎么动？
公式其实是对称的：
[vx] [ω1]
[vy] = M⁺ · [ω2]
[ω ] [ω3]
[ω4]
这里的M⁺是逆运动学矩阵的伪逆。为什么用伪逆？因为四个方程三个未知数，系统是超定的。我记得第一次做这个推导时，直接用了普通逆矩阵，结果算出来全是NaN，折腾了一下午才发现问题。
避坑指南： 我曾经在正运动学计算中忽略了轮子打滑的影响。实际测试时，车体明明应该直行，结果却偏了30度。后来加了轮速传感器做闭环，才把这个问题解决。所以，正运动学算出来的只是理论值，实际要用编码器数据做修正。
4.4 运动学核心逻辑图
下面这张图展示了逆运动学和正运动学在整个控制链路中的位置：
4.5 代码实现要点
在ROS里实现运动学，我通常写成一个独立的类。下面是一个简化版的核心代码片段：
class FourWheelOmniKinematics:
def __init__(self, wheel_radius, wheel_base):
self.R = wheel_radius
self.L = wheel_base / 2.0 # 半轴距
# 预计算逆运动学矩阵
self.inv_matrix = self._build_inv_matrix()
# 预计算正运动学矩阵（伪逆）
self.fwd_matrix = np.linalg.pinv(self.inv_matrix)
def inverse(self, vx, vy, omega):
"""逆运动学：目标速度 → 轮子转速"""
input_vec = np.array([vx, vy, omega])
wheel_speeds = self.inv_matrix @ input_vec
return wheel_speeds
def forward(self, wheel_speeds):
"""正运动学：轮子转速 → 实际速度"""
return self.fwd_matrix @ wheel_speeds
经验之谈： 我建议在初始化时就把矩阵算好，不要在控制循环里重复计算。另外，记得加一个饱和函数，防止轮子转速超过电机最大转速。我曾经因为没加这个限制，直接把一个电机烧了。
4.6 实际调试中的坑
最后分享几个我踩过的坑：
- 轮子安装方向搞反： 有一次我把左前和右后的安装角写反了，结果车体横向移动时变成了斜着走。检查了三天才发现是符号问题。
- 坐标系定义不一致： ROS里常用ENU坐标系，但有些电机驱动库用的是NED。不统一的话，y轴方向是反的。
- 轮子半径测量误差： 全向轮的小滚子会磨损，半径会变小。我建议每运行50小时后重新标定一次轮子半径。
好了，运动学这部分就讲到这里。记住，公式是死的，但应用是活的。多动手写代码，多跑几遍实际测试，你很快就能找到感觉。