2

Given a distance transform of a map with obstacles in it, how do I get the least cost path from a start pixel to a goal pixel? The distance transform image has the distance(euclidean) to the nearest obstacle of the original map, in each pixel i.e. if in the original map pixel (i,j) is 3 pixels away to the left and 2 pixels away to the down of an obstacle, then in the distance transform the pixel will have sqrt(4+9) = sqrt(13) as the pixel value. Thus darker pixels signify proximity to obstacles and lighter values signify that they are far from obstacles.

I want to plan a path from a given start to goal pixel using the information provided by this distance transform and optimize the cost of the path and also there is another constraint that the path should never reach a pixel which is less than 'x' pixels away from an obstacle.

How do I go about this?

P.S. A bit of description on the algorithm (or a bit of code) would be helpful as I am new to planning algorithms.

4

3 回答 3

4

我在标题为章节的附录 I 中找到了一个算法

贾维斯,雷。基于距离变换的机器人导航路径规划。移动机器人的最新趋势, 1993,11:3-31。

那一章在谷歌书籍中对我来说是完全可见的,这本书是

郑元芳(主编)。移动机器人的最新趋势。世界科学,1993 年。

该算法的 C++ 实现如下:

#include <vector>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <cassert>
#include <sstream>

/**
Algorithm in the appendix I of the chapter titled

JARVIS, Ray. Distance transform based path planning for robot navigation. *Recent trends in mobile robots*, 1993, 11: 3-31.

in the book

ZHENG, Yuang F. (ed.). *Recent trends in mobile robots*. World Scientific, 1993.

See also http://stackoverflow.com/questions/21215244/least-cost-path-using-a-given-distance-transform
*/

template < class T >
class Matrix
{
private:
    int    m_width;
    int    m_height;
    std::vector<T> m_data;

public:
    Matrix(int width, int height) :
        m_width(width), m_height(height), m_data(width *height) {}
    int width() const
    {
        return m_width;
    }
    int height() const
    {
        return m_height;
    }
    void set(int x, int y, const T &value)
    {
        m_data[x + y * m_width] = value;
    }
    const T &get(int x, int y) const
    {
        return m_data[x + y * m_width];
    }
};

float distance( const Matrix< float > &a, const Matrix< float > &b )
{
    assert(a.width() == b.width());
    assert(a.height() == b.height());
    float r = 0;
    for ( int y = 0; y < a.height(); y++ )
    {
        for ( int x = 0; x < a.width(); x++ )
        {
            r += fabs(a.get(x, y) - b.get(x, y));
        }
    }
    return r;
}


int PPMGammaEncode(float radiance, float d)
{
    //return int(std::pow(std::min(1.0f, std::max(0.0f, radiance * d)),1.0f / 2.2f) * 255.0f);
    return radiance;
}

void PPM_image_save(const Matrix<float> &img, const std::string &filename, float d = 15.0f)
{
    FILE *file = fopen(filename.c_str(), "wt");

    const int m_width  = img.width();
    const int m_height = img.height();
    fprintf(file, "P3 %d %d 255\n", m_width, m_height);

    for (int y = 0; y < m_height; ++y)
    {
        fprintf(file, "\n# y = %d\n", y);
        for (int x = 0; x < m_width; ++x)
        {
            const float &c(img.get(x, y));
            fprintf(file, "%d %d %d\n",
                    PPMGammaEncode(c, d),
                    PPMGammaEncode(c, d),
                    PPMGammaEncode(c, d));
        }
    }
    fclose(file);
}

void PPM_image_save(const Matrix<bool> &img, const std::string &filename, float d = 15.0f)
{
    FILE *file = fopen(filename.c_str(), "wt");

    const int m_width  = img.width();
    const int m_height = img.height();
    fprintf(file, "P3 %d %d 255\n", m_width, m_height);

    for (int y = 0; y < m_height; ++y)
    {
        fprintf(file, "\n# y = %d\n", y);
        for (int x = 0; x < m_width; ++x)
        {
            float v = img.get(x, y) ? 255 : 0;
            fprintf(file, "%d %d %d\n",
                    PPMGammaEncode(v, d),
                    PPMGammaEncode(v, d),
                    PPMGammaEncode(v, d));
        }
    }
    fclose(file);
}

void add_obstacles(Matrix<bool> &m, int n, int avg_lenght, int sd_lenght)
{
    int side = std::max(3, std::min(m.width(), m.height()) / 10);
    for ( int y = m.height() / 2 - side / 2; y < m.height() / 2 + side / 2; y++ )
    {
        for ( int x = m.width() / 2 - side / 2; x < m.width() / 2 + side / 2; x++ )
        {
            m.set(x, y, true);
        }
    }

    /*
        for ( int y = m.height()/2-side/2; y < m.height()/2+side/2; y++ ) {
          for ( int x = 0; x < m.width()/2+side; x++ ) {
            m.set(x,y,true);
          }
        }
        */

    for ( int y = 0; y < m.height(); y++ )
    {
        m.set(0, y, true);
        m.set(m.width() - 1, y, true);
    }

    for ( int x = 0; x < m.width(); x++ )
    {
        m.set(x, 0, true);
        m.set(x, m.height() - 1, true);
    }
}

class Info
{
public:
    Info() {}
    Info(float v, int x_o, int y_o): value(v), x_offset(x_o), y_offset(y_o) {}
    float value;
    int x_offset;
    int y_offset;

    bool operator<(const Info &rhs) const
    {
        return value < rhs.value;
    }
};

void next(const Matrix<float> &m, const int &x, const int &y, int &x_n, int &y_n)
{
    //todo: choose the diagonal adiacent in case of ties.
    x_n = x;
    y_n = y;

    Info neighbours[8];
    neighbours[0] = Info(m.get(x - 1, y - 1), -1, -1);
    neighbours[1] = Info(m.get(x  , y - 1), 0, -1);
    neighbours[2] = Info(m.get(x + 1, y - 1), +1, -1);

    neighbours[3] = Info(m.get(x - 1, y  ), -1, 0);
    neighbours[4] = Info(m.get(x + 1, y  ), +1, 0);

    neighbours[5] = Info(m.get(x - 1, y + 1), -1, +1);
    neighbours[6] = Info(m.get(x  , y + 1), 0, +1);
    neighbours[7] = Info(m.get(x + 1, y + 1), +1, +1);

    auto the_min = *std::min_element(neighbours, neighbours + 8);
    x_n += the_min.x_offset;
    y_n += the_min.y_offset;
}

int main(int, char **)
{
    std::size_t xMax = 200;
    std::size_t yMax = 150;

    Matrix<float> cell(xMax + 2, yMax + 2);


    Matrix<bool> start(xMax + 2, yMax + 2);
    start.set(0.1 * xMax, 0.1 * yMax, true);

    Matrix<bool> goal(xMax + 2, yMax + 2);
    goal.set(0.9 * xMax, 0.9 * yMax, true);

    Matrix<bool> blocked(xMax + 2, yMax + 2);
    add_obstacles(blocked, 1, 1, 1);

    PPM_image_save(blocked, "blocked.ppm");
    PPM_image_save(start, "start.ppm");
    PPM_image_save(goal, "goal.ppm");

    for ( int y = 0; y <= yMax + 1; y++ )
    {
        for ( int x = 0; x <= xMax + 1; x++ )
        {
            if ( goal.get(x, y) )
            {
                cell.set(x, y, 0.);
            }
            else
            {
                cell.set(x, y, xMax * yMax);
            }
        }
    }

    Matrix<float> previous_cell = cell;
    float values[5];
    int cnt = 0;
    do
    {
        std::ostringstream oss;
        oss << "cell_" << cnt++ << ".ppm";
        PPM_image_save(cell, oss.str());
        previous_cell = cell;
        for ( int y = 2; y <= yMax; y++ )
        {
            for ( int x = 2; x <= xMax; x++ )
            {
                if (!blocked.get(x, y))
                {
                    values[0] = cell.get(x - 1, y    ) + 1;
                    values[1] = cell.get(x - 1, y - 1) + 1;
                    values[2] = cell.get(x    , y - 1) + 1;
                    values[3] = cell.get(x + 1, y - 1) + 1;
                    values[4] = cell.get(x    , y    );
                    cell.set(x, y, *std::min_element(values, values + 5));
                }
            }
        }
        for ( int y = yMax - 1; y >= 1; y-- )
        {
            for ( int x = xMax - 1; x >= 1; x-- )
            {
                if (!blocked.get(x, y))
                {
                    values[0] = cell.get(x + 1, y    ) + 1;
                    values[1] = cell.get(x + 1, y + 1) + 1;
                    values[2] = cell.get(x    , y + 1) + 1;
                    values[3] = cell.get(x - 1, y + 1) + 1;
                    values[4] = cell.get(x    , y    );
                    cell.set(x, y, *std::min_element(values, values + 5));
                }
            }
        }
    }
    while (distance(previous_cell, cell) > 0.);

    PPM_image_save(cell, "cell.ppm");

    Matrix<bool> path(xMax + 2, yMax + 2);
    for ( int y_s = 1; y_s <= yMax; y_s++ )
    {
        for ( int x_s = 1; x_s <= xMax; x_s++ )
        {
            if ( start.get(x_s, y_s) )
            {
                int x = x_s;
                int y = y_s;
                while (!goal.get(x, y))
                {
                    path.set(x, y, true);
                    int x_n, y_n;
                    next(cell, x, y, x_n, y_n);
                    x = x_n;
                    y = y_n;
                }
            }
        }
    }

    PPM_image_save(path, "path.ppm");

    return 0;
}

该算法使用简单的 PPM 图像格式,例如Hughes 等人的《计算机图形学:原理与实践 - 第三版》一书的第 15 章。为了保存图像。

该算法从障碍物的图像(blocked)开始,并从中计算距离变换(cell);然后,从距离变换开始,用最速下降法计算最优路径:在变换距离势场中走下坡路。所以你可以从你自己的距离变换图像开始。

请注意,在我看来,该算法不满足您的附加约束:

路径永远不应到达距离障碍物小于“x”像素的像素。

以下 png 图像是障碍物的图像,程序生成的blocked.ppm图像通过 Gimp 导出为 png:

在此处输入图像描述

下面的 png 图像是起点的图像,程序生成的start.ppm图像是通过 Gimp 导出为 png 的:

在此处输入图像描述

下面的 png 图像是终点的图像,程序生成的goal.ppm图像是通过 Gimp 导出为 png 的:

在此处输入图像描述

以下 png 图像是计算路径的图像,程序生成的path.ppm图像通过 Gimp 导出为 png:

在此处输入图像描述

下面的 png 图像是距离变换的图像,程序生成的cell.ppm图像是通过 Gimp 导出为 png 的:

在此处输入图像描述

看了之后我找到了 Jarvis 的文章

下巴,Yew Tuck 等人。使用距离变换的视觉引导 agv。在:第 32 届 ISR(国际机器人研讨会)论文集。2001 页。21.

更新:

Jarvis 的算法在以下论文中进行了回顾,作者指出:

由于路径是通过仅在相邻小区之间进行局部选择来找到的,因此获得的路径可能是次优的

ELIZONDO-LEAL,胡安·卡洛斯;帕拉-冈萨雷斯,以斯拉·费德里科;RAMÍREZ-TORRES,何塞·加布里埃尔。精确欧几里得距离变换:通用路径规划的新算法。Int J Adv Robotic Sy,2013 年,10.266。

于 2014-01-19T15:23:27.800 回答
0

对于基于图形的解决方案,您可以查看本书的第 15 章

德伯格、马克等人。计算几何。施普林格柏林海德堡,2008 年。

标题为“可见性图 - 寻找最短路线”,可在发布者网站上免费获得

本章解释了如何从所谓的可见性图开始计算欧几里得最短路径。可见性图是从一组障碍物开始计算的,每个障碍物被描述为一个多边形。

然后将诸如 Dijkstra 算法之类的最短路径算法应用于可见性图来找到欧几里得最短路径。

在您的距离变换图像中,障碍物由具有零值的像素表示,因此您可以尝试将它们近似为多边形,然后应用引用书中描述的方法。

于 2014-01-22T19:40:17.237 回答
0

在像素的距离变换图中,您选择起始像素,然后选择其值低于起始像素的邻居 - 重复该过程,直到达到目标像素(值为零的像素)。

通常,目标像素的值为零,这是任何可通过模式中的最低值。

通过生成距离变换图使障碍物变大,不能靠近障碍物的问题是银。例如,如果您想要一个灰尘,如果有两个像素到任何屏障 - 只需添加两个像素的屏障值。通常情况下,可以通过的障碍的值是负一。我用于边缘的相同值。另一种方法是用非常高的起始值围绕障碍物 - 不能保证路径接近,但算法将尝试避开障碍物附近的路径。

于 2016-02-16T06:46:45.570 回答