Simultaneous localization and map-building in outdoors semi-structured environments

IAV2004 - PREPRINTS 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles Instituto Superior Técnico, Lisboa, Portugal July 5-7, 2004

SIMULTANEOUS LOCALIZATION AND MAP-BUILDING IN OUTDOORS SEMI-STRUCTURED ENVIRONMENTS G. C. Anousaki, K. J. Kyriakopoulos

National Technical University of Athens Iroon Polytechniou 9, Athens, GR- 15570

Abstract: This paper addresses the issue of Simultaneous localization and mapbuilding (SLAM) in large scale outdoor environments. A dead-reckoning scheme appropriate also for skid-steered mobile robots is used. A line feature-based map is constructed on-the-fly and assists in a better estimation for the robot pose. The dead-reckoning is based on internal sensors (inertial data and odometry) only. The information from an experimentally derived kinematic model and an onboard Inertial Navigation System (INS) is fused using a simple and fast maximum likelihood formulation to produce estimates for the linear and angular velocity of the robot. The map is constructed using two laser sensors located to cover a full 3600 sector around the robot. A second estimate for the robot pose is derived from a map matching algorithm. The two estimates are fused together using a covariance intersection filter. We verify our approach with large scale experiments in outdoors structured environments following paths with steep turns and variable velocity. Keywords: mobile robots,sensor fusion, localization, map-building

1. INTRODUCTION

eral framework for localization of land vehicles, there are two cases where the authors produce an experimental model for skid-steered vehicles, but they refer only to one dimension of the robot’s pose, the heading produced by the rotational velocity. In (Puneet Goel, 1999),Kalman filter is used to fuse data from the empirical model and a gyroscope for the heading; and data from GPS (outdoors) and the unicycle model for the position. The related system noises are estimated empirically and remain constant. In (John Folkesson, 2003), a rule for smooth switching between the estimation of the INS and the experimental model, based on the turning radius of the motion,is used for the heading; and for the position, they take the robot’s built-in odometry simply modelled.Finally, they realize a SLAM scheme using Extended Kalman Filter

The work presented here is oriented to the special difficulties present when an autonomous vehicle moves in large scale outdoor environments. In the special case of skid-steered vehicles this issue becomes more complicated since the conventional odometry models assume ideal rolling and thus, are not suitable. The difficulties of skid-steering have not yet been addressed completely since most of the work that has appeared treats car-like vehicles. Usually, in the research covering skid-steering vehicles, slipping is not accounted for, resulting in inaccurate kinematic modeling. In some other cases, where there was an attempt to model kinematically the skid-steered vehicle,the results are based only on simulations (Frantiek olc, 2001), (Luca Caracciolo, 1999),(S. Takezono, 1999). In the more gen-

886

(EKF). The outdoor environment presents its own challenges. The ground surface is not clean, the distances and velocities are of higher order than the ones encountered indoors. The surroundings are often not well defined and there are occlusions by vegetation. Most other work on SLAM in outdoor environments has implemented Extended Kalman Filter, like (F. Masson, 2002; Billur Barshan, 1995; John Folkesson, 2003; M. W. M. G. Dissanayake, 2001) and the justification lies in the state vector of the system that includes both the robot pose and the features of the map.In this way, the correlations are maintained, which has proved to be of great importance (J. A. Castellanos and Tardos, 1999). The great disadvantages of this approach is high computational demands, since the covariance matrix scales quadratically w.r.t. the landmarks, and a ”heavy” state vector as the exploration continues. In this paper, we propose a flexible scheme, where the state vector holds only the robot pose and the map is feature-based in the form of line segments. It is a more light representation in comparison to occupancy grids, methods where features are not extracted and Kalman methods, since we are talking about large scale environments. We have one estimation of the robot’s pose from the deadreckoning scheme and one from the laser scan matching algorithm. The fusion is done through a covariance intersection filter as described in (K¨ampke, 2001), avoiding the assumption that holds in Kalman filter that the cross correlations are zero. In this way, we minimize the uncertainty of pose estimate while the cross correlations are unknown and non zero. This approach is a good balance between computational efficiency and accuracy in outdoor environments. In the following section the general framework of the SLAM algorithm is described. In section 3, there is a short description of the dead-reckoning scheme. Section 4 presents the feature extraction scheme and the map matching algorithm. In section 5 we conclude the loop with the covariance intersection fusion of the robot pose estimates. Following, there is the description of the global map update algorithm, and we conclude by presenting the results from a representative experiment under real conditions.

Feature Merging Global Map

Visible global map

Laser scan

Feature extraction

Local Map

Map matching

Robot pose Estimation B

Robot Pose Dead-reckoning model

Robot pose Estimation A

Fig. 1. Block Diagram get a second estimate about the robot’s pose. We use these two estimates to derive a final estimate with the help of Covariance Intersection filter. We recalculate the global map innovation according to this final estimate and we update the global map, either by adding new features, or by merging some of the old with the new ones.

3. DEAD-RECKONING MODEL Here we propose an experimentally derived motion model aided by a low-budget INS unit, that provides information about 3D accelerations and angle rates. For the position estimation, we use the experimentally derived model. For the heading estimation, we use the readings from the INS unit in a discrete Kalman filter after clustering the measurements to decide which can be fused. This proposal is described in detail in (G. C. Anousaki, 2004). The key points of the proposed experimental model are : • It addresses the whole robot pose vector (x, y, θ), • the involved noises of the measurements are computed on-the-fly based on a suitable measurement-buffer from the data, • it is on various terrain types, after good calibration , • we use clustering for the measurements to remove outliers from the fusion scheme, and • we verified it in cases of extreme motion, where the robot executes steep turns and moves with variable velocity.

3.1 Experimentally derived kinematic model Executing several experiments on various terrains, covering all of the available velocity spectrum of the robot,and with the help of a home made trailer device, we managed to define a terrain-dependent relation between the velocities on the two sides of the robot with its actual resulting total velocity, linear and angular. That is, ⎤ ⎡ a11 a12   T T V = A·u ⇒ ux uy ω = ⎣a21 a22 ⎦· uright ulef t a31 a32 (1) where

2. SLAM ALGORITHM The proposed scheme is demonstrated in the following diagram. We start by getting an estimate for the robot pose from the dead-reckoning model. We form a local map from the laser scan at this pose. Next, we get the corresponding possible global map innovation using this estimation from dead-reckoning. We then perform a map matching algorithm to

887

ux is the velocity along axis wrt the local coordinate system, uy is the velocity along axis y wrt the local coordinate system, uright/lef t is the velocity on the right/left side wheels of the robot, ω the angular velocity of the robot. ai,j are the coefficients of the linear regression for the experimental model

u left

di

velocity of the skid-steered vehicle. To improve the information for the rotational velocity of the robot, another considered source of information is an onboard Inertial Navigation System mounted as close as possible to the robot mass center. The INS behavior was extensively studied wrt temperature by recording the readings as it stood still and observed its bias. In a second phase, we mounted it on the robot and recorded vibrations as it was stationary or moving. Using these observations, we were able to determine the bias for the yaw rate, that was proved to remain stable during operation. We finally calibrated a low-pass filter to remove vibration noise from the yaw rate signal. Due to the reasons described extensively in (G. C. Anousaki, 2004), we are only left with useful angular velocity estimates ωIN S .

yR u right

l

y

VR

(X

,Y

f

)

i

t1

t1

t1o

3.3 Sensor Fusion scheme

xR

t1 D

y

xt 1 O

t2

t2 o

th 1

xt1o

w1

x t2 o

th 2

yt2 o

w2

z

x

At this stage we have the following two sources of information:

yt1o

x t2

wR

(1) INS based model:

    V1 = ω = θ˙

Caster-wheel mechanism

(2) Dead reckoning model of equation (1) T T   (4) V2 = ux uy ω = A · uright ulef t

Fig. 2. Vectors and coordinate systems The velocities on the two sides of the robot (ulef t , uright ) are available through the wheel encoders and are filtered out through a low-pass filter calibrated according to several experiments. Accurate robot velocity measurements ux , uy , ω were concurrently provided via a trailer system, thoroughly described in (G. C. Anousaki, 2004). Using principles of multiple linear regression for data fitting in MATLAB, we derived an appropriate estimate of matrix A which is terrain dependent. Specifically for cement:   Acem =

a11 a12 a21 a22 a31 a32

=

−0.0407 0.0428 0.4673 0.5263 0.0178 −0.0178

(3)

The corrupting noises are such that we can make the assumption that V1 and V2 are random processes of Gaussian nature. The information from the technical specifications of the sensors are not enough to calculate the instrument statistics due to the surrounding corrupting operating conditions, that cannot be accurately described and considered to remain same during the motion. This reason leads to using the well-known simplified discrete Kalman filter (Maybeck, 1979). The only information that we need are the mean value and covariance {V¯i , PVi }. The Kalman updated fusion of the two variables is computed as:

(2)

Notice that these models differ from the usual model we meet in most of literature dealing with skid-steered vehicles, that is : VR = [ux , uy ] = ulef t +uright u −u and ω = rightl lef t , where l is the 2 vehicle axle length. The experiments proved that the derived kinematic model (1) deals better with problems like differences in the tire pressure, or defective transmission system. A more extensive description of the trailer system can be found in (G. C. Anousaki, 2004).

−1

PV = (PV1 −1 + PV2 [3] −1 ) V = PV · (PV −1 · V¯1 + PV 1

2 [3]

−1

· V¯2 [3]) (5)

where V is the sought velocity estimate PV is the covariance matrix

3.2 Inertial Navigation System

Before we apply the filter, we use a clustering method to verify that the two estimates are consistent enough to produce a valid estimate. According to (Durrant-Whyte, 1988), the two estimates need to satisfy the relation:

Within the proposed framework, equation (1) is only one source of information regarding the

1 ·(V1 −V2 (3))T ·(PV1 +PV2 (3) )−1 ·(V1 −V2 (3)) ≤ 1 2 (6)

888

ρ = x · cos θ + y · sin θ

−2 · (x − xi ) · (y − yi ) 1

θ = · arctan{ 2 (x − xi )2 · (y − yi )2

The cases where the clustering is impossible, we choose the measurement with the lowest covariance. Since we admit to have no knowledge of the variances for the two processes, we use the measurements to calculate the required values. Bearing in mind the evolution of the system’s state and the relatively low velocity of the robot, in addition to the slow operation cycle of the laser scanners (0.4 sec) we use a buffer of data sets that corresponds to dt 0.4 sec. This buffer offers adequate information for the estimation of the covariance matrix and the mean value for each variable, in order to apply the Kalman filter.

Next, we calculate the most distant point from this line. All the points of the cluster belong to the calculated line if the following condition holds: Sk = {(di , φi ) : di · cos(φi − θk ) − ρk ≤ ε} (10) If this is not true, then it contains more lines and we split it up using as limit the most distant point from the line. We repeat the linear fitting until all points belong to the extracted line, according to ε which is empirically determined. We calculate the edges of the line segments by projecting the two extreme points of the cluster onto the line. As for the covariance matrix of the extracted L features in the robot cs Cw , the specifications k of the laser device state that σdLi = 5mm and σφLi = 10−4 rads. From this point, we follow the rules of covariance propagation as described in several papers, as (J. Vandorpe, 1996),(P. Jensfelt, 2001),(Kai O. Arras, 1998), taking into account G as it is calculated the robot pose’s uncertainty CR in the covariance intersection filter described in section 6 . That is:  2 σd L 0 L i (11) Cdφ = 0 σφ2 L

4. FEATURE EXTRACTION At every time step, we are left with at most 722 laser scan points covering 3600 around the robot (xi , yi ), indicating the distance to the objects around the robot dsi at about 0.50 angle intervals φsi . We set to construct a local map based on the line segments wk = [ρk , θk ] with terminal points Lk = [xs , ys , xe , ye ], k=1,...,M. Where : [xs , ys , xe , ye ] the x-y coordinates of the start and end point of each line segment the parameters corresponding to the line expression: [ρ, θ] the parameters in the line expression ρ = x · sin θ + y · cos θ

i

The point obstacle position in polar and cartesian coordinates, in the robot cs is : T  Ppi = di φi T T   Pci = xP i yP i = di · cos φi di · sin φi (7)

wk L Cρθ k

L = G · Cdφ · GT = ∇g1 · CdL · ∇g1T + ∇g2 · CφL · ∇g2T

(12)

σθθ

·cos φsi di = dsicos φi (0, yl ) is the position of the front laser axis, and (0, −yl ) is the position of the back laser axis in the robot cs.

2

2

· (y · cos θ − x · sin θ)] · σdL · cos(φi − θ) + N ∂di i  1 = · [Q · (x · cos φ − y · sin φ − d · cos 2 · φi )− i i i (D 2 + Q2 )2

σρρ =

i ·sin φsi +yl φi = arctan( dsds ) i ·cos φsi

[

2

2

− D · (x · sin φi + y · cos φi − di · sin 2 · φi )] · σdL σθρ =

 ∂g2 ∂di

i

·

∂g1 · σdL ∂di i

(13)

Where N is the number of points in the cluster.The computational load lies in the calculation of these partial derivatives. At this point, we have acquired a local map with the parameters of the line features and their covariance matrices in the robot coordinate system.

Next, we separate them into clusters Sj . We are looking for line segments, so the clusters are elongated. Whenever two successive points (ρi , φi ) and (ρi+1 , φi+1 ) satisfy the following conditions, they belong to the same cluster. |φi+1 − φi | < φthr

= (ρk , θk ) = g(di , φi )

According to the noise model for laser range finders whose scans are subsequently considered, the angular term in (eq. 12) can be omitted.  1 ∂g2

where

|di+1 − di | < dthr

(9)

(8)

where dthr = 20 cm and φthr = 0.018 rads, according to the size of the robot. Next, we extract line segments from these point clusters, according to a recursive line splitting method. In each cluster, with more than 4 points (filter out any minor objects), we calculate a line (ρ, θ) using Mean Square Error minimization. Taken into account the cartesian expression of the range data (eq. 7), we simplify the result to:

5. MAP MATCHING/ REGISTRATION Here we attempt to find correspondences between the global map features wnG n=1,..., and the local map features wkL , k=1,.... . The first step is to convert the parameters of the local map into the global coordinate system according to the robot dr dr dr pose estimate xdr R = [xR , yR , θR ] acquired by

889

where

the dead-reckoning model.Following the rules of covariance propagation:

• ζ is a weighting factor that is determined by the minimization of the trace of CXR

θkG = θR + θkL L dr dr L dr dr L ρG k = ρk + xR · cos(θR + θk ) + yR · sin(θR + θk ) L G Cw · ∇fwL T = ∇fxdr · CxGdr · ∇fxdr T + ∇fwL · Cw k k R

R

R

k

k

7. GLOBAL MAP UPDATE

(14) The usual approach is to perform a χ2 test using the Mahalanobis distance at a 90% confidence level with two degrees of freedom. In order to reduce the computational load, we search only among the lines that present similar parameters.The requirements that must be met in order to associate two line segments wnG and wkG are: G • ρG n − ρk < ερ • θnG − θkG < εθ G G G G T • (wnG − wkG ) · (Cw ≤ χ2 G − Cw G ) · (wn − wk ) n

Closing the loop, we recalculate the local map in the global coordinate system, according to the final estimate of the robot pose. At this time, we implement an algorithm of line segments merging among the previously associated line segments. Since we are dealing with features acquired in a short period of time, we are certain that the associated line segments lie close together. We determine the new extreme points of the formed line groups and we project them to the newly produced line. Finally, we update the global map with the modified features and the new acquired features.

k

Searching on the level of line polar parameters deals better with occlusions. The matched features produce improved features according to: −1 −1 G wn+ = Cw · wn ) · (Cw · wk + Cw n n+ k −1 −1 −1 G Cw ) = (Cw + Cw n n+ k

8. EXPERIMENTAL SETUP AND RESULTS

(15)

All the algorithms presented in this paper have been tested on the well-known Activmedia PioCwk the covariance matrix of the new feature neer 2-AT mobile robot. Its sensors include two expressed in the global cs, optical encoders measuring the wheel rotation on Cwn the covariance matrix of the map feature the two sides of the robot, the inertial platform expressed in the global cs, DMU-HDX (INS) unit by Crossbow with a ±2 G the covariance matrix of the produced Cw n+ Gs acceleration range and 200 deg/sec gyro range. feature There is also the home made setup of 2 caster χ2 = 5.99 wheels attached behind the robot.All the meaFrom the set of associated line segments, surements were interpolated to the time intervals G G G G wiG = (ρG of the laser scanners, giving us an update frei , θi ), wj = (ρj , θj ) we calculate the robot pose estimate and the relating covariance quency of ∼2Hz. The INS and the robot operate at matrix in a least squares error minimization sense. higher frequencies, so we use all the intermediate map θR = θiG − θiL measurements to calculate the covariances. We G · sin θ G − ρL · sin θ G − ρG · sin θ G + ρL · sin θ G ρ carried out experiments around the campus in i j i j j i j i xmap =− R front of buildings, among cars, on various tersin(θiG − θjG ) rains, at variable velocities. Following we present G G L G L G G ρi · cos θj − ρi · cos θj + (ρj − ρj ) · cos θi map = yR a representative experiment conducted in front of sin(θiG − θjG ) buildings and among parked cars. As you can see, Cxmap = ∇fwG · CwG · ∇fwG T + ∇fwL · CwL · ∇fwL T + the improvement from the path calculated by the R i i i i i i ∇fwG · CwG · ∇fwG T + ∇fwL · CwL · ∇fwL T robot’s built-in odometry, is very significant. As j j j j j j for the resulting map, we can get a more light (16) representation if we apply a clustering algorithm every few time steps, so as not to burden the on6. POSE ESTIMATION board computer even more. The clustering will take care of lines marginally not fused together. At this point, we have an estimate and a covariAt this stage, these algorithms are tested off-line. ance matrix of the robot pose from the deadThe performance in relation to time shows that reckoning model (eq. 5) and a second set from the they will perform adequately on-line, too. This map matching step (eq. 16) . A simple covariance is very important for any ”real-time” operation intersection filter can provide us with a better with limited resources in computational power. estimate, according to : The work presented here resulted in satisfactory map results concerning the accuracy of the produced dr −1 −1 XR = CXR · (ζ · CXdr · XR + (1 − ζ) · CXmap · XR ) R R map and robot path, giving errors around 30 −1 −1 −1 ) (17) + (1 − ζ) · C CXR = (ζ · CX map dr degs,10 cm in robot pose, and around 4 cm in XR R where

890

the map model. The true map was measured repeatedly by hand as carefully as possible. Some of the methods presented have not been applied to outdoors exploration and skid-steered vehicles. The covariance intersection method does not neglect correlations, but always assumes them to be non zero. It offers a suboptimal solution that it is of adequate accuracy for the outdoors. The map registration is carried out among line parameters, which is not often used and leads to a result rapidly. There is some room for improvement in the areas of compactness in the map model and faster calculation of the uncertainties.

Frantiek olc, Ludk alud, Bohumil Honzk (2001). Mathematical model of the skid-steered mobile robot. 4th IFAC symposium on Intelligent Autonomous Vehicles. G. C. Anousaki, K. J. Kyriakopoulos (2004). A dead-reckoning scheme for skid-steered vehicles in outdoor environments. International Conference on Robotics and Automation. J. A. Castellanos, J. M. M. Montiel, J. Neira and J. D. Tardos (1999). The spmap: A probabilistic framework for simultaneous localization and map building. IEEE Transactions on Robotics and Automation 15 no. 5, 948–953. J. Vandorpe, H. Van Brussel, H. Xu (1996). Exact dynamic map building for a mobile robot using geometrical primitives produced by a 2d range finder. Proceedings of International Conference on Robotics and Automation. John Folkesson, Henrik Christensen (2003). Outdoor exploration and slam using a compressed filter. Proc. IEEE International Conference on Robotics and Automation pp. 419–426. Kai O. Arras, Sjur J. Vestli (1998). Hybrid, highprecision localization for the mail distributing mobile robot system mops. Proceedings of International Conference on Robotics and Automation. K¨ampke, Thomas (2001). Navigation methods for autonomous and partially autonomous mobile systems. Human-friendly welfare robot system engineering research center Newsletter. Luca Caracciolo, Alessandro De Luca, Stefano Iannitti (1999). Trajectory tracking control of a four-wheel differentially driven mobile robot. Proceedings of the IEEE ICRA pp. 2632–2638. M. W. M. G. Dissanayake, P. Newman, S. Clark H.F. Durrant-Whyte M. Csorba (2001). A solution to the simultaneous localization and map building (slam) problem. IEEE Transactions on Robotics and Automation 17 no. 3, 229–241. Maybeck, P. S. (1979). Stochastic models, estimation and control. In: Mathematics in Science and engineering. Vol. 141-1. Chap. 6. Academic Press. P. Jensfelt, H. I. Christensen (2001). Pose tracking using laser scanning and minimalistic environmental models. IEEE Transactions on Robotics and Automation. Puneet Goel, Stergios I. Roumeliotis, Gaurav S. Sukhatme (1999). Robust localization using relative and absolute position estimates. Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems pp. 1134– 1140. S. Takezono, H. Minamoto, K. Tao (1999). Twodimensional motion of four-wheel vehicles. Vehicle Systems Dynamics 32, 441–458.

Fig. 3. Experimental Setup

Fig. 4. The map of the area and the path (green line) of the robot during exploration

REFERENCES Billur Barshan, Hugh F. Durrant-Whyte (1995). Inertial navigation systems for mobile robots. IEEE Transactions on Robotics and Automation. Durrant-Whyte, Hugh F. (1988). Integration,Coordination and Control of Multi-Sensor Robot Systems. Kluwer Academic Publishers. F. Masson, J. Guivant, E. Nebot (2002). Architecture for simultaneous localization and map building in large outdoor areas. Proceedings of the IEEE/RSJ, Int. Conference on Intelligent Robots and Systems, EPFL.

891