2

我现在正在为自主机器人研究粒子过滤器，并且无法生成预期的距离测量值来过滤粒子。我有一张用作地图的图像。每个像素代表环境中的某个缩放区域。机器人可以占据的空间是白色的，墙壁是黑色的，环境外部的区域是灰色的。

如果您不熟悉什么是粒子过滤器，我的 python 代码将创建预定数量的随机猜测，以确定它在空白空间中的位置 (x,y,theta)。然后它将使用超声波传感器以多个角度测量到最近墙壁的距离。该脚本会将这些测量值与每个猜测位置/方向的每个角度预期的测量值进行比较。与实际测量值最接近的那些将幸免于难，而不太可能正确的猜测将被消除。

我的问题是在给定角度找到最近的墙。假设传感器在 60° 处测量。对于每个猜测，我需要调整角度以考虑猜测的机器人方向，然后测量该角度与墙壁的距离。在 x 方向上很容易找到最近的墙：

from PIL import Image
#from matplotlib._png import read_png
from matplotlib.pyplot import *
mapp = Image.open("Map.png")
pixels = mapp.load()
width = mapp.size[0]
height = mapp.size[1]
imshow(mapp)

pixelWidth = 5

for x in range(width):
    if mapp.getpixel((x, 100)) == (0,0,0,255): #Identify the first black pixel
        distance = x*pixelWidth self.x

问题是我不能告诉脚本以 60°、23° 或任何角度一次搜索一个像素。现在我能想到的最好的办法就是先往x方向走，找到一个黑色像素，然后用角度的切线来确定我需要向上或向下移动多少像素，但是有明显的问题这主要与角落有关，我无法想象需要多少 if 语句来解决它。还有其他解决方案吗？

4

1 回答 1

0

好的，我想我找到了我正在尝试做的一个很好的近似值，但我仍然想听听其他人是否有更好的解决方案。通过检查我在每个像素移动之间实际经过的角度的切线，我可以决定是在 x 方向还是在 y 方向移动一个像素。

for i in range(len(angles)):
    angle = self.orientation+angles[i]
       if angle > 360:
           angle -= 360
        x = self.x
        y = self.y
        x1 = x
        y1 = y
        xtoy_ratio = tan(angle*math.pi/180)
        if angle < 90:
            xadd = 1
            yadd = 1
        elif 90 < angle < 180:
            xadd = -1
            yadd = 1
        elif 180 < angle < 270:
            xadd = -1
            yadd = -1
        else:
            xadd = 1
            yadd = -1
        while mapp.getpixel(x,y) != (0,0,0,255):
            if (y-y1)/(x-x1) < xtoy_ratio:
                y += yadd
            else:
                x += xadd
        distance = sqrt((y-y1)^2+(x-x1)^2)*pixel_width

当然，这种方法的准确性很大程度上取决于每个像素所代表的实际长度。只要 pixel_width 很小，精度就会很好，但如果不是，它通常会在自我纠正之前走得很远。

正如我所说，我欢迎其他答案。

谢谢

于 2014-02-13T05:59:53.300 回答