A vehicle fusion positioning method and system based on GMM assistance

Abstract

The application relates to a GMM-assisted vehicle fusion positioning method and system, which comprises the following steps: calculating an IMU pre-integration term according to the angular velocity and acceleration measurement information of an IMU; calculating a dynamics pre-integration term according to the speed measurement information of a wheel speed sensor and the angular velocity measurement information of the IMU in combination with a two-degree-of-freedom vehicle model; constructing an IMU factor and a dynamics factor by using the IMU pre-integration term and the dynamics pre-integration term after obtaining a GNSS measurement signal; constructing a pseudorange factor according to the original observation information of a GNSS receiver in combination with system states; constructing a clock drift factor based on the clock error of the GNSS receiver; constructing a factor graph by combining the constructed factors, wherein the noise of the IMU factor, the dynamics factor and the clock drift factor is Gaussian modeling, and the noise of the pseudorange factor is GMM modeling; and optimizing the factor graph to estimate the positioning information of the vehicle. Compared with the prior art, the application can effectively suppress the influence of abnormal GNSS measurement on positioning, and realizes low-cost and robust high-precision positioning.

Description

Technical Field

[0001] This invention relates to the field of autonomous vehicle positioning technology, and in particular to a vehicle fusion positioning method and system based on GMM-assisted positioning. Background Technology

[0002] The three core technologies of autonomous driving are perception, planning, and control. The perception layer, composed of localization, detection, and recognition algorithms, is used to understand the vehicle's location and driving environment. Accurate and robust global localization is crucial for achieving autonomous driving and forms the basis for planning, decision-making, and motion control. Currently, vehicle localization largely relies on Global Navigation Satellite Systems (GNSS), Inertial Measurement Units (IMUs), and wheel speed sensors, which are widely used to provide vehicle positioning information due to their relatively low cost.

[0003] Since IMU and wheel speed-based odometer calculations are relative positioning methods, the final positioning accuracy is determined by the accuracy of the GNSS absolute positioning algorithm. GNSS achieves good positioning accuracy in open environments; however, in urban canyons, tunnels, and tree-lined areas, multipath effects and non-line-of-sight (NLOS) reception lead to numerous anomalous measurements. Common solutions to this problem include:

[0004] 1. Hardware design methods such as choke loops, dual-polarized antennas, and antenna arrays can be used to reduce signal interference and improve GNSS accuracy, but this method is costly.

[0005] 2. By using GNSS measurement information such as signal strength and satellite elevation angle, or based on sensors such as lidar and cameras, satellite visibility can be classified to exclude or correct NLOS reception and improve GNSS positioning performance. However, the performance of such methods is highly dependent on the quality of satellite visibility classification, and often only some NLOS receptions can be detected, resulting in limited accuracy.

[0006] Furthermore, most existing GNSS / IMU / wheel speed fusion positioning methods are based solely on the assumption of vehicle planar motion, ignoring the vehicle's pitch and roll motion. These methods are not suitable for vehicle positioning in rugged terrain and cannot guarantee positioning accuracy. Summary of the Invention

[0007] The purpose of this invention is to overcome the shortcomings of the existing technology by providing a vehicle fusion positioning method and system based on GMM (Gaussian Mixed Model) assistance, which can effectively suppress the influence of abnormal GNSS measurements on positioning and achieve high-precision positioning at low cost.

[0008] The objective of this invention can be achieved through the following technical solution: a vehicle fusion positioning method based on GMM-assisted positioning, comprising the following steps:

[0009] S1. Based on the angular velocity and acceleration measurement information of the IMU, calculate the pre-integral terms of vehicle position, velocity and attitude based on the IMU, that is, obtain the IMU pre-integral terms;

[0010] S2. Based on the speed measurement information from the wheel speed sensor and the three-axis angular velocity measurement information from the gyroscope in the IMU, and combined with the two-degree-of-freedom vehicle model, calculate the pre-integral term of the vehicle position based on vehicle dynamics, thus obtaining the dynamics pre-integral term.

[0011] S3. After obtaining the GNSS measurement signal, construct the IMU factor and dynamic factor using the IMU pre-integration term and the dynamic pre-integration term;

[0012] Based on the raw observation information from the GNSS receiver and combined with the system status, a pseudorange factor is constructed.

[0013] A clock drift factor is constructed based on the clock error of the GNSS receiver.

[0014] S4. Combine the IMU factor, kinetic factor, pseudorange factor and clock drift factor to construct a factor graph. The noise of the IMU factor, kinetic factor and clock drift factor is modeled by Gaussian, and the noise of the pseudorange factor is modeled by GMM.

[0015] The vehicle's location information was then estimated by optimizing the factor graph.

[0016] Furthermore, the specific process of step S1 is as follows:

[0017] Obtain the acceleration measurement value a of the IMU in the carrier coordinate system b at the IMU measurement time t. t Angular velocity measurement value ω t And the acceleration measurement value a at the next IMU measurement time t+1. t+1 Angular velocity measurement value ω t+1 Let the time interval between the two IMU measurements be δt. Based on the median integration method, the pre-integral term within the IMU sampling interval is calculated as follows:

[0018]

[0019]

[0020]

[0021] in, For the position pre-integration term, For the velocity pre-integral term, For the attitude pre-integration term, b a b ω These represent the zero bias of the accelerometer and the zero bias of the gyroscope, respectively. The subscripts t and t+1 indicate the IMU measurement time. Let be the average IMU acceleration at time t. Let be the average angular velocity of the IMU at time t;

[0022] Sampling interval [t] of GNSS measurement information k ,t k+1 By pre-integrating all IMU measurements within the range, [t] can be obtained. k ,t k+1 The total IMU pre-integration term.

[0023] Furthermore, step S2 specifically includes the following steps:

[0024] S21. Establish a two-degree-of-freedom model of the vehicle to calculate the velocity in the vehicle coordinate system;

[0025] S22. Based on the velocity in the vehicle coordinate system and the three-axis angular velocity measurement information from the gyroscope in the IMU, using the median integration method, considering adjacent wheel speed measurement times [t, t+1], and the time interval between the two times being δt, the discrete form of the dynamic pre-integration is derived:

[0026]

[0027] in, For the position pre-integration term, For attitude pre-integration term, The velocity in the vehicle coordinate system;

[0028] Sampling interval of GNSS measurement information [t] k ,t k+1 Pre-integrate all wheel speed measurements within ] to obtain [t] k ,t k+1 The overall dynamic pre-integral term.

[0029] Furthermore, the two-degree-of-freedom model of the vehicle in step S21 is specifically as follows:

[0030]

[0031] Where, k f and k r These are the lateral stiffnesses of the front and rear axles, I. z Let l be the moment of inertia of yaw rotation. f and l r These represent the distances from the center of gravity to the front and rear axles, respectively; m is the total vehicle mass; and α is the equivalent front wheel steering angle.

[0032] Considering the vehicle's steady-state steering characteristics: Then the sideslip angle β t and yaw rate ω r,t for:

[0033]

[0034] Where, K = m(l f k f -l r k r ) / l 2 k f k r As a stabilizing factor, v t α t These are the wheel speed and front wheel angle measured at time t, respectively. Based on the transmission ratio i between the front wheel and the steering wheel, α can be obtained. t =δα / i, where δα is the steering wheel angle, thus obtaining the velocity in the vehicle coordinate system. and angular velocity for:

[0035]

[0036] Furthermore, the IMU factor constructed in step S3 is specifically as follows:

[0037]

[0038] The kinetic factors are specifically:

[0039]

[0040] Where X is the state variable to be estimated, and optimization is performed based on a sliding window approach. The state X within the window is:

[0041]

[0042] n is the size of the sliding window, and the state variables include the position of the carrier coordinate system relative to the ENU coordinate system. speed attitude and accelerometer zero bias Gyroscope zero bias Receiver clock error δti Receiver clock drift rate

[0043] [t] k ,t k+1 Observations within the time interval Represents positional error. Represents speed error, The rotational error is represented by Euler angles, [·] xyz This indicates a computational operation to obtain the imaginary part of a quaternion.

[0044] Furthermore, the specific process of constructing the pseudo-range factor in step S3 is as follows:

[0045] The GNSS receiver acquires satellite ephemeris data and pseudorange observation data at time t. k From the observation data, satellite s is obtained based on ephemeris data. j Location Satellite clock bias Atmospheric delay δρ kn,k ionospheric delay δρ kp,k Receiver clock error δt k Distance measurement errors caused by the Sagnac effect of Earth's rotation Calculated by the following formula:

[0046]

[0047] For t k The original observation data of the j-th satellite at time j, considering the ranging error, is modeled as follows:

[0048]

[0049] Where, ω earth c is the Earth's rotational angular velocity, and c is the speed of light. These are the coordinates of the GNSS receiver in the ECEF coordinate system, and the set state variables. The relationship between them is:

[0050]

[0051] in, It is the transformation matrix between the ECEF coordinate system and the ENU coordinate system. It is the arm that runs from the IMU center to the GNSS antenna phase center. It is the rotation matrix between the carrier coordinate system and the ENU coordinate system, and it is a state variable;

[0052] Therefore, for t kThe residuals from the raw observation data of the j-th satellite at time j, which link the system state and pseudorange correlation measurements, are:

[0053]

[0054] Among them, Z ρk It is a pseudorange measurement value. This is the constructed pseudo-distance factor.

[0055] Furthermore, the specific process of constructing the clock drift factor in step S3 is as follows:

[0056] The clock error of the GNSS receiver is modeled using the Constant Clock Error Drift (CCED) model, and the clock drift factor is constructed as follows:

[0057]

[0058] Furthermore, the specific process of modeling the noise of the pseudo-range factor in step S4 using GMM is as follows: taking the pseudo-range residual sequence within the sliding window as input, estimating the GMM parameters of the pseudo-range factor based on EM (Expectation-Maximization algorithm), and then using the estimated GMM parameters as the noise model of the pseudo-range factor in factor graph optimization.

[0059] Furthermore, step S4 specifically includes the following steps:

[0060] S41. GMM Parameter Estimation: Using the pseudorange residual sequence within the sliding window as input, the GMM parameters of the pseudorange factor are estimated based on EM, yielding:

[0061]

[0062] Where o is the residual sequence, M is the total number of residuals, and e k Let H be the pseudorange residual, θ be the latent variable, θ be the parameters of the GMM to be estimated, N be the number of Gaussian components in the GMM, and ω be the pseudorange residual. j It is the weight of the j-th Gaussian component, μ j It is the mean of the j-th Gaussian component, Σ j It is the variance of the j-th Gaussian component;

[0063] Design E-step: Estimate latent variables H and α based on initial guesses of θ. kj e k The probability of belonging to the j-th Gaussian component:

[0064]

[0065]

[0066] Design M-step: Calculate θ based on the estimated H, and update the GMM parameters:

[0067]

[0068] The E-step and M-step are executed iteratively until the maximum number of iterations or the preset convergence condition is reached. At this point, the estimation of the GMM parameters is completed.

[0069] S42. Using the estimated GMM parameters as a noise model for the pseudo-range factor in factor graph optimization, the general formula for applying GMM to factor optimization is obtained as follows:

[0070]

[0071]

[0072]

[0073] Where, γ s It is a normalization constant used to keep the negative log-likelihood positive;

[0074] S43. Factor Graph Optimization Solution: Combine the pseudorange factor related terms from step S42 with the IMU factor (noise follows a Gaussian distribution), kinetic factor, and clock drift factor to construct a factor graph. Then, use the Levenberg-Marquardt algorithm in Ceres Solver to optimize and solve the factor graph, obtaining the estimated vehicle position, velocity, and attitude information.

[0075]

[0076] Where ∑IMU, ∑Dynamics, and ∑CCED are the standard deviations of the IMU factor, dynamics factor, and clock drift factor, respectively, and the last term is the pseudorange factor related term.

[0077] A vehicle fusion positioning system based on GMM-assisted positioning includes an input module, a pre-integration module, a GMM parameter estimation module, and a factor graph optimization module. The input module is connected to the pre-integration module and the GMM parameter estimation module, respectively. The pre-integration module is connected to the GMM parameter estimation module and the factor graph optimization module, respectively. The GMM parameter estimation module is connected to the factor graph optimization module. The input module is used to acquire measurement information output from an IMU, a wheel speed sensor, and a GNSS receiver, respectively, and transmit the IMU measurement information and the wheel speed sensor measurement information to the pre-integration module, and transmit the GNSS measurement information to the GMM parameter estimation module.

[0078] The pre-integration module is used to calculate the IMU pre-integration term and the dynamic pre-integration term, and transmit the IMU pre-integration term and the dynamic pre-integration term to the factor graph optimization module and the dynamic pre-integration term to the GMM parameter estimation module;

[0079] The GMM parameter estimation module uses GMM modeling, takes the pseudo-range residual sequence within the sliding window as input, and estimates the GMM parameters of the pseudo-range factor based on the expectation-maximization algorithm.

[0080] The factor graph optimization module constructs IMU factors and dynamic factors using IMU pre-integration terms and dynamic pre-integration terms.

[0081] To address the clock error in GNSS receivers, a clock drift factor is constructed.

[0082] A factor map is constructed by combining the IMU factor, kinetic factor, pseudorange factor, and clock drift factor.

[0083] The vehicle positioning information is then estimated by optimizing the factor graph.

[0084] Compared with the prior art, the present invention has the following advantages:

[0085] This invention first calculates the IMU pre-integration term based on the angular velocity and acceleration measurement information of the IMU; then, based on the velocity measurement information of the wheel speed sensor and the angular velocity measurement information of the IMU, and based on the vehicle's two-degree-of-freedom model, it calculates the dynamic pre-integration term; after obtaining the GNSS measurement signal, on the one hand, it constructs IMU factors and dynamic factors using the IMU pre-integration term and the dynamic pre-integration term, constructs a pseudorange factor based on the raw observation information of the GNSS receiver, and constructs a clock drift factor for receiver clock errors; on the other hand, it constructs a factor graph by combining the IMU factor, dynamic factor, pseudorange factor, and clock drift factor, and estimates the vehicle positioning information by optimizing and solving the factor graph. This achieves a GNSS / IMU / wheel speed tightly coupled robust positioning scheme, which can effectively suppress the adverse effects of abnormal GNSS measurements on vehicle positioning, and improves the accuracy of vehicle positioning at low cost without the need for hardware facilities to mitigate signal interference.

[0086] Second, when calculating the dynamic pre-integral term in this invention, considering that vehicles often do not move on an approximate plane, the effects of pitch and roll motions cannot be ignored. However, this information is not available in the vehicle's two-degree-of-freedom dynamic model. Therefore, angular velocity information needs to be derived by utilizing the three-axis angular velocity measurements from the gyroscope in the IMU, combined with the velocity in the vehicle coordinate system obtained from the two-degree-of-freedom model. This ensures that the calculated dynamic pre-integral term closely matches the actual vehicle motion and is suitable for vehicle positioning in rugged terrain.

[0087] Third, in constructing the factor graph, the noise of the IMU factor, dynamics factor, and clock drift factor is modeled using Gaussian methods, while the noise of the pseudorange factor is modeled using a Gaussian model (GMM). The pseudorange residual sequence within the sliding window is used as input, and the GMM parameters of the pseudorange factor are estimated based on the expectation-maximization (EM) algorithm. These estimated GMM parameters are then used as the noise model for the pseudorange factor in factor graph optimization. This fully ensures the reliability of the factor graph and enables accurate estimation of vehicle position, velocity, attitude, and other positioning information. Attached Figure Description

[0088] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0089] Figure 2 This is a schematic diagram of the linear two-degree-of-freedom vehicle dynamics model in the embodiment;

[0090] Figure 3 This is a schematic diagram of the factor graph structure constructed in the embodiment;

[0091] Figure 4 This is a schematic diagram of the system structure built in the embodiment. Detailed Implementation

[0092] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0093] Example

[0094] like Figure 1 As shown, a vehicle fusion localization method based on GMM-assisted localization includes the following steps:

[0095] S1. Based on the angular velocity and acceleration measurement information of the IMU, calculate the pre-integral terms of vehicle position, velocity and attitude based on the IMU, that is, obtain the IMU pre-integral terms;

[0096] S2. Based on the speed measurement information from the wheel speed sensor and the three-axis angular velocity measurement information from the gyroscope in the IMU, and combined with the two-degree-of-freedom vehicle model, calculate the pre-integral term of the vehicle position based on vehicle dynamics, thus obtaining the dynamics pre-integral term.

[0097] S3. After obtaining the GNSS measurement signal, construct the IMU factor and dynamic factor using the IMU pre-integration term and the dynamic pre-integration term;

[0098] Based on the raw observation information from the GNSS receiver and combined with the system status, a pseudorange factor is constructed.

[0099] A clock drift factor is constructed based on the clock error of the GNSS receiver.

[0100] S4. Combine the IMU factor, kinetic factor, pseudorange factor and clock drift factor to construct a factor graph. The noise of the IMU factor, kinetic factor and clock drift factor is modeled by Gaussian, and the noise of the pseudorange factor is modeled by GMM.

[0101] The vehicle's location information was then estimated by optimizing the factor graph.

[0102] This embodiment applies the above technical solution, and its main contents include:

[0103] 1. Calculate the pre-integral terms for vehicle position, velocity, and attitude based on the IMU's angular velocity and acceleration measurement information;

[0104] Specifically, in this embodiment, the factor map update period is first selected to be the same as the GNSS measurement period. Since the IMU output frequency (100Hz) is much higher than the GNSS output frequency (1Hz), the sampling interval of the GNSS measurement information is [t]. k ,t k+1 Within this frame, there will be multiple IMU measurements. Considering a specific IMU measurement time t, we obtain the IMU's acceleration measurement value a in the carrier coordinate system b. t and angular velocity measurement ω t The acceleration measurement value at the next IMU measurement time t+1 is a. t+1 The measured angular velocity is ω t+1 The time interval between two IMU samplings is δt. Based on the median integration method, the pre-integral term within the IMU sampling interval is calculated:

[0105]

[0106] in:

[0107]

[0108]

[0109] Sampling interval [t] of GNSS measurement information k ,t k+1 By pre-integrating all IMU measurements within ], we can obtain [t]. k ,t k+1 Total IMU pre-integration terms: Location pre-integration terms Velocity pre-integral term Attitude pre-integration term b a b ω These are the accelerometer and gyroscope zero bias, respectively.

[0110] II. Based on the speed measurement information from the wheel speed sensor and the three-axis angular velocity measurement information from the gyroscope in the IMU, and based on the vehicle's two-degree-of-freedom model (such as...), Figure 2 (As shown), calculate the pre-integral term of the vehicle position based on vehicle dynamics;

[0111] 21) Based on the following two-degree-of-freedom vehicle model:

[0112]

[0113] Where, k f and k r These are the lateral stiffnesses of the front and rear axles, I. z Let l be the moment of inertia of yaw rotation. f and l r These represent the distances from the center of gravity to the front and rear axles, respectively, where m is the vehicle mass and α is the equivalent front wheel steering angle.

[0114] Considering the vehicle's steady-state steering characteristics: Then the sideslip angle β t and yaw rate ω r,t for:

[0115]

[0116] Wherein, the stability factor K = m(l f k f -l r k r ) / l 2 k f k r v t α t These are the wheel speed and front wheel angle measured at time t, respectively. Based on the transmission ratio i between the front wheel and the steering wheel, α can be obtained. t =δα / i, where δα is the steering wheel angle, to obtain the velocity in the vehicle coordinate system. and angular velocity for:

[0117]

[0118] 22) Furthermore, considering that the actual vehicle does not move on an approximate plane, the effects of pitch and roll motions cannot be ignored. Since this information is not available in the vehicle dynamics model, the angular velocity information is not the calculated value from step 21), but rather the three-axis angular velocity measurement information ω from the gyroscope in the IMU is used. t Combined with the velocity in the vehicle coordinate system calculated in step 21), Based on the median integral method, considering adjacent wheel speed measurement times [t, t+1], and the time interval between the two times being δt, the discrete form of the dynamic pre-integration is derived:

[0119]

[0120] For [t] k ,t k+1 By pre-integrating all wheel speed measurements within the time interval, [t] can be obtained. k ,t k+1 Internal dynamic pre-integral term: position pre-integral term Attitude pre-integration term Since zero-bias constraints are not constructed for angular velocity in the dynamic factor, the previous time step t is used instead. k Angular velocity zero bias estimate Correct the angular velocity measurements over the time period.

[0121] 3. When GNSS measurements arrive (i.e. when GNSS measurement signals are obtained), construct IMU factors and dynamic factors using IMU pre-integration terms and dynamic pre-integration terms;

[0122] Based on the raw observation information from the GNSS receiver and combined with the system status, a pseudorange factor is constructed.

[0123] The constant clock error drift (CCED) model is applied to model the clock error of the GNSS receiver, and a clock drift factor is constructed.

[0124] Specifically:

[0125] 31) The state variables set in this embodiment include: the position of the carrier coordinate system relative to the ENU coordinate system. speed attitude Accelerometer zero bias Gyroscope zero bias Receiver clock error δt i and receiver clock drift rate

[0126] Based on the sliding window approach, the state X within the window can be summarized as follows:

[0127]

[0128] Where n is the size of the sliding window.

[0129] 32) In t k+1 When the GNSS measurement arrives at a certain time, based on the IMU pre-integration term: position pre-integration term Velocity pre-integral term Attitude pre-integration term And in conjunction with the system state, construct the IMU factor:

[0130]

[0131] in, [t] k ,t k+1 The observations within the time interval, where X represents the state variable to be estimated. Represents positional error. Represents speed error, The rotational error is represented by Euler angles, [·] xyz This indicates a computational operation to obtain the imaginary part of a quaternion.

[0132] 33) In t k+1 When the GNSS measurement arrives at a certain time, based on the dynamic pre-integral term: position pre-integral term Attitude pre-integration term And in conjunction with the system state, construct dynamic factors:

[0133]

[0134] 34) Since the clock errors of each satellite system are different, the clock errors between constellations need to be considered when using multi-constellation information. Therefore, this embodiment only uses a single GPS satellite system. The GNSS receiver can acquire the satellite's ephemeris data and pseudorange observation data. At time t k From the observational data, satellite s can be obtained based on ephemeris data. j Location Satellite clock bias Atmospheric delay δρ kn,k and ionospheric delay δρ kp,k Additionally, the GNSS receiver clock error δt k Distance measurement errors caused by the Sagnac effect of Earth's rotation It should not be ignored that δt is also important. k It is the maximum component and changes over time, so it must be estimated together with the position of the GNSS receiver. Calculated by the following formula:

[0135]

[0136] For t k The original observation data of the j-th satellite at time j, considering the above ranging error, is modeled as follows:

[0137]

[0138] Where, ω earthc is the Earth's rotational angular velocity, and c is the speed of light. These are the coordinates of the GNSS receiver in the ECEF coordinate system, and their relationship to the set state variables. The relationship between them is:

[0139]

[0140] In the formula, It is the transformation matrix between the ECEF coordinate system and the ENU coordinate system. In this embodiment, it is set to a constant value, which is calculated from the ECEF coordinates at the starting point. It is the arm that runs from the center of the IMU to the phase center of the GNSS antenna; It is the rotation matrix between the carrier coordinate system and the ENU coordinate system, and it is a state variable.

[0141] Therefore, for t k The residuals from the raw observation data of the j-th satellite at time j, which link the system state and pseudorange-related measurements, can be expressed as:

[0142]

[0143] In the formula, It is a pseudorange measurement value. This is the constructed pseudo-distance factor.

[0144] 35) Since the clock error of a GNSS receiver is not a constant value but typically drifts at a certain rate, this scheme applies the Constant Clock Error Drift (CCED) model to model it, and the constructed clock drift factor is:

[0145]

[0146] IV. When GNSS measurements arrive, construct a factor map by combining the IMU factor, dynamic factor, pseudorange factor, and clock drift factor, as follows: Figure 3 As shown, the noise of the IMU factor, dynamic factor, and clock drift factor is all modeled using Gaussian methods; the noise of the pseudorange factor is modeled using a Gaussian mixture model (GMM), with the pseudorange residual sequence within the sliding window as input, and the GMM parameters of the pseudorange factor estimated based on the expectation-maximization algorithm (EM). The estimated GMM parameters are then used as the noise model of the pseudorange factor in factor graph optimization; finally, the vehicle's position, velocity, attitude, and other information are estimated by optimizing and solving the factor graph.

[0147] Specifically:

[0148] 41) GMM parameter estimation: Using the pseudorange residual sequence within the sliding window as input, the GMM parameters of the pseudorange factor are estimated based on the EM algorithm. The implementation steps are as follows:

[0149]

[0150] Where o is the residual sequence, M represents the number of residuals, and e is the pseudo-millimeter residual. k The calculation method is as described in step 34); H is a latent variable; θ is the GMM parameter to be estimated, N is the number of Gaussian components contained in the GMM, and ω j It is the weight of the j-th Gaussian component, μ j It is the mean of the j-th Gaussian component, Σ j It is the variance of the j-th Gaussian component.

[0151] a) E-step: Estimating the latent variable H based on the initial guess of θ. α kj e k The probability of belonging to the j-th Gaussian component:

[0152]

[0153]

[0154] b) M-step: Calculate θ based on the estimated H to update the GMM parameters:

[0155]

[0156] The E-step and M-step processes are performed iteratively until the maximum number of iterations or the set convergence condition is reached. At this point, the estimation of the GMM parameters is complete.

[0157] 42) Using the estimated GMM parameters as the noise model for the pseudorange factor in factor graph optimization, the general formula for applying GMM to factor optimization is obtained as follows:

[0158]

[0159]

[0160]

[0161] 43) Factor Graph Optimization: The pseudorange factor related terms from step 42) are combined with the IMU factor (noise follows a Gaussian distribution), kinetic factor, and clock drift factor to construct a factor graph. Finally, the Levenberg-Marquardt algorithm in Ceres Solver is used to optimize and solve the factor graph, estimating the vehicle's position, velocity, attitude, and other information.

[0162]

[0163] In the formula, ∑IMU, ∑Dynamics, and ∑CCED are the standard deviations of the IMU factor, dynamics factor, and clock drift factor, respectively; the last term is the pseudorange factor related term given in step 42).

[0164] Based on the above method and process, this embodiment builds as follows: Figure 4 The system structure shown includes an input module, a pre-integration module, a GMM parameter estimation module, and a factor graph optimization module. The input module is connected to the pre-integration module and the GMM parameter estimation module, the pre-integration module is connected to the GMM parameter estimation module and the factor graph optimization module, and the GMM parameter estimation module is connected to the factor graph optimization module.

[0165] The input module is used to acquire measurement information output from the IMU, wheel speed sensor and GNSS receiver respectively, and transmit the IMU measurement information and wheel speed sensor measurement information to the pre-integration module and the GNSS measurement information to the GMM parameter estimation module.

[0166] The pre-integration module is used to calculate the IMU pre-integration term and the dynamic pre-integration term, and then transmits the IMU pre-integration term and the dynamic pre-integration term to the factor graph optimization module and the dynamic pre-integration term to the GMM parameter estimation module.

[0167] The GMM parameter estimation module uses GMM modeling, takes the pseudo-range residual sequence within the sliding window as input, and estimates the GMM parameters of the pseudo-range factor based on the expectation-maximization algorithm.

[0168] The factor graph optimization module uses IMU pre-integration terms and dynamic pre-integration terms to construct IMU factors and dynamic factors;

[0169] To address the clock error in GNSS receivers, a clock drift factor is constructed.

[0170] A factor map is constructed by combining the IMU factor, kinetic factor, pseudorange factor, and clock drift factor.

[0171] The vehicle positioning information is then estimated by optimizing the factor graph.

[0172] In summary, this technical solution realizes a robust positioning scheme based on GMM-assisted GNSS / IMU / wheel speed tight coupling, which can effectively suppress the impact of abnormal GNSS measurements on positioning and has the advantages of low cost, robust positioning, and low computational load.

Claims

1. A vehicle fusion localization method based on GMM-assisted localization, characterized in that, Includes the following steps: S1. Based on the angular velocity and acceleration measurement information of the IMU, calculate the pre-integral terms of vehicle position, velocity and attitude based on the IMU, that is, obtain the IMU pre-integral terms; S2. Based on the speed measurement information from the wheel speed sensor and the three-axis angular velocity measurement information from the gyroscope in the IMU, and combined with the two-degree-of-freedom vehicle model, calculate the pre-integral term of the vehicle position based on vehicle dynamics, thus obtaining the dynamics pre-integral term. S3. After obtaining the GNSS measurement signal, construct the IMU factor and dynamic factor using the IMU pre-integration term and the dynamic pre-integration term; Based on the raw observation information from the GNSS receiver and combined with the system status, a pseudorange factor is constructed. A clock drift factor is constructed based on the clock error of the GNSS receiver. S4. Combine the IMU factor, kinetic factor, pseudorange factor and clock drift factor to construct a factor graph. The noise of the IMU factor, kinetic factor and clock drift factor is modeled by Gaussian, and the noise of the pseudorange factor is modeled by GMM. Then, by optimizing the factor graph, the vehicle's location information is estimated. Step S4 specifically includes the following steps: S41. GMM Parameter Estimation: Using the pseudorange residual sequence within the sliding window as input, the GMM parameters of the pseudorange factor are estimated based on EM, yielding: in, It is a residual sequence. The total number of residuals. For pseudo-distance residuals, It is a latent variable. These are the GMM parameters to be estimated. N It is the number of Gaussian components contained in the GMM. It is the first The weights of the Gaussian components, It is the first The mean of the Gaussian components, It is the first The variance of each Gaussian component; Design E-Step: Based on Initial guesses to estimate latent variables H , express Belongs to the The probability of a Gaussian component: Design M-step: Based on the estimated H calculate Complete the GMM parameter update: The E-step and M-step are executed iteratively until the maximum number of iterations or the preset convergence condition is reached. At this point, the estimation of the GMM parameters is completed. S42. Using the estimated GMM parameters as a noise model for the pseudo-range factor in factor graph optimization, the general formula for applying GMM to factor graph optimization is obtained as follows: in, It is a normalization constant used to keep the negative log-likelihood positive; S43. Factor Graph Optimization Solution: Combine the pseudorange factor related terms from step S42 with the IMU factor (noise follows a Gaussian distribution), kinetic factor, and clock drift factor to construct a factor graph. Then, use the Levenberg-Marquardt algorithm in Ceres Solver to optimize and solve the factor graph, obtaining the estimated vehicle position, velocity, and attitude information. in, , , These are the standard deviations of the IMU factor, kinetic factor, and clock drift factor, respectively, and the last term is the pseudorange factor correlation term.

2. The vehicle fusion positioning method based on GMM-assisted positioning according to claim 1, characterized in that, The specific process of step S1 is as follows: Obtain the acceleration measurement value of the IMU in the carrier coordinate system b at the IMU measurement time t. Angular velocity measurement value And the acceleration measurement at the next IMU measurement time t+1 Angular velocity measurement value The time interval between the two IMU measurements is Based on the median integration method, the pre-integral term within the IMU sampling interval is calculated as follows: in, For the position pre-integration term, For the velocity pre-integral term, For attitude pre-integration term, , These are the zero bias of the accelerometer and the zero bias of the gyroscope, respectively. t , t +1 indicates the IMU measurement time. for t The average IMU acceleration at time t. for t The average angular velocity of the IMU at time t; Sampling interval of GNSS measurement information By pre-integrating all IMU measurements, we can obtain... Internal total IMU pre-integration term.

3. The vehicle fusion positioning method based on GMM-assisted positioning according to claim 2, characterized in that, Step S2 specifically includes the following steps: S21. Establish a two-degree-of-freedom model of the vehicle to calculate the velocity in the vehicle coordinate system; S22. Based on the velocity in the vehicle coordinate system and the three-axis angular velocity measurement information from the gyroscope in the IMU, using the median integral method, consider adjacent wheel speed measurement times [t, t+1], where the time interval between the two times is... The discrete form of the pre-integral dynamics is derived as follows: in, For the position pre-integration term, For attitude pre-integration term, The velocity in the vehicle coordinate system; Sampling interval of GNSS measurement information By pre-integrating all wheel speed measurements within the range, we obtain... The overall dynamic pre-integral term.

4. The vehicle fusion positioning method based on GMM-assisted positioning according to claim 3, characterized in that, The two-degree-of-freedom vehicle model in step S21 is specifically as follows: in, and These are the lateral stiffness of the front and rear axles, respectively. For the moment of inertia of yaw rotation, and These represent the distances from the center of mass to the front and rear axles, respectively. For the overall vehicle quality, This is the equivalent front wheel steering angle; Considering the vehicle's steady-state steering characteristics: , Then the sideslip angle and yaw speed for: in, As a stabilizing factor, , They are The wheel speed and front wheel angle are measured at all times, based on the transmission ratio between the front wheels and the steering wheel. , can be obtained ,in The steering wheel angle is used to obtain the velocity in the vehicle coordinate system. and angular velocity for: 。 5. The vehicle fusion positioning method based on GMM-assisted positioning according to claim 4, characterized in that, The IMU factor constructed in step S3 is specifically as follows: The kinetic factors are specifically: in, The state variable to be estimated is optimized using a sliding window approach, where the state within the window... for: It refers to the size of the sliding window, and the state variables include the position of the carrier coordinate system relative to the ENU coordinate system. ,speed ,attitude ; and accelerometer zero bias gyroscope zero bias Receiver clock error Receiver clock drift rate ; express Observations within the time interval Represents positional error. Represents speed error, Representing rotational error in Euler angles. This indicates a computational operation to obtain the imaginary part of a quaternion.

6. The vehicle fusion positioning method based on GMM-assisted positioning according to claim 5, characterized in that, The specific process of constructing the pseudo-range factor in step S3 is as follows: GNSS receivers acquire satellite ephemeris data and pseudorange observation data at time... From the observation data, satellites are obtained based on ephemeris data. Location Satellite clock bias Atmospheric delay Ionospheric delay Receiver clock error Distance measurement errors caused by the Sagnac effect of Earth's rotation , Calculated by the following formula: for Time of the first j The original observation data from the satellites, considering ranging errors, are modeled as follows: in, It is the Earth's rotational angular velocity. It's the speed of light. These are the coordinates of the GNSS receiver in the ECEF coordinate system, and the set state variables. The relationship between them is: in, It is the transformation matrix between the ECEF coordinate system and the ENU coordinate system. It is the arm that runs from the IMU center to the GNSS antenna phase center. It is the rotation matrix between the carrier coordinate system and the ENU coordinate system, and it is a state variable; Therefore, for Time of the first j The residuals from the raw observation data of the satellites, which link the system state and pseudorange-related measurements, are: in, It is a pseudorange measurement value. This is the constructed pseudo-distance factor.

7. The vehicle fusion positioning method based on GMM-assisted positioning according to claim 6, characterized in that, The specific process for constructing the clock drift factor in step S3 is as follows: A constant clock error drift model is applied to model the clock error of the GNSS receiver, and the resulting clock drift factor is: 。 8. A vehicle fusion positioning system applying the GMM-assisted vehicle fusion positioning method as described in any one of claims 1 to 7, characterized in that, The system includes an input module, a pre-integration module, a GMM parameter estimation module, and a factor graph optimization module. The input module is connected to the pre-integration module and the GMM parameter estimation module, respectively. The pre-integration module is connected to the GMM parameter estimation module and the factor graph optimization module, respectively. The GMM parameter estimation module is connected to the factor graph optimization module. The input module is used to acquire measurement information output from the IMU, wheel speed sensor, and GNSS receiver, respectively, and transmit the IMU measurement information and wheel speed sensor measurement information to the pre-integration module and the GNSS measurement information to the GMM parameter estimation module. The pre-integration module is used to calculate the IMU pre-integration term and the dynamic pre-integration term, and transmit the IMU pre-integration term and the dynamic pre-integration term to the factor graph optimization module and the dynamic pre-integration term to the GMM parameter estimation module; The GMM parameter estimation module uses GMM modeling, takes the pseudo-range residual sequence within the sliding window as input, and estimates the GMM parameters of the pseudo-range factor based on the expectation-maximization algorithm. The factor graph optimization module constructs IMU factors and dynamic factors using IMU pre-integration terms and dynamic pre-integration terms. To address the clock error in GNSS receivers, a clock drift factor is constructed. A factor map is constructed by combining the IMU factor, kinetic factor, pseudorange factor, and clock drift factor. The vehicle positioning information is then estimated by optimizing the factor graph.

Images