CN113155152B - 基于李群滤波的相机与惯性传感器空间关系自标定方法 - Google Patents
基于李群滤波的相机与惯性传感器空间关系自标定方法 Download PDFInfo
- Publication number
- CN113155152B CN113155152B CN202110273084.6A CN202110273084A CN113155152B CN 113155152 B CN113155152 B CN 113155152B CN 202110273084 A CN202110273084 A CN 202110273084A CN 113155152 B CN113155152 B CN 113155152B
- Authority
- CN
- China
- Prior art keywords
- camera
- imu
- matrix
- equation
- lie
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000001914 filtration Methods 0.000 title claims abstract description 30
- 239000011159 matrix material Substances 0.000 claims abstract description 64
- 230000009466 transformation Effects 0.000 claims abstract description 12
- 230000008569 process Effects 0.000 claims description 12
- 230000000007 visual effect Effects 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 230000003287 optical effect Effects 0.000 claims description 4
- 230000004069 differentiation Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 13
- 238000005457 optimization Methods 0.000 abstract description 11
- 238000013507 mapping Methods 0.000 abstract description 3
- 238000002474 experimental method Methods 0.000 description 12
- 230000005284 excitation Effects 0.000 description 6
- 238000005286 illumination Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 4
- 230000001360 synchronised effect Effects 0.000 description 4
- 230000005484 gravity Effects 0.000 description 3
- 239000000654 additive Substances 0.000 description 2
- 230000000996 additive effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000005111 flow chemistry technique Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- 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
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
- G01S11/12—Systems for determining distance or velocity not using reflection or reradiation using electromagnetic waves other than radio waves
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了基于李群滤波的相机与惯性传感器空间关系自标定方法,采用李群描述代替传统四元数描述实现对旋转姿态变换的计算,利用李群微分方程建立基于李群描述的线性标定滤波模型。并根据李代数的物理定义,利用预测向量与观测向量乘积的方向和预测向量与观测向量之间的夹角来计算误差李代数。利用SO(4)群与李代数之间的映射关系对相机‑IMU空间关系旋转矩阵与陀螺仪常值漂移进行补偿。该方法避免了四元数描述需要存在的无法同步估计陀螺仪常值漂移问题,并且不存在基于优化方法存在的非凸问题,提高了对准精度,更适合于实际工程应用。
Description
技术领域
本发明公开了一种基于李群描述的捷联惯性导航动基座初始对准方法,该方法属于导航方法及应用技术领域。
背景技术
同步定位与测绘(SLAM)是自主移动机器人在未知环境中同时构建地图和估计位置的关键技术。在众多的SLAM系统中,视觉SLAM由于其成本和功耗等优势,在小行机器人定位和导航任务中具有独特的优势。
近年来,单目SLAM系统取得了许多重大进展。但是,由于摄像机将物体从3D压缩到2D,单目SLAM系统不能根据图像的特征信息估计真实深度,因此尺度模糊和漂移是单目SLAM系统常见的问题。与传统的单目SLAM相比,单目相机与惯性导航系统的融合,即视觉惯性导航系统,取得了更好的性能和稳定性。从目前开源的VINS:MSCKF、Rovio、OKVIS、VINS等VIO系统来看, VINS可以分为三个部分:前端数据处理、初始化和后端优化。前端处理部分主要包括惯性预积分和视觉图像处理,其中视觉图像处理按照不同的方法可分为光流处理和特征点匹配。后端优化部分采用的方法可分为过滤和优化。
然而,无论采用哪种前端处理方法和后端优化方法,在结合相机和IMU测量数据时,高精度的初始化过程是系统顺利运行的前提。它不仅是系统重力和比例初始化的保证,也是摄像机提取的运动信息与IMU测量正确融合的基础。因此,为了获得摄像机与IMU之间的刚体变换,传感器空间参数的外部标定一直是众多VO研究工作的主题。标定过程中的主要问题是计算相机和IMU之间的旋转矩阵。
目前,相机-IMU标定可分为离线标定和在线标定两种类型。相比之下,在线校准方法在传感器套件的每次操作过程中都会估算校准参数,使其更加稳健,校准过程不再受制于场景的约束。目前,在线标定也可以分为两种:一种是在摄像机和IMU之间进行标定,在标定完成后进行初始化,最后在摄像机标定和初始化完成后将标准量作为一种状态进行优化。其一是在系统运行初期获得具有足够精度的定标结果,并且外部参数不包含在状态估计的优化变量中。第一种方法对相机IMU的小变形有较强的鲁棒性,但在使用过程中如果变形过大会产生较大的误差。第二种方法适用于计算量较小的移动设备。
2012年,有人首次提出了一种不考虑传感器噪声的几何标定单目相机-IMU 外参数在线估计算法,为VIO提供了一个新的研究方向。近两年来,随着视觉 SLAM算法精度的提高,相机-IMU在线标定与初始化一起成为VIO研究的一个独立方向。2018年,Li[14]将单目摄像机IMU的外部参数作为待估计的状态向量,并使用扩展卡尔曼滤波(EKF)进行估计。然后杨[15]是基于东四的工作和四元数交换定律。在初始化过程中,采用线性估计方程SVD分解求解单目摄像机 -IMU外部参数,但该算法不对IMU零点进行标定。同年,黄文[16]在VIORB 工作[17]的基础上,建立了基于四元数交换定律的线性方程组,并用最优化方法标定了单目相机IMU的外部参数。
为进一步提高在线标定的性能,本发明针对现有的在线标定方法存在的问题,借鉴李群状态卡尔曼滤波的思想,提出了一种新的李群卡尔曼滤波算法。得益于李群理论的完善,通过改进新息的表达方式来保证状态量满足特殊正交的性质。基于这种方法构建的估计模型避免了传统最优化方法中的奇异值问题,每一步的迭代计算都符合李群旋转矩阵的特性。相比于四元数优化方法而言,体现李群滤波优势,提高了在线标定的鲁棒性,并且在精度方面也有所提升。实际实验证明了该算法的可行性,可以作为一种新的相机-IMU在线标定方法。
发明内容
由于在线相机-IMU传感器空间关系标定过程中,传感器容易受到外界的各种干扰因素的影响,其所提供的传感器信息无法保证。因此,基于李群滤波的传感器空间坐标估计方法具有很高的研究意义与应用价值。本发明的目的是为了应对现有相机-IMU传感器在线标定方法存在的问题:(1)本发明通过李群代替四元数描述初始姿态矩阵,避免了传统四元数描述方法的非唯一性和非线性问题; (2)本发明将陀螺仪常值漂移与传感器空间坐标同时作为状态量,建立李群模型进行同步估计(3)本发明基于李群描述的相机-IMU空间坐标关系在线标定方法,能够有效避免现有四元数方法中由四元数向姿态矩阵转换产生的复杂表述问题和大量计算误差,并能够避免李群滤波方法中由于加性误差和计算误差产生的无法保证旋转矩阵正交性的问题。
为了达到上述目的,本发明提供如下技术方案:
基于李群滤波的相机与惯性传感器空间关系自标定方法,该方法通过下述步骤实现:
步骤(1):启动系统,计算系统所需要的输入值:
采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影wi;采用摄像头提供的图像进行FAST特征点的提取和LK 光流法追踪,并根据两帧图像间匹配的特征点计算出两帧间相机的旋转矩阵Rc;
角速度可以通过视觉信息的旋转差分获得:
由wc相机通过图像信息计算的角速度和由Rc相机通过图像信息计算的旋转。
步骤(2):基于步骤1计算的相机角速度和IMU角速度,建立基于李群描述的线性相机-IMU在线标定系统模型:
根据导航系统原理,传感器标定可以转换为姿态估计问题,姿态变换为两个坐标系之间的旋转变换,导航的姿态矩阵用一个3×3的正交变换矩阵表示;该正交变换矩阵符合李群的特殊正交群SO(n)的性质,构成了三维旋转群SO(3):
根据IMU陀螺仪性质其输出值可以分为:
根据IMU和相机两传感器其特性构建在线自标定模型:
根据在线标定模型,构建基于李群的自标定模型:
令
δα=nc
δβ=ni (5)
公式可以转化为:
β-δβ=R(α-δα) (6)
步骤(3):建立李代数空间上的等效状态无关滤波方程,并对状态量Rk进行最优估计:
由于相机-IMU空间坐标和陀螺仪常值漂移所构建的SO(4)矩阵Rk不随时间变化,所以有:
其中Rk表示k时刻的初始姿态矩阵。
根据误差状态卡尔曼滤波:
根据李群滤波原理,构造了关于李群的误差更新方程。
将δR替换为李代数形式exp([ξk]χ),将公式6替换为公式10:
其中ξk是ξk-1的后验估计。
根据公式11和指数性质,将等式两边的底数省略,等式两边的指数依然相等。
省略高阶无穷小:
通过比较公式12和公式9,可以将公式10的更新形式简化为公式8的更新形式,从而可以得出结论。式8和式10等价。
由于视觉里程表本身的局限性和在线标定周围环境的不确定性,无法保证标定过程中每一帧提供的信息都是有效的。因此,不能像传统的传感器δα那样从产品手册中获得噪声特性。同时,通过公式6可以发现,噪声δα与状态量有关,不能适应传统的AKF。
根据公式13,可以得到更新方程的表达式:
根据更新方程计算其协方差矩阵:
设[δα]χPk|k-1[δα]χ T-(δα)(δα)T是Pk|k-1的多项式函数f(Pk|k-1)。
根据公式13和多时间协方差矩阵P拟合,可以得出结论:
基于李群描述的相机-IMU空间坐标滤波估计方法归纳为:
误差重置:
δRk=I (19)
步骤(4):求解VIO系统所需的传感器坐标关系矩阵,从而VIO的初始化过程:
根据误差的协方差矩阵可以对旋转矩阵的三个轴对应的方差进行分解。通过实验可以发现,在输入数据正常的情况下,20s左右就可以基本计算出Camera-IMU的外部参数,三轴的方差基本收敛到相似的大小。然而,在标定过程中,有时无法保证传感器三个轴的运动激励满足标定的必要条件。因此,不可能保证三个轴同时收敛,导致旋转矩阵的计算不能达到后端优化所要求的精度。
因此,可以得出结论,当一个轴的方差明显大于其他轴的方差时,则可以证明该轴的运动激励不足、为估计系统提供的观测信息不准确,如果将该轴所对应的矩阵数据带入下一阶段来进行VIO系统的初始化的计算可能会导致初始化所计算的相机尺度,系统速度初始值和载体下的重力等估计不准确,因此可以增加方差较小的轴的比例,同时继续求解运动激励不足的轴。
与现有技术相比,本发明具有如下优点和有益效果:
(2)本发明将IMU的陀螺仪常值漂移作为状态量与坐标矩阵构建李群模型同步估计,提高了模型的准确性和估计结果的精度。
(3)本发明将李群模型下的状态相关噪声等效为李代数下的状态无关噪声,相比于传统的李群模型,建立了更为精确的李群线性对准模型,去除了李群滤波模型中状态相关噪声对估计结果的影响,提高对准精度。
(4)本发明采用的基于误差状态的IEKF的相机-IMU空间坐标在线标定方法,能够有效避免传统四元数卡尔曼滤波方法中由四元数向姿态矩阵转换产生的复杂表述问题和大量计算误差,并能够避免李群滤波方法中由于加性误差和计算误差产生的无法保证旋转矩阵正交性的问题。
附图说明
图1为本方法的实施简图。
图2在线标定结果图。
图3为SLAM结果图。
具体实施方式
本发明是基于李群最优估计的视觉惯性导航系统的传感空间坐标关系在线标定方法设计,下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述:
本发明提供的基于李群滤波相机-IMU空间坐标关系在线标定方法,首先获取传感器实时数据;对采集到的数据进行处理,基于传感器自身性质,建立基于李群描述的线性在线标定模型;使用李群滤波算法,估计得到基于李群描述的相加-IMU空间坐标关系矩阵和陀螺仪常值漂移,完成标定过程。
步骤(1):启动系统,计算系统所需要的输入值:
采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影wi;采用摄像头提供的图像进行FAST特征点的提取和LK 光流法追踪,并根据两帧图像间匹配的特征点计算出两帧间相机的旋转矩阵Rc;
角速度可以通过视觉信息的旋转差分获得:
由wc相机通过图像信息计算的角速度和由Rc相机通过图像信息计算的旋转。
步骤(2):基于步骤1计算的相机角速度和IMU角速度,建立基于李群描述的线性相机-IMU在线标定系统模型:
根据导航系统原理,传感器标定可以转换为姿态估计问题,姿态变换为两个坐标系之间的旋转变换,导航的姿态矩阵用一个3×3的正交变换矩阵表示;该正交变换矩阵符合李群的特殊正交群SO(n)的性质,构成了三维旋转群SO(3):
根据IMU陀螺仪性质其输出值可以分为:
根据IMU和相机两传感器其特性构建在线自标定模型:
根据在线标定模型,构建基于李群的自标定模型:
令
δα=nc
δβ=ni (24)
公式可以转化为:
β-δβ=R(α-δα) (25)
步骤(3):建立李代数空间上的等效状态无关滤波方程,并对状态量Rk进行最优估计:
由于相机-IMU空间坐标和陀螺仪常值漂移所构建的SO(4)矩阵Rk不随时间变化,所以有:
其中Rk表示k时刻的初始姿态矩阵。
根据误差状态卡尔曼滤波:
根据李群滤波原理,构造了关于李群的误差更新方程。
将δR替换为李代数形式exp([ξk]χ),将公式25替换为公式29:
其中ξk是ξk-1的后验估计。
根据公式30和指数性质,将等式两边的底数省略,等式两边的指数依然相等。
省略高阶无穷小:
通过比较公式31和公式28,可以将公式29的更新形式简化为公式27的更新形式,从而可以得出结论。式27和式29等价。
由于视觉里程表本身的局限性和在线标定周围环境的不确定性,无法保证标定过程中每一帧提供的信息都是有效的。因此,不能像传统的传感器δα那样从产品手册中获得噪声特性。同时,通过公式25可以发现,噪声δα与状态量有关,不能适应传统的AKF。
根据公式32,可以得到更新方程的表达式:
根据更新方程计算其协方差矩阵:
设[δα]χPk|k-1[δα]χ T-(δα)(δα)T是Pk|k-1的多项式函数f(Pk|k-1)。
根据公式32和多时间协方差矩阵P拟合,可以得出结论:
基于李群描述的相机-IMU空间坐标滤波估计方法归纳为:
误差重置:
δRk=I (38)
步骤(4):求解VIO系统所需的传感器坐标关系矩阵,从而VIO的初始化过程:根据误差的协方差矩阵可以对旋转矩阵的三个轴对应的方差进行分解。通过实验可以发现,在输入数据正常的情况下,20s左右就可以基本计算出Camera-IMU的外部参数,三轴的方差基本收敛到相似的大小。然而,在标定过程中,有时无法保证传感器三个轴的运动激励满足标定的必要条件。因此,不可能保证三个轴同时收敛,导致旋转矩阵的计算不能达到后端优化所要求的精度。
因此,可以得出结论,当一个轴的方差明显大于其他轴的方差时,则可以证明该轴的运动激励不足、为估计系统提供的观测信息不准确,如果将该轴所对应的矩阵数据带入下一阶段来进行VIO系统的初始化的计算可能会导致初始化所计算的相机尺度,系统速度初始值和载体下的重力等估计不准确,因此可以增加方差较小的轴的比例,同时继续求解运动激励不足的轴。
本发明的有益效果如下:
开放数据集EURC提供的图像信息可以在不改变光照和动态物体的情况下提取良好的角点,因此由图像计算的摄像机旋转和SVD分解提供的系统初始值非常准确。但是,对于在线定标任务,由于应用场景的不确定性,图像提供的观测信息的质量无法保证。当这些问题发生时,基于优化的外部参数估计方法与滤波相比会有很大的局限性,所以为了证明的算法在图像信息较差的情况下仍然可以有效地运行,选择了一个噪声较大的室外环境来测试该算法。
实施详情
为了验证复杂环境下的实验效果,搭建了由采集设备、计算设备和验证设备组成的实验平台。
RealSense D435i相机,在本实验中用于捕捉图像和IMU。差分GPS模块,分为地面站和移动终端两部分。为实验提供了全套设备的实时位置信息,验证了该方法和VINS在线标定方法对VIO系统的影响。显示便携式PC平台通过记录 ROS下的袋子来存储传感器的实时串行数据,包括时间、图像和IMU数据,以及差分GPS。
由于光照变化和动态物体在实际应用中的必然性,人们认识到光照变化和动态物体是影响视觉SLAM精度的两个最大因素,因此相机-IMU的在线标定问题也应考虑在内。在本实验中,为了验证算法在系统初值和观测信息不能得到保证的情况下仍然有效,实验地点选择在绿树成荫的校园道路上。
外部参照比较:
为了给实验提供对比,首先使用了苏黎世联邦理工学院(ETHZ)开发的最广泛使用的Kalibr相机和IMU离线校准方法,对传感器的内外参数进行校准,为实验提供真实值。然后运行带外部参数校准的VINS系统,输出计算的外部参数结果,为实验提供对比度。
将本算法计算的Camera-IMU的旋转矩阵与VINS计算的同时与工具箱计算的旋转矩阵进行比较。对收敛后参数进行了分析,对比结果和时间如图2和表 1 所示:
表 1 .
姿态角误差与对准时间的比较
如图2所示,虽然蓝线代表的改进IEKF外部参数估计误差的收敛速度慢于开放数据集,但仍能在30秒左右收敛到合理范围。从表 1 可以看出,在复杂的室外环境下,考虑陀螺恒定漂移的IEKF相机-IMU外部参数在线标定方法仍能准确估计传感器外部参数,精度小于0.5度。VINS提供的方法收敛到错误的区间,这可能是由于对光照变化的非线性初值计算错误所致。
SLAM比较:
在光照变化明显的林道上,将本文算法和VINS计算的摄像机IMU旋转引入VINS系统,将系统的位置和姿态信息与差分GPS输出的实际位置值进行比较,并比较了不同外部参数估计方法对VO系统的影响。VIO的输出如图3和表 2 所示。
表 2 .
slam结果误差统计
从图3和表 2 中的VIO误差统计特征可以看出,在复杂的室外照明环境下,图像观测信息的准确性得不到保证,的算法仍然可以为VIO系统提供良好的摄像机IMU外部参数,从而获得更准确的VIO结果。
本发明采用李群描述代替传统四元数描述实现对相机-IMU空间坐标关系的在线计算,利用李群微分方程建立基于李群描述的线性标定滤波模型。从而实现了相机-IMU空间坐标关系和陀螺仪常值漂移的同步估计,并避免了传统四元数描述初始姿态矩阵而产生的非唯一性和非线性问题;本发明利用李群的左乘不变性、误差状态卡尔曼以及李群与李代数之间的指数映射对状态相关误差进行等效变换,建立等效的状态无关李代数滤波方程,避免了状态相关噪声对滤波结果的影响,有效提高了对准精度。
以上所述仅是本发明的优选实施方式,并不用于限制本发明。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和变型,这些改进和变型也应视为本发明的保护范围。
Claims (1)
1.基于李群滤波的相机与惯性传感器空间关系自标定方法,其特征在于:该方法通过下述步骤实现:
步骤(1):启动系统,计算系统所需要的输入值:
采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影wi;采用摄像头提供的图像进行FAST特征点的提取和LK光流法追踪,并根据两帧图像间匹配的特征点计算出两帧间相机的旋转矩阵Rc;
角速度通过视觉信息的旋转差分获得:
步骤(2):基于步骤1计算的相机角速度和IMU角速度,建立基于李群描述的线性相机-IMU在线标定系统模型:
根据导航系统原理,传感器标定转换为姿态估计问题,姿态变换为两个坐标系之间的旋转变换,导航的姿态矩阵用一个3×3的正交变换矩阵表示;该正交变换矩阵符合李群的特殊正交群SO(n)的性质,构成了三维旋转群SO(3):
根据IMU陀螺仪性质其输出值分为:
根据IMU和相机两传感器其特性构建在线自标定模型:
根据在线标定模型,构建基于李群的自标定模型:
令
δα=nc
δβ=ni (5)
公式转化为:
β-δβ=R(α-δα) (6)
步骤(3):建立李代数空间上的等效状态无关滤波方程,并对状态量Rk进行最优估计:
由于相机-IMU空间坐标和陀螺仪常值漂移所构建的SO(4)矩阵Rk不随时间变化,所以有:
其中Rk表示k时刻的初始姿态矩阵;
根据误差状态卡尔曼滤波:
根据李群滤波原理,构造了关于李群的误差更新方程;
将δR替换为李代数形式exp([ξk]χ),将公式6替换为公式10:
其中ξk是ξk-1的后验估计;
根据公式11和指数性质,将等式两边的底数省略,等式两边的指数依然相等;
省略高阶无穷小:
根据公式13,得到更新方程的表达式:
根据更新方程计算其协方差矩阵:
设[δα]χPk|k-1[δα]χ T-(δα)(δα)T是Pk|k-1的多项式函数f(Pk|k-1);
根据公式13和多时间协方差矩阵P拟合,得出结论:
基于李群描述的相机-IMU空间坐标滤波估计方法归纳为:
误差重置:
δRk=I (19)
步骤(4):求解VIO系统所需的传感器坐标关系矩阵,从而VIO的初始化过程:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110273084.6A CN113155152B (zh) | 2021-03-14 | 2021-03-14 | 基于李群滤波的相机与惯性传感器空间关系自标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110273084.6A CN113155152B (zh) | 2021-03-14 | 2021-03-14 | 基于李群滤波的相机与惯性传感器空间关系自标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113155152A CN113155152A (zh) | 2021-07-23 |
CN113155152B true CN113155152B (zh) | 2023-01-03 |
Family
ID=76886939
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110273084.6A Active CN113155152B (zh) | 2021-03-14 | 2021-03-14 | 基于李群滤波的相机与惯性传感器空间关系自标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113155152B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113624254A (zh) * | 2021-07-26 | 2021-11-09 | 中国科学院上海微系统与信息技术研究所 | 一种图像传感器和陀螺仪的时间同步标定装置及方法 |
CN114329943B (zh) * | 2021-12-23 | 2023-01-24 | 哈尔滨工业大学(深圳) | 基于姿态旋转矩阵的控制性能边界设计方法、装置及介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107564061A (zh) * | 2017-08-11 | 2018-01-09 | 浙江大学 | 一种基于图像梯度联合优化的双目视觉里程计算方法 |
CN109741403A (zh) * | 2018-12-29 | 2019-05-10 | 重庆邮电大学 | 一种基于全局线性的相机平移标定方法 |
CN110610513A (zh) * | 2019-09-18 | 2019-12-24 | 郑州轻工业学院 | 自主移动机器人视觉slam的不变性中心差分滤波器方法 |
CN110702143A (zh) * | 2019-10-19 | 2020-01-17 | 北京工业大学 | 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法 |
CN111681279A (zh) * | 2020-04-17 | 2020-09-18 | 东南大学 | 基于改进李群非线性优化的行车吊臂空间位姿测量方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10254118B2 (en) * | 2013-02-21 | 2019-04-09 | Regents Of The University Of Minnesota | Extrinsic parameter calibration of a vision-aided inertial navigation system |
-
2021
- 2021-03-14 CN CN202110273084.6A patent/CN113155152B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107564061A (zh) * | 2017-08-11 | 2018-01-09 | 浙江大学 | 一种基于图像梯度联合优化的双目视觉里程计算方法 |
CN109741403A (zh) * | 2018-12-29 | 2019-05-10 | 重庆邮电大学 | 一种基于全局线性的相机平移标定方法 |
CN110610513A (zh) * | 2019-09-18 | 2019-12-24 | 郑州轻工业学院 | 自主移动机器人视觉slam的不变性中心差分滤波器方法 |
CN110702143A (zh) * | 2019-10-19 | 2020-01-17 | 北京工业大学 | 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法 |
CN111681279A (zh) * | 2020-04-17 | 2020-09-18 | 东南大学 | 基于改进李群非线性优化的行车吊臂空间位姿测量方法 |
Non-Patent Citations (1)
Title |
---|
热像仪-RGB相机-IMU传感器的空间联合标定方法;王鑫等;《仪器仪表学报》;20201130;第41卷(第11期);第216-223页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113155152A (zh) | 2021-07-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Cioffi et al. | Tightly-coupled fusion of global positional measurements in optimization-based visual-inertial odometry | |
CN110375738B (zh) | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 | |
Zuo et al. | Lic-fusion: Lidar-inertial-camera odometry | |
Brossard et al. | Unscented Kalman filter on Lie groups for visual inertial odometry | |
CN111795686B (zh) | 一种移动机器人定位与建图的方法 | |
Lupton et al. | Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions | |
CN107516326B (zh) | 融合单目视觉和编码器信息的机器人定位方法和系统 | |
Weiss et al. | Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments | |
CN107941217B (zh) | 一种机器人定位方法、电子设备、存储介质、装置 | |
CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
CN112815939B (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
CN107796391A (zh) | 一种捷联惯性导航系统/视觉里程计组合导航方法 | |
CN111707261A (zh) | 一种微型无人机高速感知和定位方法 | |
CN113155152B (zh) | 基于李群滤波的相机与惯性传感器空间关系自标定方法 | |
Zhang et al. | Vision-aided localization for ground robots | |
He et al. | Adaptive error-state Kalman filter for attitude determination on a moving platform | |
Tang et al. | LE-VINS: A robust solid-state-LiDAR-enhanced visual-inertial navigation system for low-speed robots | |
CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
CN114397642A (zh) | 一种基于图优化的三维激光雷达与imu外参标定方法 | |
CN112179373A (zh) | 一种视觉里程计的测量方法及视觉里程计 | |
CN115930959A (zh) | 视觉初始化方法、装置及飞行汽车 | |
CN115014346A (zh) | 一种面向视觉惯性定位的基于地图的一致高效滤波算法 | |
Wu et al. | AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry | |
Fu et al. | Semantic Map-based Visual Localization with Consistency Guarantee |
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 |