Upload
taehyun-shim
View
212
Download
0
Embed Size (px)
Citation preview
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: [email protected], 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
[email protected] 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.