考慮目標(biāo)視線角指標(biāo)的近距離接近位姿一體序列凸規(guī)劃

周 鼎，陳洪波，李順利

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

0 引 言

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

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

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

1 問題描述

本節(jié)主要描述近距離接近位姿一體規(guī)劃的最優(yōu)控制問題，主要包括系統(tǒng)動力學(xué)、路徑約束以及包含目標(biāo)視線角的性能指標(biāo)。

1.1 坐標(biāo)系統(tǒng)

圖1 坐標(biāo)系統(tǒng)Fig.1 Coordinate System

1.2 系統(tǒng)動力學(xué)

近距離接近的系統(tǒng)動力學(xué)由服務(wù)航天器與目標(biāo)衛(wèi)星之間的質(zhì)心相對運動模型、服務(wù)航天器姿態(tài)動力學(xué)及其相對于L 系的姿態(tài)運動學(xué)組成。

1.2.1 質(zhì)心相對運動模型

考慮近圓軌道上的目標(biāo)衛(wèi)星，服務(wù)航天器與目標(biāo)位置之間的質(zhì)心相對運動模型可由CW 方程進(jìn)行描述，寫成矩陣形式如下：

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

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

1.2.2 姿態(tài)動力學(xué)模型

假設(shè)服務(wù)航天器為剛體，其姿態(tài)動力學(xué)方程可表示為

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

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

1.2.3 姿態(tài)運動學(xué)模型

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

式中 In為n 階單位矩陣；相應(yīng)地，以MRPs 形式描述的姿態(tài)運動學(xué)方程為

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

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

1.3 路徑約束

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

1.3.1 傳感器視場角約束

為保證持續(xù)的相對導(dǎo)航，在近距離接近過程中，位姿軌跡需要使目標(biāo)一直處于服務(wù)航天器的傳感器視場內(nèi)，如圖2 所示，相應(yīng)的約束可表示為

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

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

1.3.2 碰撞規(guī)避約束

在最終與目標(biāo)進(jìn)行?？炕?qū)硬僮髑?，服?wù)航天器必須避免與目標(biāo)發(fā)生任何碰撞，所以質(zhì)心相對距離必須大于安全距離 rsafe，如圖2 所示，該約束可表示為

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

1.3.3 控制能力約束

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

1.4 性能指標(biāo)

根據(jù)上述系統(tǒng)動力學(xué)和路徑約束的描述，將服務(wù)航天器的姿態(tài)運動狀態(tài)定義為，相應(yīng)的位姿狀態(tài)和控制加速度可分別表示為和

假設(shè)服務(wù)航天器為全驅(qū)動控制，能量成本以控制u 的1L-范數(shù)形式來度量[8]，對于給定的時間區(qū)間 [ t0,ft] ，能量-最優(yōu)問題的性能指標(biāo)為

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

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

1.5 最優(yōu)控制問題

綜上所述，考慮能量+目標(biāo)視線角的復(fù)合性能指標(biāo)，服務(wù)航天器近距離接近位姿一體規(guī)劃的最優(yōu)控制問題可表示為

式中 w 為相對權(quán)重系數(shù)，w∈[0,1]； C （ x0, xf, t0,tf）表示邊值條件相關(guān)的函數(shù)，終端狀態(tài) xf利用最終的?？炕?qū)訔l件進(jìn)行計算。

2 問題的凸化松弛

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

2.1 視線角指標(biāo)的凸化

定義關(guān)于狀態(tài)軌跡x 的函數(shù)：

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

在參考狀態(tài)軌跡x~ 附近通過一階泰勒展開對性能指標(biāo)（11）近似可得：

2.2 系統(tǒng)動力學(xué)線性化與離散化

系統(tǒng)動力學(xué)中的質(zhì)心相對運動方程（式（1））原本就是線性的，所以這里主要對姿態(tài)運動方程進(jìn)行線性化。

式（2）、（3）描述的姿態(tài)動力學(xué)和運動學(xué)可以統(tǒng)一地寫成關(guān)于姿態(tài)運動狀態(tài)的微分方程：

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

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

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

2.3 路徑約束凸化

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

2.3.1 視場角約束凸化

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

2.3.2 碰撞規(guī)避約束的凸化

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

2.4 信賴域約束

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

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

式中kδ 表示信賴域半徑的平方。該約束在性能指標(biāo)中對應(yīng)的罰項可表示為

2.5 離散凸化子問題

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

式中 xi和 xf分別為初始狀態(tài)和終端狀態(tài)。

3 序列凸化算法

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

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

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

理論上當(dāng)Δx =0 時原問題（式（9））到達(dá)一個最優(yōu)解[10]，但實際數(shù)值求解時需要設(shè)置一個序列迭代過程的停止準(zhǔn)則，即：

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

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

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

c）令 fsol= False ；

d）當(dāng) fsol== False 時執(zhí)行下列程序；

e）更新迭代次數(shù)j = j+ 1；

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

j）fsol=True；

k）子命令結(jié)束；

l）程序結(jié)束；

4 數(shù)值算例及分析

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

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

序列凸化的參數(shù)設(shè)置如下：離散區(qū)間數(shù)K=60，精度要求xε=1×10-7，性能指標(biāo)相對權(quán)重w=0.5，信賴域權(quán)重系數(shù)wδ=1.0。

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

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

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

圖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 慣性姿態(tài)角速度分量Fig.9 Variations of Inertial Angular Velocities

圖10 慣性姿態(tài)角速度Fig.10 Variations of Inertial Angular Rate

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

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

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

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

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

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

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

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

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

5 結(jié) 論

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