Accurate curvature estimation algorithm for path planning of autonomous driving vehicle

Patent metadata

Field	Value
Publication number	US-9283967-B2
Application number	US-201414333144-A
Country	US
Kind code	B2
Filing date	Jul 16, 2014
Priority date	Jul 16, 2014
Publication date	Mar 15, 2016
Grant date	Mar 15, 2016

Abstract

A method for identifying roadway curvature that includes determining a range of interest and collecting shape points from a map database from a current position of the vehicle to an end of the range of interest that define the location of the roadway. The method converts the shape points from World Geodetic System 84 (WGS84) coordinates to UTM coordinates, and then fits a single set of polynomial equations to define a curve using the converted shape points. The method determines whether the single set of polynomial equations exceeds a predetermined curvature accuracy threshold, and if so, fits multiple sets of polynomial equations to multiple roadway segments over the range of interest using the converted shape points. The method then determines the roadway curvature at any roadway location using solutions to the multiple sets of polynomial equations.

First claim

What is claimed is:

1. A method for determining roadway curvature in a vehicle system on a vehicle, said method comprising:
determining a range of interest for determining the roadway curvature;
collecting shape points from a map database on the vehicle from a current position of the vehicle to an end of the range of interest that define the location of the roadway, where the shape points are defined by World Geodetic System 84 (WGS84) coordinates;
converting the shape points from the WGS84 coordinates to Universal Transverse Mercator (UTM) coordinates;
fitting a single set of polynomial equations to define a roadway curve using the converted shape points;
determining that the single set of polynomial equations exceeds a predetermined curvature accuracy threshold;
fitting multiple sets of polynomial equations to multiple roadway segments over the range of interest using the converted shape points; and
determining the roadway curvature at a roadway location using solutions to the multiple sets of polynomial equations.

2. The method according to claim 1 wherein fitting a single set of polynomial equations includes fitting two third-order polynomial equations to define the roadway curve.

3. The method according to claim 2 wherein fitting a single set of polynomial equations includes fitting two polynomial equations based on arc length, where the arc length is defined as a distance between any two shape points.

4. The method according to claim 3 wherein fitting a single set of polynomial equations includes defining each shape point as:
{circumflex over ( x )}( s )= p 13 ·s 3 +p 12 ·s 2 +p 11 ·s+p 10 ,
{circumflex over ( y )}( s )= q 13 ·s 3 +q 12 ·s 2 +q 11 ·s+q 10 ,

where s is the arc length and p and q are unknown coefficients.

5. The method according to claim 4 wherein fitting a single set of polynomial equations includes solving the equations by minimizing a cost function.

6. The method according to claim 5 wherein the cost function is defined as:

J
=

1
2

⁢

∑
i

N
⁢

⁢
1

⁢

[

(

x
i

-

x
1

-

x
^

i

)

2

+

(

y
i

-

y
1

-

y
^

i

)

2

]

.

7. The method according to claim 5 wherein determining that the single set of polynomial equations exceeds a predetermined accuracy threshold includes determining that the cost function minimization exceeds the accuracy threshold.

8. The method according to claim 1 wherein fitting multiple sets of polynomial equations to multiple roadway segments includes separately fitting a single set of polynomial equations to multiple contiguous segments where an overlap section is defined between two segments and where virtual shape points are defined from a previous segment in the overlap section.

9. The method according to claim 8 wherein each set of polynomial equations for each roadway segment is two third-order polynomial equations.

10. The method according to claim 1 wherein collecting shape points from a map database includes collecting shape points that define the roadway behind the vehicle.

11. The method according to claim 1 wherein determining the roadway curvature at any roadway location includes calculating the curvature κ as:

κ
i

=

(

x
^

i
′

⁢

y
^

i
″

-

y
^

i
′

⁢

x
^

i
″

)

/

(

x
^

i
′2

+

⁢

y
^

i
′2

Assignees

• Gm Global Tech Operations Inc

Inventors

• Lee Jin-Woo

Classifications

• G01C21/26

  specially adapted for navigation in a road network · CPC title

• B60W30/10

  Path keeping {(cruise control for automatically following a preceding vehicle B60W30/165)} · CPC title

• B60W2754/10

  Spatial relation or speed relative to objects · CPC title

• B60W2552/20

  Road profile, i.e. the change in elevation or curvature of a plurality of continuous road segments · CPC title

• B60W40/072 Primary

  Curvature of the road · CPC title