A SINS / LBL compact combination navigation method and device based on the dual isometric transformation group SE2(3)

Abstract

This invention discloses a SINS / LBL compact integrated navigation method and device based on a dual isometric transformation group SE2(3), which relates to the field of autonomous navigation technology for underwater vehicles. The method includes: acquiring navigation parameters of the inertial navigation system; transforming the inertial navigation mechanical arrangement in the Earth coordinate system using auxiliary gravity and auxiliary velocity to obtain a transformed Earth coordinate system mechanical arrangement; using the navigation parameters as group elements of SE2(3) and defining an invariant error state; performing error modeling on SE2(3) through logarithmic mapping; deriving the system matrix and noise allocation matrix to obtain an invariant error state space model; deriving the transformation relationship between measurement information and the invariant error state to obtain a SINS / LBL compact integrated measurement model; generating sampling points in the error Lie algebra to map to the SE2(3) group and performing nonlinear measurement updates to obtain the updated error state; and feeding this back to the navigation solution results of the inertial navigation system to obtain parameter estimation results. This invention improves the startup efficiency and navigation calculation accuracy of the integrated navigation system.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology for underwater vehicles, and in particular to a SINS / LBL compact combination navigation method and device based on a dual equidistant transformation group SE2(3). Background Technology

[0002] The deep-sea and near-shore operating environments place increasingly higher demands on navigation systems that are high-precision, all-weather, and capable of long-term autonomous calculations. Autonomous Underwater Vehicles (AUVs) and offshore platforms, in tasks such as search, measurement, operation, and platform docking, need to maintain stable positioning under conditions of limited or denied satellite availability, while also achieving rapid readiness and accurate estimation at the initial stages of launch / operation. Traditional navigation methods relying on a single approach cannot simultaneously meet the dual requirements of high precision and high availability. Therefore, combined navigation using multi-source information, such as Strapdown Inertial Navigation Systems (SINS) and underwater acoustic positioning systems like Long Baseline (LBL), has become one of the mainstream directions in the field of marine navigation.

[0003] SINS, with its short-term high bandwidth and inertial measurement capabilities independent of external signals, can provide continuous attitude, velocity, and position estimates on short timescales. LBL, through acoustic reference points deployed on the seabed, can provide external position observations to the vehicle and correct for the cumulative position drift of SINS on long timescales. Combining the two closely allows for direct acoustic measurement constraint of the inertial navigation state at the filtering level, thereby significantly improving positioning accuracy and robustness under weak signal, limited satellite, or complex dynamic conditions. Therefore, the close combination of SINS / LBL is one of the key technological pathways for achieving autonomous and precise navigation at sea, in nearshore, and in the deep sea.

[0004] However, existing SINS / LBL integrated navigation methods often rely on a relatively long static or quasi-static alignment process to reach a high-precision operational state, making it difficult to meet the needs of maritime emergency response or multi-platform parallel operations that require rapid deployment. Furthermore, existing SINS / LBL tightly integrated measurement models typically use first-order Taylor expansion for linearization approximation. However, this linearization introduces systematic modeling errors, thus affecting the final positioning accuracy. Summary of the Invention

[0005] Based on this, it is necessary to provide a SINS / LBL compact combination navigation method and device based on the dual equidistant transformation group SE2(3) to address the above-mentioned technical problems.

[0006] This invention provides a SINS / LBL compact combination navigation method based on the dual isometric transform group SE2(3), comprising:

[0007] Obtain the navigation parameters of the strapdown inertial navigation system (SINS), including the attitude, velocity, and position of the vehicle.

[0008] The inertial navigation mechanical arrangement in the Earth coordinate system is transformed by auxiliary gravity vector and auxiliary velocity to make the inertial navigation mechanical arrangement in the Earth coordinate system satisfy the group affine condition, and the transformed Earth coordinate system mechanical arrangement is obtained; the attitude, velocity and position of the carrier are used as the group elements of the double isometric transformation group SE2(3), the invariant error state is determined by SE2(3), and the invariant error state is projected to the Lie algebra space by logarithmic mapping to obtain the SE2(3) error vector;

[0009] Using the SE2(3) linear error vector as the state variable, and combining it with the mechanical arrangement of the transformed Earth coordinate system to construct the inertial navigation error dynamic model, the system matrix and noise distribution matrix under the affine mechanical arrangement are derived, and the invariant error state space model decoupled from the SINS estimated navigation state is obtained.

[0010] Based on the Time Difference of Arrival (TDOA) principle, a nonlinear measurement equation is established according to the nonlinear coupling relationship between the slant range difference measurement and the carrier position in the long baseline positioning system (LBL). The conversion relationship between measurement information and invariant error state is derived to obtain the SINS / LBL compact combination measurement model.

[0011] The error state of SE2(3) is updated linearly in time based on the invariant error state space model. Based on the SINS / LBL compact combination measurement model, sampling points are generated in the error Lie algebra, mapped to the SE2(3) group, and nonlinear measurement updates are performed to obtain the updated error state.

[0012] The updated error status is fed back to the navigation parameters of SINS to obtain the parameter estimation results of the SINS / LBL tightly integrated navigation system.

[0013] Optionally, the inertial navigation machine arrangement in the Earth coordinate system can be transformed based on the following formula using an auxiliary gravity vector and an auxiliary velocity:

[0014] ;

[0015] ;

[0016] ;

[0017] The mechanical arrangement of the transformed Earth coordinate system is obtained based on the following formula:

[0018] ;

[0019] in, This is the attitude matrix from the carrier coordinate system to the Earth coordinate system. The velocity is in Earth's coordinate system. This is the position vector in the Earth coordinate system. The angular velocity of Earth's rotation. This is the gravity vector within the Earth system. To assist the gravity vector, To assist speed, The angular velocity measured by the gyroscope. The specific force measured by the accelerometer. , , They are respectively , , The differential, b For the carrier coordinate system, e Using Earth coordinate system, i It is a geocentric inertial coordinate system.

[0020] Optionally, the attitude, velocity, and position of the carrier can be used as group elements of the bi-isotral transformation group SE2(3) based on the following equation.

[0021] ;

[0022] ;

[0023] The invariant error state is determined by SE2(3) based on the following formula:

[0024] ;

[0025] ;

[0026] in, This represents the left-invariant state error of SE2(3) under the transformed Earth coordinate system. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the velocity error in Euclidean space. This represents the positional error in Euclidean space. The attitude misalignment angle of the left error model. This represents the zero bias of the gyroscope. Represents the zero bias of the accelerometer. A row vector with zero elements. It is a 3x3 identity matrix. Let SE2(3) represent the navigation parameters. The inverse of the navigation parameters represented by error SE2(3), Let SE2(3) represent the error state. This is a 15-dimensional navigation error state quantity.

[0027] Optionally, based on the following equation, the SE2(3) error vector is used as the state variable, and an inertial navigation error dynamic model is constructed by combining the mechanical arrangement of the transformed Earth coordinate system. The system matrix and noise distribution matrix under the affine mechanical arrangement are then derived:

[0028] ;

[0029] ;

[0030] in, To obtain the angular velocity with errors from the gyroscope, To obtain a specific force with errors using an accelerometer, It is a 3x3 matrix with zero elements. It is a 9x3 matrix with zero elements. It is a 3x3 identity matrix. b For the carrier coordinate system, i It is a geocentric inertial coordinate system;

[0031] The invariant error state-space model decoupled from the SINS estimated navigation state is obtained based on the following equation:

[0032] ;

[0033] in, This represents the system matrix under the definition of left-invariant error. This represents the noise distribution matrix under the definition of left-invariant error. For noise from the gyroscope and accelerometer, for The differential.

[0034] Optionally, based on the Time Difference of Arrival (TDOA) principle, a nonlinear measurement equation is established according to the nonlinear coupling relationship between the slant range difference measurement and the vehicle position in the long baseline positioning system (LBL), specifically including:

[0035] The difference between the slant range difference calculated by SINS and the slant range difference measured by LBL is selected as the measurement value for integrated navigation based on the following formula:

[0036] ;

[0037] in, These are measurements from integrated navigation. The slope distance difference calculated for SINS. The difference in slope distance measured for LBL;

[0038] The nonlinear measurement equation is obtained based on the following formula:

[0039] ;

[0040] in, Indicates the first i The location of each transponder i =1, 2, 3, Indicates the location of the reference transponder. Random noise measured for LBL This indicates the AUV position calculated by SINS. This indicates the actual location of the AUV.

[0041] Optionally, the error state of SE2(3) is updated linearly in time according to the invariant error state-space model, specifically including:

[0042] The mean state is predicted using a linear state transition based on the following formula:

[0043] ;

[0044] Based on the following formula, the covariance is propagated using a linear model under the error state definition of SE2(3):

[0045] ;

[0046] in, for Time's up Predicting the state at any given moment in one step. for Error state estimation at time t. for Time's up Predict the transition matrix in one step based on the state at time t. for The state error covariance matrix at time t. for Time's up The mean square error matrix for predicting the state at time step. For noise driving matrix, Let be the system noise variance matrix.

[0047] Optionally, based on the SINS / LBL compact combination measurement model, sampling points are generated in the error Lie algebra to be mapped to the SE2(3) group and nonlinear measurement updates are performed to obtain the updated error state, specifically including:

[0048] Based on the following formula, 2 is generated from the predicted mean and covariance. n +1 Sigma point:

[0049] ;

[0050] The updated error state is obtained by calculating the nonlinear measurement for each sigma point based on the following formula:

[0051] ;

[0052] in, n Let be the state dimension. The first square root of the covariance matrix i List, For UT transform parameters, For sampling points, for The corresponding nonlinear measurement;

[0053] Based on the following formula, after the single-step filtering operation is completed, the updated error state is fed back to the navigation parameters of SINS, and the parameter estimation results of the SINS / LBL compactly integrated navigation system are obtained:

[0054] ;

[0055] in, This is the attitude matrix from the vehicle coordinate system to the Earth coordinate system after feedback correction. To provide feedback on the corrected velocity in the Earth coordinate system, This is the position vector in the Earth coordinate system after feedback correction. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the left-invariant state error of SE2(3) under the transformed Earth coordinate system. This represents the attitude misalignment angle of the left error model.

[0056] This invention also provides a SINS / LBL compact navigation device based on a double equidistant transform group SE2(3), comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements a SINS / LBL compact navigation method based on a double equidistant transform group SE2(3).

[0057] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements a SINS / LBL compact combination navigation method based on a bi-equidistant transformation group SE2(3).

[0058] This invention also provides a computer program product that, when running on a data storage device, enables the data storage device to implement a SINS / LBL compact combination navigation method based on a bi-equidistant transformation group SE2(3).

[0059] The SINS / LBL compact combination navigation method and apparatus based on the dual isometric transform group SE2(3) provided in this embodiment of the invention have the following advantages compared with the prior art:

[0060] This invention utilizes the inherent invariance of the group affine system to decouple the navigation error dynamics from the specific motion state of the carrier and the initial alignment error. This allows the SINS / LBL tightly integrated navigation system to converge quickly even when there are large errors in the carrier's dynamics or even initial attitude and position, without relying on long-term fine static alignment. This effectively improves the startup efficiency and navigation calculation accuracy of the SINS / LBL tightly integrated navigation system.

[0061] Furthermore, by directly processing nonlinear geometric relationships on the SE2(3) group and performing filtering updates only in the error Lie algebra space, this invention abandons the Taylor linearization approximation of the nonlinear measurement equations, fundamentally avoiding systematic model errors caused by linearization, thereby significantly improving the navigation accuracy and filter estimation consistency of the SINS / LBL compactly integrated navigation system. Attached Figure Description

[0062] Figure 1 A schematic diagram of the framework of a SINS / LBL compact combination navigation method based on the dual isometric transformation group SE2(3) provided in one embodiment;

[0063] Figure 2 This is a schematic diagram of the AUV simulation trajectory and navigation parameters for a SINS / LBL compact combination navigation method based on the double equidistant transform group SE2(3) provided in one embodiment. Figure 2 (a) in the diagram is the simulated motion trajectory of the AUV. Figure 2 (b) in the figure is the AUV simulation attitude change curve. Figure 2 (c) in the figure is the speed variation curve of the AUV simulation. Figure 2 (d) in the figure is the AUV simulation position change curve;

[0064] Figure 3 Pitch error curve of a SINS / LBL compact combination navigation method based on dual isometric transform group SE2(3) provided in one embodiment;

[0065] Figure 4 A roll angle error curve for a SINS / LBL compact combination navigation method based on a dual equidistant transformation group SE2(3) provided in one embodiment;

[0066] Figure 5A heading angle error curve of a SINS / LBL compact combination navigation method based on a dual isometric transformation group SE2(3) provided in one embodiment;

[0067] Figure 6 The figure shows the horizontal position error curve of a SINS / LBL compact combination navigation method based on the dual equidistant transformation group SE2(3) provided in one embodiment. Detailed Implementation

[0068] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0069] Existing SINS / LBL integrated navigation technologies suffer from severe engineering and theoretical bottlenecks. Before initial alignment is completed, SINS errors accumulate at a rapid rate. Traditional methods often rely on lengthy static or quasi-static alignment processes to achieve high-precision operation, resulting in limitations such as "inability to quickly take off / self-position," making it difficult to meet the needs of maritime emergency operations or multi-platform parallel operations requiring rapid deployment. The measurement model in LBL tight integration is inherently highly nonlinear, meaning that acoustic measurements are tightly coupled with the carrier position, inertial navigation errors, and the relative geometry of the base station.

[0070] This invention focuses on SINS / LBL compact integrated navigation and proposes a SINS / LBL compact integrated navigation method based on the dual equidistant transform group SE2(3), which effectively improves system startup efficiency and navigation calculation accuracy. For example... Figure 1 As shown, the method includes:

[0071] The navigation parameters of the strapdown inertial navigation system (SINS) are obtained, including the attitude, velocity, and position of the vehicle. The inertial navigation mechanical arrangement in the Earth coordinate system is transformed by using auxiliary gravity vectors and auxiliary velocities to ensure that the inertial navigation mechanical arrangement in the Earth coordinate system satisfies the group affine condition, thus obtaining the transformed mechanical arrangement in the Earth coordinate system. The attitude, velocity, and position of the vehicle are used as group elements of the bi-isotral transformation group SE2(3). The invariant error state is determined by SE2(3), and the invariant error state is projected onto the Lie algebra space by logarithmic mapping to obtain the SE2(3) error vector.

[0072] Using the SE2(3) error vector as a state variable, and combining it with the mechanical arrangement of the Earth coordinate system transformation, an inertial navigation error dynamic model is constructed. The system matrix and noise distribution matrix under the affine mechanical arrangement are derived, and an invariant error state space model decoupled from the SINS estimated navigation state is obtained.

[0073] Based on the Time Difference of Arrival (TDOA) principle, a nonlinear measurement equation is established according to the nonlinear coupling relationship between slant range difference measurement and carrier position in the long baseline positioning system (LBL), and the conversion relationship between measurement information and invariant error state is derived to obtain the SINS / LBL compact combination measurement model.

[0074] The error state of SE2(3) is updated linearly in time based on the invariant error state space model. Based on the SINS / LBL compact combination measurement model, sampling points are generated in the error Lie algebra, mapped to the SE2(3) group, and nonlinear measurement updates are performed to obtain the updated error state. The updated error state is fed back to the navigation parameters of the strapdown inertial navigation system SINS to obtain the parameter estimation results of the SINS / LBL compact combination navigation system.

[0075] Preferably, the inertial navigation machine arrangement in the Earth coordinate system is transformed based on the following formula using an auxiliary gravity vector and an auxiliary velocity:

[0076] (1)

[0077] (2)

[0078] (3)

[0079] The mechanical arrangement of Earth coordinate system transformations is obtained based on the following formula:

[0080] (4)

[0081] in, This is the attitude matrix from the carrier coordinate system to the Earth coordinate system. The velocity is in Earth's coordinate system. This is the position vector in the Earth coordinate system. The angular velocity of Earth's rotation. This is the gravity vector within the Earth system. To assist the gravity vector, To assist speed, The angular velocity measured by the gyroscope. The specific force measured by the accelerometer. , , They are respectively , , The differential, b For the carrier coordinate system, e Using Earth coordinate system, i It is a geocentric inertial coordinate system.

[0082] Preferably, the attitude, velocity, and position of the carrier are used as group elements of the bi-isotral transformation group SE2(3) based on the following equation:

[0083] (5)

[0084] (6)

[0085] The invariant error state is determined by SE2(3) based on the following formula:

[0086] (7)

[0087] (8)

[0088] in, This represents the left-invariant state error of SE2(3) in the Earth coordinate system. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the velocity error in Euclidean space. This represents the positional error in Euclidean space. The attitude misalignment angle of the left error model. This represents the zero bias of the gyroscope. Represents the zero bias of the accelerometer. A row vector with zero elements. It is a 3x3 identity matrix. Let SE2(3) represent the navigation parameters. The inverse of the navigation parameters represented by error SE2(3), Let SE2(3) represent the error state. This is a 15-dimensional navigation error state quantity.

[0089] Preferably, based on the following formula, the SE2(3) error vector is used as the state variable, and an inertial navigation error dynamic model is constructed by combining the mechanical arrangement of the transformed Earth coordinate system. The system matrix and noise distribution matrix under the affine mechanical arrangement are then derived:

[0090] (9)

[0091] (10)

[0092] in, To obtain the angular velocity with errors from the gyroscope, To obtain a specific force with errors using an accelerometer, It is a 3x3 matrix with zero elements. It is a 9x3 matrix with zero elements. It is a 3x3 identity matrix. b For the carrier coordinate system,i It is a geocentric inertial coordinate system;

[0093] The invariant error state-space model decoupled from the SINS estimated navigation state is obtained based on the following equation:

[0094] (11)

[0095] in, This represents the system matrix under the definition of left-invariant error. This represents the noise distribution matrix under the definition of left-invariant error. For noise from the gyroscope and accelerometer, for The differential.

[0096] Preferably, a nonlinear measurement equation is established based on the nonlinear coupling relationship between the slant range difference measurement and the carrier position in the long baseline positioning system (LBL), specifically including:

[0097] The difference between the slant range difference calculated by the strapdown inertial navigation system (SINS) and the slant range difference measured by the long baseline positioning system (LBL) is selected as the measurement value for integrated navigation based on the following formula:

[0098] (12)

[0099] in, These are measurements from integrated navigation. The slant range difference calculated for the strapdown inertial navigation system (SINS). The slope difference measured by the long baseline positioning system (LBL);

[0100] The nonlinear measurement equation is obtained based on the following formula:

[0101] (13)

[0102] in, For the first i The location of each transponder i =1, 2, 3, For the reference transponder position, Random noise measured for the long baseline positioning system (LBL). The position of the AUV calculated by the strapdown inertial navigation system (SINS). This represents the actual location of the AUV.

[0103] Preferably, the error state of SE2(3) is updated linearly in time according to the invariant error state-space model, specifically including:

[0104] The state estimate is predicted using a linear state transition based on the following formula:

[0105] (14)

[0106] Based on the following formula, the covariance is propagated using a linear model under the error state definition of SE2(3):

[0107] (15)

[0108] in, for Time's up Predicting the state at any given moment in one step. for Error state estimation at time t. for Time's up Predict the transition matrix in one step based on the state at time t. for The state error covariance matrix at time t. for Time's up The mean square error matrix for predicting the state at time step. For noise driving matrix, Let be the system noise variance matrix.

[0109] Preferably, based on the SINS / LBL compact combination measurement model, sampling points are generated in the error Lie algebra to be mapped to the SE2(3) group and nonlinear measurement updates are performed to obtain the updated error state, specifically including:

[0110] Based on the following formula, 2 is generated from the predicted mean and covariance. n +1 Sigma point:

[0111] (16)

[0112] The nonlinear measurement is calculated for each sigma point based on the following formula:

[0113] (17)

[0114] in, n Let be the state dimension. The first square root of the covariance matrix i List, For UT transform parameters, For sampling points, for The corresponding nonlinear measurement.

[0115] Based on the following formula, after the single-step filtering operation is completed, the updated error state is fed back to the navigation parameters of the strapdown inertial navigation system (SINS), and the parameter estimation results of the SINS / LBL tightly integrated navigation system are obtained:

[0116] (18)

[0117] in, This is the attitude matrix from the vehicle coordinate system to the Earth coordinate system after feedback correction. To provide feedback on the corrected velocity in the Earth coordinate system, This is the position vector in the Earth coordinate system after feedback correction. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the left-invariant state error of SE2(3) under the transformed Earth coordinate system. This represents the attitude misalignment angle of the left error model.

[0118] A specific embodiment of the present invention is provided:

[0119] Step 1: Introduce the definition of auxiliary velocity so that the inertial navigation mechanical arrangement after transformation in the Earth coordinate system satisfies the group affine condition; combine the navigation parameters of SINS (carrier attitude, velocity and position) into group elements and give the definition of invariant error on SE2(3); project the group error to the linear error vector of Lie algebra through logarithmic mapping, and incorporate the Euclidean quantity of bias into the error state in an augmented form, and perform error modeling on SE2(3).

[0120] Step 2: Using the error vector in Lie algebra as the state, and combining it with the mechanical arrangement of the transformed Earth coordinate system established in Step 1, derive the dynamic model of inertial navigation error, obtain the system matrix and noise distribution matrix under the affine mechanical arrangement, and use them for state prediction and covariance propagation to establish an invariant error state space model that can be decoupled from the SINS estimation of navigation state.

[0121] Step 3: Based on the Time Difference of Arrival (TDOA) principle, retain the true nonlinear coupling relationship between LBL slant range difference measurement and position measurement, reduce the systematic error caused by linearization approximation, establish the conversion relationship between measurement information and invariant error state, and improve the accuracy of SINS / LBL tightly integrated navigation measurement model.

[0122] Step 4: Time update recursively calculates the mean of the error state using the invariant error state-space model established in Step 2 and propagates the covariance within the Lie algebra; Measurement update uses the measurement model established in Step 3 to generate sampling points in the error Lie algebra, maps them to the group, and completes the update through nonlinear measurement. The update results are then fed back to the inertial navigation system (INS) solution, achieving effective fusion of SINS and LBL data.

[0123] A specific embodiment of the present invention is provided:

[0124] 1. Coordinate system limitation.

[0125] b The system represents the carrier coordinate system. In this invention, the "right front upper" coordinate system is selected. The origin is located at the center or centroid of the carrier. The ox axis points to the right of the carrier's horizontal axis, the oy axis points to the front of the carrier's vertical axis, and the oz axis points to the upward direction of the carrier's vertical axis.

[0126] e The system represents the Earth's coordinate system, with the origin at the Earth's center, the ox axis pointing to the intersection of the prime meridian and the equatorial plane, and the oz axis pointing to the North Pole.

[0127] n The system represents the navigation coordinate system. When the inertial navigation system solves for navigation parameters using gyroscopes and accelerometers, it will... n The system is used as the reference coordinate system. This invention selects the "Northeast Sky" geographic coordinate system as the navigation reference coordinate system for inertial navigation.

[0128] i The system represents the geocentric inertial coordinate system, with the origin located at the center of the Earth, the ox axis pointing to the vernal equinox, the oz axis pointing to the North Pole, and coaxial with the Earth's rotation axis.

[0129] 2. Implementation of the method.

[0130] The implementation method for step one is as follows:

[0131] (1) Establish an inertial navigation mechanical arrangement that satisfies the group affine transformation of the Earth coordinate system.

[0132] The mechanical arrangement of SINS in the navigation coordinate system is strongly coupled, which inevitably leads to the derived inertial navigation error state model relying on estimated navigation parameters. Traditional mechanical arrangement in the Earth coordinate system still does not satisfy the group affine condition. By performing certain transformations on the mechanical arrangement in the Earth coordinate system to make it satisfy the group affine condition, an inertial navigation error state model with state-trajectory independence can be derived.

[0133] Typically, inertial navigation machine orchestration in the Earth coordinate system can be described as follows:

[0134] (19)

[0135] The mechanical arrangement in equation (19) is still not a group affine. To make it satisfy the group affine condition, an auxiliary gravity vector is constructed. and auxiliary speed The two and and The relationship is as follows:

[0136] (20)

[0137] (twenty one)

[0138] The mechanical arrangement after transformation to Earth coordinate system is as follows:

[0139] (twenty two)

[0140] in, This is the attitude matrix from the carrier coordinate system to the Earth coordinate system. The velocity is in Earth's coordinate system. This is the position vector in the Earth coordinate system. The angular velocity of Earth's rotation. This is the gravity vector within the Earth system. To assist the gravity vector, To assist speed, The angular velocity measured by the gyroscope. The specific force measured by the accelerometer. , , They are respectively , , The differential, b For the carrier coordinate system, e Using Earth coordinate system, i It is a geocentric inertial coordinate system.

[0141] (2) Define the navigation error state based on the double equidistant transformation group SE2(3).

[0142] By defining the navigation error state on the dual isometry transformation group SE2(3), the spatial inconsistency problem of traditional error definition can be solved, and the dependence of integrated navigation on initial attitude error can be reduced.

[0143] Incorporating the attitude, velocity, and position in the Earth coordinate system into the SE2(3) group, the Lie group state variables are:

[0144] (twenty three)

[0145] Inverse equation (23):

[0146] (twenty four)

[0147] Considering the definition of left-invariant error under SE2(3): .exist e The left-invariant error state defined under the system can be expressed as:

[0148] (25)

[0149] set up Let be the attitude misalignment angle of the left-invariant error model, then when When taking the minimum value, the error states in equation (25) can be rewritten as follows:

[0150] (26)

[0151] By augmenting the inertial device error into the error state, a left-invariant error state vector can be established:

[0152] (27)

[0153] in, This represents the left-invariant state error of SE2(3) in the Earth coordinate system. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the velocity error in Euclidean space. This represents the positional error in Euclidean space. The attitude misalignment angle of the left error model. This represents the zero bias of the gyroscope. Represents the zero bias of the accelerometer. A row vector with zero elements. It is a 3x3 identity matrix. Let SE2(3) represent the navigation parameters. The inverse of the navigation parameters represented by error SE2(3), Let SE2(3) represent the error state. This is a 15-dimensional navigation error state quantity.

[0154] The implementation method for step two is as follows:

[0155] SINS solution errors accumulate and diverge over time. Accurate error modeling is fundamental to achieving accurate state estimation and filter design. Characterizing the error propagation characteristics using error differential equations derived from inertial navigation mechanical orchestration ensures consistency between the model and the physical nature of the system, and makes the numerical updates of the inertial navigation system well observable.

[0156] Taking the derivative of equation (26), the attitude error differential equation can be expressed as:

[0157] (28)

[0158] The velocity error differential equation can be expressed as:

[0159] (29)

[0160] Substituting equation (26) into equation (29) and rearranging, we get:

[0161] (30)

[0162] The differential equation for position error can be expressed as:

[0163] (31)

[0164] Substituting equation (26) into equation (31) and rearranging, we get:

[0165] (32)

[0166] Equations (28), (30), and (32) are the differential equations of inertial navigation error in the Earth coordinate system based on the left error definition of SE2(3). The corresponding state error system equations are expressed as follows:

[0167] (33)

[0168] in, This represents the system matrix under the definition of left-invariant error. This represents the noise distribution matrix under the definition of left-invariant error. For noise from the gyroscope and accelerometer, for The differential.

[0169] and These can be represented as follows:

[0170] (34)

[0171] (35)

[0172] in, To obtain the angular velocity with errors from the gyroscope, To obtain a specific force with errors using an accelerometer, It is a 3x3 matrix with zero elements. It is a 9x3 matrix with zero elements. It is a 3x3 identity matrix. b For the carrier coordinate system, i It is a geocentric inertial coordinate system.

[0173] The implementation method for step three is as follows:

[0174] Based on the TDOA principle, the SINS / LBL tight combination selects the difference between the slope range difference calculated by SINS and the slope range difference measured by LBL as the navigation measurement value. The slope range difference measured by LBL... It can be modeled as the true value of the slant distance difference plus random white noise, and its expression is:

[0175] (36)

[0176] in, Indicates the sound source and the first i The slant distance of each transponder i =(1,2,3); This represents the slant distance between the sound source and the reference transponder. If the actual AUV location is... A total of four transponders are deployed on the seabed. The locations of each transponder and the reference transponder are as follows: and ,but and satisfy:

[0177] (37)

[0178] Assuming the SINS solution yields the AUV position as follows: Then the slope difference of SINS for:

[0179] (38)

[0180] in, and Represent the sound source estimated by SINS and the first... i One transponder ( i =1,2,3) and the slant distance of the reference transponder.

[0181] When the initial navigation error is large, the traditional Taylor linearization method will cause a significant error. Therefore, this invention directly derives the LBL nonlinear measurement equation. The measurement values ​​for SINS / LBL tightly integrated navigation are:

[0182] (39)

[0183] in, These are measurements from integrated navigation. The slant range difference calculated for the strapdown inertial navigation system (SINS). The slope difference is measured for the long baseline positioning system (LBL). The values ​​of each component are known and can be obtained through calculation.

[0184] Equation (39) can be further written as:

[0185] (40)

[0186] in, For the first i The location of each transponder i =1, 2, 3, For the reference transponder position, Random noise measured for the long baseline positioning system (LBL). The position of the AUV calculated by the strapdown inertial navigation system (SINS). This represents the actual location of the AUV.

[0187] and The relationship can be represented as:

[0188] (41)

[0189] Similarly, substituting equation (26) into equation (41) yields:

[0190] (42)

[0191] Substituting equation (42) into equation (40) yields the SINS / LBL compact combination nonlinear measurement equation under the definition of left-invariant error in the e-system:

[0192] (43)

[0193] The implementation method for step four is as follows:

[0194] This invention designs an SE2(3) error state filtering method based on derived deterministic sampling, which achieves high-precision and fast state estimation of the integrated navigation system state by effectively fusing SINS and LBL information.

[0195] (1) Time update.

[0196] Predicting the state mean using linear state transition:

[0197] (44)

[0198] in, for Time's up Predicting the state at any given moment in one step. for Error state estimation at time t. for Time's up The state at time t is used to predict the transition matrix in one step.

[0199] Under the error state definition of SE2(3), the covariance is propagated using a linear model:

[0200] (45)

[0201] in, for The state error covariance matrix at time t. for Time's up The mean square error matrix for predicting the state at time step. For noise driving matrix, Let be the system noise variance matrix.

[0202] (2) Measurement update.

[0203] Let the state dimension be... n Select UT parameters ,remember Calculate the weights:

[0204] (46)

[0205] in, The mean weighting coefficient is used. This represents the covariance weighting coefficient.

[0206] for i =1,2,…,2 n have .

[0207] 2 is generated from the predicted mean and covariance. n +1 Sigma point:

[0208] (47)

[0209] in, n Let be the state dimension. The first square root of the covariance matrix represents the... i List, These are the UT transformation parameters.

[0210] Calculate the nonlinear measurement for each sigma point:

[0211] (48)

[0212] in, For sampling points, for The corresponding nonlinear measurement.

[0213] Calculate the mean of the predicted measurement With covariance :

[0214] (49)

[0215] (50)

[0216] Calculate the cross-covariance between state prediction and measurement prediction. :

[0217] (51)

[0218] Then calculate the gain. And update status With covariance :

[0219] (52)

[0220] (53)

[0221] (54)

[0222] Initial covariance setting for filtering It can be described as:

[0223] (55)

[0224] in, Let SO(3) be the initial state covariance matrix. It can be represented as:

[0225] (56)

[0226] After the filtering single-step operation is completed, its error state estimate can be fed back to the navigation parameters calculated by SINS, forming a closed-loop correction. Its feedback correction method can be described as follows:

[0227] (57)

[0228] in, This is the attitude matrix from the vehicle coordinate system to the Earth coordinate system after feedback correction. To provide feedback on the corrected velocity in the Earth coordinate system, This is the position vector in the Earth coordinate system after feedback correction. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the left-invariant state error of SE2(3) under the transformed Earth coordinate system. This represents the attitude misalignment angle of the left error model.

[0229] After the feedback correction is completed, the corresponding invariant error state estimate should be reset to zero, i.e. .

[0230] 3. Simulation verification.

[0231] To evaluate the performance of the algorithm proposed in this invention, the following simulation experiments were conducted for verification. In the simulation, the AUV was equipped with a navigation-grade inertial measurement unit (IMU). The gyroscope drift deviation was 0.01° / h, and the noise was 0.001° / √h; the accelerometer drift deviation was 100μg, and the noise was 10μg / √Hz. The IMU update interval was 0.01 s. The LBL measurement error was set to Gaussian white noise with a standard deviation of 1 m and an update interval of 1 s. The initial attitude, velocity, and position of the AUV were set to [0°; 0°; 0°], [0m / s; 0m / s; 0m / s], and [34.034°; 108.775°; -80m], respectively.

[0232] The positions of the four transponders are: [34.075°; 108.754°; -200m], [34.065°; 108.77°; -200m], [34.045°; 108.754°; -200m], and [34.035°; 108.77°; -200m]. The AUV's maneuvering scenario is as follows: The vehicle remains stationary for the first 20 seconds; then accelerates from 0 to 6 m / s within 30 seconds; next, it travels at a constant speed for 310 seconds, followed by a left turn at an angular rate of 2° / s for 45 seconds; then continues at a constant speed for 310 seconds, followed by a right turn at an angular rate of 2° / s for 45 seconds; then travels at a constant speed again for 310 seconds; finally, it decelerates to a stop within the last 30 seconds. The total simulation time is 1200 seconds, and the total travel distance is 6300 m. Figure 2 The simulation trajectory and motion parameter information of the AUV are given. Figure 2 (a) in the diagram is the simulated motion trajectory of the AUV. Figure 2 (b) in the figure is the AUV simulation attitude change curve. Figure 2 (c) in the figure is the speed variation curve of the AUV simulation. Figure 2 (d) in the figure represents the position change curve of the AUV simulation. The initial attitude error of the filter is set to [3°; 3°; 20°]; the initial velocity error and position error are set to [0.1m / s; 0.1m / s; 0.1m / s] and [1m; 1m; 1m], respectively; the initial error state estimate is set to 0. 15×1 .

[0233] Figure 3 , Figure 4 , Figure 5 and Figure 6The following curves show the changes in attitude and position root mean square errors over time for three algorithms under the same simulation conditions: the traditional SINS / LBL compact combination (SO-TKF), the SINS / LBL compact combination based on right-invariant error (RSE-TKF), and the proposed algorithm based on left-invariant error (LSE-TKF). It can be seen that LSE-TKF, defined using left-invariant error SE2(3), exhibits faster convergence speed and lower steady-state error in both attitude and position. In contrast, the traditional SO-TKF and the RSE-TKF methods defined by right-invariant error converge slowly in the initial stage of filtering and have significantly larger steady-state errors. The main reasons for this result are twofold: First, the invariant error representation based on SE2(3) and the affine error space model obtained therefrom effectively decouple the error dynamics from the estimated navigation parameters, reducing the structural bias introduced by the mutual coupling between the state and the estimator, thereby improving the numerical stability and initial convergence of the filter; Second, the measurement adopts a nonlinear model that preserves geometric coupling and uses deterministic sampling to capture high-order nonlinear information during the update, which significantly reduces the model mismatch caused by linearization approximation, making the measurement update more accurate, and thus improving the final navigation accuracy.

[0234] The present invention also discloses a SINS / LBL compact navigation device based on a bi-equidistant transform group SE2(3), including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps in an embodiment of a SINS / LBL compact navigation method based on a bi-equidistant transform group SE2(3).

[0235] The present invention also discloses an embodiment that provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps in an embodiment of a SINS / LBL compact combination navigation method based on a bi-equidistant transformation group SE2(3).

[0236] The present invention also provides a computer program product that, when running on a data storage device, enables the data storage device to implement the steps in an embodiment of a SINS / LBL compact combination navigation method based on a bi-equidistant transformation group SE2(3).

[0237] If the integrated unit module is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of the present invention can be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include at least: any entity or device capable of carrying computer program code to a storage device, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. Examples include USB flash drives, portable hard drives, magnetic disks, or optical disks.

[0238] The embodiments described above are merely examples of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention.

Claims

1. A SINS / LBL compact combination navigation method based on the dual isometric transform group SE2(3), characterized in that, include: Obtain the navigation parameters of the strapdown inertial navigation system (SINS), including the attitude, velocity, and position of the vehicle. The inertial navigation mechanical arrangement in the Earth coordinate system is transformed by auxiliary gravity vector and auxiliary velocity to make the inertial navigation mechanical arrangement in the Earth coordinate system satisfy the group affine condition, and the Earth coordinate system transformation mechanical arrangement is obtained; the attitude, velocity and position of the carrier are taken as the group elements of the double isometric transformation group SE2(3), the invariant error state is determined by SE2(3), and the invariant error state is projected to the Lie algebra space by logarithmic mapping to obtain the SE2(3) error vector; Using the SE2(3) error vector as a state variable, and combining it with the mechanical arrangement of the Earth transformation coordinate system, an inertial navigation error dynamic model is constructed. The system matrix and noise distribution matrix under the affine mechanical arrangement are derived, and an invariant error state space model decoupled from the SINS estimated navigation state is obtained. Based on the Time Difference of Arrival (TDOA) principle, a nonlinear measurement equation is established according to the nonlinear coupling relationship between the slant range difference measurement and the carrier position in the long baseline positioning system (LBL). The conversion relationship between measurement information and invariant error state is derived to obtain the SINS / LBL compact combination measurement model. The error state of SE2(3) is updated linearly in time based on the invariant error state space model. Based on the SINS / LBL compact combination measurement model, sampling points are generated in the error Lie algebra, mapped to the SE2(3) group, and nonlinear measurement updates are performed to obtain the updated error state. The updated error status is fed back to the navigation parameters of SINS to obtain the parameter estimation results of the SINS / LBL tightly integrated navigation system.

2. The SINS / LBL compact combination navigation method based on the dual isometric transform group SE2(3) as described in claim 1, characterized in that, The following formula is used to transform the inertial navigation machine arrangement in the Earth coordinate system using auxiliary gravity vector and auxiliary velocity: ； ； ； The mechanical arrangement of Earth coordinate system transformations is obtained based on the following formula: ； in, This is the attitude matrix from the carrier coordinate system to the Earth coordinate system. The velocity is in Earth's coordinate system. This is the position vector in the Earth coordinate system. The angular velocity of Earth's rotation. This is the gravity vector within the Earth system. To assist the gravity vector, To assist speed, The angular velocity measured by the gyroscope. The specific force measured by the accelerometer. , , They are respectively , , The differential, b For the carrier coordinate system, e Using Earth coordinate system, i It is a geocentric inertial coordinate system.

3. The SINS / LBL compact combination navigation method based on the dual isometric transform group SE2(3) as described in claim 2, characterized in that, The attitude, velocity, and position of the carrier are taken as group elements of the bi-isotral transformation group SE2(3) based on the following equation: ； ； The invariant error state is determined by SE2(3) based on the following formula: ； ； in, This represents the left-invariant state error of SE2(3) in the Earth coordinate system. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the velocity error in Euclidean space. Represents the position error in Euclidean space, and is the attitude misalignment angle of the left error model. This represents the zero bias of the gyroscope. Represents the zero bias of the accelerometer. A row vector with zero elements. It is a 3x3 identity matrix. Let SE2(3) represent the navigation parameters. The inverse of the navigation parameters represented by error SE2(3), Let SE2(3) represent the error state. This is a 15-dimensional navigation error state quantity.

4. The SINS / LBL compact combination navigation method based on the dual isometric transformation group SE2(3) as described in claim 1, characterized in that, Based on the following formula, the SE2(3) error vector is used as the state variable. Combined with the mechanical arrangement of the Earth coordinate system transformation, an inertial navigation error dynamic model is constructed, and the error state transition matrix and noise distribution matrix under the affine mechanical arrangement are derived: ； ； in, To obtain the angular velocity with errors from the gyroscope, To obtain a specific force with errors using an accelerometer, It is a 3x3 matrix with zero elements. It is a 9x3 matrix with zero elements. It is a 3x3 identity matrix. b For the carrier coordinate system, i It is a geocentric inertial coordinate system; The invariant error state-space model decoupled from the SINS estimated navigation state is obtained based on the following equation: ； in, This represents the system matrix under the definition of left-invariant error. This represents the noise distribution matrix under the definition of left-invariant error. For noise from the gyroscope and accelerometer, for The differential.

5. The SINS / LBL compact combination navigation method based on the dual isometric transform group SE2(3) as described in claim 1, characterized in that, The nonlinear measurement equation, based on the Time Difference of Arrival (TDOA) principle and the nonlinear coupling relationship between slant range difference measurement and vehicle position in a long baseline positioning system (LBL), is established, specifically including: The difference between the slant range difference calculated by SINS and the slant range difference measured by LBL is selected as the measurement value for integrated navigation based on the following formula: ； in, These are measurements from integrated navigation. The slope distance difference calculated for SINS. The difference in slope distance measured for LBL; The nonlinear measurement equation is obtained based on the following formula: ； in, For the first i The location of each transponder i =1, 2, 3, For the reference transponder position, Random noise measured for LBL The AUV position calculated by SINS. This represents the actual location of the AUV.

6. The SINS / LBL compact combination navigation method based on the dual isometric transform group SE2(3) as described in claim 1, characterized in that, The linear-time update of the error state of SE2(3) based on the invariant error state-space model specifically includes: The state estimate is predicted using a linear state transition based on the following formula: ； Based on the following formula, the covariance is propagated using a linear model under the error state definition of SE2(3): ； in, for Time's up Predicting the state at any given moment in one step. for Error state estimation at time t. for Time's up Predict the transition matrix in one step based on the state at time t. for The state error covariance matrix at time t. for Time's up The mean square error matrix for predicting the state at time step. For noise driving matrix, Let be the system noise variance matrix.

7. The SINS / LBL compact combination navigation method based on the dual isometric transformation group SE2(3) as described in claim 6, characterized in that, The SINS / LBL compact combination measurement model generates sampling points in the error Lie algebra, maps them to the SE2(3) group, and performs nonlinear measurement updates to obtain the updated error state, specifically including: Based on the following formula, 2 is generated from the predicted mean and covariance. n +1 sigma point: ； The nonlinear measurement is calculated for each sigma point based on the following formula: ； in, n Let be the state dimension. The first square root of the covariance matrix i List, For UT transform parameters, For sampling points, for The corresponding nonlinear measurement; Based on the following formula, after the single-step filtering operation is completed, the updated error state is fed back to the navigation parameters of SINS, and the parameter estimation results of the SINS / LBL compactly integrated navigation system are obtained: ； in, This is the attitude matrix from the vehicle coordinate system to the Earth coordinate system after feedback correction. To provide feedback on the corrected velocity in the Earth coordinate system, This is the position vector in the Earth coordinate system after feedback correction. These represent the estimated values ​​of the direction cosine matrix, velocity, and position, respectively. This represents the left-invariant state error of SE2(3) under the transformed Earth coordinate system. This represents the attitude misalignment angle of the left error model.

8. A SINS / LBL compact combination navigation device based on the double equidistant transformation group SE2(3), characterized in that, include: The memory, the processor, and the computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the SINS / LBL compact combination navigation method based on the double equidistant transformation group SE2(3) as described in any one of claims 1-7.

9. A computer-readable storage medium storing a computer program that, when executed by a processor, implements the SINS / LBL compact combination navigation method based on the double equidistant transformation group SE2(3) as described in any one of claims 1-7.

10. A computer program product, when running on a data storage device, enables the data storage device to implement the SINS / LBL compact combination navigation method based on the double equidistant transformation group SE2(3) as described in any one of claims 1-7.

Images