基于相对旋转角度的相机绝对姿态优化方法

摘要

本发明公开了一种基于相对旋转角度的相机绝对姿态优化方法，包括：基于惯性测量单元输出的角速度得到惯性相对旋转矩阵，并基于相机当前的绝对姿态得到相机相对旋转矩阵；基于惯性相对旋转矩阵与相机相对旋转矩阵得到相对旋转角度偏差；以最小化相对旋转角度偏差为目标进行迭代，并在迭代过程中基于相对旋转角度偏差相对绝对旋转的雅可比矩阵计算绝对旋转的更新量。本发明应用于定位定向领域，利用惯性测量单元和相机在姿态变化过程中各自的相对旋转角相同的性质，将惯性测量单元的相对旋转角直接作为相机的相对旋转角构建约束，基于相对旋转角度即能对绝对姿态进行大幅优化，得到高精度的相机姿态。

权利要求

1.一种基于相对旋转角度的相机绝对姿态优化方法，其特征在于，将惯性测量单元与相机固联并搭载在载体上，所述相机绝对姿态优化方法包括如下步骤：步骤1，获取惯性测量单元输出的角速度，并根据视觉估计得到相机的姿态初值，并将姿态初值作为相机的绝对姿态；

步骤2，基于惯性测量单元输出的角速度得到惯性相对旋转矩阵，并基于相机当前的绝对姿态得到相机相对旋转矩阵；

步骤3，基于惯性相对旋转矩阵与相机相对旋转矩阵得到相对旋转角度偏差；

步骤4，判断当前的相对旋转角度偏差是否符合迭代终止条件：若是，则将相机当前的绝对姿态作为最优绝对姿态并输出；

否则，计算相对旋转角度偏差相对绝对旋转的雅可比矩阵，并根据雅可比矩阵计算绝对旋转的更新量后，基于绝对旋转的更新量更新相机的绝对姿态后，再次进行步骤2至步骤

4；

步骤4中，所述根据雅可比矩阵计算绝对旋转的更新量后，基于绝对旋转的更新量更新相机的绝对姿态，具体为：根据雅可比矩阵计算绝对旋转的更新量，为：式中， 是绝对旋转 的变化量， 是权重矩阵， 为相对旋转角度偏差的列向量， 每行由雅可比矩阵 组成，即共有N个时刻，每两个时刻间计算 、 ；

更新相机的绝对姿态为 。

2.根据权利要求1所述的基于相对旋转角度的相机绝对姿态优化方法，其特征在于，步骤2具体为：基于惯性测量单元输出的角速度积分得到惯性相对旋转矩阵，为：式中， 为时刻 、 间的惯性相对旋转矩阵， 是惯性测量单元的角速度，为惯性测量单元数据输出的时间间隔，f为惯性测量单元数据的输出频率；

基于相机当前的绝对姿态 得到相机相对旋转矩阵，为：式中， 为时刻 、 间的相机相对旋转矩阵， ， 、 是相机在时刻 、 的绝对旋转，i、j为时刻的序号，N为相机绝对姿态的数量。

3.根据权利要求1所述的基于相对旋转角度的相机绝对姿态优化方法，其特征在于，步骤3中，所述相对旋转角度偏差的确定过程为：使用轴角方式表示旋转矩阵，得到惯性相对旋转矩阵对应的惯性相对旋转角度 以及相机相对旋转矩阵对应的相机相对旋转角度 ；

根据惯性相对旋转角度 与相机相对旋转角度 得到相对旋转角度偏差，为：式中， 为相对旋转角度偏差， 表示相对旋转角度偏差 为关于相机绝对旋转更新量 的函数。

4.根据权利要求3所述的基于相对旋转角度的相机绝对姿态优化方法，其特征在于，所述惯性相对旋转角度 、所述相机相对旋转角度 的计算过程为：式中， 为时刻 、 间的惯性相对旋转矩阵；

式中， 为时刻 、 间的相机相对旋转矩阵。

5.根据权利要求1至4任一项所述的基于相对旋转角度的相机绝对姿态优化方法，其特征在于，步骤4中，所述判断当前的相对旋转角度偏差是否符合迭代终止条件，具体为：判断当前是否达到最大迭代次数；或

判断当前迭代步的相对旋转角度偏差与上一迭代步的相对旋转角度偏差之间的误差是否小于精度阈值。

6.根据权利要求1至4任一项所述的基于相对旋转角度的相机绝对姿态优化方法，其特征在于，步骤4中，所述计算相对旋转角度偏差相对绝对旋转的雅可比矩阵，具体为：式中， 为相对旋转角度偏差的变化值， 是相对旋转角度的变化量，， 是相对旋转轴，根据轴角方式表示

计算得到。

说明书

基于相对旋转角度的相机绝对姿态优化方法

技术领域

[0001] 本发明涉及定位定向技术领域，具体是一种基于相对旋转角度的相机绝对姿态优化方法。

背景技术

[0002] 相机姿态优化是三维视觉中的一个基本问题，例如摄像测量、三维重建，同步定位与地图构建（SLAM）。姿态求解的一般过程为，先姿态估计得到初值，再姿态优化得到高精度的测量结果。光束法平差（BA）一种最流行的非线性优化方法，它利用成像关系中的光束约束条件，最小化重投影误差等求解最优结果。姿态优化的另一种方法是旋转平均，它利用相对旋转构建约束，求解一组绝对旋转使通过绝对旋转计算得到的相对旋转与直接测量得到的相对旋转之间差异最小。

[0003] 最近的一些应用，例如手机、无人机，都同时装有相机和惯性测量单元（IMU）等传感器。传感器之间是固连安装的，因此可以将惯性测量单元测量得到的相对旋转作为相机姿态优化的约束。若惯性测量单元与相机的相对关系已精确标定已知，根据惯性测量单元输出的姿态可以直接为相机姿态优化提供约束。但实际中，相对关系的标定并不容易。这是手眼标定问题（hand‑eye calibration）。此外，精确的标定也需要精确的视觉姿态估计。若惯性测量单元与相机之间的相对关系未知，利用惯性测量单元和相机在姿态变化过程中各自的相对旋转角相同的性质，可以直接将惯性测量单元输出的相对旋转角作为相机的相对旋转角构建约束，进行姿态优化。由于不需要标定惯性测量单元与相机间的相对关系，使得惯性测量单元和相机的融合更加灵活方便，具有更广泛的应用场景。

发明内容

[0004] 针对上述现有技术中的不足，本发明提供一种基于相对旋转角度的相机绝对姿态优化方法，可得到高精度的相机姿态。

[0005] 为实现上述目的，本发明提供一种基于相对旋转角度的相机绝对姿态优化方法，将惯性测量单元与相机固联并搭载在载体上，所述相机绝对姿态优化方法包括如下步骤：

[0006] 步骤1，获取惯性测量单元输出的角速度，并根据视觉估计得到相机的姿态初值，并将姿态初值作为相机的绝对姿态；

[0007] 步骤2，基于惯性测量单元输出的角速度得到惯性相对旋转矩阵，并基于相机当前的绝对姿态得到相机相对旋转矩阵；

[0008] 步骤3，基于惯性相对旋转矩阵与相机相对旋转矩阵得到相对旋转角度偏差；

[0009] 步骤4，判断当前的相对旋转角度偏差是否符合迭代终止条件：

[0010] 若是，则将相机当前的绝对姿态作为最优绝对姿态并输出；

[0011] 否则，计算相对旋转角度偏差相对绝对旋转的雅可比矩阵，并根据雅可比矩阵计算绝对旋转的更新量后，基于绝对旋转的更新量更新相机的绝对姿态后，再次进行步骤2至步骤4。

[0012] 在其中一个实施例，步骤2具体为：

[0013] 基于惯性测量单元输出的角速度积分得到惯性相对旋转矩阵，为：

[0014]

[0015] 式中， 为时刻 、 间的惯性相对旋转矩阵， 是惯性测量单元的角速度，为惯性测量单元数据输出的时间间隔，f为惯性测量单元数据的输出频率；

[0016] 基于相机当前的绝对姿态 得到相机相对旋转矩阵，为：

[0017]

[0018] 式中， 为时刻 、 间的相机相对旋转矩阵， ， 、 是相机在时刻、 的绝对旋转，i、j为时刻的序号，N为相机绝对姿态的数量。

[0019] 在其中一个实施例，步骤3中，所述相对旋转角度偏差的确定过程为：

[0020] 使用轴角方式表示旋转矩阵，得到惯性相对旋转矩阵对应的惯性相对旋转角度以及相机相对旋转矩阵对应的相机相对旋转角度 ；

[0021] 根据惯性相对旋转角度 与相机相对旋转角度 得到相对旋转角度偏差，为：

[0022]

[0023] 式中， 为相对旋转角度偏差， 表示相对旋转角度偏差 为关于相机绝对旋转更新量 的函数。

[0024] 在其中一个实施例，所述惯性相对旋转角度 、所述相机相对旋转角度 的计算过程为：

[0025]

[0026] 式中， 为时刻 、 间的惯性相对旋转矩阵；

[0027]

[0028] 式中， 为时刻 、 间的相机相对旋转矩阵。

[0029] 在其中一个实施例，步骤4中，所述判断当前的相对旋转角度偏差是否符合迭代终止条件，具体为：

[0030] 判断当前是否达到最大迭代次数；或判断当前迭代步的相对旋转角度偏差与上一迭代步的相对旋转角度偏差之间的误差是否小于精度阈值。

[0031] 在其中一个实施例，步骤4中，所述计算相对旋转角度偏差相对绝对旋转的雅可比矩阵，具体为：

[0032]

[0033] 式中 ， 为 雅可比 矩阵 ， 为 相对 旋 转 角度 偏差 的 变化 值 ，， 是绝对旋转 的变化量， 是相对旋转角度的变化量， ， 是相对旋转轴，根据轴角方式表示计算得到。

[0034] 在其中一个实施例，步骤4中，所述根据雅可比矩阵计算绝对旋转的更新量后，基于绝对旋转的更新量更新相机的绝对姿态，具体为：

[0035] 根据雅可比矩阵计算绝对旋转的更新量，为：

[0036]

[0037] 式中， 是权重矩阵， 为相对旋转角度偏差的列向量， 每行由雅可比矩阵组成，即共有N个时刻，每两个时刻间计算 、 ；

[0038] 更新相机的绝对姿态为 。

[0039] 与现有技术相比，本发明具有如下有益技术效果：

[0040] 1.本发明利用惯性测量单元与相机固联安装条件下，利用惯性测量单元和相机在姿态变化过程中各自的相对旋转角相同的性质，将惯性测量单元的相对旋转角直接作为相机的相对旋转角构建约束，可应用于惯性测量单元与相机之间的相对关系未知或存在变化等场景；

[0041] 2.本发明提出了一种新的相机姿态优化方法，该优化方法只基于相对旋转角度就可以对绝对姿态进行大幅优化，得到高精度的相机姿态；

[0042] 3.本发明的方法不需要对惯性测量单元与相机进行标定，使得多传感器的融合更加灵活方便，适用于无人机自主导航、自动驾驶汽车和增强现实设备等。

附图说明

[0043] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

[0044] 图1为本发明实施例中相机绝对姿态优化方法的流程图；

[0045] 图2为本发明实施例中视觉测量与IMU测量间关系示意图；

[0046] 图3为本发明实施例中相机绝对旋转与相对旋转关系示意图。

[0047] 本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

[0048] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0049] 需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

[0050] 另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

[0051] 本实施例公开了一种基于相对旋转角度的相机绝对姿态优化方法，可应用于无人机自主导航、自动驾驶汽车和增强现实设备等。该方法通过将惯性测量单元IMU与相机固联并搭载在载体上，利用惯性测量单元和相机在姿态变化过程中各自的相对旋转角相同的性质，将惯性测量单元的相对旋转角直接作为相机的相对旋转角构建约束，只基于相对旋转角度就可以对绝对姿态进行大幅优化，得到高精度的相机姿态。

[0052] 参考图1‑2，本实施例中的相机绝对姿态优化方法具体包括如下步骤：

[0053] 步骤1，获取惯性测量单元输出的角速度 ，并根据视觉估计得到相机的姿态初值，并将姿态初值作为相机的绝对姿态，其中， 为惯性测量单元在时刻t输出的角速度信息， 为相机在时刻 的绝对姿态(初始的绝对姿
态由视觉估计得到)。

[0054] 步骤2，基于惯性测量单元输出的角速度得到惯性相对旋转矩阵，并基于相机当前的绝对姿态得到相机相对旋转矩阵，其具体实施过程为：

[0055] 基于惯性测量单元输出的角速度积分得到惯性测量单元的惯性相对旋转矩阵，为：

[0056]

[0057] 式中， 为时刻 、 间的惯性相对旋转矩阵， 为惯性测量单元数据输出的时间间隔，f为惯性测量单元数据的输出频率；

[0058] 参考图3，可基于相机当前的绝对姿态 得到相机的相机相对旋转矩阵，为：

[0059]

[0060] 式中， 为时刻 、 间的相机相对旋转矩阵， ， 、 是相机在时刻 、 的绝对旋转，i、j为时刻的序号，N为相机绝对姿态的数量。

[0061] 步骤3，基于惯性相对旋转矩阵与相机相对旋转矩阵得到惯性测量单元与相机之间的相对旋转角度偏差，其具体实施过程为：

[0062] 使用轴角方式 表示旋转矩阵 ，其中，为旋转角度，为旋转轴，则有 ， ；

[0063] 根据上述轴角方式得到惯性相对旋转矩阵对应的惯性相对旋转角度 以及相机相对旋转矩阵对应的相机相对旋转角度 ，为：

[0064]

[0065]

[0066] 式中， 为惯性相对旋转角度， 为相机相对旋转角度；

[0067] 再根据惯性相对旋转角度 与相机相对旋转角度 得到相对旋转角度偏差，为：

[0068]

[0069] 式中，为相对旋转角度偏差， 表示相对旋转角度偏差 为关于相机绝对旋转更新量 的函数。

[0070] 步骤4，判断当前的相对旋转角度偏差是否符合迭代终止条件：

[0071] 若是，则将相机当前的绝对姿态作为最优绝对姿态并输出；

[0072] 否则，计算相对旋转角度偏差相对绝对旋转的雅可比矩阵，并根据雅可比矩阵计算绝对旋转的更新量后，基于绝对旋转的更新量更新相机的绝对姿态后，再次进行步骤2至步骤4。

[0073] 在步骤4中，迭代的优化目标为最小化相对旋转角度偏差，即代表此时相机的绝对姿态结果已稳定，因此相机的最优绝对姿态 。在具体实施过程中，判断当前的相对旋转角度偏差是否符合迭代终止条件的方式有多种，例如可判断当前迭代次数是否达到最大迭代次数，若达到则终止迭代，否则继续；也可以判断当前迭代步的相对旋转角度偏差与上一迭代步的相对旋转角度偏差之间的误差是否小于精度阈值，若小于精度阈值则终止迭代，否则继续；也可以同时设置达到最大迭代次数与误差小于精度阈值两种迭代终止条件，任意达到一项即终止迭代。

[0074] 步骤4中，计算相对旋转角度偏差相对绝对旋转的雅可比矩阵，并根据雅可比矩阵计算绝对旋转的更新量后，基于绝对旋转的更新量更新相机的绝对姿态具体实施过程为：

[0075] 首先，计算相对旋转角度偏差相对绝对旋转的雅可比矩阵，具体为：

[0076]

[0077] 式中 ， 为雅 可比 矩 阵 ， 为 相对 旋 转 角度 偏差 的 变化 值 ，， 是绝对旋转 的变化量， 是相对旋转角度的变化量， ， 是轴角方式得到的相对旋转轴；

[0078] 根据雅可比矩阵计算绝对旋转的更新量后，基于绝对旋转的更新量更新相机的绝对姿态，具体为：

[0079] 根据雅可比矩阵计算绝对旋转的更新量，为：

[0080]

[0081] 式中， 是权重矩阵， 为相对旋转角度偏差的列向量，每行由雅可比矩阵组成，即共有N个时刻，每两个时刻间计算 、 ；

[0082] 更新相机的绝对姿态为 。

[0083] 以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。