【三维路径规划】基于matlab无人机三维路径规划【含Matlab源码 1262期】
作者：互联网
一、无人机简介
0 引言
随着现代技术的发展，飞行器种类不断变多，应用也日趋专一化、完善化，如专门用作植保的大疆PS-X625无人机，用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机，以及用作水下救援的白鲨MIX水下无人机等，决定飞行器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题而言，在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作，可能会对操作员心理以及技术提出极高的要求，为了避免个人操作失误，进而造成飞行器损坏的危险，一种解决问题的方法就是对飞行器进行航迹规划。
飞行器的测量精度，航迹路径的合理规划，飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务，并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。
1 常见的航迹规划算法
图1 常见路径规划算法
文中主要对无人机巡航阶段的航迹规划进行研究，假设无人机在飞行中维持高度与速度不变，那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A算法计算简单，容易实现。在改进A算法基础上，提出一种新的、易于理解的改进A算法的无人机航迹规划方法。传统A算法将规划区域栅格化，节点扩展只限于栅格线的交叉点，在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化，然后分别用两段上的相应路径规划点作为切点，找到相对应的组成内切圆的圆心，然后作弧，并求出相对应的两切点之间的弧所对应的圆心角，根据下式计算出弧线的长度
式中：R———内切圆的半径；
α———切点之间弧线对应的圆心角。
二、部分源代码
%籍荤 切困 矫鼓饭捞磐涝聪促.
clear;
clc;
[Z, ref] = dted('n37.dt0');
N_s = 60;
x_s = linspace(0, N_s,N_s);
y_s = linspace(0, N_s,N_s);
x_2d_s = repmat(x_s,N_s,1);
y_2d_s = repmat(y_s',1,N_s);
x_1d_s = zeros(1,N_s*N_s);
y_1d_s = zeros(1,N_s*N_s);
z_1d_s = zeros(1,N_s*N_s);
z_2d_s = zeros(N_s,N_s);
cnt = 1;
for i = 1: N_s %1磊肺 父靛绰何盒
for j = 1: N_s
x_1d_s(cnt) = x_2d_s(i,j);
y_1d_s(cnt) = y_2d_s(i,j);
z_1d_s(cnt) = Z(i,j);
z_2d_s(i,j) = Z(i,j);
cnt = cnt+1;
end
end
%1窜拌 殿绊急瘤档 裙垫
% figure(1);
% set(gcf,'numbertitle','off','name', '殿绊急瘤档');
% contour(x_2d_s,y_2d_s,z_2d_s,'ShowText','on');
% xlabel('km');
% ylabel('km');
%2窜拌 困蛆钎扁
% 厚青 臭捞(扁霖 臭捞)
flight_height = 700;
% 厚青 臭捞焊促 角力 臭捞啊 臭酒 款亲捞 阂啊瓷茄 镑(厘局拱)
% 厘局拱狼 困摹绰 xn_new, yn_new, z_new 俊 历厘登绢乐澜
i_site = find(z_1d_s > flight_height); %臭捞 蔼林绰何盒 角力肺 荤侩瞪 农扁 魄窜
xn_new = zeros(1,length(i_site));
yn_new = zeros(1,length(i_site));
z_new = zeros(1,length(i_site));
for sitetmp = 1:length(i_site) % 臭捞俊 狼秦辑 吧矾柳 xy困摹 历厘
xn_new(sitetmp) = x_1d_s(i_site(sitetmp));
yn_new(sitetmp) = y_1d_s(i_site(sitetmp));
z_new(sitetmp) = z_1d_s(i_site(sitetmp));
end
[vx, vy] = voronoi(xn_new,yn_new);
% 1凯狼 单捞磐 裙垫
vx1 = vx(1,:);
vy1 = vy(1,:);
% 2凯狼 单捞磐 裙垫
vx2 = vx(2,:);
vy2 = vy(2,:);
c_r = 1;
vx_new = vx;
vy_new = vy;
for i = 1 : length(xn_new)
% 1凯狼 单捞磐 裙垫
vx1 = vx_new(1,:);
vy1 = vy_new(1,:);
% 2凯狼 单捞磐 裙垫
vx2 = vx_new(2,:);
vy2 = vy_new(2,:);
r_sq = (vx1-xn_new(i)).^2+(vy1-yn_new(i)).^2;
idx1 = find((r_sq < c_r^2));
r_sq = (vx2-xn_new(i)).^2+(vy2-yn_new(i)).^2;
idx2 = find((r_sq < c_r^2));
idx = union(idx1,idx2);
vx_new(:,idx) = [];
vy_new(:,idx) = [];
end
% figure(2);
% set(gcf,'numbertitle','off','name', '困蛆钎扁');
% contour(x_2d_s,y_2d_s,z_2d_s,'ShowText','on');
% xlabel('km');
% ylabel('km');
% hold on;
% %plot(vx_new,vy_new,'b.-')
% plot(xn_new,yn_new,'r.');
% axis([0 N_s 0 N_s]);
%3窜拌 款亲啊瓷 版肺 钎扁
% figure(3);
% set(gcf,'numbertitle','off','name', '版肺钎扁');
% contour(x_2d_s,y_2d_s,z_2d_s,'ShowText','on');
% xlabel('km');
% ylabel('km');
% hold on;
% plot(vx_new,vy_new,'b.-')
% plot(xn_new,yn_new,'r.');
% axis([0 N_s 0 N_s]);
%4窜拌 基荤捞飘 版肺 钎扁
th = 0:0.01:2*pi; %盔阑 弊府扁困茄 阿档 硅凯
c_cx = 15; c_cy = 17; c_r =5;
xc = c_r*cos(th)+c_cx;
yc = c_r*sin(th)+c_cy;
vx1 = vx_new(1,:);
vy1 = vy_new(1,:);
r_sq = (vx1-c_cx).^2+(vy1-c_cy).^2;
idx1 = find((r_sq < c_r^2));
vx2 = vx_new(2,:);
vy2 = vy_new(2,:);
r_sq = (vx2-c_cx).^2+(vy2-c_cy).^2;
idx2 = find((r_sq < c_r^2));
idx = union(idx1,idx2);
vx_new(:,idx) = [];
vy_new(:,idx) = [];
% figure(4);
% set(gcf,'numbertitle','off','name', '基荤捞飘馆康');
% contour(x_2d_s,y_2d_s,z_2d_s,'ShowText','on');
% xlabel('km');
% ylabel('km');
% hold on;
% plot(vx_new,vy_new,'b.-')
% plot(xn_new,yn_new,'r.');
% h = fill(xc,yc,'red');
% set(h,'facealpha',.5)
% axis([0 N_s 0 N_s]);
%5窜拌 弥辆版肺
% 瘤搁栏肺何磐 厚青阑 困秦 冻绢柳 芭府(km)
height = 5;
%xpoint = [10;49;22;46;14;30];
%ypoint = [10;42;53;13;29;17];
xpoint = [14; 10; 10];
ypoint = [29; 55; 10];
order = tsp(xpoint,ypoint);
% figure(5);
set(gcf,'numbertitle','off','name', '弥辆版肺');
hold on;
lgd = legend('免惯痢','档馒痢');
lgd.AutoUpdate = 'off';
contour(x_2d_s,y_2d_s,z_2d_s,'ShowText','on');
xlabel('km');
ylabel('km');
plot(vx_new,vy_new,'b.-');
plot(xn_new,yn_new,'r.');
h = fill(xc,yc,'red');
set(h,'facealpha',.5);
axis([0 N_s 0 N_s]);
for mainloof = 1 : length(order)-1
xy_start = [xpoint(order(mainloof)) ypoint(order(mainloof))];
xy_dest = [xpoint(order(mainloof+1)) ypoint(order(mainloof+1))];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 眠啊
% 流急狼 规沥侥 : y = ax + b
a_equation = atan((xy_dest(2) - xy_start(2)) / (xy_dest(1) - xy_start(1)));
b_equation = xy_start(2) - (a_equation * xy_start(1));
x_range = xy_start(1) : 0.0001 : xy_dest(1);
x_equation = [];
y_equation = [];
x_obs_pos = [];
y_obs_pos = [];
x_obs_location = [];
y_obs_location = [];
obs_pos = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 流急 版肺俊 乐绰 痢甸吝 老摹窍绰 瘤痢 茫扁
for i = 1 : N_s
num = find( x_range == x_1d_s(i) );
if(num)
x_equation(i) = x_1d_s(i);
end
end
% 老瞒 规沥侥
% y = ax + b
y_equation = (a_equation * x_equation) + b_equation;
for i = 1 : length(x_equation)
x_obs_pos = find(x_equation(i) == xn_new);
y_obs_pos = find(y_equation(i) == yn_new);
G = sparse(size(xy_all,1),size(xy_all,1));
for kdx = 1:length(edge_dist)
xy_s = [vx_new(1,kdx) vy_new(1,kdx)];
idx = find(sum((xy_all-kron(ones(size(xy_all,1),1),xy_s)).^2,2)==0);
xy_d = [vx_new(2,kdx) vy_new(2,kdx)];
jdx = find(sum((xy_all-kron(ones(size(xy_all,1),1),xy_d)).^2,2)==0);
G(idx,jdx) = edge_dist(kdx);
G(jdx,idx) = edge_dist(kdx);
end
st_idx = find(sum((xy_all-kron(ones(size(xy_all,1),1),xy_start)).^2,2)==0);
dest_idx = find(sum((xy_all-kron(ones(size(xy_all,1),1),xy_dest)).^2,2)==0);
[dist,paths,pred] = graphshortestpath(G,st_idx,dest_idx);
xy_opt_path = xy_all(paths,:);
% for i = 1 : length(xy_opt_path)
%
% end
%1凯狼 单捞磐 裙垫
d_r = 1.4;
d_r2 = 0.9;
xd = xy_opt_path(:,1);
yd = xy_opt_path(:,2);
trues = zeros(1,length(xy_opt_path));
for i = 1 : length(xy_opt_path)
r_sq = (xd(i)-xn_new).^2+(yd(i)-yn_new).^2;
idx1 = find((r_sq < d_r^2));
r_sq = (xd(i)-xc).^2+(yd(i)-yc).^2;
idx2 = find((r_sq < d_r2^2));
if (isempty(idx1)==1) && (isempty(idx2)==1)
trues(i) = 1;
end
end
end
end
%z_data
% using plot3 function ex. plot3(x1, y1, z1, ...)
plot3(xy_opt_path(:,1),xy_opt_path(:,2), z_data, 'LineWidth',4);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
三、运行结果
四、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]巫茜,罗金彪,顾晓群,曾青.基于改进PSO的无人机三维航迹规划优化算法[J].兵器装备工程学报. 2021,42(08)
[4]邓叶,姜香菊.基于改进人工势场法的四旋翼无人机航迹规划算法[J].传感器与微系统. 2021,40(07)
[5]马云红,张恒,齐乐融,贺建良.基于改进A*算法的三维无人机路径规划[J].电光与控制. 2019,26(10)
[6]焦阳.基于改进蚁群算法的无人机三维路径规划研究[J].舰船电子工程. 2019,39(03)
标签：路径,三维,find,2d,xy,vy,vx,new,源码 来源： https://www.cnblogs.com/QQ912100926/p/15193460.html