CN104951753B - 一种有标识物6自由度视觉跟踪系统及其实现方法 - Google Patents

Abstract

本发明公开了一种有标识物6自由度视觉跟踪系统及其实现方法，其中，所述有标识物6自由度视觉跟踪系统包含智能视觉标识、视觉标识控制器，视觉跟踪处理器；所述视觉标识控制器分别连接智能视觉标识和视觉跟踪处理器。本发明可以大大降低图像处理计算量，从而提高三维跟踪的精度，以及三维跟踪系统的响应速度。而所使用的器件和技术，都是成熟可靠，易于获得的。从而可以将原有造价高，体积庞大的三维视觉跟踪系统，改进为移动的、嵌入式的系统。

Description

一种有标识物6自由度视觉跟踪系统及其实现方法

技术领域

本发明涉及有标识物三维视觉跟踪领域，特别涉及一种具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，以及标识物辨识和预测算法。

背景技术

三维视觉跟踪系统随着计算机和电子技术的发展，被用在越来越多的领域。广为人知的是三维物体测量，物体运动捕捉，机器人导航和体感操作。三维视觉跟踪系统利用视觉图像传感器，仿照生物的视觉原理，或者通过不同位姿摄像机获取空间中一个对象的多个图像，或者通过同一摄像机获取空间中一个对象在不同投影平面上的多图像，利用几何光学得到该对象的具体空间位置和姿态（后面统一简称为位姿）。要达到亚毫米和小于一角分的高精度位姿，通常采用具有标识物的三维视觉系统，称为三维视觉测量或跟踪系统。该技术的关键，一是获取特征点在图像中的精确图像像素坐标位置；二是将标识物的物理特征点与图像特征点一一匹配，注意由于遮挡，大多情况无法将所有物理特征点匹配到图像特征点，图像特征点通常少于物理特征点数量。

现有具有标识物的三维视觉采集系统，有下面标识物类型：一种是使用分时发光的标识物，也就是保证每帧图像中只有可辨识的特征点被采集（很多时候仅一个发光点），最大问题是特征点越多则系统响应速度越慢；第二种是使用立体形状互不相同的标识物，最大问题是标识物设计难度高，且在标识物邻近或遮挡时，易发生匹配失败；第三种则是每个标识物使用互不相同的平面图案，因为要辨识图案，造成标识物体积较大，且大大提高了对视频图像解析的难度。

我们研究了现有的各种算法，发现其对图像帧的帧间信息利用很少，甚至根本没有使用，大多采用将视频分割为孤立的静态帧进行处理。而如果将标识物视为刚体目标，物体在帧间的位姿变化是受到运动学方程约束的，使用这些约束，将视频帧联系起来，可以提供额外的辨识信息。同时，使用运动学方程可较精确地估算下一图像帧中的特征点位置，可以大大降低图像处理运算量。因此相邻帧具有大量的冗余信息，可以被复用，而不是每次求解，对于低速跟踪对象，尤其显著。

有鉴于此，现有技术有待改进和提高。

发明内容

鉴于上述现有技术的不足，本发明的目的在于提供一种有标识物6自由度视觉跟踪系统及其实现方法。以解决现有技术中的标识物或者体积较大，或者需要视觉采集系统的高强度运算，因而造成三维视觉跟踪和测量系统成本高昂，使用不便的问题。

为了达到上述目的，本发明采取了以下技术方案：

一种具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其中：所述有标识物6自由度视觉跟踪系统包含智能视觉标识、视觉标识控制器，视觉跟踪处理器；其中，所述视觉标识控制器分别连接智能视觉标识和视觉跟踪处理器。

所述的具有惯性MEMS传感器和无线通讯网络的智能标识物系统，其中，所述智能视觉标识包含第一无线通讯网络模块、第一微控制器、MEMS惯性传感器、电池和一组指示灯；其中，所述MEMS惯性传感器至少具有六轴，即位移加速度和旋转角速度；

所述智能视觉标识的外形为一个非对称的多面体，其任意两棱边边长不能相等。每一智能视觉标识具有一个唯一的标识ID，存储在其第一微控制器中。

所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其中，所述视觉标识控制器包含与第一无线通讯网络模块无线连接的第二无线通讯网络模块、第二微控制器和用于与视觉跟踪处理器相连的接口；所述第二微处理器分别连接第二无线通讯网络模块和接口。

所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其中，所述第二微控制器可以和视觉跟踪处理器共用同一个物理计算机处理单元。

所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其中，所述视觉跟踪处理器包含图像采集接口和图像处理器。

一种所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统的实现方法，其中，所述方法包括以下步骤：

S100、预先将智能视觉标识设置在各待测或待跟踪物上；

S200、按照固定的采集周期，视觉标识控制器通过第二无线通讯网络模块采集各标识物的运动学参数，包括标识物的旋转角速度和位移加速度；并将运动学参数发送到视觉跟踪处理器；

S300、视觉跟踪处理器按照标识物辨识和预测算法完成标识物位姿解算。

相较于现有技术，本发明提供的一种有标识物6自由度视觉跟踪系统及其实现方法。具有以下优点：

可以大大降低图像处理计算量，从而提高三维跟踪的精度，以及三维跟踪系统的响应速度。而所使用的器件和技术，都是成熟可靠，易于获得的。从而可以将原有造价高，体积庞大的三维视觉跟踪系统，改进为移动的、嵌入式的系统。

附图说明

图1为一种具有惯性MEMS传感器和无线通讯网络的智能标识系统的系统结构框图。

图2为本发明实例使用的坐标系及其相互关系。

图3为本发明的一种具有惯性MEMS传感器和无线通讯网络的智能标识系统的实现方法的流程图。

图4为本发明实例使用的标识物辨识流程图。

具体实施方式

本发明提供一种有标识物6自由度视觉跟踪系统及其实现方法，为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

请参阅图1，其为一种具有惯性MEMS传感器和无线通讯网络的智能标识系统的系统结构框图。如图所示，所述有标识物6自由度视觉跟踪系统包含智能视觉标识100、视觉标识控制器200，视觉跟踪处理器300；其中，所述视觉标识控制器200分别连接智能视觉标识100和视觉跟踪处理器300。

具体来说，所述智能视觉标识100包含第一无线通讯网络模块110、第一微控制器120、MEMS惯性传感器130、电池140和一组指示灯（图中未示出）；其中，所述MEMS惯性传感器至少具有六轴，即位移加速度和旋转角速度。所述智能视觉标识的外形为一个非对称的多面体，其任意两棱边边长不能相等。每一智能视觉标识具有一个唯一的标识ID，存储在其微控制器中。所述视觉标识控制器200包含与第一无线通讯网络模块110无线连接的第二无线通讯网络模块210、第二微控制器220和用于与视觉跟踪处理器相连的接口230；所述第二微处理器220分别连接第二无线通讯网络模块210和接口230。所述视觉跟踪处理器300包含图像采集接口310和图像处理器320。

视觉跟踪处理器按照标识物辨识和预测算法完成标识物位姿解算，步骤如下：

为了描述方便，参考附图2，我们将第一个图像视角（或摄像机）设定为世界坐标系W，其原点为摄像机光心，光轴由光心指向被摄对象为Z轴，图像平面的x方向为X轴，y方向为Y轴。第二视角（或摄像机）坐标系相对世界坐标系的平移为，而旋转为，在世界坐标系框架中。惯性传感器坐标系相对世界坐标系的平移为，而旋转为，位移速度为，在世界坐标系框架中。惯性传感器测量得到的位移加速度记为，旋转角速度记为，二者都具有测量偏移误差，位移加速度偏移记为，旋转角速度偏移记为；同时也具有高斯白噪声类型的检测噪声，位移加速度噪声记为，旋转角速度噪声记为。

从而我们可以的到系统的状态描述向量：

={}

并且该状态受下面的运动学方程约束：

=

=C()()–g

=Ω(--)

其中，C()是根据旋转角度得到的旋转矩阵，而Ω则是根据旋转角速度得到的旋转矩阵。和这两个偏移误差分布方式可以查阅对应的MEMS芯片手册得到。g为重力加速度

而由于第二视角与第一视角的相对位姿是固定的，且可以通过标定得到，所以基本可认为其为常向量。即有：

=0，以及=0

根据惯性传感器获取到的以及，使用运动学方程，我们可以基于当前的状态，以及 ，估算下一时刻的状态，而无需从图像中获取。

要启动预估迭代，至少需要0时刻的状态，以及 。一个已获取了该状态初值的标识物，我们称之为已辨识标识物；否则称为未辨识。

简单来说，视觉跟踪处理器使用三个不同周期的主要处理循环，通常被放置在系统的三个独立线程中。分别是：运动学迭代线程，已辨识标识物图像解析线程，未辨识标识物位姿解析线程。运动学迭代线程完成系统状态预测，采用固定周期运行方式，与惯性传感器采集周期相同，通常为1KHZ。通过无线通讯，采集各标识物的位移加速度和旋转角速度，并使用该参数，基于已辨识标识物上一时刻位姿，利用传感器融合滤波器估算标识物当前位姿。对于未辨识标识物的位移加速度和旋转角速度采集值，则只是将该惯性参数，标识物ID，以及采集时间戳一起存放到未辨识标识物惯性参数数组。通过无线通讯，这个线程同时告知ID号最小的未辨识标识物点亮其指示灯。已辨识标识物图像解析线程，根据图像采集速率，运行在15-50HZ。在图像采集时刻，视觉跟踪处理器获取至少两个视角的标识物图像。并使用上述运动学迭代线程估算的标识物位姿估算值，依据投影方程计算得到各视角中标识物各特征点的像平面坐标。然后以估算像素（像平面）坐标为中心处理一个5X5窗口的角点（CORNER）信息，如果找到该角点，则寻找特征点成功；否则计算按顺时针方向依次计算该5X5窗口邻接的8个5X5窗口，以找到对应角点。只要使用上述方法能找到标识物的一个或一个以上特征点，根据已知的标识物结构特征，可直接计算得到标识物的当前位姿。使用新得到的位姿更新传感器融合滤波器状态向量，已辨识标识物图像解析线程执行完毕。对于一个特征点也无法找到的标识物，将其移动到未辨识标识物解析线程。未辨识标识物位姿解析线程运行速率，依照视觉跟踪处理器的处理能力而定，是非实时的线程。也就是没有准确的执行周期，只在上面两个线程都空闲时执行。如果未辨识标识物的数量不为零，这个线程才被执行，否则直接返回休眠状态。

未辨识标识物解析线程线程的过程如下：首先，使用传感器融合算法对ID号最小的未辨识标识物惯性参数缓存区的惯性参数进行滤波，仅使用位移加速度和旋转角速度估算该标识物的姿态角。第二步，处理视频图像，先将已辨识标识物图像解析线程检测到的特征点从视频图像中抹除。然后在整个视频图像中搜索标识物指示灯，并通过包围该指示灯图像的棱边图像，找到两幅视频图像中该标识物特征点的像素坐标。当获取到三个或三个以上的标识物特征点后，通过这一组特征点，我们可以从图像中解析出该标识物的位姿，。第三，比对标识物对应的图像解析姿态角与惯性估算姿态角，如果二者误差小于正负5度，我们就认为该标识物为适配。第四，使用当前的视觉图像解析姿态角，更新适配标识物的传感器融合滤波器状态。并连续执行上述比对和更新1秒钟，验证是否在整个比对时间内姿态角pitch和roll误差都小于正负1度。如果符合，那么辨识成功。将该标识物移动到已辨识标识物处理线程。否则提示用户该标识物辨识失败。

因为惯性传感器在采集瞬态数据时具有较高的分辨率，角速度分辨率为0.01度每秒；位移加速度分辨率为0.1mg，且具有高速简单的惯性参数获取能力，但是易受温度变化等影响。我们的模型将惯性传感器的偏移误差作为系统状态变量，使用稳定性更高的图像解析位姿数据及时调整偏移误差，相当于每个图像采集时刻对传感器进行了一次校准，大大提高了惯性传感器的长时稳定性。

而使用惯性传感器采集的惯性参数，对标识物位姿进行估算，一方面提高了标识物位姿的响应速度，另一方面使用估算投影位置查找特征点也会大大减少所处理的图像数。如上述示例，视频图像分辨率为1920X1080，如果使用HARRIS角点算子提取CORNER信息，假设使用5X5矩形窗口，方向导数使用SOBEL算子，其计算量为

乘法：214X(1920X1080) =443M；

加法：191X(1920X1080) =396M；

而如果我们使用预估值，即使第一次5X5中心矩形没有命中，需要一次额外的迭代，我们要面对的也只是一个15X15像素的图像。使用上述方法从中提取CORNER，其运算量只近似原始计算量的万分之一。目前的商用双目视觉容许同时共存的标识物通常小于15个，我们即使存在30-50个标识物，其运算量也大大小于直接计算。因而在同样的计算平台上，可以考虑提高图像采集的分辨率，比如5百万像素的摄像机，以进一步提高三维视觉跟踪系统的精度。或者使用价格友好，灵活性更高的嵌入式移动计算平台，如手机或者平板电脑进行双目视觉解析计算。

特别地，标识物本身无需通过其形状或图案以完成匹配，也使得标识物体积可以更加小巧，且当两个标识物邻近甚至互相遮挡一小部分，通过估算，我们也可以获取该标识物的位姿。这样，在同一个三维视觉跟踪系统中，可以容纳的标识物数量也可以大大增加，比对目前通常小于15个标识物的系统，智能标识显然具有更广阔的应用场景。且标识物便于进行大量制作，其精度和成本都更有优势。

另外由于拥有了两套互为冗余的传感器，视觉跟踪的可靠性也大大提高了。从上面可以看到，无论是图像采集传感器失配，或者惯性传感器失效，用户都会立即得到提示。比对目前的双目视觉跟踪系统，无疑大大提高了关键性应用的实用性，如医疗。

本发明还提供了一种具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统的实现方法，如图3所示，所述方法包括以下步骤：

S100、预先将智能视觉标识设置在各待测或待跟踪物上；

S200、按照固定的采集周期，视觉标识控制器通过第二无线通讯网络模块采集各标识物的运动学参数，包括标识物的旋转角速度和位移加速度；并将运动学参数发送到视觉跟踪处理器；

S300、视觉跟踪处理器按照标识物辨识和预测算法完成标识物位姿解算。

下面通过一个具体的实施例来说明上述具有惯性MEMS传感器和无线通讯网络的智能标识系统（结合图4）：

在本实施例中，使用两个具有明确结构特征的摄像机，摄像机图像分辨率为两百万（1920X1080），帧率为每秒钟30帧，也就是说，图像解析线程的运行频率为30HZ，大约33毫秒解算一组（两幅）视频图像。开发平台为ZYNQ7030视频开发套件，其配备有两个500万像素分辨率的CMOS摄像头，一个以太网接口，通过高速SPI接口连接一个CC2630微控制器，该CC2630微控制器负责通过2.4G无线采集各标识物的惯性参数。每隔5毫秒，以太网接口将智能标识物的当前位姿，实时地发送到用户设备，在我们的实验环境中是一个机器人仿真平台。

智能视觉标识则使用MPU6050六轴惯性传感器测量其惯性参数，其包含三个位移加速度和三个旋转角速度传感器。微控制器使用美国TI公司的CC2630，其具备一个超低功耗的CORTEX-M3内核，以及一个独立的802.15.4 MAC和RF前端。为了高效和简单，我们采取轮询的方式获取智能视觉标识的惯性参数，每个标识物具备一个8位标识号，所有标识以及视觉标识控制器都工作于同一个无线信道，标识物只在被询问时发送应答信息，其余时候都处于接收状态。

运动学迭代线程以无线通讯容许的最大速度轮询每个标识物，并告知ID号最小的未辨识标识物点亮其指示灯。每获取到一组标识物惯性参数，如果是已辨识标识物，使用EKF（扩展卡尔曼滤波器）迭代标识物位姿估算值；如果是未辨识标识物，则将这些惯性参数，标识物标识号，以及时间戳一起存放到未辨识标识物惯性参数数组。为保证精度，从标识物获取到的位移加速度和旋转角速度采样值都使用原始的16位整型值，在视觉跟踪处理器中转换为双精度浮点数并保存。

智能视觉标识的外部形状，我们采用最简单的四面体结构，其具有四个顶点，共六条棱边。也就是说，每个智能视觉标识具有四个特征点（即其顶点），并且这四个特征点通过六条棱边连接起来。我们通过棱边的联系，简单直接地就可以判断哪些特征点隶属于同一个标识物。为了保证在智能视觉标识处于任意位姿时，我们都能通过特征点无歧义的判别该标识物的位姿，我们设计该四面体六条棱边的长度都有显著的差别。并且标识物安装有一组指示灯，其点亮时四面体的每一面中心都会有一个易于识别的光点。

每33毫秒，我们将采集到两幅视频图像。首先启动已辨识标识物图像解析线程。使用与图像采集时刻匹配的EKF（扩展卡尔曼滤波器）迭代位姿估算值，使用投影方程计算二摄像机图像平面各特征点的图像坐标，并利用标识物结构信息剔除被标识物自身遮挡的特征点。在视频图像中，以估算像素坐标为中心处理一个5X5窗口的角点（CORNER）信息，如果找到该角点，则寻找特征点成功；否则按顺时针方向依次计算该5X5窗口邻接的8个5X5窗口，继续寻找特征点对应的角点。如果仍然无法找到该特征点，则标记为查找失败。因为惯性参数瞬时精度较高，MPU6050传感器其旋转角速度测量总均方根噪声小于0.1度每秒，位移加速度测量总均方根噪声小于0.008g，而使用扩展卡尔曼滤波器可以进一步提高估算值的精度，所以位姿估算值的精度很高，绝大多时候一次5X5窗口计算就可以找到匹配的特征点。

当有两个特征点预估图像坐标过于靠近时，上述方法可能会一次发现两个或多个角点，需要额外使用棱边图像寻找与该特征点连接的其它特征点，从而判别各角点所归属的标识物。所有特征点搜索完成后，使用OPENCV中的立体视觉函数，每一组二维图像特征点，可以解算得到一个三自由度坐标值，即该特征点的空间位置。只要使用上述方法能找到标识物的一个或一个以上特征点，根据已知的标识物结构特征以及位姿估算值，可直接计算得到标识物的位姿。使用新得到的位姿更新扩展卡尔曼滤波器状态向量，已辨识标识物图像解析线程执行完毕。

当运动学迭代线程和已辨识标识物图像解析线程都处于空闲状态时，未辨识标识物匹配线程首先检查是否有标识物等待匹配，可能是用户新放入的标识物或者短时离开视觉采集视域的标识物。

首先，使用Madgwick传感器融合算法对未辨识标识物惯性参数缓存区的惯性参数进行滤波，注意这里仅使用惯性参数，即位移加速度和旋转角速度估算标识物的姿态角。因为MPU6050没有电子罗盘，yaw角度估算误差较大，而pitch和roll则可以很准确。使用MPU6050传感器，我们仅使用位移加速度和旋转加速度，并使用一个开源的Madgwick融合滤波器，估算得到的pitch和roll角，精度可达正负1度以内。

然后处理视频图像，先将已辨识标识物图像解析线程检测到的标识物从视频图像中抹除，这通过一个局部图像平滑器快速完成。然后从整个图像中搜索当前待辨识标识物的指示灯图像，并通过包围指示灯图像的棱边图像，找到两幅视频图像中的标识物特征点。要辨识一个标识物，我们需要三个或三个以上的特征点。如果成功获取到该标识物的三个或三个以上特征点，通过该组特征点，可从图像中解析出待辨识标识物的位姿，。

进行标识物图像解析姿态角与惯性估算姿态角的比对。比对两组姿态角的pitch和roll值，如果二者误差小于正负5度，我们就可以认为该标识物为适配。注意，由于惯性参数的获取时刻与视频图像采集时刻存在一个时间差，这个时间差为0-33毫秒，并且对于每个标识物，这个延时可能具有小的差别。我们使用图像解析姿态角与0-33毫秒时刻的各惯性估算姿态角比对，找到误差最小的时刻，记为视频图像相对于惯性参数时间戳的延时。

未辨识标识物匹配线程使用当前的视觉图像解析姿态角，更新适配标识物的Madgwick滤波器状态变量。并连续执行上述比对和更新过程，持续1秒钟，验证是否在整个比对时间内姿态角pitch和roll误差都小于正负1度。同时，记录每次比对视频图像相对于惯性参数时间戳的延时。如果符合，那么辨识成功。将上述平均延时记录为标识物图像采集延时，并将该标识物移动到已辨识标识物处理线程，并通过运动学迭代线程告知该标识物熄灭它的指示灯。不能符合，则继续检测1秒钟。

如果一个待辨识标识物，超过30秒时间仍然无法被成功辨识，我们提示用户该标识物辨识失败，这通常是一个失效的智能标识物，或者整个图像采集系统发生了故障，比如摄像机相对位置变化了。

综上所述，比对现有的三维视觉跟踪系统，本发明提供的有标识物6自由度视觉跟踪系统及其实现方法，可以大大降低图像处理计算量，从而提高三维跟踪的精度，以及三维跟踪系统的响应速度。上面范例已经使用了较高的图像分辨率，目前商用三维跟踪系统的分辨率大多为1024X768。而所使用的器件和技术，都是成熟可靠，易于获得的。从而可以将原有造价高，体积庞大的三维视觉跟踪系统，改进为移动的，嵌入式的系统。

可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及其发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。

Claims (5)

1.一种具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其特征在于：所述有标识物6自由度视觉跟踪系统包含智能视觉标识、视觉标识控制器，视觉跟踪处理器；其中，所述视觉标识控制器分别连接智能视觉标识和视觉跟踪处理器；

所述视觉跟踪处理器用于按照标识物辨识和预测算法完成智能标识物位姿解算；

所述智能视觉标识包含第一无线通讯网络模块、第一微控制器、MEMS惯性传感器、电池和一组指示灯；其中，所述MEMS惯性传感器至少具有六轴，即位移加速度和旋转角速度；

所述智能视觉标识的外形为一个非对称的多面体，其任意两棱边边长不能相等；

每一智能视觉标识具有一个唯一的标识ID，存储在第一微控制器中；

所述视觉跟踪处理器使用三个不同周期的主要处理循环，分别是：运动学迭代，已辨识标识物图像解析，未辨识标识物位姿解析。

2.根据权利要求1所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其特征在于，所述视觉标识控制器包含与第一无线通讯网络模块无线连接的第二无线通讯网络模块、第二微控制器和用于与视觉跟踪处理器相连的接口；第二微处理器分别连接第二无线通讯网络模块和接口。

3.根据权利要求2所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其特征在于，所述第二微控制器和视觉跟踪处理器可以共用同一个物理计算机处理单元。

4.根据权利要求1所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统，其特征在于，所述视觉跟踪处理器包含图像采集接口和图像处理器。

5.一种权利要求1所述的具有惯性MEMS传感器和无线通讯网络的有标识物6自由度视觉跟踪系统的实现方法，其特征在于，所述方法包括以下步骤：

S100、预先将智能视觉标识设置在各待测或待跟踪物上；

S200、按照固定的采集周期，视觉标识控制器通过第二无线通讯网络模块采集各标识物的运动学参数，包括标识物的旋转角速度和位移加速度；并将运动学参数发送到视觉跟踪处理器；

S300、视觉跟踪处理器按照标识物辨识和预测算法完成标识物位姿解算。