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

郝 瑞，孟云鶴，郭勝鵬

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

0 引言

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

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

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

1 問題描述

1.1 飛越式接近過程

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

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

已知初始相對運動狀態與飛越式接近總任務時間，則接近軌跡確定步驟如下：

a）由目標衛星運動狀態的測量值算得飛越接近目標點的運動狀態；

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

c）已知末端接近段的初始運動狀態、目標運動狀態與調整時間，對該段的運動軌跡進行規劃。

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

1.2 末端接近段的軌跡規劃問題

1.2.1 運動狀態與狀態空間

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

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

由于服務航天器的最優運動軌跡可能在障礙區域附近，規劃問題的總狀態空間應包括自由運動集合Xfree及障礙Xobs，即狀態空間X＝Xfree∪Xobs。

1.2.2 運動狀態度量

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

式中：K1～K5為權系數；ρ（qi，qj）為姿態四元數間的度量，且

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

1.2.3 約束條件

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

a）運動動力學約束

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

b）空間幾何約束

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

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

2 末端接近段的兩級運動規劃方法

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

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

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

2.1.1 局部規劃器

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

e1，e2分別為從節點pi到pi＋1的速度和旋轉角速度方向單位矢量。

2.1.2 改進的雙向平衡RRT算法

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

2.2 基于進化算法的光滑軌跡規劃方法

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

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

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

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

為比較RRT算法結果與擬合得到的優化結果，設計了軌跡的代價值

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

3 算例

3.1 末端接近段運動規劃的相關參數

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

目標姿態

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

3.2 末端接近段初始運動規劃

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

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

3.3 基于進化算法的光滑軌跡規劃

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

擬合前后的速度、角速度、加速度和角加速度如圖6所示。由圖可知：擬合獲得了運動狀態的光滑連續曲線，且滿足服務航天器運動動力學與空間障礙約束；代價值為41.79，降低了22.5%，實現了初始軌跡的優化；擬合曲線初始狀態、目標狀態與末端接近段要求的誤差很小。

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

圖5 軌跡與姿態擬合結果Fig.5 Fitting results of orbit and attitude

RRT，EA算法的速度、角速度、加速度和角加速度功率譜如圖7所示。由圖可知：基于進化策略的平滑充分過濾了RRT軌跡規劃結果中的高頻項，使航天器的運動過程更平穩，這是該法更節省燃耗、代價指標更低的原因，平滑后的結果更利于工程實現。

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

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

3 結束語

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

［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］ 彭 輝，王 林，沈林成.區域目標搜索中基于改進RRT的UAV實時航跡規劃［J］.國防科技大學學報，2009，31（5）：86－91.

［7］ 王 平，郭繼峰，史小寧，等.基于時間狀態的敏捷自主在軌服務航天器協同運動規劃方法研究［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］ 仲維國，崔平遠，崔枯濤.航天器復雜約束姿態機動的自主規劃［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.