位姿预测方法、地图构建方法、可移动平台及存储介质

技术领域

本发明实施例涉及机器视觉领域，尤其涉及一种位姿预测方法、地图构建方法、可移动平台及存储介质。

背景技术

视觉SLAM(Simultaneous Localization and Mapping，即时定位与地图构建)指的是机器人在自身位置不确定的条件下，在完全未知环境中根据相机采集的图像创建地图，同时利用地图进行自主定位和导航。

现有的视觉SLAM方法，在出现图像中特征点稀少(无纹理)或与上一图像帧重复太多、高速运动造成的模糊或无重叠区、相机帧率偶然降低等特殊问题时，会导致视觉SLAM的追踪功能失效，也即无法获取到当前图像帧相对于上一图像帧的相对位姿，进而使得视觉SLAM无法继续工作，需要在触发闭环时通过重定位的方式才能恢复视觉。

现有的视觉SLAM方法稳定性和鲁棒性较差，当出现视觉失效时需要等到发生闭环时视觉才能恢复，因此视觉SLAM会在视觉失效时出现中断，缺失视觉失效过程中的地图点和轨迹，导致视觉失效前后的地图点和轨迹无法衔接，导致视觉SLAM无法稳定的运行。

发明内容

本发明实施例提供一种位姿预测方法、地图构建方法、可移动平台及存储介质，以提高当前图像帧的位姿获取的准确性，提高视觉SLAM的稳定性和鲁棒性，在视觉失效时视觉SLAM仍能稳定的运行。

本发明实施例的第一方面是提供一种位姿预测方法，适用于一可移动平台，所述可移动平台包括第一滚动轮、第二滚动轮、摄像头、第一编码器和第二编码器，所述第一滚动轮和第二滚动轮用于使得所述可移动平台发生位移，所述第一滚动轮和第二滚动轮的轴线重合；所述摄像头用于采集图像帧，所述第一编码器用于检测所述第一滚动轮的速率，所述第二编码器用于检测所述第二滚动轮的速率；所述方法包括：

根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；

获取所述第一时间段内第一编码器的测量数据和第二编码器测的测量数据；

根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；

根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿。

本发明实施例的第二方面是提供一种可移动平台，包括：第一滚动轮、第二滚动轮、摄像头、第一编码器、第二编码器、存储器和处理器；

所述第一滚动轮和第二滚动轮用于使得所述可移动平台发生位移，所述第一滚动轮和第二滚动轮的轴线重合；

所述摄像头用于采集图像帧；

所述第一编码器用于检测所述第一滚动轮的速率，所述第二编码器用于检测所述第二滚动轮的速率；

所述存储器用于存储程序代码；

所述处理器，调用所述程序代码，当程序代码被执行时，用于执行以下操作：

根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；

获取所述第一时间段内第一编码器的测量数据和第二编码器测的测量数据；

根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；

根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿。

本发明实施例的第三方面是提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行以实现第一方面所述的方法。

本发明实施例的第四方面是提供一种地图构建方法，适用于一可移动平台，所述可移动平台包括第一滚动轮、第二滚动轮、摄像头、第一编码器和第二编码器，所述第一滚动轮和第二滚动轮用于使得所述可移动平台发生位移，所述第一滚动轮和第二滚动轮的轴线重合；所述摄像头用于采集图像帧，所述第一编码器用于检测所述第一滚动轮的速率，所述第二编码器用于检测所述第二滚动轮的速率；所述方法包括：

根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；

获取所述第一时间段内第一编码器的测量数据和第二编码器测的测量数据；

根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；

根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿；

在获得所述当前图像帧的预测位姿后，在已构建地图的基础上，根据所述当前图像帧对所述已构建地图进行更新；所述已构建地图由在所述当前图像帧之前确定的关键帧进行构建。

本发明实施例的第五方面是提供一种可移动平台，包括：第一滚动轮、第二滚动轮、摄像头、第一编码器、第二编码器、存储器和处理器；

所述第一滚动轮和第二滚动轮用于使得所述可移动平台发生位移，所述第一滚动轮和第二滚动轮的轴线重合；

所述摄像头用于采集图像帧；

所述第一编码器用于检测所述第一滚动轮的速率，所述第二编码器用于检测所述第二滚动轮的速率；

所述存储器用于存储程序代码；

所述处理器，调用所述程序代码，当程序代码被执行时，用于执行以下操作：

根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；

获取所述第一时间段内第一编码器的测量数据和第二编码器测的测量数据；

根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；

根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿；

在获得所述当前图像帧的预测位姿后，在已构建地图的基础上，根据所述当前图像帧对所述已构建地图进行更新；所述已构建地图由在所述当前图像帧之前确定的关键帧进行构建。

本发明实施例的第六方面是提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行以实现第四方面所述的方法。

本发明实施例提供的位姿预测方法、地图构建方法、可移动平台及存储介质，通过根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；获取所述第一时间段内第一编码器的测量数据和第二编码器测的测量数据；根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿。本发明实施例通过将编码器应用到视觉SLAM中，通过编码器测量数据预测当前图像帧的位姿，可提高位姿获取的准确性，提高视觉SLAM的稳定性和鲁棒性，在视觉失效时视觉SLAM仍能稳定的运行。

附图说明

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

图1为本发明实施例提供的地图构建方法的流程图；

图2为本发明实施例提供的位姿预测方法的流程图；

图3为本发明另一实施例提供的地图构建方法的流程图；

图4为本发明另一实施例提供的地图构建方法的流程图；

图5为本发明另一实施例提供的地图构建方法的流程图；

图6为本发明另一实施例提供的地图构建方法的流程图；

图7为本发明另一实施例提供的可移动平台的结构示意图。

具体实施方式

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

需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

下面结合附图，对本发明的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互组合。

本发明实施例提供一种地图构建方法。所述地图构建方法应用于可移动平台，所述可移动平台可以为机器人、无人车等，如图7所示，所述可移动平台700具体包括第一滚动轮701、第二滚动轮702、第一编码器703、第二编码器704、摄像头705，进一步的还可包括处理器706和存储器707。其中所述第一滚动轮701和第二滚动轮702用于使得所述可移动平台发生位移，所述第一滚动轮701和第二滚动轮702的轴线重合；所述摄像头705用于采集图像帧，所述第一编码器703用于检测所述第一滚动轮701的速率，所述第二编码器704用于检测所述第二滚动轮702的速率。

其中，所述摄像头705可以采用单目相机、双目相机、RGBD深度相机等，当所述摄像头705采用单目相机时通常会与IMU(Inertial Measurement Unit，惯性测量单元)通过紧耦合配合使用，以确定地图尺度，其中IMU为集成了测量三维旋转的陀螺仪(Gyroscope)和三维加速度的加速度计(Accelerometer)两类传感器的测量单元；当然在所述摄像头705采用双目相机、RGBD深度相机时也可与IMU配合使用，可进一步提高准确度，但使用IMU对传感器标定有较高要求。编码器(encoder)是指角位移传感器，它通过检测机器人轮子在一定时间内转过的弧度数来测量轮子的速率，主要分为光电式、接触式、电磁式三种。

如图1所示，本发明实施例所述的地图构建方法可分为追踪线程S11、局部建图线程S12以及回环检测线程S13，其中追踪线程S11包括预测图像帧的位姿，如图2所示，预测图像帧的位姿具体包括：

步骤S101、根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段。

在获取任一图像帧后，可首先对图像进行预处理，预处理过程具体包括：

对于RGBD相机，其采集的图像包括RGB图和深度图，可首先将GRB图转换为灰度图，对灰度图提取特征点，然后根据灰度图和深度图计算特征点的立体坐标，也即特征点相对于虚假的右相机的坐标，将RGBD相机采集的图像等效为双目相机采集的图像。

对于双目相机，将其采集的图像也转换为灰度图，对灰度图提取特征点，并根据左相机和右相机图像中特征点匹配，计算特征点的深度，得到特征点的立体坐标。

需要说明的是，本实施例中提取特征点可以为ORB(Oriented Brief)特征点，ORB特征是一种旋转不变性特征，其具有较高的稳定性，对于光照、运动物体不敏感，且计算量较小，能只用常规笔记本电脑的CPU也做到实时处理；当然特征点也可以为Harris角点、SIFT(Scale-invariant Feature Transform)特征点、SURF(Speeded Up RobustFeatures)特征点等。

对于特征点的应用，下面将会详细介绍。

步骤S102、获取所述第一时间段内第一编码器的测量数据和第二编码器测的测量数据。

优选地，在获取到第一编码器或第二编码器的测量数据后，可判断所述测量数据是否可用，具体如下：

将所述测量数据缓存至列表，并记录所述测量数据对应的采集时间；

从缓存列表中获取与当前图像帧采集时间最接近的所述测量数据，并获取该所述测量数据的采集时间与当前图像帧采集时间的时间误差；

若时间误差超过预设时间阈值，则说明该所述测量数据不可用。

在本实施例中，编码器测量的采集频率通常较高，例如200Hz，而图像的采集频率通常较低，例如30Hz，一般情况下是无法找到与图像完全在同一时刻采集的编码器测量数据，因此可以查找与图像采集时间最接近的编码器测量数据，也即编码器测量数据的采集时间与当前图像帧采集时间的时间误差不超过预设时间阈值。而当系统存在不稳定情况时，可能会存在编码器测量数据缺失，也即无法找到与当前图像帧采集时间的时间误差不超过预设时间阈值的编码器数据，也即当前图像帧采集时间前后的编码器测量数据不可靠，可丢弃这些不可靠编码器测量数据，仅采用现有技术中的纯视觉SLAM(即只依靠视觉进行位姿预测和地图构建)。同理，对于IMU的测量数据也可采用相似的判断过程。

步骤S103、根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵。

步骤S104、根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿。

在本实施例中，可根据编码器数据对当前图像帧相机的位姿进行预测，得到当前图像帧的预测位姿。具体的，假定可移动平台的运动为二维运动(平面或曲面)，且地面具有足够的摩擦力，其中，曲面可以采用类似泰勒展开的思想，短时间内近似为平面，即帧率足够高时也能处理曲面运动。

通过编码器数据获取预测变换矩阵T

在本实施例中，可通过平面运动模型推导预测变换矩阵T

可移动平台的当前图像帧的测量速度v

其中，测量速度v

其中，

将当前图像帧的测量速度v

其中，Ei表示上一图像帧编码器坐标系，ω

将运动模型的微分公式进行积分，能够得到两帧间预测旋转矩阵

进一步的，将预测旋转矩阵

也即，T

进一步的，通过公式T

所述追踪线程S11还包括对所述预测位姿进行优化，具体为：

通过预设的纯运动束调整(Motion-only BA：motion-only bunch adjustment)对所述当前图像帧的预测位姿对当前图像帧的预测位姿进行优化，得到当前图像帧的优化位姿；其中，所述纯运动束调整包括重投影误差和编码器误差。

本发明实施例中，通过纯运动束调整对当前图像帧的预测位姿进行优化，其中BA(bundle adjustment，束调整)即调整相机参数(主要指外参数：许多关键帧的位姿；也可能包括内参数：焦距和光心像素坐标)和结构(相机所看见的地图点的位置)，使得重投影误差(也即连接两个关键帧的边对应了一堆匹配点对，其中关键帧1上的点通过几何关系投影到关键帧2上后与匹配点的坐标差值)达到最小(即上述误差关于信息矩阵的二范数的平方和作为优化的目标函数达到最小)的非线性优化方法，而Motion-only BA只调整相机参数(主要指许多关键帧的位姿)，但是优化的目标函数依旧是基于重投影误差的一种非线性优化方法。

所述重投影误差通过局部地图的地图点在当前图像帧的预测位姿上对特征点进行重投影，计算重投影后的特征点和当前图像帧的特征点之间的误差。

所述纯运动束调整的目标函数的优化公式可以如公式(5)所示：

其中，

本实施例中，编码器误差项可通过如下公式(6)获取：

其中e

计算目标函数中的编码器误差项时会用到预积分的结果和下述公式，即编码器坐标系下的相对变换和相机坐标系下相对变换的关系：

即：

其中，R

上述的Motion-only BA使用非线性优化，可计算出使目标函数最小的当前图像帧的优化位姿R

另外，本实施例中非线性优化过程中要用到的雅克比Jacobian矩阵可以采用数值的，当然也可以采用李代数相关公式推导出的解析解，后者往往能让优化更稳定些。

在另一优选实施例中，当也采用IMU(inertial measurement unit)进行位姿优化时，所述纯运动束调整还包括IMU误差项。

则所述纯运动束调整的目标函数的优化公式可以如公式8所示：

其中，

现有技术中对于当前图像帧位姿的获取，通常预测式暴力匹配的方式，采用匀速运动模型的预测变换矩阵(包含旋转矩阵和平移矢量)来将上一图像帧的特征点投影到当前图像帧后选取了个小矩形范围进行快速暴力匹配的，利用上一图像帧的位姿和速度来估计当前图像帧的初始位姿，然后再对初始位姿进行优化得到优化位姿。而本发明采用编码器测量的轮速来估计当前图像帧的初始位姿，然后再对初始位姿进行优化得到优化位姿，鲁棒性更高。

进一步的，在上述实施例的基础上，如图3所示，在进行步骤S201纯运动束调整后，还可包括：

步骤S202、判断视觉是否失效；

若视觉失效，则执行步骤S203；若视觉未失效，则执行步骤S204；

步骤S203、以当前图像帧的预测位姿作为当前图像帧的位姿；

步骤S204、以当前图像帧的优化位姿作为当前图像帧的位姿。

在本实施例中，当纯运动束调整失败时，即通过上述所述纯运动束调整的目标函数不能获得所述优化位姿时，则确定视觉失效。若视觉失效，则说明Motion-only BA过程失效，也即最终优化得到的优化位姿不准确，与真实位姿也存在较大偏差，因此不予采用，而是以预测位姿作为当前图像帧的位姿；而视觉未失效，也即Motion-only BA过程成功，可将优化位姿作为当前图像帧的位姿。

在本实施例中，视觉失效可由以下原因导致：图像中特征点稀少(无纹理)或与上一图像帧重复太多、高速运动造成的模糊或无重叠区、相机帧率偶然降低等。

在一优选实施例中，当通过所述纯运动束调整模型不能获得优化结果时，确定当前图像帧相对于上一图像帧发生视觉失效。

在另一实施例中，在判断视觉失效时，可通过Motion-only BA得到的优化位姿，将当前图像帧、以及与当前图像帧具有相同特征点的帧进行特征点的重新匹配(或者将地图中的地图点与当前图像帧进行特征点匹配)，匹配过程中，可将该些帧中与当前图像帧的相同特征点投影到当前图像帧中，若某一特征点投影到当前图像帧后与当前图像帧对应的特征点之间的距离小于阈值，则确定为内点；否则，该特征点为外点；判断当前图像帧中内点的数量是否大于预设数量阈值，若大于或等于预设数量阈值，则说明Motion-only BA成功，也即视觉未失效；若小于预设数量阈值，则说明Motion-only BA失败，也即视觉失效。

在本实施例中，在视觉SLAM的追踪线程S11中，获取到当前图像帧相对于上一图像帧的位姿信息，即可根据相对位姿追踪可移动平台的运动轨迹。

本实施例中，当相机采集到当前图像帧的图像时，可根据上一图像帧相机的位姿以及编码器检测到的第一滚动轮速度和第二滚动轮速度信息来对当前图像帧相机的位姿进行预测，得到当前图像帧的预测位姿，并根据当前图像帧的预测位姿、当前图像帧的图像以及与当前图像帧具有相同特征点的关键帧进行特征点的匹配，以对当前图像帧的预测位姿进行优化，得到当前图像帧的优化位姿。

由于图像中特征点稀少(无纹理)或与上一图像帧重复太多、高速运动造成的模糊或无重叠区、相机帧率偶然降低等特殊问题导致视觉短暂失效时，也即无法通过当前图像帧以及与当前图像帧具有相同特征点的关键帧进行特征点匹配的方式进行当前图像帧位姿优化，则可以当前图像帧的预测位姿作为当前图像帧的位姿信息，从而保证后续流程的继续进行，避免由于视觉失效前后地图点和轨迹衔接不上导致无法继续工作。由于当前图像帧的预测位姿是基于编码器获得，而基于编码器不具有零漂的特性，可以保证短时间内的积累误差不会很大，从而可以解决视觉短暂失效到视觉恢复期间的过渡问题，从而提高视觉SLAM的稳定性和鲁棒性。

在本实施例中，在获取到当前图像帧的位姿信息后，则可根据图像帧以及对应的位姿信息进行局部建图线程S12。具体的，可首先确定当前图像帧是否为关键帧，若确定为关键帧，则可在后端通过关键帧的特征点的匹配，增加新的地图点，实现局部地图的构建以及全局地图的优化。

本实施例提供的视觉SLAM方法，通过获取相机图像以及编码器数据；根据所述图像帧以及所述编码器的测量数据，获取相机的位姿信息；根据所述图像帧以及所述位姿信息进行地图的构建。通过将编码器应用到视觉SLAM中，可提高视觉SLAM的稳定性和鲁棒性，可避免视觉短暂失效时视觉失效前后地图点和轨迹衔接不上导致无法继续工作。

在局部建图线程S12中，如图4所示，在获取当前图像帧的位姿后，还可包括：

步骤S301、判断当前图像帧是否为关键帧；

若当前图像帧为关键帧，则将当前图像帧增加到局部建图线程S12中，进行地图的构建。具体的，可包括：

步骤S302、当确定当前图像帧为关键帧时，在已构建地图的基础上，根据所述当前图像帧对所述已构建地图进行更新；

其中，所述已构建地图由在所述当前图像帧之前确定的关键帧进行构建。

在本实施例中，为了减少地图构建过程的计算资源消耗，仅将能够提供足够多地图点的关键帧增加到建图线程中，对于RGBD相机或双目相机也可将距离近的地图点直接作为新地图点插入地图中。

现有技术中通常会判断当前图像帧离上一关键帧之间的距离是否处于预设距离范围内，也即在当前图像帧离上一关键帧之间的距离不太远不太近的情况下，才会将当前图像帧作为新关键帧；或者当前图像帧的特征点匹配数量大于预设匹配数量阈值且提供足够多的新地图点的时候，也会将当前图像帧作为新关键帧，其中特征点匹配数量指当前图像帧的特征点和地图中的地图点的匹配数量，或者当前图像帧的特征点和与当前图像帧具有相同特征点的关键帧的特征点之间的匹配数量；对于单目相机与IMU紧耦合的情况，为了防止零漂还需要当前图像帧与上一关键帧的时间间隔不能太长，也即当前图像帧与上一关键帧的时间间隔超过预设时间间隔阈值时，一定会将当前图像帧作为新关键帧。

而本实施例中，在视觉未失效时，可采用上述现有技术中的关键帧判断策略；而在视觉失效时，由于可通过编码器数据获取到当前图像帧的预测位姿，允许两帧之间通过编码器连接(也即允许纯编码器边pureencoder edges)，可不需要当前图像帧离上一关键帧之间的距离必须大于预设距离范围中的最小值，也不需要当前图像帧的特征点匹配数量大于预设匹配数量阈值，因此可以去掉上述两项限定；但对于RGBD相机或双目相机，需要增加当前图像帧创建地图点的最小数量限制，也即当前图像帧创建地图点的数量需要大于预设地图点数量阈值，以防止贡献的地图点太少而失去加入的意义；对于单目相机与IMU紧耦合的情况，增加一个最短时间间隔限制来试探性的在建图线程(局部建图)中通过三角化构建一些地图点以恢复视觉追踪，也即当前图像帧与上一关键帧的时间间隔需要大于预设最短时间间隔，并且由于建图线程(局部建图)中会去掉冗余或不重要的关键帧和地图点，所以不用担心单目相机所采用的时间判据会带来冗余的问题。

进一步的，当视觉恢复后，可将上述视觉失效时的关键帧判断策略恢复到上述现有技术中的关键帧判断策略。

进一步的，如图5所示，根据所述当前图像帧对所述已构建地图进行更新，包括：

步骤S401、根据预设的策略取出冗余关键帧和冗余地图点，并根据所述当前图像帧进行三角化操作生成新的地图点；

步骤S402、在生成新的地图点后，构建所述当前图像帧的共视图进行局部束调整，以对与所述当前图像帧共视的其他关键帧、当前图像帧能看到的地图点以及所述其他关键帧能看到的地图点进行优化；其中，所述局部束调整包括编码器误差项。

在本实施例中，在获取到新关键帧后，可首先将去除冗余地图点，其中冗余地图点可包括不在当前关键帧附近的被看到次数过少的地图点及前几帧里被连续看到的次数比率较小的地图点，对于当前图像帧中能够和已有地图点进行融合的特征点，可通过融合来取代已有地图点。

在进行局部BA优化关键帧位姿和地图点后，还可去除冗余关键帧，其中冗余关键帧是指存在至少1个其他的关键帧和其看到相同地图点数量超过阈值(例如90％)的情况，本实施例中，会判断和当前关键帧能看到相同地图点的那些关键帧(共视图里的帧节点)是否冗余。通过上述的冗余地图点和冗余关键帧的去除，可以避免地图中地图点过多，减少计算量，提高建图效率。

此外，如图6所示，所述构建所述当前图像帧的共视图进行局部束调整，包括：

步骤S501、当任一关键帧与前一个关键帧由纯编码器边连接，且所述关键帧与后一个关键帧也由所述纯编码器边连接时，去除所述关键帧，根据所述前一关键帧的时间戳和所述后一关键帧的时间戳获取第二时间段；其中，所述纯编码器边为相邻关键帧之间发生所述视觉失效；

步骤S502、获取所述第二时间段内第一编码器的所有测量数据和第二编码器测的所有测量数据；

步骤S503、根据所述第二时间段内第一编码器测量的所有测量数据和第二编码器的所有测量数据，获得所述后一关键帧相对于所述前一关键帧的预测变换矩阵。

具体的，在获取到新关键帧后，还可去除连续的纯编码器边连接的关键帧，其中连续的纯编码器边连接的关键帧例如“关键帧1-编码器边1-关键帧2-编码器边2-关键帧3”，关键帧2相对于关键帧1的相对位姿是通过编码器数据得到的预测位姿(也即编码器边1)，关键帧3相对于关键帧2的相对位姿也是通过编码器数据得到的预测位姿(也即编码器边2)，去除连续的纯编码器边可减少后续优化的计算量，并防止只用编码器数据来进行三角化造成误差较大(其中由于编码器的精度和与地面存在摩擦，导致编码器比视觉的精度低)。去除连续的纯编码器边连接的关键帧，针对上述举例的三个关键帧，可删除连续的纯编码器边连接的关键帧2，从而得到“关键帧1-编码器边-关键帧3”，其中编码器边需要重新计算，也即通过上述实施例中的预积分过程计算关键帧3相对于关键帧1的预测位姿。

在本实施例中，可通过三角化创建新的地图点，其中三角化是根据两个点(关键帧相对位姿)和过它们的射线(匹配点对的像素坐标，即方向)，得到射线交点相对这两个点的位置(匹配点对或3D点的深度或位置)。

具体的，对于双目相机，对于空间中存在的某一固定的3D点X，该3D点X被双目相机的左相机和右相机观测到，该3D点在左相机成像位置为x，在右相机成像位置为x’，根据针孔相机模型和相机投影方程，我们可以根据x和x’分别计算出两条射线，且这两条射线埋讹上会经过该3D点X。基于此理论，两条射线的交点就是空间3D点X的位置，从而可以确定3D点X的位置。对于单目相机，可以以相邻两帧(已知两帧间的相对位姿)代替双目相机的左相机和右相机采集的图像，该相邻两帧同时观测到空间中某一固定的3D点X，其三角化过程与双目相机相同，此处不再赘述。对于RGBD相机，由于预处理过程已计算出特征点相对于虚假的右相机的坐标，将RGBD相机采集的图像等效为双目相机采集的图像，因此可以采用与双目相机相同的三角化过程，此处也不再赘述。

在本实施例中，可通过局部BA优化关键帧位姿和地图点，其中局部BA过程与上述实施例中Motion-only BA优化过程类似，其局部BA的目标函数同样也加入编码器误差项。现有技术中的局部BA中，参与局部BA的关键帧通常为当前关键帧的共视图(也即图包含帧和地图点两类顶点、及帧和它能看见的地图点之间的重投影误差边，这些关键帧能看到当前关键帧所看到的地图点，图中地图点是这些关键帧能看到的所有地图点)，而本实施例中参与局部BA的关键帧改为当前图像帧往前推N个连续的关键帧(N大于等于1)，可通过该N个连续的关键帧构建局部BA的目标函数，从而对当前关键帧的位姿以及地图点进行优化，可减小运算量。

进一步的，在得到局部地图后，将当前关键帧送入回环检测线程S13，以进行闭环检测，以判断可移动平台是否位于曾经到达过的位置。

现有技术中的闭环检测，查询地图数据库，通过RANSAC(Random SampleConsensus，随即采样一致性)算法计算当前关键帧和闭环关键帧的相似变换(SE3或Sim3)，通过闭环融合以融合当前关键帧和闭环关键帧附近关键帧所看到的地图点。其中RANSAC算法是随机的采用几对匹配点来计算变换，使用概率统计的理论来求出最正确的变换，并区分哪些匹配点对是正确的(匹配点对正确，则称作内点)。

在本实施例中，可通过获取每一关键帧的词袋(Bag-of-words，BoW)矢量，获取任一关键帧的词袋矢量与当前关键帧的词袋矢量之间的距离(例如欧氏距离)，根据两词袋矢量之间的距离判断是否存在闭环，其中词袋矢量是以根据关键帧的特征点以及预设特征点词袋获取，其中预设特征点词袋是通过特征聚类而构建，其中包含以预定顺序排列的特征点信息，而关键帧特征点通过与预设特征点词袋中的每一特征点信息进行匹配，若预设特征点词袋中的某一特征点在关键帧中出现则置1，未出现则置0，从而得到该关键帧的词袋矢量。在数据库中会存储每一关键帧的词袋矢量，可通过将当前关键帧的词袋矢量与数据库中的各关键帧的词袋矢量进行距离判断，来判断当前关键帧是否发生闭环，其中匹配的关键帧即为当前图像帧的闭环关键帧。

进一步的，本实施例中，可在视觉失效后，降低检测闭环的阈值，其中检测闭环的阈值包括重复检测次数、计算相似变换的匹配点数和内点数的阈值，通过放宽检测闭环的条件，可以在视觉失效后快速检测闭环，以减小视觉失效过程中由纯编码器边产生的误差。而在视觉恢复后，可将检测变换的阈值恢复为正常阈值，可避免过度频繁的闭环消耗计算量。

进一步的，当检测到闭环时，可进行位姿图优化。

其中，位姿图为顶点不含地图点、只含所有关键帧、边只包含支撑树上帧之间的相对变换和所有闭环边对应的相似变换的图，其中支撑树(spanning tree)是连接整个地图中关键帧的无回环图，一般时间上连续的两关键帧会成为父子节点，但当删除某父亲节点时会根据共同看见的地图点数量去修改子节点的父节点(此时对应去除关键帧的操作，会更新子节点的预积分)，根节点为视觉SLAM系统建图时最初的关键帧。

本实施例中位姿图优化的是每个关键帧的位姿，目标函数是相对变换

本实施例中，当相邻关键帧之间发生视觉失效时，根据所述预测变换矩阵生成修正协方差矩阵修改所述相邻关键帧之间的相对变换的协方差矩阵。具体的，可通过预积分得到的协方差矩陌

进一步的，在位姿图优化完成后，还可进行全局BA优化。其中全局BA与上述实施例中Motion-only BA、局部BA优化过程类似，全局BA的目标函数同样也加入编码器误差项，而与局部BA优化过程不同之处在于，参与全局BA的关键帧为整个地图库中所有关键帧和地图点。在完成全局BA后，可更新地图，更新全部地图点、更新地图库中全部关键帧直到最新的关键帧。

在上述任一实施例的基础上，若视觉SLAM与IMU紧耦合，则需要对IMU进行初始化，以获取IMU的零偏(bias)，包括IMU中陀螺仪的零偏和加速度计的零偏，还可获取重力矢量，此外，对于单目相机与IMU紧耦合的情况，还需要通过IMU初始化确定地图尺度，而双目相机和RGBD相机通常不需要通过IMU初始化确定地图尺度，当然通过IMU初始化对双目相机和RGBD相机确定地图尺度时，初始化结果接近为1。

本实施例中，可以在视觉SLAM刚开始运行时，取最开始的预定数量的关键帧(例如20个)，根据该些关键帧获取IMU的零偏、重力矢量、地图尺度，然后将地图尺度更新到所有的关键帧中，包括该预定数量的关键帧以及在初始化过程中得到的新的关键帧中，然后进行全局BA优化更新地图尺度，之后视觉SLAM可以以更新后的地图尺度运行。

在上述任一实施例的基础上，本实施例还提供一种轻量级定位方式。

具体的，在本实施例中在完成地图构建后可采用最简保存方式，即只保存必要的地图的关键帧和地图点信息，可采用二进制文件存储。主要存储内容涉及传感器类型；关键帧的序号、位姿、词袋矢量、特征点序列和帧间传感器信息序列、帧所能观测的地图点序号信息、帧所连接的闭环关键帧序号信息；地图点的序号、位置、参考关键帧序号、地图点能被观测到的关键帧序号信息；支撑树结构(即父节点序号)

在读取时，采取逆保存操作，重新构建关键帧和地图点、更新相关信息(如支撑树结构、共视图信息和将关键帧加入地图数据库等操作)，构建关键帧和地图点时可重新创建关键帧的序号和地图点的序号，可不采用原序号。

本实施例中可将运行很久的视觉SLAM系统的地图信息以很小的存储空间进行存储，可以提高视觉SLAM系统的最大工作时长，但重新载入时需要花费较多的重构地图和轨迹的时间。在采用上述重构的地图时，视觉SLAM可仅启动追踪线程S11进行定位，而不需要再开启局部建图线程S12和闭环线程，实现轻量级定位。需要说明的是，对于单目相机与IMU紧耦合的情况，仍需要IMU初始化(获取初始的零偏与重力加速度)。

本发明实施例提供一种可移动平台。图7为本发明实施例提供的可移动平台的结构图，如图7所示，所述可移动平台700包括第一滚动轮701、第二滚动轮702、第一编码器703、第二编码器704、摄像头705、存储器707和处理器706；

所述第一滚动轮701和第二滚动轮702用于使得所述可移动平台发生位移，所述第一滚动轮701和第二滚动轮702的轴线重合；

所述摄像头705用于采集图像帧；

所述第一编码器703用于检测所述第一滚动轮701的速率，所述第二编码器704用于检测所述第二滚动轮702的速率；

所述存储器707用于存储程序代码；

所述处理器706，调用所述程序代码，当程序代码被执行时，用于执行以下操作：

根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；

获取所述第一时间段内第一编码器703的所有测量数据和第二编码器704测的所有测量数据；

根据所述第一时间段内第一编码器703测量的所有测量数据和第二编码器704的所有测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；

根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿。

在上述实施例的基础上，在所述处理器706根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵时，所述处理器被配置为：

根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获取当前图像帧的旋转矩阵关于时间的第一导数，以及当前图像帧的位置关于时间的第二导数；

基于所述第一时间段，分别对所述第一导数和第二导数进行积分获取当前图像帧相对于上一图像帧的的预测旋转矩阵和预测平移矩阵；

根据当前图像帧相对于上一图像帧的预测旋转矩阵和预测平移矩阵，获得当前图像帧相对于上一图像帧的预测变换矩阵。

在上述任一实施例的基础上，在所述处理器706根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿后，所述处理器706还被配置为：

根据预设的纯运动束调整模型对所述当前图像帧的预测位姿进行优化；其中，所述纯运动束调整模型包括编码器误差项，所述编码器误差项通过所述第一编码器703的测量数据和所述第二编码器704的测量数据得到。

在上述任一实施例的基础上，所述处理器706还被配置为：

若当前图像帧相对于上一图像帧发生视觉失效时，所述当前图像帧的位姿为所述预测位姿；

若当前图像帧相对于上一图像帧未发生视觉失效时，所述当前图像帧的位姿为所述纯运动束调整模型的优化结果。

在上述任一实施例的基础上，所述处理器706还被配置为：

当通过所述纯运动束调整模型不能获得优化结果时，确定当前图像帧相对于上一图像帧是否发生视觉失效；或，

根据所述优化位姿，将所述当前图像帧和所述上一图像帧进行特征匹配获取内点数量；

当所述内点数量小于预设的阈值时，则确定当前图像帧相对于上一图像帧是否发生视觉失效。

在上述任一实施例的基础上，所述处理器706还被配置为：

当确定当前图像帧为关键帧时，在已构建地图的基础上，根据所述当前图像帧对所述已构建地图进行更新；所述已构建地图由在所述当前图像帧之前确定的关键帧进行构建。

在上述任一实施例的基础上，在所述处理器706根据所述当前图像帧对所述已构建地图进行更新时，所述处理器706被配置为：

根据预设的策略取出冗余关键帧和冗余地图点，并根据所述当前图像帧进行三角化操作生成新的地图点；

在生成新的地图点后，构建所述当前图像帧的共视图进行局部束调整，以对与所述当前图像帧共视的其他关键帧、当前图像帧能看到的地图点以及所述其他关键帧能看到的地图点进行优化；其中，所述局部束调整包括编码器误差项。

在上述任一实施例的基础上，在所述处理器706构建所述当前图像帧的共视图进行局部束调整时，所述处理器706被配置为：

当任一关键帧与前一个关键帧由纯编码器连接，且所述关键帧与后一个关键帧也由所述纯编码器边连接时，去除所述关键帧，根据所述前一关键帧的时间戳和所述后一关键帧的时间戳获取第二时间段；其中，所述纯编码器边为相邻关键帧之间发生所述视觉失效；

获取所述第二时间段内第一编码器703的所有测量数据和第二编码器704测的所有测量数据；

根据所述第二时间段内第一编码器703测量的所有测量数据和第二编码器704的所有测量数据，获得所述后一关键帧相对于所述前一关键帧的预测变换矩阵。

在上述任一实施例的基础上，在所述处理器706构建所述当前图像帧的共视图进行局部束调整时，所述处理器706还被配置为：

获取所述当前图像帧之前预设的N个连续关键帧，根据所述当前图像帧及其所能看见的地图点、所述N个连续关键帧及其能看见的地图点构建局部优化图；N大于等于1；

根据所述局部优化图中任意两个相邻关键帧之间的预测变换矩阵构建所述编码器误差项，并根据所述地图点在各个关键帧上的位姿进行重投影获取重投影误差；

根据所述编码器误差和重投影误差对所述当前图像帧及其所能看见的地图点、所述N个连续关键帧及其能看见的地图点进行优化，更新所述局部优化图中所有关键帧的位姿及所有地图点的位置。

在上述任一实施例的基础上，所述处理器706还被配置为：

根据当前图像帧的特征点进行闭环检测；

当检测到所述当前图像帧和任一关键帧发生闭环时，融合当前图像帧及所述关键帧附件的关键帧所看到的地图点。

在上述任一实施例的基础上，在所述处理器706进行闭环检测后，所述处理器706还被配置为：

构建位姿图以对所有关键帧进行优化，以更新所有关键帧的位姿；所述位姿图包括所有的关键帧以及两个关键帧之间的相对变换。

在上述任一实施例的基础上，在所述处理器706构建位姿图以对所有关键帧进行优化时，所述处理器706被配置为：

当相邻关键帧之间发生视觉失效时，根据所述预测变换矩阵生成修正协方差矩阵修改所述相邻关键帧之间的相对变换的协方差矩阵。

在上述任一实施例的基础上，在所述处理器706构建位姿图以对所有关键帧进行优化后，所述处理器706还被配置为：

根据预设的全局束调整对所有的关键帧和所有的地图点进行更新。

在另一实施例中，所述处理器706，调用所述程序代码，当程序代码被执行时，用于执行以下操作：

根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；

获取所述第一时间段内第一编码器703的所有测量数据和第二编码器704测的所有测量数据；

根据所述第一时间段内第一编码器703测量的所有测量数据和第二编码器704的所有测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；

根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿；

在获得所述当前图像帧的预测位姿后，在已构建地图的基础上，根据所述当前图像帧对所述已构建地图进行更新；所述已构建地图由在所述当前图像帧之前确定的关键帧进行构建。

本发明实施例提供的可移动平台的具体原理和实现方式均与上述实施例类似，此处不再赘述。

本实施例提供的可移动平台，通过根据当前图像帧的时间戳和上一图像帧的时间戳获取第一时间段；获取所述第一时间段内第一编码器的测量数据和第二编码器测的测量数据；根据所述第一时间段内第一编码器的测量数据和第二编码器的测量数据，获得当前图像帧相对于上一图像帧的预测变换矩阵；根据所述预测变化矩阵以及上一图像帧的位姿信息获得所述当前图像帧的预测位姿。本实施例通过将编码器应用到视觉SLAM中，通过编码器测量数据预测当前图像帧的位姿，可提高位姿获取的准确性，提高视觉SLAM的稳定性和鲁棒性，在视觉失效时视觉SLAM仍能稳定的运行。

另外，本实施例还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行以实现上述实施例所述的位姿预测方法和/或地图构建方法。

另外，本实施例还提供计算机程序，包括程序代码，当计算机运行所述计算机程序时，所述程序代码执行如上述实施例所述的位姿预测方法和/或地图构建方法。

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

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

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施例所述方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

本领域技术人员可以清楚地了解到，为描述的方便和简洁，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将装置的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。上述描述的装置的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

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