第三节：坐标变换与运动模型
说实话，做机器人避障这么多年，我踩过最大的坑就是坐标系统一的问题。你想想看，传感器说「前方0.5米有障碍」，但它是相对于谁说的？是相对于激光雷达，还是相对于车体中心？如果这两个坐标系没对齐，那避障算法再牛也白搭。
这一节，我们就来聊聊机器人坐标系、全局坐标系、坐标变换矩阵，还有两种最常见的运动模型——差速轮和阿克曼。嗯，都是硬核干货，但我会尽量讲得接地气一点。
3.1 机器人坐标系与全局坐标系
先明确两个概念：
- 机器人坐标系（Body Frame）：固定在机器人身上的坐标系。通常以机器人中心为原点，前进方向为X轴，左侧为Y轴，垂直向上为Z轴。说白了，就是「机器人自己的视角」。
- 全局坐标系（World Frame）：固定在地面上的坐标系。通常以起点或某个参考点为原点，北向为X轴，东向为Y轴。这是「上帝视角」。
我刚开始做项目时，经常把这两个搞混。有一次在室内跑AGV，激光雷达检测到障碍物，我直接用雷达坐标系下的坐标去规划路径，结果机器人直接撞墙了。后来才发现——雷达装在车头左侧，它的坐标系和车体坐标系差了30厘米的平移！
3.2 坐标变换矩阵
坐标变换，说白了就是「怎么把一个点从一个坐标系搬到另一个坐标系」。数学上，我们用齐次变换矩阵来完成这件事。
一个2D齐次变换矩阵长这样：
| cosθ -sinθ tx |
| sinθ cosθ ty |
| 0 0 1 |其中θ是旋转角，tx和ty是平移量。为什么用齐次坐标？因为这样可以把旋转和平移写成一个矩阵，方便连续变换。
举个例子：假设机器人在全局坐标系中的位置是(1, 2)，朝向角30°。那么机器人坐标系下的一个点(0.5, 0.3)，在全局坐标系中是多少？
import numpy as np
theta = np.radians(30)
tx, ty = 1.0, 2.0
T = np.array([
[np.cos(theta), -np.sin(theta), tx],
[np.sin(theta), np.cos(theta), ty],
[0, 0, 1]
])
p_robot = np.array([0.5, 0.3, 1.0])
p_world = T @ p_robot
print(p_world) # 输出全局坐标我个人习惯把变换矩阵写成函数，每次调用时传入角度和位移，这样不容易出错。我在项目中遇到过有人手算变换矩阵，结果符号搞反了，机器人原地转圈——嗯，调试了一下午才发现。
3.3 差速轮运动模型
差速轮，就是两个独立驱动的轮子，左右各一个。通过控制左右轮的速度差来实现转向。说白了，左轮快右轮慢，车就往右转；反之亦然。
运动学模型很简单：
v = (v_left + v_right) / 2
ω = (v_right - v_left) / L其中v是线速度，ω是角速度，L是轮距（左右轮之间的距离）。
为什么叫「差速」？因为转向全靠速度差。我记得有一次做室内清扫机器人，轮子磨损不一致，导致左右轮实际速度有偏差，机器人总是走偏。后来我在代码里加了个在线校准模块，每走10米自动修正一次轮径参数。
3.4 阿克曼运动模型
阿克曼模型，就是汽车那种转向方式。前轮转向，后轮驱动。它的特点是：转弯时，内外侧前轮的转向角不同，这样才能保证所有轮子绕同一个圆心转动。
运动学模型稍微复杂一点：
δ = arctan(L / R)
v = v_rear其中δ是前轮转向角，L是轴距，R是转弯半径，v_rear是后轮速度。
实际项目中，阿克曼模型比差速轮难调。为什么？因为转向角不能突变——你不可能像差速轮那样瞬间原地掉头。我曾经在停车场项目中，把阿克曼机器人的最大转向角设得太激进，结果高速转弯时直接侧翻了。嗯，从那以后我学乖了，转向角变化率一定要加限幅。
| 特性 | 差速轮 | 阿克曼 |
|---|---|---|
| 转向方式 | 速度差 | 前轮转向 |
| 最小转弯半径 | 0（可原地旋转） | 有限值（取决于轴距） |
| 横向移动 | 不支持 | 不支持 |
| 控制复杂度 | 低 | 中 |
| 典型应用 | 室内AGV、扫地机 | 无人车、室外机器人 |
3.5 知识体系总览
下面这张图，是我自己梳理的本章知识结构。你可以把它当作一个「地图」，学完这一节后对照着检查自己是否都掌握了。
你看，整个知识体系其实就三大块：坐标系、变换矩阵、运动模型。坐标系是基础，变换矩阵是工具，运动模型是应用。三者环环相扣，缺一不可。
好了，这一节的内容就到这里。坐标变换和运动模型是避障算法的基石，后面讲路径规划时，你会发现处处都在用这些知识。嗯，先把基础打牢，后面才能跑得快。
可可精品资料库