devbox
小丑西瓜9
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

HybridAstar原理、改进、技巧、代码实现和应用案例_hybrid a star算法

作者：小丑西瓜9 | 2024-05-23 16:30:03

赞

踩

hybrid a star算法

视频讲解

HybridAstar原理、改进、技巧、代码实现、开源项目推荐和解读、应用案例_哔哩哔哩_bilibiliHybridAstar原理、改进、技巧、代码实现、开源项目推荐和解读、应用案例共计8条视频，包括：01hybridAstar跑机示例、01HybridAstar简单例程python实现、02HybridAstar效果演示：一个简单的python例程等，UP主更多精彩视频，请关注UP账号。icon-default.png?t=N7T8https://www.bilibili.com/video/BV1qu4y1h7oc/?spm_id_from=333.999.0.0

1. 实现的功能

点到点运动中规划出符合运动学约束的光滑无碰撞路径；

适用于阿克曼转向和差速转向车辆；

1.1. 效果演示：一个简单的例程

参考pythonRobotics修改（不后退，转弯半径约束，机器尺寸，构造窄通道）

vehicle尺寸参数

WB = 2.5 # rear to front wheel

W = 4. # width of car

LF = 4. # distance from rear to vehicle front end

LB = 1.0 # distance from rear to vehicle back end

MAX_STEER = 1. # [rad] maximum steering angle(对应最小转弯半径，节点扩展的最小

圆弧半径)

例程来自于pythonRobotics;

1.2. 效果演示：使用HyridAstar的跑机效果

hybrid astar，通过窄通道_哔哩哔哩_bilibili

2. HybridAstar原理

2.1. Astar的实现和优化拉直（用于HybridAstar的启发值计算）
2.1.1. 原始的astar路径；

2.1.2. 期待拉直为最短折线路径；

2.1.3. 优化拉直的实现效果；

上图中，红色路径为astar路径；靛蓝色路径为拉直后的路径，拉直可以避免因为启发值导致的直接朝向目标点的路径段；拉直后的路径近似为最短路径；

2.1.4. 优化拉直的实现原理

2.1.5. 优化拉直的代码实现
  1. /// @brief 将astar路径分段。拉直
  2. /// @param mapPlan 二值化的占据地图，5cm分辨率
  3. /// @param path astar稀疏的路径点
  4. /// @return 返回拉直的路径点，也是比较稀疏的/或者原始的astar路径
  5. std::vector<LDCV::Point> CTargetNavigatorEx::PathShorten(TMapData &mapPlan, std::vector<LDCV::Point> &path)
  6. {
  7.     // 传入的是像素路径点
  8.     auto TaceSegSparse = [&](LDCV::Point point_one, LDCV::Point point_two) -> std::vector<LDCV::Point> {
  9.         std::vector<LDCV::Point> path_sparse;
  10.         path_sparse.push_back(point_one);
  11.         // const float k_seg_len = 0.5 / 0.05;  // [pixel,]每一个小段的长度0.5m，对应10个像素距离
  12.         float dis             = sqrt((point_one.x - point_two.x) * (point_one.x - point_two.x) + (point_one.y - point_two.y) * (point_one.y - point_two.y));
  13.         int seg_num           = dis / k_seg_len + 0.5;
  14.         if (seg_num < 2) {
  15.             return path_sparse;
  16.         }
  17.         float delta_x = (point_two.x - point_one.x) / (1.f * seg_num);
  18.         float delta_y = (point_two.y - point_one.y) / (1.f * seg_num);
  19.         for (int i = 1; i < seg_num; i++) {
  20.             LDCV::Point seg_point(point_one.x + delta_x * i + 0.5f, point_one.y + delta_y * i + 0.5f);
  21.             path_sparse.push_back(seg_point);
  22.         }
  23.         return path_sparse;
  24.     };
  25.  
  26.     auto TacePathSparse = [=](std::vector<LDCV::Point> basePoints) -> std::vector<LDCV::Point> {
  27.         if (basePoints.size() < 3) {
  28.             return basePoints;
  29.         }
  30.         std::vector<LDCV::Point> ret;
  31.         for (unsigned int i = 0; i < basePoints.size() - 1; i++) {
  32.             std::vector<LDCV::Point> temp = TaceSegSparse(basePoints[i], basePoints[i + 1]);
  33.             // ret.emplace_back(basePoints[i]);
  34.             ret.insert(ret.end(), temp.begin(), temp.end());
  35.         }
  36.         ret.emplace_back(basePoints.back());
  37.         return ret;
  38.     };
  39.  
  40.     // 优化拉直拉短
  41.     auto path_points_sparse = TacePathSparse(path);
  42.     std::vector<LDCV::Point> path_adjusted;
  43.     bool is_path_adjust_work = false;
  44.     do {
  45.         path_adjust::PathShorten pp(path_points_sparse.size());
  46.         if (path_points_sparse.size() < 3) {
  47.             break;
  48.         }
  49.         for (size_t i = 0; i < path_points_sparse.size() - 1; i++) {
  50.             auto point_one = path_points_sparse[i];
  51.             auto point_two = path_points_sparse[i + 1];
  52.             float dis      = sqrt((point_one.x - point_two.x) * (point_one.x - point_two.x) + (point_one.y - point_two.y) * (point_one.y - point_two.y));
  53.             pp.AddEdge(i, i + 1, dis);
  54.         }
  55.         for (size_t i = 0; i < path_points_sparse.size() - 2; i++) {
  56.             // linehit
  57.             int meetHit = 0;
  58.             for (size_t j = i + 2; j < path_points_sparse.size(); j++) {
  59.                 int x1 = path_points_sparse[i].x;
  60.                 int y1 = path_points_sparse[i].y;
  61.                 int x2 = path_points_sparse[j].x;
  62.                 int y2 = path_points_sparse[j].y;
  63.                 int hitX;
  64.                 int hitY;
  65.                 if (LDCV::CCommonAlg::LineHit(hitX, hitY, 128, x1, y1, x2, y2,
  66.                                             &mapPlan.map[0], mapPlan.mapParam.width, mapPlan.mapParam.height)) {
  67.                     //                     LOGD("hitPoint = (%.2f, %.2f)", slamMap.idx2x(hitX), slamMap.idx2y(hitY));
  68.                     meetHit++;  // 连线发生hit最大允许向前搜索的次数
  69.                     if (meetHit > 5) {
  70.                         break;
  71.                     } else {
  72.                         continue;
  73.                     }
  74.                 } else {
  75.                     //
  76.                     auto point_one = path_points_sparse[i];
  77.                     auto point_two = path_points_sparse[j];
  78.                     float dis      = sqrt((point_one.x - point_two.x) * (point_one.x - point_two.x) + (point_one.y - point_two.y) * (point_one.y - point_two.y));
  79.                     pp.AddEdge(i, j, dis);
  80.                 }
  81.             }
  82.         }
  83.         std::vector<int> ret = pp.GetShortestPath();
  84.         for (auto item : ret) {
  85.             path_adjusted.push_back(path_points_sparse[item]);
  86.         }
  87.         if (path_adjusted.size() < path_points_sparse.size()) {
  88.             FLOGD("--PATH ADJUST WORKS.");
  89.             is_path_adjust_work = true;
  90.         }
  91.     } while (0);
  92.     // end 优化拉直
  93.     if (is_path_adjust_work == true) {
  94.         return path_adjusted;
  95.     } else {
  96.         return path;
  97.     }
  98. }

作用，生成一个参考路径：

用来计算hybridAstar启发值；

可以用来判断窄通道；

可以生成一个气泡带，减小搜索空间；

2.2. HybridAstar和Astar对比，HybridAstar的实现和优化改进；
2.2.1. 节点扩展；
2.2.1.1. astar的节点扩展

从openset里面拿出一个代价最小的节点；小顶堆;

代价值 F=G(到起点的路径长度)+H（到终点的直线距离）

节点定义（x, y）(格点整数坐标)，仅考虑格点坐标

D:\[OPENSOURCE_CODE]\apolloFiles\apollo-master\modules\planning\open_space\coarse_trajectory_generator\grid_search.cc

  1. bool GridSearch::GenerateAStarPath(
  2.     const double sx, const double sy, const double ex, const double ey,
  3.     const std::vector<double>& XYbounds,
  4.     const std::vector<std::vector<common::math::LineSegment2d>>&
  5.         obstacles_linesegments_vec,
  6.     GridAStartResult* result)
2.2.1.2. 状态空间采样（给定初末位姿计算路径、两点边界问题）与控制空间采样（给定初始位姿，给定速度、转向角度和时间，计算路径）

状态节点的定义；

  1. double x_ = 0.0;
  2. double y_ = 0.0;
  3. double phi_ = 0.0;

状态格点

  1. int x_grid_ = 0;
  2. int y_grid_ = 0;
  3. int phi_grid_ = 0;

状态栅格的划分；

状态节点转换为状态格点；

2.2.1.3. HybridAstar的节点扩展（使用定长的圆弧扩展）

节点的定义（x,y,phi物理坐标）;

节点等价（x,y,phi对应的离散化状态格点坐标相等）(判断节点是否已经搜索过)（apollo里面是将格点坐标转换成字符串）（或者计算状态格点在状态栅格数组里面的下标）;

圆弧扩展（扩展长度固定，圆弧半径/steerAngle 采样多个值）（在某些条件下可以动态调整扩展长度）;

考虑后退或者不考虑后退；

hybrid Astar的实现、优化和应用 - tmjDD - 博客园

节点扩展的动画展示；

虚拟环境中扫地机到点运动视频展示；

虚拟机里面的代码换成最新的；

2.2.2. 【优化项】启发函数和代价函数的设计

代价值 F=G(当前节点到起点的路径长度、转弯半径、steerAngle变化值/曲率变化、后退的代价)+H（当前节点到终点的astar距离/最短折线距离）;

其中H的作用是引导代价最小的节点沿着astar路径向终点扩展；

使用当前节点到目标点的astar长度用来计算启发值，计算方法：

    1. 每个节点单独计算到目标格点的astar路径长度（不可取，计算量大）；
    2. 从终点开始搜索遍历所有格点,把所有格点全部到达一遍，记录到closeSet，对已经搜索过的格点回溯，即可得到该点到终点的astar路径(apollo 中使用的是该方法)(apollo里面生成一个DPmap)(使用低分辨率地图搜索)
    3. 计算当前格点到终点的近似astar路径（改进后的方法）（最短折线路径细分成多个路径点，存到一个kdtree里面，对当前的节点，找到最近的路径点，计算最近的路径点后续的astar路径）

改进方法的实现：

  1. 计算起点到终点的最短折线路径；

上图中，红色路径为astar路径；靛蓝色路径为拉直后的路径，拉直可以避免因为启发值导致的不合理的直接朝向目标点的路径段；拉直后的路径近似为最短路径；

hybrid Astar的实现、优化和应用 - tmjDD - 博客园

  1. 在astar路径(稀疏之后的)上找到一个点，使得该点到当前节点最近且连线可通行，将连线长度与这个点后续的astar路径长度之和(也可以直接使用这个点后续的astar路径长度) 用来计算启发值H;

2.2.3. 【优化项】oneshot命中目标位姿，使用改进的dubins曲线命中目标点

在目标点附近直接计算两点边界问题，将当前节点直接和目标点用曲线连起来；

dubins(RLR)命中目标点（考虑到达目标点时的方向）；reedsheep曲线（有后退动作）

不需要考虑到达目标点时的方向(圆弧加上直线直接到达目标点)

"E:\QtCode\cleanPackTest\main\bin\MyRelease\hybrid_astar_annotation.exe"

2.2.4. 【优化项】使用astar路径扩展生成搜索区域/气泡带

只在有限指定区域进行节点扩展；减小搜索空间；

可以考虑使用astar计算出来的瓶颈点chockPoint;

2.2.5. 【优化项】碰撞检测(单独讲)

单独出视频讲解；

可以使用box2d;和障碍物做碰撞检测（点线多边形，mapConvert/ros）；

最准确同时高效：使用机器的真实轮廓计算覆盖栅格模板(官方例程)的碰撞检测表；

2.2.6. 【优化项】使用高分辨率地图

可以更容易得到通过窄通道/瓶颈口的路径；

2.2.7. 【优化项】其他

动态改变搜索圆弧的长度；

3. 【开源项目示例】apollo中HybridAstar实现；

启发函数和代价函数的设计；

惩罚项和启发值，kd树和flann，dubins和reedshep改进

碰撞检测；

4. 【开源项目示例】官方例程实现

4.1. 官方例程repo

path_planner: 混合A星代码实现

4.2. 论文

E:\[opencv_source_navigation]\Hybrid_A_Star-main

4.3. 一个带注释的repo

GitHub - teddyluo/hybrid-a-star-annotation: Hybrid A*路径规划器的代码注释

4.4. python实现例程

开源项目：python robotics /GIF

【python】E:\[OPENSOURCE_CODE]\MotionPlanning-master\HybridAstarPlanner

5. HybridAstar路径的后端优化（基于梯度的滚动优化(算法优化和改进)；

梯度下降法实现路径平滑 - 知乎

迭代计算/使用非线性优化；

对比三个开源项目

constrained smoother（ceres）(ros::navigation2);

teb no ros(g2o);

nloptSmoother(nlopt)(将优化项作为代价);

共同点：

路径长度；

smoothness;

离障碍物的距离；

hybrid Astar的实现、优化和应用 - tmjDD - 博客园

路径规划-路径的优化 - tmjDD - 博客园

6. 应用案例

6.1. 通过窄通道的实现方法

使用astar最短折线路径获得瓶颈口/窄通道位置（窄通道朝向）；

计算气泡带/安全走廊，减小搜索空间；

使用高分辨率融合地图；

技巧及补救策略：

扩展的圆弧长度取小，或者在窄通道处减小搜索步长，减小窄通道处沿窄通道朝向的搜索代价；

使用窄通道位置附近局部地图，使用高分辨率地图，状态空间的角度细分更大些，增加搜索处路径的概率；

6.2. 异形机的路径规划和脱困

异形机的碰撞检测

6.3. 靠近走廊中心的路径生成

下图中的效果实现方法是：在搜索的过程中不停的使用dubins命中骨架上的参考点；

hybrid astar和优化； 笔记记录(运动规划-到点运动)

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小丑西瓜9/article/detail/613606
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号