Perencanaan Trayektori

OUTLINE

Review Model Kinematik

Kinematika Langsung (Direct Kinematic)
Jacobian Matrix
Trajectory Planning

REVIEW
Langkah penurunan model kinematik:
Tentukan KK berdasarkan Metoda D-H
Mengukur parameter2 link
Menghitung matrik transformasi untuk setiap joint yang berdekatan
(adjacent joints)
Menghitung Matrik Kinematik (POSE)
Jika diperlukan nyatakan POSE dalam representasi sudut Euler 3 (YawPitch-Roll)

REVIEW
Matrik Transformasi D-H untuk KK yang
berdekatan, i dan i-1.
Posisi dan orientasi KK i dapat diekspresikan dalam
KK i-1melalui transformasi secara berurutan 4 urutan
transformasi dasar

REVIEW
Reference
Coordinate

i-1

A i = Tz,d Tz, Tx,a Tx,

Source coordinate

C i
S
i

C i S i

S i S i

C i C i
S i
0

S i C i
C i
0

a i C i

ai S i
di

 1

2
3
4
5

6

Kinematics

Joint Space

Inverse

nx
n
T y
nz

sx

ax

sy
sz
0

ay
az
0

px

p y
pz

Task Space

x
y

z

Euler Representation

Forward

Cartesian Representation

REVIEW

REVIEW
Representasi Yaw-Pitch-Roll
1
z ,

R T R y , Rx ,

T Rz , R y , Rx ,
C
S

S
C
0
0

0 0
0 0
1 0

0 1

C nx S n y
S n C n
x
y

nx
n
y
nz

sx

ax

sy
sz
0

ay
az
0

0
0

C
0

1 0
0 C
0 0

XX

S s x C s y

S a x C a y

nz

XX

XX

0
0
0
0

C
0

SS
C
CS
0

0
0
0

1

XX

SC
S
CC
0

1 0
0 C

0
S

S
0

C
0

0
0

0
0

0
0
0

(Persamaan A)
6

REVIEW
Bandingkan LHS dan RHS Persamaan A

sin nx cos n y 0

a tan 2(n y , nx )

cos nx sin n y cos

nz sin

a tan 2(nz , cos nz sin n y )

sin s x cos s y cos

sin a x cos a y sin

a tan 2(sin a x cos a y , sin s x cos s y )
7

REVIEW
0 90

90 180
a tan 2( y, x)

180

90

90 0

for

x and

for x and y
for x and y
for x and y

JACOBIAN MATRIX

1

2
3

4
5

6

Forward

Kinematics

Inverse

Joint Space

x
y

z

Task Space

1

2
3

4

5

x
y

Jacobian

Matrix

Jacobian Matrix: Hubungan antara joint

space velocity dengan task space velocity

JACOBIAN MATRIX
Kinematika Lngsung
x
q1
h1 (q1 , q2 , , q6 )
y
h (q , q ,, q )
q
6
2

2 1 2

z
h3 (q1 , q2 , , q6 )
q3

h
(
)

61

h
(
q
,
q
,

,
q
)
q
6

4 1 2
4

h5 (q1 , q2 , , q6 )
q5

h
(
q
,
q
,

,
q
)

q
6
6
6 1 2

x
y

dh(q )

dq

x
y

6n

61

d
dh(q ) dq dh(q )

Y61 h(qn1 )

q
dt
dq dt
dq

q 1
q
2

Y61 h(qn1 )

q n

n1

Y61 J 6n q n1
dh(q)
J
dq

10

JACOBIAN MATRIX
x
y

x
y

dh(q )

dq

q 1
q
2

6n

Matrik Jacobian adalah

fungsi dari q, (tidak
konstan !)

q n

n1

dh(q )

J
dq

6n

h1
q
1
h2
q1

h
6
q1

h1
q2
h2
q2

h6
q2

h1

qn

h2

qn

h6

qn

6n

11

JACOBIAN MATRIX
x h1 (q )
p y h2 (q )
z h3 (q )
(q ) h4 (q)
{n, s, a} (q ) h5 (q )
(q ) h6 (q )

Kinematika Langsung

n s a
T6
0 0 0

p
1

4 4

h1 (q)
h (q)
Y61 h(q ) 2

h6 (q )
Y61 J 6n q n1

x
y

V
Y

x
y

Kecepatan Linier Kecepatan Sudut

x
V y
z

12

CONTOH
2-DOF planar robot arm
Diketahui l1, l2

, Hitung : Matrik Jacobian

x l1 cos 1 l2 cos(1 2 ) h1 (1 , 2 )
y l sin l sin( ) h ( , )
1
1
2
1
2
2 1 2
1
x

Y J

y
2

h1

1
J
h2
1

(x , y)

l2

l1
1

h1
2 l1 sin 1 l2 sin(1 2 ) l2 sin(1 2 )

h2 l1 cos1 l2 cos(1 2 ) l2 cos(1 2 )
2
13

JACOBIAN MATRIX
Interpretasi Fisik

J11
J
Y Jq 21

J 61

J12 J16
J 22 J 26

J 62 J 66

x J11q 1 J12 q 2 J16 q 6

y J q J q J q
22 2
26 6
21 1
z J 31q 1 J 32 q 2 J 36 q 6

J
q

J
q

J
q

42 2
46 6
41 1
J 51q 1 J 52 q 2 J 56 q 6

J 61q 1 J 62 q 2 J 66 q 6

q 1
q
2
q 3

q 4
q 5

q 6

Menunjukan bagaimana
setiap kecepatan joint
space memberikan
kontribusi ke kecepatan
dalam task space.
14

ROBOT MOTION PLANNING

Tasks
T a s k P la n
A c tio n P la n
P a t h P la n
T r a je c t o r y
P la n
C o n t r o lle r
R obot
S ensor

Path planning

Geometric path
Path adalah adalah ruang kurva
dimana lengan robot (robot arm), yang
diwakili oleh masing-masing joint,
bergerak dari posisi awal (initial
position) menuju posisi akhir (final
position
Permasalahan : obstacle avoidance,
shortest path

Trajectory planning,

interpolasi atau aproksimasi path

yg diinginkan (desired path) dengan
fungsi-fungsi polinomial dan
membangkitkan urutan nilai set
point untuk pengendalian robot dari
posisi awal (initial point) menuju posisi
yang diinginkan (destination point).
15

ConfigurationSpace
Jika konfigurasi robot berada di dalam daerah oranye akan menabrak
halangan
360

qrobot

270

180

90

qslug
0

Anobstacleintherobotsworkspace

45

90

135

180

aCspacerepresentation
Visualization of high dimension
C-space is difficult
16

TRAJECTORY PLANNING
( c o n t in u ity ,
s m o o th n e s s )

P a th
c o n s tr a in t s

s e q u e n c e o f c o n t r o l s e t p o in t s
a lo n g d e s ir e d tr a je c to r y

P a th
s p e c if ic a t io n

T r a je c to r y
P la n n e r

jo in t s p a c e

{q(t ), q (t ), q(t )}
or

{ p (t ), v(t ), a(t )}
c a r t e s ia n s p a c e

17

TRAJECTORY PLANNING
Metoda Perencanaan trayektori :
Ruang variabel joint (joint variable space), dimana trayektori
dibentuk berdasarkan posisi,kecepatan, percepatan joint
Ruang Cartesian (Cartesian Space), dimana trayektori dibentuk
berdasarkan posisi, kecepatan, percepatan ujung lengan robot

Pendekatan ruang variable joint

Keuntungan : 1)trayektori direncanakan secara langsung sesuai
dengan variabel kendali untuk pergerakan, 2) perencanaan
trayektori dapat dilakukan real-time, 3) joint trajectory lebih
mudah dibuat
Kerugian : sulit untuk menentukan posisi link maupun lengan
robot selama bergerak, dimana biasanya sebuah pergerakan
(initial to final) harus dijamin tidak menabrak halangan (obstacle
avoidance)

18

TRAJECTORY PLANNING
J o in t i

Path Profile
Velocity Profile

Set down

q (t1 )

q (t0 )

Acceleration
Profile

F in a l

q (tf)
q (t2 )

L ift- o ff
I n it ia l

t0

t1

t2

tf

T im e

Speed

t0
A c c e le r a tio n

t0

t1

t1

t2

t2

tf

T im e

tf T im e

19

PERSYARATAN

Beberapa hal yang menentukan

perencanaan trayektori
Nilai set point harus sudah terhitung sebelum
dilakukan pergerakan
Posisi antara (intermediate position, knot point)
harus ditentukan
Kontinyuitas posisi, kecepatan dan percepatan
harus dijamin agar trayektori yang dirancang
berjalan mulus (smooth)

20

PERSYARATAN
1)
Initial position
2)
Initial velocity
3)
Initial acceleration
4)
Lift-off position
5)
Continuity in
position at t1
6)
Continuity in
velocity at t1
7)
Continuity in
acceleration at t1
8)
Set-down position
9)
Continuity in
position at t2
10)
Continuity in velocity at
t2
11)
Continuity in
acceleration at t2
12)
Final position
13)
Final velocity
14)
Final acceleration

J o in t i

F in a l

q (tf)
q (t2 )

Set down

q (t1 )

q (t0 )

L ift - o ff
I n it ia l

t0

t1

t2

tf

21

T im e

PERSYARATAN
Initial Position
Position (given)
Velocity (given, normally zero)
Acceleration (given, normally zero)
Final Position
Position (given)
Velocity (given, normally zero)
Acceleration (given, normally zero)

22

PERSYARATAN
Intermediate positions
set-down position (given)
set-down position (continuous with previous trajectory segment)
Velocity (continuous with previous trajectory segment)
Acceleration (continuous with previous trajectory segment)

23

PERSYARATAN
Intermediate positions
Lift-off position (given)
Lift-off position (continuous with previous trajectory segment)
Velocity (continuous with previous trajectory segment)
Acceleration (continuous with previous trajectory segment)

24

Trajectory Planning
Terdapat Beberapa Jenis Trajectory
4-3-4 Trajectory, dimana Path dibagi menjadi 3
segmen
segmen 1 diwakili oleh fungsi polinomial 4 derajat
(initial to lift-off position)
Segmen 2 diwakili oleh fungsi polinomial 3 derajat
(lift-off to set-down position)
Segmen 3 diwakili oleh fungsi polinomial 4 derajat
(set-down to final position)
3-5-3 Trajectory, sama seperti diatas namun derajat
fungsi polinomial berturut-turut adalah 3,5 dan 3
5-Cubic Trajectory, dimana Path dibagi dalam 5
segmen. Setiap segmen dinyatakan dalam fungsi
polinomial 3 derajat

25

TRAJECTORY PLANNING
4-3-4 trajectory
h1 (t ) a14t 4 a13t 3 a12t 2 a12t a10

t0t1, 5 unknow

h2 (t ) a23t 3 a22t 2 a21t a20

t1t2, 4 unknow

hn (t ) an 4t 4 an 3t 3 an 2t 2 an 2t an 0

t2tf, 5 unknow

3-5-3 trajectory

26

MOBILE ROBOT MOTION PLANNING

27

UNTUK APA MOTION PLANNING?

Untuk menentukan kemana akan bergerak tanpa menabrak halangan
(obstacle)

28

MATERI YANG DIBAHAS

Dasar

Ruang Konfigurasi (Configuration Space)

C-obstacles

Metoda-metoda Motion Planning

Roadmap Approaches
Visibility graphs
Voronoi diagram

Cell Decomposition

Trapezoidal Decomposition
Quadtree Decomposition

Potential Fields
Bug Algorithms

Referensi :

G. Dudek, M. Jenkin, Computational Principles of Mobile Robots,

MIT Press, 2000 (Chapter 5)
J.C. Latombe, Robot Motion Planning, Kluwer Academic
Publishers, 1991.

29

ConfigurationSpace
Notasi:
A: Obyek kaku (rigid) (misalnya robot)
2
W: Ruang Euclidean dimana A bergerak; W R

atau

R3

B1,Bm: halangan (obstacle) kaku dan tetap (fixed)

tersebar dalam W
FW KK Bumi (fixed frame)
FA KK robot (moving frame)

B1

Bm

Konfigurasi q dari A adalah spesifikasi

dari status/keadaan fisik dari A
terhadap FW.

ConfigurationSpace(RuangKonfigurasi)adalahruangsemua
kemungkinankonfigurasigerakrobot

30

ConfigurationSpace
RuangkonfigurasiAadalahruang(C )semua
kemungkinnkonfigurasiA.
Perhatikan robot titik (free-flying, no constraints)

Cfree

qgoal
Cobs

qstart
Untukrobottitikyangbergerakdalambidang2D,Ruangkonfigurasi
2
R
(Cspace)adalah

31

ConfigurationSpace
C

Cfree

qgoal
Cobs

qstart
x
Untuk robot titik yang bergerak dalam bidang 3-D , Ruang konfigurasi (C3
space) adalah R
32

CONFIGURATION SPACE
Y

Robot bergerak hanya translasi dalam bidang

C-space:

2-D
(x, y)

Euclidean space:R
Y

Robot bergerak translasi dan rotasi dalam bidang

C-space:3-D (x, y,

Y
X

x
33

ConfigurationSpace
360

qrobot

270

180

90

qslug
0

45

90

135

180

34

ConfigurationSpace
Robot akan menabrak halangan jika konfigurasi robot dalam daerah
warna oranye
360

qrobot

270

180

90

qslug
0

Berapakah dimensi dari C-space untuk

robot PUMA (6R)?

45

90

135

180

RepresentasiCspace
Visualisasi C-space untuk
dimensi yang banyak sangat
sulit

35

MotionPlanningProblem
Mencarijalurbebashalangan(collision
free)mulaidarikonfigurasiawal
menujukonfigurasiakhir(goal)dengan
memperhatikankendala2(geometri,
fisik,temporal)
Konsep C-space
memberikan
kerangka kerja
umum untuk
mempelajari
persoalan
motion planning
36

Bagaimanajikarobotbukandianggapsebagaisebuahtitik?

37

Bagaimanajikarobotbukandianggapsebagaisebuahtitik?

Memperluas
Halangan

38

OBSTACLES CONFIGURATION SPACE

C-obstacle

Point robot

39

FREE SPACE
From
Robot Motion Planning
J.C. Latombe

40

MinkowskiSums
Perluasanbentukbidangplanarolehbidanglaindisebutdengan
Minkowskisum
robot persegi yang hanya bergerak translasi

PR

(Dilation operation)

PR={p+r|pPandrR}

41

PenambahanDimensi
BagaimanabentukruangCobstaclejikarobotpersegidapatbergerak
translasidanrotasidalambidang.
(Kotakbiruadalahhalangan)

robot persegi yang dapat bergerak translasi dan rotasi

42

Cobstaclein3D
BagaimanabentukruangCobstaclejikarobotpersegidapatbergerak
translasidanrotasidalambidang.
3-D
(Kotakbiruadalahhalangan)

360

180

this is twisted...

0
43

Cobstaclein3D
BagaimanabentukruangCobstaclejikarobotpersegidapatbergerak
translasidanrotasidalambidang.
3D
(Kotakbiruadalahhalangan)

180

0
44

Untuksatuirisan
PerhatikansatuirisandariCobstacledimanarotasirobotsebesar
45o

PR
R

45degrees

45

Proyeksi2D
y

x
46

TOPIK YANG DIBAHAS

Configuration Space
Motion Planning Methods
Roadmap Approaches
Cell Decomposition
Potential Fields
Bug Algorithms

47

MetodaMotionPlanning
Komponendasarmotionplanningdapatdiuraikansbb:
Input
Output
deskripsigeometrirobotdan
jalurmulaidariposisiawal
lingkungannya(obstacles)
(start)menujuposisiakhir
konfigurasiawal(initial)dan
(finish)
tujuan(goal)
qrobot
qgoal
Aplikasi :
Robotassistedsurgery
Automatedassemblyplans
Drugdockingandanalysis
Movingpianosaround...
48

MetodaMotionPlanning
(1)Roadmapapproaches
(2)Celldecomposition
(3)PotentialFields
(4)Bugalgorithms

TujuanmengurangiNdimensiruang
konfigurasi(configurationspace)menjadi
pencarianjalursatudimensi
Tujuanmenghitungsemuaruang
bebas
Tujuanmenghasilkanstrategikendali
lokalyanglebihfleksibeldibandingkan
duametodadiatas
Perencanaanjalurdengan
pengetahuanygterbatas

49

Roadmap:VisibilityGraphs
Visibilitygraphs:Ruangkonfigurasidalambentukpoligonatau
polyhendralyangmembentuksegmengarisyangmenghubungkan
antarvertex.

50

THE VISIBILITY GRAPH

Pertama kali, tarik garis dari titik awal dan tujuan ke
semua vertex yang terlihat (visible) dan sudut
(corner) dari Kerangka bumi

goal
start

51

THE VISIBILITY GRAPH

Kedua, tarik garis dari setiap vertex halangan
(obstacle) ke vertex dan sudut yang terlihat.

goal
start

52

THE VISIBILITY GRAPH

goal
start

53

THE VISIBILITY GRAPH

goal
start

54

THE VISIBILITY GRAPH

goal
start

Karena peta ini dalam ruang konfigurasi (C-space) setiap garis dapat
merepresentasikan bagian dari jalur pergerakan dari awal menuju tujuan.
55

Visibilitygraphdrawbacks
Visibilitygraphssukarmenunjukanjaluroptimalpadadimensi
yangtinggi
shortest path

shortest path within the visibility

graph

Tidak ada clearance

56

ROADMAP: VORONOI DIAGRAMS

Generalized Voronoi
Diagram (GVG)
membentuk jalur
yang berjarak
sama
(equidistant)
terhadap 2 atau
lebih halangan
(obstacle)
termasuk batasbatas ruang
(workspace)
Memaksimumkan
clearance antara
halangan.

Metoda ini menghasilkan peta jalan (roadmap) yang

menghindari halangan seaman mungkin

57

VORONOI DIAGRAM: METRICS

Beberapa cara untuk pengukuran jarak,
diantaranya :
L1 metric
(x,y) : |x| + |y| = const

L2 metric
(x,y) : x2 +y2 = const

58

VORONOI DIAGRAM (L1)

L1 Tidak
memiliki
jalur yang
berbentuk
kurva

59

VORONOI DIAGRAM (L2)

60

PerencanaanGerak
Roadmap approaches
Visibility Graph
Voronoi Diagram

Celldecomposition
ExactCellDecomposition(Trapezoidal)
ApproximateCellDecomposition(Quadtree)
PotentialFields
Hybridlocal/global

61

EXACT CELL DECOMPOSITION

Trapezoidal Decomposition:
Penguraian (decomposition)
ruang bebas ke dalam bentuk sel
trapezoidal & triangular

(Sweeplinealgorithm)

Grafik konektifitas yang

menyatakan hubungan antara sel
yang berdekatan
62

EXACT CELL DECOMPOSITION

Trapezoidal Decomposition:

Terdapatbeberapaalgoritmagraph
searchuntukmenemukanjalurdari
posisiawalsampaitujuan
63

EXACT CELL DECOMPOSITION

Trapezoidal Decomposition:

Posisi jalur terletak di titik tengah

( mid-points) garis sambung
(intersection) antar sel

64

Optimalisasi
Trapezoidal Decomposition:

15cells

9cells

Metodatrapezoidaldecomposition Carayangdapatdilakukanhanya
eksak(exact)danlengkap
memperolehjumlahminimumsel.
(complete),tetapitidakoptimal
65

ApproximateCellDecomposition
Quadtree Decomposition:

Quadtree:

Secararekursifmembagi(subdivides)daerah
bebas/halangan(free/obstacle(sub)region)
kedalam4sisi(fourquarters)

66

ApproximateCellDecomposition
Quadtree Decomposition:

67

ApproximateCellDecomposition
Quadtree Decomposition:
sel persegi panjang secara rekursif
diuraikan menjadi luas yang lebih kecil
pada tingkat resolusi tertentu, hanya sel
yang luasnya terletak dalam ruang bebas
yang digunakan

Quadtree
68

MetodaPerencanaanPergerakan
Roadmapapproaches
Celldecomposition
ExactCellDecomposition(Trapezoidal)
ApproximateCellDecomposition(Quadtree)
PotentialFields
Hybridlocal/global

69

PotentialField
Lokasi tujuan membangkitkan potensial atraktif (attractive potential)
yang bertujuan menarik (pull) robot untuk menuju tujuan
Obyek halangan membangkitkan potensial repulsif (repulsive potential)
yang bertujuan mendorong robot untuk menjauh dari halangan
gradien negatif dari potensial total diperlakukan sebagai gaya artifisial
yang diberikan kepada robot
-- Jumlah gaya keseluruhan digunakan untuk mengendalikan robot
C-obstacles

70

PotentialField
Perhitungangayaatraktifuntukmenujuketujuan

C-obstacles

Menujunolketikarobot
semakinmendekatketujuan

Attractive potential
71

PotentialField
Perhitungan gaya repulsif untuk menghindar halangan

72

PotentialField
JumlahPotensial

Attractive potential

Repulsive potential

C-obstacle

Sum of potentials

73

PotentialField
pembangkitangayaartificial(negativegradient)
Jumlahgayakeseluruhandigunakanuntuk
mengendalikanrobot

Equipotential contours

Negative
gradient

Total potential
74

PotentialField
Pros:

Perencanaan dan kendali bergabung dalam satu fungsi

dapat diperoleh jalur pergerakan yang halus (smooth paths)

Cons:
terjebak dalam lokal minima (perlu ada teknik random walk atau
backtracking untuk menghadapi lokal minima)
pengetahuan tentang bentuk halangan harus jelas (seringkali tidak
mungkin)
75

MetodaPerencanaanPergerakan
Roadmap approaches
Visibility Graph
Voronoi Diagram

Celldecomposition
Trapezoidal decomposition

Fullknowledge
motionplanning

Quadtreedecomposition
PotentialFields
Bugalgorithm

Limited-knowledge path planning

76

BugAlgorithms

Goal

diketahui arah dari

tujuan
Hanya memiliki sensor
lokal
(walls/obstacles sensor)

Start

77

BugAlgorithms
Insectinspiredbugalgorithms

Perpindahan pada 2 jenis

tingkah laku sederhana:
1.

Bergerak langsung menuju

tujuan

2.

Menelusuri halangan
(Circumnavigating an
obstacle)

Bugalgorithm

1)Mengarahmenujutujuan
2)Ikutihalangansampai
diperolehLOSdaritujuan
assume a
leftist robot

3)Kembaliketahap1
78