一種基於慣導/DVL/USBL組合的慣導誤差估計方法與流程

2023-07-12 01:35:46 1

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

背景技術：

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

技術實現要素：

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

實現本發明目的的技術方案：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

計算主濾波器的狀態估計值，公式為：

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

所述的

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

本發明的有益技術效果在於：

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

具體實施方式

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

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

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

dvl：dopplervelocitylog，都卜勒計程儀。通過都卜勒效應測量dvl相對於水底的速度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

計算主濾波器的狀態估計值，公式為：

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

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

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

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