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

if uvx≥vwx

3.2 算法流程[10]

基于Voronoi图的最近邻点算法基本思想是：只将直线y=r(前景点纵坐标)穿过的Voronoi区域的背景点列为最近邻点的候选点，以此提高对批量前景点求最近邻点的效率。具体流程如下：首先对栅格地图进行 y顺序扫描，求出前景点在直线y=r下方的最近邻点，然后再对栅格地图进行 y顺序扫描，求出前景点在直线y=r上方的最近邻点，最后对2次得出的最近欧几里得距离求最小值，以此类推，即可得到整张地图上所有点的最近邻点。基于Voronoi图的最近邻点算法详见函数Algorithm CDT()。

Algorithm CDT() 输入：栅格地图grid

输出：栅格地图中每个点的最近邻距离CDT及对应的背景点集合Closest_p 第1步：对栅格地图进行 y顺序扫描 for i←1,nl do F←Φ;

if i≥2 then Cri ←Lri-1 else Cri ←Φ; end if

for all center p of a cell R∈εri do if R is backgound labeled then

Cri ←Cri Ux{p}; CDT(R)←0; Else

F←F Ux{p}; CDT(R)←∞; end if end for

Lri=delete_sites(Cri ); k←1, l←size(Lri); for all points p∈F do while(k≤l) do d←de(p,Lri[k]); if d<CDT(R) CDT(R)←d; Closest_p(R)←Lri[k]; else break; end if k=k+1; end while end for

end for

第2步：对栅格地图进行 y顺序扫描

在Algorithm CDT()函数中，nl为栅格地图行数，例如，分辨率为100×100的栅格地图，其行数为100；F为前景点集合；Cri为前景点(纵坐标为ri)的最近邻背景点的候选点集合；Lri为直线y=ri 穿过的Voronoi区域的