特色栏目： python 批处理 net编程 Javascript Php Asp Css Html5 Android seo centos

【无人机协同】基于卡尔曼滤波和PID控制实现多无人机目标搜索与围捕含Matla

来源：互联网收集：自由互联发布时间：2022-06-18

1 简介四旋翼无人机目前被广泛应用于民用或是军事领域.由于其灵活小巧且轻盈的优点,能代替人类探索未知地域或执行各种危险任务,如地形勘察,探取情报,目标跟踪等,而精确可靠的目

1 简介

四旋翼无人机目前被广泛应用于民用或是军事领域.由于其灵活小巧且轻盈的优点,能代替人类探索未知地域或执行各种危险任务,如地形勘察,探取情报,目标跟踪等,而精确可靠的目标跟踪算法更是在无人机领域获得了广泛的应用.本文针对运动目标跟踪问题,将经典的卡尔曼滤波应用于跟踪算法中,对目标的运动行为进行状态预估,预测目标参数,对于目标被遮挡或者丢失的情况具有良好的跟踪效果.通过实验进行跟踪仿真表明:该算法提高了目标跟踪的稳定性,快速性和精确性,能够获得良好的跟踪效果.

2 部分代码

%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;
%%%%%%%%%%%%%%%%%%子区域代码结束区域%%%%%%%%%%%%%%%%%%%%%%
k1=AUV_Model(AUV_status{1}{i}(:,auv_num(i,1)-1),angle_v(i,1));%AUV模型
AUV_status{1}{i}(:,auv_num(i,1))=AUV_status{1}{i}(:,auv_num(i,1)-1)+k1;
if(AUV_status{1}{i}(6,auv_num(i,1))>pi)
AUV_status{1}{i}(6,auv_num(i,1))=AUV_status{1}{i}(6,auv_num(i,1))-2*pi;
else if(AUV_status{1}{i}(6,auv_num(i,1))<-pi)
AUV_status{1}{i}(6,auv_num(i,1))=AUV_status{1}{i}(6,auv_num(i,1))+2*pi;
end
end
AUV_PosTemp(1:2,i)=AUV_status{1}{i}(4:5,auv_num(i,1));%当前AUV位置
AUV_AngTemp(i,1)=AUV_status{1}{i}(6,auv_num(i,1));%当前AUV角度
auv_num(i,1)=auv_num(i,1)+1;
end
%*******画动态图*******************%
environment_plot4(pos_target,step,T1(step,1),task_time(:,1)',lock_area,number_target);%环境构建
DrawAUV(AUV_PosTemp,AUV_AngTemp,AUV_num);%画AUV
made_dynamictar(dynamic_pos,dynamic_angle,2);
for i=1:AUV_num
if(isempty(obs_auv_PosAngleDis{1}{i})==0)
plot(obs_auv_PosAngleDis{1}{i}(:,1),obs_auv_PosAngleDis{1}{i}(:,2),'mo');
end
line(AUV_status{1}{i}(4,:),AUV_status{1}{i}(5,:),'linewidth',1.5,'color',line_color(i,:));%绘制AUV实时轨迹
end
hold off;
drawnow;
step = step + 1;
if(search_tar_over==1&&dy_tar_num==2||sub2overflag==0)%子区域全部分配完就结束所有任务，暂时先这样。后期还会更
break;
end
end
% figure(4);clf;
% plot(T,error,'r','LineWidth',2);grid on;xlabel('仿真时间');ylabel('误差(m)');
% figure(5);clf;
% heading_angle(:,1)=AUV_status{1}{1}(6,:)*180/pi;
% heading_speed(:,1)=AUV_status{1}{1}(3,:)*180/pi;
% subplot(2,1,1);
% plot(T1,heading_speed,'b','LineWidth',2);grid on;xlabel('仿真时间');ylabel('角速度(°)');
% subplot(2,1,2);
% plot(T1,heading_angle,'b','LineWidth',2);grid on;xlabel('仿真时间');ylabel('艏向(°)');