3、麦克纳姆轮运动学：四轮麦克纳姆轮底盘的运动学方程推导、前向运动学与逆向运动学
好，咱们今天来啃一块硬骨头——麦克纳姆轮的运动学。说实话，我第一次接触这东西的时候，看着四个轮子斜着转，脑子是懵的。但后来在AGV项目里调了两个月，才真正搞明白它背后的数学逻辑。你只要把坐标系和速度分解想清楚，其实没那么玄乎。
3.1 麦克纳姆轮的运动特点
麦克纳姆轮最神奇的地方，就是它能「斜着走」。普通轮子只能前后滚，麦克纳姆轮在轮毂上装了一圈小辊子，这些辊子与轮毂轴线成45度角。轮子转起来时，地面给辊子的摩擦力会分解成两个方向的分力。
我个人习惯把每个轮子想象成一个「矢量发生器」。轮子转动产生一个沿辊子方向的力，这个力再分解到车体的X轴和Y轴上。四个轮子的力合成，就能让底盘朝任意方向移动。
3.2 坐标系与符号约定
在推导公式前，咱们先把规矩定好。我在项目里吃过亏，坐标系没统一，结果算出来的速度和实际运动方向完全相反。
- 底盘坐标系： 以底盘中心为原点，X轴朝前，Y轴朝左，Z轴朝上（右手系）。
- 轮子编号： 左前轮为1号，右前轮为2号，左后轮为3号，右后轮为4号。逆时针编号。
- 轮子速度： ω₁、ω₂、ω₃、ω₄，单位rad/s。正转表示轮子向前滚动。
- 底盘速度： vₓ（纵向速度，前进为正）、vᵧ（横向速度，左移为正）、ω（旋转角速度，逆时针为正）。
嗯，这里要注意：不同教材的坐标系定义可能不一样。你拿到别人的代码，第一件事就是检查坐标系方向。我曾经因为坐标系没对齐，让一台底盘在实验室里原地转圈，差点撞到设备。
3.3 单个轮子的速度分解
咱们先看一个轮子。以左前轮（1号轮）为例，它的辊子方向是「从左上到右下」，与X轴夹角45度。
轮子转动时，地面接触点的速度方向沿着辊子轴线。把这个速度分解到X和Y方向：
- 轮子转动产生的速度大小：v_w = R · ω₁（R是轮子半径）
- 沿辊子方向的速度：v_r = v_w
- 分解到X轴：v_x1 = v_r · cos45° = (√2/2) · R · ω₁
- 分解到Y轴：v_y1 = v_r · sin45° = (√2/2) · R · ω₁
但注意，这个速度是轮子相对于地面的速度。底盘的运动是四个轮子速度的合成。每个轮子除了提供移动速度，还要考虑底盘旋转带来的附加速度。
3.4 四轮运动学方程推导
好，现在咱们把四个轮子一起考虑。底盘的运动状态用三个量描述：vₓ、vᵧ、ω。每个轮子的速度由三部分组成：
- 底盘纵向运动vₓ在轮子上的分量
- 底盘横向运动vᵧ在轮子上的分量
- 底盘旋转ω在轮子上的切向速度分量
以1号轮（左前轮）为例，它的位置在底盘左前方，坐标为(L, W)，其中L是轮子到中心的纵向距离，W是横向距离。
底盘旋转时，1号轮获得的切向速度为：
- X方向分量：-ω · W
- Y方向分量：ω · L
所以1号轮的总速度（沿辊子方向）为：
v₁ = vₓ · cos45° + vᵧ · sin45° + (-ω · W) · cos45° + (ω · L) · sin45°
简化后：v₁ = (√2/2) · (vₓ + vᵧ - ω · W + ω · L)
同理可得其他三个轮子的方程。写成矩阵形式就是：
| ω₁ | | 1 1 -(L+W) | | vₓ |
| ω₂ | = | 1 -1 (L+W) | · | vᵧ |
| ω₃ | | 1 -1 -(L+W) | | ω |
| ω₄ | | 1 1 (L+W) |注意，这里省略了系数(√2/2)/R，实际使用时需要乘上这个系数。不同轮子布局的符号可能不同，我上面给的是最常见的「X型」布局。
3.5 前向运动学与逆向运动学
这两个概念其实很简单：
- 前向运动学： 已知四个轮子的转速，求底盘的运动状态（vₓ、vᵧ、ω）。说白了就是「轮子转成这样，底盘会怎么动」。
- 逆向运动学： 已知底盘的目标运动状态，求四个轮子分别该转多快。这是实际控制中最常用的——你想让底盘往左走，就得算出每个轮子的转速。
前向运动学的公式就是上面矩阵的逆运算。但注意，4×3矩阵没有标准逆矩阵，需要用伪逆（Moore-Penrose逆）。实际工程中，我们通常用最小二乘法求解。
逆向运动学更常用，公式直接从上边的矩阵得到：
ω₁ = (vₓ + vᵧ - ω · (L+W)) · K
ω₂ = (vₓ - vᵧ + ω · (L+W)) · K
ω₃ = (vₓ - vᵧ - ω · (L+W)) · K
ω₄ = (vₓ + vᵧ + ω · (L+W)) · K其中K = (√2/2) / R，是一个常数。
3.6 运动学验证方法
推导完了，怎么验证对不对？我分享几个我在项目里用的方法：
- 纯平移测试： 让vₓ=1，vᵧ=0，ω=0。四个轮子应该同速正转，底盘直线前进。
- 纯横向测试： 让vₓ=0，vᵧ=1，ω=0。1号和4号轮正转，2号和3号轮反转，底盘横向平移。
- 纯旋转测试： 让vₓ=0，vᵧ=0，ω=1。1号和3号轮正转，2号和4号轮反转，底盘原地旋转。
- 斜向运动测试： 让vₓ=1，vᵧ=1，ω=0。四个轮子转速不同，底盘沿45度方向移动。
这些测试在仿真环境里跑一遍，基本就能确认运动学方程对不对。我一般先用MATLAB或Python做离线验证，再移植到嵌入式平台。
3.7 知识体系结构图
下面这张图总结了麦克纳姆轮运动学的核心逻辑，从轮子速度到底盘运动的完整链路：
这张图把整个运动学链路串起来了。左边是四个轮子的转速输入，中间是运动学方程这个「黑盒子」，右边是底盘的运动状态输出。你写代码的时候，脑子里要有这张图，就知道每一步在干什么。
3.8 实际应用中的注意事项
最后聊几个我在工程中踩过的坑：
- 轮子打滑： 麦克纳姆轮对地面摩擦力要求很高。在光滑地面上，运动学算得再准也没用，轮子会打滑。我一般要求地面摩擦系数不低于0.6。
- 辊子磨损： 辊子是消耗品，用久了会磨损，导致实际运动与理论计算有偏差。建议每运行100小时检查一次辊子状态。
- 编码器校准： 轮子转速的测量精度直接影响运动学计算。我用的是1024线编码器，配合4倍频，精度够用。
- 控制频率： 运动学计算最好在50Hz以上。频率太低，底盘运动会显得「卡顿」。我一般跑100Hz的控制循环。
好了，麦克纳姆轮的运动学就讲到这里。公式虽然看着多，但核心就是那个4×3矩阵。你把它理解成「四个轮子到三个自由度的映射」，后面用起来就顺手了。