localization 2D of outdoor mobile robots using 3D laser range data
Takeshi Takahashi CMU-RI-TR-07-11
May 2007
Robotics Institute Carnegie Mellon University Pittsburgh, Pennsylvania 15213
c Carnegie Mellon University
Submitted in partial fulfillment of the requirements for the degree of Master of Science
Abstract
Robot localization in outdoor environments is a challenging problem because of unstructured terrains. Ladars that are not horizontally attached have benefits for detecting obstacles but are not suitable for some localization algorithms used for indoor robots, which have horizontally fixed ladars. The data obtained from tilted ladars are 3D while these from non-tilted ladars are 2D. We present a 2D localization approach for these non-horizontally attached ladars. This algorithm combines 2D particle filter localization with a 3D perception system. We localize the vehicle by comparing a local map with a previously known map. These maps are created by converting 3D data into 2D data. Experimental results show that our approach is able to utilize the benefits of 3D data and 2D maps to efficiently overcome the problems of outdoor environments.
Adviser: Sanjiv Singh
I
Contents
1 2 3 4 Introduction Related work Data acquisition Map matching 4.1 Known map . . . . . . . . . 4.2 Local map . . . . . . . . . . 4.3 Elevation map . . . . . . . . 4.3.1 The Highest point . . 4.3.2 Mean of data points . 4.3.3 Number of points . . 4.3.4 Robust elevation map 4.4 Feature map . . . . . . . . . 4.5 Landmarks . . . . . . . . . 1 1 3 4 4 5 5 5 5 6 6 7 9 11 11 12 12 14 14 15 17 17 17 31 31 31
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
5
Localization 5.1 Particle filters . . . . . . . . . . 5.1.1 Recovery from failures . 5.1.2 Uncertainty . . . . . . . 5.2 Perception and comparing maps 5.2.1 Map matching . . . . . 5.2.2 Scan matching . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
6
Experimental results 6.1 Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Conclusions and future works 7.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Future works . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
III
1
Introduction
Localization is very important for autonomous vehicle to track a path, detect and avoid obstacles properly. Indoor robot localization has been done successfully,however outdoor robot localization is still a challenging problem. In outdoor environments, there are factors that make localization difficult. Terrains are usually not flat. GPS signals degrade in the presence of vegetation, buildings, and terrain features. Environments are being changed because of the presence of people, cars, and other moving objects. Terrains, especially trees, are different in seasons. Construction of roads and buildings also change the environment. Therefore we need a robust localization method that overcomes these problems. Robots performs many tasks in real time, such as obstacle detection, obstacle avoidance, path planning , path tracking, and localization for real-time operations, and therefore we need an algorithm that does not require high computational costs. Using cameras results in high computational costs and they are not suitable for night operations, while using ladars results in lower computational cost and they are applicable to night operations. It is hard to use indoor robot localization algorithm because of the property differences between indoor and outdoor environments. Indoor robots usually have horizontally fixed ladars that can obtain data at a certain height and are enough to localize the vehicle because of no elevation and the presence of walls. However, if robots have horizontally fixed ladars, it is difficult to detect obstacles lower or higher than the ladar. Tilted ladars are able to detect obstacles efficiently, but the vehicle drives at a high speed, and scenes taken from ladars at the same area could be different because the data density is very low. Although we are able to use GPS in outdoors, we cannot always rely on GPS information because of satellite occlusion. Lost GPS information sometimes causes significant errors. In real-world applications, the use of a known map for localization is a legitimate assumption. For example, security robots are different from reconnaissance robots, which sometimes needs to be operated in an unknown area. Security robots can be operated in factories, port and harbors, and military bases. This means that they are used in known areas and therefore,we do not have to solve SLAM (Simultaneous Localization and Mapping) problems for this kind of security robots. In this paper, we compare localization methods and map representations, and present a suitable localization method for our outdoor security robot. This paper consists of the following sections: data acquisition (Section 3), map creation (Section 4), and localization algorithm (Section 5). Section 6 describes the results of our experiments in the outdoor environment.
2
Related work
In this section,we review related works in localization algorithms. Particle filters have been one of the most reliable localization algorithms particularly for indoor environments [3] [4]. One problem with particle filters is the computational cost involved. The more the number of particles and state spaces we use, the more the computational 1
(a)
(b)
Figure 1: Our security robots(a)Grizzly and (b)Yogi cost. Accurate results need large number of particles and state spaces. This is a tradeoff between accuracy and computational cost. In order to reduce computational costs, particle filters for real time operations have been studied under changing number of particles and size of incoming data set [5] [6]. Another problem with the particle filter is that the robot cannot recover its pose when no particle is around the true pose in the sace of an accident. Sensor resettings are able to solve this problem [7]. Expansion resettings are also able to overcome this problem and a combination of these two resetting methods can be applied to robust localization algorithms [8]. Studies mentioned above are for indoor robots with horizontally fixed 2D laser sensors. There are the problems with the use of these sensors in outdoor environments [1]. It is difficult for the 2D navigation system to overcome unflat environments [14] and to detect and avoid obstacles; however, while tilted ladars are suitable for these purposes [12]. Histogram Matching can be applied to 3D SLAM [9]. This algorithm can successfully map very large areas and solve the kidnapped robot problem. It does not use less reliable matches. This idea can be applied to matching in the localization algorithm. However, this algorithm cannot be used for global localization. The situation is different from ours because this vehicle has horizontally fixed ladars. Although 3D outdoor localization algorithm using ladars has been studied, the computational cost involved is extremely high due to the high degree of freedom of the states [10] [11]. Other approaches for outdoor navigation are described in [1]. This system use 3D data for a 2D SLAM algorithm and map representation for semi structured outdoor environments. It has the advantage of 3D perception and less computational 2D navigation. In this paper, we describe how to convert 3D data into 2D representation and how to exploit these advantages.
2
3
Data acquisition
The ladars we use are 2D laser range finders. One ladar is fixed to the vehicle and is slightly tilted to cover a range of 10 m in front of the vehicle. As the vehicle moves, we can obtain horizontal data. We found that one sensor is not enough to detect obstacles and to avoid these obstacles when the vehicle moves at a high speed [12]. Therefore, we added another sensor that is sweeps back and forth to obtain vertical data. We can obtain 3D data by combining these two sensors. The ladars need to be well calibrated, because the data difference between fixed and sweeping laser sensors generates false obstacles, which causes serious error in obstacle avoidance. We have a gyro that give the yaw angle of the vehicle and an encoder that provides the distance of travel. We transform data obtained from ladars into the vehicle frame, and then transform them into a global coordinate using odometry information or GPS information.
Figure 2: Sweeping and fixed ladars
(a)
(b)
Figure 3: (a)(b) 3D point clouds from sweeping (blue) and fixed ladars (red) 3
4
Map matching
When the robot is operated in an unknown area, we need to create a map of the area. SLAM solves this problem if we can not use GPS. In our situation, we don not consider the computational time for creating a map and use GPS to create a map when GPS signal perception is good. The important point is that we need to create similar maps in order to compare a known map with a local map created during a task. A large difference between the known and the local map will cause serious problems in localization. Dynamic obstacles is one of the main factors that causes the difference in the maps. When the vehicle moves at a high speed, it cannot obtain the density data of environments. Our vehicle is supposed to move at a speed of 5 m/s. Therefore, we need to overcome the dynamic obstacles and the fewness of data points. Because our operation is done on-line, we need to quickly create a local map. There are many methods of creating a map. To create a 3D map or store 3D data points, we need large sources and computational time. Therefore, we use a 2D map that can utilize the features of the 3D data. We create an elevation map in which we discretize the world and each grid has a value representing its highest point, traversable costs or landmarks etc. Finding vegetation takes time to compute, but if we focus on only few features, we can compute in real time. Taking the highest point in a cell might be noisy. In our case, we select a point below which p percent of data points in the cell are included. Drawbacks of the elevation map are that we cannot represent some typical terrains, such as overhang obstacles, trees, tunnels, and roads under bridges. We need to consider these obstacles if the area where the robot is being operated has many of these features. In our case, we conducted experiments in the campus where there are no such objects. We believe that all environments do not have these many features. Partial problem caused by these features in our algorithm is insignificant fecause of its robustness. We fix the vehicle's height to ground level because our gyro cannot measure the pitch of the vehicle and the vehicle is always on the ground. We suggest three simple map representations: an elevation map known as 2 and a half dimensional map, a feature map, and landmark maps. We compare these representations in the experimental section and describe the results.
4.1
Known map
When we create a known map, we can use any of the methods and devices like beacons and GPS. If we have several robots and only one of them has GPS, we can use the robot to create a known map. Other robots do not need to have GPS, which can reduce the cost involved. In our case, we create a map using GPS and manually remove noises and obtain a decent map. Due to the property of our gyro, we can measure only the yaw angle of the vehicle. Thus, we cannot consider the pitch and roll. We do not consider the cases where there are overlapped roads and many hills. The direction of movement of the vehicle when we collect data is important. In particular, because our vehicle moves at a high speed. The data obtained from different directions would be different. Therefore, we need to collect data in at least two opposite directions. Then, we combine these data to create a known map. Our odometry 4
doesn't provide pitch angles, therefre when we created a prior map, we don't use pitch angles although GPS provides pitch angles of the vehicle. If we use pitch angles to create a known map, slopes in the map would be different from theses in a local map, and that leads to bad matchings. Not using pitch angles causes problems in loclaization when the vehicle starts to go up or down the slope and finishes to go up or down the hill. These problems occurs for only few seconds, thus it doesn't affect localization so much as shown in the result section.
4.2
Local map
The local map is a map created using just ladars and odometry, and not GPS. A gyro and an encoder provide accurate data when the vehicle moves a small distance. Thus, the local map is supposed to be almost the same as the sub map, which is extracted from a known map. We can obtain 3D data points from ladars, but due to the high computational cost invovled, we convert them into 2D data.
4.3
Elevation map
The simplest method of creating a 2D map from 3D data is to compute the elevation of each cell. First, we decide the proper bin size , which is usually 50 cm or 25 cm. The finer map requires more computational cost. We need to collect data at least in two opposite directions so that we can localize the vehicle in any direction. There are a variety of methods of creating an elevation map. First, we assign a cell {i, j} to each 3D data point pn = pnx , pny , pnz collected from ladars. We assign the cell as follows; p i = nx j = y pn = pk,{i,j} where the function x gives the smallest integer x. A cell {i, j} has K points. After we assign the cells to points, we can compute the elevation of a cell. 4.3.1 The Highest point
pn
mij = maxk pkz ,{i,j} This elevation map is very noisy because the highest point does not represent the height of the object. When the robot obtains the highest point near the object, the height would be low, while it would be high when the robot is far away from the object. 4.3.2 Mean of data points
mij = meank pkz ,{i,j} This cannot represent a map properly because the density of data points is high at a low level and low at a high level due to the tilted ladars. 5
Figure 4: Elevation map using highest points or mean of points 4.3.3 Number of points
We count the number of data points in each voxel i, j, l. Then, we use the voxel with the maximum number of points as the elevation of the cell. In our case, this method cannot be applied because we obtain more data points on lower parts of objects than upper parts because of the fact that the ladars are tilted. pk l = z pn = {i, j, l} A voxel {i, j, l} has points Pk,q mij = argmax {countpointl {i, j, l}}
4.3.4
Robust elevation map
We want to select a point below which percent of points in the cell are included. If we use this method, the resulting map has less noise than the map created using the highest points. This method is very simple to use and more robust than the others; therefore we adopt this map as the elevation map. Pk = descend sortk pkz ,{i,j} mij = p K where {i, j} has K points.
6
Figure 5: Creation of an elevation map using a voxel with the maximum number of points
Figure 6: Creation of an elevation map with a point below which p percent of points are included
4.4
Feature map
People and cars are the main moving obstacles. Most of the people and cars are less than 2 m in length. Therefore, we consider a space that is greater than 2 m. Then we create an elevation map. We want to use these data as features. Thus, we select a space such as, 2 - 3 m, 2 - 4 m or 3 - 4m. Empirically, the space from 2 to 3 m above the ground would be suitable for features as shown in Fig.??. If we use the upper space 7
Figure 7: Elevation Map such as 5 - 6 m, we obtain fewer points than with 2 - 3 m. After we extract the points in the space above 2 m and below 3 m, we can use a constant for representing the cell. The resulting map will be a occupancy map that has only two values, a constant or zero. Using a constant is a reasonable approach because considering the mean or the highest point might be noisy.
Figure 8: Extracting the space from 2 m to 3 m
8
Figure 9: Feature Map
4.5
Landmarks
We can extract landmarks and remove overhang points by a simple method [1]. Usually landmarks could be tree trunks, telephone poles, and walls of buildings. These landmarks usually stand straight. We have the sweeping laser sensor that collects vertical data. This vertical data is suitable for extracting these landmarks. We slightly change the method in [1] to be apply it to our data. We use a constant for Hmax . A 3D point pi,j is an overhang point if there is at least one point pi,k in the same vertical scan. pi,k is below pi,j and is at a larger distance to the sensor. pik,r > pij,r + Rt 0k<j (1) (2)
where points pi,j and pi,k are in the i th scan, and Rt is the minimum overhang distance. A 3D point pi,j is a landmark point if there is at least one point pi,k in the same vertical scan. pi,k is below pi,j and pi,j is not an overhang point.
Ht < pij,z - pik,z < Hmax , pij,r - pik,r < tan(t ) pij,z - pik,z 0k<j
(3) (4) (5) (6)
where Ht is the minimum height of a landmark, Hmax is the maximum difference between pi,j and pi,k , and t is the maximum angle misalignment, 9
Figure 10: Definition of landmark and overhang points
Figure 11: Tree trunks and building walls were extracted and overhang points were removed. Green points are removed overhang points, blue points are landmark points, and red points are other points.
10
Figure 12: Landmark Map
5
5.1
Localization
Particle filters
There are three types of localization problems; tracking (local localization), global localization, and kidnapping. In the tracking problem, we estimate relative poses. Kalman filter is the most widely used localization algorithm. It approximates beliefs using mean and covariance. Extended Kalman filter solves non-linearities. An advantage of Kalman is its efficiency. Due to the assumption of unimodal posteriors, Kalman filters can only solve the tracking problem. If our estimate is completely wrong due to large noise of sensors or dead reckoning, it is difficult for the algorithm to track it again. In the global localization problem, we estimate a pose in the global coordinate. an During operation, if the robot is kidnapped and the pose is replaced, it is callded as the kidnapping problem. Kidnapping problem occurs when perception systems suddenly does not work and robot moves for certain time and then the sensors start to work again. In our case, we do not exactly know the initial pose of the vehicle. We want to localize the vehicle in the global coordinate. By comparing the known map with the local map created during the operation, we can obtain the global position in the known map. Particle filter is the best choice for localization in our case. Particle filters are Bayes filters that use random samples for posterior estimation. The filter can perform global localization even when we lose the vehicle pose again. The drawback of particle filters is the computational cost involved. By reducing the dimensionality of the pose and the number of particles, we can perform real time localization. In facts, this is a tradeoff between accuracy and computational cost. If we use more particles, it is highly possible that we can obtain a more accurate result. The advantage of the use of particle filters is the representation of arbitrary probability densities; thus, they can solve the global localization problem. The efficiency of particle filterer depends on the number 11
of particles. To reduce computational cost, real-time particle filters have been studied [5]. In our case, we focus on a small area and lower dimensionality of state space; therefore, particle filters can be applied in real-time operations. The pseudo code of a particle filter is shown in Fig. 1. First, we need to create a local map using the data obtained from ladars and odometry. Then, we generate particles from previous particles using their weights. Next, we propagate particles based on the uncertainty model and odometry and compute new weights by comparing the local map with a sub map created at the location of each particle in the global map. To determine the weight of each particle, we compute correlations between these maps. We can also use imaginary rays to compute the weight. 5.1.1 Recovery from failures
When the vehicle seems to get lost, we sometimes need to recover from the failures. To determine whether the vehicle got lost, we consider the likelihood. If the likelihood is really small, this means that even if particles converge, the vehicle might get lost because the known and the local maps are very different. It is possible that the estimate is correct even if the likelihood is very small. Failure occurs when there is no particle around the real vehicle pose. Therefore, we need to expand or replace the particles. We reset some particles among N particles. The reason why we do not reset all particle is to consider the case where the estimate is correct but the likelihood is low. We decide how big area we need to expand based on the standard deviation of particle clouds. 5.1.2 Uncertainty
The encoder and the gyro have noises as shown in Fig. 13. We need an uncertainty model for propagating the particles. A good uncertainty model improves the performance of our particle filters. We collected data for constructing an uncertainty model using the vehicle with GPS and odometry. Then we computed errors between GPS data. We computed errors at different speeds. Then, we computed the mean of absolute error and the mean of error of different speeds. Mean of error is a bias of this model. Small noises for propagating particles pose a problem that the particles cannot catch up when odometry gets large noises. Therefore, we select the largest noise among noises at different speeds for uncertainty model. Errors
dsO intervals
= (dsG - dsO )
(7) (8) (9) (10) (11) (12)
v =
std(Errorsv ) v (ds) = max(v )
d O interval
Errors
= (dG - dO )
=
std(Errors ) (d) = max( ) 12
Table 1: Localization algorithm AlgorithmP articlef ilter Inputs M L : Local map M G : Global map yt : Current observation xt : Current state ut : Control measurement N : Number of particles Xt : particles [mL , M L ] := GetLocalM ap(M L , xt , yt , ut ) //Obtain a local map for n = 1, , N do [n] [n] //Propagate a particle xG := M otionM odel(ut , xG ) t t [n] [mG , M G ] := GetLocalM ap(M G , xG ) //Obtain a local map t [n] G L wt := CompareM aps(m , m ) //Compute weight end do Xt :=< empty > x = 0; t for n = 1, , N do wt = x t x t
[n] wt
[n] [n]
wt
Xt = Xt xG t = end do +
[n]
, wt
[n]
// Insert a particle into particle set //Compute weighted mean of particles
[n] [n] wt xG t
wt ) > //If the maximum likelihood is greater than threshold if max(wt Xt := GenerateP articles(Xt ) //Sample N particles based on weights end if LostLocation() == 1 //This function checks whether a robot lost its pose. Xt := ResetP articles(Xt ) //Reset Particles end Return Xt , x t //Return new particles and estimate pose
[n]
[n]
13
,where N is the number of data, dsG is the distance of travel in a second measured with GPS, dsO is the distance of travel in a second measured with odometry, dG is the relative orientation in a second measured with GPS, dO is the relative in a second orientation measured with odometry, function std() computes standard deviation, v is the velocity of the vehicle, and is the angular velocity. In this case, we compute one-second data, and the distance of travel and the relative orientation correspond to the velocity and the angular velocity respectively.
Figure 13: Difference between GPS data and Odometry data. We compute a model using the intervals 0.5 radian and 2 m.
5.2
Perception and comparing maps
When we compute the likelihood mentioned above, we need to select an appropriate perception system. There are some perception systems such as map matching, likelihood field, feature based-method and scan matching etc. We use map matching and scan matching to compute weights. 5.2.1 Map matching
We can compute correlations of these two maps [13]. The sensor measurement model compares the local map mlocal with the global map m, such that the more similar m 14
and mlocal , the larger p(mlocal |xt , m). Since the local map is represented based on the robot pose xt , this comparison requires that the sub map extracted from the global map is transformed into the estimated robot coordinate. The transformed sub map at the estimated pose x in the global map is compared with the local map using the map ^ correlation function as follows: m,mlocal ,xt (^) = x x x,y (mx,y (^) - m) (mx,y,local (xt ) - m) 2 x,y (mx,y,local (xt ) - m) (13)
x 2 x,y (mx,y (^) - m)
where mx,y,local (xt ) is the cell (x, y) in the local map at xt , and mx,y is the cell (x, y) in the sub maps at x. m is the average map value, which is expressed as ^ m= 1 2N (mx,y + mx,y,local )
x,y
(14)
,where N denotes the number of overlapped cells between the local and sub map in the global map. The correlation m,mlocal ,xt takes the value from -1 to +1. This value is interpreted as follows: p(mlocal |xt , m) = max(m,mlocal ,xt , 0) 5.2.2 Scan matching (15)
After we extract the features, we can compute imaginary 2D rays as shown in Fig.??. These imaginary rays facilitate the use of indoor-like localization method since standard indoor robots have 2D horizontally fixed ladars. Using imaginary 2D rays, we obtain a distance and a orientation to each feature. We compute a likelihood by comparing the distances and the orientations. The difference is that this localization method can utilize 3D data but uses fewer data.
k p(zt |xt , m) =
1 2 2
K
e- 2
k zk 2 1 (zi -i ) 2
(16)
p(zt |xt , m) =
k=1
k p(zt |xt , m)
(17)
where k is the k th measurement in a scan.
15
Figure 14: Imaginary 2D rays
16
6
6.1
Experimental results
Experiment
Our vehicle has two SICK laser sensors. Fixed laser sensor has 180 field of view with 1 resolution and collects data at 75 Hz. Sweeping laser sensor has 90 field of view with 0.5 resolution and collects data at 75 Hz. The vehicle has a encoder and a gyro. They provide information about relative yaw angle and travel of distance at 100 Hz. We collected data on Carnegie Mellon University campus on a weekend morning. There were few people and cars. We used GPS and laser data for creating a map, and then used odometry data and laser data for localization. We drove four laps around the campus at a speed of about 5 m/s in the counter clockwise direction and another four laps in the clockwise direction. First lap in both directions were used for creating a known map. Other laps were for testing our localization algorithms. We performed the localization off-line using the collected data. We didn't use GPS data for localization. We have done five combinations of algorithms as follows. (1) Elevation map - map matching (2) Feature map - map matching (3) Feature map - scan matching (4) Landmarks - map matching (5) Landmarks - scan matching The results depend on parameters of mapping and localizatoin, and therefore we used well-tuned parameters selected empirically. We supposed that the initial position is unknown but know that vehicle is in a certain small area such as about 50 m by 50 m. The distance of travel is about 4 km. We use 300 particles for each algorithm.
6.2
Results
Each algorithms showed that it can localize the vehicle even though the initial location is unknown. If we use map matching, it takes more time than with scan matching. In particulary, the computational cost of map matching depends on the resolution of the map. Using Landmarks also needs more time than using elevation or feature map. As shown in Fig. 21, 22, 23,24, and 25, weighted mean of particles shows the best result in terms of position but not orientation. Trajectory generated from particle with the largest weight or particle with the best history is very noisy compared to that generated from weighted mean. Therefore, we use weighted mean to estimate the pose of the vehicle. Table 2 show the mean and the standard deviation of the estimated poses. Table 3 show the mean and the standard deviation of the estimated poses not using bad matching data. Using feature map and scan matching resulted in the best performance although all algorithms gave similar errors. These results also show that elevation map should be used for map matching, and feature map and landmarks should be used for scan matching. Map matching performed better results in the orientation of the vechile while scan matching performed better results in the position of the vhicle. Fig. 16, 17, 18, 19 and 20 show trajectory of each result. Problems occures when the vehicle moves in open areas. This is because particles have orientation errors and if we propagate 17
without resampling, orientation errors affects the performance of localization. Thus, the estimated position of vehicle was off the path in the open spaces, however, we recovered from failures using resetting. Figures31, 32, 33, 34, and 35 show cross and long track error. The cross track error is the distance from the closest path. The long track error is the distance from the closest position along the path. The long track errors are larger than the cross track error, which means that the distance error originates mainly from the long track error. The vehicle can view only about 20 m in front of the vehicle. Thus, ...