基于Voronoi图的移动机器人SLAM算法(5)

背景点集合；p为栅格地图中的点；εri表示栅格地图中纵坐标为ri的点的集合；F Uxp表示将点p添加到集合F中，然后将集合F按横坐标x由小到大排序；de函数的输出是两输入点的欧几里得距离。函数Function delete_sites()实现将候选点集合Cri中直线y=ri未穿过的Voronoi区域的背景点删除，以此减少计算欧几里得距离的背景点个数，其原理是本文中的定理1。在遍历过程中，此算法可减少栅格地图中纵坐标为ri及大于ri的点的最近邻候选背景点个数，以此缩短计算所有点最近邻背景点的总时间。

4 实验结果

实验环境：装有SICK公司LMS200激光测距仪的移动机器人SunⅡ(如图4所示)，Intel(R) Core(TM) Duo CPU T2450主频 2.00GHz，内存为2.00GB的计算机，Matlab软件平台。

测试数据：移动机器人在实验室大厅(如图5所示)采集到的激光数据。 将

未

改

进

与

改

进

后

的

最

近

邻

点

算

法

分

别

应

用

到

图4 移动机器人SunⅡ Fig.4 Mobile robot SunⅡ