CN113674340A - 一种基于路标点的双目视觉导航方法与装置 - Google Patents

Abstract

本发明实施例提供了一种基于路标点的双目视觉导航方法与装置，所述方法包括以下步骤：对双目相机所获取的图像进行预处理，包括矫正图像成像过程的畸变、提取图像的路标点作为特征点以及选取图像的关键帧并形成关键帧库；根据经过预处理的图像、所述图像的特征点以及所述图像的关键帧进行位姿估计；根据位姿估计的结果筛选所述关键帧库中的关键帧；利用筛选后的关键帧库中的数据进行位姿优化。本发明在动态场景中导航精度高于现有技术；采用结合动态路标点，处理方法简单、准确；本发明采用相机作为传感器，成本较低，使得专利易于实施。

Description

一种基于路标点的双目视觉导航方法与装置

技术领域

本发明涉及导航技术领域，尤其涉及一种基于路标点的双目视觉导航方法与装置。

背景技术

视觉导航是以相机为主要传感器，能够根据相机所采集数据，根据需求准确地在场景进行导航。当前视觉导航多基于图优化进行最大后验概率问题建模，通过采集世界坐标系下路标点位置坐标、相机位姿信息，并对相机坐标下路标点观测量和控制移动输入量进行表示，进而对导航问题进行建模。由于此类型方法基于环境不变的假设作为前提，在动态环境中的实现仍是一个难题。

申请人发现现有技术中至少存在如下问题：在动态环境中，视觉导航精度不高。

发明内容

本发明实施例所解决的技术问题是在动态环境中，如何提高导航精度的问题。

为达上述目的，一方面，本发明实施例提供了一种基于路标点的双目视觉导航方法，包括以下步骤：

对双目相机所获取的图像进行预处理，包括矫正图像成像过程的畸变、提取图像的路标点作为特征点以及选取图像的关键帧并形成关键帧库；

根据经过预处理的图像、所述图像的特征点以及所述图像的关键帧进行位姿估计；

根据位姿估计的结果筛选所述关键帧库中的关键帧；

利用筛选后的关键帧库中的数据进行位姿优化。

另一方面，本发明实施例提供了一种基于路标点的双目视觉导航装置，包括：

预处理单元，用于对双目相机所获取的图像进行预处理，包括矫正图像成像过程的畸变、提取图像的路标点作为特征点以及选取图像的关键帧并形成关键帧库；

估计单元，用于根据经过预处理的图像、所述图像的特征点以及所述图像的关键帧进行位姿估计；

筛选单元，用于根据位姿估计的结果筛选所述关键帧库中的关键帧；

优化单元，用于利用筛选后的关键帧库中的数据进行位姿优化。

上述技术方案具有如下有益效果：本发明结合动态路标点利用动态路标点的运动概率，提高视觉导航在动态场景中的鲁棒性，本发明在动态场景中导航精度高于现有技术；采用结合动态路标点，处理方法简单、准确；本发明采用相机作为传感器，成本较低，使得专利易于实施。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种基于路标点的双目视觉导航方法的流程图；

图2是本发明实施例提供的一种基于路标点的双目视觉导航装置的结构示意图；

图3是本发明实施例提供的一种基于路标点的双目视觉导航方法的第一种实施方式流程图；

图4是本发明实施例提供的一种基于路标点的双目视觉导航方法的第二种实施方式的详细流程图；

图5是本发明实施例提供的一种基于路标点的双目视觉导航方法特征点检测示意图；

图6是本发明实施例提供的一种基于路标点的双目视觉导航方法的移动机器人位置和姿态变化示意图；

图7是本发明实施例提供的一种基于路标点的双目视觉导航方法的针孔相机投影示意图；

图8是本发明实施例提供的一种基于路标点的双目视觉导航方法的特征匹配流程图；

图9是本发明实施例提供的一种基于路标点的双目视觉导航方法的极限约束示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供了一种基于路标点的双目视觉导航方法，如图1所示，包括以下步骤：

S101：对双目相机所获取的图像进行预处理，包括矫正图像成像过程的畸变、提取图像的路标点作为特征点以及选取图像的关键帧并形成关键帧库；

S102：根据经过预处理的图像、所述图像的特征点以及所述图像的关键帧进行位姿估计；

S103：根据位姿估计的结果筛选所述关键帧库中的关键帧；

S104：利用筛选后的关键帧库中的数据进行位姿优化。

所述矫正图像成像过程的畸变，具体包括：

将三维空间点投影到归一化通向平面，对所述归一化通向平面上的点进行径向畸变矫正和切向畸变矫正，再将纠正后的点投得到像素平面，得到该点在图像上的正确位置。

成像过程的畸变包括径向畸变和切向畸变，预处理的第一步就是畸变矫正。将三位空间点投影到归一化通向平面，设它的归一化坐标为[x,y]^T，利用通过式(2)对给定归一化坐标，对归一化平面上的点进行径向畸变和切向畸变矫正后，再将纠正后的点通过式(3)得到像素平面，得到该点在图像上的正确位置。

其中，k₁,k₂,k₃表示径向畸变系数；p₁,p₂表示切向畸变系数；r表示待矫正点与原点的距离；f_x,f_y表示针孔相机模型中相机的内部参数；(x,y)为矫正前的点在平面上的坐标，(x_distorted,y_distorted)为校正后的点在平面上的坐标，(u,v)为校正后得到的像素平面坐标。

所述提取图像的路标点作为特征点，具体包括：

在视觉导航中路标则是指图像的特征，特征点即在相机视角发生少量变化后会保持不变的点。特征点由关键点和描述子两部分组成。采用ORB(Oriented FAST and RotatedBRIEF)进行特征检测和提取。

分别在双目相机左右两个相机对应图像中选取像素点p,设所述像素点p的亮度为I_p,设置一个图像亮度阈值为I_T,I_T＝0.25I_p，然后以所述像素点p为中心，选取半径为3像素的圆上的16个像素点，若所述半径为3像素的圆上有连续的N个点亮度大于I_p+I_T或小于I_p-I_T，那么p被认为是特征点。

所述选取图像的关键帧，具体包括：

设置选取的关键帧间最小间隔为n帧，n表示相机帧率即每秒钟显示的帧数；

和，设置选取的关键帧最少追踪特征点数目为m个，m是根据影响大小和质量预先设置的特征点数；

和，选取的下一个关键帧与上一个关键帧中的重合特征少于75％。

按照以上条件进行选择关键帧，既保证了关键帧的位姿估计精度，又保证相了邻关键帧间有足够的视差变化。

所述利用筛选后的关键帧库中的数据进行位姿优化，具体包括：

根据所述关键帧库中的数据，对双目相机所获取的图像的所有帧进行特征匹配；

根据特征匹配结果以获取窗口内所有的路标点3D、2D对应关系；

根据所述路标点3D、2D对应关系对关键帧位姿和路标点进行优化。

本发明还提供了一种基于路标点的双目视觉导航装置，如图2所示，包括：

预处理单元21，用于对双目相机所获取的图像进行预处理，包括矫正图像成像过程的畸变、提取图像的路标点作为特征点以及选取图像的关键帧并形成关键帧库；

估计单元22，用于根据经过预处理的图像、所述图像的特征点以及所述图像的关键帧进行位姿估计；

筛选单元23，用于根据位姿估计的结果筛选所述关键帧库中的关键帧；

优化单元24，用于利用筛选后的关键帧库中的数据进行位姿优化。

所述矫正图像成像过程的畸变，具体包括：

投影模块，用于将三维空间点投影到归一化通向平面，对所述归一化通向平面上的点进行径向畸变矫正和切向畸变矫正，再将纠正后的点投得到像素平面，得到该点在图像上的正确位置。

成像过程的畸变包括径向畸变和切向畸变，预处理的第一步就是畸变矫正。将三位空间点投影到归一化通向平面，设它的归一化坐标为[x,y]^T，利用通过式(2)对给定归一化坐标，对归一化平面上的点进行径向畸变和切向畸变矫正后，再将纠正后的点通过式(3)得到像素平面，得到该点在图像上的正确位置。

其中，k₁,k₂,k₃表示径向畸变系数；p₁,p₂表示切向畸变系数；r表示待矫正点与原点的距离；f_x,f_y表示针孔相机模型中相机的内部参数；(x,y)为矫正前的点在平面上的坐标，(x_distorted,y_distorted)为校正后的点在平面上的坐标，(u,v)为校正后得到的像素平面坐标。

所述提取图像的路标点作为特征点，具体包括：

在视觉导航中路标则是指图像的特征，特征点即在相机视角发生少量变化后会保持不变的点。特征点由关键点和描述子两部分组成。采用ORB(Oriented FAST and RotatedBRIEF)进行特征检测和提取。

特征点选择模块，用于分别在双目相机左右两个相机对应图像中选取像素点p,设所述像素点p的亮度为I_p,设置一个图像亮度阈值为I_T,I_T＝0.25I_p，然后以所述像素点p为中心，选取半径为3像素的圆上的16个像素点，若所述半径为3像素的圆上有连续的N个点亮度大于I_p+I_T或小于I_p-I_T，那么p被认为是特征点。

所述选取图像的关键帧，具体包括：

第一条件模块，用于设置选取的关键帧间最小间隔为n帧，n表示相机帧率即每秒钟显示的帧数；

和，第二条件模块，用于设置选取的关键帧最少追踪特征点数目为m个，m是根据影响大小和质量预先设置的特征点数；

和，第三条件模块，用于选取的下一个关键帧与上一个关键帧中的重合特征少于75％。

按照以上条件进行选择关键帧，既保证了关键帧的位姿估计精度，又保证相了邻关键帧间有足够的视差变化。

所述优化单元，具体包括：

匹配模块，根据所述关键帧库中的数据，对双目相机所获取的图像的所有帧进行特征匹配；

对应模块，根据特征匹配结果以获取窗口内所有的路标点3D、2D对应关系；

调整模块，根据所述路标点3D、2D对应关系对关键帧位姿和路标点进行优化。

上述技术方案具有如下有益效果：本发明结合动态路标点利用动态路标点的运动概率，提高视觉导航在动态场景中的鲁棒性，本发明在动态场景中导航精度高于现有技术；采用结合动态路标点，处理方法简单、准确；本发明采用相机作为传感器，成本较低，使得专利易于实施。

下面结合具体的应用实例对本发明实施例上述技术方案进行详细说明，实施过程中没有介绍到的技术细节，可以参考前文的相关描述。

实施例1：

本发明提出了一种基于路标点的双目视觉导航方法，结合动态路标点利用动态路标点的运动概率，提高视觉导航在动态场景中的鲁棒性，其实施的过程中主要步骤如图3所示，具体的详细流程图如图4所示，通过对双目相机所采集的图像进行预处理，进行位姿估计，根据位姿估计结果删选关键帧并加入关键帧库，通过关键帧库中数据进行局部线程优化来优化位姿，用于后期位姿估计。对本发明提出的方法具体实施方式描述如下:

步骤一：对双目相机所获取的立体像对进行预处理

对双目相机所获取的图像对进行预处理即对同一时刻获取的左右两幅图像进行预处理。

相机采用视觉导航中最常用的是中心投影方式下针孔相机模型。如图7所示，在世界坐标系中的描述如下：

P_uv＝K(RP_w+t)＝KTP_w (1)

其中P_uv表示相机坐标系中坐标(u,v)；P_w表示P_uv在世界坐标系中的坐标(x,y,z)；K为相机的内部参数；R,t为相机的位姿，为相机的外部参数，变换后用T表示，T∈SE(3)。

预处理一：畸变矫正

成像过程的畸变包括径向畸变和切向畸变，预处理的第一步就是畸变矫正。将三位空间点投影到归一化通向平面，设它的归一化坐标为[x,y]^T，通过式(2)对给定归一化坐标，对归一化平面上的点进行径向畸变和切向畸变矫正后，再将纠正后的点通过式(3)得到像素平面，得到该点在图像上的正确位置。

其中，k₁,k₂,k₃表示径向畸变系数；p₁,p₂表示切向畸变系数；r表示待矫正点与原点的距离；f_x,f_y表示针孔相机模型中相机的内部参数；(x,y)为矫正前的点在平面上的坐标，(x_distorted,y_distorted)为校正后的点在平面上的坐标，(u,v)为校正后得到的像素平面坐标。

预处理二：特征检测与特征提取

在视觉导航中路标则是指图像的特征，特征点即在相机视角发生少量变化后会保持不变的点。特征点由关键点和描述子两部分组成。采用ORB(Oriented FAST and RotatedBRIEF)进行特征检测和提取。

FAST关键点检测，如图5所示,：首先分别在左右相机对应图像中选取像素p(p点对应的相平面坐标为P(u,v))，设它的亮度为I_p(即图像的灰度值)。然后设置一个图像亮度阈值I_T(I_T＝0.25I_p)，然后以像素p为中心，选取半径为3的圆上的16个像素点。再然后若半径为3的圆上有连续的N(N＝9,11,12,在此N＝12)个点亮度大于I_p+I_T或小于I_p-I_T，那么p被认为是特征点。分别按照以上对左右相机对图像中每一个像素执行相同操作，获得对应的FAST关键点。

FAST关键点添加方向信息：首先在左右相机图像对中的一个小块的图像块B中，定义图像块的矩为

p,q＝{0,1}；然后通过矩可以找到图像块的质心C，

再然后连接图像块的几何中心O与质心C，得到一个方向向量

于是特征点的方向θ可以定义为θ＝arctan(m₀₁/m₁₀)。

BRIEF(Binary Robust Independent Elementary Feature)描述子：BRIEF描述子是一种二进制描述子，在此0和1编码了关键点附近两个像素(比如p和q)的大小关系，如果p＞q，则取1，反之取0。图像中随机取128个这样的p,q得到128维由0、1组成的向量即为BRIEF描述子。

步骤二：特征匹配

匹配包括路标点特征匹配和左/右相机前后帧匹配，如图8所示。

匹配一：路标点匹配。通过极线匹配对图像中左右图像进行特征匹配。第一，对得到的特征点根据Hamming距离设置一个阈值M_T和正确率p，M_T＝k(d_max-d_min)(其中k为比例系数，可根据实际匹配进行调节)，p＝N_true/N_all(其中N_all为匹配点总数，N_true为正确匹配点数)。完成粗略匹配，生成粗略匹配点集。第二，在粗略匹配点集上使用RANSAC和8点改进算法计算基本矩阵F并剔除部分点；第三，对剩余点集应用极线约束，如图9所示，剔除误匹配点，得到大量优质匹配点，完成匹配。

匹配二：左/右相机前后帧匹配及关键帧选择

本发明假定在短时间内(约30ms)左/右相机保持匀速运动，对当前帧根据对极约束求解相机的粗略位姿；进而利用当前帧位姿，根据相机投影模型，将上一关键帧的路标三维点投影到当前帧影像中，利用快速近似最邻近算法进行匹配。匹配成功，则将前一帧目标的数据赋于当前帧目标，然后将该目标发送，继续使用前一帧的目标做为下一帧目标的前一帧；当匹配不成功，则将该前一帧数据删除，并将当前帧匹配不成功的目标作为前一帧，后续迭代进行运算。

考虑视觉导航实时性，仅在采集的影像中选取一部分帧作为关键帧，在滑动窗口找到与当前帧共同观测最多的关键帧，把其作为当前帧的参考关键帧，则筛选当前帧为关键帧的条件如下：

关键帧间最小间隔为n帧(n表示相机帧率即每秒钟显示的帧数)；

当前帧最少追踪特征点数目为m个(m根据影响大小和质量预先设置的路标特征数)；

当前帧与上一关键帧中的重合特征必须少于75％；

按照以上条件进行选择关键帧，既保证了关键帧的位姿估计精度，又保证相了邻关键帧间有足够的视差变化。

步骤三：位姿估计及优化

位姿估计：采用ICP进行位姿估计。

滑动窗口插入新关键帧后，对窗口内所有帧执行特征匹配，以获取窗口内所有的路标点3D-2D对应关系；最后执行滑动窗口内的图优化，同时优化滑动窗口内的关键帧位姿和路标三维点。

首先因子图模式建模

如图6所示，移动机器人状态(在全局地图坐标系中随时间变化的位置和姿态)表示为X＝{x_i，i＝0，1，2，...，T}，在地图坐标系中的路标点位置表示为L＝{l_j，j＝1，2，...，N}，控制移动的驱动输入表示为U＝{u_i，i＝1，2，...，T}，路标点在机器人坐标系下的观测量表示为Z＝{z_k，i＝1，2，...，k}，每个路标点标记了运动指数(运动指示性参数)W＝{w_j，j＝1，2，...，N}。在每个时刻k，观测量z_k与机器人位姿

路标点

以及指示变量

相对应。其中i_k把机器人位姿和测量值关联起来，j_k把路标点与测量值关联起来。

考虑动态场景，使用运动指数W对每一个路标点的运动性进行建模，以表示其运动的可能性，使得观测量更加鲁棒。通过W，与动态路标点相关联的较差的观测量会在导航过程中去除。

根据稳健图优化视觉导航模型，顾及动态路标点的所有变量和观测量的联合概率如下：

给定观测量Z，控制量U和当前运动指数W，机器人状态X和路标点L的最大似然估计可写为：

在观测量误差项中加入稳健核函数v_k，因此z_k的条件概率可表示为增强的高斯分布：

其中，

表示在k时刻与观测量z_k相关联的路标点

为静态的可能性；v_k表示每一个路标观测量相关的稳健核函数，用来约束观测量的粗略误差。当

或v_k趋于0时，等价于把高斯分布的方差设为无穷大，使得该分布退化为均匀分布，导致该分布表示的约束对图优化过程无影响。

其次模型求解

问题中运动指数变量无法直接观测，属于隐性变量，则所描述模型为高斯混合分布模型。因此使用EM算法求解稳健增强目标函数，估计路标点运动概率并解算机器人位姿和地图三维点。

估计运动指数(M-step)：在M-step(最大化步骤)需要选取最优的w，使得联合似然最大化。首先假设所有路标点都是运动的(令所有w_k＝0)，即可最大化该似然函数(概率为1)；然后引入拉格朗日算子为惩罚项，“惩罚”“设置太多运动路标点”这个假设，也可把拉格朗日算子理解为潜在路标点运动性的先验知识。对引入拉格朗日算子的对数似然目标函数中对w求导，令导数为零可得该目标函数最大化。

增强的图优化目标函数求解(E-step)：在E-step(期望步骤)中，需要使用已经在M-step估计出的潜在隐性变量“运动指数”，求解机器人位姿以及路标点三维坐标。直接使用M-step估计的运动指数变量，采用标准图优化方法求解加入路标点运动指数和观测稳健核函数的增强目标函数

对每一个观测量应用DCS方法中的核函数v_k(其中

)，该优化问题可转化为一个标准最小二乘问题。

E-step和M-step循环迭代进行，直至达到收敛条件，输出机器人位姿和路标点坐标。

应该明白，公开的过程中的步骤的特定顺序或层次是示例性方法的实例。基于设计偏好，应该理解，过程中的步骤的特定顺序或层次可以在不脱离本公开的保护范围的情况下得到重新安排。所附的方法权利要求以示例性的顺序给出了各种步骤的要素，并且不是要限于所述的特定顺序或层次。

在上述的详细描述中，各种特征一起组合在单个的实施方案中，以简化本公开。不应该将这种公开方法解释为反映了这样的意图，即，所要求保护的主题的实施方案需要比清楚地在每个权利要求中所陈述的特征更多的特征。相反，如所附的权利要求书所反映的那样，本发明处于比所公开的单个实施方案的全部特征少的状态。因此，所附的权利要求书特此清楚地被并入详细描述中，其中每项权利要求独自作为本发明单独的优选实施方案。

为使本领域内的任何技术人员能够实现或者使用本发明，上面对所公开实施例进行了描述。对于本领域技术人员来说；这些实施例的各种修改方式都是显而易见的，并且本文定义的一般原理也可以在不脱离本公开的精神和保护范围的基础上适用于其它实施例。因此，本公开并不限于本文给出的实施例，而是与本申请公开的原理和新颖性特征的最广范围相一致。

上文的描述包括一个或多个实施例的举例。当然，为了描述上述实施例而描述部件或方法的所有可能的结合是不可能的，但是本领域普通技术人员应该认识到，各个实施例可以做进一步的组合和排列。因此，本文中描述的实施例旨在涵盖落入所附权利要求书的保护范围内的所有这样的改变、修改和变型。此外，就说明书或权利要求书中使用的术语“包含”，该词的涵盖方式类似于术语“包括”，就如同“包括，”在权利要求中用作衔接词所解释的那样。此外，使用在权利要求书的说明书中的任何一个术语“或者”是要表示“非排它性的或者”。

本领域技术人员还可以了解到本发明实施例列出的各种说明性逻辑块(illustrative logical block)，单元，和步骤可以通过电子硬件、电脑软件，或两者的结合进行实现。为清楚展示硬件和软件的可替换性(interchangeability)，上述的各种说明性部件(illustrative components)，单元和步骤已经通用地描述了它们的功能。这样的功能是通过硬件还是软件来实现取决于特定的应用和整个系统的设计要求。本领域技术人员可以对于每种特定的应用，可以使用各种方法实现所述的功能，但这种实现不应被理解为超出本发明实施例保护的范围。

本发明实施例中所描述的各种说明性的逻辑块，或单元都可以通过通用处理器，数字信号处理器，专用集成电路(ASIC)，现场可编程门阵列或其它可编程逻辑装置，离散门或晶体管逻辑，离散硬件部件，或上述任何组合的设计来实现或操作所描述的功能。通用处理器可以为微处理器，可选地，该通用处理器也可以为任何传统的处理器、控制器、微控制器或状态机。处理器也可以通过计算装置的组合来实现，例如数字信号处理器和微处理器，多个微处理器，一个或多个微处理器联合一个数字信号处理器核，或任何其它类似的配置来实现。

本发明实施例中所描述的方法或算法的步骤可以直接嵌入硬件、处理器执行的软件模块、或者这两者的结合。软件模块可以存储于RAM存储器、闪存、ROM存储器、EPROM存储器、EEPROM存储器、寄存器、硬盘、可移动磁盘、CD-ROM或本领域中其它任意形式的存储媒介中。示例性地，存储媒介可以与处理器连接，以使得处理器可以从存储媒介中读取信息，并可以向存储媒介存写信息。可选地，存储媒介还可以集成到处理器中。处理器和存储媒介可以设置于ASIC中，ASIC可以设置于用户终端中。可选地，处理器和存储媒介也可以设置于用户终端中的不同的部件中。

在一个或多个示例性的设计中，本发明实施例所描述的上述功能可以在硬件、软件、固件或这三者的任意组合来实现。如果在软件中实现，这些功能可以存储与电脑可读的媒介上，或以一个或多个指令或代码形式传输于电脑可读的媒介上。电脑可读媒介包括电脑存储媒介和便于使得让电脑程序从一个地方转移到其它地方的通信媒介。存储媒介可以是任何通用或特殊电脑可以接入访问的可用媒体。例如，这样的电脑可读媒体可以包括但不限于RAM、ROM、EEPROM、CD-ROM或其它光盘存储、磁盘存储或其它磁性存储装置，或其它任何可以用于承载或存储以指令或数据结构和其它可被通用或特殊电脑、或通用或特殊处理器读取形式的程序代码的媒介。此外，任何连接都可以被适当地定义为电脑可读媒介，例如，如果软件是从一个网站站点、服务器或其它远程资源通过一个同轴电缆、光纤电缆、双绞线、数字用户线(DSL)或以例如红外、无线和微波等无线方式传输的也被包含在所定义的电脑可读媒介中。所述的碟片(disk)和磁盘(disc)包括压缩磁盘、镭射盘、光盘、DVD、软盘和蓝光光盘，磁盘通常以磁性复制数据，而碟片通常以激光进行光学复制数据。上述的组合也可以包含在电脑可读媒介中。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于路标点的双目视觉导航方法，其特征在于，包括以下步骤：

对双目相机所获取的图像进行预处理，包括矫正图像成像过程的畸变、提取图像的路标点作为特征点以及选取图像的关键帧并形成关键帧库；

根据经过预处理的图像、图像的特征点以及所述图像的关键帧进行位姿估计；

根据位姿估计的结果筛选所述关键帧库中的关键帧；

利用筛选后的关键帧库中的数据进行位姿优化。

2.根据权利要求1所述的一种基于路标点的双目视觉导航方法，其特征在于，所述矫正图像成像过程的畸变，具体包括：

将三维空间点投影到归一化通向平面，对所述归一化通向平面上的点进行径向畸变矫正和切向畸变矫正，再将纠正后的点投得到像素平面，得到该点在图像上的正确位置。

3.根据权利要求1所述的一种基于路标点的双目视觉导航方法，其特征在于，所述提取图像的路标点作为特征点，具体包括：

分别在双目相机左右两个相机对应图像中选取像素点p,设所述像素点p的亮度为I_p,设置一个图像亮度阈值为I_T,I_T＝0.25I_p，然后以所述像素点p为中心，选取半径为3像素的圆上的16个像素点，若所述半径为3像素的圆上有连续的N个点亮度大于I_p+I_T或小于I_p-I_T，那么p被认为是特征点。

4.根据权利要求1所述的一种基于路标点的双目视觉导航方法，其特征在于，所述选取图像的关键帧，具体包括：

设置选取的关键帧间最小间隔为n帧，n表示相机帧率即每秒钟显示的帧数；

和，设置选取的关键帧最少追踪特征点数目为m个，m是根据影响大小和质量预先设置的特征点数；

和，选取的下一个关键帧与上一个关键帧中的重合特征少于75％。

5.根据权利要求4所述的一种基于路标点的双目视觉导航方法，其特征在于，所述利用筛选后的关键帧库中的数据进行位姿优化，具体包括：

根据所述关键帧库中的数据，对双目相机所获取的图像的所有帧进行特征匹配；

根据特征匹配结果以获取窗口内所有的路标点3D、2D对应关系；

根据所述路标点3D、2D对应关系对关键帧位姿和路标点进行优化。

6.一种基于路标点的双目视觉导航装置，其特征在于，包括：

预处理单元，用于对双目相机所获取的图像进行预处理，包括矫正图像成像过程的畸变、提取图像的路标点作为特征点以及选取图像的关键帧并形成关键帧库；

估计单元，用于根据经过预处理的图像、所述图像的特征点以及所述图像的关键帧进行位姿估计；

筛选单元，用于根据位姿估计的结果筛选所述关键帧库中的关键帧；

优化单元，用于利用筛选后的关键帧库中的数据进行位姿优化。

7.根据权利要求6所述的一种基于路标点的双目视觉导航装置，其特征在于，所述矫正图像成像过程的畸变，具体包括：

投影模块，用于将三维空间点投影到归一化通向平面，对所述归一化通向平面上的点进行径向畸变矫正和切向畸变矫正，再将纠正后的点投得到像素平面，得到该点在图像上的正确位置。

8.根据权利要求6所述的一种基于路标点的双目视觉导航装置，其特征在于，所述提取图像的路标点作为特征点，具体包括：

特征点选择模块，用于分别在双目相机左右两个相机对应图像中选取像素点p,设所述像素点p的亮度为I_p,设置一个图像亮度阈值为I_T,I_T＝0.25I_p，然后以所述像素点p为中心，选取半径为3像素的圆上的16个像素点，若所述半径为3像素的圆上有连续的N个点亮度大于I_p+I_T或小于I_p-I_T，那么p被认为是特征点。

9.根据权利要求6所述的一种基于路标点的双目视觉导航装置，其特征在于，所述选取图像的关键帧，具体包括：

第一条件模块，用于设置选取的关键帧间最小间隔为n帧，n表示相机帧率即每秒钟显示的帧数；

和，第二条件模块，用于设置选取的关键帧最少追踪特征点数目为m个，m是根据影响大小和质量预先设置的特征点数；

和，第三条件模块，用于选取的下一个关键帧与上一个关键帧中的重合特征少于75％。

10.根据权利要求9所述的一种基于路标点的双目视觉导航装置，其特征在于，所述优化单元，具体包括：

匹配模块，根据所述关键帧库中的数据，对双目相机所获取的图像的所有帧进行特征匹配；

对应模块，根据特征匹配结果以获取窗口内所有的路标点3D、2D对应关系；

调整模块，根据所述路标点3D、2D对应关系对关键帧位姿和路标点进行优化。

Images