HybridAstar原理、改进、技巧、代码实现和应用案例

最新推荐文章于 2024-06-08 19:37:53 发布
最新推荐文章于 2024-06-08 19:37:53 发布
阅读量1.2k 收藏 37
点赞数 24
文章标签： 自动化
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/tangyuanjieDD/article/details/137204444
版权

视频讲解

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. 优化拉直的代码实现
/// @brief 将astar路径分段。拉直
/// @param mapPlan 二值化的占据地图，5cm分辨率
/// @param path astar稀疏的路径点
/// @return 返回拉直的路径点，也是比较稀疏的/或者原始的astar路径
std::vector<LDCV::Point> CTargetNavigatorEx::PathShorten(TMapData &mapPlan, std::vector<LDCV::Point> &path)
{
    // 传入的是像素路径点
    auto TaceSegSparse = [&](LDCV::Point point_one, LDCV::Point point_two) -> std::vector<LDCV::Point> {
        std::vector<LDCV::Point> path_sparse;
        path_sparse.push_back(point_one);
        // const float k_seg_len = 0.5 / 0.05;  // [pixel,]每一个小段的长度0.5m，对应10个像素距离
        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));
        int seg_num           = dis / k_seg_len + 0.5;
        if (seg_num < 2) {
            return path_sparse;
        }
        float delta_x = (point_two.x - point_one.x) / (1.f * seg_num);
        float delta_y = (point_two.y - point_one.y) / (1.f * seg_num);
        for (int i = 1; i < seg_num; i++) {
            LDCV::Point seg_point(point_one.x + delta_x * i + 0.5f, point_one.y + delta_y * i + 0.5f);
            path_sparse.push_back(seg_point);
        }
        return path_sparse;
    };

    auto TacePathSparse = [=](std::vector<LDCV::Point> basePoints) -> std::vector<LDCV::Point> {
        if (basePoints.size() < 3) {
            return basePoints;
        }
        std::vector<LDCV::Point> ret;
        for (unsigned int i = 0; i < basePoints.size() - 1; i++) {
            std::vector<LDCV::Point> temp = TaceSegSparse(basePoints[i], basePoints[i + 1]);
            // ret.emplace_back(basePoints[i]);
            ret.insert(ret.end(), temp.begin(), temp.end());
        }
        ret.emplace_back(basePoints.back());
        return ret;
    };

    // 优化拉直拉短
    auto path_points_sparse = TacePathSparse(path);
    std::vector<LDCV::Point> path_adjusted;
    bool is_path_adjust_work = false;
    do {
        path_adjust::PathShorten pp(path_points_sparse.size());
        if (path_points_sparse.size() < 3) {
            break;
        }
        for (size_t i = 0; i < path_points_sparse.size() - 1; i++) {
            auto point_one = path_points_sparse[i];
            auto point_two = path_points_sparse[i + 1];
            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));
            pp.AddEdge(i, i + 1, dis);
        }
        for (size_t i = 0; i < path_points_sparse.size() - 2; i++) {
            // linehit
            int meetHit = 0;
            for (size_t j = i + 2; j < path_points_sparse.size(); j++) {
                int x1 = path_points_sparse[i].x;
                int y1 = path_points_sparse[i].y;
                int x2 = path_points_sparse[j].x;
                int y2 = path_points_sparse[j].y;
                int hitX;
                int hitY;
                if (LDCV::CCommonAlg::LineHit(hitX, hitY, 128, x1, y1, x2, y2,
                                            &mapPlan.map[0], mapPlan.mapParam.width, mapPlan.mapParam.height)) {
                    //                     LOGD("hitPoint = (%.2f, %.2f)", slamMap.idx2x(hitX), slamMap.idx2y(hitY));
                    meetHit++;  // 连线发生hit最大允许向前搜索的次数
                    if (meetHit > 5) {
                        break;
                    } else {
                        continue;
                    }
                } else {
                    //
                    auto point_one = path_points_sparse[i];
                    auto point_two = path_points_sparse[j];
                    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));
                    pp.AddEdge(i, j, dis);
                }
            }
        }
        std::vector<int> ret = pp.GetShortestPath();
        for (auto item : ret) {
            path_adjusted.push_back(path_points_sparse[item]);
        }
        if (path_adjusted.size() < path_points_sparse.size()) {
            FLOGD("--PATH ADJUST WORKS.");
            is_path_adjust_work = true;
        }
    } while (0);
    // end 优化拉直
    if (is_path_adjust_work == true) {
        return path_adjusted;
    } else {
        return path;
    }
}

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

用来计算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

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

状态节点的定义；

double x_ = 0.0;
double y_ = 0.0;
double phi_ = 0.0;

状态格点

int x_grid_ = 0;
int y_grid_ = 0;
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和优化； 笔记记录(运动规划-到点运动)

tmjtyj
关注
  • 24
    点赞
  • 踩
  • 37
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
[PathPlanning]Hybrid A*
qq_28366817的博客
02-15 920
HybridAStar算法主要是在A*基础上考虑车辆动力学相关的搜索算法，其放弃了A*中对搜索空间离散化，利用Dubins或者Reeps-Shepp来考虑自车能够前进的位置，使得规划的轨迹满足车辆的非完整性约束，但是该Hybrid A*牺牲了算法的完备性和最优性。Hybrid A*算法主要包括(1)在连续空间上扩张节点搜索最短路径，(3)路径平滑。Hybrid A*算法与A*算法的差异对比如下表所示A*Hybrid A*离散方法(Discrete method)
混合A星算法路径规划Hybrid-Astar以车辆的运动学模型为节点
GqEKsRH的博客
03-13 949
该算法以车辆的运动学模型为节点，并以当前点到终点的Astar距离和RS距离两者最大的距离作为H(n)函数的估计代价。同时，文章的结构清晰流畅，文字内容充实丰富，尽量贴合给定的短语和关键词，使其看起来更像一篇实实在在的技术分析文章，而非广告软文。该算法以车辆的运动学模型为节点，并以当前点到终点的Astar距离和RS距离两者最大的距离作为H(n)函数的估计代价。以车辆的运动学模型为节点，以当前点到终点的Astar距离和RS距离两者最大的距离作为H(n)函数的估计代价，使用matlab实现（2016a以上版本）
hybrid-a-star-annotation:Hybrid A*路径规划器的代码注释
05-10
Hybrid A* ROS源码中文注释 本仓库是Hybrid A* 的ROS版代码注释，原始代码地址为: 整体注释依据的主要参考文献有： DOLGOV D, THRUN S, MONTEMERLO M, et al. Practical search techniques in path planning for autonomous driving[J]. Ann Arbor, 2008, 1001(48105):18-80. KURZERK. Path Planning in Unstructured Environments: A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle[Z].2016. OK K
深入解析Hybrid A*路径规划算法 - 逐行解读MATLAB版源码，带你熟悉实现过程，深入解析混合A*算法 - 逐行剖析MATLAB版Hybrid A*路径规划
最新发布
ZnBMlDCKuv的博客
06-08 876
具体而言，该算法将连续空间离散化为一组网格，并利用传统的A*算法在网格上进行路径搜索，从而减少了搜索空间的复杂性。同时，在离散空间的搜索基础上，通过采样和优化的技术，对路径进行优化，提高路径的质量和效率。算法是一种启发式搜索算法，通过估计每个节点到目标节点的代价，并结合节点到起始节点的实际代价，选择最优的节点进行搜索。通过深入理解算法的原理和实现细节，我们可以更好地应用该算法于自动驾驶和智能导航系统中，并在实际应用中进行相应的调整和优化，以提高路径规划的效率和质量。算法之前，我们先回顾一下A。
Apollo planning之hybrid A*
weixin_65089713的博客
10-21 3160
hybrid_a_star的目标是在开放空间中产生粗轨迹。Hybrid_a_star包含 node3d、grid_search、reeds_shepp_path和hybrid_a_star。hybrid_a_star是产生粗轨迹并调用grid_search和reeds_shepp_path的最重要组件。输入：当前点（计划起点），目标点（计划终点），ROI_xy_boundary（x和y的最大和最小边界值），障碍物顶点矢量（顶点位置信息）。
Hybrid_Astar算法原理
qq_37705385的博客
09-12 1851
Hybrid A*算法原理及实现
混合A星算法-Hybrid_Astar（matlab）
12-02
以车辆的运动学模型为节点，以当前点到终点的Astar距离和RS距离两者最大的距离作为H(n)函数的估计代价，使用matlab实现（2016a以上版本）
Hybrid Astar 算法剖析和实现（一）
穿越临界点的博客
05-02 2242
本篇主要介绍了一下Hybrid Astar算法的概貌，和重要组成部分，后文我们会将这几个要素一一攻克。
混合A*算法---Hybrid A Star
世界仙境与冷酷尽头's Blog
06-06 6254
混合A*算法在路径规划当中的应用
hybrid a*(混合A星算法-hybrid a star)
热门推荐
qq_31815513的博客
03-21 4万+
版权声明：本文为博主原创文章，转载注明来源。 2010年，斯坦福首次提出一种满足车辆运动学的算法(Hybrid A*),并在(DARPA)的城市挑战赛中得以运用。 2、Hybrid A*和A*区别 Hybrid A* A* 维数 H...
Hybrd A*(混合A*)算法
u010241908的博客
03-09 1万+
Hybrid A*算法是一种图搜索算法，改进于A*算法。与普通的A*算法区别在于，Hybrid A*规划的路径考虑了车辆的运动学约束，即满足了车辆的最大曲率约束。
matlab实现AStar和 HybridAStar算法
04-07
通过阅读和理解这些代码，你可以深入了解A*和Hybrid A*算法在实际问题中的应用，并且可以将其应用于自己的项目中。 总之，A*和Hybrid A*算法是路径规划领域中的重要工具，MATLAB提供了实现它们的强大支持。通过学习...
matlab实现AStar和 HybridAStar算法.zip
08-20
标题中的“matlab实现AStar和HybridAStar算法”是指使用MATLAB编程语言实现的两种路径搜索算法：A*（A-Star）算法和Hybrid A*算法。这两种算法是图论和路径规划领域的核心算法，广泛应用于机器人导航、游戏AI路径...
混合式A星代码解析_1 概述和框架
condom10010的博客
06-19 1696
混合式A星代码解析,代码来自Kurzer, Karl.
深入解析混合A*算法 - 逐行剖析MATLAB版Hybrid A*路径规划
DnRkOOHAqir的博客
05-04 1139
具体而言，该算法将连续空间离散化为一组网格，并利用传统的A*算法在网格上进行路径搜索，从而减少了搜索空间的复杂性。同时，在离散空间的搜索基础上，通过采样和优化的技术，对路径进行优化，提高路径的质量和效率。算法是一种启发式搜索算法，通过估计每个节点到目标节点的代价，并结合节点到起始节点的实际代价，选择最优的节点进行搜索。通过深入理解算法的原理和实现细节，我们可以更好地应用该算法于自动驾驶和智能导航系统中，并在实际应用中进行相应的调整和优化，以提高路径规划的效率和质量。算法之前，我们先回顾一下A。
多个子目标命中的Hybrid Astar路径规划-目的、原理、代码实现、使用技巧和参考repo
汤元杰tmj的专栏
04-08 678
给定起始位姿和终点位姿，生成一条长度尽可能短的无碰撞的满足曲率半径约束的路径；
Hybrid Astar 算法剖析和实现（二）
穿越临界点的博客
05-03 1693
本篇介绍了状态空间栅格，以及它的作用。为后续Hybrid Astar算法的剖析和实现打下了空间基础，不然后面遇到各种坐标和空间变换很容易迷失方向。
【路径规划】Hybrid A_Star算法机器人路径规划【含Matlab源码 1390期】
Matlab武动乾坤的博客
11-30 92
Hybrid A_Star算法机器人路径规划 完整的代码，方可运行；可提供运行操作视频！适合小白！
Apollo规划代码ros移植-混合A*
weixin_39735688的博客
06-09 2611
今天学习移植一下混合A*（Hybrid a）。想要工程代码的看我这一篇： Apollo6.0规划代码ros移植-路径规划可跑工程分享1.A start算法 使用A Star 算法实现自动寻路详解 2.Rs曲线 Reeds-Shepp和Dubins曲线简介[运动规划算法]Dubins曲线和Reeds-Shepp曲线 Hybrid A star是一种带有半径约束的路径平滑规划算法，算法思想来自A算法，但A是没有考虑平滑和半径约束的路径规划算法，且基于栅格地图的网格搜索算法，它们的目标代价函数是为了形成一条路径最
使用Cplusplus编写一个混合A*
09-12
### 回答1： A*算法是一种启发式搜索算法，它可以找到在地图或图形中从起点到终点的最佳路径。混合A*算法是一种结合了A*和其他算法的算法，可以提高搜索效率。 在C++中实现A*算法的基本步骤如下： 1. 定义起点和终点，并创建一个开启列表和一个关闭列表。 2. 把起点加入开启列表。 3. 当开启列表不为空时，执行以下操作： a. 从开启列表中取出第一个节点，并加入关闭列表。 b. 如果取出的节点是终点，则结束搜索。 c. 否则，扩展该节点的所有相邻节点。对于每个相邻节点，计算其F值（F=G+H），如果该节点不在开启列表或关闭列表中，则将其加入开启列表。 4. 使用关闭列表中的节点路径来找到从起点到终点的最佳路径。 如果要实现混合A*算法，需要在这个基础上进行扩展，可以使用其他算法来提高搜索效率，例如： - 使用启发函数优化A*算法 - 将A*算法与其他 ### 回答2： 混合A*（Hybrid A*）是一种路径搜索算法，结合了传统的图搜索和启发式搜索。它克服了图搜索的弱点，同时利用启发式函数快速找到最佳路径。 在C++中，我们可以使用混合A*算法来解决路径搜索问题。以下是一个简单的实现示例： 首先，我们需要定义一个地图结构，包含节点和边的信息。每个节点都有一个状态和一个启发式值。 ```cpp struct Node { int x, y; // 节点坐标 double heuristic; // 启发式值 // 其他节点信息... }; struct Edge { Node* src; // 起始节点 Node* dest; // 目标节点 double cost; // 边的代价 // 其他边的信息... }; std::vector<Node*> nodes; // 存储节点的容器 std::vector<Edge*> edges; // 存储边的容器 ``` 接下来，我们需要实现混合A*算法的主要函数。首先，定义一个启发式函数，用于估算从当前节点到目标节点的距离。一种常用的启发式函数是欧几里得距离。 ```cpp double heuristic(const Node* node, const Node* goal) { // 欧几里得距离启发式函数 return std::sqrt(std::pow(node->x - goal->x, 2) + std::pow(node->y - goal->y, 2)); } ``` 然后，实现混合A*算法的主要函数，该函数接受起始节点和目标节点作为参数。 ```cpp std::vector<Node*> hybridAStar(const Node* start, const Node* goal) { std::vector<Node*> path; // 存储最佳路径的容器 // 初始化 std::vector<Node*> openSet; std::vector<Node*> closedSet; openSet.push_back(start); while (!openSet.empty()) { // 在openSet中找到启发式值最小的节点 Node* currentNode = openSet[0]; double currentHeuristic = currentNode->heuristic; for (const auto& node : openSet) { if (node->heuristic < currentHeuristic) { currentNode = node; currentHeuristic = node->heuristic; } } // 如果当前节点是目标节点，已找到最佳路径 if (currentNode == goal) { // 构造路径 while (currentNode != nullptr) { path.insert(path.begin(), currentNode); currentNode = currentNode->parent; } break; } // 将当前节点从openSet中移除，并添加到closedSet中 openSet.erase(std::find(openSet.begin(), openSet.end(), currentNode)); closedSet.push_back(currentNode); // 扩展当前节点 for (const auto& edge : edges) { if (edge->src == currentNode) { Node* neighbor = edge->dest; if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end()) { continue; // 已在closedSet中，跳过 } double newCost = currentNode->cost + edge->cost; // 如果邻居节点不在openSet中，或者新的路径代价更小，则更新邻居节点 if (std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end() || newCost < neighbor->cost) { neighbor->cost = newCost; neighbor->heuristic = newCost + heuristic(neighbor, goal); neighbor->parent = currentNode; if (std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end()) { openSet.push_back(neighbor); } } } } } return path; } ``` 在主函数中，我们可以使用上述函数来搜索路径。 ```cpp int main() { // 创建节点和边的示例 // 调用混合A*算法 Node* startNode = // 设置起始节点; Node* goalNode = // 设置目标节点; std::vector<Node*> path = hybridAStar(startNode, goalNode); // 打印最佳路径 for (const auto& node : path) { std::cout << "(" << node->x << "," << node->y << ") "; } std::cout << std::endl; return 0; } ``` 以上是一个基本的混合A*算法的C++实现示例。根据具体的问题需求，你可能需要进行一些调整和修改。 ### 回答3： 混合A* 是一种通过结合传统的 A* 算法和 Dijkstra 算法来解决路径规划问题的方法。它可以较好地处理具有高度不确定性和可变代价的问题，特别是在动态环境下。以下是使用 C++ 编写的一个简单的混合 A* 算法示例： ```cpp #include <iostream> #include <vector> #include <queue> #include <cmath> using namespace std; // 定义节点结构 struct Node { int x, y; // 坐标 double g, h; // 已知代价和预测代价 Node* parent; // 父节点指针 Node(int x, int y, double g, double h, Node* parent) : x(x), y(y), g(g), h(h), parent(parent) {} double f() const { return g + h; } // 计算总代价 bool operator<(const Node& other) const { return f() > other.f(); } // 重载 < 运算符，用于优先队列排序 }; // 计算两个节点之间的欧几里得距离 double heuristic(const Node& node1, const Node& node2) { return sqrt(pow(node1.x - node2.x, 2) + pow(node1.y - node2.y, 2)); } // 检查节点是否在封闭列表中 bool isInClosedList(const Node& node, const vector<Node>& closedList) { for (const auto& closedNode : closedList) { if (node.x == closedNode.x && node.y == closedNode.y) { return true; } } return false; } // 检查节点是否在开放列表中，如果在开放列表中，返回对应的迭代器，否则返回开放列表的末尾迭代器 vector<Node>::iterator findInOpenList(const Node& node, vector<Node>& openList) { return find_if(openList.begin(), openList.end(), [&node](const Node& openNode) { return node.x == openNode.x && node.y == openNode.y; }); } // 根据节点生成路径 vector<pair<int, int>> generatePath(const Node& node) { vector<pair<int, int>> path; const Node* currentNode = &node; while (currentNode != nullptr) { path.push_back(make_pair(currentNode->x, currentNode->y)); currentNode = currentNode->parent; } reverse(path.begin(), path.end()); return path; } // 混合A*算法 vector<pair<int, int>> hybridAStar(const vector<vector<int>>& grid, const pair<int, int>& start, const pair<int, int>& goal) { const int rows = grid.size(); const int cols = grid[0].size(); // 定义节点创建表 vector<vector<Node>> nodeGrid(rows, vector<Node>(cols, Node(0, 0, 0, 0, nullptr))); priority_queue<Node> openList; // 开放列表 vector<Node> closedList; // 封闭列表 // 初始化起点和目标节点 Node startNode(start.first, start.second, 0, heuristic(startNode, goalNode), nullptr); Node goalNode(goal.first, goal.second, 0, 0, nullptr); openList.push(startNode); nodeGrid[start.first][start.second] = startNode; while (!openList.empty()) { Node currentNode = openList.top(); openList.pop(); // 到达目标节点，生成路径并返回 if (currentNode.x == goalNode.x && currentNode.y == goalNode.y) { return generatePath(currentNode); } closedList.push_back(currentNode); // 遍历四个方向上的邻居节点 const vector<int> dx = {-1, 1, 0, 0}; const vector<int> dy = {0, 0, -1, 1}; for (int i = 0; i < 4; ++i) { int newX = currentNode.x + dx[i]; int newY = currentNode.y + dy[i]; // 忽略超出网格范围和障碍物的节点 if (newX < 0 || newX >= rows || newY < 0 || newY >= cols || grid[newX][newY] == 1) { continue; } // 创建邻居节点 Node neighbor(newX, newY, currentNode.g + 1, heuristic({newX, newY}, goalNode), &currentNode); // 如果邻居节点已经在封闭列表中，忽略它 if (isInClosedList(neighbor, closedList)) { continue; } // 检查邻居节点是否存在于开放列表中 auto iter = findInOpenList(neighbor, openList); if (iter != openList.end()) { // 如果邻居节点的 g 值变小，更新父节点为当前节点，并更新 g 和 f 值 if (neighbor.g < iter->g) { iter->g = neighbor.g; iter->parent = &currentNode; } } else { // 否则，将邻居节点加入到开放列表中 openList.push(neighbor); nodeGrid[newX][newY] = neighbor; } } } // 找不到路径，返回空路径 return {}; } int main() { vector<vector<int>> grid = { {0, 0, 0, 0, 0}, {0, 1, 1, 1, 0}, {0, 0, 0, 0, 0}, {0, 1, 1, 1, 0}, {0, 0, 0, 0, 0}, }; pair<int, int> start = make_pair(0, 0); pair<int, int> goal = make_pair(4, 4); vector<pair<int, int>> path = hybridAStar(grid, start, goal); cout << "Path: "; for (const auto& point : path) { cout << "(" << point.first << ", " << point.second << ") "; } cout << endl; return 0; } ``` 以上是一个简单的使用 C++ 编写混合 A* 算法的示例，它使用了优先队列来实现开放列表，vector 来实现封闭列表和节点创建表，通过欧几里得距离作为启发函数来估计预测代价。在网格上进行路径规划时，它可以找到起点到目标节点的最短路径并返回一个包含路径坐标的向量。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

tmjtyj

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

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

余额充值