本發(fā)明屬于慣導技術，具體涉及一種基于慣導/dvl/usbl組合的慣導誤差估計方法。

背景技術：

慣導/dvl組合可以實現遠距離的組合導航，但位置誤差累積。慣導/usbl組合可以實現位置誤差不累積的組合導航，然而受usbl作用范圍限制，不易實現遠距離的組合導航，一般只能在特定水域工作。慣導/dvl/usbl組合可以實現優(yōu)勢互補，提高整個系統(tǒng)的工作適應性?，F有技術中并沒有一種基于慣導/dvl/usbl組合的慣導誤差估計方法。

技術實現要素：

本發(fā)明的目的在于提供一種基于慣導/dvl/usbl組合的慣導誤差估計方法。

實現本發(fā)明目的的技術方案：

一種基于慣導/dvl/usbl組合的慣導誤差估計方法，它包括下列步驟：

步驟1：同步采集慣導的位置和速度、dvl的速度、usbl的位置等測試數據；

步驟2：采用子濾波器1實現慣導/dvl組合；

子濾波器1的輸入信息為慣導的速度與dvl的速度；子濾波器1的狀態(tài)變量為17維向量：

其中，φn、φu、φe分別為慣導的北向、天向、東向軸失準角；δvn、δvu、δve分別為慣導的北向、天向、東向軸速度誤差；δh、δλ分別為慣導的緯度、高度和經度誤差；εn、εu、εe和▽n、▽u、▽e分別為慣導的等效北向、天向、東向軸陀螺漂移和加速度計零偏；δθs為dvl的安裝誤差角，δks為dvl的刻度系數誤差；

經過經典的kalman濾波方程解算，獲得子濾波器1的狀態(tài)估計值和估計均方誤差陣；取子濾波器1的狀態(tài)估計值的前15個變量，記作估計均方誤差陣的前15行前15列矩陣，記作p1；

步驟3：采用子濾波器2實現慣導/usbl組合；

子濾波器2的輸入信息為慣導的位置與usbl的位置；子濾波器2的狀態(tài)變量為17維向量：

其中，φn、φu、φe分別為慣導的北向、天向、東向軸失準角；δvn、δvu、δve分別為慣導的北向、天向、東向軸速度誤差；δh、δλ分別為慣導的緯度、高度和經度誤差；εn、εu、εe和▽n、▽u、▽e分別為慣導的等效北向、天向、東向軸陀螺漂移和加速度計零偏；δθa為usbl的安裝誤差角，δka為usbl的刻度系數誤差；

經過經典的kalman濾波方程解算，獲得子濾波器2的狀態(tài)估計值和估計均方誤差陣；取子濾波器2的狀態(tài)估計值的前15個變量，記作估計均方誤差陣的前15行前15列矩陣，記作p2；

步驟4：采用主濾波器，獲得慣導誤差的高精度估計；

主濾波器的輸入信息為狀態(tài)估計值估計均方誤差陣p1、狀態(tài)估計值和估計均方誤差陣p2，

計算融合估計誤差方差陣，公式為：

其中，p1^-1為p1的逆矩陣，為p2的逆矩陣；

計算主濾波器的狀態(tài)估計值，公式為：

為主濾波器估計出的慣導誤差。

所述的

包括φnφuφeδvnδvuδveδhδλεnεuεe▽n▽u▽u等15個變量。

本發(fā)明的有益技術效果在于：

本發(fā)明提供的一種基于慣導/dvl/usbl組合的慣導誤差估計方法通過同步采集慣導的位置和速度、dvl的速度、usbl的位置等測試數據，并設計三個濾波器的配合：采用子濾波器1實現慣導/dvl組合、采用子濾波器2實現慣導/usbl組合，采用主濾波器，獲得慣導誤差的高精度估計；從而實現了高環(huán)境適應性，低位置累計誤差的慣導誤差估計方法。

具體實施方式

下面結合實施例對本發(fā)明作進一步詳細說明。

本發(fā)明提供的一種慣導/dvl/usbl組合導航結構包括慣導、dvl、usbl、子濾波器1、子濾波器2和主濾波器。

慣導：由陀螺、加速度計、導航計算機等組成的可實現自主導航的慣導導航系統(tǒng)。通過其導航計算機的導航解算，可給出慣導的位置、速度、姿態(tài)等導航信息。

dvl：dopplervelocitylog，多普勒計程儀。通過多普勒效應測量dvl相對于水底的速度。

usbl：ultrashortbaseline，超短基線定位系統(tǒng)。由usbl本體和應答器構成了基線，其中應答器的位置精確已知，則usbl本體通過向應答器發(fā)射聲波解算出基線長度，即可獲得usbl的位置信息。

子濾波器1：輸入慣導的速度和dvl的速度，輸出狀態(tài)估計值和估計均方誤差陣p1。

子濾波器2：輸入慣導的位置和usbl的位置，輸出狀態(tài)估計值和估計均方誤差陣p2。

主濾波器：子濾波器1和子濾波器2的輸出端與主濾波器的輸入端連接。主濾波器輸入狀態(tài)估計值估計均方誤差陣p1、狀態(tài)估計值和估計均方誤差陣p2，輸出狀態(tài)估計值

本發(fā)明提供的一種基于慣導/dvl/usbl組合的慣導誤差估計方法包括下列 步驟：

步驟1：同步采集慣導的位置和速度、dvl的速度、usbl的位置等測試數據。

步驟2：采用子濾波器1實現慣導/dvl組合。

子濾波器1的輸入信息為慣導的速度與dvl的速度。子濾波器1的狀態(tài)變量為17維向量：

其中，φn、φu、φe分別為慣導的北向、天向、東向軸失準角；δvn、δvu、δve分別為慣導的北向、天向、東向軸速度誤差；δh、δλ分別為慣導的緯度、高度和經度誤差；εn、εu、εe和▽n、▽u、▽e分別為慣導的等效北向、天向、東向軸陀螺漂移和加速度計零偏；δθs為dvl的安裝誤差角，δks為dvl的刻度系數誤差。

經過經典的kalman濾波方程解算，獲得子濾波器1的狀態(tài)估計值和估計均方誤差陣；取子濾波器1的狀態(tài)估計值的前15個變量，記作估計均方誤差陣的前15行前15列矩陣，記作p1。

步驟3：采用子濾波器2實現慣導/usbl組合。

子濾波器2的輸入信息為慣導的位置與usbl的位置。子濾波器2的狀態(tài)變量為17維向量：

其中，φn、φu、φe分別為慣導的北向、天向、東向軸失準角；δvn、δvu、δve分別為慣導的北向、天向、東向軸速度誤差；δh、δλ分別為慣導的緯度、高度和經度誤差；εn、εu、εe和▽n、▽u、▽e分別為慣導的等效北向、天向、東向軸陀螺漂移和加速度計零偏；δθa為usbl的安裝誤差角，δka為usbl的刻 度系數誤差。

經過經典的kalman濾波方程解算，獲得子濾波器2的狀態(tài)估計值和估計均方誤差陣；取子濾波器2的狀態(tài)估計值的前15個變量，記作估計均方誤差陣的前15行前15列矩陣，記作p2。

步驟4：采用主濾波器，獲得慣導誤差的高精度估計。

主濾波器的輸入信息為狀態(tài)估計值估計均方誤差陣p1、狀態(tài)估計值和估計均方誤差陣p2，

計算融合估計誤差方差陣，公式為：

其中，p1^-1為p1的逆矩陣，為p2的逆矩陣；

計算主濾波器的狀態(tài)估計值，公式為：

為主濾波器估計出的慣導誤差，

包括φnφuφeδvnδvuδveδhδλεnεuεe▽n▽u▽u等15個變量。

至此，給出了一種基于慣導/dvl/usbl組合的慣導誤差估計方法。根據此發(fā)明，可以獲得慣導誤差量的高精度估計，用以修正慣導的位置、速度、姿態(tài)角等信息后，提高慣導系統(tǒng)的導航精度。

上面結合實施例對本發(fā)明作了詳細說明，但是本發(fā)明并不限于上述實施例，在本領域普通技術人員所具備的知識范圍內，還可以在不脫離本發(fā)明宗旨的前提下作出各種變化。本發(fā)明中未作詳細描述的內容均可以采用現有技術。