【PNC】AStar及常用规划算法原理与实现

news2024/9/22 15:28:17

😏★,°:.☆(￣▽￣)/$:.°★ 😏
这篇文章主要介绍AStar规划算法原理与实现。
学其所用，用其所学。——梁启超
欢迎来到我的博客，一起学习，共同进步。
喜欢的朋友可以关注一下，下次更新不迷路🥞

文章目录

    • :smirk:1. AStar介绍
    • :satisfied:2. 应用示例
    • :satisfied:3. 其他常用算法示例

😏1. AStar介绍

A* 算法是一种常用的启发式搜索算法，用于解决图形中的路径规划问题。它是一种通用的图搜索算法，适用于各种离散空间搜索问题，包括游戏中的路径规划、机器人运动规划等。

A* 算法结合了广度优先搜索和贪婪最佳优先搜索的优点。它在搜索过程中利用了一个启发式函数（称为估价函数）来估计从当前节点到目标节点的代价。这个估价函数通常使用欧氏距离、曼哈顿距离等来度量节点之间的距离。

A* 算法的基本思想如下：

  1. 创建一个开放列表（open list）和一个关闭列表（closed list），以及一个存储每个节点的代价的数据结构。
  2. 将起始节点添加到开放列表，并将其代价设置为从起始节点到目标节点的估计代价。
  3. 当开放列表不为空时，执行以下步骤：

从开放列表中选择具有最低代价的节点作为当前节点。
如果当前节点是目标节点，则已找到路径，结束搜索。
将当前节点移入关闭列表。
对当前节点周围的每个相邻节点执行以下步骤：
1.如果相邻节点不可通过或者已经在关闭列表中，则忽略它。
2.如果相邻节点不在开放列表中，则将其添加到开放列表，并计算从起始节点到该节点的代价。
3,如果相邻节点在开放列表中，则比较当前路径是否更好（代价更低）。如果是，则更新相邻节点的代价值和父节点。

  1. 如果开放列表为空，表示无法到达目标节点，搜索失败。
    A* 算法通过启发式函数来引导搜索，使其更加高效。通过不断选择估计代价最低的节点进行扩展，A* 算法倾向于沿着代价最低的路径搜索，从而更快地接近目标节点。

😆2. 应用示例

学习Github项目地址（仅学习用）：https://github.com/JokerEyeAdas/AStarShellMapSearch

编译运行：g++ CAstar.cpp AStarTest.cpp -o AstarShellMapTest

在这里插入图片描述
该案例实现了在shell终端中显示10*10的地图，设置起点和终点，用寻路函数执行，若能找到全局路径，则用绿色的块表示路径，否则不显示并提示有障碍物。

由这个文件组成：ShellMap.hpp、CAstar.h、CAstar.cpp和AStarTest.cpp组成。

CAstar.h定义了障碍类型和openlist&closelist，以及f=g+h评价函数：

#ifndef __Astar__CAstar__
#define __Astar__CAstar__

#include <stdio.h>
#include <vector>
#include <iostream>
#include <algorithm>
#include "ShellMap.hpp"

using namespace std;

//地图最大值
#define MAX_X 10
#define MAX_Y 10

enum class AType
{
    ATYPE_UNKNOWN,
    ATYPE_CLOSED,
    ATYPE_OPENED,
    ATYPE_BARRIER   //障碍
};

class APoint
{
public:
    APoint();
    ~APoint();
    int x;
    int y;
    AType type;   //类型:障碍、开放列表、关闭列表
    int f;  //f = g+h
    int g;
    int h;
    APoint *parent;
    bool operator == (const APoint& po)
    {
        if (x == po.x && y == po.y)
        {
            return true;
        }
        return false;
    }
   
};

class CAstar
{
    vector<APoint*> _openList;      //开放列表
    vector<APoint*> _closeList;     //关闭列表
    vector<APoint*> _neighbourList; //周边节点
    APoint* _endPoint;
    APoint* _curPoint;
    vector< vector<APoint*> > _allPoints;
    ShellMap shellMap_;
public:
    CAstar();
    ~CAstar();
    APoint* findWay(APoint* beginPoint,APoint* endPoint,vector< vector<APoint*> >& allPoints);
    ShellMap& GetMap() {return shellMap_;};
//    APoint* findWay(int beginX,int beginY,int endX,int endY);
private:
    int getF(APoint *point);
    int getH(APoint *point);
    vector<APoint*> getNeighboringPoint(APoint* point);
};

#endif

CAstar.cpp主要是启发函数的实现：

#include "CAstar.h"

bool mySort(const APoint* p1,const APoint* p2)
{
    return p1->f < p2->f;
}

APoint::APoint():x(0)
                ,y(0)
                ,h(0)
                ,f(0)
                ,g(0)
                ,parent(nullptr)
                ,type(AType::ATYPE_UNKNOWN)
{
}
APoint::~APoint()
{
}

#pragma mark------CAstar-------

CAstar::CAstar():_endPoint(nullptr)
                ,_curPoint(nullptr)
{
    shellMap_.GenMap(MAX_X, MAX_Y);
    //shellMap_.SetStartEndPoint(Point(1, 1), Point(6, 6));
}

CAstar::~CAstar()
{
    _openList.clear();
    _closeList.clear();
    _neighbourList.clear();
    _allPoints.clear();
}

APoint* CAstar::findWay(APoint *beginPoint, APoint *endPoint,vector< vector<APoint*> >& allPoints)
{
    shellMap_.SetStartEndPoint(Point(beginPoint->x, beginPoint->y), Point(endPoint->x, endPoint->y));
    shellMap_.ShowMap();
    //传递地图
    _allPoints = allPoints;
    _endPoint = endPoint;

    if (_endPoint->type == AType::ATYPE_BARRIER)
    {
        cout<<"ERR the final point is barrier!!"<<endl;
        return nullptr;
    }
    if (*_endPoint == *beginPoint)
    {
        cout<<"起始点相同"<<endl;
        return nullptr;
    }
    
    _openList.push_back(beginPoint);
    beginPoint->type = AType::ATYPE_OPENED;
    beginPoint->f = getF(beginPoint);

    do
    {
        //获取最小值的节点
        _curPoint = _openList[0];
        _openList.erase(_openList.begin());
        _curPoint->type = AType::ATYPE_CLOSED;
        _closeList.push_back(_curPoint);
        
        //GetMap().GetMapPtr()[_curPoint->y * MAX_X + _curPoint->x] = CUR;
        if (*_curPoint == *_endPoint)
        {
            cout<<"have find way"<<endl;
            return _curPoint;
        }
        //获取相邻的节点
        vector<APoint*> neVec = getNeighboringPoint(_curPoint);
        for (int i = 0; i<neVec.size(); i++)
        {
            auto tmpoint = neVec[i];
            if (tmpoint->type == AType::ATYPE_CLOSED)
            {
                //GetMap().GetMapPtr()[tmpoint->y * MAX_X + tmpoint->x] = CLOSE;
                continue;
            }
            //是否在开放列表里
            if (tmpoint->type != AType::ATYPE_OPENED)
            {
                tmpoint->parent = _curPoint;
                tmpoint->g = _curPoint->g + 10;
                //计算H值
                tmpoint->h = getH(tmpoint);
                //添加到开放列表里
                _openList.push_back(tmpoint);
                tmpoint->type = AType::ATYPE_OPENED;

                //GetMap().GetMapPtr()[tmpoint->y * MAX_X + tmpoint->x] = OPEN;
            }
            else
            {
                //已经在开放列表里
                if (tmpoint->h < _curPoint->h)
                {
                    tmpoint->parent = _curPoint;
                    tmpoint->g = _curPoint->g + 10;
                    //GetMap().GetMapPtr()[tmpoint->y * MAX_X + tmpoint->x] = OPEN;
                }
            }
        }
        //排序 F值最小的排在前面
        sort(_openList.begin(), _openList.end(), mySort);
        //GetMap().ShowMap();
        //sleep(500);
    } while (_openList.size()>0);
    
    
    cout<<"---can not find way---"<<endl;
    
    return nullptr;
}

int CAstar::getF(APoint *point)
{
    return (point->g + getH(point));
}
int CAstar::getH(APoint *point)
{
    //曼哈顿城市街区估算法
    return (abs(_endPoint->y - point->y) + abs(_endPoint->x - point->x))*10;
}

vector<APoint*> CAstar::getNeighboringPoint(APoint *point)
{
    _neighbourList.clear();
//    cout<<"nei size:"<<_neighbourList.size()<<endl;
    if (point->x < MAX_X-1)
    {
        if (_allPoints[point->x+1][point->y]->type != AType::ATYPE_BARRIER)
        {
            _neighbourList.push_back(_allPoints[point->x+1][point->y]);
        }
    }
    if (point->x >0)
    {
        if (_allPoints[point->x-1][point->y]->type != AType::ATYPE_BARRIER)
        {
            _neighbourList.push_back(_allPoints[point->x-1][point->y]);
        }
    }
    if (point->y < MAX_Y-1)
    {
        if (_allPoints[point->x][point->y+1]->type != AType::ATYPE_BARRIER)
        {
            _neighbourList.push_back(_allPoints[point->x][point->y+1]);
        }
    }
    if (point->y >0)
    {
        if (_allPoints[point->x][point->y-1]->type != AType::ATYPE_BARRIER)
        {
            _neighbourList.push_back(_allPoints[point->x][point->y-1]);
        }
    }
    
    return _neighbourList;
}

AStarTest.cpp是测试示例：

#include "CAstar.h"

int main()
{
    /*ShellMap map(10, 10);
    map.SetStartEndPoint(Point(1, 1), Point(8, 8));
    map.ShowMap();
    */
    auto star = new CAstar();
    vector< vector<APoint*> > map;
    for (int i = 0; i < MAX_X; i++)
    {
        vector<APoint*> tmp;
        for (int j = 0; j < MAX_Y; j++)
        {
            APoint *point = new APoint();
            point->x = i;
            point->y = j;
            if (star->GetMap().GetMapPtr()[j * MAX_X + i] == BLOCK)
            {
                point->type = AType::ATYPE_BARRIER;
            }
            tmp.push_back(point);
        }
        map.push_back(tmp);
    }

    //开始寻路
    auto point = star->findWay(map[0][0], map[9][9], map);
   
    if (!point)
    {
        return 0;
    }
    
    while (point)
    {
        if (star->GetMap().GetMapPtr()[point->y * MAX_X + point->x] == START || star->GetMap().GetMapPtr()[point->y * MAX_X + point->x] == END) {
            ;
        } else { 
            star->GetMap().GetMapPtr()[point->y * MAX_X + point->x] = PATH;
        }
        point = point->parent;
    }
    star->GetMap().ShowMap();
    //-------------释放内存----------
    delete star;
    
    for (int i = 0; i<MAX_X; i++)
    {
        for (int j = 10; j<MAX_Y; j++)
        {
            delete map[i][j];
            map[i][j] = nullptr;
        }
    }
    
    return 0;
}

😆3. 其他常用算法示例

项目Github地址：https://github.com/czjaixuexi/path_planning

该项目实现了astar、dijkstra、rrt规划算法和bezier、b-spline曲线生成，并用matplotlibcpp.h调用python的matplot库实现了图形化显示，清楚地展示了规划算法和曲线的生成过程，可以学习。

EMPlanner项目Github地址：https://github.com/reflector-li/EMplanner

EM planner是百度Apollo自动驾驶系统的路径规划算法。通过将非凸轨迹规划问题分解为路径规划和速度规划两个子问题，并基于matplotlibcpp.h实现可视化显示规划器的基本功能。

相关库的安装可参考：http://t.csdn.cn/UoWsg

运行如下：

在这里插入图片描述

在这里插入图片描述

以上。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/855481.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

网工内推 | 信息安全负责人，需8年安全经验，CISSP证书

网工内推 | 信息安全负责人，需8年安全经验，CISSP证书

01 上海鹰角网络 招聘岗位&#xff1a;信息安全负责人 职责描述&#xff1a; 1、负责公司总体的信息安全规划、信息安全管理体系、流程、制度的设计和优化&#xff0c;确保在运营、应用、信息和业务等方面的持续安全、稳定&#xff1b; 2、负责对系统&#xff0c;网络&#xf…
阅读更多...
内网横向移动—WinLinux内存离线读取Hashcat破解RDPSSH存储提取

内网横向移动—WinLinux内存离线读取Hashcat破解RDPSSH存储提取

内网横向移动—Win&Linux&内存离线读取&Hashcat破解&RDP&SSH存储提取 1. 前言2. Windows系统密码提取2.1. 在线读取2.2. 离线读取2.2.1. 无存储读取2.2.1.1. 读取文件2.2.1.2. 本地解密 2.2.2. 有存储读取 2.3. 解决高版本2.3.1. 修改注册表2.3.2. 重新登陆…
阅读更多...
Unity3d C#利用本地网页快速打开萤石云监控视频流(ezopen)实现云台,声音等控制,支持WebGL平台，替代UMP播放（含源码）

Unity3d C#利用本地网页快速打开萤石云监控视频流(ezopen)实现云台,声音等控制,支持WebGL平台，替代UMP播放（含源码）

前言 之前我介绍了替代Universal?Media?PlayerUMP播放石云监控视频流(ezopen)的功能&#xff0c;效果还是很明显的&#xff0c;笔者的测试是差不多3-5秒就能打开监控画面&#xff0c;不过稍微遗憾的是&#xff0c;之前的功能是iframe打开石云提供的播放网页的形式&#xff0…
阅读更多...
SSM——环境搭建、产品操作、订单操作

SSM——环境搭建、产品操作、订单操作

SSM 环境搭建与产品操作 1. 环境准备 1.1 数据库与表结构 1.1.1 创建用户与授权 数据库我们使用 Oracle Oracle 为每个项目创建单独 user &#xff0c; oracle 数据表存放在表空间下&#xff0c;每个用户有独立表空间 创建用户及密码 语法 [ 创建用户 ] &#xff1a; crea…
阅读更多...
js 正则表达式

js 正则表达式

js 正则表达式 http://tool.oschina.net/regex https://developer.mozilla.org/zh-CN/docs/Web/JavaScript/Guide/Regular_Expressions 11 22
阅读更多...
【大厂面试必备】网络收发数据及断开服务器（四次挥手）

【大厂面试必备】网络收发数据及断开服务器（四次挥手）

接上一篇&#xff1a;【网络知识面试】初识协议栈和套接字及连接阶段的三次握手  前面我们了解到服务器和客户端在创建套接字&#xff0c;建立连接后&#xff0c;就可以进入到下一步&#xff0c;双发可以互相发送和接收数据&#xff0c;本篇博客就来学习一下这个过程。  我们…
阅读更多...
C++初阶——拷贝构造和运算符重载（const成员）

C++初阶——拷贝构造和运算符重载（const成员）

目录 1. 拷贝构造函数 1.2 拷贝构造函数特征&#xff1a; 2. 默认拷贝构造函数 2.1 未显式定义&#xff0c;编译器会生成默认的拷贝构造函数。 默认的拷贝构造函数对象按内存存储按字节序完成拷贝&#xff0c;这种拷贝叫做浅拷贝&#xff0c;或者值拷贝 3. 运算符重载 3.1…
阅读更多...
机器人CPP编程基础-01第一个程序Hello World

机器人CPP编程基础-01第一个程序Hello World

很多课程先讲C/C或者一些其他编程课&#xff0c;称之为基础课程。然后到本科高年级进行机器人专业课学习&#xff0c;这样时间损失非常大&#xff0c;效率非常低。 C/单片机/嵌入式/ROS等这些编程基础可以合并到一门课中进行实现&#xff0c;这些素材已经迭代三轮以上&#xf…
阅读更多...
C# Equals()方法报错：NullReferenceException was unhandled

C# Equals()方法报错：NullReferenceException was unhandled

下面是一个C# Equals()方法的例子&#xff0c;执行时报错了 static void Main(string[] args) {string name "sandeep";string myName null;Console.WriteLine(" operator result is {0}", name myName);Console.WriteLine("Equals method result…
阅读更多...
中通快递：短期财务前景良好，长期财务业绩将遭受严重打击

中通快递：短期财务前景良好，长期财务业绩将遭受严重打击

来源&#xff1a;猛兽财经 作者&#xff1a;猛兽财经 华尔街分析师对中通快递的短期财务前景预测 华尔街分析师目前预测中通快递&#xff08;ZTO&#xff09;将在2023财年全年产生一份相当不错的财务业绩。 根据S&P Capital IQ的数据&#xff0c;在过去的6个月里&#xff…
阅读更多...
代码随想录算法训练营第24天| 第七章 回溯算法part01 理论基础、leetcode 77

代码随想录算法训练营第24天| 第七章 回溯算法part01 理论基础、leetcode 77

Part I : 回溯算法基础 背景&#xff1a;一直以来都是半懂不懂的&#xff0c;在逻辑上不难&#xff0c;毕竟属于暴力搜索&#xff1b;在代码上就开始缠绕起来了&#xff0c;自己研究的时候对N皇后问题老是理不清。这次终于在Carl这开始前进啦&#xff01;何为回溯算法&#xf…
阅读更多...
【C++】STL——set/multiset 和 map/multimap的使用

【C++】STL——set/multiset 和 map/multimap的使用

文章目录 1. 关联式容器2. 树形结构的关联式容器3. set3.1 认识set3.1 set的使用 4. multiset5. map5.1 认识map5.2 pair5.3 map的使用对map中[]的理解 6. multimap 1. 关联式容器 在初阶阶段&#xff0c;我们已经接触过STL中的部分容器 比如&#xff1a;vector、list、deque、…
阅读更多...
VoxWeekly｜The Sandbox 生态周报｜20230807

VoxWeekly｜The Sandbox 生态周报｜20230807

欢迎来到由 The Sandbox 发布的《VoxWeekly》。我们会在每周发布&#xff0c;对上一周 The Sandbox 生态系统所发生的事情进行总结。 如果你喜欢我们内容&#xff0c;欢迎与朋友和家人分享。请订阅我们的 Medium 、关注我们的 Twitter&#xff0c;并加入 Discord 社区&#xf…
阅读更多...
c++--AVL树简单实现

c++--AVL树简单实现

1.什么是AVL树 AVL树就是在搜索二叉树的基础上通过控制左右子树的高度差实现的&#xff0c;在搜索二叉树的基础上&#xff0c;通过旋转来控制&#xff0c;是左右子树高度差的绝对值严格控制为不超过1&#xff08;通过旋转来控制树的高度&#xff09;。由于搜索二叉树的效率最差…
阅读更多...
一起学SF框架系列7.1-spring-AOP-基础知识

一起学SF框架系列7.1-spring-AOP-基础知识

AOP(Aspect-oriented Programming-面向切面编程&#xff09;是一种编程模式&#xff0c;是对OOP(Object-oriented Programming-面向对象编程&#xff09;一种有益补充。在OOP中&#xff0c;万事万物都是独立的对象&#xff0c;对象相互耦合关系是基于业务进行的&#xff1b;但在…
阅读更多...
【逗老师的PMP学习笔记】8、项目质量管理

【逗老师的PMP学习笔记】8、项目质量管理

目录 一、规划质量管理1、质量管理的发展历史2、戴明环&#xff0c;PDCA理论3、【关键输入】事业环境因素4、【关键输入】成本效益分析5、【关键工具】质量成本6、【关键输出】质量管理计划7、插一嘴&#xff0c;项目的三个标准8、【关键工具】质量测量指标 二、管理质量1、【关…
阅读更多...
[OnWork.Tools]系列 06-屏幕水印

[OnWork.Tools]系列 06-屏幕水印

简介 屏幕水印功能主要是在开会分享屏幕的时候在屏幕上增加水印 水印使用 水印启用和颜色设置 水印文字和大小设置 水印间距,透明度,角度调整
阅读更多...
保护电脑健康，这些维护技巧你Get了吗？

保护电脑健康，这些维护技巧你Get了吗？

文章目录 1.界面环境1.1合理布置终端桌面1.2清理垃圾信息1.3关注运行环境和系统信息 2.程序管理2.1安装软件时需谨慎2.2及时更新软件和操作系统2.3合理管理插件和工具栏 3.网络防护3.1保护个人隐私3.2防范网络攻击3.3备份重要数据 4.电源管理4.1合理关机和电源设置4.2定期清理灰…
阅读更多...
ESP32开发阶段启用 Secure Boot 与 Flash encryption

ESP32开发阶段启用 Secure Boot 与 Flash encryption

Secure Boot 与 Flash encryption详情 请参考&#xff1a;https://blog.csdn.net/espressif/article/details/79362094 1、开发环境 AT版本&#xff1a;2.4.0.0 发布IDF 与 python&#xff1a; idf4.3_py3.10_env系统&#xff1a;虚拟机 ubuntu 20 2、使能 secure boot 和 …
阅读更多...
手搓 自然语言模型 LLM 拆分em结构设计 网络参数对比

手搓 自然语言模型 LLM 拆分em结构设计 网络参数对比

数据 数据集 新的em编码参数表 voc_sizehidden_sizetotaltotal Bmax_lensecondsdays65536512374865920.03749B10242560.2655361024828375040.08284B20485120.5655362048<
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023