﻿ 基于RBPF-SLAM算法的研究与实现
 计算机系统应用  2019, Vol. 28 Issue (7): 169-173 PDF

1. 中国科学院大学, 北京 100049;
2. 中国科学院 沈阳计算技术研究所 高档数控国家工程研究中心, 沈阳 110168;
3. 沈阳高精数控智能技术股份有限公司, 沈阳 110168

Research and Implementation of RBPF-SLAM Algorithm
WANG Hui1,2, WANG Pin2,3
1. University of Chinese Academy of Sciences, Beijing 100049, China;
2. National Engineering Research Center for High-end CNC, Shenyang Institute of Computing Technology, Chinese Academy of Sciences, Shenyang 110168, China;
3. Shenyang High Precision Digital Control Intelligent Technology Co. Ltd., Shenyang 110168, China
Foundation item: National Science and Technology Major Program (2018ZX04035001)
Abstract: The Simultaneous Localization And Mapping (SLAM) is a difficult problem in the field of robotics. Rao-Blackwellized particle filters algorithm is widely used to solve this problem. In the traditional implementation, the proposed distribution with high error will calculate a large number of sampled particles to fit the target distribution. Frequent resampling steps will lead to gradual dissipation of particles and waste a lot of computing resources. In this study, the motion model and observation information are combined to optimize the proposed distribution, reduce the number of sampled particles, and the adaptive resampling method is introduced to reduce the steps of resampling. In the implementation of the algorithm, the tree data structure is used to store the environment map. The experimental results show that the improved algorithm can significantly improve the computational efficiency, reduce the storage consumption, and build more accurate map.
Key words: particle filter     Simultaneous Localization And Mapping (SLAM)     proposal distribution     target distribution     adaptive resampling

1 基于粒子滤波的RBPF算法原理

 ${Y_t} = \{ x_{1:t}^1,x_{1:t}^2,x_{1:t}^3, \cdot \cdot \cdot ,x_{1:t}^m\}$ (1)

 $x_{1:t}^k = \{ {(x,y,\theta )^T},(\mu _1^k,\Sigma _1^k),(\mu _2^k,\Sigma _2^k), \cdot \cdot \cdot ,(\mu _N^k,\Sigma _N^k)\}$ (2)

RBPF粒子滤波器可以表示为如下因式分解:

 $\begin{gathered} p({x_{1:t}},m|{z_{1:t}},{u_{1:t - 1}}) = \\ p(m|{x_{1:t}},{z_{1:t}}) \bullet p({x_{1:t}}|{z_{1:t}},{u_{1:t - 1}}) \\ \end{gathered}$ (3)

1)采样: 根据建议分布从t–1时刻粒子集合 $\scriptstyle {Y_{t - 1}}$ 采样获取t时刻粒子的先验分布集合 $\scriptstyle Y_t^ -$ , 建议分布使用运动模型 $\scriptstyle p({x_t}|{x_{t - 1}},{u_t})$ .

2)计算权重: 计算每个粒子的权重, 权重w反映了建议分布与目标后验分布的差距.

3)重采样: 根据每个粒子的权重进行重采样, 从临时粒子集合 $\scriptstyle Y_t^ -$ 中抽取更换M个粒子, 结果产生的M个粒子形成新的最终粒子集 $\scriptstyle {Y_t}$ .

4)更新地图: 对于每个粒子根据传感器观测数据 $\scriptstyle {z_{1:t}}$ 和机器人当前位姿 $\scriptstyle {x_t}$ 更新地图中的每个特征 $\scriptstyle m_t^i$ .

 $\qquad\qquad\;\;\;\;\;\;\;\;\qquad\scriptstyle {m^i} \sim p({m^i}|{x_t},{z_{1:t}})\quad\quad\qquad\;\;\;\;\;\qquad\qquad(4)$

2 RBPF-SLAM的改进算法

 $w_t^i \propto p({z_t}|x_t^i,m_{t - 1}^i) \cdot w_{t - 1}^i$ (5)

2.1 改进建议分布

 $\begin{gathered} p({x_t}|m_{t - 1}^i,x_{t - 1}^i,{z_t},{u_{t - 1}}) = \frac{{p({z_t}|m_{t - 1}^i,{x_t}) \cdot p({x_t}|x_{t - 1}^i,{u_{t - 1}})}}{{p({z_t}|m_{t - 1}^i,x_{t - 1}^i,{u_{t - 1}})}} \\ \end{gathered}$ (6)

 $w_t^i \propto w_{t - 1}^i \cdot p({z_t}|m_{t - 1}^i,x_{t - 1}^i,{u_{t - 1}})$ (7)

 $x_t^{i - } = g(x_{t - 1}^i,{u_t})$ (8)

 $\overset{\sim }{\mathop{x}}_{t}^{i} = \arg {\max _x}p(x|m_{t - 1}^i,{z_t},x_t^{i - })$ (9)

 ${x_k}\sim\{ {x_j}|\;\;\; |{x_j} - \overset{\sim }{\mathop{x}}_{t}^{i}|\;\;\; < \;\;\; \Delta \}$ (10)

${x_j} \in \{ {x_1},{x_2}, \cdots ,{x_k}\}$ , 则

 $\eta _t^i = \sum\limits_{j = 1}^k {p({z_t}} |m_{t - 1}^i,{x_j}) \cdot p({x_j}|x_{t - 1}^i,{u_{t - 1}})$ (11)
 $\mu _t^i = \frac{1}{{\eta _t^i}}\sum\limits_{j = 1}^k {{x_j}} \cdot p({z_t}|m_{t - 1}^i,{x_j}) \cdot p({x_j}|x_{t - 1}^i,{u_{t - 1}})$ (12)
 $\begin{gathered} \Sigma _t^i = \frac{1}{{\eta _t^i}}\sum\limits_{j = 1}^k {({x_j}} - u_t^i) \cdot {({x_j} - u_t^i)^T} \cdot \\ p({z_t}|m_{t - 1}^i,{x_j}) \cdot p({x_j}|x_{t - 1}^i,{u_{t - 1}}) \\ \end{gathered}$ (13)

2.2 自适应重采样

 ${N_{eff}} = \frac{1}{{\sum\nolimits_{i = 1}^M {{{\mathop {(w}\limits^ \sim }^i}{)^2}} }}$ (14)

2.3 改进算法流程描述

1)使用机器人运动模型计算粒子的期望位姿 $x_t^{i - }$ .

2)根据扫描匹配算法估计具有最大观测后验概率的机器人目标位姿 $\overset{\sim }{\mathop{x}}\,_{t}^{i}$ .

3)计算建议分布: 根据公式(11)~(13)计算建议分布 $\pi \sim N(\mu _t^i,\Sigma _t^i)$ .

4)采样: 根据所求的建议分布进行粒子采样.

5)计算权重: 根据公式 $w_t^i \propto w_{t - 1}^i \cdot \eta _t^i$ 计算粒子权重.

6)依据公式(14)计算当前粒子集的有效数目, 当 ${N_{eff}}$ <N/2时需要重采样.

7)根据传感器观测数据更新地图.

3 算法的高效实现

 图 1 改进算法流程图

 图 2 特征数M=8的粒子地图树形结构

4 实验结果

 图 3 3维可视化工具RVIZ

 图 4 室内地图效果对比

 图 5 传统算法与改进算法内存使用对比

5 总结

 [1] Dissanayake G, Durrant-Whyte H, Bailey T. A computationally efficient solution to the simultaneous localisation and map building (SLAM) problem. Proceedings of 2000 IEEE International Conference on Robotics and Automation. San Francisco, CA, USA. 2000. 1009–1014. [2] Doucet A, de Freitas JFG, Murphy K, et al. Rao-blackwellized partcile filtering for dynamic Bayesian networks. Proceedings of Conference on Uncertainty in Artificial Intelligence. Stanford, CA, USA. 2000. 176–183. [3] Cox IJ, Wilfong GT. Autonomous Robot Vehicles. New York: Springer, 1990. 167–193. [4] Montemerlo M, Thrun S, Koller D, et al. FastSLAM: A factored solution to the simultaneous localization and mapping problem. Proceedings of the 18th National Conference on Artificial Intelligence. Edmonton, Canada. 2002. 593–598. [5] Montemerlo M, Thrun S. Simultaneous localization and mapping with unknown data association using FastSLAM. Proceedings of 2003 IEEE International Conference on Robotics and Automation. Taiwan, China. 2003. 1985–1991. [6] Paskin MA. Thin junction tree filters for simultaneous localization and mapping. Proceedings of the 18th International Joint Conference on Artificial Intelligence. Acapulco, Mexico. 2003. 1157–1164. [7] van der Merwe R, Doucet A, de Freitas N, et al. The unscented particle filter. CUED/FINFENG/TR380, Cambridge: Cambridge University, 2000. 8. [8] 彭胜军, 马宏绪. 移动机器人导航空间表示及SLAM问题研究. 计算机仿真, 2006, 23(8): 1-4. DOI:10.3969/j.issn.1006-9348.2006.08.001