PythonRobotics机器人算法-映射算法之高斯网格映射(Gaussian grid map)


1.高斯网格映射(Gaussian grid map)实例

下面展示一下基于python实现的高斯网格映射。

(1)定义初始参数

1
2
3
4
5
6
7
8
import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm

EXTEND_AREA = 10.0  # [m] grid map extention length

show_animation = True

(2)算法过程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
def generate_gaussian_grid_map(ox, oy, xyreso, std):
    minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
    gmap = [[0.0 for i in range(yw)] for i in range(xw)]
    for ix in range(xw):
        for iy in range(yw):
            x = ix * xyreso + minx
            y = iy * xyreso + miny
            #Search minimum distance
            mindis = float("inf")
            for (iox, ioy) in zip(ox, oy):
                d = math.sqrt((iox - x)* * 2 + (ioy - y)* * 2)
                if mindis >= d:
                    mindis = d
            pdf = (1.0 - norm.cdf(mindis, 0.0, std))
            gmap[ix][iy] = pdf
    return gmap, minx, maxx, miny, maxy

def calc_grid_map_config(ox, oy, xyreso):
    minx = round(min(ox) - EXTEND_AREA / 2.0)
    miny = round(min(oy) - EXTEND_AREA / 2.0)
    maxx = round(max(ox) + EXTEND_AREA / 2.0)
    maxy = round(max(oy) + EXTEND_AREA / 2.0)
    xw = int(round((maxx - minx) / xyreso))
    yw = int(round((maxy - miny) / xyreso))
    return minx, miny, maxx, maxy, xw, yw

(3)结果可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
    x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso),
                    slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)]
    plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues)
    plt.axis("equal")

def main():
    print(_ _file__ + " start!!")
    xyreso = 0.5  # xy grid resolution
    STD = 5.0  # standard diviation for gaussian distribution
    for i in range(5):
        ox = (np.random.rand(4) - 0.5) * 10.0
        oy = (np.random.rand(4) - 0.5) * 10.0
        gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map(ox, oy,
                                                                 xyreso, STD)
        if show_animation:
            plt.cla()
            draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso)
            plt.plot(ox, oy, "xr")
            plt.plot(0.0, 0.0, "ob")
            plt.pause(1.0)

if _ _ name__ == '_ _main__':
    main()

执行结果如下。下面为二维高斯网格映射的实例

参考文献

  1. https://en.wikipedia.org/wiki/Gaussian_grid
  2. https://zh.wikipedia.org/wiki/高斯映射
  3. https://pythonrobotics.readthedocs.io/en/latest/
  4. https://github.com/AtsushiSakai/PythonRobotics/
  5. PythonRobotics: a Python code collection of robotics algorithms

进化学习团队将会根据大家意见和建议持续修改、维护与更新。转载请注明出处(进化学习: https://www.evolutionarylearn.com/paper/ra-pythonrobotics-mapping-ggm/)。

赞赏

微信赞赏支付宝赞赏

Have any Question or Comment?

发表评论

电子邮件地址不会被公开。 必填项已用*标注

热门主题 & 页面

分类目录

博客统计

无点击次数。