给定带有障碍物的地图的距离变换,如何获得从起始像素到目标像素的最低成本路径?距离变换图像具有距原始地图的最近障碍物的距离(欧几里德),在每个像素中,即如果在原始地图中,像素(i,j)是向左3个像素,距离向下2个像素。障碍物,然后在距离变换中像素将具有sqrt(4 + 9)= sqrt(13)作为像素值。因此,较暗的像素表示与障碍物的接近,较亮的值表示它们远离障碍物。
我想使用此距离变换提供的信息规划从给定开始到目标像素的路径,并优化路径的成本,还有另一个约束,即路径永远不会到达小于'的像素x'像素远离障碍物。
我该怎么做?
P.S。关于算法(或一些代码)的一些描述会很有帮助,因为我不熟悉规划算法。
答案 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:
以下png图像是起点的图像,程序生成的start.ppm
图像通过Gimp导出为png:
以下png图像是终点的图像,程序生成的goal.ppm
图像通过Gimp导出为png:
以下png图像是计算路径的图像,程序生成的path.ppm
图像通过Gimp导出为png:
以下png图像是距离变换的图像,程序生成的cell.ppm
图像通过Gimp导出为png:
在看了
之后,我找到了Jarvis的文章<强>更新
Jarvis'算法在以下论文中进行了回顾,其中作者声明:
由于通过仅在邻居之间本地选择来找到路径 细胞,获得的路径可能是次优的
答案 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。 我用于边缘的值相同。另一种方法是围绕具有非常高起始值的障碍 - 路径不能保证接近,但算法将尝试避开障碍物附近的路径。