1 簡(jiǎn)介

四旋翼無人機(jī)目前被廣泛應(yīng)用于民用或是軍事領(lǐng)域.由于其靈活小巧且輕盈的優(yōu)點(diǎn),能代替人類探索未知地域或執(zhí)行各種危險(xiǎn)任務(wù),如地形勘察,探取情報(bào),目標(biāo)跟蹤等,而精確可靠的目標(biāo)跟蹤算法更是在無人機(jī)領(lǐng)域獲得了廣泛的應(yīng)用.本文針對(duì)運(yùn)動(dòng)目標(biāo)跟蹤問題,將經(jīng)典的卡爾曼濾波應(yīng)用于跟蹤算法中,對(duì)目標(biāo)的運(yùn)動(dòng)行為進(jìn)行狀態(tài)預(yù)估,預(yù)測(cè)目標(biāo)參數(shù),對(duì)于目標(biāo)被遮擋或者丟失的情況具有良好的跟蹤效果.通過實(shí)驗(yàn)進(jìn)行跟蹤仿真表明:該算法提高了目標(biāo)跟蹤的穩(wěn)定性,快速性和精確性,能夠獲得良好的跟蹤效果.

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; % 最大探測(cè)范圍Vl=zeros(2000,2000);%環(huán)境代價(jià)矩陣Time_AllSteps = 50; %額定步數(shù)Y = zeros(Time_AllSteps,6); %狀態(tài)變量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];%障礙物占據(jù)的子區(qū)域中心點(diǎn)坐標(biāo)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; ? ? ? ?%%%%%%%%%%%%%%%%%%子區(qū)域代碼結(jié)束區(qū)域%%%%%%%%%%%%%%%%%%%%%% ? ? ? ?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));%當(dāng)前AUV位置 ? ? ? ?AUV_AngTemp(i,1)=AUV_status{1}{i}(6,auv_num(i,1));%當(dāng)前AUV角度 ? ? ? ?auv_num(i,1)=auv_num(i,1)+1; ? ?end ? ? ? %*******畫動(dòng)態(tài)圖*******************% ? ?environment_plot4(pos_target,step,T1(step,1),task_time(:,1)',lock_area,number_target);%環(huán)境構(gòu)建 ? ?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實(shí)時(shí)軌跡 ? ?end ? ?hold off; ? ?drawnow; ? ?step = step + 1; ? ?if(search_tar_over==1&&dy_tar_num==2||sub2overflag==0)%子區(qū)域全部分配完就結(jié)束所有任務(wù)，暫時(shí)先這樣。后期還會(huì)更 ? ? ? break; ? ?endend% figure(4);clf;% plot(T,error,'r','LineWidth',2);grid on;xlabel('仿真時(shí)間');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('仿真時(shí)間');ylabel('角速度(°)');% subplot(2,1,2);% plot(T1,heading_angle,'b','LineWidth',2);grid on;xlabel('仿真時(shí)間');ylabel('艏向(°)');

3 仿真結(jié)果

4 參考文獻(xiàn)

[1]唐大全, 鄧偉棟, 唐管政,等. 基于迭代無跡卡爾曼濾波的無人機(jī)編隊(duì)協(xié)同導(dǎo)航[J].? 2022(10).

博主簡(jiǎn)介：擅長(zhǎng)智能優(yōu)化算法、神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)、信號(hào)處理、元胞自動(dòng)機(jī)、圖像處理、路徑規(guī)劃、無人機(jī)等多種領(lǐng)域的Matlab仿真，相關(guān)matlab代碼問題可私信交流。

部分理論引用網(wǎng)絡(luò)文獻(xiàn)，若有侵權(quán)聯(lián)系博主刪除。