深空自主導(dǎo)航中的多傳感器融合技術(shù)以及解決方案

目錄CONTENTS1簡(jiǎn)介2多傳感器觀測(cè)方法3多傳感器融合技術(shù)4總結(jié)及展望GNSS,NewGlobal

Era簡(jiǎn)介深空探測(cè)一般指對(duì)月球及以遠(yuǎn)天體開(kāi)展的空間探測(cè)活動(dòng)，是人類探索宇宙的重要手段之一。深空自主導(dǎo)航是指探測(cè)器根據(jù)自身傳感器信息確定其在空間飛行時(shí)的瞬間位置和速度。簡(jiǎn)介深空導(dǎo)航存在的問(wèn)題01方法成熟簡(jiǎn)單，成功應(yīng)用于多次任務(wù)，精度隨距離增大而變差，適用于接近段任務(wù)。||rpi

||

arccos

rpi.si02Ｘ射線脈沖星導(dǎo)航理論上具有導(dǎo)航源信號(hào)穩(wěn)定、抗干擾能力強(qiáng)、可在太陽(yáng)系乃至更遠(yuǎn)區(qū)域進(jìn)行導(dǎo)航的特點(diǎn)。定位精度高，適用于深空全過(guò)程，但X射線脈沖星數(shù)量少，處于在線驗(yàn)證階段。03天文測(cè)速導(dǎo)航方法可直接通過(guò)幾何解算獲得探測(cè)器的速度，具有較高的實(shí)時(shí)性，但位置信息需通過(guò)速度積分進(jìn)行估計(jì)，存在位置誤差隨時(shí)間發(fā)散的問(wèn)題。||

r

|| ||r

||

r.r

r.vv

r

多傳感器融合技術(shù)深空自主導(dǎo)航的軌道動(dòng)力學(xué)方程r·

==

V一\I1／I廠111加項(xiàng)V知度未速二a\＼／+Il＼－r/／攝項(xiàng)壓度、石3r了II力兀速陽(yáng)加太動(dòng)工叨/1`\-\_／－pl-3

rpr-戶l-－－力項(xiàng)-－引度二rrz-

rz廠3v星r速行加－l＿i-＿-

－μl-曰／臚區(qū)個(gè)＼土＼I-r/ls3力項(xiàng)引度-5旦一rlTIIY陽(yáng)速太加＿／」＼＼＿f.Vf表示探測(cè)器再該坐標(biāo)系下的位置矢量，r=

llrll;讓是探測(cè)器再該坐標(biāo)系下的速度矢量，．石是第顆攝動(dòng)大行星相對(duì)坐標(biāo)系原點(diǎn)的位置矢量，．可是它們相對(duì)探測(cè)器的位置矢量，．np

是攝動(dòng)大行星的數(shù)目，．d為其他攝動(dòng)加速度矢量；μs是太陽(yáng)引力常數(shù)μi是第i顆攝動(dòng)大行星的引力常數(shù)||

r

|| ||r

||

r.r

r.vv

r

||rpi

||

arccos

rpi.si測(cè)角測(cè)距測(cè)速01卡爾曼濾波的五個(gè)基本公式02針對(duì)非線性系統(tǒng)，卡爾曼濾波不再適用，對(duì)此提出了擴(kuò)展卡爾曼濾波（EKF）。EKF

的基本原理是將非線性的系統(tǒng)方程以泰勒級(jí)數(shù)展開(kāi)，取一階近似，再按線性方程進(jìn)行處理。進(jìn)一步結(jié)合卡爾曼濾波的五個(gè)基本公式可得EKF的五個(gè)基本公式。狀態(tài)預(yù)測(cè)：觀測(cè)校正：031995

年，S.Julier

提出

Unsecented

卡爾曼濾波（

UKF）。UKF

的基本思想在于以無(wú)跡變換(

TransformUnscented

，UT)來(lái)遞推與更新在非線性模型中的當(dāng)前狀態(tài)值與當(dāng)前誤差協(xié)方差,從而獲取更好的性能。計(jì)算2n+1個(gè)Sigma采樣點(diǎn)1時(shí)間更新2測(cè)量更新3狀態(tài)更新4計(jì)算2n個(gè)容積點(diǎn)11時(shí)間更新

測(cè)量更新

狀態(tài)更新2 2k

|kk|k04容積卡爾曼濾波(Cubature

Kalman

filter,

CKF)由加拿大學(xué)者

Arasaratnam

和

Haykin

在

2009

年首次提出，CKF

基于三階球面徑向容積準(zhǔn)則，并使用一組容積點(diǎn)來(lái)逼近具有附加高斯噪聲的非線性系統(tǒng)的狀態(tài)均值和協(xié)方差，是當(dāng)前最接近貝葉斯濾波的近似算法。假設(shè)K時(shí)刻的狀態(tài)和協(xié)方x?

差P矩陣分別為(

j

)k|k

k|k

j

k|kx

S

x?j

1,

2......2njjjn[1] j

1,

2,......n

n[1] j

n

1,

n

2,......2n

k|kx(

j

)k

1|k

f(x(j)

)2n(j

)x

1

2nk

1|kj

1x?

k

1|k

12n(j

)ki

1k

1|k2n

P

(x

x?)(x(j)

x?)T

Q

k

1|k k

1|k k

1|k k

1|kk

1|k k

1|k

j k

1|kx(j)*

S

x?(j)

*zjk

1|kk

1|k

h(x

)12nzk

1|k2n

j

1z?

jk

1|k2nPzz12nk

1|kk

1|kj

1

)T

R

(z

j

z? )(z

j

z?k

1|k k

1|k k

1|k k

1|k2n(

j

))TPxz

12nk

1|kk

1|k k

1|k k

1|k k

1|kj

1

(x

x?)(z

j

z?

)xz zz

1k

1 k

1|k k

1|kK

P (P)x?k

1|k

1

x?k

1|k

Kk

1

(zk

1

z?k

1|kPzz

KTk

1|k

1 k

1|k k

1

k

1|k k

1P

P

KTk|k k|k

k|kP

S

S051 1粒子濾波（FilterParticle，PF）是一類通過(guò)隨機(jī)樣本點(diǎn)表示系統(tǒng)狀態(tài)變量的后驗(yàn)概率分布的遞推貝葉斯濾波方法，在實(shí)際工程應(yīng)用方面有著較好的表現(xiàn)。其將粒子（隨機(jī)樣本）通過(guò)合適的概率密度來(lái)表示，粒子的位置和攜帶的相關(guān)權(quán)值可以通過(guò)測(cè)量的方式進(jìn)行調(diào)節(jié)，這些粒子滿足導(dǎo)航系統(tǒng)的后驗(yàn)概率且攜帶相關(guān)的權(quán)值。在粒子數(shù)目達(dá)到一定的數(shù)量后就可以用來(lái)表示后驗(yàn)概率密度，與此同時(shí)就可以準(zhǔn)確的解算出系統(tǒng)狀態(tài)的估計(jì)值。初始化 計(jì)算粒子 重采樣 狀態(tài)更新2 2生成隨機(jī)數(shù)u，u~U[0,1]；從馬爾可夫鏈中進(jìn)行采樣；如果u小于閾值，則采用馬爾可夫鏈蒙特卡羅（MCMC）移動(dòng)。多傳感器融合技術(shù)BUKF算法通過(guò)UT變換近似狀態(tài)后驗(yàn)分布和協(xié)方差，對(duì)非線性函數(shù)可近似到泰勒展開(kāi)的二階項(xiàng)，但當(dāng)系統(tǒng)維數(shù)大于4時(shí)，會(huì)產(chǎn)生負(fù)的權(quán)值點(diǎn)從而引起協(xié)方差矩陣非正定，影響濾波的穩(wěn)定性。

DPF算法是基于蒙特卡洛方法和遞推貝葉斯估計(jì)的統(tǒng)計(jì)濾波方法，理論上適用于含非高斯噪聲的

非線性系統(tǒng)濾波估計(jì)，但在實(shí)際應(yīng)用中計(jì)算量大，且存在粒子退化和貧化的現(xiàn)象。

C CKF算法通過(guò)三階球徑法則選取容積點(diǎn)，通過(guò)將傳遞后的容積點(diǎn)加權(quán)來(lái)近似狀態(tài)后驗(yàn)分布和協(xié)方差。相較于UKF，CKF的數(shù)學(xué)理論推導(dǎo)更為嚴(yán)密。

A EKF算法計(jì)算效率高、應(yīng)用范圍廣，但對(duì)非線性較強(qiáng)的系統(tǒng)估計(jì)精度不高、穩(wěn)定性較差。多傳感器融合技術(shù)010203使用一組無(wú)跡卡爾曼濾波器來(lái)細(xì)化粒子濾波器的預(yù)測(cè)，利用輔助模型生成粒子濾波器中的重要度分布，有效地利用了最新的觀測(cè)信息，提高了精度。以降低計(jì)算量為目標(biāo)，提出了球形單形UT

(SSUT)算法，以減少采樣點(diǎn)的數(shù)量，在保證精度的同時(shí)提高了計(jì)算效率。提出了球面單純形算法，利用SSUT的球面單純形樣本點(diǎn)代替UT的對(duì)稱sigma點(diǎn)，減少計(jì)算量的同時(shí)提高了精度。多傳感器融合技術(shù)010203依據(jù)當(dāng)前時(shí)刻系統(tǒng)可觀度大小對(duì)誤差協(xié)方差陣進(jìn)行修正，減小了系統(tǒng)可觀度較低區(qū)段的估計(jì)誤差，使濾波更加平穩(wěn)，提高了濾波精度。將CKF的協(xié)方差矩陣更新嵌入信息濾波更新框架中，避免矩陣分解出現(xiàn)非正定的情況，進(jìn)一步提高了導(dǎo)航的精度。基于SQP優(yōu)化的約束CKF，在CKF的更新過(guò)程中利用

SQP

非線性規(guī)劃算法，優(yōu)化測(cè)量誤差和量測(cè)量；最后根據(jù)系統(tǒng)量測(cè)模型和

SQP

優(yōu)化后的量測(cè)值，進(jìn)行量測(cè)更新，進(jìn)一步減小了系統(tǒng)隨機(jī)誤差，提高了深空探測(cè)自主天文導(dǎo)航的精度。多傳感器融合技術(shù)010203在粒子濾波框架中，用

EKF

對(duì)每個(gè)粒子進(jìn)行更新，得到近似后驗(yàn)密度作為重要性密度函數(shù)，提高了導(dǎo)航精度。WOPF

的思想基于提升粒子濾波迭代過(guò)程中小權(quán)值粒子的權(quán)值。小權(quán)值粒子的權(quán)值增大之后，就能有更大的可能性不被拋棄，延緩了粒子退化的進(jìn)程。從改進(jìn)重要性密度函數(shù)和改進(jìn)重采樣方法兩方面對(duì)粒子濾波的性能進(jìn)行了提升，具有更高的精度和穩(wěn)定性。多傳感器融合技術(shù)多傳感器觀測(cè)方案tspspsppt

=arccostpppp||

r

|| ||

r

||||

r||

r

||x(x1

x)

y(y1

y)

z(z1

z)h

(x

)

arccos

arccos|| ||

r|| (x

x)2

(y

y)2

(z

z)21 1 1(xp

x)(x2

x)

(yp

y)(y2

y)

(zp

z)(z2

z)

rps

.s1

r

.sctppcp2 2 2cp

hv(x)

r

r.r

r.v

t

hp

(x

)

arccos||

r || (x

x)2

(y

y)2

(z

z)2

h

(x

)

arctan

arctan||

r ||(x

x)2

(y

y)2

(z

z)2p p p

多傳感器融合技術(shù)基于容積卡爾曼和信息濾波的多傳感器融合方法（MSRCIF）將CKF的協(xié)方差矩陣的平方根的更新融合到信息濾波的框架中，通過(guò)信息貢獻(xiàn)向量和信息貢獻(xiàn)矩陣構(gòu)建信息融合中心，實(shí)現(xiàn)多傳感器加權(quán)融合，具體方法可分為預(yù)測(cè)、測(cè)量更新和狀態(tài)更新三個(gè)部分，具體如下。假計(jì)算容積點(diǎn)設(shè)k時(shí)刻的狀態(tài)和協(xié)方差矩陣分別x?k

|為kPk|kTk|k k|k

k|kP

S

S(

j

)k|k k|k

j k|kx

S

x?j

1,

2......2njj

n[1] j

1,

2,......n

n[1]

j j

n

1,

n

2,......2n傳播容積點(diǎn)并計(jì)算狀態(tài)的一步預(yù)測(cè)k|k

k|kx(j

)*k

1|k

f(x(j),

u )12n(j

)*x2n

j

1x?k

+1|k

k

1|k計(jì)算一步預(yù)測(cè)協(xié)方差矩陣的平方根S

Tria([

* S

])k

1|k k

1 Q1[x(1)* ,

x(2)*2n

*k

1k

1|k k

1|k k

1|k k

1|k k

1 k

1|k

x?

x? ,...,

x(

2

n

)*

x? ]SQ

chol(Q)計(jì)算信息撞他和Fisher信息)T

1

1k

1|k k

1|kk

1|kk

1|kY

P

(S

S1)2nx2n

ST

ST

1

(

j

)*k

1|k k

1|k

k

1|k k

1|k

k

1|k k

1|k k

1|kk

1|kj

1y?

P

1

x?

(S )

1

x?

(S

k

1|k多傳感器融合技術(shù)基于容積卡爾曼和信息濾波的多傳感器融合方法（MSRCIF）計(jì)算容積點(diǎn)傳播容積點(diǎn)并計(jì)算測(cè)量的一步預(yù)測(cè)計(jì)算互協(xié)方差矩陣、信息貢獻(xiàn)向量和矩陣更新信息狀態(tài)和Fisher信息x(j

)k

1|k k

1|k

j k

1|k

S

x?(j

)z(j)k

1|kk

1|k

h(x2nz?zk

1|k(j)k

1|k2n

j

1

1

12n(j

)(j

)T T) Pxzk

1|kk

1|k k

+1|kk

1|k k

1|kk

1

k

12n

j

1

(x

x?)(z

z?)

SST PxzS

TI

,k

1|kk

1|k

k

1|k

k

1|k

RY

,k

1|k

Y

,k

1|k

k

1|k

R

S

T

S

1 PxzS

T

SSTk

1I

,k

1|k I

,k

1|kI

SSTx?S

1vk

1I

,k

1|k R k

1I

,k

1|k I,k

1|k

k

1|ki

S