路径规划之A星代码解读

最新推荐文章于 2024-09-11 00:16:09 发布

CAMEL__

最新推荐文章于 2024-09-11 00:16:09 发布

阅读量940

点赞数 10

分类专栏：机器人路径规划算法文章标签：算法

本文链接：https://blog.csdn.net/Carzy_/article/details/135141051

版权

机器人路径规划算法专栏收录该内容

6 篇文章 1 订阅

订阅专栏

路径规划之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();
    }