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

2056 武 汉 大 学 学 报 （信 息 科 学 版） 2025 年 10 月
n 对于任意一对匹配的线特征，基于空间三维
χ = arg min ( ρ p∑ e p Σ p e p +
*
T
线段重投影至图像平面后所得二维线段的两端
p = 1
m

ρ l∑ e l Σ l e l + λT - T IMU ） （11） 点，以图像中实际提取的对应二维线特征之间的
T
l = 1 距离作为约束条件，建立线特征的视觉测量约束
*
式中， χ 为全部待优化变量 X； λ 表示松耦合正则 方程。如图 10 所示，已知线特征端点为 PQ，线特
化比例因子； ρ p 和 ρ l 分别表示点线优化因子对应 征重投影到二维图像上为 pq，由先前图像检测获
的权函数； e p 和 e l 分别表示点线误差向量（具体见 得的线特征端点二维坐标可以计算线特征直角
§1.4.2 内容）。此外，本文采用 Huber 核函数， Σ * 坐标系下的二维表达式 ax+by+c=0，以此建立
表示点线特征测量值对应的信息矩阵， T、T IMU 分 线特征重投影误差方程：
别表示待优化的 4×4 位姿转移矩阵和 IMU 积分 ì | au s + bv s + | c
ï ï
后的旋转量 R 和平移量 t 组建的 4×4 转移矩阵。 ï d s = a + b 2
ï
ï ï
2
1.4.2 点线视觉特征误差方程 e l = í （14）
ï ï | au e + bv e + | c
用二进制描述子完成点特征的匹配，通过场 ï ï d e =
ï ï a + b 2
2
î
景中的三维点特征的重投影与图像对应特征点
式中 ， u *、 v * 分别表示线特征两个端点二维坐标
欧氏距离误差建立点特征的视觉测量误差模型：
分量。
1
-1
e p = u - K [ R cb ( R bw P + t bw )+ t cb] （12） 同理，对于位姿求解关于 R bw 和 t bw 的 6 自由
z
式中，u 是图像上特征点二维坐标；K 为相机的参 度变量 ξ 求导建立误差方程：
数矩阵；P 为世界坐标系下的三维特征点坐标； ì ∂u s ∂p cs ∂p bs + b ∂v s ∂p cs ∂p bs
ï ï a
ï
ï
ï ï
R bw 和 t bw 表示从世界坐标系转换到惯导本体坐 ∂e l = í ∂p cs ∂p bs ∂ξ ∂p cs ∂p bs ∂ξ （15）
标 系 下 旋 转 量 和 平 移 量 ； R cb 和 t cb 分 别 表 示 从 ∂ξ ï ï ∂u e ∂p ce ∂p be + b ∂v e ∂p ce ∂p be
ï a
ï
î
IMU 本体坐标系转换到像平面坐标系下旋转量 ï ï ∂p ce ∂p be ∂ξ ∂p ce ∂p be ∂ξ
和平移量。分别对位姿（3 自由度旋转量、3 自由 式中， p c∗ 和 p b∗ 分别是线段端点在相机坐标系和
度平移量）求解，则误差函数 e p 对 R bw 和 t bw 中的 6 IMU 本体坐标系下三维坐标向量； u *、 v * 分别表
自由度位姿变量 ξ 求导为： 示线特征两个端点二维坐标向量； ∂u ∗ 、 ∂v ∗ 参考
∂p c∗ ∂p c∗
∂e p ∂e p ∂P c ∂P b
= （13）
∂ξ ∂P c ∂P b ∂ξ 文献［36］中推导过程。
1.4.3 视觉惯导后端紧耦合优化模型
式中， P c 和 P b 分别是场景点特征在相机坐标系和
在本文构建的视觉与 IMU 融合的 SLAM 方
∂e p ∂P b
本体坐标系下三维坐标向量； 、 可以参考
∂P c ∂ξ 法中，为进一步提高 SLAM 定位精度和前端位姿
∂P c 跟踪的鲁棒性，系统基于点线特征和 IMU 预积分
文献［36］具体推导过程； = R cb。
∂P b 量，分别与前端的位姿跟踪和后端的局部地图做
本文建立的线特征误差模型见图 10，图 10 中
耦合优化。前端位姿跟踪模块负责估计并优化
PQ 表示世界坐标系下的三维线段， p * 和 q * 是相
系统的整体位姿，但不更新世界坐标系中点、线
对应的图像上检测的线特征端点，p 和q 是 PQ 投
*
*
特征的三维坐标。同时，系统根据当前图像帧的
影到图像帧上对应的线特征端点， O 1、 O 2、 O 3 表示
位姿信息判断其是否作为关键帧。一旦确定当
相机摄影中心。
前 帧 为 关 键 帧 ，算 法 进 入 后 端 局 部 地 图 执 行 模
块，本文 SLAM 方法中后端没有基于滑动窗口优
化的算法，而是借鉴 ORB-SLAM3 框架构建点线
特征共视图，并基于关键帧来实现后端优化。本
*
文方法的待优化变量 χ 除了对包括位姿的 6 个
变量求导外，还有点线特征三维坐标，以及上述
相应的 IMU 待优化变量 X，运用 IMU 预积分结
图 10 线特征重投影误差方程示意图
果 和 点 线 特 征 测 量 信 息 构 建 统 一 的 优 化 代 价
Fig. 10 Diagram of Re-projection Error Equation of
Line Feature 函数：