python - Python 中的直线网格单元路径平滑

Question

我正在 ROS 中构建导航节点。我是 ROS 和 Python (2.7) 的新手。我有一个网格单元格，从中我得到两个单元格(开始到目标)之间的最短路径(在单元格坐标中)。

我的问题是:什么是一种快速优雅的方式来平滑我的道路？

平滑路径意味着如果我的移动方向在两个单元格之间没有变化，我可以省略这些单元格之间的所有单元格。

我的意思是，例如，如果我的路径是 (x,y):[(1,1),(1,2),(1,3),(2,3),(2,4 ),(2,5)]

我的顺利路径应该是:[(1,1),(1,3),(2,3),(2,5)]

请注意，例如在 (1,1) 和 (1,3) 之间，方向不会改变，因此我们删除 (1 ,2).

Answer 1

您的问题可以改写为:对于每个坐标，如果下一个坐标紧随其后，则将其删除。

我假设“移动方向不变”的意思是:如果您要通过前两个点画一条线，则第三个点将在同一条线上的第一个点的第二个点之后。

所以，[(1,1), (2,2), (3,3)] 变成了 [(1,1), (3,3)] 和 [(1,1), (3,2), (5,3)] 变成 [(1,1), (5,3)] .

path = [(1, 1), (1, 2), (1, 3), (2, 3), (2, 4), (2, 5)]


def behind(x, y, z):
    # if the first two points are on a vertical line
    if x[0] == y[0]:
        # if the third point is one the same vertical line?
        if y[0] == z[0]:
            # then they still need to be in the correct order
            return x[1] > y[1] > z[1] or x[1] < y[1] < z[1]
        else:
            return False
    elif y[0] == z[0]:
        return False
    # none of the points are directly above each other, calculate and compare direction
    return (x[1] - y[1]) / (x[0] - y[0]) == (y[1] - z[1]) / (y[0] - z[0])


triples = list(zip(path[:-2], path[1:-1], path[2:]))
smooth_path = path[:1] + [
    y for x, y, z in triples
    if not behind(x, y, z)
] + path[-1:]

print(smooth_path)

第一个和最后一个坐标总是在，其余的，这个例子生成一组三元组，彼此的坐标在中间，然后检查它们是否完美对齐 - 如果是这样，它不包括坐标，否则它确实包含它。