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] → R3, Ψ(m) ∈ C2.

◮ 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

T(m) =

˙ Ψ(m) ˙ Ψ(m) ◮ Normal Vector

N(m) =

˙ T(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 {pi}n

i=0, pi ∈ R3.

◮ γi(λ) = λpi + (1 − λ)pi−1, λ ∈ [0, 1] ◮ T(λ) = pi −pi−1 pi −pi−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.

◮ γ(λ) = rR(θ0)

  cos(θ1λ) sin(θ1λ)   + pc, λ ∈ [0, 1]

◮ T(λ) =

− sin (λ + θ0) cos (λ + θ0) 0 T N(λ) = − cos (λ + θ0) − sin (λ + θ0) 0 T κ = 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) = B

  t

0 cos π 2 u2du

t

0 sin π 2 u2du

 

◮ T(t) =

• cos(πt2/2) sin(πt2/2) 0

T N(t) =

• sin(πt2/2) − cos(πt2/2) 0

T κ(t) = πt/B

−2B −B B 2B −2B −B 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 pi−1 and pi ∈ R3 with tangent vectors ti−1, ti ∈ R3. γi(λ) = QH(λ) Q =

• pi−1

ti−1 pi ti

• H(λ) = [h00(λ) h10(λ) h01(λ) h11(λ)]T

◮ It is globally C2 across

concatenation points.

◮ the parameters CHS are

compatible with the data structures of common GIS database software.

◮ the accuracy of the can be

manipulated by the addition or subtraction control points (i.e., vertices)

0.2 0.4 0.6 0.8 1 −0.2 0.2 0.4 0.6 0.8 1 λ h(λ) h00 h10 h01 h11

0.2 0.4 0.6 0.8 1 1.2 −0.2 −0.15 −0.1 −0.05 0.05 0.1 0.15 0.2 0.25 0.3 ti ti−1 pi pi−1

Lane Map Construction Data sources

◮ We assume the availability of a data set D = {Yi}, i = 1, · · · , N

• f 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 {pi, ti}n

i=0. ◮ Measurement model

Ψ(m) =

n

• i=1

Ψi(m) = Φ⊤(m)Θ Θ⊤ = [p0, t0, . . . , pn, tn]

◮ Data:

Yi = ΦT(mi)Θ + ni, where the i-th row of Yi = {(xi, yi, zi)}

◮ Parameter Estimate

ˆ ΘD =

• Φ⊤

DR−1ΦD

−1 Φ⊤

DR−1YD

PD =

• Φ⊤

DR−1ΦD

−1

Lane Map Construction Parameter Refinement

◮ At some later time an additional set of data

B = {(xi, yi, zi, mi)}¯

N i=N+1 for the same lane segment

becomes available. ΘDB = ΘD + K(YB − ˆ YB) P−1

DB

= P−1

D + Φ⊤ BR−1 B ΦB

where ˆ YB = ΦBΘD K = PDBΦ⊤

BR−1 B ◮ Note that ΘDB can be computed even if the original data in

D is not longer available.

Lane Map Construction Parameter Refinement (Example)

−100 100 200 300 400 500 600 700 450 500 550 600 650 700 750 800 850 900 dataset 1 dataset 1−2 dataset 1−3 dataset 1−4 dataset 1−5

Figure: Segment of a lane of Watkins Drive in Riverside, CA

Lane Map Construction Parameter Refinement (Example)

−100 100 200 300 400 500 600 700 450 500 550 600 650 700 750 800 850 900 dataset 1 dataset 1−2 dataset 1−3 dataset 1−4 dataset 1−5

Figure: Segment of a lane of Watkins Drive in Riverside, CA

Lane Map Construction Parameter Refinement (Example)

190 195 200 205 210 215 220 225 230 235 240 826 826.1 826.2 826.3 826.4 826.5 826.6 826.7 826.8 dataset 1 dataset 2 dataset 3 dataset 4 dataset 5

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).

Kinematic Input Sensors

Inertial Navigation by inexpensive MEMs-based IMU’s:

◮ requires persistent calibration by aiding ◮ maintains lane-level accuracy for tens of seconds

Encoder based navigation:

◮ counts related to wheel rotation are already available for

braking systems on many vehicles.

• Characteristics:

◮ Pros: High rate, high bandwidth, full state, immune to

interference, slow predictable error growth

◮ Cons: Unbound error growth over time

Aiding Sensors

Basic Idea: Directly measure some function of the vehicle state: yk = h(x(tk)) + ηk

◮ Accuracy: At any measurement instant, the measurement

error is stationary, bounded and well-understood in a stochastic sense

◮ Observability: Over a time interval the vehicle state vector

must be observable from the suite of aiding sensors. Other characteristics (e.g., integrity and availability) are important, but not the focus of our current research. Position aiding sensors principles: TOA, TDOA, AOA, RTOA, etc. δyk = yk − ˆ yk = yk − h(ˆ x(tk)) = Hδxk + ηk

Time-of-Arrival (TOA) Aiding Sensors

Measurement Method:

◮ At tr 1 receiver detects

signal sent by transmitter at ts

◮ The pseudorange

ρ = c(tr

1 − ts 0)

measurement is modeled as ρ = R − T + cδtr

1 + η

• Assumption:

Synchronized Transmitter clocks Prediction : The pseudorange prediction is computed as ˆ ρ = ˆ R − T Error Model: The residual error model is δρ = R − T R − Tδx + cδtr

1 + η

H = R − T R − T

Angle-of-Arrival (AOA) Aiding Sensors

Measurement Method: Various methods.

• Assumption:

◮ Known feature pos.: F ◮ Calibrated sensor:

intrinsic and extrinsic Prediction :

◮ ˆ

TSF = F − ˆ S

◮ ˆ

β = atan

• e2·ˆ

TSF e1·ˆ TSF

• Error Model:

The residual error model is δβ = ¯ N⊤

• δS

TSF + ¯ TSF×

• δψ
• +η

Angle-of-Arrival (AOA) Aiding Sensors

Measurement Method: Various methods.

• Assumption:

◮ Known feature pos.: F ◮ Calibrated sensor:

intrinsic and extrinsic Prediction :

◮ ˆ

TSF = F − ˆ S

◮ ˆ

β = atan

• e2·ˆ

TSF e1·ˆ TSF

• Error Model:

The residual error model is δβ = ¯ N⊤

• δS

TSF + ¯ TSF×

• δψ
• +η

Potential Aiding Sensors

◮ Assume (modernized) DGPS availability in open sky scenarios. ◮ What other aiding signals are most beneficial when open sky is not available?

◮ Beneficial: Accuracy, reliability, useful geometry . ◮ Low on-vehicle cost.

Technology Principle Accuracy Road Cost

• Veh. Cost

Req’d Adv. GPS TOA 10.00 m $0 Low Modernization DGPS TOA 1.00 m ? Low Modernization CPDGPS TOA 0.01 m ? High Modernization Pseudolites TOA, TDOA 0.01 m High High ≥ 2nd gen. Cell Phone RSSI km’s $0 Low TOA, TDOA 100’s m $0 Low

• Phy. timing

DSRC TOA, TDOA 100’s m ? Low

• Phy. timing

DTV TOA 10.00 m Base Stns. ? ? FM Radio TOA ? Base Stns. Low ? AM Radio TOA ? Base Stns. Low Time stamp Vision - point AOA maps ?

• Feat. Recog.

Radar - point AOA, RTOA maps ?

• Feat. Recog.

Lidar - point AOA, RTOA maps high

• Feat. Recog.

Potentially Useful Aiding Sensors

◮ Pseudolites are high cost. ◮ Pseudolite-like: Cell Phones, DTV, FM & AM Radio, DSRC

• Technology

Principle Accuracy Road Cost

• Veh. Cost

Req’d Adv. GPS TOA 10.00 m $0 Low Modernization DGPS TOA 1.00 m ? Low Modernization CPDGPS TOA 0.01 m ? High Modernization Pseudolites TOA, TDOA 0.01 m High High ≥ 2nd gen. – Cell Phone RSSI km’s $0 Low TOA, TDOA 100’s m $0 Low

• Phy. timing

– DSRC TOA, TDOA 100’s m ? Low

• Phy. timing

– DTV TOA 10.00 m Base Stns. ? ? – FM Radio TOA ? Base Stns. Low ? – AM Radio TOA ? Base Stns. Low Time stamp Vision - point AOA maps ?

• Feat. Recog.

Radar - point AOA, RTOA maps ?

• Feat. Recog.

Lidar - point AOA, RTOA maps high

• Feat. Recog.

Potential Aiding Sensors

◮ DSRC

◮ Limited Range ◮ Unfavorable geometry ◮ Limited accuracy

• Technology

Principle Accuracy Road Cost

• Veh. Cost

Req’d Adv. GPS TOA 10.00 m $0 Low Modernization DGPS TOA 1.00 m ? Low Modernization CPDGPS TOA 0.01 m ? High Modernization Pseudolites TOA, TDOA 0.01 m High High ≥ 2nd gen. – Cell Phone RSSI km’s $0 Low TOA, TDOA 100’s m $0 Low

• Phy. timing

– DSRC TOA, TDOA 100’s m ? Low

• Phy. timing

– DTV TOA 10.00 m Base Stns. ? ? – FM Radio TOA ? Base Stns. Low ? – AM Radio TOA ? Base Stns. Low Time stamp Vision - point AOA maps ?

• Feat. Recog.

Radar - point AOA, RTOA maps ?

• Feat. Recog.

Lidar - point AOA, RTOA maps high

• Feat. Recog.

Potentially Useful Aiding Sensors

AOA Feature sensors ◮ Geometry complements GPS ◮ Requires reliably detectable features at accurately known locations

• Technology

Principle Accuracy Road Cost

• Veh. Cost

Req’d Adv. GPS TOA 10.00 m $0 Low Modernization DGPS TOA 1.00 m ? Low Modernization CPDGPS TOA 0.01 m ? High Modernization Pseudolites TOA, TDOA 0.01 m High High ≥ 2nd gen. – Cell Phone RSSI km’s $0 Low TOA, TDOA 100’s m $0 Low

• Phy. timing

– DSRC TOA, TDOA 100’s m ? Low

• Phy. timing

– DTV TOA 10.00 m Base Stns. ? ? – FM Radio TOA ? Base Stns. Low ? – AM Radio TOA ? Base Stns. Low Time stamp Vision - point AOA maps ?

• Feat. Recog.

Radar - point AOA, RTOA maps ?

• Feat. Recog.

Lidar - point AOA, RTOA maps high

• Feat. Recog.

Ongoing Lane-level Positioning Research

◮ Identification and demonstration of sensor technologies to

augment GPS for lane-level positioning in non-open sky environments.

◮ Observability analysis of alternative sensor configurations. ◮ Full state accuracy analysis for alternative sensor

configurations.

◮ DGPS carrier phase integer ambiguity in aided navigation

applications.

◮ Time and data efficient methods for precise lane mapping. ◮ Feature based aided navigation. ◮ Near realtime estimation.

Questions?

Thank you.

Research Supported by: AFOSR, Caltrans, DARPA, FHWA, NSF , ONR