Kinematics

Robot Control
(2DOF manipulator & Mobile Robot)

Forward Kinematics

Inverse Kinematics ?

Jacobian

Robot Control

Control
Joint-space control
Task-oriented control
Force control

SKIP

q = q ref q res

f ( )

Prosedur

?????

Buat program dalam bahasa C

Gunakan looping dengan delay 1ms

Tentukan Xd
(trajectory planning)

usleep(1000);

f ( )

Hitung dx,dy

x
Hitung dq1,dq2
(reference)
dgn inverse Jacobian

Hitung x,y
(forward kinematic)

Kp = 900

x =

sekarang

x
t

sebelumnya

x sekarang = x sebelumnya + xt

Kv = 60
l1 = 0.2 m
l 2 = 0.1m
J ( motorinertia ) = 0.1

Trajectory planning

Hitung q1,q2
(double integrator)

Siapkan variabel
Untuk controller
Hitung
torque
=Kp*dq+Kv*ddq

Inverse Jacobian

Linear

x = (displacement ) * v + xinitial
Contoh perubahan dari X awal ke X
command dalam 5 detik

xdesired = ( xcmd xinitial ) t + xinitial

5

1
J acobian
= Adj ( J )

det( J )

Variabel untuk Controller

= Kp q& ref + Kv q&&ref

q& ref = J 1aco x& ref

ref
q&&ref = ( q& ref q& old
) / t

Double integrator

= J inertia q&&
q&& = / J inertia
q& = q& old + q&& t
q = qold + q& t

Forward Kinematik

Mobile Robot

y
l

P0

r
R

Mobile Robot

P0 ( x 0 , y 0 )

The robots control point

x0

The point on X-axes in the world coordinate system

y0

The point on Y-axes in the world coordinate system

The robots direction angle

The radius of a driving wheels

W
r
l

:
:

The robots tread

The rotation angle of the left wheel

The rotation angle of the right wheel

x
R
cos
x& 0 2
y& = R sin
0 2
&
R
W

v = x& 0 cos + y& 0 sin

robot tdk bisa bergerak kesamping

y& 0 cos x& 0 sin = 0

x&0 = v cos

kedua roda berputar

y& 0 = v sin

W &
= R&r
2
W &
x&0 cos + y& 0 sin = R &l
2

x& 0 cos + y& 0 sin +

v=

R
W

( r l )

x& 0 cos + y& 0 sin =

= J aco ( )&

R &
( r + &l )
2

&& = J ( )&&
X
aco

W & = R (&r &l )

J +aco

Jw
J
M
4 + W 2 + R2
r
2
=
R

l
M J2
4 W

R &
( r + &l )
2

cos
2
&
R
sin r
2
&l
R

= ( J Taco J aco ) 1 J Taco

=

M
J

&&
4 W 2 r
J &&
M
J
+ 2 + w2 l
4 W
R

1 cos

R cos

W
2
W

2

sin
sin

y
l

W
x& 0
y& =
0
&

P0

R
2 cos
R
sin
2
R
W

cos
2
&
R
sin r
2
&l
R

J +aco

= ( J Taco J aco ) 1 J Taco

=

= J aco ( )&

1 cos

R cos

sin
sin

W
2
W

2

Blok Diagram ?

&
&d
d

v = x& cos + y& sin

K P
trajectory
planning

k =

KP

x&d

l ) + in it

+ k 1
yk = y k 1 + v sin k
t
2

x&&ref ,

&&ref

&&ref
J aco1 ( )

ref
Jm

1
Jm

&&

1
s

&

J a co ( )

x&, &

KV
clock

R
W

+ k 1
xk = x k 1 + v cos k
t
2

xd

target

dead recockning

KV

x&

dead
recockning