基于STM32F103与OpenMV的舵机云台二维视觉追踪PID控制实现

简介：这是一份基于STM32F103的二维舵机云台PID追踪程序，适合正在做OpenMV视觉追踪、电磁炮或小型云台项目的嵌入式开发者。程序接收OpenMV返回的色块坐标，通过PID算法驱动双舵机实现目标跟踪，代码包含串口通信、舵机控制、PID调节等模块。压缩包共455个文件，以C语言源文件（151个c、131个h）为主，并有编译生成的o/d、配置与工程文件（uvprojx、uvoptx等），便于直接导入Keil工程查看；另有少量图片和文档辅助说明。包体约11.86MB，结构相对完整，但PID参数需根据实际硬件重新整定。目前已有8233人学习下载，适合具备一定嵌入式基础、想快速搭建二维追踪云台的开发者参考，可从中提取控制思路与框架，再结合自己的OpenMV程序联调。

1. 基于STM32F103的OpenMV舵机云台二维追踪PID程序：先跑通再谈参数

做过视觉追踪的人大概都有这种经历：OpenMV已经识别到红色小球了，STM32也接了舵机云台，把坐标直接转发过去，结果云台要么疯狂抖动，要么慢慢悠悠半天对不准目标。问题往往不在识别，而在控制。这套基于STM32F103的OpenMV舵机云台二维追踪PID程序，就是把OpenMV输出的像素坐标，处理成舵机可用的角度增量，再用位置式PID让两个舵机平顺地追上目标。它适合已经在学OpenMV和STM32、想快速搭一个二维视觉追踪云台验证PID效果的开发者，也适合电赛前期做方案验证。解压后能直接看到OpenMV端的Python控制脚本和STM32端的C工程，配合串口通信，一整条抓取、识别、控制链路都能跑起来。

2. 从像素偏差到舵机角度：二维追踪的控制链路

2.1 目标点的相对偏差：为什么要先归一化

OpenMV的find_blobs会返回色块的中心坐标(cx, cy)，图像分辨率常见是320×240。很多初学者直接把cx、cy发给STM32，让单片机去控制舵机，这是第一个翻车点。cx是一个0~320的绝对值，而舵机角度是0~180度，两者量纲不同，直接套用会带来两个问题：一是分辨率失配，一个像素的变化折算到角度上要么太小要么太大；二是目标在画面不同位置时，同样的像素偏移对应的实际角度变化并不均匀，控制响应会很怪。

常见做法是在OpenMV端先把像素偏差换算成归一化偏差，范围控制在-100到+100，代表目标相对画面中心的角度偏差。这样PID控制器处理的量纲一致，后续调整系数也更直观。下面这段是OpenMV端比较通用的偏差计算逻辑：

import sensor, image, time from pyb import UART sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QVGA) sensor.skip_frames(time=2000) sensor.set_auto_whitebal(False) uart = UART(3, 115200, timeout_char=1000) threshold = (0, 100, -50, 50, -50, 50) # 红色目标的LAB阈值 while True: img = sensor.snapshot() blobs = img.find_blobs([threshold], pixels_threshold=50, area_threshold=50, merge=True) if blobs: b = max(blobs, key=lambda x: x.area()) # 取最大色块 cx = b.cx() cy = b.cy() dx = cx - img.width() // 2 dy = cy - img.height() // 2 nx = dx / (img.width() // 2) * 100 # 归一化到 -100~100 ny = dy / (img.height() // 2) * 100 data = bytearray([0xAA, 0x55, int(nx), int(ny), 0x00]) uart.write(data)

这里dx是目标中心与画面中心的水平偏差，除以半宽得到比例，再乘100变成整型。这样STM32收到的就不是绝对坐标，而是“目标在当前画面中偏左或偏右多少”的相对量。同时一定要用max(blobs, key=lambda x: x.area())选最大色块，否则图像里有多个面积相近的色块时，跟踪点会来回跳，PID控制器会把这个跳变当成目标突变，输出剧烈波动。threshold里的LAB阈值得按你的实际目标颜色重新采，资源包里给的是红色值，只能当起点。

2.2 串口帧格式：帧头、数据、校验

上面代码里的数据帧是[0xAA, 0x55, nx, ny, sum]。0xAA和0x55是帧头，nx和ny是偏差值，最后一个字节是前面数据的累加和校验。为什么不用裸发三个字节？因为UART是字节流，没有帧边界，接收端必须靠帧头判断一帧从哪里开始。实际使用中如果波特率高、线缆长，偶尔会丢字节，少一个0x55后面的数据就全错了；加校验后，接收端发现累加和不对，直接丢弃这一帧，不会用错误的偏差驱动舵机。

STM32端接收，我一般用状态机实现，比在主循环里阻塞判断帧头要省资源：

uint8_t uart_buf[2]; uint8_t rx_state = 0; uint8_t sum = 0; volatile int16_t target_x = 0; volatile int16_t target_y = 0; void USART1_IRQHandler(void) { if (USART_GetITStatus(USART1, USART_IT_RXNE)) { uint8_t ch = USART_ReceiveData(USART1); switch (rx_state) { case 0: if (ch == 0xAA) rx_state = 1; break; case 1: if (ch == 0x55) { rx_state = 2; sum = 0; } else rx_state = 0; break; case 2: uart_buf[0] = ch; sum += ch; rx_state = 3; break; case 3: uart_buf[1] = ch; sum += ch; rx_state = 4; break; case 4: if (ch == sum) { target_x = (int8_t)uart_buf[0]; target_y = (int8_t)uart_buf[1]; } rx_state = 0; break; default: rx_state = 0; break; } } }

这里是STM32F103的USART1中断，波特率115200，8位数据位，1位停止位。几个关键参数要说清楚：rx_state是状态机当前所处的状态，0和1用来匹配帧头，2和3分别接收两个数据字节，4是校验字节。注意帧头不能是0x00或0xFF，否则串口空闲时容易误触发。另外target_x和target_y必须声明成volatile，因为它们在中断里被写、在主循环里被读，不加volatile可能会被编译器优化成寄存器缓存值，导致控制反应迟钝。

OpenMV端发送用的是UART(3)，对应STM32端要接USART1还是USART2，看硬件连线。资源包里默认接的是USART1，如果你的板子不同，要改中断入口和GPIO配置，别只改波特率。

2.3 舵机角度映射：把PID输出换算到PWM占空比

绝大多数模拟舵机是50Hz，即周期20ms，角度0度对应高电平0.5ms，180度对应高电平2.5ms。STM32F103用定时器产生PWM，假设设置ARR为1999，那么CCR设为50~250，就对应0.5~2.5ms。我一般把角度换算封装成一个函数，同时在入口做限幅：

#define SERVO_MIN_ANGLE 0 #define SERVO_MAX_ANGLE 180 #define SERVO_MIN_PWM 50 // 0.5ms #define SERVO_MAX_PWM 250 // 2.5ms uint16_t angle_to_pwm(uint16_t angle) { if (angle < SERVO_MIN_ANGLE) angle = SERVO_MIN_ANGLE; if (angle > SERVO_MAX_ANGLE) angle = SERVO_MAX_ANGLE; return SERVO_MIN_PWM + (angle * (SERVO_MAX_PWM - SERVO_MIN_PWM)) / 180; }

完整的链路是：dx/dy归一化 -> 串口帧 -> STM32解析 -> 目标角度 = 当前角度 + 系数 × 偏差 -> PID计算 -> PWM。注意有个“系数”不是PID参数，它决定像素偏移折算到目标角度变化的灵敏度。系数太大，目标稍微偏一点舵机就猛转，容易振荡；系数太小则跟不上目标。常见做法是先手动标定一次：把云台对到某个固定目标，记录图像偏差和实际舵机角度，算出每个像素对应多少度。这个标定值对准不准影响很大，不要跳过去。

3. STM32F103上的PID实现：程序结构与整定顺序

3.1 程序框架与任务划分

这个资源包解压后，OpenMV端是一个main.py，STM32端是标准库工程。F103用标准库还是HAL库都能跑，我更建议标准库，因为F103的标准库资料多，中断处理直接。控制逻辑并不复杂：主循环里读取共享变量target_x、target_y，分别计算两个舵机的目标角度，调用PID函数得到输出增量，最后更新PWM占空比。F103主频72MHz，跑两路PID加PWM输出，CPU占用很小，甚至可以再加一个OLED显示调试信息。

任务划分上，我习惯把串口接收放中断，把PID和PWM更新放主循环。不要在主循环里轮询串口，除非你能保证每次循环不会漏掉字节。中断里只做移帧和存数据，所有运算都在主循环做，这样逻辑清晰，也不容易出现并发问题。上电初期还要做一件事：把两个舵机的当前PWM强制设置到中位，也就是90度左右，再开放串口数据写target_x、target_y。如果不做这个，OpenMV和STM32上电时间不一致，STM32可能用0°或者180°作为初值，云台会突然甩头。

3.2 位置式PID代码与参数说明

这里用的是位置式PID，不是增量式。位置式输出是绝对值，适合舵机这种本身没有积分能力的执行器。结构体和计算函数如下：

typedef struct { float kp; float ki; float kd; float error; float last_error; float integral; float integral_limit; float output_limit; } PID; float PID_Calc(PID* pid, float target, float actual) { pid->error = target - actual; 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 output = pid->kp * pid->error + pid->ki * pid->integral + pid->kd * (pid->error - pid->last_error); pid->last_error = pid->error; // 输出限幅，限制云台最大转速 if (output > pid->output_limit) output = pid->output_limit; if (output < -pid->output_limit) output = -pid->output_limit; return output; }

调用时，target是当前舵机角度加上像素偏差折算出的目标角度，actual是舵机当前角度。实际项目中舵机通常没有角度反馈，actual可以取上一次输出角度作为位置反馈，也就是开环位置控制加PID闭环修正。如果你装了电位器或编码器，再把真实角度反馈进来，控制效果会更好。

初始化参数时，我给一组经验初值：kp=0.5，ki=0.01，kd=0.05，integral_limit=20，output_limit=30。这组值不是标准答案。资源包作者的平台和你的平台一定不一样，给出来的目的只是让你第一次上电不要飞车。output_limit=30的意思是PID输出最多让舵机角度目标每次调整30度，避免目标突变时舵机猛甩；如果后面发现响应太慢，再逐步放大。

3.3 为什么PID参数不能照抄

因为舵机型号、供电电压、云台重心完全不一样。9g舵机、MG996R舵机、数字舵机，它们的响应速度、死区、齿隙差距很大；供电电压从4.8V到7.4V，舵机响应速度也会变化。同一个参数在作者平台上稳定，换到你板子上可能振荡，也可能软绵绵。资源包里的参数只能作为起点。

整定顺序我一般这样走：先令ki=0、kd=0，只保留kp，从0.2开始往大加。让云台追踪一个缓慢移动的目标，当云台出现持续等幅振荡时，记下这个临界kp，乘以0.6作为基础kp。然后加入ki，从0.005开始加，观察目标静止时云台是否还能缓慢漂移到目标正中间，消除静差。最后加kd，从0.02开始加，观察快速移动目标时云台有没有超调回弹。一套参数调下来大概需要半小时到一小时，别指望一次上电就稳。资源包里的代码带了一个串口打印输出，可以直接看PID的error和output，配合串口助手作图来调，比盲调效率高得多。

4. 调试流程：分四步跑通二维追踪

4.1 第一步：串口链路自检，先别接舵机

很多人拿到资源包第一件事就是接上舵机跑，结果舵机乱抖，搞不清楚是PID问题还是通信问题。我习惯先把舵机断开，用USB-TTL把OpenMV接到电脑，再用串口助手看STM32发出来的调试数据。如果OpenMV端能正常输出AA 55 xx xx sum，并且STM32的target_x、target_y跟着变，说明通信链路是通的。

这一步还可以顺带验证校验逻辑：在串口助手里故意发一个错误校验字节，看STM32是否丢弃这一帧。做法是在中断里加一个计数器，收到合法帧加1，收到校验错误帧加1，两个数字都打印出来。如果错误帧比例高于1%，优先查波特率误差和接线，而不是调PID。

4.2 第二步：静态目标追踪，先用手移动目标

把云台装好，目标固定在桌面，不要让目标乱动。此时target_x、target_y基本恒定，云台应该能快速收敛到目标中心。观察两点：一是云台是否停在目标上不动，如果缓慢绕目标摆动，说明ki偏大；二是云台停止时是否有高频抖动，如果有，多半是kd放大噪声或者舵机死区过小。静态收敛正常后，再用手慢慢平移目标，看云台能不能平滑跟随。跟不上时，优先加大kp和output_limit，不要急着动ki。

4.3 第三步：动态追踪，用四个现象定位参数

动态追踪时的现象可以分成四类，对应不同的参数倾向。我整理了一个简单的排查表：

这个表对新手够用了。注意调整要一次只改一个参数，改完记录现象，别同时动两个，否则出了问题你根本不知道是谁引起的。我见过有人在半小时里把kp改了十次、ki改了八次，最后云台还是抖，实际上是因为舵机供电电压不够，一抖电压就掉，形成恶性循环。先从电源排查起。

4.4 第四步：目标丢失策略，避免云台飞出边界

OpenMV一帧识别不到目标时，发的是nx=0, ny=0。如果STM32把这个当成目标在正中间，云台会自己回到中位，然后目标又出现，云台再追过去，看起来就像发了神经一样来回扫。更好的策略是OpenMV端在目标丢失时发送一个特定的丢失标志，比如把nx设为127，或者单独用一字节协议位。STM32端收到丢失标志后不更新PID目标，保持当前角度一段时间，比如0.5秒，如果持续丢失再缓慢回中位。

资源包里默认的做法是发送全0帧，STM32端可以加一个丢失计时器：

uint16_t lost_count = 0; if (target_x == 0 && target_y == 0) { lost_count++; if (lost_count < 30) { // 短时间内视为目标暂时遮挡，保持当前角度 } else { // 持续丢失超过0.5秒，缓慢回到90度中位 } } else { lost_count = 0; }

这里的30是主循环执行30次，按主循环频率计算。比如主循环跑100Hz，30次就是0.3秒。具体数值看你主循环实际频率，不要照抄。

5. 避坑指南：二维云台追踪最容易翻车的五个细节

5.1 上电瞬间云台猛转到底

现象：STM32和OpenMV同时上电，云台突然甩到0度或180度极限位置，伴随异响。

原因：STM32先启动，OpenMV后启动，串口还没数据时target_x、target_y默认是0，PID目标角度被算成0度，舵机直接转过去。或者OpenMV启动时图像还没稳定，识别到干扰色块，发送了一个极端偏差。

解决：STM32上电先初始化舵机PWM到中位，再初始化串口，并且串口收到第一帧合法数据之前，不使能PID输出。代码上加一个enable_flag，只有在收到完整帧后才置1，主循环里判断该标志。

5.2 舵机挂载后高频抖动

现象：目标静止时舵机以几十赫兹的频率来回小幅度抖动，发出嗡嗡声。

原因：kd项放大了角度反馈噪声，或者kp太大导致边界振荡；也可能是舵机本身的死区太小，PID输出在目标点附近反复横跳，舵机跟着来回修正。

解决：先减小kp，让云台不要那么“灵敏”；再适当增大kd抑制超调。如果还抖，在PID输出加一个死区，偏差绝对值小于1度时不更新PWM。很多通用PID库里死区就是一个if判断，但资源包默认没有，需要自己加。

5.3 目标快速移动时云台跟不上

现象：目标从画面一侧快速移到另一侧，云台明显滞后，甚至被甩在画面边缘。

原因：PID输出限幅output_limit太小，云台最大转速被限制住了；也可能是OpenMV帧率太低，比如默认的QVGA RGB565在复杂背景下只有30fps左右。

解决：先检查output_limit，如果设成30，加大到50甚至80再试。注意加大后要重新观察静态稳定性，输出限幅同时影响快速性和稳定性。帧率方面，把图像分辨率降到QQVGA，或者关闭自动白平衡，都能提升OpenMV处理帧率。

5.4 串口丢帧导致云台偶发乱跳

现象：云台大部分时间正常，突然在某几个时刻向随机方向猛转一下，又立刻恢复。

原因：UART丢了一个字节，校验没通过时接收状态机会重新找帧头。如果下一帧恰好以0xAA开头，两帧数据交叉解析，nx、ny就乱了。

解决：优先检查接线和波特率精度。F103内部时钟用HSI时，波特率可能偏差1%，115200下累计误差更容易丢字节；换外部晶振或者把波特率降到57600，情况立刻好转。再把状态机改成必须连续两帧校验通过才更新target_x、target_y，能滤掉偶发错帧。

5.5 目标短暂丢失后云台乱扫

现象：目标被手遮挡一下，云台就从中位猛扫到另一侧，或者连续反复摆动。

原因：OpenMV在目标丢失时发全0帧，STM32把0当成“目标在正中间”的偏差，PID立即把云台拉回中位；目标重新出现时再拉回去，形成来回扫动。

解决：按前面说的，增加丢失标志位和丢失计时器。目标丢失后先保持当前角度，持续丢失超过一定时间再缓慢回中位，回中位的过程也要受output_limit限制，不能一步到位。这个细节直接影响追踪观感，这类视觉追踪项目中目标短暂遮挡是常态。

6. 进阶用法：死区、前馈与目标丢失回中，让云台更“跟手”

6.1 加一个死区

死区不是给PID代码强行加一个if那么简单。死区应该加在PID输出目标角度的差值上，而不是加在像素偏差上。比如当前舵机角度和目标角度差小于1度，就不更新PWM。这样舵机不会在高频抖动中消耗寿命。资源包的默认PID没有死区，我一般会加在PID_Calc调用前：

float angle_error = target_angle - current_angle; if (angle_error > -1.0f && angle_error < 1.0f) { // 误差在死区内，保持当前PWM不变 return; }

死区取多大要看舵机精度，9g舵机死区可能在1度左右，大扭力舵机死区更大。死区设太大，目标略偏移时云台不动，画面里目标会一直差一点；设太小，抖动过滤不了。从1度开始试，逐步加大。

6.2 加一点前馈

纯PID是滞后修正，目标移动越快，滞后越大。进阶做法是根据目标的移动速度加前馈：OpenMV端可以估算目标在两帧之间的像素位移，把位移值也打包进串口帧；STM32端在PID输出基础上加一个与目标速度成正比的偏移量。简单实现如下：

previous_cx = cx ... velocity_x = cx - previous_cx previous_cx = cx

然后把velocity_x发送到STM32，STM32端做角度折算，叠加到PID输出上。这个前馈增益要标定，一般取PIDkp的0.1到0.2倍。加完之后，云台在追快速运动目标时的滞后明显改善。

6.3 验证方法：画一条运动轨迹

调完参数后，为了确认效果，我常用OpenMV的image.draw_line把目标历史轨迹画出来。让目标走一个“8”字，观察轨迹是否连续、是否存在明显锯齿。如果轨迹光滑但滞后，加前馈；如果轨迹抖动，说明死区或kd还不够好。这套机制调通之后，云台手感会从“机械式点对点”变成“丝滑跟手”。

资源包的代码结构很清晰，扩展前馈、死区、丢失策略都很容易。ESD和接线注意一下，串口线不要太长，舵机电源一定要独立供电，别和单片机共用一个LDO，否则舵机一扭，电压跌落，OpenMV很容易复位。从那以后我每次搭这种视觉追踪Demo，都强制把电源分开、上电先回中位、串口校验加满，调试效率提升一大截。希望这些踩坑经验帮你也少走几步弯路。

本文还有配套的精品资源，点击获取