Method and related device for in-motion disturbance rejection alignment based on reverse loop and convex optimization

Abstract

The application discloses a kind of based on reverse loop and convex optimization's anti-interference alignment method during travelling and related device, it is related to inertial navigation and signal processing field, this method includes collecting and buffering the original data of inertial measurement unit, the original data after buffering is carried out forward inertial navigation solution, and reverse time series data is obtained by construction;Virtual speed observation and virtual heading change observation are constructed using loop constraint, based on reverse time series data, virtual speed observation and virtual heading change observation, the calibration parameter of inertial measurement unit and the reference trajectory after optimization are obtained;The target data of inertial measurement unit is compensated using calibration parameter, and the target data after compensation is obtained, and based on the target data after compensation and the reference trajectory after optimization, high-precision navigation state is obtained.The application significantly improves the autonomous navigation capability of inertial navigation system.

Description

Technical Field

[0001] This application relates to the fields of inertial navigation and signal processing, and in particular to an in-journey anti-disturbance alignment method and related apparatus based on inverse loop closure and convex optimization. Background Technology

[0002] High-precision inertial navigation systems (INS) can autonomously provide the position, velocity, and attitude information of a vehicle, playing a crucial supporting role in vehicle-mounted platforms with long endurance and high reliability requirements. Initial alignment and online calibration of inertial device error parameters are critical prerequisites for determining the navigation accuracy of the INS. Traditional high-precision alignment and calibration methods typically require the vehicle to remain stationary or rely on continuous and reliable external reference information such as global navigation satellite systems (GNSS) or Doppler logs, and require the vehicle to execute specific calibration maneuvers, such as figure-eight or rotational movements. However, in practical vehicle-mounted application scenarios such as military covert operations, rapid emergency launch, and complex denied environments, these conditions are often unmet, severely restricting the autonomy and rapid response capabilities of INS.

[0003] In the process of alignment while in motion on a vehicle, the system faces more complex challenges. Most existing alignment methods rely on external velocity or position references. Once the reference signal is lost or discontinuous, system errors accumulate rapidly, leading to alignment failure or a sharp drop in accuracy. To overcome this dependence on external information, some methods attempt to construct geometrically closed constraints using the natural loop paths of the moving vehicle to achieve error self-calibration. However, in real-world vehicle environments, the vehicle's trajectory often exhibits non-closed characteristics, making it difficult to form effective physical loop paths, thus rendering traditional error correction methods based on spatial loops unsuitable. Furthermore, the vehicle is inevitably affected by unknown disturbances such as bumps, vibrations, and impacts while in motion, resulting in non-Gaussian and statistically unpredictable noise characteristics. Traditional Kalman filtering based on the minimum mean square error criterion is insufficiently robust to such disturbances, easily leading to divergence in the calibration and alignment process.

[0004] To address the aforementioned issues, there is an urgent need for an efficient alignment and calibration method that can be performed without relying on external references, requiring specific maneuvers, or needing actual physical loop paths, while also being able to resist in-journey disturbances. Summary of the Invention

[0005] The purpose of this application is to provide an in-journey anti-disturbance alignment method and related apparatus based on inverse loop and convex optimization, which can significantly improve the autonomous navigation capability of inertial navigation systems in complex denial environments.

[0006] To achieve the above objectives, this application provides the following solution: Firstly, this application provides an inter-journey disturbance-resistant alignment method based on inverse loop closure and convex optimization, including: Step S1: During the movement of the carrier, continuously collect and cache the raw data of the inertial measurement unit to obtain the cached raw data; Step S2: Perform forward inertial navigation calculation on the cached raw data to construct inverse time series data; Step S3: Construct virtual velocity observations and virtual heading change observations using lap-loop constraints, and use the inverse time series data as the driving input for the state recursion of the sequential robust filter, and use the virtual velocity observations and virtual heading change observations as the measurement inputs of the sequential robust filter. Solve the filter gain through convex optimization, perform state estimation on the constructed full-state inverse error model, and output the calibration parameters of the inertial measurement unit and the optimized reference trajectory; the sequential robust filter is a measurement update based on the robust filter combined with sequential filtering processing. Step S4: First, acquire the target data of the inertial measurement unit in real time. Then, compensate the target data using the calibration parameters to obtain compensated target data. Perform forward inertial navigation calculation based on the compensated target data to obtain the real-time navigation state. Next, use the position of the optimized reference trajectory at the corresponding time as the pseudo-absolute position observation value and use the pseudo-absolute position observation value as the measurement input of the sequential Kalman filter to estimate the dimension-reduced state vector. Finally, use the dimension-reduced state vector to correct the real-time navigation state and output a high-precision navigation state. The sequential Kalman filter is a measurement update based on the Kalman filter combined with sequential filtering processing.

[0007] In a second aspect, this application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the inter-journey anti-interference alignment method based on reverse loop and convex optimization as described above.

[0008] Thirdly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the inter-journey anti-interference alignment method based on reverse loop and convex optimization as described above.

[0009] Fourthly, this application provides a computer program product, including a computer program that, when executed by a processor, implements the inter-journey anti-interference alignment method based on reverse loop and convex optimization as described above.

[0010] According to the specific embodiments provided in this application, this application has the following technical effects: This application provides a method and related apparatus for anti-disturbance alignment during transit based on inverse loopback and convex optimization. By continuously collecting and caching the raw data of the inertial measurement unit during the movement of the carrier, and performing forward inertial navigation calculation on the cached raw data, inverse time series data is constructed. This solves the problem that traditional alignment and calibration methods require the carrier to remain stationary, rely on external reference information, or execute specific maneuver trajectories. It realizes autonomous loopback triggering without relying on external references such as global navigation satellite systems and Doppler logs, and without requiring specific calibration maneuver trajectories. This improves the autonomy and rapid response capability of the inertial navigation system in scenarios such as military covert operations, emergency rapid start-up, and complex denial environments.

[0011] By constructing virtual velocity and virtual heading change observations using lap-loop constraints, and using inverse time series data as the driving input for state recursion and virtual observations as the measurement input, a sequential robust filter is employed to estimate the state of the full-state inverse error model. This solves the problem of decreased alignment and calibration accuracy or even divergence caused by unknown disturbances during the carrier's movement. Robust estimation is achieved under non-Gaussian, bounded disturbance conditions such as turbulence, vibration, and impact, ensuring that the energy gain of the estimation error is bounded under the worst-case disturbance. The system outputs high-precision inertial device calibration parameters and optimized reference trajectories.

[0012] By first compensating the real-time acquired inertial measurement unit data using calibration parameters, and then performing inertial navigation calculations based on the compensated data to obtain the real-time navigation state, the position of the optimized reference trajectory at the corresponding time is used as the pseudo-absolute position observation value. A sequential Kalman filter is used to estimate the dimension-reduced state vector, and finally the dimension-reduced state vector is used to correct the real-time navigation state. This solves the problem of rapid accumulation of system errors in traditional in-journey alignment methods when the reference signal is lost or discontinuous. It achieves high-precision estimation of attitude misalignment angle and position error under the condition of no external reference information, and outputs a high-precision navigation state.

[0013] By employing sequential robust filters and sequential Kalman filters for measurement updates, multidimensional observations are decomposed into scalars for sequential processing. Scalar reciprocals are used instead of matrix inversion operations, and convex optimization ensures the global optimality of the filter gain. This solves the problem that the computational complexity of matrix inversion in standard Kalman filter measurement updates is proportional to the cube of the observation dimension, placing a heavy burden on resource-constrained embedded navigation computers. The computational complexity is reduced from being proportional to the cube of the observation dimension to being proportional to the square of the state dimension, significantly improving the algorithm's computational efficiency and meeting the real-time requirements of embedded systems. Attached Figure Description

[0014] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0015] Figure 1 A flowchart illustrating an inter-journey anti-disturbance alignment method based on inverse loop and convex optimization, provided as an embodiment of this application; Figure 2 A schematic diagram of a two-stage filtering architecture of "reverse loop-forward catch-up" provided in an embodiment of this application; Figure 3 A schematic diagram showing the comparison of measurement updates between sequential Kalman filtering and standard Kalman filtering provided in an embodiment of this application; Figure 4 This is a schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0016] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0017] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0018] The in-journey anti-disturbance alignment method based on inverse loop closure and convex optimization provided in this application is executed by a computer device. The computer device contains a memory to cache the original data, and the steps described in this application are implemented by executing the computer program stored in the memory. First, the cached original data is processed by forward inertial navigation to construct inverse time series data. Then, virtual velocity observations and virtual heading change observations are constructed using loop closure constraints. The inverse time series data is used as the driving input of the sequential robust filter, and the virtual observations are used as the measurement input. The filter gain is solved by convex optimization to estimate the state of the constructed full-state inverse error model, and the calibration parameters of the inertial measurement unit and the optimized reference trajectory are output. Finally, the target data of the inertial measurement unit is compensated using the calibration parameters to obtain compensated target data. Based on the compensated target data and the optimized reference trajectory, a high-precision navigation state is obtained.

[0019] In one exemplary embodiment, such as Figure 1 As shown, an inter-journey anti-disturbance alignment method based on inverse loop and convex optimization is provided. This method is executed by a computer device and includes the following steps S1 to S4 in this embodiment: Step S1: During the movement of the carrier, continuously collect and cache the raw data of the inertial measurement unit to obtain the cached raw data.

[0020] Step S2: Perform forward inertial navigation calculation on the cached raw data to construct inverse time series data.

[0021] Step S3: Construct virtual velocity observations and virtual heading change observations using lap-loop constraints, and use the inverse time series data as the driving input for the state recursion of the sequential robust filter, and use the virtual velocity observations and virtual heading change observations as the measurement inputs of the sequential robust filter. Solve the filter gain through convex optimization, perform state estimation on the constructed full-state inverse error model, and output the calibration parameters of the inertial measurement unit and the optimized reference trajectory. The sequential robust filter is a measurement update based on the robust filter combined with sequential filtering processing.

[0022] Step S4: First, acquire the target data of the inertial measurement unit in real time, compensate the target data using calibration parameters to obtain compensated target data, and perform forward inertial navigation calculation based on the compensated target data to obtain the real-time navigation state. Then, use the position of the optimized reference trajectory at the corresponding time as the pseudo-absolute position observation value, and use the pseudo-absolute position observation value as the measurement input of the sequential Kalman filter to estimate the dimension-reduced state vector. Finally, use the dimension-reduced state vector to correct the real-time navigation state and output the high-precision navigation state. The sequential Kalman filter is a measurement update based on the Kalman filter combined with sequential filtering processing.

[0023] Implementing steps S1 to S4 as described above has the following beneficial effects: (i) Through steps S1 and S2, the raw data of the inertial measurement unit is continuously collected and cached during the movement of the carrier. The cached raw data is then used for forward inertial navigation calculation to construct inverse time series data. This solves the problem that traditional alignment and calibration methods require the carrier to remain stationary, rely on external reference information, or execute specific maneuver trajectories. It enables autonomous loop triggering without relying on external references such as global navigation satellite systems or Doppler logs, and without requiring specific calibration maneuver trajectories. This improves the autonomy and rapid response capability of the inertial navigation system in scenarios such as military covert operations, emergency rapid start-up, and complex denial environments.

[0024] (ii) In step S3, the loop path data is first processed in reverse order according to the timestamp to obtain the reverse time series data, and a full-state reverse error model is constructed. Then, virtual velocity observation and virtual heading change observation are constructed using loop constraints. The reverse time series data is used as the driving input for state recursion and the virtual observation is used as the measurement input. The filter gain is solved by convex optimization. The state estimation of the full-state reverse error model is performed by sequential robust filter. This solves the problem of decreased alignment and calibration accuracy or even divergence caused by unknown disturbances during the carrier's movement. Robust estimation is achieved under non-Gaussian, bounded disturbance conditions such as turbulence, vibration, and impact. It ensures that the energy gain of the estimation error is bounded under the worst-case disturbance and outputs high-precision inertial device calibration parameters and optimized reference trajectory.

[0025] (III) In step S4, the inertial measurement unit data acquired in real time is first compensated using calibration parameters. Based on the compensated data, the inertial navigation solution is performed to obtain the real-time navigation state. Then, the position of the optimized reference trajectory at the corresponding time is used as the pseudo-absolute position observation value. The sequential Kalman filter is used to estimate the dimension-reduced state vector. Finally, the dimension-reduced state vector is used to correct the real-time navigation state. This solves the problem of rapid accumulation of system error in the traditional in-journey alignment method when the reference signal is lost or discontinuous. It realizes high-precision estimation of attitude misalignment angle and position error under the condition of no external reference information and outputs high-precision navigation state.

[0026] (iv) By using sequential robust filters and sequential Kalman filters for measurement updates in steps S3 and S4 respectively, multidimensional observations are decomposed into scalars for sequential processing. The reciprocal of the scalars is used to replace the matrix inversion operation, and the global optimality of the filter gain is guaranteed by convex optimization. This solves the problem that the computational complexity of matrix inversion in standard Kalman filter measurement updates is proportional to the cube of the observation dimension, which puts a heavy burden on resource-constrained embedded navigation computers. The computational complexity is reduced from being proportional to the cube of the observation dimension to being proportional to the square of the state dimension, which significantly improves the computational efficiency of the algorithm and meets the real-time requirements of embedded systems.

[0027] In summary, this application achieves integrated autonomous alignment and calibration with high computational efficiency by adopting a two-stage recursive estimation architecture of loop closure detection, reverse anti-disturbance calibration, and forward refinement alignment, combined with sequential filtering and convex optimization techniques. This is possible without relying on external references, without requiring specific maneuvers, and with the ability to resist in-journey disturbances. This significantly improves the autonomous navigation capability of inertial navigation systems in complex denied environments.

[0028] Further, step S1, system initialization and data caching: After the vehicle (such as a vehicle or autonomous underwater vehicle) is powered on and begins to move, the navigation system continuously acquires raw data from the inertial measurement unit (IMU) at a high frequency (e.g., 100Hz). The IMU includes a three-axis gyroscope and a three-axis accelerometer. The raw data from the IMU includes the raw angular increments output by the three-axis gyroscope and the raw velocity increments output by the three-axis accelerometer. These data, along with precise timestamps, are buffered in a circular buffer to obtain the buffered raw data. The buffer length should be sufficient to cover the time required for the vehicle to form a typical loop.

[0029] Further, step S2, forward inertial navigation solution and reverse time series data construction: The system uses the raw data from the inertial measurement unit in real time to perform strapdown inertial navigation calculations (i.e., forward inertial navigation calculations) in forward chronological order, obtaining initial navigation state data containing timestamps. The initial navigation state data is a real-time navigation trajectory including errors. and heading Meanwhile, in order to utilize loop closure constraints to estimate errors in reverse, the initial navigation state data is rearranged from largest to smallest based on timestamps to construct inverse time series data: Extract the loopback time period from the cached raw data. , All raw data within ]

[0030] Arrange these data in descending order of timestamp (i.e., from...) arrive Rearranged to form reverse time series data , },in It is a reverse time variable.

[0031] The starting time of the reverse time series data (corresponding to the original) This is denoted as the starting point of the inverse filter. End time (corresponding to the original) ) is denoted as the endpoint of the inverse filtering. .

[0032] The core "reverse loop-forward catch-up" two-stage filtering architecture is as follows: Figure 2 As shown, step S3, the reverse loop stage—online interference rejection calibration: The goal of this phase is to robustly estimate the system's full state using closure constraints, with a focus on obtaining the gyroscope's constant drift. and accelerometer zero bias .

[0033] S301. Construct a full-state inverse error model: Constructing a system that includes attitude misalignment angles Speed ​​error Position error gyroscope constant drift and accelerometer zero bias Full state vector The inverse time state equation is derived from the standard (forward) inertial navigation error equation through time inversion. The combination of the full state vector, the inverse time series data, and the inverse time state equation is used as the full-state inverse error model. φ Taking the angular inverse error model as an example, its inverse time state equation can be approximately expressed as: ; in, Let be the derivative of the total state vector; This is the full state vector; This is the coefficient matrix of the inverse time state equation; This represents the system noise term.

[0034] S302. Constructing virtual velocity observations and virtual heading change observations using laparoscopy constraints: a) Virtual Velocity Observation: The start time of the inverse time series data is taken as the starting point of the inverse filtering, and the end time of the inverse time series data is taken as the ending point of the inverse filtering. Utilizing the strong geometric constraint that the start and end points of the data loop should be the same, the observed values ​​of the carrier velocity error are set to known values ​​at the inverse filtering start and end points to construct virtual velocity observation. The observation equation for virtual velocity observation is: ; ; in, and These are virtual velocity observations; This serves as the starting point for inverse filtering. This is the endpoint of the reverse filtering process; For speed error; To observe noise.

[0035] b) Virtual Heading Change Observation: Obtain the total heading change. Based on the total heading change, establish the observation equation for virtual heading change observation. This observation equation describes the quantitative relationship between the total heading change and the azimuth misalignment angle and gyro drift. Loop closure implies a total heading change of approximately 360°. The actual change can be estimated by integrating the angle increments during the inverse processing. The observation equation can be modeled as follows: ; in, These are virtual heading change observations; This is the measured value of the total heading change along the loop path; These are the observed coefficients; This is the azimuth misalignment angle; The top is drifting upwards; To observe noise.

[0036] S303. Disturbance immunity estimation based on sequential robust filter: Using the inverse time series data as the driving input for state recursion, and the virtual velocity observation and the virtual heading change observation as measurement inputs, a sequential robust filter is used to estimate the state of the full-state inverse error model.

[0037] The sequential robust filter is a sequential H∞ filter, which decomposes multidimensional observations into scalars for sequential processing during measurement updates, avoiding matrix inversion operations. The design of the sequential H∞ filter includes considering the existence of energy-bounded perturbations in the system. The objective of the sequential H∞ filter is to minimize the worst-case estimation error. Relative to disturbance The energy gain. This problem can be transformed into a convex optimization problem, specifically by solving the following linear matrix inequality (LMI) to obtain the filter gain matrix. K : ; in, Let be the positive definite matrix to be solved. For a given attenuation level, , , Let be the system matrix. The feasible solution to this linear matrix inequality can be efficiently solved using convex optimization algorithms (such as the interior-point method), and since the problem itself is convex, the obtained solution is globally optimal. The obtained filter gain matrix ensures that, under worst-case perturbation, the energy gain of the estimation error is less than the preset attenuation level. ,Right now , Let be the transfer function from the disturbance to the estimation error.

[0038] During measurement updates, a sequential processing approach is employed. Since the virtual velocity observation and virtual heading change observation constructed in step S302 are independent at different times, their measurement noise covariance matrix can be considered a diagonal matrix, satisfying the sequential processing condition. For each observation time, the multidimensional observation is decomposed into scalars and processed sequentially: the virtual velocity observation is a three-dimensional vector, and its three components are decomposed into three independent scalar observations; the virtual heading change observation is a scalar and processed as a single scalar observation. The sequential filtering update formula is applied sequentially for scalar gain calculation, state update, and covariance update. This process avoids matrix inversion operations, significantly reducing computational complexity.

[0039] The inverse sequential H∞ filter is run iteratively. After processing all the inverse data, the output is adjusted for constant gyroscope drift. and accelerometer zero bias The calibration estimate is obtained, and simultaneously, an optimized reference trajectory that has been filtered and smoothed and satisfies the lapsing constraint is obtained. .

[0040] The specific filtering process is as follows: For the current moment in the reverse time series data, based on the state estimation vector and the estimation error covariance matrix of the previous moment, and combined with the reverse time state equation and the reverse time series data of the current moment, a time update is performed to obtain the predicted state estimation vector and the predicted estimation error covariance matrix of the current moment.

[0041] The virtual velocity observation and virtual heading change observation at the current moment are used as measurement inputs for measurement updates: the virtual velocity observation is a three-dimensional vector, and its three components are decomposed into three scalars and processed sequentially; the virtual heading change observation is a scalar and is processed as a single scalar observation. Each scalar observation undergoes scalar gain calculation, state update, and covariance update sequentially to obtain the updated state estimation vector and the updated estimation error covariance matrix at the current moment. The scalar gain calculation is based on the H∞ criterion, and the parameters for scalar gain calculation (i.e., the filter gain matrix) are determined by solving a linear matrix inequality convex optimization problem.

[0042] Determine whether all moments in the inverse time series data have been processed. If yes, output the updated state estimation vector at the current moment and the updated estimation error covariance matrix at all moments. Extract the gyroscope constant drift and accelerometer zero bias from the updated state estimation vector at the current moment as calibration parameters for the inertial measurement unit. Extract the position information from the updated state estimation vector at all moments, arrange them in chronological order, and use them as the optimized reference trajectory. If no, use the updated state estimation vector at the current moment as the previous moment's state estimation vector for the next iteration, and use the updated estimation error covariance matrix at the current moment as the previous moment's estimation error covariance matrix for the next iteration, and then proceed with the next iteration.

[0043] S4, Positive Catching-Up Phase – Dimensional Reduction and Precise Alignment: The goal of this phase is to quickly and accurately estimate the remaining navigation error state, especially the azimuth misalignment angle, based on known device errors.

[0044] S401, Data Compensation and Inertial Navigation Calculation: Acquire the target data of the inertial measurement unit in real time, and use the calibration parameters (i.e., gyroscope constant drift) output in step S3. and accelerometer zero bias The target data is compensated to obtain the compensated target data; the compensation formula is: ; ; in, The angle increment after compensation, This is the original angle increment. This is an estimate of the gyroscope's constant drift. The sampling time interval, The speed increment after compensation The speed increment after compensation This is the zero bias estimate for the accelerometer.

[0045] Inertial navigation calculations are performed based on the compensated target data to obtain the real-time navigation status.

[0046] S402. Constructing a dimension-reduced state vector and pseudo-absolute position observation: The state vector is reduced to a reduced-dimensional state vector that includes attitude misalignment angle, velocity error, and position error. Using the position of the optimized reference trajectory at the corresponding time as the pseudo-absolute position observation, an observation equation is constructed as the difference between the inertial calculated position and the position of the optimized reference trajectory: To ensure time alignment, the system needs to use the precise timestamps recorded in step S1 at each moment of the forward filtering. The position that strictly corresponds to the current forward calculation time is obtained from the optimized reference trajectory data through timestamp indexing or interpolation. Specifically, if the optimized reference trajectory is in If a data point exists at any given time, it is directly indexed; if no corresponding point exists due to differences in sampling rates, linear or higher-order interpolation is performed using optimized trajectory data from adjacent time points to obtain the data. High-precision reference position at any given time.

[0047] The observed value is the difference between the inertial calculated position and the reference position: ; The observation equation is: ; in, These are pseudo-absolute position observations. For inertial position calculation, For positional error, To observe noise.

[0048] S403. Fine estimation based on sequential Kalman filter: A dimensionality-reduced standard forward inertial navigation error equation is established as the state equation. A Kalman filter is used for estimation. During measurement updates, position observations are typically two-dimensional (planar) or three-dimensional vectors. Their measurement noise covariance matrix can usually also be modeled as a diagonal matrix (assuming position errors in each direction are independent). Therefore, sequential processing can be applied again to decompose the two-dimensional or three-dimensional position observations into independent scalar observations for processing, avoiding matrix inversion.

[0049] Since device errors are compensated and observation accuracy is high, the standard Kalman filter can achieve optimal estimation. The filter converges quickly, outputting the final high-precision attitude misalignment angle and position error.

[0050] The pseudo-absolute position observations are used as measurement inputs, and a sequential Kalman filter is used to estimate the dimensionality-reduced state vector.

[0051] The specific filtering process is as follows: For the current moment in the optimized reference trajectory, based on the reduced state estimation vector and the reduced estimation error covariance matrix of the previous moment, and combined with the reduced forward inertial navigation error equation, time update is performed to obtain the predicted reduced state estimation vector and the predicted reduced estimation error covariance matrix of the current moment.

[0052] The pseudo-absolute position observation at the current moment is used as the measurement input for measurement update: the pseudo-absolute position observation is a two-dimensional or three-dimensional vector, and the pseudo-absolute position observation is decomposed into scalars with the same number of dimensions and processed sequentially. Each scalar observation undergoes scalar gain calculation, state update, and covariance update sequentially, where the denominator of the scalar gain calculation is a scalar, and the inversion operation degenerates into a reciprocal calculation.

[0053] When the measurement noise covariance matrix is ​​a non-diagonal constant matrix, the orthogonal matrix and diagonal matrix are first obtained through eigenvalue decomposition. The observations are then orthogonally transformed to obtain the transformed observations. Finally, the transformed observations are processed by scalars in sequence.

[0054] After processing all scalar observations at the current time, we obtain the updated dimensionality-reduced state estimation vector and the updated dimensionality-reduced estimation error covariance matrix at the current time.

[0055] Determine whether all time points in the optimized reference trajectory have been processed. If yes, output the dimension-reduced state estimation vector and the dimension-reduced estimation error covariance matrix updated at all time points, and use them as the dimension-reduced state vector. If no, use the dimension-reduced state estimation vector updated at the current time point as the dimension-reduced state estimation vector of the previous time point for the next iteration, and use the dimension-reduced estimation error covariance matrix updated at the current time point as the dimension-reduced estimation error covariance matrix of the previous time point for the next iteration, and then perform the next iteration.

[0056] The attitude at the current moment is corrected by using the attitude misalignment angle in the dimensionality-reduced state estimation vector updated at all time points, and the position at the current moment is corrected by using the position error in the dimensionality-reduced estimation error covariance matrix updated at all time points, thus obtaining the high-precision navigation state at the current moment.

[0057] like Figure 3 As shown, this application compares the measurement update processes of sequential Kalman filtering and standard Kalman filtering. When processing multi-dimensional observations, the standard Kalman filter requires direct inversion of the matrix in the observation vector dimension, with computational complexity proportional to the cube of the observation dimension, posing a heavy burden on resource-constrained embedded navigation computers. In contrast, the sequential Kalman filter used in this application first decomposes the multi-dimensional observations into multiple independent scalar observations, and then sequentially updates the measurement for each scalar observation. In this process, the matrix inversion in gain calculation degenerates into scalar division, reducing computational complexity to a level proportional to the square of the state dimension, significantly improving computational efficiency. Figure 3 The left side shows the parallel matrix inversion processing method of standard filtering, while the right side shows the serial scalar processing flow of this application, clearly demonstrating the advantages of this application in terms of computational efficiency. By splitting multidimensional observations into scalars and processing them sequentially, this application effectively avoids high-dimensional matrix inversion operations, reduces the computational burden on the embedded navigation computer, and meets real-time requirements.

[0058] This completes the entire process of autonomous alignment and calibration during travel, enabling efficient autonomous alignment and calibration without relying on external references, requiring specific maneuvers, resisting disturbances during travel, and simultaneously calculating.

[0059] Furthermore, sequential filtering is integrated into the measurement update steps of steps S3 and S4. Its key advantage lies in reducing computational complexity from... Reduce to ,in, As an observation dimension, n For the state dimension.

[0060] In the filter measurement update of steps S3 and S4, if the observation dimension is And if the sequential condition is satisfied (i.e., the measurement noise covariance matrix R is a diagonal matrix, or can be diagonalized by orthogonal transformation), then the multidimensional observation vector can be decomposed into... r The first scalar observation is processed sequentially. For the first... scalar observations ( (e.g., 1, 2, ..., r), its filter update formula is as follows: The specific formula for calculating scalar gain is as follows: ; in, For current scalar observation The corresponding scalar gain, To process the previous scalar observation The subsequent estimated error covariance matrix, For current scalar observation The corresponding observation matrix, For current scalar observation The transpose of the corresponding observation matrix, For current scalar observation The variance of observation noise.

[0061] The specific formula for state update is: ; in, To process current scalar observations The subsequent state estimation vector, To process the previous scalar observation The subsequent state estimation vector, This represents the current scalar observation value.

[0062] The specific formula for covariance update is: ; in, To process current scalar observations The subsequent estimated error covariance matrix, It is an identity matrix.

[0063] Complete all r After processing the scalar observations, let the final filtered output be: .

[0064] The following points should be noted during implementation: When the measured noise covariance matrix When the matrix is ​​diagonal, the original observations can be processed sequentially directly.

[0065] When the measured noise covariance matrix When the matrix is ​​a non-diagonal constant matrix, eigenvalue decomposition is required first to obtain an orthogonal matrix and a diagonal matrix. An orthogonal transformation is then performed on the observations to obtain the transformed observations, which are then subjected to sequential processing. This decomposition can be completed offline.

[0066] In sequential processing, the denominator in the scalar gain calculation formula It is a scalar, so we can directly find its reciprocal without matrix inversion.

[0067] This application also provides an application scenario in which the above-mentioned in-journey anti-disturbance alignment method based on inverse loopback and convex optimization is applied. Specifically, the in-journey anti-disturbance alignment method provided in this embodiment can be applied to the autonomous navigation scenario of an inertial navigation system. The autonomous navigation scenario of an inertial navigation system includes a high-precision navigation state generation stage; the high-precision navigation state generation stage is used to generate a high-precision navigation state based on the cached raw data and the target data of the inertial measurement unit. The in-journey anti-disturbance alignment method provided in this embodiment belongs to the high-precision navigation state generation stage.

[0068] In one exemplary embodiment, a computer device is provided, the internal structure of which can be as shown in the figure. Figure 4 As shown, this computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and a database. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The database stores and processes data. The I / O interfaces are used for exchanging information between the processor and external devices. The communication interface is used for communicating with external terminals via a network connection. When the computer program is executed by the processor, it implements an inter-journey anti-disturbance alignment method based on inverse loopback and convex optimization.

[0069] Those skilled in the art will understand that Figure 4 The structures shown are merely block diagrams of some structures related to the present application and do not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than shown in the figures, or combine certain components, or have different component arrangements. In an exemplary embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps in the above-described method embodiments.

[0070] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0071] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0072] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties. Moreover, the collection, use and processing of the relevant data are carried out in compliance with the relevant data protection laws and policies of the country where the location is located, and with the authorization granted by the owner of the corresponding device.

[0073] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments described above. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

[0074] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0075] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0076] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for inter-journey anti-disturbance alignment based on inverse loop and convex optimization, characterized in that, The inter-journey anti-disturbance alignment method based on inverse loop and convex optimization includes: Step S1: During the movement of the carrier, continuously collect and cache the raw data of the inertial measurement unit to obtain the cached raw data; Step S2: Perform forward inertial navigation calculation on the cached raw data to construct inverse time series data; Step S3: Construct virtual velocity observations and virtual heading change observations using lap-loop constraints, and use the inverse time series data as the driving input for the state recursion of the sequential robust filter, and use the virtual velocity observations and virtual heading change observations as the measurement inputs of the sequential robust filter. Solve the filter gain through convex optimization, perform state estimation on the constructed full-state inverse error model, and output the calibration parameters of the inertial measurement unit and the optimized reference trajectory; the sequential robust filter is a measurement update based on the robust filter combined with sequential filtering processing. Step S4: First, acquire the target data of the inertial measurement unit in real time. Then, compensate the target data using the calibration parameters to obtain compensated target data. Perform forward inertial navigation calculation based on the compensated target data to obtain the real-time navigation state. Next, use the position of the optimized reference trajectory at the corresponding time as the pseudo-absolute position observation value and use the pseudo-absolute position observation value as the measurement input of the sequential Kalman filter to estimate the dimension-reduced state vector. Finally, use the dimension-reduced state vector to correct the real-time navigation state and output a high-precision navigation state. The sequential Kalman filter is a measurement update based on the Kalman filter combined with sequential filtering processing.

2. The inter-journey anti-disturbance alignment method based on reverse loop and convex optimization according to claim 1, characterized in that, In step S2, the cached raw data is subjected to forward inertial navigation calculation to construct inverse time series data, specifically including: The cached raw data is subjected to forward inertial navigation calculation to obtain initial navigation state data containing timestamps; Based on the timestamp, the initial navigation state data is rearranged from largest to smallest to obtain reverse time series data.

3. The inter-journey anti-disturbance alignment method based on reverse loop and convex optimization according to claim 1, characterized in that, In step S3, the construction process of the full-state inverse error model is as follows: A full state vector containing attitude misalignment angle, velocity error, position error, gyroscope constant drift, and accelerometer zero bias is constructed, and the inverse time state equation is derived from the standard inertial navigation error equation through time inversion. The combination of the full-state vector and the inverse-time state equation is used as the full-state inverse error model.

4. The inter-journey anti-disturbance alignment method based on reverse loop and convex optimization according to claim 3, characterized in that, In step S3, virtual velocity observations and virtual heading change observations are constructed using laparoscopy constraints, specifically including: The start and end times of the reverse time series data are obtained and used as the start and end times of the reverse filtering, respectively. At the inverse filtering start point and the inverse filtering end point, the observed value of the carrier velocity error is set to zero to construct a virtual velocity observation; Obtain the total heading change of the loop path, and construct a virtual heading change observation based on the total heading change; the virtual heading change observation is used to describe the quantitative relationship between the total heading change and the azimuth misalignment angle and the yaw gyroscope drift.

5. The inter-journey anti-disturbance alignment method based on reverse loop and convex optimization according to claim 1, characterized in that, In step S3, the sequential robust filter is a sequential H∞ filter; the inverse time series data is used as the driving input for the state recursion of the sequential robust filter, and the virtual velocity observation and the virtual heading change observation are used as the measurement input for the sequential robust filter. The filter gain is solved through convex optimization to estimate the state of the full-state inverse error model, and the calibration parameters of the inertial measurement unit and the optimized reference trajectory are output. Specifically, this includes: For the current moment in the inverse time series data, based on the state estimation vector and the estimation error covariance matrix of the previous moment, the full-state inverse error model is used for time update to obtain the predicted state estimation vector and the predicted estimation error covariance matrix of the current moment. The virtual velocity observation is split into multiple scalar observations, and the virtual heading change observation is treated as a single scalar observation. For each scalar observation, scalar gain calculation, state update, and covariance update are performed sequentially to obtain the updated state estimation vector and the updated estimation error covariance matrix at the current time. The scalar gain calculation is based on the H∞ criterion, and the parameters for scalar gain calculation are determined by solving a linear matrix inequality convex optimization problem. Determine whether all moments in the reverse time series data have been processed; If so, output the updated state estimation vector at the current time and the updated estimation error covariance matrix at all times. Extract the gyroscope constant drift and accelerometer zero bias from the updated state estimation vector at the current time as calibration parameters for the inertial measurement unit. Extract the position information from the updated state estimation vector at all times, arrange them in chronological order, and use them as the optimized reference trajectory. If not, the updated state estimation vector at the current time is used as the previous state estimation vector for the next iteration, and the updated estimation error covariance matrix at the current time is used as the previous estimation error covariance matrix for the next iteration, and the next iteration is performed.

6. The inter-journey anti-disturbance alignment method based on reverse loop and convex optimization according to claim 1, characterized in that, In step S4, the position of the optimized reference trajectory at the corresponding time is used as the pseudo-absolute position observation, and the pseudo-absolute position observation is used as the measurement input of the sequential Kalman filter to estimate the dimensionless state vector, specifically including: The position of the optimized reference trajectory at the corresponding time is used as the pseudo-absolute position observation; For the current moment in the optimized reference trajectory, the predicted reduced-dimensional state estimation vector and the predicted reduced-dimensional estimation error covariance matrix at the current moment are calculated based on the reduced-dimensional state estimation vector and the reduced-dimensional estimation error covariance matrix at the previous moment. The pseudo-absolute position observations are split into scalar observations with the same number of dimensions, and scalar gain calculation, state update and covariance update are performed on each scalar observation in sequence to obtain the dimension-reduced state estimation vector updated at the current time and the dimension-reduced estimation error covariance matrix updated at the current time. Determine whether all moments in the optimized reference trajectory have been processed; If so, output the dimension-reduced state estimation vector updated at all times and the dimension-reduced estimation error covariance matrix updated at all times, and use them as the dimension-reduced state vector; If not, the updated dimensionality reduction state estimation vector at the current moment is used as the previous dimensionality reduction state estimation vector for the next iteration, and the updated dimensionality reduction estimation error covariance matrix at the current moment is used as the previous dimensionality reduction estimation error covariance matrix for the next iteration, and the next iteration is performed.

7. The inter-journey anti-disturbance alignment method based on reverse loop and convex optimization according to claim 5 or 6, characterized in that, The specific formula for calculating the scalar gain is as follows: ； in, For current scalar observation The corresponding scalar gain, To process the previous scalar observation The subsequent estimated error covariance matrix, For current scalar observation The corresponding observation matrix, For current scalar observation The transpose of the corresponding observation matrix, For current scalar observation The observation noise variance; The specific formula for state update is: ； in, To process current scalar observations The subsequent state estimation vector, To process the previous scalar observation The subsequent state estimation vector, This is the current scalar observation value; The specific formula for covariance update is: ； in, To process current scalar observations The subsequent estimated error covariance matrix, It is an identity matrix.

8. The inter-journey anti-disturbance alignment method based on reverse loop and convex optimization according to claim 1, characterized in that, The inertial measurement unit includes a three-axis gyroscope and a three-axis accelerometer. The raw data of the inertial measurement unit includes raw angular increments and raw velocity increments. The reduced state vector includes attitude misalignment angle, velocity error, and position error.

9. A computer device, comprising: A memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that the processor executes the computer program to implement the inter-journey anti-disturbance alignment method based on reverse loop and convex optimization as described in any one of claims 1-8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the inter-journey anti-disturbance alignment method based on reverse loop and convex optimization as described in any one of claims 1-8.

Images