【無人機】基於matlab多AUV目標搜素與圍捕【含Matlab原始碼 1205期】
阿新 • • 發佈：2021-08-11
一、簡介
基於matlab多AUV目標搜素與圍捕
二、部分原始碼
%name:AUV targets searching and avoid obstacles clc; clf; clear all; global obstacle; obstacle=[]; global fls; global auvpos; fls=cell(1,1); auvpos=cell(1,1); insight_point=cell(1,1); MaximumRange = 150; % 最大探測範圍 Vl=zeros(2000,2000);%環境代價矩陣 Time_AllSteps = 50; %額定步數 Y = zeros(Time_AllSteps,6); %狀態變數 global subregion; subregion=[]; sub_obs_coor=[300,900;500,1300;500,1500;700,1300;700,1100;900,700;900,500; 1100,900;1100,700;1100,500;1300,1500;1300,1300;1300,700; 1300,900;1300,500;1500,1300;1500,700;1500,500;1500,1500];%障礙物佔據的子區域中心點座標 n1=1; n2=1; sub_area=[]; pp=0; sub_pp_number=0; for i=1:5 for j=1:5 p_obs=0; for n=1:2 for p=1:2 pp=0; for m=1:size(sub_obs_coor,1) if((200+(i-1)*400+(-1)^n*100~=sub_obs_coor(m,1))||(200+(j-1)*400+(-1)^p*100~=sub_obs_coor(m,2))) p_obs=1; else pp=1; break; end end if(pp==0) sub_pp_number=sub_pp_number+1; sub_area=[sub_area,200+(i-1)*400+(-1)^n*100,200+(j-1)*400+(-1)^p*100,0]; end end end if(p_obs~=0) mm=size(sub_area,2); if(mm>0) subregion(n1,1:5+mm) = [n1,200+(i-1)*400,200+(j-1)*400,0,0,sub_area]; subregion(n1,18)=sub_pp_number; sub_pp_number=0; sub_area=[]; n1=1+n1; end end end end global AUV_status; AUV_status=cell(1,1); AUV_Angle(:,1)=[0,0,0,0,0,0,0,0,0,0];%AUV的初始艏向角 auv_num(:,1)=[2,2,2,2,2,2,2,2,2,2]; global num_target; global number_target; global pos_target; global pos_tar1; global sub_auv; global sublock_area; global auv_hunt_flag; global tracking_tar; global auv_collaborators; auv_collaborators=zeros(2,3); [num_target,pos_target,Obstacles]=Make_obstacles4();%障礙物邊界 pos_tar1=[pos_target',zeros(size(pos_target,2),1)]; number_target=num_target; button = 0; mouse_click=0; a_n = 1; while button ~= 3 [x,y,button]=ginput(1); %十字游標 pin= ~isempty(x); if (pin==1 && button ~= 3) mouse_click=mouse_click+1; plot(x,y,'mo') AUV_status{1}{a_n}(:,1)=[20,0,0,x,y,AUV_Angle(a_n,1)]; a_n = a_n+1; end end hold off; AUV_num=mouse_click; AUV_AngTemp=AUV_Angle(1:mouse_click,1); %******視線導引的切換點的半徑********* R=8; n=ones(AUV_num,1); angle_v=zeros(AUV_num,1); flag_obsfind=zeros(AUV_num,1); line_ok=zeros(AUV_num,1); sub_flag = zeros(AUV_num,1); sub_ok = zeros(AUV_num,1); task_time = zeros(AUV_num,1); sub_counter = 0; flag_a=ones(AUV_num,1); excute_task=zeros(AUV_num,1); to_sub=zeros(AUV_num,1); task_do=zeros(AUV_num,1); tracking_tar=zeros(AUV_num,2); task_continue=ones(AUV_num,1); lock_area=zeros(AUV_num,4); auv_task_start=zeros(AUV_num,2); auv_hunt_flag=zeros(AUV_num,1); allsub_task_over=0; end_flag=0; stop_sub=zeros(AUV_num,2); taskovercomingflag=0; change_flag=0; dy_search_over=0; flag_changesub=zeros(AUV_num,3); sub_auv=zeros(AUV_num,1); sublock_area=zeros(AUV_num,3); obs_auv_PosAngleDis=cell(1,1); sub_n=8; sub2overflag=1; line_color=[0,0,1;0,1,0;1,0,0;1,1,0;0,1,1;1,0,1;1,1,1]; global pid_err; global pid_errobs; global dynamic_mess; global Dynamic_pos; global hunt_time; global dy_n; global auv_group; global vanish_flag; vanish_flag=zeros(2,3); auv_group=zeros(6,4); dy_n=ones(AUV_num,1); Dynamic_pos=zeros(2,3); dy_tar_auv_num=zeros(2,1); hunt_time=zeros(2,1); pid_err=cell(1,3); pid_errobs=cell(1,1); dynamic_mess=cell(1,1); dy_tar_num=0; for i=1:AUV_num pid_err{1}{i}(1:3,1)=0; pid_errobs{1}{i}(1:3,1)=0; obs_auv_PosAngleDis{1}{i}=[]; AUV_PosTemp(:,i)=AUV_status{1}{i}(4:5,1);%AUV位置臨時變數 end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %******視線導引控制PID引數 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global K_p1; global K_i1; global K_d1; % K_p1 =1.0; K_i1 = 0.525; K_d1 =0; %艏向控制 K_p1 =0.35; K_i1 = 0.85; K_d1 =0; %艏向控制 global K_p1obs; global K_i1obs; global K_d1obs; % K_p1obs =0.5; K_i1obs = 3.5; K_d1obs =0; K_p1obs =0.1; K_i1obs = 0.225; K_d1obs =0;%1 0.625 %%%%%%%%%%%%%%%%%%% %隨機動目標位置生成 x_rt=800;%隨機目標位置初始化 y_rt=1600; global x_sin; global x_change_flag; global tar0_Angle; x_sin=400; x_change_flag=0; tar0_Angle=0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Env_handle=environment_buliding4(pos_target);%環境構建 DrawAUV(AUV_PosTemp,AUV_Angle,AUV_num); for step=1:1:4000%最大執行步數 % step=1; % while(1) t_obs = 0.1; T1(step,1)=0.1*step; [OBS_point,flag_obs]=FLS_obsmeasure(Obstacles,AUV_PosTemp,AUV_AngTemp); [Dynamic_pos(:,1:2),dynamic_pos,dynamic_angle]=RandTarget(x_rt,y_rt); for i=1:length(AUV_AngTemp) [search_tar_over]=target_search(AUV_PosTemp(:,i),AUV_AngTemp(i,1)); if(dy_tar_num<2) [finddynamic_ok,angle_v(i,1),vanish,dy_tar_n,dynamic_tar_pos,find_tar_auv,hunt_group_done]=DynamicTargetSearchFun(AUV_PosTemp,AUV_AngTemp(i,1),Dynamic_pos,i); end if(search_tar_over==1)%靜態目標和動態目標搜尋完畢 if(dy_tar_num==2) break; else dy_search_over=1; subregion(:,4:5)=0; end end if(flag_obs(i,1)==1&&line_ok(i,1)==0) %上一個時間AUV完成了導引避障 flag_a(i,1)=0; finddynamic_ok=0; % pid_err{1}{i}(:,1)=0; flag_obsfind(i,1)=1;%第i個AUV遇到障礙物 AUV_status{1}{i}(1,auv_num(i,1)-1)=20;%有障礙減速 insight_point{1}{i}(:,1)=AUV_status{1}{i}(4:5,auv_num(i,1)-1);%視線導引起點為當前AUV的位置 insight_point{1}{i}(:,2)= OBS_point(i,:)'; %終點為推算的避障點 obs_auv_PosAngleDis{1}{i}(n(i,1),:)=OBS_point(i,:); n(i,1)= n(i,1)+1; else if((flag_obs(i,1)==1&&line_ok(i,1)==1)) %當前時刻檢測到障礙物但是上一時刻未完成導引避障 flag_a(i,1)=0; finddynamic_ok=0; flag_obsfind(i,1)=1;%第i個AUV遇到障礙物 % AUV_status{1}{i}(1,auv_num(i,1)-1)=20;%有障礙減速 else if(flag_obs(i,1)==0&&line_ok(i,1)==1)%當前時刻未檢測到障礙物但是上一時刻未完成導引避障 flag_a(i,1) =0; finddynamic_ok=0; flag_obsfind(i,1)=1; else%當前時刻AUV未檢測到障礙物，上一時刻的導引已經完成 flag_a(i,1) =1; % pid_err{1}{i}(:,1)=0; flag_obsfind(i,1)=0;%第i個AUV未遇到障礙物 % angle_v(i,1)=0; %不需要避障的，角速度為0 AUV_status{1}{i}(1,auv_num(i,1)-1)=40;%無障礙加速 end end end if(flag_obsfind(i,1)==1)%判斷第i個AUV需要視線導引避障 line_ok(i,1)=1; [r,error,dt,emix]=insight_line(AUV_status{1}{i}(:,auv_num(i,1)-1),insight_point{1}{i}(:,1),insight_point{1}{i}(:,2),i);%直線視線導引控制避障 if(dt<=R)%AUV滿足線段切換條件 flag_a(i,1)=1; flag_obsfind(i,1)=0;%重啟聲吶檢測障礙物 angle_v(i,1)=0; pid_err{1}{i}(:,1)=0; line_ok(i,1)=0; else angle_v(i,1)=r; end end % %%%%%%%%%%AUV走向分配的子區域過程中或執行子區域任務與避障的臨時切換機制%%%%%%%% % %如果在AUV走向分配的子區域過程中或執行子區域任務時需要避障， % %那麼進行避障，結束後繼續執行被中斷的過程 % %而不是再重新分配子區域，因為被中斷的過程還沒有完成 % %%%%%%%%%%%%%%%%%%%切換機制程式碼區%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if(flag_a(i,1)==0) if(task_do(i,1)==1) excute_task(i,1)=1;%在執行子區域任務時被避障事件中斷 else if(sub_ok(i,1)==0) to_sub(i,1)=1;%在走向分配的子區域是被避障事件中斷 end end % if(tracking_tar(i,1)==1)%目標圍捕過程中發生避障事件 % excute_task(i,1)=0; % to_sub(i,1)=0;%前期執行的走向子區域和子區域搜尋都作廢，先避障， % %如果動態目標不在視域內，完事再重新進行子區域分配;如果在視域內繼續進行目標圍捕 % end end if(finddynamic_ok==1)%發現動態目標，進行跟蹤 tracking_tar(i,1)=1;%當前AUV處於動目標圍捕過程 tracking_tar(i,2)=dy_tar_n;%當前AUV圍捕的目標編號 flag_a(i,1)=0;%進行動目標圍捕不執行子區域分配 if(find_tar_auv~=0) dy_tar_auv_num(dy_tar_n,1)=find_tar_auv; organizing_auv( AUV_PosTemp,find_tar_auv,dy_tar_n);%一個AUV發現目標後確定隊形的其他AUV編號，組成圍捕隊形 tracking_tar(auv_collaborators(dy_tar_n,2),1)=1;%當前AUV處於動目標圍捕過程 tracking_tar(auv_collaborators(dy_tar_n,2),2)=dy_tar_n;%當前AUV圍捕的目標編號 flag_a(auv_collaborators(dy_tar_n,2),1)=0; tracking_tar(auv_collaborators(dy_tar_n,3),1)=1;%當前AUV處於動目標圍捕過程 tracking_tar(auv_collaborators(dy_tar_n,3),2)=dy_tar_n;%當前AUV圍捕的目標編號 flag_a(auv_collaborators(dy_tar_n,3),1)=0; sub_auv(find_tar_auv)=0;%進行目標圍捕過程中，以前的子區域鎖定沒到達的取消，取消AUV鎖定子區域的標誌 sub_auv(auv_collaborators(dy_tar_n,2))=0; sub_auv(auv_collaborators(dy_tar_n,3))=0; if(lock_area(find_tar_auv,1)~=0) if(subregion(lock_area(find_tar_auv,1),lock_area(find_tar_auv,2))==0.5) subregion(lock_area(find_tar_auv,1),lock_area(find_tar_auv,2))=0; end end if(lock_area(auv_collaborators(dy_tar_n,2),1)) if(subregion(lock_area(auv_collaborators(dy_tar_n,2),1),lock_area(auv_collaborators(dy_tar_n,2),2))==0.5) subregion(lock_area(auv_collaborators(dy_tar_n,2),1),lock_area(auv_collaborators(dy_tar_n,2),2))=0; end end if(lock_area(auv_collaborators(dy_tar_n,3),1)) if(subregion(lock_area(auv_collaborators(dy_tar_n,3),1),lock_area(auv_collaborators(dy_tar_n,3),2))==0.5) subregion(lock_area(auv_collaborators(dy_tar_n,3),1),lock_area(auv_collaborators(dy_tar_n,3),2))=0; end end end end
三、執行結果
四、備註
版本：2014a