视觉/惯性里程计组合导航方法、电子设备及存储介质技术

针对现有的视觉/惯性里程计组合导航滤波技术中存在的方差估计不一致问题，本发明专利技术提供了一种视觉/惯性里程计组合导航方法、电子设备及存储介质。IMU接收载体的运动信息，相机同步采集图像信息，根据IMU接收到的运动信息进行捷联惯性导航解算，得到载体的姿态、速度和位置信息；对相机同步采集到的图像信息提取特征点并对提取的特征点进行跟踪；通过速度误差的严格状态变换，构建视觉/惯性里程计组合导航滤波的系统模型和观测模型，完成卡尔曼滤波，输出滤波后的组合导航系统的姿态、速度、位置信息。本发明专利技术克服了视觉/惯性里程计组合导航滤波的方差估计不一致问题，提高组合导航的精度。

【技术实现步骤摘要】
视觉/惯性里程计组合导航方法、电子设备及存储介质
本专利技术涉及视觉/惯性里程计组合导航
，特别是涉及一种具有滤波一致性的基于状态变换的视觉/惯性里程计组合导航方法。
技术介绍
视觉/惯性里程计组合导航问题一直是状态估计研究领域中的一个热点问题，特别是对于无人载体如无人车、无人机在卫星拒止的环境中的应用来说意义重大。在所有的视觉/惯性里程计组合导航方案中，使用一个惯性测量单元(IMU)和单目摄像机进行组合的方案因体积小、成本低而备受关注。但是，单目摄像机存在度量尺度不确定的问题。然而，一个惯性测量单元(IMU)可以为视觉里程计提供绝对尺度，也可以用来辅助视觉特征的提取和匹配。在组合导航系统失去视觉信息后，惯性导航系统仍能在短时间内高精度地工作。对于视觉/惯性里程计组合导航方法，根据融合的方式，可以分为松组合和紧组合。松组合方式将惯导信息和视觉信息分别处理，惯性测量单元(IMU)的测量信息用于状态传递，视觉信息计算的位姿用于更新，这种方法没有利用惯性测量单元(IMU)对视觉的辅助作用，无法纠正视觉位姿引入的漂移。紧组合方式利用惯性和视觉的原始信息，将IMU和相机的测量值全部优化，然后基于滤波或者非线性优化实现。由于IMU与相机的测量值会相互影响，因此紧组合比松组合具有更好的鲁棒性和精度。一般认为基于非线性优化的紧组合方法比基于滤波的紧组合方法精度更高，因为在每个迭代周期中使用了更多的测量信息。但是，非线性方程的迭代求解需要大量的计算资源，因此很难在资源受限的平台上实现实时计算。与此相反，基于扩展卡尔曼滤波的方法具有更好的计算效率，并且易于在小型移动设备上实现。然而，早期基于扩展卡尔曼滤波的视觉/惯性里程计紧组合导航方法存在方差估计不一致性的问题。例如在多状态约束卡尔曼滤波（MSCKF）的惯性/视觉紧组合导航系统中，偏航角是不可观测的，但是滤波过程中却误认为它是可观测的，进而方差减小。这里产生方差不一致性的根本原因源于组合导航滤波的雅可比矩阵的计算方法。为了解决方差不一致性的问题，目前比较流行的两种方法是基于可观测性约束的扩展卡尔曼滤波方法(OCEKF)和不变扩展卡尔曼滤波方法(IEKF)。使用基于可观测性约束的扩展卡尔曼滤波方法(OCEKF)的困难在于，首先需要对系统在不同场景下的不可观测子空间进行深入研究，然后基于观测性约束对雅克比矩阵进行重新计算。然而，在实际应用中，很难从静态和动态两个方面全面分析系统的可观测性。有时，基于静态基座的可观测性分析在动态基座上是不适用的。不变扩展卡尔曼滤波方法(IEKF)的缺点是滤波推导和实现过程非常复杂。因此，亟需设计一种能够比基于可观测性约束的扩展卡尔曼滤波方法(OCEKF)和不变扩展卡尔曼滤波方法(IEKF)更简单、更容易实现的可解决方差不一致问题的视觉/惯性里程计紧组合导航技术方案。
技术实现思路
针对现有的视觉/惯性里程计组合导航滤波技术中存在的方差估计不一致问题，本专利技术提供了一种视觉/惯性里程计组合导航方法、电子设备及存储介质。本专利技术旨在通过速度误差的严格状态变换，推导视觉/惯性里程计组合导航滤波的系统模型和观测模型，进而克服视觉/惯性里程计组合导航滤波的方差估计不一致问题，提高组合导航的精度。为实现上述技术目的，本专利技术采用的具体技术方案如下：视觉/惯性里程计组合导航方法，包括：在载体上安装视觉/惯性里程计组合导航系统，视觉/惯性里程计组合导航系统包括IMU以及相机，IMU接收载体的运动信息，相机同步采集图像信息，根据IMU接收到的运动信息进行捷联惯性导航解算，得到载体的姿态、速度和位置信息；对相机同步采集到的图像信息提取特征点并对提取的特征点进行跟踪；构建视觉/惯性里程计组合导航系统的系统模型和观测模型，根据视觉/惯性里程计组合导航系统的系统模型和观测模型完成卡尔曼滤波，输出滤波后的组合导航系统的姿态、速度、位置信息。其中，本专利技术中IMU接收载体的运动信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力积分增量信息。单目摄像机采集的图像信息为与IMU同步采集的二维图像。本专利技术中，构建视觉/惯性里程计组合导航系统的系统模型和观测模型，包括：（1）确定视觉/惯性里程计组合导航模型的状态向量。t时刻视觉/惯性里程计组合导航模型的状态向量x由与惯导相关的状态向量和与相机相关的状态向量组成。其中，xST-IMU是t时刻与惯导相关的状态向量，xST-C是t时刻与相机相关的状态向量。（2）构建基于状态变换的与惯导误差状态相关的系统模型：其中FST-IMU表示系统矩阵，δxST-IMU表示与xST-IMU相对应的误差状态向量，GST-IMU为噪声转移矩阵，为系统噪声。基于状态变换的与惯导误差状态相关的系统模型中的各参数定义如下：假设t时刻与惯导相关的状态向量xST-IMU为：是Hamilton单位四元数，表示从IMU坐标系到世界参考坐标系的旋转，是IMU相对于世界参考坐标系的速度在世界参考坐标系中的投影，是IMU在世界参考坐标系中的位置坐标，bg和ba分别是三轴陀螺仪零偏和三轴加速度计零偏，表示从相机坐标系到IMU坐标系的旋转，表示IMU坐标系的原点到相机坐标系的原点的矢量在IMU坐标系下的投影。则与xST-IMU相对应的误差状态向量δxST-IMU表示为：其中分别代表t时刻的惯导姿态角误差、基于状态变换后的速度误差、位置误差、陀螺零偏误差、三轴加速度计零偏误差、相机和IMU之间的安装误差角、相机和IMU之间的杆臂误差。定义基于状态变换后的新的速度误差为：代表IMU坐标系到世界参考坐标系的旋转矩阵，代表世界参考坐标系到IMU坐标系的旋转矩阵估计值，为惯导的姿态角误差。基于状态变换后的系统矩阵为：其中为地球自转角速度在世界参考坐标系中的投影，为重力加速度，I3为三维单位矩阵，为速度向量对应的斜对称矩阵。基于状态变换后的噪声转移矩阵GST-IMU为：定义系统噪声wST-IMU为：其中wg和wa分别为三轴陀螺仪和三轴加速度计的测量白噪声，wwg和wwa是驱动随机游走误差的白噪声，和wp分别为惯导和相机之间的相对姿态角噪声和相对位移噪声。（3）构建基于状态变换的与相机位姿误差状态相关的状态和协方差传递模型：状态向量x除了由与惯导相关的状态向量xST-IMU有关之外，还与相机相关的状态向量xST-C组成。xST-C包含t时刻之前N帧的相机位姿，N取决于当前跟踪的特征点已经被跟踪的长度，同时还取决于设置的窗口最大宽度。xST-C表示为：其中四元数表示相机坐标系到世界参考坐标系的旋转，表示相机在世界参考坐标系的位置坐标，下角标1至N表示特征点被跟踪的帧数。与xST-C对应的与相机相关的误差状态向量δxST-C表示为：其中代表相机在世界参考坐标系的姿态角误差，本文档来自技高网...

【技术保护点】
1.视觉/惯性里程计组合导航方法，其特征在于，包括：/n在载体上安装视觉/惯性里程计组合导航系统，视觉/惯性里程计组合导航系统包括IMU以及相机，IMU接收载体的运动信息，相机同步采集图像信息，根据IMU接收到的运动信息进行捷联惯性导航解算，得到载体的姿态、速度和位置信息；对相机同步采集到的图像信息提取特征点并对提取的特征点进行跟踪；/n构建视觉/惯性里程计组合导航系统的系统模型和观测模型，根据视觉/惯性里程计组合导航系统的系统模型和观测模型完成卡尔曼滤波，输出滤波后的组合导航系统的姿态、速度、位置信息。/n

【技术特征摘要】
1.视觉/惯性里程计组合导航方法，其特征在于，包括：
在载体上安装视觉/惯性里程计组合导航系统，视觉/惯性里程计组合导航系统包括IMU以及相机，IMU接收载体的运动信息，相机同步采集图像信息，根据IMU接收到的运动信息进行捷联惯性导航解算，得到载体的姿态、速度和位置信息；对相机同步采集到的图像信息提取特征点并对提取的特征点进行跟踪；
构建视觉/惯性里程计组合导航系统的系统模型和观测模型，根据视觉/惯性里程计组合导航系统的系统模型和观测模型完成卡尔曼滤波，输出滤波后的组合导航系统的姿态、速度、位置信息。


2.根据权利要求1所述的视觉/惯性里程计组合导航方法，其特征在于：IMU接收的运动信息包括三轴陀螺仪的角增量或角速度信息以及三轴加速度计的比力或比力积分增量信息。


3.根据权利要求1所述的视觉/惯性里程计组合导航方法，其特征在于：对相机同步采集到的图像信息提取FAST角点作为特征点，采用KLT光流跟踪法对提取的特征点进行跟踪。


4.根据权利要求1所述的视觉/惯性里程计组合导航方法，其特征在于：构建视觉/惯性里程计组合导航系统的系统模型和观测模型，包括：
（1）确定视觉/惯性里程计组合导航模型的状态向量；

t时刻视觉/惯性里程计组合导航模型的状态向量x由与惯导相关的状态向量和与相机相关的状态向量组成：



其中，xST-IMU是t时刻与惯导相关的状态向量，xST-C是t时刻与相机相关的状态向量；
（2）构建基于状态变换的与惯导误差状态相关的系统模型：



其中FST-IMU表示系统矩阵，δxST-IMU表示与xST-IMU相对应的误差状态向量，GST-IMU为噪声转移矩阵，为系统噪声；
基于状态变换的与惯导误差状态相关的系统模型中的各参数定义如下：
设t时刻与惯导相关的状态向量xST-IMU为：




是Hamilton单位四元数，表示从IMU坐标系到世界参考坐标系的旋转，是IMU相对于世界参考坐标系的速度在世界参考坐标系中的投影，是IMU在世界参考坐标系中的位置坐标，bg和ba分别是三轴陀螺仪零偏和三轴加速度计零偏，表示从相机坐标系到IMU坐标系的旋转，表示IMU坐标系的原点到相机坐标系的原点的矢量在IMU坐标系下的投影；
则与xST-IMU相对应的误差状态向量δxST-IMU表示为：



其中分别代表t时刻的惯导姿态角误差、基于状态变换后的速度误差、位置误差、陀螺零偏误差、三轴加速度计零偏误差、相机和IMU之间的安装误差角、相机和IMU之间的杆臂误差；
定义基于状态变换后的新的速度误差为：




代表IMU坐标系到世界参考坐标系的旋转矩阵，代表世界参考坐标系到IMU坐标系的旋转矩阵估计值，为惯导的姿态角误差；



其中为地球自转角速度在世界参考坐标系中的投影，为重力加速度，I3为三维单位矩阵，为速度向量对应的斜对称矩阵；
基于状态变换后的噪声转移矩阵GST-IMU为：



定义系统噪声wST-IMU为：



其中wg和wa分别为三轴陀螺仪和三轴加速度计的测量白噪声，wwg和wwa是驱动随机游走误差的白噪声，和wp分别为惯导和相机之间的相对姿态角噪声和相对位移噪声；
（3）构建基于状态变换的与相机位姿误差状态相关的状态和协方差传递模型：
状态向量x除了由与惯导相关的状态向量xST-IMU有关之外，还与相机相关的状态向量xST-C组成；xST-C包含t时刻之前N帧的...

【专利技术属性】
技术研发人员：王茂松，赵启超，吴文启，
申请(专利权)人：中国人民解放军国防科技大学，
类型：发明
国别省市：湖南;43