机器人 / 系统课时
二维位姿如何表示平移和旋转
二维位姿如何表示平移和旋转
二维激光 SLAM 必须把机器人位姿和激光观测统一到地图坐标系。若只叠加平移、忽略航向角,转弯后的扫描点就会错位。本课用 描述平面位姿,再以 SE(2) 矩阵计算局部点、相对位姿和变换复合。
学习目标
- 按机器人平面坐标约定解释位置与航向角的正负方向。
- 从 构造 SE(2) 矩阵,将局部点换算到地图系。
- 用变换求逆与矩阵复合计算两个位姿间的相对关系,并用代码验证。
位姿的三个自由度
在 ROS 常用的右手坐标约定下,机器人自身的 x 轴朝前、y 轴朝左、z 轴朝上;平面航向角 从参考坐标系正 x 轴逆时针增加。位置 以米计,三角函数中的角度使用弧度,。
例如,地图系 M 中的位姿 A 是 ,位姿 B 是 。A 的车头朝地图 +y,B 的车头朝地图 +x。相同的空间位移在各自局部系中会有不同的分量。
SE(2) 合并旋转和平移
记 为将 A 系点坐标转换到地图系 M 的变换矩阵。SE(2) 用一个 齐次矩阵同时存放平面旋转和位置:
对于 A 系中的点 ,有 。上例中,机器人 A 正前方 1 m 的点是 ,在地图系中变为 :先把局部偏移旋转 90°,再加上 米平移。

计算相对位姿与变换复合
要得出 B 在 A 局部系中的位姿,不能只用两组地图坐标相减,因为差值仍处于地图系。应先求 A 的逆变换,再复合 B 的地图变换:
本例得出 B 相对 A 的位姿为 。逆变换的旋转部分是 ,平移部分为 ;不能仅把原来的 改成 。矩阵复合顺序不可交换。
以下脚本只依赖 Python 3 标准库。输入角度使用度,构造旋转矩阵时统一转为弧度:
import math
def pose(x: float, y: float, yaw_deg: float):
yaw = math.radians(yaw_deg)
c, s = math.cos(yaw), math.sin(yaw)
return ((c, -s, x), (s, c, y), (0.0, 0.0, 1.0))
def multiply(a, b):
return tuple(
tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
for i in range(3)
)
def invert(t):
c, s = t[0][0], t[1][0]
x, y = t[0][2], t[1][2]
# 逆平移不是简单取负,还要经过转置旋转
return ((c, s, -c * x - s * y),
(-s, c, s * x - c * y),
(0.0, 0.0, 1.0))
def values(t):
yaw = math.degrees(math.atan2(t[1][0], t[0][0]))
return t[0][2], t[1][2], yaw
# 两个机器人位姿均以地图坐标系 M 为参考
t_map_a = pose(2.0, 1.0, 90.0)
t_map_b = pose(1.5, 2.0, 0.0)
t_a_b = multiply(invert(t_map_a), t_map_b)
t_map_b_again = multiply(t_map_a, t_a_b)
ax, ay, ayaw = values(t_a_b)
bx, by, byaw = values(t_map_b_again)
print(f"B 相对 A: ({ax:.2f}, {ay:.2f}, {ayaw:.1f}°)")
print(f"复合后 B 在 map: ({bx:.2f}, {by:.2f}, {byaw:.1f}°)")
assert all(abs(a - b) < 1e-9 for a, b in zip((bx, by, byaw), (1.5, 2.0, 0.0)))
print("数值校验通过")预期输出:
B 相对 A: (1.00, 0.50, -90.0°)
复合后 B 在 map: (1.50, 2.00, 0.0°)
数值校验通过断言检查复合结果是否回到 B 的原始地图位姿,容差为 。可以把 A 的朝向改为 0° 重新运行;此时相对平移应变成 米,说明相对平移依赖参考系朝向。
对应到 ROS 2 的坐标变换
数学记号 便于计算,但 ROS 2 的 geometry_msgs/msg/TransformStamped 使用 transform.translation 表示平移、transform.rotation 的四元数表示旋转。纯平面偏航对应四元数 ;上例 A 的 90° 偏航约为 。
在 tf2 中,header.frame_id 指定参考坐标系,child_frame_id 指定子坐标系。实际 SLAM 系统常通过 map、odom、base_link 的相邻变换,复合得到 base_link 在 map 下的位姿;若还需投影雷达点,应再包含雷达安装坐标系的变换。