A heterogeneous multi-source navigation information inversion method for incomplete measurement conditions

Abstract

This invention discloses a method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions, belonging to the field of positioning and navigation technology. It employs cubic spline interpolation to augment sparse waypoint data, making the waypoint data of the carrier smoother. The waypoint data is processed and analyzed to calculate the carrier's original physical variables such as velocity, acceleration, and angular velocity. The noise-free output values ​​of the inverted gyroscopes and accelerometers are used to extrapolate the carrier's waypoint data, which is then compared and evaluated with the actual waypoint data. If the accuracy requirements are not met, feedback iterative correction is performed; otherwise, multi-navigation sensor simulation data inversion is performed. This invention can invert the output of multiple navigation sensors to a certain extent based on limited waypoint data, making it suitable for practical applications.

Description

Technical Field

[0001] This invention relates to a method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions, belonging to the field of positioning and navigation technology. Background Technology

[0002] In complex environments such as satellite denial and interference, modern navigation systems widely employ heterogeneous multi-source sensors as navigation signal sources for intelligent information fusion to output navigation information. Navigation systems using heterogeneous multi-source sensors have attracted widespread attention from scholars both domestically and internationally due to their strong adaptability to complex environments and accurate output of carrier navigation status information. However, in some costly, difficult-to-repeat, or one-off multi-source navigation experiments, failure of some components can lead to the loss of partial navigation system data, affecting the overall experimental results and subsequent analysis. Traditional navigation information inversion methods are generally open-loop inversions, lacking evaluation and iterative correction of the accuracy of the output inversion information, resulting in low accuracy and reliability of the output navigation information. Furthermore, the types and number of sensors used for inversion are relatively limited.

[0003] The method of this invention compares and evaluates waypoint data derived from the noiseless output values ​​of gyroscopes and accelerometers with actual waypoint data. If the accuracy is not satisfactory, feedback iterative correction is performed until the set inversion accuracy is met. At the same time, this invention inverts the output of various navigation sensor information based on limited waypoint data, which has good application prospects and engineering value. Summary of the Invention

[0004] Purpose of the invention: To propose a method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions, which overcomes the problem that some heterogeneous multi-source navigation data cannot be obtained under incomplete measurement conditions. It can invert the experimental output data of multiple navigation sensors to a certain extent, so that it can be well applied in actual testing.

[0005] Technical solution: A method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions, comprising the following steps:

[0006] A method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions includes the following steps:

[0007] Step (1): Obtain waypoint data during the carrier's movement;

[0008] Step (2): Select the corresponding scheme for data preprocessing based on the waypoint data type;

[0009] Step (3): Use interpolation to augment the preprocessed waypoint data;

[0010] Step (4): Calculate the acceleration and angular velocity of the carrier.

[0011] Step (5): Calculate the noise-free output of the inverted gyroscope and accelerometer;

[0012] Step (6): Calculate the coordinates of the inverted waypoints of the carrier;

[0013] Step (7): Evaluate whether the inversion data meets the accuracy requirements;

[0014] Step (8) Invert the simulation output data of multiple navigation sensors.

[0015] Furthermore, in step (1), the carrier waypoint data type is latitude, longitude, and altitude data or local coordinate system coordinate data, acquiring the data WP1, WP2, WP3, ..., WP1, WP2, WP3, ..., WP4, which are the waypoints the carrier passes through during its movement. n If the data is in the local coordinate system, the latitude, longitude, and altitude coordinates O of the local coordinate system origin should also be obtained. The expression for O is:

[0016] O=(L0,λ0,h0)

[0017] WP1 = (t1, x1, y1, z1, ψ1) T

[0018] WP2 = (t2,x2,y2,z2,ψ2) T

[0019]

[0020] WP n =(t n ,x n ,y n ,z n ,ψ n ) T

[0021] Where (L0,λ0,h0) are the latitude, longitude, and altitude coordinates of the local coordinate system origin, respectively, and t1, t2, ..., t... n These represent the times when the carrier passes through waypoints 1 to n, where (x1, y1, z1), (x2, y2, z2), ..., (x n ,y n ,z n ψ1, ψ2, ..., ψn are the three-dimensional coordinates of the carrier when it passes through waypoints 1 to n. n These are the yaw angles of the carrier when it passes through waypoints 1 to n.

[0022] Furthermore, step (2) includes the following specific steps:

[0023] Step (2-1): Determine the operation to be performed based on the waypoint data type in step (1). If the waypoint data is local coordinate system coordinate data, then execute step (2-2); otherwise, execute step (2-3).

[0024] Step (2-2): Based on the data obtained in step (1), convert the local coordinates of all waypoints into latitude, longitude, and altitude coordinates.

[0025] Step (2-3): Based on the data obtained in step (1), convert all waypoint coordinates into latitude, longitude, and altitude coordinates;

[0026] Step (2-4) stores all waypoint data in the path matrix.

[0027] Furthermore, step (3) includes the following specific steps:

[0028] Step (3-1) involves using the discrete data points obtained in step (2-4). The relationships between time and longitude, time and latitude, time and altitude, and time and yaw angle were found using cubic spline interpolation algorithms.

[0029] Step (3-2): Set a suitable and sufficiently small initial sampling period T, in seconds;

[0030] Step (3-3): Based on the functional relationships (f1,f2,f3,f4) between time and longitude, time and latitude, time and altitude, and time and yaw angle variables obtained in step (3-1) and the sampling period T obtained in step (3-2), interpolation is performed to expand the data between two adjacent waypoints at the current time.

[0031] Furthermore, step (4) includes the following specific steps:

[0032] Step (4-1): Based on the longitude λ and latitude L data between adjacent waypoints i and i+1 obtained in step (3-3) after sampling period T, calculate the speed of the two-dimensional planar carrier in the direction of the vehicle head.

[0033] Step (4-2) involves determining the velocity of the two-dimensional planar carrier in the direction of its head, based on the velocity obtained in step (4-1). The expression for the acceleration of the vehicle's front in the two-dimensional planar direction is as follows:

[0034]

[0035] Step (4-3): Based on the carrier height data obtained in step (3-3), calculate the carrier's celestial velocity, which is expressed as:

[0036]

[0037] Step (4-4): Based on the celestial velocity of the carrier obtained in step (4-3) The expression for calculating the axial acceleration of a two-dimensional planar carrier is as follows:

[0038]

[0039] Step (4-5): Based on the yaw angle data obtained in step (3-3), calculate the yaw angle rate of the vehicle, which is expressed as:

[0040]

[0041] Furthermore, step (5) includes the following specific steps:

[0042] Step (5-1): Based on the yaw angle variable ψ obtained from the interpolation between waypoints in step (3-3), calculate the attitude transformation matrix of the vehicle system relative to the geographic system. Its expression is:

[0043]

[0044] Where (γ,θ,ψ) are the roll, pitch, and yaw angle variables of the carrier, respectively, γ=0,θ=0;

[0045] Step (5-2): Based on the attitude transformation matrix obtained in step (5-1) The vehicle's frontal velocity ve obtained in step (4-1) and the vehicle's upward velocity vu obtained in step (4-3) are used to calculate the vehicle's geographic velocity. Its expression is:

[0046]

[0047]

[0048]

[0049] in, Let be the component of the velocity of the geographic system relative to the Earth system in the northeast-sky direction within the geographic system. It is a column vector consisting of the components of the velocity of the geographic system relative to the Earth system along the axis of the carrying system;

[0050] Step (5-3) involves determining the geographic velocity of the carrier based on the information obtained in step (5-2). The attitude transformation matrix obtained in step (5-1) The yaw rate ωb obtained in step (4-5), the carrier yaw angle ψ obtained in step (3-3), and the noiseless output of the gyroscope are calculated. Its expression is:

[0051]

[0052] in, Let be the column vector consisting of the components of the Earth system's angular velocity relative to the inertial frame along the axis of the carrying system. It is a column vector consisting of the components of the angular velocity of the geographic system relative to the Earth system along the axis of the carrying system. Let be the column vector formed by the components of the angular velocity of the carrying system relative to the geographic frame along the axis of the carrying system. Let be the column vector consisting of the components of the Earth system's angular velocity relative to the inertial frame along the geographic axis. is the column vector consisting of the components of the angular velocity of the geographic system relative to the Earth system along the axis of the geographic system; (γ, θ, ψ) are the roll, pitch, and yaw angle variables of the vehicle, respectively, γ = 0, θ = 0. These are the yaw angle, pitch angle, roll angle, and angular rate of the carrier, respectively. ie The Earth's rotational angular rate is taken as 7.292115147 × 10⁻⁶. -5 rad / s;

[0053] Step (5-4): Based on the carrier's forward velocity ve obtained in step (4-1), the carrier's forward acceleration ae obtained in step (4-2), the carrier's upward velocity vu obtained in step (4-3), the carrier's upward velocity au obtained in step (4-4), and the angular velocity obtained in step (5-3). The geographic velocity of the carrier obtained in step (5-2) The attitude transformation matrix obtained in step (5-1) Calculate the specific force f measured by the accelerometer b Its expression is:

[0054]

[0055] in,

[0056]

[0057] g is the acceleration due to gravity, f n The column vector is composed of the components of the force in the navigation system. For speed The differential.

[0058] Furthermore, step (6) includes the following specific steps:

[0059] Step (6-1) is based on the geographic velocity of the carrier obtained in step (5-2). The latitude, longitude, and altitude coordinates of the carrier are calculated using the following expression:

[0060]

[0061]

[0062]

[0063] in, and These are the coordinates of two adjacent discrete waypoints, i and i+1, after being discretized over a sampling period T, in degrees, degrees, and meters. The superscript indicates that this data is the discrete waypoint data interpolated between the i-th waypoint and the (i+1)-th waypoint, and the subscript indicates that this data is the j-th discrete waypoint data interpolated between the i-th waypoint and the (i+1)-th waypoint.

[0064] Step (6-2): Based on the waypoint coordinate calculation formula obtained in step (6-1), the coordinates of the (i+1)th waypoint are continuously calculated from the calculated coordinates of the i-th waypoint. The expression is as follows:

[0065] h( i+1 ) t =h it +Δh i

[0066] L( i+1 ) t =L it +ΔL i

[0067] λ (i+1)t =λ it +Δλ i

[0068] Among them, (L) (i+1)t , λ (i+1)t h (i+1)t ) and (L it , λ it h it ) are the calculated coordinates of the (i+1)th and ith waypoints, respectively, (ΔL) i ,Δλ i ,Δh i The coordinate increments in latitude, longitude, and altitude of the (i+1)th waypoint are continuously calculated from the coordinate data of the i-th waypoint.

[0069] Furthermore, step (7) includes the following specific steps:

[0070] Step (7-1): Based on the coordinates of the (i+1)th calculated waypoint obtained in step (6-2) and the coordinates of the (i+1)th actual waypoint obtained in step (2-4), compare them to evaluate whether the inversion data meets the accuracy requirements. If the inversion data meets the accuracy requirements, proceed directly to step (8); otherwise, proceed to step (7-2). The accuracy judgment expression for the inversion data is:

[0071]

[0072] Where R is the set accuracy assessment value of the inversion data, in meters, and Re is the Earth's radius, which is 6,378,137.0 meters.

[0073] Step (7-2): Reset the sampling period T, return to step (3-3), and recalculate the interpolation. The expression is:

[0074]

[0075] Among them, T old This is the value from the previous sampling period.

[0076] Beneficial Effects: This invention discloses a method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions, belonging to the field of positioning and navigation technology. Cubic spline interpolation is used to augment sparse waypoint data, making the waypoint data of the carrier smoother. The waypoint data is processed and analyzed to calculate the carrier's original physical variables such as velocity, acceleration, and angular velocity. The noise-free output values ​​of the inverted gyroscope and accelerometer are used to extrapolate the carrier's waypoint data, which is then compared and evaluated with the actual waypoint data. If the accuracy requirements are not met, feedback iterative correction is performed; otherwise, multi-navigation sensor simulation data inversion is performed. This invention can invert the output of multiple navigation sensors to a certain extent based on limited waypoint data, making it suitable for practical applications. Attached Figure Description

[0077] Figure 1 This is a schematic diagram illustrating the principle and flow of the method of the present invention;

[0078] Figure 2 This is a comparison chart showing the positioning error values ​​of the carrier waypoint coordinates retrieved using the method of this invention and the traditional method, respectively.

[0079] Figure 3 A comparison chart showing the trajectory calculated by multi-sensor data fusion using the method of this invention, the actual motion trajectory of the carrier, and the trajectory calculated by multi-sensor data fusion using traditional methods;

[0080] Figure 4This is a top view comparing the trajectory calculated by multi-sensor data fusion using the method of this invention, the actual motion trajectory of the carrier, and the trajectory calculated by multi-sensor data fusion using traditional methods.

[0081] Figure 5 A comparison chart showing the longitude, latitude, and altitude values ​​of the multi-sensor data fusion-based trajectory derived using the method of this invention, the actual motion trajectory of the carrier, and the multi-sensor data fusion-based trajectory derived using traditional methods.

[0082] Figures 6-17 The data are various navigation sensor data retrieved using the method of this invention;

[0083] Figure 18 , 19 Figures 20 and 21 are comparison curves of the carrier position, velocity, and attitude calculated by fusing multi-sensor data obtained by the method of this invention and the traditional method, and the actual position, velocity, and attitude error curves of the carrier. Detailed Implementation

[0084] The invention will now be further explained with reference to the accompanying drawings.

[0085] The method of this invention expands sparse waypoint data using cubic sample interpolation, and then analyzes and processes the interpolated data to calculate the output data of multiple navigation sensors. This solves the problem of experimental data not being available due to the failure of some sensors in multi-source navigation experiments, and to a certain extent, reverses the experimental output data of multiple navigation sensors.

[0086] like Figure 1 As shown, a method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions includes the following steps:

[0087] A method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions includes the following steps:

[0088] Step (1): Obtain waypoint data during the carrier's movement;

[0089] In step (1), the carrier waypoint data type is latitude, longitude, and altitude data or local coordinate system coordinate data, and the data WP1, WP2, WP3, ..., WP2, WP3, ..., WP4, which are the waypoints the carrier passes through during its movement, are obtained. n If the data is in the local coordinate system, the latitude, longitude, and altitude coordinates O of the local coordinate system origin should also be obtained. The expression for O is:

[0090] O=(L0,λ0,h0)

[0091] WP1 = (t1, x1, y1, z1, ψ1) T

[0092] WP2 = (t2,x2,y2,z2,ψ2) T

[0093]

[0094] WP n =(t n ,x n ,y n ,z n ,ψ n ) T

[0095] Where (L0,λ0,h0) are the latitude, longitude, and altitude coordinates of the local coordinate system origin, respectively, and t1, t2, ..., t... n These represent the times when the carrier passes through waypoints 1 to n, where (x1, y1, z1), (x2, y2, z2), ..., (x n ,y n ,z n ψ1, ψ2, ..., ψn are the three-dimensional coordinates of the carrier when it passes through waypoints 1 to n. n These are the yaw angles of the carrier when it passes through waypoints 1 to n.

[0096] Step (2): Select the corresponding scheme for data preprocessing based on the waypoint data type;

[0097] Step (2-1): Determine the operation to be performed based on the waypoint data type in step (1). If the waypoint data is local coordinate system coordinate data, then execute step (2-2); otherwise, execute step (2-3).

[0098] Step (2-2): Based on the data obtained in step (1), convert the local coordinates of all waypoints to latitude, longitude, and altitude coordinates. The expression is as follows:

[0099] h i =h0+z i

[0100]

[0101]

[0102] Among them, R M R is the radius of curvature of the Earth's geoid. N Let (L0, λ0, h0) be the radius of curvature of the Earth's meridian, and (L0, λ0, h0) be the latitude, longitude, and altitude coordinates of the local coordinate system origin. i ,y i ,z i Let (L) be the coordinates of the i-th waypoint in the local coordinate system. i ,λi ,h i () represents the latitude, longitude, and altitude coordinates of the i-th waypoint after transformation, and π represents pi;

[0103] Step (2-3): Based on the data obtained in step (1), convert all waypoint coordinates into latitude, longitude, and altitude coordinates. The expression is as follows:

[0104] h i =z i

[0105] L i =y i

[0106] λ i =x i

[0107] Among them, (x i ,y i ,z i ) represents the three-dimensional coordinates of the i-th waypoint, (L i ,λ i ,h i ) represents the latitude, longitude, and altitude coordinates of the i-th waypoint after assignment;

[0108] Step (2-4) stores all waypoint data in the path matrix, whose expression is:

[0109]

[0110] Step (3): Use interpolation to augment the preprocessed waypoint data;

[0111] Step (3-1) involves using the discrete data points obtained in step (2-4). Using cubic spline interpolation, the relationships between time and longitude, time and latitude, time and altitude, and time and yaw angle were found respectively. Their expressions are as follows:

[0112] λ = f1(t)

[0113] L=f2(t)

[0114] h = f3(t)

[0115] ψ=f4(t)

[0116] Where (λ,L,h,ψ) represent longitude, latitude, altitude and yaw angle variables respectively, t represents time variable, and (f1,f2,f3,f4) represent the functional relationships between time and longitude, time and latitude, time and altitude, and time and yaw angle variables respectively.

[0117] Step (3-2): Set a suitable and sufficiently small initial sampling period T, in seconds;

[0118] Step (3-3): Based on the functional relationships (f1, f2, f3, f4) between time and longitude, time and latitude, time and altitude, and time and yaw angle variables obtained in step (3-1) and the sampling period T obtained in step (3-2), interpolation is performed between the data of two adjacent waypoints at the current time. The expression is as follows:

[0119]

[0120]

[0121]

[0122]

[0123] Where m is the number of discrete points between adjacent waypoints i and i+1 (i = 1 to n-1). (t i t i +T、t i +2T、…、t i+1 -T、t i+1 The time between adjacent waypoints i and i+1, after a sampling period T, is the discrete time. (j=1~m) represent the j-th longitude, latitude, altitude, and yaw angle variables after sampling period T between adjacent waypoints i and i+1.

[0124] Step (4): Calculate the acceleration and angular velocity of the carrier.

[0125] Step (4-1): Based on the longitude λ and latitude L data of adjacent waypoints i and i+1 obtained in step (3-3) after sampling period T, calculate the velocity of the two-dimensional planar carrier in the direction of its front end. The expression is as follows:

[0126]

[0127] in,

[0128]

[0129] and These represent the latitude and longitude coordinates of two adjacent discrete waypoints i and i+1 after a sampling period T. and These represent the distance between two adjacent discrete waypoints i and i+1 after a sampling period T, and the velocity of the carrier, respectively.

[0130] Step (4-2) involves determining the velocity of the two-dimensional planar carrier in the direction of its head, based on the velocity obtained in step (4-1). The expression for the acceleration of the vehicle's front in the two-dimensional planar direction is as follows:

[0131]

[0132] Step (4-3): Based on the carrier height data obtained in step (3-3), calculate the carrier's celestial velocity, which is expressed as:

[0133]

[0134] Step (4-4): Based on the celestial velocity of the carrier obtained in step (4-3) The expression for calculating the axial acceleration of a two-dimensional planar carrier is as follows:

[0135]

[0136] Step (4-5): Based on the yaw angle data obtained in step (3-3), calculate the yaw angle rate of the vehicle, which is expressed as:

[0137]

[0138] Step (5): Calculate the noise-free output of the inverted gyroscope and accelerometer;

[0139] Step (5-1): Based on the yaw angle variable ψ obtained from the interpolation between waypoints in step (3-3), calculate the attitude transformation matrix of the vehicle system relative to the geographic system. Its expression is:

[0140]

[0141] Where (γ,θ,ψ) are the roll, pitch, and yaw angle variables of the carrier, respectively, γ=0,θ=0;

[0142] Step (5-2): Based on the attitude transformation matrix obtained in step (5-1) The vehicle's frontal velocity ve obtained in step (4-1) and the vehicle's upward velocity vu obtained in step (4-3) are used to calculate the vehicle's geographic velocity. Its expression is:

[0143]

[0144]

[0145]

[0146] in, Let be the component of the velocity of the geographic system relative to the Earth system in the northeast-sky direction within the geographic system. It is a column vector consisting of the components of the velocity of the geographic system relative to the Earth system along the axis of the carrying system;

[0147] Step (5-3) involves determining the geographic velocity of the carrier based on the information obtained in step (5-2). The attitude transformation matrix obtained in step (5-1) The yaw rate ωb obtained in step (4-5), the carrier yaw angle ψ obtained in step (3-3), and the noiseless output of the gyroscope are calculated. Its expression is:

[0148]

[0149] in, Let be the column vector consisting of the components of the Earth system's angular velocity relative to the inertial frame along the axis of the carrying system. It is a column vector consisting of the components of the angular velocity of the geographic system relative to the Earth system along the axis of the carrying system. Let be the column vector formed by the components of the angular velocity of the carrying system relative to the geographic frame along the axis of the carrying system. Let be the column vector consisting of the components of the Earth system's angular velocity relative to the inertial frame along the geographic axis. is the column vector consisting of the components of the angular velocity of the geographic system relative to the Earth system along the axis of the geographic system; (γ, θ, ψ) are the roll, pitch, and yaw angle variables of the vehicle, respectively, γ = 0, θ = 0. These are the yaw angle, pitch angle, roll angle, and angular rate of the carrier, respectively. ie The Earth's rotational angular rate is taken as 7.292115147 × 10⁻⁶. -5 rad / s;

[0150] Step (5-4): Based on the carrier's forward velocity ve obtained in step (4-1), the carrier's forward acceleration ae obtained in step (4-2), the carrier's upward velocity vu obtained in step (4-3), the carrier's upward velocity au obtained in step (4-4), and the angular velocity obtained in step (5-3). The geographic velocity of the carrier obtained in step (5-2) The attitude transformation matrix obtained in step (5-1) Calculate the specific force f measured by the accelerometer b Its expression is:

[0151]

[0152] in,

[0153]

[0154] g is the acceleration due to gravity, f n The column vector is composed of the components of the force in the navigation system. For speed The differential.

[0155] Step (6): Calculate the coordinates of the inverted waypoints of the carrier;

[0156] Step (6-1) is based on the geographic velocity of the carrier obtained in step (5-2). The latitude, longitude, and altitude coordinates of the carrier are calculated using the following expression:

[0157]

[0158]

[0159]

[0160] in, and These are the coordinates of two adjacent discrete waypoints, i and i+1, after being discretized over a sampling period T, in degrees, degrees, and meters. The superscript indicates that this data is the discrete waypoint data interpolated between the i-th waypoint and the (i+1)-th waypoint, and the subscript indicates that this data is the j-th discrete waypoint data interpolated between the i-th waypoint and the (i+1)-th waypoint.

[0161] Step (6-2): Based on the waypoint coordinate calculation formula obtained in step (6-1), the coordinates of the (i+1)th waypoint are continuously calculated from the calculated coordinates of the i-th waypoint. The expression is as follows:

[0162] h( i+1 ) t =h it +Δh i

[0163] L( i+1 ) t =L it +ΔL i

[0164] λ (i+1)t =λ it +Δλ i

[0165] Among them, (L) (i+1)t , λ (i+1)t h (i+1)t ) and (L it , λ it h it) are the calculated coordinates of the (i+1)th and ith waypoints, respectively, (ΔL) i ,Δλ i ,Δh i The coordinate increments in latitude, longitude, and altitude of the (i+1)th waypoint are continuously calculated from the coordinate data of the i-th waypoint.

[0166] Step (7): Evaluate whether the inversion data meets the accuracy requirements;

[0167] Step (7-1): Based on the coordinates of the (i+1)th calculated waypoint obtained in step (6-2) and the coordinates of the (i+1)th actual waypoint obtained in step (2-4), compare and evaluate whether the inversion data meets the accuracy requirements. If the inversion data meets the accuracy requirements, proceed directly to step (8); otherwise, proceed to step (7-2). The accuracy judgment expression for the inversion data is:

[0168]

[0169] Where R is the set accuracy assessment value of the inversion data, in meters, and Re is the Earth's radius, which is 6,378,137.0 meters.

[0170] Step (7-2): Reset the sampling period T, return to step (3-3), and recalculate the interpolation. The expression is:

[0171]

[0172] Among them, T old This is the value from the previous sampling period.

[0173] Step (8): Invert simulation output data from multiple navigation sensors;

[0174] Step (8-1) is based on the specific force f measured by the accelerometer obtained in step (5-4). b Calculate the accelerometer simulation output Its expression is:

[0175]

[0176] in, The error model for accelerometer drift is as follows: for The differential, T a For the relevant time, ω a To measure white noise.

[0177] Step (8-2) is based on the noiseless gyroscope output obtained in step (5-3). Computational gyroscope simulation output Its expression is:

[0178]

[0179] Where ε is the gyroscope drift, (ε b ε r ω g The components are the gyroscope random constant, the first-order Markov process, and the measurement white noise, respectively. The model for the gyroscope random constant is as follows: The first-order Markov process of a gyroscope is T r For the relevant time, ω r To drive white noise;

[0180] Step (8-3) involves using the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3) and the carrier's geographical velocity obtained in step (5-2). The variables are used to calculate the position and velocity data of the vehicle in the GNSS simulation output, and their expressions are as follows:

[0181]

[0182]

[0183]

[0184]

[0185]

[0186]

[0187] in, Let (L, λ, h) be the latitude, longitude, and altitude coordinates of the inverted carrier, and (L, λ, h) be the discrete waypoint coordinate variables between adjacent waypoints i and i+1 after a sampling period T. x GNSS V y GNSS V z GNSS The numbers represent the measurement noise errors in the GNSS longitude, latitude, and altitude directions, respectively, in meters, meters, and meters. The velocity of the carrier in the northeast-sky direction of the geographic system is used for inversion. The noise error in GNSS velocity measurement in the northeast-sky direction of the geographic system is expressed in m / s, m / s, m / s;

[0188] Step (8-4): Calculate the timer simulation output data based on the time variable set in step (3-3). The expression is:

[0189]

[0190] Where t is the time variable for the carrier to pass through waypoint i, and V TM This represents the time deviation on the timer, in seconds.

[0191] Step (8-5): Based on the attitude transformation matrix obtained in step (5-1) The expression for calculating the simulation output data of the magnetic sensor is as follows:

[0192]

[0193] Among them, (H) x H y H z ) represents the known geomagnetic components along the three axes of the northeast celestial sphere in the geographic system, with units of Gs, Gs, and GS. These represent the measurement noise error of the magnetic sensor in the northeast-sky direction of the carrier, in units of Gs, Gs, and GS.

[0194] Step (8-6) calculates the odometer displacement simulation output data based on the motion velocity variable ve of the two-dimensional planar carrier in the direction of the vehicle's front, obtained in step (4-1). The unit is meters, and its expression is:

[0195]

[0196] Among them, V odo The measurement noise error of the odometer displacement is expressed in meters.

[0197] Step (8-7) calculates the number of step sensor pulse outputs within time period T based on the motion velocity variable ve of the two-dimensional planar carrier in the direction of the vehicle head obtained in step (4-1). Its expression is:

[0198]

[0199] V step The measurement noise error of the linear displacement of the stepper sensor is expressed in meters (ΔS). p The unit pulse linear displacement is expressed in meters.

[0200] Step (8-8) calculates the barometric altimeter simulation output data based on the height variable h interpolated from the carrier obtained in step (3-3), and its expression is:

[0201]

[0202] Among them, V BH These are the measurement noise errors of the barometric altimeter in the altitude direction, in meters;

[0203] Step (8-9): Calculate the laser altimeter simulation output data based on the height variable h interpolated from the carrier obtained in step (3-3). The expression is as follows:

[0204]

[0205] Among them, h LH V represents the known local terrain elevation value in meters. LA These are the measurement noise errors in the height direction of the laser altimeter, in meters;

[0206] Step (8-10) calculates the tilt compass simulation output data, the expression of which is:

[0207]

[0208]

[0209] in, These are the measured values ​​of the vehicle's roll and pitch angles from the tilt compass, respectively. (γ, θ) are the interpolated variables corresponding to the vehicle's roll and pitch angles, where γ = 0, θ = 0, (V COMP_DR V COMP_DP The values ​​are the measurement noise errors of the tilt compass for the roll and pitch angles of the carrier, respectively, in degrees.

[0210] Step (8-11) calculates the yaw angle variable ψ obtained from the interpolation of the carrier in step (3-3) to calculate the simulation output data of the gyrocompass, in degrees. The expression is as follows:

[0211]

[0212] in, V is the measured value of the heading angle of the carrier by the gyratory compass. COMP_GY The noise error in the measurement of the carrier's heading angle by the rotary compass is expressed in degrees.

[0213] Step (8-12) calculates the polarization compass simulation output data based on the carrier yaw angle variable ψ obtained from the carrier interpolation in step (3-3). The expression for this data is:

[0214]

[0215] in, Output data for the measured heading angle of the polarized compass, V COMP_PL The measurement noise error of the polarized compass for the heading angle of the carrier is expressed in degrees.

[0216] Step (8-13) calculates the visual sensor simulation output data based on the carrier yaw angle variable ψ obtained from the carrier interpolation in step (3-3). Its expression is:

[0217]

[0218]

[0219]

[0220] in, Let represent the roll, pitch, and yaw angles of the vehicle measured by the visual sensor, respectively. (γ, θ, ψ) are the interpolation variables corresponding to the roll, pitch, and yaw angles of the vehicle. VS_R V VS_P V VS_Y ) represent the measurement noise errors of the visual sensors for the roll, pitch, and yaw angles of the carrier, respectively, in degrees, degrees, and degrees;

[0221] Step (8-14): Calculate the simulation output data of the stop marker sensor based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3). If the distance between the carrier and the marker point is less than the set value D... ST If the stop marker sensor outputs 1, it will output 1; otherwise, it will output 0. The distance decision expression is:

[0222]

[0223] Where (L, λ, h) are the discrete waypoint coordinate variables between adjacent waypoints i and i+1 after a sampling period T, (L ST , λ ST h ST V represents the latitude, longitude, and altitude coordinates of the marker point measured by the stop marker sensor. ST To indicate the noise error in the sensor distance measurement, the unit is meters;

[0224] Step (8-15) calculates the simulated output data of the ultrasonic sensor distance based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows:

[0225]

[0226] Where (L, λ, h) are the discrete waypoint coordinate variables between adjacent waypoints i and i+1 after a sampling period T, (L US , λ US h US V represents the latitude, longitude, and altitude coordinates of the marker point measured by the ultrasonic sensor. US The noise error in distance measurement by the ultrasonic sensor is expressed in meters.

[0227] Step (8-16) calculates the infrared sensor distance simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows:

[0228]

[0229] Where (L, λ, h) are the discrete waypoint coordinate variables between adjacent waypoints i and i+1 after a sampling period T, (L IS , λ IS h IS V represents the latitude, longitude, and altitude coordinates of the marker point measured by the infrared sensor. IS The distance measurement noise error of the infrared sensor is expressed in meters.

[0230] Step (8-17) calculates the two-dimensional lidar range simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows:

[0231]

[0232] Among them, (L) t , λ t () represents the two-dimensional latitude and longitude coordinates of the lidar at time t, in degrees. This is expressed as the rotation angle of the two-dimensional plane at time t. The two-dimensional latitude and longitude coordinates of the target point scanned by the lidar transmitter, in degrees; V LD The distance measurement noise error of the lidar sensor is expressed in meters.

[0233] Step (8-18) calculates the Bluetooth sensor simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows:

[0234]

[0235] Where (L, λ, h) are the discrete waypoint coordinate variables between adjacent waypoints i and i+1 after a sampling period T, (L BT , λ BT h BT (V) represents the latitude, longitude, and altitude coordinates of the Bluetooth base station. BT The noise error in Bluetooth sensor distance measurement is expressed in meters.

[0236] Step (8-19) calculates the WIFI simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows:

[0237]

[0238] Where (L, λ, h) are the discrete waypoint coordinate variables between adjacent waypoints i and i+1 after a sampling period T, (L WiFi , λ WiFi h WiFi (V) represents the latitude, longitude, and altitude coordinates of the WIFI base station. WiFi This is the noise error for WIFI distance measurement, in meters.

[0239] Step (8-20) calculates the UWB simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows:

[0240]

[0241] Where (L, λ, h) are the discrete waypoint coordinate variables between adjacent waypoints i and i+1 after a sampling period T, (L UWB , λ UWB h UWB () represents the latitude, longitude, and altitude coordinates of the UWB base station, V UWB The noise error for UWB distance measurement is expressed in meters.

[0242] To verify the effectiveness of the proposed heterogeneous multi-source navigation information inversion method for incomplete measurement conditions, digital simulation analysis was conducted. The accuracy of the simulated data was demonstrated by using the unbiased gyroscope and accelerometer data inverted by the method of this invention to positively extrapolate the vehicle's trajectory. Figure 2 This is a comparison chart showing the positioning error values ​​of the carrier waypoint coordinates retrieved using the method of this invention and the traditional method, respectively. Figure 3 A comparison chart showing the trajectory calculated by multi-sensor data fusion using the method of this invention, the actual motion trajectory of the carrier, and the trajectory calculated by multi-sensor data fusion using traditional methods; Figure 4 This is a top view comparing the trajectory calculated by multi-sensor data fusion using the method of this invention, the actual motion trajectory of the carrier, and the trajectory calculated by multi-sensor data fusion using traditional methods. Figure 5 A comparison chart showing the longitude, latitude, and altitude values ​​of the multi-sensor data fusion-based trajectory derived using the method of this invention, the actual motion trajectory of the carrier, and the multi-sensor data fusion-based trajectory derived using traditional methods. Figures 6-17 The data are various navigation sensor data retrieved using the method of this invention; Figure 18 , 19 Figures 20 and 21 are comparison curves of the carrier position, velocity, and attitude calculated by fusing multi-sensor data obtained by the method of this invention and the traditional method, and the actual position, velocity, and attitude error curves of the carrier.

[0243] Depend on Figure 2 It can be seen that the positioning error of the carrier waypoint coordinates inverted by the method of the present invention is significantly smaller than that of the carrier waypoint coordinates inverted by the traditional method, indicating that the waypoint coordinates inverted by the noiseless output of the IMU after feedback correction and iteration by the method of the present invention are close to the real waypoint coordinate data.

[0244] Depend on Figure 3 , Figure 4 and Figure 5 It can be seen that the carrier motion trajectory calculated by multi-sensor data fusion using traditional methods cannot fit the actual carrier motion trajectory well, while the carrier motion trajectory calculated by multi-sensor data fusion using the method of this invention can fit the actual carrier motion trajectory well, thus reflecting the accuracy of the carrier multi-navigation sensor data retrieved using the method of this invention.

[0245] Depend on Figure 18 , 19 As can be seen from Figure 20, the navigation state quantity error value obtained by multi-sensor information fusion through traditional methods is significantly greater than that obtained by multi-sensor information fusion through the method of this invention, which demonstrates the effectiveness of this invention. Furthermore, the position, velocity, and attitude errors obtained by the method of this invention are all relatively small, within the specified range, and meet practical requirements.

[0246] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions, characterized in that, Includes the following steps: Step (1): Obtain waypoint data during the carrier's movement; Step (2): Select the corresponding scheme for data preprocessing based on the waypoint data type; Step (3): Use interpolation to augment the preprocessed waypoint data; Step (4): Calculate the acceleration and angular velocity of the carrier. Step (5): Calculate the noise-free output of the inverted gyroscope and accelerometer; Step (6): Calculate the coordinates of the inverted waypoints of the carrier; Step (7): Evaluate whether the inversion data meets the accuracy requirements; Step (6) includes the following specific steps: Step (6-1), based on the geographical speed of the carrier The latitude, longitude, and altitude coordinates of the carrier are calculated using the following expression: in, The radius of curvature of the Earth's geoid. The radius of curvature of the Earth's meridian. Adjacent waypoints and Between sampling periods Discretized coordinates of two adjacent discrete waypoints, in degrees, meters; the superscript indicates that this data is the [number]th [value]. The waypoint and the first Discrete waypoint data interpolated between waypoints, with the symbolic subscript indicating that this data is the nth waypoint. The waypoint and the first The interpolation between waypoints yields the first... Discrete waypoint data; Step (6-2): Based on the waypoint coordinate calculation formula obtained in step (6-1), the calculated first... The coordinates of the waypoints are used to continuously calculate the first... The coordinates of each waypoint are expressed as follows: in, and The calculated number of , Coordinates of each waypoint. The first one obtained by calculation Using the coordinates of each waypoint to continuously calculate the... Increment of latitude, longitude, and altitude coordinates for each waypoint; Step (7) includes the following specific steps: Step (7-1), according to the first The calculated waypoint coordinates and the first The coordinates of the actual waypoints are compared to evaluate whether the inverted data meets the accuracy requirements. If the inverted data meets the accuracy requirements, proceed directly to step (8); otherwise, proceed to step (7-2). The accuracy judgment expression for the inverted data is: in, This is the set accuracy evaluation value for the inversion data, in meters. Let be the Earth's radius, and take the value . ; Step (7-2), reset the sampling period Return to step (3-3) and recalculate the interpolation. The expression is: in, This is the value from the previous sampling period; Step (8) Invert the simulation output data of multiple navigation sensors.

2. The method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions according to claim 1, characterized in that, In step (1), the carrier waypoint data type is latitude, longitude, and altitude data or local coordinate system coordinate data, to obtain the number of times the carrier passes through during its movement. Data for each waypoint If the data is in a local coordinate system, the latitude, longitude, and altitude coordinates of the local coordinate system origin should also be obtained. Its expression is: in, These represent the latitude, longitude, and altitude coordinates of the local coordinate system origin. The carriers passed through the first Waypoint times The carriers passed through the first The three-dimensional coordinates at the waypoint The carriers passed through the first Yaw angle at waypoints.

3. The method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions according to claim 2, characterized in that, Step (2) includes the following specific steps: Step (2-1): Determine the operation to be performed based on the waypoint data type in step (1). If the waypoint data is local coordinate system coordinate data, then execute step (2-2); otherwise, execute step (2-3). Step (2-2): Based on the data obtained in step (1), convert the local coordinates of all waypoints to latitude, longitude, and altitude coordinates. The expression is as follows: in, The coordinates are the latitude, longitude, and altitude coordinates of the local coordinate system origin. For the first The coordinates of each waypoint in the local coordinate system For the transformed first Latitude, longitude, and altitude coordinates of each waypoint Pi; Step (2-3): Based on the data obtained in step (1), convert all waypoint coordinates into latitude, longitude, and altitude coordinates. The expression is as follows: in, For the first Three-dimensional coordinates of each waypoint For the first time after assignment Latitude, longitude, and altitude coordinates of each waypoint; Step (2-4) involves storing all waypoint data in the track matrix. In Chinese, its expression is: 。 4. The method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions according to claim 3, characterized in that, Step (3) includes the following specific steps: Step (3-1) involves using the discrete data points obtained in step (2-4). , , , Using cubic spline interpolation, the relationships between time and longitude, time and latitude, time and altitude, and time and yaw angle were found respectively. Their expressions are: in, These represent longitude, latitude, altitude, and yaw angle variables, respectively. Represents a time variable. These represent the functional relationships between time and longitude, time and latitude, time and altitude, and time and yaw angle, respectively. Step (3-2): Set a suitable initial sampling period that is small enough. The unit is seconds; Step (3-3) involves establishing the functional relationships between time and longitude, time and latitude, time and altitude, and time and yaw angle obtained in step (3-1). The sampling period obtained in step (3-2) The interpolation augmentation is performed between the data of two adjacent waypoints at the current time, and its expression is: in, Adjacent waypoints and The number of discrete points between them , Adjacent waypoints and Between sampling periods Discretized time, Adjacent waypoints and Between sampling periods Discretized The variables are longitude, latitude, altitude, and yaw angle.

5. The method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions according to claim 4, characterized in that, Step (4) includes the following specific steps: Step (4-1) involves using the adjacent waypoints obtained in step (3-3). and Between sampling periods Discrete longitude with latitude The data is used to calculate the velocity of the two-dimensional planar carrier in the direction of its front end. The expression for this velocity is: in, Adjacent waypoints and Between sampling periods The latitude and longitude coordinates of two adjacent discrete waypoints after discretization. and Adjacent waypoints and Between sampling periods The distance between two adjacent discrete waypoints after discretization and the speed of the vehicle; Step (4-2) involves determining the velocity of the two-dimensional planar carrier in the direction of its head, based on the velocity obtained in step (4-1). The acceleration of the two-dimensional planar carrier in the direction of its front is calculated using the following expression: Step (4-3): Based on the carrier height data obtained in step (3-3), calculate the carrier's celestial velocity, which is expressed as: Step (4-4): Based on the celestial velocity of the carrier obtained in step (4-3) The expression for calculating the axial acceleration of a two-dimensional planar carrier is as follows: Step (4-5): Based on the yaw angle data obtained in step (3-3), calculate the yaw angle rate of the vehicle, which is expressed as: 。 6. The method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions according to claim 5, characterized in that, Step (5) includes the following specific steps: Step (5-1) involves interpolating the yaw angle variables between waypoints obtained in step (3-3). The attitude transformation matrix of the carrier system relative to the geographic system is calculated, and its expression is: in, These are the vehicle's roll, pitch, and yaw angle variables, respectively. ; Step (5-2): Based on the attitude transformation matrix obtained in step (5-1) The carrier head speed obtained in step (4-1) The celestial velocity of the carrier obtained in step (4-3) Calculate the geographic speed of the carrier Its expression is: in, , Let be the component of the velocity of the geographic system relative to the Earth system in the northeast-sky direction within the geographic system. It is a column vector consisting of the components of the velocity of the geographic system relative to the Earth system along the axis of the carrying system; Step (5-3) involves determining the geographic velocity of the carrier based on the information obtained in step (5-2). The attitude transformation matrix obtained in step (5-1) The yaw rate obtained in step (4-5) The yaw angle of the carrier obtained in step (3-3) The computational gyroscope has no noise output. Its expression is: in, Let be the column vector consisting of the components of the Earth system's angular velocity relative to the inertial frame along the axis of the carrying system. It is a column vector consisting of the components of the angular velocity of the geographic system relative to the Earth system along the axis of the carrying system. Let be the column vector formed by the components of the angular velocity of the carrying system relative to the geographic frame along the axis of the carrying system. Let be the column vector consisting of the components of the Earth system's angular velocity relative to the inertial frame along the geographic axis. It is a column vector consisting of the components of the angular velocity of the geographic system relative to the Earth system along the axis of the geographic system; These are the vehicle's roll, pitch, and yaw angle variables, respectively. , These are the yaw angle, pitch angle, roll angle, and angular rate of the carrier, respectively. The Earth's rotation angular rate is given by a value of ; Step (5-4), based on the carrier head direction velocity obtained in step (4-1) Based on the acceleration in the direction of the vehicle's front end obtained in step (4-2) Based on the carrier celestial velocity obtained in step (4-3) Based on the carrier celestial velocity obtained in step (4-4) Based on the angular velocity obtained in step (5-3) The geographic velocity of the carrier obtained in step (5-2) The attitude transformation matrix obtained in step (5-1) Calculate the specific force measured by the accelerometer. Its expression is: in, It is the acceleration due to gravity. The column vector is composed of the components of the force in the navigation system. For speed The differential.

7. The method for inverting heterogeneous multi-source navigation information under incomplete measurement conditions according to claim 6, characterized in that, Step (8) includes the following specific steps: Step (8-1) is based on the specific force measured by the accelerometer obtained in step (5-4). Calculate the accelerometer simulation output Its expression is: in, The error model for accelerometer drift is as follows: , for The differential, For the relevant time, To measure white noise; Step (8-2) is based on the noiseless gyroscope output obtained in step (5-3). Calculate the output of the gyroscope simulation. Its expression is: in, For gyroscope drift, These represent the gyroscope random constant, a first-order Markov process, and measurement white noise, respectively. The gyroscope random constant model is as follows: The first-order Markov process of the gyroscope is as follows: , For the relevant time, To drive white noise; Step (8-3) involves using the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3) and the geographical velocity of the carrier obtained in step (5-2). The variables are used to calculate the position and velocity data of the vehicle in the GNSS simulation output, and their expressions are as follows: in, The latitude, longitude, and altitude coordinates are used as the carriers for inversion. Adjacent waypoints and Between sampling periods Discretize the waypoint coordinates. These are the measurement noise errors in the GNSS longitude, latitude, and altitude directions, respectively, in meters, meters, and meters. The velocity of the carrier in the northeast-sky direction of the geographic system is used for inversion. The noise error in GNSS velocity measurement in the northeast-sky direction of the geographic system is expressed in m / s, m / s, m / s; Step (8-4) calculates the timer simulation output data based on the time variable set in step (3-3), and its expression is: in, For the carrier to pass through waypoints The time variable, This represents the time deviation on the timer, in seconds. Step (8-5): Based on the attitude transformation matrix obtained in step (5-1) The expression for calculating the simulation output data of the magnetic sensor is as follows: in, These are the known geomagnetic components along the three axes of the northeast celestial sphere in the geographic system, in units of: ; These represent the measurement noise error of the magnetic sensor in the northeast-sky direction of the carrier, in units of: ; Step (8-6) involves obtaining the velocity variable of the two-dimensional planar carrier in the direction of the vehicle's front end from step (4-1). To calculate the odometer displacement simulation output data, in meters, the expression is: in, The measurement noise error of the odometer displacement is expressed in meters. Step (8-7) involves obtaining the velocity variable of the two-dimensional planar carrier in the direction of the vehicle's front end from step (4-1). To calculate within the time period Number of pulse outputs from internal stepper sensor Its expression is: The measurement noise error of the linear displacement of the stepper sensor is expressed in meters. The unit pulse linear displacement is expressed in meters. Step (8-8) involves interpolating the height variable from the carrier obtained in step (3-3). The expression for calculating the simulated output data of the barometric altimeter is as follows: in, These are the measurement noise errors of the barometric altimeter in the altitude direction, in meters; Steps (8-9) involve interpolating the height variable from the carrier obtained in step (3-3). The expression for calculating the simulated output data of the laser altimeter is as follows: in, The known local terrain elevation values ​​are in meters. These are the measurement noise errors in the height direction of the laser altimeter, in meters; Step (8-10) calculates the tilt compass simulation output data, the expression of which is: in, These are the measured values ​​of the roll and pitch angles of the vehicle obtained by the tilt compass. To correspond to the interpolation variables of the vehicle's roll and pitch angles, , These are the measurement noise errors of the tilt compass for the roll and pitch angles of the carrier, respectively, in degrees. Step (8-11) involves interpolating the carrier yaw angle variable obtained in step (3-3). To calculate the simulation output data of the rotary compass, in degrees, the expression is: in, This is the measured value of the heading angle of the carrier by the rotary compass. The noise error in the measurement of the carrier's heading angle by the rotary compass is expressed in degrees. Step (8-12) involves interpolating the carrier yaw angle variable obtained in step (3-3). The expression for calculating the simulation output data of the polarization compass is as follows: in, Output data for the measured heading angle of the polarized optical compass. The measurement noise error of the polarized compass for the heading angle of the carrier is expressed in degrees. Step (8-13) involves interpolating the carrier yaw angle variable obtained in step (3-3). The expression for calculating the simulated output data of the vision sensor is as follows: in, These are the measurements of the carrier's roll, pitch, and yaw angles taken by the visual sensors, respectively. To correspond to the interpolation variables of the vehicle's roll, pitch, and yaw angles, These are the measurement noise errors of the visual sensors for the roll, pitch, and yaw angles of the carrier, respectively, in degrees, degrees, and degrees. Step (8-14): Calculate the simulation output data of the stop marker sensor based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3). If the distance between the carrier and the marker point is less than the set value... If the stop marker sensor outputs 1, it will output 1; otherwise, it will output 0. The distance decision expression is: in, Adjacent waypoints and Between sampling periods Discretize the waypoint coordinates. To stop marking the latitude, longitude, and altitude coordinates of the marker points measured by the sensor, To indicate the noise error in the sensor distance measurement, the unit is meters; Step (8-15) calculates the simulated output data of the ultrasonic sensor distance based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows: in, Adjacent waypoints and Between sampling periods Discretize the waypoint coordinates. The latitude, longitude, and altitude coordinates of the marker points measured by the ultrasonic sensor. The noise error in distance measurement by the ultrasonic sensor is expressed in meters. Step (8-16) calculates the infrared sensor distance simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows: in, Adjacent waypoints and Between sampling periods Discretize the waypoint coordinates. The latitude, longitude, and altitude coordinates of the marker point measured by the infrared sensor. The distance measurement noise error of the infrared sensor is expressed in meters. Step (8-17) calculates the two-dimensional lidar range simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows: in, For lidar in time The two-dimensional latitude and longitude coordinates of time, in degrees; Represented as in time The rotation angle of the two-dimensional plane is The two-dimensional latitude and longitude coordinates of the target point scanned by the lidar transmitter, in degrees; The distance measurement noise error of the lidar sensor is expressed in meters. Step (8-18) calculates the Bluetooth sensor simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows: in, Adjacent waypoints and Between sampling periods Discretize the waypoint coordinates. The latitude, longitude, and altitude coordinates of the Bluetooth base station. The noise error in Bluetooth sensor distance measurement is expressed in meters. Step (8-19) calculates the WIFI simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows: in, Adjacent waypoints and Between sampling periods Discretize the waypoint coordinates. The latitude, longitude, and altitude coordinates of the WIFI base station. This is the noise error for WIFI distance measurement, in meters. Step (8-20) calculates the UWB simulation output data based on the latitude, longitude, and altitude variables interpolated from the carrier obtained in step (3-3), in meters. The expression is as follows: in, Adjacent waypoints and Between sampling periods Discretize the waypoint coordinates. The latitude, longitude, and altitude coordinates of the UWB base station. The noise error for UWB distance measurement is expressed in meters.

Images