机器人 / 系统课时

二维位姿如何表示平移和旋转

二维位姿如何表示平移和旋转

二维激光 SLAM 必须把机器人位姿和激光观测统一到地图坐标系。若只叠加平移、忽略航向角,转弯后的扫描点就会错位。本课用 (x,y,θ)(x,y,\theta) 描述平面位姿,再以 SE(2) 矩阵计算局部点、相对位姿和变换复合。

学习目标

  • 按机器人平面坐标约定解释位置与航向角的正负方向。
  • 从 (x,y,θ)(x,y,\theta) 构造 SE(2) 矩阵,将局部点换算到地图系。
  • 用变换求逆与矩阵复合计算两个位姿间的相对关系,并用代码验证。

位姿的三个自由度

在 ROS 常用的右手坐标约定下,机器人自身的 x 轴朝前、y 轴朝左、z 轴朝上;平面航向角 θ\theta 从参考坐标系正 x 轴逆时针增加。位置 (x,y)(x,y) 以米计,三角函数中的角度使用弧度,90∘=π/290^\circ=\pi/2。

例如,地图系 M 中的位姿 A 是 (2.0 m,1.0 m,90∘)(2.0\ \mathrm{m},1.0\ \mathrm{m},90^\circ),位姿 B 是 (1.5 m,2.0 m,0∘)(1.5\ \mathrm{m},2.0\ \mathrm{m},0^\circ)。A 的车头朝地图 +y,B 的车头朝地图 +x。相同的空间位移在各自局部系中会有不同的分量。

SE(2) 合并旋转和平移

记 MTA{}^{M}T_A 为将 A 系点坐标转换到地图系 M 的变换矩阵。SE(2) 用一个 3×33\times3 齐次矩阵同时存放平面旋转和位置:

MTA=[cos⁡θ−sin⁡θxsin⁡θcos⁡θy001]{}^{M}T_A=\begin{bmatrix}\cos\theta&-\sin\theta&x\\\sin\theta&\cos\theta&y\\0&0&1\end{bmatrix}

对于 A 系中的点 pA=[u,v,1]Tp^A=[u,v,1]^T,有 pM=MTApAp^M={}^MT_Ap^A。上例中,机器人 A 正前方 1 m 的点是 pA=[1,0,1]Tp^A=[1,0,1]^T,在地图系中变为 pM=[2,2,1]Tp^M=[2,2,1]^T:先把局部偏移旋转 90°,再加上 (2,1)(2,1) 米平移。

地图坐标系中两个位姿及相对变换

计算相对位姿与变换复合

要得出 B 在 A 局部系中的位姿,不能只用两组地图坐标相减,因为差值仍处于地图系。应先求 A 的逆变换,再复合 B 的地图变换:

ATB=(MTA)−1MTB,MTB=MTAATB{}^{A}T_B=({}^{M}T_A)^{-1}{}^{M}T_B,\qquad{}^{M}T_B={}^{M}T_A{}^{A}T_B

本例得出 B 相对 A 的位姿为 (1.0 m,0.5 m,−90∘)(1.0\ \mathrm{m},0.5\ \mathrm{m},-90^\circ)。逆变换的旋转部分是 RTR^T,平移部分为 −RTt-R^Tt;不能仅把原来的 (x,y)(x,y) 改成 (−x,−y)(-x,-y)。矩阵复合顺序不可交换。

以下脚本只依赖 Python 3 标准库。输入角度使用度,构造旋转矩阵时统一转为弧度:

python
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("数值校验通过")

预期输出:

text
B 相对 A: (1.00, 0.50, -90.0°)
复合后 B 在 map: (1.50, 2.00, 0.0°)
数值校验通过

断言检查复合结果是否回到 B 的原始地图位姿,容差为 10−910^{-9}。可以把 A 的朝向改为 0° 重新运行;此时相对平移应变成 (−0.5,1.0)(-0.5,1.0) 米,说明相对平移依赖参考系朝向。

对应到 ROS 2 的坐标变换

数学记号 (x,y,θ)(x,y,\theta) 便于计算,但 ROS 2 的 geometry_msgs/msg/TransformStamped 使用 transform.translation 表示平移、transform.rotation 的四元数表示旋转。纯平面偏航对应四元数 (qx,qy,qz,qw)=(0,0,sin⁡(θ/2),cos⁡(θ/2))(q_x,q_y,q_z,q_w)=(0,0,\sin(\theta/2),\cos(\theta/2));上例 A 的 90° 偏航约为 (0,0,0.7071,0.7071)(0,0,0.7071,0.7071)。

在 tf2 中,header.frame_id 指定参考坐标系,child_frame_id 指定子坐标系。实际 SLAM 系统常通过 map、odom、base_link 的相邻变换,复合得到 base_link 在 map 下的位姿;若还需投影雷达点,应再包含雷达安装坐标系的变换。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

二维位姿如何表示平移和旋转

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通