[发明专利]基于滑动窗口优化的模板匹配视觉和惯性组合里程计

摘要

本发明公开了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计，在载体上安装VO和IMU，根据IMU测量到的米制轨迹对VO测量到的非米制轨迹进行校正，得到单目VO的尺度因子；对VO采集到的图像进行仿射变换和模板匹配，得到基于VO观测量的视觉残差；对IMU测量值进行预积分，得到基于VO观测量的惯性残差；建立滑动窗口，判断当前帧是否发生了足够的运动，若是，将当前帧加入滑动窗口进行优化，并将窗口内最早的一帧边缘化，否则，将当前帧直接从滑动窗口中剔除；解算滑动窗口的非线性最小二乘方程，得到载体位置、速度、姿态的最优估计值。本发明有效提高了组合里程计的定位实时性，且具有更好的鲁棒性。