系统以 Stellar-SR5E1E3Cortex-M7为主控融合双频 GNSS、IMU、轮式编码器与 OpenMV 视觉模块实现了“航点巡航—近场寻人—视觉跟随—身份核验—任务结束”的完整闭环。相比单独调通某个传感器本项目更大的难点在于多种控制模式之间的交接以及真实电机、舵机和通信链路在极端工况下的可靠性。1. 项目简介传统小车项目往往只实现循迹、避障或单一视觉跟随。本项目希望模拟一个更完整的派件过程车辆先依据 GNSS 航点自主驶向目标区域到达后由 OpenMV 接管近距离搜索利用二维云台锁定目标并牵引底盘靠近最后完成人脸特征核验并安全驻车。一次完整任务的执行流程如下在示教模式下记录目标点的 GNSS 坐标发车前检查定位状态进入自动巡航使用 GNSS 计算目标距离和方位角使用 IMU 估计车体航向通过外环航向 PD 与内环轮速 PID 控制差速底盘距离目标小于 2.5 m 后停车并触发视觉模块接管OpenMV 搜索红色目标二维云台进行 PD 跟踪底盘随云台偏角联动进入 2040 cm 的近距离区域后驻车切换到 LBP 特征核验核验成功后关闭运动输出、云台归中并通过指示灯提示任务完成。硬件环境主控Stellar-SR5E1E3Cortex-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 特征核验当目标距离进入 2040 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帧头0xFF1帧头0xFE2vision_type3X 方向误差4Y 方向误差5距离估计值6保留字节7Checksum视觉状态定义如下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. 底盘串级闭环与安全保护上层导航和视觉模块输出的是左右轮目标速度底层先通过斜坡函数限制指令变化速度再进入轮速 PIDif (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 或多状态机调度感兴趣后续我会继续拆分成独立文章进行详细介绍。