Hybrid Astar 算法剖析和实现

Posted 2022-05-27 穿越临界点

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

文章目录

• • 0 前言
  • 1 碰撞检测和边界检测
  • 2 节点状态离散化
  • 3 代码实现
  • 4 总结

0 前言

本篇继续介绍节点拓展的内容，以及示例代码实现。

1 碰撞检测和边界检测

上一篇（Hybrid Astar 算法剖析和实现（五）_穿越临界点的博客-CSDN博客）讲到中间节点的存在就是为了进行相对可靠的碰撞检测。

节点每次拓展都要进行碰撞检测和边界检测（边界检测可以理解为碰撞检测的一种）。

节点每次拓展需要检测几次边界内的碰撞呢？有几个中间节点（包括最后一个节点）就要检测几次。

节点每次拓展需要做几次边界碰撞检测呢？只需要做一次。

2 节点状态离散化

按照上篇中的公式进行节点拓展后，得到的是节点连续状态空间$(x,y,\theta )$，需要将其离散化后映射到状态栅格中，这样做是为了方便识别两个节点状态是否是同一个节点状态。

离散化请参考 Hybrid Astar 算法剖析和实现（二）_穿越临界点的博客-CSDN博客

3 代码实现

为了将抽象概念具体化，下面给出节点拓展流程的示例代码。

/*依据当前节点获得邻居节点（节点拓展）*/
void HybridAStar::GetNeighborNodes(const StateNode& curr_node,
                                   std::vector<StateNode> &neighbor_nodes) 
    neighbor_nodes.clear();

    /*第一次循环是对前轮转角的离散*/
    for (int i = -steering_discrete_num_; i <= steering_discrete_num_; ++i) 
        VectorVec3d intermediate_state;
        bool has_obstacle = false;

        double x = curr_node.state_.x();
        double y = curr_node.state_.y();
        double theta = curr_node.state_.z();

        const double phi = i * steering_radian_step_size_;

        // forward：v取正向最大值
        /*该层循环实质上是对时间t进行离散，这里用前进距离等效*/
        for (int j = 1; j <= segment_length_discrete_num_; j++) 
            /*基于运动学模型进行中间节点拓展
            （最后一个中间节点是最终的状态节点）*/
            DynamicModel(move_step_size_, phi, x, y, theta);
            intermediate_state.emplace_back(Vec3d(x, y, theta));
		   /*对每个中间节点进行碰撞检测*/
            if (!CheckCollision(x, y, theta)) 
                has_obstacle = true;
                break;
            
        

        /*状态节点离散化，映射到状态栅格中*/
        Vec3i ids = State2Index(intermediate_state.back());
        /*判断节点所处集合*/
        StateNode::NODE_STATUS node_state = StateNode::NOT_VISITED;
        auto pnode = visited_.find(ids);
        if (pnode != visited_.end()) 
            node_state = pnode->second.node_status_;
         else 
            node_state = StateNode::NOT_VISITED;
        

        /*进行边界检测并提前过滤掉已经存在于close set中的节点（提升效率）*/
        if (node_state != StateNode::IN_CLOSESET &&
            !BeyondBoundary(intermediate_state.back().head(2)) &&
            !has_obstacle) 
            StateNode neighor_node(ids);
            neighor_node.node_status_ = node_state;
            neighor_node.intermediate_states_ = intermediate_state;
            neighor_node.state_ = intermediate_state.back();
            neighor_node.steering_grade_ = i;
            neighor_node.direction_ = StateNode::FORWARD;
            neighbor_nodes.push_back(neighor_node);
        

        // backward: v取负向最大值情况省略
        ...

单个节点拓展的示例代码如下：

其中的NormalizeAngle函数实现可以参照 角度归一化实现_穿越临界点的博客-CSDN博客_角度归一化

void HybridAStar::DynamicModel(const double &step_size, const double &phi,
                               double &x, double &y, double &theta) const 
    x = x + step_size * std::cos(theta);
    y = y + step_size * std::sin(theta);
    theta = NormalizeAngle(theta + step_size / wheel_base_ * std::tan(phi));

4 总结

本篇主要介绍了Hybrid Astar算法节点拓展的代码实现。下一篇重点介绍碰撞检测算法。

恭喜你又坚持看完了一篇博客，又进步了一点点！如果感觉还不错就点个赞再走吧，你的点赞和关注将是我持续输出的哒哒哒动力~~