楊盛慶，王 禹，陳 樺，王嘉軼，劉美師

(1. 上海航天控制技術(shù)研究所·上?！?01109； 2. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室·上?！?01109)

0 引 言

飛行器個(gè)體的航跡優(yōu)化可分為離線算法和在線算法兩類。離線的航跡優(yōu)化算法著眼于全局最優(yōu)的可行路徑(算法的數(shù)值結(jié)果多為可行航跡點(diǎn)的序列)，可以脫離個(gè)體的力學(xué)模型；在線的航跡優(yōu)化算法傾向于采用反饋控制和局部的最優(yōu)控制，與個(gè)體的力學(xué)模型密不可分。個(gè)體航跡優(yōu)化的離線算法種類繁多?？焖匐S機(jī)生成樹(Rapidly Exploring Random Tree, RRT)以隨機(jī)的數(shù)據(jù)結(jié)構(gòu)為基礎(chǔ)，其算法具有高效、收斂的特性。本文選取RRT來規(guī)劃空間中的可行航跡點(diǎn)序列，并將所得的航跡點(diǎn)用作局部最優(yōu)控制的邊界條件，從而實(shí)現(xiàn)了離線算法與在線算法的結(jié)合，得到了飛行器個(gè)體在區(qū)域內(nèi)的航跡優(yōu)化結(jié)果。

飛行器系統(tǒng)的力學(xué)模型是研究其最優(yōu)控制的基礎(chǔ)，分析其力學(xué)特性，特別是其拉格朗日函數(shù)和運(yùn)動(dòng)方程，具有重要的研究意義。對(duì)于六自由度(6DOF)的飛行器模型，其位形變量可分為位移變量和姿態(tài)變量兩類。與分析力學(xué)著眼于力學(xué)系統(tǒng)的運(yùn)動(dòng)方程有所不同，幾何力學(xué)強(qiáng)調(diào)力學(xué)系統(tǒng)自身的幾何特性(不變量及其守恒律)[1-2]。在描述空間中具有對(duì)稱性的個(gè)體的運(yùn)動(dòng)方程時(shí)，可以借助流形的工具來刻畫變量在運(yùn)動(dòng)過程中所需滿足的性質(zhì)。譬如，剛體在空間中的姿態(tài)變量屬于李群，基于李群概念構(gòu)建的運(yùn)動(dòng)方程可以時(shí)刻確保姿態(tài)變量屬于李群，進(jìn)而提高計(jì)算的精度[3]。李群變分積分子是對(duì)剛體進(jìn)行數(shù)值積分的一種數(shù)值格式[4-6]，Marsden、Junge和Ober將其用于構(gòu)造離散位形空間中的離散力學(xué)與最優(yōu)控制問題(Discrete Mechanics and Optimal Control, DMOC)[7]，將原本的連續(xù)空間中的最優(yōu)控制問題轉(zhuǎn)化為離散空間中的一個(gè)最優(yōu)化問題，并實(shí)現(xiàn)了個(gè)體之間不存在相互作用的質(zhì)點(diǎn)編隊(duì)的最優(yōu)控制[8-9]。Lee、Leok和McClamroch等基于DMOC原理，研究了由李群變分積分子構(gòu)造的剛體最優(yōu)控制問題，并將其成功應(yīng)用于衛(wèi)星編隊(duì)等領(lǐng)域[10-12]。近期，國外學(xué)者將變分積分子領(lǐng)域的研究拓展到了衛(wèi)星軌道積分[13]和受力拉格朗日動(dòng)力學(xué)系統(tǒng)[14]。這些研究成果為自主智能的無人系統(tǒng)發(fā)展[15-16]打下了良好的數(shù)學(xué)理論基礎(chǔ)。

結(jié)合基于隨機(jī)算法的航跡點(diǎn)規(guī)劃和基于幾何力學(xué)的最優(yōu)控制，實(shí)現(xiàn)了飛行器個(gè)體在區(qū)域內(nèi)的航跡優(yōu)化。首先介紹了DMOC的基本理論，推導(dǎo)得到了剛體的李群變分積分子，然后基于李群變分積分子給出了剛體最優(yōu)控制的數(shù)值求解格式。最后，結(jié)合全局的RRT算法和局部的最優(yōu)控制(DMOC)，實(shí)現(xiàn)了全局環(huán)境中飛行器個(gè)體的航跡優(yōu)化。

1 變分原理與剛體運(yùn)動(dòng)方程

1.1 拉格朗日動(dòng)力學(xué)系統(tǒng)與變分原理

定義1：位形空間Q上的Lagrange力學(xué)系統(tǒng)，其拉格朗日函數(shù)Lagrangian的定義為L:TQ→R，滿足Euler-Lagrange方程

(1)

定義2：設(shè)Φ是群G在流形Q上的作用，將Φ對(duì)于辛流形T*Q的右作用提升Φ*定義為

(2)

左作用下Φ不變。

Noether定理可以理解為，當(dāng)系統(tǒng)滿足左(右)提升不變的條件時(shí)(譬如自由系統(tǒng)的平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng))，其Lagrange流(運(yùn)動(dòng)狀態(tài))將保持不變。將上述概念拓展到離散Lagrange力學(xué)系統(tǒng)，即可得到變分積分子的數(shù)值特性。

1.2 剛體運(yùn)動(dòng)的變量空間

(3)

角速度Ω是體坐標(biāo)系下的一類變量，用以描述運(yùn)動(dòng)中姿態(tài)角的變化率?；诮撬俣圈?，可以定義角速度矩陣S(Ω)，且S(Ω)滿足

(4)

1.3 李群與李代數(shù)

李群是具有旋轉(zhuǎn)不變性的群，可以描述具有對(duì)稱性的運(yùn)動(dòng)物體。例如，空間中固定質(zhì)心的剛體可由SO(3)刻畫其姿態(tài)，其運(yùn)動(dòng)狀態(tài)則可由李代數(shù)SO(3)中的元素表述。SO(3)是一類經(jīng)典的李群。

李群G是一個(gè)Banach空間，具有零元和單位元。其群結(jié)構(gòu)與其流形結(jié)構(gòu)在如下意義下相容

μ:G×G→G,(g,h)|→gh

(5)

李代數(shù)是李群在零元鄰域內(nèi)的局部線性化，用以刻畫李群的局部信息，從而表述其運(yùn)動(dòng)狀態(tài)。

定義3：指數(shù)映射EXP:g→G，滿足

EXP(X)=eεX∈G

(6)

命題1：對(duì)于任意S(Ω)，存在η∈SO(3)，滿足δR=Rη。

證明：根據(jù)指數(shù)映射的定義，對(duì)于任意R∈G，存在η∈g，滿足R=Reεη(ε→0)。對(duì)指數(shù)函數(shù)進(jìn)行Talyor展開，滿足

(7)

由變分定義可得

(8)

將式(7)代入式(8)，計(jì)算可得

δR=Rη

(9)

同理，δq=Rξ。

1.4 剛體運(yùn)動(dòng)方程

對(duì)剛體的Lagrangian作變分處理，可以得到剛體運(yùn)動(dòng)的Euler-Lagrange方程?？紤]三軸向?qū)ΨQ的剛體，其轉(zhuǎn)動(dòng)慣量矩陣為一個(gè)對(duì)角矩陣，不妨令J=diag{J1,J2,J3}。引入非標(biāo)準(zhǔn)轉(zhuǎn)動(dòng)慣量矩陣Jd，滿足

J=tr(Jd)I3×3-Jd
Jd=tr(J)I3×3-J

(10)

在剛體的動(dòng)能函數(shù)中，關(guān)于角速度的部分可以改寫為

ΩTJΩ=Ω1J1Ω1+Ω2J2Ω2+Ω3J3Ω3
=tr(S(Ω)JdS(Ω)T)

(11)

記m為個(gè)體質(zhì)量，慣性矩陣M=diag{m,m,m}；重力方向矢量k=(0,0,1)T。剛體的Lagrangian可以定義為

L(Ω,v,R,q)=E(Ω,v)-V(R,q)

(12)

Lagrangian的變分滿足

(13)

(14)

記控制扭矩τ1∈SO(3)*和控制力τ2∈(3)*。由Lagrange-d-Alembert原理可得剛體運(yùn)動(dòng)方程為

(15)

2 李群變分積分子及其數(shù)值特性

2.1 離散變分原理

在連續(xù)空間中，可以定義位形空間、Lagrangain函數(shù)和作用函數(shù)，通過變分原理得到Euler-Lagrange方程。同樣地，在離散變量空間中可以定義上述空間和函數(shù)的離散形式，得到離散的Euler-Lagrange方程,如圖1所示。

圖1 連續(xù)空間與離散空間的運(yùn)動(dòng)方程推導(dǎo)體系Fig.1 Euler-Lagrange equation induced in continuous and discrete variable spaces

2.2 變分積分子的數(shù)值特性

定義4：將Legendre變換定義為位形空間的切叢到余切叢的映射FL:TQ→T*Q，滿足

(16)

相應(yīng)地，離散的Legendre變換可定義為

(17)

(18)

離散Legendre變換定義的變分積分子是一種數(shù)據(jù)積分格式，能夠保持動(dòng)力學(xué)系統(tǒng)的能量特性。基于變分積分子的數(shù)值積分能夠長時(shí)間地保持系統(tǒng)能量，從而可得到更加符合實(shí)際的運(yùn)動(dòng)過程，如圖2所示。

圖2 變分積分子的數(shù)值積分流程Fig.2 Numerical process of variational integrator

引理2[1]：(Noether定理離散形式)一個(gè)離散Lagrange系統(tǒng)，如果其離散Lagrangian函數(shù)在左(右)提升Φ:G×Q→Q的作用下不變，那么對(duì)應(yīng)的離散Lagrange動(dòng)量映射JLd:Q×Q→g*關(guān)于系統(tǒng)的離散Lagrange流FLd:Q×Q→Q×Q則是守恒的，即有JLd°FLd=JLd。

Noether定理的離散形式揭示了變分積分子的數(shù)值特性，即對(duì)于一個(gè)不受外力作用的動(dòng)力學(xué)系統(tǒng)而言，變分積分子可以圍繞不變的離散Lagrange流而構(gòu)造算法，實(shí)現(xiàn)可長時(shí)間保持精度的數(shù)值積分方法。

2.3 離散剛體運(yùn)動(dòng)方程

李群變分積分子是剛體的離散Lagrangian作離散變分而得到的數(shù)值積分格式。首先，給出離散變量的差分格式，記時(shí)間步長為h，選取向前差分格式

(19)

離散Lagrangian的定義為

Ld(Fk,Ck,Rk,qk)

(20)

Rkξk)+mgkRkξk}

(21)

整理可得剛體的離散運(yùn)動(dòng)方程

(22)

2.4 李群變分積分子

在數(shù)值計(jì)算的過程中，為了保證Fk∈SO(3)，需引入其近似的表達(dá)式

(23)

在數(shù)值計(jì)算的過程中，可利用公式(23)將離散運(yùn)動(dòng)方程中的Fk替換為與fk相關(guān)的項(xiàng)。只需求解滿足方程組的fk，即可得到相應(yīng)的Fk。

3 離散力學(xué)與最優(yōu)控制與飛行器航跡優(yōu)化

3.1 離散力學(xué)與最優(yōu)控制(DMOC)

DMOC屬于直接法，相較傳統(tǒng)的直接法，DMOC能夠更好地保持系統(tǒng)的守恒量。不同于傳統(tǒng)直接法中的離散運(yùn)動(dòng)方程，DMOC是在離散的Lagrangian的基礎(chǔ)上利用離散變分原理求得運(yùn)動(dòng)的離散方程，使得離散變量能保持其性質(zhì)。在由此得到的離散運(yùn)動(dòng)方程和李群變分積分子的基礎(chǔ)上，求解與最優(yōu)控制等價(jià)的最優(yōu)化問題，規(guī)劃離散節(jié)點(diǎn)處的運(yùn)動(dòng)狀態(tài)和對(duì)應(yīng)的控制量。控制力和控制力矩的工程約束主要體現(xiàn)于優(yōu)化問題構(gòu)造過程中的變量取值范圍的約束。將DMOC進(jìn)行航跡規(guī)劃的具體形式如下

使得

滿足

(24)

最優(yōu)航跡的目標(biāo)有多種，比如能耗最小或距離最短。針對(duì)耗能最小的規(guī)劃問題，其代價(jià)函數(shù)可表述為

(25)

代價(jià)函數(shù)T是關(guān)于控制力和控制力矩的正定函數(shù)。針對(duì)控制力和控制力矩的區(qū)別，根據(jù)經(jīng)驗(yàn)調(diào)整加權(quán)矩陣Wr,Wt，從而體現(xiàn)兩者實(shí)現(xiàn)代價(jià)的不同性。

對(duì)于路徑最短的最優(yōu)控制問題，其代價(jià)函數(shù)可表述為

(26)

圖3(a)為不同代價(jià)函數(shù)的DMOC求解結(jié)果。對(duì)于能耗最小的最優(yōu)控制問題，其個(gè)體的運(yùn)動(dòng)更依賴于慣性，進(jìn)而需要以更長的路徑作為代價(jià)；對(duì)于路徑最短的最優(yōu)控制問題，其更依賴于由外部控制實(shí)現(xiàn)航跡的改變，從而需要付出更多的能耗代價(jià)。圖3(b)為不同末態(tài)約束的最短路徑示意。一般而言，在最優(yōu)化求解兩點(diǎn)邊值問題時(shí)，初態(tài)和末態(tài)的時(shí)間是給定的。對(duì)于末態(tài)時(shí)間不確定的時(shí)間最優(yōu)問題，需要重新構(gòu)造DMOC的目標(biāo)函數(shù)和約束。

(a)不同代價(jià)函數(shù)的DMOC求解結(jié)果

(b)不同末態(tài)約束的最短路徑示意圖3 不同代價(jià)函數(shù)的DMOC求解結(jié)果與 不同末態(tài)約束的最短路徑示意Fig.3 DMOC results for shortest routing and minimum energy with DMOC results for different final conditions

3.2 RRT的基本原理

RRT是一種高效的離線算法，可以得到給定障礙區(qū)域內(nèi)的可行航跡點(diǎn)序列。算法的主體為一個(gè)循環(huán)過程，具有快速收斂到最優(yōu)(次優(yōu))航跡點(diǎn)序列的性質(zhì)[17-18]。RRT算法可分為如下四個(gè)部分。

(1)樹的延拓：在每次循環(huán)中，尋找一個(gè)可行的新航跡點(diǎn)。

(2)樹枝的優(yōu)化，參見表1和圖4所示。

表1 RRT樹枝的優(yōu)化

圖4 RRT樹枝優(yōu)化算法的示意圖Fig.4 Illustrations of RRT branch optimizations

(3)樹結(jié)構(gòu)的優(yōu)化，參見表2和圖5所示。

表2 RRT樹結(jié)構(gòu)的優(yōu)化

圖5 RRT樹結(jié)構(gòu)優(yōu)化算法的示意圖Fig.5 Illustrations of RRT structure optimizations

(4)尋找最優(yōu)航跡：在所生成的樹的結(jié)構(gòu)中找一條對(duì)應(yīng)航跡最短的樹枝，樹枝上的航跡點(diǎn)即為最優(yōu)航跡點(diǎn)序列。

3.3 基于RRT與DMOC結(jié)合的飛行器的航跡優(yōu)化

(a)左視圖

(b)右視圖圖6 三維歐式空間中RRT的規(guī)劃效果Fig.6 Illustrations of RRT optimizations in 3D Euclid space

(a)障礙環(huán)境中的RRT規(guī)劃航跡點(diǎn)

(b)DMOC實(shí)現(xiàn)的無人飛行器航跡優(yōu)化的結(jié)果圖7 由RRT生成的障礙環(huán)境中的最優(yōu)航跡點(diǎn)和 由DMOC規(guī)劃的最優(yōu)航跡示意Fig.7 Trajectory points of RRT method in obstacles environ- ment and DMOC results of trajectory planning of UAV

4 結(jié) 論

航跡優(yōu)化算法需要兼顧最優(yōu)性和實(shí)時(shí)性，將不同層次的規(guī)劃算法進(jìn)行結(jié)合是實(shí)現(xiàn)此項(xiàng)要求的一種選擇。由隨機(jī)離散算法(RRT)給出可行航跡點(diǎn)序列，繼而通過離散力學(xué)與最優(yōu)控制(DMOC)實(shí)現(xiàn)相鄰航跡點(diǎn)之間的銜接。本文推導(dǎo)了在重力環(huán)境下，六自由度剛體的李群變分積分子形式，將其應(yīng)用于相鄰航跡點(diǎn)之間最優(yōu)控制的求解。此類方法亦能應(yīng)用于衛(wèi)星編隊(duì)的航跡優(yōu)化，實(shí)現(xiàn)衛(wèi)星小推力控制形式的過程最優(yōu)控制。