【DOA三维路径规划】野狗算法无人机避障三维航迹规划【含Matlab源码 3618期】

最新推荐文章于 2024-04-23 08:34:37 发布
最新推荐文章于 2024-04-23 08:34:37 发布
阅读量1.1k 收藏 21
点赞数 16
分类专栏： Matlab路径规划（高阶版） 文章标签： matlab
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_63266434/article/details/134961615
版权

在这里插入图片描述

⛄一、野狗算法无人机避障三维航迹规划简介

1 无人机航迹规划问题的数学模型
建立三维航迹规划问题的数学模型时, 不但考虑无人机基本约束, 还考虑复杂的飞行环境, 包括山体地形和雷暴威胁区。

1.1 无人机基本约束
规划的无人机三维航迹, 通常需要满足一些基本约束, 包括最大转弯角、最大爬升角或下滑角、最小航迹段长度、最低和最高飞行高度, 以及最大航迹长度等约束。其中, 最大转弯角约束, 是指无人机只能在水平面内小于或等于指定的最大转弯角内转弯;最大爬升角或下滑角约束, 是指无人机只能在垂直平面内小于或等于指定的最大爬升角或下滑角内爬升或下滑;最小航迹段长度约束, 要求无人机改变飞行姿态之前, 按目前的航迹方向飞行的最短航程;最低和最高飞行高度约束, 要求无人机在指定的飞行高度区间飞行;最大航迹长度约束, 是指无人机的航迹长度小于或等于指定的阈值。

记q (x, y, z, θ, ψ) 为无人机的飞行位置与姿态, 其中, (x, y, z) 为无人机的位置, θ为无人机的水平转弯角, ψ为无人机的竖直爬升角或下滑角, 进而建立上述基本约束的数学表达式。
在这里插入图片描述
1.2 飞行环境障碍物和威胁区建模
在飞行环境中, 高耸的山体近似采用圆锥体等效表示, 用以e为底的自然指数图形生成, 那么, 山体地形可以通过多个位置不同的圆锥体叠加而成。若将参考海拔基准高度设置为xOy平面, 记 (x, y, z) 为山体地形中的点, 那么
在这里插入图片描述
式中:N为山体个数;xk0和yk0为第k座山体中心对称轴的横坐标和纵坐标;hk为第k座山体的最大高度;xki和yki为第k座山体的横向斜度和纵向斜度。

在飞行环境中, 山体附近通常存在雷暴等极端气象, 本文视为飞行威胁区, 并通过球体近似等效表示, 且记第k座山体附近飞行威胁区的球心坐标为 (xks0, yks0, zks0) , 半径为rk。

1.3 目标函数及航迹表示
在本文中, 执行任务的某型无人机, 其航迹规划的目标函数是生成一条由起始点到目标点的无碰撞可行航迹。采用q (x, y, z, θ, ψ) 表示无人机在飞行空域中某特定位置的特定姿态, 那么 (x, y, z) 则表示无人机所在航迹点, θ表示无人机的水平转弯角, ψ表示无人机的竖直爬升角或下滑角。采用r (q) 表示由起始点qinitial到目标点qgoal的无碰撞可行航迹, 那么航迹规划的过程可以写成如下形式:
在这里插入图片描述

2 野狗算法
野狗优化算法(Dingo Optimization Algorithm,DOA)是于2021年提出的一种新型智能优化算法，该算法是根据澳大利亚野狗的社交行为设计的，具有寻优能力强，收敛速度快等特点。
DOA算法的灵感来源于野狗的狩猎策略，即迫害攻击、分组策略和清扫行为。

2.1 种群初始化
野狗种群在搜索边界内随机初始化。

2.2 策略一：群体攻击
捕食者通常使用高度智能的狩猎技术，野狗通常单独捕食小猎物，如兔子，但当捕食大猎物，如袋鼠时，它们会成群结队。野狗能找到猎物的位置并将其包围。

2.3 策略二：迫害攻击
野狗通常捕猎小猎物，直到单独捕获为止。

2.4 策略三：清扫
清扫行为为被定义为当野狗在它们的栖息地随意行走时找到腐肉吃的行为。

2.5 策略四：野狗的存活率

⛄二、部分源代码

%% 三维地图-无人机寻路
% 3D map - aircraft pathfinding
%% 这是使用原始算法的直接求解结果，添加专用于本问题的更新方式可以进一步提高精度
% This is the direct result of using the original algorithm,
% adding some specific update methods to this problem can further improve the accuracy
clc;
clear;
close all;
warning off

%% 载入数据
data.S=[1,950,12]; %起点位置
data.E=[950,1,1]; %终点点位置
data.Obstacle=xlsread(‘data.xls’);
data.numObstacles=length(data.Obstacle(:,1));
data.mapSize=[1000,1000,20]; %10m 地图尺寸
data.unit=[50,50,1]; %地图精度
data.S0=ceil(data.S./data.unit);
data.E0=ceil(data.E./data.unit);
data.mapSize0=data.mapSize./data.unit;
data.map=zeros(data.mapSize0);
figure
plot3(data.S(:,1),data.S(:,2),data.S(:,3),‘o’,‘LineWidth’,1,…
‘MarkerEdgeColor’,‘g’,…
‘MarkerFaceColor’,‘g’,…
‘MarkerSize’,8)
hold on
plot3(data.E(:,1),data.E(:,2),data.E(:,3),‘h’,‘LineWidth’,1,…
‘MarkerEdgeColor’,‘g’,…
‘MarkerFaceColor’,‘g’,…
‘MarkerSize’,8)
for i=1:data.numObstacles
x=1+data.Obstacle(i,1);
y=1+data.Obstacle(i,2);
z=1+data.Obstacle(i,3);
long=data.Obstacle(i,4);
wide=data.Obstacle(i,5);
pretty=data.Obstacle(i,6);
[V,F] = DrawCuboid(long, wide, pretty, x,y,z);
x0=ceil(x/data.unit(1));
y0=ceil(y/data.unit(2));
z0=ceil(z/data.unit(3));
long0=ceil(long/data.unit(1));
wide0=ceil(wide/data.unit(2));
pretty0=ceil(pretty/data.unit(3));
data.map(x0:x0+long0,y0:y0+wide0,z0:z0+pretty0)=1;
end
legend(‘起点’,‘终点’)
title(‘三维地形地图’)
grid on
axis equal
%%
% index=find(data.map==1);
% [p1,p2,p3] = ind2sub(size(data.map), index);
% figure
% plot3(data.S0(:,1),data.S0(:,2),data.S0(:,3),‘o’,‘LineWidth’,1,…
% ‘MarkerEdgeColor’,‘g’,…
% ‘MarkerFaceColor’,‘g’,…
% ‘MarkerSize’,8)
% hold on
% plot3(data.E0(:,1),data.E0(:,2),data.E0(:,3),‘h’,‘LineWidth’,1,…
% ‘MarkerEdgeColor’,‘g’,…
% ‘MarkerFaceColor’,‘g’,…
% ‘MarkerSize’,8)
% plot3(p1,p2,p3,‘.’,‘LineWidth’,1,…
% ‘MarkerEdgeColor’,‘k’,…
% ‘MarkerFaceColor’,‘g’,…
% ‘MarkerSize’,10)
% legend(‘起点’,‘终点’)
% title(‘三维地形地图’)
% grid on
% axis equal
% xlabel(‘x（km）’)
% ylabel(‘y（km）’)
% zlabel(‘z（km）’)
%% 生成可移动方向
temp=[1,0,-1];
direction=[];
for i=1:3
for j=1:3
for k=1:3
direction=[direction;temp(i),temp(j),temp(k)];
end
end
end
position=find(direction(:,1)==0 & direction(:,2)==0 & direction(:,3)==0);
direction(position,:)=[];
data.direction=direction;

%% 算法参数设置 Parameters
% 基本参数
numAgent=20; %种群个体数 size of population，可自行修改
Max_iter=20; %最大迭代次数 maximum number of interation，可自行修改
lb=0;%下限，可自行修改
ub=1;%上限，可自行修改
dim=prod(data.mapSize0); % 优化变量个数
fobj=@(x) aimFcn(x,data);%目标函数，用以优化

%% 使用优化算法求解

Optimal_results{2,index}=recording;%迭代曲线
Optimal_results{3,index}=bestY;%最佳函数值
Optimal_results{4,index}=bestX; %最佳变量值
Optimal_results{5,index}=result; %优化结果
Optimal_results{6,index}=toc; %运行时间
index = index +1;

⛄三、运行结果

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]田疆,李二超.用于无人机三维航迹规划改进连接型快速扩展随机树算法[J].航空工程进展. 2018,9(04)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

Matlab领域
关注
  • 16
    点赞
  • 踩
  • 21
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
【路径规划】fmincon无人机航路避障规划【含Matlab源码 2723期】
Matlab领域的博客
06-22 385
fmincon无人机航路避障规划 完整的代码，方可运行；可提供运行操作视频！适合小白！
2023年深圳杯数学建模C题之无人机协同避障航迹规划附思路及参考代码
matlab_dingdang的博客
07-25 7254
无人机的转弯半径不小于30 m。本质还是二维的路径优化问题，对于求解算法的选择以及模型的假设，都需要根据自己队伍的能力进行选择。需要注意的是，无人机协同避障航迹规划是一个复杂的问题，需要综合考虑多个因素，如传感器准确性、通信延迟、任务优先级等。问题5 当B机的恒定速率在[10,50] m/s内变化、B站点到圆心的距离在[1,10] km内变化(其他参数保持不变)时，问题2中的最优航迹会如何变化？无人机协同避障航迹规划是指多架无人机在共享空域中进行避障飞行，以确保它们之间的安全距离，并完成各自的任务。
基于控制的角度无人机集群——避障（有源代码）
weixin_43389824的博客
05-21 7646
谈一下自己对避障的理解，着重介绍Olfati-Saber论文中的算法，因为这篇论文堪称集群领域的顶峰。
基于机器学习的无人机避障技术详解，无人机避障技术应用前景
专注多旋翼、复合翼无人机技术研究，从事低空经济行业。
04-23 1260
本文将详细探讨无人机避障技术在不同领域的应用，包括无人机快递与物流、灾害现场侦察、农业无人机应用、城市交通管理、建筑施工监控、环境保护与监测、娱乐与航拍应用以及军事侦察与防御。基于机器学习的无人机避障技术具有广泛的应用场景，包括航空摄影、环境监测、农业无人机、无人机快递等。未来，随着技术的进一步发展，我们期待无人机能够在更多领域发挥其独特的作用，同时，也面临着更多挑战，如复杂环境下的避障、多无人机协同避障等。通过不断的研究和实践，我们有信心能够进一步提高无人机的避障性能，推动无人机技术的发展和应用。
【路径规划】基于fmincon实现无人机航路避障规划附matlab代码
m0_60703264的博客
06-15 156
根据敌方防御雷达,防空火力等威胁以及禁飞区的分布情况,构造基于战场威胁中心图,得到可以规避各种威胁的航迹线段,结合战场威胁信息,计算航迹段的代价,形成有向图,计算出无人机初始最优航路,利用无人机初始状态和性能约束进行航路的进一步修正,满足了无人机的飞行特点.并运用MATLAB编制图形化界面,实现仿真结果的图形显示.​。
【无人机三维路径规划】基于多元宇宙算法实现多无人机避障航迹规划附matlab代码...
Matlab_dashi的博客
04-30 922
✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。????个人主页：Matlab科研工作室????个人信条：格物致知。⛄ 内容介绍四旋翼飞行器具有机械设计简单,坚固耐用,体积小成本低,易于携带传感器,相机等优点.特有的悬停能力和垂直起降的特点使其在军用领域和民用邻域的使用愈加普遍化.在四旋翼...
智能优化算法：野狗优化算法 - 附代码
Jack旭的博客
01-07 7271
智能优化算法：野狗优化算法 文章目录智能优化算法：野狗优化算法1.算法原理1.1 种群初始化1.2 策略一：群体攻击1.3 策略二：迫害攻击1.4 策略三：清扫1.5 策略四：野狗的存活率2.实验结果3.参考文献4.Matlab代码 摘要:野狗优化算法(Dingo Optimization Algorithm,DOA)是于2021年提出的一种新型智能优化算法，该算法是根据澳大利亚野狗的社交行为设计的，具有寻优能力强，收敛速度快等特点。 1.算法原理 DOA算法的灵感来源于野狗的狩猎策略，即迫害攻击、分组策略
【WOA三维路径规划】基于matlab鲸鱼算法无人机避障三维航迹规划【含Matlab源码 4760期】.mp4
最新发布
06-21
Matlab研究室上传的视频均有对应的完整代码，皆可运行，亲测可用，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 运行结果效果图； 2、代码运行版本 Matlab 2019b；若运行有误，...
【WSO三维路径规划】基于matlab白鲨算法无人机避障三维航迹规划【含Matlab源码 4761期】.mp4
06-21
Matlab研究室上传的视频均有对应的完整代码，皆可运行，亲测可用，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 运行结果效果图； 2、代码运行版本 Matlab 2019b；若运行有误，...
【TSO三维路径规划】基于matlab金枪鱼算法无人机避障三维航迹规划【含Matlab源码 4758期】.mp4
06-21
Matlab研究室上传的视频均有对应的完整代码，皆可运行，亲测可用，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 运行结果效果图； 2、代码运行版本 Matlab 2019b；若运行有误，...
【CPO三维路径规划】基于matlab豪猪算法无人机避障三维航迹规划【含Matlab源码 4051期】.mp4
03-08
Matlab研究室上传的视频均有对应的完整代码，皆可运行，亲测可用，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 运行结果效果图； 2、代码运行版本 Matlab 2019b；若运行有误，...
一个多无人机协调编队避障控制算法的思路
02-24
近几年来，无人机集群研究过程中，多机无人机避障控制，成为了当下热门话题，我们都知道无人机在实际的“飞行空域中可能会存在建筑物、山峰、鸟群等障碍物，这些障碍物的存在将威胁无人机的飞行安全。除此之外，避障过程中，无人机之间的距离也会随着编队避障机动发生改变，处理不当就十分容易发生相撞。因此，无人机编队要能够根据不同环境情况做出决策,同时规避威胁障碍物和其他无人机。目前，针对适应环境的控制算法问题，国内外已经做出了很多研究，但是普遍存在协同性不高和队形保持不好的缺陷。很多研究人员将无人机群的编队任务和避障任务考虑为竞争关系，认为避障时应首先解除编队，飞过危险区域后再恢复编队继续飞行，但在一些环境条件
基于改进A*算法的无人机避障路径规划
05-06
近年来, 物流行业的飞速发展, 运输是物流的重要环节之一, 根据数据显示, 运输的成本占据整个物流成本的50%以上. 无人机的使用有效的控制了运输成本, 合理规划物流无人机的飞行路线, 也起着至关重要的作用. 在物流无人机的航迹规划中, 必须保证无人机飞行过程中能够准确避开禁飞区. 本文基于A*算法, 结合多种类型的禁飞区, 设计出一种改进算法, 能够找到任意两客户点间无人机避障飞行的最优路线. 仿真结果表明, 本文所设计的算法能够有效解决多类型禁飞区并存的无人机避障路径规划问题.
[matlab] 机械臂三维空间避障路径规划（可直接运行）
10-16
This file contains the value iteration algorithm written to evaluate the optimal path for a 3-Link Planar Robotic Arm with obstacles.
【WHO三维路径规划】基于matlab人野马算法无人机避障三维航迹规划【含Matlab源码 4759期】.mp4
06-21
Matlab研究室上传的视频均有对应的完整代码，皆可运行，亲测可用，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 运行结果效果图； 2、代码运行版本 Matlab 2019b；若运行有误，...
【TGA三维路径规划】基于matlab树木生长算法无人机避障三维航迹规划【含Matlab源码 4755期】.mp4
06-21
Matlab研究室上传的视频均有对应的完整代码，皆可运行，亲测可用，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 运行结果效果图； 2、代码运行版本 Matlab 2019b；若运行有误，...
【路径规划】野狗算法栅格地图机器人路径规划【含Matlab源码 2834期】
Matlab武动乾坤的博客
11-20 610
野狗算法栅格地图机器人路径规划 完整的代码，方可运行；可提供运行操作视频！适合小白！
ardupilot避障代码分析
陌城烟雨
10-31 960
避障传感器驱动；近年来，随着无人机市场的飞速增长， 避障技术已成为中高端消费级无人机的标配系统， 避障能力是实现自动化乃至智能化的关键环节，完善的自主避障系统将能够在很大程度上，减少因操作失误造成的无人机损坏和伤及人身和建筑物的事故发生率，可见无人机实现避障的重要性。
四旋翼无人机避障初窥
weixin_43180028的博客
03-14 768
https://b23.tv/av95512867 可以实现在任意建模场景中完成避障。 有兴趣的可以留言交流
用RRT*算法写一个无人机三维空间的避障路径规划MATLAB程序
05-13
好的，以下是一个基本的无人机三维空间的避障路径规划MATLAB程序。 首先，我们需要定义一个类来表示无人机。这个类包括位置、速度、加速度等属性，以及一些方法来更新这些属性。这里我们假设无人机可以以任意姿态飞行，因此我们需要使用四元数来表示无人机的方向。 ```matlab classdef UAV properties position % 当前位置 [x, y, z] velocity % 当前速度 [vx, vy, vz] acceleration % 当前加速度 [ax, ay, az] orientation % 当前方向 [qw, qx, qy, qz] max_speed % 最大飞行速度 max_acceleration % 最大加速度 end methods function obj = UAV(position, velocity, acceleration, orientation, max_speed, max_acceleration) obj.position = position; obj.velocity = velocity; obj.acceleration = acceleration; obj.orientation = orientation; obj.max_speed = max_speed; obj.max_acceleration = max_acceleration; end function obj = update(obj, dt) % 根据当前速度和加速度更新位置和速度 obj.position = obj.position + obj.velocity * dt + 0.5 * obj.acceleration * dt^2; obj.velocity = obj.velocity + obj.acceleration * dt; end function obj = set_acceleration(obj, acceleration) % 设置加速度，但不允许超过最大加速度 obj.acceleration = acceleration; if norm(obj.acceleration) > obj.max_acceleration obj.acceleration = obj.max_acceleration * obj.acceleration / norm(obj.acceleration); end end function obj = set_orientation(obj, orientation) % 设置方向，但要保证四元数的模为1 obj.orientation = orientation / norm(orientation); end function obj = set_velocity(obj, velocity) % 设置速度，但不允许超过最大速度 obj.velocity = velocity; if norm(obj.velocity) > obj.max_speed obj.velocity = obj.max_speed * obj.velocity / norm(obj.velocity); end end end end ``` 接下来，我们需要定义一个类来表示障碍物。这个类包括位置、大小等属性，以及一些方法来检查无人机是否与障碍物相交。 ```matlab classdef Obstacle properties position % 障碍物中心位置 [x, y, z] size % 障碍物大小 [dx, dy, dz] end methods function obj = Obstacle(position, size) obj.position = position; obj.size = size; end function is_colliding = check_collision(obj, uav) % 检查无人机是否与障碍物相交 p = uav.position; r = max(uav.size) / 2; q = obj.position; s = obj.size / 2; is_colliding = (abs(p(1) - q(1)) <= r + s(1)) && ... (abs(p(2) - q(2)) <= r + s(2)) && ... (abs(p(3) - q(3)) <= r + s(3)); end end end ``` 然后，我们需要定义一个函数来生成一组随机目标点。这个函数会在三维空间中随机生成一些点，并检查这些点是否与障碍物相交。如果某个点与障碍物相交，则重新生成一个新的点。 ```matlab function goals = generate_goals(num_goals, obstacles, xmin, xmax, ymin, ymax, zmin, zmax) goals = zeros(num_goals, 3); for i = 1:num_goals while true x = xmin + rand() * (xmax - xmin); y = ymin + rand() * (ymax - ymin); z = zmin + rand() * (zmax - zmin); is_colliding = false; for j = 1:length(obstacles) if obstacles(j).check_collision(UAV([x, y, z], [0, 0, 0], [0, 0, 0], [1, 0, 0, 0], 0, 0)) is_colliding = true; break; end end if ~is_colliding goals(i, :) = [x, y, z]; break; end end end end ``` 接下来，我们需要实现 RRT* 算法。这是一个基于 RRT 算法的路径规划算法，可以在具有复杂障碍物的三维空间中寻找无人机的安全路径。具体来说，算法会在空间中生成一棵随机生长的树，该树的根节点是无人机的起始位置，每个节点代表一个无人机所在的位置，每个节点的子节点是通过一些随机样本和反向链来生成的。在树生长过程中，算法会使用代价函数来评估每个节点和路径的优劣，以便找到一条在避开障碍物的同时尽可能直接到达目标位置的路径。 ```matlab function path = RRT_star(start_position, goal_positions, obstacles, xmin, xmax, ymin, ymax, zmin, zmax, max_iter, goal_tolerance) % 参数定义 step_size = 1; % 每一步的最大距离 num_goals = size(goal_positions, 1); num_obstacles = length(obstacles); tree = struct('position', {}, 'parent', {}, 'cost', {}, 'children', {}); % 树的结构体 tree(1).position = start_position; tree(1).parent = 0; tree(1).cost = 0; tree(1).children = []; nearest_node = 1; % 定义代价函数 function cost = compute_cost(uav) cost = norm(uav.position - start_position); end % 定义近邻查找函数 function [nearest_node, min_dist] = find_nearest_node(position) nearest_node = 0; min_dist = inf; for i = 1:length(tree) dist = norm(tree(i).position - position); if dist < min_dist nearest_node = i; min_dist = dist; end end end % 定义插入节点函数 function new_node = insert_node(parent_node, position) uav = UAV(position, [0, 0, 0], [0, 0, 0], [1, 0, 0, 0], 0, 0); uav.set_velocity(step_size * (position - tree(parent_node).position) / norm(position - tree(parent_node).position)); if norm(uav.velocity) > uav.max_speed uav.set_velocity(uav.max_speed * uav.velocity / norm(uav.velocity)); end uav.set_acceleration(zeros(1, 3)); uav.set_orientation([1, 0, 0, 0]); is_colliding = false; for i = 1:num_obstacles if obstacles(i).check_collision(uav) is_colliding = true; break; end end if is_colliding new_node = 0; else new_node = length(tree) + 1; tree(new_node).position = position; tree(new_node).parent = parent_node; tree(new_node).cost = tree(parent_node).cost + norm(position - tree(parent_node).position); tree(new_node).children = []; tree(parent_node).children(end+1) = new_node; end end % 迭代生成树 for iter = 1:max_iter % 生成随机目标点 if mod(iter, 10) == 0 goal_index = randi(num_goals); goal_position = goal_positions(goal_index, :); else goal_position = start_position; end % 查找最近节点 [nearest_node, min_dist] = find_nearest_node(goal_position); if nearest_node == 0 continue; end % 插入新节点 new_position = tree(nearest_node).position + step_size * (goal_position - tree(nearest_node).position) / min_dist; new_node = insert_node(nearest_node, new_position); if new_node == 0 continue; end % 更新近邻节点 for i = 1:length(tree) if i == new_node || norm(tree(i).position - new_position) > step_size continue; end uav = UAV(tree(i).position, [0, 0, 0], [0, 0, 0], [1, 0, 0, 0], 0, 0); uav.set_velocity(step_size * (new_position - tree(i).position) / norm(new_position - tree(i).position)); if norm(uav.velocity) > uav.max_speed uav.set_velocity(uav.max_speed * uav.velocity / norm(uav.velocity)); end uav.set_acceleration(zeros(1, 3)); uav.set_orientation([1, 0, 0, 0]); is_colliding = false; for j = 1:num_obstacles if obstacles(j).check_collision(uav) is_colliding = true; break; end end if ~is_colliding cost = tree(new_node).cost + norm(tree(i).position - new_position); if tree(i).parent == 0 || cost < tree(i).cost tree(i).parent = new_node; tree(i).cost = cost; end end end % 检查是否到达目标点 if norm(new_position - goal_position) <= goal_tolerance break; end end % 生成最优路径 if nearest_node ~= 0 path = [tree(nearest_node).position]; while tree(nearest_node).parent ~= 0 nearest_node = tree(nearest_node).parent; path = [tree(nearest_node).position; path]; end else path = []; end end ``` 最后，我们可以使用上述函数来进行路径规划： ```matlab % 定义起始点、目标点、障碍物等参数 start_position = [0, 0, 0]; goal_positions = generate_goals(10, obstacles, xmin, xmax, ymin, ymax, zmin, zmax); obstacles = [Obstacle([5, 5, 5], [2, 2, 2]), Obstacle([-5, -5, -5], [2, 2, 2])]; xmin = -10; xmax = 10; ymin = -10; ymax = 10; zmin = -10; zmax = 10; max_iter = 1000; goal_tolerance = 1; % 进行路径规划 path = RRT_star(start_position, goal_positions, obstacles, xmin, xmax, ymin, ymax, zmin, zmax, max_iter, goal_tolerance); % 绘制路径和障碍物 figure; hold on; for i = 1:length(obstacles) draw_obstacle(obstacles(i)); end plot3(path(:, 1), path(:, 2), path(:, 3), 'LineWidth',2); plot3(goal_positions(:, 1), goal_positions(:, 2), goal_positions(:, 3), 'ro', 'MarkerSize', 10); plot3(start_position(1), start_position(2), start_position(3), 'go', 'MarkerSize', 10); axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); % 定义绘制障碍物的函数 function draw_obstacle(obstacle) x = obstacle.position(1) + [-1, -1, 1, 1, -1, -1, 1, 1] * obstacle.size(1) / 2; y = obstacle.position(2) + [-1, 1, 1, -1, -1, 1, 1, -1] * obstacle.size(2) / 2; z = obstacle.position(3) + [-1, -1, -1, -1, 1, 1, 1, 1] * obstacle.size(3) / 2; patch(x, y, z, 'r'); end ```

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Matlab领域

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值