【技术实现步骤摘要】
一种基于地图的一致性高效视觉惯性定位算法
[0001]本专利技术涉及机器人定位技术，具体地说，涉及一种基于地图的一致性高效视觉惯性定位算法。
技术介绍
[0002]精确的定位算法对于移动机器人来说是非常重要的。移动机器人的运动导航离不开地图。这就要求系统能够实时地获取机器人在地图中的位姿。同时，借助事先建好的地图信息，来约束里程计产生的累积漂移。
[0003]目前绝大多数的基于地图的定位算法都是认为地图信息是完美的，没有不确定性的。同时，通过构造代价函数，利用非线性优化的方法来获取定位信息。这类方法的主要缺陷在于，错误地将不完全精准的地图看成的精准的。这就导致系统过度信任地图信息，进而使系统的估计过于乐观，产生不一致的问题。此外，基于非线性优化的算法对计算资源有较高的要求，这使得算法难以部署在轻量化的平台上。
[0004]相比之下，基于滤波的算法可以天然地将地图的不确定度考虑进去。而且，相比于非线性优化的算法，基于滤波的算法对计算资源的要求低，适合部署在轻量化的平台上。但是基于滤波的算法需要将地图信息加入到状...
【技术保护点】
【技术特征摘要】
1.一种基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的算法通过以下系统来实现，所述的系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，所述的局部里程计用于接收相机和IMU的数据，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；所述的地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；所述的基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后的局部里程计状态以及局部参考和地图参考系的相对变换。2.根据权利要求1所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的观测融合状态更新模块。3.根据权利要求2所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的IMU用于为系统提供实时的旋转角速度和线加速度；所述的状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将系统的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量与协方差；获得的状态变量与协方差信号传递给观测模块，所述的特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给观测融合状态更新模块；所述的观测融合状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用扩展卡尔曼滤波(Extended Kalman Filter，EKF)来更新状态变量与协方差，整个过程与多状态约束卡尔曼滤波(MSCKF)一致。4.根据权利要求3所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的状态变量及其对应的协方差需要包含以下变量：1)系统当前时刻本体的状态：包括由旋转矩阵和平移向量组成的位姿，线速度；2)IMU传感器的关于角速度与线加速度的偏差(bias)；3)系统过去N个时刻的本体位姿，N为所设定的滑动窗口的大小；4)局部里程计参考系和地图参考系之间的位姿变换
G
T
L
，该变量需要在基于地图的定位模块中的初始化局部与地图参考系之间的相对位姿
G
T
L
模块加入到状态变量中；5)地图中关键帧的位姿，该变量在基于地图的定位模块中将地图关键帧加入状态向量模块加入到状态变量中。5.根据权利要求1或2或3或4所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的地图特征匹配模块用于通过当前相机与地图的信息，获得与当前相机匹配到的地图特征点以及能观测到这些地图特征点的地图关键帧，即匹配的特征对与地图关键帧，所述的匹配的特征对与地图关键帧信号传递至基于地图的定位模块。6.根据权利要求5所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的基于地图的定位模块包括以下模块：初始化局部与地图参考系之间的相对位姿
G
T
L
模块，将地图关键帧加入状态向量模块和利用地图信息进行状态更新模块；所述的初始化局部与地图参考系之间的相对位姿
G
T
L
模块用于计算
G
T
L
，并将其加入到系统的状态向量中，获得增广的系统状态变量和协方差；将地图关键帧加入状态向量模块用于将地图关键帧的位姿和协方差加入系统的状态变量和系统的协方差中，从而天然地考虑了地图的不确定度，进而提升系统的一致性，通过该模块我们
获得了增广的状态变量和协方差；所述的利用地图信息进行状...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。