CN104748727A - 阵列式高速视觉里程计及其实现方法 - Google Patents

Abstract

本发明为一种阵列式高速视觉里程计及其实现方法。本发明包括若干个视觉测量单元、数据采集电路板和工控机。利用的单个视觉测量单元并行计算特征点的三维位置；根据视觉测量单元的空间位置变换关系和空间运动一致性，对各视觉测量单元输出的特征点的三维位置数据进行校验；利用多个视觉测量单元输出的特征点的三维位置数据，通过无色卡尔曼滤波方法进行数据融合；并根据视觉测量单元的空间位置变换关系，通过非线性优化方法，精确估计机器人的位姿变化量。本发明具有响应速度快、鲁棒性好、参数估计精度高、能耗低、隐蔽性好和使用方便等优点，完全可以满足机器人在非结构环境下对自主定位导航方法的实时性和鲁棒性的要求。

Description

阵列式高速视觉里程计及其实现方法

技术领域

本发明属于机器人自主定位导航领域，涉及数字图像处理、计算机视觉和机器人学等技术，为一种机器人高精度实时定位定姿方法，具体为一种阵列式高速视觉里程计及其实现方法。

背景技术

在机器人自主定位导航领域中，传统的机器人定位定姿方法主要包括：以码盘式里程计和惯导等设备以及相应的航迹推算算法（Dead-Reckoning）实现的相对定位方法；以固定路标和GPS技术为代表的绝对定位方法。其中，相对定位方法通过码盘式里程计实现定位，但由于码盘式里程计存在车轮半径测量误差带来的系统误差和车轮滑动造成的非系统误差，在高滑动地形条件下误差累积的速度非常快，因此往往与惯导组合使用，利用惯导实时估计机器人的姿态变化并推算其位置变化，通过数据融合相互校正。然而惯导存在以时间次方速率产生漂移的问题，当机器人进行长时间、大范围作业时，仍需要进行周期性校正。在人工环境下，这个问题可以通过设置固定路标或使用GPS解决，但在不存在固定路标的野外环境等非人工环境下，这种相对定位方法对GPS的依赖就十分严重了。但是由于GPS技术的敏感性（主要由美国掌握）、不确定性（受到干扰或卫星故障）、定位精度和应用空间的局限性，导致其在野外环境中的应用受限甚至无法应用，在这种情况下，传统技术方法的缺点更加突出，已明显无法满足机器人的技术要求。

为了满足机器人在非结构化环境下长时间、长距离作业时的精确自主导航技术要求，上世纪80年代中期以来，出现了被称为视觉里程计（Visual Odometry）的机器人实时定位定姿方法，是计算机视觉理论在机器人领域的一个重要应用。视觉里程计仅利用视觉系统获得的环境图像，对机器人运动过程中的姿态和位置变化进行精确估计，属于被动非接触式测量方法，具有精度高、隐蔽性好、不依赖GPS校正以及不受环境地表特性和应用空间限制等突出的优点。虽然视觉里程计仍是一种相对定位方法，但已有的研究表明，视觉里程计的误差呈非单应性，即使机器人在高滑动地形条件（滑动比>100％）下进行长距离（作业距离>1km）导航时，仍能有效将累积误差控制在一定范围内，并且当与测角仪和惯导等组合使用时，累积误差的范围更小。2004年美国国家航空和宇宙航行局（NASA）将视觉里程计应用于火星探测并取得了巨大的成功，充分说明了视觉里程计方法的有效性以及在某些特定环境下的不可替代性。

目前，现有的视觉里程计方法基本上都采用了“特征－特征三维位置－运动估计”的方法框架，只是在结构上有单目和双目之分，以及具体实现方法的差异。现有的方法都存在实时性差的瓶颈问题（处理速度<15Hz）。这主要是由于现有的这些方法对环境图像的处理过程采用的都是逐帧逐点的计算方式，这就造成当图像尺寸较大时，待处理的数据量很大，而计算机的处理能力与之相比就明显不够。而通过减小图像大小，降低数据量，将导致视觉里程计精度的大幅下降。此外，在非结构化环境下，存在各种导致视觉里程计失效的不确定性因素。现有的视觉里程计方法仅采用单目或双目结构，遇到相机或线路损坏等情况时，整个视觉里程计就完全失效，鲁棒性差。

发明内容

针对现有技术中存在的上述不足之处，本发明要解决的技术问题是提供一种解决机器人作业过程中实时精确定位定姿问题的高速鲁棒三维视觉里程计及其实现方法。

本发明为实现上述目的所采用的技术方案是：一种阵列式高速视觉里程计，包括

若干个视觉测量单元，呈平面或空间的几何阵列分布，用于观测机器人的姿态和位置参数，并行计算，独立输出，多个视觉测量单元设置图像各个方向上不同的长宽比，所提取的特征点在图像中均匀分布；

数据采集电路板，连接各个视觉测量单元，用于采集所述视觉测量单元监测到的图像数据并对其进行数据校验和故障诊断，然后传送给工控机；

工控机，用于将多个视觉测量单元间的数据进行融合，输出最终的机器人的姿态和位置。

所述视觉测量单元包括：

相机，用于拍摄机器人图像；

固化算法的专用电路板，连接相机，用于计算图像特征点的空间三维位置。

所述相机采集的图像大小不大于10000像素，数据传输速率不低于2Mbps。

所述数据采集电路板还用于关闭失效的视觉测量单元通道。

一种阵列式高速视觉里程计的实现方法，包括以下步骤：

利用的单个视觉测量单元并行计算特征点的三维位置；

根据视觉测量单元的空间位置变换关系和空间运动一致性，对各视觉测量单元输出的特征点的三维位置数据进行校验；

利用多个视觉测量单元输出的特征点的三维位置数据，通过无色卡尔曼滤波方法进行数据融合；并根据视觉测量单元的空间位置变换关系，通过非线性优化方法，精确估计机器人的位姿变化量。

所述单个视觉测量单元并行计算特征点的三维位置，包括以下步骤：

步骤1，当i＝1时，从t_i-1和t_i时刻立体视觉图像对和中，采用Harris算子提取特征点；当i＞1时，仅从t_i时刻立体视觉图像中，提取特征点；

步骤2，当i＝1时，分别对t_i-1和t_i时刻立体视觉图像对和中提取的特征点进行匹配；当i＞1时，仅对t_i时刻立体视觉图像中提取的特征点进行匹配；

步骤3，对立体视觉图像对和中提取和匹配的特征点，在左右图像序列中分别进行跟踪，

步骤4，对匹配和跟踪的特征点，进行最小二乘意义下的三维重建；

步骤5，将中提取和匹配的特征点作为中提取和匹配的特征点，进入下一时刻的特征点匹配与跟踪。

所述对特征点进行匹配采用改进的MNCC算法计算特征点在立体视觉图像对之间的匹配相关性，采用外极线校正和视差约束减小搜索范围；通过双向一致性约束和唯一性约束滤除误匹配特征点。

所述步骤3采用改进的MNCC算法计算特征点之间的相关性，相关性最大的特征点将作为跟踪的特征点；仅采用极线约束条件，通过Hartley八点法估计基础矩阵，利用约束条件滤除误跟踪的特征点。

所述对各视觉测量单元输出的特征点的三维位置数据进行校验，包括以下步骤：

利用采集的视觉测量数据，通过加权最小二乘法估计机器人运动初值M0；

设定一个阀，通过RANSAC法去除无效数据点值t′，当根据特征点计算得到的机器人运动与最优值之间的误差时，当前的跟踪特征点分别为离群值，反之为有效特征点；

根据测量单元坐标变换关系，进行数据有效性的二次判断；

如果视觉测量单元的输出数据过少或输出的有效数据过少时，将认为视觉测量单元出现故障，关闭该视觉单元。

所述利用多个视觉测量单元输出的特征点的三维位置数据，通过无色卡尔曼滤波方法进行数据融合；并根据视觉测量单元的空间位置变换关系，通过非线性优化方法，精确估计机器人的位姿变化量，包括以下步骤：

根据数据校验算法计算得到的机器人位姿估计机器人运动初值M₀和全部视觉测量单元中匹配和跟踪的特征点，计算逆投影误差，与设定的误差阈值进行比较，滤除可能存在的无效数据；

利用计算得到的有效数据，根据加权最小二乘法重新计算机器人位姿估计初值，并将其作为机器人位姿优化估计的初始值；

根据稀疏LM算法计算机器人的位姿变化，通过位姿推算算法计算机器人的当前位姿。

本发明克服了已有方法的实时性和鲁棒性差的问题，借鉴了现有方法参数估计的精度优势和实时性差的瓶颈问题，将昆虫蜂眼结构和全向快速运动感知能力的生物特征与计算机视觉理论有机融合，设计出的视觉里程计方法属于非接触式被动测量，具有响应速度快、鲁棒性好、参数估计精度高、能耗低、隐蔽性好和使用方便等优点，完全可以满足机器人在非结构环境下对自主定位导航方法的实时性和鲁棒性的要求。具体具有如下优点和有益效果：

1.本发明以小尺寸图象作为输入，通过底层并行计算的高速视觉测量单元阵列，实现了高速处理，解决了由于对整帧大尺寸图像逐点计算的低效串行计算方式和计算机处理能力不足所导致的现有视觉里程计方法实时性差的瓶颈问题。

2.本发明通过多个高速视觉测量单元组成阵列、各测量单元定向敏感和测量单元间的数据融合，实现在大幅提高视觉里程计实时性的同时，保证其高精度的参数估计输出，解决了现有视觉里程计方法通过减小图像大小提高实时所导致的精度下降的问题。

3.由于非结构化环境下存在各种导致视觉里程计失效的不确定因素，而现有视觉里程计方法仅采用单目或双目结构，当某一相机失效时，整个视觉里程计就完全失效，鲁棒性差。在本发明中，由多个高速视觉测量单元组成的阵列为方法的鲁棒性提供了硬件实现条件，通过在数据采集和处理过程中，建立故障诊断和阵列重构机制，从而即使出现多个相机同时失效的极端条件下，仍可以保证视觉里程计底层数据输出的有效性，具有很好的鲁棒性。

4.在本发明中，多个高速视觉测量单元的阵列分布设计不局限于实例中所示的圆周分布，理论上可以为任意分布。在应用中，由于测量单元的定向敏感，用户仅需要将高速视觉测量单元尽可能地在各个方向上均匀分布，即可很好地保证系统精度。因此，用户的使用十分方便。

5.本发明是一种三维视觉里程计，并且不依赖环境先验知识（已知的环境信息），属于被动式非接触测量方法，不受环境地质地貌条件的限制，非常适用于环境信息动态变化或复杂未知的非结构化环境。

6.本发明精度高、误差累积小，既可不依赖GPS独立使用，也可与GPS、码盘、惯导和测角仪等设备组合使用，应用灵活且不受环境空间的限制（如GPS尚无法应用于外太空星球探测等）。

7.本发明为模块化设计，系统结构简单，通用性好，适用于各种机器人平台，具有能耗低、成本低廉和维护保养方便等突出的性价比优势。

8.本发明的底层硬件设备，如高速CCD相机、数据采集卡等已有成熟的技术产品，无需独立研制，因此，可以大大降低制造成本，便于迅速产业化。

附图说明

图1为本发明的系统结构示意图；

图2为本发明的系统硬件结构示意图；

其中，1为工控机，2为数据采集电路板，3为多个视觉测量单元组成的阵列，4为固化算法的专用电路板，5为相机；

图3为本发明的视觉测量单元算法结构示意图；

图4a为本发明实施实例的视觉测量单元的阵列分布俯视图；

图4b为本发明实施实例的视觉测量单元的阵列分布右视图；

图5为本发明实施实例的视觉测量单元定向敏感的图像采集示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明的原理是受昆虫复眼特性的启发。蜜蜂等具有复眼结构的昆虫具有惊人的自主导航能力和运动敏感性，如蜜蜂飞行4.5×105km却不会迷路，对移动目标的反应时间仅为0.01s。生物神经学家Wehner等人对苍蝇等昆虫复眼（Compound Eye）的研究成果表明，这种复眼属于阵列式结构，由数目不等的小眼依次排列组成，其视觉系统分为视网膜层和中枢神经层，其中视网膜层由许多面向不同方向的视神经感杆组成，每个视神经感杆由8个单向垂直交叉的感光器（Photoreceptor）组成，而且这些感光器仅对与其同向的偏振光（PolarizedLight）敏感。这就在视网膜上形成了一个偏振敏感阵列，最后由中枢神经进行处理，从而为昆虫带来了全向快速的运动感知能力。根据昆虫复眼视觉系统的结构和特性，本发明以底层硬件并行计算、视觉单元单向敏感和集中处理的思想作为框架，建立故障诊断和阵列重构机制，以计算机视觉理论为基础，形成一种新的高速鲁棒视觉里程计方法。

本发明是一种阵列式高速鲁棒三维视觉里程计，：主要贡献(创新)有以下四个方面（1）由多个算法硬件化高速测量单元组成的阵列，各个测量单元以小尺寸图像作为输入，并行计算，独立输出，实现高速的视觉处理过程；（2）如图5所示,根据昆虫复眼偏振敏感的特点，在本发明中的高速视觉测量单元设计中，将测量单元阵列分布，通过设置图像的不同长宽比和提取的特征点在图像中均匀分布，使每个视觉测量单元对某一方向上的机器人姿态和位置参数变化估计具有很高的精度，从而实现测量单元的定向敏感性；（3）通过多个测量单元间的数据融合，实现机器人全部姿态和位置变化参数的精确估计，有效解决了小尺寸图像对单一视觉测量单元精度的影响，保证了高速视觉里程计方法的终端输出精度；（4）在本发明中，多个视觉测量单元组成的阵列为提高视觉里程计方法的鲁棒性提供了硬件实现条件，在此基础上，我们在测量单元数据采集过程中，将引入故障诊断和阵列重构的机制，以保证高速视觉里程计方法的底层数据输出有效性，从而实现方法的鲁棒性。

如图1所示，本实施例包括三个部分：（1）由8个高速视觉测量单元组成圆周阵列，各测量单元并行计算、独立输出；（2）阵列数据集中采集，在数据采集过程中对各测量单元进行输出数据校验和故障诊断，并关闭失效的测量单元数据通道，进行阵列分布重构；（3）对各测量单元的进行数据融合，并反馈阵列信息，对阵列信息进行校正。具体为：

（1）8个高速视觉测量单元组成的圆周阵列。

本实施例中，视觉里程计的高速视觉测量单元阵列分布俯视及右视图如图4所示，可实现实时精确估计机器人本体6个自由度的姿态参数变化和空间三维位置变化。这样的阵列设计来源于我们从昆虫复眼结构获得的启发。与昆虫小眼类似，每个测量单元偏重于某一姿态参数的精度，因此，这样的设计非常有利于提高数据融合的精度。同时这样做的另一个好处是可在圆心建立一个平面坐标系，仅由半径r以及各测量单元和圆心的连线与坐标轴所成的夹角θ两个参数，就可以表示各个测量单元的平面坐标（即阵列分布信息），这样阵列分布信息的重构和校正过程就变成了对两个参数的调整过程，十分简单方便。

每个高速视觉测量单元的硬件组成主要包括：一个高速图像采集相机，采集的图像大小不大于10000像素，相机数据传输速率不低于2Mbps；固化算法的专用电路板。在高速测量单元的算法方面，每个测量单元将独立计算其图像特征点的空间三维位置，通过设置相机镜头方向（如图2所示）和均匀提取特征，使其对设定方向的参数估计精确；其中每个视觉测量单元的算法流程如图3所示：在相机已经标定的条件下，在高频图像序列中用Harris算子提取特征点，通过MNCC相关性对特征点进行跟踪；然后根据相机标定后得到的基础矩阵和本质矩阵来计算相机的旋转矩阵R和平移向量T；最后利用结合特征点跟踪的结果进行特征点的三维重建。

通过3帧图像来优化计算图像中的特征点空间三维位置，计算过程实际上是三个三角测量值的最小二乘加权平均，算法复杂度很低；根据测量单元的阵列分布位置，为每一个测量单元设置相应的精度加权系数，加权系数将随阵列分布校正或重构同步进行校正和重置，并与特征点的三维空间位置信息一起被用于测量单元间的数据融合。

（2）数据采集、故障诊断和阵列重构

通过硬件通讯，对各测量单元的计算数据进行集中采集，并在数据采集过程中，利用数据间的阵列分布关系进行相互校验，根据数据校验的结果进行故障诊断，如果某一测量单元被认为已失效，则关闭其数据通道，将其从阵列中剔除，并对阵列信息进行重构。重构的阵列信息将随有效测量单元计算数据一起输出。通过硬件通讯，对各测量单元的计算数据进行集中采集

（3）数据融合和阵列反馈

利用特征点集的三维位置变化，通过无色卡尔曼滤波（UKF）方法进行数据融合和处理，得到高精度的机器人姿态和位置变化估计。此外，在视觉里程计初始化校正过程中对阵列设计分布和实际分布之间的误差进行校正。

本发明所述的改进的MNCC算法为现有技术，参考杜英魁，韩建达，唐延东，Feature Matching And Tracking For Visual Odometry Of Mobile Robot【C】Proceedings of The International Conference Information Computing and Automation,Vols1-3.Singapore.World Scientific Publ Co Pte Ltd.2007,927-930.

本发明所述的外极线校正、视差约束、双向一致性约束、唯一性约束和极线约束条件在马颂德，张正友所著：计算机视觉—计算理论与算法基础【M】北京：科学出版社，1998.中有论述。

本发明所述的Hartley八点法参考：R.Hartley，In Defense of The8-Pointalgorithm【J】IEEE Transaetions on Pattern Analysis and MachineIntelligence,1997,19(6):580-593.

本发明所述的RANSAC法、计算逆投影误差的方法、加权最小二乘法、稀疏LM算法、位姿推算算法在周船，杜英魁所著：一种基于双目重建不确定性分析的运动估计方法【J】仪器仪表学报，2007，28(S):15-17.中有论述。

Claims (10)

1.一种阵列式高速视觉里程计，其特征在于，包括

若干个视觉测量单元，呈平面或空间的几何阵列分布，用于观测机器人的姿态和位置参数，并行计算，独立输出，多个视觉测量单元设置图像各个方向上不同的长宽比，所提取的特征点在图像中均匀分布；

数据采集电路板，连接各个视觉测量单元，用于采集所述视觉测量单元监测到的图像数据并对其进行数据校验和故障诊断，然后传送给工控机；

工控机，用于将多个视觉测量单元间的数据进行融合，输出最终的机器人的姿态和位置。

2.根据权利要求1所述的阵列式高速视觉里程计，其特征在于，所述视觉测量单元包括：

相机，用于拍摄机器人图像；

固化算法的专用电路板，连接相机，用于计算图像特征点的空间三维位置。

3.根据权利要求2所述的阵列式高速视觉里程计，其特征在于，所述相机采集的图像大小不大于10000像素，数据传输速率不低于2Mbps。

4.根据权利要求1所述的阵列式高速视觉里程计，其特征在于，所述数据采集电路板还用于关闭失效的视觉测量单元通道。

5.一种权利要求1所述阵列式高速视觉里程计的实现方法，其特征在于，包括以下步骤：

利用的单个视觉测量单元并行计算特征点的三维位置；

根据视觉测量单元的空间位置变换关系和空间运动一致性，对各视觉测量单元输出的特征点的三维位置数据进行校验；

利用多个视觉测量单元输出的特征点的三维位置数据，通过无色卡尔曼滤波方法进行数据融合；并根据视觉测量单元的空间位置变换关系，通过非线性优化方法，精确估计机器人的位姿变化量。

6.根据权利要求5所述的阵列式高速视觉里程计的实现方法，其特征在于，所述单个视觉测量单元并行计算特征点的三维位置，包括以下步骤：

步骤1，当i＝1时，从t_i-1和t_i时刻立体视觉图像对和中，采用Harris算子提取特征点；当i＞1时，仅从t_i时刻立体视觉图像中，提取特征点；

步骤2，当i＝1时，分别对t_i-1和t_i时刻立体视觉图像对和中提取的特征点进行匹配；当i＞1时，仅对t_i时刻立体视觉图像中提取的特征点进行匹配；

步骤3，对立体视觉图像对和中提取和匹配的特征点，在左右图像序列中分别进行跟踪，

步骤4，对匹配和跟踪的特征点，进行最小二乘意义下的三维重建；

步骤5，将中提取和匹配的特征点作为中提取和匹配的特征点，进入下一时刻的特征点匹配与跟踪。

7.根据权利要求6所述的阵列式高速视觉里程计的实现方法，其特征在于，所述对特征点进行匹配采用改进的MNCC算法计算特征点在立体视觉图像对之间的匹配相关性，采用外极线校正和视差约束减小搜索范围；通过双向一致性约束和唯一性约束滤除误匹配特征点。

8.根据权利要求6所述的阵列式高速视觉里程计的实现方法，其特征在于，所述步骤3采用改进的MNCC算法计算特征点之间的相关性，相关性最大的特征点将作为跟踪的特征点；仅采用极线约束条件，通过Hartley八点法估计基础矩阵，利用约束条件滤除误跟踪的特征点。

9.根据权利要求5所述的阵列式高速视觉里程计的实现方法，其特征在于，所述对各视觉测量单元输出的特征点的三维位置数据进行校验，包括以下步骤：

利用采集的视觉测量数据，通过加权最小二乘法估计机器人运动初值M₀；

设定一个阀，通过RANSAC法去除无效数据点值t′，当根据特征点计算得到的机器人运动与最优值之间的误差时，当前的跟踪特征点分别为离群值，反之为有效特征点；

根据测量单元坐标变换关系，进行数据有效性的二次判断；

如果视觉测量单元的输出数据过少或输出的有效数据过少时，将认为视觉测量单元出现故障，关闭该视觉单元。

10.根据权利要求5所述的阵列式高速视觉里程计的实现方法，其特征在于，所述利用多个视觉测量单元输出的特征点的三维位置数据，通过无色卡尔曼滤波方法进行数据融合；并根据视觉测量单元的空间位置变换关系，通过非线性优化方法，精确估计机器人的位姿变化量，包括以下步骤：

根据数据校验算法计算得到的机器人位姿估计机器人运动初值M0和全部视觉测量单元中匹配和跟踪的特征点，计算逆投影误差，与设定的误差阈值进行比较，滤除可能存在的无效数据；

利用计算得到的有效数据，根据加权最小二乘法重新计算机器人位姿估计初值，并将其作为机器人位姿优化估计的初始值；

根据稀疏LM算法计算机器人的位姿变化，通过位姿推算算法计算机器人的当前位姿。