一种基于立体视觉惯性里程计的车辆定位导航方法及系统

摘要

本发明公开了一种基于立体视觉惯性里程计的车辆定位导航方法及系统，该方法包括如下步骤：捕获待观测点的位姿信息和特征数据，进行预校准，得到位姿偏差信息和状态信息；将位姿信息和特征数据进行融合，得到相机位姿估计，加入到预设的INS状态向量中，增强INS状态和相机位姿的协方差矩阵；通过左侧相机和右侧相机观测所有图像中的特征，进行相机校准、特征检测、特征匹配，据此跟踪所有特征，得到特征信息；根据特征信息、增强后的INS状态和相机位姿的协方差矩阵进行滤波处理，校准位姿偏差信息和状态信息，更新完成跟踪的特征、INS状态和相机位姿，据此输出定位结果。