基于A*的二维多无人机航线规划

news2024/11/25 22:50:07

Matlab航迹规划仿真——A*算法_航迹起始算法 matlab_致守的博客-CSDN博客

matlab2016及以上可以运行 

astar.m 

function [] = astar(Spoint,Epoint,Matrix,m,n,h1,h2)
%%寻路
Matrix(Spoint(1),Spoint(2))=0;
Matrix(Epoint(1),Epoint(2))=inf;
G=Matrix;
F=Matrix;
openlist=Matrix;
closelist=Matrix;
parentx=Matrix;
parenty=Matrix;
openlist(Spoint(1),Spoint(2)) =0;
%closelist(Epoint(1),Epoint(2))=inf;
 
for i = 1:n+2
    for j = 1:m+2
        k = Matrix(i,j);
        if(k == -inf)
            %subplot(2,2,1);
            h3 = plot(i,j,'k.');
%         elseif(k == inf)  % show green feasible point
%             %subplot(2,2,1);
%             plot(i,j,'gh');
%         else
%             %subplot(2,2,1);
%             plot(i,j,'gh');
        end
        hold on
    end
end
axis([0 m+3 0 n+3]);
%subplot(2,2,1);
plot(Epoint(1),Epoint(2),'b+');
%subplot(2,2,1);
plot(Spoint(1),Spoint(2),'b+');
while(1)
    num=inf;
    for p=1:m+2
        for q=1:n+2
            if(openlist(p,q)==0&&closelist(p,q)~=1)
                Outpoint=[p,q];
                if(F(p,q)>=0&&num>F(p,q))
                    num=F(p,q);
                    Nextpoint=[p,q];
                end
            end
        end
    end
    closelist(Nextpoint(1),Nextpoint(2))=1;
    for i = 1:3
        for j = 1:3
            k = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
            if(i==2&&j==2|closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)==1)
                continue;
            elseif (k == -inf)
                G(Nextpoint(1)-2+i,Nextpoint(2)-2+j) = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
                closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=1;
            elseif (k == inf)
                distance=((i-2)^2+(j-2)^2)^0.5;
                G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1),Nextpoint(2))+distance;
                openlist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=0;
               % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;%欧几里德距离启发函数
                H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
                H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
                H=sqrt(2)*H_diagonal+(H_straight-2*H_diagonal);
 
 
                % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
                
                F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H;
                parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
                parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
            else distance=((i-2)^2+(j-2)^2)^0.5;
                if(k>(distance+G(Nextpoint(1),Nextpoint(2))))
                k=distance+G(Nextpoint(1),Nextpoint(2));
               % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;  %欧几里德距离启发函数
                H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
                H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
                H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal);
 
 
                 % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
                
                F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H;
                parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
                parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
                end
            end
            if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
                 parentx(Epoint(1),Epoint(2))=Nextpoint(1);
                parenty(Epoint(1),Epoint(2))=Nextpoint(2);
                break;
            end
        end
        if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
             parentx(Epoint(1),Epoint(2))=Nextpoint(1);
                parenty(Epoint(1),Epoint(2))=Nextpoint(2);
            break;
        end
    end
    if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
         parentx(Epoint(1),Epoint(2))=Nextpoint(1);
                parenty(Epoint(1),Epoint(2))=Nextpoint(2);
        break;
    end
end
    P=[];
    s=1;
while(1)
    if(num==inf)
        break;
    end
    %subplot(2,2,1);
    h4 = plot(Epoint(1),Epoint(2),'b+');
 
    P(s,:)=Epoint;
    s=s+1;
%      pause(1);
    xx=Epoint(1);
    Epoint(1)=parentx(Epoint(1),Epoint(2));
    Epoint(2)=parenty(xx,Epoint(2));
    if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2))
        %subplot(2,2,1);
        plot(Epoint(1),Epoint(2),'b+');
        P(s,:)=Epoint;
        break;
    end
end
P(s+1,:)=Spoint;
legend([h1,h2,h3,h4],'起始点','目标点','障碍物','航迹点');
 
count=0;
for i=2:12
    for j=2:12
        if(G(i,j)~=inf&&G(i,j)~=-inf)
            count=count+1;
        end
    end
end
count
end

main.m 

clc
clear all
m = 30;n = 30;

%%构建地图
for i = 1:m+2
    if i == 1
        for j = 1:n+2
            Matrix(i,j) = -inf;
        end
    elseif i == m+2
        for j = 1:n+2
            Matrix(i,j) = -inf;
        end
    else
        for j = 1:n+2
            if ((j == 1)|(j == n+2))
                Matrix(i,j) = -inf;
            else
                Matrix(i,j) = inf;
            end
        end
    end
end


%%障碍1
for j=15:18
    Matrix(5,j)=-inf;end
for j=15:18
     Matrix(8,j)=-inf;end     
for i=5:8 
     Matrix(i,18)=-inf;end
for i=5:8
    Matrix(i,15)=-inf;end

%%障碍2
for j=17:20
    Matrix(12,j)=-inf;end
for j=17:20
     Matrix(15,j)=-inf;end     
for i=12:15 
     Matrix(i,17)=-inf;end
for i=12:15
    Matrix(i,20)=-inf;end

%%障碍3
for j=15:18
    Matrix(20,j)=-inf;end
for j=15:18
     Matrix(23,j)=-inf;end     
for i=20:23 
     Matrix(i,18)=-inf;end
for i=20:23
    Matrix(i,15)=-inf;end

%%%%%%%%第一条航线
Spoint = [3 3];	  %起始点坐标
Epoint = [7 30];	%目标点坐标
% 显示地图
%subplot(2,2,1);
h1 = plot(Spoint(1),Spoint(2),'rO');
hold on
h2 = plot(Epoint(1),Epoint(2),'rO');
astar(Spoint,Epoint,Matrix,m,n,h1,h2);

%%%%%%%%第二条航线
Spoint2 = [8 4];	  %起始点坐标
Epoint2 = [15 28];	%目标点坐标
% 显示地图
h12 = plot(Spoint2(1),Spoint2(2),'gO');
hold on
h22 = plot(Epoint2(1),Epoint2(2),'gO');
astar(Spoint2,Epoint2,Matrix,m,n,h12,h22);


%%%%%%%%第三条航线
Spoint3 = [14 3];	  %起始点坐标
Epoint3 = [19 29];	%目标点坐标
% 显示地图
h13 = plot(Spoint3(1),Spoint3(2),'yO');
hold on
h23 = plot(Epoint3(1),Epoint3(2),'yO');
astar(Spoint3,Epoint3,Matrix,m,n,h13,h23);

%%%%%%%%第四条航线
Spoint4 = [17 2];	  %起始点坐标
Epoint4 = [25 30];	%目标点坐标
% 显示地图
h14 = plot(Spoint4(1),Spoint4(2),'bO');
hold on
h24 = plot(Epoint4(1),Epoint4(2),'bO');
astar(Spoint4,Epoint4,Matrix,m,n,h14,h24);



% %将得到的折现曲线拟合成光滑的曲线
% P=P';
% a=[];
% b=[];
% a=P(1,:);
% b=P(2,:);
% figure
% %subplot(2,2,3);
% plot(a,b);
% axis([0,n+3,0,n+3]);
%  
% values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);
% figure
% %subplot(2,2,4);
% plot(values(1,:),values(2,:),'r');
% axis([0,m+3,0,m+3]);

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

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

相关文章

安全防御 --- DDOS攻击（01）

安全防御 --- DDOS攻击（01）

DOS攻击（deny of service）--- 拒绝式服务攻击 例：2016年10月21日，美国提供动态DNS服务的DynDNS遭到DDOS攻击，攻击导致许多使用DynDNS服务的网站遭遇访问问题，此事件中，黑客人就是运用了DNS洪水…
阅读更多...
ceph--RBD的使用

ceph--RBD的使用

Ceph-RDB 1、RBD架构图 Ceph 可以同时提供对象存储 RADOSGW、块存储 RBD、文件系统存储 Ceph FS,RBD 即 RADOS Block Device 的简称，RBD 块存储是常用的存储类型之一，RBD 块设备类似磁盘 可以被挂载，RBD 块设备具有快照、多副本、克隆和一致…
阅读更多...
今天实习第一天，用git

今天实习第一天，用git

老板问了我是否用过gitee，并且是否用过git，在集成工具中，会git来提交代码。我说没有。 所以，先使用gitee。 01.登录gitee的官网，在此处登录。 02.绑定邮箱，我用的是QQ邮箱。
阅读更多...
服务端研发提测模板

服务端研发提测模板

test环境分支自测通过 提测邮件标注test环境分支 【xxxxxx需求】服务端研发提测了，快去测试吧!
阅读更多...
vue3中的provide

vue3中的provide

作用:实现祖孙件通信套路:父组件有一个provide选项来提供数据,子组件有一个inject选项来开始使用这些数据具体写法: 祖组件中: <template><div class"lim"><h2>祖组件</h2><child></child>名字:{{ car.name }}<br>价格:…
阅读更多...
SpringBoot与Vue前后端分离项目。用Nginx代理。

SpringBoot与Vue前后端分离项目。用Nginx代理。

Nginx代理主要是解决跨域与负载均衡的作用。 我这里用的自己的电脑&#xff0c;用的windows系统&#xff0c;不过配置基本是和Linux一样的。 下载Nginx nginx: download Nginx常用命令&#xff0c;先cd到解文件夹路径&#xff1a; nginx.exe&#xff1a;开启服务。nginx -s…
阅读更多...
预训练模型相关整理

预训练模型相关整理

1、怎么使用预训练网络&#xff1f; 使用预训练网络有两种方法&#xff1a;特征提取&#xff08;feature extraction&#xff09;和微调模型&#xff08;fine-tuning&#xff09;。 1、特征提取 特征提取是使用之前网络学到的表示来从新样本中提取出有趣的特征。然后将这些特…
阅读更多...
深入理解Linux网络——内核是如何发送网络包的

深入理解Linux网络——内核是如何发送网络包的

文章目录 一、相关实际问题二、网络包发送过程总览三、网卡启动准备四、数据从用户进程到网卡的详细过程1&#xff09;系统调用实现2&#xff09;传输层处理1. 传输层拷贝2. 传输层发送 3&#xff09;网络层发送处理4&#xff09;邻居子系统5&#xff09;网络设备子系统6&#…
阅读更多...
【微信小程序创作之路】- 小程常用页面样式

【微信小程序创作之路】- 小程常用页面样式

【微信小程序创作之路】- 小程常用页面样式 第四章 微信小程序用页面样式 文章目录 【微信小程序创作之路】- 小程常用页面样式前言一、总体样式--全局样式and局部样式1.全局样式2.局部样式 二、Flex布局&#x1f349;&#x1f349;&#x1f349;rpx单位 三、样式导入四、常用…
阅读更多...
unpkg 与 npm 的基本介绍

unpkg 与 npm 的基本介绍

目录 定义 特点 原理 使用 npm安装流程 npm install npm update registry 区别 总结 定义 UNPKG是一个基于npm registry 的静态资源 CDN 服务&#xff0c;它可以快速获取和使用任何JavaScript包&#xff0c;无需安装任何软件或包。UNPKG可以从NPM仓库中获取任何包&am…
阅读更多...
不受环境干扰，这套声学全息方案实现了虚实交互

不受环境干扰，这套声学全息方案实现了虚实交互

一谈到全息显示&#xff0c;大家默认想到的就是光学全息方案&#xff0c;比如需要将光投射到某样东西上&#xff0c;比如视网膜&#xff0c;或是烟雾等介质上&#xff0c;才能成像。市面上一些常见的2D、3D全息方案&#xff0c;如全息风扇、Voxon全息系统等等&#xff0c;分别采…
阅读更多...
【STM32MP135】修复10.1寸屏显示异色问题，添加极性配置

【STM32MP135】修复10.1寸屏显示异色问题，添加极性配置

文件路径&#xff1a;u-boot-stm32mp-v2021.10-stm32mp1-r1/drivers/video/stm32/stm32_ltdc.c
阅读更多...
代码随想录算法训练营第17期第14天 | 理论基础 、递归遍历、迭代遍历、统一迭代

代码随想录算法训练营第17期第14天 | 理论基础 、递归遍历、迭代遍历、统一迭代

理论基础 种类 满二叉树、完全二叉树、二叉搜索树 满二叉树 如果一颗二叉树只有度为0的节点和度为2的节点&#xff0c;并且度为0的节点都在同一层 完全二叉树 在完全二叉树中&#xff0c;除了最底层节点可能没填满外&#xff0c;其余每层节点数都达到最大值&#xff0c;并…
阅读更多...
Android Java代码与JNI交互 引用类型转换(五)

Android Java代码与JNI交互 引用类型转换(五)

🔥 Android Studio 版本 🔥 🔥 Java中基础数据类型对应Native的数据类型 🔥 * Java中基础数据类型对应Native中的数据类型* ｜Java ｜Native ｜* ｜boolean ｜jboolean ｜* ｜byte ｜jbyte ｜* ｜short ｜jshort ｜* ｜int ｜jint ｜* ｜long…
阅读更多...
c++编写消消乐游戏

c++编写消消乐游戏

#include <SFML/Graphics.hpp> #include <SFML/Audio.hpp> #include <time.h> using namespace sf;#define GAME_ROWS_COUNT 8 #define GAME_COLS_COUNT 8int ts 57; // 每一个游戏小方块区域的大小bool isMoving false; bool isSwap false;// 相邻位置…
阅读更多...
springboot基于协同过滤算法商品推荐系统

springboot基于协同过滤算法商品推荐系统

开发语言&#xff1a;Java 框架&#xff1a;springboot JDK版本&#xff1a;JDK1.8 服务器&#xff1a;tomcat7 数据库&#xff1a;mysql 5.7 数据库工具&#xff1a;Navicat11 开发软件&#xff1a;eclipse/myeclipse/idea Maven包&#xff1a;Maven3.3.9 浏览器&…
阅读更多...
MybatisPlus中的save方法和insert方法区别

MybatisPlus中的save方法和insert方法区别

insert方法时直接插入一条数据&#xff0c;需要手动设置每个字段的值&#xff0c;例如&#xff1a; save方法首先会先判断该数据是否已经存在于数据库中&#xff0c;如果存在则更新数据&#xff0c;如果不存在则插入数据。使用save方法时&#xff0c;需要先设置主键值&#xff…
阅读更多...
41.RocketMQ之高频面试题大全

41.RocketMQ之高频面试题大全

消息重复消费 影响消息正常发送和消费的重要原因是网络的不确定性。 引起重复消费的原因 ACK 正常情况下在consumer真正消费完消息后应该发送ack&#xff0c;通知broker该消息已正常消费&#xff0c;从queue中剔除。当ack因为网络原因无法发送到broker&#xff0c;broker会认为…
阅读更多...
【单片机】msp430f5529 万年历，数字时钟，矩阵键盘修改时间，7针OLED显示，内部温度读取

【单片机】msp430f5529 万年历，数字时钟，矩阵键盘修改时间，7针OLED显示，内部温度读取

文章目录 功能实物图原理图一些程序片段矩阵键盘内部温度读取将年月日转为星期 功能 1 显示万年历&#xff0c;利用内部RTC模块 2 按键修改时间 3 显示芯片内部的温度数值 实物图 原理图 一些程序片段 矩阵键盘 https://qq742971636.blog.csdn.net/article/details/1316505…
阅读更多...
Vue3 + TS + Element-Plus 封装Tree组件 《亲测可用》

Vue3 + TS + Element-Plus 封装Tree组件 《亲测可用》

前期回顾 Vite Vue3 Ts 《企业级项目》二次封装 el-table、el-pagination、el-tooltip、el-dialog_vue后台管理系统需要二次封装的组件有哪些_彩色之外的博客-CSDN博客封装的功能有哪些&#xff1f;分页、表格排序、文字居中、溢出隐藏、操作列、开关、宽、最小宽、type类型…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023