VINS 基础知识¶

VINS 分类¶

VINS 可以分为 filter-based 和 optimization-based。

• 基于滤波器 (Filter-based) 的方法：将 VINS 看作一个在线的、递归的状态估计问题。它像一个流水线，新的传感器数据（测量值）源源不断地流入，滤波器利用这些新数据来更新对当前时刻状态的估计。它不回头修改过去的状态

• 基于优化 (Optimization-based) 的方法：将 VINS 看作一个批量处理的非线性最小二乘问题。它会收集一段时间内（一个“窗口”或全部历史）的所有传感器数据，然后寻找一条最优的轨迹（包含多个时刻的姿态、位置等），使得这条轨迹能够最好地“解释”所有观测到的数据。

• Point-based methods：VINS-Mono，由于对点提取的依赖性，T 可能会在充满挑战的场景中产生低临界性姿势估计
• Point and line-based methods：会为后面优化提供额外的约束
  • 同时跟踪点和线路特征，并预先整合 IMU 测量信息，包括陀螺仪和加速度计
  • 通过联合最小化与点的重投影、线的重投影以及 IMU 测量相关的三个残差误差项，来对相机的位姿进行估计

VINS 的基本框架¶

• tracking to estimate pose, mapping to reconstruct the environment, and optimization through bundle adjustment with loop closure detection

• Mapping（地图管理）

• 决定是否 插入关键帧（keyframe）与三角化新的 3D 地点（landmarks）。

• 构建观测（观测 = 某 keyframe 对某 3D 点的投影 \(z_{ij}\)），并把这些观测加入因子图 / 图结构（nodes = keyframe poses + landmarks，edges = 观测约束）。

• Local Mapping（局部优化）: 在滑动窗口/局部子图内获得高精度、一致的局部地图；保证实时性（窗口大小限制计算量）

• Bundle Adjustment（局部 BA）：对最近的若干关键帧与它们观测到的 3D 点同时做联立非线性最小二乘，目标是最小化重投影误差(联合优化位姿与 3D 点, 重投影误差最小化)

  \[ \min_{\{T_i\},\{X_j\}} \sum_{i,j} \rho\!\left(\left\| z_{ij} - \pi(T_i, X_j) \right\|_{\Sigma_{ij}}^2\right) \]
  • \(T_i\)：第 \(i\) 个相机位姿（\(SE(3)\)）
  • \(X_j\)：第 \(j\) 个 3D 点坐标
  • \(\pi(\cdot)\)：投影函数（把 3D 点投到图像平面）
  • \(z_{ij}\): 实际观测到的像素坐标
  • \(\rho\)：鲁棒核函数（如 Huber），处理外点

  优点：精度高（同时优化结构与位姿）
  缺点：计算昂贵（尤其地图点很多时）

• Remove Outliers：根据重投影误差、可观测性、被观测次数等剔除不可靠的地图点与观测。

• Loop Closure（回环检测）: 回环约束是在全局尺度上把两个远处时刻连接起来，是纠正累计漂移的关键证

• Place Recognition（BoW / NetVLAD / DBoW）发现“现在的关键帧与过去某一关键帧表示相同地点”。

• Geometric Verification：对候选回环做几何验证（匹配 + 估计相对位姿 + RANSAC）。若通过，则产生一个回环约束（一条新的边 \(z_{ij}\)）加入

• Global / Pose-Graph Optimization（后端全局一致性修正）

• 收到回环后，系统会把新增的回环边加入图，并运行全局优化（pose-graph optimization 或全局 BA），把局部误差沿整张图传播，矫正累积漂移。

• 常见策略：先做较轻量的 pose-graph 优化（只调整位姿），随后根据需要做一次全局 BA（同时调整位姿与地图点）以获得最优解。

• pose graph opimization: 只优化位姿，利用相机 i 到相机 j 的相对位姿约束

  • 残差定义为：
  \[ e_{ij} = \mathrm{Log}\!\left( z_{ij}^{-1} \left( T_i^{-1} T_j \right) \right) \]
  • 优化目标为：
  \[ \min_{\{T_i\}} \sum_{(i,j)} \rho\!\left( e_{ij}^\top \Omega_{ij} e_{ij} \right) \]
  • \(T_i\)：第 \(i\) 个相机位姿
  • \(z_{ij}\)：观测到的相对位姿约束
  • \(e_{ij}\)：该约束下的误差项
  • \(\Omega_{ij}\)：信息矩阵（测量协方差的逆）

  优点：轻量，能在大规模节点上快速运行
  缺点：只优化位姿，不直接修正地图点

Active SLAM¶

如何找到 Frontier(“已知区域”和“未知区域”之间的边界)¶

地图由体素（3D）或栅格（2D）组成，每个格子有状态：

• 已知自由 (free)

• 已知占据 (occupied)

• 未知 (unknown)

一个格子是 frontier 的条件：

• 该格子是 free（机器人能到达）。
• 至少有一个相邻格子是 unknown。

frontiers 会被聚类（clustered），形成更大的“前沿区域”。
聚类后的 frontiers 用来引导导航，因为去探索这些位置就能让机器人逐步把未知区域变为已知区域。

对每个 cluster 计算一个代表点（通常是质心或离机器人最近的点）。结合信息增益（information gain） 和运动代价（travel cost） 选取最优 frontier，作为下一步探索目标

Active Planner¶

主动规划器的目标是通过智能地选择机器人下一步的行动，来最大化其对环境的认知和理解。它需要在“探索”（Exploration）和“利用”（Exploitation）之间进行权衡，在每个决策周期，规划器都会同时评估两个选项：

• 继续执行基于 COP(Correlated Orienteering Problem)的探索路径，以获取新知识。

• 中断探索，执行一次基于 SLC(Active Semantic Loop Closure)的利用任务，以巩固和修正现有知识。

• 主动规划器通过将上述两个目标分别建模并持续评估，来制定具体的行动计划。

探索规划：基于“相关性寻路问题”（Correlated Orienteering Problem, COP）¶

当规划器的目标是“探索”时，它将路径规划问题形式化为一个相关性寻路问题 (COP) 。这个数学模型非常适合资源受限的机器人探索任务，其核心要素如下 ：

• 顶点奖励 (Rewards)： 环境中的潜在目标点（例如，未知区域的边界，即“前沿”）被视为图中的顶点，每个顶点都被赋予一个“奖励值”，代表访问该点能获得的信息增益。

• 预算约束 (Budget Constraint)： 机器人执行任务的资源是有限的，这被建模为一个“预算”，例如总路径长度、可用时间或电池电量。机器人规划的路径总成本不能超过这个预算。

• 奖励相关性 (Correlation)： 这是 COP 模型的一个关键特性。它假定访问一个顶点（位置）可能会同时提供关于其邻近顶点的信息。例如，从一个有利位置环顾四周，可能会一次性探索到好几个“前沿”区域。

通过求解这个 COP 模型，规划器能够生成一条在预算范围内，最大化总信息奖励的探索路径。这确保了探索行为本身是高效且有目的性的 。

利用规划：基于“主动语义闭环检测”（Active Semantic Loop Closure, SLC）¶

当规划器的目标是“利用”或削减不确定性时，它会启动主动语义闭环检测 (SLC) 模块 。这个过程与传统的被动闭环完全不同，其规划步骤如下：

• 候选生成与评估： 规划器会持续回顾已经构建的稀疏语义地图。它会主动识别出那些曾经观测到的、由多个鲁棒语义地标（如“椅子”、“显示器”组合）构成的区域，并将这些区域作为潜在的“闭环候选点” 。

• 成本效益分析： 对于每一个候选点，规划器会进行一次虚拟的成本效益分析。它会计算：

• 收益 (Utility)： “如果我现在规划一条路径回到这个点，并成功重新观测到这组物体，我的整体定位不确定性会降低多少？”

• 成本 (Cost)： “规划并执行这条路径需要消耗多少预算（时间、能量）？” 。

• 目标导向的路径生成： 如果规划器发现某个 SLC 候选点的“收益/成本”比非常高，它就会主动地、意图明确地规划一条专门的路径，引导机器人重返该地点 。这个行动的唯一目的就是触发一次高质量的闭环，从而校正累积的误差。

综合决策机制¶

在每个决策周期，主动规划器都会同时评估“探索”和“利用。最终的决策取决于哪个选项能在当前状态下提供最大的边际效用

例如，当机器人刚开始探索、不确定性较低时，规划器会优先选择探索。但随着探索距离变长、累积误差增大，SLC 任务的潜在收益会急剧上升，此时规划器就可能会判断执行一次闭环任务是更明智的选择。

通过这种方式，主动规划器将机器人从一个简单的地图绘制工具，提升为一个能够进行深思熟虑、主动管理自身认知不确定性的智能代理。

何时停止¶

• 无新前沿 (No New Frontiers)： 最常见的终止条件是当探索算法再也无法在地图中找到任何已知空间与未知空间的边界（即“前沿”）时，系统会判定探索已完成并停止 。

• 覆盖率阈值 (Coverage Threshold)： 规划器可以被设定一个目标，例如当已建图区域达到预定环境总体积的 95%时，任务终止 。

• 预算耗尽 (Budget Depletion)： 在“相关性寻路问题”（COP）的框架下，机器人有一个明确的“预算”（如路径长度或续航时间）。当这个预算被完全消耗时，任务自然终止 。

• 人工干预 (Manual Intervention)： 在实际部署中，操作员可以随时通过远程指令终止任务，例如当他们对地图的完整性和精度感到满意时

效用函数的作用¶

探索的效用 (Utility of Exploration)：¶

当规划器评估“探索”这一选项时，它通过相关性寻路问题 (COP) 模型来计算效用。在这个模型中，环境中的每一个潜在探索目标点（即前沿）都被赋予一个reward 。这个奖励值就是探索该点的直接“效用”，代表了访问该点预期能获得的新信息量（Information Gain）。

规划器会求解这个 COP 问题，寻找一条能在预算约束内总效用的路径。因此，效用函数直接引导规划器选择信息量最丰富的探索方向，避免低效的徘徊。

利用的效用 (Utility of Exploitation / Uncertainty Reduction)：¶

当规划器评估“利用”（即主动闭环）这一选项时，它会启动主动语义闭环 (Active SLC) 模块。该模块会识别出历史地图中潜在的闭环候选点。对于每一个候选点，效用函数会进行一次成本效益分析，其核心是计算执行这次闭环的“效用”——即预期能够带来的不确定性削减量。这个效用值越高，意味着这次闭环对提升定位和地图精度的价值越大。

如规划器会比较不同闭环候选点的效用值。如果发现某个闭环任务的效用（即不确定性削减量）非常高，足以超过继续探索带来的信息增益效用，规划器就会中断当前的探索任务，转而生成一条专门用于执行这次高价值闭环任务的路径 。

后端优化¶

• 里程计约束 (Odometry Factors): 连接连续两个机器人位姿节点（例如，\(Pose_t\) 和 \(Pose_{t+1}\)）。这种约束来源于前端的运动估计（如视觉-惯性里程计），它描述了机器人从一个时刻到下一个时刻的相对运动 。

• 物体-机器人观测约束 (Object-Robot Observation Factors): 这是系统的核心约束。当机器人在某个位姿（\(Pose_t\)）观测到一个语义地标（\(Object_j\)）时，就会在\(Pose_t\)节点和\(Object_j\)节点之间添加一个因子。这个因子编码了机器人与物体之间的相对位姿关系 。论文中明确提到，系统通过定制化的因子 customized factors来编码这种物体-机器人约束，从而最小化定位漂移 。

• 语义闭环约束 (Semantic Loop Closure Factors): 这是一种特殊但极其强大的观测约束。当机器人通过其主动规划，重新观测到一个很久以前就已经加入地图的语义地标时（例如，在\(Pose_t\)时刻重新看到了在\(Pose_{t-k}\)时刻首次发现的\(Object_j\)），系统就会在\(Pose_t\)和\(Object_j\)之间添加一个新的观测约束。由于\(Object_j\)已经与\(Pose_{t-k}\)等历史位姿相关联，这个新的约束就有效地在当前位姿和遥远的历史位姿之间建立了一座“桥梁”，从而在全局优化时校正整个轨迹的累积误差

Support Me!