CN112304307A - 一种基于多传感器融合的定位方法、装置和存储介质 - Google Patents

Abstract

本申请公开了一种基于多传感器融合的定位方法、装置和存储介质，该方法包括：获取图像数据、IMU观测数据以及激光点云数据；联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子构建约束方程，以获取机器人的位姿，其中，先验约束因子为激光点云数据或IMU观测数据对应的误差约束因子，无结构化视觉约束因子为图像数据对应的误差约束因子，IMU预积分约束因子为IMU观测数据对应的误差约束因子，激光匹配约束因子为激光点云数据对应的误差约束因子；利用回环检测对机器人的位姿进行修正，得到修正位姿；对修正位姿进行全局位姿优化，以得到全局一致的位姿信息。通过上述方式，本申请能够提高定位精度与鲁棒性。

Description

一种基于多传感器融合的定位方法、装置和存储介质

技术领域

本申请涉及机器人技术领域，具体涉及一种基于多传感器融合的定位方法、装置和存储介质。

背景技术

移动机器人在进行任务操作时，比如在仓库中进行物体搬运，如果仓库环境发生变化，移动机器人需要在变换的环境中精确地知道自己在当前环境的位置，从而能够准确地执行任务；但是目前的定位方案中有些方案存在双目相机的测量精度受光照影响严重、不能从观测量层面上直接进行耦合以及不能在定位场景变化的环境下通用的问题；有些方案需采用滤波算法进行融合，滤波算法相当于在求解最优值过程中只进行一次迭代，精度不如非线性优化方法，且无法对累计的误差进行纠正。

发明内容

本申请提供一种基于多传感器融合的定位方法、装置和存储介质，能够提高定位精度与鲁棒性。

为解决上述技术问题，本申请采用的技术方案是提供一种基于多传感器融合的定位方法，该方法包括：获取图像数据、IMU观测数据以及激光点云数据；联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子构建约束方程，以获取机器人的位姿，其中，先验约束因子为激光点云数据或IMU观测数据对应的误差约束因子，无结构化视觉约束因子为图像数据对应的误差约束因子，IMU预积分约束因子为IMU观测数据对应的误差约束因子，激光匹配约束因子为激光点云数据对应的误差约束因子；利用回环检测对机器人的位姿进行修正，得到修正位姿；对修正位姿进行全局位姿优化，以得到全局一致的位姿信息。

为解决上述技术问题，本申请采用的另一技术方案是提供一种基于多传感器融合的定位装置，该定位装置包括互相连接的存储器和处理器，其中，存储器用于存储计算机程序，计算机程序在被处理器执行时，用于实现上述的基于多传感器融合的定位方法。

为解决上述技术问题，本申请采用的另一技术方案是提供一种计算机可读存储介质，该计算机可读存储介质用于存储计算机程序，计算机程序在被处理器执行时，用于实现上述的基于多传感器融合的定位方法。

通过上述方案，本申请的有益效果是：本申请先获取利用双目相机获取的图像数据、利用IMU获取的IMU观测数据以及利用激光雷达传感器获取的激光点云数据，然后基于获取的数据构建先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子，通过联合这些约束因子来构建约束方程，然后利用该约束方程来生成机器人的位姿，并通过回环检测来进一步修正生成的位姿，最后通过全局位姿图优化来得到全局一致的位姿信息；本申请的方案相比于传统的视觉惯性***，融合了激光点云信息，可通过紧耦合的方式将IMU预积分约束因子、无结构化视觉约束因子和激光匹配约束因子联合在一起，能够提高***的定位精度；相比于传统的3D激光和惯导***，加入了利用双目相机获取的视觉图像信息，能够在激光退化的应用环境中，提高定位的鲁棒性。

附图说明

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

图1是本申请提供的基于多传感器融合的定位方法一实施例的流程示意图；

图2是本申请提供的基于多传感器融合的定位方法另一实施例的流程示意图；

图3是图2所示的实施例中步骤202的流程示意图；

图4是图2所示的实施例中进行外点剔除的示意图；

图5是图2所示的实施例中步骤204的流程示意图；

图6是图2所示的实施例中IMU预积分约束因子图；

图7是图2所示的实施例中无结构化视觉约束因子图；

图8(a)是图2所示的实施例中先验约束因子图；

图8(b)是边缘化最老激光帧后生成的先验约束因子图；

图9是图2所示的实施例中联合先验约束、IMU预积分约束、无结构化视觉约束和激光匹配约束的因子图；

图10是图2所示的实施例中多传感器信息融合因子图；

图11是图2所示的实施例中全局位姿联合优化对应的因子图；

图12是本申请提供的基于多传感器融合的定位装置一实施例的结构示意图；

图13是本申请提供的计算机可读存储介质一实施例的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性的劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

请参阅图1，图1是本申请提供的基于多传感器融合的定位方法一实施例的流程示意图，该方法包括：

步骤11：获取图像数据、IMU观测数据以及激光点云数据。

图像数据为利用双目相机采集到的数据，双目相机包括左目相机与右目相机，可通过同步采集左右目相机的图像，计算图像间视差；然后根据左右目相机之间的基线距离，计算出像素点对应的三维地标点与相机的距离，视差与距离成反比，视差越小，距离越大。

IMU观测数据为利用惯性测量单元(IMU，Inertial Measurement Unit)采集到的数据；具体地，利用IMU可直接得到在IMU坐标(机器人本体坐标系)下的角速度和线加速度，通过积分能够估计前后时刻的位姿变化量，然后结合前一时刻的位姿来估计出当前时刻机器人的位姿。

激光点云数据为利用3D多线激光雷达获取的数据，3D多线激光雷达为获取三维空间信息的传感器，一般分为16线、32线和64线，能够提供周围环境的激光点云数据信息，测距精度比双目视觉的精度高。

步骤12：联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子构建约束方程，以获取机器人的位姿。

先验约束因子为激光点云数据或IMU观测数据对应的误差约束因子，无结构化视觉约束因子为图像数据对应的误差约束因子，IMU预积分约束因子为IMU观测数据对应的误差约束因子，激光匹配约束因子为激光点云数据对应的误差约束因子。

在构建出先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子后，可通过紧耦合的方式将先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子联合在一起，构建约束方程，然后利用该约束方程来计算机器人的位姿，该机器人的位姿包括位置信息与偏航角。

步骤13：利用回环检测对机器人的位姿进行修正，得到修正位姿。

在定位的过程中，在机器人的位置和偏航角四个维度上可能存在累计误差，为了减小累计误差，可通过回环检测进行处理，以对获取到的位姿进行修正，得到修正后的位姿。

步骤14：对修正位姿进行全局位姿优化，以得到全局一致的位姿信息。

在进行回环检测之后，为了能够得到全局一致的位姿，可利用优化方法对回环检测输出的结果进行全局位姿图优化，从而得到机器人最终的位姿信息，实现实时定位。

本实施例结合IMU、双目相机以及3D激光雷达传感器进行移动机器人的高精度定位，可通过联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子来构建约束方程，从而得到机器人的位姿，并通过回环检测来进一步修正生成的位姿，最后再进行全局位姿优化来得到全局一致的位姿信息；本实施例的方案相比于传统的视觉惯性***，融合了3D激光点云信息，可通过紧耦合的方式将多种约束因子联合在一起，能够提高***的定位精度；相比于传统的3D激光和惯导***，加入了利用双目相机获取的视觉信息，能够在激光退化的环境中，提高算法的鲁棒性。

请参阅图2，图2是本申请提供的基于多传感器融合的定位方法另一实施例的流程示意图，该方法包括：

步骤201：获取图像数据、IMU观测数据以及激光点云数据。

图像数据包括多个图像帧，每个图像帧包括利用左目相机采集到的左目图像帧以及利用右目相机采集到的右目图像帧。

步骤202：对左目图像帧进行特征提取，生成左目特征点，并利用左目特征点得到同一时刻右目图像帧中的右目特征点；对左目特征点与对应的右目特征点进行处理，以剔除左目图像帧中的外点与右目图像帧中的外点。

可采用如图3所示的步骤进行处理，以得到图像中的特征点，具体包括：

步骤2021：利用左目特征点进行左右目的立体匹配，得到右目特征点，将右目特征点作为光流跟踪的初始值。

对左右目相机采集到的图像构建图像金子塔以用于光流跟踪，然后在左目图像帧上提取特征点，进行左右目的立体匹配，即先通过左目相机下的像素坐标和左目相机的内参转换到左目相机下的相机归一化坐标系下，并去除畸变的影响，然后通过左目相机和右目相机的外参，求出对应的右目相机下的归一化坐标，并根据右目相机的内参和畸变系数，求出右目相机下的像素坐标，将该像素坐标作为右目相机LKT(Lucas Kanade Tomasi)光流跟踪的初始值。

步骤2022：进行光流跟踪得到左目跟踪特征点与右目跟踪特征点，对左目图像帧中的左目特征点和右目图像帧中的右目特征点进行处理，以筛除左目图像帧中的外点与右目图像帧中的外点。

利用初始值进行LKT光流跟踪，得到左目图像帧中跟踪到的特征点(即左目跟踪特征点)与右目图像帧中跟踪到的特征点(即右目跟踪特征点)，然后进行立体匹配，以对左目图像帧与右目图像帧进行外点的第一次剔除。

步骤2023：分别求取左目跟踪特征点与右目跟踪特征点在对应相机坐标系下的归一化坐标，得到左目坐标和右目坐标。

分别对左右目相机上能跟踪到的特征点求取其在对应相机坐标系下的归一化坐标，即左目坐标与右目坐标。

步骤2024：利用左目相机的外参和右目相机的外参构建本质矩阵，并利用对极几何约束进行外点剔除，得到剩余特征点。

可利用左右目相机的外参来构建本质矩阵E，利用对极几何约束来进行外点剔除，如下所示：

x₀Ex₁＝0 (1)

其中，E＝t^R，t是左目相机相对于右目相机的平移，其对应的反三角对称矩阵如下所示：

其中，R为左目相机相对于右目相机的旋转，x₀为左目相机下的归一化坐标，x₁为左目相机上提取的特征点通过LKT光流跟踪在右目相机下的归一化坐标。

可以理解地，还可依据x₀Ex₁是否大于一定阈值来剔除外点，即判断每个特征点对应的左目坐标、本质矩阵以及右目坐标的乘积是否大于预设值，该预设值可以为根据经验设置的值；若该特征点对应的左目坐标、本质矩阵以及右目坐标的乘积大于预设值，则该特征点为外点；具体地，如果x₀Ex₁等于0，说明x₀和x₁完全匹配，没有任何误差；如果x₀Ex₁>预设值，说明x₀与x₁的匹配程度较低。

步骤2025：将左目图像帧与右目图像帧划分为第一预设数量的网格，将剩余特征点划分到对应的网格中。

将左目相机采集到的图像帧划分成一定数量的网格，即将左目图像帧拆分成多个小块；然后将剔除外点后的特征点分布在每个网格中，并可根据提取特征点的阈值限制每个网格中特征点的数量，保证每个网格中的特征点的数量小于第二预设数量；这样能够限制每帧图像上特征点的数量和分布，避免提取的特征点集中在一块区域，且能够提高后期特征匹配的速度和精度。例如，左目图像帧的大小为600*800，可将该其拆分成40个大小均匀的网格，在每个网格上做特征提取，保证特征点在每个网格分布均匀，不会使得特征点都集中在一起，防止出现因特征点集中在一起导致的相对位置关系不准确。

步骤203：对前后两帧IMU观测数据进行特征跟踪，以剔除前一图像帧中的外点。

经过上述步骤进行初步的外点剔除后，当来到一帧新的图像帧数据时，可进行前后两帧的特征跟踪。

在一具体的实施例中，可利用前后两帧IMU观测数据，得到前一图像帧相对于当前图像帧的相对姿态，然后利用前一图像帧中的像素坐标和相对姿态，预测当前图像帧的像素坐标，将当前图像帧的像素坐标作为前后帧LKT光流跟踪的初值，然后进行光流跟踪，得到当前图像帧中的左目跟踪特征点和右目跟踪特征点。

依据跟踪上的特征点，对前一帧左右目图像帧上的特征点都进行外点剔除，具体地，对左目跟踪特征点和右目跟踪特征点进行立体匹配，根据立体匹配的结果对前一帧左右目图像帧上的特征点进行第二次外点剔除，以进一步筛除前一图像帧中的外点。

然后分别对前后两帧图像中的左目图像帧和右目图像帧进行处理，以筛除前一图像帧中的外点；具体地，通过2点随机抽样一致性算法(RANSAC，Random Sample Consensus)进行第三次外点剔除，以保证匹配上的特征点足够准确，三次外点剔除的示意图可如图4所示，前一图像帧为k-1时刻对应的图像帧，当前图像帧为k时刻对应的图像帧。

将经过上面三次外点剔除后且能跟踪上的特征点分布到当前图像帧对应的各个网格中。

可以理解地，在定位过程中，当视觉特征点过少导致视觉跟踪失效时，可以根据前一个激光采集时刻的位置或者最近的能正常跟踪的图像采集时刻的位置和IMU观测数据递推出当前时刻相机位姿，并在该图像上重新提取特征点，进行特征的重新跟踪和约束的建立，即在当前图像帧中的特征点的数量小于第三预设数量时，对当前图像帧进行特征提取，得到新的左目特征点，该第三预设数量小于第二预设数量；对新的左目特征点进行立体匹配，得到新的右目特征点；对新的左目特征点与新的右目特征点进行立体匹配，以筛除外点，得到新增的左目特征点与新增的右目特征点；将新增的左目特征点添加到左目图像帧对应的网格中，将新增的右目特征点添加到右目图像帧对应的网格中，直到每个网格中特征点的数量不大于第二预设数量为止，当前图像帧中的特征点可用于下一次的特征跟踪。

步骤204：对激光点云数据进行去畸变去除处理，提取去畸变后的激光点云数据中的线特征点或面特征点。

3D激光的频率较低，一般为10Hz，当激光运动时，采集的激光点云数据可能产生畸变，不适用于直接进行点云匹配以进行位姿估计，所以可对激光点云数据进行运动畸变去除。对激光点云数据进行去畸变去除处理与进行特征点提取可采用图5所示的方法，具体包括以下步骤：

步骤2041：利用IMU观测数据预测第k帧激光点云数据在采集结束时刻的位姿。

假设第k帧激光点云数据刚采集的时刻为t_k-1，扫描结束的时刻记为t_k，利用IMU观测数据预测t_k时刻的位姿，记作T_k。

步骤2042：利用激光点云数据在采集结束时刻的位姿、第k帧激光点云数据的采集开始时刻、第k帧激光点云数据的采集结束时刻以及当前时刻，计算出当前时刻激光点云数据对应的位姿。

假设一帧激光数据扫描过程中激光点云数据对应的时刻(即当前时刻)为t，其对应的位姿应为T_t，利用该位姿将该帧点云数据投影到k时刻，如下所示：

通过上述公式可实现对激光点云数据的去畸变处理。

步骤2043：计算去畸变后的激光点云数据中每个激光点与距离激光点预设距离的激光点之间的曲率，并将曲率小于预设曲率的激光点作为面特征点。

对激光点云数据进行运动畸变去除后，可利用激光点云数据和距离该激光点云数据一定范围内的激光点云数据进行曲率计算，选择曲率小于一定阈值(即预设曲率)的点，将其作为面特征点，曲率的计算公式如下所示：

其中，c为曲率，S是计算曲率用的激光点云数据集合，L为激光坐标系，X为激光点云数据，k为激光点云数据的序号，i和j为第k个激光点云数据中点云的序号。

步骤205：利用线特征点或面特征点构建局部地图，并将当前帧激光点云数据与局部地图进行匹配，以估计激光点云数据在采集时刻的位姿。

在估计机器人位姿时，在获取到激光点云数据对应的线特征或面特征后，可在滑窗的基础上，利用获取到的特征点去构建局部地图，然后将当前帧激光点云数据与建立的局部地图进行匹配，从而估计出该激光点云数据在采集时刻的位姿，以提高激光匹配的精度，同时能够减少整体的计算耗时。

步骤206：利用中值积分法对IMU观测数据与当前时刻的前一时刻的位姿进行处理，以预估当前时刻的位姿，将当前时刻的位姿作为位姿初值。

利用中值积分法根据IMU观测数据和前一时刻的位姿，可预估出当前时刻的位姿，将其作为初值，然后构建对应位姿的约束因子，即执行步骤207。

步骤207：联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子构建约束方程；利用位姿初值与约束方程，得到机器人的位姿。

各个约束因子的建立过程如下：

a)IMU预积分约束因子

IMU的运动学模型如下所示：

其中，p_wbt为t时刻机器人相对于世界坐标系下的位姿，v_wbt为t时刻机器人相对于世界坐标系下的速度，q_wbt为t时刻机器人相对于世界坐标系下的角度，可用四元数表示；a_wbt为t时刻机器人相对于世界坐标系下的加速度；wbt为t时刻机器人相对于世界坐标系下的角速度在机器人坐标系下的表示；

为四元数之间的乘法。

IMU的观测模型如下所示：

其中，

为在机器人坐标系下IMU的角速度观测值，

为在机器人坐标系下IMU的加速度观测值，b_g和n_g分别为IMU的陀螺仪零偏和观测噪声，b_a和n_a分别为IMU的加速度计零偏和观测噪声，g^w为重力加速度，其为负值。

根据运动模型、观测模型以及前一时刻k-1的状态，可以得到k时刻位移、速度、旋转和偏移量的积分公式，如下所示：

其中，p^w _bk为k时刻机器人相对世界坐标系下的位移，v^w _bk为k时刻机器人相对于世界坐标系下的速度，q^w _bk为k时刻机器人相对于世界坐标系下的旋转，可用四元数表示；b^a _bk与b^g _bk为k时刻加速度计和陀螺仪的零偏，α^bk _bk+1为IMU在k时刻和k+1时刻计算的位移预积分量，β^bk _bk+1为IMU在k时刻和k+1时刻计算的速度预积分量，q^bk _bk+1为IMU在k时刻和k+1时刻计算的角度预计分量。

由公式(2)-(4)可得IMU的预积分约束因子如下所示：

其中，r_p、r_q、r_v、r_ba和r_bg分别对应位移、速度、旋转、加速度计零偏和陀螺仪零偏的残差，α^bk _bk+1、β^bk _bk+1和q^bk _bk+1是通过IMU观测量直接求出的，待求的变量为：p^w _bk、v^w _bk、q^w _bk、p^w _bk+1、v^w _bk+1、q^w _bk+1、b^a _bk+1、b^a _bk、b^g _bk+1和b^g _bk，Δt为k时刻和k+1时刻之间的时间差。

IMU预积分约束对应的因子图如图6所示，平移和旋转对应因子图中的圆圈61，速度、加速度计和陀螺仪零偏对应IMU因子图的圆圈62，圆圈61表示相机时刻或者激光时刻对应的机器人位姿，圆圈62表示相机时刻或者机器人时刻对应的机器人速度、陀螺仪与加速度计的零偏，方框63为IMU预积分约束因子。

b)无结构化视觉约束因子：

在视觉前端中，可对特征点进行跟踪并在当前帧中添加新的特征点，在联合优化中，如果将相机对应的3D路标点也作为优化变量，则将非常耗时，本实施例使用的是无结构化视觉约束因子；如果跟踪的特征点不能在当前帧中观测到，而被观测到的共视帧的数量大于3，且观测到该特征点的首尾两帧的视角差足够大，可利用共视帧的相对位姿关系恢复出该特征点的3D坐标，具体恢复3D坐标的步骤为：

1)根据共视帧的首尾两帧图像的相对位姿和跟踪上的特征点的像素坐标进行三角化，恢复特征点在首帧下3D坐标初值，可采用如下公式进行计算：

其中，u和v是在相机中像素平面上的像素坐标，x、y和z是在首帧相机坐标系下的坐标，z₁和z₂分别对应的3D点是在图像帧c1和对应的图像帧c2下的深度值，R_c2c1和t_c2c1分别是图像帧c1相对图像帧c2的旋转量和平移量。

2)根据共视帧中首帧相对于共视帧中其他帧的相对位姿关系，利用首帧坐标系下的3D点到其他帧的重投影误差优化该3D点，透视投影方程如下所示：

其中，α和β是3D路标点在首帧图像坐标系下的归一化坐标，ρ是该路标点在首帧图像坐标系下的逆深度值，R_cic1和t_cic1分别是首帧图像相对于共视帧中其他帧的旋转量和平移量。

重投影误差如下所示：

其中，r为重投影误差，h(0)、h(1)和h(2)是由公式(8)求出的在其他共视帧上的重投影坐标，z(0)和z(1)是该3D点在共视帧中跟踪上的特征点的像素坐标，对公式(11)利用LM(Levenberg-Marquardt)方法进行优化求解，可以得到该特征点准确的3D坐标。

记观测到特征点p的共视帧集合为{S}_p，特征点p对应的3D坐标为X_p，则在k时刻相机的重投影误差为：

其中，z是特征点p在当前图像中的观测值，h是透视投影方程，T_k是相机位姿。

对上述方程进行近似展开，并将共视帧的残差进行叠加可得到视觉约束因子如下所示：

||r_c||_Σc＝||H_x·δT_k+H_p·δx_p+n_p|| (13)

其中，H_x为误差项关于相机位姿的雅克比矩阵，H_p为误差项关于3D点位置的雅克比矩阵，n_p为特征点的观测误差。

为了提高计算效率，在优化的状态变量中不优化3D点坐标，将上述方程往H_p的左零空间进行投影，记H_p的左零空间矩阵为V_f，则对应的无结构特征约束方程为：

||r_c||_Σc＝||V_fH_x·δT_k+V_fn_p|| (14)

通过上式将相机与3D点坐标之间的约束转换到相机与相机之间的约束。上述操作对应的因子图如图7所示，方框71为无结构化视觉约束因子，圆圈62表示相机位姿，即待求的优化变量，方框63表示IMU预积分约束因子。

c)先验约束因子

为了减少计算量，提高运算效率，本实施例采用基于滑窗优化的方法，每次优化滑窗内固定数量的激光数据以及相机数据对应的位姿、速度和零偏。当有新的激光数据或者相机数据加入到滑窗内，此时就移除滑窗内最老的激光，而将与最老激光数据帧关联的IMU预积分约束转移到滑窗内和激光帧有约束信息的其他激光数据帧或者相机帧；若移除的最老帧是相机帧，则将与最老相机帧关联的IMU预积分约束转移到与其有关联的激光帧和相机帧上，并将与其相关联的视觉约束也边缘化到滑窗内其他共视帧上，作为下一次联合优化的先验信息。

当边缘化掉最老激光数据帧时，可将局部地图的坐标系重新投影到滑窗内首帧激光坐标系下，并将局部地图中最老激光数据帧所关联的激光点云删除，以减小对点云内存空间的占用。

去除最老激光帧之前的因子图如图8(a)所示，边缘化掉最老激光帧产生的先验约束对应的因子图如图8(b)所示，圆圈81为激光时刻机器人的位姿，方框82为激光匹配约束因子，方框83为先验约束因子。

d)激光匹配约束因子

可对去除畸变后的激光点云数据进行面特征的提取；假设当前滑窗内的激光数据帧的下标为{p，p+1，…，j}，将滑窗外已经经过联合优化的m帧数据(比如m＝20)和滑窗内已经经过前期联合优化的{p，p+1，…，j-1}帧数据中的面特征构建局部地图ML，将面特征点保存在3D-KDTree(k-dimensional Tree，k维树)中，便于后期特征点的最近邻搜索，此处将所有特征都转换到Lp坐标下，转换公式如下所示：

其中，Lp为下标为p的激光坐标系，F_Lp为首帧。

对于滑窗内激光帧中的特征点F_Li，i属于{p，p+1，…，j-1，j}，在局部地图ML中，找到离其最近的非共线的K个特征点，利用这K个特征点拟合一个平面，记该平面的法向量和截距为[n，d]，则对于F_Li中的一个特征点，其构建的残差方程如公式(16)所示，能够求出滑窗内所有帧相对于首帧F_Lp的相对位姿，从而可以得到相邻两帧之间的相对约束残差方程如公式(17)所示：

联合上述四个约束因子，可建立约束方程，如下所示：

其中，r_p和Σ_p是滑窗内激光帧或者相机帧进行边缘操作后得到的先验信息构成的残差和对应的协方差，其对应的因子图如图9所示，对公式(18)进行非线性优化求解，即可求出基于滑窗优化的多传感器信息融合的位姿。

步骤208：构建视觉词袋库，利用视觉词袋库对当前帧与关键帧进行处理，得到与对当前帧对应的回环帧。

对于关键帧的相机数据，在定位过程中提取特征点、计算描述子的基础上，进一步提取新的特征点并计算对应的描述子，将该关键帧的特征点和描述子保存在关键帧数据库中，而不需要保存原始的图像数据，从而缩小内存占用。

在定位的过程中，将当前帧和关键帧数据库中的相机帧通过DBOW2(Bags ofbinary words for fast place recognition in image sequence，用于图像序列快速位置识别的词袋)方法找到对应的回环帧。

步骤209：通过当前帧中的特征点与回环帧中的特征点建立数据关联；利用对极几何约束对当前帧中的特征点与回环帧中的特征点进行外点剔除。

通过当前帧和回环帧上的特征点建立数据关联，然后通过对极几何约束来进行外点剔除。

步骤210：利用当前帧和闭环帧构建视觉回环约束因子，联合优化滑窗内的先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及视觉回环约束因子，得到修正位姿。

利用当前帧和闭环帧构建视觉回环约束因子，通过联合优化滑窗内的先验约束因子、IMU预积分约束因子、无结构特征视觉约束因子和视觉回环约束因子，得到联合优化后的机器人位姿，使得定位精度更高，轨迹更加平滑。

在进行上述联合优化后，分别找到与当前图像帧的时刻和对应的闭环帧的时刻最近的激光点云数据；利用当前图像帧的位姿、当前图像帧与最近的激光点云数据之间的IM观测数据，预测最近的激光点云数据的位姿；然后通过隐式移动最小二乘法(IMLS-ICP，Implicit Moving Least Squares)对两帧最近的激光点云数据进行匹配，得到当前帧和闭环帧的相对位姿。

进一步地，IMLS-ICP匹配过程如下所示：

A)对于参考帧激光点云数据中的点云，分别计算每个点云的曲率和法向量，选择结构性比较好的激光点云数据，具体地，曲率较小，法向量一致性好，则结构性比较好。

B)在参考帧中找到离当前帧的点云x距离比较近的集合P_k，对于点云集合P_k中的任意一个点p_i，其对应的法向量记为n_i，对于当前帧的点云x来说，其离参考帧中隐藏的曲面的高度为I^PK(x)，I^PK(x)的计算公式如下所示：

其中，权重W_i(x)的计算公式如下所示：

其中，h为指定阈值，表示当前帧的点离参考帧曲面的最远距离。

C)设P_k中离点x_i最近点的法向量为n_i，则点x_i在曲面上的投影y_i为：

D)通过匹配点x_i和投影y_i，求解出当前帧和参考帧之间的相对位姿：

∑((Rx_i+t-y_i)·n_i)² (22)

其中，R为旋转矩阵，t为平移向量。

通过上述匹配能够得到更加准确的当前帧和闭环帧的相对位姿，其对应的因子图如图10所示，方框101为闭环检测约束。

本实施例所采用的回环检测方案，在基于传统的DBOW2找到当前帧的回环帧以及求出当前帧相对于回环帧的基础上，通过激光点云数据的匹配和IMU观测数据，提高当前帧和回环帧的相对位姿估计，在闭环检测联合优化方程中，加入离当前帧和回环帧最近的激光时刻的相对位姿，从而提高闭环检测的精度，能进一步缩小***的累计误差，提高***的定位精度和平滑度。

步骤211：对修正位姿进行全局位姿优化，以得到全局一致的位姿信息。

a)对于滑窗内的相机帧来说，可将相机关键帧和它相邻的历史四个相机关键帧以及相邻的历史两个激光帧建立相对约束关系；对于激光帧来说，可将激光帧和它相邻的历史四个相机关键帧和两个相邻的历史激光帧构建相对约束关系，对应的相对观测信息如下所示：

上式等号右边所有的变量都是已知的，其是通过滑窗联合优化得到的位姿；等号左边的值在后期全局位姿图优化中作为观测值；当该相机关键帧或者激光帧从滑窗内边缘出来时，将其所关联的序列边和对应的节点加入到全局位姿图(GPG，Global Pose Graph)中。

b)当边缘化掉一帧激光数据或者相机关键帧时，如果该激光帧或者相机帧有对应的闭环帧，则在该激光帧或者相机关键帧和对应的闭环帧之间添加相对位姿约束信息，作为闭环边加入到GPG中，其相对约束关系表达如公式(23)所示。

GPG中有相对约束的两个节点间的残差方程如下所示：

上式中，待求的量是

ψ_j和ψ_i，

是j时刻机器人位姿的平移量，

为i时刻机器人位姿的平移量，ψ_j和ψ_i分别为j时刻和i时刻对应机器人位姿的偏航角。

位姿图中所有的序列边和回环检测边构成的代价函数如公式(25)所示，为了减少错误回环的影响，对闭环残差项添加了核函数，如下所示：

其中，S表示所有相邻帧序列边集合，lo表示所有具有回环边集合，r_ij是两帧位姿对应的相对约束残差项，对应公式(24)，整个全局位姿图优化对应的因子图如图11所示。

本实施例融合IMU、双目相机和3D激光数据进行移动机器人的位姿估计，首先基于IMU观测数据预估机器人在图像采集或者3D激光数据采集时刻的位姿；同时在3D激光数据的采集过程中，利用IMU观测数据进行运动去畸变处理，获取去畸变的激光数据；然后对于3D激光数据，提取线特征或者面特征来构建局部地图，将当前帧和局部地图匹配估计出3D激光采集时刻的位姿；而在双目相机数据采集过程中，通过左目相机上的特征点提取，前后图像帧的LKT特征点跟踪和左右目相机LKT特征点跟踪，以及通过左右目的对极几何约束和前后帧的2点RANSAC方法进行外点剔除，将跟踪上的特征点进行三角化，并将相机与3D点的约束转换成共视相机之间的无结构的视觉约束，使得相机帧关联的路标点不需要参与到联合优化中，优化变量数量大大减少，提高优化效率；然后基于滑动窗口方法，联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子和激光匹配约束因子构建联合优化方程，通过紧耦合方式将IMU、相机和3D激光雷达的约束项联系在一起，使得求解出来的机器人的位姿精度更加准确，能够提高算法的定位精度；接着进行回环检测，先通过视觉DBOW2方法进行候选帧检测和剔除，进行回环帧之间相对位姿的初步计算，然后通过对应帧的激光点云数据匹配进一步提高对应的回环检测精度，消除***的累计误差，进一步提高算法的定位精度和轨迹平滑度；最后在所有的节点间添加相邻序列边和回环边约束，加入到全局位姿图中，并将最老闭环边固定，进行全局位姿联合优化得到全局一致的位姿。

请参阅图12，图12是本申请提供的基于多传感器融合的定位装置一实施例的结构示意图，基于多传感器融合的定位装置120包括互相连接的存储器121和处理器122，存储器121用于存储计算机程序，计算机程序在被处理器122执行时，用于实现上述实施例中的基于多传感器融合的定位方法。

本实施例所提供的装置，相比于传统的视觉惯性***，融合了3D激光信息，通过紧耦合的方式将IMU预积分约束因子、无结构化视觉约束因子和激光匹配约束因子联合在一起，提高***的定位精度；相比于传统的3D激光和惯导***，加入视觉信息，能够在激光退化的环境中，提高算法的鲁棒性；而且在定位过程中，通过统计当前相机帧或者当前激光帧中特征点的数量，自适应修改相机约束和激光匹配约束的残差，从而改变相机约束或者激光匹配约束对定位结果的影响；此外，在回环检测环节，在基于词袋数模型找到闭环帧以及求解当前帧与闭环帧的相对位姿的基础上，通过3D激光的IMLS-ICP匹配算法和对应激光帧和相机帧之间的IMU信息，提高当前相机帧和闭环帧的相对位姿的计算精度；同时，对于能找到闭环帧的相机帧，分别找到离其最近的激光帧进行IMLS-ICP匹配，得到对应激光帧的相对约束关系，加入闭环检测约束中，从而进一步减少累计误差，提高算法的定位精度和轨迹平滑度。

请参阅图13，图13是本申请提供的计算机可读存储介质一实施例的结构示意图，计算机可读存储介质130用于存储计算机程序131，计算机程序131在被处理器执行时，用于实现上述实施例中的基于多传感器融合的定位方法。

计算机可读存储介质130可以是服务端、U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

在本申请所提供的几个实施方式中，应该理解到，所揭露的方法以及设备，可以通过其它的方式实现。例如，以上所描述的设备实施方式仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。

另外，在本申请各个实施方式中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

以上仅为本申请的实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (14)

1.一种基于多传感器融合的定位方法，其特征在于，包括：

获取图像数据、IMU观测数据以及激光点云数据；

联合先验约束因子、无结构化视觉约束因子、IMU预积分约束因子以及激光匹配约束因子构建约束方程，以获取机器人的位姿，其中，所述先验约束因子为所述激光点云数据或所述IMU观测数据对应的误差约束因子，所述无结构化视觉约束因子为所述图像数据对应的误差约束因子，所述IMU预积分约束因子为所述IMU观测数据对应的误差约束因子，所述激光匹配约束因子为所述激光点云数据对应的误差约束因子；

利用回环检测对所述机器人的位姿进行修正，得到修正位姿；

对所述修正位姿进行全局位姿优化，以得到全局一致的位姿信息。

2.根据权利要求1所述的基于多传感器融合的定位方法，其特征在于，所述图像数据包括多个图像帧，所述图像帧包括利用左目相机采集到的左目图像帧以及利用右目相机采集到的右目图像帧，所述方法还包括：

对所述左目图像帧进行特征提取，生成左目特征点；

利用所述左目特征点得到同一时刻所述右目图像帧中的右目特征点；

对所述左目特征点与对应的所述右目特征点进行处理，以剔除所述左目图像帧中的外点与所述右目图像帧中的外点；

对前后两帧IMU观测数据进行特征跟踪，以剔除前一图像帧中的外点。

3.根据权利要求2所述的基于多传感器融合的定位方法，其特征在于，所述利用所述左目特征点得到同一时刻所述右目图像帧中的右目特征点的步骤，包括：

利用所述左目特征点进行左右目的立体匹配，得到所述右目特征点，将所述右目特征点作为光流跟踪的初始值；

进行光流跟踪得到左目跟踪特征点与右目跟踪特征点；

对所述左目图像帧中的左目特征点和所述右目图像帧中的右目特征点进行处理，以筛除所述左目图像帧中的外点与所述右目图像帧中的外点；

分别求取所述左目跟踪特征点与所述右目跟踪特征点在对应相机坐标系下的归一化坐标，得到左目坐标和右目坐标；

利用所述左目相机的外参和所述右目相机的外参构建本质矩阵，利用对极几何约束进行外点剔除，得到剩余特征点；

将所述左目图像帧与所述右目图像帧划分为第一预设数量的网格，将所述剩余特征点划分到对应的网格中，其中，每个所述网格中的特征点的数量小于第二预设数量。

4.根据权利要求3所述的基于多传感器融合的定位方法，其特征在于，所述利用对极几何约束进行外点剔除的步骤，包括：

判断每个所述特征点对应的所述左目坐标、所述本质矩阵以及所述右目坐标的乘积是否大于预设值；

若是，则所述特征点为所述外点。

5.根据权利要求2所述的基于多传感器融合的定位方法，其特征在于，所述对前后两帧IMU观测数据进行特征跟踪，以剔除前一图像帧中的外点的步骤，包括：

利用所述前后两帧IMU观测数据，得到前一图像帧相对当前图像帧的相对姿态；

利用所述前一图像帧中的像素坐标和所述相对姿态，预测所述当前图像帧的像素坐标；

将所述当前图像帧的像素坐标作为前后帧光流跟踪的初值，进行光流跟踪，得到当前图像帧中的左目跟踪特征点和右目跟踪特征点；

对所述左目跟踪特征点和所述右目跟踪特征点进行立体匹配，以筛除所述前一图像帧中的外点；

分别对所述前后两帧图像中的左目图像帧和右目图像帧进行处理，以筛除所述前一图像帧中的外点。

6.根据权利要求5所述的基于多传感器融合的定位方法，其特征在于，所述方法还包括：

在所述当前图像帧中的特征点的数量小于第三预设数量时，对所述当前图像帧进行特征提取，得到新的左目特征点；

对所述新的左目特征点进行立体匹配，得到新的右目特征点；

对所述新的左目特征点与所述新的右目特征点进行立体匹配，以筛除外点，得到新增的左目特征点与新增的右目特征点；

将所述新增的左目特征点添加到所述左目图像帧对应的网格中，将所述新增的右目特征点添加到所述右目图像帧对应的网格中。

7.根据权利要求1所述的基于多传感器融合的定位方法，其特征在于，所述方法还包括：

对所述激光点云数据进行去畸变去除处理，提取去畸变后的激光点云数据中的线特征点或面特征点；

利用所述线特征点或所述面特征点构建局部地图；

将当前帧激光点云数据与所述局部地图进行匹配，以估计所述激光点云数据在采集时刻的位姿。

8.根据权利要求7所述的基于多传感器融合的定位方法，其特征在于，所述对所述激光点云数据进行去畸变去除处理的步骤，包括：

利用所述IMU观测数据预测第k帧所述激光点云数据在采集结束时刻的位姿；

利用所述激光点云数据在采集结束时刻的位姿、第k帧所述激光点云数据的采集开始时刻、第k帧所述激光点云数据的采集结束时刻以及当前时刻，计算出当前时刻所述激光点云数据对应的位姿。

9.根据权利要求7所述的基于多传感器融合的定位方法，其特征在于，所述提取去畸变后的激光点云数据中的线特征点或面特征点的步骤，包括：

计算所述去畸变后的激光点云数据中每个激光点与距离所述激光点预设距离的激光点之间的曲率；

将所述曲率小于预设曲率的激光点作为面特征点。

10.根据权利要求1所述的基于多传感器融合的定位方法，其特征在于，所述方法还包括：

利用中值积分法对所述IMU观测数据与当前时刻的前一时刻的位姿进行处理，以预估所述当前时刻的位姿，将所述当前时刻的位姿作为位姿初值；

利用所述位姿初值与所述约束方程，得到所述机器人的位姿。

11.根据权利要求1所述的基于多传感器融合的定位方法，其特征在于，所述利用回环检测对所述机器人的位姿进行修正，得到修正位姿的步骤，包括：

构建视觉词袋库，利用所述视觉词袋库对当前帧与关键帧进行处理，得到与所述对当前帧对应的回环帧；

通过所述当前帧中的特征点与所述回环帧中的特征点建立数据关联；

利用对极几何约束对所述当前帧中的特征点与所述回环帧中的特征点进行外点剔除；

利用所述当前帧和所述闭环帧构建视觉回环约束因子，联合优化滑窗内的所述先验约束因子、所述无结构化视觉约束因子、所述IMU预积分约束因子以及所述视觉回环约束因子，得到所述修正位姿。

12.根据权利要求11所述的基于多传感器融合的定位方法，其特征在于，所述方法还包括：

分别找到与当前图像帧的时刻和对应的闭环帧的时刻最近的激光点云数据；

利用所述当前图像帧的位姿、所述当前图像帧与最近的激光点云数据之间的IM观测数据，预测最近的激光点云数据的位姿；

对两帧最近的激光点云数据进行匹配，得到所述当前帧和所述闭环帧的相对位姿。

13.一种基于多传感器融合的定位装置，其特征在于，包括互相连接的存储器和处理器，其中，所述存储器用于存储计算机程序，所述计算机程序在被所述处理器执行时，用于实现权利要求1-12中任一项所述的基于多传感器融合的定位方法。

14.一种计算机可读存储介质，用于存储计算机程序，其特征在于，所述计算机程序在被处理器执行时，用于实现权利要求1-12中任一项所述的基于多传感器融合的定位方法。

Images