Hybrid_Astar算法原理

1、前言

Hybrid_astar是一种考虑机器运动学(转弯半径、车辆运动模型等)的非完整约束的全局路径规划算法,算法中主要实现了两大功能模块,第一是使用混合astar(2D(x+y)-astar + 3D(x+y+&)-astar)搜索出一条起到到终点的考虑非完整约束的全局路径;第二是对产生的全局路径进行平滑,主要考虑四种平滑系数项(wSmoothness + wCurvature + wVoronoi + wObstacle),分别代表路径的平滑层度、路径的曲率层度、路径的居中层度、距离障碍物远近的层度。其主体还是在于路径规划,而路径规划的核心思想仍就是Astar,所有本文主要介绍规划部分,如果读者对Astar非常了解的话,那么Hybrid_astar的原理及源码就很简单了。
在这里插入图片描述

2、应用领域

APA(Auto Parking Asist,辅助泊车)、AVP(Automated Valet Parking,代客泊车)、U-Turn(行车场景下的车辆掉头)。其中AVP是目前较为热门的技术(用户在指定下客点下车,通过手机 APP下达泊车指令,车辆在接收到指令后可自动行驶到停车场的停车位,不需要用户操纵与监控,用车反之),旨在解决用户停车、用车最后一公里问题,属于低速自动驾驶技术,是能够实现商业落地的自动驾驶技术应用场景(如下图所示)。
在这里插入图片描述

3、算法流程

传统Astar搜索 和 考虑车辆运动约束搜索的区别:
Astar:只需要将搜索空间进行栅格化,然后按照栅格坐标去扩展路径,即只需要考虑X,Y;
考虑车辆运动约束搜索:需要考虑车辆的坐标(X,Y)、车头朝向(决定下一时刻可搜索方向)、转弯半径(决定可到达点);

  1. 栅格化环境地图,用于空间搜索;
  2. 初次更新起点到终点的启发代价值,使用三种方式进行预估(代码中其实只有实现两种),考虑车辆运动约束但不考虑障碍物的dubins曲线、考虑车辆运动约束但不考虑障碍物的reed_shepp曲线(可后退)、不考虑运动约束但考虑障碍物的传统Astar算法(其实是用Astar的G值来充当Hy_astar的H值),然后取三者的最大值作为Hybrid_astar的启发值,这样能够加速搜索。
  3. 将起点加入到open_list表(一般使用优先队列或堆进行自动排序)中,然后进行循环扩展,搜索没有碰撞的子节点,并计算代价值G,将这些子节点加入到open_list表中,并将起点(当前节点)加入到close_list中。结束条件是open_list为空,或者搜索到终点,或者达到最大搜索次数。
  4. 整体流程跟astar类似,流程如下:
    在这里插入图片描述

4、核心代码实现与理解

下面可以通过截取的部分核心代码及注释更加详细的了解整个Hybrid_astar的过程。

#include "algorithm.h"
#include <boost/heap/binomial_heap.hpp>
using namespace HybridAStar;

//原始A*算法,用来搜索计算 holonomic-with-obstacles heuristic
float aStar(Node2D &start, Node2D &goal, Node2D *nodes2D, int width, int height, CollisionDetection &configurationSpace, Visualize &visualization);
//计算start到目标点goal的启发式代价(即:cost-to-go) 会比较Astar、 dubins、 Reeds-Shepp
void updateH(Node3D &start, const Node3D &goal, Node2D *nodes2D, float *dubinsLookup, int width, int height, CollisionDetection &configurationSpace,
             Visualize &visualization);
// Dubins shot的路径
Node3D *dubinsShot(Node3D &start, const Node3D &goal, CollisionDetection &configurationSpace);

/**
 * 重载运算符,用来生成节点的比较
 * “boost::heap::compare<CompareNodes>”获得使用
 * 小顶堆 用于创建open_list(优先队列)
 */
struct CompareNodes {
    /// Sorting 3D nodes by increasing C value - the total estimated cost
    bool operator()(const Node3D *lhs, const Node3D *rhs) const { return lhs->getC() > rhs->getC(); }
    /// Sorting 2D nodes by increasing C value - the total estimated cost
    bool operator()(const Node2D *lhs, const Node2D *rhs) const { return lhs->getC() > rhs->getC(); }
};

/** 
 * 功能:Hybrid A* 的主调用函数
 * 输入:起始点、目标点、配置空间的3维和2维表示(2D用来A*,3D用于hybrid A*)、搜索网格的宽度及高度、配置空间的查找表、Dubins查找表(程序实际上没有使用该表,而是直接调用OMPL库计算)、rviz可视化类(用于显示结果)
 * 输出:满足约束条件的节点(数据结构用指针表示)
 **/
Node3D *Algorithm::hybridAStar(Node3D &start, const Node3D &goal, Node3D *nodes3D, Node2D *nodes2D, int width, int height, CollisionDetection &configurationSpace, float *dubinsLookup, Visualize &visualization) {
    // 父节点 和 子节点的 index
    int iPred, iSucc;
    // 用于记录新扩展节点到起点的代价
    float newG;
    // 可后退可前进6、只能前进3
    int dir = Constants::reverse ? 6 : 3;
    // 迭代计数
    int iterations = 0;
    // 优先列表 可以从小到大自动排序
    typedef boost::heap::binomial_heap<Node3D *, boost::heap::compare<CompareNodes>> priorityQueue;
    // openList表
    priorityQueue O; 

    // 计算到起点到目标点的启发式值  混合Astar的核心点,使用 dubins或者reedsShepp 和 2D-astar计算代价H
    updateH(start, goal, nodes2D, dubinsLookup, width, height, configurationSpace, visualization);
    // 将起点标记为open
    start.open();
    // 将起点加入open_list集合
    O.push(&start);
    // 计算出起点的索引值(输入整副地图的宽高)
    iPred = start.setIdx(width, height);
    nodes3D[iPred] = start;

    // 定义父节点 、子节点 node
    Node3D *nPred;
    Node3D *nSucc;

    // 路径搜索 退出条件为 open表为空 或 搜索到目标点 或 搜索迭代次数超过最大限制
    while (!O.empty()) {
        // 循环部分:从优先队列中取出一个最低代价的点
        nPred = O.top();
        // 计算索引位置(输入地图的宽高) i 代表index
        iPred = nPred->setIdx(width, height);
        // 记录迭代次数
        iterations++;

        //如果为closed,说明该点已经处理过,忽略(将它从open_list 中移除)
        if (nodes3D[iPred].isClosed()) {
            O.pop();
            continue; 
        }
        else if (nodes3D[iPred].isOpen()) {
        //如果该点是在open状态,即正在扩张的点(常规astar中一般会有四种状态open、close、free、obs),这里应该是用碰撞检测      (configurationSpace)替代了obs
            // 标记为closed,即访问过
            nodes3D[iPred].close();
            // 从优先队列中移除
            O.pop();

            // 检测当前节点是否是终点或者是否超出解算的最大时间(最大迭代次数30000)
            if (*nPred == goal || iterations > Constants::iterations) {
                return nPred;
            } else { // 不是目标点,继续搜索
                // 使用dubinsShot && 距离范围内 && 前进方向(<3)
                if (Constants::dubinsShot && nPred->isInRange(goal) && nPred->getPrim() < 3) {
                    nSucc = dubinsShot(*nPred, goal, configurationSpace);
                    //如果Dubins方法能直接命中,即不需要进入Hybrid A*搜索了,直接返回结果
                    if (nSucc != nullptr && *nSucc == goal) {
                        // 是目标点点,直接返回
                        return nSucc;
                    }
                }

                // 3/6  (6表示可以倒退、3表示只能前进)
                for (int i = 0; i < dir; i++) { //每个方向都搜索
                    // 创建下一个扩展节点,这里有三种可能的方向,如果可以倒车的话是6种方向(astar中只考虑八邻域或者四邻域)
                    nSucc = nPred->createSuccessor(i); //找到下一个点 里面的x,y,t其实会变成浮点型
                    // 设置节点遍历标识 //索引值 浮点类型会强制转换为int类型
                    iSucc = nSucc->setIdx(width, height);
                    // 首先判断产生的节点是否在范围内;其次判断产生的节点会不会产生碰撞;
                    if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc)) {
                        // 确定新扩展的节点不在close状态,或者没有在之前遍历过(iPred == iSucc 表明是扩展出来的,且没有超过当前栅格)
                        if (!nodes3D[iSucc].isClosed() || iPred == iSucc) {
                            // 更新G值
                            nSucc->updateG();
                            newG = nSucc->getG();

                            // 如果扩展节点不在OPEN LIST中,或者找到了更短G值的路径,或者该节点索引与前一节点在同一网格中(用新点代替旧点)
                            if (!nodes3D[iSucc].isOpen() || newG < nodes3D[iSucc].getG() || iPred == iSucc) {
                                // 当前点到目标点的启发式值  混合Astar的核心点,使用 dubins或者reedsShepp 和 2D-astar计算代价H
                                updateH(*nSucc, goal, nodes2D, dubinsLookup, width, height, configurationSpace, visualization);

                                // 如果下一个点仍在相同的cell、并且cost变大,那继续找
                                if (iPred == iSucc && nSucc->getC() > nPred->getC() + Constants::tieBreaker) {
                                    // 删除这个子节点,继续搜索
                                    delete nSucc;
                                    continue;
                                }
                                // 如果下一节点仍在相同的cell,但是代价值要小,则用当前successor替代前一个节点(这里仅更改指针,数据留在内存中)
                                else if (iPred == iSucc && nSucc->getC() <= nPred->getC() + Constants::tieBreaker) {
                                    // 将当前节点的父节点设置为 其前父节点的父节点
                                    nSucc->setPred(nPred->getPred());
                                }

                                // 给出原地踏步的提示
                                if (nSucc->getPred() == nSucc) {
                                    std::cout << "looping";
                                }

                                nSucc->open();
                                // 将生成的子节点加入到open list中
                                nodes3D[iSucc] = *nSucc; 
                                O.push(&nodes3D[iSucc]);
                                delete nSucc;
                            } else {
                                delete nSucc;
                            }
                        } else {
                            delete nSucc;
                        }
                    } else {
                        delete nSucc;
                    }
                }
            }
        }
    }

    if (O.empty()) {
        return nullptr;
    }

    return nullptr;
}

// 原始 2D A*算法,返回当前点到终点的启发值,即H
float aStar(Node2D &start, Node2D &goal, Node2D *nodes2D, int width, int height, CollisionDetection &configurationSpace, Visualize &visualization) {
    int iPred, iSucc;
    float newG;

    // 将open list和close list重置
    for (int i = 0; i < width * height; ++i) {
        nodes2D[i].reset();
    }


    boost::heap::binomial_heap<Node2D *,boost::heap::compare<CompareNodes>> O; // Open list
    // astar中的启发值,代码中使用的是欧式距离
    start.updateH(goal);
    // 将起点设置为open
    start.open();
    // 推入open表
    O.push(&start);
    // 其实是计算得到索引号
    iPred = start.setIdx(width);
    nodes2D[iPred] = start;

    Node2D *nPred;
    Node2D *nSucc;

    while (!O.empty()) {
        //从Open集合中找出代价最低的元素
        nPred = O.top();
        // set index 得到索引号
        iPred = nPred->setIdx(width);

        // 检查:如果已扩展,则从open set中移除,处理下一个
        if (nodes2D[iPred].isClosed()) {
            O.pop();
            continue;
        } else if (nodes2D[iPred].isOpen()) { 
            //没有进行扩展
            nodes2D[iPred].close();    //标记为close
            nodes2D[iPred].discover(); // 标记为探索过
            O.pop();

            // 如果当前节点为goal ,找到目标点,返回G值
            if (*nPred == goal) {
                return nPred->getG();
            }

            //非目标点,则从可能的方向寻找
            else {
                // 8邻域搜索
                for (int i = 0; i < Node2D::dir; i++) {
                    nSucc = nPred->createSuccessor(i);
                    iSucc = nSucc->setIdx(width);

                    // 约束性检查:在有效网格范围内、且不是障碍(是不是障碍物其实可以通过访问地图当前栅格确定)、没有扩展过 open 或者 free
                    if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc) && !nodes2D[iSucc].isClosed()) {
                        //更新G值 也使用的欧式距离
                        nSucc->updateG();
                        newG = nSucc->getG();

                        // 如果子节点不在open集中,或者它的G值(cost-so-far)比之前要小,则为可行的方向
                        if (!nodes2D[iSucc].isOpen() || newG < nodes2D[iSucc].getG()) {
                            // calculate the H value //计算H值
                            nSucc->updateH(goal);
                            // put successor on open list
                            nSucc->open();
                            nodes2D[iSucc] = *nSucc;
                            //将该点移到open set中
                            O.push(&nodes2D[iSucc]);
                            delete nSucc;
                        } else {
                            delete nSucc;
                        }
                    } else {
                        delete nSucc;
                    }
                }
            }
        }
    }
    // 搜索不到路径的情况 返回一个较大值
    return 1000;
}

// 计算当前点到目标的启发值
// 混合astar的精髓部分:计算astar 和 dubins(或者reedsShepp)到达终点的代价值,然后取大值
void updateH(Node3D &start, const Node3D &goal, Node2D *nodes2D, float *dubinsLookup, int width, int height, CollisionDetection &configurationSpace,Visualize &visualization) {
    float dubinsCost = 0;
    float reedsSheppCost = 0;
    float twoDCost = 0;
    float twoDoffset = 0;

    // 使用dubins 只能前进不能后退
    if (Constants::dubins) {
        //这里改用open motion planning library的算法
        ompl::base::DubinsStateSpace dubinsPath(Constants::r);
        State *dbStart = (State *)dubinsPath.allocState();
        State *dbEnd = (State *)dubinsPath.allocState();
        dbStart->setXY(start.getX(), start.getY());
        dbStart->setYaw(start.getT());
        dbEnd->setXY(goal.getX(), goal.getY());
        dbEnd->setYaw(goal.getT());
        dubinsCost = dubinsPath.distance(dbStart, dbEnd);
    }

    // 假如车子可以后退,使用Reeds-Shepp 算法
    if (Constants::reverse && !Constants::dubins) {
        // 使用ompl库并设置最小转弯半径
        ompl::base::ReedsSheppStateSpace reedsSheppPath(Constants::r); 
        State *rsStart = (State *)reedsSheppPath.allocState();
        State *rsEnd = (State *)reedsSheppPath.allocState();
        rsStart->setXY(start.getX(), start.getY());
        rsStart->setYaw(start.getT());
        rsEnd->setXY(goal.getX(), goal.getY());
        rsEnd->setYaw(goal.getT());
        // 利用reeds-shepp计算起点至终点的代价
        reedsSheppCost = reedsSheppPath.distance(rsStart, rsEnd);
    }

    // 使用2D启发值 && 没有被发现过(遍历)
    if (Constants::twoD && !nodes2D[(int)start.getY() * width + (int)start.getX()].isDiscovered()) {
        Node2D start2d(start.getX(), start.getY(), 0, 0, nullptr);
        Node2D goal2d(goal.getX(), goal.getY(), 0, 0, nullptr);
        //调用A*算法,返回cost-so-far, 并在2D网格中设置相应的代价值
        nodes2D[(int)start.getY() * width + (int)start.getX()].setG(
            aStar(goal2d, start2d, nodes2D, width, height, configurationSpace, visualization));
    }

    if (Constants::twoD) {
        twoDoffset = sqrt(((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) *
                              ((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) +
                          ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())) *
                              ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())));
        // getG()返回A*的启发式代价,twoDoffset为start与goal各自相对自身所在2D网格的偏移量的欧氏距离
        twoDCost = nodes2D[(int)start.getY() * width + (int)start.getX()].getG() - twoDoffset;
    }

    //将两个代价中的最大值作为启发式值(虽然这里有三个数值,但Dubins和Reeds-Shepp的启用是互斥的,所以实际上是计算两种cost)
    start.setH(std::max(reedsSheppCost, std::max(dubinsCost, twoDCost)));
}

// 看能不能一次性规划到目标点的路径
Node3D *dubinsShot(Node3D &start, const Node3D &goal, CollisionDetection &configurationSpace) {
    // start
    double q0[] = {start.getX(), start.getY(), start.getT()};
    // goal
    double q1[] = {goal.getX(), goal.getY(), goal.getT()};
    DubinsPath path;
    // 计算路径 输入:起点、终点、转弯半径  输出:路径
    dubins_init(q0, q1, Constants::r, &path);

    int i = 0;
    float x = 0.f;
    float length = dubins_path_length(&path);

    //采样dubins路径(中间取点,dubinsStepSize为分辨率),最后用于碰撞检测
    Node3D *dubinsNodes = new Node3D[(int)(length / Constants::dubinsStepSize) + 1];

    while (x < length) { //这是跳出循环的条件之一:生成的路径没有达到所需要的长度  基本都能够满足需求
        double q[3];
        dubins_path_sample(&path, x, q);
        dubinsNodes[i].setX(q[0]);
        dubinsNodes[i].setY(q[1]);
        dubinsNodes[i].setT(Helper::normalizeHeadingRad(q[2]));

        //跳出循环的条件之二:生成的路径存在碰撞节点
        if (configurationSpace.isTraversable(&dubinsNodes[i])) {
            // set the predecessor to the previous step
            if (i > 0) {
                dubinsNodes[i].setPred(&dubinsNodes[i - 1]);
            } else {
                dubinsNodes[i].setPred(&start);
            }
            if (&dubinsNodes[i] == dubinsNodes[i].getPred()) {
                std::cout << "looping shot";
            }
            x += Constants::dubinsStepSize;
            i++;
        } else {

            delete[] dubinsNodes;
            return nullptr;
        }
    }
    //返回末节点,通过getPred()可以找到前一节点。
    return &dubinsNodes[i - 1];
}

  • 3
    点赞
  • 17
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值