附錄B模擬軌跡程序

1、附錄 B 模擬軌跡程序function trajectoryglobal Re e g0 wieRe = 6378160; e = 1/298.3; wie = 7.2921151467e-5; g0 = 9.7803267714;th = 0.002;%龍格 -庫(kù)塔步長(zhǎng) 2msdeg = pi/180; min = deg/60;att = pi/180*0; 0; 0;%pitch roll azimuthvb = 0; 0; 0; %0 vby 0pos = 34*deg+14.76289014*min; 108*deg+54.57983*min; 380;%latitude longi

2、tude height%rk(1:3) att(pitch roll azimuth);rk(2:6) vn(vnE vnN vnU);rk(7:9) pos(lti lgi hgt)%rk(10:12) wm; rk(13:15) vm;角增量從零時(shí)刻起的累計(jì)增量 比力增量rk = att; Att2Mat(att)*vb; pos; zeros(6,1);fid = fopen(e:/ygm/vehicle/trace2.bin,wb); %4 goodwat2 = getwat(0);for k=2:2:1000*1000wat0 = wat2;k1 = funrk(rk,wat0);w

3、at1 = getwat(k-1); k2 = funrk(rk+th/2*k1, wat1);k3 = funrk(rk+th/2*k2, wat1);wat2 = getwat(k);k4 = funrk(rk+ th*k3, wat2);rk = rk + th/6*(k1+2*k2+2*k3+k4);if mod(k,10)=0 %10 msfwrite(fid, rk; k4(10:15), real*8);%保存數(shù)據(jù) , k4(10:15): wb, fbif mod(k,1000)=0step=k/1000,% 進(jìn)度顯示endendendfclose(fid);function

4、drk = funrk(rk, wat)global Re e g0 wie%通用變量計(jì)算si = sin(rk(1); ci = cos(rk(1); sj = sin(rk(2); cj = cos(rk(2); sk = sin(rk(3); ck = cos(rk(3); slti = sin(rk(7); clti = cos(rk(7); tlti = slti/clti; slti2 = slti2; slti4 = slti22;RM = Re*(1-2*e+3*e*slti2); RN = Re*(1+e*slti2); RMh = RM + rk(9); RNh = RN

5、+ rk(9); wnie = wie * 0; clti; slti;wnen = -rk(5)/RMh; rk(4)/RNh; rk(4)/RNh*tlti;Cnb = cj*ck+si*sj*sk, ci*sk, sj*ck-si*cj*sk;100-cj*sk+si*sj*ck, ci*ck, -sj*sk-si*cj*ck;-ci*sj, si, ci*cj ;%姿態(tài)增量datt = wat(1:3);%速度增量Cnt = ck, ci*sk, -si*sk;-sk, ci*ck, -si*ck;0, si, ci ;dvn = Cnt*wat(4:6);%位置增量dpos = rk

6、(5)/RMh; rk(4)/(RNh*clti); rk(6);%角增量wnin = wnie + wnen; a2w = cj, 0, sj*ci;0, 1, -si;sj, 0, -cj*ci ;wbnb = a2w*wat(1:3);dwm = Cnb*wnin + wbnb;%比力增量g = g0*(1+5.27094e-3*slti2+2.32718e-5*slti4) - 3.086e-6*rk(9); gn = 0; 0; -g;dvm = Cnb * (dvn+cross(2*wnie+wnen,rk(4:6)-gn);%rk 增量drk = datt; dvn; dpos;

7、 dwm; dvm;function wat = getwat(k)% 軌跡設(shè)置%姿態(tài)角變化率 wat(1:3) wPitch wRoll wAzimuth; 軌跡加速度 wat(4:6) atx aty atz%階段動(dòng)作 起始時(shí)間 (s)航向 azimuth(deg) 俯仰 pitch(deg)速度 (m/s)%階段動(dòng)作持續(xù)時(shí)間(s)傾斜roll(deg)加速度 (m/s2)%停止 010000000if k=100*1000wat=0; 0; 0;0; 0; 0;%加速 1002000010elseif k=120*1000wat=0; 0; 0;0; 1; 0;%勻速 120800000

8、20elseif k=200*1000wat=0; 0; 0;0; 0; 0;%錐運(yùn)動(dòng) 200100000020elseif k=300*1000;k=k-200*1000; t = k/1000; a = 1*pi/180; w = 2*pi*5; if k=1*1000 % 進(jìn)入錐運(yùn)動(dòng) 1s101wat = a*sin(w*1); a*cos(w*1); 0;0; 0; 0;elseif k=99*1000%錐運(yùn)動(dòng) 98swat = a*w*cos(w*t); -a*w*sin(w*t); 0;0; 0; 0;elseif k=100*1000%退出錐運(yùn)動(dòng) 1swat = -a*sin(w

9、*99); a*cos(w*99); 0;0; 0; 0;end%減速 30010000-120elseif k=310*1000wat=0; 0; 0;0; -1; 0;%右轉(zhuǎn)彎 31030/1500010elseif k=313*1000v=10; w=45*pi/180/3; a=v*w;wat=0; 0; w;a; 0; 0;%勻速 313974500010elseif k=400*1000wat=0; 0; 0;0; 0; 0;%進(jìn)入爬升 40054500/3010elseif k=405*1000v=10; w=15*pi/180/5; a=v*w;wat=w; 0; 0;0; 0

10、; a;%勻速 4058045015010elseif k=485*1000wat=0; 0; 0;0; 0; 0;%退出爬升 4851545015/-1010elseif k=500*1000v=10; w=15*pi/180/15; a=v*w;wat=-w; 0; 0;0; 0; -a;%左轉(zhuǎn)彎 50031.545/-1000010elseif k=531.5*1000v=10; w=315*pi/180/31.5; a=v*w;wat=0; 0 ; -w;-a; 0; 0;%勻速 531.568.59000010elseif k=600*1000wat=0; 0; 0;0; 0; 0;

11、%減速 600109000-110elseif k=610*1000wat=0; 0; 0;0; -1; 0;%停止 61090900000elseif k=700*1000wat=0; 0; 0;0; 0; 0;%劃船運(yùn)動(dòng) 700100900000elseif k=800*1000;102k=k-700*1000; t = k/1000; a = 1*pi/180; p = 1.0e-3; w = 2*pi*2; if k=1*1000% 進(jìn)入劃船運(yùn)動(dòng) 1swat = a*cos(w*1); 0; 0;0; -p*w*sin(w*1); 0;elseif k=99*1000%劃船運(yùn)動(dòng) 98swat = -a*w*sin(w*t); 0; 0;0; -p*w2*cos(w*t); 0;elseif k=100*1000%退出劃船運(yùn)動(dòng) 1swat = -a*cos(w*99); 0; 0; 0; -p*w*sin(w*99); 0; end%加速 800 elseif k=810*1000