郝 瑞，孟云鶴，郭勝鵬

（國防科學(xué)技術(shù)大學(xué) 航天科學(xué)與工程學(xué)院，湖南 長沙 410073）

0 引言

在軌服務(wù)航天器對(duì)大型空間站或衛(wèi)星進(jìn)行維護(hù)時(shí)，首先需實(shí)現(xiàn)安全接近過程。飛越式接近方法可使服務(wù)航天器沿一條無碰撞的路徑到達(dá)目標(biāo)，且保證出現(xiàn)故障時(shí)也能安全撤離。文獻(xiàn)［1－2］給出了飛越式接近方法的基本過程，并驗(yàn)證了飛越式接近相對(duì)基于數(shù)值優(yōu)化方法的接近路徑有更好的安全性，且節(jié)省燃料。在軌服務(wù)航天器飛越式接近目標(biāo)要求其軌道與姿態(tài)同步快速精確機(jī)動(dòng)，屬于高維非線性問題，尤其是考慮航天器機(jī)動(dòng)性能及空間環(huán)境約束時(shí)，一般規(guī)劃方法難以獲得滿足要求的軌跡。

快速搜索隨機(jī)樹（RRT）算法廣泛用于運(yùn)動(dòng)規(guī)劃問題［3］。有研究發(fā)現(xiàn)基本RRT算法結(jié)果為概率非最優(yōu)，并通過在生成新節(jié)點(diǎn)時(shí)加入最優(yōu)路徑選擇，得到RRT＊算法。文獻(xiàn)［4］對(duì)RRT＊算法進(jìn)一步改進(jìn)，通過限定最大存儲(chǔ)節(jié)點(diǎn)數(shù)，得到了搜索效率更高的RRT＊FN算法。Heuristically－guided RRT（hRRT）算法引入了類似的近鄰節(jié)點(diǎn)概念［5］。文獻(xiàn)［6］在其基礎(chǔ)上采用多重隨機(jī)采樣策略，得到了滿足無人機(jī)實(shí)時(shí)航跡規(guī)劃需求的改進(jìn)RRT算法。文獻(xiàn)［7］針對(duì)RRT規(guī)劃方法易進(jìn)入局部陷阱的缺點(diǎn)，在算法中引入考慮障礙約束的平衡生長機(jī)制，提高了算法的繞障礙能力，使其在解決高維、復(fù)雜約束規(guī)劃問題時(shí)有較高的可靠性。文獻(xiàn)［8］提出了兩級(jí)路徑規(guī)劃方法，先用RRT算法規(guī)劃路徑初值，再將路徑優(yōu)化轉(zhuǎn)化為最優(yōu)控制問題，用高斯偽譜法（GPM）求解。RRT－GPM方法的缺點(diǎn)是使用SNOPT非線性求解器求解非線性系統(tǒng)時(shí)常出現(xiàn)收斂困難。

本文基于復(fù)合軌跡規(guī)劃，對(duì)一種由RRT運(yùn)動(dòng)規(guī)劃算法與進(jìn)化算法（EA）結(jié)合的兩級(jí)軌跡規(guī)劃方法進(jìn)行了研究。

1 問題描述

1.1 飛越式接近過程

根據(jù)文獻(xiàn)［1］，飛越式接近過程分為末端接近段、軌跡修正段和飛越接近段（如圖1所示），如接近中發(fā)生意外，服務(wù)航天器將會(huì)進(jìn)入飛越逃離段。假定末端接近段完成了軌跡修正過程，則需研究飛越接近段的起點(diǎn)運(yùn)動(dòng)狀態(tài)確定與末端接近段的軌跡規(guī)劃問題。

圖1 飛越接近Fig.1 Fly－by approach

已知初始相對(duì)運(yùn)動(dòng)狀態(tài)與飛越式接近總?cè)蝿?wù)時(shí)間，則接近軌跡確定步驟如下：

a）由目標(biāo)衛(wèi)星運(yùn)動(dòng)狀態(tài)的測(cè)量值算得飛越接近目標(biāo)點(diǎn)的運(yùn)動(dòng)狀態(tài)；

b）選擇合適的飛越近階段時(shí)間，計(jì)算飛越接近段初始點(diǎn)，確保其處于安全區(qū)域；

c）已知末端接近段的初始運(yùn)動(dòng)狀態(tài)、目標(biāo)運(yùn)動(dòng)狀態(tài)與調(diào)整時(shí)間，對(duì)該段的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃。

本文用Hill坐標(biāo)系描述相對(duì)軌道運(yùn)動(dòng)，且假設(shè)軌道為圓軌道。以服務(wù)航天器出發(fā)時(shí)刻為零時(shí)刻，飛越接近總?cè)蝿?wù)時(shí)間為T，末端接近段時(shí)間為t，則飛越接近段時(shí)間為T－t。服務(wù)航天器預(yù)測(cè)得到目標(biāo)相對(duì)軌道運(yùn)動(dòng)狀態(tài)為［（rT）H（vT）H］T，且要求其姿態(tài)為（qT）H，角速度（ωT）H＝0，則求解得到時(shí)刻t相對(duì)軌道運(yùn)動(dòng)狀態(tài)。此處：（rt）H，（vt）H分別為飛越式接近初始點(diǎn)的相對(duì)位置與速度；Φ11，Φ12，Φ21，Φ22為軌道運(yùn)動(dòng)轉(zhuǎn)移陣。

1.2 末端接近段的軌跡規(guī)劃問題

1.2.1 運(yùn)動(dòng)狀態(tài)與狀態(tài)空間

已知末端接近段的初始和目標(biāo)運(yùn)動(dòng)狀態(tài)分別為

式中：（ω0）H，（ωt）H分別為初始時(shí)刻和時(shí)刻t的角速度。軌跡規(guī)劃問題中的運(yùn)動(dòng)狀態(tài)還應(yīng)包含時(shí)間狀態(tài)t，則服務(wù)航天器的運(yùn)動(dòng)狀態(tài)描述

由于服務(wù)航天器的最優(yōu)運(yùn)動(dòng)軌跡可能在障礙區(qū)域附近，規(guī)劃問題的總狀態(tài)空間應(yīng)包括自由運(yùn)動(dòng)集合Xfree及障礙Xobs，即狀態(tài)空間X＝Xfree∪Xobs。

1.2.2 運(yùn)動(dòng)狀態(tài)度量

運(yùn)動(dòng)狀態(tài)的度量主要用于描述運(yùn)動(dòng)狀態(tài)空間中的距離。本文用實(shí)數(shù)方程ρ（pi，pj）表述服務(wù)航天器兩個(gè)運(yùn)動(dòng)狀態(tài)間的度量，有

式中：K1～K5為權(quán)系數(shù)；ρ（qi，qj）為姿態(tài)四元數(shù)間的度量，且

式（4）表示兩個(gè)姿態(tài)間度量是四元數(shù)qi與qj，－qj兩個(gè)距離中的最小值，這是因?yàn)閝j，－qj代表同一姿態(tài)的兩種旋轉(zhuǎn)方式［8］。此處：‖qi－qj‖＝arccos（qi·qj）。

1.2.3 約束條件

末端接近段路徑規(guī)劃問題主要考慮運(yùn)動(dòng)動(dòng)力學(xué)約束和空間幾何約束［9－10］。

a）運(yùn)動(dòng)動(dòng)力學(xué)約束

推進(jìn)裝置提供的軌道運(yùn)動(dòng)所需的加速度和速、執(zhí)行機(jī)構(gòu)提供的最大姿態(tài)控制力矩，以及有效載荷對(duì)運(yùn)動(dòng)速度的限制等，分別對(duì)速度、加速度、姿態(tài)角速度和角加速度提出了約束要求［11］。可表示為

b）空間幾何約束

空間幾何約束主要指空間環(huán)境中固定或時(shí)變的實(shí)體障礙約束或航天器指向約束等。假設(shè)所有約束均為實(shí)體障礙約束，即

式中：Robs為障礙影響半徑；Rmin為航天器最小安全半徑；edge（ri，ri＋1）為兩相鄰狀態(tài)間的路徑。式（6）的第一式表示航天器本體不與實(shí)體障礙相交；第二式表示edge（ri，ri＋1）可保證航天器整體在運(yùn)動(dòng)過程中不與障礙區(qū)相交。

2 末端接近段的兩級(jí)運(yùn)動(dòng)規(guī)劃方法

用改進(jìn)的雙向平衡RRT規(guī)劃算法與進(jìn)化算法結(jié)合的兩級(jí)運(yùn)動(dòng)規(guī)劃方法對(duì)在軌服務(wù)航天器的末端接近段進(jìn)行分析，并對(duì)該規(guī)劃方法的基本內(nèi)容進(jìn)行描述。其中兩級(jí)軌跡規(guī)劃方法的主要流程如圖2所示。

圖2 兩級(jí)軌跡規(guī)劃算法流程Fig.2 Flowchart for two－stage path planning method

2.1 基于改進(jìn)雙向平衡RRT算法的初始軌跡規(guī)劃

2.1.1 局部規(guī)劃器

局部規(guī)劃器主要用于確定節(jié)點(diǎn)pi，pj間的運(yùn)動(dòng)，要求其滿足整體算法的快速性需求［12］。已知兩節(jié)點(diǎn)狀態(tài)pi，pj，認(rèn)為從pi出發(fā)至pj的平動(dòng)速度方向與姿態(tài)運(yùn)動(dòng)角速度矢量方向保持不變。此處規(guī)劃器的作用是求取從pi出發(fā)經(jīng)過時(shí)間Δt的新節(jié)點(diǎn)pi＋1，要求兩節(jié)點(diǎn)的時(shí)間狀態(tài)應(yīng)滿足tj－tj≥Δt，以保證時(shí)間不可逆的要求。新節(jié)點(diǎn)pi＋1可表示為

e1，e2分別為從節(jié)點(diǎn)pi到pi＋1的速度和旋轉(zhuǎn)角速度方向單位矢量。

2.1.2 改進(jìn)的雙向平衡RRT算法

圖2給出了改進(jìn)的雙向平衡RRT的主要規(guī)劃步驟，通過比較TreeA，TreeB兩搜索樹的節(jié)點(diǎn)數(shù)實(shí)現(xiàn)互換，實(shí)現(xiàn)兩樹的平衡生長。在此基礎(chǔ)上，當(dāng)某方向隨機(jī)樹所求新節(jié)點(diǎn)的預(yù)測(cè)點(diǎn)不滿足障礙約束時(shí)，則程序換為先計(jì)算該隨機(jī)樹。這種改進(jìn)可解決由障礙相對(duì)較大帶來的近距障礙“陷阱”問題（如圖3所示）。兩搜索樹在同一次搜索中分別按規(guī)劃方法求出新節(jié)點(diǎn)a和預(yù)測(cè)點(diǎn)b。由于障礙較大，預(yù)測(cè)點(diǎn)b不滿足障礙約束，又因?yàn)樾鹿?jié)點(diǎn)a在更換隨機(jī)點(diǎn)后仍在障礙附近，從而使TreeB得不到合適的預(yù)測(cè)點(diǎn)，導(dǎo)致迭代受阻。改進(jìn)后，若預(yù)測(cè)點(diǎn)b不滿足障礙約束條件，則重新生成隨機(jī)點(diǎn)，并先計(jì)算出新節(jié)點(diǎn)c。

2.2 基于進(jìn)化算法的光滑軌跡規(guī)劃方法

圖3 障礙相對(duì)較大時(shí)運(yùn)動(dòng)規(guī)劃Fig.3 Motion planning when obstacles were relatively large

因RRT規(guī)劃算法中節(jié)點(diǎn)隨機(jī)生長，有限次數(shù)搜索得到的結(jié)果一般是曲折軌跡，故可用基于進(jìn)化算法的光滑軌跡規(guī)劃方法對(duì)初始軌跡進(jìn)行優(yōu)化。由于進(jìn)化算法簡單、實(shí)現(xiàn)易且使用效果明顯，在最優(yōu)化、機(jī)器學(xué)習(xí)和并行處理等領(lǐng)域得到了廣泛應(yīng)用［13］。

光滑軌跡規(guī)劃的目標(biāo)是找到使軌跡接近最優(yōu)軌跡的一組函數(shù)系數(shù)（如圖2所示）。將服務(wù)航天器的全部約束作為算法的約束條件，以曲線上對(duì)應(yīng)時(shí)間點(diǎn)的取值與樣本點(diǎn)狀態(tài)的偏差作為優(yōu)化指標(biāo)，當(dāng)偏差小于預(yù)定目標(biāo)時(shí)輸出當(dāng)前最優(yōu)的一組系數(shù)。最后將系數(shù)代入擬合函數(shù)得到優(yōu)化的光滑運(yùn)動(dòng)軌跡。選用的狀態(tài)擬合函數(shù)為帶有權(quán)系數(shù)的拉格朗日插值多項(xiàng)式

式中：Xi為待擬合參數(shù)在第i個(gè)樣本點(diǎn)處的取值；n為樣本點(diǎn)數(shù)；ki為第i狀態(tài)點(diǎn)的權(quán)系數(shù)，即為待優(yōu)化曲線參數(shù)；ti為第i狀態(tài)點(diǎn)對(duì)應(yīng)的時(shí)間。X（t）的一階導(dǎo)數(shù)形式為

為比較RRT算法結(jié)果與擬合得到的優(yōu)化結(jié)果，設(shè)計(jì)了軌跡的代價(jià)值

式中：n為RRT算法所得軌跡中總的節(jié)點(diǎn)數(shù)；δpi，δvi，δqi，δωi，δti分別為第i節(jié)點(diǎn)相對(duì)上一節(jié)點(diǎn)的位置、速度、四元數(shù)、角速度和時(shí)間的變化量；k1～k5為系數(shù)，作用是將各狀態(tài)量調(diào)整到統(tǒng)一或近似量級(jí)。

3 算例

3.1 末端接近段運(yùn)動(dòng)規(guī)劃的相關(guān)參數(shù)

為簡便直觀，將運(yùn)動(dòng)規(guī)劃坐標(biāo)系（Y、Z軸與質(zhì)心軌道坐標(biāo)系方向相反）原點(diǎn)設(shè)在末端接近段的初始位置，且規(guī)劃過程中只考慮速度、角速度的大小，則在此坐標(biāo)系中規(guī)劃問題基本參數(shù)為：初始位置［0 0 0］Tm；目標(biāo)位置［42.22 38.44 41.45］Tm；速度，初值0m／s，目標(biāo)值0.142m／s；初始姿態(tài)

目標(biāo)姿態(tài)

角速度，初值和目標(biāo)值均為0rad／s；目標(biāo)時(shí)間100s；飛行區(qū)域［43 43 43］m；障礙參數(shù)，球心［10.5 10.5 7.5］Tm，［31.5 31.5 34.5］Tm，立方體中心

3.2 末端接近段初始運(yùn)動(dòng)規(guī)劃

設(shè)服務(wù)航天器運(yùn)動(dòng)動(dòng)力學(xué)約束條件為恒定值，速度、加速度、角速度與角加速度的最大值分別為2.5m／s，0.4m／s2，0.2rad／s，0.05rad／s2。以一次仿真結(jié)果為例，步長2s時(shí)100 000次所用搜索時(shí)間241.5s，找到325路徑，其中代價(jià)最小值為53.90。最優(yōu)結(jié)果如圖4所示。

增加搜索次數(shù)可降低軌跡所需的代價(jià)值，但根據(jù)文獻(xiàn)［3］，搜索次數(shù)較大時(shí)，增加計(jì)算量對(duì)代價(jià)值降低的效果并不明顯，很難找到更優(yōu)路徑。

3.3 基于進(jìn)化算法的光滑軌跡規(guī)劃

取100 000次搜索所得結(jié)果中的20個(gè)狀態(tài)點(diǎn)作為初始點(diǎn)，進(jìn)化算法參數(shù)設(shè)置為種群數(shù)200個(gè)；新個(gè)體30個(gè)；進(jìn)化代數(shù)800；適應(yīng)度函數(shù)0.25。其中一次時(shí)間為452.6s，擬合所得軌跡和四元數(shù)如圖5所示。

擬合前后的速度、角速度、加速度和角加速度如圖6所示。由圖可知：擬合獲得了運(yùn)動(dòng)狀態(tài)的光滑連續(xù)曲線，且滿足服務(wù)航天器運(yùn)動(dòng)動(dòng)力學(xué)與空間障礙約束；代價(jià)值為41.79，降低了22.5%，實(shí)現(xiàn)了初始軌跡的優(yōu)化；擬合曲線初始狀態(tài)、目標(biāo)狀態(tài)與末端接近段要求的誤差很小。

圖4 100 000次搜索結(jié)果Fig.4 Result for search of 100 000times

圖5 軌跡與姿態(tài)擬合結(jié)果Fig.5 Fitting results of orbit and attitude

RRT，EA算法的速度、角速度、加速度和角加速度功率譜如圖7所示。由圖可知：基于進(jìn)化策略的平滑充分過濾了RRT軌跡規(guī)劃結(jié)果中的高頻項(xiàng)，使航天器的運(yùn)動(dòng)過程更平穩(wěn)，這是該法更節(jié)省燃耗、代價(jià)指標(biāo)更低的原因，平滑后的結(jié)果更利于工程實(shí)現(xiàn)。

圖6 各狀態(tài)量擬合前后對(duì)比Fig.6 Comparison of all states before and after fitting

圖7 功率譜分析Fig.7 Power spectrum analysis

3 結(jié)束語

本文利用基于采樣理論的快速搜索隨機(jī)樹方法解決高維問題的快速性以及進(jìn)化算法處理優(yōu)化問題的強(qiáng)魯棒性特點(diǎn)，提出了一種兩級(jí)軌跡規(guī)劃方法。先用改進(jìn)的雙向平衡搜索方式的RRT規(guī)劃方法，快速得到初始運(yùn)動(dòng)軌跡，再用進(jìn)化算法將軌跡擬合為光滑連續(xù)曲線，使代價(jià)指標(biāo)進(jìn)一步優(yōu)化。仿真結(jié)果表明：在滿足飛越式接近要求的同時(shí)，實(shí)現(xiàn)了對(duì)快速搜索隨機(jī)樹所得初始軌跡的優(yōu)化，降低了代價(jià)值，同時(shí)克服了進(jìn)化算法解決此類復(fù)雜約束問題時(shí)效率低的缺陷，確保了規(guī)劃速度，將總規(guī)劃時(shí)間控制在可接受范圍內(nèi)。

［1］ MATSUMOTO S，DUBOWSKY S，JACOBSEN S，et al.Fly－by approach and guidance for uncontrolled rotating satellite capture：AIAA Guidance，Navigation，and Control Conference and Exhibit［C］.Austin：2003.

［2］ MATSUMOTO S，JACOBSEN S，DUBOWSKY S，et al.Approach planning and guidance for uncontrolled rotating satellite capture considering collision avoidance：Proceeding of the 7th International Symposium on Artificial Intelligence，Robotics and Automation in Space：i－SAIRAS 2003［C］.NARA：2003.

［3］ LAVALLE S M，KUFFNER J J.Randomized kinodynamic planning［J］.International Journal of Robotics Research，2001，20：378－400.

［4］ ADIYATOV O，VAROL H A.Rapidly－exploring random tree based memory efficient motion planning［C］／／Mechatronics and Automation （ICMA），2013 IEEE International Conference on，2013.［s.l.］：［s.n.］，2013：354－359.

［5］ URMSON C，SIMMONS R.Approaches for heuristically biasing RRT growth［C］.Proceedings of the IEEE Conference on Intelligent Robots and Systems.Las Vegas：IEEE，2003：1178－1183.

［6］ 彭 輝，王 林，沈林成.區(qū)域目標(biāo)搜索中基于改進(jìn)RRT的UAV實(shí)時(shí)航跡規(guī)劃［J］.國防科技大學(xué)學(xué)報(bào)，2009，31（5）：86－91.

［7］ 王 平，郭繼峰，史小寧，等.基于時(shí)間狀態(tài)的敏捷自主在軌服務(wù)航天器協(xié)同運(yùn)動(dòng)規(guī)劃方法研究［J］.控制與決策，2011，26（11）：1699－1705.

［8］ AOUDE G S，HOW J P，GARCIA I M.Two－stage path planning approach for designing multiple spacecraft reconfiguration maneuvers［J］.Journal of the Astronautical Sciences，2008，56（4）：515－544.

［9］ HUNTINGTON G T.Advancement and analysis of a Gauss pseudospectral transcription for optimal control problems［D］.Cambridge：Massachusetts Institute of Technology，2007.

［10］ 仲維國，崔平遠(yuǎn)，崔枯濤.航天器復(fù)雜約束姿態(tài)機(jī)動(dòng)的自主規(guī)劃［J］.航空學(xué)報(bào)，2007，28（5）：1091－1097.

［11］ GARCIA I M，HOW J P.Trajectory optimization for satellite reconfiguration maneuvers with position and attitude constraints［C］／／ American Control Conf.Portland：［s.n.］，2005：889－894.

［12］ CREAMER G，DELAHUNT P，GATES S，et al.Attitude determination and control of cementine during Lunar mapping description and on－orbit performance［C］.AIAA Guidance，Navigation，and Control Conf.Baltimore：［s.n.］，1995：920－930.

［13］ KAVRAKI L E，LAVALLE S M，SICILIANO I B，et al.Springer handbook of robotics［M］.Berlin：Springer－Verlag，2008：109－133.

［14］ TASGETIREN M F，SUGANTHAN P N，PAN Q K，et al.An ensemble of differential evolution algorithms for constrained function optimization：Evolution Computation （CEC），2010IEEE Congress on［C］.IEEE，2010：1－8.