框架
global_planner规划器的整体框架如下
参考:https://zhuanlan.zhihu/p/46212318
A*算法的原理讲解,A*算法
这个是自己看到的网上很多的一篇。讲的也的确很棒。
Dijkstra算法原理:Dijkstra
由于之前A*和Dijkstra算法基于栅格的路线不会比较光滑,但是在navigation中有基于梯度势场的,这样会使得其比较光滑。所以想对比一下GridPath::getPath 和 GradientPath::getPath这两个程序中算法区别(之前接触较少)
GridPath
首先上代码,行数很少。。
bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
std::pair<float, float> current;
current.first = end_x;
current.second = end_y;
int start_index = getIndex(start_x, start_y);
path.push_back(current);
int c = 0;
int ns = xs_ * ys_;
while (getIndex(current.first, current.second) != start_index) {
float min_val = 1e10;
int min_x = 0, min_y = 0;
for (int xd = -1; xd <= 1; xd++) {
for (int yd = -1; yd <= 1; yd++) {
if (xd == 0 && yd == 0)
continue;
int x = current.first + xd, y = current.second + yd;
int index = getIndex(x, y);
if (potential[index] < min_val) {
min_val = potential[index];
min_x = x;
min_y = y;
}
}
}
if (min_x == 0 && min_y == 0)
return false;
current.first = min_x;
current.second = min_y;
path.push_back(current);
if(c++>ns*4){
return false;
}
}
return true;
}
代码比较简单,主要是两个for循环那一块的内容。对应就是寻找当前点周围8个点位中potential最小的那个,作为该点位的前一个需要经过的点位。注意这个寻找是从目标点到起始点逐个寻找,并且逐个加入到向量最后,这样可能会在移动的时候更方便点位的提取。也是因此,while里的判断条件是:
//不等于起始点的索引,就继续寻找。
while (getIndex(current.first, current.second) != start_index)
最后的一个判断是判断是否溢出的。
if(c++>ns*4){
return false;
}
Gradient_path
这个文件并没找到相关博客,是自己理解的,可能存在问题。若有不同理解可以联系
这个基于梯度的利用的是插补法使路径变得光滑,和ARA*那两种利用预先的光滑路径基元进行拟合不同。原理就是,在当前点和下一个点位的dx和dy方向上加上插值。
有三个主要的函数 setsize, getpath, gradcell.
设置大小的函数就是申请一个指定大小的内存。比较简单
void GradientPath::setSize(int xs, int ys) {
Traceback::setSize(xs, ys);
if (gradx_)
delete[] gradx_;
if (grady_)
delete[] grady_;
//申请gradx_和gray_;
gradx_ = new float[xs * ys];
grady_ = new float[xs * ys];
}
gradcell这个相对重要一些,是计算梯度以及切向信息的。
float GradientPath::gradCell(float* potential, int n) {
//如果这个梯度是增加的,就直接返回。 实际上在getPtah函数调用这个函数的时候
//并没有调用返回值,而是对gradx_和grady_进行使用。增加的梯度相当于向这个方向
//移动会使得potential增加。但是我们应该是让它减小。
if (gradx_[n] + grady_[n] > 0.0) // check this cell
return 1.0;
//判断当前的n是不是越界,之前是否已经检测过了或者是超出了
if (n < xs_ || n > xs_ * ys_ - xs_) // would be out of bounds
return 0.0;
float cv = potential[n];
float dx = 0.0;
float dy = 0.0;
//检测障碍物,如果当前的potential大于阈值,就判定有障碍,向小于阈值的方向移动。
//因此dx 和dy就给定在没有障碍方向的一个固定值 lethal_cost
// check for in an obstacle
if (cv >= POT_HIGH) {
if (potential[n - 1] < POT_HIGH)
dx = -lethal_cost_;
else if (potential[n + 1] < POT_HIGH)
dx = lethal_cost_;
if (potential[n - xs_] < POT_HIGH)
dy = -lethal_cost_;
else if (potential[n + xs_] < POT_HIGH)
dy = lethal_cost_;
}
//如果没有遇到障碍,就计算dx和dy。
else // not in an obstacle
{
// dx calc, average to sides
if (potential[n - 1] < POT_HIGH)
dx += potential[n - 1] - cv;
if (potential[n + 1] < POT_HIGH)
dx += cv - potential[n + 1];
// dy calc, average to sides
if (potential[n - xs_] < POT_HIGH)
dy += potential[n - xs_] - cv;
if (potential[n + xs_] < POT_HIGH)
dy += cv - potential[n + xs_];
}
// normalize 对dx和dy进行归一,并且赋值给gradx_和grady_
float norm = hypot(dx, dy);
if (norm > 0) {
norm = 1.0 / norm;
gradx_[n] = norm * dx;
grady_[n] = norm * dy;
}
return norm;
}
这里有个细节(个人感觉),是在计算dx和dy时,都是先判断n-1的potential.
然后就是主要的getpath函数
首先是一些预处理,设置一些初始值等
std::pair<float, float> current;
int stc = getIndex(goal_x, goal_y);
// set up offset
float dx = goal_x - (int)goal_x;
float dy = goal_y - (int)goal_y;
int ns = xs_ * ys_;
memset(gradx_, 0, ns * sizeof(float));
memset(grady_, 0, ns * sizeof(float));
int c = 0;
最主要的是while循环里的一组 if else
最开始是判断这个点位距离目标点是不是已经足够近,如果足够近,就将目标点压入path,然后返回true。(这里的目标点实际是起始点,因为vector的结构,可以有多种数据结构 vector讲解链接)其中nx和ny是路径中下一位置的点位,等待压入path.
double nx = stc % xs_ + dx, ny = stc / xs_ + dy;
if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) {
current.first = start_x;
current.second = start_y;
path.push_back(current);
return true;
}
接下来判断是否越界
if (stc < xs_ || stc > xs_ * ys_ - xs_) // would be out of bounds
{
printf("[PathCalc] Out of bounds\n");
return false;
}
然后将当前的nx和ny压入path,这个循环是每次开始的时候进行最后数据的存入,再开始下一步的操作,而不是操作后再进行存入。
path.push_back(current);
bool oscillation_detected = false;
int npath = path.size();
if (npath > 2 && path[npath - 1].first == path[npath - 3].first
&& path[npath - 1].second == path[npath - 3].second) {
ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
oscillation_detected = true;
}
int stcnx = stc + xs_;
int stcpx = stc - xs_;
接下来的一组if else是最关键的gradient相关数据处理。
首先还是检测该点周围八个点的potential值,如果有一个超出阈值,就检测这八个值里最小的,并且不进行gradient,而是采用grid的。因此dx=dy=0。如果都超过了阈值,返回0;
int stcnx = stc + xs_;
int stcpx = stc - xs_;
// check for potentials at eight positions near cell
if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH
|| potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH
|| potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH
|| oscillation_detected) {
ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size());
// check eight neighbors to find the lowest
int minc = stc;
int minp = potential[stc];
int st = stcpx - 1;
//检测stcpx一行
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
//开始新一行检测
st = stc - 1;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st = stc + 1;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
//新一行检测
st = stcnx - 1;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
stc = minc;
dx = 0;
dy = 0;
//ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
// potential[stc], path[npath-1].first, path[npath-1].second);
if (potential[stc] >= POT_HIGH) {
ROS_DEBUG("[PathCalc] No path found, high potential");
//savemap("navfn_highpot");
return 0;
}
}
如果八个方位都没有超过阈值,表示周围具有很好的梯度。此时采用梯度插补的方法计算dx dy.
首先是计算四个点的梯度方向,不过不太清楚为什么是4个不是8个。
gradCell(potential, stc);
gradCell(potential, stc + 1);
gradCell(potential, stcnx);
gradCell(potential, stcnx + 1);
接下来就是梯度插补值的计算
float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1];
float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1];
float x = (1.0 - dy) * x1 + dy * x2; // interpolated x
float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1];
float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1];
float y = (1.0 - dy) * y1 + dy * y2; // interpolated y
// move in the right direction
float ss = pathStep_ / hypot(x, y);
dx += x * ss;
dy += y * ss;
最后检测dx和dy是否出界,如果大于1,就相当于是在下一个格子,这样还是会不光滑,跟grid一样。因此不能让其大于1 (个人理解)
// check for overflow
if (dx > 1.0) {
stc++;
dx -= 1.0;
}
if (dx < -1.0) {
stc--;
dx += 1.0;
}
if (dy > 1.0) {
stc += xs_;
dy -= 1.0;
}
if (dy < -1.0) {
stc -= xs_;
dy += 1.0;
}
更多推荐
ROS-Navigation学习(4)——global_planner学习
发布评论