我先说个真实经历。之前调试一台移动底盘电机参数都调得没问题但小车就是不走直线给固定线速度以后总往右偏。改增益、换轮径、重新标定 IMU反复折腾一下午没结论。后来我写了个小脚本把/odom话题里的位置坐标用 matplotlib 画出来不到十分钟就找到问题了——左右轮实际转速差了一截轨迹从起点开始就带弧度。从那天起/odom轨迹图就成了我调试移动机器人时的标配工具。很多人第一反应是看轨迹用 rviz 不行吗rviz 当然可以但 rviz 的轨迹不能保存成图、不方便叠加多条数据做对比而且遇到反复改参数试跑的场景你只能一遍遍回放日志。用rospy把/odom数据接出来再用matplotlib画图代码量不大胜在灵活离线分析能慢慢看现场调试能实时刷新甚至能把速度、偏航角叠加到轨迹上。这篇文章就围绕这个主题把我实际用过的分析思路、完整代码、踩过的坑都写出来给正在调试底盘或做导航的朋友做个参考。1. 为什么偏偏是 /odom 轨迹图三个让我离不开它的调试场景1.1 排查底盘行走畸变移动底盘最常见的故障表现是“不走直线”。给一个恒定线速度命令理论上里程计反馈的轨迹应该是一条直线但实际情况往往是圆弧、S 形或者锯齿形。不看轨迹图的时候你只能通过“小车偏了”“转多了”这种模糊描述来猜问题效率特别低。把cmd_vel固定同时订阅/odom把pose.pose.position.x和pose.pose.position.y画出来就能直接看到轨迹的几何形态。圆弧说明左右轮速度不一致S 形说明转向存在过冲或震荡锯齿说明编码器数据里有噪声或者轮子打滑。这个方法是纯几何判断不依赖任何传感器标定值比看电机反馈数据直观得多。1.2 判断导航避障时的运动质量导航模块很多人只看“到没到目标点”但“导航过程好不好”同样重要。比如局部路径规划在绕障时机器人轨迹如果剧烈拐弯甚至来回摆动说明代价地图参数或者速度限制设置得不合理。有一次我做避障调试目标点就在前方 3 米中间放一个障碍物。用轨迹图拉了一遍全程发现机器人绕障以后明显往回倒了一下再重新前进在这个位置看地图数据完全看不出问题因为路径预规划是平滑的问题出在局部规划器的减速半径和障碍物膨胀层重叠了。这种问题只有在全局轨迹图上才能一眼定位。后面我养成了习惯每轮导航测试都顺手存一条/odom轨迹结束以后和预期路径叠在一起看偏差。1.3 评估定位与里程计偏差/odom是轮式里程计直接推算的结果会随时间产生累积误差。如果你在场地里跑一个矩形路线起点和终点应该重合如果轨迹终点和起点偏了一大截说明里程计需要重新标定或者有打滑情况。更实用的是对比多圈运行让机器人沿同一路径跑三次把三条轨迹画在同一张图上如果三次轨迹的差异越来越大那么轮子磨损、机械间隙这类问题就要重点怀疑。这种多轨迹叠加只有自己画图才能实现rviz 里想做到这个效果得对每个话题单独配置显示项。2. 绘图前必须理顺的三件事消息结构、坐标系和时间戳2.1 从 Odometry 消息里取到我们真正需要的值/odom话题的消息类型是nav_msgs/Odometry它包含header、child_frame_id、pose和twist四个部分。绘图最常用的就是pose.pose.position.x和pose.pose.position.y这两个字段表示机器人在里程计坐标系下的平面坐标。除了位置pose.pose.orientation里的四元数也经常要用到它可以转换成偏航角yaw。转换公式是import math def quat_to_yaw(msg): q msg.pose.pose.orientation # 四元数转欧拉角只需要绕 z 轴的偏航角 yaw math.atan2( 2.0 * (q.w * q.z q.x * q.y), 1.0 - 2.0 * (q.y * q.y q.z * q.z) ) return yaw这个转换我每次写脚本都会用到建议直接封装成工具函数。twist.twist.linear.x也能顺带取出来后面可以用颜色表示速度。2.2 odom 系与 base_link 系要画的是谁在哪个系下的坐标很多刚接触 ROS 的人会混淆一个点/odom里面的坐标并不是“机器人在地图上的绝对位置”而是base_link原点在odom坐标系下的坐标。odom坐标系的原点通常是机器人启动时的位置方向是固定的而机器人的base_link坐标系跟随机器人本体旋转。所以pose.pose.position.x/y描述的是“在固定坐标系中机器人本体移动到了哪里”这正好是我们绘制轨迹需要的含义。我这里画轨迹用的是header.frame_id /odom时理解出的平面坐标而不是base_link坐标系下的局部坐标。前者是固定的参考系后者跟着机器人转画出来就是一团螺旋线没有分析价值。如果你看到轨迹变成一个奇怪的螺旋或者不断转圈的形状先检查是不是把坐标系理解错了。2.3 时间戳、帧率与四元数转偏航角还有一个细节是时间戳。/odom消息的header.stamp是 ROS 时间通常用秒和纳秒表示。如果你后面要计算速度、加速度或者要对轨迹做时间窗口滤波就需要把时间戳统一转换成浮点数t msg.header.stamp.secs msg.header.stamp.nsecs * 1e-9里程计话题的发布频率一般在 10Hz 到 50Hz 之间。画轨迹时这个频率通常不需要处理直接每个回调点都描上就行。但如果做实时图刷新需要控制绘图频率不能把每一条消息都立即重绘一次否则窗口会卡死。这个话题我在第 5 节细说。另外提一句我这里讲的是直接订阅/odom话题。有人会问监听/tf不也能拿到odom - base_link的变换吗能但/tf数据经过变换树管理可能受tf缓冲时间影响而且某些临时变换没广播时就拿不到。调试阶段直接订阅/odom看到的是发布者原发的数据链路更短出问题更容易定位。3. 离线方案先录 bag 再慢慢画分析、出图和对比都方便3.1 录制 /odom 数据的两个注意点实时绘制的场景适合现场调参但很多分析工作其实不需要实时。比如在场地里跑完一轮测试回头慢慢看轨迹细节或者把今天的轨迹和昨天的对比这时候离线方案更合适。录制数据用rosbag命令很简单rosbag record /odom -O odom_trace.bag注意两点。第一最好只录需要的主题别把整个系统所有话题都录进去bag 文件会变得很大后面读取也慢。第二录制时让机器人完整跑完设定的路径再停止否则轨迹会断在中间分析时要额外拼接。如果机器人系统里有多个里程计来源比如差分轮和全向轮各有一个/odom最好在录制前确认你录的是哪个。实际调试中我遇到过robot_pose_ekf融合后的/odom和底盘直接发布的/odom同时存在名字一样但数据完全不同。这种话题重名问题录制前用rostopic info /odom查一下发布者是谁比事后排查省事多了。3.2 用脚本重放 bag 并绘制轨迹录制完以后用rosbag库读取消息数据再交给 matplotlib 绘图。下面是一个完整的分析脚本import rosbag import matplotlib.pyplot as plt bag rosbag.Bag(odom_trace.bag) xs, ys [], [] for topic, msg, t in bag.read_messages(topics[/odom]): x msg.pose.pose.position.x y msg.pose.pose.position.y xs.append(x) ys.append(y) bag.close() plt.figure(figsize(8, 8)) plt.plot(xs, ys, -, linewidth1.5) plt.xlabel(x / m) plt.ylabel(y / m) plt.axis(equal) # 保持横纵比例一致 plt.grid(True) plt.title(odom trajectory (offline)) plt.savefig(odom_trajectory.png, dpi150) plt.show()这里额外提两个细节plt.axis(equal)很关键。机器人轨迹的纵横比例如果被拉伸圆弧看起来像椭圆直线看起来像斜线容易误判。用savefig保存结果而不是只能截图方便后续放到调试记录里也方便和其它轨迹做图像级别的对比。离线方案最大的价值在于可以反复重放。一次录制后面改绘图参数颜色、线宽、加航向箭头都不用重新跑机器人只要重新运行脚本就行。我通常会让机器人跑同一段路径多圈然后用一个脚本循环读多个 bag 文件把多条轨迹画在同一张图上机械间隙变化导致的轨迹偏移就能很直观地暴露出来。4. 实时方案订阅 /odom 边跑边画现场调参立刻见效4.1 一个不会被阻塞的回调结构现场调参的场景下离线的“先录再画”节奏太慢。跑一圈、停机器、拷 bag、跑脚本改完参数再来一遍一个参数可能要折腾十几分钟。实时绘图就方便得多参数一改下一圈直接能看到轨迹变化。实时绘图的核心难点是不要阻塞 rospy 的回调线程。很多人写的第一版是直接在订阅回调里调用plt.draw()结果画图的时候 ROS 消息还在不断进来回调被阻塞里程计数据丢失驱动可能还会因为长时间没收到反馈而报错。我的做法是把数据收集和绘图拆开。订阅回调只负责往列表里追加坐标点绘图由一个定时器或者独立线程负责。这样即使绘制画面花了几百毫秒也不会影响里程计数据接收。下面是完整示例import rospy import math from nav_msgs.msg import Odometry import matplotlib.pyplot as plt class OdomTrajectoryPlotter: def __init__(self): self.xs [] self.ys [] rospy.init_node(odom_plotter, anonymousTrue) # 订阅 /odom回调只存数据 rospy.Subscriber(/odom, Odometry, self.odom_callback) # 初始化绘图 self.fig, self.ax plt.subplots(figsize(8, 8)) self.line, self.ax.plot([], [], -, linewidth1.5) self.ax.set_xlabel(x / m) self.ax.set_ylabel(y / m) self.ax.grid(True) # 每隔 0.2 秒刷新一次绘图 rospy.Timer(rospy.Duration(0.2), self.draw_callback) def odom_callback(self, msg): self.xs.append(msg.pose.pose.position.x) self.ys.append(msg.pose.pose.position.y) def draw_callback(self, event): if not self.xs: return self.line.set_data(self.xs, self.ys) self.ax.relim() self.ax.autoscale_view() self.fig.canvas.draw() self.fig.canvas.flush_events() def spin(self): plt.ion() rospy.spin() if __name__ __main__: plotter OdomTrajectoryPlotter() plotter.spin()注意这里用到了plt.ion()让 matplotlib 进入交互模式然后通过rospy.Timer周期性地刷新画布。relim()和autoscale_view()的作用是自动调整坐标范围轨迹跑出当前窗口时不用手动改范围。4.2 绘制出的轨迹图和原始数据同步实时方案跑起来以后页面上的轨迹会像手写笔一样一点点往前延伸。这个方案特别适合调整底盘控制参数改一下最大线速度重新跑直线轨迹在同样的位移里是否平直、是否有抖动一眼就能看出来。如果只是验证话题数据是否正常不需要额外画图窗口还可以用下面这个非常简单的测试方法先启动脚本再用命令行发布模拟里程计数据看轨迹是否按预期生成rostopic pub -r 10 /odom nav_msgs/Odometry ...不过实际调试时我更建议直接跑机器人本体因为模拟数据永远没有真实运行时的打滑、震动、以及电机响应延时。我后来做得比较多的是把实时图和离线 bag 录制一起开一边现场调参一边后台录制 bag 文件等参数调好了再用第 3 节的离线脚本分析精确数值两边的好处都拿到。5. 实战遇到的三个古怪问题镜像翻转、里程计跳变、绘图卡顿5.1 轨迹整体反转或镜像先查坐标系方向再查数据符号有次我在调试全向轮底盘实时画出来的轨迹整体左右翻转了。给机器人发一个“向前”指令轨迹却朝左上方走。我第一反应是电机方向反了但检查底盘驱动四个轮子的方向都对。后来才发现是坐标系的问题。底盘驱动发布的/odom和常规 ROS 标准约定存在差异base_link的 x 轴指向车头但该底盘的 odom 坐标系 y 轴方向和标准 ROS 约定相反导致同样的位置数据画在 matplotlib 里呈现出镜像效果。排查方法是先给机器人发一个明确的“沿 x 轴正方向移动”指令观察实时轨迹在图中是往右还是往左。如果轨迹方向和命令符号相反那说明坐标系定义或数据符号有问题仔细检查驱动代码里坐标系定义和base_link的朝向。不要一上来就改底盘反馈极性先确认参考坐标系。5.2 轨迹瞬间飞出几十米跳变点的判别与过滤另一个高频问题是轨迹中间突然出现一条长线从当前点直接跳到几十米外然后再跳回来。这种情况通常是里程计受到了外部干扰。比如底盘在光滑地面上打滑编码器以为轮子转了实际上车身没动或者是导航切换地图时里程计被重置。处理办法是做跳变过滤。相邻两个点之间的距离如果超过一个物理上不可能的速度上限就把这个点视为跳变点不绘制它。比如机器人的最大线速度是 1m/s里程计频率 20Hz那相邻两点最大位移约 0.05m。留点余量超过 0.5m 的点直接忽略MAX_JUMP 0.5 # 单位米 def is_valid_point(last_x, last_y, x, y): dist ((x - last_x) ** 2 (y - last_y) ** 2) ** 0.5 return dist MAX_JUMP实际代码里我会维护一个last_valid_point每次新点进来先判断和它的距离超过阈值就不追加到轨迹里。这个过滤对离线分析和实时绘图都适用。但要留意过滤逻辑不能解决里程计本身的错误只能让轨迹图更清晰真正要根治还是要检查底盘编码器连接和地面条件。5.3 图窗口卡死与 CPU 过高绘制频率与后端的选择实时绘图最常见的性能问题是窗口卡死。早期版本我直接让绘图频率等于/odom的发布频率比如 50Hz 的消息频率每秒重绘 50 次。结果是图窗口响应迟钝CPU 飙得非常高机器人控制节点都受到影响。两个改进方向。第一降低绘制频率30Hz 或 50Hz 的里程计数据完全没有必要按同样频率重绘人眼在 10Hz 左右的刷新率下已经觉得流畅我用的是 5Hz。第二避免不必要的全图重绘relim()、autoscale_view()这些操作每次都会重新计算整个坐标轴的范围数据量大了以后计算量不小可以在机器人移动范围较大时才重新调整一次否则固定坐标范围。另外再说一个 GUI 后端的坑。matplotlib 在不同系统下默认后端不同某些后端在交互模式下会出现窗口无响应的问题。如果遇到窗口点了没反应试试在脚本开头强制指定后端import matplotlib matplotlib.use(TkAgg)当然如果你的环境不支持图形界面那就只能走离线方案。只需要把数据存成 CSV 或者先用plt.savefig保存图片之后在另一台机器上绘制。6. 让轨迹图从“能看”到“好用”叠加航向、速度和多重坐标对比6.1 用散点颜色表达线速度一眼看出运动平稳性纯线条轨迹只能反映路径形态看不出机器人在每个位置跑得快还是慢。把速度信息叠加到图上以后很多问题会变得更直观。做法是不要用plt.plot画连续线改用plt.scatter点的颜色用当前时刻的线速度表示import numpy as np from matplotlib import cm def draw_trajectory_with_speed(xs, ys, speeds): plt.figure(figsize(8, 8)) v np.array(speeds) sc plt.scatter(xs, ys, cv, cmapviridis, s4) plt.colorbar(sc, labellinear speed / m/s) plt.xlabel(x / m) plt.ylabel(y / m) plt.axis(equal) plt.grid(True) plt.show()调试时这个图特别好用。比如匀速运动中颜色分布应该是一条均匀的渐变带如果轨迹中间某段颜色明显变深或变浅说明那里机器人突然加速或者减速了。还有一次我发现轨迹颜色出现了很规律的深浅交替后来定位到是电机控制周期和里程计采集周期之间存在周期性耦合这种规律性问题单纯看线轨迹完全发现不了。6.2 用箭头绘制偏航角展现姿态变化位置轨迹图只反映“走到哪”偏航角反映“朝向哪里”这两个信息在很多底盘调试中要合起来看。比如在狭窄通道里掉头轨迹可能显示路径很平滑但如果偏航角变化不平滑机器人可能是靠原地转圈加平移凑出来的路径。用quiver可以在轨迹上叠加方向箭头。每 N 个点画一个箭头方向根据 yaw 值生成def draw_with_heading(xs, ys, yaws, stride10): plt.figure(figsize(8, 8)) plt.plot(xs, ys, -, linewidth1.2, alpha0.6) # 每隔 stride 个点取一个位置画航向箭头 qx xs[::stride] qy ys[::stride] # 将偏航角转换为箭头向量 dx [math.cos(y) * 0.1 for y in yaws[::stride]] dy [math.sin(y) * 0.1 for y in yaws[::stride]] plt.quiver(qx, qy, dx, dy, anglesxy, scale_unitsxy, scale1, width0.005) ...箭头长度可以按实际速度缩放也可以统一长度只看方向。我一般调参时用统一长度分析行驶品质时按速度缩放。加上航向箭头以后机器人是否倒着走、是否原地旋转一眼就能区分。6.3 多轨迹对比把 /map、/odom、/odom_raw 放在同一张图里单条轨迹再怎么画也只是“一条线”多条轨迹放在一起对比价值才会真正出来。我常用的对比组合是/odom和/map的轨迹对比以及底盘原始里程计和融合后里程计的对比。/map轨迹的获取方式和/odom本质相同但要注意坐标系不同直接画在一起需要把两套数据都转换到同一个参考坐标系下。如果你的系统有robot_pose_ekf或robot_localization通常它们发布的/odom已经做了坐标变换可以和底盘直接发布的原始里程计放在同一张图上对比累积误差的大小。这个图形往往是关键证据。有一次调试定位系统纯看位置数据两个话题都挺平滑但叠加画图以后发现融合后的轨迹在转弯处有明显的滞后弧线原始里程计轨迹反而更贴实际路径。顺着这条线索查下去才发现融合节点的线性加速度输入量过大导致转弯时位置被过度修正。这种对比结论靠单独分析任何一条轨迹都很难得到。最后再分享一个小技巧。脚本里顺手把数据存成 CSV每行包含t, x, y, yaw, v几十行代码的事但留下来以后团队里其他人要做定量分析就不用再重新跑机器人。我已经习惯了每次调试都留一份数据一份图遇到问题回翻记录经常能发现当时没注意到的细节。