一种基于地图的一致性高效视觉惯性定位算法

摘要

本发明公开了一种基于地图的一致性高效视觉惯性定位算法，算法通过以下系统来实现，系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块；本发明能够将地图的不确定性考虑进去，通过实时地估计里程计坐标系和地图坐标系的相对位姿，完成基于地图的一致性定位，通过利用Schmidt滤波，在维护机器人状态与地图信息之间协方差的同时，保证了算法的计算效率，使系统的计算量随着地图信息量的增多而仅呈线性增长，为了保证系统的正确的能观性，该算法提出了“第一次估计雅克比”(FEJ)算法，提出了误差补偿算法，该算法在低频地图匹配的大场景中非常有必要。