基于粒子滤波和图优化的机器人定位和建图系统及方法技术方案

本发明专利技术公开了基于粒子滤波和图优化的机器人定位和建图系统及方法。基于SIR滤波器进行重采样策略优化，使用最小采样方差重采样方法代替简单判断权重大小的重采样方法，有效提高粒子多样性。引入子图系统，同时维护两张活跃子图，一帧激光数据与两张子图构建约束，并通过相关性扫描匹配构建回环约束，进行后端优化。利用非线性最小二乘法最小化误差，优化所有历史位姿与子图位姿，并在优化后的位姿集中继续添加新位姿。本发明专利技术使用粒子滤波器作为前端里程计，估计良好的位姿初值，生成局部子图，并结合图优化利用所有时刻信息最小化误差的优势，在满足实时性要求的前提下，获得高精度的位姿和栅格地图，为后续变电站巡检工作提供参考。参考。参考。

【技术实现步骤摘要】
基于粒子滤波和图优化的机器人定位和建图系统及方法


[0001]本专利技术属于自主移动机器人定位
，具体涉及一种基于粒子滤波和图优化的变电站巡检机器人定位和建图方法。

技术介绍

[0002]在电力行业大力发展的背景下，为不断适应传输电力电压等级提高和输入线路增加的需要，变电站作为电力生产运输过程中不可缺失的一环持续进行着扩容和升级。这导致了工作人员巡检难度日益增长，变电站巡检机器人代替人工巡检迫在眉睫。变电站巡检机器人是结合环境感知、动态决策、运动控制和设备检测等功能为一体的综合系统。它可以通过地图构建、自主定位、路径规划和设备检测完成巡检任务，在提升巡检效率的同时，也有效避免了人工巡检所需面对的一系列问题。
[0003]地图构建和自主定位是机器人导航移动的基础，也是机器人巡检作业的核心。因此，同时定位与建图技术(Simultaneous Localization and Mapping，SLAM)是智能变电站巡检机器人的关键技术之一。
[0004]基于蒙特卡洛采样理论的RBPF(Rao
‑
Blackwellized Particle Filter)方法是滤波SLAM算法中的重要方法，其核心思想是通过从后验概率中抽取的随机状态粒子表达其分布，粒子的数量越多，RBPF方法的精度就越高。由于每一个粒子都携带一副完整的地图，过多的粒子会对内存造成极大的压力，并且会对整个系统的实时性造成影响，如何用更少的粒子来提高RBPF方法的精度是目前学者们对RBPF的改进方向。
[0005]目前，图优化方法是学术热点之一，以顶点作为待优化变量，以观测方程作为约束边，构建非线性最小二乘问题最小误差，利用梯度下降法、高斯
‑
牛顿法、列文伯格
‑
马夸尔特算法(Levenberg
‑
Marquardt，LM)等方法迭代优化。图优化SLAM结合过去所有时刻状态，累计误差发散较慢，稳定性较强，但计算量大大增加，随着优化矩阵的稀疏性被学者发现以及计算机性能的不断飞跃，实时优化SLAM问题成为可能。
[0006]由于在变电站室外复杂巡检环境下，场地较为空旷，结构化场景较少，单线激光雷达获得的环境信息有限，优化SLAM前端匹配误差较大，极大限制了一般的优化方法，而粒子滤波算法虽可在短期内获得较为理想的效果，但其误差会随着时间不断发散。

技术实现思路

[0007]针对现有技术中存在的不足，本专利技术提出一种基于粒子滤波和图优化的机器人定位和建图系统及方法，使用粒子滤波器作为前端里程计，估计良好的位姿初值，生成局部子图，并结合图优化利用所有时刻信息最小化误差的优势，在满足实时性要求的前提下，获得高精度的位姿和栅格地图。
[0008]SLAM问题在仅给定观测输入的条件下，同时输出对机器人位姿和当前地图的最佳估计，RBPF方法将该问题分解为独立的机器人定位问题，和在已知机器人位姿的基础上进行环境地图构建的问题，如式(1)所示。
[0009]P(x
1:t
,m|u
1:t
‑1,z
1:t
)＝P(x
1:t
|u
1:t
‑1,z
1:t
)P(m|x
1:t
,z
1:t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0010]式中u
1:t
表示机器人的1～t时刻所有的轮速计信息，z
1:t
表示机器人1～t时刻的激光观测，x
1:t
和m分别表示机器人定位和建图系统估计的机器人1～t时刻的位姿和地图。
[0011]利用条件贝叶斯公式和条件联合概率公式对机器人定位问题进行分解和化简，将位姿估计问题转变为增量估计问题，如式(2)所示。
[0012]P(x
1:t
|u
1:t
‑1,z
1:t
)＝P(z
t
|x
t
)P(x
t
|x
t
‑1,u
t
‑1)P(x
1:t
‑1|u
1:t
‑2,z
1:t
‑1)
ꢀꢀ
(2)
[0013]其中，P(x
1:t
‑1|u
1:t
‑2,z
1:t
‑1)表示上一时刻粒子群携带的机器人位姿，P(x
t
|x
t
‑1,u
t
‑1)为每一个粒子根据机器人运动学模型累加上一时刻的位姿和里程计得到的预测位姿，并使用观测模型P(z
t
|x
t
)进行权重计算。
[0014]本专利技术基于SIR(sampling importance resampling)滤波器进行重采样策略优化，使用最小采样方差重采样方法代替简单判断权重大小的重采样方法，有效提高粒子多样性。引入子图系统，同时维护两张活跃子图，一帧激光数据与两张子图构建约束，并通过相关性扫描匹配构建回环约束，进行后端优化。利用非线性最小二乘法最小化误差，优化所有历史位姿与子图位姿，并在优化后的位姿集中继续添加新位姿。
[0015]基于粒子滤波和图优化的机器人定位和建图系统，包括局部SLAM模块和全局SLAM模块；
[0016]所述的局部SLAM模块包括粒子集合生成模块、重采样模块和子图系统；所述的全局SLAM模块包括约束构建模块和位姿优化模块；
[0017]所述的粒子集合生成模块用于进行扫描匹配和粒子采样，生成粒子集合，其中每一个粒子都会得到一个里程计位姿，生成并携带一幅当前子图；
[0018]所述的重采样模块采用改进后的重采样方法，粒子集合生成模块通过重采样模块不断重采样直至粒子集的权重离散程度高于设定阈值，得到优化后的粒子集合；
[0019]所述的子图系统用于同时维护两张活跃子图，保持足够多的激光帧间位姿约束关系；
[0020]所述的约束构建模块，使用CSM匹配与分支定界加速获得回环约束；
[0021]所述的位姿优化模块，根据局部SLAM模块计算得到的里程计约束和约束构建模块得到的回环约束构建图优化问题，最小二乘求解最优位姿和地图。
[0022]当一帧新的激光数据进入系统(进行了一次观测)，局部SLAM模块接收该帧激光数据，并积分当前时刻与上一帧激光接收时刻间所有的轮速计数据。通过粒子集合生成模块进行扫描匹配和粒子采样，生成粒子集合，其中每一个粒子都会得到一个里程计位姿，生成并携带一幅当前子图。通过重采样模块的改进重采样方法，不断重采样直至粒子集的权重离散程度高于设定阈值，得到优化后的粒子集合。通过子图系统同时维护两张活跃子图，保持足够多的激光帧间位姿约束关系。进入全局SLAM模块，通过约束构建模块，使用CSM匹配与分支定界加速获得回环约束。最后通过位姿优化模块，根本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于粒子滤波和图优化的机器人定位和建图系统，其特征在于，包括局部SLAM模块和全局SLAM模块；所述的局部SLAM模块包括粒子集合生成模块、重采样模块和子图系统；所述的全局SLAM模块包括约束构建模块和位姿优化模块；所述的粒子集合生成模块用于进行扫描匹配和粒子采样，生成粒子集合，其中每一个粒子都会得到一个里程计位姿，生成并携带一幅当前子图；所述的重采样模块采用改进后的重采样方法，粒子集合生成模块通过重采样模块不断重采样直至粒子集的权重离散程度高于设定阈值，得到优化后的粒子集合；所述的子图系统用于同时维护两张活跃子图，保持足够多的激光帧间位姿约束关系；所述的约束构建模块，使用CSM匹配与分支定界加速获得回环约束；所述的位姿优化模块，根据局部SLAM模块计算得到的里程计约束和约束构建模块得到的回环约束构建图优化问题，最小二乘求解最优位姿和地图。2.基于粒子滤波和图优化的机器人定位和建图方法，其特征在于，步骤如下：步骤1：通过粒子集合生成模块进行扫描匹配和粒子采样，生成粒子集合；步骤2：改进重采样，不断重采样直至粒子集的权重离散程度高于设定阈值，得到优化后的粒子集合；步骤3：通过子图系统完成子图维护；步骤4：回环检测，使用CSM匹配与分支定界加速获得回环约束；步骤5：根据计算得到的里程计约束和约束构建模块得到的回环约束构建图优化问题，最小二乘求解最优位姿和地图。3.根据权利要求2所述的基于粒子滤波和图优化的机器人定位和建图方法，其特征在于，步骤1具体方法如下；一帧新激光数据进入机器人定位和建图系统后，机器人定位和建图系统初始化当前时刻粒子群S
t
，循环历遍上一时刻的粒子群S
t
‑1，获取上一时刻粒子的位姿权重和携带子图并利用轮速计的运动模型状态传播预测t时刻位姿其中u
t
‑1为t
‑
1时刻轮速计数据；由于粒子不唯一，同一时刻将估计出数条轨迹，每条轨迹的权重至关重要，采用表示该粒子的轨迹，其重要性权重可定义为式(3)：其中，分子为目标分布，分母为建议分布，每进行一次观测，如计算完整轨迹权重，则轨迹的重要性权重计算量会随时间增长；在已知粒子轨迹的条件下，通过计算轨迹对应子图并对其进行展开和化简，可得到重要性权重递归式(4)；
当激光帧与现有地图匹配度高时，观测的可靠性高，观测分布L
(i)
区间小，使用激光匹配采样生成的粒子落在区间L
(i)
的数量更多，能够用更少的粒子覆盖位姿的概率分布；当激光匹配度低时，观测可靠性低，则使用运动模型采样生成粒子；观测可靠性通过极大似然估计地图匹配度判断，首先在预测位姿附近进行搜索，计算当前帧的观测z
t
与子图的匹配度，若匹配度超过设定阈值，则认为观测可靠性高，如式(5)所示：若在预测位姿周围搜索到匹配度超过阈值的极值点极值点即代表观测区间L
(i)
中概率峰值区域，以为中心，Δ为半径随机采样固定数量的k个点{x1，...，x
k
}，即完成采样；因激光匹配模型方差较小，令采样粒子集服从高斯分布，并结合激光观测和轮速计信息，计算其矢量均值和协方差如式(6)所示：如式(6)所示：其中归一化参数T为矩阵转置符；新的粒子点集将从中使用多元正态分布公式进行概率计算，其权重中的建议分布π改进为则新粒子对应的权重计算方式为式(7)；若通过式(5)计算不存在与地图匹配度超过阈值的则使用运动模型进行新粒子的采样，建议分布π使用运动模型，其重要性权重计算方式为式(8)；
采样完成后，在已知上一时刻地图此刻位姿}和观测z
t
的条件下，通过更新每一个粒子所携带的子图并以权重比例整合每条轨迹对应子图，得到子图m
t
。4.根据权利要求3所述的基于粒子滤波和图优化的机器人定位和建图方法，其特征在于，步骤2具体方法如下；重采样即删除低权重粒子，并生成新粒子保持粒子总数不变的过程，不断进行重采样则带来粒子耗散问题，即所有粒子都从一个粒子复制而来，丧失了粒子的多样性；使用式(9)判断权重的离散程度N
eff
，决定是否进行重采样，其中为粒子归一化权重，当N
eff
小于设定离散阈值时，认为各个粒子的权重差异较大，进行重采样，否则不进行重采样；改进重采样方法，用采样方差度量粒子分布在重采样前后的差异，衡量重采样对粒子多样性的破坏程度；采样方差计算粒子实际复制的次数和期望复制的次数的差的平方和，采样方差越小，粒子的分布在重采样前后的差异就越小，粒子的多样性相对就会更高，采样方差(SV)的计算公式如式(10)；其中，M表示重采样前粒子总数，N表示重采样后粒子的总数，为粒子实际复制的次数，在t时刻粒子的权重为用表示该粒子期望复制的次数；采用最小采样方差重采样方法代替简单根据权重高低的重采样方法，使重采样前后粒子分布趋于一致；使用floor函数向下取整，赋予粒子期望复制次数使用floor函数向下取整，赋予粒子期望复制次数计算粒子的剩余权重和所有粒子复制次数总数L；和所有粒子复制次数总数L；复制剩余权重最大的M
‑
L个粒子，完成重采样，即可有效减少权重较低的优质粒子被删除。5.根据权利要求4所述的基于粒子滤波和图优化的机器人定位和建图方法，其特征在于，步骤3：通过子图系统完成子图维护；为了保证全局优化中拥有足够多约束关系，机器人定位和建图系统采用子图系统，使用双线程同时维护两张活跃子图，一帧激光扫描帧数据将通过粒子滤波器同时在两张活跃子图内进行匹配；当一张活跃子图中的激光帧数量达到设定阈值ρ
max
，该子图...

【专利技术属性】
技术研发人员：屠铭扬，曾平良，吴秋轩，郭志民，田杨阳，孟秦源，经韬，毛万登，刘昊，库永恒，张波涛，袁少光，刘善峰，梁允，
申请(专利权)人：国网河南省电力公司电力科学研究院国家电网有限公司，
类型：发明
国别省市：