★ FEATURED ARTICLE 简介面向自动驾驶规划与控制场景的完整工程基于C与carla-ros-bridge构建覆盖从仿真环境搭建、消息定义到规划控制算法实现的全过程适合毕业设计、课程设计及项目二次开发面向车辆工程、计算机、自动化方向学生与工程师。源码按路点消息、规划、控制等模块划分各模块均配有独立启动文件、可视化文件和参数文件可在ROS环境下直接加载运行便于分模块学习与剪裁复用。压缩包共60个文件大小31.71MB类型涵盖C核心源码、启动与参数配置、演示动图、开发文档、工作空间配置和快捷部署脚本结构清晰无冗余。已有404人学习。包内还提供算法示意图与说明文档配合快速安装脚本和工作空间配置可快速搭好完整开发环境源码经严格测试规划与控制逻辑清晰便于在此基础上延展、修改、验证或继续研究。1. 在CARLA里用C跑通planning与control一份毕业设计向的源码完整拆解先说结论这份资源是一份面向毕业设计和课程设计的自动驾驶联合仿真源码包工程入口是 CARLA 模拟器 carla-ros-bridge C 节点业务功能覆盖了规划planning与控制control两大块并且自带开发文档、launch 启动文件、RViz 可视化配置和快速环境安装脚本。也就是说你不需要自己从零搭 ROS 工作空间也不需要拿 Python 脚本拼一个不具备工程结构的 demo而是可以直接在 CARLA 里看到基于 C 写的规划器发布路径、控制器跟踪路径、车辆绕障碍最终停在目标点。适合 C 基础一般但毕设要求有深度、时间又紧的同学也适合想从“会跑 Python 算法”过渡到“能组织 C 工程”的从业人员。下面我把这份源码从工作空间、消息接口、规划器、控制器到避坑点逐层拆开讲。2. 工作空间与消息桥接rosinstall 一键搭建和 waypoint_msgs 接口设计2.1 ROS桥接与C节点架构为什么数据流向这样设计先理清楚整个链路的拓扑。CARLA 本身以服务端进程跑仿真里面有一辆 ego vehicle自车周围有交通车辆、行人、红绿灯。carla-ros-bridge 是一个 ROS 节点它把 CARLA 里的传感器数据、车辆状态、自车控制命令翻译成 ROS 话题反方向它也接收 ROS 里的控制指令把油门、刹车、转向角写回 CARLA 的 vehicle control 接口。这套方案的价值在于算法侧不直接调用 CARLA Python API而是通过 ROS 话题做通信。这套源码包里规划与控制的 C 节点位于桥接节点的下游。规划节点订阅三种消息车辆状态话题比如车速、位姿、加速度、当前路径目标/路由信息、以及感知目标列表障碍物的位置、速度、朝向。经过 EM 解算后发布一条带航向和曲率的路径点序列。控制节点再订阅这条路径结合车辆状态计算出期望转向角和期望速度通过桥接节点写入 CARLA。我一般会建议拿到源码后先用一条命令把话题列表打出来确认桥接状态rostopic list | grep -E carla|planning|control正常情况下你至少应该看到这三类话题/carla/ego_vehicle/vehicle_status车辆状态、/planning/path或等价的轨迹话题、/carla/ego_vehicle/vehicle_control_cmd控制命令话题。如果 planning 和 control 的话题迟迟不出现问题大概率出在节点启动顺序或者坐标系配置上后面的避坑章节会展开。参数说明grep -E是正则匹配把关键命名空间一次性筛出来rostopic list本身不消耗多少 CPU适合做链路健康检查。若你改了命名空间前缀把这个正则里的关键字同步换掉即可。2.2 quickly_setup.sh 与 workspace.rosinstall一键搭建背后配置了什么源码根目录下带scripts/workspace.rosinstall和scripts/quickly_setup.sh这是整个工作空间的初始化和依赖管理入口。很多同学拿到源码习惯直接catkin_make结果报缺这个缺那个。正确顺序是先跑脚本。workspace.rosinstall是 ROS 的依赖清单文件里面会列出 carla-ros-bridge 主包、消息依赖和可选的自定义包常见格式大致是这样的以实际内容为准- other: local-name: carla_ros_bridge uri: https://github.com/carla-simulator/ros-bridge.git version: master - other: local-name: waypoint_msgs uri: file://src/waypoint_msgs第一项从远端拉取桥接包第二项导入了源码自带的自定义消息包。local-name指定包在工作空间里的落点uri决定拉取地址version锁定分支。如果你有内网镜像需求直接改uri即可。quickly_setup.sh通常做的事情包括检测当前 ROS 版本、创建 catkin 工作空间、调用wstool merge合并 rosinstall、再执行rosdep install补系统依赖、最后catkin_make。我建议在跑之前先逐行查看脚本内容确认它执行了哪几步常见做法是加set -e保证任一步失败直接退出避免带着残缺依赖继续编译。一个很重要的细节catkin_make时如果报waypoint_msgs找不到多半是因为消息包没有先编译。你需要确保自定义消息包排在依赖链最前面我自己会把waypoint_msgs单独先catkin_make一次再编译整体这是个非常保险的步骤。2.3 waypoint_msgs 消息包设计规划与控制之间的接口契约waypoint_msgs是这套源码里的自定义消息包定义的是规划器输出给控制器的轨迹点格式。I才能用消息源自定义生成头文件C 节点里才能#include waypoint_msgs/WaypointArray.h。一个典型的消息定义会包含坐标、航向、曲率和目标速度例如Waypoint.msg# waypoint_msgs/Waypoint.msg float32 x # 世界坐标 X float32 y # 世界坐标 Y float32 z # 世界坐标 Z float32 heading # 期望航向角 float32 curvature # 期望曲率 float32 target_speed # 期望纵向速度 m/s bool is_final # 是否为路径终点逻辑说明x、y、z给控制器提供空间位置基准heading用于航向偏差计算curvature是控制器做前馈补偿的关键输入高速过弯时没有曲率前馈纯靠反馈会明显滞后target_speed让控制器根据规划结果提前减速is_final帮助控制器判断何时停车。再定义一个WaypointArray.msg来装整条路径# waypoint_msgs/WaypointArray.msg std_msgs/Header header waypoint_msgs/Waypoint[] waypointsheader里面带时间戳控制器收到消息后可以用它判断路径是否过期。如果控制端发现最新的路径时间戳已经是很久之前的就应该停下来而不是继续跟踪旧路径这一点在实时系统中很常用也在避坑章节里会再次提到。设计接口时建议做三件事一是坐标系字段要标明是世界系还是车身系避免控制器把局部坐标当真值二是单位统一用国际制三是消息里最重要的两个字段curvature和target_speed不能省少了它们控制器的跟踪精度会明显下降。3. Planning 模块复现EM 规划器如何生成一条可跟踪的路径3.1 EM 规划器拆解横向选路与纵向调速为什么分开做这套源码里的规划模块核心是 EM 系规划器就是你在figure目录里的 EM1.gif 和 EM2.gif 中看到的那个算法路径可视化。EM 的思路是把原本复杂的规划问题拆成两个子问题先在“横向维度”上选出一条几何路径再在“纵向维度”上为这条路径分配速度曲线。横向维度上规划器在 Frenet 坐标系里以参考线为中心向左向右撒若干候选轨迹。每一条候选轨迹会用一个代价函数评估代价项通常包括偏离参考线的程度、曲率的平滑度、与障碍物的距离、以及边界约束。得分最高的那一条作为当前周期的路径解。纵向维度上对选出的路径做速度规划。环境中的动态障碍物和静态障碍物会被投影到这条路径的纵向坐标上形成所谓的 S-T 图。规划器在 S-T 图中搜索一条代价最小、同时满足车辆加加速度限制的速度曲线最终生成一条带目标速度的路径点序列。这里给个极其容易误解的点EM 这个名字听起来像概率里的期望最大化但早期的自动驾驶规划框架里它就是借用了“先粗估再迭代细化”的思想并不是在做统计学里的 EM 推断。拿到代码不要去源码里搜索 “E-step”“M-step” 这种关键词会找不到的。真正核心的是采样函数和代价函数。3.2 源码主循环从车辆状态订阅到路径点发布打开 planning 包里的src目录核心文件一般是一个继承自ros::NodeHandle的规划节点类。主循环做的事情是监听当前车辆位姿、目标路由、障碍物列表然后调用 EM 更新函数。// planning/src/planning_node.cpp 片段 #include ros/ros.h #include waypoint_msgs/WaypointArray.h #include nav_msgs/Odometry.h class EMPlanner { public: EMPlanner(ros::NodeHandle nh) { sub_odom_ nh.subscribe(/carla/ego_vehicle/odometry, 1, EMPlanner::OdomCallback, this); pub_path_ nh.advertisewaypoint_msgs::WaypointArray(/planning/path, 1); } private: void OdomCallback(const nav_msgs::Odometry msg) { // 用当前位姿作为规划起点 double x msg.pose.pose.position.x; double y msg.pose.pose.position.y; // 调用 EM 规划主函数生成路径 waypoint_msgs::WaypointArray path GeneratePath(x, y); path.header.stamp ros::Time::now(); pub_path_.publish(path); } waypoint_msgs::WaypointArray GeneratePath(double x, double y) { waypoint_msgs::WaypointArray path; // 1. 横向采样生成候选路径 // 2. 评估代价并选出最优路径 // 3. 纵向 S-T 图求解速度曲线 return path; } };逻辑说明odometry回调每来一帧 pose 就触发一次规划这里用的是事件驱动方式比固定频率定时器更能与仿真时钟对齐。GeneratePath是规划主函数里面三个步骤对应 EM 的三个阶段。waypoint_msgs::WaypointArray是上一步定义好的路径消息类型发布后 controller 节点就能订阅。参数说明订阅队列深度设成 1只保留最新一帧避免规划端堆积陈旧位姿导致路径滞后。如果你在真机或高频率仿真中发现路径抖动可以改成 2 或 3但不要设太大否则规划周期会不稳定。/planning/path话题名建议不要在多个节点里重复定义RViz 添加话题时按这个名字过滤就行。3.3 参数调节与 RViz 可视化验证规划器能否输出可跟踪的路径很大程度取决于 Freme t 采样参数。源码 config 目录里通常有一个 yaml 文件管理这些参数常见的有参数名含义常见取值区间lateral_step横向采样步长0.3 ~ 1.0 mlateral_range横向最大偏移3.0 ~ 8.0 mlongitudinal_interval路径点纵向间隔1.0 ~ 3.0 mobstacle_clearance障碍物最小距离0.5 ~ 1.2 mmax_curvature单点最大曲率限制0.1 ~ 0.3如果车辆频繁出现“找不到路径”的问题优先检查lateral_step是不是太小了步长太小导致采样空间过大、计算超时反过来如果老是贴着障碍物走检查obstacle_clearance是否设得过小。RViz 验证是判断规划器是否正常的黄金标准。源码包里带着.rviz文件启动后添加两个显示组件一个是Path显示/planning/path用来观察规划路径的形态另一个是MarkerArray用来查看候选路径和障碍物采样的可视化。如果你在 RViz 里看到规划路径在每帧之间剧烈跳变一般是参考线切换时帧不一致导致的重点检查fixed frame是否设置成了 world 系。rosrun rviz rviz -d src/carla_ros_bridge_pnc/rviz/sample.rviz参数说明-d指定 rviz 配置文件避免每次手动添加话题。这个指令要在编译结束后执行并且在启动规划节点之后运行时间太早会导致 RViz 里看不到话题。4. Control 模块落地LQR 控制器的跟踪效果与参数调试4.1 控制选型的理由为什么这套代码用 LQR 而不是 PID做路径跟踪最常被拿来比较的两个控制器是 PID 和 LQR。PID 的优点是调参直观但它是基于误差标量做的比例、积分、微分运算处理不了横向误差和航向误差之间的耦合关系。高速过弯时PID 往往会表现为转向不足或过度修正抖动。LQR 是在状态空间模型上求解最优反馈增益的控制器。对车辆路径跟踪问题状态向量定义成 [横向偏差, 航向偏差, 横向偏差变化率, 航向偏差变化率]控制器基于线性化的车辆动力学模型求解一个使状态平方和与控制量平方和加权最小的增益矩阵 K。这个 K 矩阵会把横向偏差和航向偏差同时映射到方向盘转角天然解决了两个通道的耦合问题。在毕设答辩场景下选 LQR 还有一个非常实际的优势你能自然的把“动力学模型建得对不对”“权重矩阵 Q、R 怎么选”“闭环极点落在哪”讲成一条完整的技术线。这些内容是评分点所在比“我调了 PID 三个参数”有深度得多。4.2 源码结构控制器输入、输出与状态量计算Controller 包里的核心类是车辆横向控制器主要处理流程是订阅规划路径、找到最近点、计算偏差、套用 LQR 公式、输出方向盘转角和控制速度。// controller/src/lqr_controller.cpp 片段 #include waypoint_msgs/WaypointArray.h #include Eigen/Dense void LQRController::CalculateControl(const waypoint_msgs::WaypointArray path, const double x, const double y, const double heading, double steer) { // 找距离车辆最近的路径点索引 int closest_idx FindClosestPoint(path, x, y); const auto ref path.waypoints[closest_idx]; // 计算横向偏差与航向偏差 double offset_y (x - ref.x) * sin(ref.heading) - (y - ref.y) * cos(ref.heading); double heading_err ref.heading - heading; // 构造状态向量 Eigen::VectorXd state(4); state offset_y, heading_err, 0.0, 0.0; // LQR 反馈增益矩阵 K由一个独立的 compute_K 函数离线算出 Eigen::MatrixXd K LoadGainMatrix(lqr_gain.csv); Eigen::VectorXd u -K * state; steer u(0); }逻辑说明FindClosestPoint是路径跟踪最基础的一步它用最小欧几里得距离在整条路径里找最近点offset_y是横向偏差用参考点航向做了坐标旋转得到的是车身系下的横向偏差。heading_err是航向偏差状态向量里的后两个分量在这里简化成 0但实际工程中会通过轮速差或 IMU 的 yaw rate 估算能明显提升过弯效果。参数说明lqr_gain.csv是增益矩阵的存放文件由离线脚本根据车辆动力学参数算出来运行时直接读取。这样做的好处是模型参数变动时不需要重新编译代码只需要重新算矩阵。我建议控制频率不低于 20 Hz不然高速段跟踪效果会打折扣。4.3 LQR 调参与 GIF 输出对比源码的figure目录里放着 LQR1.gif、LQR2.gif 等动画它们展示了不同参数下的跟踪效果对比。这也是这套资源非常方便的一点作者把实验动画导出成了 GIF你不需要自己跑一遍仿真就能直观看到横向偏差的收敛差异。LQR 需要调的不是三个 PID 系数而是状态加权矩阵 Q 和控制量加权矩阵 R。Q 越大表示越看重状态误差收敛R 越大表示越不希望输出太大的控制量。我把常见现象和调整方向列成一个速查表调试现象Q 矩阵调整R 矩阵调整横向偏差收敛太慢增大 Q(0,0)保持不变或略降方向盘抖动剧烈保持不变或略降增大 R弯道内侧偏移明显增大 Q(1,1) 航向偏差项保持不变直道小幅振荡增大 Q(2,2) 偏差变化率项增大 R我第一次调这套代码时只加大 Q 的横向偏差项结果是直道没问题、弯道直接冲进对向车道原因就是航向偏差项的权重没跟上。LQR 必须同时考虑横向偏差和航向偏差两个权重之比决定车辆“贴近路径”与“对准航向”之间的优先级。5. 避坑专题从环境搭建到闭环跑通最常踩的四个坑5.1 CARLA 与桥接版本不一致桥节点启动即退现象运行roslaunch carla_ros_bridge carla_ros_bridge.launch后节点在几秒内自动退出日志末尾出现Segmentation fault或Connection refused。原因CARLA 仿真器版本和 carla-ros-bridge 的编译版本不匹配这是最常见的启动崩溃原因。比如 CARLA 是 0.9.11桥接代码却按照 0.9.13 的接口生成两者通信协议不一致就会在握手阶段挂掉。解决查看 CARLA 启动时终端输出的版本号和scripts/workspace.rosinstall里version字段做比对确保一致。如果不一致最简单的方式是把 CARLA 版本切到与桥接锁定的版本。没有必要自己改源码适配那样浪费时间且容易引入新问题。5.2 规划器不输出路径三个排查顺序现象控制节点报错“waiting for /planning/path”RViz 里没有任何路径显示。原因这种问题通常不是规划算法挂了而是上游输入缺失。规划器需要 odometry、路由信息和障碍物信息三种输入缺任何一样它都会停在等待状态。解决按以下顺序排查。先看 odometry 是否有输出rostopic hz /carla/ego_vehicle/odometry如果频率是 0 说明桥接与仿真器的连接出了问题。再查路由信息是否发布有些版本桥接需要你先在 CARLA 里设置自动驾驶目标点没设置时路由话题永远不发布。最后检查障碍物列表如果感知话题空缺规划器可能因为前方所有采样点都被视为有障碍而拒绝输出路径。5.3 LQR 横向震荡、瞬时甩尾控制频率与模型参数不匹配现象车辆在直道上走“S”形过弯时方向盘打到头车辆明显横摆。原因一方面可能是 LQR 状态模型里的轴距、质量参数与实际车辆不一致导致增益矩阵 K 计算偏差另一方面是控制频率太低每两次控制之间车辆已经跑出去很远误差积累后被 K 矩阵放大。解决先检查lqr_gain.csv是不是按当前车辆参数生成的轴距参数差 10 cm 就会在 20 m/s 车速下产生明显航向偏差。再把控制循环从 20 Hz 提到 50 Hz同一套 K 矩阵在高频控制下稳定性会显著改善。提频后如果转向指令依然抖考虑给控制输出加一个一阶低通滤波。5.4 仿真时间与真实时间不同步导致的“假超时”现象仿真里车辆不动但 ROS 端控制节点还在发指令RViz 中的路径也不更新。原因CARLA 默认用仿真时间推进而 roslaunch 如果没有指定use_sim_timeROS 节点用的是真实时间。两个时间戳对不上控制节点的时间同步检查就会认为所有消息都过期。解决在 launch 文件里加上param name/use_sim_time valuetrue/并确保 bridge 节点与规划控制节点都在同一个 ROS master 下运行。如果情况出现在没有 launch 文件的时候手动运行rosparam set /use_sim_time true6. 毕业设计验收场景指标定义、跨场景调参与日志回放6.1 验收指标怎么定义才经得起答辩毕业设计不能只展示“能跑”。我建议把横向偏差均值、最大值纵向速度误差以及原路径跟踪完成率作为三个核心指标记录到答辩材料里。方法是在控制器代码里加一段统计逻辑rostopic echo -n 200 /planning/control_debug这段命令能拿到控制器输出的调试数据帧里面通常包含当前横向偏差、航向偏差、目标速度。把数据存成 CSV 后用脚本计算均值和标准差画成曲线放进论文比口头描述效果好得多。6.2 跨场景调参换地图后必调的两个参数换一张地图或者换一个起点最常见的两个“惊喜”是横向采样范围不够和 LQR 增益过强。我习惯的做法是先在 RViz 里看规划器生成的候选路径有没有覆盖到目标车道如果覆盖不到把lateral_range从 3.0 提到 6.0同时把lateral_step从 0.5 提到 0.8否则采样点数爆炸导致规划周期变成龟速。6.3 数据回放与日志留存调参过程中最容易犯的错是改了参数就跑新实验回头看数据时已经不记得哪组参数对应哪组结果。我后来强制自己每次实验前记录参数 hash跑完后立刻导出 CSV把参数文件、日志和截图放进同一个文件夹。这个习惯保住了我无数个原本要重跑实验的下午。如果你也是做毕设建议第一周不要急着调算法先把环境和话题链路跑通把数据采集和回放机制搭好。从那以后我每次做仿真实验都强制走一遍“记录参数 - 跑通场景 - 导出数据 - 对比曲线”的流程基本没有翻车。希望这套源码和这篇拆解能帮到你至少在毕业设计这条路上少踩几个坑。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
