CN111815684A - 一种基于统一残差模型的空间多元特征配准优化方法及装置 - Google Patents

Abstract

本发明涉及一种基于统一残差模型的空间多元特征配准优化方法及装置，该方法提出一种对点、线、面三种不同类型的特征点云构建统一的参数化表达模型，在前端匹配跟踪环节根据不同类型的特征匹配关系设计统一的残差模型进行融合配准。在后端局部优化环节根据统一的残差模型定义局部优化方程，实现统一的优化方程下对局部地图中的点、线、面三种类型的特征要素的联合优化。该方法将不同类型参数同一参数化表达，并定义同一的残差模型，保证了地图优化时的稳定性、效率以及精度。

Description

一种基于统一残差模型的空间多元特征配准优化方法及装置

技术领域

本发明涉及摄影测量与计算机视觉领域，尤其涉及一种基于统一残差模型的空间多元特征配准优化方法及装置。

背景技术

在自动驾驶导航及高精度地图等相关领域，SLAM技术一直是研究的热点，目前算法实现较为成熟，尤其是基于特征点的视觉SLAM技术，通过前端的特征提取、跟踪和多视交会，计算得到特征点在局部坐标系下的空间坐标，从而根据空间三维点进行实时的配准跟踪与局部优化，完成同步定位与建图的全流程，是目前视觉SLAM领域的主流算法之一。

基于点特征构造出的三维点地图无法有效表达现实中的场景结构，同时特征点地图数据量过大，存在大量信息冗余。因此基于线特征和面特征的的 SLAM算法逐渐被研究者所关注，特别是在街道、室内等人造建筑较多的环境下，线、面特征相对于特征点更能够反映出道路场景的结构特征，带有更高丰富的语义信息，用从点云中提取的线、面特征来表示点特征可以极大的压缩地图数据量，且在跟踪中更能抵抗环境光照等因素的影响。因此基于线、面特征的视觉SLAM算法开始成为车辆制图的核心技术。

但是由于场景的多样化，在某些特殊的场景内，线特征和面特征反而不如点特征更能够反映场景的结构特征。另外，传统的基于点、线、面特征的 SLAM算法通常是提取影像上的特征点和结构线、面特征，根据深度信息获取特征点云，进行特征匹配、跟踪，建立观测误差方程，并通过线性或者非线性优化求解观测误差值最小来计算最优的机器人位置和周围环境特征。对于每一种不同类型的特征点云都需要针对性的采用不同的参数化表达方式进行建模并构建不同的残差方程进行配准和优化，其中配准算法本质上依然是基于传统ICP算法的扩展，同时对不同类型特征采取不同的残差模型和配准策略会带来优化模型的不稳定性与效率及精度的降低。

名词解释：

参考帧：优先与当前帧匹配的帧，一般为上一轮跟踪成功的帧或局部地图中匹配特征数最多的帧；

共视帧：与当前帧匹配(观测)到相同路标的帧；

全局路标：世界坐标系下由点、线、面特征构成的对象。

发明内容

本发明针对现有技术中存在的技术问题，提供一种基于统一残差模型的空间多元特征配准优化方法及装置，该方法提出一种对点、线、面三种不同类型的特征点云构建统一的参数化表达模型，在前端匹配跟踪环节根据不同类型的特征匹配关系设计统一的残差模型进行融合配准。在后端局部优化环节根据统一的残差模型定义局部优化方程，实现统一的优化方程下对局部地图中的点、线、面三种类型的特征要素的联合优化。

本发明解决上述技术问题的技术方案如下：

第一方面，本发明提供一种基于统一残差模型的空间多元特征配准优化方法，包括以下步骤：

对单帧图像中的点、线、面特征进行统一参数化描述；

根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素 m转换到全局坐标系，得到局部特征要素m的全局参数m'；所述的全局坐标系为起始帧相机坐标系；

将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

定义统一的残差模型，根据匹配关系，计算当前帧的局部特征要素m与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

进一步的，所述的对单帧图像中的点、线、面特征进行统一参数化描述，包括：

定义点p、线L、面α特征的三维中心点p'、方向矢量

3*3姿态矩阵 R、3*3形态矩阵Ω；

其中点p、线L、面α特征的三维中心点p'、方向矢量

如下所示：

点p、线L、面α特征的3*3姿态矩阵R如下所示：

点p、线L、面α特征的3*3形态矩阵Ω如下所示：

特征类型	形态矩阵Ω
		点p	diag(1,1,1)
线L	diag(0,1,1)
		面α	diag(1,0,0)

基于上述定义，将任意点p、线L、面α特征参数化为：m:{p'_m,R_m,Ω_m}。

进一步的，所述的根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数 m'，包括：

获取当前帧在全局坐标系下的位姿初值X；定义X＝{R_X|t_X}∈SE3，其中 SE3为三维特殊欧式群，R_X代表当前帧在全局坐标系下对应的3*3维姿态旋转矩阵,t_X代表当前帧在全局坐标系下对应的3*1维位姿平移矩阵；

根据公式

将当前帧中所包含的局部特征要素m转换到全局坐标系，得到全局参数 m'。

进一步的，所述的将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系，包括：

根据全局路标的特征类型，将全局路标参数化为空间节点并存储为二叉树结构；

将上一轮跟踪成功或局部地图匹配中匹配量最大的帧作为参考帧，将当前帧中的局部特征要素与参考帧中的全局路标进行匹配；

以当前帧为基准更新局部地图，并将当前帧中的局部特征要素与局部地图中存储的全局路标进行匹配；

若当前帧中的全局特征要素与参考帧中的路标的匹配以及当前帧中的局部特征要素与局部地图中存储的路标的匹配均失败时，进行全局搜索，将当前帧与全局地图中的全局路标进行匹配。

进一步的，所述的将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系，还包括：如果当前帧与参考帧、局部地图、全局地图的特征匹配均未成功，则将局部特征要素 m的全局参数m'作为新的全局路标加入全局地图。

进一步的，所述的统一的残差模型如下定义：

定义第i帧的第j个局部特征要素m_ij:{p'_ij,R_ij,Ω_ij}与全局路标

之间的7维残差模型：

其中，

为第i帧位姿矩阵，SE3为三维特殊欧式群，

代表第i帧在全局坐标系下的3*3维姿态旋转矩阵,t_i代表第i帧在全局坐标系下对应的3*1维位姿平移矩阵；

为全局路标的三维中心点，

为全局路标的方向矢量。

进一步的，所述的根据匹配关系，计算当前帧的局部特征要素m与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿，包括：

步骤301，根据外源传感器信息进行当前帧位姿插值或根据匀速运动模型基于历史帧预测当前帧的位姿

步骤302，按照残差模型，根据当前帧的位姿计算当前帧的每一局部特征要素的匹配残差；定义第k个局部特征要素的匹配残差为e_k，按照下式计算当前帧的所有局部特征要素的匹配残差

步骤303，判断

是否相对于上一轮迭代更新后已不再减小或迭代次数是否达到最大，若是则迭代结束并取当前的

作为当前帧的最优位姿，否则执行步骤304；

步骤304，遍历当前帧每一局部特征要素的匹配残差，定义第k项残差相对状态量的雅克比矩阵为J_k，按照下式计算各要素残差相对状态量的雅克比矩阵；

步骤305，分别计算中间变量矩阵：H矩阵和b矩阵，计算公式如下所示：

其中，Ω_k为残差信息矩阵；

步骤306，计算位姿迭代更新的扰动量Δx：Δx←-H^-1b；

步骤307，通过下式，用扰动量Δx更新当前帧预测位姿

并跳转至步骤302；

其中，R_x()、R_y()、R_z()分别代表绕局部坐标系x、y、z轴旋转一定角度的Rodriguz变换，Δα_x，Δα_y，Δα_z为绕局部坐标系x、y、z轴的相对旋转角。

第二方面，本发明提供一种基于统一残差模型的空间多元特征配准优化装置，包括：

参数描述模块，用于对单帧图像中的点、线、面特征进行统一参数化描述；

匹配模块，用于根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'；用于将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用 KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

配准模块，用于定义统一的残差模型，用于根据匹配关系，计算当前帧的局部特征要素与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

优化模块，用于利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

第三方面，本发明提供一种电子设备，包括：

存储器，用于存储计算机软件程序；

处理器，用于读取并执行所述存储器内存储的计算机软件程序，进而实现本发明第一方面所述的一种基于统一残差模型的空间多元特征配准优化方法。

第四方面，本发明提供一种非暂态计算机可读存储介质，所述存储介质中存储有用于实现本发明第一方面所述的一种基于统一残差模型的空间多元特征配准优化方法的计算机软件程序。

本发明的有益效果是：

1、对于提取的点、线、面不同类型的特征点云，需要构建不同的参数化表达模型，本发明定义了一个统一的参数化模型，可以由一个参数形式表达点、线、面三种不同类型的基元特征。

2、给定一组匹配好的点、线、面特征，需要针对不同类型的特征设计不同的配准方法，本发明根据统一的特征参数化形式针对所有类型的要素特征提出统一的残差模型进行融合配准。

3、构建局部优化方程时，对于点、线、面三种不同类型特征需分别定义不同的残差模型和优化函数进行优化，本发明对三种不同类型的要素定义统一的残差模型，在一个优化方程中进行局部图优化。

附图说明

图1为本发明实施例提供的一种基于统一残差模型的空间多元特征配准优化方法流程图；

图2为本发明实施例提供的局部地图更新流程图；

图3为本发明实施例提供的特征匹配流程图；

图4为本发明实施例提供的局部优化因子图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

实施例1

如图1所示，本发明实施例提供一种基于统一残差模型的空间多元特征配准优化方法，包括以下步骤：

对单帧图像中的点、线、面特征进行统一参数化描述；

根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素 m转换到全局坐标系，得到局部特征要素m的全局参数m'；所述的全局坐标系为起始帧相机坐标系；

将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用 KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

定义统一的残差模型，根据匹配关系，计算当前帧的局部特征要素与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

具体的，该方法包括以下步骤：

S100，点、线、面特征统一参数化

获取单帧图像提取的点p、线L、面α特征点云后，对特征要素进行统一的参数化表示。空间中对于点、线、面任意特征都可以根据式(1.1)进行表示，

(p-p')^TRΩR^T(p-p')＝0 (1.1)

其中p'为特征的3维中心点、R为3*3姿态矩阵、Ω为3*3维形态矩阵。

因此，对任意点p、线L、面α对象，根据式(1.1)，可以将其形式化为式(1.2)的形式。

m:{p'_m,R_m,Ω_m} (1.2)

表1.1所示为点p、线L、面α特征的中心点p'和方向矢量

的定义方式。姿态矩阵R根据方向矢量进行计算，形态矩阵Ω根据点、线、面三种要素的类型进行定义。点p、线L、面α特征的姿态矩阵R和形态矩阵Ω如表1.2、表1.3所示进行定义。

表1.1多元特征要素的中心点p'与方向矢量

定义

表1.2多元特征要素的姿态矩阵R定义

表1.3多元特征要素的形态矩阵Ω定义

特征类型	形态矩阵Ω
		点p	diag(1,1,1)
线L	diag(0,1,1)
		面α	diag(1,0,0)

S200，特征匹配

首先预测当前帧在全局坐标系下的位姿初值，以起始帧相机坐标系作为全局坐标系，可以根据IMU等外源传感器信息进行当前帧位姿插值或在没有外源传感器的情况下根据匀速运动模型，以前两帧相对运动估计出当前帧在全局坐标系下4*4维位姿初值X，定义X＝{R_X|t_X}∈SE3，其中SE3为三维特殊欧式群(Special Euclidean Group),满足

R_X代表全局坐标系下3*3维位姿旋转矩阵，满足R_X∈SO3，其中SO3为三维特殊正交群(Special Orthogonal Group)，满足SO3＝{R∈^3*3|RR^T＝I}，t_X代表全局坐标系下3*1维位姿平移矩阵，满足t_X∈³。当前帧按照式(1.1)、式(1.2)创建相机坐标系下局部特征要素m之后，根据位姿初值X按式(1.3)将m转换到全局坐标系，得到m'。

将m'与全局路标进行匹配，如果全局路标不存在或未匹配到全局路标，则m'作为新的全局路标加入全局地图。为加速匹配过程，m'与全局路标的匹配基于KDTree搜索的方式进行，根据全局路标的要素类型，将全局路标按表1.4所示参数化为空间结点并存储为二叉树结构。根据局部特征要素类型，按表1.4所示将m'参数化为待搜索结点，作为输入参数在空间二叉树内进行最近邻搜索，从而得到匹配的全局路标。

表1.4多元特征要素KDTree结点参数模型

为提高匹配效率，匹配优先在参考帧和局部地图中进行。图3所示为基于KDTree的匹配流程，步骤包括：

S201，将上一轮跟踪成功或局部地图匹配中匹配量最大的帧作为全局参考帧，将当前帧要素和参考帧的观测路标进行匹配。

S202：为保证当前帧尽可能多的匹配全局路标，以当前帧为基准更新局部地图，然后将当前帧要素和局部地图存储的路标进行匹配。

S203：当运动过快、场景特征稀疏时可能存在匹配失败，如果S201，S202 两步均未匹配到全局路标，则进行全局搜索，将当前帧与全局地图的路标进行匹配。

局部地图更新流程如图2所示，包括：

(1)提取与当前帧有共同观测特征的帧构成共视帧集合；

(2)提取与共视帧相邻的但与当前帧无共同观测特征的帧构成领域帧集合；

(3)由共视帧和领域帧共同构成局部帧集合，获取局部帧匹配的所有路标，构成局部路标集合；

(4)局部路标和局部帧共同构成局部地图。

步骤300，单帧匹配

定义第i帧的第j个局部特征要素m_ij:{p'_ij,R_ij,Ω_ij}与全局路标

之间的7维残差模型：

其中，

为第i帧位姿矩阵，

为全局路标的方向矢量。

残差协方差矩阵定义如式(1.5)所示。

对于不同特征类型之间的匹配残差，协方差矩阵(信息矩阵)的定义如表1.5所示。

表1.5残差信息矩阵定义

根据当前帧与全局路标的匹配残差模型，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿。算法步骤包括：

S301，根据外源传感器信息进行当前帧位姿插值或根据匀速运动模型基于历史帧预测当前帧的位姿。本实施例根据匀速运动模型基于历史帧预测当前帧的初始位姿

S302，按照式(1.4)，根据当前帧的预测位姿

计算当前帧每一个匹配要素的匹配残差。定义第k个要素的匹配残差为e_k，按照式(1.6)计算当前帧所有局部特征要素的匹配残差

如果

相对上一轮迭代更新后已不再减小或达到最大迭代次数，迭代结束，取当前

为当前帧最优位姿，否则进入步骤S303。

S303，遍历当前帧每一个要素的匹配残差，定义第k项残差相对状态量的雅克比矩阵为J_k，按照式(1.7)所示计算各要素残差相对状态量的雅克比矩阵。

S304，按照式(1.8)，式(1.9)的形式分别计算中间变量矩阵:H矩阵，b矩阵。其中，Ω_k为残差信息矩阵。

S305，按照式(1.10)计算位姿迭代更新的扰动量Δx。

Δx←-H^-1b (1.10)

S306：用扰动量Δx更新当前帧预测位姿

回到步骤3.2。

Δx为6*1维扰动量，表示成如下形式：

其中Δt为平移分量，Δα为旋转分量。为了将扰动量Δx作用于

首先根据式(1.12)通过v2t()函数将Δx转换成旋转矩阵和平移向量的形式ΔX。

其中，R_x()，R_y()，R_z()分别代表绕局部坐标系x,y,z轴旋转一定角度的 Rodriguz变换，Δα_x，Δα_y，Δα_z为绕局部坐标系x、y、z轴的相对旋转角。

根据式(1.13)将ΔX左乘

实现当前帧

的迭代预测。

S400，局部地图优化

利用非线性最小二乘法，通过迭代矫正局部地图中的路标以及观测帧的位姿，达到匹配残差总体最小化的目的。根据图3所示更新当前帧所在的局部地图要素，包括局部帧和局部路标要素。设定待优化的局部帧为与当前帧共视特征数最多的前N个共视帧，与局部帧共视的其它帧设定为优化中的固定帧，利用图优化算法自定义相应的图优化顶点和边，设定每条边的顶点为待优化的全局路标位姿和观测帧位姿，每条边对应帧与全局路标的匹配残差项，由式(1.4)定义。每条优化边的迭代优化流程按照步骤S301～步骤S306 所述的高斯牛顿法进行。局部优化因子如图4所示，其中圆圈节点代表待估计的***状态，方框节点所连的方框代表观测或者先验，包含了对它所连变量的一个约束。

被优化的状态变量χ包括N个位姿和M个路标参数χ:{m₁,m₂,...,m_M,X₁,X₂,...,X_N}，其中X_i代表第i帧位姿矩阵，m_j代表第j个全局路标参数。相应的被优化函数为如式(1.14)、(1.15)所示，其中e_ij如式1.4所示，Ω_ij的定义如式(1.5)所示。χ^*指代满足最小化的最优变量。

实施例2

第二方面，本发明提供一种基于统一残差模型的空间多元特征配准优化装置，包括：

参数描述模块，用于对单帧图像中的点、线、面特征进行统一参数化描述；

匹配模块，用于根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'；用于将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用 KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

配准模块，用于定义统一的残差模型，用于根据匹配关系，计算当前帧的局部特征要素与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

优化模块，用于利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

实施例3

本发明提供一种电子设备，包括：

存储器，用于存储计算机软件程序；

处理器，用于读取并执行所述存储器内存储的计算机软件程序，进而实现本发明第一方面所述的一种基于统一残差模型的空间多元特征配准优化方法。

同时还需说明的是，计算机软件程序中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM， RandomAccess Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于统一残差模型的空间多元特征配准优化方法，其特征在于，包括以下步骤：

对单帧图像中的点、线、面特征进行统一参数化描述；

根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'；所述的全局坐标系为起始帧相机坐标系；

将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

定义统一的残差模型，根据匹配关系，计算当前帧的局部特征要素与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

2.根据权利要求1所述的方法，其特征在于，所述的对单帧图像中的点、线、面特征进行统一参数化描述，包括：

定义点p、线L、面α特征的三维中心点p'、方向矢量

3*3姿态矩阵R、3*3形态矩阵Ω；

其中点p、线L、面α特征的三维中心点p'、方向矢量

如下所示：

点p、线L、面α特征的3*3姿态矩阵R如下所示：

点p、线L、面α特征的3*3形态矩阵Ω如下所示：

特征类型 形态矩阵Ω 点p diag(1,1,1) 线L diag(0,1,1) 面α diag(1,0,0)

基于上述定义，将任意点p、线L、面α特征参数化为：m:{p'_m,R_m,Ω_m}。

3.根据权利要求2所述的方法，其特征在于，所述的，根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'，包括：

获取当前帧在全局坐标系下的位姿初值X；定义X＝{R_X|t_X}∈SE3，其中SE3为三维特殊欧式群，R_X代表当前帧在全局坐标系下对应的3*3维姿态旋转矩阵,t_X代表当前帧在全局坐标系下对应的3*1维位姿平移矩阵；

根据公式

将当前帧中所包含的局部特征要素m转换到全局坐标系，得到全局参数m'。

4.根据权利要求1所述的方法，其特征在于，所述的将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系，包括：

根据全局路标的特征类型，将全局路标参数化为空间节点并存储为二叉树结构；

将上一轮跟踪成功或局部地图匹配中匹配量最大的帧作为参考帧，将当前帧中的局部特征要素与参考帧中的全局路标进行匹配；

以当前帧为基准更新局部地图，并将当前帧中的局部特征要素与局部地图中存储的全局路标进行匹配；

若当前帧中的全局特征要素与参考帧中的路标的匹配以及当前帧中的局部特征要素与局部地图中存储的路标的匹配均失败时，进行全局搜索，将当前帧与全局地图中的全局路标进行匹配。

5.根据权利要求4所述的方法，其特征在于，所述的将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系，还包括：如果当前帧与参考帧、局部地图、全局地图的特征匹配均未成功，则将局部特征要素m的全局参数m'作为新的全局路标加入全局地图。

6.根据权利要求1所述的方法，其特征在于，所述的统一的残差模型如下定义：

定义第i帧的第j个局部特征要素m_ij:{p'_ij,R_ij,Ω_ij}与全局路标

之间的7维残差模型：

其中，

为第i帧位姿矩阵，SE3为三维特殊欧式群，

代表第i帧在全局坐标系下的3*3维姿态旋转矩阵,t_i代表第i帧在全局坐标系下对应的3*1维位姿平移矩阵；

为全局路标的三维中心点，

为全局路标的方向矢量。

7.根据权利要求1或6所述的方法，其特征在于，所述的根据匹配关系，计算当前帧的局部特征要素m与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿，包括：

步骤301，根据外源传感器信息进行当前帧位姿插值或根据匀速运动模型基于历史帧预测当前帧的位姿

步骤302，按照残差模型，根据当前帧的位姿计算当前帧的每一局部特征要素的匹配残差；定义第k个局部特征要素的匹配残差为e_k，按照下式计算当前帧的所有局部特征要素的匹配残差

步骤303，判断

是否相对于上一轮迭代更新后已不再减小或迭代次数是否达到最大，若是则迭代结束并取当前的

作为当前帧的最优位姿，否则执行步骤304；

步骤304，遍历当前帧每一局部特征要素的匹配残差，定义第k项残差相对状态量的雅克比矩阵为J_k，按照下式计算各要素残差相对状态量的雅克比矩阵；

步骤305，分别计算中间变量矩阵：H矩阵和b矩阵，计算公式如下所示：

其中，Ω_k为残差信息矩阵；

步骤306，计算位姿迭代更新的扰动量Δx：Δx←-H^-1b；

步骤307，通过下式，用扰动量Δx更新当前帧预测位姿

并跳转至步骤302；

其中，R_x()、R_y()、R_z()分别代表绕局部坐标系x、y、z轴旋转一定角度的Rodriguz变换，Δα_x，Δα_y，Δα_z为绕局部坐标系x、y、z轴的相对旋转角。

8.一种基于统一残差模型的空间多元特征配准优化装置，其特征在于，包括：

参数描述模块，用于对单帧图像中的点、线、面特征进行统一参数化描述；

匹配模块，用于根据当前帧在全局坐标系下的初始位姿X，将当前帧中的局部特征要素m转换到全局坐标系，得到局部特征要素m的全局参数m'；用于将全局参数m'作为输入结点，在参考帧、局部地图、全局地图中利用KDTree搜索算法找到空间上与全局参数m'最近邻的全局路标，建立当前帧局部特征要素m与全局路标的匹配关系；

配准模块，用于定义统一的残差模型，用于根据匹配关系，计算当前帧的局部特征要素与全局路标之间的匹配残差，并根据所述匹配残差，采用高斯牛顿法迭代求解当前帧在全局坐标系下的最优位姿；

优化模块，用于利用局部地图中各个帧在全局坐标系下的最优位姿对局部地图进行图优化。

9.一种电子设备，其特征在于，包括：

存储器，用于存储计算机软件程序；

处理器，用于读取并执行所述存储器内存储的计算机软件程序，进而实现权利要求1-7任一项所述的一种基于统一残差模型的空间多元特征配准优化方法。

10.一种非暂态计算机可读存储介质，其特征在于，所述存储介质中存储有用于实现权利要求1-7任一项所述的一种基于统一残差模型的空间多元特征配准优化方法的计算机软件程序。

Images