CN109376785B - 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 - Google Patents

基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 Download PDF

Info

Publication number
CN109376785B
CN109376785B CN201811282269.8A CN201811282269A CN109376785B CN 109376785 B CN109376785 B CN 109376785B CN 201811282269 A CN201811282269 A CN 201811282269A CN 109376785 B CN109376785 B CN 109376785B
Authority
CN
China
Prior art keywords
state
imu
deviation
equation
follows
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
CN201811282269.8A
Other languages
English (en)
Other versions
CN109376785A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201811282269.8A priority Critical patent/CN109376785B/zh
Publication of CN109376785A publication Critical patent/CN109376785A/zh
Priority to PCT/CN2019/079138 priority patent/WO2020087846A1/zh
Application granted granted Critical
Publication of CN109376785B publication Critical patent/CN109376785B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
    • G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Multimedia (AREA)
  • Data Mining & Analysis (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

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

Description

基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
技术领域
本发明涉及导航技术领域，特别是涉及基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法。
背景技术
近年来，导航相关的仪器设备有着较为突破的发展，设备的性能和精度有着大幅度的提升，但单一传感器实现的的导航方法仍具有某些性能上的局限性。为了满足高性能的导航需求，组合导航方法近些年来得到了广泛的重视和发展。组合导航方法将多种导航传感器组合在一起，利用多种传感器测得信息对其各自局限性进行相互补偿以实现高精度导航并且增强系统的鲁棒性。
惯性导航是一门综合类技术，是现代科学发展到一定阶段的产物。惯性导航中主要利用了IMU作为传感器进行数据采集，一般IMU中会包含一个三轴的陀螺仪和加速度计，陀螺仪用于测量角速率，加速度计用于测量三轴方向上的加速度。在已知IMU初始位置、速度和姿态的前提下，利用航位推算的方法可以实现实时估算IMU位置、速度和姿态。纯惯性的导航方法只在初始时刻附近有较好的精度，这是因为IMU采集到的数据包含陀螺和加速度计的漂移使得纯惯性导航的精度会随时间发散。
单目相机以其结构简单、标定简单与价格低廉的优点而得到广泛的应用，但是单目相机由于只能在同一时刻获得一张图片而无法从图像中直接获得像素的深度信息。除此之外，单目相机还有着尺度不确定性的特点，该特点也是导致单目测量产生误差的主要原因。通过加入IMU数据可以解决单目相机初始化尺度的问题。
基于滤波的视觉和惯性的组合导航方法按照是否把图像状态信息加入到状态向量可以分为紧耦合和松耦合两种方式。紧耦合方式由于将图像特征信息加入到状态向量中使得计算量大大增加，其计算复杂度与图像特征数量程线性关系，且可扩展性差。松耦合虽然精度上略差于紧耦合，但其计算复杂度却远小于紧耦合。其次，松耦合具有较好的可扩展性和帧间计算复杂度不变的优点。本文采用了一次迭代扩展卡尔曼滤波的方法实现单目相机与惯性传感器的松耦合，该方法相较于扩展卡尔曼滤波有着更高的精度，且在计算复杂度上比无极卡尔曼滤波方法更为简单。
发明内容
为了解决以上问题，本发明提供一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，本发明可以在长时间实时定位与导航过程中保持较高的精度，且具有帧间计算复杂度不变的优点，为达此目的，本发明提供基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，该方法包括如下步骤：
步骤1：对IMU以及单目相机采集到的信息进行时间戳同步；
步骤2：计算单目相机连续两帧图像间的位姿变化；
步骤3：解算图像间IMU数据得到惯性测量的位置、速度与姿态变化；
步骤4：建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合。
作为本发明进一步改进，所述步骤1中对IMU以及单目相机采集到的信息进行时间戳同步，具体方法如下：
在机器人操作系统ROS平台上，利用ROS中消息过滤器进行传感器采集信息时间戳的匹配，每两帧图像之间有较多IMU数据，从图像的时间戳上搜索最近的IMU采集信息进行时间戳的同步。
作为本发明进一步改进，所述步骤2中计算单目相机连续两帧图像间的位姿变化，具体方法为：
1)在已知单目相机内参的前提下，提取初始时刻两帧图像作为初始帧进行初始化，提取图像的ORB特征点，利用对极约束得到对极几何关系，并计算其本质矩阵；
2)根据上述估计的本质矩阵，由奇异值分解恢复出相机的旋转和平移；
3)通过三角化的方法得到特征点的深度信息，并在之后的相机图像中利用PnP的方法解算相机的位姿。
作为本发明进一步改进，所述步骤3中解算图像间IMU数据得到惯性测量的位置、速度与姿态变化，具体方法为：
1)在已知载体初始位置、速度与姿态的前提下，利用IMU采集到的数据进行航位推算可得到载体实时位置、速度与姿态变化；
2)上述步骤1实现了IMU以及单目相机采集到的信息的时间戳同步，利用航位推算计算对应帧间IMU所测得的相对位置、速度与姿态变化。
作为本发明进一步改进，所述步骤4中建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合，具体方法为：
1)建立惯性传感器模型：
假设IMU中含有零偏误差b和高斯噪声n，所以在IMU框架下得到的真实的角速度和加速度为：
ω＝ωm-bω-nω
a＝am-ba-na
其中下标m表示为测量值，将以上零偏建模为动态随机过程可以得到：
Figure BDA0001848270530000031
2)选择状态变量
系统的状态变量表示如下，其中
Figure BDA0001848270530000032
分别表示IMU得到的从世界坐标系，选取东-北-天坐标系为世界坐标系，到IMU坐标系之间的位置、速度和旋转四元数变化，bω和ba为上述IMU中陀螺和加速度计的零偏，L表示为相机的尺度因子，
Figure BDA0001848270530000033
和
Figure BDA0001848270530000034
分别表示从IMU坐标系到相机坐标系的旋转四元数和位置变换，由此可以得到24维的系统状态向量；
Figure BDA0001848270530000035
由此状态变量可以得到以下微分方程：
Figure BDA0001848270530000036
Figure BDA0001848270530000037
Figure BDA0001848270530000038
Figure BDA0001848270530000039
其中g表示世界坐标系下的重力矢量，ω＝[ωx ωy ωz]T表示角速度变化
Figure BDA00018482705300000310
Ω(ω)为ω四元数微分方程的乘法阵，
Figure BDA00018482705300000317
表示向量ω的反对称矩阵，
Figure BDA00018482705300000311
为IMU坐标系到世界坐标系的方向余弦矩阵，假设尺度漂移非常缓慢，所以可以得到
Figure BDA00018482705300000312
由以上求导结果和滤波器的状态估计噪声模型可以得到：
Figure BDA00018482705300000313
Figure BDA00018482705300000314
Figure BDA00018482705300000315
Figure BDA00018482705300000316
3)建立状态偏差方程
通过上述状态向量及其微分方程，可以得到系统误差状态变量，其中四元数的误差不是通过简单的算术计算得到，而是使用四元数误差，如下所示：
Figure BDA0001848270530000041
Figure BDA0001848270530000042
所以利用
Figure BDA0001848270530000043
可以得到22维的系统状态偏差变量：
Figure BDA0001848270530000044
根据连续时间系统状态偏差变量建立其微分方程如下：
Figure BDA0001848270530000045
Figure BDA0001848270530000046
Figure BDA0001848270530000047
Figure BDA0001848270530000048
其中
Figure BDA0001848270530000049
使用以上方程可以得到如下线性化连续时间状态偏差方程：
Figure BDA00018482705300000410
其中n为系统噪声，其表示为
Figure BDA00018482705300000411
对上述线性化连续时间误差状态方程进行离散化：
Figure BDA00018482705300000412
取其前两项截断后可以得到：
Figure BDA00018482705300000413
其中：
Figure BDA00018482705300000414
Figure BDA0001848270530000051
Figure BDA0001848270530000052
F4＝-F1
Figure BDA0001848270530000053
Figure BDA0001848270530000054
通过连续系统噪声协方差矩阵
Figure BDA0001848270530000055
可以得到该离散系统的噪声协方差矩阵：
Figure BDA0001848270530000056
可以得到系统状态偏差的一步预测和一步预测的协方差矩阵：
Figure BDA0001848270530000057
Figure BDA0001848270530000058
4)建立系统观测模型
上述根据惯性测量单元以及其相关模型建立了系统状态偏差方程，更新系统状态偏差与其协方差矩阵，利用单目视觉传感器测得的信息建立观测方程，用于矫正上述系统状态偏差方程的状态量；
假设可以得到相机变换中旋转轴和旋转幅值的偏差，使用np表示位置观测噪声，使用nq表示旋转观测噪声，可以得到系统的观测噪声为nm＝[np nq]T，并以此得到系统的观测噪声协方差矩阵为R；
通过对单目相机采集到的图片进行处理可以得到视觉解算的系统位置观测量：
Figure BDA0001848270530000059
位置观测量偏差如下：
Figure BDA00018482705300000510
根据误差四元数的定义：
Figure BDA00018482705300000511
Figure BDA0001848270530000061
Figure BDA0001848270530000062
通过单目相机测得的旋转观测量可表示为：
Figure BDA0001848270530000063
利用上述误差四元数的定义可以得到系统旋转观测量偏差如下：
Figure BDA0001848270530000064
系统的观测量模型为：
Z＝[Zp Zq]T＝h(Xk,k)
将上述位置观测量偏差和旋转观测量偏差结合得到系统观测量偏差模型：
Figure BDA0001848270530000065
5)迭代扩展卡尔曼滤波更新状态
由上述状态偏差变量的一步预测可以得到该状态变量的一步预测，将系统观测方程在一步预测处进行泰勒展开并取一阶近似得Hk+1|k，所以可以得到：
Figure BDA0001848270530000066
有了上述推导，一次迭代扩展卡尔曼滤波描述如下：
计算滤波增益，下标0表示未进行迭代之前的值：
Figure BDA0001848270530000067
计算k+1时刻状态偏差向量的次优解，此处与标准离散扩展卡尔曼滤波是相同的：
Figure BDA0001848270530000068
可以得到系统在k+1时刻状态变量的次优解为：
Figure BDA0001848270530000069
状态方差协方差矩阵更新如下：
Figure BDA00018482705300000610
利用以上得到的次优解进行一次迭代，首先将系统的观测方程在次优解Xk+1|k+1,0处进行泰勒展开并取一阶近似得：
Figure BDA00018482705300000611
一次迭代后的滤波增益为：
Figure BDA0001848270530000071
一次迭代后得到的k+1时刻状态偏差向量的最优解为：
Figure BDA0001848270530000072
一次迭代后的状态方差协方差矩阵更新如下：
Figure BDA0001848270530000073
所以k+1时刻状态向量的最优解为：
Figure BDA0001848270530000074
本发明使用一次迭代扩展卡尔曼滤波对单目相机与惯性传感器进行数据融合，该方案相较于标准扩展卡尔曼滤波在精度上有了提升，虽然在计算复杂度上有所增加，但仍比无迹卡尔曼滤波计算复杂度简单。在系统状态方程的建立中未加入图像特征点信息大大减少了计算，并且实现了图像帧间计算复杂度不变的功能。
附图说明
图1是本发明一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法流程图；
图2是本发明单目相机与IMU采集数据时间戳同步示意图；
图3是本发明单目相机位姿估计的示意图；
图4是本发明惯性导航IMU解算位置、速度和姿态变化流程图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
本发明提供一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，本发明可以在长时间实时定位与导航过程中保持较高的精度，且具有帧间计算复杂度不变的优点。
如图1-4所示，一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，该方法包含如下步骤：
步骤1：对IMU以及单目相机采集到的信息进行时间戳同步，具体方法如下：
普通单目相机的采样频率为30HZ左右，而IMU采样频率较高可以达到几百甚至上千赫兹。在机器人操作系统ROS平台上，利用ROS中消息过滤器进行传感器采集信息时间戳的匹配。如图2所示，每两帧图像之间有较多IMU数据，从图像的时间戳上搜索最近的IMU采集信息进行时间戳的同步。
步骤2：计算单目相机连续两帧图像间的位姿变化，具体方法为：
1)在已知单目相机内参的前提下，提取初始时刻两帧图像作为初始帧进行初始化，提取图像的ORB特征点，利用对极约束得到对极几何关系，并计算其本质矩阵；
2)根据上述估计的本质矩阵，由奇异值分解恢复出相机的旋转和平移；
3)通过三角化的方法得到特征点的深度信息，并在之后的相机图像中利用PnP的方法解算相机的位姿。
步骤3：解算图像间IMU数据得到惯性测量的位置、速度与姿态变化，具体方法为：
1)在已知载体初始位置、速度与姿态的前提下，利用IMU采集到的数据进行航位推算可得到载体实时位置、速度与姿态变化；
2)上述步骤1实现了IMU以及单目相机采集到的信息的时间戳同步，利用航位推算计算对应帧间IMU所测得的相对位置、速度与姿态变化。
步骤4：建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合，具体方法为：
1)建立惯性传感器模型：
假设IMU中含有零偏误差b和高斯噪声n，所以在IMU框架下得到的真实的角速度和加速度为：
ω＝ωm-bω-nω
a＝am-ba-na
其中下标m表示为测量值，将以上零偏建模为动态随机过程可以得到：
Figure BDA0001848270530000081
2)选择状态变量
系统的状态变量表示如下，其中
Figure BDA0001848270530000082
分别表示IMU得到的从世界坐标系(选取东-北-天坐标系为世界坐标系)到IMU坐标系之间的位置、速度和旋转四元数变化，bω和ba为上述IMU中陀螺和加速度计的零偏，L表示为相机的尺度因子，
Figure BDA0001848270530000083
和
Figure BDA0001848270530000084
分别表示从IMU坐标系到相机坐标系的旋转四元数和位置变换，由此可以得到24维的系统状态向量。
Figure BDA0001848270530000085
由此状态变量可以得到以下微分方程：
Figure BDA0001848270530000086
Figure BDA0001848270530000087
Figure BDA0001848270530000091
Figure BDA0001848270530000092
其中g表示世界坐标系下的重力矢量，ω＝[ωx ωy ωz]T表示角速度变化，
Figure BDA0001848270530000093
Ω(ω)为ω四元数微分方程的乘法阵，
Figure BDA0001848270530000094
表示向量ω的反对称矩阵，
Figure BDA0001848270530000095
为IMU坐标系到世界坐标系的方向余弦矩阵，我们假设尺度漂移非常缓慢，所以可以得到
Figure BDA0001848270530000096
由以上求导结果和滤波器的状态估计噪声模型可以得到：
Figure BDA0001848270530000097
Figure BDA00018482705300000921
Figure BDA0001848270530000098
Figure BDA0001848270530000099
3)建立状态偏差方程
通过上述状态向量及其微分方程，可以得到系统误差状态变量，其中四元数的误差不是通过简单的算术计算得到，而是使用四元数误差，如下所示：
Figure BDA00018482705300000910
Figure BDA00018482705300000911
所以利用
Figure BDA00018482705300000912
可以得到22维的系统状态偏差变量：
Figure BDA00018482705300000913
根据连续时间系统状态偏差变量建立其微分方程如下：
Figure BDA00018482705300000914
Figure BDA00018482705300000915
Figure BDA00018482705300000916
Figure BDA00018482705300000917
其中
Figure BDA00018482705300000918
使用以上方程可以得到如下线性化连续时间状态偏差方程：
Figure BDA00018482705300000919
其中n为系统噪声，其表示为
Figure BDA00018482705300000920
对上述线性化连续时间误差状态方程进行离散化：
Figure BDA0001848270530000101
取其前两项截断后可以得到：
Figure BDA0001848270530000102
其中：
Figure BDA0001848270530000103
Figure BDA0001848270530000104
Figure BDA0001848270530000105
F4＝-F1
Figure BDA0001848270530000106
Figure BDA0001848270530000107
通过连续系统噪声协方差矩阵
Figure BDA0001848270530000108
可以得到该离散系统的噪声协方差矩阵：
Figure BDA0001848270530000109
可以得到系统状态偏差的一步预测和一步预测的协方差矩阵：
Figure BDA00018482705300001010
Figure BDA00018482705300001011
4)建立系统观测模型
上述根据惯性测量单元以及其相关模型建立了系统状态偏差方程，更新系统状态偏差与其协方差矩阵。利用单目视觉传感器测得的信息建立观测方程，用于矫正上述系统状态偏差方程的状态量。
假设可以得到相机变换中旋转轴和旋转幅值的偏差，使用np表示位置观测噪声，使用nq表示旋转观测噪声，可以得到系统的观测噪声为nm＝[np nq]T，并以此得到系统的观测噪声协方差矩阵为R。
通过对单目相机采集到的图片进行处理可以得到视觉解算的系统位置观测量：
Figure BDA0001848270530000111
位置观测量偏差如下：
Figure BDA0001848270530000112
根据误差四元数的定义：
Figure BDA0001848270530000113
Figure BDA0001848270530000114
Figure BDA0001848270530000115
通过单目相机测得的旋转观测量可表示为：
Figure BDA0001848270530000116
利用上述误差四元数的定义可以得到系统旋转观测量偏差如下：
Figure BDA0001848270530000117
系统的观测量模型为：
Z＝[Zp Zq]T＝h(Xk,k)
将上述位置观测量偏差和旋转观测量偏差结合得到系统观测量偏差模型：
Figure BDA0001848270530000118
5)迭代扩展卡尔曼滤波更新状态
由上述状态偏差变量的一步预测可以得到该状态变量的一步预测，将系统观测方程在一步预测处进行泰勒展开并取一阶近似得Hk+1|k，所以可以得到：
Figure BDA0001848270530000119
有了上述推导，一次迭代扩展卡尔曼滤波描述如下：
计算滤波增益，下标0表示未进行迭代之前的值：
Figure BDA00018482705300001110
计算k+1时刻状态偏差向量的次优解，此处与标准离散扩展卡尔曼滤波是相同的：
Figure BDA00018482705300001111
可以得到系统在k+1时刻状态变量的次优解为：
Figure BDA0001848270530000121
状态方差协方差矩阵更新如下：
Figure BDA0001848270530000122
利用以上得到的次优解进行一次迭代，首先将系统的观测方程在次优解Xk+1|k+1,0处进行泰勒展开并取一阶近似得：
Figure BDA0001848270530000123
一次迭代后的滤波增益为：
Figure BDA0001848270530000124
一次迭代后得到的k+1时刻状态偏差向量的最优解为：
Figure BDA0001848270530000125
一次迭代后的状态方差协方差矩阵更新如下：
Figure BDA0001848270530000126
所以k+1时刻状态向量的最优解为：
Figure BDA0001848270530000127
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (1)

1.基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，其特征在于：
该方法包括如下步骤：
步骤1：对IMU以及单目相机采集到的信息进行时间戳同步；
所述步骤1中对IMU以及单目相机采集到的信息进行时间戳同步，具体方法如下：
在机器人操作系统ROS平台上，利用ROS中消息过滤器进行传感器采集信息时间戳的匹配，每两帧图像之间有较多IMU数据，从图像的时间戳上搜索最近的IMU采集信息进行时间戳的同步；
步骤2：计算单目相机连续两帧图像间的位姿变化；
所述步骤2中计算单目相机连续两帧图像间的位姿变化，具体方法为：
1)在已知单目相机内参的前提下，提取初始时刻两帧图像作为初始帧进行初始化，提取图像的ORB特征点，利用对极约束得到对极几何关系，并计算其本质矩阵；
2)根据上述估计的本质矩阵，由奇异值分解恢复出相机的旋转和平移；
3)通过三角化的方法得到特征点的深度信息，并在之后的相机图像中利用PnP的方法解算相机的位姿；
步骤3：解算图像间IMU数据得到惯性测量的位置、速度与姿态变化；
所述步骤3中解算图像间IMU数据得到惯性测量的位置、速度与姿态变化，具体方法为：
1)在已知载体初始位置、速度与姿态的前提下，利用IMU采集到的数据进行航位推算可得到载体实时位置、速度与姿态变化；
2)上述步骤1实现了IMU以及单目相机采集到的信息的时间戳同步，利用航位推算计算对应帧间IMU所测得的相对位置、速度与姿态变化；
步骤4：建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合；
所述步骤4中建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合，具体方法为：
1)建立惯性传感器模型：
假设IMU中含有零偏误差b和高斯噪声n，所以在IMU框架下得到的真实的角速度和加速度为：
ω＝ωm-bω-nω
a＝am-ba-na
其中下标m表示为测量值，将以上零偏建模为动态随机过程可以得到：
Figure FDA0003196022880000011
2)选择状态变量
系统的状态变量表示如下，其中
Figure FDA0003196022880000021
分别表示IMU得到的从世界坐标系到IMU坐标系之间的位置、速度和旋转四元数变化，选取东-北-天坐标系为世界坐标系，bω和ba为上述IMU中陀螺和加速度计的零偏，L表示为相机的尺度因子，
Figure FDA0003196022880000022
和
Figure FDA0003196022880000023
分别表示从IMU坐标系到相机坐标系的旋转四元数和位置变换，由此可以得到24维的系统状态向量；
Figure FDA0003196022880000024
由此状态变量可以得到以下微分方程：
Figure FDA0003196022880000025
Figure FDA0003196022880000026
Figure FDA0003196022880000027
Figure FDA0003196022880000028
其中g表示世界坐标系下的重力矢量，ω＝[ωx ωy ωz]T表示角速度变化，
Figure FDA0003196022880000029
Ω(ω)为ω四元数微分方程的乘法阵，
Figure FDA00031960228800000216
表示向量ω的反对称矩阵，
Figure FDA00031960228800000210
为IMU坐标系到世界坐标系的方向余弦矩阵，假设尺度漂移非常缓慢，所以可以得到
Figure FDA00031960228800000211
由以上求导结果和滤波器的状态估计噪声模型可以得到：
Figure FDA00031960228800000212
Figure FDA00031960228800000213
Figure FDA00031960228800000214
Figure FDA00031960228800000215
3)建立状态偏差方程
通过上述状态向量及其微分方程，可以得到系统误差状态变量，其中四元数的误差不是通过简单的算术计算得到，而是使用四元数误差，如下所示：
Figure FDA0003196022880000031
Figure FDA0003196022880000032
所以利用
Figure FDA0003196022880000033
可以得到22维的系统状态偏差变量：
Figure FDA0003196022880000034
根据连续时间系统状态偏差变量建立其微分方程如下：
Figure FDA0003196022880000035
Figure FDA0003196022880000036
Figure FDA0003196022880000037
Figure FDA0003196022880000038
其中
Figure FDA0003196022880000039
使用以上方程可以得到如下线性化连续时间状态偏差方程：
Figure FDA00031960228800000310
其中n为系统噪声，其表示为
Figure FDA00031960228800000311
对上述线性化连续时间误差状态方程进行离散化：
Figure FDA00031960228800000312
取其前两项截断后可以得到：
Figure FDA00031960228800000313
其中：
Figure FDA00031960228800000314
Figure FDA00031960228800000315
Figure FDA0003196022880000041
F4＝-F1
Figure FDA0003196022880000042
Figure FDA0003196022880000043
通过连续系统噪声协方差矩阵
Figure FDA0003196022880000044
可以得到该离散系统的噪声协方差矩阵：
Figure FDA0003196022880000045
可以得到系统状态偏差的一步预测和一步预测的协方差矩阵：
Figure FDA0003196022880000046
Figure FDA0003196022880000047
4)建立系统观测模型
上述根据惯性测量单元以及其相关模型建立了系统状态偏差方程，更新系统状态偏差与其协方差矩阵，利用单目视觉传感器测得的信息建立观测方程，用于矫正上述系统状态偏差方程的状态量；
假设可以得到相机变换中旋转轴和旋转幅值的偏差，使用np表示位置观测噪声，使用nq表示旋转观测噪声，可以得到系统的观测噪声为nm＝[np nq]T，并以此得到系统的观测噪声协方差矩阵为R；
通过对单目相机采集到的图片进行处理可以得到视觉解算的系统位置观测量：
Figure FDA0003196022880000048
位置观测量偏差如下：
Figure FDA0003196022880000049
根据误差四元数的定义：
Figure FDA00031960228800000410
Figure FDA00031960228800000411
Figure FDA0003196022880000051
通过单目相机测得的旋转观测量可表示为：
Figure FDA0003196022880000052
利用上述误差四元数的定义可以得到系统旋转观测量偏差如下：
Figure FDA0003196022880000053
系统的观测量模型为：
Z＝[Zp Zq]T＝h(Xk,k)
将上述位置观测量偏差和旋转观测量偏差结合得到系统观测量偏差模型：
Figure FDA0003196022880000054
5)迭代扩展卡尔曼滤波更新状态
由上述状态偏差变量的一步预测可以得到该状态变量的一步预测，将系统观测方程在一步预测处进行泰勒展开并取一阶近似得Hk+1|k，所以可以得到：
Figure FDA0003196022880000055
有了上述推导，一次迭代扩展卡尔曼滤波描述如下：
计算滤波增益，下标0表示未进行迭代之前的值：
Figure FDA0003196022880000056
计算k+1时刻状态偏差向量的次优解，此处与标准离散扩展卡尔曼滤波是相同的：
Figure FDA0003196022880000057
可以得到系统在k+1时刻状态变量的次优解为：
Figure FDA0003196022880000058
状态方差协方差矩阵更新如下：
Figure FDA0003196022880000059
利用以上得到的次优解进行一次迭代，首先将系统的观测方程在次优解Xk+1|k+1,0处进行泰勒展开并取一阶近似得：
Figure FDA00031960228800000510
一次迭代后的滤波增益为：
Figure FDA0003196022880000061
一次迭代后得到的k+1时刻状态偏差向量的最优解为：
Figure FDA0003196022880000062
一次迭代后的状态方差协方差矩阵更新如下：
Figure FDA0003196022880000063
所以k+1时刻状态向量的最优解为：
Figure FDA0003196022880000064
CN201811282269.8A 2018-10-31 2018-10-31 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 Active CN109376785B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201811282269.8A CN109376785B (zh) 2018-10-31 2018-10-31 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
PCT/CN2019/079138 WO2020087846A1 (zh) 2018-10-31 2019-03-21 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811282269.8A CN109376785B (zh) 2018-10-31 2018-10-31 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

Publications (2)

Publication Number Publication Date
CN109376785A CN109376785A (zh) 2019-02-22
CN109376785B true CN109376785B (zh) 2021-09-24

Family

ID=65391020

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811282269.8A Active CN109376785B (zh) 2018-10-31 2018-10-31 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

Country Status (2)

Country Link
CN (1) CN109376785B (zh)
WO (1) WO2020087846A1 (zh)

Families Citing this family (161)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109376785B (zh) * 2018-10-31 2021-09-24 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111723624B (zh) * 2019-03-22 2023-12-05 京东方科技集团股份有限公司 一种头部运动跟踪方法和系统
CN110018691B (zh) * 2019-04-19 2022-04-15 天津大学 小型多旋翼无人机飞行状态估计系统和方法
CN110136168B (zh) * 2019-04-26 2021-06-18 北京航空航天大学 一种基于特征点匹配和光流法的多旋翼速度测量方法
CN110234029B (zh) * 2019-07-31 2021-12-17 上海商汤临港智能科技有限公司 多传感器数据的播放处理方法、装置、设备和存储介质
CN110517324B (zh) * 2019-08-26 2023-02-17 上海交通大学 基于变分贝叶斯自适应算法的双目vio实现方法
CN110455309B (zh) * 2019-08-27 2021-03-16 清华大学 具备在线时间校准的基于msckf的视觉惯性里程计
CN110490933A (zh) * 2019-09-18 2019-11-22 郑州轻工业学院 基于单点ransac的非线性状态空间中心差分滤波器方法
CN110864685B (zh) * 2019-10-18 2023-03-21 浙江天尚元科技有限公司 一种基于松耦合的车辆单目视觉轮式里程计定位方法
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法
CN110793526A (zh) * 2019-11-18 2020-02-14 山东建筑大学 基于可穿戴单目视觉和惯性传感器融合的行人导航方法及系统
CN110865650B (zh) * 2019-11-19 2022-12-20 武汉工程大学 基于主动视觉的无人机位姿自适应估计方法
CN110887481B (zh) * 2019-12-11 2020-07-24 中国空气动力研究与发展中心低速空气动力研究所 基于mems惯性传感器的载体动态姿态估计方法
CN111156994B (zh) * 2019-12-31 2023-10-27 上海星思半导体有限责任公司 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN111197984A (zh) * 2020-01-15 2020-05-26 重庆邮电大学 一种基于环境约束下的视觉-惯性运动估计方法
CN111450515B (zh) * 2020-03-20 2022-02-15 北京理工大学 一种用于机器人射击对抗比赛的自动识别打击系统及方法
CN111580596B (zh) * 2020-05-19 2022-04-15 北京数字绿土科技股份有限公司 多个imu时间同步方法、装置、终端
CN113701745B (zh) * 2020-05-21 2024-03-08 杭州海康威视数字技术股份有限公司 一种外参变化检测方法、装置、电子设备及检测系统
CN111578937B (zh) * 2020-05-29 2024-01-09 上海新天策数字科技有限公司 同时优化外参数的视觉惯性里程计系统
CN111833281B (zh) * 2020-06-08 2024-04-09 上海宇航系统工程研究所 面向可重复使用火箭回收的多视觉敏感器数据融合方法
CN111984919B (zh) * 2020-06-16 2023-06-23 济南大学 水泥回转窑温度场与物料密度的计算方法及系统
CN111739063B (zh) * 2020-06-23 2023-08-18 郑州大学 一种基于多传感器融合的电力巡检机器人定位方法
CN113932820A (zh) * 2020-06-29 2022-01-14 杭州海康威视数字技术股份有限公司 对象检测的方法和装置
CN111985300B (zh) * 2020-06-29 2023-11-03 魔门塔(苏州)科技有限公司 自动驾驶动态目标定位方法、装置、电子设备及存储介质
CN111949123B (zh) * 2020-07-01 2023-08-08 青岛小鸟看看科技有限公司 多传感器手柄控制器混合追踪方法及装置
CN112131928B (zh) * 2020-08-04 2024-06-18 浙江工业大学 一种rgb-d图像特征融合的人体姿态实时估计方法
CN112114665B (zh) * 2020-08-23 2023-04-11 西北工业大学 一种基于多模态融合的手部追踪方法
CN112129287B (zh) * 2020-09-24 2022-06-10 北京华捷艾米科技有限公司 一种基于视觉惯性里程计处理的方法和相关装置
CN114322996B (zh) * 2020-09-30 2024-03-19 阿里巴巴集团控股有限公司 一种多传感器融合定位系统的位姿优化方法和装置
CN112284381B (zh) * 2020-10-19 2022-09-13 北京华捷艾米科技有限公司 视觉惯性实时初始化对准方法及系统
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112344914A (zh) * 2020-10-30 2021-02-09 哈尔滨工业大学（深圳） 基于柔性臂手眼相机的自适应视觉测量方法及系统
CN112269317B (zh) * 2020-11-04 2024-03-15 天津大学 一种基于扩展卡尔曼滤波器的双边遥操作控制方法
CN112346032B (zh) * 2020-11-10 2023-07-14 中国科学院数学与系统科学研究院 基于一致性扩展卡尔曼滤波的单红外传感器目标定轨方法
CN114812610B (zh) * 2020-11-16 2024-07-16 浙江商汤科技开发有限公司 视觉惯性系统的参数标定方法及装置、电子设备和介质
CN112562077B (zh) * 2020-11-25 2024-01-09 西北工业大学 一种融合pdr和先验地图的行人室内定位方法
CN112472432B (zh) * 2020-11-25 2023-06-30 武汉理工大学 一种手杖-轮椅自动跟随系统及方法
CN112734765B (zh) * 2020-12-03 2023-08-22 华南理工大学 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN112683271B (zh) * 2020-12-17 2023-10-27 东南大学 一种考虑可观测性的水域观测平台的组合定位方法
CN112697142B (zh) * 2020-12-21 2023-03-10 南京航空航天大学 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法
CN115701810A (zh) * 2020-12-25 2023-02-14 深圳怪虫机器人有限公司 一种光伏清洁机器人辅助定位的方法
CN112712565B (zh) * 2020-12-28 2024-03-01 中国民航大学 基于视觉与imu融合的飞机蒙皮损伤无人机绕检定位方法
CN112833892B (zh) * 2020-12-31 2022-12-16 杭州自适应科技有限公司 一种基于轨迹对齐的语义建图方法
CN112833788B (zh) * 2021-01-07 2022-07-08 深圳许多米科技有限公司 一种枪体定位方法、装置、设备、仿真枪及可读存储介质
CN112880674A (zh) * 2021-01-21 2021-06-01 深圳市镭神智能系统有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN112883134A (zh) * 2021-02-01 2021-06-01 上海三一重机股份有限公司 数据融合建图方法、装置、电子设备及存储介质
CN113008229B (zh) * 2021-02-26 2024-04-05 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113091748A (zh) * 2021-04-12 2021-07-09 北京航空航天大学 一种室内自校准导航定位方法
CN113220017A (zh) * 2021-04-16 2021-08-06 同济大学 一种地下无人机飞行方法及系统
CN113324544B (zh) * 2021-04-19 2023-02-28 北京化工大学 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN113189578B (zh) * 2021-04-20 2022-09-16 浙江大学 一种扩展目标跟踪方法
CN115307626A (zh) * 2021-05-06 2022-11-08 北京航通云科技有限公司 一种应用于小型无人机的冗余定位方法
CN113240597B (zh) * 2021-05-08 2024-04-26 西北工业大学 基于视觉惯性信息融合的三维软件稳像方法
CN113516692B (zh) * 2021-05-18 2024-07-19 上海汽车集团股份有限公司 一种多传感器融合的slam方法和装置
CN113124856B (zh) * 2021-05-21 2023-03-14 天津大学 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN113298881B (zh) * 2021-05-27 2023-09-12 中国科学院沈阳自动化研究所 单目相机-imu-机械臂的空间联合标定方法
CN113640791B (zh) * 2021-06-09 2023-12-26 西安电子科技大学 一种基于距离和瞬时速度的空间目标三维姿态重构方法
CN113341718B (zh) * 2021-06-10 2022-07-08 浙江大学 欠驱动水面机器人的水动力系数混合自适应估计方法
CN113551671B (zh) * 2021-06-10 2023-04-11 中国科学院西安光学精密机械研究所 一种无人机姿态与位置的实时高精度测量方法
CN113313116B (zh) * 2021-06-20 2024-06-21 西北工业大学 一种基于视觉的水下人工目标准确检测与定位方法
CN113408623B (zh) * 2021-06-21 2022-10-04 北京理工大学 非合作目标柔性附着多节点融合估计方法
CN113406682B (zh) * 2021-06-22 2024-03-12 腾讯科技（深圳）有限公司 一种定位方法、装置、电子设备及存储介质
CN113465596B (zh) * 2021-06-25 2023-05-30 电子科技大学 一种基于多传感器融合的四旋翼无人机定位方法
CN113432603B (zh) * 2021-06-25 2022-09-16 中国船舶重工集团公司第七0七研究所 一种复杂环境下母平台与子平台相对位姿测量方法
CN113674327B (zh) * 2021-07-01 2024-04-12 北京航空航天大学 一种适用于无人机空中防撞的入侵机航迹融合跟踪方法
CN113551666A (zh) * 2021-07-06 2021-10-26 杭州鸿泉物联网技术股份有限公司 自动驾驶多传感器融合定位方法和装置、设备及介质
CN113516714B (zh) * 2021-07-15 2024-08-13 北京理工大学 基于imu预积分信息加速特征匹配的视觉slam方法
CN113608556B (zh) * 2021-07-19 2023-06-30 西北工业大学 一种基于多传感器融合的多机器人相对定位方法
CN113534227B (zh) * 2021-07-26 2022-07-01 中国电子科技集团公司第五十四研究所 一种适用于复杂非合作场景的多传感器融合绝对定位方法
CN113709662B (zh) * 2021-08-05 2023-12-01 北京理工大学重庆创新中心 一种基于超宽带的自主式三维反演定位方法
CN113674412B (zh) * 2021-08-12 2023-08-29 浙江工商大学 基于位姿融合优化的室内地图构建方法、系统及存储介质
CN113639782A (zh) * 2021-08-13 2021-11-12 北京地平线信息技术有限公司 车载传感器的外参标定方法和装置、设备和介质
CN113865584B (zh) * 2021-08-24 2024-05-03 知微空间智能科技(苏州)有限公司 一种基于视觉惯性里程计的uwb三维寻物方法和装置
CN113724337B (zh) * 2021-08-30 2024-02-23 合肥工业大学 一种无需依赖云台角度的相机动态外参标定方法及装置
CN113884098B (zh) * 2021-10-15 2024-01-23 上海师范大学 一种基于具体化模型的可迭代的卡尔曼滤波定位方法
CN113938825B (zh) * 2021-10-15 2024-06-14 太原理工大学 一种基于蓝牙aoa的综采工作面采煤机定位方法及系统
CN114018250B (zh) * 2021-10-18 2024-05-03 杭州鸿泉物联网技术股份有限公司 惯性导航方法、电子设备、存储介质和计算机程序产品
CN113759982B (zh) * 2021-10-19 2024-05-28 中国人民解放军军事科学院国防科技创新研究院 一种仅基于视线测量信息的无人机编队相对状态估计方法
CN113984057A (zh) * 2021-10-19 2022-01-28 山东中瑞电气有限公司 基于多数据分析的移动机器人定位方法
CN113989371B (zh) * 2021-10-28 2024-05-03 山东大学 一种基于视觉的模块化平台相对位姿估计系统
CN114199233B (zh) * 2021-11-08 2024-04-05 北京旷视科技有限公司 位姿确定方法及可移动设备
CN114488241A (zh) * 2021-11-20 2022-05-13 国网江西省电力有限公司检修分公司 基于北斗导航系统的智能巡检机器人定位方法
CN114040128B (zh) * 2021-11-24 2024-03-01 视辰信息科技(上海)有限公司 时间戳延时标定方法及系统、设备和计算机可读存储介质
CN114139580B (zh) * 2021-11-24 2024-08-23 中电科航空电子有限公司 一种甚高频全向信标方位解算方法、装置、设备及介质
CN114147717B (zh) * 2021-12-09 2024-05-24 乐聚(深圳)机器人技术有限公司 机器人运动轨迹估计方法、装置、控制器及存储介质
CN114485577B (zh) * 2021-12-13 2024-04-12 丁莹莹 一种基于视觉的无人机着陆姿态估计方法
CN114565669A (zh) * 2021-12-14 2022-05-31 华人运通(上海)自动驾驶科技有限公司 一种场端多相机融合定位方法
CN114322943B (zh) * 2021-12-15 2023-03-28 南京航空航天大学 一种基于无人机前视图像的目标距离测量方法及装置
CN114241005A (zh) * 2021-12-20 2022-03-25 泉州装备制造研究所 一种光学与惯性融合的定位跟踪方法及装置
CN114485574B (zh) * 2021-12-21 2023-03-21 武汉大学 基于卡尔曼滤波模型的三线阵影像pos辅助对地定位方法
CN114241010B (zh) * 2021-12-23 2024-09-03 青岛科技大学 一种融合三维点云与图像特征点匹配的着陆器导航方法
CN114296069B (zh) * 2021-12-23 2024-05-28 青岛科技大学 一种基于视觉雷达的小天体探测器多模型导航方法
CN114280925B (zh) * 2021-12-24 2023-07-18 合肥工业大学 基于单目视觉的可变杆长倒立摆目标捕捉控制方法及系统
CN114323003B (zh) * 2021-12-27 2024-07-19 青岛慧拓智能机器有限公司 一种基于uwb、imu及激光雷达的井工矿融合定位方法
CN114554389B (zh) * 2021-12-29 2023-06-20 重庆邮电大学 一种行人导航定位系统融合方法
CN114459467B (zh) * 2021-12-30 2024-05-03 北京理工大学 一种未知救援环境中基于vi-slam的目标定位方法
CN114549622B (zh) * 2021-12-31 2024-08-16 广州文远知行科技有限公司 状态更新优化方法、装置、设备及存储介质
CN114370870B (zh) * 2022-01-05 2024-04-12 中国兵器工业计算机应用技术研究所 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法
CN114323050B (zh) * 2022-01-07 2024-07-19 苏州挚途科技有限公司 车辆定位方法、装置和电子设备
CN114396943B (zh) * 2022-01-12 2024-07-23 国家电网有限公司 一种融合定位方法与终端
CN114413893A (zh) * 2022-01-19 2022-04-29 天津大学 一种融合惯性测量信息的动态位置、姿态同步的测量方法
CN114485648B (zh) * 2022-02-08 2024-02-02 北京理工大学 一种基于仿生复眼惯性系统的导航定位方法
CN114184200B (zh) * 2022-02-14 2022-06-17 南京航空航天大学 一种结合动态建图的多源融合导航方法
CN115060260A (zh) * 2022-02-22 2022-09-16 上海大学 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法
CN114608568B (zh) * 2022-02-22 2024-05-03 北京理工大学 一种基于多传感器信息即时融合定位方法
CN114529585A (zh) * 2022-02-23 2022-05-24 北京航空航天大学 基于深度视觉和惯性测量的移动设备自主定位方法
CN114648062B (zh) * 2022-03-08 2024-08-27 中国科学院数学与系统科学研究院 一种针对主动和被动传感器量测数据的多目标匹配方法
CN114623819A (zh) * 2022-03-21 2022-06-14 中国电信股份有限公司 机器人定位方法、系统、装置、设备及存储介质
CN114413898B (zh) * 2022-03-29 2022-07-29 深圳市边界智控科技有限公司 多传感器数据融合方法、装置、计算机设备及存储介质
CN115031726A (zh) * 2022-03-29 2022-09-09 哈尔滨工程大学 一种数据融合导航定位方法
CN114662544B (zh) * 2022-04-02 2024-09-24 天津国科医疗科技发展有限公司 基于ckf的色谱峰估计方法、系统、存储介质及计算机设备
CN114567401B (zh) * 2022-04-14 2023-02-14 中国人民解放军火箭军工程大学 一种基于感知通信一体化的无人机蜂群状态联合估计方法
CN114993296B (zh) * 2022-04-19 2024-03-15 北京自动化控制设备研究所 一种制导炮弹高动态组合导航方法
CN114923479A (zh) * 2022-04-19 2022-08-19 北京自动化控制设备研究所 一种惯性/里程计组合导航误差补偿方法
CN114895241B (zh) * 2022-05-09 2024-05-03 知微空间智能科技(苏州)有限公司 一种基于数据和模型联合驱动的弹性融合定位方法和装置
CN114972528B (zh) * 2022-05-13 2024-09-24 南开大学深圳研究院 一种智慧手术室全局-局部多目标联合定位方法
CN114973660B (zh) * 2022-05-13 2023-10-24 黄河科技学院 一种模型线性化迭代更新法的交通决策方法
CN114820721B (zh) * 2022-05-17 2024-03-26 苏州轻棹科技有限公司 一种卡尔曼滤波观测噪声的可视化调制方法和装置
CN114838732B (zh) * 2022-05-18 2024-04-09 北京航空航天大学 一种基于图优化的通信受限环境下协同导航方法
CN114979943A (zh) * 2022-05-18 2022-08-30 东南大学 一种基于raukf的rssi室内定位算法
CN115024715B (zh) * 2022-05-20 2023-06-06 北京航天时代光电科技有限公司 一种人体运动智能测量与数字训练系统
CN115127547B (zh) * 2022-06-27 2024-04-19 长安大学 一种基于捷联惯导系统和图像定位的隧道检测车定位方法
CN115265591B (zh) * 2022-07-15 2024-04-09 北京航空航天大学 一种双轴旋转惯导imu与转位机构安装误差的标定方法
CN114964266B (zh) * 2022-07-26 2022-10-21 中国人民解放军国防科技大学 基于多视觉矢量的运动状态协同群组相对姿态确定方法
CN115371668B (zh) * 2022-07-29 2024-08-23 重庆大学 一种基于图像识别和惯性导航的隧道无人机定位系统
CN115655265B (zh) * 2022-09-23 2024-07-16 河海大学 一种基于惯性传感的行人导航室内定位方法
CN115639521B (zh) * 2022-10-24 2023-05-05 青岛柯锐思德电子科技有限公司 一种基于uwb的移动目标运动状态判断方法
CN115859212B (zh) * 2022-11-17 2023-07-18 广东智能无人系统研究院(南沙) 一种用于海洋设备的自主布放回收的方法及系统
CN115574816B (zh) * 2022-11-24 2023-03-14 东南大学 仿生视觉多源信息智能感知无人平台
CN115523920B (zh) * 2022-11-30 2023-03-10 西北工业大学 一种基于视觉惯性gnss紧耦合的无缝定位方法
CN115752442B (zh) * 2022-12-07 2024-03-12 运来智能装备(无锡)有限公司 一种基于单目视觉辅助惯性定位方法
CN115855042B (zh) * 2022-12-12 2024-09-06 北京自动化控制设备研究所 一种基于激光雷达协同辅助的行人视觉导航方法
CN115616641B (zh) * 2022-12-16 2023-03-17 南京航空航天大学 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法
CN115930971B (zh) * 2023-02-01 2023-09-19 七腾机器人有限公司 一种机器人定位与建图的数据融合处理方法
CN115855456A (zh) * 2023-03-03 2023-03-28 北京航空航天大学 一种基于视觉无人机平台的舰载机光学助降系统标校方法
CN116519011B (zh) * 2023-03-11 2024-03-01 中国人民解放军国防科技大学 基于Psi角误差修正模型的长航时双惯导协同标定方法
CN116182873B (zh) * 2023-05-04 2023-07-11 长沙驰芯半导体科技有限公司 室内定位方法、系统及计算机可读介质
CN116609776B (zh) * 2023-05-23 2023-11-14 兰州理工大学 复杂环境下的基于人工势场法的星凸形扩展目标跟踪方法
CN116736286B (zh) * 2023-05-24 2024-02-06 兰州理工大学 基于随机超曲面的渐进贝叶斯扩展目标跟踪方法及系统
CN116380056B (zh) * 2023-06-02 2023-08-15 中国船舶集团有限公司第七〇七研究所 惯性定位方法、装置、电子设备及存储介质
CN116543057B (zh) * 2023-06-27 2023-10-10 华南理工大学 一种水下多相机与imu一体化标定方法
CN116502478B (zh) * 2023-06-29 2023-09-01 中国电建集团华东勘测设计研究院有限公司 一种基于海底地形监测的自提升平台下桩辅助决策方法
CN116592896B (zh) * 2023-07-17 2023-09-29 山东水发黄水东调工程有限公司 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法
CN116734864B (zh) * 2023-08-14 2023-11-28 中国西安卫星测控中心 一种常值观测偏差条件下航天器自主相对导航方法
CN116772903B (zh) * 2023-08-16 2023-10-20 河海大学 基于迭代ekf的sins/usbl安装角估计方法
CN117058430B (zh) * 2023-10-12 2023-12-22 北京万龙精益科技有限公司 用于视场匹配的方法、装置、电子设备和存储介质
CN117148406B (zh) * 2023-10-30 2024-01-30 山东大学 一种室内外无缝弹性融合定位方法、系统、介质及设备
CN117495900B (zh) * 2023-11-06 2024-06-07 长沙理工大学 基于相机运动趋势估计的多目标视觉跟踪方法
CN117214857B (zh) * 2023-11-09 2024-02-02 中国人民解放军海军航空大学 一种三维场景下高斯多假设多扩展目标的跟踪方法
CN117433564B (zh) * 2023-12-05 2024-03-01 北京航空航天大学 一种基于太阳敏感器辅助的偏振传感器误差在线标定方法
CN117346794B (zh) * 2023-12-05 2024-02-23 山东省科学院海洋仪器仪表研究所 一种用于浒苔跟踪的无人船组合导航系统及导航方法
CN117793317A (zh) * 2024-01-05 2024-03-29 中国传媒大学 一种多传感器卡尔曼融合的异形面动态投影方法和装置
CN117609737B (zh) * 2024-01-18 2024-03-19 中国人民解放军火箭军工程大学 一种惯性导航系统健康状态预测方法、系统、设备及介质
CN117874982B (zh) * 2024-01-30 2024-09-20 中国气象局地球系统数值预报中心 一种多变量融合北斗导航探空站网布局方法、系统及设备
CN117788302B (zh) * 2024-02-26 2024-05-14 山东全维地信科技有限公司 一种测绘图形处理系统
CN117875226B (zh) * 2024-03-13 2024-06-18 江南大学 基于流形空间协方差递推的连续离散卡尔曼状态估计方法
CN118154700B (zh) * 2024-05-10 2024-09-06 常州星宇车灯股份有限公司 车辆传感器外参准确性的在线监测方法
CN118262336B (zh) * 2024-05-30 2024-09-03 南昌智能新能源汽车研究院 一种基于视觉slam的室内停车场定位方法及系统
CN118293925B (zh) * 2024-06-05 2024-09-06 山东大学 基于多传感器信息融合的双足机器人状态估计方法及系统
CN118518095A (zh) * 2024-07-24 2024-08-20 东南大学 基于观测量弹性调整的pdr/gnss自主导航方法
CN118654683A (zh) * 2024-08-16 2024-09-17 清华大学 一种基于视觉惯性里程计的车载导航系统与优化方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105931275A (zh) * 2016-05-23 2016-09-07 北京暴风魔镜科技有限公司 基于移动端单目和imu融合的稳定运动跟踪方法和装置
CN105953796A (zh) * 2016-05-23 2016-09-21 北京暴风魔镜科技有限公司 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN108090921A (zh) * 2016-11-23 2018-05-29 中国科学院沈阳自动化研究所 单目视觉和imu融合的自适应室内定位方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10295365B2 (en) * 2016-07-29 2019-05-21 Carnegie Mellon University State estimation for aerial vehicles using multi-sensor fusion
CN107014371A (zh) * 2017-04-14 2017-08-04 东南大学 基于扩展自适应区间卡尔曼的无人机组合导航方法与装置
CN108646760B (zh) * 2018-07-11 2022-03-25 中国人民解放军火箭军工程大学 基于单目视觉移动机器人目标跟踪及平台控制系统和方法
CN109376785B (zh) * 2018-10-31 2021-09-24 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105931275A (zh) * 2016-05-23 2016-09-07 北京暴风魔镜科技有限公司 基于移动端单目和imu融合的稳定运动跟踪方法和装置
CN105953796A (zh) * 2016-05-23 2016-09-21 北京暴风魔镜科技有限公司 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN108090921A (zh) * 2016-11-23 2018-05-29 中国科学院沈阳自动化研究所 单目视觉和imu融合的自适应室内定位方法

Also Published As

Publication number Publication date
CN109376785A (zh) 2019-02-22
WO2020087846A1 (zh) 2020-05-07

Similar Documents

Publication Publication Date Title
CN109376785B (zh) 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN108627153B (zh) 一种基于惯性传感器的刚体运动追踪系统及其工作方法
CN110095116A (zh) 一种基于lift的视觉定位和惯性导航组合的定位方法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN106017463B (zh) 一种基于定位传感装置的飞行器定位方法
CN104501814B (zh) 一种基于视觉和惯性信息的姿态与位置估计方法
CN112985450B (zh) 一种具有同步时间误差估计的双目视觉惯性里程计方法
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN105931275A (zh) 基于移动端单目和imu融合的稳定运动跟踪方法和装置
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
CN107014371A (zh) 基于扩展自适应区间卡尔曼的无人机组合导航方法与装置
CN110986939A (zh) 一种基于imu预积分的视觉惯性里程计方法
CN102445200A (zh) 微小型个人组合导航系统及其导航定位方法
CN111723624B (zh) 一种头部运动跟踪方法和系统
CN114199239B (zh) 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统
CN114383612B (zh) 一种视觉辅助惯性差分位姿测量系统
CN116047567B (zh) 基于深度学习辅助的卫惯组合定位方法及导航方法
CN115574816A (zh) 仿生视觉多源信息智能感知无人平台
CN112729283A (zh) 一种基于深度相机/mems惯导/里程计组合的导航方法
CN111811421B (zh) 一种高速实时形变监测方法及系统
WO2024041347A1 (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN117073720A (zh) 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备
CN117710476A (zh) 基于单目视觉的无人机位姿估计与稠密建图方法

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