Hybrid_Astar算法原理_voronoi-based-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];
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341