做自动驾驶这套东西最容易被人低估的环节不是感知也不是控制而是路径点。前两篇日记里我把 Autoware 的环境、地图、定位都折腾通了真正卡住我的却是“车该沿着哪条线走”这件事——地图给的是静态环境ROSBAG 给的是动态数据中间那条把两者串起来的纽带就是一份看起来平平无奇的 CSV 路径点文件。这篇就聊聊我是怎么用 Autoware 自带的工具从一段录好的 ROSBAG 里把车辆轨迹“榨”成可用路径点的中间踩过的坑一个不落地写出来包括 yaw 弧度陷阱、速度字段全零、bag 里缺 TF 导致工具静默不输出这些让人抓狂的问题。如果你也在 Autoware 1.x 这套体系里折腾或者只是好奇一份路径点文件到底怎么来的这篇应该能帮你少熬两个晚上。1. 路径点在 Autoware 里到底算个什么东西1.1 它不是“地图”但比地图更贴近车的行为刚开始接触的时候我一直有个误解觉得路径点等于地图的一部分。实际上完全不是。Autoware 里的地图vector map 也好点云地图也好描述的是环境车道线在哪、路口在哪、限速牌在哪。而路径点描述的是车的意图从 A 点到 B 点车打算沿着哪条折线走每个点上大概跑多快哪里要停一下。这两者的关系有点像导航软件里的“路网数据”和“你这次要走的路线”。路网是公共的、全量的路线是你这次私有的、带偏好的。路径点就是 Autoware 眼中那条“你这次要走的路”。所以路径点文件里带的信息很有意思x, y, z是位置yaw是车头朝向velocity是这个点的期望速度change_flag一般用来标记换挡点或者停车点。你看位置和朝向是“空间”速度和标志位是“行为”。这也解释了为什么路径点质量直接决定车跑起来是丝滑还是抽搐——它不只是几何它还是决策的输入。我见过有人把路径点当成纯几何折线来理解随便拿地图上的车道中心线导出一份 CSV 就往车里灌结果车在弯道里画龙。原因很简单车道中心线的点间距可能是 5 米一个曲率变化根本体现不出来而且速度字段要么没有要么是常量规划模块拿到的是一份“没有意图”的几何数据自然跑不出人的驾驶感。1.2 三条造点路线为什么我最后选了“从 ROSBAG 里榨”在实际操作里造路径点大致有三条路可走我先把它们的取舍摆出来后面的操作你就知道为什么我要这么绕。路线做法优点硬伤手动打点在 RViz 里用工具一个个点门槛最低改起来直观点间距靠手感长路线极其费时间弯道容易画歪地图导出从矢量地图里按车道中心线抽点一次能生成很长一段几何规整缺少真实驾驶行为速度曲线得另外编路口处理生硬ROSBAG 录制人工驾驶或遥控跑一遍录下位姿再抽点轨迹天然带人类驾驶的平滑性和真实速度依赖定位质量录制过程有讲究bag 处理需要额外一步我选第三条的根本原因是人的驾驶轨迹本身就是一份高质量的“参考解”。你让一个熟练的人开一段路他自然会在进弯前减速、出弯时提前给油、路口前松油门滑行这些行为模式如果你靠手工编速度曲线几乎不可能编得像。而 ROSBAG 记录的正好是这套行为在时空上的投影。代价也是真实的。第一定位必须稳map 到 base_link 的变换一旦跳变抽出来的点就是一团麻花第二录制的时候得有点“表演意识”不能猛地急刹急加速否则轨迹抽稀之后全是噪声第三bag 处理这一步需要你懂一点 ROS 的回放机制不然工具跑起来没反应你还以为是编译问题。提示如果你手上一时没有实车条件仿真环境里跑一段带定位的录制同样有效。关键不在于车是真车还是虚拟车而在于map→base_link 这条 TF 链路在那个 bag 里是连续且可信的。1.3 先把工具链的角色分清楚在 Autoware 1.x 这套体系里和路径点相关的工具主要是三个名字很像但分工完全不一样初学者极容易搞混waypoint_loader负责读。把 CSV 读进来发布成路径话题同时把速度信息一起发布出去。它是消费端。waypoint_saver负责写。订阅实时的位姿话题按一定间隔把位置、朝向、速度存成 CSV。它是生产端。waypoint_maker负责编。在 RViz 里提供交互可以手动加点、拖点、改速度。它是编辑器。我一开始就是没分清这三个以为waypoint_saver能直接吃 bag 文件。实际上它订阅的是实时的位姿话题所以正确姿势是先把 bag 回放起来让位姿话题“活”过来然后waypoint_saver在旁边安静地把流过的位姿记下来。这个“回放 旁听”的模式是整件事的核心机制理解了这个后面所有操作都顺了。2. 录制之前先把“位姿可信”这件事坐实2.1 map→base_link 的连续性比精度更重要很多人纠结定位精度是 5 厘米还是 20 厘米但在生成路径点这个场景下连续性比绝对精度重要得多。原因很直白路径点最终是一串点如果相邻两帧的位姿突然跳了半米那抽出来的点就会在局部形成一个诡异的折角车跑到那里会突然打方向。所以录制前我会先做一件事把车停在地图里的起点打开 RViz盯着map坐标系下的车模型看 30 秒。判断标准很朴素——车模型在 RViz 里应该是静止的或者只有极其轻微的抖动。如果你看到它每隔几秒轻微“呼吸”一下或者方向角在缓慢漂移那就说明定位还没收敛好这时候录出来的 bag 全是垃圾。还有一个更隐蔽的问题定位模块在启动初期往往会有一段“从粗到精”的收敛过程。我踩过一次坑车刚启动就开录前 20 秒的位姿是飘的结果生成的路径开头有十几米的歪线。后来我养成了习惯——先原地静止 10 秒再起步这 10 秒在后续处理时直接裁掉宁可多录不要少录。2.2 一份够用但不臃肿的话题清单录制 bag 最容易犯的错是“全都录”。rosbag record -a一时爽回头 bag 文件几个 GB回放的时候硬盘跟不上位姿话题掉帧抽出来的点间距忽大忽小。我的做法是精确指定话题。以 Autoware 1.x 的常见话题命名为例核心是这几类rosbag record -O waypoint_raw.bag \ /tf \ /tf_static \ /current_pose \ /vehicle_status \ /gnss/fix \ /gnss/pose_with_covariance逐个说一下为什么/tf和/tf_static是必须的。waypoint_saver内部要做坐标变换如果 bag 里只有位姿没有 TF 树工具会一直等变换表现就是“启动了但什么也不输出”。这是最常见的静默失败。/current_pose是 Autoware 里常用的当前位姿话题geometry_msgs/PoseStamped类型。有些配置下它由定位模块发布有些配置下需要你自己用 TF 转出来。/vehicle_status或者底盘反馈话题用来记录真实车速。如果你打算用“按里程间隔抽点 用真实车速填 velocity 字段”这套流程这个必须有。GNSS 相关的原始话题可以作为备份万一后面想用 GNSS 轨迹做交叉验证。注意话题名在不同分支和不同车辆配置下差异很大。不要照抄先用rostopic list确认你车上实际在发什么再决定录哪些。我见过有人照着别人的命令录结果录了一堆不存在的空话题关键话题反而漏了。2.3 驾驶动作本身就是数据质量的一部分这段听起来有点玄但是真的。同样的路线不同的人开抽出来的路径点质量能差出一个档次。我总结了几个录制时的原则第一速度要稳。别一脚油门一脚刹车匀速巡航为主速度控制在你能从容处理弯道的水平。原因在于waypoint_saver是按固定间隔时间或距离抽点的如果速度剧烈变化抽出来的点在空间上就是疏密不均的——直道上稀稀拉拉急减速区挤成一团。第二弯道提前减速但方向要一次到位。人的自然反应是边转边调但这对路径点不友好会让弯道里的点呈现出锯齿状的横向摆动。稍微刻意一点把弯走圆。第三路口和停止点要“停顿一下”。如果你希望生成的路径里有明确的停车点就在该停的位置真正停稳一到两秒。这样抽点的时候这个位置会有多个几乎重合的点后处理时很容易识别成停车段change_flag也就有了依据。第四跑完别急着关。到终点后再静置几秒让轨迹有个明确的收尾。我把这套原则叫做“为数据而驾驶”。听起来有点反人类但做过一次你就会发现录制时多花的那点心思在后处理阶段能省掉大量返工。3. 用 waypoint_saver 把一段 bag 榨成 CSV3.1 它的工作机制其实很朴素waypoint_saver的逻辑用一句话概括订阅位姿攒够一个间隔就写一行。它内部维护一个计时器或者距离累计器每当满足间隔条件就把当前位姿的 x/y/z/yaw 写下来同时把速度也填进去。间隔到了就写不管你有没有在动——这一点很关键意味着如果你在录制时停车等待它照样会写出一堆重复点。它的核心参数一般是这几项不同分支命名可能略有差异以你本地 launch 文件里的实际字段为准waypoint_saver: save_dir: /home/user/waypoints/ # 输出目录文件自动命名 interval: 1.0 # 抽点间隔单位取决于实现秒或米 velocity: 5.0 # 位姿话题里没有速度时使用的默认速度 accel_limit: 1.0 decel_limit: 1.0这里有两个坑值得单独说。第一个坑是interval的单位。不同实现里它可能是秒也可能是米。如果是秒1.0 意味着每 1 秒写一个点——你开 30 km/h 的话点间距就是 8 米多对于弯道来说太粗了。如果是米1.0 意味着每 1 米一个点这个密度对城市道路比较合适但文件会很大。务必先确认单位再调否则你调了半天参数发现方向反了。第二个坑是velocity字段的来源。如果位姿话题里不带速度信息工具会退化成用这个固定的velocity值填所有点。这就是为什么很多人生成的 CSV 里速度全是同一个数——不是工具坏了是它没拿到真实速度。想要真实速度要么位姿话题里带 twist 信息要么后处理时用/vehicle_status的时间戳对齐补上。3.2 离线回放的完整操作顺序我现在的标准流程是这样的顺序不能乱第一步单独起一个终端把 Autoware 的运行时框架拉起来确保 TF 树是完好的。如果只是离线处理不需要启动感知、控制这些模块但定位和地图相关的部分要起来因为waypoint_saver做坐标变换时依赖map帧存在。第二步把 bag 用暂停模式起播rosbag play --pause waypoint_raw.bag加--pause是为了给自己留出准备时间。我曾经犯过一个低级错误——直接rosbag play然后手忙脚乱地去启动waypoint_saver结果最精彩的那段起点被跳过了白录。第三步检查话题是否正常rostopic hz /current_pose看到频率稳定输出说明位姿流是通的。这一步千万别省我至少三次是因为位姿话题其实没发出来白等半天。第四步启动waypoint_saver然后在播放终端按空格继续播放。第五步盯着输出目录确认文件在增长。看到文件大小在变心里就踏实了。第六步bag 播完后等几秒再关waypoint_saver让它把缓冲区写完。提示回放速度可以用-r参数调慢。如果你的机器性能一般用-r 0.5半速回放位姿话题的连续性会好很多抽出来的点也更均匀。这个技巧在处理大 bag 时特别有用。3.3 生成结果的第一眼检查文件生成出来别急着用先在终端里瞄一眼head -5 waypoint_20240101_120000.csv wc -l waypoint_20240101_120000.csv正常情况下应该是每行 6 列有些版本是 7 列多一个转向字段。用wc -l看行数心里有个估算如果你跑了一公里点间距 1 米那大概一千行左右。如果只有几十行说明抽点间隔太大或者中途工具挂了如果有几十万行说明间隔太小或者你在原地停了很久。我还习惯用一个小脚本快速画一下轨迹肉眼判断形状对不对import csv import matplotlib.pyplot as plt xs, ys [], [] with open(waypoint.csv) as f: for row in csv.reader(f): xs.append(float(row[0])) ys.append(float(row[1])) plt.plot(xs, ys, -o, markersize1) plt.axis(equal) plt.show()图一出来什么问题都藏不住轨迹是不是一段完整的路线、有没有突然的折返、有没有一根莫名其妙的直线拉到远处。我见过最离谱的一次是轨迹里出现一条笔直跨越半个地图的线排查半天发现是 bag 中途断开重连位姿从终点跳回了起点。4. CSV 字段里的三个隐藏地雷4.1 yaw 单位弧度还是角度这是个哲学问题waypoint_saver输出的 yaw 通常是弧度范围在 -π 到 π 之间。但很多人脑子里想的是角度看到1.57会觉得“这车头朝向才 1.57 度”实际上那是 90 度。这个坑的真正危害不在读的时候而在你手工编辑文件的时候。我有一次想微调某个停车点的朝向凭直觉填了个90结果车到位之后原地转了 20 多圈——因为 90 弧度相当于转了 14 圈多。更隐蔽的问题是角度回绕。当车头朝向从接近 π 转到接近 -π 时数值会从 3.14 直接跳到 -3.14。你自己写脚本做平滑滤波的时候如果直接对 yaw 序列做滑动平均这个跳变会污染整段数据导致某个点的朝向变成 0 附近车在那里会突然摆正。处理办法是做角度展开unwrap把跳变拉平import numpy as np yaws np.unwrap(np.array(yaws))numpy.unwrap会自动处理 ±π 的跳变让序列连续。这一行代码救过我无数次。4.2 velocity 字段全 0 和全 30 都是一场灾难速度字段是最容易被忽略、又最容易出事的地方。如果全是 0规划模块拿到的是一条“要求车一直停着”的路径表现就是规划失败或者车原地不动。如果全是同一个比较大的值车在弯道里不会减速你会看到一个非常刺激的画面。我的做法是后处理时按曲率重新生成速度曲线而不是信任录制时抽出来的原始速度。原因在于录制时的速度包含了人的判断但抽点后的点序列已经变了原来那个速度对应的空间位置可能已经不精确了而且抽稀之后点变少了速度曲线会变得阶梯化。重算速度的逻辑其实不复杂核心思想是“曲率大的地方限速低”import numpy as np def curvature(x, y, z, i): # 用前后各两个点估算局部曲率 p0 np.array([x[max(i-2, 0)], y[max(i-2, 0)]]) p1 np.array([x[i], y[i]]) p2 np.array([x[min(i2, len(x)-1)], y[min(i2, len(y)-1)]]) a np.linalg.norm(p1 - p0) b np.linalg.norm(p2 - p1) c np.linalg.norm(p2 - p0) if a * b * c 0: return 0.0 s (a b c) / 2 area max(s * (s-a) * (s-b) * (s-c), 0) ** 0.5 return 4 * area / (a * b * c) v_max 8.0 # 直道最高速m/s k_limit 0.15 # 曲率阈值 v [] for i in range(len(x)): k curvature(x, y, z, i) v.append(min(v_max, v_max * k_limit / max(k, 1e-6)))这段代码不完美但对于城市道路这种曲率不极端的场景够用了。生成完记得做一次前向和后向的加速度约束避免速度突变否则车的纵向控制会很难受。4.3 change_flag不写也能跑写对了才好用change_flag这个字段很多人直接全填 0确实也能跑。但它其实承担着一个挺重要的职责标记行为切换点。常见的约定是0 表示正常行驶1 表示这个点需要停车或者换挡。比如你在一条路线的终点标记一个 1规划和控制模块就知道这里要减速停止而不是傻乎乎地冲过去。我一般的做法是起点附近标一个 1确保车辆从静止状态开始终点标一个 1中间如果有明确的停车点比如某个路口需要等待也标 1。其余全 0。这里有个细节标 1 的位置要留提前量。如果你的停车点是在路径的最后一行车很可能来不及减速就冲过头了。我通常会在目标停车点前面 10 到 15 米的位置提前标一个减速意图让纵向控制有足够的距离。5. 把糙点修成能跑的路径后处理实操5.1 抽稀和重采样先把密度调舒服waypoint_saver抽出来的点密度往往不均匀。直道上因为速度快点间距大转弯和减速时因为速度慢点挤在一起。这种不均匀对下游的规划算法不友好。我做两步处理。第一步是按固定距离重采样把原始折线当成一条连续的曲线每隔固定的弧长取一个点比如 1 米取一个。这样输出就是等间距的后续所有计算都简单了。import numpy as np def resample(x, y, z, yaw, step1.0): pts np.column_stack([x, y, z]) seg np.linalg.norm(np.diff(pts, axis0), axis1) s np.concatenate([[0], np.cumsum(seg)]) total s[-1] s_new np.arange(0, total, step) x_new np.interp(s_new, s, x) y_new np.interp(s_new, s, y) z_new np.interp(s_new, s, z) yaw_new np.interp(s_new, s, np.unwrap(yaw)) return x_new, y_new, z_new, yaw_new第二步是平滑。等间距之后用一个简单的移动平均或者高斯滤波去毛刺。但这里要小心平滑会削掉弯道的顶点让转弯变得“提前”和“滞后”。我的经验是窗口不要超过 3 个点宁可少平滑也不要过度。平滑之后还有一个必须做的动作重新计算 yaw。因为你动过点的位置了原来的朝向就不准了。正确做法是用相邻点连线的方向角重算yaw_new np.arctan2(np.gradient(y_new), np.gradient(x_new))这个细节很多人漏掉结果就是路径点位置改了、朝向还是旧的车沿着路径走的时候车头一直斜着。5.2 自交和回环人工检查绕不开等间距重采样有个副作用如果原始轨迹里有回头、绕圈或者倒车重采样之后可能出现路径自交或者重叠。可视化的时候一眼就能看出问题但很多人只做数值检查不画图。我的标准动作是把路径画成图叠加在点云地图的俯视图上肉眼看三件事第一路径是不是贴着你想要的车道走。有时候录制时车稍微偏了一点抽出来的路径就整体偏移了半米虽然能跑但不好看。第二有没有突然的折角。正常的轨迹应该是连续弯曲的出现尖角一定是数据有问题。第三起终点的位置对不对。我遇到过生成的路径开头多了几米“倒车”原因是录制时车先往后退了一点再前进。注意如果发现路径整体偏移不要用平移去“修”。整体平移会让路径和地图里的车道线、路口关系全部错位。正确做法是回到录制环节重新跑一遍或者用编辑工具逐点修正局部。5.3 一个实用的小检查清单在后处理完、准备灌进车里之前我会跑一份检查脚本输出几个关键指标检查项期望值异常说明点总数与路线长度/点间距吻合数量级不对说明抽点或重采样出问题相邻点间距集中在设定值附近方差小方差大说明重采样没生效或有断点相邻点朝向差小于 0.2 rad突变说明 yaw 有跳变或路径有折角速度范围在 0 到设定上限之间出现负数或超大值说明字段错位首末点标志位均为 1缺失会导致起停车行为异常这份清单看起来啰嗦但它帮我抓到过好几次“文件看起来正常、一跑就出问题”的隐蔽错误。最典型的一次是速度字段里混进了一个负数原因是某行数据少了一列导致整体错位肉眼完全看不出来。6. 加载回去跑一遍才算真正验证6.1 waypoint_loader 的启动与显示路径点文件做好了第一件事不是直接上车而是在 RViz 里加载看效果。启动waypoint_loader的时候要注意配置里的文件路径和文件格式waypoint_loader: lane_csv: /home/user/waypoints/final.csv velocity_max: 10.0启动后 RViz 里应该能看到一条彩色的路径线颜色的含义通常和速度相关——速度高的地方偏暖色低的地方偏冷色。如果整条线颜色一致说明速度字段没被正确读取大概率是列数不对或者分隔符问题。另外一个必须检查的点路径的朝向箭头。RViz 里显示的箭头方向应该和车实际行驶方向一致。如果箭头反了说明 yaw 的基准差了 180 度车加载后会朝着反方向规划。我踩过的一个坑是 CSV 用了中文逗号或者全角空格工具解析时跳过了一部分行却不报错表现就是路径中间缺了一段。保存文件时确认是纯 ASCII 的逗号分隔这个细节太容易被忽略。6.2 上车前必须做的三项确认第一项原地加载不报错。把车停在路径起点附近加载路径点观察规划模块有没有异常输出。如果规划一直失败先检查路径的起点是不是离车太远——很多规划器要求车辆初始位置和路径起点在某个距离阈值内。第二项低速跟一遍。用很低的速度比如 5 km/h让车沿着路径走一小段重点看两件事车是不是贴在路径上、转弯的时候方向盘动作是不是平滑。我一般只跑前 30 米够判断了。第三项检查停车点行为。把车开到靠近终点或者某个标记了change_flag的位置观察它是不是按预期减速。这一步最容易被跳过但恰恰是最容易出问题的——减速太晚冲过头、减速太早停在半路都是常见现象。提示如果车在路径上左右轻微摆动先别怀疑控制参数八成是路径点太密或者 yaw 抖动。把点间距从 1 米放宽到 2 米试试很多摆动会自然消失。这个反直觉的经验我验证过好几次。6.3 一套流程复用起来之后把这套流程走顺之后我现在的节奏是跑一趟录制大概 20 分钟回放抽点 5 分钟后处理脚本一键跑完 1 分钟RViz 检查 5 分钟整个过程半小时左右能出一条可用的路径。相比一开始手工在 RViz 里一个个点、一条两公里的路点了一下午效率完全是两个量级。更重要的变化是可复现性。手工打点这件事换个人来做结果完全不同而录制加脚本处理这套流程只要录制条件一致、脚本版本一致出来的结果基本一致。这对团队协作来说价值很大。7. 我在这个环节攒下的几点私人体会最后说几个不成体系但挺有用的经验。一是永远保留原始 bag。路径点文件出了问题第一反应应该是回到 bag 重新抽一次而不是在 CSV 上修修补补。bag 是唯一的真值来源CSV 是派生物把派生物当宝贝是舍本逐末。我的习惯是每段路线一个目录里面放原始 bag、抽点参数、后处理脚本和最终 CSV版本对得上才敢往车里灌。二是脚本比工具可靠。waypoint_saver这种工具受话题命名、TF 状态、参数单位影响太大稍微一个环境差异就静默失败。我后来干脆写了个小节点直接订阅位姿话题自己攒点逻辑简单到不可能出错。工具用不顺的时候自己写二十行代码往往更快。三是别小看静止段。录制时那些原地等待的几秒钟在后处理时是识别起终点的绝佳锚点——连续多个几乎重合的点一眼就能定位。所以我现在录制时会刻意在起点和终点各停三秒专门留给后处理用。四是速度曲线值得单独调。路径几何对了只是及格速度曲线才是决定乘坐体验的部分。我的做法是先按曲率生成一版然后实际跑的时候坐在车里感受哪里觉得“该慢没慢”或者“该走不走”就单独回去调那一段。这个调优过程没有捷径就是跑几遍的事。五是记录版本信息。CSV 文件我一般会在文件名里带日期同时在同目录放一个说明文件写清楚这段路径对应哪条路线、录制时用的是什么定位配置、后处理用了哪个版本的脚本。隔两周回来再看没有这些信息你根本分不清哪份是哪份。