[发明专利]基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

说明书

本发明公开了一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，该方法具体如下：在载体上安装单目相机与惯性测量单元，运用ROS中消息过滤器实现单目相机和惯性测量单元的时间戳同步，计算前后两帧图像之间的位姿变化，并计算其相应时间内的惯性测量单元解算得到的位置，速度，旋转等变化信息，将惯性测量单元得到的位置、速度与旋转等作为系统的状态变量，视觉传感器得到的位姿变化信息作为观测量建立系统方程。并通过一次迭代扩展卡尔曼滤波的方法对两种传感器获得的信息进行融合，实现载体的实时状态估计与导航。本发明可以在长时间实时定位与导航过程中保持较高的精度，且具有帧间计算复杂度不变的优点。

技术领域

本发明涉及导航技术领域，特别是涉及基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法。

背景技术

近年来，导航相关的仪器设备有着较为突破的发展，设备的性能和精度有着大幅度的提升，但单一传感器实现的的导航方法仍具有某些性能上的局限性。为了满足高性能的导航需求，组合导航方法近些年来得到了广泛的重视和发展。组合导航方法将多种导航传感器组合在一起，利用多种传感器测得信息对其各自局限性进行相互补偿以实现高精度导航并且增强系统的鲁棒性。

惯性导航是一门综合类技术，是现代科学发展到一定阶段的产物。惯性导航中主要利用了IMU作为传感器进行数据采集，一般IMU中会包含一个三轴的陀螺仪和加速度计，陀螺仪用于测量角速率，加速度计用于测量三轴方向上的加速度。在已知IMU初始位置、速度和姿态的前提下，利用航位推算的方法可以实现实时估算IMU位置、速度和姿态。纯惯性的导航方法只在初始时刻附近有较好的精度，这是因为IMU采集到的数据包含陀螺和加速度计的漂移使得纯惯性导航的精度会随时间发散。

单目相机以其结构简单、标定简单与价格低廉的优点而得到广泛的应用，但是单目相机由于只能在同一时刻获得一张图片而无法从图像中直接获得像素的深度信息。除此之外，单目相机还有着尺度不确定性的特点，该特点也是导致单目测量产生误差的主要原因。通过加入IMU数据可以解决单目相机初始化尺度的问题。

基于滤波的视觉和惯性的组合导航方法按照是否把图像状态信息加入到状态向量可以分为紧耦合和松耦合两种方式。紧耦合方式由于将图像特征信息加入到状态向量中使得计算量大大增加，其计算复杂度与图像特征数量程线性关系，且可扩展性差。松耦合虽然精度上略差于紧耦合，但其计算复杂度却远小于紧耦合。其次，松耦合具有较好的可扩展性和帧间计算复杂度不变的优点。本文采用了一次迭代扩展卡尔曼滤波的方法实现单目相机与惯性传感器的松耦合，该方法相较于扩展卡尔曼滤波有着更高的精度，且在计算复杂度上比无极卡尔曼滤波方法更为简单。

发明内容

为了解决以上问题，本发明提供一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，本发明可以在长时间实时定位与导航过程中保持较高的精度，且具有帧间计算复杂度不变的优点，为达此目的，本发明提供基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，该方法包括如下步骤：

步骤1：对IMU以及单目相机采集到的信息进行时间戳同步；

步骤2：计算单目相机连续两帧图像间的位姿变化；

步骤3：解算图像间IMU数据得到惯性测量的位置、速度与姿态变化；

步骤4：建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合。

作为本发明进一步改进，所述步骤1中对IMU以及单目相机采集到的信息进行时间戳同步，具体方法如下：

在机器人操作系统ROS平台上，利用ROS中消息过滤器进行传感器采集信息时间戳的匹配，每两帧图像之间有较多IMU数据，从图像的时间戳上搜索最近的IMU采集信息进行时间戳的同步。

作为本发明进一步改进，所述步骤2中计算单目相机连续两帧图像间的位姿变化，具体方法为：