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

一种基于优化的视觉和激光slam融合定位方法
技术领域
1.本发明属于移动机器人定位技术领域，具体涉及一种基于优化的视觉和激光slam融合定位方法。


背景技术：

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


技术实现要素：

5.鉴于上述，本发明提出了一种基于优化的视觉和激光slam融合定位方法，融合了两种相对独立且精准度较高的定位方法，使定位系统在一种方法失效情况下仍能正常工作，同时提高了精准度，最后对融合方法的改进也提高了系统的实时性。
6.一种基于优化的视觉和激光slam融合定位方法，包括如下步骤：
7.(1)获取视觉slam模块通过定位计算得到的机器人位置估计结果x及其高斯参数；
8.(2)获取激光slam模块通过粒子滤波定位算法得到的机器人位置估计结果y；
9.(3)对激光slam模块计算过程中的粒子分布结果进行近似高斯处理，计算得到位置估计结果y的高斯参数；
10.(4)利用两个模块定位输出的时间差对位置估计结果x及其高斯参数进行时间修正；
11.(5)根据上述信息建立目标函数，优化求解出其中的最优融合系数α；
12.(6)根据最优融合系数α通过融合函数计算出融合后最优的机器人位置估计结果z。
13.进一步地，所述视觉slam模块在定位计算过程中融合了imu数据。
14.进一步地，所述视觉slam模块前端使用特征法，根据对极约束求解相机运动并作为观测结果，进而使用imu和轮速计根据机器人运动模型得到预测结果，之后使用扩展卡尔曼滤波得到机器人的位置估计结果x及其高斯参数。
15.进一步地，所述目标函数的表达式如下：
[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]
为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。
[0035]
视觉里程计的主要问题是如何根据摄像头采集的相邻帧图像估计相机的运动状态。对于特征点方法，本发明首先需要对图像进行特征点提取，之后再对比两幅图像相应特征点的变化；对于路标点方法，则可以直接将路标点作为特征点。两种方法的本质都是寻找地图坐标系下的位置不变特征，设图像中共同的特征有n个，用y
i
(i＝1,2,
…
,n)表示，系统在k时刻的状态为x
k
，观测噪声ε
i,k
服从高斯分布，则系统的观测方程为：
[0036]
z
k
＝h(y
i
,x
k
)+ε
i,k
ꢀꢀꢀ
(1)
[0037]
其中抽象函数h是系统状态和特征到观测结果的映射，与相机模型有关。根据轮速计和imu的测量值u
k
可以根据k
‑
1时刻的系统状态推算出k时刻的系统状态，即系统的运动方程为：
[0038]
x
k
＝g(x
k
‑1,u
k
)+δ
k
ꢀꢀꢀ
(2)
[0039]
其中：抽象函数g描述了运动模型，δ
k
为系统噪声，服从高斯分布。
[0040]
由于系统的观测模型和运动模型是非线性的，因此使用扩展卡尔曼滤波方法进行状态估计，设观测模型和运动模型在k时刻的jacobi矩阵分别为h
k
和g
k
，观测噪声ε
i,k
～n(0,r
k
)，系统噪声δ
k
～n(0,q
k
)，则状态估计的期望值和协方差预测量为：
[0041][0042][0043]
卡尔曼增益为：
[0044][0045]
最后根据摄像头的观测更新状态估计的期望值和协方差矩阵如下：
[0046][0047][0048]
在平面运动中，机器人的位姿状态由二维位置坐标和偏航角表示，系统状态x
k
＝
[x y θ]
t
，至此得到视觉slam模块的定位估计及其协方差矩阵。
[0049]
本发明方法中激光slam使用通用的粒子滤波方法进行定位估计，粒子滤波的结果是一组离散的状态估计样本集及其对应置信度，但后续的优化算法需要连续的置信度估计，本发明方法根据粒子权重计算高斯近似，具体步骤如下：
[0050]
粒子滤波器的粒子数量为n，在k时刻输出状态样本集x
k,i
(i＝1,2,
…
,n)，对应的粒子归一化权重为w
i
(i＝1,2,
…
,n)，则高斯近似的均值为：
[0051][0052]
协方差矩阵为：
[0053][0054]
由于只对激光slam模块的位置估计进行融合，因此只计算位置坐标的高斯近似，至此得到激光slam模块的位置估计及其协方差矩阵。
[0055]
激光slam模块的定位频率约为10hz，在得到激光定位结果分布的高斯近似后，对视觉slam模块状态估计均值和协方差根据式(3)和式(4)进行时差补正，得到当前时刻的位置估计。为了方便讨论，设视觉和激光slam的位置估计为分别随机变量x＝[x
1 x2]
t
和y＝[y
1 y2]
t
，且均服从以下二维高斯分布：
[0056][0057][0058]
z为融合后的位置估计，即：
[0059]
z＝αx+(1
‑
α)y
ꢀꢀꢀ
(10)
[0060]
式中α为融合系数，z服从以下高斯分布：
[0061][0062]
二维高斯分布中，标准差椭圆的面积反映了随机变量的分布集中度，在定位问题中，我们希望位置估计结果尽可能集中在期望值附近；误差椭圆面积s与随机变量z协方差矩阵的关系为：
[0063][0064]
其中：
[0065][0066]
z的协方差矩阵元素值分别为：
[0067][0068][0069]
ρ
z
＝e(z1z2)
‑
p
x
p
y
[0070]
其中：
[0071]
e(z1z2)＝α2e(x1x2)+α(1
‑
α)e(x1y2)+α(1
‑
α)e(x2y1)+(1
‑
α)2(y1y2)
[0072]
由于摄像头、imu和激光雷达高斯噪声的独立性，则：
[0073]
e(x1y2)＝e(x2y1)＝cov(x1，y2)+ex1ey2＝pxp
y
[0074]
且：
[0075]
e(x1x2)＝ρ
x
+p
x
p
y
[0076]
e(y1y2)＝ρ
y
+p
x
p
y
[0077]
所以：
[0078]
ρ
z
＝α2ρ
x
+(1
‑
α)2ρ
y
[0079]
设m＝α2，n＝(1
‑
α)2，优化的目标函数为：
[0080][0081]
约束条件为：
[0082][0083]
最后通过拉格朗日乘数法等优化方法求得m和n，融合结果为：
[0084][0085]
理论和实践证明，融合之后的定位误差精准度均比融合前的精准度高。
[0086]
图1给出了本发明方法的系统实现框架，imu集成了加速度计、陀螺仪和磁力计，测量数据输出频率100hz；加速度计积分得到的速度测量结果不够准确，本发明使用轮速计模块计算速度，轮速计使用光电式旋转编码器，速度计算结果输出频率100hz。视觉slam模块的前端部分使用特征法，特征可以是orb等特征提取算法提取的特征点，也可以是色块之类的路标点之后使用扩展卡尔曼滤波算法与imu和轮速计的测量结果融合，得到位置估计和协方差矩阵，视觉slam模块的定位频率为30hz。激光slam模块使用imu和轮速计进行初始位姿估计，目的是减少粒子收敛时间，之后通过粒子滤波定位算法得到位置估计和一组粒子权重，激光slam的定位频率为10hz。粒子的权重分布反映了位置的概率分布，对粒子的权重分布进行近似高斯处理，得到高斯分布表示的位置概率分布，之后将两个模块的位置估计和高斯参数输入到融合模块。
[0087]
图2为融合模块的执行流程，经过对位置估计的处理，得到了两组服从高斯分布的位置概率分布，视觉slam模块的位置输出记为随机变量x，激光slam模块的位置输出记为随机变量y。融合模块在得到激光slam的位置估计结果之后进行融合处理，但是由于两个定位模块的计算结果并不会同时输出，所以需要对视觉slam位置估计结果根据运动模型和imu、轮速计数据进行时差补正得到当前时刻的位置估计及高斯参数。融合模块使用了线型融合函数z＝αx+(1
‑
α)y，目标是减小融合结果的不确定度。融合模块采用优化的方式求解融合系数，根据融合系数输出最后的定位结果。
[0088]
图3、图4和图5是本发明系统在不同情况下的定位融合效果对比。第一排第二列图片的点表示了视觉slam得到的位置分布，第二排第一列图片的点表示了激光slam得到的位置分布，第二排第二列图片的点表示了融合后的位置分布，第一排第一列图片表示了三种
位置分布结果的对比，椭圆a、b、c分别为视觉slam、激光slam和融合后的定位位置分布。图3是两种定位精度相差较大的情况，融合后的结果稍优于激光slam的效果；图4是两种定位精度相近的情况，融合后的结果明显优于未融合前的效果；图5是一种定位模块失效的情况，融合后的结果基本与正常工作的定位方法相同。
[0089]
以上所述的具体实施方式对本发明的技术方案和效果进行了详尽说明，目的是便于本领域的技术人员能够理解和应用本发明。熟悉本领域技术的人员显然可以容易地对上述实施例做出各种修改，并把在此说明的一般原理应用到其他实施例中而不必经过创造性的劳动。因此，本发明不限于上述实施例，本领域技术人员根据本发明的揭示，对于本发明做出的改进和修改都应该在本发明的保护范围之内。