企业官网建设流程全解析

1. 项目概述：一条视觉链路的诞生，不是炫技，是让机器人真正“看见”并“理解”世界

RoboMaster 视觉招新考核，这个名字在高校机器人社团圈子里几乎等同于“视觉方向的成人礼”。它不考你背了多少公式，也不看你调过多少次超参数，而是用一套高度凝练、环环相扣的实战任务，逼你把书本上的离散知识点，焊接到一条能跑通、能输出、能驱动硬件的真实视觉链路上。标题里那串看似随意的关键词——“蓝色灯条 → 相机标定 → AprilTag 位姿 → 串口”，就是这条链路最精悍的骨架。它背后藏着的是一个完整的机器视觉闭环：从最底层的图像信号捕获（灯条），到中间层的空间坐标校准（标定），再到上层的语义级空间理解（AprilTag位姿），最终落脚到与物理世界的交互接口（串口通信）。我带过几届招新，也作为考生被考过，最深的体会是：能一次性把这四个环节全打通的人，基本已经跨过了“会调库”的门槛，进入了“懂系统”的阶段。这个考核适合两类人：一类是刚学完OpenCV、对SLAM或机器人导航有朦胧兴趣的大二学生；另一类是想快速验证自己工程能力、为后续参加RoboMaster机甲大师赛打基础的高年级同学。它不追求算法有多前沿，但要求每个环节都稳如磐石——因为你的代码，下一秒就要控制一个几十公斤重的机器人底盘，在高速运动中精准识别目标、计算距离、并发出转向指令。这种压力下的链路稳定性，才是它真正的筛选逻辑。

2. 内容整体设计与思路拆解：为什么是这四步？每一步都在解决一个不可绕过的“地基问题”

这套考核流程绝非拍脑袋定下的顺序，而是对机器人视觉系统工程化落地的一次精准解剖。它的设计逻辑，本质上是在模拟一个真实机器人在未知环境中执行定位-导航-交互任务时，必须经历的四个不可跳过的“地基”环节。我们来一层层剥开它的设计哲学。

2.1 第一环：蓝色灯条检测——图像感知的“第一道滤网”

为什么起点是“蓝色灯条”，而不是更通用的YOLOv8或SVM分类器？答案很务实：鲁棒性优先，而非泛化性。在RoboMaster比赛现场，光照条件极其恶劣——室内场馆顶灯、对手机器人LED阵列、甚至观众手机闪光灯，都会造成剧烈的环境光干扰。而“蓝色灯条”是一个强约束的先验：它意味着目标具有明确的颜色（HSV空间H通道集中在100-130）、固定的几何形态（细长矩形）、以及稳定的发光特性（高饱和度、高亮度）。这使得开发者可以放弃复杂的深度学习模型，转而使用极轻量、极快速的传统图像处理流水线：BGR→HSV转换→蓝色通道阈值分割→形态学去噪→轮廓提取→长宽比与面积过滤→最小外接矩形拟合。整个过程在树莓派4B上也能稳定跑出60FPS。这一步的设计意图非常清晰：它不考察你是否能识别千奇百怪的物体，而是考察你能否在资源受限、环境嘈杂的嵌入式端，构建一条抗干扰、低延迟、可预测的原始感知通道。它筛掉的是那些只会调用cv2.findContours()却不知道cv2.morphologyEx()为何物，或者对HSV色彩空间理解仅停留在“蓝色是B=255”的同学。

2.2 第二环：相机标定——从“像素”到“米”的“翻译官”

灯条检测只给了你图像上的二维坐标（x, y），但机器人需要知道“目标离我有多远”、“目标在我左前方还是右后方”。这就引出了第二步：相机标定。这里的关键在于，它考的不是你会不会用cv2.calibrateCamera()函数，而是你是否真正理解内参矩阵（focal length, principal point, distortion coefficients）和外参矩阵（rotation & translation）的物理意义与耦合关系。考核中通常会提供一张标准棋盘格，要求你拍摄多张不同角度的照片，并手动分析标定结果。一个典型的陷阱是：很多同学标定完就直接拿cv2.solvePnP()去算位姿，却忽略了镜头畸变（尤其是径向畸变k1, k2）对角点坐标的扭曲。实测发现，未校正畸变的情况下，即使标定误差只有0.1像素，在2米距离上也会导致3-5厘米的深度计算偏差——这对需要毫米级精度的云台瞄准来说是致命的。因此，这一环的设计，是在强制你建立一个关键认知：图像坐标系、相机坐标系、世界坐标系，三者之间必须通过精确的数学映射才能转换，任何一步的松懈，都会在后续环节被指数级放大。

2.3 第三环：AprilTag位姿估计——从“点”到“姿态”的“空间建模师”

有了标定好的相机，下一步就是赋予它“空间理解”能力。为什么选AprilTag而不是ArUco？这是一个经过深思熟虑的取舍。AprilTag的编码密度更高（6x6码元 vs ArUco的4x4或5x5），在同样大小的标签下，它能提供更丰富的边缘信息，这对solvePnP算法求解旋转矩阵R和位移向量t至关重要。更重要的是，AprilTag的解码库（apriltagPython binding）对噪声和部分遮挡的鲁棒性更强，其内部采用的亚像素角点精炼和迭代重投影优化，能显著提升位姿估计的稳定性。考核中，你不仅要把标签贴在墙上，还要把它贴在移动的靶机上，甚至故意用手半遮挡标签一角。这时候，一个只依赖cv2.aruco.detectMarkers()的同学会发现，一旦标签被遮挡，位姿就会剧烈抖动甚至完全丢失；而一个深入研究过AprilTag源码、理解其tag_family选择（如tag36h11）与decimation（降采样）参数意义的同学，则能通过调整quad_decimate=2.0和refine_edges=1，在保证速度的同时，将位姿抖动控制在±0.5°以内。这一环的设计，是在考察你是否具备将抽象的数学模型（PnP）与具体的物理现象（标签边缘模糊、光照不均）进行动态适配的能力。

2.4 第四环：串口通信——从“数据”到“动作”的“最后一百米”

前三步产出的都是“数据”：灯条中心坐标、相机内参、目标在相机坐标系下的三维位置（x, y, z）和欧拉角（roll, pitch, yaw）。但机器人不会看数据，它只认指令。这就是第四步——CV1串口通信——存在的全部意义。这里的“CV1”并非特指某款芯片，而是代指一个通用的、基于UART协议的嵌入式控制模块，它负责接收上位机（通常是树莓派）发来的结构化指令，并将其转化为PWM信号去驱动舵机或电机。考核要求你编写一个串口协议解析器，例如：<STX>0x01<POS_X_H><POS_X_L><POS_Y_H><POS_Y_L><YAW_H><YAW_L><ETX>。难点不在于发送字节，而在于时序与容错。比如，当机器人高速移动时，云台需要实时微调，这意味着串口必须以50Hz以上的频率稳定收发数据；同时，你必须设计帧头帧尾、校验和（如XOR校验），并实现超时重传机制——因为一根劣质USB转TTL线缆，在电机启动瞬间产生的电磁干扰，足以让一帧关键指令彻底丢失。我见过太多同学，前三步完美，却在串口这一步栽跟头：他们用ser.write()发完数据就不管了，结果云台要么原地乱转，要么完全不动。这恰恰暴露了工程思维的断层：再完美的算法，如果无法可靠地抵达执行单元，就等于零。这一环，是整条链路从“实验室Demo”迈向“真实机器人”的分水岭。

3. 核心细节解析与实操要点：手把手拆解每个环节的“魔鬼细节”

光知道流程远远不够。RoboMaster招新考核的残酷之处在于，它会在每一个环节埋下几个“魔鬼细节”，这些细节往往决定了你是顺利通关，还是卡在某个环节反复调试到深夜。下面，我将结合多年带教和参赛经验，把每个环节最易踩坑、最值得深挖的核心细节掰开揉碎讲清楚。

3.1 蓝色灯条检测：HSV阈值不是“调出来就行”，而是要“算出来”

很多同学打开HSV颜色拾取器，对着屏幕上的蓝色灯条随便拖动滑块，得到一组[90, 50, 50]到[130, 255, 255]的阈值，然后就以为万事大吉。这是最大的误区。HSV空间的H（色相）通道是环形的，0和180都代表红色，而蓝色实际分布在100-130区间，但这个区间会因摄像头白平衡、环境光色温发生漂移。真正的做法，是用统计学方法“算”出阈值。

具体操作如下：

1. 在目标场景下，用摄像头拍摄一张包含清晰蓝色灯条的图片（确保灯条占画面1/4以上）。
2. 将图片转换为HSV格式：hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)。
3. 提取H通道的所有像素值，并绘制直方图：plt.hist(hsv[:,:,0].ravel(), bins=180, range=[0,180])。
4. 观察直方图，你会发现蓝色像素在H通道上会形成一个明显的尖峰（peak），而背景和其他干扰物则呈宽泛的分布。记下这个峰值的中心位置H_center。
5. 阈值的H范围应设为[H_center - 15, H_center + 15]，S（饱和度）和V（明度）则根据直方图的双峰分布来定：S的下限应设在背景与灯条分离的谷底位置，V的下限同理。这样得到的阈值，才具备场景自适应性。

提示：在树莓派上部署时，务必关闭自动白平衡（picamera2中设置controls={'AwbEnable': False}），否则每次开机H值都会漂移，导致阈值失效。

另一个常被忽视的细节是形态学操作的顺序与核尺寸。很多教程直接给cv2.MORPH_RECT核，但对灯条这种细长目标，cv2.MORPH_ELLIPSE核（椭圆形）更能保留其几何特征。开运算（cv2.MORPH_OPEN）用于去噪，闭运算（cv2.MORPH_CLOSE）用于连接断裂的灯条。核尺寸不能过大，否则会吞噬灯条本身。我的经验是：对于1080p图像，开运算核尺寸设为(3, 3)，闭运算核尺寸设为(7, 3)（长轴沿灯条方向），效果最佳。

3.2 相机标定：棋盘格不是“拍得越多越好”，而是要“覆盖全空间”

标定的终极目标，是获得一个能准确描述相机成像畸变的数学模型。而这个模型的精度，直接取决于你采集的标定板图像的质量和多样性。新手常犯的错误是：站在同一个位置，只改变棋盘格的俯仰角，拍了20张图，结果标定出来的重投影误差高达1.5像素。问题出在数据集的“维度缺失”。

一个合格的标定数据集，必须在三个维度上充分覆盖：

• 平移维度：棋盘格需出现在图像的四个角（左上、右上、左下、右下）以及中心区域。
• 旋转维度：棋盘格需有明显的绕X轴（俯仰）、Y轴（偏航）、Z轴（滚转）的旋转，不能只在一个平面内转动。
• 深度维度：棋盘格需在至少3个不同的距离上拍摄（如0.5m, 1.0m, 1.5m），以充分激发径向畸变模型的参数。

实操中，我推荐一个“九宫格+三距离”法：在0.5m、1.0m、1.5m三个距离上，分别将棋盘格置于图像的9个固定位置（想象一个3x3网格），每个位置拍1张，共27张。这样采集的数据，能让cv2.calibrateCamera()收敛到0.1像素以内的重投影误差。此外，务必检查每张图的角点检测质量。cv2.findChessboardCorners()返回的ret值为True，只是说明找到了角点，但不保证它们是准确的。你需要用cv2.drawChessboardCorners()画出来，肉眼确认所有角点都被精确地标在了棋盘格交点上。哪怕有一张图的角点偏移了半个像素，它也会成为整个标定结果的“毒丸”。

注意：标定板的物理尺寸（单个方格边长）必须精确测量并输入到代码中。我曾见过有同学用卷尺粗略量了个“2.5cm”，结果导致后续所有位姿计算的z轴（深度）误差放大3倍。建议用游标卡尺测量，并在代码中写死为square_size = 0.0254（2.54cm，即1英寸，这是工业标准棋盘格尺寸）。

3.3 AprilTag位姿估计：solvePnP不是“一键求解”，而是要“理解假设”

cv2.solvePnP()是位姿估计的常用工具，但它背后有一系列严格的数学假设。考核中，如果你直接把AprilTag检测到的4个角点坐标喂给它，得到的结果很可能抖动得像帕金森病患者的手。原因在于，solvePnP默认使用的是cv2.SOLVEPNP_ITERATIVE（迭代法），它对初始值和噪声极其敏感。

更稳健的做法是：

1. 先用cv2.SOLVEPNP_AP3P（AP3P算法）获取一个粗略的初始解。AP3P只需要3个点，且对噪声鲁棒性更好，能快速给出一个合理的R和t初值。
2. 再将这个初值作为cv2.SOLVEPNP_ITERATIVE的输入，进行精细化求解。这相当于给迭代算法装上了“导航仪”，大大提升了收敛速度和精度。
3. 最后，务必进行重投影验证。将计算出的R和t，连同已知的标签3D点坐标，通过相机模型重新投影回图像平面，计算重投影点与检测到的角点之间的平均距离（reprojection_error）。这个值应小于1像素，否则说明位姿解不可信，需要丢弃该帧。

另一个关键细节是标签的3D模型定义。AprilTag的坐标系原点默认在标签中心，Z轴垂直于标签平面指向相机。但很多同学在定义obj_points（世界坐标系下的标签角点）时，错误地将其设为[[-0.5,-0.5,0], [0.5,-0.5,0], [0.5,0.5,0], [-0.5,0.5,0]]（单位：米）。这会导致计算出的z值是相对于标签中心的，而非标签平面。正确的做法是，将z值设为0，并确保标签的物理尺寸（如0.15m）被精确代入，即obj_points = np.array([[-0.075,-0.075,0], [0.075,-0.075,0], [0.075,0.075,0], [-0.075,0.075,0]], dtype=np.float32)。

3.4 CV1串口通信：协议不是“定义好就行”，而是要“经得起电磁脉冲考验”

串口通信的协议设计，是整个链路中最体现工程素养的一环。一个常见的错误协议设计是：<ID><X><Y><YAW><CHECKSUM>，其中<ID>是设备地址，<X><Y><YAW>是16位整数，<CHECKSUM>是简单累加和。这个协议在实验室电脑上运行良好，但一上机器人，就会在电机启动的瞬间出现大量乱码。

根本原因在于缺乏同步机制和抗干扰设计。正确的做法是引入一个完整的帧结构：

| STX (0x02) | LEN (1B) | CMD (1B) | PAYLOAD (N B) | CHECKSUM (1B) | ETX (0x03) |

• STX/ETX：帧头帧尾，用于接收端快速同步。
• LEN：有效载荷长度，让接收端知道该读多少字节，避免因丢包导致的后续所有帧错位。
• CMD：命令类型，如0x01表示“云台控制”，0x02表示“LED状态”，为未来扩展留余地。
• PAYLOAD：结构化数据，X,Y,YAW均用32位浮点数（struct.pack('f', value)），虽然体积变大，但消除了整数缩放带来的精度损失。
• CHECKSUM：必须使用CRC8或更优的CRC16算法，而非简单XOR。XOR在连续多个0字节时会失效。

在代码层面，发送端必须实现阻塞式发送与超时重传：

def send_cmd(ser, cmd_data): frame = build_frame(cmd_data) for attempt in range(3): # 最多重传3次 ser.write(frame) # 等待来自CV1的ACK响应 if wait_for_ack(ser, timeout=0.1): return True time.sleep(0.05) # 重传前小憩 return False # 三次都失败，报错

而接收端（CV1固件）必须实现环形缓冲区和状态机解析，确保即使在数据流中混入了干扰噪声，也能在下一个STX到来时，自动丢弃错误帧，重新开始同步。这才是工业级串口通信的正确打开方式。

4. 实操过程与核心环节实现：从零开始，搭建一条可运行的完整链路

现在，让我们把前面所有的理论、细节和避坑指南，整合成一份可直接上手、逐行执行的实操手册。以下代码和步骤，均基于Python 3.9、OpenCV 4.8、Apriltag 4.2和PySerial 3.5，在Raspberry Pi 4B（8GB RAM）上实测通过。整个过程分为环境准备、模块开发、联调测试三大阶段，总耗时约6-8小时。

4.1 环境准备：一次配齐，永绝后患

在树莓派上，不要用apt install python3-opencv，它版本太老且不带contrib模块。必须从源码编译，这是唯一能保证所有功能（尤其是cv2.aruco和cv2.sfm）正常工作的方案。

步骤1：安装基础依赖

sudo apt update && sudo apt upgrade -y sudo apt install -y build-essential cmake git pkg-config libjpeg-dev libtiff-dev libjasper-dev libpng-dev libavcodec-dev libavformat-dev libswscale-dev libv4l-dev libxvidcore-dev libx264-dev libfontconfig1-dev libcairo2-dev libgdk-pixbuf2.0-dev libpango1.0-dev libgtk2.0-dev libgtk-3-dev libatlas-base-dev gfortran libhdf5-dev libhdf5-serial-dev libhdf5-dev python3-dev python3-pip

步骤2：编译安装OpenCV

cd ~ wget -O opencv.zip https://github.com/opencv/opencv/archive/4.8.0.zip wget -O opencv_contrib.zip https://github.com/opencv/opencv_contrib/archive/4.8.0.zip unzip opencv.zip && unzip opencv_contrib.zip mkdir -p ~/opencv_build && cd ~/opencv_build cmake -D CMAKE_BUILD_TYPE=RELEASE \ -D CMAKE_INSTALL_PREFIX=/usr/local \ -D OPENCV_EXTRA_MODULES_PATH=~/opencv_contrib-4.8.0/modules \ -D ENABLE_NEON=ON \ -D ENABLE_VFPV3=ON \ -D BUILD_TESTS=OFF \ -D OPENCV_ENABLE_NONFREE=ON \ -D CMAKE_SHARED_LINKER_FLAGS=-latomic \ -D BUILD_EXAMPLES=OFF \ -D PYTHON3_EXECUTABLE=/usr/bin/python3 \ -D PYTHON3_INCLUDE_DIR=/usr/include/python3.9 \ -D PYTHON3_PACKAGES_PATH=/usr/lib/python3/dist-packages \ -D PYTHON3_LIBRARY=/usr/lib/arm-linux-gnueabihf/libpython3.9.so \ ../opencv-4.8.0 make -j4 # 使用4个核心编译，约45分钟 sudo make install sudo ldconfig

步骤3：安装Apriltag与PySerial

pip3 install apriltag pyserial

步骤4：配置摄像头

# 启用摄像头接口 sudo raspi-config # 进入 Interface Options -> Camera -> Enable # 重启 sudo reboot

完成以上步骤后，你的树莓派就拥有了一个“开箱即用”的RoboMaster视觉开发环境。记住，这一步花的时间越长，后面调试时节省的时间就越多。我见过太多同学因为OpenCV版本不匹配，在cv2.solvePnP()上卡了两天，最后发现只是少了一个-D OPENCV_ENABLE_NONFREE=ON参数。

4.2 模块开发：分而治之，逐个击破

我们将整个链路拆分为4个独立的Python模块，每个模块负责一个核心环节，并通过统一的VisionPipeline类进行组装。这种设计便于单独测试、复用和调试。

模块1：detector.py—— 蓝色灯条检测器

import cv2 import numpy as np class BlueLightDetector: def __init__(self, h_low=100, h_high=130, s_low=50, s_high=255, v_low=50, v_high=255): self.hsv_lower = np.array([h_low, s_low, v_low]) self.hsv_upper = np.array([h_high, s_high, v_high]) # 形态学核 self.kernel_open = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3)) self.kernel_close = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (7, 3)) def detect(self, frame): hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, self.hsv_lower, self.hsv_upper) # 形态学去噪 mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, self.kernel_open) mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel_close) # 轮廓检测 contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) centers = [] for cnt in contours: area = cv2.contourArea(cnt) if area < 100: # 过滤小噪点 continue x, y, w, h = cv2.boundingRect(cnt) aspect_ratio = max(w, h) / min(w, h) if min(w, h) > 0 else 0 # 灯条是细长的，长宽比应大于3 if aspect_ratio > 3 and area > 200: center_x, center_y = int(x + w/2), int(y + h/2) centers.append((center_x, center_y)) return centers, mask # 测试代码 if __name__ == "__main__": cap = cv2.VideoCapture(0) detector = BlueLightDetector() while True: ret, frame = cap.read() if not ret: break centers, mask = detector.detect(frame) for (cx, cy) in centers: cv2.circle(frame, (cx, cy), 5, (0, 255, 0), -1) cv2.imshow("Detection", frame) cv2.imshow("Mask", mask) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows()

模块2：calibrator.py—— 相机标定器

import cv2 import numpy as np import pickle class CameraCalibrator: def __init__(self, pattern_size=(9, 6), square_size=0.0254): self.pattern_size = pattern_size self.square_size = square_size self.obj_points = [] # 3D点 self.img_points = [] # 2D点 self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) def add_image(self, img_path): img = cv2.imread(img_path) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, self.pattern_size, None) if ret: # 亚像素精炼 corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), self.criteria) self.obj_points.append(self._generate_obj_points()) self.img_points.append(corners2) # 可视化 cv2.drawChessboardCorners(img, self.pattern_size, corners2, ret) cv2.imshow('Calibration', img) cv2.waitKey(500) return ret def _generate_obj_points(self): objp = np.zeros((self.pattern_size[0] * self.pattern_size[1], 3), np.float32) objp[:, :2] = np.mgrid[0:self.pattern_size[0], 0:self.pattern_size[1]].T.reshape(-1, 2) objp *= self.square_size return objp def calibrate(self): if len(self.obj_points) < 15: # 至少需要15张高质量图 raise ValueError("Not enough calibration images!") ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera( self.obj_points, self.img_points, (640, 480), None, None ) # 计算重投影误差 mean_error = 0 for i in range(len(self.obj_points)): imgpoints2, _ = cv2.projectPoints( self.obj_points[i], rvecs[i], tvecs[i], mtx, dist ) error = cv2.norm(self.img_points[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2) mean_error += error print(f"Total reprojection error: {mean_error/len(self.obj_points)}") return { 'camera_matrix': mtx, 'dist_coeffs': dist, 'rvecs': rvecs, 'tvecs': tvecs } def save_calibration(self, filename): with open(filename, 'wb') as f: pickle.dump(self.calibrate(), f) print(f"Calibration saved to {filename}") # 使用方法：先用手机或树莓派摄像头拍27张棋盘格照片，存为calib_01.jpg...calib_27.jpg # 然后运行： # calibrator = CameraCalibrator() # for i in range(1, 28): # calibrator.add_image(f"calib_{i:02d}.jpg") # calibrator.save_calibration("calibration.pkl")

模块3：pose_estimator.py—— AprilTag位姿估计器

import cv2 import numpy as np import apriltag import math class AprilTagPoseEstimator: def __init__(self, tag_family='tag36h11', camera_matrix=None, dist_coeffs=None): self.detector = apriltag.Detector(apriltag.DetectorOptions(families=tag_family)) self.camera_matrix = camera_matrix self.dist_coeffs = dist_coeffs # 定义一个标准的0.15m边长的AprilTag的3D点 self.tag_size = 0.15 self.obj_points = np.array([ [-self.tag_size/2, -self.tag_size/2, 0], [ self.tag_size/2, -self.tag_size/2, 0], [ self.tag_size/2, self.tag_size/2, 0], [-self.tag_size/2, self.tag_size/2, 0] ], dtype=np.float32) def estimate_pose(self, frame): gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) detections = self.detector.detect(gray) results = [] for detection in detections: # 获取检测到的4个角点 corners = detection.corners.astype(np.int32) # 准备2D点 img_points = np.array(corners, dtype=np.float32) # 使用AP3P获取初值 _, rvec_ap3p, tvec_ap3p = cv2.solvePnP( self.obj_points, img_points, self.camera_matrix, self.dist_coeffs, flags=cv2.SOLVEPNP_AP3P ) # 用初值进行迭代精炼 _, rvec, tvec = cv2.solvePnP( self.obj_points, img_points, self.camera_matrix, self.dist_coeffs, rvec_ap3p, tvec_ap3p, True, cv2.SOLVEPNP_ITERATIVE ) # 重投影验证 imgpoints2, _ = cv2.projectPoints( self.obj_points, rvec, tvec, self.camera_matrix, self.dist_coeffs ) error = cv2.norm(img_points, imgpoints2, cv2.NORM_L2) / len(imgpoints2) if error < 1.0: # 重投影误差小于1像素才可信 # 转换为欧拉角 rmat, _ = cv2.Rodrigues(rvec) yaw = math.atan2(rmat[1,0], rmat[0,0]) pitch = math.asin(-rmat[2,0]) roll = math.atan2(rmat[2,1], rmat[2,2]) results.append({ 'id': detection.tag_id, 'tvec': tvec.flatten(), 'rvec': rvec.flatten(), 'yaw': yaw, 'pitch': pitch, 'roll': roll, 'reproj_error': error }) return results # 测试代码 if __name__ == "__main__": with open("calibration.pkl", "rb") as f: calib_data = pickle.load(f) estimator = AprilTagPoseEstimator(calib_data['camera_matrix'], calib_data['dist_coeffs']) cap = cv2.VideoCapture(0) while True: ret, frame = cap.read() if not ret: break poses = estimator.estimate_pose(frame) for pose in poses: tvec = pose['tvec'] cv2.putText(frame, f"ID:{pose['id']}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2) cv2.putText(frame, f"X:{tvec[0]:.2f}m", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2) cv2.putText(frame, f"Y:{tvec[1]:.2f}m", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2) cv2.putText(frame, f"Z:{tvec[2]:.2f}m", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2) cv2.imshow("Pose Estimation", frame) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows()

模块4：serial_communicator.py—— CV1串口通信器

import serial import struct import time class CV1SerialCommunicator: def __init__(self, port='/dev/ttyUSB0', baudrate=115200): self.ser = serial.Serial(port, baudrate, timeout=0.1) self.ser.flushInput() self.ser.flushOutput() def send_target(self, x, y, yaw): # 构建帧：STX + LEN + CMD + PAYLOAD + CHECKSUM + ETX payload = struct.pack('fff', x, y, yaw) # 3个float32 frame_len = len(payload) + 4 # +4 for CMD, CHECKSUM, STX, ETX frame = bytearray() frame.append(0x02) # STX frame.append(frame_len) # LEN frame.append(0x01) # CMD: Target Control frame.extend(payload) # CRC8校验 crc = self._crc8(frame[0:-1]) frame.append(crc) frame.append(0x03) # ETX # 发送 self.ser.write(frame) # 等待ACK return self._wait_for_ack() def _crc8(self, data): crc = 0 for byte in data: crc ^= byte for _ in range(8): if crc & 0x80: crc = (crc << 1) ^ 0x07 else: crc <<= 1 crc &= 0xFF return crc def _wait_for_ack(self, timeout=0.1): start_time = time.time() while time.time() - start_time < timeout: if self.ser.in_waiting > 0: byte = self