Inertial navigation method based on single-star vector assistance

Abstract

The invention discloses an inertial navigation method based on single-star vector assistance, and belongs to the technical field of satellite navigation and inertial navigation combination. In particular to a method and a system for realizing high-precision positioning of a dynamic target by fusing observed quantities of a pseudo range, a pitch angle and an azimuth angle under the condition that only a single satellite is available, and the method and the system are suitable for complex environments where satellite signals of unmanned aerial vehicles, vehicle navigation and the like are limited in urban canyons, indoor environments and the like. According to the method, high-precision positioning can be realized under the condition that a single satellite is available, the method is suitable for complex environments with limited satellite signals, such as urban canyons or indoor environments, and the robustness and stability of a navigation system in a dynamic environment are enhanced. Compared with a method only using single-star pseudo-range or single-star angle observed quantity, the method provided by the invention has the advantages that through multi-dimensional observed quantity fusion, the accumulation speed of errors of the inertial navigation system is effectively slowed down, the risk of system divergence is reduced, and the effective working time of the system is prolonged.

Description

Technical Field

[0001] This invention belongs to the field of satellite navigation and inertial navigation combination technology, specifically relating to a method and system for achieving high-precision positioning of dynamic targets by fusing pseudorange, pitch angle and azimuth angle observations when only a single satellite is available. It is applicable to complex environments such as urban canyons and indoor spaces where satellite signals are limited, such as those used by UAVs and vehicle navigation systems. Background Technology

[0002] Existing inertial navigation systems (INS) rely on inertial devices such as accelerometers and gyroscopes to calculate position, velocity, and attitude, offering the advantage of high autonomy. However, due to the error accumulation problem inherent in low-cost MEMS devices, the navigation accuracy of INS degrades rapidly over time, even diverging. Traditional solutions rely on multiple GNSS satellite signals for correction, but in complex environments such as urban canyons, indoor spaces, or complex electromagnetic environments, multi-satellite positioning is difficult to achieve, limiting the application of this method. To address navigation requirements in constrained environments, existing research has explored using pseudorange observations from a single satellite to assist INS error correction. This limitation of a single observation dimension leads to insufficient robustness and stability of the system in dynamic environments, making it difficult to meet the application requirements of high-precision navigation. Therefore, there is an urgent need to develop a novel navigation method that integrates multiple observation information to improve the overall performance of INS under conditions where only a single satellite is available. Summary of the Invention

[0003] The technical problem to be solved: In existing single-satellite assisted inertial navigation and positioning systems, relying solely on single-satellite pseudorange observations cannot effectively suppress error accumulation. Considering the nonlinear distribution of errors in inertial navigation systems, this invention proposes a particle filter fusion positioning method that integrates single-satellite pseudorange and angle observations with the inertial navigation system. This method mainly includes initial calculation of strapdown inertial navigation positioning, and using particle filtering to fuse the position, velocity, and attitude output by the INS inertial navigation part to correct the results. This corrects system error divergence in environments with limited satellite signals, thereby improving the accuracy and robustness of the navigation system.

[0004] The inertial navigation method based on single-star vector assistance described in this invention is characterized by the following steps:

[0005] Step 1: The angular velocity and acceleration information output by the MEMS inertial measurement unit are processed by the strapdown inertial navigation algorithm. The attitude of the inertial measurement unit is updated using the quaternion method. Based on the ENU coordinate system of the northeast sky, the recursive calculation of the carrier's velocity and position is completed to obtain the initial position, velocity, and attitude information of the carrier.

[0006] X INS =[P INS VINS A INS ] T (1)

[0007] Among them, X INS This is the initial navigation state of the inertial navigation system. V provides initial latitude, longitude, and altitude information. INS =[v EINS v NINS v UINS ] T Let be the initial velocity along the coordinate axes in the ENU coordinate system. This provides initial roll, pitch, and yaw angle information;

[0008] Step 2: Receive satellite signals through a satellite receiver, and obtain single-satellite ephemeris data after demodulation, pseudo-code acquisition and tracking, and navigation message decoding; calculate the pseudorange observation ρ from the single satellite to the receiver based on the pseudo-random code in the satellite signal; simultaneously, measure the phase difference of the satellite signal relative to different antennas using an antenna array; perform angle measurement through phase difference calculation to obtain the azimuth observation Azi and elevation observation Ele of the single satellite relative to the receiver.

[0009] Step 3: Construct the system state equations;

[0010] The system state equation is:

[0011]

[0012] System error state vector These represent 3D position error, 3D velocity error, 3D attitude error, 3-axis accelerometer error, and 3-axis gyroscope error, respectively. The accelerometer error and gyroscope error are modeled as first-order Gaussian Markov processes. W represents the error of the state equation, which is represented by a zero-mean white noise vector. G represents the noise figure matrix, and F represents the state transition matrix.

[0013] Step 4: Construct the system observation equations;

[0014] When using single-satellite pseudoranges as the observation measure, the observation equation is:

[0015] Z = HX + η (4)

[0016] At the same time, Z = ρ - ρ INS It is the difference between the observed pseudorange of a single star and the calculated pseudorange, where ρ represents the measured pseudorange of the single star. INS The pseudorange value is calculated from the positioning results of the inertial navigation system, and η is the observation noise; in Formula 4, H is the pseudorange observation Jacobian matrix, and f represents the functional relationship between latitude, longitude, altitude, and distance. The symbol is for partial differentials;

[0017] When using the azimuth and elevation angles of a single satellite as observations, the system observation equations are:

[0018] Z = HX + η (5)

[0019] at the same time Azi is the difference between the observed angle of a single star and the calculated angle. Azi represents the observed azimuth angle of a single star. INS Ele represents the azimuth angle calculated from the inertial navigation positioning results, and Ele represents the elevation angle observation of a single satellite. INS This represents the pitch angle result calculated based on the inertial navigation positioning result. In Formula 5, H is the angle observation Jacobian matrix, and f1 and f2 represent the functional relationships between latitude, longitude, and altitude and azimuth and pitch angles, respectively.

[0020] Step 5: Set the number of particles N p An initial particle swarm is generated based on the initial calculation results of position, velocity, and attitude from step 1. State vector Random noise N(0,Q) is superimposed on the system state vector X from step 3. This random noise is represented by a zero-mean white noise vector, consistent with the IMU error model; initial weights... Ensure uniform particle distribution;

[0021] Step 6: Fuse the system's state equation and observation equation using particle filtering to estimate the state error of the inertial navigation system; for each particle i, perform nonlinear prediction based on the system state equation from Step 3, where i = 1, 2, ..., N p ;

[0022]

[0023] in, Let f(·) be the predicted state vector of particle i at time k, and let f(·) be the nonlinear state transition function. The process noise at time k-1 is modeled as zero-mean Gaussian white noise; when the observation is a pseudorange, the observation model in formula (4) is used to predict the particle state. Calculate pseudorange measurement value

[0024]

[0025] in, To predict the pseudorange, f(·) is a nonlinear function that maps the state vector to the pseudorange. The predicted state of the i-th particle at time k is given; and compared with the measured pseudorange in step 2, the pseudorange observation residual is calculated:

[0026]

[0027] When the observation is an angle, the observation model based on formula (5) is used to predict the particle state. Azimuth measurement value and pitch angle measurement value

[0028]

[0029] Where f1(·) and f2(·) are nonlinear functions mapping the state vector to the azimuth and elevation angles, respectively; and compared with the measured azimuth and elevation angles in step 2, the angle observation residuals are calculated:

[0030]

[0031] in, α represents the azimuth observation residual. k This represents the measured azimuth at time k. β represents the observation residual of the pitch angle. k The measured pitch angle at time k is used to update the particle weights based on the observation difference.

[0032]

[0033] Where ∝ represents a proportional relationship, used to simplify the expression for calculating particle weights. z represents the predicted state vector of particle i at time k based on the state at time k-1. k Let h(·) be the pseudorange or angle observation value at time k, h(·) be the corresponding observation function, and R be the observation noise covariance matrix; normalize the weights of all particles.

[0034]

[0035] in The normalized ion weights are represented; the optimal estimated state error of the system state is obtained by weighted summation.

[0036]

[0037] Based on normalized weights Resampling is performed to avoid particle degradation; a systematic resampling method is used to generate a new particle swarm. Ensure that the particle distribution is concentrated in the high likelihood region;

[0038] Step 7: Estimating the state error from the particle filter The error is decomposed into position error δP, velocity error δV, and attitude error δA. The error terms are fed back to the inertial navigation system, and the position, velocity, and attitude output by the inertial navigation system are corrected in real time through closed-loop feedback.

[0039] Position correction value P = P INS -δP, velocity correction value V = V INS -δV, attitude correction value A = A INS -δA;

[0040] Where P represents the actual location result, P INS V represents the position result calculated by the inertial navigation system, and V represents the actual velocity result. INS A represents the velocity result calculated by the inertial navigation system, and A represents the actual attitude result. INS This represents the attitude result calculated by the inertial navigation system.

[0041] The navigation solution is continuously iterated and optimized, and the final navigation result is output after the stopping condition is met.

[0042] Furthermore, in step 3:

[0043]

[0044] in:

[0045]

[0046]

[0047] Where R represents the Earth's average radius, M represents the radius of curvature of the meridian, N represents the radius of curvature of the geocentric circle, γ represents the Earth's gravitational acceleration, and ω... ie Represents the Earth's angular velocity of rotation. Indicates the rate of change of dimension. f represents the rate of change of longitude. U Indicates the force of the heavens, f E Indicates the eastward force, f N Indicates northward comparison, τ represents the direction cosine matrix. ax τ represents the relevant time constant of the x-axis accelerometer error. ay τ represents the relevant time constant of the y-axis accelerometer error. az τ represents the relevant time constant of the z-axis accelerometer error. gx τ represents the relevant time constant of the x-axis gyroscope error. gy τ represents the relevant time constant of the y-axis gyroscope error. gz This represents the relevant time constant of the z-axis gyroscope error;

[0048] Furthermore, in step 4, the pseudorange observation Jacobian matrix in formula 4 is:

[0049]

[0050] Furthermore, the angle observation Jacobian matrix in formula 5 of step 4 is:

[0051]

[0052] Furthermore, the stopping condition in step 7 is if P < 0.1m for three consecutive iterations or if the maximum number of iterations of 100 is reached.

[0053] Beneficial effects:

[0054] This invention effectively suppresses the accumulation of horizontal and vertical errors in inertial navigation systems and significantly improves positioning accuracy by fusing single-satellite pseudorange observations and single-satellite angle observations.

[0055] This invention enables high-precision positioning when a single satellite is available, and is suitable for complex environments where satellite signals are limited, such as urban canyons or indoors, enhancing the robustness and stability of the navigation system in dynamic environments.

[0056] Compared to methods that only use single-star pseudorange or single-star angle observations, this invention effectively slows down the accumulation rate of errors in inertial navigation systems, reduces the risk of system divergence, and extends the effective working time of the system by fusing multi-dimensional observations. Attached Figure Description

[0057] Figure 1 This is a structural diagram of a combined inertial system / single-satellite pseudorange observation / angle observation system. Detailed Implementation

[0058] This invention proposes an inertial navigation method based on single-satellite vector assistance, applicable to dynamic target navigation scenarios in environments with limited satellite signals, such as urban canyons, tunnels, or indoors where only a single satellite signal can be received. This method is based on the fusion positioning of the inertial navigation system with single-satellite pseudorange and angle observations, and employs a particle filter framework to estimate and correct the navigation state.

[0059] In practice, the acceleration and angular velocity obtained by the inertial measurement unit (IMU) are first input into the strapdown inertial navigation solution module. The attitude is updated using the quaternion method, and the velocity and position are recursively calculated in the northeast-northeast coordinate system to obtain the current initial navigation state of the system, including position, velocity and attitude information.

[0060] The angular velocity and acceleration information output by the MEMS inertial measurement unit are processed by the strapdown inertial navigation algorithm. The attitude of the inertial measurement unit is updated using the quaternion method. Based on the ENU coordinate system of the northeast sky, the velocity and position of the carrier are recursively calculated to obtain the initial position, velocity and attitude information of the carrier.

[0061] X INS =[P INS V INS A INS ] T (1)

[0062] Among them, X INS This is the initial navigation state of the inertial navigation system. V provides initial latitude, longitude, and altitude information. INS =[v EINS v NINS v UINS ] T Let be the initial velocity along the coordinate axes in the ENU coordinate system. This provides the initial roll angle, pitch angle, and yaw angle information.

[0063] Subsequently, the satellite receiver receives signals from a single satellite, and after signal acquisition and navigation message decoding, satellite ephemeris and pseudorange observations are obtained. The phase difference of the received signals is measured by the array antenna, and azimuth and elevation angle observation information are further calculated.

[0064] The inertial navigation system (INS) calculation results are used as the initial values ​​of the system state to establish a system state vector that includes position, velocity, attitude, and sensor error terms. The system state transition model adopts a discrete-time nonlinear model, considering the zero-bias drift of the accelerometer and gyroscope, and an observation model is constructed to describe the nonlinear relationship between pseudorange and angle observations and state variables.

[0065] The system state equation is:

[0066]

[0067] System error state vector These represent 3D position error, 3D velocity error, 3D attitude error, 3-axis accelerometer error, and 3-axis gyroscope error, respectively. The accelerometer error and gyroscope error are modeled as first-order Gaussian Markov processes. W represents the error of the state equation, which is represented by a zero-mean white noise vector. G represents the noise figure matrix, and F represents the state transition matrix.

[0068] When using single-satellite pseudoranges as the observation measure, the observation equation is:

[0069] Z = HX + η (4)

[0070] At the same time, Z = ρ - ρ INS It is the difference between the observed pseudorange of a single star and the calculated pseudorange, where ρ represents the measured pseudorange of the single star. INS The pseudorange value is calculated from the positioning results of the inertial navigation system, and η is the observation noise; in Formula 4, H is the pseudorange observation Jacobian matrix, and f represents the functional relationship between latitude, longitude, altitude, and distance. The symbol is for partial differentials;

[0071] When using the azimuth and elevation angles of a single satellite as observations, the system observation equations are:

[0072] Z = HX + η (5)

[0073] at the same time Azi is the difference between the observed angle of a single star and the calculated angle. Azi represents the observed azimuth angle of a single star. INS Ele represents the azimuth angle calculated from the inertial navigation positioning results, and Ele represents the elevation angle observation of a single satellite. INS This represents the pitch angle result calculated based on the inertial navigation positioning result. In Formula 5, H is the angle observation Jacobian matrix, and f1 and f2 represent the functional relationships between latitude, longitude, and altitude and azimuth and pitch angles, respectively.

[0074] In the particle filtering fusion process, the number of particles is first set and an initial particle swarm is generated. The state of each particle is then predicted using the state equation. Based on the current particle state, pseudorange or angle prediction values ​​are calculated and compared with measured values ​​to calculate the observation residual. Particle weights are calculated using the Gaussian likelihood function and then normalized. Subsequently, a system resampling algorithm is used to generate a new particle set to avoid particle degradation.

[0075] The weighted average of all particles is used as the current navigation state estimate. Position error, velocity error, and attitude error are extracted from this estimate and fed back to the inertial navigation system (INS) to achieve real-time closed-loop correction of the INS output. Through iterative calculations, the system's positioning accuracy and attitude calculation stability are gradually improved.

[0076] Set the number of particles N p An initial particle swarm is generated based on the initial calculation results of position, velocity, and attitude from step 1. State vector Random noise N(0,Q) is superimposed on the system state vector X from step 3. This random noise is represented by a zero-mean white noise vector, consistent with the IMU error model; initial weights... Ensure uniform particle distribution;

[0077] The state equation and observation equation of the system are fused by particle filtering to estimate the state error of the inertial navigation system; for each particle i, a nonlinear prediction is performed based on the system state equation from step 3, where i = 1, 2, ..., N. p ;

[0078]

[0079] in, Let f(·) be the predicted state vector of particle i at time k, and let f(·) be the nonlinear state transition function. The process noise at time k-1 is modeled as zero-mean Gaussian white noise; when the observation is a pseudorange, the observation model in formula (4) is used to predict the particle state. Calculate pseudorange measurement value

[0080]

[0081] in, To predict the pseudorange, f(·) is a nonlinear function that maps the state vector to the pseudorange. The predicted state of the i-th particle at time k is given; and compared with the measured pseudorange in step 2, the pseudorange observation residual is calculated:

[0082]

[0083] When the observation is an angle, the observation model based on formula (5) is used to predict the particle state. Azimuth measurement value and pitch angle measurement value

[0084]

[0085] Where f1(·) and f2(·) are nonlinear functions mapping the state vector to the azimuth and elevation angles, respectively; and compared with the measured azimuth and elevation angles in step 2, the angle observation residuals are calculated:

[0086]

[0087] in, α represents the azimuth observation residual. k This represents the measured azimuth at time k. β represents the observation residual of the pitch angle. k The measured pitch angle at time k is used to update the particle weights based on the observation difference.

[0088]

[0089] Where ∝ represents a proportional relationship, used to simplify the expression for calculating particle weights. z represents the predicted state vector of particle i at time k based on the state at time k-1. k Let h(·) be the pseudorange or angle observation value at time k, h(·) be the corresponding observation function, and R be the observation noise covariance matrix; normalize the weights of all particles.

[0090]

[0091] in The normalized ion weights are represented; the optimal estimated state error of the system state is obtained by weighted summation.

[0092]

[0093] Based on normalized weights Resampling is performed to avoid particle degradation; a systematic resampling method is used to generate a new particle swarm. Ensure that the particle distribution is concentrated in the high likelihood region.

[0094] This implementation method does not rely on multi-satellite observation conditions. In complex environments where only one satellite is visible, it can effectively suppress error divergence of the inertial navigation system, maintain the continuity and stability of the system's navigation solution, and is suitable for the high-precision navigation needs of UAVs, vehicle-mounted platforms and other mobile targets in GNSS-restricted areas.

Claims

1. An inertial navigation method based on single-star vector assistance, characterized in that: Includes the following steps: Step 1: The angular velocity and acceleration information output by the MEMS inertial measurement unit are processed by the strapdown inertial navigation algorithm. The attitude of the inertial measurement unit is updated using the quaternion method. Based on the ENU coordinate system of the northeast sky, the recursive calculation of the carrier's velocity and position is completed to obtain the initial position, velocity, and attitude information of the carrier. X INS =[P INS V INS A INS ] T (1) Among them, X INS This is the initial navigation state of the inertial navigation system. V provides initial latitude, longitude, and altitude information. INS =[v EINS v NINS v UINS ] T Let be the initial velocity along the coordinate axes in the ENU coordinate system. This provides initial roll, pitch, and yaw angle information; Step 2: Receive satellite signals through a satellite receiver, and obtain single-satellite ephemeris data after demodulation, pseudo-code acquisition and tracking, and navigation message decoding; calculate the pseudorange observation ρ from the single satellite to the receiver based on the pseudo-random code in the satellite signal; simultaneously, measure the phase difference of the satellite signal relative to different antennas using an antenna array; perform angle measurement through phase difference calculation to obtain the azimuth observation Azi and elevation observation Ele of the single satellite relative to the receiver. Step 3: Construct the system state equations; The system state equation is: System error state vector These represent 3D position error, 3D velocity error, 3D attitude error, 3-axis accelerometer error, and 3-axis gyroscope error, respectively. The accelerometer error and gyroscope error are modeled as first-order Gaussian Markov processes. W represents the error of the state equation, which is represented by a zero-mean white noise vector. G represents the noise figure matrix, and F represents the state transition matrix. Step 4: Construct the system observation equations; When using single-satellite pseudoranges as the observation measure, the observation equation is: Z = HX + η (4) At the same time, Z = ρ - ρ INS It is the difference between the observed pseudorange of a single star and the calculated pseudorange, where ρ represents the measured pseudorange of the single star. INS The pseudorange value is calculated from the positioning results of the inertial navigation system, and η is the observation noise; in Formula 4, H is the pseudorange observation Jacobian matrix, and f represents the functional relationship between latitude, longitude, altitude, and distance. The symbol is for partial differentials; When using the azimuth and elevation angles of a single satellite as observations, the system observation equations are: at the same time Azi is the difference between the observed angle of a single star and the calculated angle. Azi represents the observed azimuth angle of a single star. INS Ele represents the azimuth angle calculated from the inertial navigation positioning results, and Ele represents the elevation angle observation of a single satellite. INS This represents the pitch angle result calculated based on the inertial navigation positioning result. In Formula 5, H is the angle observation Jacobian matrix, and f1 and f2 represent the functional relationships between latitude, longitude, and altitude and azimuth and pitch angles, respectively. Step 5: Set the number of particles N p An initial particle swarm is generated based on the initial calculation results of position, velocity, and attitude from step 1. State vector Random noise N(0,Q) is superimposed on the system state vector X from step 3. This random noise is represented by a zero-mean white noise vector, consistent with the IMU error model; initial weights... Ensure uniform particle distribution; Step 6: Fuse the system's state equation and observation equation using particle filtering to estimate the state error of the inertial navigation system; for each particle i, perform nonlinear prediction based on the system state equation from Step 3, where i = 1, 2, ..., N p ; in, Let f(·) be the predicted state vector of particle i at time k, and let f(·) be the nonlinear state transition function. The process noise at time k-1 is modeled as zero-mean Gaussian white noise; when the observation is a pseudorange, the observation model in formula (4) is used to predict the particle state. Calculate pseudorange measurement value in, To predict the pseudorange, f(·) is a nonlinear function that maps the state vector to the pseudorange. The predicted state of the i-th particle at time k is given; and compared with the measured pseudorange in step 2, the pseudorange observation residual is calculated: When the observation is an angle, the observation model based on formula (5) is used to predict the particle state. Azimuth measurement value and pitch angle measurement value Where f1(·) and f2(·) are nonlinear functions mapping the state vector to the azimuth and elevation angles, respectively; and compared with the measured azimuth and elevation angles in step 2, the angle observation residuals are calculated: in, α represents the azimuth observation residual. k This represents the measured azimuth at time k. β represents the observation residual of the pitch angle. k The measured pitch angle at time k is used to update the particle weights based on the observation difference. Where ∝ represents a proportional relationship, used to simplify the expression for calculating particle weights. z represents the predicted state vector of particle i at time k based on the state at time k-1. k Let h(·) be the pseudorange or angle observation value at time k, h(·) be the corresponding observation function, and R be the observation noise covariance matrix; normalize the weights of all particles. in The normalized ion weights are represented; the optimal estimated state error of the system state is obtained by weighted summation. Based on normalized weights Resampling is performed to avoid particle degradation; a systematic resampling method is used to generate a new particle swarm. Ensure that the particle distribution is concentrated in the high likelihood region; Step 7: Estimating the state error from the particle filter The error is decomposed into position error δP, velocity error δV, and attitude error δA. The error terms are fed back to the inertial navigation system, and the position, velocity, and attitude output by the inertial navigation system are corrected in real time through closed-loop feedback. Position correction value P = P INS -δP, velocity correction value V = V INS -δV, attitude correction value A = A INS -δA; Where P represents the actual location result, P INS V represents the position result calculated by the inertial navigation system, and V represents the actual velocity result. INS A represents the velocity result calculated by the inertial navigation system, and A represents the actual attitude result. INS This represents the attitude result calculated by the inertial navigation system. The navigation solution is continuously iterated and optimized, and the final navigation result is output after the stopping condition is met.

2. The inertial navigation method based on single-star vector assistance as described in claim 1, characterized in that, In step 3: in: Where R represents the Earth's average radius, M represents the radius of curvature of the meridian, N represents the radius of curvature of the geocentric circle, γ represents the Earth's gravitational acceleration, and ω... ie Represents the Earth's angular velocity of rotation. Indicates the rate of change of dimension. f represents the rate of change of longitude. U Indicates the force of the heavens, f E Indicates the eastward force, f N Indicates northward comparison, τ represents the direction cosine matrix. ax τ represents the relevant time constant of the x-axis accelerometer error. ay τ represents the relevant time constant of the y-axis accelerometer error. az τ represents the relevant time constant of the z-axis accelerometer error. gx τ represents the relevant time constant of the x-axis gyroscope error. gy τ represents the relevant time constant of the y-axis gyroscope error. gz The time constant represents the relevant time constant of the z-axis gyroscope error.

3. The inertial navigation method based on single-star vector assistance as described in claim 1, characterized in that, In step 4, the pseudorange observation Jacobian matrix in formula 4 is:

4. The inertial navigation method based on single-star vector assistance as described in claim 1, characterized in that, In step 4, the Jacobian matrix for angular observation in formula 5 is:

5. The inertial navigation method based on single-star vector assistance as described in claim 1, characterized in that, The stopping condition in step 7 is if P < 0.1m for three consecutive iterations or if the maximum number of iterations of 100 is reached.

Images