[發(fā)明專利]一種基于慣導(dǎo)/DVL/USBL組合的慣導(dǎo)誤差估計(jì)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201610068960.0
• 申請(qǐng)日: 2016-02-01
• 公開(kāi)（公告）號(hào): CN107024226B
• 公開(kāi)（公告）日: 2021-03-16
• 發(fā)明（設(shè)計(jì)）人: 林翰;周東靈;徐興華;劉崇亮;扈光鋒;曲雪云;劉壘;李文耀
• 申請(qǐng)（專利權(quán)）人: 北京自動(dòng)化控制設(shè)備研究所
• 主分類號(hào): G01C25/00
• 分類號(hào): G01C25/00;G01C21/16
• 代理公司: 核工業(yè)專利中心 11007
• 代理人: 高尚梅

本發(fā)明屬于慣導(dǎo)技術(shù)，具體公開(kāi)一種基于慣導(dǎo)/DVL/USBL組合的慣導(dǎo)誤差估計(jì)方法，包括同步采集慣導(dǎo)的位置和速度、DVL的速度、USBL的位置等測(cè)試數(shù)據(jù)，并設(shè)計(jì)三個(gè)通過(guò)經(jīng)典Kalman濾波實(shí)現(xiàn)的濾波器的配合：采用子濾波器1實(shí)現(xiàn)慣導(dǎo)/DVL組合、采用子濾波器2實(shí)現(xiàn)慣導(dǎo)/USBL組合，采用主濾波器，獲得慣導(dǎo)誤差的高精度估計(jì)；從而實(shí)現(xiàn)了高環(huán)境適應(yīng)性，低位置累計(jì)誤差的慣導(dǎo)誤差估計(jì)方法。

技術(shù)領(lǐng)域

本發(fā)明屬于慣導(dǎo)技術(shù)，具體涉及一種基于慣導(dǎo)/DVL/USBL組合的慣導(dǎo)誤差估計(jì)方法。

背景技術(shù)

慣導(dǎo)/DVL組合可以實(shí)現(xiàn)遠(yuǎn)距離的組合導(dǎo)航，但位置誤差累積。慣導(dǎo)/USBL組合可以實(shí)現(xiàn)位置誤差不累積的組合導(dǎo)航，然而受USBL作用范圍限制，不易實(shí)現(xiàn)遠(yuǎn)距離的組合導(dǎo)航，一般只能在特定水域工作。慣導(dǎo)/DVL/USBL組合可以實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)，提高整個(gè)系統(tǒng)的工作適應(yīng)性。現(xiàn)有技術(shù)中并沒(méi)有一種基于慣導(dǎo)/DVL/USBL組合的慣導(dǎo)誤差估計(jì)方法。

發(fā)明內(nèi)容

本發(fā)明的目的在于提供一種基于慣導(dǎo)/DVL/USBL組合的慣導(dǎo)誤差估計(jì)方法。

實(shí)現(xiàn)本發(fā)明目的的技術(shù)方案：

一種基于慣導(dǎo)/DVL/USBL組合的慣導(dǎo)誤差估計(jì)方法，它包括下列步驟：

步驟1：同步采集慣導(dǎo)的位置和速度、DVL的速度、USBL的位置等測(cè)試數(shù)據(jù)；

步驟2：采用子濾波器1實(shí)現(xiàn)慣導(dǎo)/DVL組合；

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

其中，φ_N、φ_U、φ_E分別為慣導(dǎo)的北向、天向、東向軸失準(zhǔn)角；δv_N、δv_U、δv_E分別為慣導(dǎo)的北向、天向、東向軸速度誤差；δh、δλ分別為慣導(dǎo)的緯度、高度和經(jīng)度誤差；ε_N、ε_U、ε_E和▽_N、▽_U、▽_E分別為慣導(dǎo)的等效北向、天向、東向軸陀螺漂移和加速度計(jì)零偏；δθ_s為DVL的安裝誤差角，δk_s為DVL的刻度系數(shù)誤差；

經(jīng)過(guò)經(jīng)典的Kalman濾波方程解算，獲得子濾波器1的狀態(tài)估計(jì)值和估計(jì)均方誤差陣；取子濾波器1的狀態(tài)估計(jì)值的前15個(gè)變量，記作估計(jì)均方誤差陣的前15行前15列矩陣，記作P₁；

步驟3：采用子濾波器2實(shí)現(xiàn)慣導(dǎo)/USBL組合；

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

其中，φ_N、φ_U、φ_E分別為慣導(dǎo)的北向、天向、東向軸失準(zhǔn)角；δv_N、δv_U、δv_E分別為慣導(dǎo)的北向、天向、東向軸速度誤差；δh、δλ分別為慣導(dǎo)的緯度、高度和經(jīng)度誤差；ε_N、ε_U、ε_E和▽_N、▽_U、▽_E分別為慣導(dǎo)的等效北向、天向、東向軸陀螺漂移和加速度計(jì)零偏；δθ_a為USBL的安裝誤差角，δk_a為USBL的刻度系數(shù)誤差；

經(jīng)過(guò)經(jīng)典的Kalman濾波方程解算，獲得子濾波器2的狀態(tài)估計(jì)值和估計(jì)均方誤差陣；取子濾波器2的狀態(tài)估計(jì)值的前15個(gè)變量，記作估計(jì)均方誤差陣的前15行前15列矩陣，記作P₂；