CN104061930A

CN104061930A - Navigation method based on strapdown inertial guidance and Doppler log

Info

Publication number: CN104061930A
Application number: CN201310653821.0A
Authority: CN
Inventors: 程向红; 许立平
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2013-12-05
Filing date: 2013-12-05
Publication date: 2014-09-24
Anticipated expiration: 2033-12-05
Also published as: CN104061930B

Abstract

The invention discloses a navigation method based on strapdown inertial guidance and a Doppler log. The navigation method is characterized in that the navigation method is applied to a navigation system based on strapdown inertial guidance and the Doppler log, wherein the navigation system comprises the Doppler log, a strapdown inertial measurement unit, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log comprises a transceiver, four energy transducers and an interface unit; the Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit comprises a three-axis gyroscope and a three-axis accelerator; the strapdown inertial measurement unit is connected with the strapdown inertial measurement unit processing module; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises a plurality of steps, and the method overcomes the defects that an AUV (autonomous underwater vehicle) navigation system is easily influenced by environment and the navigation and positioning error of a strapdown inertial navigation system cannot reach the requirements of accuracy with continued accumulation of time.

Description

Navigation Method Based on Strapdown Inertial Guidance and Doppler Log

技术领域 technical field

本发明涉及一种基于捷联惯性制导和多普勒计程仪的导航方法，属于水下及水面航行器的导航及定位跟踪领域。 The invention relates to a navigation method based on strapdown inertial guidance and a Doppler log, and belongs to the field of navigation, positioning and tracking of underwater and water surface vehicles.

背景技术 Background technique

AUV(Autonomous Underwater Vehicle自主水下航行器)是探索和开发利用海洋环境资源的重要工具，也在海洋军事应用方面发挥着重要作用。因其远程性、隐蔽性和自主性，使得AUV的导航问题仍是目前面临的主要技术挑战之一。由于水下环境复杂，常采用组合导航方式对AUV进行导航定位，而组合导航系统滤波器设计是保证导航精度的关键。目前常规AUV的导航定位精度可达到<0.8nmile／h左右，在一定程度上满足了AUV导航定位的精度要求。 AUV (Autonomous Underwater Vehicle) is an important tool for exploring, developing and utilizing marine environmental resources, and also plays an important role in marine military applications. Because of its long-distance, concealment and autonomy, the navigation problem of AUV is still one of the main technical challenges. Due to the complex underwater environment, the integrated navigation method is often used to navigate and position the AUV, and the filter design of the integrated navigation system is the key to ensure the navigation accuracy. At present, the navigation and positioning accuracy of conventional AUV can reach about <0.8nmile/h, which meets the accuracy requirements of AUV navigation and positioning to a certain extent.

SINS(Strapdown Inertial Navigation Systems捷联惯性导航系统)系统具有自主导航能力，不受环境、载体机动及无线电干扰的影响，能够连续的提供载体位置、速度和姿态等导航定位信息，其数据更新频率快，且在短时间内具有较高的相对精度。但是，随着系统工作时间的延长，捷联惯性导航系统的导航误差会随之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿捷联惯性导航系统，以抑制其随时间积累的误差。 The SINS (Strapdown Inertial Navigation Systems) system has autonomous navigation capabilities and is not affected by the environment, carrier maneuvers and radio interference. It can continuously provide navigation and positioning information such as carrier position, speed and attitude, and its data update frequency is fast. , and has high relative accuracy in a short time. However, with the prolongation of the working time of the system, the navigation error of the strapdown inertial navigation system will accumulate and increase. Accumulated errors over time.

DVL(Doppler Velocity Log多普勒计程仪)是广泛应用于水下及水面航行器组合系统的速度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。多普勒系统通过安装于AUV底部的四个换能器信息求得声波频移，进而计算得到载体在载体系的三维速度信息，具有精度高、可靠性、实时性等优点。但是由于水下环境复杂、航位推算系统位置误差随时间累积以及AUV的长航时、隐蔽性要求，基于捷联惯性导航/多普勒的组合系统的常规滤波算法精度难以达到高精度导航定位功能需求。 DVL (Doppler Velocity Log) is a speed measurement device widely used in combined systems of underwater and surface vehicles. It is an acoustic navigation and positioning system capable of outputting the three-dimensional velocity and range of the carrier in real time. The Doppler system obtains the frequency shift of the sound wave through the information of the four transducers installed at the bottom of the AUV, and then calculates the three-dimensional velocity information of the carrier in the carrier system, which has the advantages of high precision, reliability, and real-time performance. However, due to the complex underwater environment, the accumulation of dead reckoning system position errors over time, and the long endurance and concealment requirements of AUVs, the conventional filtering algorithm accuracy of the combined system based on strapdown inertial navigation/Doppler is difficult to achieve high-precision navigation and positioning. Functional Requirements.

常规H∞滤波算法对系统统计特性不作任何假设，具有滤波模型简单、鲁棒性好的优点，因此常应用于AUV导航系统。但是常规H∞滤波器跟踪机动过程能力较弱，实时性难以保证。当由于模型不确定性等因素的影响，造成滤波器的状态估计值偏离系统状态时，必然会在输出残差序列的均值与幅值上有所表现，此时若在线调整时变增益矩阵，使得系统估计误差方差最小的同时保持残差序列仍然相互正交，即满足正交性原理，则可迫使滤波器保持对实际系统状态的高精度跟踪。本发明通过在H∞滤波算法中引入次优渐消因子对过去的数据实施渐消，以减弱老数据对当前滤波估计值的影响，即通过实时调节估计误差方差矩阵以及相应的增益矩阵以满足正交性原理，迫使滤波器对实际系统状态的高精度跟踪，并最大程度地提取输出残差中一切有效信息，得到一种高跟踪性能的滤波算法。该滤波算法具有较强的关于模型不确定性的鲁棒性以及对系统机动过程的跟踪能力，并保持了H∞滤波模型简单的优点，实现滤波器对于AUV机动过程系统状态的跟踪，具有重要的理论意义和工程实用价值。 The conventional H∞ filtering algorithm does not make any assumptions on the statistical characteristics of the system, and has the advantages of simple filtering model and good robustness, so it is often used in AUV navigation systems. But the ability of conventional H∞ filter to track the maneuvering process is weak, and the real-time performance is difficult to guarantee. When the estimated value of the state of the filter deviates from the system state due to factors such as model uncertainty, it will inevitably appear in the mean and amplitude of the output residual sequence. At this time, if the time-varying gain matrix is adjusted online, The residual sequence is still orthogonal to each other while minimizing the variance of the system estimation error, that is, the principle of orthogonality is satisfied, and the filter can be forced to maintain high-precision tracking of the actual system state. In the present invention, by introducing a suboptimal fading factor into the H∞ filtering algorithm, the past data is fading away, so as to weaken the influence of the old data on the current filtering estimated value, that is, by adjusting the estimation error variance matrix and the corresponding gain matrix in real time to meet the The principle of orthogonality forces the filter to track the actual system state with high precision, and extracts all effective information in the output residual to the greatest extent, and obtains a filtering algorithm with high tracking performance. The filtering algorithm has strong robustness to model uncertainty and the ability to track the system maneuvering process, and maintains the advantages of a simple H∞ filtering model. It is important to realize the tracking of the system state by the filter for the AUV maneuvering process. theoretical significance and engineering practical value.

基于捷联惯性导航/多普勒的AUV组合导航系统将捷联惯性导航、多普勒与本发明所提出的滤波算法的优势结合在一起，可提供载体高精度的实时导航跟踪参数，解决了AUV导航系统易受环境影响以及捷联惯性导航的导航定位误差随时间延续不断累积达不到精度要求的问题。 The AUV integrated navigation system based on strapdown inertial navigation/Doppler combines the advantages of strapdown inertial navigation, Doppler and the filtering algorithm proposed by the present invention, can provide high-precision real-time navigation tracking parameters of the carrier, and solve the problem of The AUV navigation system is easily affected by the environment and the navigation positioning error of strapdown inertial navigation accumulates over time and fails to meet the accuracy requirements.

发明内容 Contents of the invention

发明目的：本发明的目的在于，克服现有技术的不足，提出了一种用于捷联惯性导航／多普勒的AUV组合导航系统的滤波方法。利用所设计的滤波器将多普勒测速信息引入AUV组合系统，以辅助捷联惯性导航进行导航定位。该方法可以整合多个子导航信息，克服了AUV导航系统易受环境影响以及捷联惯性导航系统的导航定位误差随时间延续不断累积达不到精度要求的缺陷。 Purpose of the invention: The purpose of the present invention is to overcome the deficiencies of the prior art and propose a filtering method for the strapdown inertial navigation/Doppler AUV integrated navigation system. The designed filter is used to introduce the Doppler velocity measurement information into the AUV combination system to assist the strapdown inertial navigation for navigation and positioning. This method can integrate multiple sub-navigation information, and overcomes the defect that the AUV navigation system is easily affected by the environment and the navigation positioning error of the strapdown inertial navigation system accumulates over time and fails to meet the accuracy requirements.

技术方案：本发明所述的一种基于捷联惯性制导和多普勒计程仪的导航方法，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和中央处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与中央处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和三轴加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与中央处理单元相连接；该导航方法包括以下步骤： Technical scheme: a kind of navigation method based on strapdown inertial guidance and Doppler log according to the present invention, the navigation method is applied to a navigation system based on strapdown inertial guidance and Doppler log, the navigation The system includes a Doppler log, a strapdown inertial measurement unit, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log includes a transceiver, four transducers and an interface unit; the The Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit includes a three-axis gyroscope and a three-axis accelerometer; the strapdown inertial measurement unit and the strapdown inertial measurement unit processing module connected; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises the following steps:

(1)通过捷联惯性测量单元组件中的三轴陀螺测得三轴角速度信息和加速度计测得三轴加速度信息，捷联惯性测量单元处理模块接收捷联惯性测量单元输出的导航信息，通过导航积分计算获得载体位置、速度和姿态等导航信息； (1) The three-axis angular velocity information and the three-axis acceleration information measured by the accelerometer are measured by the three-axis gyroscope in the strapdown inertial measurement unit assembly, and the strapdown inertial measurement unit processing module receives the navigation information output by the strapdown inertial measurement unit, and passes Navigation integral calculation to obtain navigation information such as carrier position, speed and attitude;

(2)通过多普勒计程仪的收发器中发射电振荡信号，送给换能器； (2) Transmit an electrical oscillation signal through the transceiver of the Doppler log and send it to the transducer;

(3)通过多普勒计程仪的四个换能器发射超声波和接收具有频移特性的反射回波； (3) Transmit ultrasonic waves and receive reflected echoes with frequency shift characteristics through the four transducers of the Doppler log;

(4)利用收发器中接收系统将换能器送来的回波信号经放大处理后求得多普勒频移并转换为航速模拟信号送给接口单元。设载体航行速度为V，波束发射俯角为θ，声速C≈1500m／s，则单波束频移计算公式为：Δf＝2Vf₀cosθ／C，由此可得速度计算公式为：V＝ΔfC／(2f₀cosθ)； (4) Use the receiving system in the transceiver to amplify the echo signal sent by the transducer to obtain the Doppler frequency shift and convert it into a speed analog signal and send it to the interface unit. Assuming that the carrier’s navigation speed is V, the beam launch depression angle is θ, and the sound velocity C≈1500m/s, then the single-beam frequency shift calculation formula is: Δf=2Vf ₀ cosθ/C, and thus the speed calculation formula is: V=ΔfC/ (2f ₀ cosθ);

(5)利用收发器中接口单元将收发器送来的四个航速模拟信号转换为航速，并以数字方式向中央处理单元输出； (5) Use the interface unit in the transceiver to convert the four speed analog signals sent by the transceiver into the speed, and output it to the central processing unit in digital form;

(6)中央处理单元中的运算器根据多普勒计程仪的接口单元输出的速度信息周期性地计算载体的实时三维载体系速度量； (6) The arithmetic unit in the central processing unit periodically calculates the real-time three-dimensional body velocity of the carrier according to the velocity information output by the interface unit of the Doppler log;

(7)中央处理单元中的滤波模块对接收到的捷联惯性导航导航信息、多普勒计程仪的三维速度信息进行滤波融合计算得到k时刻的惯性捷联测量系统校正量校正后得到最终高精度的导航定位信息； (7) The filtering module in the central processing unit performs filtering and fusion calculation on the received strapdown inertial navigation navigation information and the three-dimensional velocity information of the Doppler log to obtain the correction value of the inertial strapdown measurement system at time k After correction, the final high-precision navigation and positioning information is obtained;

所述的滤波融合计算得到惯性捷联测量系统校正量的方法为： The method for obtaining the correction amount of the inertial strapdown measurement system through the filter fusion calculation is as follows:

将捷联惯性导航作为参考系统，状态变量X取速度误差姿态角误差(φ_e，φ_n，φ_u)、加速度计随机常值偏置和陀螺随机常值漂移(ε_x，ε_y，ε_z)，共10维： $X = {[{δV}_{e}^{n}, {δV}_{n}^{n}, φ_{e}, φ_{n}, φ_{u}, {&dtri;}_{x}, {&dtri;}_{y}, ϵ_{x}, ϵ_{y}, ϵ_{z}]}^{T};$ W=[w_ax，w_ay，w_gx，w_gy，w_gz，0，0，0，0，0]^T为系统噪声向量； The strapdown inertial navigation is used as the reference system, and the state variable X takes the speed error Attitude angle error (φ _e , φ _n , φ _u ), accelerometer random constant bias and gyroscope random constant drift (ε _x , ε _y , ε _z ), a total of 10 dimensions: $x = {[{δV}_{e}^{no}, {δV}_{no}^{no}, φ_{e}, φ_{no}, φ_{u}, {&dtri;}_{x}, {&dtri;}_{the y}, ϵ_{x}, ϵ_{the y}, ϵ_{z}]}^{T};$ W=[w _ax , w _ay , w _gx , w _gy , w _gz , 0, 0, 0, 0, 0] ^T is the system noise vector;

以捷联惯性导航解算出的速度与多普勒计程仪测得的载体系速度经转换到导航系后两者之差作为系统的量测值，则量测方程表示为： The difference between the speed calculated by the strapdown inertial navigation and the vehicle body speed measured by the Doppler log after being converted to the navigation system is used as the measurement value of the system, and the measurement equation is expressed as:

$Z Z = = [\begin{matrix} {V V}_{SINSe SINSe}^{n no} - - {C C}_{b b}^{n no} {V V}_{DVLe DVLe} \\ {V V}_{SINSn SINSn}^{n no} - - {C C}_{b b}^{n no} {V V}_{DVLn DVL} \end{matrix}] = = HX HX + + V V$

其中，观测向量为量测噪声向量V＝[w_vx，w_vy]^T，系统量测矩阵为H，将系统状态方程和量测方程离散化可得离散系统滤波方程；利用初值和P₀，根据k时刻的量测Z_k就可以递推算到k时刻的状态估计 ${\hat{X}}_{k} (k = 1,2,3, . . .) :$ Among them, the observation vector is The measurement noise vector V=[w _vx , w _vy ] ^T , the system measurement matrix is H, and the discrete system filter equation can be obtained by discretizing the system state equation and measurement equation; using the initial value and P ₀ , according to the measurement Z _k at time k, the state estimation at time k can be recursively calculated ${\hat{x}}_{k} (k = 1,2,3, . . .) :$

${\overset{^^}{X x}}_{k k} = = {Φ Φ}_{k k,, k k - - 11} {\overset{^^}{X x}}_{k k - - 11} + + {K K}_{k k} (({Z Z}_{k k} - - {H h}_{k k} {Φ Φ}_{k k,, k k - - 11} {\overset{^^}{X x}}_{k k - - 11}))$

${K K}_{k k} = = {P P}_{k k} {H h}_{k k}^{T T} {(({H h}_{k k} {P P}_{k k} {H h}_{k k}^{T T} + + I I))}^{- - 11}$

其中 $P_{k} = Γ_{k, k - 1} Γ_{k, k - 1}^{T} + Λ (k) {Φ_{k, k - 1} P_{k - 1} Φ_{k, k - 1}^{T} - Φ_{k, k - 1} P_{k - 1} [\begin{matrix} H_{k}^{T} & L_{k}^{T} \end{matrix}] R_{e, k}^{- 1} [\begin{matrix} H_{k} \\ L_{k} \end{matrix}] P_{k - 1} Φ_{k, k - 1}^{T}}$ in $P_{k} = Γ_{k, k - 1} Γ_{k, k - 1}^{T} + Λ (k) {Φ_{k, k - 1} P_{k - 1} Φ_{k, k - 1}^{T} - Φ_{k, k - 1} P_{k - 1} [\begin{matrix} h_{k}^{T} & L_{k}^{T} \end{matrix}] R_{e, k}^{- 1} [\begin{matrix} h_{k} \\ L_{k} \end{matrix}] P_{k - 1} Φ_{k, k - 1}^{T}}$

其中：Λ(k)=diag[λ₁(k)，λ₂(k)，…λ_n(k)]，λ_i(k)≥1；i＝1，2，…，n。在次寻优方法中，根据系统的先验知识大致确定λ₁(k)：λ₂(k)：…：λ_n(k)的初值为：λ₁(k)：λ₂(k)：…：λ_n(k)=α₁：α₂：…：α_n，式中，α_i≥1(i=1，2，…，n)，则可得λ_i(k)的近似算法如下： Wherein: Λ(k)=diag[λ ₁ (k), λ ₂ (k), . . . λ _n (k)], λ _i (k)≥1; i=1, 2, . . . , n. In the sub-optimization method, the initial value of λ ₁ (k): λ ₂ (k): ... : λ _n (k) is roughly determined according to the prior knowledge of the system: λ ₁ (k): λ ₂ (k) : ...: λ _n (k) = α ₁ : α ₂ : ...: α _n , where α _i ≥ 1 (i=1, 2, ..., n), then the approximate algorithm of λ _i (k) can be obtained as follows:

${λ λ}_{i i} ((k k)) = = \{\begin{matrix} {α α}_{i i} {λ λ}_{00 i i,, k k},, & {α α}_{i i} {λ λ}_{00 i i,, k k} &GreaterEqual; &Greater Equal; 11 \\ 11,, & {α α}_{i i} {λ λ}_{00 i i,, k k} \leq \leq 11 \end{matrix}$

其中， in,

${λ λ}_{00 i i,, k k} = = \frac{tr tr [[N N ((k k))]]}{{Σ Σ}_{i i = = 11}^{n no} {α α}_{i i} \cdot &Center Dot; {M m}_{ii i} ((k k))}$

$N N ((k k)) = = {V V}_{00,, k k} - - {H h}_{k k} {Γ Γ}_{k k} {Γ Γ}_{k k}^{T T} {H h}_{k k}^{T T} - - β β {I I}_{k k}$

$M m ((k k)) = = {{{Φ Φ}_{k k,, k k - - 11} {P P}_{k k - - 11} {Φ Φ}_{k k,, k k - - 11}^{T T} - - {Φ Φ}_{k k,, k k - - 11} {P P}_{k k - - 11} [\begin{matrix} {H h}_{k k}^{T T} & {L L}_{k k}^{T T} \end{matrix}] {R R}_{e e,, k k}^{- - 11} [\begin{matrix} {H h}_{k k} \\ {L L}_{k k} \end{matrix}] {P P}_{k k - - 11} {Φ Φ}_{k k,, k k - - 11}^{T T}}} {H h}_{k k}^{T T} {H h}_{k k}$

tr(A)表示对矩阵A的求迹运算，式中的V_0，k由下式解算出： tr(A) represents the trace operation of the matrix A, V _{0, k} in the formula is calculated by the following formula:

${V V}_{00,, k k} = = \{\begin{matrix} {γ γ}_{11} {γ γ}_{11}^{T T},, & k k = = 00 \\ \frac{{ρV ρV}_{00,, k k - - 11} + + {γ γ}_{k k} {γ γ}_{k k}^{T T}}{11 + + ρ ρ},, & k k &GreaterEqual; &Greater Equal; 11 \end{matrix}$

上式中，残差0.95≤ρ≤1为遗忘因子，一般取ρ＝0.95；β≥1为一选定的弱化因子，一般取β=1。 In the above formula, the residual 0.95≤ρ≤1 is the forgetting factor, generally take ρ=0.95; β≥1 is a selected weakening factor, generally take β=1.

进一步地，所述的捷联惯性测量单元的三轴陀螺仪三轴正交安装，所述的三轴加速度计三轴正交安装。 Further, the three-axis gyroscope of the strapdown inertial measurement unit is installed orthogonally to three axes, and the three-axis accelerometer is installed orthogonally to three axes.

进一步地，所述的捷联惯性测量单元的三轴陀螺仪为光纤陀螺仪。 Further, the three-axis gyroscope of the strapdown inertial measurement unit is a fiber optic gyroscope.

进一步地，所述的三轴加速度计为石英挠性加速度计。 Further, the three-axis accelerometer is a quartz flexible accelerometer.

进一步地，所述多普勒计程仪的收发器包括发射系统、接收系统和计算补偿电路。 Further, the transceiver of the Doppler log includes a transmitting system, a receiving system and a calculation compensation circuit.

进一步地，所述导航系统还包括显示器，所述显示器与运算器相连接，显示中央处理器运算得到的导航信息。 Further, the navigation system further includes a display, which is connected to the calculator and displays the navigation information obtained by the calculation of the central processing unit.

该导航系统包括：捷联惯性测量单元及捷联惯性测量单元处理模块、多普勒计程仪，安装于AUV上的收发器、换能器、接口单元及中央处理单元。捷联惯性测量单元用于输出惯性测量数据；捷联惯性测量单元处理模块用于接收捷联惯性测量单元输出的数字信号，通过导航积分计算获得载体位置、速度和姿态数据；多普勒计程仪用于测量载体相对于海底的纵向速度以及横向速度；可安装于AUV底端的换能器用于发射超声波和接收反射回波；安装于AUV底端的收发器包括发射系统、接收系统及计算补偿电路等单元；接口单元用于将收发器送来的航速信号转换为航速，并以数字方式或模拟方式向外部设备输出数据；中央处理单元包括一个运算器和一个滤波模块，能够接收上述捷联惯性测量单元处理模块、多普勒计程仪的导航信息，通过实时滤波计算，利用估计出的系统状态修正捷联惯性测量单元处理模块的参数，得到最终的组合导航信息数据。 The navigation system includes: a strapdown inertial measurement unit and a strapdown inertial measurement unit processing module, a Doppler log, a transceiver installed on the AUV, a transducer, an interface unit and a central processing unit. The strapdown inertial measurement unit is used to output inertial measurement data; the strapdown inertial measurement unit processing module is used to receive the digital signal output by the strapdown inertial measurement unit, and obtain the carrier position, velocity and attitude data through navigation integral calculation; Doppler distance meter The instrument is used to measure the longitudinal velocity and lateral velocity of the carrier relative to the seabed; the transducer that can be installed at the bottom of the AUV is used to transmit ultrasonic waves and receive reflected echoes; the transceiver installed at the bottom of the AUV includes a transmitting system, a receiving system and a calculation compensation circuit and other units; the interface unit is used to convert the speed signal sent by the transceiver into the speed, and output data to external devices in digital or analog mode; the central processing unit includes an arithmetic unit and a filter module, which can receive the above-mentioned strapdown inertia The navigation information of the measurement unit processing module and the Doppler log is calculated through real-time filtering, and the parameters of the strapdown inertial measurement unit processing module are corrected by using the estimated system state to obtain the final combined navigation information data.

本发明可应用于AUV系统，AUV是探索和开发利用海洋环境资源的重要工具，也在海洋军事应用方面发挥着重要作用。因其远程性、隐蔽性和复杂的水下环境，常采用组合导航方式对AUV进行导航定位。捷联惯性导航系统具有自主导航能力，不受环境、载体机动及无线电干扰的影响，能够连续的提供载体位置、速度和姿态等导航定位信息，其数据更新频率快，且在短时间内具有较高的相对精度。但是随着系统工作时间的延长，捷联惯性导航系统的导航误差会随之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿捷联惯性导航系统，以抑制其随时间积累的误差。多普勒是广泛应用于水下及水面航行器组合系统的速度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。具有精度高、可靠性、实时性等优点。但是由于水下环境复杂、航位推算系统位置误差随时间累积以及AUV的长航时、隐蔽性要求，基于捷联惯性导航/多普勒的组合系统的常规滤波算法精度难以达到高精度导航定位功能需求。常规H∞滤波算法对系统统计特性不作任何假设，具有滤波模型简单、鲁棒性好的优点。但是常规H∞滤波器跟踪机动过程能力较弱，实时性难以保证。本发明通过在H∞滤波算法中引入次优渐消因子对过去的数据实施渐消，以减弱老数据对当前滤波估计值的影响，即通过实时调节估计误差方差矩阵以及相应的增益矩阵以满足正交性原理，迫使滤波器保持对实际系统状态的高精度跟踪，并最大程度地提取输出残差中一切有效信息，得到一种高跟踪性能的滤波算法。该滤波算法具有较强的关于模型不确定性的鲁棒性以及对系统机动过程的跟踪能力，并保持了H∞滤波模型简单的优点，实现滤波器对于AUV机动过程系统状态的跟踪。本发明利用该滤波技术将捷联惯性导航导航信息、多普勒计程仪的三维速度信息进行数据融合计算，得到最终的高精度、实时性强，持续性好的导航定位结果。 The invention can be applied to an AUV system, and the AUV is an important tool for exploring, developing and utilizing marine environmental resources, and also plays an important role in marine military applications. Because of its remoteness, concealment and complex underwater environment, integrated navigation is often used to navigate and position AUV. The strapdown inertial navigation system has autonomous navigation capability, is not affected by the environment, carrier maneuvering and radio interference, and can continuously provide navigation and positioning information such as carrier position, speed and attitude. Its data update frequency is fast, and it has a relatively short time High relative accuracy. However, with the prolongation of the working time of the system, the navigation error of the strapdown inertial navigation system will accumulate and increase. accumulated errors. Doppler is a speed measurement device widely used in combined systems of underwater and surface vehicles. It is an acoustic navigation and positioning system with the ability to output the three-dimensional speed and range of the carrier in real time. It has the advantages of high precision, reliability and real-time performance. However, due to the complex underwater environment, the accumulation of dead reckoning system position errors over time, and the long endurance and concealment requirements of AUVs, the conventional filtering algorithm accuracy of the combined system based on strapdown inertial navigation/Doppler is difficult to achieve high-precision navigation and positioning. Functional Requirements. The conventional H∞ filtering algorithm does not make any assumptions on the statistical characteristics of the system, and has the advantages of simple filtering model and good robustness. But the ability of conventional H∞ filter to track the maneuvering process is weak, and the real-time performance is difficult to guarantee. In the present invention, by introducing a suboptimal fading factor into the H∞ filtering algorithm, the past data is fading away, so as to weaken the influence of the old data on the current filtering estimated value, that is, by adjusting the estimation error variance matrix and the corresponding gain matrix in real time to meet the The principle of orthogonality forces the filter to maintain high-precision tracking of the actual system state, and extracts all effective information in the output residual to the greatest extent, and obtains a filtering algorithm with high tracking performance. The filtering algorithm has strong robustness to model uncertainty and tracking ability of the system maneuvering process, and maintains the advantages of the simplicity of the H∞ filtering model, and realizes the tracking of the system state of the AUV maneuvering process by the filter. The present invention utilizes the filtering technology to perform data fusion calculation on the strapdown inertial navigation navigation information and the three-dimensional velocity information of the Doppler log to obtain the final navigation positioning result with high precision, strong real-time performance and good continuity.

具体说明如下： The specific instructions are as follows:

(1)地球几何模型与物理参数 (1) Earth geometric model and physical parameters

①地球形状描述 ①Description of the shape of the earth

惯性导航解算中使用WGS-84参考椭球体作为地球模型，其赤道半径长半轴R_e＝6378137.0m，椭球扁率e＝1/298.257223563。子午面曲率半径R_N和卯酉圈曲率半径R_E可以由下列方程求得： In the inertial navigation calculation, the WGS-84 reference ellipsoid is used as the earth model, its equatorial radius semi-major axis _Re = 6378137.0m, ellipsoid oblateness e = 1/298.257223563. The radius of curvature R _N of the meridian surface and the radius of curvature R _E of the Maoyou circle can be obtained by the following equations:

$\{\begin{matrix} {R R}_{N N} = = \frac{{R R}_{e e} {((11 - - e e))}^{22}}{{[[{((11 - - e e))}^{22} {sin sin}^{22} L L + + {cos cos}^{22} L L]]}^{\frac{33}{22}}} \\ {R R}_{E E.} = = \frac{{R R}_{e e}}{{[[{((11 - - e e))}^{22} {sin sin}^{22} L L + + {cos cos}^{22} L L]]}^{\frac{11}{22}}} \end{matrix}$

②地球自转角速率(ω_ie) ②Earth rotation angular rate (ω _ie )

在惯性导航系统中，地球自转角速度为： In the inertial navigation system, the angular velocity of the earth's rotation is:

ω_ie=15.0411°/h=7.29211585×10^-5rad/s ω _ie =15.0411°/h=7.29211585×10 ^-5 rad/s

③地球重力加速度(g) ③Earth's gravitational acceleration (g)

WGS-84参考椭球的重力加速度为： The gravitational acceleration of the WGS-84 reference ellipsoid is:

$g g = = 9.7803267714 9.7803267714 \times \times \frac{11 + + 0.00193185138639 0.00193185138639 {sin sin}^{22} L L}{\sqrt{11 - - 0.00669437999013 0.00669437999013 {sin sin}^{22} L L}} {((11 + + \frac{h h}{{R R}_{e e}}))}^{- - 22}$

式中，L表示当地纬度，h表示高度，R_e为赤道半径长半轴。 In the formula, L represents the local latitude, h represents the height, and _Re represents the semi-major axis of the equatorial radius.

(2)坐标系定义 (2) Coordinate system definition

①地心惯性坐标系(i系) ①Earth-centered inertial coordinate system (i system)

用ox_iy_iz_i表示，原点位于地球中心，ox_i与oy_i轴在地球赤道平面内，ox_i轴指向春分点，oz_i轴和地球自转轴重合，oy_i与ox_i、oz_i构成右手直角坐标系。三个坐标轴在惯性空间的指向固定。捷联惯性测量单元的输出以i系为参考基准。 Indicated by ox _i y _i z _i , the origin is located at the center of the earth, the axes of ox _i and oy _i are in the plane of the earth's equator, the axis of ox _i points to the vernal equinox, the axis of oz _i coincides with the axis of rotation of the earth, oy _i forms with ox _i and oz _i Right-handed Cartesian coordinate system. The orientation of the three coordinate axes in inertial space is fixed. The output of the strapdown inertial measurement unit is referenced to the i frame.

②地球坐标系(e系) ②Earth coordinate system (e system)

用ox_ey_ez_e表示，原点位于地球中心，oz_e轴和地球自转轴重合，ox_e轴沿格林尼治子午面和地球赤道平面的交线，oy_e轴在赤道平面内，ox_e、oy_e、oz_e三轴构成右手直角坐标系。地球坐标系与地球固连，e系相对i系转动的角速率即为地球自转角速度ω_ie。 Indicated by ox _e y _e z _e , the origin is located at the center of the earth, the oz _e axis coincides with the earth's rotation axis, the ox _e axis is along the intersection line between the Greenwich meridian and the earth's equator plane, the oy _e axis is in the equatorial plane, ox _e , oy The three axes of _e and oz _e form a right-handed Cartesian coordinate system. The earth coordinate system is fixedly connected with the earth, and the angular velocity of the rotation of the e system relative to the i system is the earth's rotation angular velocity ω _ie .

③地理坐标系(g系) ③Geographic coordinate system (g system)

用ox_gy_gz_g表示，原点位于载体重心，一般采用东北天坐标系作为地理坐标系，即ox_g轴指向东(E)，oy_g轴指向北(N)，oz_g轴指向天(U)。 Indicated by ox _g y _g z _g , the origin is located at the center of gravity of the carrier. Generally, the northeast sky coordinate system is used as the geographic coordinate system, that is, the ox _g axis points to the east (E), the oy _g axis points to the north (N), and the oz _g axis points to the sky ( U).

④导航坐标系(n系) ④Navigation coordinate system (n system)

用ox_ny_nz_n表示，采用东北天地理坐标系(ENU)作为导航坐标系。 It is represented by ox _n y _n z _n , and the Northeast Sky Geographical Coordinate System (ENU) is used as the navigation coordinate system.

⑤载体坐标系(b系) ⑤Carrier coordinate system (b system)

用ox_by_bz_b表示，原点一般取捷联惯性测量单元几何中心，ox_b轴沿载体横轴向右，oy_b轴沿载体纵轴向前，oz_b轴沿载体立轴向上。 Expressed by ox _b y _b z _b , the origin generally takes the geometric center of the strapdown inertial measurement unit, the ox _b axis is rightward along the transverse axis of the carrier, the oy _b axis is forward along the longitudinal axis of the carrier, and the oz _b axis is upward along the vertical axis of the carrier.

(3)姿态角与姿态矩阵之间的转换关系 (3) Conversion relationship between attitude angle and attitude matrix

①姿态角定义 ①Definition of attitude angle

航向角：载体纵轴oy_b在水平面上的投影与地理子午线北向之间的夹角，称为航向角，计为ψ。航向角数值是以地理北向为起点顺时针方向计算的，其定义域为0～360°。 Course angle: the angle between the projection of the longitudinal axis oy _b of the carrier on the horizontal plane and the north direction of the geographic meridian is called the course angle and is calculated as ψ. The heading angle value is calculated clockwise from the geographic north direction, and its definition range is 0 to 360°.

俯仰角：载体绕横轴ox_b转动时，载体纵轴和水平面的夹角，称为俯仰角，记为θ。俯仰角从水平面算起，向上为正，向下为负，其定义域为-90°～+90°。 Pitch angle: When the carrier rotates around the horizontal axis ox _b , the angle between the longitudinal axis of the carrier and the horizontal plane is called the pitch angle, denoted as θ. The pitch angle is calculated from the horizontal plane, upward is positive and downward is negative, and its definition range is -90°～+90°.

横摇角：载体纵向对称平面与纵向铅垂平面之间的夹角，称为横摇角，记为γ。横摇角从铅垂平面算起，右倾为正，左倾为负，其定义域为-180°～+180°。 Roll angle: The angle between the longitudinal symmetry plane of the carrier and the longitudinal vertical plane is called the roll angle, denoted as γ. The roll angle is calculated from the vertical plane, the right tilt is positive, and the left tilt is negative, and its definition range is -180°～+180°.

②姿态矩阵 ②Attitude matrix

实现载体坐标系b系到地理坐标系(导航系n系)的变换可按下列顺序经三次转动得到，具体顺序为：绕-oz_g轴转ψ角，绕ox₁轴转θ角，再绕oy₂轴转γ角。它们之间的转换关系如下式所示。 The conversion from the carrier coordinate system b to the geographic coordinate system (navigation system n) can be obtained through three rotations in the following sequence, the specific sequence is: rotate the angle ψ around the -oz _g axis, rotate the angle θ around the ox ₁ axis, and then rotate around oy _2- axis rotation γ angle. The conversion relationship between them is shown in the following formula.

$o o {x x}_{g g} {y the y}_{g g} {z z}_{g g} {&RightArrow; &Right Arrow;}_{y the y}^{- - o o {z z}_{g g}} o o {x x}_{11} {y the y}_{11} {z z}_{11} {&RightArrow; &Right Arrow;}_{q q}^{o o {x x}_{11}} o o {x x}_{22} {y the y}_{22} {z z}_{22} {&RightArrow; &Right Arrow;}_{g g}^{o o {y the y}_{22}} o o {x x}_{b b} {y the y}_{b b} {z z}_{b b}$

三次转动对应的变换矩阵分别为 The transformation matrices corresponding to the three rotations are

${C C}_{ψ ψ} = = [\begin{matrix} cos cos ψ ψ & - - sin sin ψ ψ & 00 \\ sin sin ψ ψ & cos cos ψ ψ & 00 \\ 00 & 00 & 11 \end{matrix}]$

${C C}_{θ θ} = = [\begin{matrix} 11 & 00 & 00 \\ 00 & cos cos θ θ & sin sin θ θ \\ 00 & - - sin sin & cos cos θ θ \end{matrix}]$

${C C}_{γ γ} = = [\begin{matrix} cos cos γ γ & 00 & - - sin sin \\ 00 & 11 & 00 \\ sin sin γ γ & 00 & cos cos γ γ \end{matrix}]$

所以地理坐标系到载体坐标系的转换矩阵为： Therefore, the conversion matrix from the geographic coordinate system to the carrier coordinate system is:

${C C}_{g g}^{b b} = = [\begin{matrix} cos cos γ γ cos cos ψ ψ + + sin sin γ γ sin sin ψ ψ sin sin θ θ & - - cos cos γ γ sin sin ψ ψ + + sin sin γ γ cos cos ψ ψ sin sin θ θ & - - sin sin γ γ cos cos θ θ \\ sin sin ψ ψ cos cos θ θ & cos cos ψ ψ cos cos θ θ & sin sin θ θ \\ sin sin γ γ cos cos ψ ψ - - cos cos γ γ sin sin ψ ψ sin sin θ θ & - - sin sin γ γ sin sin ψ ψ - - cos cos γ γ cos cos ψ ψ sin sin θ θ & cos cos γ γ cos cos θ θ \end{matrix}]$

以地理坐标系作为导航坐标系，则有捷联惯性导航系统姿态矩阵为 Taking the geographic coordinate system as the navigation coordinate system, the attitude matrix of the strapdown inertial navigation system is

${C C}_{b b}^{n no} = = {(({C C}_{g g}^{b b}))}^{T T}$

(③姿态角和姿态矩阵的转换关系 (③ Conversion relationship between attitude angle and attitude matrix

${C C}_{b b}^{n no} = = {(({C C}_{g g}^{b b}))}^{T T} = = {[[{T T}_{ij ij}]]}_{33 \times \times 33}$

即可解得姿态矩阵至主值区间姿态角的转换关系为： The transformation relationship from the attitude matrix to the attitude angle in the main value interval can be solved as follows:

(4)捷联惯性导航系统基本原理 (4) Basic principles of strapdown inertial navigation system

①捷联惯性导航系统微分方程 ①Differential equation of strapdown inertial navigation system

姿态矩阵微分方程为： The attitude matrix differential equation is:

其中，为载体角速度，由陀螺仪测得，且有： in, is the angular velocity of the carrier, measured by the gyroscope, and have:

${ω ω}_{ie ie}^{n no} = = [\begin{matrix} 00 \\ {ω ω}_{ie ie} cos cos L L \\ {ω ω}_{ie ie} sin sin L L \end{matrix}],, {ω ω}_{en en}^{n no} = = [\begin{matrix} - - \frac{{V V}_{N N}^{n no}}{{R R}_{N N} + + h h} \\ \frac{{V V}_{E E.}^{n no}}{{R R}_{E E.} + + h h} \\ \frac{{V V}_{E E.}^{n no}}{{R R}_{E E.} + + h h} tan the tan L L \end{matrix}]$

式中， $V^{n} = {[\begin{matrix} V_{E}^{n} & V_{N}^{n} & V_{U}^{n} \end{matrix}]}^{T}$ 为载体对地速度在导航系的大小。 In the formula, $V^{no} = {[\begin{matrix} V_{E.}^{no} & V_{N}^{no} & V_{u}^{no} \end{matrix}]}^{T}$ is the magnitude of the carrier's ground speed in the navigation system.

载体在导航系内的比力方程为： ${\overset{\cdot}{V}}^{n} = f^{n} - ({2 ω}_{ie}^{n} + ω_{en}^{n}) \times V^{n} + g^{n}$ The specific force equation of the carrier in the navigation system is: ${\overset{\cdot}{V}}^{no} = f^{no} - ({2 ω}_{ie}^{no} + ω_{en}^{no}) \times V^{no} + g^{no}$

式中f^b为加速度计测得的比力，gⁿ=[0 0 -g]^T。 In the formula f ^b is the specific force measured by the accelerometer, g ⁿ =[0 0 -g] ^T .

位置更新微分方程：在以地理坐标系作为导航坐标系的系统中，纬度L、经度λ和高度h的微分方程为： Position update differential equation: In a system that uses the geographic coordinate system as the navigation coordinate system, the differential equations for latitude L, longitude λ, and altitude h are:

$\overset{\cdot \cdot}{L L} = = \frac{{V V}_{N N}^{n no}}{{R R}_{N N} + + h h},, \overset{\cdot \cdot}{λ λ} = = \frac{{V V}_{E E.}^{n no}}{(({R R}_{E E.} + + h h)) cos cos L L},, \overset{\cdot &Center Dot;}{h h} = = {V V}_{U u}^{n no}$

②捷联惯性导航系统误差模型 ② Error model of strapdown inertial navigation system

陀螺仪误差模型：ε_i=ε_bi+ε_wi(i=x，y，z)，ε_bi为随机常数，ε_wi为白噪声； Gyroscope error model: ε _i =ε _bi +ε _wi (i=x, y, z), ε _bi is a random constant, ε _wi is white noise;

加速度计误差模型：(i=x，y，z)，为随机常数，为白噪声； Accelerometer error model: (i=x, y, z), is a random constant, is white noise;

姿态误差方程为： ${\overset{\cdot}{φ}}^{n} = - ω_{in}^{n} \times φ^{n} + δ ω_{ie}^{n} + δ ω_{en}^{n} - C_{b}^{n} ϵ^{b}, φ^{n} = {[\begin{matrix} φ_{x} & φ_{y} & φ_{z} \end{matrix}]}^{T}$ 为姿态误差，ε^b=[ε_x ε_y ε_z]^T为陀螺仪误差。 The attitude error equation is: ${\overset{\cdot}{φ}}^{no} = - ω_{in}^{no} \times φ^{no} + δ ω_{ie}^{no} + δ ω_{en}^{no} - C_{b}^{no} ϵ^{b}, φ^{no} = {[\begin{matrix} φ_{x} & φ_{the y} & φ_{z} \end{matrix}]}^{T}$ is the attitude error, ε ^b =[ε _x ε _y ε _z ] ^T is the gyroscope error.

速度误差方程为： The velocity error equation is:

$δ δ {\overset{\cdot \cdot}{V V}}^{n no} = = - - φ φ \times \times {f f}^{n no} - - ((22 {δω δω}_{ie ie}^{n no} + + δ δ {ω ω}_{en en}^{n no})) \times \times {V V}^{n no} - - ((22 {ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) \times \times {δV δV}^{n no} + + {C C}_{b b}^{n no} {&dtri; &dtri;}^{b b},,$

$δ V^{n} = {[\begin{matrix} δ V_{e}^{n} & δ V_{n}^{n} & ϵ V_{u}^{n} \end{matrix}]}^{T}$ 为速度误差， ${&dtri;}^{b} = {[\begin{matrix} {&dtri;}_{x} & {&dtri;}_{y} & {&dtri;}_{z} \end{matrix}]}^{T}$ 为加速度计误差。 $δ V^{no} = {[\begin{matrix} δ V_{e}^{no} & δ V_{no}^{no} & ϵ V_{u}^{no} \end{matrix}]}^{T}$ is the speed error, ${&dtri;}^{b} = {[\begin{matrix} {&dtri;}_{x} & {&dtri;}_{the y} & {&dtri;}_{z} \end{matrix}]}^{T}$ is the accelerometer error.

位置误差方程为： The position error equation is:

$δ δ \overset{\cdot &Center Dot;}{L L} = = \frac{δ δ {V V}_{n no}^{n no}}{{R R}_{N N} + + h h} - - \frac{{V V}_{N N}^{n no}}{{(({R R}_{N N} + + h h))}^{22}} δh δh$

$δ δ \overset{\cdot &Center Dot;}{λ λ} = = \frac{sec sec L L}{{R R}_{E E.} + + h h} δ δ {V V}_{e e}^{n no} + + \frac{{V V}_{E E.}^{n no} sec sec L L tan the tan L L}{{R R}_{E E.} + + h h} δL δ L - - \frac{{V V}_{E E.}^{n no} sec sec L L}{{(({R R}_{E E.} + + h h))}^{22}} δh δh$

$δ δ \overset{\cdot &Center Dot;}{h h} = = δ δ {V V}_{u u}^{n no}$

(5)滤波算法 (5) Filtering algorithm

①H∞滤波算法介绍 ① Introduction to H∞ filtering algorithm

考虑如下基于Krein空间离散状态空间系统模型： Consider the following discrete state-space system model based on Krein space:

$\{\begin{matrix} {X x}_{k k} = = {Φ Φ}_{k k,, k k - - 11} {X x}_{k k - - 11} + + {Γ Γ}_{k k,, k k - - 11} {W W}_{k k - - 11} \\ [\begin{matrix} {Z Z}_{k k} \\ {y the y}_{k k} \end{matrix}] = = [\begin{matrix} {H h}_{k k} \\ {L L}_{k k} \end{matrix}] {X x}_{k k} + + {V V}_{k k} \end{matrix}$

X_k为t_k时刻的待估计系统状态向量； X _k is the state vector of the system to be estimated at time t _k ;

Φ_k，k-1为t_k-1时刻至t_k时刻的一步转移矩阵； Φ _{k, k-1} is the one-step transition matrix from time t _k-1 to time t _k ;

Γ_k-1为系统噪声驱动阵； Γ _k-1 is the system noise driving array;

Z_k为量测状态向量； Z _k is the measurement state vector;

H_k为量测阵； H _k is the measurement array;

W_k-1为系统激励噪声序列； W _k-1 is the system excitation noise sequence;

V_k为量测噪声序列； V _k is the measurement noise sequence;

X_k，k-1为系统状态一步预测； X _{k, k-1} is the one-step prediction of the system state;

P_k为估计误差方差阵； P _k is the estimated error variance matrix;

K_k为滤波增益矩阵； K _k is the filter gain matrix;

y_k是给定的系统状态X_k的线性组合观测量。 y _k is a linear combination of observations for a given system state X _k .

其中，W_k和V_k为I₂能量有界的噪声，即对其统计特性不做任何假设。设系统的初始状态为X₀，表示对系统初始状态X₀的一个估计，定义初始估计误差方差阵为： Among them, W _k and V _k are I ₂ energy bounded noises, namely No assumptions are made about its statistical properties. Let the initial state of the system be X ₀ , Represents an estimate of the initial state X ₀ of the system, and the initial estimate error variance matrix is defined as:

令表示在给定观测值{Z_k}条件下对y_k的估计，定义如下的滤波误差 $e_{k} = {\hat{y}}_{k} - L_{k} X_{k} .$ make Represents the estimation of y _k under the condition of given observation value {Z _k }, and the filtering error is defined as follows $e_{k} = {\hat{the y}}_{k} - L_{k} x_{k} .$

设T_k(F_f)表示将未知干扰映射至滤波误差{e_k}的传递函数。次优H∞估计问题描述：给定正数γ>0，导找次优H∞估计 ${\hat{y}}_{k} = F_{f} (Z_{0}, Z_{1}, . . ., Z_{k}),$ 使得||T_k(F_f)||_∞<γ，即满足 Let T _k (F _f ) represent the unknown interference The transfer function that maps to the filter error {e _k }. Suboptimal H∞ estimation problem description: Given a positive number γ>0, guide to find a suboptimal H∞ estimate ${\hat{the y}}_{k} = f_{f} (Z_{0}, Z_{1}, . . ., Z_{k}),$ Make ||T _k (F _f )|| _∞ <γ, which satisfies

$\underset{{F f}_{f f}}{inf inf} \underset{{X x}_{00},, W W,, V V &Element; &Element; {h h}_{22}}{sup sup} \frac{{| | | | {e e}_{k k} | | | |}_{22}^{22}}{{| | | | {X x}_{00} - - {\overset{^^}{X x}}_{00} | | | |}_{{P P}_{00}^{- - 11}}^{22} + + {| | | | {W W}_{k k} | | | |}_{22}^{22} + + {| | | | {V V}_{k k} | | | |}_{22}^{22}} < < {γ γ}^{22}$

对于给定的γ>0，如果[Φ_k，k-1，Γ_k，k-1]是满秩的，则满足条件||T_k(F_f)||_∞≤γ的滤波器存在，当且仅当对所有的k，有 For a given γ>0, if [Φ _{k, k-1} , Γ _{k, k-1} ] is full rank, then a filter satisfying the condition ||T _k (F _f )|| _∞ ≤ γ exists, If and only if for all k, there is

${P P}_{k k}^{- - 11} + + {H h}_{k k}^{T T} {H h}_{k k} - - {γ γ}^{- - 22} {L L}_{k k}^{T T} {L L}_{k k} > > 00$

其中，P_k满足如下递推Riccati方程 Among them, P _k satisfies the following recursive Riccati equation

${P P}_{k k} = = {Φ Φ}_{k k . . k k - - 11} {P P}_{k k - - 11} {Φ Φ}_{k k,, k k - - 11}^{T T} + + {Γ Γ}_{k k,, k k - - 11} {Γ Γ}_{k k,, k k - - 11}^{T T} - - {Φ Φ}_{k k,, k k - - 11} {P P}_{k k - - 11} [\begin{matrix} {H h}_{k k}^{T T} & {L L}_{k k}^{T T} \end{matrix}] {R R}_{e e,, k k}^{- - 11} [\begin{matrix} {H h}_{k k} \\ {L L}_{k k} \end{matrix}] {P P}_{k k - - 11} {Φ Φ}_{k k,, k k - - 11}^{T T}$

${R R}_{e e,, k k} = = [\begin{matrix} I I & 00 \\ 00 & - - {γ γ}^{22} I I \end{matrix}] + + [\begin{matrix} {H h}_{k k} \\ {L L}_{k k} \end{matrix}] {P P}_{k k} [\begin{matrix} {H h}_{k k}^{T T} & {L L}_{k k}^{T T} \end{matrix}]$

若上述不等式式成立，可得H∞滤波递推算法如下： If the above inequality is established, the H∞ filter recursive algorithm can be obtained as follows:

${\overset{^^}{y the y}}_{k k} = = {L L}_{k k} {\overset{^^}{X x}}_{k k}$

次优H∞滤波算法在P_k的求解过程中，给定正数γ值通过R_e，k进而调整P_k，滤波算法的鲁棒性较好。只要给定初值和P₀，根据k时刻的量测Z_k就可以递推算到k时刻的状态估计 ${\hat{X}}_{k} (k = 1,2,3, . . .) .$ In the process of solving P _k , the suboptimal H∞ filtering algorithm adjusts P _k by giving a positive γ value through Re _{, k} , and the robustness of the filtering algorithm is better. As long as the initial value is given and P ₀ , according to the measurement Z _k at time k, the state estimation at time k can be recursively calculated ${\hat{x}}_{k} (k = 1,2,3, . . .) .$

②改进算法 ② Improved algorithm

滤波器状态估计值具有如下的一般结构： The filter state estimate has the following general structure:

式中 $\begin{matrix} {\hat{X}}_{k, k - 1} = Φ_{k, k - 1} {\hat{X}}_{k - 1} \\ γ_{k} = Z_{k} - H_{k} {\hat{X}}_{k, k - 1} \end{matrix},$ γ_k表示k时刻的残差向量； In the formula $\begin{matrix} {\hat{x}}_{k, k - 1} = Φ_{k, k - 1} {\hat{x}}_{k - 1} \\ γ_{k} = Z_{k} - h_{k} {\hat{x}}_{k, k - 1} \end{matrix},$ γ _k represents the residual vector at time k;

在线选择一个适当的时变增益矩阵K_k，使同时满足如下条件，即正交性原理： An appropriate time-varying gain matrix K _k is selected online so that the following conditions are satisfied at the same time, that is, the principle of orthogonality:

$E E. [[(({X x}_{k k} - - {\overset{^^}{X x}}_{k k})) {(({X x}_{k k} - - {\overset{^^}{X x}}_{k k}))}^{T T}]] = = min min$

$E E. [[{γ γ}_{k k + + j j} {γ γ}_{k k}^{T T}]] = = 00,, k k = = 0,1,2 0,1,2,, . . . . . .,, j j = = 1,2 1,2,, . . . . . .$

当由于模型不确定性等因素的影响，造成滤波器的状态估计值偏离系统状态时，必然会在输出残差序列的均值与幅值上有所表现，此时若在线调整增益阵K_k，使得残差序列仍然保持相互正交，则可迫使滤波器保持对实际系统状态的跟踪。 When the estimated value of the state of the filter deviates from the system state due to factors such as model uncertainty, it will inevitably appear in the mean and amplitude of the output residual sequence. At this time, if the gain matrix K _k is adjusted online, Keeping the residual sequences still mutually orthogonal forces the filter to keep track of the actual system state.

在H∞滤波算法中引入次优渐消因子对过去的数据实施渐消，以减弱老数据对当前滤波估计值的影响，通过实时调节估计误差方差矩阵P_k以及相应的增益矩阵K_k来强迫输出残差近似为高斯白噪声，可最大程度地提取输出残差中一切有效信息，即得种跟踪性能较强的滤波算法。 In the H∞ filtering algorithm, the suboptimal fading factor is introduced to fade out the past data, so as to weaken the influence of the old data on the current filtering estimated value. By adjusting the estimation error variance matrix P _k and the corresponding gain matrix K _k in real time to force The output residual is approximately Gaussian white noise, which can extract all effective information in the output residual to the greatest extent, that is, a filter algorithm with strong tracking performance.

将P_k写成如下形式： Write P _k as follows:

${P P}_{k k} = = {Γ Γ}_{k k,, k k - - 11} {Γ Γ}_{k k,, k k - - 11}^{T T} + + Λ Λ ((k k)) {{{Φ Φ}_{k k,, k k - - 11} {P P}_{k k - - 11} {Φ Φ}_{k k,, k k - - 11}^{T T} - - {Φ Φ}_{k k,, k k - - 11} {P P}_{k k - - 11} [\begin{matrix} {H h}_{k k}^{T T} & {L L}_{k k}^{T T} \end{matrix}] {R R}_{e e,, k k}^{- - 11} [\begin{matrix} {H h}_{k k} \\ {L L}_{k k} \end{matrix}] {P P}_{k k - - 11} {Φ Φ}_{k k,, k k - - 11}^{T T}}}$

其中：Λ(k)=diag[λ₁(k)，λ₂(k)，…λ_n(k)]，λ_i(k)≥1；i=1，2，…，n。在次寻优方法中，根据系统的先验知识大致确定λ₁(k)：λ₂(k)：…：λ_n(k)的初值为：λ₁(k)：λ₂(k)：…：λ_n(k)=α₁：α₂：…：α_n，式中，α_i≥1(i=1，2，…，n)，则可得λ_i(k)的近似算法如下： Where: Λ(k)=diag[λ ₁ (k), λ ₂ (k), ...λ _n (k)], λ _i (k)≥1; i=1, 2, ..., n. In the sub-optimization method, the initial value of λ ₁ (k): λ ₂ (k): ... : λ _n (k) is roughly determined according to the prior knowledge of the system: λ ₁ (k): λ ₂ (k) : ...: λ _n (k) = α ₁ : α ₂ : ...: α _n , where α _i ≥ 1 (i=1, 2, ..., n), then the approximate algorithm of λ _i (k) can be obtained as follows:

其中， in,

tr(A)表示对任意矩阵A的求迹运算，式中的V_0，k由下式解算出： tr(A) represents the trace operation for any matrix A, V _{0 and k} in the formula are calculated by the following formula:

上式中，残差0.95≤ρ≤1为遗忘因子，一般取ρ＝0.95；β≥1为弱化因子，引入此弱化因子目的是使状态估计值更加平滑。 In the above formula, the residual 0.95≤ρ≤1 is the forgetting factor, generally ρ=0.95; β≥1 is the weakening factor, the purpose of introducing this weakening factor is to make the state estimate smoother.

该方法以H∞滤波算法为基础，通过实时调节估计误差方差矩阵以及相应的增益矩阵以满足正交性原理，迫使滤波器保持对实际系统状态的高精度跟踪，得到一种高跟踪性能的滤波算法。该滤波算法具有较强的关于模型不确定性的鲁棒性以及对系统机动过程的跟踪能力，并保持了H∞滤波模型简单的优点，实现滤波器对于载体机动过程系统状态的高精度跟踪。 This method is based on the H∞ filtering algorithm. By adjusting the estimated error variance matrix and the corresponding gain matrix in real time to meet the principle of orthogonality, the filter is forced to maintain high-precision tracking of the actual system state, and a filter with high tracking performance is obtained. algorithm. The filtering algorithm has strong robustness to model uncertainty and tracking ability of the system maneuvering process, and maintains the advantages of the simplicity of the H∞ filtering model, and realizes high-precision tracking of the system state of the carrier maneuvering process by the filter.

将上述提出的滤波算法应用于AUV组合导航系统，对惯性捷联导航系统导航信息、多普勒计程仪的三维速度信息进行数据融合，得到高精度、实时性强，持续性好的导航定位结果，有效的提高了组合系统导航定位性能。 Apply the filtering algorithm proposed above to the AUV integrated navigation system, and integrate the navigation information of the inertial strapdown navigation system and the three-dimensional velocity information of the Doppler log to obtain high-precision, real-time, and continuous navigation and positioning As a result, the navigation and positioning performance of the combined system is effectively improved.

本发明与现有技术相比，其有益效果是：该方法可以整合多个子导航传感器信息，优势互补以得到更高精度和更可靠的导航信息。 Compared with the prior art, the present invention has the beneficial effect that the method can integrate the information of multiple sub-navigation sensors and complement each other to obtain higher-precision and more reliable navigation information.

附图说明 Description of drawings

图1为本发明基于捷联惯性制导和多普勒计程仪的导航方法中的基于捷联惯性制导和多普勒计程仪的导航系统组成的主框图； Fig. 1 is the main block diagram based on the navigation system of strapdown inertial guidance and Doppler log in the navigation method of the present invention based on strapdown inertial guidance and Doppler log;

图2为本发明基于捷联惯性制导和多普勒计程仪的导航方法组合模型框图； Fig. 2 is the combined model block diagram of the navigation method based on strapdown inertial guidance and Doppler log of the present invention;

图3为本发明基于捷联惯性制导和多普勒计程仪的导航方法结构原理图； Fig. 3 is the structural principle diagram of the navigation method based on strapdown inertial guidance and Doppler log of the present invention;

图4为本发明基于捷联惯性制导和多普勒计程仪的导航方法中滤波融合计算算法流程图； Fig. 4 is the flow chart of filter fusion calculation algorithm in the navigation method based on strapdown inertial guidance and Doppler log of the present invention;

图5为本发明基于捷联惯性制导和多普勒计程仪的导航方法中的捷联惯性导航和多普勒系统总体流程图； Fig. 5 is the general flowchart of strapdown inertial navigation and Doppler system in the navigation method based on strapdown inertial guidance and Doppler log of the present invention;

图6为该为本发明基于捷联惯性制导和多普勒计程仪的导航方法的实施例1的仿真分析效果图。 Fig. 6 is a simulation analysis effect diagram of embodiment 1 of the navigation method based on strapdown inertial guidance and Doppler log of the present invention.

具体实施方式 Detailed ways

下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。 The technical solutions of the present invention will be described in detail below, but the protection scope of the present invention is not limited to the embodiments.

实施例： Example:

本实施例的一种基于捷联惯性制导和多普勒计程仪的导航方法，在本实施例中，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和中央处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与中央处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和三轴加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与中央处理单元相连接；该导航方法包括以下步骤： A kind of navigation method based on strapdown inertial guidance and Doppler log of the present embodiment, in the present embodiment, this navigation method is applied to a navigation system based on strapdown inertial guidance and Doppler log, The navigation system includes a Doppler log, a strapdown inertial measurement unit, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log includes a transceiver, four transducers and an interface unit ; The Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit includes a three-axis gyroscope and a three-axis accelerometer; the strapdown inertial measurement unit and the strapdown inertial measurement unit The processing module is connected; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method includes the following steps:

其中，观测向量为量测噪声向量V=[w_vx，w_vy]^T，系统量测矩阵为H，将系统状态方程和量测方程离散化可得离散系统滤波方程；利用初值和P₀，根据k时刻的量测Z_k就可以递推算到k时刻的状态估计 ${\hat{X}}_{k} (k = 1,2,3, . . .) :$ Among them, the observation vector is The measurement noise vector V=[w _vx , w _vy ] ^T , the system measurement matrix is H, and the discrete system filter equation can be obtained by discretizing the system state equation and measurement equation; using the initial value and P ₀ , according to the measurement Z _k at time k, the state estimation at time k can be recursively calculated ${\hat{x}}_{k} (k = 1,2,3, . . .) :$

其中， in,

${λ λ}_{00 i i,, k k} = = \frac{tr tr [[N N ((k k))]]}{{Σ Σ}_{i i = = 11}^{n no} {α α}_{i i} \cdot \cdot {M m}_{ii i} ((k k))}$

本实施例的捷联惯性测量单元的三轴陀螺仪三轴正交安装，所述的三轴加速度计三轴正交安装。 The three-axis gyroscope of the strapdown inertial measurement unit in this embodiment is installed orthogonally to three axes, and the three-axis accelerometer is installed orthogonally to three axes.

本实施例的捷联惯性测量单元的三轴陀螺仪为光纤陀螺仪。 The three-axis gyroscope of the strapdown inertial measurement unit in this embodiment is a fiber optic gyroscope.

本实施例的三轴加速度计为硅微加速度计。 The triaxial accelerometer in this embodiment is a silicon micro accelerometer.

本实施例的多普勒计程仪的收发器包括发射系统、接收系统和计算补偿电路。 The transceiver of the Doppler log in this embodiment includes a transmitting system, a receiving system and a calculation compensation circuit.

本实施例的导航系统还包括显示器，所述显示器与运算器相连接，显示中央处理器运算得到的导航信息。 The navigation system of this embodiment further includes a display, which is connected to the computing unit and displays the navigation information obtained by the calculation of the central processing unit.

将基于H∞滤波算法的常规导航方法(对比例)、和本实施例的基于惯性捷联导航和多普勒计程仪的导航方法(实施例)在AUV组合导航系统中的滤波性能进行仿真分析，给出了AUV航迹曲线跟踪对比效果如图6所示，该新的导航方法更接近理论上的预定导航AUV航迹曲线。 The filter performance of the conventional navigation method (comparative example) based on H∞ filtering algorithm and the navigation method (embodiment) based on inertial strapdown navigation and Doppler log of this embodiment in the AUV integrated navigation system is simulated According to the analysis, the comparison effect of AUV track curve tracking is given, as shown in Figure 6, the new navigation method is closer to the theoretical predetermined navigation AUV track curve.

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。 As stated above, while the invention has been shown and described with reference to certain preferred embodiments, this should not be construed as limiting the invention itself. Various changes in form and details may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.

Claims

1. A navigation method based on strapdown inertial guidance and a Doppler log is characterized in that the navigation method is applied to a navigation system based on the strapdown inertial guidance and the Doppler log, and the navigation system comprises the Doppler log, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log comprises a transceiver, four transducers and an interface unit; the Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit comprises a three-axis gyroscope and a three-axis accelerometer; the strapdown inertial measurement unit is connected with the processing module of the strapdown inertial measurement unit; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises the following steps:

(1) measuring triaxial angular velocity information through a triaxial gyroscope and triaxial acceleration information through an accelerometer in the strapdown inertial measurement unit assembly, receiving navigation information output by the strapdown inertial measurement unit through a processing module of the strapdown inertial measurement unit, and obtaining navigation information such as carrier position, velocity and attitude through navigation integral calculation;

(2) transmitting an electric oscillation signal to the transducer through a transceiver of the Doppler log;

(3) transmitting ultrasonic waves and receiving reflected echoes with frequency shift characteristics through four transducers of a Doppler log;

(4) the receiving system in the transceiver is used for amplifying the echo signal sent by the transducer, obtaining Doppler frequency shift, converting the echo signal into an analog signal of navigational speed and sending the analog signal to the interface unit. If the carrier navigation speed is V, the beam launching depression angle is theta, and the sound velocity C is approximately equal to 1500 m/s, the single-beam frequency shift calculation formula is as follows: Δ f 2Vf₀cos θ/C, the velocity calculation from which is derived is: v ═ Δ fC/(2 f)₀cosθ)；

(5) Converting the four navigational speed analog signals sent by the transceiver into navigational speed by using an interface unit in the transceiver, and outputting the navigational speed analog signals to the central processing unit in a digital mode;

(6) an arithmetic unit in the central processing unit periodically calculates the real-time three-dimensional carrier system speed of the carrier according to the speed information output by the interface unit of the Doppler log;

(7) a filtering module in the central processing unit carries out filtering and platform-fusing calculation on the received strapdown inertial navigation information and the three-dimensional speed information of the Doppler log to obtain the correction value of the inertial strapdown measurement system at the moment kAfter correction, the final high-precision navigation positioning information is obtained;

the method for obtaining the correction value of the inertia strapdown measurement system through filtering fusion calculation comprises the following steps:

taking strapdown inertial navigation as a reference system, and taking a speed error by a state variable XAttitude angle error (phi)_e，φ_n，φ_u) Accelerometer random constant biasAnd gyro random constant drift (epsilon)_x，ε_y，ε_z) And 10 dimensions in total:

W=[w_ax，w_ay，w_gx，w_gy，w_gz，0，0，0，0，0]^Tis a system noise vector;

the difference between the velocity solved by the strapdown inertial navigation and the velocity of the carrier system measured by the Doppler log after being converted into the navigation system is used as the measurement value of the system, and then the measurement equation is expressed as follows:

Z = [\begin{matrix} V_{SINSe}^{n} - C_{b}^{n} V_{DVLe} \\ V_{SINSn}^{n} - C_{b}^{n} V_{DVLn} \end{matrix}] = HX + V

wherein the observation vector isMeasurement noise vector V = [ w = [)_vx，w_vy]^TThe system measurement matrix is H, and a discrete system filter equation can be obtained by discretizing a system state equation and a measurement equation; using initial valuesAnd P₀According to the measurement of time k_kThe state estimate up to time k can be deduced

{\hat{X}}_{k} (k = 1,2,3, . . .) :

K_{k} = P_{k} H_{k}^{T} {(H_{k} P_{k} H_{k}^{T} + I)}^{- 1}

Wherein

Wherein: Λ (k) = diag [ λ₁(k)，λ₂(k)，…λ_n(k)]，λ_i(k) Not less than 1; 1, 2, …, n; in the sub-optimization method, λ is roughly determined based on a priori knowledge of the system₁(k)：λ₂(k)：…：λ_n(k) The initial values of (a) are: lambda [ alpha ]₁(k)：λ₂(k)：…：λ_n(k)=α₁：α₂：…：α_nIn the formula, α_i1(i ═ 1, 2, …, n), then λ is obtained_i(k) The approximation algorithm of (d) is as follows:

<math> <mrow> <msub> <mi>λ</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>α</mi> <mi>i</mi> </msub> <msub> <mi>λ</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>,</mo> </mtd> <mtd> <msub> <mi>α</mi> <mi>i</mi> </msub> <msub> <mi>λ</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> <mo>,</mo> </mtd> <mtd> <msub> <mi>α</mi> <mi>i</mi> </msub> <msub> <mi>λ</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>≤</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

wherein,

tr (A) represents a trace-finding operation for an arbitrary matrix A, in the formulaV_0，kIs calculated by the following formula:

<math> <mrow> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>γ</mi> <mn>1</mn> </msub> <msubsup> <mi>γ</mi> <mn>1</mn> <mi>T</mi> </msubsup> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>=</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>ρV</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>γ</mi> <mi>k</mi> </msub> <msubsup> <mi>γ</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <mi>ρ</mi> </mrow> </mfrac> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

in the above equation, residual errorRho is more than or equal to 0.95 and less than or equal to 1 and is a forgetting factorGenerally, ρ is 0.95; beta.gtoreq.1 is a weakening factor, and generally beta =1 is taken.

2. The strapdown inertial guidance and doppler log-based navigation method of claim 1, wherein the three-axis gyroscope of the strapdown inertial measurement unit is orthogonally mounted in three axes, and the three-axis accelerometer is orthogonally mounted in three axes.

3. The strapdown inertial guidance and doppler velocity log based navigation method of claim 1, wherein the three-axis gyroscope of the strapdown inertial measurement unit is a fiber optic gyroscope.

4. The strapdown inertial guidance and doppler log-based navigation method of claim 1, wherein the three-axis accelerometer is a silicon micro-accelerometer.

5. The strapdown inertial guidance and doppler log based navigation method of claim 1, wherein the transceiver of the doppler log comprises a transmitting system, a receiving system and a computational compensation circuit.

6. The strapdown inertial guidance and doppler log-based navigation method of claim 1, wherein the navigation system further comprises a display, the display is connected to the operator, and the display displays the navigation information obtained by the operation of the cpu.