Vehicle positioning method based on precision evaluation

Abstract

The invention discloses a vehicle positioning method based on precision evaluation, and the method comprises the steps: S1, simulating the motion pose of a vehicle through a plurality of particles, calculating the weight of the particles through the road sign condition of a map road sign sensed by a sensor of the vehicle, screening out the particle with the maximum weight as the optimal particle, and carrying out the calculation of the optimal particle; taking the position value of the optimal particle as the position value of the vehicle; s2, calculating a predicted position value of the vehicle through the probability; s3, calculating a difference value between the position value of the vehicle obtained in the step S1 and the predicted position value of the vehicle obtained in the step S2, and taking the difference value as a positioning precision evaluation value Et; and S4, carrying out resampling, selecting a proper number of particles, and returning to S1 to carry out vehicle position estimation. According to the method, particle resampling is carried out according to the positioning precision aiming at the problems of particle degeneration and particle recession of a particle filtering algorithm, so that the problems of particle degeneration and particle recession can be avoided, and the positioning precision and the positioning efficiency are improved.

Description

Technical Field

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

[0002] The particle filter algorithm is widely used in autonomous vehicle positioning systems because it is easy to implement and has certain robustness. However, the particle filter algorithm has certain limitations. After running the algorithm for a period of time, particle degradation and particle decay will occur, resulting in a decrease in positioning accuracy. Therefore, how to judge particle degradation and replenish 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 assessment, which can judge particle decay 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 assessment comprises the following steps:

[0006] S1, using a number of particles to simulate the motion posture of the vehicle, and calculating the weight of the particles through the map landmarks perceived by the vehicle's sensors, selecting 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;

[0007] S2, calculate the probability of the vehicle moving to various positions in the space according to the vehicle's movement pattern, select the largest probability, 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 vehicle position value obtained in S1 and the predicted vehicle position value obtained in S2, and use the difference as the positioning accuracy evaluation value E t ;

[0009] S4, based on the positioning accuracy evaluation value E t Resample the particles, select the 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, particle filter algorithm initialization:

[0012] First, a plane rectangular coordinate system is established on the area map to be evaluated, and the area map is divided into L*M map grids of the same size; the total number of particles is initially set 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 weight of the particle:

[0016] The weights of all particles at time t are calculated by the following formula:

[0017]

[0018] Among them, w n,t is the weight of particle n at time t, n is the number of the particle, n∈[1,N]; p t represents the number of map landmarks sensed by the sensors of the vehicle at the position of particle n at time t, p t ∈[1,K], K is the total number of map landmarks on the map; o i is the distance from the origin of the plane rectangular coordinate system to the map landmark i; μ t,i It represents the distance value from the vehicle to the map landmark i measured by the sensor of the vehicle at the position of particle n at time t, where i is the number of the map landmark, 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 all the weights of the particles at time t, the particle with the largest weight is selected as the optimal particle, and the optimal particle is represented as Pf opt ; Its corresponding position is the optimal position at time t, expressed as And the optimal position is used as the position of the vehicle.

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

[0022] (1) When the particle's motion direction θ 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 particle's motion direction θ changes:

[0027]

[0028] Among them, x t-1 is the value of the particle in the x-axis direction at time t-1, x t is the x-axis direction value of the particle at time t; y t-1 is the value of the particle in the y-axis direction at time t-1, t is the y-axis direction value of the particle at time t; v is the velocity value of the particle; θ t-1 is the particle's position at time t-1, θ t is the particle's position at time t, is the rate of change of particle position; All are 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 vehicle position:

[0031] S22, predicting the position of the vehicle according to the movement pattern of the vehicle, comprising the following steps:

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

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

[0034] Finally, according to the total probability formula, the probability of the vehicle being on the map square k at time t is obtained: have:

[0035]

[0036] in, ~N means it obeys Gaussian distribution; σ s represents the vehicle motion variance, the σ s It is obtained by calibrating the vehicle, s is the distance moved by the vehicle in the time Δt;

[0037] S222, calculate the time t when the vehicle is at X t The probability p(z t |X t ),have:

[0038]

[0039] 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;

[0040] 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 ];

[0041] S223, calculate the vehicle position probability data set at time t:

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

[0043]

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

[0045] S224, filter out the maximum value in the vehicle location probability data set And get the predicted position of the vehicle here

[0046] S3 is specifically:

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

[0048]

[0049] S4 specifically includes the following steps

[0050] E at time t t and E at the previous moment t-1 t-1 Make comparisons;

[0051] When E t <E t-1 When , the particles with the best and second-best particle weights are selected, and the remaining particles are discarded. The total number of particles is reduced, and the process returns to S12 to re-evaluate the vehicle positioning accuracy.

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

[0053] 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 program in the memory so that the electronic device executes the method as described in any one of claims 1-6.

[0054] A computer-readable storage medium, characterized in that it includes instructions, and when the instructions are executed on a computer, the computer executes the method according to any one of claims 1 to 6.

[0055] A computer program product comprising instructions, characterized in that when the computer program product is run on a computer, the computer is caused to execute the method according to any one of claims 1 to 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, the number of particles is appropriately reduced, thereby improving the overall operating efficiency of the system. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0059] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0060] See also Figure 1 The vehicle positioning method based on accuracy assessment of the present invention comprises the following steps:

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

[0062] Initialize the particle filter algorithm and set it 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 will be reduced; if it is set too small, the positioning accuracy will decrease and the unmanned vehicle positioning cannot be achieved.

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

[0064] The specific initialization includes the following steps:

[0065] S11, particle filter algorithm initialization:

[0066] First, a plane rectangular coordinate system is established on the map of the area to be evaluated, and the area map is divided into L*M map grids of the same size.

[0067] Then, the total number of particles is initially set to N;

[0068] Finally, the initial point of each particle is represented 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] The position of the particle at time t is simulated with the position of the vehicle at time t. According to the vehicle motion equation, the new position value of each particle is calculated, and the position of the particle after the movement time Δt is calculated. Δt is the movement time of the vehicle from time t-1 to time t.

[0071] The equation of motion for the vehicle is as follows;

[0072] (1) When the direction of motion 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 direction of motion 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 x-axis direction value of the particle at time t; y t-1 is the value of the particle in the y-axis direction at time t-1, t is the y-axis direction value of the particle at time t; v is the velocity value of the particle; θ t-1 is the particle's position at time t-1, θ t is the particle's position at time t, is the rate of change of particle position. v and All of them are simulated by particle filter algorithm.

[0079] S13, calculate the weight of the particle:

[0080] The weights of all particles at time t are calculated by the following formula:

[0081]

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

[0083] S14, calculate the optimal particle position

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

[0085] S2, calculate the predicted position of the vehicle:

[0086] S21, initialize vehicle position:

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

[0088] S22, predicting the position of the vehicle according to the movement pattern of the vehicle, comprising the following steps:

[0089] S221, the probability of a vehicle being in map square k at time t:

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

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

[0092] Then, the probability of the vehicle being on the map grid k at time t is calculated as:

[0093] Where k is the number of the map grid, k∈[1,L*M];

[0094] Finally, according to the total probability formula, the probability of the vehicle being on the map grid k at time t is obtained have:

[0095]

[0096] in, express Obey expectations The variance of vehicle motion is σ s Gaussian distribution of s It is obtained by calibrating the vehicle, and s is the distance the vehicle moves in time Δt.

[0097] S222, calculate the vehicle's position in X at time t. t The probability of observing a map landmark at a location:

[0098] At time t, the vehicle actually observes q t map landmarks, represented by q t ∈[1,K]; z represents the coordinate value of the map landmark, l represents the number of the map landmark actually observed by the vehicle, l∈[1,q t ];

[0099] At time t, the vehicle is at X t The probability p(z t |X t ),have:

[0100]

[0101] 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, which is obtained by calibrating the vehicle.

[0102] S223, calculate the vehicle position probability data set at time t:

[0103] At time t, the vehicle's position probability is obtained by the following formula:

[0104]

[0105] Get the vehicle position probability data set at time t

[0106] S224, filter out the maximum value in the vehicle location probability data set And get the predicted position of the vehicle here

[0107] S3, calculate the positioning accuracy evaluation value:

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

[0109]

[0110] S4, particle resampling:

[0111] According to the calculation results of S3, the particle positioning accuracy is evaluated and sampling is performed based on the positioning accuracy. At the same time, the number of particles is not fixed. While taking into account both accuracy and operating efficiency, a good positioning effect is achieved. Specifically, the following steps are included:

[0112] The E at the current time t t and E at the previous moment t-1 t-1 Make comparisons;

[0113] When E t <E t-1 When , it means that the vehicle positioning accuracy at time t is improved, the particles with the best and second-best particle weights are selected, and the remaining particles are discarded. The total number of particles is reduced, and the process returns to S12 to re-evaluate the vehicle positioning accuracy at the next moment;

[0114] When E t >E t-1 When , it means that the vehicle positioning accuracy at time t is worse than that at time t-1, and the number of particles needs to be increased. At this time, the particles with the best particle weights are copied until the number of particles reaches N, and then return to S12 to re-evaluate the vehicle positioning accuracy at the next moment.

[0115] After returning to S12, the positioning prediction using the resampled particles is more accurate.

[0116] An embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium 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 assessment provided by the above method embodiment.

[0117] An embodiment of the present invention further provides a computer program product, 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 assessment provided by the above method embodiment.

[0118] An embodiment of the present invention further provides a chip, including a processor, wherein the processor is coupled to a memory and is used to call a program in the memory so that the chip implements the vehicle positioning method based on accuracy assessment provided by the above method embodiment.

Claims

1. A vehicle positioning method based on accuracy evaluation, characterized in that: The following steps are involved: S1, using a number of particles to simulate the motion posture of the vehicle, and calculating the weight of the particles through the map landmarks perceived by the vehicle's sensors, selecting 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, calculate the probability of the vehicle moving to various positions in the space according to the vehicle's movement pattern, select the largest probability, 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 vehicle position value obtained in S1 and the predicted vehicle position value obtained in S2, and use the difference as the positioning accuracy evaluation value E t ; S4, based on the positioning accuracy evaluation value E t Resample the particles, select the appropriate number of particles, and return to S1 to estimate the vehicle position at the next moment.

2. The vehicle positioning method based on accuracy assessment according to claim 1, characterized in that: S1 includes the following steps: S11, particle filter algorithm initialization: First, a plane rectangular coordinate system is established on the area map to be evaluated, and the area map is divided into L*M map grids of the same size; the total number of particles is initially set to N; S12, calculate the position of the particle at time t: Calculate the position values ​​of all the particles at time t; S13, calculate the weight of the particle: The weights of all particles at time t are calculated by the following formula: Among them, 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 landmarks 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 map landmarks on the map; o i is the distance from the origin of the plane rectangular coordinate system to the map landmark i; μ t,i It represents the distance value from the vehicle to the map landmark i measured by the sensor of the vehicle at the position of particle n at time t, where i is the number of the map landmark, i∈[1,p t ]; ∑ represents the sensor measurement covariance matrix, and the ∑ is obtained by calibrating the vehicle sensor; S14, calculate the optimal particle position: Among all the weights of the particles at time t, the particle with the largest weight is selected as the optimal particle, and the optimal particle is represented as Pf opt ; Its corresponding position is the optimal position at time t, expressed as And the optimal position is used as the position of the vehicle.

3. The vehicle positioning method based on accuracy assessment according to claim 2, characterized in that: Step S12 includes the following sub-steps: (1) When the particle's motion direction θ remains unchanged: x t =x t-1 +v*Δt*cos(θ t-1 ); and t =and t-1 +v*Δt*sin(θ t-1 ); i t =θ t-1 ; (2) When the particle's motion direction θ changes: Among them, x t-1 is the value of the particle in the x-axis direction at time t-1, x t is the x-axis direction value of the particle at time t; y t-1 is the value of the particle in the y-axis direction at time t-1, t is the y-axis direction value of the particle at time t; v is the velocity value of the particle; θ t-1 is the particle's position at time t-1, θ t is the particle's position at time t, is the rate of change of particle position; All are obtained by simulating the particle filter algorithm; the Δt is the movement duration of the particle from time t-1 to time t.

4. The vehicle positioning method based on accuracy assessment according to claim 2, characterized in that: S2 includes the following steps: S21, initialize vehicle position: S22, predicting the position of the vehicle according to the movement pattern of the vehicle, comprising the following steps: S221, first, calculate the probability that the vehicle is on the map grid j at time t-1: Where j is the number of the map grid, j∈[1,L*M]; Then, the probability of the vehicle being on the map grid k at time t is calculated as: Where k is the number of the map grid, k∈[1,L*M]; Finally, according to the total probability formula, the probability of the vehicle being on the map square k at time t is obtained: have: in, ~N means it obeys Gaussian distribution; σ s represents the vehicle motion variance, the σ s It is obtained by calibrating the vehicle, s is the distance moved by the vehicle in the time Δt; S222, calculate the time t when the vehicle is at X t The probability p(z t |X t ),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 vehicle position probability data set at time t: At time t, the vehicle's position probability is obtained by the following formula: Get the vehicle position probability data set at time t S224, filter out the maximum value in the vehicle location probability data set And get the predicted position of the vehicle here 5. The vehicle positioning method based on accuracy assessment according to claim 4, characterized in that: S3 is specifically: Calculate the difference between the vehicle's position at time t and the vehicle's predicted position as the positioning accuracy evaluation value E t :

6. The vehicle positioning method based on accuracy assessment according to claim 2, characterized in that: S4 specifically includes the following steps E at time t t Compared with E at the previous moment t-1 t-1 Make comparisons; When E t <E t-1 When , the particles with the best and second-best particle weights are selected, and the remaining particles are discarded. The total number of particles is reduced, and the process returns to S12 to re-evaluate the vehicle positioning accuracy. When E t >E t-1 When , the particle with the best particle weight is copied until the number of particles reaches N, and then return to S12 to re-evaluate the vehicle positioning accuracy.

7. 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 program in the memory so that the electronic device executes the method according to any one of claims 1-6.

8. A computer-readable storage medium, characterized in that: The method comprises instructions, which, when executed on a computer, cause the computer to execute the method according to any one of claims 1 to 6.

9. A computer program product comprising instructions, characterized in that When the computer program product is run on a computer, the computer is enabled to execute the method according to any one of claims 1 to 6.

Images