[发明专利]一种单目视觉惯性组合的定位导航方法

权利要求书

1.一种单目视觉惯性组合的定位导航方法，其特征在于，包括：

步骤一、通过单目相机采集视频流；通过IMU采集IMU数据流；

步骤二、将采集的视频流和IMU数据流进行对齐，再将对齐后的视频流和IMU数据流打包在一起；

步骤三、系统数据初始化；若初始化成功，则顺序执行步骤四和步骤五；若初始化失败，则重新进行初始化；初始化过程包括视觉初始化、IMU初始化、确定视觉坐标系和IMU世界坐标系之间的转换关系、非线性优化确定尺度初始值和用λI-EKF对尺度进行求精估计；

步骤四、获取IMU数据流中的惯导数据，对惯导数据进行互补滤波得到姿态变化量，再结合预积分计算得到IMU位姿，并将IMU位姿变化量给步骤五作为计算当前视觉帧位姿的参考变化量；

步骤五、逐帧提取视频流中的图像特征，利用提取的图像特征进行视觉跟踪和匹配，得到视觉位姿；

步骤六、判断步骤五中的视觉跟踪是否丢失，若视觉跟踪丢失并且不能重定位，则用IMU位姿进行运动定位；若视觉跟踪未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终用于定位导航的相机位姿。

2.如权利要求1所述的方法，其特征在于，所述步骤二中的对齐包括时间戳同步；打包包括数据打包。

3.如权利要求1所述的方法，其特征在于，所述步骤三中初始化的步骤具体包括：

3.1)视觉初始化：选取特征点数目大于设定值的连续图像帧进行匹配，当前后帧匹配点对大于设定值时，进行视觉初始化；如果匹配点对小于设定值时，重新开始选择图像帧；如果可以进行视觉初始化，运用RANSAC 8点法同时计算单应矩阵和基础矩阵，择优选择计算得到相机的位姿，之后用视觉里程计持续计算相机位姿，同时对相机位姿计算得到视觉位姿；

3.2)输出视觉初始化得到的视觉位姿；

3.3)IMU初始化：配置相机与IMU外参，此时IMU外参为标定后的初始值，表示相机与IMU之间的平移，表示相机与IMU之间的旋转四元数；配置初始重力向量g，计算IMU位姿；

3.4)输出IMU初始化得到的IMU位姿以及重力向量g；

3.5)配置步骤3.3)中初始重力向量：通过视觉初始化得到某一静止图像帧的相机位姿，将与此静止图像帧对应的IMU帧的加速度计读数作为重力向量g；

3.6)确定视觉坐标系与IMU的世界坐标系之间转换关系：静止图像帧的位置即为系统的视觉坐标系的原点，将该静止图像帧的位置与姿态作为视觉坐标系，该静止图像帧也是此系统的视觉第一帧，用vicon表示视觉坐标系，将与此静止图像帧对应的IMU帧的位置与姿态作为IMU的世界坐标系，同时该IMU帧也是此系统的IMU第一帧，用world表示IMU的世界坐标系，此时，相机与IMU外参为视觉坐标系与IMU的世界坐标系的转换关系，用表示视觉坐标系与IMU的世界坐标系之间的平移，用表示视觉坐标系与IMU的世界坐标系之间的旋转四元数，其中转换关系中涉及到的尺度λ为初始配置值；

3.7)非线性优化确定尺度初始值：根据相同时间内相机的运动与IMU的运动实际是一致的，利用视觉坐标系与IMU的世界坐标系之间转换关系、IMU运动模型以及已知的重力向量，得到IMU位姿和视觉位姿之间的转换关系，从而通过非线性优化求解最小二乘解，可得到尺度的初始估计；

3.8)用λI-EKF对尺度进行求精估计：通过λI-EKF(Lamdba Iterated Extended KalmanFilter)算法进一步精确尺度λ的值，并更新状态向量X,其中b_w、b_a分别为IMU中陀螺仪的零漂、IMU中加速度计的零漂。

4.如权利要求3所述的方法，其特征在于，所述步骤3.1)中计算视觉位姿方法具体包括：按照步骤3.6)确定的视觉坐标系，将相机位姿转换为此视觉坐标系下的视觉位姿，用表示在视觉坐标系下的相机平移，用表示在视觉坐标系下的相机旋转四元数。