一种融合GPS的机器人视觉惯导组合定位方法

摘要

本发明公开了融合GPS的机器人视觉惯导组合定位方法，包括提取并匹配双目相机左右图像与前后图像特征点，计算特征点三维坐标与图像帧相对位姿；选取图像流中的关键帧，创建滑动窗口，并将关键帧加入滑动窗口；计算视觉重投影误差、IMU预积分残差与零偏残差并组合为联合位姿估计残差；使用L‑M法对联合位姿估计残差进行非线性优化，得到优化后的视觉惯导(VIO)机器人位姿；若当前时刻有GPS数据，则对GPS位置数据与VIO位姿估计数据作自适应抗差卡尔曼滤波，得到最终的机器人位姿；若无GPS数据，则以VIO位姿数据代替最终位姿数据。本发明提高了机器人的定位精度，减小了计算消耗，适应大范围、长时间的巡检需求。