4、目标跟随系统架构：感知模块、决策模块、控制模块的划分与数据流

目标跟随系统并非单一算法或单一传感器的堆叠，而是一个典型的感知-决策-控制闭环系统。在机器人工程实践中，若缺乏清晰的模块划分与接口定义，往往会导致代码耦合严重、调试困难、性能指标无法定位等问题。本章将围绕这一经典三层架构，详细拆解各模块的职责边界、内部处理流程、模块间的数据流方向以及接口设计规范，帮助读者建立从传感器原始数据到执行器控制指令的完整工程视图。

上图展示了目标跟随系统的整体架构与数据流。感知模块位于最左侧，负责将传感器原始数据转换为结构化的目标状态量；决策模块接收该状态量，结合任务逻辑生成高层控制指令；控制模块则将指令转换为具体的执行器动作。值得注意的是，反馈回路贯穿整个系统——执行器状态和新的传感器数据会不断输入感知模块，形成闭环。下面我们将逐一深入各模块的内部设计与接口规范。

4.1 感知模块：从原始数据到目标状态量

核心原理：感知模块的职责是“看懂世界”。它接收RGB图像、深度图或激光雷达点云等原始数据，通过目标检测算法（如YOLO、颜色阈值分割、模板匹配）找到目标在图像中的位置，再结合传感器标定参数，将像素坐标转换为机器人坐标系下的空间位置。该模块的输出是目标状态量，通常包括：目标中心在图像中的像素坐标 (u, v)、目标宽度 w 与高度 h（用于计算距离）、目标深度 d（若使用深度相机或激光雷达）、以及可选的目标运动速度。

关键处理流程：

1. 数据预处理：对图像进行去畸变、缩放、归一化，确保输入尺寸一致；若使用深度相机，还需对齐RGB与深度图。
2. 目标检测：根据应用场景选择算法。固定颜色目标可用HSV阈值分割；复杂场景使用YOLOv5/v8等深度学习模型。
3. 状态提取：计算检测框的中心坐标、面积、宽高比；若检测到多个目标，需通过最近距离或最大面积策略选择主目标。
4. 坐标转换：利用相机内参将像素坐标 (u, v) 转换为相机坐标系下的归一化坐标，再结合深度值得到三维位置 (Xc, Yc, Zc)。

4.2 决策模块：状态机与策略生成

核心原理：决策模块是系统的“大脑”，它不关心具体像素，而是根据感知模块提供的目标状态量，结合当前任务阶段（正常跟随、目标丢失、重新搜索），输出高层控制指令。该模块通常实现为一个有限状态机（FSM），包含三个核心状态：跟随状态（TRACKING）、丢失状态（LOST）、重搜状态（SEARCHING）。

状态迁移逻辑：

• TRACKING → LOST：当连续N帧（例如N=10）感知模块返回 is_detected=false，或目标置信度持续低于阈值时触发。
• LOST → SEARCHING：进入丢失状态后立即启动重搜策略，决策模块生成搜索指令（如原地旋转角速度0.5 rad/s）。
• SEARCHING → TRACKING：一旦感知模块重新检测到目标且置信度达标，立即切换回跟随状态，并重置丢失计数器。

决策输出（接口B）：控制指令同样定义为结构体 ControlCmd，包含：linear_vel（线速度 m/s）、angular_vel（角速度 rad/s）、mode（枚举：FOLLOW、SEARCH、STOP）。该接口是决策模块与控制模块之间的唯一契约。

4.3 控制模块：PID闭环与执行器映射

核心原理：控制模块是系统的“小脑”，负责将决策模块的宏观指令转化为精确的执行器动作。在目标跟随场景中，通常采用双闭环PID控制：角度环控制机器人转向，使目标始终位于图像中心；距离环控制机器人与目标保持期望距离。

控制算法实现：

// 角度环PID（伪代码）
float error_angle = target_u - image_center_u;  // 像素误差
float angular_vel = Kp_angle * error_angle + Ki_angle * integral_angle + Kd_angle * derivative_angle;

// 距离环PID
float error_dist = current_depth - desired_depth;  // 距离误差
float linear_vel = Kp_dist * error_dist + Ki_dist * integral_dist + Kd_dist * derivative_dist;

// 输出限幅
linear_vel = clamp(linear_vel, -0.5f, 0.8f);
angular_vel = clamp(angular_vel, -1.0f, 1.0f);

执行器映射：对于差速底盘，将线速度与角速度通过运动学模型转换为左右轮速度：v_left = linear_vel - angular_vel * wheel_base / 2，v_right = linear_vel + angular_vel * wheel_base / 2。对于带云台的机器人，角速度指令可直接驱动云台舵机，而底盘仅负责前进后退。

4.4 案例拆解：基于ROS2的跟随系统调试实录

场景背景：某巡检机器人项目需要跟随一名工作人员穿越走廊，走廊宽度约2米，光照变化明显（有窗户和阴影区域）。机器人搭载Intel RealSense D435i深度相机，底盘为差速驱动，主控运行Ubuntu 20.04 + ROS2 Foxy。

初始现象：系统在直道跟随时表现良好，但在经过窗户区域时，目标检测框频繁丢失，机器人急停后原地旋转搜索，恢复跟随后又出现明显的左右摆动（振荡）。

关键数据分析：

• 感知模块日志显示：经过窗户时，目标检测置信度从0.85骤降至0.3以下，持续约0.5秒。
• 决策模块状态切换记录：TRACKING→LOST→SEARCHING→TRACKING，整个周期约3秒。
• 控制模块反馈：恢复跟随后，角度环误差从-80像素突变至+50像素，PID输出饱和，导致摆动。

分析判断过程：窗户区域强光导致目标颜色特征偏移，检测算法失效。决策模块的丢失阈值（N=10帧）过于敏感，且SEARCHING状态采用固定角速度旋转，未考虑目标可能仍在视野边缘的情况。控制模块的PID参数在正常光照下已调优，但未针对大误差突变做抗积分饱和处理。

实际操作步骤：

1. 感知优化：在检测算法中增加光照自适应预处理（直方图均衡化），并将检测置信度阈值从0.5降至0.35，同时启用目标运动预测（卡尔曼滤波），在短暂遮挡时预测目标位置。
2. 决策优化：将丢失判定帧数从10帧提升至20帧（约0.7秒），避免瞬时遮挡触发重搜；SEARCHING状态改为先执行±30°扇形扫描，若未找到再执行360°旋转。
3. 控制优化：在PID控制器中加入积分限幅（积分项最大不超过输出限幅的30%），并在状态切换时执行PID参数重置函数。

结果对比：优化后，经过窗户区域时机器人仅出现轻微减速，未触发丢失状态；恢复跟随后无振荡，角度误差稳定在±10像素内。整体跟随成功率从78%提升至96%。

复盘经验：模块化架构的价值在此案例中充分体现——三个模块独立修改、独立测试，无需改动其他模块代码。若采用单体架构，定位问题将耗费数倍时间。

4.5 常见误区与设计复盘

误区一：接口设计过于随意。例如直接传递全局变量或使用无类型定义的字典，导致模块间隐式耦合。正确做法是使用强类型消息（如ROS2的Custom Message或C++的struct）。

误区二：忽视时间戳同步。感知模块处理需要时间，决策模块拿到的数据可能是50ms前的。在高速运动中，这会导致控制滞后。建议在接口中携带时间戳，决策模块可据此进行数据外推。

误区三：控制模块过度设计。有些开发者一上来就使用LQR或MPC，但目标跟随场景中PID已足够。过度复杂的控制器反而增加调试难度和计算开销。

误区四：状态机缺少超时保护。若SEARCHING状态持续旋转超过设定时间（如10秒）仍未找到目标，应切换至STOP状态并报警，而不是无限旋转消耗电量。

综上所述，感知-决策-控制三层架构的核心价值在于关注点分离。感知模块只回答“目标在哪里”，决策模块只回答“接下来做什么”，控制模块只回答“具体怎么做”。通过定义清晰的接口（TargetState和ControlCmd），三个模块可以独立开发、并行调试、分别优化。在实际项目中，这种架构不仅提升了开发效率，更使得系统具备良好的可扩展性——例如更换更好的检测算法，或升级底盘电机，都只需修改对应模块，而无需重构整个系统。