一种位姿获取方法、装置及移动设备

技术领域

本申请涉及智能家居技术领域，尤其涉及一种位姿获取方法、装置及移动设备。

背景技术

同时定位与建图(Simultaneous Localization and Mapping，SLAM)技术成为当前机器人领域研究的热点，SLAM技术按照传感器的不同可以分为激光SLAM和视觉SLAM。其中，视觉SLAM是通过一张张连续运动的图像，推断机器人的位姿以及周围环境的情况。视觉SLAM具体是指搭载特定传感器的机器人，在没有环境先验信息的情况下，在运动过程中建立环境模型，并且估计自己的运动位姿。

而目前基于SLAM定位的算法都是基于环境是静态或者准静态的假设，也就是整个场景都是静态的。但是实际的机器人运行场景中大多数都是动态场景，相对运动的物体会对位姿获取造成干扰，因此会存在定位准确性较低的情况。

因此，亟需一种能够提高SLAM定位准确性的技术方案。

发明内容

有鉴于此，本申请提供一种位姿获取方法、装置及移动设备，用以解决现有技术中动态场景下的移动设备定位准确性较低的技术问题。

本申请提供了一种位姿获取方法，包括：

获得当前帧图像，所述当前帧图像为移动设备上的图像采集装置所采集到的图像；

获得所述当前帧图像中匹配出的特征点，其中，所述当前帧图像中匹配出的特征点为在所述当前帧图像对应的滑动窗口所包含的历史图像中有相匹配的特征点的特征点；所述滑动窗口中包含多帧历史图像，所述历史图像为所述当前帧图像之前的关键帧图像；

分别获得每个所述当前帧图像中匹配出的特征点的第一像素模型，所述第一像素模型对应于所述当前帧图像中匹配出的特征点的多个邻居特征点，且所述第一像素模型还具有所述第一像素模型的方向向量和所述当前帧图像中匹配出的特征点的重投影误差；

分别将每个所述当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，以得到模型比对结果，所述模型比对结果表征所述当前帧图像中匹配出的特征点对应的空间点是否属于运动物体；其中，所述第二像素模型为所述历史图像中与所述当前帧图像中匹配出的特征点相匹配的特征点的像素模型，所述第二像素模型对应于所述历史图像中匹配出的特征点的多个邻居特征点，且所述第二像素模型还具有所述第二像素模型的方向向量和所述历史图像中匹配出的特征点的重投影误差的均值；

根据所述模型比对结果，筛选出所述当前帧图像中的目标特征点，所述目标特征点为所述当前帧图像中对应的空间点不属于运动物体的特征点；

根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿。

上述方法，优选的，根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿，包括：

根据所述目标特征点对应的空间点的深度值，获得所述目标特征点对应的权重值；

根据所述目标特征点对应的权重值，获得所述目标特征点的重投影误差最小时对应的位姿变换矩阵，所述重投影误差根据所述目标特征点在所述历史图像中对应的三维坐标值和所述目标特征点在所述当前帧图像中的二维坐标值获得；

根据所述位姿变换矩阵，获得所述移动设备的当前位姿。

上述方法，优选的，分别获得每个所述当前帧图像中匹配出的特征点的第一像素模型，包括：

分别以每个所述当前帧图像中匹配出的特征点为中心，获得所述当前帧图像中匹配出的特征点的多个邻居特征点；

其中，所述当前帧图像中匹配出的特征点与其对应的邻居特征点的深度值不同，且所述当前帧图像中匹配出的特征点的邻居特征点为深度值大于目标深度且与所述当前帧图像中匹配出的特征点之间的距离满足距离排序规则的特征点，所述目标深度与所述当前帧图像中匹配出的特征点的邻域像素点的深度均值相关；

至少根据所述当前帧图像中匹配出的特征点的多个邻居特征点，建立所述当前帧图像中匹配出的特征点的第一像素模型，以使得所述第一像素模型对应于所述当前帧图像中匹配出的特征点的多个邻居特征点且还具有所述第一像素模型的方向向量和所述当前帧图像中匹配出的特征点的重投影误差。

上述方法，所述第一像素模型的方向向量基于所述第一像素模型中的邻居特征点对应的空间中心点的三维坐标值和所述当前帧图像中匹配出的特征点对应的空间点的三维坐标值获得，所述当前帧图像中匹配出的特征点的重投影误差基于所述历史图像获得。

上述方法，优选的，分别将每个所述当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，以得到模型比对结果，包括：

获得所述第一像素模型和其对应的第二像素模型中相匹配的邻居特征点的数量；

获得所述第一像素模型中的方向向量和其对应的第二像素模型中的方向向量之间的差值的模长；

判断所述第一像素模型中所述当前帧图像中匹配出的特征点的重投影误差是否小于或等于与所述第二像素模型中所述历史图像中匹配出的特征点对应的重投影误差的均值，以得到判断结果；所述均值为所述历史图像中匹配出的特征点在所述滑动窗口中累积重投影误差的均值；

根据所述数量以及所述模长和所述判断结果，获得模型比对结果。

上述方法，优选的，还包括：

获得针对于所述历史图像中的特征点的像素模型的更新请求；

获得所述滑动窗口内新加入的图像；

对所述新加入的图像进行特征点提取，以得到所述新加入的图像中的特征点；

将所述新加入的图像中的特征点与所述滑动窗口中的历史图像中的特征点进行匹配，以得到所述新加入的图像中与所述历史图像中的特征点相匹配的匹配特征点和所述新加入的图像中与所述历史图像中的特征点均不匹配的非匹配特征点；

根据所述匹配特征点，对所述历史图像中与所述匹配特征点相匹配的特征点的像素模型进行更新；

获得所述非匹配特征点的像素模型，以使得在所述新加入的图像作为所述滑动窗口中的历史图像时，所述非匹配特征点的像素模型作为所述历史图像中新的特征点的像素模型。

上述方法，优选的，在根据所述模型比对结果，筛选出所述当前帧图像中的目标特征点之后，在根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿之前，所述方法还包括：

在所述当前帧图像中的目标特征点中，筛选出与所述历史图像中的特征点满足极线约束的目标特征点。

上述方法，优选的，获得所述当前帧图像中匹配出的特征点，包括：

对所述当前帧图像中的特征点进行提取；

将所述当前帧图像中提取出的特征点与所述历史图像中提取到的特征点进行匹配，以得到所述当前帧图像中与所述历史图像中的特征点相匹配的特征点。

本申请还提供了一种位姿获取装置，包括：

图像获得单元，用于获得当前帧图像，所述当前帧图像为移动设备上的图像采集装置所采集到的图像；

特征点处理单元，用于获得所述当前帧图像中匹配出的特征点，其中，所述当前帧图像中匹配出的特征点为在所述当前帧图像对应的滑动窗口所包含的历史图像中有相匹配的特征点的特征点；所述滑动窗口中包含多帧历史图像，所述历史图像为所述当前帧图像之前的关键帧图像；

模型建立单元，用于分别获得每个所述当前帧图像中匹配出的特征点的第一像素模型，所述第一像素模型对应于所述当前帧图像中匹配出的特征点的多个邻居特征点，且所述第一像素模型还具有所述第一像素模型的方向向量和所述当前帧图像中匹配出的特征点的重投影误差；

模型比对单元，用于分别将每个所述当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，以得到模型比对结果，所述模型比对结果表征所述当前帧图像中匹配出的特征点对应的空间点是否属于运动物体；其中，所述第二像素模型为所述历史图像中与所述当前帧图像中匹配出的特征点相匹配的特征点的像素模型，所述第二像素模型对应于所述历史图像中匹配出的特征点的多个邻居特征点，且所述第二像素模型还具有所述第二像素模型的方向向量和所述历史图像中匹配出的特征点的重投影误差的均值；

特征点筛选单元，用于根据所述模型比对结果，筛选出所述当前帧图像中的目标特征点，所述目标特征点为所述当前帧图像中对应的空间点不属于运动物体的特征点；

位姿获得单元，用于根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿。

本申请还提供了一种移动设备，包括：

存储器，用于存储应用程序和所述应用程序运行所产生的数据；

处理器，用于执行所述应用程序，以实现：

获得当前帧图像，所述当前帧图像为移动设备上的图像采集装置所采集到的图像；

获得所述当前帧图像中匹配出的特征点，其中，所述当前帧图像中匹配出的特征点为在所述当前帧图像对应的滑动窗口所包含的历史图像中有相匹配的特征点的特征点；所述滑动窗口中包含多帧历史图像，所述历史图像为所述当前帧图像之前的关键帧图像；

分别获得每个所述当前帧图像中匹配出的特征点的第一像素模型，所述第一像素模型对应于所述当前帧图像中匹配出的特征点的多个邻居特征点，且所述第一像素模型还具有所述第一像素模型的方向向量和所述当前帧图像中匹配出的特征点的重投影误差；

分别将每个所述当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，以得到模型比对结果，所述模型比对结果表征所述当前帧图像中匹配出的特征点对应的空间点是否属于运动物体；其中，所述第二像素模型为所述历史图像中与所述当前帧图像中匹配出的特征点相匹配的特征点的像素模型，所述第二像素模型对应于所述历史图像中匹配出的特征点的多个邻居特征点，且所述第二像素模型还具有所述第二像素模型的方向向量和所述历史图像中匹配出的特征点的重投影误差的均值；

根据所述模型比对结果，筛选出所述当前帧图像中的目标特征点，所述目标特征点为所述当前帧图像中对应的空间点不属于运动物体的特征点；

根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿。

从上述技术方案可以看出，本申请公开的一种位姿获取方法、装置及移动设备，在移动设备上的图像采集装置所采集到的当前帧图像之后，对当前帧图像中与之前滑动窗口所包含的历史图像中的特征点相匹配的特征点进行获取，进而在分别获得这些当前帧图像中的特征点的对应于多个邻居特征点的像素模型之后，将该像素模型与滑动窗口所包含的历史图像中的特征点的像素模型进行比对，进而根据模型比对结果就可以筛选出当前帧图像中对应的空间点不属于运动物体的目标特征点，基于此，就能够根据这些目标特征点的重投影误差来获得移动设备的当前位姿。可见，本申请中利用前后图像中相匹配的特征点的像素模型来对属于运动物体的特征点进行剔除，从而根据筛选出来的不属于运动物体的特征点实现对移动设备的定位，由此，能够避免运动物体的特征点对位姿获取的干扰，从而提高移动设备在动态场景下的定位准确性。

附图说明

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

图1为本申请实施例一提供的一种位姿获取方法的流程图；

图2-图6分别为本申请实施例的示例图；

图7-图8分别为本申请实施例一提供的一种位姿获取方法的部分流程图；

图9为本申请实施例的另一示例图；

图10-图11分别为本申请实施例一提供的一种位姿获取方法的另一部分流程图；

图12为本申请实施例一提供的一种位姿获取方法的另一流程图；

图13为本申请实施例二提供的一种位姿获取装置的结构示意图；

图14为本申请实施例二提供的一种位姿获取装置的另一结构示意图；

图15为本申请实施例三提供的一种移动设备的结构示意图；

图16-图19分别为本申请实施例适用于移动机器人的示例图。

具体实施方式

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

参考图1，为本申请实施例一提供的一种位姿获取方法的实现流程图，该方法可以适用于能够对图像进行处理的电子设备中，如具有图像采集设备的移动设备，如移动机器人等，或者可以为与移动设备相连接的设备，如计算机或服务器等。本实施例中的技术方案用于提高对移动设备进行位姿获取的准确性。

具体的，本实施例中的方法可以包括以下步骤：

步骤101：获得当前帧图像。

其中，当前帧图像为移动设备上的图像采集装置所采集到的最新图像，图像采集装置可以为摄像头或录像机等设备，能够对移动设备对应的采集范围内的图像进行采集，如图2中所示，移动机器人的顶端设置有摄像头，在移动机器人移动过程中，摄像头能够采集前方范围内的图像。

需要说明的是，本实施例中可以通过与图像采集装置之间的连接接口获得图像采集设备所采集到的当前帧图像，或者，本实施例中可以通过与移动设备之间的通信接口获得移动设备所传输的图像采集装置采集到的当前帧图像。

另外，本实施例中的当前帧图像可以理解为当前时刻需要进行处理的图像。

步骤102：获得当前帧图像中匹配出的特征点。

其中，步骤102中所获得到的当前帧图像中匹配出的特征点为当前帧图像中与历史图像中的特征点相匹配的特征点，历史图像为当前帧图像之前的滑动窗口中的图像，如图3中所示，历史图像可以在被采集到之后，一帧或多帧历史图像组成当前帧图像对应的滑动窗口，并被存储到图像库中，基于此，本实施例中在获得当前帧图像之后，可以在图像库中读取已经保存的滑动窗口中的历史图像。其中，当前帧图像中与历史图像中的特征点相匹配的特征点是与历史图像中的特征点对应于地图中的同一空间点的特征点。

需要说明的是，本实施例中被添加到滑动窗口中的历史图像为图像采集装置所采集到的图像中的关键帧图像，关键帧是指具有代表性意义的图像帧，可以理解为角色或者物体运动或变化中的关键动作所处的图像帧，如I帧等。

在一种实现方式中，本实施例中在步骤102中获得当前帧图像中与历史图像中的特征点相匹配的特征点时，可以通过以下方式实现：

首先，对当前帧图像中的特征点进行提取，图像中的特征点是指图像中具有代表性的点，是图像信息的另一种数字表达形式。本实施例中的特征点可以为当前帧图像中的ORB(riented Fast and Rotated Brief)特征，由关键点和描述子组成，其中，关键点可以为FAST角点，具有尺度和旋转不变性，而描述子可以为二进制BRIEF描述子。具体实现中，本实施例可以采用预设的特征提取算法对当前帧图像中的特征点进行提取，由此得到当前帧图像中满足提取条件的特征点。而为了进一步提高准确性，本实施例中可以先将当前帧图像划分为多个小方格，再分别针对每个小方格进行特征点提取，从而增加特征点提取的均匀性，使得当前帧图像的特征点能够均匀分布在整个图像视野，此时所提取出的特征点包含当前帧图像中的特征点；

然后，将当前帧图像中提取出的特征点与历史图像中提取到的特征点进行匹配。其中，历史图像中的特征点是通过与当前帧图像相同的特征提取方式进行特征点提取所得到的特征点。

具体的，本实施例中可以使用预设的匹配算法将当前帧图像中的特征点与历史图像中的特征点进行匹配，例如对特征点进行ORB描述子匹配。为了进一步提高匹配速率，本实施例中可以采用DBOW2词袋技术等匹配算法对当前帧图像中的特征点和历史图像中的特征进行加速匹配，从而匹配出当前帧图像中与历史图像中的特征点对应于同一空间点的特征点，如图4中所示，此时，当前帧图像中匹配出的特征点在历史图像中有相匹配的特征点。

具体的，本实施例中可以先将当前帧图像中的特征点与滑动窗口中与当前帧图像最近的一帧关键帧图像中提取到的特征点进行点匹配，如果当前帧图像中与滑动窗口中与当前帧图像最近的一帧关键帧图像中提取到的特征点相匹配的特征点的数量没有满足要求数目，那么再将当前帧图像中的特征点与滑动窗口中所有关键帧图像中的特征点进行匹配，直到要求数目被满足。

进一步的，为了提高本实施例的可靠性，需要对当前帧图像中匹配出的特征点的数量是否达到预设的要求数目进行判断，如果当前帧图像中匹配出的特征点的数量没有达到要求数目，那么可以修改匹配算法，如放宽匹配算法中的匹配条件，再利用放宽匹配条件后的匹配算法将当前帧图像中提取出的特征点与滑动窗口中的历史图像中的特征点进行匹配，例如，利用放宽匹配条件的DBOW2词袋技术加速匹配出当前帧图像中与历史图像中的特征点相匹配的特征点，从而增加当前帧图像中匹配出的特征点的数量，使得匹配出的特征点的数量能够满足要求数目。

需要说明的是，滑动窗口中的历史图像为关键帧图像，因此，在对当前帧图像中的所有特征进行匹配时，使用的是滑动窗口内的关键帧的特征点，由此，匹配出当前帧图像中与滑动窗口中关键帧特征点相匹配的特征点。

进一步的，本实施例中在步骤102中获得到当前帧图像中与历史图像中的特征点相匹配的特征点之后，可以基于当前帧图像中匹配出的特征点进行重投影误差计算，并基于计算出的重投影误差初步估计出移动设备的当前位姿。例如，本实施例中根据历史图像对应的深度图像获得历史图像中与当前帧图像中的特征点相匹配的特征点对应的空间点的深度值，进而得到这些空间点的三维坐标值，再利用初始的位姿变换矩阵将三维坐标值转换到当前帧图像对应的当前坐标系中，之后投影出这些空间点在当前坐标系中的二维坐标值，再将这些坐标值与当前帧图像中匹配出的相应特征点的二维坐标值进行误差计算，由此，根据计算出的重投影误差进行位姿估计，由此初步估计出移动设备的当前位姿。

步骤103：分别获得每个当前帧图像中匹配出的特征点的第一像素模型。

其中，第一像素模型也可以称为特征点模型，第一像素模型对应于当前帧图像中匹配出的特征点的多个邻居特征点、方向向量以及重投影误差，进一步的，当前帧图像中匹配出的特征点的邻居特征点的深度值与相应当前帧图像中的特征点的深度值是不同的，也就是说，本实施例中针对每个当前帧图像中匹配出的特征点分别建立一个第一像素模型，而每个当前帧图像中匹配出的特征点的第一像素模型是根据与当前帧图像中匹配出的特征点的深度值不同的多个邻居特征点所建立的，这些邻居特征点之间在深度值上可能相同也可能不同。基于此，本实施例中在获得当前帧图像中匹配出的特征点的第一像素模型时，具体可以通过以下方式实现：

首先，获得当前帧图像中匹配出的特征点的多个邻居特征点，邻居特征点分布在当前帧图像中匹配出的特征点周边，如图5中所示；

然后，剔除这多个邻居特征点中深度值相同的冗余邻居特征点；

之后，再根据剩余的邻居特征点建立当前帧图像中匹配出的特征点的第一像素模型。

具体的，第一像素模型中可以包含有多个邻居特征点的特征数据，如邻居特征点的像素值、坐标值等，还包含有：第一像素模型的方向向量和当前帧图像中匹配出的特征点的重投影误差，其中，第一像素模型的方向向量基于第一像素模型中的邻居特征点对应的空间中心点的三维坐标值和当前帧图像中匹配出的特征点对应的空间点的三维坐标值获得，而当前帧图像中匹配出的特征点的重投影误差基于历史图像如滑动窗口中的历史图像获得，具体的，重投影误差为根据历史图像中的3D空间点投影到当前帧图像中的几何误差获得。例如，根据历史图像对应的深度图像获得历史图像中与当前帧图像中匹配出的特征点相匹配的特征点对应的空间点的深度值，进而得到这些空间点的三维坐标值，再利用初始的位姿变换矩阵将三维坐标值转换到当前帧图像对应的当前坐标系中，之后投影出这些空间点在当前坐标系中的二维坐标值，再将这些坐标值与当前帧图像中匹配出的相应特征点的二维坐标值进行误差计算。

具体的，第一像素模型可以通过{p′

需要说明的是，由于步骤102中所获得的当前帧图像中匹配的特征点为当前帧图像中与滑动窗口中的历史图像中的特征点相匹配的特征点，因此，第一像素模型是根据当前帧图像的之前的历史图像的特征点对当前帧图像的原特征点经过筛选后所得到的特征点的像素模型。另外，对于当前帧图像中除了与历史图像中的特征点相匹配的特征点之外的其他特征点也需要建立相应的像素模型，这些其他特征点建立像素模型的方式可以参考第一像素模型的建立方式，这些其他特征点的像素模型主要用于对历史图像中与当前帧图像的特征点相匹配的特征点对应的第二像素模型进行更新，具体见后文内容。因此，当前帧图像中匹配出的特征点的第一像素模型的建立是基于该特征点是与历史图像中相匹配的特征点的前提下实现的，因此，本实施例中当前帧图像中匹配出的特征点的第一像素模型的建立是在具有历史图像的基础上建立的。

步骤104：分别将每个当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，以得到模型比对结果。

其中，模型比对结果表征当前帧图像中匹配出的特征点对应的空间点是否属于运动物体；第二像素模型为历史图像中的特征点的像素模型，第二像素模型对应于历史图像中的特征点的多个邻居特征点、方向向量和重投影误差的均值，进一步的，历史图像中的特征点的邻居特征点的深度值与其对应的历史图像中的特征点的深度值是不同的。

需要说明的是，本实施例中历史图像中匹配出的特征点的第二像素模型的建立方式可以参考前文中第一像素模型的建立方式，此处不再详述。其中，在包含有至少两帧历史图像的滑动窗口内基于前一帧图像建立后一帧图像中的匹配特征点的第二像素模型，而滑动窗口内第一帧图像的特征点可以通过初始化预设，由此，滑动窗口内第一帧图像中的特征点的像素模型基于初始化预设的特征点建立，在滑动窗口中新加入的图像后，利用这些新加入的图像中提取出的特征点的像素模型对滑动窗口中前一帧图像中被匹配出的特征点的第二像素模型进行更新。

当然，第二像素模型中的内容是与第一像素模型中的内容相一致的，例如，第一像素模型中包含有当前帧图像中的特征点的多个邻居特征点的特征数据，还包含有第一像素模型的方向向量和历史图像中的特征点的重投影误差，而第二像素模型中包含有历史图像中的特征点的多个邻居特征点的特征数据，还包含有第二像素模型的方向向量和历史图像中的特征点的重投影误差的均值。

基于此，本实施例中将每个当前帧图像中匹配出的特征点的第一像素模型分别与各个历史图像中的与当前帧图像中匹配的特征点相匹配的特征点的第二像素模型进行比对，即一一对比，如图6中所示，由此，得到每个当前帧图像中匹配出的特征点对应的模型比对结果。

具体的，本实施例中分别将每个当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，可以是每个当前帧图像中匹配出的特征点的第一像素模型分别与相应的第二像素模型在多个邻居点、方向向量和重投影误差中的每一项分别进行比对，进而得到每个当前帧图像中匹配出的特征点各自对应的多个邻居特征点、方向向量和重投影误差中每一项的比对结果，进而基于这些比对结果，获得到每个当前帧图像中匹配出的特征点对应的模型比对结果。

步骤105：根据模型比对结果，筛选出当前帧图像中的目标特征点。

其中，目标特征点为当前帧图像中对应的空间点不属于运动物体的特征点。

具体的，本实施例中模型比对结果表征当前帧图像中匹配出的特征点对应的空间点是否属于运动物体，在模型比对结果中表征当前帧图像中匹配出的特征点与其相匹配的历史图像中的特征点之间的差异性超出一定的标准的情况下，表征当前帧图像中匹配出的特征点为相对于其相匹配的历史图像中的特征点运动的特征点，那么可以确定当前帧图像中匹配出的特征点对应的空间点属于运动物体，如果模型比对结果中表征当前帧图像中匹配出的特征点与其相匹配的历史图像中的特征点之间的差异性没有超出一定的标准，那么可以表征当前帧图像中匹配出的特征点为相对于其相匹配的历史图像中的特征点静止的特征点，那么可以确定当前帧图像中匹配出的特征点对应的空间点不属于运动物体，基于此，根据模型比对结果，对当前帧图像中对应的空间点属于运动物体的特征点进行剔除，从而筛选出对应的空间点不属于运动物体的目标特征点，即静态特征点。

步骤106：根据当前帧图像中的目标特征点对应的重投影误差，获得移动设备的当前位姿。

具体的，本实施例中可以通过对重投影误差进行最小化，进而获得到目标特征点的重投影误差最小时对应的位姿变换矩阵，这里重投影误差根据目标特征点的与其匹配的历史图像中的特征点对应的空间点的三维坐标值和目标特征点在当前帧图像中的二维坐标值获得，之后，就可以根据位姿变换矩阵，获得移动设备的当前位姿。

进一步的，本实施例中可以在初步估计的移动设备的当前位姿的基础上，以初步估计的当前位姿为优化初始值，通过对当前帧图像中的目标特征点对应的重投影误差进行最小化，进而获得到目标特征点的重投影误差最小时对应的位姿变换矩阵，进而根据位姿变换矩阵获得到更为精确的移动设备的当前位姿。

由上述方案可知，本申请实施例一提供的一种位姿获取方法中，在移动设备上的图像采集装置所采集到的当前帧图像之后，对当前帧图像中与之前滑动窗口所包含的历史图像中的特征点相匹配的特征点进行获取，进而在分别获得这些当前帧图像中匹配出的特征点的对应于的像素模型之后，将该像素模型与滑动窗口所包含的历史图像中的特征点的像素模型进行比对，进而根据模型比对结果就可以筛选出当前帧图像中对应的空间点不属于运动物体的目标特征点，基于此，就能够根据这些目标特征点的重投影误差来获得移动设备的当前位姿。可见，本实施例中利用前后图像中相匹配的特征点的像素模型来对属于运动物体的特征点进行剔除，从而根据筛选出来的不属于运动物体的特征点实现对移动设备的定位，由此，能够避免运动物体的特征点对位姿获取的干扰，从而提高移动设备在动态场景下的定位准确性。

为了进一步提高定位准确性，步骤106中在根据当前帧图像中的目标特征点对应的重投影误差，获得移动设备的当前位姿时，具体可以通过以下步骤实现，如图7中所示：

步骤701：根据目标特征点对应的空间点的深度值，获得目标特征点对应的权重值。

其中，目标特征点对应的空间点的深度值可以根据目标特征点所在当前帧图像对应的深度图像中的深度值获得，基于此，权重值基于该深度值获得。例如，第i个目标特征点对应的空间点的深度值以ω

步骤702：根据目标特征点对应的权重值，获得目标特征点的重投影误差最小时对应的位姿变换矩阵。

其中，重投影误差根据目标特征点在历史图像中对应的三维坐标值和目标特征点在当前帧图像中的二维坐标值获得。例如，本实施例中根据历史图像对应的深度图像获得历史图像中目标特征点对应的空间点的深度值，进而得到这些空间点的三维坐标值，再利用初始的位姿变换矩阵将三维坐标值转换到当前帧图像对应的当前坐标系中，之后投影出这些空间点在当前坐标系中的二维坐标值，再将这些二维坐标值与当前帧图像中匹配出的相应特征点的二维坐标值进行误差计算，得到重投影误差。基于此，本实施例中根据目标特征点对应的权重值，对相应的重投影误差进行加权，以得到最优的位姿变换矩阵。

步骤703：根据位姿变换矩阵，获得移动设备的当前位姿。

其中，本实施例中可以采用预设的定位算法根据位姿变换矩阵，计算出移动设备的当前位姿。

具体的，本实施例中可以先根据历史图像对应的深度图像获得与目标特征点相匹配的历史图像中的特征点的深度值，进而还原出历史图像中分别与每个目标特征点相匹配的特征点在空间中的三维坐标值，基于此，就可以将历史图像中的与每个目标特征点相匹配的特征点的三维坐标值和当前帧图像中各个目标特征点的二维坐标值构成三维-二维点对，即3D-2D点对，基于此，构建重投影误差方程以及相应的位姿变换矩阵，以

基于此，在最小化重投影误差的函数时，就可以得到最合适的位姿变换矩阵。具体的，本实施例中可以通过EPNP算法对重投影误差函数进行非线性优化求解，由此得到位姿变换矩阵。基于此，本实施例中以初步估计的移动设备的当前位姿为初始值，就可以利用位姿变换矩阵获得到更为精确的移动设备的当前位姿。

进一步的，为了提高第一像素模型的准确性，本实施例中需要在当前帧图像中匹配出的特征点的邻居特征点中筛选出最能够表征当前帧图像中的特征点的特点的邻居特征点，基于此，本实施例中在步骤103中分别获得每个所述当前帧图像中匹配出的特征点的第一像素模型时，具体可以通过以下步骤实现，如图8中所示：

步骤801：分别以每个当前帧图像中匹配出的特征点为中心，获得当前帧图像中匹配出的特征点的多个邻居特征点。

其中，当前帧图像中匹配出的特征点与其对应的邻居特征点的深度值不同，也就是说，当前帧图像中匹配出的特征点的邻居特征点的深度值与作为中心的当前帧图像中匹配出的特征点的深度值不同，且当前帧图像中匹配出的特征点的邻居特征点为深度值大于目标深度且与当前帧图像中匹配出的特征点之间的距离满足距离排序规则的特点，目标深度与所述当前帧图像中匹配出的特征点的24邻域像素点的深度均值相关。

以筛选出N个邻居特征点为例，具体筛选方式如下：

首先，以每个当前帧图像中匹配出的特征点为中心，获得当前帧图像中匹配出的特征点的24邻域或8邻域内的像素点，如图9中所示；

然后，计算当前帧图像中匹配出的特征点p′的24邻域或8邻域的像素点的深度值的均值，以

之后，计算当前帧图像中匹配出的特征点p′与各个邻居特征点之间的距离，如欧式距离，将深度值大于

以上筛选方案可以排除与当前帧图像中匹配出的特征点p′属于同一刚体即同一物体的邻居特征点也被加入到第一像素模型的建立中的情况，这是因为在深度接近的情况下，像素点极有可能处于同一刚体，即使运动发生了变化，它们之间的关系也不会发生太大变化，因此，本实施例中剔除可能与当前帧图像中匹配出的特征点属于同一刚体的邻居特征点，能够提高第一像素模型的准确性。

步骤802：至少根据当前帧图像中匹配出的特征点的多个邻居特征点，建立当前帧图像中匹配出的特征点的第一像素模型。

具体的，本实施例中可以根据这多个邻居特征点的特征数据建立第一像素模型，第一像素模型中可以包含有这多个邻居特征点各自的特征数据，还包含有这多个邻居特征点对应的方向向量和当前帧图像中匹配出的特征点的重投影误差。

进一步的，步骤104中在分别将每个当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对时，具体可以通过以下步骤实现，如图10中所示：

步骤1001：获得第一像素模型和其对应的第二像素模型中相匹配的邻居特征点的数量。

其中，相匹配的邻居特征点是指第一像素模型中与其对应的第二像素模型中相对应的邻居特征点之间的差异值小于或等于差异阈值。以当前帧图像中匹配出的特征点p′的第一像素模型{p′

基于此，本实施例中获得到相匹配的邻居特征点的数量，即第一像素模型中与其对应的第二像素模型中差异值小于或等于差异阈值的邻居特征点的数量。

步骤1002：获得第一像素模型中的方向向量和其对应的第二像素模型中的方向向量之间的差值的模长。

其中，在运动物体是垂直于相机平面运动的情况下，两个像素模型中的邻居特征点之间会出现相似度极高的情况，即差异值极低的情况，因此，为了进一步提高准确性，本实施例中增加方向向量的对比条件，基于此，本实施例中需要获得两个像素模型的方向向量之间的差值的模长，以||n-n′||

步骤1003：判断第一像素模型中当前帧图像中匹配出的特征点的重投影误差是否小于或等于与第二像素模型中历史图像中匹配出的特征点对应的重投影误差的均值，以得到判断结果。

其中，均值为历史图像中匹配出的特征点在滑动窗口中累积重投影误差的均值。

需要说明的是，为了通过重投影误差的累积对比特征点是动态点的可能性，本实施例中的均值以

其中，步骤1002和步骤1003以及步骤1001之间的执行顺序可以变化，例如，可以先执行步骤1002，再执行其他步骤，或者同时执行三个步骤，所得到的不同的技术方案属于同一发明构思，均在本申请的保护范围内。

步骤1004：根据数量以及模长和判断结果，获得模型比对结果。

其中，在数量超过预设的数值、模长小于预设值且重投影误差小于均值的情况下，所得到的模型比对结果表征当前帧图像中匹配出的特征点对应的空间点不属于运动物体，即静态点。

也就是说，在至少数量超过预设的数值如3或5的情况下，为了进一步提高准确性，判断方向向量差值的模长是否在一定范围内如小于预设值δ和重投影误差是否小于均值，来获得模型比对结果。

以根据数量是否超过预设的数值、方向向量差值的模长是否在一定范围内和重投影误差是否小于均值来获得模型比对结果为例：

如果数量超过预设的数值、方向向量差值的模长在一定范围内而且重投影误差小于滑动窗口内对应的均值，那么可以得到确定当前帧图像中匹配出的特征点对应的空间点属于静态点即不属于运动物体的模型比对结果；

如果数量没有超过预设的数值、或者方向向量差值的模长超出预设的范围内或者重投影误差已经大于滑动窗口内对应的均值，那么可以得到确定当前帧图像中匹配出的特征点对应的空间点属于动态点即属于运动物体的模型比对结果。

也就是说，以上三个条件中：数量超过预设的数值、方向向量差值的模长在一定范围内即||n-n′||

在具体实现中，滑动窗口内历史图像中的每个特征点均具有像素模型，本实施例中将历史图像中与当前帧图像中匹配出的特征点相匹配的特征点的像素模型记为第二像素模型，以便于与当前帧图像中匹配出的特征点的第一像素模型和没有特征点相匹配的特征点的像素模型进行区分。

基于此，进一步的，为了增加滑动窗口内历史图像中每个特征点的像素模型内特征点的多样性，保证历史图像所在滑动窗口内老旧图像的像素模型中的特征点不被轻易代替，本实施例中可以对历史图像中的像素模型进行更新，具体图11中所示：

步骤1101：获得针对于历史图像中特征点的像素模型的更新请求。

其中，更新请求可以自动生成或者用户操作生成，该更新请求中可以包含有需要被更新的历史图像中的某一个或多个特征点的标识。

步骤1102：获得滑动窗口内新加入的图像。

其中，新加入的图像可以为最近一次经过位姿定位处理的且为关键帧图像的当前帧图像，此时，该当前帧图像已经作为历史图像加入到滑动窗口中，记为新加入的图像。需要说明的是，在当前帧图像为关键帧图像的情况下，该当前帧图像会被加入到滑动窗口中。

需要说明的是，新加入的图像是经过定位处理的关键帧图像，该经过定位处理的关键帧图像在有新的图像被获得并作为新的当前帧图像时被当成历史图像加入到滑动窗口中。

步骤1103：对新加入的图像进行特征点提取，以得到新加入的图像中的特征点。

其中，新加入的图像中可以通过特征提取算法等方式提取到各种特征点，新加入的图像中的每个特征点在历史图像中可能有相匹配的特征点，如在历史图像中有特征点与新加入的图像中的特征点关于描述子相匹配，也可能新加入的图像中的每个特征点在历史图像中没有相匹配的特征点。

步骤1104：将新加入的图像中的特征点与滑动窗口中的历史图像中的特征点进行匹配，以得到新加入的图像中与历史图像中的特征点相匹配的匹配特征点和新加入的图像中与历史图像中的特征点均不匹配的非匹配特征点。

其中，匹配特征点是指新加入的图像中的在历史图像中存在特征点相匹配的特征点，非匹配特征点是指新加入的图像中的在历史图像中没有特征点相匹配的特征点。

具体的，本实施例中可以将新加入的图像中的特征点与滑动窗口中的历史图像中的特征点ORB描述子匹配，进而得到匹配特征点和非匹配特征点。

步骤1105：根据匹配特征点，对历史图像中与匹配特征点相匹配的特征点的像素模型进行更新。

具体的，本实施例中可以先对匹配特征点建立像素模型，该匹配特征点的像素模型对应于匹配特征点的多个邻居特征点、方向向量以及重投影误差，之后，本实施例中利用匹配特征点的像素模型中的邻居特征点对历史图像中与匹配特征点相匹配的特征点的像素模型进行更新。

例如，对于历史图像中的特征点p的第二像素模型

基于此，在第二像素模型中的像素特征点被更新后，可以重新计算第二像素模型中的方向向量以及重投影误差。而更新后的第二像素模型可以用于模型比对，从而得到更准确的模型比对结果，提高位姿获取的准确性。

步骤1106：获得非匹配特征点的像素模型，以使得在新加入的图像作为滑动窗口中的历史图像时，非匹配特征点的像素模型作为历史图像中新的特征点的像素模型。

也就是说，新加入的图像被添加到滑动窗口之后，该新加入的图像已经作为滑动窗口中的历史图像，因此新加入的图像中的非匹配特征点的像素模型直接作为历史图像中特征点的像素模型。基于此，在使用新的当前帧图像对移动设备的当前位姿进行定位时，将新的当前帧图像中提取出的特征点与滑动窗口中加入有新图像的历史图像中提取到的特征点进行匹配，在新的当前帧图像中匹配出与历史图像中的特征点相匹配的特征点之后，将新的当前帧图像中匹配出的特征点的像素模型与其在历史图像中相匹配的特征点的像素模型进行比对，进而筛选出新的当前帧图像中的目标特征点，即静态点，再基于这些静态点对应的重投影误差对移动设备的当前位姿进行精确估计。

需要说明的是，步骤1105和步骤1106的执行顺序不限于附图中所示的顺序，例如，也可以先执行步骤1106，再执行步骤1105，或者，同时执行步骤1105和步骤1106，步骤1105和步骤1106的执行顺序不同所形成的不同的技术方案属于同一发明构思，均在本申请的保护范围内。

为进一步提高准确性，本实施例中在步骤105根据所述模型比对结果，筛选出所述当前帧图像中的目标特征点之后，在步骤106中根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿之前，本实施例中的方法还可以包括以下步骤，如图12中所示：

步骤107：在当前帧图像中的目标特征点中，筛选出与历史图像中的特征点满足极线约束的目标特征点，之后，再执行步骤106。

也就是说，考虑到可能存在漏检或者之前处理中没有找到匹配的特征点的情况，本申请中引入多视几何的约束进行处理，进而提出目标特征点中不满足极线约束的特征点，只保留满足极线约束的目标特征点，从而提高后续位姿获取的准确性。

参考图13，为本申请实施例二提供的一种位姿获取装置的结构示意图，该装置可以配置在能够对图像进行处理的电子设备中，如具有图像采集设备的移动设备，如移动机器人等，或者可以为与移动设备相连接的设备，如计算机或服务器等。本实施例中的技术方案用于提高对移动设备进行位姿获取的准确性。

具体的，本实施例中的装置可以包括以下单元：

图像获得单元1301，用于获得当前帧图像，所述当前帧图像为移动设备上的图像采集装置所采集到的图像；

特征点处理单元1302，用于获得所述当前帧图像中匹配出的特征点，其中，所述当前帧图像中匹配出的特征点为在所述当前帧图像对应的滑动窗口所包含的历史图像中有相匹配的特征点的特征点；所述滑动窗口中包含多帧历史图像，所述历史图像为所述当前帧图像之前的关键帧图像；

模型建立单元1303，用于分别获得每个所述当前帧图像中匹配出的特征点的第一像素模型，所述第一像素模型对应于所述当前帧图像中匹配出的特征点的多个邻居特征点，且所述第一像素模型还具有所述第一像素模型的方向向量和所述当前帧图像中匹配出的特征点的重投影误差；

模型比对单元1304，用于分别将每个所述当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，以得到模型比对结果，所述模型比对结果表征所述当前帧图像中匹配出的特征点对应的空间点是否属于运动物体；其中，所述第二像素模型为所述历史图像中与所述当前帧图像中匹配出的特征点相匹配的特征点的像素模型，所述第二像素模型对应于所述历史图像中匹配出的特征点的多个邻居特征点，且所述第二像素模型还具有所述第二像素模型的方向向量和所述历史图像中匹配出的特征点的重投影误差的均值；

特征点筛选单元1305，用于根据所述模型比对结果，筛选出所述当前帧图像中的目标特征点，所述目标特征点为所述当前帧图像中对应的空间点不属于运动物体的特征点；

位姿获得单元1306，用于根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿。

由上述方案可知，本申请实施例一提供的一种位姿获取装置，在移动设备上的图像采集装置所采集到的当前帧图像之后，对当前帧图像中与之前滑动窗口所包含的历史图像中的特征点相匹配的特征点进行获取，进而在分别获得这些当前帧图像中的特征点的对应于多个邻居特征点的像素模型之后，将该像素模型与滑动窗口所包含的历史图像中的特征点的像素模型进行比对，进而根据模型比对结果就可以筛选出当前帧图像中对应的空间点不属于运动物体的目标特征点，基于此，就能够根据这些目标特征点的重投影误差来获得移动设备的当前位姿。可见，本实施例中利用前后图像中相匹配的特征点的像素模型来对属于运动物体的特征点进行剔除，从而根据筛选出来的不属于运动物体的特征点实现对移动设备的定位，由此，能够避免运动物体的特征点对位姿获取的干扰，从而提高移动设备在动态场景下的定位准确性。

在一种实现方式中，位姿获得单元1306具体用于：根据所述目标特征点对应的空间点的深度值，获得所述目标特征点对应的权重值；根据所述目标特征点对应的权重值，获得所述目标特征点的重投影误差最小时对应的位姿变换矩阵，所述重投影误差根据所述目标特征点在所述历史图像中对应的三维坐标值和所述目标特征点在所述当前帧图像中的二维坐标值获得；根据所述位姿变换矩阵，获得所述移动设备的当前位姿。

在一种实现方式中，模型建立单元1303具体用于：分别以每个所述当前帧图像中匹配出的特征点为中心，获得所述当前帧图像中匹配出的特征点的多个邻居特征点；其中，且所述当前帧图像中匹配出的特征点与其对应的邻居特征点为深度值大于目标深度且与所述当前帧图像中匹配出的特征点之间的距离满足距离排序规则的特征点，所述目标深度与所述当前帧图像中匹配出的特征点的邻域像素点的深度均值相关；至少根据所述当前帧图像中匹配出的特征点的多个邻居特征点，建立所述当前帧图像中匹配出的特征点的第一像素模型，以使得所述第一像素模型对应于所述当前帧图像中匹配出的特征点的多个邻居特征点且还具有所述第一像素模型的方向向量和所述当前帧图像中匹配出的特征点的重投影误差。

其中，所述第一像素模型的方向向量基于所述第一像素模型中的邻居特征点对应的空间中心点的三维坐标值和所述当前帧图像中匹配出的特征点对应的空间点的三维坐标值获得，所述当前帧图像中匹配出的特征点的重投影误差基于所述历史图像获得。

基于此，模型比对单元1304具体用于：获得所述第一像素模型和其对应的第二像素模型中相匹配的邻居特征点的数量；获得所述第一像素模型中的方向向量和其对应的第二像素模型中的方向向量之间的差值的模长；判断所述第一像素模型中所述当前帧图像中匹配出的特征点的重投影误差是否小于或等于与所述第二像素模型中所述历史图像中匹配出的特征点对应的重投影误差的均值，以得到判断结果；所述均值为所述历史图像中匹配出的特征点在所述滑动窗口中累积重投影误差的均值；根据所述数量以及所述模长和所述判断结果，获得模型比对结果。

在一种实现方式中，本实施例中的装置还可以包括以下单元，如图14中所示：

模型更新单元1307，用于：获得针对于所述历史图像中的特征点的像素模型的更新请求；获得所述滑动窗口内新加入的图像；对所述新加入的图像进行特征点提取，以得到所述新加入的图像中的特征点；将所述新加入的图像中的特征点与所述滑动窗口中的历史图像中的特征点进行匹配，以得到所述新加入的图像中与所述历史图像中的特征点相匹配的匹配特征点和所述新加入的图像中与所述历史图像中的特征点均不匹配的非匹配特征点；根据所述匹配特征点，对所述历史图像中与所述匹配特征点相匹配的特征点的像素模型进行更新；获得所述非匹配特征点的像素模型，以使得在所述新加入的图像作为所述滑动窗口中的历史图像时，所述非匹配特征点的像素模型作为所述历史图像中新的特征点的像素模型。

在一种实现方式中，特征点筛选单元1305在根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿之前，还用于：在所述当前帧图像中的目标特征点中，筛选出与所述历史图像中的特征点满足极线约束的目标特征点。

在一种实现方式中，特征点处理单元1302具体用于：对所述当前帧图像中的特征点进行提取；将所述当前帧图像中提取出的特征点与所述历史图像中提取到的特征点进行匹配，以得到所述当前帧图像中与所述历史图像中的特征点相匹配的特征点。

在一种实现方式中，特征点处理单元1302还用于：基于当前帧图像中匹配出的特征点进行重投影误差计算，并基于计算出的重投影误差初步估计出移动设备的当前位姿。

需要说明的是，本实施例中各单元的具体实现可以参考前文中的相应内容此处不再详述。

参考图15，为本申请实施例三提供的一种移动设备的结构示意图，该移动设备可以为能够对图像进行处理的电子设备，如具有图像采集设备的移动设备，如移动机器人等，或者可以为与移动设备相连接的设备，如计算机或服务器等。本实施例中的技术方案用于提高对移动设备进行位姿获取的准确性。

具体的，本实施例中的移动设备中至少包括以下结构：

存储器1501，用于存储应用程序和所述应用程序运行所产生的数据；

处理器1502，用于执行所述应用程序，以实现：

获得当前帧图像，所述当前帧图像为移动设备上的图像采集装置所采集到的图像；

获得所述当前帧图像中匹配出的特征点，其中，所述当前帧图像中匹配出的特征点为在所述当前帧图像对应的滑动窗口所包含的历史图像中有相匹配的特征点的特征点；所述滑动窗口中包含多帧历史图像，所述历史图像为所述当前帧图像之前的关键帧图像；

分别获得每个所述当前帧图像中匹配出的特征点的第一像素模型，所述第一像素模型对应于所述当前帧图像中匹配出的特征点的多个邻居特征点，且所述第一像素模型还具有所述第一像素模型的方向向量和所述当前帧图像中匹配出的特征点的重投影误差；

分别将每个所述当前帧图像中匹配出的特征点的第一像素模型与相应的第二像素模型进行比对，以得到模型比对结果，所述模型比对结果表征所述当前帧图像中匹配出的特征点对应的空间点是否属于运动物体；其中，所述第二像素模型为所述历史图像中与所述当前帧图像中匹配出的特征点相匹配的特征点的像素模型，所述第二像素模型对应于所述历史图像中匹配出的特征点的多个邻居特征点，且所述第二像素模型还具有所述第二像素模型的方向向量和所述历史图像中匹配出的特征点的重投影误差的均值；

根据所述模型比对结果，筛选出所述当前帧图像中的目标特征点，所述目标特征点为所述当前帧图像中对应的空间点不属于运动物体的特征点；

根据所述当前帧图像中的目标特征点对应的重投影误差，获得所述移动设备的当前位姿。

从上述技术方案可以看出，本申请实施例三的一种移动设备，在移动设备上的图像采集装置所采集到的当前帧图像之后，对当前帧图像中与之前滑动窗口所包含的历史图像中的特征点相匹配的特征点进行获取，进而在分别获得这些当前帧图像中的特征点的对应于多个邻居特征点的像素模型之后，将该像素模型与滑动窗口所包含的历史图像中的特征点的像素模型进行比对，进而根据模型比对结果就可以筛选出当前帧图像中对应的空间点不属于运动物体的目标特征点，基于此，就能够根据这些目标特征点的重投影误差来获得移动设备的当前位姿。可见，本实施例中利用前后图像中相匹配的特征点的像素模型来对属于运动物体的特征点进行剔除，从而根据筛选出来的不属于运动物体的特征点实现对移动设备的定位，由此，能够避免运动物体的特征点对位姿获取的干扰，从而提高移动设备在动态场景下的定位准确性。

其中，在电子设备为移动设备的情况下，电子设备中还包含有图像采集装置，如摄像头等。

需要说明的是，本实施例中处理器的具体实现可以参考前文中的相应内容此处不再详述。

以机器人的定位为例，以下对本申请的技术方案实现SLAM进行详细的举例说明：

首先，本申请的发明人在采用SLAM进行机器人定位过程中，发现存在如下缺陷：

在所采用的视觉SLAM框架中，需要假设环境为静态环境，或者环境中的静态特征占据多数的准静态场景。无论是基于直接法或是特征点法的视觉SLAM系统，都很难应对运动物体的干扰，而且在SLAM运行过程中，对于相机位姿的解算如果完全依赖于最小化能量函数或者随机采样一致性RANSAC方法的话，当静态特征点在整个图像中未占据多数，或者占比较低，则在有限次的迭代计算中，往往很难获得正确的相机运动模型；

本申请的发明人基于以上缺陷继续对定位方案进行试验，还发现：对于基于惯性测量单元IMU(Inertial Measurement Unit)惯性测量单元的解决动态环境的SLAM算法，这种方法并没有从本质解决动态环境问题，它只是通过提供给系统一个运动先验，增加特征点匹配的准确度，仍有可能在环境相似度较大的场景发生大量误匹配；而且增加额外传感器会增加定位算法的复杂度，增加传感器硬件成本；

由于动态物体并不一定是运动的物体，如果盲目的将预定义动态物体排除，这将很容易造成图像中信息大大减少，而采用目前的语义动态SLAM则是识别动态物体并将其剔除，这将会就会使得其在特征点稀少到场景中适用性更差，而且引入深度学习网络，会对系统计算力要求很高，在不含图形处理器GPU(Graphics Processing Unit)的硬件平台，该算法很难达到实时性，若增加GPU则会增加硬件成本，使得产品应用受到极大限制。另一方面由于深度学习的目标检测算法需要大量数据进行预训练，这也会一定程度限制基于视觉SLAM定位算法的机器人对于未知环境的探索；

本申请的发明人还发现：对于基于运动分割的算法，它能够同时估计出相机和场景中刚性动态物体的运动，这种方法不依赖于图像的语义信息，通过将相同运动的点聚类为一个运动模型进行运动估计，从而将动态场景中不同运动刚体分割出来。但是该方法对于场景的条件要求较高，在场景中可测3D点深度过少的情况下，相机和动态物体运动估计的误差较大，而且由于聚类分割的不准确性，会造成某个聚类包含多个运动刚体。

因此，本申请的发明人为了解决传统视觉SLAM定位算法在动态环境下精度降低的问题，提出了一种基于特征点建模和多视几何的方法来剔除场景中的动态区域和异常特征点的方法。该方法以视觉SLAM为主体，在一个滑动窗口内建立为特征点建立模型，通过将模型与当前帧进行对比，有效剔除场景中动态物体在图像上对应的特征点，实现在动态环境下的相机位姿的鲁棒性估计。具体如下：

结合图16中所示的本申请的技术框架，本申请中可以分为五个部分：基于最小化重投影误差计算粗略的位姿变换矩阵、动态区域判断、特征点建模、极线约束判断外点和精确位姿估计。下面对这五部分进行详细介绍：

1)基于最小化重投影误差计算位姿估计

位姿估计流程图17所示。本申请中对位姿估计采取了一个由粗到精的方式。这一步的位姿估计属于粗略位姿估计，核心是通过构造重投影误差计算3D-2D点对的运动。这部分算法步骤与原理如下：

(1)在当前帧图像中提取特征点。特征点是图像中具有代表性的点，是图像信息的另一种数字表达形式。发明中，我们对图像提取的是ORB特征，它由关键点和描述子组成。关键点称为“Oriented FAST”,这是一种改进的FAST角点，具有尺度和旋转不变性。描述子采用的的是二进制BRIEF描述子。在特征提取过程中，可能会因为图像中某一处纹理丰富，造成特征点集中于一个区域，使得位姿估计趋向于局部运动。为了提高系统精度，我们增加特征提取的均匀性：将图像划分为S*S的小方格，对于每个方格进行特征点提取，使得图像特征尽量分布在整个图像视野。

(2)将当前帧提取的特征点与所属的上一关键帧中对应的特征点进行ORB描述子匹配，这里采用DBOW2词袋技术对当前帧提取的特征点和上一关键帧中的特征点进行加速匹配，匹配结束后判断特征点匹配的数量是否达到要求数目，如果达到了，直接进行下一步构建重投影误差，估计相机位姿。若未达到要求，则放宽匹配条件，再次利用DBOW2词袋技术加速匹配当前帧提取的特征点和前面帧所构建的局部地图点。

(3)利用上一关键帧传感器输入的深度图像数据获取上一关键帧特征点的深度值，还原特征点3D坐标。把之前的3D点与当前帧2D像素点构成3D-2D点对，构建重投影误差方程如

其中，ω

2)特征点建模

这一步是本申请的核心部分，主要是为特征点建立一个阶段性模型，即前文中的像素模型，因为模型会在一个滑动窗口内进行替换和更新，故称阶段性，流程图如图18所示。建立模型的目的是通过将当前帧特征点与匹配的特征点模型进行比较，通过对比当前帧特征点与特征点模型的差异，对特征点是否为动态进行判断。

模型的建立和初始化如下：首先，对于每个特征点p，即历史图像中的特征点，建立其像素模型M(p)，即第二像素模型，其中，取

这一步出于的考虑是排除与特征点p属于同一刚体的特征点被加入模型，在深度接近的情况下，像素点极大可能处于同一个刚体，即使运动发生了改变，它们之间的关系也不会发生太大变化。n为该模型的方向向量，它是通过计算模型中所有邻居特征点对应的3D地图点的空间中心点的位置

3)动态特征点判断与模型更新

在对当前帧特征点进行判断之前，还需要同样对当前帧有效特征点(即在历史图像中有相匹配的特征点的特征点)建立一个像素特征点模型，即第一像素模型，类似于上文中的步骤，首先寻找特征点p′即当前帧图像中的特征点的N个深度不一致邻居特征点，然后计算模型的方向向量n′，最后加入第一步中计算的重投影误差r

(1)利用在第1)步中进行特征点匹配的结果，在特征点像素模型即第二像素模型中找到与当前帧特征点p′匹配的特征点(也可以称为特征点模型)，将特征点模型中的邻居特征点

(2)考虑到动态物体是垂直于相机平面运动，特征点像素模型中的深度不一致邻居特征点与当前帧特征点p′的N个深度不一致邻居特征点会出现相似度极高的情况，进一步增加方向向量对比条件，即如果特征点模型中的方向向量n与当前帧特征点p′模型中方向向量n′差值的模长在一定范围内，就满足该特征点为静态的要求：||n-n′||

(3)这一项条件可以看成额外项约束，主要是为了通过重投影误差的累计对比出特征点是动态点的可能性，这里的重投影误差计算使用的是第一步计算的结果。公式表达为

进一步的，随着机器人对图像的获取，机器人将经过定位的当前帧图像作为历史图像进行处理，如移动到滑动窗口中，同时采集到新的当前帧图像。基于此，在滑动窗口中的历史图像存在更新时，可以对于滑动窗口中的历史图像中的静态特征点p的模型即第二像素模型进行更新，本申请中采取的是一种随机更新策略，主要做可以保证滑动窗口内老旧帧产生的模型像素点不被轻易代替，增加模型内像素特征点的多样性：

首先，对于滑动窗口中特征点p的特征点模型

接着，对于采用模型M(p

4)极线约束判断外点

在利用特征点模型进行动态点移除(筛选)后，考虑到可能存在漏检或者上一步中没有在特征点模型中找到匹配的特征点的问题，本申请通过引入多视几何的约束进行处理。本申请中基于的事实是在静态场景，动态特征会违反在多视几何结构中的标准约束，如图19所示。

其中x

5)精确位姿估计

这一步是将当前帧经过前面步骤后存活的特征点即目标特征点与局部地图中的静态特征点进行局部优化，优化的方法仍然是通过计算重投影误差函数的最小值，使得当前帧的位姿估计最优。重投影误差函数的形式为

可见，本申请的发明人在应对动态环境下定位问题时提出了一种特征点建模的概念，并通过将特征点与模型对比来剔除动态。而且在计算重投影误差函数时，引入了预权重的概念，利用远的动态点在连续帧中不会在像素平面产生较大位移和静态点更容易是远处背景的思想，对误差值进行预加权处理。

进一步的，本申请中提出了一种利用深度不一致特征点建模的方法，思想是利用周围环境信息对特征点进行建模，而不是像素周围信息建模，通过将模型与需要判断的特征点进行三个条件判定，可以有效的区分动态和静态特征点。而且在本申请中采用了滑动窗口的方法对多连续帧进行判断，通过累计的信息对特征点判断，避免了短暂两帧之间运动不明显的缺点，有效地提高了特征点静态动态区分的准确性。

可见，本申请中不同于现有处理动态环境的方式，采取了特征点建模的方式，利用特征点在时间和空间的信息，建立一种像素模型，空间上是采用了深度不一致邻居特征点对像素进行代表，时间上采用了一种随机更新策略，可以增加模型中所包含的过去信息量。并且设定了三个条件对特征点进行判断，只有同时符合三个条件的特征点才能被判断为静态点，然后再利用这些特征点进行位姿的精确估计。进一步的，本申请是一种利用像素周围环境信息的方式，不同于采用深度学习的预训练和运动分割的方法，不需要对场景环境有预先的了解，也不需要对场景中不同运动刚体进行分割，减少了对硬件水平的需求，是一种实用性很强的处理动态环境的的SLAM算法。

需要说明的是，在传感器方面，本申请中筛选静态特征点的的方法不仅仅适用于RGB-D相机也同样使用，对于单目和双目相机也同样适用，只是RGB-D相机可以直接从传感器或得像素点深度信息，不需要额外算法进行计算。而且，本申请所使用的像素建模的方式是基于动态特征点其周围像素特征点也会发生变化的特点，对特征点进行建模，如果可以找到别的方案区分动态和静态特征点随着时间的变化有何相同和不同的地方，仍在本申请的保护范围内。另外，本申请是基于视觉SLAM的算法，但是实质上也是基于环境特征点采样的，但对于使用激光传感器的SLAM算法来说，也是适用的，只需要找到对激光点进行建模的其他特征点就行，然后再根据激光传感器获取的采样点特点设定静态和动态特征点判断条件，区分动态和静态特征点，利用静态点对机器人位姿进行估计。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。