机器人学在Python中的实现（5）：高斯栅格地图＆K均值

Hello，大家好！我是MPIG2018级研究生汪国会。在前面的章节中已经学习了机器人定位的相关算法，今天我要开始为大家介绍高斯栅格地图及k均值聚类这两部分内容。

(一)高斯栅格地图

首先我们来学习三个概念，：

1，栅格地图

2，高斯分布

3，热图

(1)栅格地图

机器人描述环境，认识环境的过程主要就是依靠地图。它利用环境地图来描述其当前环境信息，并随着使用的算法和传感器采用不同的地图来描述。

机器人学中地图的表示方法有四种：特征地图，拓扑地图，直接表征法和栅格地图。

栅格地图是把环境划分为一系列栅格，其中每个栅格给定一个特定值，表示该栅格被占据的概率。

(2)高斯分布

若随机变量X服从一个位置参数为μ，尺度参数为σ的概率分布，且其概率密度函数为

随机变量服从的分布就称为高斯分布

上图为高斯分布的概率密度函数图像。横坐标对应的是随机变量x，不同的x对应在图像中的面积代表随机变量的概率。当x为正值时，x越大，面积越大，即概率越大。

(3)热图

热图是将数值的大小以定义的颜色深浅表示出来。

比如拿到一批离散的数据，想看一下在哪个点数值比较大，在哪个点值比较低，你想把这样的一个值的变化，用颜色来区分出来，就可以用热图来实现。

w.heatmap(data,vmin,vmax,cmap)

data：矩阵数据集，需要显示的数据

cmap是从数字到颜色的映射，取值是matplotlib包里的colormap名称或颜色对象，或者表示颜色列表

vmax，vmin:分别是热力图的颜色取值最大和最小范围，默认是根据data数据表里的取值确定

将上面三个概念整合到一起，就得到了高斯栅格地图。

第一步,根据位姿，得到栅格范围(这里我们主要讨论的是地图构建，不考虑定位问题，因此在机器人位姿已知的条件下进行)

第二步，计算每个栅格到位姿的距离，将距离作为高斯分布的随机变量，得到每个栅格的概率

第三步，将概率通过热图可视化

具体在代码中的实现如下：

在主函数随机产生4个位姿的坐标，STD为高斯分布的标准差σ

根据位姿，得出最大值，最小值作为以位姿中心延伸的栅格的范围

计算每个栅格到位姿的距离d，将d作为随机变量带入高斯分布中，由于概率为高斯分布的密度函数的面积，d越大则概率越大，在将概率反映在热图中，概率越大颜色越深。但高斯栅格地图希望离位姿近的栅格用深色表示，因此需要在给每个栅格赋值时用1-（高斯分布概率）

先将之前得到的栅格范围通过np.mgrid()进行分片，得到一个一个小的栅格，再将每个栅格的赋值通过热图用颜色深浅表示出来

仿真结果如下

(二)K均值聚类

回顾一下K均值的算法流程

1，随机创建K个质心,

2，遍历数据集的每个数据点，计算其到每个质心的相似度，把每个数据点分3，配到距其最近的质心那一类,对每一簇，重新计算每个簇新的质心

4，迭代2,3，直到收敛

在主函数中定义了cx，cy的坐标，规定了分为两簇，这里的代码没有定义变量K，不能变动簇的个数

由于仿真结果为动图，每张图产生新的cx，cy通过update_position()得到

根据cx,cy，产生我们进行聚类的数据点

根在k均值函数中，通过调用calc_centroid()产生聚类的质心，再调用update_cluster()计算每个点到质心的距离

将数据点分到最近的质心那一簇中

仿真结果中，圆点表示的为根据起始cx，cy得到的每张图的cx，cy，并不是簇的质心。所以这里K均值是将目标点附近很小区域内的点标注出来。与之前介绍的K均值不同的就是，这里需要根据随机生成的目标点产生聚类的数据点，再按照算法流程聚类。

想了解更详细的内容，就来看看我的Presantation吧：