CN117073720A - 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备 - Google Patents
弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备 Download PDFInfo
- Publication number
- CN117073720A CN117073720A CN202310982450.4A CN202310982450A CN117073720A CN 117073720 A CN117073720 A CN 117073720A CN 202310982450 A CN202310982450 A CN 202310982450A CN 117073720 A CN117073720 A CN 117073720A
- Authority
- CN
- China
- Prior art keywords
- imu
- information
- visual
- camera
- rotation
- 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.)
- Pending
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 115
- 238000000034 method Methods 0.000 title claims abstract description 77
- 230000009471 action Effects 0.000 title claims abstract description 16
- 238000013519 translation Methods 0.000 claims abstract description 34
- 238000005457 optimization Methods 0.000 claims abstract description 21
- 230000033001 locomotion Effects 0.000 claims abstract description 18
- 230000010354 integration Effects 0.000 claims description 52
- 238000006073 displacement reaction Methods 0.000 claims description 34
- 238000005259 measurement Methods 0.000 claims description 33
- 239000011159 matrix material Substances 0.000 claims description 29
- 230000005484 gravity Effects 0.000 claims description 28
- 238000005070 sampling Methods 0.000 claims description 26
- 230000001133 acceleration Effects 0.000 claims description 14
- 230000008859 change Effects 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 10
- 230000000694 effects Effects 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 6
- 238000011084 recovery Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000009977 dual effect Effects 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 238000011423 initialization method Methods 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- 238000004378 air conditioning Methods 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 abstract description 20
- 238000010276 construction Methods 0.000 abstract description 2
- 230000001105 regulatory effect Effects 0.000 abstract description 2
- 230000004927 fusion Effects 0.000 description 5
- 230000008569 process Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 1
- 238000007792 addition Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- 230000009012 visual motion Effects 0.000 description 1
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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
本发明涉及弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备。针对相机惯性系统级标定依赖特定标定物物理特征的问题以及需要规定运动来采集数据限制，以视觉惯性传感器空间运动相对一致性为基础构建了初始对准‑精确标定的双重图优化结构，实现一种普适性较高、不依赖用户干预的相机惯性在线标定方法。针对现有视觉惯性初始化算法忽略平移外参对初始状态估计贡献的问题，在估计初始状态前将平移外参与其他初始状态参数进行解耦、计算并补偿进后续的初始化中。因为更完整初始化模型的构建，获取了更好的初始化结果。因此，本发明可以实现高精度的视觉惯性导航系统初始状态的估计，为后续系统持续、稳定、高精度的导航定位服务提供支持。
Description
技术领域
本发明属于视觉惯性标定领域，涉及一种弱环境控制与弱运动控制下的快速单目视觉惯性标定与初始化算法。
背景技术
现代组合导航存在传感器、平台、应用多样化的趋势，视觉传感器作为一种能提供载体周边纹理信息的传感器具有很强的功能性，惯性传感器可以全天候工作，即使是低精度的IMU也可以在短时间内为载体提供可靠的刚体运动量测信息。因为视觉与惯性传感器工作特点与数据特性，两者是存在高度互补的并且常用于进行数据融合来获取更好的导航效果。并且两部分数据融合的基础是获取视觉与惯性传感器之间的空间关系(即两者的外参)，外参的精度很大程度上决定了两部分数据融合的效果进而影响导航结果的精度。
常用的离线标定方法通常需要借助标定物来提供一个可靠的外部控制，进而获取标定数据。通常该操作需要小心的将捆绑的视觉惯性传感器移动至标定物前采集数据，这些需求在用户使用时通常是无法满足的。尽管在线标定弥补了离线标定的不足，但是现有的在线标定算法通常需要足量且一定规则的运动来采集数据，对于外部动作控制有较强的依赖增加某些场景下的使用难度。
现阶段在线标定算法通常是与导航系统的初始化紧密耦合的，较好的初始状态估计可以避免系统陷入滤波器或优化器陷入局部最优解，进而影响系统的导航结果。然而现阶段的系统初始化算法中通常会将平移置零，忽略平移外参对于初始状态参数估计的贡献，这在视觉惯性传感器空间位移较小地情况下是可以接受的，比如微型无人机等载体，自身体积较小对传感器的间距有一个硬件约束。但是在无人车等载具的实验场景下当传感器之间的空间位移较大，对初始状态估计产生的误差难以控制。
因此，提出一种弱环境控制与弱运动控制下的快速单目视觉惯性标定与初始化算法，是本领域急需解决的难题。
发明内容
本发明主要是解决现有技术所存在的相机惯性系统级标定对于特定标定物物理特征的依赖以及打破需要规定运动来采集数据限制，提供了一种普适性较高、不依赖用户干预的相机惯性在线标定方法。
本发明还有一目的是解决现有视觉惯性初始化算法忽略了平移外参对初始状态估计贡献的问题，提出了一种改进的视觉惯性系统初始化算法。
本发明的上述技术问题主要是通过下述技术方案得以解决的：弱环境与弱动作控制下快速视觉惯性标定与初始化方法，包括如下步骤：
步骤1，惯性数据处理：构建惯性测量单元IMU预积分模型，用于获取连续历元之间的相对位置、速度、姿态的增量信息，同时构建预积分的误差传播模型，用于获取连续历元之间状态量误差之间的线性关系，进而获取位置、速度、姿态误差关于陀螺零偏与加速度计零偏的雅各比矩阵；
步骤2，视觉数据的处理：对于获取到的视觉数据，首先使用描述子来提取特征点信息，使用光流法进行追踪并对追踪到的特征点进行误差点的剔除；
步骤3，弱动作控制的双重优化器：基于步骤1与步骤2获取到的惯性部分与视觉部分的位姿解算结果以空间运动一致性为基础构建双重优化器进行旋转外参的解算，进而获取视觉惯性的旋转外参与陀螺零偏；
步骤4，视觉惯性系统的初始化：在步骤3中获取到了视觉与惯性传感器之间的旋转外参与惯性传感器的陀螺零偏，采用无绝对速度的平移外参估计器降低初始化中绝对速度的影响，包括如下子步骤：
步骤4.1，无绝对速度的平移外参估计器，首先构建视觉惯性导航系统的滑窗，通过惯性部分与视觉部分的位姿解算结果构建位置约束方程，根据滑窗内连续三帧的位置约束方程推算获取无绝对速度的唯一约束方程，在方程中待估参数只有重力、尺度以及平移外参，构建优化器获取参数解；
步骤4.2，通过步骤4.1获取了滑窗内的部分初始状态如重力、尺度以及平移外参，依据滑窗内通过惯性部分与视觉部分的解算结果构建位置与速度约束方程，对滑窗内进行全局的优化，获取完整的初始状态量，至此完成视觉惯性系统的初始化。
进一步的，步骤1中，IMU的采样频率高于相机，这意味着相机的采样周期内存在多个IMU的采样周期，设置存在一相机采样周期[i,j]，在i时刻与j时刻之间存在一IMU采样周期[k,k+1]，则惯性测量单元IMU预积分模型表示为：
其中与/>分别代表了i时刻至k和k+1时刻IMU的相对位移，/>与/>分别代表了i时刻至k和k+1时刻IMU的姿态，/>与/>分别代表了i时刻至k和k+1时刻IMU的相对速度，δt为k与k+1时刻的时差，a为加速度，ω为角速度。
进一步的，采用中值定理计算加速度a和角速度ω；
其中为k与k+1时刻陀螺仪离散采样的量测角速度信息，/>为k与k+1时刻加速度计离散采样的量测加速度信息，/>分别代表了IMU从i时刻至k和k+1时刻姿态变化的旋转矩阵，且在同一采样周期内IMU零偏常被认为是不变化的，/>则分别代表了k时刻的加速度计与陀螺零偏。
进一步的，步骤1中，考虑加速度计零偏误差δba以及陀螺零偏误差δbg对IMU预积分的影响，则IMU预积分模型表示如下；
其中分别代表了i时刻的加速度计零偏的误差，/>分别为i时刻至j时刻相机关键帧之间的误差修正后的相对位移、速度、姿态，/>分别为i时刻至j时刻相机关键帧之间的误差修正前的相对位移、速度、姿态，/>分别为相对位置与相对速度的误差关于加速度误差的雅各比矩阵，/>分别为相对位置、相对旋转以及相对速度的误差关于角速度误差的雅各比矩阵，/>表示四元数的乘法；
在进行外参标定时，将加速度计零偏ba和陀螺零偏bg置零，并使用公式(3)代替原有预积分模型考虑两者对IMU预积分的影响，进而通过对δba和δba的估计实现对零偏ba和bg的估计，接着再次使用公式(3)对现有IMU预积分进行补偿获取更精确的IMU预积分结果；
公式(3)中的雅各比矩阵从IMU预积分的误差传播模型获取，根据误差状态卡尔曼滤波的原理，获取IMU预积分误差传播模型：
其中分别代表了IMU从i时刻至k与k+1时刻的相对位移、姿态、速度的误差，/>分别代表了k与k+1时刻的加速度计零偏的误差，分别代表了k与k+1时刻的陀螺零偏误差；nak、ngk为k时刻IMU的随机白噪声，为k+1时刻IMU的随机白噪声，/>为k时刻IMU零偏的随机游走噪声。Fk为k+1时刻IMU预积分状态误差对于k时刻IMU预积分状态误差的一阶雅各比矩阵，Gk为k+1时刻IMU预积分状态误差对于k与k+1时刻IMU离散噪声的一阶雅各比矩阵，公式(3)中预积分速度、位置、姿态误差关于IMU零偏误差的雅各比矩阵/> 可以从Fk矩阵中获取，至此完成IMU数据的处理。
进一步的，步骤2中对于获取到的视觉数据，首先利用对极几何获取视觉姿态信息，然后以对极几何的位姿结果为状态量，同时对图像中的共视特征点进行三角化作为量测信息进行约束；最后使用全局的光束法平差优化来最小化滑窗内特征的重投影误差获取精确的所有相机帧位姿信息，供外参的精确优化使用。
进一步的，步骤3中对于同一时空的相机与IMU姿态建立以下的约束：
其中为i时刻至j时刻IMU的相对姿态，/>为相机求解的视觉旋转信息，/>为相机与IMU之间的旋转外参；
首先使用对极几何获取的视觉姿态信息与IMU预积分，以公式(5)为约束构建第一层优化器，进行旋转约束获取粗略的外参标定结果；
接着使用步骤2中光束法平差的视觉姿态信息与IMU预积分，以公式(5)为约束构建第二层优化器，在第二层优化器中使用第一层的标定结果为初值进行优化求解，获取最终标定结果。
进一步的，旋转约束的优化部分如下所示，
其中χq为优化参数的集合，为滑窗内的陀螺常值零偏；
由步骤1中的预积分推算方程，真实的预积分旋转表示如下：
其中Jt是预积分旋转误差δγ关于陀螺零偏的一阶雅可比矩阵，且在优化前将陀螺零偏/>的初值设置为零；
基于上述公式，旋转约束的误差方程规定如下：
上式中Zq部分包含了和Jt+1，该部分由IMU的测量信息获取，/>为相机的视觉旋转信息，粗标定部分由对极几何约束获取，精确标定部分由视觉重建获取；在考虑了所有的匹配的相机与IMU旋转对之后其代价函数为：
在此处使用四元数表示的空间姿态变化，tc表示标定数据的时间序列，(.)xyz表示姿态四元数的虚部，wq为降低异常值影响的权重信息其中a为规定的阈值信息，δc为相机的视觉旋转信息与经由本次迭代的旋转外参由IMU预积分旋转信息恢复的相机旋转量的差值，通过对代价函数的求解，获取视觉惯性的旋转外参/>与陀螺零偏/>在获取的精确的陀螺零偏/>后，将IMU的预积分信息进行更新：
进一步的，步骤4.1的具体实现方式如下；
在初始化之前，将滑窗内首个关键帧的相机坐标系定义为临时的参考坐标系，并定义为C0，在完成该帧的重力量测值的估计后将临时参考坐标系与世界坐标系对齐，在步骤3中获取视觉惯性的旋转外参/>与陀螺零偏/>并使用陀螺零偏/>对现有的预积分信息进行了补偿，同时因为旋转外参/>的获取，将滑窗内由步骤2获取的以C0系为参考的相机姿态/>转换为IMU姿态/>但由于纯视觉位姿解算中尺度信息s未知，所以需要对尺度信息s进行估计，用于恢复滑窗内的位置状态信息/>同时对滑窗内各帧的绝对速度信息/>进行估计，为了使视觉与惯性数据进行融合，视觉惯性的初始化需要对以下状态量进行估计：
其中s为IMU量测位移与视觉恢复位移的比例因子，即尺度信息，为第n个滑窗中对应的速度信息，/>为参考相机坐标系下的重力向量；
通过滑窗内相邻关键帧之间的IMU预积分位置信息与速度信息建立残差方程：
其中分别为t时刻至t+1时刻的IMU预积分相对位移、速度，/>为滑窗内首个相机关键帧至t时刻相机关键帧的位置，但由于其为纯视觉结算结果，该位置尺度未知需要比例因子s恢复真实尺度，/>为滑窗内首个相机关键帧中的重力量测值，/>为t时刻IMU至首个相机关键帧姿态变化的旋转矩阵，/>为首个相机关键帧至t+1时刻IMU姿态变化的旋转矩阵，/>为IMU与相机之间的平移外参，/>为t时刻IMU的绝对速度，/>为t+1时刻IMU的绝对速度，δtt为t与t+1时刻的时差；
公式12的矩阵形式如下：
然后采用无绝对速度的平移外参估计器进行求解：
平移外参估计器中的约束方程通过相邻关键帧之间的预积分已知的相对速度信息代替滑窗内未知的绝对速度信息，12式中的位置约束方程可以转换为如下形式：
在连续三帧的数据里，将关键帧数据t、t+1、t+2描述为1、2、3，由14式可以获取相邻帧之间的关系：
由12式中预积分的速度约束和15式可以获取无速度估计的约束方程：
至此可以在规避了对绝对速度信息的情况下对χp参数进行估计：
位置约束的误差方程规定如下：
上式中Zp部分包含了和/> 由IMU的测量信息获取，/>为相机的视觉重建获取的无尺度位移信息，Zv包括/>表示IMU的预积分的速度测量信息，Zr包括/>表示相机的视觉旋转信息；
通过在初始化前期所有的配对的相机与IMU位置后，其代价函数为：
通过对代价函数的求解获取χp结果，即IMU量测位移与视觉恢复位移的比例因子s，滑窗内首个相机关键帧中的重力量测值平移外参/>
进一步的，步骤4.2的具体实现方式如下；
通过步骤4.1获取了滑窗内的部分初始状态，包括重力、尺度以及平移外参，以公式17中的参数χp作为先验信息，由公式12构建位置与速度的约束方程对公式11中完整的初始状态χI进行估计，获取最终的重力、尺度滑窗内的速度量以及平移外参，最后，将滑窗内的首个关键帧至t时刻相机的位移速度/>姿态/>状态量依据临时参考系C0与当地重力的旋转变化对齐至世界坐标系W下，完成初始化。
另一方面，本发明还提供一种弱环境与弱动作控制下快速视觉惯性标定与初始化设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如上述技术方案所述的方法。
本发明可以让视觉惯性导航系统在传感器空间关系未知且不借助外部环境控制以及外部动作控制的情况下完成外参标定，并进行初始状态量的估计完成初始化，提高的视觉惯性导航的适用性,降低了用户的干预。对该视觉惯性导航的发展具有积极的推动作用。
附图说明
图1为本发明的整体流程图；
图2为本发明实施例中对极几何约束算法的原理示意图；
图3为本发明实施例中运动重建视觉算法的整体流程图；
图4为本发明实施例标定结果的可视化图；
图5为本发明实施例计算过程耗时对比图；
图6为本发明实施例不同算法初始化轨迹对比图；
图7为本发明实施例不同算法位置误差对比图；
图8为本发明实施例不同算法尺度因子对比图。
具体实施方式
下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
如图1所示，本发明提供了一种弱环境与弱动作控制下快速视觉惯性标定与初始化方法，包括以下步骤：
步骤1，惯性数据处理：构建IMU(惯性测量单元)预积分模型，用于获取连续历元之间的相对位置、速度、姿态的增量信息，但是每一个历元下积分出来的值是存在误差的。因此需要同时构建预积分误差传播模型，该模型的主要作用是获取连续历元之间状态量误差之间的线性关系，进而获取位置、速度、姿态误差关于陀螺零偏与加速度计零偏的雅各比矩阵。该步骤主要用于给后续标定算法与初始化算法提供惯性量测信息。
步骤2，视觉数据的处理：对于获取到的视觉数据，首先使用描述子来提取足够的特征点，在首次获取到足够的特征点信息后，使用光流法进行追踪并对追踪到的特征点进行误差点的剔除。因为特征点追踪无法避免会丢弃一些特征点信息，所以对追踪到的图像帧需要使用描述子进行特征提取以获取足够的特征点。在获取了特征点信息后进而使用不同的视觉导航算法解算出视觉位姿信息，参见步骤2.1和步骤2.2。
步骤2.1，对极几何约束的位姿解算是一种常用的由相邻图像信息获取相对位姿的算法，在此步骤中使用步骤2中获取到的相邻图像中匹配到的特征点对解算获取视觉的位姿信息。
步骤2.2，虽然对极几何可以获取的视觉位姿结果，但是其精度相对较差。为了获取高精度的视觉位姿，基于滑动窗口实现视觉的运动重建以及全局优化。在确定了滑窗内的首个关键帧后，选择与该帧具有足够视差的一帧进行相对位姿的解算并对两帧的所有特征点信息进行三角化作为路标点信息供后续优化使用，且该路标点信息没有真实尺度。基于前期获取到的路标点信息，使用Perspective-n-Point(多点透视成像)方法估计窗口中其他帧的姿态，同时不断三角化新的路标点信息提供更多的约束。最后，使用全局的图优化来最小化滑窗内特征的重投影误差获取精确的所有相机帧位姿信息。
步骤3，弱动作控制的双重优化器：基于步骤1与步骤2获取到的惯性部分与视觉部分的位姿解算结果以空间运动一致性为基础构建双重优化器进行旋转外参的解算。
步骤3.1，在第一层优化器中使用步骤2.1中对极几何获取的视觉位姿信息与惯性位姿信息构建约束方程进行旋转约束获取粗略的外参标定值。由于对极几何约束的本质是2D-2D图像之间相对空间关系的求解其鲁棒性较差，视觉的位姿解算结果会存在一些错误的信息，但是在粗标定阶段忽略这些错误的状态。因为该步骤的主要目的是为在下一层优化器提供旋转外参与陀螺零偏的初始解，避免陷入局部最优解。在该层优化器中，以图像帧之间累积的旋转量为收敛指标之一。通过对于累积的旋转量来控制第一层优化器的收敛和第一层标定结果的精度，进而间接影响最终的标定结果。
步骤3.2，在第二层精确优化器中，使用步骤2.2中全局光束法平差优化获取到的视觉位姿信息与惯性部分位姿结果进行约束实现精确标定结果的获取，且该优化器的初始参数以步骤3.1为先验值。
步骤4，视觉惯性系统的初始化：在步骤3中获取到了视觉与惯性传感器之间的旋转外参与惯性传感器的陀螺零偏，但是平移外参与滑窗内其余的初始状态量紧密耦合，滑窗内其余的初始状态量包括重力、尺度以及滑窗内的绝对速度。由于待估参数中绝对速度占据多数，采用常规的初始化方法会导致平移外参的估计误差较大，因此本发明设计了无绝对速度的平移外参估计器降低初始化中绝对速度的影响参见步骤4.1。
步骤4.1，无绝对速度的平移外参估计器，首先构建视觉惯性导航系统的滑窗。通过惯性部分与视觉部分的位姿解算结果构建位置约束方程，根据滑窗内连续三帧的位置约束方程推算获取无绝对速度的唯一约束方程，在该方程中待估参数只有重力、尺度以及平移外参，构建优化器获取参数解。
步骤4.2，通过步骤4.1获取了滑窗内的部分初始状态如重力、尺度以及平移外参。依据滑窗内通过惯性部分与视觉部分的解算结果构建位置与速度约束方程，对滑窗内进行全局的优化，获取完整的初始状态量，至此完成视觉惯性系统的初始化。
本发明中，主要分为标定部分以及初始化部分，前者为是后者进行的基础，也决定了后者估计的精度。后者为系统提供了良好的初值，也反映了前者精度对于数据融合效果的影响。
下面通过具体的实施例对每个步骤的具体实现进行详细的说明：
步骤1，构建IMU预积分及误差传播模型
捷联惯导的位姿信息根据加速度计与陀螺仪的量测信息计算获取，本发明使用IMU预积分来处理加速度计与陀螺仪的数据。IMU预积分也可以按照离散数据和连续数据有不同的表达形式，此处使用离散数据来表示相邻关键帧之间的位置、速度、姿态的增量信息。
通常IMU的采样频率要高于相机，这意味着相机的采样周期内存在多个IMU的采样周期。假设存在一相机采样周期[i,j],在i时刻与j时刻之间存在一IMU采样周期[k,k+1]，则IMU预积分模型可以表示为：
其中与/>分别代表了i时刻至k和k+1时刻IMU的相对位移，/>与/>分别代表了i时刻至k和k+1时刻IMU的姿态(四元数形式)，/>与/>分别代表了i时刻至k和k+1时刻IMU的相对速度，δt为k与k+1时刻的时差(即IMU的采样周期)。因为使用的是IMU的离散数据，所以采用中值定理计算加速度a和角速度ω：
其中为k与k+1时刻陀螺仪离散采样的量测角速度信息，/>为k与k+1时刻加速度计离散采样的量测加速度信息，/>分别代表了IMU从i时刻至k和k+1时刻姿态变化的旋转矩阵，且在同一采样周期内IMU零偏常被认为是不变化的，/>则分别代表了k时刻的加速度计与陀螺零偏。在获取了k,k+1时刻的IMU预积分信息后，可以使用连续传播的方式计算获取与i,j时刻相邻相机关键帧匹配的预积分量测值。
但是上述IMU预积分模型也存在误差。一般情况下认为零偏是伴随着时间产生变化的，因此会考虑加速度计零偏误差δba以及陀螺零偏误差δbg对IMU预积分的影响，以相机的采样周期为例：
其中分别代表了i时刻的加速度计零偏的误差，/>分别为i时刻至j时刻相机关键帧之间的误差修正后的相对位移、速度、姿态，/>分别为i时刻至j时刻相机关键帧之间的误差修正前的相对位移、速度、姿态，/>分别为相对位置与相对速度的误差关于加速度误差的雅各比矩阵，/>分别为相对位置、相对旋转以及相对速度的误差关于角速度误差的雅各比矩阵，/>表示四元数的乘法。
在进行外参标定时，通常会将加速度计零偏ba和陀螺零偏bg置零，并使用公式(3)代替原有预积分模型考虑两者对IMU预积分的影响，进而通过对δba和δba的估计实现对零偏ba和bg的估计。接着再次使用公式(3)对现有IMU预积分进行补偿获取更精确的IMU预积分结果。
公式(3)中的雅各比矩阵可以从IMU预积分的误差传播模型获取。根据误差状态卡尔曼滤波的原理，可以获取IMU预积分误差传播模型：
其中分别代表了IMU从i时刻至k与k+1时刻的相对位移、姿态、速度的误差，/>分别代表了k与k+1时刻的加速度计零偏的误差，分别代表了k与k+1时刻的陀螺零偏误差。下面是k与k+1时刻IMU离散噪声的介绍，/>为k时刻IMU的随机白噪声，/>为k+1时刻IMU的随机白噪声，/>为k时刻IMU零偏的随机游走噪声。Fk为k+1时刻IMU预积分状态误差对于k时刻IMU预积分状态误差的一阶雅各比矩阵，Gk为k+1时刻IMU预积分状态误差对于k与k+1时刻IMU离散噪声的一阶雅各比矩阵。公式(3)中预积分速度、位置、姿态误差关于IMU零偏误差的雅各比矩阵可以从Fk矩阵中获取，至此完成IMU数据的处理。
步骤2，视觉导航算法
在获取到的视觉数据后需要进行预处理获取特征点坐标，首先使用描述子来提取足够的特征点，在首次获取到足够的特征点信息后，使用光流法进行追踪并对追踪到的特征点进行误差点的剔除。因为特征点追踪无法避免会丢弃一些特征点信息，所以对追踪到的图像帧需要使用描述子进行特征提取以获取足够的特征点，至此完成视觉数据的预处理。
对极几何本质上是指“两幅图像之间的对极几何”。即在两张图像存在共视特征时，两个时刻的相机的光心、像平面中的像点以及对应的空间物点共计五个点时处于同一平面，通过五点平面的约束构建方程获取两幅图像之间的位姿变换。
为了获取更好的位姿结果，以对极几何的位姿结果为状态量，同时对图像中的共视特征点进行三角化作为量测信息进行约束，具体操作如下：在确定了滑窗内的首个关键帧后，选择与该帧具有足够视差的一帧进行相对位姿的解算并对两帧的所有特征点信息进行三角化作为路标点信息供后续优化使用，且该路标点信息没有真实尺度。基于前期获取到的路标点信息，使用Perspective-n-Point(多点透视成像)方法估计窗口中其他帧的姿态，同时不断三角化新的路标点信息提供更多的约束。
最后，使用全局的光束法平差优化来最小化滑窗内特征的重投影误差获取精确的所有相机帧位姿信息，供外参的精确优化使用。其中滑窗内的位姿结果都以滑窗内第一帧相机坐标系C0为参考系，即滑窗内第t帧姿态为位置为/>
步骤3，弱动作控制的双重优化器设计
在步骤1以及步骤2分别获取到了IMU的位姿信息以及相机对应的位姿信息。与位移信息不同的是，由于旋转不存在尺度未知的信息，对于同一时空的相机与IMU姿态可以建立以下的约束：
其中为i时刻至j时刻IMU的相对姿态，/>为相机求解的视觉旋转信息，/>为相机与IMU之间的旋转外参。
与现有标定算法只使用单一视觉位姿结果与IMU预积分约束直接求解不同的是，首先使用对极几何获取的视觉姿态信息与IMU预积分，以公式(5)为约束构建第一层优化器，进行旋转约束获取粗略的外参标定结果。由于对极几何的本质信息是2D-2D图像之间相对信息的求解，所以其鲁棒性较差，视觉部分势必会存在一些错误的信息，但是在粗标定阶段这些错误的状态可以被忽略。
接着使用步骤2中光束法平差的视觉姿态信息与IMU预积分，以公式(5)为约束构建第二层优化器。在第二层优化器中使用第一层的标定结果为初值进行优化求解，获取最终标定结果。
旋转约束的优化部分如下所示(第一层优化器和第二层优化器的优化约束方程是一样的，只是使用的视觉结果和初值不一样)，需要注意的是在此处，优化的参数不仅仅包含的旋转外参同样包括了在滑窗内的陀螺常值零偏/>即使在粗标定的阶段依然会对陀螺零偏进行一个粗略估值。该做法不仅仅是为了为精确标定时提供一个较好的初始估计值，也因为考虑了IMU旋转误差会对外参结果有一个更好的初始估计，该部分的优化参数如下：
其中χq为优化参数的集合。
由步骤1中的预积分推算方程，真实的预积分旋转可以被如下表示：
其中Jt是预积分旋转误差δγ关于陀螺零偏的一阶雅可比矩阵，且在优化前通常将陀螺零偏/>的初值设置为零。
基于上述公式，旋转约束的误差方程规定如下：
上式中Zq部分包含了和Jt+1，该部分由IMU的测量信息获取，/>为相机的视觉旋转信息，粗标定部分由对极几何约束获取，精确标定部分由视觉重建获取。在考虑了所有的匹配的相机与IMU旋转对之后其代价函数为：
在此处由于是使用四元数表示的空间姿态变化，tc表示标定数据的时间序列，(.)xyz表示姿态四元数的虚部。wq为降低异常值影响的权重信息其中a为规定的阈值信息，δc为相机的视觉旋转信息与经由本次迭代的旋转外参由IMU预积分旋转信息恢复的相机旋转量的差值。通过对代价函数的求解，获取视觉惯性的旋转外参/>与陀螺零偏/>在获取的精确的陀螺零偏/>后，将IMU的预积分信息进行更新：
因为陀螺零偏的补偿，降低了预积分结果中对于姿态、位置误差的二次方发散，提高了视觉惯性导航精度。
步骤4，视觉惯性初始化与无绝对速度的平移外参估计
在初始化之前，将滑窗内首个关键帧的相机坐标系定义为临时的参考坐标系，并定义为C0，在完成该帧的重力量测值的估计后将临时参考坐标系与世界坐标系对齐。在步骤3可以获取视觉惯性的旋转外参/>与陀螺零偏/>并使用陀螺零偏/>对现有的预积分信息进行了补偿。同时因为旋转外参/>的获取，可以将滑窗内由步骤2获取的以C0系为参考的相机姿态/>转换为IMU姿态/>但由于纯视觉位姿解算中尺度信息s未知所以也需要对尺度信息s进行估计，用于恢复滑窗内的位置状态信息/>同时也需要对滑窗内各帧的绝对速度信息/>进行估计。为了使视觉与惯性数据进行融合，视觉惯性的初始化需要对以下状态量进行估计：
其中s为IMU量测位移与视觉恢复位移的比例因子，为第n个滑窗中对应的速度信息，/>为参考相机坐标系下的重力向量。
可以通过滑窗内相邻关键帧之间的IMU预积分位置信息与速度信息建立残差方程：
其中分别为t时刻至t+1时刻的IMU预积分相对位移、速度，/>为滑窗内首个相机关键帧至t时刻相机关键帧的位置，但由于其为纯视觉结算结果，该位置尺度未知需要比例因子s恢复真实尺度，/>为滑窗内首个相机关键帧中的重力量测值，/>为t时刻IMU至首个相机关键帧姿态变化的旋转矩阵，/>为首个相机关键帧至t+1时刻IMU姿态变化的旋转矩阵，/>为IMU与相机之间的平移外参，/>为t时刻IMU的绝对速度，/>为t+1时刻IMU的绝对速度，δtt为t与t+1时刻的时差。
公式12的矩阵形式如下：
进而对参数χI求解，但是在初始化过程中假设滑窗数量为n+1，χI中需要估计的状态量为3n+7维，构建的约束方程数量为6n维，大量的运算被用于估计速度信息；且在先验信息较少的情况下的初始化过程中经常被设置为零值，忽略了/>对初始化状态估计的贡献。
步骤4.1，因此，本发明建立了一种不需要估计速度信息的约束方程，在初始化之前对视觉惯性平移外参进行估计并进行相应补偿提高初始化的质量，该步骤也称为无绝对速度的平移外参估计器。
该约束方程如下，通过相邻关键帧之间的预积分已知的相对速度信息代替了滑窗内未知的绝对速度信息。12式中的位置约束方程可以转换为如下形式：
在连续三帧的数据里，如果将关键帧数据t、t+1、t+2描述为1、2、3，由14式可以获取相邻帧之间的关系：
由12式中预积分的速度约束和15式可以获取无速度估计的约束方程：
至此可以在规避了对绝对速度信息的情况下对χp参数进行估计：
位置约束的误差方程规定如下：
上式中Zp部分包含了和/>前者由IMU的测量信息获取，后者为相机的视觉重建获取的无尺度位移信息，Zv包括/>表示IMU的预积分的速度测量信息，Zr包括/>表示相机的视觉旋转信息。
通过在初始化前期所有的配对的相机与IMU位置后其代价函数为：
通过对代价函数的求解获取χp结果，即IMU量测位移与视觉恢复位移的比例因子s，滑窗内首个相机关键帧中的重力量测值平移外参/>
步骤4.2，通过步骤4.1获取了滑窗内的部分初始状态如重力、尺度以及平移外参。以公式17中的参数χp作为先验信息，由12式构建位置与速度的约束方程对公式11中完整的初始状态χI进行估计，获取最终的重力、尺度滑窗内的速度量以及平移外参。最后，将滑窗内的首个关键帧至t时刻相机的位移速度/>姿态/>等状态量依据临时参考系C0与当地重力的旋转变化对齐至世界坐标系W下，完成初始化。
另一实施例中，本发明提供一种弱环境与弱动作控制下快速视觉惯性标定与初始化设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如上述技术方案中所述的方法。
下面通过实验对本发明方法的技术效果进行验证：
为了验证上述方法，使用Euroc公开数据集进行了测试，该数据集以无人机为载体提供了所需的相机与IMU测量数据，同时提供了IMU零偏、传感器的外参以及真实轨迹的参考值用于验证本发明方法可行性。
从Euroc数据集的厂房场景数据截取7组数据进行处理并了误差统计，该统计为双重优化器的结果误差统计。表1记录了VINS-MONO与本发明方法获取的旋转外参误差欧拉角以及三轴总误差，表2记录了VINS-MONO与本发明方法获取的常值陀螺零偏误差欧拉角以及三轴总误差。图3为不同实验场景下旋转外参与陀螺零偏三轴总误差的对比柱状图。本方法精度相对于VINS-MONO的标定结果提升明显，VINS-MONO旋转单轴最大误差不低于4.7度，其三轴欧拉角总误差均值约为11.87度，本发明方法总误差均值为1.12度。VINS-MONO陀螺零偏三轴欧拉角模长误差均值为3.03度/秒，本发明方法误差均为为0.45度/秒。从图2中可得，本方法最快5.146s可完成标定，平均耗时为8.63秒，相对于VINS-MONO的计算用时提升约10s。
表1旋转外参标定结果(单位：度)
表2陀螺零偏标定结果(单位：度/秒)
同时使用初始化后的导航结果展示初始化状态估计的提升。使用统一数据进行测试，本文方法导航轨迹总长约为12米，VINS-MONO在本文算法完成初始化且开始导航约8米处才完成初始化开始导航，证明了本方法的初始化速度远快于VINS-MONO。
图6中a部分展示了VINS-MONO的初始化完成后短时间内的导航轨迹，因为标定误差以及初始化误差的存在导致导航系统对于视觉惯性数据不能产生最佳的组合效果，系统的位姿状态的估计出现偏差，体现在a部分中VINS-MONO在蓝色圆圈处完成了初始化，其导航轨迹与黄色的真值没有很好的符合度，特别是在转向处蓝色轨迹出现了明显的锯齿状导航结果，而本方法的轨迹因为更好的标定与初始化结果没有出现明显的数据融合异常。且VINS-MONO初始化第一帧的位置误差为0.049m，本方法的第一帧位置误差为0.026m。在该数据场景中，VINS-MONO位置误差与尺度偏移均值分别为0.047m以及1.633％，本文的方法位置误差与尺度偏移均值分别为0.014m以及0.332％。
通过上述实验结果证明了，因为更好的标定结果和初始化估计，我们的方法可以更好的融合视觉惯性数据，在后续时间内稳定保持厘米级的导航精度。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
Claims (10)
1.弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于，包括以下步骤：
步骤1，惯性数据处理：构建惯性测量单元IMU预积分模型，用于获取连续历元之间的相对位置、速度、姿态的增量信息，同时构建预积分的误差传播模型，用于获取连续历元之间状态量误差之间的线性关系，进而获取位置、速度、姿态误差关于陀螺零偏与加速度计零偏的雅各比矩阵；
步骤2，视觉数据的处理：对于获取到的视觉数据，首先使用描述子来提取特征点信息，使用光流法进行追踪并对追踪到的特征点进行误差点的剔除；
步骤3，弱动作控制的双重优化器：基于步骤1与步骤2获取到的惯性部分与视觉部分的位姿解算结果以空间运动一致性为基础构建双重优化器进行旋转外参的解算，进而获取视觉惯性的旋转外参与陀螺零偏；
步骤4，视觉惯性系统的初始化：在步骤3中获取到了视觉与惯性传感器之间的旋转外参与惯性传感器的陀螺零偏，采用无绝对速度的平移外参估计器降低初始化中绝对速度的影响，包括如下子步骤：
步骤4.1，无绝对速度的平移外参估计器，首先构建视觉惯性导航系统的滑窗，通过惯性部分与视觉部分的位姿解算结果构建位置约束方程，根据滑窗内连续三帧的位置约束方程推算获取无绝对速度的唯一约束方程，在方程中待估参数只有重力、尺度以及平移外参，构建优化器获取参数解；
步骤4.2，通过步骤4.1获取了滑窗内的部分初始状态如重力、尺度以及平移外参，依据滑窗内通过惯性部分与视觉部分的解算结果构建位置与速度约束方程，对滑窗内进行全局的优化，获取完整的初始状态量，至此完成视觉惯性系统的初始化。
2.根据权利要求1所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：步骤1中，IMU的采样频率高于相机，这意味着相机的采样周期内存在多个IMU的采样周期，设置存在一相机采样周期[i,j]，在i时刻与j时刻之间存在一IMU采样周期[k,k+1]，则惯性测量单元IMU预积分模型表示为：
其中与/>分别代表了i时刻至k和k+1时刻IMU的相对位移，/>与/>分别代表了i时刻至k和k+1时刻IMU的姿态，/>与/>分别代表了i时刻至k和k+1时刻IMU的相对速度，δt为k与k+1时刻的时差，a为加速度，ω为角速度。
3.根据权利要求2所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：采用中值定理计算加速度a和角速度ω；
其中为k与k+1时刻陀螺仪离散采样的量测角速度信息，/>为k与k+1时刻加速度计离散采样的量测加速度信息，/>分别代表了IMU从i时刻至k和k+1时刻姿态变化的旋转矩阵，且在同一采样周期内IMU零偏常被认为是不变化的，/>则分别代表了k时刻的加速度计与陀螺零偏。
4.根据权利要求1所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：步骤1中，考虑加速度计零偏误差δba以及陀螺零偏误差δbg对IMU预积分的影响，则IMU预积分模型表示如下；
其中分别代表了i时刻的加速度计零偏的误差，/>分别为i时刻至j时刻相机关键帧之间的误差修正后的相对位移、速度、姿态，/>分别为i时刻至j时刻相机关键帧之间的误差修正前的相对位移、速度、姿态，/>分别为相对位置与相对速度的误差关于加速度误差的雅各比矩阵，/>分别为相对位置、相对旋转以及相对速度的误差关于角速度误差的雅各比矩阵，/>表示四元数的乘法；
在进行外参标定时，将加速度计零偏ba和陀螺零偏bg置零，并使用公式(3)代替原有预积分模型考虑两者对IMU预积分的影响，进而通过对δba和δba的估计实现对零偏ba和bg的估计，接着再次使用公式(3)对现有IMU预积分进行补偿获取更精确的IMU预积分结果；
公式(3)中的雅各比矩阵从IMU预积分的误差传播模型获取，根据误差状态卡尔曼滤波的原理，获取IMU预积分误差传播模型：
其中分别代表了IMU从i时刻至k与k+1时刻的相对位移、姿态、速度的误差，/>分别代表了k与k+1时刻的加速度计零偏的误差，分别代表了k与k+1时刻的陀螺零偏误差；/>为k时刻IMU的随机白噪声，为k+1时刻IMU的随机白噪声，/>为k时刻IMU零偏的随机游走噪声。Fk为k+1时刻IMU预积分状态误差对于k时刻IMU预积分状态误差的一阶雅各比矩阵，Gk为k+1时刻IMU预积分状态误差对于k与k+1时刻IMU离散噪声的一阶雅各比矩阵，公式(3)中预积分速度、位置、姿态误差关于IMU零偏误差的雅各比矩阵/> 可以从Fk矩阵中获取，至此完成IMU数据的处理。
5.根据权利要求1所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：步骤2中对于获取到的视觉数据，首先利用对极几何获取视觉姿态信息，然后以对极几何的位姿结果为状态量，同时对图像中的共视特征点进行三角化作为量测信息进行约束；最后使用全局的光束法平差优化来最小化滑窗内特征的重投影误差获取精确的所有相机帧位姿信息，供外参的精确优化使用。
6.根据权利要求1所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：步骤3中对于同一时空的相机与IMU姿态建立以下的约束：
其中为i时刻至j时刻IMU的相对姿态，/>为相机求解的视觉旋转信息，/>为相机与IMU之间的旋转外参；
首先使用对极几何获取的视觉姿态信息与IMU预积分，以公式(5)为约束构建第一层优化器，进行旋转约束获取粗略的外参标定结果；
接着使用步骤2中光束法平差的视觉姿态信息与IMU预积分，以公式(5)为约束构建第二层优化器，在第二层优化器中使用第一层的标定结果为初值进行优化求解，获取最终标定结果。
7.根据权利要求6所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：旋转约束的优化部分如下所示，
其中χq为优化参数的集合，为滑窗内的陀螺常值零偏；
由步骤1中的预积分推算方程，真实的预积分旋转表示如下：
其中Jt是预积分旋转误差δγ关于陀螺零偏的一阶雅可比矩阵，且在优化前将陀螺零偏/>的初值设置为零；
基于上述公式，旋转约束的误差方程规定如下：
上式中Zq部分包含了和Jt+1，该部分由IMU的测量信息获取，/>为相机的视觉旋转信息，粗标定部分由对极几何约束获取，精确标定部分由视觉重建获取；在考虑了所有的匹配的相机与IMU旋转对之后其代价函数为：
在此处使用四元数表示的空间姿态变化，tc表示标定数据的时间序列，(.)xyz表示姿态四元数的虚部，wq为降低异常值影响的权重信息其中a为规定的阈值信息，δc为相机的视觉旋转信息与经由本次迭代的旋转外参由IMU预积分旋转信息恢复的相机旋转量的差值，通过对代价函数的求解，获取视觉惯性的旋转外参/>与陀螺零偏/>在获取的精确的陀螺零偏/>后，将IMU的预积分信息进行更新：
8.根据权利要求1所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：步骤4.1的具体实现方式如下；
在初始化之前，将滑窗内首个关键帧的相机坐标系定义为临时的参考坐标系，并定义为C0，在完成该帧的重力量测值的估计后将临时参考坐标系与世界坐标系对齐，在步骤3中获取视觉惯性的旋转外参/>与陀螺零偏/>并使用陀螺零偏/>对现有的预积分信息进行了补偿，同时因为旋转外参/>的获取，将滑窗内由步骤2获取的以C0系为参考的相机姿态转换为IMU姿态/>但由于纯视觉位姿解算中尺度信息s未知，所以需要对尺度信息s进行估计，用于恢复滑窗内的位置状态信息/>同时对滑窗内各帧的绝对速度信息/>进行估计，为了使视觉与惯性数据进行融合，视觉惯性的初始化需要对以下状态量进行估计：
其中s为IMU量测位移与视觉恢复位移的比例因子，即尺度信息，为第n个滑窗中对应的速度信息，/>为参考相机坐标系下的重力向量；
通过滑窗内相邻关键帧之间的IMU预积分位置信息与速度信息建立残差方程：
其中分别为t时刻至t+1时刻的IMU预积分相对位移、速度，/>为滑窗内首个相机关键帧至t时刻相机关键帧的位置，但由于其为纯视觉结算结果，该位置尺度未知需要比例因子s恢复真实尺度，/>为滑窗内首个相机关键帧中的重力量测值，/>为t时刻IMU至首个相机关键帧姿态变化的旋转矩阵，/>为首个相机关键帧至t+1时刻IMU姿态变化的旋转矩阵，/>为IMU与相机之间的平移外参，/>为t时刻IMU的绝对速度，/>为t+1时刻IMU的绝对速度，δtt为t与t+1时刻的时差；
公式12的矩阵形式如下：
然后采用无绝对速度的平移外参估计器进行求解：
平移外参估计器中的约束方程通过相邻关键帧之间的预积分已知的相对速度信息代替滑窗内未知的绝对速度信息，12式中的位置约束方程可以转换为如下形式：
在连续三帧的数据里，将关键帧数据t、t+1、t+2描述为1、2、3，由14式可以获取相邻帧之间的关系：
由12式中预积分的速度约束和15式可以获取无速度估计的约束方程：
至此可以在规避了对绝对速度信息的情况下对χp参数进行估计：
位置约束的误差方程规定如下：
上式中Zp部分包含了和/>由IMU的测量信息获取，为相机的视觉重建获取的无尺度位移信息，Zv包括/>表示IMU的预积分的速度测量信息，Zr包括/>表示相机的视觉旋转信息；
通过在初始化前期所有的配对的相机与IMU位置后，其代价函数为：
通过对代价函数的求解获取χp结果，即IMU量测位移与视觉恢复位移的比例因子s，滑窗内首个相机关键帧中的重力量测值平移外参/>
9.根据权利要求8所述的弱环境与弱动作控制下快速视觉惯性标定与初始化方法，其特征在于：步骤4.2的具体实现方式如下；
通过步骤4.1获取了滑窗内的部分初始状态，包括重力、尺度以及平移外参，以公式17中的参数χp作为先验信息，由公式12构建位置与速度的约束方程对公式11中完整的初始状态χI进行估计，获取最终的重力、尺度滑窗内的速度量以及平移外参，最后，将滑窗内的首个关键帧至t时刻相机的位移速度/>姿态/>状态量依据临时参考系C0与当地重力的旋转变化对齐至世界坐标系W下，完成初始化。
10.弱环境与弱动作控制下快速视觉惯性标定与初始化设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如权利要求1至9中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310982450.4A CN117073720A (zh) | 2023-08-04 | 2023-08-04 | 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310982450.4A CN117073720A (zh) | 2023-08-04 | 2023-08-04 | 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117073720A true CN117073720A (zh) | 2023-11-17 |
Family
ID=88712574
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310982450.4A Pending CN117073720A (zh) | 2023-08-04 | 2023-08-04 | 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117073720A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117968940A (zh) * | 2024-03-29 | 2024-05-03 | 长城汽车股份有限公司 | 惯性参数合成方法、装置、终端设备及存储介质 |
-
2023
- 2023-08-04 CN CN202310982450.4A patent/CN117073720A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117968940A (zh) * | 2024-03-29 | 2024-05-03 | 长城汽车股份有限公司 | 惯性参数合成方法、装置、终端设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110375738B (zh) | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 | |
| CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
| CN109376785B (zh) | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 | |
| CN111811506B (zh) | 视觉/惯性里程计组合导航方法、电子设备及存储介质 | |
| CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
| CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
| CN112649016A (zh) | 一种基于点线初始化的视觉惯性里程计方法 | |
| CN111862316B (zh) | 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 | |
| Zhang et al. | Vision-aided localization for ground robots | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN113551665B (zh) | 一种用于运动载体的高动态运动状态感知系统及感知方法 | |
| CN110207693B (zh) | 一种鲁棒立体视觉惯性预积分slam方法 | |
| CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与系统 | |
| CN114964276B (zh) | 一种融合惯导的动态视觉slam方法 | |
| CN117073720A (zh) | 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备 | |
| CN114136315A (zh) | 一种基于单目视觉辅助惯性组合导航方法及系统 | |
| CN114440877B (zh) | 一种异步多相机视觉惯性里程计定位方法 | |
| CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
| CN113155152B (zh) | 基于李群滤波的相机与惯性传感器空间关系自标定方法 | |
| CN112284381B (zh) | 视觉惯性实时初始化对准方法及系统 | |
| CN113358134A (zh) | 一种视觉惯性里程计系统 | |
| CN114543786B (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 |