時變指向約束下姿態參數空間離散化路徑規劃方法

朱哲，徐瑞，李朝玉，朱圣英，龍嘉騰，朱雷

（1.北京理工大學深空探測技術研究所，北京 100081；2.深空自主導航與控制工信部重點實驗室（北京理工大學），北京 100081；3.上海航天控制技術研究所，上海 201109）

0 引言

航天器在姿態機動時，其本體或可動部件的姿態受到指向約束，這類約束縮小了航天器姿態機動的可行域，被稱作姿態指向約束。航天器姿態違反姿態指向約束時，會出現強光天體進入相機視場、天線偏離通信目標等問題，航天器的導航定姿、科學觀測或通信傳輸能力將遭到損壞，影響任務正常執行。姿態指向約束包括禁忌指向約束和強制指向約束［1］。進一步地，隨著深空探測任務的發展，航天器在姿態機動任務中可能同時面臨多種姿態指向約束。例如在以OSIRIS-REx 為代表的小行星采樣返回任務中，探測器在對自旋小行星進行附著抵近時，需要同時規避太陽強光干擾并對小行星地貌進行觀測［2-4］。前者是傳統靜態禁忌指向約束；而后者在快速自旋小行星的條件下，對探測器提出了強制指向約束，且約束方向矢量在日心軌道慣性系下是動態變化的，形成的指向變化角不可忽視，構成了動態時變的強制指向約束。另外，探測器在行星探測任務中，慣性系下部分天體，例如行星的衛星，相對于探測器有較高動態的相對指向變化。當該天體具有高反射率時，例如卡西尼任務中探測器飛掠經過土衛二時，會對光學敏感器（如導航相機、星敏感器）等構成強光干擾［5-7］，該類天體對探測器造成了時變禁忌指向約束。又例如，在類似羅塞塔號的彗星探測任務中，探測器在彗星抵近觀測時受到彗尾和彗星氣體噴流對敏感器和載荷視場的影響，需執行一定的姿態機動進行規避［8-9］。由于彗星相關地質活動具有不確定和動態特性，這類深空環境對探測器載荷指向軸構成了一類動態的禁忌指向約束。這類新型復雜任務場景使得深空探測器面臨與以往任務不同的、隨時間變化的姿態指向約束，該約束使姿態機動可行域成為一個動態空間，探測器需要在姿態參數的動態空間中滿足姿態對準或跟蹤的機動需求。而傳統姿態路徑規劃大多只能使用靜態空間的路徑搜索算法［10-12］處理靜態指向約束。時變指向約束帶來的新問題對姿態路徑規劃方法的應用提出了挑戰。

針對復雜多約束下航天器的姿態機動規劃問題，姿態規劃方法需要統一考慮指向約束和動力學約束等多種約束，綜合求解得到姿態機動路徑（姿態參數序列）和包括角速度和控制力矩序列在內的姿態機動軌跡。目前，求多約束下無目標優化航天器的姿態機動離散可行解主要采用包括勢函數法、隨機規劃算法、空間離散化路徑搜索算法在內的啟發式約束評價規避算法。其中勢函數法適合處理簡單指向約束的情況［13］。隨機規劃方法能高效適應航天器的各種復雜約束情況，He等［14］使用RRTGoalBias 算法生成了時變指向約束下的姿態機動路徑，并提出姿態隨動球面來表示姿態機動和指向約束變化的動態過程。但其隨機擴展過程生成的路徑具有不確定性，路徑不具有全局優化性。空間離散化路徑搜索算法因具備較強的幾何約束處理能力和較為完備的全局優化性能，在時變指向約束下的姿態規劃問題中具有較高研究價值。

空間離散化路徑搜索算法首先對姿態空間或姿態參數空間進行投影離散化，然后采用圖搜索算法求解得到滿足指向約束的姿態機動路徑。圖搜索算法以已知信息的姿態和約束空間為基礎，通過構建圖搜索尋找到達目標的機動路徑。首先對姿態空間或者姿態參數空間進行投影離散網格劃分，然后求解得到滿足指向約束的姿態機動路徑。常見的圖搜索算法包括A*搜索算法，其最初由Hart等［15］發表。通過構造啟發式函數，A*算法可以獲得較好的尋的性能。Stentz［16］在此基礎上提出D*和Focused D*算法，在處理動態插入障礙的地圖路徑搜索方面獲得了較好效果。近年來圖搜索算法被應用在姿態機動路徑的規劃問題求解上，Kjellberg等［17］通過二十面體方法將單位天球離散化為描述某個本體固聯軸方向的像素圖，而后利用A*搜索算法得到滿足單軸禁忌約束的姿態機動路徑。在此基礎上，Kjellberg等［18］又進一步擴展算法的通用性，將二十面體方法應用于姿態四元數表示方式形成的超球面空間的離散化當中，實現了求解復雜多軸指向約束下的姿態機動路徑。Tanygin［19］采用最小失真變換將完整的三軸姿態表示投影到一個三維空間，并將該投影空間在三維笛卡爾網格上離散化，而后利用圖搜索算法得到滿足復雜多軸約束的姿態機動路徑。進一步地，Tanygin［20］又將該算法由求解單一目標點問題擴展到求解多個目標點構成的連續體問題。Calaon等［21］針對航天器敏感器對準導航天體和載荷規避強光天體的需求，將路徑搜索算法應用于修正羅德里格斯參數的三維網格中，生成滿足指向約束的離散參考路徑，進而采用B-樣條曲線擬合路徑離散點，滿足姿態運動學的同時能夠優化控制力矩代價。進一步地，Calaon等［22］將擬合曲線擴展到非均勻有理B 樣條曲線（NURBS），從而得到整體控制力矩代價更優的姿態軌跡。空間離散化路徑搜索算法可以有效地處理多軸復雜指向約束，并通過啟發式評價函數最小化從起點到終點的路徑上所有節點的總路徑代價。但該算法本身適用于幾何類型約束，生成姿態路徑離散節點，無法處理角速度和控制力矩有界約束以及姿態動力學約束，動力學類型的約束需要在路徑擬合和軌跡規劃算法中考慮。

為了使圖搜索算法生成的路徑滿足動力學約束，引入軌跡規劃算法生成含有角速度和控制力矩的完整姿態機動規劃序列。以多項式方法［23-24］為例，采用一定階數的多項式來表示航天器姿態機動過程中的姿態變化曲線，然后結合逆動力學方法［25］求解角速度和控制力矩。通過優化多項式系數，實現匹配邊界條件。

本文提出了一種姿態參數空間離散化路徑規劃方法，通過姿態參數空間離散化、動態空間路徑搜索、逆動力學軌跡規劃3 部分實現完整的姿態機動規劃。首先對基于修正羅德里格斯參數的空間離散化方法進行研究，實現姿態空間離散化的空間構建和指向約束表達。在此基礎上，設計時變指向約束下的姿態路徑搜索方法，設計非奇異空間搜索代價函數，通過引入節點時間特性處理時變指向約束，實現姿態路徑生成。進一步采用多項式路徑擬合，用逆動力學法計算姿態機動的角速度和控制力矩軌跡。最后數值仿真展示了不同組合的時變指向約束下，采用空間離散化方法規劃的姿態機動路徑的效果，驗證了時變指向約束下路徑規劃的可行解。

1 姿態參數空間離散化

對于姿態參數空間離散化方法，需要針對不同的姿態描述方式，即姿態描述參數，選擇不同的三維空間離散化方法，并實現姿態機動的指向約束表示。本節對修正羅德里格斯參數（MRP）描述方式進行了參數空間離散化處理，從而構建姿態空間的三維離散網格，結合離散空間的指向約束表達，得到姿態機動的路徑規劃空間，為空間離散化路徑搜索算法設計提供了基礎。

1.1 姿態描述參數

航天器姿態采用本體坐標系與慣性坐標系之間的相對轉動關系來描述，剛體航天器姿態可以采用四元數和修正羅德里格斯參數進行描述。

1）四元數

采用單位四元數描述航天器的姿態：

式中：ev表示沿歐拉軸的特征向量；?表示歐拉旋轉角，?∈[0，180°]。由于‖q‖=1（‖ · ‖表示歐幾里得范數），四元數的4 個參數中只有3 個獨立，四元數空間事實上是一個四維超球面。

2）修正羅德里格斯參數

修正羅德里格斯參數（MRP）用于表示SO（3）空間中的剛體旋轉姿態，是三維向量表示的姿態參數，可由四元數導出：

式中：?∈[0，360°)

相較于四元數空間，對MRP 參數空間進行離散化處理好處在于，MRP的3個參數相互獨立，因此構成一個無限大的三維空間，R3內任意點均表示一個有效的三軸姿態指向（但一個三軸姿態并不唯一對應于R3內某一個點）。但與此同時，?等于360°時MRP發生奇異，為了避免該奇異性，在MRP的“無限空間”中劃出一定范圍的集合，為后續路徑搜索提供一個非奇異空間。

由于對MRP 表示的每個旋轉σ=(ev，?)，存在一個對應的影集（Shadow set）σ'=(ev，?')，?'=?-2π，表示相同的姿態［26］。對于單個σ對應的σ'，下文稱為節點σ對應的影節點σ'。對于σ'的分量：

式（3）成立時，2 組MRP 參數σ和σ'取值不同，是參數空間中不同的點，但表示相同的姿態，當‖σ‖＞1時‖σ'‖＜1。可以證明，對于MRP在表示一個360°旋轉，即?=360°時的奇異情況，有MRP 空間原點處σ0可表示同一姿態。

由此，選取限定區域的MRP 空間集合，MRP 空間以單位球為界，對于球外的一個節點所表示的姿態，存在球內某一個對應的節點所表示姿態與之相同。而對于單位球球面上的點，有‖σ‖=1，σ'=-σ，其中心對稱的節點即其“影節點”。

由上述可以得到，所需離散化的MRP 參數空間可以限制在一個單位球內部及其球面，任意剛體航天器姿態可對應該集合內的一個三維MRP 坐標，該單位球及其球面構成了路徑搜索的非奇異空間。

采用MRP 參數σ描述慣性系下的航天器姿態，在σ表示下的姿態運動學方程如下所示，方程說明了MRP 參數的時間導數與本體系下角速度ω的關系。

式中：σ2=‖σ‖2，[B(σ)]定義為：

式中：σ×是σ的斜對稱叉乘矩陣。

1.2 修正羅德里格斯參數空間離散化

由于MRP 的參數空間不需經過投影、扭曲，其離散化節點通過三維笛卡爾網格劃分即可實現均勻分布，相鄰節點距離相等（其中單位球面上的離散節點取笛卡爾網格與球面的交點），路徑搜索算法以離散化MRP 空間為工作空間。笛卡爾網格的離散化精細度可用笛卡爾坐標系一條半軸上單位球內等距排列的節點個數表示，例如單元格長度為1/a，離散化精細度N=a。

以精細度N=60 為例，離散化MRP 空間如圖1所示。

圖1 修正羅德里格斯參數空間笛卡爾網格劃分效果及其剖面局部Fig.1 Cartesian grid of MRP space and its cross-section（part）

由立方體結構可知，對于距離單位球表面足夠遠的每個節點，其相鄰節點共26個，分3 種相鄰節點距離的取值‖ ‖σ=1 球面表面節點的鄰近節點應包括其對應“影節點”σ'=-σ，如以為當前節點σi的相鄰擴展節點，位于單位球的對側，從而保證姿態機動在歐拉旋轉角大于180°時的路徑連續性。

1.3 指向約束在姿態參數空間的表示

姿態指向約束是一種幾何約束，在參數空間的路徑搜索中縮小了姿態機動路徑的可行域，限定了規劃路徑的邊界。因此，需要將指向約束也映射到參數空間中。姿態指向約束包括禁忌指向約束和強制指向約束，分別表示規避特定指向進入視場角的約束和“強制”姿態處于指向角范圍內的約束。rgi表示光敏元件、載荷等設備在本體系下的視場中心（特征軸）方向矢量，rI表示約束關聯天體在慣性系下的方向矢量，θj表示約束對應的視場角。本文考慮時變指向約束，當載荷目標天體相對于航天器本體運動，例如著陸器對自旋小行星的表面觀測，rI隨時間發生指向變化，約束視場角也可能時變。指向約束要求天體指向和設備特征軸的方向矢量夾角與視場角構成不等式約束，下式中符號“≤”表示禁忌約束：

式中：CIB，CBI是本體系與慣性系之間的方向余弦矩陣。

符號“≥”表示強制約束：

對于MRP 參數空間內的姿態指向約束表達問題，由于空間中任一節點需要同時滿足全部約束，任意一個在禁忌指向約束之內，或強制指向約束之外的姿態對應的姿態參數節點都應從路徑搜索的可行域中排除。值得注意的是，對于相鄰兩個可行域上的節點，其節點連接的轉移路徑事實上不一定在連續的每一處都滿足約束，但由于姿態指向約束均為凸約束，該情況可通過設置適當的離散化精細度N避免。

以圖例說明，對于安裝在剛體探測器本體系z軸上的敏感器，視場角為20°，考慮兩個方位角-仰角分別為［90°，-30°］，［-90°，30°］和一個沿z軸正方向的禁入錐，其指向約束在慣性系單位天球和MRP參數空間的表示分別如圖2、圖3所示。

圖2 慣性系下指向約束Fig.2 Pointing constraints in inertial frame

圖3 修正羅德里格斯參數空間內的指向約束Fig.3 Pointing constraints in MRP space

類似地，對于兩個方位角-仰角分別為［0°，0°］、［-180°，0°］，視場角分別為20°的禁入錐，和方位角-仰角為［-180°，0°］，視場角為20°的強制錐，其指向約束在慣性系單位天球和MRP 參數空間的表示分別如圖4、圖5所示：

圖4 慣性系下指向約束Fig.4 Pointing constraints in inertial frame

圖5 修正羅德里格斯參數空間內的指向約束Fig.5 Pointing constraints in MRP space

圖6 靜態約束下借助MRP影節點的連續路徑Fig.6 Continuous path under static constraints by MRP switching

由此，在MRP 空間中，每個姿態指向約束構成了一塊連通的幾何區域，姿態對應的節點需要受到在該區域之內或之外的約束，即強制或禁忌指向約束。進而，時變指向約束是約束方向矢量rI在慣性系下動態變化的指向約束，時變指向約束在MRP 空間表示為隨時間運動的一個連通區域，使得MRP 空間中路徑搜索的可行域邊界動態變化。

2 時變指向約束下姿態路徑搜索方法

在姿態參數空間離散化和指向約束表達實現的基礎上，姿態機動的路徑規劃問題已經轉化為一個路徑搜索問題。問題求解的目標是在離散化的MRP 空間內，從起點代表的初始姿態，到終點代表的目標姿態，找到一條能夠滿足時變指向約束的三維路徑，時變指向約束由動態變化的幾何障礙/可行域所代表。本文分析了以Focused D*為代表的路徑搜索算法在MRP 空間中進行時變約束下路徑規劃的不足，從非奇異空間搜索和時變約束沖突定位兩個方面進行改進，設計了非奇異空間時變約束路徑搜索方法，得到滿足時變約束的姿態機動執行路徑，該路徑由離散節點序列組成。

2.1 Focused D*路徑搜索算法分析

由于D*和Focused D*分別是Dijkstra 和A*的反向搜索，Focused D*算法中當前節點σi的啟發式評價函數包括兩個部分。前序代價函數（h(σi)）表示從終點到當前節點的路徑代價，當當前節點與指向約束沖突時，代價函數將賦予較大的懲罰值。后序代價函數（g(σi)）表示當前節點到起點的路徑代價。每個節點的總路徑代價f(σi)=g(σi)+h(σi)。

Focused D*算法中的主要函數包括：1）PROCESS_STATE（），用于尋找f(σi)最小的最優路徑；2）MODIFY_COST（），用于路徑與動態約束沖突時做節點代價修正，從而規劃新的路徑。

每個節點的總路徑代價f(σi)=g(σi)+h(σi)。前序代價函數h(σi)等于前一節點h(σi-1)加上前一節點網格坐標grid(σi-1)到當前節點坐標grid(σi)的路徑轉移代價，一般采用歐幾里得距離計算，σ0=σG為終點坐標。如果當前節點違反指向約束則懲罰值設為inf，節點擴展過程中動態維護節點的h(σi)和k值。

后序代價函數g(σi)由當前節點網格坐標grid(σi)到起點網格坐標grid(S)的歐幾里得距離計算得到。

對于本文所解決的時變指向約束問題，Focused D*算法存在以下不足：

1）規劃解在MRP空間無最優性

將Focused D*路徑搜索算法應用于MRP 空間，應考慮到將搜索空間設定在MRP 單位球范圍內時，該空間的非奇異性是由單位球外側節點集的影集能映射到球內部來保證的，該搜索空間實際上無邊界。因此應當存在這樣的路徑：從起點向球面外側方向擴展，到達球面后，取在球面上的路徑節點的影節點，通過相對于球心處于對側位置的影節點回到球面內部，擴展到達目標位置。該路徑是連續的。

原Focused D*算法以當前節點到目標節點的歐幾里得距離為代價函數，生成解將排除所有上述穿越邊界、對側返回的路徑（如圖8所示），路徑解失去了最優性。為實現非奇異空間搜索，路徑代價計算時的歐幾里得距離計算應與影節點到目標節點的距離做對比。

2）路徑修正函數無法定位與時間相關的約束沖突節點

MODIFY_COST（）的路徑修正函數在原框架下是觸發式的，即一旦將初始路徑所含節點取代為障礙位置，便修正從被取代節點開始到目標節點的路徑，生成新的路徑。因此原方法無法估計時變指向約束在某一時刻與初始路徑的沖突節點位置，進而無法定位路徑修正的觸發點，無法完成時變約束下的路徑規劃。

對于時變指向約束問題，在考慮時間特性的情況下，航天器執行的姿態機動在當前時刻的姿態對應了路徑的當前節點σi，過去時刻的姿態對應路徑上身后的節點σi-m，未來預期執行的姿態對應路徑上身前的節點σi+n，n＞1。如果因為指向約束的時變，由過去時刻姿態組成的已執行路徑在當前時刻與約束發生沖突，或未來預期執行的路徑在當前時刻與約束發生沖突，都不能被證明會影響航天器實時姿態機動的執行，因而無需修正現有路徑。因此，在時變指向約束下的姿態路徑規劃當中，需要將節點放在時間線上，每個節點具有自身唯一對應的時刻，即航天器姿態機動至該狀態的時刻。判斷當前節點到下一節點的擴展是否與該時刻的指向約束沖突，沖突則調用MODIFY_COST（）修正路徑。值得注意的是，這一路徑上的動態修正是預先規劃的，建立在已知時變函數的動態約束條件上，而非執行過程中的實時修正。

根據上述不足，本文對時變指向約束下的路徑搜索算法進行改進，提出了非奇異空間時變約束路徑搜索算法。

2.2 非奇異空間時變約束路徑搜索算法

在Focused D*算法應用中，路徑代價函數是基于MRP 空間中節點間的歐幾里得距離d(σ1，σ2)計算的。由于MRP 節點σ存在影節點σ'，節點之間的距離計算應當選取grid(σi)和grid()到另一節點的歐氏距離的最小值，即：

如此則路徑搜索算法可以在MRP 非奇異空間中向各個方向均等地擴展，確保規劃出總代價函數最優的全局路徑的合理性。前序代價函數h(σi)定義如下：

后序代價函數g(σi)定義如下：

當姿態機動歐拉旋轉角大于180°時，借助MRP影節點的連續路徑效果如下。

由式（9），在‖ ‖σ=1 的球面表面節點與其影節點σ'的距離=0。當路徑擴展出單位球面，而通過影節點從球面的中心對稱一側進入時，=0，說明路徑是連續的。

在時變指向約束條件下，離散化空間和指向約束構成的路徑搜索空間需要隨節點在初始規劃路徑上的運動動態更新。由路徑搜索算法生成的含N+1 個節點{q0，q1，…，qN} 的姿態機動路徑被分為N段，需對每一段路徑的邊界節點賦予時刻特性。由于姿態總機動時間T未知，對T的估計可以通過假設一個期望的平均機動角速度ωˉ，結合式（4）、（5）得到：

通過估計的總機動時間和期望平均角速度，可將路徑上節點對應時刻{t0，t1，…，tN}設為與節點路徑長度成比例關系的數值，即：

節點在路徑上對應時刻的時變指向約束為{rI(ti)，θk(ti) }，i=0，…，N，k表示第k個指向約束的視場角。將時變指向約束引入路徑搜索算法的動態環境更新中，可采取兩種方式：

1）沿總路徑代價最小的最優路徑轉動過程中算法存儲的map 空間動態更新，這種方法需要在循環中重復離散化的空間構建，用時較長；

2）在節點遇到障礙的判斷函數isCollision（X）中加入對時變指向約束的判斷，需要將MRP 笛卡爾網格節點坐標轉換為姿態參數，進而利用式（7）、（8）對指向約束特征軸方向矢量rgj（j個指向約束）進行判斷。

采用兩種方式的非奇異空間時變約束路徑搜索算法結構中如圖7所示：

圖7 非奇異空間時變約束路徑搜索算法流程圖Fig.7 Flowchart of nonsingular time-variant path searching algorithm

3 逆動力學方法求解姿態連續軌跡

對于路徑搜索生成的離散節點序列，需要采用插值擬合曲線將離散路徑節點串起來，從而生成一條航天器能夠跟蹤執行的姿態機動軌跡。本節首先給出四元數表示下的姿態逆運動學和動力學方程。進一步地，由于路徑搜索的MRP 節點序列可以簡單地轉換為四元數節點序列，對路徑節點序列采用四元數多項式插值進行擬合。生成的四元數時間擬合曲線便可使用逆動力學方法計算角速度和控制力矩曲線，得到完整的姿態機動軌跡。

3.1 姿態逆運動學和逆動力學

剛體航天器本體坐標系下的姿態動力學方程為：

式中：J是慣量矩陣，ω=[ωx，ωy，ωz]T是角速度矢量，T是控制力矩。采用單位四元數q=描述慣性系下的航天器姿態，設Ω=[0，ωx，ωy，ωz]T，姿態運動學方程如下：

式中：運算符?表示四元數乘法。

設q*是q的共軛四元數，逆運動學方程如下［25］：

對上式求微分得到：

式中：qI=[1，0，0，0]T。結合式（14）的動力學方程和式（17），可以得到用于計算控制力矩T的逆動力學方程：

式中：vec(·)表示四元數的向量部分。式（16）和（18）說明可以由q(t)推導ω(t)和T(t)。

3.2 基于四元數多項式插值的路徑擬合

采用基于四元數多項式插值的逆動力學方法［27］，生成滿足離散路徑點的姿態機動連續軌跡，包括姿態角速度和控制力矩。

對于路徑搜索算法生成的含有N+1 個節點{σ0，σ1，…，σN}的姿態機動路徑，使用式（2）的轉換關系得到{q0，q1，…，qN} 的四元數路徑。路徑分為N段，在每段路徑的時間[ti-1，ti]內，對四元數4 個分量的分別采用三次多項式插值，即分段三次樣條插值，滿足邊界條件{q0，q1，…，qN} 。插值多項式如下：

總共有4N個三次插值多項式，有16N個多項式系數需求解。姿態機動路徑節點和邊界條件提供了等數量的約束方程。其中邊界條件{q0，q1，…，qN}和初始及末端角速度為零提供了8N+8個約束方程：

式中：qi，j是qi第j個分量。

由分段多項式的分段連續條件，除去總路徑的兩個端點，中間N-1 個節點處角速度和角加速度，即的一階和二階導數相等，這提供了余下的8(N-1)個約束方程：

進一步地，姿態機動軌跡的四元數需要滿足歸一化條件，即‖q(t)‖=1。歸一化插值函數表示為：

綜上所述，通過計算多項式全部系數，可以得到q(t)，，的分段數值表達式，角速度ω和控制力矩T可以由式（16）和（18）計算。

特殊地，當離散節點序列類似圖8，通過MRP影節點保證路徑連續到達目標節點時，難以直接進行插值擬合。可以先將節點序列映射為影節點序列，與原影節點分段相連接，構成連續路徑。進而可以采用本節方法插值擬合生成解析軌跡，得到角速度和控制力矩時間曲線。

圖8 姿態參數空間離散化方法在MRP空間的規劃路徑Fig.8 Path planned by attitude parameter space discretization path planning method in MRP space

4 數值仿真

為了驗證所提出的姿態參數空間離散化路徑規劃方法的可行性，本節進行了數值仿真。考慮了深空探測器在外部動態指向約束下，從初始到目標姿態的重新定向姿態機動過程。首先，對多個時變禁忌指向約束下的姿態機動路徑規劃進行仿真，對比生成路徑與傳統方法結果的更優性。然后，同時考慮禁忌約束和時變強制約束，給出這一條件下的姿態規劃結果。

4.1 動態禁忌指向約束下姿態規劃

對姿態路徑規劃的參數空間離散化方法進行仿真，考慮深空探測器在本體系z軸方向上安裝一個光學敏感器，其單位方向矢量用rB表示；需要在姿態機動過程中規避兩個明亮天體，在慣性系下的單位方向矢量用rF1，rF2表示；要求rB與rF1，rF2之間的最小夾角為θF1，θF2；考慮指向約束的時變條件，對rF1，rF2有旋轉角速度ωF1，ωF2。環繞器轉動慣量為J，初始姿態為q0，目標姿態為qf；初始和目標角速度符合ω0，ωf符合雙零約束。在路徑規劃和軌跡規劃的參數定義方面，空間離散化的精細度定義為N，期望平均角速度為，δt是路徑擬合時間步長。具體數值如表1所示。

表1 禁忌指向約束仿真條件Table 1 Simulation parameters under forbidden constraints

仿真環境為Matlab R2021b，計算機主頻3.20 GHz，內存16 G。在時變指向約束下，采用基于MRP 空間離散化的姿態路徑搜索算法，在表1 仿真條件下對問題求解。姿態參數空間離散化方法在MRP 空間的規劃路徑如圖8 所示，算法在單位天球慣性系上的姿態指向路徑如圖9 所示，該路徑是探測器在算法的規劃結果不斷修正過程中實際執行的節點路徑。

圖9 敏感器光軸在單位天球慣性系上的指向路徑（圖中所示為初始指向約束）Fig.9 Path of sensor axis on unit celestial sphere in inertial frame（initial pointing constraints displayed）

圖8展示了MRP空間中成功規避兩個禁忌約束的實際運動節點組成的機動路徑。由于禁忌約束的方向矢量隨時間在慣性系下擺動變化，圖9 展示了與圖8 對應的在單位天球慣性系下，敏感器指向實際執行的機動路徑。該實際機動路徑與每一次路徑搜索算法規劃的路徑都不同，這是因為每當原先規劃路徑與該時刻時變指向約束沖突時，算法都對路徑進行動態修正，生成從當前點到目標的新路徑。

規劃路徑動態修正中的變化展示在圖10中（選取了3次修正路徑），三角形符號表示當前時刻姿態機動的敏感器指向，即路徑上的當前姿態節點；指向約束隨時間從圖中左側擺向右側。從起點到終點的路徑經過算法修正的次數實際上等于時變指向約束與當前節點沿先前路徑擴展發生沖突的次數。通過多次路徑修正，最終確定一條與圖9所示相同的、探測器可實際執行規避時變約束的機動路徑。

圖10 單位天球慣性系上不同時刻下算法修正的三維指向路徑（指向約束隨時間在圖中從左向右擺動）Fig.10 3D pointing path on unit sphere in inertial frame at different time（as time-variant pointing constraints swinging from left to right）

圖11 給出了MRP 和單位四元數及角速度、控制力矩隨時間的變化軌跡。MRP 和四元數之間可以相互轉換，其時間曲線說明姿態機動過程無退繞。角速度時間曲線顯示角速度的模在期望值ωˉ=0.03 rad/s 附近保持小幅波動。單軸控制力矩最大值不超過1 N · m。其中，控制力矩時間曲線在起始和末端的較大幅值對應角速度在起始和末端的加速和減速，從而滿足角速度的雙零約束。在40～50 s時間段控制力矩的快速增加造成了角速度分量的較大變化，角速度發生轉向，對應了此時路徑對于禁忌約束2的“應急”修正。

圖11 MRP、單位四元數及角速度、控制力矩隨時間的變化軌跡Fig.11 Trajectory of attitude（MRP and quaternion），rate and control torque over time

與文獻［14］中時變指向約束下基于改進RRT的姿態規劃方法進行對比，由于RRT 方法規劃結果具有隨機不確定性，取多次重復規劃結果，將本文方法與其進行比較。在相同仿真條件下，將上述結果的規劃路徑長度與RRT 方法的40 次規劃結果進行比較，如圖12所示。

圖12 空間離散化方法規劃路徑長度與RRT方法的40次重復仿真結果對比Fig.12 Path length comparison between space discretization method and RRT for 40 times

其中姿態機動路徑的長度計算方式如下［18］：

式（24）計算得到兩姿態節點對應姿態qm，qn之間的路徑長度（rad）。由此可以計算整條四元數姿態路徑的機動“路程長度”，即路徑長度。圖12 顯示，本文的路徑搜索算法基于Focused D*算法進行改進，規劃路徑的評價函數與路徑長度相關，因此具有較好的路徑長度優化效果，與文獻［14］中RRT 方法的結果相比，路徑長度大大縮短。圖13直觀展示了天球坐標系下本文方法與隨機選取的文獻［14］中RRT 方法生成的2 條路徑的對比，RRT 方法結果因其隨機不確定性，對約束的規避方向可能不同，路徑的長度分布差異較大。

圖13 敏感器光軸在單位天球慣性系上的指向路徑（本文方法與RRT方法結果對比）Fig.13 Path of sensor axis on unit celestial sphere in inertial flame（results compared with RRT）

需要說明的是，圖13反映了單位天球上的敏感器指向路徑，該路徑長度與式（24）計算的四元數路徑長度不同：前者只能反映敏感器方向矢量在天球上劃過的角度，后者則是探測器三軸姿態在三軸姿態機動中的路徑長度度量。圖13 中兩條RRT 方法路徑由式（24）計算，長度分別為5.085 6和7.140 8，作為對比，本文方法生成路徑的長度為1.925 3.

4.2 動態強制指向約束下姿態規劃

進一步地，考慮小行星探測器在附著過程中對自旋小行星進行定向觀測的時變強制指向約束條件，定義附著相機安裝在本體系z軸方向上。在姿態機動過程中同時規避2 個明亮天體，構成靜態的禁忌約束，方向矢量為rF1，rF2，視場角設置與前一節相同。在慣性系下的強制約束方向矢量用rM1表示，要求rB與rM1之間的最小夾角為θM1，rM1有旋轉角速度ωM1。對應ωM1的取值大小，設置期望平均角速度ωˉ。定義新的初始與目標姿態q0，qf，仿真條件設置如表2。

表2 加入強制指向約束的仿真條件Table 2 Simulation parameters introducing mandatory constraints

其中強制指向約束角速度ωM1參考小行星2016HO3自旋周期0.467 h，角速度大小設為0.2°/s，約等于0.003 5 rad/s.

在上述條件下，姿態參數空間離散化方法在MRP 空間和慣性系單位天球上，探測器實際機動路徑如圖14，15所示。由于強制約束要求三角形符號表示的當前姿態節點始終在圖15 中淺藍色強制約束范圍內，當前節點到目標姿態節點σG的路徑由于超出強制約束邊界而無法規劃，需要設置一個臨時目標節點。將臨時目標節點設為：位于當前時刻指向約束的可行域內，到目標姿態節點的距離（用式（9）計算）最小的節點，該節點在強制約束邊界上。采用分段式規劃，分別規劃一條滿足強制約束和禁忌約束的、從當前節點σi到臨時目標節點的路徑，和一條只需滿足禁忌約束的、從臨時目標節點σ'G到目標姿態節點σG的路徑。兩段路徑相連接即從起始到目標姿態，且滿足當前時刻強制約束和禁忌約束的機動路徑。當前節點擴展與時變強制約束沖突時，如果≠σG，則選取新的臨時目標節點，修正σi到的路徑，并修正到σG的路徑；如果=σG，直接修正σi到σG的路徑。最終得到一條每一時刻姿態節點都滿足時變強制指向約束的探測器可執行路徑，即圖14、15所示路徑。

圖15 單位天球慣性系上不同時刻下算法修正的三維指向路徑（強制約束隨時間在圖中從上向下擺動）Fig.15 3D pointing path on unit sphere in inertial frame at different time（as time-variant mandatory constraints swinging from top to bottom）

MRP 和單位四元數及角速度、控制力矩隨時間的變化軌跡如圖16所示。角速度時間曲線顯示，角速度的模在期望值ωˉ=0.004 rad/s附近保持小幅波動。單軸控制力矩最大值不超過0.02 N·m。由此可知，與前一節角速度期望值ωˉ=0.03 rad/s時單軸控制力矩最大值不超過1 N·m 相比，角速度期望值越大，控制力矩幅值的峰值越大，通過設置期望角速度可以使控制力矩滿足姿態機動的有界約束。

圖16 MRP、單位四元數及角速度、控制力矩隨時間的變化軌跡Fig.16 Trajectory of attitude（MRP and quaternion），rate and control torque over time

將強制約束下的規劃結果同樣與RRT 方法進行對比，取多次重復規劃結果。在相同仿真條件下，將上述結果的規劃路徑長度與RRT方法的40次規劃結果進行比較，如圖17所示。

圖17 空間離散化方法規劃路徑長度與RRT方法的40次重復仿真結果對比Fig.17 Path length comparison between space discretization method and results produced by RRT for 40 times

圖17 顯示本文方法的路徑搜索算法具有較好的路徑長度優化效果，與RRT 方法的路徑結果相比路徑長度大大縮短。圖18 直觀展示了天球坐標系下本文方法與RRT 方法生成的兩條路徑的對比。強制約束在圖中隱去，姿態沿路徑機動過程中實際受到強制約束。

圖18 敏感器光軸在單位天球慣性系上的指向路徑（本文方法與RRT方法結果對比）Fig.18 Path of sensor axis on unit celestial sphere in inertial flame（results compared with RRT）

5 結論

本文通過MRP 參數空間離散化求解了時變指向約束下的姿態規劃問題，針對深空條件下姿態機動任務中動態環境約束的情況，提出了一種將三維空間路徑搜索算法和逆動力學多項式軌跡規劃方法結合的姿態規劃方法。為了滿足時變指向約束，通過笛卡爾網格劃分和邊界定義，構建了三維MRP空間中的非奇異空間，實現姿態參數空間離散化和指向約束表征。進一步在Focused D*算法基礎上，從非奇異空間搜索和時變約束沖突定位兩個方面設計適應于MRP 動態空間的路徑擴展策略。利用算法動態避障能力，在路徑節點的對應時刻判斷約束沖突情況，對路徑進行實時修正，最終生成從初始到目標姿態滿足時變指向約束的姿態機動執行路徑。仿真結果表明，本文姿態參數空間離散化路徑規劃方法能夠處理時變多類型多指向約束的復雜情況，生成安全姿態機動路徑。

所提出的姿態規劃方法應用于仿真案例時，表現出良好的約束處理能力和路徑長度的優化效果。但同時當前方法仍存在一些待解決，且未來值得研究的問題：1）當前路徑搜索算法生成的優化路徑是以當前節點到起點和終點的距離為評價函數，可以對啟發式評價函數進行設計，研究提升規劃路徑的能量優化效果的改進方法；2）由于Focused D*、D*等圖搜索算法需要離散化空間構建，計算復雜度受到離散精細度的影響，未來考慮構建全局規劃與局部優化結合的規劃架構，研究采用路徑搜索算法進行局部路徑優化的快速高效的姿態規劃方法。