手撕A*算法(详解A*算法)

A*算法原理

全局路径规划算法,根据给定的起点和终点在全局地图上进行总体路径规划。

导航中使用A*算法计算出机器人到目标位置的最优路线,一般作为规划的参考路线

在这里插入图片描述

// 定义地图上的点
struct Point
{int x,y; // 栅格行列Point(int x, int y):x(x),y(y){}; // 参数列表初始化double distance(Point& p)        // 求距离{return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)); // 欧几里得距离}
};
// 定义节点
struct Node
{Point point; // 栅格点double g,h,f;// 代价值,f总价值,g到起点的代价值,h到终点的估计代价(启发式函数)Node *parent;// 父节点指针Node(Point point, double g, double h, Node* parent = nullptr):point(point), g(g), h(h), f(g+h), parent(parent){}
};

在这里插入图片描述

// 定义地图vector<vector<int>> gridmap = {{0, 1, 0, 0, 0},{0, 0, 1, 0, 0},{0, 0, 1, 1, 0},{0, 0, 1, 0, 0},{0, 0, 1, 1, 0}};// 定义起点和终点Point start{0, 0};Point goal{4, 4};

A*算法的寻路原理
在这里插入图片描述

A的结束条件
在这里插入图片描述
A
算法的寻路详细步骤
在这里插入图片描述
在这里插入图片描述

手撕A*代码

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
using namespace std;
// 定义地图上的点
struct Point
{int x,y; // 栅格行列Point(int x, int y):x(x),y(y){}; // 参数列表初始化double distance(Point& p)        // 求距离{return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)); // 欧几里得距离}
};// 定义节点
struct Node
{Point point; // 栅格点double g,h,f;// 代价值,f总价值,g到起点的代价值,h到终点的估计代价(启发式函数)Node *parent;// 父节点指针Node(Point point, double g, double h, Node* parent = nullptr):point(point), g(g), h(h), f(g+h), parent(parent){}
};// 自定义Node*排序规则
struct NodeCompare{bool operator()(Node* n1, Node* n2){return (n1->f) < (n2->f); // 表示升序排列}
};
// 基于栅格地图的路径规划A*算法,返回由Point组成的路径path,输入为地图,起点和终点
vector<Point> AstarPathPlanning(vector<vector<int>> &gridmap, Point& start, Point& goal)
{// 获取地图参数int row = gridmap.size(); // 行,表示地图的宽度int col = gridmap[0].size(); // 列,表示地图的长// 定义openlist, closelistvector<Node *> openlist; // openlist 表示待搜索的节点vector<Node *> closelist;// closelist表示已搜索的节点openlist.push_back(new Node(start, start.distance(start), start.distance(goal))); // 将起点加入openlist中,作为初始化int count1 = 1;// 进入循环,开始搜索,搜索到终点则返回路径vector<Point> path;while (!openlist.empty()) // 当openlist为空,表示所有可搜索节点已经被搜索,此时循环结束{// 获取当前搜索节点current,即openlist中f最小节点sort(openlist.begin(), openlist.end(), NodeCompare{}); // 先对openlist排序,这里自定义排序规则(从小到大)Node* current = *openlist.begin(); // *openlist.begin()排序后即为f最小的迭代器位置// 将current对应的元素从openlist中删除openlist.erase(openlist.begin());// 将current加入到closelist中closelist.push_back(current);// 对当前搜索节点current进行分类讨论// 1-current是终点,则返回路径,表示找到路径if (current->point.x == goal.x && current->point.y == goal.y){while (current != nullptr) // 利用父节点,从终点向起点回溯最短路径,因为起点没有父节点,所以起点current父节点为nullptr{path.push_back(current->point);current = current->parent;}reverse(path.begin(), path.end()); // 路径是反的,翻转路径int count2 = 0; // delete 次数for (auto o : openlist){delete o;count2++;}for (auto c : closelist){delete c;count2++;}cout << "new times: " << count1 << endl;cout << "delete times: " << count2 << endl;return path;}// 2-current 不是终点,需要讨论其邻近的节点neighborsint x = current->point.x;int y = current->point.y;vector<Point> neighbors = { // 8个邻近节点的坐标{x-1,y-1}, {x-1,y}, {x-1,y+1},{x,y-1},     {x,y+1},{x+1,y-1}, {x+1,y}, {x+1,y+1}};// 遍历所有的临近节点,每一个邻近节点n必须满足在地图范围内同时不是障碍物for (auto n : neighbors){if ((n.x >= 0 && n.x < row) && (n.y >= 0 && n.y < col) && gridmap[n.x][n.y]==0){// 1 n在closelist中,表示已经搜索过了,此时直接跳过bool incloselist = false;for (auto c : closelist){if (c->point.x == n.x && c->point.y == y){incloselist = true;break;}}if (incloselist){continue;}// 2 n是否在openlist中进行讨论bool inopenlist = false;for (auto o : openlist){if (o->point.x == n.x && o->point.y == n.y){inopenlist = true;// n 在openlist中,对比f值,更新代价值和父节点parentdouble g = current->g + n.distance(current->point); // 临近节点n到起点的距离 = 当前搜索节点current到起点的距离 + 当前搜索节点current到邻近节点n距离double h = n.distance(goal); // 临近节点n到终点的估计距离代价double f = g + h;if (f < (o->f)){o->f = f;o->parent = current;}break;}}if (!inopenlist) // n不在openlist中,对比f值,计算代价值,添加到openlist中,下次备选{double g = current->g + n.distance(current->point); // 临近节点n到起点的距离 = 当前搜索节点current到起点的距离 + 当前搜索节点current到邻近节点n距离double h = n.distance(goal); // 临近节点n到终点的估计距离代价double f = g + h;openlist.push_back(new Node(n,g,h,current));count1++;}}}}// 搜索完成没有路径,表示路径规划失败,此时返回空路径return path;
}
int main()
{// 定义地图vector<vector<int>> gridmap = {{0, 1, 0, 0, 0},{0, 0, 1, 0, 0},{0, 0, 1, 1, 0},{0, 0, 1, 0, 0},{0, 0, 1, 1, 0}};// 定义起点和终点Point start{0, 0};Point goal{4, 4};vector<Point> path = AstarPathPlanning(gridmap, start, goal);cout << path.size() << endl;for (auto p : path){if (p.x == goal.x && p.y == goal.y){cout << "(" << p.x << ',' << p.y << ")" << endl;}else{cout << "(" << p.x << ',' << p.y << ")" << "->";}}return 0;
}

A*算法总结

把起点加入 open list 。
重复如下过程:
a. 遍历 open list ,查找 F 值最小的节点,把它作为当前要处理的节点。
b. 把这个节点移到 close list 。
c. 对当前方格的 8 个相邻方格的每一个方格?
◆ 如果它是不可抵达的或者它在 close list 中,忽略它。否则,做如下操作。
◆ 如果它不在 open list 中,把它加入 open list ,并且把当前方格设置为它的父亲,记录该方格的 F , G 和 H 值。
◆ 如果它已经在 open list 中,检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好,用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样,把它的父亲设置为当前方格,并重新计算它的 G 和 F 值。如果你的 open list 是按 F 值排序的话,改变后你可能需要重新排序。
d. 停止,当你
◆ 把终点加入到了 open list 中,此时路径已经找到了,或者
◆ 查找终点失败,并且 open list 是空的,此时没有路径。
3.保存路径。从终点开始,每个方格沿着父节点移动直至起点,这就是你的路径。

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

java学习part18抽象类

Java抽象类 详解-CSDN博客 111-面向对象(高级)-抽象类与抽象方法的使用_哔哩哔哩_bilibili 1.概念 2.抽象类 抽象类不能实例化&#xff0c;可以有属性&#xff0c;也可以有方法。 方法可以实现或者只声明不实现&#xff0c;要加一个abstract abstract class A{//定义一个抽…

springboot整合redis+自定义注解+反射+aop实现分布式锁

1.定义注解 import java.lang.annotation.*; import java.util.concurrent.TimeUnit;/** Author: best_liu* Description:* Date: 16:13 2023/9/4* Param * return **/ Retention(RetentionPolicy.RUNTIME) Target({ElementType.METHOD}) Documented public interface RedisLo…

详细学习Pyqt5中的5种布局方式

Pyqt5相关文章: 快速掌握Pyqt5的三种主窗口 快速掌握Pyqt5的2种弹簧 快速掌握Pyqt5的5种布局 快速弄懂Pyqt5的5种项目视图&#xff08;Item View&#xff09; 快速弄懂Pyqt5的4种项目部件&#xff08;Item Widget&#xff09; 快速掌握Pyqt5的6种按钮 快速掌握Pyqt5的10种容器&…

Go语言基础:包、函数、语句和注释解析

一个 Go 文件包含以下几个部分&#xff1a; 包声明导入包函数语句和表达式 看下面的代码&#xff0c;更好地理解它&#xff1a; 例子 package mainimport "fmt"func main() { fmt.Println("Hello World!") }例子解释 第 1 行&#xff1a; 在 Go 中&am…

基于SSM的仓库管理系统的设计与实现

末尾获取源码 开发语言&#xff1a;Java Java开发工具&#xff1a;JDK1.8 后端框架&#xff1a;SSM 前端&#xff1a;Vue 数据库&#xff1a;MySQL5.7和Navicat管理工具结合 服务器&#xff1a;Tomcat8.5 开发软件&#xff1a;IDEA / Eclipse 是否Maven项目&#xff1a;是 目录…

算法通关村第一关——链表经典问题之删除链表元素笔记

删除链表节点 总结一下高频常用的删除链表结点的情况&#xff0c;无论对链表进行何种操作&#xff0c;都需要精确查找和精确指向。另外&#xff0c;在删除链表节点时有一个很好用的技巧&#xff1a;虚头结点&#xff0c;将头结点的特殊性转化为一般&#xff0c;在后面具体阐述…

15个超级实用的Python操作,肯定有你意想不到的!

文章目录 1&#xff09;映射代理&#xff08;不可变字典&#xff09;2&#xff09;dict 对于类和对象是不同的3) any() 和 all()4) divmod()5) 使用格式化字符串轻松检查变量6) 我们可以将浮点数转换为比率7) 用globals()和locals()显示现有的全局/本地变量8) import() 函数9) …

Linux C语言 33-排序算法

Linux C语言 33-排序算法 本节关键字&#xff1a;C语言 排序算法 选择排序 冒泡排序 快速排序 相关C库函数&#xff1a; 冒泡排序 冒泡排序&#xff08;英语&#xff1a;Bubble Sort&#xff09;是一种简单的排序算法。它重复地走访过要排序的数列&#xff0c;一次比较两个元…

【LeetCode刷题】--90.子集II

90.子集II class Solution {public List<List<Integer>> subsetsWithDup(int[] nums) {List<List<Integer>> ans new ArrayList<>();List<Integer> list new ArrayList<>();//排序后便于去重Arrays.sort(nums);dfs(0,nums,ans,lis…

fastReID论文总结

fastReID论文总结 fastReIDReID所面临的挑战提出的背景概念&#xff1a;所谓ReID就是从视频中找出感兴趣的物体&#xff08;人脸、人体、车辆等&#xff09;应用场景&#xff1a;存在的问题&#xff1a;当前的很多ReID任务可复用性差&#xff0c;无法快速落地使用解决方式&…

常见的 QML 类型

在 QML&#xff08;Qt Modeling Language&#xff09;中&#xff0c;“type” 通常指的是定义特定界面组件或功能的元素。QML 类型可以是基本的用户界面元素&#xff0c;如按钮或文本框&#xff0c;也可以是更复杂的控件&#xff0c;如列表视图或滑块。QML 类型还可以是用于组织…

基于SpringBoot实现的教务查询系统

一、系统架构 前端&#xff1a;html | js | css | jquery | bootstrap 后端&#xff1a;springboot | springdata-jpa 环境&#xff1a;jdk1.7 | mysql | maven 二、代码及数据库 三、功能介绍 01. 登录页 02. 管理员端-课程管理 03. 管理员端-学生管理 04. 管理员端-教师管理…

EntityFrameworkCore数据库优先命令总结

初始化&#xff0c;直接使用数据库逆向生成模型 命令如下&#xff1a; Scaffold-DbContext connectionString providerName options C#有偿Q群&#xff1a;927860652这里的每个参数都有自己的用途&#xff1a; connectionString&#xff1a;数据库连接字符串。这应包括服…

C++ day45 爬楼梯 零钱兑换 完全平方数

题目1&#xff1a;70 爬楼梯&#xff08;进阶版&#xff09; 题目链接&#xff1a;爬楼梯 对题目的理解 需要爬n阶才能到达楼顶&#xff0c;每次可以至多爬m个台阶&#xff0c;m的区间是[1,n)&#xff0c;有多少种方法爬到楼顶 本题是一个完全背包问题&#xff0c;每一阶都…

史上最全接单平台集锦,程序员不容错过!

非典型程序员不是每天都累成狗&#xff0c;天天”996"甚至”007“。可能&#xff0c;面临着上班摸鱼没事干&#xff0c;下班躺尸打游戏的无聊境况。那么&#xff0c;如果你也是这样的程序员&#xff0c;有没有什么安排可以打发时间&#xff1f; 闲着还不如挣钱~心情好的时…

uniapp 使用 flex布局 将 图片展示 循环排列两列

将以下代码改成图片展示 循环排列两列 展示 <template><view><image v-for"(image, index) in imageList" :key"index" :src"image"></image></view> </template><script> export default {data() {…

【QML】qml+gstreamer显示的同时录像,避免卡顿

1. 问题 使用QML的CameravideoRecorder(Camera)VideoOutput实现显示加录像功能。在Ubuntu上运行正常&#xff0c;视频流畅。但是在开发板上&#xff08;RK3568&#xff09;上出现明显卡顿&#xff0c;无法正常录像。 2. 解决方案 将摄像头数据通过gstreamer共享内存到某个位…

cddd 安装指南(pip install cddd)

pip install cddd 这个命令可能会报错&#xff0c;因为要求是TensorFlow1.10.0 TensorFlow1.10.0对应的Python版本是3.6&#xff0c;所以如果你的Python版本是3.6以上是不行的.....

PTApt——2023年软件设计综合实践_7(数据结构)

6-1 递增的整数序列链表的插入 本题要求实现一个函数&#xff0c;在递增的整数序列链表&#xff08;带头结点&#xff09;中插入一个新整数&#xff0c;并保持该序列的有序性。 答案&#xff1a; 语言选C(gcc) List Insert(List L, ElementType X) {List tmp (List) mal…

142. 环形链表 II --力扣 --JAVA

题目 给定一个链表的头节点 head &#xff0c;返回链表开始入环的第一个节点。 如果链表无环&#xff0c;则返回 null。 如果链表中有某个节点&#xff0c;可以通过连续跟踪 next 指针再次到达&#xff0c;则链表中存在环。 为了表示给定链表中的环&#xff0c;评测系统内部使…