用 AprilTag 做视觉定位：从生成标签到解算位姿（实践指南）

关键词：AprilTag、视觉定位、位姿估计、旋转矩阵 R、平移 t、无人机降落

本文用一个可运行的最小 demo，带你从「生成一张标准标签」一路走到「读懂检测出来的旋转矩阵 R 和平移向量 t」，最后聊聊它在无人机上的真实用法。代码全程在 Python + OpenCV + dt-apriltags 下验证通过。

---

0. 为什么是 AprilTag，而不是 ArUco？

做标志物（fiducial marker）检测时，最常见的两个选择是 ArUco 和 AprilTag：

• ArUco：OpenCV 自带 cv2.aruco 模块，开箱即用，但它是 OpenCV 自己的码表（DICT_4X4_50 等），图案体系独立。
• AprilTag：由 APRIL Robotics Lab（密歇根大学 Edwin Olson 组）发布，码字公开固定，任何兼容 AprilTag 的检测器全球都能识别同一张图，在机器人/无人机圈事实标准。

重要坑：AprilTag 和 ArUco 是两套不同的码，互相不认。你用 AprilTag 检测器生成的标签，OpenCV 的 aruco 模块检测不出来；反过来也一样。本文全程使用 AprilTag。

---

1. 环境准备

# 需要一个 OpenCV 和一个 AprilTag 检测库
pip install opencv-python numpy
pip install dt-apriltags        # 自带 C 库，支持位姿估计，推荐
# 备选：pip install apriltag   # pupil-labs 的包，也行

# 有相机就插上（/dev/video0）；没有也能跑离线图片模式
ls /dev/video*

需要说明：有些 Python 镜像源里没有 pupil-apriltag，但 dt-apriltags 一般能直接装，本文用它。

---

2. 项目结构

apriltag_demo/
├── apriltag_generate.py   # 生成标签图案
├── apriltag_detect.py     # 检测 + 画框 + 位姿
├── apriltag_pose_demo.py  # 演示 yaw/pitch/roll 下 R 的形状
├── tag36h11.c             # 官方码表源码（生成器解析用）
└── tag36h11_0.png ...     # 生成的样例标签

---

3. 第一步：生成一张标准标签

AprilTag 每个 (family, id) 对应一个固定的码字（codeword）。要画出正确图案，需要知道：

• codedata[]：该 family 每个 id 的码字（uint64）
• bit_x / bit_y：每个数据比特在网格里的坐标
• width_at_border / total_width：网格尺寸

这些东西都在官方源码 <family>.c 里。我们的生成器直接解析这个 .c 文件，再按官方 apriltag_to_image() 的算法渲染，跨版本都稳，不依赖解析动态库内部结构。

运行：

python3 apriltag_generate.py                 # 默认生成 tag36h11 的 id=0
python3 apriltag_generate.py --ids 0 1 2 3   # 生成多个
python3 apriltag_generate.py --family tag25h9 --ids 5 --px 12
python3 apriltag_generate.py --print         # 用字符打印图案结构

--px 20 表示每个格子放大 20 像素，方便打印/显示给相机识别。生成出来的 id=0/1/2 是标准 tag36h11 码，和世界上任何人用官方工具生成的是同一张图。

小知识：tag36h11 里的「36」是数据比特数，「11」是最小汉明距离（容错能力），整个家族共 587 个 id。id=0/1/2 没有特殊地位，只是编号最小的三个。

生成器核心代码（apriltag_generate.py）

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""生成 AprilTag 图案（自包含、离线可用）。解析官方 <family>.c 渲染。"""
import argparse, os, re, sys
import cv2
import numpy as np

BASE = os.path.dirname(os.path.abspath(__file__))
SRC_URL = "https://raw.githubusercontent.com/AprilRobotics/apriltag/master/{}.c"

def load_family(family):
    """解析官方的 <family>.c，返回 (codes, bit_x, bit_y, nbits, w_border, total, reversed)"""
    path = os.path.join(BASE, f"{family}.c")
    if not os.path.exists(path):
        import urllib.request
        print(f"[info] 本地没有 {family}.c，尝试下载…")
        urllib.request.urlretrieve(SRC_URL.format(family), path)
    txt = open(path, "r", encoding="utf-8", errors="ignore").read()

    # 1) codedata[N] = { 0x..., 0x..., ... };
    m = re.search(r"codedata\s*\[\s*(\d+)\s*\]\s*=\s*\{(.*?)\};", txt, re.S)
    ncodes = int(m.group(1))
    codes = [int(h.replace("UL", "").replace("ul", ""), 16)
             for h in re.findall(r"0x[0-9a-fA-F]+", m.group(2))]
    assert len(codes) == ncodes

    # 2) 标量参数
    def grab_int(name):
        mm = re.search(rf"tf->{name}\s*=\s*([^;]+);", txt)
        return mm.group(1).strip() if mm else None
    nbits = int(grab_int("nbits"))
    w_border = int(grab_int("width_at_border"))
    total = int(grab_int("total_width"))
    rev = grab_int("reversed_border")
    reversed_border = (rev == "true" or rev == "1") if rev else False

    # 3) bit_x[i] / bit_y[i]
    bx = [0] * nbits
    by = [0] * nbits
    for i, v in re.findall(r"tf->bit_x\[(\d+)\]\s*=\s*(\d+)", txt):
        bx[int(i)] = int(v)
    for i, v in re.findall(r"tf->bit_y\[(\d+)\]\s*=\s*(\d+)", txt):
        by[int(i)] = int(v)
    return codes, bx, by, nbits, w_border, total, reversed_border

def render_tag(codes, bit_x, bit_y, nbits, w_border, total, reversed_border, idx, px):
    """按官方 apriltag_to_image() 算法渲染第 idx 个码字"""
    code = codes[idx]
    grid = np.zeros((total, total), dtype=np.uint8)        # 黑底
    white_w = w_border + (0 if reversed_border else 2)     # 白色 quiet zone
    ws = (total - white_w) // 2
    for i in range(white_w - 1):
        grid[ws, ws + i] = 255
        grid[ws + i, total - 1 - ws] = 255
        grid[total - 1 - ws, ws + i + 1] = 255
        grid[ws + 1 + i, ws] = 255
    border_start = (total - w_border) // 2
    for i in range(nbits):
        if code & (1 << (nbits - i - 1)):                  # 第 i 位为 1 -> 画白
            grid[bit_y[i] + border_start, bit_x[i] + border_start] = 255
    return cv2.resize(grid, (total * px, total * px), interpolation=cv2.INTER_NEAREST)

SUPPORTED = ["tag16h5","tag25h7","tag25h9","tag36h10","tag36h11","tag36artoolkit"]

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--family", default="tag36h11", choices=SUPPORTED)
    ap.add_argument("--ids", nargs="+", type=int, default=[0])
    ap.add_argument("--px", type=int, default=10)
    ap.add_argument("--out", default=None)
    ap.add_argument("--print", action="store_true")
    args = ap.parse_args()
    codes, bx, by, nbits, w_border, total, rev = load_family(args.family)
    if max(args.ids) >= len(codes):
        sys.exit(f"id 超出范围，{args.family} 只有 0..{len(codes)-1}")
    for tid in args.ids:
        img = render_tag(codes, bx, by, nbits, w_border, total, rev, tid, args.px)
        out = args.out if (len(args.ids)==1 and args.out) else f"{args.family}_{tid}.png"
        cv2.imwrite(out, img)
        print(f"[ok] 已保存 {out}  ({img.shape[1]}x{img.shape[0]})")

if __name__ == "__main__":
    main()

验证：生成 id=0/1/2 后立刻用检测器回环识别，能正确认出 0/1/2，说明图案是标准的。

---

4. 第二步：检测 + 位姿估计

检测脚本支持相机实时和单张图片两种模式，并可选绘制 xyz 坐标系。

python3 apriltag_detect.py                      # 相机实时，tag36h11
python3 apriltag_detect.py --camera 1           # 指定设备
python3 apriltag_detect.py --image tag36h11_0.png
python3 apriltag_detect.py --pose --tag-size 0.1   # 开启位姿 + 画坐标系

检测参数到底是什么意思

Detector(...) 构造参数（决定怎么找标签）

参数	含义
families	用哪个家族，如 'tag36h11'
nthreads	并行线程数
quad_decimate	检测前把图缩小几倍（如 2.0 = 长宽各半），大幅加速、精度略降
quad_sigma	降采样后高斯模糊的 sigma，抑制噪点
refine_edges	是否对四边形边缘做亚像素细化（1=开，角点更准）
decode_sharpening	解码前锐化强度，提升解码成功率

detector.detect(...) 调用参数（决定解出什么）

dets = detector.detect(
    gray,                                       # 位置参数：必须是灰度图
    estimate_tag_pose=args.pose,                # 是否估位姿
    camera_params=(fx, fy, cx, cy),             # 相机内参
    tag_size=args.tag_size,                     # 标签真实边长
)

• 第 1 个参数 gray：灰度图（单通道 8-bit），彩图会报错。
• estimate_tag_pose=True 才解算旋转 R / 平移 t。
• camera_params=(fx, fy, cx, cy)：焦距（像素）+ 主点。没标定时用代码中 default_camera_matrix（fx=fy=宽×0.9，主点取中心）近似。
• tag_size：标签真实物理边长，单位要和相机内参一致（如都填米）。只有开位姿时才用，用来把像素尺度换算成真实距离——t[2] 就是标签到相机的距离。

每个检测结果 det 提供：

• det.tag_id：标签编号
• det.center：中心点 (x, y)
• det.corners：4 个角点（顺序：左上、右上、右下、左下）
• det.pose_R（3×3）、det.pose_t（3×1）：位姿（开启时）

检测脚本核心代码（apriltag_detect.py）

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""AprilTag 检测学习 demo（相机 / 图片两种模式，可选位姿）。"""
import argparse
import cv2
import numpy as np
from dt_apriltags import Detector

FAMILY = "tag36h11"

def draw_and_print(frame, detections, estimate_pose, camera_matrix, dist_coeffs, tag_size):
    for det in detections:
        pts = det.corners.astype(int).reshape((-1, 1, 2))
        cv2.polylines(frame, [pts], isClosed=True, color=(0, 255, 0), thickness=2)
        cx, cy = int(det.center[0]), int(det.center[1])
        cv2.circle(frame, (cx, cy), 4, (0, 0, 255), -1)
        cv2.putText(frame, f"id={det.tag_id}", (cx-10, cy-10),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0), 2)
        if estimate_pose and det.pose_R is not None:
            R, t = det.pose_R, det.pose_t
            rvec, _ = cv2.Rodrigues(R)
            cv2.drawFrameAxes(frame, camera_matrix, dist_coeffs, rvec, t,
                              length=tag_size, thickness=2)
            cv2.putText(frame, f"z={t[2][0]:.2f}m", (cx+10, cy+10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 2)
        line = (f"[detect] id={det.tag_id}  center=({det.center[0]:.1f},{det.center[1]:.1f})  "
                f"corners={det.corners.astype(int).tolist()}")
        if estimate_pose and det.pose_t is not None:
            t = det.pose_t.ravel()
            line += (f"  t=[{t[0]:+.3f},{t[1]:+.3f},{t[2]:+.3f}]m  "
                     f"dist={np.linalg.norm(t):.3f}m")
        print(line)
        if estimate_pose and det.pose_R is not None:
            R = det.pose_R
            euler = cv2.RQDecomp3x3(R)[0]               # (rotX, rotY, rotZ)
            R_show = np.where(np.abs(R) < 0.5, 0.0, R)  # 小于0.5当0，结构更清晰
            print(f"         R=\n{np.round(R_show, 3)}\n         euler(deg): "
                  f"rotX{euler[0]:+.1f}  rotY{euler[1]:+.1f}  rotZ{euler[2]:+.1f}")

def default_camera_matrix(w, h):
    fx = fy = w * 0.9
    cx, cy = w / 2.0, h / 2.0
    return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float64)

def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--camera", type=int, default=0)
    ap.add_argument("--image", default=None)
    ap.add_argument("--family", default=FAMILY)
    ap.add_argument("--pose", action="store_true")
    ap.add_argument("--tag-size", type=float, default=0.1)
    args = ap.parse_args()
    detector = Detector(families=args.family, nthreads=4, quad_decimate=2.0,
                        quad_sigma=0.8, refine_edges=1, decode_sharpening=0.25)
    cam_w = cam_h = 640
    camera_matrix = default_camera_matrix(cam_w, cam_h)
    dist_coeffs = np.zeros((4, 1), dtype=np.float64)
    if args.image:
        frame = cv2.imread(args.image)
        cam_h, cam_w = frame.shape[:2]
        camera_matrix = default_camera_matrix(cam_w, cam_h)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        dets = detector.detect(gray, estimate_tag_pose=args.pose,
            camera_params=(camera_matrix[0,0], camera_matrix[1,1],
                           camera_matrix[0,2], camera_matrix[1,2]),
            tag_size=args.tag_size)
        draw_and_print(frame, dets, args.pose, camera_matrix, dist_coeffs, args.tag_size)
        cv2.imshow("AprilTag", frame); cv2.waitKey(0); cv2.destroyAllWindows()
        return
    cap = cv2.VideoCapture(args.camera)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640); cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
    while True:
        ok, frame = cap.read()
        if not ok: break
        cam_h, cam_w = frame.shape[:2]
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        dets = detector.detect(gray, estimate_tag_pose=args.pose,
            camera_params=(camera_matrix[0,0], camera_matrix[1,1],
                           camera_matrix[0,2], camera_matrix[1,2]),
            tag_size=args.tag_size)
        draw_and_print(frame, dets, args.pose, camera_matrix, dist_coeffs, args.tag_size)
        cv2.imshow("AprilTag", frame)
        if (cv2.waitKey(1) & 0xFF) in (ord("q"), 27): break
    cap.release(); cv2.destroyAllWindows()

if __name__ == "__main__":
    main()

---

5. R 和 t 到底是谁相对谁？

屏幕/终端里打印出来的 R、t 表达的关系是：

p_cam = R · p_tag + t

即 (R, t) 是标签坐标系 → 相机坐标系的外参（extrinsic），描述「标签相对相机」的位姿。

坐标系约定（OpenCV / AprilTag 标准）：

• 相机坐标系：原点在光心，+X 向右，+Y 向下，+Z 向前（朝拍摄方向）。
• 标签坐标系：原点在标签中心，+X 向右，+Y 向下（沿标签平面），+Z 垂直标签面、朝外。

所以 t 就是标签中心在相机坐标系里的坐标，t[2]（打印的 z=…m）≈ 标签到相机的距离。

举例（标签居中、正对相机）：

[detect] id=0  ...  t=[-0.000,-0.000,+0.113]m  dist=0.113m
         R=
[[ 1.   0.   0. ]
 [ 0.   1.   0. ]
 [ 0.   0.   1. ]]
         euler(deg): rotX-0.0  rotY-0.0  rotZ-0.0

R = I 表示标签正对相机、没有倾斜；t≈[0,0,0.11] 表示标签在相机正前方约 11cm。

想求「相机在标签（世界）系下的位姿」？取逆即可：R_cam = R.T，t_cam = -R.T @ t。

---

6. 三种旋转下，R 分别长什么样？

纯 yaw / pitch / roll 各自绕一根轴转，R 有标准形状。看哪一行/列保持单位向量，那根轴就没动；非对角项出现在另外两根轴的平面上。

为了直观，我们用 3D 投影「合成」一张被倾斜的标签图，再交给检测器读回 R 对比。核心演示脚本：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""演示纯 yaw / pitch / roll 时 R 长什么样（3D 投影合成，无需相机）。"""
import sys
import numpy as np
import cv2
from dt_apriltags import Detector

W = H = 400
fx = fy = W * 0.9
K = np.array([[fx, 0, W/2], [0, fy, H/2], [0, 0, 1]], dtype=np.float64)
TAG, DIST = 0.1, 0.5   # 标签边长(米)、离相机0.5m

def rx(a): c,s=np.cos(a),np.sin(a); return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def ry(a): c,s=np.cos(a),np.sin(a); return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def rz(a): c,s=np.cos(a),np.sin(a); return np.array([[c,-s,0],[s,c,0],[0,0,1]])

def synth_tag(tag_img, R, t):
    half = TAG/2
    obj = np.array([[-half,-half,0],[half,-half,0],[half,half,0],[-half,half,0]], dtype=np.float64)
    rvec,_ = cv2.Rodrigues(R)
    pts,_ = cv2.projectPoints(obj, rvec, t, K, np.zeros((4,1)))
    pts = pts.reshape(4,2).astype(np.float32)
    src = np.array([[0,0],[tag_img.shape[1],0],[tag_img.shape[1],tag_img.shape[0]],[0,tag_img.shape[0]]], dtype=np.float32)
    M = cv2.getPerspectiveTransform(src, pts)
    return cv2.warpPerspective(tag_img, M, (W, H), borderValue=0)

def show(name, deg):
    tag = cv2.imread("tag36h11_0.png", cv2.IMREAD_GRAYSCALE)
    ang = np.deg2rad(deg)
    R = rz(ang) if name=="roll" else (rx(ang) if name=="pitch" else ry(ang))
    t = np.array([0,0,DIST], dtype=np.float64)
    canvas = synth_tag(tag, R, t)
    det = Detector(families="tag36h11", nthreads=4, quad_decimate=2.0,
                   quad_sigma=0.8, refine_edges=1, decode_sharpening=0.25)
    res = det.detect(canvas, estimate_tag_pose=True, camera_params=(fx,fy,W/2,H/2), tag_size=TAG)
    print(f"\n===== 纯 {name} {deg}° =====")
    print("施加的 R =\n", np.round(np.where(np.abs(R)<0.5,0.0,R), 3))
    if res:
        d = res[0]
        e = cv2.RQDecomp3x3(d.pose_R)[0]
        print("检测到的 R =\n", np.round(np.where(np.abs(d.pose_R)<0.5,0.0,d.pose_R), 3))
        print(f"检测 euler(deg): rotX{e[0]:+.1f}  rotY{e[1]:+.1f}  rotZ{e[2]:+.1f}")

if __name__ == "__main__":
    d = float(sys.argv[1]) if len(sys.argv) > 1 else 30.0
    for ax in ["roll","pitch","yaw"]:
        show(ax, d)

运行 python3 apriltag_pose_demo.py 30，输出（已把 <0.5 的数值归零，结构更清楚）：

Roll（绕 Z 轴，标签在画面里自旋）

[[ 0.865 -0.501  0.   ]
 [ 0.     0.865  0.   ]
 [ 0.     0.     0.999]]

→ 第三行/列是 [0,0,1]，只有左上 2×2 混 x、y；Z 轴没动。对应 rotZ=+30。

Pitch（绕 X 轴，上下点头）

[[1.    0.    0.   ]
 [0.    0.866 0.   ]
 [0.    0.    0.866]]

→ 第一行/列是 [1,0,0]，只有右下 2×2 混 y、z；X 轴没动。对应 rotX=+30。

Yaw（绕 Y 轴，左右摆头）

[[0.867 0.    0.   ]
 [0.    1.    0.   ]
 [0.    0.    0.867]]

→ 第二行/列是 [0,1,0]，只有 x、z 交叉混；Y 轴没动。对应 rotY=+30。

记忆口诀：自旋看左上 2×2，点头看右下 2×2，摆头看 x/z 反对角。

关于 cv2.RQDecomp3x3：它返回的欧拉角顺序是 (rotX, rotY, rotZ)，即分别对应绕 X / Y / Z 的转角，别和航空航天里的 pitch/yaw/roll 命名混为一谈（本文 demo 的 roll=绕Z，pitch=绕X，yaw=绕Y，符合 OpenCV 图像坐标系）。

关于「<0.5 当 0」的阈值：30° 时 sin30°=0.5 刚好保留；角度更小时（如 20°，sin≈0.34）非对角项会被清零，R 看起来像单位阵——这时以打印的 rotX/Y/Z 角度为准，「看形状认轴」适合中等以上角度。

---

7. 实战：无人机怎么用这些数据？

无人机（四旋翼）识别到 AprilTag 后，常见疑问是：是不是先把 R 调好，再动 t？

概念上：是的，姿态（R）通常先于/内于位置（t）处理，但不是串行“调完 R 再动 t”。

为什么先处理 R

你 demo 里算的 R 是标签→相机。如果标签平放地面、无人机正上方且机身水平，理想 R = I。所以：

• R 偏离 I = 无人机相对标签平面倾斜了（roll/pitch），或机头没对准（yaw）。
• 无人机一旦倾斜，标签中心在画面里的偏移会混进倾斜误差——你分不清是「偏左了」还是「只是歪了」。不先调平，位置控制会飘、会震荡。

所以降落/对准的逻辑是：先把 R 收敛到接近 I（调平 + 对齐 yaw），这时 t 的 x/y 偏移才可信 → 驱动到标签正上方 → 最后降 z。

关键前提：坐标系要转换

你打印的 R 在相机系下。飞控用的是机体系/世界系。若摄像头不是竖直朝下（有安装倾角），必须先转：

R_body = R_cam_to_body @ R_tag2cam
t_body = R_cam_to_body @ t_tag2cam (+ 平移)

不转，朝向就错了。

真实工程里的「但书」

实际无人机不会拿 AprilTag 的 R 当主姿态源——它的 roll/pitch 估计噪声大、易抖。真正做法是：

• 姿态（R）主要靠 IMU / 气压计（高频、稳）；
• AprilTag 的 R 主要用来对齐 yaw（机头朝向标签），并作为「有没有大致对准」的粗校验；
• 位置（t）才是 AprilTag 的强项：用 x/y 偏移做水平定位、用 z（距离）控制高度/降落。

典型串级控制结构

外环(位置): 看 t 的 x/y/z 偏差 -> 期望速度/高度
内环(姿态): 看 R 偏差(含 yaw 对齐) -> 调 roll/pitch/yaw 让机体跟上

内环（R/姿态）频率高于外环，体感上确实「先把姿态调好」，但两者是同时闭环，不是做完一个再做另一个。

---

8. 小结 & 常见坑

1. AprilTag ≠ ArUco，两套码互不相认，别用错检测器。
2. 生成的 id=0/1/2 是标准 tag36h11 码，全球通用；id 之间无特殊地位。
3. R|t 是标签→相机：R=I 表示正对，t[2] 是距离。
4. 看 R 认旋转轴：哪根对角轴独自为 1，哪根轴没转。
5. 阈值 <0.5 当 0 能滤掉数值噪声、让结构清晰，但小角度下会把真实旋转也清零，此时以欧拉角为准。
6. 没标定内参时用近似（fx=fy=宽×0.9），距离/角度绝对值会有误差；要准就喂自己标定好的相机矩阵。
7. 无人机落地场景：姿态靠 IMU，AprilTag 主要提供 yaw 对齐与 x/y/z 位置。

---

完整可运行代码见同目录的 apriltag_generate.py / apriltag_detect.py / apriltag_pose_demo.py。本文所有输出均在 Python 3.10 + OpenCV 4.5.4 + dt-apriltags 3.1.7 下验证。