CN111429524A - 一种相机与惯性测量单元在线初始化与标定方法及*** - Google Patents

一种相机与惯性测量单元在线初始化与标定方法及*** Download PDF

Info

Publication number
CN111429524A
CN111429524A CN202010197372.3A CN202010197372A CN111429524A CN 111429524 A CN111429524 A CN 111429524A CN 202010197372 A CN202010197372 A CN 202010197372A CN 111429524 A CN111429524 A CN 111429524A
Authority
CN
China
Prior art keywords
measurement unit
inertial measurement
camera
frame
rotation angle
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.)
Granted
Application number
CN202010197372.3A
Other languages
English (en)
Other versions
CN111429524B (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.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong 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 Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202010197372.3A priority Critical patent/CN111429524B/zh
Publication of CN111429524A publication Critical patent/CN111429524A/zh
Application granted granted Critical
Publication of CN111429524B publication Critical patent/CN111429524B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供了一种相机与惯性测量单元在线初始化与标定方法及***，包括：步骤M1：纯视觉初始化，利用相机的观测量得到初始关键帧的位姿并构建初始地图；步骤M2：视觉与惯性联合标定与初始化，基于初始关键帧的位姿与初始地图完成惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定；步骤M3：相机与惯性测量单元融合的同步定位与建图的过程中，对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定进行异常检测与更新。本发明不需要标定板等人工标定工具即可联合初始化过程完成对相机与惯性测量单元相对旋转角和相对平移量的标定，扩大了使用范围。

Description

一种相机与惯性测量单元在线初始化与标定方法及***
技术领域
本发明涉及传感器融合领域，具体地，涉及一种相机与惯性测量单元在线初始化与标定方法及***，更为具体地，涉及基于信息量窗口的单目相机与惯性测量单元联合在线初始化与标定的方法及***。
背景技术
相机和惯性测量单元(Inertial Measurement Unit,IMU)具有互补的特性，因而被组合起来广泛应用于导航和三维重建等领域。相机能够获得外界环境的丰富信息，但在快速运动、光照改变等情况下容易失效。IMU能够高频地获得机器人内部的运动信息，并且不受周围环境的影响，从而弥补视觉传感器的不足。同时，通过视觉匹配完成的回环检测与回环校正可以有效地修正IMU的累计漂移误差。
初始化是基于优化的SLAM***中必不可少的环节。初始化过程确定了待估计变量的初始值和***运行所需的参数，包括初始几帧的位姿、初始地图点、尺度因子、重力向量和IMU零漂值等。由于***的非线性，初值的确定能够避免非线性优化陷入局部最优。现有的初始化方法通过将角速度进行积分得到各帧的姿态，随着时间的增长累计误差不断增加。
相机与IMU的外参包括相机与IMU的相对旋转角、相对平移量。确定视觉传感器与IMU外参是对相机与IMU进行融合应用的前提。目前，视觉传感器与IMU外参的标定主要依靠离线的人工操作，需要操作者手持标定板并进行适当的移动和旋转操作。整个过程费时耗力，而且需要操作者具备相关的知识。目前已有一些在线标定的方法。但是，在运动情况退化情况下(机器人的加速度和旋转量很小)，这些方法的计算量都会随时间快速增长。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种相机与惯性测量单元在线初始化与标定方法及***。
根据本发明提供的一种相机与惯性测量单元在线初始化与标定方法，包括：
步骤M1：纯视觉初始化，利用相机的观测量得到初始关键帧的位姿并构建初始地图；
步骤M2：视觉与惯性联合标定与初始化，基于初始关键帧的位姿以及初始地图完成惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定；
步骤M3：相机与惯性测量单元融合的同步定位与建图的过程中，对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定进行异常检测与更新。
优选地，所述步骤M1包括：
步骤M1.1：根据进入的新关键帧，寻找符合预设条件的视差角和匹配点对数的当前帧和历史帧；
步骤M1.2：利用对极约束求取当前帧和历史帧之间的相对位姿；
步骤M1.3：根据得到的当前帧和历史帧之间的相对位姿，通过三角化得到当前帧和历史帧共同观测到的地图点的世界坐标系坐标，构建初始地图；
步骤M1.4：利用透视n点法将初始地图中的点投影到各关键帧的像素平面上，求解各关键帧在世界坐标系中的位姿。
优选地，所述步骤M2包括：
步骤M2.1：基于信息量对相机与惯性测量单元的观测量进行筛选，筛选出最大化总信息量的预设数量的观测量；利用筛选后得到的相机与惯性测量单元的观测量进行惯性测量单元角速度零漂值的初始化和相机与惯性测量单元相对旋转角的标定，直到判断相机与惯性测量单元的相对旋转角收敛；
步骤M2.2：假设惯性测量单元加速度的零漂值为零，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量和相机与惯性测量单元的相对平移量进行粗略估计；
步骤M2.3：利用粗略估计的重力向量的方向，并引入重力大小作为新的约束，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量、相机与惯性测量单元相对平移量以及惯性测量单元中加速度计的零漂值进行精细化估计；当尺度因子和相机与惯性测量单元的相对平移量都收敛后，视觉与惯性联合初始化和标定完成；
所述信息量为相机与惯性测量单元的观测量中包括相机与惯性测量单元相对旋转角信息量。
优选地，所述步骤M2.1包括：
步骤M2.1.1：利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过对惯性测量单元的角速度进行积分得到相邻两帧之间惯性测量单元的相对旋转角
Figure BDA0002418099640000031
Figure BDA0002418099640000032
其中，ΔRi,i+1表示惯性测量单元角速度积分值；
Figure BDA0002418099640000033
表示惯性测量单元相对于角速度零漂值的雅可比矩阵；bg表示惯性测量单元角速度零漂值；
步骤M2.1.2：利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过视觉同步定位与建图得到相邻两帧的相机在世界坐标系下的位姿
Figure BDA0002418099640000034
利用相机与惯性测量单元的相对旋转角得到惯性测量单元的相对旋转角
Figure BDA0002418099640000035
Figure BDA0002418099640000036
其中，Rcb表示相机与惯性测量单元的相对旋转角；
步骤M2.1.3：最小化利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角和利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角的误差得到惯性测量单元角速度零漂值的最优估计，即得到惯性测量单元角速度零漂值的初始化；
通过最小化上述两相对旋转角误差得到惯性测量单元角速度零漂值的最优估计
Figure BDA0002418099640000037
Figure BDA0002418099640000038
其中，N表示关键帧个数，R2表示利用相机观测量得到的惯性测量单元的相对旋转角，R1表示利用惯性测量单元角速度积分得到的惯性测量单元的相对旋转角，bi表示第i个关键帧的惯性测量单元，bi+1表示第i+1个关键帧的惯性测量单元，bg表示惯性测量单元角速度零漂值；
步骤M2.1.4：对任意相邻两关键帧，观测量所包含的关于相机与惯性测量单元相对旋转角的信息量度量为：
iQi,i+1＝trace((QL(qbi,bi+1)-QR(qci,ci+1))T·(QL(qbi,bi+1)-QR(qci,ci+1))) (4)
其中，QL(q)表示四元数q对应的左乘矩阵；QR(q)表示四元数q对应的右乘矩阵；qbi,bi+1表示第i帧与第i+1帧惯性测量单元的相对旋转角对应的四元数；qci,ci+1表示第i帧与第i+1帧相机的相对旋转角对应的四元数；
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定。如果关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
步骤M2.1.5：利用筛选后的观测量构建以相机和惯性测量单元相对旋转角为未知量的方程组，利用最小二乘法求解方程组，得到以四元数形式表示的相机与惯性测量单元相对旋转角的最优估计
Figure BDA0002418099640000041
即相机与惯性测量单元相对旋转角的标定；
对于任意相邻两关键帧，相机和惯性测量单元相对旋转角为未知量，满足如下方程组：
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc＝0 (5)
其中，QL表示四元数的左乘矩阵，qbi，bi+1表示第i帧和第i+1帧下惯性测量单元相对旋转角的四元数表示，QR表示四元数的右乘矩阵，qci,ci+1表示第i帧和第i+1帧下相机相对旋转角的四元数表示，qbc表示相机与惯性测量单元的相对旋转角的四元数表示；
将(QL(qbi,bi+1)-QR(qci,ci+1))表示为Qi,i+1，利用筛选后的观测量Qi,i+1组成观测矩阵Q，建立方程：
Q·qbc＝0 (6)
利用最小二乘法求解相机与惯性测量单元的相对旋转角的最优估计
Figure BDA0002418099640000042
Figure BDA0002418099640000043
步骤M2.1.6：通过判断最近Nwinc个相机与惯性测量单元相对旋转角对应的欧拉角的偏航角、俯仰角和横滚角的标准差σyaw、σpitch、σroll是否小于阈值
Figure BDA0002418099640000044
来确定视觉惯性旋转角是否标定完成；当标定未完成，即相机与惯性测量单元的相对旋转角未收敛，则等待新的关键帧重复执行步骤M2.1.1至步骤M2.1.5，直至视觉惯性旋转角标定完成；
所述步骤M2.2包括：
步骤M2.2.1：利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角和平移量；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量和相对线速度；
构建第i帧以尺度因子s，重力向量gw和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000051
Figure BDA0002418099640000052
Figure BDA0002418099640000053
Figure BDA0002418099640000054
Figure BDA0002418099640000055
其中，假设惯性测量单元加速度零漂值ba为零；αi表示方程中尺度因子前的系数，βi表示方程中重力向量的初始化值前的系数，γi表示方程中视觉惯性平移量前的系数，ηi表示方程中的常数项系数，s表示尺度因子；gw表示重力向量的初始化值；Pcb表示视觉惯性平移量；Rwc表示相机在世界坐标系中的旋转角；Pwc相机在世界坐标系中的平移量；
Figure BDA0002418099640000056
表示第i+1帧相机在世界坐标系中的平移量，
Figure BDA0002418099640000057
表示第i帧相机在世界坐标系中的平移量，dti+1,i+2表示第i+1帧和第i+2帧之间的时间差，
Figure BDA0002418099640000058
表示第i+2帧相机在世界坐标系中的平移量，dti,i+1表示第i帧和第i+1帧之间的时间差，I3×3表示单位矩阵，
Figure BDA0002418099640000059
表示第i+2帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400000510
表示第i+1帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400000511
表示第i帧相机在世界坐标系中的旋转角，Rcb表示相机与惯性测量单元之间的相对旋转角，ΔPi,i+1表示第i帧和i+1帧惯性测量单元的相对平移量，ΔPi+1,i+2表示第i+1帧和i+2帧惯性测量单元的相对平移量，Δvi,i+1表示第i+1帧和i+2帧惯性测量单元的相对线速度；
步骤M2.2.2：假设Ai＝[αi βi γi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA0002418099640000061
设置信息量窗口大小为Nwin，当关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于等于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
步骤M2.2.3：假设
Figure BDA0002418099640000062
Bi＝ηi，对于每相邻3帧，有方程Ai·x＝Bi；利用筛选后的信息量构建信息矩阵A和常数矩阵B,构建方程：
A·x＝B (9)
利用最小二乘法求解尺度因子的最优估计s*，重力向量的最优估计gw *和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000063
所述步骤M2.3包括：引入预设值大小的重力向量为约束，得到尺度因子s、重力向量gw的初始化值和视觉惯性平移量Pcb的标定值；
根据得到的重力向量gw得到重力的旋转角：
Figure BDA0002418099640000064
其中，RWI表示重力在世界坐标系中的旋转角，
Figure BDA0002418099640000065
表示与重力向量
Figure BDA0002418099640000066
和固定向量gI垂直的单位向量，θ表示与重力向量
Figure BDA0002418099640000067
和固定向量gI之间的夹角，gI表示固定向量；将重力向量用旋转角和重力大小表示并进行一阶泰勒展开：
Figure BDA0002418099640000068
其中，δθ表示重力向量旋转角的更新值；G表示重力大小，δθx表示重力向量旋转角的更新值在世界坐标系的x轴下的分量，δθy表示重力向量旋转角的更新值在世界坐标系的y轴下的分量；
利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角Rwc和平移量Pwc；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量ΔPi,i+1和相对线速度Δvi,i+1；构建第i帧以尺度因子s，重力向量gw，惯性测量单元加速度零漂值ba和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000071
Figure BDA0002418099640000072
Figure BDA0002418099640000073
Figure BDA0002418099640000074
Figure BDA0002418099640000075
Figure BDA0002418099640000076
其中，λi表示方程中尺度因子前的系数，μi表示方程中重力向量旋转角的更新值前的系数，ωi表示方程中惯性测量单元加速度零漂值前的系数，υi表示方程中相机与惯性测量单元相对平移量前的系数，ρi表示方程中的常数项系数，
Figure BDA0002418099640000077
表示第i帧和第i+2帧的惯性测量单元的相对线速度相比于惯性测量单元加速度零漂值的雅可比矩阵，dt12表示第i帧和第i+1帧之间的时间差，dt23表示第i+1帧和i+2帧之间的时间差，
Figure BDA0002418099640000078
表示第i+1帧和第i+2帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，
Figure BDA0002418099640000079
表示第i帧和第i+1帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，小标[:,1:2]表示只取矩阵的前两列；
假设Ci＝[λi μi ωi υi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA00024180996400000710
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
构建方程组并利用最小二乘法求解
假设
Figure BDA0002418099640000081
Di＝ρi，对于每相邻3帧，有方程Ci·y＝Di；利用筛选后的观测量构建观测矩阵C和常数矩阵D，构建方程：
C·y＝D (14)
利用最小二乘法求解尺度因子的最优估计s*，重力向量旋转角的更新值的最优估计δθ*，惯性测量单元加速度零漂值最优估计
Figure BDA0002418099640000082
和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000083
通过判断最近Nwinc个视觉惯性平移量的各维度值所对应的标准差σx、σy、σz是否小于阈值
Figure BDA0002418099640000084
来确定视觉惯性平移量是否标定完成；通过判断最近Nwinc个尺度因子的标准差σs是否小于阈值thrs来确定视觉惯性初始化是否完成；当视觉惯性平移量标定未完成或视觉惯性初始化未完成，则等待新的关键帧后重复执行步骤M2.2至步骤M2.3直至视觉与惯性联合初始化和标定完成。
优选地，所述步骤M3包括：在相机与惯性测量单元融合的同步定位与建图过程中，每隔预设值周期判断相机与惯性测量单元相对位姿是否改变，当改变时，则根据最新帧中的任意相邻两帧对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量进行重新标定；
步骤M3.1：根据比较相机与惯性测量单元的观测量和相机与惯性测量单元相对旋转角的初始标定值之间的一致性误差是否超过阈值判断相机与惯性相对旋转角标定值是否需要更新。
根据本发明提供的一种相机与惯性测量单元在线初始化与标定***，包括：
模块M1：纯视觉初始化，利用相机的观测量得到初始关键帧的位姿并构建初始地图；
模块M2：视觉与惯性联合标定与初始化，基于初始关键帧的位姿以及初始地图完成惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定；
模块M3：相机与惯性测量单元融合的同步定位与建图的过程中，对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定进行异常检测与更新。
优选地，所述模块M1包括：
模块M1.1：根据进入的新关键帧，寻找符合预设条件的视差角和匹配点对数的当前帧和历史帧；
模块M1.2：利用对极约束求取当前帧和历史帧之间的相对位姿；
模块M1.3：根据得到的当前帧和历史帧之间的相对位姿，通过三角化得到当前帧和历史帧共同观测到的地图点的世界坐标系坐标，构建初始地图；
模块M1.4：利用透视n点法将初始地图中的点投影到各关键帧的像素平面上，求解各关键帧在世界坐标系中的位姿。
优选地，所述模块M2包括：
模块M2.1：基于信息量对相机与惯性测量单元的观测量进行筛选，筛选出最大化总信息量的预设数量的观测量；利用筛选后得到的相机与惯性测量单元的观测量进行惯性测量单元角速度零漂值的初始化和相机与惯性测量单元相对旋转角的标定，直到判断相机与惯性测量单元的相对旋转角收敛；
模块M2.2：假设惯性测量单元加速度的零漂值为零，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量和相机与惯性测量单元的相对平移量进行粗略估计；
模块M2.3：利用粗略估计的重力向量的方向，并引入重力大小作为新的约束，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量、相机与惯性测量单元相对平移量以及惯性测量单元中加速度计的零漂值进行精细化估计；当尺度因子和相机与惯性测量单元的相对平移量都收敛后，视觉与惯性联合初始化和标定完成；
所述信息量为相机与惯性测量单元的观测量中包括相机与惯性测量单元相对旋转角信息量。
优选地，所述模块M2.1包括：
模块M2.1.1：利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过对惯性测量单元的角速度进行积分得到相邻两帧之间惯性测量单元的相对旋转角
Figure BDA0002418099640000091
Figure BDA0002418099640000092
其中，ΔRi,i+1表示惯性测量单元角速度积分值；
Figure BDA0002418099640000101
表示惯性测量单元相对于角速度零漂值的雅可比矩阵；bg表示惯性测量单元角速度零漂值；
模块M2.1.2：利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过视觉同步定位与建图得到相邻两帧的相机在世界坐标系下的位姿
Figure BDA0002418099640000102
利用相机与惯性测量单元的相对旋转角得到惯性测量单元的相对旋转角
Figure BDA0002418099640000103
Figure BDA0002418099640000104
其中，Rcb表示相机与惯性测量单元的相对旋转角；
模块M2.1.3：最小化利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角和利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角的误差得到惯性测量单元角速度零漂值的最优估计，即得到惯性测量单元角速度零漂值的初始化；
通过最小化上述两相对旋转角误差得到惯性测量单元角速度零漂值的最优估计
Figure BDA0002418099640000105
Figure BDA0002418099640000106
其中，N表示关键帧个数，R2表示利用相机观测量得到的惯性测量单元的相对旋转角，R1表示利用惯性测量单元角速度积分得到的惯性测量单元的相对旋转角，bi表示第i个关键帧的惯性测量单元，bi+1表示第i+1个关键帧的惯性测量单元，bg表示惯性测量单元角速度零漂值；
模块M2.1.4：对任意相邻两关键帧，观测量所包含的关于相机与惯性测量单元相对旋转角的信息量度量为：
Figure BDA0002418099640000107
其中，QL(q)表示四元数q对应的左乘矩阵；QR(q)表示四元数q对应的右乘矩阵；qbi,bi+1表示第i帧与第i+1帧惯性测量单元的相对旋转角对应的四元数；qci,ci+1表示第i帧与第i+1帧相机的相对旋转角对应的四元数；
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定。如果关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
模块M2.1.5：利用筛选后的观测量构建以相机和惯性测量单元相对旋转角为未知量的方程组，利用最小二乘法求解方程组，得到以四元数形式表示的相机与惯性测量单元相对旋转角的最优估计
Figure BDA0002418099640000111
即相机与惯性测量单元相对旋转角的标定；
对于任意相邻两关键帧，相机和惯性测量单元相对旋转角为未知量，满足如下方程组：
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc＝0 (5)
其中，QL表示四元数的左乘矩阵，qbi，bi+1表示第i帧和第i+1帧下惯性测量单元相对旋转角的四元数表示，QR表示四元数的右乘矩阵，qci,ci+1表示第i帧和第i+1帧下相机相对旋转角的四元数表示，qbc表示相机与惯性测量单元的相对旋转角的四元数表示；
将(QL(qbi,bi+1)-QR(qci,ci+1))表示为Qi,i+1，利用筛选后的观测量Qi,i+1组成观测矩阵Q，建立方程：
Q·qbc＝0 (6)
利用最小二乘法求解相机与惯性测量单元的相对旋转角的最优估计
Figure BDA0002418099640000112
Figure BDA0002418099640000113
模块M2.1.6：通过判断最近Nwinc个相机与惯性测量单元相对旋转角对应的欧拉角的偏航角、俯仰角和横滚角的标准差σyaw、σpitch、σroll是否小于阈值
Figure BDA0002418099640000114
来确定视觉惯性旋转角是否标定完成；当标定未完成，即相机与惯性测量单元的相对旋转角未收敛，则等待新的关键帧重复触发模块M2.1.1至模块M2.1.5执行，直至视觉惯性旋转角标定完成；
所述模块M2.2包括：
模块M2.2.1：利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角和平移量；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量和相对线速度；
构建第i帧以尺度因子s，重力向量gw和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000121
Figure BDA0002418099640000122
Figure BDA0002418099640000123
Figure BDA0002418099640000124
Figure BDA0002418099640000125
其中，假设惯性测量单元加速度零漂值ba为零；αi表示方程中尺度因子前的系数，βi表示方程中重力向量的初始化值前的系数，γi表示方程中视觉惯性平移量前的系数，ηi表示方程中的常数项系数，s表示尺度因子；gw表示重力向量的初始化值；Pcb表示视觉惯性平移量；Rwc表示相机在世界坐标系中的旋转角；Pwc相机在世界坐标系中的平移量；
Figure BDA0002418099640000126
表示第i+1帧相机在世界坐标系中的平移量，
Figure BDA0002418099640000127
表示第i帧相机在世界坐标系中的平移量，dti+1,i+2表示第i+1帧和第i+2帧之间的时间差，
Figure BDA0002418099640000128
表示第i+2帧相机在世界坐标系中的平移量，dti,i+1表示第i帧和第i+1帧之间的时间差，I3×3表示单位矩阵，
Figure BDA0002418099640000129
表示第i+2帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400001210
表示第i+1帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400001211
表示第i帧相机在世界坐标系中的旋转角，Rcb表示相机与惯性测量单元之间的相对旋转角，ΔPi,i+1表示第i帧和i+1帧惯性测量单元的相对平移量，ΔPi+1,i+2表示第i+1帧和i+2帧惯性测量单元的相对平移量，Δvi,i+1表示第i+1帧和i+2帧惯性测量单元的相对线速度；
模块M2.2.2：假设Ai＝[αi βi γi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA00024180996400001212
设置信息量窗口大小为Nwin，当关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于等于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
模块M2.2.3：假设
Figure BDA00024180996400001213
Bi＝ηi，对于每相邻3帧，有方程Ai·x＝Bi；利用筛选后的信息量构建信息矩阵A和常数矩阵B,构建方程：
A·x＝B (9)
利用最小二乘法求解尺度因子的最优估计s*，重力向量的最优估计gw *和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000131
所述模块M2.3包括：引入预设值大小的重力向量为约束，得到尺度因子s、重力向量gw的初始化值和视觉惯性平移量Pcb的标定值；
根据得到的重力向量gw得到重力的旋转角：
Figure BDA0002418099640000132
其中，RWI表示重力在世界坐标系中的旋转角，
Figure BDA0002418099640000133
表示与重力向量
Figure BDA0002418099640000134
和固定向量gI垂直的单位向量，θ表示与重力向量
Figure BDA0002418099640000135
和固定向量gI之间的夹角，gI表示固定向量；将重力向量用旋转角和重力大小表示并进行一阶泰勒展开：
Figure BDA0002418099640000136
其中，δθ表示重力向量旋转角的更新值；G表示重力大小，δθx表示重力向量旋转角的更新值在世界坐标系的x轴下的分量，δθy表示重力向量旋转角的更新值在世界坐标系的y轴下的分量；
利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角Rwc和平移量Pwc；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量ΔPi,i+1和相对线速度Δvi,i+1；构建第i帧以尺度因子s，重力向量gw，惯性测量单元加速度零漂值ba和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000141
Figure BDA0002418099640000142
Figure BDA0002418099640000143
Figure BDA0002418099640000144
Figure BDA0002418099640000145
Figure BDA0002418099640000146
其中，λi表示方程中尺度因子前的系数，μi表示方程中重力向量旋转角的更新值前的系数，ωi表示方程中惯性测量单元加速度零漂值前的系数，υi表示方程中相机与惯性测量单元相对平移量前的系数，ρi表示方程中的常数项系数，
Figure BDA0002418099640000147
表示第i帧和第i+2帧的惯性测量单元的相对线速度相比于惯性测量单元加速度零漂值的雅可比矩阵，dt12表示第i帧和第i+1帧之间的时间差，dt23表示第i+1帧和i+2帧之间的时间差，
Figure BDA0002418099640000148
表示第i+1帧和第i+2帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，
Figure BDA0002418099640000149
表示第i帧和第i+1帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，小标[:,1:2]表示只取矩阵的前两列；
假设Ci＝[λi μi ωi υi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA00024180996400001410
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
构建方程组并利用最小二乘法求解
假设
Figure BDA00024180996400001411
Di＝ρi，对于每相邻3帧，有方程Ci·y＝Di；利用筛选后的观测量构建观测矩阵C和常数矩阵D，构建方程：
C·y＝D (14)
利用最小二乘法求解尺度因子的最优估计s*，重力向量旋转角的更新值的最优估计δθ*，惯性测量单元加速度零漂值最优估计
Figure BDA0002418099640000151
和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000152
通过判断最近Nwinc个视觉惯性平移量的各维度值所对应的标准差σx、σy、σz是否小于阈值
Figure BDA0002418099640000153
来确定视觉惯性平移量是否标定完成；通过判断最近Nwinc个尺度因子的标准差σs是否小于阈值thrs来确定视觉惯性初始化是否完成；当视觉惯性平移量标定未完成或视觉惯性初始化未完成，则等待新的关键帧后重复触发模块M2.2至模块M2.3执行直至视觉与惯性联合初始化和标定完成。
优选地，所述模块M3包括：在相机与惯性测量单元融合的同步定位与建图过程中，每隔预设值周期判断相机与惯性测量单元相对位姿是否改变，当改变时，则根据最新帧中的任意相邻两帧对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量进行重新标定；
模块M3.1：根据比较相机与惯性测量单元的观测量和相机与惯性测量单元相对旋转角的初始标定值之间的一致性误差是否超过阈值判断相机与惯性相对旋转角标定值是否需要更新。
与现有技术相比，本发明具有如下的有益效果：
1、本发明不需要标定板等人工标定工具即可联合初始化过程完成对相机与惯性测量单元相对旋转角和相对平移量的标定，扩大了使用范围；
2、本发明在运行过程中能够自主判断是否需要重新标定，能够应对相机与惯性测量单元相对位姿随时间变化的情况；
3、本发明利用信息量作为衡量指标，对参与初始化和标定的观测量进行了筛选，抑制了运动退化情况下计算量的不断增长。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为纯视觉初始化流程图；
图2为视觉与惯性融合初始化与标定流程图；
图3为基于信息量窗口的筛选算法流程图；
图4为标定值异常判断与更新流程图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
本发明提供了一种单目相机与惯性测量单元联合在线初始化与标定的方法,以关键帧周期为周期进行，包括纯视觉初始化和视觉与惯性联合标定与初始化以及标定值异常检测与更新三个阶段。在纯视觉初始化阶段，首先寻找符合视差条件和匹配点对数条件的当前帧和历史帧对。然后，利用当前帧和历史帧对中的匹配点对对各关键帧在世界坐标系中的位姿进行初始化，并得到各个地图点在世界坐标系中的坐标，构建初始地图。在视觉与惯性联合标定和初始化阶段，首先，基于信息量对用于标定和初始化的关键帧进行筛选。然后，利用筛选后留下的关键帧的观测量进行惯性测量单元角速度零漂值的初始化和相机与惯性测量单元相对旋转角的标定，直到判断相机与惯性测量单元的相对旋转角收敛。然后，假设加速度的零漂值为零，对尺度因子、重力向量和相机与惯性测量单元的相对平移量进行粗略估计。之后，利用粗略估计的重力向量的方向，并引入重力大小作为新的约束，对尺度因子、重力向量、相机与惯性测量单元相对平移量以及惯性测量单元中加速度计的零漂值进行精细化估计。当尺度因子和视觉惯性平移量都收敛后，初始化和标定完成。标定值更新在***运行过程中进行，隔固定周期判断相机与惯性测量单元相对位姿是否改变，若改变，则对相对位姿进行重新标定。相比较其它的单目相机与惯性测量单元初始化和标定方法，本算法能够直接利用实际环境中的信息而不需要额外的标定板，能够应对长期运行过程中标定值变化的情况，且能够应对本体运动不充分情况下初始化和标定的计算量快速增长的问题。
针对现有相机和惯性测量单元在线初始化和标定在运动退化情况下计算量随时间快速增长的问题，本发明提出了一种基于信息量窗口的单目相机与惯性测量单元联合在线初始化与标定的方法，旨在抑制计算量随时间推移的快速增长。
根据本发明提供的一种相机与惯性测量单元在线初始化与标定方法，包括：
定义待初始化变量：
Figure BDA0002418099640000171
其中
T1,T2......TN：前N帧相机在世界坐标系中的位姿；
Figure BDA0002418099640000172
k个地图点在世界坐标系中的坐标；
s：单目相机尺度因子；
gw：世界坐标系中的重力向量；
bg：惯性测量单元角速度零漂值；
ba：惯性测量单元加速度零漂值；
定义待标定变量：
θ2＝{Rbc,Pbc}，其中
Rbc：相机与惯性测量单元相对旋转角；
Pbc：相机与惯性测量单元相对平移量；
步骤M1：如图1所示，纯视觉初始化，利用相机的观测量得到初始关键帧的位姿并构建初始地图；进行纯视觉初始化是进行纯视觉同步建图与定位的前提条件，在步骤M2与步骤M3中，各关键帧下相机的位姿来源于视觉同步定位与建图；各点是初始关键帧中观测到的点，各点坐标的确定依赖于初始关键帧位姿的确定，各帧位姿就是初始关键帧位姿。
步骤M2：如图2所示，视觉与惯性联合标定与初始化，基于初始关键帧的位姿以及初始地图完成惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定；
步骤M3：如图4所示，相机与惯性测量单元融合的同步定位与建图的过程中，对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定进行异常检测与更新。对异常进行检测与更新是在步骤M1和步骤M2初始化和初始标定完成之后，在整个***运行的过程中以预设的频率进行的，目的在于纠正随时间推移可能产生变化的标定值。
具体地，所述步骤M1包括：
步骤M1.1：根据进入的新关键帧，寻找符合预设条件的视差角和匹配点对数的当前帧和历史帧；
步骤M1.2：利用对极约束求取当前帧和历史帧之间的相对位姿；
步骤M1.3：根据得到的当前帧和历史帧之间的相对位姿，通过三角化得到当前帧和历史帧共同观测到的地图点的世界坐标系坐标，构建初始地图；
步骤M1.4：利用透视n点法(PnP：Perspective-n-Point)将初始地图中的点投影到各关键帧的像素平面上，求解各关键帧在世界坐标系中的位姿。
具体地，所述步骤M2包括：
步骤M2.1：基于信息量对相机与惯性测量单元的观测量进行筛选，筛选出最大化总信息量的预设数量的观测量；利用筛选后得到的相机与惯性测量单元的观测量进行惯性测量单元角速度零漂值的初始化和相机与惯性测量单元相对旋转角的标定，直到判断相机与惯性测量单元的相对旋转角收敛；
步骤M2.2：假设惯性测量单元加速度的零漂值为零，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量和相机与惯性测量单元的相对平移量进行粗略估计；
步骤M2.3：利用粗略估计的重力向量的方向，并引入重力大小作为新的约束，对尺度因子、重力向量、相机与惯性测量单元相对平移量以及惯性测量单元中加速度计的零漂值进行精细化估计；当尺度因子和相机与惯性测量单元的相对平移量都收敛后，视觉与惯性联合初始化和标定完成；
所述信息量为相机与惯性测量单元的观测量中包括相机与惯性测量单元相对旋转角信息量。
具体地，所述步骤M2.1包括：
步骤M2.1.1：利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过对惯性测量单元的角速度进行积分得到相邻两帧之间惯性测量单元的相对旋转角
Figure BDA0002418099640000181
Figure BDA0002418099640000182
其中，ΔRi,i+1表示惯性测量单元角速度积分值；
Figure BDA0002418099640000183
表示惯性测量单元相对于角速度零漂值的雅可比矩阵；bg表示惯性测量单元角速度零漂值；
步骤M2.1.2：利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过视觉同步定位与建图(SLAM：Simultaneous Localization and Mapping)得到相邻两帧的相机在世界坐标系下的位姿
Figure BDA0002418099640000191
利用相机与惯性测量单元的相对旋转角得到惯性测量单元的相对旋转角
Figure BDA0002418099640000192
Figure BDA0002418099640000193
其中，Rcb表示相机与惯性测量单元的相对旋转角；
步骤M2.1.3：最小化利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角和利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角的误差得到惯性测量单元角速度零漂值的最优估计，即得到惯性测量单元角速度零漂值的初始化；
通过最小化上述两相对旋转角误差得到惯性测量单元角速度零漂值的最优估计
Figure BDA0002418099640000194
Figure BDA0002418099640000195
其中，N表示关键帧个数，R2表示利用相机观测量得到的惯性测量单元的相对旋转角，R1表示利用惯性测量单元角速度积分得到的惯性测量单元的相对旋转角，bi表示第i个关键帧的惯性测量单元，bi+1表示第i+1个关键帧的惯性测量单元，bg表示惯性测量单元角速度零漂值；
步骤M2.1.4：对任意相邻两关键帧，观测量所包含的关于相机与惯性测量单元相对旋转角的信息量度量为
Figure BDA0002418099640000196
其中
其中，QL(q)：四元数q对应的左乘矩阵；QR(q)：四元数q对应的右乘矩阵；qbi,bi+1：第i帧与第i+1帧惯性测量单元的相对旋转角对应的四元数；qci,ci+1：第i帧与第i+1帧相机的相对旋转角对应的四元数；
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定。如果关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量。
步骤M2.1.5：利用筛选后的观测量构建以相机和惯性测量单元相对旋转角为未知量的方程组，利用最小二乘法求解方程组，得到以四元数形式表示的相机与惯性测量单元相对旋转角的最优估计
Figure BDA0002418099640000201
即相机与惯性测量单元相对旋转角的标定；
对于任意相邻两关键帧，相机和惯性测量单元相对旋转角为未知量，满足如下方程组：
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc＝0 (4)
其中，QL表示四元数的左乘矩阵，qbi，bi+1表示第i帧和第i+1帧下惯性测量单元相对旋转角的四元数表示，QR表示四元数的右乘矩阵，qci,ci+1表示第i帧和第i+1帧下相机相对旋转角的四元数表示，qbc表示相机与惯性测量单元的相对旋转角的四元数表示；
将(QL(qbi,bi+1)-QR(qci,ci+1))表示为Qi,i+1，利用筛选后的观测量Qi,i+1组成观测矩阵Q，建立方程：
Q·qbc＝0 (5)
利用最小二乘法求解相机与惯性测量单元的相对旋转角的最优估计
Figure BDA0002418099640000202
Figure BDA0002418099640000203
步骤M2.1.6：通过判断最近Nwinc个相机与惯性测量单元相对旋转角对应的欧拉角的偏航角、俯仰角和横滚角的标准差σyaw、σpitch、σroll是否小于阈值
Figure BDA0002418099640000204
来确定视觉惯性旋转角是否标定完成；当标定未完成，即相机与惯性测量单元的相对旋转角未收敛，则等待新的关键帧重复执行步骤M2.1.1至步骤M2.1.5，直至视觉惯性旋转角标定完成；
所述步骤M2.2包括：
步骤M2.2.1：利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角和平移量；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量和相对线速度；
构建第i帧以尺度因子s，重力向量gw和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000211
Figure BDA0002418099640000212
Figure BDA0002418099640000213
Figure BDA0002418099640000214
Figure BDA0002418099640000215
其中，假设惯性测量单元加速度零漂值ba为零；αi表示方程中尺度因子前的系数，βi表示方程中重力向量的初始化值前的系数，γi表示方程中视觉惯性平移量前的系数，ηi表示方程中的常数项系数，s表示尺度因子；gw表示重力向量的初始化值；Pcb表示视觉惯性平移量；Rwc表示相机在世界坐标系中的旋转角；Pwc相机在世界坐标系中的平移量；
Figure BDA0002418099640000216
表示第i+1帧相机在世界坐标系中的平移量，
Figure BDA0002418099640000217
表示第i帧相机在世界坐标系中的平移量，dti+1,i+2表示第i+1帧和第i+2帧之间的时间差，
Figure BDA0002418099640000218
表示第i+2帧相机在世界坐标系中的平移量，dti,i+1表示第i帧和第i+1帧之间的时间差，I3×3表示单位矩阵，
Figure BDA0002418099640000219
表示第i+2帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400002110
表示第i+1帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400002111
表示第i帧相机在世界坐标系中的旋转角，Rcb表示相机与惯性测量单元之间的相对旋转角，ΔPi,i+1表示第i帧和i+1帧惯性测量单元的相对平移量，ΔPi+1,i+2表示第i+1帧和i+2帧惯性测量单元的相对平移量，Δvi,i+1表示第i+1帧和i+2帧惯性测量单元的相对线速度；
如图3所示，步骤M2.2.2：假设Ai＝[αi βi γi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA00024180996400002112
设置信息量窗口大小为Nwin，当关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于等于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
步骤M2.2.3：假设
Figure BDA00024180996400002113
Bi＝ηi，对于每相邻3帧，有方程Ai·x＝Bi；利用筛选后的信息量构建信息矩阵A和常数矩阵B,构建方程：
A·x＝B (8)
利用最小二乘法求解尺度因子的最优估计s*，重力向量的最优估计gw *和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000221
所述步骤M2.3包括：引入重力向量大小为G＝9.8为约束，得到尺度因子s、重力向量gw的初始化值和视觉惯性平移量Pcb的标定值；
根据得到的重力向量gw得到重力的旋转角：
Figure BDA0002418099640000222
其中，RWI表示重力在世界坐标系中的旋转角，
Figure BDA0002418099640000223
表示与重力向量
Figure BDA0002418099640000224
和固定向量gI垂直的单位向量，θ表示与重力向量
Figure BDA0002418099640000225
和固定向量gI之间的夹角，gI表示固定向量[0,0,-1]；将重力向量用旋转角和重力大小表示并进行一阶泰勒展开：
Figure BDA0002418099640000226
其中，δθ表示重力向量旋转角的更新值；G表示重力大小，δθx表示重力向量旋转角的更新值在世界坐标系的x轴下的分量，δθy表示重力向量旋转角的更新值在世界坐标系的y轴下的分量；
利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角Rwc和平移量Pwc；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量ΔPi,i+1和相对线速度Δvi,i+1；构建第i帧以尺度因子s，重力向量gw，惯性测量单元加速度零漂值ba和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000231
Figure BDA0002418099640000232
Figure BDA0002418099640000233
Figure BDA0002418099640000234
Figure BDA0002418099640000235
Figure BDA0002418099640000236
其中，λi表示方程中尺度因子前的系数，μi表示方程中重力向量旋转角的更新值前的系数，ωi表示方程中惯性测量单元加速度零漂值前的系数，υi表示方程中相机与惯性测量单元相对平移量前的系数，ρi表示方程中的常数项系数，
Figure BDA0002418099640000237
表示第i帧和第i+2帧的惯性测量单元的相对线速度相比于惯性测量单元加速度零漂值的雅可比矩阵，dt12表示第i帧和第i+1帧之间的时间差，dt23表示第i+1帧和i+2帧之间的时间差，
Figure BDA0002418099640000238
表示第i+1帧和第i+2帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，
Figure BDA0002418099640000239
表示第i帧和第i+1帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，小标[:,1:2]表示只取矩阵的前两列；
假设Ci＝[λi μi ωi υi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA00024180996400002310
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
构建方程组并利用最小二乘法求解
假设
Figure BDA00024180996400002311
Di＝ρi，对于每相邻3帧，有方程Ci·y＝Di；利用筛选后的观测量构建观测矩阵C和常数矩阵D，构建方程：
C·y＝D (13)
利用最小二乘法求解尺度因子的最优估计s*，重力向量旋转角的更新值的最优估计δθ*，惯性测量单元加速度零漂值最优估计
Figure BDA0002418099640000241
和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000242
通过判断最近Nwinc个视觉惯性平移量的各维度值所对应的标准差σx、σy、σz是否小于阈值
Figure BDA0002418099640000243
来确定视觉惯性平移量是否标定完成；通过判断最近Nwinc个尺度因子的标准差σs是否小于阈值thrs来确定视觉惯性初始化是否完成；当视觉惯性平移量标定未完成或视觉惯性初始化未完成，则等待新的关键帧后重复执行步骤M2.2至步骤M2.3直至视觉与惯性联合初始化和标定完成。
具体地，所述步骤M3包括：在相机与惯性测量单元融合的同步定位与建图过程中，每隔预设值周期判断相机与惯性测量单元相对位姿是否改变，当改变时，则根据最新帧中的任意相邻两帧对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量进行重新标定；
根据比较相机与惯性测量单元的观测量和相机与惯性测量单元相对旋转角的初始标定值之间的一致性误差是否超过阈值判断相机与惯性相对旋转角标定值是否需要更新。
步骤M3.1：判断相机与惯性相对旋转角标定值是否需要更新。
以最新的Nupdate帧作为观测量，对于任意相邻两帧i和i+1,计算：
Qi,i+1＝(QL(qbi,bi+1)-QR(qci,ci+1))。
计算
Figure BDA0002418099640000244
其中
qbc：更新前相机与惯性测量单元的相对旋转角。
将计算结果与阈值thrupdateR比较。如果大于阈值thrupdateR则需要重新对相对旋转角进行标定。转步骤M3.2；否则转步骤M3.3；
步骤M3.2:基于信息量对参与标定的观测量进行筛选。利用筛选后的观测量对相机与惯性测量单元的相对旋转角进行标定；
对最新的Nupdate帧中的任意相邻两帧，计算观测量包含的信息量度量
Figure BDA0002418099640000251
挑选信息量度量最大的Nwin个观测量。利用这些观测量组成观测矩阵Q，利用最小二乘法求解相机与惯性测量单元的相对旋转角
Figure BDA0002418099640000252
Figure BDA0002418099640000253
步骤M3.3:判断相机与惯性测量单元的相对平移量标定值是否需要更新。
以最新的Nupdate帧作为观测量，利用任意相邻三帧i和i+1和i+2的观测量通过视觉与惯性融合的SLAM算法得到各帧相机在世界坐标系下的旋转角Rwc，相邻两帧的相对平移量预积分ΔP和相对线速度预积分Δv，计算：
Figure BDA0002418099640000254
Figure BDA0002418099640000255
计算
Figure BDA0002418099640000256
其中:
Pcb为更新前相机与惯性测量单元的相对平移量。
将计算结果与阈值thrupdateP比较。如果大于阈值thrupdateP则需要重新对相对平移量进行标定，转步骤M3.4；否则回到步骤M3.1；
步骤M3.4：基于信息量对参与标定的观测量进行筛选。利用筛选后的观测量对相机与惯性测量单元的相对平移量进行标定。
对最新的Nupdate帧中的任意相邻两帧，计算观测量包含的信息量度量
Figure BDA0002418099640000257
挑选信息量度量最大的Nwin个观测量。利用这些观测量组成观测矩阵V和P，利用最小二乘法求解相机与惯性测量单元的相对平移量
Figure BDA0002418099640000258
Figure BDA0002418099640000259
帧筛选时信息量窗口大小Nwin＝12，收敛性判断参数Nwinc＝15，
Figure BDA00024180996400002510
Figure BDA0002418099640000261
thrs＝0.02。判断是否需要重新标定的窗口大小Nupdate＝50，阈值大小thrupdateR＝0.2°，thrupdateP＝0.1m。
根据本发明提供的一种相机与惯性测量单元在线初始化与标定***，包括：
定义待初始化变量：
Figure BDA0002418099640000262
其中
T1,T2......TN：前N帧相机在世界坐标系中的位姿；
Figure BDA0002418099640000263
k个地图点在世界坐标系中的坐标；
s：单目相机尺度因子；
gw：世界坐标系中的重力向量；
bg：惯性测量单元角速度零漂值；
ba：惯性测量单元加速度零漂值；
定义待标定变量：
θ2＝{Rbc,Pbc}，其中
Rbc：相机与惯性测量单元相对旋转角；
Pbc：相机与惯性测量单元相对平移量；
模块M1：如图1所示，纯视觉初始化，利用相机的观测量得到初始关键帧的位姿并构建初始地图；进行纯视觉初始化是进行纯视觉同步建图与定位的前提条件，在模块M2与模块M3中，各关键帧下相机的位姿来源于视觉同步定位与建图；各点是初始关键帧中观测到的点，各点坐标的确定依赖于初始关键帧位姿的确定，各帧位姿就是初始关键帧位姿。
模块M2：如图2所示，视觉与惯性联合标定与初始化，基于初始关键帧的位姿以及初始地图完成惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定；
模块M3：如图4所示，相机与惯性测量单元融合的同步定位与建图的过程中，对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定进行异常检测与更新。对异常进行检测与更新是在模块M1和模块M2初始化和初始标定完成之后，在整个***运行的过程中以预设的频率进行的，目的在于纠正随时间推移可能产生变化的标定值。
具体地，所述模块M1包括：
模块M1.1：根据进入的新关键帧，寻找符合预设条件的视差角和匹配点对数的当前帧和历史帧；
模块M1.2：利用对极约束求取当前帧和历史帧之间的相对位姿；
模块M1.3：根据得到的当前帧和历史帧之间的相对位姿，通过三角化得到当前帧和历史帧共同观测到的地图点的世界坐标系坐标，构建初始地图；
模块M1.4：利用透视n点法(PnP：Perspective-n-Point)将初始地图中的点投影到各关键帧的像素平面上，求解各关键帧在世界坐标系中的位姿。
具体地，所述模块M2包括：
模块M2.1：基于信息量对相机与惯性测量单元的观测量进行筛选，筛选出最大化总信息量的预设数量的观测量；利用筛选后得到的相机与惯性测量单元的观测量进行惯性测量单元角速度零漂值的初始化和相机与惯性测量单元相对旋转角的标定，直到判断相机与惯性测量单元的相对旋转角收敛；
模块M2.2：假设惯性测量单元加速度的零漂值为零，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量和相机与惯性测量单元的相对平移量进行粗略估计；
模块M2.3：利用粗略估计的重力向量的方向，并引入重力大小作为新的约束，对尺度因子、重力向量、相机与惯性测量单元相对平移量以及惯性测量单元中加速度计的零漂值进行精细化估计；当尺度因子和相机与惯性测量单元的相对平移量都收敛后，视觉与惯性联合初始化和标定完成；
所述信息量为相机与惯性测量单元的观测量中包括相机与惯性测量单元相对旋转角信息量。
具体地，所述模块M2.1包括：
模块M2.1.1：利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过对惯性测量单元的角速度进行积分得到相邻两帧之间惯性测量单元的相对旋转角
Figure BDA0002418099640000271
Figure BDA0002418099640000272
其中，ΔRi,i+1表示惯性测量单元角速度积分值；
Figure BDA0002418099640000273
表示惯性测量单元相对于角速度零漂值的雅可比矩阵；bg表示惯性测量单元角速度零漂值；
模块M2.1.2：利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过视觉同步定位与建图(SLAM：Simultaneous Localization and Mapping)得到相邻两帧的相机在世界坐标系下的位姿
Figure BDA0002418099640000281
利用相机与惯性测量单元的相对旋转角得到惯性测量单元的相对旋转角
Figure BDA0002418099640000282
Figure BDA0002418099640000283
其中，Rcb表示相机与惯性测量单元的相对旋转角；
模块M2.1.3：最小化利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角和利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角的误差得到惯性测量单元角速度零漂值的最优估计，即得到惯性测量单元角速度零漂值的初始化；
通过最小化上述两相对旋转角误差得到惯性测量单元角速度零漂值的最优估计
Figure BDA0002418099640000284
Figure BDA0002418099640000285
其中，N表示关键帧个数，R2表示利用相机观测量得到的惯性测量单元的相对旋转角，R1表示利用惯性测量单元角速度积分得到的惯性测量单元的相对旋转角，bi表示第i个关键帧的惯性测量单元，bi+1表示第i+1个关键帧的惯性测量单元，bg表示惯性测量单元角速度零漂值；
模块M2.1.4：对任意相邻两关键帧，观测量所包含的关于相机与惯性测量单元相对旋转角的信息量度量为iQi,i+1＝trace((QL(qbi,bi+1)-QR(qci,ci+1))T·(QL(qbi,bi+1)-QR(qci,ci+1)))，其中
其中，QL(q)：四元数q对应的左乘矩阵；QR(q)：四元数q对应的右乘矩阵；qbi,bi+1：第i帧与第i+1帧惯性测量单元的相对旋转角对应的四元数；qci,ci+1：第i帧与第i+1帧相机的相对旋转角对应的四元数；
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定。如果关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量。
模块M2.1.5：利用筛选后的观测量构建以相机和惯性测量单元相对旋转角为未知量的方程组，利用最小二乘法求解方程组，得到以四元数形式表示的相机与惯性测量单元相对旋转角的最优估计
Figure BDA0002418099640000291
即相机与惯性测量单元相对旋转角的标定；
对于任意相邻两关键帧，相机和惯性测量单元相对旋转角为未知量，满足如下方程组：
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc＝0 (4)
其中，QL表示四元数的左乘矩阵，qbi，bi+1表示第i帧和第i+1帧下惯性测量单元相对旋转角的四元数表示，QR表示四元数的右乘矩阵，qci,ci+1表示第i帧和第i+1帧下相机相对旋转角的四元数表示，qbc表示相机与惯性测量单元的相对旋转角的四元数表示；
将(QL(qbi,bi+1)-QR(qci,ci+1))表示为Qi,i+1，利用筛选后的观测量Qi,i+1组成观测矩阵Q，建立方程：
Q·qbc＝0 (5)
利用最小二乘法求解相机与惯性测量单元的相对旋转角的最优估计
Figure BDA0002418099640000292
Figure BDA0002418099640000293
模块M2.1.6：通过判断最近Nwinc个相机与惯性测量单元相对旋转角对应的欧拉角的偏航角、俯仰角和横滚角的标准差σyaw、σpitch、σroll是否小于阈值
Figure BDA0002418099640000294
来确定视觉惯性旋转角是否标定完成；当标定未完成，即相机与惯性测量单元的相对旋转角未收敛，则等待新的关键帧重复触发模块M2.1.1至模块M2.1.5执行，直至视觉惯性旋转角标定完成；
所述模块M2.2包括：
模块M2.2.1：利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角和平移量；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量和相对线速度；
构建第i帧以尺度因子s，重力向量gw和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000301
Figure BDA0002418099640000302
Figure BDA0002418099640000303
Figure BDA0002418099640000304
Figure BDA0002418099640000305
其中，假设惯性测量单元加速度零漂值ba为零；αi表示方程中尺度因子前的系数，βi表示方程中重力向量的初始化值前的系数，γi表示方程中视觉惯性平移量前的系数，ηi表示方程中的常数项系数，s表示尺度因子；gw表示重力向量的初始化值；Pcb表示视觉惯性平移量；Rwc表示相机在世界坐标系中的旋转角；Pwc相机在世界坐标系中的平移量；
Figure BDA0002418099640000306
表示第i+1帧相机在世界坐标系中的平移量，
Figure BDA0002418099640000307
表示第i帧相机在世界坐标系中的平移量，dti+1,i+2表示第i+1帧和第i+2帧之间的时间差，
Figure BDA0002418099640000308
表示第i+2帧相机在世界坐标系中的平移量，dti,i+1表示第i帧和第i+1帧之间的时间差，I3×3表示单位矩阵，
Figure BDA0002418099640000309
表示第i+2帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400003010
表示第i+1帧相机在世界坐标系中的旋转角，
Figure BDA00024180996400003011
表示第i帧相机在世界坐标系中的旋转角，Rcb表示相机与惯性测量单元之间的相对旋转角，ΔPi,i+1表示第i帧和i+1帧惯性测量单元的相对平移量，ΔPi+1,i+2表示第i+1帧和i+2帧惯性测量单元的相对平移量，Δvi,i+1表示第i+1帧和i+2帧惯性测量单元的相对线速度；
如图3所示，模块M2.2.2：假设Ai＝[αi βi γi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA00024180996400003012
设置信息量窗口大小为Nwin，当关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于等于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
模块M2.2.3：假设
Figure BDA00024180996400003013
Bi＝ηi，对于每相邻3帧，有方程Ai·x＝Bi；利用筛选后的信息量构建信息矩阵A和常数矩阵B,构建方程：
A·x＝B (8)
利用最小二乘法求解尺度因子的最优估计s*，重力向量的最优估计gw *和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000311
所述模块M2.3包括：引入重力向量大小为G＝9.8为约束，得到尺度因子s、重力向量gw的初始化值和视觉惯性平移量Pcb的标定值；
根据得到的重力向量gw得到重力的旋转角：
Figure BDA0002418099640000312
其中，其中，RWI表示重力在世界坐标系中的旋转角，
Figure BDA0002418099640000313
表示与重力向量
Figure BDA0002418099640000314
和固定向量gI垂直的单位向量，θ表示与重力向量
Figure BDA0002418099640000315
和固定向量gI之间的夹角，gI表示固定向量[0,0,-1]；将重力向量用旋转角和重力大小表示并进行一阶泰勒展开：
Figure BDA0002418099640000316
其中，δθ表示重力向量旋转角的更新值；G表示重力大小，δθx表示重力向量旋转角的更新值在世界坐标系的x轴下的分量，δθy表示重力向量旋转角的更新值在世界坐标系的y轴下的分量；
利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角Rwc和平移量Pwc；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量ΔPi,i+1和相对线速度Δvi,i+1；构建第i帧以尺度因子s，重力向量gw，惯性测量单元加速度零漂值ba和相机与惯性测量单元相对平移量Pcb的方程：
Figure BDA0002418099640000321
Figure BDA0002418099640000322
Figure BDA0002418099640000323
Figure BDA0002418099640000324
Figure BDA0002418099640000325
Figure BDA0002418099640000326
其中，λi表示方程中尺度因子前的系数，μi表示方程中重力向量旋转角的更新值前的系数，ωi表示方程中惯性测量单元加速度零漂值前的系数，υi表示方程中相机与惯性测量单元相对平移量前的系数，ρi表示方程中的常数项系数，
Figure BDA0002418099640000327
表示第i帧和第i+2帧的惯性测量单元的相对线速度相比于惯性测量单元加速度零漂值的雅可比矩阵，dt12表示第i帧和第i+1帧之间的时间差，dt23表示第i+1帧和i+2帧之间的时间差，
Figure BDA0002418099640000328
表示第i+1帧和第i+2帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，
Figure BDA0002418099640000329
表示第i帧和第i+1帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，小标[:,1:2]表示只取矩阵的前两列；
假设Ci＝[λi μi ωi υi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure BDA00024180996400003210
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
构建方程组并利用最小二乘法求解
假设
Figure BDA00024180996400003211
Di＝ρi，对于每相邻3帧，有方程Ci·y＝Di；利用筛选后的观测量构建观测矩阵C和常数矩阵D，构建方程：
C·y＝D (13)
利用最小二乘法求解尺度因子的最优估计s*，重力向量旋转角的更新值的最优估计δθ*，惯性测量单元加速度零漂值最优估计
Figure BDA0002418099640000331
和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure BDA0002418099640000332
通过判断最近Nwinc个视觉惯性平移量的各维度值所对应的标准差σx、σy、σz是否小于阈值
Figure BDA0002418099640000333
来确定视觉惯性平移量是否标定完成；通过判断最近Nwinc个尺度因子的标准差σs是否小于阈值thrs来确定视觉惯性初始化是否完成；当视觉惯性平移量标定未完成或视觉惯性初始化未完成，则等待新的关键帧后重复触发模块M2.2至模块M2.3执行直至视觉与惯性联合初始化和标定完成。
具体地，所述模块M3包括：在相机与惯性测量单元融合的同步定位与建图过程中，每隔预设值周期判断相机与惯性测量单元相对位姿是否改变，当改变时，则根据最新帧中的任意相邻两帧对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量进行重新标定；
根据比较相机与惯性测量单元的观测量和相机与惯性测量单元相对旋转角的初始标定值之间的一致性误差是否超过阈值判断相机与惯性相对旋转角标定值是否需要更新。
模块M3.1：判断相机与惯性相对旋转角标定值是否需要更新。
以最新的Nupdate帧作为观测量，对于任意相邻两帧i和i+1,计算：
Qi,i+1＝(QL(qbi,bi+1)-QR(qci,ci+1))。
计算
Figure BDA0002418099640000334
其中
qbc：更新前相机与惯性测量单元的相对旋转角。
将计算结果与阈值thrupdateR比较。如果大于阈值thrupdateR则需要重新对相对旋转角进行标定。转模块M3.2；否则转模块M3.3；
模块M3.2:基于信息量对参与标定的观测量进行筛选。利用筛选后的观测量对相机与惯性测量单元的相对旋转角进行标定；
对最新的Nupdate帧中的任意相邻两帧，计算观测量包含的信息量度量
Figure BDA0002418099640000341
挑选信息量度量最大的Nwin个观测量。利用这些观测量组成观测矩阵Q，利用最小二乘法求解相机与惯性测量单元的相对旋转角
Figure BDA0002418099640000342
Figure BDA0002418099640000343
模块M3.3:判断相机与惯性测量单元的相对平移量标定值是否需要更新。
以最新的Nupdate帧作为观测量，利用任意相邻三帧i和i+1和i+2的观测量通过视觉与惯性融合的SLAM算法得到各帧相机在世界坐标系下的旋转角Rwc，相邻两帧的相对平移量预积分ΔP和相对线速度预积分Δv，计算：
Figure BDA0002418099640000344
Figure BDA0002418099640000345
计算
Figure BDA0002418099640000346
其中:
Pcb为更新前相机与惯性测量单元的相对平移量。
将计算结果与阈值thrupdateP比较。如果大于阈值thrupdateP则需要重新对相对平移量进行标定，转模块M3.4；否则回到模块M3.1；
模块M3.4：基于信息量对参与标定的观测量进行筛选。利用筛选后的观测量对相机与惯性测量单元的相对平移量进行标定。
对最新的Nupdate帧中的任意相邻两帧，计算观测量包含的信息量度量
Figure BDA0002418099640000347
挑选信息量度量最大的Nwin个观测量。利用这些观测量组成观测矩阵V和P，利用最小二乘法求解相机与惯性测量单元的相对平移量
Figure BDA0002418099640000348
Figure BDA0002418099640000349
帧筛选时信息量窗口大小Nwin＝12，收敛性判断参数Nwinc＝15，
Figure BDA00024180996400003410
thrPcb＝0.01m，thrs＝0.02。判断是否需要重新标定的窗口大小Nupdate＝50，阈值大小thrupdateR＝0.2°，thrupdateP＝0.1m。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的***、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的***、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的***、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种相机与惯性测量单元在线初始化与标定方法，其特征在于，包括：
步骤M1：纯视觉初始化，利用相机的观测量得到初始关键帧的位姿并构建初始地图；
步骤M2：视觉与惯性联合标定与初始化，基于初始关键帧的位姿以及初始地图完成惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定；
步骤M3：相机与惯性测量单元融合的同步定位与建图的过程中，对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定进行异常检测与更新。
2.根据权利要求1所述的相机与惯性测量单元在线初始化与标定方法，其特征在于，所述步骤M1包括：
步骤M1.1：根据进入的新关键帧，寻找符合预设条件的视差角和匹配点对数的当前帧和历史帧；
步骤M1.2：利用对极约束求取当前帧和历史帧之间的相对位姿；
步骤M1.3：根据得到的当前帧和历史帧之间的相对位姿，通过三角化得到当前帧和历史帧共同观测到的地图点的世界坐标系坐标，构建初始地图；
步骤M1.4：利用透视n点法将初始地图中的点投影到各关键帧的像素平面上，求解各关键帧在世界坐标系中的位姿。
3.根据权利要求1所述的相机与惯性测量单元在线初始化与标定方法，其特征在于，所述步骤M2包括：
步骤M2.1：基于信息量对相机与惯性测量单元的观测量进行筛选，筛选出最大化总信息量的预设数量的观测量；利用筛选后得到的相机与惯性测量单元的观测量进行惯性测量单元角速度零漂值的初始化和相机与惯性测量单元相对旋转角的标定，直到判断相机与惯性测量单元的相对旋转角收敛；
步骤M2.2：假设惯性测量单元加速度的零漂值为零，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量和相机与惯性测量单元的相对平移量进行粗略估计；
步骤M2.3：利用粗略估计的重力向量的方向，并引入重力大小作为新的约束，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量、相机与惯性测量单元相对平移量以及惯性测量单元中加速度计的零漂值进行精细化估计；当尺度因子和相机与惯性测量单元的相对平移量都收敛后，视觉与惯性联合初始化和标定完成；
所述信息量为相机与惯性测量单元的观测量中包括相机与惯性测量单元相对旋转角信息量。
4.根据权利要求3所述的相机与惯性测量单元在线初始化与标定方法，其特征在于，所述步骤M2.1包括：
步骤M2.1.1：利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过对惯性测量单元的角速度进行积分得到相邻两帧之间惯性测量单元的相对旋转角
Figure FDA0002418099630000021
Figure FDA0002418099630000022
其中，ΔRi,i+1表示惯性测量单元角速度积分值；
Figure FDA0002418099630000023
表示惯性测量单元相对于角速度零漂值的雅可比矩阵；bg表示惯性测量单元角速度零漂值；
步骤M2.1.2：利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过视觉同步定位与建图得到相邻两帧的相机在世界坐标系下的位姿
Figure FDA0002418099630000024
利用相机与惯性测量单元的相对旋转角得到惯性测量单元的相对旋转角
Figure FDA0002418099630000025
Figure FDA0002418099630000026
其中，Rcb表示相机与惯性测量单元的相对旋转角；
步骤M2.1.3：最小化利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角和利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角的误差得到惯性测量单元角速度零漂值的最优估计，即得到惯性测量单元角速度零漂值的初始化；
通过最小化上述两相对旋转角误差得到惯性测量单元角速度零漂值的最优估计
Figure FDA0002418099630000027
Figure FDA0002418099630000028
其中，N表示关键帧个数，R2表示利用相机观测量得到的惯性测量单元的相对旋转角，R1表示利用惯性测量单元角速度积分得到的惯性测量单元的相对旋转角，bi表示第i个关键帧的惯性测量单元，bi+1表示第i+1个关键帧的惯性测量单元，bg表示惯性测量单元角速度零漂值；
步骤M2.1.4：对任意相邻两关键帧，观测量所包含的关于相机与惯性测量单元相对旋转角的信息量度量为：
iQi,i+1＝trace((QL(qbi,bi+1)-QR(qci,ci+1))T·(QL(qbi,bi+1)-QR(qci,ci+1))) (4)
其中，QL(q)表示四元数q对应的左乘矩阵；QR(q)表示四元数q对应的右乘矩阵；qbi,bi+1表示第i帧与第i+1帧惯性测量单元的相对旋转角对应的四元数；qci,ci+1表示第i帧与第i+1帧相机的相对旋转角对应的四元数；
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定。如果关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
步骤M2.1.5：利用筛选后的观测量构建以相机和惯性测量单元相对旋转角为未知量的方程组，利用最小二乘法求解方程组，得到以四元数形式表示的相机与惯性测量单元相对旋转角的最优估计
Figure FDA0002418099630000031
即相机与惯性测量单元相对旋转角的标定；
对于任意相邻两关键帧，相机和惯性测量单元相对旋转角为未知量，满足如下方程组：
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc＝0 (5)
其中，QL表示四元数的左乘矩阵，qbi，bi+1表示第i帧和第i+1帧下惯性测量单元相对旋转角的四元数表示，QR表示四元数的右乘矩阵，qci,ci+1表示第i帧和第i+1帧下相机相对旋转角的四元数表示，qbc表示相机与惯性测量单元的相对旋转角的四元数表示；
将(QL(qbi,bi+1)-QR(qci,ci+1))表示为Qi,i+1，利用筛选后的观测量Qi,i+1组成观测矩阵Q，建立方程：
Q·qbc＝0 (6)
利用最小二乘法求解相机与惯性测量单元的相对旋转角的最优估计
Figure FDA0002418099630000032
Figure FDA0002418099630000033
步骤M2.1.6：通过判断最近Nwinc个相机与惯性测量单元相对旋转角对应的欧拉角的偏航角、俯仰角和横滚角的标准差σyaw、σpitch、σroll是否小于阈值
Figure FDA0002418099630000041
来确定视觉惯性旋转角是否标定完成；当标定未完成，即相机与惯性测量单元的相对旋转角未收敛，则等待新的关键帧重复执行步骤M2.1.1至步骤M2.1.5，直至视觉惯性旋转角标定完成；
所述步骤M2.2包括：
步骤M2.2.1：利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角和平移量；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量和相对线速度；
构建第i帧以尺度因子s，重力向量gw和相机与惯性测量单元相对平移量Pcb的方程：
Figure FDA0002418099630000042
Figure FDA0002418099630000043
Figure FDA0002418099630000044
Figure FDA0002418099630000045
Figure FDA0002418099630000046
其中，假设惯性测量单元加速度零漂值ba为零；αi表示方程中尺度因子前的系数，βi表示方程中重力向量的初始化值前的系数，γi表示方程中视觉惯性平移量前的系数，ηi表示方程中的常数项系数，s表示尺度因子；gw表示重力向量的初始化值；Pcb表示视觉惯性平移量；Rwc表示相机在世界坐标系中的旋转角；Pwc相机在世界坐标系中的平移量；
Figure FDA0002418099630000047
表示第i+1帧相机在世界坐标系中的平移量，
Figure FDA0002418099630000048
表示第i帧相机在世界坐标系中的平移量，dti+1,i+2表示第i+1帧和第i+2帧之间的时间差，
Figure FDA0002418099630000049
表示第i+2帧相机在世界坐标系中的平移量，dti,i+1表示第i帧和第i+1帧之间的时间差，I3×3表示单位矩阵，
Figure FDA00024180996300000410
表示第i+2帧相机在世界坐标系中的旋转角，
Figure FDA00024180996300000411
表示第i+1帧相机在世界坐标系中的旋转角，
Figure FDA00024180996300000412
表示第i帧相机在世界坐标系中的旋转角，Rcb表示相机与惯性测量单元之间的相对旋转角，ΔPi,i+1表示第i帧和i+1帧惯性测量单元的相对平移量，ΔPi+1,i+2表示第i+1帧和i+2帧惯性测量单元的相对平移量，Δvi,i+1表示第i+1帧和i+2帧惯性测量单元的相对线速度；
步骤M2.2.2：假设Ai＝[αi βi γi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure FDA0002418099630000051
设置信息量窗口大小为Nwin，当关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于等于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
步骤M2.2.3：假设
Figure FDA0002418099630000052
Bi＝ηi，对于每相邻3帧，有方程Ai·x＝Bi；利用筛选后的信息量构建信息矩阵A和常数矩阵B,构建方程：
A·x＝B (9)
利用最小二乘法求解尺度因子的最优估计s*，重力向量的最优估计gw *和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure FDA0002418099630000053
所述步骤M2.3包括：引入预设值大小的重力向量为约束，得到尺度因子s、重力向量gw的初始化值和视觉惯性平移量Pcb的标定值；
根据得到的重力向量
Figure FDA0002418099630000054
得到重力的旋转角：
Figure FDA0002418099630000055
其中，RWI表示重力在世界坐标系中的旋转角，
Figure FDA0002418099630000056
表示与重力向量
Figure FDA0002418099630000057
和固定向量gI垂直的单位向量，θ表示与重力向量
Figure FDA0002418099630000058
和固定向量gI之间的夹角，gI表示固定向量；将重力向量用旋转角和重力大小表示并进行一阶泰勒展开：
Figure FDA0002418099630000059
其中，δθ表示重力向量旋转角的更新值；G表示重力大小，δθx表示重力向量旋转角的更新值在世界坐标系的x轴下的分量，δθy表示重力向量旋转角的更新值在世界坐标系的y轴下的分量；
利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角Rwc和平移量Pwc；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量ΔPi,i+1和相对线速度Δvi,i+1；构建第i帧以尺度因子s，重力向量gw，惯性测量单元加速度零漂值ba和相机与惯性测量单元相对平移量Pcb的方程：
Figure FDA0002418099630000061
Figure FDA0002418099630000062
Figure FDA0002418099630000063
Figure FDA0002418099630000064
Figure FDA0002418099630000065
Figure FDA0002418099630000066
其中，λi表示方程中尺度因子前的系数，μi表示方程中重力向量旋转角的更新值前的系数，ωi表示方程中惯性测量单元加速度零漂值前的系数，υi表示方程中相机与惯性测量单元相对平移量前的系数，ρi表示方程中的常数项系数，
Figure FDA0002418099630000067
表示第i帧和第i+2帧的惯性测量单元的相对线速度相比于惯性测量单元加速度零漂值的雅可比矩阵，dt12表示第i帧和第i+1帧之间的时间差，dt23表示第i+1帧和i+2帧之间的时间差，
Figure FDA0002418099630000068
表示第i+1帧和第i+2帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，
Figure FDA0002418099630000069
表示第i帧和第i+1帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，小标[:,1:2]表示只取矩阵的前两列；
假设Ci＝[λi μi ωi υi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure FDA00024180996300000610
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
构建方程组并利用最小二乘法求解
假设
Figure FDA0002418099630000071
Di＝ρi，对于每相邻3帧，有方程Ci·y＝Di；利用筛选后的观测量构建观测矩阵C和常数矩阵D，构建方程：
C·y＝D (14)
利用最小二乘法求解尺度因子的最优估计s*，重力向量旋转角的更新值的最优估计δθ*，惯性测量单元加速度零漂值最优估计
Figure FDA0002418099630000072
和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure FDA0002418099630000073
通过判断最近Nwinc个视觉惯性平移量的各维度值所对应的标准差σx、σy、σz是否小于阈值
Figure FDA0002418099630000074
来确定视觉惯性平移量是否标定完成；通过判断最近Nwinc个尺度因子的标准差σs是否小于阈值thrs来确定视觉惯性初始化是否完成；当视觉惯性平移量标定未完成或视觉惯性初始化未完成，则等待新的关键帧后重复执行步骤M2.2至步骤M2.3直至视觉与惯性联合初始化和标定完成。
5.根据权利要求1所述的相机与惯性测量单元在线初始化与标定方法，其特征在于，所述步骤M3包括：在相机与惯性测量单元融合的同步定位与建图过程中，每隔预设值周期判断相机与惯性测量单元相对位姿是否改变，当改变时，则根据最新帧中的任意相邻两帧对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量进行重新标定；
步骤M3.1：根据比较相机与惯性测量单元的观测量和相机与惯性测量单元相对旋转角的初始标定值之间的一致性误差是否超过阈值判断相机与惯性相对旋转角标定值是否需要更新。
6.一种相机与惯性测量单元在线初始化与标定***，其特征在于，包括：
模块M1：纯视觉初始化，利用相机的观测量得到初始关键帧的位姿并构建初始地图；
模块M2：视觉与惯性联合标定与初始化，基于初始关键帧的位姿以及初始地图完成惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定；
模块M3：相机与惯性测量单元融合的同步定位与建图的过程中，对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量的标定进行异常检测与更新。
7.根据权利要求6所述的相机与惯性测量单元在线初始化与标定***，其特征在于，所述模块M1包括：
模块M1.1：根据进入的新关键帧，寻找符合预设条件的视差角和匹配点对数的当前帧和历史帧；
模块M1.2：利用对极约束求取当前帧和历史帧之间的相对位姿；
模块M1.3：根据得到的当前帧和历史帧之间的相对位姿，通过三角化得到当前帧和历史帧共同观测到的地图点的世界坐标系坐标，构建初始地图；
模块M1.4：利用透视n点法将初始地图中的点投影到各关键帧的像素平面上，求解各关键帧在世界坐标系中的位姿。
8.根据权利要求6所述的相机与惯性测量单元在线初始化与标定***，其特征在于，所述模块M2包括：
模块M2.1：基于信息量对相机与惯性测量单元的观测量进行筛选，筛选出最大化总信息量的预设数量的观测量；利用筛选后得到的相机与惯性测量单元的观测量进行惯性测量单元角速度零漂值的初始化和相机与惯性测量单元相对旋转角的标定，直到判断相机与惯性测量单元的相对旋转角收敛；
模块M2.2：假设惯性测量单元加速度的零漂值为零，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量和相机与惯性测量单元的相对平移量进行粗略估计；
模块M2.3：利用粗略估计的重力向量的方向，并引入重力大小作为新的约束，基于信息量对相机与惯性测量单元的观测量进行筛选，保留最大化总信息量的固定数量的观测量，对尺度因子、重力向量、相机与惯性测量单元相对平移量以及惯性测量单元中加速度计的零漂值进行精细化估计；当尺度因子和相机与惯性测量单元的相对平移量都收敛后，视觉与惯性联合初始化和标定完成；
所述信息量为相机与惯性测量单元的观测量中包括相机与惯性测量单元相对旋转角信息量。
9.根据权利要求8所述的相机与惯性测量单元在线初始化与标定***，其特征在于，所述模块M2.1包括：
模块M2.1.1：利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过对惯性测量单元的角速度进行积分得到相邻两帧之间惯性测量单元的相对旋转角
Figure FDA0002418099630000091
Figure FDA0002418099630000092
其中，ΔRi,i+1表示惯性测量单元角速度积分值；
Figure FDA0002418099630000093
表示惯性测量单元相对于角速度零漂值的雅可比矩阵；bg表示惯性测量单元角速度零漂值；
模块M2.1.2：利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角；
通过视觉同步定位与建图得到相邻两帧的相机在世界坐标系下的位姿
Figure FDA0002418099630000094
利用相机与惯性测量单元的相对旋转角得到惯性测量单元的相对旋转角
Figure FDA0002418099630000095
Figure FDA0002418099630000096
其中，Rcb表示相机与惯性测量单元的相对旋转角；
模块M2.1.3：最小化利用惯性测量单元的观测量得到相邻两帧之间惯性测量单元的相对旋转角和利用相机的观测量得到相邻两帧之间惯性测量单元的相对旋转角的误差得到惯性测量单元角速度零漂值的最优估计，即得到惯性测量单元角速度零漂值的初始化；
通过最小化上述两相对旋转角误差得到惯性测量单元角速度零漂值的最优估计
Figure FDA0002418099630000097
Figure FDA0002418099630000098
其中，N表示关键帧个数，R2表示利用相机观测量得到的惯性测量单元的相对旋转角，R1表示利用惯性测量单元角速度积分得到的惯性测量单元的相对旋转角，bi表示第i个关键帧的惯性测量单元，bi+1表示第i+1个关键帧的惯性测量单元，bg表示惯性测量单元角速度零漂值；
模块M2.1.4：对任意相邻两关键帧，观测量所包含的关于相机与惯性测量单元相对旋转角的信息量度量为：
Figure FDA0002418099630000099
其中，QL(q)表示四元数q对应的左乘矩阵；QR(q)表示四元数q对应的右乘矩阵；qbi,bi+1表示第i帧与第i+1帧惯性测量单元的相对旋转角对应的四元数；qci,ci+1表示第i帧与第i+1帧相机的相对旋转角对应的四元数；
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定。如果关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
模块M2.1.5：利用筛选后的观测量构建以相机和惯性测量单元相对旋转角为未知量的方程组，利用最小二乘法求解方程组，得到以四元数形式表示的相机与惯性测量单元相对旋转角的最优估计
Figure FDA0002418099630000101
即相机与惯性测量单元相对旋转角的标定；
对于任意相邻两关键帧，相机和惯性测量单元相对旋转角为未知量，满足如下方程组：
(QL(qbi,bi+1)-QR(qci,ci+1))·qbc＝0 (5)
其中，QL表示四元数的左乘矩阵，qbi，bi+1表示第i帧和第i+1帧下惯性测量单元相对旋转角的四元数表示，QR表示四元数的右乘矩阵，qci,ci+1表示第i帧和第i+1帧下相机相对旋转角的四元数表示，qbc表示相机与惯性测量单元的相对旋转角的四元数表示；
将(QL(qbi,bi+1)-QR(qci,ci+1))表示为Qi,i+1，利用筛选后的观测量Qi,i+1组成观测矩阵Q，建立方程：
Q·qbc＝0 (6)
利用最小二乘法求解相机与惯性测量单元的相对旋转角的最优估计
Figure FDA0002418099630000102
Figure FDA0002418099630000103
模块M2.1.6：通过判断最近Nwinc个相机与惯性测量单元相对旋转角对应的欧拉角的偏航角、俯仰角和横滚角的标准差σyaw、σpitch、σroll是否小于阈值
Figure FDA0002418099630000104
来确定视觉惯性旋转角是否标定完成；当标定未完成，即相机与惯性测量单元的相对旋转角未收敛，则等待新的关键帧重复触发模块M2.1.1至模块M2.1.5执行，直至视觉惯性旋转角标定完成；
所述模块M2.2包括：
模块M2.2.1：利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角和平移量；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量和相对线速度；
构建第i帧以尺度因子s，重力向量gw和相机与惯性测量单元相对平移量Pcb的方程：
Figure FDA0002418099630000111
Figure FDA0002418099630000112
Figure FDA0002418099630000113
Figure FDA0002418099630000114
Figure FDA0002418099630000115
其中，假设惯性测量单元加速度零漂值ba为零；αi表示方程中尺度因子前的系数，βi表示方程中重力向量的初始化值前的系数，γi表示方程中视觉惯性平移量前的系数，ηi表示方程中的常数项系数，s表示尺度因子；gw表示重力向量的初始化值；Pcb表示视觉惯性平移量；Rwc表示相机在世界坐标系中的旋转角；Pwc相机在世界坐标系中的平移量；
Figure FDA0002418099630000116
表示第i+1帧相机在世界坐标系中的平移量，
Figure FDA0002418099630000117
表示第i帧相机在世界坐标系中的平移量，dti+1,i+2表示第i+1帧和第i+2帧之间的时间差，
Figure FDA0002418099630000118
表示第i+2帧相机在世界坐标系中的平移量，dti,i+1表示第i帧和第i+1帧之间的时间差，I3×3表示单位矩阵，
Figure FDA0002418099630000119
表示第i+2帧相机在世界坐标系中的旋转角，
Figure FDA00024180996300001110
表示第i+1帧相机在世界坐标系中的旋转角，
Figure FDA00024180996300001111
表示第i帧相机在世界坐标系中的旋转角，Rcb表示相机与惯性测量单元之间的相对旋转角，ΔPi,i+1表示第i帧和i+1帧惯性测量单元的相对平移量，ΔPi+1,i+2表示第i+1帧和i+2帧惯性测量单元的相对平移量，Δvi,i+1表示第i+1帧和i+2帧惯性测量单元的相对线速度；
模块M2.2.2：假设Ai＝[αi βi γi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure FDA00024180996300001112
设置信息量窗口大小为Nwin，当关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于等于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
模块M2.2.3：假设
Figure FDA0002418099630000121
Bi＝ηi，对于每相邻3帧，有方程Ai·x＝Bi；利用筛选后的信息量构建信息矩阵A和常数矩阵B,构建方程：
A·x＝B (9)
利用最小二乘法求解尺度因子的最优估计s*，重力向量的最优估计gw *和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure FDA0002418099630000122
所述模块M2.3包括：引入预设值大小的重力向量为约束，得到尺度因子s、重力向量gw的初始化值和视觉惯性平移量Pcb的标定值；
根据得到的重力向量gw得到重力的旋转角：
Figure FDA0002418099630000123
其中，RWI表示重力在世界坐标系中的旋转角，
Figure FDA0002418099630000124
表示与重力向量
Figure FDA0002418099630000125
和固定向量gI垂直的单位向量，θ表示与重力向量
Figure FDA0002418099630000126
和固定向量gI之间的夹角，gI表示固定向量；将重力向量用旋转角和重力大小表示并进行一阶泰勒展开：
Figure FDA0002418099630000127
其中，δθ表示重力向量旋转角的更新值；G表示重力大小，δθx表示重力向量旋转角的更新值在世界坐标系的x轴下的分量，δθy表示重力向量旋转角的更新值在世界坐标系的y轴下的分量；
利用视觉同步定位与建图算法根据相机观测量得到相机在世界坐标系中的旋转角Rwc和平移量Pwc；利用预积分算法根据惯性测量单元观测量得到惯性测量单元相邻两关键帧的相对平移量ΔPi,i+1和相对线速度Δvi,i+1；构建第i帧以尺度因子s，重力向量gw，惯性测量单元加速度零漂值ba和相机与惯性测量单元相对平移量Pcb的方程：
Figure FDA0002418099630000131
Figure FDA0002418099630000132
Figure FDA0002418099630000133
Figure FDA0002418099630000134
Figure FDA0002418099630000135
Figure FDA0002418099630000136
其中，λi表示方程中尺度因子前的系数，μi表示方程中重力向量旋转角的更新值前的系数，ωi表示方程中惯性测量单元加速度零漂值前的系数，υi表示方程中相机与惯性测量单元相对平移量前的系数，ρi表示方程中的常数项系数，
Figure FDA0002418099630000137
表示第i帧和第i+2帧的惯性测量单元的相对线速度相比于惯性测量单元加速度零漂值的雅可比矩阵，dt12表示第i帧和第i+1帧之间的时间差，dt23表示第i+1帧和i+2帧之间的时间差，
Figure FDA0002418099630000138
表示第i+1帧和第i+2帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，
Figure FDA0002418099630000139
表示第i帧和第i+1帧之间的惯性测量单元的相对平移量相比于惯性测量单元加速度零漂值的雅可比矩阵，小标[:,1:2]表示只取矩阵的前两列；
假设Ci＝[λi μi ωi υi]，相邻3个关键帧构成的观测量中包含未知量的信息量度量
Figure FDA00024180996300001310
设置信息量窗口大小为Nwin，如果关键帧总数小于Nwin，则将最新的关键帧加入到序列中用于初始化和标定；当关键帧总数大于Nwin时，则将当前的信息量度量与已有序列中的信息量度量进行比较，剔除信息量度量最小的观测量；
构建方程组并利用最小二乘法求解
假设
Figure FDA00024180996300001311
Di＝ρi，对于每相邻3帧，有方程Ci·y＝Di；利用筛选后的观测量构建观测矩阵C和常数矩阵D，构建方程：
C·y＝D (14)
利用最小二乘法求解尺度因子的最优估计s*，重力向量旋转角的更新值的最优估计δθ*，惯性测量单元加速度零漂值最优估计
Figure FDA0002418099630000141
和相机与惯性测量单元相对平移量的最优估计Pcb *：
Figure FDA0002418099630000142
通过判断最近Nwinc个视觉惯性平移量的各维度值所对应的标准差σx、σy、σz是否小于阈值
Figure FDA0002418099630000143
来确定视觉惯性平移量是否标定完成；通过判断最近Nwinc个尺度因子的标准差σs是否小于阈值thrs来确定视觉惯性初始化是否完成；当视觉惯性平移量标定未完成或视觉惯性初始化未完成，则等待新的关键帧后重复触发模块M2.2至模块M2.3执行直至视觉与惯性联合初始化和标定完成。
10.根据权利要求6所述的相机与惯性测量单元在线初始化与标定***，其特征在于，所述模块M3包括：在相机与惯性测量单元融合的同步定位与建图过程中，每隔预设值周期判断相机与惯性测量单元相对位姿是否改变，当改变时，则根据最新帧中的任意相邻两帧对惯性测量单元角速度零漂值的初始化与相机和惯性测量单元相对旋转角和平移量进行重新标定；
模块M3.1：根据比较相机与惯性测量单元的观测量和相机与惯性测量单元相对旋转角的初始标定值之间的一致性误差是否超过阈值判断相机与惯性相对旋转角标定值是否需要更新。
CN202010197372.3A 2020-03-19 2020-03-19 一种相机与惯性测量单元在线初始化与标定方法及*** Active CN111429524B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010197372.3A CN111429524B (zh) 2020-03-19 2020-03-19 一种相机与惯性测量单元在线初始化与标定方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010197372.3A CN111429524B (zh) 2020-03-19 2020-03-19 一种相机与惯性测量单元在线初始化与标定方法及***

Publications (2)

Publication Number Publication Date
CN111429524A true CN111429524A (zh) 2020-07-17
CN111429524B CN111429524B (zh) 2023-04-18

Family

ID=71548171

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010197372.3A Active CN111429524B (zh) 2020-03-19 2020-03-19 一种相机与惯性测量单元在线初始化与标定方法及***

Country Status (1)

Country Link
CN (1) CN111429524B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112229424A (zh) * 2020-11-16 2021-01-15 浙江商汤科技开发有限公司 视觉惯性***的参数标定方法及装置、电子设备和介质
CN112629565A (zh) * 2021-03-08 2021-04-09 中国人民解放军国防科技大学 像机与惯性测量单元旋转关系校准方法、装置和设备
CN113313763A (zh) * 2021-05-26 2021-08-27 珠海深圳清华大学研究院创新中心 一种基于神经网络的单目相机位姿优化方法及装置
CN114018291A (zh) * 2021-11-08 2022-02-08 中国科学院空天信息创新研究院 一种惯性测量单元参数的标定方法、装置
CN114199275A (zh) * 2020-09-18 2022-03-18 阿里巴巴集团控股有限公司 传感器的参数确定方法和装置
CN115451958A (zh) * 2022-11-10 2022-12-09 中国人民解放军国防科技大学 基于相对旋转角度的相机绝对姿态优化方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827315A (zh) * 2018-08-17 2018-11-16 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及***
WO2019169540A1 (zh) * 2018-03-06 2019-09-12 斯坦德机器人（深圳）有限公司 紧耦合视觉slam的方法、终端及计算机可读存储介质
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110726406A (zh) * 2019-06-03 2020-01-24 北京建筑大学 一种改进的非线性优化单目惯导slam的方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及***
WO2019169540A1 (zh) * 2018-03-06 2019-09-12 斯坦德机器人（深圳）有限公司 紧耦合视觉slam的方法、终端及计算机可读存储介质
CN108827315A (zh) * 2018-08-17 2018-11-16 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN110726406A (zh) * 2019-06-03 2020-01-24 北京建筑大学 一种改进的非线性优化单目惯导slam的方法
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王昱欣等: "软体机器人手眼视觉/形状混合控制", 机器人 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114199275A (zh) * 2020-09-18 2022-03-18 阿里巴巴集团控股有限公司 传感器的参数确定方法和装置
CN112229424A (zh) * 2020-11-16 2021-01-15 浙江商汤科技开发有限公司 视觉惯性***的参数标定方法及装置、电子设备和介质
CN112229424B (zh) * 2020-11-16 2022-04-22 浙江商汤科技开发有限公司 视觉惯性***的参数标定方法及装置、电子设备和介质
CN112629565A (zh) * 2021-03-08 2021-04-09 中国人民解放军国防科技大学 像机与惯性测量单元旋转关系校准方法、装置和设备
CN112629565B (zh) * 2021-03-08 2021-06-01 中国人民解放军国防科技大学 像机与惯性测量单元旋转关系校准方法、装置和设备
CN113313763A (zh) * 2021-05-26 2021-08-27 珠海深圳清华大学研究院创新中心 一种基于神经网络的单目相机位姿优化方法及装置
CN114018291A (zh) * 2021-11-08 2022-02-08 中国科学院空天信息创新研究院 一种惯性测量单元参数的标定方法、装置
CN115451958A (zh) * 2022-11-10 2022-12-09 中国人民解放军国防科技大学 基于相对旋转角度的相机绝对姿态优化方法
CN115451958B (zh) * 2022-11-10 2023-02-03 中国人民解放军国防科技大学 基于相对旋转角度的相机绝对姿态优化方法

Also Published As

Publication number Publication date
CN111429524B (zh) 2023-04-18

Similar Documents

Publication Publication Date Title
CN111429524B (zh) 一种相机与惯性测量单元在线初始化与标定方法及***
CN111415387B (zh) 相机位姿确定方法、装置、电子设备及存储介质
Chilian et al. Multisensor data fusion for robust pose estimation of a six-legged walking robot
EP2728548B1 (en) Automated frame of reference calibration for augmented reality
WO2020253260A1 (zh) 时间同步处理方法、电子设备及存储介质
CN110081881B (zh) 一种基于无人机多传感器信息融合技术的着舰引导方法
CN110986968B (zh) 三维重建中实时全局优化和错误回环判断的方法及装置
Yang et al. Online imu intrinsic calibration: Is it necessary?
CN112183171A (zh) 一种基于视觉信标建立信标地图方法、装置
CN111707261A (zh) 一种微型无人机高速感知和定位方法
CN113223161B (zh) 一种基于imu和轮速计紧耦合的鲁棒全景slam***和方法
JP6229041B2 (ja) 基準方向に対する移動要素の角度偏差を推定する方法
CN111754579A (zh) 多目相机外参确定方法及装置
CN112347205A (zh) 一种车辆误差状态的更新方法和装置
CN112388635B (zh) 机器人多传感器融合感知与空间定位的方法、***及装置
CN115540860A (zh) 一种多传感器融合位姿估计算法
CN110986928A (zh) 光电吊舱三轴陀螺仪漂移实时修正方法
Liu et al. High altitude monocular visual-inertial state estimation: Initialization and sensor fusion
JP2730457B2 (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
CN113436267A (zh) 视觉惯导标定方法、装置、计算机设备和存储介质
CN114812601A (zh) 视觉惯性里程计的状态估计方法、装置、电子设备
CN115797490B (zh) 基于激光视觉融合的建图方法及***
CN112712107B (zh) 一种基于优化的视觉和激光slam融合定位方法
CN111812668B (zh) 绕机检查装置及其定位方法、存储介质
CN111811501B (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