【無人機編隊】基于動態(tài)窗口法實現(xiàn)的無人機編隊目標(biāo)分配及路徑規(guī)劃問題研究附matlab代
?作者簡介：熱愛科研的Matlab仿真開發(fā)者，修心和技術(shù)同步精進，matlab項目合作可私信。
??個人主頁：Matlab科研工作室
??個人信條：格物致知。
更多Matlab仿真內(nèi)容點擊??
智能優(yōu)化算法 ?神經(jīng)網(wǎng)絡(luò)預(yù)測 雷達通信 無線傳感器
信號處理 圖像處理 路徑規(guī)劃 元胞自動機 無人機
? 內(nèi)容介紹
動態(tài)窗口法(DWA)將局部路徑規(guī)劃問題描述為速度矢量空間上的約束優(yōu)化問題，是一種有效的局部避障算法[21]。DWA算法的主要思想是，根據(jù)機器人速度、加速度以及環(huán)境障礙約束，將速度矢量空間(v，ω)限制在一個動態(tài)窗口中，對此窗口內(nèi)的線速度v和角速度ω進行多組采樣；然后，預(yù)測每個采樣速度在下一個周期內(nèi)對應(yīng)的機器人運動軌跡；最后，引入一個評價函數(shù)對預(yù)測的運動軌跡進行評估，選擇得分最高的軌跡對應(yīng)的速度來控制機器人運動。DWA算法的核心在于動態(tài)窗口的建立以及評價函數(shù)的選擇，下面根據(jù)清掃機器人自身性能和環(huán)境的約束建立速度采樣動態(tài)窗口。
清掃機器人受硬件性能的約束，其最大、最小速度都被限制在一定范圍內(nèi)，所以清掃機器人允許的極限速度矢量集合Vs為:
式中，vmax、vmin為清掃機器人的最大、最小線速度，ωmax、ωmin為清掃機器人的最大、最小角速度。為了保護自身的安全，清掃機器人要與障礙物保持一定距離，并且在碰到障礙物前能夠及時停止，因此，清掃機器人的安全速度矢量集合Vd可根據(jù)下式確定:
式中，dist(v，ω)為采樣速度(v，ω)對應(yīng)軌跡與障礙物的最小距離，v?a、ω?a分別為清掃機器人減速時的最大線加速度、最大角加速度。
根據(jù)清掃機器人的最大加減速能力，可以計算出某時刻采樣速度在下一個周期Δt內(nèi)的變化范圍，用Va表示:
式中，(vc，ωc)為當(dāng)前時刻采樣速度，v?b、ω?b分別為清掃機器人的最大線加速度、最大角加速度，Δt為采樣周期。將滿足Vs、Va、Vd的速度矢量空間構(gòu)建成動態(tài)窗口，可得到最終速度搜索空間Vr:
對于采樣的速度空間Vr，每一組采樣速度對應(yīng)的預(yù)測軌跡都滿足約束條件，需引入評價函數(shù)評估軌跡的優(yōu)劣性。最優(yōu)軌跡應(yīng)使清掃機器人在避開障礙物的情況下更快更準(zhǔn)地向目標(biāo)移動，因此本文采用的評價函數(shù)如下:
式中，heading(v，ω)為清掃機器人軌跡終點的航向與目標(biāo)點的夾角，dist(v，ω)為軌跡與障礙物的最小距離，vel(v，ω)為采樣速度，α、β、γ為權(quán)重比。
? 部分代碼
close all;
clear all;
clc;
disp('Formation program start!!')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%init%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
num=4;%參與編隊的無人機初始數(shù)量
target_num=4;
% quad_init_x=12*rand(num,1);%初始位置生成
% quad_init_y=12*rand(num,1);
%%%%%%%%%%%%%%%%%%%%%%%%quad_init_for_test%%%%%%%%%%%%%%%%%%%%%%%%%
quad_init_x=[9.940364; 10.902533; 8.463561; 2.919964];%[8.971954; 7.746414; 1.478634; 6.052774];%[8.971954; 7.746414; 1.478634; 6.052774];%[6.184407; 7.890367; 11.410982; 8.668182];%
quad_init_y=[7.070779; 11.266800; 10.746024; 7.274714];%[4.167136; 1.105772; 1.774194; 2.378036];%[4.167136; 1.105772; 1.774194; 2.378036];%[4.800957; 9.982456; 1.612060; 0.725601];%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
goal=[3 3;?
? ? ? 3 8;
? ? ? 8 3;
? ? ? 8 8];% 目標(biāo)點位置 [x(m),y(m)]
temp_goal=goal;
goal_series=[1;2;3;4];
obstacleR=0.5;% 沖突判定用的障礙物半徑
global dt; dt=0.1;% 時間[s]
% 機器人運動學(xué)模型
% 最高速度m/s],最高旋轉(zhuǎn)速度[rad/s],加速度[m/ss],旋轉(zhuǎn)加速度[rad/ss],
% 速度分辨率[m/s],轉(zhuǎn)速分辨率[rad/s]]
Kinematic=[1.0,toRadian(120.0),0.4,toRadian(60.0),0.01,toRadian(1)];
% 評價函數(shù)參數(shù) [heading,dist,velocity,predictDT]
evalParam=[0.1,0.2,0.2,3.0 ];
area=[-1 12 -1 12];% 模擬區(qū)域范圍 [xmin xmax ymin ymax]
mirror_dis=zeros(4,4);
% 模擬實驗的結(jié)果
result.quad1=[];
result.quad2=[];
result.quad3=[];
result.quad4=[];
traj1=[];
traj2=[];
traj3=[];
traj4=[];
tic;
% Main loop
% writerObj=VideoWriter('formation.avi');? % 定義一個視頻文件用來存動畫??
% open(writerObj);? ? ? ? ? ? ? ? ? ? % 打開該視頻文件??
%Target Allocation
[goal]=Target_Allocation(goal,quad_init_x,quad_init_y,num,target_num,goal_series,temp_goal,mirror_dis);
x=[quad_init_x(1,1) quad_init_y(1,1) atan2((goal(1,2)-quad_init_y(1,1)),(goal(1,1)-quad_init_x(1,1))) 0 0;
? ?quad_init_x(2,1) quad_init_y(2,1) atan2((goal(2,2)-quad_init_y(2,1)),(goal(2,1)-quad_init_x(2,1))) 0 0;
? ?quad_init_x(3,1) quad_init_y(3,1) atan2((goal(3,2)-quad_init_y(3,1)),(goal(3,1)-quad_init_x(3,1))) 0 0;
? ?quad_init_x(4,1) quad_init_y(4,1) atan2((goal(4,2)-quad_init_y(4,1)),(goal(4,1)-quad_init_x(4,1))) 0 0]';% 機器人的初期狀態(tài)[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
% x=[quad_init_x(1,1) quad_init_y(1,1) pi/2 0 0;
%? ? quad_init_x(2,1) quad_init_y(2,1) pi/2 0 0;
%? ? quad_init_x(3,1) quad_init_y(3,1) pi/2 0 0;
%? ? quad_init_x(4,1) quad_init_y(4,1) pi/2 0 0]';% 機器人的初期狀態(tài)[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:5000
? ? % DWA參數(shù)輸入
? ? obstacle1=[x(1,2) x(2,2) x(3,2) x(4,2);x(1,3) x(2,3) x(3,3) x(4,3);x(1,4) x(2,4) x(3,4) x(4,4)];
? ? obstacle2=[x(1,1) x(2,1) x(3,1) x(4,1);x(1,3) x(2,3) x(3,3) x(4,3);x(1,4) x(2,4) x(3,4) x(4,4)];
? ? obstacle3=[x(1,1) x(2,1) x(3,1) x(4,1);x(1,2) x(2,2) x(3,2) x(4,2);x(1,4) x(2,4) x(3,4) x(4,4)];
? ? obstacle4=[x(1,1) x(2,1) x(3,1) x(4,1);x(1,2) x(2,2) x(3,2) x(4,2);x(1,3) x(2,3) x(3,3) x(4,3)];
%? ? ?obstacle4=[8 8;x(1,2) x(2,2);x(1,3) x(2,3)];
% [Pt b]=FindIntersection(x(1,3),x(2,3),goal(3,1),goal(3,2),x(1,4),x(2,4),goal(4,1),goal(4,2));
? ? if norm(x(1:2,1)-goal(1,:)')>0.1
? ? ? ? [x(:,1),traj1]=distributed_planning(x(:,1),Kinematic,goal(1,:),evalParam,obstacle1,obstacleR);
? ? end
? ? if norm(x(1:2,2)-goal(2,:)')>0.1
? ? ? ? [x(:,2),traj2]=distributed_planning(x(:,2),Kinematic,goal(2,:),evalParam,obstacle2,obstacleR);
? ? end
? ? if norm(x(1:2,3)-goal(3,:)')>0.1
? ? ? ? [x(:,3),traj3]=distributed_planning(x(:,3),Kinematic,goal(3,:),evalParam,obstacle3,obstacleR);
? ? end
? ? if norm(x(1:2,4)-goal(4,:)')>0.1
? ? ? ? [x(:,4),traj4]=distributed_planning(x(:,4),Kinematic,goal(4,:),evalParam,obstacle4,obstacleR);
? ? end
? ??
? ? % 模擬結(jié)果的保存
? ? result.quad1=[result.quad1; x(:,1)'];
? ? result.quad2=[result.quad2; x(:,2)'];
? ? result.quad3=[result.quad3; x(:,3)'];
? ? result.quad4=[result.quad4; x(:,4)'];
? ? % 是否到達目的地
? ? ? ? if norm(x(1:2,1)-goal(1,:)')<0.1 && norm(x(1:2,2)-goal(2,:)')<0.1 && norm(x(1:2,3)-goal(3,:)')<0.1 && norm(x(1:2,4)-goal(4,:)')<0.1
? ? ? ? ? ? disp('Arrive Goal!!!');break;
? ? ? ? end
? ? ? ? if norm(x(1:2,1)-x(1:2,2))<obstacleR || norm(x(1:2,1)-x(1:2,3))<obstacleR || norm(x(1:2,1)-x(1:2,4))<obstacleR || norm(x(1:2,2)-x(1:2,3))<obstacleR || norm(x(1:2,2)-x(1:2,4))<obstacleR || norm(x(1:2,3)-x(1:2,4))<obstacleR
? ? ? ? ? ?disp('Error!!!');break;
? ? ? ? end
? ??
? ? %====Animation====
? ? hold off;
? ? ArrowLength=0.5;%?
? ? % 機器人?
? ? plot(result.quad1(:,1),result.quad1(:,2),'-b');hold on;
? ? plot(result.quad2(:,1),result.quad2(:,2),'-m');hold on;
? ? plot(result.quad3(:,1),result.quad3(:,2),'-r');hold on;
? ? plot(result.quad4(:,1),result.quad4(:,2),'-c');hold on;? ??
? ? plot(goal(1,1),goal(1,2),'*b');hold on;
? ? plot(goal(2,1),goal(2,2),'*m');hold on;
? ? plot(goal(3,1),goal(3,2),'*r');hold on;
? ? plot(goal(4,1),goal(4,2),'*c');hold on;
? ??
% [Pt b]=FindIntersection(x(1,1),x(2,1),goal(1,1),goal(1,2),x(1,3),x(2,3),goal(3,1),goal(3,2))
% str=['***1 ' num2str(toDegree(x(3,1)))];
% disp(str);
% str=['***3 ' num2str(toDegree(x(3,3)))];
% disp(str);
? ? % 探索軌跡
? ? if ~isempty(traj1)
? ? ? ? for it=1:length(traj1(:,1))/5
? ? ? ? ? ? ind=1+(it-1)*5;
? ? ? ? ? ? plot(traj1(ind,:),traj1(ind+1,:),'-g');hold on;
? ? ? ? end
? ? end
? ??
? ? if ~isempty(traj2)
? ? ? ? for it=1:length(traj2(:,1))/5
? ? ? ? ? ? ind=1+(it-1)*5;
? ? ? ? ? ? plot(traj2(ind,:),traj2(ind+1,:),'-g');hold on;
? ? ? ? end
? ? end
? ? ? ??
? ? if ~isempty(traj3)
? ? ? ? for it=1:length(traj3(:,1))/5
? ? ? ? ? ? ind=1+(it-1)*5;
? ? ? ? ? ? plot(traj3(ind,:),traj3(ind+1,:),'-g');hold on;
? ? ? ? end
? ? end
? ? ? ??
? ? if ~isempty(traj4)
? ? ? ? for it=1:length(traj4(:,1))/5
? ? ? ? ? ? ind=1+(it-1)*5;
? ? ? ? ? ? plot(traj4(ind,:),traj4(ind+1,:),'-g');hold on;
? ? ? ? end
? ? end
%? ? ?DrawQuadrotor(x(1,1),x(2,1));
? ? quiver(x(1,1),x(2,1),ArrowLength*cos(x(3,1)),ArrowLength*sin(x(3,1)),'ok');hold on;
%? ? ?DrawQuadrotor(x(1,2),x(2,2));
? ? quiver(x(1,2),x(2,2),ArrowLength*cos(x(3,2)),ArrowLength*sin(x(3,2)),'ok');hold on;
%? ? ?DrawQuadrotor(x(1,3),x(2,3));
? ? quiver(x(1,3),x(2,3),ArrowLength*cos(x(3,3)),ArrowLength*sin(x(3,3)),'ok');hold on;
%? ? ?DrawQuadrotor(x(1,4),x(2,4));
? ? quiver(x(1,4),x(2,4),ArrowLength*cos(x(3,4)),ArrowLength*sin(x(3,4)),'ok');hold on;? ??
? ? axis(area);
? ? grid on;
? ? drawnow;
%? ? ?frame = getframe;? ? ? ? ? ? %// 把圖像存入視頻文件中??
%? ? ?writeVideo(writerObj,frame); %// 將幀寫入視頻??
end
toc
% close(writerObj); %// 關(guān)閉視頻文件句柄??
close all;
? 運行結(jié)果
? 參考文獻
[1]孫航. 無人機編隊,路徑規(guī)劃目標(biāo)點交換方法,系統(tǒng),介質(zhì)及終端:, CN112578812A[P]. 2021.
[2]袁塘, 李廷元. 融合A*算法與動態(tài)窗口法的無人機路徑規(guī)劃研究[J]. 現(xiàn)代計算機：下半月版, 2022(028-001).
