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

Pr. Olivier RUATTA, Pr. Ouiddad Labbani-Igbida

11 June 2015

Mohammad ISSA 11 June 2015 1 / 16

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

be limited.

Bézier Curve

be limited.

Bézier Curve

be limited.

Bézier Curve

be limited.

Bézier Curve

be limited.

Bézier Curve

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) =

�t i (1− t)n−i where

i = 0, · · · , n

De�nition (Bézier Curve)

B(t) =n∑

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) =

i = 0, · · · , n

B(t) =n∑

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) =

i = 0, · · · , n

B(t) =n∑

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) =

i = 0, · · · , n

B(t) =n∑

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)

�︸︷︷︸

�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}.

�= B−1

.... . .

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

�︸︷︷︸

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

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

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

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

�= B−1

.... . .

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

�︸︷︷︸

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

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

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

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

�= B−1

.... . .

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

�︸︷︷︸

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

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

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

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

�= B−1

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

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

Let ~s, ~d and ~t denote the headings at the positions Ps ,Pd and Pr

respectively.

minP1,P2,Q1,Q2

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

respectively.

minP1,P2,Q1,Q2

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

respectively.

minP1,P2,Q1,Q2

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

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)

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)

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

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

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

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

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

= 1.4394× 10−04

PsPd and_

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

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

= 1.4394× 10−04

PsPd and_

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

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

= 1.4394× 10−04

Experimental Results