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）、车头朝向（决定下一时刻可搜索方向）、转弯半径（决定可到达点）；
- 栅格化环境地图，用于空间搜索；
- 初次更新起点到终点的启发代价值，使用三种方式进行预估（代码中其实只有实现两种），考虑车辆运动约束但不考虑障碍物的dubins曲线、考虑车辆运动约束但不考虑障碍物的reed_shepp曲线(可后退)、不考虑运动约束但考虑障碍物的传统Astar算法（其实是用Astar的G值来充当Hy_astar的H值），然后取三者的最大值作为Hybrid_astar的启发值，这样能够加速搜索。
- 将起点加入到open_list表（一般使用优先队列或堆进行自动排序）中，然后进行循环扩展，搜索没有碰撞的子节点，并计算代价值G，将这些子节点加入到open_list表中，并将起点（当前节点）加入到close_list中。结束条件是open_list为空，或者搜索到终点，或者达到最大搜索次数。
- 整体流程跟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];
}
377
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
