精准导航:用A*算法优化栅格地图的路径规划【附Matlab代码】

news2024/11/7 22:41:16

目录

    • 1.算法原理
    • 2.代码讲解
    • 3.结果展示
    • 4.代码获取


1.算法原理

A* 算法是一种基于传统图搜索的智能启发式算法,它具有稳定性高、节点搜索效率高等优点。主要原理为:以起点作为初始节点,搜索初始节点旁 8 个邻域,并通过启发函数评估后选择代价最小的节点,然后搜索这个节点的 8 个邻域,选择下一个代价最小的节点,重复上述步骤,直到选择的节点与目标点重合,将这些代价最小的节点连接起来就得到一条最优路径。

A*算法代价函数:
f ( n ) = g ( n ) + h ( n ) (1) f\begin{pmatrix}n\end{pmatrix}=g\begin{pmatrix}n\end{pmatrix}+h\begin{pmatrix}n\end{pmatrix}\tag{1} f(n)=g(n)+h(n)(1)
其中, f(n)为n节点的总代价值, g(n)代表从n节点到初始节点的最短路径代价值, h(n)代表从n节点到目标节点代价的估计值。
在这里插入图片描述

2.代码讲解

地图参数初始化

start_node = [1, 1]; %起点
target_node = [20, 20];%坐标
% 栅格地图的行数、列数定义
m = 20;
n = 20; 

% 加载地图
MAP = [0 1 1 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0; 
   0 1 1 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0; 
   0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 1 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 1 1 0 0; 
   0 1 1 0 1 0 0 0  0 0 0 0 0 0 1 1 1 0 0 0; 
   0 1 1 0 1 0 0 1 1 0 0 0 0 0 1 1 0 0 0 0;
   0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0; 
   0 1 1 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 1 1 0 0; 
   0 1 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0; 
   0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 0 0 0 0; 
   0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 1 1 1 1 0; 
   1 1 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 1 1 0; 
   1 1 0 0 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0 0; 
   0 0 0 1 0 0 1 1 0 1 1 1 0 0 0 0 0 1 1 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0; 
   0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;];

地图中1代表障碍物,0代表可行区域,绿色代表起点,红色代表终点:
在这里插入图片描述

获得父结点的可行子结点列表

function child_nodes = child_nodes_cal(parent_node, m, n, obs, closeList)

child_nodes = [];
field = [1,1; n,1; n,m; 1,m]; %边界

% 左上结点
child_node = [parent_node(1)-1, parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2)) %判断是否在地图内
    if ~ismember(child_node, obs, 'rows') %如果子结点不是障碍物
        child_nodes = [child_nodes; child_node];
    end
end

% 上结点
child_node = [parent_node(1), parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 右上结点
child_node = [parent_node(1)+1, parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 左结点
child_node = [parent_node(1)-1, parent_node(2)];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 右结点
child_node = [parent_node(1)+1, parent_node(2)];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 左下结点
child_node = [parent_node(1)-1, parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 下结点
child_node = [parent_node(1), parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

% 右下结点
child_node = [parent_node(1)+1, parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
    if ~ismember(child_node, obs, 'rows')
        child_nodes = [child_nodes; child_node];
    end
end

%% 排除已经存在于closeList的节点
delete_idx = [];
for i = 1:size(child_nodes, 1)
    if ismember(child_nodes(i,:), closeList , 'rows')
        delete_idx(end+1,:) = i;
    end
end
child_nodes(delete_idx, :) = [];

采用8领域搜索方式,红色箭头为可行走8个方向:

在这里插入图片描述

函数child_nodes_cal.m得到父结点的可行子结点列表,可行子结点需要判断是否在地图内,通过代码进行判断:

inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))

另一方面,可行子结点不能为障碍物:

~ismember(child_node, obs, 'rows')

起点预处理

% 初始化closeList
closeList = start_node;
closeList_path = {start_node,start_node};
closeList_cost = 0;
child_nodes = child_nodes_cal(start_node, m,n,obs,closeList);  

% 初始化openList
openList = child_nodes;
for i = 1:size(openList,1)
    openList_path{i,1} = openList(i,:);
    openList_path{i,2} = [start_node;openList(i,:)];
end

for i = 1:size(openList, 1)
    g = norm(start_node - openList(i,1:2)); %欧氏距离
    h = abs(target_node(1) - openList(i,1)) + abs(target_node(2) - openList(i,2)); %曼哈顿距离
    f=g+h;
    openList_cost(i,:) = [g, h, f];
end

这里g(n)采用欧式距离(表示结点n到起点实际距离),h(n)采用曼哈顿距离(表示结点n到终点估计值)

计算迭代

% 从openList开始搜索移动代价最小的节点
[~, min_idx] = min(openList_cost(:,3));
parent_node = openList(min_idx,:);


%% 进入循环
flag = 1;
while flag   
    
    % 找出父节点的忽略closeList的子节点
    child_nodes = child_nodes_cal(parent_node,  m, n, obs, closeList); 
    
    % 判断这些子节点是否在openList中,若在,则比较更新;没在则追加到openList中
    for i = 1:size(child_nodes,1)
        child_node = child_nodes(i,:);
        [in_flag,openList_idx] = ismember(child_node, openList, 'rows');   %in_flag=1则该child_node节点在openList中,openList_idx保存openList中第几位与child_node节点相同
        g = openList_cost(min_idx, 1) + norm(parent_node - child_node);
        h = abs(child_node(1) - target_node(1)) + abs(child_node(2) - target_node(2));
        f=g+h;
        
        if in_flag   % 若在,更新路径和成本     
            if g < openList_cost(openList_idx,1)
                openList_cost(openList_idx, 1) = g;
                openList_cost(openList_idx, 3) = f;
                openList_path{openList_idx,2} = [openList_path{min_idx,2}; child_node];
            end
        else         % 若不在,加入openList
            openList(end+1,:) = child_node;
            openList_cost(end+1, :) = [g, h, f];
            openList_path{end+1, 1} = child_node;
            openList_path{end, 2} = [openList_path{min_idx,2}; child_node];
        end
    end
   
    
    % 从openList移除移动代价最小的节点到 closeList
    closeList(end+1,: ) =  openList(min_idx,:);
    closeList_cost(end+1,1) =   openList_cost(min_idx,3);
    closeList_path(end+1,:) = openList_path(min_idx,:);
    openList(min_idx,:) = [];
    openList_cost(min_idx,:) = [];
    openList_path(min_idx,:) = [];
 
    % 重新搜索:从openList搜索移动代价最小的节点
    [~, min_idx] = min(openList_cost(:,3));
    parent_node = openList(min_idx,:);
    
    % 判断是否搜索到终点
    if parent_node == target_node
        closeList(end+1,: ) =  openList(min_idx,:);
        closeList_cost(end+1,1) =   openList_cost(min_idx,1);
        closeList_path(end+1,:) = openList_path(min_idx,:);
        flag = 0;
    end
end

3.结果展示

在这里插入图片描述

4.代码获取

完整代码公众号回复:A*,免费获取

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

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

相关文章

Kubernetes 二进制安装

目录 一、环境介绍 1.1、本节实验环境 1.2、实验拓扑 1.3、实验要求 1.4、实现思路 二、系统环境准备 2.1、主机配置 2.2、安装 Docker 2.3、设置防火墙 2.4、禁用 SELinux 三、生成通信加密证书 3.1、生成 CA 证书 3.2、生成 server 证书 3.3、生成 …

最大的游戏交流社区Steam服务器意外宕机 玩家服务受影响

易采游戏网6月3日消息&#xff1a;众多Steam游戏玩家报告称&#xff0c;他们无法访问Steam平台上的个人资料、好友列表和社区市场等服务。同时&#xff0c;社区的讨论功能也无法正常使用。经过第三方网站SteamDB的确认&#xff0c;&#xff0c;这一现象是由于Steam社区服务器突…

【MySQL03】【 Buffer Pool】

文章目录 一、前言二、缓冲池&#xff08;Buffer Pool &#xff09;1. 缓冲池的概念2. LRU List、Free List 和 Flush List2.1 Free 链表2.1.1 缓冲页的哈希处理 2.2 Flush 链表2.3 LRU 链表2.3.1 简单 LRU 链表2.3.2 优化后的 LRU 列表2.3.3 更进一步的优化 3. 脏页的刷新4. 多…

光猫、路由器的路由模式、桥接模式、拨号上网

下面提到的路由器都是家用路由器 一、联网条件 1.每台电脑、路由器、光猫想要上网&#xff0c;都必须有ip地址。 2.电脑获取ip 可以设置静态ip 或 向DHCP服务器(集成在路由器上) 请求ip 电话线上网时期&#xff0c;猫只负责模拟信号和数字信号的转换&#xff0c;电脑需要使…

从零开始:腾讯云轻量应用服务器上部署MaxKB项目(基于LLM大语言模型的知识库问答系统)

使用腾讯云轻量应用服务器部署和使用MaxKB项目 前言 一&#xff0c; MaxKB介绍 MaxKB是基于LLM大语言模型的知识库问答系统&#xff0c;旨在成为企业的最强大脑。它支持开箱即用&#xff0c;无缝嵌入到第三方业务系统&#xff0c;并提供多模型支持&#xff0c;包括主流大模型…

c# 输出二进制字符串

参考链接 C#二进制输出数据_c# 输出二进制 123.5的方法-CSDN博客https://blog.csdn.net/a497785609/article/details/4572112标准数字格式字符串 - .NET | Microsoft Learnhttps://learn.microsoft.com/zh-cn/dotnet/standard/base-types/standard-numeric-format-strings#BFo…

医学领域科技查新点提炼方法!---附案例分析

医学领域的查新项目研究范围较广&#xff0c;涉及基础医学、临床医学、中医学、预防医学、卫生学、特种医学等众多与人类健康和疾病有关的科学。查新目的主要包括立项、成果鉴定和报奖&#xff0c;有的期刊投稿也要求作者提供查新报告。 医学领域查新项目的两极化较明显&#…

dynamic多数据源的简单使用

背景 这几天搞了个saas项目&#xff0c;里面用到了多数据和execl模板导出功能&#xff0c; 其实我是经常用到的&#xff0c;但没在博客中写过&#xff0c;最近有点时间&#xff0c;正好稍微写一下。 方便大家使用 这次我先写多数据&#xff0c;execl模板导出下次有空在写。 使…

找好看的简历模板,就上这6个网站。

找好看的简历模板就上这6个网站&#xff0c;免费下载&#xff01; 1、菜鸟图库 个人简历模板|WORD文档模板免费下载 - 菜鸟图库 站内有超多办公类素材&#xff0c;PPT、world、excel模板都能找到&#xff0c;简历模板有非常详细的分类&#xff0c;风格类型也很多&#xff0c;想…

Kafka 如何基于 KRaft 实现集群最终一致性协调

01 架构概览 Zookeeper 提供了配置服务、分布式同步、命名服务、Leader 选举和集群管理等功能&#xff0c;在大数据时代的开始很多开源产品都依赖 Zookeeper 来构建&#xff0c;Apache Kafka 也不例外。但是随着 Kafka 功能的演进和应用的场景越来越多&#xff1a; 基于 Zoo…

linux命令别名与shell函数

# 修改网卡配置 alias vinetwork"vi /etc/sysconfig/network-scripts/ifcfg-ens33" 1. 方法和调用在同一个文件 # 定义shell函数,返回值通过$?获取 function say_hello(){ echo "hello shell" return 1 } # 使用shell函数 say_hello # 执行脚本后接收返…

怎么解决Hbuilderx的侧边栏不显示文件目录问题

第一步&#xff1a;找到视图 第二步&#xff1a;再视图中找到(显示项目管理器等左边视图)点击就可以了&#xff01;

时间卷积网络(TCN):概述及与CNN和RNN的比较

TCN 时间卷积网络&#xff08;TCN&#xff09;&#xff1a;概述及与CNN和RNN的比较1. 时间卷积网络&#xff08;TCN&#xff09;定义与特点应用场景 2. 卷积神经网络&#xff08;CNN&#xff09;定义与特点应用场景 3. 循环神经网络&#xff08;RNN&#xff09;定义与特点应用场…

【Mongodb】Mongodb亿级数据性能测试和压测

一&#xff0c;mongodb数据性能测试 如需转载&#xff0c;请标明出处&#xff1a;https://zhenghuisheng.blog.csdn.net/article/details/139505973 mongodb数据性能测试 一&#xff0c;mongodb数据性能测试1&#xff0c;mongodb数据库创建和索引设置2&#xff0c;线程池批量…

FatFs文件系统移植到MCU平台详细笔记经验教程

0、准备工作 在移植FatFs文件系统前&#xff0c;需要准备好一块开发板&#xff0c;和一张SD卡&#xff0c;且需要已经实现开发板正常的读写SD卡或其它硬件设备。 本文笔记教程中使用的硬件设备是STM32F407VET6开发板&#xff08;板载SD插槽&#xff09;&#xff0c;配备8G和32G…

【Vue3】vue3快速实现响应式数据恢复初始值。浅拷贝与深拷贝的应用。

有一个经常遇到的场景就是&#xff0c;一个表单最后一列有个编辑按钮&#xff0c;点击编辑按钮之后打开表单弹窗&#xff0c;修改其中的数据&#xff0c;但是如果此弹窗再作为新增弹窗打开的时候&#xff0c;弹窗数据会缓存上次编辑的数据。 在 Vue 3 中&#xff0c;由于引入了…

C语言二级指针、指针数组

一、二级指针 指针变量也是变量&#xff0c;是变量就应有地址&#xff0c;那指针变量的地址存放在哪里&#xff1f;存放在二级指针变量。 此时&#xff0c;*ppa pa&#xff0c;**ppa a。 二、指针数组 指针数组&#xff0c;顾名思义就是存放指针的数组。 数组每个元素为int类…

java自学阶段二:JavaWeb开发45(git学习)

目录&#xff1a; 学习目标git的使用&#xff08;工作流程、常用命令、idea集成&#xff09; 一、学习目标&#xff1a; 了解Git基本概念能够了解git的工作流程能够使用Git常用命令熟悉Git代码托管服务能够使用idea操作git 二、git的使用 1&#xff09;git的概念&#xff1…

学校教学选择SOLIDWORKS教育版的理由

在现代工程和技术教育领域中&#xff0c;计算机辅助设计软件&#xff08;CAD&#xff09;已成为不可或缺的教学工具。SOLIDWORKS作为一款功能强大、易于上手的CAD软件&#xff0c;其教育版在学校教学中备受青睐。本文将从多个方面探讨学校教学选择SOLIDWORKS教育版的理由。 一…

堪称2024最强的前端面试场景题,让419人成功拿到offer

前言 2024年的秋季招聘还有两个月就即将到来&#xff0c;很多同学开始思考前端面试中场景题的重要性。这里我提供一些见解和建议来帮助大家准备即将到来的面试。 首先&#xff0c;理解面试中场景题的必要性是至关重要的。与算法或理论问题不同&#xff0c;场景题更贴近实际工…