Method for estimating the position of a vehicle on a map
A method for estimating a precise position of a vehicle on a map includes acquiring at least one geolocated position of the vehicle by way of a geolocation system, pre-positioning the vehicle on the map, and particle filtering in which possible positions of the vehicle, called particles are processed. The particle filtering includes distributing particles on the map, and then updating the particles on the map, calculating the likelihood of each particle, selecting a limited set of particles, and, if an indicator relating to the likelihood of the selected particles and to the number of selected particles drops below a threshold, resampling particles on the map.
1. A method for estimating a precise position of a vehicle on a map, comprising:
acquiring at least one geolocated position of the vehicle via a geolocation system;
pre-positioning the vehicle on the map, at the geolocated position of the vehicle; and
particle filtering, via a computer of the vehicle, in which possible positions of the vehicle, called particles, are processed so as to determine the precise position of the vehicle on the map,
wherein the particle filtering comprises:
distributing particles on the map as a function of the geolocated position of the vehicle on the map, the particles being arranged in spiral extending outward from the geolocated position of the vehicle at a constant angular offset, and then updating the particles on the map,
calculating a likelihood of each particle being the precise position of the vehicle based at least on data from the map,
selecting a limited set of particles, and
starting a resampling of the particles on the map in response to an indicator dropping below a threshold such that the resampling does not occur while the indicator is above the threshold, the indicator relating to the likelihood of the selected particles being the precise position of the vehicle and to the number of selected particles.
2. The estimation method as claimed in claim 1 , wherein the likelihood of each particle is calculated as a function only of data from the map.
3. The estimation method as claimed in claim 1 , wherein the likelihood of each particle is calculated as a function of data from sensors allowing the vehicle to perceive its surroundings, on the condition that these data are deemed to be reliable.
4. The estimation method as claimed in claim 1 , wherein the particle filtering includes choosing the precise position from among the limited set of selected particles as a function of the likelihood of each particle.
5. The estimation method as claimed in claim 1 , wherein, with the map storing data relating to road sections, the likelihood of each particle is calculated as a function of the position of the closest road section with respect to the particle.
6. The estimation method as claimed in claim 1 , wherein the distributing particles on the map includes distributing the particles in a disk centered on the geolocated position of the vehicle.
7. The estimation method as claimed in claim 6 , wherein a radius of the disk is determined as a function of a horizontal protection level assigned to the geolocated position of the vehicle.
8. The estimation method as claimed in claim 1 , wherein the selecting includes selecting the particles as a function of a distance between them and the geolocated position of the vehicle.
9. The estimation method as claimed in claim 1 , wherein the updating includes moving the particles on the map as a function only of information relating to dynamics of the vehicle.
10. The estimation method as claimed in claim 1 , wherein the resampling includes resampling the particles using a low-variance technique.
11. The estimation method as claimed in claim 1 , wherein, when the indicator is not below the threshold, the resampling is not performed.
12. The estimation method as claimed in claim 1 , wherein the threshold is stored in a read-only memory of the computer.
13. The estimation method as claimed in claim 1 , wherein a plurality of the particles distributed on the map are not distributed on any road section on the map.
14. The estimation method as claimed in claim 13 , wherein all of the particles distributed on the map are associated with at least one of the road sections on the map.
15. The estimation method as claimed in claim 14 , wherein the distributing includes, after the particles are associated with at least one of the road sections, orienting each of the particles as function of an orientation of the associated road section.
16. The estimation method as claimed in claim 14 , wherein the updating the particles on the map includes, for each of the particles, calculating a ratio r to determine if each particle should be moved to new road section using the following formula:
r
=
(
AB
→
·
AM
i
→
)
AB
→
2
with AB being a length of the road section for the associated with the particle and AM being a length from a beginning of the road section to a center of the particle,
wherein when the ratio r is between 0 and 1, the association of the particle with and the road section should not be changed, and
wherein when the ratio r is strictly greater than 1, the particle is associated with a following road section.
17. The estimation method as claimed in claim 1 , wherein all of the particles distributed on the map are associated with at least one of the road sections on the map.
18. The estimation method as claimed in claim 17 , wherein the distributing includes, after the particles are associated with at least one of the road sections, orienting each of the particles as function of an orientation of the associated road section.
19. The estimation method as claimed in claim 17 , wherein the updating the particles on the map includes, for each of the particles, calculating a ratio r to determine if each particle should be moved to new road section using the following formula:
r
=
(
AB
→
·
AM
i
→
)
AB
→
2
with AB being a length of the road section for the associated with the particle and AM being a length from a beginning of the road section to a center of the particle,
wherein when the ratio r is between 0 and 1, the association of the particle with and the road section should not be changed, and
wherein when the ratio r is strictly greater than 1, the particle is associated with a following road section.
20. A vehicle comprising:
a map;
a geolocation system; and
a computer configured to pre-position the vehicle on the map,
wherein the computer is configured to:
acquire at least one geolocated position of the vehicle via the geolocation system;
pre-position the vehicle on the map, at the geolocated position of the vehicle; and
particle filter in which possible positions of the vehicle, called particles, are processed so as to determine the precise position of the vehicle on the map, wherein the particle filtering comprises:
distributing particles on the map as a function of the geolocated position of the vehicle on the map, the particles being arranged in spiral extending outward from the geolocated position of the vehicle at a constant angular offset, and then updating the particles on the map,
calculating a likelihood of each particle being the precise position of the vehicle based at least on data from the map,
selecting a limited set of particles, and
starting a resampling of the particles on the map in response to an indicator dropping below a threshold such that the resampling does not occur while the indicator is above the threshold, the indicator relating to the likelihood of the selected particles being the precise position of the vehicle and to the number of selected particles.