LADAR-Based Vehicle Tracking and Trajectory Estimation for Urban Driving

Transcription

1 LADAR-Based Vehile Traking and Trajetory Estimation for Urban Driving Daniel Morris, Paul Haley, William Zahar, Steve MLean General Dynamis Roboti Systems, Tel: , Fax: Abstrat Safe mobility for unmanned ground vehiles requires reliable detetion of other vehiles, along with preise estimates of their loations and trajetories. Here we desribe the algorithms and system we have developed for aurate trajetory estimation of nearby vehiles using an onboard sanning LADAR. We introdue a variable-axis Akerman steering model and ompare this to an independent steering model. Then for robust traking we propose a multi-hypothesis traker that ombines these kinemati models to leverage the strengths of eah. When trajetories estimated with our tehniques are input into a planner, they enable an unmanned vehile to negotiate traffi in urban environments. Results have been evaluated running in real time on a moving vehile with a sanning LADAR. 1. Introdution Vehile detetion, traking and trajetory estimation are ruial omponents of the mobility system for unmanned ground vehiles (UGVs. There has been signifiant work in this area for highway driving where the lane network is well strutured, see [12] for an overview. Muh less work addresses the problems of urban driving with the need to negotiate with lose-in maneuvering vehiles, parked vehiles and other objets. This 1

2 work fouses on traking and estimating the trajetories of nearby vehiles in urban type of lutter. LADAR sensing is one of the most useful modalities for deteting and loating stationary objets, see [1], [3], [4] and [13]. It provides aurate day/night range estimates that an be readily aumulated to enable ategorization [4]. However, appliation to moving objets has proven more diffiult due to the registration diffiulty. Detetion and traking based on global segmentation [2] along with luster similarity [9], [11] and feature detetion [8], [10], [14] have been proposed. In all these ases very simple motion models are used that do not take advantage of the nonholonomi kinemati onstraints of vehiles. In this paper we investigate how inorporating target vehile kinematis and shape information an improve the auray and robustness of vehile traking from a UGV. To do this we introdue a variable-axis Akerman steering model (VASM and ompare this with the more standard independent steering model (ISM. These models are adaptive and general enough for traking most vehile types. Then we propose a ombined multi-hypothesis traker that leverages the strengths of eah of these to gain both effiieny and robustness. We show results of real-time vehile traking from a moving UGV. 2. Vehile Modeling We address both kinemati and shape modeling of vehiles. Our two different kinemati models are desribed in Setions 2.1 and 2.2. For shape modeling we assume an adjustable retangular outline for vehiles aligned with the vehile orientation whose dimensions are inrementally estimated as desribed in Setion

3 ( θ, θ θ ( v, θ ( x, y ( x, y ( x, y x y C L w x w y w x w y (a (b Figure 1 Parameters for (a ISM and (b VASM. Vehile veloity is independent of C 0 vehile orientation in the ISM but is onstrained to follow a partiular ar in the VASM. 2.1 Independent Steered Model (ISM The ISM is so alled sine its veloity diretion is not onstrained by its orientation, see Figure 1(a. It is desribed by a 6-parameter onstant veloity model with a state vetor, x ( t k, ontaining position, x, y, orientation, θ, and their time derivatives, along with a state transition matrix, Φ ( t k : r x x y x = y, θ θ 1 t 1 1 t Φ = 1, (1 1 t 1 where for notational simpliity the time, t k, is dropped. A system noise matrix an be derived by assuming an unknown ontinuous white noise aeleration in the interval Treating the x, y, θ omponents independently, the differential state transition matrix for t. the ( x ẋ T omponent is 0 1 F x =, and so x an be expressed as the solution to the 0 0 differential equation: 3

4 T 0 0 x = Fxx + xfx +, (2 0 α where α is the variane of the white noise aeleration term. Given that symmetri, we integrate its omponents over the interval t to obtain: x is 3 2 t 3 t 2 x = α 2. (3 t 2 t The same proedure is followed to obtain y and θ, whih are idential exept that θ has a different white noise variane, β. Hene the full system noise is: x = y. (4 θ 2.2 Variable-axis Akerman Steered Model (VASM The VASM moves along an ar illustrated in Figure 1(b and is speified by a six parameter state vetor: x = ( x y L v θ θ T. Here x, y, is the enter position, L is the loation along the enterline of the rotation axis in the vehile-entered oordinates, v is the ar speed, and θ, θ are the orientation and angular speed respetively. One of the hallenges in using VASM is that urved trajetories are quite sensitive to the position of rotation axis, L, whih for most ars is the unknown loation of the rear-wheel axis. By estimating this, our model an adapt to a variety of vehiles, inluding skid-steering vehiles that an have a varying rotation axis. Estimating L also enables the model to adjust to either forward or bakward vehile orientations simplifying initialization. While the position is in world oordinates, it is easiest to derive the state transition matrix in the loal oordinate systems, C 0 and C, defined in Figure 1(b, and then to transform to world oordinates. We use the notation 0 x, and x to denote positions 4

5 in these oordinate systems. First onsider a point at the origin of the C 0 oordinate system. Its loation at time t is given by: 0 t ( = os( θ = sin( θ 0 t ( = ( θ = θ ( θ 0 x t v t dt v t t y t v sin t dt v t sin t (5 where we are using a onstant speed, v, and onstant angular speed, θ, assumption. If instead we work in the vehile enter oordinate system and alulate the enter loation at time t we get: 0 2 ( = ( + os( θ = sin( θ + 2 sin ( θ ( = ( sin ( θ = θ sin ( θ 2 2 sin ( θ x t x t L L t v t t L t y t y t L t v t t L t (6 For simpliity we ontinue to let θ be the orientation of the vehile, and v be the speed of the rear axle (or more speifially, the point on the enterline whose veloity is parallel to the orientation, θ. The veloity at the vehile enter will have an orientation offset θ and speed v given by: ( L v v v ( θ = artan 2 θ,, = os θ. (7 Now the loal oordinate update step in Eq. (6 an be transformed to world oordinates as an update from time t to t + t : xt + t xt x( t = + R ( θt yt + t y t y ( t, where ( os( θt sin ( θt R θt sin ( θt os( θt = (8 From this we obtain the disrete state transition matrix: 0 0 x L x v y x θ 0 0 y L y v x y θ Φ ( t = I + T ( θt t (9 5

6 Where the transformation to world oordinates is: T ( θ t ( R θt 0 = 0 I4 (10 and the partials are: x 2 y = 2sin ( θ t 2, = sin ( θ t L L x y 2 2 = tsin ( θ t, = θ t sin ( θ t 2 2 v v x 2 = v t sin ( θ t + L t sin ( θ t θ y = θ ( sin( θ 2 os( θ 2 sin ( θ 2 2 os( θ 2 2 v t t t t L t t (11 The system noise matrix,, is derived in an analogous way to that for ISM. By integrating in a loal urvilinear oordinate system that follows the ar of the vehile three independent omponents: xv, yθθ and L are isolated. L is a salar that is ideally zero for a fixed rotation axis vehile, but nevertheless we set to a small onstant, ε L, to apture imperfetions in our steering model as well to model vehiles with varying rotation axis. xv is the integration of a white noise aeleration along the motion ar and thus is idential to x in (3, but in this ase ating on v and x. yθθ integrates a white noise angular aeleration whih gives a seond order noise term perpendiular to the ar, namely in diretion y. Performing this integration gives a term: v t 20 v t 8 v t 6 γ v t 6 t 2 t v t 8 t 3 t 2 yθθ =. ( Putting these terms together and shifting rows to align with the state vetor we get: 6

7 3 2 α t α t v t v t v t 0 0 ε γ γ 8 γ 6 L ( θ ( θ T = T t T 2 t (13 α t α t γ v t γ t γ t γ v t γ t γ t where we have rotated into world oordinates. We note that this system noise matrix is alulated for the enter rear axle loation, but we expet this to be a very good approximation to that at the vehile enter. 2.3 Vehile Shape Model Our vehile model is a 2D retangle entered at the vehile loation and oriented with the main edge of the vehile. After robust fitting at eah time step (Setion 2.4 we obtain measurements of the width and/or length. Eah length measurement is added to a normalized histogram of lengths (where the normalization determines the half-life. To eliminate large outliers, the largest 5% of measurements are exluded. But the main outliers to exlude are from measurements taken during partial olusions whih an generate large peaks in the histogram. These peaks will always be at shorter lengths than the true length, and so they an be exluded by hoosing the peak with greatest length. The same proedure is used to estimate width. In this way we maintain a robust and adaptive shape estimate. 2.4 Robust Data Fitting At any time at most two sides of a vehile are visible. This means there are two fitting senarios: either a single edge or a perpendiular pair of edges as illustrated in Figure 2. In eah ase an estimate on the vehile position and orientation is obtained giving a measurement vetor and matrix: 7

8 x z = y, and θ H = ( (a (b Figure 2 Robust data fitting of an edge (thik dark line (a, or a L-shape (b. The inlier LADAR points (shown in green are used to improve the fit, whih in eah ase defines a orner position and orientation. Using this orner, the vehile enter is estimated along with a ovariane. We use a RANSAC-based tehnique to robustly fit edges and orners to LADAR points on the vehile. Two points are needed for a line hypothesis, and three points for a perpendiular orner. The orner fitting an be made more effiient and robust by taking into aount visibility onstraints illustrated in Figure 3: The orner must be visible from the exterior, and Neither edge an olude the other. These are enoded in the following visibility ondition. Let θ be the viewing diretion, and θ a and θ b be the orientations of the two perpendiular edges from the orner. Both edges are visible if and only if: θ θa < 90 s and θ θ 90 <, (15 b s where s is the minimum slant angle at whih an edge is visible, in our ase 10 degrees. 8

9 θ θ a θ b (a (b ( (d (e Figure 3 Three non-olinear points define three distint perpendiular orners, see (b, ( and (d. In this ase physial onsiderations exlude all three, given the viewing diretion shown in (a. (e shows a valid orner passing the onditions in Eq. (15. One inliers to a orner are found, the orner parameters are estimated by minimizing the squared perpendiular distane of the points to the edges: ( ( φ ( φ ( ( φ ( J x, y, = os xi x + sin yi y 2 + i inliers ( edge1 ( ( φ ( y j y ( φ ( x j x os sin 2 ( 2 j inliers edge 2 2 (16 Here ( x, y, φ enode the orner loation and orientation. The Hessian an be approximated as the 3x3 matrix: 2 T J J J, and we found that a single Gauss- Newton step was suffiient to optimize the result from RANSAC. The final step in fitting is to onvert the orner fit into a measurement vetor, z, and ovariane, R. Using the urrent width and length estimates of the vehile and the fitted orientation, ϕ, let (, x y be the offset from the orner to the vehile enter, and d d τ be the rotation in multiples of 90 o that aligns the orner edge with the appropriate vehile edge. Then using the Gauss-Newton optimized orner parameters, we obtain: x + x d z = y + yd. (17 ϕ + τ Now a ovariane estimate for the orner loation an be obtained as the inverse of the Hessian. However, we need the ovariane of the vehile enter whih may have 9

10 signifiant orrelation effets due to being offset from the orner. But this is easy to find; we simply evaluate the Hessian using the vehile enter oordinates rather than the orner oordinates: ( J ( J ( 1 R = σ z z, (18 2 T 2 where σ is the variane of the LADAR returns. 2.5 Multi-Hypothesis Traking All of the omponents for building a Kalman Filter-based traker have been desribed. These omponents are brought together with the following standard equations: x = Φ x ( ( + t t t 1 P = Φ P Φ + ( ( + T t t t 1 t t 1 T ( ( K = P H H P H + R ( T ( t t t t t t t x = x + K z H x ( + ( ( t t t t t t P = P K H P ( + ( ( t t t t t 1 (19 A key step that remains is hoie of model. The VASM better aptures onstraints on vehile motion. It is has less bias and more aurate trajetory predition, espeially in urves as illustrated in Figure 4. However, it has two main drawbaks. (1 Given the detetion of a orner of a stationary vehile there is a 90 degree ambiguity in orientation. This means vehiles would need to be initialized with two states 90 degrees apart. (2 The flip side of having more onstrained motion is that if spurious data ause the model to beome misaligned with the true state, it is more diffiult for it to reover than ISM. In urban environments most objets deteted will be stationary lutter or stationary vehiles. To avoid needing two models for eah of these, we initialize all 10

11 objets with a single ISM. Objets are traked and any that are determined to be movers are subsequently initialized with an additional VASM. In this way multiple hypothesis traking is only applied to movers. Having both an ISM and VASM and swithing between them based on fitting sore gives robustness to eah of their weaknesses. If one trak fails to fit for a few frames it is reinitialized with the other. There are a few possible errors in fitting inluding when the edge or orner is fit to internal struture of the vehile or to lutter points grouped in with the vehile. Cases like these an be handled by adding onstraints to the model fitting. However, adding onstraints makes the model less flexible and potentially reates additional failure modes. To get the benefits without the harm of onstraints, an additional VASM trak hypothesis is added for eah onstraint. A trak manager traks eah hypothesis independently and at any given time selets the best model as the urrent target state. In our implementation we maintain up to four traks per mover and use the additional traks to vary the fration of points that must lie inside the vehile boundary, and also vary the sale fator on the system noise whih impliitly adjusts how muh we trust the model predition versus the measurements. 3. Results The traker has been tested on a large number of senarios inluding moving sensor platform, multiple target vehiles and high lutter examples. Figure 4 illustrates how the VASM model has redued bias and better trajetory estimation than ISM for turning vehiles. However, using ISM for traking stationary objets improves effiieny sine only one trak per stationary objet is needed, see Figure 5. Also ISM is less likely 11

12 to suffer trak loss with noisy measurements and so by ombining ISM and VASM we are able to robustly trak movers even for very noisy senarios, see Figure (a (b ( Figure 4 Comparison of vehile traking with ISM in (a, and VASM in (b and (, where the latter shows a 3D plot. The gray dots are LADAR hits, the green retangle and ellipse represent the measurement, z, along with ovariane in position. The blak dashed lines show whih edges are visible and fit. The orange retangle and ellipse show the updated state vetor plus position ovariane. In (b and ( the dashed red line is the estimated loation of the rear axle (L. The blue line leading up to the urrent position is the reent vehile trajetory and the blak line leading from this is the predited trajetory whih an be ompared to the true trajetory shown in red. 12

13 Figure 5 A senario in whih a target vehile is being followed by a sensor vehile (blue. The target vehile is maneuvering through lutter objets that are being traked with ISM. The target vehile itself is being traked with both ISM and VASM. Note in this ase, where only noisy rear-end measurements are obtained, the rotation axis is inorretly estimated near the front of the vehile, although when the target vehile turns sharply the axis shifts to the orret loation (a (b ( (d Figure 6 Senario in whih a target vehile is being followed and traked using the multi-hypothesis traker. (a A view of the sensor vehile (blue following the target vehile. (b A 4-hypothesis traker with 1 ISM and 3 VASMs. ( An ambiguous fitting example leads to one of the VASMs diverging. If it diverges suffiiently, as shown in (d, it fails to fit, is lost and needs to be reinitialized using one of the good hyptheses. 13

14 4. Conlusion We have developed a vehile traking and trajetory estimation approah that involves adaptively estimating vehile shape along with the kinemati parameters. We introdued VASM, a kinemati model that an apture Akerman steering when the rear axis is unknown as well as model skid-steering. We ompared it to ISM and identified the strengths of eah. Then we proposed a multi-hypothesis traker that ombines both of these to ahieve both high effiieny and improved robustness. In the future we plan a quantitative omparison of these models using ground truth. We also plan to extend them to model vehiles with trailers. Aknowledgements This work was sponsored by the Army Researh Lab under ontrat: DAAD Referenes [1] C. Keat, C. Pradalier, and C. Laugier, Vehile detetion and ar park mapping using laser sanner, Pro of Intelligent Robots and Systems (IROS, 2005, pp [2] B. Kluge, C. Kohler and E. Prassler, Fast and Robust Traking of Multiple Objets with a Laser Range Finder, IEEE Int. Conf. on Robotis and Automation, pp , [3] A. Laaze, K. Murphy, and M. Delgiorno, "Autonomous mobility for the Demo III experimental unmanned vehiles," in Proeedings of the AUVSI Conferene, [4] J-F Lalonde, N. Vandapel, D. Huber, and M. Hebert. Natural Terrain Classifiation using Three-Dimensional Ladar Data for Ground Robot Mobility, Journal of Field Robotis, Volume 23, Issue 10, [5] D. Langer, J. Rosenblatt, and M. Hebert, "A Behavior-Based System for Off-Road Navigation," IEEE Trans. Robotis and Automation, Vol. 10, No. 6, Deember, 1994, pp [6] R. Madhavan, T. Hong. Robust Detetion and Reognition of Buildings in Urban Environments from LADAR Data, Pro. Applied Imagery Pattern Reognition Workshop,

15 [7] L. Matthies, C. Bergh, A. Castano, J. Maedo, R. Manduhi, Obstale Detetion In Foliage with LADAR and Radar in International Symposium on Roboti Researh, [8] C. Mertz, D. Duggins, J. Gowdy, J. Kozar, R. MaLahlan, A. Steinfeld, A. Suppe, C. Thorpe, and C. Wang. "Collision Warning and Sensor Data Proessing in Urban Areas," Pro. 5th Int onf. on ITS teleoms., pp , June, [9] D. Morris, B. Colonna, P. Haley. LADAR-based Mover Detetion from Moving Vehiles, in Pro. 25th Army Siene Conf., [10] L. Navarro-Serment, C. Mertz, M. Hebert. Preditive Mover Detetion and Traking in Cluttered Environments, in Pro. 25th Army Siene Conf., [11] S. Thornton, M. Hoffelder, D. Morris. Multi-sensor Detetion and Traking of Humans for Safe Operations with Unmanned Ground Vehiles Workshop on Human Detetion from Mobile Platforms, Pasadena, May [12] Z. Sun, G. Bebis, R. Miller, On-Road Vehile Detetion : A Review, in IEEE Trans. Pattern Analysis and Mahine Intelligene, Vol. 28, No. 5, May 2006, pp: [13] S. Thrun. Roboti mapping: A survey. In G. Lakemeyer and B. Nebel, editors, Exploring Artifiial Intelligene in the New Millenium. Morgan Kaufmann, [14] C. Wang, C. Thorpe, A. Suppe, LADAR-based Detetion and Traking of Moving Objets from Ground Vehile at High Speeds, Intelligent Vehiles Symposium, June 2003, pp: