[發(fā)明專利]一種基于環(huán)路和積數(shù)據(jù)關(guān)聯(lián)算法的協(xié)同導(dǎo)航方法

在審

• 申請?zhí)枺?/td>: 202010792458.0
• 申請日: 2020-08-09
• 公開（公告）號: CN112068173A
• 公開（公告）日: 2020-12-11
• 發(fā)明（設(shè)計）人: 陳紅梅;常林江;徐赟;王慧娟;張會娟
• 申請（專利權(quán)）人: 河南工業(yè)大學(xué)
• 主分類號: G01S19/46
• 分類號: G01S19/46;G01S19/47
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于環(huán)路和積數(shù)據(jù)關(guān)聯(lián)算法的協(xié)同導(dǎo)航，其特征在于：協(xié)同導(dǎo)航系統(tǒng)內(nèi)有多個平臺，平臺自身用衛(wèi)星(GPS)和慣導(dǎo)(INS)進(jìn)行絕對導(dǎo)航，平臺間用GPS和超寬帶(UWB)進(jìn)行相對導(dǎo)航，且平臺間可相互通信，它包括以下步驟：

步驟一：平臺自身采用慣導(dǎo)，衛(wèi)星等傳感器進(jìn)行絕對導(dǎo)航，獲取平臺的位置、速度、姿態(tài)等信息；

步驟二：平臺間采用衛(wèi)星差分和超寬帶的測距測速，借助絕對導(dǎo)航輸出結(jié)果進(jìn)行相對導(dǎo)航，獲取平臺間的相對位置、速度信息；

步驟三：平臺間互相傳遞信息，構(gòu)建基于環(huán)路和積數(shù)據(jù)關(guān)聯(lián)算法的協(xié)同導(dǎo)航濾波器，估計出所有平臺的位置、速度信息。

2.根據(jù)權(quán)力要求1所述的基于環(huán)路和積數(shù)據(jù)關(guān)聯(lián)算法的協(xié)同導(dǎo)航，其特征在于：步驟一所述的絕對導(dǎo)航，為INS和GPS緊組合導(dǎo)航，主要根據(jù)GPS接收機(jī)計算出的衛(wèi)星星歷數(shù)據(jù)和慣導(dǎo)系統(tǒng)計算出的速度位置，解算出對應(yīng)于慣性導(dǎo)航系統(tǒng)的偽距和偽距率，再與GPS接收機(jī)測量到的偽距偽距率相比較，估計出慣性導(dǎo)航系統(tǒng)和GPS接收機(jī)的誤差量，然后將估出的參數(shù)反饋至INS內(nèi)部，得到平臺自身的運動狀態(tài)，以平臺i為例進(jìn)行說明：

絕對導(dǎo)航采用坐標(biāo)系為東北天地理坐標(biāo)系，其狀態(tài)包括9維的INS誤差，6維的INS元件誤差，2維的GPS誤差，狀態(tài)方程可寫作：

其中：

代表k時刻編號為i的平臺狀態(tài)，為東北天三個方向的平臺誤差角，δv_E,δv_N,δv_U和δL,δλ,δh為三個方向的速度和位置誤差，ε_x,ε_y,ε_z和Δ_x,Δ_y,Δ_z陀螺儀和加速度計在三個方向的偏置誤差，δt_u為時鐘誤差等效的測距誤差，δt_ru為時鐘頻率誤差等效的距離變化率,F為狀態(tài)轉(zhuǎn)移矩陣，G為噪聲系數(shù)矩陣，W為噪聲向量，上角標(biāo)i為平臺編號，下角標(biāo)k代表時間；

設(shè)平臺在地球大地坐標(biāo)系下慣導(dǎo)輸出位置為經(jīng)度λ_I，緯度L_I，高度h_I，則平臺的真實位置的λ,L,h可表示成如下形式：

量測為GPS接收機(jī)測得的四顆衛(wèi)星的偽距和偽距率，量測方程可寫作：

其中ρ^j為偽距測量，具體展開如下：

式中，為衛(wèi)星j在地心地球固連(ECEF)坐標(biāo)系下各方向位置，下角標(biāo)s代表衛(wèi)星，上角標(biāo)j為衛(wèi)星編號，x,y,z為平臺自身在ECEF坐標(biāo)系下各方向位置，v_ρ為偽距測量噪聲，ECEF坐標(biāo)系與東北天地理坐標(biāo)系轉(zhuǎn)換關(guān)系如下：

式中，R_N為地球半徑，f為地球的橢圓扁率；

為偽距率測量，具體展開如下：

式中，為平臺自身在ECEF坐標(biāo)系下各方向速度，為衛(wèi)星在ECEF坐標(biāo)系下各方向速度，其與v_E,v_N,v_U的轉(zhuǎn)換關(guān)系如下：