GB2622193A

GB2622193A - A method for determining a position of a vehicle

Info

Publication number: GB2622193A
Application number: GB2212536.3A
Authority: GB
Inventors: Mlynarczyk Wojciech; Zieba Bogumil
Original assignee: Individual
Current assignee: Individual
Priority date: 2022-08-30
Filing date: 2022-08-30
Publication date: 2024-03-13
Also published as: GB202212536D0

Abstract

A method for determining a position of a vehicle, the method comprising calculating (203, 204) a correction of the position of the vehicle based on a laser scan and a hybrid map (information on positions of solid elements and reflectors) by means of an Iterative Closest Point (ICP) algorithm. Updating the position of the vehicle may be based on odometer measurements or on extrapolation of a series of previous positions. Points on the laser scan may be matched with points on the hybrid map depending on a point type, e.g. a solid element or a reflector. A multistage ICP algorithm may be applied, decreasing a maximal possible points matching distance for each consecutive stage. Artifacts from the laser scan may be removed if the calculated angle (A) between two adjacent points (A1, A2) is less than a threshold value, removing both points (A1, A2) from the laser scan. Artifacts may also be removed by removing points that are not adjacent to other points. The initial position of the vehicle may be determined by comparing the measured laser scan performed on the real surrounding of the vehicle to the expected scan performed on the map for a particular initial position of the vehicle.

Description

A METHOD FOR DETERMINING A POSITION OF A VEHICLE

TECHNICAL FIELD

The present invention is related to a method for determining a position of a vehicle, in particular within a closed area. The method is particularly useful for vehicle navigation systems.

BACKGROUND

One of significant problems in development of systems for autonomous vehicles is a system for determining a position of the vehicle. A large group of autonomous vehicles are vehicles which move within restricted areas, such as industrial sites.

Navigation systems for autonomous vehicles are known to use odometers and lidars. The lidar is a device which allows measuring a distance from objects by illuminating the objects with a laser beam and measuring a resulting reflection using a sensor. Differences in time of return of the laser beam and change in the wavelength can be subsequently used to generate a two-dimensional or a three-dimensional model of surroundings. The odometer allows measuring a traveled distance and a steering angle of the vehicle wheel. A combination of these two techniques allows calculating changes in vehicle position over time. However, navigation which is based solely on the odometer is burdened with an error, commonly referred to as a drift (i.e. the position accumulating error). Starting from a known initial position of the vehicle, an estimated position of the vehicle over time can be calculated, wherein said estimated position is burdened with a measurement error which accumulates in the course of increasing distance traveled by the vehicle moving further.

There are known some techniques for correction of errors in position measurement. One technique relates to natural navigation, which uses a map of surroundings prepared by means of Simultaneous Localization And Mapping (SLAM), which is based on localization algorithms and a map of surroundings. However, natural navigation is not suitable for variable work environment, for example an environment within which there are large movable elements that obstruct walls; furthermore, the natural navigation does not work in places without characteristic elements that facilitate navigation (for example spaces with long hallways). Another technique is called reflector navigation -it uses a map of positions of reflectors, wherein said reflectors constitute reflective surfaces that can reflect laser beams, wherein the reflectors are located on a closed area of navigation, such as manufacturing facility. Reflector navigation is costly, because it requires arranging a significant number of reflectors within the whole area to be adapted for navigation by autonomous vehicles.

SUMMARY OF THE INVENITON

There is a need to provide a new method for determining a position of a vehicle within a closed area, which would be devoid of at least of some of the aforementioned disadvantages.

The present invention relates to a method for determining a position of a vehicle, the method comprising the steps of: reading a hybrid map that comprises information on positions of solid elements and reflectors within an area of movement of the vehicle; determining an initial position of the vehicle in a coordinate system of the hybrid map; during movement of the vehicle: updating the position of the vehicle; calculating a correction of the position of the vehicle based on a laser scan and the hybrid map by means of an Iterative Closest Point algorithm; correcting the position of the vehicle based on the calculated correction; and outputting the corrected position of the vehicle as the determined position of the vehicle.

Preferably, updating the position of the vehicle is based on odometer measurements.

Preferably, updating the position of the vehicle is based on extrapolation of a series of previous positions.

The method may comprise calculating the correction of the position of the vehicle by matching points on the laser scan with points on the hybrid map using the Iterative Closest Point algorithm, wherein the points are matched depending on a point type.

The point type may determine at least one of elements: the solid element and the reflector.

The method may comprise calculating the correction of the position of the vehicle by: calculating the correction of the vehicle position by matching points on the laser scan with points on the hybrid map using the Iterative Closest Point algorithm, only for points representing the solid elements; and calculating the correction of the vehicle position by matching points on the laser scan with points on the hybrid map using the Iterative Closest Point algorithm, only for points representing the reflectors.

The method may comprise applying a multi-stage Iterative Closest Point algorithm and decreasing a maximal possible points matching distance for each consecutive stage.

The method may comprise removing points considered as artifacts from the laser scan, by calculating an angle between two adjacent points on the laser scan and if the calculated angle is less than a threshold value, removing both points from the laser scan.

The method may comprise removing points considered as artifacts from the laser scan, by removing points that are not adjacent to other points The method may comprise determining the initial position of the vehicle by comparing the measured laser scan performed on the real surrounding of the vehicle to the expected scan performed on the map for a particular initial position of the vehicle.

The method may comprise locating solid elements on the hybrid map by using a Simultaneous Localization And Mapping (SLAM) algorithm, and automatically locating reflectors on the hybrid map by: indicating the position of the vehicle on the map and carrying out the navigation of the vehicle on the map; recognizing the position of the reflectors on the laser scan using information on a brightness of the scanned objects on the laser scan; based on the position of the vehicle calculated by means of a natural navigation and the recognized positions of reflectors on the laser scan, determining the positions of reflections from the reflectors on the map as potential positions of the reflectors; grouping the points denoting the potential positions of the reflectors; based on a shape of a group of points, determining positions of the reflectors; and indicating the positions of the reflectors on the map.

BRIEF DESCRIPTION OF DRAWINGS

The present invention is shown by means of example embodiments in a drawing, wherein: Fig. 1 presents schematically example positions of points Al, A2, scanned with a laser L; Fig. 2 presents schematically scanning beams of the laser L that are incident on a reflector R; Fig. 3 presents schematically steps of a method for determining a position of a vehicle without using reflectors; Fig. 4 presents schematically example indications of initial positions of a vehicle; Fig. 5 presents schematically example reflector positions read out with respect to a vehicle at different positions; Fig. 6 presents a method for determining a position of a vehicle using a hybrid approach; Fig. 7 presents a method for determining a position of a vehicle using a type-determination approach.

DETAILED DESCRIPTION

The present disclosure relates to a method for determining a position of an autonomous vehicle in a closed space (for example in a manufacturing site). The method involves use of a hybrid map comprising information concerning a layout of stationary elements of surroundings (i.e. non-movable elements of the surroundings within which the vehicle moves, such as walls, columns etc.), and information concerning a layout of laser reflectors, as well hybrid positioning using a hybrid map and a laser installed on the vehicle for simultaneously detecting the elements of the surroundings and positions of the laser reflectors.

The hybrid map used in the system according to the invention is generated based on a standard map (a plan) enhanced by a layout (positions) of the laser reflectors. The map may have a form of a bitmap file that defines the surrounding layout in a vertical projection, at a particular spatial resolution (for example, expressed as number of meters or centimeters per pixel). The standard map may be generated using a SLAM algorithm and comprises information concerning occupied and unoccupied areas.

The position of the reflectors on the map may be indicated manually or automatically. Manual marking consists in indicating the position of the reflectors on the map. Automatic marking of the reflectors positions consists in marking a position of the vehicle on the map and moving the vehicle within the map area. Basing on laser scanning and using the information on brightness of the scanned objects, the reflectors positions can be recognized. Next, based on the position of the vehicle calculated by means of a natural navigation and an algorithm of finding the reflectors using laser scanning, information on reflections from the reflectors are registered on the map with the use of information on beam direction and a reflection distance. Subsequently, points denoting potential positions of the reflectors are grouped (clustered). In the next step, the shape of the reflectors is matched to the groups of the points of potential positions of the reflectors, for example by determining an average of the position of all points or by means of matching the reflector shape to the cloud of points using the least squares method (RANSAC -Random Sample Consensus) or the like. After matching the points of reflector to its shape, a center of a circle being a vertical projection of the reflector is calculated, and it is marked on the map as the reflector position -in the map coordinates.

In the system according to the invention, the solid elements that are present in the navigation area are used as laser reflectors; these elements are covered by highly reflective foil. These elements may constitute pipes or columns present in the manufacturing site. Therefore, the reflectors are clearly visible on the image obtained using laser scanning, as the reflectors differ (by being brighter) from other background elements (not marked by reflective foil). It is necessary to use a laser scanner that provides information about the brightness of the scanned points.

Thus, to obtain a scan image (a scan), the surrounding of the vehicle is scanned with the laser L such that it is possible to recognize the positions of the reflectors on the scan.

Preferably, after the operation of scanning and before the operation of recognizing reflectors, so-called "shades" are removed from the scan (wherein said "shades" constitute measurement artifacts). The artifacts appear because additional points appear at the edges of the objects on the laser scan, which do not exist in reality. These points should be removed as they deteriorate the quality of navigation. To this end, for each two adjacent points Al, A2 on the scan, an angle A between points Al and A2 (as shown in Fig. 1) is calculated, for example by means of Formula 1 -presented below: A = min (arcctg ((r2-r1) / (B * r1)), arcctg ((r1-r2) / (B * r2))) (Formula 1) If the angle A is smaller than a predetermined value, such as 15 degrees, the two points are removed from the scan image, because there is a high probability that they may be artifacts that do not represent reality. The presence of such artifacts can noticeably deteriorate the quality of navigation to such an extent that it is even better to risk removing points corresponding to real objects than to allow the presence of artifacts.

Additionally, all points that are not adjacent to other points are removed from the scan image, namely points surrounded by empty space. These points are considered as undesirable, as they typically represent measurement artifacts that do not represent real objects.

Next, it is necessary to determine a transformation (displacement and rotation) between two different laser scans. The laser scan is obtained in a form of a point cloud. In order to detect translations and rotations between two point clouds (i.e. between the corresponding points on the subsequent scans), an Iterative Closest Point (ICP) algorithm is used. The algorithm determines a transform that minimizes differences between the measured laser scan and the expected scan. The expected scan is generated from the selected point on the map by means of a ray tracing method. The map has a form of a bitmap of a known resolution. The ray tracing method includes transmitting virtual beams of the laser on the map, starting from a predetermined laser point for each angle, and calculating a distance of their intersection with the nearest obstacle on the map. This way, an artificial scan for the predetermined position of the vehicle is obtained. The measured scan is generated by scanning a real surrounding of the vehicle with the laser installed on the vehicle.

The ICP algorithm comprises steps wherein, for each point from the first scan of the surrounding, the nearest point from the second scan of the surrounding is matched. The algorithm uses a minimum_corresponding_distance parameter, such that the distance between said points must not exceed its value. If the distance to the nearest point exceeds said value, it is assumed that matching for a particular point does not exists. If there is no matching point for a given point, then that point does not participate in a given iteration of the algorithm. In this step, imposing further criteria of points matching is also possible. An example is the element-type determination approach which is described hereinbelow.

Subsequently, a combination of translation and rotation of the points of the second scan is determined, to obtain a minimum value of the sum of squared distances between the matched points of the two scans. In this step, it is also possible to weight the distances of pairs of the points based on various criteria. For example, if the points are reflectors, they may have higher weight, as described further in the description.

Next, a second set of the points is transformed using the calculated translation and rotation, and if the criteria of algorithm termination are not met, the method returns to the first step of the ICP algorithm.

The result of the ICP algorithm operation is a final translation and rotation between the first scan and the second scan, as well as a number of matched points. There are also known variations of the ICP algorithm, that match a point to a section and a section to a section. However, this does not substantially affect the operation of the described method Furthermore, in the method according to the present invention, a multi-step (such as a two-step) ICP algorithm can be used. For example, in the first phase the ICP algorithm can be activated with a minimum_corresponding_distance parameter that is set to a large distance, such as 2 meters, and a first transform between the scans is determined. Next, the second scan is transformed according to the determined transform. In the second phase the ICP algorithm with the minimum_corresponding_distance paramter is activated again, wherein the parameter is set to a smaller distance, such as 0,2 m, a second transform between the scans is determined and the number of the matched points is registered. Subsequently, a final transform is determined which is a combination of the two above transforms. The final number of matched points is the number of matched points determined by the second activation of the algorithm.

Using the two-step (or in general, a multi-step) algorithm ICP, it is possible to obtain better matches for larger range of translations and rotations.

Recognizing the reflectors on the laser scan by using information on brightness (the result of which is the obtained from the laser scan) is burden with a random error. For that reason, other points of high brightness, that do not correspond to reflectors, can also appear in the laser image. In particular, such points may appear for the elements present at a close distance from the laser. The greater the distance between the laser and the obstacle, the easier is to distinguish an ordinary element from a reflector. To identify the points which are potential reflections from the reflectors, the points are filtered by rejecting points that are below a minimal distance which is specified by a reflector_min_distance parameter, and below a minimal brightness which is specified by a reflector_min_intensity parameter.

Moreover, optionally, if a probable position of the vehicle on the map is known, and if positions of the reflectors are marked on the hybrid map, this information may be additionally used to increase reliability of reflectors detection on the laser scan. Based on the hybrid map and the vehicle position on the map, it is possible to determine ranges of points and distances in the laser scan that match each of the known reflectors. The determined ranges and distances take into account additional tolerance resulting from predetermined measurement errors of the laser, as well as an error of estimation of a position and an angle of the vehicle.

Fig. 3 presents steps of the method for determining a vehicle position, without using the reflectors. In the first step 101, the standard map is retrieved and an initial position of the vehicle in the map coordinate system is determined. Next, in the second step 102, the vehicle position changing over time is calculated using the odometer. Thus, information on a traveled distance, wheel steering angle, and vehicle model, is used. For example, in a forklift-type vehicle, a so-called tricycle model ca be used (drive and a measurement of traveled distance are based on movement of a swivel wheel). However, due to random phenomena and measurement inaccuracy (for example resulting from slipping wheels), a drift may occur (i.e. an increasing position error). Together with periodically performed scans of the surrounding using the laser, in the third step 103 a regular correction of the predetermined vehicle position is carried out by comparing the laser scans with the scan generated from a particular position. Next, the method returns to the second step 102 to calculate subsequent positions of the vehicle.

Fig. 4 presents schematically example indications of initial vehicle positions. On the map and on a substrate on which the vehicle travels, the possible initial positions of the vehicle are marked, for example, as positions P1, P2. Each initial position of the vehicle comprises a translation parameter and a vehicle angle rotation parameter in the map coordinate system. When starting determination of the vehicle position, the first operation is to determine the initial position. This position can be determined manually, for example by indicating it on the map. The initial position may also be determined semi-automatically. Said operation can be performed, for example, at the moment of vehicle activation, by positioning it at the determined position and switching it to the automatic mode. If there are more than one determined positions, it is necessary to determine at which of the determined positions the vehicle is positioned. The initial position may be determined automatically by comparison of the measured laser scan with respect to the expected scan, for a particular initial position from a list P1, P2..... P, the latter scan being determined on the map by means of the ray tracing method, for each initial position. The comparison is carried out using the ICP algorithm providing as a result a number of matched points and a correction between the two scans. Next, the best matched expected scan and a position Pi that is assigned to said scan, are selected. The scan that has the highest number of matched points is recognized as the best matched scan. Next, a correction between the measured scan and the expected scan is calculated onto the vehicle coordinate system. The initial position is calculated by imposing the correction, in the vehicle coordinate system, onto the selected initial position Pi. It is also possible that an operator indicates a specific initial position, based on which the vehicle position can be calculated using the ICP algorithm.

As described above, the predetermined vehicle position (for example, of a forklift truck) is calculated based on information from the odometer. Over time, drift appears. To this end, the map of surrounding is used; said map is generated using the SLAM method and regularly performed laser scans. Based on that, the drift correction is calculated; and said correction is regularly applied onto the predetermined position of the vehicle.

The vehicle correction procedure comprises the following steps: - specifying a predetermined position of the vehicle on the map, wherein said position can be burdened with an error resulting from odometer drift; - retrieving a measured scan form the laser, in the laser coordinate system (the laser coordinate system is used instead of the map coordinate system, because it is the measurement from the device performed in the coordinate system of the device); - based on the predetermined position of the vehicle on the map and the position of the laser on the vehicle, calculating the laser position on the map; - using the ray tracing method, generating an expected scan in the predetermined laser position on the map. The obtained artificial scan is also in the laser coordinate system; - using the ICP method or the like, calculating translation and rotation between the measured scan and the expected scan, which corresponds to the vehicle position error in the laser coordinate system; - to limit errors resulting from randomness of the measurements, the following operations are carried out: o limiting the values of correction of angle and correction of position -to the maximal values that are the algorithm parameters -the corr_angle_max, and corr_position_max, wherein said limitations aim to secure operation of the navigation against randomly occurring errors and incorrect effects of operation of the ICP algorithm (outliers); o limiting the values of correction of angle and correction of positions by multiplying them by a factor within the range of 0 to 1 -an angle_dumping_factor and an position_dumpingfactor, to minimize rapidity of changes in positions by averaging the successive corrections; - recalculating the position correction from the laser coordinate system into the vehicle coordinate system; - applying the calculated correction into the predetermined position of the vehicle.

Fig. 6 presents a method for determining a vehicle position by means of a hybrid approach. The method can be used for the hybrid navigation, i.e. the navigation that uses a laser attached on the vehicle; said laser is configured to detect elements of surrounding and positions of laser reflectors (to reflect the laser beams) and it uses a hybrid map encompassing positions of the elements of surrounding and positions (positions) of the reflectors.

In the first step 201, an initial position of the vehicle is specified according to the determination procedure of the vehicle initial position described above. Next, in step 202, the vehicle position is updated based on indications from the odometer. In step 203, the correction of the vehicle position is calculated based on the laser scan, for example by using the ICP algorithm or similar, for the solid elements of the surrounding. Subsequently, in step 204, the correction of the vehicle position is calculated based on the laser scan by using the ICP algorithm for the reflectors. This means that the algorithm is applied with the use of the artificial scan based on the map on which only the elements being reflectors are located, or the artificial scan is generated by using the known position of the vehicle and the list of positions and sizes of the reflectors located in its surrounding. Next, the method returns to step 202.

Preferably, when generating the artificial scan, at least one beam intersecting the reflector is indicated as that incident to the reflector. Both steps 203, 204 using the ICP algorithm have independent parameters -corr_angle_max, corr_position_max, angle_dumping_factor, position_dumping_factor. However, the order of performing the steps 203 and 204 may be reversed.

Fig. 7 presents a method for determining the position of the vehicle with the use of type determination approach. The method combines steps 203, 204 into a single step. In a particular embodiment, the method anticipates in steps 203, 204, using the ICP algorithm with an additional information for each point on the scan -point type. Then, in the step of matching the points from two scans, the ICP algorithm takes into account only the type which is a point type. Thus, the ICP algorithm calculates in a single step the vehicle position correction based simultaneously on information concerning solid elements of the surrounding and reflectors. For example, the type of the point may be determined as a wall (i.e. a solid element of the surrounding) or a reflector. In more complex versions, the reflector type may be distinguished -for example reflector 1, reflector 2. Then, the matched points are of the same type, thereby, the points of a wall are matched with the points of the wall, points of the reflector are matched with the points of the reflector. To summarize, on the measured scan, all of the points which are not reflectors are determined, said points are indicated with the wall type. The remaining points in the scan are indicated with the reflector type.

Next, the expected scan is generated using the method for ray tracing on the map, wherein the beams incident to the walls are indicated with the wall type, and the beams incident to the reflector are indicated with the reflector type. At least one beam is indicated as that incident the reflector. Even if the reflector is smaller than the size of the beam, it is still denoted as reflector.

Subsequently, the ICP algorithm using information on the types in a way that only the points of the same types are matched, is applied. Optionally, it is also possible to match the walls from the artificial scan to the reflector on the scan from the measurement, due to the possibility of erroneous recognition of the wall as a reflector on the real laser scan. Additionally, different weights for walls and reflectors may be used. For example, the points with the type of reflector are expressed with a weight -reflector_weight.

Determining the vehicle position without using information from the odometer is possible as well. In such case, the calculated change in the vehicle position between the scans (updating the position over time by means of odometer from the previous scan), is replaced with the one from the following values: zero translation and rotation, or expected translation and rotation the latter being calculated from linear and angular velocity based on a sequence of earlier positions (extrapolation, dead reckoning).

In order to increase accuracy of the position estimation and frequency of updating of the position estimation, measurements from the odometer may be used.

The solution according to the present invention allows arranging the laser reflectors only in the positions where this is necessary; whilst in the remaining positions the natural navigation based on determining positions of vehicle basing on the objects present in the area of the vehicle movement may be used. Thereby, the hybrid determination of the vehicle position solves the problem of varying surroundings, by using the solid reflectors. Thanks to that, installation of the reflectors on the entire area is not necessary; the reflectors are installed only on the required positions where the natural navigation is not useful. This limits the costs and complexity of implementation of the solution.

Claims

CLAIMS: 1 A method for determining a position of a vehicle, the method comprising the steps of: - reading a hybrid map that comprises information on positions of solid elements and reflectors within an area of movement of the vehicle; - determining (201) an initial position of the vehicle in a coordinate system of the hybrid map; - during movement of the vehicle: o updating (202) the position of the vehicle; o calculating (203, 204) a correction of the position of the vehicle based on a laser scan and the hybrid map by means of an Iterative Closest Point algorithm; o correcting the position of the vehicle based on the calculated correction; - and outputting the corrected position of the vehicle as the determined position of the vehicle.
2. The method according to claim 1, wherein updating (202) the position of the vehicle is based on odometer measurements.
3. The method according to claim 1, wherein updating (202) the position of the vehicle is based on extrapolation of a series of previous positions.
4 The method according to any of previous claims, comprising calculating (203, 204) the correction of the position of the vehicle by matching points on the laser scan with points on the hybrid map using the Iterative Closest Point algorithm, wherein the points are matched depending on a point type.
5. The method according to claim 4, wherein the point type determines at least one of elements: the solid element and the reflector.
6 The method according to any of claims 1-3, comprising calculating (203, 204) the correction of the position of the vehicle by: - calculating (203) the correction of the vehicle position by matching points on the laser scan with points on the hybrid map using the Iterative Closest Point algorithm, only for points representing the solid elements; and - calculating (204) the correction of the vehicle position by matching points on the laser scan with points on the hybrid map using the Iterative Closest Point algorithm, only for points representing the reflectors.
7. The method according to any of previous claims, comprising applying a multistage Iterative Closest Point algorithm and decreasing a maximal possible points matching distance for each consecutive stage.
8 The method according to any of previous claims, comprising removing points considered as artifacts from the laser scan, by calculating an angle (A) between two adjacent points (Al, A2) on the laser scan and if the calculated angle (A) is less than a threshold value, removing both points (Al, A2) from the laser scan.
9 The method according to any of previous claims, comprising removing points considered as artifacts from the laser scan, by removing points that are not adjacent to other points.
10. The method according to any of previous claims, comprising determining (201) the initial position of the vehicle by comparing the measured laser scan performed on the real surrounding of the vehicle to the expected scan performed on the map for a particular initial position of the vehicle.
11. The method according to any of previous claims, comprising locating solid elements on the hybrid map by using a Simultaneous Localization And Mapping (SLAM) algorithm, and automatically locating reflectors on the hybrid map by: - indicating the position of the vehicle on the map and carrying out the navigation of the vehicle on the map; - recognizing the position of the reflectors on the laser scan using information on a brightness of the scanned objects on the laser scan; - based on the position of the vehicle calculated by means of a natural navigation and the recognized positions of reflectors on the laser scan, determining the positions of reflections from the reflectors on the map as potential positions of the reflectors; - grouping the points denoting the potential positions of the reflectors; - based on a shape of a group of points, determining positions of the reflectors; and - indicating the positions of the reflectors on the map.