Laser Radar based Real-time Ego-motion Estimation for Intelligent Vehicles Ming Yang, Bin Dong, Hong Wang, Bo Zhang State Key Laboratory of Intelligent Technology and Systems Tsinghua University, 100084 Beijing, CHINA

[email protected] Abstract Ego-motion estimation is a key issue in intelligent vehicles and important to moving objects tracking. In this paper, after a simple overview of existed method, we present a tangent based hybrid real-time ego-motion estimation algorithm based on laser radar, which is composed of Iterative Tangent weighted Closest Point (ITCP) and Hough transform based Tangent Angle Histogram (HTAH) algorithms to overcome problems with past methods, such as local minimum, aperture-like, and high computation problem, etc. This algorithm has been tested on both synthetic data and real range data in outdoor environment. Experimental results demonstrate its high accuracy, low computation, wide applicability and high robustness to aperture-like problem, occlusion and noises.

1. Introduction Detecting and tracking moving obstacles is one of the key issues to the successful application of intelligent vehicles. The inherent difficulty here is to distinguish the moving objects from stationary objects since both of them appear to be moving from a moving vehicle. The most intuitional solution is to compensate the vehicle’s egomotion, which leads to the problem of ego-motion estimation. In a 2D environment, the vehicle’s ego-motion can be described by a triplet (vx,vy,w). If the interval ∆T between two instants is measurable, the problem of egomotion estimation turns to the problem of relative pose estimation as follows, v x = t x ∆T , v y = t y ∆T , w = θ ∆T where (tx,ty,θ ) is the vehicle’s relative pose between two instants, known as translation (tx,ty) and rotation θ . Pose estimation is a fundamental problem for mobile robots. Many approaches have been proposed to solve this problem in the past 20 years [1]. The simplest and cheapest method is Dead Reckoning (DR) using wheel encoders. Relative pose is obtained by step counting the revolution of wheels. However, DR method is intrinsically vulnerable to bad calibration, imperfect wheel contact, upsetting events, etc. Inertial Navigation Systems (INS) use multi-

ple gyros and accelerometers to provide an acceleration history for relative pose estimation. Although having higher accuracy than DR, INS method is more expensive and subject to gyro drift, calibration problems, and sensitivity limitations. Both DR and INS methods are not suit for global pose estimation due to the large cumulative error over time. Beacon based method doesn’t suffer from this problem since beacons are placed in the environment and identified by sensors onboard the robot. However, this method isn’t appropriate in many situations, especially outdoor environment. A variant of beacon-based method is to triangulate position, which uses features of naturally occurring landmarks extracted from a camera. Major drawbacks of this method are the poor accuracy as well as the need for a suitable environment. A better solution is to use laser radar mounted on the vehicle since no modification of the environment is required. Thanks to the high accuracy of laser radar, high relative pose estimation can be obtained by registration of two adjacent range scan. Besides that, in our case, using laser radar instead of other sensors allows for a simple integration of pose information into other laser radar based algorithms without the need for calibration between sensors, such as obstacle tracking and avoidance [2], map building, etc, which reduces maintenance and cost. In the next section, we first analyze and compare some typical existed range data based pose estimation algorithm. Then, we present a real-time tangent based hybrid relative pose estimation algorithm for outdoor environment, which requires higher accuracy and robustness.

2. Laser radar based pose estimation 2.1. State of arts The task of relative pose estimation is to find the best transformation (tx,ty,θ ), which registers last scan with current scan perfectly. Many algorithms have been proposed in the last decade. The most straightforward one is the search-based method. After computing the error between two scans for every cell in the quantized 3D pose space, we can take the cell with minimal error as the solution. Although having the advantages of global minimum and

easy implementation, this method suffers from the high computation due to the large 3D search space. To address this problem, Weiss proposed an Angle Histogram (AH) algorithm [3], which reduces 3D search to three 1D searches. However, this method requires the environments to have obvious line features. The accuracy of searchbased method is limited in practice since it depends on the level of quantization. Correspondence based method is another common method for pose estimation, which consists of correspondence search and error minimization. The former one determines which primitives in each scan represent the same object in the world, while the latter estimates the relative pose by minimizing the error of correspondences. These two steps can be iteratively performed to obtain higher accuracy. The primitives used in correspondence search can be features, such as line segments and corners if the environment is polygonal. Iterative Closest Point (ICP) algorithm is a general-purpose algorithm for shape registration [4,5], which directly uses the range points as primitives. Since more data is used, ICP has higher accuracy and robustness than other feature based algorithm. However, it suffers from high computation and slow convergence for the same reason. All the correspondence based methods have the problem of local minimum. Other methods include: Velocity Constraint Equation algorithm based on the spatial and temporal linearization of the range function [6], which is inspired by the Optical Flow Constraint Equation in computer vision; Anchor Point Relation matching algorithm [7], which makes use of relations between characteristic coordinates of object properties in order to prevent information loss and expensive computation in line abstraction; Principal Components Analysis based algorithm [8], which works in high dimension space rather than in 3D Cartesian coordinate, but only suits for fixed environment since training is needed.

Table 1. Comparison between ICP and AH ICP AH Accuracy High Low Computation High Low Environment All Polygonal Local Minimum Yes No Robustness High High According to the complementation between ICP and AH, we present a tangent based hybrid method composed of the following part, 1. Robust tangents computation by M-estimators; 2. Initial rotation estimation by Hough transform based Tangent Angle Histogram (HTAH); 3. Initial translation estimation by Iterative Tangent weighted Closest Point (ITCP); 4. Relative pose estimation by Iterative Tangent weighted Closest Point (ITCP). The combination of ICP and AH, on one hand, overcomes the low convergence speed and the local minimum problem with ICP by the initial rotation estimation from AH; on the other hand, keeps the algorithm away from low accuracy and polygonal environment requirement with AH. The introduction of tangent information makes AH more efficient and improves the robustness of ICP to noises, occlusion, and aperture-like problem. In the rest of this paper, we first discuss the problem of robust computation of tangent in section 3. Then, Hough based Tangent Angle Histogram (HTAH) and Iterative Tangent weighted Closest Point (ITCP) are described in section 4 and 5, respectively. This method has been tested on Tsinghua Mobile Robot V (THMR-V), which is an intelligent vehicle developed at Tsinghua University. Some experimental results with both synthetic and real data are presented in section 6. In the end, we conclude with some conclusions and future works.

3. Robust computation of tangent direction 2.2. Tangent based hybrid pose estimation For the relative pose estimation in intelligent vehicles, the algorithm must satisfy the following requirements: (a) High Accuracy, which determines the success of moving objects tracking and other related applications; (b) Low Computation, which meets the real-time requirement of mobile applications with high velocity; (c) Wide Applicability, which enables the intelligent vehicle work in all kinds of environment rather than only polygonal environment or fixed environment; (d) High Robustness, which ensures the intelligent vehicle against the occlusion and noise existed in real outdoor environments. Although none of above algorithm can satisfy all of these requirements, some of them are complemental which makes it possible to fuse different methods. For example, a comparison between ICP and AH is show in Table 1.

If linking adjacent range points in turn, we obtain a piecewise linear approximation of environment’s profile, which can be used to compute the tangent direction at each point. Let Pi (xi , yi), i = 1,…,n, be the i th range point, tangent direction φi at Pi can be approximately estimated by its two neighboring points as follows [5], y −y  φi = tg −1  i +1 i −1  , i = 2, , n − 1  xi +1 − xi −1  This approximation is simple, fast, and easy to implementation. However, it’s subject to noises and occlusion since only two neighboring points are taken into consideration. In order to consider more points, a common classical line fitting method -- Least-Square Estimator (LSE) can be applied here. Although LSE is able to handle the Gaussian white noises in the data by minimizing the sum of

L

squares of residual errors, some errors cannot be modeled as Gaussian white noise, such as noises by occlusion. Therefore, robust estimation is expected here since sometimes even one such bad data will perturb LSE. During the last three decades, many robust techniques have been proposed [10]. In this paper, we use a M-estimator to compute the tangent direction since it can be implemented in the form of weighted LSE, which has the advantage of easy implementation and low computation due to the available closed-form solution. Considering the 2n+1 neighbors around Pi , we can define the error function as follows, 2 n +1 2 min E (φ , d ) = ∑ λi ( xi cosφ + yi sin φ − d ) (1) i =1

where, (φ ,d ) are the direction and perpendicular distance of tangent to be estimated; λi is the weight function of Mestimators, which can be selected as follows, 1,  λi = σ ei ,  0,

ei ≤ σ

σ < ei ≤ 3σ , i = 1, 3σ < ei

L , 2n + 1

(2)

where, ei is the residual error of neighboring point i; σ is the standard deviation of residual errors, which can be estimated by a standard LSE. A closed-form solution of (1) can be derived as follows, 2 n +1 1 −1 − 2∑i =1 λi (xi − x )( yi − y ) (3) φ = tg 2 n +1 2 2 2 λ ( y − y ) − ( x − x ) ∑i=1 i i i

[

2 n +1

x = ∑i =1 λi xi

∑

2 n +1

i =1

]

2 n +1

λi , y = ∑i =1 λi y i

∑

2 n +1

i =1

λi (4)

4. Hough transform based tangent angle histogram 4.1. Angle histogram Taking the assumption that two adjacent points in a scan represent a line, we can easily compute the line direction. By doing this for all pairs of adjacent points and rounding off the directions to discrete values, we obtain the discrete distribution of these directions in one scan, called Angle Histogram (AH). The analyses show that an angle histogram is approximately invariant against translation, and rotation will only show a phase shift in the histogram if there is no limitation for sensors. This is because the angle histogram is a representation of the distribution of directions. Therefore, we can estimate the rotation by measuring the phase shift between two angle histograms from the adjacent scans, which can be done by the crosscorrelation of two histograms. A normalized crosscorrelation function is given as follows, n ∑i=1[(hc (i ) − hc )⋅ (hl (i + j ) − hl )] (5) c( j ) = 2 2 n n ∑i=1 (hc (i ) − hc ) ⋅ ∑i=1 (hl (i ) − hl )

where, hc and hl are angle histograms of current and last scan, respectively; hc and hl are the mean values of two histogram; n is the length of the histogram, which is related to the quantization of direction; c is the measurement of cross-correlation, where zero means uncorrelated while one means fully correlated. The advantage of using a normalized cross-correlation function is that we can measure how good these two histogram correlates. In practice, AH method is robust to small far moving objects since they only produce few points in the range scan. However, they aren’t neglectable if they are near the sensor. This problem can be solved by using the lengthangle histogram instead of the angle histogram, which represents the total length of lines with the same direction. Occlusion problem is another problem with AH method shown as AB in Fig.1. A sharp angle criterion can be used here to B check the occlusion: If ∠AOB is smaller than a A given threshold, Line(AB) is considered as occlusion O edge and excluded from Figure 1. Occlusion the statistic of angle histogram.

4.2. Hough transform based angle histogram Since only one neighboring point is considered in direction computation, the standard AH method is subject to the noise in range measurement. In order to improve the robustness of AH, we can use more neighboring points in the computation of angle histogram by applying a classical robust line fitting method, called Hough Transform, which is a discredited version of Maximum Likelihood Estimation. Dubrawski presented a Hough transform based Angle Histogram (HAH) method with polar representation [11], where segments of collinear points are searched by finding groups of neighboring points that belong to lines which bear the same direction as shown in Fig. 3.a Given a direction α, the perpendicular distance of the line through point Pi can be computed by, (6) d i = Ri ⋅ sin (ϕ i − α ) where (Ri,ϕi) are polar coordinates of Pi. The collinearity of the 2n+1 points around Pi can be checked by (7) ∀j ∈ [i − n, i + n ], d j − d 'i ≤ t line where, d´i = (di-n+di+n)/2 is the average distance; tline is the collinearity threshold. Although the Hough transform improves the robustness to noises, the computation complexity increases from O(n) to O(m⋅n) at the same time, where n is the number of points in one scan and m is the length of histogram.

According to the fact that directions in one scan only appear in a small fraction of whole 180° scope, it is possible to only compute the histogram at directions within Angle-of-Interest (AOI), which reduces the computation by decreasing m. Considering the little difference between last and current scan, we can compute the AOI from the standard angle histogram of last scan by a threshold as the dash lines shown in Fig. 2. With AOI, the computation could be reduced 2-5 times depending on the structure of the environment.

4.3. Hough transform based tangent angle histogram In HAH method, Line(d´i,α) is considered as an approximation of the 2n+1 points near Pi. However, this simplification makes it sensitive to noises in Pi-n and Pi+n. In addition, the computation of HAH is still too high to meet the real-time requirements even with AOI. In order to address the above problems, we present a Hough transform based Tangent Angle Histogram (HTAH) method. The tangent angle histogram is a generalization of angle histogram and represents the distribution of tangent’s direction at each point rather than the

Length of Lines(cm)

300

direction of line segment formed by two adjacent points. This makes it more robust to noises. In addition, α0 from robust tangent computation is taken as the estimation of line direction of the 2n+1 points, which also improves the robustness. From the fact that votes from Pi are usually continuous in histogram, we simplify the vote process of Pi as following iterative steps, 1) Let α´=α0 ; 2) For direction α=α´+∆α, where ∆α is the resolution of angle histogram; 3) Compute the perpendicular distance d of the line through Pi with direction α by (6); 4) Check collinearity of the 2n neighbors by the boundary lines as shown in Fig. 3.b; 5) If collinear, let α´=α and go back to step 2. The reverse search is completely same if we simply replace α=α´+∆α with α=α´-∆α. By this search strategy, HTAH is 5-10 times faster than HAH.

5. Iterative tangent weighted closest point 5.1. Closest point for correspondence Let Pi be a point in current scan, the ICP algorithm considers its Closest Point (CP) in last scan as the correspondence point P´i. The closest point can be defined as the point satisfying [5], min dist (Pi , Pj )

L

250

j =1, ,n

where Pj is j th point in last scan; dist(⋅) is the distance function of two points, such as commonly used Euclidean distance. For the sake of pseudo match problem produced by limited samples and occlusion, a better definition is to select the orthogonal point of Pi with the line segment, which meets, (8) min dist (Pi , line(Pj , Pj +1 ))

200 150 100

L

50

j =1, , n −1

0 0

50

100 degree

150

Figure 2. Computation of AOI Pi+n ∆α

Y

Y

Pj

boundary line

Pj

Pi-n

di+n di-n

Pi

α0 α

α

d'i

boundary line

X

t

t

X

t

t

(a) (b) Figure 3. HAH method(a) and HTAH method(b)

Lu presented another correspondence rule: MatchingRange-Point (MRP) rule [9], whose nature is closest point rule with polar distance. Although MRP rule converges faster than CP rule, especially in rotation, it has a potential stable problem if the searching scope of correspondence is fixed. Assuming the motion between two adjacent scan is limited, we can use a maximal search radius Br to reduce the number of candidate points in correspondence search. Br can be adaptively selected by an exponentially decreasing function or using mean and deviation of residual errors in the iteration. Besides reducing computation, Br has an effect of removing outliers by occlusion, limitation of sensors, moving objects, etc. Another criterion to reject outliers is consistency of tangent directions of two matching points taking the assumption of small rotation between two scans.

5.2. Aperture-like problem

5.3. Iterative tangent weighted closest point

Given m matching points {Pi,P’i}, it is possible to estimate the relative pose by minimizing the error function between two scans defined as follows, 1 m 2 (9) E dist (θ , T ) = ∑i =1 Rθ Pi + T − P ' i m Usually, a least-square estimator can be used to solve this minimization problem. Since the noises in matches by outliers can hardly be modeled as Gaussian white noise, it’s better to use a robust estimation here [5]. However, this will lead ICP to failure in aperture-like problem. As known in optical flow, the motion is locally ambiguous in aperture problem due to the finite receptive field of sensor. For instance, if the sensor moves in the environment as shown in Fig. 4.a, where Al and Ac are the same line in last and current scan, respectively. Since all the matches are from the main direction of the environment, the motion along line A is indistinguishable. Aperture problem is unsolvable without the help of other sensor. Aperture-like problem is a little different from the aperture problem, where few matches from non-main direction exist in the environment, as line B show in Fig. 4.b. In principle, aperture-like problem is solvable. Let’s consider a common case where the sensor moving along the main direction as shown in Fig. 4.b. By CP rules, the points from main direction are wrongly matched while the points from non-main direction are matched correctly. A´l and B´l are the results of least-square estimation. For the ICP based on LSE, the accurate estimation can hardly be obtained due to the slow convergence in this case. For the robust method, the correct matches from line B are discarded as outliers due to larger residuals, while the wrong matches from line A are reserved. This leads to a pure aperture problem, which cannot be solved in principle. The problem with robust method exists in that the residual cannot be used to check outliers alone.

To address the aperture-like problem, we present Iterative Tangent weighted Closest Point (ITCP), whose basic idea is to let the matches from non-main direction have same total weights as the matches from main direction. By this, the translation along main direction could converge much faster. The weights can be selected as follows, Pi ∈ main direction m (2 × nmain ) (10) λi =  ( ) m 2 × n P non−main i ∈ non − main direction  where, nnon-main and nmain are the number of matches from non-main direction and main direction, respectively; m is the number of matches. Now, (9) is turned to a weighted least-square form as 1 m 2 (11) E dist (θ , T ) = ∑i =1 λi Rθ Pi + T − P 'i m A closed-form solution of (11) can be derived, m λ [(xi − xc )( y 'i − y ') − ( yi − y )(x'i − x ')] −1 ∑i =1 i (12) θ = tg m λ [ ( x − x )( x ' − x ' ) + ( y − y )( y ' − y ' ) ] ∑i=1 i i c i i i

Bc B' l Bl

y o

y

T(tx,ty) Ac

x

o

T(tx,ty) Ac

x

A' l Al

Al (a)

(b)

Figure 4. Aperture(a) and aperture-like(b) problem

t x = x '−(x cosθ − y sinθ ) , t y = y '−(x sin θ + y cosθ )

(13)

x = ∑i =1 λi xi m , y = ∑i =1 λi yi m ,

(14)

m

m

x ' = ∑i =1 λi x'i m , y ' = ∑i =1 λi y 'i m m

m

(15)

6. Experiments The proposed algorithms in this paper have been implemented in both Matlab and C++ on a 1.33GHz PC without any code optimization. Both synthetic and real data have been applied in order to test the performance.

6.1. Experiments with synthetic data A digital map of Tsinghua University is used to generate the synthetic range data with a maximal range of 50m, range resolution of 1cm, field-of-view of 180°, angular resolution of 0.5°. In order to evaluate the estimated result, true poses are recorded at the same time. Fig. 5 shows a typical example, where the relative pose between two scans is 25.07° for rotation and (-243.9cm, 356.8cm) for translation (Fig. 5.a). After the robust estimations of tangent directions by M-estimator described in section 3, angle histograms of both scans are computed with a resolution of 0.2° by HTAH algorithm presented in section 4(Fig. 5.b). Cross-correlation of these two histograms shows a clear maximum near 25.2°(Fig. 5.c). Taking this result as the initial rotation estimation, the ITCP successfully converge in 14 steps (Fig. 5.d-e). In the end, last scan are transformed with estimated relative pose (Fig. 5.f). Comparing Fig. 5.f with 5.a, we can find two scans register perfectly, which illuminates the high accuracy in this algorithm. Same result can be found from the error

analysis in Table 2., where the relative error is defined as the ratio of the translation error to the maximal range of scan. Table 2. Results of a typical example True Estimated Error Relative Error 25.07° 25.20° 0.13° -243.9cm -243.3cm 0.6cm 0.012% 356.8cm 354.4cm 2.4cm 0.048%

In order to further testify the performance, some scan sequences are generated. Satisfactory results are obtained for all the sequences. Fig. 6 shows the environment and pose track of one sequence with 929 scans. Several AH algorithm mentioned above are tested. Results in Table 3 show that both HAH and HTAH are more accurate than standard AH’s owing to the use of Hough transform. Compared with HAH, HTAH is 8 times faster with slightly higher accuracy. Then, we test both ICP and ITCP on the same sequence with the rotation estimation from HTAH. Thanks to combination with HTAH, both ICP and ITCP converge quickly. The average time for ITCP is about 14ms/scan, which is a little faster than 16ms of ICP. This is because

Figure 6. Pose track of a synthetic scan sequence

5

Current Last 30

60

20

50

2 1.5 1 0.5

-1000

0

20 40 60 80 100 120 140 160 180 Angle

10

-30 0

200

20

2

4

6 8 Iteration

(c)

10

12

14

(d) 4500 4000 3500 3000 2500 2000 1500 1000 500 0 -500

200

50

2

4

6 8 Iteration

10

12

14

1000

-2000-1000

2000 Y(cm)

0

0

1000 2000 3000 4000 X(cm)

(e) (f) Figure 5. A typical example (a) shows current and last scan; (b) are HTAH of both scans; (c) is the cross-correlation of two HTAHs; (d) and (e) show the convergence of translation and residual, respectively; (f) shows last scan and its transformed scan with estimated relative pose.

0

-2000 -4000 200

400

600 Scan

800

1000

5000

(c)

Trans. Last

10000 X(cm)

15000

(d)

6000

6000

4000

4000

2000

2000

Y(cm)

100

800

4000

-5 0

Y(cm)

150

400 600 Scan

6000

0

-300

30

200

(b)

Y(cm)

0 10 degree

0

-10 0

1000

Tx Ty

100

-200 -10

800

5

200

-100

-20

400 600 Scan

(a)

300

250 Distance Residual(cm)

20

400

0.5 0.4 0.3 0.2 0.1 0 -30

0

30

0

-20

(b) Translation(cm)

Cross-correlation Value

(a) 1 0.9 0.8 0.7 0.6

40

10

-10

0 0

1000 2000 3000 4000 X(cm)

Translation in Y(cm)

3 2.5 Translation in X(cm)

Length of Lines

3.5

Y(cm)

3000 2500 2000 1500 1000 500 0

x 10

4

Current Last

4500 4000 3500

Rotation(degree)

θ tx ty

ICP need more iteration in aperture-like case. Relative pose estimation by ITCP is shown in Fig. 7.a-c, where the scans between dot lines are aperture scans, which is automatically detected by a threshold of ratio of nnon-main/nmain. Since the translation estimation in aperture scan is not available in principle, they are excluded from the error statistic. The comparison in Table 4 shows that ITCP is a little more accurate than ICP in rotation estimation, but much better in translation estimation since ICP suffers from the aperture-like problem while ITCP doesn’t. Similar conclusion can be found in the comparison of pose track between ICP (Fig. 7.e) and ITCP (Fig. 7.f).

0

-2000

0

-2000

-4000

-4000 5000

10000 X(cm)

15000

5000

10000 X(cm)

15000

(e) (f) Figure 7. Results of ITCP (a) and (b) are the estimation of translation in X and Y, respectively; (c) is the estimation of rotation; (d) is the true pose track; (e) and (f) show the pose track by ICP and ITCP, respectively.

Time/scan 2 ms 74 ms 24 ms 9 ms

Table 4. Errors comparison between ICP and ITCP ICP ITCP Mean Std. Mean Std. θ 0.026° 0.039° 0.022° 0.037° tx 0.021% 0.023% 0.013% 0.021% ty 0.059% 0.084% 0.017% 0.027% In order to testify the robustness of the proposed algorithms, we add Gaussian white noise with different standard deviation to the synthetic data. Results in Fig. 8.a show that HAH and HTAH have similar robustness, which is much better than the standard AH as the result of using Hough transform. Since the noise in real range measurement from laser radar in our case is about 5-10cm, both ICP and ITCP have enough robustness to noises as shown in Fig. 8.b-d. However, similar to the above results, ITCP always exhibits better performance than ICP in both rotation and translation estimation.

6.2. Experiments with real data

0.08 0.06

0.02

0 0

10 20 30 40 Magnitude of noise(cm)

0 0

50

10 20 30 40 Magnitude of noise(cm)

x 10

ICP ITCP

1.5

2

0.5

x 10

(c)

50

400 scan

600

50

(d)

Figure 8. Comparisons of robustness to noises (a) is the comparison in rotation estimation among different AH method; (b)-(d) show the comparison between ICP and ITCP.

400 scan

600

(b) -1000

Y(cm)

-2000

0

-3000 -4000 -5000

-10 -6000

-20 0

200

400 scan

600

-7000 -2000

0

2000 4000 X(cm)

6000

(c) (d) Figure 9. An example of straight-line motion (a)-(c) are velocity in x, y, and angular velocity, respectively; (d) is the pose track by relative pose estimation. 5 4 3 2 1 0 -1 -2 -3 -4 -5 0

ICP ITCP

10 20 30 40 Magnitude of noise(cm)

200

12 10 8 6 4 2 0 200

400

600 scan

800

-2 0

1000 1200

200

(a)

1

0 0

0

-5 0

20

-3

0.5

10 20 30 40 Magnitude of noise(cm)

200

5

(a)

50

1.5

1

0 0

-10 0

(b)

-3

Relative error of translation in Y

Relative error of translation in X

(a) 2

-5

Velocity in Y(m/s)

0.04

0.2

10

0

400

600 800 scan

1000 1200

(b)

30

2000

25

1500

20

1000

Y(cm)

0.4

15

5

Velocity in X(m/s)

0.6

ICP ITCP

Error of Rotation(degree)

Mean Error of Rotation(degree)

0.1 AH HAH HAH with AOI HTAH

20

10

10

The proposed algorithms have also been tested on real data from TsingHua Mobile Robot V (THMR-V), which is an intelligent vehicle developed at Tsinghua University. A LMS 220 laser radar made by Sick Ltd. is equipped on THMR-V to provide range data. It is a pulsed Time-OfFlight (TOF) radar system with a maximal range of 50m, a range resolution of 5cm, an angular resolution of 0.5°, 0.8

scan time of 40ms, field-of-view of 180°. The outdoor environment is composed of buildings, walls, posts, barriers, cars, and even few small moving objects. In the first example, the vehicle moved straight (Fig. 9). A clear accelerated motion can be found in the first 100 scans (Fig. 9.b). The estimated velocity in x and angular velocity are remained near zero in all the 700 scans as expected (Fig. 9.a&c). In the second example, the vehicle moved along a circle with a diameter about 27 meters for nearly two cycles as shown in Fig. 10. Although this algorithm is not intended to be used for global pose estimation due to the cumulative error over time, the two circle in pose tracks fit each other quite well by relative pose estimation in the 1200 scans, which strongly proves the high accuracy and robustness of proposed algorithms.

Velocity in Y(m/s)

Std. 0.523° 0.080° 0.083° 0.070°

Velocity in X(m/s)

Max. 2.545° 0.352° 0.346° 0.275°

Angular Velocity(degree/s)

AH HAH HAH with AOI HTAH

Mean 0.338° 0.048° 0.050° 0.042°

Angular velocity(angle/scan)

Table 3. Error comparisons among different AH

15

500

10 0 5 0 0

-500 200

400

600 800 scan

1000 1200

0

500 1000 1500 2000 2500 X(cm)

(c) (d) Figure 10. An example of circular motion (a)-(c) are velocities in x, y, and angle, respectively; (d) is the pose track by relative pose estimation.

7. Conclusions and future work In this paper, we present a tangent based hybrid relative pose estimation algorithm with real-time performance, which is composed of HTAH algorithm and ITCP algorithm. The combination of these two algorithms succeed in addressing not only low convergence speed and local minimum problem with ICP algorithm, but also low accuracy and polygonal environment requirement with AH algorithm. Experiments on both synthetic and real data prove its high accuracy, real-time performance, and robustness to noises and outliers. The introduction of tangent information obtained from a robust estimation greatly improves system’s robustness to noises. Instead of standard AH algorithm, we present a hough transform based tangent AH algorithm, which is a generalization of the standard AH algorithm. Experimental results show that HTAH is 5-10 times faster than existed Hough transform based method while the accuracy and robustness remain the same. As a modification of ICP algorithm, we present an iterative tangent weighted closest point algorithm to address the aperture-like problem disturbing most robust method. This modification speeds up the convergence of ICP and improves the robustness as shown in experimental results. Future work is expected to estimate the relative pose by a Kalman filter to further improve the accuracy and address the aperture problem. The combinations with object tracking and map building are also considered.

Acknowledgements This research was supported in part by Chinese High Technology Development Program and Portugal-China Science and Technology Cooperation Project. We wish to thank Prof. Helder Araujo for his valuable comments and discussions, Mr. Jinkai Yu for his collaboration in the early development of the simulated environment.

References [1] M. Yang, H. Wang, B. Zhang, “Laser Radar Based Pose Estimation For Mobile Robots”, Chinese Journal of Robots, 2001, Accepted. (In Chinese)

[2] M. Yang, H. Wang, K.Z. He, B. Zhang, “Obstacle Avoidance using Range Data in Autonomous Navigation of Mobile Robots”, In Procs. of International Symposium on Test and Measurement’99, (Xi'an, China), pp.929-933, Jun. 1999. [3] G. Weiss, C. Wetzler, E.V. Puttkamer, “Keeping Track of Position and Orientation of Moving Indoor Systems by Correlation of Range-Finder Scans”, in Procs. IEEE/RSJ/GI International Conference on Intelligent Robots and Systems'94, (Munich, Germany), vol.1, pp.595-601, Sep. 1994.

[4] P.J. Besl, N. .D. McKay, “A Method for Registration of 3-D Shapes”, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.14, no.2, pp.239-256, Feb. 1992. [5] Z. Zhang, “Iterative Point Matching for Registration of Freeform Curves”, INRIA Technical Report, RR-1658, INRIA, 1992. [6] J. Gonzalez, R. Gutierrez, “Mobile Robot Motion Estimation from a Range Scan Sequence”, in Procs. of IEEE International Conference on Robotics and Automation, vol.2, pp.1034–1039, 1997. [7] J. Weber, K.W. Jorg, E. Puttkamer, “APR - Global Scan Matching Using Anchor Point Relationships”, in Procs. of 6th International Conference on Intelligent Autonomous Systems, (Venice, Italy), pp.471-478, Jul. 2000. [8] J.L. Crowley, F. Wallner, B. Schiele, “Position Estimation Using Principal Components of Range Data”, in Procs. of IEEE International Conference on Robotics and Automation, vol.4, pp.3121-3128, 1998. [9] F. Lu, E. Milios, “Robot Pose Estimation in Unknown Environments by matching 2D Range Scans”, Journal of Intelligent and Robotic Systems, vol.18, pp.249-275, 1997. [10] Z. Zhang, “Parameter Estimation Techniques: A Tutorial with Application to Conic Fitting”, Image and Vision Com-

puting Journal, vol.15, no.1, pp.59-76, 1997. [11] A. Dubrawski, B. Siemiatkowska, “A Method for Tracking the Pose of a Mobile Robot Equipped with a Scanning Laser Range Finder”, in Procs. IEEE International Conference on Robotics and Automation’98, vol.3, pp.2518-2523, 1998.