企业官网建设流程全解析

1. 从一堆跳动的坐标说起：cartographer定位结果到底该怎么看

如果你正在做机器人或者自动驾驶相关的项目，大概率绕不开cartographer这个激光SLAM方案。它建图稳、回环准、社区活跃，很多室内机器人、AGV、巡检车都在用它。但真正把系统跑起来之后，很多人会卡在同一个地方：cartographer输出的定位结果到底靠不靠谱？轨迹漂了多少？哪个时间段定位跳变了？回环有没有真正生效？这些问题光靠rviz里那条绿色轨迹是看不出来的，必须把定位结果的数据拉出来做分析。

这篇内容就是围绕“cartographer定位结果的数据分析”展开的。我会从数据来源、数据格式、分析维度、实操脚本、常见异常排查几个层面，把整套分析流程讲清楚。适合已经跑通cartographer、但不知道怎么评估定位质量的朋友，也适合需要给定位模块做量化测试的工程同学。核心目标只有一个：让你拿到定位数据之后，知道该看什么、怎么算、怎么判断好坏，而不是凭感觉说“看起来还行”。

cartographer本身会发布多个话题，最常用来做分析的是/tf、/cartographer/trajectory_node_list、/cartographer/scan_matched_points2以及/cartographer/submap_list。其中轨迹节点列表是分析定位结果的核心数据源，它包含了每个轨迹节点的位姿、时间戳和对应的子图信息。很多人只订阅/tf看实时位姿，但/tf是不断被覆盖的，做离线分析必须用轨迹节点列表或者自己录制bag。这一点后面会详细展开。

我自己的习惯是：先把bag录下来，然后用Python脚本把轨迹节点解析成CSV，再用pandas和matplotlib做统计和可视化。整套流程不复杂，但细节很多，踩过的坑也不少。下面按模块拆开讲。

2. 定位结果的数据来源与格式拆解

2.1 cartographer到底发布了哪些定位相关数据

cartographer在运行过程中会发布一系列话题，和定位结果直接相关的有以下几个：

做定位结果分析，最核心的是trajectory_node_list。它里面每个节点包含：节点ID、时间戳、位姿（四元数+平移）、对应的子图ID列表。这个数据是累积的，不会像/tf那样被覆盖，所以适合离线分析。

constraint_list也很重要，尤其是做回环分析的时候。它能告诉你哪些节点之间建立了约束，约束的残差是多少。残差大的约束往往意味着回环有问题。

2.2 为什么不能只看/tf

很多人做分析的时候直接订阅/tf，把map到base_link的变换记录下来。这样做有两个问题：第一，/tf是实时广播的，频率高但会被覆盖，如果你录bag的时候没录全，后面就补不回来；第二，/tf里的位姿是经过插值和平滑的，不能反映cartographer内部优化后的真实轨迹节点位姿。真正做精度评估，必须用trajectory_node_list里的原始节点位姿。

还有一个细节：cartographer在回环优化之后，会重新发布轨迹节点列表，但/tf的更新可能会有延迟。如果你在回环发生的瞬间看/tf，可能会看到位姿跳变，但轨迹节点列表里其实已经平滑过渡了。所以分析回环效果，一定要用轨迹节点数据。

2.3 数据录制与提取的实操方法

录制bag的时候，建议至少录以下话题：

rosbag record /tf /tf_static /cartographer/trajectory_node_list \ /cartographer/submap_list /cartographer/constraint_list \ /scan /imu/data

录完之后，用Python提取轨迹节点数据。cartographer_ros_msgs里TrajectoryNodeList的定义大致是这样的：每个节点有node_id、timestamp、pose（geometry_msgs/Pose）、submap_index。提取脚本可以这样写：

import rosbag import csv from cartographer_ros_msgs.msg import TrajectoryNodeList bag = rosbag.Bag('your_bag.bag') nodes = [] for topic, msg, t in bag.read_messages(topics=['/cartographer/trajectory_node_list']): for node in msg.trajectory_nodes: nodes.append({ 'node_id': node.node_id, 'timestamp': node.timestamp.secs + node.timestamp.nsecs * 1e-9, 'x': node.pose.position.x, 'y': node.pose.position.y, 'z': node.pose.position.z, 'qx': node.pose.orientation.x, 'qy': node.pose.orientation.y, 'qz': node.pose.orientation.z, 'qw': node.pose.orientation.w, }) with open('trajectory_nodes.csv', 'w', newline='') as f: writer = csv.DictWriter(f, fieldnames=nodes[0].keys()) writer.writeheader() writer.writerows(nodes)

这里有个坑：trajectory_node_list是累积发布的，每次发布都包含之前所有节点。如果你直接遍历所有消息，会得到大量重复节点。正确做法是只取最后一次发布的消息，或者按node_id去重。我一般用字典按node_id去重，保留最新的位姿。

注意：如果bag里录了多个轨迹（比如多传感器融合），trajectory_node_list会包含多个轨迹的节点，需要按trajectory_id区分。单轨迹情况下可以忽略。

3. 定位精度分析的五个核心维度

3.1 轨迹平滑度：用曲率和加速度判断跳变

轨迹平滑度是最直观的指标。cartographer优化后的轨迹应该是连续光滑的，如果出现突然的折线或者跳变，说明定位有问题。计算平滑度可以用相邻节点之间的位移和角度变化。

具体做法：对每个节点，计算它到前一个节点的欧氏距离，再除以时间差，得到瞬时速度。如果某个节点的速度突然变成前一个节点的好几倍，或者方向突变超过阈值，就标记为异常点。

import pandas as pd import numpy as np df = pd.read_csv('trajectory_nodes.csv') df = df.sort_values('timestamp').reset_index(drop=True) df['dx'] = df['x'].diff() df['dy'] = df['y'].diff() df['dt'] = df['timestamp'].diff() df['dist'] = np.sqrt(df['dx']**2 + df['dy']**2) df['speed'] = df['dist'] / df['dt'] speed_mean = df['speed'].mean() speed_std = df['speed'].std() df['speed_zscore'] = (df['speed'] - speed_mean) / speed_std outliers = df[df['speed_zscore'].abs() > 3] print(f"检测到 {len(outliers)} 个速度异常点")

实测下来，室内AGV正常行驶速度在0.3到1.0 m/s之间，如果某个节点速度超过2 m/s，基本可以判定为跳变。跳变的原因可能是扫描匹配失败、回环误匹配、或者传感器数据丢帧。

3.2 回环效果：约束残差与轨迹闭合误差

回环是cartographer的核心能力，但回环不一定每次都正确。判断回环效果有两个指标：约束残差和轨迹闭合误差。

约束残差可以从constraint_list里读。每个约束有一个residual字段，表示优化后该约束的误差。残差越小，说明回环越准。一般来说，室内场景下残差应该在0.1以下，如果超过0.5，说明这个回环可能有问题。

轨迹闭合误差是指机器人回到起点时，估计位姿和实际起点的偏差。计算方法：找到时间上最早和最晚的节点，如果机器人确实回到了起点附近，计算这两个节点的位姿差。

start_node = df.iloc[0] end_node = df.iloc[-1] closure_error = np.sqrt((end_node['x'] - start_node['x'])**2 + (end_node['y'] - start_node['y'])**2) print(f"轨迹闭合误差: {closure_error:.3f} m")

室内场景下，闭合误差在0.1到0.3米之间算正常，超过0.5米就需要检查回环参数了。常见原因是回环搜索半径太小、或者子图分辨率不够。

3.3 位姿协方差：不确定性的量化表达

cartographer在优化过程中会计算位姿协方差，虽然默认不直接发布，但可以通过trajectory_node_list里的位姿和约束信息间接估计。更直接的方法是订阅/cartographer/pose_graph相关的服务，获取优化后的协方差矩阵。

如果拿不到协方差，可以用一个近似方法：计算每个节点周围K个邻居的位姿分布，用局部方差来表征不确定性。局部方差大的区域，说明定位置信度低。

from scipy.spatial import KDTree coords = df[['x', 'y']].values tree = KDTree(coords) k = 10 local_vars = [] for i, point in enumerate(coords): dist, idx = tree.query(point, k=k) neighbors = coords[idx] local_vars.append(np.var(neighbors, axis=0).sum()) df['local_var'] = local_vars

局部方差高的地方，往往是走廊、长直通道、或者特征稀疏的区域。这些区域激光扫描匹配容易退化，定位精度会下降。

3.4 时间一致性：时间戳对齐与延迟分析

cartographer的定位结果时间戳应该和传感器数据时间戳对齐。如果出现时间戳跳变或者延迟，会导致定位结果和实际运动不匹配。分析方法是：把轨迹节点的时间戳和IMU、激光的时间戳做对比，看是否有系统性偏移。

# 假设imu_timestamps是从imu数据提取的时间戳 imu_df = pd.read_csv('imu_data.csv') time_diff = df['timestamp'].values[:len(imu_df)] - imu_df['timestamp'].values print(f"平均时间偏移: {np.mean(time_diff):.6f} s") print(f"时间偏移标准差: {np.std(time_diff):.6f} s")

正常情况下，时间偏移应该在毫秒级，标准差小于10毫秒。如果偏移超过50毫秒，说明时间同步有问题，需要检查传感器驱动的时间戳设置。

3.5 子图一致性：子图位姿与节点位姿的匹配度

cartographer的定位结果是基于子图的。每个轨迹节点会关联到一个或多个子图，节点位姿和子图位姿应该是一致的。如果某个节点关联的子图位姿和节点位姿差异很大，说明这个节点的定位可能有问题。

分析方法：从submap_list里读取子图位姿，然后对每个节点，计算它关联子图的位姿和节点位姿的差异。

# 假设submap_poses是子图位姿字典，key是submap_index for _, node in df.iterrows(): submap_idx = node['submap_index'] if submap_idx in submap_poses: submap_pose = submap_poses[submap_idx] diff = np.sqrt((node['x'] - submap_pose['x'])**2 + (node['y'] - submap_pose['y'])**2) if diff > 1.0: print(f"节点 {node['node_id']} 与子图 {submap_idx} 位姿差异过大: {diff:.3f} m")

这个指标能帮你发现子图切换时的定位跳变。尤其是在大场景下，子图之间的拼接误差会累积，导致定位结果在子图边界处出现不连续。

4. 完整分析流程与脚本实现

4.1 从bag到CSV：数据提取的完整步骤

第一步，确认bag里录了/cartographer/trajectory_node_list。如果没有，可以用rosbag info查看话题列表。如果确实没录，只能重新跑一遍并录制。

第二步，用前面给的Python脚本提取轨迹节点数据。注意去重和排序。去重逻辑可以这样写：

node_dict = {} for topic, msg, t in bag.read_messages(topics=['/cartographer/trajectory_node_list']): for node in msg.trajectory_nodes: node_dict[node.node_id] = { 'node_id': node.node_id, 'timestamp': node.timestamp.secs + node.timestamp.nsecs * 1e-9, 'x': node.pose.position.x, 'y': node.pose.position.y, 'z': node.pose.position.z, 'qx': node.pose.orientation.x, 'qy': node.pose.orientation.y, 'qz': node.pose.orientation.z, 'qw': node.pose.orientation.w, }

第三步，把字典转成DataFrame，按时间戳排序，保存为CSV。这一步做完，数据就准备好了。

4.2 可视化：轨迹图、速度曲线、残差分布

可视化是分析的第一步。我一般画三张图：轨迹图、速度曲线、残差分布。

轨迹图用matplotlib画，横轴x，纵轴y，颜色可以用时间戳渐变，这样能看出轨迹的时间顺序。

import matplotlib.pyplot as plt fig, ax = plt.subplots(figsize=(10, 8)) sc = ax.scatter(df['x'], df['y'], c=df['timestamp'], cmap='viridis', s=2) plt.colorbar(sc, label='Time (s)') ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_title('Cartographer Trajectory') ax.axis('equal') plt.savefig('trajectory.png', dpi=150)

速度曲线用折线图，横轴时间，纵轴速度。异常点用红色标记。

残差分布用直方图，横轴残差，纵轴频次。如果残差分布集中在0附近，说明回环质量好；如果出现长尾，说明有异常约束。

4.3 参数计算：漂移率、闭合误差、RMSE

漂移率是衡量定位精度的核心指标。计算方法：在没有回环的纯里程计轨迹上，计算估计轨迹和真实轨迹的偏差。如果没有真实轨迹（比如没有动捕或者RTK），可以用轨迹闭合误差来近似。

漂移率 = 闭合误差 / 轨迹总长度。室内场景下，漂移率应该小于1%，即每走100米，误差小于1米。

RMSE（均方根误差）需要真实轨迹。如果有动捕数据，可以把cartographer轨迹和动捕轨迹做时间对齐，然后计算每个节点的位置误差，再求RMSE。

from scipy.interpolate import interp1d # 假设gt_df是真实轨迹，包含timestamp, x, y gt_interp_x = interp1d(gt_df['timestamp'], gt_df['x'], fill_value='extrapolate') gt_interp_y = interp1d(gt_df['timestamp'], gt_df['y'], fill_value='extrapolate') errors = [] for _, node in df.iterrows(): gt_x = gt_interp_x(node['timestamp']) gt_y = gt_interp_y(node['timestamp']) err = np.sqrt((node['x'] - gt_x)**2 + (node['y'] - gt_y)**2) errors.append(err) rmse = np.sqrt(np.mean(np.array(errors)**2)) print(f"RMSE: {rmse:.4f} m")

实测下来，室内AGV在良好环境下，cartographer的RMSE可以做到0.05到0.1米。如果超过0.2米，就需要调参了。

4.4 自动化分析脚本的封装

把上面的步骤封装成一个脚本，输入bag路径，输出分析报告。报告包含：轨迹图、速度曲线、残差分布、漂移率、RMSE、异常点列表。

def analyze_cartographer_bag(bag_path, output_dir): # 提取数据 nodes = extract_trajectory_nodes(bag_path) df = pd.DataFrame(nodes).sort_values('timestamp').reset_index(drop=True) # 计算指标 df = compute_speed(df) closure_error = compute_closure_error(df) drift_rate = closure_error / compute_trajectory_length(df) # 可视化 plot_trajectory(df, output_dir) plot_speed(df, output_dir) # 输出报告 report = { 'closure_error': closure_error, 'drift_rate': drift_rate, 'num_nodes': len(df), 'duration': df['timestamp'].max() - df['timestamp'].min(), } return report

这个脚本可以集成到CI里，每次跑完bag自动分析，生成报告。对于需要频繁测试定位参数的团队，能省不少时间。

5. 常见异常与排查技巧实录

5.1 轨迹跳变：原因分析与定位

轨迹跳变是最常见的问题。表现是轨迹图上出现明显的折线或者断点。原因可能有以下几种：

第一，扫描匹配失败。激光数据在特征稀疏区域（比如长走廊）匹配不上，导致位姿估计突变。排查方法：看跳变时刻的激光点云，如果点云稀疏或者全是平面，基本可以确认。

第二，回环误匹配。回环约束把两个不相关的节点连在一起，优化后轨迹被拉偏。排查方法：看constraint_list里跳变时刻附近的约束，如果残差很大，说明回环有问题。

第三，时间戳跳变。传感器数据时间戳不连续，导致cartographer内部时间计算错误。排查方法：对比轨迹节点时间戳和传感器时间戳，看是否有跳变。

第四，子图切换。大场景下子图切换时，如果子图位姿估计不准，会导致轨迹跳变。排查方法：看跳变时刻是否正好是子图切换点。

5.2 回环失效：参数调整与验证

回环失效的表现是轨迹闭合误差大，或者轨迹在回到起点时没有闭合。常见原因和调整方法：

我自己的经验是：室内场景下，min_score设置在0.55到0.65之间比较合适。太低会误匹配，太高会漏检。huber_scale设置在1e1左右，能有效抑制异常约束的影响。

5.3 定位漂移：从数据看漂移模式

定位漂移分两种：缓慢漂移和突然漂移。缓慢漂移通常是里程计累积误差导致的，表现为轨迹逐渐偏离真实路径。突然漂移通常是扫描匹配失败或者回环误匹配导致的。

分析漂移模式的方法：计算每个节点的局部漂移率，即相邻节点之间的位移和真实位移的偏差。如果没有真实轨迹，可以用轨迹的曲率变化来近似。

df['curvature'] = np.abs(df['dx'].diff() / df['dt'].diff()) high_curvature = df[df['curvature'] > df['curvature'].quantile(0.95)] print(f"高曲率点数量: {len(high_curvature)}")

高曲率点集中的区域，往往是漂移严重的区域。可以结合激光点云和子图信息进一步分析。

5.4 数据丢帧与时间戳异常的处理

数据丢帧会导致轨迹节点不连续。排查方法：计算相邻节点的时间差，如果某个时间差远大于平均时间差，说明有丢帧。

df['dt'] = df['timestamp'].diff() median_dt = df['dt'].median() gaps = df[df['dt'] > 3 * median_dt] print(f"检测到 {len(gaps)} 个时间间隙") for _, gap in gaps.iterrows(): print(f"时间间隙: {gap['dt']:.3f} s, 节点ID: {gap['node_id']}")

时间戳异常的处理方法是：如果丢帧不严重，可以用插值补全；如果丢帧严重，需要检查传感器驱动或者录制脚本。

提示：cartographer对时间戳非常敏感，如果时间戳有跳变，优化结果会严重失真。建议在录制bag之前，先检查所有传感器的时间戳是否同步。

5.5 常见问题速查表

6. 分析结果如何反哺定位调参

6.1 从数据指标到参数调整的映射

分析结果不是终点，而是调参的起点。比如，如果发现回环残差大，可以调整constraint_builder.min_score；如果发现轨迹跳变频繁，可以调整pose_graph.optimization_problem.huber_scale；如果发现漂移率高，可以增加回环搜索频率。

我一般会建立一个映射表：

这个映射表不是绝对的，但能给你一个调参的方向。实际调参还需要结合具体场景和传感器配置。

6.2 迭代验证：分析-调参-再分析的闭环

调参不是一次性的，而是一个闭环。每次调整参数后，重新跑bag，重新分析，对比指标变化。我一般会记录每次调参后的指标，形成一个调参日志。

import json def log_experiment(params, metrics, log_file='experiment_log.json'): try: with open(log_file, 'r') as f: log = json.load(f) except FileNotFoundError: log = [] log.append({ 'params': params, 'metrics': metrics, 'timestamp': time.time() }) with open(log_file, 'w') as f: json.dump(log, f, indent=2)

这样能清晰看到哪个参数组合效果最好。实测下来，经过3到5轮迭代，定位精度通常能提升30%以上。

6.3 不同场景下的分析侧重点

不同场景下，分析侧重点不一样。室内结构化场景，重点看回环和子图一致性；室外非结构化场景，重点看漂移率和时间一致性；动态场景，重点看跳变和异常点。

比如，在长走廊场景下，激光扫描匹配容易退化，漂移率会明显升高。这时候分析的重点是漂移模式，看漂移是线性的还是随机的。如果是线性的，说明里程计标定有问题；如果是随机的，说明扫描匹配不稳定。

在回环密集的场景下，重点看约束残差和回环延迟。如果回环延迟大，说明计算资源不足，需要降低子图分辨率或者减少回环搜索频率。

7. 我踩过的坑和几条实用建议

第一个坑：直接用/tf做分析。前面说过，/tf是实时广播的，会被覆盖，而且经过插值。我一开始用/tf分析，发现轨迹特别平滑，但实际定位精度很差。后来换成trajectory_node_list，才看到真实的跳变和漂移。

第二个坑：忽略时间戳去重。trajectory_node_list是累积发布的，如果不去重，会得到大量重复节点，导致速度计算错误。我一开始没去重，算出来的速度全是0，因为相邻节点时间戳一样。

第三个坑：回环残差阈值设得太松。我一开始觉得残差0.5以下都算正常，结果发现有些回环残差0.4但轨迹明显偏了。后来把阈值调到0.2，才准确识别出问题回环。

第四个坑：没有真实轨迹就下结论。没有动捕或者RTK的情况下，只能靠闭合误差和漂移率来近似。但闭合误差受起点和终点选择影响很大，如果机器人没有精确回到起点，闭合误差会偏大。我一般会结合多个指标综合判断，而不是只看一个。

几条实用建议：第一，录制bag的时候尽量多录话题，尤其是constraint_list和submap_list，后面分析回环和子图一致性会用到。第二，分析脚本尽量模块化，提取、计算、可视化分开，方便复用。第三，调参的时候一次只调一个参数，否则无法判断哪个参数起了作用。第四，建立调参日志，记录每次参数和指标，方便回溯。

这个内容后续还可以这样扩展：把分析脚本集成到ROS节点里，实时监控定位质量，一旦发现异常就报警；或者用机器学习方法，从轨迹数据里自动识别异常模式，减少人工分析的工作量。我现在正在尝试后者，用简单的聚类算法对轨迹节点做异常检测，初步效果还不错。