A SINS / DVL integrated navigation backtracking initial alignment method and device based on Lie group SE3(3)

Abstract

The application discloses a SINS / DVL combined navigation backtracking initial alignment method and device based on a Lie group SE3(3), and relates to the technical field of inertial-based navigation initial alignment. The SINS / DVL dead reckoning model is established in an initial navigation coordinate system; a DVL dead reckoning position obtained by the SINS / DVL dead reckoning model in the initial navigation coordinate system is taken as an element of the Lie group SE3(3), and a right-invariant error state space model and a measurement model based on the Lie group SE3(3) are constructed; Kalman filtering state estimation is performed by using the right-invariant error state space model and the measurement model, so as to determine a navigation error in an inertial navigation solution process; the navigation error is fed back to navigation parameters of the inertial navigation solution, and updated navigation parameters are obtained; and a time-varying attitude matrix corresponding to the updated navigation parameters is determined, so that the initial alignment process of the SINS / DVL combined navigation is completed. The application can solve the problem of fast and high-precision initial alignment under a large misalignment angle condition, and effectively suppresses the divergence of the position error under the assistance of the DVL speed.

Description

Technical Field

[0001] This invention relates to the field of initial alignment technology for inertial-based navigation, and particularly to a retrospective initial alignment method and apparatus for SINS / DVL integrated navigation based on Lie group SE3(3). Background Technology

[0002] Oceans cover over 70% of the Earth's surface and contain abundant, undiscovered resources. To exploit these valuable resources, autonomous underwater vehicles (AUVs) have become indispensable core equipment, receiving high attention in both military and civilian fields. Among the various navigation schemes for AUVs, the SINS / DVL integrated navigation system, consisting of a strapdown inertial navigation system (SINS) and a Doppler velocity log (DVL), has been widely used due to its high accuracy, low cost, and strong anti-interference capabilities.

[0003] As a dead reckoning system, the ability of SINS to quickly and accurately complete initial alignment and obtain a reliable initial navigation state before commencing operations directly impacts the overall system's performance and accuracy. A key challenge in initial alignment technology lies in balancing alignment accuracy and speed. Traditional alignment processes typically consist of two stages: "coarse alignment" and "fine alignment." The coarse alignment stage utilizes static measurements from inertial sensors (gyroscopes, accelerometers) to rapidly estimate a limited-precision attitude matrix within seconds through analytical calculations or simple algorithms. The fine alignment stage, based on the initial attitude provided by coarse alignment, models the SINS error as a state variable and uses optimal estimation methods such as Kalman filtering for iterative optimization estimation. This process takes tens of seconds to several minutes, ultimately yielding a high-precision attitude matrix (with errors reaching the arcminute or even arcsecond level).

[0004] However, under the moving base, it is difficult to obtain accurate attitude information during the coarse alignment stage, which makes it impossible for the subsequent fine alignment stage to meet the small angle error assumption required, resulting in a decrease in overall alignment accuracy; and using the DVL carrier velocity as measurement information has the problem of position estimation error accumulating and diverging over time. Summary of the Invention

[0005] Based on this, it is necessary to provide a backtracking initial alignment method and device for SINS / DVL integrated navigation based on Lie group SE3(3) to address the above-mentioned technical problems.

[0006] This invention provides a backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3), comprising:

[0007] Gyroscope and accelerometer data are acquired through the inertial measurement unit, and the velocity of the carrier coordinate system is obtained through DVL.

[0008] In the initial navigation coordinate system, an inertial navigation mechanical arrangement equation with group affine properties is constructed, and the gyroscope data and accelerometer data are solved into navigation parameters estimated by the inertial navigation system. Based on the navigation parameters estimated by the inertial navigation system and the system velocity of the carrier measured by DVL, the differential equation of DVL dead reckoning position is determined, and the SINS / DVL dead reckoning model under the initial navigation coordinate system is obtained.

[0009] The DVL dead reckoning position obtained from the SINS / DVL dead reckoning model under the initial navigation coordinate system is used as an element of the Lie group SE3(3) to obtain the right-invariant error state space model based on the state representation of the Lie group SE3(3) under the initial navigation coordinate system; the measurement value of the SINS / DVL dead reckoning position is derived from the right-invariant error state space model to obtain the measurement model based on the Lie group SE3(3);

[0010] Kalman filtering state estimation is performed using the right-invariant error state space model and the measurement model to determine the navigation error in the inertial navigation solution process;

[0011] The navigation error is fed back to the navigation parameters calculated by the inertial navigation system to estimate the initial attitude matrix in each forward navigation process, and the updated navigation parameters are obtained. Based on the initial attitude matrix optimized by the forward navigation process, backtracking is performed to redetermine the initial attitude matrix calculated by the inertial navigation system and the measurement values ​​of the SINS / DVL dead reckoning position for the next forward navigation process, and the time-varying attitude matrix corresponding to the updated navigation parameters is determined to complete the initial alignment process of the SINS / DVL integrated navigation.

[0012] Optionally, an inertial navigation mechanical arrangement equation with group affine properties is constructed in the initial navigation coordinate system based on the following equation:

[0013] ;

[0014] ;

[0015] in, The initial navigation coordinate system is the carrier attitude matrix. This refers to the angular rate output by the gyroscope. The initial navigation coordinate system vehicle velocity, The initial navigation coordinate system is the position of the carrier. , , They are respectively , , The differential, The specific force output by the accelerometer. This is the local gravitational acceleration vector. This is the projection of the Earth's rotational angular velocity onto the initial navigation coordinate system. The initial navigation coordinate system position vector is the one pointing from the origin of the inertial frame to the origin of the vehicle frame. For universal gravitation, n 0 is the initial navigation coordinate system. b For the carrier coordinate system, i It is a geocentric inertial coordinate system;

[0016] Based on the following equation, the differential equation for DVL dead reckoning position is determined using the navigation parameters estimated by the inertial navigation system and the system velocity measured by DVL, thus obtaining the SINS / DVL dead reckoning model in the initial navigation coordinate system:

[0017] ;

[0018] in, For DVL dead reckoning, calculate the position vector. This is the DVL velocity vector.

[0019] Optionally, the DVL dead reckoning position obtained from the SINS / DVL dead reckoning model in the initial navigation coordinate system is used as an element of the Lie group SE3(3) based on the following formula:

[0020] ;

[0021] The right-invariant state error of the Lie group SE3(3) is determined based on the following formula:

[0022] ;

[0023] in, The attitude error state is the right-invariant error state space of the Lie group SE3(3). For the velocity error state in the right-invariant error state space of the Lie group SE3(3), The position error state of the right-invariant error state space of the Lie group SE3(3). The DVL dead reckoning error state is the right-invariant error state space of the Lie group SE3(3). A row vector with zero elements. Let SE3(3) represent the navigation parameters. The inverse of the navigation parameters represented by error SE3(3), Let SE3(3) represent the error state;

[0024] Based on the Lie group error definition, the right-invariant error state-space model based on the Lie group SE3(3) state representation in the initial navigation coordinate system is obtained according to the following equation:

[0025] ;

[0026] ;

[0027] ;

[0028] ;

[0029] in, for n 18-dimensional error states in the 0 series For the system matrix, Assign a matrix to the noise. Let be the attitude misalignment angle in the right-invariant error state space of the Lie group. For zero bias of the gyroscope, For zero bias of the accelerometer, For noise from the gyroscope and accelerometer, It is a 3x3 matrix with zero elements. It is a 3x3 identity matrix. , , , , They are respectively , , , , The estimated value;

[0030] Based on the following formula, the measurement values ​​of the SINS / DVL dead reckoning position are derived from the right-invariant error state-space model, resulting in a measurement model based on the Lie group SE3(3):

[0031] ;

[0032] ;

[0033] in, Measurements for SINS / DVL dead reckoning positions. For the measurement matrix, for The error between the actual position vector and the initial navigation coordinate system for Error between the initial navigation coordinate system and the true position vector.

[0034] Optionally, when using the right-invariant error state-space model and the measurement model for Kalman filtering state estimation, the initial error state covariance matrix of the Lie group SE3(3) is set based on the following equation:

[0035] ;

[0036] ;

[0037] in, Let the initial error state covariance matrix be... To calculate the initial velocity for the inertial navigation system in the initial navigation coordinate system, The inertial navigation system (INS) calculates the position in the initial navigation coordinate system. Initial values ​​for DVL dead reckoning in the initial navigation coordinate system. Let be the initial error state covariance matrix defined in Euclidean space. The transformation matrix is ​​the initial error state covariance matrix. for Transpose of;

[0038] Kalman filter state estimation is performed using the right-invariant error state-space model and the measurement model, based on the following formula:

[0039] ;

[0040] After feedback correction, the error state estimate of the Lie group SE3(3) is reset based on the following formula:

[0041] ;

[0042] in, Estimation of linear Kalman filters k Attitude error at any moment Estimation of linear Kalman filters k Momental velocity error Estimation of linear Kalman filters k Time and position error, Estimation of linear Kalman filters k Error in DVL dead reckoning position at any given time. To correct and compensate the initial navigation coordinate system carrier attitude matrix, To correct the vehicle velocity in the initial navigation coordinate system after compensation, To correct the position of the carrier in the initial navigation coordinate system after compensation, To correct and compensate the DVL dead reckoning position vector, for The first to 12th elements, It is a column vector with zero elements.

[0043] Optionally, the navigation error is fed back to the navigation parameters calculated by the inertial navigation system to estimate the initial attitude matrix in each forward navigation process, thereby obtaining updated navigation parameters, specifically including:

[0044] In the forward-backward alignment strategy, the time-varying attitude matrix is ​​decomposed based on the following formula:

[0045] ;

[0046] in, For time-varying attitude matrix, The initial attitude matrix, This is the attitude matrix from the initial navigation coordinate system to the current navigation coordinate system. This is the attitude matrix from the current carrier coordinate system to the initial carrier coordinate system;

[0047] Based on the right-invariant initial alignment strategy of the Lie group SE3(3) in the initial navigation coordinate system, and based on the initial attitude matrix iteratively optimized by the previous backtracking alignment result, the inertial navigation solution attitude matrix and the measurement values ​​of the SINS / DVL dead reckoning position in the next backtracking alignment are re-determined.

[0048] This invention also provides a backtracking initial alignment device for SINS / DVL integrated navigation based on Lie group SE3(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 backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3).

[0049] This invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements a backtracking initial alignment method for SINS / DVL integrated navigation based on the Lie group SE3(3).

[0050] This invention also provides a computer program product that, when running on a data storage device, enables the data storage device to implement a backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3).

[0051] The SINS / DVL integrated navigation retrospective initial alignment method and apparatus based on Lie group SE3(3) provided in this embodiment of the invention have the following advantages compared with the prior art:

[0052] This invention first establishes a SINS / DVL dead reckoning model in the initial navigation coordinate system. Using the Lie group SE3(3) theory, the carrier attitude, velocity, position, and DVL dead reckoning position are uniformly represented as Lie group states. A measurement model is constructed based on the difference between the SINS position and the DVL dead reckoning position. This modeling approach, on the one hand, eliminates the need for the traditional small-angle linearization assumption, enabling direct handling of state estimation under large misalignment angles; on the other hand, by introducing the dead reckoning position difference as observation information, it can more effectively constrain the system's accumulated error compared to simple velocity assistance, thus significantly suppressing the position error divergence problem under DVL velocity assistance.

[0053] Based on this, the present invention deeply integrates the aforementioned error state space model based on the Lie group SE3(3) into the forward-backward alignment strategy. This strategy utilizes the same set of initial alignment data to perform forward navigation calculations and Kalman filtering iterations multiple times, achieving in-depth mining and reuse of data with a limited duration. Through this fusion mechanism, the present invention can further accelerate the filtering convergence process, ultimately achieving fast and high-precision initial alignment under large misalignment angle conditions. Attached Figure Description

[0054] Figure 1 This is a schematic diagram illustrating the working principle of a backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3) provided in one embodiment;

[0055] Figure 2 A diagram showing the actual attitude change of a ship in a backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3) provided in one embodiment;

[0056] Figure 3 A graph showing the actual speed variation of a ship in a backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3) provided in one embodiment;

[0057] Figure 4 A comparison of pitch angle alignment results between a SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) and an error state Kalman filter algorithm provided in one embodiment;

[0058] Figure 5 A comparison of roll angle alignment results between a SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) and an error state Kalman filter algorithm provided in one embodiment;

[0059] Figure 6A comparison of the heading angle alignment results of a SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) and an error state Kalman filter algorithm provided in one embodiment;

[0060] Figure 7 A comparison of position error results between a backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3) and an error state Kalman filter algorithm provided in one embodiment. Detailed Implementation

[0061] 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.

[0062] While coarse and fine alignment modes are widely used in inertial-based integrated navigation systems (SINS), coarse alignment often fails to provide sufficiently accurate initial attitude for fine alignment in complex marine environments. Forward backtracking methods improve alignment speed by reusing sensor data from the forward navigation process; however, under moving base conditions, this method often fails due to the inability to accurately obtain attitude angles, leading to the failure of fine alignment models based on small-angle assumptions. Furthermore, existing initial alignment methods using DVL velocities as measurements generally suffer from the problem of position estimation errors accumulating and diverging over time. Therefore, developing a fast, high-precision initial alignment method that can adapt to large misalignment angles and suppress position error divergence is a pressing technical challenge in the current SINS / DVL integrated navigation field.

[0063] This invention uses DVL dead reckoning position instead of DVL carrier velocity as the measurement information for the SINS / DVL integrated system. Within the basic framework of forward backtracking alignment, it employs Lie groups and Lie algebras as mathematical tools. By constructing a system error model based on Lie group states, it proposes a backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3), aiming to solve the problem of rapid and high-precision initial alignment of the SINS / DVL integrated system under large misalignment angles. Figure 1 As shown, the method includes:

[0064] Gyroscope and accelerometer data are acquired through the inertial measurement unit, and the velocity of the carrier coordinate system is obtained through DVL.

[0065] An inertial navigation mechanical arrangement equation with group affine properties is constructed in the initial navigation coordinate system, and gyroscope and accelerometer data are solved into navigation parameters estimated by the inertial navigation system. Based on the navigation parameters estimated by the inertial navigation system and the system velocity measured by DVL, the differential equation for DVL dead reckoning position is determined, resulting in the SINS / DVL dead reckoning model in the initial navigation coordinate system.

[0066] The DVL dead reckoning position obtained from the SINS / DVL dead reckoning model in the initial navigation coordinate system is used as an element of the Lie group SE3(3) to obtain a right-invariant error state-space model based on the state representation of the Lie group SE3(3) in the initial navigation coordinate system. The measurement value of the SINS / DVL dead reckoning position is derived from the right-invariant error state-space model to obtain a measurement model based on the Lie group SE3(3).

[0067] Kalman filtering state estimation is performed using a right-invariant error state-space model and a measurement model to determine the navigation error in the inertial navigation solution process.

[0068] The navigation error is fed back to the navigation parameters calculated by the inertial navigation system (INS) to estimate the initial attitude matrix in each forward navigation process, thus obtaining the updated navigation parameters. Based on the initial attitude matrix iteratively optimized during the forward navigation process, a backtracking process is performed to redetermine the initial attitude matrix calculated by the INS and the measured values ​​of the SINS / DVL dead reckoning position for the next forward navigation process. The time-varying attitude matrix corresponding to the updated navigation parameters is then determined to complete the initial alignment process of the SINS / DVL integrated navigation system.

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

[0070] Step 1: Construct an inertial navigation mechanical arrangement with group affine properties in the initial navigation coordinate system, and derive the SINS / DVL dead reckoning model based on this.

[0071] Step 2: Use the DVL dead reckoning position vector as an element of the Lie group SE3(3) to construct... n The right-invariant error state space model RSE-n0-DR based on the state representation of the Lie group SE3(3) under the 0 system is constructed. The measurement of the Kalman filter is calculated based on the difference between the SINS position and the DVL dead reckoning position, and the measurement model based on the error definition of SE3(3) is constructed.

[0072] Step 3: Use the constructed right-invariant error state space model based on the Lie group SE3(3) state representation and the measurement model to perform Kalman filtering update, and obtain the inertial navigation solution attitude matrix after feedback correction.

[0073] Step 4: Use the time-invariant attitude angles obtained by Kalman filtering (initial attitude matrix) To approximate the estimation of time-varying attitude angles Furthermore, by repeatedly performing forward navigation calculations using the same set of initial alignment data, high-precision attitude estimation can be obtained.

[0074] Another specific embodiment of the present invention is provided:

[0075] 1. Coordinate system limitation.

[0076] 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.

[0077] 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.

[0078] 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.

[0079] n The 0-frame represents the initial navigation coordinate system, which is established by "fixing" the initial navigation coordinate system in inertial space.

[0080] e The 0-frame represents the initial Earth coordinate system, which is established by "fixing" the Earth coordinate system at the initial moment in inertial space.

[0081] b The 0-frame represents the initial carrier coordinate system, which is established by "fixing" the carrier coordinate system at the initial moment in inertial space.

[0082] 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.

[0083] 2. Implementation of the method.

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

[0085] (1) n 0-series affine inertial navigation mechanical arrangement.

[0086] Tradition The mechanical arrangement of the system is given by the following formula:

[0087] (1)

[0088] in, The initial navigation coordinate system is the carrier attitude matrix. This refers to the angular rate output by the gyroscope. For the ground velocity vector in n Projection in the 0 series, The specific force output by the accelerometer. This is the projection of the Earth's rotational angular velocity onto the initial navigation coordinate system. For universal gravitation, for n 0 series relative to e The angular velocity of the system is n 0 series projection, for e The system position vector is in n Projection in the 0 series.

[0089] The SINS error model in Equation (1) suffers from internal state coupling, causing traditional linear alignment methods to fail when faced with large misalignment angles. In contrast, methods based on Lie groups and Lie algebras can construct an error model independent of the system trajectory by building a group affine system, which is the key advantage in solving the large misalignment angle problem. The current model, Equation (1), does not satisfy the group affine property. Therefore, in order to utilize Lie groups and Lie algebras to construct a state-independent error model, a new model satisfying the group affine property will be constructed. n 0-series inertial navigation mechanical arrangement.

[0090] for n 0-series velocity differential equations n The velocity vector of a 0-series carrier is defined as follows: b System relative to n The velocity vector of the 0 system is in n The projection in the 0-frame represents the vehicle velocity in the initial navigation coordinate system. . n The 0-series carrier speed can be further broken down, among which... i The system is an intermediate transition coordinate system:

[0091] (2)

[0092] In the formula, because n 0 series and i Since the system is relatively stationary, i System relative to n The speed of the 0 series is n 0 series projection . express b System relative to i The velocity vector of the system is nThe projection under the 0 series will be derived below.

[0093] According to the principle of velocity superposition, the absolute velocity of any instantaneous moving point is equal to the vector sum of its entrainment velocity and relative velocity. Therefore, the absolute velocity of the moving point can be expressed by the following formula:

[0094] (3)

[0095] Among them, absolute speed It is the velocity of that point relative to the static coordinate system; relative velocity. It is the velocity relative to the moving coordinate system; the entrainment velocity. It is the velocity of the moving coordinate system relative to the stationary coordinate system.

[0096] In a strapdown inertial navigation system, the vehicle can be considered as a moving point, and an inertial frame can be selected. i The system is used as a static coordinate system, while the local navigation coordinate system is used as a static coordinate system. n The system is a moving coordinate system. Therefore, the entrainment velocity is... n Relative i The velocity of the system is caused by the Earth's rotation, that is... n System relative to i The system is undergoing uniform rotation about a fixed axis. According to the velocity composition theorem, the absolute velocity of the carrier can be derived as follows:

[0097] (4)

[0098] in, yes i Tie n Rotation matrix of the 0 system; It is projected on i Earth's rotational angular velocity under the system; It is the position vector pointing from the origin of the inertial frame to the origin of the carrier frame, and its magnitude is the radius of the Earth's circumpolar orbit. The relative velocity of the carrier is i The projection of the system; The initial navigation coordinate system position vector is the one pointing from the origin of the inertial frame to the origin of the carrier frame. The relative velocity of the carrier is n Projection of the 0 series.

[0099] Substituting equation (4) into equation (2) yields... n The download speed of the 0 series is given by the following formula:

[0100] (5)

[0101] Differentiating both sides of the above equation, we get n 0-series carrier velocity differential equation:

[0102] (6)

[0103] Next derivation and The expression. The derivation is given by the following formula:

[0104] (7)

[0105] in, The initial navigation coordinate system position vector points from the origin of the carrier system to the origin of the Earth system; The initial navigation coordinate system position vector points from the origin of the Earth system to the origin of the carrier system. Because... e System and i The origins of the systems coincide, so . Further simplification yields the following formula:

[0106] (8)

[0107] It can be given by the following formula:

[0108] (9)

[0109] In the formula, yes n Tie n Rotation matrix of the 0 system; The relative velocity of the carrier is n The projection of the system. Simplifying, we get:

[0110] (10)

[0111] Substituting equations (8) and (10) into equation (6), we can obtain... n The velocity differential equation for the 0-series carrier is as follows:

[0112] (11)

[0113] in, The local gravitational acceleration vector is composed of centripetal acceleration. With gravity It consists of two parts.

[0114] Next derivation n The differential equation for the position vector in the 0-system. (and) n Different latitude and longitude coordinates under different systems define... n 0 series origin point n The vector of the origin is n 0-series position vector . It can be broken down into and Two vectors sum. It is a fixed value, representing n 0 series origin point i The position vector of the origin is in n The projection of the 0 series has a magnitude equal to the radius of the Earth's zonal circle. The formula for calculating it is given by the following equation:

[0115] (12)

[0116] in, Let be the rotation matrix from the initial Earth frame to the initial navigation frame. for e 0 series origin point b The position vector of the origin of the 0 series is in e Projection of the 0 series.

[0117] Further derivation is possible n Differential equation of the position vector in the 0 system:

[0118] (13)

[0119] It can be seen from equations (8) and (5) that:

[0120] (14)

[0121] Substituting equation (14) into equation (13) yields the result. n The differential equation for the position vector in system 0 is given by the following equation:

[0122] (15)

[0123] In conclusion, n The mechanical arrangement model of the 0-series affine inertial navigation system can be given by the following formula:

[0124] (16)

[0125] in, The initial navigation coordinate system is the carrier attitude matrix. This refers to the angular rate output by the gyroscope. The initial navigation coordinate system vehicle velocity, The initial navigation coordinate system is the position of the carrier. , , They are respectively , , The differential, The specific force output by the accelerometer. This is the local gravitational acceleration vector. This is the projection of the Earth's rotational angular velocity onto the initial navigation coordinate system. The initial navigation coordinate system position vector is the one pointing from the origin of the inertial frame to the origin of the vehicle frame. For universal gravitation, n 0 is the initial navigation coordinate system. b For the carrier coordinate system, i It is a geocentric inertial coordinate system.

[0126] (2) n 0-series SINS / DVL dead reckoning model.

[0127] Because there is no continuous and accurate position information provided by a positioning system, the position error of the SINS / DVL integrated navigation system cannot converge. Next, a SINS / DVL dead reckoning model will be constructed to improve the initial alignment accuracy of SINS / DVL. First, the attitude calculated by SINS and the vehicle velocity information output by DVL at the previous moment are updated in real time through multiplication and integration operations, just like the SINS calculation. Then, when new DVL velocity data is available, the aforementioned dead reckoning position will be input into the Kalman filter as independent observation information.

[0128] Based on equation (3) to equation (5), the derivation is as follows: n The process of deriving the velocity calculation formula for the 0-series inertial navigation system is similar. n 0-series DVL velocity vector It is given by the following formula:

[0129] (17)

[0130] and n 0 series speed The difference lies in the fact that the relative velocity is composed of DVL velocity measurement and SINS attitude calculation.

[0131] DVL dead reckoning position vector Differential equations can be derived from n The 0-series inertial navigation position vector and DVL velocity vector are given below:

[0132] (18)

[0133] It can be seen that DVL dead reckoning requires attitude information from strapdown inertial navigation systems. and location information This value can be used as location information to assist SINS in the fusion of integrated navigation information. In the SINS / DVL indirect Kalman filter navigation framework, it can... and The difference serves as supplementary information.

[0134] Combined formula (16) n 0-series inertial navigation mechanical arrangement equations and formula (18), final n The SINS / DVL dead reckoning model under the 0 series is given by the following formula:

[0135] (19)

[0136] in, For DVL dead reckoning, calculate the position vector. This is the DVL velocity vector.

[0137] on the one hand, will be as n In the 0-series navigation states, one of the navigation states undergoes time updates and, like other navigation states, is continuously filtered to correct errors. On the other hand, The measurements will be updated as SINS / DVL location measurement information.

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

[0139] (1) SINS / DVL right-invariant error state model based on Lie group SE3(3).

[0140] To address the shortcomings of the traditional Euclidean space vector error definition, the navigation state is represented using the Lie group SE3(3), and a SINS right-invariant error state model is constructed using Lie groups and Lie algebras as mathematical tools. n 0-series carrier attitude ,speed ,Location and DVL dead reckoning position Together as group elements of the Lie group SE3(3), they are given by the following equation:

[0141] (20)

[0142] The state does not satisfy the group affine condition (21). Nevertheless, it is still recommended to use Lie groups to define the state because the coordinate system mismatch problem can be naturally handled. In addition, within the framework of Lie groups, a linear error state model independent of attitude error can be derived, which is beneficial for handling large misalignment problems.

[0143] (twenty one)

[0144] According to Lie group theory, the formula for calculating the right-invariant state error of Lie group SE3(3) is given by the following equation:

[0145] (twenty two)

[0146] in, Let the attitude state be the right-invariant error state space of the Lie group SE3(3). For the velocity state in the right-invariant error state space of the Lie group SE3(3), The position error state of the right-invariant error state space of the Lie group SE3(3). The DVL dead reckoning error state is the right-invariant error state space of the Lie group SE3(3). A row vector with zero elements. Let SE3(3) represent the navigation parameters. The inverse of the navigation parameters represented by error SE3(3), SE3(3) represents the error state.

[0147] Right-invariant error The differential equation is given by the following equation:

[0148] (twenty three)

[0149] Convert to the form of a system of equations:

[0150] (twenty four)

[0151] in, , , and The formula is given as follows:

[0152] (25)

[0153] The error term of the right-invariant error state differential equation of the Lie group SE3(3) is:

[0154] Equation (25) is The corresponding differential equation.

[0155] Based on the right-invariant error state differential equation of the Lie group SE3(3), the corresponding state-space model is named RSE-n0-DR and is given by the following equation:

[0156] (26)

[0157] in:

[0158] (27)

[0159] (28)

[0160] in, for n18-dimensional error states in the 0 series For the system matrix, Assign a matrix to the noise. Let be the attitude misalignment angle in the right-invariant error state space of the Lie group. For zero bias of the gyroscope, For zero bias of the accelerometer, For noise from the gyroscope and accelerometer, A matrix with zero elements It is a 3x3 identity matrix. , , , , They are respectively , , , , The estimated value.

[0161] Due to the dynamic model It is not a group affine system, and without considering inertial sensor errors, i.e. and In the case of right-invariant error Related to trajectory state. However, RSE- n The 0-DR spatial model can still be applied to initial alignment with large misalignment angles. The key is that the attitude, velocity, and position of the inertial navigation system itself are still decoupled, and the DVL dead reckoning position is only related to the navigation position state and is not coupled with the attitude angle. The linearized model based on the Lie group SE3(3) has significant advantages. Compared with the traditional nonlinear model, it does not require the calculation of complex Jacobian matrices and does not have the problem of approximate system models or approximate probability distributions, thus improving the performance of the initial alignment method under large misalignment angles.

[0162] (2) SINS / DVL dead reckoning measurement model based on group error.

[0163] In the SINS / DVL integrated navigation system, in addition to directly using the vehicle speed... In addition to measurement The dead reckoning position derived from DVL (Direct Vehicle Language) using SINS navigation state can also be used as a measurement value. The fusion of SINS output information and DVL measurement information using an error state model is actually an indirect process. Therefore, the measurement value should actually be the position vector calculated by SINS. Position vectors obtained from DVL dead reckoning The difference. According to the right-invariant error state model of equation (26), the SINS / DVL dead reckoning position measurement value is given by the following equation:

[0164] (29)

[0165] in, . Measurements for SINS / DVL dead reckoning positions. For the measurement matrix, for The error between the actual position vector and the initial navigation coordinate system for Error between the initial navigation coordinate system and the true position vector.

[0166] The results show that since the position calculated by DVL dead reckoning is derived from the integration of DVL velocity, using it as a filter measurement can, to some extent, suppress the position error accumulation problem caused by the integration of SINS / DVL velocity.

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

[0168] (1) Setting the initial filter value.

[0169] In the SINS / DVL dead reckoning navigation system, the DVL dead reckoning position is also updated over time, just like the navigation status. Therefore, the DVL dead reckoning position status, like the navigation attitude, velocity, and position calculated by inertial navigation, requires setting an initial process covariance matrix.

[0170] According to equation (24), calculate the right-invariant state error of the Lie group SE3(3). Initial process covariance matrix The formula is as follows:

[0171] (30)

[0172] in:

[0173] (31)

[0174] in, Let the initial error state covariance matrix be... To calculate the initial velocity for the inertial navigation system in the initial navigation coordinate system, The inertial navigation system (INS) calculates the position in the initial navigation coordinate system. Initial values ​​for DVL dead reckoning in the initial navigation coordinate system. Let be the initial error state covariance matrix defined in Euclidean space. The transformation matrix is ​​the initial error state covariance matrix. for The transpose of .

[0175] (2) Filtering estimation.

[0176] The core idea of ​​Kalman filtering is to continuously optimize the system state estimation using a recursive method. First, the current state and its uncertainties are predicted based on the system's dynamic model. Then, the prediction results are corrected using actual observation data. During this process, Kalman filtering dynamically adjusts the weight distribution between the predicted and observed values ​​by calculating the Kalman gain. By continuously iterating through the "prediction-update" cycle, Kalman filtering can gradually improve the accuracy of state estimation in noisy environments, ultimately achieving the optimal estimation of the system state. The one-step state transition matrix is ​​calculated based on the error state-space model of equation (26):

[0177] (32)

[0178] in, For inertial navigation system update time interval, Update the number of samples for the inertial navigation system.

[0179] Substitute the state one-step transition matrix of equation (32) into the following equation to calculate the state one-step prediction:

[0180] (33)

[0181] Calculate the mean square error matrix:

[0182] (34)

[0183] in, Let be the system noise covariance matrix.

[0184] Calculate the filter gain:

[0185] (35)

[0186] in, This is the measurement noise covariance matrix.

[0187] Calculate the state estimate:

[0188] (36)

[0189] Calculate the mean square error matrix of the state estimation:

[0190] (37)

[0191] (3) Error feedback.

[0192] RSE of the SINS / DVL dead reckoning combined system n The 0-DR filter correction feedback can be obtained from equation (24). k At that moment, RSE- n The 0-DR model exponential mapping feedback correction formula is as follows:

[0193] (38)

[0194] Substituting equation (24) into the above equation, we get:

[0195] (39)

[0196] After completing the feedback correction process, the error state estimate needs to be reset:

[0197] (40)

[0198] in, Estimation of linear Kalman filters k Attitude error at any moment Estimation of linear Kalman filters k Momental velocity error Estimation of linear Kalman filters k Time and position error, Estimation of linear Kalman filters k Error in DVL dead reckoning position at any given time. To correct and compensate the initial navigation coordinate system carrier attitude matrix, To correct the vehicle velocity in the initial navigation coordinate system after compensation, To correct the position of the carrier in the initial navigation coordinate system after compensation, To correct and compensate the DVL dead reckoning position vector, for The first to 12th elements, It is a column vector with zero elements.

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

[0200] The key to forward-backtracking alignment based on Lie group SE3(3) lies in repeatedly using the same set of alignment data to estimate the initial pose matrix. The attitude matrix estimated by equation (39) is used to approximate the time-varying attitude, thereby accelerating the alignment convergence speed and accuracy. The time-varying attitude matrix... The following split is performed:

[0201] (41)

[0202] in, For time-varying attitude matrix, Let be the attitude matrix at the initial time step. The attitude matrix is ​​the distance from the initial navigation coordinate system to the current navigation coordinate system, expressed as an equivalent rotation vector. This is used for calculation. Since the initial alignment time is usually short, the error in this term can be ignored. This is the attitude matrix from the current carrier coordinate system to the initial carrier coordinate system, and its calculation is related to the angular velocity output by the gyroscope. The error is related only to the device error of the gyroscope.

[0203] Based on the right-invariant initial alignment strategy of the Lie group SE3(3) in the initial navigation coordinate system, and based on the initial attitude matrix iteratively optimized from the previous backtracking alignment result, the inertial navigation solution attitude matrix and the measured values ​​of the SINS / DVL dead reckoning position in the next backtracking alignment are re-determined. As can be seen from equation (41), after the attitude matrix is ​​split, the initial attitude matrix can be used to determine the position. To approximate the solution of the time-varying attitude matrix That is, repeatedly using the same set of initial alignment data to perform forward navigation calculations to obtain a more accurate initial attitude angle.

[0204] Unlike traditional methods, the forward-forward backtracking strategy employed in this invention eliminates the need for coarse alignment, allowing direct entry into the fine alignment stage. This strategy leverages multi-process parallel computation to improve real-time performance, with one process responsible for performing backtracking alignment and another dedicated to real-time data acquisition. The workflow is as follows: After the initial fine alignment, the system stores the initial gyroscope, accelerometer, and DVL data. The backtracking alignment process uses this data to begin its initial calculations, while the data acquisition process continuously concatenates and integrates the SINS and DVL outputs with the previous data. Thus, with each subsequent backtracking alignment initiation, the computation process can utilize continuously updated and most complete data for initial alignment. A significant advantage of this method is the reduced time required for the backtracking alignment itself. Much shorter than the time required for the first data collection This ensures that the system can immediately transition to the navigation phase after the entire alignment process is completed.

[0205] 3. Experimental verification.

[0206] The total duration of the lake survey data was 12,000 seconds, with the first 1,000 seconds constituting the initial alignment phase. To comprehensively evaluate the algorithm's performance, three typical 400-second data segments were selected: a medium-range maneuver from 3500-3900 seconds, a low-range maneuver with small turns from 4000-4400 seconds, and a high-range maneuver with continuous rapid turns from 8500-8900 seconds. The attitude and velocity reference curves for the entire experiment are shown below. Figure 2 , Figure 3 As shown.

[0207] The experiment was conducted on a Windows 10 system with an i5-12400 CPU and 8GB of RAM. Algorithm performance was evaluated using the output of the SINS / GPS integrated navigation system as the attitude reference. The inertial measurement unit (IMU) data sampling rate was set to 200Hz. The gyroscope was set to a constant drift value of [missing value]. Its angle random walk coefficient is set to The accelerometer's triaxial zero-bias error is... The corresponding speed random walk is Furthermore, the scaling factor error of DVL was set to 0.3%, the measurement noise to 0.003 m / s, and the update frequency to 1 Hz. Simultaneously, the collected raw IMU data and DVL measurements were fused to evaluate the performance of different initial alignment algorithms. Moreover, the introduction of a forward-backtracking alignment framework avoids the need for extensive navigation solution data updates in traditional methods, enabling single backtracking alignment to be completed within a short timeframe. Completed within the timeframe, the additional three backtracking processes take a computation time of [time value missing]. The entire forward-backward process takes time. For the three test scenarios, the initial errors for pitch, roll, and yaw angles were set as follows: The initial velocity error and position error are set separately. and The initial value of the system noise covariance matrix is ​​set based on prior empirical values. for:

[0208] (42)

[0209] According to equation (30), the following can be calculated: .

[0210] The initial process noise covariance matrix is:

[0211] (43)

[0212] RSE-BT- n The 0-DR measurement is based on DVL dead reckoning, and its measurement noise covariance matrix is ​​set as follows: The measurement information for the remaining algorithms is DVL velocity, and the measurement noise covariance matrix is ​​set as follows. .

[0213] exist Figure 4 , Figure 5 , Figure 6 and Figure 7 In the middle, RSE-BT- n 0-DR represents the method proposed in this invention, and 1st and 4th represent its first and fourth forward backtracking alignments, respectively. RSE-BT-n The results of the first and fourth backtracking iterations of 0-DR are compared with e The LSE-KF and RSE-KF models under the system are compared. From equations (26) and (29), it is found that although the process model and the measurement model are not independent of the navigation state, they are not related to the attitude error. Therefore, within 400s, the RSE-BT- n 0-DR can achieve initial SINS / DVL alignment under large misalignment angles. Although the proposed SINS / DVL dead reckoning model is not a strictly affine group system, and the DVL dead reckoning measurement matrix is ​​also related to the navigation state, the experimental attitude angle error data show that RSE-BT- n The alignment accuracy of 0-DR is superior to that of LSE-KF and RSE-KF, and thanks to the use of dead reckoning measurements, the proposed algorithm can promote the convergence of the initial alignment position error to a certain extent. It can be seen from equations (18) and (29) that by using RSE- n RSE-BT- is obtained by combining 0-DR with forward-forward backtracking alignment. n The 0-DR algorithm, after each backtracking iteration, yields the solution obtained by SINS. n 0 series attitude information and location information This will make the DVL dead reckoning more accurate, allowing for more precise location calculations. It is also more accurate, thus resulting in a more accurate final filter measurement. It will also be more reliable. Experimental results verify the effectiveness of the forward-forward backtracking strategy based on the Lie group SE3(3).

[0214] In summary, this invention cleverly combines the advantages of forward-forward backtracking to fully utilize sensor data and improve alignment convergence speed, with a filtering algorithm based on Lie group state representation effectively solving the initial alignment problem with large misalignment angles. Furthermore, it can promote the convergence of initial alignment position errors to a certain extent, thus demonstrating superior overall performance.

[0215] The present invention also discloses a backtracking initial alignment device for SINS / DVL integrated navigation based on Lie group SE3(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 above-mentioned backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3).

[0216] 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 the above-described method embodiments.

[0217] The present invention also provides a computer program product that, when run on a data storage device, enables the data storage device to implement the steps in the above-described method embodiments.

[0218] 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.

[0219] 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 backtracking initial alignment method for SINS / DVL integrated navigation based on Lie group SE3(3), characterized in that, include: Gyroscope and accelerometer data are acquired through the inertial measurement unit, and the velocity of the carrier coordinate system is obtained through DVL. In the initial navigation coordinate system, an inertial navigation mechanical arrangement equation with group affine properties is constructed, and the gyroscope data and accelerometer data are solved into navigation parameters estimated by the inertial navigation system. Based on the navigation parameters estimated by the inertial navigation system and the system velocity of the carrier measured by DVL, the differential equation of DVL dead reckoning position is determined, and the SINS / DVL dead reckoning model under the initial navigation coordinate system is obtained. The DVL dead reckoning position obtained from the SINS / DVL dead reckoning model under the initial navigation coordinate system is used as an element of the Lie group SE3(3) to obtain the right-invariant error state space model based on the state representation of the Lie group SE3(3) under the initial navigation coordinate system; the measurement value of the SINS / DVL dead reckoning position is derived from the right-invariant error state space model to obtain the measurement model based on the Lie group SE3(3); Kalman filtering state estimation is performed using the right-invariant error state space model and the measurement model to determine the navigation error in the inertial navigation solution process; The navigation error is fed back to the navigation parameters calculated by the inertial navigation system to estimate the initial attitude matrix in each forward navigation process, and thus obtain the updated navigation parameters. Backtracking is performed based on the initial attitude matrix optimized iteratively during the forward navigation process. The initial attitude matrix of the inertial navigation solution and the measured values ​​of the SINS / DVL dead reckoning position are re-determined for the next forward navigation process. The time-varying attitude matrix corresponding to the updated navigation parameters is then determined to complete the initial alignment process of the SINS / DVL integrated navigation.

2. The SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) as described in claim 1, characterized in that, Based on the following equation, construct the inertial navigation mechanical arrangement equations with group affine properties in the initial navigation coordinate system: ； ； in, The initial navigation coordinate system is the carrier attitude matrix. This refers to the angular rate output by the gyroscope. The initial navigation coordinate system vehicle velocity, The initial navigation coordinate system is the position of the carrier. , , They are respectively , , The differential, The specific force output by the accelerometer. This is the local gravitational acceleration vector. This is the projection of the Earth's rotational angular velocity onto the initial navigation coordinate system. The initial navigation coordinate system position vector is the one pointing from the origin of the inertial frame to the origin of the vehicle frame. For universal gravitation, n 0 is the initial navigation coordinate system. b For the carrier coordinate system, i It is a geocentric inertial coordinate system; Based on the following equation, the differential equation for DVL dead reckoning position is determined using the navigation parameters estimated by the inertial navigation system and the system velocity measured by DVL, thus obtaining the SINS / DVL dead reckoning model in the initial navigation coordinate system: ； in, For DVL dead reckoning, calculate the position vector. This is the DVL velocity vector.

3. The SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) as described in claim 2, characterized in that, The DVL dead reckoning position obtained from the SINS / DVL dead reckoning model in the initial navigation coordinate system is used as an element of the Lie group SE3(3) based on the following formula: ； The right-invariant state error of the Lie group SE3(3) is determined based on the following formula: ； in, The attitude error state is the right-invariant error state space of the Lie group SE3(3). For the velocity error state in the right-invariant error state space of the Lie group SE3(3), The position error state of the right-invariant error state space of the Lie group SE3(3). The DVL dead reckoning error state is the right-invariant error state space of the Lie group SE3(3). A row vector with zero elements. Let SE3(3) represent the navigation parameters. The inverse of the navigation parameters represented by error SE3(3), Let SE3(3) represent the error state; Based on the Lie group error definition, the right-invariant error state-space model based on the Lie group SE3(3) state representation in the initial navigation coordinate system is obtained according to the following equation: ； ； ； ； in, for n 18-dimensional error states in the 0 series For the system matrix, Assign a matrix to the noise. Let be the attitude misalignment angle in the right-invariant error state space of the Lie group. For zero bias of the gyroscope, For zero bias of the accelerometer, For noise from the gyroscope and accelerometer, It is a 3x3 matrix with zero elements. It is a 3x3 identity matrix. , , , , They are respectively , , , , The estimated value; Based on the following formula, the measurement values ​​of the SINS / DVL dead reckoning position are derived from the right-invariant error state-space model, resulting in a measurement model based on the Lie group SE3(3): ； ； in, Measurements for SINS / DVL dead reckoning positions. For the measurement matrix, for The error between the actual position vector and the initial navigation coordinate system for Error between the initial navigation coordinate system and the true position vector.

4. The SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) as described in claim 3, characterized in that, When performing Kalman filter state estimation using the right-invariant error state-space model and the measurement model, the initial error state covariance matrix of the Lie group SE3(3) is set based on the following formula: ； ； in, Let the initial error state covariance matrix be... To calculate the initial velocity for the inertial navigation system in the initial navigation coordinate system, The inertial navigation system (INS) calculates the position in the initial navigation coordinate system. Initial values ​​for DVL dead reckoning in the initial navigation coordinate system. Let be the initial error state covariance matrix defined in Euclidean space. The transformation matrix is ​​the initial error state covariance matrix. for Transpose of; Kalman filter state estimation is performed using the right-invariant error state-space model and the measurement model, based on the following formula: ； After feedback correction, the error state estimate of the Lie group SE3(3) is reset based on the following formula: ； in, Estimation of linear Kalman filters k Attitude error at any moment Estimation of linear Kalman filters k Momental velocity error Estimation of linear Kalman filters k Time and position error, Estimation of linear Kalman filters k Error in DVL dead reckoning position at any given time. To correct and compensate the initial navigation coordinate system carrier attitude matrix, To correct the vehicle velocity in the initial navigation coordinate system after compensation, To correct the position of the carrier in the initial navigation coordinate system after compensation, To correct and compensate the DVL dead reckoning position vector, for The first to 12th elements, It is a column vector with zero elements.

5. The SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) as described in claim 1, characterized in that, The step of feeding back navigation errors to the navigation parameters calculated by the inertial navigation system to estimate the initial attitude matrix in each forward navigation process and obtain updated navigation parameters specifically includes: In the aforementioned forward-backward alignment strategy, the time-varying attitude matrix is ​​decomposed based on the following formula: ； in, For time-varying attitude matrix, The initial attitude matrix, This is the attitude matrix from the initial navigation coordinate system to the current navigation coordinate system. This is the attitude matrix from the current carrier coordinate system to the initial carrier coordinate system; Based on the right-invariant initial alignment strategy of the Lie group SE3(3) in the initial navigation coordinate system, and based on the initial attitude matrix iteratively optimized by the previous backtracking alignment result, the inertial navigation solution attitude matrix and the measurement values ​​of the SINS / DVL dead reckoning position in the next backtracking alignment are re-determined.

6. A backtracking initial alignment device for SINS / DVL integrated navigation based on Lie group SE3(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 backtracking initial alignment method of SINS / DVL integrated navigation based on Lie group SE3(3) as described in any one of claims 1-5.

7. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the backtracking initial alignment method of SINS / DVL integrated navigation based on Lie group SE3(3) as described in any one of claims 1-5.

8. A computer program product, characterized in that, When the computer program product runs on the data storage device, it enables the data storage device to implement the SINS / DVL integrated navigation backtracking initial alignment method based on Lie group SE3(3) as described in any one of claims 1-5.

Images