Page 120 - 《武汉大学学报（信息科学版）》2025年第10期

第 50 卷第 10 期 张晨阳等：一种自适应点线特征和 IMU 耦合的视觉 SLAM 方法 2055

w
w
w
式中， p bi、 v bi、 q bi 依次表示惯性测量单元在 i 时刻
世界坐标系中的位置、速度、旋转量； ⊗表示四元
数之间的乘法操作； ω bi 为陀螺仪在 i 时刻的角速
度变量。对上述变量在 i、j 时刻 Δ t 时间段内的预
积分观测量表达形式对时间小量 δ t 的积分得：
w w w w w 2
p bj = p bi + v i Δt + ∬ R bt ( a bt - g ) δt （5）
∫
w
w
v bj = v bi + R bt ( a bt - g ) δt （6）
w
w
ê
é 0 ù ú ú
ê
∫
w w ê ú ú δt
q bj = q bt ⊗ ê 1 ú ú （7）
ê ê
ë 2 ω bt û
w
w
w
式中， p b*、 v b*、 q b* 分别是不同时刻对应的位移、速
w
度和旋转； R b* 表示相对世界坐标系下对应时刻间
的 IMU 本体坐标系旋转量。
不难看出，在求解过程中每一次迭代都要进
行相应的积分操作，计算量相对较大，因此为避
免 大 量 的 重 复 积 分 运 算 ，对 上 述 公 式 做 如 下
变形：
∬ R bibt ( a bt - g ) δt 2 （8）
w
α bibj =
∫
w
图 8 由粗到细的匹配策略的线特征匹配结果对比 β bibj = R bibt ( a bt - g ) δt （9）
Fig. 8 Comparison of Matching Result of Line Feature
ê é 0 ù ú ú
ê
∫
Matching Based on Matching Strategy from Coarse to Fine ê ú ú δt （10）
q bibj = q bibt ⊗ ê 1 ú ú
ê ê
ë 2 ω bt û
相机图像的输出频率较低，为了实现两类传感器 式 中 ， α bibj、 β bibj 和 q bibj 便 是 相 邻 图 像 帧 间 的 IMU
数据时间戳能够对齐，本文首先要对 IMU 测量值 预积分观测量。
进 行 预 积 分 处 理 ，IMU 的 全 部 待 优 化 变 量 定 前端视觉图像匹配以及前端位姿估计精度
义为： 的稳健性是 IMU 获取准确的初始化结果的关键。
X =[ p q v b g b a ] （2） 实际运动中视觉图像质量会影响点线特征的提
式中，q、p、v 分别表示刚体旋转的四元数、刚体位 取与匹配，最终影响前端位姿优化收敛的速度和
移和速度变量； b g 和 b a 分别表示三轴陀螺仪和三 精度。本文结合预积分观测量，首先恢复 IMU 本
轴加速度计的偏置。q 为： 体坐标在世界坐标系下的旋转量 R 和平移向量 t，
q w ] 然后联合点线投影误差建立视觉和 IMU 的松耦
q =[ q x q y q z （3）
式中， q x、 q y、 q z 是旋转四元数对应的虚部； q w 为四 合相机位姿估计模型，优化并获取稳健的位姿结
果 ，从 而 完 成 IMU 的 初 始 化 ，整 个 初 始 化 流 程
元数的实部。
见图 9。
对 X 中的变量依次求导可得：
在系统前端初始化过程中，本文运用 IMU 预
é 0 ù ú ú
ê
ê
w w w w w w ê ú 积分量和点线视觉特征构建统一的松耦合位姿
p ̇ bi = v bi,v ̇ bi = a bi,q ̇ bi = q bi ⊗ ê 1 ú （4）
ê ê ω bi ú ú
ë 2 û 优化代价函数：

图 9 基于点线特征测量误差和 IMU 松耦合的初始化步骤
Fig. 9 Initialization Step Based on Measurement Error of Point‑Line Feature and IMU Loose Coupling