On the Comparison of Gauge Freedom Handling in Optimization-based Visual-Inertial State Estimation

在融合视觉与惯性传感器数据以优化状态估计的系统中，在Gauge Freedom处理方法方面展开分析与比较。

摘要

众所周知，在视觉-惯性融合系统中实现四自由度（four degrees of freedom, DoF）变换（包括围绕重力轴的旋转和平移），并正确处理额外引入的一组规范自由度（normative gauge）是必要的。尽管已有多种不同的规范自由度处理方法被实际应用过，在理论上对其差异性进行系统分析仍处于空白状态。本文首次开展基于优化框架下视觉-惯性状态估计系统的规范自由度处理效果比较研究。具体而言，在三种典型方法之间进行了实验对比：1.将不可观状态（the unobservable states）固定为某些预设值；2.对这些不可观状态施加先验信息；3.允许其在优化过程中自然演进。研究表明：第一种方案与第二种方案在估计精度和计算时间上表现相似；第三种方案虽然计算效率略低但其协方差矩阵估计呈现出显著差异；同时第三种方案与前两种方案之间并无实质区别。此外，在仿真实验以及实际数据集测试中均验证了本研究结论的有效性。

Ⅰ. 介绍

视觉-惯性传感器融合技术 是机器人领域的重要研究方向之一。相机与惯性传感器具有互补性能[1] ，两者的协同工作能够提供更加可靠的状态估计结果。尽管基于滤波器的研究方法[2] 、[3] 、[4] 在视觉-惯性融合领域占据主导地位，但近年来非线性优化方法逐渐展现出更大的应用潜力。相较于传统滤波器方法，在非线性优化框架下系统的线性化误差累积问题得到了显著缓解[5] 、[6] 。值得注意的是，在这一领域取得进展的同时也需要注意到其计算成本较高的问题已有有效解决方案出现。基于非线性优化的方法近期研究工作[5] 、[7] 、[8] 、[9] 在复杂环境下实现了令人瞩目的实时视觉-惯性状态估计效果

尽管这些工作基于相同的理论基础——将状态估计问题转化为非线性最小二乘优化问题来求解——但它们采用了不同的策略来处理VI系统中不可观测的状态自由度（DoF）。众所周知，在VI系统中全局位置（global position）以及偏航角（yaw）是无法直接测量的状态变量[3]、[10]。在本研究中我们将其归类为"规范自由度"（normative degrees of freedom, DoF）按照BA领域内的常用术语[11]。如果VI系统包含这些规范自由度，则可以通过在优化过程中固定相应状态变量（即参数化处理）来获得唯一的解这一直观方法实现求解另一种可行的方法是在不可观测的状态变量上引入先验信息作为约束条件该先验信息在优化过程中本质上相当于引入了虚拟测量结果[5]、[8]、[13]、[7]。第三种方法允许优化算法在迭代过程中动态调整这些不可观测的状态变量值最终这三种方法都已在相关文献中证明其有效性但目前尚未就它们在VI状态估计中的异同进行系统性比较这种比较通常被作为一种实施细节而未得到充分的研究与理解此外与BA问题领域类似已有研究表明单目视线条件下存在7个规范自由度（如文献[11]、[14]）但在当前文献中尚无类似研究针对VI系统的规范自由度情况目前仅发现存在4个规范自由度

在本研究工作中,我们对基于优化的视觉-惯性状态估计系统中不同规范自由度处理方法进行了首次全面比较分析.该研究涵盖了以下三种方法:(1) 规范自由度固定法;(2) 规范自由度先验法;(3) 自由规范自由度法.从估计精度、计算复杂度以及协方差特性(该指标为SLAM领域的关键指标[16])等方面进行评估.结果表明,尽管所有这些方法在估计误差方面表现相似,但自适应收敛速度更快.进一步研究表明,与固定规范自由度的方法不同,通过协方差变换得到的自适应规范自由度解其协方差特性与特定参考系无关(见图1).

其他各部分内容按序展开。在第二部分系统性地阐述了基于优化理论的状态估计方法VI及其存在的非唯一解问题。第三部分则详细探讨了如何处理规范自由度带来的多解性问题。接着，在第四部分对所设计仿真的实验环境进行了全面描述。针对精度与时间以及协方差等指标的具体表现，在第五和第六部分分别进行了详细分析。最后部分则展示了基于真实世界数据集的实验结果。

Ⅱ. 问题公式化和不确定性

视觉-惯性状态估计问题涉及对相机-惯性（IMU）传感器组合运动及其在场景中移动时被相机观测到的3D地标位置的估计。通过采集视觉特征与加速度计与陀螺仪测得的数据建立方程组。这些方程可被组织成为非线性最小二乘优化模型以最小化基于高斯误差模型的目标函数：

J(\theta)=\| r^V(\theta)\|^2_{\Sigma_V}+\| r^I(\theta)\|^2_{\Sigma_I} \tag{1}

方程（1）中的视觉误差项是基于测量图像点x_{ij}与其通过米制重建得到的预测点\hat x_{ij}之间的重投影误差进行定义的。具体而言，在本研究中我们假设采用针孔相机模型来进行外参数估计，在这种情况下预测点\hat x_{ij}(\theta)与相机外参(R_i,p_i)之间满足关系式：\hat x_{ij}(\theta)\propto K_i(R^T_i|-R^T_ip_i)(X^T_j,1)^T。值得注意的是，在本研究中内参矩阵K_i被认为是精确已知且不含噪声干扰的。此外，在方程（1）中所定义的惯性误差项则反映了基于惯性测量系统（IMU）所得轨迹与其预期轨迹之间的偏差。例如，在文献[17]中考虑了原始加速度计和陀螺仪测量数据中的噪声影响，在文献[5]中则提出了基于低速率测量数据的一种改进方法（即以视觉数据率为基准的速度积分预处理）。然而，在本研究工作中我们主要采用后者作为研究基础。

问题的参数（也称为状态）：

\theta =\{ p_i,R_i, v_i, X_i\} \tag{2}

包括相机运动参数（外参和线速度）和3D场景（路标点）。

加速度计和陀螺仪的偏差通常在IMU框架内表示，并不受坐标系固定性影响。因此，在本研究中我们排除了状态误差的影响，并假设IMU测得的数据已进行了校正处理。由于本研究主要关注于惯性和视觉测量模型的基础构建过程及其相互关联关系，并未对这些模型进行详细阐述与推导证明，在此我们仅作简要说明，并建议读者参考文献[5]以获取更为深入的技术细节

A. 解的歧义性和几何等价

在解决VI状态估计问题时，需要考虑目标函数（1）对于参数空间中的一些变换g保持不变。其数学表达式为：

J(\theta) = J(g(\theta))

具体来说，在方程(4)中被定义为以下形式的齐次变换矩阵：

H = \begin{pmatrix} R_{zy} & t \\ 0 & 1 \end{pmatrix},

这是一个具有4个自由度的变换操作，在三维空间中包括任意平移运动以及绕重力轴（以 yaw 角度表示）的所有旋转运动。为了便于表示这一操作关系，在后续推导中将采用以下符号表示方法：对于任意向量 θ∈ℝ³ ，其对应的旋转变换矩阵记为 Exp(θ) = exp(θ^∧)，其中 exp 表示特殊正交群 SO(3) 上的操作符

将变换（4）应用于重构过程(编号为(1))其结果将是新的构建g(\theta)=\theta ^{\prime }=\{\ p^{\prime }_{1},R^{\prime }_{1},v^{\prime }_{1},X^{\prime }_{1}\}

\begin{matrix} p^{\prime }_{1}=R_{z}p_{1}+t & R^{\prime }_{1}=R_{z}R_{1} \\ v^{\prime }_{1}=R_{z}v_{1} & X^{\prime }_{j}=R_{z}X_{j}+t \end{matrix} \tag{5} 这两个参数θ与θ′代表同一场景的几何结构$...$ （同一个潜在的场景几何$...$ ），即它们在同一个潜在的场景几何中运行着不同的相机轨迹$...$ 3D点）。这意味着无论从哪个角度观察都会得到一致的结果；因此产生的误差相同。（1） 由（3）所体现的不变性下，我们可以将参数空间$\mathcal{M}$划分为若干个互不相交且具有几何等价地被重新构造的部分。每个这样的部分都被称作轨道或叶子，并且在文献中常被引用为参考依据[11, 14]。具体而言，在与给定参数$\theta$相关联的情况下，

\mathcal{M}_\theta = { g(\theta) \mid g \in \mathcal{G} } \tag{6}

构成了一个4-dimensional manifold。 其中, $\mathcal{G}$是由公式（4）所描述的变换构成的一个群.值得注意的是, 目标函数（1）在每一条轨道上都是一个常量. 公式（3）的**不变特性** 直接导致了**目标函数** （1）无法被唯一确定其最小化值。也就是说，在满足相同最小误差的前提下，存在无限多种不同的重建方法。具有最低成本的轨道（6）上的所有重建方案（如图2所示），仅限于4-DoF变换（4）。因此，VI估计问题面临一定的**不确定性** 或不可观状态：这些系统方程不足以唯一确定系统的全部状态变量。 ##### B. 附加的约束：指定一个规范 通过在目标函数（1）中加入补充约束，并结合公式（7），从而得出唯一解的过程等同于定义一个规范$\mathcal{C}$[14], [11]。 另一种说法是说，在VI系统中可以通过指定参考坐标系的方式来进行3D重建操作。例如，在相机运动估计中所采用的标准规范包括了此类重建方法：即在第一台摄像机位置处设置零偏航角的参考坐标系。（the reconstruction that has the reference coordinate frame located at the first camera position with zero yaw）。这些约束条件确定了一个独特的变换(4)，从而使得方程组$\theta_C = \mathcal{C} \cap \mathcal{M}_\theta$存在唯一解。通过引入这样的构造方法，则规范$\mathcal{C}$与轨道$\mathcal{M}_\theta$保持横跨关系 #### Ⅲ. 优化和规范处理 在优化领域内，在应用Gauss–Newton算法来最小化非线性平方误差函数时会遇到一定的挑战性问题。即便当我们选择最低维度的状态空间模型来描述系统的状态时，在不可观测自由度的情况下(如方程(4)所示)，该函数仍然无法得到有效的估计结果。进一步说明的是，在这种情况下海森矩阵的存在缺陷使得其秩小于4对应的自由度数量。 解决这一问题的方法多种多样。如表Ⅰ所示，在较小维度的空间内进行优化能够有效缓解该问题，并由此可知对应的Hessian矩阵具有可逆性特征。这种方法本质上是对解施加了严格限制（即规范自由度被严格固定），从而确保了解的有效性。另一个策略则是通过引入额外惩罚项来增大目标函数值，在这种方式下能够以较为柔和的方式（即基于先验知识对规范自由度加以限制）支持解满足特定约束条件。最后一种情况则利用奇异Hessian矩阵及其伪逆特性来进行隐式约束（即采用最小范数参数更新以获得唯一解）。值得注意的是前两种策略均需依赖于特定于变分推断的知识体系（即在已知约束条件下求解），而第三种方案则是一种更为通用的方法框架 ##### A. Gauge fixation or prior的旋转参数 基于gauge-fixation与gauge-prior的方法而言，在相机姿态的1自由度（1-DoF）仰角的固定问题上存在显著挑战性，在后续部分中我们将深入探讨这一技术细节。 在解决优化问题的过程中，在每一次迭代期间的主要方法是基于局部坐标系进行调整以实现模型参数的有效优化。其中，在每一次迭代过程中具体而言，在第q次迭代期间其更新过程如下所示：

R^{q+1}=Exp(\delta \phi^q)Rq \tag{8}

其z分量设为零，则该向量即使其绕着y轴保持稳定；然而，在进行多个Q迭代时， 结合上述等式可以看出， 其结果并未使相对于起始旋转R⁰固定的方位角， 从而得出结论说， 这种参数化方法无法将RQ的方位角固定在初始值R⁰对应的方位角上 在处理任意帧的相机姿势时，通常在处理任意帧的相机姿势时，在处理不同时间点之间的姿态变化时，在处理不同时间点之间的姿态变化时，在处理不同时间点之间的姿态变化时，在处理不同时间点之间的姿态变化时，在处理不同时间点之间的姿态变化时， 其中一种常用的方法是基于**第一帧相机**（i=0）。在此基础上， 为了实现对后续帧的姿态估计， 我们采用标准的迭代更新方法（8）； 而对于初始状态下的姿态估计， 由于其具有特殊的性质， 我们可以采用更加灵活简便的方式来进行参数化。 为了简化表示和计算过程， 在参数化过程中我们采用的是基于左乘法增量（left-multiplicative）的方法。 在这种情况下， 初始状态下的旋转矩阵可以表示为：

R_0 = \text{Exp}(\Delta \phi_0) R^{\circ 0}

其中旋转矢量Δφ₀被设定为初始值，并在其过程中不断更新。然而，在Δφ₀的模长等于π时该公式存在奇点；不过，在实际应用中通常会遇到这种情况：Δφ₀的模长接近最佳值且小于π；由于前端能够提供较为理想的初始值，在这种情况下该方法是有效的 ##### B. 处理Gauge Freedom的不同方法 基于之前的讨论，涉及在整个优化过程中固定 camera 的 primary pose 的位置和 yaw 角度。这是通过以下方式来实现：$$p_0=(p^0_0), \quad (\Delta \phi_{0z}=e_z^T\Delta \phi_0 = 0) \tag{10}

其中

gauge prior策略引入了惩罚项到目标函数表达式（1），其中引入了惩罚项的形式为\|r^{p}_{0}\|^{2}_{\Sigma^{p}_{0}}；其中r^{p}_{0}(\theta)被定义为(p_{0}-p^{0}_{0}, \Delta\phi_{0z})

关于\Sigma^p_0的选择将会在第五部分讨论。

最后，在free gauge方法中使参数向量在其优化过程中自由演变。为了解决海森矩阵奇异的问题，我们采用了伪逆法或引入阻尼因子（如Levenberg-Marquardt算法），从而确保NLLS问题具有明确定义的参数更新规则。
随后，在图2中我们展示了三种不同方法在优化过程中的参数轨迹进行对比分析。
接下来我们将分别介绍这三种规格化处理方法的具体实验结果并进行比较分析。