CN116576849A - Vehicle fusion positioning method and system based on GMM assistance - Google Patents
Vehicle fusion positioning method and system based on GMM assistance Download PDFInfo
- Publication number
- CN116576849A CN116576849A CN202310733137.7A CN202310733137A CN116576849A CN 116576849 A CN116576849 A CN 116576849A CN 202310733137 A CN202310733137 A CN 202310733137A CN 116576849 A CN116576849 A CN 116576849A
- Authority
- CN
- China
- Prior art keywords
- imu
- factor
- gmm
- vehicle
- pseudo
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 230000004927 fusion Effects 0.000 title claims abstract description 20
- 238000005259 measurement Methods 0.000 claims abstract description 92
- 230000010354 integration Effects 0.000 claims abstract description 63
- 230000001133 acceleration Effects 0.000 claims abstract description 14
- 238000005457 optimization Methods 0.000 claims description 31
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 4
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 claims description 3
- 230000005540 biological transmission Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 2
- 230000000087 stabilizing effect Effects 0.000 claims 1
- 230000002159 abnormal effect Effects 0.000 abstract description 5
- 230000033001 locomotion Effects 0.000 description 6
- 230000000694 effects Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention relates to a vehicle fusion positioning method and system based on GMM assistance, wherein the method comprises the following steps: calculating an IMU pre-integration item according to the angular velocity and the acceleration measurement information of the IMU; according to the speed measurement information of the wheel speed sensor and the angular speed measurement information of the IMU, combining a two-degree-of-freedom model of the vehicle, and calculating a dynamics pre-integration item; after GNSS measurement signals are obtained, IMU factors and dynamics factors are constructed by using IMU pre-integration items and dynamics pre-integration items; constructing pseudo-range factors according to the original observation information of the GNSS receiver and the system state; constructing a clock drift factor based on a GNSS receiver clock error; constructing factor graphs by combining the constructed factors, wherein IMU factors, dynamic factors and clock drift factor noise are Gaussian modeling, and pseudo-range factor noise is GMM modeling; and optimizing the solution factor graph and estimating the positioning information of the vehicle. Compared with the prior art, the method can effectively inhibit the influence of GNSS abnormal measurement on positioning, and realize high-precision positioning with low cost and robustness.
Description
Technical Field
The invention relates to the technical field of automatic driving vehicle positioning, in particular to a vehicle fusion positioning method and system based on GMM assistance.
Background
The three core technologies of automatic driving are perception, planning and control respectively, wherein a perception layer consists of a positioning, detecting and identifying algorithm and is used for understanding the position around the vehicle and the driving environment. Accurate and robust global positioning is critical to the implementation of autonomous driving of a vehicle and is also the basis for planning, decision making and motion control. Currently, vehicle positioning is most commonly used to provide positioning information for vehicles by means of global satellite navigation systems (Global Navigation Satellite System, GNSS), inertial measurement units (Inertial Measurement Unit, IMU) and wheel speed sensors, due to their relatively low cost.
Because the mileage calculation method of the IMU and the wheel speed is relative positioning, the final positioning accuracy is determined by the accuracy of the absolute positioning algorithm of the GNSS, and the GNSS can obtain better positioning accuracy in open scenes, however, in urban canyons, tunnels, forest shadows and other scenes, more abnormal measurement is caused due to multipath effect and influence of non-line-of-sight (Non Line of Sight, NLOS) reception. For this problem, common solutions are:
1. the method for improving the GNSS precision by reducing signal interference is characterized in that the method comprises the steps of reducing signal interference by adopting a method such as a choke ring, a dual-polarized antenna, an antenna array and the like in hardware design, but the method has higher cost;
2. satellite visibility is classified by using GNSS measurement information such as signal intensity, satellite elevation angle and the like or based on sensors such as a laser radar, a camera and the like, so that NLOS (non-linear optical system) reception is eliminated or corrected to improve GNSS positioning performance, but the performance of the method is strongly dependent on quality of satellite visibility classification, and partial NLOS reception can be detected frequently, so that the accuracy is limited.
In addition, in the existing GNSS/IMU/wheel speed fusion positioning method, the pitching and rolling movements of the vehicle are mostly ignored only based on the plane movement assumption of the vehicle, and the method is not suitable for positioning the vehicle in rough terrain and cannot guarantee the positioning accuracy.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide a vehicle fusion positioning method and system based on GMM (Gaussian Mixed Model, gaussian mixture model) assistance, which can effectively inhibit the influence of GNSS abnormal measurement on positioning and realize high-precision positioning at low cost.
The aim of the invention can be achieved by the following technical scheme: a vehicle fusion positioning method based on GMM assistance comprises the following steps:
s1, calculating a pre-integral item based on the position, the speed and the gesture of the IMU according to the angular speed and the acceleration measurement information of the IMU, and obtaining the IMU pre-integral item;
s2, calculating a pre-integral term of a vehicle position based on vehicle dynamics according to the speed measurement information of the wheel speed sensor and the triaxial angular speed measurement information of the gyroscope in the IMU and combining a vehicle two-degree-of-freedom model, and obtaining a dynamics pre-integral term;
s3, after GNSS measurement signals are obtained, IMU factors and dynamics factors are constructed by using the IMU pre-integration items and the dynamics pre-integration items;
according to the original observation information of the GNSS receiver, combining with the system state, and constructing pseudo-range factors;
constructing a clock drift factor based on a GNSS receiver clock error;
s4, combining an IMU factor, a dynamics factor, a pseudo-range factor and a clock drift factor, and constructing a factor graph, wherein the noise of the IMU factor, the dynamics factor and the clock drift factor is Gaussian modeling, and the noise of the pseudo-range factor is GMM modeling;
and then, the positioning information of the vehicle is obtained through the estimation of the optimized solution factor graph.
Further, the specific process of step S1 is as follows:
obtaining an acceleration measurement value a of the IMU in a carrier coordinate system b system at the IMU measurement time t t Angular velocity measurement ω t And acceleration measurement a at next IMU measurement time t+1 t+1 Angular velocity measurement ω t+1 The time interval between two IMU measurement moments is δt, and IM is calculated based on a median integration methodThe pre-integral term within the U sample interval is:
wherein ,for the position pre-integral term->For the velocity pre-integral term +.>Pre-integrating items for gestures, b a 、b ω The accelerometer zero bias and the gyroscope zero bias are respectively adopted, and subscripts t and t+1 represent IMU measurement time and +.>For the IMU acceleration average at time t, +.>The average value of the IMU angular velocity at the time t;
sampling interval [ t ] using GNSS measurement information k ,t k+1 ]Pre-integrating all IMU measured values in the internal to obtain [ t ] k ,t k+1 ]The total IMU pre-integral term.
Further, the step S2 specifically includes the following steps:
s21, establishing a two-degree-of-freedom model of the vehicle, and calculating to obtain the speed in a vehicle coordinate system;
s22, according to the speed in the vehicle coordinate system, combining with the three-axis angular speed measurement information of the gyroscope in the IMU, and based on a median integration method, considering adjacent wheel speed measurement time [ t, t+1], wherein the time interval between the two time points is δt, and deducing and obtaining a discrete form of kinetic pre-integration:
wherein ,for the position pre-integral term->Pre-integrating items for gestures, < >>Is the speed in the vehicle coordinate system;
sampling interval t for GNSS measurement information k ,t k+1 ]All wheel speed measured values in the wheel are pre-integrated to obtain [ t ] k ,t k+1 ]An internal total kinetic pre-integral term.
Further, the two-degree-of-freedom model of the vehicle in step S21 is specifically:
wherein ,kf and kr The cornering stiffness of the front and rear axles, respectively, I z For yaw moment of inertia, l f and lr Respectively representing the distances between the mass center and the front axle and the rear axle, wherein m is the mass of the whole vehicle, and alpha is the equivalent front wheel corner;
consider the steady-state steering characteristics of the vehicle:then the slip angle beta t And yaw rate omega r,t The method comprises the following steps:
wherein k=m (l f k f -l r k r )/l 2 k f k r As a stabilization factor, v t 、α t The wheel speed and the front wheel angle measured at the moment t respectively can be obtained according to the transmission ratio i of the front wheel and the steering wheel t δα/i, where δα is the steering wheel angle, thus yielding a speed in the vehicle coordinate systemAnd angular velocity->The method comprises the following steps:
further, the IMU factor constructed in step S3 specifically includes:
the kinetic factors are specifically:
wherein, X is the state quantity to be estimated, optimization is carried out based on the sliding window mode, and the state X in the window is:
n is the sliding window size, and the state quantity includes the position of the carrier coordinate system relative to the ENU coordinate systemSpeed->Posture->Zero offset of accelerometer>Zero offset of gyroscope>Receiver clock error δt i Receiver clock drift rate
Representation [ t ] k ,t k+1 ]Observed quantity within a time interval +.>Representing position error, +.>Representing speed error, +.>Representing the rotation error expressed in Euler angle, [ · ]] xyz Representing a computing operation that takes the imaginary part of the quaternion.
Further, the specific process of constructing the pseudo-range factor in the step S3 is as follows:
the GNSS receiver acquires ephemeris data and pseudorange observation data of the satellites at time t k Obtaining satellite s from ephemeris data j Position ofSatellite clock error->Atmospheric layer delay δρ kn,k Ionospheric delay δρ kp,k Receiver clock error δt k Distance-measuring error +.>Calculated from the following formula:
for t k The original observation data of the j satellite at the moment is modeled as pseudo-range measurement considering the ranging error:
wherein ,ωearth The rotational angular velocity of the earth, c is the speed of light,is the coordinates of the GNSS receiver in ECEF coordinate system, and the set state quantity +.>The relation between the two is:
wherein ,is a transformation matrix between ECEF and ENU coordinate systems, < >>A lever arm from the IMU center to the GNSS antenna phase center, < >>Is a rotation matrix between the carrier coordinate system and the ENU coordinate system, and is a state quantity;
thus for t k The residual error which links the system state and the pseudo-range related measurement is as follows:
wherein ,Zρk Is a pseudorange measurement,i.e. the constructed pseudo-range factor.
Further, the specific process of constructing the clock drift factor in the step S3 is as follows:
modeling the clock error of the GNSS receiver by applying a constant clock error drift (Constant Clock Error Drift, CCED) model, and constructing to obtain a clock drift factor as follows:
further, the specific process of modeling the noise of the pseudo-range factor in the step S4 by using GMM is as follows: the pseudo-range residual sequence in the sliding window is taken as input, the GMM parameters of the pseudo-range factors are estimated based on an EM (Expectation-maximization-Maximization algorithm) algorithm, and then the estimated GMM parameters are taken as a noise model of the pseudo-range factors in factor graph optimization.
Further, the step S4 specifically includes the following steps:
s41, GMM parameter estimation: taking a pseudo-range residual sequence in a sliding window as input, and based on GMM parameters of an EM estimated pseudo-range factor, obtaining:
where o is the residual sequence, M is the total number of residuals, e k For the pseudorange residuals, H is the hidden variable, θ is the GMM parameter to be estimated, N is the number of Gaussian components contained in the GMM, ω j Is the weight of the j-th Gaussian component, μ j Is the mean value of the j-th Gaussian component, Σ j Is the variance of the j-th gaussian component;
e-step of design: estimating hidden variable H, alpha based on initial guess of theta kj Representation e k Probability of belonging to the j-th gaussian component:
designing M-step: and (3) calculating theta according to the estimated H, and finishing updating of the GMM parameters:
iteratively executing the E-step and the M-step until the maximum iteration times or preset convergence conditions are reached, so as to finish the estimation of the GMM parameters;
s42, taking the estimated GMM parameter as a noise model of a pseudo-range factor in factor graph optimization, and obtaining a general formula for applying the GMM to factor optimization, wherein the general formula comprises:
wherein ,γs Is a normalization constant for keeping the negative log likelihood positive;
s43, factor graph optimization solving: the pseudo-range factor related items in the step S42 are combined with IMU factors, dynamic factors and clock drift factors of which the noise obeys Gaussian distribution to construct factor graphs together, and the Levenberg-Marquardt algorithm in Ceres Solver is used for optimizing and carrying out factor graph solving to obtain the position, speed and attitude information of the estimated vehicle:
wherein, sigma IMU, sigma Dynamics, sigma CCED are standard deviations of IMU factor, dynamics factor and clock drift factor, respectively, and the last term is a pseudo-range factor related term.
The vehicle fusion positioning system based on the GMM assistance comprises an input module, a pre-integration module, a GMM parameter estimation module and a factor graph optimization module, wherein the input module is respectively connected with the pre-integration module and the GMM parameter estimation module, the pre-integration module is respectively connected with the GMM parameter estimation module and the factor graph optimization module, the GMM parameter estimation module is connected with the factor graph optimization module, and the input module is used for respectively acquiring measurement information output by an IMU, a wheel speed sensor and a GNSS receiver, transmitting the measurement information of the IMU and the measurement information of the wheel speed sensor to the pre-integration module and transmitting the measurement information of the GNSS to the GMM parameter estimation module;
the pre-integration module is used for calculating to obtain an IMU pre-integration term and a dynamics pre-integration term, transmitting the IMU pre-integration term and the dynamics pre-integration term to the factor graph optimization module, and transmitting the dynamics pre-integration term to the GMM parameter estimation module;
the GMM parameter estimation module adopts GMM modeling, takes a pseudo-range residual sequence in a sliding window as input, and estimates GMM parameters of pseudo-range factors based on a maximum expectation algorithm;
the factor graph optimization module utilizes an IMU pre-integration term and a dynamics pre-integration term to construct an IMU factor and a dynamics factor;
constructing a clock drift factor aiming at a GNSS receiver clock error;
and combines IMU factors, dynamics factors, pseudo-range factors and clock drift factor construction factors,
and then, the factor graph is optimized and solved, and the vehicle positioning information is estimated and obtained.
Compared with the prior art, the invention has the following advantages:
1. according to the invention, firstly, according to the angular velocity and acceleration measurement information of the IMU, an IMU pre-integral item is calculated; then according to the speed measurement information of the wheel speed sensor and the angular speed measurement information of the IMU, calculating and obtaining a dynamics pre-integral item based on a two-degree-of-freedom model of the vehicle; after the GNSS measurement signal is obtained, on one hand, IMU factors and dynamics factors are built by using IMU pre-integration items and dynamics pre-integration items, pseudo-range factors are built according to original observation information of a GNSS receiver, clock drift factors are built aiming at receiver clock errors, and on the other hand, an IMU factor, dynamics factors, pseudo-range factors and clock drift factor construction factor graph is combined, and the factor graph is solved through optimization to estimate and obtain vehicle positioning information. Therefore, the GNSS/IMU/wheel speed tight coupling robust positioning scheme is realized, adverse effects of GNSS abnormal measurement on vehicle positioning can be effectively restrained, and meanwhile, the accuracy of vehicle positioning can be improved at low cost without setting hardware facilities for reducing signal interference.
2. When the dynamic pre-integral term is calculated, the influence caused by pitching and rolling motion cannot be ignored because the vehicle does not move on an approximate plane, and the information cannot be obtained in a two-degree-of-freedom dynamic model of the vehicle, so that the angular velocity information needs to be obtained by utilizing the three-axis angular velocity measurement information of a gyroscope in an IMU and combining the velocities in a vehicle coordinate system obtained by the two-degree-of-freedom model of the vehicle so as to deduce the dynamic pre-integral term. This ensures that the calculated kinetic pre-integral term can be fitted to the actual movement of the vehicle, suitable for vehicle positioning in rough terrain.
3. When the factor graph is constructed, the noise of the IMU factor, the dynamics factor and the clock drift factor is Gaussian modeling, the noise of the pseudo-range factor adopts GMM modeling, a pseudo-range residual sequence in a sliding window is taken as input, the GMM parameter of the pseudo-range factor is estimated based on a maximum expectation-maximization algorithm (EM), and then the estimated GMM parameter is taken as a noise model of the pseudo-range factor in factor graph optimization. Therefore, the reliability of the factor graph is fully ensured, and the positioning information such as the position, the speed, the gesture and the like of the vehicle can be accurately estimated.
Drawings
FIG. 1 is a schematic flow chart of the method of the present invention;
FIG. 2 is a schematic illustration of a linear two-degree-of-freedom vehicle dynamics model in an embodiment;
FIG. 3 is a schematic view of a factor graph constructed in the example;
fig. 4 is a schematic diagram of a system constructed in an embodiment.
Detailed Description
The invention will now be described in detail with reference to the drawings and specific examples.
Examples
As shown in fig. 1, a vehicle fusion positioning method based on GMM assistance includes the following steps:
s1, calculating a pre-integral item based on the position, the speed and the gesture of the IMU according to the angular speed and the acceleration measurement information of the IMU, and obtaining the IMU pre-integral item;
s2, calculating a pre-integral term of a vehicle position based on vehicle dynamics according to the speed measurement information of the wheel speed sensor and the triaxial angular speed measurement information of the gyroscope in the IMU and combining a vehicle two-degree-of-freedom model, and obtaining a dynamics pre-integral term;
s3, after GNSS measurement signals are obtained, IMU factors and dynamics factors are constructed by using the IMU pre-integration items and the dynamics pre-integration items;
according to the original observation information of the GNSS receiver, combining with the system state, and constructing pseudo-range factors;
constructing a clock drift factor based on a GNSS receiver clock error;
s4, combining an IMU factor, a dynamics factor, a pseudo-range factor and a clock drift factor, and constructing a factor graph, wherein the noise of the IMU factor, the dynamics factor and the clock drift factor is Gaussian modeling, and the noise of the pseudo-range factor is GMM modeling;
and then, the positioning information of the vehicle is obtained through the estimation of the optimized solution factor graph.
By applying the technical scheme, the main content of the embodiment comprises the following steps:
1. calculating a pre-integral term based on the position, the speed and the gesture of the IMU according to the angular speed and the acceleration measurement information of the IMU;
specifically, in the present embodiment, the update period of the selected factor graph is the same as the measurement period of the GNSS, and the output frequency (100 Hz) of the IMU is much higher than the output frequency (1 Hz) of the GNSS, so that the sampling interval [ t ] of the measurement information of the GNSS k ,t k+1 ]There are multiple IMU measurements. Taking a certain IMU measurement time t into consideration, obtaining an acceleration measurement value a of the IMU under a carrier coordinate system b system t And angular velocity measurement ω t The acceleration measured value at the next IMU measuring time t+1 is a t+1 Measured value of angular velocity omega t+1 . The time interval of two IMU samples is δt, and a pre-integration term in the IMU sampling interval is calculated based on a median integration method:
wherein ：
sampling interval [ t ] using GNSS measurement information k ,t k+1 ]All IMU measured values in the internal are pre-integrated to obtain [ t ] k ,t k+1 ]Inner total IMU pre-integral term: position pre-integral termSpeed pre-integral term->Gesture pre-integral termb a 、b ω Accelerometer and gyroscope zero offset.
2. Calculating a pre-integral term of a vehicle position based on vehicle dynamics based on a two-degree-of-freedom model (shown in fig. 2) of the vehicle based on speed measurement information of a wheel speed sensor and three-axis angular speed measurement information of a gyroscope in an IMU;
21 Based on the following two degree of freedom model of the vehicle:
wherein ,kf and kr The cornering stiffness of the front and rear axles, respectively, I z For yaw moment of inertia, l f and lr The distances between the mass center and the front axle and the rear axle are respectively represented, m is the mass of the whole vehicle, and alpha is the equivalent front wheel corner.
Consider the steady-state steering characteristics of the vehicle:then the slip angle beta t And yaw rate omega r,t The method comprises the following steps:
wherein the stability factor k=m(l f k f -l r k r )/l 2 k f k r ，v t 、α t The wheel speed and the front wheel angle measured at the moment t respectively can be obtained according to the transmission ratio i of the front wheel and the steering wheel t δα/i, where δα is the steering wheel angle, resulting in a speed in the vehicle coordinate systemAnd angular velocity->The method comprises the following steps:
22 In addition, the effects of pitch and roll motions cannot be ignored in view of the fact that the vehicle is not moving in an approximate plane. Since these information are not available in the vehicle dynamics model, the angular velocity information does not use the calculated values in step 21), but rather uses the three-axis angular velocity measurement information ω of the gyroscopes in the IMU t Combining the speeds in the vehicle coordinate system calculated in step 21)Method based on median integral, consider adjacent wheel speed measurement time [ t, t+1]]The time interval between the two moments is δt, the discrete form of kinetic pre-integration is derived:
pair [ t ] k ,t k+1 ]All wheel speed measured values in the time interval are pre-integrated to obtain [ t ] k ,t k+1 ]Inner total kinetic pre-integral term: position pre-integral termPosture of the objectPre-integration term->Since no constraint is built on angular velocity zero offset in the kinetic factor, the previous time t is used k Is +.>The angular velocity measurements over the time period are corrected.
3. When the GNSS measurement arrives (namely when the GNSS measurement signal is obtained), an IMU factor and a dynamics factor are constructed by using an IMU pre-integration term and a dynamics pre-integration term;
constructing pseudo-range factors according to the original observation information of the GNSS receiver and the system state;
modeling a GNSS receiver clock error by applying a Constant Clock Error Drift (CCED) model to construct a clock drift factor;
specific:
31 State amounts set in the present embodiment include: position of the Carrier coordinate System relative to the ENU coordinate SystemSpeed of speedPosture->Accelerometer zero bias->Zero offset of gyroscope>Receiver clock error δt i And receiver clock drift rate->
Optimizing based on a sliding window mode, and the state X in the window can be summarized as follows:
where n is the sliding window size.
32 At t) k+1 When the time GNSS measurement arrives, according to the IMU pre-integral term: position pre-integral termSpeed pre-integral term->Posture pre-integration term->And combining the system state to construct IMU factors:
wherein ,representation [ t ] k ,t k+1 ]Observed quantity within a time interval, X represents the state quantity to be estimated, < >>Representing position error, +.>Representing speed error, +.>Representing the rotation error expressed in Euler angle, [ · ]] xyz Representing a computing operation that takes the imaginary part of the quaternion.
33 At t) k+1 Upon arrival of the time GNSS measurements, according toKinetic pre-integration term: position pre-integral termPosture pre-integration term->And combining the system state to construct a kinetic factor:
34 Because the clock errors of the satellite systems are different, the clock errors among the constellations need to be considered by utilizing the multi-constellation information, the GPS single satellite system is only used in the embodiment, and the GNSS receiver can acquire the ephemeris data and the pseudo-range observation data of the satellites. At time t k From the ephemeris data, satellites s can be obtained j Position ofSatellite clock error->Atmospheric layer delay δρ kn,k Ionospheric delay δρ kp,k . In addition, GNSS receiver clock error δt k Distance-measuring error +.>Nor is it negligible, where δt k Is the largest component and varies with time, must be estimated together with the position of the GNSS receiver,/and>calculated from the following formula:
for t k The original observation data of the j-th satellite at the moment is modeled by taking the pseudo-range measurement of the ranging error into consideration:
wherein ,ωearth The rotational angular velocity of the earth, c is the speed of light,is the coordinates of the GNSS receiver in ECEF coordinate system, which is associated with the set state quantity +.>The relation between the two is: />
in the formula ,is a transformation matrix between the ECEF coordinate system and the ENU coordinate system, which is set as a constant value in the embodiment, which is calculated from the ECEF coordinate at the start point; />A lever arm from the IMU center to the GNSS antenna phase center; />Is the rotation matrix between the carrier coordinate system and the ENU coordinate system, and is the state quantity.
Thus for t k The residual error that links the system state and the pseudo-range related measurement for the original observation data of the j-th satellite at the moment can be expressed as:
in the formula ,is a pseudo-range measurement, +.>I.e. the constructed pseudo-range factor.
35 Since the clock error of the GNSS receiver is not a constant value, but usually drifts at a certain speed, the present solution applies a constant clock error drift (ccod) model to model it, and the clock drift factor built is:
4. upon arrival of a GNSS measurement, the IMU factor, the kinetic factor, the pseudo-range factor and the clock drift factor are combined to construct a factor graph, as shown in FIG. 3, wherein the noise of the IMU factor, the kinetic factor and the clock drift factor is Gaussian modeling; modeling the noise of the pseudo-range factor by adopting a Gaussian Mixture Model (GMM), taking a pseudo-range residual sequence in a sliding window as input, estimating the GMM parameter of the pseudo-range factor based on a maximum expectation algorithm (EM), and taking the estimated GMM parameter as a noise model of the pseudo-range factor in factor graph optimization; and finally, estimating the position, speed, attitude and other information of the vehicle through optimizing and solving factor diagrams.
Specific:
41 GMM parameter estimation: the GMM parameter of the pseudo range factor is estimated based on an EM algorithm by taking a pseudo range residual sequence in a sliding window as input, and the implementation steps are as follows:
where o is the residual sequence, M represents the number of residuals, and the pseudorange residuals e k As described in step 34); h is a hidden variable; θ is the GMM parameter to be estimated, N is the number of Gaussian components contained in the GMM, ω j Is the firstWeights of j Gaussian components, μ j Is the mean value of the j-th Gaussian component, Σ j Is the variance of the j-th gaussian component.
a) E-step: the hidden variable H is estimated based on an initial guess of θ. Alpha kj Representation e k Probability of belonging to the j-th gaussian component:
b) M-step: and (3) calculating theta according to the estimated H, and finishing updating of the GMM parameters:
and iteratively executing the steps E-and M-until the maximum iteration number or the set convergence condition is reached. Thus, the estimation of the GMM parameters is completed.
42 Using the estimated GMM parameters as a noise model of pseudo-range factors in the factor graph optimization, the general formula for applying GMM to the factor optimization is:
43 Factor graph optimization: constructing a factor graph by combining the pseudo-range factor related items in the step 42) with IMU factors, dynamic factors and clock drift factors of which the noise obeys Gaussian distribution, and finally optimizing the factor graph by using a Levenberg-Marquardt algorithm in Ceres Solver to solve the factor graph, and estimating information such as the position, the speed, the gesture and the like of the vehicle:
in the formula, sigma IMU, sigma Dynamics and sigma CCED are standard deviations of IMU factors, dynamics factors and clock drift factors respectively; the last term is the pseudo-range factor related term given in step 42).
Based on the above method, the embodiment builds a system structure as shown in fig. 4, which comprises an input module, a pre-integration module, a GMM parameter estimation module and a factor graph optimization module, wherein the input module is respectively connected with the pre-integration module and the GMM parameter estimation module, the pre-integration module is respectively connected with the GMM parameter estimation module and the factor graph optimization module, and the GMM parameter estimation module is connected with the factor graph optimization module.
The input module is used for respectively acquiring measurement information output by the IMU, the wheel speed sensor and the GNSS receiver, transmitting the measurement information of the IMU and the measurement information of the wheel speed sensor to the pre-integration module and transmitting the measurement information of the GNSS to the GMM parameter estimation module;
the pre-integration module is used for calculating to obtain an IMU pre-integration term and a dynamics pre-integration term, transmitting the IMU pre-integration term and the dynamics pre-integration term to the factor graph optimization module, and transmitting the dynamics pre-integration term to the GMM parameter estimation module;
the GMM parameter estimation module adopts GMM modeling, takes a pseudo-range residual sequence in a sliding window as input, and estimates the GMM parameters of pseudo-range factors based on a maximum expectation algorithm;
the factor graph optimization module utilizes the IMU pre-integration term and the dynamics pre-integration term to construct IMU factors and dynamics factors;
constructing a clock drift factor aiming at a GNSS receiver clock error;
and combines IMU factors, dynamics factors, pseudo-range factors and clock drift factor construction factors,
and then, the factor graph is optimized and solved, and the vehicle positioning information is estimated and obtained.
As can be seen from the above, the technical scheme realizes a GNSS/IMU/wheel speed tight coupling robust positioning scheme based on GMM assistance, can effectively inhibit the influence of GNSS abnormal measurement on positioning, and has the advantages of low cost, positioning robustness, small calculated amount and the like.
Claims (10)
1. The vehicle fusion positioning method based on the GMM assistance is characterized by comprising the following steps of:
s1, calculating a pre-integral item based on the position, the speed and the gesture of the IMU according to the angular speed and the acceleration measurement information of the IMU, and obtaining the IMU pre-integral item;
s2, calculating a pre-integral term of a vehicle position based on vehicle dynamics according to the speed measurement information of the wheel speed sensor and the triaxial angular speed measurement information of the gyroscope in the IMU and combining a vehicle two-degree-of-freedom model, and obtaining a dynamics pre-integral term;
s3, after GNSS measurement signals are obtained, IMU factors and dynamics factors are constructed by using the IMU pre-integration items and the dynamics pre-integration items;
according to the original observation information of the GNSS receiver, combining with the system state, and constructing pseudo-range factors;
constructing a clock drift factor based on a GNSS receiver clock error;
s4, combining an IMU factor, a dynamics factor, a pseudo-range factor and a clock drift factor, and constructing a factor graph, wherein the noise of the IMU factor, the dynamics factor and the clock drift factor is Gaussian modeling, and the noise of the pseudo-range factor is GMM modeling;
and then, the positioning information of the vehicle is obtained through the estimation of the optimized solution factor graph.
2. The vehicle fusion positioning method based on GMM assistance according to claim 1, wherein the specific process of step S1 is as follows:
obtaining an acceleration measurement value a of the IMU in a carrier coordinate system b system at the IMU measurement time t t Angular velocity ofMeasured value omega t And acceleration measurement a at next IMU measurement time t+1 t+1 Angular velocity measurement ω t+1 The time interval between two IMU measurement moments is δt, and the pre-integration term in the IMU sampling interval is calculated based on a median integration method as follows:
wherein ,for the position pre-integral term->For the velocity pre-integral term +.>Pre-integrating items for gestures, b a 、b ω The accelerometer zero bias and the gyroscope zero bias are respectively adopted, and subscripts t and t+1 represent IMU measurement time and +.>For the IMU acceleration average at time t, +.>The average value of the IMU angular velocity at the time t;
sampling interval [ t ] using GNSS measurement information k ,t k+1 ]All IMU measurements withinPre-integrating to obtain [ t ] k ,t k+1 ]The total IMU pre-integral term.
3. The GMM-assisted vehicle fusion positioning method according to claim 2, wherein the step S2 specifically comprises the steps of:
s21, establishing a two-degree-of-freedom model of the vehicle, and calculating to obtain the speed in a vehicle coordinate system;
s22, according to the speed in the vehicle coordinate system, combining with the three-axis angular speed measurement information of the gyroscope in the IMU, and based on a median integration method, considering adjacent wheel speed measurement time [ t, t+1], wherein the time interval between the two time points is δt, and deducing and obtaining a discrete form of kinetic pre-integration:
wherein ,for the position pre-integral term->Pre-integrating items for gestures, < >>Is the speed in the vehicle coordinate system;
sampling interval t for GNSS measurement information k ,t k+1 ]All wheel speed measured values in the wheel are pre-integrated to obtain [ t ] k ,t k+1 ]An internal total kinetic pre-integral term.
4. The vehicle fusion positioning method based on GMM assistance according to claim 3, wherein the two-degree-of-freedom model of the vehicle in step S21 is specifically:
wherein ,kf and kr The cornering stiffness of the front and rear axles, respectively, I z For yaw moment of inertia, l f and lr Respectively representing the distances between the mass center and the front axle and the rear axle, wherein m is the mass of the whole vehicle, and alpha is the equivalent front wheel corner;
consider the steady-state steering characteristics of the vehicle:then the slip angle beta t And yaw rate omega r,t The method comprises the following steps:
wherein k=m (l f k f -l r k r )/l 2 k f k r As a stabilizing factor, v t 、α t The wheel speed and the front wheel angle measured at the moment t respectively can be obtained according to the transmission ratio i of the front wheel and the steering wheel t δα/i, where δα is the steering wheel angle, thus yielding a speed in the vehicle coordinate systemAnd angular velocity->The method comprises the following steps:
5. the GMM-assisted vehicle fusion positioning method according to claim 4, wherein the IMU factor constructed in step S3 is specifically:
the kinetic factors are specifically:
wherein, X is the state quantity to be estimated, optimization is carried out based on the sliding window mode, and the state X in the window is:
n is the sliding window size, and the state quantity includes the position of the carrier coordinate system relative to the ENU coordinate systemSpeed->Posture of the objectZero offset of accelerometer>Zero offset of gyroscope>Receiver clock error δt i Receiver clock drift rate->
Representation [ t ] k ,t k+1 ]Observed quantity within a time interval +.>Representing position error, +.>Representing the error in the velocity of the object,representing the rotation error expressed in Euler angle, [ · ]] xyz Representing a computing operation that takes the imaginary part of the quaternion.
6. The GMM-assisted vehicle fusion positioning method according to claim 5, wherein the specific process of constructing the pseudo-range factor in step S3 is as follows:
the GNSS receiver acquires ephemeris data and pseudorange observation data of the satellites at time t k Obtaining satellite s from ephemeris data j Position ofSatellite clock error->Atmospheric layer delay->Ionospheric delay->Receiver clock error δt k Distance-measuring error +.> Calculated from the following formula:
for t k The original observation data of the j satellite at the moment is modeled as pseudo-range measurement considering the ranging error:
wherein ,ωearth The rotational angular velocity of the earth, c is the speed of light,is the coordinates of the GNSS receiver in ECEF coordinate system, and the set state quantity +.>The relation between the two is:
wherein ,is a transformation matrix between ECEF and ENU coordinate systems, < >>A lever arm from the IMU center to the GNSS antenna phase center, < >>Is a rotation matrix between the carrier coordinate system and the ENU coordinate system, and is a state quantity;
thus for t k The residual error which links the system state and the pseudo-range related measurement is as follows:
wherein ,Zρk Is a pseudorange measurement,i.e. the constructed pseudo-range factor.
7. The GMM-assisted vehicle fusion positioning method according to claim 6, wherein the specific process of constructing the clock drift factor in step S3 is as follows:
modeling the clock error of the GNSS receiver by applying a constant clock error drift model, and constructing to obtain a clock drift factor as follows:
8. the GMM-assisted vehicle fusion positioning method according to claim 1, wherein the specific process of GMM modeling for the noise of the pseudo-range factor in step S4 is as follows: and taking a pseudo-range residual sequence in the sliding window as input, estimating the GMM parameters of the pseudo-range factors based on the EM, and then taking the estimated GMM parameters as a noise model of the pseudo-range factors in factor graph optimization.
9. The GMM-assisted vehicle fusion positioning method according to claim 8, wherein the step S4 specifically comprises the steps of:
s41, GMM parameter estimation: taking a pseudo-range residual sequence in a sliding window as input, and based on GMM parameters of an EM estimated pseudo-range factor, obtaining:
where o is the residual sequence, M is the total number of residuals, e k For the pseudorange residuals, H is the hidden variable, θ is the GMM parameter to be estimated, N is the number of Gaussian components contained in the GMM, ω j Is the weight of the j-th Gaussian component, μ j Is the mean value of the j-th Gaussian component, Σ j Is the variance of the j-th gaussian component;
e-step of design: estimating hidden variable H, alpha based on initial guess of theta kj Representation e k Probability of belonging to the j-th gaussian component:
designing M-step: and (3) calculating theta according to the estimated H, and finishing updating of the GMM parameters:
iteratively executing the E-step and the M-step until the maximum iteration times or preset convergence conditions are reached, so as to finish the estimation of the GMM parameters;
s42, taking the estimated GMM parameter as a noise model of a pseudo-range factor in factor graph optimization, and obtaining a general formula for applying the GMM to factor optimization, wherein the general formula comprises:
wherein ,γs Is a normalization constant for keeping the negative log likelihood positive;
s43, factor graph optimization solving: the pseudo-range factor related items in the step S42 are combined with IMU factors, dynamic factors and clock drift factors of which the noise obeys Gaussian distribution to construct factor graphs together, and the Levenberg-Marquardt algorithm in Ceres Solver is used for optimizing and carrying out factor graph solving to obtain the position, speed and attitude information of the estimated vehicle:
wherein, sigma IMU, sigma Dynamics, sigma CCED are standard deviations of IMU factor, dynamics factor and clock drift factor, respectively, and the last term is a pseudo-range factor related term.
10. A vehicle fusion positioning system applying the vehicle fusion positioning method based on the GMM assistance according to claim 1, which is characterized by comprising an input module, a pre-integration module, a GMM parameter estimation module and a factor graph optimization module, wherein the input module is respectively connected with the pre-integration module and the GMM parameter estimation module, the pre-integration module is respectively connected with the GMM parameter estimation module and the factor graph optimization module, the GMM parameter estimation module is connected with the factor graph optimization module, and the input module is used for respectively acquiring measurement information output by an IMU, a wheel speed sensor and a GNSS receiver, transmitting the measurement information of the IMU and the measurement information of the wheel speed sensor to the pre-integration module and transmitting the measurement information of the GNSS to the GMM parameter estimation module;
the pre-integration module is used for calculating to obtain an IMU pre-integration term and a dynamics pre-integration term, transmitting the IMU pre-integration term and the dynamics pre-integration term to the factor graph optimization module, and transmitting the dynamics pre-integration term to the GMM parameter estimation module;
the GMM parameter estimation module adopts GMM modeling, takes a pseudo-range residual sequence in a sliding window as input, and estimates GMM parameters of pseudo-range factors based on a maximum expectation algorithm;
the factor graph optimization module utilizes an IMU pre-integration term and a dynamics pre-integration term to construct an IMU factor and a dynamics factor;
constructing a clock drift factor aiming at a GNSS receiver clock error;
and combines IMU factors, dynamics factors, pseudo-range factors and clock drift factor construction factors,
and then, the factor graph is optimized and solved, and the vehicle positioning information is estimated and obtained.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310733137.7A CN116576849A (en) | 2023-06-19 | 2023-06-19 | Vehicle fusion positioning method and system based on GMM assistance |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310733137.7A CN116576849A (en) | 2023-06-19 | 2023-06-19 | Vehicle fusion positioning method and system based on GMM assistance |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116576849A true CN116576849A (en) | 2023-08-11 |
Family
ID=87545392
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310733137.7A Pending CN116576849A (en) | 2023-06-19 | 2023-06-19 | Vehicle fusion positioning method and system based on GMM assistance |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116576849A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116817928A (en) * | 2023-08-28 | 2023-09-29 | 北京交通大学 | Method for multi-source fusion positioning of guard/inertial navigation train based on factor graph optimization |
-
2023
- 2023-06-19 CN CN202310733137.7A patent/CN116576849A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116817928A (en) * | 2023-08-28 | 2023-09-29 | 北京交通大学 | Method for multi-source fusion positioning of guard/inertial navigation train based on factor graph optimization |
| CN116817928B (en) * | 2023-08-28 | 2023-12-01 | 北京交通大学 | Method for multi-source fusion positioning of guard/inertial navigation train based on factor graph optimization |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108226980B (en) | Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit | |
| CN110779521A (en) | Multi-source fusion high-precision positioning method and device | |
| US7979231B2 (en) | Method and system for estimation of inertial sensor errors in remote inertial measurement unit | |
| CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
| CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
| CN112505737B (en) | GNSS/INS integrated navigation method | |
| Bevly et al. | GNSS for vehicle control | |
| CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| WO2016203744A1 (en) | Positioning device | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| JP2000502801A (en) | Improved vehicle navigation system and method using multi-axis accelerometer | |
| CN113340298A (en) | Inertial navigation and dual-antenna GNSS external reference calibration method | |
| CN115388884A (en) | Joint initialization method for intelligent body pose estimator | |
| CN116576849A (en) | Vehicle fusion positioning method and system based on GMM assistance | |
| JP2014240266A (en) | Sensor drift amount estimation device and program | |
| CN117053782A (en) | Combined navigation method for amphibious robot | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN116558512A (en) | GNSS and vehicle-mounted sensor fusion positioning method and system based on factor graph | |
| CN112923934A (en) | Laser SLAM technology suitable for combining inertial navigation in unstructured scene | |
| CN114435630B (en) | Method for relatively tracking non-cooperative target by using limited vision measurement | |
| CN114264304B (en) | High-precision horizontal attitude measurement method and system for complex dynamic environment | |
| JP2013108961A (en) | Positioning device and program | |
| CN113503872A (en) | Low-speed unmanned vehicle positioning method based on integration of camera and consumption-level IMU | |
| CN114111792A (en) | Vehicle-mounted GNSS/INS/odometer combined navigation method | |
| CN108931247B (en) | Navigation method and device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |