[发明专利]一种基于方差分量估计理论的实时自身位姿计算方法

权利要求书

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的空间坐标进行校正。