1、第十二章第十二章 用用MATLABMATLAB解最解最優(yōu)控制問(wèn)題及應(yīng)用實(shí)例優(yōu)控制問(wèn)題及應(yīng)用實(shí)例第十二章第十二章 用用MATLABMATLAB解最優(yōu)解最優(yōu)控制問(wèn)題及應(yīng)用實(shí)例控制問(wèn)題及應(yīng)用實(shí)例12.1 MATLAB12.1 MATLAB工具簡(jiǎn)介工具簡(jiǎn)介12.2 12.2 用用MATLABMATLAB解線性二次型最優(yōu)控制問(wèn)題解線性二次型最優(yōu)控制問(wèn)題12.3 12.3 用用MATLABMATLAB解最優(yōu)控制問(wèn)題應(yīng)用實(shí)例解最優(yōu)控制問(wèn)題應(yīng)用實(shí)例12.4 12.4 小結(jié)小結(jié) MATLAB是集數(shù)值運(yùn)算、符號(hào)運(yùn)算及圖形處理等強(qiáng)大功能于一體的科學(xué)計(jì)算語(yǔ)言。作為強(qiáng)大的科學(xué)計(jì)算平臺(tái)，它幾乎能滿足所有的計(jì)算需求。MAT

2、LAB具有編程方便、操作簡(jiǎn)單、可視化界面、優(yōu)良的仿真圖形環(huán)境、豐富的多學(xué)科工具箱等優(yōu)點(diǎn)，尤其是在自動(dòng)控制領(lǐng)域中MATLAB顯示出更為強(qiáng)大的功能。 最優(yōu)控制是在一定的約束條件下，從已給定的初始狀態(tài)出發(fā)，確定最優(yōu)控制作用的函數(shù)式，使目標(biāo)函數(shù)為極小或極大。在設(shè)計(jì)最優(yōu)控制器的過(guò)程中，運(yùn)用MATLAB最優(yōu)控制設(shè)計(jì)工具，會(huì)大大減小設(shè)計(jì)的復(fù)雜性。 在前面的幾章中，我們已經(jīng)介紹了一些最優(yōu)控制方法，在本章中我們將介紹一個(gè)最優(yōu)控制問(wèn)題的應(yīng)用實(shí)例，討論如何使用最優(yōu)控制方法來(lái)設(shè)計(jì)自尋的制導(dǎo)導(dǎo)彈的最優(yōu)導(dǎo)引律，并采用MATLAB工具實(shí)現(xiàn)最優(yōu)導(dǎo)引律，通過(guò)仿真來(lái)驗(yàn)證最優(yōu)導(dǎo)引律的有效性。12.1 MATLAB12.1 MATL

3、AB工具簡(jiǎn)介工具簡(jiǎn)介DuCxyBuAxx 1, 系統(tǒng)模型的建立 系統(tǒng)的狀態(tài)方程為： 在MATLAB中只需要將各個(gè)系數(shù)按照常規(guī)矩陣的方式輸入到工作空間即可 ss(A,B,C,D),;,;,;,;,;,;,;,;,212222111211212222111211212222111211212222111211qpqqppqnqqnnnpnnppnnnnnndddddddddDcccccccccCbbbbbbbbbBaaaaaaaaaA傳遞函數(shù)的零極點(diǎn)模型為：)()()()()(2121nmpspspszszszsKsG在MATLAB中可以采用如下語(yǔ)句將零極點(diǎn)模型輸入到工作空間:;2121nmppp

4、PzzzZKKGainzpk(Z,P,KGain)傳遞函數(shù)模型在更一般的情況下，可以表示為復(fù)數(shù)變量s的有理函數(shù)形式：nnnnnmmmmasasasasbsbsbsbsG122111121)(在MATLAB中可以采用如下語(yǔ)句將以上的傳遞函數(shù)模型輸入到工作空間:G=tf(num,den);, 1 ;,121121nnmmaaaadenbbbbnum2, 系統(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)由傳

5、遞函數(shù) (num,den) 描述 roots(den) 系統(tǒng)由狀態(tài)方程 (A,B,C,D) 描述 eig(A)4, 系統(tǒng)的可控性與可觀測(cè)性分析 在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)的可觀測(cè)階梯變換，該函數(shù)的調(diào)用格式為： Ao,Bo,Co,Do,To,Ko=obsvf(A,B,C)5, 系統(tǒng)的時(shí)域分析 對(duì)于系統(tǒng)的階躍響應(yīng)，控制系統(tǒng)工具箱中給出了一個(gè)函數(shù)step()來(lái)直接求取系統(tǒng)的階躍

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

7、st(G,w) 對(duì)于Bode圖，MATLAB控制工具箱中提供了bode()函數(shù)來(lái)求取、繪制系統(tǒng)的Bode圖，該函數(shù)可以由下面的格式來(lái)調(diào)用 mag,pha=bode(G,w)12.2 12.2 用用MATLABMATLAB解線性二次型最優(yōu)控制問(wèn)題解線性二次型最優(yōu)控制問(wèn)題一般情況的線性二次問(wèn)題可表示如下：設(shè)線性時(shí)變系統(tǒng)的方程為其中， 為 維狀態(tài)向量， 為 維控制向量， 為維輸出向量。 )()()()()(tUtBtXtAtX)()()(tXtCtY( )X tn( )U tm)(tYl尋找最優(yōu)控制，使下面的性能指標(biāo)最小fttTTffTdttUtRtUtetQtetPeteuJ0)()()()()(

8、)(21)()(21)(l其中， 是 對(duì)稱半正定常數(shù)陣， 是 對(duì)稱半正定陣, 是 對(duì)稱正定陣。l ll l)(tQ)(tRmmP 我們用最小值原理求解上述問(wèn)題，可以把上述問(wèn)題歸結(jié)為求解如下黎卡提（Riccati）矩陣微分方程：1( )( ) ( )( ) ( )( ) ( )( )( ) ( )( )TTK tK t AtA t K tK t Bt R t B t K tQt可以看出，上述的黎卡提矩陣微分方程求解起來(lái)非常困難，所以我們往往求出其穩(wěn)態(tài)解。例如目標(biāo)函數(shù)中指定終止時(shí)間可以設(shè)置成 ，這樣可以保證系統(tǒng)狀態(tài)漸進(jìn)的趨近于零值，這樣可以得出矩陣趨近于常值矩陣 ，且 ，這樣上述黎卡提矩陣微分方程

9、可以簡(jiǎn)化成為：ft)(tK0)(tK1( ) ( )( )( )( ) ( )( )( )( )( )0TTK t A tAt K tK t B t Rt Bt K tQ t這個(gè)方程稱為代數(shù)黎卡提方程。代數(shù)黎卡提方程的求解非常簡(jiǎn)單，并且其求解只涉及到矩陣運(yùn)算，所以非常適合使用MATLAB來(lái)求解。方法一：方法一：求解代數(shù)黎卡提方程的算法有很多，下面我們介紹一種簡(jiǎn)單的迭代算法來(lái)解該方程，令 ，則可以寫(xiě)出下面的迭代公式00 11TTTTiiiiiEEEGWGGHEGWQAIAIE 1BAIG12BAIQAIBRHTT11)(BAIQW1如果 收斂于一個(gè)常數(shù)矩陣，即 ，則可以得出代數(shù)黎卡提方程的解為：

10、上面的迭代算法可以用MATLAB來(lái)實(shí)現(xiàn)：1iii 11112AIAIPiT%*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; endendP=2*iA*P*iA;我們把這個(gè)文件命名為mylq.m，方便我們以后調(diào)用來(lái)求解代數(shù)黎卡提方程。方法二：

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

12、方程，可以容易得出VTM,0VXTXXMMXTAMTBBRT1QV方法三：方法三： 我們也可以采用care()函數(shù)對(duì)連續(xù)時(shí)間代數(shù)黎卡提 方程求解，其調(diào)用方法如下：P,E,K,RR=care(A,B,Q,R,zeros(size(B),eye(size(A) 式中輸入矩陣為A,B,Q,R，其中(A,B)為給定的對(duì)象狀態(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)計(jì)算）。 采用care函數(shù)的

13、優(yōu)點(diǎn)在于可以設(shè)置P的終值條件，例如我們可以在下面的程序中設(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-112-1 線性系統(tǒng)為： ，其目標(biāo)函數(shù)是：uxx10351006667. 1 10020020050021dtuuxxJTT確定最優(yōu)控制。解：解：方法一：A=0 1;-5,-3;B=0;1;Q=500 200;200 100;R=1.6667;mylqK=inv(R)*B*PPE運(yùn)行結(jié)果：K = 13.0276 6.7496P = 67.9406 21.713

14、1 21.7131 11.2495E = -0.1111 0.2222 -1.1111 -0.7778方法二: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.7496P = 67.9406 21.7131 21.7131 11.2495E =-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 =

15、67.9406 21.7131 21.7131 11.2495E = -7.2698 -2.4798K =13.0276 6.7496RR = 2.8458e-015 以上的三種方法的運(yùn)行結(jié)果相同。我們可以得到，最優(yōu)控制變量與狀態(tài)變量之間的關(guān)系： 在以上程序的基礎(chǔ)上，可以得到在最優(yōu)控制的作用下的最優(yōu)控制曲線與最優(yōu)狀態(tài)曲線，其程序如下：)(6.7496)(13.0276)(21*txtxtu%*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

16、,cp,dp=augstate(ap,bp,C,D);cp=cp;-K;dp=dp;0;G=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é)果：運(yùn)行結(jié)果： 圖圖12-1 12-1 最優(yōu)控制曲線與最優(yōu)狀態(tài)曲線最優(yōu)控制曲線與最優(yōu)狀態(tài)曲線 該程序采用augstate函數(shù)將狀態(tài)變量作為輸出變量，用于顯示；輸出項(xiàng)作為最優(yōu)控制的輸出。因此，階躍響應(yīng)輸出y中，y(1)是系統(tǒng)輸出

17、，y(2)和y(3)是狀態(tài)變量輸出，y(4)是系統(tǒng)控制變量輸出。用plotyy函數(shù)進(jìn)行雙坐標(biāo)顯示，并設(shè)置相應(yīng)的坐標(biāo)范圍。 以上三種方法中，第一種方法易于理解黎卡提方程的解法，其解法簡(jiǎn)單但是并不可靠。第二種方法比起另兩種方法使用方便，不易出錯(cuò)，所以我們推薦使用這種方法。但是采用lqr()函數(shù)不能設(shè)置代數(shù)黎卡提方程的邊界條件，所以，如果題目設(shè)置了P的終值條件，我們只能使用第三種方法來(lái)求解，例如設(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

18、;0.2,eye(size(A)運(yùn)行結(jié)果：P =67.7233 21.5685 21.5685 11.0961E =-7.3052 -2.4723K =13.0608 6.7775RR =1.2847e-014最優(yōu)控制變量與狀態(tài)變量之間的關(guān)系：)(6.7775)(13.0608)(21*txtxtu例例12-212-2 無(wú)人飛行器的最優(yōu)高度控制，飛行器的控制方程如下 )(2/100)()()(2/100100010)()()(tuthththththth 是飛行器的高度； 是油門(mén)輸入；設(shè)計(jì)控制律使得如下指標(biāo)最小)(th)(tudttuththththththtutxJ02)(2)()()(00

19、0000001)(),(),(21)(),( 初始狀態(tài) 。繪制系統(tǒng)狀態(tài)與控制輸入，對(duì)如下給定的 矩陣進(jìn)行仿真分析.Tththth0 , 0 ,10)(),()( ，,QRa).b).c).d).100000 ,R2000Q2000R,000000001Q2,0000000010RQ10001000 ,2000QR解：解：線性二次型最優(yōu)控制指標(biāo)如下： 其中Q和R分別是對(duì)狀態(tài)變量和控制量的加權(quán)矩陣， 線性二次型最優(yōu)控制器設(shè)計(jì)如下：1）、Q=diag（1，0，0），R=2時(shí)，由MATLAB求得最優(yōu)狀態(tài)反饋矩陣為 k1=0.7071 2.0772 2.0510， u（t）=k1*x（t）； 所畫(huà)狀態(tài)響

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

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

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

23、幅值變大，達(dá)到穩(wěn)態(tài)所需時(shí)間變短； 當(dāng)Q的對(duì)角線上第2個(gè)元素變大時(shí)，各曲線波動(dòng)幅值變小；達(dá)到穩(wěn)態(tài)所需時(shí)間，狀態(tài)x1，x2增長(zhǎng)，狀態(tài)x3，控制輸入u變短； 當(dāng)R變大時(shí)，反饋矩陣變小；各曲線波動(dòng)幅值變??；達(dá)到穩(wěn)態(tài)所需時(shí)間變長(zhǎng)。 所以根據(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,x

24、0,20);n=length(x(:,3);T=0:20/n:20-20/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時(shí)狀態(tài)響應(yīng)曲線)grid,hold onfor j=1:n u(j,:)=-k*(x(j,:);endfigure(2)plot(T,u);xlabel(time-s);ylabel(response);title(圖(1.b) Q=diag(1,0,0),R=2時(shí)控制輸入u的響應(yīng)曲線)grid,h

25、old on%*figure(3)qa=1 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時(shí)狀態(tài)響應(yīng)曲線)grid,hold onf

26、or j=1:na ua(j,:)=-ka*(xa(j,:);endfigure(4)plot(Ta,ua);xlabel(time-s);ylabel(response);title(圖(2.b) Q=diag(1,0,0),R=2000時(shí)控制輸入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;plot(

27、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時(shí)狀態(tài)響應(yīng)曲線)grid,hold onfor j=1:nb ub(j,:)=-kb*(xb(j,:);endfigure(6)plot(Tb,ub);xlabel(time-s);ylabel(response);title(圖(3.b) Q=diag(10,0,0),R=2時(shí)控制輸入u的響應(yīng)曲線)grid,hold on%*figure(7)qc=1 0 0;0 10

28、0 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時(shí)狀態(tài)響應(yīng)曲線)grid,hold onfor j=1:nc uc(j,:)=-kc*(xc(j,:);e

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

30、的彈目視線，并且正比于導(dǎo)彈與目標(biāo)之間的視線角速率 ，假設(shè)目標(biāo)的法向加速度為零，那么可得： (12-1)q qN其中， 為導(dǎo)彈的速度與基準(zhǔn)方向的夾角， 為導(dǎo)彈與目標(biāo)連線與基準(zhǔn)方向的夾角，稱為視線角， 是視線角速率， 是比例常數(shù)，稱為導(dǎo)航比，通常為36。比例導(dǎo)引的實(shí)質(zhì)是使導(dǎo)彈向著 減小的方向運(yùn)動(dòng)，抑制視線旋轉(zhuǎn)，也就是使導(dǎo)彈的相對(duì)速度對(duì)準(zhǔn)目標(biāo)，保證導(dǎo)彈向著前置碰撞點(diǎn)飛行。qq Nq 比例導(dǎo)引法是經(jīng)典的導(dǎo)引方法。下面我們從最優(yōu)控制理論的觀點(diǎn)來(lái)研究自尋的導(dǎo)彈的最優(yōu)導(dǎo)引規(guī)律問(wèn)題。12.3.1 12.3.1 導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程的建立導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程的建立 導(dǎo)彈與目標(biāo)的運(yùn)動(dòng)關(guān)系是非線性的，如果把導(dǎo)彈與目標(biāo)的運(yùn)動(dòng)

31、方程相對(duì)于理想彈道線性化，可得導(dǎo)彈運(yùn)動(dòng)的線性狀態(tài)方程. 假設(shè)導(dǎo)彈和目標(biāo)在同一平面內(nèi)運(yùn)動(dòng)，如圖12-6所示。選 為固定坐標(biāo)。導(dǎo)彈速度向量 與 軸成 角，目標(biāo)速度向量 為與 軸成 角。導(dǎo)彈與目標(biāo)的連線 與軸 成 角。假定導(dǎo)彈以尾追的方式攻擊目標(biāo)。坐標(biāo)軸 和 的方向可以任意選擇，使 和 都比較小。再假定導(dǎo)彈和目標(biāo)均勻速飛行，也就是說(shuō) 和 均為恒值。使用相對(duì)坐標(biāo)狀態(tài)變量，設(shè) 為導(dǎo)彈與目標(biāo)在 軸方向上的距離偏差， 為導(dǎo)彈與目標(biāo)在 軸方向上的距離偏差，即 (12-2)oxyMVoyTVoyTMToyqoxoyT,qMVTVxoxyoyMTMTyyyxxxxxyyMxTxMyTyMVTV0qTRTMH圖圖1

32、2-6 12-6 導(dǎo)彈和目標(biāo)運(yùn)動(dòng)幾何關(guān)系圖導(dǎo)彈和目標(biāo)運(yùn)動(dòng)幾何關(guān)系圖假定 和 比較小，因此，則1cos, 1cos,sin,sinTTTT將上式對(duì)t求導(dǎo)，并根據(jù)導(dǎo)彈和目標(biāo)的關(guān)系（如圖12-6所示）可得 (12-3) coscossinsinMTTMTMTTMTVVyyyVVxxxMTMTMTTMTVVyyyVVxxx (12-4)以 表示 ， 表示 （即 ），則 (12-5) (12-6)1xx2xx 1x 21xx MTTVVxx2式中 表示目標(biāo)的橫向加速度， 表示導(dǎo)彈橫向加速度，分別以 和 表示，那么 (12-7)TTVMVTaMaMTaax2導(dǎo)彈的橫向加速度 為一控制量。一般將控制信號(hào)加給

33、舵機(jī)，舵面偏轉(zhuǎn)后產(chǎn)生彈體攻角 ，而后產(chǎn)生橫向加速度 。如果忽略舵機(jī)和彈體的慣性，而且假設(shè)控制量的單位與加速度單位相同，則可用控制量 來(lái)表示 ，也就是令 (12-8) 所以(12-7)式為： (12-9)MaMauMaMauuaxT2這樣可得導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程為： (12-10) (12-11)21xx Taux2可寫(xiě)成矩陣的形式： (12-12)式中， ， ， ， 。 (12-13)如果不考慮目標(biāo)的機(jī)動(dòng)，即 ，則在這種情況下，式(12-12)變成： (12-14)TDaBuAXX21xxX0010A10B10D0TaBuAXX下面來(lái)考慮(12-4)式，該式可寫(xiě)成 (12-15) 其中 表示導(dǎo)彈相

34、對(duì)目標(biāo)的接近速度。由于 的值都比較小， 可近似表示導(dǎo)彈與目標(biāo)之間的距離。設(shè) 為導(dǎo)彈與目標(biāo)的遭遇時(shí)刻(即導(dǎo)彈與目標(biāo)相碰撞或兩者之間的距離為最短的時(shí)刻)，則在某一瞬時(shí) ，導(dǎo)彈與目標(biāo)的距離 可近似用下式表示： (12-16)(TMVVyCTMVVVT,和qyftty)()()(ttVttVVtyfCfTM又考慮到對(duì)于導(dǎo)彈制導(dǎo)來(lái)說(shuō)，最基本的要求是脫靶量越小越好，因此，應(yīng)該選擇最優(yōu)控制量 ，使得下面的指標(biāo)函數(shù)為最小。 (12-17)u22)()()()(fMfTfMfTtytytxtxJ然而，當(dāng)要求一個(gè)反饋形式的控制時(shí)，按上式列出的問(wèn)題很難求解。所以我們以 時(shí)刻，即 時(shí) 的值 作為脫靶量，要求 值越小越

35、好。另外，由于舵偏角受到限制，導(dǎo)彈結(jié)構(gòu)能夠承受的最大載荷也受到限制，所以控制信號(hào) 也應(yīng)該受到限制。 ftt 0)()(ffCfttVty)(1ftx)(1ftxu因此，我們選擇以下形式的二次型指標(biāo)函數(shù)： (12-18)式中 ， 。 (12-19)即 (12-20)給定初始條件 ，應(yīng)用最優(yōu)控制理論，可以求出使 為最小的 。dtRuuQXXtCXtXJfttTTffT021212100ccC0000QdtRutCXtXJfttffT022121 0tXJu 由于系統(tǒng)是線性的，指標(biāo)函數(shù)是二次型的，因此，求最優(yōu)控制規(guī)律就可以認(rèn)為是一個(gè)求解線性二次型的過(guò)程。 對(duì)于線性二次型問(wèn)題，可采用變分法、極小值原理

36、、動(dòng)態(tài)規(guī)劃或其他方法求得最優(yōu)控制 (12-21)式中 滿足下列黎卡提矩陣微分方程 (12-22) 的終端條件為 (12-23)因此求解線性二次型問(wèn)題的關(guān)鍵是求解黎卡提矩陣微分方程。 KXBRtuT1*QKBKBRKAKAKTT1KKCtKf12.3.2 12.3.2 最優(yōu)導(dǎo)引律的求解與仿真驗(yàn)證最優(yōu)導(dǎo)引律的求解與仿真驗(yàn)證當(dāng)不考慮彈體慣性時(shí)，而且假定目標(biāo)不機(jī)動(dòng)，即，導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程為 (12-24)BuAXX指標(biāo)函數(shù)為 (12-25)式中 ， , ， 。 dtRuuQXXtCXtXJfttTTffT021210010A10B2100ccC0000Q給出 時(shí)刻， 的初值 ，采用極小值原理可求得最優(yōu)控

37、制 為 (12-26)0tt 21xx 和 0201txtx和 tu* 2321 21 211212*34211 22231312fffffffcc ttcc ttc ttxcc ttxRRu tc ttc ttcc ttRRRR 在指標(biāo)函數(shù)中，如不考慮導(dǎo)彈的相對(duì)運(yùn)動(dòng)速度 項(xiàng)，則可令 。 變成 (12-27)以 除上式的分子和分母，得 (12-28)2x02c tu* RttcRxttcxttctufff313122111*1c 31221*333ttcRxttxtttufff為了使脫靶量為最小，應(yīng)選取 ，則 (12-29)根據(jù)圖12-6可得 1c ttxttxtuff221*3ttVxyxt

38、gqfc11當(dāng) 比較小時(shí)， ，則 (12-30) (12-31)qqtgq ttVxqfc1ttxttxVttVxttxqffcfcf2212111將上式代入(12-29)式，可得 (12-32)在上式中， 的單位是加速度的單位 。把 與導(dǎo)彈速度向量 的旋轉(zhuǎn)角速度 聯(lián)系起來(lái)，則有 (12-33) qVtuc3*u2秒米DVqVVVuMcM3u從(12-32)和(12-33)式可以看出，當(dāng)不考慮彈體慣性時(shí)，最優(yōu)導(dǎo)引規(guī)律就是比例導(dǎo)引，其導(dǎo)航比為 。這證明了比例導(dǎo)引是一種很好的導(dǎo)引方法。最優(yōu)導(dǎo)引規(guī)律的形成可用圖12-7來(lái)表示。McVV3 下面將對(duì)最優(yōu)導(dǎo)引律進(jìn)行MATLAB仿真，并給出源代碼和仿真結(jié)果

39、。 cV3s1s1ttVfc121ttVfcq*u1x2x圖圖12-7 12-7 最優(yōu)導(dǎo)引方框圖最優(yōu)導(dǎo)引方框圖xxyyMxTxMyTyMVTV0HqTTMRTaMaMTLHE圖圖12-8 12-8 最優(yōu)導(dǎo)引攻擊幾何平面最優(yōu)導(dǎo)引攻擊幾何平面最優(yōu)導(dǎo)引攻擊幾何關(guān)系如圖12-8所示，在這里討論的目標(biāo)和導(dǎo)彈均認(rèn)為是二維攔截幾何平面上的質(zhì)點(diǎn)，分別以速度 和 運(yùn)動(dòng)。導(dǎo)彈的初始位置為相對(duì)坐標(biāo)系的參考點(diǎn)，導(dǎo)彈初始速度矢量指向目標(biāo)的初始位置， 為導(dǎo)彈的指令（垂直于視線）。TVMVMa其中： (12-34) (12-35) (12-36) 為目標(biāo)速度在 軸上的分解， 是目標(biāo)的角度。導(dǎo)彈和目標(biāo)之間的接近速度為： (1

40、2-37)TTTVaTTTyVVcosTTTxVVsinTcTMVR ,TxTyVV, x y目標(biāo)的速度分量可由其位置變化得到： (12-38)同樣地，我們可以得到導(dǎo)彈的位置和速度的微分方程： ， (12-39) ， (12-40)TxTxTyTyVRVR,MxMxaVMyMyaVMxMxVRMyMyVR上面幾式中的下標(biāo)x，y分別表示在x和y軸上的分量。 是導(dǎo)彈在地球坐標(biāo)系的加速度分量。為了得到導(dǎo)彈的加速度分量，我們必須得到彈目的相對(duì)位移： (12-41) (12-42)MyMxaa,MxTxTMxRRRMyTyTMyRRRTMyTMxRRq1tanMxTxTMxVVVMyTyTMyVVV從圖

41、12-8中，根據(jù)三角關(guān)系我們可以得到視線角： (12-43)如果定義地球坐標(biāo)系的速度分量為： (12-44) (12-45)我們可以根據(jù)視線角的公式求導(dǎo)后得到視線角速率： (12-46) (12-47)所以我們不難得出彈目的接近速度為： (12-48)TMTMyTMxTMxTMyRVRVRq22122)(TMyTMxTMRRRTMTMyTMyTMxTMxTMcRVRVRRV)(根據(jù)最優(yōu)導(dǎo)引制導(dǎo)律： (12-49)可得到導(dǎo)彈的加速的分量為： (12-50) (12-51) (12-52)qVVMc3)(tan1MyMxVVqVaMMxcosqVaMMysin 以上列出了兩維的最優(yōu)導(dǎo)引制導(dǎo)的必要方

42、程，但是使用最優(yōu)導(dǎo)引制導(dǎo)的導(dǎo)彈并不是直接向著目標(biāo)發(fā)射的，而是向著一個(gè)能夠?qū)б龑?dǎo)彈命中目標(biāo)的方向發(fā)射，考慮了視線角之后可以得到導(dǎo)彈的指向角L。從圖12-8中我們可以看出，如果導(dǎo)彈進(jìn)入了碰撞三角區(qū)（如果目標(biāo)和導(dǎo)彈同時(shí)保持勻速直線運(yùn)動(dòng)，導(dǎo)彈必定會(huì)命中目標(biāo)），這時(shí)利用正弦公式可以得到指向角的表達(dá)式：MTTVqVL)sin(sin1(12-53) 但是實(shí)際上導(dǎo)彈不可能能確切地在碰撞三角區(qū)發(fā)射，所以不能精確地得到攔截點(diǎn)。因?yàn)槲覀儾恢滥繕?biāo)將會(huì)如何機(jī)動(dòng)，所以攔截點(diǎn)位置只能大概地估計(jì)。事實(shí)上，這也是需要導(dǎo)航系統(tǒng)的原因！初始時(shí)刻導(dǎo)彈偏離碰撞三角的角度稱之為指向角誤差（Head-Error）?？紤]了導(dǎo)彈初始時(shí)刻的

43、指向角和指向角誤差之后，導(dǎo)彈的初始速度分量可以表示為：)cos()0(HeadErrorLqVVMMy)sin()0(HeadErrorLqVVMMx(12-54)(12-55)使用MATLAB編程，具體代碼如下：%*MATLAB程序*%最優(yōu)制導(dǎo)律仿真，初始化系統(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; %彈目相對(duì)距離Rtmy=Rty-Rmy;AmMax=15*9.8; %導(dǎo)彈的最大機(jī)動(dòng)能力為15GRtm=sqrt(Rtmx2+Rtmy2);SightAngle=atan(Rtmx/Rtmy); %視線角LeadAngle=asin(Vt