无人驾驶(移动机器人)路径规划之A star(Tie Breaker)算法及其matlab实现

news2024/9/23 22:53:31

     在自动驾驶与移动机器人路径规划时,必定会用到经典的算法A star。下面是我未加入与加入Tie Breaker 的matlab实现效果。可以发现加入Tie Breaker之后效果明显改善。

目录

一、效果比较

1.未加入Tie Breaker(黑色为障碍物,菱形绿色为目标点与起始点,红色为close,绿色为open,黄色为最终路径)

2.加入Tie Breaker(黑色为障碍物,菱形绿色为目标点与起始点,红色为close,绿色为open,黄色为最终路径)

二、A star算法

1.算法背景与原理

2.算法流程

3.算法应用与优化

4.算法特点与局限性

5.总结与展望

6.A star的Tie Breaker

三、核心代码

1.Main代码

2.A star算法

3.地图创建


一、效果比较

1.未加入Tie Breaker(黑色为障碍物,菱形绿色为目标点与起始点,红色为close,绿色为open,黄色为最终路径)

代码链接:

移动机器人自主路径规划之Astar算法MATLAB实现代码资源-CSDN文库

(1)原始地图信息。

(2)规划地图信息

(3)路径信息

2.加入Tie Breaker(黑色为障碍物,菱形绿色为目标点与起始点,红色为close,绿色为open,黄色为最终路径)

𝑑𝑥1 = 𝑎𝑏𝑠 𝑛𝑜𝑑𝑒. 𝑥 − 𝑔𝑜𝑎𝑙. 𝑥
𝑑𝑦1 = 𝑎𝑏𝑠(𝑛𝑜𝑑𝑒. 𝑦 − 𝑔𝑜𝑎𝑙. 𝑦)
𝑑𝑥2 = 𝑎𝑏𝑠 𝑠𝑡𝑎𝑟𝑡. 𝑥 − 𝑔𝑜𝑎𝑙. 𝑥
𝑑𝑦2 = 𝑎𝑏𝑠 𝑠𝑡𝑎𝑟𝑡. 𝑦 − 𝑔𝑜𝑎𝑙. 𝑦
𝑐𝑟𝑜𝑠𝑠 = 𝑎𝑏𝑠(𝑑𝑥1 × 𝑑𝑦2 − 𝑑𝑥2 × 𝑑𝑦1)
h = ℎ + 𝑐𝑟𝑜𝑠𝑠 × 0.001

代码链接:

无人驾驶(移动机器人)路径规划之Astar(TieBreaker)算法及其matlab实现资源-CSDN文库

(1)原始地图信息。

(2)规划地图信息

(3)路径信息

二、A star算法

       Astar算法是一种广泛使用的路径规划算法,它通过启发式搜索的方式,在图形或网络中寻找两个节点之间的最短路径。A算法结合了广度优先搜索和最佳优先搜索的特点,通过评估每个可能的路径,以找到从起点到目标节点的最佳路径。以下是对A*算法的详细介绍。

1.算法背景与原理

        A*算法最早于1964年在IEEE Transactions on Systems Science and Cybernetics中的论文《A Formal Basis for the Heuristic Determination of Minimum Cost Paths》中首次提出。它属于经典的启发式搜索方法,其核心思想在于当前搜索结点往下选择下一步结点时,可以通过一个启发函数来进行选择,选择代价最少的结点作为下一步搜索结点而跳转其上。

        在Astar算法中,每个节点都有两个关键值:G值和H值。G值表示从起点到当前节点的实际代价,即已经走过的路径长度;H值表示从当前节点到目标节点的估计代价,即预计还需要走多远才能达到目标。Astar算法在搜索过程中,始终选择F值最小的节点进行扩展,其中F=G+H。这种策略使得A*算法能够尽可能地沿着最短路径进行搜索,从而提高搜索效率。

2.算法流程

A*算法的流程大致如下:

  1. 初始化:创建一个开放列表和一个关闭列表,用于存储待探索和已探索的节点。将起点加入开放列表。
  2. 选择节点:从开放列表中选择F值最小的节点作为当前节点。如果开放列表为空,则算法结束,表示没有找到路径。
  3. 扩展节点:将当前节点从开放列表移到关闭列表,并检查其所有邻居节点。对于每个邻居节点,如果它已经在关闭列表中,则忽略它;如果它不在开放列表和关闭列表中,则计算其G值、H值和F值,并将其添加到开放列表;如果它已经在开放列表中,但新计算的G值更小,则更新其G值和F值。
  4. 重复搜索:重复步骤2和3,直到目标节点被加入关闭列表或开放列表为空。如果目标节点被加入关闭列表,则算法找到了一条从起点到目标节点的路径;如果开放列表为空,则算法结束,表示没有找到路径。

3.算法应用与优化

        Astar算法在游戏开发、机器人学和其他相关领域有着广泛的应用。在游戏中,Astar算法被用来实现人物的寻路功能,使得角色能够智能地找到从起点到终点的最短路径。在机器人学中,A*算法被用于机器人的路径规划,帮助机器人避开障碍物并高效地到达目的地。

      为了提高A*算法的性能和效率,研究者们进行了大量的优化工作。例如,通过改进数据结构(如使用优先队列来存储开放列表中的节点),可以减少算法的时间复杂度;通过引入预处理技术(如构建网格地图或生成路标图),可以进一步提高算法的搜索速度;通过引入动态障碍物处理和实时地图更新机制,可以使算法更好地适应复杂和动态的环境。

4.算法特点与局限性

        Astar算法具有方向性、智能性等特点,能够结合搜索任务中的环境情况,缩小搜索范围,提高搜索效率。然而,Astar算法也存在一些局限性。首先,它依赖于启发式函数的选择,如果启发式函数设计不当,可能导致算法性能下降或无法找到最优解。其次,Astar算法在复杂的环境或图形中可能不是最优的,因为它需要对每个可能的路径进行评估和比较。此外,Astar算法的空间复杂度较高,需要存储大量的节点信息,这可能导致内存消耗较大。

5.总结与展望

        Astar算法作为一种高效的路径规划算法,在多个领域得到了广泛应用。通过不断优化和改进算法的实现方式,可以进一步提高Astar算法的性能和效率,使其更好地适应复杂和动态的环境。未来,随着人工智能和机器人技术的不断发展,A*算法有望在更多领域发挥重要作用,为智能系统的路径规划和决策提供支持。

     请注意,以上是对A star算法的简要介绍,实际应用中可能还需要考虑更多的细节和特殊情况。此外,由于篇幅限制,这里无法对Astar算法的每个方面都进行深入的探讨。如果需要更详细或专业的介绍,建议查阅相关学术论文或技术文档。

6.A star的Tie Breaker

        A star算法中的Tie Breaker(平局决胜者)是一个解决在搜索过程中遇到多个具有相同F值的节点时,如何选择下一个扩展节点的问题的机制。在A star算法中,当开放列表中存在多个具有相同最小F值的节点时,如果没有明确的选择标准,算法可能会陷入非确定性的行为,导致每次运行的结果不一致或者搜索性能下降。

     Tie Breaker的作用就是提供一个确定性的选择标准,确保在面临多个相同F值的节点时,算法能够一致地选择下一个扩展节点。这样可以提高算法的稳定性和可预测性。

     Tie Breaker的具体实现方式可以有多种。一种常见的做法是按照节点的其他属性进行排序,比如按照节点的G值或者H值进行次级排序。如果G值和H值也相同,还可以考虑使用节点的位置、编号或者其他自定义的属性进行排序。这样,当遇到多个具有相同F值的节点时,算法会根据Tie Breaker的规则选择一个确定的节点进行扩展。

        另外,有些实现中还可能采用随机选择的方式来处理平局情况,但这通常不是首选方法,因为它可能导致算法的不稳定性和不可预测性。

      总之,Tie Breaker是A算法中一个重要的机制,用于解决在选择扩展节点时遇到的平局情况,确保算法的一致性和稳定性。通过合理地设计Tie Breaker的规则,可以提高A算法的性能和可靠性。

三、核心代码

1.Main代码

function Main()
%主程序
clc
clear all
close all;
disp('A Star Path Planing start!!');
[map,node,obstacle]=createmap();

map_start = node(1:2);
map_goal = node(3:4);
xmax = size(map,1);
ymax = size(map,2);
figure;
pause(3);
axis([1 xmax+1 1 ymax+1])
set(gca,'YTick',0:1:xmax);
set(gca,'XTick',0:1:ymax);
grid on;
hold on;
% 绘制边界和障碍点
plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'o',  'MarkerFaceColor', 'k', 'MarkerEdgeColor', 'k');
hold on;
% 绘制起始点
plot(map_start(1)+.5,map_start(2)+.5,'d','MarkerFaceColor','g');
hold on;
text(map_start(1)+.5,map_start(2)+.5,'Start');
hold on;
% 绘制终止点
plot(map_goal(1)+.5,map_goal(2)+.5,'d','MarkerFaceColor','g');
hold on;
text(map_goal(1)+1,map_goal(2)+.5,'Target');hold on;
path = FAstar(obstacle,map,map_start,map_goal);% A*算法

%画出路径
figure;
axis([1 xmax+1 1 ymax+1])
set(gca,'YTick',0:1:xmax);
set(gca,'XTick',0:1:ymax);
grid on;
hold on;
% 绘制边界和障碍点
plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'o',  'MarkerFaceColor', 'k', 'MarkerEdgeColor', 'k');
hold on;
% 绘制起始点
plot(map_start(1)+.5,map_start(2)+.5,'d','MarkerFaceColor','g');
hold on;
text(map_start(1)+.5,map_start(2)+.5,'Start');
% 绘制终止点
plot(map_goal(1)+.5,map_goal(2)+.5,'d','MarkerFaceColor','g');
hold on;
text(map_goal(1)+1,map_goal(2)+.5,'Target');
if length(path)>=1
    plot(path(:,1)+0.5,path(:,2)+0.5,'-y','LineWidth',3);
    hold on;
end
%}

grid on;
hold on;
pause(5);
%close(figure(1));

end

2.A star算法

function path  = FAstar(obstacle,map,map_start,map_goal)
%Tie Breaker
dx0 = abs(map_goal(1)-map_start(1));
dy0 = abs(map_goal(2)-map_start(2));
% A*程序算法
path = [] ;
%openlist
open = [];
%closelist
close = [];

%findflag用于判断while循环是否结束
findflag = false;
%1.起始点放在openlist中
open = [map_start(1),map_start(2),0+h(map_start,map_goal),0,map_start(1),map_start(2)];%节点坐标、代价值F,G,父节点
%更新八节点
next = model();
while ~findflag
    %首先判断是否到达目标点
    if isempty(open(:,1))
        disp('No path to goal!');
        return;
    end
    %判断目标点是否在open中
    [openflag,id] = Isopen(map_goal,open);
    if openflag
        disp('Find goal!');
        close = [open(id,:);close];%close的第一行
        findflag = true;
        break;
    end
    %判断openlist中F排序
    %寻找F最小点
    [Y,I] = sort(open(:,3));
    open =open(I,:);
    %F值排序后的open
    %将F最小的节点(open中第一行节点)放到close
    close = [open(1,:);close];
    current = open(1,:);
    open(1,:) = [];%open第一行置为空
    %对当前节点周围4个相邻节点进行判断
    for in = 1:length(next(:,1))
        %获得相邻节点的坐标,F置为0,G置为0
        %父坐标暂定为0
        m = [current(1,1)+next(in,1) , current(1,2)+next(in,2),0,0,0,0];
        m(4) = current(1,4) + next(in,3);%相邻节点G
        m(3) = m(4) + h1(m(1:2),map_goal,dx0,dy0);%相邻节点F
        %判断当前节点是否为阻碍点
        if Isobstacle(m,obstacle)
            continue;
        end

        %{
          如果相邻节点,在closelist中,则flag=1 targetInd=其close的行数
          如果相邻节点,不在openlist中,则flag=2 targetInd=[]
          如果相邻节点,在openlist中,则flag=3 targetInd=其open的行数
        %}
        [flag,targetInd] = Findlist(m,open,close);
        %如果它在Closelist中,忽略此相邻节点
        if flag==1
            continue;
        %如果它不在Openlist中,加入Openlist,并把当前节点设置为它的父节点
        elseif flag==2
            m(5:6) = [current(1,1),current(1,2)];
            open = [open;m];
        %剩下的情况就是它在Openlist中,检查由当前节点到相邻节点是否更好
        % 如果更好则将当前节点设置为其父节点,并更新F,G值;否则不操作
        else
            %由当前节点到达相邻节点更好(targetInd是此相邻节点在open中的行号 此行的第3列是代价函数F值)
            if m(3) < open(targetInd,3)
                m(5:6) = [current(1,1),current(1,2)];
                open(targetInd,:) = m;      %更好,则将此相邻节点的父节点设置为当前节点,否则不作处理
            end
        end
       
    end
    plotmap(map,open,close);
end
%追溯路径
path = getpath(close,map_start);

end

3.地图创建

function [map,node,obstacle] = createmap()
%创建地图
clear all;
figure;%创建新窗口

%地图参数初始化
max_x = 100;%长
max_y = 100;%宽
p_obstacle = 0.3;%阻碍率

%设置阻碍点
obstacle0 = ones( max_x ,max_y ) * p_obstacle;%创建矩阵
%MAP中阻碍点设为-1,非阻碍点设为9998
map = 9999*((rand(max_x,max_y))>obstacle0) - 1;%-1代表阻碍物
YMAX = size(map,2);%y轴最大
XMAX = size(map,1);%x轴最大
obstacle = [];
for i1 = 0 : (YMAX+1)
    obstacle = [obstacle;[0 i1]];
end
for i2 = 0 : (XMAX+1)
    obstacle = [obstacle;[i2 0]];
end
for i3 = 0 : (YMAX+1)
    obstacle = [obstacle;[XMAX+1 i3]];
end
for i4 = 0 : (XMAX+1)
    obstacle = [obstacle;[i4 YMAX+1]];
end
%障碍点坐标
for i = 1 : XMAX
    for j = 1 : YMAX
        if map(i,j) == -1
            obstacle = [obstacle;[i j]];
        end
    end
end
axis([1 max_x + 1 1 max_y + 1])%x,y轴1-50图像
set(gca,'XTick',0:1:max_x);%x轴的间隔为1
set(gca,'YTick',0:1:max_y);%y轴的间隔为1
grid on;%设置网格线
hold on;%保持图形
%绘制地图障碍物
for i = 1 : max_x
    for j = 1 : max_y
        if map(i,j) == -1
            plot(i+0.5, j+0.5, 'o',  'MarkerFaceColor', 'k', 'MarkerEdgeColor', 'k');  %中心位置绘制点
        end
    end
end
pause(1);%延时一秒
%初始点
h=msgbox('初始位置标记!');%弹出初始框提示标记初始位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框
    delete(h);
end
xlabel('请设置初始点X轴! ','Color','black');%设置x轴
but = 0;
while(but ~= 1)%收到左键点击
    [xval,yval,but] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
xstart = xval;%初始位置
ystart = yval;
map(xval,yval) = 0;
plot(xval + 0.5,yval + 0.5,'d','MarkerFaceColor','g');%绘制初始点
text(xval + 1,yval + 0.5,'Start');
pause(1);%延时一秒
%目标点
h=msgbox('目标位置标记!');%弹出目标提示标记目标位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框
    delete(h);
end
xlabel('请设置目标点X轴! ','Color','black');%设置x轴
but1 = 0;
while(but1 ~= 1)%收到左键点击
    [xval,yval,but1] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
xTarget = xval;%目标位置
yTarget = yval;
map(xval,yval) = 9998;
plot(xval + 0.5,yval + 0.5,'d','MarkerFaceColor','g');%绘制目标点
text(xval + 1,yval + 0.5,'Target');
node = [xstart,ystart,xTarget,yTarget];

h=msgbox('请确认地图信息!');%确认地图信息
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框
    delete(h);
end
pause(5);

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

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

相关文章

婴儿洗衣机有必要买吗?四大满分婴儿洗衣机入围榜单

宝宝衣服的清洗对父母来说都很重要&#xff0c;所以挑选一款适合宝宝的波轮洗衣机显得尤为重要。也许有许多人认为&#xff0c;为婴儿购买独立的洗衣机是不必要的&#xff0c;但是你是否了解呢&#xff1f;新生婴儿的肌肤要比成人更脆弱&#xff0c;更易受到感染而受到伤害&…

vue3.0 + ts + eslint报错:error Parsing error: ‘>‘ expected

eslint报错 这里加上对应的 eslint配置即可&#xff1a; parser: vue-eslint-parser, parserOptions: {parser: "typescript-eslint/parser",ecmaVersion: 2020,sourceType: module, }具体如下&#xff1a; module.exports {parser: vue-eslint-parser,parserOpti…

浏览器输入框自动填充默认样式移除

文章目录 浏览器输入框自动填充默认样式移除问题现象以及探索过程尝试代码有效关键代码&#xff08;解决方案&#xff09; 浏览器输入框自动填充默认样式移除 问题现象以及探索过程 &#xff08;在 uniapp 语法下&#xff09;本文的写法在 Edge 119.0.2151.58 (正式版本) (64 …

第九届蓝桥杯大赛个人赛省赛(软件类)真题C 语言 A 组-乘积尾零

solution 找末尾0的个数&#xff0c;即找有多少对2和5 >问题等价于寻找所给数据中&#xff0c;有多少个2和5的因子&#xff0c;较少出现的因子次数即为0的个数 #include <iostream> using namespace std; int main() {// 请在此输入您的代码printf("31");…

docker配置镜像加速后容器和镜像消失

一、问题描述 根据阿里云给docker配置镜像加速器 sudo mkdir -p /etc/docker sudo tee /etc/docker/daemon.json <<-EOF {"registry-mirrors": ["https://gt6j98xi.mirror.aliyuncs.com"] } EOF sudo systemctl daemon-reload sudo systemctl rest…

大模型落地实战指南:从选择到训练,深度解析显卡选型、模型训练技、模型选择巧及AI未来展望---打造AI应用新篇章

大模型落地实战指南&#xff1a;从选择到训练&#xff0c;深度解析显卡选型、模型训练技、模型选择巧及AI未来展望—打造AI应用新篇章 0.前言大模型发展史 早期阶段&#xff08;1950s~1980s&#xff09; 在1950年代初期&#xff0c;人们开始尝试使用计算机处理自然语言文本。…

C语言:给结构体取别名的4种方法

0 前言 在进行嵌入式开发的过程中&#xff0c;我们经常会见到typedef这个关键字&#xff0c;这个关键字的作用是给现有的类型取别名&#xff0c;在实际使用过程中往往是将一个复杂的类型名取一个简单的名字&#xff0c;便于我们的使用。就像我们给很熟的人取外号一样&#xff…

对标开源3D建模软件blender,基于web提供元宇宙3D建模能力的dtns.network德塔世界是否更胜一筹?

对标开源3D建模软件blender&#xff0c;基于web提供元宇宙3D建模能力的dtns.network德塔世界是否更胜一筹&#xff1f; blender是一款优秀的3D建模开源软件&#xff0c;拥有免费开源、功能强大、渲染速度优秀的优点。而开源的dtns.network德塔世界&#xff0c;亦是专业级的元宇…

计算机网络——27路由选择算法

路由选择算法 路由协议 路由协议的目标&#xff1a;确定从发送主机到接收主机之间&#xff0c;通过路由器的网络“较好”的路径&#xff08;等价于路由器的序列&#xff09; 路径&#xff1a;路由器的序列&#xff0c;分组将会沿着该序列从源主机到达最后的目标主机“较好”…

vue 中实现下载后端返回的流式数据

验证是否是blob /*** Event 验证是否为blob格式* */export async function blobValidate(data) {try {const text await data.text();JSON.parse(text);return false;} catch (error) {return true;}}get请求 /*** Event: get请求下载后端返回的数据流* description: url[Stri…

【No.17】蓝桥杯图论上|最短路问题|Floyd算法|Dijkstra算法|蓝桥公园|蓝桥王国(C++)

图的基本概念 图&#xff1a; 由点(node&#xff0c;或者 vertex)和连接点的边(edge)组成。图是点和边构成的网。 树&#xff1a;特殊的图树&#xff0c;即连通无环图树的结点从根开始&#xff0c;层层扩展子树&#xff0c;是一种层次关系&#xff0c;这种层次关系&#xff0…

Multimodal Chain-of-Thought Reasoning in Language Models阅读笔记

论文&#xff08;2023年&#xff09;链接&#xff1a;https://arxiv.org/pdf/2302.00923.pdf GitHub项目链接&#xff1a;GitHub - amazon-science/mm-cot: Official implementation for "Multimodal Chain-of-Thought Reasoning in Language Models" (stay tuned a…

美国socks5动态IP代理如何提升网络效率?

在探讨美国socks5代理动态IP的奥秘之前&#xff0c;我们需要先深入理解其背后的基本概念和原理。Socks5代理是一种先进的网络协议&#xff0c;它像一位中转站&#xff0c;默默地帮用户转发网络请求。它让网络流量得以通过代理服务器传输&#xff0c;进而隐藏用户的真实IP地址。…

独立游戏《星尘异变》UE5 C++程序开发日志3——UEC++特供的数据类型

本篇日志将介绍FString&#xff0c;FText、FName的用法和相互转换&#xff0c;以及容器TMap&#xff0c;TArray的增删查改 一、字符串相关数据类型&#xff1a;FString、FText、FName FString是最接近std::string的类型&#xff0c;字符串本身可以看做一个存储char型的动态数…

Spire.PDF for .NET【文档操作】演示:查找并删除 PDF 中的空白页

PDF 中的空白页并不罕见&#xff0c;因为它们可能是作者故意留下的或在操作文档时意外添加的。当您阅读或打印文档时&#xff0c;这些空白页可能会很烦人&#xff0c;因此可能非常有必要将其删除。在本文中&#xff0c;您将了解如何使用Spire.PDF for .NET以编程方式查找和删除…

Docker创建mysql容器本地版

1.下载docker desktop https://www.docker.com/products/docker-desktop/ 2. 创建容器 docker run 创建并启动mysql容器 --name 容器名字 -d 后台运行容器 -p 映射端口 -e 参数 截至目前最新的mysql是mysql8.0.36 那就整这个试下。 docker run -d --name mysql-cont…

【笔记】深入理解JVM机制

&#x1f3a5; 个人主页&#xff1a;Dikz12&#x1f4d5;格言&#xff1a;吾愚多不敏&#xff0c;而愿加学欢迎大家&#x1f44d;点赞✍评论⭐收藏 目录 JVM 运⾏流程图 JVM 中内存区域划分 方法区 / 元数据区 堆 栈 程序计数器 本地方法栈 内存区域总结 JVM 中类加载过程 …

flask_restful渲染模版

渲染模版就是在 Flask_RESTful 的类视图中要返回 html 片段代码&#xff0c;或 者是整个html 文件代码。 如何需要浏览器渲染模板内容应该使用 api.representation 这个装饰器来定 义一个函数&#xff0c; 在这个函数中&#xff0c;应该对 html 代码进行一个封装&#xff…

【Java程序设计】【C00368】基于(JavaWeb)Springboot的箱包存储系统(有论文)

TOC 博主介绍&#xff1a;java高级开发&#xff0c;从事互联网行业六年&#xff0c;已经做了六年的毕业设计程序开发&#xff0c;开发过上千套毕业设计程序&#xff0c;博客中有上百套程序可供参考&#xff0c;欢迎共同交流学习。 项目简介 项目获取 &#x1f345;文末点击卡片…

编译u-boot(硬件: atk-dl6y2c)和NFS/EMMC模式启动Linux Kernel

目录 概述 1 编译u-boot 1.1 解压文件 1.2 编译u-boot 2 配置环境 2.1 在Ubunt 搭建TFTP 2.2 建立下载目录 3 烧写bootloader到SD 4 使用NFS模式启动板卡 5 从EMMC 启动 Linux 系统 5.1 通过配置参数方式 5.2 使用命令直接启动内核 文中使用的代码下载地址&#xf…