【电赛/毕设降维打击】别再按像素算距离了！相机标定、单目 PnP 姿态解算与三维真实坐标系转换硬核指南-尧图网络科技

前言
无论是激光打靶、机械臂物料抓取，还是无人机的 AprilTag 自主降落，机器视觉的最终目的只有一个：把图像上的“二维像素坐标”，转换成真实世界中的“三维物理坐标（毫米/厘米）”！
很多新手队伍是怎么做的？
“画面总宽度 320 像素，我量了一下现实中是 32 厘米，所以 10 像素 = 1 厘米！”
这是极其致命的玩具级思维！只要你的小车往前走了一步，或者摄像头稍微低了一下头，这个比例关系瞬间作废，你的激光笔会打得满天乱飞！
真正的大疆工程师和国一队伍，全都在使用工业级的视觉底层数学模型：针孔相机模型与 PnP（Perspective-n-Point）解算。
本文将用大白话带你跨越二次元与三维物理世界的鸿沟，教你如何用单目摄像头，精准算出目标物体在真实世界中的 X、Y、Z 坐标与俯仰、偏航角！

@TOC

一、认知颠覆：为什么“按像素算距离”绝对会死？

由于透视原理（近大远小），真实世界中长度相等的两根棍子，如果一根离得近，一根离得远，它们在画面上占用的像素是完全不一样的。
此外，摄像头镜头存在畸变（Distortion），画面边缘的直线会被拉弯（鱼眼效应/桶形畸变）。如果你在画面边缘依然用线性比例去算物理距离，误差可能高达十几厘米！

🏆 视觉的真相：针孔相机模型

摄像头的本质，是一个**“降维机器”，它把真实世界的 3D 坐标

(X,Y,Z)(X,Y,Z)

，拍扁成了图像上的 2D 坐标

(u,v)(u,v)

。
我们要做的，是把这个降维过程“反推”回去！要反推，单片机或电脑就必须确切地知道这颗摄像头的“物理性格”——这就叫相机内参（Intrinsic Parameters）**。

二、视觉灵魂注入：相机标定（Camera Calibration）

如果你买回摄像头直接用，那它就是个瞎子。所有用于精密测量的工业摄像头，使用前必须进行“标定”！

1. 内参矩阵与畸变系数

你需要告诉 OpenCV 或大模型以下两个核心参数：

内参矩阵（Camera Matrix）：包含焦距
```
(fx,fy)(fx,fy)
```
和光学中心点
```
(cx,cy)(cx,cy)
```
。它决定了光线是怎么折射进感光芯片的。
畸变系数（Distortion Coeffs）：通常是 5 个数字
```
(k1,k2,p1,p2,k3)(k1,k2,p1,p2,k3)
```
。它描述了镜头的玻璃是如何把画面“拉弯”的。

2. 怎么标定？（张正友黑白棋盘格法）

找一张极其平整的硬纸板，打印一张标准的黑白棋盘格。

拿着摄像头，从各个角度、远近、倾斜拍摄这张棋盘格（约 20~30 张照片）。
把这些照片丢进 MATLAB 的 Camera Calibrator 工具箱，或者跑一段简单的 OpenCV Python 脚本。
电脑会自动寻找黑白角点，利用复杂的非线性优化，几秒钟内算出你这颗摄像头的绝对内参和畸变系数！

赛场避坑：电赛由于时间紧迫，你可以赛前就在实验室把你的摄像头标定好，把内参矩阵硬编码写死在你的 C 语言或 Python 脚本里！只要你比赛时不换镜头、不调焦，这组参数一辈子管用！

三、核心黑魔法：单目 PnP 姿态解算（SolvePnP）

现在我们的摄像头已经被“矫正”成了一只完美的眼睛。接下来，我们要用它来测距和测角度！
如果是双目摄像头，靠视差可以直接测距；但电赛中最常用的是单目摄像头（只有一个镜头），怎么算出物体的远近

(Z轴)(Z轴)

呢？

🏆 PnP 算法的前提：你必须知道目标的“真实尺寸”！

单目测距的终极物理定律：因为“近大远小”，只要我知道这个东西在现实中到底有多大，再看看它在画面里占了多少像素，经过内参矩阵反推，我就能绝对精准地算出它离我有多远、以什么姿态摆放！

实战：已知 4 个角点的 PnP 解算

假设我们要瞄准一个正方形的靶纸，现实中我们用尺子量了，它边长为 10cm。
我们把靶子的中心定义为三维世界坐标的 (0, 0, 0)。
那么它的四个角在现实中的 3D 坐标是：
(-5, 5, 0), (5, 5, 0), (5, -5, 0), (-5, -5, 0) （单位：厘米）

当摄像头拍到靶子时，画面上找到了这四个角点的 2D 像素坐标（如 (100, 120)...）。

Python + OpenCV 极简解算代码（核弹级）：

codePython

import cv2 import numpy as np # 1. 填入你提前标定好的相机内参矩阵和畸变系数 camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float64) dist_coeffs = np.array([k1, k2, p1, p2, k3], dtype=np.float64) # 2. 定义目标物体在真实世界中的 3D 坐标 (比如边长10cm的正方形) obj_points = np.array([ [-5.0, 5.0, 0], [ 5.0, 5.0, 0], [ 5.0, -5.0, 0], [-5.0, -5.0, 0] ], dtype=np.float64) # 3. 图像处理提取到的 4 个角点的 2D 像素坐标 img_points = np.array([ [u1, v1], [u2, v2], [u3, v3], [u4, v4] ], dtype=np.float64) # 4. ！！！核心黑魔法：调用 SolvePnP ！！！ success, rotation_vector, translation_vector = cv2.solvePnP( obj_points, img_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE ) # 5. 打印结果 print("平移向量 (X, Y, Z 厘米):") print(translation_vector) # 这就是摄像头距离靶心在真实空间中的绝对 X,Y,Z 距离！

震撼威力：translation_vector 里的三个数，就是物理世界中，摄像头相对于靶心绝对的左右偏移 X、上下偏移 Y、以及直线距离 Z（单位就是你定义的厘米）！误差可以控制在毫米级！

四、降维外挂：AprilTag 与 OpenMV 极速空间定位

如果你不会用 OpenCV 搞复杂的角点提取，电赛官方和 OpenMV 给你准备了一个终极“物理外挂”：AprilTag（视觉二维码）。

在无人机定点降落、或者机械臂物料抓取时，只要允许在场地上贴二维码，无脑上 AprilTag！
它的内部封装了极其强悍的防遮挡算法和上述的 PnP 解算。

在 OpenMV 中的极简调用：

codePython

import sensor, image, math # 必须设置你的摄像头焦距等参数才能算物理距离（需要标定） f_x = (2.8 / 3.984) * 160 # 依据镜头换算 f_y = (2.8 / 2.952) * 120 c_x = 160 * 0.5 c_y = 120 * 0.5 sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) # 160x120 sensor.skip_frames(time = 2000) while(True): img = sensor.snapshot() # 直接扫描画面中的 AprilTag for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): img.draw_rectangle(tag.rect(), color = (255, 0, 0)) # tag.x_translation(), tag.y_translation(), tag.z_translation() # 就是摄像头距离这个二维码的真实 3D 距离！ # 将 Tx, Ty, Tz 打包通过串口发送给 STM32 print("X偏移: %f, Y偏移: %f, 距离Z: %f" % (tag.x_translation(), tag.y_translation(), tag.z_translation()))