机器人 / 系统课时

将传感器观测变到机体坐标

将传感器观测变到机体坐标

传感器报告的点通常位于自身坐标系,不能直接当作底盘前方的障碍物坐标。本课以一个侧向安装的激光雷达为例,发布安装变换,用 ROS 2 Jazzy 的 tf2 将激光端点换到 base_link,再用数值和 RViz 2 核查方向;相机三维点可按相同方法处理。

学习目标

  • 区分传感器源坐标系、机体目标坐标系和变换查询方向。
  • 使用 tf2_ros 与 tf2_geometry_msgs 转换一个带坐标系的三维点。
  • 通过可计算的坐标结果与可视化发现安装偏移、轴向和时间戳错误。

认清坐标系和变换方向

按 ROS 常用机体约定,base_link 的 x 轴指向车头、y 轴指向左侧、z 轴向上。laser_frame 的 x 轴沿激光零度方向;相机的 camera_optical_frame 通常是 z 轴向前、x 轴向右、y 轴向下,不能把光学坐标直接当成机体坐标。

假设雷达安装在机体中心前方 0.20 m、上方 0.25 m,且绕 z 轴正向偏航 90°。雷达自身坐标中的点 (1.00, 0, 0) m 在机体坐标中应为 (0.20, 1.00, 0.25) m:先按安装姿态旋转,再加安装平移。tf2 用 target=base_link、source=laser_frame 查询的就是这一方向的变换。

激光点从雷达坐标转换到机体坐标的旋转和平移关系

发布固定安装变换

在 Ubuntu 24.04、已安装 ROS 2 Jazzy 且配置好 ROS 环境的终端执行下面命令。这里只为独立验收创建演示坐标系;实际机器人若已由 URDF 和 robot_state_publisher 发布同一父子变换,不能重复发布。

bash
source /opt/ros/jazzy/setup.bash
ros2 run tf2_ros static_transform_publisher --x 0.20 --y 0 --z 0.25 --roll 0 --pitch 0 --yaw 1.57079632679 --frame-id base_link --child-frame-id laser_frame

预期结果:发布节点持续运行,/tf_static 中存在父帧 base_link、子帧 laser_frame 的固定变换,平移为 (0.20, 0, 0.25) m,偏航约 1.571 rad。让此终端保持运行;实际启动日志会因补丁版本而不同。

--frame-id 指父坐标系,--child-frame-id 指子坐标系;参数的平移是在父坐标系中描述子原点,RPY 单位是弧度。把父子关系写反,即使 tf2 能求逆,也会使安装含义与实际模型不符。

用 tf2 转换一束激光点

本例把一束距雷达 1 m、角度为 0° 的激光端点封装为 PointStamped。保存以下完整 Python 程序为 sensor_point_to_base.py;另开终端先执行 source /opt/ros/jazzy/setup.bash,再运行 python3 sensor_point_to_base.py。需有 rclpy、geometry_msgs、tf2_ros 和 tf2_geometry_msgs(缺少后者可安装 ros-jazzy-tf2-geometry-msgs)。

python
import rclpy
from geometry_msgs.msg import PointStamped
from rclpy.node import Node
from rclpy.time import Time
from tf2_geometry_msgs import do_transform_point
from tf2_ros import Buffer, TransformException, TransformListener

class SensorPointDemo(Node):
    def __init__(self):
        super().__init__('sensor_point_demo')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.done = False
        self.timer = self.create_timer(0.2, self.convert)

    def convert(self):
        point = PointStamped()
        point.header.frame_id = 'laser_frame'
        # 雷达零度方向上的 1 m 端点;此例只使用固定安装变换
        point.point.x = 1.0
        point.point.y = 0.0
        point.point.z = 0.0

        try:
            # 参数顺序是目标帧、源帧、查询时间;零时间取最新
            tf = self.buffer.lookup_transform(
                'base_link', point.header.frame_id, Time())
        except TransformException as exc:
            self.get_logger().warning(f'等待 TF:{exc}')
            return

        result = do_transform_point(point, tf)
        print(f'{result.header.frame_id}: '
              f'({result.point.x:.2f}, {result.point.y:.2f}, '
              f'{result.point.z:.2f})', flush=True)
        self.done = True
        self.timer.cancel()

def main():
    rclpy.init()
    node = SensorPointDemo()
    try:
        while rclpy.ok() and not node.done:
            rclpy.spin_once(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

预期输出(允许最初短暂等待 TF):

text
base_link: (0.20, 1.00, 0.25)

这是旋转和平移都生效的证据:只把点的 x 值加上 0.20 会错误得到 (1.20, 0, 0.25)。对真实 LaserScan,先按 range 与对应 angle_min + i * angle_increment 还原点,再转换坐标;原始距离数组本身不是机体坐标点。

验证方向并处理时间问题

先在另一个已加载 Jazzy 环境的终端检查安装变换:

bash
ros2 run tf2_ros tf2_echo base_link laser_frame

预期结果:持续输出平移约 (0.200, 0.000, 0.250) m、四元数约 (0, 0, 0.707, 0.707),偏航约 +1.571 rad;时间和输出格式随环境变化。然后在 RViz 2 中将 Global Options > Fixed Frame 设为 base_link,添加 TF 显示,检查雷达局部 x 轴是否指向机体左侧,局部 z 轴是否仍向上。

观察现象 首先检查
计算结果变成 (1.20, 0, 0.25) 漏了 90° 旋转,或安装 yaw 被设成 0
正向激光点落在机体右侧 正负 yaw、雷达真实零度方向是否相反
提示坐标帧不存在或不连通 header.frame_id 拼写、TF 父子链、节点是否运行
运动中出现时间外推异常 观测时间戳、use_sim_time、TF 历史缓存及发布延迟

若数据来自相机,先利用深度和相机内参把像素反投影成 camera_optical_frame 内的三维米制点。例如 fx=fy=600、cx=320、cy=240、像素 (380,240)、深度 2 m,对应光学坐标 (0.20,0,2.00) m,再经相机光学帧到 base_link 的 TF 链转换。单纯的像素 (u,v) 不能直接传给三维坐标变换。

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

目录与阅读设置

阅读设置

17

记录一个想法

将传感器观测变到机体坐标

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

登录哪都通