CN112902953B - 一种基于slam技术的自主位姿测量方法 - Google Patents

Abstract

本发明涉及远程态势感知领域，尤其涉及一种基于SLAM技术的自主位姿测量方法，基于视觉传感器、IMU惯导传感器和GPS传感器实现，所述方法包括：接收待建场景的点云数据、图像信息、姿态信息和目标位置信息；基于改进的极几何约束3D点云运动分割算法对高动态场景进行对象级运动场景分割；对点云数据、图像信息、姿态信息和目标位置信息采用多源数据定位定姿算法:对静态部分的图像信息采用2D‑3D匹配算法，对动态目标采用动态目标与视点位置关联估计算法，实现定位；对场景中的静态部分进行三维点云场景构建得到精确匹配的点云数据，对场景中的动态目标进行动态物体构建，并对定位定姿结果辅助进行全局优化，进而完成位姿测量。

Description

一种基于SLAM技术的自主位姿测量方法

技术领域

本发明涉及远程态势感知领域，涉及SLAM技术在态势感知领域协同交互应用中的视觉定位定姿方法，尤其涉及一种基于SLAM技术的自主位姿测量方法。

背景技术

随着近几年硬件能力和智能算法的不断进步，增强现实技术从实验室开始大步走向实际应用，逐渐进入人们的视野。增强现实既允许用户看到真实世界，同时也能看到叠加在真实世界上的虚拟对象，将计算机生成的图形与用户在真实物理世界获取的视觉信息组合在一起，具有虚实结合、实时交互的新特点，使用户产生全新体验，并提高对现实世界中的事物和物理现象的洞察力，因此增强现实系统在远程态势感知领域具有重大的应用潜力。

传统的视觉同步定位与场景重构(Simultaneous Localization And Mapping，以下简称SLAM)技术解决的是当视点自身位置未知、场景未知的情况下，利用视觉图像对自身定位的同时完成场景构建。通过SLAM技术，能够显著提高作战人员对周边环境的感知能力、对战场态势的认知深度。而这类算法有一个前提假设，就是周边环境必须是静态的。如果将动态物体引入场景，按照现有的算法，会带来很大的估计误差，这会极大地影响现有视觉SLAM算法的有效性。

动态场景下的SLAM的难点之一在于矩阵病态(ill-posed)特性，即观测与重投影误差不稳定导致求解最小化问题时的矩阵也不稳定，求解结果通常难以收敛到一个恒定值。此外，动态物体对场景描述也带来了困难，以往的静态点云难以表达各种刚性、柔性的运动过程，必须建立新的场景描述方法，同时尽量减少数据存储量。与此同时，从静态点云到动态点云数据量会激增，目前的主流方案对柔体、或者周期性运动场景物体缺乏有效的简练描述方式。

目前，国际上很少有对整个动态场景视觉SLAM的研究，大多数工作主要集中在其中的小部分内容，相关工作有单独考虑运动分割，将极几何约束或场景流引入解决动、静态环境判别问题；也有工作针对已有运动分割或其他先验的前提下进行视点定位或动态地图构建等算法。

综上所述，在复杂环境的应用场景中，大量存在的动态物体使目前SLAM计算难以适应，因此要构建一个完整而有效的动态场景视觉SLAM框架是一个巨大的挑战。

发明内容

本发明的目的在于克服现有技术缺陷，提出了一种基于SLAM技术的自主位姿测量方法。主要围绕在态势感知领域协同交互的定位与重构需求和对于数据的时空一致性、虚实场景一致性问题，开展基于动态场景下视觉SLAM技术的研究与应用。

为了实现上述目的，本发明提出了一种基于SLAM技术的自主位姿测量方法，基于视觉传感器、IMU惯导传感器和GPS传感器实现，所述方法包括：

步骤1)接收待建场景的点云数据、图像信息、姿态信息和目标位置信息；

步骤2)基于改进的极几何约束3D点云运动分割算法对高动态场景进行对象级运动场景分割；

步骤3)对点云数据、图像信息、姿态信息和目标位置信息采用多源数据定位定姿算法，对静态部分的图像信息采用2D-3D匹配算法，对动态目标采用动态目标与视点位置关联估计算法，实现定位；

步骤4)对场景中的静态部分进行三维点云场景构建得到精确匹配的点云数据，对场景中的动态目标进行动态物体构建，并对定位定姿结果辅助进行全局优化，进而完成位姿测量。

作为上述方法的一种改进，所述步骤2)具体包括：

步骤2-1)对前后帧的图像进行特征点提取；

步骤2-2)根据提取的特征点与环境3D点云是否符合极几何约束条件，进行动静态物体的区分，对不同运动物体的3D点云场景进行初步分割；

步骤2-3)通过条件随机场的优化，得到对象级别的运动场景分割数据。

作为上述方法的一种改进，所述步骤2-2)具体包括：

对前后两帧的特征点与环境3D点云间的几何关系进行判断，如果符合极几何约束条件，则对前后两帧图像上每个特征点做一次动静态判断，然后在三维空间中，计算特征点x_i和x_j之间的相对距离d_ij＝||x_i-x_j||，判断相对距离d_ij是否稳定，如果稳定，为同一个运动物体，否则，为不同的运动物体。

作为上述方法的一种改进，所述步骤2-3)具体包括：

基于条件随机场的联合估计能量函数E^J(z)为：

其中，z_i＝[x_i,y_i]为随机变量Z_i对应的运动标签，Z_i＝[X_i,Y_i]，x_i为运动物体标签，y_i为运动模式；

其中，M为运动模式，

表示变量(x_i)对应某个物体的损失值，

表示变量对应某个标签的损失值，

表示两个变量之间的匹配值，具体为：

条件随机场为全连接拓扑图，p(i,j)为：

其中，p_i表示第i个像素，I_i表示第i个像素的亮度，θ_β、θ_v、θ_p是训练数据中学习到的模型参数；

通过条件随机场的优化，得到对象级别的运动场景分割数据。

作为上述方法的一种改进，所述步骤3)具体包括：

步骤3-1)在2D环境中，对点云数据、图像信息、姿态信息和目标位置信息采用“视觉+IMU+GPS”多源数据定位定姿算法；

步骤3-2)在3D静态环境中，对图像信息采用2D-3D匹配算法，通过引入环境信息加强定位解算；

步骤3-3)在3D动态环境中，采用动态目标与视点位置关联估计算法，将分割后的动态目标运动信息进行建模，建立运动物体运动模型及与视点间的数据关联，从动态物体提取出有利于提高视点定位精度的相关信息，实现定位。

作为上述方法的一种改进，所述步骤3-1)具体为：

当GPS状态正常时，以GPS和IMU组合导航算法进行定位定姿计算；

在GPS状态异常但未失锁时，以GPS、视觉和IMU组合导航算法进行定位定姿计算，引入优化变量的边缘化处理；

当GPS为失锁状态，以视觉和IMU组合导航算法进行定位定姿计算。

作为上述方法的一种改进，所述步骤3-2)具体为：

采用2D-2D方法对相机定位：

其中，x_ij为特征点，E_i(R′,t′)是本质矩阵；

通过前后两帧图像特征点的8点法计算得出本质矩阵，通过SDV分解得到四组解，通过判断得到其中的唯一解，从而得到相机姿态；

对于含有3D环境点云数据，将第一帧观测到的3D点重投影到第二帧上面，得到最小误差的相机姿态

为：

其中，R为第一帧图像到相机坐标系下的旋转矩阵，R′为第二帧图像到相机坐标系下的旋转矩阵，t_i为第一帧图像到相机坐标系下的平移变量，t′为第二帧图像到相机坐标系下的平移变量，P为重投影矩阵，x_ij为第二帧特征点，

为第一帧特征点；i表示第一帧图像的横坐标，j表示第一帧图像的纵坐标，m表示第一帧图像的横坐标像素点，n表示第一帧图像的纵坐标像素点；

当2D与3D分割不一致时，结合两种定位方法联合估计相机位置，表达公式如下：

对于被动态物体遮挡的静态点云，在目标函数上增加权重，属于动态物体及其遮挡的点云权重设置为零，其他静态点权重设置为1。

作为上述方法的一种改进，所述对场景中的静态部分进行三维点云场景构建得到精确匹配的点云数据；具体包括：

在点云对应的2D纹理图像上进行特征检测和提取；

将提取的特征和匹配关系映射到三维点云数据，得到多源点云数据的特征点和匹配关系；

通过SVD方法估计得到旋转矩阵和平移矩阵，进而将多源的点云数据坐标变换到一个统一坐标系，实现已知对应关系的大场景三维点云快速匹配；

进行进一步的处理，消除多层重叠冗余、去除噪声和色差，得到精确匹配的点云数据。

作为上述方法的一种改进，所述对场景中的动态目标进行动态物体构建，并对定位定姿结果辅助进行全局优化，进而完成位姿测量；具体包括：

根据场景构建表示递归贝叶斯滤波p(y_t,j|χ^t，P^t，F^t)为：

p(y_t，j|χ^t，P^t，F^t)＝ηp(x_t|y_t，j，p_t)p(t_r，j|χ^t-1，P^t，F^t)

其中，t为时间，P^t为相机位置，F^t为场景流，χ^t＝{x₀…x_t}为观测量，x_t＝{d_t,s_t}，d_t表示观测到的深度信息，s_t表示语义分割信息，v_j为地图中的体素，p(o_j|χ^t,P^t,F^t)为该体素被占有的概率分布，p(l_j|χ^t,P^t，F^t)为语义标签概率分布，y_t，j为地图中每个体素被占有标签的联合表示；

将观测量χ^t加入到场景构建模型中，建立预测的概率分布模型为：

其中，

为观测到的体素被占有标签的概率，p(y_t，j|χ^t-1，F^t)为场景中被观测到的体素被占有标签的概率；

为每个动态的体素单元贴分类标签，根据核函数进行空间一致性及运动物体分类的判断，进而完成位姿测量；其中，核函数k(f_j，f_k)为：

其中，f_j和f_k分别是体素中心p_j、p_k及体素平均颜色c_j、c_k的特征一致性表征，w₁、w₂是两种核函数的权重，σ_α、σ_β、σ_γ是相应的核函数的标准差。

与现有技术相比，本发明的优势在于：

1、本发明突破了动目标分割、动态场景同步定位与虚拟场景构建、多源数据融合等关键技术，实现高动态场景下对环境精确感知和精确定位技术，为动态环境下的增强现实和环境感知提供技术支撑；

2、本发明在增强动态环境下融合视觉传感器、GPS传感器、IMU惯导传感器等多源传感器进行环境感知和空间定位定姿的准确性，包括自身定位、场景定位、目标定位等；

3、本发明推动以视觉SLAM为基础的多源场景感知和动态场景定位与重构技术的发展，突破现有技术在高动态场景下难以进行自主定位与场景重构的技术难题；

4、本发明提出了基于GPS(或北斗)、IMU和视觉传感器组合的高精度定位定姿方法，针对复杂战场环境中所面临的高精确空间定位问题，提出了将视觉信息与GPS、IMU方法相融合的解决思路，有效利用视觉图像匹配，降低姿态与定位误差，有效修正长期位姿偏移，实现多源数据融合的高精度定姿定位，具有很高的应用价值；

5、本发明提出了高动态场景的视觉SLAM技术，针对高动态场景定位与重构这一前沿技术难题，提出了运动视角下的动态点云分割、对象级的场景重构与融合的体系化解决思路，是一项具有一定原创性的基础研究工作，在增强现实领域基于该思路的对象级动态SLAM研究属于前沿，国内外还未见完整技术体系的文献报道。

附图说明

图1是本发明实施例1的基于SLAM技术的自主位姿测量方法的路线图；

图2(a)是本发明实施例1的极几何约束第一示意图；

图2(b)是本发明实施例1的极几何约束第二示意图；

图3是本发明实施例1的分割边界精细化流程图；

图4是本发明实施例1的多源数据融合时各状态切换原理图；

图5是本发明实施例1的GPS/IMU导航的卡尔曼滤波器；

图6是本发明实施例1的优化问题的图形式及图稀疏化性质；

图7(a)是本发明实施例1的图优化拓扑结构；

图7(b)是本发明实施例1的边缘化图像位姿；

图7(c)是本发明实施例1的边缘化3D空间点；

图8是本发明实施例1的视觉/IMU融合位姿计算算法优化过程；

图9(a)是本发明实施例1的特征点法视点定位示意图；

图9(b)是本发明实施例1的关键帧法视点定位示意图；

图10(a)是本发明实施例1的视点与动态物体的关联过程；

图10(b)是本发明实施例1的视点与动态物体的关联过程；

图11是本发明实施例1的点云数据拼接技术图；

图12(a)是本发明实施例1的哈希体素表达第一示意图；

图12(b)是本发明实施例1的哈希体素表达第二示意图；

图13是仿真示例选用的EuRoC MAV数据集；

图14是仿真示例采用算法定位结果的效果界面；

图15(a)是整体场景构建仿真示例的效果图；

图15(b)是局部场景构建仿真示例的效果图。

具体实施方式

本专利设计了一套用于态势感知的基于动态场景视觉同步定位与场景重构算法，其技术路线如图1所示。针对高动态场景，首先基于改进的极几何约束3D点云运动分割算法进行场景的对象级精确运动分割；之后将2D静态环境中“视觉+IMU+GPS”多源数据定位定姿算法、3D静态环境中2D-3D匹配算法、动态目标与视点位置关联估计算法获得的定位数据融合后进行更为精确的视点定位定姿；最后对场景中动、静态部分分别进行场景重构，针对静态部分采用点云数据拼接的场景构建算法进行3D点云场景构建，针对动态部分采用基于哈希体素的动态场景构建算法进行全局的动态场景构建。

下面结合附图和实施例对本发明的技术方案进行详细的说明。

实施例1

如图1所示，本发明的实施例1提出了一种基于SLAM技术的自主位姿测量方法。本方法主要涉及三个关键技术：高动态场景的动静分离技术、多源数据融合的高精度自主定位技术、虚实场景构建与一致性匹配技术。

一、高动态场景的动静分离技术

由于动态环境的引入，视觉SLAM的定位、地图构建、回环检测等各个模块都会受到不同程度的影响。不同运动物体的运动模式不同，导致SLAM模块的各个环节同时存在多个运动物体解空间，本专利技术为了避免直接求解动态场景下的多运动模式的定位及环境三维建模方程组，在求解复杂的含有多个运动模式的方程组之前，使用改进的极几何约束进行3D点云动态场景的运动分割，并引入对象级分割进行边界调优，实现高精度的动、静态场景分离，相关技术细节描述如下：

1、基于改进的极几何约束3D点云运动分割算法

在动态场景中，相机前后两帧的特征点与环境3D点云间的几何关系如符合极几何约束条件，大概率是静态物体，如不符合则极有可能是动态物体。假设前后两帧对应点为x和x′，基础矩阵为F，则他们符合极几何关系x^′TFx＝0。

所以第二帧中的对应点理论上应该落在在极线Fx上，但实际上由于误差的存在，该对应点只能落在一个区域内，并且可以假设该区域内经过特征点

的极线簇概率服从正态分布

而经过特征点

的极线可表示为

则在极线约束下相应的特征点为静态的概率分布为：

实际计算时，会通过在极线簇中采样部分极线

来进行搜索，相应的极线属于采样极线的概率设为

以确定后一帧中对应的特征点，并且考虑到运动物体在极平面内运动的退化情形，最终得到某一特征点属于静态环境的概率为：

其中退化情形的概率为：

其中距离信息是由环境建模过程中得出的，极几何约束示意图如图2(a)、(b)所示，该退化情形主要考虑相机做纯平移运动时，对应特点在极线上的位置区间，结果取在区间范围内时该特征点为静态，反之在区间之外则为动态。

通常在一帧图像中，难以对某个特征点是否为静态过早地下结论，根据前后多帧的观测情况能够更加真实地反映实际情况，考虑帧之间的马尔科夫性，最终的判断概率如下：

针对图像上每个特征点做一次动静态判断，然后在三维空间中，判断特征点之间的相对距离d_ij＝||x_i-x_j||。若特征点间相对距离稳定，则为同一个运动物体，若彼此相对距离不稳定，则为不同的运动物体。

通过上述算法，可以完成稀疏特征点情况下的图像中动静态物体区分，以及不同运动物体彼此区分的3D点云场景初步分割。可以看到上述概率计算只适用于动态物体较少的情况，且动态物体的运动模式区分准则不够鲁棒与精确，这对后面的定位定姿及场景重构中的数据关联有一定的影响。

2、对象级精细分割与边界调优算法

由于稀疏点云是看不出形状且没有对象意义的，由此得到的分割结果存在分割边界不精确等问题。在运动分割基础上，通过建立对象级标签实现精细分割，具体流程如图3所示。

为了能够更为准确的区分动态物体，引入对象级分割，这样的分割结果不仅精确，而且容易区分不同的运动物体。假设随机变量Z_i＝[X_i，Y_i]对应的运动标签为z_i＝[x_i，y_i]，表示变量Z_i对应运动物体标签x_i和运动模式y_i，由此，基于条件随机场的联合估计能量函数表示如下：

其中，第一项表达了对象与运动模式之间的交互关系，具体表达如下：

第二项表达了对象与运动模式的领域像素的一致性，具体表达如下：

上面的关系式中，

表示变量对应某个标签的损失值，

表示两个变量之间的匹配值，具体为：

其中，若CRF为全连接拓扑图，则p(i，j)的表达公式如下：

其中，p_i表示第i个像素，I_i表示第i个像素的亮度，θ_β、θ_v、θ_p是训练数据中学习到的模型参数。通过条件随机场的优化，可以得到更为精确的、对象级别的运动场景分割数据。

二、多源数据融合的高精度自主定位技术

为了能够同时反映平面和三维环境两方面的环境信息变化，提高定位定姿精度，本专利提出一种多源数据融合的高精度自主定位技术：在2D静态环境中采用“视觉+IMU+GPS”的多源数据定位定姿算法，通过视觉传感器感知整体环境与可见目标、通过IMU惯导传感器感知自身的姿态、在室外通过GPS传感器感知自身的绝对位置，从而提高对整体战场环境的感知能力；在3D静态环境中，利用2D-3D匹配算法引入环境信息加强定位解算；在动态环境中利用动态目标与视点位置关联估计算法，将分割后的动态目标运动信息进行建模，建立合理的运动物体运动模型及与视点间的数据关联，从动态物体提取出有利于提高视点定位精度的相关信息辅助定位，相关技术细节描述如下：

1、“视觉+IMU+GPS”的多源数据定位定姿算法

在2D静态环境中，多源数据主要为视觉传感器探测的点云数据与图像信息、IMU惯导和GPS传感器采集到的环境、目标位置、姿态等信息。如图4所示，多源数据定位定姿算法中各数据源切换大体分为GPS正常、GPS异常及GPS失锁三种情况：

1)在GPS状态良好的情况下，以GPS/IMU组合导航作为定位定姿算法，该情况下视觉/IMU组合导航算法不再输出运算结果。

GPS作为最成熟、使用时间最长的卫星导航系统，具有不易发散、覆盖面积广、长时间稳定准确输出载体经纬高以及速度信息等优点，而GPS/IMU组合导航能实现两者优势互补，有效增强算法的抗干扰、动态响应快速、静态系统稳定不漂移等现象。IMU运动学方程作为组合导航算法的离散状态传递方程：

X(k)＝F(k-1)X(k-1)+G(k-1)η(k-1)

其中

为状态变量，依次为载体的位置、姿态角、速度和陀螺仪偏置、加速度计的偏置，

是运动学方程的噪声信号，G(k)是噪声信号的加权系数，如下所示：

Z(t)＝H(t)X(t)+η(t)

其中

依次为位置、姿态角、速度和IMU偏置观测值，

为量测方程Jacobian矩阵，η(t)为误差值。

如图5所示，线性连续系统的卡尔曼滤波器可以看作为线性控制系统，其输入为观测变量为Z(t)，状态反馈传递函数为H(t)，K(t)为卡尔曼增益，通过输出反馈进行运动学状态方程的校正。

GPS/IMU组合导航算法的量测方程如下所示：

Z(t)＝H(t)X(t)+η(t)

其中

依次为位置、姿态角、速度和IMU偏置观测值，

为量测方程Jacobian矩阵，η(t)为误差值。

如图5所示，线性连续系统的卡尔曼滤波器可以看作为线性控制系统，其输入为观测变量为Z(t)，状态反馈传递函数为H(t)，K(t)为卡尔曼增益，通过输出反馈进行运动学状态方程的校正。

2)在GPS状态异常但未失锁时，系统采用GPS/视觉/IMU组合导航算法作为定位定姿方式。该情况下，若系统能够采集得到GPS定位数据，通过扩展卡尔曼滤波算法对GPS、IMU数据进行融合滤波，并将此作为新图像帧的初始位姿估计值，应用于后续特征匹配、相机位姿解算以及图优化过程。

在GPS/视觉/IMU多传感器融合导航算法中，由于待优化变量繁多，GPS、IMU和图像量测数据也会随系统运行时间逐渐增加，为了维持算法实时运算能力，引入图优化、边缘化过程优化求解状态变量，从而提高优化问题的计算速度。

GPS/视觉/IMU多传感器融合位姿计算算法中，将系统待优化状态变量(相机位姿、3D点云图的世界坐标)作为图优化的顶点，将重投影误差和运动学残差作为边连接顶点，描述了顶点之间的相互约東关系。

基于图优化方法最大的优点是可以同时考虑轨迹中多帧图像和多个传感器之间的约束关系，如同时对多个重投影误差和多帧之间的运动学残差方程进行同时迭代寻优。由于图优化的顶点、边可以包含多次状态、多帧图像，因此随着图优化的不断进行，会极大的抑制系统的漂移，提高导航算法抗干扰性，使系统的解趋于连续稳定。

多传感器融合系统中构建的图并非是全连通的，如图6所示，视觉SLAM系统的图优化问题一般具有稀疏性质。对于图6的3D观测路标点l₀，其只会在特定位置x₀、x₁、x₂才能被图像观测到，此时该3D点只能与部分图像的2D特征点匹配和建立重投影误差关系，而其它的重投影误差函数对该顶点的雅克比矩阵J则为零，二阶Hessian矩阵中也大量为零，即误差函数关于观测点l₀的Jacobian矩阵为[J₀ J₁ J₂ 0 0 0]，同理误差函数关于观测点l₀的Jacobian矩阵为[0 0 J₂ J₃ J₄ 0]。图优化问题的稀疏性质大幅提高了求解速度。

随着系统运行时间的增加，图的顶点和边会不断递增，尽管Jacobian矩阵和Hessian矩阵具有稀疏性质，实际的优化速度化难以满足实际要求，尤其多传感器融合优化问题中，既有视觉的重投影误差，也有GPS/IMU估计的残差方程，待优化变量繁多。因此在优化过程引入优化变量的边缘化过程，通过边缘化方法减少运算量，提高系统的运行速度。

边缘化的过程是指在优化的过程中只对部分变量进行更新迭代过程，从而维持系统的计算量不增加。边缘化过程并不是直接删除变量，而是不更新被边缘化的变量，因此边缘化可以保留该变量对应的约束信息。随着多传感器融合导航算法的不断运行，在加入新待优化变量的同时将先前帧的变量进行边缘化，从而使得计算量维持在一定水平。

图7展示了图优化的边缘化过程，图7(a)中l_i表示图像能观测到的3D空间点，x_i表示导航定位过程中的图像帧位姿，在局部优化的过程中，将3D空间点和图像帧对应的状态变量作为顶点、误差函数作为边构造图7(a)的局部图拓扑结构。

随着载体运动至x₅的位置，此时x₀、l₀渐渐不再被后面的图像帧观测到，为了维持系统的计算量不再持续增加，可以边缘化方法处理x₀、l₀，图7(b)中首先对图像位姿x₀进行了边缘化的过程中，在边缘化过程中系统的顶点和边会发生变化。

当系统继续边缘化3D空间点时，过程如图7(c)所示。图中被边缘化的空间点l₀只与图像x_i连接，边缘化处理后对系统的Hessian矩阵不会有太大影响。

3)当处于完全遮挡或强干扰对抗环境时GPS存在失锁现象，其定位数据处于不可用状态，此时完全由视觉/IMU组合导航估计载体的运动状态。

在传统的视觉SLAM系统中新图像对应的相机位姿是由上一帧图像或者关键顿进行估计和帧间匹配得到的。如果没有对系统进行局部、全局优化，则任意一帧出现位姿漂移偏差都会影响后面的相机位姿估计，即累积误差会很大，所以需要对系统进行局部以及全局优化的过程以抑制漂移和累积误差。

在Visual/IMU融合位姿计算过程中，通过最小化残差平方和进行相机位姿的求解。优化目标泛函定义如下所示：

其中

为视觉重投影误差的残差，上标i为图像，l为3D点云，

为运动学残差方程，

分别为视觉残差和运动学残差的权值。

视觉/IMU融合导航系统需要求解

残差函数，然后通过LM算法求解该最小二乘估计问题(对残差函数线性化泰勒级数展开并计算雅克比矩阵)，视觉/IMU组合导航算法的优化过程如图8所示。

2、2D-3D匹配定位算法

由于相机(尤其是单目)对3D场景的不可完全观测性，动态物体分割在图像平面及空间3D点不能实时完全对应，从而降低相机的定位精度。因此本专利提出2D-3D匹配算法，充分利用已有相机观测到的信息，建立依赖平面和空间信息的2D-3D定位模型。

相机定位的2D-2D方法如下：

其中，E_i(R′，t′)是本质矩阵，如图9(a)、(b)所示由前后两帧图像特征点的8点法计算得出，然后通过SDV分解得到四组解，通过判断得到其中的唯一解，即相机姿态。

当含有3D环境点云数据时，采用2D-3D匹配算法，利用环境信息能够加强相机的定位解算。具体方式为将第一帧观测到的3D点重投影到第二帧上面，之后最小化投影与观测间的误差，表达公式如下：

当2D与3D分割存在不一致的情况时，需结合两种定位方法联合估计相机位置，能够在一定程度上减少2D和3D运动分割不一致带来的误差，表达如下：

如果考虑到被动态物体遮挡的静态点云，可以在目标函数上加一个权重，属于动态物体及其遮挡的点云权重接近于零，其他静态点权重接近于1。

3、动态目标与视点位置关联估计定位算法

当下大部分动态场景下的视点定位方法，都是把检测出的运动点当作噪声过滤掉，把剩余的静态环境部分用于视点的姿态定位。该方法的缺陷是没有充分利用动态物体的信息，若建立合理的运动模型及与视点间的数据关联，则可能从动态物体中提取有利于提高视点定位精度的相关信息。于是本专利提出一种关联估计的定位算法，其视点与动态物体的关联过程如图10(a)、(b)所示。

假设所有位置(包括视点及动态物体)为

测量值及先验为

视点与动态物体关联信息L为

将这些变量联合在一起的联合概率可表达为：

其中，Z为视点与动态物体的位置，

Y为测量值及先验，

E为视点与动态物体的关联信息，

其中，建立动态物体的运动模型如下：

上式中的

表示动态物体运动模型的误差项，通常假设服从正态分布。建立关联模型为：

通过以上分析，可以将以将联合概率表达成求解一个最大后验概率估计问题，该问题实际上变成了非线性最小二乘问题，表达如下：

其中，

是马氏距离，∑为协方差，p^m为联合概率，

为动态物体的运动模型，

为动态物体的关联模型；

三、虚实场景构建与一致性匹配技术

针对动态场景下的场景构建问题，本专利提出一种虚实场景构建与一致性匹配技术，针对场景中动、静态部分分别进行虚拟场景重构：针对静态部分采用点云数据拼接的场景构建算法直接进行3D点云场景构建，针对动态部分采用基于哈希体素的动态场景构建算法进行全局的动态场景构建。相关技术细节描述如下：

1、点云数据拼接的场景构建算法

对于传感器点云数据，在一定时间内单个相机采集的数据范围有限，因此本专利提出一种点云数据拼接的场景构建算法，能够将多个相机获得的局部区域多源点云数据进行拼接，并将拼接结果转化为三维网格的形式便于渲染，最后基于多源点云数据拼接进行场景构建。

点云数据拼接的关键是点云数据配准与融合，即如何将不同坐标系的点云数据转换到同一坐标系，本质是求取两个坐标系之间的旋转参数和平移矩阵，与此同时消除由拼接造成的一些明显的拼接痕迹，真正做到无缝拼接，其示意图如图11所示：

该算法在基于2D场景纹理图像特征的基础上进行空间点云的匹配：首先在点云对应的2D纹理图像上进行特征检测和提取，其次将提取的特征和匹配关系映射到三维点云数据，获取多源点云数据的特征点和匹配关系，为点云数据的融合做准备。在获得匹配点对后，为获得两视角下测得的三维数据点之间的坐标变换关系R和T，需要使得下面的目标函数最小：

可以通过SVD方法估计旋转和平移矩阵，可在一定程度上减少噪声干扰。假设匹配点对为{pi}和{qi}，其重心分别为p和q，有：

匹配点对为：

p'_i＝p_i-p

q'_i＝q_i-q

规范化后匹配点误差方程为：

将前面两组等式带入得：

要使规范化后匹配误差最小，需使得

最大，故而：

令

将H进行奇异值分解得U和V，当R＝VUT时可以获得最小的规范化匹配误差。

通过上述方法，可以把多源的点云经过坐标变换到一个统一坐标系，由于计算误差的存在，变换后的点云并不能很好重合，需要对点云再进行精确匹配。

将变换后获得的统一坐标系下匹配三维特征点对{pi}和{qi}分别投影到YOZ屏幕，它们的投影分别为pi、qi。将pi、qi与坐标坐标原点连线,设

与Y轴的夹角为α_pi，

与X轴的夹角为α_qi,则

和

之间的夹角为：

α_i＝α_qi-α_qi

所有匹配三维点对在YOZ平面投影之间的平均夹角为：

同理，匹配三维点对在XOZ、XOY平面投影之间的平均夹角为：

由于计算误差的存在，变换后的统一坐标系下匹配的三维点对在YOZ、XOY和XOZ三个投影面的投影不能完全重合，他们之间的误差角分别为α,β,γ。点集{pi}要绕X轴旋转角度α，绕Y轴旋转角度β，绕Z轴旋转角度γ，才能与点集{qi}比较接近，则旋转矩阵为：Rxyz＝RxRyRz。

三维点云的再次平移则是将同名点对分别投影到x轴，y轴，z轴，其投影到坐标轴上的平均向量之差分别为

三维点云的平移量为

通过粗匹配和精确匹配两步，能够实现已知对应关系的大场景三维点云快速匹配。之后对得到的点云数据进行进一步的处理，消除多层重叠冗余、去除噪声和色差，得到理想的点云数据。

2、基于哈希体素的动态场景构建算法

若要建立动态场景的重构地图，点云表达方式的计算量太大，对运动物体的场景构建难以做到实时更新。于是本专利提出一种基于哈希体素的动态场景构建算法，只对动态物体的运动场景实时更新，其他静态物体不更新，在物体对象表面建立体素场景，并存储成哈希表结构，方便重构场景的快速更新，如图12(a)、(b)所示。

假设随时间t变化的视觉里程计给出的相机位置为P^t，场景流为F^t，观测量表示为χ^t＝{x₀…x_t}，其中x_t＝{d_t，s_t}，d_t表示观测到的深度信息，s_t表示语义分割信息。地图中的体素用v_j表示，则该体素被占有概率分布为p(o_j|χ^t，P^t，F^t)，语义标签概率分布为p(l_j|χ^t，P^t，F^t)。若地图中每个体素被占有及标签联合的表示为y_t，j，则该地图表示的递归贝叶斯滤波更新如下

p(y_t，j|χ^t，P^t，F^t)＝ηp(x_t|y_t，j，p_t)p(t_r，j|χ^t-1，P^t，F^t)

可以看到，概率波主要分为预测及更新两部分，其中预测步骤，我们可以建立如下的概率分布模型

上面的模型是比较理想的地图预测，实际情况下还需要考虑不确定性因素，以及随后局部地图的平滑；然后，将观测量加入到地图模型中，进行地图更新

在地图有效的更新之后，还需要对地图的空间一致性进行验证，并且由此可以进行语义级别的动态物体分类，体素地图的空间一致性可以用稠密连接的条件随机场(CRF)来界定，该方法主要作用是为每个动态的体素单元贴分类标签，从而可以完成空间一致性及运动物体分类的判断，其核函数如下

其中f_j和f_k是体素中心p_j、p_k，体素平均颜色c_j、c_k的特征一致性表征，w₁、w₂是两种核函数的权重，θ_α、θ_β、θ_γ是相应的核函数的标准差。

3、场景的虚实一致性匹配算法

虚实一致性场景是指场景中的虚拟物体在视觉上需要以一种更真实的效果呈现给用户，达到虚实难辨的效果，这就需要使得计算机所产生的虚拟场景与现实世界中真实环境之间实时对准。在对准过程中，需要将虚拟场景显示在真实场景中的具体空间位置以坐标的形式表示出来，这就需要建立两个空间坐标的转换关系，以实现将由计算机生成的虚拟场景准确的叠加到真实场景的正确位置上，同时必须跟踪观察者不断变化的视点实时更新坐标系的对应关系。

本专利提出一种场景的虚实一致性匹配算法，根据是否在增强现实系统中采用了标识物，可以分为基于精确位置的虚拟跟踪注册和基于标识物的虚拟跟踪注册。如果虚拟对象在场景中已经有非常精确的空间定位，可以根据坐标系的转换关系把虚拟对象准确注册到重构场景的对应位置，此时需要考虑其与周边不同深度的实物实现交互，确保正确的遮挡关系和交互关系。这就需要场景绘制系统能够在实时估计场景深度的基础上，实现有效的遮挡处理、碰撞检测以及渲染绘制。对于没有确切位置的动态虚拟对象，则可以通过在场景中预置特征标识或者特征对象来进行，系统可以预先学习这些特征标识或对象的相应特征来识别位置并进行持续跟踪。

仿真示例：

具体实现效果验证

1)在EuRoC MAV数据集上评估算法定位精度

EuRoC MAV数据集是在微型飞行器MAV上收集的12种不同场景下的视觉惯性图像数据集，包含拍摄的视频帧图像、同步IMU测量数据以及精确的真实标定数据。本方法实验选择工厂场景下拍摄的数据集Machine Hall 01_easy_01-02，如图13。

算法在Machine Hall 01_easy_01数据集上的实验结果，如图14所示其中左侧为摄像头采集图像后的特征点提取，以及两帧图像间的特征点匹配过程，右侧为算法运行过程中定位出的相机在世界坐标系下的运行轨迹图像。

2)对场景中的静态部分进行三维点云场景构建得到精确匹配的点云数据，对场景中的动态目标进行动态物体构建，实现全局点云数据的优化，从而构建完整场景的三维重建结果，具体效果图见图15(a)、(b)，分别为整体场景与局部场景的构建。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (4)

1.一种基于SLAM技术的自主位姿测量方法，基于视觉传感器、IMU惯导传感器和GPS传感器实现，所述方法包括：

步骤1)接收待建场景的点云数据、图像信息、姿态信息和目标位置信息；

步骤2)基于改进的极几何约束3D点云运动分割算法对高动态场景进行对象级运动场景分割；

步骤3)对点云数据、图像信息、姿态信息和目标位置信息采用多源数据定位定姿算法，对静态部分的图像信息采用2D-3D匹配算法，对动态目标采用动态目标与视点位置关联估计算法，实现定位；

步骤4)对场景中的静态部分进行三维点云场景构建得到精确匹配的点云数据，对场景中的动态目标进行动态物体构建，并对定位定姿结果辅助进行全局优化，进而完成位姿测量；

所述步骤2)具体包括：

步骤2-1)对前后帧的图像进行特征点提取；

步骤2-2)根据提取的特征点与环境3D点云是否符合极几何约束条件，进行动静态物体的区分，对不同运动物体的3D点云场景进行初步分割；

步骤2-3)通过条件随机场的优化，得到对象级别的运动场景分割数据；

所述步骤2-2)具体包括：

对前后两帧的特征点与环境3D点云间的几何关系进行判断，如果符合极几何约束条件，则对前后两帧图像上每个特征点做一次动静态判断，然后在三维空间中，计算特征点x_i和x_j之间的相对距离d_ij＝‖x_i-x_j‖，判断相对距离d_ij是否稳定，如果稳定，为同一个运动物体，否则，为不同的运动物体；

所述步骤3)具体包括：

步骤3-1)在2D环境中，对点云数据、图像信息、姿态信息和目标位置信息采用“视觉+IMU+GPS”多源数据定位定姿算法；

步骤3-2)在3D静态环境中，对图像信息采用2D-3D匹配算法，通过引入环境信息加强定位解算；

步骤3-3)在3D动态环境中，采用动态目标与视点位置关联估计算法，将分割后的动态目标运动信息进行建模，建立运动物体运动模型及与视点间的数据关联，从动态物体提取出有利于提高视点定位精度的相关信息，实现定位；

所述步骤3-2)具体为：

采用2D-2D方法对相机定位：

其中，x_ij为特征点，E_i(R′,t′)是本质矩阵；

通过前后两帧图像特征点的8点法计算得出本质矩阵，通过SDV分解得到四组解，通过判断得到其中的唯一解，从而得到相机姿态；

对于含有3D环境点云数据，将第一帧观测到的3D点重投影到第二帧上面，得到最小误差的相机姿态

为：

其中，R为第一帧图像到相机坐标系下的旋转矩阵，R′为第二帧图像到相机坐标系下的旋转矩阵，t_i为第一帧图像到相机坐标系下的平移变量，t′为第二帧图像到相机坐标系下的平移变量，P为重投影矩阵，x_ij为第二帧特征点，

为第一帧特征点；i表示第一帧图像的横坐标，j表示第一帧图像的纵坐标，m表示第一帧图像的横坐标像素点，n表示第一帧图像的纵坐标像素点；

当2D与3D分割不一致时，结合两种定位方法联合估计相机位置，表达公式如下：

条件满足：

对于被动态物体遮挡的静态点云，在目标函数上增加权重，属于动态物体及其遮挡的点云权重设置为零，其他静态点权重设置为1。

2.根据权利要求1所述的基于SLAM技术的自主位姿测量方法，其特征在于，所述步骤3-1)具体为：

当GPS状态正常时，以GPS和IMU组合导航算法进行定位定姿计算；

在GPS状态异常但未失锁时，以GPS、视觉和IMU组合导航算法进行定位定姿计算，引入优化变量的边缘化处理；

当GPS为失锁状态，以视觉和IMU组合导航算法进行定位定姿计算。

3.根据权利要求1所述的基于SLAM技术的自主位姿测量方法，其特征在于，所述对场景中的静态部分进行三维点云场景构建得到精确匹配的点云数据；具体包括：

在点云对应的2D纹理图像上进行特征检测和提取；

将提取的特征和匹配关系映射到三维点云数据，得到多源点云数据的特征点和匹配关系；

通过SVD方法估计得到旋转矩阵和平移矩阵，进而将多源的点云数据坐标变换到一个统一坐标系，实现已知对应关系的大场景三维点云快速匹配；

进行进一步的处理，消除多层重叠冗余、去除噪声和色差，得到精确匹配的点云数据。

4.根据权利要求1所述的基于SLAM技术的自主位姿测量方法，其特征在于，所述对场景中的动态目标进行动态物体构建，并对定位定姿结果辅助进行全局优化，进而完成位姿测量；具体包括：

根据场景构建表示递归贝叶斯滤波p(y_t,j|χ^t,P^t,F^t)为：

p(y_t，j|χ^t，p^t，F^t)＝ηp(x_t|y_t，j，p_t)p(y_t，j|χ^t-l，p^t，F^t)

其中，t为时间，P^t为相机位置，F^t为场景流，χ^t＝{x₀…x_t}为观测量，x_t＝{d_t,s_t}，d_t表示观测到的深度信息，s_t表示语义分割信息，y_t,j为地图中每个体素被占有标签的联合表示；

将观测量χ^t加入到场景构建模型中，建立预测的概率分布模型为：

其中，

为观测到的体素被占有标签的概率，p(y_t,j|χ^t-1,F^t)为场景中被观测到的体素被占有标签的概率；

为每个动态的体素单元贴分类标签，根据核函数进行空间一致性及运动物体分类的判断，进而完成位姿测量；其中，核函数k(f_j,f_k)为：

其中，f_j和f_k分别是体素中心p_j、p_k及体素平均颜色c_j、c_k的特征一致性表征，w₁、w₂是两种核函数的权重，σ_α、σ_β、σ_γ是相应的核函数的标准差。

Images