Pole region dynamic base alignment method, apparatus and medium using unified modeling language

Abstract

The application discloses a polar region moving base alignment method and device based on a unified modeling principle, and a medium, and the method comprises the following steps: acquiring sensor data and satellite receiver data; a vector observer model is constructed by using a unified modeling method; the vector observer model is discretized, a vector difference method is designed to optimize the discretized vector observer model, and a velocity error at an initial moment is obtained; attitude estimation is performed by using an OBA algorithm based on the optimized vector observer model; calibration is performed based on the attitude estimation value, and the above steps are repeated until a set calibration time length or iteration number is reached. By constructing the vector observer model by using the unified modeling method, the application can eliminate the influence of polar region singular points on initial alignment, and solves the problem that the azimuth is affected by the convergence of the longitude in the polar region alignment process.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation systems, specifically to a polar dynamic base alignment method, device, and medium based on a unified modeling principle. Background Technology

[0002] Current navigation and positioning systems typically use attitude, velocity, and position data within a navigation system for navigation and positioning. This method is widely used in low and mid-latitude regions. However, as latitude increases, longitude lines converge rapidly, leading to uncertain errors, especially in azimuth angles. In polar regions, undefined singularities exist, posing challenges to navigation and positioning. Furthermore, in the initial alignment phase, the inability to determine the azimuth angle prevents the alignment process from being completed.

[0003] The Chinese patent with publication number CN114910097A and titled "Method for Alignment of Polar Dynamic Base to Eliminate Initial Velocity Disturbance" uses a grid coordinate system for error modeling. The constructed model is based on transformed high-latitude data. Although it can be applied to polar navigation, it has computational singularity problems in mid- and low-latitude regions. Summary of the Invention

[0004] The purpose of this invention is to provide a polar dynamic base alignment method, device and medium based on a unified modeling principle, which can be applied not only to polar regions but also to low and medium latitudes, and can eliminate the influence of polar singularities on the initial alignment.

[0005] A unified modeling principle method for polar dynamic base alignment is proposed. The invention constructs a vector observer using the unified modeling principle and utilizes vector differences to mitigate the influence of initial velocity errors, thereby achieving the goal of polar dynamic base alignment.

[0006] The technical solution to achieve the purpose of this invention is as follows:

[0007] The polar dynamic base alignment method based on the unified modeling principle includes the following steps:

[0008] Acquire sensor data and satellite receiver data;

[0009] A vector observer model is constructed using a unified modeling approach; the constructed vector observer model is as follows:

[0010]

[0011] In the formula, This indicates the output acceleration of the accelerometer at any latitude; This represents the direction cosine matrix of the loaded system relative to the initial loaded system at time t; Indicates the reference vector; This represents the mapping of the velocity output by the satellite navigation receiver in the Earth coordinate system. This represents the direction cosine matrix of the Earth system relative to the initial Earth system at time t; This represents the mapping of the velocity output by the satellite navigation receiver at the initial moment in the Earth coordinate system; This represents the mapping of the Earth's rotational angular velocity in the Earth system; g e This represents the mapping of the gravity vector in the Earth system;

[0012] Discretize the vector observer model, design a vector interpolation method to optimize the discretized vector observer model, and calculate the initial velocity error.

[0013] An optimized vector observer model is used to estimate pose using the OBA algorithm.

[0014] Calibrate based on the attitude estimate, and repeat the above steps until the set number of iterations is reached.

[0015] A polar dynamic base alignment device based on a unified modeling principle includes: a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the polar dynamic base alignment method based on the unified modeling principle.

[0016] A computer storage medium storing an executable program, the executable program being executed by a processor to implement the steps of the polar dynamic base alignment method based on the unified modeling principle.

[0017] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0018] (1) This invention uses a unified modeling theory to construct a vector observer and uses vector difference to weaken the influence of initial velocity error, thereby achieving the purpose of polar dynamic base alignment. It can be applied not only to polar regions but also to mid- and low-latitude regions, avoiding the polar singularity problem.

[0019] (2) The present invention uses the vector difference method to eliminate the influence of the initial velocity error on the vector observer. Attached Figure Description

[0020] Figure 1 It is a diagram of the polar region dynamic base alignment structure based on the unified modeling principle.

[0021] Figure 2 This is a schematic diagram comparing the pitch and roll error angles of the traditional method and the method of this invention.

[0022] Figure 3 This is a schematic diagram comparing the alignment roll error angle of the traditional method and the method of this invention.

[0023] Figure 4 This is a schematic diagram comparing the heading error angle of the traditional method and the method of this invention. Detailed Implementation

[0024] The present invention will now be described in further detail with reference to the accompanying drawings and examples:

[0025] This invention proposes a polar region dynamic base alignment method based on a unified modeling principle, such as... Figure 1 As shown, it includes the following steps:

[0026] Step 1: Acquire sensor data and satellite receiver data;

[0027] From the inertial sensor measurement model, we can see that:

[0028]

[0029] In the formula, f represents the output acceleration of the accelerometer at any latitude; b b represents the true acceleration at any latitude; a Indicates zero bias acceleration; η a This indicates the noise level measured by the accelerometer; This represents the output angular velocity of a gyroscope at any latitude; b represents the true angular velocity at any latitude; g Indicates zero bias of the gyroscope; η g This indicates the noise level measured by the gyroscope.

[0030] When the satellite navigation receiver does not receive data, the inertial sensor is used to calculate the increments of angular velocity and acceleration.

[0031]

[0032] In the formula, This indicates the output acceleration of the accelerometer at any latitude; Δv1 represents the output angular velocity of the gyroscope at any latitude; Δv2 represents the velocity sample of the system in the first half-cycle; Δv1 represents the velocity sample of the system in the second half-cycle; Δθ1 represents the angular increment sample in the first half-cycle; Δθ2 represents the angular increment sample in the second half-cycle; Δt g Indicates the data sampling period of the satellite navigation system;

[0033] Step 2: Construct a vector observer using a unified modeling approach;

[0034] Upon receiving satellite navigation data, a unified modeling theory vector observer is constructed:

[0035]

[0036] In the formula, This indicates the output acceleration of the accelerometer at any latitude; This represents the direction cosine matrix of the loaded system relative to the initial loaded system at time t; Indicates the reference vector; This represents the mapping of the velocity output by the satellite navigation receiver in the Earth coordinate system. This represents the direction cosine matrix of the Earth system relative to the initial Earth system at time t; This represents the mapping of the velocity output by the satellite navigation receiver at the initial moment in the Earth coordinate system; This represents the mapping of the Earth's rotational angular velocity in the Earth system; g e This represents the mapping of the gravity vector in the Earth system;

[0037] Discretizing the above equation yields:

[0038]

[0039] In the formula, Represents the reference vector at time k; The reference vector at time k-1; Δv represents the direction cosine matrix of the loaded system relative to the initial loaded system at time k; b The velocity increment of the load system can be calculated using the following formula:

[0040]

[0041] In the formula, Δv b Δv1 represents the velocity increment of the load system; Δv2 represents the velocity sample of the load system in the first half-cycle; Δθ1 represents the angular increment sample in the first half-cycle; Δθ2 represents the angular increment sample in the second half-cycle.

[0042] Similarly, the discretization of the observation vector can be expressed as:

[0043]

[0044] In the formula, Represents the observation vector at time k; This represents the direction cosine matrix of the Earth system relative to the initial Earth system at time k; This represents the mapping of the velocity output by the satellite navigation receiver at time k in the Earth coordinate system; This represents the mapping of the velocity output by the satellite navigation receiver at the initial moment in the Earth coordinate system; β′ v，k The intermediate vector at time k can be calculated using the following formula:

[0045]

[0046] In the formula, β′ v，k Represents the intermediate vector at time k; β′ v，k-1 This represents the intermediate vector at time k-1; Δt represents the direction cosine matrix of the Earth system relative to the initial Earth system at time k-1; g Indicates the data sampling period of the satellite navigation system; This represents the mapping of the Earth's rotational angular velocity within the Earth system; This represents the mapping of the velocity output by the satellite navigation receiver at time k in the Earth coordinate system; This represents the mapping of the velocity output by the satellite navigation receiver at time k-1 in the Earth coordinate system. This represents the mapping of the gravity vector in the Earth system at time k;

[0047] Step 3: Design a vector interpolation method;

[0048] Since the vector observer's construction is related to the initial velocity, the initial velocity error will accumulate in the vector observer. Therefore, a vector interpolation method is designed for optimization:

[0049]

[0050] In the formula, Represents the optimized reference vector at time k; Represents the optimized observation vector at time k; Represents the reference vector at time k; Represents the observation vector at time k; Represents the reference vector at time i; Represents the observation vector at time i;

[0051] Step 4: Perform attitude estimation using the OBA algorithm;

[0052] The attitude K matrix can be constructed using an optimized vector observer:

[0053]

[0054] In the formula, K k Denotes the K matrix at time k; K k-1 Represents the K matrix at time k-1; Represents the optimized reference vector at time k; This represents the optimized observation vector at time k; the calculation process can be expressed as:

[0055]

[0056] In the formula, Represents the optimized reference vector at time k; This represents the optimized observation vector at time k.

[0057] Step 5: The number of iterations of the initial alignment process is M. If k = M, the alignment result is output and the alignment process is completed. If k < M, indicating that the alignment process is not completed, repeat the above Steps 1 to 5 until the alignment process ends.

[0058] Figures 2-4 This is a calibration schematic diagram of the calibration method of the present invention and the traditional method. It can be seen from the comparison diagram that the calibration error of the present invention is smaller and the convergence is better.

[0059] The present invention constructs a vector observer based on the unified modeling theory, and optimizes the vector observer by using the vector difference method to weaken the influence of the initial velocity error, realizes the alignment process of the moving base in the polar region, and solves the problem that the azimuth is affected by the longitude convergence in the polar region alignment process. It can be applied not only to the polar region but also to the mid-low latitudes, and can eliminate the influence of the polar singularity on the initial alignment. It should be noted that the innovation of the present invention lies in constructing a vector observer by unified modeling, which is an improvement on the existing calibration methods, and some well-known common knowledge involved will not be elaborated here.

Claims

1. A polar region dynamic base alignment method based on a unified modeling principle, characterized in that, Including the following steps: Acquire sensor data and satellite receiver data; A vector observer model is constructed using a unified modeling approach; the constructed vector observer model is as follows: In the formula, This indicates the output acceleration of the accelerometer at any latitude; This represents the direction cosine matrix of the loaded system relative to the initial loaded system at time t; Indicates the reference vector; This represents the mapping of the velocity output by the satellite navigation receiver in the Earth coordinate system. This represents the direction cosine matrix of the Earth system relative to the initial Earth system at time t; This represents the mapping of the velocity output by the satellite navigation receiver at the initial moment in the Earth coordinate system; This represents the mapping of the Earth's rotational angular velocity within the Earth system. This represents the mapping of the gravity vector in the Earth system; Discretize the vector observer model, design a vector interpolation method to optimize the discretized vector observer model, and calculate the initial velocity error. An optimized vector observer model is used to estimate pose using the OBA algorithm. Calibrate based on the attitude estimate, and repeat the above steps until the set number of iterations is reached; The discretized vector observer model is as follows: In the formula, Represents the reference vector at time k; The reference vector at time k-1; This represents the direction cosine matrix of the loaded system relative to the initial loaded system at time k; The velocity increment of the load system is calculated using the following formula: In the formula, Indicates the velocity increment of the load system; This represents the velocity sample of the system during the first half of the cycle; This represents the velocity sample of the load system in the second half of the cycle; This represents the first half-cycle angular increment sample; This indicates the second half-cycle angular increment sample; In the formula, Represents the observation vector at time k; This represents the direction cosine matrix of the Earth system relative to the initial Earth system at time k; This represents the mapping of the velocity output by the satellite navigation receiver at time k in the Earth coordinate system; This represents the mapping of the velocity output by the satellite navigation receiver at the initial moment in the Earth coordinate system; The intermediate vector at time k is represented by the following formula: In the formula, Represents the intermediate vector at time k; This represents the intermediate vector at time k-1; This represents the direction cosine matrix of the Earth system relative to the initial Earth system at time k-1; Indicates the data sampling period of the satellite navigation system; This represents the mapping of the Earth's rotational angular velocity within the Earth system. This represents the mapping of the velocity output by the satellite navigation receiver at time k in the Earth coordinate system; This represents the mapping of the velocity output by the satellite navigation receiver at time k-1 in the Earth coordinate system. This represents the mapping of the gravity vector in the Earth system at time k; The optimized vector observer model is as follows: In the formula, Represents the optimized reference vector at time k; Represents the optimized observation vector at time k; Represents the reference vector at time k; Represents the observation vector at time k; Represents the reference vector at time i; Represents the observation vector at time i; Pose estimation using the OBA algorithm specifically includes: Construct the pose K matrix based on the optimized vector observer model: In the formula, Represents the K matrix at time k; Represents the K matrix at time k-1; Represents the optimized reference vector at time k; This represents the optimized observation vector at time k; the calculation process is as follows: In the formula, Represents the optimized reference vector at time k; This represents the optimized observation vector at time k.

2. The polar region dynamic base alignment method based on a unified modeling principle according to claim 1, characterized in that, The acquisition of sensor data includes: Acceleration and angular velocity values ​​are obtained from the inertial sensor measurement model: In the formula, This represents the acceleration output by the accelerometer in any dimension. Represents real acceleration in any dimension; Indicates zero bias acceleration; This indicates the noise level measured by the accelerometer; This represents the output angular velocity of a gyroscope at any latitude; Represents the true angular velocity at any latitude; This indicates that the gyroscope has zero bias. This indicates the noise level measured by the gyroscope. Calculate the increments of angular velocity and acceleration using inertial sensors: In the formula, This indicates the output acceleration of the accelerometer at any latitude; This represents the output angular velocity of a gyroscope at any latitude; This represents the velocity sample of the system during the first half of the cycle; This represents the velocity sample of the load system in the second half of the cycle; This represents the first half-cycle angular increment sample; This indicates the second half-cycle angular increment sample; This indicates the data sampling period of the satellite navigation system.

3. The polar region dynamic base alignment method based on the unified modeling principle according to claim 1, characterized in that, The satellite receiver has a sampling period of 1 second and a set number of iterations of 600.

4. A polar region moving base alignment device based on a unified modeling principle, characterized in that, include: A memory, a processor, and a computer program stored in the memory, wherein the processor, when executing the computer program, implements the polar dynamic base alignment method of the unified modeling principle as described in any one of claims 1-3.

5. A computer storage medium, characterized in that, The computer storage medium stores an executable program, which is executed by a processor to implement the steps of the polar dynamic base alignment method according to the unified modeling principle of any one of claims 1-3.

Images