Full-autonomous rapid meter-scale three-dimensional positioning method based on rotation modulation inertial navigation

Through the combination of rotary modulation inertial navigation and Kalman filtering models, the problem of meter-level positioning of inertial navigation in unknown environments is solved, and fully autonomous, all-weather and high-precision three-dimensional fast positioning is achieved, suitable for special environments.

CN118960779AActive Publication Date: 2024-11-15XIAN FLIGHT SELF CONTROL INST OF AVIC

Patent Information

Application Number
CN202411003400.8
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-25
Publication Date
2024-11-15
Estimated Expiration
2044-07-25

AI Technical Summary

Technical Problem

In unknown environments, existing inertial navigation technology is difficult to achieve fully autonomous and fast meter-level three-dimensional positioning, especially under the influence of inertial device errors, the navigation accuracy is divergent and cannot meet the meter-level positioning needs.

Method used

Using a high-precision error model and Kalman filtering model based on rotation modulation inertial navigation, error correction and state transition calculation are performed through the real-time solution data output from the inertial navigation to achieve fully autonomous meter-level three-dimensional fast positioning.

Benefits of technology

It realizes fast single-point meter positioning in seconds during fully autonomous travel, with the advantages of high accuracy, full autonomy, all-weather, fast and convenient, and is suitable for special environments such as tunnels and mines.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118960779A_ABST
    Figure CN118960779A_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of high-precision inertial navigation, and particularly relates to a full-autonomous rapid meter-scale three-dimensional positioning method based on rotation modulation inertial navigation. The method is applied to a movable carrier carrying rotation modulation inertial navigation and comprises the following steps that the rotation modulation inertial navigation completes rotation modulation high-precision alignment and then is switched into navigation, and the inertial navigation outputs real-time resolving data; carrying out transfer calculation on east speed and north speed in real-time calculation data output by inertial navigation from an I MU measurement center to a measurement center of a frame rotation center; establishing a rotation modulation inertial navigation Kalman filtering model, and performing state transition calculation on an error of inertial navigation real-time calculation data; judging whether the carrier reaches a static state or not; if the static state is reached, skipping to the next step; if the static state is not reached, returning to the previous step; filtering calculation is carried out, after the filtering time is longer than 5 seconds, error amounts of longitude, latitude, east velocity and north velocity are obtained, and error correction is carried out on the longitude, latitude, east velocity and north velocity of rotation modulation inertial navigation; performing error correction on the rotation modulation inertial navigation height according to a height error correction formula; and outputting the corrected horizontal position and height of the current target measurement point, and completing positioning surveying and mapping of the current target measurement point.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of high-precision inertial navigation, and in particular relates to a fully autonomous rapid meter-level three-dimensional positioning method based on rotation modulation inertial navigation. Background Art

[0002] Achieving fully autonomous meter-level rapid three-dimensional positioning without relying on any external information is of great significance for the rapid mapping and detection of target measurement points such as station positions in unknown environments. Similarly, there is a strong demand for fully autonomous meter-level rapid three-dimensional positioning in special environments such as tunnels and mines.

[0003] Inertial navigation equipment is currently the only fully autonomous and all-weather navigation method, but due to the error of inertial devices, navigation accuracy diverges over time. Rotational modulation inertial navigation automatically offsets the error of inertial devices through a reasonable and orderly rotation modulation strategy, which can greatly improve the accuracy of autonomous navigation positioning. However, due to the random error of inertial devices, the error of inertial devices cannot be completely offset and still cannot meet the needs of meter-level positioning. Therefore, how to achieve fully autonomous and rapid meter-level positioning without any reference information is a technical problem in the navigation industry at home and abroad. Summary of the invention

[0004] The purpose of the present invention is: for the use scenarios of rapid surveying and detecting positions in unknown environments, the present invention establishes a high-precision error model of three-dimensional position of rotational modulation inertial navigation based on vehicle-mounted high-precision rotational modulation inertial navigation, and adopts a vehicle-mounted inertial navigation interval error correction scheme to achieve fully autonomous meter-level three-dimensional rapid positioning of intervals. It has the advantages of high precision, full autonomy, all-weather, fast and convenient. It has been verified by engineering implementation that it can achieve fully autonomous single-point meter-level rapid positioning in seconds during travel, and can be widely used in fully autonomous three-dimensional meter-level positioning and surveying in special environments such as tunnels and mines.

[0005] Technical solution of the present invention: In order to achieve the above-mentioned invention object, according to the first aspect of the present invention, a fully autonomous rapid meter-level three-dimensional positioning method based on rotation modulation inertial navigation is proposed, which is applied to a movable carrier equipped with rotation modulation inertial navigation, comprising the following steps:

[0006] Step 1: After the rotation modulation inertial navigation completes the rotation modulation high-precision alignment, it enters the navigation mode and outputs the real-time solution data; the real-time solution data of the inertial navigation includes latitude φ, longitude λ, altitude h, and eastward speed V E_S , northward speed V N_S , celestial velocity V U , pitch angle θ, roll angle γ, heading angle ψ;

[0007] Step 2: Calculate the measurement center transfer of the east velocity and north velocity 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 rotation geometric center point of the rotation modulation inertial navigation frame;

[0008] Step 2 is calculated throughout the navigation process to provide a stable eastward speed and northward speed for subsequent calculations.

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

[0010] Step 3 is calculated throughout the navigation process and is the basis for subsequent filtering calculations after the carrier is automatically judged to be stationary.

[0011] Step 4: Determine whether the carrier has reached a stationary state; if so, jump to step 5; if not, return to step 2;

[0012] Step 5: Use the established Kalman filter model to perform filtering calculation. After the filtering time is greater than 5 seconds, the longitude, latitude, eastward speed, and northward speed errors are obtained, and the longitude, latitude, eastward speed, and northward speed of the rotating modulated inertial navigation are corrected; the error of the rotating modulated inertial navigation altitude is corrected according to the altitude error correction formula;

[0013] Step 6: Output the horizontal position and height of the current target measurement point corrected in step 5 to complete the positioning surveying and mapping of the current target measurement point.

[0014] Preferably, during the alignment process, the rotation modulated inertial navigation IMU performs single-axis alternating positive and negative rotations around the Zs axis to modulate the constant error of the inertial navigation device, improve the alignment and navigation accuracy, and provide a basis for subsequent accurate error calibration. In the rotation modulated inertial navigation IMU Xs axis, Ys axis, and Zs axis coordinate system, the Xs axis represents the pitch axis, the Ys axis represents the roll axis, and the Zs axis represents the heading axis.

[0015] In one possible embodiment, after the positioning surveying and mapping of the current target measurement point is completed in step 6, the next target measurement point is moved to, and step 4 is returned to perform the positioning surveying and mapping of the next target measurement point. In one possible embodiment, in the step 5, the horizontal deflection of the rotation modulation inertial navigation mathematical solution platform is also corrected; the purpose is to improve the deflection accuracy of the rotation modulation inertial navigation mathematical solution platform, and provide a basis for high-precision measurement of the next target measurement point.

[0016] In a possible embodiment, in step 2, the calculation process of transferring the eastward velocity and the northward velocity in the real-time solution data output by the inertial navigation from the IMU measurement center to the measurement center of 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 is the east and north speed of the frame rotation center; E_S 、V N_S is the eastward speed and northward speed of the IMU measurement center; r x 、r y 、r z are the transfer calculation parameters from the IMU measurement center to the frame rotation center; ω y ,ω z are the angular rates of IMUYs and Zs axes respectively; C ij is the element in the i-th row and j-th column of the IMU attitude matrix.

[0020] In a possible embodiment, in step 3, the established rotation modulation inertial navigation Kalman filter model is specifically as follows:

[0021] State variables:

[0022]

[0023] Note: δφ is the latitude error, δλ is the longitude error, δh is the height error, δV E is the eastward velocity error, δVN is the north velocity error, δV U is the celestial velocity error, φ E is the east declination angle, φ N is the north declination angle, φ U is the celestial declination angle, D X is the Xs axis gyro drift, D Y is the Ys axis gyro drift, D Z is the Zs axis gyro drift, Add zero position to Xs axis. Add zero position to Ys axis. Add zero position of Zs axis, D N The northward drift of the geographical system, D U is the celestial drift of the geographic system, α XZ is the gyro installation deflection angle of the Xs axis rotating around the positive direction of the Zs axis, α XY The gyro installation deflection angle, α, of the Xs axis rotating around the positive direction of the Ys axis YX The gyro installation deflection angle, α, of the Ys axis rotating around the positive direction of the Xs axis ZX is the gyro installation deflection angle of the Zs axis rotating around the positive direction of the Xs axis, ε XY It is the installation deflection angle of the Xs axis rotating around the Ys axis in the positive direction, δK GZ is the Zs axis gyro scale coefficient error, δK AX Xs plus the scale factor error, δK AZ The error of the scale factor of the Zs axis is added;

[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 the F matrix is ​​defined as follows, and all undefined elements are 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)=f U; 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: is the average angular rate of the IMU Ys and Zs axes during the filtering period; is the average specific force of IMUXs and Zs in the axial filtering period; ω ie is the Earth's rotation angular velocity; f E It is the eastward force of the geographical system; f N It is the north direction relative force of the geographical system; U It is a comparison of the geographical direction.

[0044] In a possible embodiment, in step 3, the specific process of performing state transfer calculation on the error of the real-time solution data output by the inertial navigation system 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 t K-1 to K One-step transfer matrix at time;

[0052] Q K-1 is the system noise variance matrix;

[0053] X K / K-1 t K-1 to K One-step prediction of the state at time X K-1 t K-1 The state variables at the moment are initialized to 0;

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

[0055] I is the unit matrix, with the same dimension as F matrix; T F is the filtering period;

[0056] The filter mean square error matrix P K-1 Initialize to 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 dimensional 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 a possible embodiment, in step 4, the specific process of determining whether the carrier reaches a stationary state includes:

[0062] The judgment is made every 10ms. If the following two conditions are met simultaneously for 1 second, it is determined that the stationary state has been reached. Otherwise, it is determined that the stationary state has not been reached:

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

[0064]

[0065] Where: ΔV E , ΔV N They are respectively the increments of eastward speed and northward speed within the current 0.5 second;

[0066] They are respectively the accumulated values ​​of the Xs, Ys and Zs axial angle increment sliding within the current 0.5 seconds.

[0067] In a possible embodiment, in step 5, the specific process of using the established Kalman filter model to perform filtering calculation includes:

[0068] When it is judged to be stationary, the filtering calculation starts according to the following formula, and the filtering cycle range is 0.05-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 is the filter gain matrix;

[0074] X K After filtering, t K Moment state variables;

[0075] Z K t K Time measurement;

[0076] P K tK The moment estimation mean square error matrix;

[0077] H k is the measurement matrix, with dimension 3×25, and all elements except the following are 0:

[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 a possible embodiment, in step 5, the specific process of performing error correction on longitude and latitude includes:

[0082] When the filtering time is greater than 5 seconds, the filtered t K The state variable X at the moment K , the 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 Kalman filter model state variables at time instant.

[0086] In a possible embodiment, in step 5, while correcting 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 is the corrected altitude value, Δh is the estimated altitude error; ΔA U is the cumulative estimated celestial direction plus zero position, with an initial value of 0; ΔA Ui It is the celestial zero increment of this correction, and the initial value is 0;

[0093] δV Ui is the celestial velocity error at the current moment, unit: meter / second;

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

[0095] g is the gravity value at the current position, and R is the radius of the earth.

[0096] In a possible embodiment, after completing the above horizontal position error correction, 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 is the calculated platform deflection angle to the east and north;

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

[0102] It is the average value of eastward velocity and northward velocity within 5s;

[0103] ΔT=t i -t i-1 is the interval between two corrections, the last target measurement point measurement correction time t i-1 Correction time t to the current target measurement point i , unit: second;

[0104] g is the current gravitational acceleration calculated by inertia, unit: meter / second 2 .

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

[0106] Beneficial technical effects of the present invention:

[0107] The present invention aims at the use scenario of rapid surveying and mapping of stations in unknown environments. It is based on the vehicle-mounted high-precision rotational modulation inertial navigation and automatically judges the operating status of the carrier to perform interval error correction according to the rotational modulation inertial navigation navigation error model, thereby realizing fully autonomous meter-level three-dimensional rapid positioning of intervals. It has the advantages of high precision, full autonomy, all-weather, fast and convenient. It has been verified by engineering implementation that it can realize fully autonomous single-point meter-level rapid positioning in seconds while moving, and can effectively solve the problem of fully autonomous single-point three-dimensional meter-level positioning in battlefield satellite denial environments. The method of the present invention can also be promoted and applied to fully autonomous three-dimensional meter-level positioning and mapping in special environments such as tunnels and mines, and has high application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0108] In order to more clearly explain the technical solution implemented by the present invention, the following will be a simple explanation of the drawings needed in the description of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative work.

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

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

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

[0112] Figure 4 This is the height error curve of the navigation full-range rotation modulation inertial navigation output in Example 1. DETAILED DESCRIPTION

[0113] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0114] The features and illustrative embodiments of various aspects of the present invention will be described in detail below. In the detailed description below, many specific details are proposed to provide a comprehensive understanding of the present invention. However, it is obvious to those skilled in the art that the present invention can be implemented without the need for some of these specific details. The following description of the embodiments is only to provide a better understanding of the present invention by illustrating examples of the present invention. The present invention is by no means limited to any specific settings and methods proposed below, but covers any improvements, replacements and modifications of structures, methods, devices without departing from the spirit of the present invention.

[0115] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other, and the embodiments can refer to and quote each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0116] Example 1

[0117] like Figure 1 As shown, a fully autonomous rapid meter-level three-dimensional positioning method based on rotational modulation inertial navigation is applied to a movable carrier equipped with a rotational modulation inertial navigation, comprising the following steps:

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

[0119] Step 2: After 10 minutes of alignment, the rotation modulated inertial navigation automatically switches to navigation according to the carrier's motion state. During the navigation process, the rotation modulated inertial navigation IMU performs single-axis positive and negative rotation around the Zs axis to modulate the constant error of the inertial navigation device, improve the navigation accuracy during the dynamic process, and provide a basis for the subsequent accurate error calibration after the carrier is stationary; during the navigation process, the inertial navigation system calculates and outputs the latitude φ, longitude λ, altitude h, and eastward speed V in real time. E_S , northward speed V N_S , celestial velocity V U , pitch angle θ, roll angle γ, heading angle ψ;

[0120] Step 3: If Figure 2 As shown, the horizontal velocity measurement center transfer calculation is:

[0121] During the entire navigation process, the horizontal velocity is calculated from the IMU measurement center to the frame rotation center:

[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 is the east and north speed of the frame rotation center; E_S 、V N_S is the eastward speed and northward speed of the IMU measurement center; r x 、r y 、r z are the transfer calculation parameters from the IMU measurement center to the frame rotation center; ω y ,ω z are the angular rates of IMUYs and Zs axes respectively; C ij is the element in the i-th row and j-th column of the IMU attitude matrix.

[0125] After the center transfer calculation of horizontal velocity measurement, a stable horizontal velocity is obtained, which provides stable measurement information for subsequent filter calculation and error calibration;

[0126] Step 4: During the entire navigation process, the state transfer calculation of the rotation modulation inertial navigation error is performed according to the rotation modulation inertial navigation Kalman filter model (as shown in the following formula); this step is the basis for the subsequent filtering calculation after the carrier is automatically judged to be stationary;

[0127] Kalman filter model state transfer 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] t K-1 to K One-step transfer matrix at time;

[0135] Q K-1 is the system noise variance matrix;

[0136] X K / K-1 t K-1 to K One-step prediction of the state at time X K-1 t K-1 The state variable at the moment is initialized to 0; P K / K-1 t K-1 to K The one-step prediction mean square error matrix at time; P K-1 t K-1 The mean square error matrix at time. I is the unit matrix, with the same dimension as F matrix; T F is the filtering period, set to 0.1 seconds;

[0137] The filter mean square error matrix P K-1 Initialize to 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 matrix 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 of the rotation modulation inertial navigation error is as follows:

[0142] Filter state variables:

[0143]

[0144] Note: δφ is the latitude error, δλ is the longitude error, δh is the height error, δV E is the eastward velocity error, δVN is the north velocity error, δV U is the celestial velocity error, φ E is the east declination angle, φ N is the north declination angle, φ U is the celestial declination angle, D X is the Xs axis gyro drift, D Y is the Ys axis gyro drift, D Z is the Zs axis gyro drift, Add zero position to Xs axis. Add zero position to Ys axis. Add zero position of Zs axis, D N The northward drift of the geographical system, D U is the celestial drift of the geographic system, α XZ is the gyro installation deflection angle of the Xs axis rotating around the positive direction of the Zs axis, α XY The gyro installation deflection angle, α, of the Xs axis rotating around the positive direction of the Ys axis YX The gyro installation deflection angle, α, of the Ys axis rotating around the positive direction of the Xs axis ZX is the gyro installation deflection angle of the Zs axis rotating around the positive direction of the Xs axis, ε XY It is the installation deflection angle of the Xs axis rotating around the Ys axis in the positive direction, δK GZ is the Zs axis gyro scale coefficient error, δK AX Xs plus the scale factor error, δK AZ is the error of the scale factor added 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 the F matrix is ​​defined as follows, and all undefined elements are 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)=f U; 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: is the average angular rate of the IMU Ys and Zs axes during the filtering period; is the average specific force of IMU Xs and Zs axial filtering period. ie is the Earth's rotation angular velocity. E It is the eastward force of the geographical system; f N It is the north direction relative force of the geographical system; U It is a comparison of the geographical direction.

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

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

[0167] The judgment is made every 10ms. If the following two conditions are met simultaneously for 1 second, it is determined that the stationary state has been reached. Otherwise, it is determined that the stationary state has not been reached:

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

[0169]

[0170] Where: ΔV E , ΔV N They are respectively the increments of eastward speed and northward speed within the current 0.5 second;

[0171] They are respectively the accumulated values ​​of the Xs, Ys and Zs axial angle increment sliding 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, and after the filtering time is greater than 5 seconds, the accurate longitude, latitude, eastward speed, and northward speed error are obtained;

[0173] When it is judged to be stationary, the filtering calculation starts according to the following formula. The filtering cycle is 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 is the filter gain matrix;

[0178] X K After filtering, t K Moment state variables;

[0179] Z K t K Time measurement; P K t K The moment estimation mean square error matrix;

[0180] H k is the measurement matrix, with dimension 3×25, and all elements except the following are 0:

[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 according to the filtering result of the filter in step 6;

[0185] After the continuous estimation time is greater than 5 seconds, the filtered t K The state variable X at the moment K , the 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 Kalman filter model state variables at the moment;

[0189] Step 8: Mathematical Platform Horizontal Deflection Correction

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

[0191] When it is determined that the carrier is in a stationary state for more than 5 seconds, after completing the horizontal position error correction as above, the mathematical platform deflection angle is calculated and corrected according to the following formula:

[0192]

[0193]

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

[0195] Where: φ E ,φ N is the calculated platform deflection angle to the east and north;

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

[0197] It is the average value of eastward velocity and northward velocity within 5s;

[0198] ΔT=t i -t i-1 is the interval between two corrections (the time of correction of the last target measurement point measurement t i-1 Correction time t to the current target measurement point i ), unit: second;

[0199] g is the current gravitational acceleration calculated by inertia, unit: meter / second 2 ;

[0200] Step 9: Altitude error correction When it is determined that the carrier is in a stationary state for more than 5 seconds, the altitude error is calculated and corrected according to the following formula after the horizontal position error correction is completed:

[0201] h mdy =h-Δh

[0202]

[0203]

[0204]

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

[0206] Where: h mdy is the corrected altitude value, Δh is the estimated altitude error; ΔA U is the cumulative estimated celestial direction plus zero position, with an initial value of 0; ΔA Ui The initial value of the zero-position increment in the celestial direction for this correction is 0.

[0207] δV Ui is the celestial velocity error at the current moment, unit: meter / second;

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

[0209] g is the gravity value at the current position, and R is the radius of the earth.

[0210] Step 10: Output the horizontal position and height corrected by steps 7 and 9 to complete the positioning survey of the current target measurement point. The horizontal position refers to longitude and latitude.

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

[0212] Example 2

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

[0214]

[0215]

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

[0217] Where: φ E ,φ N is the calculated platform deflection angle to the east and north;

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

[0219] It is the average value of eastward velocity and northward velocity within 5s;

[0220] ΔT=t i -t i-1 is the interval between two corrections, the last target measurement point measurement correction time t i-1 Correction time t to the current target measurement point i , unit: second;

[0221] g is the current gravitational acceleration calculated by inertia, unit: meter / second 2 .

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

[0223] Table 1 Measurement results of Example 2

[0224]

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

[0226] like Figure 3 , Figure 4 As shown in the figure, it is the rotation modulation inertial navigation error curve of the whole measurement process. It can be seen from the figure that at the measurement point position, the horizontal position and the height accuracy are accurately corrected to achieve meter-level measurement and positioning accuracy.

[0227] Comparative Example 1:

[0228] Comparative Example 1 does not adopt Step 3, Step 8, and Step 9 in Embodiment 1 and Embodiment 2 of the present invention, and only outputs through the Kalman filter model; 10 target measurement points are measured, and the measurement results are shown in Table 2;

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

[0230] Table 2 Comparative Example 1 Measurement Results

[0231]

[0232] Although the embodiments disclosed in the present invention are as above, the contents described are only embodiments adopted to facilitate understanding of the present invention and are not intended to limit the present invention. Any technician in the field to which the present invention belongs can make any modifications and changes in the form and details of implementation without departing from the spirit and scope disclosed in the present invention, but the patent protection scope of the present invention shall still be subject to the scope defined in the attached claims.

Claims

1. A fully autonomous rapid meter-level three-dimensional positioning method based on rotational modulation inertial navigation, characterized in that: The method is applied to a movable carrier equipped with a rotary modulated inertial navigation system, and comprises the following steps: Step 1: After the rotation modulation inertial navigation completes the rotation modulation high-precision alignment, it enters the navigation mode and outputs the real-time solution data; the real-time solution data of the inertial navigation includes latitude φ, longitude λ, altitude h, and eastward speed V E_S , northward speed V N_S , celestial velocity V U , pitch angle θ, roll angle γ, heading angle ψ; Step 2: Calculate the measurement center transfer of the east velocity and north velocity 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 rotation geometric center point of the rotation modulation inertial navigation frame; Step 3: Establish a rotation modulation inertial navigation Kalman filter model and perform state transfer calculation on the error of the real-time solution data output by the inertial navigation; Step 4: Determine whether the carrier has reached a stationary state; if so, jump to step 5; if not, return to step 3; Step 5: Use the established Kalman filter model to perform filtering calculation. After the filtering time is greater than 5 seconds, the longitude, latitude, eastward speed, and northward speed errors are obtained, and the errors of the longitude, latitude, eastward speed, and northward speed of the rotating modulated inertial navigation are corrected; the error of the rotating modulated inertial navigation altitude is corrected according to the altitude error correction formula; Step 6: Output the horizontal position and height of the current target measurement point corrected in step 5, and complete the positioning surveying 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 is characterized in that: After the positioning surveying and mapping of the current target measurement point is completed in step 6, proceed to the next target measurement point and return to step 4 to perform the positioning surveying 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: In the step 5, it 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 is characterized in that: In step 2, the calculation process of transferring the eastward velocity and the northward velocity in the real-time solution data output by the inertial navigation 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 is the east and north speed of the frame rotation center; E_S 、V N_S is the eastward speed and northward speed of the IMU measurement center; r x 、r y 、r z are the transfer calculation parameters from the IMU measurement center to the frame rotation center; ω y ,ω z are the angular rates of the IMU Ys and Zs axes respectively; C ij is 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 is characterized in that: In step 3, the established rotation modulation inertial navigation Kalman filter model is specifically as follows: State variables: Note: δφ is the latitude error, δλ is the longitude error, δh is the height error, δV E is the eastward velocity error, δV N is the north velocity error, δV U is the celestial velocity error, φ E is the east declination angle, φ N is the north declination angle, φ U is the celestial declination angle, D X is the Xs axis gyro drift, D Y is the Ys axis gyro drift, D Z is the Zs axis gyro drift, Add zero position to Xs axis. Add zero position to Ys axis. Add zero position of Zs axis, D N The northward drift of the geographical system, D U is the celestial drift of the geographic system, α XZ is the gyro installation deflection angle of the Xs axis rotating around the positive direction of the Zs axis, α XY The gyro installation deflection angle, α, of the Xs axis rotating around the positive direction of the Ys axis YX The gyro installation deflection angle, α, of the Ys axis rotating around the positive direction of the Xs axis ZX is the gyro installation deflection angle of the Zs axis rotating around the positive direction of the Xs axis, ε XY It is the installation deflection angle of the Xs axis rotating around the Ys axis in the positive direction, δK GZ is the Zs axis gyro scale coefficient error, δK AX Xs plus the scale factor error, δK AZ The error of the scale factor of the Zs axis is added; 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 the F matrix is ​​defined as follows, and all undefined elements are 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: is the average angular rate of the IMU Ys and Zs axes during the filtering period; is the average specific force of the IMU X and Z axes during the filtering period; ω ie is the Earth's rotation angular velocity; f E It is the eastward force of the geographical system; f N It is the north direction relative force of the geographical system; f U It is a comparison of the geographical direction.

6. The fully autonomous rapid meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 5 is characterized in that: In step 3, the specific process of performing state transfer calculation on the error of the real-time solution data output by the inertial navigation 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 t K-1 to K One-step transfer matrix at time; Q K-1 is the system noise variance matrix; X K / K-1 t K-1 to K One-step prediction of the state at time X K-1 t K-1 The state variable at the moment is initialized to 0; P K / K-1 t K-1 to K The one-step prediction mean square error matrix at time; P K-1 t K-1 The mean square error matrix at time; I is the unit matrix, with the same dimension as F matrix; T F is the filtering period; The filter mean square error matrix P K-1 Initialize to 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 filter system noise matrix Q is initialized to a 25×25 dimensional 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 is characterized in that: In step 4, the specific process of determining whether the carrier has reached a stationary state includes: The judgment is made every 10ms. If the following two conditions are met simultaneously for 1 second, it is determined that the stationary state has been reached. Otherwise, it is determined that the stationary state has not been reached: |ΔV E |<0.01m / s,|ΔV N |<0.01m / s; Where: ΔV E , ΔV N They are respectively the increments of eastward speed and northward speed within the current 0.5 second; They are respectively the accumulated values ​​of the Xs, Ys and Zs axial angle increment sliding within the current 0.5 seconds.

8. According to the fully autonomous rapid meter-level three-dimensional positioning method based on rotation modulation inertial navigation of claim 6, in step 5, the specific process of using the established Kalman filter model to perform filtering calculation includes: When it is judged to be stationary, the filtering calculation starts according to the following formula, and the filtering period ranges 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 is the filter gain matrix; X K After filtering, t K Moment state variables; Z K t K Time measurement; P K t K The moment estimation mean square error matrix; H k is the measurement matrix, with dimension 3×25, and all elements except the following are 0: 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. The fully autonomous rapid meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 8 is characterized in that: When the filtering time is greater than 5 seconds, the filtered t K The state variable X at the moment K , the 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 Kalman filter model state variables at time instant.

10. The fully autonomous rapid meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 9, characterized in that: While correcting the horizontal position error, calculate the height error and make corrections according to the following formula: h mdy =h-Δh ΔA U =ΔA U +ΔA Ui Where: h mdy is the corrected altitude value, Δh is the estimated altitude error; ΔA U is the cumulative estimated celestial direction plus zero position, with an initial value of 0; ΔA Ui It is the celestial zero increment of this correction, and the initial value is 0; δV Ui is the celestial velocity error at the current moment, unit: meter / second; ΔT is the time interval between two altitude corrections, in seconds; t i is the current moment, t i-1 The time of the last altitude correction, unit: second; g is the gravity value at the current position, and R is the radius of the earth.

11. The fully autonomous rapid meter-level three-dimensional positioning method based on rotational modulation inertial navigation according to claim 3 is characterized in that: Calculate the mathematical platform deflection angle and make corrections according to the following formula: k=k0+k1·ΔT+k2·ΔT 2 Where: φ E ,φ N is the calculated platform deflection angle to the east and north; k is the correction coefficient, k0=1.0038, k1=-6.7e-6, k2=-5.21e-7 are all constants; It is the average value of eastward velocity and northward velocity within 5s; ΔT=t i -t i-1 is the interval between two corrections, the last target measurement point measurement correction time t i-1 Correction time t to the current target measurement point i , unit: second; g is the current gravitational acceleration calculated by inertia, unit: meter / second 2 .

12. A computer-readable storage medium, characterized in that: When the computer program is executed by a processor, the steps of the method described in any one of claims 1 to 11 are implemented.

Citation Information

Patent Citations

  • Completely independent relative inertial navigation method

    CN102628691A

  • Single-axis revolution modulation inertia-astronomy deep combined navigation method

    CN108871326A

  • High-precision surveying and mapping method based on inertial system

    CN109974697A

  • Sea-air rotation modulation inertial navigation / astronomical integrated navigation method

    CN113108782A

  • Navigation method and navigation system based on three-axis rotary inertial navigation system

    CN115574817A

Cited By

  • Ship track information display method, device, equipment and medium

    CN119714267A