路径规划之A星代码解读
目标:
给定起点,终点,代价地图 生成路径
伪代码流程
1. 将终点坐标转为索引值 end_index
2. 将起点坐标转为索引值 index
3. 将当前的索引值 对应的带价值设置为0
使用如下的类进行构建
class Index {
public:
Index(int a, float b) {
i = a;
cost = b;
}
int i;
float cost;
};
4. 将当前的索引push到vector queue_中
queue_.push_back(Index(start_i, 0));
5. 使用一个浮点型指针记录代价值 float *potential
起点的代价值为0
6. 使用二叉树结构std::pop_heap(最小堆)构造容器queue_
代码实现(只将最小值移到vector最后 不负责删除)
std::pop_heap(queue_.begin(), queue_.end(), greater1());
struct greater1 {
bool operator()(const Index& a, const Index& b) const {
return a.cost > b.cost;
}
7. 获取最小堆的堆顶(代价值最小) 并从vector中将其删除
queue_.pop_back();
8. 如果当前索引等于目标点索引 表明找到位置则退出
9. 遍历当前点周围的四个点
1. 判断其在代价地图中的代价值,未知或者是障碍物则丢弃
2. 指针 potential 记录每次索引所对应的代价值f(不考虑与目标点的关系)
3. vector记录周围的代价值,包括f起点与目标点的曼哈顿距离值(比如每个栅格代表50)转为带价值
4.
10. 通过不断查找vector中的最小代价值,同步更新潜在的代价值potential 直到遇到终点,结束遍历。
11. 根据得到的潜在的代价地图potential 从终点遍历寻找周围最小的值,直到找到起点,途经的点即为A*规划出的路径
1.
. A星中用的二叉树
1. 对加入的再进行堆排序, 把最小代价点放到front队头queue_[0]
std::push_heap(queue_.begin(), queue_.end(), greater1());
2. std::pop_heap(最小堆)将堆顶移到容器的末尾
3. [二叉树原理参考链接 ](https://blog.csdn.net/baidu_36161424/article/details/90632661)
核心代码实现
bool AStarExpansion::calculatePotentials(unsigned char *costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float *potential)
{
LOG(INFO) << "AStarExpansion::calculatePotentials";
LOG(INFO)<<" start_x = "<<start_x<<" start_y = "<<start_y<<" end_x = "<<end_x<<" end_y = "<<end_y;
queue_.clear();
int start_i = toIndex(start_x, start_y);
queue_.push_back(Index(start_i, 0));
// 2 将所有点的potential都设为一个极大值1e10
// potential就是估计值g, f=g+h
// std::fill函数的作用是:将一个区间的元素都赋予指定的值,即在[first, last)范围内填充指定值
// 将数组设置为最大值
std::fill(potential, potential + ns_, POT_HIGH);
LOG(INFO) << " ns_ = " << ns_;
// 推入第一个点
potential[start_i] = 0;
// 获取索引值
int goal_i = toIndex(end_x, end_y);
int cycle = 0;
LOG(INFO) << " start_i = " << start_i << " goal_i = " << goal_i << " cycles = " << cycles;
while (queue_.size() > 0 && cycle < cycles)
{
Index top = queue_[0];
/// 将首元素放到最后,其他元素按照Cost值从小到大排列
// pop_heap() 是将堆顶元素与最后一个元素交换位置,之后用pop_back将最后一个元素删除
// greater1()是按小顶堆
std::pop_heap(queue_.begin(), queue_.end(), greater1());
// 移除堆顶元素
queue_.pop_back();
// 删除最后一个元素,即删除最小代价的点
int i = top.i;
// LOG(INFO) << " top.i = " << top.i << " goal_i = " << goal_i << " cost = " << top.cost;
如果到了目标点,就结束了
if (i == goal_i)
return true;
// 对前后左右四个点执行add函数,将代价最小点i周围点加入搜索队里并更新代价值
add(costs, potential, potential[i], i + 1, end_x, end_y);
add(costs, potential, potential[i], i - 1, end_x, end_y);
add(costs, potential, potential[i], i + nx_, end_x, end_y);
add(costs, potential, potential[i], i - nx_, end_x, end_y);
cycle++;
}
return false;
}
/**
* @brief 该函数用于从潜在场地图中获取规划路径
*
* @param start_x
* @param start_y
* @param goal_x
* @param goal_y
* @param goal
* @param plan
* @return true
* @return false
*/
bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan)
{
if (!initialized_)
{
LOG(ERROR) << "This planner has not been initialized yet, but it is being used, please call initialize() before use";
return false;
}
std::string global_frame = frame_id_;
// clear the plan, just in case
plan.clear();
LOG(INFO) << "start_x " << start_x << " start_y = " << start_y << " goal_x = " << goal_x << " goal_y = " << goal_y;
std::vector<std::pair<float, float>> path;
// 从潜在场地图中获取路径
if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path))
{
LOG(ERROR) << "NO PATH!";
return false;
}
u_int64_t plan_time = common::getNowStampUMs();
for (int i = path.size() - 1; i >= 0; i--)
{
std::pair<float, float> point = path[i];
// convert the plan to world coordinates
double world_x, world_y;
mapToWorld(point.first, point.second, world_x, world_y);
geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = global_frame;
pose.pose.position.x = world_x;
pose.pose.position.y = world_y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
plan.push_back(pose);
}
if (old_navfn_behavior_)
{
plan.push_back(goal);
}
return !plan.empty();
}
说明:更详细代码可以参考ros