开篇：基于 Cortex-M7、OpenMV 与双频 GNSS 的智能派件 AGV 设计与实现

原创于 2026-08-21 10:25:00 发布 · 置顶 · 1k 阅读

本内容遵循CC 4.0 BY-SA版权协议

话题

系统以 Stellar-SR5E1E3（Cortex-M7）为主控，融合双频 GNSS、IMU、轮式编码器与 OpenMV 视觉模块，实现了“航点巡航—近场寻人—视觉跟随—身份核验—任务结束”的完整闭环。相比单独调通某个传感器，本项目更大的难点在于多种控制模式之间的交接，以及真实电机、舵机和通信链路在极端工况下的可靠性。

1. 项目简介

传统小车项目往往只实现循迹、避障或单一视觉跟随。本项目希望模拟一个更完整的派件过程：车辆先依据 GNSS 航点自主驶向目标区域，到达后由 OpenMV 接管近距离搜索，利用二维云台锁定目标并牵引底盘靠近，最后完成人脸特征核验并安全驻车。

一次完整任务的执行流程如下：

在示教模式下记录目标点的 GNSS 坐标；
发车前检查定位状态，进入自动巡航；
使用 GNSS 计算目标距离和方位角，使用 IMU 估计车体航向；
通过外环航向 PD 与内环轮速 PID 控制差速底盘；
距离目标小于 2.5 m 后停车，并触发视觉模块接管；
OpenMV 搜索红色目标，二维云台进行 PD 跟踪，底盘随云台偏角联动；
进入 20～40 cm 的近距离区域后驻车，切换到 LBP 特征核验；
核验成功后关闭运动输出、云台归中，并通过指示灯提示任务完成。

硬件环境：

主控：Stellar-SR5E1E3，Cortex-M7，主频 300 MHz；
定位：TAU1201 双频 GNSS；
姿态：IMU660RB 六轴惯性传感器；
视觉：OpenMV 核心板；
执行机构：双轮差速底盘、轮式编码器、TB6612 电机驱动、二维舵机云台；
人机交互：IPS200 显示屏与板载按键；
开发环境：StellarStudio 7.0.0、OpenMV IDE；
编程语言：C、MicroPython。

核心技术点：

GNSS 绝对定位与 IMU 相对航向融合；
外环航向 PD + 内环轮速 PID 串级控制；
Cortex-M7 DWT 周期计数器动态测量控制周期；
OpenMV 红色目标检测、单目距离估计与 LBP 特征核验；
UART 接收中断 + 8 字节定长帧状态机解析；
GPS 与视觉之间基于事件标志的控制权交接；
速度斜坡、积分抗饱和、输出限幅与堵转保护。

2. 系统整体设计

系统采用“感知—决策—执行—反馈”的闭环架构。GNSS、IMU、编码器和 OpenMV 构成感知层；Cortex-M7 主控完成状态调度、导航解算和安全仲裁；电机与二维舵机云台构成执行层阶段由 GNSS 决定“车辆是否到达目标区域”，微观阶段由 OpenMV 决定“目标位于画面什么位置、距离是否合适、身份是否匹配”。两种模式不同时抢占底盘，而是通过事件标志完成控制权交接。

从软件角度看，系统可以划分为四个阶段：

T0：启动与预备。 初始化主控、显示屏、电机、编码器、IMU、GNSS、OpenMV 串口和二维云台；
T1：巡航导航。 根据目标方位角与当前航向角的差值控制底盘；
T2：视觉接管。 根据视觉状态完成扫描、跟随、驻车和核验；
T3：底层保护。 所有运动命令在下发前必须经过斜坡、限幅、PID 和堵转保护。

3. 主控软件架构

主控程序没有把所有逻辑堆在一个函数中，而是按职责拆分为传感器更新、导航任务、视觉任务、底盘任务和 UI 刷新。主循环以约 50 Hz 的频率执行：
int main(void)
{
    System_Init();

    for (;;)
    {
        Sensor_Update();        // GNSS、IMU、编码器与时间基准
        Navigation_Task();      // 宏观航点巡航
        Vision_Track_Task();    // 近场视觉追踪与云台控制
        Chassis_Task();         // 速度闭环、安全保护与电机输出
        UI_Update();            // 本地状态显示

        zf_delay_ms(20);        // 约 50 Hz 控制周期
    }
}
这种拆分的关键不只是代码更整齐，而是明确了控制权边界：导航任务和视觉任务只生成期望速度，真正的电机输出统一由 Chassis_Task() 完成。无论上层处于哪一种模式，底层保护始终有效。

4. GNSS + IMU 导航设计

4.1 为什么不能只依赖 GNSS 航向

GNSS 可以提供绝对经纬度，但低速、转弯和弱信号环境下，单靠相邻坐标估计车辆朝向不够稳定。IMU 更新频率高，适合提供短时间内连续的航向变化；因此系统使用 GNSS 计算“目标在哪个方向”，使用 IMU 估计“车头当前朝向哪里”。

系统首先根据当前坐标和目标坐标计算：
目标距离      nav_distance
目标方位角    nav_azimuth
航向误差      nav_yaw_error = nav_azimuth - yaw_angle
考虑到角度在 0°/360° 附近会发生跳变，航向误差最终被折叠到 [-180°, 180°]。例如目标方位为 359°、当前航向为 1° 时，正确误差应为 -2°，而不是 358°。

4.2 使用 DWT 动态测量积分时间

早期代码直接使用固定的 0.02 s 对陀螺仪积分，但屏幕刷新、串口输出和 GNSS 解析都会改变单次循环耗时。固定时间步长会把调度抖动不断累加到航向角中。

最终版本读取 Cortex-M7 的 DWT 周期计数器：
uint32 current_cycles = *(volatile uint32 *)0xE0001004;
real_dt = (float)(current_cycles - last_cycles) / 300000000.0f;

if (real_dt <= 0.0f || real_dt > 0.1f)
    real_dt = 0.02f;

last_cycles = current_cycles;
yaw_angle -= (gyro_z - gyro_z_offset) * real_dt;
在 300 MHz 主频下，一个计数周期对应约 3.33 ns 的时间分辨率。这里需要注意，3.33 ns 是计数器分辨率，并不等于整个姿态系统具有纳秒级测量精度；IMU 零偏、噪声和安装误差仍然会影响最终航向。

4.3 外环航向 PD

导航时，外环根据航向误差生成左右轮差速补偿：
int turn_comp = (int)(nav_yaw_error * turn_Kp
                    + (nav_yaw_error - last_nav_yaw_error) * turn_Kd);

if (turn_comp > 6)  turn_comp = 6;
if (turn_comp < -6) turn_comp = -6;
int base_speed = 10 - abs(turn_comp);

left_target_cmd  = base_speed + turn_comp;
right_target_cmd = base_speed - turn_comp;
与固定速度的 if-else 转向相比，base_speed = 10 - abs(turn_comp) 会让车辆在大角度修正时自动减速，在方向逐渐对准后恢复速度。微分项则用于抑制转向惯性和过冲。

当航向误差超过约 80° 时，系统不再一边前进一边转向，而是先降低前进速度并进行原地调整，避免车辆绕出过大的弧线。

5. 跨模态调度：从 GNSS 巡航切换到视觉接管

本项目最有特点的部分不是单独的 GPS 或视觉算法，而是二者之间的自动接力。

导航任务检测到 nav_distance <= 2.5 m 后，不直接调用视觉算法，而是停止底盘并置位事件标志：
left_target_cmd  = 0;
right_target_cmd = 0;
current_state = STATE_IDLE;
gps_arrived_flag = 1;
视觉任务统一处理人工按键与自动到达事件：
if (key_c9_click || gps_arrived_flag)
{
    key_c9_click = 0;
    gps_arrived_flag = 0;
    ptz_auto_mode = !ptz_auto_mode;
    mission_completed = 0;
}
这样做的好处是导航模块只负责发出“已经到达”的事实，不直接操作摄像头和云台；视觉模块也不需要了解 GNSS 解算过程，只需要响应一个清晰的接管事件。两个任务通过少量接口变量协同，降低了模块之间的耦合。

6. OpenMV 视觉追踪与身份核验

6.1 红色目标搜索

OpenMV 首先在 RGB565 图像中寻找满足阈值的红色色块，并选择面积最大的目标。目标中心与画面中心的偏差作为云台控制输入：
raw_x_error = max_blob.cx() - 80
raw_y_error = max_blob.cy() - 60

if abs(raw_x_error) < DEAD_ZONE:
    raw_x_error = 0
if abs(raw_y_error) < DEAD_ZONE:
    raw_y_error = 0

smooth_x_error = int(last_x_error * 0.3 + raw_x_error * 0.7)
smooth_y_error = int(last_y_error * 0.3 + raw_y_error * 0.7)
这里同时使用了死区和一阶平滑：

死区用于过滤目标已经接近画面中心时的小幅抖动；
一阶平滑用于减小色块边缘和环境噪声造成的像素跳变。

目标距离使用色块宽高的平均值进行单目近似估计：
Lm = (max_blob.w() + max_blob.h()) / 2
distance_cm = int(K_FACTOR / Lm) if Lm > 0 else 255
该方法不是高精度测距，但在目标尺寸相对固定的比赛场景中，可以用于划分“继续接近、保持距离、主动后退”三个区间。

6.2 云台与底盘联动

只让摄像头转向目标，会出现“云台看见了人，但车头没有跟上”的假追踪。因此主控还会把云台水平偏角映射为底盘差速补偿：
float chassis_turn_Kp = 0.2f;
int turn_comp = (int)(ptz_pan_angle * chassis_turn_Kp);
if (turn_comp > 15)  turn_comp = 15;
if (turn_comp < -15) turn_comp = -15;

left_target_cmd  = base_speed + turn_comp;
right_target_cmd = base_speed - turn_comp;
云台先快速把目标拉回视野中心，底盘再逐步转向云台朝向，最终使车身正对目标。相比直接用像素误差控制底盘，这种方式把快速视线调整与较慢的机械转向分成了两个时间尺度。

6.3 LBP 特征核验

当目标距离进入 20～40 cm 且水平误差进入死区后，OpenMV 向主控发送 Type = 2。主控驻车并调整云台俯仰角，OpenMV 随后切换为灰度图模式，提取当前画面的 LBP 描述子，与本地样本库逐一计算差异度。
d0 = img.find_lbp((0, 0, img.width(), img.height()))
d1 = img_ref.find_lbp((0, 0, img_ref.width(), img_ref.height()))
dist += image.match_descriptor(d0, d1)

if pmin < 9500:
    send_vision_data(4, 0, 0, 0)
    system_mode = 2
本方案的目标是展示本地闭环核验流程，并不等同于通用、高安全等级的人脸识别。LBP 对光照、姿态和样本质量较敏感，因此阈值需要结合现场环境标定，不能直接照搬到其他场景。

7. UART 定长帧与中断解析

OpenMV 与主控之间使用 115200 bps UART 通信。数据帧固定为 8 字节：

字节含义
0 帧头 0xFF
1 帧头 0xFE
2 vision_type
3 X 方向误差
4 Y 方向误差
5 距离估计值
6 保留字节
7 Checksum

视觉状态定义如下：

Type = 0：未发现目标，执行云台扫描；
Type = 1：发现红色目标，执行云台与底盘跟随；
Type = 2：进入近距离区域，底盘驻车并准备核验；
Type = 4：核验成功，结束任务。

早期版本在 20 ms 主循环中轮询串口，容易因为处理不及时造成 FIFO 数据堆积。最终改为 UART RX 中断，并使用状态机逐字节解析：
case 0:
    if (rx_byte == 0xFF) { state = 1; sum = 0; }
    break;
case 1:
    state = (rx_byte == 0xFE) ? 2 : 0;
    break;
case 2:
    buffer[2] = rx_byte; sum += rx_byte; state = 3;
    break;
/* 中间字段依次接收 */
case 7:
    if (sum == rx_byte) {
        vision_type     = buffer[2];
        vision_x_err    = (int8)buffer[3];
        vision_y_err    = (int8)buffer[4];
        vision_distance = buffer[5];
    }
    state = 0;
    break;
这种实现不会阻塞主循环，并且帧头与校验失败后可以自动重新同步。后续还可以增加帧序号、超时看门狗和 CRC8，进一步提高通信可靠性。

8. 底盘串级闭环与安全保护

上层导航和视觉模块输出的是左右轮目标速度，底层先通过斜坡函数限制指令变化速度，再进入轮速 PID：
if (left_target_smooth < left_target_cmd)
    left_target_smooth += step_limit;
else if (left_target_smooth > left_target_cmd)
    left_target_smooth -= step_limit;
斜坡缓冲尤其适合直流电机方向切换。当指令从正转突然变为反转时，它会先逐步减速到零，再反向加速，减小机械冲击和供电波动。

PID 内部还加入了三项保护：

停车死区：目标和误差均接近零时清空积分，避免停车抖动；
积分抗饱和：PWM 已到上限且误差仍推动同一方向时暂停积分；
输出限幅：最终 PWM 始终限制在安全范围内。

系统还根据“高 PWM 输出 + 编码器低脉冲”判断疑似堵转：
if ((abs(left_pwm) > 3000 && abs(left_pulse) < 2) ||
    (abs(right_pwm) > 3000 && abs(right_pulse) < 2)) {
    stall_count++;
} else {
    stall_count = 0;
}

if (stall_count > 50) {
    left_pwm = 0;
    right_pwm = 0;
    left_integral = right_integral = 0;
    current_state = STATE_IDLE;
}
这里检测的是堵转特征，而不是直接测量电流。若后续增加电流采样芯片，可将电流、PWM 与编码器反馈联合起来，使保护判断更加可靠。

9. 开发过程中遇到的典型问题

9.1 固定积分时间导致航向漂移

现象： 小车运行一段时间后，估计航向与实际方向偏差越来越大。

原因： 主循环真实周期会被显示刷新、串口打印和传感器解析改变，固定使用 0.02 s 积分会累积系统误差。

处理： 使用 DWT 周期计数器测量每次循环的真实 dt，同时保留异常值回退机制。

9.2 IMU 零点与地理正北不在同一坐标系

现象： 一进入导航，小车就可能原地大幅转向。

原因： IMU 上电后以当前朝向为相对零点，而 GNSS 方位角以地理北向为参考。

处理： 建立发车前校准流程，使车辆按规定方向摆放后再重置航向基准。更进一步的做法是加入磁力计或利用运动过程中的 GNSS 航向进行在线对准。

9.3 主循环轮询串口导致视觉数据丢帧

现象： OpenMV 已经检测到目标，但云台响应迟钝或状态偶尔错乱。

原因： 视觉数据持续到达，而主循环还要执行导航、显示和 PID；轮询不及时会造成硬件缓冲区堆积。

处理： 使用 UART 接收中断及时取走数据，再用有限状态机完成流式解包。

9.4 舵机快速回正导致 OpenMV 掉电

现象： 目标在视野边缘丢失后，云台快速回正，OpenMV 偶发复位或停止发送数据。

原因： 舵机大角度瞬时动作会带来较大的电流变化，供电设计和线束压降不足时可能引起电压跌落。

处理： 对云台每帧最大角度变化进行限制，并改善舵机独立供电、储能电容、接地和线束连接。主控端还应增加通信超时保护，不能一直使用最后一帧旧数据。

9.5 多个状态机争夺底盘控制权

现象： GPS 导航已经停止，但视觉任务仍在下发运动指令；或者底层触发保护后，上层下一帧又把电机重新启动。

原因： 导航、视觉和安全保护分别维护独立状态，没有统一的执行出口和优先级。

处理： 所有上层模块只生成目标指令，底盘模块掌握最终输出权；急停、堵转和任务完成状态具有更高优先级，可以否决所有运动命令。

后续计划继续整理并开源以下内容：

SR5E1E3 主控工程；
OpenMV 视觉程序；
主从机 UART 通信协议；
硬件框架、软件状态机和导航流程图；
编译、烧录、接线和参数标定说明；
调试过程中遇到的典型故障与解决思路。

如果大家对 GNSS/IMU 导航、OpenMV 视觉跟随、差速底盘 PID 或多状态机调度感兴趣，后续我会继续拆分成独立文章进行详细介绍。

字节	含义
0	帧头 `0xFF`
1	帧头 `0xFE`
2	`vision_type`
3	X 方向误差
4	Y 方向误差
5	距离估计值
6	保留字节
7	Checksum

标签

#嵌入式硬件 #单片机 #OpenMV #GNSS #智能车