1. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Real-Time B´zier Trajectory Deformation for
e
Potential Fields Planning Methods
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´
e o
September 25-30, 2011 San Francisco
International Conference on Intelligent Robots and Systems
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 1/24
2. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
1 Introduction.
2 B´zier Trajectory Deformation (BTD) in Mobile Robots and
e
Obstacles.
3 Simulations Results.
4 Conclusions and Future Works.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 2/24
3. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
B´zier Trajectory Deformation
e
The objective of this work:
A New Technique for obtaining a Flexible Trajectory Free of
Collisions based on the Deformation of a B´zier curve through a
e
Field of Vectors.
BTD
This technique is called B´zier Trajectory Deformation (BTD) .
e
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 3/24
4. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Trajectory Generation Problem
The parametric curves (B´zier, B-Splines, NURBS, RBC) are the
e
most widely used in computer graphics and geometric modelling
since points on the curve are easily computed.
The representation of this kind of parametric curves is a SMOOTH
CURVE.
It is a useful property for the Trajectory Generation Problem in
Mobile Robots.
A lot of researchers consider parametric curves in the construction of
trajectories for wheeled robots, (see for example, Choi et. al,
2008-2009, Skrjanc and Klancar, 2007), etc.
Our algorithm
BTD is developed with B´zier curves. They are a polynomial curves and
e
they possess a number of mathematical properties which facilitate their
manipulation and analysis.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 4/24
5. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Collision Avoidance Problem
Collision avoidance is a fundamental problem in many areas such as
robotics.
An extreme situation of collision avoidance.......
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 5/24
6. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Collision Avoidance Problem
The generation of the path can be properly done using reactive path
planning methods adapting to environmental changes.
One of the most popular reactive methods is Artificial Potential
Fields(APF) (see Khatib, 1986), that is the basis of the Potential
Field Projection method (PFP) (see Mora and Tornero, 2007)
used in this work.
APF consists in filling the robot’s workspace
with an artificial potential field in which the
robot is attracted by the goal and repelled
by the obstacles.
APF produces a field of vectors that guides
the robot to non-collision positions.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 6/24
7. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Trajectory Generation+Collision Avoidance
Design and Modify a Parametric Curve is an important research
issue, (Wu et al.2005, Xu et al. 2002)
One of these techniques (Wu et al., 2005) has been adapted for its
use in path planning for Holonomic Robots.
This technique modifies the parametric curve through a field of
vectors.
The shape of the B´zier curve is modified.
e
The changes of the shape are minimized from the original one.
These vectors are computed with PFP. The Repulsive Forces will
modify the Original Trajectory to avoid every obstacle.
We called: B´zier Trajectory Deformation, BTD.
e
The First Technique joining:
Trajectory Generation using Parametric Curves
Avoiding the Obstacles using Potential Field methods
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 7/24
8. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Definitions
Definition
A B´zier Curve is defined as,
e
n
α(u) = Pi · Bi,n (u) (1)
i=0
n is the Order of the B´zier curve.
e
n
Bi,n (u) = i u i (1 − u)n−i Bernstein Basis
u ∈ [0, 1] is the Intrinsic Parameter.
(n + 1) Control Points, Pi such that i = 0, 1, · · · , n.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 8/24
9. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Definitions
The intrinsic parameter, u, is non-dimensional.
In order to use a B´zier curve as a trajectory, this parameter must be
e
redefined as a time variable, associating each curve position (robot
position) with a time instant t ∈ [t0 , tf ], where t0 and tf are the
initial and final trajectory instants.
The definition of the B´zier curve has to change:
e
Definition
n
α(t) = Pi · Bi,n (t); t ∈ [t0 , tf ] (2)
i=0
In this case, the definition of the Bernstein Basis is:
−t
Bi,n (t) = n ( tt−t00 )i ( ttff−t0 )n−i such that i = 0, 1, · · · , n
i f −t
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 9/24
10. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Definitions
Definition
A Modified B´zier curve is defined as,
e
n
Sε (α(t)) := (Pi + εi ) · Bi,n (t); t ∈ [t0 , tf ] (3)
i=0
To deform a given B´zier curve describing a Trajectory, the control
e
points must be changed and the perturbation, εi , of every control
point must be computed.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 10/24
11. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Constrained optimization problem.
This problem is solved defining a constrained optimization
problem.
It is solved with the Lagrange Multipliers Theorem.
The optimization function minimizes the distance between the
orginal B´zier curve, α(t), and the modified B´zier curve, Sε (α(t)).
e e
Thus, this function minimizes the changes of the shape.(Wu et
al.2005)
Definition
The optimization function is defined as,
tf
2
α(t) − Sε (α(t)) 2 dt (4)
t0
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 11/24
12. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Optimization Function
Disadvantage
A B´zier curve is numerically unstable if the B´zier curve has a large
e e
number of control points.
In Mobile Robots, it is necessary to concatenate some B´zier
e
curves to obtain the complete trajectory.
So the optimization function is redefined.
Definition
The optimization function using k-B´zier curves is defined as,
e
k (l)
tf
2
g := αl (t) − Sε (αl (t)) 2 dt (5)
(l)
l=1 t0
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 12/24
13. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
The set of Constraints
First Constraint
The robot is guided to non-collision positions. For that reason, The
Modified B´zier, Sε (αi (t)), passes through the Target Point, Ti . The
e
vectors joining the Start point and the Target Point are the field of
Forces computed through the PFP.
Mathematical Formulation
k rl
(l) (l)
r1 = λ, Tj − Sε (αl (tj )) (6)
l=1 j=1
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 13/24
14. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
The set of Constraints
Second Constraint
The Trajectory of the robot must be a smooth Trajectory. So,
Continuity and derivability is necessary to impose on the joined points
of the concatenated curves.
Mathematical Formulation
k−1
(l) (l+1)
r2 = λ, Sε (αl (tf )) − Sε (αl+1 (t0 )) (7)
l=1
k−1
(l) (l+1)
r3 = λ, Sε (αl (tf )) − Sε (αl+1 (t0 )) (8)
l=1
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 14/24
15. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
The set of Constraints
Third Constraint
The continuity between the Present position and the predicted
Future position is ensured. Therefore, derivative constraints on the
start and end points of the resulting concatenated curves are imposed.
Mathematical Formulation
(1) (1) (k) (k)
r4 = λ, α1 (t0 ) − Sε (α1 (t0 )) + λ, αk (tf ) − Sε (αk (tf )) (9)
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 15/24
16. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
The Lagrange Multipliers
Lagrange Multipliers
The Lagrange Multipliers Theorem has been applied to solve the
constrained optimization problem. The idea is to minimize the function
defined in 5 including the set of constraints defined below.
Lagrange Function
L(ε(1) , · · · , ε(k) , λ) = g + r1 + r2 + r3 + r4 (10)
The solution of the problem
In order to obtain the Minimum of this convex function, we only to
compute the stationary point of the Lagrangian derivative.
∂L
= 0; (l) = 1, · · · , k (11)
∂ε(l)
∂L
=0 (12)
∂λ
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 16/24
17. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
The solution
A square linear system of equations is obtained: A · X = b. It is
solvable and the solution X = (ε, λ) computes the perturbation of
every control point.
Example
Advantages
The Computational Cost is
reduced if the order of the
B´zier curve is maintained
e
invariable, because the matrix
can be computed in advanced.
It is possible to include more
8 B´zier curves are used.The
e constraints to improve the
modified trajectory is computed in algorithm.
0.23 ms in a Pentium IV 2.4 Ghz
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 17/24
18. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
PFP+BTD
BTD requires an algorithm that generates an Initial Trajectory and
a Field of Vectors to modify it in case obstacles are detected,
ensuring that the deformed trajectory is collision-free.
The BTD technique is evaluated through a predictive PF path
planning method, (see M.C.Mora et al., 2007-2008): the Potential
Field Projection (PFP) method.
Potential Field Projection
This method is based on the combination of:
The Classical PF (see O. Khatib, 1986)
The Multi-rate Kalman Filter estimation (see J.Tornero et al., 1999
and R. Piz´, 2003))
a
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 18/24
19. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Potential Field Projection
Potential Field Projection
This method takes into account:
Uncertainties on locations.
The future trajectory of the robot and the obstacles.
Multi-rate information supplied by sensors.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 19/24
20. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Potential Field Projection
Predicted future positions and uncertainties are obtained from the
prediction equations of the Multi-rate Kalman filter for every object
in the environment.
The predicted positions and their uncertainties are used in the
generation of a PF that guides the robot to the goal avoiding the
obstacles in the environment.
These PF generate forces in every prediction instant.
The set of repulsive forces are transformed into displacements.
These displacements affect the shape of the initial parametric
trajectory.
The displacements are used in BTD algorithm and the robot is guided to
the goal without colliding with the obstacles.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 20/24
21. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Simulations Results
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 21/24
22. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Conclusions
The Robot’s Trajectory is computed with a parametric curve.(in this
case B´zier curves).
e
The BTD algorithm has been devoloped to compute the
deformation of the Trajectory through a field of vectors.
This algorithm needs a set of vectors. In this case, the field of
repulsive forces necessary to modify the Trajectory (the B´zier
e
curve) are obtained by PFP.
The Modified Trajectory avoids the obstacles.
It is the FIRST technique joining PFP with Parametric Curves.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 22/24
23. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Future Works
The Improvement of the algorithm using Tensorial Notation.
This structure improves the Computational Cost.
To include more constraints in the algorithm, for example, the curvature.
To develop this algorithm in three dimensions. Design trajectories free
of collisions in 3D, for example in a UAV (Unmanned Aerial Vehicle) or in
a Robot Arm.
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 23/24
24. Introduction. B´zier Trajectory Deformation (BTD) in Mobile Robots and Obstacles. Simulations Results. Conclusions and Future Works.
e
Thank you for your attention!
Questions?
L. Hilario, N. Mont´s, M.C.Mora, A. Falc´ — BTD for Potential Fields Planning Methods
e o 24/24