✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
动态窗口法(DWA)将局部路径规划问题描述为速度矢量空间上的约束优化问题，是一种有效的局部避障算法[21]。DWA算法的主要思想是，根据机器人速度、加速度以及环境障碍约束，将速度矢量空间(v，ω)限制在一个动态窗口中，对此窗口内的线速度v和角速度ω进行多组采样；然后，预测每个采样速度在下一个周期内对应的机器人运动轨迹；最后，引入一个评价函数对预测的运动轨迹进行评估，选择得分最高的轨迹对应的速度来控制机器人运动。DWA算法的核心在于动态窗口的建立以及评价函数的选择，下面根据清扫机器人自身性能和环境的约束建立速度采样动态窗口。
清扫机器人受硬件性能的约束，其最大、最小速度都被限制在一定范围内，所以清扫机器人允许的极限速度矢量集合Vs为:
式中，vmax、vmin为清扫机器人的最大、最小线速度，ωmax、ωmin为清扫机器人的最大、最小角速度。为了保护自身的安全，清扫机器人要与障碍物保持一定距离，并且在碰到障碍物前能够及时停止，因此，清扫机器人的安全速度矢量集合Vd可根据下式确定:
式中，dist(v，ω)为采样速度(v，ω)对应轨迹与障碍物的最小距离，v̇a、ω̇a分别为清扫机器人减速时的最大线加速度、最大角加速度。
根据清扫机器人的最大加减速能力，可以计算出某时刻采样速度在下一个周期Δt内的变化范围，用Va表示:
式中，(vc，ωc)为当前时刻采样速度，v̇b、ω̇b分别为清扫机器人的最大线加速度、最大角加速度，Δt为采样周期。将满足Vs、Va、Vd的速度矢量空间构建成动态窗口，可得到最终速度搜索空间Vr:
对于采样的速度空间Vr，每一组采样速度对应的预测轨迹都满足约束条件，需引入评价函数评估轨迹的优劣性。最优轨迹应使清扫机器人在避开障碍物的情况下更快更准地向目标移动，因此本文采用的评价函数如下:
式中，heading(v，ω)为清扫机器人轨迹终点的航向与目标点的夹角，dist(v，ω)为轨迹与障碍物的最小距离，vel(v，ω)为采样速度，α、β、γ为权重比。
⛄ 部分代码
close all;
clear all;
clc;
disp('Formation program start!!')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%init%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
num=4;%参与编队的无人机初始数量
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];% 目标点位置 [x(m),y(m)]
temp_goal=goal;
goal_series=[1;2;3;4];
obstacleR=0.5;% 冲突判定用的障碍物半径
global dt; dt=0.1;% 时间[s]
% 机器人运动学模型
% 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],
% 速度分辨率[m/s],转速分辨率[rad/s]]
Kinematic=[1.0,toRadian(120.0),0.4,toRadian(60.0),0.01,toRadian(1)];
% 评价函数参数 [heading,dist,velocity,predictDT]
evalParam=[0.1,0.2,0.2,3.0 ];
area=[-1 12 -1 12];% 模拟区域范围 [xmin xmax ymin ymax]
mirror_dis=zeros(4,4);
% 模拟实验的结果
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]';% 机器人的初期状态[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]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:5000
% DWA参数输入
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
% 模拟结果的保存
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); %// 关闭视频文件句柄
close all;
⛄ 运行结果
⛄ 参考文献
[1]孙航. 无人机编队,路径规划目标点交换方法,系统,介质及终端:, CN112578812A[P]. 2021.
[2]袁塘, 李廷元. 融合A*算法与动态窗口法的无人机路径规划研究[J]. 现代计算机：下半月版, 2022(028-001).
