A fully autonomous, fast, meter-level 3D positioning method based on rotation-modulated inertial navigation

Abstract

This invention belongs to the field of high-precision inertial navigation technology, specifically relating to a fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation. Applied to a mobile carrier equipped with rotational modulation inertial navigation, the method includes the following steps: After completing high-precision alignment, the rotational modulation inertial navigation system (INS) transitions to navigation, outputting real-time calculated data; the eastward and northward velocities in the real-time calculated data output by the INS are transferred from the IMU measurement center to the frame rotation center; a Kalman filter model for the rotational modulation INS is established, and state transition calculations are performed on the errors in the real-time calculated data; it is determined whether the carrier has reached a stationary state; if it has reached a stationary state, the process proceeds to the next step; if it has not reached a stationary state, the process returns to the previous step; filtering calculations are performed, and after a filtering time greater than 5 seconds, the errors in longitude, latitude, eastward velocity, and northward velocity are obtained, and errors in the longitude, latitude, eastward velocity, and northward velocity of the rotational modulation INS are corrected; the altitude of the rotational modulation INS is corrected according to the altitude error correction formula; the corrected horizontal position and altitude of the current target measurement point are output, completing the positioning and mapping of the current target measurement point.

Description

Technical Field

[0001] This invention belongs to the field of high-precision inertial navigation technology, specifically relating to a fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation. Background Technology

[0002] The ability to achieve fully autonomous meter-level rapid 3D positioning without relying on any external information is of great significance for the rapid mapping and reconnaissance of target measurement points such as station locations in unknown environments. Similarly, there is a strong demand for fully autonomous meter-level rapid 3D positioning in special environments such as tunnels and mines.

[0003] Inertial navigation systems (INS) are currently the only fully autonomous, all-weather navigation method. However, due to errors in inertial components, navigation accuracy diverges over time. Rotation-modulated INS, through a rational and orderly rotation modulation strategy, automatically cancels out inertial component errors, significantly improving autonomous navigation and positioning accuracy. However, due to random errors in inertial components, these errors cannot be completely canceled, and thus cannot meet the requirements for meter-level positioning. Therefore, achieving fully autonomous, rapid, meter-level positioning without any reference information remains a technical challenge for the navigation community both domestically and internationally. Summary of the Invention

[0004] The purpose of this invention is to address the application scenarios of rapid site mapping and reconnaissance in unknown environments. Based on a vehicle-mounted high-precision rotary modulation inertial navigation system, a high-precision three-dimensional position error model of the rotary modulation inertial navigation system is established. By adopting a vehicle-mounted inertial navigation interval error correction scheme, fully autonomous meter-level three-dimensional rapid positioning is achieved. It has the advantages of high accuracy, full autonomy, all-weather operation, and speed and convenience. Engineering implementation has verified that it can achieve fully autonomous, second-level rapid single-point meter-level positioning during travel. It can be widely applied to fully autonomous three-dimensional meter-level positioning and mapping in special environments such as tunnels and mines.

[0005] The technical solution of the present invention: In order to achieve the above-mentioned objective, according to the first aspect of the present invention, a fully autonomous and rapid meter-level three-dimensional positioning method based on rotational modulation inertial navigation is proposed, which is applied to a mobile carrier equipped with rotational modulation inertial navigation, and includes the following steps:

[0006] Step 1: After completing high-precision alignment using rotation modulation, the inertial navigation system (INS) transitions to navigation. The INS outputs real-time calculated data, including latitude φ, longitude λ, altitude h, and eastward velocity V. E_S Northbound speed V N_S Horizontal velocity V U Pitch angle θ, roll angle γ, yaw angle ψ;

[0007] Step 2: Perform the calculation of the transfer of the eastward and northward velocities in the real-time solution data output by the inertial navigation system from the IMU measurement center to the frame rotation center; the IMU measurement center refers to the original measurement point of the real-time solution data output by the inertial navigation system; the frame rotation center refers to the rotational geometric center point of the rotational modulation inertial navigation frame.

[0008] Step 2 is performed throughout the navigation process to provide stable eastward and northward velocities for subsequent steps.

[0009] Step 3: Establish a rotating modulation inertial navigation Kalman filter model and perform state transition calculations on the error of the real-time solution data output by the inertial navigation system;

[0010] Step 3 is calculated throughout the entire navigation process and forms the basis for subsequent filtering calculations after the vehicle is automatically determined to be stationary.

[0011] Step 4: Determine if the carrier has reached a stationary state; if it has reached a stationary state, proceed to Step 5; if it has not reached a stationary state, return to Step 2.

[0012] Step 5: Perform filtering calculations using the established Kalman filter model. After the filtering time is greater than 5 seconds, obtain the errors in longitude, latitude, eastward velocity, and northward velocity. Correct the errors in longitude, latitude, eastward velocity, and northward velocity of the rotating modulation inertial navigation system. Correct the altitude error of the rotating modulation inertial navigation system according to the altitude error correction formula.

[0013] Step 6: Output the horizontal position and height of the current target measurement point after correction in Step 5, and complete the positioning and mapping of the current target measurement point.

[0014] Preferably, during the alignment process, the rotary modulation inertial navigation IMU rotates alternately around the Zs axis in both forward and reverse directions to modulate the constant error of the inertial navigation device, improve alignment and navigation accuracy, and provide a basis for subsequent precise error calibration. In the coordinate system of the rotary modulation inertial navigation IMU's Xs, Ys, and Zs axes, the Xs axis represents the pitch axis, the Ys axis represents the roll axis, and the Zs axis represents the yaw axis.

[0015] In one possible embodiment, after step 6 completes the localization and mapping of the current target measurement point, the process moves to the next target measurement point and returns to step 4 to perform localization and mapping of the next target measurement point. In another possible embodiment, step 5 further includes correcting the horizontal deflection angle of the rotating modulation inertial navigation mathematical solution platform; the purpose is to improve the deflection angle accuracy of the rotating modulation inertial navigation mathematical solution platform, providing a foundation for high-precision measurement of the next target measurement point.

[0016] In one possible embodiment, step 2, the calculation process for transferring the eastward and northward velocities in the real-time solution data output by the inertial navigation system from the IMU measurement center to the frame rotation center specifically includes:

[0017] V E =V E_S +(C 13 ·ω y -C 12 ·ω z )·r x +C 11 ·ω z ·r y -C 11 ·ω y ·r z

[0018] V N =V N_S +(C 23 ·ω y -C 22 ·ω z )·r x +C 21 ·ω z ·r y -C 21 ·ω y ·r z

[0019] Where V E V N V represents the eastward and northward velocities of the frame's rotation center; E_S V N_S For the eastward and northward velocities of the IMU measurement center; r x r y r z These are the parameters for calculating the transfer from the IMU measurement center to the frame rotation center; ω y ω z These are the angular velocities of the IMUYs and Zs axes, respectively; C ij Let be the element in the i-th row and j-th column of the IMU attitude matrix.

[0020] In one possible embodiment, the rotationally modulated inertial navigation Kalman filter model established in step 3 is as follows:

[0021] State variables:

[0022]

[0023] Note: δφ is latitude error, δλ is longitude error, δh is altitude error, and δV is latitude error. E For eastward velocity error, δVN For northbound velocity error, δV U For the upward velocity error, φ E Eastward deflection angle, φ N Northward deflection angle, φ U For the celestial deflection angle, D X For Xs-axis gyroscope drift, D Y For Ys-axis gyroscope drift, D Z For Zs-axis gyroscope drift, Add zero position to Xs axis Add zero position to the Ys axis, Add zero position and D to the Zs axis. N For the northward drift of the geography system, D U For the celestial drift of the geography system, α XZ The installation angle α is the tilt angle for a gyroscope that rotates around the positive Zs axis on the Xs axis. XY Install the deflection angle and α for the gyroscope that rotates around the positive direction of the Ys axis on the Xs axis. YX Install the deflection angle and α for the gyroscope that rotates around the positive direction of the Xs axis on the Ys axis. ZX The installation angle ε is the deflection angle for a gyroscope that rotates around the positive Xs axis on the Zs axis. XY The mounting angle δK is the angle at which the Xs axis rotates about the positive direction of the Ys axis. GZ For the Zs-axis gyroscope calibration coefficient error, δK AX Add scale coefficient error and δK to Xs AZ Add a scale coefficient error to the Zs axis;

[0024] F is the state matrix; the matrix dimension is 25×25, where the element F(i,j) in the i-th row and j-th column of matrix F is defined as follows, and undefined elements are all 0;

[0025]

[0026]

[0027] F(3,6) = 1;

[0028]

[0029] F(4,8)=-f U F(4,9)=f N F(4:6,13:15)=C;

[0030]

[0031]

[0032] F(5,7)=fU F(5,9)=-f E ;

[0033] F(6,1)=-2V E ω ie sinφ;

[0034] F(6,7)=-f N F(6,8)=f E ;

[0035]

[0036] F(7:9,10:12) = C;

[0037] F(8,1)=-ω ie sinφ;

[0038] F(8,16) = 1;

[0039]

[0040] F(9,17) = 1;

[0041]

[0042]

[0043] Note: The average angular velocity within the axial filtering period of IMU Ys and Zs; ω represents the average specific force within the axial filtering period of IMUXs and Zs. ie f is the Earth's rotational angular rate. E For the eastward direction of the geography system; f N For the northward comparison of the geography system; f U It is a comparison of the celestial direction of the geography system.

[0044] In one possible embodiment, the specific process of performing state transition calculation on the error of the real-time solution data output by the inertial navigation system in step 3 includes:

[0045]

[0046]

[0047] X K / K-1 =φ K / K-1 ·X K-1

[0048]

[0049] P K-1 =P K / K-1

[0050] X K-1 =X K / K-1

[0051] φ K / K-1 For t K-1 to t K The one-step transition matrix at time step;

[0052] Q K-1 Let V be the system noise variance matrix;

[0053] X K / K-1 For t K-1 to t K Predicting the state at any given moment in one step; X K-1 For t K-1 The time-state variable is initialized to 0.

[0054] P K / K-1 For t K-1 to t K The one-step prediction mean square error matrix at time P; K-1 For t K-1 The mean square error matrix at time t;

[0055] I is the identity matrix, with the same dimension as matrix F; T F The filter period;

[0056] Filter mean square error matrix P K-1 Initialize as a 25×25 dimensional matrix:

[0057] P K-1 =diag(6e-8,6e-8,0,0.25,0.25,0,3e-4,3e-4,6e-2,1e-12,1e-12,1e-12,

[0058] 2.5e-5, 2.5e-5, 2.5e-5, 2.35e-15, 2.35e-15,0,…,0)

[0059] The filter system noise matrix Q is initialized as a 25×25 matrix:

[0060] Q=diag(0,0,0,5e-5,5e-5,5e-5,5e-13,5e-13,5e-13,0,...,0,0,0).

[0061] In one possible embodiment, the specific process of determining whether the carrier has reached a stationary state in step 4 includes:

[0062] A check is performed every 10ms. If the following two conditions are met simultaneously for one consecutive second, the state of stillness is determined to have been reached; otherwise, the state of stillness is determined not to have been reached:

[0063] |ΔV E |<0.01m / s,|ΔV N |<0.01m / s;

[0064]

[0065] Where: ΔV E ΔV N These are the eastward and northward speed increments within the current 0.5 seconds, respectively.

[0066] These are the incremental sliding values ​​of the Xs-axis, Ys-axis, and Zs-axis angles within the current 0.5 seconds.

[0067] In one possible embodiment, the specific process of performing filtering calculations using the established Kalman filter model in step 5 includes:

[0068] Once the system is determined to be stationary, the filtering calculation begins using the following formula, with a filtering period ranging from 0.05 to 0.5 seconds.

[0069]

[0070] X K =X K / K-1 +K K ·(Z K -H K ·X K / K-1 )

[0071] P K =(IK K ·H K )·P K / K-1

[0072] P K-1 =P K

[0073] Where K K This is the filter gain matrix;

[0074] X K For the filtered t K Time-state variables;

[0075] Z K For t K Time measurement;

[0076] P K For tK Time-based mean square error matrix estimation;

[0077] H k This is a measurement matrix with dimensions 3×25, where all elements are 0 except for the following:

[0078] H k (1,4)=1,H k (2,5)=1,H k (3,6)=1

[0079] R k Measure the noise matrix for the Kalman filter model:

[0080] R k =diag(4e-2,4e-2,4e-2).

[0081] In one possible embodiment, the specific process of longitude and latitude error correction in step 5 includes:

[0082] After the filtering time is greater than 5 seconds, the filtered t is obtained. K Time-state variable X K Longitude and latitude errors are corrected according to the following formula, and the corrected longitude λ is output. mdy latitude φ mdy ;

[0083] φ mdy =φ-X K (1)

[0084] λ mdy =λ-X K (2)

[0085] Where X K (1) X K (2) is t K The first and second elements of the state variables in the Kalman filter model at time step.

[0086] In one possible embodiment, in step 5, while correcting for the horizontal position error, the height error is calculated and corrected according to the following formula:

[0087] h mdy =h-Δh

[0088]

[0089]

[0090]

[0091] ΔA U =ΔA U+ΔA Ui

[0092] Where: h mdy The corrected height value is given by Δh, which is the height error estimate; ΔA U To add zero to the cumulative estimated celestial position, the initial value is 0; ΔA Ui Add a zero increment to the celestial axis for this correction, with an initial value of 0;

[0093] δV Ui The current celestial velocity error is expressed in meters per second.

[0094] ΔT is the time interval between two altitude corrections, in seconds; t i For the current time, t i-1 The last altitude correction time, in seconds;

[0095] g represents the gravity at the current location, and R represents the Earth's radius.

[0096] In one possible embodiment, after completing the horizontal position error correction as described above, the mathematical platform deflection angle is calculated and corrected according to the following formula:

[0097]

[0098]

[0099] k = k0 + k1·ΔT + k2·ΔT 2

[0100] Where: φ E φ N These are the calculated eastward and northward platform deflection angles;

[0101] k is a correction coefficient, and k0 = 1.0038, k1 = -6.7e-6, and k2 = -5.21e-7 are all constants;

[0102] It is the average of the eastward and northward speeds over 5 seconds;

[0103] ΔT=t i -t i-1 It is the time interval between two corrections, the time t of the previous target measurement point measurement correction. i-1 Correction time t to the current target measurement point i Unit: seconds;

[0104] g is the current gravitational acceleration calculated using inertia, in meters per second. 2 .

[0105] According to a second aspect of the present invention, a computer-readable storage medium is provided, wherein the computer program, when executed by a processor, implements the steps of the above-described method.

[0106] Beneficial technical effects of the present invention:

[0107] This invention addresses the application scenario of rapid site mapping in unknown environments. Based on a vehicle-mounted high-precision rotational modulation inertial navigation system (INS), and according to the INS navigation error model, it automatically judges the vehicle's operating status and performs interval error correction to achieve fully autonomous meter-level three-dimensional rapid positioning. It has advantages such as high accuracy, full autonomy, all-weather capability, and speed and convenience. Engineering implementation has verified that it can achieve meter-level single-point positioning within seconds while fully autonomously moving. It can effectively solve the problem of fully autonomous single-point three-dimensional meter-level positioning in battlefield satellite-denied environments. The method of this invention can also be extended to fully autonomous three-dimensional meter-level positioning and mapping in special environments such as tunnels and mines, and has high application value. Attached Figure Description

[0108] To more clearly illustrate the technical solutions implemented in this invention, a simple explanation of the accompanying drawings used in the description of this invention will be provided below. Obviously, the drawings described below are merely some embodiments of this invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0109] Figure 1 This is a flowchart of a preferred embodiment of the fully autonomous meter-level three-dimensional positioning method of the present invention;

[0110] Figure 2 This is a schematic diagram illustrating the definition of the measurement center transfer parameters from the IMU measurement center to the frame rotation center in step 2 of this invention;

[0111] Figure 3 This is the horizontal position error curve of the rotation modulation inertial navigation output during the entire navigation process in Example 1;

[0112] Figure 4 This is the altitude error curve of the full-course rotation modulation inertial navigation output in Example 1. Detailed Implementation

[0113] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0114] The features and illustrative embodiments of various aspects of the present invention will now be described in detail. In the following detailed description, numerous specific details are set forth in order to provide a thorough understanding of the invention. However, it will be apparent to those skilled in the art that the invention may be practiced without requiring some of these specific details. The following description of embodiments is merely intended to provide a better understanding of the invention by illustrating examples of the invention. The invention is by no means limited to any specific setup and method set forth below, but covers any improvements, substitutions, and modifications to the structures, methods, and devices without departing from the spirit of the invention.

[0115] It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other, and the various embodiments can be referenced and cited in each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0116] Example 1

[0117] like Figure 1 As shown, a fully autonomous, fast, meter-level 3D positioning method based on rotational modulation inertial navigation is applied to a mobile carrier equipped with rotational modulation inertial navigation, and includes the following steps:

[0118] Step 1: Under the condition of stationary rotation modulation inertial navigation, complete the high-precision single-axis rotation alignment of the IMU;

[0119] Step 2: After 10 minutes of alignment, the rotary modulation inertial navigation system (IMU) automatically switches to navigation based on the carrier's motion state. During navigation, the IMU rotates in both directions around the Zs axis to modulate the constant error of the inertial navigation device, improving navigation accuracy during dynamic processes and providing a foundation for accurate error calibration after the carrier comes to a standstill. During navigation, the inertial navigation system calculates and outputs latitude φ, longitude λ, altitude h, and eastward velocity V in real time. E_S Northbound speed V N_S Horizontal velocity V U Pitch angle θ, roll angle γ, yaw angle ψ;

[0120] Step 3: As Figure 2 As shown, the calculation of the horizontal velocity measurement center shift is as follows:

[0121] The horizontal velocity is calculated from the IMU measurement center to the frame rotation center during the entire navigation process.

[0122] V E =V E_S +(C 13 ·ω y -C 12 ·ω z )·r x +C 11 ·ω z·r y -C 11 ·ω y ·r z

[0123] V N =V N_S +(C 23 ·ω y -C 22 ·ω z )·r x +C 21 ·ω z ·r y -C 21 ·ω y ·r z

[0124] Where V E V N V represents the eastward and northward velocities of the frame's rotation center; E_S V N_S For the eastward and northward velocities of the IMU measurement center; r x r y r z These are the parameters for calculating the transfer from the IMU measurement center to the frame rotation center; ω y ω z These are the angular velocities of the IMUYs and Zs axes, respectively; C ij Let be the element in the i-th row and j-th column of the IMU attitude matrix.

[0125] After the horizontal velocity measurement center is transferred and calculated, a stable horizontal velocity is obtained, which provides stable measurement information for subsequent filter calculations and error calibration.

[0126] Step 4: During the entire navigation process, calculate the state transition of the rotation modulation inertial navigation error based on the rotation modulation inertial navigation Kalman filter model (as shown in the following formula); this step is the basis for the subsequent automatic determination of the filtering calculation after the vehicle comes to rest;

[0127] Kalman filter model state transition calculation:

[0128]

[0129]

[0130] X K / K-1 =φ K / K-1 ·X K-1

[0131]

[0132] P K-1 =PK / K-1

[0133] X K-1 =X K / K-1

[0134] For t K-1 to t K The one-step transition matrix at time step;

[0135] Q K-1 Let V be the system noise variance matrix;

[0136] X K / K-1 For t K-1 to t K Predicting the state at any given moment in one step; X K-1 For t K-1 The state variable at time point is initialized to 0; P K / K-1 For t K-1 to t K The one-step prediction mean square error matrix at time P; K-1 For t K-1 The mean square error matrix at time T. I is the identity matrix, with the same dimension as matrix F; F The filter period is set to 0.1 seconds.

[0137] Filter mean square error matrix P K-1 Initialize as a 25×25 dimensional matrix:

[0138] P K-1 =diag(6e-8,6e-8,0,0.25,0.25,0,3e-4,3e-4,6e-2,1e-12,1e-12,1e-12,2.5e-5,2.5e-5,2.5e-5,2.35e-15,2.35e-15,0,…,0) The filter system noise array Q is initialized to 25×

[0139] 25-dimensional matrix:

[0140] Q=diag(0,0,0,5e-5,5e-5,5e-5,5e-13,5e-13,5e-13,0,...,0,0,0);

[0141] The Kalman filter model for rotation-modulated inertial navigation error is as follows:

[0142] Filter state variables:

[0143]

[0144] Note: δφ is latitude error, δλ is longitude error, δh is altitude error, and δV is latitude error. E For eastward velocity error, δVN For northbound velocity error, δV U For the upward velocity error, φ E Eastward deflection angle, φ N Northward deflection angle, φ U For the celestial deflection angle, D X For Xs-axis gyroscope drift, D Y For Ys-axis gyroscope drift, D Z For Zs-axis gyroscope drift, Add zero position to Xs axis Add zero position to the Ys axis, Add zero position and D to the Zs axis. N For the northward drift of the geography system, D U For the celestial drift of the geography system, α XZ The installation angle α is the tilt angle for a gyroscope that rotates around the positive Zs axis on the Xs axis. XY Install the deflection angle and α for the gyroscope that rotates around the positive direction of the Ys axis on the Xs axis. YX Install the deflection angle and α for the gyroscope that rotates around the positive direction of the Xs axis on the Ys axis. ZX The installation angle ε is the deflection angle for a gyroscope that rotates around the positive Xs axis on the Zs axis. XY The mounting angle δK is the angle at which the Xs axis rotates about the positive direction of the Ys axis. GZ For the Zs-axis gyroscope calibration coefficient error, δK AX Add scale coefficient error and δK to Xs AZ Add a scale coefficient error to the Zs axis. F is the state matrix. The matrix dimension is 25×25, where the element F(i,j) in the i-th row and j-th column of matrix F is defined as follows: undefined elements are all 0.

[0145]

[0146]

[0147] F(3,6) = 1;

[0148]

[0149] F(4,8)=-f U ;

[0150] F(4,9)=f N F(4:6,13:15)=C;

[0151]

[0152]

[0153] F(5,7)=fU F(5,9)=-f E ;

[0154]

[0155] F(6,7)=-f N F(6,8)=f E ;

[0156]

[0157] F(7:9,10:12) = C;

[0158] F(8,1)=-ω ie sinφ;

[0159] F(8,16) = 1;

[0160]

[0161] F(9,17) = 1;

[0162]

[0163]

[0164] Note: The average angular velocity within the axial filtering period of IMU Ys and Zs; ω represents the average specific force within the axial filtering cycles of the IMU (Xs, Zs). ie f is the Earth's rotational angular rate. E For the eastward direction of the geography system; f N For the northward comparison of the geography system; f U It is a comparison of the celestial direction of the geography system.

[0165] Step 5: Automatically determine whether the carrier has reached a stationary state; if it has reached a stationary state, proceed to step 6; if it has not reached a stationary state, return to step 3.

[0166] The rotary modulation inertial navigation system automatically determines whether the carrier is stationary or moving according to the following criteria.

[0167] A check is performed every 10ms. If the following two conditions are met simultaneously for one consecutive second, the state of stillness is determined to have been reached; otherwise, the state of stillness is determined not to have been reached:

[0168] |ΔV E |<0.01m / s,|ΔV N |<0.01m / s;

[0169]

[0170] Where: ΔV E ΔV N These are the eastward and northward speed increments within the current 0.5 seconds, respectively.

[0171] These are the incremental sliding values ​​of the Xs-axis, Ys-axis, and Zs-axis angles within the current 0.5 seconds.

[0172] Step 6: Perform Kalman filter model filtering calculation according to the following formula. The filtering period is 0.1 seconds. After the filtering time is greater than 5 seconds, the accurate longitude, latitude, eastward velocity, and northward velocity error are obtained.

[0173] Once the system is determined to be stationary, the filtering calculation begins using the following formula, with a filtering period of 0.1 seconds.

[0174] X K =X K / K-1 +K K ·(Z K -H K ·X K / K-1 )

[0175] P K =(IK K ·H K )·P K / K-1

[0176] P K-1 =P K

[0177] Where K K This is the filter gain matrix;

[0178] X K For the filtered t K Time-state variables;

[0179] Z K For t K Time measurement; P K For t K Time-based mean square error matrix estimation;

[0180] H k This is a measurement matrix with dimensions 3×25, where all elements are 0 except for the following:

[0181] H k (1,4)=1,H k (2,5)=1,H k (3,6)=1

[0182] R k Measure the noise matrix for the Kalman filter model:

[0183] R k =diag(4e-2,4e-2,4e-2);

[0184] Step 7: Correct the horizontal position error based on the filter results from Step 6;

[0185] After continuously estimating the time to be greater than 5 seconds, the filtered t is obtained. K Time-state variable X K Longitude and latitude errors are corrected according to the following formula, and the corrected longitude λ is output. mdy latitude φ mdy .

[0186] φ mdy =φ-X K (1)

[0187] λ mdy =λ-X K (2)

[0188] Where X K (1) X K (2) is t K The first and second elements of the state variables in the Kalman filter model at time step;

[0189] Step 8: Correction of horizontal deflection angle on the mathematical platform

[0190] The horizontal deflection angle of the rotating modulation inertial navigation mathematical solution platform is corrected; the purpose is to improve the deflection angle accuracy of the mathematical platform and provide a basis for high-precision measurement of the next target measurement point.

[0191] When the carrier is determined to be stationary for more than 5 seconds, after completing the horizontal position error correction as described above, calculate and correct the mathematical platform deflection angle using the following formula:

[0192]

[0193]

[0194] k = k0 + k1·ΔT + k2·ΔT 2

[0195] Where: φ E φ N These are the calculated eastward and northward platform deflection angles;

[0196] k is a correction coefficient, and k0 = 1.0038, k1 = -6.7e-6, and k2 = -5.21e-7 are all constants;

[0197] It is the average of the eastward and northward speeds over 5 seconds;

[0198] ΔT=t i -t i-1 It is the time interval between two corrections (the time t of the previous target measurement point measurement correction). i-1 Correction time t to the current target measurement point i (Unit: seconds)

[0199] g is the current gravitational acceleration calculated using inertia, in meters per second. 2 ;

[0200] Step 9: Height Error Correction. If the carrier remains stationary for more than 5 seconds, after correcting the horizontal position error, calculate and correct the height error using the following formula:

[0201] h mdy =h-Δh

[0202]

[0203]

[0204]

[0205] ΔA U =ΔA U +ΔA Ui

[0206] Where: h mdy The corrected height value is given by Δh, which is the height error estimate; ΔA U To add zero to the cumulative estimated celestial position, the initial value is 0; ΔA Ui The zero-position increment of the astronomical accelerator has been corrected in this update, with an initial value of 0.

[0207] δV Ui The current celestial velocity error is expressed in meters per second.

[0208] ΔT is the time interval between two altitude corrections, in seconds; t i For the current time, t i-1 The last altitude correction time, in seconds;

[0209] g represents the gravity at the current location, and R represents the Earth's radius.

[0210] Step 10: Output the horizontal position and altitude corrected in steps 7 and 9 to complete the location mapping of the current target measurement point. Horizontal position refers to longitude and latitude.

[0211] Step 11: Return to step 5 and proceed to the next target measurement point for mapping.

[0212] Example 2

[0213] The difference between Example 2 and Example 1 is that step 5 further includes, after completing the horizontal position error correction, calculating and correcting the mathematical platform deflection angle according to the following formula:

[0214]

[0215]

[0216] k = k0 + k1·ΔT + k2·ΔT 2

[0217] Where: φ E φ N These are the calculated eastward and northward platform deflection angles;

[0218] k is a correction coefficient, and k0 = 1.0038, k1 = -6.7e-6, and k2 = -5.21e-7 are all constants;

[0219] It is the average of the eastward and northward speeds over 5 seconds;

[0220] ΔT=t i -t i-1 It is the time interval between two corrections, the time t of the previous target measurement point measurement correction. i-1 Correction time t to the current target measurement point i Unit: seconds;

[0221] g is the current gravitational acceleration calculated using inertia, in meters per second. 2 .

[0222] As shown in Table 1, measurements were taken at 10 target measurement points, and the results are as follows;

[0223] Table 1 Measurement results of Example 2

[0224]

[0225] The measurement results (horizontal and vertical positioning results) are compared with the known positions of 10 measurement points, as shown in Table 1. The measurement error statistics are as follows: longitude accuracy is 3.1 meters, latitude accuracy is 2.7 meters, and height accuracy is 0.6 meters, achieving meter-level measurement and positioning accuracy.

[0226] like Figure 3 , Figure 4The figure shows the rotation modulation inertial navigation error curve for the entire measurement process. As can be seen from the figure, the horizontal position and height accuracy at the measurement point are accurately corrected, achieving meter-level measurement and positioning accuracy.

[0227] Comparative Example 1:

[0228] Comparative Example 1 did not use steps 3, 8, and 9 of Embodiments 1 and 2 of the present invention, but only output through the Kalman filter model; measurements were performed on 10 target measurement points, and the measurement results are shown in Table 2;

[0229] The measurement results (horizontal and altitude positioning results) are compared with the known positions of 10 measurement points, as shown in Table 2. The measurement error statistics are as follows: the longitude accuracy is about 17.01 meters, the latitude accuracy is about 16.72 meters, and the altitude accuracy is about 18.59 meters, which cannot reach the meter-level measurement and positioning accuracy.

[0230] Table 2 shows the measurement results of Comparative Example 1.

[0231]

[0232] While the embodiments disclosed in this invention are as described above, the content is merely for the purpose of facilitating understanding of the invention and is not intended to limit the invention. Any person skilled in the art to which this invention pertains may make any modifications and changes to the form and details of the implementation without departing from the spirit and scope disclosed herein; however, the scope of patent protection of this invention shall still be determined by the scope defined in the appended claims.

Claims

1. A fully autonomous, rapid, meter-level three-dimensional positioning method based on rotationally modulated inertial navigation, characterized in that, The method for application to a mobile carrier equipped with a rotation-modulated inertial navigation system includes the following steps: Step 1: After completing high-precision alignment using rotation modulation, the inertial navigation system (INS) transitions to navigation. The INS outputs real-time calculated data, including latitude φ, longitude λ, altitude h, and eastward velocity V. E_S Northbound speed V N_S Horizontal velocity V U Pitch angle θ, roll angle γ, yaw angle ψ; Step 2: Perform the calculation of the transfer of the eastward and northward velocities in the real-time solution data output by the inertial navigation system from the IMU measurement center to the frame rotation center; the IMU measurement center refers to the original measurement point of the real-time solution data output by the inertial navigation system; the frame rotation center refers to the rotational geometric center point of the rotational modulation inertial navigation frame. Step 3: Establish a rotating modulation inertial navigation Kalman filter model and perform state transition calculations on the error of the real-time solution data output by the inertial navigation system; Step 4: Determine if the carrier has reached a stationary state; if it has reached a stationary state, proceed to Step 5; if it has not reached a stationary state, return to Step 3. Step 5: Perform filtering calculations using the established Kalman filter model. After the filtering time is greater than 5 seconds, obtain the errors in longitude, latitude, eastward velocity, and northward velocity. Correct the errors in longitude, latitude, eastward velocity, and northward velocity of the rotating modulation inertial navigation system. Correct the altitude error of the rotating modulation inertial navigation system according to the altitude error correction formula. Step 6: Output the horizontal position and height of the current target measurement point after correction in Step 5, and complete the positioning and mapping of the current target measurement point; the carrier continues to move to the next target point for measurement, and returns to Step 4.

2. The fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 1, characterized in that, After step 6 completes the positioning and mapping of the current target measurement point, proceed to the next target measurement point, return to step 4, and perform the positioning and mapping of the next target measurement point.

3. A fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to any one of claims 1 or 2, characterized in that, Step 5 also includes correcting the horizontal deflection angle of the rotation modulation inertial navigation mathematical solution platform.

4. The fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 1, characterized in that, In step 2, the calculation process for transferring the eastward and northward velocities in the real-time solution data output by the inertial navigation system from the IMU measurement center to the frame rotation center specifically includes: V E =V E_S +(C 13 ·oh y -C 12 ·oh z )·r x +C 11 ·oh z ·r y -C 11 ·oh y ·r z V N =V N_S +(C 23 ·oh y -C 22 ·oh z )·r x +C 21 ·oh z ·r y -C 21 ·oh y ·r z Where V E V N V represents the eastward and northward velocities of the frame's rotation center; E_S V N_S For the eastward and northward velocities of the IMU measurement center; r x r y r z These are the parameters for calculating the transfer from the IMU measurement center to the frame rotation center; ω y ω z These are the angular velocities of the IMU Ys and Zs axes, respectively; C ij Let be the element in the i-th row and j-th column of the IMU attitude matrix.

5. The fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 4, characterized in that, In step 3, the established rotationally modulated inertial navigation Kalman filter model is as follows: State variables: Note: δφ is latitude error, δλ is longitude error, δh is altitude error, and δV is latitude error. E For eastward velocity error, δV N For northbound velocity error, δV U For the upward velocity error, φ E Eastward deflection angle, φ N Northward deflection angle, φ U For the celestial deflection angle, D X For Xs-axis gyroscope drift, D Y For Ys-axis gyroscope drift, D Z For Zs-axis gyroscope drift, Add zero position to Xs axis Add zero position to the Ys axis, Add zero position and D to the Zs axis. N For the northward drift of the geography system, D U For the celestial drift of the geography system, α XZ The installation angle α is the tilt angle for a gyroscope that rotates around the positive Zs axis on the Xs axis. XY Install the deflection angle and α for the gyroscope that rotates around the positive direction of the Ys axis on the Xs axis. YX Install the deflection angle and α for the gyroscope that rotates around the positive direction of the Xs axis on the Ys axis. ZX The installation angle ε is the deflection angle for a gyroscope that rotates around the positive Xs axis on the Zs axis. XY The mounting angle δK is the angle at which the Xs axis rotates about the positive direction of the Ys axis. GZ For the Zs-axis gyroscope calibration coefficient error, δK AX Add scale coefficient error and δK to Xs AZ Add a scale coefficient error to the Zs axis; F is the state matrix; the matrix dimension is 25×25, where the element F(i,j) in the i-th row and j-th column of matrix F is defined as follows, and undefined elements are all 0; F(3,6)＝1； F(4,8)=-f U ;F(4,9)=f N ;F(4:6,13:15)=C; F(5,7)=f U ;F(5,9)=-f E ; F(6,1)=-2V E oh ie sinφ; F(6,7)=-f N ;F(6,8)=f E ; F(7:9,10:12) = C; F(8,1)=-ω ie sinφ; F(8,16)＝1； F(9,17)＝1； Note: The average angular velocity within the axial filtering period of IMU Ys and Zs; ω represents the average specific force within the filtering period along the X and Z axes of the IMU. ie f is the Earth's rotational angular rate. E For the eastward direction of the geography system; f N For the northward comparison of the geography system; f U It is a comparison of the celestial directions in the geography system.

6. The fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 5, characterized in that, In step 3, the specific process of calculating the state transition error of the real-time solution data output by the inertial navigation system includes: X K / K-1 =φ K / K-1 ·X K-1 P K-1 =P K / K-1 X K-1 =X K / K-1 φ K / K-1 For t K-1 to t K The one-step transition matrix at time step; Q K-1 Let V be the system noise variance matrix; X K / K-1 For t K-1 to t K Predicting the state at any given moment in one step; X K-1 For t K-1 The state variable at time point is initialized to 0; P K / K-1 For t K-1 to t K The one-step prediction mean square error matrix at time P; K-1 For t K-1 The mean square error matrix at time t; I is the identity matrix, with the same dimension as matrix F; T F The filter period; Filter mean square error matrix P K-1 Initialize as a 25×25 dimensional matrix: P K-1 =diag(6e-8,6e-8,0,0.25,0.25,0,3e-4,3e-4,6e-2,1e-12,1e-12,1e-12, The noise matrix Q of the filter system (2.5e-5, 2.5e-5, 2.5e-5, 2.35e-15, 2.35e-15, 0, ..., 0) is initialized as a 25×25 matrix. Q=diag(0,0,0,5e-5,5e-5,5e-5,5e-13,5e-13,5e-13,0,...,0,0,0).

7. The fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 1, characterized in that, In step 4, the specific process of determining whether the carrier has reached a static state includes: A check is performed every 10ms. If the following two conditions are met simultaneously for one consecutive second, the state of stillness is determined to have been reached; otherwise, the state of stillness is determined not to have been reached: |ΔV E |<0.01m / s,|ΔV N |<0.01m / s; Where: ΔV E ΔV N These are the eastward and northward speed increments within the current 0.5 seconds, respectively. These are the incremental sliding values ​​of the Xs-axis, Ys-axis, and Zs-axis angles within the current 0.5 seconds.

8. The fully autonomous, fast, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 6, wherein the specific process of performing filtering calculations using the established Kalman filter model in step 5 includes: Once the system is determined to be stationary, the filtering calculation begins using the following formula, with a filtering period ranging from 0.05 to 0.5 seconds: X K =X K / K-1 +K K ·(Z K -H K ·X K / K-1 ) P K =(I-K K ·H K )·P K / K-1 P K-1 =P K Where K K This is the filter gain matrix; X K For the filtered t K Time-state variables; Z K For t K Time measurement; P K For t K Time-based mean square error matrix estimation; H k This is a measurement matrix with dimensions 3×25, where all elements are 0 except for the following: H k (1,4)=1,H k (2,5)=1,H k (3,6)=1 R k Measure the noise matrix for the Kalman filter model: R k =diag(4e-2,4e-2,4e-2).

9. A fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 8, characterized in that, After the filtering time is greater than 5 seconds, the filtered t is obtained. K Time-state variable X K Longitude and latitude errors are corrected according to the following formula, and the corrected longitude λ is output. mdy latitude φ mdy ; f mdy =φ-X K (1) l mdy =λ-X K (2) where X K (1) X K (2) is t K The first and second elements of the state variables in the Kalman filter model at time step.

10. A fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 9, characterized in that, While correcting for horizontal position errors, calculate and correct for height errors using the following formula: h mdy =h-Δh ΔA U =ΔA U +ΔA Ui Where: h mdy The corrected height value is given by Δh, which is the height error estimate; ΔA U To add zero to the cumulative estimated celestial position, the initial value is 0; ΔA Ui Add a zero increment to the celestial axis for this correction, with an initial value of 0; δV Ui The current celestial velocity error is expressed in meters per second. ΔT is the time interval between two altitude corrections, in seconds; t i For the current time, t i-1 The last altitude correction time, in seconds; g represents the gravity at the current location, and R represents the Earth's radius.

11. The fully autonomous, rapid, meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 3, characterized in that, Calculate and correct the deflection angle of the mathematical platform using the following formula: k=k0+k1·ΔT+k2·ΔT 2 Where: φ E φ N These are the calculated eastward and northward platform deflection angles; k is a correction coefficient, and k0 = 1.0038, k1 = -6.7e-6, and k2 = -5.21e-7 are all constants; It is the average of the eastward and northward speeds over 5 seconds; ΔT=t i -t i-1 It is the time interval between two corrections, the time t of the previous target measurement point measurement correction. i-1 Correction time t to the current target measurement point i Unit: seconds; g is the current gravitational acceleration calculated using inertia, in meters per second. 2 .

Images