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

3 仿真结果

4 参考文献

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

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

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