用于估计运动的方法、移动代理以及编码有用于使得处理器执行用于估计运动的方法的计算机程序代码的非暂时性计算机可读介质与流程

本发明的实施例总体上涉及用于运动估计的方法和系统，具体地，涉及用于对自身运动(ego-motion)进行基于视觉的计算的方法和系统。

背景技术：

对车辆相对于车辆行驶的表面路径的自身运动进行精确估计可以是用于自动驾驶和基于计算机视觉的驾驶辅助的关键要素。与其他传感器相比，使用一个或更多个摄像机来计算自身运动可以允许将自身运动数据集成到其他基于视觉的算法(例如，障碍物检测和/或回避、行人检测和/或回避、物体检测和/或回避以及其他基于视觉的算法)中，而不需要在传感器之间进行校准。这可以减少维护要求和成本。仅使用来自被附接到代理(agent)(例如，车辆、人、机器人和其他移动代理)的一个或更多个摄像机的输入来估计该代理的自身运动的过程被称为视觉里程计(vo)。

在vo中，通过检查由于移动代理的运动引起的在由一个或更多个机载摄像机获得的图像上的变化，来增量式地估计移动代理的姿态。为了使vo有效地工作，可能需要充分的环境照明和具有足够纹理的静态场景，以允许提取明显的运动。此外，应当捕获时间连续的帧以确保充分的场景交叠。

用于提供运动估计的vo的一个优点在于：vo不受不平坦地形和其他不利条件下的车轮滑移的影响。此外，vo可以向其他运动估计过程和系统(例如，全球定位系统(gps)、惯性测量单元(imu)、激光里程计和提供运动估计的其他系统)提供重要的补充信息。此外，在拒绝gps的环境(例如，水下、空中和可能拒绝gps的其他环境)以及gps信息由于例如多径、卫星覆盖差和其他可靠性因素而不可靠的环境中，可能增加vo的重要性。

使用专门的视频输入来估计运动的许多运动估计算法假定了静态场景。此外，使用专门的视频输入来估计运动的许多运动估计算法不能处理动态和/或混乱的环境或由过往车辆产生的较大遮挡。此外，运动估计中的特征匹配和异常值去除可能不是鲁棒的，且可能随后失败。许多运动估计方案需要大量的关键点，且在缺少结构的场景中可用的关键点数量有限的情况下，这些运动估计方案可能失败。

不依赖于上述假设并克服上述限制的实时vo方法和系统可能是期望的。

技术实现要素：

本发明的一些实施例包括用于使用摄像机来估计自身运动的方法和系统。

根据本发明的第一方面，精确校准的摄像机可以相对于移动代理和地平面被刚性安装在移动代理上。通过假定移动代理行驶在主要为平面状的地平面上，从而在移动代理行驶期间的每个时刻保持固定的摄像机与地面关系，可以将移动代理在任何时刻的运动的参数表示从六个自由度简化到三个自由度。

根据本发明的第二方面，可以从摄像机接收输入图像。根据本发明的第三方面，可以对接收的输入图像执行特征检测。根据本发明的第四方面，可以计算针对每个检测到的特征的特征描述符。根据本发明的第五方面，可以在针对接收的输入图像计算出的特征描述符和针对先前接收的图像计算出的特征描述符之间执行特征匹配。根据本发明的第六方面，可以将与特征对应关系相关联的关键点从图像坐标投影到世界坐标。根据本发明的第七方面，可以根据与特征对应关系相关联的世界坐标对来计算运动估计。根据本发明的第八方面，当检测到关键姿态(keypose)时，可以产生全局坐标系下的当前摄像机姿态。根据本发明的第九方面，可以根据当前摄像机姿态确定运动轨迹。根据本发明的第十方面，可以将针对先前接收的图像计算出的特征描述符更新为针对接收的输入图像计算出的特征描述符。

在结合附图考虑本发明的以下详细描述时，本发明的以上和其它目的、特征和优点将更易于理解。

附图说明

图1是示出了本发明的示例性实施例的图，其包括：对接收的输入图像执行特征检测；计算每个检测到的特征的特征描述符；在针对接收的输入图像计算出的特征描述和针对先前接收的图像计算出的特征描述符之间执行特征匹配；将与特征对应关系相关联的关键点从图像坐标投影到世界坐标；根据与所述特征对应关系相关联的世界坐标对计算运动估计；当检测到关键姿态时，在全局坐标系下产生当前摄像机姿态；根据当前摄像机姿态确定运动轨迹；以及将针对先前接收的图像计算出的特征描述符更新为针对接收的输入图像计算出的特征描述符；

图2是示出了与本发明的实施例相关的多个坐标系的图片；

图3是示出了根据本发明示例性实施例的移动代理平台上的示例性摄像机配置的图片；

图4是示出了包括基于绝对取向(基于ao)的运动估计的本发明的示例性实施例的图表；以及

图5是示出了包括基于正交procrustes分析的运动估计的本发明的示例性实施例的图表。

具体实施方式

通过参考附图将最佳理解本发明的实施例，其中贯穿附图，类似的部件由类似的附图标记表示。以上所列的附图被明确地并入作为该详细描述的一部分。

将容易理解的是可以以各种不同配置来布置和设计在本文附图中概括描述和示出的本发明的部件。因此，以下对本发明的方法、系统和装置的实施例的更详细描述并不旨在限制本发明的范围，而是仅代表本发明的当前优选实施例。

本发明的实施例的元件可以以硬件、固件和/或包括计算机可读存储介质的非暂时性计算机程序产品来实现，其中所述计算机可读存储介质将可以用于对计算系统进行编程的指令存储在其上/其中。虽然本文公开的示例性实施例仅可以描述这些形式中的一种，但是应当理解，本领域技术人员将能够以这些形式中的任何形式实现这些元件，同时保持在本发明的范围内。

尽管附图中的图表和图可以示出特定执行顺序，但是应当理解，所述执行顺序可以不同于所描绘的顺序。例如，可以相对于所示顺序改变所述块的执行顺序。此外，作为另一示例，可以同时或部分同时地执行图中连续示出的两个或更多个块。本领域普通技术人员应当理解，本领域的普通技术人员可以创建包括计算机可读存储介质的非暂时性计算机程序产品以执行本文所描述的各种逻辑功能，其中所述计算机可读存储介质将可以用于对计算系统、硬件和/或固件进行编程的指令存储在其上/其中。

对车辆相对于车辆行驶的表面路径的自身运动进行的精确估计可以是用于自动驾驶和基于计算机视觉的驾驶辅助的关键要素。与其他传感器相比，使用一个或更多个摄像机来计算自身运动可以允许将自身运动数据集成到其他基于视觉的算法(例如，障碍物检测和/或回避、行人检测和/或回避、物体检测和/或回避以及其他基于视觉的算法)中，而不需要在传感器之间进行校准。这可以减少维护要求和成本。仅使用来自被附接到代理(例如，车辆、人、机器人和其他移动代理)的一个或更多个摄像机的输入来估计该代理的自身运动的过程被称为视觉里程计(vo)。

在vo中，通过检查由于移动代理的运动引起的在由一个或更多个机载摄像机获得的图像上的变化，来增量式地估计移动代理的姿态。为了使vo有效地工作，可能需要充分的环境照明和具有足够纹理的静态场景，以允许提取明显的运动。此外，应当捕获时间连续的帧以确保充分的场景交叠。

用于提供运动估计的vo的一个优点在于：vo不受不平坦地形和其他不利条件下的车轮滑移的影响。此外，vo可以向其他运动估计过程和系统(例如，全球定位系统(gps)、惯性测量单元(imu)、激光里程计和提供运动估计的其他系统)提供重要的补充信息。此外，在拒绝gps的环境(例如，水下、空中和可能拒绝gps的其他环境)以及gps信息由于例如多径、卫星覆盖差和其他可靠性因素而不可靠的环境中，可能增加vo的重要性。

使用专门的视频输入来估计运动的许多运动估计算法假定了静态场景。此外，使用专门的视频输入来估计运动的许多运动估计算法不能处理动态和/或混乱的环境或由过往车辆产生的较大遮挡。此外，运动估计中的特征匹配和异常值去除可能不是鲁棒的，且可能随后失败。许多运动估计方案需要大量的关键点，且在缺少结构的场景中可用的关键点数量有限的情况下，这些运动估计方案可能失败。

不依赖于上述假设并克服上述限制的实时vo方法和系统可能是期望的。

结合图1描述的本发明的一些实施例可以包括用于视觉里程计的方法100。可以在处理器组件中接收102输入图像(也被称为输入图像帧和输入帧)。在本发明的一些实施例中，可以直接从图像获取组件接收输入图像。在备选实施例中，可以间接从图像获取组件接收输入图像。在本发明的一些实施例中，图像获取组件和处理器组件可以都布置在刚性附接到移动代理的摄像机中。在本发明的备选实施例中，图像获取组件可以布置在刚性附接到移动代理的摄像机中，而处理器组件可以布置在移动代理上，但不与被刚性附接的摄像机集成在一起。在本发明的一些实施例中，输入图像可以是彩色图像。在本发明的备选实施例中，输入图像可以是灰度图像。在本发明的一些实施例中，输入图像可以是根据获取的彩色图像产生的灰度图像。在这些实施例的一些实施例中，可以在图像获取组件内产生输入灰度图像。在这些实施例的备选实施例中，可以在图像获取组件的外部根据所获取的彩色图像产生输入灰度图像。

可以对接收的输入图像执行104特征检测，以识别在接收的输入图像中的表现出明显强度变化的一个或更多个区域(例如，边缘、拐角、斑点、尖峰和表现出明显强度变化的其它空间特征)。一个或更多个识别出的区域中的每一个的中心(或基本上的中心)可以被指定为关键点，从而产生与接收的输入图像相关联的一个或更多个关键点。执行特征检测可以包括：向接收的输入图像应用本领域已知的一个或更多个关键点特征检测器，例如，拐角检测器、斑点检测器、尖峰检测器、尺度不变特征变换(sift)关键点检测器、加速鲁棒特征(surf)关键点检测器、加速分割检测特征(fast)关键点检测器、取向fast和旋转brief(orb)关键点检测器、最大稳定极值区域(mser)关键点检测器、二进制鲁棒独立基本特征(brief)关键点检测器、二进制鲁棒不变可调关键点(brisk)关键点检测器、快速retina关键点(freak)关键点检测器、star关键点检测器和其他关键点特征检测器。在本发明的一些实施例中，由于fast关键点检测器具有较快的检测速度使其适合于实时自主导航应用，因此可以使用fast关键点检测器。

在本发明的一些实施例中，可以使用基于网格的fast特征检测器来执行特征检测104。在这些实施例中，接收的输入图像可以被划分为“m×n”网格。在本发明的一些实施例中，“m”和“n”可以是不同的值。在备选实施例中，“m”的值和“n”的值可以是相同的。可以将fast关键点检测器应用于每个网格单元。在本发明的一些实施例中，可以针对图像中检测到的关键点的最大计数(maximumcount)指定最大关键点计数门限值。最大关键点计数门限值的值可以用于调整针对每个输入图像帧的处理速度。在本发明的一些实施例中，可以基于输入图像的期望处理速度来选择网格单元的数量。

可以计算106用于表征每个指定关键点周围的区域的特征描述符。在本发明的一些实施例中，与区域相关联的特征描述符可以是区域的外观，例如，像素强度或颜色值。在本发明的一些实施例中，与区域相关联的特征描述符可以基于区域的外观，例如，原始像素值可以通过像素值、像素值的能量或根据原始像素值计算的一些其他统计量的平均值来归一化。在备选实施例中，可以通过像素值的平均值来调整原始像素值，例如，可以从原始像素值中减去平均像素值。在本发明的一些实施例中，特征描述符可以是区域上的空间梯度。在本发明的一些实施例中，与区域相关联的特征描述符可以对于视点变化、旋转和尺度变化中的一个或更多个是鲁棒的。示例性描述符包括sift特征描述符、surf特征描述符、orb特征描述符、brief特征描述符、brisk特征描述符、freak特征描述符以及其它视点不变、旋转不变或尺度不变的特征描述符。在本发明的一些实施例中，特征描述符可以是紧凑型特征描述符。以上所列的特征描述符旨在用于说明而不是限制的目的。

可以在根据接收的输入帧计算出的特征描述符以及根据前一帧计算出的特征描述符之间执行108特征匹配。接收的输入帧中的每个关键点可以与来自前一帧的关键点相关联，从而产生多个特征对应关系，其中每个特征对应关系将接收的输入帧中的关键点与前一帧中的关键点相关联。

在本发明的一些实施例中，针对接收的输入帧计算出的每个特征描述符可以与针对前一帧计算出的每个特征描述符进行穷举比较。输入帧特征描述符与前一帧特征描述符的比较可以包括计算相似性度量。在特征描述符是特征外观的本发明的一些实施例中，相似性度量可以是平方差的和。在特征描述符是特征外观的本发明的备选实施例中，相似性度量可以是归一化互相关。

在本发明的一些实施例中，执行特征匹配108可以包括利用“k”维树的快速最近邻搜索。在特征描述符是诸如brief描述符的二进制描述符的本发明的一些实施例中，可以使用汉明距离来测量相似性。在可以使用其他非二进制特征描述符的备选实施例中，可以将数学范数(例如，l2范数、l1范数和由范数限定的其他度量)用于相似性度量。

在本发明的一些实施例中，可以修剪特征匹配，以仅保留足够好的匹配用于计算运动变换。在本发明的一些实施例中，可以在最好的两个匹配之间执行距离比率测试。可以计算最接近的匹配和第二接近的匹配之间的比率。如果比率小于用户指定的门限值，则可以接受最接近的匹配。在本发明的一些实施例中，用户指定的门限值可以等于0.65。在用距离比率测试执行匹配之后，可以执行相互一致性检查以消除在接收的输入图像中的与前一图像中的多于一个特征相匹配的特征。相互一致性检查消除了不相互优选的所有匹配。

可以将与每个特征对应关系对相关联的关键点的坐标从图像坐标系投影110到世界坐标系(也被称为移动代理坐标系)。

图2示出了与本发明的实施例相关的多个坐标系。图2分别描绘了在两个时刻(t＝k和t＝k+1)下的具有单个机载刚性安装的摄像机204、206的移动代理200、202。可以将具有表示为xw、yw和zw的坐标轴的全局世界坐标系208表示为w。当移动代理200、202沿着平面状地平面210路径行驶时，全局世界坐标系208(w)是静止的。可以假定平面状地平面210位于z＝0的平面中。全局世界坐标系208(w)的原点是路径的起始位置。可以将摄像机坐标系212表示为c，其中坐标轴被表示为xc、yc和zc；可以将图像坐标系214表示为i，其中坐标轴被表示为xi、yi和zi；且可以将移动代理坐标系216表示为m，其中坐标轴被表示为xm、ym和zm。移动代理坐标系216也可以被称为世界坐标系。摄像机坐标系212(c)和移动代理坐标系216(m)二者可以随着移动代理200、202的移动而移动。

由于摄像机204、206被刚性安装在移动代理200、202上，摄像机坐标系212(c)与移动代理坐标系216(m)具有固定关系。在移动代理坐标系216(m)下与可以表示为p的点相对应的坐标向量xxm可以根据下式与p点在摄像机坐标系212(c)下的坐标向量xxc相关：

xxc＝rc*xxm+tc，

其中rc可以表示从移动代理坐标系216(m)到摄像机坐标系212(c)的旋转矩阵，且tc可以表示平移向量，所述平移向量是在移动代理坐标系216(m)的原点在摄像机坐标系212(c)下的坐标向量。rc和tc是外部摄像机参数，且可以通过摄像机校准获得。点p可以由摄像机投影到图像平面上。将投影图像点表示为具有图像坐标系坐标向量xxi的p，摄像机坐标向量xxc可以根据下式与xxi相关：

l*xxi＝k*xxc，

其中k可以表示摄像机的内在矩阵，且l可以表示对由于从三维到二维的投影而导致的深度丢失加以表示的比例因子。

可以通过本领域中公知技术来估计内在(intrinsic)的和外部(extrinsic)的摄像机参数。

在本发明的一些实施例中，可以获得内在参数并将内在参数永久地存储在移动代理中。在本发明的一些实施例中，可以在移动代理运行时执行对外部摄像机参数的重新校准。可以捕获地面上的校准图案。可以从校准图案的图像提取拐角点，并可以计算外部摄像机参数。当需要重新校准时，这些新计算的外部摄像机参数和所存储的内部摄像机参数可以用于更新摄像机参数。

对于在图像坐标表示为[x，y]^t的图像上的关键点，可以根据下式来计算移动代理坐标系(也被称为世界坐标系)下的对应点：

【式1】

其中k可以表示摄像机内在参数，rc和tc可以表示摄像机外部参数，[x，y，z]^t可以表示移动代理坐标系下的坐标，且1可以表示该坐标在移动代理坐标系下的未知比例因子。因此，可以根据下式来计算移动代理坐标：

【式2】

假设移动代理主要在地平面上行驶，且摄像机只看地平面，则世界坐标系下的关键点的坐标必须满足地平面方程。任何平面的一般形式可以表示为：

ax+by+cz+d＝0

并可以写成：

【式3】

因此：

【式4】

以及

【式5】

假设地面位于z＝0的平面上，则d＝0，地平面的法向向量[abc]^t＝[001]^t，且上述等式中的所有变量是已知的。因此，可以针对关键点的每个坐标计算1值，且可以使用计算出的1值来计算移动代理坐标。

再次参考图1，可以使用针对特征对应关系对的关键点的世界坐标来计算112运动估计。运动估计可以包括最佳的刚性变换(可以被表示为tk，k-1)，其可以将前一帧中的与特征对应关系点相关联的每个关键点的三维坐标和当前帧中的对应关键点的三维坐标相对准。

将前一帧中的与特征对应关系点相关联的关键点的三维坐标集合表示为a，并将当前帧中的对应关键点的三维坐标集合表示为b，a和b之间的关系可以写成：

b＝tk，k-1a，

其中tk，k-1可以表示当前帧和前一帧之间的变换。由于摄像机运动是刚性运动，因此变换tk，k-1可以包括前一时刻和当前时刻之间的刚性旋转以及前一时刻和当前时刻之间的平移，其中可以将所述刚性旋转表示为rk，k-1，且可以将所述平移表示为tk，k-1。因此，

【式6】

在本发明的一些实施例中，可以结合图3描述在移动代理平台上的摄像机配置。精确校准的摄像机300可以相对于移动代理302和地平面304刚性安装。在本发明的一些实施例中，摄像机300可以安装在移动代理302的前端，在主体底盘上方。摄像机300可以向地面倾斜以提供在移动代理302正前方的地面的良好视图。在本发明的备选实施例(未示出)中，摄像机可以安装在移动代理的后端，位于主体底盘上方。摄像机可以向地面倾斜以提供在移动代理正后方的地面的良好视图。

可以假设地平面304之上的高度306(可以表示为h)以及在地平面304的法线310和摄像机300视线312之间的角度308(可以表示为θ)是恒定的。此外，可以假设当移动代理302在地面上移动时，角度308(θ)可以不受较大振动的影响。相对于地面的外部摄像机参数表征h和θ值，以及摄像机坐标系相对于移动代理坐标系的旋转和平移。

此外，假设移动代理302在主要为平面状的地平面304上行驶，于是在移动代理行驶期间的每个时刻具有固定的摄像机对地面的关系的假设将保持有效。

上述假设允许将对移动代理在任意时刻的运动的参数表示从六个自由度简化到三个自由度：x和y方向上的平移，其可以表示为[xk，k-1，yk，k-1]；以及相对于全局世界坐标系的旋转，其可以表示为θk，k-1。

因此，利用帧间变换，可以将tk，k-1写为：

【式7】

可以结合图4描述包括用于估计刚性旋转rk，k-1和平移tk，k-1的基于绝对取向方法400的一些实施例。在这些示例性实施例中，可以通过使中心点之间的均方误差(mse)(可以表示为e)最小化来估计刚性旋转rk，k-1和平移tk，k-1，其中所述中心点对应于a和b中的配对点，根据下式将e表示为：

【式8】

其中n可以表示特征对应关系对的数量，且ai’和bi’可以表示根据下式计算的中心点：

【式9】

在这些示例性实施例中，可以分别根据下式来计算402每个点集(a和b)的质心：

【式10】

可以根据下式将每个点集带到404其原点：

【式11】

可以根据下式来计算406旋转角θk，k-1：

【式12】

以及

【式13】

其中

【式14】

以及

【式15】

其中·可以表示点积，×可以表示叉积，且

【式16】

可以表示旋转所在的平面的法线。

可以通过使用旋转角θk，k-1来根据下式构造408旋转矩阵rk，k-1：

【式17】

可以根据下式来计算410平移tk，k-1：

【式18】

可以结合图5描述包括用于估计刚性旋转rk，k-1和平移tk，k-1的基于正交procrustes分析的方法500的备选实施例。

在这些示例性实施例中，可以分别根据下式来计算502每个点集(a和b)的质心：

【式19】

可以根据下式将每个点集带到504其原点：

【式20】

通过使用奇异值分解(svd)来计算506旋转矩阵。首先，可以根据下式计算协方差矩阵，其中协方差矩阵可以表示为h：

【式21】

可以计算协方差矩阵(h)的奇异值分解，其中：

【式22】

svd(h)＝u∑v^t

可以根据下式来计算旋转矩阵rk，k-1：

【式23】

其中

d＝sign(det(vu^t))

且确保右手坐标系统。

可以根据下式来计算508平移tk，k-1：

【式24】

在本发明的一些实施例中，运动估计112可以包括异常值剔除，以去除由于例如图像噪声、遮挡、模糊、视点变化、照明变化和特征检测器的数学模型无法解释的其他原因引起的异常值。在本发明的一些实施例中，可以执行随机抽样一致性(ransac)迭代运动估计。ransac根据随机抽样的数据点集计算模型假设，然后在其他数据点上验证这些假设。将示出了与其他数据有最高一致性的假设选择作为解。在这些实施例中，估计模型是两个摄像机位置之间的相对运动rk，k-1和tk，k-1，且数据点是候选特征对应关系。在本发明的一些实施例中，可以根据以下伪码来执行运动估计：

【句法1】

其中在一些实施例中，迭代次数“n”被设置为50，要采样的对的数量“k”被设置为3，误差门限值t1在3-5mm的范围内，内点的数量门限值为t2＝5，且内点比率的门限值为t3＝0.55。

漂移可能随着误差累积变得严重。由于误差累积随着增加的运动估计的级联而增加，为了减小漂移，本发明的一些实施例可以不确定与每个运动估计相对应的摄像机姿态。因此，再次参考图1，可以确定118当前计算的运动估计是否应当用于估计摄像机姿态，从而识别关键姿态。在本发明的一些实施例中，如果当前运动估计的旋转角θk，k-1满足针对角度门限值的第一条件，或如果行驶距离满足针对距离门限值的第二条件，则可以认为当前摄像机姿势是关键姿态。在一些实施例中，当旋转角θk，k-1大于角度门限值或行驶距离大于距离门限值时，可以认为当前摄像机姿势是关键姿势。可以根据下式来计算行驶距离：

【式25】

||tk，k-1||²

如果当前摄像机姿态没被确定120为关键姿态，则可以通过等待接收102下一输入图像来继续所述方法100。

如果当前摄像机姿态122被确定为关键姿态，则可以根据下式生成当前摄像机姿态：

ck＝ck-1tk，k-1，

其中，ck-1表示在k-1时刻的摄像机姿态。在本发明的一些实施例中，可以将初始摄像机姿态设置为单位矩阵。当前摄像机姿态包括所有变换的级联tk，k-1(k＝1，...，n)，其中n是当前时刻。当前摄像机姿态ck具有以下形式：

【式26】

且可以根据关键姿态确定128运动参数[xk，yk，θk]，并存储所述运动参数。当前帧的描述符集合可以更新130先前帧的描述符集合。可以通过等待接收102下一输入帧来继续所述方法100。

本发明的一些实施例可以包括含有计算机可读存储介质的计算机程序产品，其中所述计算机可读存储介质将可以用于对计算系统进行编程以执行本文所述的任何特征和方法的指令存储在其上/其中。示例性计算机可读存储介质可以包括但不限于闪存设备、盘存储介质(例如，软盘、光盘、磁光盘、数字通用盘(dvd)、压缩盘(cd)、微驱动器和其它盘存储介质)、只读存储器(rom)、可编程只读存储器(prom)、可擦除可编程只读存储器(eproms)、电可擦除可编程只读存储器(eeprom)、随机访问存储器(rams)、视频随机访问存储器(vram)、动态随机访问存储器(dram)以及适于存储指令和/或数据的任何类型的介质或设备。

在上述说明书中采用的术语和表述在本文中用作描述性而不是限制性的术语，且在使用这些术语和表述时不排除所示和所描述的特征或其部分的等同物，应当认识到，本发明的范围仅由所附权利要求限定和限制。