V.

Perencanaan Trayektori (Trajectory Planning)

Outline
• Review Model Kinematik
– Kinematika Langsung (Direct Kinematic)
• Jacobian Matrix
• Trajectory Planning

1
 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 (Yaw-Pitch-Roll)

2
 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

3
 Review
Reference
Coordinate

• i-1
A i = Tz,d Tz, Tx,a Tx,

Source coordinate C i  C  i S i S  i S i a i C i 

 S C  i C i  S i C  i 
a i S i 
 i
 0 S i C i di 
 
 0 0 0 1 

4
 Review
1  Forward

Cartesian Representation
 
 2  nx sx ax px 
 3  n sy ay p y 
  Kinematics T  y
 4   nz sz az pz 
 5   
0 0 0 1
 
 6  Inverse Task Space
x 

Euler Representation
Joint Space y
 
z 
 
 
 
 
 
5
 Review
• Representasi Yaw-Pitch-Roll
1
T  Rz , R y , Rx , R T  R y , Rx ,
z ,
 C S 0 0  nx sx ax 0   C 0 S 0 1 0 0 0
  S 0 0 n
 C
 y sy ay 0  0 1 0 0  0 C
  S 0

 0 0 1 0  nz sz az 0    S 0 C 0  0 S C 0
       
 0 0 0 1 0 0 0 1  0 0 0 1 0 0 0 1

 C  nx  S  n y XX XX 0
 S  n  C  n  S  s  C  s  S  a x  C  a y 0
 x y x y

 nz XX XX 0
 
 0 0 0 1
 C SS SC 0
 0 C   S  0 
  (Persamaan A)
 S CS CC 0
 
 0 0 0 1 
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 for  x and  y
  
90    180 for  x and  y
  a tan 2( y, x)    
  180     90 for  x and  y
  90    0 for  x and  y

8
 Jacobian Matrix

1  x  1 
 
Forward  x 
y    y 
 2    2  Jacobian  
 3  z  3  Matrix  z 
  Kinematics      
 4     4   x 
 5       y 
     5  
 6  Inverse   6   z 

Joint Space Task Space Jacobian Matrix: Hubungan antara joint

space velocity dengan task space velocity

9
 Jacobian Matrix
Kinematika Lngsung
x   q1   h1 (q1 , q2 , , q6 ) 
y q  h (q , q , , q )
    2  2 1 2 6 
z   q3   h3 (q1 , q2 , , q6 )  Y61  h(qn1 )
   h ( )
  61   

  q
 4 h (
 4 1 2q , q ,  , q )
6 
   q5   h5 (q1 , q2 , , q6 ) 
     
  q
 6  h ( q
 6 1 2 , q ,  , q )
6  61

 d dh(q ) dq dh(q )
Y61  h(qn1 )   q
dt dq dt dq
 x 
 y   q 1 
  q 
 z   dh(q )   2 
   dq     Y61  J 6n q n1
 x    6n
 y    dh(q)
  q n  n1 J
 z  dq
10
 Jacobian Matrix
 x 
 y 
   q 1 
 z   dh(q )  q 2  Matrik Jacobian adalah
    
 fungsi dari q, (tidak
 x   dq  6n    konstan !)
 y   
 q n  n1
 
 z   h1 h1 h1 
 q 
q2 qn 
 1 
 dh(q )   h2 h2

h2 
J      q1 q2 qn 
 dq  6n     
 h h6 h6 
 6  
 q1 q2 qn  6n
11
 Jacobian Matrix
Kinematika Langsung
 x   h1 (q ) 
p   y   h2 (q )
0 n s a p  z   h3 (q ) 
T6  
0 0 0 1  44
 (q )  h4 (q)
 h1 (q)  {n, s, a}   (q )    h5 (q ) 
h (q )  (q ) h6 (q ) 
Y61  h(q )   2 
    x 
   y  Kecepatan Linier Kecepatan Sudut
 
h6 (q )   z  V  x  
Y      
 
    
  x    V   y    
Y61  J 6n q n1  y 
 
   z   
 z 
12
 Contoh
• 2-DOF planar robot arm (x , y)
– Diketahui l1, l2 , Hitung : Matrik Jacobian
 2

 x  l1 cos1  l2 cos(1   2 )  h1 (1 , 2 )  l2

 y    l sin   l sin(   )   h ( , )
  1 1 2 1 2   2 1 2 

  x  1 
Y     J 
  l1
 y   2 
1

 h1 h1 
   2   l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 )
J  1 
 h2 h2   l1 cos1  l2 cos(1   2 ) l2 cos(1   2 ) 
 1  2 
13
 Jacobian Matrix
•Interpretasi Fisik
 q 1 
 J11 J12  J16  q 
J J 22  J 26 
 2
 q 3 
Y  Jq   21  
     q 4 
 q 5 
   
 J 61 J 62  J 66   q 6 

 x   J11q 1  J12 q 2    J16 q 6 

 y   J q  J q    J q 
   21 1 22 2 26 6 

 z   J 31q 1  J 32 q 2    J 36 q 6  Menunjukan bagaimana

     setiap kecepatan joint

   41 1 J q  J q    J q
46 6 
42 2 space memberikan
   J 51q 1  J 52 q 2    J 56 q 6  kontribusi ke kecepatan
    dalam task space.
   J 61q 1  J 62 q 2    J 66 q 6 
14
 Robot Motion Planning
T asks • Path planning
T ask P lan – Geometric path
– Path adalah adalah ruang kurva
dimana lengan robot (robot arm), yang
A ction P lan diwakili oleh masing-masing joint,
bergerak dari posisi awal (initial
position) menuju posisi akhir (final
P ath P lan position
– Permasalahan : obstacle avoidance,
T rajectory
shortest path
P lan • Trajectory planning,
– “interpolasi” atau “aproksimasi” path yg
diinginkan (desired path) dengan
C ontroller fungsi-fungsi polinomial dan
membangkitkan urutan nilai “set point”
untuk pengendalian robot dari posisi
R obot awal (initial point) menuju posisi yang
diinginkan (destination point).
S ensor
15
 Configuration Space
Jika konfigurasi robot berada di dalam daerah biru akan menabrak
halangan
360
qrobot
270

180


 90

qslug
0 
45 90 135 180
An obstacle in the robot’s workspace a “C-space” representation
Visualization of high dimension
C-space is difficult
16
 Trajectory Planning
(continuity,
sm oothness)
P ath
constraints

joint space
{q (t ), q (t ), q(t )}
P ath T rajectory sequence of control set points
or
specification P lanner along desired trajectory

{ p(t ), v(t ), a(t )}

cartesian space

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
Joint i
F inal
q(tf)
q(t2)

• Path Profile S et dow n

• Velocity Profile q(t1)

Lift-off
Initial
q(t0)

• Acceleration Profile
t0 t1 t2 tf T im e
S peed

t0 t1 t2 tf T im e
A cceleration

t0 t1 t2 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 Jo in t i
2) Initial velocity F inal
q(tf)
3) Initial acceleration q(t2)
Se t dow n
4) Lift-off position
5) Continuity in position at t1
6) Continuity in velocity at t1
7) Continuity in acceleration at t1 q(t1)
8) Set-down position Lift-off
Initial
9) Continuity in position at t2 q(t0)

10) Continuity in velocity at t2

t0 t1 t2 tf T im e
11) Continuity in acceleration at t2
12) Final position
13) Final velocity
14) Final acceleration

21
 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 t0t1, 5 unknow

h2 (t )  a23t 3  a22t 2  a21t  a20 t1t2, 4 unknow

hn (t )  an 4t 4  an 3t 3  an 2t 2  an 2t  an 0 t2tf, 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
 Configuration Space
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)

Konfigurasi q dari A adalah

spesifikasi dari status/keadaan fisik
Bm dari A terhadap FW.
B1

Configuration Space (Ruang Konfigurasi) adalah ruang semua

kemungkinan konfigurasi gerak robot
30
 Configuration Space
Ruang konfigurasi A adalah ruang (C ) semua
kemungkinn konfigurasi A.
Perhatikan robot titik (free-flying, no constraints)

C
Cfree
qgoal
Cobs

qstart

Untuk robot titik yang bergerak dalam bidang 2-D , Ruang konfigurasi
2
(C-space) adalah R 31
 Configuration Space
C

y
 Cfree qgoal

Cobs
qstart
x

Untuk robot titik yang bergerak dalam bidang 3-D , Ruang konfigurasi (C-
3
space) adalah R

32
 Configuration Space
Y
Robot bergerak hanya translasi dalam bidang

C-space: 2-D (x, y)

X

2
Euclidean space:R
Y Robot bergerak translasi dan rotasi dalam bidang

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


Y
X
x

33
 Configuration Space

360

qrobot
270

180


 90

qslug
0 
45 90 135 180

34
 Configuration Space
Robot akan menabrak halangan jika konfigurasi robot dalam daerah
warna biru
360
qrobot
270

180


 90

qslug
0 
45 90 135 180
Representasi “C-space”
Berapakah dimensi dari C-space untuk Visualisari C-space untuk
robot PUMA (6R)? dimensi yang banyak sangat
sulit 35
 Motion Planning Problem
Mencari jalur bebas halangan (collision
free) mulai dari konfigurasi awal
menuju konfigurasi akhir (goal) dengan
memperhatikan kendala2 (geometri,
fisik, temporal)

Konsep C-space
memberikan
kerangka kerja
umum untuk
mempelajari
persoalan
motion planning

36
Bagaimana jika robot bukan dianggap sebagai sebuah titik ?

37
Bagaimana jika robot bukan dianggap sebagai sebuah titik ?

Memperluas
Halangan

38
Obstacles Configuration Space
C-obstacle

Point robot
39
Free Space
From
Robot Motion Planning
J.C. Latombe

40
 Minkowski Sums
Perluasan bentuk bidang planar oleh bidang lain disebut dengan
Minkowski sum 

robot persegi yang hanya bergerak translasi

PR
R

(Dilation operation) P  R = { p + r | p  P and r  R }

41
 Penambahan Dimensi
Bagaimana bentuk ruang C-obstacle jika robot persegi dapat bergerak
translasi dan rotasi dalam bidang.
(Kotak biru adalah halangan)

y
robot persegi yang dapat bergerak translasi dan rotasi

x
42
 C-obstacle in 3-D
Bagaimana bentuk ruang C-obstacle jika robot persegi dapat bergerak
translasi dan rotasi dalam bidang.
3-D
(Kotak biru adalah halangan)

y 360º

180º

0º
x this is twisted...
43
 C-obstacle in 3-D
Bagaimana bentuk ruang C-obstacle jika robot persegi dapat bergerak
translasi dan rotasi dalam bidang.
3-D
(Kotak biru adalah halangan)

y 180º

0º
x
44
 Untuk satu irisan
Perhatikan satu irisan dari C-obstacle dimana rotasi robot sebesar
45o

PR
y R
45 degrees

x
45
Proyeksi 2-D

46
 Topik yang dibahas

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

47
 Metoda Motion Planning
Komponen dasar motion planning dapat diuraikan sbb :
Input Output
• deskripsi geometri robot dan
lingkungannya (obstacles) • jalur mulai dari posisi awal
(start) menuju posisi akhir
• konfigurasi awal (initial) dan
(finish)
tujuan (goal)
qrobot qgoal
Aplikasi :
Robot-assisted surgery
Automated assembly plans
Drug-docking and analysis
Moving pianos around...

48
 Metoda Motion Planning
Tujuan mengurangi N-dimensi ruang
konfigurasi (configuration space) menjadi
(1) Roadmap approaches pencarian jalur satu dimensi

(2) Cell decomposition Tujuan menghitung semua ruang

bebas
Tujuan menghasilkan strategi kendali
(3) Potential Fields lokal yang lebih fleksibel dibandingkan
dua metoda diatas
Perencanaan jalur dengan
(4) Bug algorithms
pengetahuan yg terbatas

49
 Roadmap: Visibility Graphs
Visibility graphs: Ruang konfigurasi dalam bentuk poligon atau
polyhendral yang membentuk segmen garis yang menghubungkan
antar vertex.

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
 Visibility graph drawbacks
Visibility graphs sukar menunjukan jalur optimal pada dimensi
yang tinggi

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 batas-
batas 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
 Perencanaan Gerak
Roadmap approaches
• Visibility Graph
• Voronoi Diagram

Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global

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

Grafik konektifitas yang

(Sweepline algorithm) menyatakan hubungan antara sel
yang berdekatan

62
 Exact Cell Decomposition
Trapezoidal Decomposition:

Terdapat beberapa algoritma graph-

search untuk menemukan jalur dari
posisi awal sampai tujuan

63
 Exact Cell Decomposition
Trapezoidal Decomposition:

Posisi jalur terletak di titik tengah

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

64
 Optimalisasi
Trapezoidal Decomposition:

15 cells 9 cells
Metoda trapezoidal decomposition Cara yang dapat dilakukan hanya
eksak (exact) dan lengkap memperoleh jumlah minimum sel.
(complete), tetapi tidak optimal

65
 Approximate Cell Decomposition
Quadtree Decomposition:

Secara rekursif membagi (subdivides) daerah

Quadtree:
bebas/halangan (free/obstacle (sub)region)
kedalam 4 sisi (four quarters)
66
Approximate Cell Decomposition
Quadtree Decomposition:

67
Approximate Cell Decomposition
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
 Metoda Perencanaan Pergerakan
Roadmap approaches
Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global

69
 Potential Field
– 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
 Potential Field
• Perhitungan gaya atraktif untuk menuju ke tujuan

Menuju nol ketika robot

C-obstacles semakin mendekat ke tujuan

Attractive potential

71
 Potential Field
• Perhitungan gaya repulsif untuk menghindar halangan

72
 Potential Field
• Jumlah Potensial Attractive potential Repulsive potential

C-obstacle

Sum of potentials
73
 Potential Field
• pembangkitan gaya artificial (negative gradient)
• Jumlah gaya keseluruhan digunakan untuk
mengendalikan robot
Negative
gradient
Equipotential contours

Total potential

74
 Potential Field
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
 Metoda Perencanaan Pergerakan
Roadmap approaches
• Visibility Graph
• Voronoi Diagram

Cell decomposition Full-knowledge

motion planning
• Trapezoidal decomposition
• Quadtree decomposition
Potential Fields
Bug algorithm Limited-knowledge path planning

76
 Bug Algorithms

• diketahui arah dari tujuan

Goal
• Hanya memiliki sensor lokal

(walls/obstacles sensor)

Start
77
 Bug Algorithms
Insect-inspired “bug” algorithms Perpindahan pada 2 jenis tingkah laku
sederhana:
1. Bergerak langsung menuju tujuan
2. Menelusuri halangan
(Circumnavigating an obstacle)

“Bug” algorithm

1) Mengarah menuju tujuan

2) Ikuti halangan sampai
diperoleh LOS dari tujuan

assume a leftist 3) Kembali ke tahap 1

robot

78