空間自旋目標(biāo)混合EFIR/DFT運(yùn)動(dòng)軌跡預(yù)測(cè)算法

韓 冬，黃攀峰，劉正雄，鹿振宇，齊志剛

(1. 西北工業(yè)大學(xué)航天學(xué)院智能機(jī)器人研究中心，西安 710072；2.西北工業(yè)大學(xué)航天飛行動(dòng)力學(xué)技術(shù)重點(diǎn)實(shí)驗(yàn)室，西安 710072；3.山西師范大學(xué)物理與信息工程學(xué)院，臨汾 041000)

空間自旋目標(biāo)混合EFIR/DFT運(yùn)動(dòng)軌跡預(yù)測(cè)算法

韓 冬1, 2, 3，黃攀峰1, 2，劉正雄1, 2，鹿振宇1, 2，齊志剛1, 2

(1. 西北工業(yè)大學(xué)航天學(xué)院智能機(jī)器人研究中心，西安 710072；2.西北工業(yè)大學(xué)航天飛行動(dòng)力學(xué)技術(shù)重點(diǎn)實(shí)驗(yàn)室，西安 710072；3.山西師范大學(xué)物理與信息工程學(xué)院，臨汾 041000)

針對(duì)自旋目標(biāo)運(yùn)動(dòng)軌跡的跟蹤與預(yù)測(cè)中魯棒性與時(shí)效性問(wèn)題，本文提出一種在視覺(jué)測(cè)量目標(biāo)位姿的基礎(chǔ)上，通過(guò)混合擴(kuò)展有限沖擊響應(yīng)(EFIR)/離散傅立葉變換(DFT)估計(jì)目標(biāo)狀態(tài)與特征參數(shù)，進(jìn)而預(yù)測(cè)目標(biāo)軌跡的方法。在視覺(jué)相機(jī)對(duì)目標(biāo)特征點(diǎn)位姿測(cè)量的基礎(chǔ)上，將運(yùn)動(dòng)過(guò)程分解為平動(dòng)與轉(zhuǎn)動(dòng)，時(shí)域與頻域同步估計(jì)目標(biāo)的狀態(tài)與動(dòng)力學(xué)參數(shù)，采用DFT估計(jì)與平動(dòng)相關(guān)參量，采用EFIR估計(jì)與轉(zhuǎn)動(dòng)相關(guān)參量，根據(jù)空間漂浮目標(biāo)動(dòng)力學(xué)方程，實(shí)現(xiàn)在過(guò)程噪聲與量測(cè)噪聲未知的復(fù)雜條件下對(duì)目標(biāo)軌跡的長(zhǎng)期準(zhǔn)確預(yù)測(cè)，并通過(guò)地面機(jī)器人模擬試驗(yàn)對(duì)預(yù)測(cè)方法的正確性和有效性開(kāi)展驗(yàn)證。結(jié)果表明：利用本文提出的方法實(shí)現(xiàn)了對(duì)空間自旋目標(biāo)運(yùn)動(dòng)軌跡的準(zhǔn)確預(yù)測(cè)；與傳統(tǒng)基于擴(kuò)展卡爾曼濾波的預(yù)測(cè)方法相比，在過(guò)程噪聲、量測(cè)噪聲未知的條件下，文中提出的方法有效縮短了參數(shù)收斂時(shí)間，提高了參數(shù)估計(jì)與軌跡預(yù)測(cè)精度。

空間機(jī)器人；空間自旋目標(biāo)；運(yùn)動(dòng)軌跡預(yù)測(cè)；擴(kuò)展有限沖擊響應(yīng)濾波器；離散傅立葉變換

0 引 言

空間機(jī)器人具有視覺(jué)辨識(shí)與運(yùn)動(dòng)控制系統(tǒng)，可完成對(duì)合作或非合作目標(biāo)的在軌捕獲與釋放，執(zhí)行如軌道垃圾清理、航天器在軌維護(hù)等空間任務(wù)[1-3]。為完成對(duì)目標(biāo)的在軌服務(wù)，在接近與停靠目標(biāo)的過(guò)程中，目標(biāo)相對(duì)空間智能機(jī)器人往往存在一定的線(xiàn)速度與角速度，由于信息處理以及位姿解算需要耗費(fèi)一定時(shí)間，因此，實(shí)時(shí)追蹤目標(biāo)運(yùn)動(dòng)軌跡并對(duì)運(yùn)動(dòng)目標(biāo)的軌跡進(jìn)行預(yù)測(cè)是十分必要的，也是開(kāi)展目標(biāo)捕獲與在軌服務(wù)的基礎(chǔ)[4-5]。

國(guó)內(nèi)外許多學(xué)者開(kāi)展了空間智能機(jī)器人相關(guān)領(lǐng)域的研究，在路徑規(guī)劃方面，Zhou等[6]提出了奇異魯棒規(guī)劃方法，解決了路徑規(guī)劃中的算法與動(dòng)力學(xué)奇異問(wèn)題。在基于視覺(jué)的目標(biāo)識(shí)別方面，Linchter等[7]將雙卡爾曼濾波器應(yīng)用到自由漂浮目標(biāo)的位姿估計(jì)中，通過(guò)雙濾波器差分降低視覺(jué)傳感器噪聲，因?yàn)槊恳淮蔚膮?shù)估計(jì)都需要進(jìn)行兩次濾波器遞歸運(yùn)算，降低了執(zhí)行器的工作效率，增加了處理時(shí)間。Aghili等[8-9]提出了擴(kuò)展卡爾曼濾波(Extended Kalman filter,EKF)與最近點(diǎn)迭代算法；劉厚德等[10]針對(duì)自旋目標(biāo)運(yùn)動(dòng)預(yù)測(cè)，提出采用無(wú)損卡爾曼濾波方法，但是這些方法均需要預(yù)先估計(jì)量測(cè)噪聲與過(guò)程噪聲的協(xié)方差，這對(duì)于空間任務(wù)來(lái)說(shuō)是十分困難的。Hillenbrand等[11]利用最小二乘法對(duì)空間目標(biāo)進(jìn)行參數(shù)辨識(shí)與運(yùn)動(dòng)估計(jì)，這種方法在發(fā)生遮擋、抖動(dòng)等情況造成非等間隔采樣的條件下會(huì)顯著降低預(yù)測(cè)精度。

前期的研究工作以及地面驗(yàn)證試驗(yàn)表明，在目標(biāo)位置坐標(biāo)獲得過(guò)程中，可能出現(xiàn)的相機(jī)抖動(dòng)、光照變化、目標(biāo)漂浮及遮擋等影響，造成采樣時(shí)間較短，采樣過(guò)程不連續(xù)及測(cè)量噪聲與過(guò)程噪聲未知等問(wèn)題[12]。因此，設(shè)計(jì)基于位姿測(cè)量信息，對(duì)目標(biāo)運(yùn)動(dòng)軌跡進(jìn)行追蹤與預(yù)測(cè)的魯棒性算法便十分必要。

本文首先假設(shè)已經(jīng)獲得了帶有噪聲的目標(biāo)位姿測(cè)量數(shù)據(jù)，在空間自旋目標(biāo)動(dòng)力學(xué)方程的基礎(chǔ)上，設(shè)計(jì)EFIR/DFT濾波器，為滿(mǎn)足濾波器的要求，對(duì)目標(biāo)動(dòng)力學(xué)方程進(jìn)行了線(xiàn)性化，從而實(shí)現(xiàn)運(yùn)動(dòng)狀態(tài)信息與動(dòng)力學(xué)參數(shù)的準(zhǔn)確估計(jì)，最后根據(jù)動(dòng)力學(xué)方程，獲得目標(biāo)的軌跡預(yù)測(cè)。本文算法的特點(diǎn)在于通過(guò)分別處理平動(dòng)與轉(zhuǎn)動(dòng)，減少了估計(jì)狀態(tài)，提高了收斂速度。通過(guò)采用加窗的方法，避免了噪聲累加對(duì)估計(jì)值的影響，在噪聲統(tǒng)計(jì)特性未知的條件下有更好的估計(jì)效果，對(duì)擾動(dòng)有更強(qiáng)的魯棒性，提高了復(fù)雜條件下自旋目標(biāo)的軌跡預(yù)測(cè)精度。

1 空間自旋目標(biāo)運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)建模

本文主要研究對(duì)象為漂浮旋轉(zhuǎn)目標(biāo)，為預(yù)測(cè)目標(biāo)的運(yùn)動(dòng)軌跡，首先建立空間坐標(biāo)系模型，如圖1所示。坐標(biāo)系{A}為慣性坐標(biāo)系，坐標(biāo)系{B}為固聯(lián)于衛(wèi)星本體的坐標(biāo)系，衛(wèi)星質(zhì)心O為坐標(biāo)系{B}原點(diǎn)，坐標(biāo)軸平行于衛(wèi)星主慣量軸。坐標(biāo)系{C}以目標(biāo)點(diǎn)為原點(diǎn)，坐標(biāo)軸平行于帆板支架，坐標(biāo)系{C} 的位姿通過(guò)視覺(jué)系統(tǒng)測(cè)量獲得。ρ為衛(wèi)星質(zhì)心到目標(biāo)點(diǎn)位置矢量，ω為目標(biāo)旋轉(zhuǎn)角速度，ρ,ω均為在坐標(biāo)系{B}下的表達(dá)。

空間自旋目標(biāo)姿態(tài)運(yùn)動(dòng)學(xué)建?？梢圆捎梅较蛴嘞揖仃嚒⑺脑獢?shù)以及修正的羅德里格參數(shù)等，文獻(xiàn)[13]提供了一種微分建模思路，為使無(wú)窮小角位移描述形式簡(jiǎn)單，本文采用四元數(shù)描述空間自旋目標(biāo)姿態(tài)運(yùn)動(dòng)學(xué)，坐標(biāo)系{B}相對(duì)于{A}的旋轉(zhuǎn)通過(guò)四元數(shù)q表示，運(yùn)動(dòng)學(xué)方程可表示為：

(1)

圖1 空間自旋目標(biāo)軌跡預(yù)測(cè)Fig. 1 Trajectory prediction of space spinning target

q??q0E4+Ω(qv)

(2)

R(q)=(2q02-1)E3+2q0[qv×]+2qvqvT

(3)

為了估計(jì)目標(biāo)的運(yùn)動(dòng)狀態(tài)與參數(shù)，建立通過(guò)歐拉方程描述的目標(biāo)姿態(tài)動(dòng)力學(xué)方程：

(4)

式中：I為轉(zhuǎn)動(dòng)慣量，τ為擾動(dòng)力矩，根據(jù)漂浮目標(biāo)動(dòng)力學(xué)方程，當(dāng)τ=0時(shí)無(wú)法通過(guò)目標(biāo)運(yùn)動(dòng)軌跡分別獨(dú)立估計(jì)出三個(gè)方向轉(zhuǎn)動(dòng)慣量，為解決這個(gè)問(wèn)題，引入慣量參數(shù)表示：

(5)

式中：Ixx,Iyy,Izz為目標(biāo)主慣量，應(yīng)滿(mǎn)足以下條件：

進(jìn)而可求得慣量參數(shù)的約束條件

px>-1,py>-1,pz>-1

用慣性參數(shù)改寫(xiě)動(dòng)力學(xué)方程為：

(6)

式中:

Ic=diag(Ixx,Iyy,Izz)。tr(Ic)表示Ic的跡，ετ用來(lái)描述太陽(yáng)能帆板支架、重力梯度等產(chǎn)生的阻尼效果，可以選擇采用過(guò)程噪聲來(lái)描述。

目標(biāo)點(diǎn)的位置與速度表達(dá)式可由以下方程描述：

rs=r+R(q)ρ=r0+vt+R(q)ρ

(7)

(8)

至此，通過(guò)式(1)、(4)及其簡(jiǎn)化與變形獲得了空間自旋目標(biāo)的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)方程。式(7)、(8)描述了目標(biāo)的運(yùn)動(dòng)狀態(tài)。

2 EFIR/DFT濾波器設(shè)計(jì)與目標(biāo)軌跡預(yù)測(cè)

為了實(shí)現(xiàn)對(duì)運(yùn)動(dòng)目標(biāo)的軌跡預(yù)測(cè)，需要獲得目標(biāo)的動(dòng)力學(xué)參數(shù)，因?yàn)闇y(cè)量系統(tǒng)僅能提供目標(biāo)的位姿信息，所以有必要對(duì)動(dòng)力學(xué)模型中的未知參數(shù)進(jìn)行估計(jì)。根據(jù)式(7),考慮到運(yùn)動(dòng)目標(biāo)平動(dòng)與轉(zhuǎn)動(dòng)的可分解性，分別設(shè)計(jì)了頻域與時(shí)域的估計(jì)濾波器，對(duì)于平動(dòng)參量采用頻域最優(yōu)濾波器DFT估計(jì)目標(biāo)衛(wèi)星質(zhì)心位置及線(xiàn)速度。對(duì)于轉(zhuǎn)動(dòng)參量采用時(shí)域EFIR濾波器估計(jì)運(yùn)動(dòng)狀態(tài)與動(dòng)力學(xué)參數(shù)?？紤]到動(dòng)力學(xué)方程的非線(xiàn)性無(wú)法滿(mǎn)足線(xiàn)性濾波器的要求，首先對(duì)動(dòng)力學(xué)方程進(jìn)行了線(xiàn)性化。

2.1 量測(cè)方程線(xiàn)性化

假定視覺(jué)測(cè)量系統(tǒng)可以獲得帶有噪聲的目標(biāo)點(diǎn)相對(duì)位姿并可解算到慣性坐標(biāo)系下，如圖1所示，目標(biāo)位置由矢量rs表示，目標(biāo)所在坐標(biāo)系{C}相對(duì)于慣性坐標(biāo)系{A}的姿態(tài)由四元數(shù)η表示，測(cè)量向量Z可表示為：

(9)

η=μ?q

(10)

式中:?為四元數(shù)乘，定義與轉(zhuǎn)動(dòng)有關(guān)的狀態(tài)矢量：

(11)

式中：qv,μv分別為四元數(shù)的矢量部分。則觀測(cè)方程中轉(zhuǎn)動(dòng)部分可表示為：

(12)

式中:

(13)

(14)

描述微小旋轉(zhuǎn)的四元數(shù)方程：

(15a)

(15b)

將式(15)代入式(10)并忽略高階小項(xiàng)可得：

(16)

式中：

Γ=

至此，式(14)、(16)給出了觀測(cè)方程的一階近似值，并可以由此得出量測(cè)矩陣：

(17)

2.2 狀態(tài)方程線(xiàn)性化

根據(jù)式(1)、(6)所描述的目標(biāo)動(dòng)力學(xué)方程，將連續(xù)時(shí)間狀態(tài)空間模型描述為：

(18)

(19a)

(19b)

對(duì)于方程(6)的線(xiàn)性化可直接由全微分公式給出：

將方程(18)寫(xiě)成離散形式，有

X(n)=Φ(n)X(n-1)+v(n)

(20)

其中，v(n)表示離散的過(guò)程噪聲，Φ(n)表示離散狀態(tài)轉(zhuǎn)移矩陣。對(duì)于離散系統(tǒng)有

Φ(n) ?Φ(tn+1,tn)=Φ(tn+1-tn)=

eF(tn)(tn+1-tn)≈E15+(tn+1-tn)F(tn)

(21)

2.3 混合EFIR/DFT濾波器設(shè)計(jì)

滿(mǎn)足無(wú)偏要求的擴(kuò)展有限沖擊響應(yīng)(EFIR)濾波器最早由Shmaliy[15]提出，不同于采用遞歸算法的卡爾曼濾波器，EFIR濾波器采用迭代算法，因?yàn)樵谒惴ㄖ凶隽思哟疤幚?，不?huì)產(chǎn)生因持續(xù)的偏差累計(jì)而導(dǎo)致濾波器發(fā)散的現(xiàn)象，魯棒性好。但是，加窗的影響會(huì)增加計(jì)算量，使收斂時(shí)間變長(zhǎng)，針對(duì)這一問(wèn)題，本文根據(jù)式(7)～(8)將目標(biāo)運(yùn)動(dòng)分解為相對(duì)慣性系的平動(dòng)與轉(zhuǎn)動(dòng)，分別進(jìn)行時(shí)域與頻域處理，對(duì)于平動(dòng)采用離散傅立葉變換(DFT)估計(jì)質(zhì)心位置與線(xiàn)速度，對(duì)于轉(zhuǎn)動(dòng)則采用擴(kuò)展有限沖擊響應(yīng)(EFIR)濾波器估計(jì)角速度、慣性參量及目標(biāo)點(diǎn)距質(zhì)心位置，從而降低了估計(jì)維數(shù)，提高了收斂速度。

2.3.1 DFT濾波器設(shè)計(jì)

對(duì)目標(biāo)位置序列進(jìn)行DFT變換，相當(dāng)于把這個(gè)序列通過(guò)一個(gè)FIR數(shù)字濾波器，位置序列rs(n)的離散傅立葉變換式

(22)

根據(jù)式(7)、(8)，質(zhì)心位置r0與線(xiàn)速度v均為目標(biāo)位置序列及其差分的直流分量，因此可通過(guò)DFT濾波器準(zhǔn)確估計(jì)出，并且由于估計(jì)值為直流分量，因而對(duì)高頻觀測(cè)噪聲以及信號(hào)的非等間隔采樣具有很強(qiáng)抑制性。理論上角速度為周期函數(shù)，也可由DFT估計(jì)得出，但由于估計(jì)需要滿(mǎn)足最低周期要求，這需要觀測(cè)最小一個(gè)周期的時(shí)間，這會(huì)使得觀測(cè)時(shí)間過(guò)長(zhǎng)，正是這個(gè)原因使我們提出了通過(guò)EFIR濾波器進(jìn)行角速度與動(dòng)力學(xué)參數(shù)估計(jì)。

2.3.2 EFIR濾波器設(shè)計(jì)

將離散線(xiàn)性系統(tǒng)模型采用狀態(tài)空間法進(jìn)行描述，由式(12)、(20)描述的狀態(tài)矩陣和觀測(cè)矩陣表示為：

Xn=ΦnXn-1+Vn

(23)

Zn=HnXn+Wn

(24)

其中，過(guò)程噪聲與量測(cè)噪聲均假定為零均值分布的高斯白噪聲，并且假定彼此互不相關(guān)。設(shè)計(jì)EFIR濾波器的有效觀測(cè)長(zhǎng)度N，這可以通過(guò)地面試驗(yàn)獲得，也可以通過(guò)計(jì)算量測(cè)偏差的均方值在線(xiàn)估計(jì)得出[16]。當(dāng)獲得第n時(shí)刻量測(cè)值時(shí)，從時(shí)刻m=n-N+1到時(shí)刻n之間的N個(gè)量測(cè)值為有效量測(cè)值，為保證系統(tǒng)的因果性，需要滿(mǎn)足m≥0，即n≥N-1。

目標(biāo)n時(shí)刻狀態(tài)的估計(jì)值可以表示為

(25)

其中

而

該算法是使用正交條件，通過(guò)優(yōu)化估計(jì)均方誤差最小原則獲得，其優(yōu)點(diǎn)是能夠在噪聲統(tǒng)計(jì)特性未知的情況下對(duì)信號(hào)進(jìn)行估計(jì)，當(dāng)N=Nopt時(shí)可以獲得近似最優(yōu)估計(jì)。其不足之處在于N的增大會(huì)導(dǎo)致矩陣維數(shù)的增大，從而使計(jì)算量增加，求解速度減慢。引入類(lèi)卡爾曼濾波器的迭代算法可以很好地解決這一問(wèn)題，目標(biāo)狀態(tài)的迭代估計(jì)值為

(26)

(27)

式中:Gl為廣義噪聲功率增益，通過(guò)下式迭代計(jì)算得出

(28)

可見(jiàn)，Gl的計(jì)算僅需已知矩陣Hl與Φl，Hl為l時(shí)刻觀測(cè)矩陣，Φl為l時(shí)刻狀態(tài)轉(zhuǎn)移矩陣，在濾波器增益計(jì)算中，不需要預(yù)測(cè)噪聲統(tǒng)計(jì)特性，該特點(diǎn)使算法適合噪聲特性未知的復(fù)雜條件下空間任務(wù)。算法流程如圖2所示。

圖2 EFIR濾波算法Fig.2 EFIR filter algorithm

2.4 目標(biāo)軌跡預(yù)測(cè)

圖3 測(cè)量系統(tǒng)實(shí)驗(yàn)平臺(tái)Fig. 3 The equipment of measuring system

3 試驗(yàn)驗(yàn)證

為驗(yàn)證本文提出的混合EFIR/DFT軌跡預(yù)測(cè)算法的正確性，現(xiàn)利用兩臺(tái)機(jī)器人對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)試驗(yàn)驗(yàn)證，測(cè)試平臺(tái)如圖3所示，采用ABB-14000機(jī)器人抓持衛(wèi)星模型，該機(jī)器人具有6自由度，可以根據(jù)航天器零重力空間動(dòng)力學(xué)模型解算出軌跡精確的模擬漂移與自旋運(yùn)動(dòng)，采用ABB-120機(jī)器人模擬空間平臺(tái)，機(jī)械臂末端裝備手眼相機(jī)，通過(guò)振動(dòng)模擬平臺(tái)隨機(jī)抖動(dòng)。手眼相機(jī)采集目標(biāo)位置數(shù)據(jù)并解算姿態(tài)信息，數(shù)據(jù)采樣頻率為10 Hz，采樣數(shù)據(jù)傳輸?shù)缴衔粰C(jī)中進(jìn)行處理。

衛(wèi)星模型的轉(zhuǎn)動(dòng)慣量設(shè)計(jì)為Ixx=0.08 kg·m2,Iyy=0.04 kg·m2,Izz=0.05 kg·m2，由此可根據(jù)式(8)計(jì)算得到慣性參量值

R=diag((0.015)2E3m2,(0.04)2E4)

試驗(yàn)首先通過(guò)視覺(jué)系統(tǒng)獲得目標(biāo)的位姿數(shù)據(jù)，由于目標(biāo)與相機(jī)所處角度的不同所導(dǎo)致的光照變化及機(jī)械臂的振動(dòng)，造成了采樣數(shù)據(jù)的非平穩(wěn)，量測(cè)數(shù)據(jù)如圖4所示。根據(jù)獲得的量測(cè)數(shù)據(jù)，頻域上通過(guò)DFT算法估計(jì)目標(biāo)質(zhì)心位置及漂移線(xiàn)速度，離散傅立葉變換是頻域上的最優(yōu)估計(jì)，如圖5所示，其中質(zhì)心位置的估計(jì)采用128個(gè)數(shù)據(jù)點(diǎn)，12.8 s即可完成采樣，平均誤差0.019 m。線(xiàn)速度的估計(jì)采用256個(gè)數(shù)據(jù)點(diǎn)，平均誤差0.45 mm，數(shù)據(jù)中的有限零點(diǎn)并不影響估計(jì)結(jié)果，對(duì)非平穩(wěn)采樣有較強(qiáng)的魯棒性,其中估計(jì)誤差僅與采樣點(diǎn)數(shù)目有關(guān)，采樣點(diǎn)增加可提高估計(jì)精度。時(shí)域上通過(guò)EFIR算法估計(jì)動(dòng)力學(xué)參數(shù)與目標(biāo)相對(duì)質(zhì)心位置及旋轉(zhuǎn)角速度，根據(jù)狀態(tài)數(shù)選取批處理長(zhǎng)度K=5，并通過(guò)計(jì)算求出此時(shí)UFIR濾波器最優(yōu)窗長(zhǎng)Nopt=37[16]，估計(jì)結(jié)果如圖6所示，從試驗(yàn)結(jié)果可以看出，估計(jì)值較準(zhǔn)確地收斂到了真值。

圖4 手眼相機(jī)測(cè)量目標(biāo)位置與姿態(tài)Fig. 4 Target pose measurements obtained from hand-eye camera

圖5 DFT濾波器質(zhì)心位置與線(xiàn)速度估計(jì)Fig. 5 Estimate position of the target center of mass and drift velocity with DFT filter

圖6 EFIR濾波器目標(biāo)參數(shù)估計(jì)Fig. 6 Estimate parameters of the target with EFIR filter

選取一步預(yù)測(cè)協(xié)方差的歐幾里得范數(shù)作為估計(jì)值收斂的判斷，當(dāng)范數(shù)值小于設(shè)定的閘值時(shí)對(duì)已采樣數(shù)據(jù)進(jìn)行DFT變換。

對(duì)于εth的選取兼顧了準(zhǔn)確性與收斂時(shí)間的矛盾，增大閘值可以縮短收斂時(shí)間，但也會(huì)降低預(yù)測(cè)精度。本文將閘值設(shè)定為0.2，滿(mǎn)足后續(xù)抓捕對(duì)預(yù)測(cè)精度的要求。由圖7可知，與通常采用的EKF相比，本文提出的算法由于采用了時(shí)域與頻域的同步處理，降低了估計(jì)維度，縮短了收斂時(shí)間。

圖7 估計(jì)器收斂檢測(cè)Fig. 7 Detecting the estimator convergence

圖8 目標(biāo)軌跡預(yù)測(cè)結(jié)果Fig. 8 Predictive results of target trajectory

為比較不同方法獲得的軌跡預(yù)測(cè)準(zhǔn)確度，選取EKF作為比較對(duì)象，通過(guò)計(jì)算實(shí)驗(yàn)結(jié)果與預(yù)測(cè)值的均方根偏差作為準(zhǔn)確性評(píng)估標(biāo)準(zhǔn)

(29)

式中：δx,δy,δz為實(shí)驗(yàn)結(jié)果與預(yù)測(cè)值在x軸，y軸，z軸的差值。本文算法的均方根偏差值為0.0120，EKF的均方根偏差為0.0175 ?？梢?jiàn)在噪聲參數(shù)未知的條件下，本文提出的算法具有更高的準(zhǔn)確性。

4 結(jié) 論

本文針對(duì)空間抓捕任務(wù)中的自旋目標(biāo)軌跡預(yù)測(cè)問(wèn)題，提出了混合EFIR/DFT濾波算法，根據(jù)公式的推導(dǎo)與試驗(yàn)的結(jié)果可得出如下結(jié)論：

1)在空間變化的光照環(huán)境與不確定的量測(cè)角度等復(fù)雜條件下，目標(biāo)的視覺(jué)識(shí)別會(huì)產(chǎn)生采樣的不連續(xù)，過(guò)程噪聲與量測(cè)噪聲的不可預(yù)知等問(wèn)題，在這種條件下，算法魯棒性便愈加重要，本算法采用的EFIR算法通過(guò)時(shí)域加窗來(lái)解決估計(jì)偏差累計(jì)造成的估計(jì)發(fā)散問(wèn)題，具有較強(qiáng)的魯棒性。

2)本文算法在時(shí)域與頻域同步對(duì)采樣數(shù)據(jù)進(jìn)行處理，與傳統(tǒng)的僅從時(shí)域?qū)?shù)據(jù)進(jìn)行處理相比，最大限度的使用了采樣數(shù)據(jù)所提供的信息，將時(shí)域?yàn)V波器的維度從7維降低到5維，并且不需要預(yù)估噪聲統(tǒng)計(jì)特性參數(shù)，針對(duì)復(fù)雜噪聲的空間環(huán)境，可以有效地減少濾波器的收斂時(shí)間。

3)機(jī)械臂模擬目標(biāo)運(yùn)動(dòng)學(xué)試驗(yàn)證實(shí)，在噪聲環(huán)境未知的條件下，本文所提出算法的準(zhǔn)確性?xún)?yōu)于傳統(tǒng)擴(kuò)展卡爾曼濾波算法，能夠?qū)崿F(xiàn)對(duì)目標(biāo)位置的長(zhǎng)期預(yù)測(cè)。

[1] 劉正雄,鹿振宇,黃攀峰. 基于遞推差分進(jìn)化算法的空間機(jī)器人參數(shù)辨識(shí)[J]. 宇航學(xué)報(bào), 2014(10):1127-1134 .[Liu Zheng-xiong, Lu Zhen-yu, Huang Pan-feng. Parameter identification of space robot based on recursive different evolution algorithm[J]. Journal of Astronautics, 2014(10):1127-1134.]

[2] 張帆,黃攀峰. 空間繩系機(jī)器人抓捕非合作目標(biāo)的質(zhì)量特性參數(shù)辨識(shí)[J]. 宇航學(xué)報(bào), 2015(6):630-639.[Zhang Fan, Huang Pan-feng. Inertia parameter estimation for an non-cooperative target captured by a space tethered system[J]. Journal of Astronautics, 2015(6):630-639.]

[3] 蔡佳,黃攀峰,王東科,等. 空間繩系機(jī)器人單目視覺(jué)伺服控制系統(tǒng)[J]. 宇航學(xué)報(bào), 2015(2):133-141.[Cai Jia, Huang Pan-feng, Wang Dong-ke, et al. Monocular visual servo control system scheme for tethered space robot[J]. Journal of Astronautics, 2015(2):133-141.]

[4] Angel F A,Ou M, Khanh P, et al. A review of space robotics technologies for on-orbit servicing[J]. Progress in Aerospace Sciences, 2014, 68: 1-26.

[5] 梁斌, 徐文福, 李成, 等. 地球靜止軌道在軌服務(wù)技術(shù)研究現(xiàn)狀與發(fā)展趨勢(shì)[J]. 宇航學(xué)報(bào), 2010, 31(1): 1-13.[ Liang Bin, Xu Wen-fu,Liu Cheng, et al. The status and prospect of orbital servicing in the geostationary orbit[J]. Journal of Astronautics, 2010, 31(1): 1-13.]

[6] Zhou C, Jin M H, Liu Y C, et al. Singularity robust path planning for real time base attitude adjustment of free-floating space robot [J]. International Journal of Automation and Computing, 201714(2): 169-178.

[7] Lichter M D, Dubowsky S. State, shape, and parameter estimation of space objects from range images[C].Robotics and Automation, ICRA′04, New Orleans, USA, April 26-May 1, 2004.

[8] Aghili F. A prediction and motion-planning scheme for visually guided robotic capturing of free-floating tumbling objects with uncertain dynamics [J]. IEEE Transactions on Robotics, 2012, 28(3): 634-49.

[9] Aghili F, Kuryllo M, Okouneva G, et al. Fault-tolerant position/attitude estimation of free-floating space objects using a laser range sensor [J]. IEEE Sensors Journal, 2011, 11(1): 176-85.

[10] 劉厚德, 梁斌, 徐文福, 等. 自旋目標(biāo)運(yùn)動(dòng)預(yù)測(cè)及自主捕獲路徑規(guī)劃方法 [J]. 吉林大學(xué)學(xué)報(bào)(工學(xué)版), 2014(3): 757-764. [ Liu Hou-de, Liang Bin, Xu Wen-fu, et al. Motion prediction and autonomous path planning for spinning target capturing[J]. Journal of Jilin University (Engineering and Technology Edition), 2014(3): 757-764.]

[11] Hillenbrand U,Lampariello R. Motion and parameter estimation of a free-floating space object from range data for motion prediction[C].SAIRAS, Munich, Germany, September 5-8, 2005.

[12] Wensong J, Zhongyu W, Mourelatos Z P. Application of nonequidistant fractional-order accumulation model on trajectory prediction of space manipulator [J]. IEEE/ASME Transactions on Mechatronics, 2016, 21(3): 1420-1427.

[13] Zhou C, Jin M, Liu Y, et al. Motion planning for redundant free-floating space robot with local optimization of reaction torque andjoint torque simultaneously[C]. IEEE International Conference on Robotics and Biomimetics (ROBIO), Qingdao, China, Dec. 3-7, 2016.

[14] Pittelkau M E. Kalman filtering for spacecraft system alignment calibration [J]. Journal of Guidance, Control, and Dynamics, 2001, 24(6): 1187-1195.

[15] Shmaliy Y S. An unbiased FIR filter for TIE model of a local clock in applications to GPS-based timekeeping [J].Ieee Transactions on Ultrasonics, Ferroelectrics, and Frequency Control, 2006, 53(5): 862-870.

[16] Ramirez-Echeverria F, Sarr A, Shmaliy Y S. Optimal memory for discrete-time FIR filters in state-space [J]. IEEE Transactions on Signal Processing, 2014, 62(3): 557-561.

韓 冬 (1983-) 男，博士生,主要從事空間機(jī)器人學(xué)、復(fù)合體穩(wěn)定控制等研究。

通信地址：西安市友誼西路127號(hào)(710072)

電話(huà)：13934179244

E-mail:alexcn@mail.nwpu.edu.cn

黃攀峰(1974-) 男，博士，教授,主要從事空間機(jī)器人學(xué)、空間遙操作等研究。本文通信作者。

通信地址：西安市友誼西路127號(hào)(710072)

電話(huà)：(029)88460366

E-mail:pfhuang@nwpu.edu.cn

A Hybrid EFIR/DFT Algorithm on TrajectoryPrediction of Space Spinning Target

HAN Dong1, 2, 3, HUANG Pan-feng1, 2, LIU Zheng-xiong1, 2, LU Zhen-yu1, 2, QI Zhi-gang1, 2

(1. Research Center of Intelligent Robotics, School of Astronautics, Northwestern Polytechnical University, Xi’an 710072, China;2. National Key Laboratory of Aerospace Flight Dynamics, Northwestern Polytechnical University, Xi’an 710072, China;3. School of Physics and InformationEngineering, Shanxi Normal University, Linfen 041000, China )

Based on the visual camera, a robust and efficient method for tracking and predicting the trajectory of a space spinning target is proposed in this paper. The initial discrete point set of a motion trajectory is firstly accumulated in real-time, and then the motion is decomposed into translation and rotation. A hybrid extended finite impulse response (EFIR)/discrete Fourier transform (DFT) is used to estimate the states and dynamics parameters in time domain and frequency domain simultaneously. According to the dynamic equations of a free-floating object, we achieve the long-term and precise prediction while the measurement noise and process noise are unknown. The experiment with ground robot is presented to verify the correctness and effectiveness of the proposed method. The results show that the trajectory of a space spinning target can be predicted accurately using our proposed method. Comparing with the traditional extended Kalman filtering algorithm, the presented control method can improve the the speed of the parameter estimation and the accuracy of the trajectory prediction despite the noise covariance and the initial conditions are not exactly known in advance.

Space robot; Space spinning target; Trajectory prediction; Extended finite impulse response; Discrete Fourier transform

2017-03-10;

2017-05-31

國(guó)家自然科學(xué)基金(11272256，60805034，61503231)

V448.2

A

1000-1328(2017)08-0804-09

10.3873/j.issn.1000-1328.2017.08.004