OpenMV与STM32联动的视觉追踪系统实战：从颜色识别到云台控制

在创客和机器人爱好者圈子里，视觉追踪一直是个热门话题。想象一下，你的设备能够自动识别并追踪一个红色小球，通过云台转动始终保持目标在视野中央——这种"看得见、跟得上"的能力，正是许多智能项目的基础。本文将带你用OpenMV和STM32搭建一套完整的视觉追踪系统，从颜色阈值设定到PWM信号生成，实现"感知-决策-执行"的闭环控制。

1. 硬件选型与系统架构设计

选择适合的硬件是项目成功的第一步。我们需要兼顾性能、成本和易用性，构建一个高效的视觉处理与运动控制平台。

核心硬件组件：

• OpenMV Cam H7：搭载STM32H743VI芯片，主频480MHz，支持Python编程
• STM32F103C8T6最小系统板：72MHz Cortex-M3内核，PWM输出能力强
• SG90舵机云台：两个自由度，扭矩1.6kg/cm，响应速度快
• 红色目标物体：直径3-5cm的乒乓球最佳

硬件连接示意图：

OpenMV TX(P4) —— STM32 RX(PB11)
OpenMV RX(P5) —— STM32 TX(PB10)
STM32 PWM1 —— 云台水平舵机
STM32 PWM2 —— 云台垂直舵机

注意：OpenMV和STM32之间需要通过3.3V电平通信，避免使用5V TTL模块，防止损坏OpenMV的串口。

系统工作流程分为三个主要阶段：

1. 视觉感知层 ：OpenMV采集图像，识别目标颜色，计算位置信息
2. 数据传输层 ：通过串口将坐标数据打包发送给STM32
3. 运动控制层 ：STM32解析数据，生成PWM信号驱动云台

2. OpenMV端的视觉处理实现

OpenMV的强项在于其开箱即用的计算机视觉功能。我们通过MicroPython编程，可以快速实现颜色识别和特征提取。

2.1 基础环境配置

首先初始化摄像头和串口：

import sensor, image, time, pyb
from pyb import UART

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)  # 色彩格式
sensor.set_framesize(sensor.QVGA)    # 分辨率320x240
sensor.skip_frames(time=2000)        # 跳过初始不稳定帧
sensor.set_auto_gain(False)          # 关闭自动增益
sensor.set_auto_whitebal(False)      # 关闭自动白平衡

# 初始化串口3
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)

2.2 颜色阈值设定技巧

颜色识别效果直接取决于阈值设置。推荐使用OpenMV IDE中的阈值编辑器：

1. 将目标物体放在典型环境下
2. 打开Tools→Machine Vision→Threshold Editor
3. 调整LAB三个通道的阈值范围，确保只选中目标区域

对于红色物体，典型的LAB阈值范围可能是：

L: [20, 100]
A: [30, 80]
B: [-20, 30]

提示：环境光照变化会影响颜色识别，建议在固定光源下测试，或增加自动阈值调整算法。

2.3 目标检测与数据打包

检测到目标后，我们需要提取其特征信息并通过串口发送：

def find_max_blob(blobs):
    max_size = 0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    return max_blob

def send_data(cx, cy, cw, ch):
    data = bytearray([
        0x2C, 0x12,        # 帧头
        cx & 0xFF,         # X坐标低字节
        (cx >> 8) & 0xFF,  # X坐标高字节
        cy & 0xFF,         # Y坐标低字节
        (cy >> 8) & 0xFF,  # Y坐标高字节
        0x5B               # 帧尾
    ])
    uart.write(data)

while True:
    img = sensor.snapshot()
    blobs = img.find_blobs([red_threshold])
    
    if blobs:
        target = find_max_blob(blobs)
        img.draw_rectangle(target.rect())  # 绘制矩形框
        img.draw_cross(target.cx(), target.cy())  # 绘制中心十字
        
        # 发送坐标数据
        send_data(target.cx(), target.cy(), target.w(), target.h())

3. STM32端的串口通信实现

STM32需要可靠地接收OpenMV发送的数据，并解析出目标位置信息。

3.1 串口初始化配置

使用STM32CubeMX配置USART3：

void MX_USART3_UART_Init(void)
{
  huart3.Instance = USART3;
  huart3.Init.BaudRate = 115200;
  huart3.Init.WordLength = UART_WORDLENGTH_8B;
  huart3.Init.StopBits = UART_STOPBITS_1;
  huart3.Init.Parity = UART_PARITY_NONE;
  huart3.Init.Mode = UART_MODE_TX_RX;
  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart3) != HAL_OK)
  {
    Error_Handler();
  }
}

3.2 自定义通信协议解析

为增强通信可靠性，我们设计了一个简单协议：

• 帧头：0x2C 0x12
• 数据：cx(2字节), cy(2字节)
• 帧尾：0x5B

中断接收处理代码：

#define BUF_SIZE 32
uint8_t rx_buf[BUF_SIZE];
uint16_t cx = 0, cy = 0;

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    static uint8_t state = 0;
    static uint8_t index = 0;
    
    if(huart->Instance == USART3)
    {
        uint8_t data = rx_buf[0];
        
        switch(state)
        {
        case 0: // 等待第一个帧头
            if(data == 0x2C) state = 1;
            break;
            
        case 1: // 等待第二个帧头
            if(data == 0x12) state = 2;
            else state = 0;
            break;
            
        case 2: // 接收数据
            if(index < 4) // 接收4字节数据
            {
                if(index % 2 == 0)
                    cx = data;
                else
                    cy = data;
                index++;
            }
            else if(data == 0x5B) // 帧尾
            {
                // 触发控制逻辑
                control_servo(cx, cy);
                state = 0;
                index = 0;
            }
            break;
        }
        
        HAL_UART_Receive_IT(&huart3, rx_buf, 1);
    }
}

4. 云台控制与PID调节

获得目标坐标后，需要将其转换为舵机角度，实现平滑追踪。

4.1 PWM舵机驱动配置

STM32的定时器产生PWM信号：

void MX_TIM2_Init(void)
{
  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 71; // 1MHz时钟
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 19999; // 20ms周期
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  
  if (HAL_TIM_Base_Init(&htim2) != HAL_OK) Error_Handler();
  
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK) Error_Handler();
  
  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK) Error_Handler();
  
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) Error_Handler();
  
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 1500; // 初始1.5ms
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) Error_Handler();
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) Error_Handler();
  
  HAL_TIM_MspPostInit(&htim2);
}

4.2 位置映射与PID控制

将图像坐标转换为舵机角度：

// 图像坐标系到舵机角度的映射
void map_position_to_angle(uint16_t cx, uint16_t cy, float *pan_angle, float *tilt_angle)
{
    // 水平方向(pan)
    *pan_angle = (cx - 160) * 0.1f; // 每像素0.1度
    
    // 垂直方向(tilt)
    *tilt_angle = (120 - cy) * 0.1f; // 图像坐标系Y轴向下
}

实现简单的PID控制器：

typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
} PID_Controller;

void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->integral = 0;
    pid->prev_error = 0;
}

float PID_Update(PID_Controller *pid, float setpoint, float measurement, float dt)
{
    float error = setpoint - measurement;
    pid->integral += error * dt;
    float derivative = (error - pid->prev_error) / dt;
    pid->prev_error = error;
    
    return pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
}

4.3 云台控制主逻辑

结合位置映射和PID控制：

PID_Controller pan_pid, tilt_pid;

void control_servo(uint16_t cx, uint16_t cy)
{
    static float pan_angle = 0, tilt_angle = 0;
    static uint32_t last_time = 0;
    
    // 计算时间间隔(ms)
    uint32_t current_time = HAL_GetTick();
    float dt = (current_time - last_time) / 1000.0f;
    last_time = current_time;
    
    // 目标角度
    float target_pan, target_tilt;
    map_position_to_angle(cx, cy, &target_pan, &target_tilt);
    
    // PID计算
    float pan_output = PID_Update(&pan_pid, target_pan, pan_angle, dt);
    float tilt_output = PID_Update(&tilt_pid, target_tilt, tilt_angle, dt);
    
    // 更新角度
    pan_angle += pan_output * dt;
    tilt_angle += tilt_output * dt;
    
    // 转换为PWM脉宽(500-2500us)
    uint16_t pan_pulse = 1500 + pan_angle * 11.11f; // 11.11 = 1000us/90°
    uint16_t tilt_pulse = 1500 + tilt_angle * 11.11f;
    
    // 限制范围
    pan_pulse = (pan_pulse < 500) ? 500 : (pan_pulse > 2500) ? 2500 : pan_pulse;
    tilt_pulse = (tilt_pulse < 500) ? 500 : (tilt_pulse > 2500) ? 2500 : tilt_pulse;
    
    // 更新PWM
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pan_pulse / (20000 / 65536));
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, tilt_pulse / (20000 / 65536));
}

5. 系统调试与性能优化

一个稳定的视觉追踪系统需要反复调试和优化。以下是几个关键调试环节：

5.1 串口通信调试技巧

1. 使用逻辑分析仪 ：验证串口波形是否符合预期
2. 添加校验和 ：在协议中增加校验字节，提高可靠性
3. 错误恢复机制 ：当连续多次接收失败时，重置系统状态

改进后的数据帧格式：

[0x2C][0x12][cx_l][cx_h][cy_l][cy_h][checksum][0x5B]

校验和计算示例：

def calculate_checksum(data):
    return sum(data) % 256

# 在发送前计算
checksum = calculate_checksum([cx & 0xFF, (cx >> 8) & 0xFF, 
                              cy & 0xFF, (cy >> 8) & 0xFF])

5.2 PID参数整定方法

1. 先调P ：逐步增大Kp直到系统开始振荡，然后取该值的50%
2. 再调D ：增加Kd抑制振荡，通常为Kp的1/10到1/4
3. 最后调I ：小幅增加Ki消除静差

典型参数范围：

水平轴：Kp=2.0, Ki=0.05, Kd=0.3
垂直轴：Kp=1.5, Ki=0.03, Kd=0.2

5.3 性能优化策略

1. 图像处理优化 ：

   • 降低分辨率到QQVGA(160x120)
   • 使用ROI(Region of Interest)只处理图像中心区域
   • 开启OpenMV的JPEG压缩

2. 通信优化 ：

   • 降低发送频率(如10Hz)
   • 使用差分发送(只发送坐标变化量)

3. 控制优化 ：

   • 加入死区控制，微小变化不响应
   • 实现速度前馈控制

优化后的主循环：

# 设置ROI(中心区域)
roi = (80, 60, 160, 120)  # x,y,w,h

while True:
    img = sensor.snapshot()
    blobs = img.find_blobs([red_threshold], roi=roi, merge=True)
    
    if blobs:
        target = find_max_blob(blobs)
        # 只有当位置变化超过阈值时才发送
        if abs(target.cx() - last_cx) > 5 or abs(target.cy() - last_cy) > 5:
            send_data(target.cx(), target.cy())
            last_cx, last_cy = target.cx(), target.cy()
    
    time.sleep_ms(100)  # 控制发送频率

6. 项目扩展与进阶方向

基础视觉追踪系统完成后，可以考虑以下扩展方向：

6.1 多目标识别与追踪

修改颜色识别代码，支持多个颜色标签：

# 定义多个颜色阈值
thresholds = [
    (30, 100, 40, 80, -20, 30), # 红色
    (20, 80, -50, -20, 10, 50)  # 绿色
]

# 识别时指定多个阈值
blobs = img.find_blobs(thresholds, merge=False)

6.2 加入距离估计

通过目标在图像中的大小估算实际距离：

# 已知目标实际直径和焦距
KNOWN_DIAMETER = 4.0  # cm
FOCAL_LENGTH = 2.8    # mm

def estimate_distance(pixel_width):
    return (KNOWN_DIAMETER * FOCAL_LENGTH) / (pixel_width * 0.0176)  # 0.0176mm/像素

6.3 与ROS集成

通过串口转发数据到ROS系统：

1. 在STM32上实现USB CDC虚拟串口
2. 使用rosserial_python包建立连接
3. 发布geometry_msgs/Pose消息

示例ROS节点：

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Pose

def serial_callback(data):
    pose = Pose()
    pose.position.x = data[0]  # cx
    pose.position.y = data[1]  # cy
    pub.publish(pose)

rospy.init_node('openmv_tracker')
pub = rospy.Publisher('target_pose', Pose, queue_size=10)
rospy.Subscriber('/serial', Float32MultiArray, serial_callback)
rospy.spin()

在实际项目中，我发现云台机械结构的刚性对追踪效果影响很大。廉价舵机存在的回差问题会导致追踪时出现抖动，建议使用数字舵机并做好机械固定。另外，环境光照变化是颜色识别最大的挑战，可以考虑加入自适应阈值算法或切换到基于特征的追踪方法。