给定距离变换找到最佳路径

时间:2014-01-19 09:49:57

标签: algorithm search image-processing planning motion-planning

给定带有障碍物的地图的距离变换,如何获得从起始像素到目标像素的最低成本路径?距离变换图像具有距原始地图的最近障碍物的距离(欧几里德),在每个像素中,即如果在原始地图中,像素(i,j)是向左3个像素,距离向下2个像素。障碍物,然后在距离变换中像素将具有sqrt(4 + 9)= sqrt(13)作为像素值。因此,较暗的像素表示与障碍物的接近,较亮的值表示它们远离障碍物。

我想使用此距离变换提供的信息规划从给定开始到目标像素的路径,并优化路径的成本,还有另一个约束,即路径永远不会到达小于'的像素x'像素远离障碍物。

我该怎么做?

P.S。关于算法(或一些代码)的一些描述会很有帮助,因为我不熟悉规划算法。

3 个答案:

答案 0 :(得分:4)

我在标题为

的章节的附录I中找到了一个算法

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

我在Google books中完全看到了这一章,而这本书是

郑,Yuang F.(编辑)。 移动漫游器的最新趋势。 World Scientific,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等人的书中的{em>计算机图形学:原理与实践 - 第三版中的Chapter 15。为了保存图像。

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

请注意,在我看来,该算法无法满足您的额外约束:

  

路径永远不会到达小于'x'像素的像素   来自障碍。

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

enter image description here

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

enter image description here

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

enter image description here

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

enter image description here

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

enter image description here

在看了

之后,我找到了Jarvis的文章

CHIN, Yew Tuck, et al. Vision guided agv using distance transform. In: Proceedings of the 32nd ISR (International Symposium on Robotics). 2001. p. 21.

<强>更新

Jarvis'算法在以下论文中进行了回顾,其中作者声明:

  

由于通过仅在邻居之间本地选择来找到路径   细胞,获得的路径可能是次优的

ELIZONDO-LEAL, Juan Carlos; PARRA-GONZÁLEZ, Ezra Federico; RAMÍREZ-TORRES, José Gabriel. The Exact Euclidean Distance Transform: A New Algorithm for Universal Path Planning. Int J Adv Robotic Sy, 2013, 10.266.

答案 1 :(得分:0)

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

DE BERG,Mark,et al。 计算几何。施普林格柏林海德堡,2008年。

标题为"Visibility Graphs - Finding the Shortest Route" and it is freely available at the publisher site

本章介绍了如何从所谓的可见性图开始计算欧几里德最短路径。从障碍物集开始计算可见性图,每个障碍物被描述为多边形。

然后找到欧几里德最短路径,将最短路径算法(如Dijkstra算法)应用于可见性图。

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

答案 2 :(得分:0)

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

通常目标像素的值为零,是任何可通行模式的最低数量。

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