考慮目標視線角指標的近距離接近位姿一體序列凸規劃

周 鼎，陳洪波，李順利

（1.上海宇航系統工程研究所，上海，201109；2.哈爾濱工業大學航天學院，哈爾濱，150001；3.中山大學系統科學與工程學院，廣州，510275）

0 引 言

針對服務航天器近距離接近非合作目標的制導與控制問題，目前的挑戰主要來自以下幾個方面：a）傳統的地面在回路的方案不能滿足中高軌道接近任務的自主性需求；b）目標的非合作特點要求服務航天器自身進行相對測量，傳感器的指向約束使得位姿耦合，因而需要進行位姿一體軌跡規劃；c）高維狀態的非線性和位姿耦合的路徑約束使得規劃問題的求解變得更加困難[1～3]。近年來，由于計算制導與控制技術[4]的發展，凸規劃方法以其良好的收斂性和全局最優性保證逐漸被應用于交會對接[5]、行星軟著陸[6]及火箭垂直起降[7]的自主制導。本文旨在對凸規劃方法進行應用擴展，利用序列迭代求解近距離接近的位姿一體軌跡規劃問題。

凸規劃方法應用的主要障礙是處理規劃問題中的非凸約束。針對自主交會問題，文獻[5]將接近過程的障礙區域近似為旋轉超平面，并將原問題松弛為一系列二階錐規劃進行迭代求解。在對偶四元數描述的六自由度行星著陸制導中，控制約束通過引入維度增廣被無損凸化，相關指向約束被近似為二階錐約束，然后通過凸規劃方法求解以進行模型預測控制[6]。為減小凸化近似帶來的人工不可行性，一種基于軌跡偏差的信賴域約束被引入序列迭代算法中以促進收斂，該方法可有效應對火箭垂直起降制導中氣動參數帶來的不確定性[7]。

在能量-最優的基礎上，本文考慮目標視線角-最小性能指標，使得視覺傳感器光軸盡可能對準目標以保證良好的相對導航質量。首先建立近距離接近位姿一體規劃的最優控制問題，然后對其中的非凸指標函數、非線性姿態動力學、位姿耦合視場角約束、碰撞規避約束進行凸化，并通過信賴域約束的序列迭代算法進行求解。最后，通過數值算例驗證了目標視線角-最小性能指標及序列凸化方法的有效性。

1 問題描述

本節主要描述近距離接近位姿一體規劃的最優控制問題，主要包括系統動力學、路徑約束以及包含目標視線角的性能指標。

1.1 坐標系統

圖1 坐標系統Fig.1 Coordinate System

1.2 系統動力學

近距離接近的系統動力學由服務航天器與目標衛星之間的質心相對運動模型、服務航天器姿態動力學及其相對于L 系的姿態運動學組成。

1.2.1 質心相對運動模型

考慮近圓軌道上的目標衛星，服務航天器與目標位置之間的質心相對運動模型可由CW 方程進行描述，寫成矩陣形式如下：

式 中 xp為 平 動 運 動 狀 態 ，xp= [ ρT, vT]T=[ x , y , z , vx, vy, vz]T； up為 控 制 加 速 度，up=[ ax, ay, az]T； Λp為動力學矩陣； Gp為控制矩陣，分別表示為

式中 nref為參考軌道平均角速率。

1.2.2 姿態動力學模型

假設服務航天器為剛體，其姿態動力學方程可表示為

式中 ωSI為服務航天器慣性角速度在S系中的分量列陣， ωSI=[ωx,ωy,ωz]T； JS為其轉動慣量在S系中的分量矩陣，相應的主軸慣量為 Ixx, Iyy和 Izz；a×表示向量a 的反對稱矩陣。為了便于后續性能指標和約束的描述，通過服務航天器的質量sm 和一個特征長度 leq定義了一個如下的等效控制加速度：

式中ST 表示控制力拒矢量在S 系中的分量列陣，TS=[Tx, Ty, Tz]T。

1.2.3 姿態運動學模型

考慮接近過程中可能存在較大范圍的姿態機動，為避免可能出現的姿態解算的奇異，采用修正羅德里格參數（MRPs）來描述空間指向，定義服務航天器本體相對于參考軌道坐標系的姿態 MRPs 為σSL=[σ1, σ2,σ3]T，通過其表示的從L 系到S 系的姿態變換矩陣為

式中 In為n 階單位矩陣；相應地，以MRPs 形式描述的姿態運動學方程為

式中 ωSL為S 系相對于L 系的角速度在S系中的分量列陣， ωSL=[ω1, ω2,ω3]T，可通過如下關系式計算：

式中 ?L=[ 0 ,0,nref]T。

1.3 路徑約束

在接近非合作目標的過程中，服務航天器的位姿軌跡需要滿足傳感器視場角、碰撞規避及控制能力限制等路徑約束。

1.3.1 傳感器視場角約束

為保證持續的相對導航，在近距離接近過程中，位姿軌跡需要使目標一直處于服務航天器的傳感器視場內，如圖2 所示，相應的約束可表示為

式中 β 為目標視線角；b 表示傳感器的安裝位置在S系中的分量列陣；d 表示傳感器光軸方向矢量在S系中的分量列陣； βFOV為傳感器視場角。注意到，非線性項使得位姿狀態耦合到一起。

圖2 視場角約束及碰撞規避約束示意Fig.2 Illustration of Constraints On Field of View and Collision Avoidance

1.3.2 碰撞規避約束

在最終與目標進行?？炕驅硬僮髑?，服務航天器必須避免與目標發生任何碰撞，所以質心相對距離必須大于安全距離 rsafe，如圖2 所示，該約束可表示為

本文中考慮服務航天器和目標的外形尺寸，安全距離 rsafe=6.6 m。

1.3.3 控制能力約束

考慮服務航天器的敏捷機動能力，其位姿控制均通過推力器來實現，其物理特性（如最大推力等）使得可用的控制力/力矩在有限的范圍內，相應地施加到控制加速度上的約束可表示為

1.4 性能指標

根據上述系統動力學和路徑約束的描述，將服務航天器的姿態運動狀態定義為，相應的位姿狀態和控制加速度可分別表示為和

假設服務航天器為全驅動控制，能量成本以控制u 的1L-范數形式來度量[8]，對于給定的時間區間 [ t0,ft] ，能量-最優問題的性能指標為

此外，雖然有路徑約束（4）來保證接近過程中目標處于傳感器視場內，但考慮視場邊緣的畸變和噪聲會顯著增加從而影響相對導航精度，接近過程的位姿軌跡應盡可能使目標視線矢量處在傳感器光軸附近，即目標視線角盡可能小，相應的性能指標為

式中 β∈( -π /2,π/2)。

1.5 最優控制問題

綜上所述，考慮能量+目標視線角的復合性能指標，服務航天器近距離接近位姿一體規劃的最優控制問題可表示為

式中 w 為相對權重系數，w∈[0,1]； C （ x0, xf, t0,tf）表示邊值條件相關的函數，終端狀態 xf利用最終的?？炕驅訔l件進行計算。

2 問題的凸化松弛

式（9）描述的問題指標函數和約束中均包含非凸項，在利用凸規劃方法求解前需要進行凸化松弛，并通過離散化將無限維的連續時間微分約束問題轉化為有限維的代數約束凸規劃問題。

2.1 視線角指標的凸化

定義關于狀態軌跡x 的函數：

式中 Cρ為相對位置狀態提取矩陣，，其使得 ρ = Cρx。由于光軸方向矢量為單位向量，即所 以 gβ( x ) =- cosβ。 注 意 到， -cosβ是 關 于β∈( - π /2,π/2)的凸函數，所以視線角對應的性能指標（式（8））可等價地表示為

在參考狀態軌跡x~ 附近通過一階泰勒展開對性能指標（11）近似可得：

2.2 系統動力學線性化與離散化

系統動力學中的質心相對運動方程（式（1））原本就是線性的，所以這里主要對姿態運動方程進行線性化。

式（2）、（3）描述的姿態動力學和運動學可以統一地寫成關于姿態運動狀態的微分方程：

在參考軌跡附近對式（13）中的 fr(rx) 進行一階泰勒近似可得：

所以，姿態運動方程（13）可近似地線性化為

將給定的飛行時間區間 [ t0,ft] 離散成K 個子區間，利用零階-保持方法對連續-時間控制 u( t) 進行離散化：

2.3 路徑約束凸化

在1.3 節描述路徑約束中，視場角和碰撞規避約束均為非凸約束，本節將對它們進行凸化處理；而控制約束為錐約束，原本就是凸的，無需再進行凸化。

2.3.1 視場角約束凸化

根據函數 gβ( x )的定義，式（4）描述的視場角約束可以等價地改寫成如下形式：

2.3.2 碰撞規避約束的凸化

式（5）描述的碰撞規避約束可等價改寫為

2.4 信賴域約束

上述對性能指標和約束的松弛處理都是在參考軌跡x~ 附近進行，所以求解的軌跡與參考軌跡之間的偏差不能過大，否則松弛的問題無法逼近原問題的非凸性。為此，定義如下變量來度量軌跡的偏差：

并針對離散后的問題引入信賴域約束以確保偏差有界，即：

式中kδ 表示信賴域半徑的平方。該約束在性能指標中對應的罰項可表示為

2.5 離散凸化子問題

經過上述對性能指標及約束的凸化處理，連續-時間的非凸原問題（式（9））可在參考狀態軌跡x~ 附近松弛為如下離散的凸化子問題：

式中 xi和 xf分別為初始狀態和終端狀態。

3 序列凸化算法

本節提出通過序列求解凸化子問題（式（24））對非凸原問題（式（9））進行逼近的迭代算法。

首先，給出初始迭代的參考軌跡猜想。與序列二次規劃[9]等非線性規劃方法相比，序列凸化方法對初始猜想的敏感度較低，為方便迭代的快速啟動，本文按如下方法選取初始參考軌跡：

式中 x(j)表示第 j ( j= 0,1,2,… ) 次迭代求解凸化子問題（式（24））的狀態軌跡解。從第1 次迭代開始，第j 次迭代中用于問題凸化處理時各系數矩陣或常數向量計算的的參考軌跡為 x(j-1)。

理論上當Δx =0 時原問題（式（9））到達一個最優解[10]，但實際數值求解時需要設置一個序列迭代過程的停止準則，即：

式中 Δx(j)= x(j)- x(j-1), ( j ≥1)；εx為容許的狀態軌跡偏差上界。

接下來給出序列凸化算法。

b）令j= 1，只考慮系統動力學及控制約束求解凸化子問題（式（24））得到解

c）令 fsol= False ；

d）當 fsol== False 時執行下列程序；

e）更新迭代次數j = j+ 1；

h）計算狀態軌跡偏差 Δx(j)= x(j)-x(j-1)；

j）fsol=True；

k）子命令結束；

l）程序結束；

4 數值算例及分析

本節對上述求解含視線角指標的近距離接近位姿一體規劃問題的序列凸化方法進行數值算例仿真及分析。仿真環境為Matlab R2014a（×64），計算機主頻2.30 GHz，內存4 GB。底層凸化子問題的通過CVX軟件進行求解。目標衛星及服務航天器的物理參數見文獻[2]，其中傳感器視場角 βFOV=25°，允許的最大控制力為8 N，最大控制力矩為10 N·m。飛行時間區間設置為[0，360] s，根據?？織l件計算服務航天器邊值狀態如表1 所示。

表1 服務航天器的邊值條件Tab.1 Boundary Conditions of the Service Spacecraft

序列凸化的參數設置如下：離散區間數K=60，精度要求xε=1×10-7，性能指標相對權重w=0.5，信賴域權重系數wδ=1.0。

經過5 次凸化子問題求解后，迭代過程滿足停止準則，信賴域半徑達到9.6×10-10，求解過程耗時102 s。如圖3 所示，與單獨的能量-最優相比，將視線角作為優化指標可有效降低機動過程中目標視線角的峰值，使目標視線矢量保持在傳感器光軸附近，有益于相對導航的成像質量。

圖3 不同性能指標下目標視線角Fig.3 Variations of LOS Angle of the Target with Different Performance Index

圖4 ～12 給出規劃的位姿狀態變化、約束滿足情況及控制力/力矩。機動過程位姿狀態變化平穩，控制力/力矩均未超出容許上界，最大控制力為7.999 N，最大控制力矩為0.376 N·m，控制呈現明顯的分段常值特征。相對速度最大為0.135 m/s，整個機動過程中速度大小比較穩定，無明顯振蕩，慣性姿態角速度最大值為0.095 (°)/s。目標視線角最大值為0.88°，遠小于βFOV，位姿狀態滿足視場角約束。接近過程相對距離變化一直大于安全距離，表明位置狀態滿足碰撞規避約束。

圖4 相對位置分量Fig.4 Variations of Relative Positions

圖6 相對距離Fig.6 Variations of Relative Distance

圖7 相對速度Fig.7 Variations of Relative Distance

圖8 控制力分量Fig.8 Variations of Control Forces

圖9 慣性姿態角速度分量Fig.9 Variations of Inertial Angular Velocities

圖10 慣性姿態角速度Fig.10 Variations of Inertial Angular Rate

圖11 相對于軌道系的MRPFig.11 MRP with Respect to LVLH

圖12 控制力矩分量Fig.12 Variations of Control Torques

如圖13 所示，在不設置停止準則精度要求的情況下，序列凸規劃算法經過6 次迭代后，信賴域半徑收斂到2.1×10-10；相比之下，在不考慮信賴域約束的情況下，算法不能收斂，信賴域半徑在1×10-2附近振蕩，如圖14 所示?？梢钥闯?，信賴域約束能夠有效改善序列凸化方法求解非凸約束的高維位姿一體規劃問題的收斂性。

圖13 信賴域約束序列凸規劃算法的收斂過程Fig.13 Convergence Process of SCP with Trust Region Constraints

圖14 無信賴域約束序列凸規劃算法的迭代過程Fig.14 Iterations of SCP without Trust Region Constraints

通過對信賴域權重系數的調整來觀察其對序列凸規劃算法收斂性能的影響。如圖15 所示，隨著信賴域權重從1×10-2量級逐漸增加到1×103量級，收斂的信賴域半徑從1×10-8量級減小到量級1×10-11，整體上精度逐漸提高；相應地，收斂所需的迭代次數也呈現下降趨勢。由此可見，適當提高信賴域權重有助于改善收斂性能。

圖15 信賴域權重的變化對收斂性的影響Fig.15 Effects of Trust Region Weight on Convergence

然而，另一方面，隨著信賴域權重的增加，規劃的位姿狀態對應的目標視線角最大值逐漸增大，如圖16 所示，使得指標函數中的視線角項失去效果。所以，在實際應用中選取信賴域權重時需要注意性能指標與收斂性之間的權衡。

圖16 信賴域權重對視線角峰值的影響Fig.16 Effects of Trust Region Weight on Peak LOS Angle

5 結 論

本文針對服務航天器自主接近非合作目標的任務，考慮相對導航需求，提出了考慮目標視線角性能的近距離接近位姿一體規劃問題，設計了信賴域約束的序列凸規劃方法進行求解。算例仿真表明，視線角指標可有效降低機動過程中目標視線角的峰值，使目標視線矢量保持在傳感器光軸附近；信賴域約束的序列凸規劃方法可有效求解非凸約束的高維位姿規劃問題，將凸規劃方法從三自由度的自主交會軌跡規劃擴展到了六自由度的近距離接近階段。此外，通過對比仿真可以看出，適當提高信賴域權重系數有助于提高序列凸規劃算法的收斂性能，但需要與最優控制問題的性能指標進行權衡。