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

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

0 引言

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

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

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

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

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

1 姿態(tài)參數(shù)空間離散化

對于姿態(tài)參數(shù)空間離散化方法，需要針對不同的姿態(tài)描述方式，即姿態(tài)描述參數(shù)，選擇不同的三維空間離散化方法，并實現(xiàn)姿態(tài)機動的指向約束表示。本節(jié)對修正羅德里格斯參數(shù)（MRP）描述方式進行了參數(shù)空間離散化處理，從而構(gòu)建姿態(tài)空間的三維離散網(wǎng)格，結(jié)合離散空間的指向約束表達，得到姿態(tài)機動的路徑規(guī)劃空間，為空間離散化路徑搜索算法設(shè)計提供了基礎(chǔ)。

1.1 姿態(tài)描述參數(shù)

航天器姿態(tài)采用本體坐標系與慣性坐標系之間的相對轉(zhuǎn)動關(guān)系來描述，剛體航天器姿態(tài)可以采用四元數(shù)和修正羅德里格斯參數(shù)進行描述。

1）四元數(shù)

采用單位四元數(shù)描述航天器的姿態(tài)：

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

2）修正羅德里格斯參數(shù)

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

式中：?∈[0，360°)

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

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

式（3）成立時，2 組MRP 參數(shù)σ和σ'取值不同，是參數(shù)空間中不同的點，但表示相同的姿態(tài)，當‖σ‖＞1時‖σ'‖＜1?？梢宰C明，對于MRP在表示一個360°旋轉(zhuǎn)，即?=360°時的奇異情況，有MRP 空間原點處σ0可表示同一姿態(tài)。

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

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

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

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

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

1.2 修正羅德里格斯參數(shù)空間離散化

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

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

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

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

1.3 指向約束在姿態(tài)參數(shù)空間的表示

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

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

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

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

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

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

圖3 修正羅德里格斯參數(shù)空間內(nèi)的指向約束Fig.3 Pointing constraints in MRP space

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

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

圖5 修正羅德里格斯參數(shù)空間內(nèi)的指向約束Fig.5 Pointing constraints in MRP space

圖6 靜態(tài)約束下借助MRP影節(jié)點的連續(xù)路徑Fig.6 Continuous path under static constraints by MRP switching

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

2 時變指向約束下姿態(tài)路徑搜索方法

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

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

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

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

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

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

對于本文所解決的時變指向約束問題，F(xiàn)ocused D*算法存在以下不足：

1）規(guī)劃解在MRP空間無最優(yōu)性

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

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

2）路徑修正函數(shù)無法定位與時間相關(guān)的約束沖突節(jié)點

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

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

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

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

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

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

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

當姿態(tài)機動歐拉旋轉(zhuǎn)角大于180°時，借助MRP影節(jié)點的連續(xù)路徑效果如下。

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

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

通過估計的總機動時間和期望平均角速度，可將路徑上節(jié)點對應(yīng)時刻{t0，t1，…，tN}設(shè)為與節(jié)點路徑長度成比例關(guān)系的數(shù)值，即：

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

1）沿總路徑代價最小的最優(yōu)路徑轉(zhuǎn)動過程中算法存儲的map 空間動態(tài)更新，這種方法需要在循環(huán)中重復離散化的空間構(gòu)建，用時較長；

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

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

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

3 逆動力學方法求解姿態(tài)連續(xù)軌跡

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

3.1 姿態(tài)逆運動學和逆動力學

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

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

式中：運算符?表示四元數(shù)乘法。

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

對上式求微分得到：

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

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

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

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

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

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

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

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

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

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

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

圖8 姿態(tài)參數(shù)空間離散化方法在MRP空間的規(guī)劃路徑Fig.8 Path planned by attitude parameter space discretization path planning method in MRP space

4 數(shù)值仿真

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

4.1 動態(tài)禁忌指向約束下姿態(tài)規(guī)劃

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4.2 動態(tài)強制指向約束下姿態(tài)規(guī)劃

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

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

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

在上述條件下，姿態(tài)參數(shù)空間離散化方法在MRP 空間和慣性系單位天球上，探測器實際機動路徑如圖14，15所示。由于強制約束要求三角形符號表示的當前姿態(tài)節(jié)點始終在圖15 中淺藍色強制約束范圍內(nèi)，當前節(jié)點到目標姿態(tài)節(jié)點σG的路徑由于超出強制約束邊界而無法規(guī)劃，需要設(shè)置一個臨時目標節(jié)點。將臨時目標節(jié)點設(shè)為：位于當前時刻指向約束的可行域內(nèi)，到目標姿態(tài)節(jié)點的距離（用式（9）計算）最小的節(jié)點，該節(jié)點在強制約束邊界上。采用分段式規(guī)劃，分別規(guī)劃一條滿足強制約束和禁忌約束的、從當前節(jié)點σi到臨時目標節(jié)點的路徑，和一條只需滿足禁忌約束的、從臨時目標節(jié)點σ'G到目標姿態(tài)節(jié)點σG的路徑。兩段路徑相連接即從起始到目標姿態(tài)，且滿足當前時刻強制約束和禁忌約束的機動路徑。當前節(jié)點擴展與時變強制約束沖突時，如果≠σG，則選取新的臨時目標節(jié)點，修正σi到的路徑，并修正到σG的路徑；如果=σG，直接修正σi到σG的路徑。最終得到一條每一時刻姿態(tài)節(jié)點都滿足時變強制指向約束的探測器可執(zhí)行路徑，即圖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 和單位四元數(shù)及角速度、控制力矩隨時間的變化軌跡如圖16所示。角速度時間曲線顯示，角速度的模在期望值ωˉ=0.004 rad/s附近保持小幅波動。單軸控制力矩最大值不超過0.02 N·m。由此可知，與前一節(jié)角速度期望值ωˉ=0.03 rad/s時單軸控制力矩最大值不超過1 N·m 相比，角速度期望值越大，控制力矩幅值的峰值越大，通過設(shè)置期望角速度可以使控制力矩滿足姿態(tài)機動的有界約束。

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

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

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

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

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

5 結(jié)論

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

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