Hybrid Astar 算法剖析和实现（四）

穿越临界点

已于 2022-05-18 11:25:19 修改

阅读量1.2k

点赞数 8

分类专栏：自动驾驶

于 2022-05-18 10:45:14 首次发布

本文链接：https://blog.csdn.net/weixin_44873133/article/details/124837713

版权

算法自动驾驶

自动驾驶专栏收录该内容

13 篇文章 37 订阅

订阅专栏

在学习资料满天飞的大环境下，知识变得非常零散，体系化的知识并不多，这就导致很多人每天都努力学习到感动自己，最终却收效甚微，甚至放弃学习。我的使命就是过滤掉大量的无效信息，将知识体系化，以短平快的方式直达问题本质，把大家从大海捞针的痛苦中解脱出来。

文章目录

0 前言

上一篇我们从理论角度分析了Hybrid Astar迭代搜索的流程。本篇承接上篇，将理论落地到代码，从代码角度来观察Hybrid Astar算法的迭代搜索过程。

1 迭代搜索代码实现

1.1 状态节点设计

设计状态节点主要目的是将和节点相关的信息分配到各个节点自己保存，就好比是“包产到户”。

状态节点的设计并不复杂，包含下述信息，详见代码注释。

struct StateNode {
    //三个集合
    enum NODE_SET { 
        NOT_VISITED = 0, 
        IN_OPENSET = 1, 
        IN_CLOSESET = 2
    };
	//行驶方向
    enum DIRECTION {
        NO = 0，   
        FORWARD = 1, 
        BACKWARD = 2, 
    };

    //一个节点由离散状态空间变量唯一标识
    explicit StateNode(const Vec3i& __ids):ids(__ids){}

    bool operator==(const Eigen::Vector3i& __ids) {
        return (__ids.x() == ids.x()) && (__ids.y() == ids.y()) && (__ids.z() == ids.z());
    }

    NODE_SET node_set; //标识节点所处集合
    DIRECTION direction; //标识行驶方向

    Vec3d state; //连续状态空间向量
    Vec3i ids; //离散状态空间向量

    //记录cost
    double g_cost
    double f_cost;

    StateNode *parent_node = nullptr; //指向父节点，用于路径回溯
    VectorVec3d intermediate_states; //记录中间节点，用于路径回溯
};

1.2 搜索算法实现

搜索的主流程代码如下：

tips：此处代码是示例代码，仅用作搜索逻辑的说明，是非完整可运行的。

bool HybridAStar::Search(const Vec3d &start_state, const Vec3d &goal_state) 
{
    const Vec3i start_grid_index = State2Index(start_state);
    const Vec3i goal_grid_index = State2Index(goal_state);
	
    //创建目标节点并做初始化
    StateNode goal_node(goal_grid_index);
    goal_node.state = goal_state;
    goal_node.direction = StateNode::NO;

    //创建起始节点并做初始化
    StateNode start_node(start_grid_index);
    start_node.state = start_state;
    start_node.direction = StateNode::NO;
    start_node.node_set = StateNode::IN_OPENSET;
    start_node.intermediate_states.emplace_back(start_state);
    start_node.g_cost = 0.0;
    start_node.f_cost = ComputeH(start_node, goal_node);

	//初始化open set 和 close set
    //具体来说，frontier_起到open set作用，visited_为open+close set
    frontier_.clear();
    frontier_.put(start_node.ids, start_node.f_cost);
    visited_.clear();
    visited_[start_node.ids] = start_node;

    std::vector<StateNode> neighbor_nodes;

    int count = 0;
    while (!frontier_.empty()) {
		//从open set获取当前节点，并置为IN_CLOSESET
        auto ids = frontier_.get();
        auto& cur_node = visited_[ids];
        cur_node.node_set = StateNode::IN_CLOSESET;
        
        //提前退出，相当于超时访问
        if (++count > MAX_ITERATIONS) {
            std::cerr << "Exceeded max number of iterations, Search failed.\n";
            return false;
        }
        
        //距离终点小于等于指定距离，进行RS曲线拼接，如果成功直接退出
        if ((cur_node.state.head(2) - goal_node.state.head(2)).norm() <= 
            	shot_distance_) {
            double rs_length = 0.0;
            if (AnalyticExpansions(cur_node, goal_node, rs_length)) {
                visited_[goal_node.ids] = goal_node;
                terminal_node_ptr_ = &visited_[goal_node.ids];
                return true;
            }
        }

        //节点拓展（在下一节介绍）
        GetNeighborNodes(cur_node, neighbor_nodes);

        for (unsigned int i = 0; i < neighbor_nodes.size(); ++i) {
            StateNode neighbor_node = neighbor_nodes[i];
            //计算cost
            const double cur_neigh_g_cost = ComputeG(cur_node, neighbor_node);
            const double neighbor_h = ComputeH(neighbor_node, goal_node);
            double g_cost_cur = cur_node.g_cost_ + cur_neigh_g_cost;
            
            if (neighbor_node.node_set == StateNode::NOT_VISITED/*1.如果是界外节点*/ || 
                /*2.如果是边界节点（节点拓展环节已经剔除了界内节点，以提升性能）
                  且新的g_cost比上一次要小*/
                g_cost_cur < neighbor_node.g_cost_) { 

                neighbor_node.g_cost_ = g_cost_cur;
                neighbor_node.parent_node_ = &cur_node;
                neighbor_node.node_set = StateNode::IN_OPENSET;
                neighbor_node.f_cost_ = neighbor_node.g_cost_ + neighbor_h;
                //对于第2个条件也直接添加一个新的节点，而不做替换（以提升性能）
                frontier_.put(neighbor_node.ids, neighbor_node.f_cost_);
                visited_[neighbor_node.ids] = neighbor_node;
                continue;
            } else {/*其它情况：边界节点并且g_cost不比上一次小*/
                continue;
            }
        }
    }

    return false;
}

1.3 两个循环三个分支

第一次看到上面的代码肯定会很懵，接触新东西都要有个过程。

下面对上述代码做一个结构上的剖析，就可以更清晰地看到搜索逻辑背后的思想了。

循环1：从边界点集合中选出cost最小的那个（关心顺序）作为当前节点
{
	循环2：从当前节点的邻居节点中选出一个（不关心顺序）
	{
		分支1：如果当前邻居节点在为界外节点
		{
			将该节点放入边界点集合中去
		}
		分支2：如果当前邻居节点为边界节点
		{
			也就这种情况麻烦一点，需要看一下g_cost值，
			如果新的g_cost值比老的g_cost值小，那么就更新该节点的父节点为当前节点
		}
		分支3：如果当前邻居节点为界内节点
		{
			什么都不做
		}
	}
}