郭延寧，韓 旭，郭增千，張 瑤，馬廣富

(1.哈爾濱工業(yè)大學控制科學與工程系，哈爾濱 150001； 2.上海衛(wèi)星工程研究所，上海 200240)

?

基于雙目視覺的非合作目標逼近控制系統(tǒng)設計與仿真*

郭延寧1，韓 旭2，郭增千1，張 瑤1，馬廣富1

(1.哈爾濱工業(yè)大學控制科學與工程系，哈爾濱 150001； 2.上海衛(wèi)星工程研究所，上海 200240)

主要研究空間非合作目標近距離逼近過程的控制系統(tǒng)設計與仿真問題.針對采用雙目視覺敏感器實現(xiàn)對非合作目標的觀測情況，提出一種考慮成像誤差的目標位置矢量計算方法，有效保證目標位置解算的可行性.在主星視線坐標系下，分別考慮逼近過程的最大相對速度約束、控制推力和力矩約束，設計了基于非線性項解耦的遞階飽和PID形式的近距離逼近位置控制律和姿態(tài)控制律，并在逼近過程中設置停泊點以確保與目標無碰撞.最后對典型航天器非合作目標抓捕任務進行了數(shù)學仿真，仿真結果表明所提出的方法可在滿足各種約束的情況下有效實現(xiàn)任意方向的空間非合作目標的抓捕任務.

非合作目標；雙目視覺敏感器；遞階飽和PID；位置控制；姿態(tài)控制；解耦控制.

0 引 言

空間非合作目標(non-cooperative target)，是指并非設計用于對接或捕獲任務的航天器或其他空間飛行物體，即該目標上不安裝專門的抓持或對接機構以及用于輔助測量的合作標志器或特征塊，不具備主動的姿態(tài)及軌道控制能力.其常泛指不能提供有效合作信息的空間目標，如未裝置合作部件的衛(wèi)星、燃料耗盡或發(fā)生故障的衛(wèi)星、廢舊棄用衛(wèi)星、空間碎片，也可以是敵方衛(wèi)星、敵方空間武器等[1-2].

空間非合作目標逼近過程的制導與控制系統(tǒng)的設計是實現(xiàn)抓捕或交會任務的最重要的任務之一，該問題一直受到國內外學者的重視.在相對導航的研究中，解永春等[3]提出了一種相對位置和相對速度導航方法，但其系統(tǒng)中采用單個敏感器，且需借助地面信息，主要工作于非合作目標遠距離情況.張世杰等[4]提出了一種基于兩次迭代計算的單目視覺相對位置姿態(tài)確定方法.徐文福等[5]系統(tǒng)地提出了基于立體視覺的空間機器人捕獲非合作目標的測量與規(guī)劃方法.已有研究多集中于對對象的識別與匹配，未考慮識別誤差對導航計算方法的影響.

非合作目標的捕獲任務最后逼近階段指主星利用自身攜帶的光學載荷探測目標特性，逐漸近距離接近到達目標，然后利用機械臂或網(wǎng)爪等機構捕獲目標的過程，該階段是決定非合作目標抓捕任務成敗的最關鍵的階段[6].

已有用于合作目標交會對接任務的最終逼近段制導策略有R-bar、V-bar、基于C-W方程的反饋制導等[7-8].已有的方法在應對執(zhí)行機構的控制能力、相對運動狀態(tài)約束和與目標性的碰撞回避問題中均呈現(xiàn)諸多不足.雙目視覺導航和和激光成像導航為最后逼近段的主要導航方法.

本文主要旨在解決采用雙目視覺敏感器非合作目標逼近的兩個問題：視覺敏感器存在測量誤差情況的目標星矢量計算問題；考慮執(zhí)行機構的控制約束、狀態(tài)約束和碰撞約束的姿態(tài)及位置控制律設計問題.

1 敏感器建模及目標位置解算

1.1 雙目視覺敏感器模型建立

雙目視覺敏感器已經(jīng)廣泛應用于如機器人和各種飛行器的定位及導航中，工作原理圖如圖1.對于非合作目標捕獲任務，不失一般性，認為雙目視覺敏感器的基準坐標系Oxyz與主星本體坐標系三軸平行，其原點位于衛(wèi)星本體系+x軸與外表面交點，其測量的相對距離信息即為衛(wèi)星與目標之間的實際距離，可直接用于逼近控制階段控制方案設計.

定義兩個平行安裝的敏感器焦點連線中心為基準坐標系原點O-xyz，視線方向定義為x軸，焦點連線方向定義為y軸(方向見圖1)，z軸構成右手坐標系，焦點距離為2a，兩個敏感器的焦距均為f.視覺敏感器原點Ol在基準坐標系中的坐標為(f,-a,0)，Or的坐標為(f,a,0).此外，為了描述單個敏感器觀測范圍，定義參考視場角θ如圖1所示.

圖1 雙目視覺敏感器工作原理Fig.1 Structure of a binocular vision sensor

給定非合作目標中一特征點P(xt,yt,zt)，不失一般性，認為該點為非合作目標且目標無高速自旋，在左、右敏感器的成像面上的像點分別為P1(0,y1,z1)和P2(0,y2,z2).由于這兩個像點是空間中同一個對像點P在不同敏感器中的像，稱為“共軛點”.當已知兩個共軛像點和對應敏感器特性參數(shù)，分別作共軛像點與敏感器的焦點連線，在沒有誤差的情況下，就可以得到它們的交點，即為對像點P.

1.2 目標位置矢量解算

a)不考慮成像誤差情況

在不考慮雙目視覺敏感器的成像誤差時，則根據(jù)透鏡成像原理，有如下等式成立：

(1)

(2)

目標星的位置矢量的具體形式為：

(3)

b)考慮成像誤差情況

考慮敏感器成像誤差時，設左、右攝像機的誤差矢量表示為(εy1,εz1)、(εy2,εz2)，則有如下等式成立：

(4)

(5)

由于噪聲的加入，使得左、右攝像機的共軛點與焦點連線不存在交點，進而無法得到目標位置，因此設計如下改進的位置計算策略.

根據(jù)三維空間的解析幾何，有決定行列式

(6)

當Δ=2a(z1-z2)=0時，兩視線相交，交點即為目標空間位置；當Δ=2a(z1-z2)≠0時，兩視線異面，不存在空間交點，為使得視覺敏感器的誤差折中抵消，不妨取兩視線公垂線的中點為目標空間位置.

從而可知，觀測的視線相交的充要條件為

z1=z2

(7)

當視線不相交時，有z1≠z2，兩視線矢量為

(8)

可得兩視線對應的直線方程分別為

(9)

則接下來的問題即為計算兩條直線的公垂線和垂足，并將兩個垂足的中點設為目標點即可，具體計算過程參見文獻[9]，此處不再贅述.

2 空間非合作目標逼近控制系統(tǒng)設計

2.1 系統(tǒng)總體設計

在對目標逼近過程中，利用主星配置的GPS和速率陀螺獲取主星的絕對位置、速度和慣性角速度，由雙目視覺敏感器解算目標的相對位置、相對速度等信息，并基于相對位置獲取指向姿態(tài)，主星的軌道控制主要完成軌道機動及調整任務，不屬于本文研究范疇；主星與目標的相對運動中，由主星位置控制器根據(jù)兩星的相對位置及相對速度產(chǎn)生期望控制力指令，主星姿態(tài)控制器根據(jù)目標的視線角矢量和角速度信息提供控制力矩指令，再由推力器和反作用飛輪分別產(chǎn)生控制力和控制力矩作用至主星的軌道和姿態(tài)動力學模型中，最終完成空間非合作目標的逼近，系統(tǒng)結構如圖2所示.

圖2 空間非合作目標逼近控制系統(tǒng)結構Fig.2 Control system structure of non-cooperative target proximity operation

2.2 運動方程

選取地心慣性系為基準坐標系，則有考慮各種攝動力作用下的主星及目標軌道動力學方程為

(10)

(11)

式中，rs和rt分別為主星和目標在地心慣性坐標系下的三軸位置矢量，對應rs和rt分別為主星和目標地心距，μ為地球引力常數(shù)，fc為待設計的主星控制力，fsp和ftp為主星和目標的攝動集合.

不失一般性，考慮雙目視覺與捕獲系統(tǒng)安裝在同一個方向，為滿足以上需求，需要在視線坐標系下完成主星的控制系統(tǒng)設計.以主星本體系為參考系的視線坐標系，考慮忽略非合作目標和主星之間的萬有引力作用，有視線坐標系的距離r、方位角α、高程角β計算如下：

(12)

對應分量表示的相對運動學方程為[8]

(13)

式中fi(i=r,α,β),Δgi(i=r,α,β)分別對應控制力和地心重力加速度差在不同運動維度的分量.對于最終逼近段而言，可以認為主星和目標所受重力加速度相同，即Δgi=0(i=r,α,β).

考慮飛輪作為執(zhí)行機構，可得衛(wèi)星本體系下的剛體姿態(tài)非線性動力學方程

(14)

式中，I為主星轉動慣量矩陣，Tc為待設計控制力矩，Td為環(huán)境干擾力矩，ω×表示向量ω的反對稱形式矩陣

2.3 雙積分系統(tǒng)的遞階飽和PID控制器

針對如下典型的控制受限情況下的雙積分系統(tǒng)

(15)

其中，x為系統(tǒng)狀態(tài)，u為控制量，k為常系數(shù)，對應U為控制量最大幅值.

對于給定期望狀態(tài)xd，有時間最優(yōu)控制[10]

(16)

其中，e=x-xd，sgn為符號函數(shù).

可以看出，由式(16)所示的最優(yōu)控制在臨界狀態(tài)存在控制切換，進而帶來抖動問題.因此，文獻[11]提出了如下形式的PID遞階飽和控制器

(17)

其中，p、i、d分別為待選取得PID控制參數(shù)，飽和函數(shù)sat(·)定義為

(18)

(19)

(20)

根據(jù)e的取值不同，最終對應兩種可能的控制量，分別為

(21)

對應為普通飽和PD控制，以及

(22)

可等價對應得到時間最優(yōu)控制(16).

2.4 基于遞階飽和PID控制的逼近段位姿控制

在非合作目標的最后逼近段，主星需要完成以下任務：1)逼近過程始終保持目標星在主星視線范圍內，或者使得視線方向與目標矢量方向重合，且兩者無相對旋轉；2)主星具備從任意方向逼近目標的能力，且逼近過程中兩者不發(fā)生碰撞；3)主星和目標的相對距離和相對速度在一定時間內收斂到原點或指定狀態(tài)；4)主星和目標的相對速度小于一定閾值，兩者最終接近速度小于安全接近速度閾值.

要實現(xiàn)沿著任意方向逼近目標，可直接使得主星始終沿著視線方向逐漸逼近，因此可通過設計基于視線坐標系的控制策略完成位置和速度的控制，設計基于相對姿態(tài)的姿態(tài)控制策略完成指向控制.

對于位置控制而言，主星需要在視線軸方向上逐漸逼近目標，并使得視線在空間的轉動角速度為零，不考慮導航誤差和未建模特性，設運動過程中最大相對速度為vmax，最大控制推力為Fmax，則有基于非線性解耦的遞階飽和PID位置控制器為

(23)

式中，m為主星質量，pr、ir、dr、dα、dβ為待定參數(shù)，且

將控制器(23)代入運動學方程(13)，有

(24)

根據(jù)定義，有視線坐標系到本體坐標系的轉換矩陣如下所示：

(25)

則有主星本體系下的控制力指令矢量為

(26)

對于相對姿態(tài)控制，則需要滿足主星逼近過程始終指向目標，需將方位角和高程角均調節(jié)到零，且在視線軸方向無相對旋轉運動，有三軸本體系對應的控制力矩分量分別為：

(27)

其中，ωx為視線軸慣性角速度，dtx，pty等均為待定控制參數(shù)，Tmax為各軸最大力矩.

2.5 停泊點設置

為保證在近距離交會過程中不發(fā)生碰撞損失，并增強任務的靈活性，在對目標星抓捕之前可增加一些特定的控制目標，即設置停泊點.在停泊點處可以實現(xiàn)抓捕設備的校準、故障排查以及指令等待，通過停泊點的設置可以增強最終逼近段的時間調整能力.

3 仿真系統(tǒng)建立與分析

為驗證本文設計系統(tǒng)的有效性，利用Matlab/Simulink，構建了可模擬非合作目標逼近過程的仿真系統(tǒng).

不失一般性，認為主星和目標處于同一軌道的不同位置，對應軌道半長軸7 000 km，偏心率0，軌道傾角97°，近地點幅角30°，升交點赤經(jīng)70°，主星和目標的平近點角分別為25°和25.000 2°，對應的地心慣性坐標系的初始速度和位置參數(shù)以及系統(tǒng)參數(shù)如表1所示.

設初始條件下目標位于主星視線軸方向，對應目標在主星本體系相對位置矢量為[24.43 0 0] m.不考慮雙目視覺敏感器視場約束和軌道攝動加速度以及干擾力矩作用，利用基于本文設計的算法構建的仿真系統(tǒng)，針對上述系統(tǒng)進行數(shù)學仿真，可得結果如圖3～7所示.

由圖3可以明顯看出，在考慮雙目視覺敏感器一定測量誤差的情況下，主星依次完成了對兩個停泊點的逼近和懸停任務，且始終沒有與目標發(fā)生碰撞，且停泊點2可以根據(jù)主星抓捕系統(tǒng)配置選取，以實現(xiàn)安全高效逼近；圖4給出的相對速度表明本文設計的位置控制器可滿足對最大相對速度的約束，且以近似時間最優(yōu)控制的特性依次完成不同停泊點逼近任務，其中前50 s的速度抖動主要由于初始速度誤差和姿態(tài)控制的疊加作用引起；圖5為主星控制推力矢量變化曲線，可以看出推力器近似呈現(xiàn)“bang-off-bang”的工作模式，有效發(fā)揮推力器最大控制能力，并在到達停泊點附近后切換為連續(xù)控制，避免系統(tǒng)振動；圖6給出了目標相對主星的方位角和高程角變化曲線，可以看出兩個角度在整個逼近過程始終保持在極小的范圍內，說明了姿態(tài)控制的有效性，需要說明的是出于任務需求和能量節(jié)約的綜合考慮，此處姿態(tài)控制的積分參數(shù)設置為0，且在目標和主星相對距離小于0.5 m時即停止姿態(tài)控制(對應約220～290 s和340～500 s)，對應的控制力矩變化曲線如圖7所示.

表1 衛(wèi)星及控制器仿真參數(shù)表

圖3 相對位置變化曲線(主星本體坐標系)Fig.3 Time history of relative position in spacecraft body frame

圖4 相對速度變化曲線(主星本體坐標系)Fig.4 Time history of relative velocity in spacecraft body frame

圖5 控制推力變化曲線(主星本體坐標系)Fig.5 Time history of control force in spacecraft body frame

圖6 目標指向角度變化曲線(主星視線坐標系)Fig.6 Time history of pointing angles of target in spacecraft line-of-sight frame

圖7 控制力矩變化曲線(主星本體坐標系)Fig.7 Time history of control torque in spacecraft body frame

4 結 論

本文設計了一種帶有成像誤差的雙目視覺敏感器目標位置矢量解算方法，用于非合作目標的最后逼近任務.考慮系統(tǒng)的快速性、能量消耗、可靠性需求以及速度約束和控制約束等，設計具有時間最優(yōu)特性的遞階飽和PID位置控制律和姿態(tài)控制律.通過構建一個包含軌道及姿態(tài)動力學模型、敏感器測量模型、執(zhí)行機構動力學模型以及控制律模塊的綜合仿真系統(tǒng)，對本文提出的方法進行驗證.數(shù)學仿真結果表明，設計的控制方案可依次完成停泊點的逼近任務，結合停泊點的設計可完成任意方向的目標逼近任務，且整個飛行過程未與目標發(fā)生碰撞，充分說明了設計算法的有效性和優(yōu)越性.

此外，設計過程并未考慮推力器具體配置，即考慮主星可在任意方向產(chǎn)生推力而無需姿態(tài)機動，并且在實際的非合作和合作目標的近距離逼近過程中，姿態(tài)和軌道的耦合動力學是需要重點考慮的重要內容，在未來研究中可針對特定推力器配置開展控制分配優(yōu)化和考慮推力配置約束的位姿控制方案設計.

[1] 李金宗, 逯仁貴等. 空間交會對接技術[M]. 哈爾濱工業(yè)大學出版社,1997.

[2] 唐國金, 羅亞中等. 空間交會對接任務規(guī)劃[M]. 科學出版社,2008.

[3] 劉濤,解永春. 非合作目標交會相對導航方法研究[J]. 航天控制,2006,24(2):48-53. LIU T, XIE Y C. A Study on relative navigation for spacecraft rendezvous with a non-cooperative target[J]. Aerospace Control, 2006,24(2):48-53.

[4] 張世杰,曹喜濱,張凡. 基于特征點的空間目標三維位姿單目視覺確定算法[J]. 中國科學：信息科學, 2010,40(4):591-604.

[5] 徐文福,梁斌,李成.空間機器人捕獲非合作目標的測量與規(guī)劃方法[J]. 機器人, 2010,32(1):61-69. XU W F, LIANG B, LI C. Measurement and planning approach of space robot for capturing non-cooperative target[J]. Robot, 2010,32(1):61-69.

[6] 陳小平.航天器自主交會對接導航與控制關鍵技術研究[D]. 電子科技大學博士學位論文, 2012. CHENG X P. Key techniques of navigation and control for autonomous rendezvous and docking of spacecraft[D]. Chengdu: University of Electronic Science and Technology of China, 2012.

[7] 張柏楠. 航天器交會對接任務分析與設計[M]. 北京:科學出版社,2011.

[8] 張立佳. 空間非合作目標飛行器在軌交會控制研究[D]. 哈爾濱工業(yè)大學博士學位論文, 2008. ZHANG L J. Research on control for rendezvous with noncooperative target in orbit[D]. Harbin: Harbin Institute of Technology, 2008.

[9] 董玉久,潘秀英,楊欣欣. 兩異面直線公垂線垂足位置的計算方法[J]. 哈爾濱科學技術大學學報, 1990,14(1):84-88. DONG Y J, PAN X Y, YANG X X. A calculation method for finding the foot of common perpendciular of lines on different places[J]. Journal of Harbin University of Science and Technology, 1990,14(1):84-88.

[10] 李傳江, 馬廣富. 最優(yōu)控制[M]. 北京:科學出版社, 2011.

[11] WIE B, LU J. Feedback control logic for spacecraft eigenaxis rotations under slew rate and control constraints [J]. Journal of Guidance, Control, and Dynamics, 1995,18(6):1372-1379.

Control System Design and Simulation of Binocular Vision-Based Non-Cooperative Target Proximity Mission

GUO Yanning1, HAN Xu2, GUO Zengqian1, ZHANG Yao1, MA Guangfu1

(1.HarbinInstituteofTechnology,Dept.ofcontrolscienceandengineering,Harbin150001，China；2.ShanghaiInstituteofSatelliteEngineering,Shanghai200240，China)

The control system design and simulation of non-cooperative target proximity mission are investigated. Considering binocular vision sensor with imaging errors, a new relative position computation method is proposed to obtain the position of non-cooperative target. With constraints like maximum relative velocity, control force and control torque, the cascade saturation PID control based on nonlinear decoupled position control and attitude control algorithms are developed in the line-of-sight coordinate system. Further, holding points are adopted in conjunction with the proposed control methods to avoid collision with the target. Numerical simulation results in a typical non-cooperative target proximity scenario are provided to demonstrate the effectiveness and reliability of the proposed strategy.

non-cooperative target; binocular vision sensor; cascade saturation PID; position control; attitude control; decoupling control

*國家自然科學基金資助項目(61403103)，中國博士后科學基金資助項目(2014M550195)和中央高?；究蒲袠I(yè)務費專項資金資助項目(HIT.NSRIF. 2013135，HIT.NSRIF.2014035).

2015-03-03

V412.4

A

1674-1579(2015)05-0006-06

10.3969/j.issn.1674-1579.2015.05.002

郭延寧(1985—)，男，博士，講師，研究方向為航天器姿態(tài)動力學與控制，深空探測制導與控制技術等；韓 旭(1982—)，男，高級工程師，研究方向為航天器系統(tǒng)試計與測試，航天器動力學與控制；郭增千(1994—)，男,本科，研究方向為航天器控制系統(tǒng)設計；張 瑤(1989—)，女，博士研究生，研究方向為航天器制導系統(tǒng)設計、非線性控制等；馬廣富(1963—)，男，教授，研究方向為最優(yōu)控制、航天器動力學與控制，非合作目標近距離操作等.