一种移动平台上基于视觉和惯导融合SLAM的标定外参和时序的方法与流程

本发明涉及融合相机和imu(inertialmeasurementunit，imu)信息进行定位和地图构建的方法，特别是涉及一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法。

背景技术

随着人们需求的日益丰富和机器人技术的迅速发展，相关领域如无人机、无人驾驶、增强现实等也随之快速进步。这些领域所涉技术众多，包括机械控制、并行计算、传感器技术、信号处理等。在感知上，高度智能的设备离不开自身的定位和对环境的感知，slam(simultaneouslocationandmapping,slam)技术正是解决这一问题的关键技术，可以实现在未知环境中进行定位和地图构建，被认为是实现智能设备真正自主的关键。

slam(simultaneouslocationandmapping,slam)是指在未知环境中进行定位和地图构建，视觉和惯导融合slam是指融合相机和imu(inertialmeasurementunit，imu)信息进行定位和地图构建。移动设备上的视觉惯导融合的slam主要应用在无人机、增强现实、室内扫地机器人等方面。

按照核心功能模块来区分，slam又分为激光雷达slam(激光slam)和视觉slam(visualslam,vslam)。激光雷达能够测得比较准确的距离信息，误差模型简单，理论研究比较成熟，落地产品更加丰富。但是由于激光雷达价格昂贵，在很多激光雷达应用受限的场景(如小型无人机、增强现实等)，vslam成为首选解决方案。另外相对于激光雷达，视觉能够提供丰富的纹理信息，具有很强的场景识别能力。近年来，随着相机技术、非线性优化理论的发展和机器性能的提高，vslam已经取得了先进的理论成果并具有较为完整的工程体系。视觉slam又有单目视觉slam和双目视觉slam之分，双目slam需要两个相机，在系统工作之前，需要进行严格的标定工作以获得相机的外参信息。双目slam可以获得真实的尺度，但是由于较长的基线(两相机相对位置)才能保证精确的尺度信息，所以双目slam的装置一般比较庞大，不适合应用在一些轻巧的设备上，如手机、小型无人机等。而单目slam技术只需要一个相机，但是却无法获取场景的精确尺度，常常需要融合其他传感器信息。

随着移动设备的普及以及其计算能力和电池储能的提高，更多技术应用到移动设备上来。移动设备上的slam技术具有广泛的应用场景，如移动设备上的增强现实、三维重建、室内场景的定位等等。现有手持移动设备大都装配一个普通相机和多个运动传感器。其中，惯性测量单元(inertialmeasurementunit，imu)被广泛使用，它是一种能够提供设备加速度和角速度信息的六轴传感器。融合相机和imu信息的单目slam技术能够解决纯单目视觉slam方法无法估计尺度和对视觉特征的过度依赖等问题。目前，该技术主要面临如下难点：

1、由于移动设备上的imu自身误差很大，如何对imu的运动进行建模，如何将imu信息融合到视觉中来是解决本问题的关键；

2、外参估计。如何在初始化阶段估计相机与imu的外参也是需要解决的问题之一。

3、另外由于硬件的限制，相机和imu具有不同的时钟，如何在异步的条件下融合相机和imu也是解决本问题的难点之一。

技术实现要素：

发明目的：针对现有技术的不足，提供一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法。

技术方案：本发明提供了一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法，包括以下步骤：

(1)初始化阶段：使用松耦合的方法对齐通过相机和imu估计的两帧之间的相对旋转参数，估计得到相机和imu的相对旋转参数；

(2)前端阶段：前端完成视觉里程计功能，即根据前几帧估计的相机在世界坐标系中的位姿大致估计当前帧相机在世界坐标系中的位姿，估计的值作为后端优化的初始值；

(3)后端阶段：在所有帧中挑选一些关键帧，设置待优化变量，建立一个统一的目标函数，根据对应的约束条件进行优化，从而得到精确的外参。

进一步的，所述步骤(1)包括以下步骤：

(11)在序列帧上建立大小为n的滑动窗口；

(12)通过纯视觉的方法计算窗口内第k(1≤k≤n)帧和第k+1帧之间相机的相对旋转矩阵

(13)利用imu信息中的角速度积分得到窗口内第k帧和第k+1帧之间imu的相对旋转矩阵

(14)假设相机与imu之间的相对位姿为根据之间的关系建立方程：

将用四元数表示为：则上述公式变为：

其中，表示四元数之间的相乘；

将公式四元数相乘展开得到一个约束方程：

其中，是一个4×4的矩阵，w表示四元数的实部，v表示四元数的虚部。(·)^表示向量的外积，e3×3表示3×3的单位矩阵；

(15)重复步骤(12)-(14)，得到多个约束方程，进一步将多个约束方程组成一个超定方程组为：

其中，qn是一个4(n-1)×4的矩阵，通过最小二乘法求解方程组得到的估计；

(16)判断估计是否成功

对qn进行奇异值分解，判断第三大的奇异值是否大于预设阈值，如果大于阈值则估计成功，结束估计；否则，滑动窗口，即移除窗口内的第一帧数据，并把窗口外下一帧数据加入到窗口末端；然后从步骤(12)开始重新进行估计。

进一步的，所述步骤(2)包括以下步骤：

(21)提取特征点，使用orb特征提取图像上的特征点；

orb特征的提取包含两部分：改进的fast关键点的提取和brief描述子的生成；

(a)改进的fast关键点

①对图像上的一个像素点p，该像素的亮度为ip；

②设定阈值t；

③以像素点p为中心，选取以p为中心，半径为3的圆上16个像素点；

④如果圆上有连续n个点的像素亮度大于ip+t或者小于ip-t，那么像素p被认为是fast关键点，n通常取12；

⑤对图像上的每一个像素点执行以上四步；

⑥使用非极大值抑制，即在一定区域内仅仅保留harris响应值最大的关键点；

⑦固定图像上关键点的数量n，对一幅图像上的所有关键点按照harris响应值排序，选取前n个关键点；

⑧计算特征点的方向，灰度质心法，质心是指图像块中以灰度值作为衡量权重的重心，连接图像块的几何中心和质心得到的方向向量作为特征点的方向；

(b)计算brief描述子

brief是一种二进制描述子，描述向量由0和1组成，0和1是对关键点附近的两个像素p1和p2之间的亮度的比较进行编码的结果，如果则编码为1，否则编码为0；在关键点附近选取128对这样的像素进行比较，并把比较结果写成一个128维的向量作为该关键点的特征描述子；

(22)在相邻帧之间通过匹配特征点的描述子进行特征匹配；

(23)计算特征点在图像上的移动速度：

假设某一特征点pk在前一帧坐标系上的坐标为q1，归一化之后的坐标为q1，在当前帧j坐标系上的坐标为q2，归一化之后的坐标为q2，两帧之间的时间差为t，那么特征点pk在当前帧上的速度为：

进一步的，所述步骤(3)包括以下步骤：

(31)设置待优化项

外参标定待优化项为：

时序标定待优化项为：

其中，表示第j关键帧上imu的旋转矩阵、平移向量、速度、陀螺仪的偏差、加速度计的偏差，n表示窗口的大小；表示空间点pk在当前帧坐标系中的逆深度，即z轴上的值的倒数，其中k＝1,2,…,m。表示待估计外参，为从imu坐标系转换到相机坐标系的旋转矩阵，为从imu坐标系转换到相机坐标系的平移向量，△t表示每一帧中相机和imu的时间偏差；

(32)建立统一的目标函数：

其中，pk表示第k个空间点，j表示当前帧，j-1表示上一帧，eproj(pk,1,j)表示第k个空间点在当前帧的重投影误差，eimu(j-1,j)表示imu误差项；m表示该帧相机能够观测到的空间点的个数，n表示窗口的大小，eproj和eimu分别表示相机的重投影误差和imu的一致性误差；

外参标定中eproj的定义为：

时序标定中eproj的定义为：

其中，表示当前帧相机坐标系和第j关键帧相机坐标系之间的相对旋转矩阵，表示从世界坐标系转换到第1帧imu坐标系的旋转矩阵，表示从第j帧imu坐标系到世界坐标系的旋转矩阵，表示包括从相机坐标系转换到imu坐标系的旋转矩阵；表示当前帧相机坐标系和第j关键帧相机坐标系之间的平移向量，norm(pk)表示pk(x,y,z)的归一化坐标pkj表示pk在第j关键帧的观测点坐标；

外参标定和时序标定中eimu(i,j)的定义为：

其中，er表示imu的旋转矩阵的前后帧一致性误差，ev表示imu的速度的前后帧一致性误差，ep表示imu的位置的前后帧一致性误差，eb表示imu的偏差的前后一致性误差，σi和σr表示误差的协方差矩阵，△rij表示第i帧坐标系和第j帧坐标系之间在世界坐标系下的旋转矩阵，表示△rij关于的雅可比矩阵，表示从世界坐标系转换到第i帧imu坐标系的旋转矩阵，表示第j帧imu在世界坐标系中的速度，表示第i帧imu在世界坐标系中的速度，g^w表示在世界坐标系下的重力加速度，△tij表示第i帧和第j帧之间的时间差，△vij表示第i帧imu和第j帧imu的速度差异，表示△vij关于的雅可比矩阵，表示△vij关于的雅可比矩阵，表示第j帧imu在世界坐标系下的位置，表示第i帧imu在世界坐标系下的位置，表示第i帧imu在世界坐标系中的速度，△pij表示第i帧imu和第j帧imu在世界坐标系下的位置差异，表示△pij关于的雅可比矩阵，表示△pij关于的雅可比矩阵；

(33)设置优化初始值

的初始值来自前端里程计的估计，的初始值来自步骤(1)得到的估计值对应的旋转矩阵的转置矩阵，和△t的初始值均设为0；

(34)使用g2o图优化方法进行优化

进一步的，所述步骤(34)中使用g2o实现的列文伯格-马夸尔特算法进行优化，在每次迭代时，需要计算目标函数关于待优化变量的雅可比矩阵：计算方式如下：

外参标定中，目标函数关于的雅可比矩阵为：

时序标定中，目标函数关于δt的雅可比矩阵为：

有益效果：与现有技术相比，本发明具有以下优点：

(1)针对外参标定，本发明方法所估计的外参与数据集中的外参进行了比较，结果表明相对旋转参数和平移参数的误差基本能降低到0.02到0.025之间。并且分别比较了使用本发明估计的外参与数据集外参的轨迹误差。能够看出使用本发明提出的标定方法得到的外参估计的轨迹误差与使用数据集中外参估计的轨迹误差相差并不大，甚至在某些序列上估计得到的轨迹精度已经超过了使用数据集中的外参估计得到的轨迹精度。

(2)针对时序标定，本发明提出的方法与几种先进的方法进行了比较，对于相机的imu存在时钟异步的数据，本发明提出的方法的轨迹精度要优于其他两种方法。

附图说明

图1是本发明方法流程图；

图2是外参标定的流程图；

图3是时序标定的流程图；

图4是相机和imu的频率示意图；

图5是本发明方法估计的外参与数据集中的外参的误差曲线图。

具体实施方式

下面结合附图和具体实施例对本发明的技术方案进行详细说明。

本发明针对目前单目视觉slam系统无法估计尺度的问题，采用了融合imu的视觉惯导slam系统，并且针对在移动设备上的应用，提出了一种相机与imu外参自动标定的方法，能够解决在线外参标定的问题。针对移动设备上传感器时钟异步的问题，提出了一种在异步条件下，进行传感器融合的方法。实验结果表明，本发明提出的方法能够有效解决上述问题。

本发明的一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法，包括外参标定和时序标定两部分，其中：

外参标定：估计相机和imu之间的相对位姿。在初始化阶段，使用松耦合方法对齐通过相机和imu估计的相对位姿，估计得到外参中的旋转参数；在后端优化阶段，将外参作为待优化项，根据视觉和惯导的约束条件，优化得到相机和imu的相对位姿。

时序标定：估计相机和imu之间的时钟偏差。在前端里程计阶段，计算特征点在图像上的移动速度；在后端优化阶段，将相机和imu之间的时钟偏差作为待优化项，根据视觉和惯导的约束条件，优化得到每一帧上相机和imu之间的时钟偏差。

原理说明：

前端视觉里程计负责跟踪，即根据前面几帧的位姿，计算得到当前帧的位姿；视觉里程计得到的位姿并不是非常精确的，只是为后端优化提供了一个初始值。

imu包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统中独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号。imu测量物体在三维空间中的角速度和加速度，通过积分能够得到imu的位姿。

imu的状态变量可以表示为：

其中，w表示世界坐标系，b表示imu坐标系，表示从imu到世界坐标系的平移，表示从imu相对世界坐标系的速度，是旋转矩阵表示的从imu到世界坐标系的旋转，ba表示加速度计的偏差，bω表示陀螺仪的偏差。

后端优化是指利用前端视觉里程计计算得到初始化位姿，考虑窗口内的所有位姿约束，优化得到更加精确的关键帧位姿和空间点。

如图1所示，一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法，具体包括以下步骤：

(1)初始化阶段：使用松耦合的方法对齐通过相机和imu估计的两帧之间的相对旋转参数，估计得到相机和imu的相对旋转参数。

相机与imu的相对参数包括相对旋转矩阵r和平移向量t，其中t相对来说对结果影响较小，特别是在移动设备上，因为相机和imu在移动设备上的位置比较接近。初始化阶段标定相对旋转矩阵r，并设置平移向量t为0；在后端优化阶段，进一步优化相对旋转矩阵r和平移向量t。

初始化阶段具体包括以下步骤：

(11)在图像序列帧上建立大小为n的滑动窗口。

(12)通过纯视觉的方法计算窗口内第k(1≤k≤n)帧数据和第k+1帧数据之间相机的相对旋转矩阵纯视觉常用的方法是参考帧模型的光束平差法。

参考帧一般为上一关键帧，参考帧模型是指对参考帧中的空间点与当前帧中的特征点进行暴力匹配，得到匹配点对。然后设置当前帧位姿初始值为上一帧位姿，使用匹配点对的约束优化当前帧位姿。目标函数如下所示：

其中，e是误差项，δ表示优化项当前帧的位姿(r,t)，n表示匹配点对的数量。pj是第j空间点在当前帧图像上的位置，是观测值。k表示相机的内参矩阵。其中r表示当前帧与参考帧之间的相对旋转矩阵。设上一帧与参考帧之间的相对旋转矩阵为r'，则当前帧与上一帧的相对旋转矩阵

(13)利用imu信息中的角速度积分得到窗口内第k帧数据和第k+1帧数据之间imu的相对旋转矩阵。如图4所示，在相邻两帧图像之间包含多组imu数据。对相邻两帧图像之间的imu进行积分：

相邻两帧图像之间的imu数据的积分为：

本发明假设第k帧和第k+1帧图像之间的ωτ不会发生变化，则

(14)假设相机与imu之间的相对旋转矩阵根据之间的关系建立方程：

将用四元数表示为：则：

其中，表示四元数之间的相乘，例如：qa＝[wa,va]，qb＝[wb,vb]，wa和wb表示四元数的实部，va和vb表示四元数的虚部；

将公式(7)四元数相乘展开得到一个约束方程：

其中，是一个4×4的矩阵，w表示四元数的实部，v表示四元数的虚部，(·)^表示向量的外积，e3×3表示3×3的单位矩阵。

(15)重复步骤(12)-(14)，得到多个约束方程，进一步将多个约束方程组成一个超定方程组为：

其中，qn是一个4(n-1)×4的矩阵。通过最小二乘法求解方程组得到的估计。

(16)判断估计是否成功。

图像的每帧数据对于都会有一个估计值，受噪声的影响，每次估计得到的值并不一定准确，如果imu三轴充分旋转，那么qn零空间的秩为1，对qn进行奇异值分解，判断第三大的奇异值是否大于预设阈值(本实施例中取0.25)，如果大于阈值则估计成功，结束估计；否则，滑动窗口，即移除窗口内的第一帧数据，并把窗口外下一帧数据加入到窗口末端；然后从步骤(12)开始重新进行估计。最后将估计得到的转换为旋转矩阵

(2)前端阶段：前端完成视觉里程计功能，即根据前几帧估计的相机在世界坐标系中的位姿大致估计当前帧相机在世界坐标系中的位姿，估计的值作为后端优化的初始值。本发明在前端阶段加入了计算特征点在图像上的移动速度的步骤。具体如下：

(21)提取特征点：使用orb特征提取图像上的特征点，特征点指的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点。

orb特征的提取包含两部分：改进的fast关键点的提取和brief描述子的生成。

1)改进的fast关键点

①对图像上的一个像素点p，该像素的亮度为ip；

②设定阈值t；

③以像素点p为中心，选取以p为中心半径为3的圆上16个像素点；

④如果圆上有连续n个点的像素亮度大于ip+t或者小于ip-t。那么像素p被认为是fast关键点。n通常取12；

⑤对图像上的每一个像素点执行以上四步；

⑥使用非极大值抑制，即在一定区域内仅仅保留harris响应值最大的关键点；

⑦固定图像上关键点的数量n，对一幅图像上的所有关键点按照harris响应值排序，选取前n个关键点；

⑧计算特征点的方向。灰度质心法，质心是指图像块中以灰度值作为衡量权重的重心，连接图像块的几何中心和质心得到的方向向量作为特征点的方向；

2)计算brief描述子

brief是一种二进制描述子，描述向量由0和1组成，0和1是对关键点附近的两个像素p1和p2之间的亮度的比较进行编码的结果。如果则编码为1，否则编码为0。在关键点附近选取128对这样的像素进行比较，并把比较结果写成一个128维的向量作为该关键点的特征描述子。

(22)在相邻帧之间通过匹配特征点的描述子进行特征匹配在进行特征匹配时，可以通过计算特征点描述子之间的距离来进行匹配，距离越小说明特征点越相似，相似度最高的两个特征点事匹配点对。

(23)计算特征点在图像上的移动速度：

假设某一特征点pk在前一帧坐标系上的坐标为q1，归一化之后的坐标为q1，在当前帧j坐标系上的坐标为q2，归一化之后的坐标为q2，两帧之间的时间差为t，那么特征点pk在当前帧上的速度为：

(3)后端阶段：在所有帧中挑选一些关键帧。选取关键帧的判断条件主要包括两类，第一类条件是与上一关键帧的间距是否达到阈值，这里的间距是指时间或者帧数。第二类条件是当前帧的特征点数是否达到要求。下面具体介绍系统中插入关键帧的方法。

条件1：与上一关键帧的的时间戳相差一定阈值；

条件2：与上一关键帧之间的帧数是否达到一定阈值；

条件3：当前帧上非异常匹配点对数达到一定阈值；

条件4：当前帧上所有的特征点中已被匹配的是否小于一定阈值且未被匹配的大于一定阈值；

条件5：当前帧上非异常匹配点对数是否小于上一关键帧上非异常匹配点对数*阈值(0～1.0)；

条件4和条件5是判断当前帧是否具有继续与空间点匹配的潜力。

假设使用&&表示且，即两条件均满足才为真，使用||表示或，即两条件中任意一条件满足即为真，则是否插入关键帧的条件为：((条件1)||(条件2))&&((条件4)||(条件5))&&(条件3)。

然后建立一个统一的目标函数，继续精确优化前端估计的位姿。在本发明中将相机和imu之间的相对旋转参数、平移参数和时钟偏差作为待优化变量，根据对应的约束条件进行优化，从而得到更加精确的外参。

具体步骤包括：

(31)设置待优化项：

外参标定待优化项为：

其中，表示第j关键帧上imu的旋转矩阵、平移向量、速度、陀螺仪的偏差、加速度计的偏差。n表示窗口的大小。表示空间点pk在当前帧坐标系中的逆深度，即z轴上的值的倒数，其中k＝1,2,…,m。表示待估计外参，为从imu坐标系转换到相机坐标系的旋转矩阵，为从imu坐标系转换到相机坐标系的平移向量。

时序标定待优化项为：

其中，表示第j关键帧上imu的旋转矩阵、平移向量、速度、陀螺仪的偏差、加速度计的偏差。n表示窗口的大小。表示空间点pk在当前帧坐标系中的逆深度，即z轴上的值的倒数，其中k＝1,2,…,m。表示待估计外参，为从imu坐标系转换到相机坐标系的旋转矩阵，为从imu坐标系转换到相机坐标系的平移向量；△t表示每一帧中相机和imu的时间偏差。

(32)建立统一的目标函数：

其中，pk表示第k个空间点，j表示当前帧，j-1表示上一帧，eproj(pk,1,j)表示第k个空间点在当前帧的重投影误差，eimu(j-1,j)表示imu误差项；m表示该帧相机能够观测到的空间点的个数，n表示窗口的大小。

目标函数包括两部分：eproj和eimu分别表示相机的重投影误差和imu的一致性误差。其中，外参标定中eproj的定义为：

时序标定中eproj的定义为：

其中，表示当前帧相机坐标系和第j关键帧相机坐标系之间的相对旋转矩阵，表示从世界坐标系转换到第1帧imu坐标系的旋转矩阵。表示从第j帧imu坐标系到世界坐标系的旋转矩阵。表示从相机坐标系转换到imu坐标系的旋转矩阵。表示当前帧相机坐标系和第j关键帧相机坐标系之间的平移向量，表示第1帧imu坐标系和世界坐标系之间的平移向量，表示第j帧imu坐标系和世界坐标系之间的平移向量；norm(pk)表示pk(x,y,z)的归一化坐标pkj表示pk在第j关键帧的观测点坐标。

外参标定和时序标定中eimu(i,j)的定义为：

其中，

其中，er表示imu的旋转矩阵的前后帧一致性误差，ev表示imu的速度的前后帧一致性误差，ep表示imu的位置的前后帧一致性误差，eb表示imu的偏差的前后一致性误差。σi和σr表示误差的协方差矩阵，一般为单位矩阵。△rij表示第i帧坐标系和第j帧坐标系之间在世界坐标系下的旋转矩阵。表示△rij关于的雅可比矩阵，表示从世界坐标系转换到第i帧imu坐标系的旋转矩阵。表示第j帧imu在世界坐标系中的速度，表示第i帧imu在世界坐标系中的速度。g^w表示在世界坐标系下的重力加速度。△tij表示第i帧和第j帧之间的时间差。△vij表示第i帧imu和第j帧imu的速度差异。表示△vij关于的雅可比矩阵。表示△vij关于的雅可比矩阵。表示第j帧imu在世界坐标系下的位置，表示第i帧imu在世界坐标系下的位置，表示第i帧imu在世界坐标系中的速度，△pij表示第i帧imu和第j帧imu在世界坐标系下的位置差异，表示△pij关于的雅可比矩阵，表示△pij关于的雅可比矩阵；和分别表示第j关键帧上陀螺仪的偏差和加速度计的偏差。

(33)设置优化初始值：

θ1,θ2,θ3…θn,p1,p2,p3…pm的初始值来自前端里程计的估计，的初始值来自步骤(1)得到的估计值对应的旋转矩阵的转置矩阵，和△t的初始值均设为0。

(34)使用g2o图优化方法进行优化

图优化是指将优化问题看作成图，待优化变量是顶点，约束条件是边。

使用g2o实现的列文伯格-马夸尔特算法进行优化，在每次迭代时，需要计算目标函数关于待优化变量的雅可比矩阵：计算方式如下：

外参标定中，目标函数关于的雅可比矩阵为：

其中，

时序标定中，目标函数关于δt的雅可比矩阵为：

其中，

针对外参标定，将本发明方法所估计的外参与数据集中的外参进行了比较，如图5所示，本发明方法相对旋转和平移的误差基本能降低到0.02到0.025之间。其中mh_01、mh_02、mh_03、mh_04、mh_05、v1_01、v1_02、v1_03、v2_01、v2_02、v2_03分别表示euroc数据集中的11个序列。并且我们分别比较了使用本发明估计的外参与数据集外参的轨迹误差。如表1所示，表示了使用本发明的标定方法标定的外参与数据集中的外参之间的差异，能够看出使用本发明提出的标定方法得到的外参估计的轨迹误差与使用数据集中外参估计的轨迹误差相差并不大，甚至在某些序列上估计得到的轨迹精度已经超过了使用数据集中的外参估计得到的轨迹精度。

表1

针对时序标定，我们比较了三种方法：vins、viorb和本发明提出的方法。以轨迹的误差作为衡量标准。误差比较如表2所示，表2表示使用viorb、vins和本发明提出的方法得到的轨迹精度比较。从表2能够看出，在11个序列中，8个序列的轨迹均方根误差和平均误差小于其他两种方法，6个序列的尺度误差小于其他两种方法。从轨迹精度上看，针对相机和imu异步的问题，本文提出的方法的精度要优于其他两种方法。

表2

本发明的一种移动平台上基于视觉和惯导融合slam的标定相机和imu的外参和时序的解决方法。在系统初始化阶段，首先进行相机的初始化，然后使用松耦合方法对齐通过视觉和imu估计的相对位姿，估计得到外参中的旋转参数。在前端视觉里程计阶段，计算特征点在图像上的移动速度。在后端优化阶段，将外参中的旋转参数和平移参数作为待优化变量，根据对应的约束条件进行优化，从而得到更加精确的外参。并且将相机与imu的时间偏差作为待优化变量，最后根据约束条件优化得到每一帧的时间偏差。