随着机器人在不同领域的应用,机器人的位姿估计成为研究热点。机器人的位姿估计是机器人应用的基础技术,是实现机器人上层需求的关键。当下机器人的应用场景不断增加,为了实现机器人在不同复杂环境下的精确位姿估计,本文实现了基于扩展卡尔曼滤波的激光雷达、双目视觉、惯性测量元件融合的里程计。本文设计了一种基于双阶扩展卡尔曼滤波的双目视觉惯性里程计,第一阶扩展卡尔曼滤波用作融合惯性测量元件中陀螺仪和加速度计的测量输出;第二阶扩展卡尔曼滤波用作融合第一阶扩展卡尔曼滤波的输出和双目的测量。相比于之前的基于扩展卡尔曼滤波的视觉惯性里程计,增加了利用加速度计修正陀螺仪误差的环节,更加充分的利用了传感器之间的互补特性。在公开数据集Eu Roc上,本文的方法精度比改进前的方法提升15.7%;在实际场景的实验中也有更好的表现。同时,本文对随机采样一致性算法在视觉匹配点对筛选的应用进行了改进。利用两帧图像之间IMU解算的转换信息,把上一帧图像旋转到当前帧,再利用不同的特征点对在两个图像帧归一化平面上的距离比近似相等,来减少随机采样一致性算法的循环次数。通过在公开数据集Eu Roc上的实验,该改进能在保证匹配精度的前提下,将RANSAC算法耗时减少74%。针对双目视觉惯性里程计在黑暗环境、纹理不足场景下视觉失效的情况,在双目视觉惯性里程计中加入激光雷达的信息作为测量更新,提高原系统的鲁棒性。通过在激光点云上提取角点和平面点,并在相邻帧根据点线、点面ICP寻找匹配点,把匹配点对形成的观测约束加入到视觉惯性里程计中作为测量更新。通过实际场景实验,验证融合激光雷达后,提高了原始双目视觉惯性里程计在复杂场景下的鲁棒性。