本發(fā)明涉及一種導(dǎo)航方法，尤其涉及一種海上運(yùn)載體的組合導(dǎo)航方法及裝置，屬于導(dǎo)航定位技術(shù)領(lǐng)域。

背景技術(shù)：

慣性導(dǎo)航是海上運(yùn)載體最主要的導(dǎo)航方式之一，具有不依賴外界信息、隱蔽性好、抗輻射性強(qiáng)、全天候等優(yōu)點(diǎn)，是完全自主的導(dǎo)航。由于陀螺儀和加速度計(jì)的誤差漂移，慣導(dǎo)系統(tǒng)必須借助于其他導(dǎo)航方式的輔助才能長(zhǎng)時(shí)間使用。通?？梢圆捎枚嗥绽諟y(cè)速儀(DVL，Doppler Velocity Log)輔助慣導(dǎo)的工作方式，該方式具有自主性高、設(shè)備簡(jiǎn)單的特點(diǎn)，通過(guò)DVL提供的速度信息，可以校正慣導(dǎo)的速度誤差、橫滾角誤差和俯仰角誤差，然而，隨時(shí)間累積的定位誤差和航向角誤差是無(wú)法校正的。

為了提高慣導(dǎo)系統(tǒng)的精度，研究者提出了利用GPS導(dǎo)航系統(tǒng)、重力輔助導(dǎo)航系統(tǒng)、天文導(dǎo)航系統(tǒng)等與慣導(dǎo)系統(tǒng)相結(jié)合的多種組合導(dǎo)航方法。

文獻(xiàn)[潘學(xué)松.基于SINS/DVL/GPS的AUV組合導(dǎo)航系統(tǒng)關(guān)鍵技術(shù)研究[D].青島：中國(guó)海洋大學(xué)，2011.]給出了SINS/DVL/GPS組合導(dǎo)航方式，該方式利用多普勒測(cè)速儀提供的速度信息，校正慣導(dǎo)的速度和姿態(tài)誤差，但無(wú)法對(duì)航向進(jìn)行持續(xù)有效的校正；利用GPS進(jìn)行位置修正，但其可靠性和抗干擾性較差；利用基于遺傳算法的神經(jīng)網(wǎng)絡(luò)濾波來(lái)進(jìn)行數(shù)據(jù)融合，提高了濾波精度和可靠性，但增加了算法的復(fù)雜性且難以滿足實(shí)時(shí)性的要求。

文獻(xiàn)[彭富清.海洋重力輔助導(dǎo)航方法及應(yīng)用[D].鄭州：解放軍信息工程大學(xué)，2009.]給出了海洋重力輔助慣性導(dǎo)航的方法，該方法利用重力儀的測(cè)量值與重力圖進(jìn)行匹配來(lái)校正慣性導(dǎo)航位置誤差，保證了系統(tǒng)的自主性，但其難以對(duì)航向進(jìn)行有效修正，并且該導(dǎo)航方式對(duì)重力異常圖的制作精度要求較高且僅適用于重力特征明顯區(qū)域，無(wú)法成為一種獨(dú)立應(yīng)用的導(dǎo)航手段。

文獻(xiàn)[孫劍明.基于星圖識(shí)別的艦船天文導(dǎo)航關(guān)鍵技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.]給出了一種恒星敏感器輔助慣性的傳統(tǒng)天文導(dǎo)航方法，該方法利用星圖識(shí)別技術(shù)進(jìn)行定姿定向，校正慣性誤差，但難以提供有效的位置修正，且數(shù)據(jù)更新率較低。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明所要解決的技術(shù)問(wèn)題在于克服現(xiàn)有技術(shù)不足，提供一種海上運(yùn)載體的組合導(dǎo)航方法及裝置，能夠在擺脫對(duì)衛(wèi)星定位技術(shù)的依賴的同時(shí)，有效提高導(dǎo)航精度。

本發(fā)明具體采用以下技術(shù)方案解決上述技術(shù)問(wèn)題：

一種海上運(yùn)載體的組合導(dǎo)航方法，所述海上運(yùn)載體上裝備有慣性導(dǎo)航主系統(tǒng)、水聲定位子系統(tǒng)、偏振光導(dǎo)航子系統(tǒng)；首先利用慣性導(dǎo)航主系統(tǒng)進(jìn)行捷聯(lián)解算，利用水聲定位子系統(tǒng)進(jìn)行測(cè)距計(jì)算，利用偏振光導(dǎo)航子系統(tǒng)進(jìn)行方位計(jì)算；然后建立以慣性導(dǎo)航主系統(tǒng)的誤差為狀態(tài)量、水聲定位子系統(tǒng)的測(cè)距信息和偏振光導(dǎo)航子系統(tǒng)的航向信息為觀測(cè)量的擴(kuò)展卡爾曼濾波模型，對(duì)慣性導(dǎo)航主系統(tǒng)的誤差進(jìn)行閉環(huán)修正。

優(yōu)選地，所述水聲定位子系統(tǒng)為長(zhǎng)基線水聲定位子系統(tǒng)或者偽長(zhǎng)基線水聲定位子系統(tǒng)。

優(yōu)選地，作為擴(kuò)展卡爾曼濾波模型的狀態(tài)量的慣性導(dǎo)航子系統(tǒng)的誤差為15階誤差狀態(tài)，如下式所示：

其中，φ_E、φ_N、φ_U分別為東向、北向和天向的數(shù)學(xué)平臺(tái)失準(zhǔn)角，δv_E、δv_N、δv_U分別為東向、北向和天向的速度誤差，δλ、δL、δh分別為經(jīng)度、緯度和高度誤差，ε_x、ε_y、ε_z分別三軸陀螺的誤差，▽_x、▽_y、▽_z分別三軸加速度計(jì)的誤差。

優(yōu)選地，所述擴(kuò)展卡爾曼濾波模型的觀測(cè)方程如下：

其中，(X,Y,Z)為利用慣性導(dǎo)航主系統(tǒng)定位得到的海上運(yùn)載體的坐標(biāo)；(X_ri,Y_ri,Z_ri)為三個(gè)位置已知的固定基站的坐標(biāo)；為海上運(yùn)載體中心與聲納接收機(jī)之間的桿臂效應(yīng)；表示載體系與導(dǎo)航系之間的轉(zhuǎn)換矩陣；為利用慣性導(dǎo)航主系統(tǒng)所估計(jì)的位置信息計(jì)算出的水聲定位子系統(tǒng)中的接收機(jī)與所述三個(gè)位置已知的固定基站之間的距離；R_1m、R_2m、R_3m為利用水聲定位子系統(tǒng)測(cè)量得到的水聲定位子系統(tǒng)中的接收機(jī)與所述三個(gè)固定基站之間的距離；為利用慣性導(dǎo)航主系統(tǒng)解算得到的海上運(yùn)載體的航向角；γ_m為利用偏振光導(dǎo)航子系統(tǒng)計(jì)算得到的海上運(yùn)載體的航向角。

根據(jù)相同的發(fā)明思路還可以得到以下技術(shù)方案：

一種海上運(yùn)載體的組合導(dǎo)航裝置，所述組合導(dǎo)航裝置包括慣性導(dǎo)航主系統(tǒng)、水聲定位子系統(tǒng)、偏振光導(dǎo)航子系統(tǒng)，以及數(shù)據(jù)融合單元，所述數(shù)據(jù)融合單元用于根據(jù)慣性導(dǎo)航主系統(tǒng)的捷聯(lián)解算結(jié)果、水聲定位子系統(tǒng)的測(cè)距計(jì)算結(jié)果、偏振光導(dǎo)航子系統(tǒng)的方位計(jì)算結(jié)果，建立以慣性導(dǎo)航主系統(tǒng)的誤差為狀態(tài)量、水聲定位子系統(tǒng)的測(cè)距信息和偏振光導(dǎo)航子系統(tǒng)的航向信息為觀測(cè)量的擴(kuò)展卡爾曼濾波模型，對(duì)慣性導(dǎo)航主系統(tǒng)的誤差進(jìn)行閉環(huán)修正。

優(yōu)選地，所述水聲定位子系統(tǒng)為長(zhǎng)基線水聲定位子系統(tǒng)或者偽長(zhǎng)基線水聲定位子系統(tǒng)。

優(yōu)選地，作為擴(kuò)展卡爾曼濾波模型的狀態(tài)量的慣性導(dǎo)航子系統(tǒng)的誤差為15階誤差狀態(tài)，如下式所示：

其中，φ_E、φ_N、φ_U分別為東向、北向和天向的數(shù)學(xué)平臺(tái)失準(zhǔn)角，δv_E、δv_N、δv_U分別為東向、北向和天向的速度誤差，δλ、δL、δh分別為經(jīng)度、緯度和高度誤差，ε_x、ε_y、ε_z分別三軸陀螺的誤差，▽_x、▽_y、▽_z分別三軸加速度計(jì)的誤差。

優(yōu)選地，所述擴(kuò)展卡爾曼濾波模型的觀測(cè)方程如下：

其中，(X,Y,Z)為利用慣性導(dǎo)航主系統(tǒng)定位得到的海上運(yùn)載體的坐標(biāo)；(X_ri,Y_ri,Z_ri)為三個(gè)位置已知的固定基站的坐標(biāo)；為海上運(yùn)載體中心與聲納接收機(jī)之間的桿臂效應(yīng)；表示載體系與導(dǎo)航系之間的轉(zhuǎn)換矩陣；為利用慣性導(dǎo)航主系統(tǒng)所估計(jì)的位置信息計(jì)算出的水聲定位子系統(tǒng)中的接收機(jī)與所述三個(gè)位置已知的固定基站之間的距離；R_1m、R_2m、R_3m為利用水聲定位子系統(tǒng)測(cè)量得到的水聲定位子系統(tǒng)中的接收機(jī)與所述三個(gè)固定基站之間的距離；為利用慣性導(dǎo)航主系統(tǒng)解算得到的海上運(yùn)載體的航向角；γ_m為利用偏振光導(dǎo)航子系統(tǒng)計(jì)算得到的海上運(yùn)載體的航向角。

相比現(xiàn)有技術(shù)，本發(fā)明具有以下有益效果：

本發(fā)明針對(duì)海上運(yùn)載體提出了慣性/APS/偏振光組合的自主導(dǎo)航方式，構(gòu)建了以慣導(dǎo)誤差為狀態(tài)量，聲學(xué)距離、偏振航向?yàn)橛^測(cè)量的擴(kuò)展卡爾曼濾波模型，來(lái)對(duì)慣導(dǎo)誤差進(jìn)行閉環(huán)修正，一方面有效提高了導(dǎo)航精度，另一方面由于擺脫了對(duì)衛(wèi)星定位技術(shù)的依賴，因此具有更好的隱蔽性和抗干擾性。

附圖說(shuō)明

圖1為具體實(shí)施方式中水聲定位子系統(tǒng)的測(cè)量原理示意圖；

圖2為天空偏振光分布模型；

圖3為具體實(shí)施方式中偏振光導(dǎo)航子系統(tǒng)的航向角解算過(guò)程示意圖；

圖4為驗(yàn)證試驗(yàn)中的載體航路圖；

圖5為驗(yàn)證試驗(yàn)中的組合導(dǎo)航位置誤差；

圖6為驗(yàn)證試驗(yàn)中的組合導(dǎo)航航向角誤差。

具體實(shí)施方式

針對(duì)現(xiàn)有組合導(dǎo)航技術(shù)的不足，本發(fā)明提出了一種慣性/APS/偏振光組合的導(dǎo)航方法，利用慣性導(dǎo)航主系統(tǒng)進(jìn)行捷聯(lián)解算，利用水聲定位子系統(tǒng)進(jìn)行測(cè)距計(jì)算，利用偏振光導(dǎo)航子系統(tǒng)進(jìn)行方位計(jì)算；然后建立以慣性導(dǎo)航主系統(tǒng)的誤差為狀態(tài)量、水聲定位子系統(tǒng)的測(cè)距信息和偏振光導(dǎo)航子系統(tǒng)的航向信息為觀測(cè)量的擴(kuò)展卡爾曼濾波模型，對(duì)慣性導(dǎo)航主系統(tǒng)的誤差進(jìn)行閉環(huán)修正。本發(fā)明一方面可有效提高導(dǎo)航精度，另一方面由于擺脫了對(duì)衛(wèi)星定位技術(shù)的依賴，因此具有更好的隱蔽性和抗干擾性。

為了便于公眾理解，下面以一個(gè)具體實(shí)施例并結(jié)合附圖來(lái)對(duì)本發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說(shuō)明：

首先利用海上運(yùn)載體所裝備的慣性導(dǎo)航主系統(tǒng)、水聲定位子系統(tǒng)、偏振光導(dǎo)航子系統(tǒng)(為簡(jiǎn)便起見(jiàn)，下文分別簡(jiǎn)稱為慣導(dǎo)、聲納、偏振)分別進(jìn)行導(dǎo)航計(jì)算。

其中的水聲定位子系統(tǒng)可包括現(xiàn)有的各種聲學(xué)基線，如長(zhǎng)基線(LBL，Long Base Line)、超短基線(USBL，Ultra Short Base Line)等，本發(fā)明優(yōu)選采用長(zhǎng)基線水聲定位子系統(tǒng)或者偽長(zhǎng)基線水聲定位子系統(tǒng)。長(zhǎng)基線系統(tǒng)包含兩部分，一部分是安裝在載體上的接收機(jī)，另一部分是位置已知的固定在海底的基站?；局g的距離構(gòu)成基線，長(zhǎng)度在上百米到幾千米之間。長(zhǎng)基線系統(tǒng)是通過(guò)測(cè)量接收機(jī)和基站之間的距離，采用測(cè)量的交會(huì)對(duì)目標(biāo)定位。偽長(zhǎng)基線定位系統(tǒng)即PLBL(Pseudo Long Base Line)，它的聲納基站具備偽隨機(jī)碼信號(hào)(Pseudo Random Code)發(fā)射能力，測(cè)距精度和抗干擾能力進(jìn)一步增強(qiáng)。本實(shí)施例中采用PLBL系統(tǒng)，圖1即顯示了三個(gè)基站的PLBL測(cè)量原理示意圖，三個(gè)基站的位置已知，并能通過(guò)偽隨機(jī)碼發(fā)送給聲納接收機(jī)。

設(shè)三個(gè)基站坐標(biāo)分別為(X_r1,Y_r1,Z_r1)、(X_r2,Y_r2,Z_r2)、(X_r3,Y_r3,Z_r3)，載體中心坐標(biāo)為(X,Y,Z)，載體中心與聲納接收機(jī)之間的桿臂效應(yīng)為設(shè)測(cè)量距離為R₁、R₂和R₃，則有：

式(1)中表示載體系與導(dǎo)航系之間的轉(zhuǎn)換矩陣。

上述距離測(cè)量值利用解析矢量可表示為：

式(2)中

則可以通過(guò)慣導(dǎo)估計(jì)位置和已知基站位置計(jì)算距離從而與聲學(xué)測(cè)量的距離R_i作差構(gòu)成觀測(cè)量。

偏振光導(dǎo)航起源于對(duì)蜜蜂、沙蟻等昆蟲(chóng)捷徑返巢能力的探索，是一種以太陽(yáng)方向矢量為基準(zhǔn)的定向方式，具有抗干擾及誤差不隨時(shí)間累積等優(yōu)點(diǎn)。大氣散射輻射具有偏振特性，蜜蜂、沙蟻等昆蟲(chóng)便是利用自身復(fù)眼敏感偏振信息來(lái)獲得方位。在晴朗無(wú)云的條件下，大氣對(duì)于太陽(yáng)光的散射主要是瑞利散射，散射出射的光線主要是線偏振光，以偏振度和偏振方位角來(lái)描述。天空偏振光分布模型如圖2所示，其中，O表示地面上觀測(cè)點(diǎn)，S表示太陽(yáng)，箭頭指向表示偏振方向。天空偏振模式具有兩條對(duì)稱線：一條是與太陽(yáng)角距為90°的最大偏振線；另一條是過(guò)太陽(yáng)和天頂?shù)奶?yáng)子午線。本具體實(shí)施例中通過(guò)偏振光導(dǎo)航傳感器敏感大氣偏振特性，獲取航向參數(shù)的具體過(guò)程如圖3所示，具體如下：

首先根據(jù)偏振導(dǎo)航模型獲取載體長(zhǎng)軸與太陽(yáng)子午線的夾角ψ；然后參考慣性導(dǎo)航信息消除偏振方位的180°方向模糊性問(wèn)題；接著計(jì)算出太陽(yáng)方向矢量，即高度角hs和方位角As，在天球坐標(biāo)系中，擬通過(guò)以下兩式求解：

其中，δ為太陽(yáng)赤緯，為地面上觀測(cè)點(diǎn)O的緯度，t為太陽(yáng)時(shí)角。δ和t可以根據(jù)天文歷換算而得。

則航向角由下式確定：

γ＝ψ+A_S (5)

建立以慣性導(dǎo)航主系統(tǒng)的誤差為狀態(tài)量、水聲定位子系統(tǒng)的測(cè)距信息和偏振光導(dǎo)航子系統(tǒng)的航向信息為觀測(cè)量的擴(kuò)展卡爾曼濾波模型，對(duì)慣性導(dǎo)航主系統(tǒng)的誤差進(jìn)行閉環(huán)修正。

該組合導(dǎo)航系統(tǒng)的狀態(tài)方程如下：

狀態(tài)變量選為15階慣性誤差狀態(tài)，如式(7)所示。

其中，φ_E、φ_N、φ_U分別為東向、北向和天向的數(shù)學(xué)平臺(tái)失準(zhǔn)角，δv_E、δv_N、δv_U分別為東向、北向和天向的速度誤差，δλ、δL、δh分別為經(jīng)度、緯度和高度誤差，ε_x、ε_y、ε_z分別三軸陀螺的誤差，▽_x、▽_y、▽_z分別三軸加速度計(jì)的誤差。

F(t)為系統(tǒng)的狀態(tài)矩陣，可由慣導(dǎo)的動(dòng)力學(xué)方程確定，W(t)為系統(tǒng)的噪聲矩陣，包括三軸陀螺儀和三軸加速度計(jì)的白噪聲。

設(shè)該組合導(dǎo)航系統(tǒng)的觀測(cè)方程如下：

Y(t)＝H(t)X(t)+V(t) (8)

其中，Y(t)為系統(tǒng)的觀測(cè)向量，H(t)為觀測(cè)矩陣，V(t)為觀測(cè)白噪聲向量。

由慣導(dǎo)估計(jì)位置和已知基站位置計(jì)算的距離和可表示為：

其中，R_i為距離的真值，δR_i為陀螺儀和加速度計(jì)的漂移引起的距離誤差。

PLBL測(cè)量的距離R_1m、R_2m和R_3m可表示為：

R_im＝R_i+v_i (10)

其中，R_i仍為距離的真值，v_i為量測(cè)白噪聲，為了簡(jiǎn)化分析，本實(shí)施例中不考慮水聲傳播的時(shí)延誤差和多路徑效應(yīng)。

慣導(dǎo)計(jì)算的航向角可表示為：

其中，γ為航向角的真值，δγ為慣導(dǎo)計(jì)算的航向角誤差。

偏振光測(cè)量的航向角γ_m可表示為：

γ_m＝γ+v_γ (12)

其中，γ仍為航向角的真值，v_γ為偏振測(cè)角的量測(cè)白噪聲。

由式(9)—(12)可得系統(tǒng)的觀測(cè)方程如下：

根據(jù)以上構(gòu)建的擴(kuò)展卡爾曼濾波模型，即可對(duì)慣性導(dǎo)航主系統(tǒng)的誤差進(jìn)行閉環(huán)修正。

為了驗(yàn)證本發(fā)明的效果，構(gòu)建了半實(shí)物仿真系統(tǒng)來(lái)進(jìn)行仿真驗(yàn)證，各仿真計(jì)算機(jī)之間通過(guò)串口進(jìn)行通信并傳輸信息。仿真過(guò)程說(shuō)明如下：航跡發(fā)生器產(chǎn)生載體導(dǎo)航參數(shù)真值，慣導(dǎo)仿真計(jì)算機(jī)根據(jù)航跡真值生成IMU真值，與實(shí)際IMU的噪聲合成IMU輸出值，然后進(jìn)行捷聯(lián)解算得到導(dǎo)航參數(shù)計(jì)算值；PLBL仿真計(jì)算機(jī)根據(jù)航跡真值以及布設(shè)的基站位置生成聲學(xué)距離測(cè)量值；偏振光仿真計(jì)算機(jī)根據(jù)航跡真值以及天文歷生成航向角測(cè)量值；三者通過(guò)組合導(dǎo)航計(jì)算機(jī)作為數(shù)據(jù)融合單元，進(jìn)行擴(kuò)展卡爾曼濾波計(jì)算，得到狀態(tài)估計(jì)值并進(jìn)行誤差校正，更新導(dǎo)航參數(shù)。

假定載體的初始導(dǎo)航時(shí)間為2016年5月1日上午(北京時(shí)間)8時(shí)30分，初始位置為北緯30.7°、東經(jīng)124.2°，初始姿態(tài)水平。載體的航跡設(shè)置為“割草機(jī)”式航路，如圖4所示，航行速度為4節(jié)，仿真時(shí)間共4400s。傳感器相關(guān)參數(shù)如下：陀螺漂移為1(°)/h，加速度計(jì)零偏為10^-4g；聲納測(cè)距精度為1m；偏振光傳感器測(cè)角精度為0.2°。

圖5、圖6分別為試驗(yàn)中所得到的組合導(dǎo)航位置誤差、航向角誤差。由圖5可知，通過(guò)聲納提供的測(cè)距信息與慣性的組合，有效的校正了慣導(dǎo)的定位誤差，位置輸出不再發(fā)散，整個(gè)仿真時(shí)間段位置精度均在4m以內(nèi)；由圖6可知，通過(guò)偏振光提供的航向角觀測(cè)量，能準(zhǔn)確估計(jì)出慣導(dǎo)的方位失準(zhǔn)角，航向角精度始終在0.15以內(nèi)°。