从零复刻国赛一等奖项目手把手教你做激光自动追踪系统最近有不少同学在准备电赛后台收到很多关于2023年E题“运动目标控制与自动追踪系统”的咨询。正好去年我们团队Phantom用STM32F407OpenMV的方案拿了个国一今天我就把这个项目的完整设计思路和实现细节分享出来手把手教大家怎么从零搭建一套能稳定工作的激光自动追踪系统。这个项目听起来高大上其实拆解开来并不复杂。核心就是两个部分一是让红色激光点按指定轨迹运动运动目标控制二是让绿色激光点自动追踪红色激光点自动追踪。咱们一步步来我会把当时踩过的坑、调试的技巧都讲清楚。1. 系统整体设计思路1.1 题目要求理解与拆解先别急着看代码、画电路咱们得先把题目要求吃透。2023年E题的核心任务就两个第一部分运动目标控制红色激光红色激光点要能“回家”——从屏幕上任意位置回到中心原点误差≤2cm要能沿着屏幕边线走一圈像个听话的巡逻兵要能沿着一个贴了黑色胶带的A4纸边框走即使纸是斜着贴的也得走好第二部分自动追踪绿色激光绿色激光点要在2秒内找到并追上红色激光点两个光斑中心距离≤3cm红色激光点运动时绿色激光点要一直跟着跟丢了要扣分两个系统都要有“暂停键”一按就停方便裁判测量硬件摆放要求这个很容易被忽略红色和绿色激光笔都装在独立的二维云台上就是能上下左右转的那种绿色激光笔要放在红色激光笔两侧距离40cm到1米之间屏幕是白色的大小至少60cm×60cm注意题目要求绿色激光笔可以放在指定线段上的任意位置这意味着你的追踪算法不能依赖于固定的相对位置必须能适应不同的摆放位置。1.2 我们的方案选择当时我们团队讨论了几个方案最后选定了下面这个组合主控芯片STM32F407VGT6为什么选它168MHz的主频够快1MB的Flash和192KB的RAM够大最重要的是它有12个16位ADC和32个DAC外设资源丰富价格也合适。操作系统FreeRTOS裸机编程行不行行但不好维护。我们这个系统要同时处理读取OpenMV数据、控制两个云台、处理按键、更新显示……这么多任务用FreeRTOS可以很方便地管理每个任务独立运行互不干扰。视觉模块OpenMV为什么不直接用OpenCV在电脑上处理因为比赛要求是独立系统不能连接电脑。OpenMV是个嵌入式视觉模块自带摄像头和处理器可以直接在模块上跑图像识别算法然后通过串口把结果发给STM32。机械结构自己搭建的铝型材框架这里有个坑题目要求绿色激光笔距离红色激光笔0.4-1米而且位置不固定。如果随便用个三脚架每次调试都要重新对位置太麻烦了。我们做了个带滑动导轨的框架绿色激光笔可以在导轨上滑动轻松满足距离要求。电路设计自己画板子很多同学喜欢用杜邦线连接各种模块方便是方便但比赛现场一碰就松稳定性太差。我们选择自己设计电路板把所有模块都固定在一块板上再用铝型材做个外壳既美观又稳固。2. 硬件设计与搭建2.1 核心板选型与最小系统我们用的是STM32F407VET6基于ARM Cortex-M4内核。这块芯片有几个特点特别适合我们这个项目速度快168MHz主频处理PID运算、串口通信绰绰有余内存大1MB Flash存程序192KB RAM跑FreeRTOS和各个任务外设丰富12个16位ADC虽然我们没用上、多个定时器用来产生PWM控制舵机、多个串口和OpenMV通信我们直接用了mcauser的开源开发板设计如果你也想用可以在GitHub上找到https://github.com/mcauser/MCUDEV_DEVEBOX_F407VET6提示开源板子的原理图和PCB都有你可以直接打板使用也可以根据自己的需求修改。我们就是在它的基础上增加了电源模块和扩展接口。2.2 电源系统设计电源是系统的“心脏”一定要设计好。我们的供电方案是这样的12V锂电池 → DC-DC降压 → 5V/6V → 各模块供电具体电路设计第一路12V转5V给OpenMV模块供电OpenMV需要5V给舵机供电大部分舵机工作电压4.8V-6V5V刚好第二路12V转6V备用电源如果需要更高扭矩的舵机可以用6V供电第三路5V转3.3V这个一般开发板自带的LDO线性稳压器就能搞定给STM32、OLED屏等3.3V器件供电保护电路这个很重要可恢复保险丝每个电压节点都加万一短路了能自我保护防反接电路用N-MOS管实现电池接反了不会烧板子稳压二极管在5V输出端加个5.1V的稳压管过压时钳位保护电源部分的原理图长这样2.3 OpenMV连接与通信OpenMV是我们的“眼睛”它负责识别红色激光点和绿色激光点在屏幕上的位置。连接方式很简单供电用XH 2.54mm 4Pin接口从我们的电源板取5V给OpenMV通信用串口UARTOpenMV的TX接STM32的RXRX接TX固定把OpenMV用螺丝固定在铝型材框架上确保摄像头正对屏幕这里有个大坑要注意我们设计时留了两个串口UART1和UART3。UART1本来想接蓝牙模块做无线调试UART3接OpenMV。结果调试时发现STM32F407的UART1默认是用于系统调试的有些库函数会占用它。最后我们改用UART3和OpenMV通信一切正常。所以如果你也遇到串口通信不正常先检查是不是串口冲突了。我们的连接实物2.4 云台与舵机选择云台就是能让激光笔上下左右转动的机械结构。我们用的是最常见的二自由度云台X轴舵机控制左右转动偏航Y轴舵机控制上下转动俯仰舵机选型建议扭矩至少15kg·cm因为要带动激光笔和OpenMV自动追踪系统的OpenMV是装在云台上的速度0.1-0.15秒/60°为宜太快了容易抖动太慢了跟不上精度这是最大的痛点很多舵机标称精度高实际用起来有“回差”和“不连续”问题关于舵机精度我多说几句我们测试了好几种舵机从几十块的到几百块的都试过发现一个共性问题当你均匀调整PWM占空比时舵机转动并不是均匀的会有累积误差转到某些位置还会“抽搐”。这不是我们程序的问题是舵机本身的机械结构决定的。如果预算充足建议用步进电机或者FOC磁场定向控制电机精度和稳定性好很多。安装技巧 装舵机时一定要先找到“机械中点”。就是让激光笔正对屏幕中心时舵机的位置。把这个位置设为0°往左转是负角度往右转是正角度。这样控制起来逻辑清晰。3. 软件架构与实现3.1 FreeRTOS任务划分用操作系统的好处就是可以把复杂问题分解。我们的系统主要跑这几个任务任务1OpenMV通信任务优先级最高每隔20ms读取一次OpenMV发来的数据数据包括红色激光点坐标、绿色激光点坐标、识别状态把数据放到队列里供其他任务使用任务2红色激光控制任务根据题目要求控制红色激光点运动四种模式复位回原点、沿屏幕边线运动、沿A4纸边框运动、暂停用PID算法计算舵机应该转的角度任务3绿色激光追踪任务读取红色激光点位置计算绿色激光点与红色激光点的偏差用PID控制绿色激光云台让绿色点追上红点任务4人机交互任务扫描按键处理“开始”、“暂停”、“模式切换”在OLED屏上显示当前状态、坐标、误差等信息任务5声光提示任务控制蜂鸣器和LED在追踪成功、失败、暂停时给出提示任务间的通信用FreeRTOS的队列和信号量这样各个任务独立运行互不干扰。主程序流程图如下3.2 OpenMV视觉识别OpenMV上跑的是MicroPython主要做两件事1. 识别红色激光点import sensor, image, time # 初始化摄像头 sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QVGA) # 320x240 sensor.skip_frames(time 2000) # 颜色阈值红色激光点 red_threshold (30, 100, 15, 127, 15, 127) # LAB颜色空间 while(True): img sensor.snapshot() # 寻找红色色块 blobs img.find_blobs([red_threshold], pixels_threshold10, area_threshold10) if blobs: # 取最大的色块假设是激光点 largest_blob max(blobs, keylambda b: b.pixels()) x largest_blob.cx() # 中心x坐标 y largest_blob.cy() # 中心y坐标 # 通过串口发送给STM32 uart.write(R,%d,%d\n % (x, y))2. 识别绿色激光点在自动追踪系统中绿色激光点的识别代码类似只是颜色阈值不同。但这里有个技巧因为红色激光点也在屏幕上OpenMV要同时识别红绿两个点然后计算它们的距离。# 同时识别红绿两个点 red_blobs img.find_blobs([red_threshold], mergeTrue) green_blobs img.find_blobs([green_threshold], mergeTrue) if red_blobs and green_blobs: red red_blobs[0] green green_blobs[0] # 计算两个光斑中心的距离像素 dx red.cx() - green.cx() dy red.cy() - green.cy() distance (dx*dx dy*dy)**0.5 # 发送给STM32红色坐标、绿色坐标、距离 uart.write(B,%d,%d,%d,%d,%.1f\n % (red.cx(), red.cy(), green.cx(), green.cy(), distance))注意OpenMV的坐标系原点在左上角x向右增加y向下增加。而我们的云台控制需要的是以屏幕中心为原点的坐标系。所以收到坐标后要做一个转换x_centered x_pixel - 160假设图像宽度320y_centered 120 - y_pixel假设高度240这样向上为正。3.3 PID控制算法实现PID是让云台平稳、准确运动的关键。我们用的是位置式PID分别控制X轴和Y轴。PID结构体定义typedef struct { float Kp; // 比例系数 float Ki; // 积分系数 float Kd; // 微分系数 float target; // 目标值目标角度 float actual; // 实际值当前角度 float error; // 当前误差 float last_error; // 上次误差 float prev_error; // 上上次误差用于微分 float integral; // 积分项 float output; // 输出值PWM占空比 float integral_limit; // 积分限幅 float output_limit; // 输出限幅 } PID_Controller;PID计算函数float PID_Calculate(PID_Controller *pid, float target, float actual) { pid-target target; pid-actual actual; // 计算误差 pid-error pid-target - pid-actual; // 比例项 float p_out pid-Kp * pid-error; // 积分项带限幅防饱和 pid-integral pid-error; if (pid-integral pid-integral_limit) pid-integral pid-integral_limit; if (pid-integral -pid-integral_limit) pid-integral -pid-integral_limit; float i_out pid-Ki * pid-integral; // 微分项用一阶后向差分 float d_out pid-Kd * (pid-error - pid-last_error); // 更新误差记录 pid-prev_error pid-last_error; pid-last_error pid-error; // 计算总输出并限幅 pid-output p_out i_out d_out; if (pid-output pid-output_limit) pid-output pid-output_limit; if (pid-output -pid-output_limit) pid-output -pid-output_limit; return pid-output; }PID参数整定经验先调P比例让系统能快速响应但会有震荡。慢慢加大P值直到出现轻微震荡。再调D微分加入D项抑制震荡。D值太大会让系统反应迟钝太小抑制不了震荡。最后调I积分消除静态误差。如果云台总是差一点到不了目标位置就加一点I值。我们的经验值Kp0.8, Ki0.01, Kd0.5是个不错的起点具体还要根据你的舵机响应速度调整。3.4 运动轨迹生成红色激光点的运动轨迹有几种模式这里以“沿屏幕边线运动”为例// 定义屏幕边界单位角度根据实际标定 #define SCREEN_LEFT -30.0f // 屏幕最左边对应的云台角度 #define SCREEN_RIGHT 30.0f // 屏幕最右边 #define SCREEN_TOP 20.0f // 屏幕最上边 #define SCREEN_BOTTOM -20.0f // 屏幕最下边 // 沿屏幕边线顺时针运动 void move_along_border(void) { // 第一阶段从当前位置移动到左上角 move_to_point(SCREEN_LEFT, SCREEN_TOP); // 第二阶段从左到右上边线 for (float x SCREEN_LEFT; x SCREEN_RIGHT; x 0.5f) { set_red_target(x, SCREEN_TOP); vTaskDelay(10); // 每10ms移动一小步 } // 第三阶段从上到下右边线 for (float y SCREEN_TOP; y SCREEN_BOTTOM; y - 0.5f) { set_red_target(SCREEN_RIGHT, y); vTaskDelay(10); } // 第四阶段从右到左下边线 for (float x SCREEN_RIGHT; x SCREEN_LEFT; x - 0.5f) { set_red_target(x, SCREEN_BOTTOM); vTaskDelay(10); } // 第五阶段从下到上左边线 for (float y SCREEN_BOTTOM; y SCREEN_TOP; y 0.5f) { set_red_target(SCREEN_LEFT, y); vTaskDelay(10); } }关键技巧运动轨迹不是一次性发给舵机的而是一小步一小步地走。每走一步比如0.5度等10ms让PID有足够时间调整到位。这样运动更平滑不会抖动。4. 系统调试与优化4.1 摄像头标定这是最关键的步骤直接决定系统精度。标定的目的是建立“像素坐标”和“云台角度”的对应关系。标定步骤在屏幕上画一个5×5的网格共25个点手动控制云台让激光点依次对准每个网格点记录每个点对应的云台角度X轴和Y轴同时用OpenMV读取该点的像素坐标得到25组对应数据(像素X, 像素Y) ↔ (角度X, 角度Y)建立映射关系 最简单的是用线性插值。假设我们标定了四个角点左上角像素(0,0) → 角度(-30°, 20°)右上角像素(320,0) → 角度(30°, 20°)左下角像素(0,240) → 角度(-30°, -20°)右下角像素(320,240) → 角度(30°, -20°)那么对于任意像素点(x,y)对应的角度为float pixel_to_angle_x(float pixel_x) { // 像素x从0到320对应角度从-30°到30° return -30.0f (pixel_x / 320.0f) * 60.0f; } float pixel_to_angle_y(float pixel_y) { // 像素y从0到240对应角度从20°到-20°注意y轴方向反了 return 20.0f - (pixel_y / 240.0f) * 40.0f; }更精确的方法是用多项式拟合但线性插值对我们这个项目已经够用了。4.2 PID参数整定实战调PID是个耐心活我们当时调了整整两天。分享几个实用技巧1. 先单独调一个轴把Y轴固定只调X轴的PID。在屏幕上画一条水平线让激光点从左边移动到右边观察运动曲线。2. 用OLED显示实时数据我们在OLED上实时显示目标位置实际位置误差PID输出值 这样能直观看到系统响应。3. 分段调试先让激光点从A点快速移动到B点调响应速度再让激光点精确停在某个位置调稳态精度最后让激光点沿直线匀速运动调平滑性4. 常见问题及解决震荡严重P太大D太小。减小P或增大D。响应太慢P太小。增大P。总是到不了目标位置静态误差大。适当增加I。到目标位置后抖动积分饱和。给积分项加限幅。4.3 抗干扰处理实际环境中会有各种干扰环境光变化白天晚上光线不同影响颜色识别屏幕反光激光点在屏幕上会有光晕其他红色/绿色物体干扰应对措施动态阈值OpenMV开机时先采样环境光自动调整颜色阈值面积过滤只识别特定大小的色块激光点直径约1cm在1米距离下在图像中占多少像素要提前算好位置预测如果当前帧没识别到用上一帧的位置运动速度来预测当前位置中值滤波对连续多帧的位置取中值消除跳变// 简单的位置滤波 #define FILTER_SIZE 5 float x_buffer[FILTER_SIZE]; float y_buffer[FILTER_SIZE]; int buffer_index 0; // 添加新数据 x_buffer[buffer_index] new_x; y_buffer[buffer_index] new_y; buffer_index (buffer_index 1) % FILTER_SIZE; // 取中值 float get_median(float arr[], int size) { // 排序后取中间值 // ... 排序代码 ... return arr[size/2]; } float filtered_x get_median(x_buffer, FILTER_SIZE); float filtered_y get_median(y_buffer, FILTER_SIZE);5. 比赛现场注意事项5.1 机械结构稳定性“精度不够结构来凑”。机械结构的稳定性直接影响最终效果云台安装一定要锁紧不能有晃动。我们用的是带锁紧螺母的舵机臂。框架刚性铝型材要选厚实的连接处用角码加固。屏幕固定屏幕要垂直地面不能前倾或后仰。整体重心重心要低防止不小心碰到就晃动。5.2 电源管理比赛现场可能提供的电源不稳定要做好准备带足电池12V锂电池至少带两块一块用一块备。电压监测在程序中加入电压检测电压过低时报警。上电顺序先开总电源再开各个模块。关机时顺序相反。5.3 快速标定流程比赛时可能换场地需要重新标定。我们设计了一套5分钟快速标定流程摆好屏幕固定好设备运行“标定模式”激光点会自动移动到9个标定点四角四边中点中心在每个点暂停手动微调如果需要标定数据自动保存到Flash下次开机直接使用5.4 故障应急处理比赛时最怕出故障我们准备了几个应急预案OpenMV死机按复位键系统会自动重新初始化视觉模块。舵机卡住程序里设了软件限位不会一直输出PWM烧坏舵机。通信中断串口通信加了校验和超时重传。按键失灵除了实体按键我们还留了串口命令接口可以用电脑发送控制命令。6. 项目资源与源码我们把所有资料都开源了包括硬件部分原理图立创EDA格式PCB文件可直接打板元器件清单BOM表软件部分STM32完整工程Keil MDKOpenMV脚本MicroPythonFreeRTOS配置文件机械结构铝型材框架设计图3D打印件STL文件用于固定OpenMV、激光笔等获取方式GitHub仓库https://github.com/Darrenlv-usst/TI-CUP2023/tree/master百度网盘包含视频演示https://pan.baidu.com/s/1S5uNIWxGAjjT2FKS6pPTMw 提取码9riq演示视频可以在B站观看https://www.bilibili.com/video/BV1PH4y1D75c/这个项目我们从零开始做了两个月期间踩了无数坑也积累了很多嵌入式开发、机器视觉、控制算法的实战经验。最大的体会是嵌入式开发不能只写代码硬件、机械、算法要一起考虑。希望这份详细的教程能帮到正在准备电赛或做类似项目的同学。如果遇到问题欢迎在GitHub上提Issue我们会尽力解答。最后提醒一点比赛时心态很重要。我们第一次调试时绿色激光点死活追不上红色点后来发现是OpenMV的帧率设太高处理不过来。把帧率从60fps降到30fps问题就解决了。所以遇到问题不要慌一步步排查从硬件到软件从机械到算法总能找到原因。