企业官网建设流程全解析

我先说个真实经历。之前调试一台移动底盘，电机参数都调得没问题，但小车就是不走直线，给固定线速度以后总往右偏。改增益、换轮径、重新标定 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, '-', linewidth=1.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', dpi=150) 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', anonymous=True) # 订阅 /odom，回调只存数据 rospy.Subscriber('/odom', Odometry, self.odom_callback) # 初始化绘图 self.fig, self.ax = plt.subplots(figsize=(8, 8)) self.line, = self.ax.plot([], [], '-', linewidth=1.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, c=v, cmap='viridis', s=4) plt.colorbar(sc, label='linear 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, stride=10): plt.figure(figsize=(8, 8)) plt.plot(xs, ys, '-', linewidth=1.2, alpha=0.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, angles='xy', scale_units='xy', scale=1, width=0.005) ...

箭头长度可以按实际速度缩放，也可以统一长度只看方向。我一般调参时用统一长度，分析行驶品质时按速度缩放。加上航向箭头以后，机器人是否倒着走、是否原地旋转，一眼就能区分。

6.3 多轨迹对比：把 /map、/odom、/odom_raw 放在同一张图里

单条轨迹再怎么画也只是“一条线”，多条轨迹放在一起，对比价值才会真正出来。我常用的对比组合是/odom和/map的轨迹对比，以及底盘原始里程计和融合后里程计的对比。

/map轨迹的获取方式和/odom本质相同，但要注意坐标系不同，直接画在一起需要把两套数据都转换到同一个参考坐标系下。如果你的系统有robot_pose_ekf或robot_localization，通常它们发布的/odom已经做了坐标变换，可以和底盘直接发布的原始里程计放在同一张图上对比累积误差的大小。

这个图形往往是关键证据。有一次调试定位系统，纯看位置数据两个话题都挺平滑，但叠加画图以后发现融合后的轨迹在转弯处有明显的滞后弧线，原始里程计轨迹反而更贴实际路径。顺着这条线索查下去，才发现融合节点的线性加速度输入量过大，导致转弯时位置被过度修正。这种对比结论，靠单独分析任何一条轨迹都很难得到。

最后再分享一个小技巧。脚本里顺手把数据存成 CSV，每行包含t, x, y, yaw, v，几十行代码的事，但留下来以后，团队里其他人要做定量分析就不用再重新跑机器人。我已经习惯了每次调试都留一份数据一份图，遇到问题回翻记录，经常能发现当时没注意到的细节。