用MATLAB解最優(yōu)控制問題及應(yīng)用實(shí)例課件

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

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

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

12.1MATLAB工具簡(jiǎn)介12.2用MATLAB解線性二次型最優(yōu)控制問題12.3用MATLAB解最優(yōu)控制問題應(yīng)用實(shí)例12.4小結(jié)2第十二章用MATLAB解最優(yōu)控制問題及應(yīng)用實(shí)例

12.1MATLAB是集數(shù)值運(yùn)算、符號(hào)運(yùn)算及圖形處理等強(qiáng)大功能于一體的科學(xué)計(jì)算語(yǔ)言。作為強(qiáng)大的科學(xué)計(jì)算平臺(tái)，它幾乎能滿足所有的計(jì)算需求。MATLAB具有編程方便、操作簡(jiǎn)單、可視化界面、優(yōu)良的仿真圖形環(huán)境、豐富的多學(xué)科工具箱等優(yōu)點(diǎn)，尤其是在自動(dòng)控制領(lǐng)域中MATLAB顯示出更為強(qiáng)大的功能。3MATLAB是集數(shù)值運(yùn)算、符號(hào)運(yùn)算及圖形處理等強(qiáng)

最優(yōu)控制是在一定的約束條件下，從已給定的初始狀態(tài)出發(fā)，確定最優(yōu)控制作用的函數(shù)式，使目標(biāo)函數(shù)為極小或極大。在設(shè)計(jì)最優(yōu)控制器的過程中，運(yùn)用MATLAB最優(yōu)控制設(shè)計(jì)工具，會(huì)大大減小設(shè)計(jì)的復(fù)雜性。在前面的幾章中，我們已經(jīng)介紹了一些最優(yōu)控制方法，在本章中我們將介紹一個(gè)最優(yōu)控制問題的應(yīng)用實(shí)例，討論如何使用最優(yōu)控制方法來設(shè)計(jì)自尋的制導(dǎo)導(dǎo)彈的最優(yōu)導(dǎo)引律，并采用MATLAB工具實(shí)現(xiàn)最優(yōu)導(dǎo)引律，通過仿真來驗(yàn)證最優(yōu)導(dǎo)引律的有效性。4最優(yōu)控制是在一定的約束條件下，從已給定的初始狀態(tài)12.1MATLAB工具簡(jiǎn)介

1,系統(tǒng)模型的建立系統(tǒng)的狀態(tài)方程為：512.1MATLAB工具簡(jiǎn)介

1,系統(tǒng)模型的建立5

在MATLAB中只需要將各個(gè)系數(shù)按照常規(guī)矩陣的方式輸入到工作空間即可

ss(A,B,C,D)6在MATLAB中只需要將各個(gè)系數(shù)按照常規(guī)矩陣的方傳遞函數(shù)的零極點(diǎn)模型為：在MATLAB中可以采用如下語(yǔ)句將零極點(diǎn)模型輸入到工作空間:zpk(Z,P,KGain)7傳遞函數(shù)的零極點(diǎn)模型為：在MATLAB中可以采用如下語(yǔ)句將零傳遞函數(shù)模型在更一般的情況下，可以表示為復(fù)數(shù)變量s的有理函數(shù)形式：8傳遞函數(shù)模型在更一般的情況下，可以表示為復(fù)數(shù)變量s的有理函數(shù)在MATLAB中可以采用如下語(yǔ)句將以上的傳遞函數(shù)模型輸入到工作空間:G=tf(num,den);9在MATLAB中可以采用如下語(yǔ)句將以上的傳遞函數(shù)模型輸入到工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)102,系統(tǒng)模型的轉(zhuǎn)換103,系統(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)113,系統(tǒng)穩(wěn)定性判據(jù)114,系統(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)124,系統(tǒng)的可控性與可觀測(cè)性分析125,系統(tǒng)的時(shí)域分析對(duì)于系統(tǒng)的階躍響應(yīng)，控制系統(tǒng)工具箱中給出了一個(gè)函數(shù)step()來直接求取系統(tǒng)的階躍響應(yīng)，該函數(shù)的可以有如下格式來調(diào)用：

y=step(G,t)

對(duì)于系統(tǒng)的脈沖響應(yīng)，控制系統(tǒng)工具箱中給出了一個(gè)函數(shù)impulse()來直接求取系統(tǒng)的脈沖響應(yīng)，該函數(shù)的可以有如下格式來調(diào)用：

y=impulse(G,t) 135,系統(tǒng)的時(shí)域分析136,系統(tǒng)的復(fù)域與頻域分析對(duì)于根軌跡的繪制，控制系統(tǒng)工具箱中給出了一個(gè)函數(shù)rlocus()函數(shù)來繪制系統(tǒng)的根軌跡，該函數(shù)的可以由如下格式來調(diào)用：

R=rlocus(G,k)146,系統(tǒng)的復(fù)域與頻域分析14

對(duì)于Nyquist曲線的繪制，控制系統(tǒng)工具箱中給出了一個(gè)函數(shù)nyquist()函數(shù)，該環(huán)數(shù)可以用來直接求解Nyquist陣列，繪制出Nyquist曲線，該函數(shù)的可以由如下格式來調(diào)用：

[rx,ry]=nyquist(G,w)

對(duì)于Bode圖，MATLAB控制工具箱中提供了bode()函數(shù)來求取、繪制系統(tǒng)的Bode圖，該函數(shù)可以由下面的格式來調(diào)用

[mag,pha]=bode(G,w)15對(duì)于Nyquist曲線的繪制，控制系統(tǒng)工具箱中給12.2用MATLAB解線性二次型最優(yōu)控制問題

一般情況的線性二次問題可表示如下：設(shè)線性時(shí)變系統(tǒng)的方程為其中，為維狀態(tài)向量，為維控制向量，為維輸出向量。1612.2用MATLAB解線性二次型最優(yōu)控制問題

一般情況尋找最優(yōu)控制，使下面的性能指標(biāo)最小其中，是對(duì)稱半正定常數(shù)陣，是對(duì)稱半正定陣,是對(duì)稱正定陣。17尋找最優(yōu)控制，使下面的性能指標(biāo)最小其中，是對(duì)稱半正

我們用最小值原理求解上述問題，可以把上述問題歸結(jié)為求解如下黎卡提（Riccati）矩陣微分方程：18我們用最小值原理求解上述問題，可以把上述問題歸結(jié)可以看出，上述的黎卡提矩陣微分方程求解起來非常困難，所以我們往往求出其穩(wěn)態(tài)解。例如目標(biāo)函數(shù)中指定終止時(shí)間可以設(shè)置成，這樣可以保證系統(tǒng)狀態(tài)漸進(jìn)的趨近于零值，這樣可以得出矩陣趨近于常值矩陣，且，這樣上述黎卡提矩陣微分方程可以簡(jiǎn)化成為：19可以看出，上述的黎卡提矩陣微分方程求解起來非常困難，所以我們這個(gè)方程稱為代數(shù)黎卡提方程。代數(shù)黎卡提方程的求解非常簡(jiǎn)單，并且其求解只涉及到矩陣運(yùn)算，所以非常適合使用MATLAB來求解。20這個(gè)方程稱為代數(shù)黎卡提方程。代數(shù)黎卡提方程的求解非常簡(jiǎn)單，并方法一：求解代數(shù)黎卡提方程的算法有很多，下面我們介紹一種簡(jiǎn)單的迭代算法來解該方程，令，則可以寫出下面的迭代公式21方法一：求解代數(shù)黎卡提方程的算法有很多，下面我們介紹一種簡(jiǎn)單2222如果收斂于一個(gè)常數(shù)矩陣，即，則可以得出代數(shù)黎卡提方程的解為：上面的迭代算法可以用MATLAB來實(shí)現(xiàn)：23如果收斂于一個(gè)常數(shù)矩陣，即%***************MATLAB程序***************%I=eye(size(A));iA=inv(I-A);E=iA*(I+A);G=2*iA^2*B;H=R+B'*iA'*Q*iA*B;W=Q*iA*B;P0=zeros(size(A));i=0;24%***************MATLAB程序******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)用來求解代數(shù)黎卡提方程。25while(1),i=i+1;25方法二：在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)。26方法二：26這里的求解是建立在MATLAB的控制系統(tǒng)工具箱中給出的一個(gè)基于Schur變換的黎卡提方程求解函數(shù)are()基礎(chǔ)上的，該函數(shù)的調(diào)用格式為：

X=are(M,T,V)27這里的求解是建立在MATLAB的控制系統(tǒng)工具箱中給出的一個(gè)基其中，矩陣滿足下列代數(shù)黎卡提方程，are是AlgebraicRiccatiEquation的縮寫。對(duì)比前面給出的黎卡提方程，可以容易得出28其中，矩陣滿足下列代數(shù)黎卡提方程，are是Al方法三：我們也可以采用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ì)算）。29方法三：29

采用care函數(shù)的優(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ù)黎卡提方程的邊界條件。30采用care函數(shù)的優(yōu)點(diǎn)在于可以設(shè)置P的終值條件例12-1

線性系統(tǒng)為：，其目標(biāo)函數(shù)是：確定最優(yōu)控制。31例12-1 線性系統(tǒng)為：解：方法一：A=[01;-5,-3];B=[0;1];Q=[500200;200100];R=1.6667;mylqK=inv(R)*B'*PPE32解：32運(yùn)行結(jié)果：K=13.02766.7496P=67.940621.713121.713111.2495E=-0.11110.2222-1.1111-0.777833運(yùn)行結(jié)果：33方法二:A=[01;-5,-3];B=[0;1];Q=[500200;200100];R=1.6667;[K,P,E]=lqr(A,B,Q,R)34方法二:34運(yùn)行結(jié)果：K=13.02766.7496P=67.940621.713121.713111.2495E=-7.2698-2.479835運(yùn)行結(jié)果：35方法三：A=[01;-5,-3];B=[0;1];Q=[500200;200100];R=1.6667;[P,E,K,RR]=care(A,B,Q,R,zeros(size(B)),eye(size(A)))36方法三：36運(yùn)行結(jié)果：P=67.940621.713121.713111.2495E=-7.2698-2.4798K=13.02766.7496RR=2.8458e-01537運(yùn)行結(jié)果：37

以上的三種方法的運(yùn)行結(jié)果相同。我們可以得到，最優(yōu)控制變量與狀態(tài)變量之間的關(guān)系：在以上程序的基礎(chǔ)上，可以得到在最優(yōu)控制的作用下的最優(yōu)控制曲線與最優(yōu)狀態(tài)曲線，其程序如下：38以上的三種方法的運(yùn)行結(jié)果相同。我們可以得%***************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;39%***************MATLAB程序******[ap,bp,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),[02.500.1]),axis(ax(2),[02.5-10])40[ap,bp,cp,dp]=augstate(ap,bp,C運(yùn)行結(jié)果：

圖12-1最優(yōu)控制曲線與最優(yōu)狀態(tài)曲線41運(yùn)行結(jié)果：41

該程序采用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)范圍。42該程序采用augstate函數(shù)將狀態(tài)變量作為輸出

以上三種方法中，第一種方法易于理解黎卡提方程的解法，其解法簡(jiǎn)單但是并不可靠。第二種方法比起另兩種方法使用方便，不易出錯(cuò)，所以我們推薦使用這種方法。但是采用lqr()函數(shù)不能設(shè)置代數(shù)黎卡提方程的邊界條件，所以，如果題目設(shè)置了P的終值條件，我們只能使用第三種方法來求解，例如設(shè)置P的終值條件為[0.2;0.2]。43以上三種方法中，第一種方法易于理解黎卡提方程的解程序如下：%***************MATLAB程序***************%A=[01;-5,-3];B=[0;1];Q=[500200;200100];R=1.6667;[P,E,K,RR]=care(A,B,Q,R,[0.2;0.2],eye(size(A)))44程序如下：44運(yùn)行結(jié)果：P=67.723321.568521.568511.0961E=-7.3052-2.4723K=13.06086.7775RR=1.2847e-014最優(yōu)控制變量與狀態(tài)變量之間的關(guān)系：45運(yùn)行結(jié)果：45例12-2

無(wú)人飛行器的最優(yōu)高度控制，飛行器的控制方程如下46例12-2 無(wú)人飛行器的最優(yōu)高度控制，飛行器的控制方程如下

是飛行器的高度；是油門輸入；設(shè)計(jì)控制律使得如下指標(biāo)最小初始狀態(tài)。繪制系統(tǒng)狀態(tài)與控制輸入，對(duì)如下給定的矩陣進(jìn)行仿真分析.47是飛行器的高度；是油門輸入；a).b).c).d).4848解：線性二次型最優(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.70712.07722.0510]，u（t）=–k1*x（t）；所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線如下圖12-2所示：49解：線性二次型最優(yōu)控制指標(biāo)如下：495050圖12-2狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線51圖12-2狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線512）、Q=diag（1，0，0），R=2000時(shí)，由MATLAB求得最優(yōu)狀態(tài)反饋矩陣為k2=[0.02240.25170.4166]，u（t）=–k2*x（t）；

所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線如下圖12-3所示：522）、Q=diag（1，0，0），R=2000時(shí)，由MATL5353圖12-3狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線54圖12-3狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線543）、Q=diag（10，0，0），R=2時(shí)，由MATLAB求得最優(yōu)狀態(tài)反饋矩陣為

k3=[2.23614.38923.3077]，u（t）=–k3*x（t）；所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線如下圖12-4所示：553）、Q=diag（10，0，0），R=2時(shí)，由MATLAB5656圖12-4狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線57圖12-4狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線574）、Q=diag（1，100，0），R=2時(shí)，由MATLAB求得最優(yōu)狀態(tài)反饋矩陣為

k4=[0.70717.61124.6076]，u（t）=–k4*x（t）；所畫狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線如下圖12-5所示：584）、Q=diag（1，100，0），R=2時(shí)，由MATLA5959圖12-5狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線60圖12-5狀態(tài)響應(yīng)曲線及控制輸入響應(yīng)曲線60由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)幅值變小，反饋矩陣變??；圖12-4與圖12-2和圖12-3相比，當(dāng)Q對(duì)角線上第1個(gè)元素增大時(shí)，各相應(yīng)曲線達(dá)到穩(wěn)態(tài)所需時(shí)間變短，即響應(yīng)快；但波動(dòng)幅值變大，反饋矩陣增大；61由1），2），3），4）可分析如下：圖12-3與圖12-2相由圖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大；反饋矩陣最大。62由圖12-5可知，當(dāng)Q對(duì)角線上第2個(gè)元素增大時(shí)，狀態(tài)x1，x

綜上所述可得結(jié)論：Q=diag（1，0，0），R=2時(shí)，系統(tǒng)各方面響應(yīng)較好。矩陣Q變大時(shí)，反饋矩陣變大；當(dāng)Q的對(duì)角線上第1個(gè)元素變大時(shí)，各曲線波動(dòng)幅值變大，達(dá)到穩(wěn)態(tài)所需時(shí)間變短；

63綜上所述可得結(jié)論：Q=diag（1，0，0），R=2時(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。64當(dāng)Q的對(duì)角線上第2個(gè)元素變大時(shí)，各曲線波動(dòng)幅值變??；%***************MATLAB程序***************%a=[010;001;00-1/2];b=[0;0;1/2];c=[100;010;001];d=[0;0;0];figure(1)q=[100;000;000];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);65%***************MATLAB程序******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,holdonforj=1:nu(j,:)=-k*(x(j,:))';end66n=length(x(:,3));66figure(2)plot(T,u);xlabel('time-s');ylabel('response');title('圖(1.b)Q=diag(1,0,0),R=2時(shí)控制輸入u的響應(yīng)曲線')grid,holdon%**************************67figure(2)67figure(3)qa=[100;000;000];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));68figure(3)68Ta=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,holdonforj=1:naua(j,:)=-ka*(xa(j,:))';end69Ta=0:60/na:60-60/na;69figure(4)plot(Ta,ua);xlabel('time-s');ylabel('response');title('圖(2.b)Q=diag(1,0,0),R=2000時(shí)控制輸入u的響應(yīng)曲線')grid,holdon%%%*******************************70figure(4)70figure(5)qb=[1000;000;000];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));71figure(5)71Tb=0:20/nb:20-20/nb;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時(shí)狀態(tài)響應(yīng)曲線')grid,holdonforj=1:nbub(j,:)=-kb*(xb(j,:))';end72Tb=0:20/nb:20-20/nb;72figure(6)plot(Tb,ub);xlabel('time-s');ylabel('response');title('圖(3.b)Q=diag(10,0,0),R=2時(shí)控制輸入u的響應(yīng)曲線')grid,holdon%%%*************73figure(6)73figure(7)qc=[100;01000;000];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));74figure(7)74Tc=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,holdonforj=1:ncuc(j,:)=-kc*(xc(j,:))';end75Tc=0:50/nc:50-50/nc;75figure(8)plot(Tc,uc);xlabel('time-s');ylabel('response');title('圖(4.b)Q=diag(1,100,0),R=2時(shí)控制輸入u的響應(yīng)曲線')grid,holdon76figure(8)7612.3用MATLAB解最優(yōu)控制問題應(yīng)用實(shí)例

12.3.1導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程的建立12.3.2最優(yōu)導(dǎo)引律的求解與仿真驗(yàn)證7712.3用MATLAB解最優(yōu)控制問題應(yīng)用實(shí)例

12.3.在現(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í)的彈目視線，并且正比于導(dǎo)彈與目標(biāo)之間的視線角速率，假設(shè)目標(biāo)的法向加速度為零，那么可得：

(12-1)78在現(xiàn)有的自尋的導(dǎo)彈中，大都采用比例導(dǎo)引法。假設(shè)導(dǎo)彈和目標(biāo)在同其中，為導(dǎo)彈的速度與基準(zhǔn)方向的夾角，為導(dǎo)彈與目標(biāo)連線與基準(zhǔn)方向的夾角，稱為視線角，是視線角速率，是比例常數(shù)，稱為導(dǎo)航比，通常為3~6。比例導(dǎo)引的實(shí)質(zhì)是使導(dǎo)彈向著減小的方向運(yùn)動(dòng)，抑制視線旋轉(zhuǎn)，也就是使導(dǎo)彈的相對(duì)速度對(duì)準(zhǔn)目標(biāo)，保證導(dǎo)彈向著前置碰撞點(diǎn)飛行。79其中，為導(dǎo)彈的速度與基準(zhǔn)方向的夾角，為導(dǎo)彈與目標(biāo)

比例導(dǎo)引法是經(jīng)典的導(dǎo)引方法。下面我們從最優(yōu)控制理論的觀點(diǎn)來研究自尋的導(dǎo)彈的最優(yōu)導(dǎo)引規(guī)律問題。80比例導(dǎo)引法是經(jīng)典的導(dǎo)引方法。下面我們從12.3.1導(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)方程相對(duì)于理想彈道線性化，可得導(dǎo)彈運(yùn)動(dòng)的線性狀態(tài)方程.8112.3.1導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程的建立

導(dǎo)彈與目標(biāo)假設(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)均勻速飛行，也就是說和均為恒值。使用相對(duì)坐標(biāo)狀態(tài)變量，設(shè)為導(dǎo)彈與目標(biāo)在軸方向上的距離偏差，為導(dǎo)彈與目標(biāo)在軸方向上的距離偏差，即

(12-2)82假設(shè)導(dǎo)彈和目標(biāo)在同一平面內(nèi)運(yùn)動(dòng)，如圖12-6所示。選為圖12-6導(dǎo)彈和目標(biāo)運(yùn)動(dòng)幾何關(guān)系圖83圖12-6導(dǎo)彈和目標(biāo)運(yùn)動(dòng)幾何關(guān)系圖83假定和比較小，因此，則將上式對(duì)t求導(dǎo)，并根據(jù)導(dǎo)彈和目標(biāo)的關(guān)系（如圖12-6所示）可得

(12-3)

(12-4)84假定和比較小，因此，將上式對(duì)t求導(dǎo)，并根據(jù)導(dǎo)以表示，表示（即），則

(12-5)

(12-6)式中表示目標(biāo)的橫向加速度，表示導(dǎo)彈橫向加速度，分別以和表示，那么

(12-7)85以表示，表示（即），則式中導(dǎo)彈的橫向加速度為一控制量。一般將控制信號(hào)加給舵機(jī)，舵面偏轉(zhuǎn)后產(chǎn)生彈體攻角，而后產(chǎn)生橫向加速度。如果忽略舵機(jī)和彈體的慣性，而且假設(shè)控制量的單位與加速度單位相同，則可用控制量來表示，也就是令

(12-8)

所以(12-7)式為：

(12-9)86導(dǎo)彈的橫向加速度為一控制量。一般將控制信號(hào)加給舵機(jī)，舵這樣可得導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程為：

(12-10)

(12-11)87這樣可得導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程為：87可寫成矩陣的形式：

(12-12)式中，

，，，。(12-13)如果不考慮目標(biāo)的機(jī)動(dòng)，即，則在這種情況下，式(12-12)變成：

(12-14)88可寫成矩陣的形式：88下面來考慮(12-4)式，該式可寫成

(12-15)

其中表示導(dǎo)彈相對(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)89下面來考慮(12-4)式，該式可寫成89又考慮到對(duì)于導(dǎo)彈制導(dǎo)來說，最基本的要求是脫靶量越小越好，因此，應(yīng)該選擇最優(yōu)控制量，使得下面的指標(biāo)函數(shù)為最小。

(12-17)90又考慮到對(duì)于導(dǎo)彈制導(dǎo)來說，最基本的要求是脫靶量越小越好，因此然而，當(dāng)要求一個(gè)反饋形式的控制時(shí)，按上式列出的問題很難求解。所以我們以時(shí)刻，即時(shí)的值作為脫靶量，要求值越小越好。另外，由于舵偏角受到限制，導(dǎo)彈結(jié)構(gòu)能夠承受的最大載荷也受到限制，所以控制信號(hào)也應(yīng)該受到限制。91然而，當(dāng)要求一個(gè)反饋形式的控制時(shí)，按上式列出的問題很難求解。因此，我們選擇以下形式的二次型指標(biāo)函數(shù)：

(12-18)式中，。(12-19)即

(12-20)給定初始條件，應(yīng)用最優(yōu)控制理論，可以求出使為最小的。92因此，我們選擇以下形式的二次型指標(biāo)函數(shù)：92

由于系統(tǒng)是線性的，指標(biāo)函數(shù)是二次型的，因此，求最優(yōu)控制規(guī)律就可以認(rèn)為是一個(gè)求解線性二次型的過程。對(duì)于線性二次型問題，可采用變分法、極小值原理、動(dòng)態(tài)規(guī)劃或其他方法求得最優(yōu)控制93由于系統(tǒng)是線性的，指標(biāo)函數(shù)是二次型的，(12-21)式中滿足下列黎卡提矩陣微分方程

(12-22)

的終端條件為

(12-23)因此求解線性二次型問題的關(guān)鍵是求解黎卡提矩陣微分方程。94

12.3.2最優(yōu)導(dǎo)引律的求解與仿真驗(yàn)證

當(dāng)不考慮彈體慣性時(shí)，而且假定目標(biāo)不機(jī)動(dòng)，即，導(dǎo)彈運(yùn)動(dòng)狀態(tài)方程為

(12-24)9512.3.2最優(yōu)導(dǎo)引律的求解與仿真驗(yàn)證

當(dāng)不考慮彈體慣性時(shí)指標(biāo)函數(shù)為

(12-25)式中

，,，。96指標(biāo)函數(shù)為96給出時(shí)刻，的初值，采用極小值原理可求得最優(yōu)控制為

(12-26)97給出時(shí)刻，的初值在指標(biāo)函數(shù)中，如不考慮導(dǎo)彈的相對(duì)運(yùn)動(dòng)速度項(xiàng)，則可令。變成

(12-27)以除上式的分子和分母，得

(12-28)98在指標(biāo)函數(shù)中，如不考慮導(dǎo)彈的相對(duì)運(yùn)動(dòng)速度項(xiàng)，則可令為了使脫靶量為最小，應(yīng)選取，則

(12-29)根據(jù)圖12-6可得

99為了使脫靶量為最小，應(yīng)選取，則99當(dāng)比較小時(shí)，，則

(12-30)

(12-31)100當(dāng)比較小時(shí)，，則100將上式代入(12-29)式，可得

(12-32)在上式中，的單位是加速度的單位。把與導(dǎo)彈速度向量的旋轉(zhuǎn)角速度聯(lián)系起來，則有

(12-33)101將上式代入(12-29)式，可得101從(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來表示。102從(12-32)和(12-33)式可以看出，當(dāng)不考慮彈體慣性

下面將對(duì)最優(yōu)導(dǎo)引律進(jìn)行MATLAB仿真，并給出源代碼和仿真結(jié)果。圖12-7最優(yōu)導(dǎo)引方框圖103圖12-7最優(yōu)導(dǎo)引方框圖103圖12-8最優(yōu)導(dǎo)引攻擊幾何平面104圖12-8最優(yōu)導(dǎo)引攻擊幾何平面104最優(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)彈的指令（垂直于視線）。105最優(yōu)導(dǎo)引攻擊幾何關(guān)系如圖12-8所示，在這里討論的目標(biāo)和導(dǎo)彈其中： (12-34)

(12-35)

(12-36)

為目標(biāo)速度在軸上的分解，是目標(biāo)的角度。導(dǎo)彈和目標(biāo)之間的接近速度為：

(12-37)106其中： (12-目標(biāo)的速度分量可由其位置變化得到：

(12-38)同樣地，我們可以得到導(dǎo)彈的位置和速度的微分方程：， (12-39)

， (12-40)107目標(biāo)的速度分量可由其位置變化得到：107上面幾式中的下標(biāo)x，y分別表示在x和y軸上的分量。是導(dǎo)彈在地球坐標(biāo)系的加速度分量。為了得到導(dǎo)彈的加速度分量，我們必須得到彈目的相對(duì)位移：

(12-41) (12-42)108上面幾式中的下標(biāo)x，y分別表示在x和y軸上的分量。從圖12-8中，根據(jù)三角關(guān)系我們可以得到視線角：

(12-43)如果定義地球坐標(biāo)系的速度分量為：

(12-44)

(12-45)109從圖12-8中，根據(jù)三角關(guān)系我們可以得到視線角：109我們可以根據(jù)視線角的公式求導(dǎo)后得到視線角速率：

(12-46) (12-47)所以我們不難得出彈目的接近速度為：

(12-48)110我們可以根據(jù)視線角的公式求導(dǎo)后得到視線角速率：110根據(jù)最優(yōu)導(dǎo)引制導(dǎo)律：

(12-49)可得到導(dǎo)彈的加速的分量為：

(12-50) (12-51) (12-52)111根據(jù)最優(yōu)導(dǎo)引制導(dǎo)律：111

以上列出了兩維的最優(yōu)導(dǎo)引制導(dǎo)的必要方程，但是使用最優(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á)式：

(12-53)112以上列出了兩維的最優(yōu)導(dǎo)引制導(dǎo)的必要方程，但是使用

但是實(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í)刻的指向角和指向角誤差之后，導(dǎo)彈的初始速度分量可以表示為：

(12-54)

(12-55)113但是實(shí)際上導(dǎo)彈不可能能確切地在碰撞三角區(qū)發(fā)射，所使用MATLAB編程，具體代碼如下：%***************MATLAB程序***************%%最優(yōu)制導(dǎo)律仿真，初始化系統(tǒng)的參數(shù)clearall; %清除所有內(nèi)存變量globalSignVc;pi=3.14159265;Vm=1000;Vt=500;%導(dǎo)彈和目標(biāo)的速度HeadError=0;%指向角誤差114使用MATLAB編程，具體代碼如下：114ThetaT=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(Rtmx^2+Rtmy^2);115ThetaT=pi;%目標(biāo)的速度方向115SightAngle=atan(Rtmx/Rtmy);%視線角LeadAngle