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在运行过程中会发布一系列话题和定位结果直接相关的有以下几个话题名称数据类型用途/tftf2_msgs/TFMessage实时位姿变换包含map到odom、odom到base_link等/cartographer/trajectory_node_listcartographer_ros_msgs/TrajectoryNodeList轨迹节点列表含位姿、时间戳、子图索引/cartographer/submap_listcartographer_ros_msgs/SubmapList子图列表含子图位姿和版本/cartographer/scan_matched_points2sensor_msgs/PointCloud2扫描匹配后的点云/cartographer/constraint_listcartographer_ros_msgs/ConstraintList约束列表含回环约束做定位结果分析最核心的是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、posegeometry_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, fieldnamesnodes[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(dropTrue) 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, kk) neighbors coords[idx] local_vars.append(np.var(neighbors, axis0).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], cdf[timestamp], cmapviridis, s2) plt.colorbar(sc, labelTime (s)) ax.set_xlabel(X (m)) ax.set_ylabel(Y (m)) ax.set_title(Cartographer Trajectory) ax.axis(equal) plt.savefig(trajectory.png, dpi150)速度曲线用折线图横轴时间纵轴速度。异常点用红色标记。残差分布用直方图横轴残差纵轴频次。如果残差分布集中在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_valueextrapolate) gt_interp_y interp1d(gt_df[timestamp], gt_df[y], fill_valueextrapolate) 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(fRMSE: {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(dropTrue) # 计算指标 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 回环失效参数调整与验证回环失效的表现是轨迹闭合误差大或者轨迹在回到起点时没有闭合。常见原因和调整方法问题现象可能原因调整方法回环检测不到搜索半径太小增大pose_graph.optimization_problem.huber_scale回环误匹配匹配阈值太松减小constraint_builder.min_score回环后轨迹跳变约束权重不合理调整pose_graph.constraint_builder.global_localization_min_score回环延迟大计算资源不足减少子图数量或降低分辨率我自己的经验是室内场景下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 常见问题速查表问题可能原因排查方法解决方案轨迹跳变扫描匹配失败查看跳变时刻点云增加特征点或调整匹配参数回环失效搜索半径小检查constraint_list增大搜索半径漂移大里程计误差累积计算漂移率增加回环频率时间戳异常传感器不同步对比时间戳重新同步传感器子图不一致子图位姿估计不准对比节点和子图位姿调整子图分辨率定位延迟计算资源不足查看CPU占用降低分辨率或减少子图6. 分析结果如何反哺定位调参6.1 从数据指标到参数调整的映射分析结果不是终点而是调参的起点。比如如果发现回环残差大可以调整constraint_builder.min_score如果发现轨迹跳变频繁可以调整pose_graph.optimization_problem.huber_scale如果发现漂移率高可以增加回环搜索频率。我一般会建立一个映射表数据指标阈值对应参数调整方向回环残差0.5min_score增大轨迹跳变次数5huber_scale减小漂移率1%回环频率增大时间偏移50ms时间同步重新校准子图差异1m子图分辨率减小这个映射表不是绝对的但能给你一个调参的方向。实际调参还需要结合具体场景和传感器配置。6.2 迭代验证分析-调参-再分析的闭环调参不是一次性的而是一个闭环。每次调整参数后重新跑bag重新分析对比指标变化。我一般会记录每次调参后的指标形成一个调参日志。import json def log_experiment(params, metrics, log_fileexperiment_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, indent2)这样能清晰看到哪个参数组合效果最好。实测下来经过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节点里实时监控定位质量一旦发现异常就报警或者用机器学习方法从轨迹数据里自动识别异常模式减少人工分析的工作量。我现在正在尝试后者用简单的聚类算法对轨迹节点做异常检测初步效果还不错。