bezier curves based trajectory planning
TRANSCRIPT
Bézier Curve based Trajectory Planning for an intelligent
Wheelchair to Pass a Doorway
Mohammad ISSA
Université de LIMOGES, XLIM
Laboratoire Mathématique
avec
Pr. Olivier RUATTA, Pr. Ouiddad Labbani-Igbida
11 June 2015
Mohammad ISSA 11 June 2015 1 / 16
Aim
To propose an e�cient strategy for an intelligent wheelchair to pass
through a narrow doorway.
The actual trajectory has to be smooth
without oscillating.
The curvature of the trajectory need to
be limited.
The heading of the wheelchair at Pd is
perpendicular to the door plane.
To acheive our requirements, we need the help of :
Bézier Curve
Constrained Optimisation Problem
Mohammad ISSA 11 June 2015 2 / 16
Aim
To propose an e�cient strategy for an intelligent wheelchair to pass
through a narrow doorway.
The actual trajectory has to be smooth
without oscillating.
The curvature of the trajectory need to
be limited.
The heading of the wheelchair at Pd is
perpendicular to the door plane.
To acheive our requirements, we need the help of :
Bézier Curve
Constrained Optimisation Problem
Mohammad ISSA 11 June 2015 2 / 16
Aim
To propose an e�cient strategy for an intelligent wheelchair to pass
through a narrow doorway.
The actual trajectory has to be smooth
without oscillating.
The curvature of the trajectory need to
be limited.
The heading of the wheelchair at Pd is
perpendicular to the door plane.
To acheive our requirements, we need the help of :
Bézier Curve
Constrained Optimisation Problem
Mohammad ISSA 11 June 2015 2 / 16
Aim
To propose an e�cient strategy for an intelligent wheelchair to pass
through a narrow doorway.
The actual trajectory has to be smooth
without oscillating.
The curvature of the trajectory need to
be limited.
The heading of the wheelchair at Pd is
perpendicular to the door plane.
To acheive our requirements, we need the help of :
Bézier Curve
Constrained Optimisation Problem
Mohammad ISSA 11 June 2015 2 / 16
Aim
To propose an e�cient strategy for an intelligent wheelchair to pass
through a narrow doorway.
The actual trajectory has to be smooth
without oscillating.
The curvature of the trajectory need to
be limited.
The heading of the wheelchair at Pd is
perpendicular to the door plane.
To acheive our requirements, we need the help of :
Bézier Curve
Constrained Optimisation Problem
Mohammad ISSA 11 June 2015 2 / 16
Aim
To propose an e�cient strategy for an intelligent wheelchair to pass
through a narrow doorway.
The actual trajectory has to be smooth
without oscillating.
The curvature of the trajectory need to
be limited.
The heading of the wheelchair at Pd is
perpendicular to the door plane.
To acheive our requirements, we need the help of :
Bézier Curve
Constrained Optimisation Problem
Mohammad ISSA 11 June 2015 2 / 16
Aim
To propose an e�cient strategy for an intelligent wheelchair to pass
through a narrow doorway.
The actual trajectory has to be smooth
without oscillating.
The curvature of the trajectory need to
be limited.
The heading of the wheelchair at Pd is
perpendicular to the door plane.
To acheive our requirements, we need the help of :
Bézier Curve
Constrained Optimisation Problem
Mohammad ISSA 11 June 2015 2 / 16
Bézier Curves
Let P0, P1, · · · , Pn be the control points of the Bézier curve
B([P0, · · · ,Pn], t) of degree n.
Bernstein polynomial of degree n
Let n be any integer, We de�ne
bi ,n(t) =
�ni
�t i (1− t)n−i where
i = 0, · · · , n
De�nition (Bézier Curve)
B(t) =n∑
i=0
bi ,n(t)Pi
where t ∈ [0, 1].
Properties Of Bézier Curve
1− Endpoint interpolation property.
B([P0, · · · ,Pn], 0) = P0 and B([P0, · · · ,Pn], 1) = Pn
2− Endpoint Tangent Property.
B([P0,P1, · · · ,Pn], 0) = n−−−→P0P1
B([P0,P1, · · · ,Pn], 1) = n−−−−→Pn−1Pn.
Mohammad ISSA 11 June 2015 3 / 16
Bézier Curves
Let P0, P1, · · · , Pn be the control points of the Bézier curve
B([P0, · · · ,Pn], t) of degree n.
Bernstein polynomial of degree n
Let n be any integer, We de�ne
bi ,n(t) =
�ni
�t i (1− t)n−i where
i = 0, · · · , n
De�nition (Bézier Curve)
B(t) =n∑
i=0
bi ,n(t)Pi
where t ∈ [0, 1].
Properties Of Bézier Curve
1− Endpoint interpolation property.
B([P0, · · · ,Pn], 0) = P0 and B([P0, · · · ,Pn], 1) = Pn
2− Endpoint Tangent Property.
B([P0,P1, · · · ,Pn], 0) = n−−−→P0P1
B([P0,P1, · · · ,Pn], 1) = n−−−−→Pn−1Pn.
Mohammad ISSA 11 June 2015 3 / 16
Bézier Curves
Let P0, P1, · · · , Pn be the control points of the Bézier curve
B([P0, · · · ,Pn], t) of degree n.
Bernstein polynomial of degree n
Let n be any integer, We de�ne
bi ,n(t) =
�ni
�t i (1− t)n−i where
i = 0, · · · , n
De�nition (Bézier Curve)
B(t) =n∑
i=0
bi ,n(t)Pi
where t ∈ [0, 1].
Properties Of Bézier Curve
1− Endpoint interpolation property.
B([P0, · · · ,Pn], 0) = P0 and B([P0, · · · ,Pn], 1) = Pn
2− Endpoint Tangent Property.
B([P0,P1, · · · ,Pn], 0) = n−−−→P0P1
B([P0,P1, · · · ,Pn], 1) = n−−−−→Pn−1Pn.
Mohammad ISSA 11 June 2015 3 / 16
Bézier Curves
Let P0, P1, · · · , Pn be the control points of the Bézier curve
B([P0, · · · ,Pn], t) of degree n.
Bernstein polynomial of degree n
Let n be any integer, We de�ne
bi ,n(t) =
�ni
�t i (1− t)n−i where
i = 0, · · · , n
De�nition (Bézier Curve)
B(t) =n∑
i=0
bi ,n(t)Pi
where t ∈ [0, 1].
Properties Of Bézier Curve
1− Endpoint interpolation property.
B([P0, · · · ,Pn], 0) = P0 and B([P0, · · · ,Pn], 1) = Pn
2− Endpoint Tangent Property.
B([P0,P1, · · · ,Pn], 0) = n−−−→P0P1
B([P0,P1, · · · ,Pn], 1) = n−−−−→Pn−1Pn.
Mohammad ISSA 11 June 2015 3 / 16
Evaluation and Interpolation of Bézier Curves
Let t0 = 0 < t1 < t2 < t3 = 1 be a subdivision of [0, 1]
Evaluation of Bézier Curve�b0,3(t0) · · · b3,3(t0)
.... . .
...
b0,3(t3) · · · b3,3(t3)
�︸ ︷︷ ︸
Bt,3
�P0
...
P3
�=
�B([P0, · · · ,P3], t0)
...
B([P0, · · · ,P3], t3)
�
Interpolation of Bézier Curve
Let C0, · · · ,C3 ∈ U ⊂ R2, then there
exists one and only one Bézier curve
B([P0, · · · ,P3], t) of degree 3 such that
B([P0, · · · ,P3], ti ) = Ci for all
i ∈ {0, · · · , 3}.
�P0
...
P3
�= B−1
t,3
�C0
...
C3
�
Mohammad ISSA 11 June 2015 4 / 16
Evaluation and Interpolation of Bézier Curves
Let t0 = 0 < t1 < t2 < t3 = 1 be a subdivision of [0, 1]
Evaluation of Bézier Curve�b0,3(t0) · · · b3,3(t0)
.... . .
...
b0,3(t3) · · · b3,3(t3)
�︸ ︷︷ ︸
Bt,3
�P0
...
P3
�=
�B([P0, · · · ,P3], t0)
...
B([P0, · · · ,P3], t3)
�
Interpolation of Bézier Curve
Let C0, · · · ,C3 ∈ U ⊂ R2, then there
exists one and only one Bézier curve
B([P0, · · · ,P3], t) of degree 3 such that
B([P0, · · · ,P3], ti ) = Ci for all
i ∈ {0, · · · , 3}.
�P0
...
P3
�= B−1
t,3
�C0
...
C3
�
Mohammad ISSA 11 June 2015 4 / 16
Evaluation and Interpolation of Bézier Curves
Let t0 = 0 < t1 < t2 < t3 = 1 be a subdivision of [0, 1]
Evaluation of Bézier Curve�b0,3(t0) · · · b3,3(t0)
.... . .
...
b0,3(t3) · · · b3,3(t3)
�︸ ︷︷ ︸
Bt,3
�P0
...
P3
�=
�B([P0, · · · ,P3], t0)
...
B([P0, · · · ,P3], t3)
�
Interpolation of Bézier Curve
Let C0, · · · ,C3 ∈ U ⊂ R2, then there
exists one and only one Bézier curve
B([P0, · · · ,P3], t) of degree 3 such that
B([P0, · · · ,P3], ti ) = Ci for all
i ∈ {0, · · · , 3}.
�P0
...
P3
�= B−1
t,3
�C0
...
C3
�
Mohammad ISSA 11 June 2015 4 / 16
Evaluation and Interpolation of Bézier Curves
Let t0 = 0 < t1 < t2 < t3 = 1 be a subdivision of [0, 1]
Evaluation of Bézier Curve�b0,3(t0) · · · b3,3(t0)
.... . .
...
b0,3(t3) · · · b3,3(t3)
�︸ ︷︷ ︸
Bt,3
�P0
...
P3
�=
�B([P0, · · · ,P3], t0)
...
B([P0, · · · ,P3], t3)
�
Interpolation of Bézier Curve
Let C0, · · · ,C3 ∈ U ⊂ R2, then there
exists one and only one Bézier curve
B([P0, · · · ,P3], t) of degree 3 such that
B([P0, · · · ,P3], ti ) = Ci for all
i ∈ {0, · · · , 3}.
�P0
...
P3
�= B−1
t,3
�C0
...
C3
�
Mohammad ISSA 11 June 2015 4 / 16
Trajectory Planning
We propose two methods for the wheelchair to pass the doorway
First Method
∗ 2 Patches Method : we divide the hall trajectoryü�PsPdPt into two
patches ùPsPd and ùPdPt . Then, we design the desired trajectory based
on Piecewise Bézier Curves.
Second Method
∗ 2× 2 Patches Method :we divide each patch into two connected
patches so we get 2× 2 patches.
Mohammad ISSA 11 June 2015 5 / 16
Trajectory Planning
We propose two methods for the wheelchair to pass the doorway
First Method
∗ 2 Patches Method : we divide the hall trajectoryü�PsPdPt into two
patches ùPsPd and ùPdPt . Then, we design the desired trajectory based
on Piecewise Bézier Curves.
Second Method
∗ 2× 2 Patches Method :we divide each patch into two connected
patches so we get 2× 2 patches.
Mohammad ISSA 11 June 2015 5 / 16
Trajectory Planning
We propose two methods for the wheelchair to pass the doorway
First Method
∗ 2 Patches Method : we divide the hall trajectoryü�PsPdPt into two
patches ùPsPd and ùPdPt . Then, we design the desired trajectory based
on Piecewise Bézier Curves.
Second Method
∗ 2× 2 Patches Method :we divide each patch into two connected
patches so we get 2× 2 patches.
Mohammad ISSA 11 June 2015 5 / 16
First Method : 2 Patches
We divide the hall trajectory into two patches ùPsPd and ùPdPt .
Denote the control points ùPsPd and ùPdPt are Pi and Qi , i = 1 · · · 3respectively.
The main idea is to determine the control points which totally de�ne the
shape of Bézier curve using an optimization problem. It is to �nd
P1,P2,Q1,Q2 such that the curvature and the rate of its change should be
small. So, we can formulate it as
Optimisation Problem
minP1,P2,Q1,Q2
∫ 1
0
�(k(t))2 + (k̇(t))2
�dt
Let ~s, ~d and ~t denote the headings at the positions Ps ,Pd and Pr
respectively.
Mohammad ISSA 11 June 2015 6 / 16
First Method : 2 Patches
We divide the hall trajectory into two patches ùPsPd and ùPdPt .
Denote the control points ùPsPd and ùPdPt are Pi and Qi , i = 1 · · · 3respectively.
The main idea is to determine the control points which totally de�ne the
shape of Bézier curve using an optimization problem. It is to �nd
P1,P2,Q1,Q2 such that the curvature and the rate of its change should be
small. So, we can formulate it as
Optimisation Problem
minP1,P2,Q1,Q2
∫ 1
0
�(k(t))2 + (k̇(t))2
�dt
Let ~s, ~d and ~t denote the headings at the positions Ps ,Pd and Pr
respectively.
Mohammad ISSA 11 June 2015 6 / 16
First Method : 2 Patches
We divide the hall trajectory into two patches ùPsPd and ùPdPt .
Denote the control points ùPsPd and ùPdPt are Pi and Qi , i = 1 · · · 3respectively.
The main idea is to determine the control points which totally de�ne the
shape of Bézier curve using an optimization problem. It is to �nd
P1,P2,Q1,Q2 such that the curvature and the rate of its change should be
small. So, we can formulate it as
Optimisation Problem
minP1,P2,Q1,Q2
∫ 1
0
�(k(t))2 + (k̇(t))2
�dt
Let ~s, ~d and ~t denote the headings at the positions Ps ,Pd and Pr
respectively.
Mohammad ISSA 11 June 2015 6 / 16
First Method : 2 Patches
We divide the hall trajectory into two patches ùPsPd and ùPdPt .
Denote the control points ùPsPd and ùPdPt are Pi and Qi , i = 1 · · · 3respectively.
The main idea is to determine the control points which totally de�ne the
shape of Bézier curve using an optimization problem. It is to �nd
P1,P2,Q1,Q2 such that the curvature and the rate of its change should be
small. So, we can formulate it as
Optimisation Problem
minP1,P2,Q1,Q2
∫ 1
0
�(k(t))2 + (k̇(t))2
�dt
Let ~s, ~d and ~t denote the headings at the positions Ps ,Pd and Pr
respectively.Mohammad ISSA 11 June 2015 6 / 16
Additional Constraints and Reference Trajectory
a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]
b) Corridor and Room structure :
P1,P2 ∈ C and Q1,Q2 ∈ R
c) C 1−continuity :
−−−→P2P3 =
−−−→Q0Q1.
d) Orientation : ~s = ~d = ~t = [0, 1].
arccos
� −−−→P2P3 ·
−→d
‖−−−→P2P3‖ · ‖
−→d ‖
�= 0
e) Avoiding Cycles :−−−→P0P3 ·
−−−→P0P2 ≥ 0
−−−→P0P3 ·
−−−→P0P2 −
−−−→P0P3 ·
−−−→P0P1 ≥ 0
−−−→P0P3 ·
−−−→P3Q1 ≥ 0and
−−−→P0P3 ·
−−−→P3Q2 ≥ 0
−−−→P0P3 ·
−−−→P3Q2 −
−−−→P0P3 ·
−−−→P3Q1 ≥ 0
Mohammad ISSA 11 June 2015 7 / 16
Additional Constraints and Reference Trajectory
a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]
b) Corridor and Room structure :
P1,P2 ∈ C and Q1,Q2 ∈ R
c) C 1−continuity :
−−−→P2P3 =
−−−→Q0Q1.
d) Orientation : ~s = ~d = ~t = [0, 1].
arccos
� −−−→P2P3 ·
−→d
‖−−−→P2P3‖ · ‖
−→d ‖
�= 0
e) Avoiding Cycles :−−−→P0P3 ·
−−−→P0P2 ≥ 0
−−−→P0P3 ·
−−−→P0P2 −
−−−→P0P3 ·
−−−→P0P1 ≥ 0
−−−→P0P3 ·
−−−→P3Q1 ≥ 0and
−−−→P0P3 ·
−−−→P3Q2 ≥ 0
−−−→P0P3 ·
−−−→P3Q2 −
−−−→P0P3 ·
−−−→P3Q1 ≥ 0
Mohammad ISSA 11 June 2015 7 / 16
Additional Constraints and Reference Trajectory
a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]
b) Corridor and Room structure :
P1,P2 ∈ C and Q1,Q2 ∈ R
c) C 1−continuity :
−−−→P2P3 =
−−−→Q0Q1.
d) Orientation : ~s = ~d = ~t = [0, 1].
arccos
� −−−→P2P3 ·
−→d
‖−−−→P2P3‖ · ‖
−→d ‖
�= 0
e) Avoiding Cycles :−−−→P0P3 ·
−−−→P0P2 ≥ 0
−−−→P0P3 ·
−−−→P0P2 −
−−−→P0P3 ·
−−−→P0P1 ≥ 0
−−−→P0P3 ·
−−−→P3Q1 ≥ 0and
−−−→P0P3 ·
−−−→P3Q2 ≥ 0
−−−→P0P3 ·
−−−→P3Q2 −
−−−→P0P3 ·
−−−→P3Q1 ≥ 0
Mohammad ISSA 11 June 2015 7 / 16
Additional Constraints and Reference Trajectory
a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]
b) Corridor and Room structure :
P1,P2 ∈ C and Q1,Q2 ∈ R
c) C 1−continuity :
−−−→P2P3 =
−−−→Q0Q1.
d) Orientation : ~s = ~d = ~t = [0, 1].
arccos
� −−−→P2P3 ·
−→d
‖−−−→P2P3‖ · ‖
−→d ‖
�= 0
e) Avoiding Cycles :−−−→P0P3 ·
−−−→P0P2 ≥ 0
−−−→P0P3 ·
−−−→P0P2 −
−−−→P0P3 ·
−−−→P0P1 ≥ 0
−−−→P0P3 ·
−−−→P3Q1 ≥ 0and
−−−→P0P3 ·
−−−→P3Q2 ≥ 0
−−−→P0P3 ·
−−−→P3Q2 −
−−−→P0P3 ·
−−−→P3Q1 ≥ 0
Mohammad ISSA 11 June 2015 7 / 16
Additional Constraints and Reference Trajectory
a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]
b) Corridor and Room structure :
P1,P2 ∈ C and Q1,Q2 ∈ R
c) C 1−continuity :
−−−→P2P3 =
−−−→Q0Q1.
d) Orientation : ~s = ~d = ~t = [0, 1].
arccos
� −−−→P2P3 ·
−→d
‖−−−→P2P3‖ · ‖
−→d ‖
�= 0
e) Avoiding Cycles :−−−→P0P3 ·
−−−→P0P2 ≥ 0
−−−→P0P3 ·
−−−→P0P2 −
−−−→P0P3 ·
−−−→P0P1 ≥ 0
−−−→P0P3 ·
−−−→P3Q1 ≥ 0and
−−−→P0P3 ·
−−−→P3Q2 ≥ 0
−−−→P0P3 ·
−−−→P3Q2 −
−−−→P0P3 ·
−−−→P3Q1 ≥ 0
Mohammad ISSA 11 June 2015 7 / 16
Additional Constraints and Reference Trajectory
a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]
b) Corridor and Room structure :
P1,P2 ∈ C and Q1,Q2 ∈ R
c) C 1−continuity :
−−−→P2P3 =
−−−→Q0Q1.
d) Orientation : ~s = ~d = ~t = [0, 1].
arccos
� −−−→P2P3 ·
−→d
‖−−−→P2P3‖ · ‖
−→d ‖
�= 0
e) Avoiding Cycles :−−−→P0P3 ·
−−−→P0P2 ≥ 0
−−−→P0P3 ·
−−−→P0P2 −
−−−→P0P3 ·
−−−→P0P1 ≥ 0
−−−→P0P3 ·
−−−→P3Q1 ≥ 0and
−−−→P0P3 ·
−−−→P3Q2 ≥ 0
−−−→P0P3 ·
−−−→P3Q2 −
−−−→P0P3 ·
−−−→P3Q1 ≥ 0
Mohammad ISSA 11 June 2015 7 / 16
Additional Constraints and Reference Trajectory
a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]
b) Corridor and Room structure :
P1,P2 ∈ C and Q1,Q2 ∈ R
c) C 1−continuity :
−−−→P2P3 =
−−−→Q0Q1.
d) Orientation : ~s = ~d = ~t = [0, 1].
arccos
� −−−→P2P3 ·
−→d
‖−−−→P2P3‖ · ‖
−→d ‖
�= 0
e) Avoiding Cycles :−−−→P0P3 ·
−−−→P0P2 ≥ 0
−−−→P0P3 ·
−−−→P0P2 −
−−−→P0P3 ·
−−−→P0P1 ≥ 0
−−−→P0P3 ·
−−−→P3Q1 ≥ 0and
−−−→P0P3 ·
−−−→P3Q2 ≥ 0
−−−→P0P3 ·
−−−→P3Q2 −
−−−→P0P3 ·
−−−→P3Q1 ≥ 0
Mohammad ISSA 11 June 2015 7 / 16
Trajectory Planning With Obstacles
The problem can be addressed by an additional constraint :
(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]
where (x(tk), y(tk)) is the discrete points of the Bézier curve, and dr is theminimum allowable distance between the center of obstacle and wheelchair.
Ps = [40; 60],Pd = [265; 160],andPt = [450; 450]
~s = [1,−0.5], ~d = [0, 1],and~t = [1, 1]
(y(tk)− 300)2 + (x(tk)− 350)2 > 402
(y(tk)− 90)2 + (x(tk)− 200)2 > 202
tk ∈ [0, 1] k ∈ [1, 100]
Mohammad ISSA 11 June 2015 8 / 16
Trajectory Planning With Obstacles
The problem can be addressed by an additional constraint :
(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]
where (x(tk), y(tk)) is the discrete points of the Bézier curve, and dr is theminimum allowable distance between the center of obstacle and wheelchair.
Ps = [40; 60],Pd = [265; 160],andPt = [450; 450]
~s = [1,−0.5], ~d = [0, 1],and~t = [1, 1]
(y(tk)− 300)2 + (x(tk)− 350)2 > 402
(y(tk)− 90)2 + (x(tk)− 200)2 > 202
tk ∈ [0, 1] k ∈ [1, 100]
Mohammad ISSA 11 June 2015 8 / 16
Trajectory Planning With Obstacles
The problem can be addressed by an additional constraint :
(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]
where (x(tk), y(tk)) is the discrete points of the Bézier curve, and dr is theminimum allowable distance between the center of obstacle and wheelchair.
Ps = [40; 60],Pd = [265; 160],andPt = [450; 450]
~s = [1,−0.5], ~d = [0, 1],and~t = [1, 1]
(y(tk)− 300)2 + (x(tk)− 350)2 > 402
(y(tk)− 90)2 + (x(tk)− 200)2 > 202
tk ∈ [0, 1] k ∈ [1, 100]
Mohammad ISSA 11 June 2015 8 / 16
Trajectory Planning With Obstacles
The problem can be addressed by an additional constraint :
(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]
where (x(tk), y(tk)) is the discrete points of the Bézier curve, and dr is theminimum allowable distance between the center of obstacle and wheelchair.
Ps = [40; 60],Pd = [265; 160],andPt = [450; 450]
~s = [1,−0.5], ~d = [0, 1],and~t = [1, 1]
(y(tk)− 300)2 + (x(tk)− 350)2 > 402
(y(tk)− 90)2 + (x(tk)− 200)2 > 202
tk ∈ [0, 1] k ∈ [1, 100]
Mohammad ISSA 11 June 2015 8 / 16
Trajectory Planning With Obstacles
The problem can be addressed by an additional constraint :
(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]
where (x(tk), y(tk)) is the discrete points of the Bézier curve, and dr is theminimum allowable distance between the center of obstacle and wheelchair.
Ps = [40; 60],Pd = [265; 160],andPt = [450; 450]
~s = [1,−0.5], ~d = [0, 1],and~t = [1, 1]
(y(tk)− 300)2 + (x(tk)− 350)2 > 402
(y(tk)− 90)2 + (x(tk)− 200)2 > 202
tk ∈ [0, 1] k ∈ [1, 100]
Mohammad ISSA 11 June 2015 8 / 16
Trajectory Planning With Obstacles
The problem can be addressed by an additional constraint :
(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]
where (x(tk), y(tk)) is the discrete points of the Bézier curve, and dr is theminimum allowable distance between the center of obstacle and wheelchair.
Ps = [40; 60],Pd = [265; 160],andPt = [450; 450]
~s = [1,−0.5], ~d = [0, 1],and~t = [1, 1]
(y(tk)− 300)2 + (x(tk)− 350)2 > 402
(y(tk)− 90)2 + (x(tk)− 200)2 > 202
tk ∈ [0, 1] k ∈ [1, 100]
Mohammad ISSA 11 June 2015 8 / 16
Splitting a patch into two patches
Given that Γ(t) =ùPsPd , we will compute 7 points, on this patch such thatM0 = Γ(0) = Ps
M1 = Γ(1/6)M2 = Γ(1/3)M3 = Γ(1/2)
and
N0 = Γ(1/2)N1 = Γ(2/3)N2 = Γ(5/6)N3 = Γ(1) = Pd
Interpolating each set of the
points, give us the control
points Ri and Qi where
i = 0, · · · , 3 that de�nes the
two patches ùR0R3 andúQ0Q3.
Mohammad ISSA 11 June 2015 9 / 16
Splitting a patch into two patches
Given that Γ(t) =ùPsPd , we will compute 7 points, on this patch such thatM0 = Γ(0) = Ps
M1 = Γ(1/6)M2 = Γ(1/3)M3 = Γ(1/2)
and
N0 = Γ(1/2)N1 = Γ(2/3)N2 = Γ(5/6)N3 = Γ(1) = Pd
Interpolating each set of the
points, give us the control
points Ri and Qi where
i = 0, · · · , 3 that de�nes the
two patches ùR0R3 andúQ0Q3.
Mohammad ISSA 11 June 2015 9 / 16
Splitting a patch into two patches
Given that Γ(t) =ùPsPd , we will compute 7 points, on this patch such thatM0 = Γ(0) = Ps
M1 = Γ(1/6)M2 = Γ(1/3)M3 = Γ(1/2)
and
N0 = Γ(1/2)N1 = Γ(2/3)N2 = Γ(5/6)N3 = Γ(1) = Pd
Interpolating each set of the
points, give us the control
points Ri and Qi where
i = 0, · · · , 3 that de�nes the
two patches ùR0R3 andúQ0Q3.
Mohammad ISSA 11 June 2015 9 / 16
Second Method : 2× 2 Patches
Denote the control points of_
PsPd and_
PdPt are Pi , Qi , and Ri ,
Mi , i = 1 · · · 3 respectively.
Optimisation Problem Using 2× 2 Patches
minP1,P2,P3,Q1,Q2,R1,R2,R3,M1M2
∫ 1
0
�(k(t))2 + (k̇(t))2
�dt
F (P1,P2,P3,Q1,Q2,R1,R2,R3,M1,M2)
= 1.4394× 10−04
Mohammad ISSA 11 June 2015 10 / 16
Second Method : 2× 2 Patches
Denote the control points of_
PsPd and_
PdPt are Pi , Qi , and Ri ,
Mi , i = 1 · · · 3 respectively.
Optimisation Problem Using 2× 2 Patches
minP1,P2,P3,Q1,Q2,R1,R2,R3,M1M2
∫ 1
0
�(k(t))2 + (k̇(t))2
�dt
F (P1,P2,P3,Q1,Q2,R1,R2,R3,M1,M2)
= 1.4394× 10−04
Mohammad ISSA 11 June 2015 10 / 16
Second Method : 2× 2 Patches
Denote the control points of_
PsPd and_
PdPt are Pi , Qi , and Ri ,
Mi , i = 1 · · · 3 respectively.
Optimisation Problem Using 2× 2 Patches
minP1,P2,P3,Q1,Q2,R1,R2,R3,M1M2
∫ 1
0
�(k(t))2 + (k̇(t))2
�dt
F (P1,P2,P3,Q1,Q2,R1,R2,R3,M1,M2)
= 1.4394× 10−04
Mohammad ISSA 11 June 2015 10 / 16
Experimental Results
Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]
~s = ~d = ~t = [0, 1].
Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]
~s = ~t = [0, 0.6], and~d = [0, 1]Two obstacles at (120, 120), (220,250) of dr1 = 15
Two obstacles at (80, 120), (240,320) of dr2 = 25
Mohammad ISSA 11 June 2015 11 / 16
Experimental Results
Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]
~s = ~d = ~t = [0, 1].
Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]
~s = ~t = [0, 0.6], and~d = [0, 1]Two obstacles at (120, 120), (220,250) of dr1 = 15
Two obstacles at (80, 120), (240,320) of dr2 = 25
Mohammad ISSA 11 June 2015 11 / 16
Experimental Results
Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]
~s = ~d = ~t = [0, 1].
Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]
~s = ~t = [0, 0.6], and~d = [0, 1]Two obstacles at (120, 120), (220,250) of dr1 = 15
Two obstacles at (80, 120), (240,320) of dr2 = 25
Mohammad ISSA 11 June 2015 11 / 16
Experimental Results
Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]
~s = ~d = ~t = [0, 1].
Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]
~s = ~t = [0, 0.6], and~d = [0, 1]Two obstacles at (120, 120), (220,250) of dr1 = 15
Two obstacles at (80, 120), (240,320) of dr2 = 25
Mohammad ISSA 11 June 2015 11 / 16
Experimental Results
Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]
~s = ~d = ~t = [0, 1].
Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]
~s = ~t = [0, 0.6], and~d = [0, 1]
Two obstacles at (120, 120), (220,250) of dr1 = 15
Two obstacles at (80, 120), (240,320) of dr2 = 25
Mohammad ISSA 11 June 2015 11 / 16
Experimental Results
Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]
~s = ~d = ~t = [0, 1].
Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]
~s = ~t = [0, 0.6], and~d = [0, 1]Two obstacles at (120, 120), (220,250) of dr1 = 15
Two obstacles at (80, 120), (240,320) of dr2 = 25
Mohammad ISSA 11 June 2015 11 / 16
Experimental Results
Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]
~s = ~d = ~t = [0, 1].
Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]
~s = ~t = [0, 0.6], and~d = [0, 1]Two obstacles at (120, 120), (220,250) of dr1 = 15
Two obstacles at (80, 120), (240,320) of dr2 = 25
Mohammad ISSA 11 June 2015 11 / 16
Experimental Results
Ps = [10, 125]Pd = [180, 200]Pt = [350, 350]
~s = [1, 0]~t = [1, 0.2]~d = [0, 1]
Two obstacles at (150, 150),(220, 220) of dr1 = 15
Two obstacles at (80, 120),(240, 320) of dr2 = 25
Mohammad ISSA 11 June 2015 12 / 16
Experimental Results
Ps = [10, 125]Pd = [180, 200]Pt = [350, 350]
~s = [1, 0]~t = [1, 0.2]~d = [0, 1]
Two obstacles at (150, 150),(220, 220) of dr1 = 15
Two obstacles at (80, 120),(240, 320) of dr2 = 25
Mohammad ISSA 11 June 2015 12 / 16
Experimental Results
Ps = [10, 125]Pd = [180, 200]Pt = [350, 350]
~s = [1, 0]~t = [1, 0.2]~d = [0, 1]
Two obstacles at (150, 150),(220, 220) of dr1 = 15
Two obstacles at (80, 120),(240, 320) of dr2 = 25
Mohammad ISSA 11 June 2015 12 / 16
Experimental Results
Ps = [10, 125]Pd = [180, 200]Pt = [350, 350]
~s = [1, 0]~t = [1, 0.2]~d = [0, 1]
Two obstacles at (150, 150),(220, 220) of dr1 = 15
Two obstacles at (80, 120),(240, 320) of dr2 = 25
Mohammad ISSA 11 June 2015 12 / 16
Comparison Between the Two Methods
2 patches 2× 2 patches
case 1 1.4777× 10−4 1.4777× 10−4
case 2 5.0703× 10−5 5.0703× 10−5
case 3 2.6100× 10−4 1.7992× 10−4
case 4 0.0012 3.6005× 10−4
case 5 2.3385× 10−4 2.0869× 10−4
case 6 Nofeasiblesolution 7.7193× 10−4
The Optimal Value
obtained using the
two methods.
The Total-Time consumed by
fmincon using each Method
2 patches
Total-Time
2× 2 patches
Total-Time
case 1 2.724 1.477
case 2 1.651 1.157
case 3 7.504 8.824
case 4 6.287 25.064
case 5 6.979 13.815
case 6 24.492 28.082
Mohammad ISSA 11 June 2015 13 / 16
Comparison Between the Two Methods
2 patches 2× 2 patches
case 1 1.4777× 10−4 1.4777× 10−4
case 2 5.0703× 10−5 5.0703× 10−5
case 3 2.6100× 10−4 1.7992× 10−4
case 4 0.0012 3.6005× 10−4
case 5 2.3385× 10−4 2.0869× 10−4
case 6 Nofeasiblesolution 7.7193× 10−4
The Optimal Value
obtained using the
two methods.
The Total-Time consumed by
fmincon using each Method
2 patches
Total-Time
2× 2 patches
Total-Time
case 1 2.724 1.477
case 2 1.651 1.157
case 3 7.504 8.824
case 4 6.287 25.064
case 5 6.979 13.815
case 6 24.492 28.082
Mohammad ISSA 11 June 2015 13 / 16
Conclusion and Perspectives
Two patches Four patches
Degree Of Freedom 5 13
Number of constraints without Obstacles
Number of constraints with one Obstacles
11
12
15
16
Table: Degree Of Freedom and Number Of Constraints
Comparing the above tables, we can clearly conclude that 2× 2 patches
method is much better than 2 patches method :
The Optimal Value obtained by 2× 2 patches method is less than that
obtained by 2 patches method
The Degree Of Freedon is closer to the number of constraints. ( The
curve is more free )
Mohammad ISSA 11 June 2015 14 / 16
Conclusion and Perspectives
Two patches Four patches
Degree Of Freedom 5 13
Number of constraints without Obstacles
Number of constraints with one Obstacles
11
12
15
16
Table: Degree Of Freedom and Number Of Constraints
Comparing the above tables, we can clearly conclude that 2× 2 patches
method is much better than 2 patches method :
The Optimal Value obtained by 2× 2 patches method is less than that
obtained by 2 patches method
The Degree Of Freedon is closer to the number of constraints. ( The
curve is more free )
Mohammad ISSA 11 June 2015 14 / 16
Conclusion and Perspectives
To improve the method of �nding the desired trajectory, we can
Implement a code which solves the problem dyamically ( If no feasible
solution found, split each patch into two patches and try to �nd
optimal trajectory using more patches)
Try to reformulate the Objective Function , and make some
simpli�cation in order to reduce the total-time consumed to �nd the
optimal trajectory.
Mohammad ISSA 11 June 2015 15 / 16
Conclusion and Perspectives
To improve the method of �nding the desired trajectory, we can
Implement a code which solves the problem dyamically ( If no feasible
solution found, split each patch into two patches and try to �nd
optimal trajectory using more patches)
Try to reformulate the Objective Function , and make some
simpli�cation in order to reduce the total-time consumed to �nd the
optimal trajectory.
Mohammad ISSA 11 June 2015 15 / 16
Thank You
Mohammad ISSA 11 June 2015 16 / 16