第1章：坐标变换基础
各位同学，欢迎来到《扫地机路径规划》的第一课。
说实话，坐标变换这东西，看着像数学，其实是扫地机的「命根子」。你想想看，机器人连自己在哪、要去哪都搞不清楚，还谈什么规划？我当年刚入行时，就吃过这个亏——自以为写好了路径规划算法，结果扫地机在客厅里原地转圈，撞墙撞得砰砰响。后来一查，坐标系搞反了。
嗯，咱们今天就把这个基础打牢。
1.1 机器人坐标系 vs 世界坐标系
先问个问题：扫地机怎么知道自己的位置？
它身上有激光雷达、里程计、IMU（惯性测量单元）。这些传感器给出的数据，都是相对于机器人自身的。比如「前方0.5米有障碍物」——这个「前方」是机器人的前方，不是房间的前方。
这就引出了两个核心概念：
- 机器人坐标系（Robot Frame）：以机器人为原点，x轴朝前，y轴朝左（或朝右，看具体定义），z轴朝上。说白了，就是「我眼中的世界」。
- 世界坐标系（World Frame）：以房间某个固定点为原点，比如充电桩的位置。这是「上帝视角」，所有物体都在这个坐标系下有唯一坐标。
我个人的习惯是：世界坐标系的原点选在房间的西北角，x轴朝东，y轴朝北。这样符合地图的常规方向。当然，你也可以选别的位置，但一旦定下来就别改了。
核心要点：传感器数据在机器人坐标系下，地图和路径规划在世界坐标系下。两者之间必须能互相转换。
3.2 坐标变换矩阵
好，现在问题来了：怎么把机器人坐标系下的一个点，转换到世界坐标系下？
答案就是——坐标变换矩阵。
假设机器人当前在世界坐标系下的位置是 (x, y)，朝向角是 θ。那么机器人坐标系下的点 (x_r, y_r) 转换到世界坐标系下的公式是：
x_w = x + x_r * cos(θ) - y_r * sin(θ)
y_w = y + x_r * sin(θ) + y_r * cos(θ)写成矩阵形式就是：
[x_w] [cosθ -sinθ x] [x_r]
[y_w] = [sinθ cosθ y] [y_r]
[1 ] [ 0 0 1] [ 1 ]这个3x3的矩阵，就是二维空间中的坐标变换矩阵。它包含了旋转（左上角2x2）和平移（右上角2x1）。
我在项目中遇到过一个问题：当时用了一个第三方库，它返回的旋转矩阵是转置的。我花了整整一个下午才排查出来。所以，拿到别人的代码，第一件事就是验证矩阵的乘法顺序。
小技巧：验证变换矩阵是否正确，可以取几个特殊点测试。比如机器人坐标系下的 (1,0) 点，转换后应该落在世界坐标系下机器人的正前方。
3.3 齐次坐标
你可能会问：为什么矩阵最后一行是 [0 0 1]？为什么点坐标要写成 (x, y, 1) 而不是 (x, y)？
这就是齐次坐标的妙处。
说白了，齐次坐标就是用n+1维向量表示n维坐标。在二维空间中，我们用 (x, y, w) 表示一个点，其中 w 通常为1。当 w=0 时，表示一个方向向量（无穷远处的点）。
这样做的好处是：
- 统一了旋转和平移：用一个矩阵就能搞定，不用分开计算。
- 支持连续变换：多个变换可以合并成一个矩阵，减少计算量。
- 方便处理投影：在三维空间中，齐次坐标还能处理透视投影。
举个例子，如果机器人先旋转再平移，我们可以把两个变换矩阵相乘，得到一个复合矩阵。然后直接用这个复合矩阵去变换所有点。效率高得多。
# Python示例：齐次坐标变换
import numpy as np
def transform_point(point_r, robot_pose):
"""
point_r: 机器人坐标系下的点 (x_r, y_r)
robot_pose: 机器人位姿 (x, y, theta)
返回: 世界坐标系下的点 (x_w, y_w)
"""
x, y, theta = robot_pose
# 构建变换矩阵
T = np.array([
[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y],
[0, 0, 1]
])
# 齐次坐标
p_r = np.array([point_r[0], point_r[1], 1])
p_w = T @ p_r
return p_w[0], p_w[1]曾经踩过的坑：我刚开始写代码时，忘了把点转换成齐次坐标，直接用2x2的旋转矩阵去乘，结果平移分量全丢了。扫地机定位偏差了半米多。嗯，从那以后，我每次写变换代码都会先检查维度。
3.4 逆变换
有正向变换，自然就有逆向变换。如果知道世界坐标系下的点，想求它在机器人坐标系下的坐标，怎么办？
答案很简单：求变换矩阵的逆矩阵。
# 逆变换
T_inv = np.linalg.inv(T)
p_r = T_inv @ p_w对于二维旋转平移矩阵，逆矩阵有解析形式：
T_inv = [[cosθ, sinθ, -x*cosθ - y*sinθ],
[-sinθ, cosθ, x*sinθ - y*cosθ],
[0, 0, 1 ]]我个人建议：能用解析解就别用数值求逆。数值求逆在矩阵接近奇异时会有数值误差，而解析解是精确的。扫地机的嵌入式处理器性能有限，能省一点是一点。
3.5 实战：坐标系对齐
最后，咱们聊聊实际中怎么用。
扫地机启动时，通常不知道自己在世界坐标系下的位置。它需要先「定位」——也就是估计出机器人坐标系和世界坐标系之间的变换关系。
这个过程叫做「坐标系对齐」或「初始化定位」。常见的方法有：
- 手动指定：用户把扫地机放到充电桩上，按下「开始」，机器人就知道自己在家里的位置了。
- 自动匹配：机器人扫描周围环境，与已有地图进行匹配，找到自己的位置。
- 概率方法：用粒子滤波或卡尔曼滤波，估计位姿的概率分布。
我记得有一次做项目，扫地机在瓷砖地面上打滑，里程计数据全废了。坐标系对齐完全靠激光雷达的扫描匹配。那段时间我天天跟ICP算法（迭代最近点）打交道，也算是因祸得福，把点云配准学了个透。
总结一下：
- 机器人坐标系是「我眼中的世界」，世界坐标系是「上帝视角」。
- 坐标变换矩阵 = 旋转 + 平移，用齐次坐标统一表示。
- 逆变换用解析解，别偷懒用数值求逆。
- 坐标系对齐是定位的第一步，搞不定它，后面全是白搭。
好了，这一章就到这里。下一章咱们聊聊「传感器模型与观测方程」——说白了，就是扫地机怎么「看」这个世界。到时候我会分享一个我当年在激光雷达标定上栽的跟头，保证让你少走弯路。