A non-integrity constraint measurement noise optimization method

Abstract

This invention discloses a non-integrity constraint measurement noise optimization method, comprising: Step 1: estimating vehicle motion information and mounting angle; Step 2: modeling the vehicle's lateral acceleration in real time based on a two-degree-of-freedom vehicle model; Step 3: estimating the vehicle's lateral velocity component considering tire side-slip effect; Step 4: adaptively adjusting NHC measurement noise. This invention first estimates the vehicle motion information and mounting angle, then models the lateral acceleration in real time using a two-degree-of-freedom vehicle model, and theoretically derives the lateral velocity component under the tire side-slip effect. Finally, it constructs a nonlinear coupling function between forward velocity and lateral acceleration, and introduces a variance-constrained mechanism to achieve dynamic adaptive adjustment of constraint weights, effectively improving the heading angle accuracy of the integrated system. This invention is particularly significant in improving the integrated navigation performance based on low-cost GNSS / SINS systems. In continuous turning scenarios, compared with the traditional NHC fixed measurement noise method, the heading angle accuracy of the integrated navigation system can be improved by more than 20%.

Description

Technical Field

[0001] This invention discloses a non-integrity constraint measurement noise optimization method, belonging to the field of navigation and positioning technology, particularly the robust and adaptive measurement noise algorithm. Background Technology

[0002] Non-holonomic constraints (NHCs) refer to the condition where a land vehicle's lateral and vertical velocities are zero without jumping, skidding, or bouncing. In vehicle-mounted integrated navigation systems, GNSS (Global Navigation Satellite System) signals are easily affected by obstructed environments (such as overpasses, urban canyons, and tunnels), and relying solely on IMU (Inertial Measurement Unit) positioning may lead to accumulated or even divergent positioning errors. To improve positioning accuracy and reliability, NHCs can be introduced as virtual observations to suppress IMU error divergence.

[0003] The effectiveness of NHC observations depends on the reasonable setting of their measurement noise, which needs to be accurately represented in the measurement noise matrix R of the filtering algorithm.

[0004] In existing studies, the measurement noise variance of NHC is usually determined by experience using a fixed value. Currently, many scholars have proposed methods to correct the measurement noise variance of NHC, mainly including: directly determining the measurement noise variance based on experience; identifying the vehicle's motion state based on its speed and angular velocity information, setting different fixed values ​​for the measurement noise variance for different motion states, or designing adjustment functions to dynamically correct the measurement noise variance; correcting the measurement noise based on the correlation between the vehicle's motion parameters and the NHC observation error; and using neural network models to learn the complex mapping relationship between vehicle motion information and the measurement noise variance of NHC, thereby correcting the measurement noise variance.

[0005] However, the above methods all have certain limitations: First, using empirical fixed values ​​as the measurement noise variance of NHC is difficult to accurately reflect the actual uncertainty of vehicle motion, and may even lead to filter divergence in severe cases; Second, the adjustment method based on motion state classification is highly dependent on the accuracy of state prediction, and its noise adaptation mechanism adopts a discrete piecewise function design, which has obvious discontinuity when switching states; At the same time, the correlation-based method lacks rigorous theoretical derivation and is difficult to accurately characterize the complex nonlinear dynamic coupling relationship between vehicle lateral velocity and motion state; Although the neural network method can accurately predict the measurement noise variance, its training data acquisition cost is high, and the model inference latency is difficult to meet the real-time requirements of the vehicle embedded platform.

[0006] Existing methods for correcting non-integrity constraint (NHC) measurement noise still have room for improvement in terms of further enhancing the heading accuracy and robustness of integrated navigation systems. Summary of the Invention

[0007] The purpose of this invention is to address the shortcomings of the prior art by providing a non-integrity constraint measurement noise optimization method. This method has broad scenario adaptability, is not limited by the accuracy of motion state prediction, and effectively improves the heading accuracy of the integrated navigation system while ensuring real-time performance.

[0008] This invention provides a method for optimizing measurement noise under non-integrity constraints, the method comprising the following steps:

[0009] Step 1: Estimate vehicle motion information and mounting angle;

[0010] Step 2: Based on the two-degree-of-freedom model of the vehicle, model the lateral acceleration of the vehicle in real time;

[0011] Step 3: Considering the tire side slip effect, estimate the lateral velocity component of the vehicle;

[0012] Step 4: Adaptive adjustment of NHC measurement noise. Based on a simplified vehicle model, a nonlinear coupling function based on forward velocity and lateral acceleration is constructed to dynamically adjust the NHC lateral measurement noise and accurately characterize the lateral velocity noise characteristics. At the same time, based on the variance constraint adaptive adjustment mechanism, the dynamic optimization of NHC constraint weights is achieved while ensuring the stability of the filter.

[0013] As a further improvement of the present invention, in step 1, the vehicle kinematic parameters are estimated in real time using the observation data of the vehicle-mounted strapdown inertial navigation system (SINS) and the global navigation satellite system (GNSS), and the sensor pitch and heading installation angles are calculated simultaneously.

[0014] As a further improvement of the present invention, in step 2, the lateral acceleration of the vehicle is modeled in real time using the following formula:

[0015]

[0016]

[0017] in, This refers to lateral acceleration, that is, the acceleration during lateral motion. and the centripetal acceleration of yaw motion The sum of It can be decomposed into forward velocity and rate of change of heading angle The product of these factors is considered only in the case of normal steady-state turning of the vehicle, therefore lateral acceleration is ignored. The impact; This refers to the yaw rate; The forward speed of the vehicle is defined in the vehicle system. , and These are the vehicle's eastward, northward, and upward speeds, defined under navigation; , These are the pitch installation angle and the yaw installation angle, respectively.

[0018] As a further improvement of the present invention, in step 3, the tire side slip effect is considered, combined with the vehicle lateral acceleration modeled in step 2. The following formula is used to calculate the vehicle's slip angle. Make an estimate:

[0019]

[0020]

[0021]

[0022] In the formula, This is the lateral force of the tire. To measure the eccentric stiffness of the tires, the lateral acceleration should not exceed [a certain value] during normal vehicle operation. Side slip angle not exceeding It can be assumed that the sideslip angle and the sideslip force have a linear relationship. This is the lateral force of the tire, used to balance the centrifugal force. For vehicle quality.

[0023] Transform the above formula and... By making a second-order approximation, the sideslip angle can be obtained. The expression:

[0024]

[0025] Modeling to obtain the sideslip angle Then, combine the forward velocity output by SINS Use the following formula to calculate the lateral speed of the vehicle. Make an estimate:

[0026]

[0027] From the above formula, we can see that: The size is simultaneously affected by the variable and The combined effects of these factors.

[0028] As a further improvement of the present invention, in step 4, considering that the turning of the vehicle will be more complex during actual driving, the tire physical characteristics, vertical load, aerodynamics, and road surface roughness will all affect the tire lateral slip characteristics, making it difficult to accurately model the lateral velocity. Therefore, while maintaining the core physical relationship, dynamic optimization of the lateral velocity measurement noise is achieved. The following formula is used to define an adaptive noise adjustment function based on forward velocity and lateral acceleration:

[0029]

[0030] In the formula, Noise was measured to measure the lateral velocity of the vehicle. For vehicle parameters, size is , This is a scaling factor, set according to the actual situation.

[0031] As a further improvement of the present invention, in step 4, an adaptive adjustment mechanism based on variance constraints is used to dynamically optimize the NHC constraint weights while ensuring the stability of the filtering process. The variance constraint is performed using the following formula:

[0032]

[0033]

[0034] In the formula, To determine the final variance of the lateral velocity measurement, The variance adjustment for measuring residuals. , These represent the maximum and minimum variances of the lateral velocity, set empirically. is the forgetting factor, initially set to 1, and decays according to a certain pattern after each update of the quantity. b is the decay factor, which is set to 0.5 based on experience.

[0035] An electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, implements the aforementioned non-integrity constraint measurement noise optimization method.

[0036] A computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the aforementioned method for optimizing non-integrity constraint measurement noise.

[0037] The beneficial effects of this invention are as follows: First, this invention establishes a dynamic model based on a two-degree-of-freedom vehicle to address the tire side-slip effect, and provides a rigorous theoretical derivation of the complex nonlinear dynamic coupling relationship between the vehicle's lateral velocity and motion state. Second, by constructing a nonlinear coupling function between forward velocity and lateral acceleration, adaptive dynamic adjustment of non-holonomic constraint measurement noise is achieved, avoiding the degradation of filtering performance caused by a fixed noise model. This effectively improves the heading estimation accuracy and robustness of the integrated navigation system in different driving scenarios while ensuring the system's real-time performance. Furthermore, this method does not rely on high-precision motion state prediction, and has broader scenario adaptability and practicality. This invention is particularly significant in improving the performance of integrated navigation based on low-cost GNSS / SINS systems. In continuous turning scenarios, compared to the traditional NHC fixed measurement noise method, the heading angle accuracy of the integrated navigation system can be improved by more than 20%. Attached Figure Description

[0038] Figure 1 This is a flowchart illustrating the present invention.

[0039] Figure 2 This is a comparison chart of the heading angle error of the integrated navigation system of this invention with the heading angle error of integrated navigation systems of other methods. Detailed Implementation

[0040] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the present invention.

[0041] NHC refers to the condition where a land vehicle's lateral and vertical speeds are zero without jumping, skidding, or bouncing.

[0042] Taking the right front-upper (xyz) coordinate system as an example, construct a virtual velocity observation:

[0043]

[0044]

[0045] in, The lateral velocity of the vehicle is indicated by the measurement. This represents the observed vertical velocity of the vehicle.

[0046] NHC can significantly suppress the accumulation of errors in inertial navigation systems and improve the positioning accuracy of integrated navigation systems. This constraint is effective when the vehicle is traveling straight, but when turning, due to the tire slip effect, there is a slip angle between the actual forward speed and the theoretical forward speed, resulting in a velocity component in the lateral direction of the vehicle, that is, the lateral speed is not zero.

[0047] Please refer to Figure 1The non-integrity constraint adaptive measurement noise method of this invention includes steps 1 to 4.

[0048] Step 1: Estimate vehicle motion information and mounting angle;

[0049] The specific method is as follows: using the observation data of the vehicle-mounted strapdown inertial navigation system (SINS) and the global navigation satellite system (GNSS), the adaptive Kalman filter algorithm is used to estimate the vehicle's kinematic parameters in real time, and the sensor pitch and heading installation angles are calculated simultaneously.

[0050] Step 2: Based on the two-degree-of-freedom model of the vehicle, model the lateral acceleration of the vehicle in real time;

[0051] The specific method is as follows:

[0052]

[0053]

[0054] in, This refers to lateral acceleration, that is, the acceleration during lateral motion. and the centripetal acceleration of yaw motion The sum of It can be decomposed into forward velocity and rate of change of heading angle The product of these factors is considered only in the case of normal steady-state turning of the vehicle, therefore lateral acceleration is ignored. The impact; This refers to the yaw rate; The forward speed of the vehicle is defined in the vehicle system. , and These are the vehicle's eastward, northward, and upward speeds, defined under the navigation system; , These are the pitch installation angle and the yaw installation angle, respectively.

[0055] Step 3: Considering the tire side slip effect, estimate the lateral velocity component of the vehicle;

[0056] The specific method is as follows:

[0057]

[0058]

[0059]

[0060] In the formula, This is the lateral force of the tire. To measure the eccentric stiffness of the tires, the lateral acceleration should not exceed [a certain value] during normal vehicle operation. Side slip angle not exceeding It can be assumed that the sideslip angle and the sideslip force have a linear relationship. This is the lateral force of the tire, used to balance the centrifugal force. For vehicle quality.

[0061] Transform the above formula and... By making a second-order approximation, the sideslip angle can be obtained. The expression:

[0062]

[0063] Modeling to obtain the sideslip angle Then, combine the forward velocity output by SINS Use the following formula to calculate the lateral speed of the vehicle. Make an estimate:

[0064]

[0065] From the above formula, we can see that: The size is simultaneously affected by the variable and The combined effects of these factors.

[0066] Step 4: Adaptively adjust NHC measurement noise.

[0067] (1) Construct a nonlinear coupling function of forward velocity and lateral acceleration to accurately characterize the lateral velocity noise characteristics and dynamically adjust the lateral measurement noise of NHC.

[0068] The specific method is as follows:

[0069]

[0070] In the formula, Noise was measured to measure the lateral velocity of the vehicle. For vehicle parameters, size is , This is a scaling factor, set according to the actual situation.

[0071] (2) At the same time, an adaptive adjustment mechanism based on variance constraints is adopted to achieve dynamic optimization of NHC constraint weights while ensuring the stability of filtering.

[0072] The specific method is as follows:

[0073]

[0074]

[0075] In the formula, To determine the final variance of the lateral velocity measurement, The variance adjustment for measuring residuals. , These represent the maximum and minimum variances of the lateral velocity, set empirically. is the forgetting factor, initially set to 1, and decays according to a certain pattern after each update of the quantity. b is the decay factor, which is set to 0.5 based on experience.

[0076] Finally, the effectiveness of the proposed method was verified through real-vehicle testing:

[0077] The SINS gyroscope has a zero-bias stability and a random walk of 0.5. and 0.15 The zero-bias stability and random walk of the accelerometer are 50. and 60 The IMU output frequency is 125Hz; the GNSS output frequency is 1Hz. A comparison is made between the filtering methods based on fixed NHC measurement noise (FNHC-KF), correlation-adjusted NHC measurement noise (CNHC-KF), and the proposed filtering method based on dynamic coupling-adjusted NHC measurement noise (DNHC-KF).

[0078] like Figure 2 The heading angle error curves of several algorithms are presented. FNHC-KF is most affected by vehicle turning, exhibiting significant error fluctuations. CNHC-KF, due to its failure to accurately establish the nonlinear coupling relationship between lateral velocity and vehicle dynamic parameters, still suffers from insufficient overall accuracy. DNHC-KF has the smallest heading error compared to the other two algorithms, showing improved estimation accuracy. The root mean square error of the heading angle for the three methods is 0.152203. (FNHC-KF), 0.133048 (CNHC-KF) and 0.109722 (DNHC-KF) Compared to the previous two algorithms, the DNHC-KF algorithm improves the heading angle accuracy by 27.91% and 17.53%, respectively.

[0079] Those skilled in the art should understand that the above embodiments are merely preferred implementations of the technical solutions of the present invention and are not intended to limit the scope of protection of the present invention. Any equivalent substitutions, adaptive modifications, or optimizations made based on the technical solutions of the present invention without departing from the design concept and core principles of the present invention should be considered to fall within the scope of protection of the claims of the present invention.

Claims

1. A method for optimizing measurement noise under non-integrity constraints, characterized in that, Includes the following steps: Step 1: Estimate vehicle motion information and mounting angle; Step 2: Based on the two-degree-of-freedom model of the vehicle, model the lateral acceleration of the vehicle in real time; Step 3: Considering the tire side slip effect, estimate the lateral velocity component of the vehicle; Step 4: Adaptively adjust the NHC measurement noise, construct a nonlinear coupling function of forward velocity and lateral acceleration to accurately characterize the lateral velocity noise characteristics, and dynamically adjust the NHC lateral measurement noise; at the same time, adopt an adaptive adjustment mechanism based on variance constraints to achieve dynamic optimization of NHC constraint weights while ensuring filter stability. In step 3, the tire side slip effect is considered, combined with the vehicle lateral acceleration modeled in step 2. The following formula is used to calculate the vehicle's slip angle. Make an estimate: In the formula, This is the lateral force of the tire. To measure the eccentric stiffness of the tires, the lateral acceleration should not exceed [a certain value] during normal vehicle operation. Side slip angle not exceeding It is assumed that the sideslip angle and sideslip force have a linear relationship. This is the lateral force of the tire, used to balance the centrifugal force. For vehicle quality, Transform the above formula and... By making a second-order approximation, the sideslip angle is obtained. The expression: Modeling to obtain the sideslip angle Then, combine the forward velocity output by SINS Use the following formula to calculate the lateral speed of the vehicle. Make an estimate: From the above formula, we can see that: The size is simultaneously affected by the variable and The combined effects; In step 4, considering that vehicle cornering is more complex during actual driving, tire physical characteristics, vertical load, aerodynamics, and road surface roughness all affect tire lateral slip characteristics, making it difficult to accurately model lateral velocity. Therefore, while maintaining the core physical relationships, dynamic optimization of lateral velocity measurement noise is achieved. The following formula is used to define an adaptive noise adjustment function based on forward velocity and lateral acceleration: In the formula, Forward velocity, Noise was measured to measure the lateral velocity of the vehicle. For vehicle parameters, size is , This is a scaling factor, set according to the actual situation; In step 4, an adaptive adjustment mechanism based on variance constraints is used to dynamically optimize the NHC constraint weights while ensuring filter stability. Variance constraints are applied using the following formula: In the formula, To determine the final variance of the lateral velocity measurement, The variance adjustment for measuring residuals. , These represent the maximum and minimum variances of the lateral velocity, set empirically. is the forgetting factor, and b is the decay factor.

2. The method for optimizing non-integrity constraint measurement noise according to claim 1, characterized in that, Step 1 includes: using observation data from the vehicle-mounted strapdown inertial navigation system (SINS) and the global navigation satellite system (GNSS), an adaptive Kalman filter algorithm is used to estimate the vehicle's kinematic parameters in real time, and the sensor pitch and heading installation angles are calculated simultaneously.

3. The method for optimizing non-integrity constraint measurement noise according to claim 2, characterized in that, In step 2, the vehicle's lateral acceleration is modeled in real time using the following formula: in, This refers to lateral acceleration, that is, the acceleration during lateral motion. and the centripetal acceleration of yaw motion The sum of It can be decomposed into forward velocity and rate of change of heading angle The product of these terms only considers the normal steady-state turning of the vehicle and ignores lateral acceleration. The impact; This refers to the yaw rate; The forward speed of the vehicle is defined in the vehicle system. , and These are the vehicle's eastward, northward, and upward speeds, defined under the navigation system; , These are the pitch installation angle and the yaw installation angle, respectively.

4. The method for optimizing non-integrity constraint measurement noise as described in claim 3, characterized in that, The initial value is set to 1, and it decays according to a certain pattern after each update of the quantity side, with b set to 0.

5.

5. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements a non-integrity constraint measurement noise optimization method as described in any one of claims 1 to 4 above.

6. A computer-readable storage medium storing computer instructions thereon, characterized in that, When executed by the processor, the computer instructions implement a non-integrity constraint measurement noise optimization method as described in any one of claims 1-4.

Images