[發(fā)明專利]導(dǎo)航系統(tǒng)以及確定導(dǎo)航信息的方法

有效

• 申請?zhí)枺?/td>: 201910281625.2
• 申請日: 2019-04-09
• 公開（公告）號: CN109974709B
• 公開（公告）日: 2021-10-15
• 發(fā)明（設(shè)計(jì)）人: 楊君;吳尉
• 申請（專利權(quán)）人: 天津時空經(jīng)緯信息技術(shù)有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 北京萬思博知識產(chǎn)權(quán)代理有限公司 11694
• 代理人: 劉冀

1.一種確定導(dǎo)航信息的方法，用于重新啟動的船舶載體上的慣性導(dǎo)航設(shè)備，其特征在于，包括：

第一慣性導(dǎo)航設(shè)備從第二慣性導(dǎo)航設(shè)備接收第一導(dǎo)航信息，并利用所述第一導(dǎo)航信息針對預(yù)先設(shè)置的慣性導(dǎo)航計(jì)算模型進(jìn)行速度、位置和姿態(tài)的初始化，其中所述第一慣性導(dǎo)航設(shè)備與所述第二慣性導(dǎo)航設(shè)備為對等的慣性導(dǎo)航設(shè)備，位于相同的船舶載體的不同位置，并且所述第一慣性導(dǎo)航設(shè)備為重新啟動的慣性導(dǎo)航設(shè)備；

第一慣性導(dǎo)航設(shè)備從第二慣性導(dǎo)航設(shè)備接收第二導(dǎo)航信息；以及

所述第一慣性導(dǎo)航設(shè)備基于所述第二導(dǎo)航信息以及所述第一慣性導(dǎo)航設(shè)備測量的第三導(dǎo)航信息，利用所述慣性導(dǎo)航計(jì)算模型以及預(yù)先設(shè)置的遞歸運(yùn)算模型，確定第四導(dǎo)航信息作為所述第一慣性導(dǎo)航設(shè)備輸出的導(dǎo)航信息，其中

所述慣性導(dǎo)航計(jì)算模型為基于所述第一慣性導(dǎo)航設(shè)備測量的導(dǎo)航參數(shù)的慣性導(dǎo)航計(jì)算模型，并且其中

所述第一導(dǎo)航信息和所述第二導(dǎo)航信息包括所述第二慣性導(dǎo)航設(shè)備測量的姿態(tài)、速度以及位置信息；

所述第三導(dǎo)航信息包括所述第一慣性導(dǎo)航設(shè)備測量的姿態(tài)、速度以及位置信息；

所述第四導(dǎo)航信息包括所述第一慣性導(dǎo)航設(shè)備的載體的速度以及位置信息；

所述慣性導(dǎo)航計(jì)算模型是基于第一慣性導(dǎo)航設(shè)備，利用捷聯(lián)慣性導(dǎo)航算法獲得的線性模型；并且

所述遞歸運(yùn)算模型為卡爾曼濾波器，并且其中

所述卡爾曼濾波器是基于以下方程的卡爾曼濾波器：

狀態(tài)方程：以及

量測方程：

其中，X為狀態(tài)變量，并且

式中，φ_E為第一慣性導(dǎo)航設(shè)備的橫搖角誤差、φ_N為第一慣性導(dǎo)航設(shè)備的縱搖角誤差、φ_U為第一慣性導(dǎo)航設(shè)備的航向角誤差、δv_E為第一慣性導(dǎo)航設(shè)備的東向速度誤差、δv_N為第一慣性導(dǎo)航設(shè)備的北向速度誤差、δv_U為第一慣性導(dǎo)航設(shè)備的垂向速度誤差、δL為第一慣性導(dǎo)航設(shè)備的緯度誤差、δλ為第一慣性導(dǎo)航設(shè)備的經(jīng)度誤差、δh為第一慣性導(dǎo)航設(shè)備的高度誤差、為第一慣性導(dǎo)航設(shè)備的X陀螺零偏、為第一慣性導(dǎo)航設(shè)備的Y陀螺零偏、為第一慣性導(dǎo)航設(shè)備的Z陀螺零偏、為第一慣性導(dǎo)航設(shè)備的X加速度計(jì)零偏、為第一慣性導(dǎo)航設(shè)備的Y加速度計(jì)零偏以及為第一慣性導(dǎo)航設(shè)備的Z加速度計(jì)零偏，并且

其中，

式中，L為第二慣性導(dǎo)航設(shè)備緯度、λ為第二慣性導(dǎo)航設(shè)備經(jīng)度、h為第二慣性導(dǎo)航設(shè)備高度、v_E為第二慣性導(dǎo)航設(shè)備東速、v_N為第二慣性導(dǎo)航設(shè)備北速、v_U為第二慣性導(dǎo)航設(shè)備垂速、f_E為第一慣性導(dǎo)航設(shè)備東向加速度、f_N為第一慣性導(dǎo)航設(shè)備北向加速度、f_U為第一慣性導(dǎo)航設(shè)備垂向加速度、ω_ie為地球自轉(zhuǎn)角速度、R_E為地球子午圈半徑以及R_N為地球卯酉圈半徑、為第二慣性導(dǎo)航設(shè)備的X陀螺零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的Y陀螺零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的Z陀螺零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的X加速度計(jì)零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的Y加速度計(jì)零偏過程白噪聲以及為第二慣性導(dǎo)航設(shè)備的Z加速度計(jì)零偏過程白噪聲，并且

L_s、λ_s、h_s、v_Es、v_Ns和v_Us分別表示第一慣性導(dǎo)航設(shè)備系統(tǒng)解算輸出的緯度、經(jīng)度、高度、東速、北速和垂速，L_r、λ_r、h_r、v_Er、v_Nr和v_Ur表示第二慣性導(dǎo)航設(shè)備系統(tǒng)解算輸出的緯度、經(jīng)度、高度、東速、北速和垂速，以及

H＝[0_6×3 I_6×6 0_6×6]，V為6維向量，表示緯度的觀測白噪聲、經(jīng)度的觀測白噪聲、高度的觀測白噪聲、東速的觀測白噪聲、北速的觀測白噪聲和垂速的觀測白噪聲，并且其中

方法還包括：第一慣性導(dǎo)航設(shè)備針對所述第一慣性導(dǎo)航設(shè)備與所述第二慣性導(dǎo)航設(shè)備之間桿臂補(bǔ)償，并且其中

針對所述第一慣性導(dǎo)航設(shè)備與所述第二慣性導(dǎo)航設(shè)備進(jìn)行桿臂補(bǔ)償?shù)牟僮鳎ò凑找韵鹿竭M(jìn)行桿臂補(bǔ)償：

以及

式中，L_b、λ_b、h_b、v_Eb、v_Nb和v_Ub分別表示第一慣性導(dǎo)航設(shè)備載體坐標(biāo)系下的緯度、經(jīng)度、高度、東速、北速和垂速，L_B、λ_B、h_B、v_EB、v_NB和v_UB分別表示第二慣性導(dǎo)航設(shè)備載體坐標(biāo)系下的緯度、經(jīng)度、高度、東速、北速和垂速，以及

以及為第二慣性導(dǎo)航設(shè)備的姿態(tài)矩陣，計(jì)算如下

其中R、P和H為第二慣性導(dǎo)航設(shè)備的橫搖角、縱搖角和航向角，以及

為第二慣性導(dǎo)航設(shè)備在地理坐標(biāo)系上的三個姿態(tài)角速度；

r_x、r_y、r_z為沿橫軸、縱軸和方位軸方向的桿臂，并且

方法還包括：確定所述第二慣性導(dǎo)航設(shè)備與所述第一慣性導(dǎo)航設(shè)備之間的延時；以及根據(jù)所確定的延時，對所述第一導(dǎo)航信息進(jìn)行補(bǔ)償，并且其中

根據(jù)所確定的延時，對所述第一導(dǎo)航信息進(jìn)行補(bǔ)償?shù)牟僮鳎ǎ?/p>

若Td＝△T

info_pps＝info[k]*(△T-Td)/T+info[k+1]*(T-(△T-Td))/T

若Td△T

設(shè)△T1＝Td-△T，n＝1，

若△T1T，則進(jìn)行以下循環(huán)計(jì)算：△T1＝△T1-T，n++，直至△T1T為止，此時info_pps＝info[k+n]*(T-△T1)/T+info[k+n+1]*△T1/T，其中

Td為第二慣性導(dǎo)航設(shè)備的激光陀螺采集數(shù)據(jù)進(jìn)行濾波預(yù)處理的濾波延時，并且根據(jù)以下公式計(jì)算：Td＝濾波階數(shù)/(2*采樣頻率)；

info_pps是所述第二慣性導(dǎo)航設(shè)備在k+1中斷時刻計(jì)算完后輸出至被牽引慣導(dǎo)的導(dǎo)航信息；

info[k]為所述第二慣性導(dǎo)航設(shè)備在第k個內(nèi)部時鐘所測量的導(dǎo)航信息，info[k+1]為所述第二慣性導(dǎo)航設(shè)備在第k+1個內(nèi)部時鐘所測量的導(dǎo)航信息；

△T是第一慣性導(dǎo)航設(shè)備的第n個內(nèi)部時鐘與第二慣性導(dǎo)航設(shè)備第k+1個內(nèi)部時鐘之間的延時；以及

T是第一慣性導(dǎo)航設(shè)備與第二慣性導(dǎo)航設(shè)備的內(nèi)部時鐘的周期。

2.一種用于船舶載體的導(dǎo)航系統(tǒng)，包括通信連接的第一慣性導(dǎo)航設(shè)備和第二慣性導(dǎo)航設(shè)備，其中所述第一慣性導(dǎo)航設(shè)備與所述第二慣性導(dǎo)航設(shè)備為對等的慣性導(dǎo)航設(shè)備，位于所述船舶載體的不同位置，并且所述第一慣性導(dǎo)航設(shè)備為重新啟動的慣性導(dǎo)航設(shè)備，其特征在于，所述第一慣性導(dǎo)航設(shè)備被配置為執(zhí)行以下操作：

所述第一慣性導(dǎo)航設(shè)備從所述第二慣性導(dǎo)航設(shè)備接收第一導(dǎo)航信息，并利用所述第一導(dǎo)航信息針對預(yù)先設(shè)置的慣性導(dǎo)航計(jì)算模型進(jìn)行速度、位置和姿態(tài)的初始化；

第一慣性導(dǎo)航設(shè)備從第二慣性導(dǎo)航設(shè)備接收第二導(dǎo)航信息；

以及

所述第一慣性導(dǎo)航設(shè)備基于所述第二導(dǎo)航信息以及所述第一慣性導(dǎo)航設(shè)備測量的第三導(dǎo)航信息，利用預(yù)先設(shè)置的慣性導(dǎo)航計(jì)算模型以及遞歸運(yùn)算模型，確定第四導(dǎo)航信息作為所述第一慣性導(dǎo)航設(shè)備輸出的導(dǎo)航信息，其中

所述慣性導(dǎo)航計(jì)算模型為基于所述第一慣性導(dǎo)航設(shè)備測量的導(dǎo)航參數(shù)的慣性導(dǎo)航計(jì)算模型；并且

所述遞歸運(yùn)算模型為用于修正所述第一慣性導(dǎo)航設(shè)備的遞歸運(yùn)算模型，并且其中

所述第一慣性導(dǎo)航設(shè)備和所述第二慣性導(dǎo)航設(shè)備能夠互換，并且其中

所述第一導(dǎo)航信息和所述第二導(dǎo)航信息包括所述第二慣性導(dǎo)航設(shè)備測量的姿態(tài)、速度以及位置信息；

所述第三導(dǎo)航信息包括所述第一慣性導(dǎo)航設(shè)備測量的姿態(tài)、速度以及位置信息；

所述第四導(dǎo)航信息包括所述第一慣性導(dǎo)航設(shè)備的載體的速度以及位置信息；

所述慣性導(dǎo)航計(jì)算模型是基于第一慣性導(dǎo)航設(shè)備，利用捷聯(lián)慣性導(dǎo)航算法獲得的線性模型；并且

所述遞歸運(yùn)算模型為卡爾曼濾波器，并且其中

所述卡爾曼濾波器是基于以下方程的卡爾曼濾波器：

狀態(tài)方程：以及

量測方程：

其中，X為狀態(tài)變量，并且

式中，φ_E為第一慣性導(dǎo)航設(shè)備的橫搖角誤差、φ_N為第一慣性導(dǎo)航設(shè)備的縱搖角誤差、φ_U為第一慣性導(dǎo)航設(shè)備的航向角誤差、δv_E為第一慣性導(dǎo)航設(shè)備的東向速度誤差、δv_N為第一慣性導(dǎo)航設(shè)備的北向速度誤差、δv_U為第一慣性導(dǎo)航設(shè)備的垂向速度誤差、δL為第一慣性導(dǎo)航設(shè)備的緯度誤差、δλ為第一慣性導(dǎo)航設(shè)備的經(jīng)度誤差、δh為第一慣性導(dǎo)航設(shè)備的高度誤差、為第一慣性導(dǎo)航設(shè)備的X陀螺零偏、為第一慣性導(dǎo)航設(shè)備的Y陀螺零偏、為第一慣性導(dǎo)航設(shè)備的Z陀螺零偏、為第一慣性導(dǎo)航設(shè)備的X加速度計(jì)零偏、為第一慣性導(dǎo)航設(shè)備的Y加速度計(jì)零偏以及為第一慣性導(dǎo)航設(shè)備的Z加速度計(jì)零偏，并且

其中，

式中，L為第二慣性導(dǎo)航設(shè)備緯度、λ為第二慣性導(dǎo)航設(shè)備經(jīng)度、h為第二慣性導(dǎo)航設(shè)備高度、v_E為第二慣性導(dǎo)航設(shè)備東速、v_N為第二慣性導(dǎo)航設(shè)備北速、v_U為第二慣性導(dǎo)航設(shè)備垂速、f_E為第一慣性導(dǎo)航設(shè)備東向加速度、f_N為第一慣性導(dǎo)航設(shè)備北向加速度、f_U為第一慣性導(dǎo)航設(shè)備垂向加速度、ω_ie為地球自轉(zhuǎn)角速度、R_E為地球子午圈半徑以及R_N為地球卯酉圈半徑、為第二慣性導(dǎo)航設(shè)備的X陀螺零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的Y陀螺零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的Z陀螺零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的X加速度計(jì)零偏過程白噪聲、為第二慣性導(dǎo)航設(shè)備的Y加速度計(jì)零偏過程白噪聲以及為第二慣性導(dǎo)航設(shè)備的Z加速度計(jì)零偏過程白噪聲，并且

L_s、λ_s、h_s、v_Es、v_Ns和v_Us分別表示第一慣性導(dǎo)航設(shè)備系統(tǒng)解算輸出的緯度、經(jīng)度、高度、東速、北速和垂速，L_r、λ_r、h_r、v_Er、v_Nr和v_Ur表示第二慣性導(dǎo)航設(shè)備系統(tǒng)解算輸出的緯度、經(jīng)度、高度、東速、北速和垂速，以及

H＝[0_6×3 I_6×6 0_6×6]，V為6維向量，表示緯度的觀測白噪聲、經(jīng)度的觀測白噪聲、高度的觀測白噪聲、東速的觀測白噪聲、北速的觀測白噪聲和垂速的觀測白噪聲，并且其中

并且，所述第一慣性導(dǎo)航設(shè)備被配置為執(zhí)行以下操作：第一慣性導(dǎo)航設(shè)備針對所述第一慣性導(dǎo)航設(shè)備與所述第二慣性導(dǎo)航設(shè)備之間桿臂補(bǔ)償，并且其中

針對所述第一慣性導(dǎo)航設(shè)備與所述第二慣性導(dǎo)航設(shè)備進(jìn)行桿臂補(bǔ)償?shù)牟僮鳎ò凑找韵鹿竭M(jìn)行桿臂補(bǔ)償：

以及

式中，L_b、λ_b、h_b、v_Eb、v_Nb和v_Ub分別表示第一慣性導(dǎo)航設(shè)備載體坐標(biāo)系下的緯度、經(jīng)度、高度、東速、北速和垂速，L_B、λ_B、h_B、v_EB、v_NB和v_UB分別表示第二慣性導(dǎo)航設(shè)備載體坐標(biāo)系下的緯度、經(jīng)度、高度、東速、北速和垂速，以及

以及為第二慣性導(dǎo)航設(shè)備的姿態(tài)矩陣，計(jì)算如下

其中R、P和H為第二慣性導(dǎo)航設(shè)備的橫搖角、縱搖角和航向角，以及

為第二慣性導(dǎo)航設(shè)備在地理坐標(biāo)系上的三個姿態(tài)角速度；

r_x、r_y、r_z為沿橫軸、縱軸和方位軸方向的桿臂，并且

第一慣性導(dǎo)航設(shè)備還被配置為進(jìn)一步執(zhí)行以下操作：確定所述第二慣性導(dǎo)航設(shè)備與所述第一慣性導(dǎo)航設(shè)備之間的延時；以及根據(jù)所確定的延時，對所述第一導(dǎo)航信息進(jìn)行補(bǔ)償，并且其中

根據(jù)所確定的延時，對所述第一導(dǎo)航信息進(jìn)行補(bǔ)償?shù)牟僮鳎ǎ?/p>

若Td＝△T

info_pps＝info[k]*(△T-Td)/T+info[k+1]*(T-(△T-Td))/T

若Td△T

設(shè)△T1＝Td-△T，n＝1，

若△T1T，則進(jìn)行以下循環(huán)計(jì)算：△T1＝△T1-T，n++，直至△T1T為止，此時info_pps＝info[k+n]*(T-△T1)/T+info[k+n+1]*△T1/T，其中

Td為第二慣性導(dǎo)航設(shè)備的激光陀螺采集數(shù)據(jù)進(jìn)行濾波預(yù)處理的濾波延時，并且根據(jù)以下公式計(jì)算：Td＝濾波階數(shù)/(2*采樣頻率)；

info_pps是所述第二慣性導(dǎo)航設(shè)備在k+1中斷時刻計(jì)算完后輸出至被牽引慣導(dǎo)的導(dǎo)航信息；

info[k]為所述第二慣性導(dǎo)航設(shè)備在第k個內(nèi)部時鐘所測量的導(dǎo)航信息，info[k+1]為所述第二慣性導(dǎo)航設(shè)備在第k+1個內(nèi)部時鐘所測量的導(dǎo)航信息；

△T是第一慣性導(dǎo)航設(shè)備的第n個內(nèi)部時鐘與第二慣性導(dǎo)航設(shè)備第k+1個內(nèi)部時鐘之間的延時；以及

T是第一慣性導(dǎo)航設(shè)備與第二慣性導(dǎo)航設(shè)備的內(nèi)部時鐘的周期。