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

周 鼎，陳洪波，李順利

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

0 引 言

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

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

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

1 問(wèn)題描述

本節(jié)主要描述近距離接近位姿一體規(guī)劃的最優(yōu)控制問(wèn)題，主要包括系統(tǒng)動(dò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)動(dòng)力學(xué)

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

1.2.1 質(zhì)心相對(duì)運(yùn)動(dòng)模型

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

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

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

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

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

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

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

1.2.3 姿態(tài)運(yùn)動(dòng)學(xué)模型

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

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

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

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

1.3 路徑約束

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

1.3.1 傳感器視場(chǎng)角約束

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

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

圖2 視場(chǎng)角約束及碰撞規(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ì)心相對(duì)距離必須大于安全距離 rsafe，如圖2 所示，該約束可表示為

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

1.3.3 控制能力約束

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

1.4 性能指標(biāo)

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

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

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

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

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

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

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

2 問(wèn)題的凸化松弛

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

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

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

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

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

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

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

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

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

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

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

2.3 路徑約束凸化

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

2.3.1 視場(chǎng)角約束凸化

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

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

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

2.4 信賴域約束

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

并針對(duì)離散后的問(wèn)題引入信賴域約束以確保偏差有界，即：

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

2.5 離散凸化子問(wèn)題

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

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

3 序列凸化算法

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

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

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

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

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

接下來(lái)給出序列凸化算法。

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

c）令 fsol= False ；

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

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

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

j）fsol=True；

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

l）程序結(jié)束；

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

本節(jié)對(duì)上述求解含視線角指標(biāo)的近距離接近位姿一體規(guī)劃問(wèn)題的序列凸化方法進(jìn)行數(shù)值算例仿真及分析。仿真環(huán)境為Matlab R2014a（×64），計(jì)算機(jī)主頻2.30 GHz，內(nèi)存4 GB。底層凸化子問(wèn)題的通過(guò)CVX軟件進(jìn)行求解。目標(biāo)衛(wèi)星及服務(wù)航天器的物理參數(shù)見文獻(xiàn)[2]，其中傳感器視場(chǎng)角 βFOV=25°，允許的最大控制力為8 N，最大控制力矩為10 N·m。飛行時(shí)間區(qū)間設(shè)置為[0，360] s，根據(jù)停靠條件計(jì)算服務(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)相對(duì)權(quán)重w=0.5，信賴域權(quán)重系數(shù)wδ=1.0。

經(jīng)過(guò)5 次凸化子問(wèn)題求解后，迭代過(guò)程滿足停止準(zhǔn)則，信賴域半徑達(dá)到9.6×10-10，求解過(guò)程耗時(shí)102 s。如圖3 所示，與單獨(dú)的能量-最優(yōu)相比，將視線角作為優(yōu)化指標(biāo)可有效降低機(jī)動(dòng)過(guò)程中目標(biāo)視線角的峰值，使目標(biāo)視線矢量保持在傳感器光軸附近，有益于相對(duì)導(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)變化、約束滿足情況及控制力/力矩。機(jī)動(dòng)過(guò)程位姿狀態(tài)變化平穩(wěn)，控制力/力矩均未超出容許上界，最大控制力為7.999 N，最大控制力矩為0.376 N·m，控制呈現(xiàn)明顯的分段常值特征。相對(duì)速度最大為0.135 m/s，整個(gè)機(jī)動(dòng)過(guò)程中速度大小比較穩(wěn)定，無(wú)明顯振蕩，慣性姿態(tài)角速度最大值為0.095 (°)/s。目標(biāo)視線角最大值為0.88°，遠(yuǎn)小于βFOV，位姿狀態(tài)滿足視場(chǎng)角約束。接近過(guò)程相對(duì)距離變化一直大于安全距離，表明位置狀態(tài)滿足碰撞規(guī)避約束。

圖4 相對(duì)位置分量Fig.4 Variations of Relative Positions

圖6 相對(duì)距離Fig.6 Variations of Relative Distance

圖7 相對(duì)速度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 相對(duì)于軌道系的MRPFig.11 MRP with Respect to LVLH

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

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

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

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

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

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

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

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

5 結(jié) 論

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