一种单目相机和惯性测量单元相对安装角标定方法与流程

本发明属于传感器标定领域，特别是涉及一种单目相机和惯性测量单元的测量组件中两者测量坐标系空间相对安装角的标定方法，涉及标定过程中的设备旋转方式，属于多传感器外参标定方法。

背景技术：

基于单目相机和惯性测量单元(inertialmeasurementunit，imu)的传感器组合在导航、三维重建等领域得到了广泛的应用，所述imu由三轴加速度计和三轴陀螺仪组成。imu输出带宽大，对高机动跟踪性好，短时计算精度高，可提供尺度信息，但长期计算积累误差巨大；由单目相机采集的图像信息获得的相对运动信息不存在积累误差，长期稳定性高，但受特征点匹配等因素影响，计算精度一般，且采样率低，载体做高机动时无法进行有效的运动恢复，单目相机无法提供尺度。两种传感器在测量特性上存在互补，将它们组合起来进行数据融合能够大大提升系统性能。

多传感器的数据融合首先要解决时空对准问题。对于空间对准来说，就是要将不同传感器测量数据统一到同一坐标系下，这就需要求取传感器之间的相对位姿关系。针对单目相机和imu的组合，就是要求取单目相机坐标系和imu载体系的相对安装角以及原点平移。

针对这一问题，已经有不少学者进行了研究，但大都需要依托绝对参考信息来进行衡量，例如引入重力测量来寻找水平面基准，或者引入靶标等。当重力无法准确测量或需要快速标定无法预置靶标时，这些方法将不再适用。

技术实现要素：

本发明针对由单目相机和imu两者固连组合而成的测量组件，即所涉及的测量组件在使用过程中不存在单目相机和imu间相对位置和姿态变化，或由于震动造成的相对位置和姿态变化可以忽略不计。

本发明的目的是为了解决imu和单目相机两者坐标系空间相对安装角的无依托标定问题，但同时也适用于有靶标的情况，提出一种基于单目相机和imu的相对安装角标定方法，该方法对靶标无硬性要求，在需要快速标定时可避免前期的绝对参照靶标准备环节，同时规避了对靶标或重力矢量等绝对参考测量不准确时带来的标定误差，为特定应用的快速标定提供了实现方案。同时当存在绝对靶标时，本方法也同样适用。

本发明提供的所述单目相机与imu相对安装角标定方法，是一种非线性优化标定方法，具体包括以下步骤：

步骤一：转动测量组件，采集标定用数据；所述的测量组件包括单目相机和imu；

步骤二：从单目相机采集的图像中提取相对姿态信息；

步骤三：利用非线性最小二乘法求解安装矩阵，实现单目相机和imu相对安装角的标定。

本发明的优点在于：

(1)所需的设备简单，必要的设备只包括单目相机和imu组成的测量组件以及配套的数据采集装置和标定计算平台；

(2)提出的标定方法不依赖于绝对参考信息的测量，可省去前期准备时间，同时避免了绝对参考信息测量误差带来的标定误差；

(3)在特定的旋转条件下，只需很短的数据采集时间就可进行标定，且所谓的特定旋转实际上很容易进行；

(4)采用的代价函数引入了李代数的概念，关于旋转摄动的误差项模型易于线性化，使得常规的非线性最小二乘优化方法得以适用；

(5)标定结果精度高，可以满足单目相机和imu组合配置在多种应用中的空间对准需求。

附图说明

图1是本发明提供的单目相机和惯性测量单元相对安装角标定方法流程图；

图2为单目相机采集图像时的旋转过程以及旋转过程中旋转轴交变的示意图；

图3为手持相机具体转动示意图。

具体实施方式

下面将结合附图和具体实施方式对本发明作进一步的详细说明。

在现有单目相机和imu测量组件中，二者的相对安装角标定方法需要对绝对参考信息进行测量，而这些绝对参考信息或需要事先布置，或需要进行较高精度的测量，导致前期准备时间长、开销大，且标定结果易受绝对参考信息测量误差影响。为了解决上述技术问题，本发明提出了一种只依赖相对姿态测量的相对安装角非线性优化标定方法，给出了标定过程应当采取的几种旋转方式，并提出了非线性最小二乘法需要最小化的代价函数的几种形式。

本发明是一种单目相机和imu相对安装角标定方法，其目的是求解单目相机的相机坐标系相对于imu的载体系的旋转矩阵(或称“安装矩阵”)。

由于涉及到多个坐标系，先对各坐标系符号和定义作出说明：

(1)i系：惯性参考系；

(2)b系：与惯性测量单元(imu)固连的载体系，右手系，三轴方向分别为右-下-前，陀螺的测量输出即为该b系三轴投影坐标；每个瞬时的载体系也是一个惯性系；

(3)c系：与单目相机固连的相机坐标系，右手系，三轴方向分别为右-下-前；每个瞬时的相机坐标系也是一个惯性系；

(4)c1系：标定时间段的起始瞬时(起始时刻即用于标定的首帧图像采集时刻)的相机坐标系，在起始瞬时其指向不变，因此它是一个惯性系。

另外需要说明的几点有：本发明的方法只对相对旋转关系进行标定，不关注原点平移；本发明方法不包括数据采集系统的时间同步策略，认为不同传感器的时间同步是已经完成了的；本发明对组成测量组件的单目相机和imu的选型、数据的存储格式等没有硬性要求，不涉及测量组件的软硬件、电路等具体设计，对采样频率等有一定要求；本发明不关注各传感器本身的噪声特性，因此认为陀螺仪的测量噪声特性是事先标定好的，其零偏得到有效补偿，短时间内的测量噪声对积分运算影响不大；本方法只关注单目相机和imu间相对安装角的标定，因此认为单目相机的内参矩阵是事先标定好的。

所述的单目相机和imu相对安装角标定方法，如图1所示流程，包括以下步骤：

步骤一：转动测量组件，采集标定用数据；

所述的测量组件包括单目相机和imu；采集的标定用数据包括陀螺仪测量的角速度和单目相机采集的图像。标定用数据采集过程中标定场选择、测量组件转动方式、数据采集频率以及采集时长应当按如下要求：

(1)标定场选择；

单目相机采集的图像中特征点方位的精度，将影响视觉测量姿态解算的精度。特征点的精度则由单目相机分辨率、焦距以及特征点对应的实际点位置到单目相机的距离决定。在进行标定时，单目相机分辨率和焦距都已经固定，因此只能依赖标定场的选择来提高特征点测量精度。

在不考虑边缘畸变的情况下，受限于单目相机分辨率，特征点对应的实际点位置距单目相机越远，其方位测量精度越差。而考虑边缘畸变时，即便是距离较近的特征点，如果位于图像的边缘，其测量精度也会较差。考虑单目相机的上述测量特性，针对室内机器人、小型无人机以及手机等常选用的单目相机的焦距范围(12mm-35mm)和像素分辨率(600万dpi-2000万dpi)，对标定场选择提出以下指导方案：

选择特征丰富、易识别、光照变化不显著且无移动物体的场景进行标定，例如选择有很多店铺的街道、在建筑风格不单一的建筑前，避免选择无纹理、弱纹理或重复纹理较多的场景。90％以上的特征点对应的实际点位置到单目相机的距离应位于10m至20m之间，且这些处在适宜距离的特征点对应的实际点在视场内分布比较均匀，避免大多数特征集中在一小块区域。

(2)测量组件转动方式；

为实现安装角的在线标定，需要利用测量组件的旋转，通过短时间内(一分钟内)视觉测量与惯性递推解算的差异性获得安装角的估计。在转动过程中，为实现三维安装角的估计，从安装角的可观性角度出发，需要设计特定的转动方案。

转动方案的核心设计原则是出现绕不定轴的转动，即进行旋转轴交变或连续的改变旋转轴指向，避免测量组件在旋转过程中旋转轴指向固定或近似固定，旋转轴交变旋转的示意图如图2所示，图中展示的是先绕zc轴旋转，再绕xc轴旋转，最后绕yc轴旋转的旋转轴交变，但实际过程中并不一定要按照这种旋转顺序。该示意图强调的是应当让三个轴都能敏感到旋转，以及旋转过程中旋转轴应当发生变化。同时为防止图像模糊或连续帧缺乏共视特征点，旋转轴交变过程中旋转角速度建议不超过20°/s，不宜过大，若出现图像模糊现象则需要降低转速使图像清晰。

定性的给出一种满足要求的旋转轴交变过程中旋转角速度，如下式：

和分别表示旋转角速度矢量在对应时刻相机系下x、y、z三轴的投影，单位为°/s。π为圆周率。为周期为10s、幅值为ax°/s的正弦曲线；为周期为20s、幅值为ay°/s的正弦曲线；为周期为20s、幅值为az°/s的余弦曲线；t为时间。ax、ay和az取0.5°/s。

在快速标定的应用场景中，实际转动过程往往是手持进行的，不可能进行类似上述的标准旋转。展示上述公式的作用，是对旋转轴的交变概念作出定性描述，并对转动幅值进行大致定量的示范。实际的转速可以提高，原则是只要拍出的图像不模糊即可。

给出一种实际可执行的测量组件手持转动方案。根据相机坐标系(c系)的定义可知，其x轴(xc)由相机光心指向右，z轴(zc)由相机光心指向前，y轴(yc)由相机光心指向下。首先将相机对准标定场中心位置(即zc轴大致对准标定场中心位置)，同时使其xc轴大致水平，yc轴大致指向铅垂线方向。随后手持相机进行具体的转动方案如图3所示：手持相机进行绕yc轴的左右(观察视角为沿yc轴方向)转动，先向左转动，再向右转动，最后回到原始位置，转动幅度不超过30°以避免标定场完全不在视野内。然后进行绕xc轴的上下(观察视角为沿xc轴反向)转动，先向上转动，再向下转动，最后回到原始位置，转动幅度不超过30°以避免标定场完全不在视野内。然后绕zc轴转动一周回到原始位置。

(3)数据采集频率及采集时长；

为保证积分精度，陀螺仪的数据采集频率不应低于100hz。同时考虑到低成本惯性器件的测量误差较大，较长时间的积分将使得积累误差不可接受，因此数据采集时间不宜过长，推荐有效数据采集时长在10s至20s之间。

为使单目相机采集图像的相邻帧尽量敏感到较明显的旋转，同时在较短的采集时间内数据量足够，单目相机图像采集频率推荐在1hz至5hz之间。

为使单目相机采集的图像不模糊，快门速度建议在10ms以下。

步骤二：从单目相机采集的图像中提取相对姿态信息；

本步骤将从单目相机采集的图像中提取相对姿态信息，包括每个图像采样时刻的相机坐标系相对于上一个图像采样时刻相机坐标系的相对姿态，以及每个图像采样时刻的相机坐标系相对于标定初始时刻相机坐标系的相对姿态，作为后面优化处理中代价函数误差项的参考基准。

本方法采用无依托标定，没有绝对参考系作为参考，因此需要选定参考坐标系作为定义姿态的基准。选取这样的一个惯性系作为参考坐标系：它相对于惯性空间静止，且坐标原点与标定用数据中第一帧图像拍摄时刻(即起始瞬时)的相机坐标系(c1系)的原点重合，坐标轴与c1系的坐标轴重合，在不引起混淆的情况下，认为瞬时的c1系即该参考坐标系，其符号也取c1。

(1)相邻帧特征点匹配；

先进行相邻帧之间的图像的特征点匹配。包括单帧特征点提取、特征点粗匹配和匹配后的特征点筛选等。

现有技术中已经有很多成熟的图像特征点提取和特征点粗匹配的方法，例如harris角点(harriscorner)、sift(scaleinvariantfeaturetransform)、surf(speededuprobustfeatures)和orb(orientedfastandrotatedbrief)等。针对需要快速标定的应用，为了尽量提高标定的计算速度，在不损失精度的情况下，应选取运算速度快的特征检测子和描述子，orb特征基于fast(featuresfromacceleratedsegmenttest)特征改进，并且采用brief(binaryrobustindependentelementaryfeatures)二进制描述子，在特征明显的场景下，其精度和快速性都能满足要求。

实际操作中可根据需求选择相应特征和描述子进行匹配：需要快速标定时可选择采用orb特征；需要较高精度而不考虑计算速度时可采用sift或surf特征。前者适用于只要给定粗略安装角的场景，如有些应用场景中会在线对安装矩阵进行精细的估计，但需要一个合理的初值，例如港科大的单目视觉惯性里程计方案vsins。后者只能适用于一次性标定，但由不具备专用靶标的应用场景，例如引入惯性测量信息的单目视觉orbslam方案。

匹配后的特征点筛选包括剔除误匹配野值以及特征点分布筛选。针对特征点粗匹配结果中的误匹配野值，采用标准的ransac(randomsampleconsensus)算法进行剔除。在剔除误匹配野值后的特征点中，再进行特征点分布筛选，以提高本质矩阵计算精度，所述本质矩阵用来描述两帧图像采集时刻的相机坐标系间的对极约束关系，它可以反映两帧间的相对位姿关系。可采用限定图像中特征点间最小距离的方式进行特征点分布筛选，使得筛选后的特征点分布均匀。同时，筛选完成后的特征点不应靠近图像边缘，以减少图像畸变影响，可通过限定特征点像素坐标的行列上下限来实现。

(2)相邻帧旋转矩阵计算；

利用筛选后的特征点计算相邻两帧的本质矩阵，可采用8点法或5点法进行求解。

然后从本质矩阵中恢复单目相机相对运动。对本质矩阵进行svd分解，获得多组旋转矩阵r和多组单位平移量v，并利用特征点应当在单目相机前面这一特性从多组r和多组v中筛选出正确的r和v组合。记第k-1帧和第k帧间的旋转矩阵为rk-1,k，表示相邻帧间旋转关系的方向余弦阵即为旋转矩阵，即

(3)计算每帧相对初始帧的旋转；

对图像特征点连续进行上述操作即可得到描述所有相邻两帧图像相机坐标系之间姿态变化的旋转矩阵r1,2、…、rk-1,k。

利用上述的相邻两帧间旋转矩阵，可计算除初始帧外每一帧相机坐标系相对于初始帧相机坐标系的旋转矩阵，计算初始帧(第1帧)到第k帧的旋转矩阵(方向余弦阵)计算公式如下：

式中，π表示连乘符号。

步骤三：利用非线性最小二乘法求解安装矩阵，实现单目相机和imu相对安装角的标定；

接下来利用非线性最小二乘方法对单目相机相对于imu的安装矩阵进行优化求解。该步骤包括设计代价函数、非线性最小二乘法优化和对于不同参考坐标系代价函数优化结果的选择。

(1)设计代价函数；

进行非线性优化的关键是确定代价函数。根据考量相对姿态的参考坐标系的选择，提出的标定方法中所使用的代价函数分为固定参考形式和增量形式两种；考虑到求解安装矩阵是流形优化问题，可引入李代数对代价函数作进一步变形。因此，代价函数将具有多种形式，在实际的计算中，针对每种参考坐标系，可选用其对应的任意一种代价函数进行优化。

①固定参考形式的代价函数设计；

当选择瞬时的c1系为参考坐标系，相对姿态始终相对该坐标系进行考量时，代价函数的形式称为固定参考形式。

标定过程中所有采样时刻陀螺仪测量的角速度可经过如下推导得到对应时刻的c系相对于瞬时的c1系的角速度：

其中表示陀螺仪测量的角速度，即b系相对于惯性系的旋转角速度在b系下的投影；表示c1系相对于惯性系的旋转角速度在b系下的投影；表示b系相对于c1系的旋转角速度在b系下的投影，由于c1系为惯性系，因此该项始终为0；表示c系相对于c1系的旋转角速度在b系下的投影；表示b系相对于c系的旋转角速度在b系下的投影，由于imu和单目相机始终固连，因此该项始终为0；表示c系相对于c1系的旋转角速度在b系下的投影。上式的推导利用了瞬时的c1系相对于惯性系固定，且imu和单目相机固连安装因此任意时刻的b系相对于c系固定。

安装矩阵定义为由b系到c系的方向余弦阵。描述该安装矩阵的具体的旋转方式可自行定义，本发明中采用如下定义：b系和c系均采用右下前右手系，三个安装误差角分别为方位误差角ψ、俯仰误差角θ和滚转误差角γ，则可通过b系和c系间坐标系按三个安装误差角的依次旋转来描述和构造安装矩阵，有由b系到c系的旋转方式为：先绕yb轴旋转ψ，再绕xb轴旋转θ，最后绕zb轴旋转γ。于是可得的表达式如下：

根据上式，任意给定一组安装误差角ψ、θ和γ，可计算得到一个安装矩阵。设定一个安装矩阵估计值则可利用陀螺测量来进行对应时刻单目相机的相机坐标系相对初始帧相机坐标系的相对姿态的解算：

其中表示c1系到c系的方向余弦阵的估计值；表示c系相对于c1系的旋转角速度在c系下的投影的估计值；表示安装矩阵估计值；表示c系相对于c1系的旋转角速度在b系下的投影，由前述分析可知它等于陀螺仪测量的角速度表示角速度对时间积分，将得到旋转矢量；∧运算符表示由矢量求反对称阵；exp(·)为矩阵指数映射，将反对称阵转化为so(3)(三维特殊正交群)的元素。

于是安装矩阵可由令下述代价函数最小来得到，实际计算时可任选其一：

其中，k＝1,...,n表示单目相机的每个采样时刻，n表示采样总数；表示矩阵的转置，即函数eulr(·)表示由方向余弦阵求欧拉角；矩阵对数映射log(·)表示将so(3)(三维特殊正交群)中的方向余弦阵转换到(三维特殊正交群对应的李代数)中的旋转矢量反对称阵；∨运算符表示由反对称阵求旋转矢量；函数qtn(·)表示由方向余弦阵求单位四元数；表示四元数乘法；||·||2表示求矩阵二范数；i表示单位矩阵。

实际计算中，根据参考系的选择，若确定采用固定参考形式的代价函数，且旋转不会出现奇异影响解算精度，则在上述四个代价函数中选择一个进行优化。

上述代价函数的物理意义，是比较利用陀螺仪测量的角速度投影到相机坐标系积分出来的,相对于初始相机坐标系的姿态，和单目相机采集的图像计算出的相对于初始相机坐标系的姿态。每个采样时刻的方向余弦阵，表示的是当前相机坐标系相对于初始相机系的方向余弦矩阵；每个采样时刻的欧拉角，表示的是当前相机坐标系相对于初始相机系的欧拉角；每个采样时刻旋转矢量的物理意义，表示的是当前相机坐标系相对于初始相机系的旋转，该矢量的方向是旋转轴的方向，而模值的大小表示旋转的角度；每个采样时刻的旋转四元数，表示的是当前相机坐标系相对于初始相机系的归一化旋转四元数(即单位四元数)。简而言之，此时每个误差项考察的是某帧拍摄时刻的相机坐标系相对于第一帧拍摄时刻的相机坐标系的姿态。

②增量形式的代价函数设计；

当代价函数中的每个误差项仅考虑相邻两帧的相对姿态变化时，可设计增量形式的代价函数。此时不存在统一的参考坐标系，每个误差项中选择相邻两帧中前一帧拍摄时刻的相机坐标系作为度量相对姿态的参考。

标定过程中单目相机采集的第k-1帧图像和第k帧图像间，陀螺仪测量的角速度可经过如下推导得到对应时刻的c系相对于瞬时的ck-1系的角速度：

其中表示陀螺仪测量的角速度，即b系相对于惯性系的旋转角速度在b系下的投影；表示b系相对于瞬时的ck-1系的旋转角速度在b系下的投影，表示瞬时的ck-1系相对于惯性系的旋转角速度在b系下的投影，由于任意瞬时的ck-1系为惯性系，因此该项始终为0；表示c系相对于瞬时的ck-1系的旋转角速度在b系下的投影；表示b系相对于c系的旋转角速度在b系下的投影，由于imu和单目相机始终固连，因此该项始终为0；表示c系相对于瞬时的ck-1系的旋转角速度在b系下的投影。上式的推导利用了瞬时的ck-1系相对于惯性系固定，且任意瞬时的b系相对于c系固定。

设定一个安装矩阵估计值则可利用陀螺测量来进行单目相机采集的相邻帧图像相机坐标系间相对姿态的解算：

其中表示瞬时的ck-1系到瞬时的ck系的方向余弦阵的估计值；表示c系相对于瞬时的ck-1系的旋转角速度在c系下的投影的估计值；表示安装矩阵估计值；表示c系相对于瞬时的ck-1系旋转角速度在b系下的投影，由前述分析可知它等于陀螺仪测量值表示角速度对时间积分，将得到旋转矢量。

于是安装矩阵可令下述代价函数最小来得到，实际计算时可任选其一：

实际计算中，根据参考系的选择，若确定采用增量形式的代价函数，且旋转不会出现奇异影响解算精度，则在上述四个代价函数选择一个进行优化。

上述代价函数的物理意义，是对利用陀螺测量的角速度投影到相机坐标系积分出来的每个相机采样时刻相对于上个相机采样时刻的姿态增量，和单目相机采集的图像计算出的相邻帧姿态增量进行比较。每个采样时刻的方向余弦阵，表示的是当前相机坐标系相对于上一帧相机坐标系的方向余弦矩阵；每个采样时刻的欧拉角，表示的是当前相机坐标系相对于上一帧相机坐标系的欧拉角；每个采样时刻旋转矢量的物理意义，表示的是当前相机坐标系相对于上一帧相机坐标系的旋转，该矢量的方向是旋转轴的方向，而模值的大小表示旋转的角度；每个采样时刻的旋转四元数，表示的是当前相机坐标系相对于上一帧相机坐标系的归一化旋转四元数(即单位四元数)。

③引入李代数的代价函数设计；

综上所述，代价函数无论是一般形式还是增量形式，姿态的数学表达方式都包括了方向余弦阵、欧拉角、旋转矢量以及归一化四元素等。在实际计算时，可根据标定过程中旋转幅度的大小，从非奇异性、表达的直观性以及计算的便利性等方面考虑，选择合适的代价函数。

求解使得代价函数最小的这一问题可由非线性最小二乘方法实现，而常用的非线性最小二乘方法中都需要求解jocobian矩阵。注意到，安装矩阵属于so(3)，其对加法不封闭，不便于求取jocobian矩阵，因此在代价函数中引入李代数，在求解过程中将其转化为(3)上旋转矢量的求解，转换成流形上的优化问题，即在代价函数中用下式替换

其中exp(·)为矩阵指数映射，将反对称阵转化为so(3)的元素；θbc表示安装矩阵对应的旋转矢量；∧运算符表示由矢量求反对称阵。上述固定参考形式和增量形式的代价函数的形式变化如下，实际计算时可任选其一：

表示t时刻陀螺仪测量的角速度。

实际计算中，根据参考系的选择，首先分别选择一个固定参考形式和增量形式的代价函数(若考虑到旋转可能出现奇异影响解算精度，则选择李代数表示的代价函数)，即在上述八个代价函数选择2个进行优化(f11(θbc)～f14(θbc)中选择一个；f21(θbc)～f24(θbc)中选择一个)。

(2)非线性最小二乘法优化；

通过最小化上述代价函数之一求得安装矩阵是一个无约束的非线性最小二乘问题，解决该问题常用的算法包括一阶(最速下降)和二阶梯度法、高斯牛顿法、列文伯格-马奎尔特法等。其中列文伯格-马奎尔特法以其较高的鲁棒性在非线性最小二乘问题求解中得到广泛采用。本方法即采用列文伯格-马奎尔特法进行非线性优化。

优化上述相应的代价函数fjq(θbc)(j＝1,2，q＝1,2,3,4)获得安装矩阵的过程用数学公式描述如下：

(3)对于不同参考坐标系代价函数优化结果的选择

对于分别采用不同参考坐标系的代价函数优化的结果，需进行选择，确定一种代价函数优化的结果为最终的安装矩阵。设采用一个固定参考形式代价函数和一个增量形式代价函数求得的方向余弦阵分别为和具体方法如下：

利用矩阵对数映射log(·)以及∨运算符将和分别转换为对应的安装角旋转矢量和

求和模值之差的绝对值：

其中norm(·)为向量求模运算符；abs(·)为求绝对值运算符；δσ为两个旋转矢量模值之差的绝对值，单位采用度(°)。

当δσ小于一定阈值(例如取0.01°)时，认为采用不同参考坐标系的代价函数估计结果接近，在和中取任意一个即可。

当δσ大于等于阈值时，则将两种参考坐标系代价函数的估计结果分别代入选取的两个代价函数中，以引入李代数的代价函数为例，即分别求取和和(m和n在1～4中任取一值)。若判断且则认为固定参考系形式代价函数的估计结果好，最终安装矩阵取若判断且则认为增量形式代价函数的估计结果好，最终安装矩阵取其他情况时则认为本次标定采集数据或其他中间解算过程误差较大，结果不予采纳，需要重新标定。