A vehicle positioning method based on precision evaluation

Abstract

The present invention discloses a vehicle positioning method based on precision evaluation, including: S1, using a number of particles to simulate the motion pose of the vehicle, and calculating the weights of the particles according to the road sign conditions of the map road signs sensed by the vehicle's sensors, screening out the particle with the largest weight as the optimal particle, and taking the position value of the optimal particle as the position value of the vehicle; S2, calculating the predicted position value of the vehicle through probability; S3, calculating the difference between the position value of the vehicle obtained in S1 and the predicted position value of the vehicle obtained in S2, and taking this difference as the positioning precision evaluation value E t ; S4, performing resampling, selecting an appropriate number of particles and returning to S1 for estimating the vehicle position. The method of the present invention aims at the problems of particle degradation and particle decline of the particle filter algorithm, and performs particle resampling according to the positioning precision, so as to avoid the problems of particle degradation and particle decline, and improve the positioning precision and positioning efficiency.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving evaluation, and specifically relates to a vehicle positioning method based on accuracy evaluation. Background Art

[0002] The particle filter algorithm is widely used in the positioning system of autonomous driving vehicles because it is easy to implement and has a certain degree of robustness. However, the particle filter algorithm has certain limitations. After running the algorithm for a period of time, problems of particle degradation and particle decline will occur, resulting in a decrease in positioning accuracy. Therefore, how to judge particle degradation and supplement particles has become an urgent problem to be solved in the particle filter algorithm. Summary of the Invention

[0003] In order to solve the problems of the prior art, the present invention proposes a vehicle positioning method based on accuracy evaluation that can judge particle decline and supplement particles to improve positioning accuracy.

[0004] To this end, the present invention adopts the following technical solutions:

[0005] A vehicle positioning method based on accuracy evaluation, comprising the following steps:

[0006] S1. Use a number of particles to simulate the motion pose of the vehicle, and calculate the weights of the particles according to the situation of the map road signs sensed by the vehicle's sensors. Select the particle with the largest weight as the optimal particle, and use the position value of the optimal particle as the position value of the vehicle;

[0007] S2. Calculate the probability of the vehicle moving to each position in space according to the vehicle's motion mode, select the largest probability among them, and use the spatial position value corresponding to the largest probability as the predicted position value of the vehicle;

[0008] S3. Calculate the difference between the position value of the vehicle obtained in S1 and the predicted position value of the vehicle obtained in S2, and use this difference as the positioning accuracy evaluation value E t ;

[0009] S4. Resample the particles according to the positioning accuracy evaluation value E t select an appropriate number of particles, and return to S1 to estimate the vehicle position at the next moment.

[0010] S1 includes the following steps:

[0011] S11. Initialization of the particle filter algorithm:

[0012] First, establish a plane rectangular coordinate system on the area map to be evaluated, and divide the area map into L*M map squares of the same size; initialize the total number of particles to N;

[0013] S12. Calculate the position of the particle at time t:

[0014] Calculate the position values of all the particles at time t;

[0015] S13. Calculate the weights of the particles:

[0016] Calculate the weights of all the particles at time t through the following formula

[0017]

[0018] where w n,t is the weight of particle n at time t, n is the particle number, n ∈ [1, N]; p t represents the number of the map road signs sensed by the sensor of the vehicle at the position of particle n at time t, p t ∈ [1, K], K is the total number of the map road signs on the map; o i is the distance value from the origin of the plane rectangular coordinate system to map road sign i; μ t,i represents the distance value measured by the sensor of the vehicle at the position of particle n to map road sign i at time t, i is the map road sign number, i ∈ [1, p t ; ∑ represents the sensor measurement covariance matrix, and the ∑ is obtained by calibrating the vehicle sensor;

[0019] S14. Calculate the optimal particle position:

[0020] Among the weights of all the particles at time t, select the particle with the largest weight as the optimal particle, and represent the optimal particle as Pf opt ; its corresponding position is the optimal position at time t, represented as And use the optimal position as the position of the vehicle.

[0021] Step S12 includes the following sub-steps:

[0022] (1) When the movement direction θ of the particle remains unchanged:

[0023] x t = x t-1 + v * Δt * cos(θ t-1 );

[0024] y t = y t-1 + v * Δt * sin(θ t-1 );

[0025] θ t = θ t-1 ;

[0026] (2) When the movement direction θ of the particle changes:

[0027]

[0028] Wherein, x t-1 is the value of the particle in the x-axis direction at time t-1, and x t is the value of the particle in the x-axis direction at time t; y t-1 is the value of the particle in the y-axis direction at time t-1, and y t is the value of the particle in the y-axis direction at time t; v is the velocity value of the particle; θ t-1 is the azimuth value of the particle at time t-1, and θ t is the azimuth value of the particle at time t, is the azimuth change rate of the particle; the v and are both obtained by simulating the particle filter algorithm; the Δt is the movement duration of the particle from time t-1 to time t.

[0029] S2 includes the following steps:

[0030] S21, Initialize the vehicle position:

[0031] S22, Predict the vehicle position according to the vehicle movement mode, including the following steps:

[0032] S221, First, calculate the probability that the vehicle is on the map grid j at time t-1 as: Wherein, j is the number of the map grid, j ∈ [1, L*M];

[0033] Then, calculate the probability that the vehicle is on the map grid k at time t as: Wherein, k is the number of the map grid, k ∈ [1, L*M];

[0034] Finally, according to the total probability formula, obtain the probability that the vehicle is on the map grid k at time t There is:

[0035]

[0036] Wherein, ~N represents obeying the Gaussian distribution; σ s represents the vehicle movement variance, and the σ s is obtained by calibrating the vehicle, s is the distance traveled by the vehicle in the Δt time;

[0037] S222, Calculate the probability p(z t |X t |X t ) that the vehicle observes the map road sign at the X

[0038]

[0039] Among them, represents the total *q map road signs that can be observed by the vehicle at the X t position obtained through the particle filter algorithm, and *l represents the number of the map road sign that can be observed by the vehicle at the X t position obtained through the particle filter algorithm; t The number of the map road sign; represents the sensor measurement variance of the vehicle, which is obtained by calibrating the vehicle;

[0040] represents the q t map road sign markers observed by the vehicle, where q t ∈ [1, K], and the l represents the number of the map road sign actually observed by the vehicle, l ∈ [1, q t ;

[0041] S223. Calculate the position probability data set of the vehicle at time t:

[0042] At time t, the position probability of the vehicle is obtained by the following formula:

[0043]

[0044] Obtain the position probability data set of the vehicle at time t

[0045] S224. Screen out the maximum value in the position probability data set of the vehicle and obtain the predicted position of the vehicle here

[0046] S3 specifically is:

[0047] Calculate the difference between the position of the vehicle at time t and the predicted position of the vehicle as the positioning accuracy evaluation value E t :

[0048]

[0049] S4 specifically includes the following steps

[0050] Compare E at time t t with E at the previous time t - 1 t-1 ;

[0051] When E t < E t-1 , select the particles with the optimal and sub-optimal particle weights, discard the remaining particles, the total number of particles decreases, and return to S12 to re-evaluate the vehicle positioning accuracy;

[0052] When Et >E t-1 When it is >E, copy the particle with the optimal particle weight until the number of particles reaches N, return to S12, and re-evaluate the vehicle positioning accuracy.

[0053] An electronic device, comprising a processor and a memory, wherein: the memory is used to store programs; the processor is coupled to the memory and is used to call the programs in the memory so that the electronic device executes the method according to any one of claims 1-6.

[0054] A computer-readable storage medium, comprising instructions, which when run on a computer, cause the computer to execute the method according to any one of claims 1-6.

[0055] A computer program product containing instructions, which when run on a computer, cause the computer to execute the method according to any one of claims 1-6.

[0056] Compared with the prior art, the present invention has the following beneficial effects:

[0057] The method of the present invention can improve the positioning accuracy by evaluating the positioning accuracy of the particle filter positioning algorithm; and when the positioning accuracy can meet the requirements, appropriately reduce the number of particles, thereby improving the overall operation efficiency of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 is a flowchart of the method of the present invention. DETAILED DESCRIPTION

[0059] The technical solution of the present invention will be further described in detail below with reference to the drawings and embodiments.

[0060] See Figure 1 , the vehicle positioning method based on accuracy evaluation of the present invention includes the following steps:

[0061] S1, calculate the position of the optimal particle:

[0062] Initialize the particle filter algorithm, which can be set according to the actual autonomous driving environment. The number of particles is the key to the particle filter algorithm. If the number of particles is set too large, the positioning accuracy will be improved, but the real-time performance of the operation will be reduced; if it is set too small, the positioning accuracy will decrease and the positioning of the driverless vehicle cannot be achieved.

[0063] In addition, the initial position of the particle needs to be set, and the initial position of the vehicle can be used as the initial position of the particle.

[0064] The specific initialization includes the following steps:

[0065] S11, Initialization of the particle filter algorithm:

[0066] First, establish a Cartesian coordinate system on the area map to be evaluated, and divide the area map into L*M map grids of the same size.

[0067] Then, initialize the total number of particles to N;

[0068] Finally, represent the initial point of each particle as Pf(U), where U represents the coordinate value of the particle, U = (x, y), x represents the value of the particle in the x-axis direction; y represents the value of the particle in the y-axis direction.

[0069] S12, Calculate the position of the particle at time t:

[0070] Simulate the position of the vehicle at time t with the position of the particle at time t. According to the vehicle motion equation, calculate the new position value of each particle, and obtain the position of the particle after a movement duration of Δt. Δt is the movement duration of the vehicle from time t - 1 to time t.

[0071] The vehicle motion equation is as follows;

[0072] (1) When the motion direction θ of the vehicle (particle) remains unchanged:

[0073] x t = x t-1 + v * Δt * cos(θ t-1 );

[0074] y t = y t-1 + v * Δt * sin(θ t-1 );

[0075] θ t = θ t-1 ;

[0076] (2) When the motion direction θ of the vehicle (particle) changes:

[0077]

[0078] Among them, x t-1 is the value of the particle in the x-axis direction at time t - 1, x t is the value of the particle in the x-axis direction at time t; y t-1 is the value of the particle in the y-axis direction at time t - 1, y t is the value of the particle in the y-axis direction at time t; v is the speed value of the particle; θ t-1 is the azimuth value of the particle at time t - 1, θ t is the azimuth value of the particle at time t, is the azimuth change rate of the particle. v and All are obtained by simulating through the particle filter algorithm.

[0079] S13. Calculate the weights of the particles:

[0080] Calculate the weights of all particles at time t through the following formula.

[0081]

[0082] where w n,t is the weight of particle n at time t, n is the particle number, n ∈ [1, N]; p t represents the number of map road signs sensed by the sensors of the vehicle at the position of this particle at time t, p t ∈ [1, K], K is the total number of map road signs on the map, and the map road signs are set according to the actual situation of the map, such as landmark buildings in the map; o i is the distance value from the origin of the coordinate system to map road sign i; μ t,i represents the distance value from the vehicle to map road sign i measured by the vehicle sensor at time t, i is the number of the map road sign, i ∈ [1, p t ; ∑ represents the sensor measurement covariance matrix, which is obtained by calibrating the vehicle's sensors.

[0083] S14. Calculate the optimal particle position

[0084] Among the weights of all particles obtained in S13 at time t, select the particle with the largest weight as the optimal particle, and represent the optimal particle as Pf opt ; its corresponding position is the optimal position at time t, represented as And use the optimal position as the position of the vehicle.

[0085] S2. Calculate the predicted position of the vehicle:

[0086] S21. Initialize the vehicle position:

[0087] The vehicle position is represented as Pa(X); X represents the coordinate value of the vehicle;

[0088] S22. Predict the position of the vehicle according to the vehicle's motion mode, including the following steps:

[0089] S221. The probability of the vehicle in map grid k at time t:

[0090] First, calculate the probability of the vehicle on map grid j at time t - 1 as:

[0091] where j is the number of the map grid, j ∈ [1, L * M];

[0092] Then, calculate the probability that the vehicle is on map grid \(k\) at time \(t\) as:

[0093] where \(k\) is the number of the map grid, and \(k\in[1,L\times M]\);

[0094] Finally, according to the total probability formula, obtain the probability that the vehicle is on map grid \(k\) at time \(t\) There is:

[0095]

[0096] where, denotes subject to a Gaussian distribution with an expectation of The vehicle motion variance is \(\sigma\) s \(\sigma\) s is obtained by calibrating the vehicle, and \(s\) is the distance traveled by the vehicle in \(\Delta t\) time.

[0097] S222. Calculate the probability of the map road signs observed by the vehicle at its position \(X\) at time \(t\): t At time \(t\), assume the vehicle actually observes \(q\)

[0098] map road signs, denoted as t \(q\) \(q\) t \(\in[1,K]\); \(z\) represents the coordinate value of the map road sign, and \(l\) represents the number of the map road sign actually observed by the vehicle, \(l\in[1,q\) t ;

[0099] Then at time \(t\), the probability \(p(z\) t |\(X\) t |\(X\) t ) of the map road signs observed by the vehicle at position \(X\) is:

[0100]

[0101] where, denotes the total of \(q\) t map road signs that the vehicle can observe at position \(X\) obtained by the particle filter algorithm, and \(*l\) represents the number of the map road sign that the vehicle can observe at position \(X\) obtained by the particle filter algorithm; t t denotes the sensor measurement variance of the vehicle, which is obtained by calibrating the vehicle.

[0102]

[0102] S223. Calculate the position probability data set of the vehicle at time \(t\):

[0103] At time \(t\), the position probability of the vehicle is obtained by the following formula:

[0104]

[0105] Obtain the position probability dataset of the vehicle at time t

[0106] S224. Screen out the maximum value from the position probability dataset of the vehicle And obtain the predicted position of the vehicle here

[0107] S3. Calculate the positioning accuracy evaluation value:

[0108] Calculate the difference between the position of the vehicle at time t and the predicted position of the vehicle as the positioning accuracy evaluation value E t :

[0109]

[0110] S4. Particle resampling:

[0111] According to the calculation result of S3, evaluate the particle positioning accuracy, and perform sampling according to the positioning accuracy. At the same time, the number of particles is not fixed. While taking into account both accuracy and operation efficiency, achieve a good positioning effect, which specifically includes the following steps:

[0112] Take the E at the current time t t And the E at the previous time t - 1 t-1 Compare them;

[0113] When E t < E t-1 It indicates that the vehicle positioning accuracy at time t has improved. Select the particles with the optimal and sub - optimal particle weights, discard the remaining particles, the total number of particles decreases, return to S12, and re - evaluate the vehicle positioning accuracy at the next time;

[0114] When E t > E t-1 It indicates that the vehicle positioning accuracy at time t is worse than that at time t - 1. It is necessary to increase the number of particles. At this time, copy the particle with the optimal particle weight until the number of particles reaches N, return to S12, and re - evaluate the vehicle positioning accuracy at the next time.

[0115] After returning to S12, the positioning prediction using the resampled particles has higher accuracy.

[0116] An embodiment of the present invention also provides a computer - readable storage medium, which is used to store a computer program. When the computer program is executed by a computer, the computer can implement the vehicle positioning method based on accuracy evaluation provided by the above - mentioned method embodiment.

[0117] An embodiment of the present invention further provides a computer program product for storing a computer program, which, when executed by a computer, enables the computer to implement the vehicle positioning method based on accuracy evaluation provided by the above method embodiment.

[0118] An embodiment of the present invention further provides a chip, including a processor coupled to a memory, for calling a program in the memory to enable the chip to implement the vehicle positioning method based on accuracy evaluation provided by the above method embodiment.

Claims

1. A vehicle positioning method based on accuracy evaluation, characterized in that It includes the following steps: S1. Use a number of particles to simulate the motion pose of the vehicle, calculate the weights of the particles based on the situation of the map road signs sensed by the vehicle's sensors, select the particle with the largest weight as the optimal particle, and use the position value of the optimal particle as the position value of the vehicle, including the following steps: S11. Initialize the particle filter algorithm; S12. Calculate the positions of the particles at time t, simulate the position of the vehicle at time t with the positions of the particles at time t, and calculate the new position values of each particle according to the vehicle motion equation, including the following sub-steps: (1) When the motion direction θ of the particle remains unchanged: x t = x t-1 + v * Δt * cos(θ t-1 ); y t = y t-1 + v * Δt * sin(θ t-1 ); θ t = θ t-1 ; (2) When the motion direction θ of the particle changes: where x t-1 is the value of the particle in the x-axis direction at time t-1, and x t is the value of the particle in the x-axis direction at time t; y t-1 is the value of the particle in the y-axis direction at time t-1, and y t is the value of the particle in the y-axis direction at time t; v is the velocity value of the particle; θ t-1 is the azimuth value of the particle at time t-1, and θ t is the azimuth value of the particle at time t, is the azimuth change rate of the particle; both the v and are obtained by simulating the particle filter algorithm; the Δt is the movement duration of the particle from time t-1 to time t; S13. Calculate the weights of the particles; S14. Calculate the position of the optimal particle; S2. Calculate the probabilities of the vehicle moving to various positions in space according to the vehicle's motion mode, select the largest probability among them, and use the spatial position value corresponding to the largest probability as the predicted position value of the vehicle; S3. Calculate the difference between the position value of the vehicle obtained in S1 and the predicted position value of the vehicle obtained in S2, and use this difference as the positioning accuracy evaluation value E t ; S4, according to the positioning accuracy evaluation value E t resample the particles, select an appropriate number of particles, and return to S12 to estimate the vehicle position at the next moment. The specific steps are as follows: Compare E at time t t with E at the previous time t-1 t-1 ; When E t <E t-1 , select the particles with the optimal and sub-optimal particle weights, discard the remaining particles, reduce the total number of particles, return to S12, and re-evaluate the vehicle positioning accuracy; When E t > E t-1 , copy the particles with the optimal particle weights until the number of particles reaches N, return to S12, and re-evaluate the vehicle positioning accuracy.

2. The vehicle positioning method based on accuracy evaluation according to claim 1, wherein: S1 includes the following steps: S11. Initialize the particle filter algorithm: First, establish a plane rectangular coordinate system on the area map to be evaluated, and divide the area map into L*M map squares of the same size; initialize the total number of particles to N; S12. Calculate the positions of the particles at time t: Calculate the position values of all the particles at time t; S13. Calculate the weights of the particles: Calculate the weights of all the particles at time t through the following formula Among them, w n,t is the weight of particle n at time t, where n is the particle number, and n ∈ [1, N]; p t represents the number of the map road signs sensed by the sensor of the vehicle at the position of particle n at time t, p t ∈ [1, K], where K is the total number of the map road signs on the map; o i is the distance value from the origin of the plane rectangular coordinate system to map road sign i; μ t,i represents the distance value measured by the sensor of the vehicle at the position of particle n to map road sign i at time t, where i is the map road sign number, and i ∈ [1, p t ; ∑ represents the sensor measurement covariance matrix, and the ∑ is obtained by calibrating the vehicle sensor; S14. Calculate the position of the optimal particle; Among the weights of all the particles at time t, select the particle with the largest weight as the optimal particle, and denote the optimal particle as Pf opt ; its corresponding position is then the optimal position at time t, denoted as And use the optimal position as the position of the vehicle.

3. The vehicle positioning method based on precision evaluation according to claim 2, characterized in that S2 includes the following steps: S21. Initialize the vehicle position: S22. Predict the position of the vehicle according to the vehicle's motion mode, including the following steps: S221. First, calculate that at time t-1, the probability of the vehicle on map square j is: where j is the number of the map grid, and j ∈ [1, L*M]; Then, calculate that at time t, the probability of the vehicle on map square k is: Where k is the number of the map grid, and k ∈ [1, L * M]; Finally, according to the total probability formula, the probability that the vehicle is on the map grid k at time t is obtained There is: Among them, ~N represents following a Gaussian distribution; σ s represents the vehicle motion variance, and the σ s is obtained by calibrating the vehicle, and s is the distance traveled by the vehicle in Δt time; S222, calculate the probability p(z t observed by the vehicle at position X at time t t |X t ), we have: in, Indicates the vehicle obtained by the particle filter algorithm in X t The total number of observations at the position t map landmarks, *l represents the vehicle at X obtained by the particle filter algorithm. t The number of the map landmark that can be observed at the location; represents the sensor measurement variance of the vehicle, obtained by calibrating the vehicle; represents the q observed by the vehicle t Map signposts, q t ∈[1,K], where l represents the number of the map landmark actually observed by the vehicle, l∈[1,q t ]; S223. Calculate the position probability data set of the vehicle at time t: At time t, the position probability of the vehicle is obtained through the following formula: Obtain the position probability data set of the vehicle at time t S224, screen out the maximum value in the vehicle position probability dataset and obtain the predicted position of the vehicle here 4. The vehicle positioning method based on precision evaluation according to claim 3, wherein S3 specifically is: Calculate the difference between the position of the vehicle at time t and the predicted position of the vehicle as the positioning accuracy evaluation value E t :

5. An electronic device, characterized in that, It includes a processor and a memory, wherein: the memory is used to store programs; the processor is coupled to the memory and is used to call the programs in the memory to make the electronic device execute the method according to any one of claims 1-4.

6. A computer-readable storage medium, characterized in that, It includes instructions that, when running on a computer, cause the computer to execute the method according to any one of claims 1-4.

7. A computer program product comprising instructions, characterized in that, When the computer program product runs on a computer, it causes the computer to execute the method according to any one of claims 1-4.

Images