CN108629793B - 使用在线时间标定的视觉惯性测程法与设备 - Google Patents

使用在线时间标定的视觉惯性测程法与设备 Download PDF

Info

Publication number
CN108629793B
CN108629793B CN201810239278.2A CN201810239278A CN108629793B CN 108629793 B CN108629793 B CN 108629793B CN 201810239278 A CN201810239278 A CN 201810239278A CN 108629793 B CN108629793 B CN 108629793B
Authority
CN
China
Prior art keywords
image
time
imu
camera
frame
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810239278.2A
Other languages
English (en)
Other versions
CN108629793A (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.)
Institute of Automation of Chinese Academy of Science
Original Assignee
Institute of Automation of Chinese Academy of Science
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 Institute of Automation of Chinese Academy of Science filed Critical Institute of Automation of Chinese Academy of Science
Priority to CN201810239278.2A priority Critical patent/CN108629793B/zh
Publication of CN108629793A publication Critical patent/CN108629793A/zh
Application granted granted Critical
Publication of CN108629793B publication Critical patent/CN108629793B/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/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10004—Still image; Photographic image

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Studio Devices (AREA)

Abstract

本发明涉及多传感器时间标定算法领域，具体涉及一种使用在线时间标定的视觉惯性测程法与设备，目的在于提高相机位姿计算的精度。本发明的视觉惯性测程法包括：将相机图片的时间戳与IMU的时间系统粗略对齐，并把相机和IMU的数据作为输入；把由相机数据和IMU数据分别计算出的相机姿态最接近的时间延迟作为相机时间延迟的初步估计值；采用带有时间延迟参数的IMU误差项表示方法，在IMU的预积分结果中添加与时间延迟有关的修正项，将其作为非线性优化的参数，实现时间延迟的精确标定。本方法对时间延迟的标定比现有的方法更准确，对于未经硬件同步的设备，在设备快速转弯和突然停止的时候有更好的性能。

Description

使用在线时间标定的视觉惯性测程法与设备
技术领域
本发明涉及多传感器时间标定算法领域，具体涉及一种使用在线时间标定的视觉惯性测程法与设备。
背景技术
使用相机和IMU(惯性测量单元)的视觉惯性测程法近年来取得了很好的效果，因为视觉和IMU的信息有互补的特点。早期有MSCKF算法使用扩展卡尔曼滤波(EKF)实现相机跟踪，随后出现了使用非线性优化的OKVIS和VINS，以及ORB-SLAM算法的使用视觉和IMU信息的版本。这些算法被广泛地用于微型飞行器定位和增强现实。这些算法都假设相机和IMU的时间戳是同步的，但是在安卓手机上相机和IMU并没有做到硬件同步，相机的时间戳相对于它的曝光时间有时间延迟。因为这个时间延迟在每次程序运行的时候都不一样，所以无法使用离线的时间标定算法如Kalibr工具包进行时间标定。
现有的相机和IMU之间的可以实现时间标定的方法主要有以下四种：
(1)首先使用相机和IMU的数据分别计算相机和IMU的姿态，然后通过计算相关系数或相位一致性，或使用ICP算法对齐；
(2)使用非线性优化的离线标定；
(3)使用EKF算法的在线标定；
(4)使用非线性优化的在线标定。
其中，第(1)种方法因为分成两步计算，所以在对齐的时候无法利用相机和IMU姿态的精度信息，因此，一个被错误估计的姿态会对时间标定结果产生较大影响；第(2)种方法无法实现在线计算；第(3)种方法是基于EKF算法的，无法用于OKVIS、VINS、ORB-SLAM等使用非线性优化的VIO(Visual–Inertial Odometry，视觉惯性里程计)算法。
VINS最新更新的开源代码是在此之前唯一公开的使用非线性优化的在线时间标定方法。这种方法用连续两帧图像上特征点坐标的差除以两帧之间的时间间隔计算出特征点在图像上移动的速度，再用它乘以时间延迟作为特征点坐标的改正量加到特征点坐标的观测值上。使用这种方法，时间延迟就成了非线性优化的参数之一。然而，图像采集的频率显著低于IMU的量测值的频率，因此，使用上述方法计算出的特征点在图像上移动的速度也有较大的误差。
发明内容
为了解决现有技术中的上述问题，本发明提出了一种使用在线时间标定的视觉惯性测程法与设备，提高了相机位姿计算的精度。
本发明的一方面，提出一种使用在线时间标定的视觉惯性测程法，包括以下步骤：
步骤S1000，实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
步骤S3000，当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，
所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
优选地，步骤S1000具体包括：
步骤S1001，使用camera2 API实时获取相机图像，并获取相机时间基准下所述相机图像的曝光时刻时间戳；根据IMU时间基准，调整所述相机图像的曝光时刻时间戳；实时获取IMU量测值；
步骤S1002，将调整时间戳后的所述相机图像，以及获取的IMU量测值加入滑动窗口；
其中，
所述IMU量测值的获取频率高于所述相机图像的获取频率；
所述相机图像的曝光时刻时间戳根据下式调整：
Figure GDA0002622637630000031
Figure GDA0002622637630000032
表示调整后的第i帧所述相机图像的曝光时刻时间戳；
Figure GDA0002622637630000033
表示在IMU时间基准下第1帧图像对应的回调函数执行时刻的时间戳；
Figure GDA0002622637630000034
和
Figure GDA0002622637630000035
分别表示在相机时间基准下第i帧和第1帧图像的曝光时刻时间戳。
优选地，所述获取相机时间基准下所述相机图像的曝光时刻时间戳的方法为：
通过onImageAvailable()函数获取Image类型的对象，调用该对象的成员函数getTimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；
所述回调函数执行时刻的时间戳的获取方法为：
使用SystemClock类的elapsedRealtime()函数获取IMU时间基准下所述回调函数执行时刻时间戳。
优选地，步骤S3000具体包括：
步骤S3001，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为K；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于
Figure GDA0002622637630000036
和
Figure GDA0002622637630000037
之间的IMU量测值进行预积分，得到
Figure GDA0002622637630000038
和
Figure GDA0002622637630000039
其中，TK、TK-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤S1000调整后的值；
Figure GDA00026226376300000310
为所述倒数第二帧图像的时间延迟，初值为0；
步骤S3002，按照基于非线性优化的VIO算法的通用做法在局部窗口中构造代价函数，使用Dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值；
其中，
所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于非线性优化的VIO算法的通用做法进行构造；
所述IMU误差项使用下述方法进行构造：
Figure GDA0002622637630000041
其中，CIMU(x)表示IMU误差项；x由x1,x2,...,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，
Figure GDA0002622637630000042
表示协方差矩阵，通过IMU预积分计算出；
Figure GDA0002622637630000043
表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关：
Figure GDA0002622637630000044
Figure GDA0002622637630000045
为待估计的所述时间延迟，
Figure GDA0002622637630000046
和
Figure GDA0002622637630000047
分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；
Figure GDA0002622637630000048
和
Figure GDA0002622637630000049
分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；
Figure GDA00026226376300000410
和
Figure GDA00026226376300000411
分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00026226376300000412
和
Figure GDA00026226376300000413
分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00026226376300000414
为用四元数表示的在第k帧曝光时刻从世界坐标系到IMU本体坐标系的旋转；
Figure GDA00026226376300000415
为
Figure GDA00026226376300000416
的旋转矩阵形式；
Figure GDA00026226376300000417
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值，Δtk＝Tk+1-Tk；
Figure GDA00026226376300000418
和
Figure GDA00026226376300000419
的计算方法为：
Figure GDA00026226376300000420
Figure GDA00026226376300000421
Figure GDA0002622637630000051
Figure GDA0002622637630000052
表示在对当前滑动窗口内第k帧和第k+1帧之间的IMU量测值预积分的时候对时间偏移的估计值；ab,k和ae,k的计算方法分别为
Figure GDA0002622637630000053
其中
Figure GDA0002622637630000054
和
Figure GDA0002622637630000055
分别为
Figure GDA0002622637630000056
和
Figure GDA0002622637630000057
时刻IMU量测值中的加速度；
Figure GDA0002622637630000058
和
Figure GDA0002622637630000059
分别为IMU第k帧和第k+1帧图像之间IMU预积分的结果；
Figure GDA00026226376300000510
和
Figure GDA00026226376300000511
分别表示为：
Figure GDA00026226376300000512
Figure GDA00026226376300000513
Figure GDA00026226376300000514
和
Figure GDA00026226376300000515
分别为
Figure GDA00026226376300000516
和
Figure GDA00026226376300000517
时刻IMU量测值中的角速度；
Figure GDA00026226376300000518
和
Figure GDA00026226376300000519
是四元数
Figure GDA00026226376300000520
和
Figure GDA00026226376300000521
对应的旋转矩阵；
Figure GDA00026226376300000522
Figure GDA00026226376300000523
和
Figure GDA00026226376300000524
均为待优化的参数；
步骤S3003，将非线性优化求解得到的所述时间延迟
Figure GDA00026226376300000529
作为当前滑动窗口中最新图像对应的时间延迟的估计值：
Figure GDA00026226376300000525
所述时间延迟的估计值
Figure GDA00026226376300000526
在下一帧图像进入滑动窗口后，将对应于步骤S3001中
Figure GDA00026226376300000527
的值。
优选地，所述视觉惯性测程法还包括一次性执行：
步骤S2000，分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为时间延迟的初步估计值；
相应地，将步骤S2000计算得到的所述时间延迟的初步估计值作为步骤S3001中所述倒数第二帧图像的时间延迟
Figure GDA00026226376300000528
的初值。
优选地，步骤S2000具体包括：
步骤S2001，检测是否有新的IMU量测值加入滑动窗口，若是，则转至步骤S2003；否则，转至步骤S2002；
步骤S2002，检测是否有新的一帧图像加入滑动窗口，若是，则转至步骤S2004；否则，转至步骤S2001；
步骤S2003，计算转角变化量Δq(t)并存入队列，更新非线性优化的滑动窗口中的所述累计转角q；转至步骤S2005；
其中，
Figure GDA0002622637630000061
Figure GDA0002622637630000062
q为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；
Figure GDA0002622637630000063
为IMU量测值的角速度；Δt为IMU相邻两次观测的时间差；
Figure GDA0002622637630000064
表示四元数的乘法；t为当前最新IMU量测值的时间戳；
步骤S2004，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个IMU量测值中角速度差的模长的平均值
Figure GDA0002622637630000065
用该平均值来衡量角速度变化的情况；
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角：
Figure GDA0002622637630000066
其中，Δq(t′)为根据时间戳为t′的IMU量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳；
步骤S2005，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)：
Figure GDA0002622637630000067
其中，qx，qy，qz和qw表示四元数q的四个分量；
步骤S2006，若θ1<θ(qa)<θ2且
Figure GDA0002622637630000071
或
Figure GDA0002622637630000072
则转至步骤S2007；否则，转至步骤S2001；
其中，阈值θ1和θ2用来筛选出具有较好几何结构的场景；阈值ω1和ω2用来平衡时间延迟估计的及时性和可靠性，若要提高及时性则减小ω1和ω2，若要提高可靠性则增大ω1和ω2；
步骤S2007，在应用本方法的VIO算法运行的过程中，滑动窗口中的图像特征点和IMU信息会共同参与优化得到滑动窗口中的相机位姿，将这个位姿作为本步骤中仅使用图像特征点的捆绑调整的初值。只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整；
步骤S2008，若捆绑调整失败，则继续转至步骤S2001；若成功，则转至步骤S2009；
步骤S2009，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转：
Figure GDA0002622637630000073
其中，
Figure GDA0002622637630000074
和
Figure GDA0002622637630000075
分别为通过捆绑调整得到的第k帧和第k+1帧相机坐标系到世界坐标系的旋转四元数，
Figure GDA0002622637630000076
为从第k+1帧相机坐标系到第k帧相机坐标系的旋转四元数；
步骤S2010，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于Tk-td和Tk+1-td之间的IMU量测值中的角速度推算相邻两帧之间IMU本体坐标系间的旋转四元数：
Figure GDA0002622637630000077
其中，Tk和Tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为IMU量测值的时间戳；
Figure GDA0002622637630000078
表示时间戳为t的IMU量测值中的角速度；П表示四元数的连乘；
步骤S2011，结合已知的相机和IMU坐标系间的旋转四元数
Figure GDA0002622637630000079
计算相邻两帧之间相机坐标系间的旋转四元数：
Figure GDA0002622637630000081
步骤S2012，对于不同的时间延迟td，计算由相机信息和IMU信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td：
Figure GDA0002622637630000082
若
Figure GDA0002622637630000083
则记录一个时间延迟的估计值
Figure GDA0002622637630000084
其中，K为滑动窗口中包含的帧的数目；
步骤S2013，判断是否记录了预设个数的所述时间延迟的估计值
Figure GDA0002622637630000085
若是，则对所述预设个数的所述时间延迟的估计值求中值，作为最终得到的时间延迟的初步估计值；否则，转至步骤S2001。
优选地，步骤S2007中“只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整”，具体为：
使用Ceres Solver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵均为：
Figure GDA0002622637630000086
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距；
若捆绑调整完成后代价函数值小于预设的代价函数阈值，则认为捆绑调整成功，否则认为捆绑调整失败。
本发明的第二方面，提出一种存储设备，其存储有程序，所述程序适于由处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的第三方面，提出一种处理设备，包括：处理器和存储器；
所述处理器，适于执行程序；
所述存储器，适于存储该程序；
所述程序适于由所述处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的第四方面，提出一种使用在线时间标定的视觉惯性测程设备，包括：图像与IMU量测值获取模块、时间延迟与相机姿态计算模块；
所述图像与IMU量测值获取模块，配置为：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
所述时间延迟与相机姿态计算模块，配置为：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
优选地，所述视觉惯性测程设备还包括：时间延迟初值估计模块；
所述时间延迟初值估计模块，包括：分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
本发明的有益效果：
本发明所提出的使用在线时间标定的视觉惯性测程法，能够在对时间延迟没有任何先验知识的情况下，在基于非线性优化的视觉惯性测程法运行的同时估计出时间延迟，同时调整IMU的预积分时间范围，使IMU的预积分结果与图像数据在时间上保持一致。在EuRoc MAV数据集上的测试结果显示，与已有的方法相比，本方法对时间延迟的估计更准确，而且当时间延迟存在时，使用本方法的VIO算法得到的轨迹更准确。当不存在时间延迟时，本方法也不会对VIO算法得到的轨迹产生明显的负面影响。在安卓手机采集的数据上，本方法相比已有的方法能更快地得到时间延迟的较准确的估计值，使得相机图像和IMU数据的对应更准确，因此在设备快速转弯或突然停止的时候使用本方法的VIO算法有更好的表现。
附图说明
图1是本发明的使用在线时间标定的视觉惯性测程法实施例一的流程示意图；
图2是本发明的使用在线时间标定的视觉惯性测程法实施例二的流程示意图；
图3(a)-(b)是视觉惯性测程法实施例一和视觉惯性测程法实施例二中时间延迟标定值随时间变化的比较图；
其中，图3(a)表示在EuRoc MAV数据集的MH_01_easy数据上人工添加30ms的时间延迟的情况下对时间延迟的标定，图3(b)表示在安卓手机采集的数据上对时间延迟的标定；
图4(a)-(d)是本发明视觉惯性测程法实施例一和现有的时间标定方法在EuRocMAV数据集上时间标定误差随时间变化的比较图；
其中图4(a)、图4(b)、图4(c)、图4(d)分别表示在数据集上将图像的时间戳人工添加60ms、30ms、0ms、-30ms的时间延迟后时间标定的误差；
图5(a)-(b)是使用本发明视觉惯性测程法实施例一的VIO算法、使用现有的时间标定方法的VIO算法以及不进行时间标定的VIO算法在安卓手机采集的含有快速转弯和突然停止的数据上的轨迹比较图；
其中，图5(a)为含有相机突然停止的数据时对应的轨迹曲线；图5(b)为含有相机快速转弯的数据时对应的轨迹曲线；
图6是本发明的使用在线时间标定的视觉惯性测程设备实施例一的构成示意图；
图7是本发明的使用在线时间标定的视觉惯性测程设备实施例二的构成示意图。
具体实施方式
下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。
本发明提出了一种使用在线时间标定的视觉惯性测程法，通过对相机和IMU系统的时间延迟的在线精确标定，有效提高了相机跟踪的准确度。本发明用于在基于非线性优化的VIO算法例如VINS中实现时间延迟的在线标定；这些算法首先使用IMU量测值进行预积分，然后在局部滑动窗口内，通过对代价函数进行非线性优化得到相机的实时位姿；代价函数中包含重投影误差项，IMU误差项和边缘化误差项，其中IMU误差项由IMU预积分的结果和待优化参数构成。
本专利为了使时间标定的结果更精确，在IMU预积分结果中添加与时间延迟有关的项，而不在特征点坐标上添加。在测试数据集上，本算法有更高的时间标定精度。
图1是本发明的使用在线时间标定的视觉惯性测程法实施例一的流程示意图。如图1所示，本实施例的视觉惯性测程法包括以下步骤：
在步骤S1000中，进行数据采集与时间粗略对齐：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳，使其与IMU量测值数据大致位于同一时间基准下；将调整时间戳后的所述相机图像和IMU量测值作为输入。该步骤可以具体包括步骤S1001-S1002：
在步骤S1001中，在安卓系统上使用camera2 API实时获取相机图像，并获取相机时间基准下所述相机图像的曝光时刻时间戳；然后用回调函数第一次的调用时刻代替相机第一次曝光时刻，根据IMU时间基准，调整所述相机图像的曝光时刻时间戳；另外，还要实时获取IMU量测值。
其中，所述IMU量测值的获取频率高于所述相机图像的获取频率。调整相机图像的曝光时刻时间戳的方法如公式(1)所示：
Figure GDA0002622637630000111
Figure GDA0002622637630000112
表示在IMU时间基准下第1帧图像对应的回调函数执行时刻的时间戳；
Figure GDA0002622637630000113
和
Figure GDA0002622637630000114
分别表示在相机时间基准下第i帧和第1帧图像的曝光时刻时间戳；上述两种时间戳都可以直接获取。
Figure GDA0002622637630000115
表示粗略对齐后的IMU时间基准下第i帧相机图像的曝光时刻时间戳，它与实际值的偏差为IMU时间基准下第1帧图像对应的回调函数执行时刻和第1帧图像曝光时刻的时间差，一般为数十毫秒，且为正值。
本实施例中，通过onImageAvailable()函数获取Image类型的对象，调用该对象的成员函数getTimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；使用SystemClock类的elapsedRealtime()函数获取IMU时间基准下所述回调函数执行时刻时间戳。
在步骤S1002中，将调整时间戳后的所述相机图像，以及获取的IMU量测值加入滑动窗口。
在步骤S3000中，进行时间延迟的精确标定：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位姿一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位姿；其中，所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。这种方法对IMU误差项的表示比现有方法更加准确，而且利用到了采集的所有数据，因此对时间延迟的标定结果更加准确。
该步骤可以具体包括步骤S3001-S3003：
在步骤S3001中，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为K；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于
Figure GDA0002622637630000121
和
Figure GDA0002622637630000122
之间的IMU量测值进行预积分，得到
Figure GDA0002622637630000123
和
Figure GDA0002622637630000124
这里所说的“最后一帧”和“最前面一帧”，分别指滑动窗口中时间戳最新的和最旧的一帧图像。
其中，TK、TK-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤S1000调整后的值；
Figure GDA0002622637630000125
为所述倒数第二帧图像的时间延迟，初值为0；当滑动窗口中图像数量未达到预设的滑动窗口中最大图像数量时，暂时不执行步骤S3002至步骤S3003；
在步骤S3002中，按照基于非线性优化的VIO算法的通用做法在局部窗口中构造代价函数，使用Dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值。
其中，所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；所述重投影误差项和所述边缘化误差项使用基于非线性优化的VIO算法的通用做法进行构造；所述IMU误差项使用如公式(2)所示的方法进行构造：
Figure GDA0002622637630000131
其中，CIMU(x)表示IMU误差项；x由x1,x2,...,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，
Figure GDA0002622637630000132
表示协方差矩阵，通过IMU预积分计算出；
Figure GDA0002622637630000133
表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关，如公式(3)所示：
Figure GDA0002622637630000134
Figure GDA0002622637630000135
为待估计的所述时间延迟，
Figure GDA0002622637630000136
和
Figure GDA0002622637630000137
分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；
Figure GDA0002622637630000138
和
Figure GDA0002622637630000139
分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；
Figure GDA00026226376300001310
和
Figure GDA00026226376300001311
分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00026226376300001312
和
Figure GDA00026226376300001313
分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00026226376300001314
为用四元数表示的在第k帧曝光时刻从世界坐标系到IMU本体坐标系的旋转；
Figure GDA00026226376300001315
为
Figure GDA00026226376300001316
的旋转矩阵形式；
Figure GDA00026226376300001317
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值，Δtk＝Tk+1-Tk；
Figure GDA0002622637630000141
和
Figure GDA0002622637630000142
可以由
Figure GDA0002622637630000143
和其他IMU量测值表示，如公式(4)-(6)所示：
Figure GDA0002622637630000144
Figure GDA0002622637630000145
Figure GDA0002622637630000146
Figure GDA0002622637630000147
表示在对当前滑动窗口内第k帧和第k+1帧之间的IMU量测值预积分的时候对时间偏移的估计值；ab,k和ae,k的计算方法分别为
Figure GDA0002622637630000148
其中
Figure GDA0002622637630000149
和
Figure GDA00026226376300001410
分别为
Figure GDA00026226376300001411
和
Figure GDA00026226376300001412
时刻IMU量测值中的加速度；
Figure GDA00026226376300001413
和
Figure GDA00026226376300001414
分别为IMU第k帧和第k+1帧图像之间IMU预积分的结果；
Figure GDA00026226376300001415
和
Figure GDA00026226376300001416
分别如公式(7)、(8)所示：
Figure GDA00026226376300001417
Figure GDA00026226376300001418
Figure GDA00026226376300001419
和
Figure GDA00026226376300001420
分别为
Figure GDA00026226376300001421
和
Figure GDA00026226376300001422
时刻IMU量测值中的角速度；
Figure GDA00026226376300001423
和
Figure GDA00026226376300001424
是四元数
Figure GDA00026226376300001425
和
Figure GDA00026226376300001426
对应的旋转矩阵。
Figure GDA00026226376300001427
Figure GDA00026226376300001428
和
Figure GDA00026226376300001429
均为待优化的参数。利用
Figure GDA00026226376300001430
构造IMU误差项CIMU(x)，与重投影误差项、边缘化误差项一起构成非线性优化的代价函数，使用Dogleg算法非线性优化求解，得到所有待优化参数的估计值。
在步骤S3003中，将非线性优化求解得到的所述时间延迟
Figure GDA00026226376300001431
作为当前滑动窗口中最新图像对应的时间延迟的估计值，如公式(9)所示：
Figure GDA0002622637630000151
所述时间延迟的估计值
Figure GDA0002622637630000152
在下一帧图像进入滑动窗口后，将对应于步骤S3001中
Figure GDA0002622637630000153
的值。
随着滑动窗口不断向前滑动，时间延迟的估计量
Figure GDA0002622637630000154
越来越接近真值
Figure GDA0002622637630000155
当新的一帧加入窗口时，当前对时间偏移的估计值
Figure GDA0002622637630000156
也越来越接近真值
Figure GDA0002622637630000157
这时候IMU预积分的起始时刻
Figure GDA0002622637630000158
和结束时刻
Figure GDA0002622637630000159
就越来越接近相机曝光时间tK-1和tK，也就实现了相机和IMU系统的精确时间同步。
图2是本发明的使用在线时间标定的视觉惯性测程法实施例二的流程示意图。如图2所示，本实施例的视觉惯性测程法与实施例一相比，还包括一次性执行步骤S2000：
在步骤S2000中，进行时间延时的初步估计：分别根据所述相机图像和所述IMU量测值计算相机的姿态；再计算出使得上述两组姿态偏差最小的时间延迟，作为相机时间延迟的初步估计值。本步骤只在最初执行一次，算出一个初步估计值，作为步骤S3001中所述倒数第二帧图像的时间延迟
Figure GDA00026226376300001510
的初值。
当实际的时间延迟较大时，采用实施例二的方法将这里算出的初步估计之作为步骤S3000中精确标定所需要的初值，相比实施例一中以0为初值可以增加算法的稳定性。
步骤S2000可以具体包括：
在步骤S2001中，检测是否有新的IMU量测值加入滑动窗口，若是，则转至步骤S2003；否则，转至步骤S2002。
在步骤S2002中，检测是否有新的一帧图像加入滑动窗口，若是，则转至步骤S2004；否则，转至步骤S2001。
在步骤S2003中，计算转角变化量Δq(t)并存入队列，更新非线性优化的滑动窗口中的所述累计转角q；转至步骤S2005。
其中，Δq(t)与q分别如公式(10)、(11)所示：
Figure GDA00026226376300001511
Figure GDA00026226376300001512
q为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；
Figure GDA0002622637630000161
为IMU量测值的角速度；Δt为IMU相邻两次观测的时间差；
Figure GDA0002622637630000162
表示四元数的乘法；t为当前最新IMU量测值的时间戳；
在步骤S2004中，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个IMU量测值中角速度差的模长的平均值
Figure GDA0002622637630000163
用该平均值来衡量角速度变化的情况；这里的“最后一帧”和“最前面一帧”，分别指滑动窗口中时间戳最新的和最旧的图像。
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角，如公式(12)所示：
Figure GDA0002622637630000164
其中，Δq(t′)为根据时间戳为t′的IMU量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳。
在步骤S2005中，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)，如公式(13)所示：
Figure GDA0002622637630000165
其中，qx，qy，qz和qw表示四元数q的四个分量；
在步骤S2006中，若θ1<θ(qa)<θ2且
Figure GDA0002622637630000166
或
Figure GDA0002622637630000167
则转至步骤S2007-S2012进行时间戳延迟的初步估计；否则，转至步骤S2001。
其中，阈值θ1和θ2用来筛选出具有较好几何结构的场景，θ1取10°，θ2取50°；阈值ω1和ω2用来平衡时间延迟估计的及时性和可靠性，若要提高及时性则减小ω1和ω2，若要提高可靠性则增大ω1和ω2。ω1取0.25-0.35弧度/秒，ω2取0.5-0.7弧度/秒。
在步骤S2007中，在应用本方法的VIO算法运行的过程中，滑动窗口中的图像特征点和IMU信息会共同参与优化得到滑动窗口中的相机位姿，将这个位姿作为本步骤中仅使用图像特征点的捆绑调整的初值。只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整。
只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整的时候，使用Ceres Solver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵如公式(14)所示：
Figure GDA0002622637630000171
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距。若捆绑调整完成后代价函数值小于预设的代价函数阈值0.07，则认为捆绑调整成功，否则认为捆绑调整失败。
在步骤S2008中，若捆绑调整失败，则继续转至步骤S2001；若成功，则转至步骤S2009。
在步骤S2009中，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转，如公式(15)所示：
Figure GDA0002622637630000172
其中，
Figure GDA0002622637630000173
和
Figure GDA0002622637630000174
分别为通过捆绑调整得到的第k帧和第k+1帧相机坐标系到世界坐标系的旋转四元数，
Figure GDA0002622637630000175
为从第k+1帧相机坐标系到第k帧相机坐标系的旋转四元数。
在步骤S2010中，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于Tk-td和Tk+1-td之间的IMU量测值中的角速度推算相邻两帧之间IMU本体坐标系间的旋转四元数，如公式(16)所示：
Figure GDA0002622637630000176
其中，Tk和Tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为IMU量测值的时间戳；
Figure GDA0002622637630000181
表示时间戳为t的IMU量测值中的角速度；П表示四元数的连乘。
本实施例中，时间延迟td在th1和th2之间采样，采样间隔为δt。th1和th2分别取0ms,100ms,δt取3ms。
在步骤S2011中，结合已知的相机和IMU坐标系间的旋转四元数
Figure GDA0002622637630000182
计算相邻两帧之间相机坐标系间的旋转四元数，如公式(17)所示：
Figure GDA0002622637630000183
在步骤S2012中，对于不同的时间延迟td，计算由相机信息和IMU信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td，如公式(18)所示：
Figure GDA0002622637630000184
若满足公式(19)所示的条件：
Figure GDA0002622637630000185
则记录一个时间延迟的估计值
Figure GDA0002622637630000186
其中，K为滑动窗口中包含的帧的数目。本实施例中预设的旋转角度阈值为1.5°。
在步骤S2013中，判断是否记录了预设个数的所述时间延迟的估计值
Figure GDA0002622637630000187
若是，则对所述预设个数(本实施例中为5)的所述时间延迟的估计值求中值，作为最终得到的时间延迟的初步估计值；否则，转至步骤S2001。
图3(a)-(b)是本发明视觉惯性测程法实施例一和实施例二中时间延迟标定值随时间变化的比较图，其中图3(a)表示在EuRoc MAV数据集的MH_01_easy数据上人工添加30ms的时间延迟的情况下对时间延迟的标定，图3(b)表示在安卓手机采集的数据上对时间延迟的标定。
我们在实施例二中以步骤S2000为时间延迟的初值，在实施例一中以0为时间延迟的初值，从图3(a)-(b)中可以看出两种策略下时间标定性能较为接近，而实施例二中使用步骤S2000的初步估计值作为初值在刚开始优化的时候时间延迟参数的值较少波动，能使算法更为稳定。作为代价的是步骤S2000需要一定时间，在此期间步骤S3000无法进行。
图4(a)-(d)是本发明视觉惯性测程法实施例一和现有的时间标定方法在EuRocMAV数据集上时间标定误差随时间变化的比较图，其中图4(a)、图4(b)、图4(c)、图4(d)分别表示在数据集上将图像的时间戳人工添加60ms、30ms、0ms、-30ms的时间延迟后时间标定的误差。可以看出，总体来说使用本方法的时间标定误差更小。如图4(c)所示，两种方法在事实上没有时间延迟的数据上都不会错误地估计时间延迟。
图5(a)-(b)是使用本发明视觉惯性测程法实施例一的VIO算法与使用现有的时间标定方法的VIO算法以及不进行时间标定的VIO算法在安卓手机采集的含有快速转弯和突然停止的数据上的轨迹比较图，轨迹是三维的，而图中表示的是轨迹在X轴和Y轴形成的平面上的投影，因为相机在Z方向的运动可以忽略不计。其中图5(a)、图5(b)分别对应含有突然停止和快速转弯的数据。图5(b)的圆点为时间间隔相等的相机位置，圆点的疏密程度反映相机运动的速度。图5(a)中，轨迹上A点为轨迹的起点，而使用三种方法分别计算出来的轨迹终点分别为B1，B2和B3。事实上，数据采集者绕着一个大约11m×21m的矩形区域走了一圈，且轨迹上A，B1，B2和B3四个点事实上应该重合。使用本方法的相机轨迹在相机经历突然停止后仍然能保持正确的尺度，而其他两种算法得出的相机轨迹的尺度会错误地缩小。图5(b)中，数据采集者事实上近似匀速运动，期间快速拐过一个接近90°的转角。使用本发明方法的算法在转弯之后估计出来的相机运动仍然近似匀速，而另外两种方法估计的相机速度在转弯后明显变快，即发生“漂走”现象。
本发明的一种存储设备的实施例，其存储有程序，所述程序适于由处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的一种处理设备的实施例，包括：处理器和存储器；
所述处理器，适于执行程序；所述存储器，适于存储该程序；所述程序适于由所述处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
图6是本发明的使用在线时间标定的视觉惯性测程设备实施例一的构成示意图。如图6所示，本实施例的视觉惯性测程设备包括：图像与IMU量测值获取模块10、时间延迟与相机姿态计算模块30；
所述图像与IMU量测值获取模块10，配置为：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
所述时间延迟与相机姿态计算模块30，配置为：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；其中，IMU误差项的残差向量中包含与所述时间延迟有关的变量。
图7是本发明的使用在线时间标定的视觉惯性测程设备实施例二的构成示意图。如图7所示，本实施例的视觉惯性测程设备还包括：时间延迟初值估计模块20。
所述时间延迟初值估计模块20，包括：分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (11)

1.一种使用在线时间标定的视觉惯性测程法，其特征在于，包括以下步骤：
步骤S1000，实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
步骤S3000，当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，
所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
2.根据权利要求1所述的视觉惯性测程法，其特征在于，步骤S1000具体包括：
步骤S1001，使用camera2 API实时获取相机图像，并获取相机时间基准下所述相机图像的曝光时刻时间戳；根据IMU时间基准，调整所述相机图像的曝光时刻时间戳；实时获取IMU量测值；
步骤S1002，将调整时间戳后的所述相机图像，以及获取的IMU量测值加入滑动窗口；
其中，
所述IMU量测值的获取频率高于所述相机图像的获取频率；
所述相机图像的曝光时刻时间戳根据下式调整：
Figure FDA0002461322330000011
Figure FDA0002461322330000012
表示调整后的第i帧所述相机图像的曝光时刻时间戳；
Figure FDA0002461322330000013
表示在IMU时间基准下第1帧图像对应的回调函数执行时刻的时间戳；
Figure FDA0002461322330000014
和
Figure FDA0002461322330000015
分别表示在相机时间基准下第i帧和第1帧图像的曝光时刻时间戳。
3.根据权利要求2所述的视觉惯性测程法，其特征在于，
所述获取相机时间基准下所述相机图像的曝光时刻时间戳的方法为：
通过onImageAvailable()函数获取Image类型的对象，调用该对象的成员函数getTimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；
所述回调函数执行时刻的时间戳的获取方法为：
使用SystemClock类的elapsedRealtime()函数获取IMU时间基准下所述回调函数执行时刻时间戳。
4.根据权利要求2所述的视觉惯性测程法，其特征在于，步骤S3000具体包括：
步骤S3001，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为K；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于
Figure FDA0002461322330000021
和
Figure FDA0002461322330000022
之间的IMU量测值进行预积分，得到
Figure FDA0002461322330000023
和
Figure FDA0002461322330000024
其中，TK、TK-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤S1000调整后的值；
Figure FDA0002461322330000025
为所述倒数第二帧图像的时间延迟，初值为0；
Figure FDA0002461322330000026
和
Figure FDA0002461322330000027
是利用延迟时间
Figure FDA0002461322330000028
对时间戳TK-1和TK之间的时间区间进行调整后，在该调整后的时间区间内对IMU量测值进行预积分的结果；
步骤S3002，按照基于非线性优化的VIO算法的通用做法在局部窗口中构造代价函数，使用Dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值；
其中，
所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于非线性优化的VIO算法的通用做法进行构造；
所述IMU误差项使用下述方法进行构造：
Figure FDA0002461322330000031
其中，CIMU(x)表示IMU误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，
Figure FDA0002461322330000032
表示协方差矩阵，通过IMU预积分计算出；
Figure FDA0002461322330000033
表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关：
Figure FDA0002461322330000034
Figure FDA0002461322330000035
为待估计的所述时间延迟，
Figure FDA0002461322330000036
和
Figure FDA0002461322330000037
分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；
Figure FDA0002461322330000038
和
Figure FDA0002461322330000039
分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；
Figure FDA00024613223300000310
和
Figure FDA00024613223300000311
分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure FDA00024613223300000312
和
Figure FDA00024613223300000313
分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure FDA00024613223300000314
为用四元数表示的在第k帧曝光时刻从世界坐标系到IMU本体坐标系的旋转；
Figure FDA00024613223300000315
为
Figure FDA00024613223300000316
的旋转矩阵形式；
Figure FDA00024613223300000317
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值，Δtk＝Tk+1-Tk；
Figure FDA00024613223300000318
Figure FDA00024613223300000319
和
Figure FDA00024613223300000320
的计算方法为：
Figure FDA00024613223300000321
Figure FDA00024613223300000322
Figure FDA0002461322330000041
Figure FDA0002461322330000042
和
Figure FDA0002461322330000043
是从真实的第k帧图像曝光时刻到真实的第k+1帧图像曝光时刻之间的时间区间内对IMU量测值进行预积分的结果；
Figure FDA0002461322330000044
表示在对当前滑动窗口内第k帧和第k+1帧之间的IMU量测值预积分的时候对时间偏移的估计值；ab,k和ae,k的计算方法分别为
Figure FDA0002461322330000045
Figure FDA0002461322330000046
其中
Figure FDA0002461322330000047
和
Figure FDA0002461322330000048
分别为
Figure FDA0002461322330000049
和
Figure FDA00024613223300000410
时刻IMU量测值中的加速度；
Figure FDA00024613223300000411
和
Figure FDA00024613223300000412
是利用延迟时间
Figure FDA00024613223300000413
对时间戳TK和TK+1之间的时间区间进行调整后，在该调整后的时间区间内对IMU量测值进行预积分的结果；
Figure FDA00024613223300000414
和
Figure FDA00024613223300000415
分别表示为：
Figure FDA00024613223300000416
Figure FDA00024613223300000417
Figure FDA00024613223300000418
和
Figure FDA00024613223300000419
分别为
Figure FDA00024613223300000420
和
Figure FDA00024613223300000421
时刻IMU量测值中的角速度；
Figure FDA00024613223300000422
和
Figure FDA00024613223300000423
是四元数
Figure FDA00024613223300000424
和
Figure FDA00024613223300000425
对应的旋转矩阵；
Figure FDA00024613223300000426
和
Figure FDA00024613223300000427
均为待优化的参数；
步骤S3003，将非线性优化求解得到的所述时间延迟
Figure FDA00024613223300000428
作为当前滑动窗口中最新图像对应的时间延迟的估计值：
Figure FDA00024613223300000429
所述时间延迟的估计值
Figure FDA00024613223300000430
在下一帧图像进入滑动窗口后，将对应于步骤S3001中
Figure FDA00024613223300000431
的值。
5.根据权利要求4所述的视觉惯性测程法，其特征在于，还包括一次性执行：
步骤S2000，分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为时间延迟的初步估计值；
相应地，将步骤S2000计算得到的所述时间延迟的初步估计值作为步骤S3001中所述倒数第二帧图像的时间延迟
Figure FDA0002461322330000051
的初值。
6.根据权利要求5所述的视觉惯性测程法，其特征在于，步骤S2000具体包括：
步骤S2001，检测是否有新的IMU量测值加入滑动窗口，若是，则转至步骤S2003；否则，转至步骤S2002；
步骤S2002，检测是否有新的一帧图像加入滑动窗口，若是，则转至步骤S2004；否则，转至步骤S2001；
步骤S2003，计算转角变化量Δq(t)并存入队列，更新非线性优化的滑动窗口中的累计转角q；转至步骤S2005；
其中，
Figure FDA0002461322330000052
Figure FDA0002461322330000053
q为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；
Figure FDA0002461322330000054
为IMU量测值的角速度；Δt为IMU相邻两次观测的时间差；
Figure FDA0002461322330000057
表示四元数的乘法；t为当前最新IMU量测值的时间戳；
步骤S2004，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个IMU量测值中角速度差的模长的平均值
Figure FDA0002461322330000055
用该平均值来衡量角速度变化的情况；
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角：
Figure FDA0002461322330000056
其中，Δq(t′)为根据时间戳为t′的IMU量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳；
步骤S2005，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)：
Figure FDA0002461322330000061
其中，qx，qy，qz和qw表示四元数q的四个分量；
步骤S2006，若θ1＜θ(qa)＜θ2且
Figure FDA0002461322330000062
或
Figure FDA0002461322330000063
则转至步骤S2007；否则，转至步骤S2001；
其中，阈值θ1和θ2用来筛选出具有较好几何结构的场景；阈值ω1和ω2用来平衡时间延迟估计的及时性和可靠性，若要提高及时性则减小ω1和ω2，若要提高可靠性则增大ω1和ω2；
步骤S2007，在应用本方法的VIO算法运行的过程中，滑动窗口中的图像特征点和IMU信息会共同参与优化得到滑动窗口中的相机位姿，将这个位姿作为本步骤中仅使用图像特征点的捆绑调整的初值，只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整；
步骤S2008，若捆绑调整失败，则继续转至步骤S2001；若成功，则转至步骤S2009；
步骤S2009，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转：
Figure FDA0002461322330000064
其中，
Figure FDA0002461322330000065
和
Figure FDA0002461322330000066
分别为通过捆绑调整得到的第k帧和第k+1帧相机坐标系到世界坐标系的旋转四元数，
Figure FDA0002461322330000067
为从第k+1帧相机坐标系到第k帧相机坐标系的旋转四元数；
步骤S2010，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于Tk-td和Tk+1-td之间的IMU量测值中的角速度推算相邻两帧之间IMU本体坐标系间的旋转四元数：
Figure FDA0002461322330000068
其中，Tk和Tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为IMU量测值的时间戳；
Figure FDA0002461322330000071
表示时间戳为t的IMU量测值中的角速度；П表示四元数的连乘；
步骤S2011，结合已知的相机和IMU坐标系间的旋转四元数
Figure FDA0002461322330000072
计算相邻两帧之间相机坐标系间的旋转四元数：
Figure FDA0002461322330000073
步骤S2012，对于不同的时间延迟td，计算由相机信息和IMU信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td：
Figure FDA0002461322330000074
若
Figure FDA0002461322330000075
则记录一个时间延迟的估计值
Figure FDA0002461322330000076
其中，K为滑动窗口中包含的帧的数目；
步骤S2013，判断是否记录了预设个数的所述时间延迟的估计值
Figure FDA0002461322330000077
若是，则对所述预设个数的所述时间延迟的估计值求中值，作为最终得到的时间延迟的初步估计值；否则，转至步骤S2001。
7.根据权利要求6所述的视觉惯性测程法，其特征在于，步骤S2007中“只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整”，具体为：
使用Ceres Solver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵均为：
Figure FDA0002461322330000078
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距；
若捆绑调整完成后代价函数值小于预设的代价函数阈值，则认为捆绑调整成功，否则认为捆绑调整失败。
8.一种存储设备，其存储有程序，其特征在于，所述程序适于由处理器加载并执行，以实现权利要求1-7中任一项所述的使用在线时间标定的视觉惯性测程法。
9.一种处理设备，包括：处理器和存储器；
所述处理器，适于执行程序；
所述存储器，适于存储该程序；
其特征在于，所述程序适于由所述处理器加载并执行，以实现权利要求1-7中任一项所述的使用在线时间标定的视觉惯性测程法。
10.一种使用在线时间标定的视觉惯性测程设备，其特征在于，包括：图像与IMU量测值获取模块、时间延迟与相机姿态计算模块；
所述图像与IMU量测值获取模块，配置为：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
所述时间延迟与相机姿态计算模块，配置为：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，
所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
11.根据权利要求10所述的视觉惯性测程设备，其特征在于，还包括：时间延迟初值估计模块；
所述时间延迟初值估计模块，包括：分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
CN201810239278.2A 2018-03-22 2018-03-22 使用在线时间标定的视觉惯性测程法与设备 Active CN108629793B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810239278.2A CN108629793B (zh) 2018-03-22 2018-03-22 使用在线时间标定的视觉惯性测程法与设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810239278.2A CN108629793B (zh) 2018-03-22 2018-03-22 使用在线时间标定的视觉惯性测程法与设备

Publications (2)

Publication Number Publication Date
CN108629793A CN108629793A (zh) 2018-10-09
CN108629793B true CN108629793B (zh) 2020-11-10

Family

ID=63696233

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810239278.2A Active CN108629793B (zh) 2018-03-22 2018-03-22 使用在线时间标定的视觉惯性测程法与设备

Country Status (1)

Country Link
CN (1) CN108629793B (zh)

Families Citing this family (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109275048A (zh) * 2018-11-21 2019-01-25 北京猎户星空科技有限公司 一种应用于机器人的数据处理方法、装置、设备及介质
CN109544630B (zh) * 2018-11-30 2021-02-02 南京人工智能高等研究院有限公司 位姿信息确定方法和装置、视觉点云构建方法和装置
CN111383282B (zh) * 2018-12-29 2023-12-01 杭州海康威视数字技术股份有限公司 位姿信息确定方法及装置
CN109785428B (zh) * 2019-01-21 2023-06-23 苏州大学 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN109764880B (zh) * 2019-02-19 2020-12-25 中国科学院自动化研究所 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统
CN110174120B (zh) * 2019-04-16 2021-10-08 百度在线网络技术（北京）有限公司 用于ar导航模拟的时间同步方法及装置
CN110084832B (zh) * 2019-04-25 2021-03-23 亮风台（上海）信息科技有限公司 相机位姿的纠正方法、装置、系统、设备和存储介质
CN111854770B (zh) * 2019-04-30 2022-05-13 北京魔门塔科技有限公司 一种车辆的定位系统和方法
CN111998870B (zh) * 2019-05-26 2022-08-26 北京魔门塔科技有限公司 一种相机惯导系统的标定方法和装置
CN110207722A (zh) * 2019-06-11 2019-09-06 远形时空科技（北京）有限公司 一种自动化标定相机里程计系统及方法
CN110379017B (zh) * 2019-07-12 2023-04-28 北京达佳互联信息技术有限公司 一种场景构建方法、装置、电子设备及存储介质
CN112414400B (zh) * 2019-08-21 2022-07-22 浙江商汤科技开发有限公司 一种信息处理方法、装置、电子设备和存储介质
CN110567453B (zh) * 2019-08-21 2021-05-25 北京理工大学 仿生眼多通道imu与相机硬件时间同步方法和装置
CN110455309B (zh) * 2019-08-27 2021-03-16 清华大学 具备在线时间校准的基于msckf的视觉惯性里程计
CN110595479B (zh) * 2019-09-23 2023-11-17 云南电网有限责任公司电力科学研究院 一种基于icp算法的slam轨迹评估方法
CN110702139B (zh) * 2019-09-29 2021-08-27 百度在线网络技术（北京）有限公司 一种时延标定方法、装置、电子设备和介质
CN111308415B (zh) * 2019-11-01 2022-09-02 华为技术有限公司 一种基于时间延迟的在线估计位姿的方法和设备
CN110956666B (zh) * 2019-11-12 2023-05-12 深圳市瑞立视多媒体科技有限公司 运动数据标定方法、装置、终端设备及存储介质
CN110956665B (zh) * 2019-12-18 2023-06-23 中国科学院自动化研究所 车辆拐弯轨迹双向计算方法、系统、装置
CN111257853B (zh) * 2020-01-10 2022-04-01 清华大学 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN111325803B (zh) * 2020-02-12 2023-05-12 清华大学深圳国际研究生院 一种评估双目相机内外参与时间同步的标定的方法
CN113311463A (zh) * 2020-02-26 2021-08-27 北京三快在线科技有限公司 Gps延迟时间在线补偿方法、装置、电子设备和存储介质
CN111307176B (zh) * 2020-03-02 2023-06-16 北京航空航天大学青岛研究院 一种vr头戴显示设备中视觉惯性里程计的在线标定方法
CN111580596B (zh) * 2020-05-19 2022-04-15 北京数字绿土科技股份有限公司 多个imu时间同步方法、装置、终端
CN112097771B (zh) * 2020-08-18 2022-04-29 青岛职业技术学院 Gps延迟时间自适应的扩展卡尔曼滤波导航方法
CN112129287B (zh) * 2020-09-24 2022-06-10 北京华捷艾米科技有限公司 一种基于视觉惯性里程计处理的方法和相关装置
CN112945231A (zh) * 2021-01-28 2021-06-11 深圳市瑞立视多媒体科技有限公司 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质
CN112923923A (zh) * 2021-01-28 2021-06-08 深圳市瑞立视多媒体科技有限公司 一种imu与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质
CN114979456B (zh) * 2021-02-26 2023-06-30 影石创新科技股份有限公司 视频数据的防抖处理方法、装置、计算机设备和存储介质
CN114040128B (zh) * 2021-11-24 2024-03-01 视辰信息科技(上海)有限公司 时间戳延时标定方法及系统、设备和计算机可读存储介质
CN114459523B (zh) * 2021-12-10 2024-04-30 红云红河烟草(集团)有限责任公司 一种在线质量检测仪器的标定预警方法
CN115239758A (zh) * 2022-05-24 2022-10-25 广东人工智能与先进计算研究院 时间戳校正方法、装置、设备、介质及计算机程序产品

Family Cites Families (7)

* Cited by examiner, † Cited by third party
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
US9709404B2 (en) * 2015-04-17 2017-07-18 Regents Of The University Of Minnesota Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation
CN105698765B (zh) * 2016-02-22 2018-09-18 天津大学 双imu单目视觉组合测量非惯性系下目标物位姿方法
CN107193279A (zh) * 2017-05-09 2017-09-22 复旦大学 基于单目视觉和imu信息的机器人定位与地图构建系统
CN107564012B (zh) * 2017-08-01 2020-02-28 中国科学院自动化研究所 面向未知环境的增强现实方法及装置
CN107656545A (zh) * 2017-09-12 2018-02-02 武汉大学 一种面向无人机野外搜救的自主避障与导航方法
CN107767425A (zh) * 2017-10-31 2018-03-06 南京维睛视空信息科技有限公司 一种基于单目vio的移动端AR方法

Also Published As

Publication number Publication date
CN108629793A (zh) 2018-10-09

Similar Documents

Publication Publication Date Title
CN108629793B (zh) 使用在线时间标定的视觉惯性测程法与设备
CN104501814B (zh) 一种基于视觉和惯性信息的姿态与位置估计方法
Hanning et al. Stabilizing cell phone video using inertial measurement sensors
Li et al. Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
CN110081881B (zh) 一种基于无人机多传感器信息融合技术的着舰引导方法
Kneip et al. Deterministic initialization of metric state estimation filters for loosely-coupled monocular vision-inertial systems
CN110411476B (zh) 视觉惯性里程计标定适配及评价方法和系统
CN109059907B (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
US9037411B2 (en) Systems and methods for landmark selection for navigation
US11223764B2 (en) Method for determining bias in an inertial measurement unit of an image acquisition device
CN111721288B (zh) 一种mems器件零偏修正方法、装置及存储介质
CN109612471B (zh) 一种基于多传感器融合的运动体姿态解算方法
GB2498177A (en) Apparatus for determining a floor plan of a building
WO2008024772A1 (en) Image-based system and method for vehicle guidance and navigation
CN110260861B (zh) 位姿确定方法及装置、里程计
CN110956665A (zh) 车辆拐弯轨迹双向计算方法、系统、装置
JP6291519B2 (ja) 三次元点群データに実寸法を付与する方法とそれを用いた管路等の位置測定
CN107621266B (zh) 基于特征点跟踪的空间非合作目标相对导航方法
CN110231028B (zh) 飞行器导航方法、装置和系统
Scandaroli et al. A nonlinear observer approach for concurrent estimation of pose, IMU bias and camera-to-IMU rotation
CN111707261A (zh) 一种微型无人机高速感知和定位方法
CN109631894A (zh) 一种基于滑动窗口的单目视觉惯性紧耦合方法
CN114623817B (zh) 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN112113564B (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
CB03 Change of inventor or designer information

Inventor after: Gao Wei

Inventor after: Liu Jinxu

Inventor after: Hu Zhanyi

Inventor before: Liu Jinxu

Inventor before: Gao Wei

Inventor before: Hu Zhanyi

CB03 Change of inventor or designer information
GR01 Patent grant
GR01 Patent grant