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

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('艏向(°)');
          
          

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.
• 16.
• 17.
• 18.
• 19.
• 20.
• 21.
• 22.
• 23.
• 24.
• 25.
• 26.
• 27.
• 28.
• 29.
• 30.
• 31.
• 32.
• 33.
• 34.
• 35.
• 36.
• 37.
• 38.
• 39.
• 40.
• 41.
• 42.
• 43.
• 44.
• 45.
• 46.
• 47.
• 48.
• 49.
• 50.
• 51.
• 52.
• 53.
• 54.
• 55.
• 56.
• 57.
• 58.
• 59.
• 60.
• 61.
• 62.
• 63.
• 64.
• 65.
• 66.
• 67.
• 68.
• 69.
• 70.
• 71.
• 72.
• 73.
• 74.
• 75.
• 76.
• 77.
• 78.
• 79.
• 80.
• 81.
• 82.
• 83.
• 84.
• 85.
• 86.
• 87.
• 88.
• 89.
• 90.
• 91.
• 92.
• 93.
• 94.
• 95.
• 96.
• 97.
• 98.
• 99.
• 100.
• 101.
• 102.
• 103.
• 104.
• 105.
• 106.
• 107.
• 108.
• 109.
• 110.
• 111.
• 112.
• 113.

3 仿真结果

4 参考文献

[1]唐大全, 邓伟栋, 唐管政,等. 基于迭代无迹卡尔曼滤波的无人机编队协同导航[J].  2022(10).

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。