经过前期的努力《龙芯2k0300- 走马观碑组Gazebo仿真环境搭建(上)》现在Gazebo仿真环境已经跑通小车能动了摄像头也在发布图像。下一步就是让小车能“看见”赛道并自主巡线。本文主要介绍car_ws工作空间中的仿真小车模型、Gazebo仿真环境以及car_vision视觉巡线功能包。car_vision的核心任务可以概括为订阅 /camera/image_raw 图像 ↓ 图像预处理与边线提取 ↓ 识别普通道路、十字、圆环等道路状态 ↓ 根据道路状态补线并计算中线偏差 ↓ PID 控制计算 linear.x 和 angular.z ↓ 发布 /cmd_vel 控制小车运动一、car_ws项目总览car_ws是本文使用的ROS2工作空间主要用于在Gazebo中验证视觉巡线算法。它和后续实车工程car_project的关系是car_ws偏向仿真验证使用ROS2、Gazebo、/camera/image_raw、/cmd_velcar_project偏向实车运行使用C直接读取摄像头、编码器、陀螺仪、电机驱动等硬件接口car_vision/tracking视觉巡线算法的Python参考实现后续可以迁移到实车C工程。1.1 目录结构当前car_ws目录结构如下car_ws/ ├── README.md └── src/ ├── car_description/ │ ├── launch/ │ ├── rviz/ │ └── urdf/ ├── car_gazebo/ │ ├── launch/ │ ├── models/ │ └── worlds/ └── car_vision/ ├── car_vision/ │ ├── realtime_node.py │ ├── pid_controller.py │ └── tracking/ ├── images/ ├── launch/ ├── package.xml └── setup.py几个主要功能包的职责如下功能包主要职责说明car_description小车模型描述提供URDF/Xacro模型、摄像头模型和RViz显示配置car_gazebo仿真环境提供Gazebo世界、赛道贴图、仿真启动脚本car_vision视觉巡线订阅摄像头图像识别赛道发布/cmd_vel控制指令1.2 数据流仿真运行时各模块之间的数据流如下car_gazebo │ ├── 加载 car_description 中的小车模型 │ ├── Gazebo 摄像头插件发布 /camera/image_raw │ ▼ car_vision/realtime_node │ ├── CvBridge 转 OpenCV 图像 ├── TrackingBase 计算巡线误差 ├── PidController 计算速度和角速度 │ ▼ /cmd_vel │ ▼ Gazebo 差速驱动插件控制小车运动这里需要注意仿真环境中car_vision发布的是geometry_msgs/msg/Twist也就是linear.x和angular.z。实车工程中通常不会直接使用Twist而是把目标速度和目标角速度转换成左右轮差速再输出到电机。二、car_vision功能包2.1 功能包创建创建Python版本的ROS2功能包zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src$ ros2 pkg create --build-type ament_python car_vision创建完成后基础目录结构如下car_vision/ ├── car_vision/ │ └── __init__.py ├── package.xml ├── resource/ ├── setup.cfg ├── setup.py └── test/为了支持一键启动巡线节点需要增加launch/目录zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src/car_vision$ mkdir launch2.2setup.pysetup.py负责告诉ament_python如何安装Python模块、资源文件和可执行入口。当前car_vision的入口节点是realtime_nodeentry_points{ console_scripts: [ realtime_node car_vision.realtime_node:main, ] },同时为了让ros2 launch能够找到launch文件需要安装launch/*.launch.pyimport os from glob import glob ... data_files[ (share/ament_index/resource_index/packages, [resource/ package_name]), (share/ package_name, [package.xml]), (os.path.join(share, package_name, launch), glob(os.path.join(launch, *.launch.py))), ], ...2.3package.xmlpackage.xml中需要声明运行依赖。car_vision主要依赖依赖作用rclpyROS2 Python节点支持sensor_msgs订阅Image图像消息geometry_msgs发布Twist速度消息cv_bridgeROS图像和OpenCV图像互转opencv2图像处理ros2launch启动launch文件对应配置如下dependrclpy/depend dependsensor_msgs/depend dependgeometry_msgs/depend dependcv_bridge/depend dependopencv2/depend exec_dependros2launch/exec_depend2.4cascaded.launch.pylaunch/cascaded.launch.py用于启动视觉巡线节点。当前实际可执行程序是realtime_nodefrom launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( packagecar_vision, executablerealtime_node, namerealtime_node, outputscreen, parameters[{ image_topic: /camera/image_raw, cmd_vel_topic: /cmd_vel, debug_view: True, reset_on_start: True, reset_wait_timeout_sec: 3.0, kp: 0.045, ki: 0.001, kd: 0.008, }] ) ])这里要注意如果节点代码只声明了kp、ki、kd那么kp_outer、kd_outer、base_speed、wheel_track这类参数即使写在launch文件中也不会被当前realtime_node读取。参数名称必须和节点中declare_parameter()声明的名称保持一致。三、视觉巡线模块car_vision/car_vision/tracking/是巡线算法的主体部分。它不是单个函数直接完成所有事情而是拆成了“图像预处理、边线提取、拐点检测、道路状态判断、策略补线、中线误差计算”几个步骤。3.1 模块结构核心目录如下tracking/ ├── config.py ├── context.py ├── line_follower_algo.py ├── edge_tracker.py ├── corner_detector.py ├── line_fitter.py ├── tracking_base.py ├── enums/ ├── handler/ ├── strategy/ └── tools/主要模块说明如下模块主要功能逻辑说明config.py参数配置集中保存图像尺寸、PID参数、圆环阈值、十字阈值、透视半宽表等context.py状态上下文保存当前帧边线、拐点、道路状态、圆环状态、十字状态line_follower_algo.py八邻域边线追踪二值化图像后从图像底部寻找左右边线起点并沿边界追踪edge_tracker.py边线整理把原始边线点转换成按y索引的边界数组并统计丢线情况corner_detector.py拐点检测根据边线方向变化识别左下拐点、右下拐点、突变点等line_fitter.py补线算法普通道路求中线特殊道路根据单侧边线补另一侧边线handler/元素状态机处理十字、左圆环、右圆环的状态切换和补线过程strategy/道路策略根据道路类型选择普通、十字、圆环、障碍、坡道处理策略tracking_base.py总调度串联上述模块输出中线偏差和调试图像3.2config.pyconfig.py保存巡线算法的全局配置。这样做的好处是图像尺寸、前瞻点、PID参数和元素识别阈值集中在一个地方调试时不用到处找参数。主要参数包括参数说明img_width、img_height图像处理尺寸当前为160 × 120track_aim普通道路的前瞻行ring_track_aim圆环道路的前瞻行kp、ki、kd控制器参数normal_base_speed普通道路基础速度ring_base_speed圆环基础速度ring_loss_count_threshold圆环侧边线丢线点数阈值cross_loss_y_threshold十字入口丢线高度阈值bridge_enable坡道检测开关barrier_enable障碍检测开关half_road不同y坐标下的赛道半宽补偿表其中half_road用于透视补偿。因为图像底部离小车近赛道看起来更宽图像上方离小车远赛道看起来更窄所以不能用固定宽度补线。3.3context.pyTrackingContext用于保存当前帧的中间结果。它相当于巡线算法内部的数据总线后续模块都通过它读取或更新状态。主要保存内容包括当前道路类型NORMAL、CROSS、LEFT_RING、RIGHT_RING、BARRIER、BRIDGE当前十字状态NO_CROSS、APPROACHING、ENTERING、EXITING当前圆环状态NO_RING、APPROACHING、ENTERING、INSIDE、EXITING左右边线数组左右边线原始点左右边线丢线数量左右下拐点、左右上拐点左右突变点当前前瞻行track_aim。将这些状态集中保存可以避免函数之间传递大量参数也方便后续调试时把每一帧的状态打印出来。3.4line_follower_algo.pyLineFollowerAlgo负责最基础的图像处理和边线追踪。3.4.1 图像预处理摄像头输入是BGR图像算法会先转换到HSV颜色空间然后筛选白色赛道区域lower_white np.array([0, 0, 200]) upper_white np.array([180, 30, 255]) mask cv2.inRange(hsv, lower_white, upper_white)预处理后的图像只有黑白两类白色赛道区域黑色非赛道区域。为了避免追线跑到图像边界程序还会给图像四周绘制黑色边框。3.4.2 起点搜索小车正常行驶时赛道从图像底部进入画面。因此算法会从图像底部靠近中心的位置开始分别向左、向右搜索左右边线起点左边线找到白色赛道和左侧黑色背景的交界右边线找到白色赛道和右侧黑色背景的交界。如果起点找不到说明当前图像无法可靠判断赛道后续控制会进入失败保护逻辑。3.4.3 八邻域追线找到起点后算法使用八邻域搜索沿边界向前追踪。八邻域就是当前像素周围的8个方向左上 上 右上 左 当前 右 左下 下 右下对于左边线和右边线搜索方向顺序不同左边线按一个方向绕边界走右边线按相反方向绕边界走当左右边线相遇或者连续多次没有新点时停止追踪。最终输出二值图binary左边界数组left_border右边界数组right_border左边线原始点left_points右边线原始点right_points交叉点cross_point。3.5edge_tracker.pyEdgeTracker负责把原始边线点整理成更容易使用的边界数组。原始追线结果是一串点例如(x, y)。但是后续补线和误差计算更希望按行读取边线位置也就是left_border[y] 当前 y 行的左边线 x 坐标 right_border[y] 当前 y 行的右边线 x 坐标同时EdgeTracker还会统计丢线情况loss_count边线丢失的行数first_loss_y第一次丢线出现的位置。这些信息对十字和圆环识别很重要。例如圆环入口通常会出现一侧边线大面积丢失十字入口也会出现左右边线开口。3.6corner_detector.pyCornerDetector负责检测边线中的关键拐点包括left_turn_down左下拐点right_turn_down右下拐点left_turn_up左上拐点right_turn_up右上拐点left_mutation左边线突变点right_mutation右边线突变点。拐点检测的基本思想是如果一段边线的方向连续变化并且变化模式符合某类元素的几何特征就认为这里存在拐点。比如十字路口常见特征是左右边线在前方突然断开因此会出现明显的下拐点圆环入口常见特征是内侧边线丢失同时外侧边线仍然连续。3.7line_fitter.pyLineFitter负责补线和中线生成。普通道路下左右边线都比较完整可以直接取中线center_x (left_x right_x) / 2特殊道路下不能简单取左右边线中点。例如十字路口左右边线可能断开需要根据下方拐点和上方拐点补线左圆环左侧边线可能丢失需要根据右边线和赛道宽度补左侧右圆环右侧边线可能丢失需要根据左边线和赛道宽度补右侧。补线的目的不是让图像看起来完整而是为了生成一个稳定的中线让控制算法有可靠的误差输入。四、道路元素处理道路元素识别不应该只看单帧图像因为单帧图像容易受到光照、阴影、遮挡影响。当前工程使用“状态机 连续帧确认”的方式处理十字和圆环。4.1 道路状态道路类型定义在road_state.py中主要包括状态含义NORMAL普通道路CROSS十字路口LEFT_RING左圆环RIGHT_RING右圆环BARRIER障碍BRIDGE坡道TrackingBase._update_road_state()按照优先级更新道路状态十字 ↓ 左圆环 ↓ 右圆环 ↓ 障碍 ↓ 坡道 ↓ 普通道路之所以要设置优先级是因为多个元素特征可能同时出现。例如圆环入口也可能出现一侧丢线如果不区分优先级很容易误判。4.2 普通道路普通道路由NormalRoadStrategy处理。主要逻辑使用默认前瞻行track_aim使用左右边线直接求中线根据前瞻区域的中线位置计算偏差将偏差交给PID控制器。普通道路下控制最简单核心是让中线尽量稳定不要因为局部噪声导致输出抖动。4.3 十字路口十字路口由CrossroadHandler和CrossRoadStrategy处理。十字路口的典型特征前方赛道突然变宽左右边线出现明显丢线左右下拐点明显有时可以检测到上方拐点。状态机大致分为状态说明NO_CROSS当前不是十字APPROACHING接近十字开始检测拐点和丢线ENTERING进入十字开始补线并保持直行EXITING离开十字等待左右边线恢复十字路口的处理重点是“不要被横向边缘带偏”。进入十字后算法会通过补线保持中线方向稳定让小车尽量直行穿过十字等重新看到连续边线后再恢复普通巡线。4.4 左圆环左圆环由LeftRingHandler和LeftRingRoadStrategy处理。左圆环的典型特征左侧边线逐渐丢失右侧边线仍然比较连续左下拐点或左侧突变点明显曲率和丢线位置满足圆环阈值。状态机大致分为状态说明NO_RING当前不是圆环APPROACHING接近圆环入口ENTERING开始入环INSIDE已经在圆环内部EXITING准备出环COMPILE出环完成恢复普通巡线左圆环处理中算法通常会更多依赖右边线进行补线。因为入左圆环时左侧内环边界更容易丢失而右侧外环边界更稳定。4.5 右圆环右圆环由RightRingHandler和RightRingRoadStrategy处理。右圆环和左圆环逻辑对称右侧边线容易丢失左侧边线更稳定通过右侧丢线、右下拐点、右侧突变点等特征确认入环入环后根据左侧边线补右侧边线再生成中线。圆环处理最关键的是“状态不能反复跳变”。如果刚进入圆环又马上回到NORMAL小车会在入口处左右摆动。因此圆环状态机需要连续帧确认并且出环后要有完成标记避免同一个圆环被重复识别。4.6 障碍和坡道当前工程中BARRIER和BRIDGE已经预留策略接口但默认配置里bridge_enable False barrier_enable False也就是说障碍和坡道检测默认关闭。后续如果需要启用可以在config.py中打开对应开关并继续完善检测逻辑。五、主节点与控制算法5.1realtime_node.pyrealtime_node.py是car_vision的运行入口。它负责把ROS2消息系统和视觉算法连接起来。主要流程如下初始化 ROS2 节点 ↓ 声明参数 image_topic、cmd_vel_topic、debug_view、kp、ki、kd ↓ 创建 /camera/image_raw 订阅者 ↓ 创建 /cmd_vel 发布者 ↓ 每收到一帧图像执行 image_callback()image_callback()中的核心逻辑ROS Image ↓ CvBridge 转 BGR 图像 ↓ TrackingBase.process() 得到 error 和 debug_img ↓ PidController.compute_control() 得到 linear_speed 和 angular_speed ↓ publish_command() 发布 Twist ↓ 如果 debug_viewTrue则显示调试图像如果启动时reset_on_startTrue节点会尝试调用/reset_world服务把Gazebo世界复位。如果不是在Gazebo中运行可以关闭这个功能ros2 run car_vision realtime_node --ros-args -p reset_on_start:false5.2TrackingBaseTrackingBase是视觉算法总调度器。它的process()函数是单帧图像处理入口1. _extract_boundaries() 2. _update_road_state() 3. strategy_mgr.get_strategy() 4. strategy.process() 5. _calculate_error() 6. _draw_debug()5.2.1 边线提取_extract_boundaries()会调用LineFollowerAlgo.process()二值化和八邻域追线EdgeTracker.scan_left_line()整理左边线EdgeTracker.scan_right_line()整理右边线CornerDetector.detect_all()检测拐点和突变点。最终结果保存在TrackingContext中。5.2.2 道路状态更新_update_road_state()根据当前边线、丢线和拐点信息判断道路类型。它不会只依赖单个特征而是结合当前道路状态十字状态机左圆环状态机右圆环状态机障碍和坡道开关。5.2.3 策略处理道路状态确定后StrategyManager会选择对应策略道路状态策略NORMALNormalRoadStrategyCROSSCrossRoadStrategyLEFT_RINGLeftRingRoadStrategyRIGHT_RINGRightRingRoadStrategyBARRIERBarrierRoadStrategyBRIDGEBridgeRoadStrategy策略的输出是中线点center_points。控制算法不直接关心当前是十字还是圆环只关心最后得到的中线是否稳定。5.2.4 误差计算误差计算使用前瞻行附近的中线点error 图像中心 x - 中线 x当前实现会在track_aim ± 7的窗口内取多个中线点做加权平均这样比只取单个点更稳定。误差含义error 0中线在图像中心左侧小车需要向左侧修正error 0中线在图像中心右侧小车需要向右侧修正error 0中线基本居中。具体正负方向是否和车辆实际转向一致还要结合PID输出符号、Gazebo坐标系和底盘差速方向共同确认。5.3PidControllerPidController把视觉误差转换成linear.x和angular.z。当前控制链路不是复杂的曲率内外环而是视觉误差 error ↓ PID 计算 angular.z ↓ 根据 road_state 和 |angular.z| 计算 linear.x ↓ 发布 Twist(linear.x, angular.z)控制器主要逻辑如下5.3.1 时间间隔计算每帧都会根据当前时间和上一帧时间计算dtdt (current_time - self.prev_time).nanoseconds / 1e9 dt max(0.001, min(0.1, dt))这里对dt做限幅是为了避免某一帧卡顿导致微分项突然变得很大。5.3.2 积分处理当前实现使用条件积分if abs(error) 50: self.integral error * dt else: self.integral 0.0这样可以避免大偏差时积分持续累加导致小车回正后仍然被积分项拖着继续转向。5.3.3 角速度限幅与滤波PID输出会先限制最大角速度target_angular max(-self.max_angular, min(self.max_angular, target_angular))然后限制每帧角速度变化率并做低通滤波限幅避免角速度突变 滤波避免输出抖动这两个步骤能让小车动作更平滑但如果限制太强也可能导致转向响应不足。5.3.4 速度调整线速度由道路状态和角速度共同决定普通道路使用normal_base_speed圆环使用ring_base_speed角速度越大线速度越低最终速度不低于min_speed。这符合智能车常见策略直道快一些急弯和圆环慢一些。六、编译运行6.1 编译进入car_ws目录zhengyangubuntu:~$ cd /opt/2k0300/loongson_2k300_lib/car_ws编译car_vision功能包zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ colcon build --packages-select car_vision或者编译整个工作空间zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ colcon build编译完成后加载环境zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source install/setup.sh6.2 测试6.2.1 启动Gazebo第一个终端启动仿真环境zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source /usr/share/gazebo/setup.sh zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source install/setup.sh zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 launch car_gazebo gazebo.launch.py6.2.2 启动视觉巡线第二个终端启动视觉节点zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source install/setup.sh zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 launch car_vision cascaded.launch.py如果不需要launch文件也可以直接运行节点zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 run car_vision realtime_node如果当前没有启动Gazebo建议关闭启动复位zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 run car_vision realtime_node --ros-args -p reset_on_start:false运行效果如下巡线视频《巡线视频.mp4》。6.2.3 查看话题查看摄像头话题zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 topic list | grep camera查看速度指令zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 topic echo /cmd_vel查看节点参数zhengyangubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 param list /realtime_node七、补充7.1 参数调节7.1.1PID参数当前节点声明了kp、ki、kd三个参数ros2 param set /realtime_node kp 0.045 ros2 param set /realtime_node ki 0.001 ros2 param set /realtime_node kd 0.008需要注意当前代码的参数回调中已经处理了kp和ki如果希望运行时动态修改kd还需要在parameters_callback()中补充kd更新逻辑。调参建议参数作用调节建议kp偏差响应强度从小到大增加直到小车能跟上弯道ki消除静态误差先设为0最后少量增加kd抑制误差变化小车左右摆动时适当增加7.1.2 速度参数速度参数在config.py中参数作用normal_base_speed普通道路基础速度ring_base_speed圆环基础速度min_speed控制器内部最低速度调试顺序建议先把速度调低让小车稳定跑完整圈再增加kp让小车能跟上弯道如果出现左右摆动再增加kd如果直道长期偏一边再少量增加ki最后逐步提高normal_base_speed。7.1.3 图像参数图像参数主要包括参数作用img_width、img_height图像处理分辨率track_aim普通道路前瞻行ring_track_aim圆环前瞻行half_road透视补偿半宽track_aim越小控制越偏向看近处响应直接但容易抖track_aim越大控制越偏向看远处提前量更大但如果远处识别不稳定也会导致误判。7.2、常见问题7.2.1 节点启动后没有图像先检查话题是否存在ros2 topic list | grep /camera/image_raw如果没有/camera/image_raw说明Gazebo摄像头没有启动或者模型没有正确加载摄像头插件。7.2.2 小车不动检查是否有/cmd_vel输出ros2 topic echo /cmd_vel如果没有输出说明realtime_node没有正常运行或者图像处理一直失败。7.2.3 小车左右摆动常见原因kp过大kd过小图像二值化不稳定track_aim太近边线丢失后补线不稳定。可以先降低速度再降低kp最后适当增加kd。7.2.4 小车转弯不足常见原因kp太小max_angular太小max_angular_change限制太严速度过高前瞻行太远导致误差不明显。可以先降低速度再逐步增加kp或放宽角速度变化率限制。7.2.5 圆环入口反复误判圆环入口反复误判通常是状态机稳定性不够或者丢线阈值过低。可以检查ring_loss_count_threshold是否过小ring_loss_y_threshold是否过小左右拐点是否被阴影误触发出环后是否正确回到NORMAL。八、代码下载loongson_2k300_lib参考文章[1] 智能车镜头组入门(四)元素识别[2] 第18届全国大学生智能汽车竞赛四轮车开源讲解【5】--直道、弯道、十字