简介思玄机器人队工程机器人视觉系统源码来自RoboMaster2022赛场以Python为主要语言面向机器人竞赛团队及计算机视觉入门者展示了从图像采集、目标识别到决策控制的完整视觉链路。压缩包内共33个文件包含16个py源码文件如main、camera、detect、auto_alignment、pose等用于主流程、相机处理、目标检测、自动对准、姿态解算等8个png与6个jpg作为测试图与流程图另有config.json配置文件、LICENSE及README文档便于复现和理解总大小仅2.31MB。目前已有164人学习适合希望借鉴真实赛队工程方案的开发者。资源价值在于模块划分清晰既有底层通信与日志处理也有上层分类与弹道修正配合测试图片可直接运行观察效果能够帮助读者快速理解机器人视觉系统的代码组织和算法落地方式。1. 拿到RoboMaster工程机器人视觉系统源码先拆这三层再改代码工程机器人和步兵机器人共享大部分视觉算法但任务面完全不同工程要面对取弹、救援、破障碍2022 赛季还有一台随时可能被激活的能量机关。真正决定一场比赛上限的往往不是模型精度而是这套视觉系统能否在 90120FPS 下稳定输出云台角度并在地形起伏和灯光干扰下保持同一套标定结果。把思玄机器人队这份 2022 源码包里的代码按“采集—识别—解算—通信”四条线拆开会发现它没有魔法更多是标定、阈值、坐标系和丢帧处理这些基本功的堆叠。适合刚接手视觉组或准备从零搭一套工程机器人视觉系统的开发者读上面已经踩过的坑下面直接按可复现的顺序给出。2. 工程机器人视觉系统的整体架构从图像采集到弹仓对接的流水线2.1 视觉流水线的四个环节采集、识别、解算、执行先说结论工程机器人视觉系统的可靠性与算法数量成反比。功能越多越要依赖固定化的前置条件——固定曝光、固定白平衡、固定 ROI、固定帧率。拿到源码包先别急着注释功能开关先把主循环的四个环节画出来再决定改哪里。采集工业相机或 USB 相机按固定参数取帧同时记录采集时刻这个时间戳后面做延迟补偿要反复用到。识别从图像里找出灯条、装甲板、弹仓口或能量机关叶片。RoboMaster 场景目标特征明确颜色分割加几何筛选比训练 YOLO 更稳、更快。解算把目标像素坐标投影到相机坐标系再转到云台坐标系输出 yaw/pitch 角度。距离越远炮弹下坠越明显还要加弹道模型。执行通过 UART 串口把角度和目标 ID 发给下位机由底盘和云台去追踪。通信协议必须带校验否则一帧错码会导致云台抽动。这四个环节在代码里通常对应四个独立模块源码包的目录结构一般也会按这条线划分。改任何一个模块的前提是保证输入输出接口不变否则调试时无法定位是哪一环出了问题。2.2 相机采集与触发方式固定曝光是硬要求工程机器人比赛场地的灯光是“舞台级”的聚光灯直射、LED 灯条反光、地面高亮地胶自动曝光在这种环境下会让灯条区域瞬间过曝红色和蓝色全部发白颜色阈值直接失效。因此上电后第一步就是关闭自动曝光和自动白平衡手动拉死曝光时间。// 以 OpenCV VideoCapture 为例很多工业相机的 SDK 也提供同样的开关 cv::VideoCapture cap; cap.open(0); // 相机索引按实际枚举结果 cap.set(cv::CAP_PROP_AUTO_EXPOSURE, 0.25); // 0.25 表示关闭自动曝光 cap.set(cv::CAP_PROP_EXPOSURE, 200); // 曝光时间单位随后端 cap.set(cv::CAP_PROP_AUTO_WB, 0); // 关闭自动白平衡 cap.set(cv::CAP_PROP_GAIN, 100); // 增益锁死避免亮度浮动代码逻辑很直白先把相机的自动策略全关再给一个固定的曝光基础值。200 这个数值在不同相机 SDK 下含义不同有的是微秒有的是相对档位跑通后要记下实际视觉亮度再微调。判断标准是让敌方灯条轮廓清晰、颜色饱和同时背景不过亮。增益也一样锁死之后逐级加直到夜间场地整帧不过暗。2.3 任务状态机同一套视觉系统切换多个任务的主循环工程机器人的多任务切换逻辑一般用一个有限状态机管理视觉主线程按状态调用对应的识别函数而不是每一帧把取弹、救援、能量机关全跑一遍。这种做法不仅省 CPU还避免目标串扰——比如取弹状态下误把敌方装甲板当弹仓口。状态触发条件识别目标输出指令SELF_AIM按键或者裁判系统敌方装甲板yaw、pitch、fireTAKE_BALL进入取弹区弹仓口、弹药箱标识yaw、pitch、triggerRESCUE救援状态救援站标识yaw、pitch、releaseENERGY能量机关激活旋转叶片、中心标识yaw、pitch、firewhile (true) { cv::Mat frame cap.read(); // 读一帧 uint32_t t0 getTimeUs(); // 记录采集时刻 switch (state) { case SELF_AIM: detectArmor(frame); // 装甲板识别 break; case TAKE_BALL: detectBallBox(frame); // 弹仓口识别 break; case ENERGY: detectEnergyFan(frame); // 能量机关识别 break; default: break; } if (target.valid) { AimCmd cmd solver.getAngle(target); // 像素 - 云台角 protocol.send(cmd); // 串口发出 } uint32_t t1 getTimeUs(); fps 1000000.0f / (t1 - t0); // 每帧耗时统计 }主循环里最容易被忽略的是getTimeUs()的采集时刻记录。很多队直接以“收到图像”的时刻作为测量基准但工业相机触发到数据到达主机之间有传输延迟忽略这一段会让后续延迟补偿出现系统性偏差。帧率统计不能只算识别耗时要算整个循环的耗时这样才暴露串口发送被阻塞的问题。3. 装甲板识别与 PnP 测距工程机器人视觉系统最常用的两个算法3.1 灯条提取HSV 颜色阈值为什么比深度学习更稳比赛里的装甲板由左右两根灯条构成灯条是自发光 LED颜色饱和度高、形状长条与场地背景差异极大。常用做法是把图像从 BGR 转 HSV用inRange提取敌方颜色再做一次闭运算填补灯条内部噪声然后找轮廓。import cv2 import numpy as np cap cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25) cap.set(cv2.CAP_PROP_EXPOSURE, 200) while True: ret, frame cap.read() if not ret: break hsv cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # 以敌方红色为例红色色相在 0 附近和 180 附近各有一片 mask_red cv2.inRange(hsv, (0, 80, 80), (10, 255, 255)) mask_red2 cv2.inRange(hsv, (170, 80, 80), (180, 255, 255)) mask cv2.bitwise_or(mask_red, mask_red2) # 闭运算填充灯条内部断裂 mask cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((5, 5), np.uint8)) contours, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for cnt in contours: area cv2.contourArea(cnt) if area 50: continue rect cv2.minAreaRect(cnt) w, h rect[1] if h w: w, h h, w # 灯条必须是细长的长宽比一般大于 2 if h / w 2.0: continue box cv2.boxPoints(rect) box np.int0(box) cv2.drawContours(frame, [box], -1, (0, 255, 0), 2) # 把可视化窗口压缩处理降低调试时的绘制开销 cv2.imshow(debug, cv2.resize(frame, (640, 360))) if cv2.waitKey(1) 0xFF ord(q): break cap.release() cv2.destroyAllWindows()代码里把红色拆成两个 HSV 区间因为 OpenCV 的 H 范围是 0180红色同时落在 010 和 170180 两段蓝色只要一个区间比如 100124。这里的 80 作为 S 和 V 下限作用是过滤掉灰色背景和亮度不足的暗区具体数值要按场地光照环境重调。闭运算的卷积核不能太大5×5 在 640×480 下够用再大会把两个灯条连成一个块。这一段跑通后应该能在画面上看到每个灯条一个绿色矩形框。3.2 灯条配对与装甲板候选生成单根灯条不能构成目标需要左右两根灯条按几何约束配对才能组成装甲板。常见的约束条件有四个两根灯条的长度比、水平偏移比例、垂直偏移量、角度差。长度比要小于 2垂直偏差不能超过灯条本身长度的 0.5 倍角度差不能超过 15°。配对后取左右灯条的四条外边合成装甲板的四个角点再计算紧包矩形的宽高比。小装甲板宽高比接近 1.3大装甲板接近 2.3不同规则下的具体数值略有差异按比赛手册里的机械化尺寸填写即可用这个比例可以过滤掉大量误匹配。3.3 用 PnP 解算距离和角度四个点就能测距装甲板的物理尺寸是已知的四个角点在世界坐标系里的坐标可以预先写死。那么问题就变成已知 3D 点和对应 2D 像素点求相机位姿。这就是经典的 PnP 问题OpenCV 的solvePnP可以直接解输出旋转向量rvec和平移向量tvec其中tvec的模长就是相机光心到装甲板中心的距离。# 相机内参和畸变系数来自 5.1 章节的标定 mtx np.load(camera_mtx.npy) dist np.load(camera_dist.npy) # 装甲板真实尺寸单位 mm按比赛规则填写 # 这里以常见大装甲板 230mm x 180mm 为例 board_half_w 115.0 board_half_h 90.0 object_points np.array([ [-board_half_w, -board_half_h, 0], [ board_half_w, -board_half_h, 0], [ board_half_w, board_half_h, 0], [-board_half_w, board_half_h, 0], ], dtypenp.float32) # 2D 像素点顺序要和 3D 点一一对应按左上、右上、右下、左下 image_points np.array([ [tl_x, tl_y], [tr_x, tr_y], [br_x, br_y], [bl_x, bl_y] ], dtypenp.float32) ret, rvec, tvec cv2.solvePnP(object_points, image_points, mtx, dist, flagscv2.SOLVEPNP_ITERATIVE) distance float(np.linalg.norm(tvec))solvePnP的输入顺序必须严格对应左上角像素点对应左上角 3D 点否则解出来的位姿是错误的。SOLVEPNP_ITERATIVE适用于点数少且带有较好初始值的场景装甲板四个共面点的情况下收敛稳定。距离输出的是毫米在自瞄流程里把这个 distance 传给弹道模型用于计算枪口抬升角度同时rvec里存着装甲板的朝向可以判断目标是否背对或者侧倾过大这种目标应该直接丢弃。3.4 颜色阈值和轮廓筛选的 5 个必调参数参数红色典型值蓝色典型值调整依据H 下限 / 上限0~10 与 170~180 合并100~124色相漂移大就放宽过宽会误检S 下限80100过滤灰色背景反光强时提高V 下限8080过暗灯光下调低到 60灯条最小面积5050图像分辨率低时降到 20灯条长宽比2.02.0数值过大会漏掉远距离灯条参数调整的普遍顺序是先调曝光和增益保证画面里灯条区域不过曝再调 HSV 三个通道的下限拉到“背景全黑、灯条全白”的效果最后再动轮廓筛选参数。如果灯条时不时断成两截优先加闭运算的迭代次数而不是放宽颜色阈值。放宽颜色阈值是最差的方案它会把场地里的红色和蓝色装饰物全部引进来后续误匹配会非常难处理。4. 能量机关识别的时序与解算工程机器人视觉系统的转速预测4.1 能量机关的识别难点高速旋转加高曝光2022 赛季的能量机关是旋转式叶片目标叶片绕中心旋转视觉系统需要在正确的时间窗口内开火打中叶片上的指定装甲区。这个任务和普通自瞄的最大区别在于目标一直在动而且是绕固定圆心的高速转动。识别上叶片上的装甲区同样使用灯条提取难点在判断“什么时候叶片转到了准星附近”这要求视觉系统输出目标当前的角度和角速度而不是单纯输出距离。帧率不够会直接导致采样点稀疏角速度估计错误开火时机彻底偏离。所以能量机关状态下要把 ROI 裁剪到只保留圆心附近区域缩小处理面积尽量把识别帧率顶到 150FPS 以上。4.2 目标提取与中心点计算# 假设 detect_fan 已从 ROI 中提取出叶片装甲区的中心和旋转半径 # 返回值 center: 圆心像素坐标, angle: 叶片当前角度(弧度), t: 时间戳 center, angle, t detect_fan(roi_frame) # 计算叶片与圆心的连线角度作为当前相位 dx blade_center_x - fan_center_x dy blade_center_y - fan_center_y phase math.atan2(dy, dx) # 叶片当前相位代码逻辑是先找到整个能量机关的圆心再找到叶片装甲区的中心用atan2计算两点连线与水平轴的夹角这个 angle 就是叶片当前的相位。量纲上注意atan2输出范围是 -π 到 π跨帧比较角度时要处理 ±π 跳变否则角速度会算出一个离谱的巨大值。处理办法是if d_angle math.pi: d_angle - 2*math.pi同理小于 -π 时加回来。4.3 角速度估计与开火窗口计算角速度估计用一阶低通滤波器把高频抖动压掉。纯差分求角速度会把像素噪声放大成剧烈跳变云台会跟着乱摆所以必须滤波。alpha 0.8 # 低通系数越大越平滑但延迟也越大 omega 0.0 # 角速度估计值 prev_phase 0.0 prev_t 0.0 while run: center, phase, t read_fan_state() # 读取新一帧 dt t - prev_t if dt 1e-4 and prev_t 0: d_phase phase - prev_phase # 处理角度跨越 ±π 的跳变 if d_phase math.pi: d_phase - 2 * math.pi elif d_phase -math.pi: d_phase 2 * math.pi omega_now d_phase / dt omega alpha * omega (1 - alpha) * omega_now prev_phase, prev_t phase, t # 计算叶片转到准星相位还需要的时间 if omega 0: t_wait normalize_angle(target_phase - phase) / omega aim_and_fire(t_wait)低通系数 alpha 取 0.8 意味着新值只占 20% 权重滤波效果强但滞后大如果发现预测角度一直慢半拍把 alpha 降到 0.6。角速度符号决定旋转方向工程机器人的云台要往对应方向预转。最后输出的t_wait是“叶片转到准星相位还需要的时间”视觉线程把t_wait发给下位机让云台提前旋转并延时开火这个延迟正好覆盖机械响应时间。5. 相机标定与串口部署把工程机器人视觉系统搬上实车的关键步骤5.1 先用标定程序把相机内参定死很多调试问题的根源不是识别算法而是相机内参是“借来的”。焦距、主点坐标和畸变参数不准确PnP 解出来的距离和角度会系统性偏移近距离看着正常打到 5 米外全部偏下。以 OpenCV 为例标定流程是打印一张 9×6 棋盘格标定板贴平在平面上从多个角度拍 2030 张图片然后统一交给cv2.calibrateCamera求解内参矩阵和畸变系数。import numpy as np import cv2 import glob CHECKERBOARD (9, 6) # 内角点数 objp np.zeros((CHECKERBOARD[0] * CHECKERBOARD[1], 3), np.float32) objp[:, :2] np.mgrid[0:9, 0:6].T.reshape(-1, 2) obj_points [] img_points [] for fname in sorted(glob.glob(chessboard/*.jpg)): img cv2.imread(fname) gray cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners cv2.findChessboardCorners(gray, CHECKERBOARD, None) if ret: obj_points.append(objp) img_points.append(corners) ret, mtx, dist, rvecs, tvecs cv2.calibrateCamera( obj_points, img_points, gray.shape[::-1], None, None) np.save(camera_mtx.npy, mtx) np.save(camera_dist.npy, dist)标定时要注意图片里棋盘格必须有明显的角度变化不能全是正对着相机拍的否则内参解算退化距离也要有远有近让畸变模型能覆盖整个画面。标定完成后保留reprojection error一般要低于 0.3 像素才算合格超过 1.0 说明有图片拍糊了或者角点提取错误直接重新标。5.2 外参标定摄像头与云台的相对位置关系内参解决“像素坐标到相机坐标”的问题外参解决“相机坐标到云台坐标”的问题。工程机器人上相机一般装在云台上或者车体上装云台上时相机和云台同步转动相对位姿变化小装车体上时需要把目标从相机系转到云台系再叠加云台转角。外参的测量可以用机械量法量出相机光心相对云台转轴的 X/Y/Z 偏移在代码里做成一个常量矩阵。偏移量精确到毫米量级即可因为距离十几米外的目标几毫米的平移误差可以忽略。5.3 串口协议与 CRC 校验嵌入式端最看重的三个字段视觉和电控的通信几乎全部走 UART常见波特率 115200 或 460800。自定协议中至少要有帧头、数据长度、命令 ID、数据负载、CRC16 校验。帧头用两个固定字节0xAA 0x55数据长度字段防止粘包解析错位CRC16 用于丢弃被干扰的坏帧。字节偏移字段长度说明0帧头11固定 0xAA1帧头21固定 0x552数据长度1只计算数据域长度不含帧头和 CRC3命令 ID1区分自瞄、取弹等4~7数据域4例如 yaw 转角 int16 pitch 转角 int168~9CRC162对数据长度到数据域末尾做校验import serial import struct ser serial.Serial(/dev/ttyUSB0, 115200, timeout0.01) def crc16(data: bytes) - int: crc 0xFFFF for b in data: crc ^ b for _ in range(8): if crc 0x0001: crc (crc 1) ^ 0xA001 else: crc 1 return crc 0xFFFF def send_aim(yaw_mrad: int, pitch_mrad: int, fire: int) - None: cmd_id 0x01 payload struct.pack(BhhB, cmd_id, yaw_mrad, pitch_mrad, fire) crc crc16(bytes([len(payload)]) payload) frame b\xAA\x55 bytes([len(payload)]) payload struct.pack(H, crc) ser.write(frame)发送端的 yaw/pitch 用毫弧度 int16电控直接按这个值转动云台省去浮点换算的踩内存问题。CRC16 先算长度字段再算数据域接收端收到后先查帧头、再查长度、最后查 CRC任何一步不通过就整帧丢弃不做半包恢复。这种协议看着简单但比裸发角度值和仅用和校验要可靠得多比赛现场的电磁干扰能直接干扰无保护的串口数据。5.4 部署避坑清单部署到实车后常见三个问题。第一视觉线程和串口发送线程共用同一个锁导致发送阻塞图像采集耗时翻倍解决方法是使用环形缓冲区发送失败直接丢包绝不让发送阻塞识别。第二相机和嵌入式主控共用一个电源云台电机启动瞬间电压跌落导致相机掉帧排查时看相机是否周期性丢帧解决方法是相机单独供电或用隔离模块。第三调试时笔记本通过 USB 连接相机会拉低整个系统的电源稳定性建议调试阶段用外接电源给相机供电别直接从笔记本取电。这三个问题在仿真环境或桌面端几乎不会出现只在实车烧录后才暴露。6. 工程机器人视觉系统的全链路延迟测量与补偿技巧全链路延迟是云台命中率的最大隐形杀手。从相机曝光开始到图像传输、算法处理、串口发送、下位机接收、云台电机响应全链路延迟通常有 3080ms取决于帧率和硬件。如果不做延迟补偿云台看到的永远是“过去”的目标追踪运动目标时会持续滞后一个固定角度。测量延迟的常见做法是“LED 回显法”在相机视野里放一个 LED代码里控制 GPIO 点亮它同时记录当前帧时间戳再用同一台相机拍下 LED 实际亮起的时刻两个时间差就是全链路延迟。没有 GPIO 设备时可以用串口发送一个标记信号让下位机收到后翻转一个大电流 LED相机同时拍摄画面计算翻转时刻与期望时刻的差。测出延迟 T 后在解算模块里加一阶相位补偿目标当前角度加上角速度乘以 T得到“未来 T 毫秒后”的预测角度。注意角速度要从低通滤波器取不要直接取原始差分值否则补偿值放大噪声。# 全链路延迟单位秒由 LED 回显法测出 T 0.045 # yaw_rate 是低通滤波后的角速度 yaw_predict yaw_current yaw_rate * T pitch_predict pitch_current pitch_rate * T云台角度响应本身也有延迟如果电机响应慢这个 T 还要再加半拍。抗扭摆比较严重的车云台会有机械谐振反馈回来的角速度里带着谐振频段成分这时把低通滤波的截止频率压在 25Hz 区间配合角速度预测基本不会出现云台的持续抖动。补偿的最终效果应该用“固定目标命中率”和“运动目标命中率”两个指标分开记录前者不掉则补偿没有破坏静态精度后者提升则补偿方向正确。本文还有配套的精品资源点击获取