基于李群滤波的相机与惯性传感器空间关系自标定方法

本发明公开了一种基于李群描述的捷联惯性导航动基座初始对准方法，该方法属于导航方法及应用技术领域。

背景技术：

同步定位与测绘(slam)是自主移动机器人在未知环境中同时构建地图和估计位置的关键技术。在众多的slam系统中，视觉slam由于其成本和功耗等优势，在小行机器人定位和导航任务中具有独特的优势。

近年来，单目slam系统取得了许多重大进展。但是，由于摄像机将物体从3d压缩到2d，单目slam系统不能根据图像的特征信息估计真实深度，因此尺度模糊和漂移是单目slam系统常见的问题。与传统的单目slam相比，单目相机与惯性导航系统的融合，即视觉惯性导航系统，取得了更好的性能和稳定性。从目前开源的vins：msckf、rovio、okvis、vins等vio系统来看，vins可以分为三个部分：前端数据处理、初始化和后端优化。前端处理部分主要包括惯性预积分和视觉图像处理，其中视觉图像处理按照不同的方法可分为光流处理和特征点匹配。后端优化部分采用的方法可分为过滤和优化。

然而，无论采用哪种前端处理方法和后端优化方法，在结合相机和imu测量数据时，高精度的初始化过程是系统顺利运行的前提。它不仅是系统重力和比例初始化的保证，也是摄像机提取的运动信息与imu测量正确融合的基础。因此，为了获得摄像机与imu之间的刚体变换，传感器空间参数的外部标定一直是众多vo研究工作的主题。标定过程中的主要问题是计算相机和imu之间的旋转矩阵。

目前，相机-imu标定可分为离线标定和在线标定两种类型。相比之下，在线校准方法在传感器套件的每次操作过程中都会估算校准参数，使其更加稳健，校准过程不再受制于场景的约束。目前，在线标定也可以分为两种：一种是在摄像机和imu之间进行标定，在标定完成后进行初始化，最后在摄像机标定和初始化完成后将标准量作为一种状态进行优化。其一是在系统运行初期获得具有足够精度的定标结果，并且外部参数不包含在状态估计的优化变量中。第一种方法对相机imu的小变形有较强的鲁棒性，但在使用过程中如果变形过大会产生较大的误差。第二种方法适用于计算量较小的移动设备。

2012年，有人首次提出了一种不考虑传感器噪声的几何标定单目相机-imu外参数在线估计算法，为vio提供了一个新的研究方向。近两年来，随着视觉slam算法精度的提高，相机-imu在线标定与初始化一起成为vio研究的一个独立方向。2018年，li[14]将单目摄像机imu的外部参数作为待估计的状态向量，并使用扩展卡尔曼滤波(ekf)进行估计。然后杨[15]是基于东四的工作和四元数交换定律。在初始化过程中，采用线性估计方程svd分解求解单目摄像机-imu外部参数，但该算法不对imu零点进行标定。同年，黄文[16]在viorb工作[17]的基础上，建立了基于四元数交换定律的线性方程组，并用最优化方法标定了单目相机imu的外部参数。

为进一步提高在线标定的性能，本发明针对现有的在线标定方法存在的问题，借鉴李群状态卡尔曼滤波的思想，提出了一种新的李群卡尔曼滤波算法。得益于李群理论的完善，通过改进新息的表达方式来保证状态量满足特殊正交的性质。基于这种方法构建的估计模型避免了传统最优化方法中的奇异值问题，每一步的迭代计算都符合李群旋转矩阵的特性。相比于四元数优化方法而言，体现李群滤波优势，提高了在线标定的鲁棒性，并且在精度方面也有所提升。实际实验证明了该算法的可行性，可以作为一种新的相机-imu在线标定方法。

技术实现要素：

由于在线相机-imu传感器空间关系标定过程中，传感器容易受到外界的各种干扰因素的影响，其所提供的传感器信息无法保证。因此，基于李群滤波的传感器空间坐标估计方法具有很高的研究意义与应用价值。本发明的目的是为了应对现有相机-imu传感器在线标定方法存在的问题：(1)本发明通过李群代替四元数描述初始姿态矩阵，避免了传统四元数描述方法的非唯一性和非线性问题；(2)本发明将陀螺仪常值漂移与传感器空间坐标同时作为状态量，建立李群模型进行同步估计(3)本发明基于李群描述的相机-imu空间坐标关系在线标定方法，能够有效避免现有四元数方法中由四元数向姿态矩阵转换产生的复杂表述问题和大量计算误差，并能够避免李群滤波方法中由于加性误差和计算误差产生的无法保证旋转矩阵正交性的问题。

为了达到上述目的，本发明提供如下技术方案：

基于李群滤波的相机与惯性传感器空间关系自标定方法，该方法通过下述步骤实现：

步骤(1)：启动系统，计算系统所需要的输入值：

采集惯性测量单元imu中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影wi；采用摄像头提供的图像进行fast特征点的提取和lk光流法追踪，并根据两帧图像间匹配的特征点计算出两帧间相机的旋转矩阵rc；

角速度可以通过视觉信息的旋转差分获得：

由wc相机通过图像信息计算的角速度和由rc相机通过图像信息计算的旋转。

步骤(2)：基于步骤1计算的相机角速度和imu角速度，建立基于李群描述的线性相机-imu在线标定系统模型：

根据导航系统原理，传感器标定可以转换为姿态估计问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态矩阵用一个3×3的正交变换矩阵表示；该正交变换矩阵符合李群的特殊正交群so(n)的性质，构成了三维旋转群so(3)：

其中，r∈so(3)表示为姿态矩阵，表示3×3的向量空间，上标t表示矩阵的转置，i表示三维单位矩阵，det(r)表示为矩阵r的行列式；

根据imu陀螺仪性质其输出值可以分为：

根据imu和相机两传感器其特性构建在线自标定模型：

其中，为相机到imu的空间坐标关系，b表示imu的常值漂移，wc是相机的角速度。wi是陀螺仪的角速度，nc是相机角速度的噪声，ni是imu角速度的噪声。

根据在线标定模型，构建基于李群的自标定模型：

令

δα＝nc

δβ＝ni(5)

公式可以转化为：

β-δβ＝r(α-δα)(6)

步骤(3)：建立李代数空间上的等效状态无关滤波方程，并对状态量rk进行最优估计：

由于相机-imu空间坐标和陀螺仪常值漂移所构建的so(4)矩阵rk不随时间变化，所以有：

其中rk表示k时刻的初始姿态矩阵。

根据误差状态卡尔曼滤波：

第一个方程是所谓的预测步骤，第二个方程是校正步骤。设和δxk-1＝xk-1'-xk-1表示预测和校正的状态估计误差。

根据李群滤波原理，构造了关于李群的误差更新方程。

将δr替换为李代数形式exp([ξk]χ)，将公式6替换为公式10：

其中ξk是ξk-1的后验估计。

根据公式11和指数性质，将等式两边的底数省略，等式两边的指数依然相等。

省略高阶无穷小：

通过比较公式12和公式9，可以将公式10的更新形式简化为公式8的更新形式，从而可以得出结论。式8和式10等价。

由于视觉里程表本身的局限性和在线标定周围环境的不确定性，无法保证标定过程中每一帧提供的信息都是有效的。因此，不能像传统的传感器δα那样从产品手册中获得噪声特性。同时，通过公式6可以发现，噪声δα与状态量有关，不能适应传统的akf。

根据公式13，可以得到更新方程的表达式：

根据更新方程计算其协方差矩阵：

设[δα]χpk|k-1[δα]χ^t-(δα)(δα)^t是pk|k-1的多项式函数f(pk|k-1)。

根据公式13和多时间协方差矩阵p拟合，可以得出结论：

基于李群描述的相机-imu空间坐标滤波估计方法归纳为：

误差重置：

δrk＝i(19)

步骤(4)：求解vio系统所需的传感器坐标关系矩阵，从而vio的初始化过程：

根据误差的协方差矩阵可以对旋转矩阵的三个轴对应的方差进行分解。通过实验可以发现，在输入数据正常的情况下，20s左右就可以基本计算出camera-imu的外部参数，三轴的方差基本收敛到相似的大小。然而，在标定过程中，有时无法保证传感器三个轴的运动激励满足标定的必要条件。因此，不可能保证三个轴同时收敛，导致旋转矩阵的计算不能达到后端优化所要求的精度。

因此，可以得出结论，当一个轴的方差明显大于其他轴的方差时，则可以证明该轴的运动激励不足、为估计系统提供的观测信息不准确，如果将该轴所对应的矩阵数据带入下一阶段来进行vio系统的初始化的计算可能会导致初始化所计算的相机尺度，系统速度初始值和载体下的重力等估计不准确，因此可以增加方差较小的轴的比例，同时继续求解运动激励不足的轴。

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

(1)本发明采用李群描述初始姿态矩阵能够有效避免传统四元数描述初始姿态矩阵产生的非唯一性和非线性问题。

(2)本发明将imu的陀螺仪常值漂移作为状态量与坐标矩阵构建李群模型同步估计，提高了模型的准确性和估计结果的精度。

(3)本发明将李群模型下的状态相关噪声等效为李代数下的状态无关噪声，相比于传统的李群模型，建立了更为精确的李群线性对准模型，去除了李群滤波模型中状态相关噪声对估计结果的影响，提高对准精度。

(4)本发明采用的基于误差状态的iekf的相机-imu空间坐标在线标定方法，能够有效避免传统四元数卡尔曼滤波方法中由四元数向姿态矩阵转换产生的复杂表述问题和大量计算误差，并能够避免李群滤波方法中由于加性误差和计算误差产生的无法保证旋转矩阵正交性的问题。

附图说明

图1为本方法的实施简图。

图2在线标定结果图。

图3为slam结果图。

具体实施方式

本发明是基于李群最优估计的视觉惯性导航系统的传感空间坐标关系在线标定方法设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：

本发明提供的基于李群滤波相机-imu空间坐标关系在线标定方法，首先获取传感器实时数据；对采集到的数据进行处理，基于传感器自身性质，建立基于李群描述的线性在线标定模型；使用李群滤波算法，估计得到基于李群描述的相加-imu空间坐标关系矩阵和陀螺仪常值漂移，完成标定过程。

步骤(1)：启动系统，计算系统所需要的输入值：

采集惯性测量单元imu中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影wi；采用摄像头提供的图像进行fast特征点的提取和lk光流法追踪，并根据两帧图像间匹配的特征点计算出两帧间相机的旋转矩阵rc；

角速度可以通过视觉信息的旋转差分获得：

由wc相机通过图像信息计算的角速度和由rc相机通过图像信息计算的旋转。

步骤(2)：基于步骤1计算的相机角速度和imu角速度，建立基于李群描述的线性相机-imu在线标定系统模型：

根据导航系统原理，传感器标定可以转换为姿态估计问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态矩阵用一个3×3的正交变换矩阵表示；该正交变换矩阵符合李群的特殊正交群so(n)的性质，构成了三维旋转群so(3)：

其中，r∈so(3)表示为姿态矩阵，表示3×3的向量空间，上标t表示矩阵的转置，i表示三维单位矩阵，det(r)表示为矩阵r的行列式；

根据imu陀螺仪性质其输出值可以分为：

根据imu和相机两传感器其特性构建在线自标定模型：

其中，为相机到imu的空间坐标关系，b表示imu的常值漂移，wc是相机的角速度。wi是陀螺仪的角速度，nc是相机角速度的噪声，ni是imu角速度的噪声。

根据在线标定模型，构建基于李群的自标定模型：

令

δα＝nc

δβ＝ni(24)

公式可以转化为：

β-δβ＝r(α-δα)(25)

步骤(3)：建立李代数空间上的等效状态无关滤波方程，并对状态量rk进行最优估计：

由于相机-imu空间坐标和陀螺仪常值漂移所构建的so(4)矩阵rk不随时间变化，所以有：

其中rk表示k时刻的初始姿态矩阵。

根据误差状态卡尔曼滤波：

第一个方程是所谓的预测步骤，第二个方程是校正步骤。设和δxk-1＝xk-1'-xk-1表示预测和校正的状态估计误差。

根据李群滤波原理，构造了关于李群的误差更新方程。

将δr替换为李代数形式exp([ξk]χ)，将公式25替换为公式29：

其中ξk是ξk-1的后验估计。

根据公式30和指数性质，将等式两边的底数省略，等式两边的指数依然相等。

省略高阶无穷小：

通过比较公式31和公式28，可以将公式29的更新形式简化为公式27的更新形式，从而可以得出结论。式27和式29等价。

由于视觉里程表本身的局限性和在线标定周围环境的不确定性，无法保证标定过程中每一帧提供的信息都是有效的。因此，不能像传统的传感器δα那样从产品手册中获得噪声特性。同时，通过公式25可以发现，噪声δα与状态量有关，不能适应传统的akf。

根据公式32，可以得到更新方程的表达式：

根据更新方程计算其协方差矩阵：

设[δα]χpk|k-1[δα]χ^t-(δα)(δα)^t是pk|k-1的多项式函数f(pk|k-1)。

根据公式32和多时间协方差矩阵p拟合，可以得出结论：

基于李群描述的相机-imu空间坐标滤波估计方法归纳为：

误差重置：

δrk＝i(38)

步骤(4)：求解vio系统所需的传感器坐标关系矩阵，从而vio的初始化过程：根据误差的协方差矩阵可以对旋转矩阵的三个轴对应的方差进行分解。通过实验可以发现，在输入数据正常的情况下，20s左右就可以基本计算出camera-imu的外部参数，三轴的方差基本收敛到相似的大小。然而，在标定过程中，有时无法保证传感器三个轴的运动激励满足标定的必要条件。因此，不可能保证三个轴同时收敛，导致旋转矩阵的计算不能达到后端优化所要求的精度。

因此，可以得出结论，当一个轴的方差明显大于其他轴的方差时，则可以证明该轴的运动激励不足、为估计系统提供的观测信息不准确，如果将该轴所对应的矩阵数据带入下一阶段来进行vio系统的初始化的计算可能会导致初始化所计算的相机尺度，系统速度初始值和载体下的重力等估计不准确，因此可以增加方差较小的轴的比例，同时继续求解运动激励不足的轴。

本发明的有益效果如下：

开放数据集eurc提供的图像信息可以在不改变光照和动态物体的情况下提取良好的角点，因此由图像计算的摄像机旋转和svd分解提供的系统初始值非常准确。但是，对于在线定标任务，由于应用场景的不确定性，图像提供的观测信息的质量无法保证。当这些问题发生时，基于优化的外部参数估计方法与滤波相比会有很大的局限性，所以为了证明的算法在图像信息较差的情况下仍然可以有效地运行，选择了一个噪声较大的室外环境来测试该算法。

实施详情

为了验证复杂环境下的实验效果，搭建了由采集设备、计算设备和验证设备组成的实验平台。

realsensed435i相机，在本实验中用于捕捉图像和imu。差分gps模块，分为地面站和移动终端两部分。为实验提供了全套设备的实时位置信息，验证了该方法和vins在线标定方法对vio系统的影响。显示便携式pc平台通过记录ros下的袋子来存储传感器的实时串行数据，包括时间、图像和imu数据，以及差分gps。

由于光照变化和动态物体在实际应用中的必然性，人们认识到光照变化和动态物体是影响视觉slam精度的两个最大因素，因此相机-imu的在线标定问题也应考虑在内。在本实验中，为了验证算法在系统初值和观测信息不能得到保证的情况下仍然有效，实验地点选择在绿树成荫的校园道路上。

外部参照比较：

为了给实验提供对比，首先使用了苏黎世联邦理工学院(ethz)开发的最广泛使用的kalibr相机和imu离线校准方法，对传感器的内外参数进行校准，为实验提供真实值。然后运行带外部参数校准的vins系统，输出计算的外部参数结果，为实验提供对比度。

将本算法计算的camera-imu的旋转矩阵与vins计算的同时与工具箱计算的旋转矩阵进行比较。对收敛后参数进行了分析，对比结果和时间如图2和表4所示：

表.4.

姿态角误差与对准时间的比较

如图2所示，虽然蓝线代表的改进iekf外部参数估计误差的收敛速度慢于开放数据集，但仍能在30秒左右收敛到合理范围。从表4可以看出，在复杂的室外环境下，考虑陀螺恒定漂移的iekf相机-imu外部参数在线标定方法仍能准确估计传感器外部参数，精度小于0.5度。vins提供的方法收敛到错误的区间，这可能是由于对光照变化的非线性初值计算错误所致。

slam比较：

在光照变化明显的林道上，将本文算法和vins计算的摄像机imu旋转引入vins系统，将系统的位置和姿态信息与差分gps输出的实际位置值进行比较，并比较了不同外部参数估计方法对vo系统的影响。vio的输出如图3和表5所示。

表.5.

slam结果误差统计

从图3和表5中的vio误差统计特征可以看出，在复杂的室外照明环境下，图像观测信息的准确性得不到保证，的算法仍然可以为vio系统提供良好的摄像机imu外部参数，从而获得更准确的vio结果。

本发明采用李群描述代替传统四元数描述实现对相机-imu空间坐标关系的在线计算，利用李群微分方程建立基于李群描述的线性标定滤波模型。从而实现了相机-imu空间坐标关系和陀螺仪常值漂移的同步估计，并避免了传统四元数描述初始姿态矩阵而产生的非唯一性和非线性问题；本发明利用李群的左乘不变性、误差状态卡尔曼以及李群与李代数之间的指数映射对状态相关误差进行等效变换，建立等效的状态无关李代数滤波方程，避免了状态相关噪声对滤波结果的影响，有效提高了对准精度。

以上所述仅是本发明的优选实施方式，并不用于限制本发明。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。