ppt第十二章用MATLAB解最優(yōu)控制問題及應(yīng)用實(shí)例.ppt

1、第十二章 用MATLAB解最優(yōu)控制問題及應(yīng)用實(shí)例,第十二章 用MATLAB解最優(yōu)控制問題及應(yīng)用實(shí)例,12.1 MATLAB工具簡介 12.2 用MATLAB解線性二次型最優(yōu)控制問題 12.3 用MATLAB解最優(yōu)控制問題應(yīng)用實(shí)例 12.4 小結(jié),MATLAB是集數(shù)值運(yùn)算、符號運(yùn)算及圖形處理等強(qiáng)大功能于一體的科學(xué)計算語言。作為強(qiáng)大的科學(xué)計算平臺，它幾乎能滿足所有的計算需求。MATLAB具有編程方便、操作簡單、可視化界面、優(yōu)良的仿真圖形環(huán)境、豐富的多學(xué)科工具箱等優(yōu)點(diǎn)，尤其是在自動控制領(lǐng)域中MATLAB顯示出更為強(qiáng)大的功能。,最優(yōu)控制是在一定的約束條件下，從已給定的初始狀態(tài)出發(fā)，確定最優(yōu)控制作用的函

2、數(shù)式，使目標(biāo)函數(shù)為極小或極大。在設(shè)計最優(yōu)控制器的過程中，運(yùn)用MATLAB最優(yōu)控制設(shè)計工具，會大大減小設(shè)計的復(fù)雜性。 在前面的幾章中，我們已經(jīng)介紹了一些最優(yōu)控制方法，在本章中我們將介紹一個最優(yōu)控制問題的應(yīng)用實(shí)例，討論如何使用最優(yōu)控制方法來設(shè)計自尋的制導(dǎo)導(dǎo)彈的最優(yōu)導(dǎo)引律，并采用MATLAB工具實(shí)現(xiàn)最優(yōu)導(dǎo)引律，通過仿真來驗(yàn)證最優(yōu)導(dǎo)引律的有效性。,12.1 MATLAB工具簡介,1, 系統(tǒng)模型的建立 系統(tǒng)的狀態(tài)方程為：,在MATLAB中只需要將各個系數(shù)按照常規(guī)矩陣的方式輸入到工作空間即可 ss(A,B,C,D),傳遞函數(shù)的零極點(diǎn)模型為：,在MATLAB中可以采用如下語句將零極點(diǎn)模型輸入到工作空間:,z

3、pk(Z,P,KGain),傳遞函數(shù)模型在更一般的情況下，可以表示為復(fù)數(shù)變量s的有理函數(shù)形式：,在MATLAB中可以采用如下語句將以上的傳遞函數(shù)模型輸入到工作空間: G=tf(num,den);,2, 系統(tǒng)模型的轉(zhuǎn)換 把其他形式轉(zhuǎn)換成狀態(tài)方程模型 G1=ss(G) 把其他形式轉(zhuǎn)換成零極點(diǎn)模型 G1=zpk(G) 把其他形式轉(zhuǎn)換成一般傳遞函數(shù)模型 G1=tf(G),3, 系統(tǒng)穩(wěn)定性判據(jù) 求出系統(tǒng)所有的極點(diǎn)，并觀察系統(tǒng)是否有實(shí)部大于0的極點(diǎn)。 系統(tǒng)由傳遞函數(shù) (num,den) 描述 roots(den) 系統(tǒng)由狀態(tài)方程 (A,B,C,D) 描述 eig(A),4, 系統(tǒng)的可控性與可觀測性分析 在

4、MATLAB的控制系統(tǒng)工具箱中提供了ctrbf()函 數(shù)。該函數(shù)可以求出系統(tǒng)的可控階梯變換，該函數(shù) 的調(diào)用格式為： Ac,Bc,Cc,Dc,Tc,Kc=ctrbf(A,B,C) 在MATLAB的控制系統(tǒng)工具箱中提供了obsvf()函 數(shù)。該函數(shù)可以求出系統(tǒng)的可觀測階梯變換，該函 數(shù)的調(diào)用格式為： Ao,Bo,Co,Do,To,Ko=obsvf(A,B,C),5, 系統(tǒng)的時域分析 對于系統(tǒng)的階躍響應(yīng)，控制系統(tǒng)工具箱中給出了 一個函數(shù)step()來直接求取系統(tǒng)的階躍響應(yīng)，該函數(shù) 的可以有如下格式來調(diào)用： y=step(G,t) 對于系統(tǒng)的脈沖響應(yīng)，控制系統(tǒng)工具箱中給出了 一個函數(shù)impulse()

5、來直接求取系統(tǒng)的脈沖響應(yīng)，該 函數(shù)的可以有如下格式來調(diào)用： y=impulse (G,t),6, 系統(tǒng)的復(fù)域與頻域分析 對于根軌跡的繪制，控制系統(tǒng)工具箱中給出了一 個函數(shù)rlocus()函數(shù)來繪制系統(tǒng)的根軌跡，該函數(shù)的 可以由如下格式來調(diào)用： R=rlocus(G,k),對于Nyquist曲線的繪制，控制系統(tǒng)工具箱中給出了一個函數(shù)nyquist()函數(shù)，該環(huán)數(shù)可以用來直接求解Nyquist陣列，繪制出Nyquist曲線，該函數(shù)的可以由如下格式來調(diào)用： rx,ry=nyquist(G,w) 對于Bode圖，MATLAB控制工具箱中提供了bode()函數(shù)來求取、繪制系統(tǒng)的Bode圖，該函數(shù)可以由下

6、面的格式來調(diào)用 mag,pha=bode(G,w),12.2 用MATLAB解線性二次型最優(yōu)控制問題,一般情況的線性二次問題可表示如下： 設(shè)線性時變系統(tǒng)的方程為 其中， 為 維狀態(tài)向量， 為 維控制向量， 為維輸出向量。,尋找最優(yōu)控制，使下面的性能指標(biāo)最小,其中， 是 對稱半正定常數(shù)陣， 是 對稱半正定陣, 是 對稱正定陣。,我們用最小值原理求解上述問題，可以把上述問題歸結(jié)為求解如下黎卡提（Riccati）矩陣微分方程：,這個方程稱為代數(shù)黎卡提方程。代數(shù)黎卡提方程的求解非常簡單，并且其求解只涉及到矩陣運(yùn)算，所以非常適合使用MATLAB來求解。,方法一：,求解代數(shù)黎卡提方程的算法有很多，下面我們

7、介紹一種簡單的迭代算法來解該方程，令 ，則可以寫出下面的迭代公式,%*MATLAB程序*% I=eye(size(A); iA=inv(I-A); E=iA*(I+A); G=2*iA2*B; H=R+B*iA*Q*iA*B; W=Q*iA*B; P0=zeros(size(A); i=0;,while(1),i=i+1; P=E*P0*E-(E*P0*G+W)*inv(G*P0*G+H)*(E*P0*G+W)+Q; if(norm(P-P0)eps),break; else,P0=P; end end P=2*iA*P*iA; 我們把這個文件命名為mylq.m，方便我們以后調(diào)用來 求解代數(shù)黎

8、卡提方程。,方法二： 在MATLAB的控制系統(tǒng)工具箱中提供了求解代數(shù)黎卡提方程的函數(shù)lqr()，其調(diào)用的格式為： K,P,E=lqr(A,B,Q,R) 式中輸入矩陣為A,B,Q,R，其中(A,B)為給定的對象狀態(tài)方程模型，(Q,R)分別為加權(quán)矩陣Q和R；返回矩陣K為狀態(tài)反饋矩陣，P為代數(shù)黎卡提方程的解，E為閉環(huán)系統(tǒng)的零極點(diǎn)。,這里的求解是建立在MATLAB的控制系統(tǒng)工具箱中給出的一個基于Schur變換的黎卡提方程求解函數(shù)are()基礎(chǔ)上的，該函數(shù)的調(diào)用格式為： X=are(M,T,V),其中， 矩陣滿足下列代數(shù)黎卡提方程，are是Algebraic Riccati Equation的縮寫。 對

9、比前面給出的黎卡提方程，可以容易得出,方法三： 我們也可以采用care()函數(shù)對連續(xù)時間代數(shù)黎卡提 方程求解，其調(diào)用方法如下： P,E,K,RR=care(A,B,Q,R,zeros(size(B),eye(size(A) 式中輸入矩陣為A,B,Q,R，其中(A,B)為給定的對象狀態(tài)方程模型，(Q,R)分別為加權(quán)矩陣Q和R；返回矩陣P為代數(shù)黎卡提方程的解，E為閉環(huán)系統(tǒng)的零極點(diǎn)，K為狀態(tài)反饋矩陣，RR是相應(yīng)的留數(shù)矩陣Res的Frobenius范數(shù)（其值為：sqrt(sum(diag(Res*Res)，或者用Norm(Res, fro)計算）。,采用care函數(shù)的優(yōu)點(diǎn)在于可以設(shè)置P的終值條件，例如

10、我們可以在下面的程序中設(shè)置P的終值條件為0.2;0.2。 P,E,K,RR=care(A,B,Q,R,0.2;0.2,eye(size(A) 采用lqr()函數(shù)不能設(shè)置代數(shù)黎卡提方程的邊界條件。,例12-1,線性系統(tǒng)為： ， 其目標(biāo)函數(shù)是：,確定最優(yōu)控制。,解： 方法一： A=0 1;-5,-3; B=0;1; Q=500 200;200 100; R=1.6667; mylq K=inv(R)*B*P P E,運(yùn)行結(jié)果： K = 13.0276 6.7496 P = 67.9406 21.7131 21.7131 11.2495 E = -0.1111 0.2222 -1.1111 -0.7

11、778,方法二: A=0 1;-5,-3; B=0;1; Q=500 200;200 100; R=1.6667; K,P,E=lqr(A,B,Q,R),運(yùn)行結(jié)果： K = 13.0276 6.7496 P = 67.9406 21.7131 21.7131 11.2495 E =-7.2698 -2.4798,方法三： A=0 1;-5,-3; B=0;1; Q=500 200;200 100; R=1.6667; P,E,K,RR=care(A,B,Q,R,zeros(size(B),eye(size(A),運(yùn)行結(jié)果： P = 67.9406 21.7131 21.7131 11.2495

12、 E = -7.2698 -2.4798 K =13.0276 6.7496 RR = 2.8458e-015,以上的三種方法的運(yùn)行結(jié)果相同。我們可以得到，最優(yōu)控制變量與狀態(tài)變量之間的關(guān)系： 在以上程序的基礎(chǔ)上，可以得到在最優(yōu)控制的作用下的最優(yōu)控制曲線與最優(yōu)狀態(tài)曲線，其程序如下：,%*MATLAB程序*% figure(pos,50,50,200,150,color,w); axes(pos,0.15,0.14,0.72,0.72) ap=A-B*K; bp=B; C=1,0; D=0;,ap,bp,cp,dp=augstate(ap,bp,C,D); cp=cp;-K; dp=dp;0; G

13、=ss(ap,bp,cp,dp); y,t,x=step(G); plotyy(t,y(:,2:3),t,y(:,4) ax,h1,h2=plotyy(t,y(:,2:3),t,y(:,4); axis(ax(1),0 2.5 0 0.1),axis(ax(2),0 2.5 -1 0),運(yùn)行結(jié)果： 圖12-1 最優(yōu)控制曲線與最優(yōu)狀態(tài)曲線,該程序采用augstate函數(shù)將狀態(tài)變量作為輸出變量，用于顯示；輸出項(xiàng)作為最優(yōu)控制的輸出。因此，階躍響應(yīng)輸出y中，y(1)是系統(tǒng)輸出，y(2)和y(3)是狀態(tài)變量輸出，y(4)是系統(tǒng)控制變量輸出。用plotyy函數(shù)進(jìn)行雙坐標(biāo)顯示，并設(shè)置相應(yīng)的坐標(biāo)范圍。,以上三

14、種方法中，第一種方法易于理解黎卡提方程的解法，其解法簡單但是并不可靠。第二種方法比起另兩種方法使用方便，不易出錯，所以我們推薦使用這種方法。但是采用lqr()函數(shù)不能設(shè)置代數(shù)黎卡提方程的邊界條件，所以，如果題目設(shè)置了P的終值條件，我們只能使用第三種方法來求解，例如設(shè)置P的終值條件為0.2;0.2。,程序如下： %*MATLAB程序*% A=0 1;-5,-3; B=0;1; Q=500 200;200 100; R=1.6667; P,E,K,RR=care(A,B,Q,R,0.2;0.2,eye(size(A),運(yùn)行結(jié)果： P =67.7233 21.5685 21.5685 11.0961

15、 E =-7.3052 -2.4723 K =13.0608 6.7775 RR =1.2847e-014 最優(yōu)控制變量與狀態(tài)變量之間的關(guān)系：,例12-2,無人飛行器的最優(yōu)高度控制，飛行器的控制方程如下,是飛行器的高度； 是油門輸入；設(shè)計控制律使得如下指標(biāo)最小,初始狀態(tài) 。繪制系統(tǒng)狀態(tài)與控制輸入，對如下給定的 矩陣進(jìn)行仿真分析.,a). b). c). d).,解：線性二次型最優(yōu)控制指標(biāo)如下： 其中Q和R分別是對狀態(tài)變量和控制量的加權(quán)矩陣， 線性二次型最優(yōu)控制器設(shè)計如下： 1）、Q=diag（1，0，0），R=2時，由MATLAB求得最優(yōu)狀態(tài)反饋矩陣為 k1=0.7071 2.0772 2.0

16、510， u（t）=k1*x（t）； 所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線如下圖12-2所示：,圖12-2 狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線,2）、Q=diag（1，0，0），R=2000時，由MATLAB求得最優(yōu)狀態(tài)反饋矩陣為 k2=0.0224 0.2517 0.4166， u（t）= k2*x（t）； 所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線如下圖12-3所示：,圖12-3 狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線,3）、Q=diag（10，0，0），R=2時，由MATLAB求得最優(yōu)狀態(tài)反饋矩陣為 k3=2.2361 4.3892 3.3077，u（t）= k3*x（t）； 所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲

17、線如下圖12-4所示：,圖12-4 狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線,4）、Q=diag（1，100，0），R=2時，由MATLAB求 得最優(yōu)狀態(tài)反饋矩陣為 k4=0.7071 7.6112 4.6076， u（t）= k4*x（t）； 所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線如下圖12-5所示：,圖12-5 狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線,由1），2），3），4）可分析如下：,圖12-3與圖12-2相比，當(dāng)Q不變，R增大時，各相應(yīng)曲線達(dá)到穩(wěn)態(tài)所需時間增長，即響應(yīng)變慢；但波動幅值變小，反饋矩陣變小；,圖12-4與圖12-2和圖12-3相比，當(dāng)Q對角線上第1個元素增大時，各相應(yīng)曲線達(dá)到穩(wěn)態(tài)所需時間變短，

18、即響應(yīng)快；但波動幅值變大，反饋矩陣增大；,由圖12-5可知，當(dāng)Q對角線上第2個元素增大時，狀態(tài)x1，x2曲線達(dá)到穩(wěn)態(tài)所需時間較長，即響應(yīng)較慢，平緩的趨于零；狀態(tài)x3，控制輸入u達(dá)到穩(wěn)態(tài)所需時間短，即響應(yīng)快；狀態(tài)x2，x3波動幅值較小，比圖12-2和圖12-4小，比圖12-3稍大，控制輸入u波動幅值比圖12-2和圖12-4小，比圖12-3大；反饋矩陣最大。,綜上所述可得結(jié)論：Q=diag（1，0，0），R=2時，系統(tǒng)各方面響應(yīng)較好。 矩陣Q變大時，反饋矩陣變大； 當(dāng)Q的對角線上第1個元素變大時，各曲線波動幅值變大，達(dá)到穩(wěn)態(tài)所需時間變短；,當(dāng)Q的對角線上第2個元素變大時，各曲線波動幅值變??；達(dá)到穩(wěn)

19、態(tài)所需時間，狀態(tài)x1，x2增長，狀態(tài)x3，控制輸入u變短； 當(dāng)R變大時，反饋矩陣變??；各曲線波動幅值變??；達(dá)到穩(wěn)態(tài)所需時間變長。 所以根據(jù)實(shí)際的系統(tǒng)允許，我們應(yīng)該適當(dāng)選擇Q和R。,%*MATLAB程序*% a=0 1 0;0 0 1;0 0 -1/2;b=0;0;1/2;c=1 0 0;0 1 0;0 0 1;d=0;0;0; figure(1) q=1 0 0;0 0 0;0 0 0; r=2; k,p,e=lqr(a,b,q,r) x0=10;0;0; a1=a-b*k; y,x=initial(a1,b,c,d,x0,20);,n=length(x(:,3); T=0:20/n:20-2

20、0/n; plot(T,x(:,1),black,T,x(:,2),red,T,x(:,3),green); xlabel(time-s);ylabel(response); title(圖(1.a) Q=diag(1,0,0),R=2時狀態(tài)響應(yīng)曲線) grid,hold on for j=1:n u(j,:)=-k*(x(j,:); end,figure(2) plot(T,u);xlabel(time-s);ylabel(response); title(圖(1.b) Q=diag(1,0,0),R=2時控制輸入u的響應(yīng)曲線) grid,hold on %*,figure(3) qa=1

21、0 0;0 0 0;0 0 0; ra=2000; ka,pa,ea=lqr(a,b,qa,ra) x0=10;0;0; aa1=a-b*ka; ya,xa=initial(aa1,b,c,d,x0,60); na=length(xa(:,3);,Ta=0:60/na:60-60/na; plot(Ta,xa(:,1),black,Ta,xa(:,2),red,Ta,xa(:,3),green); xlabel(time-s);ylabel(response); title(圖(2.a) Q=diag(1,0,0),R=2000時狀態(tài)響應(yīng)曲線) grid,hold on for j=1:na

22、ua(j,:)=-ka*(xa(j,:); end,figure(4) plot(Ta,ua);xlabel(time-s);ylabel(response); title(圖(2.b) Q=diag(1,0,0),R=2000時控制輸入u的響應(yīng)曲線) grid,hold on %*,figure(5) qb=10 0 0;0 0 0;0 0 0; rb=2; kb,pb,eb=lqr(a,b,qb,rb) x0=10;0;0; ab1=a-b*kb; yb,xb=initial(ab1,b,c,d,x0,20); nb=length(xb(:,3);,Tb=0:20/nb:20-20/nb;

23、 plot(Tb,xb(:,1),black,Tb,xb(:,2),red,Tb,xb(:,3),green); xlabel(time-s);ylabel(response); title(圖(3.a) Q=diag(10,0,0),R=2時狀態(tài)響應(yīng)曲線) grid,hold on for j=1:nb ub(j,:)=-kb*(xb(j,:); end,figure(6) plot(Tb,ub);xlabel(time-s);ylabel(response); title(圖(3.b) Q=diag(10,0,0),R=2時控制輸入u的響應(yīng)曲線) grid,hold on %*,figur

24、e(7) qc=1 0 0;0 100 0;0 0 0; rc=2; kc,pc,ec=lqr(a,b,qc,rc) x0=10;0;0; ac1=a-b*kc; yc,xc=initial(ac1,b,c,d,x0,50); nc=length(xc(:,3);,Tc=0:50/nc:50-50/nc; plot(Tc,xc(:,1),black,Tc,xc(:,2),red,Tc,xc(:,3),green); xlabel(time-s);ylabel(response); title(圖(4.a) Q=diag(1,100,0),R=2時狀態(tài)響應(yīng)曲線) grid,hold on for

25、 j=1:nc uc(j,:)=-kc*(xc(j,:); end,figure(8) plot(Tc,uc);xlabel(time-s);ylabel(response); title(圖(4.b) Q=diag(1,100,0),R=2時控制輸入u的響應(yīng)曲線) grid,hold on,12.3 用MATLAB解最優(yōu)控制問題應(yīng)用實(shí)例,12.3.1 導(dǎo)彈運(yùn)動狀態(tài)方程的建立 12.3.2 最優(yōu)導(dǎo)引律的求解與仿真驗(yàn)證,在現(xiàn)有的自尋的導(dǎo)彈中，大都采用比例導(dǎo)引法。假設(shè)導(dǎo)彈和目標(biāo)在同一平面內(nèi)運(yùn)動，按比例導(dǎo)引制導(dǎo)律，假設(shè)導(dǎo)彈的速度向量的旋轉(zhuǎn)角速度 垂直于瞬時的彈目視線，并且正比于導(dǎo)彈與目標(biāo)之間的視線角

26、速率 ，假設(shè)目標(biāo)的法向加速度為零，那么可得： (12-1),比例導(dǎo)引法是經(jīng)典的導(dǎo)引方法。下面我們從最優(yōu)控制理論的觀點(diǎn)來研究自尋的導(dǎo)彈的最優(yōu)導(dǎo)引規(guī)律問題。,12.3.1 導(dǎo)彈運(yùn)動狀態(tài)方程的建立,導(dǎo)彈與目標(biāo)的運(yùn)動關(guān)系是非線性的，如果把導(dǎo)彈與目標(biāo)的運(yùn)動方程相對于理想彈道線性化，可得導(dǎo)彈運(yùn)動的線性狀態(tài)方程.,圖12-6 導(dǎo)彈和目標(biāo)運(yùn)動幾何關(guān)系圖,假定 和 比較小，因此， 則,(12-4),以 表示 ， 表示 （即 ），則 (12-5) (12-6),式中 表示目標(biāo)的橫向加速度， 表示導(dǎo)彈橫向加速度，分別以 和 表示，那么 (12-7),這樣可得導(dǎo)彈運(yùn)動狀態(tài)方程為： (12-10) (12-11),由于

27、系統(tǒng)是線性的，指標(biāo)函數(shù)是二次型的，因此，求最優(yōu)控制規(guī)律就可以認(rèn)為是一個求解線性二次型的過程。 對于線性二次型問題，可采用變分法、極小值原理、動態(tài)規(guī)劃或其他方法求得最優(yōu)控制,12.3.2 最優(yōu)導(dǎo)引律的求解與仿真驗(yàn)證,當(dāng)不考慮彈體慣性時，而且假定目標(biāo)不機(jī)動，即，導(dǎo)彈運(yùn)動狀態(tài)方程為 (12-24),下面將對最優(yōu)導(dǎo)引律進(jìn)行MATLAB仿真，并給出源代碼和仿真結(jié)果。,圖12-7 最優(yōu)導(dǎo)引方框圖,圖12-8 最優(yōu)導(dǎo)引攻擊幾何平面,以上列出了兩維的最優(yōu)導(dǎo)引制導(dǎo)的必要方程，但是使用最優(yōu)導(dǎo)引制導(dǎo)的導(dǎo)彈并不是直接向著目標(biāo)發(fā)射的，而是向著一個能夠?qū)б龑?dǎo)彈命中目標(biāo)的方向發(fā)射，考慮了視線角之后可以得到導(dǎo)彈的指向角L。從

28、圖12-8中我們可以看出，如果導(dǎo)彈進(jìn)入了碰撞三角區(qū)（如果目標(biāo)和導(dǎo)彈同時保持勻速直線運(yùn)動，導(dǎo)彈必定會命中目標(biāo)），這時利用正弦公式可以得到指向角的表達(dá)式：,(12-53),但是實(shí)際上導(dǎo)彈不可能能確切地在碰撞三角區(qū)發(fā)射，所以不能精確地得到攔截點(diǎn)。因?yàn)槲覀儾恢滥繕?biāo)將會如何機(jī)動，所以攔截點(diǎn)位置只能大概地估計。事實(shí)上，這也是需要導(dǎo)航系統(tǒng)的原因！初始時刻導(dǎo)彈偏離碰撞三角的角度稱之為指向角誤差（Head-Error）?？紤]了導(dǎo)彈初始時刻的指向角和指向角誤差之后，導(dǎo)彈的初始速度分量可以表示為：,(12-54) (12-55),使用MATLAB編程，具體代碼如下： %*MATLAB程序*% %最優(yōu)制導(dǎo)律仿真，初

29、始化系統(tǒng)的參數(shù) clear all; %清除所有內(nèi)存變量 global SignVc; pi=3.14159265; Vm=1000;Vt=500;%導(dǎo)彈和目標(biāo)的速度 HeadError=0; %指向角誤差,ThetaT=pi; %目標(biāo)的速度方向 Rmx=0;Rmy=0; %導(dǎo)彈的位置 Rtx=5000;Rty=10000;%目標(biāo)的位置 At=0; %目標(biāo)法向加速度 Vtx=Vt*sin(ThetaT);%目標(biāo)的速度分量 Vty=Vt*cos(ThetaT); Rtmx=Rtx-Rmx; %彈目相對距離 Rtmy=Rty-Rmy; AmMax=15*9.8; %導(dǎo)彈的最大機(jī)動能力為15G Rt

30、m=sqrt(Rtmx2+Rtmy2);,SightAngle=atan(Rtmx/Rtmy); %視線角 LeadAngle=asin(Vt*sin(SightAngle-ThetaT)/Vm); %指向角 Vmx=Vm*sin(SightAngle-LeadAngle+HeadError); %導(dǎo)彈的速度分量 Vmy=Vm*cos(SightAngle-LeadAngle+HeadError); Vtmx=Vtx-Vmx; Vtmy=Vty-Vmy; %彈目的相對運(yùn)動速度,Vc=-(Rtmx*Vtmx+Rtmy*Vtmy)/Rtm; SignVc=sign(Vc); %Vc的符號 Time

31、=0;TimeStep=0.1; %時間和時間步長 file=fopen(output.txt,w);%將數(shù)據(jù)寫入文件 %循環(huán) while(1) %Vc改變符號仿真結(jié)束 if(sign(Vc) = SignVc) break; else,if(Rtm100) TimeStep=0.005; end SignVc=sign(Vc); %Vc的符號 %視線角速率 dSightAngle=(Rtmy*Vtmx-Rtmx*Vtmy)/(Rtm2); dTheta=3*Vc*dSightAngle/Vm; Theta=atan(Vmx/Vmy); %導(dǎo)彈加速度,Am=Vm*dTheta; %限制機(jī)動能力 if(AmAmMax) Am=AmMax; end