CN101694716A - 一种针对多点目标的立体视觉光学跟踪系统 - Google Patents

Abstract

本发明公开了一种针对多点目标的立体视觉光学跟踪系统，属于机器视觉和定位跟踪技术领域。本系统采用基于硬件实现的方式，通过利用双目摄像机采集目标和背景的原始图像，利用并行流水线对目标进行快速识别和标记，采用双核数字信号处理器进行并行计算，完成对目标的空间定位和跟踪。将并行流水线处理和双核串行计算相结合，根据不同的算法特点，合理地分配硬件资源。本系统摆脱了PC机或工作站的束缚，打破了PC机或工作站的软件计算资源瓶颈。本发明能够在不同的光照环境和目标发光强度条件下，对空间中的多个点目标进行实时定位和跟踪，可实时、不丢帧地跟踪空间中超过100个点目标，跟踪精度能够达到0.5mm RMS。

Description

一种针对多点目标的立体视觉光学跟踪系统

技术领域

本发明涉及一种光学跟踪系统，尤其涉及一种基于硬件的立体视觉光学跟踪系统，属于机器视觉和定位跟踪技术领域。

背景技术

多点目标的识别和跟踪在诸多应用领域都起着重要的作用，如图像引导外科手术、运动捕捉、增强现实、娱乐等。利用光学跟踪设备对附着在目标刚体表面的标记点进行识别和跟踪，并通过获取多个标记点的空间坐标及各个标记点之间的距离，能够确定刚体目标的位置和朝向。

近年来，研究人员对光学跟踪系统的应用做了大量研究。在“The role ofnavigation in knee surgery and evaluation of three-dimensional knee kinematics”Operative Techniques in Orthopaedics.2005，15，1，pp.64-69.“Development ofactive ir-based surgical marker tracking and positioning systems”(IEEE InternationalConference on Systems，Man and Cybernetics.2005，3，pp.2443-2448.)中，以及“Surgical navigation for total knee arthroplasty：perspective”(Journal ofBiomechanics.2007，40，4，pp.728-735.)中，介绍了基于光学跟踪的外科手术导航系统。该系统通过光学跟踪设备，对固定在手术器械和病人体表的标记点进行定位和跟踪，对实际手术中病灶、手术器械和前期的MT/CRI图像进行配准，由此实现外科手术导航，有助于减少患者的失血量、手术创伤及并发症。在“Skeletal parameter estimation from optical motion capture data”(IEEEComputer Society Conference on Computer Vision and Pattern Recognition.2005，2，pp.1185.)和“High marker density motion capture by retroreflective mesh suit”(IEEE International Conference on Robotics and Automation.2005，2884-2889.)中，介绍了光学跟踪系统在运动捕捉、运动分析、角色动画制作等方面的应用，通过将多个标记点附着在人体四肢和头部，采用多个CCD摄像机跟踪空间中的这些特定点目标，从而解算出人体的运动姿态。这类运功捕捉方面的应用还可以拓展到虚拟现实和娱乐等领域。

在以上这些文献中所提及的光学跟踪系统，其原理都是通过高速的视频接口将多个工业用摄像机连接至PC或工作站，并将视频流送入PC或工作站，采用基于CPU的软件方法来实现光学跟踪。由于软件处理方法的固有的串行特点，应用系统的实时性会受到影响，系统延迟较大，数据刷新率低。

发明内容

本发明的目的是为了克服现有技术的缺陷，为解决对点目标进行实时跟踪与识别并精确定出目标位置方向的问题，提出了一种针对多点目标的立体视觉光学跟踪系统。本系统采用基于硬件实现的方式，通过利用双目摄像机采集目标和背景的原始图像，利用并行流水线对目标进行快速识别和标记，采用双核数字信号处理器进行并行计算，完成对目标的空间定位和跟踪。将并行流水线处理和双核串行计算相结合，根据不同的算法特点，合理地分配硬件资源。本系统作为一套独立运行的光学设备，以硬件并行处理对所提出的算法进行加速和优化，从而摆脱了PC机或工作站的束缚，打破了PC机或工作站的软件计算资源瓶颈。本发明能够在不同的光照环境和目标发光强度条件下，对空间中的多个点目标进行实时定位和跟踪，可实时、不丢帧地跟踪空间中超过100个点目标，跟踪精度能够达到0.5mm RMS。

为实现上述目的，本发明所采用的技术方案如下：

一种针对多点目标的立体视觉光学跟踪系统，包括图像采集模块，目标标记与识别模块，目标定位跟踪模块，以及电源模块。

(1)图像采集模块：用于采集目标和背景的双目同步原始图像。

该模块由两片或两片以上的图像传感器组成。当图像传感器为串行输出时，还需要配置一片图像传感器数据解串芯片；若图像传感器为并行输出时，则不需要配置此芯片。

图像传感器作为前端的图像采集部分，采集从不同方位所采集到的图像的同步数据。优选的，每一片图像传感器都采用全帧同时曝光快门，以便适合捕捉快速运动物体的图像。

(2)目标标记与识别模块：基于并行流水线方式，用于实现双目或多目同步原始图像中目标的识别和标记，即，识别目标重心在同步原始图像中的二维位置，并给每一个点目标赋予一个唯一的标记。

所述目标标记与识别模块包括一片FPGA芯片以及一片FPGA配置芯片，用于对双目或多目同步原始图像数据进行处理。

(3)目标定位跟踪模块：用于实现目标的三维空间定位和对运动目标的跟踪。

所述目标定位跟踪模块由一片双核DSP、一片DSP指令存储芯片以及一片SDRAM组成。双核DSP接收FPGA芯片处理得到的目标识别和标记结果，并在每片图像传感器的帧回扫过程中进行多点目标定位。SDRAM对处理过程中的临时数据进行缓存，此外，通过将原始图像数据直接传入SDRAM，然后转存成图像文件，以此作为摄像机标定过程所需要的原始图像。DSP指令存储芯片用于存储双核DSP的指令。

DSP指令存储芯片与双核DSP之间通过SPI接口相连，SDRAM与双核DSP之间通过数据总线和地址总线相连。

(4)电源模块：用于为系统中各模块提供电力。

上述组成部分的连接关系如下：

当图像传感器为串行输出时，目标标记与识别模块中FPGA芯片的输入端同图像采集模块中的图像传感器数据解串芯片的输出端相连。当图像传感器为并行输出时，目标标记与识别模块中FPGA芯片的输入端分别同图像采集模块中每片图像传感器的输出端相连。

FPGA芯片的数据输出接口同目标定位跟踪模块中双核DSP的数据输入接口相连接。双核DSP的输出端同系统外部的上位机的数据输入端相连。

电源模块分别同上述各模块相连。

本系统的工作过程及信号转换关系如下：

首先，利用图像采集模块中的图像传感器采集目标和背景的原始双目或多目图像，并将图像数据送入目标标记与识别模块中的FPGA芯片中。

然后，在目标标记与识别模块中，采用并行流水线方式实现原始双目或多目图像中所有点目标的重心识别和连通域的最终标记。优选的，并行流水线具体步骤如下：

a.采集原始图像，并对图像进行平滑滤波，使点目标连通域的边缘毛刺得到削减。

b.进行自适应阈值计算：

先逐行扫描经步骤a平滑滤波后的图像中的每个像素，获取所有像素灰度值中最大值和最小值之间的中间值作为迭代计算的初始估计值T。

再逐行扫描经步骤a平滑滤波后的图像中的每个像素，在扫描过程中，计算像素灰度值大于T的所有像素的灰度平均值m₁，计算像素灰度值小于T的所有像素的灰度平均值m₂，然后由m₁和m₂的均值得到一个新的阈值T_new。

之后，通过以下机制对迭代终止与否进行决策：如果新阈值T_new和旧阈值T之差的绝对值大于或等于预设值ε，则新阈值将做为下一帧图像的输入阈值，并对下一帧图像进行相同的迭代过程；如果新阈值和旧阈值之差的绝对值小于预设值ε，则迭代结束。其中，ε的取值由迭代过程的计算精度决定。在进行决策的同时，保存此时的新阈值T_new，用以分割下一帧图像。

c.采用步骤b中得到的新阈值T_new，对图像进行分割，将目标从背景中提取出来。

d.设计一个m×n的滑动窗口，用以进行连通域标记(Connected componentlabeling)。该滑动窗口的最下面一行的中间位置是当前像素的灰度值，当前像素的前面位置和上面几行的位置均为已经被处理过的像素被赋予的初次标记结果，这些初步标记结果按从左到右、从上到下依次记为L₁至L_k，标记过程如下：当某个当前像素被扫描时，会出现两种情况。情况一，当前像素表示背景，则它被直接标记为零；情况二，当前像素表示有效目标像素，则依次扫描L₁至L_k，一旦探测到一个有效的初步标记结果，则将该结果赋给当前像素；如果L₁至L_k位置均没有有效的初步标记结果，则给当前像素赋予一个新的标记。

e.步骤d完成之后，每个像素位置都被赋予了一个临时标记。将同一连通域中的不同标记区域的像素位置的带灰度加权的坐标累加值进行再次累加，从而计算出目标的重心位置。同时，将同一连通域中产生标记冲突的区域设置为等价连通域，得到每个点目标连通域的最终标记。

f.根据图像中点目标连通域的标记结果，得到双目图像中目标的匹配关系。

当所有点目标的重心识别和标记完成后，将识别和标记结果发送至目标定位跟踪模块中的双核DSP内进行并行计算，实现目标的定位和跟踪，最终结果送至系统外部的上位机中供应用系统使用。优选的，所述并行计算方法包括如下步骤：

设定双核DSP的两个核分别为A核和B核。

①根据上述步骤e中得到的点目标重心在图像中的位置，对这些位置进行畸变校正。左右两幅图像中的目标位置校正由双核处理器的两个核分别进行，左右图像的校正过程通过双核并行完成。畸变校正完成后，即可得到点目标在像面上的满足针孔模型的归一化投影位置。

②根据步骤①得到的归一化投影位置，采用立体视觉三角定位算法对空间中的点目标进行定位，得到目标在世界坐标系中的三维坐标。

此时，A核进行立体视觉的三角定位和卡尔曼滤波所需要的状态预测，结束后进入等待状态；与此同时，B核计算单步预测误差协方差矩阵和卡尔曼增益，计算完毕后，B核给双核DSP芯片的共享存储空间内标志Flag1进行置位。

③将步骤②得到的三维坐标作为观测值，对目标建立运动模型，采用卡尔曼滤波对点目标的位置及运动参数进行跟踪。

当A核探测到标志Flag1后，对目标的状态进行更新；与此同时，B核进行卡尔曼滤波误差协方差矩阵计算，以备下一帧滤波过程。A核对目标的状态更新完毕以后，将双核共享存储空间内的标志Flag2置位，并将跟踪结果送出至上位机。B核探测到标志Flag2后，双核同时对各自管辖的相关标志和变量进行重置，开始下一帧数据的处理过程。

有益效果

本发明所提出的一种针对多点目标的立体视觉光学跟踪系统，以硬件并行处理对所提出的算法进行了加速和优化，能够实时不丢帧地跟踪空间中超过100个点目标，跟踪精度达到0.5mm RMS。本系统具有实时性强、跟踪目标多、精度高、跟踪范围广、小巧灵活等特点，对比现有技术，摆脱了PC机或工作站的束缚，打破了基于CPU的软件计算资源瓶颈，提高了系统的实时性和灵活性。在手术导航、普适数字娱乐、增强现实等领域中具有广阔的应用前景。

附图说明

图1为本发明具体实施方式中的结构组成框图。

图2为本发明具体实施方式中的用于连通域标记的滑动窗口。其中，P0表示当前像素，L1至L17表示被标记过的像素位置被赋予的初步标记结果。

图3为本发明所提出的并行校正方法示意图。

图4为本发明具体实施方式中的双核并行计算流程。

图5为本发明具体实施方式的流水线间的延迟结构。

图6为本发明具体实施方式在进行跟踪时，被跟踪目标数目和处理时间之间的关系曲线图。

图7为本发明具体实施方式在深度方向上的跟踪精度实验数据。

具体实施方式

下面结合附图对本发明的具体实施方式做进一步详细说明。

一种针对多点目标的立体视觉光学跟踪系统，其结构组成框图如图1所示，包括图像采集模块(1)，目标标记与识别模块(2)，目标定位跟踪模块(3)，以及电源模块(4)。

(1)图像采集模块：用于采集目标和背景的双目同步原始图像。

该模块由两片Micron公司的机器视觉专用CMOS图像传感器MT9V022以及一片DS92LV16型图像传感器数据解串芯片组成。MT9V022的图像采集频率为60帧/秒，两片图像传感器为串行输出。

两片MT9V022作为前端的图像采集部分，具有性噪比高、集成度高以及在低照度环境等不利条件下仍能稳定工作的特点，同时由于其采用了全帧同时曝光快门，因此适合捕捉快速运动物体的图像。MT9V022的有效像素数目为752×480，在全分辨率条件下其帧速可达60帧/秒(frame per second，fps)。利用一对这样的CMOS传感器可有效的采集双目图像的同步数据。

两片MT9V022所产生的图像信号、行同步和帧同步信号以及像素时钟信号均被串化为LVDS信号后串行输出。将其中一片MT9V022作为主传感器，另一片MT9V022作为从传感器。两片MT9V022相结合，由主传感器对两幅图像的数据进行整合打包，打包后的LVDS串行数据中包括每个传感器的图像数据(每个像素分辨率为8位)和像素时钟，其中像素时钟信号以首位高电平、末尾低电平的形式嵌在串行数据流中，而图像的行同步和帧同步信号则以保留字的形式嵌在数据流中，这样的LVDS串行数据流送入DS92LV16型图像传感器数据解串芯片中进行解串。

采用LVDS作为图像数据的传输方式是因为LVDS能够支持长距离传输。由于在LVDS系统中采用差分的方式传送数据，因此具有比单端传输方式更强的共模噪声抑制能力。同时，采用超五类网线中的双绞线来作为LVDS信号的传输媒介，由此保证差分信号的传输途径，提高数据传输的距离，同时数据传输还具有低电压、低辐射、低功耗、抗干扰性强等优点。在本实施方式中，系统时钟为27M，传输距离可达5米以上，这样的传输距离使得双摄像机之间的位置以及摄像机和主电路板间的位置可以随应用场合的需要而改变。

LVDS串行数据送入DS92LV16中进行解串处理后，得到两幅图像的各8位并行数据，同时恢复出像素时钟，而每幅图像的行同步和帧同步信号则以保留字的形式内嵌在数据流中。

(2)目标标记与识别模块

所述目标标记与识别模块包括一片FPGA芯片以及一片FPGA配置芯片组成，用于对双目同步原始图像数据进行处理。

其中，FPGA芯片采用XILINX公司的Spartan 3E系列FPGA芯片——XC3S1200E型，FPGA配置芯片采用XILINX公司的XCF04S型FPGA配置芯片。该型FPGA芯片采用先进的90纳米制造工艺技术，在保证足够的系统门的前提下，增加了每个IO口对应的逻辑单元数量，因此在很大程度上减少了每个逻辑单元的造价，降低了系统成本。

(3)目标定位跟踪模块

该模块由一片双核DSP、一片DSP指令存储芯片以及一片SDRAM组成。其中，双核DSP采用ADSP-BF561型，其两个对称的blackfin核分别控制两个PPI(Parallel Peripheral Interface)接口，以DMA的中断方式对FPGA芯片处理得到的目标识别和标记结果进行接收，并在主MT9V022、从MT9V022的帧回扫过程中进行多点目标定位。FPGA芯片顶层的输出端口的信号和时序只要满足PPI接口的时序，双核DSP即可接收来自FPGA芯片输出端的数据。MT9V022型图像传感器的摄像机图像采集频率为60帧/秒，因此中断的频率为60Hz。双核DSP能够在MT9V022的帧回扫过程中进行多点目标定位。

SDRAM采用是Micron公司的MT48LC8M32型SDRAM，其容量为256Mbits，用于对处理过程中的临时数据进行缓存，此外，通过将双目同步原始图像数据直接传入SDRAM，然后转存成图像文件，以此作为摄像机标定过程所需要的原始图像。

DSP指令存储芯片采用Atmel公司的AT45DB321D型DataFlash，其容量为32Mbits，用于对双核DSP进行加载，通过串行加载，可大大地减小电路板的面积，使得系统更加小巧灵活。

(4)电源模块

采用德州仪器(TI)公司的TPS75003型号电源。

上述组成部件间的连接关系如下：

MT9V022型从图像传感器的输出端同MT9V022型主图像传感器相连。

MT9V022型主图像传感器的输出端同DS92LV16型图像传感器数据解串芯片的输入端相连。

目标标记与识别模块中XC3S1200E型FPGA芯片的输入端同DS92LV16型图像传感器数据解串芯片的输出端相连。

XCF04S型FPGA配置芯片同XC3S1200E型FPGA芯片相连。

XC3S1200E型FPGA芯片的数据输出接口同目标定位跟踪模块中ADSP-BF561型双核DSP的数据输入接口相连接。

ADSP-BF561型双核DSP的输出端与系统外部的上位机的数据输入端之间采用RS-232接口相连，RS-232接口采用115200bps的传输速率，该速率足够保证处理数据能够不丢帧地进行传输，完全满足系统的实时刷新要求。

TPS75003型电源分别同上述各部件相连。

AT45DB321D型DSP指令存储芯片与ADSP-BF561型双核DSP之间通过SPI接口相连，MT48LC8M32型SDRAM与ADSP-BF561型双核DSP之间通过数据总线和地址总线相连。

采用本系统进行多点目标立体视觉光学跟踪的方法如下：

步骤一、两片MT9V022采集目标和背景的原始双目图像，将两片MT9V022所产生的图像信号、行同步和帧同步信号以及像素时钟信号均串化为LVDS信号后串行输出。由主传感器对两幅图像的数据进行整合打包，打包后的LVDS串行数据中包括每个传感器的图像数据(每个像素分辨率为8位)和像素时钟，其中像素时钟信号以首位高电平、末尾低电平的形式嵌在串行数据流中，而图像的行同步和帧同步信号则以保留字的形式嵌在数据流中，这样的LVDS串行数据流送入DS92LV16型图像传感器数据解串芯片中进行解串。

LVDS串行数据送入DS92LV16中进行解串处理后，得到两幅图像的各8位并行数据，同时恢复出像素时钟，而每幅图像的行同步和帧同步信号则以保留字的形式内嵌在同步视频数据流中。

步骤二、对双目同步原始图像数据进行处理，得到目标识别和标记结果。具体方法如下：

(1)采集双目原始图像，对图像进行平滑滤波，使点目标连通域的边缘毛刺得到削减。以3×3的滤波窗口为例，例化两个1024×8的FIFO，将两个FIFO首尾相接，对上两行的像素数据进行缓存，抽取出滤波窗口所需要的九个像素数据，对这些数据进行滤波处理得到滤波后的图像。

(2)进行自适应阈值计算

首先，逐行扫描步骤(1)平滑滤波后的图像中的每个像素，获取所有像素灰度值中最大值和最小值之间的中间值作为迭代计算的初始估计值T。

然后，逐行扫描步骤(1)平滑滤波后的图像中的每个像素，在扫描过程中，计算像素灰度值大于T的所有像素的灰度平均值m1，计算像素灰度值小于T的所有像素的灰度平均值m2，然后由m1和m2的均值得到一个新的阈值T_new。

之后，通过以下机制对迭代终止与否进行决策：如果新阈值T_new和旧阈值T之差的绝对值大于或等于预设值ε，则新阈值将做为下一帧图像的输入阈值，对下一帧图像进行相同的迭代过程；如果新阈值和旧阈值之差的绝对值小于预设值ε，则迭代结束。其中，ε的取值由迭代过程的计算精度决定。在进行决策的同时，保存此时的新阈值T_new，用以分割下一帧图像。

(3)采用步骤(2)得到的新阈值T_new，对图像进行分割，将目标从背景中提取出来。具体可采用以下方法：

设滤波后像素灰度表示为f(x，y)，其中x＝1，2，…m；y＝1，2，…n。阈值化过程如公式1.1所示：

$F(x,y)=\left\{\begin{array}{cc}f(x,y),& f(x,y)\&GreaterEqual;T\\ 0,& f(x,y)<T\end{array}\right.---1.1$

公式1.1中，T表示自适应阈值模块所计算得出的最佳阈值，F(x，y)表示进行分割后的图像像素的灰度值。

(4)设计一个3×7的滑动窗口，用以对经步骤(3)得到的目标分割图像进行连通域标记。图2为滑动窗口的结构。

本滑动窗口的最下面一行的中间位置是当前像素的灰度值P0，当前像素的前面位置和上面两行的位置均为已经被处理过的像素被赋予的初步标记结果，这些初步标记结果按从左到右，从上到下依次记为L1至L17。标记过程如下：当某个当前像素被扫描时，会出现两种情况。情况一，当前像素表示背景，则它被直接标记为零；情况二，当前像素表示有效的目标像素，则依次扫描L1至L17，一旦探测到一个有效的初步标记结果，则将该结果赋给当前像素；如果L1至L17位置均没有有效的初步标记结果，则给当前像素赋予一个新的标记。

(5)当步骤(4)完成之后，每个像素位置都被赋予了一个临时的标记。但是，由于目标边缘的毛刺，会产生标记冲突。因此，将同一连通域中的不同标记区域的像素位置的带灰度加权的坐标累加值进行再次累加，从而计算出目标的重心位置。同时，将同一连通域中产生标记冲突的区域设置为等价连通域，得到每个点目标连通域的最终标记。

实现目标的重心位置识别的具体方法为：根据透视投影变换所固有的特性，椭圆或圆在摄像机像平面上所成的像一般仍为椭圆。椭圆或圆在摄像机平面上所成的像比较小，一般只占几十个像素时，重心法是提取椭圆或圆孔图像中心比较有效的方法。重心法实际上就是计算二值化后椭圆或圆图像的一阶矩，公式1.2和公式1.3说明了重心计算原理：

$x_p=\frac{\underset{x=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{y=1}{\overset{n}{\mathrm{\&Sigma;}}}E(x,y)x}{\underset{x=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{y=1}{\overset{n}{\mathrm{\&Sigma;}}}F(x,y)}---1.2$

$y_p=\frac{\underset{x=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{y=1}{\overset{n}{\mathrm{\&Sigma;}}}F(x,y)y}{\underset{x=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{y=1}{\overset{n}{\mathrm{\&Sigma;}}}F(x,y)}---1.3$

在公式1.2和公式1.3中，F(x，y)表示某像素经过高斯滤波后的灰度值，x和y分别表示有效目标像素在图像平面内的横纵坐标。具有等价标志的像素的x坐标和y坐标的灰度加权累加和被再次累加，由累加结果根据公式1.2和公式1.3计算即可得出目标的重心位置(x_p，y_p)^T。

步骤三、当所有点目标的重心位置识别和点目标连通域标记完成后，将识别和标记结果发送至双核DSP内进行并行计算，实现目标的定位和跟踪，最终结果送至系统外部的上位机中供应用系统使用。并行计算方法包括如下步骤：

设定双核DSP的两个核分别为A核和B核。

(1)对步骤二中测得的点目标在图像中的连通域重心的二维坐标(x_p，y_p)^T进行并行畸变校正。具体方法如下：

根据(x_p，y_p)^T由公式1.4解算出含畸变的归一化点坐标(x_d，y_d)^T，

$x_d=\frac{x_p-u_0}{\mathrm{\&alpha;}}$ 1.4

$y_b=\frac{y_p-v_0}{\mathrm{\&beta;}}$

其中，α表示相机在水平方向上的焦距，β表示相机在垂直方向上的焦距，(u₀，v₀)表示相机主点的位置。

然后，根据非线性方程组1.5解算出满足针孔模型的归一化投影坐标(x，y)^T，

$x_d=\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]=(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}2p_1\mathrm{xy}+p_2(r^2+2x^2)\\ p_1(r^2+2y^2)+2p_2\mathrm{xy}\end{array}\right]---1.5$

其中，r²＝x²+y²，k_i(i＝1，2，3)表示径向畸变系数，p_i(i＝1，2)表示切向畸变系数。

图3为并行畸变校正过程的示意图。双核DSP芯片提供了两个PPI(ParallelPeripheral Interface)接口：PPI0和PPI1。将两幅图像分为左图像和右图像，左图像和右图像的识别和标记结果分别通过PPI0和PPI1接口送入双核DSP芯片中。数据传送过程采用DMA(Direct Memory Access)的方式，将PPI接口的数据直接在独立于双核DSP芯片的CPU控制下，送入双核DSP芯片内部的双核共享存储器中。通过采用DMA的方式，使双核DSP芯片的CPU专注于畸变校正，从而提高系统的计算效率。

一旦一帧识别结果(包括左右双图像中目标的识别结果)被传送到双核DSP芯片后，A核和B核分别进入自己的中断向量。在中断服务函数中，A核负责对左图像内的目标二维位置进行校正；与此同时，B核则对右图像内的目标二维位置进行相同的操作。畸变校正完成之后，即可得到满足针孔模型的归一化的目标投影位置。

(2)畸变校正完成后，A核进行立体视觉的三角定位和卡尔曼滤波所需要的状态预测，完成后进入等待状态。与此同时，B核计算单步预测误差协方差矩阵和卡尔曼增益，计算完毕后，B核给双核DSP芯片的共享存储器内的标志Flag1进行置位。待A核探测到标志Flag1后，A核对目标的状态进行更新；与此同时，B核进行卡尔曼滤波误差协方差矩阵计算，以备下一帧滤波过程。A核对目标的状态更新完毕以后，将双核DSP芯片的共享存储器内的标志Flag2置位，并将跟踪结果送出至上位机中。B核探测到标志Flag2后，双核同时对各自管辖的相关标志和变量进行重置，开始下一帧数据的处理过程。

整个并行计算过程具体流程如图4所示。

上述过程中，A核进行立体视觉的三角定位的具体实现过程如下：

假设点目标P在左右摄像机坐标系中的坐标分别为P_L＝[X_L Y_L Z_L]^T和P_R＝[X_R Y_R Z_R]^T，它们的关系如公式1.6所示：

P_R＝RP_L+T    1.6

其中，R为旋转矩阵。R和平移矢量T描述了右摄像机相对于左摄像机的位置关系。

令q_L＝P_L/Z_L＝[x_l y_l 1]^T、q_R＝P_R/Z_R＝[x_r y_r 1]^T，分别表示P点在左右两幅图像上的投影点的齐次坐标。由畸变校正已经得到[x_l y_l]^T和[x_r y_r]^T，将公式1.6重写成公式1.7：

Z_Rq_R-Z_LRq_L＝T    1.7

通过求解公式1.7所示的非齐次方程组，即可得到P点在左摄像机坐标系中的坐标[X_L Y_L Z_L]^T，实现对目标的三角定位。

卡尔曼滤波的运动方程和观测方程的建立过程如下：

将A核进行立体视觉的三角定位后得到的三维坐标[X_L Y_L Z_L]^T作为观测值，对目标建立运动模型，采用卡尔曼滤波对点目标的位置及运动参数进行跟踪。空间中的多个点目标存在于一个带噪声的双目视频图像序列中，令I是左相机所在的坐标系统，世界坐标系与左摄像机坐标系重合，假设P是双目有效视场空间内的一点，定义以下三个矢量：

p＝[X_L Y_L Z_L]^T是P在I坐标系中的位置矢量；

v＝(v_x v_y v_z)^T是P在I坐标系中的速度矢量；

a＝(a_x a_y a_z)^T是P在I坐标系中的加速度矢量。

运动点目标的状态转移方程如公式1.8所示：

$\left[\begin{array}{c}p\left(t_2\right)\\ v\left(t_2\right)\\ a\left(t_2\right)\end{array}\right]=\left[\begin{array}{ccc}{I}_3& I_3\mathrm{\&Delta;t}& \frac{1}{2}{I}_3\mathrm{\&Delta;}{t}^2\\ 0_3& I_3& I_3\mathrm{\&Delta;t}\\ 0_3& 0_3& I_3\end{array}\right]\left[\begin{array}{c}p\left(t_1\right)\\ v\left(t_1\right)\\ a\left(t_1\right)\end{array}\right]+m\left(t_1\right)=\mathrm{\&Phi;}(t_2,t_1)\left[\begin{array}{c}p\left(t_1\right)\\ v\left(t_1\right)\\ a\left(t_1\right)\end{array}\right]+m\left(t_1\right)---1.8$

其中，状态矢量为[p^T(t₂)v^T(t₂)a^T(t₂)]^T，状态转移矩阵为Φ(t₂，t₁)，I₃为三阶单位矩阵，离散时间间隔Δt＝t₂-t₁，状态转移噪声为m(t₁)，假设它为白噪声，满足高斯分布。观测方程如公式1.9所示：

$p\left(t_2\right)=\left[\begin{array}{ccc}{I}_3& 0_3& 0_3\end{array}\right]\left[\begin{array}{c}p\left(t_2\right)\\ v\left(t_2\right)\\ a\left(t_2\right)\end{array}\right]+n\left(t_2\right)=H\left(t_2\right)\left[\begin{array}{c}p\left(t_2\right)\\ v\left(t_2\right)\\ a\left(t_2\right)\end{array}\right]+n\left(t_2\right)---1.9$

其中，p(t₂)是观测矢量，I₃是三阶单位矩阵，H(t₂)是观测矩阵，观测噪声为n(t₂)，假设它为白噪声，满足高斯分布。

下面通过实验来验证本发明所涉及的光学跟踪系统的实时性和精度等性能。

图5为本具体实施方式所涉及的光学跟踪系统的延迟结构。系统的延迟主要来自两个方面：其一，FPGA芯片的并行流水线算法会带来识别结果和原始图像之间的延迟；其二，双核DSP芯片的并行算法会带来跟踪结果和识别结果之间的延迟。对FPGA芯片而言，在每相邻两级流水线之间存在一个像素时钟的延迟。由关于并行流水线算法的描述可知，二维识别和标记结果与原始图像之间的延迟仅为7个像素时钟，若像素时钟为27MHz，那么系统延迟仅为259ns。对双核DSP芯片而言，由于其单核串行处理的机制，其对系统造成的延迟远大于FPGA芯片的影响。所以整个系统的延迟由双核DSP芯片决定。

图6为本发明具体实施方式中所涉及的光学跟踪系统在工作时，被跟踪目标数目和处理时间之间的曲线图。实验方法如下：将不同数目的目标(点目标数目分别为4，16，32，48，64，80，96，112，128)依次放在双目系统的公共视场中。在每次实验过程中，记录下跟踪一帧目标所需要的时间，也就是系统处理一帧图像所产生的延迟。图6中，横轴表示目标数目，纵轴表示系统处理含对应数量目标的一帧图像所需要的时间。双核DSP(ADSP-BF561)内核的频率为600MHz，前端CMOS图像传感器的帧率为60帧/秒，为了使系统能够实时不丢帧地跟踪目标，处理一帧图像所耗用的内核执行周期数不能超过10⁷，如图6中标注所示，系统能够实时不丢帧地跟踪至多达110个目标，系统处理结果的刷新率与原始图像的帧率相同。

通过运动导轨使目标在导轨平台上做匀速运动，在某运动速率条件下，记录连续100帧的图像处理结果，即100帧卡尔曼滤波的最优线性估计结果。根据每帧的卡尔曼滤波结果，计算出目标2和目标3之间的最优估计距离。在每个不同的速率条件下，重复以上实验。对应每个速率值条件下的均值(Mean)和均方差(RMS)列于

表1中。此外，根据游标卡尺所测得的目标2和目标3之间的距离为45.19mm。由实验结果可知，在不同的运动速率条件下，系统均能对空间中的多个目标进行实时地跟踪，跟踪精度可达0.5mm RMS。

表1目标2和目标3之间距离的100帧最优卡尔曼估计统计值

在导轨上标出沿深度方向的连续21个位置，每个相邻位置之间的距离由导轨精确确定为22.00mm，深度范围约为0.75m至0.45m之间。将目标刚体分别置于这21个位置上，分别测试系统计算得出的目标2在每个位置的3D坐标，并用矢量相减的办法得到系统计算得出的相邻位置之间的距离。实验数据如图7所示，跟踪系统计算出的20个深度距离的统计结果列于

表2中。可见，系统计算出的20个深度距离并不都是标准的22.00mm，而是存在一定误差。在0.75m至0.45m的深度范围内，系统对目标的跟踪精度为0.45mm RMS。

表2深度方向的距离测量数据统计(标准距离：22mm；深度范围：0.75m至0.45m)

Claims (7)

1.一种针对多点目标的立体视觉光学跟踪系统，包括图像采集模块、电源模块，其特征在于，还包括目标标记与识别模块与目标定位跟踪模块；

目标标记与识别模块：基于并行流水线方式，用于实现双目或多目同步原始图像中目标的识别和标记，即，识别目标重心在同步原始图像中的二维位置，并给每一个点目标赋予一个唯一的标记；目标标记与识别模块包括一片FPGA芯片以及一片FPGA配置芯片，用于对双目或多目同步原始图像数据进行处理；

目标定位跟踪模块：用于实现目标的三维空间定位和对运动目标的跟踪；目标定位跟踪模块由至少一片双核DSP、一片DSP指令存储芯片以及一片SDRAM组成；双核DSP接收FPGA芯片处理得到的目标识别和标记结果，并在每片图像传感器的帧回扫过程中进行多点目标定位；SDRAM对处理过程中的临时数据进行缓存，此外，通过将原始图像数据直接传入SDRAM，然后转存成图像文件，以此作为摄像机标定过程所需要的原始图像；DSP指令存储芯片用于存储双核DSP的指令；DSP指令存储芯片与双核DSP之间通过SPI接口相连，SDRAM与双核DSP之间通过数据总线和地址总线相连；

上述组成部分的连接关系如下：

当图像传感器为串行输出时，目标标记与识别模块中FPGA芯片的输入端同图像采集模块中的图像传感器数据解串芯片的输出端相连；当图像传感器为并行输出时，目标标记与识别模块中FPGA芯片的输入端分别同图像采集模块中每片图像传感器的输出端相连；

FPGA芯片的数据输出接口同目标定位跟踪模块中双核DSP的数据输入接口相连接，双核DSP的输出端同系统外部的上位机的数据输入端相连；

电源模块分别同上述各模块相连；

本系统的工作过程及部件间的信号转换关系如下：

首先，利用图像采集模块中的图像传感器采集目标和背景的原始双目或多目图像，并将图像数据送入目标标记与识别模块中的FPGA芯片中；

然后，在目标标记与识别模块中，采用并行流水线方式实现原始双目或多目图像中所有点目标的重心识别和连通域的最终标记；

当所有点目标的重心识别和标记完成后，将识别和标记结果发送至目标定位跟踪模块中的双核DSP内进行并行计算，实现目标的定位和跟踪，最终结果送至系统外部的上位机中供应用系统使用。

2.如权利要求1所述的一种针对多点目标的立体视觉光学跟踪系统，其特征在于，所述图像采集模块的每一片图像传感器都采用全帧同时曝光快门。

3.如权利要求1所述的一种针对多点目标的立体视觉光学跟踪系统，其特征在于，所述目标标记与识别模块中采用并行流水线方式实现原始双目或多目图像中所有点目标的重心识别和连通域的最终标记的具体步骤如下：

a.采集原始图像，并对图像进行平滑滤波，使点目标连通域的边缘毛刺得到削减；

b.进行自适应阈值计算：

先逐行扫描经步骤a平滑滤波后的图像中的每个像素，获取所有像素灰度值中最大值和最小值之间的中间值作为迭代计算的初始估计值T；

再逐行扫描经步骤a平滑滤波后的图像中的每个像素，计算得到像素灰度值大于T的所有像素的灰度平均值m₁，以及像素灰度值小于T的所有像素的灰度平均值m₂，然后由m₁和m₂的均值得到一个新阈值T_new；

如果新阈值T_new和旧阈值T之差的绝对值大于或等于预设值ε，则新阈值T_new将做为下一帧图像的输入阈值，并对下一帧图像进行相同的迭代过程；如果新阈值T_new和初始估计值T之差的绝对值小于预设值ε，则迭代结束；其中，ε的取值由迭代过程的计算精度决定；在进行决策的同时，保存此时的新阈值T_new，用以分割下一帧图像；

c.采用步骤b中得到的新阈值T_new，对图像进行分割，将目标从背景中提取出来；

d.设一个m×n的滑动窗口，用以进行连通域标记；该滑动窗口的最下面一行的中间位置是当前像素的灰度值，当前像素的前面位置和上面几行的位置均为已经被处理过的像素被赋予的初次标记结果，这些初步标记结果按从左到右、从上到下依次记为L₁至L_k，标记过程如下：当某个当前像素被扫描时，会出现两种情况；情况一，当前像素表示背景，则它被直接标记为零；情况二，当前像素表示有效目标像素，则依次扫描L₁至L_k，一旦探测到一个有效的初步标记结果，则将该结果赋给当前像素；如果L₁至L_k位置均没有有效的初步标记结果，则给当前像素赋予一个新的标记；

e.步骤d完成之后，每个像素位置都被赋予了一个临时标记；将同一连通域中的不同标记区域的像素位置的带灰度加权的坐标累加值进行再次累加，从而计算出目标的重心位置；同时，将同一连通域中产生标记冲突的区域设置为等价连通域，得到每个点目标连通域的最终标记；

f.根据图像中点目标连通域的标记结果，得到双目图像中目标的匹配关系。

4.如权利要求1所述的一种针对多点目标的立体视觉光学跟踪系统，其特征在于，在双核DSP内进行并行计算的具体步骤如下：

设定双核DSP的两个核分别为A核和B核；

①根据经并行流水线方式实现得到的点目标重心在图像中的位置，对目标位置进行畸变校正，左右两幅图像中的目标位置校正由双核处理器的两个核分别进行，左右图像的校正过程通过双核并行完成；畸变校正完成后，即可得到点目标在像面上的满足针孔模型的归一化投影位置；

②根据步骤①得到的归一化投影位置，采用立体视觉三角定位算法对空间中的点目标进行定位，得到目标在世界坐标系中的三维坐标；

此时，A核进行立体视觉的三角定位和卡尔曼滤波所需要的状态预测，结束后进入等待状态；与此同时，B核计算单步预测误差协方差矩阵和卡尔曼增益，计算完毕后，B核给双核DSP芯片的共享存储空间内标志Flag1进行置位；

③将步骤②得到的三维坐标作为观测值，对目标建立运动模型，采用卡尔曼滤波对点目标的位置及运动参数进行跟踪；

5.如权利要求4所述的一种针对多点目标的立体视觉光学跟踪系统，其特征在于，对测得的点目标在图像中的连通域重心的二维坐标(x_p，y_p)^T进行并行畸变校正的方法如下：

当A核探测到标志Flag1后，对目标的状态进行更新；与此同时，B核进行卡尔曼滤波误差协方差矩阵计算，以备下一帧滤波过程；A核对目标的状态更新完毕以后，将双核共享存储空间内的标志Flag2置位，并将跟踪结果送出至上位机；B核探测到标志Flag2后，双核同时对各自管辖的相关标志和变量进行重置，开始下一帧数据的处理过程；具体方法如下：

根据(x_p，y_p)^T由公式1.4解算出含畸变的归一化点坐标(x_d，y_d)^T，

$x_d=\frac{x_p-u_0}{\mathrm{\&alpha;}}$ 1.4

$y_d=\frac{y_p-v_0}{\mathrm{\&beta;}}$

其中，α表示相机在水平方向上的焦距，β表示相机在垂直方向上的焦距，(u₀，v₀)表示相机主点的位置；

然后，根据非线性方程组1.5解算出满足针孔模型的归一化投影坐标(x，y)^T，

$x_d=\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]=(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}{2p}_1\mathrm{xy}+p_2(r^2+{2x}^2)\\ p_1(r^2+{2y}^2)+{2p}_2\mathrm{xy}\end{array}\right]---1.5$

其中，r²＝x²+y²，k_i(i＝1，2，3)表示径向畸变系数，p_i(i＝1，2)表示切向畸变系数。

6.如权利要求4所述的一种针对多点目标的立体视觉光学跟踪系统，其特征在于，A核对空间中的点目标进行立体视觉的三角定位的具体实现过程如下：

假设点目标P在左右摄像机坐标系中的坐标分别为P_L＝[X_L Y_L Z_L]^T和P_R＝[X_R Y_R Z_R]^T，它们的关系如公式1.6所示：

P_R＝RP_L+T    1.6

其中，R为旋转矩阵；R和平移矢量T描述了右摄像机相对于左摄像机的位置关系；

令q_L＝P_L/Z_L＝[x_l y_l 1]^T、q_R＝P_R/Z_R＝[x_r y_r 1]^T，分别表示P点在左右两幅图像上的投影点的齐次坐标；由畸变校正已经得到[x_l y_l]^T和[x_r y_r]^T，将公式1.6重写成公式1.7：

Z_Rq_R-Z_LRq_L＝T    1.7

通过求解公式1.7所示的非齐次方程组，即可得到P点在左摄像机坐标系中的坐标[X_L Y_L Z_L]^T，实现对目标的三角定位。

7.如权利要求5所述的一种针对多点目标的立体视觉光学跟踪系统，其特征在于，B核进行卡尔曼滤波误差协方差矩阵计算时，卡尔曼滤波的运动方程和观测方程的建立过程如下：

将A核进行立体视觉的三角定位后得到的三维坐标[X_L Y_L Z_L]^T作为观测值，对目标建立运动模型，采用卡尔曼滤波对点目标的位置及运动参数进行跟踪；空间中的多个点目标存在于一个带噪声的双目视频图像序列中，令I是左相机所在的坐标系统，世界坐标系与左摄像机坐标系重合，假设P是双目有效视场空间内的一点，定义以下三个矢量：

p＝[X_L Y_L Z_L]^T是P在I坐标系中的位置矢量；

v＝(v_x v_y v_z)^T是P在I坐标系中的速度矢量；

a＝(a_x a_y a_z)^T是P在I坐标系中的加速度矢量；

运动点目标的状态转移方程如公式1.8所示：

$\left[\begin{array}{c}p\left(t_2\right)\\ v\left(t_2\right)\\ a\left(t_2\right)\end{array}\right]=\left[\begin{array}{ccc}{I}_3& I_3\mathrm{\&Delta;t}& \frac{1}{2}{I}_3{\mathrm{\&Delta;t}}^2\\ 0_3& I_3& I_3\mathrm{\&Delta;t}\\ 0_3& 0_3& I_3\end{array}\right]\left[\begin{array}{c}p\left(t_1\right)\\ v\left(t_1\right)\\ a\left(t_1\right)\end{array}\right]+m\left(t_1\right)=\mathrm{\&Phi;}(t_2,t_1)\left[\begin{array}{c}p\left(t_1\right)\\ v\left(t_1\right)\\ a\left(t_1\right)\end{array}\right]+m\left(t_1\right)---1.8$

其中，状态矢量为[p^T(t₂)v^T(t₂)a^T(t₂)]^T，状态转移矩阵为Φ(t₂，t₁)，I₃为三阶单位矩阵，离散时间间隔Δt＝t₂-t₁，状态转移噪声为m(t₁)，假设它为白噪声，满足高斯分布，观测方程如公式1.9所示：

$p\left(t_2\right)=\left[\begin{array}{ccc}{I}_3& 0_3& 0_3\end{array}\right]\left[\begin{array}{c}p\left(t_2\right)\\ v\left(t_2\right)\\ a\left(t_2\right)\end{array}\right]+n\left(t_2\right)=H\left(t_2\right)\left[\begin{array}{c}p\left(t_2\right)\\ v\left(t_2\right)\\ a\left(t_2\right)\end{array}\right]+n\left(t_2\right)---1.9$

其中，p(t₂)是观测矢量，I₃是三阶单位矩阵，H(t₂)是观测矩阵，观测噪声为n(t₂)，假设它为白噪声，满足高斯分布。

Images