一种位姿预测方法、位姿预测装置及机器人

技术领域

本申请属于视觉算法技术领域，尤其涉及一种位姿预测方法、位姿预测装置、机器人及计算机可读存储介质。

背景技术

在机器人的定位建图过程中，需要获得机器人准确的位姿预测。现有技术中，往往依赖于视觉传感器，采用基于特征点匹配的特征点法或基于光度一致的直接法来构建约束条件，实现对机器人的位姿预测。但是在某些室内场景下，比如低纹理环境或暗光环境时，采用基于特征点匹配的特征点法难以提取到有效的特征点，而基于光度一致的直接法的使用性也大大下降。也即，在弱纹理和暗光场景下，可能无法对机器人的位姿进行精确预测。

发明内容

本申请提供了一种位姿预测方法、位姿预测装置、机器人及计算机可读存储介质，可实现更加精确及鲁棒的机器人的位姿预测。

第一方面，本申请提供了一种位姿预测方法，上述位姿预测方法应用于设置有双目摄像头的机器人，上述双目摄像头包括第一摄像头及第二摄像头，上述位姿预测方法包括：

在第一图像及第二图像中，查找出相匹配的至少一对特征点，其中，每一对特征点包含一个第一特征点及一个第二特征点，上述第一图像基于上述第一摄像头当前时刻所采集的图像而得，上述第二图像基于上述第二摄像头当前时刻所采集的图像而得，上述第一特征点在上述第一图像内，上述第二特征点在上述第二图像内；

在上述第一图像及上述第二图像中，查找出相匹配的至少一对线特征，其中，每一对线特征包含一条第一线特征及一条第二线特征，上述第一线特征在上述第一图像内，上述第二线特征在上述第二图像内；

基于上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据，获得上述机器人的预测位姿。

第二方面，本申请提供了一种位姿预测装置，上述位姿预测装置应用于设置有双目摄像头的机器人，上述双目摄像头包括第一摄像头及第二摄像头，上述位姿预测装置包括：

第一查找单元，用于在第一图像及第二图像中，查找出相匹配的至少一对特征点，其中，每一对特征点包含一个第一特征点及一个第二特征点，上述第一图像基于上述第一摄像头当前时刻所采集的图像而得，上述第二图像基于上述第二摄像头当前时刻所采集的图像而得，上述第一特征点在上述第一图像内，上述第二特征点在上述第二图像内；

第二查找单元，用于在上述第一图像及上述第二图像中，查找出相匹配的至少一对线特征，其中，每一对线特征包含一条第一线特征及一条第二线特征，上述第一线特征在上述第一图像内，上述第二线特征在上述第二图像内；

预测单元，用于基于上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据，获得上述机器人的预测位姿。

第三方面，本申请提供了一种机器人，上述机器人包括存储器、处理器、双目摄像头以及存储在上述存储器中并可在上述处理器上运行的计算机程序，上述处理器执行上述计算机程序时实现如上述第一方面的方法的步骤。

第四方面，本申请提供了一种计算机可读存储介质，上述计算机可读存储介质存储有计算机程序，上述计算机程序被处理器执行时实现如上述第一方面的方法的步骤。

第五方面，本申请提供了一种计算机程序产品，上述计算机程序产品包括计算机程序，上述计算机程序被一个或多个处理器执行时实现如上述第一方面的方法的步骤。

本申请与现有技术相比存在的有益效果是：对于设置有第一摄像头及第二摄像头(也即双目摄像头)的机器人来说，在第一图像及第二图像中，查找出相匹配的至少一对特征点，其中，每一对特征点包含一个第一特征点及一个第二特征点，上述第一图像基于第一摄像头当前时刻所采集的图像而得，上述第二图像基于上述第二摄像头当前时刻所采集的图像而得，上述第一特征点在上述第一图像内，上述第二特征点在上述第二图像内，同时还会在上述第一图像及上述第二图像中，查找出相匹配的至少一对线特征，其中，每一对线特征包含一条第一线特征及一条第二线特征，上述第一线特征在上述第一图像内，上述第二线特征在上述第二图像内，最后可基于上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据，获得上述机器人的预测位姿。上述过程结合了特征点和线特征来对机器人的位姿进行预测，可通过特征点和线特征的结合来获得机器人所处环境内的几何结构信息，使得机器人能够在有挑战性的弱纹理和低可见光照场景下也能够实现精确及鲁棒的位姿预测。可以理解的是，上述第二方面至第五方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。

附图说明

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

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

图2是本申请实施例提供的拼接前的两条线段的示例图；

图3是本申请实施例提供的拼接后所得线段的示例图；

图4是本申请实施例提供的三维的线特征重投影残差的示意图；

图5是本申请实施例提供的位姿预测装置的结构示意图；

图6是本申请实施例提供的机器人的结构示意图。

具体实施方式

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

为了说明本申请所提出的技术方案，下面通过具体实施例来进行说明。

下面对本申请实施例提供的一种位姿预测方法进行描述。该位姿预测方法应用于设置有双目摄像头的机器人，为便于说明，记双目摄像头中的其中一个摄像头为第一摄像头，另一个摄像头为第二摄像头。仅作为示例，该第一摄像头可以是左目摄像头，该第二摄像头可以是右目摄像头。请参阅图1，该位姿预测方法包括：

步骤与101，在第一图像及第二图像中，查找出相匹配的至少一对特征点。

在本申请实施例中，第一摄像头及第二摄像头所采集的原始图像会先被机器人转换为灰度图像，并对该灰度图像进行一系列预处理，以提升后续的数据处理效率。举例来说，预处理操作可以是高斯模糊操作，以减少灰度图像的噪声，使得灰度图像更加平滑，降低灰度图像的细节层次。预处理后的灰度图像即可投入本申请实施例的各个步骤中进行使用。也即，第一图像指的是当前时刻基于该第一摄像头所采集的原始图像进行预处理操作后所得的灰度图像，而第二图像则指的是当前时刻基于该第二摄像头所采集的原始图像进行预处理操作后所得的灰度图像。

对于一对特征点，也即一特征点对来说，该特征点对由第一特征点及第二特征点这两个特征点所构成，其中，第一特征点在第一图像内，第二特征点在第二图像内。也即，第一图像中的一个特征点，和第二图像中的与该特征点相匹配的特征点构成了一对特征点。

在一些实施例中，机器人可先从第一图像中提取出至少一个特征点，然后再在第二图像中通过光流匹配的方式查找出与第一图像的每个特征点相匹配的特征点，以获得至少一对特征点。需要注意的是，第二图像中的一个特征点至多仅能与第一图像中的一个特征点相匹配，同样，第一图像中的一个特征点也至多仅能与第二图像中的一个特征点相匹配；也即，不同的特征点对中，不会有特征点发生重复。为实现特征点的快速提取，机器人将首先判断当前所获得的第一图像是否为初始的第一图像，也即，该第一图像是否基于第一摄像头启动后所采集到的第一帧图像而得；若该第一图像是初始的第一图像，则可基于预设的第一特征点提取方式提取该第一图像的特征点，其中，该第一特征点提取方式与角点相关；若第一图像不是初始的第一图像，则可基于预设的第二特征点提取方式提取该第一图像的特征点，其中，该第二特征点提取方式与光流相关。下面对该第一特征点提取方式及第二特征点提取方式进行简单介绍：

第一特征点提取方式指的是：对第一图像进行FAST(Features from AcceleratedSegment Test)角点检测，将提取到的角点作为特征点。考虑到通常情况下，所提取到的角点的数量较多，甚至可能达到上千个，因而，此处设定有一特征点数量阈值N，也即，可从提取到的角点中基于各角点的响应值而选定N个角点作为特征点。考虑到特征点太少可能会导致所预测的位姿不准确，特征点太多可能会增加机器人的系统运算量，因而可设定N为[70,100]这一区间中的任一整数。

第二特征点提取方式指的是：对第一图像的前一帧图像所获得的特征点进行光流跟踪，获得该第一图像的M个特征点。需要注意的是，这里所说的第一图像的前一帧图像，指的是基于第一摄像头在前一时刻所获得的原始图像进行预处理而得的第一图像，也即前一帧第一图像。举例来说，i时刻基于第一摄像头获得了第一图像A1，i+1时刻基于第一摄像头获得了第一图像A2，那么即可认为第二图像A1就是第一图像A2的前一帧图像。考虑到机器人处于移动状态时，第一图像A1的N个特征点难以都在第二图像A2中出现，因而，在对第一图像A1的N个特征点进行光流跟踪后所得到的第二图像A2中的特征点的数量为M个，且M≤N。

在一些实施例中，可能出现M远小于N的情况下，例如

步骤102，在上述第一图像及上述第二图像中，查找出相匹配的至少一对线特征。

在本申请实施例中，机器人还会在第一图像及第二图像中，查找出相匹配的至少一对线特征，其中，每一对线特征包含一条第一线特征及一条第二线特征，该第一线特征在第一图像内，该第二线特征在第二图像内。也即，第一图像中的一条线特征，和第二图像中的与该线特征相匹配的线特征构成了一对线特征。示例性地，机器人可先分别在第一图像及第二图像进行线特征提取操作，获得第一图像中所提取到的至少一条线特征(记作第三线特征)，以及第二图像中所提取到的至少一条线特征(记作第四线特征)；然后，机器人可将每条第三线特征分别与各条第四线特征行匹配操作，并基于相匹配的第三线特征及第四线特征来获得至少一对线特征。与步骤101类似，不同的线特征对中，不会有线特征发生重复。

针对任一图像，该线特征提取操作可通过如下方式实现：先基于预设的直线提取算法，提取出图像中的线段，其中，该直线提取算法可以是LSD(Line Segment Detector)算法，也可以是其它算法，此处不作限定。为了避免线特征的误提取，提升后续的线特征处理效率，此处仅考虑长于预设长度的线段，例如，可仅考虑像素长度超过30个像素的线段。又考虑到在图像噪声或局部模糊的影响下，一条长线段可能在提取过程中被提取为了多条短线段，因而，机器人还会先检测是否有相邻的两条线段处于同一直线，并将处于同一直线的相邻的两条线段进行拼接，以保障线段跟踪的连续性。其中，相邻的两条线段指的是：一条线段的终点在另一条线段的起点的预设距离内。需要注意的是，线段的终点和起点是由直线提取算法而判断的，此处不作赘述。在上述过程完成后，所获得的图像中的线段即为该图像的线特征。

示例性地，机器人可通过如下方式判断相邻的两条线段是否处于同一直线：将相邻的两条线段分别记作第一线段及第二线段，并分别计算第一线段的法向量及第二线段的法向量；若第一线段的法向量与第二线段的法向量的误差在预设误差范围内，则确定该第一线段及该第二线段处于同一直线。请参阅图2及图3，图2给出了处于同一直线的相邻的两条线段的示例，图3给出了对图2所示出的处于同一直线的相邻的两条线段进行拼接后所获得的新的线段的示例。可见，处于同一直线的相邻的两条线段(线段1及线段2)，被拼接为了新的线段3，该新的线段3即可作为图像的一条线特征。

在一些实施例中，机器人可通过LBD(Line Band Discriptor)描述子来描述所提取出的线特征，并基于LBD描述子来对两条线特征是否相匹配进行判断。

在一些实施例中，机器人在对两条线特征是否相匹配进行判断时，可能出现误匹配的情况。本申请实施例可通过如下方式来剔除可能出现误匹配的两条线特征：分别获取待检测的两条线特征的中点及斜率，以此来判断这两条线特征是否误匹配。

则针对每一对相匹配的第三线特征及第四线特征来说，可基于该第三线特征的中点、该第四线特征的中点、该第三线特征的斜率及该第四线特征的斜率，来判断该第三线特征与该第四线特征是否为误匹配，具体为：获取该第三线特征的中点在第一图像中的坐标，记作第一坐标；获取该第四线特征的中点在第二图像中的坐标，记作第二坐标；检测该第一坐标与该第二坐标的距离是否在预设的误差距离内；基于该第三线特征的斜率计算该第三线特征相对于图像坐标系的x轴的倾斜角度，记作第一角度；基于该第四线特征的斜率计算该第四线特征相对于图像坐标系的x轴的倾斜角度，记作第二角度；检测该第一角度与该第二角度的角度差值是否在预设的误差角度内；只有该第一坐标与该第二坐标的距离在预设的误差距离内，并且，该第一角度与该第二角度的角度差值在预设的误差角度内时，才认为该第三线特征及该第四线特征确实是相匹配的，可确定为一对线特征。

类似地，还可以通过上述方式来对第一图像中所提取出的线特征进行一次筛除，具体为：基于光流跟踪的方式，确定第一图像的前一帧图像所获得的线特征与该第一图像所获得的线特征的匹配关系。为便于说明，在一个匹配关系中，将第一图像的前一帧图像所获得的线特征记作第五线特征，将该第一图像所获得的线特征记作第六线特征；由于机器人通常不会在短时间内出现大幅度的移动，因而，针对每一对相匹配的第五线特征及第六线特征来说，可基于该第五线特征的中点、该第六线特征的中点、该第五线特征的斜率及该第六线特征的斜率，来判断该第五线特征与该第六线特征是否为误匹配，具体为：获取该第五线特征的中点在该第一图像的前一帧图像中的坐标，记作第三坐标；获取该第六线特征的中点在该第一图像中的坐标，记作第四坐标；检测该第三坐标与该第四坐标的距离是否在预设的误差距离内；基于第五线特征的斜率计算第五线特征相对于图像坐标系的x轴的倾斜角度，记作第三角度；基于第六线特征的斜率计算第六线特征相对于图像坐标系的x轴的倾斜角度，记作第四角度；检测该第三角度与该第四角度的角度差值是否在预设的误差角度内；只有该第三坐标与该第四坐标的距离在预设的误差距离内，并且，该第三角度与该第四角度的角度差值在预设的误差角度内时，才确定该第五线特征及该第六线特征确实是相匹配的，此时，可保留该第六线特征；反之，可确定该第五线特征及该第六线特征不相匹配，则可将该第六线特征从该第一图像中剔除，以实现对第一图像中所提取到的线特征的筛选。

在一些实施例中，考虑到不是所有第一图像中的线特征均可在第二图像中找到相匹配的线特征，也即，可能有一些第一图像中的线特征无法在第二图像中找到相匹配的线特征，因而，对于第一图像中的线特征来说，可被划分为两类：第一类是能够在第二图像有所匹配的线特征，第二类是不能在第二图像中有所匹配的线特征。对于第一类，可直接基于第二图像来获取与自身相匹配的线特征，以获得线特征对。对于第二类，可基于第一摄像头所获得的该第一图像的前一帧图像来获取与自身相匹配的线特征，以获得线特征对。通过上述方式，可使得第一图像筛选后所保留的各个线特征均能够形成对应的一对线特征。为便于说明，本申请实施例可将第一类的线特征记作双目线特征，将第二类的线特征记作非双目线特征，则在存在非双目线特征的情况下，机器人可在第一图像及第一图像的前一帧图像中，基于非双目线特征查找出相匹配的至少一对线特征，其中，以这种方式所形成的每一对线特征包含一条非双目第一线特征及一条非双目第二线特征，该非双目第一线特征在该第一图像内，该非双目第二线特征在该第一图像的前一帧图像内。这样一来，机器人最终所获得的线特征对不仅可能包括基于双目线特征、第一图像及第二图像所获得的至少一对线特征，还可能包括基于非双目线特征、第一图像及第一图像的前一帧图像所获得的至少一对线特征。

步骤103，基于上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据，获得上述机器人的预测位姿。

在本申请实施例中，机器人预先设定有一目标函数，该目标函数可基于步骤101所确定的至少一对特征点，步骤102所确定的至少一对线特征及机器人的惯性测量单元(Inertial Measurement Unit，IMU)所输出的惯性数据来进行约束，该目标函数具体为：

其中，B表示IMU所输出的惯性数据的集合；C表示由步骤101所获得的至少一对特征点构成的集合；L表示由步骤102所获得的至少一对线特征构成的集合；X表示机器人的系统状态估计值，其包含了机器人的位姿，以及特征点和线特征在空间中的位姿；z表示机器人的系统状态观测值；r表示系统状态观测值与系统状态估计值的差值，即系统状态差值。

需要注意的是，集合L可能包含有基于非双目线特征、第一图像及第一图像的前一帧图像所获得的线特征对。举例来说，机器人在第一图像中提取出了10条线特征；这10条线特征中，有1条线特征因无法在前一帧图像中找到相匹配的线特征而被剔除，仅剩余9条线特征；也即，这9条线特征均可在前一帧图像中找到相匹配的线特征，记作线特征1、2、3、4、5、6、7、8及9；通过与第二图像中所提取出的线特征进行匹配，发现线特征1、3及4无法与第二图像中的任一线特征匹配上，则线特征1、3及4将分别与前一帧图像中相匹配的线特征一起，构成3对线特征；线特征2、5、6、7、8及9将分别与第二图像中相匹配的线特征一起，构成6对线特征；至此，完成9对线特征的构建，这9对线特征即可构成集合L。

由上式可知，r具体包括三部分：

第一部分为IMU的积分值与真实值的残差。

第二部分为三维的特征点重投影回二维的图像坐标系后所得的坐标(也即像素位置)与真实观测到的该特征点在第一图像中的坐标的残差。其中，三维的特征点由一对特征点进行双目三角化而获得。

第三部分与第二部分的原理相似，为三维的线特征重投影残差。请参阅图4，该三维的线特征重投影残差指的是：三维的线特征由世界坐标系投影到第一摄像头及第二摄像头的归一化平面后所得的直线，与该归一化平面中原来的第一图像的线特征的起点和终点的垂直距离。其中，三维的线特征由一对线特征进行三角化而获得。

将这3个残差加在一起，构成上述目标函数。通过最小二乘方法紧耦合优化包含机器人的位姿的系统状态估计值X，使得整个目标函数的值最小，此时的X即为最优解，该最优解的X包含有当前机器人的位姿预测值。

在一些实施例中，考虑到三维的特征点及线特征也是目标函数的优化变量，二者在对目标函数进行迭代求解时一同被优化；也即，机器人的位姿、三维的特征点及三维的线特征在一同被迭代调整，使得目标函数最小。这样一来，最优解的X除了包含有当前机器人的位姿预测值之外，还包含有优化后的三维的特征点及线特征。机器人由此可将优化后的特征点和线特征在空间中的位置及其特征描述进行保存，构成一个词袋，保存为地图，用于回环检测及校正；同时，该地图也可以用于下一次导航定位时，做重定位使用。

由上可见，通过本申请实施例，结合了特征点和线特征来对机器人的位姿进行预测，可通过特征点和线特征的结合来获得机器人所处环境内的几何结构信息，使得机器人能够在有挑战性的弱纹理和低可见光照场景下也能够实现精确及鲁棒的位姿预测。

对应于前文所提出的位姿预测方法，本申请实施例提供了一种位姿预测装置，上述位姿预测装置应用于设置有双目摄像头的机器人，上述双目摄像头包括第一摄像头及第二摄像头。请参阅图5，本申请实施例中的位姿预测装置500包括：

第一查找单元501，用于在第一图像及第二图像中，查找出相匹配的至少一对特征点，其中，每一对特征点包含一个第一特征点及一个第二特征点，上述第一图像基于上述第一摄像头当前时刻所采集的图像而得，上述第二图像基于上述第二摄像头当前时刻所采集的图像而得，上述第一特征点在上述第一图像内，上述第二特征点在上述第二图像内；

第二查找单元502，用于在上述第一图像及上述第二图像中，查找出相匹配的至少一对线特征，其中，每一对线特征包含一条第一线特征及一条第二线特征，上述第一线特征在上述第一图像内，上述第二线特征在上述第二图像内；

预测单元503，用于基于上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据，获得上述机器人的预测位姿。

可选地，上述第一查找单元501，包括：

特征点提取子单元，用于从第一图像中提取出至少一个特征点；

特征点匹配子单元，用于在第二图像中，分别查找与上述第一图像中的各个特征点相匹配的特征点，以获得至少一对特征点。

可选地，上述特征点提取子单元，包括：

检测子单元，用于检测上述第一图像是否基于上述第一摄像头启动后所采集到的第一帧图像而得；

第一特征点提取子单元，用于若上述第一图像基于上述第一帧图像而得，则基于预设的第一特征点提取方式提取特征点，其中，上述第一特征点提取方式与角点相关；

第二特征点提取子单元，用于若上述第一图像不基于上述第一帧图像而得，则基于预设的第二特征点提取方式提取特征点，其中，上述第二特征点提取方式与光流相关。

可选地，上述第二查找单元502，包括：

线特征提取子单元，用于分别在上述第一图像及上述第二图像进行线特征提取操作，获得第三线特征以及第四线特征，其中，上述第三线特征为上述第一图像中所提取到的线特征，上述第四线特征为上述第二图像中所提取到的线特征；

线特征匹配子单元，用于将每条第三线特征分别与各条第四线特征行匹配操作；

线特征对获取子单元，用于基于相匹配的第三线特征及第四线特征，获得至少一对线特征。

可选地，上述线特征提取子单元，包括：

线段提取子单元，用于针对上述第一图像及上述第二图像中的任一图像，基于预设的直线提取算法，提取上述图像中超过预设长度的线段；

线段拼接子单元，用于将处于同一直线的相邻的两条线段进行拼接，其中，上述相邻的两条线段指的是：一条线段的终点在另一条线段的起点的预设距离内。

可选地，上述线特征对获取子单元，包括：

判断子单元，用于针对每一对相匹配的第三线特征及第四线特征，基于上述第三线特征的中点、上述第四线特征的中点、上述第三线特征的斜率及上述第四线特征的斜率，判断上述第三线特征与上述第四线特征是否为误匹配；

确定子单元，用于若上述第三线特征及上述第四线特征不为误匹配，则将上述第三线特征及上述第四线特征确定为一对线特征。

可选地，上述预测单元503，包括：

目标函数优化子单元，用于基于预设的优化方法对目标函数进行优化，其中，上述目标函数采用上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据作为约束条件；

预测位姿获取子单元，用于基于优化后的目标函数，获得上述机器人的预测位姿。

由上可见，通过本申请实施例，结合了特征点和线特征来对机器人的位姿进行预测，可通过特征点和线特征的结合来获得机器人所处环境内的几何结构信息，使得机器人能够在有挑战性的弱纹理和低可见光照场景下也能够实现精确及鲁棒的位姿预测。

本申请实施例还提供了一种机器人，请参阅图6，本申请实施例中的机器人6包括：存储器601，一个或多个处理器602(图6中仅示出一个)、双目摄像头603及存储在存储器601上并可在处理器上运行的计算机程序。其中，双目摄像头603包括第一摄像头及第二摄像头；存储器601用于存储软件程序以及单元，处理器602通过运行存储在存储器601的软件程序以及单元，从而执行各种功能应用以及数据处理，以获取上述预设事件对应的资源。具体地，处理器602通过运行存储在存储器601的上述计算机程序时实现以下步骤：

在第一图像及第二图像中，查找出相匹配的至少一对特征点，其中，每一对特征点包含一个第一特征点及一个第二特征点，上述第一图像基于上述第一摄像头当前时刻所采集的图像而得，上述第二图像基于上述第二摄像头当前时刻所采集的图像而得，上述第一特征点在上述第一图像内，上述第二特征点在上述第二图像内；

在上述第一图像及上述第二图像中，查找出相匹配的至少一对线特征，其中，每一对线特征包含一条第一线特征及一条第二线特征，上述第一线特征在上述第一图像内，上述第二线特征在上述第二图像内；

基于上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据，获得上述机器人的预测位姿。

假设上述为第一种可能的实施方式，则在第一种可能的实施方式作为基础而提供的第二种可能的实施方式中，上述在第一图像及第二图像中，查找出相匹配的至少一对特征点，包括：

从第一图像中提取出至少一个特征点；

在第二图像中，分别查找与上述第一图像中的各个特征点相匹配的特征点，以获得至少一对特征点。

在上述第二种可能的实施方式作为基础而提供的第三种可能的实施方式中，上述从第一图像中提取出至少一个特征点，包括：

检测上述第一图像是否基于上述第一摄像头启动后所采集到的第一帧图像而得；

若上述第一图像基于上述第一帧图像而得，则基于预设的第一特征点提取方式提取特征点，其中，上述第一特征点提取方式与角点相关；

若上述第一图像不基于上述第一帧图像而得，则基于预设的第二特征点提取方式提取特征点，其中，上述第二特征点提取方式与光流相关。

在上述第一种可能的实施方式作为基础而提供的第四种可能的实施方式中，上述在上述第一图像及上述第二图像中，查找出相匹配的至少一对线特征，包括：

分别在上述第一图像及上述第二图像进行线特征提取操作，获得第三线特征以及第四线特征，其中，上述第三线特征为上述第一图像中所提取到的线特征，上述第四线特征为上述第二图像中所提取到的线特征；

将每条第三线特征分别与各条第四线特征行匹配操作；

基于相匹配的第三线特征及第四线特征，获得至少一对线特征。

在上述第四种可能的实施方式作为基础而提供的第五种可能的实施方式中，针对上述第一图像及上述第二图像中的任一图像，上述线特征提取操作包括：

基于预设的直线提取算法，提取上述图像中超过预设长度的线段；

将处于同一直线的相邻的两条线段进行拼接，其中，上述相邻的两条线段指的是：一条线段的终点在另一条线段的起点的预设距离内。

在上述第四种可能的实施方式作为基础而提供的第六种可能的实施方式中，上述基于相匹配的第三线特征及第四线特征，获得至少一对线特征，包括：

针对每一对相匹配的第三线特征及第四线特征，基于上述第三线特征的中点、上述第四线特征的中点、上述第三线特征的斜率及上述第四线特征的斜率，判断上述第三线特征与上述第四线特征是否为误匹配；

若上述第三线特征及上述第四线特征不为误匹配，则将上述第三线特征及上述第四线特征确定为一对线特征。

在上述第一种可能的实施方式作为基础而提供的第七种可能的实施方式中，上述基于上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据，预测得到上述机器人的位姿，包括：

基于预设的优化方法对目标函数进行优化，其中，上述目标函数采用上述至少一对线特征、上述至少一对特征点及上述机器人的惯性测量单元所输出的惯性数据作为约束条件；

基于优化后的目标函数，获得上述机器人的预测位姿。

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

存储器601可以包括只读存储器和随机存取存储器，并向处理器602提供指令和数据。存储器601的一部分或全部还可以包括非易失性随机存取存储器。例如，存储器601还可以存储设备类别的信息。

由上可见，通过本申请实施例，结合了特征点和线特征来对机器人的位姿进行预测，可通过特征点和线特征的结合来获得机器人所处环境内的几何结构信息，使得机器人能够在有挑战性的弱纹理和低可见光照场景下也能够实现精确及鲁棒的位姿预测。

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

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

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者外部设备软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的系统实施例仅仅是示意性的，例如，上述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

上述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

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

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