前言
在阅读无人机跟踪论文Fast-Tracker时,突然好奇是如何实现目标识别与定位的,发现文中是使用了一种“二维码”AprilTag进行定位。使用ApirlTag,系统可以迅速得到该二维码距离距离自己(摄像头)有多远,处于什么位姿。这是一种稳定的目标识别与定位方法,常用于后续算法,如跟踪算法,的验证。![]()
补充,在现实应用与商业无人机(例如大疆)上,目标的识别通常通过图像识别算法来实现。
关于ApirlTag,其是由密歇根大学的Edwin Olson于2011年发表的,虽然看起来像是二维码,但是于日常中常见的二维码有许多不同。即AprilTag的核心目的不是为了存储大量数据,而是为了在极端的环境(光照不均、距离很远、角度倾斜、部分被遮挡)下,依然能被摄像头瞬间认出来,并精确计算出它在三维空间中的位置和姿态(即 6个自由度:前后、左右、上下,以及俯仰、偏航、滚转)。 所以AprilTag看起来比一般的二维码简单许多。
注意AprilTag不仅是一种标签生成的方式,也是一种标签检测系统
算法框架
对于每一张输入的灰度图,都将进行以下操作
边缘检测
- 计算像素“梯度”:计算相邻点像素值变化的值与方向
- 聚类与画线:将变化程度与方向相近的点聚类,然后拟合出一条直线
四边形检测
- 在上述所有线段中寻找首尾相连、逆时针环绕的四条线段
- 抗遮挡:允许线段的端点之间存在缺口,即不必真的“首尾相连”
位姿估计
- 系统提前知道真实的AprilTag是边长已知的正方形,但由于透视,其在照片中可能变成了一个梯形或者菱形
- 系统通过对比已知的正方形和照片中畸变的四边形,并利用相机参数,可以求出 单应性矩阵 ,类似于变换矩阵,从而可以逆向推断出照片中的ApirlTag的位姿
读取ID
- AprilTag不仅能推断出标签的位姿,还能识别内部的自带的ID
- 而AprilTag并不是简单将低于/高于固定灰度值的部分分为白色/黑色,而是建立一个随空间变化的光照模型,即通过相对亮度区分,于是可以应对光照不均匀的问题
纠错防伪
画面中可能存在例如斑马线、黑白格子衫这样的物体,要如何保证系统不会认错标签?
- 保证汉明距离:系统生成的每一张AprilTag,其黑白格子的图案和其他的标签都有巨大差别,保证即使读错了几个格子也能猜出正确的ID
- 图案复杂:AprilTag编码在设计之初就淘汰了如纯黑这样几何复杂度低、且易在自然中产生的图案
具体实现
假设输入一张(w, h)的灰度图,其数值为0~255,系统应该如何处理
边缘检测
此处方法与ARTag的检测器相似。是整个过程中最为耗时的部分,通常可以以一半的分辨率执行,提高4倍速度。
计算梯度
首先对于图像上的任意一点 (x,y),考察它水平方向和垂直方向的亮度差
- 水平差分:$g_x = I(x+1,y) - I(x-1,y)$
- 垂直差分:$g_y = I(x,y+1) - I(x,y-1)$
从而可以计算出梯度的大小与方向 - 梯度幅值:$M(x,y) = \sqrt{g_x(x,y)^2 + g_y(x,y)^2}$
- 梯度方向:$\theta(x,y) = \operatorname{arctan2}(g_y(x,y), g_x(x,y))$
聚类
经过上述操作,图上每一个像素点p都有了两个物理量,然后使用基于图的聚类方法。
创建一个图,其中每个节点代表一个像素。在相邻像素之间添加边,边权重等于像素在梯度方向上的差异:
然后按照权重从小到大去尝试把节点合并为一个“component”。即初始时一个节点代表了一个component,后续可能有多个component的合并。假设现在有两个相邻的component,$n$ 和 $m$,通过以下条件判断是否要将其合并:
$$ D(n \cup m) \le \min(D(n), D(m)) + \frac{K_D}{|n \cup m|} $$ $$ M(n \cup m) \le \min(M(n), M(m)) + \frac{K_M}{|n \cup m|} $$其中:
- $D(n) = \max_{p \in n} \theta(p) - \min_{p \in n} \theta(p)$ 为方向极差
- $K(n) = \max_{p \in n} M(p) - \min_{p \in n} M(p)$ 为幅值极差
- $|n \cup m|$:代表合并后群落包含的像素总数。
- $K_D$:人为设定的常数。
- 巧妙之处:公式右侧的 $\frac{K_D}{|n \cup m|}$ 是一个动态宽容度项。当群落很小(像素少)时,分母小,宽容度极大,允许像素快速抱团,具有极强的抗噪能力;当群落成长为长线段时(像素多),宽容度迅速趋近于 0,算法变得极其严苛,强制保证最终提取出的线段必须是“笔直且方向均匀”的。
画线
使用最小二乘法,对于一个component的像素拟合出几何直线方程
1.计算加权重心 以像素的梯度幅值 $M$ 作为权重 $w_i$,由于处在“悬崖”边缘的像素更具代表性,赋予其更高权重。设共有 $N$ 个像素,计算加权平均坐标 $(\bar{x}, \bar{y})$:
随后将所有坐标平移至重心处(去中心化):$x_i^{\prime} = x_i - \bar{x}, \quad y_i^{\prime} = y_i - \bar{y}$
2. 构建加权协方差矩阵 利用去中心化后的坐标,构建一个 $2 \times 2$ 的加权协方差矩阵 $\mathbf{C}$ 来描述像素的几何分布形态:
$$ \mathbf{C} = \begin{bmatrix} C_{xx} & C_{xy} \\ C_{xy} & C_{yy} \end{bmatrix} $$其中矩阵元素的计算公式如下:
$$ C_{xx} = \frac{\sum_{i=1}^{N} w_i \cdot (x_i^{\prime})^2}{\sum_{i=1}^{N} w_i}, \quad C_{yy} = \frac{\sum_{i=1}^{N} w_i \cdot (y_i^{\prime})^2}{\sum_{i=1}^{N} w_i}, \quad C_{xy} = \frac{\sum_{i=1}^{N} w_i \cdot x_i^{\prime} \cdot y_i^{\prime}}{\sum_{i=1}^{N} w_i} $$3. 求解特征向量得到直线方程 直线的走向(倾斜角 $\theta$)即为该协方差矩阵最大特征值对应的特征向量方向。其解析解可以直接通过下式计算:
$$ \theta = \frac{1}{2} \operatorname{arctan2}\bigl(2 C_{xy}, C_{xx} - C_{yy}\bigr) $$最终,利用倾斜角 $\theta$ 和重心 $(\bar{x}, \bar{y})$ ,我们可以直接写出没有任何数学奇点的亚像素级直线标准方程 $Ax + By + C = 0$:
$$ A = -\sin\theta, \quad B = \cos\theta, \quad C = -(A\bar{x} + B\bar{y}) $$四边形检测
使用深度为4的深度优先搜索,并在深度一考虑所有线段,而在深度二至四考虑所有开始距离前一个线段结束位置“足够近”并且遵循逆时针缠绕 顺序的线段。
通过调整“足够接近”阈值来处理对遮挡和分割错误的鲁棒性:通过使阈值变大,可以处理边缘 周围的显着间隙。文中“足够接近”的阈值是线长度的两倍加上五个额外像素。
在找到四边形后边可以得到对应四个角在二维图像中的坐标
位姿估计
在找到了二维照片上的 4 个精确角点后,我们需要将其映射回真实的三维物理空间。这里用到了计算机视觉中最经典的“小孔相机投影模型”。
1. 建立巧妙的世界坐标系 将世界坐标系的原点强行定义在真实 AprilTag 的正中心,并让标签平铺在 $X-Y$ 平面上。这一设定的绝妙之处在于,标签上所有点的物理 $Z$ 坐标全部归零:
2. 投影矩阵的“降维打击” 根据相机的透视投影公式,世界坐标 $(X_w, Y_w, Z_w)$ 投影到图像像素坐标 $(u, v)$ 满足以下关系:
$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{r_3} & \mathbf{t} \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} $$参数说明:
- $s$:透视缩放因子(由于“近大远小”丢失的深度信息)。
- $\mathbf{K}$:相机的 $3 \times 3$ 内参矩阵(包含焦距和光心)。
- $\mathbf{r_1}, \mathbf{r_2}, \mathbf{r_3}$:相机旋转矩阵 $\mathbf{R}$ 的三个列向量。
- $\mathbf{t}$:平移向量 $[t_x, t_y, t_z]^T$,即相机到标签中心的真实物理距离。
由于 $Z_w = 0$,矩阵乘法中涉及 $\mathbf{r_3}$ 的项统统被消灭(乘以 0)。原本 $3 \times 4$ 的复杂空间投影,完美坍缩为一个 $3 \times 3$ 的平面映射矩阵,这就是**单应性矩阵 $\mathbf{H}$**:
$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{t} \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ 1 \end{bmatrix} \quad \Longrightarrow \quad \mathbf{H} = \mathbf{K} \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{t} \end{bmatrix} $$(通过四对已知的角点坐标,运用直接线性变换 DLT 算法即可解出 $\mathbf{H}$ 的所有元素。)
解出单应性矩阵 $\mathbf{H}$ 后,我们需要逆向剥离出我们真正想要的平移向量 $\mathbf{t}$(距离)和旋转矩阵 $\mathbf{R}$(姿态)。
3. 消除相机内参干扰 在等式两边同时左乘相机内参矩阵的逆 $\mathbf{K}^{-1}$,我们将计算结果记为三个列向量 $\mathbf{h_1}, \mathbf{h_2}, \mathbf{h_3}$:
这实际上给出了三个极其关键的等式:
$$ \mathbf{h_1} = s \cdot \mathbf{r_1}, \quad \mathbf{h_2} = s \cdot \mathbf{r_2}, \quad \mathbf{h_3} = s \cdot \mathbf{t} $$4. 利用刚体物理定理求解缩放因子 $s$ 在真实的物理空间中,旋转矩阵的列向量 $\mathbf{r_1}$ 和 $\mathbf{r_2}$ 代表方向轴,它们的模长(长度)必须严格等于 1(即 $|\mathbf{r_1}| = |\mathbf{r_2}| = 1$)。基于这个物理公理,我们可以直接算出透视缩放因子 $s$。为了减少图像噪点误差,通常取前两列模长的几何平均值:
$$ s = \sqrt{\|\mathbf{h_1}\| \cdot \|\mathbf{h_2}\|} $$5. 提取平移距离与旋转轴 既然 $s$ 已经求出,终极目标瞬间解开:
- 物理距离(平移向量):直接拿到了无人机距离目标的绝对 $X, Y, Z$ 坐标!
- 物理姿态(旋转向量):解出真实的 $X$ 轴和 $Y$ 轴朝向。
- 找回丢失的 $Z$ 轴:利用三维几何的右手法则,通过向量的叉乘(Cross Product)无中生有地生成垂直于平面的第三个旋转轴:
至此,我们初步拼凑出了 $3 \times 3$ 的旋转矩阵 $\tilde{\mathbf{R}} = \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{r_3} \end{bmatrix}$。
6.旋转矩阵正交化 由于像素离散误差和镜头畸变,我们粗略拼凑出来的 $\tilde{\mathbf{R}}$ 往往并不完美(三个轴可能不绝对正交)。如果将其直接输入无人机飞控,会导致严重的姿态计算错误。
数学解法(极分解): 我们需要寻找一个在三维旋转群 $SO(3)$ 中,距离 $\tilde{\mathbf{R}}$ 最近的“绝对完美的正交旋转矩阵” $\mathbf{R}^*$。通过对粗糙矩阵 $\tilde{\mathbf{R}}$ 进行奇异值分解(SVD):
将代表拉伸变形的对角阵 $\mathbf{\Sigma}$ 强制替换为单位阵 $\mathbf{I}$,即可得到完全正交的最优旋转矩阵:
$$ \mathbf{R}^* = \mathbf{U} \mathbf{V}^T $$读取ID
在真实的物理环境中,光照往往是极度不均匀的。比如标签一半暴露在刺眼的阳光下,一半掩盖在浓重的树荫里。此时,阴影区域的“纯白色”像素,其绝对亮度可能比阳光区域的“纯黑色”像素还要暗。如果使用一个全局固定的亮度阈值(如 128)去二值化图像,阴影区的密码将全军覆没。
数学解法:建立空间光照曲面 AprilTag 利用了其本身固有的几何设计:标签的最外圈固定是一层白边,往内固定是一层黑框。这些边框的颜色是绝对已知的。 算法在这些已知的白边和黑框中采样若干像素点 $(x_i, y_i)$ 以及它们的真实灰度值 $I_i$,并在数学上假设光照强度的变化在二维空间中是一个双线性平面(Bilinear Surface)。
为此,算法分别构建了两个连续的二维空间亮度函数:
黑色区域的光照曲面模型:
白色区域的光照曲面模型:
$$ I_{\text{white}}(x, y) = A_2 x + B_2 xy + C_2 y + D_2 $$现在,算法准备去读取标签内部的任意一个数据网格(密码位)。假设该网格中心的像素坐标为 $(x_k, y_k)$。我们不再去和一个死板的固定数字比大小,而是将该坐标分别代入刚刚拟合出的两张曲面方程中,计算出它**专属的动态阈值 $T(x_k, y_k)$**(即黑与白的中间值):
$$ T(x_k, y_k) = \frac{I_{\text{black}}(x_k, y_k) + I_{\text{white}}(x_k, y_k)}{2} $$最后,用该坐标点真实的相机传感器亮度 $I_{\text{actual}}(x_k, y_k)$ 与其专属阈值进行比较,直接输出二进制位:
$$ \text{Bit}(x_k, y_k) = \begin{cases} 1 \text{ (白)} & \text{if } I_{\text{actual}}(x_k, y_k) > T(x_k, y_k) \\ 0 \text{ (黑)} & \text{if } I_{\text{actual}}(x_k, y_k) \le T(x_k, y_k) \end{cases} $$纠错防伪
除了上面所有的检测措施之外,AprilTag在设计时还加入了许多限制
汉明距离纠错
为了保证即使部分被遮挡也能成功读取,在设计时保证了任何两个标签之间有足够的汉明距离,例如36h10一族标签代表了有36个格子,最小汉明距离为10
旋转不变性
AprilTag还限制了任意标签在旋转后与原来的自己也保持最小汉明距离,从而可以迅速读出标签的旋转
几何复杂度
自然界中也可能出现类似的图形,例如一个方形窗户中的光阴。AprilTag设定了每个标签绘制所需要的最少矩形个数,排除了如纯黑色标签这种形式
总结
AprilTag通过数学上的精妙运算,实现了一种快速准确且能够在极端环境运行的视觉定位系统
ApirlTag(2011)在后续还推出了ApirlTag2(2016)、ApirlTag3(2019)。分别实现了更精准快速的检测器、更多样化的标签生成机制。