1 简介
近年来，随着科学技术的不断发展，信息技术的日新月异，战争的智能化、信息化和一体化，使得任务规划成为高技术战争的重要支撑。自 1917年美国研制出第一架无人机以来，无人机先后经历了靶机、侦察机和诱饵机几个发展阶段。无人机作为一种可重复使用的飞行器，以其结构简单、续航时间长、造价低、隐蔽性强和安全性高等优势，广泛地应用在信息化战争中执行监视、侦察、攻击、战场评估、精确打击、充当诱饵等任务，极大地提高了部队的指挥控制和多兵种协作作战的能力。早在越南战争、中东战争、科索沃战争、海湾战争及阿富汗战争中，无人机以其出色的表现受到世界各国的广泛关注。美国是无人机任务规划起步最早、发展最快、技术最先进的国家，在国外无人机技术发展的同时，中国也先后开启了无人机任务规划的研究。其中，北京航空航天大学、南京航空航天大学、西北工业大学和哈尔滨工业大学等高校先后成立了无人机相关研究机构，并取得了可喜的研究成果。自 2000 年以来一些民用无人机投入研制，无人机任务规划系统也从最初的单平台向多平台交互发展，目前有国防科技大学的多无人机协同任务资源分配与编队轨迹优化研究，哈尔滨工业大学的多无人机系统的协同目标分配和航迹规划方法研究、西北工业大学的无人机航路规划方法研究和北京航空航天大学的无人机路径规划技术研究等。此外，2015 年我国在纪念抗战 70 周年阅兵式上，首次展示了作战无人机，这表明了我国无人机的发展已经走向高新技术的前沿。随着全球格局的不断演变、军事科技的飞速发展，武器装备由机械化发展为信息化，战争方式较以前有了翻天覆地的变化。单无人机执行任务的局限性，使得多机协同作战成为当下研究的热点。多机协同执行高危任务，一方面可有效地降低毁伤概率，另一方面可提高战斗力。在多机协同执行任务中，当整个无人机系统达到最优时，并不能保证每架无人机均能达到最优。
指出了飞行器航迹规划与路径规划的区别;提出了一种给定威胁分布下的无人机路径规划算法.根据威胁分布情况构造无人机可能飞行的航路集,用voronoi图表示出来,采用Dijkstra算法搜索威胁分布图,求解粗略最短路径.在粗略最短路径的基础上,应用三次样条曲线和序列二次规划的方法求解最优路径.用Matlab进行仿真验证,证明了算法的有效性.
2 部分代码
%籍荤 切困 矫鼓饭捞磐涝聪促.
clear;
clc;
[Z, ref] = dted('n37.dt0');
N_s = 100;
x_s = linspace(1, N_s,N_s);
y_s = linspace(1, 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');
%% height
%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;
c_cx = 0; c_cy = 0; c_r = 0;
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);
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]);
% 急雀 版肺 醚 辨捞
path_length_all = 0;
part_length = [];
% 弥措 惑铰 臭捞(弥措 款侩 绊档)
max_flight_height = 1000;
%% 惑铰
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% z绵 埃拜
z_alpha = 20;
% 厘局拱 困摹 颇厩
x_obs = [];
y_obs = [];
count_obs = 0;
% 流急狼 规沥侥 : y = ax + b
res = polyfit(xpoint, ypoint, 1);
x_equation = xpoint(1) : (x_s(2)-x_s(1)) : xpoint(2);
y_equation = res(1)*x_equation + res(2);
% 惑铰 棺 窍碍困茄 臭捞蔼 眠免 函荐
z_lift_height = [];
% flight height甫 馆康茄 臭捞 沥焊
lift_height = [];
y_index = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 流急 版肺俊 乐绰 痢甸吝 老摹窍绰 y 瘤痢 茫扁
for i = 1 : length(y_equation)
flag = 0;
for j = 2 : N_s-1
num = find( y_equation(i) == y_s(j) );
if(num)
y_equation(i) = y_s(j);
flag = 1;
break;
else
flag = 0;
end
end
if(flag == 0)
y_equation(i) = y_s(knnsearch(y_s', y_equation(i)));
end
y_index(i) = knnsearch(y_s', y_equation(i));
end
%x_equation
%y_equation
lift_height = [];
obs_index = [];
% 厘局拱狼 俺荐
count_obs = 0;
% 流急 版肺俊 乐绰 痢甸吝 老摹窍绰 z 瘤痢 茫扁
for k = 1 : length(y_equation)
for i = 1 : N_s
% x客 悼老茄 瘤痢 2d俊辑 茫扁
if(find( x_equation(k) == x_s(i) ))
z_lift_height(k) = z_2d_s( (y_index(k)-1)*N_s + i );
if(z_lift_height(k) < flight_height)
lift_height(k) = flight_height;
% elseif(z_lift_height(k) == flight_height)
count_obs = count_obs + 1;
% lift_height(k) = z_lift_height(k);
obs_index(count_obs) = k;
else
lift_height(k) = z_lift_height(k);
end
end
end
end
% z_lift_height
% plot3(x_equation, y_equation, lift_height + z_alpha, 'black', 'LineWidth',4);
%% 惑铰阿 备窍扁
% obs_index
% 厘局拱狼 困摹 沥焊 历厘
x_obs = [];
y_obs = [];
th = [];
% x_equation
% y_equation
% lift_height
% for k = 1 : length(y_equation)
%
% if((lift_height(k)-3) == flight_height)
% % 厘局拱 困摹 颇厩
% % count_obs = count_obs + 1;
% x_obs(count_obs) = x_equation(k);
% y_obs(count_obs) = y_equation(k);
%
% z_obs(count_obs) = lift_height(k);
%
% end
%
% end
temp_away = 5;
% k == obs_index(i)
for i = 1 : count_obs
k = obs_index(i);
if( (k > temp_away) && (k ~= obs_index(end)) )
% disp('yes');
if( lift_height(k) < lift_height(k+1) )
% disp('up');
% y_equation(k+1)
% y_equation(k)
% x_equation(k+1)
% x_equation(k)
a = ( (lift_height(k+1) - lift_height(k-temp_away)) / (x_equation(k+1) - x_equation(k-temp_away)) );
th_temp = atan(a);
% radian -> degree
th(i) = th_temp * 180 / pi;
elseif( lift_height(k) < lift_height(k-1) )
% disp('down');
else
% disp('nothing');
end
end
end
% x_obs
% y_obs
% z_obs
count_obs
th
%% 傈眉 惑铰 版肺 辨捞 备窍扁
% 惑铰 棺 窍碍 版肺 醚 辨捞
lift_length = 0;
%z_lift_height
lift_length_part = [];
%lift_heigth
for i = 1 : length(y_equation)-1
lift_length_part(i) = sqrt( (x_equation(i+1)-x_equation(i))^2 + (y_equation(i+1)-y_equation(i))^2 + ( (lift_height(i+1)-lift_height(i))/1000 )^2 );
lift_length = lift_length + sqrt( (x_equation(i+1)-x_equation(i))^2 + (y_equation(i+1)-y_equation(i))^2 + ( (lift_height(i+1)-lift_height(i))/1000 )^2 );
end
disp('傈眉 惑铰 版肺 辨捞');
lift_length
%%
for mainloof = 1 : length(order)-1
xy_start = [xpoint(order(mainloof)) ypoint(order(mainloof))];
xy_dest = [xpoint(order(mainloof+1)) ypoint(order(mainloof+1))];
%惑铰 棺 窍碍
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 眠啊
x_obs_pos = [];
y_obs_pos = [];
x_obs_location = [];
y_obs_location = [];
obs_pos = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 厘局拱 困摹 颇厩
x_obs = [];
y_obs = [];
count_obs = 0;
% 流急狼 规沥侥 : y = ax + b
res = polyfit(xpoint, ypoint, 1);
x_equation = xpoint(1) : (x_s(2)-x_s(1)) : xpoint(2);
y_equation = res(1)*x_equation + res(2);
% 惑铰 棺 窍碍困茄 臭捞蔼 眠免 函荐
z_lift_height = [];
% flight height甫 馆康茄 臭捞 沥焊
lift_height = [];
y_index = [];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 流急 版肺俊 乐绰 痢甸吝 老摹窍绰 y 瘤痢 茫扁
for i = 1 : length(y_equation)
flag = 0;
for j = 2 : N_s-1
num = find( y_equation(i) == y_s(j) );
if(num)
y_equation(i) = y_s(j);
flag = 1;
break;
else
flag = 0;
end
end
if(flag == 0)
y_equation(i) = y_s(knnsearch(y_s', y_equation(i)));
end
y_index(i) = knnsearch(y_s', y_equation(i));
end
%x_equation
%y_equation
% 流急 版肺俊 乐绰 痢甸吝 老摹窍绰 z 瘤痢 茫扁
for k = 1 : length(y_equation)
for i = 1 : N_s
% x客 悼老茄 瘤痢 2d俊辑 茫扁
if(find( x_equation(k) == x_s(i) ))
z_lift_height(k) = z_2d_s( (y_index(k)-1)*N_s + i );
if(z_lift_height(k) < flight_height)
lift_heigth(k) = flight_height;
z_lift_height(k) = 300 + z_lift_height(k);
else
% 厘局拱 困摹 颇厩
count_obs = count_obs + 1;
x_obs(count_obs) = x_equation(k);
y_obs(count_obs) = y_equation(k);
lift_heigth(k) = z_lift_height(k);
end
end
end
end
%z_lift_height
lift_length_part = [];
%lift_heigth
% for i = 1 : length(y_equation)-1
% lift_length_part(i) = sqrt( (x_equation(i+1)-x_equation(i))^2 + (y_equation(i+1)-y_equation(i))^2 + ( (lift_heigth(i+1)-lift_heigth(i))/1000 )^2 );
%
% lift_length = lift_length + sqrt( (x_equation(i+1)-x_equation(i))^2 + (y_equation(i+1)-y_equation(i))^2 + ( (lift_heigth(i+1)-lift_heigth(i))/1000 )^2 );
%
% end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
z_alpha = 10;
% if(z_lift_height < flight_height)
% z_lift_height = z_lift_height + flight_height;
%
% end
% 惑铰 棺 窍碍 角氰
figure(3)
% making 3D MAP
meshc(x_2d_s, y_2d_s, z_2d_s);
hold on;
% plot3(x_equation, y_equation, z_lift_height + z_alpha, 'black', 'LineWidth',4);
plot3(x_equation, y_equation, lift_heigth + z_alpha, 'red', 'LineWidth',4);
for i = 1 : length(x_equation)
x_obs_pos = find(x_equation(i) == xn_new);
y_obs_pos = find(y_equation(i) == yn_new);
end
for i = 1 : length(x_obs_pos)
obs_pos = find(x_obs_pos(i) == y_obs_pos);
%obs_pos
if(obs_pos)
x_obs_location(i) = xn_new(x_obs_pos(obs_pos));
y_obs_location(i) = yn_new(y_obs_pos(obs_pos));
end
end
% 咯扁 鳖瘤
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
vx_all = vx_new(:);
vy_all = vy_new(:);
dr = kron(ones(length(vx_all),1),xy_start)-[vx_all vy_all];
[min_val,min_id] = min(sum(dr.^2,2));
vx_new = [vx_new [xy_start(1); vx_all(min_id)]];
vy_new = [vy_new [xy_start(2); vy_all(min_id)]];
dr = kron(ones(length(vx_all),1),xy_dest)-[vx_all vy_all];
[min_val,min_id] = min(sum(dr.^2,2));
vx_new = [vx_new [xy_dest(1); vx_all(min_id)]];
vy_new = [vy_new [xy_dest(2); vy_all(min_id)]];
xy_all = unique([vx_new(:) vy_new(:)],'rows');
dv = [vx_new(1,:); vy_new(1,:)] - [vx_new(2,:); vy_new(2,:)];
edge_dist = sqrt(sum(dv.^2));
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
% % 流急 版肺 单捞磐
% ttemp_xy_path_all{mainloof} = xy_opt_path;
%
% rootCount = 0;
%
% % 傈眉 版肺 辨捞 备埃喊 拌魂
% path1 = 0;
%
% for i = 1 : length(xy_opt_path)-1
%
% path_part = sqrt((xy_opt_path(i,1) - xy_opt_path(i+1,1))^2 + (xy_opt_path(i,2) - xy_opt_path(i+1,2))^2);
% path1 = path_part + path1;
% %part_length(i) = path_part;
%
% end
%
% path_length(mainloof) = path1;
% %part_length_all{mainloof} = part_length;
%
% % path_length_all = path_length(mainloof) + path_length_all;
% path_length_all = path1 + path_length_all;
figure(1);
plot(xy_start(1),xy_start(2),'S',xy_dest(1),xy_dest(2),'D','MarkerSize',10, 'MarkerFaceColor','black');
% plot(xy_opt_path(:,1),xy_opt_path(:,2), 'green', 'LineWidth',4);
% for i = 1 : length(xy_opt_path)
% if trues(i) == 0
% xx = [xy_opt_path(i,1) xy_opt_path(i+1,1)];
% yy = [xy_opt_path(i,2) xy_opt_path(i+1,2)];
% plot(xx,yy, 'r-','LineWidth',3);
% end
% end
%唱吝俊 秦搬
figure(2)
% making 3D MAP
meshc(x_2d_s, y_2d_s, z_2d_s);
hold on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% z 绵狼 单捞磐 档免
zd = [];
z_data = [];
for i = 1 : length(xd)
pos_x = 10000;
pos_y = 10000;
x_data = 0;
y_data = 0;
% 啊厘 啊鳖款 困摹肺 瘤沥
for j = 1 : length(x_s)
x_length = abs( x_s(j) - xd(i) );
y_length = abs( y_s(j) - yd(i) );
if( pos_x > x_length )
pos_x = x_length;
x_data = x_s(j);
else
pos_x = pos_x;
x_data = x_data;
end
if( pos_y > y_length )
pos_y = y_length;
y_data = y_s(j);
else
pos_y = pos_y;
y_data = y_data;
end
end
% 秦寸 困摹狼 x谅钎狼 蔼苞 y谅钎狼 蔼阑 茫酒 历厘
temp_pos_x = find( x_data == x_2d_s );
temp_pos_y = find( y_data == y_2d_s );
temp_pos_z = 0;
for k = 1 : length(temp_pos_x)
temp = find( temp_pos_x(k) == temp_pos_y );
if( temp )
temp_pos_z = temp;
z_data(i) = z_2d_s(temp_pos_z);
% 厚青绊档 力茄 拌魂
if( z_data(i) < flight_height )
z_data(i) = flight_height + height;
end
else
temp_pos_z = temp_pos_z;
end
end
end
%z_data
% 急雀 流急 版肺 单捞磐
ttemp_xy_path_all{mainloof} = xy_opt_path;
rootCount = 0;
% 傈眉 版肺 辨捞 备埃喊 拌魂
path1 = 0;
for i = 1 : length(xy_opt_path)-1
path_part = sqrt((xy_opt_path(i,1) - xy_opt_path(i+1,1))^2 + (xy_opt_path(i,2) - xy_opt_path(i+1,2))^2 + ( (z_data(i+1)-z_data(i)) /1000)^2);
path1 = path_part + path1;
part_length(i) = path_part;
end
path_length(mainloof) = path1;
part_length_all{mainloof} = part_length;
% path_length_all = path_length(mainloof) + path_length_all;
path_length_all = path1 + path_length_all;
% using plot3 function ex. plot3(x1, y1, z1, ...)
plot3(xy_opt_path(:,1),xy_opt_path(:,2), z_data, 'red', 'LineWidth',4);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
disp('傈眉 急雀 版肺 辨捞');
path_length_all
3 仿真结果
4 参考文献
[1]符小卫, and 高晓光. "一种无人机路径规划算法研究." 系统仿真学报 01(2004):20-21.
5 MATLAB代码与数据下载地址
见博客主页
