相机位姿估计方法

技术领域

本发明涉及机器人导航与定位技术领域，更具体地说，本发明涉及一种 相机位姿估计方法。

背景技术

机器人的导航定位一般由雷达、视觉、惯导等单独或融合技术完成，视 觉也分为单目，双目及深度相机等多种图像获取方式，

惯性相对位姿测量方法将惯性传感器如加速度计和陀螺仪直接固连在被 测物体和运动参考系上，也可以将多种惯性传感器组合成惯性测量单元后再 进行安装。视觉位姿测量方法通常是从拍摄的图像中提取特征点信息，之后 计算得到目标物相对于参考坐标系的位姿信息。

但是视觉测量量容易受遮挡物干扰、环境光线干扰，目标物和参考系同 时运动时靶标更容易移出摄像机视场，导致拍摄画面无明显变化，例如受强 烈光照影响，或面对一片纯色区域时，没有特征点；运动速度过快时，相邻 图像里边搜索的范围可能会很大，在固定较短时间间隔下，两帧图像可能差 别过大，这些都无法满足对机器人本体的快速、准确、实时定位。为了保证 机器人速度里程计的精确性，迫切需要一种高精度的相机位姿估计方法。

发明内容

针对上述技术中存在的不足之处，本发明提供一种相机位姿估计方法， 通过深度相机与惯性测量融合协同进行数据采集和匹配来对机器人本体的姿 态进行估计，解决了机器人速度里程计的设计精度不高的问题，用于提升机 器人速度里程计的设计精度。

为了实现根据本发明的这些目的和其它优点，本发明通过以下技术方案 实现：

本发明所述的相机位姿估计方法，其包括以下步骤：

数据采集：深度相机采集图像帧数据流，以第一分辨率获取包含RGB图 像和深度图像的图像数据帧；

数据处理：从所述图像数据帧中提取ORB特征，获得具有方向向量的 FAST角点和包含若干维二进制向量的BRIEF描述子；搭建加速度与陀螺仪 的角速度测量模型，获得惯性测量单元数据的方向向量；

数据融合：保留与所述惯性测量单元数据方向向量方向一致的FAST角 点，对两幅图像的BRIEF描述子进行匹配，匹配成功，从深度相机中获得所 述FAST角点的深度数据；

位姿预估：根据匹配成功的所述FAST角点的深度数据，采用李代数表 示位姿，获得目标函数，使用李代数扰动模型对所述目标函数求导，找到局 部极小值作为位姿预估值。

优选的是，获得具有方向向量的FAST角点，包括以下步骤：

一次降采样：对第一分辨率的图像数据帧进行一次降采样处理，获得第 二分辨率的图像数据帧；

提取FAST角点：通过像素亮度比较，从第二分辨率的图像数据帧中提 取出FAST角点，加入特征点数组；

获得FAST角点的方向向量：判断获取的FAST角点是否在特征点数组 中：若在，保留该FAST角点的特征值，对该FAST角点进行旋转获得其方 向向量；若不在，删除该FAST角点的特征值；

二次降采样：对保留特征值的FAST角点的图像数据帧进行二次降采样 处理，并循环至提取FAST角点的步骤。

优选的是，提取FAST角点，包括以下步骤：

从第二分辨率的图像数据帧中提取亮度为Ai的目标像素，并设置亮度阈 值为T；

以目标像素为原点取一半径圆周上的若干个像素点并顺时针编号；

检测顺时针编号的若干个像素点的亮度来判断所述目标像素是否为 FAST角点：若满足3个及以上亮度同时大于Ai+T或大于Ai-T，并且连续 12个像素点亮度同时大于Ai+T或小于Ai-T，则将该目标像素确定为FAST 角点，加入特征点数组；

若不满足三个及以上亮度同时大于Ai+T或Ai-T，或者不满足连续12个 像素点亮度同时大于Ai+T或小于Ai-T，或者确定FAST角点并加入特征数 组后，将下一像素设为目标像素。

优选的是，获得包含若干维二进制向量的BRIEF描述子，包括以下步骤：

对一FAST角点附近的一对像素进行大小比较，若大则取1，否则取0， 形成由0和1组成的一维二进制向量的BRIEF描述子；

对一所述FAST角点附近的其他若干对像素进行大小比较，形成包含若 干维二进制向量的BRIEF描述子。

优选的是，对两幅图像的BRIEF描述子进行匹配，采用快速近似最近邻 算法，包括以下步骤：

检测FAST角点位置，保留与惯性测量单元运动方向一致的FAST角点 位置；

计算BRIEF描述子；

对两幅图像的BRIEF描述子进行汉明距离匹配；

找出最大距离和最小距离，判断两幅图像的BRIEF描述子之间的距离与 最大距离和最小距离之间的关系：若两幅图像的BRIEF描述子之间的距离小 于0.3倍最大距离，则匹配成功，若两幅图像的BRIEF描述子之间的距离大 于2倍最小距离且大于0.3倍最大距离，匹配有误。

优选的是，还包括匹配有误导致视觉缺失的处理步骤：

若对两幅图像的BRIEF描述子的匹配有误，切换惯性测量单元运动方程 中的传感器相对惯性系的速度的导数与从传感器到惯性系的四元数的导数 项，以一步长为单位，计算FAST角点的方向向量与惯性测量单元的运动加 速度向量，作为视觉估计缺失时的补充代替。

优选的是，还包括采用最小二乘法对位姿预估值进行优化的步骤：

获取估计初值，初始化半径μ；

求解最小二乘问题；

计算近似模型与实际函数的差异ρ，通过一次判断ρ大小获取μ值：若 ρ＞3/4，则μ＝18μ；若ρ<1/4，则μ＝0.5μ；

获得μ值后，二次判断ρ大小：若ρ＞0.95，令x

本发明至少包括以下有益效果：

1.本发明提供的相机位姿估计方法，通过深度相机与惯性测量融合协同 进行数据采集和匹配来对机器人本体的姿态进行估计，可以减小图像匹配的 搜索范围，缩短匹配时间，解决了机器人速度里程计的设计精度不高的问题， 用于提升机器人速度里程计的设计精度。

2.本发明切换惯性测量单元运动方程中的传感器相对惯性系的速度的导 数与从传感器到惯性系的四元数的导数项，以一步长为单位，计算FAST角 点的方向向量与惯性测量单元的运动加速度向量，作为视觉估计缺失时的补 充代替，可以在很少或没有特征点时持续跟踪，在没有特征点，比如受强烈 光照影响，或面对一片纯色区域时，也能够基本正确地跟踪数十秒的时间；

3.本发明采用最小二乘法对位姿预估值进行优化，提高预估精度。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将 通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1-1为本发明所述的相机位姿估计方法的流程图；

图1-2为本发明所述的相机成像模型示意图；

图2-1为本发明所述的深度相机采集的RGB图像示例；

图2-2为本发明所述的深度相机采集的深度图像示例；

图3-1为本发明所述的获得具有方向向量的FAST角点的流程图；

图3-2为本发明所述的获得具有方向向量的FAST角点的示例图；

图3-3为本发明所述的提取的FAST角点的示意图；

图4-1为本发明所述的对两幅图像的BRIEF描述子进行匹配的流程图；

图4-2为本发明所述的特征点图；

图4-3为本发明所述的特征点匹配示意图；

图4-4为本发明所述的融合了深度数据的匹配图片；

图5为本发明所述的采用最小二乘法对位姿预估值进行优化的示例。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照 说明书文字能够据以实施。

应当理解，本文所使用的诸如“具有”、“包含”以及“包括”术语并不排除一 个或多个其它元件或其组合的存在或添加。

<实施例1>

如图1-1所示，本发明提供一种相机位姿估计方法，其运用在机器人导 航与定位技术领域。相机位姿估计方法包括以下步骤，如图1所示：

S10.数据采集：深度相机采集图像帧数据流，以第一分辨率获取包含RGB 图像和深度图像的图像数据帧；

S20.数据处理：从图像数据帧中提取ORB特征，获得具有方向向量的 FAST角点和包含若干维二进制向量的BRIEF描述子；搭建加速度与陀螺仪 的角速度测量模型，获得惯性测量单元数据的方向向量；

S30.数据融合：保留与惯性测量单元数据方向向量方向一致的FAST角 点，对两幅图像的BRIEF描述子进行匹配，匹配成功，从深度相机中获得FAST 角点的深度数据；

S40.位姿预估：根据匹配成功的FAST角点的深度数据，采用李代数表示 位姿，获得目标函数，使用李代数扰动模型对目标函数求导，找到局部极小 值作为位姿预估值。

上述实施方式中，通过提取图像的ORB(Oriented FAST and Rotated BRIEF)特征改进了传统的FAST检测子不具有方向性的问题，且采用速度较 快的二进制描述子BRIEF，使得图像的ORB特征提取速度加快。运动速度过 快的时候，相邻图像里边搜索的范围可能会很大，在固定较短时间间隔下， 两帧图像可能差别过大，设置惯性测量单元，保留与惯性测量单元数据方向 向量方向一致的FAST角点，对两幅图像的BRIEF描述子进行匹配，筛选出 大致位置，进而缩小匹配范围，缩短匹配时间，提高效率。具体地，图像帧 数据流可采用RGB-D相机进行采集，在图像帧数据流中以1280*720的第一 分辨率获取同时包含RGB图像和深度图像的图像数据帧，RGB图像示例如 图2-1所示，深度图像示例如图2-2所示。因为相机摄像头的采集速率一般比 较低，而惯性测量单元可以达到每秒1000帧或者更高，加到图像帧间预测， 就可以以每秒1000帧的速率输出位置，而不是只在有图像的时间输出位置，因此，深度相机采集图像帧数据流与惯性测量单元的运动数据融合，可以在 图像帧间预测位置，提高位姿的估计精度。

本发明实施例提供的相机位姿估计方法，通过深度相机与惯性测量融合 协同进行数据采集和匹配来对机器人本体的姿态进行估计，可以减小图像匹 配的搜索范围，缩短匹配时间，解决了机器人速度里程计的设计精度不高的 问题，用于提升机器人速度里程计的设计精度。

作为上述实施方式的具体说明，步骤S40中位姿预估具体如下：

通用的相机运动方程为：X

结合通用相机的运动方程，惯性测量单元的运动方程如下：

其中，距离对时间的导数是速度，速度对时间的导数是加速度。P

通用的相机观测方程为：Z

其中，Z

根据如图1-2所示的相机成像的空间关系，可将问题表述为：

sz

其中，K是相机内参，S为相机在K处，对j点观测到的像素距离，exp() 为李代数表示的相机位姿，y为j点的世界坐标。

如图1-2中，假设相机在空间中从A点运动到B点，空间中的P点在两 个位置所获取的图像中均被捕获，出现在图像的不同位置。

用李代数表示位姿，目标函数可以写成：

进一步使用李代数扰动模型求导：

在唯一解的情况下，找到局部极小值，即是全局最优值，作为初步估计 结果。

<实施例2>

在实施例1的基础上，本发明实施例主要对获取FAST角点进行说明。 获得具有方向向量的FAST角点，如图3-1所示，包括以下步骤：

S21.一次降采样：对第一分辨率的图像数据帧进行一次降采样处理，获 得第二分辨率的图像数据帧；

S22.提取FAST角点：通过像素亮度比较，从第二分辨率的图像数据帧中 提取出FAST角点，加入特征点数组；

S23.获得FAST角点的方向向量：判断获取的FAST角点是否在特征点数 组中：若在，保留该FAST角点的特征值，对该FAST角点进行旋转获得其 方向向量；若不在，删除该FAST角点的特征值；

S24.二次降采样：对保留特征值的FAST角点的图像数据帧进行二次降采 样处理，并循环至提取FAST角点的步骤。

上述事实方式中，对图像数据帧依次进行步骤S21的一次降采样和步骤 S24的二次降采样的多次采样，是为了不管距离目标远近，都能保持尺度上 的一致，有些图片放大之后原来的角点可能就不再是角点。在第一分辨率优 选为1280*720的基础上，一次降采样获得的第二分辨率优选为480*360，二 次降采样获得分辨率优选为240*180，可有效短时间内快速获取精确的搜索 范围。步骤S22提供了一种通过像素亮度比较来获取FAST角点的方法。步 骤S23中保留FAST角点特征值的同时通过旋转获得特征点的方向向量，作 为优选，可通过灰度质心法对FAST角点进行旋转以获得所有特征点的方向 向量。步骤S23中，FAST角点的特征值提取，使用高成本的GPU作为上位 机计算资源时，可用SIFT算法进行。

<实施例3>

在实施例2的基础上，本发明实施例主要给出步骤S22中提取FAST角 点的一种示例。如图3-2所示，提取FAST角点，包括以下步骤：

从第二分辨率的图像数据帧中提取亮度为Ai的目标像素，并设置亮度阈 值为T；

以目标像素为原点取一半径圆周上的若干个像素点并顺时针编号；

检测顺时针编号的若干个像素点的亮度来判断目标像素是否为FAST角 点：若满足3个及以上亮度同时大于Ai+T或大于Ai-T，并且连续12个像素 点亮度同时大于Ai+T或小于Ai-T，则将该目标像素确定为FAST角点，加 入特征点数组；

若不满足三个及以上亮度同时大于Ai+T或Ai-T，或者不满足连续12个 像素点亮度同时大于Ai+T或小于Ai-T，或者确定FAST角点并加入特征数 组后，将下一像素设为目标像素。

该实施方式中，作为优选，为了有效提取FAST角点，亮度阈值T大小 设置为0.3*Ai，取半径为3的圆周上的16个像素点并顺时针编号，并在16 个像素点中，初始重点筛选圆周上1.5.9.13四个像素的亮度进行比较，若3 个及以上亮度同时大于Ai+T或大于Ai-T，进一步地，再连续12个像素点的 亮度同时大于Ai+T或大于Ai-T，即可确定像素A为FAST角点，加入数组 A[]中，提取的FAST角点如图3-3所示；否则，将下一个像素设为A。通过 本实施方式，可快速、精确提取FAST角点。

<实施例4>

在实施例3的基础上，本发明实施例主要给出步骤S20中BRIEF描述子 的获取方法。获得包含若干维二进制向量的BRIEF描述子，包括以下步骤：

对一FAST角点附近的一对像素进行大小比较，若大则取1，否则取0， 形成由0和1组成的一维二进制向量的BRIEF描述子；

对一FAST角点附近的其他若干对像素进行大小比较，形成包含若干维 二进制向量的BRIEF描述子。

该实施方式中，BRIEF描述子全部是由0和1组成的二进制向量，0，1 表示FAST角点附近两个像素的大小关系，大则取1，否则取0。为了获得多 维向量，本发明实施方式优选对FAST角点附近的512对像素进行大小比较， 获得512维的二进制向量，形成本发明所需的BRIEF描述子。至于FAST角 点附近的像素选取，本发明优选采用泊松分布来挑选位置，因为FAST角点 的提取过程保留了方向与旋转，因此BRIEF描述子在图像旋转时也不会丢失。

<实施例5>

在实施例4的基础上，本发明实施例主要给出步骤S30中对两幅图像的 BRIEF描述子进行匹配的方法。

惯性测量单元作为运动方程的输入，加速度与陀螺仪角速度测量模型如 下：

a

其中，a

本发明实施方式的匹配方法选择快速近似最近邻算法，即根据惯性测量 单元给出的运动数据(包括加速度与陀螺仪角速度)所表示的方向向量，采 用快速近似最近邻算法使得ORB特征点在匹配时与惯性测量单元方向结合。 具体地，如图4-1所示，包括以下步骤：

检测FAST角点位置，保留与惯性测量单元运动方向一致的FAST角点 位置；

计算BRIEF描述子；

对两幅图像的BRIEF描述子进行汉明距离匹配；

找出最大距离和最小距离，判断两幅图像的BRIEF描述子之间的距离与 最大距离和最小距离之间的关系：若两幅图像的BRIEF描述子之间的距离小 于0.3倍最大距离，则匹配成功，若两幅图像的BRIEF描述子之间的距离大 于2倍最小距离且大于0.3倍最大距离，匹配有误。

以图4-2所示的本发明获取的特征点图为例，通过上述实施方式提供的 快速近似最近邻算法对两幅图像的BRIEF描述子进行匹配，匹配成功后如图 4-3所示。匹配成功后，再从RGB-D相机读取该点的深度数据，融合了深度 数据的匹配图像如图4-4所示。

<实施例6>

在实施例5的基础上，若对两幅图像的BRIEF描述子的匹配有误，例如 匹配不到深度数据，甚至极端情况下无关键点匹配时，本实施方式提供一种 匹配有误导致视觉缺失的处理方法。

即切换惯性测量单元运动方程中的传感器相对惯性系的速度Q

上述实施方式提供的匹配有误导致视觉缺失的处理方法，可以在很少或 没有特征点时持续跟踪。在没有特征点，比如受强烈光照影响，或面对一片 纯色区域时，也能够基本正确地跟踪数十秒的时间。

<实施例7>

在实施例6的基础上，本发明实施例主要给出采用最小二乘法对位姿预 估值进行优化的示例。

根据通用相机的运动方程和观测方程可知，对姿态估计问题转化为在什 么样的情况下最有可能产生现在的观测数据，条件概率表示如下：

设噪声满足高斯分布,那么：P(z

对其求负对数就相当于求最小化噪声项的平方，也就是最小二乘问题。

由于高斯分布在负对数下有较好的数学形式，推导出数据与估计值之间 的误差平方和为：

最优解即是状态的最大似然估计。

接下来需要确定一个可信区域，并在区域内寻找最优值，因此可选用列 文伯格-马夸尔特方法。

近似模型与实际函数的差异使用下式判断：

第k次迭代的优化半径为μ，最小二乘解法表示为：

具体的优化步骤如图5所示：

获取估计初值X()，初始化半径μ；

求解最小二乘问题；

计算ρ，通过一次判断ρ大小获取μ值：若ρ＞3/4，则μ＝18μ；若ρ<1/4， 则μ＝0.5μ；

获得μ值后，二次判断ρ大小：若ρ＞0.95，令x

通过本实施方式中最小二乘法对位姿预估值的优化，可以进一步提升机 器人速度里程计的设计精度。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方 式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领 域的人员而言可容易地实现另外的修改。因此在不背离权利要求及等同范围 所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。