High-precision lane-level road map building for vehicle navigation - PowerPoint PPT Presentation

High-precision lane-level road map building for vehicle navigation Anning Chen Jay A. Farrell Collaborators: Matt Barth, Lili Huang, Arvind Ramanandan, Behlul Suterwala, Anh Vu Department of Electrical Engineering University of California, Riverside ION Meeting May, 27 2010

Content Lab Research Overview (Farrell) Introduction: Lane Level Positioning (Chen) Lane Level Maps (Chen) Lane Level Aided Navigation (Farrell)

Introduction Motivation Various VAA (Vehicle Assist and Automation) applications require reliable and accurate lane-relative vehicle positioning ◮ lane-level driver guidance ◮ lane departure warning ◮ intelligent cruise control Lane relative accuracy better than 1.0 m (no current specification). Two approaches: ◮ Lane relative position sensing ◮ Absolute position sensing, Accurate lane mapping, Lane relative position is computed analytically. Commercially available road databases are mainly aimed for route planning. ◮ the topological structure of the road network is critical ◮ 2-10 meter accuracy is sufficient

Introduction Structure of the lane-level map ◮ Follow the structure of GIS ��� database for roadway maps ◮ Nodes indicate the locations of the interconnections or roadway segments ◮ Vertices are inserted between nodes to represent roadway shape ���

Introduction Road Map Requirements ◮ Each road is decomposed into a sequence of road segments. ◮ For each road segment, the number of lanes is constant. ◮ Adjacent lanes of a given road segment going in the same direction are implicitly connected everywhere. ◮ Lanes of distinct roads can cross without connecting. ◮ Each lane of a road segment will be defined by its own analytic curve Ψ ( m ) : [ a , b ] �→ R 3 , Ψ ( m ) ∈ C 2 . ◮ Decimeter level accuracy of each lane trajectory map Ψ ( m ) is desired (no current specification).

Analytic Lane Definition Useful quantities To compute lane-relative vehicle state information, we need analytic representations of the following quantities ◮ Tangent � ˙ Ψ ( m ) T ( m ) = � � ˙ Ψ ( m ) � � ◮ Normal Vector ˙ T ( m ) N ( m ) = � || ˙ T ( m ) || � ◮ Curvature � κ ( m ) = � ˙ T ( m ) � � ˙ Ψ ( m ) � � � � � � � � �

Analytic Lane Definition Splines 1. Straight line ◮ Current GIS software has the option of representing roadways as polylines – a sequence of connected straight line segments between the points { p i } n i = 0 , p i ∈ R 3 . ◮ γ i ( λ ) = λ p i + ( 1 − λ ) p i − 1 , λ ∈ [ 0 , 1 ] p i − p i − 1 ◮ T ( λ ) = � p i − p i − 1 � N ( λ ) is not well defined κ ( λ ) = 0 ◮ Pros: simple to understand and implement low storage memory requirements ◮ Cons: difficult to represent curvy roads concatenated lines are not globally differentiable

Analytic Lane Definition Splines 2. Circular Arcs ◮ If it can be assumed that lanes are the concatenation of segments with piecewise constant curvature, then each lane can be represented by smoothly connecting lines and arcs of circles.  cos ( θ 1 λ )   + p c , λ ∈ [ 0 , 1 ] ◮ γ ( λ ) = r R ( θ 0 ) sin ( θ 1 λ )  0 � − sin ( λ + θ 0 ) 0 � T ◮ T ( λ ) = cos ( λ + θ 0 ) N ( λ ) = � − cos ( λ + θ 0 ) 0 � T − sin ( λ + θ 0 ) κ = 1 r ◮ Pros: can get a globally differentiable path ◮ Cons: nonlinear representation in the parameter may want to use clothoid for smoother connection

Analytic Lane Definition Splines 3. Clothoid ◮ A clothoid is a spline with constant curvature change as a function of arc length. ◮ Clothoids are used in road design to transition between linear and/or circular segments. � t 2 u 2 du   0 cos π 2B � t ◮ γ ( t ) = B 2 u 2 du 0 sin π   B 0 0 � T ◮ T ( t ) = � cos ( π t 2 / 2 ) sin ( π t 2 / 2 ) 0 � T −B � sin ( π t 2 / 2 ) − cos ( π t 2 / 2 ) 0 N ( t ) = κ ( t ) = π t / B −2B −2B −B 0 B 2B ◮ Pros: it IS the spline used in roadway design ◮ Cons: it is a transcendental function

Analytic Lane Definition Splines 4. Cubic Hermite Spline (CHS) Each CHS segment is a third-order polynomial between two control points p i − 1 and p i ∈ R 3 with tangent vectors t i − 1 , t i ∈ R 3 . 1 γ i ( λ ) = QH ( λ ) h00 h10 h01 0.8 � � h11 Q = p i − 1 t i − 1 p i t i 0.6 H ( λ ) = [ h 00 ( λ ) h 10 ( λ ) h 01 ( λ ) h 11 ( λ )] T h( λ ) 0.4 ◮ It is globally C 2 across 0.2 concatenation points. 0 ◮ the parameters CHS are −0.2 0 0.2 0.4 0.6 0.8 1 λ compatible with the data 0.3 structures of common GIS 0.25 0.2 database software. t i−1 0.15 ◮ the accuracy of the can be 0.1 0.05 p i manipulated by the addition or 0 p i−1 −0.05 t i subtraction control points (i.e., −0.1 vertices) −0.15 −0.2 0 0.2 0.4 0.6 0.8 1 1.2

Lane Map Construction Data sources ◮ We assume the availability of a data set D = { Y i } , i = 1 , · · · , N of position data representing the lane segment centerline. ◮ For example, D could be acquired by driving a probe vehicle along the desired lane centerline while logging CPDGPS aided INS position data. ◮ D is subject to two kinds of errors ◮ Driving error: the difference between the vehicle position and the lane center ◮ Positioning error: the difference between the true and estimated vehicle position

Lane Map Construction Segmentation CHS Based Approach

Lane Map Construction Segmentation CHS Based Approach ◮ Road intersections provide an initial decomposition of a long road into segments.

Lane Map Construction Segmentation CHS Based Approach ◮ Road intersections provide an initial decomposition of a long road into segments. ◮ Between 2 nodes, assuming an total on ( n − 1 ) vertices, there will be a total of n basis functions to represent the lane. ◮ The parameter n will be different for each lane segment. It determines the memory requirements and is determined by the accuracy requirement and the shape of the lane segment.

Lane Map Construction Segmentation CHS Based Approach ◮ Road intersections provide an initial decompostition of a long road into segments. ◮ Between 2 nodes, assuming an total on ( n − 1 ) vertices, there will be a total of n basis functions to represent the lane. ◮ The parameter n will be different for each lane segment. It determines the memory requirements and is determined by the accuracy requirement and the shape of the lane segment. ◮ Arc length values where the nature of the curvature changes tend to be good ver- tices locations.

Lane Map Construction Parameter Estimation Estimation of the vertices positions and tangents { p i , t i } n i = 0 . ◮ Measurement model n � Ψ i ( m ) = Φ ⊤ ( m ) Θ Ψ ( m ) = i = 1 Θ ⊤ = [ p 0 , t 0 , . . . , p n , t n ] ◮ Data: Y i = Φ T ( m i ) Θ + n i , where the i -th row of Y i = { ( x i , y i , z i ) } ◮ Parameter Estimate � − 1 � ˆ D R − 1 Φ D D R − 1 Y D Φ ⊤ Φ ⊤ Θ D = � − 1 � D R − 1 Φ D Φ ⊤ P D =

Lane Map Construction Parameter Refinement ◮ At some later time an additional set of data B = { ( x i , y i , z i , m i ) } ¯ N i = N + 1 for the same lane segment becomes available. Θ D + K ( Y B − ˆ Θ DB = Y B ) P − 1 P − 1 D + Φ ⊤ B R − 1 = B Φ B DB where ˆ Y B = Φ B Θ D P DB Φ ⊤ B R − 1 K = B ◮ Note that Θ DB can be computed even if the original data in D is not longer available.

Lane Map Construction Parameter Refinement (Example) 900 dataset 1 dataset 1−2 850 dataset 1−3 dataset 1−4 dataset 1−5 800 750 700 650 600 550 500 450 −100 0 100 200 300 400 500 600 700 Figure: Segment of a lane of Watkins Drive in Riverside, CA

Lane Map Construction Parameter Refinement (Example) 900 dataset 1 dataset 1−2 850 dataset 1−3 dataset 1−4 dataset 1−5 800 750 700 650 600 550 500 450 −100 0 100 200 300 400 500 600 700 Figure: Segment of a lane of Watkins Drive in Riverside, CA

Lane Map Construction Parameter Refinement (Example) 826.8 dataset 1 dataset 2 826.7 dataset 3 dataset 4 dataset 5 826.6 826.5 826.4 826.3 826.2 826.1 826 190 195 200 205 210 215 220 225 230 235 240 Figure: Parameter Refinement from Multiple Data Sets

Lane Mapping Summary Discussed methods to build lane-level maps with high position accuracy in a GIS compatible database. ◮ Each lane is segmented for convenience of segment interconnections within GIS ◮ Each lane segment has an CHS analytic representation as a series of vertices defined by location and tangent vector. ◮ The lane fitting method is easily automated and applied to the numerous lanes segments. Existing GIS database software can build detailed roadway maps and road network models using such lane representations.

Aided Navigation Basic Ideas: ◮ Vehicle kinematics are known exactly. ◮ Integration of high-rate kinematic input sensors provide an estimated state trajectory: ˆ x ( t ) . ◮ The estimated trajectory allows prediction of lower rate aiding sensor measurements. ◮ Error between predicted and actual aiding sensor measurements allows calibration of the sensors and estimation of the error in the estimated trajectory: δ x ( t ) . Figure: Actual Traj. (solid black). Estimated Traj. (dotted blue).