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

（1.北京理工大學(xué)深空探測(cè)技術(shù)研究所，北京 100081；2.深空自主導(dǎo)航與控制工信部重點(diǎn)實(shí)驗(yàn)室（北京理工大學(xué)），北京 100081；3.上海航天控制技術(shù)研究所，上海 201109）

0 引言

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

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

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

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

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

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

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

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

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

1）四元數(shù)

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

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

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

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

式中：?∈[0，360°)

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

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

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

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

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

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

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

式中：σ×是σ的斜對(duì)稱叉乘矩陣。

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

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

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

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

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

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

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

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

符號(hào)“≥”表示強(qiáng)制約束：

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

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

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

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

類似地，對(duì)于兩個(gè)方位角-仰角分別為［0°，0°］、［-180°，0°］，視場(chǎng)角分別為20°的禁入錐，和方位角-仰角為［-180°，0°］，視場(chǎng)角為20°的強(qiáng)制錐，其指向約束在慣性系單位天球和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é)點(diǎn)的連續(xù)路徑Fig.6 Continuous path under static constraints by MRP switching

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

由式（9），在‖ ‖σ=1 的球面表面節(jié)點(diǎn)與其影節(jié)點(diǎn)σ'的距離=0。當(dāng)路徑擴(kuò)展出單位球面，而通過(guò)影節(jié)點(diǎn)從球面的中心對(duì)稱一側(cè)進(jìn)入時(shí)，=0，說(shuō)明路徑是連續(xù)的。

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

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

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

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

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

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

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

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

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

3.1 姿態(tài)逆運(yùn)動(dòng)學(xué)和逆動(dòng)力學(xué)

剛體航天器本體坐標(biāo)系下的姿態(tài)動(dòng)力學(xué)方程為：

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

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

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

對(duì)上式求微分得到：

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

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

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

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

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

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

式中：qi，j是qi第j個(gè)分量。

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

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

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

特殊地，當(dāng)離散節(jié)點(diǎn)序列類似圖8，通過(guò)MRP影節(jié)點(diǎn)保證路徑連續(xù)到達(dá)目標(biāo)節(jié)點(diǎn)時(shí)，難以直接進(jìn)行插值擬合?？梢韵葘⒐?jié)點(diǎn)序列映射為影節(jié)點(diǎn)序列，與原影節(jié)點(diǎn)分段相連接，構(gòu)成連續(xù)路徑。進(jìn)而可以采用本節(jié)方法插值擬合生成解析軌跡，得到角速度和控制力矩時(shí)間曲線。

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

4 數(shù)值仿真

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

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

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

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

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

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

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

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

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

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

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

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

其中姿態(tài)機(jī)動(dòng)路徑的長(zhǎng)度計(jì)算方式如下［18］：

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

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

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

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

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

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

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

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

圖15 單位天球慣性系上不同時(shí)刻下算法修正的三維指向路徑（強(qiáng)制約束隨時(shí)間在圖中從上向下擺動(dòng)）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ù)及角速度、控制力矩隨時(shí)間的變化軌跡如圖16所示。角速度時(shí)間曲線顯示，角速度的模在期望值ωˉ=0.004 rad/s附近保持小幅波動(dòng)。單軸控制力矩最大值不超過(guò)0.02 N·m。由此可知，與前一節(jié)角速度期望值ωˉ=0.03 rad/s時(shí)單軸控制力矩最大值不超過(guò)1 N·m 相比，角速度期望值越大，控制力矩幅值的峰值越大，通過(guò)設(shè)置期望角速度可以使控制力矩滿足姿態(tài)機(jī)動(dòng)的有界約束。

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

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

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

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

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

5 結(jié)論

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

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