CN113450411B - 一种基于方差分量估计理论的实时自身位姿计算方法 - Google Patents

Abstract

该发明公开了一种基于方差分量估计理论的实时自身位姿计算方法，属于同时定位与地图构建技术领域。该方法首先通过利用视觉测量信息完成系统的初始化，然后根据测量器件成像特点计算图像中特征点的三维空间坐标，接下来通过聚类处理，利用方差分量估计计算各测量信息权重分布，最后将计算结果带入系统整体的非线性优化方程。该发明在使用过程中，不仅仅适用于视觉导航算法，对视觉/惯性融合导航算法同样具有优化作用。通过使用本方法优化系统中的随机误差，提高了系统的准确度和可靠性。

Description

一种基于方差分量估计理论的实时自身位姿计算方法

技术领域

本发明属于同时定位与地图构建(Simultaneous Localization and Mapping,SLAM)技术领域，具体是一种利用方差-协方差分量估计进行随机模型优化提升视觉导航系统中位姿计算准确度的方法。

背景技术

近年来，随着扫地机器人、无人驾驶、无人机、VR等产业的蓬勃发展，应用同时定位于地图构建(SLAM)方法的智能导航定位设备广泛服务于日常生活。现有的视觉导航方法主要有以下流程：1、视觉传感器信息读取，获取相机实时图像信息；2、前端视觉里程计，利用获取的图像信息计算相邻图像间相机的运动；3、后端非线性优化，利用不同时刻视觉里程计测量信息及回环检测信息构建非线性优化系统。4、回环检测，利用图像信息检测是否回到先前位置；5、建图，根据计算轨迹信息建立环境地图。然而，在上述流程中视觉里程计计算及利用重投影误差构建非线性方程进行优化位姿时，并未精确考虑视觉测量器件随机误差的影响，其计算过程通常仅利用经验信息剔除阈值范围外的特征点，其非线性优化过程中也仅根据经验调整方差-协方差的传播，难以保证位姿信息的精密求解。

发明内容

针对上述问题，本发明提出一种基于方差分量估计理论的视觉导航方法。该方法针对视觉传感器测量中随机误差的分布特征，利用方差分量估计理论进一步精化系统位姿计算过程中所采用的随机模型，从而提高位姿估计的精度和可靠性。

本发明的技术方案为：一种基于方差分量估计理论的实时自身位姿计算方法，具体包括以下步骤：

步骤1：使用视觉传感器实时获取图像测量信息，完成系统初始化；其中双目立体相机按照特征点提取、左右相机图像特征点匹配、单位原点位置信息构建，实现系统初始化；

步骤2：完成初始化后，利用测量信息计算三维空间点坐标；

步骤3：根据视觉测量中随机误差分布特点对三维空间点坐标进行聚类；

在测量时，由于使用过程中相机焦距、基线均为固定参数，造成随机误差影响的主要可变因素为被测物体与相机之间的距离大小，因此将视觉测量信息中计算得到的三维空间点按照与被测物体距离远近做聚类处理；

步骤4：构建随机误差模型V；设包括两类观测数据，第一类观测数据为B₁，第二类观测数据为B₂，

其中，v₁、v₂表示第一类、第二类观测数据对应的残差，f(x)为三维空间坐标按PNP方式求解对应二维坐标的线性化函数，L为上一帧中各二维特征点经匹配方式获取其在当前帧的二维坐标向量，L₁、L₂为两类观测数据对应的测量值，B为两类观测B₁、B₂的集合，R为旋转信息、t为平移信息，I为单位矩阵；

将公式(4)按照两类观测数据分别展开，可表示为：

对公式(5)构建法方程求解：

对应优化状态向量x的解

为：

其中，

其中，N₁、N₂表示第一类、第二类观测值与权重信息构建的计算矩阵，W₁、W₂表示第一类、第二类观测值与测量值构建的计算矩阵；式(7)求解过程中，由于三维空间点与对应二维平面特征点为非线性变换，利用重投影误差优化模型进行线性化操作，此时B、L应为其三维空间点线性化展开对应形式；在公式(8)中P为各类测量对应的权重矩阵；第一次平差时给定的两类观测值的权P₁和P₂是不恰当的，或者说两类观测值所对应的单位权方差并不相等，令其系数分别为

当观测数据优化过程中方差-协方差矩阵记为D(L),两类数据各自的方差-协方差记为D(L₁)、D(L₂)时,其与权重矩阵P关系可由式11表示

对公式(11)中

利用方差分量估计进行修正，其严密计算公式为：

式(12)中n₁、n₂分别表示两类观测矩阵的维数；对方差分量进行简化操作，得到：

即各类观测值的单位权方差的估值为：

步骤5：对步骤3聚类后的每一个聚类簇分配一个步骤4建立的随机误差模型，然后对每个聚类簇的随机误差模型进行优化；

步骤5-1：对聚类后每一聚类簇数据计算平差信息:

步骤5-2：对两类测量信息计算权方差

如公式16所示：

然后，利用权方差获得对应的权重，其中c为常数；

步骤5-3：随机误差模型的迭代优化；在进行优化过程中，为使随机误差模型达到最优效果，对步骤5-1和步骤5-2进行反复迭代求解，直至公式16中的

收敛，获取权重系数最优解；在实验具体计算过程中，为保证系统计算实时性及优化效果收敛性，设置条件为：

步骤5-4：采用优化后的随机误差模型计算得到每一个聚类簇的方差-协方差D(L₁)、D(L₂)；

步骤6：采用步骤5得到的所有聚类簇的方差-协方差D(L₁)、D(L₂)计算位姿信息。

进一步的，所述步骤2的具体方法为：

步骤2.1：在利用视觉传感器进行位姿计算的过程中，对双目测量信息直接利用相机的基线信息计算空间坐标，计算方法如下：

其中，f为相机焦距，b为相机基线大小，u_L、u_R为空间点P在左右相平面成像坐标为，u 为理论视差；

步骤2.2：在对三维空间点进行计算的过程中，目标点深度测量误差计算为：

ε为匹配误差，x为当前帧的姿态信息；

对公式(2)进行优化得到深度测量误差应表示为：

步骤2.3：采用步骤2.2的误差对步骤1的空间坐标进行校正。

进一步的，其特征在于，所述步骤6的具体方法为：

步骤6-1：利用图像测量信息进行系统后端的优化过程中，根据使用传感器类型分为两类：

(1)视觉非线性优化系统，优化随机误差模型中的R，t；

优化函数为：

其中，ρ为鲁棒核函数、π表示将三维空间信息转换为2d图像位置计算过程、x为特征点二维空间坐标、X为特征点三维空间坐标；M为关键帧的数据、i为各关键帧图像上的特征点；

(2)视觉/惯性非线性优化系统，优化随机误差模型中的x；

该系统优化部分包括滑动窗口中先验残差r_p-H_px²、滑动窗口中IMU测量构建残差为

图像帧间的视觉测量残差

其中，状态向量包括位置、姿态、速度、加速度、陀螺仪，J_p表示状态向量的雅可比矩阵、

表示前后两帧间的IMU预积分误差、Q为滑动窗口中关键帧的数目、

表示相对于惯性坐标系、

表示前后两帧中的特征点残差、F表示当前滑动窗口内特征点的集合、

表示相对于视觉测量坐标系，将所有误差构建整体优化公式为：

步骤6-2：构建非线性优化系统中的随机模型优化；在步骤6-1中，对于后端非线性优化系统主要由公式(18)、(19)构成，其中均含有视觉测量优化部分；在计算过程中，利用高斯牛顿法时有以下优化方程：

f(x+Δx)≈f(x)+J(x)Δx (20)

J(x)表示优化信息的雅可比矩阵，对上式位姿参数Δx进行最优化求解则有：

式21展开后对变量Δx求导，可得：

利用步骤5得到的各类权重信息优化位姿估计结果：

其中，D为各类观测的方差-协方差矩阵，由步骤4中的所有聚类簇对应的D(L₁)、D(L₂) 构成。

本发明公开了一种利用方差分量估计实现视觉导航算法中随机误差模型优化的方法。该方法首先利用视觉传感器、视觉/惯性传感器获取图像中特征点的三维空间坐标信息，然后对这些特征点按照随机误差分布特征的不同做聚类处理，最后基于方差分量估计理论精化系统随机误差模型从而获取高精度载体位姿信息。大量实验证明，在使用本发明估计载体位姿及构建环境地图时，可显著提高系统位姿精度。

附图说明

图1是具有方差分量估计的随机误差模型优化算法流程图。

图2是随机误差模型优化方法的流程图。

图3是方差分量估计的随机模型优化方法在ORB_SLAM2上的系统流程图。

图4是方差分量估计的随机模型优化方法在ORB_SLAM3上的系统流程图。

图5是利用ORB_SLAM2系统与本发明位姿计算准确性的对比图。

图6是利用ORB_SLAM3系统与本发明位姿计算准确性的对比图。

具体实施方式

下面对本发明中的基于方差分量估计的视觉导航算法进行详细说明：

步骤1：使用视觉传感器实时获取图像测量信息，完成系统初始化；其中双目立体相机按照特征点提取、左右相机图像特征点匹配、单位原点位置信息构建，实现系统初始化。

步骤2：完成初始化后，利用测量信息计算三维空间点坐标。

步骤2.1：在利用视觉传感器进行位姿计算的过程中，对双目测量信息直接利用相机的基线信息计算空间坐标，计算方法如下：

其中，f为相机焦距、b为相机基线大小、u_L、u_R为空间点P在左右相平面成像坐标为，u 为理论视差；

步骤2.2：在对三维空间点进行计算的过程中，目标点深度测量误差计算为：

ε为匹配误差，x为当前帧的姿态信息；

对公式(2)进行优化得到深度测量误差应表示为：

步骤3：根据视觉测量中随机误差分布特点对三维空间点坐标进行聚类；

在测量时，由于使用过程中相机焦距、基线均为固定参数，造成随机误差影响的主要可变因素为被测物体与相机之间的距离大小；因此将视觉测量信息中计算得到的三维空间点按照与被测物体距离远近做聚类处理。

步骤4：构建随机误差模型V；设包括两类观测数据，第一类观测数据为B₁，第二类观测数据为B₂，

其中，v₁ v₂表示第一类、第二类观测数据对应的残差，f(x)为三维空间坐标按PNP方式求解对应二维坐标的线性化函数，L为上一帧中各二维特征点经匹配方式获取其在当前帧的二维坐标向量，L₁、L₂为两类观测数据对应的测量值，B为两类观测B₁、B₂的集合，R为旋转信息、t为平移信息，I为单位矩阵；

将公式(4)按照两类观测数据分别展开，可表示为：

对公式(5)构建法方程求解：

对应优化状态向量x的解

为：

其中，

其中，N₁、N₂表示第一类、第二类观测值与权重信息构建的计算矩阵，W₁、W₂表示第一类、第二类观测值与测量值构建的计算矩阵；式(7)求解过程中，由于三维空间点与对应二维平面特征点为非线性变换，利用重投影误差优化模型进行线性化操作，此时B、L应为其三维空间点线性化展开对应形式。在公式(8)中P为各类测量对应的权重矩阵；第一次平差时给定的两类观测值的权P₁和P₂是不恰当的，或者说两类观测值所对应的单位权方差并不相等，令其系数分别为

当观测数据优化过程中方差-协方差矩阵记为D(L),两类数据各自的方差-协方差记为D(L₁)、D(L₂)时,其与权重矩阵P关系可由式11表示

对公式(11)中

利用方差分量估计进行修正，其严密计算公式为：

式(12)中n₁、n₂分别表示两类观测矩阵的维数。在实际计算过程中，由于矩阵N的求逆操作造成占用大量存储空间且计算量增大，因此对方差分量进行简化操作，得到：

即各类观测值的单位权方差的估值为：

对该式求和可知，其属于有偏估计被称为赫尔默特近似公式。

步骤5：对步骤3聚类后的每一个聚类簇分配一个步骤4建立的随机误差模型，然后对每个聚类簇的随机误差模型进行优化

步骤5-1：对聚类后每一聚类簇数据计算平差信息:

步骤5-2：对两类测量信息计算权方差

如公式16所示：

然后，利用权方差获得对应的权重，其中c为常数；

步骤5-3：随机误差模型的迭代优化；在进行优化过程中，为使随机误差模型达到最优效果，对步骤5-1和步骤5-2进行反复迭代求解，直至公式16中的

收敛，获取权重系数最优解；在实验具体计算过程中，为保证系统计算实时性及优化效果收敛性，设置条件为：

步骤6：在非线性优化系统中对各聚类簇对应的随机误差模型进行优化；

步骤6-1：利用图像测量信息进行系统后端的优化过程中，根据使用传感器类型分为两类：

(1)视觉非线性优化系统，优化随机误差模型中的R，t；

优化函数为：

其中，ρ为鲁棒核函数、π表示将三维空间信息转换为2d图像位置计算过程、x为特征点二维空间坐标、X为特征点三维空间坐标；N为多个关键帧、i为各关键帧图像上的特征点；

(2)视觉/惯性非线性优化系统，优化随机误差模型中的x；

该系统优化部分包括滑动窗口中先验残差r_p-H_px²、滑动窗口中IMU测量构建残差为

图像帧间的视觉测量残差

其中，状态向量包括位置、姿态、速度、加速度，J_p表示状态向量的雅可比矩阵、

表示前后两帧间的IMU预积分误差、N为滑动窗口中关键帧的数目、

表示相对于惯性坐标系、

表示前后两帧中的特征点残差、F表示当前滑动窗口内特征点的集合、

表示相对于视觉测量坐标系，将所有误差构建整体优化公式为：

步骤6-2：构建非线性优化系统中的随机模型优化；在步骤6-1中，对于后端非线性优化系统主要由公式(18)、(19)构成，其中均含有视觉测量优化部分。在计算过程中，利用高斯牛顿法时有以下优化方程：

f(x+Δx)≈f(x)+J(x)Δx (20)

J(x)表示优化信息的雅可比矩阵，对上式位姿参数Δx进行最优化求解则有：

式21展开后对变量Δx求导，可得：

利用步骤5得到的各类权重信息优化位姿估计结果：

其中，D为各类观测的方差-协方差矩阵，由步骤4中的D(L)构成。

相较于目前各视觉、视觉/惯性融合导航系对视觉测量信息间的方差-协方差关系设置为单位矩阵从而进行高斯牛顿求解的过程，本发明可以充分考虑相机测量器件带来的随机误差影响，经过大量实验证明其对视觉导航系统、视觉/惯性融合导航系统位姿计算准确性均有较明显的提升效果，且可广泛适用于诸如VINS-mono、ROVIO、VINS-fusion等系统。

Claims (2)

1.一种基于方差分量估计理论的实时自身位姿计算方法，具体包括以下步骤：

步骤1：使用视觉传感器实时获取图像测量信息，完成系统初始化；其中双目立体相机按照特征点提取、左右相机图像特征点匹配、单位原点位置信息构建，实现系统初始化；

步骤2：完成初始化后，利用测量信息计算三维空间点坐标；

步骤3：根据视觉测量中随机误差分布特点对三维空间点坐标进行聚类；

在测量时，由于使用过程中相机焦距、基线均为固定参数，造成随机误差影响的主要可变因素为被测物体与相机之间的距离大小，因此将视觉测量信息中计算得到的三维空间点按照与被测物体距离远近做聚类处理；

步骤4：构建随机误差模型V；设包括两类观测数据，第一类观测数据为B₁，第二类观测数据为B₂，

其中，v₁、v₂表示第一类、第二类观测数据对应的残差，f(x)为三维空间坐标按PNP方式求解对应二维坐标的线性化函数，L为上一帧中各二维特征点经匹配方式获取其在当前帧的二维坐标向量，L₁、L₂为两类观测数据对应的测量值，B为两类观测数据B₁、B₂的集合，R为旋转信息、t为平移信息，I为单位矩阵；

将公式(4)按照两类观测数据分别展开，可表示为：

对公式(5)构建法方程求解：

对应优化状态向量x的解

为：

其中，

其中，N₁、N₂表示第一类、第二类观测值与权重信息构建的计算矩阵，W₁、W₂表示第一类、第二类观测值与测量值构建的计算矩阵；式(7)求解过程中，由于三维空间点与对应二维平面特征点为非线性变换，利用重投影误差优化模型进行线性化操作，此时B、L应为其三维空间点线性化展开对应形式；在公式(8)中P为各类测量对应的权重矩阵；第一次平差时给定的两类观测值的权P₁和P₂是不恰当的，或者说两类观测值所对应的单位权方差并不相等，令其系数分别为

当观测数据优化过程中方差-协方差矩阵记为D(L),两类数据各自的方差-协方差记为D(L₁)、D(L₂)时,其与权重矩阵P关系可由式11表示

对公式(11)中

利用方差分量估计进行修正，其严密计算公式为：

式(12)中n₁、n₂分别表示两类观测矩阵的维数；对方差分量进行简化操作，得到：

即各类观测值的单位权方差的估值为：

步骤5：对步骤3聚类后的每一个聚类簇分配一个步骤4建立的随机误差模型，然后对每个聚类簇的随机误差模型进行优化；

步骤5-1：对聚类后每一聚类簇数据计算平差信息:

步骤5-2：对两类测量信息计算权方差

如公式17所示：

然后，利用权方差获得对应的权重，其中c为常数；

步骤5-3：随机误差模型的迭代优化；在进行优化过程中，为使随机误差模型达到最优效果，对步骤5-1和步骤5-2进行反复迭代求解，直至公式16中的

收敛，获取权重系数最优解；在实验具体计算过程中，为保证系统计算实时性及优化效果收敛性，设置条件为：

步骤5-4：采用优化后的随机误差模型计算得到每一个聚类簇的方差-协方差D(L₁)、D(L₂)；

步骤6：采用步骤5得到的所有聚类簇的方差-协方差D(L₁)、D(L₂)计算位姿信息；

步骤6-1：利用图像测量信息进行系统后端的优化过程中，根据使用传感器类型分为两类：

(1)视觉非线性优化系统，优化随机误差模型中的R，t；

优化函数为：

其中，ρ为鲁棒核函数、π表示将三维空间信息转换为2d图像位置计算过程、x为特征点二维空间坐标、X为特征点三维空间坐标；M为关键帧的数据、i为各关键帧图像上的特征点；

(2)视觉/惯性非线性优化系统，优化随机误差模型中的x；

该系统优化部分包括滑动窗口中先验残差r_p-H_px²、滑动窗口中IMU测量构建残差为

图像帧间的视觉测量残差

其中，状态向量包括位置、姿态、速度、加速度、陀螺仪，J_p表示状态向量的雅可比矩阵、

表示前后两帧间的IMU预积分误差、Q为滑动窗口中关键帧的数目、

表示相对于惯性坐标系、

表示前后两帧中的特征点残差、F表示当前滑动窗口内特征点的集合、

表示相对于视觉测量坐标系，将所有误差构建整体优化公式为：

步骤6-2：构建非线性优化系统中的随机模型优化；在步骤6-1中，对于后端非线性优化系统主要由公式(18)、(19)构成，其中均含有视觉测量优化部分；在计算过程中，利用高斯牛顿法时有以下优化方程：

f(x+Δx)≈f(x)+J(x)Δx (20)

J(x)表示优化信息的雅可比矩阵，对上式位姿参数Δx进行最优化求解则有：

式21展开后对变量Δx求导，可得：

利用步骤5得到的各类权重信息优化位姿估计结果：

其中，D为各类观测数据的方差-协方差矩阵，由步骤4中的所有聚类簇对应的D(L₁)、D(L₂)构成。

2.如权利要求1所述的一种基于方差分量估计理论的实时自身位姿计算方法，其特征在于，所述步骤2的具体方法为：

步骤2.1：在利用视觉传感器进行位姿计算的过程中，对双目测量信息直接利用相机的基线信息计算空间坐标，计算方法如下：

其中，f为相机焦距，b为相机基线大小，u_L、u_R为空间点P在左右相平面成像坐标，u为理论视差；

步骤2.2：在对三维空间点进行计算的过程中，目标点深度测量误差计算为：

ε为匹配误差，x为当前帧的姿态信息；

对公式(2)进行优化得到深度测量误差应表示为：

步骤2.3：采用步骤2.2的误差对步骤1的空间坐标进行校正。

Images