多約束航天器飛越式接近的兩級運動規(guī)劃方法

郝 瑞，孟云鶴，郭勝鵬

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

0 引言

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

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

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

1 問題描述

1.1 飛越式接近過程

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

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

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

a）由目標衛(wèi)星運動狀態(tài)的測量值算得飛越接近目標點的運動狀態(tài)；

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

c）已知末端接近段的初始運動狀態(tài)、目標運動狀態(tài)與調(diào)整時間，對該段的運動軌跡進行規(guī)劃。

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

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

1.2.1 運動狀態(tài)與狀態(tài)空間

已知末端接近段的初始和目標運動狀態(tài)分別為

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

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

1.2.2 運動狀態(tài)度量

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

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

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

1.2.3 約束條件

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

a）運動動力學約束

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

b）空間幾何約束

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

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

2 末端接近段的兩級運動規(guī)劃方法

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

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

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

2.1.1 局部規(guī)劃器

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

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

2.1.2 改進的雙向平衡RRT算法

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

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

圖3 障礙相對較大時運動規(guī)劃Fig.3 Motion planning when obstacles were relatively large

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

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

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

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

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

3 算例

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

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

目標姿態(tài)

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

3.2 末端接近段初始運動規(guī)劃

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

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

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

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

擬合前后的速度、角速度、加速度和角加速度如圖6所示。由圖可知：擬合獲得了運動狀態(tài)的光滑連續(xù)曲線，且滿足服務(wù)航天器運動動力學與空間障礙約束；代價值為41.79，降低了22.5%，實現(xiàn)了初始軌跡的優(yōu)化；擬合曲線初始狀態(tài)、目標狀態(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所示。由圖可知：基于進化策略的平滑充分過濾了RRT軌跡規(guī)劃結(jié)果中的高頻項，使航天器的運動過程更平穩(wěn)，這是該法更節(jié)省燃耗、代價指標更低的原因，平滑后的結(jié)果更利于工程實現(xiàn)。

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

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

3 結(jié)束語

本文利用基于采樣理論的快速搜索隨機樹方法解決高維問題的快速性以及進化算法處理優(yōu)化問題的強魯棒性特點，提出了一種兩級軌跡規(guī)劃方法。先用改進的雙向平衡搜索方式的RRT規(guī)劃方法，快速得到初始運動軌跡，再用進化算法將軌跡擬合為光滑連續(xù)曲線，使代價指標進一步優(yōu)化。仿真結(jié)果表明：在滿足飛越式接近要求的同時，實現(xiàn)了對快速搜索隨機樹所得初始軌跡的優(yōu)化，降低了代價值，同時克服了進化算法解決此類復(fù)雜約束問題時效率低的缺陷，確保了規(guī)劃速度，將總規(guī)劃時間控制在可接受范圍內(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ū)域目標搜索中基于改進RRT的UAV實時航跡規(guī)劃［J］.國防科技大學學報，2009，31（5）：86－91.

［7］ 王 平，郭繼峰，史小寧，等.基于時間狀態(tài)的敏捷自主在軌服務(wù)航天器協(xié)同運動規(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］ 仲維國，崔平遠，崔枯濤.航天器復(fù)雜約束姿態(tài)機動的自主規(guī)劃［J］.航空學報，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.