(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211402595.4 (22)申请日 2022.11.10 (71)申请人 中国人民解 放军国防科技大 学 地址 410073 湖南省长 沙市开福区德雅路 109号 (72)发明人 尚洋　李彬　关棒磊　梁顺坤　 (74)专利代理 机构 长沙国科天河知识产权代理 有限公司 432 25 专利代理师 赵小龙 (51)Int.Cl. G01C 21/16(2006.01) G06F 17/16(2006.01) G01C 21/20(2006.01) (54)发明名称 基于相对旋转角度的相机绝对姿态优化方 法 (57)摘要 本发明公开了一种基于相对旋转角度的相 机绝对姿态优化方法， 包括： 基于惯性测量单元 输出的角速度得到惯性相对旋转矩阵， 并基于相 机当前的绝对姿态得到相机相对旋转矩阵； 基于 惯性相对旋转矩 阵与相机相对旋转矩 阵得到相 对旋转角度偏差； 以最小化相对旋转角度偏差为 目标进行迭代， 并在迭代过程中基于相对旋转角 度偏差相对绝对旋转的雅可比矩 阵计算绝对旋 转的更新量。 本发明应用于定位定向领域， 利用 惯性测量单元和相机在姿态变化过程中各自的 相对旋转角相同的性质， 将惯 性测量单元的相对 旋转角直接作为相机的相对旋转角构建约束， 基 于相对旋转角度即能对绝对姿态进行大幅优化， 得到高精度的相机姿态。 权利要求书3页 说明书6页 附图1页 CN 115451958 A 2022.12.09 CN 115451958 A 1.一种基于相对旋转角度的相机绝对姿态优化方法， 其特征在于， 将惯性测量单元与 相机固联并搭 载在载体上， 所述相机绝对姿态优化方法包括如下步骤： 步骤1， 获取惯性测量单元输出的角速度， 并根据视觉估计得到相机的姿态初值， 并将 姿态初值作为相机的绝对姿态； 步骤2， 基于惯性测量单元输出的角速度得到惯性相对旋转矩阵， 并基于相机当前的绝 对姿态得到相机相对旋转矩阵； 步骤3， 基于惯性相对旋转矩阵与相机相对旋转矩阵得到相对旋转角度偏差； 步骤4， 判断当前的相对旋转角度偏差是否符合迭代终止条件： 若是， 则将相机当前的绝对姿态作为 最优绝对姿态并输出； 否则， 计算相对旋转角度偏差相对绝对旋转的雅可比矩阵， 并根据雅可比矩阵计算绝 对旋转的更新量后， 基于绝对旋转的更新量更新相机的绝对姿态后， 再次进行步骤2至步骤 4。 2.根据权利要求1所述的基于相对旋转角度的相机绝对姿态优化方法， 其特征在于， 步 骤2具体为： 基于惯性测量单 元输出的角速度积分得到惯性相对旋转矩阵， 为： 式中， 为时刻 、 间的惯性相对旋转矩阵， 是惯性测量单元的角速度， 为 惯性测量单 元数据输出的时间 间隔，f为惯性测量单 元数据的输出 频率； 基于相机当前的绝对姿态 得到相机相对旋转矩阵， 为： 式中， 为时刻 、 间的相机相对旋转矩阵， ， 、 是相机在时刻 、 的绝对旋转， i、j为时刻的序号， N为相机绝对姿态的数量。 3.根据权利要求1所述的基于相对旋转角度的相机绝对姿态优化方法， 其特征在于， 步 骤3中， 所述相对旋转角度偏差的确定过程 为： 使用轴角方式表示旋转矩阵， 得到惯性相对旋转矩阵对应的惯性相对旋转角度 以及 相机相对旋转矩阵对应的相机相对旋转角度 ； 根据惯性相对旋转角度 与相机相对旋转角度 得到相对旋转角度偏差， 为： 式中， 为相对旋转角度偏差， 表示相对旋转角度偏差 为关于相机绝对旋 转更新量 的函数。 4.根据权利要求3所述的基于相对旋转角度的相机绝对姿态优化方法， 其特征在于， 所 述惯性相对旋转角度 、 所述相机相对旋转角度 的计算过程 为：权　利　要　求　书 1/3 页 2 CN 115451958 A 2式中， 为时刻 、 间的惯性相对旋转矩阵； 式中， 为时刻 、 间的相机相对旋转矩阵。 5.根据权利要求1至4任一项所述的基于相对旋转角度的相机绝对姿态优化方法， 其特 征在于， 步骤4中， 所述判断当前的相对旋转角度偏差是否符合迭代终止条件， 具体为： 判断当前 是否达到最大迭代次数； 或 判断当前迭代步的相对旋转角度偏差与上一迭代步的相对旋转角度偏差之间的误差 是否小于精度阈值。 6.根据权利要求1至4任一项所述的基于相对旋转角度的相机绝对姿态优化方法， 其特 征在于， 步骤4中， 所述计算相对旋转角度偏差相对绝对旋转的雅可比矩阵， 具体为： 式中， 为雅可比矩阵， 为相对旋转角度偏差的变化值， ， 是绝对旋转 的变化量， 是相对旋转角度的变 化量， ， 是相对旋转轴， 根据轴角方式表示计算得到 。 7.根据权利要求6所述的基于相对旋转角度的相机绝对姿态优化方法， 其特征在于， 步 骤4中， 所述根据雅可比矩阵计算绝对旋转的更新量后， 基于绝对旋转的更新量更新相机的 绝对姿态， 具体为： 根据雅可比矩阵计算 绝对旋转的更新 量， 为： 式中， 是权重矩阵， 为相对旋转角度偏差的列向量， 每行由雅权　利　要　求　书 2/3 页 3 CN 115451958 A 3