CN117705107B - 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 - Google Patents

Abstract

本发明公开了一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法，属于视觉定位技术领域。本发明使用两阶段稀疏舒尔补将对海森矩阵进行降维处理，不仅显著降低了面向视觉惯性定位时的方程组求解的计算量，从而提升面向视觉惯性定位的定位效率，同时还利用海森矩阵的特殊稀疏性来进一步减少降维过程中产生的计算量。此外，本发明方法还通过惯性变量和路标点变量的并行求解模式，以进一步减少冗余计算，提升面向视觉惯性定位的定位效率。本发明实施例方法在保证计算精度的同时提升了同步定位的运算速度，使其更好地应用到自动驾驶，移动机器人，增强现实等应用。

Description

基于两阶段稀疏舒尔补的面向视觉惯性定位方法

技术领域

本发明属于视觉定位技术领域，具体涉及一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法。

背景技术

基于视觉惯性传感器的同步定位是自动驾驶，移动机器人，增强现实等应用中的核心模块，由于其利用了视觉传感器和惯性传感器的互补特性，使得定位结果相比纯视觉方法更加鲁棒，即使应对一些对于纯视觉传感器相对棘手的场景，也能表现出不错的效果。但是与之相对的，由于引入了惯性传感器，基于视觉惯性传感器的同步定位的理论更加复杂，算法复杂度更高，对于自动驾驶，移动机器人，增强现实这些实时性需求比较强的应用很不友好。在基于视觉惯性传感器的同步定位中，方程组求解占据了大量的时间，导致了视觉惯性同步定位处理的计算复杂度高，计算资源浪费。

发明内容

本发明提供了一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法，可用于降低视觉惯性同步定位处理的计算复杂度。

本发明采用的技术方案为：

基于两阶段稀疏舒尔补的面向视觉惯性定位方法，该方法包括下列步骤：

步骤S1，输入待定位目标的初始位姿；

步骤S2，基于置于待定位目标上或待定位目标携带的视觉传感器和惯性传感器，分别获取视觉数据和惯性数据（通常指待定位目标的加速度和角速度）；

步骤S3，确定待定位目标的当前位姿：

若当前为首次定位处理，则以输入的初始位姿作为待定位目标的当前位姿；

否则，以上一时刻更新后的位姿得到待定位目标的当前位姿；

步骤S4，基于当前位姿、当前获取的视觉数据和惯性数据构建海森矩阵及其标准向量；

步骤S5，对海森矩阵和标准向量进行两阶段稀疏舒尔补处理，得到最终的面向视觉惯性同步定位的待求解方程组；

步骤S6，对步骤S5得到的待求解方程组进行求解，得到视觉变量；再并行求解惯性变量和路标点变量；

步骤S7，基于当前求解得到的视觉变量、惯性变量和路标点变量更新待定位目标在当前时刻的位姿；

步骤S8，检测是否收到同步定位结束指令，若否，则返回步骤；若是，则输出所有时刻的位姿和路标点变量，基于所有时刻的位姿构成待定位目标的轨迹图，基于所有时刻的路标点变量构成待定位目标的稀疏的点云地图。

进一步的，步骤S4具体包括下列步骤：

步骤S401，基于视觉数据获取位姿残差变量r_v，位姿雅可比矩阵J_v，路标点残差变量r_m，路标点雅可比矩阵J_m，基于惯性数据获取惯性残差变量r_i，惯性雅可比矩阵J_i；

步骤S402，按照惯性变量、位姿变量和路标点变量的顺序确定海森矩阵结构和标准向量结构，其中，海森矩阵为方阵，其行和列方向的变量依次为：惯性变量、位姿变量和路标点变量，海森矩阵的标准向量为N维的列向量，行方向的变量依次为：惯性变量、位姿变量和路标点变量，其中，N表示海森矩阵行或列的维度。

定义惯性变量的维度为Ni，位姿变量的维度为Nv，路标点变量的维度为Nm，一般Ni与Nv与实际优化中包含的帧数有关，比如实际中每次优化帧数为Nk，每帧表示最近某一时刻的所有观测数据（包括视觉数据和惯性数据），假设每一帧的惯性变量维度为9，视觉变量维度为6，那么Ni等于9Nk，Nv等于6Nk。Nm跟实际中需要优化的路标点个数有关，假设实际中需要优化的路标点个数为M，则Nm等于M，从而最终得到海森矩阵的维度N等于9Nk +6Nk +M。

海森矩阵的矩阵块包括：惯性-惯性矩阵块H_ii、惯性-视觉矩阵块H_iv、视觉-惯性矩阵块H_vi、视觉-视觉矩阵块H_vv、视觉-路标点矩阵块H_vm、路标点-视觉矩阵块H_mv和路标点-路标点矩阵块H_mm；

标准向量的向量块包括：惯性向量块b_i，位姿向量块b_v和路标点向量块b_m；

步骤S403，按照既定的数据填充规则填充海森矩阵的各矩阵块，以及标准向量的各向量块；再将矩阵块和向量块按照位置关系填充到海森矩阵和标准向量。

进一步的，步骤S5具体包括：

步骤S501，对海森矩阵和标准向量进行分块：

其中，

矩阵块包括：惯性-惯性矩阵块H_ii、惯性-视觉矩阵块H_iv、视觉-惯性矩阵块H_vi和视觉-视觉矩阵块H_vv；

矩阵块包括视觉-路标点矩阵块H_vm；

矩阵块包括路标点-视觉矩阵块H_mv；

向量块包括惯性向量块b_i和位姿向量块b_v；

步骤S502，求矩阵块H_mm的逆矩阵；

步骤S503，对步骤S501的矩阵分块形式进行变换处理：

其中，I表示单位矩阵，表示惯性变量，/>表示路标点变量；

步骤S504，得到第二阶段的标准方程需要的新的海森矩阵和标准向量：

更新矩阵块为：/>；

并更新标准向量为：/>；

步骤S505，进行第二阶段稀疏舒尔补，得到最终的求解方程组：

基于和/>进行第二阶段的矩阵分块：

其中，表示位姿变量，/>表示惯性变量；

求矩阵的逆矩阵；

对第二阶段的矩阵分块进行变换处理：

得到最终的待求解方程组：

。

本发明提供的技术方案至少带来如下有益效果：

本发明采用两阶段稀疏舒尔补减少了基于视觉惯性传感器的同步定位中方程组求解的计算复杂度，大大提升了方程组求解的速度。同时减少了冗余计算，并且实现了惯性变量和路标点变量的并行求解，更有利于并行硬件结构的实现。另一方面，本发明在减少计算复杂度的同时保证了求解的正确性，使得基于视觉惯性传感器的同步定位可以更好地应用于自动驾驶，移动机器人，增强现实等应用。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是第一种变量排列下的海森矩阵和标准向量结构示意图；

图2是第二种变量排列下的海森矩阵和标准向量结构示意图；

图3是海森矩阵和标准向量结构示意图；

图4是海森矩阵和标准向量第一阶段分块示意图；

图5是新海森矩阵和标准向量第二阶段分块示意图；

图6是新惯性-惯性矩阵块结构示意图；

图7是基于两阶段稀疏舒尔补的面向视觉惯性定位方法的流程示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

本发明实施例提出了一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法，其使用两阶段稀疏舒尔补将对海森矩阵进行降维处理，不仅显著降低了面向视觉惯性定位时的方程组求解的计算量，从而提升面向视觉惯性定位的定位效率，同时还利用海森矩阵的特殊稀疏性来进一步减少降维过程中产生的计算量。此外，本发明实施例方法还通过惯性变量和路标点变量的并行求解模式，进一步减少冗余计算，提升面向视觉惯性定位的定位效率。本发明实施例方法在保证计算精度的同时提升了同步定位的运算速度，使其更好地应用到自动驾驶，移动机器人，增强现实等应用。

基于视觉惯性传感器（通过加速度计和陀螺仪测量设备的加速度和角速度）的同步定位的处理流程中方程组求解往往占据了大量的时间，通常求解的标准方程如下所示：

（1）

其中，H是海森矩阵，b是标准向量，x是待求解变量。通常先计算出海森矩阵H和标准向量b后，然后采用cholesky方法（又称平方根法）求解，得到x。通常标准方程的维度比较大，比如海森矩阵维度是465×465，标准向量是465×1，大维度的方程组求解的计算量会大量增加，不利于快速计算。

本发明实施例中，采用两阶段稀疏舒尔补对海森矩阵进行降维，以减少方程组求解的计算量，同时还利用海森矩阵的特殊稀疏性来进一步减少降维过程中产生的计算量，此外，通过惯性变量和路标点变量的并行求解，以进一步减少冗余计算。

对海森矩阵和标准向量的结构进行分析可知，以465×465的海森矩阵，465×1的标准向量为例，其中465表示了所有变量的维度，海森矩阵跟标准向量是统一的。整个海森矩阵是对称矩阵，总共由三种变量构成，分别是惯性变量，位姿变量和路标点变量。每一个惯性变量有9个自由度，每一个位姿变量有6个自由度，表示对应时刻的设备位置和旋转，每一个路标点变量有1个自由度。根据基于视觉惯性传感器的同步定位构建的问题，假设有11个关键帧，每一帧有一个惯性变量和一个位姿变量，则所有关键帧一共关联海森矩阵中的165维，关联标准向量的165维。假设有300个路标点，则关联海森矩阵中的300维，关联标准向量中的300维。所以海森矩阵维度是465×465，标准向量的维度是465×1。

作为一种可能的实现方式，本发明实施例中，首先基于两阶段稀疏舒尔补对视觉惯性同步定位中的海森矩阵和标准向量进行求解，其具体实现过程包括：

第一步：通过传感器获取输入数据，即通过视觉传感器获取图片，通过惯性传感器获取6维数据，其中加速度有3维，角加速度有3维。

第二步：根据获取的当前位姿，分别对视觉图片和6维惯性数据进行不同的预处理，得到位姿残差变量r_v，位姿雅可比矩阵J_v，路标点残差变量r_m，路标点雅可比矩阵J_m；惯性残差变量r_i，惯性雅可比J_i。

记整体的雅可比矩阵为J，整体的残差变量为r，则可以根据下面的公式计算出海森矩阵H中对应的矩阵块以及标准向量b中对应的向量块：

（2）

由于惯性变量与路标点变量的雅可比之间没有相关性，H矩阵中右上角和左下角部分，由惯性变量和路标点变量雅可比构成的部分都是0示。

第三步：确定海森矩阵的维度和结构。

海森矩阵和标准向量的结构由三种变量的排列关系决定，比如图1和图2所示的两种不同的变量排列，虽然海森矩阵和标准向量的维度一样，但是内部结构却全然不同。可以发现有规律的变量排列，更容易进行优化。所以本发明实施例方法使用了如下的特定排列：

按照惯性变量，位姿变量和路标点变量排列，对应的海森矩阵和标准向量结构如图3所示，进一步可以把海森矩阵进行分块，如图4所示，分成四部分，左上角的H_pp，右上角的H_pm，左下角的H_mp和右下角的H_mm。把标准向量分块，上面是b_p，下面是b_m。

其中，图3和和图4中的参数符号注释如下：

H_pp：指惯性变量和位姿变量相关的数据，包括惯性-惯性矩阵块H_ii，惯性-视觉矩阵块H_iv和视觉-惯性矩阵块H_vi，视觉-视觉矩阵块H_vv；

H_mm：路标点变量相关的数据，是路标点-路标点矩阵块，并且H_mm是对角线矩阵；

H_pm：位姿变量与路标点变量相关的数据，包括视觉-路标点矩阵块H_vm；

H_pm：路标点变量与位姿变量相关的数据，包括路标点-视觉矩阵块H_mv；

b_p：惯性变量和位姿变量相关的数据，包括惯性向量块b_i，位姿向量块b_v。

b_m：指路标点变量相关的数据，是路标点向量块。

第四步：填充海森矩阵。

通过预处理得到的位姿残差变量r_v，位姿雅可比矩阵J_v，路标点残差变量r_m，路标点雅可比矩阵J_m；惯性残差变量r_i，惯性雅可比J_i。

第五步：两阶段稀疏舒尔补求解。

501：第一阶段稀疏舒尔补：消除路标点变量带来的维度。

基于公式（1）所示的待求解的标准方程进行矩阵和向量分块，具体分块情况如图4所示，将路标点变量从所有变量x中分块出来，位姿变量和惯性变量放在一起记为/>，得到如下：

（3）

通过矩阵乘法实现矩阵的变换，方程左右同时左乘一个变换矩阵，得到如下：

（4）

（5）

此时公式（3）等效的转化为公式（5），此时可以根据公式（5）化简得到公式（6）和公式（7）并且可以根据公式（5）先求出位姿变量和惯性变量，再根据公式（7）求出路标点变量/>：

（6）

（7）

由于Hmm是对角块矩阵，所以在计算Hmm的逆的时候只需要把对角元逐个取反即可，不需要进行复杂的方程组取逆操作，以减少了方程降维时的计算量。

第一阶段稀疏舒尔补的用途主要体现在：

（1）在方程组求解中消除了路标点变量的维度，将原来465x465维度的方程组求解，转成了165x165的方程组求解和矩阵乘法，大大减少了方程组求解的计算量。

（2）因为Hmm是对角线矩阵，所以利用其特殊稀疏性，Hmm的逆矩阵很容易求出。对于路标点的变量x_m使用矩阵乘法求出，具有更高的并行度，便于硬件加速。

502：第二阶段稀疏舒尔补。

在第一阶段稀疏舒尔补之后，因为H_pm中与惯性变量相关的位置都是0，所以对于路标点的变量x_m的计算中存在大量0元素乘法，这些计算在矩阵乘法中都可以省去。另一方面，由于路标点变量与惯性变量不相关，所以是可以并行处理的。综上，一方面为了省去路标点的变量x_m计算中的冗余计算，另一方面为了提高处理的并行度。

由第一阶段稀疏舒尔补之后，得到公式（5），将其重新记为新的海森矩阵：

（8）

新的标准向量为：

（9）

进而得到公式（10）：

（10）

将新的海森矩阵和标准向量进行分块，具体分块情况如图5所示，并且将位姿变量和惯性变量分块，其中位姿变量为/>，惯性变量为/>，得到公式（11），其中得到新惯性-惯性矩阵块/>，新惯性-位姿矩阵块/>，新位姿-惯性矩阵块/>，新位姿-位姿矩阵块/>。分别计算出对应的向量块，新惯性向量块/>，新视觉向量块/>：

（11）

通过矩阵乘法实现矩阵的变换，方程左右同时左乘一个变换矩阵，得到如下：

（12）

（13）

此时公式（10）等效的转化为公式（13），然后化简得到公式（14）和公式（15）并且可以根据公式（14）先求出位姿变量，再根据公式（15）求出惯性变量/>：

（14）

（15）

由于新惯性-惯性矩阵块为对角块矩阵，具体结构如图6所示，本发明实施例中，采用针对对角块矩阵的求逆方式（利用/>的特殊稀疏性），相比于完整的矩阵求逆，节省了零元素位置的计算量，大大减少了求逆的复杂度，进一步减少了矩阵降维时的计算量。

通过第二阶段稀疏舒尔补可以发现，实际中可以先进行公式（14）的求解，再得到位姿变量之后，可以并行求解路标点变量/>和惯性变量/>。根据公式（7）的求解路标点变量/>时，因为不需要惯性变量/>，所以把相应的那部分0元素乘法计算都省去，直接使用位姿变量/>求解即可，见公式（16）：

（16）

与此同时，可以根据公式（15）求解惯性变量。这两个求解路线没有关联，完全可以并行执行。

第二阶段稀疏舒尔补的用途主要体现在：

（1）进一步减少方程组求解的维度，从第一阶段舒尔补165x165降低到66x66，进一步减少了方程组求解的复杂度，提升计算速度。

（2）实现了并行求解路标点变量和惯性变量，方便硬件实现加速。

（3）省去了计算路标点变量时的0元素乘法计算，减少了冗余计算。

（4）因为是对角块矩阵，相比普通矩阵更容易求逆，没有引入更大的计算量。

作为一种可能的实现方式，如图7所示，本发明实施例提供的基于两阶段稀疏舒尔补的面向视觉惯性定位方法具体包括下列步骤：

步骤1：通过初始化方法得到（待定位）设备的初始位姿；

步骤2：通过视觉传感器获取视觉传感器数据（图像数据），通过惯性传感器获取惯性数据；

步骤3：获取设备当前的位姿。

如果是第一次，则取出初始化得到的初始位姿；否则，则取出步骤11更新后的位姿。

然后带着图像数据和惯性数据，到步骤4进行预处理。

步骤4：对视觉数据和惯性数据分别进行预处理：

步骤4.1：单独对视觉数据进行预处理，得到位姿残差变量r_v，位姿雅可比矩阵J_v，路标点残差变量r_m，路标点雅可比矩阵J_m；

步骤4.2：单独对惯性数据进行预处理，得到惯性残差变量r_i，惯性雅可比J_i；

步骤5：构建并填充海森矩阵和标准向量：

步骤5.1：按照惯性变量，位姿变量，路标点变量的顺序，确定海森矩阵结构和标准向量结构。

步骤5.2：分别计算出对应的矩阵块，惯性-惯性矩阵块H_ii，惯性-视觉矩阵块H_iv和H_vi，视觉-视觉矩阵块H_vv，视觉-路标点矩阵块H_vm和H_mv，路标点-路标点矩阵块H_mm。分别计算出对应的向量块，惯性向量块b_i，视觉向量块b_v，路标点向量块b_m。

步骤5.3：将计算出来的矩阵块和向量块按照位置关系填充到海森矩阵和标准向量。

步骤6：进行第一阶段稀疏舒尔补，得到第二阶段的标准方程中新的海森矩阵和标准向量。

步骤6.1：根据公式（3）将矩阵和向量分块；

步骤6.2：求矩阵H_mm的逆矩阵；

步骤6.3：方程左右乘以变换矩阵，得到公式（4）、（5）所示的变换结果：

步骤6.4：得到第二阶段的标准方程需要的新的海森矩阵和标准向量： ；

步骤7：进行第二阶段稀疏舒尔补，得到最终的求解方程组；

步骤7.1：根据公式（11）将矩阵分块；

步骤7.2：求矩阵的逆矩阵；

步骤7.3：方程左右乘以变换矩阵，得到如公式（12）、（13）所示的变换结果；

步骤7.4：从而基于步骤7.3得到最终的待求解方程组，如公式（14）所示；

步骤8：利用cholesky进行方程组求解，得到视觉变量x_v；

步骤9：并行求解惯性变量x_i和路标点变量x_m，即步骤9.1和步骤9.2是可以并行执行的。

步骤9.1：利用矩阵乘法求解惯性变量x_i，得到如公式（15）所示的求解结果；

步骤9.2：利用矩阵乘法求解路标点变量x_m，得到如公式（16）所示的求解结果。注意这里与惯性变量相关的地方都直接去掉，减少0的冗余计算；

步骤10：输出所有的求解变量，即视觉变量x_v，惯性变量x_i和路标点变量x_m；

步骤11：根据步骤10输出的结果，更新当前位姿，完成同步定位。

步骤12：判断是否结束，如果是，则进行步骤13；否则进行步骤2。

步骤13：输出所有的时刻的位姿和路标点变量，所有的位姿构成轨迹图，所有的路标点变量构成稀疏的点云地图。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

以上所述的仅是本发明的一些实施方式。对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (3)

1.基于两阶段稀疏舒尔补的面向视觉惯性定位方法，其特征在于，包括下列步骤：

步骤S1，输入待定位目标的初始位姿；

步骤S2，基于置于待定位目标上或待定位目标携带的视觉传感器和惯性传感器，分别获取视觉数据和惯性数据；

步骤S3，确定待定位目标的当前位姿：

若当前为首次定位处理，则以输入的初始位姿作为待定位目标的当前位姿；否则，以上一时刻更新后的位姿得到待定位目标的当前位姿；

步骤S4，基于当前位姿、当前获取的视觉数据和惯性数据构建海森矩阵及其标准向量：

按照惯性变量、位姿变量和路标点变量的顺序确定海森矩阵结构和标准向量结构，其中，海森矩阵为方阵，其行和列方向的变量依次为：惯性变量、位姿变量和路标点变量，海森矩阵的标准向量为N维的列向量，行方向的变量依次为：惯性变量、位姿变量和路标点变量，其中，N表示海森矩阵行或列的维度；

海森矩阵的矩阵块包括：惯性-惯性矩阵块H_ii、惯性-视觉矩阵块H_iv、视觉-惯性矩阵块H_vi、视觉-视觉矩阵块H_vv、视觉-路标点矩阵块H_vm、路标点-视觉矩阵块H_mv和路标点-路标点矩阵块H_mm；

标准向量的向量块包括：惯性向量块b_i，位姿向量块b_v和路标点向量块b_m；

步骤S5，对海森矩阵和标准向量进行两阶段稀疏舒尔补处理，得到最终的面向视觉惯性同步定位的待求解方程组；

步骤S6，对步骤S5得到的待求解方程组进行求解，得到视觉变量；再并行求解惯性变量和路标点变量；

步骤S7，基于当前求解得到的视觉变量、惯性变量和路标点变量更新待定位目标在当前时刻的位姿；

步骤S8，检测是否收到同步定位结束指令，若否，则返回步骤S2；若是，则输出所有时刻的位姿和路标点变量，基于所有时刻的位姿构成待定位目标的轨迹图，基于所有时刻的路标点变量构成待定位目标的稀疏的点云地图；

步骤S5具体包括：

步骤S501，对海森矩阵和标准向量进行分块：

；

其中，

矩阵块包括：惯性-惯性矩阵块H_ii、惯性-视觉矩阵块H_iv、视觉-惯性矩阵块H_vi和视觉-视觉矩阵块H_vv；

矩阵块包括视觉-路标点矩阵块H_vm；

矩阵块包括路标点-视觉矩阵块H_mv；

向量块包括惯性向量块b_i和位姿向量块b_v；

步骤S502，求矩阵块H_mm的逆矩阵；

步骤S503，对步骤S501的矩阵分块形式进行变换处理：

；

；

其中，I表示单位矩阵，表示位姿变量和惯性变量，/>表示路标点变量；

步骤S504，得到第二阶段的标准方程需要的新的海森矩阵和标准向量：

更新矩阵块为：/>；

并更新标准向量为：/>；

步骤S505，进行第二阶段稀疏舒尔补，得到最终的求解方程组：

基于和/>进行第二阶段的矩阵分块：

；

其中，表示位姿变量，/>表示惯性变量；

求矩阵的逆矩阵；

对第二阶段的矩阵分块进行变换处理：

；

；

得到最终的待求解方程组：

。

2.如权利要求1所述的基于两阶段稀疏舒尔补的面向视觉惯性定位方法，其特征在于，步骤S4具体包括下列步骤：

步骤S401，基于视觉数据获取位姿残差变量r_v，位姿雅可比矩阵J_v，路标点残差变量r_m，路标点雅可比矩阵J_m，基于惯性数据获取惯性残差变量r_i，惯性雅可比矩阵J_i；

步骤S402，记整体的雅可比矩阵为J，整体的残差变量为r，根据公式计算出海森矩阵H中对应的矩阵块以及标准向量b中对应的向量块：

；

步骤S403，按照既定的数据填充规则填充海森矩阵的各矩阵块，以及标准向量的各向量块；再将矩阵块和向量块按照位置关系填充到海森矩阵和标准向量。

3.如权利要求1所述的基于两阶段稀疏舒尔补的面向视觉惯性定位方法，其特征在于，海森矩阵H的维度N具体设置为：n₁×N_k+n₂×N_k+M；

其中，N_k为预置的同步定位处理的目标帧数，用于表征定义惯性变量和位姿变量的维度，n₁表示每一帧的惯性变量维度，n₂表示每一帧的视觉变量维度，M表示同步定位处理的路标点的目标点数，用于表征位姿变量的维度。