基于間接Radau偽譜法的滑翔段軌跡跟蹤制導(dǎo)律

廖宇新，李惠峰，包為民，2

(1．北京航空航天大學(xué)宇航學(xué)院，北京100191;2．中國(guó)航天科技集團(tuán)公司科技委，北京100048)

0 引言

在滑翔段飛行過程中，制導(dǎo)系統(tǒng)承擔(dān)著引導(dǎo)高超聲速飛行器在不違背各種約束的前提下準(zhǔn)確到達(dá)目標(biāo)這一至關(guān)重要的作用。因此，研究具有魯棒性和實(shí)時(shí)性的滑翔段制導(dǎo)律意義重大。

按照制導(dǎo)策略通常將滑翔段制導(dǎo)律分為預(yù)測(cè)校正制導(dǎo)［1－3］和標(biāo)稱軌跡跟蹤制導(dǎo)［4－10］。預(yù)測(cè)校正制導(dǎo)是根據(jù)飛行器當(dāng)前的真實(shí)狀態(tài)對(duì)軌跡進(jìn)行快速預(yù)測(cè)，得到終端狀態(tài)量，然后根據(jù)所得結(jié)果與終端約束條件的偏差值對(duì)控制量進(jìn)行校正。預(yù)測(cè)校正制導(dǎo)不需要存儲(chǔ)標(biāo)稱軌跡，能適應(yīng)更大范圍的偏差和擾動(dòng)。但是，數(shù)值預(yù)測(cè)校正制導(dǎo)對(duì)彈載計(jì)算機(jī)的計(jì)算能力要求很高，難以滿足實(shí)時(shí)性的需求，且不能保證收斂的可行解的存在;而解析預(yù)測(cè)校正制導(dǎo)所用的模型都經(jīng)過了簡(jiǎn)化處理，解的預(yù)測(cè)精度和對(duì)飛行任務(wù)的適應(yīng)能力均不理想。

標(biāo)稱軌跡跟蹤制導(dǎo)主要包括標(biāo)稱軌跡生成和軌跡在線跟蹤兩部分。為了提高標(biāo)稱軌跡跟蹤制導(dǎo)的自主性、自適應(yīng)性和魯棒性，對(duì)于現(xiàn)有方法的改進(jìn)主要沿著兩條技術(shù)路線展開:一是研究軌跡在線生成算法，二是研究能夠在線實(shí)時(shí)解算且具有魯棒性和自適應(yīng)性的標(biāo)稱軌跡跟蹤方法［4］。文獻(xiàn)［5］以高度、速度、航跡傾角的變化曲線作為標(biāo)稱軌跡，對(duì)降階的運(yùn)動(dòng)方程組進(jìn)行線性化，將二維平面軌跡跟蹤問題轉(zhuǎn)化為線性時(shí)變(Linear Time-Varying，LTV)系統(tǒng)狀態(tài)調(diào)節(jié)器問題來求解，但基于系數(shù)凍結(jié)法的在線增益調(diào)參技術(shù)在處理LTV系統(tǒng)狀態(tài)調(diào)節(jié)器問題時(shí)存在費(fèi)時(shí)、魯棒性差、穩(wěn)定性無理論保證等不足。文獻(xiàn)［6］通過求解Riccati矩陣微分方程來求解航天飛機(jī)三維平面標(biāo)稱軌跡跟蹤問題轉(zhuǎn)化得到的LTV系統(tǒng)狀態(tài)調(diào)節(jié)器問題，但在線求解Riccati矩陣微分方程也存在費(fèi)時(shí)、數(shù)值不穩(wěn)定等不足［8］。文獻(xiàn)［7］將三維平面標(biāo)稱軌跡跟蹤問題轉(zhuǎn)化為L(zhǎng)TV系統(tǒng)滾動(dòng)時(shí)域控制問題，為避免求解Riccati矩陣微分方程的數(shù)值計(jì)算負(fù)擔(dān)，將LTV系統(tǒng)滾動(dòng)時(shí)域控制問題離散化，近似轉(zhuǎn)化為二次規(guī)劃(Quadratic Programming，QP)問題來求解，并證明了閉環(huán)系統(tǒng)的穩(wěn)定性。文獻(xiàn)［7］的方法具有很強(qiáng)的魯棒性，但當(dāng)離散的階次較高時(shí)，推導(dǎo)和計(jì)算都比較復(fù)雜。文獻(xiàn)［11］利用間接Legendre偽譜法(Indirect Legendre Pseudospectral Method，ILPM)求解LTV系統(tǒng)狀態(tài)調(diào)節(jié)器問題轉(zhuǎn)化得到的線性兩點(diǎn)邊值問題(Two Point Boundary Value Problem，TPBVP)，不存在求解Riccati矩陣微分方程和在線增益調(diào)參的過程。文獻(xiàn)［4，8－10］結(jié)合相關(guān)策略進(jìn)一步研究了基于ILPM的高超聲速飛行器滑翔段軌跡跟蹤制導(dǎo)律，驗(yàn)證了制導(dǎo)律的魯棒性和實(shí)時(shí)性。

本文提出了間接Radau偽譜法(Indirect Radau Pseudospectral Method，IRPM)來求解LTV系統(tǒng)狀態(tài)調(diào)節(jié)器問題轉(zhuǎn)化得到的線性TPBVP。文獻(xiàn)［11］中，線性TPBVP經(jīng)過ILPM離散轉(zhuǎn)化后，得到的是超定線性方程組，只存在最小二乘解。本文與文獻(xiàn)［11］不同的是，線性TPBVP經(jīng)過IRPM離散轉(zhuǎn)化后，得到的是適定線性方程組，能求得唯一的精確解。基于IRPM求解得到的最優(yōu)反饋控制律，提出了一種滑翔段全狀態(tài)標(biāo)稱軌跡跟蹤制導(dǎo)律。該制導(dǎo)律具有良好的魯棒性和在線執(zhí)行的能力。

1 標(biāo)稱軌跡跟蹤制導(dǎo)問題的轉(zhuǎn)化

假設(shè)地球?yàn)樽赞D(zhuǎn)圓球，高超聲速飛行器滑翔段無動(dòng)力飛行的三自由度運(yùn)動(dòng)方程組參考文獻(xiàn)［3］。飛行器的狀態(tài)量x=［r，θ，φ，V，γ，ψ］，分別為地心矢徑、經(jīng)度、緯度、速度、航跡傾角、航跡偏角;控制量u=［α，σ］，分別為攻角和傾側(cè)角。

實(shí)際飛行軌跡與標(biāo)稱軌跡之間存在偏差，其中狀態(tài)量偏差量為 δx(t)∈Rn，控制量偏差量為δu(t)∈Rm?；跇?biāo)稱軌跡(xr，ur)對(duì)運(yùn)動(dòng)方程組進(jìn)行線性化，得到LTV系統(tǒng)的狀態(tài)方程和初始條件分別為

進(jìn)一步考慮如下的二次型性能指標(biāo)

式中:Sf∈Rn×n和Q(t)∈Rn×n是對(duì)稱半正定矩陣，R(t)∈Rm×m是對(duì)稱正定矩陣。標(biāo)稱軌跡跟蹤制導(dǎo)問題可以轉(zhuǎn)化為L(zhǎng)TV系統(tǒng)狀態(tài)調(diào)節(jié)器問題:確定最優(yōu)反饋控制律 δu*(δx*，t)，在滿足式(1)、(2)所示約束的前提下，使式(3)所示性能指標(biāo)函數(shù)取最小值。

令哈密爾頓函數(shù)為

式中:λ(t)∈Rn為協(xié)態(tài)變量。由Pontryagin極大值原理可得線性TPBVP為

一階最優(yōu)必要條件為

進(jìn)一步可得最優(yōu)反饋控制律為

由此可知，通過求解上述線性TPBVP，獲得協(xié)態(tài)變量 λ(t)，即可得到最優(yōu)反饋控制律 δu*(δx*，t)。

2 基于間接Radau偽譜法的最優(yōu)反饋控制律

為了避免后向掃描法中反向積分Riccati矩陣微分方程時(shí)的數(shù)值敏感性，以及轉(zhuǎn)換矩陣法中可能產(chǎn)生病態(tài)轉(zhuǎn)換矩陣導(dǎo)致數(shù)值計(jì)算不穩(wěn)定的問題［8］，同時(shí)滿足實(shí)時(shí)計(jì)算最優(yōu)反饋控制律的需求，本文采用間接Radau偽譜法來求解上述線性TPBVP，進(jìn)而獲得最優(yōu)反饋控制律。

由于Legendre-Gauss-Radau(LGR)點(diǎn) τk∈［－1，1)，首先需要將時(shí)間區(qū)間t∈［t0，tf］通過式(9)轉(zhuǎn)換到［－1，1］

時(shí)間區(qū)間轉(zhuǎn)換后的線性TPBVP為

以N個(gè)LGR點(diǎn)τk和終端時(shí)刻點(diǎn)τf=τN+1=+1為節(jié)點(diǎn)，利用Lagrange插值方法對(duì)狀態(tài)量偏差量δx(τ)和協(xié)態(tài)變量λ(τ)進(jìn)行近似

式中:τj∈［－1，1］為插值節(jié)點(diǎn)，?j(τ)為L(zhǎng)agrange插值基函數(shù)。由此可得，狀態(tài)量偏差量和協(xié)態(tài)變量關(guān)于變換后時(shí)間的導(dǎo)數(shù)在LGR點(diǎn)處的值分別為

將式(15)、(16)代入式(10)，線性TPBVP中的微分方程組轉(zhuǎn)化為如下的代數(shù)方程組

令δx=［(δx(τ1))T，(δx(τ2))T，…，(δx(τN+1))T］T，λ=［λT(τ1)，λT(τ2)，…，λT(τN+1)］T，Ze=［(δx(τ2))T，…，(δx(τN+1))T，λT(τ1)，…，λT(τN+1)］T，Z=［(δx)T，λT］T=，綜合式(11)、(17)、(18)并寫成如下式所示的分塊的形式

式中:E、F、G、H∈RNn×(N+1)n，Z∈R(2N+2)n×1，V∈R(2N+1)n×(2N+2)n，P1=［0n×n，…，0n×n，Sf］ ∈Rn×(N+1)n，P2=［0n×n，…，0n×n，－In×n］∈Rn×(N+1)n，δx0∈Rn×1，V0∈R(2N+1)n×n，Ze∈R(2N+1)n×1，Ve∈R(2N+1)n×(2N+1)n，

式(19)中有(2N+1)n個(gè)方程，對(duì)應(yīng)于(2N+1)n個(gè)未知數(shù)，這表明VeZe=－V0δx0是適定線性方程組。由文獻(xiàn)［10］可知，矩陣V是列滿秩的，且可分解為矩陣V0和方陣Ve，所以Ve是列滿秩的方陣。因此，可求得VeZe=－V0δx0唯一的精確解為

由此可得

式中:W∈R(2N+2)n×n，Wx、Wλ∈R(N+1)n×n，(Wx)j、(Wλ)j∈Rn×n分別為Wx、Wλ中對(duì)應(yīng)于 τj時(shí)刻的子矩陣。

將式(26)代入式(8)，當(dāng)初始時(shí)刻的狀態(tài)量偏差值δx0已知時(shí)，最優(yōu)反饋控制律在離散節(jié)點(diǎn)處的值可以表示為

非離散節(jié)點(diǎn)處的標(biāo)稱控制量和最優(yōu)反饋控制律求得的控制量偏差值則根據(jù)離散節(jié)點(diǎn)處的值通過Lagrange插值的方法獲得。

3 閉環(huán)制導(dǎo)律

本文研究的滑翔段軌跡跟蹤制導(dǎo)律的執(zhí)行過程如圖1所示，對(duì)應(yīng)的流程如下:

圖1 閉環(huán)制導(dǎo)律的執(zhí)行過程示意圖Fig．1 Closed-loop guidance law execution diagram

(2)以當(dāng)前時(shí)刻標(biāo)稱控制量ur與當(dāng)前制導(dǎo)周期內(nèi)計(jì)算所得的當(dāng)前時(shí)刻最優(yōu)反饋控制律δu(τ0)之和ur+δu(τ0)作為實(shí)際飛行的控制量，轉(zhuǎn)入步驟(4);

(3)以當(dāng)前時(shí)刻標(biāo)稱控制量ur與由上一個(gè)制導(dǎo)周期的離散節(jié)點(diǎn)處的最優(yōu)反饋控制律(τj)插值所得的當(dāng)前時(shí)刻最優(yōu)反饋控制律δu(τ)之和ur+δu(τ)作為實(shí)際飛行的控制量，轉(zhuǎn)入步驟(4);

上述滑翔段標(biāo)稱軌跡跟蹤制導(dǎo)律不需要離線設(shè)計(jì)誤差反饋增益系數(shù)和在線增益調(diào)參，在制導(dǎo)指令解算過程中不涉及迭代、在線積分求解Riccati矩陣微分方程等復(fù)雜數(shù)值運(yùn)算，計(jì)算量小，因此便于在線執(zhí)行。

4 仿真校驗(yàn)

4．1標(biāo)稱軌跡

以CAV-H飛行器［12］為研究對(duì)象，質(zhì)量m為907 kg，參考面積S為0．48387 m2;飛行任務(wù)的端點(diǎn)約束如表1所示，過程約束(包括熱流密度Q˙、總過載nz和動(dòng)壓q)和控制量約束如表2所示，標(biāo)稱飛行攻角剖面αref為

表1 飛行任務(wù)的端點(diǎn)約束Table 1 Endpoint constraints for flight mission

表2 過程約束和控制量約束Table 2 Path constraints and control constraints

軌跡優(yōu)化時(shí)選擇的LGR點(diǎn)數(shù)目為40，優(yōu)化所得軌跡的終端狀態(tài)量分別為hf=30000 m、θf=65°、φf=3°、Vf=2500 m/s、γf=－0．8536°、ψf=51．6679°，飛行時(shí)間為1481．80 s。狀態(tài)量、控制量和過程約束隨飛行時(shí)間的變化過程分別如圖2～圖4中的實(shí)線所示?？梢钥闯鰞?yōu)化所得的飛行軌跡滿足飛行任務(wù)的要求且未違背過程約束，這說明優(yōu)化所得的軌跡是正確和可行的。下文的制導(dǎo)仿真都以本節(jié)優(yōu)化生成的軌跡作為標(biāo)稱軌跡，

4．2制導(dǎo)律魯棒性驗(yàn)證

為了驗(yàn)證本文的滑翔段制導(dǎo)律的魯棒性，在飛行器初始狀態(tài)量存在偏差和質(zhì)量、氣動(dòng)參數(shù)、大氣密

利用Radau偽譜法［13］離線優(yōu)化獲得滑翔段的標(biāo)稱軌跡。為滿足飛行軌跡平滑的需求，離線軌跡優(yōu)化采用的性能指標(biāo)為度等飛行環(huán)境參數(shù)有擾動(dòng)情況下進(jìn)行制導(dǎo)仿真。

仿真環(huán)境為雙核3．00 GHz主頻CPU、3 GB內(nèi)存及Matlab2012a軟件。制導(dǎo)律解算采用的LGR點(diǎn)數(shù)目為10，制導(dǎo)周期和積分步長(zhǎng)均取為1 s，權(quán)值矩陣Sf、Q、R的取值參考Bryson法則［14］，且在本文所有的仿真中，制導(dǎo)周期、積分步長(zhǎng)和權(quán)值矩陣的取值都一致。制導(dǎo)律仿真過程中，傾側(cè)角σ是主要的軌跡控制量，攻角α只在標(biāo)稱剖面附近進(jìn)行小范圍調(diào)整，調(diào)整范圍如表2中Δα所示。

初始狀態(tài)量偏差值、飛行環(huán)境參數(shù)擾動(dòng)量和閉環(huán)制導(dǎo)仿真所得的終端狀態(tài)量相對(duì)于標(biāo)稱軌跡的偏差值如表3所示。其中，組合1和組合2分別對(duì)應(yīng)于 δh0=3000 m、δθ0=0．3°、δφ0=0．3°、δV0=100 m/s、δγ0=0．5°、δψ0=1°和 δh0=－3000 m、δθ0=－0．3°、δφ0=－0．3°、δV0=－100 m/s、δγ0=－0．5°、δψ0=－1°的工況;組合3和組合4分別對(duì)應(yīng)于m(1－5%)、ρ(1－15%)、CL(1－10%)、CD(1－10%)和m(1+5%)、ρ(1+15%)、CL(1+10%)、CD(1+10%)的工況。組合工況下仿真所得的實(shí)際狀態(tài)量、控制量和過程約束隨飛行時(shí)間的變化過程分別如圖2～圖4所示?？梢钥闯?，在閉環(huán)制導(dǎo)律的作用下，實(shí)際飛行軌跡未違背過程約束，終端狀態(tài)量的偏差值滿足制導(dǎo)精度的要求。這說明本文的制導(dǎo)律對(duì)初始狀態(tài)量的較大范圍偏差和飛行環(huán)境參數(shù)的有限擾動(dòng)有良好的魯棒性。

從表3可以看出，飛行環(huán)境參數(shù)擾動(dòng)對(duì)制導(dǎo)精度的影響比初始狀態(tài)量偏差的影響大，制導(dǎo)律在飛行器升阻比存在常值偏差情況下的魯棒性有限。這是因?yàn)樵谥淮嬖诔跏紶顟B(tài)量偏差情況下，制導(dǎo)律解算用的系統(tǒng)模型很準(zhǔn)確;而在飛行環(huán)境參數(shù)存在擾動(dòng)時(shí)，制導(dǎo)律解算所使用的系統(tǒng)模型中的飛行環(huán)境參數(shù)與真實(shí)的飛行環(huán)境參數(shù)不一致，制導(dǎo)律解算用的系統(tǒng)模型不準(zhǔn)確，從而影響制導(dǎo)律的魯棒性和制導(dǎo)精度。同時(shí)由于控制量數(shù)目的限制和過程約束對(duì)控制量調(diào)節(jié)能力(主要是攻角調(diào)節(jié)能力)的限制，當(dāng)飛行環(huán)境參數(shù)存在擾動(dòng)時(shí)，跟蹤指定的標(biāo)稱剖面的制導(dǎo)律比跟蹤全狀態(tài)標(biāo)稱軌跡的制導(dǎo)律的魯棒性更強(qiáng)［7］。

4．3制導(dǎo)律實(shí)時(shí)性驗(yàn)證

為了驗(yàn)證本文的滑翔段制導(dǎo)律的實(shí)時(shí)性，選擇初始狀態(tài)量偏差值為δV0=100 m/s的工況，LGR點(diǎn)分別選10、20、30、40和50，對(duì)LGR點(diǎn)數(shù)目不同的五種工況的仿真結(jié)果和制導(dǎo)律解算時(shí)間進(jìn)行對(duì)比。從表4可以看出，五種工況的結(jié)果都滿足制導(dǎo)精度要求;假定在其它計(jì)算誤差相同的情況下，當(dāng)LGR點(diǎn)的數(shù)目增加到一定量級(jí)時(shí)，終端狀態(tài)量偏差值不會(huì)再有特別顯著的變化;五種工況下制導(dǎo)律平均解算時(shí)間和最大解算時(shí)間均小于1 s的制導(dǎo)周期，充分說明制導(dǎo)律滿足實(shí)時(shí)性要求。隨著LGR點(diǎn)數(shù)目增加，制導(dǎo)律平均解算時(shí)間也增加，這是因?yàn)橹茖?dǎo)律解算中主要耗時(shí)的計(jì)算是式(24)中矩陣Ve的求逆，在滿足制導(dǎo)精度要求的前提下，選取少量數(shù)目的LGR點(diǎn)能夠減少矩陣Ve的維數(shù)，從而減少制導(dǎo)律解算的耗時(shí)。

圖2 狀態(tài)量隨飛行時(shí)間的變化過程Fig．2 Time histories of state variable

表3 初始狀態(tài)量存在偏差和飛行環(huán)境參數(shù)有擾動(dòng)情況下的終端狀態(tài)量偏差Table 3 Terminal states deviations of biased initial states and disturbed flight environment parameters conditions

續(xù)表

表4 LGR點(diǎn)數(shù)目不同時(shí)的終端狀態(tài)量偏差和制導(dǎo)律解算時(shí)間Table 4 Terminal states deviations and calculation time of guidance law for different numbers of LGR points

圖3 控制量隨飛行時(shí)間的變化過程Fig．3 Time history of control variable

圖4 過程約束隨飛行時(shí)間的變化過程Fig．4 Time histories of path constraints

5 結(jié)論

本文提出了一種新的最優(yōu)反饋控制求解方法——間接Radau偽譜法，并基于此方法提出了一種高超聲速飛行器滑翔段標(biāo)稱軌跡跟蹤制導(dǎo)律。制導(dǎo)律解算不存在迭代、在線積分求解Riccati矩陣微分方程等復(fù)雜的數(shù)值計(jì)算，也不包含在線增益調(diào)參的過程，便于在線執(zhí)行;對(duì)于不同情況，軌跡跟蹤控制器的結(jié)構(gòu)和參數(shù)不需要改變。數(shù)值仿真結(jié)果表明，制導(dǎo)律能滿足實(shí)時(shí)性的要求，在初始狀態(tài)量存在較大范圍偏差和飛行環(huán)境參數(shù)存在有限擾動(dòng)的情況下具有良好的魯棒性;在保證制導(dǎo)精度的前提下，通過選擇合適的離散點(diǎn)數(shù)目，能夠減少制導(dǎo)律的解算時(shí)間。

［1］ 胡正東，郭才發(fā)，蔡洪．天基對(duì)地打擊動(dòng)能武器再入解析預(yù)測(cè)制導(dǎo)技術(shù)［J］．宇航學(xué)報(bào)，2009，30(3):1039－1044．［Hu Zheng-dong，Guo Cai-fa，Cai Hong．Analytical predictive guidance for space-to-ground kinetic weapon in reentry［J］．Journal of Astronautics，2009，30(3):1039－1044．］

［2］ 水尊師，周軍，葛致磊．基于高斯偽譜方法的再入飛行器預(yù)測(cè)校正制導(dǎo)方法研究［J］．宇航學(xué)報(bào)，2011，32(6):1249－1255．［Shui Zun-shi，Zhou Jun，Ge Zhi-lei．On-line predictorcorrector reentry guidance law based on Gauss pseudospectral method［J］．Journal of Astronautics，2011，32(6):1249－1255．］

［3］Xue S B，Lu P．Constrained predictor-corrector entry guidance［J］．Journal of Guidance，Control，and Dynamics，2010，33(4):1273－1281．

［4］ 陳剛，康興無，閆桂榮，等．基于偽譜方法的自適應(yīng)魯棒實(shí)時(shí)再入制導(dǎo)律研究［J］．系統(tǒng)仿真學(xué)報(bào)，2008，20(20):5623－5634．［Chen Gang，Kang Xing-wu，Yan Gui-rong，et al．Real time robust adaptive reentry guidance law based on pseudospectral method［J］．Journal of System Simulation，2008，20(20):5623－5634］．

［5］Roenneke A J，Cornwell P J．Trajectory control for a low-lift re-entry vehicle［J］．Journal of Guidance，Control，and Dynamics，1993，16(5):927－933．

［6］ 南英．航天飛機(jī)再入抗干擾軌道優(yōu)化設(shè)計(jì)［D］．西安:西北工業(yè) 大 學(xué)，1990．［Nan Ying．Anti-disturbance reentry trajectory optimization design for the Space Shuttle［D］．Xi’an:Northwestern Polytechnical University，1990．］

［7］Lu P．Regulation about time-varying trajectories:precision entry guidance illustrated［J］．Journal of Guidance，Control，and Dynamics，1999，22(6):784－790．

［8］Tian B L，Zong Q．Optimal guidance for reentry vehicles based on indirect legendre pseudospectral method［J］．Acta Astronautica，2011，68(7):1176－1184．

［9］Zhou H，Tawfiqur R，Wang D W，et al．Onboard pseudospectral guidance for re-entry vehicle［J］．Proceedings of the Institution of Mechanical Engineers，Part G:Journal of Aerospace Engineering，2014，228(11):1925－1936．

［10］ 吳旭忠，唐勝景，郭杰，等．基于滾動(dòng)時(shí)域控制的再入軌跡跟蹤制導(dǎo)律［J］．系統(tǒng)工程與電子技術(shù)，2014，36(8):1602－1608．［Wu Xu-zhong，Tang Sheng-jing，Guo Jie，et al．Trajectory tracking guidance law for reentry based on receding horizon control［J］．Systems Engineering and Electronics，2014，36(8):1602－1608．］

［11］Yan H，F(xiàn)ahroo F，Ross I M．Optimal feedback control laws by legendre pseudospectral approximations［C］．The American Control Conference，Arlington，USA，June 25－27，2001．

［12］Phillips T H．A common aero vehicle(CAV)model description and employment guide［R］．Schafer Corporation for AFRL and AFSPC，2003．

［13］Garg D，Patterson M A，Darby C L，et al．Direct trajectory optimization and costate estimation of finite-horizon and infinitehorizon optimal control problems using a Radau Pseudospectral Method［J］．Computational Optimization and Applications，2011，49(2):335－358．

［14］Bryson A E，Ho Y C．Applied optimal control:optimization，estimation and control［M］．Washington DC:Hemisphere Publishing Company，1975．