【无人机路径规划】无人机三维路径规划中蚁群算法、A* 与 RRT* 算法对（Matlab实现）-CSDN博客

本文链接：https://blog.csdn.net/qq_46009436/article/details/146806207

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

一、引言

随着无人机技术的快速发展，其在军事侦察、物流配送、环境监测等众多领域的应用日益广泛。在实际应用场景中，无人机需要在复杂的三维空间内规划出一条安全、高效的飞行路径，以避开障碍物并满足任务需求。蚁群算法、A* 算法和 RRT* 算法是目前无人机三维路径规划中常用的算法，它们各自具有独特的原理和特点，对其进行详细对比有助于根据具体应用场景选择最合适的算法。

二、算法原理

蚁群算法蚁群算法是一种模拟蚂蚁觅食行为的启发式优化算法。蚂蚁在寻找食物的过程中，会在走过的路径上释放信息素，信息素浓度越高的路径对其他蚂蚁的吸引力越大。在无人机路径规划中，将三维空间划分为多个节点，每只“虚拟蚂蚁”从起点开始，根据信息素浓度和启发式信息选择下一个节点，不断迭代更新信息素浓度，最终找到一条从起点到终点的最优路径。

A* 算法 A* 算法是一种基于图搜索的路径规划算法，它结合了 Dijkstra 算法的广度优先搜索和贪心最佳优先搜索的特点。A* 算法通过定义一个评估函数f(n)=g(n)+h(n)，其中 (g(n)) 是从起点到节点 (n) 的实际代价，(h(n)) 是从节点 (n) 到终点的启发式估计代价。算法从起点开始，不断扩展具有最小 (f(n)) 值的节点，直到找到终点或遍历完所有可达节点。

RRT* 算法 RRT* 算法是快速随机树算法（RRT）的改进版本，它通过在三维空间中随机采样来构建一棵搜索树。从起点开始，每次随机选择一个采样点，然后在搜索树中找到距离该采样点最近的节点，将其连接到采样点并扩展树。RRT* 算法在 RRT 的基础上，增加了路径优化机制，通过重新连接节点和修剪树枝，不断优化树的结构，最终得到一条接近最优的路径。

三、性能对比

路径质量 -蚁群算法：通过信息素的更新和积累，能够在多次迭代后找到全局较优的路径。但由于信息素的更新过程相对缓慢，在复杂环境下可能陷入局部最优解，导致路径质量下降。A* 算法：在有明确启发式函数的情况下，能够保证找到最优路径。但启发式函数的选择对算法性能影响较大，如果选择不当，可能会增加搜索时间。RRT* 算法：能够在复杂的三维环境中快速找到一条可行路径，并且随着采样点数的增加，路径质量会逐渐提高，趋近于全局最优解。但在采样点数有限的情况下，路径可能存在一定的冗余。

搜索效率蚁群算法：由于需要多次迭代更新信息素，算法的收敛速度较慢，尤其是在大规模搜索空间中，搜索效率较低。 A* 算法：搜索效率取决于启发式函数的准确性和搜索空间的复杂度。在简单环境中，A* 算法能够快速找到最优路径；但在复杂环境中，搜索空间呈指数级增长，导致搜索效率急剧下降。 RRT* 算法：通过随机采样的方式扩展搜索树，能够快速覆盖搜索空间，在复杂环境中具有较高的搜索效率。但随着采样点数的增加，算法的计算量也会相应增加。

环境适应性蚁群算法：对环境的适应性较强，能够处理复杂的障碍物分布和动态环境。但由于信息素的更新需要一定的时间，在环境变化较快的情况下，算法的响应速度较慢。A* 算法*：适用于静态环境和障碍物分布规则的场景。在动态环境中，由于需要重新规划路径，算法的效率会受到影响。 RRT* 算法：具有较好的环境适应性，能够在复杂的三维环境中快速找到可行路径。并且可以通过实时采样和更新搜索树，适应动态环境的变化。

四、应用场景

蚁群算法适用于对路径质量要求较高、环境相对稳定且搜索空间不是特别大的场景，如城市环境中的无人机巡检任务。在这种场景下，蚁群算法可以通过多次迭代找到一条较为优化的路径，同时对环境的适应性也能保证路径的安全性。

A* 算法适用于静态环境和障碍物分布规则的场景，如室内无人机导航。在这种场景下，A* 算法能够利用准确的启发式函数快速找到最优路径，提高导航效率。

RRT* 算法适用于复杂的三维环境和动态环境，如森林火灾监测、军事侦察等。在这些场景中，RRT* 算法能够快速找到可行路径，并且通过实时更新搜索树适应环境的变化，保证无人机的安全飞行。

五、结论

蚁群算法、A* 算法和 RRT* 算法在无人机三维路径规划中各有优劣。蚁群算法注重全局搜索，能够找到较优路径，但搜索效率较低；A* 算法能够保证找到最优路径，但对环境的适应性较差；RRT* 算法具有较高的搜索效率和环境适应性，但路径质量在采样点数有限时可能存在一定的不足。在实际应用中，需要根据具体的场景需求和环境特点，选择最合适的路径规划算法，以实现无人机的高效、安全飞行。

📚2 运行结果

主函数代码：

%公众号：荧光Matlab
%Wishing you to encourage yourself！

clc;clear;close all
%载入函数路径
addpath(genpath('./ACO3D'))
addpath(genpath('./Astar3D'))
addpath(genpath('./RRT3D'))
addpath(genpath('./Evaluation'))
Algorithm_name={'ACO','Astar','RRT'};
%地图规模在Makemap3D函数中设置
map = Makemap3D;%地图
source=[10 10 1]; %起点
goal=[450 400 50];%终点
max_item = 1000;%最大迭代次数
comparative_data ={};%记录需要比较的内容
Global_data={};
Straight_distance = sqrt(sum((source-goal).^2, 2));%直线距离
fprintf('起点到终点的直线距离： \n%d\n\n',Straight_distance); 
Global_data(1,end+1) = {num2str(source)};
Global_data(1,end+1) = {num2str(goal)};
Global_data(1,end+1) = {Straight_distance};
t1=clock;
%% ********蚁群算法********************************
figure(1)
plot3DMap(map);
text(source(1),source(2),source(3),'起点','color','r');
text(goal(1),goal(2),goal(3),'终点','color','r');
scatter3(source(1),source(2),source(3),"filled","g");
scatter3(goal(1),goal(2),goal(3),"filled","b");
title('ACO');
popNum = 10; %蚁群数量
tic
[aco_path,aco_cost,aco_Number_of_searches,aco_Number_of_successful_searches,aco_Number_of_failed_searches] = aco(source,goal,map,popNum);%蚁群主函数
aco_time = toc;
fprintf('ACO 历时：%0.3f 秒\n',aco_time); 
hold on
plot3(aco_path(:,1),aco_path(:,2),aco_path(:,3),'LineWidth',2,'color','r');
view(-30,30);
fprintf('ACO 路径长度：%d \n\n',aco_cost); 
[aco_max_turning_angle,aco_turning_num,aco_index] = Max_turning_angle(aco_path,1);
comparative_data(1,end+1) = {aco_time};
comparative_data(2,end) = {aco_cost};
comparative_data(3,end) = {size(aco_path,1)};
comparative_data(4,end) = {aco_Number_of_searches};
comparative_data(5,end) = {aco_Number_of_successful_searches};
comparative_data(6,end) = {aco_Number_of_successful_searches/aco_Number_of_searches};
comparative_data(7,end) = {aco_max_turning_angle};
comparative_data(8,end) = {aco_turning_num};
%% ************Astar********************************
figure(2)
plot3DMap(map);
text(source(1),source(2),source(3),'起点','color','r');
text(goal(1),goal(2),goal(3),'终点','color','r');
scatter3(source(1),source(2),source(3),"filled","g");
scatter3(goal(1),goal(2),goal(3),"filled","b");
title('Astar');
tic%计算运行时间
[astar_path,astar_cost,astar_Number_of_searches,astar_Number_of_successful_searches,astar_Number_of_failed_searches] = Astar_main(source,goal,map,max_item);%A*主函数
astar_time = toc;
fprintf('Astar 历时：%0.3f 秒\n',astar_time); 
plot3(astar_path(:,1),astar_path(:,2),astar_path(:,3),'LineWidth',2,'color','r');
view(-30,30);
fprintf('Astar 路径长度：%d\n\n',astar_cost); 
[astar_max_turning_angle,astar_turning_num,astar_index] = Max_turning_angle(astar_path,1);
%记录展示数据
comparative_data(1,end+1) = {astar_time};
comparative_data(2,end) = {astar_cost};
comparative_data(3,end) = {size(astar_path,1)};
comparative_data(4,end) = {astar_Number_of_searches};
comparative_data(5,end) = {astar_Number_of_successful_searches};
comparative_data(6,end) = {astar_Number_of_successful_searches/astar_Number_of_searches};
comparative_data(7,end) = {astar_max_turning_angle};
comparative_data(8,end) = {astar_turning_num};

%% ****************RRT********************************
% subplot(1,3,3); 
figure(3)
plot3DMap(map);
text(source(1),source(2),source(3),'起点','color','r');
text(goal(1),goal(2),goal(3),'终点','color','r');
scatter3(source(1),source(2),source(3),"filled","g");
scatter3(goal(1),goal(2),goal(3),"filled","b");
title('RRT');
step = 10;%设置步长
tic
[rrt_path,rrt_cost,rrt_Number_of_searches,rrt_Number_of_successful_searches,rrt_Number_of_failed_searches] = RRT_main(source,goal,map,step);%RRT主函数
rrt_time = toc;
fprintf('RRT 历时：%0.3f 秒\n',rrt_time); 
plot3(rrt_path(:,1),rrt_path(:,2),rrt_path(:,3),'LineWidth',2,'color','r');
view(-30,30);
fprintf('RRT 路径长度：%d \n\n',rrt_cost); 
[rrt_max_turning_angle,rrt_turning_num,rrt_index] = Max_turning_angle(rrt_path,1);
%记录搜索覆盖范围
comparative_data(1,end+1) = {toc};
comparative_data(2,end) = {rrt_cost};
comparative_data(3,end) = {size(rrt_path,1)};
comparative_data(4,end) = {rrt_Number_of_searches};
comparative_data(5,end) = {rrt_Number_of_successful_searches};
comparative_data(6,end) = {rrt_Number_of_successful_searches/rrt_Number_of_searches};
comparative_data(7,end) = {rrt_max_turning_angle};
comparative_data(8,end) = {rrt_turning_num};
%% 打印运行时间
t2=clock;
fprintf('程序总运行时间：%0.3f 秒\n\n', etime(t2,t1));
%计算最优项 ，其中展示表格顺序为aco、astar、rrt
comparative_data(1,end+1) = {Algorithm_name{Min_value(comparative_data{1,1},comparative_data{1,2},comparative_data{1,3})}};
for i = 2:size(comparative_data,1)
    %求每行的最小值
    comparative_data(i,end) = {Algorithm_name{Min_value(comparative_data{i,1},comparative_data{i,2},comparative_data{i,3})}};
end
%求最大值
comparative_data(6,end) = {Algorithm_name{Max_value(comparative_data{6,1},comparative_data{6,2},comparative_data{6,3})}};


%展示比较结果
Show_Comparative_result(Global_data,comparative_data)

%三种算法展示到一张图中
figure(4)
plot3DMap(map);
text(source(1),source(2),source(3),'起点','color','r');
text(goal(1),goal(2),goal(3),'终点','color','r');
h1 = plot3(aco_path(:,1),aco_path(:,2),aco_path(:,3),'LineWidth',2,'color','r');
h2 = plot3(astar_path(:,1),astar_path(:,2),astar_path(:,3),'LineWidth',2,'color','k');
h3 = plot3(rrt_path(:,1),rrt_path(:,2),rrt_path(:,3),'LineWidth',2,'color','m');
legend([h1,h2,h3],'蚁群','A*','RRT')
%legend("boxoff")