智能车环岛处理：基于状态机与动态圆弧跟踪的鲁棒控制方案

1. 项目概述：环岛处理的挑战与核心思路

在智能车竞赛的赛道上，环岛是一个标志性的、极具挑战性的元素。它不像直道那样可以靠简单的偏差控制，也不像弯道那样有连续的曲率变化。环岛是一个离散的、结构化的特殊赛道元素，要求小车在高速行进中，精准地识别入口、执行绕行、并平滑地驶出。很多队伍在直道和弯道上表现优异，却常常在环岛这里“翻车”——要么是识别失败直接冲出赛道，要么是绕行轨迹丑陋导致速度骤降，甚至因为误判而原地打转。可以说，环岛处理的稳定性和流畅度，是区分一支队伍技术实力的关键分水岭。

我这次分享的“智能车学习日记【四】——环岛”，就是聚焦于如何让我们的摄像头智能车，在面对环岛时能够像老司机一样从容应对。核心目标很明确：第一，要100%稳定识别，绝不能漏判或误判；第二，入岛和出岛的轨迹要平滑，速度损失要小；第三，整个控制逻辑要清晰可靠，便于调试和优化。这不仅仅是写几行判断代码那么简单，它涉及到图像处理、状态机设计、控制算法和参数整定等多个层面的协同工作。下面，我就把自己在实现环岛处理时，从思路构建到代码落地，再到反复调试的完整过程和核心心得，毫无保留地分享出来。

2. 环岛特征分析与识别策略设计

环岛在摄像头图像中，会呈现出一系列独特的、连续的特征变化。我们的识别策略，就是基于对这些特征变化的捕捉和逻辑判断来构建的。一个鲁棒的识别方案，不能只依赖单一特征，必须是多特征、多条件的联合判决。

2.1 图像中的环岛特征提取

首先，我们需要明确环岛在二值化后的赛道图像中长什么样。假设我们的摄像头是前瞻的，拍摄到的环岛近似一个圆形或椭圆形的白色区域（赛道），其外侧被黑色（背景）包围。但更重要的是其边缘信息：

1. 左边线突变：当小车即将进入环岛时，赛道的左边线会突然向右大幅度弯曲（对于逆时针环岛而言），形成一个向内凹的弧线。在巡线常用的“扫线法”中，你会发现在某些行，左边线的横坐标值会急剧增大。
2. 右边线消失/突变：与左边线突变同时，赛道的右边线可能会因为环岛弧度的遮挡，在图像中下部突然消失（变成背景黑色），或者也发生向左的弯曲。
3. 边缘斜率变化：通过计算左右边线在特定区域的斜率，可以发现进入环岛区域时，单侧边缘的斜率绝对值会显著变大。
4. 赛道宽度突变：在环岛入口处，由于边线弯曲，计算出的赛道宽度会先变窄（因为边线靠拢），进入环岛后又可能变宽。

我的策略是综合以上特征。我会在图像中划定一个“兴趣区域”(ROI)，通常是在图像底部往上的一部分区域，因为这里距离车近，特征最明显、最稳定。在这个ROI内，我不仅计算中心偏差，还会实时计算左右边线的位置、斜率，以及它们的历史变化趋势。

注意：单纯依赖某一帧图像的某个特征（比如左边线突然右偏）是非常危险的，很容易受到赛道污渍、光线突变或图像噪点的干扰。必须引入状态机和持续帧数判断。

2.2 基于状态机的识别逻辑实现

为了将上述特征转化为可靠的识别，我引入了有限状态机（FSM）。这是处理环岛这类离散事件最有效的方法。我将环岛过程分为以下几个状态：

• 状态0：巡航状态 (CRUISE)：默认状态，进行普通的L形或S形赛道巡线。
• 状态1：环岛入口识别 (ENTER_DETECT)：当连续多帧（例如5帧）检测到左边线持续右偏且斜率大于阈值，同时右边线异常（消失或左偏），并且赛道宽度符合环岛入口特征时，认为可能遇到了环岛入口。触发状态转移。
• 状态2：环岛内绕行 (IN_LOOP)：确认进入环岛后，切换控制模式。不再以赛道中心线为控制目标，而是以一个虚拟的“环岛中心”为圆心，进行圆弧跟踪。
• 状态3：环岛出口识别 (EXIT_DETECT)：在绕行过程中，持续检测出口特征。出口的特征通常是左边线开始向左恢复（对于逆时针环岛），或者右边线重新出现并变得规则。当满足出口条件持续多帧后，准备出岛。
• 状态4：环岛出口处理 (EXIT_LOOP)：这是一个短暂的过渡状态，用于执行一个平滑的出岛轨迹，避免突然的方向切换导致车身晃动，然后切回巡航状态。

状态机的核心优势在于“记忆”和“容错”。一旦进入IN_LOOP状态，即使中间有几帧图像没识别到环岛特征（可能被遮挡或处理异常），只要没检测到强烈的出口特征，就会保持绕行状态，避免了在环岛内误判退出。同样，入口识别需要连续多帧确认，避免了因单帧噪声导致的误触发。

// 状态机伪代码示例 typedef enum { CRUISE, ENTER_DETECT, IN_LOOP, EXIT_DETECT, EXIT_LOOP } LoopState; LoopState current_state = CRUISE; int enter_confirm_cnt = 0; int exit_confirm_cnt = 0; void state_machine_process() { switch(current_state) { case CRUISE: if (detect_enter_features()) { enter_confirm_cnt++; if (enter_confirm_cnt > ENTER_THRESHOLD) { current_state = ENTER_DETECT; enter_confirm_cnt = 0; // 初始化环岛绕行参数，如估算圆心 init_loop_control(); } } else { enter_confirm_cnt = 0; // 特征不连续，计数器清零 } break; case ENTER_DETECT: // 短暂过渡，确保稳定进入 current_state = IN_LOOP; break; case IN_LOOP: run_loop_control(); // 执行环岛绕行控制 if (detect_exit_features()) { exit_confirm_cnt++; if (exit_confirm_cnt > EXIT_THRESHOLD) { current_state = EXIT_DETECT; exit_confirm_cnt = 0; } } else { exit_confirm_cnt = 0; } break; // ... 其他状态处理 } }

3. 环岛内控制算法详解

识别出环岛只是第一步，如何在环岛内控制小车平稳、快速地绕行，才是真正的技术核心。直接使用普通的PD巡线在环岛内会完全失效，因为控制目标变了。

3.1 圆心估算与圆弧跟踪

我的方法是动态圆弧跟踪。核心思想是：在进入环岛时，根据入口处的边线信息，估算出环岛的大致圆心坐标（相对于车体坐标系）。

一个简单的估算方法是：利用入口处多行左右边线的中点，拟合一个圆。由于我们图像是俯视的，且环岛近似标准圆形，可以使用最小二乘法进行圆拟合。得到圆心坐标(circle_center_x, circle_center_y)和半径R。

进入环岛内后，控制目标不再是让车对准赛道中心，而是让车沿着一个以估算圆心为圆心的、半径为(R - offset)的圆弧行驶。这里的offset是一个内移量，可以让小车走内圈，缩短路径。

控制量计算：此时的方向偏差error不再是车与赛道中心的横向距离，而是当前车体朝向与目标圆弧切线方向之间的角度差。我们可以计算：

1. 车辆当前位置（假设为图像底部中心(0,0)）到圆心(cx, cy)的向量。
2. 该向量的垂直方向（法向量）即为该点圆弧的切线方向。
3. 计算车辆当前航向角（可以通过陀螺仪积分或图像历史中线估算）与这个切线方向的差值，作为新的error输入到转向PD控制器中。

// 圆弧跟踪偏差计算伪代码 float calculate_arc_error(float car_x, float car_y, float circle_cx, float circle_cy, float car_heading) { // 计算车辆到圆心的向量 float dx = circle_cx - car_x; float dy = circle_cy - car_y; // 注意坐标系定义 // 计算该点圆弧的切线方向（与径向垂直） // 对于逆时针绕行，切线方向是径向向量逆时针旋转90度 float tangent_heading = atan2f(-dx, dy); // 根据坐标系调整 // 计算航向偏差 float error = tangent_heading - car_heading; // 将误差规范化到 -PI 到 PI 之间 while (error > PI) error -= 2 * PI; while (error < -PI) error += 2 * PI; return error; }

3.2 速度规划与出岛补偿

在环岛内，速度控制也需要特别规划。

• 入岛减速：在识别到入口并确认后，就需要开始线性减速，确保以合适的速度进入环岛。入岛速度太高，离心力大，容易甩出去。
• 岛内恒速/微调：进入环岛后，可以保持一个相对稳定的中速。如果发现转向不足（误差持续为正或负），可以适当降低速度来辅助转向。
• 出岛加速：在识别到出口，进入EXIT_LOOP状态时，就要开始提前加速，为出岛后的直道或弯道储备速度。同时，转向控制需要有一个“预补偿”，即提前施加一个反向的转向量，帮助车头更快速地摆正，对准出岛后的赛道方向。这个补偿量和补偿时机需要反复调试。

实操心得：环岛内的PD参数通常与普通巡线不同。因为误差的性质从位置偏差变成了角度偏差，单位不同，所以P和D系数需要重新整定。我一般会单独设置一套环岛专用的PD参数。另外，圆心坐标的估算可能随着小车行进有误差，可以采用滑动平均或卡尔曼滤波进行平滑，避免控制量抖动。

4. 调试过程与参数整定实录

理论设计得再完美，不上车调试都是空谈。环岛的调试是一个“识别-控制”联调的过程，非常考验耐心。

4.1 图像识别调试：让车“看得准”

首先，我确保在静态情况下，车放在环岛入口、内部、出口等多个关键位置，通过无线串口或者屏幕，将图像处理的结果实时发送到电脑上位机观察。

1. 绘制特征线：在上位机图像上，不仅绘制出左右边线，我还用不同的颜色标出计算出的斜率变化曲线、赛道宽度曲线。这样能直观看到环岛特征出现的位置和幅度。
2. 打印状态变量：将状态机状态current_state、入口确认计数器enter_confirm_cnt、估算的圆心坐标和半径等关键变量实时打印出来。观察状态转换是否准确、及时。
3. 调整阈值：ENTER_THRESHOLD（入口确认帧数）是关键。设得太小（如2帧），在急弯处容易误触发；设得太大（如10帧），可能车都快进环岛了才识别，导致准备不足。我一般从5帧开始调。斜率阈值、宽度阈值也需要根据实际赛道宽度和摄像头角度反复调整。

常见问题1：误识别

• 现象：在非环岛的普通弯道，状态机跳入了ENTER_DETECT。
• 排查：检查该弯道的图像特征。可能是弯道曲率也很大，导致边线斜率超过了阈值。解决方法：增加识别条件。例如，除了左边线右偏，还必须同时满足右边线“消失”（在图像有效行数内找不到连续边线），或者增加对边线连续弯曲弧度的判断，而不仅仅是单点斜率。

常见问题2：漏识别

• 现象：车已经进入环岛，但状态还在CRUISE，导致按普通弯道处理，直接冲出去。
• 排查：检查环岛入口处图像。可能是摄像头前瞻太远，入口特征出现在图像较上方，而我的ROI区域设置得太靠下，没检测到。也可能是二值化阈值在环岛区域不理想，导致边线提取断裂。解决方法：调整ROI区域，使其覆盖可能出现特征的图像行；优化二值化算法，或采用动态阈值；降低入口判断的斜率阈值。

4.2 控制算法调试：让车“走得稳”

识别稳定后，开始调试环岛内的控制。

1. 静态测试圆心估算：车停在环岛入口，查看上位机显示的估算圆心坐标和半径，与实际情况对比是否合理。如果圆心坐标偏差巨大，检查圆拟合所用的边线点集是否准确，尝试调整拟合所用的行范围。
2. 低速动态测试：让车以很低的速度（如0.3m/s）自动进入环岛。观察以下关键点：
   • 入岛瞬间：是否平稳？有没有明显的方向突变或抖动？
   • 岛内绕行：轨迹是否是光滑的圆弧？车身是否一直与圆弧切线方向保持基本一致？误差error曲线是否在0附近小范围波动？
   • 出岛瞬间：能否平滑切出？车头是否快速对准了出口赛道？出岛后会不会有“画龙”现象？
3. 参数整定顺序：
   • 先调P：将D和出岛补偿设为0，I通常也为0。逐渐增大环岛控制的P值，直到小车能大致跟着圆弧走，但会在目标轨迹附近来回振荡。
   • 再调D：增加D值，用来抑制P引起的振荡。D值能明显让车身姿态更稳定，过弯更丝滑。但D值太大会导致系统响应迟钝，出岛时回正慢。
   • 最后调出岛补偿：出岛补偿是一个在EXIT_LOOP状态临时叠加的固定转向量或一段时间的转向偏置。需要仔细调试其大小和持续时间，使得出岛动作干净利落。

常见问题3：绕行轨迹不圆，向内或向外漂移

• 现象：小车不是在绕一个稳定的圆，而是螺旋形向内或向外移动。
• 排查：这通常说明估算的圆心不准，或者控制算法中的offset内移量设置不当。解决方法：首先确认圆心估算的准确性。如果圆心估算是准的，那么调整跟踪的半径。如果小车向外漂，说明跟踪半径太大了，应减小offset（甚至为负值，即外移）。反之则增加offset。这本质上是调整路径的“内切”程度。

常见问题4：出岛后剧烈摇摆

• 现象：小车成功驶出环岛，但进入直道后，左右摇摆好几次才能稳定。
• 排查：这是出岛切换回普通巡线时，误差和控制器参数突变引起的。解决方法：第一，优化EXIT_LOOP状态。这个状态不要立刻切换回普通PD，而是作为一个过渡，让环岛控制的误差和普通巡线的误差进行一个平滑的插值过渡。第二，检查出岛瞬间的赛道中心线识别是否正常，有时环岛出口的图像比较乱，会导致巡线计算出错，可以适当提高出岛后的图像处理稳定性。

5. 高级优化与抗干扰策略

当基本的环岛功能实现后，为了追求更高的速度和鲁棒性，还可以进行以下优化：

5.1 多传感器融合辅助判断

单纯依靠摄像头图像，在极端光照或赛道反光情况下可能不可靠。可以引入其他传感器进行交叉验证：

• 编码器积分：在进入IN_LOOP状态后，通过编码器记录行驶距离。环岛一周的路径长度大约是2 * PI * R。当行驶距离接近这个值时，可以强制或辅助进行出口判断，防止摄像头误判导致一直绕圈。
• 陀螺仪（IMU）：环岛行驶时，车身会持续进行大约360度的旋转（取决于绕行角度）。通过积分陀螺仪的Z轴角速度，可以估算出累计旋转角度。当角度接近360度时，也可以作为出岛的强触发条件。这能有效应对图像完全丢失的情况。

5.2 动态参数与自适应控制

让控制参数根据小车状态动态调整，能进一步提升性能：

• 速度前馈：转向PD的输出，可以叠加一个与速度成正比的预瞄前馈量。速度越快，提前转向的量越大。这在环岛高速绕行时尤为重要。
• 曲率前馈：如果能够实时估算出环岛的曲率（1/R），可以将曲率值直接映射为一个基础转向量，PD控制器只负责微调。这相当于开环和闭环的结合。
• 参数模糊切换：不要粗暴地在状态切换时立刻改变PD参数。可以设置一个短暂的过渡区，让参数在几帧或几十毫秒内线性变化到新值，避免控制量突变引起抖动。

5.3 赛道记忆与预测

对于规则赛场，环岛的位置和大小是固定的。可以在第一次成功通过后，记录下环岛入口的图像坐标、估算的圆心和半径。下次再跑到赛道这个区域时，可以提前加载这些“记忆”，进行预测性判断和控制，从而大幅提高识别速度和入岛的平滑性。这需要结合编码器或里程计进行粗略的赛道定位。

整个环岛功能的开发，是一个典型的“感知-决策-控制”闭环的微缩实践。它强迫你去深入思考图像处理的细节、状态管理的严谨性以及控制算法的适应性。调试过程可能很痛苦，你会遇到各种奇怪的失败案例，但每一次问题的解决，都会让你对智能车的理解加深一层。我最深的体会是，鲁棒性永远比炫酷的单一最高速度更重要。一个能在十次里稳定通过九次环岛的方案，远胜于一个能一次飙得飞快但另外九次都冲出去的方案。耐心地打磨每一个判断条件，审慎地调整每一个参数，你的小车才会真正变得“智能”起来。