基于粒子滤波和图优化的机器人定位和建图系统及方法与流程

1.本发明属于自主移动机器人定位技术领域，具体涉及一种基于粒子滤波和图优化的变电站巡检机器人定位和建图方法。


背景技术：

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


技术实现要素：

7.针对现有技术中存在的不足，本发明提出一种基于粒子滤波和图优化的机器人定位和建图系统及方法，使用粒子滤波器作为前端里程计，估计良好的位姿初值，生成局部子图，并结合图优化利用所有时刻信息最小化误差的优势，在满足实时性要求的前提下，获得高精度的位姿和栅格地图。
8.slam问题在仅给定观测输入的条件下，同时输出对机器人位姿和当前地图的最佳估计，rbpf方法将该问题分解为独立的机器人定位问题，和在已知机器人位姿的基础上进行环境地图构建的问题，如式(1)所示。
9.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)
10.式中u
1:t
表示机器人的1～t时刻所有的轮速计信息，z
1:t
表示机器人1～t时刻的激光观测，x
1:t
和m分别表示机器人定位和建图系统估计的机器人1～t时刻的位姿和地图。
11.利用条件贝叶斯公式和条件联合概率公式对机器人定位问题进行分解和化简，将位姿估计问题转变为增量估计问题，如式(2)所示。
12.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)
13.其中，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
)进行权重计算。
14.本发明基于sir(sampling importance resampling)滤波器进行重采样策略优化，使用最小采样方差重采样方法代替简单判断权重大小的重采样方法，有效提高粒子多样性。引入子图系统，同时维护两张活跃子图，一帧激光数据与两张子图构建约束，并通过相关性扫描匹配构建回环约束，进行后端优化。利用非线性最小二乘法最小化误差，优化所有历史位姿与子图位姿，并在优化后的位姿集中继续添加新位姿。
15.基于粒子滤波和图优化的机器人定位和建图系统，包括局部slam模块和全局slam模块；
16.所述的局部slam模块包括粒子集合生成模块、重采样模块和子图系统；所述的全局slam模块包括约束构建模块和位姿优化模块；
17.所述的粒子集合生成模块用于进行扫描匹配和粒子采样，生成粒子集合，其中每一个粒子都会得到一个里程计位姿，生成并携带一幅当前子图；
18.所述的重采样模块采用改进后的重采样方法，粒子集合生成模块通过重采样模块不断重采样直至粒子集的权重离散程度高于设定阈值，得到优化后的粒子集合；
19.所述的子图系统用于同时维护两张活跃子图，保持足够多的激光帧间位姿约束关系；
20.所述的约束构建模块，使用csm匹配与分支定界加速获得回环约束；
21.所述的位姿优化模块，根据局部slam模块计算得到的里程计约束和约束构建模块得到的回环约束构建图优化问题，最小二乘求解最优位姿和地图。
22.当一帧新的激光数据进入系统(进行了一次观测)，局部slam模块接收该帧激光数据，并积分当前时刻与上一帧激光接收时刻间所有的轮速计数据。通过粒子集合生成模块进行扫描匹配和粒子采样，生成粒子集合，其中每一个粒子都会得到一个里程计位姿，生成并携带一幅当前子图。通过重采样模块的改进重采样方法，不断重采样直至粒子集的权重离散程度高于设定阈值，得到优化后的粒子集合。通过子图系统同时维护两张活跃子图，保持足够多的激光帧间位姿约束关系。进入全局slam模块，通过约束构建模块，使用csm匹配与分支定界加速获得回环约束。最后通过位姿优化模块，根据局部slam模块计算得到的里程计约束和约束构建模块得到的回环约束构建图优化问题，最小二乘求解最优位姿和地图。
23.基于粒子滤波和图优化的机器人定位和建图方法，步骤如下：
24.步骤1：通过粒子集合生成模块进行扫描匹配和粒子采样，生成粒子集合；
25.步骤2：改进重采样，不断重采样直至粒子集的权重离散程度高于设定阈值，得到
优化后的粒子集合；
26.步骤3：通过子图系统完成子图维护；
27.步骤4：回环检测，使用csm匹配与分支定界加速获得回环约束；
28.步骤5：根据计算得到的里程计约束和约束构建模块得到的回环约束构建图优化问题，最小二乘求解最优位姿和地图。
29.本发明有益效果如下：
30.本发明提出一种粒子滤波和图优化结合的slam方法，使用粒子滤波器作为前端里程计，估计良好的位姿初值，生成局部子图，并结合图优化利用所有时刻信息最小化误差的优势，在满足实时性要求的前提下，获得高精度的位姿和栅格地图，为后续变电站巡检工作提供参考。
附图说明
31.图1为本发明实施例系统处理流程图；
32.图2为本发明实施例子图生成算法流程图；
33.图3为本发明实施例全局优化示意图；
34.图4为本发明实施例观测可靠性对观测分布l(i)的影响；
35.图5为本发明实施例子图维护示意图；
36.图6为本发明实施例分支定界加速示意图；
具体实施方式
37.以下结合附图与实施例对本发明方法进行进一步描述。
38.图1为本发明实施例系统处理流程图；
39.基于sir(sampling importance resampling)滤波器的局部slam子图生成算法具体流程如图2所示。
40.全局优化采用图优化方法，以激光帧位姿和子图位姿作为节点，位姿间的位姿约束作为边，构建误差的最小二乘问题并求解，得到各位姿的最优解，如图3所示。其中，位姿约束包括局部slam构建的前端约束和回环检测构建的回环约束。
41.基于粒子滤波和图优化的机器人定位和建图方法，步骤如下：
42.步骤1：通过粒子集合生成模块进行扫描匹配和粒子采样，生成粒子集合；
43.一帧新激光数据进入机器人定位和建图系统后，机器人定位和建图系统初始化当前时刻粒子群s
t
，循环历遍上一时刻的粒子群s
t-1
，获取上一时刻粒子(以第i个粒子为例)的位姿权重和携带子图并利用轮速计的运动模型状态传播预测t时刻位姿其中u
t-1
为t-1时刻轮速计数据。由于粒子不唯一，同一时刻将估计出数条轨迹，每条轨迹的权重至关重要，采用表示该粒子的轨迹，其重要性权重可定义为式(3)：
44.45.其中，分子为目标分布，分母为建议分布，每进行一次观测，如计算完整轨迹权重，则轨迹的重要性权重计算量会随时间增长。在已知粒子轨迹的条件下，通过计算轨迹对应子图并对其进行展开和化简，可得到重要性权重递归式(4)。
[0046][0047]
激光雷达匹配相较里程计测量更为精准，在分布上，其方差也比里程计更具优势，如图4所示，当激光帧与现有地图匹配度高时，观测的可靠性高，观测分布l(i)区间小，使用激光匹配采样生成的粒子落在区间l(i)的数量更多，能够用更少的粒子覆盖位姿的概率分布。
[0048]
当激光匹配度低时，观测可靠性低，则使用运动模型采样生成粒子。
[0049]
观测可靠性通过极大似然估计地图匹配度判断，首先在预测位姿附近进行搜索，计算当前帧的观测z
t
与子图的匹配度，若匹配度超过设定阈值，则认为观测可靠性高，如式(5)所示：
[0050][0051]
若在预测位姿周围搜索到匹配度超过阈值的极值点极值点即代表观测区间l(i)中概率峰值区域，以为中心，δ为半径随机采样固定数量的k个点{x1,
…
,xk}，即完成采样。
[0052]
因激光匹配模型方差较小，令采样粒子集服从高斯分布，并结合激光观测和轮速计信息，计算其矢量均值和协方差如式(6)所示：
[0053][0054][0055]
其中归一化参数t为矩阵转置符。
[0056]
新的粒子点集将从中使用多元正态分布公式进行概率计算，其权重中的建议分布π改进为则新粒子对应的权重计算方式为式(7)。
[0057][0058]
若通过式(5)计算不存在与地图匹配度超过阈值的则使用运动模型进行新粒子的采样，建议分布π使用运动模型，其重要性权重计算方式为式(8)。
[0059][0060]
采样完成后，在已知上一时刻地图此刻位姿}和观测z
t
的条件下，通过更新每一个粒子所携带的子图并以权重比例整合每条轨迹对应子图，得到子图m
t
。
[0061]
步骤2：改进重采样，不断重采样直至粒子集的权重离散程度高于设定阈值，得到优化后的粒子集合；
[0062]
重采样即删除低权重粒子，并生成新粒子保持粒子总数不变的过程，不断进行重采样则带来粒子耗散问题，即所有粒子都从一个粒子复制而来，丧失了粒子的多样性。使用式(9)判断权重的离散程度n
eff
，决定是否进行重采样，其中为粒子归一化权重，当n
eff
小于设定离散阈值时，认为各个粒子的权重差异较大，进行重采样，否则不进行重采样。
[0063][0064]
改进重采样方法，用采样方差度量粒子分布在重采样前后的差异，衡量重采样对粒子多样性的破坏程度。采样方差计算粒子实际复制的次数和期望复制的次数的差的平方和，采样方差越小，粒子的分布在重采样前后的差异就越小，粒子的多样性相对就会更高，采样方差(sv)的计算公式如式(10)。
[0065][0066]
其中，m表示重采样前粒子总数，n表示重采样后粒子的总数，为粒子实际复制的次数，在t时刻粒子的权重为用表示该粒子期望复制的次数。
[0067]
采用最小采样方差重采样方法代替简单根据权重高低的重采样方法，使重采样前后粒子分布趋于一致。
[0068]
使用floor函数向下取整，赋予粒子期望复制次数
[0069]
[0070]
计算粒子的剩余权重和所有粒子复制次数总数l。
[0071][0072][0073]
复制剩余权重最大的m-l个粒子，完成重采样，即可有效减少权重较低的优质粒子被删除。
[0074]
步骤3：通过子图系统完成子图维护；
[0075]
为了保证全局优化中拥有足够多约束关系，机器人定位和建图系统采用子图系统，使用双线程同时维护两张活跃子图，一帧激光扫描帧数据将通过粒子滤波器同时在两张活跃子图内进行匹配。当一张活跃子图中的激光帧数量达到设定阈值ρ
max
，该子图停止维护，从活跃子图转为历史子图，并将最后一帧激光帧位姿作为新生成的活跃子图的起始位姿。系统初始化时仅维护一张活跃子图，当该子图中激光帧数量达到ρ
max
/2时，开启新线程，生成并维护第二张活跃子图，该过程如图5所示。
[0076]
步骤4：回环检测，使用csm匹配与分支定界加速获得回环约束；
[0077]
回环检测使用scan-to-map匹配方式，并对每一帧激光帧进行搜索匹配。当存在历史子图m
his
与当前帧节点x
now
相对距离低于设定阈值，则进行扫描匹配。为了避免因搜索全部地图造成的计算资源浪费，限制搜索窗口w为以当前帧节点x
now
位姿ξ为中心，边长为d的正方形区域。
[0078]
扫描匹配算法使用csm帧匹配算法，假设当前帧节点x
now
的回环约束位姿ξ'一定存在搜索窗口w内。在w内以步长λ＝(δx,δy,δθ)进行暴力搜索，历遍w内的所有位姿。具体至每一个位姿，当前帧的雷达位姿用转移矩阵t
ξ
＝(r
ξ
,t
ξ
)表示，将当前帧的雷达观测数据hk投影至局部子图m
his
上，即可用式(14)表示。
[0079]
t
ξ
·hk
＝r
ξ
·hk
t
ξ
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0080]
使用score()函数累加所有被激光击中栅格的概率值，即为该位姿的得分。当得分超过设定阈值后，则认为构成回环，得分最高的位姿为当前帧x
now
在历史子图m
his
中的观测位姿(即回环约束位姿)ξ'，如式(15)所示。
[0081]
其中
[0082]
虽然暴力搜索匹配不会存在陷入局部极值，但其巨大的计算量会对系统的实时性造成影响。以搜索窗口w大小为10m
×
10m，步长λ＝(0.01m,0.01m,1
°
)，方向角搜索范围30
°
为例，则需进行103×
103×
30＝3
×
107次计算。为了减少计算量，使用分支定界法进行加速计算。
[0083]
分支定界法将搜索范围内的地图进行平滑模糊处理，使其分辨率从r平滑模糊到21r、22r、23r至2dr。从分辨率最低的地图使用式(15)进行计算，保留得分最大的区域，剪去低分区域。将得分最高的区域作为新的搜索窗口，并提高一倍分辨率继续搜索匹配。不断循环此过程，直至搜索分辨率达到最高分辨率为止，如图6所示。
[0084]
步骤5：根据计算得到的里程计约束和约束构建模块得到的回环约束构建图优化
问题，最小二乘求解最优位姿和地图。
[0085]
将一个激光帧位姿和一个子图位姿作为顶点x
β
和m
α
，顶点间的位姿约束作为边，进行全局优化。具体至误差函数的构建：通过步骤4回环检测匹配计算得到顶点x
β
和m
α
的回环相对位姿，即观测值z'
αβ
＝(t
αβ
,θ
αβ
)，转换矩阵z'
αβ
＝v2t(z'
αβ
)，其中t为平移向量，θ为旋转量。通过步骤1和步骤2得到顶点x
β
和m
α
的位姿为ξ
β
和ξ
α
，使用位姿ξ
β
和ξ
α
计算得到一个里程计相对位姿，即预测值z
αβ
＝v2t(ξ
α
)
t
v2t(ξ
β
)。
[0086]
v2t表示该点在全局地图坐标系下的转换矩阵。因此误差函数设为式(16)。
[0087]eαβ
＝t2v(z
αβ-1
z'
αβ
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0088]
该误差表示顶点m
α
和x
β
通过回环检测得到的回环相对位姿与通过局部slam模块得到的里程计相对位姿的相对位姿。
[0089]
其中r为通过旋转量θ得到的旋转矩阵,t为平移向量。
[0090]
计算得
[0091]
故误差函数的矩阵表达式为式(17)。
[0092][0093]
从全局考虑，将全部激光帧全局位姿记作x＝{ξ
β
},β＝1,2,
…
,n，将全部子图全局位姿记作m＝{ξ
α
},α＝1,2,
…
,s，并通过粒子滤波算法产生的局部位姿ξ
αβ
进行约束。则全局优化的非线性最小二乘表达式为式(18)。
[0094][0095]
其中，e2(ξ
α
,ξ
β
；ω
αβ
,ξ
αβ
)＝e(ξ
α
,ξ
β
；ξ
αβ
)
t
ω
αβ
e(ξ
α
,ξ
β
；ξ
αβ
)，e为使用式(17)构建的误差函数，ω
αβ
为高斯分布的信息矩阵，表示对误差每一项的重视程度。函数ρ为huber损失函数，用于惩罚误差过大的项。
[0096]
将所有通过激光帧间匹配得到的顶点和边输入谷歌开源ceres库，求解该优化问题，即可得到各顶点的最优位姿。