Vehicle positioning method, vehicle positioning device and system under short-term GNSS failure based on driving behavior and inertial navigation

Abstract

The present application relates to the field of vehicle positioning technology, and discloses a vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation. The vehicle positioning method includes: obtaining and determining driving state information based on driving behavior information; predicting the occurrence probability of each preset motion constraint model based on driving behavior information and driving state information; determining a mixed probability matrix based on the occurrence probability corresponding to all preset motion constraint models; obtaining inertial positioning data, and calculating the overall estimated value and overall covariance of the filtering results of each preset motion constraint model based on the inertial positioning data and the mixed probability; correcting the inertial positioning data based on the overall estimated value and the overall covariance, and determining the position information of the vehicle based on the corrected inertial positioning data. The use of this vehicle positioning method can improve positioning accuracy. The present application also discloses a vehicle positioning device and a vehicle positioning system under short-term GNSS failure based on driving behavior and inertial navigation.

Description

Technical Field

[0001] The present application relates to the field of vehicle positioning technology, for example, to a vehicle positioning method, a vehicle positioning device and a system under short-term GNSS failure based on driving behavior and inertial navigation. Background Art

[0002] At present, with a large amount of capital, technology and talents pouring into the autonomous driving industry, smart cars can already cope with most driving scenarios. The mainstream high-precision positioning solution for autonomous driving vehicles is: through the combination of the Global Navigation Satellite System (GNSS) and the Inertial Measurement Unit (IMU), and the use of high-precision real-time differential positioning technology (RTK), centimeter-level positioning can be effectively achieved. Among them, GNSS satellite navigation is responsible for clarifying the absolute position, and IMU inertial navigation is responsible for clarifying the relative position of the vehicle body. It can supplement the position positioning in sections such as tunnels where satellite signals are blocked, and realize the body's perception of its own position through fusion algorithms. Combined with sensors such as lidar / cameras, the environmental features are described by point clouds and matched with high-precision maps to further determine the position of the vehicle, thereby achieving accurate positioning with high redundancy.

[0003] The above positioning scheme makes full use of the advantages of different positioning methods and achieves the effect of complementary advantages.

[0004] In the process of implementing the embodiments of the present application, it is found that there are at least the following problems in the related technology:

[0005] With the rapid development of three-dimensional transportation, more and more tunnels and overpasses are put into use. Due to the influence of non-line-of-sight and multipath effects, GNSS signals have defects such as long-term failure and local pollution. In particular, in tunnels, there are often failures of several minutes or even dozens of minutes. For overpasses and cities, local pollution of GNSS signals is often difficult to predict and control. IMU has a higher frequency and can provide relative position information to assist positioning when GNSS is not working, but its positioning error will accumulate with working time and distance, and the positioning error of low-cost civilian IMU has a significant impact on high-precision positioning of autonomous driving.

[0006] It should be noted that the information disclosed in the above background technology section is only used to enhance the understanding of the background of the present application, and therefore may include information that does not constitute the prior art known to ordinary technicians in the field. Summary of the invention

[0007] In order to provide a basic understanding of some aspects of the disclosed embodiments, a brief summary is given below. The summary is not an extensive review, nor is it intended to identify key / critical components or delineate the scope of protection of these embodiments, but rather serves as a prelude to the detailed description that follows.

[0008] The embodiment of the present application provides a vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation, so as to reduce the positioning error of the IMU and improve the positioning accuracy when the GNSS is not working.

[0009] In some embodiments, a vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation includes:

[0010] Obtain driving behavior information, and determine driving state information according to the driving behavior information; the driving behavior information includes: steering wheel angle, vehicle lateral speed and yaw angular velocity; the driving state information includes: straight driving, left lane change and right lane change;

[0011] The occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information; the preset motion constraint model includes: a uniform linear motion model, a uniformly accelerated linear motion model, a constant turning rate and speed motion model, and a constant turning rate and acceleration model;

[0012] Determine a mixed probability matrix according to the occurrence probabilities corresponding to all the preset motion constraint models;

[0013] Obtaining inertial positioning data, and calculating an overall estimated value and an overall covariance of filtering results of each of the preset motion constraint models according to the inertial positioning data and the mixed probability; the inertial positioning data includes one or more of yaw angular velocity information, heading angle information, and acceleration information;

[0014] The inertial positioning data is corrected according to the overall estimated value and the overall covariance, and the position information of the vehicle is determined according to the corrected inertial positioning data.

[0015] The vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in the embodiment of the present application can achieve the following technical effects:

[0016] Driving behavior information such as steering wheel angle, vehicle lateral speed and yaw rate belongs to short-term micro information. First, driving state information is determined based on such short-term micro data, and then propagated to driving behavior information. Driving state information such as straight driving, left lane change and right lane change belongs to long-term macro information. Then, in the process of predicting the occurrence probability of each preset motion constraint model based on driving behavior information and driving state information, driving state information can show long-term macro constraint characteristics for vehicle positioning, that is, the occurrence probability of the preset motion constraint model can show long-term macro constraint characteristics for vehicle positioning. This makes the mixed probability of the occurrence probability corresponding to all preset motion constraint models reflect the long-term macro constraint characteristics, and then in the Interacting Multiple Model Filter (IMM), using the mixed probability with such long-term macro constraint characteristics as input can improve the filtering accuracy and stability of the IMM, and then obtain the overall estimation value and overall covariance with higher accuracy and stability.

[0017] In the process of correcting the inertial positioning data with such overall estimates and overall covariance, data with certain long-term macro-constraint characteristics are used to reduce the cumulative error of the IMU, and data with short-term micro-constraint characteristics substituted into the correction process are reduced as much as possible. The high-frequency and short-term precise positioning characteristics provided by the IMU are retained as much as possible. The vehicle position information determined based on such corrected inertial positioning data has higher accuracy and robustness.

[0018] Optionally, the inertial positioning data is corrected according to the overall estimated value and the overall covariance, comprising:

[0019] Determining a first weight of the population estimate according to the population covariance; the first weight is negatively correlated with the uncertainty represented by the population covariance;

[0020] Determining a second weight of the inertial positioning data according to the covariance of the matrix formed by the inertial positioning data; the second weight is negatively correlated with the uncertainty represented by the covariance of the matrix formed by the inertial positioning data;

[0021] Calculate a weighted average value or a weighted sum according to the overall estimated value, the first weight, the inertial positioning data matrix, and the second weight;

[0022] The second weighted average value or the weighted sum is used as the corrected inertial positioning data.

[0023] Optionally, determining a first weight of the population estimate according to the population covariance includes:

[0024]

[0025] Among them, w IMM is the first weight, P(k|k) is the population covariance at time k, trace(P(k|k)) is the trace of P(k|k), n is the dimension of P(k|k), (P(k|k)) ii is the i-th diagonal element of P(k|k).

[0026] Optionally, determining the second weight of the inertial positioning data according to the covariance of the matrix formed by the inertial positioning data includes:

[0027]

[0028] Among them, w IMU is the second weight, P IMU is the covariance of the matrix of inertial positioning data, trace(P IMU ) is P IMU traces, m is P IMU The dimension of (P IMU ) ii P IMU The i-th diagonal element of .

[0029] Optionally, calculating the overall estimated value and overall covariance of the filtering results of each of the preset motion constraint models according to the inertial positioning data and the mixing probability includes:

[0030] The initial conditions of each of the preset motion constraint model filtering algorithms are determined in the following manner:

[0031]

[0032] in, is the state prediction value of model j at time k-1; r is the number of models, r=4; is the state prediction value of model i at time k-1; p j,0 (k-1|k-1) is the covariance prediction value of model j at time k-1; is the predicted value of the state covariance of model i at time k-1; is the mixing probability from model i to model j at time k-1;

[0033] p ij is the transition probability from model i to model j, and is the transition probability matrix element of the prior Markov model; is the occurrence probability of model i at time k-1;

[0034] The population estimate and the population covariance are determined as follows:

[0035]

[0036] p j (k|k-1)=φ j (k-1)p j,0 (k-1|k-1)φ j (k-1) T +B j Q j B j T

[0037] K j (k) = p j (k|k-1)H T [Hp j (k|k-1)H T +P] -1

[0038]

[0039] p j (k|k)=[IK j (k)H(k)]p j (k|k-1)

[0040] in, is the state prediction value of model j at time k; φ j (k-1) is the state transfer matrix at time k-1, which is used to describe the state change relationship of the system from time k-1 to time k; is the state prediction value of model j at time k-1, that is, the posterior state estimate at the previous moment; p j (k|k-1) is the covariance prediction value of model j at time k; p j,0 (k-1|k-1) is the covariance prediction value of model j at time k-1, that is, the posterior covariance estimate of the previous moment; B j is the control input matrix, which is used to represent the impact of noise on the system state; Q j is the process covariance matrix, which is used to represent the size of the system process noise; B j T is the control input matrix B j The transpose of K j (k) is the Kalman gain matrix of model j at time k, which determines the update weight of the observation value on the state estimation; H is the observation matrix, which is used to represent the relationship between the state vector and the observation vector; R is the observation noise covariance matrix; is the state update matrix for model j at time k; Z(k) is the observation vector matrix at time k, which is composed of inertial positioning data; is the observed residual; p j (k|k) is the covariance update matrix for model j at time k; I is the identity matrix.

[0041] The model probability is updated through the following likelihood function:

[0042]

[0043] Among them, ∧ j (k) is the likelihood function of model j at time k; is the transpose of the measurement method for model j, is the residual covariance of model j; the model probability is updated as:

[0044]

[0045] Among them, μ j (k) is the posterior probability of model j at time k, μ j (k-1) is the posterior probability of model j at time k-1;

[0046] The population estimates and population covariances are:

[0047]

[0048] in, is the population estimate at time k; P(k|k) is the population covariance at time k.

[0049] Optionally, determining the driving state information based on the driving behavior information includes: obtaining driving behavior information of multiple consecutive moments before the current moment to form an observation sequence of the driving behavior information; and determining the driving state information based on the observation sequence of the driving behavior information.

[0050] Optionally, determining the driving state information according to the observation sequence of the driving behavior information includes:

[0051] The observation sequence of the driving behavior information is input into a Hidden Markov Model (HMM) to obtain the driving state information output by the HMM.

[0052] Optionally, predicting the occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information includes:

[0053] Obtaining multiple sequences of the driving behavior information and the driving state information according to the order of the timestamps;

[0054] Multiple sequences of the driving behavior information and the driving state information are input into an artificial neural network model with sequence processing capability to obtain occurrence probabilities of multiple preset motion constraint models output by the artificial neural network model with sequence processing capability.

[0055] Optionally, determining a mixed probability matrix according to the occurrence probabilities corresponding to all the preset motion constraint models includes:

[0056] Determine the transition probability matrix of the prior Markov model according to the occurrence probabilities corresponding to all the preset motion constraint models;

[0057] The mixing probability matrix is ​​determined according to the transition probability.

[0058] In some embodiments, a vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation includes a driving state prediction module, a motion model probability prediction module, a first filtering module, a second filtering module and a correction module.

[0059] The driving state prediction module is used to obtain driving behavior information and determine driving state information according to the driving behavior information; the driving behavior information includes: steering wheel angle, vehicle lateral speed and yaw angular velocity; the driving state information includes: straight driving, left lane change and right lane change;

[0060] The motion model probability prediction module is used to predict the occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information; the preset motion constraint model includes: a uniform linear motion model, a uniformly accelerated linear motion model, a constant turning rate and speed motion model, and a constant turning rate and acceleration model;

[0061] The first filtering module is used to determine a mixed probability matrix according to the occurrence probabilities corresponding to all the preset motion constraint models;

[0062] The second filtering module is used to obtain inertial positioning data, and calculate the overall estimation value and overall covariance of the Kalman filter algorithm according to the inertial positioning data and the mixed probability; the inertial positioning data includes one or more of yaw angular velocity information, heading angle information and acceleration information;

[0063] The correction module is used to correct the inertial positioning data according to the overall estimated value and the overall covariance, and determine the position information of the vehicle according to the corrected inertial positioning data.

[0064] The vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation also has the technical effect of improving positioning accuracy. The specific analysis process refers to the aforementioned analysis of the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation, which will not be repeated here.

[0065] In some embodiments, a vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation includes a processor and a memory storing program instructions, and the processor is configured to execute the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in the above embodiments when executing the program instructions.

[0066] The vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation also has the technical effect of improving positioning accuracy.

[0067] In some embodiments, the vehicle positioning system includes the vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation provided in the aforementioned embodiments.

[0068] Similarly, the vehicle positioning device also has the technical effect of improving positioning accuracy.

[0069] The above general description and the following description are exemplary and explanatory only and are not intended to limit the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] One or more embodiments are exemplarily described by corresponding drawings, and these exemplary descriptions and drawings do not limit the embodiments. Elements with the same reference numerals in the drawings are regarded as similar elements, and wherein:

[0071] Figure 1 It is a flow chart of a vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in an embodiment of the present application;

[0072] Figure 2 It is a schematic diagram of a vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation provided in an embodiment of the present application;

[0073] Figure 3 It is a schematic diagram of a specific implementation process of a vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in an embodiment of the present application;

[0074] Figure 4 It is a schematic diagram of a vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation provided in an embodiment of the present application. DETAILED DESCRIPTION

[0075] In order to be able to understand the features and technical contents of the embodiments of the present application in more detail, the implementation of the embodiments of the present application is described in detail below in conjunction with the accompanying drawings. The attached drawings are for reference only and are not used to limit the embodiments of the present application. In the following technical description, for the convenience of explanation, a full understanding of the disclosed embodiments is provided through multiple details. However, in the absence of these details, one or more embodiments can still be implemented. In other cases, to simplify the drawings, well-known structures and devices can be simplified for display.

[0076] The terms "first", "second", etc. in the specification and claims of the embodiments of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the embodiments of the present application described here. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions.

[0077] Unless otherwise stated, the term "plurality" means more than two.

[0078] In the embodiment of the present application, the character " / " indicates that the preceding and following objects are in an "or" relationship. For example, A / B means: A or B.

[0079] The term "and / or" is a description of the association relationship between objects, indicating that three relationships can exist. For example, A and / or B means: A or B, or, A and B.

[0080] Figure 1 The present invention provides a flow chart of a vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in an embodiment of the present invention. The vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation can be performed by a vehicle positioning navigation device, or can also be performed by a mobile device.

[0081] Combination Figure 1 As shown in the figure, the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation includes:

[0082] S101. Obtain driving behavior information, and determine driving state information according to the driving behavior information.

[0083] The driving behavior information includes: steering wheel angle, vehicle lateral speed and yaw rate. The driving state information includes: straight driving, left lane change and right lane change.

[0084] In some implementation scenarios, the steering wheel angle may be obtained through a steering wheel angle sensor, the vehicle lateral speed may be obtained through a wheel speed sensor, and the yaw angular velocity may be obtained through IMU detection.

[0085] The above-mentioned steering wheel angle sensor can be a vehicle-built-in sensor or an additionally installed sensor; the wheel speed sensor can be a vehicle-built-in sensor or an additionally installed sensor; the IMU can be a vehicle-built-in IMU or other IMU fixed to the vehicle and stationary relative to the vehicle.

[0086] Different types of sensors have different sampling frequencies. First, a sampling reference frequency can be determined according to the sampling frequencies of multiple sensors, and then the sampling data of each sensor can be timestamped according to the reference sampling frequency.

[0087] In the process of determining the sampling reference frequency according to the sampling frequencies of multiple sensors, the lowest sampling frequency among the multiple sensors may be used as the sampling reference frequency, or the average sampling frequency of the multiple sensors may be used as the sampling reference frequency.

[0088] In the process of aligning the timestamps of the sampling data of each sensor according to the reference sampling frequency, the sampling data of each sensor may be aligned for timestamps by using a linear interpolation method; the sampling data of each sensor may also be aligned for timestamps by using a polynomial difference method.

[0089] Of course, the two methods listed above for determining the sampling reference frequency according to the sampling frequencies of multiple sensors, and the two methods for aligning the timestamps of the sampling data of each sensor according to the reference sampling frequency, are only exemplary. In specific application scenarios, those skilled in the art can also determine the method of determining the sampling reference frequency and the method of aligning the timestamps that are in line with the actual application situation based on experience.

[0090] The above-mentioned driving behavior information includes steering wheel angle, vehicle lateral speed and yaw rate, which is used to illustrate that the driving behavior information is short-term microscopic vehicle characteristic information of the vehicle itself. In specific application scenarios, those skilled in the art may also detect other specific types of short-term microscopic driving behavior information of the vehicle itself similar to the above-mentioned steering wheel information, vehicle lateral speed and yaw rate, which will not be described one by one here.

[0091] The driving state information listed above includes straight driving, left lane change and right lane change, which is used to illustrate that the driving state information is relatively long-term macro-specific vehicle information. In specific application scenarios, those skilled in the art can also obtain other types of specific driving state information similar to the long-term macro-specific driving state information of straight driving, left lane change and right lane change mentioned above, which will not be described here one by one.

[0092] The aforementioned step of determining the driving state information based on the driving behavior information serves to transform the short-term microscopic constraint characteristics of the vehicle positioning into long-term macroscopic constraint characteristics.

[0093] Optionally, determining the driving state information based on the driving behavior information includes: obtaining driving behavior information of multiple consecutive moments before the current moment to form an observation sequence of the driving behavior information; and determining the driving state information based on the observation sequence of the driving behavior information.

[0094] Specifically, the driving state information is determined according to the observation sequence of the driving behavior information, including: inputting the observation sequence of the driving behavior information into the HMM to obtain the driving state information output by the HMM.

[0095] For example, the observation sequence of driving behavior information can be expressed as follows:

[0096] o=O 1 O 2 O 3 ... T , where o is the observation sequence of driving behavior information, T is the length of the time series; the observation value at time t in the observation sequence of driving behavior information is the steering wheel angle, is the vehicle lateral velocity, Of course, in the specific application process, if more types of driving behavior information are determined according to the actual situation, the number of adaptively increased O t The number of elements in , will not be repeated here one by one.

[0097] The observation sequence O of driving behavior information is input into HMM, and the output of HMM is the driving state information.

[0098] Of course, the HMM refers to the trained HMM.

[0099] During the training process of HMM, the observation sequence of driving behavior information and the state sequence of driving state information are collected at the same time. For example, the observation sequence of driving behavior information and the state sequence of driving state information are recorded as {O, Q}, where O is the observation sequence of driving behavior information, Q is the state sequence of driving state information, and Q = q 1 q 2 q 3 ...q T , T is the length of the time series, the driving state information can be obtained by manual recording, or by a camera set up in the test section; the state at the tth moment in the state sequence Q of the driving state information is q t ,q t The value set S = {s 2 ,s 3 ,…,s N}, N is the number of types of vehicle driving state information. In the embodiment of the present application, N=3 is taken as an example for exemplary description (the driving state information includes straight driving, left lane change and right lane change).

[0100] According to the Markov hypothesis, the state of the system at any time is only related to the state of the previous time. Then, the joint probability distribution of the observation sequence O of driving behavior information and the state sequence Q of driving state information is:

[0101]

[0102] Among them, π is the state probability of the vehicle driving state information at the initial moment, π=(π 1 ,π 1 ,…,π N ), N is the number of types of vehicle driving status information, π i =P(q 1 =s i ),i∈[1,N];

[0103] A is the probability of the system transitioning between various states, usually an N×N dimensional matrix. The element a in matrix A is ij =P(q t+1 =s j |q t =s i ),i,j∈[1,N],t∈[1,T-1];

[0104] φ is the probability of the system outputting an observation value in each state.

[0105] The probability b that the system outputs the value of the observation variable O in state i at time t is i (O t )=P(O t |q t =s i ,φ),i∈[1,N],t∈[1,T].

[0106] The HMM can be trained by obtaining offline vehicle driving information and vehicle status information to obtain an HMM for identifying vehicle status information.

[0107] Of course, the above-mentioned use of HMM to process the observation sequence of driving behavior information is only an exemplary description. Other algorithm models with the ability to process sequences, such as the Long Short-Term Memory (LSTM) network and its deformation algorithms, can also be used to process the observation sequence of driving behavior information to obtain driving state information.

[0108] S102: predicting the occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information.

[0109] The preset motion constraint models include: uniform linear motion model, uniformly accelerated linear motion model, constant turning rate and speed motion model and constant turning rate and acceleration model.

[0110] Optionally, predicting the occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information includes:

[0111] Obtain multiple sequences of driving behavior information and driving state information according to the order of timestamps;

[0112] Multiple sequences of driving behavior information and driving state information are input into an artificial neural network model with sequence processing capability, and the occurrence probability of each preset motion constraint model output by the artificial neural network model with sequence processing capability is obtained.

[0113] When the preset motion constraint model includes the above four models, the output of the model usually includes the occurrence probabilities of the uniform linear motion model, the uniformly accelerated linear motion model, the constant turning rate and speed motion model and the constant turning rate and acceleration model.

[0114] LSTM can be used as an artificial neural network model with sequence processing capabilities.

[0115] In LSTM, the retention of the previous moment is first determined. This function is completed by the forget gate, and the formula of this function is:

[0116] F t =σ(W f ·[h t-1 ,X t ]+b f )

[0117] Among them, F t is the forget gate at time t, σ is the sigmoid activation function, W f is the weight of the forget gate, h t-1 is the hidden state at time t-1, X t is the vehicle driving information and vehicle status information input at time t, b f is the bias of the forget gate.

[0118] The forget gate uses the Sigmoid function to map the input and the state of the previous moment to a value between 0 and 1 to determine the retention of the state of the previous moment. 1 means complete retention and 0 means discarding.

[0119] The input gate then determines the output information at the current moment.

[0120] The input gate first constructs a candidate vector through the tanh function, and the calculation formula is:

[0121]

[0122] in, is the candidate vector, W c is the weight when constructing the candidate vector, h t-1 is the hidden state at time t-1, X t is the vehicle driving information and vehicle status information input at time t, b c is the bias when constructing the candidate vector.

[0123] The input gate then selects the forgetting ratio, and the calculation formula is:

[0124] i t =σ(W i ·[h t-1 ,X t ]+b i )

[0125] Among them, i t is the input gate at time t, σ is the Sigmoid function, W i is the weight of the input gate, h t-1 is the hidden state at time t-1, X t is the vehicle driving information and vehicle status information input at time t, b i is the bias of the input gate.

[0126] Finally, the output gate determines the output value, and the calculation formula is:

[0127]

[0128] O t =σ(W o [h t-1 ,X t ]+b o )

[0129] h t =O t tanh(C t )

[0130] Among them, C t is the memory unit at time t, F t is the forget gate at time t, C t-1 is the memory unit at time t-1, i t is the forget gate at time t, is the candidate vector at time t, O tis the output gate at time t, σ is the Sigmoid function, W o is the weight of the output gate, h t-1 is the hidden state at time t, X t is the vehicle driving information and vehicle status information input at time t, b o is the weight of the output gate, h t is the hidden state at time t.

[0131] The LSTM can be trained by obtaining the observation sequence of driving behavior information, the observation sequence of driving state information and the preset motion constraint model to obtain the probability of occurrence for predicting each preset motion constraint model.

[0132] The LSTM listed above is only used for illustrative purposes. In specific application scenarios, those skilled in the art may also select other artificial neural network models with sequence processing capabilities. For example, the most basic recurrent neural network (RNN) may be used, that is, multiple sequences of driving behavior information and driving state information are input into the RNN to obtain the occurrence probability of each preset motion constraint model output by the RNN.

[0133] S103, determining a mixed probability matrix according to the occurrence probabilities corresponding to all preset motion constraint models.

[0134] The transition probability matrix of the priori Markov model can be determined according to the occurrence probabilities corresponding to all preset motion constraint models; and the mixing probability matrix can be determined according to the transition probability matrix.

[0135] In the case where the preset motion constraint model includes a uniform linear motion model, a uniformly accelerated linear motion model, a constant turning rate and speed motion model, and a constant turning rate and acceleration model, the transition probability matrix refers to the transition probabilities of the four models of the uniform linear motion model, the uniformly accelerated linear motion model, the constant turning rate and speed motion model, and the constant turning rate and acceleration model. Of course, in a specific application process, if the preset motion constraint model also includes other models, the transition probability matrix between all the preset motion constraint models is adaptively calculated.

[0136] The following is a further explanation of the transition probability matrix:

[0137]

[0138] Among them, π is the transition probability matrix, and the four models are sorted in the following order: the transition probability of the uniform linear motion model, the uniformly accelerated linear motion model, the constant turning rate and speed motion model, and the constant turning rate and acceleration model; then the interpretation of each element in the transition probability matrix π is as follows:

[0139] p 11 =P(uniform linear motion → uniform linear motion)

[0140] p 12 =P(uniform linear motion → uniformly accelerated linear motion)

[0141] p 13 = P (uniform linear motion → constant turning rate and speed motion)

[0142] p 14 = P (uniform linear motion → constant turning rate and acceleration motion)

[0143] p 21 =P(uniformly accelerated linear motion → uniformly accelerated linear motion)

[0144] p 22 =P(uniformly accelerated linear motion → uniformly accelerated linear motion)

[0145] p 23 =P (uniformly accelerated linear motion → constant turning rate and speed motion)

[0146] p 24 = P (uniformly accelerated linear motion → constant turning rate and acceleration motion)

[0147] p 31 =P (constant turning rate and speed motion → uniform linear motion)

[0148] p 32 =P (constant turning rate and speed motion → uniformly accelerated linear motion)

[0149] p 33 = P (constant turning rate and speed movement → constant turning rate and speed movement)

[0150] p 34 = P (constant turning rate and speed motion → constant turning rate and acceleration motion)

[0151] p 41 =P (constant turning rate and acceleration motion → uniform linear motion)

[0152] p 42 =P (constant turning rate and acceleration motion → uniformly accelerated linear motion)

[0153] p 43 = P (constant turning rate and acceleration motion → constant turning rate and speed motion)

[0154] p 44 =P(constant turning rate and acceleration motion → constant turning rate and acceleration motion)

[0155] In the process of determining the transition probability matrix of the priori Markov model, the transition frequencies between the four models of uniform linear motion model, uniform accelerated linear motion model, constant turning rate and speed motion model and constant turning rate and acceleration model can be statistically analyzed by analyzing historical data, and then normalized to obtain the transition probability matrix. Alternatively, those skilled in the art can also set a suitable transition probability matrix based on experience.

[0156] S104, obtaining inertial positioning data, and calculating the overall estimated value and overall covariance of the filtering results of each preset motion constraint model according to the inertial positioning data and the mixing probability.

[0157] The overall estimated value is the estimated value of the inertial positioning data, and the overall covariance is used to represent the uncertainty of the estimated value of the inertial positioning data or the statistical characteristics of the error of the estimated value of the inertial positioning data.

[0158] The above-mentioned overall estimated value and overall covariance are the filtering results of the filtering algorithm with state estimation function.

[0159] The filtering algorithm may be a Kalman filter (Kalman Filter) and its extended algorithm, such as an extended Kalman filter (Extended Kalman Filter, EKF), an unscented Kalman filter (Unscented Kalman Filter, UKF), etc.

[0160] The filtering algorithm may also be other filtering algorithms besides Kalman Filter, such as Particle Filter, H-infinity Filter, Bayesian Filter, etc.

[0161] The inertial positioning data includes one or more of yaw rate information, heading angle information and acceleration information. The inertial positioning data can be obtained through one or more of IMU, magnetic sensor and accelerometer.

[0162] Optionally, the inertial positioning data may include yaw rate information and acceleration information, or the inertial positioning data may include heading angle information and acceleration information, or the inertial positioning data may include yaw rate information, heading angle information and acceleration information.

[0163] The inertial positioning data is a quantitative measurement of the filtering algorithm, which is used to correct and update the system state estimation. That is, by inputting the inertial positioning data into the filtering algorithm, the overall estimated value and overall covariance of the filtering algorithm output can be obtained.

[0164] S105 . Correct the inertial positioning data according to the overall estimated value and the overall covariance, and determine the position information of the vehicle according to the corrected inertial positioning data.

[0165] The vehicle's position information can be calculated based on the corrected inertial positioning data in the following way:

[0166] At the moment of GNSS failure, the integral of the corrected inertial positioning data is calculated, and the integral result is the vehicle's position information; or, the vehicle's position information is calculated from the corrected inertial positioning data using an inertial navigation system (INS). Alternatively, those skilled in the art may also select an algorithm that meets the actual situation based on their own experience and actual situation, and calculate the vehicle's position information from the corrected inertial positioning data.

[0167] Optionally, the inertial positioning data is corrected according to the overall estimated value and the overall covariance correction, including: determining an adjustment coefficient of the overall estimated value according to the overall covariance; obtaining the product of the overall estimated value and the adjustment coefficient; correcting the inertial positioning data according to the product to obtain corrected inertial positioning data; wherein the adjustment coefficient is negatively correlated with the uncertainty represented by the overall covariance.

[0168] The higher the correlation between the adjustment coefficient and the uncertainty represented by the population covariance, for example, the larger the absolute value of the correlation coefficient, the lower the degree of correction of the inertial positioning data by the population estimate. That is, only when the accuracy of the population covariance is high, the inertial positioning data is corrected to a greater extent by the population covariance, otherwise the correction of the inertial positioning data by the population covariance is small.

[0169] In the specific application process, when the accuracy of the filtering result is confirmed to be relatively high through experiments, the correlation between the adjustment coefficient and the uncertainty represented by the overall covariance can be set to be low, for example, the absolute value of the correlation coefficient between the two is small; when the accuracy of the filtering result is confirmed to be relatively low through experiments, the correlation between the adjustment coefficient and the uncertainty represented by the overall covariance can be set to be high, for example, the absolute value of the correlation coefficient between the two is large. Those skilled in the art can set the correlation coefficient between the adjustment coefficient and the uncertainty represented by the overall covariance that conforms to the actual situation accordingly, so as to obtain the adjustment coefficient that conforms to the actual situation, and then obtain relatively accurate corrected inertial positioning data, and finally obtain the vehicle's position information.

[0170] Further, the inertial positioning data is corrected according to the product to obtain the corrected inertial positioning data, including: taking the sum of the product and the inertial positioning data as the corrected inertial positioning data, where the sum is a vector sum.

[0171] Optionally, the inertial positioning data is corrected according to the overall estimated value and the overall covariance, including: determining a first weight of the overall estimated value according to the overall covariance; the first weight is negatively correlated with the uncertainty represented by the overall covariance; determining a second weight of the inertial positioning data according to the covariance of the inertial positioning data constituting the matrix; the second weight is negatively correlated with the uncertainty represented by the covariance of the inertial positioning data constituting the matrix; calculating a weighted average value or a weighted sum according to the overall estimated value, the first weight, the inertial positioning data constituting the matrix and the second weight; and using the second weighted average value or the weighted sum as the corrected inertial positioning data.

[0172] The higher the correlation between the first weight and the uncertainty represented by the population covariance, for example, the larger the absolute value of the correlation coefficient, the lower the degree of correction of the inertial positioning data by the population estimate. That is, only when the accuracy of the population covariance is high, the inertial positioning data is corrected to a greater extent by the population covariance, otherwise the correction degree of the inertial positioning data by the population covariance is small.

[0173] In the specific application process, when the accuracy of the filtering result is confirmed to be relatively high through experiments, the correlation between the first weight and the uncertainty represented by the overall covariance can be set to be low, for example, the absolute value of the correlation coefficient between the two is small; when the accuracy of the filtering result is confirmed to be relatively low through experiments, the correlation between the first weight and the uncertainty represented by the overall covariance can be set to be high, for example, the absolute value of the correlation coefficient between the two is large. Those skilled in the art can set the correlation coefficient between the first weight that conforms to the actual situation and the uncertainty represented by the overall covariance accordingly, so as to obtain the first weight that conforms to the actual situation, and then obtain relatively accurate corrected inertial positioning data, and finally obtain the vehicle's position information.

[0174] The vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in the embodiment of the present application can achieve the following technical effects:

[0175] Driving behavior information such as steering wheel angle, vehicle lateral speed and yaw rate belongs to short-term micro information. First, driving state information is determined based on such short-term micro data, and then propagated to driving behavior information. Driving state information such as straight driving, left lane change and right lane change belongs to long-term macro information. Then, in the process of predicting the occurrence probability of each preset motion constraint model based on driving behavior information and driving state information, driving state information can show long-term macro constraint characteristics for vehicle positioning, that is, the occurrence probability of the preset motion constraint model can show long-term macro constraint characteristics for vehicle positioning. This makes the mixed probability of the occurrence probability corresponding to all preset motion constraint models reflect the long-term macro constraint characteristics, and then in the Interacting Multiple Model Filter (IMM), using the mixed probability with such long-term macro constraint characteristics as input can improve the filtering accuracy and stability of the IMM, and then obtain the overall estimation value and overall covariance with higher accuracy and stability.

[0176] In the process of correcting the inertial positioning data with such overall estimates and overall covariance, data with certain long-term macro-constraint characteristics are used to reduce the cumulative error of the IMU, and data with short-term micro-constraint characteristics substituted into the correction process are reduced as much as possible. The high-frequency and short-term precise positioning characteristics provided by the IMU are retained as much as possible. The vehicle position information determined based on such corrected inertial positioning data has higher accuracy and robustness.

[0177] In the above technical solution, the inertial positioning data needs to be corrected according to the overall estimated value and the overall covariance to obtain more accurate corrected inertial positioning data, and in the correction process, the first weight of the overall estimated value and the second weight of the inertial positioning data matrix are used.

[0178] The following is an exemplary description of how to obtain the first weight and the second weight.

[0179] Optionally, determining a first weight of the population estimate according to the population covariance includes:

[0180]

[0181] Among them, w IMM is the first weight, P(k|k) is the population covariance at time k, trace(P(k|k)) is the trace of P(k|k), n is the dimension of P(k|k), (P(k|k)) ii is the i-th diagonal element of P(k|k).

[0182] Optionally, determining the second weight of the inertial positioning data according to the covariance of the matrix formed by the inertial positioning data includes:

[0183]

[0184] Among them, w IMU is the second weight, P IMU is the covariance of the matrix of inertial positioning data, trace(P IMU ) is P IMU traces, m is P IMU The dimension of (P IMU ) ii P IMU The i-th diagonal element of .

[0185] The first weight of the overall estimated value and the second weight of the inertial positioning data can be obtained through the above technical solution.

[0186] Furthermore, the first weight of the overall estimated value is determined according to the overall covariance, which can be optimized as follows:

[0187]

[0188] Among them, w IMM is the first weight, a IMM is the first adjustment coefficient, which can be obtained through experiments, P(k|k) is the population covariance at time k, trace(P(k|k)) is the trace of P(k|k), n is the dimension of P(k|k), (P(k|k)) ii is the i-th diagonal element of P(k|k).

[0189] Correspondingly, determining the second weight of the inertial positioning data according to the covariance of the matrix formed by the inertial positioning data includes:

[0190]

[0191] Among them, w IMU is the second weight, a IMU is the second adjustment coefficient, which can be obtained through experiments, P IMU is the covariance of the matrix of inertial positioning data, trace(P IMU ) is P IMU traces, m is P IMU The dimension of (P IMU ) ii P IMU The i-th diagonal element of .

[0192] The above IMMFor the first weight w IMM The correlation between the reciprocal of trace(P(k|k)) and the above a is adjusted. IMU For the second weight w IMU With trace(P IMU ) is used to adjust the correlation between the inverse of the two. In the application process, a IMM and a IMU Set to 1, and then adjust a separately IMM and a IMU After each adjustment, the calculated vehicle position information is compared with the actual detected vehicle position information. When the error between the calculated vehicle position information and the actual detected vehicle position information is small or minimal, the current a is retained. IMU and / or a IMU The above “small error” means that the prediction error meets the accuracy requirement; the above “minimum error” may be the extreme value or maximum value of the error.

[0193] By adopting the above technical solution, both the first weight and the second weight can be more adapted to actual needs.

[0194] In the above embodiment, the step of "calculating the overall estimated value and the overall covariance of the filtering results of each preset motion constraint model based on the inertial positioning data and the mixing probability" is briefly described. The following uses the filtering algorithm as Kalman Filter to exemplify the step of calculating the overall estimated value and the overall covariance of the filtering results of each preset motion constraint model based on the inertial positioning data and the mixing probability.

[0195] Optionally, the overall estimated value and overall covariance of the filtering results of each preset motion constraint model are calculated according to the inertial positioning data and the mixed probability, including:

[0196] The initial conditions of each preset motion constraint model filtering algorithm are determined as follows:

[0197]

[0198] in, is the state prediction value of model j at time k-1; r is the number of models, r=4; is the state prediction value of model i at time k-1; p j,0 (k-1|k-1) is the covariance prediction value of model j at time k-1; is the predicted value of the state covariance of model i at time k-1; is the mixing probability from model i to model j at time k-1;

[0199] p ij is the transition probability from model i to model j, and is the transition probability matrix element of the prior Markov model; is the occurrence probability of model i at time k-1.

[0200] The population estimate and population covariance are determined as follows:

[0201]

[0202] p j (k|k-1)=φ j (k-1)p j,0 (k-1|k-1)φ j (k-1) T +B j Q j B j T

[0203] K j (k) = p j (k|k-1)H T [Hp j (k|k-1)H T +R] -1

[0204]

[0205] p j (k|k)=[IK j (k)H(k)]p j (k|k-1)

[0206] in, is the state prediction value of model j at time k; φ j (k-1) is the state transfer matrix at time k-1, which is used to describe the state change relationship of the system from time k-1 to time k; is the state prediction value of model j at time k-1, that is, the posterior state estimate at the previous moment; p j (k|k-1) is the covariance prediction value of model j at time k; p j,0 (k-1|k-1) is the covariance prediction value of model j at time k-1, that is, the posterior covariance estimate of the previous moment; B j is the control input matrix, which is used to represent the impact of noise on the system state; Q j is the process covariance matrix, which is used to represent the size of the system process noise; B j T is the control input matrix B j The transpose of K j(k) is the Kalman gain matrix of model j at time k, which determines the update weight of the observation value on the state estimation; H is the observation matrix, which is used to represent the relationship between the state vector and the observation vector; R is the observation noise covariance matrix; is the state update matrix for model j at time k; Z(k) is the observation vector matrix at time k, which is composed of inertial positioning data; is the observed residual; p j (k|k) is the covariance update matrix for model j at time k; I is the identity matrix.

[0207] It should be understood that the steps are only illustrated here by taking the basic Kalman Filter as an example. In the specific application process, when other algorithms such as EKF, UKF, Particle Filter, H-infinity Filter or Bayesian Filter are used, those skilled in the art can adaptably replace the above formulas.

[0208] The model probability is updated through the following likelihood function:

[0209]

[0210] Among them, ∧ j (k) is the likelihood function of model j at time k; is the transpose of the measurement method for model j, is the residual covariance of model j; the model probability is updated as:

[0211]

[0212] Among them, μ j (k) is the posterior probability of model j at time k, μ j (k-1) is the posterior probability of model j at time k-1;

[0213] The population estimates and population covariances are:

[0214]

[0215] in, is the population estimate at time k; P(k|k) is the population covariance at time k.

[0216] The above technical solution can be used to obtain the overall estimated value and the overall covariance. The overall covariance can be used to calculate the first weight, so as to calculate the weighted average value or the weighted sum according to the overall estimated value, the first weight, the matrix composed of the inertial positioning data and the second weight.

[0217] In the aforementioned embodiment, the second weight is obtained according to the covariance of the inertial positioning data constituting the matrix. The following is an exemplary description of the manner of obtaining the covariance of the inertial positioning data constituting the matrix.

[0218] The covariance of the inertial positioning data matrix can be calculated using the following covariance update matrix:

[0219]

[0220] Among them, P IMU (t k ) is the covariance of the updated inertial positioning data matrix, F IMU is the state transfer matrix, P IMU (t k-1 ) is the covariance of the matrix of inertial positioning data known at time k-1, obtained by the update formula of the previous moment, is the state transfer matrix F IMU The transpose of Q IMU is the process noise covariance.

[0221] Where I is the unit matrix, Δt is the update period, which can also be called discrete duration;

[0222] Among them, σ IMU 2 is the variance of process noise, which is used to indicate the intensity of acceleration noise.

[0223] State transition matrix F IMU Used to describe the change of a system between two states. When the inertial positioning data includes yaw rate information and acceleration information, the system can be composed of yaw rate information and acceleration information. For example:

[0224] In the case where the inertial positioning data includes heading angle information and acceleration information, the system may be composed of the heading angle information and the acceleration information, for example:

[0225] In the case where the inertial positioning data includes yaw rate information, heading angle information and acceleration information, the system may be composed of the yaw rate information, heading angle information and acceleration information, for example:

[0226] The following is an exemplary description of determining the position information of the vehicle based on the corrected inertial positioning data.

[0227] When the inertial positioning data includes yaw rate information and acceleration information, the heading angle information is first calculated by integrating the yaw rate information:

[0228]

[0229] in, is the heading angle information at time k, is the yaw angular velocity information detected at time k-1, ω IMU (t k ) is the yaw angular velocity information detected at time k, and Δt is the update period, which can also be called discrete duration.

[0230] When the inertial positioning data includes heading angle information and acceleration information, the heading angle information at time k can be directly detected.

[0231] In the case where the inertial positioning data includes yaw rate information, heading angle information, and acceleration information, the first heading angle information can be calculated by integrating the yaw rate information, and then the final heading angle information can be determined according to the first heading angle information and the second heading angle information obtained by detection. For example, the average of the first heading angle information and the second heading angle information can be used as the final heading angle information, or the weighted average of the first heading angle information and the second heading angle information can be used as the final heading angle information.

[0232] Then convert the acceleration information detected by the IMU into acceleration in the world coordinate system:

[0233]

[0234] Among them, a w (t k ) is the acceleration information of the vehicle at time k in the world coordinate system, is the heading angle information at time k, a IMU (t k ) is the acceleration information detected at time k, is the heading angle information at time k The resulting rotation matrix is:

[0235]

[0236] Update the speed information:

[0237] v(t k+1 )=v(t k )+a w (t k )Δt

[0238] Among them, v(t k+1 ) is the updated speed information, v(t k) is the known speed information at the current moment, obtained through the update formula of the previous moment, a w (t k ) is the acceleration information of the vehicle at the current time k in the world coordinate system, and Δt is the update period, which can also be called discrete duration.

[0239] To update the location information:

[0240] p(t k+1 )=p(t k )+v(t k+1 )Δt

[0241] Among them, p(t k+1 ) is the updated position information, p(t k ) is the known position information at the current moment, obtained by the update formula at the previous moment, the position information at the moment when GNSS starts to fail p(0)=0, v(t k+1 ) is the updated speed information, Δt is the update period, which can also be called discrete duration.

[0242] After GNSS fails, the most recently updated location information is the vehicle's location information.

[0243] The inertial positioning data involved in the above-mentioned embodiments for calculating the vehicle position information are all corrected inertial positioning data, that is, the yaw rate information in the above-mentioned embodiments is the corrected yaw rate information, the heading angle information in the above-mentioned embodiments is the corrected heading angle information, and the acceleration information in the above-mentioned embodiments is the corrected acceleration information.

[0244] The above specific process of calculating the vehicle position information is only for illustrative purposes. In specific application scenarios, those skilled in the art can select a specific algorithm that suits the actual situation based on their own experience, and will not be described in detail here.

[0245] Figure 2 Schematic diagram of a vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation provided by an embodiment of the present application. The vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation can be implemented in the form of software, hardware or a combination of the two.

[0246] Combination Figure 2 As shown, the vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation includes a driving state prediction module 21, a motion model probability prediction module 22, a first filtering module 23, a second filtering module 24 and a correction module 25.

[0247] The driving state prediction module 21 is used to obtain driving behavior information and determine driving state information according to the driving behavior information; the driving behavior information includes: steering wheel angle, vehicle lateral speed and yaw angular velocity; the driving state information includes: straight driving, left lane change and right lane change;

[0248] The motion model probability prediction module 22 is used to predict the occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information; the preset motion constraint models include: uniform linear motion model, uniform accelerated linear motion model, constant turning rate and speed motion model and constant turning rate and acceleration model;

[0249] The first filtering module 23 is used to determine a mixed probability matrix according to the occurrence probabilities corresponding to all preset motion constraint models;

[0250] The second filtering module 24 is used to obtain inertial positioning data, and calculate the overall estimation value and overall covariance of the Kalman filter algorithm according to the inertial positioning data and the mixed probability; the inertial positioning data includes one or more of yaw angular velocity information, heading angle information and acceleration information;

[0251] The correction module 25 is used to correct the inertial positioning data according to the overall estimated value and the overall covariance, and determine the position information of the vehicle according to the corrected inertial positioning data.

[0252] The vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation also has the technical effect of improving positioning accuracy. The specific analysis process refers to the aforementioned analysis of the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation, which will not be repeated here.

[0253] Optionally, the correction module 25 includes:

[0254] A first determining unit is used to determine a first weight of the overall estimated value according to the overall covariance; the first weight is negatively correlated with the uncertainty represented by the overall covariance;

[0255] A second determination unit is used to determine a second weight of the inertial positioning data according to the covariance of the inertial positioning data constituting the matrix; the second weight is negatively correlated with the uncertainty represented by the covariance of the inertial positioning data constituting the matrix;

[0256] A first calculation unit, configured to calculate a weighted average value or a weighted sum according to the overall estimated value, the first weight, a matrix formed by inertial positioning data, and the second weight;

[0257] The third determination unit is used to use the second weighted average value or the weighted sum as the corrected inertial positioning data.

[0258] Optionally, the first determining unit is specifically configured to execute the following formula:

[0259]

[0260] Among them, w IMM is the first weight, P(k|k) is the population covariance at time k, trace(P(k|k)) is the trace of P(k|k), n is the dimension of P(k|k), (P(k|k)) ii is the i-th diagonal element of P(k|k).

[0261] Optionally, the second determining unit is specifically configured to execute the following formula:

[0262]

[0263] Among them, w IMU is the second weight, P IMU is the covariance of the matrix of inertial positioning data, trace(P IMU ) is P IMU traces, m is P IMU The dimension of (P IMU ) ii P IMU The i-th diagonal element of .

[0264] The second filtering module 54 is specifically used for:

[0265] The initial conditions of each preset motion constraint model filtering algorithm are determined as follows:

[0266]

[0267] in, is the state prediction value of model j at time k-1; r is the number of models, r=4; is the state prediction value of model i at time k-1; p j,0 (k-1|k-1) is the covariance prediction value of model j at time k-1; is the predicted value of the state covariance of model i at time k-1; is the mixing probability from model i to model j at time k-1;

[0268] p ij is the transition probability from model i to model j, and is the transition probability matrix element of the prior Markov model; is the occurrence probability of model i at time k-1;

[0269] The population estimate and population covariance are determined as follows:

[0270]

[0271] p j (k|k-1)=φ j (k-1)p j,0 (k-1|k-1)φ j (k-1) T +B j Q j B j T

[0272] K j (k) = p j (k|k-1)H T [Hp j (k|k-1)H T +R] -1

[0273]

[0274] p j (k|k)=[IK j (k)H(k)]p j (k|k-1)

[0275] in, is the state prediction value of model j at time k; φ j (k-1) is the state transfer matrix at time k-1, which is used to describe the state change relationship of the system from time k-1 to time k; p j (k|k-1) is the covariance prediction value of model j at time k; B j is the control input matrix, which is used to represent the impact of noise on the system state; Q j is the process covariance matrix, which is used to represent the size of the system process noise; K j (k) is the Kalman gain matrix for model j at time k; H is the observation matrix; R is the observation noise covariance matrix; is the state update matrix for model j at time k; Z(k) is the observation vector matrix at time k, which is composed of inertial positioning data; is the observed residual; p j (k|k) is the covariance update matrix for model j at time k; I is the identity matrix;

[0276] The model probability is updated through the following likelihood function:

[0277]

[0278] Among them, ∧ j(k) is the likelihood function of model j at time k; is the transpose of the measurement method for model j, is the residual covariance of model j;

[0279] The model probability is updated as:

[0280]

[0281] Among them, μ j (k) is the posterior probability of model j at time k, μ j (k-1) is the posterior probability of model j at time k-1;

[0282] The population estimates and population covariances are:

[0283]

[0284] in, is the population estimate at time k; P(k|k) is the population covariance at time k.

[0285] Optionally, the driving state prediction module 21 includes:

[0286] A first obtaining unit, used to obtain driving behavior information of a plurality of consecutive moments before a current moment to form an observation sequence of the driving behavior information;

[0287] The fourth determining unit is used to determine the driving state information according to the observation sequence of the driving behavior information.

[0288] Optionally, the motion model probability prediction module 22 includes:

[0289] A second obtaining unit, used to obtain multiple sequences of driving behavior information and driving state information according to the order of time stamps;

[0290] The fifth determination unit is used to input multiple sequences of driving behavior information and driving state information into an artificial neural network model with sequence processing capability to obtain the occurrence probability of multiple preset motion constraint models output by the artificial neural network model with sequence processing capability.

[0291] Optionally, the first filtering module 23 includes:

[0292] A sixth determining unit, configured to determine a transition probability matrix of a priori Markov model according to occurrence probabilities corresponding to all preset motion constraint models;

[0293] The seventh determining unit is used to determine the mixing probability matrix according to the transition probability.

[0294] Figure 3It is a schematic diagram of a specific implementation process of a vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in an embodiment of the present application.

[0295] Combination Figure 3 As shown, the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation includes the following five steps performed in sequence: a driving behavior information acquisition step, a driving behavior state recognition step, a motion constraint model prediction step, a key motion parameter estimation step, and an auxiliary positioning step.

[0296] In the driving behavior information acquisition step, the vehicle steering wheel angle, vehicle lateral speed and vehicle yaw angular velocity are obtained through the vehicle-mounted sensor, and the steering wheel angle, vehicle lateral speed and yaw angular velocity form a time series {O}, where O is the observation sequence of the driving behavior information, and the driving behavior information includes the vehicle steering wheel angle, vehicle lateral speed and vehicle yaw angular velocity.

[0297] In the driving state recognition step, the HMM is first trained using the training data collected from the actual vehicle test, and then the time series {O} obtained in the driving behavior information acquisition step is input into the trained HMM. The HMM recognizes the driving state information online, and the output result of the HMM is the vehicle going straight, the vehicle changing lanes to the left, or the vehicle changing lanes to the right.

[0298] In the motion constraint model prediction, the LSTM is first trained offline using the data obtained from the actual vehicle test. Then the time series {O} in the driving behavior information acquisition step and the output results of the HMM in the driving state recognition step are input into the LSTM. The LSTM outputs the probability of the uniform linear motion model, the probability of the uniform accelerated linear motion model, the probability of the constant turning rate and speed motion model, and the probability of the constant turning rate and acceleration model.

[0299] In the key motion parameter estimation step, the four LSTM outputs: uniform linear motion model probability, uniform accelerated linear motion model probability, constant turning rate and speed motion model probability, and constant turning rate and acceleration model probability constitute the constraint model of the vehicle navigation system. Combined with the yaw rate information obtained by the IMU, the heading angle information obtained by the magnetic sensor, and the acceleration information obtained by the accelerometer, the overall estimation value is obtained through the improved interactive multi-model. The overall estimation value includes: the optimal estimation value of the yaw rate, the optimal estimation value of the heading angle, and the optimal estimation value of the acceleration.

[0300] In the auxiliary positioning step, the overall estimation value obtained in the key motion parameter estimation step is used to assist in correcting the vehicle positioning when the vehicle GNSS fails for a short time, taking the current position obtained by the IMU as the origin.

[0301] In some embodiments, a vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation includes a processor and a memory storing program instructions. The processor is configured to execute the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in the aforementioned embodiments when executing the program instructions.

[0302] Figure 4 Schematic diagram of a vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation provided by an embodiment of the present application. Figure 4 As shown, the vehicle positioning device 40 under short-term GNSS failure based on driving behavior and inertial navigation includes:

[0303] The processor 41 and the memory 42 may also include a communication interface 43 and a bus 44. The processor 41, the communication interface 43, and the memory 42 may communicate with each other through the bus 44. The communication interface 43 may be used for information transmission. The processor 41 may call the logic instructions in the memory 42 to execute the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation provided in the aforementioned embodiment.

[0304] In addition, the logic instructions in the memory 42 can be implemented in the form of software functional units and can be stored in a computer-readable storage medium when sold or used as an independent product.

[0305] The memory 42 is a computer-readable storage medium that can be used to store software programs and computer executable programs, such as program instructions / modules corresponding to the methods in the embodiments of the present application. The processor 41 executes functional applications and data processing by running the software programs, instructions, and modules stored in the memory 42, that is, implementing the methods in the above method embodiments.

[0306] The memory 42 may include a program storage area and a data storage area, wherein the program storage area may store an operating system and an application required for at least one function; the data storage area may store data created according to the use of the terminal device, etc. In addition, the memory 42 may include a high-speed random access memory and may also include a non-volatile memory.

[0307] In some embodiments, the vehicle positioning system includes the vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation provided in the aforementioned embodiments.

[0308] The present application embodiment provides a computer-readable storage medium storing computer-executable instructions, wherein the computer-executable instructions are configured as follows:

[0309] Obtain driving behavior information, and determine driving state information according to the driving behavior information; the driving behavior information includes: steering wheel angle, vehicle lateral speed and yaw angular velocity; the driving state information includes: straight driving, left lane change and right lane change;

[0310] According to the driving behavior information and the driving state information, the occurrence probability of each preset motion constraint model is predicted; the preset motion constraint models include: uniform linear motion model, uniform accelerated linear motion model, constant turning rate and speed motion model and constant turning rate and acceleration model;

[0311] Determine a mixed probability matrix according to the occurrence probabilities corresponding to all preset motion constraint models;

[0312] Obtaining inertial positioning data, and calculating overall estimated values ​​and overall covariance of filtering results of each preset motion constraint model according to the inertial positioning data and the mixed probability; the inertial positioning data includes one or more of yaw angular velocity information, heading angle information, and acceleration information;

[0313] The inertial positioning data is corrected according to the overall estimated value and the overall covariance, and the position information of the vehicle is determined according to the corrected inertial positioning data.

[0314] The above-mentioned computer-readable storage medium may be a transient computer-readable storage medium.

[0315] The technical solution of the embodiment of the present application can be embodied in the form of a software product, which is stored in a storage medium and includes one or more instructions for enabling a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method in the embodiment of the present application. The aforementioned storage medium may be a non-transient storage medium, including: a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, and other media that can store program codes, or a transient storage medium.

[0316] The above description and the accompanying drawings fully illustrate the embodiments of the present application so that those skilled in the art can practice them. Other embodiments may include structural, logical, electrical, process and other changes. The embodiments represent only possible changes. Unless explicitly required, individual components and functions are optional, and the order of operations may vary. Parts and features of some embodiments may be included in or replace parts and features of other embodiments. Moreover, the words used in this application are only used to describe the embodiments and are not used to limit the claims. As used in the description of the embodiments and the claims, unless the context clearly indicates otherwise, the singular forms of "a", "an" and "the" are intended to include plural forms as well. In addition, when used in this application, the term "comprise" and its variants "comprises" and / or including (comprising) refer to the existence of stated features, wholes, steps, operations, elements, and / or components, but do not exclude the existence or addition of one or more other features, wholes, steps, operations, elements, components and / or these groups. In the absence of further restrictions, the elements defined by the sentence "comprising a ..." do not exclude the existence of other identical elements in the process, method or device comprising the elements. In this article, each embodiment may focus on the differences from other embodiments, and the same or similar parts between the embodiments may refer to each other. For the methods, products, etc. disclosed in the embodiments, if they correspond to the method part disclosed in the embodiments, then the relevant parts can refer to the description of the method part.

[0317] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented with electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software may depend on the specific application and design constraints of the technical solution. Technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of the embodiments of the present application. Technicians can clearly understand that for the convenience and simplicity of description, the specific working process of the above-described system, device and unit can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.

[0318] In the embodiments disclosed herein, the disclosed methods and products (including but not limited to devices, equipment, etc.) can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of units can be only a logical function division, and there can be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between each other shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms. The units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, which may be located in one place or distributed on multiple network units. Some or all of the units can be selected according to actual needs to implement this embodiment. In addition, each functional unit in the embodiment of the present application can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit.

[0319] The flow chart and block diagram in the accompanying drawings show the possible architecture, function and operation of the system, method and computer program product according to the embodiment of the present application. In this regard, each square box in the flow chart or block diagram can represent a part of a module, program segment or code, and a part of a module, program segment or code includes one or more executable instructions for realizing the logical function of the specification. In some implementations as replacements, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two continuous square boxes can actually be executed substantially in parallel, and they can also be executed in the opposite order sometimes, which can depend on the functions involved. Each square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be realized by a dedicated hardware-based system that performs the function or action of the specification, or can be realized by a combination of special-purpose hardware and computer instructions.

Claims

1. A vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation, characterized in that: include: Obtaining driving behavior information, and determining driving state information according to the driving behavior information; The driving behavior information includes: steering wheel angle, vehicle lateral speed and yaw rate; the driving state information includes: straight driving, left lane change and right lane change; According to the driving behavior information and the driving state information, predict the occurrence probability of each preset motion constraint model; the preset motion constraint model includes: a uniform linear motion model, a uniformly accelerated linear motion model, a constant turning rate and speed motion model, and a constant turning rate and acceleration model; Determine a mixed probability matrix according to the occurrence probabilities corresponding to all the preset motion constraint models; Obtaining inertial positioning data, and calculating an overall estimated value and an overall covariance of filtering results of each of the preset motion constraint models according to the inertial positioning data and the mixed probability matrix; the inertial positioning data includes one or more of yaw angular velocity information, heading angle information, and acceleration information; The inertial positioning data is corrected according to the overall estimated value and the overall covariance, and the position information of the vehicle is determined according to the corrected inertial positioning data.

2. The vehicle positioning method according to claim 1, characterized in that: The inertial positioning data is corrected according to the overall estimated value and the overall covariance, including: Determining a first weight of the population estimate according to the population covariance; the first weight is negatively correlated with the uncertainty represented by the population covariance; Determining a second weight of the inertial positioning data according to the covariance of the matrix formed by the inertial positioning data; the second weight is negatively correlated with the uncertainty represented by the covariance of the matrix formed by the inertial positioning data; Calculate a weighted average value or a weighted sum according to the overall estimated value, the first weight, the inertial positioning data matrix, and the second weight; The weighted average value or the weighted sum is used as the corrected inertial positioning data.

3. The vehicle positioning method according to claim 2, characterized in that: Determining a first weight of the population estimate according to the population covariance includes: in, is the first weight, For the moment The population covariance of for traces, , for The dimension of for No. diagonal elements; The second weight of the inertial positioning data is determined according to the covariance of the matrix formed by the inertial positioning data, including: in, is the second weight, is the covariance of the matrix of inertial positioning data, for traces, , for The dimension of for No. diagonal elements.

4. The vehicle positioning method according to claim 1, characterized in that: Calculating the overall estimated value and overall covariance of the filtering results of each of the preset motion constraint models according to the inertial positioning data and the mixed probability matrix, including: The initial conditions of each of the preset motion constraint model filtering algorithms are determined in the following manner: in, For the moment Model The predicted value of the state; is the number of models, ; For the moment Model The predicted value of the state; For the moment Model The covariance prediction value of For the moment Model The state covariance prediction value of ; For the moment From the model To Model The mixing probability of ; From the model To Model The transition probability is the transition probability matrix element of the prior Markov model; For the moment Model The probability of occurrence; The population estimate and the population covariance are determined as follows: in, For the moment Model The predicted value of the state; For the moment The state transition matrix is ​​used to describe the system from time To time The state change relationship; For the moment Model The predicted value of the state; For the moment Model The covariance prediction value of For the moment Model The covariance prediction value of is the control input matrix, which is used to represent the impact of noise on the system state; The control input matrix The transpose of is the process covariance matrix, which is used to represent the size of the system process noise; For the moment Model The Kalman gain matrix of is the observation matrix; is the observation noise covariance matrix; For the moment Model The state update matrix of For the moment The observation vector matrix is ​​composed of the inertial positioning data; is the observed residual; For the moment Model The covariance update matrix of ; is the identity matrix; The model probability is updated through the following likelihood function: in, For the moment Model The likelihood function of ; It is a model The transposition of the measurement method, It is a model The residual covariance of The model probability is updated as: in, For the moment Model The posterior probability of For the moment Model The posterior probability of The population estimates and population covariances are: in, For the moment The overall estimated value of For the moment The population covariance of .

5. The vehicle positioning method according to any one of claims 1 to 4, characterized in that: Determining driving state information according to the driving behavior information includes: Obtain driving behavior information of multiple consecutive moments before the current moment to form an observation sequence of driving behavior information; Driving state information is determined according to the observation sequence of the driving behavior information.

6. The vehicle positioning method according to any one of claims 1 to 4, characterized in that: Predicting the occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information includes: Obtaining multiple sequences of the driving behavior information and the driving state information according to the order of the timestamps; Multiple sequences of the driving behavior information and the driving state information are input into an artificial neural network model with sequence processing capability to obtain occurrence probabilities of multiple preset motion constraint models output by the artificial neural network model with sequence processing capability.

7. The vehicle positioning method according to any one of claims 1 to 4, characterized in that: Determining a mixed probability matrix according to the occurrence probabilities corresponding to all the preset motion constraint models includes: Determine the transition probability matrix of the prior Markov model according to the occurrence probabilities corresponding to all the preset motion constraint models; The mixing probability matrix is ​​determined according to the transition probability matrix.

8. A vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation, characterized in that: include: A driving state prediction module, used to obtain driving behavior information and determine driving state information according to the driving behavior information; The driving behavior information includes: steering wheel angle, vehicle lateral speed and yaw rate; the driving state information includes: straight driving, left lane change and right lane change; A motion model probability prediction module, used to predict the occurrence probability of each preset motion constraint model according to the driving behavior information and the driving state information; the preset motion constraint models include: a uniform linear motion model, a uniformly accelerated linear motion model, a constant turning rate and speed motion model, and a constant turning rate and acceleration model; A first filtering module, used for determining a mixed probability matrix according to the occurrence probabilities corresponding to all the preset motion constraint models; A second filtering module is used to obtain inertial positioning data, and calculate the overall estimated value and overall covariance of the filtering results of each of the preset motion constraint models according to the inertial positioning data and the mixed probability matrix; the inertial positioning data includes one or more of yaw angular velocity information, heading angle information and acceleration information; The correction module is used to correct the inertial positioning data according to the overall estimated value and the overall covariance, and determine the position information of the vehicle according to the corrected inertial positioning data.

9. A vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation, comprising a processor and a memory storing program instructions, characterized in that: The processor is configured to execute the vehicle positioning method under short-term GNSS failure based on driving behavior and inertial navigation as described in any one of claims 1 to 7 when executing the program instructions.

10. A vehicle positioning system, characterized in that: include: A vehicle positioning device under short-term GNSS failure based on driving behavior and inertial navigation as described in claim 8 or 9.

Images