一种基于优化的视觉和激光SLAM融合定位方法技术

本发明专利技术公开了一种基于优化的视觉和激光SLAM融合定位方法，首先分别获取视觉SLAM模块前端视觉惯性里程计与激光SLAM模块粒子滤波定位算法得到的定位估计结果，对粒子的分布进行近似高斯估计，然后根据定位时间差值对视觉SLAM模块进行补正，根据设定的目标函数和补正后高斯参数计算最优融合系数，得到最终的融合结果。本发明专利技术融合了两种相对独立且精准度较高的定位方法，使定位系统在一种方法失效情况下仍能正常工作，同时提高了精准度，最后对融合方法的改进也提高了系统的实时性。方法的改进也提高了系统的实时性。方法的改进也提高了系统的实时性。

【技术实现步骤摘要】
一种基于优化的视觉和激光SLAM融合定位方法


[0001]本专利技术属于移动机器人定位
，具体涉及一种基于优化的视觉和激光SLAM融合定位方法。

技术介绍

[0002]定位问题始终是移动机器人领域的核心问题之一，定位结果的准确度对移动机器人后续的规划和控制等模块有很大的影响。目前移动机器人的定位方法有很多，如视觉系列方法、激光雷达系列方法、RFID系列方法、GPS系列方法和路标点(Landmark)系列方法等。改善定位效果的方法主要有两种：使用更高精度传感器和改进定位算法，然而高精度传感器往往价格昂贵，且价格与精度的性价比随着精度提高迅速降低，因此对定位算法的改进在实际场景中应用较为广泛。
[0003]现阶段定位算法改进集中在两个方向：一种是依赖高速发展的硬件平台运行高计算复杂度的高精度定位方法，特点是计算量大，使用的方法在几年前的硬件平台很难满足实时性，但近几年随着硬件技术飞快发展，这类方法的优势逐渐明显，如SLAM(Simultaneous Localization And Mapping，即时定位与地图构建)后端的处理方法从扩展卡尔曼滤波发展为图优化；另一种是利用不同的传感器之间的特性，优势互补，融合测量数据得到更加精准的定位效果，如视觉和激光SLAM中常常使用IMU(Inertial Measurement Unit，惯性测量单元)的数据得到定位预测值，通过卡尔曼滤波得到融合的定位结果。
[0004]现有的定位融合方法大多基于卡尔曼滤波及其变种，公开号为CN107289933B的中国专利提出了一种融合MEMS传感器和VLC传感器的定位方法，该方法通过MEMS传感器得到PDR定位结果，通过VLC定位模块得到VLC定位结果，通过定位卡尔曼滤波器融合两种定位结果，得到最终定位信息。但实际上价格适中的MEMS传感器往往零飘和测量精准度很差，这会导致PDR的定位精度较差，定位效果仍然很大程度上取决于VLC定位方法。公开号为CN105737832B的中国专利介绍了一种利用分布式结构将整个状态向量分为机器人位姿估计和路标估计五维状态分布化处理再进行融合的定位方法，该方法依据每个有效路标点单独建立多个相互平行的子滤波器，然后将子滤波器的机器人位姿估计结果在主滤波器中进行融合，并将子滤波器的融合结果通过全局预测器反馈修正，最后得到全局最优的机器人位姿估计结果；但是该专利技术中的子滤波器使用扩展卡尔曼滤波，主滤波器将子滤波器的估计结果加权平均得到最优估计值，但并未说明如何得到子滤波器的权值系数。

技术实现思路

[0005]鉴于上述，本专利技术提出了一种基于优化的视觉和激光SLAM融合定位方法，融合了两种相对独立且精准度较高的定位方法，使定位系统在一种方法失效情况下仍能正常工作，同时提高了精准度，最后对融合方法的改进也提高了系统的实时性。
[0006]一种基于优化的视觉和激光SLAM融合定位方法，包括如下步骤：
[0007](1)获取视觉SLAM模块通过定位计算得到的机器人位置估计结果X及其高斯参数；
[0008](2)获取激光SLAM模块通过粒子滤波定位算法得到的机器人位置估计结果Y；
[0009](3)对激光SLAM模块计算过程中的粒子分布结果进行近似高斯处理，计算得到位置估计结果Y的高斯参数；
[0010](4)利用两个模块定位输出的时间差对位置估计结果X及其高斯参数进行时间修正；
[0011](5)根据上述信息建立目标函数，优化求解出其中的最优融合系数α；
[0012](6)根据最优融合系数α通过融合函数计算出融合后最优的机器人位置估计结果Z。
[0013]进一步地，所述视觉SLAM模块在定位计算过程中融合了IMU数据。
[0014]进一步地，所述视觉SLAM模块前端使用特征法，根据对极约束求解相机运动并作为观测结果，进而使用IMU和轮速计根据机器人运动模型得到预测结果，之后使用扩展卡尔曼滤波得到机器人的位置估计结果X及其高斯参数。
[0015]进一步地，所述目标函数的表达式如下：
[0016][0017]其中：m和n为中间变量，f(m,n)为关于m和n的目标函数，n＝(1
‑
α)2，m＝α2，σ
x1
为机器人位置估计结果X的x轴分量的标准差，σ
x2
为机器人位置估计结果X的y轴分量的标准差，ρ
x
为机器人位置估计结果X的协方差，σ
y1
为机器人位置估计结果Y的x轴分量的标准差，σ
y2
为机器人位置估计结果Y的y轴分量的标准差，ρ
y
为机器人位置估计结果Y的协方差。
[0018]进一步地，所述步骤(5)在优化求解过程中需满足以下约束条件：
[0019][0020]其中：m和n为中间变量，n＝(1
‑
α)2，m＝α2。
[0021]进一步地，所述步骤(6)通过以下融合函数计算出融合后最优的机器人位置估计结果Z；
[0022][0023]其中：m和n为中间变量，n＝(1
‑
α)2，m＝α2。
[0024]本专利技术SLAM融合定位方法所使用的视觉SLAM模块前端使用特征点或路标点匹配，与IMU和轮速编码器通过扩展卡尔曼滤波融合作为VIO(visual
‑
inertial odometry，视觉惯性里程计)；激光SLAM模块使用粒子滤波(particle filter)得到位置估计，根据粒子权重计算近似的高斯分布协方差矩阵，最后视觉和激光SLAM模块得到的位置估计结果和协方差矩阵输入到定位融合模块根据目标函数优化计算两种定位方法的权值系数，得到融合的最优位置估计结果；机器人的朝向角度估计不经过融合模块，直接根据视觉SLAM方法得出。
[0025]与现有技术相比较，本专利技术具有以下有益技术效果：
[0026]1.定位精准度更高；本专利技术融合了两种较高精度定位方法而非使用IMU或MEMS直接位置估计等低精度方法，得到了更加精准的融合定位结果。
[0027]2.具有更强的鲁棒性；本专利技术当一种定位模块失效时，定位融合结果精准度不会低于另一个定位模块；
[0028]3.时间复杂度低。本专利技术融合方法基于优化而非卡尔曼滤波，融合结果由迭代或解析方式得出，在状态数较少时相比卡尔曼滤波矩阵乘法和求逆运算时间复杂度更低。
附图说明
[0029]图1为本专利技术SLAM融合定位方法的系统实现框架示意图。
[0030]图2为本专利技术融合模块的执行流程示意图。
[0031]图3为在两种定位精度差距较大情况下本专利技术的融合效果对比示意图。
[0032]图4为在两种定位精度基本相同情况下本专利技术的融合效果对比示意图。
[0033]图5为在一种定位失效情况下本专利技术的融合效果对比示意图。
具体实施方式
[0034]为了更为具体地描述本专利技术，下面结合附图及具体实施方式对本专利技术的技术方案进本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于优化的视觉和激光SLAM融合定位方法，包括如下步骤：(1)获取视觉SLAM模块通过定位计算得到的机器人位置估计结果X及其高斯参数；(2)获取激光SLAM模块通过粒子滤波定位算法得到的机器人位置估计结果Y；(3)对激光SLAM模块计算过程中的粒子分布结果进行近似高斯处理，计算得到位置估计结果Y的高斯参数；(4)利用两个模块定位输出的时间差对位置估计结果X及其高斯参数进行时间修正；(5)根据上述信息建立目标函数，优化求解出其中的最优融合系数α；(6)根据最优融合系数α通过融合函数计算出融合后最优的机器人位置估计结果Z。2.根据权利要求1所述基于优化的视觉和激光SLAM融合定位方法，其特征在于：所述视觉SLAM模块在定位计算过程中融合了IMU数据。3.根据权利要求1所述基于优化的视觉和激光SLAM融合定位方法，其特征在于：所述视觉SLAM模块前端使用特征法，根据对极约束求解相机运动并作为观测结果，进而使用IMU和轮速计根据机器人运动模型得到预测结果，之后使用扩展卡尔曼滤波得到机器人的位置估计结果X及其高斯参数。4.根据权利要求1所述基于优化的视觉和激光SLAM融合定位方法，其特征在于：所述目标函数的表达式如下：其中：m和n为中间变量，f(m,n)为关于m和n的目标函数，n＝(1
‑
α)2，m＝α2，σ
x1
为机器人位置估计结果X的x轴分量的标准差，σ
x2
为机器人位置估计结果X的y轴分量的标准差，ρ
x
为机器人位置估计结果X的协方差，σ
y1
为机器人位置估计结果Y的x轴分量的标准差，σ
y2

【专利技术属性】
技术研发人员：李红，杨国青，辛越，吕攀，吴朝晖，
申请(专利权)人：浙江大学，
类型：发明
国别省市：