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

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

Info

Publication number
CN111811506B
CN111811506B CN202010963803.2A CN202010963803A CN111811506B CN 111811506 B CN111811506 B CN 111811506B CN 202010963803 A CN202010963803 A CN 202010963803A CN 111811506 B CN111811506 B CN 111811506B
Authority
CN
China
Prior art keywords
camera
error
coordinate system
imu
inertial
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010963803.2A
Other languages
English (en)
Other versions
CN111811506A (zh
Inventor
王茂松
赵启超
吴文启
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202010963803.2A priority Critical patent/CN111811506B/zh
Publication of CN111811506A publication Critical patent/CN111811506A/zh
Application granted granted Critical
Publication of CN111811506B publication Critical patent/CN111811506B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

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

Description

视觉/惯性里程计组合导航方法、电子设备及存储介质
技术领域
本发明涉及视觉/惯性里程计组合导航技术领域，特别是涉及一种具有滤波一致性的基于状态变换的视觉/惯性里程计组合导航方法。
背景技术
视觉/惯性里程计组合导航问题一直是状态估计研究领域中的一个热点问题，特别是对于无人载体如无人车、无人机在卫星拒止的环境中的应用来说意义重大。在所有的视觉/惯性里程计组合导航方案中，使用一个惯性测量单元(IMU)和单目摄像机进行组合的方案因体积小、成本低而备受关注。但是，单目摄像机存在度量尺度不确定的问题。然而，一个惯性测量单元(IMU)可以为视觉里程计提供绝对尺度，也可以用来辅助视觉特征的提取和匹配。在组合导航系统失去视觉信息后，惯性导航系统仍能在短时间内高精度地工作。
对于视觉/惯性里程计组合导航方法，根据融合的方式，可以分为松组合和紧组合。松组合方式将惯导信息和视觉信息分别处理，惯性测量单元(IMU)的测量信息用于状态传递，视觉信息计算的位姿用于更新，这种方法没有利用惯性测量单元(IMU)对视觉的辅助作用，无法纠正视觉位姿引入的漂移。紧组合方式利用惯性和视觉的原始信息，将IMU和相机的测量值全部优化，然后基于滤波或者非线性优化实现。由于IMU与相机的测量值会相互影响，因此紧组合比松组合具有更好的鲁棒性和精度。一般认为基于非线性优化的紧组合方法比基于滤波的紧组合方法精度更高，因为在每个迭代周期中使用了更多的测量信息。但是，非线性方程的迭代求解需要大量的计算资源，因此很难在资源受限的平台上实现实时计算。与此相反，基于扩展卡尔曼滤波的方法具有更好的计算效率，并且易于在小型移动设备上实现。
然而，早期基于扩展卡尔曼滤波的视觉/惯性里程计紧组合导航方法存在方差估计不一致性的问题。例如在多状态约束卡尔曼滤波（MSCKF）的惯性/视觉紧组合导航系统中，偏航角是不可观测的，但是滤波过程中却误认为它是可观测的，进而方差减小。这里产生方差不一致性的根本原因源于组合导航滤波的雅可比矩阵的计算方法。为了解决方差不一致性的问题，目前比较流行的两种方法是基于可观测性约束的扩展卡尔曼滤波方法(OCEKF)和不变扩展卡尔曼滤波方法(IEKF)。使用基于可观测性约束的扩展卡尔曼滤波方法(OCEKF)的困难在于，首先需要对系统在不同场景下的不可观测子空间进行深入研究，然后基于观测性约束对雅克比矩阵进行重新计算。然而，在实际应用中，很难从静态和动态两个方面全面分析系统的可观测性。有时，基于静态基座的可观测性分析在动态基座上是不适用的。不变扩展卡尔曼滤波方法(IEKF)的缺点是滤波推导和实现过程非常复杂。
因此，亟需设计一种能够比基于可观测性约束的扩展卡尔曼滤波方法(OCEKF)和不变扩展卡尔曼滤波方法(IEKF)更简单、更容易实现的可解决方差不一致问题的视觉/惯性里程计紧组合导航技术方案。
发明内容
针对现有的视觉/惯性里程计组合导航滤波技术中存在的方差估计不一致问题，本发明提供了一种视觉/惯性里程计组合导航方法、电子设备及存储介质。本发明旨在通过速度误差的严格状态变换，推导视觉/惯性里程计组合导航滤波的系统模型和观测模型，进而克服视觉/惯性里程计组合导航滤波的方差估计不一致问题，提高组合导航的精度。
为实现上述技术目的，本发明采用的具体技术方案如下：
视觉/惯性里程计组合导航方法，包括：
在载体上安装视觉/惯性里程计组合导航系统，视觉/惯性里程计组合导航系统包括IMU以及相机，IMU接收载体的运动信息，相机同步采集图像信息，根据IMU接收到的运动信息进行捷联惯性导航解算，得到载体的姿态、速度和位置信息；对相机同步采集到的图像信息提取特征点并对提取的特征点进行跟踪；
构建视觉/惯性里程计组合导航系统的系统模型和观测模型，根据视觉/惯性里程计组合导航系统的系统模型和观测模型完成卡尔曼滤波，输出滤波后的组合导航系统的姿态、速度、位置信息。
其中，本发明中IMU接收载体的运动信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力积分增量信息。单目摄像机采集的图像信息为与IMU同步采集的二维图像。
本发明中，构建视觉/惯性里程计组合导航系统的系统模型和观测模型，包括：
（1）确定视觉/惯性里程计组合导航模型的状态向量。
t时刻视觉/惯性里程计组合导航模型的状态向量x由与惯导相关的状态向量和与相机相关的状态向量组成。
Figure 842007DEST_PATH_IMAGE001
其中，
Figure 227989DEST_PATH_IMAGE002
是t时刻与惯导相关的状态向量，
Figure 476568DEST_PATH_IMAGE003
是t时刻与相机相关的状态向量。
（2）构建基于状态变换的与惯导误差状态相关的系统模型：
Figure 340618DEST_PATH_IMAGE004
其中F ST-IMU 表示系统矩阵，
Figure 623832DEST_PATH_IMAGE005
表示与
Figure 915136DEST_PATH_IMAGE006
相对应的误差状态向量，G ST-IMU 为噪声转移矩阵，
Figure 651011DEST_PATH_IMAGE007
为系统噪声。
基于状态变换的与惯导误差状态相关的系统模型中的各参数定义如下：
假设t时刻与惯导相关的状态向量
Figure 318753DEST_PATH_IMAGE008
为：
Figure 456473DEST_PATH_IMAGE009
Figure 918678DEST_PATH_IMAGE010
是Hamilton单位四元数，表示从IMU坐标系到世界参考坐标系的旋转，
Figure 141849DEST_PATH_IMAGE011
是IMU相对于世界参考坐标系的速度在世界参考坐标系中的投影，
Figure 82124DEST_PATH_IMAGE012
是IMU在世界参考坐标系中的位置坐标，
Figure 74350DEST_PATH_IMAGE013
和
Figure 973036DEST_PATH_IMAGE014
分别是三轴陀螺仪零偏和三轴加速度计零偏，
Figure 680574DEST_PATH_IMAGE015
表示从相机坐标系到IMU坐标系的旋转，
Figure 424539DEST_PATH_IMAGE016
表示IMU坐标系的原点到相机坐标系的原点的矢量在IMU坐标系下的投影。
则与
Figure 536851DEST_PATH_IMAGE017
相对应的误差状态向量
Figure 340859DEST_PATH_IMAGE018
表示为：
Figure 538622DEST_PATH_IMAGE019
其中
Figure 86278DEST_PATH_IMAGE020
分别代表t时刻的惯导姿态角误差、基于状态变换后的速度误差、位置误差、陀螺零偏误差、三轴加速度计零偏误差、相机和IMU之间的安装误差角、相机和IMU之间的杆臂误差。
定义基于状态变换后的新的速度误差
Figure 53097DEST_PATH_IMAGE021
为：
Figure 559165DEST_PATH_IMAGE022
Figure 978645DEST_PATH_IMAGE023
代表IMU坐标系到世界参考坐标系的旋转矩阵，
Figure 329992DEST_PATH_IMAGE024
代表世界参考坐标系到IMU坐标系的旋转矩阵估计值，
Figure 885738DEST_PATH_IMAGE025
为惯导的姿态角误差。
基于状态变换后的系统矩阵
Figure 297128DEST_PATH_IMAGE026
为：
Figure 203904DEST_PATH_IMAGE027
其中
Figure 93363DEST_PATH_IMAGE028
为地球自转角速度在世界参考坐标系中的投影，
Figure 34774DEST_PATH_IMAGE029
为重力加速度，I 3为三维单位矩阵，
Figure 617065DEST_PATH_IMAGE030
为速度向量对应的斜对称矩阵。
基于状态变换后的噪声转移矩阵G ST-IMU 为：
Figure 748488DEST_PATH_IMAGE031
定义系统噪声
Figure 707216DEST_PATH_IMAGE032
为：
Figure 971976DEST_PATH_IMAGE033
其中
Figure 990747DEST_PATH_IMAGE034
和
Figure 872115DEST_PATH_IMAGE035
分别为三轴陀螺仪和三轴加速度计的测量白噪声，
Figure 634535DEST_PATH_IMAGE036
和
Figure 284959DEST_PATH_IMAGE037
是驱动随机游走误差的白噪声，
Figure 943474DEST_PATH_IMAGE038
和
Figure 312138DEST_PATH_IMAGE039
分别为惯导和相机之间的相对姿态角噪声和相对位移噪声。
（3）构建基于状态变换的与相机位姿误差状态相关的状态和协方差传递模型：
状态向量x除了由与惯导相关的状态向量
Figure 347090DEST_PATH_IMAGE040
有关之外，还与相机相关的状态向量
Figure 383179DEST_PATH_IMAGE041
组成。
Figure 212595DEST_PATH_IMAGE042
包含t时刻之前N帧的相机位姿，N取决于当前跟踪的特征点已经被跟踪的长度，同时还取决于设置的窗口最大宽度。
Figure 68556DEST_PATH_IMAGE043
表示为：
Figure 907199DEST_PATH_IMAGE044
其中四元数
Figure 266636DEST_PATH_IMAGE045
表示相机坐标系到世界参考坐标系的旋转，
Figure 798111DEST_PATH_IMAGE046
表示相机在世界参考坐标系的位置坐标，下角标1至N表示特征点被跟踪的帧数。
与x ST-C 对应的与相机相关的误差状态向量
Figure 875789DEST_PATH_IMAGE047
表示为：
Figure 515193DEST_PATH_IMAGE048
其中
Figure 994716DEST_PATH_IMAGE049
代表相机在世界参考坐标系的姿态角误差，
Figure 431513DEST_PATH_IMAGE050
代表相机在世界参考坐标系的位置坐标误差。
每次获得相机的关键帧即具有平移运动或旋转和平移运动均足够大的帧后，需要增加关键帧对应的相机位姿状态到与相机相关的状态向量中，并且扩增状态协方差矩阵；
Figure 730908DEST_PATH_IMAGE051
表示增加了新的关键帧的相机位姿状态后的与相机相关的状态向量，
Figure 176933DEST_PATH_IMAGE052
对应的与相机相关的误差状态向量
Figure 510962DEST_PATH_IMAGE053
表示为：
Figure 118661DEST_PATH_IMAGE054
其中
Figure 170930DEST_PATH_IMAGE055
为新增加的关键帧对应的相机姿态角误差向量，
Figure 889488DEST_PATH_IMAGE056
为新增加的关键帧对应的相机位置误差在世界参考坐标系的向量。
同时，
Figure 343603DEST_PATH_IMAGE057
，
Figure 387782DEST_PATH_IMAGE058
扩增后的协方差矩阵表示为：
Figure 927348DEST_PATH_IMAGE059
其中
Figure 715175DEST_PATH_IMAGE060
为扩增前的协方差矩阵，
Figure 23797DEST_PATH_IMAGE061
为6N加15维的单位矩阵；
Figure 973298DEST_PATH_IMAGE062
为扩增后的协方差矩阵，表示为：
Figure 734581DEST_PATH_IMAGE063
其中
Figure 60520DEST_PATH_IMAGE064
为新增加的与相机相关的状态误差与未增加之前的所有状态误差
Figure 489227DEST_PATH_IMAGE065
的协方差，
Figure 335262DEST_PATH_IMAGE066
为新增加的与相机相关的状态误差的方差，表示为：
Figure 114999DEST_PATH_IMAGE067
Figure 979050DEST_PATH_IMAGE068
（4）根据特征点的重投影误差构建视觉/惯性里程计组合导航系统的观测模型。
设提取的一系列特征点中，第j个特征点
Figure 996684DEST_PATH_IMAGE069
包含在第
Figure 553568DEST_PATH_IMAGE070
帧图像中，即在第
Figure 289443DEST_PATH_IMAGE070
帧图像对应的相机坐标系中的三维坐标为
Figure 957184DEST_PATH_IMAGE071
，在第i帧图像中的二维坐标为
Figure 94905DEST_PATH_IMAGE072
，有如下关系：
Figure 822689DEST_PATH_IMAGE073
Figure 45860DEST_PATH_IMAGE074
分别为第
Figure 251714DEST_PATH_IMAGE075
帧图像的第j个特征点在第
Figure 243940DEST_PATH_IMAGE075
帧图像对应的相机坐标系中的X，Y，Z坐标；
Figure 142626DEST_PATH_IMAGE076
表示第j个特征点在世界参考坐标系中的坐标；
Figure 587514DEST_PATH_IMAGE077
表示第
Figure 862638DEST_PATH_IMAGE075
个相机位置在世界参考坐标系中的坐标，其中第
Figure 974950DEST_PATH_IMAGE075
个相机位置与第i帧图像对应；
Figure 778958DEST_PATH_IMAGE078
表示世界参考坐标系到第
Figure 973791DEST_PATH_IMAGE075
个相机坐标系的旋转矩阵；
Figure 787027DEST_PATH_IMAGE079
其中
Figure 753846DEST_PATH_IMAGE080
表示第
Figure 728755DEST_PATH_IMAGE075
帧图像中的第j个特征点投影到相机平面的观测噪声向量，建模为高斯白噪声；
第
Figure 413814DEST_PATH_IMAGE075
帧图像中的第j个特征点的重投影误差为：
Figure 30740DEST_PATH_IMAGE081
其中
Figure 586487DEST_PATH_IMAGE082
为第
Figure 997876DEST_PATH_IMAGE075
帧图像中的第j个特征点二维坐标的估计值，
Figure 904652DEST_PATH_IMAGE083
是直接由特征提取获得的第i帧图像中的第j个特征点二维坐标：
Figure 59690DEST_PATH_IMAGE084
Figure 1101DEST_PATH_IMAGE086
将重投影误差
Figure 317813DEST_PATH_IMAGE087
用系统误差状态
Figure 711886DEST_PATH_IMAGE088
和特征点位置误差
Figure 405035DEST_PATH_IMAGE089
线性表示：
Figure 200953DEST_PATH_IMAGE090
其中
Figure 219724DEST_PATH_IMAGE091
表示第j个特征点在世界参考坐标系中的坐标，
Figure 101093DEST_PATH_IMAGE092
和
Figure 597933DEST_PATH_IMAGE093
是
Figure 251287DEST_PATH_IMAGE094
分别相对于误差状态向量和特征点位置误差的雅克比矩阵：
Figure 175381DEST_PATH_IMAGE096
Figure 544045DEST_PATH_IMAGE097
Figure 578997DEST_PATH_IMAGE098
分别为第
Figure 349507DEST_PATH_IMAGE099
帧图像的第j个特征点在第
Figure 444502DEST_PATH_IMAGE075
帧图像对应的相机坐标系中的X，Y，Z坐标的估计值，
Figure 300463DEST_PATH_IMAGE100
代表
Figure 873526DEST_PATH_IMAGE101
的估计值。
假设特征点
Figure 764122DEST_PATH_IMAGE102
共被所采集的所有图像中的
Figure 30018DEST_PATH_IMAGE103
帧图像所包含，将
Figure 107696DEST_PATH_IMAGE102
在所有这些图像中的重投影误差堆叠为一列，得到：
Figure 750030DEST_PATH_IMAGE104
其中
Figure 229552DEST_PATH_IMAGE105
；
最后，假设A是一个酉矩阵，A的列构成
Figure 931929DEST_PATH_IMAGE106
左零空间的一组基，将上式左乘
Figure 496903DEST_PATH_IMAGE107
得：
Figure 411769DEST_PATH_IMAGE108
其中
Figure 11378DEST_PATH_IMAGE109
。
堆叠而成的
Figure 616147DEST_PATH_IMAGE110
是列满秩的，因此酉矩阵A的维度为
Figure 668417DEST_PATH_IMAGE111
，因而
Figure 386974DEST_PATH_IMAGE112
的维度为
Figure 106668DEST_PATH_IMAGE113
，使用变换后的重投影误差
Figure 885268DEST_PATH_IMAGE112
进行卡尔曼滤波观测与更新。
本发明提供一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述视觉/惯性里程计组合导航方法的步骤。
本发明提供一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述视觉/惯性里程计组合导航方法的步骤。
本发明所涉及的滤波方法并不局限于单目视觉/惯性里程计的组合导航系统，亦可以扩展应用于双目视觉/惯性里程计的组合导航系统。
与现有技术相比，本发明具有以下优点：
1）本发明中的视觉/惯性组合导航方法的速度误差定义更为严格，物理意义更为明确。
2）本发明中的视觉/惯性组合导航系统模型中不再有比力相关项，而是被重力相关项替代，使得组合导航系统可以更好的适用于复杂的动态环境，滤波具有更好的一致性。
3）本发明方法实现容易，并且计算量小。与基于可观测性约束的扩展卡尔曼滤波方法(OCEKF)和不变扩展卡尔曼滤波方法(IEKF)相比，本发明的滤波过程与标准的扩展卡尔曼滤波过程完全一致，实现起来简单，更容易理解。
本发明所提出的方法具有很强的工程应用灵活性和适用性，由于计算量相对较小，因此可以为组合导航系统增加额外的功能留有足够的空间。
附图说明
图1 为本发明的流程图。
图2 为开源数据集某次实验的位置轨迹。
图3 为开源数据集某次实验的水平位置误差结果。
图4 为开源数据集某次实验的姿态误差结果。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚明白，下面将以附图及详细叙述清楚说明本发明所揭示内容的精神，任何所属技术领域技术人员在了解本发明内容的实施例后，当可由本发明内容所教示的技术，加以改变及修饰，其并不脱离本发明内容的精神与范围。本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。
参照图1是本实施例的流程图，一种视觉/惯性里程计组合导航方法，具体包括如下步骤：
步骤一，初始化数据。
视觉/惯性里程计组合导航系统中的IMU接收载体的运动信息，相机同步采集图像信息。
根据IMU接收到的运动信息进行惯性导航解算，得到IMU的姿态、速度和位置信息；根据相机同步采集到的图像信息提取FAST（Features from Accelerated Segment Test）角点作为特征点，将特征点的二维像素坐标信息进行存储；采用KLT（Kanade LucasTomasi）光流跟踪法对提取的特征点进行跟踪。
IMU接收的数据信息包括：三轴陀螺仪的角增量或角速度信息和三轴加速度计的比力或比力积分增量信息，利用这些数据完成惯性导航的推算。单目相机接收到的数据信息为与惯性测量单元数据时间同步的二维图像，根据二维图像完成FAST角点的提取和角点像素坐标的存储，对提取的角点利用KLT光流跟踪算法进行跟踪。
利用第一帧图像进行图像关键特征点的提取，作为跟踪的初始特征点。FAST（Features from Accelerated Segment Test）角点由于其计算量小、实时性强等诸多优点，在视觉导航领域中得到了广泛应用，因此本发明选择FAST角点作为初始化的特征点。
在正式进行组合视觉/惯性的组合导航滤波之前，视觉图像需要完成特征点的跟踪与匹配、剔除异常特征点以及提取新的特征点的任务。选择使用KLT（KanadeLucasTomasi）跟踪法，该算法以光流跟踪为基础，对于特征的形变有较好的适应性。另外在跟踪到特征点后，在跟踪点之外的图像区域中检测新的特征点，以补足跟踪过程中特征点的数量。
步骤二，构建视觉/惯性里程计组合导航系统的系统模型和观测模型。
首先明确本发明中涉及到的坐标系：当地地理坐标系N、世界参考坐标系W、车体坐标系M、IMU坐标系I、相机坐标系C、地固地心坐标系E，定义如下：
当地地理坐标系N：本发明取为北东地（NED）坐标系作为当地地理坐标系，视觉/惯性里程计组合导航系统最终输出的导航状态，如速度、姿态角在当地地理坐标系中表示。
世界参考坐标系W：该世界参考坐标系即当地切平面坐标系，与地球固联，与所在点的当地地理坐标系重合。为了满足算法的无缝运行，IMU和相机的位置、姿态以及场景中的特征点都在世界参考坐标系中表示。
车体坐标系M：该坐标系与车体固联，原点位于车辆质心，坐标轴方向为前右下。
IMU坐标系I：该坐标系即导航解算载体系，以三个加速度计的中心为原点，以三个敏感轴的敏感方向为X，Y，Z轴，三个方向满足右手法则，分别指向前右下，安装时应使其坐标轴方向尽量与车体坐标系一致，以使安装角较小。
相机坐标系C：该坐标系与相机固联，坐标系原点在相机的光学中心，X轴和Y轴平行于成像平面，其中X轴为横向，Y轴为纵向，Z轴平行于透镜的轴线，方向根据右手法则由X轴和Y轴确定。
地心地固坐标系E：该坐标系与地球固联，坐标原点位于地球球心，X轴指向赤道与参考子午线的交点，Z轴沿地球自转轴指向北极点，Y轴由右手法则确定，此坐标系用于世界参考坐标系与当地地理坐标系转换的中间坐标系。
在传统的视觉/惯性里程计中，通常把特征点位置加入状态向量中，通过新的观测不断地去优化特征点的位置和载体位姿，直至特征点消失。但是与同时定位与建图（SLAM）系统不同，惯性/视觉里程计并不需要得到环境地图，因此将特征点加入到状态向量中进行优化，将使计算过程变得复杂。多状态约束卡尔曼滤波(MSCKF)算法在这一点上做了改进，状态向量中不再包含特征点位置，而是以过去时刻的相机位姿作为代替，状态向量是一个滑动窗口的形式，不断加入新的相机位姿，并不断舍弃旧的相机位姿。
（1）确定视觉/惯性里程计组合导航模型的状态向量；
t时刻组合导航滤波器的状态向量x由与惯导相关的量和与相机相关的量组成：
Figure 424834DEST_PATH_IMAGE114
(1)
其中，
Figure 212662DEST_PATH_IMAGE115
是t时刻与惯导相关的状态向量，
Figure 521283DEST_PATH_IMAGE116
是t时刻与相机相关的状态向量。
（2）构建基于状态变换的与惯导误差状态相关的系统模型：
Figure 470785DEST_PATH_IMAGE117
中包括t时刻惯导的姿态、速度和位置，以及惯导的惯性器件的零偏向量，t时刻与惯导相关的状态向量可表示为：
Figure 232067DEST_PATH_IMAGE118
(2)
式中，
Figure 823586DEST_PATH_IMAGE119
是Hamilton单位四元数，表示从IMU坐标系到世界参考坐标系的旋转，
Figure 986714DEST_PATH_IMAGE120
是IMU相对于世界参考坐标系的速度在世界参考坐标系中的投影，
Figure 372696DEST_PATH_IMAGE121
是IMU在世界参考坐标系中的位置坐标，
Figure 886854DEST_PATH_IMAGE122
和
Figure 750904DEST_PATH_IMAGE123
分别是三轴陀螺仪零偏和三轴加速度计零偏，
Figure 768539DEST_PATH_IMAGE124
表示从相机坐标系到IMU坐标系的旋转，
Figure 325422DEST_PATH_IMAGE125
表示IMU坐标系的原点到相机坐标系的原点的矢量在IMU坐标系下的投影。
与公式(2)中状态向量相对应的误差状态向量为公式(3)所示。
Figure 61297DEST_PATH_IMAGE126
(3)
式(3)中
Figure 729039DEST_PATH_IMAGE127
分别代表t时刻的惯导姿态角误差、基于状态变换后的速度误差、位置误差、陀螺零偏误差、加速度计零偏误差、相机和IMU之间的安装误差角、相机和IMU之间的杆臂误差。
传统的速度误差定义为
Figure 881407DEST_PATH_IMAGE128
，其中
Figure 609192DEST_PATH_IMAGE129
为速度的估计值，
Figure 566784DEST_PATH_IMAGE130
为速度的实际真值。然而这种定义仅考虑了向量之间的大小差异，未考虑到不同坐标系下的向量的方向差异，严格的速度误差应在同一坐标系中定义。
本发明的基于状态变换的速度误差
Figure 303796DEST_PATH_IMAGE131
定义为：
Figure 296022DEST_PATH_IMAGE132
(4)
式中，
Figure 929129DEST_PATH_IMAGE133
代表IMU坐标系到世界参考坐标系的旋转矩阵,
Figure 639596DEST_PATH_IMAGE134
代表世界参考坐标系到IMU坐标系的旋转矩阵估计值，
Figure 649140DEST_PATH_IMAGE135
为惯导的姿态角误差。
根据以上定义，本发明所构建的基于状态变换的与惯导误差状态相关的系统模型为
Figure 761453DEST_PATH_IMAGE136
，其中
Figure 831040DEST_PATH_IMAGE137
(5)
其中
Figure 763224DEST_PATH_IMAGE138
为地球自转角速度在世界参考坐标系中的投影，
Figure 576459DEST_PATH_IMAGE139
为重力加速度，I 3为三维单位矩阵，
Figure 543278DEST_PATH_IMAGE140
为速度向量对应的斜对称矩阵。
基于状态变换后的噪声转移矩阵G ST-IMU 为：
Figure 518187DEST_PATH_IMAGE141
(6)
定义系统噪声
Figure 203247DEST_PATH_IMAGE142
为：
Figure 820173DEST_PATH_IMAGE143
(7)
其中
Figure 641498DEST_PATH_IMAGE144
和
Figure 784379DEST_PATH_IMAGE145
分别为三轴陀螺仪和三轴加速度计的测量白噪声，
Figure 691155DEST_PATH_IMAGE146
和
Figure 111772DEST_PATH_IMAGE147
是驱动随机游走误差的白噪声，
Figure 787604DEST_PATH_IMAGE148
和
Figure 369895DEST_PATH_IMAGE149
分别为惯导和相机之间的相对姿态角噪声和相对位移噪声。
（3）构建基于状态变换的与相机位姿误差状态相关的状态和协方差传递模型：
状态向量x除了由与惯导相关的状态向量
Figure 29547DEST_PATH_IMAGE150
有关之外，还与相机相关的状态向量
Figure 722696DEST_PATH_IMAGE151
组成。
Figure 518614DEST_PATH_IMAGE152
包含t时刻之前N帧的相机位姿，N取决于当前跟踪的特征点已经被跟踪的长度，同时还取决于设置的窗口最大宽度
Figure 271806DEST_PATH_IMAGE153
表示为：
Figure 153175DEST_PATH_IMAGE154
(8)
其中四元数
Figure 650015DEST_PATH_IMAGE155
表示相机坐标系到世界参考坐标系的旋转，
Figure 300439DEST_PATH_IMAGE156
表示相机在世界参考坐标系的位置坐标，下角标1-N表示特征点被跟踪的帧数。
与公式(8)对应的与相机相关的误差状态向量表示为：
Figure 224533DEST_PATH_IMAGE157
(9)
其中
Figure 593197DEST_PATH_IMAGE158
代表相机在世界参考坐标系的姿态角误差，
Figure 893729DEST_PATH_IMAGE159
代表相机在世界参考坐标系的位置坐标误差。
在每次获得IMU测量值后，进行状态和协方差矩阵的传递过程。而在每次获得相机的关键帧即具有平移运动或旋转和平移运动的帧后，需要增加关键帧对应的相机位姿状态到与相机相关的状态向量中，并且扩增状态协方差矩阵；
Figure 398659DEST_PATH_IMAGE160
表示增加了新的关键帧的相机位姿状态后的与相机相关的状态向量，
Figure 493654DEST_PATH_IMAGE161
对应的与相机相关的误差状态向量
Figure 349615DEST_PATH_IMAGE162
表示为：
Figure 188258DEST_PATH_IMAGE163
(10)
其中
Figure 816204DEST_PATH_IMAGE164
为新增加的相机姿态角误差向量，
Figure 82100DEST_PATH_IMAGE165
为新增加的相机位置误差在世界参考坐标系的向量。
Figure 159778DEST_PATH_IMAGE166
。
Figure 536532DEST_PATH_IMAGE167
的具体形式如下：
Figure 281635DEST_PATH_IMAGE168
(11)
扩增后的协方差矩阵表示为：
Figure 718432DEST_PATH_IMAGE169
(12)
其中
Figure 283406DEST_PATH_IMAGE170
为扩增前的协方差矩阵，
Figure 463851DEST_PATH_IMAGE171
为6N加15维的单位矩阵。
Figure 329039DEST_PATH_IMAGE172
为扩增后的协方差矩阵，具体表示为：
Figure 936738DEST_PATH_IMAGE173
(13)
其中
Figure 723428DEST_PATH_IMAGE174
为新增加的与相机相关的状态误差与未增加之前的所有状态误差
Figure 707565DEST_PATH_IMAGE175
的协方差，
Figure 427259DEST_PATH_IMAGE176
为新增加的与相机相关的状态误差的方差。具体可表示为：
Figure 205859DEST_PATH_IMAGE177
(14)
Figure 479846DEST_PATH_IMAGE178
(15)
（4）根据特征点的重投影误差构建视觉/惯性里程计组合导航系统的观测模型；
在视觉/惯性里程计组合导航系统中，使用IMU信息进行状态传递，使用视觉信息进行观测，在多状态约束卡尔曼滤波（MSCKF）方法中使用的是特征点重投影误差，根据特征点重投影误差建立观测模型。
假设在一系列FAST特征点中，第j个特征点
Figure 267673DEST_PATH_IMAGE179
包含在第
Figure 576295DEST_PATH_IMAGE075
帧图像中，即在第
Figure 791376DEST_PATH_IMAGE075
帧图像对应的相机坐标系中的三维坐标为
Figure 815308DEST_PATH_IMAGE180
，在第
Figure 875668DEST_PATH_IMAGE075
帧图像中的二维坐标为
Figure 569954DEST_PATH_IMAGE181
，它们有如下关系：
Figure 690357DEST_PATH_IMAGE182
(16)
Figure 938936DEST_PATH_IMAGE183
分别为第
Figure 68566DEST_PATH_IMAGE075
帧图像的第j个特征点在第
Figure 351779DEST_PATH_IMAGE075
帧图像对应的相机坐标系中的X，Y，Z坐标；
Figure 643083DEST_PATH_IMAGE184
表示第j个特征点在世界参考坐标系中的坐标；
Figure 644538DEST_PATH_IMAGE185
表示第
Figure 312279DEST_PATH_IMAGE075
个相机位置在世界参考坐标系中的坐标，其中第
Figure 450000DEST_PATH_IMAGE075
个相机位置与第
Figure 912205DEST_PATH_IMAGE075
帧图像对应；
Figure 400955DEST_PATH_IMAGE186
表示世界参考坐标系到第
Figure 872388DEST_PATH_IMAGE075
个相机坐标系的旋转矩阵。
Figure 864614DEST_PATH_IMAGE187
(17)
其中
Figure 497721DEST_PATH_IMAGE188
表示第
Figure 473767DEST_PATH_IMAGE075
帧图像中的第j个特征点投影到相机平面的观测噪声向量，建模为高斯白噪声。
第
Figure 483312DEST_PATH_IMAGE075
帧图像中的第j个特征点的重投影误差为：
Figure 330045DEST_PATH_IMAGE189
(18)
其中
Figure 390843DEST_PATH_IMAGE190
为第
Figure 323027DEST_PATH_IMAGE075
帧图像中的第j个特征点二维坐标的估计值，
Figure 401841DEST_PATH_IMAGE191
是直接由特征提取获得的第i帧图像中的第j个特征点二维坐标：
Figure 103081DEST_PATH_IMAGE192
(19)
Figure 343570DEST_PATH_IMAGE194
(20)
其中公式(20)中对应的所有符号含义与公式(16)相同，只不过这里上波浪线代表估计值。第
Figure 763050DEST_PATH_IMAGE075
帧图像中的第j个特征点三维位置
Figure 114397DEST_PATH_IMAGE195
估计值可通过最小化重投影误差的非线性优化过程进行估计，主要使用Bundle Adjustment和逆深度参数化实现。
重投影误差
Figure 201301DEST_PATH_IMAGE196
与相机位姿和特征点的位置有关，由于状态向量中不包含特征点位置，因此无法将
Figure 878270DEST_PATH_IMAGE196
仅用系统误差状态线性表示。
因此，本发明构建视觉/惯性里程计组合导航系统的观测模型，包括以下步骤：
首先，将
Figure 785046DEST_PATH_IMAGE197
用系统误差状态
Figure 940084DEST_PATH_IMAGE198
和特征点位置误差
Figure 881495DEST_PATH_IMAGE199
线性表示：
Figure 463786DEST_PATH_IMAGE200
(21)
其中
Figure 857859DEST_PATH_IMAGE201
和
Figure 816587DEST_PATH_IMAGE202
是
Figure 612505DEST_PATH_IMAGE203
分别相对于误差状态向量和特征点位置误差的雅克比矩阵：
Figure 365698DEST_PATH_IMAGE204
(22)
Figure 247066DEST_PATH_IMAGE205
(23)
然后，假设特征点
Figure 743906DEST_PATH_IMAGE206
共被所采集的所有图像中的
Figure 656980DEST_PATH_IMAGE207
帧图像所包含，将
Figure 581074DEST_PATH_IMAGE208
在所有这些图像中的重投影误差堆叠为一列，得到：
Figure 684159DEST_PATH_IMAGE209
(24)
其中
Figure 984690DEST_PATH_IMAGE210
。
最后，假设A是一个酉矩阵，A的列构成
Figure 755200DEST_PATH_IMAGE211
左零空间的一组基，将上式左乘
Figure 850195DEST_PATH_IMAGE212
得：
Figure 706156DEST_PATH_IMAGE213
(25)
其中
Figure 544799DEST_PATH_IMAGE214
。堆叠而成的
Figure 435394DEST_PATH_IMAGE215
明显是列满秩的，因此酉矩阵A的维度为
Figure 701290DEST_PATH_IMAGE216
，因而
Figure 44547DEST_PATH_IMAGE217
的维度为
Figure 421302DEST_PATH_IMAGE218
。
在上式(25)中，等式右边不再包含特征点位置，这样就满足了卡尔曼滤波中观测方程样式，可以使用变换后的重投影误差
Figure 900825DEST_PATH_IMAGE219
进行卡尔曼滤波观测与更新。
视觉信息作为观测量时没有固定的滤波周期，而是采取一定的策略进行滤波更新，滤波更新在两种情况下会被触发。第一种情况是某一个特征点跟踪结束。当一个特征点从视野中消失时，此时该特征点的跟踪过程结束，对包含该特征点的相机位姿进行滤波更新。第二种情况是状态向量中的滤波状态达到上限。每当产生新的关键帧，状态向量中将会添加新的相机位姿，如果某些特征点的生命周期很长，将会使状态向量的维度很大，为了提高计算效率和避免长时间不更新造成误差积累过大，对状态向量中的相机状态维数设置最大阈值，当达到此阈值后，删除部分相机状态，在删除的同时，利用已有的观测信息对这些状态进行滤波更新。滤波的预测和更新过程所涉及的公式为公式(1)- (25)所示。
步骤三，根据视觉/惯性里程计组合导航系统的系统模型和观测模型完成卡尔曼滤波，输出滤波后的组合导航系统的姿态、速度、位置信息。
为验证本发明所提供方法的有效性，以开源数据集KITTI的惯导、视觉数据为例，对本发明提供的视觉/惯性里程计组合导航方法与传统的多状态约束卡尔曼滤波（MSCKF）的惯性/视觉紧组合导航方法进行性能对比。IMU输出频率为100 Hz, 图像的帧率为10 Hz。
位置误差结果如图2所示，从图2可以看出，本发明所提出的视觉/惯性里程计组合导航方法具有比传统的多状态约束卡尔曼滤波（MSCKF）的惯性/视觉紧组合导航方法更高的位置精度。
航向角误差结果如图3所示，从图3可以看出，本发明所提出的视觉/惯性里程计组合导航方法具有比传统的多状态约束卡尔曼滤波（MSCKF）的惯性/视觉紧组合导航方法更高的航向估计精度。
航向角滤波均方根误差如图4所示，从图4可以看出，本发明所提出的基于状态变换的视觉/惯性里程计多状态约束滤波方法的航向角估计具有更好的一致性。虽然航向角是不可观的，但是传统的多状态约束卡尔曼滤波（MSCKF）的惯性/视觉紧组合导航方法的航向角滤波均方根却逐渐减小，说明滤波错误的认为航向角是可观的，进而产生了错误的估计。
综上所述，虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明，任何本领域普通技术人员，在不脱离本发明的精神和范围内，当可作各种更动与润饰，因此本发明的保护范围当视权利要求书界定的范围为准。

Claims (5)

1.视觉/惯性里程计组合导航方法，其特征在于，包括：
在载体上安装视觉/惯性里程计组合导航系统，视觉/惯性里程计组合导航系统包括IMU以及相机，IMU接收载体的运动信息，相机同步采集图像信息，根据IMU接收到的运动信息进行捷联惯性导航解算，得到载体的姿态、速度和位置信息；对相机同步采集到的图像信息提取特征点并对提取的特征点进行跟踪；
构建视觉/惯性里程计组合导航系统的系统模型和观测模型，根据视觉/惯性里程计组合导航系统的系统模型和观测模型完成卡尔曼滤波，输出滤波后的组合导航系统的姿态、速度、位置信息，其中构建视觉/惯性里程计组合导航系统的系统模型和观测模型，包括：
（1）确定视觉/惯性里程计组合导航模型的状态向量；
t时刻视觉/惯性里程计组合导航模型的状态向量x由与惯导相关的状态向量和与相机相关的状态向量组成：
Figure 563442DEST_PATH_IMAGE001
其中，
Figure 100733DEST_PATH_IMAGE002
是t时刻与惯导相关的状态向量，
Figure 770749DEST_PATH_IMAGE003
是t时刻与相机相关的状态向量；
（2）构建基于状态变换的与惯导误差状态相关的系统模型：
Figure 88598DEST_PATH_IMAGE004
其中F ST-IMU 表示系统矩阵，
Figure 747768DEST_PATH_IMAGE005
表示与
Figure 264200DEST_PATH_IMAGE006
相对应的误差状态向量，G ST-IMU 为噪声转移矩阵，
Figure 980483DEST_PATH_IMAGE007
为系统噪声；
基于状态变换的与惯导误差状态相关的系统模型中的各参数定义如下：
设t时刻与惯导相关的状态向量
Figure 582366DEST_PATH_IMAGE008
为：
Figure 307876DEST_PATH_IMAGE009
Figure 678815DEST_PATH_IMAGE010
是Hamilton单位四元数，表示从IMU坐标系到世界参考坐标系的旋转，
Figure 565999DEST_PATH_IMAGE011
是IMU相对于世界参考坐标系的速度在世界参考坐标系中的投影，
Figure 124020DEST_PATH_IMAGE012
是IMU在世界参考坐标系中的位置坐标，
Figure 653221DEST_PATH_IMAGE013
和
Figure 878666DEST_PATH_IMAGE014
分别是三轴陀螺仪零偏和三轴加速度计零偏，
Figure 936752DEST_PATH_IMAGE015
表示从相机坐标系到IMU坐标系的旋转，
Figure 513227DEST_PATH_IMAGE016
表示IMU坐标系的原点到相机坐标系的原点的矢量在IMU坐标系下的投影；
则与
Figure 314961DEST_PATH_IMAGE017
相对应的误差状态向量
Figure 394912DEST_PATH_IMAGE018
表示为：
Figure 889479DEST_PATH_IMAGE019
其中
Figure 687670DEST_PATH_IMAGE020
、
Figure 24586DEST_PATH_IMAGE021
、
Figure 959044DEST_PATH_IMAGE022
、
Figure 890091DEST_PATH_IMAGE023
、
Figure 910000DEST_PATH_IMAGE024
、
Figure 319115DEST_PATH_IMAGE025
、
Figure 576921DEST_PATH_IMAGE026
分别代表
Figure 272345DEST_PATH_IMAGE027
时刻的惯导姿态角误差、基于状态变换后的速度误差、位置误差、陀螺零偏误差、三轴加速度计零偏误差、相机和IMU之间的安装误差角、相机和IMU之间的杆臂误差；
定义基于状态变换后的新的速度误差
Figure 920495DEST_PATH_IMAGE028
为：
Figure 992356DEST_PATH_IMAGE029
Figure 42352DEST_PATH_IMAGE030
为速度的估计值，
Figure 174256DEST_PATH_IMAGE031
为速度的实际真值,
Figure 44123DEST_PATH_IMAGE032
代表IMU坐标系到世界参考坐标系的旋转矩阵，
Figure 919675DEST_PATH_IMAGE033
代表世界参考坐标系到IMU坐标系的旋转矩阵估计值，
Figure 824177DEST_PATH_IMAGE034
为惯导的姿态角误差；
Figure 64666DEST_PATH_IMAGE035
其中
Figure 812042DEST_PATH_IMAGE036
为地球自转角速度在世界参考坐标系中的投影，
Figure 101072DEST_PATH_IMAGE037
为重力加速度，I 3为三维单位矩阵，
Figure 250293DEST_PATH_IMAGE038
为速度向量对应的斜对称矩阵；
基于状态变换后的噪声转移矩阵G ST-IMU 为：
Figure 348436DEST_PATH_IMAGE039
定义系统噪声
Figure 317529DEST_PATH_IMAGE040
为：
Figure 410249DEST_PATH_IMAGE041
其中
Figure 148398DEST_PATH_IMAGE042
和
Figure 668373DEST_PATH_IMAGE043
分别为三轴陀螺仪和三轴加速度计的测量白噪声，
Figure 328024DEST_PATH_IMAGE044
和
Figure 755594DEST_PATH_IMAGE045
是驱动随机游走误差的白噪声，
Figure 613829DEST_PATH_IMAGE046
和
Figure 632600DEST_PATH_IMAGE047
分别为惯导和相机之间的相对姿态角噪声和相对位移噪声；
（3）构建基于状态变换的与相机位姿误差状态相关的状态和协方差传递模型：
状态向量x除了由与惯导相关的状态向量
Figure 982810DEST_PATH_IMAGE048
有关之外，还与相机相关的状态向量
Figure 745230DEST_PATH_IMAGE049
组成；
Figure 130075DEST_PATH_IMAGE050
包含t时刻之前N帧的相机位姿，N取决于当前跟踪的特征点已经被跟踪的长度，同时还取决于设置的窗口最大宽度；
Figure 319748DEST_PATH_IMAGE051
表示为：
Figure 626095DEST_PATH_IMAGE052
其中四元数
Figure 988944DEST_PATH_IMAGE053
表示相机坐标系到世界参考坐标系的旋转，
Figure 431557DEST_PATH_IMAGE054
表示相机在世界参考坐标系的位置坐标，下角标1至N表示特征点被跟踪的帧数；
与
Figure 588869DEST_PATH_IMAGE055
对应的与相机相关的误差状态向量
Figure 444830DEST_PATH_IMAGE056
表示为：
Figure 14964DEST_PATH_IMAGE057
其中
Figure 905559DEST_PATH_IMAGE058
代表相机在世界参考坐标系的姿态角误差，
Figure 109139DEST_PATH_IMAGE059
代表相机在世界参考坐标系的位置坐标误差；
每次获得相机的关键帧即具有平移运动或旋转和平移运动的帧后，需要增加关键帧对应的相机位姿状态到与相机相关的状态向量中，并且扩增状态协方差矩阵；
Figure 249133DEST_PATH_IMAGE060
表示增加了新的关键帧的相机位姿状态后的与相机相关的状态向量，
Figure 829150DEST_PATH_IMAGE061
对应的与相机相关的误差状态向量
Figure 370990DEST_PATH_IMAGE062
表示为：
Figure 745471DEST_PATH_IMAGE063
其中
Figure 372761DEST_PATH_IMAGE064
为新增加的关键帧对应的相机姿态角误差向量，
Figure 490890DEST_PATH_IMAGE065
为新增加的关键帧对应的相机位置误差在世界参考坐标系的向量；
同时，
Figure 152815DEST_PATH_IMAGE066
，
Figure 698197DEST_PATH_IMAGE067
扩增后的协方差矩阵表示为：
Figure 547205DEST_PATH_IMAGE068
其中
Figure 796920DEST_PATH_IMAGE069
为扩增前的协方差矩阵，
Figure 188718DEST_PATH_IMAGE070
为6N加15维的单位矩阵；
Figure 29636DEST_PATH_IMAGE071
为扩增后的协方差矩阵，表示为：
Figure 506884DEST_PATH_IMAGE072
其中
Figure 91449DEST_PATH_IMAGE073
为新增加的与相机相关的状态误差与未增加之前的所有状态误差
Figure 340684DEST_PATH_IMAGE074
的协方差，
Figure 352502DEST_PATH_IMAGE075
为新增加的与相机相关的状态误差的方差，表示为：
Figure 317047DEST_PATH_IMAGE076
Figure 439724DEST_PATH_IMAGE077
（4）根据特征点的重投影误差构建视觉/惯性里程计组合导航系统的观测模型；
设提取的一系列特征点中，第j个特征点
Figure 806114DEST_PATH_IMAGE078
包含在第
Figure 988834DEST_PATH_IMAGE079
帧图像中，即在第
Figure 175096DEST_PATH_IMAGE079
帧图像对应的相机坐标系中的三维坐标为
Figure 101464DEST_PATH_IMAGE080
，在第i帧图像中的二维坐标为
Figure 650257DEST_PATH_IMAGE081
，有如下关系：
Figure 879244DEST_PATH_IMAGE082
Figure 677435DEST_PATH_IMAGE083
分别为第
Figure 282860DEST_PATH_IMAGE084
帧图像的第j个特征点在第
Figure 482897DEST_PATH_IMAGE084
帧图像对应的相机坐标系中的X，Y，Z坐标；
Figure 882786DEST_PATH_IMAGE085
表示第j个特征点在世界参考坐标系中的坐标；
Figure 168274DEST_PATH_IMAGE086
表示第
Figure 842969DEST_PATH_IMAGE084
个相机位置在世界参考坐标系中的坐标，其中第
Figure 897512DEST_PATH_IMAGE084
个相机位置与第
Figure 465372DEST_PATH_IMAGE084
帧图像对应；
Figure 238156DEST_PATH_IMAGE087
表示世界参考坐标系到第
Figure 185384DEST_PATH_IMAGE084
个相机坐标系的旋转矩阵；
Figure 94434DEST_PATH_IMAGE088
其中
Figure 101704DEST_PATH_IMAGE089
表示第
Figure 299467DEST_PATH_IMAGE084
帧图像中的第j个特征点投影到相机平面的观测噪声向量，建模为高斯白噪声；
第
Figure 847123DEST_PATH_IMAGE084
帧图像中的第j个特征点的重投影误差为：
Figure 813942DEST_PATH_IMAGE090
其中
Figure 116748DEST_PATH_IMAGE091
为第
Figure 739490DEST_PATH_IMAGE084
帧图像中的第j个特征点二维坐标的估计值，
Figure 153154DEST_PATH_IMAGE092
是直接由特征提取获得的第
Figure 912162DEST_PATH_IMAGE084
帧图像中的第j个特征点二维坐标：
Figure 385869DEST_PATH_IMAGE093
Figure 230328DEST_PATH_IMAGE094
将重投影误差
Figure 447683DEST_PATH_IMAGE095
用系统误差状态
Figure 389094DEST_PATH_IMAGE096
和特征点位置误差
Figure 909068DEST_PATH_IMAGE097
线性表示：
Figure 631037DEST_PATH_IMAGE098
其中
Figure 253080DEST_PATH_IMAGE099
表示第j个特征点在世界参考坐标系中的坐标，
Figure 111315DEST_PATH_IMAGE100
和
Figure 536611DEST_PATH_IMAGE101
是
Figure 683559DEST_PATH_IMAGE102
分别相对于误差状态向量和特征点位置误差的雅克比矩阵：
Figure 242716DEST_PATH_IMAGE103
Figure 830823DEST_PATH_IMAGE104
Figure 817234DEST_PATH_IMAGE105
分别为第i帧图像的第j个特征点在第
Figure 858002DEST_PATH_IMAGE084
帧图像对应的相机坐标系中的X，Y，Z坐标的估计值，
Figure 955271DEST_PATH_IMAGE106
代表
Figure 929043DEST_PATH_IMAGE107
的估计值;
假设特征点
Figure 86355DEST_PATH_IMAGE108
共被所采集的所有图像中的
Figure 614420DEST_PATH_IMAGE109
帧图像所包含，将
Figure 249800DEST_PATH_IMAGE110
在所有这些图像中的重投影误差堆叠为一列，得到：
Figure 78079DEST_PATH_IMAGE111
其中
Figure 406292DEST_PATH_IMAGE112
；
最后，假设A是一个酉矩阵，A的列构成
Figure 749549DEST_PATH_IMAGE113
左零空间的一组基，将上式左乘
Figure 63987DEST_PATH_IMAGE114
得：
Figure 605827DEST_PATH_IMAGE115
其中
Figure 977378DEST_PATH_IMAGE116
；
堆叠而成的
Figure 604668DEST_PATH_IMAGE117
是列满秩的，因此酉矩阵A的维度为
Figure 722797DEST_PATH_IMAGE118
，因而
Figure 525668DEST_PATH_IMAGE119
的维度为
Figure 195683DEST_PATH_IMAGE120
，使用变换后的重投影误差
Figure 185636DEST_PATH_IMAGE122
进行卡尔曼滤波观测与更新。
2.根据权利要求1所述的视觉/惯性里程计组合导航方法，其特征在于：IMU接收的运动信息包括三轴陀螺仪的角增量或角速度信息以及三轴加速度计的比力或比力积分增量信息。
3.根据权利要求1所述的视觉/惯性里程计组合导航方法，其特征在于：对相机同步采集到的图像信息提取FAST角点作为特征点，采用KLT光流跟踪法对提取的特征点进行跟踪。
4.一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于：所述处理器执行所述计算机程序时实现权利要求1至3中任一项所述视觉/惯性里程计组合导航方法的步骤。
5.一种存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现权利要求1至3中任一项所述视觉/惯性里程计组合导航方法的步骤。
CN202010963803.2A 2020-09-15 2020-09-15 视觉/惯性里程计组合导航方法、电子设备及存储介质 Active CN111811506B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010963803.2A CN111811506B (zh) 2020-09-15 2020-09-15 视觉/惯性里程计组合导航方法、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010963803.2A CN111811506B (zh) 2020-09-15 2020-09-15 视觉/惯性里程计组合导航方法、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN111811506A CN111811506A (zh) 2020-10-23
CN111811506B true CN111811506B (zh) 2020-12-01

Family

ID=72859232

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010963803.2A Active CN111811506B (zh) 2020-09-15 2020-09-15 视觉/惯性里程计组合导航方法、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN111811506B (zh)

Families Citing this family (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112461237B (zh) * 2020-11-26 2023-03-14 浙江同善人工智能技术有限公司 一种应用于动态变化场景下的多传感器融合定位方法
CN112556719B (zh) * 2020-11-27 2022-01-21 广东电网有限责任公司肇庆供电局 一种基于cnn-ekf的视觉惯性里程计实现方法
CN112683305B (zh) * 2020-12-02 2022-03-04 中国人民解放军国防科技大学 一种基于点线特征的视觉-惯性里程计状态估计方法
CN112697142B (zh) * 2020-12-21 2023-03-10 南京航空航天大学 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法
CN112729283A (zh) * 2020-12-21 2021-04-30 西北工业大学 一种基于深度相机/mems惯导/里程计组合的导航方法
CN112985388B (zh) * 2021-02-08 2022-08-19 福州大学 基于大位移光流法的组合导航方法及系统
CN114964216A (zh) * 2021-02-23 2022-08-30 广州汽车集团股份有限公司 一种车辆定位方法及系统
CN113218394A (zh) * 2021-04-20 2021-08-06 浙江大学 扑翼飞行器室内视觉定位方法及系统
CN113358134A (zh) * 2021-04-20 2021-09-07 重庆知至科技有限公司 一种视觉惯性里程计系统
CN113514058A (zh) * 2021-04-23 2021-10-19 北京华捷艾米科技有限公司 融合msckf和图优化的视觉slam定位方法及装置
CN113503872B (zh) * 2021-06-03 2024-04-12 浙江工业大学 一种基于相机与消费级imu融合的低速无人车定位方法
CN113390408A (zh) * 2021-06-30 2021-09-14 深圳市优必选科技股份有限公司 一种机器人定位方法、装置、机器人及存储介质
CN113587916B (zh) * 2021-07-27 2023-10-03 北京信息科技大学 实时稀疏视觉里程计、导航方法以及系统
CN113701749A (zh) * 2021-07-29 2021-11-26 江苏师范大学 一种基于立体视觉惯性里程计的车辆定位导航方法及系统
CN114018284B (zh) * 2021-10-13 2024-01-23 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114018254B (zh) * 2021-10-27 2024-03-12 南京师范大学 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法
CN114001733B (zh) * 2021-10-28 2024-03-15 浙江大学 一种基于地图的一致性高效视觉惯性定位算法
CN114184209B (zh) * 2021-10-29 2023-10-13 北京自动化控制设备研究所 用于低速检测平台系统的惯性误差抑制方法
CN114046800B (zh) * 2021-11-09 2023-09-29 浙江大学 一种基于双层滤波框架的高精度里程估计方法
CN114199084A (zh) * 2021-12-31 2022-03-18 陕西北斗东芯科技有限公司 一种图像制导控制系统及微型制导子弹
CN114184200B (zh) * 2022-02-14 2022-06-17 南京航空航天大学 一种结合动态建图的多源融合导航方法
CN115127554B (zh) * 2022-08-31 2022-11-15 中国人民解放军国防科技大学 一种基于多源视觉辅助的无人机自主导航方法与系统
CN115523929B (zh) * 2022-09-20 2023-05-12 北京四维远见信息技术有限公司 一种基于slam的车载组合导航方法、装置、设备及介质
CN116088020A (zh) * 2022-12-23 2023-05-09 中国铁路设计集团有限公司 一种基于低成本传感器集成的融合轨道三维重建方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014089113A (ja) * 2012-10-30 2014-05-15 Yamaha Corp 姿勢推定装置及びプログラム
CN103414451B (zh) * 2013-07-22 2015-11-25 北京理工大学 一种应用于飞行器姿态估计的扩展卡尔曼滤波方法
CN107796391A (zh) * 2017-10-27 2018-03-13 哈尔滨工程大学 一种捷联惯性导航系统/视觉里程计组合导航方法
US20190301871A1 (en) * 2018-03-27 2019-10-03 Artisense Corporation Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization
CN109084762A (zh) * 2018-08-12 2018-12-25 西北工业大学 基于惯导辅助单星定位的卡尔曼滤波动目标定位方法

Also Published As

Publication number Publication date
CN111811506A (zh) 2020-10-23

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
US9243916B2 (en) Observability-constrained vision-aided inertial navigation
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN109764880B (zh) 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
US20220051031A1 (en) Moving object tracking method and apparatus
CN110956665B (zh) 车辆拐弯轨迹双向计算方法、系统、装置
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN111380514A (zh) 机器人位姿估计方法、装置、终端及计算机存储介质
CN112577493B (zh) 一种基于遥感地图辅助的无人机自主定位方法及系统
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与系统
CN109141411B (zh) 定位方法、定位装置、移动机器人及存储介质
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN110749308A (zh) 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法
Hong et al. Visual inertial odometry using coupled nonlinear optimization
CN109785428A (zh) 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN114993338B (zh) 基于多段独立地图序列的一致性高效视觉惯性里程计算法
CN115235454A (zh) 行人运动约束的视觉惯性融合定位与建图方法和装置
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN114111769A (zh) 一种视觉惯性定位方法、装置及自动驾驶装置
CN110864685A (zh) 一种基于松耦合的车辆单目视觉轮式里程计定位方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant