CN114359338A - 位姿估计方法、装置、终端设备及计算机可读存储介质 - Google Patents

Abstract

本申请适用于机器人技术领域，提供了一种位姿估计方法、装置、终端设备及计算机可读存储介质，包括：检测第一目标视频中的特征点和特征线段，其中，目标视频通过安装于所述机器人上的拍摄装置获取；对所述第一目标视频中的特征点进行跟踪处理，获得第一点特征信息；对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息；获取所述第一目标视频对应时间段内的第一惯导数据，其中，惯导数据通过安装于所述机器人上的惯性传感器获取；根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。通过上述方法，可以有效提高机器人的位姿估计精度。

Description

位姿估计方法、装置、终端设备及计算机可读存储介质

技术领域

本申请属于机器人技术领域，尤其涉及位姿估计方法、装置、终端设备及计算机可读存储介质。

背景技术

即时定位与地图构建(simultaneous localization and mapping，SLAM)技术广泛应用于机器人。当机器人放入未知环境中的未知位置，随着机器人的移动，通过SLAM技术能够逐步描绘出当前环境的地图。

在SLAM过程中，需要随着机器人的移动，实时估计机器人的位姿。现有技术中，通过安装在机器人上的摄像头获取连续的图像帧，从连续的图像帧中检测图像特征，再根据图像特征估计机器人的位姿。但是拍摄图像容易受到环境光的影响，尤其在机器人高速运动时，容易出现丢帧和果冻效应等情况，进而影响位姿估计的精度。

发明内容

本申请实施例提供了一种位姿估计方法、装置、终端设备及计算机可读存储介质，可以有效提高机器人的位姿估计精度。

第一方面，本申请实施例提供了一种位姿估计方法，包括：

检测第一目标视频中的特征点和特征线段，其中，目标视频通过安装于所述机器人上的拍摄装置获取；

对所述第一目标视频中的特征点进行跟踪处理，获得第一点特征信息；

对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息；

获取所述第一目标视频对应时间段内的第一惯导数据，其中，惯导数据通过安装于所述机器人上的惯性传感器获取；

根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

本申请实施例中，通过对目标视频进行图像处理，获得视觉特征信息，即点特征信息和线特征信息；然后根据惯导数据和视觉特征信息估计机器人的位姿，相当于同时考虑了由惯导数据获得的位姿和由视觉检测获得的位姿。由于惯性传感器具有高频抗干扰特性，因此，根据惯导数据和视觉特征信息联合估计位姿，利于避免由于视觉影响导致的位姿估计误差；尤其当机器人运动速度较快时，能够有效防止特征数据的丢失，提高特征数据的准确性。另外，视觉特征信息中包括了点特征信息和线特征信息，通过不同类型的图像特征联合估计位姿，进一步提高了位姿估计的精度。

在第一方面的一种可能的实现方式中，所述检测第一目标视频中的特征点和特征线段，包括：

对于所述第一目标视频中的每一帧图像，根据所述第一惯导数据预测所述图像中特征点的位置和特征线段的位置；

根据预测出的所述图像中的特征点的位置和特征线段的位置确定所述图像中的检测范围；

在所述检测范围内检测所述图像中的特征点和特征线段。

在第一方面的一种可能的实现方式中，所述对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息，包括：

对所述第一目标视频的任意相邻两帧图像中的特征线段进行匹配处理，获得多个线段组，每个线段组中包括相匹配的两条特征线段；

删除所述多个线段组中满足预设条件的线段组，其中，所述线段组满足预设条件为所述线段组中的两条特征线段在图像上的位置差大于第一预设值、或角度差大于第二预设值，所述第一线特征信息包括剩余的线段组。

在第一方面的一种可能的实现方式中，在根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿之后，所述方法还包括：

当获取到第二目标视频，获取所述第二目标视频对应时间段内的第二惯导数据，其中，所述第二目标视频为在获取所述第一目标视频后的第一预设时间内获取到的视频；

对所述第二惯导数据进行预积分处理，获得预积分数据；

获取所述第二目标视频的第二点特征信息和第二线特征信息；

根据所述预积分数据、所述第二点特征信息和所述第二线特征信息对所述机器人的位姿进行局部优化处理。

在第一方面的一种可能的实现方式中，在获取所述第一目标视频对应时间段内的第一惯导数据之后，所述方法还包括：

获取所述第一目标视频对应时间段内的第一点云数据，其中，点云数据通过安装于所述机器人上的雷达获取，所述第一点云数据包括多个扫描点和每个扫描点的深度值；

将所述第一点云数据中的扫描点和所述第一目标视频中的特征点进行关联处理，获得所述第一目标视频中特征点的深度值；

根据所述第一目标视频中特征点的深度值、所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

在第一方面的一种可能的实现方式中，所述将所述第一点云数据中的扫描点和所述第一目标视频中的特征点进行关联处理，获得所述第一目标视频中特征点的深度值，包括：

将所述第一点云数据中的扫描点和所述第一目标视频中的特征点分别映射到预设平面；

获取所述预设平面内第一映射点的预设范围内的N个第二映射点，其中，所述第一映射点为所述第一目标视频中的特征点在所述预设平面内的映射点，所述第二映射点为所述第一点云数据中的扫描点在所述预设平面内的映射点，所述N为正整数；

若所述N个第二映射点之间的位置差小于第三预设值，则将所述N个第二映射点的平均深度值确定为所述第一映射点的深度值。

在第一方面的一种可能的实现方式中，在根据所述第一目标视频中特征点的深度值、所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿之后，所述方法还包括：

当获取到第三目标视频，对所述第三目标视频进行视觉回环检测，得到检测结果，其中，所述第三目标视频为在获取所述第一目标视频后的第二预设时间内获取到的视频；

若所述检测结果表示发生回环，则根据所述第一目标视频和所述第三目标视频对所述机器人的位姿进行全局优化处理。

第二方面，本申请实施例提供了一种位姿估计装置，包括：

特征检测单元，用于检测第一目标视频中的特征点和特征线段，其中，目标视频通过安装于所述机器人上的拍摄装置获取；

特征跟踪单元，用于对所述第一目标视频中的特征点进行跟踪处理，获得第一点特征信息；

特征匹配单元，用于对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息；

数据获取单元，用于获取所述第一目标视频对应时间段内的第一惯导数据，其中，惯导数据通过安装于所述机器人上的惯性传感器获取；

第一估计单元，用于根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

第三方面，本申请实施例提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如上述第一方面中任一项所述的位姿估计方法。

第四方面，本申请实施例提供了一种计算机可读存储介质，本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上述第一方面中任一项所述的位姿估计方法。

第五方面，本申请实施例提供了一种计算机程序产品，当计算机程序产品在终端设备上运行时，使得终端设备执行上述第一方面中任一项所述的位姿估计方法。

可以理解的是，上述第二方面至第五方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。

附图说明

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

图1是本申请实施例提供的位姿估计方法的流程示意图；

图2是本申请实施例提供的特征线段的示意图；

图3是本申请实施例提供的利用点线特征的双目视觉惯性里程计的框架示意图；

图4是本申请另一实施例提供的位姿估计方法的流程示意图；

图5是本申请实施例提供的全局位姿优化的示意图；

图6是本申请实施例提供的利用点线特征的双目视觉惯性里程计和雷达融合的框架示意图；

图7是本申请实施例提供的位姿估计装置的结构框图；

图8是本申请实施例提供的终端设备的结构示意图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定***结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的***、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。

应当理解，当在本申请说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

如在本申请说明书和所附权利要求书中所使用的那样，术语“若”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。

另外，在本申请说明书和所附权利要求书的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

在本申请说明书中描述的参考“一个实施例”或“一些实施例”等意味着在本申请的一个或多个实施例中包括结合该实施例描述的特定特征、结构或特点。由此，在本说明书中的不同之处出现的语句“在一个实施例中”、“在一些实施例中”、“在其他一些实施例中”、“在另外一些实施例中”等不是必然都参考相同的实施例，而是意味着“一个或多个但不是所有的实施例”，除非是以其他方式另外特别强调。

本申请实施例提供的位姿估计方法可以应用于即时定位与地图构建(simultaneous localization and mapping，SLAM)的应用场景中。在该应用场景中，机器人一边移动，一边描绘出当前环境的地图。机器人上安装有拍摄装置、惯性传感器和雷达。通过下述位姿估计方法，能够实时估计出机器人的位姿，进而根据机器人的位姿将检测出的视频中各帧图像上的特征点/线在三维空间中的位置拼接在一起，以构建出当前环境的三维地图。

参见图1，是本申请实施例提供的位姿估计方法的流程示意图，作为示例而非限定，所述方法可以包括以下步骤：

S101，检测第一目标视频中的特征点和特征线段。

其中，目标视频通过安装于所述机器人上的拍摄装置获取。

具体的，对于第一目标视频中的每一帧图像，进行特征点和特征线段的检测。其中，特征点可以是当前环境中物体上的关键点，如桌子的角点、钟表上表针的转动轴点等。特征线段可以是用于表征物体形状的线段，如桌子的边缘线、墙体的踢脚线等。

本申请实施例中，可以采用角点检测方法(Features from Accelerated SegmentTest，FAST)检测特征点，可以采用特征线检测方法(line segment detector，LSD)检测特征线段。

可选的，可以每获取预设时长的目标视频，执行一遍本申请实施例中的位姿估计方法。这里的预设时长可以根据实际需要预先设定。当预设时长较长时，位姿估计的实时性较差，但估计精度较高；反之，当预设时长较短时，位姿估计的实时性较好，但估计精度较低。

在一个实施例中，S101的一种实现方式包括：

对于第一目标视频中的每一帧图像，根据第一惯导数据预测图像中特征点的位置和特征线段的位置；根据预测出的图像中的特征点的位置和特征线段的位置确定图像中的检测范围；在检测范围内检测图像中的特征点和特征线段。

由于惯导数据的采样频率较高，可以利用惯导数据预测下一帧图像中的特征位置，然后在预测出的特征位置的一定范围内进一步检测特征点和特征线段。通过上述方法，能够缩小特征检测的范围，减少特征检测过程中的数据处理量，进而提高特征检测效率。

S102，对第一目标视频中的特征点进行跟踪处理，获得第一点特征信息。

对特征点进行跟踪，相当于将特征点作为目标，可以利用现有的目标跟踪算法对特征点进行跟踪。

本申请实施例中，可以采用光流跟踪方法对第一目标视频中的特征点进行跟踪。光流是空间运动物体在观察成像平面上的像素运动的瞬时速度。光流跟踪法是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息的一种方法。通过光流跟踪方法计算出的物体的运动信息，即可计算出机器人的运动轨迹。本申请实施例中的第一点特征信息可以包括用于计算机器人运动轨迹的信息，如速度、位置和移动方向等。

S103，对第一目标视频中的特征线段进行匹配处理，获得第一线特征信息。

本申请实施例中，可以利用描述子匹配方法对特征线段进行匹配处理。每条特征线段用一个描述子描述，然后将相邻两帧图像中具有相同描述子的特征线段匹配在一起。其中，描述子可以用来表示特征线段的位置特征和形状特征等，如特征线段是桌子某个位置上的边，特征线段是墙面某个墙角出的地面水平线。

本申请实施例中，可以采用两种线特征的描述子表示方法：普吕克坐标和正交坐标。普吕克坐标由6个参数确定，正交坐标由4个参数确定。普吕克坐标可以用于前端的坐标变换和重投影处理，正交坐标可以用于后端的优化处理。

在对特征线段匹配处理的过程中，选用的匹配条件可能较为宽松，这将增加了匹配成功的几率，同时降低了匹配精度。为了提高匹配精度，在一个实施例中，对特征线段进行匹配处理的一种实现方式包括：

对第一目标视频的任意相邻两帧图像中的特征线段进行匹配处理，获得多个线段组，每个线段组中包括相匹配的两条特征线段；删除多个线段组中满足预设条件的线段组，其中，线段组满足预设条件为线段组中的两条特征线段在图像上的位置差大于第一预设值、或角度差大于第二预设值，第一线特征信息包括剩余的线段组。

参见图2，是本申请实施例提供的特征线段的示意图。如图2所示，图中的δd表示两条特征线段之间的位置差，图中的δγ表示两条特征线段之间的角度差。

通过上述方法，相当于对匹配成功的特征线段进行筛选，删除角度、位置差异较大的特征线段的匹配关系，以此来提高匹配的精度。

通过对特征线段的匹配处理，可以通过相邻帧之间的特征线段的相关性来确定相邻帧之间物体的运动信息，进而计算出机器人的运动轨迹。本申请实施例中的第一线特征信息可以包括用于计算机器人运动轨迹的信息，如速度、位置和移动方向等。

S104，获取第一目标视频对应时间段内的第一惯导数据。

其中，惯导数据通过安装于机器人上的惯性传感器(Inertial MeasurementUnit，IMU)获取。IMU内部包含陀螺仪和加速度计，可以测量移动物体的加速度和角速度。在知道加速度和加速度信息之后可以通过运动学公式积分的到旋转、位姿和速度信息(P，v，q)。

如上所述，惯性传感器的采样频率通常较高，而拍摄装置的采样频率相对较低，由于惯导数据和拍摄装置的采样频率不一致，可能惯导数据和视觉数据的时间戳无法对其的情况。例如，采集到了10组惯导数据时，才获取了一帧图像。如果惯导数据和视觉数据的时间戳无法对其，那么后续无法进行数据融合。

为了解决该问题，本申请实施例中，可以先对惯导数据进行预积分处理。预积分处理的过程是，将一段时间内的惯导数据积分，得到一组惯导数据。通过预积分处理，可以将多组惯导数据积分为1组惯导数据，然后将该1组惯导数据与图像进行时间对齐。

S105，根据第一惯导数据、第一点特征信息和第一线特征信息估计机器人的位姿。

一种实现方式中，可以根据第一惯导数据、第一点特征信息和第一线特征信息分别估计出机器人的位姿，然后将估计出的多组位姿加权，获得最终的位姿。

为了提高位姿估计的精度，对位姿进行优化处理。在一个实施例中，在S105之后，所述方法还包括：

当获取到第二目标视频，获取第二目标视频对应时间段内的第二惯导数据，其中，第二目标视频为在获取所述第一目标视频后的第一预设时间内获取到的视频；对第二惯导数据进行预积分处理，获得预积分数据；获取第二目标视频的第二点特征信息和第二线特征信息；根据预积分数据、第二点特征信息和第二线特征信息对机器人的位姿进行局部优化处理。

其中，第一预设时间相当于一个滑动的时间窗口。在获取第一目标视频后时间窗口内，获取第二目标视频，利用第二目标视频进行优化处理。依次类推，将时间窗口向后滑动，每获取到最新的时间窗口内的目标视频，即利用最新获取到的目标视频进行优化处理。

对第二惯导数据进行预积分处理的过程可以参照S104实施例中的描述，在此不再赘述。

需要说明的是，如S104实施例中所述，IMU的采样频率高，通常为100Hz-1000Hz，数据量非常大。在做优化的时候，不可能将如此多的数据都放到状态变量中，因此通常的做法是每隔一段时间提取一个数据，比如每隔1秒提取一个。基本过程就是：已知第i秒的PVQ(旋转、位姿和速度)、以及第i秒和第j秒中间所有数据(如100个)，从第i秒一点一点积分得到第j秒的PVQ。但是这样在做后端优化的过程中，当进行迭代求解计算来更新和调整PVQ的值时，一旦某个时刻的PVQ进行了调整，每一个中间过程以及后面所有的轨迹都要重新再积分算一遍，如果是100Hz，两秒之间有100个采集数据，就要计算100次积分。预积分的目的就是尝试将这100次积分过程变成只有1次积分，或者说用1个值来代替100个值，通过预积分模型的应用可以大大节省了计算量。

可选的，根据惯导数据和视觉数据联合优化的目标函数为：

其中{r_p,J_p}表示先验信息残差，通过滑窗边缘化可以得到。J_p是先前的优化之后所得的海森(Hessian)矩阵。ρ是用来抑制外点的鲁棒核函数。

是IMU在第k帧和第k+1的IMU预积分残差，

是滑窗(即第一预设时间)内所有IMU预积分测量集合。

是特征点n到第j帧图像的重投影误差，

是特征线段o到第j帧图像的重投影误差，

和

是相机观测到的特征点的集合和特征线段的集合。上述目标函数相当于最小二乘问题，本申请实施例中可以使用LM算法求解上述目标函数。示例性的，参见图3，是本申请实施例提供的利用点线特征的双目视觉惯性里程计的框架示意图。如图3所示，获取传感器数据、以及对获取到的数据进行预处理的过程可以由前端执行，而优化的过程可以由后端执行。本申请实施例中，前端和后端的数据处理任务可以由两条进程分别执行。初始化的过程可以采用视觉数据和惯导数据松融合的方式，得到优化后的初始状态量。前端对IMU数据进行预积分处理，对视觉图像分别进行线特征检测和点特征检测，并进行线特征匹配处理和点特征跟踪处理。后端优化时，根据滑动窗口内的惯导数据和视觉数据进行6自由度位姿的优化处理。

其中，三角化可以计算出图像中的点、线在三维空间中的深度信息，从而确定出点、线在三维空间中的位置。在建立环境的三维地图时，根据确定出的6自由度位姿，将不同帧图像中的特征点和特征线段在三维空间中的位置拼接在一起，得到三维地图。

图1实施例中，通过对目标视频进行图像处理，获得视觉特征信息，即点特征信息和线特征信息；然后根据惯导数据和视觉特征信息估计机器人的位姿，相当于同时考虑了由惯导数据获得的位姿和由视觉检测获得的位姿。由于惯性传感器具有高频抗干扰特性，因此，根据惯导数据和视觉特征信息联合估计位姿，利于避免由于视觉影响导致的位姿估计误差；尤其当机器人运动速度较快时，能够有效防止特征数据的丢失，提高特征数据的准确性。另外，视觉特征信息中包括了点特征信息和线特征信息，通过不同类型的图像特征联合估计位姿，进一步提高了位姿估计的精度。

在一个实施例中，为了进一步提高位姿估计精度，还可以增加特征数据，如雷达获取到的点云数据。参见图4，是本申请另一实施例提供的位姿估计方法的流程示意图。如图4所示，所述方法还包括：

S401，检测第一目标视频中的特征点和特征线段。

S402，对第一目标视频中的特征点进行跟踪处理，获得第一点特征信息。

S403，对第一目标视频中的特征线段进行匹配处理，获得第一线特征信息。

S404，获取第一目标视频对应时间段内的第一惯导数据。

其中，S401-S404步骤与S101-S104相同，具体可参见S101-S104实施例中的描述，在此不再赘述。

S405，获取第一目标视频对应时间段内的第一点云数据。

其中，点云数据通过安装于机器人上的雷达获取，第一点云数据包括多个扫描点和每个扫描点的深度值。

S406，将所述第一点云数据中的扫描点和所述第一目标视频中的特征点进行关联处理，获得所述第一目标视频中特征点的深度值。

将点云数据中的扫描点和目标视频中的特征点进行关联处理，实质是找到雷达数据和视觉数据的对应关系。可以预先标定雷达坐标系和相机坐标系之间的转换关系，根据该转换关系确定扫描点和特征点之间的映射关系。

在关联处理时，通常是将一个扫描点和一个特征点关联，但扫描点的数量通常较多，且部分区域扫描点较密集，如果将一个扫描点和一个特征点关联，可能存在由于关联错误导致的特征的的深度值不准确的问题，进而影响位姿估计精度。为了解决该问题，在一个实施例中，S406可以包括：

将第一点云数据中的扫描点和第一目标视频中的特征点分别映射到预设平面；获取预设平面内第一映射点的预设范围内的N个第二映射点；若N个第二映射点之间的位置差小于第三预设值，则将N个第二映射点的平均深度值确定为第一映射点的深度值。

其中，第一映射点为第一目标视频中的特征点在预设平面内的映射点，第二映射点为第一点云数据中的扫描点在预设平面内的映射点，N为正整数。预设平面可以是位于符合相机坐标系下的单位球坐标系中的平面。

通过上述方法，如果N个第二映射点之间的位置差小于第三预设值，说明这N个第二映射点距离较近，取其平均深度值作为第一映射点的深度值，可以避免由于关联错误导致的特征的的深度值不准确的问题，进而影响位姿估计精度。

S407，根据第一目标视频中特征点的深度值、第一惯导数据、第一点特征信息和第一线特征信息估计机器人的位姿。

一种实现方式中，可以根据第一目标视频中特征点的深度值、第一惯导数据、第一点特征信息和第一线特征信息分别估计出机器人的位姿，然后将估计出的多组位姿加权，获得最终的位姿。

在一个实施例中，在S407之后，所述方法还包括：

当获取到第三目标视频，对第三目标视频进行视觉回环检测，得到检测结果；若检测结果表示发生回环，则根据第一目标视频和第三目标视频对机器人的位姿进行全局优化处理。

其中，第三目标视频为在获取第一目标视频后的第二预设时间内获取到的视频。

视觉回环检测用于判断当前帧是否和历史帧相似，通过回环检测，可以判断出机器人是否经过相同的地方。如果检测到回环，就可以为后端的位姿图提供更多有效的数据，使之得到更好的估计，特别是得到一个全局一致的估计，可以利用回环检测进行重定位或位姿优化。

当检测关键帧数据库中(该数据库中存储了历史帧)与滑窗内被边缘化的图像发生回环时，***会对数据库中全部关键帧执行位姿图优化。参见图5，是本申请实施例提供的全局位姿优化的示意图。如图5所示，虚线框内是被边缘化的帧，虚线代表发生了回环，即回环边，经过全局位姿图优化后，重新修正了位姿。

本申请实施例中，可以采用视觉BRIEF描述符为基础建立的DBoW2词袋模型。***会对新来的关键帧补充新检测的若干个角点，同时，对所有的特征点进行BRIEF描述符计算。***计算出当前帧图像与历史帧的相似度，满足阈值分数，即判定发生回环。当发生回环，***根据当前已获取到的所有帧图像进行位姿图优化，即全局图优化。

其中，相似度计算的一种方式可以为：随着机器人的移动，可以保存历史帧图像中的特征点信息，将历史特征点信息与当前帧中特征点信息匹配处理；若相匹配的特征点的个数与当前帧中特征点的总个数的比值，将该比值作为相似度。若相似度满足预设分数，说明当前帧与历史帧相似，即发生回环。之后可以根据历史特征点修正当前特征点，即全局图优化过程。

在匹配历史特征点和当前帧特征点时，可以利用BRIEF描述符描述特征点，将描述符相同的特征点确定为相匹配的特征点。

与图1实施例中的局部优化不同，局部优化是根据滑动时间窗口内的最新多种数据进行优化，而全局图优化是当前获取到的所有视觉数据进行优化。另外，局部优化采用的是多种数据，包括视觉数据、惯导数据和点云数据；而全局图优化采用的是视觉数据。局部优化的频率相对较高，而全局图优化结果的准确度相对较高。实际应用中，***可以按照滑动时间窗口定时进行局部优化，并在检测到回环时采用全局图优化。

需要说明的是，上述S105步骤之后，也可以执行上述的视觉回环检测、以及全局优化的步骤。在S407之后，也可以执行如上述的局部优化处理的步骤。

由于初始化时已经对齐重力方向到惯性坐标系，故***的横滚角φ、俯仰角θ可以观测到，即roll角、pitch角可观。因此在全局位姿图优化时，只需优化位置(x,y,z)以及偏航角ψ，即yaw角，总共4个自由度。则第i帧和第j帧的残差可定义为：

其中，

分别表示视觉惯性里程计估计得到的横滚角和俯仰角。

分别表示第i帧与第j帧之间的序列边，序列边即滑动窗口中两个关键帧之间的相对转换。同时，回环边表示回环检测正确的两帧之间的4自由度的位姿变换。最终将所有序列边

和回环边

进行优化。

则全局图优化目标函数为：

其中，ρ表示鲁棒核函数。

通过上述回环检测，可以修正累积误差，在后续优化过程中加入回环的约束，可以有效的提高优化结果的精度。

示例性的，参见图6，是本申请实施例提供的利用点线特征的双目视觉惯性里程计和雷达融合的框架示意图。如图6所示，在图3实施例的基础上，添加了雷达点云数据，增加了数据类型，进一步提高了检测精度。通过将点云和点特征关联，可以获取点特征的更精确的深度值。本申请实施例中，添加了视觉回环检测，用于进行全局位姿图优化，获得4个自由度的位姿(上述的特征线的4参数正交表示可以用于该全局位姿图优化)。

在SLAM过程中，可以优先考虑通过点云和点特征关联确定出的点特征的深度值。当未获取到点云数据时，再通过三角化处理获取点特征的深度值。

图4实施例中，通过对目标视频进行图像处理，获得视觉特征信息，即点特征信息和线特征信息；通过点云数据和特征点的关联，能够获取每个特征点更精确的深度值；然后根据特征点的深度值、惯导数据和视觉特征信息估计机器人的位姿，相当于同时考虑了由雷达获取到的深度信息、由惯导数据获得的位姿和由视觉检测获得的位姿；通过上述方法，将不同类型的检测数据进行融合，利用不同类型的检测数据综合估计机器人的位姿，大大提高了位姿估计的精度。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。

对应于上文实施例所述的位姿估计方法，图7是本申请实施例提供的位姿估计装置的结构框图，为了便于说明，仅示出了与本申请实施例相关的部分。

参照图7，该装置包括：

特征检测单元701，用于检测第一目标视频中的特征点和特征线段，其中，目标视频通过安装于所述机器人上的拍摄装置获取。

特征跟踪单元702，用于对所述第一目标视频中的特征点进行跟踪处理，获得第一点特征信息。

特征匹配单元703，用于对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息。

数据获取单元704，用于获取所述第一目标视频对应时间段内的第一惯导数据，其中，惯导数据通过安装于所述机器人上的惯性传感器获取。

第一估计单元705，用于根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

可选的，特征检测单元701还用于：

对于所述第一目标视频中的每一帧图像，根据所述第一惯导数据预测所述图像中特征点的位置和特征线段的位置；

根据预测出的所述图像中的特征点的位置和特征线段的位置确定所述图像中的检测范围；

在所述检测范围内检测所述图像中的特征点和特征线段。

可选的，特征匹配单元703还用于：

对所述第一目标视频的任意相邻两帧图像中的特征线段进行匹配处理，获得多个线段组，每个线段组中包括相匹配的两条特征线段；

删除所述多个线段组中满足预设条件的线段组，其中，所述线段组满足预设条件为所述线段组中的两条特征线段在图像上的位置差大于第一预设值、或角度差大于第二预设值，所述第一线特征信息包括剩余的线段组。

可选的，装置7还包括：

局部优化单元706，用于在根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿之后，当获取到第二目标视频，获取所述第二目标视频对应时间段内的第二惯导数据，其中，所述第二目标视频为在获取所述第一目标视频后的第一预设时间内获取到的视频；对所述第二惯导数据进行预积分处理，获得预积分数据；获取所述第二目标视频的第二点特征信息和第二线特征信息；根据所述预积分数据、所述第二点特征信息和所述第二线特征信息对所述机器人的位姿进行局部优化处理。

可选的，装置7还包括：

点云获取单元707，用于在获取所述第一目标视频对应时间段内的第一惯导数据之后，获取所述第一目标视频对应时间段内的第一点云数据，其中，点云数据通过安装于所述机器人上的雷达获取，所述第一点云数据包括多个扫描点和每个扫描点的深度值。

特征关联单元708，用于将所述第一点云数据中的扫描点和所述第一目标视频中的特征点进行关联处理，获得所述第一目标视频中特征点的深度值。

第二估计单元709，用于根据所述第一目标视频中特征点的深度值、所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

可选的，特征关联单元708还用于：

将所述第一点云数据中的扫描点和所述第一目标视频中的特征点分别映射到预设平面；

获取所述预设平面内第一映射点的预设范围内的N个第二映射点，其中，所述第一映射点为所述第一目标视频中的特征点在所述预设平面内的映射点，所述第二映射点为所述第一点云数据中的扫描点在所述预设平面内的映射点，所述N为正整数；

若所述N个第二映射点之间的位置差小于第三预设值，则将所述N个第二映射点的平均深度值确定为所述第一映射点的深度值。

可选的，装置7还包括：

全局优化单元710，用于在根据所述第一目标视频中特征点的深度值、所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿之后，当获取到第三目标视频，对所述第三目标视频进行视觉回环检测，得到检测结果，其中，所述第三目标视频为在获取所述第一目标视频后的第二预设时间内获取到的视频；若所述检测结果表示发生回环，则根据所述第一目标视频和所述第三目标视频对所述机器人的位姿进行全局优化处理。

需要说明的是，上述装置/单元之间的信息交互、执行过程等内容，由于与本申请方法实施例基于同一构思，其具体功能及带来的技术效果，具体可参见方法实施例部分，此处不再赘述。

另外，图7所示的位姿估计装置可以是内置于现有的终端设备内的软件单元、硬件单元、或软硬结合的单元，也可以作为独立的挂件集成到所述终端设备中，还可以作为独立的终端设备存在。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述***中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

图8是本申请实施例提供的终端设备的结构示意图。如图8所示，该实施例的终端设备8包括：至少一个处理器80(图8中仅示出一个)处理器、存储器81以及存储在所述存储器61中并可在所述至少一个处理器80上运行的计算机程序82，所述处理器80执行所述计算机程序82时实现上述任意各个位姿估计方法实施例中的步骤。

所述终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。该终端设备可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，图8仅仅是终端设备8的举例，并不构成对终端设备8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如还可以包括输入输出设备、网络接入设备等。

所称处理器80可以是中央处理单元(Central Processing Unit，CPU)，该处理器80还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器81在一些实施例中可以是所述终端设备8的内部存储单元，例如终端设备8的硬盘或内存。所述存储器81在另一些实施例中也可以是所述终端设备8的外部存储设备，例如所述终端设备8上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器81还可以既包括所述终端设备8的内部存储单元也包括外部存储设备。所述存储器81用于存储操作***、应用程序、引导装载程序(BootLoader)、数据以及其他程序等，例如所述计算机程序的程序代码等。所述存储器81还可以用于暂时地存储已经输出或者将要输出的数据。

本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现可实现上述各个方法实施例中的步骤。

本申请实施例提供了一种计算机程序产品，当计算机程序产品在终端设备上运行时，使得终端设备执行时实现可实现上述各个方法实施例中的步骤。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质至少可以包括：能够将计算机程序代码携带到装置/终端设备的任何实体或装置、记录介质、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random AccessMemory)、电载波信号、电信信号以及软件分发介质。例如U盘、移动硬盘、磁碟或者光盘等。在某些司法管辖区，根据立法和专利实践，计算机可读介质不可以是电载波信号和电信信号。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (10)

1.一种位姿估计方法，其特征在于，应用于机器人，所述方法包括：

检测第一目标视频中的特征点和特征线段，其中，目标视频通过安装于所述机器人上的拍摄装置获取；

对所述第一目标视频中的特征点进行跟踪处理，获得第一点特征信息；

对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息；

获取所述第一目标视频对应时间段内的第一惯导数据，其中，惯导数据通过安装于所述机器人上的惯性传感器获取；

根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

2.如权利要求1所述的位姿估计方法，其特征在于，所述检测第一目标视频中的特征点和特征线段，包括：

对于所述第一目标视频中的每一帧图像，根据所述第一惯导数据预测所述图像中特征点的位置和特征线段的位置；

根据预测出的所述图像中的特征点的位置和特征线段的位置确定所述图像中的检测范围；

在所述检测范围内检测所述图像中的特征点和特征线段。

3.如权利要求1所述的位姿估计方法，其特征在于，所述对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息，包括：

对所述第一目标视频的任意相邻两帧图像中的特征线段进行匹配处理，获得多个线段组，每个线段组中包括相匹配的两条特征线段；

删除所述多个线段组中满足预设条件的线段组，其中，所述线段组满足预设条件为所述线段组中的两条特征线段在图像上的位置差大于第一预设值、或角度差大于第二预设值，所述第一线特征信息包括剩余的线段组。

4.如权利要求1所述的位姿估计方法，其特征在于，在根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿之后，所述方法还包括：

当获取到第二目标视频，获取所述第二目标视频对应时间段内的第二惯导数据，其中，所述第二目标视频为在获取所述第一目标视频后的第一预设时间内获取到的视频；

对所述第二惯导数据进行预积分处理，获得预积分数据；

获取所述第二目标视频的第二点特征信息和第二线特征信息；

根据所述预积分数据、所述第二点特征信息和所述第二线特征信息对所述机器人的位姿进行局部优化处理。

5.如权利要求1所述的位姿估计方法，其特征在于，在获取所述第一目标视频对应时间段内的第一惯导数据之后，所述方法还包括：

获取所述第一目标视频对应时间段内的第一点云数据，其中，点云数据通过安装于所述机器人上的雷达获取，所述第一点云数据包括多个扫描点和每个扫描点的深度值；

将所述第一点云数据中的扫描点和所述第一目标视频中的特征点进行关联处理，获得所述第一目标视频中特征点的深度值；

根据所述第一目标视频中特征点的深度值、所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

6.如权利要求5所述的位姿估计方法，其特征在于，所述将所述第一点云数据中的扫描点和所述第一目标视频中的特征点进行关联处理，获得所述第一目标视频中特征点的深度值，包括：

将所述第一点云数据中的扫描点和所述第一目标视频中的特征点分别映射到预设平面；

获取所述预设平面内第一映射点的预设范围内的N个第二映射点，其中，所述第一映射点为所述第一目标视频中的特征点在所述预设平面内的映射点，所述第二映射点为所述第一点云数据中的扫描点在所述预设平面内的映射点，所述N为正整数；

若所述N个第二映射点之间的位置差小于第三预设值，则将所述N个第二映射点的平均深度值确定为所述第一映射点的深度值。

7.如权利要求5所述的位姿估计方法，其特征在于，在根据所述第一目标视频中特征点的深度值、所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿之后，所述方法还包括：

当获取到第三目标视频，对所述第三目标视频进行视觉回环检测，得到检测结果，其中，所述第三目标视频为在获取所述第一目标视频后的第二预设时间内获取到的视频；

若所述检测结果表示发生回环，则根据所述第一目标视频和所述第三目标视频对所述机器人的位姿进行全局优化处理。

8.一种位姿估计装置，其特征在于，应用于机器人，所述装置包括：

特征检测单元，用于检测第一目标视频中的特征点和特征线段，其中，目标视频通过安装于所述机器人上的拍摄装置获取；

特征跟踪单元，用于对所述第一目标视频中的特征点进行跟踪处理，获得第一点特征信息；

特征匹配单元，用于对所述第一目标视频中的特征线段进行匹配处理，获得第一线特征信息；

数据获取单元，用于获取所述第一目标视频对应时间段内的第一惯导数据，其中，惯导数据通过安装于所述机器人上的惯性传感器获取；

第一估计单元，用于根据所述第一惯导数据、所述第一点特征信息和所述第一线特征信息估计所述机器人的位姿。

9.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述的方法。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的方法。

Images