[发明专利]一种视觉惯性定位方法、装置及自动驾驶装置

权利要求书

1.一种视觉惯性定位方法，其特征在于，所述方法包括：

获取传感器采集的当前帧、历史地图以及惯性信息，其中，所述历史地图包括由所述当前帧之前的历史帧构建的三维地图，所述惯性信息包括所述当前帧和所述当前帧的前一帧之间的惯性数据；

将所述当前帧加入时域滑窗，并基于所述时域滑窗以及空域滑窗，更新先验约束条件；

基于所述当前帧以及所述历史地图，确定视觉约束条件；

基于所述当前帧与所述前一帧之间的惯性数据，确定惯性约束条件；

基于先验约束条件、所述视觉约束条件以及所述时域滑窗中每相邻两帧图像帧之间的惯性约束条件进行视觉惯性优化，确定所述传感器的当前位姿，其中，所述先验约束条件为上一次更新得到的。

2.根据权利要求1所述的方法，其特征在于，所述基于所述时域滑窗以及空域滑窗，更新先验约束条件的步骤，包括：

按照预设策略更新所述时域滑窗以及空域滑窗，并更新先验约束条件；

其中，所述预设策略使得所述时域滑窗包括的图像帧在时域的分布范围不超过第一阈值，所述空域滑窗包括的图像帧在空域上的分布范围不小于第二阈值，且所述空域滑窗包括的图像帧的数量保持不变。

3.根据权利要求2所述的方法，其特征在于，所述按照预设策略更新所述时域滑窗以及空域滑窗，并更新先验约束条件的步骤，包括：

基于所述时域滑窗中的最老帧与所述时域滑窗的最老帧的前一帧的数据关联量，判断所述时域滑窗中的最老帧是否为关键帧；

如果所述最老帧不为关键帧，将所述最老帧从所述时域滑窗中删除，并保存所述最老帧对应的惯性信息至先验约束条件；

如果所述最老帧为关键帧，将所述空域滑窗中的目标帧删除，并将所述最老帧加入所述空域滑窗，保存所述最老帧的惯性信息至先验约束条件，其中，所述目标帧与所述当前帧的数据关联量不超过第三阈值且在空域上与所述当前帧的距离不超过第四阈值。

4.根据权利要求1所述的方法，其特征在于，所述基于所述当前帧以及所述历史地图，确定视觉约束条件的步骤，包括：

基于所述当前帧中特征点的位置与目标投影点的位置之间的位置关系，确定视觉约束条件；

其中，所述目标投影点为所述特征点在所述历史地图中的对应点投影至所述当前帧中的点。

5.根据权利要求1所述的方法，其特征在于，在所述将所述当前帧加入时域滑窗的步骤之前，所述方法还包括：

确定所述当前帧与所述前一帧的数据关联量；

如果所述数据关联量小于预设值，确定所述当前帧为关键帧，并基于所述关键帧，通过三角化方式构建地图；

基于所构建的地图更新所述历史地图。

6.根据权利要求1所述的方法，其特征在于，所述传感器为单目里程计；

在所述获取传感器采集的当前帧、历史地图以及惯性信息的步骤之前，所述方法还包括：

获取传感器采集的预设数量的图像帧，基于所述预设数量的图像帧进行视觉初始化，得到无尺度地图；

获取传感器采集的图像帧，每获取一帧图像帧，基于该图像帧对所述无尺度地图进行优化，得到目标无尺度地图；

获取传感器采集的图像帧，每获取一帧图像帧，基于该图像帧以及所述目标无尺度地图进行惯性数据初始化，得到地图尺度和重力向；

按照所述地图尺度，调整所述目标无尺度地图的地图尺度，并按照所述重力向，将所述目标无尺度地图对齐至惯性系。

7.根据权利要求6所述的方法，其特征在于，所述基于所述预设数量的图像帧进行视觉初始化，得到无尺度地图的步骤，包括：

针对采集时间相邻的每两个图像帧，计算得到该两个图像帧的帧间光流数据；

根据所述帧间光流数据计算得到所述传感器的基础矩阵，并分解所述基础矩阵得到该两个图像帧对应的位姿；

基于该两个图像帧对应的位姿，通过三角化方式确定地图点；

将所述地图点投影至对应的图像帧中，确定重投影误差；

在对应的重投影误差小于预设值的地图点满足预设条件的情况下，得到无尺度地图。