视觉伺服系统设计：从状态机到双环PID的自动追踪实战

1. 从“识别-跟踪-击打”到“识别-跟踪-击打-复位”的闭环设计2023年电赛E题“运动目标控制与自动追踪系统”本质上是一个经典的“视觉伺服”问题但它在经典框架上增加了一个关键的“复位”环节这恰恰是区分方案优劣、拉开分数差距的核心。很多队伍初期只关注如何让云台快速、准确地跟踪并击打目标却忽略了题目中“自动复位”这一明确要求。这个“复位”不是简单的回到原点而是要求在目标消失或丢失后系统能自主、稳定地回到一个预设的“待命”或“搜索”状态为下一次捕获做好准备。这构成了一个完整的控制闭环感知识别目标→ 决策计算偏差→ 执行驱动云台→ 状态重置复位。任何一个环节的薄弱都会导致系统在长时间运行中失稳。我当时的方案硬件上采用了经典的“OpenMV/K210 STM32”组合。视觉模块负责“眼睛”主控STM32负责“大脑”和“小脑”。这里有一个关键选择为什么不用OpenCV在PC上处理然后串口下发指令原因在于实时性与可靠性。电赛现场环境复杂有线连接笨重且易受干扰无线图传又有延迟和丢包风险。将视觉算法下沉到嵌入式视觉模块OpenMV/K210与STM32通过串口进行紧耦合通信构成了一个独立、可靠的嵌入式系统这符合工业上“边缘计算”的思路能有效规避上位机的不确定性。在软件架构上我摒弃了简单的“查询-响应”模式而是采用了状态机State Machine的设计思想。整个系统被划分为几个明确的状态SEARCHING搜索、TRACKING跟踪、ATTACKING击打、RESETTING复位。状态之间的转换由视觉模块的识别结果、定时器超时、外部按键等事件触发。例如当连续10帧未识别到目标则从TRACKING状态跳转到RESETTING状态。这种设计使得程序逻辑无比清晰调试时可以通过一个全局变量轻松知道系统当前在干什么极大降低了后期排查复杂bug的难度。2. 视觉识别模块的选型与优化OpenMV vs K210视觉模块是系统的“眼睛”它的性能直接决定了上限。2023年E题的目标通常是特定颜色如红色的方形或圆形标靶。围绕OpenMV和K210我做了深入的对比和实测。OpenMV Cam H7的优势在于生态和易用性。其MicroPython开发环境对新手极其友好内置的find_blobs函数对于颜色追踪是“开箱即用”的。你可以在半小时内就写出一个能稳定识别色块的程序。它的弱点在于性能瓶颈处理复杂图像如高分辨率、多目标时帧率下降明显且复杂的机器学习模型部署较为困难。对于纯颜色追踪的题目OpenMV完全够用且开发速度快能为其他模块节省大量时间。K210如MaixPy系列的优势在于强大的算力与灵活性。它搭载了KPU神经网络处理器可以跑YOLO、MobileNet等轻量级模型这意味着你可以做更复杂的识别比如特定形状、数字甚至自定义图标。对于可能出现的多目标、相似色干扰或需要特征匹配的题目K210是降维打击。它的缺点也很明显开发环境相对复杂需要接触MicroPython或C内存管理需要更小心。如果题目只是单一颜色块它的算力优势无法完全体现反而可能因开发周期长而拖累进度。我的选择策略是如果赛题目标特征简单单一颜色优先使用OpenMV追求快速稳定出效果。如果目标特征复杂或有升级需求果断选用K210。在实际备赛时我们两种平台都做了准备核心算法如颜色阈值选取、轮廓过滤封装成接口一致的函数确保主控STM32的代码无需大幅改动即可切换视觉模块。在算法层面无论是OpenMV还是K210核心步骤是一致的图像预处理转换为HSV色彩空间。RGB对光线敏感而HSV能将颜色Hue、饱和度Saturation、明度Value分离更容易在变化光照下稳定提取特定颜色。通过反复实测确定一套鲁棒的阈值范围例如红色在HSV中可能对应(0, 100, 100)到(10, 255, 255)和(160, 100, 100)到(180, 255, 255)两个区间。目标提取与过滤使用find_blobs或轮廓查找函数。然后根据面积blob.area()、宽高比、圆形度等几何特征过滤噪声。一个关键技巧是设置面积变化率阈值连续两帧间目标面积不应发生突变这可以有效滤除瞬间闪过的干扰色块。坐标计算与滤波得到目标中心像素坐标(cx, cy)后不能直接使用。需要经过卡尔曼滤波Kalman Filter或简单的移动平均滤波。卡尔曼滤波能很好地预测运动目标的下一个位置在目标被短暂遮挡时尤其有效。如果实现复杂度高一个五点的移动平均滤波也能大幅平滑云台运动避免抖动。# OpenMV 颜色块识别与滤波示例 (MicroPython) import sensor, image, time from pyb import UART import ulab as np # 如果需要简单的滤波计算 # 初始化 sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QVGA) # 320x240 sensor.skip_frames(time 2000) uart UART(3, 115200) # 与STM32通信的串口 # 红色阈值 (HSV) red_thresholds [(0, 100, 100, 10, 255, 255), (160, 100, 100, 180, 255, 255)] # 简单的移动平均滤波队列 pos_history [(160, 120)] * 5 # 初始化为图像中心 history_index 0 while(True): img sensor.snapshot() blobs img.find_blobs(red_thresholds, pixels_threshold200, area_threshold200) if blobs: # 找到面积最大的色块 largest_blob max(blobs, keylambda b: b.area()) # 更新滤波历史 pos_history[history_index] (largest_blob.cx(), largest_blob.cy()) history_index (history_index 1) % 5 # 计算平均值 avg_x sum([p[0] for p in pos_history]) // 5 avg_y sum([p[1] for p in pos_history]) // 5 # 通过串口发送滤波后的坐标 uart.write($%d,%d#\n % (avg_x, avg_y)) else: # 目标丢失发送特定标志 uart.write($LOST#\n)3. STM32主控双环PID与云台控制逻辑STM32作为主控承担着“大脑”和“小脑”的职责。它需要解析视觉模块发来的数据驱动两个舵机俯仰和偏航构成的云台并管理整个系统的状态。通信协议设计这是软硬件联调的第一个坑。务必设计一个简单、可靠、带校验的协议。我采用的是“$”起始“#”结束中间用逗号分隔数据的ASCII协议例如$150,120#表示目标在图像坐标(150,120)。附加一个$LOST#表示目标丢失。STM32端使用串口空闲中断IDLE Interrupt来接收一帧完整数据再解析效率远高于单个字符中断或轮询。核心双环PID控制器。云台跟踪的本质是一个位置随动系统。我们采用位置式PID。但要注意直接用一个PID环控制舵机角度响应可能不够快且容易超调振荡。更优的方案是串级PIDCascade PID外环位置环输入是目标与图像中心的像素偏差(dx, dy)输出是期望的云台角速度。内环速度环输入是外环输出的期望角速度与当前估计角速度的偏差输出是最终的PWM占空比。对于电赛常用的舵机云台其本身可视为一个位置伺服机构我们常常简化处理使用单环PID但结合前馈控制。公式为PWM_Output Kp * error Kd * (error - last_error) FeedForward。其中error是像素偏差Kd项能有效抑制振荡FeedForward项可以根据目标运动趋势预先给出一个输出提高跟踪动态目标的性能。// STM32 HAL 库环境下简化版PID位置控制示例针对一个轴 typedef struct { float Kp, Ki, Kd; float integral; float last_error; float output_max, output_min; // 输出限幅 } PID_Controller; void PID_Init(PID_Controller *pid, float kp, float ki, float kd, float out_max, float out_min) { pid-Kp kp; pid-Ki ki; pid-Kd kd; pid-integral 0; pid-last_error 0; pid-output_max out_max; pid-output_min out_min; } float PID_Calculate(PID_Controller *pid, float setpoint, float measurement, float dt) { float error setpoint - measurement; // 积分项抗饱和处理 pid-integral error * dt; if (pid-integral pid-output_max) pid-integral pid-output_max; else if (pid-integral pid-output_min) pid-integral pid-output_min; // 微分项采用不完全微分可减少噪声影响 float derivative (error - pid-last_error) / dt; // PID输出 float output pid-Kp * error pid-Ki * pid-integral pid-Kd * derivative; // 输出限幅 if (output pid-output_max) output pid-output_max; else if (output pid-output_min) output pid-output_min; pid-last_error error; return output; } // 在定时器中断中调用假设50Hz控制频率 void TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim-Instance CONTROL_TIM_INST) { // 获取当前目标像素坐标来自视觉模块 int target_cx get_vision_target_x(); int target_cy get_vision_target_y(); // 图像中心坐标 const int IMG_CENTER_X 160; const int IMG_CENTER_Y 120; // 计算像素偏差可归一化处理 float error_x (float)(target_cx - IMG_CENTER_X) / IMG_CENTER_X; // 归一化到[-1, 1] float error_y (float)(target_cy - IMG_CENTER_Y) / IMG_CENTER_Y; // 计算PID输出输出对应PWM的增量或绝对角度 float output_x PID_Calculate(pid_yaw, 0, error_x, 0.02); // dt 0.02s (50Hz) float output_y PID_Calculate(pid_pitch, 0, error_y, 0.02); // 将输出转换为舵机PWM值并输出 set_servo_pwm(SERVO_YAW, 1500 (int)(output_x * 500)); // 假设1500为中位±500为范围 set_servo_pwm(SERVO_PITCH, 1500 (int)(output_y * 500)); } }状态机实现在main.c或一个单独的任务中实现系统的状态逻辑。typedef enum { SYS_SEARCH, SYS_TRACK, SYS_ATTACK, SYS_RESET } SystemState; SystemState g_state SYS_SEARCH; uint32_t lost_tick_counter 0; void SystemStateMachine_Update(void) { switch(g_state) { case SYS_SEARCH: // 云台缓慢扫描或停留在复位位置 perform_scanning(); if (vision_target_found()) { g_state SYS_TRACK; lost_tick_counter 0; } break; case SYS_TRACK: // 正常PID跟踪 pid_tracking(); if (!vision_target_found()) { lost_tick_counter; if (lost_tick_counter LOST_THRESHOLD) { // 例如连续10帧丢失 g_state SYS_RESET; } } else { lost_tick_counter 0; // 如果目标进入“击打区”如图像中心小范围内 if (target_in_attack_zone()) { g_state SYS_ATTACK; } } break; case SYS_ATTACK: // 执行击打动作如控制电磁铁吸合 trigger_attack(); delay_ms(200); // 击打持续时间 stop_attack(); // 击打后可能目标消失直接进入复位或搜索 g_state SYS_RESET; break; case SYS_RESET: // 平滑移动云台回零点 smooth_reset_to_home(); if (is_reset_complete()) { g_state SYS_SEARCH; } break; } }4. “复位”功能的工程实现与平滑运动控制“复位”功能看似简单但实现不好会导致云台动作生硬、产生机械冲击、甚至丢失精度。核心要求是平滑、快速、准确地回到预设原点。1. 复位轨迹规划不要直接给舵机发送“归中”的PWM信号。这会导致舵机以最大速度冲过去产生抖动和噪音。应该采用梯形速度规划或S曲线规划。对于电赛应用梯形规划足够用了。即匀加速 → 匀速 → 匀减速。我们需要计算出现位置与目标位置的角度差然后将其分解为多个小步长setpoint逐步发给舵机。2. 插补算法在复位过程中STM32需要以固定频率如100Hz计算下一个“路点”的角度值并更新舵机PWM。这可以通过线性插值实现// 从当前位置 current_pos 平滑移动到目标位置 target_pos // total_steps: 总步数决定移动时间 // current_step: 当前步数 float linear_interpolate(float start_pos, float target_pos, int current_step, int total_steps) { if (current_step total_steps) return target_pos; float t (float)current_step / (float)total_steps; // 也可以在这里加入缓动函数如 t t*t*(3-2*t) 实现平滑出入 return start_pos (target_pos - start_pos) * t; }在定时器中断中current_step递增计算新的角度并设置舵机直到current_step total_steps复位完成。3. 复位位置标定与存储系统的“原点”需要在初次上电时进行标定。可以通过按键进入标定模式手动将云台调整到理想的待命位置通常是视野中心且水平然后按下保存键将此时两个舵机的PWM值存入STM32的Flash中。以后每次复位就读取这个存储值作为目标。4. 抗干扰与异常处理复位过程中如果突然又识别到目标怎么办这需要在状态机中处理。在SYS_RESET状态下仍然可以保留视觉数据的读取和判断。一旦发现目标可以立即中断复位流程切换到SYS_TRACK状态。这要求复位控制函数是可中断的。5. 系统联调与现场避坑指南电赛最后一天往往是联调这时会暴露出无数问题。以下是我总结的“血泪”经验1. 电源是万恶之源舵机尤其是MG996R这类大扭矩舵机在启动和堵转时瞬间电流可达2A以上。如果和STM32、OpenMV共用一套5V电源会导致电压瞬间被拉低引起单片机复位、图像传感器掉线。必须使用独立电源供电大功率舵机由一路大电流如3ADC-DC或稳压模块供电主控和视觉模块由另一路干净的LDO供电。两地之间共地即可。2. 机械结构决定性能下限云台舵盘的轴心必须与摄像头光轴尽量重合否则会产生控制耦合调整偏航会影响俯仰方向的成像。云台本身要牢固任何晃动都会被视觉传感器捕捉到形成正反馈导致系统振荡。在舵机输出轴和云台之间增加橡胶垫片可以吸收高频振动。3. 通信干扰与数据丢包OpenMV/K210与STM32的串口线要短并做好屏蔽。波特率115200足够不必盲目求高。在协议中增加校验和Checksum例如$150,120,45#最后一位45是前两个数字的和取模。STM32端校验失败则丢弃该帧。同时STM32发送给视觉模块的指令如切换模式也应遵循同样格式。4. 光照适应性调整比赛场地灯光可能偏色、有闪烁。解决方案 -自动白平衡开启OpenMV的sensor.set_auto_whitebal(True)但注意这会引入延迟在跟踪快速目标时可能关闭更好。 -动态阈值调整上电后先将目标放在摄像头前按下一个按键程序自动采样当前画面中目标区域的HSV值并计算出一个阈值范围存储起来。比赛开始前重新标定一次。 -曝光时间固定手动设置一个合适的曝光时间sensor.set_auto_exposure(False, exposure_us)避免画面因自动曝光忽明忽暗。5. 调试信息可视化一定要预留一个调试接口可以通过STM32的另一个串口连接电脑实时打印系统状态、PID输出、目标坐标、当前状态等。或者利用OpenMV的IDE直接在图像上画出色块、中心十字和跟踪轨迹。这比盲目猜测高效一百倍。6. 代码版本管理最后时刻千万不要在唯一的一份代码上做重大修改。使用Git如Gitee或简单地在电脑上按日期备份文件夹。改崩了能立刻回退到上一个能工作的版本这是救命的习惯。6. 核心代码片段与模块化设计思想将系统模块化是高效开发和调试的基础。以下是我建议的代码文件结构及核心接口Project/ ├── Core/ │ ├── sys_state_machine.c/.h // 系统状态机 │ ├── pid_controller.c/.h // PID算法库 │ └── data_filter.c/.h // 滤波算法如移动平均、卡尔曼 ├── Drivers/ │ ├── servo.c/.h // 舵机驱动提供角度设置函数 │ ├── uart_protocol.c/.h // 串口通信协议打包/解析 │ └── debug_console.c/.h // 调试信息输出 ├── Hardware/ │ └── pin_def.h // 引脚定义所有硬件接口在此映射 ├── Tasks/ │ ├── vision_task.c/.h // 处理视觉数据在中断或RTOS任务中 │ ├── control_task.c/.h // 云台控制任务 │ └── reset_task.c/.h // 复位控制任务 └── main.c // 初始化主循环调度关键模块接口示例uart_protocol.h中定义通信帧结构#pragma once #define FRAME_START $ #define FRAME_END # typedef struct { int16_t target_x; // 目标X坐标 int16_t target_y; // 目标Y坐标 uint8_t target_lost; // 目标丢失标志 uint8_t checksum_ok; // 校验通过标志 } Vision_Data_t; void UART_ParseByte(uint8_t byte); // 在串口中断中调用 Vision_Data_t* UART_GetVisionData(void); // 获取最新解析的数据servo.c中提供平滑设置角度的函数void Servo_SetAngle_Smooth(Servo_TypeDef* servo, float target_angle, uint32_t duration_ms) { servo-start_angle servo-current_angle; servo-target_angle target_angle; servo-move_duration duration_ms; servo-start_tick HAL_GetTick(); servo-is_moving 1; } // 在定时器中断中调用此函数来更新位置 void Servo_Update(Servo_TypeDef* servo) { if (servo-is_moving) { uint32_t elapsed HAL_GetTick() - servo-start_tick; if (elapsed servo-move_duration) { servo-current_angle servo-target_angle; servo-is_moving 0; } else { float ratio (float)elapsed / (float)servo-move_duration; // 使用缓动函数使运动更平滑 ratio ratio * ratio * (3.0f - 2.0f * ratio); // 三次贝塞尔缓动 servo-current_angle servo-start_angle (servo-target_angle - servo-start_angle) * ratio; } // 将角度转换为PWM并输出 PWM_Set(servo-pwm_channel, Angle_To_PWM(servo-current_angle)); } }这种模块化设计使得在比赛现场如果视觉模块需要从OpenMV换成K210你只需要替换vision_task.c中解析数据的那部分代码而主控逻辑、PID控制、状态机完全无需改动极大提升了方案的容错性和可维护性。电赛E题是一个典型的软硬件结合项目它考察的不仅仅是代码能力和电路知识更是系统工程思维、调试方法和临场应变能力。从明确的题目要求中提炼出“复位”这一关键得分点并围绕它构建一个鲁棒的状态机系统在视觉处理上根据题目复杂度合理选型并做好滤波在控制上深入理解PID实现平滑跟踪与复位最后在联调中解决电源、机械、干扰等实际问题——这套组合拳下来一个稳定可靠的自动追踪系统便水到渠成。希望这份结合了具体代码和实战经验的思路能为你未来的竞赛或项目提供一份可靠的蓝图。