Bezier Curves Based trajectory planning

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


Aim






be limited.




Bézier Curve



Aim






be limited.




Bézier Curve



Aim






be limited.




Bézier Curve



Aim






be limited.




Bézier Curve



Aim






be limited.




Bézier Curve



Aim






be limited.




Bézier Curve



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.


Bézier Curves





bi ,n(t) =

�ni


i = 0, · · · , n


B(t) =n∑

i=0

bi ,n(t)Pi

where t ∈ [0, 1].



B([P0, · · · ,Pn], 0) = P0 and B([P0, · · · ,Pn], 1) = Pn


B([P0,P1, · · · ,Pn], 0) = n−−−→P0P1

B([P0,P1, · · · ,Pn], 1) = n−−−−→Pn−1Pn.


Bézier Curves





bi ,n(t) =

�ni


i = 0, · · · , n


B(t) =n∑

i=0

bi ,n(t)Pi

where t ∈ [0, 1].



B([P0, · · · ,Pn], 0) = P0 and B([P0, · · · ,Pn], 1) = Pn


B([P0,P1, · · · ,Pn], 0) = n−−−→P0P1

B([P0,P1, · · · ,Pn], 1) = n−−−−→Pn−1Pn.


Bézier Curves





bi ,n(t) =

�ni


i = 0, · · · , n


B(t) =n∑

i=0

bi ,n(t)Pi

where t ∈ [0, 1].



B([P0, · · · ,Pn], 0) = P0 and B([P0, · · · ,Pn], 1) = Pn


B([P0,P1, · · · ,Pn], 0) = n−−−→P0P1

B([P0,P1, · · · ,Pn], 1) = n−−−−→Pn−1Pn.


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

�





.... . .

...

b0,3(t3) · · · b3,3(t3)

�︸︷︷︸

Bt,3

�P0

...

P3

�=

�B([P0, · · · ,P3], t0)

...

B([P0, · · · ,P3], t3)

�





B([P0, · · · ,P3], ti ) = Ci for all

i ∈ {0, · · · , 3}.

�P0

...

P3

�= B−1

t,3

�C0

...

C3

�





.... . .

...

b0,3(t3) · · · b3,3(t3)

�︸︷︷︸

Bt,3

�P0

...

P3

�=

�B([P0, · · · ,P3], t0)

...

B([P0, · · · ,P3], t3)

�





B([P0, · · · ,P3], ti ) = Ci for all

i ∈ {0, · · · , 3}.

�P0

...

P3

�= B−1

t,3

�C0

...

C3

�





.... . .

...

b0,3(t3) · · · b3,3(t3)

�︸︷︷︸

Bt,3

�P0

...

P3

�=

�B([P0, · · · ,P3], t0)

...

B([P0, · · · ,P3], t3)

�





B([P0, · · · ,P3], ti ) = Ci for all

i ∈ {0, · · · , 3}.

�P0

...

P3

�= B−1

t,3

�C0

...

C3

�


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.


Trajectory Planning


First Method




Second Method




Trajectory Planning


First Method




Second Method




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.










minP1,P2,Q1,Q2

∫ 1

0

�(k(t))2 + (k̇(t))2

�dt


respectively.










minP1,P2,Q1,Q2

∫ 1

0

�(k(t))2 + (k̇(t))2

�dt


respectively.










minP1,P2,Q1,Q2

∫ 1

0

�(k(t))2 + (k̇(t))2

�dt


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



a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]


P1,P2 ∈ C and Q1,Q2 ∈ R


−−−→P2P3 =

−−−→Q0Q1.


arccos

� −−−→P2P3 ·

−→d

‖−−−→P2P3‖ · ‖

−→d ‖

�= 0


−−−→P0P2 ≥ 0

−−−→P0P3 ·

−−−→P0P2 −

−−−→P0P3 ·

−−−→P0P1 ≥ 0

−−−→P0P3 ·

−−−→P3Q1 ≥ 0and

−−−→P0P3 ·

−−−→P3Q2 ≥ 0

−−−→P0P3 ·

−−−→P3Q2 −

−−−→P0P3 ·

−−−→P3Q1 ≥ 0



a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]


P1,P2 ∈ C and Q1,Q2 ∈ R


−−−→P2P3 =

−−−→Q0Q1.


arccos

� −−−→P2P3 ·

−→d

‖−−−→P2P3‖ · ‖

−→d ‖

�= 0


−−−→P0P2 ≥ 0

−−−→P0P3 ·

−−−→P0P2 −

−−−→P0P3 ·

−−−→P0P1 ≥ 0

−−−→P0P3 ·

−−−→P3Q1 ≥ 0and

−−−→P0P3 ·

−−−→P3Q2 ≥ 0

−−−→P0P3 ·

−−−→P3Q2 −

−−−→P0P3 ·

−−−→P3Q1 ≥ 0



a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]


P1,P2 ∈ C and Q1,Q2 ∈ R


−−−→P2P3 =

−−−→Q0Q1.


arccos

� −−−→P2P3 ·

−→d

‖−−−→P2P3‖ · ‖

−→d ‖

�= 0


−−−→P0P2 ≥ 0

−−−→P0P3 ·

−−−→P0P2 −

−−−→P0P3 ·

−−−→P0P1 ≥ 0

−−−→P0P3 ·

−−−→P3Q1 ≥ 0and

−−−→P0P3 ·

−−−→P3Q2 ≥ 0

−−−→P0P3 ·

−−−→P3Q2 −

−−−→P0P3 ·

−−−→P3Q1 ≥ 0



a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]


P1,P2 ∈ C and Q1,Q2 ∈ R


−−−→P2P3 =

−−−→Q0Q1.


arccos

� −−−→P2P3 ·

−→d

‖−−−→P2P3‖ · ‖

−→d ‖

�= 0


−−−→P0P2 ≥ 0

−−−→P0P3 ·

−−−→P0P2 −

−−−→P0P3 ·

−−−→P0P1 ≥ 0

−−−→P0P3 ·

−−−→P3Q1 ≥ 0and

−−−→P0P3 ·

−−−→P3Q2 ≥ 0

−−−→P0P3 ·

−−−→P3Q2 −

−−−→P0P3 ·

−−−→P3Q1 ≥ 0



a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]


P1,P2 ∈ C and Q1,Q2 ∈ R


−−−→P2P3 =

−−−→Q0Q1.


arccos

� −−−→P2P3 ·

−→d

‖−−−→P2P3‖ · ‖

−→d ‖

�= 0


−−−→P0P2 ≥ 0

−−−→P0P3 ·

−−−→P0P2 −

−−−→P0P3 ·

−−−→P0P1 ≥ 0

−−−→P0P3 ·

−−−→P3Q1 ≥ 0and

−−−→P0P3 ·

−−−→P3Q2 ≥ 0

−−−→P0P3 ·

−−−→P3Q2 −

−−−→P0P3 ·

−−−→P3Q1 ≥ 0



a) Ps = [40; 40], Pd = [265; 160],and Pt = [450; 450]


P1,P2 ∈ C and Q1,Q2 ∈ R


−−−→P2P3 =

−−−→Q0Q1.


arccos

� −−−→P2P3 ·

−→d

‖−−−→P2P3‖ · ‖

−→d ‖

�= 0


−−−→P0P2 ≥ 0

−−−→P0P3 ·

−−−→P0P2 −

−−−→P0P3 ·

−−−→P0P1 ≥ 0

−−−→P0P3 ·

−−−→P3Q1 ≥ 0and

−−−→P0P3 ·

−−−→P3Q2 ≥ 0

−−−→P0P3 ·

−−−→P3Q2 −

−−−→P0P3 ·

−−−→P3Q1 ≥ 0


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]




(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]


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]




(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]


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]




(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]


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]




(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]


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]




(y(tk)− y0)2 + (x(tk)− x0)2 > dr2 tk ∈ [0, 1] k ∈ [1, 100]


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]


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.




M1 = Γ(1/6)M2 = Γ(1/3)M3 = Γ(1/2)

and

N0 = Γ(1/2)N1 = Γ(2/3)N2 = Γ(5/6)N3 = Γ(1) = Pd









M1 = Γ(1/6)M2 = Γ(1/3)M3 = Γ(1/2)

and

N0 = Γ(1/2)N1 = Γ(2/3)N2 = Γ(5/6)N3 = Γ(1) = Pd







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




PsPd and_





∫ 1

0

�(k(t))2 + (k̇(t))2

�dt

F (P1,P2,P3,Q1,Q2,R1,R2,R3,M1,M2)

= 1.4394× 10−04




PsPd and_





∫ 1

0

�(k(t))2 + (k̇(t))2

�dt

F (P1,P2,P3,Q1,Q2,R1,R2,R3,M1,M2)

= 1.4394× 10−04


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



Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]

~s = ~d = ~t = [0, 1].

Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]





Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]

~s = ~d = ~t = [0, 1].

Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]





Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]

~s = ~d = ~t = [0, 1].

Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]





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]





Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]

~s = ~d = ~t = [0, 1].

Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]





Ps = [10, 12],Pd = [160, 180]andPt = [340, 350]

~s = ~d = ~t = [0, 1].

Ps = [10, 125],Pd = [180, 200]andPt = [350, 350]





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




Ps = [10, 125]Pd = [180, 200]Pt = [350, 350]

~s = [1, 0]~t = [1, 0.2]~d = [0, 1]





Ps = [10, 125]Pd = [180, 200]Pt = [350, 350]

~s = [1, 0]~t = [1, 0.2]~d = [0, 1]





Ps = [10, 125]Pd = [180, 200]Pt = [350, 350]

~s = [1, 0]~t = [1, 0.2]~d = [0, 1]




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


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


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 )



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 )



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.



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.


Thank You


Science

Bezier Curves Based trajectory planning