基于视觉的传感器位置记忆追踪方法

史勇民，楼顺天，安盼盼

（1.中电海康集团有限公司，浙江 杭州 311100；2.西安电子科技大学电子工程学院，陕西 西安 710071）

引言

传感器的记忆追踪在实际生活中有着广泛的应用价值，尤其是在中医诊断中具有重要的应用价值。中医诊断理论已经越来越得到人们的认可，在缓解病人痛苦、疾病预防与治愈等方面具有独特的作用[1-2]。传统的望闻问切与现代电子测量的结合，通过检测穴位异常表现，可以提高患者寒热虚实症状的诊断率，使中医诊断治疗发挥更大的作用。由西安电子科技大学专门研制的穴位检测传感器，可以一次性获得患者128个经络穴位的异常表现测量结果，在穴位传感器移走后，如何指示出实际的测量位置，对干预治疗的效果具有至关重要的作用。

对传感器的记忆追踪的研究鲜有报道，相关的研究主要表现在遮挡目标的跟踪算法，用于解决目标短时或长时被遮挡时的跟踪问题。Comaniciu D.等专家提出了一种Mean-shift（均值漂移）算法[3-4]，利用区域加权的方法解决了目标遮挡时的跟踪问题；Maggio E.、Wang F.等专家提出了基于子块匹配的目标跟踪算法[5-7]，通过将目标区域分成若干子块，分别进行独立跟踪，这样就可以对未全部遮挡的目标进行跟踪；对于目标完全遮挡的目标，采用卡尔曼滤波算法进行轨迹预测[8-9]，可以实现运动目标的跟踪；但对非线性、非高斯的动态系统的目标跟踪问题，卡尔曼滤波已经不再适用，Gordon N.J.、Doucet A.和Isard M.研究了贝叶斯理论下的粒子滤波跟踪算法[10-12]，解决了目标部分遮挡、短时间全部遮挡以及相似物干扰情况下目标的跟踪问题。虽然研究工作可以解决因摄像头视角不佳所导致的短时间目标遮挡问题，但针对已经完全消失目标的追踪问题，这些算法几乎无能为力。

密歇根大学Olson E.教授及其团队在2011年提出了一种标记识别算法[13]，将AprilTag（一个类似于二维码的视觉基准库标签）粘贴于目标物体上，基于视觉系统通过标记识别算法来实现目标的跟踪；2016年对AprilTag标准库及其识别算法进行了改进[14]。Tang D.等专家研究了基于AprilTag的多传感器外部校准的方法[15]；何浩楠等探讨了AprilTag在小车定位追踪中的应用[16]，能够实现自动泊车和行驶追踪的功能；焦传佳等专家将AprilTag应用于移动机器人的定位系统[17]，使实时定位误差控制在1 cm～2 cm。这些成功应用示例说明基于视觉的AprilTag检测算法可望在更广泛的领域得到应用。

在穴位传感器测量患者的经络穴位时，穴位传感器与患者之间就形成了一种固定的相对位置关系，这种固定关系为传感器位置的记忆追踪提供了基础。由于每次传感器测量时，并没有一个固定的测量位置，必须在测量过程中才能形成传感器与被测对象之间相对固定的关系，因此，精确测定它们之间的位置关系是记忆追踪的关键。论文采用2个不同的AprilTag分别粘贴于穴位传感器和患者身体上，在测量过程中，穴位传感器与患者之间形成了固定的位置关系，这时通过视觉系统检测出这2个AprilTag，并计算出它们之间的位置关系；在穴位传感器移走后就可以利用患者身体上的AprilTag追踪到穴位传感器的测量位置，实现穴位位置的精确指示。

1 AprilTag标签检测算法

AprilTag是一种特定的二维码[13-14]，根据其组成维度和汉明距离可以分成3类：Tag36h11、Tag25h9和Tag16h5，分别表示由6×6、5×5和4×4像素构成的二维码标签。Tag36h11标签示例如图1所示。

图1 Tag36h11标签示例Fig.1 Schematic diagram of Tag36h11 label

采用AprilTag标签检测算法对摄像头获取的图片进行处理，可以检测出AprilTag标签。

1）计算每个有效像素梯度的强度和方向，通过聚类方法检测出线段；

2）按逆时针方向检测线段，在给定的阈值范围内，根据一条边的末端点检测闭环图形的下一条边，当再次检测到第一条边时，就构成了封闭的图形，包含该图形的四边形就是候选的二维码标签图像；

3）将四边形的4个顶点（x(k),y(k),k=1,2,3,4）映射到基准四边形的4个顶点（x0(k),y0(k),k=1,2,3,4 ，即 (−1,1),(1,1),(1,−1),(−1,−1)），可以得到单应性矩阵H：

4）四边形图像通过单应性矩阵H映射到图像平面，得到一组编码字串；

5）如果在Tag36h11库中搜索到该编码字串，就表示该四边形包含一个有效的AprilTag标签。

在检测Tag36h11标签的同时，还可以估计出标签与摄像头之间的距离。

2 传感器位置记忆追踪算法

通过在穴位传感器和患者身体上粘贴AprilTag，记作为TagA和TagB，在测量经络穴位时，利用摄像头获取同时包括TagA和TagB的图片，确定这2个Tag之间的关系；当传感器移走后，可以通过TagB的位置追踪出传感器的测量位置。

2.1 AprilTag之间的位置关系

通过AprilTag标签检测算法对摄像头获取的图片进行处理，可以获取2个AprilTag中心点的3D位置信息（ TagA:(xA,yA,dA),TagB:(xB,yB,dB)），如图2所示，xoy为视觉坐标系。

图2 AprilTag之间的关系Fig.2 Relationship between two AprilTag

设TagB的顶点A和B的平面位置分别为TagA的顶点E和F的平面位置分别为则TagA和TagB的倾角γ、α为

用TagB的顶点A与TagA的中心点来表示这2个AprilTag之间的位置关系：

式中： β、θ 参见图2所示；R表示TagA中心点与TagB的顶点A之间的距离。

2.2 传感器位置记忆追踪原理

图3给出了由TagB（k）追踪TagA（k）的示意图，灰色的TagA和TagA（k）分别表示原穴位传感器所在的位置，由于在穴位传感器移走后TagB（k）位置发生了些许变化，使要求追踪的传感器的位置也发生了相应的变化。当穴位传感器移走后，通过摄像头图片只能检测出TagB的位置TagB(k):(xB(k),yB(k),dB(k))，其顶点A和B的平面位置分别为通过这2个点可以计算出当前视频帧中TagB（k）的倾角 α(k)：

图3 由TagB（k）追踪TagA（k）的示意图Fig.3 Schematic diagram of TagA(k) tracked by TagB(k)

由于TagA与TagB的位置保持不变，则 β(k)为

由于当前视频帧中TagB（k）与摄像头的距离dB(k)≠dB，也会导致2个AprilTag标签之间的距离发生变化：

由此可以计算出标签TagA（k）中心点的位置为

标签TagA（k）的倾角 γ (k)计算公式为

这样，通过当前视频帧中TagB（k）的位置信息，就可以追踪出TagA（k）所处的位置，实现了穴位传感器（TagA）位置的追踪。

2.3 传感器位置记忆追踪算法

在实际操作过程中，在开始穴位测量操作时，主控系统会给穴位传感器位置追踪系统发出测量命令；在测量结束后，穴位传感器移出视觉场景，这时主控系统发出跟踪命令。

在穴位传感器位置追踪系统中，当接收到主控系统的测量命令时，系统通过摄像头获取图像，并采用AprilTag标签检测算法检测出TagA和TagB，标签类型为Tag36h11，计算TagA和TagB之间的位置关系；当接收到主控系统的追踪命令时，系统通过摄像头获取图像，只能检测到TagB标签，通过穴位传感器位置追踪算法计算出TagA（穴位传感器）位置和倾角。传感器位置记忆追踪算法的流程框图如图4所示。

图4 传感器位置记忆追踪算法流程图Fig.4 Flow chart of memory tracking method of sensor location

在测量命令的作用下，采用AprilTag标签检测算法检测出图像中的2个AprilTag标签，并根据编码值识别出TagA和TagB；当检测到的2个标签具有稳定的位置关系时，表示测量过程正常完成，可以作为后续追踪命令的依据；在接收到主控系统的追踪命令时，系统通过AprilTag标签检测算法只能检测出图像中的一个AprilTag标签（TagB），根据测量过程中得到的2个标签之间的位置关系，可以追踪到TagA的位置和倾角，实现穴位传感器（TagA）位置记忆追踪的目的。

3 算法实验结果

为了验证论文的传感器位置记忆追踪算法的有效性，在与摄像头平行的邻近位置上放置高清晰度投影机，主控系统发出测量命令后的3 s 内，投影出具有固定位置关系的TagA和TagB；然后主控系统发出追踪命令，之后投影机只投影出TagB，并缓慢改变TagB的位置，本系统通过穴位传感器位置记忆追踪算法由TagB追踪到TagA的位置。

在跟踪阶段，主控系统控制投影机投射出仅改变位置的TagB（角度不变），探索TagB位置变化对传感器位置记忆追踪的影响。2个Tag之间的位置关系和TagA位置记忆追踪误差如图5所示，摄像头距离投影平面2 m，TagB按照圆形轨迹移动。从图5中可以看出，在传感器（TagA）移除后，被测对象的平移动作会影响TagA位置的追踪精度，追踪位置的均方误差为（0.360 mm，0.364 mm），可以满足实际要求。

图5 Tag位置关系与TagA位置记忆追踪误差曲线Fig.5 Tag position relationships and error curves of TagA memory tracking of location

在跟踪阶段，主控系统控制投影机投射出仅改变角度的TagB（位置不变），探索TagB角度变化对传感器位置记忆追踪的影响。2个Tag之间的位置关系和TagA位置追踪误差如图6所示，TagB绕中心点旋转，位置固定不变。从图6中可以看出，在传感器（TagA）移除后，被测对象的转动动作也会影响TagA位置的追踪精度，追踪位置的均方误差为（0.745 mm，0.737 mm），追踪精度在1 mm以内。

图6 Tag位置关系与TagA位置记忆追踪误差曲线Fig.6 Tag position relationships and error curves of TagA memory tracking of location

在跟踪阶段，主控系统控制投影机投射出位置和角度缓慢变化的TagB，探究TagA追踪位置的误差，TagB的运动轨迹和TagA位置记忆追踪误差曲线如图7所示，从图7中可以看出，论文算法可以得到较小的TagA位置追踪误差，其均方误差为（0.959 mm，0.674 mm），能够满足实际应用要求。

图7 TagB运动轨迹与TagA位置记忆追踪误差曲线Fig.7 TagB motion trails and error curves of TagA memory tracking of location

4 结论

在中医诊断治疗中，通过穴位传感器可以同时获得128个穴位的测量结果，通常情况下只能将测量结果显示在屏幕上，医护人员很难准确找到实际的异常表现的穴位位置。论文采用2个不同的AprilTag分别粘贴于穴位传感器和患者身体上，在测量过程中，穴位传感器与患者之间形成了固定的位置关系，这时通过视觉系统检测出这2个AprilTag，计算出它们之间的位置关系；在穴位传感器移走后，即使患者做出轻微的移动或转动，论文算法仍可以利用患者身体上的AprilTag追踪到穴位传感器的测量位置，实现穴位位置的精确指示。如果将本文的传感器位置记忆追踪算法与投影设备配合，还可以直接将测量结果投射到患者身体上，清晰显示出具有异常表现的穴位位置，为治疗与干预提供了良好的参考。本文方法已经为多家医院临床试验所采用，受到用户的一致好评。