Search

CN-121977546-A - Based on plum group SE3(3) SINS/DVL integrated navigation backtracking type initial alignment method and device

CN121977546ACN 121977546 ACN121977546 ACN 121977546ACN-121977546-A

Abstract

The invention discloses a SINS/DVL integrated navigation retrospective initial alignment method and device based on a Leu SE 3 (3), and relates to the technical field of inertial-based navigation initial alignment. The method comprises the steps of establishing an SINS/DVL dead reckoning model under an initial navigation coordinate system, taking a DVL dead reckoning position obtained by the SINS/DVL dead reckoning model under the initial navigation coordinate system as an element of a Liqun SE 3 (3), constructing a right invariable error state space model and a measuring model based on the Liqun SE 3 (3), carrying out Kalman filtering state estimation by utilizing the right invariable error state space model and the measuring model to determine a navigation error in an inertial navigation resolving process, and feeding back the navigation error to navigation parameters of the inertial navigation resolving to obtain updated navigation parameters. And determining a time-varying gesture matrix corresponding to the updated navigation parameters to complete the initial alignment process of SINS/DVL integrated navigation. The invention can solve the problems of quick and high-precision initial alignment under the condition of large misalignment angle, and effectively inhibit the divergence of position errors under the assistance of DVL speed.

Inventors

  • Hu Gaoge
  • GENG JIALE
  • XIANG PENG
  • CHANG LUBIN
  • ZHAO XINGWANG

Assignees

  • 西北工业大学

Dates

Publication Date
20260505
Application Date
20260402

Claims (8)

  1. 1. The SINS/DVL integrated navigation retrospective initial alignment method based on the Liquorum SE 3 (3) is characterized by comprising the following steps: Acquiring gyroscope data and accelerometer data through an inertial measurement unit, and acquiring carrier coordinate system speed through a DVL; An inertial navigation mechanical programming equation with group affine property is built in an initial navigation coordinate system, gyroscope data and accelerometer data are solved into navigation parameters of inertial navigation estimation, and a DVL dead reckoning position differential equation is determined according to the navigation parameters of the inertial navigation estimation and the carrier system speed measured by the DVL, so that an SINS/DVL dead reckoning model under the initial navigation coordinate system is obtained; taking the DVL dead reckoning position obtained by the SINS/DVL dead reckoning model under the initial navigation coordinate system as an element of the Liqun SE 3 (3) to obtain a right invariable error state space model expressed based on the state of the Liqun SE 3 (3) under the initial navigation coordinate system; Carrying out Kalman filtering state estimation by utilizing the right invariable error state space model and the measurement model to determine a navigation error in the inertial navigation calculation process; The initial attitude matrix is iterated and optimized based on the forward navigation process, the measurement value of the inertial navigation solution initial attitude matrix and the SINS/DVL dead reckoning position in the next forward navigation process is redetermined, and the time-varying attitude matrix corresponding to the updated navigation parameters is determined so as to complete the initial alignment process of SINS/DVL combined navigation.
  2. 2. The method of initial alignment of SINS/DVL integrated navigation retrospective based on the prune cluster SE 3 (3) of claim 1, wherein the inertial navigation mechanical programming equation with cluster affine properties is constructed in the initial navigation coordinate system based on the following formula: ; ; Wherein, the For the initial navigational coordinate system carrier pose matrix, For the angular rate of the gyroscope output, For the initial navigational coordinate system carrier speed, For the initial navigation coordinate system carrier position, 、 、 Respectively is 、 、 Is used for the differentiation of the (c) and (d), For the specific force output by the accelerometer, In order to be a local gravitational acceleration vector, For the projection of the rotational angular velocity of the earth in the initial navigation coordinate system, An initial navigation coordinate system position vector pointing to the origin of the carrier system for the origin of the inertial system, The gravity is universal, n0 is an initial navigation coordinate system, b is a carrier coordinate system, and i is a geocentric inertial coordinate system; Determining a DVL dead reckoning position differential equation according to navigation parameters estimated by inertial navigation and a carrier system speed measured by the DVL based on the following formula to obtain an SINS/DVL dead reckoning model under an initial navigation coordinate system: ; Wherein, the For a DVL dead reckoning position vector, Is a DVL velocity vector.
  3. 3. The method for initial alignment of SINS/DVL combined navigation back based on the prune group SE 3 (3) of claim 2, wherein the DVL dead reckoning position obtained by the SINS/DVL dead reckoning model under the initial navigation coordinate system is used as the element of the prune group SE 3 (3) based on the following formula: ; Determining a right invariant state error for the prune group SE 3 (3) based on: ; Wherein, the For the attitude error state of the right invariant error state space of the prune group SE 3 (3), For the velocity error state of the right invariant error state space of the prune group SE 3 (3), For the position error state of the right invariant error state space of the prune group SE 3 (3), For the DVL dead reckoning error state of the right invariant error state space of the lie group SE 3 (3), Is the line vector of the element of zero, For the navigation parameters denoted SE 3 (3), For the inverse of the navigation parameters represented with error SE 3 (3), An error state represented by SE 3 (3); according to the Liqun error definition, a right invariant error state space model based on the Liqun SE 3 (3) state representation in the initial navigation coordinate system is obtained based on the following formula: ; ; ; ; Wherein, the Is an 18-dimensional error state in the n0 series, In the form of a system matrix, A matrix is assigned to the noise and, For the attitude misalignment angle of the state space of the right invariant error of the lie group, Is the zero offset of the gyroscope, For a zero-bias of the accelerometer, As noise for gyroscopes and accelerometers, Is a matrix of three rows and three columns of zero elements, Is a three-row three-column identity matrix, 、 、 、 、 Respectively is 、 、 、 、 Is a function of the estimated value of (2); Deriving a measurement value of the SINS/DVL dead reckoning position according to the right invariant error state space model based on the following formula to obtain a measurement model based on the Liquorum SE 3 (3): ; ; Wherein, the Is a measurement of SINS/DVL dead reckoning position, In order to measure the matrix of the device, Is that Error from the original navigation coordinate system true position vector, Is that Error from the original navigation coordinate system true position vector.
  4. 4. A method for initial alignment of SINS/DVL combined navigation back based on the prune group SE 3 (3) as claimed in claim 3, wherein when the kalman filter state estimation is performed by using the right invariant error state space model and the metrology model, the initial error state covariance matrix of the prune group SE 3 (3) is set based on the following formula: ; ; Wherein, the For the initial error state covariance matrix, The initial velocity is solved for inertial navigation in an initial navigation coordinate system, For inertial navigation solution position in the initial navigation coordinate system, For the initial value of DVL dead reckoning in the initial navigation coordinate system, An initial error state covariance matrix defined for errors in euclidean space, A transformation matrix that is an initial error state covariance matrix, Is that Is a transpose of (2); performing Kalman filtering state estimation by using the right invariant error state space model and the measurement model based on the following steps: ; After the feedback correction, the error state estimate for the prune set SE 3 (3) is reset based on the following equation: ; Wherein, the The k-moment attitude error is estimated for a linear kalman filter, The k-moment velocity error is estimated for a linear kalman filter, The k-moment position error is estimated for a linear kalman filter, The error of the DVL dead reckoning position at time k is estimated for a linear kalman filter, To correct the compensated initial navigational coordinate system carrier pose matrix, To correct the compensated initial navigational coordinate system carrier speed, To correct the compensated initial navigational coordinate system carrier position, To correct the compensated DVL dead reckoning position vector, Is that Is a group consisting of the 1 st to 12 th elements, Is a zero element column vector.
  5. 5. The method for initial alignment of the SINS/DVL integrated navigation retrospective based on the prune group SE 3 (3) of claim 1, wherein the feeding back the navigation error to the navigation parameters calculated by inertial navigation to estimate an initial pose matrix in each forward navigation process, obtaining updated navigation parameters, specifically comprises: In the forward-forward backtracking alignment strategy, splitting the time-varying gesture matrix based on the following formula: ; Wherein, the For a time-varying gesture matrix, For the initial moment of time of the gesture matrix, For the gesture matrix from the initial navigation coordinate system to the current navigation coordinate system, The gesture matrix from the current carrier coordinate system to the initial carrier coordinate system is adopted; Based on the right unchanged initial alignment strategy of the prune group SE 3 (3) under the initial navigation coordinate system and based on the initial posture matrix iteratively optimized by the previous backtracking alignment result, the inertial navigation solution posture matrix and the measurement value of the SINS/DVL dead reckoning position in the next backtracking alignment are redetermined.
  6. 6. The SINS/DVL integrated navigation back-tracking type initial alignment device based on the Liquorum SE 3 (3) is characterized by comprising a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the processor realizes the SINS/DVL integrated navigation back-tracking type initial alignment method based on the Liquorum SE 3 (3) when executing the computer program.
  7. 7. A computer readable storage medium storing a computer program, wherein the computer program when executed by a processor implements a SINS/DVL combined navigation traceback initial alignment method based on a prune group SE 3 (3) as claimed in any one of claims 1 to 5.
  8. 8. A computer program product, characterized in that the computer program product, when run on a data storage device, is adapted to implement a SINS/DVL combined navigation retrospective initial alignment method based on a prune group SE 3 (3) as claimed in any one of claims 1-5 when executed on the data storage device.

Description

SINS/DVL integrated navigation backtracking type initial alignment method and device based on Liquorum SE 3 (3) Technical Field The invention relates to the technical field of inertial-based navigation initial alignment, in particular to a SINS/DVL combined navigation retrospective initial alignment method and device based on a Litsea SE 3 (3). Background The ocean covers more than 70% of the surface area of the earth and contains rich undetermined resources. To exploit these precious resources, autonomous underwater vehicles (Autonomous Underwater Vehicle, AUV) have become an indispensable core equipment, receiving great attention in both military and civilian fields. In various navigation schemes of AUV, SINS/DVL integrated navigation system composed of strapdown inertial navigation system (Strapdown Inertial Navigation System, SINS) and Doppler log (Doppler velocity log, DVL) is widely used due to the advantages of high precision, low cost, strong anti-interference capability and the like. SINS as a dead reckoning system, whether initial alignment can be quickly and accurately accomplished and reliable initial navigational state obtained before starting work will directly affect the performance and accuracy of the overall system. A key challenge of the initial alignment technique is how to balance the accuracy and speed of alignment. Conventional alignment procedures are generally divided into two stages, coarse alignment and fine alignment. The coarse alignment stage utilizes static measurement values of inertial sensors (gyroscopes and accelerometers) to quickly estimate a gesture matrix with limited precision in a few seconds through analytic calculation or simple algorithm. In the fine alignment stage, on the basis of the initial gesture provided by coarse alignment, the error of the SINS is modeled as a state variable, and iterative optimization estimation is performed by using optimal estimation methods such as Kalman filtering and the like. This process takes tens of seconds to minutes, eventually resulting in a high precision (errors up to angular or even angular seconds) pose matrix. However, under the moving base, it is difficult to obtain accurate attitude information in the coarse alignment stage, which makes the subsequent fine alignment stage unable to meet the small angle error assumption required thereby to cause a decrease in the overall alignment accuracy, and the adoption of the DVL carrier speed as the measurement information has a problem that the position estimation error is cumulatively divergent over time. Disclosure of Invention Based on this, it is necessary to provide a SINS/DVL integrated navigation retrospective initial alignment method and apparatus based on the prune group SE 3 (3) aiming at the above technical problems. The embodiment of the invention provides a SINS/DVL integrated navigation retrospective initial alignment method based on a plum group SE 3 (3), which comprises the following steps: Acquiring gyroscope data and accelerometer data through an inertial measurement unit, and acquiring carrier coordinate system speed through a DVL; An inertial navigation mechanical programming equation with group affine property is built in an initial navigation coordinate system, gyroscope data and accelerometer data are solved into navigation parameters of inertial navigation estimation, and a DVL dead reckoning position differential equation is determined according to the navigation parameters of the inertial navigation estimation and the carrier system speed measured by the DVL, so that an SINS/DVL dead reckoning model under the initial navigation coordinate system is obtained; taking the DVL dead reckoning position obtained by the SINS/DVL dead reckoning model under the initial navigation coordinate system as an element of the Liqun SE 3 (3) to obtain a right invariable error state space model expressed based on the state of the Liqun SE 3 (3) under the initial navigation coordinate system; Carrying out Kalman filtering state estimation by utilizing the right invariable error state space model and the measurement model to determine a navigation error in the inertial navigation calculation process; The initial attitude matrix is iterated and optimized based on the forward navigation process, the measurement value of the inertial navigation solution initial attitude matrix and the SINS/DVL dead reckoning position in the next forward navigation process is redetermined, and the time-varying attitude matrix corresponding to the updated navigation parameters is determined so as to complete the initial alignment process of SINS/DVL combined navigation. Optionally, an inertial navigation mechanical programming equation with group affine properties is constructed in the initial navigation coordinate system based on: ; ; Wherein, the For the initial navigational coordinate system carrier pose matrix,For the angular rate of the gyroscope output,For the initial navigational coordinate