INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 14, No. 3, pp. 359-365 MARCH 2013 / 359

© KSPE and Springer 2013

Model Based Real-Time Collision-Free Motion Planningfor Nonholonomic Mobile Robots in Unknown DynamicEnvironments

Hongliang Yuan1 and Taehyun Shim2,#

1 School of Electronics & Information Engineering, Tongji University, Shanghai, China, 2018042 Department of Mechanical Engineering, University of Michigan-Dearborn, Dearborn, US, 48128

# Corresponding Author / E-mail: tshim@umich.edu, TEL: +1-313-593-5127, FAX: +1-313-593-3851

KEYWORDS: Trajectory planning, Obstacle avoidance, Optimal planning, Differential flatness

In this paper, by explicitly considering a dynamic model of the robots, the coefficients of trajectories are determined by boundary

conditions, optimal performance index and collision avoidance conditions. The planned trajectory is feasible and has a closed loop

expression, which is efficient for real-time updating. There are two main improvements compared with existing parametric

approaches. Firstly, most of existing methods use the kinematic models of the robots, which could cause curvature discontinuities when

trajectories are updated in real-time. Secondly, in some existing parametric methods, the initial position and ending waypoints cannot

be aligned vertically due to singularities. The approach proposed in this paper overcomes this limitation. Computer simulations

verified the effectiveness of the proposed method.

Manuscript received: January 17, 2012 / Accepted: April 20, 2012

1. Introduction

The motion planning problem has been widely studied in the past

decades. Dubins’ pioneer work1 proposed the first set of paths going

from a point to another with minimal time. Several extensions can be

found in references,2,3 where, in Reeds and Shepp,2 the approach is

extended to forward/backward driving vehicles and in Sussmann3 the

angular acceleration is used as input instead of angular velocity.

However, these approaches showed limitations which are unable to

handle collisions and curvature discontinuities. Typical heuristic

searching algorithms are the Dijkstra algorithm,4 A* and D*5,6 In these

methods, maps are gridized and searched for consecutive grids that

connect the starting and ending points. Vehicle models are seldom

considered in these methods and computational time could be

problematic for real-time applications. Recently, some variants of A*

and D* have been developed by considering a vehicle model with

lattice planner.17 For genetic and neural network approaches, their

performances largely depends on experimental settings. Because of

that, these methods often lead to trial and error situations not suitable

for real-time applications. Randomized motion planner includes

randomized potential field,7 probabilistic roadmap,8 rapidly exploring

random tree,9 etc. A common issue of these approaches is that it is

unclear how to optimize a path. Their performances also heavily

depend on empirical choices of certain parameters.

The potential field approach is also well known.10,11 The basic idea

is to assign repulsive potential fields to obstacles and attractive

potential fields to the goal. Path planning is performed by traversing

down the surface of overall potential functions. The problem with this

method is that a vehicle can be easily trapped in local minima, which

in general is inevitable in complex environments. Although some

methods can help the vehicle to escape, it imposes extra difficulties for

designing controllers.

Parametric trajectory methods can be found in references.12-14 All of

these approaches used kinematic models in generating trajectories. In

Yuan and Qu12 there exist curvature discontinuities due to non-smooth

inputs. These trajectories are polynomial type: y = y(x), with (x, y) to be

the robot's coordinate. This parameterization causes problems as the

tangent of such a polynomial cannot be vertical, which means if a pair

of waypoints are close to a vertical line, this algorithm may not work

properly. In such a situation, Yang, et. al.14 proposed to solve the

problem by rotating the coordinate system and insert intermediate

waypoints. In this paper, the work focuses directly on a dynamic model

DOI: 10.1007/s12541-013-0050-x

360 / MARCH 2013 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 14, No. 3

of car-like robots, resulting in a smoother trajectory with continuous

curvature. We first showed the differential flatness property of the

model that justifies the parametric approach. Then (x(t), y(t)) are both

explicitly parameterized to be a polynomial of time t which overcomes

the mentioned vertical tangent problem. Optimal performance index

and collision avoidance conditions are established to obtain optimal

collision-free trajectory in closed form, which is very efficient for real-

time updating.

2. Problem Formulation

A car-like robot is illustrated in Fig. 1. This robot has the following

dynamic model:15

(1)

where (x, y) is the Cartesian coordinate of the middle point of the rear

axle, ρ is the wheel radius, l is the wheel base, θ is the heading angle,

φ is the steering angle, ω1 is the wheel angular rate, ω2 is the steering

rate of front wheels and

with F as the driving force along the longitudinal direction, τ as the

steering torque, m as the robot’s mass, Jb as the body’s inertia around

the vertical axis and Js as the inertia of front-steering mechanism. It

shows that η and λ are transformed from the actual force and torque,

therefore can be viewed as generalized force and torque inputs.

A general setting of the trajectory planning problem is illustrated by

Fig. 2. Possible environmental changes are due to limited sensing

ranges and to the appearance of and/or motion of obstacles. In order to

model the vehicle and environment, the following assumptions can be

made without loss of generality.

1. The envelope of a robot is a circle centered at (x, y), its radius is r0,

and its sensing range is also a circle centered at (x, y) with radius Rs.

2. The obstacles are also represented by circles, centered at

with radius ri. The velocity of obstacles

are , where subscripts x and y denote the components

on x and y axes respectively.

The trajectory planning problem is to find a feasible and collision-

free trajectory that connects the robot’s initial configuration q0 at t0 and

final configuration qf at tf, where

3. Optimal Trajectory Planning Without Obstacles

In this section, the parametric trajectory is presented and boundary

conditions are derived. Optimal performance index is established and

analytical solutions of optimal trajectories are presented.

3.1 Parametric Trajectories

Differential Flatness was introduced by Fliess, et. al.16 If a system

is flat, then states and inputs can be expressed by a finite subset of

states called flat outputs and its derivatives. This is quite useful in

trajectory planning because it would be sufficient to parameterize the

flat outputs and then other states in the system can be derived. It is

straightforward to verify by this definition that the robot model is flat

and its flat outputs are found to be its Cartesian coordinates. As a

result of this flatness property of model (1), the following polynomial

trajectory is proposed:

The 10th order polynomials are chosen by considering the number

of boundary constraints imposed by boundary conditions q0 and qf. In

what follows, we show that each of x(t) and y(t) has 10 constraints:

x· ρω1cosθ=

y· ρω1sinθ=

θ·

ρω1tanφ/l=

φ·

ω2=

ω1

·η=

ω2

·λ=

⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧

ηJbρω1ω2tanφsec

2φ l

2F+⁄

m Jbtan2φ l

2⁄+( )ρ------------------------------------------------------------= λ

τ

Js

----=,

Oi xoi yoi,( )= i 1 2 … n , , , ,=,

vi vi x, vi y,,( )=

q0 x0 y0 θ0 φ0 ω10 ω20 η0 λ0, , , , , , ,( ) ,=

qf xf yf θf φf ω1f ω2f ηf λ f, , , , , , ,( ).=

xd t( ) a0 a1t a2t2

a3t3

a4t4

a5t5

a6t6

+ + + + + +=

+a7t7

a8t8

a9t9

a10t10

,+ + +

yd t( ) b0 b1t b2t2

b3t3

b4t4

b5t5

b6t6

+ + + + + +=

+b7t7

b8t8

b9t9

b10t10

. + + +

Fig. 1 A car-like robot Fig. 2 General setting of trajectory planning

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 14, No. 3 MARCH 2013 / 361

Remark: The main consideration in the choice of 10th order

polynomials is to have one more degree of freedom in the selection of

optimal coefficients. It is possible to use, higher order polynomials, but

it is more difficult to find optimal solutions and requires more

computational work.

In the process, we stopped differentiating at as both inputs have

appeared. In addition, since the input η appears earlier than λ, we

introduced two auxiliary states . Their boundary conditions

should also be supplied.

By replacing t0 with tf and q0 with qf, the constraints from the final

conditions can be established as:

For the y-coordinate, the same process can be applied:

And

Since both xd and yd have 10 constraints, the polynomials need at

least 10 free coefficients to accommodate these constraints. In order to

have the flexibility of choosing a collision-free trajectory, at least one

more coefficient is needed. Therefore, the polynomial trajectory is

proposed to be 10th order with 11 coefficients.

3.2 Optimal Trajectory Planning

In this section, an optimal approach to determine the coefficients of

xd(t) and yd(t) is proposed. Consider boundary constraints, the

following equations must hold for xd(t):

These equations can be put in a matrix form:

where

Similarly, for the y-system, we have:

where

For a practical mission, it is always true that tf > t0. Hence the matrix L

is nonsingular. Thus

. (2)

The planned trajectory (xd, yd) can be rewritten as:

xd t0

( ) x0

h1

= = ,

x·d t0

( ) ρcosθ0ω

10⋅ h

2= = ,

x··d t0

( ) ρ–2sinθ

0tanφ

0ω

10

2⋅ /l ρcosθ

0η0

⋅+ h3

= = ,

xd3( )

t0

( ) ρ–3cosθ

0tan

2φ0ω

10

3( )/l

2ρ2 sinθ

0

lcos2φ0

-----------------ω10

2ω

20–=

3ρ–2sinθ

0tanφ

0ω

10η0/l ρcosθ

0ξ0

⋅+ h4

= = ,

xd4( )

t0

( )ρ4

l3-----sinθ

0tan

3φ0ω

10

4 2ρ3

l2

--------cosθ0tanφ

0

ω20ω

10

3

cos2φ0

----------------–=

3ρ4

l2

--------cosθ0tan

2φ0ω

10

2η0

ρ2

l-----(

ρ

l---cosθ

0tanφ

0––

ω10

3ω

20

cos2φ0

---------------- 2sinθ

0sinφ

0

cos3φ0

-------------------------ω10

2ω

20

2 2sinθ0

cos2φ0

---------------ω10ω

20η0

+ +

sinθ

0

cos2φ0

---------------ω10

2λ0)

3ρ2

l--------(

ρ

l---cosθ

0tan

2φ0ω

10

2η0

–+

sinθ

0

cos2φ0

---------------ω10ω

20η0) sinθ

0tanφ

0η0

2sinθ

0tanφ

0+ + +

ω10ξ0)

ρ2

l-----sinθ

0tanφ

0ω

10ξ0

ρcosθ0ζ0

h5.=+–

xd

4( )

η·

ξ= ξ·

ζ=,

ξ0 ξf ζ0 ζf, , ,( )

xd tf( ) h6= x·d tf( ) h7= x··d tf( ) h8= xd

3( )tf( ) h9= yd

4( )tf( ) h10=, , , , .

yd t0( ) h11= y·d t0( ) h12= y··d t0( ) h13= yd

3( )t0( ) h14= yd

4( )t0( ) h15=, , , , .

yd tf( ) h16= y·d tf( ) h17= y··d tf( ) h18= yd

3( )tf( ) h19= yd

4( )tf( ) h20=, , , , .

a0

a1t0

a2t0

2a3t0

3a4t0

4a5t0

5a6t0

6a7t0

7+ + + + + + +

a8t0

8a9t0

9h1

a10

t0

10–=+

a1

2a2t0

3a3t0

24a

4t0

35a

5t0

46a

6t0

57a

7t0

6+ + + + + +

8a8t0

79a

9t0

8h2

10a10

t0

9–=+

2a2

6a3t0

12a4t0

220a

5t0

330a

6t0

442a

7t0

5+ + + + +

56a8t0

672a

9t0

7h3

90a10

t0

8–=+

6a3

24a4t0

60a5t0

2120a

6t0

3210a

7t0

4336a

8t0

5+ + + + +

540a9t0

6h4

720a10

t0

7–=+

24a4120a

5t0

360a6t0

2840a

7t0

31680a

8t0

4+ + + +

3024a9t0

5h5

5040a10

t0

6–=+

a0

a1tf a

2tf2

a3tf3

a4tf4

a5tf5

a6tf6

a7tf7

+ + + + + + +

a8tf8

a9tf9

h6

a10

tf10

–=+

a12a

2tf 3a

3tf2

4a4tf3

5a5tf4

6a6tf5

7a7tf6

+ + + + + +

8a8tf7

9a9tf8

h7

10a10

t0f

9–=+

2a2

6a3tf 12a

4tf2

20a5tf3

30a6tf4

42a7tf5

+ + + + +

56a8tf6

72a9tf7

h8

90a10

tf8

–=+

6a324a

4tf 60a

5tf2

120a6tf3

210a7tf4

336a8tf5

+ + + + +

540a9tf6

h9

720a10

tf7

–=+

24a4

120a5tf 360a

6tf2

840a7tf3

1680a8tf4

+ + + +

3024a9tf5

h10

5040a10

tf6

–=+

La H1 Ma10 ,–=

a a0 a1 a2 a3 a4 a5 a6 a7 a8 a9[ ]T ,=

H1 h1 h2 h3 h4 h5 h6 h7 h8 h9 h10[ ]T ,=

M t0

10 10t0

9 90t0

8 720t0

7 5040t0

6 tf

10 10tf

9 90tf

8 720tf

7 5040tf

6[ ].=

Lb H2 Mb10 ,–=

b b0 b1 b2 b3 b4 b5 b6 b7 b8 b9[ ]T ,=

H2 h11 h12 h13 h14 h15 h16 h17 h18 h19 h20[ ]T. =

a L1–

H1 Ma10–( )=

b L1–

H2 Mb10–( )=⎩⎪⎨⎪⎧

362 / MARCH 2013 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 14, No. 3

, (3)

where

Equation (2) shows that the coefficients a0 to a9, b0 to b9 can be

expressed in terms of a10 and b10, respectively. Hence, the combination

of (a10, b10) determines the shape of the trajectory. Therefore, one

optimal criteria needs to be set up to determine a trajectory with good

performance. It is also desired that (a10, b10) has closed form solutions

for fast real-time updating. With these considerations, the following

performance index is proposed:

, (4)

where

,

with

.

Here, is a straight line connecting the starting and ending

positions. Therefore the performance index is a measure of difference

between the planned trajectory and a straight line.

Substituting (3) into (4), yields:

(5)

where

The minimum of (5) is obtained at:

. (6)

The optimal trajectory is:

, (7)

and the optimal performance index is:

.

A more useful interpretation can be obtained for the relationship

between the optimal and suboptimal solutions. Rewrite equation (5)

into the following form:

.

It shows that suboptimal solution (a10, b10) with the same performance

index J lies on concentric circles centered at . In addition a

smaller Euclidean distance between (a10, b10) and results in

a smaller J. This conclusion is useful because when obstacles are

considered, the optimal trajectory may not be feasible due to

collision, then a suboptimal solution has to be sought.

4. Real-Time Collision Avoidance

In order to realize the real-time updating mechanism, the trajectory

planning method is modified to piecewise constant polynomial

parameterization. Let T be the time for a car to travel from the starting

position q0 to the ending position qf, i.e. T = tf - t0. Let Ts be the

sampling period such that is an integer. Let

. The result of trajectory planning may be

updated at each sampling time. When k = 0, the initial condition is q0,

for , the initial conditions is:

The terminal condition is always qf. Using the boundary conditions at

kth sampling time, the trajectory planning algorithm can be used for

xd t( ) f t( )a a10t10

+ f t( )L 1–H1 Ma10–( ) a10t

10+= =

yd t( ) f t( )b b10t10

+ f t( )L 1–H2 Mb10–( ) b10t

10+= =

⎩⎪⎨⎪⎧

f t( ) 1 t t2 t

3 t

4 t

5 t

6 t

7 t

8 t

9[ ],=

L

1 t0 t0

2 t0

3 t0

4 t0

5 t0

6 t0

7 t0

8 t 0

9

0 1 2t0

2 3t0

3 4t0

3 5t0

4 6t0

5 7t0

6 8t0

7 9t0

8

0 0 2 6t0 12t0

2 20t0

3 30t0

4 42t0

5 56t0

6 72t0

7

0 0 0 6 24t0 60t0

2 120t0

3 210t0

4 336t0

5 504t0

6

0 0 0 0 24 120t0 360t0

2 840t0

3 1680t0

4 3024t0

5

1 tf tf

2 tf

3 tf

4 tf

5 tf

6 tf

7 tf

8 tf

9

0 1 2tf 3tf

2 4tf

3 5tf

4 6tf

5 7tf

6 8tf

7 9tf

8

0 0 2 6tf 12tf

2 20tf

3 30tf

4 42tf

5 56tf

6 72tf

7

0 0 0 6 24tf 60tf

2 120tf

3 210tf

4 336tf

5 504tf

5

0 0 0 0 24 120tf 360tf

2 840tf

2 1680tf

4 3024tf

5

=

J xd yd,( ) xd x'–( )2yd y'–( )2

+[ ] tdt0

tf

∫=

x' kx t t0–( ) x0+=

y' ky t t0–( ) y0+=⎩⎨⎧

kx

xf x0–

tf t0–-------------= ky

yf y0–

tf t0–-------------=,

x' y',( )

J { f t( )L 1–H1 Ma10–( ) a10t

10x'–+[ ]

2

t0

tf

∫=

+ f t( )L 1–H2 Mb10–( ) b10t

10y'–+[ ]

2}dt

f1a10

f2 x'–+( )2f1b

10f3 y'–+( )2

+[ ]dtt0

tf

∫=

p1a10

22p2a10 p1b10

22p3b10 p4+ + + +=

p1 a10

p2

p1

-----+⎝ ⎠⎛ ⎞

2

p1 b10

p3

p1

-----+⎝ ⎠⎛ ⎞

2

p4

p2

2

p1

-----p3

2

p1

-----,––+ +=

f1 t10

f t( )L 1–M,–=

f2 f t( )L 1–H1,=

f3 f t( )L 1–M2,=

p1 f1

2t,d

t0

tf

∫=

p2 f1 f2 x'–( ) t,dt0

tf

∫=

p3 f1 f3 y'–( ) t,dt0

tf

∫=

p4 f2 x'–( )2f3 y'–( )2

+[ ] t,dt0

tf

∫=

a10

*p2– p1⁄=

b10

*p3– p1⁄=⎩

⎨⎧

xd

*t( ) f t( )L 1–

H1 Ma10

*–( ) a10

*t10

+=

yd

*t( ) f t( )L 1–

H2 Mb10

*–( ) b10

*t10

+=⎩⎨⎧

J*

xd

*yd

*,( ) p4

p2

p1

-----–p3

p1

-----–=

a10 a10

*–( )

2b10 b10

*–( )

2+

J J*

–

p1

-----------=

a10

*b10

*,( )

a10

*b10

*,( )

a10

*b10

*,( )

k T Ts⁄=

tk t0 kTs k 0 1 … k 1–, , ,=,+=

0 k k< <

qk xk yk θk φk ω1k ω2k ηk λk, , , , , , ,( )=

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 14, No. 3 MARCH 2013 / 363

real-time replanning as k increases. In the latter part of this paper, all

the notations with superscript k or subscript k mean that they are

versions of the corresponding variables at the kth sampling time.

Consider the scenario in figure 2. Suppose at kth sampling time, the

obstacle’s center is at and the obstacle’s

velocities are Note that if static obstacles

are considered, the obstacle velocity could be set to 0.

Accordingly, a trajectory planned at the kth sampling time is:

It follows that:

, (8)

Where

and and are L, H1 and H2 with t0 replaced by tk, and q0

replaced by qk. Therefore, the planned trajectory at kth sampling time

is: for ,

. (9)

The collision avoidance criterion is: for and ,

(10)

Substitute (10) into (11), the collision avoidance condition becomes:

for

(11)

In the plane of , equation (11) represents an area outside of

a series of circles. To solve (11) analytically, one can confine

to be on a straight line, such as . In such a case, the following

condition on is obtained:

. (12)

If the right hand side of (12) is negative or zero, then can be any

real number. Otherwise, the range for is:

or

Therefore the solution set for is:

The optimal solution of is:

(13)

Equation (13) shows that if , then . If , then

or .

5. Simulations

In this section, simulation results for the proposed algorithm are

provided. The algorithm is implemented on a PC with an Intel i7

M620@2.8GHz CPU and 8G internal RAM. The operating system is

64-bit Windows 7 Professional and the software is 64-bit version of

Matlab R2008a. The algorithm is extremely time-efficient. It is almost

instantaneous (< 1 sec) to generate a solution at each sampling period

when a replanning is required. The simulation scenario is set as

follows:

It shows that the vehicle moves from (0, 0) to (100, 100), while heading

is aligned at π/3. The initial and ending steering angle is 0, and at

boundary points, it is supposed to have no driving force or steering

torque. The vehicle parameters are:

The radius of the vehicle is r0 = 2, the radius of two moving obstacles

are r1 = r2 = 8. Obstacle 1 is initially at (10, 70), with a constant speed

v1 = (5, -3). Obstacle 2 is initially at (50, 90), its speed is v1 = (4, -3).

The vehicle's sensing range is 30. The algorithm is updated every 1

second. However, if no new obstacle is detected at a sampling time, the

vehicle will keep its current trajectory parameters.

The simulation results are illustrated in Fig. 3 to Fig. 5. It can be

verified from these figures that the boundary conditions are all matched

with specified values in q0 and qf.

Fig. 3 illustrates the vehicle's trajectory and obstacles' trajectories.

The vehicle starts at position 1, which is. The initial planning gives the

Oi

kxoi

kyoi

k,( ) i 1 2 … n,, , ,=,=

vi

kvi x,

kyi y,

k,( ) i 1 2 … n., , ,=,=

xd

kt( ) a0

ka1

kt a2

kt2

a3

kt3

a4

kt4

a5

kt5

a6

kt6

+ + + + + +=

a7

kt7

a8

kt8

a9

kt9

a10

kt10

,+ + + +

yd

kt( ) b0

kb1

kt b2

kt2

b3

kt3

b4

kt4

b5

kt5

b6

kt6

+ + + + + +=

b7

kt7

b8

kt8

b9

kt9

b10

kt10

.+ + + +

ak

Lk

1–H1

kM

ka10

k–( )=

bk

Lk

1–H2

kM

kb10

k–( )=

⎩⎪⎨⎪⎧

ak

a0

k a1

k a2

k a3

k a4

k a5

k a6

k a7

k a8

k a9

k[ ]T,=

bk

b0

k b1

k b2

k b3

k b4

k b5

k b6

k b7

k b8

k b9

k[ ]T,=

Mk

tk

10 10tk

9 90tk

8 720tk

7 5040tk

6 tf

10 10tf

9 90tf

8 720tf

7 5040tf

6[ ].=

Lk H1

k, H2

k

tk t tf≤ ≤

xd

kt( ) f t( )a

ka10

kt10

+ f t( )Lk

1–H1

kM

ka10

k–( ) a10

kt10

+= =

yd

kt( ) f t( )b

kb10

kt10

+ f t( )Lk

1–H2

kM

kb10

k–( ) b10

kt10

+= =⎩⎪⎨⎪⎧

i 1 2 … n , , , ,= tk t tf≤ ≤

xd

kt( ) xoi

k– vi x,

kt tk–( )–[ ]

2yd

kt( ) yoi

k– vi y,

kt tk–( )–[ ]

2r0 ri+( )2≥+

i 1 2 … n , , , ,=

a10

k f2

kxoi

k– vi x,

kt tk–( )–

f1

k----------------------------------------+

2

b10

k f3

kyoi

k– vi y,

kt tk–( )–

f1

k----------------------------------------+

2

+

r0 ri+( )2

f1

k------------------- tk t tf≤ ≤,≥

a10

kb10

k,( )

a10

kb10

k,( )

b10

k0=

a10

k

a10

k f2

kxoi

k– vi x,

kt tk–( )–

f1

k----------------------------------------+

2r0 ri+( )2

f3

kyoi

k– vi y,

kt tk–( )–( )–

f1

k( )2

---------------------------------------------------------------------

2

≥

a10

k

a10

k

a10

kmax

r0 ri+( )2f3

kyoi

k– vi y,

kt tk–( )–( )

2–

f1

k

---------------------------------------------------------------------------f2

kxoi

k– vi x,

kt tk–( )–

f1

k----------------------------------------–

⎩ ⎭⎨ ⎬⎧ ⎫

≥

a10

k=

a10

kmin

r0 ri+( )2f3

kyoi

k– vi y,

kt tk–( )–( )

2–

f1

k

---------------------------------------------------------------------------f2

kxoi

k– vi x,

kt tk–( )–

f1

k----------------------------------------–

⎩ ⎭⎨ ⎬⎧ ⎫

≤

a10

k=

t∈[tk tf]

t∈[tk tf]

a10

k

Ω ∞– a10

k] [∪ a10

k ∞( )=

a10

k

a10

k Δa10

k:a10

k Ω∈ min a10

ka10

k*–,{ }.=

a10

k

a10

k* Ω∈ a10

k Δa10

k*= a10

k* Ω∉

a10

k Δa10

k= a10

k Δa10

k=

t0 0= q0 0 0 π 3 ⁄ 0 10 0 0 0[ ],=,

tf 15= qf 100 100 π 3 ⁄ 0 0 10 0 0[ ].=,

m 1500 kg( )= Jb 1000 kg m2⋅( )= Js 20 kg m

2⋅( ),=, ,

ρ 0.5 m( )= l 3 m( ).=,

364 / MARCH 2013 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 14, No. 3

following (a10, b10):

At position 2 (4th second), it detects the obstacle 1 and updated

(a10, b10), which is:

It is seen that the updates successfully avoided the obstacle 1. At

position 3 (10th second), while obstacle 1 is still within sensing range,

the vehicle detected obstacle 2, and did another update, which yields:

The obstacle avoidance maneuver averted collision with both

obstacles and reached its destination. In addition, part of the trajectory

between position 3 and position 4 is closed to vertical, which causes no

problem.

Fig. 4 illustrates the steering angle and heading angle, Fig. 5

illustrates the wheel angular velocity and steering rate. It is seen that

these signals are all continuous. In addition, the curvature for a

parametric trajectory is given by:

.

Using the vehicles dynamic equations, it is derived that:

,

which conforms with the Ackermann steering geometry. From Fig. 4,

the steering angle φ is continuous even though the trajectory is updated

in the middle, therefore, we know that the trajectory curvature is also

continuous.

6. Conclusions

In this paper, an improved real-time trajectory generation algorithm

based on parametric approach is proposed. While keeping the

advantages of parametric trajectory such as smooth feasible

trajectories, high computational efficiency, the proposed approach

achieves curvature continuity by considering a vehicle’s dynamic

model instead of a kinematic model. In addition, the approach can

handle the situation where two waypoints are aligned vertically, which

traditionally requires a rotation of the coordinate system.

ACKNOWLEDGEMENT

This work is supported by the Tongji University research grant

2011KJ019, and the Fundamental Research Funds for Central

Universities of Tongji University.

REFERENCES

1. Dubins, L. E., “On Curves of Minimal Length with a Constraint on

Average Curvature, and with Prescribed Initial and Terminal

Positions and Tangents,” American Journal of Mathematics, Vol. 79,

No. 3, pp. 497-517, 1957.

2. Reeds, J. A. and Shepp, L. A., “Optimal Paths for a Car that Goes

both Forward and Backwards,” Pacific Journal of Mathematics, Vol.

145, No. 2, pp. 367-393, 1990.

a10

10.000768004815410–= b10

10.003161173915454.=,

a10

20.0024760169= b10

20.0089997887.=,

a10

30.0000271322= b10

30.0000191191– .=,

κ t( )x·dy··d x··dy·d–

x·d

2y·d

2+( )

3 2⁄------------------------=

κ t( ) tanφ l⁄=

Fig. 3 Collision avoidance trajectory

Fig. 4 Vehicle heading angle and steering angle

Fig. 5 Wheel angular rate and steering rate

INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 14, No. 3 MARCH 2013 / 365

3. Sussmann, H. J., “The Markov-Dubins Problem with Angular

Acceleration Control,” IEEE Conference on Decision and Control,

pp. 2639-2643, 1997.

4. Dijkstra, E. W., “A Note on Two Problems in Connecting with

Graphs,” Numerische Mathmatik, Vol. 1, No. 5, pp. 269-271, 1959.

5. Stentz, A., “Optimal and Efficient Path Planning for Partially-

Known Environments,” IEEE Conference on Robotics and

Automation, pp. 3310-3317, 1994.

6. Stentz, A., “The Focused D* Algorithm for Real-time replanning,”

International Joint Conference on Artificial Intelligence, pp. 1652-

1659, 1995.

7. Barraquand, J. and Latombe, J., “Robot Motion Planning: A

Distributed Representation Approach,” International Journal of

Robot Research, Vol. 10, No. 6, pp. 628-649, 1991.

8. Laumond, J., Sekhavat, S., and Lamiraux, F., “Guidelines in

Nonholonomic Motion Planning for Mobile Robots, in Laumond, J.,

(ed.), Robot Motion Planning and Control,” Springer-Verlag, Berlin,

pp. 1-53, 1998.

9. Lavalle, S. and Kuffner, J., “Rapidly-Exploring Random Trees:

Progress and Prospects, in Donald, B. R., Lynch, K. M., and Rus,

D., (eds.), Algorithmic and Computational Robotics: New

Directions,” CRC Press, pp. 293-308, 2001.

10. Khatib, O., “Real-time Obstacle Avoidance for Manipulators and

Mobile Robots,” International Journal of Robotics Research, Vol. 5,

No. 1, pp. 90-98, 1986.

11. Chuang, J., “Potential-Based Modeling of Three Dimensional

Workspace for Obstacle Avoidance,” IEEE Transaction on Robotics

and Automation, Vol. 14, No. 5, pp. 778-785, 1998.

12. Yuan, H. and Qu, Z., “Optimal Real-time Collision-Free Motion

Planning for AUVs in a 3D Underwater Space,” IET Control

Theory and Applications, Vol. 3, No. 6, pp. 712-721, 2009.

13. Qu, Z., Wang, J., and Plaisted, C., “A New Analytical Solution to

Mobile Robot Trajectory Generation in the Presence of Moving

Obstacles,” IEEE Transactions on Robotics, Vol. 20, No. 6, pp. 978-

993, 2004.

14. Yang, J., Daoui, A., Qu, Z., Wang, J., and Hull, R. A., “An Optimal

and Real-Time Solution to Parameterized Mobile Robot Trajectories

in the Presence of Moving Obstacles,” IEEE Conference on

Robotics and Automation, pp. 4412-4417, 2005.

15. Qu, Z., “Cooperative Control of Dynamical Systems: Applications

to Autonomous Vehicles,” Spring-Verlag, 1st ed., 2009.

16. Fliess, M., Levine, J., Martin, P., and Rouchon, P., “Flatness and

Defect of Non-Linear Systems: Introductory Theory and Examples,”

International Journal of Control, Vol. 61, No. 6, pp. 1327-1361,

1994.

17. Knepper, R. and Kelly, A., “High Performance State Lattice

Planning Using Heuristic Look-Up Tables,” IEEE Conference on

Intelligent Robots and Systems (IROS), pp. 3375-3380, 2006.