机器人 / 系统课时
将传感器观测变到机体坐标
将传感器观测变到机体坐标
传感器报告的点通常位于自身坐标系,不能直接当作底盘前方的障碍物坐标。本课以一个侧向安装的激光雷达为例,发布安装变换,用 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 发布同一父子变换,不能重复发布。
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)。
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):
base_link: (0.20, 1.00, 0.25)这是旋转和平移都生效的证据:只把点的 x 值加上 0.20 会错误得到 (1.20, 0, 0.25)。对真实 LaserScan,先按 range 与对应 angle_min + i * angle_increment 还原点,再转换坐标;原始距离数组本身不是机体坐标点。
验证方向并处理时间问题
先在另一个已加载 Jazzy 环境的终端检查安装变换:
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) 不能直接传给三维坐标变换。