Ground planeScanning plane
4.3. Estimating the Robot Motion
intersecting line. Using fundamental geometry we can see from Figure 4.2 that the pitch angle θ is
In the previous section we showed how roll and pitch orientations of the LIDAR can be estimated using assumptions about the environment. The LIDARs full pose is defined by 6 degrees of freedom (DOF) - with roll, pitch and height 3 out of these 6 are known. In this section we describe how we estimate the missing 3 parameters.
4.3.1. Scan Projection
Here we introduce two local coordinate systems. Let the robot’s coordinate system be defined by three vectors: one normal to the ground plane, another is the projection of the 0◦beam of the LIDAR on the ground plane and the third is normal to the previous two with its direction chosen so a that a right-handed or positive coordinate system is formed.
The origin is chosen to be in the center of the laser rangefinder.
The second coordinate system is the LIDAR’s own coordinate system, defined by a vector in the zero degree beam direction, one in the 90◦direction and a third one that is normal to the previous two also forming a right-hand coordinate system.
The x axis is defined in both by the forward (0◦) beam of the scanner while the z axis is defined in the robots coordinate system by the normal of the ground plane, and in the LIDARS coordinate system by the vector normal to the scanning plane. The rotation be-tween the two coordinate systems can be defined by the previously calculated pitch and roll angles so that for θ = 0◦and φ = 0◦the two coordinate systems coincide.
A rotational matrix is defined that projects each point in the LIDAR’s coordinate sys-tem pito the xy plane of the robots coordinate system
pxyi = R · pi (4.3)
4.3 ESTIMATING THE ROBOT MOTION
where R is composed from Rx(θ ) · Ry(φ )and pxyi denotes a projected point. The new projected scan is parallel to the floor the robot is moving on. Vertical structures appear in the same x-y location independent of the height at which the scanner actually mea-sured them. This allows to adopt known techniques to estimate the displacement within the groundplane between two consecutive scans given by the parameters x, y and ψ. The displacement of the two scans is equivalent to the robots movement.
4.3.2. Scan Matching Challenges
To estimate the displacement of two consecutive, projected scans the Iterative Closest Point algorithm (ICP) which was introduced in Section 3.3 is used. While the naive imple-mentation works reasonably good on scans that are captured within the same plane in a static environment, the projected data of a tilted LIDAR introduces several challenges.
First the points representing the ground need to be removed from both point sets used for matching. This is done by calculating the line-point distance for each point to the ground line and rejecting the ones below a certain threshold. Removing these points reduces the number of points available for matching, but is necessary for the matching procedure.
Tilting the LIDAR towards the ground reduces the maximum range in the scanners x-direction. While a commonly used SICK LMS 291 has 81 meters maximum range, the range in x-direction decreases down to a few meters when tilting the rangefinder. This makes it harder to estimate the forward movement of the robot. Figure 4.3 shows the differences between a horizontal and tilted scan that is reprojected of a hallway.
A scanner in horizontal orientation senses structures up to its maximum range within the full field of view which allows good matching, a tilted LIDAR sees less objects due the reduced forward range.
Strong features for estimating rotation and displacement in y-direction are usually available for a tilted scanner, while there is a lack of features that allow to estimate the forward movement. For example a long hallway without doors leads after removal of the groundpoints to two lines of points on both sides of the robot which allow excellent estimation of y and ψ but do not give any constraints on the forward direction.
24
4.3 ESTIMATING THE ROBOT MOTION
(a) horizontal scan of a hallway
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
(b) The projected scan of the same scene captured with LIDAR tilted 20 degree towards the ground.
Red points belong to the ground and are removed before matching.
Figure 4.3. Example of a horizontal and projected scan. After removing the points representing the ground in the tilted scan (right figure, red points) less features in moving direction of the robot are available.
Vertical structures in two tilted scans will allow proper matching, non-vertical struc-tures might degrade the result and lead to ambiguities. Horizontal objects in indoor envi-ronments like tables or low walls outdoor will have constant projected locations for every point on their horizontal surface. Using them for matching would lead to a false result.
Given enough vertical structures as postulated in Assumption 3 this issue can be avoided.
The reduced number of points in each scan and few features that establish constraints in moving direction together with the ambiguities introduced by non vertical structures can decrease the accuracy of ICP results. To deal with these issues, a good initial guess for the ICP algorithm is needed. To find this good initial guess a constant velocity motion model is used as described in the next section.
4.3 ESTIMATING THE ROBOT MOTION
4.3.3. Constant Velocity Motion Model
To find a good initial condition for the scan matching procedure, we use the previous estimates for the robots velocity to project where the robot might be located at the time the next scan is taken. the two previous scans used for matching and Tiis the initial guess for the next matching.
dtnis the time difference between the current and previous scan.
This simple approach assumes that the velocity is changing at relatively slow rates.
Slow meaning that the changes between two consecutive scans are small, which is a rea-sonable assumption.
4.3.4. Sampling based Approach
As described in Section 3.3, ICP requires a good initial guess to work since it is prone to converge towards local minima. Ambiguities in the scan data can introduce several local minima which might lead ICP to give an incorrect matching result. To make the approach more robust to outliers and to aventually cover ambiguities, the ICP algorithm is performed several times with the same two scans, but using different initial conditions.
Figure 4.4 shows an example of 200 initial conditions where all results clustering to-wards one maxima (red dots). In Figure 4.5 also 200 initial conditions are displayed, but the results in red agglomerate around various maxima. Having sufficient vertical structures will make sure that at least one of these clusters corresponds to the correct result. To find the actual clusters from the set of the ICP results, the algorithm described in Section 3.4 is used.
When the environment is sufficiently structured and has few ambiguities, all initial guesses tend to converge towards the same result. If the environment is not sufficiently structured, has ambiguities or the two scans overlap only in small regions, multiple clusters are likely to occur. In case of multiple solutions these solutions can be used to estimate the uncertainty of the localization, or they can be saved as mulitple hpotheses and later an
26
4.3 ESTIMATING THE ROBOT MOTION
Figure 4.4. Sampling based results with no ambiguities. The black points corre-spond to initial guesses, the red ones are the results and the green circle with cross is the center of the clustering result
Figure 4.5. Sampling based results with ambiguities. The black points correspond to initial guesses, the red ones are the results and the green circle with cross is the center of the clustering result. 3 possible solutions can be identified.
offline optimization process tries to find the best solution of each timestep. The multiple hypotheses approach will be discussed in section 6.2.