CN110657803B - 机器人定位方法、装置以及存储装置 - Google Patents

Abstract

本申请公开了一种机器人定位方法、装置及存储装置，该方法包括：获取包含机器人距离信息的当前帧图像，距离信息由超宽带基站提供；将当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点；判断当前帧图像是否为关键帧图像；若判断为是，则利用联合目标函数对关键帧图像进行优化，联合目标函数至少包括超宽带基站的距离代价函数及视觉残差代价函数。通过上述方式，本申请能够提高机器人的定位精度。

Description

机器人定位方法、装置以及存储装置

技术领域

本申请涉及机器人视觉定位技术领域，特别是涉及一种机器人定位方法、装置及存储装置。

背景技术

基于机器视觉的定位导航是机器人领域中一项很关键的技术。但是现有的视觉方案鲁棒性仍然不够高，经常存在跟踪丢失，以及重定位失败等问题。

在大多数的视觉SLAM(Simultaneous Localization and Mapping，同时定位与地图构建)系统中，都是根据视觉图像特征提取与匹配的方法来构建环境特征地图，并估计机器人当前位姿实现同时定位。由于位姿估计和地图构件都是有噪声的，特别是在大尺度环境中SLAM过程具有更大的不确定性。基于滤波的SLAM方法，如EKF-SLAM(Extended KalmanFilter，扩展卡尔曼滤波器)及PF-SLAM(Particle Filter，粒子滤波)都是以概率方法来表示观测信息的不确定性，认为路标是独立的，导致长期运行时路标数量将会增加至计算机资源不足以实时更新地图,且将非线性近似为线性化的方法导致误差的产生，任何数据关联的错误都会带到SLAM状态估计中去，从而导致机器人的定位不准确、定位精度不高等问题。

发明内容

本申请提供一种机器人定位方法、装置及存储装置，能够提高机器人定位精度和及时矫正机器人的累积误差的问题。

为解决上述技术问题，本申请采用的一个技术方案是：提供一种机器人定位方法，所述方法包括：获取包含机器人距离信息的当前帧图像，所述距离信息由超宽带基站提供；将所述当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点；判断所述当前帧图像是否为关键帧图像；若判断为是，则利用联合目标函数对所述关键帧图像进行优化，所述联合目标函数至少包括所述超宽带基站的距离代价函数及视觉残差代价函数。

为解决上述技术问题，本申请采用的另一个技术方案是：提供一种机器人定位装置，所述装置包括处理器及存储器，所述处理器连接所述存储器；其中，所述处理器用于获取包含机器人距离信息的当前帧图像，所述距离信息由超宽带基站提供；将所述当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点；判断所述当前帧图像是否为关键帧图像；若判断为是，则利用联合目标函数对所述关键帧图像进行优化，所述联合目标函数至少包括所述超宽带基站的距离代价函数及视觉残差代价函数。

为解决上述技术问题，本申请采用的又一个技术方案是：提供一种存储装置，存储有能够实现上述任一项所述方法的程序文件

本申请的有益效果是：提供一种机器人定位方法、装置及存储装置，通过采用联合目标优化函数(至少包括超宽带基站的距离代价函数及视觉残差代价函数)对获取到关键帧图像进行优化，可以提高机器人的定位精度。

附图说明

图1是本申请机器人定位方法第一实施方式的流程示意图；

图2是本申请图1中步骤S1一实施实施方式的流程示意图；

图3是本申请图1中步骤S2一实施实施方式的流程示意图；

图4是本申请提高机器人定位精度装置一实施方式的结构示意图；

图5是本申请存储装置一实施方式的结构示意图。

具体实施方式

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

请参阅图，图1本申请机器人定位的方法第一实施方式流程示意图。

UWB(Ultra Wide Band，超宽带)是一种以极低功率在短距离内高速传输数据的无线通信技术，UWB具有抗干扰性能强、传输速率高、带宽极宽、消耗电能小、发送功率小等诸多优势，系统容量大发送功率非常小，其电磁波辐射对人体的影响也会很小，应用面较广。

本申请采用的技术方案中，采用单个超宽带设备辅助增强机器视觉定位鲁棒性，且在本申请的实施例中，可以将该UWB设备集成到机器人的充电桩内，可节省安装多个UWB基站设施，让机器人的移动更加方便。当然，在其他实施方式中，该UWB基站也可以是集成在机器人的其他部件，此处不做进一步限定。

本申请中机器人定位的方法具体包括如下步骤：

S1，获取包含机器人距离信息的当前帧图像，距离信息由超宽带基站提供。

本申请中UWB基站的扫描时间和相机的扫描频率可能不是同步的，在具体实施方式中，可能相机扫描一帧图像的频率为30HZ，而UWB基站可能为10HZ，也就是说不是每一帧图像中都包含有UWB基站的距离信息，当然，在其他实施方式中，相机和UWB的扫描频率也可以是相同的，此处不做进一步限定。对于二者扫描频率不相同的情况，此时就需要将二者进行数据同步。参见图2，步骤S1进一步包括如下子步骤：

S11，获取超宽带基站的参数信息，参数信息包括距离信息及时间信息。

本申请中的UWB包括基站和定位标签，本实施例中，基站设置在机器人的充电桩内，定位标签为有源标签可以设置在机器人身上，该UWB基站可以提供机器人到基站的绝对距离信息以及时间信息，即UWB基站发射信号给定位标签，通过定位标签返回的时间可以计算出机器人距离基站的绝对距离信息。当然，在其它实施例中，也可以是定位标签发出UWB脉冲信号，通过UWB基站接收计算出机器人距离基站的绝对距离信息，此处不做进一步限定。

S12，将超宽带基站的参数信息和相邻的图像进行数据同步，以得到包含机器人距离信息的当前帧图像。

如上文所述，相机和UWB基站的扫描频率可能是不相同的，在获取到UWB基站的参数信息后，需要将UWB基站的参数信息和相机获取到的和所述UWB基站时间信息最邻近的图像进行数据同步，以得到包含机器人距离UWB基站之间绝对距离信息的当前帧图像。

S2，将当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点。

特征匹配是视觉SLAM中极为关键的一步，宽泛地说，特征匹配解决了SLAM中的数据关联问题(data association)，即确定当前看到的路标与之前看到的路标之间的对应关系。通过对图像与图像或者图像和地图之间的描述子进行准确匹配，可以为后续姿态估计，优化等操作减轻大量负担。参见图3，步骤S2进一步包括如下子步骤：

S21，分别提取当前帧图像和相邻帧图像的特征点。

步骤S21中，一般采用ORB特征提取算法，即利用FAST焦点提取算法提取图像中的角点，并利用BRIEF特征描述算法生成二进制的特征描述符，具有很快的执行速度。其中，ORB特征提取算法可以分为两部分，首先可以利用FAST特征点提取算法获得图像上的特征点，然后利用强度质心的概念来计算特征点的方向，并利用BRIEF特征描述算法生成二进制的ORB描述子。

FAST特征点检测是基于以候选点为圆心，设定长度为半径的圆的圆周上的像素点，比较圆周上各个像素点的灰度值与候选点灰度值的大小来判断候选点是否为一个特征点。如果该圆周上有连续四分之三的点与中心点的差值同号且绝对值都大于设定的阈值，那么就可以认定该中心点是特征点。本实施例中，通过上述算法可以实现当前帧图像和相邻帧图像特征点提取。且在提取完当前帧图像的特征点后，还可以进一步对提取到当前帧图像的特征点进行图像去畸处理。

S22，判断当前帧图像是否初始化成功。

步骤S22中，进一步判断该当前帧图像是否可以初始化成功。在具体实施方式中，可以将相机获取到的第一帧图像作为参考帧图像，即将第一帧图像提取特征点，进行去畸处理后，对其进行归一化处理，得到一相对坐标系。其中，归一化是通过一些列变换(即利用图像的不变矩寻找一组参数使其能够消除其他变换函数的影响)，将待处理原始图像转换成相应的唯一标准形式，且该标准形式图像对频域、旋转、缩放等仿射变换具有不变特性。

在获得当前帧图像后，将步骤S21中获取到的进行去畸处理后的特征点坐标进行初始化，即将获取到的当前帧图像以第一帧图形作为参考帧图像(参考标准)进行初始化处理。若判断该当前帧图像初始化成功，则执行步骤S23，若该当前帧图像初始化失败，则执行步骤S25。

S23，判断当前帧图像的将特征点和相邻帧图像的特征点之间是否匹配。

步骤S23中，若获取到的当前帧图像的特征点可以被初始化，则进一步判断当前帧图像的将特征点和相邻帧图像的特征点之间是否匹配。其中，图像特征匹配是计算机视觉领域最常用的处理方法，在图像拼接等很多领域都有着广泛的应用。一般来说，在对两帧图像进行特征匹配时，需要计算两帧图像相应特征点的特征描述子之间的距离，并根据该距离的大小来判断是否匹配成功。在具体实施方式中，需要分别对当前帧图像和相邻帧图像中特征描述子进行提取。

可选地，进一步判断当前帧图像中的特征描述子和相邻帧帧图像中的特征描述子之间的距离是否满足预设阈值，本申请中，特征描述子间的距离判定准则可以采用如下几种方式来判定：

1.单一阈值法，给定一阈值，当特征点间的特征描述子距离小于此阈值时，则判定特征点间匹配成功。

2.最邻近法，给定一较小的阈值，寻找特征点间的特征描述子距离最小匹配，当该距离小于阈值时，则判定匹配成功。

3.最邻近比值法，选取特征点领域内特征描述子距离最近和次近的两个候选点，当两候选点间的距离较大时，即两候选点距特征点的距离小于一定阈值时，则判定最近的候选点为匹配点。

当然，在其他实施方式中，也可以采取其他匹配方式，此处不做进一步限定。且若判断当前帧图像的将特征点和相邻帧图像的特征点之间匹配，则执行步骤S24，若判断为否，则执行步骤S1。

S24，若判断为是，则根据当前帧图像的特征点生成局部地图点。

步骤S23中若判断当前帧图像的将特征点和相邻帧图像的特征点之间匹配，则根据两帧图像中的特征匹配点进行计算和相机的位姿估计，以构建当前帧图像每一特征点对应的地图点组成局部地图点。

S25，若判断为否，则初始化当前帧图像作为参考帧图像。

步骤S22中，若判断当前帧图像未能初始化成功，则舍弃掉作为参考帧图像的第一帧，并对该当前帧图像进行归一化处理，以使得该当前帧图像作为新的参考帧图像，并继续获取下一帧图像，将下一帧图像和该当前帧图像进行初始化处理，且其后续的处理过程和上文描述类似，可以参见上文的具体描述，此处不再赘述。

此外，该步骤S2中还包括更新相机的运动速度，以在后续关键帧图像特征点进行匹配时，缩小地图数据库的数据范围，提高其搜索效率。

S3，判断当前帧图像是否为关键帧图像。

在具体视频处理、保持、搜索等领域，并不会对每个图像都做详细的优化和回环检测，而只针对其中的部分关键帧进行处理，达到缩小体积，加快传输的目的。其中，关键帧是相机运动过程当中某几个特殊的帧，且关键帧常见的选取方法一般是当相机运动经过一定间隔，就取一个新的关键帧并保存起来。这些关键帧的位姿将被仔细优化，而位于两个关键帧之间的那些东西，除了对地图贡献一些地图点外，可以忽略掉。

可选地，一般关键帧图像的选择方法可以分为三类，基于时间的选择法、基于空间的选择法以及基于视觉内容的选择法。其中，基于时间的选择方法，机器人每间隔固定一段时间就将当前帧图像视为关键帧图形，且该固定时间可以使用经过一定时间的秒数决定也可以使用警告过一定的图像帧数决定。基于空间的选择法，机器人每间隔一定的距离或者角度就将该当前帧设为关键帧图像。基于视觉内容的选择法，每当视觉内容发生了一定程度的变化，就将该当前帧图像设为关键帧图像，即通过图像处理来计算图像之间的相似度，衡量图像间的视觉内容变化并用以判断是否选择当前帧图像作为代表该场景的关键帧。本实施例中，可以选用基于视觉内容的选择法来选取关键帧图像，且本实施例中，关键帧图像的选取方法可以包括如下条件：

1.当前帧图像与其前一关键帧至少间隔20个图像帧，避免了相邻关键帧图像之间存在大量冗余信息。

2.当前帧图像与其前一关键帧至少包含50个相同的特征点，保证关键帧图像间有足够的匹配信息来获得好的位姿跟踪。

3.当前帧图像与参考关键帧图像相同的特征点数目小于其特征总数的90％，使得两相邻关键帧图像有足够的图像变化。

本实施例中关键帧图像的选取方法，是结合视觉内容选择方法和时间选择方法，可以保证系统的实时性。当然在其他实施方式中，也可以采用关键帧图像选取的方法，此处不做进一步限定。且若判断该当前帧图像为关键帧图像，则执行步骤S4，若判断该当前帧图像不为关键帧图像，则执行步骤S1。

S4，若判断为是，则利用联合目标函数对关键帧图像进行优化，联合目标函数至少包括所述超宽带基站的距离代价函数及视觉残差代价函数。

步骤S4中，若判断当前帧图像为关键帧图像，则利用联合目标函数对该关键帧图像进行优化处理。在SLAM系统中，由于相机的标定误差会影响到3D重建时的精度，追踪的误差则体现在一些列关键帧之间的位姿和重建中。且该误差的不断累积会造成后续图像帧的位姿距离实际位姿越来越远，最终现在整个系统的精度，在闭环检测在可以通过检测闭环、融合闭环可以能很大程度上减小系统的漂移，得到精确的位姿与地图，但仅仅有闭环检测还不够，在大规模场景中不一定有数量众多的闭环，这时整个视觉SLAM系统就退化成视觉里程就，使得误差不断增大。故需要在每一关键帧图像插入局部地图时，在其关键帧图像处进行一次局部优化。其中，局部优化只是在一小部分关键帧图像和地图点之间的优化，其耗费的即使时间并不长，对每一关键帧图像都进行一次局部优化，使用很少时间可以提高关键帧图像位姿和地图点的准确性。

和现有技术不同之处在于，本申请中采用联合目标函数对该当前帧图像进行优化，其中，该联合目标函数至少包括超宽带基站的距离代价函数，即在现有的视觉残差代价函数的基础上增加UWB基站的距离代价函数，二者联合对该关键帧图像进行优化。其中，现有技术中一般是将相机的位姿及地图点都看成优化变量，放在一起优化，且其视觉残差代价函数为：

其中，Xⁱ为待优化的地图点坐标，R₁，t₁为待优化的机器人的旋转和平移位姿，P_L为待优化的地图点坐标的集合，K_F为待优化的机器人的位置集合，X_k为机器人在k位置时，所述待优化地图点和关键帧图像匹配的点集，

为关键帧图像和所述待优化地图点匹配时的特征点，π_(·)(R_kX^j+t_k)为待优化地图点投影到所述关键帧图像上的坐标，ρ为优化核函数。

其中，等式(4)表示每一待优化地图点在每一关键帧图像上的投影误差和，即将当前帧图像的与地图点按照当前估计的位姿进行投影得到的位置相比较得到的误差，称之为重投影误差，且该视觉残差代价函数的优化目标是得到能使该重投影误差和最小化的所有地图点和关键帧图像的位姿。

进一步，该联合目标函数至少还包括UWB基站的代价函数，且该UWB基站的距离代价函数为：

E_k(p_k)＝∑_kρ(||p^uwb-p_k||-d^k) (2)

其中，p^uwb为超宽带基站的坐标，p_k为预测的超宽带基站定位标签的坐标，Np为局部优化内所有超宽带基站位置数据的集合，d^k为超宽带基站测得的机器人的实际位置。可选地，预测的超宽带基站标签的坐标p_k可以由和UWB基站进行绑定的当前帧图像之间的时间差以及相机的运动速度计算得出，即p_k＝[t_k+(t_UWB-t_img)]×V，其中V为相机的运动速度。可选地，等式(1)表示优化机器人位姿，当机器人获取UWB基站提供的绝对距离信息时，其与预测的超宽带基站定位标签p_k的坐标之间的误差最小。

故将视觉残差代价函数和UWB基站的距离代价函数联合后得到的联合目标优化函数为：

本申请中，通过将视觉残差代价函数和UWB基站的代价函数联合起来对当前帧图像的位姿和空间地图点进行迭代优化，可以提高机器人的定位精度。当然，本实施例中只考虑局部地图中地图点和关键帧图像的优化问题，实际运用中也可以运用与全局地图中地图点和关键帧图像的优化，此处不做进一步限定。

S5，对优化后的关键帧图像进行回环检测，以更新局部地图。

回环检测，指的是如何有效的检测出相机经过同一个地方这件事，关系到估计的轨迹和地图在长时间下的正确性。回环检测对SLAM系统意义重大，其提供了当前数据与所有历史数据的关联，在跟踪算法丢失之后，还可以利用回环检测进行重定位。因此，回环检测对整个SLAM系统精度与鲁棒性的提升非常明显的。

其中，本实施例中回环检测算法中，核心问题是如何计算图像间的相似性，该相似性计算可以采用词袋模型。采用该词袋模型可以分为两个步骤，产生一个词典，即将上述提取到的所有的已保存的关键帧图像的特征描述子进行聚类，形成离散的单词，采用K-means++的算法将特征点描述子集合聚为k类，最后聚出的每一类的中心就是视觉词典中的一个单词为了进行高效的搜索，视觉词典采用分层树形结构存储，每层的节点再次进行K-means聚类得到下一层节点，直至达到设置的最大深度，这样词典的单词存储在树的多个叶子节点中。且该词典可以在后续回环检测—几何结构验证阶段，加速匹配候选帧图像和当前帧图像之间特征点对。

本申请中，结合UWB基站的提供机器定位标签人到基站的绝对距离可以缩小该关键帧图像在地图数据库中的搜索范围，可以减小该词袋模型的搜索范围，让词袋模型不必对整个地图数据进行搜索，从而降低了搜索时间，提高后续的匹配效率。且本实施例中，当前帧图像可以通过视觉词典转换为词袋向量，且将该当前帧图像的词袋向量和该搜索范围内的所有关键帧图像的词袋向量进行筛选匹配，并分别计算该搜索范围内的所有关键帧图像词袋向量的相似度，若该相似度满足一定阈值，则可以认定该当前帧图像在该局部地图数据库中检测到回环，若该相似度不满足阈值条件，则说明该当前帧图像在该局部地图数据库中未检测到回环，则将该当前帧图像的地图点数据添加到该局部地图数据库中，以更新该局部地图数据。

上述实施方式中，通过采用联合目标优化函数(至少包括超宽带基站的距离代价函数及视觉残差代价函数)对获取到关键帧图像进行优化，可以提高机器人的定位精度。

请参阅图4，图4为本申请机器人定位装置一实施方式的结构示意图。如图4所示，该装置包括处理器11及存储器12，处理器11连接存储器12。

其中，处理器11用于获取包含机器人距离信息的当前帧图像，该距离信息由超宽带基站提供；将当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点；判断当前帧图像是否为关键帧图像；若判断为是，则利用联合目标函数对关键帧图像进行优化，该联合目标函数至少包括超宽带基站的距离代价函数及视觉残差代价函数。

其中，本申请采用的技术方案中，采用单个超宽带(Ultra Wideband,UWB)基站辅助增强机器视觉定位鲁棒性，且在本申请的实施例中，可以将该UWB基站集成到机器人的充电桩内，可节省安装多个UWB基站设施，让机器人的移动更加方便。当然，在其他实施方式中，该UWB基站也可以是集成在机器人的其他部件，此处不做进一步限定。该UWB基站提供机器人到基站的绝对距离信息，可以缩小视觉重定位时搜索范围，加快重定位速度和可靠性。且在后续对关键帧图像进行优化时候增加该超宽带基站的距离代价函数及视觉残差代价函数，可以提高机器人的定位精度。

其中，处理器11还可以称为CPU(Central Processing Unit，中央处理单元)。处理器11可能是一种集成电路芯片，具有信号的处理能力。处理器11还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

上述装置中处理器可分别执行上述方法实施例中对应的步骤，故此处不再赘述，详细请参阅以上对应步骤的说明。

上述实施方式中，通过结通过采用联合目标优化函数(至少包括超宽带基站的距离代价函数及视觉残差代价函数)对获取到关键帧图像进行优化，可以提高机器人的定位精度。

参阅图5，图5为本申请存储装置一实施方式的结构示意图。本申请的存储装置存储有能够实现上述所有方法的程序文件21，其中，该程序文件21可以以软件产品的形式存储在上述存储装置中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储装置包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等终端设备。

综上所述，本领域技术人员容易理解，本申请提供一种机器人定位方法、装置及存储装置，通过采用联合目标优化函数(至少包括超宽带基站的距离代价函数及视觉残差代价函数)对获取到关键帧图像进行优化，可以提高机器人的定位精度。

以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (10)

1.一种机器人定位方法，其特征在于，所述方法包括：

获取包含机器人距离信息的当前帧图像，所述距离信息由超宽带基站提供；

将所述当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点；

判断所述当前帧图像是否为关键帧图像；

若判断为是，则利用联合目标函数对所述关键帧图像进行优化，所述联合目标函数至少包括所述超宽带基站的距离代价函数及视觉残差代价函数之和。

2.根据权利要求1所述的方法，其特征在于，所述超宽带基站的距离代价函数为：

其中，p^uwb为所述超宽带基站的坐标，p_k为预测的所述超宽带基站标签的坐标，Np为局部优化内所有所述超宽带基站位置数据的集合，d^k为所述超宽带基站测得的所述机器人的实际位置，ρ为优化核函数。

3.根据权利要求2所述的方法，其特征在于，所述视觉残差代价函数为：

其中，Xⁱ为待优化的地图点坐标，P_l，t_l为待优化的所述机器人的旋转和平移位姿，P_L为所述待优化的地图点坐标的集合，K_F为待优化的所述机器人的位置集合，X_k为所述机器人在k位置时，所述待优化地图点和所述关键帧图像匹配的点集，

为所述关键帧图像和所述待优化地图点匹配时的特征点，π_(·)(R_kXⁱ+t_k)为所述待优化地图点投影到所述关键帧图像上的坐标；

所述联合目标函数为：

4.根据权利要求1所述的方法，其特征在于，所述获取包含机器人距离信息的当前帧图像包括：

获取所述超宽带基站的参数信息，所述参数信息包括所述距离信息及时间信息；

将所述超宽带基站的所述参数信息和相邻的图像进行数据同步，以得到包含所述机器人距离信息的所述当前帧图像。

5.根据权利要求1所述的方法，其特征在于，所述结合所述超宽带基站的距离代价函数对所述关键帧图像进行优化之后包括：

对优化后的所述关键帧图像进行回环检测，以更新局部地图。

6.根据权利要求1所述的方法，其特征在于，所述将所述当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点包括：

分别提取所述当前帧图像和所述相邻帧图像的特征点；

判断所述当前帧图像的将所述特征点和所述相邻帧图像的特征点之间是否匹配；

若判断为是，则根据所述当前帧图像的特征点生成所述局部地图点。

7.根据权利要求6所述的方法，其特征在于，所述提取所述当前帧图像的特征点之后进一步包括：

判断所述当前帧图像是否初始化成功；

若判断为否，则初始化所述当前帧图像作为参考帧图像。

8.一种机器人定位装置，其特征在于，所述装置包括处理器及存储器，所述处理器连接所述存储器；

其中，所述处理器用于获取包含机器人距离信息的当前帧图像，所述距离信息由超宽带基站提供；将所述当前帧图像和相邻帧图像进行特征匹配，以生成局部地图点；判断所述当前帧图像是否为关键帧图像；若判断为是，则利用联合目标函数对所述关键帧图像进行优化，所述联合目标函数至少包括所述超宽带基站的距离代价函数及视觉残差代价函数之和。

9.根据权利要求8所述的装置，其特征在于，所述超宽带基站的距离代价函数为：

E_k(p_k)＝∑_kρ(||p^uwb-p_k||-d^k) (2)

其中，p^uwb为所述超宽带基站的坐标，p_k为预测的所述超宽带基站标签的坐标，Np为局部优化内所有所述超宽带基站位置数据的集合，d^k为所述超宽带基站测得的所述机器人的实际位置，ρ为优化核函数。

10.一种存储装置，其特征在于，存储有能够实现如权利要求1-7中任一项所述方法的程序文件。

Images