ROS2 - 智能车走马观碑组Gazebo仿真环境巡线算法

经过前期的努力《龙芯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_vel；
car_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功能包：

zhengyang@ubuntu:/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/目录：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src/car_vision$ mkdir launch

2.2 `setup.py`

setup.py负责告诉ament_python如何安装Python模块、资源文件和可执行入口。

当前car_vision的入口节点是realtime_node：

entry_points={
    'console_scripts': [
        'realtime_node = car_vision.realtime_node:main',
    ]
},

同时，为了让ros2 launch能够找到launch文件，需要安装launch/*.launch.py：

import 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.3 `package.xml`

package.xml中需要声明运行依赖。car_vision主要依赖：

依赖	作用
`rclpy`	`ROS2 Python`节点支持
`sensor_msgs`	订阅`Image`图像消息
`geometry_msgs`	发布`Twist`速度消息
`cv_bridge`	`ROS`图像和`OpenCV`图像互转
`opencv2`	图像处理
`ros2launch`	启动`launch`文件

对应配置如下：

<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>opencv2</depend>

<exec_depend>ros2launch</exec_depend>

2.4 `cascaded.launch.py`

launch/cascaded.launch.py用于启动视觉巡线节点。当前实际可执行程序是realtime_node：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='car_vision',
            executable='realtime_node',
            name='realtime_node',
            output='screen',
            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.2 `config.py`

config.py保存巡线算法的全局配置。这样做的好处是：图像尺寸、前瞻点、PID参数和元素识别阈值集中在一个地方，调试时不用到处找参数。

主要参数包括：

参数	说明
`img_width`、`img_height`	图像处理尺寸，当前为`160 × 120`
`track_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.3 `context.py`

TrackingContext用于保存当前帧的中间结果。它相当于巡线算法内部的数据总线，后续模块都通过它读取或更新状态。

主要保存内容包括：

当前道路类型：NORMAL、CROSS、LEFT_RING、RIGHT_RING、BARRIER、BRIDGE；
当前十字状态：NO_CROSS、APPROACHING、ENTERING、EXITING；
当前圆环状态：NO_RING、APPROACHING、ENTERING、INSIDE、EXITING；
左右边线数组；
左右边线原始点；
左右边线丢线数量；
左右下拐点、左右上拐点；
左右突变点；
当前前瞻行track_aim。

将这些状态集中保存，可以避免函数之间传递大量参数，也方便后续调试时把每一帧的状态打印出来。

3.4 `line_follower_algo.py`

LineFollowerAlgo负责最基础的图像处理和边线追踪。

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.5 `edge_tracker.py`

EdgeTracker负责把原始边线点整理成更容易使用的边界数组。

原始追线结果是一串点，例如(x, y)。但是后续补线和误差计算更希望按行读取边线位置，也就是：

left_border[y]  = 当前 y 行的左边线 x 坐标
right_border[y] = 当前 y 行的右边线 x 坐标

同时，EdgeTracker还会统计丢线情况：

loss_count：边线丢失的行数；
first_loss_y：第一次丢线出现的位置。

这些信息对十字和圆环识别很重要。例如圆环入口通常会出现一侧边线大面积丢失；十字入口也会出现左右边线开口。

3.6 `corner_detector.py`

CornerDetector负责检测边线中的关键拐点，包括：

left_turn_down：左下拐点；
right_turn_down：右下拐点；
left_turn_up：左上拐点；
right_turn_up：右上拐点；
left_mutation：左边线突变点；
right_mutation：右边线突变点。

拐点检测的基本思想是：如果一段边线的方向连续变化，并且变化模式符合某类元素的几何特征，就认为这里存在拐点。

比如十字路口常见特征是左右边线在前方突然断开，因此会出现明显的下拐点；圆环入口常见特征是内侧边线丢失，同时外侧边线仍然连续。

3.7 `line_fitter.py`

LineFitter负责补线和中线生成。

普通道路下，左右边线都比较完整，可以直接取中线：

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.1 `realtime_node.py`

realtime_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_view=True，则显示调试图像

如果启动时reset_on_start=True，节点会尝试调用/reset_world服务，把Gazebo世界复位。如果不是在Gazebo中运行，可以关闭这个功能：

ros2 run car_vision realtime_node --ros-args -p reset_on_start:=false

5.2 `TrackingBase`

TrackingBase是视觉算法总调度器。它的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会选择对应策略：

道路状态	策略
`NORMAL`	`NormalRoadStrategy`
`CROSS`	`CrossRoadStrategy`
`LEFT_RING`	`LeftRingRoadStrategy`
`RIGHT_RING`	`RightRingRoadStrategy`
`BARRIER`	`BarrierRoadStrategy`
`BRIDGE`	`BridgeRoadStrategy`

策略的输出是中线点center_points。控制算法不直接关心当前是十字还是圆环，只关心最后得到的中线是否稳定。

5.2.4 误差计算

误差计算使用前瞻行附近的中线点：

error = 图像中心 x - 中线 x

当前实现会在track_aim ± 7的窗口内取多个中线点做加权平均，这样比只取单个点更稳定。

误差含义：

error > 0：中线在图像中心左侧，小车需要向左侧修正；
error < 0：中线在图像中心右侧，小车需要向右侧修正；
error = 0：中线基本居中。

具体正负方向是否和车辆实际转向一致，还要结合PID输出符号、Gazebo坐标系和底盘差速方向共同确认。

5.3 `PidController`

PidController把视觉误差转换成linear.x和angular.z。

当前控制链路不是复杂的曲率内外环，而是：

视觉误差 error
    ↓
PID 计算 angular.z
    ↓
根据 road_state 和 |angular.z| 计算 linear.x
    ↓
发布 Twist(linear.x, angular.z)

控制器主要逻辑如下：

5.3.1 时间间隔计算

每帧都会根据当前时间和上一帧时间计算dt：

dt = (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目录：

zhengyang@ubuntu:~$ cd /opt/2k0300/loongson_2k300_lib/car_ws

编译car_vision功能包：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ colcon build --packages-select car_vision

或者编译整个工作空间：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ colcon build

编译完成后加载环境：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source install/setup.sh

6.2 测试

6.2.1 启动`Gazebo`

第一个终端启动仿真环境：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source /usr/share/gazebo/setup.sh
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source install/setup.sh
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 launch car_gazebo gazebo.launch.py

6.2.2 启动视觉巡线

第二个终端启动视觉节点：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source install/setup.sh
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 launch car_vision cascaded.launch.py

如果不需要launch文件，也可以直接运行节点：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 run car_vision realtime_node

如果当前没有启动Gazebo，建议关闭启动复位：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 run car_vision realtime_node --ros-args -p reset_on_start:=false

运行效果如下：

巡线视频：《巡线视频.mp4》。

6.2.3 查看话题

查看摄像头话题：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 topic list | grep camera

查看速度指令：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 topic echo /cmd_vel

查看节点参数：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 param list /realtime_node

七、补充

7.1 参数调节

7.1.1 `PID`参数

当前节点声明了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】--直道、弯道、十字

posted @ 2026-04-07 14:33 大奥特曼打小怪兽阅读(104) 评论(0) 收藏举报

刷新页面返回顶部

大奥特曼打小怪兽（❤️ 支持作者）