Chapter 6. Motion Control (Robot Trajectory Simulation)mercury.kau.ac.kr/sjkwon/Lecture/robotics/Robotics-ch… · · 2016-11-07Motion Control (Robot Trajectory Simulation)

2016 (KAU AME)

, Chapter 6

1

Chapter 6. Motion Control

(Robot Trajectory Simulation)

(Robotics)

1)

2) RK4

2016 (KAU AME)

, Chapter 6

2

Robot dynamics Simulation: (1) Joint

1

2

3

1 1 1q q q

2 2 2q q q

3 3 3q q q

1

( , ) ( ) ( )

( )

( , ) ( )

( , ) ( )

Hq C q q G q t

t

Hq C q q G q

q H C q q G q

(Ex.

Robot dynamic equation of motion:

joint torque

(Forward dynamics)

:

( ), ( )

( )

q t q t

t

) RK4

Joint torque ?

1) torque : step input, sinusoidal input

2)

2016 (KAU AME)

, Chapter 6

3

Robot dynamics Simulation: (2) End-effector

1 1 1

2 2 2

1

,

,

( , ) ( )

,

( , , )

RK4 Joints

(position & velocity)

1) End-effec

Robot dynamic equation :

Position:

tor

n n n

x y z

q q q

q q q

q H C q q G q

q q q

p p p

/0

1 1 2 2 3 3

0 0 0 1( , , )

( ) ( ) ( ) ( )

( , , )

( ,

Position kinematics

Ori

2) En

entaion:

d-effecto

Velocity:

RPY rate:

r

x x x x

y y y y

n

z z z z

n n

x y z

n o a p

n o a p

n o a pT

A q A q A q A q

v v v

1 1

0

( ) 0

1 0

, )

,

Velocity kinematics

x

y

z

P

x x

y y

z n

o

P o

z

s c c

c c s

s

v q q

v

v q

J

JJ

J J

p = q = q

nq

2016 (KAU AME)

, Chapter 6

4

Joint Position Control

Joint loop

Task space Joint space Joint Position control loop desired

command

Robot dynamics (joint angle) feedback

Joint angle error (e)

Joint control algorithm (ex, PID control)

(u) + disturbance(d) Joint torque

( ) ( ) ( )

( ) ( ) ( )

1( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )

d

d

p D I p D I

e t q t q t

e t q t q t

K s k k s k e s u t k e t k e t k e t dts

t u t d t

- Joint angle error:

- Joint velocity error:

- PID contro

Input torque

l:

- :

Robotdynamics

K(s)

Joint controller(ex, PID)

( )Command

dq t ( )q t( )t( )e t ( )u t

( )d t

2016 (KAU AME)

, Chapter 6

5

Independent Joint Control

n-dof robot manipulator (multi-loop)

Joint controllers(ex. PID)

Joint spacetrajectory

Joint spacevariables

Robot

Dynamics

K1(s)1Command

dq 1q1e

Kn(s)ndq ne

nq

2dq K2(s) 2q

Joint sensors

Task space(end-effector)

trajectory

Inverse kinematics

Pathplanning 1 1 1u d

n n nu d

2 2 2u d

( )d t

forward kinematics

0

1 1 2 2 3 3( ) ( ) ( ) ( )n nT

P o

nT A q A q A q A q

J J Jp = q = qTask space

variables

2016 (KAU AME)

, Chapter 6

6

211 12 1 11 1 1 2

212 22 2 22 2 2 1

1

11 12 11

12 22 22

H H G

H H G

H H

H H

Robot dynamic equation of motion:

2 2

:

211 1 2 1 1 2 1 2 1

222 2 1 2 1 2 1 1

1

2

( , , , , )

( , , , , )

l

G f

G f

4 1

2 2

: link

:

(Ex.)

(

= 2 1 2

( ) ( ) ( )t q t t

l

q

input torque

(Ex.) RK4

RK4 step = sampli

li

ng

nk

time ( )1ms = 0.001

:

(

m

1, =0.5m), =7kg, m =3kg)

Plant

torque

[1] (function RK4) 2-DOF or 3-DOF Robot dynamics RK4

(Input: joint torque Output: ( )

Dynamic Simulation of Two-link Robot Term Project

2016 (KAU AME)

, Chapter 6

7

[2] (function ForKinem) End-effector (, )

Input: function [1] ( , ) Output: end-effector ,

Forward Kinematics

/ plot

[3] (function InvKinem) End-effector

Input: function [4] (end-effector ) Output:

Inverse kinematics joint .

joint control loop reference command .

/ plot

[4] (function PathGen) 2-DOF, 3-DOF robot end-effector

Input: ( , ) Output: end-effector

Task space ( 2 )

1) (Line trajectory): y=ax+b

2) (Circle trajectory): (x-a)^2 + (y-b)^2=R^2

// .

Dynamic Simulation of Two-link Robot

2016 (KAU AME)

, Chapter 6

8

0

0 0( , ) (0,2 cos 45 )x y l

End-effector :

x

0

1 135

y

0

2 90

(Ex. 1) (Line trajectory)

0 0

2sec

( , ) ( , )f

f f

t

x y x l y

End-effector (at ):

1 2

1 2

( .)

1000 , 500

7 , 3

Ex

l mm l mm

m kg m kg

link :

link :

2 3

0

0

0 0 0

1 2 3

( ) , ( ) 3

0) : ( ) 0, ( ) 0

2 ) : ( ) 1.0 , ( ) 0

( )

Task space trajectory

f f f

x t c c x

y t y x t polynomial

t x t x t

t x t m x

c x c x

t

(at

(at


Sampling time = RK4 step () 1ms = 0.001

2016 (KAU AME)

, Chapter 6

9

0

0 0( , ) (0,2 cos 45 )Ene-effector

x y l =

(Ex. 2) (Circle trajectory)

0

22 2

0

0 0

0

2 3

0 1 2 3

0

( ) sin ( )

( ) ( ) cos (

(0, ), 0.5

( )

( ) 3

0) : ( ) 0, ( ) 0

2sec) : )

)

(

( )

f f

x t R t

y t y R R t

y R R l

x y y R R

t polyno

t c

mial

t t t

t

t c c t

t

c t

Circle equation:

(at

(at 2 , ( ) 0f ft

x

0

1 135

y

0

2 90 R


1 2

1 2

( .)

1000 , 500

7 , 3

Ex

l mm l mm

m kg m kg

link :

link :

2016 (KAU AME)

, Chapter 6

10

[5] (function JointCtrl) Joint position control

Input: (reference com.): joint [3]

: joint [1]

Output: (control input)

Joint torque = + disturbance (, sine )


( ) ( ) ( )

( ) ,

( )

1( ) ( ) ( )

( ) ( 1)( ) ( ) (

p D I

p I

d

d

D

p D I u t k e t k e

e t q q

e t q q

K s k k s k t k e t dt

e k e ku k k e k k k Te k

T

e ss

Joint angle error:

Joint velocity error:

PID control:

discrete-time :

max

, , )

(Ex.

) ( 1)

(

0.2 , 50 ra

) sin( )

) /

,

d se

)

c

p D I

d d

d

d

k k k

u k

d t D t D

D

D

u

* PID gain( tuning

square wav

e(

Disturbance :

(ex.)

max max max( ) 400,300,200,10

(

0,50

)

[

) )

]

( (

u u t u N

t t d t

m

u

Joint torque :

: ( ) u

2016 (KAU AME)

, Chapter 6

11

[6] (function Main) function [1]~[5] simulation program

1) Matlab Simulink m-file coding

2) Visual C++


Robot

Dynamics

K1(s)1Command

dq

1 1,q q1e 1u

Kn(s)2e

2dq

2u 2 2,q q

Task space trajectory(end-effector)

Inverse kinematics

Path planning

Forward kinematics Torque !(motor torque )

1

1d

2

2d

disturbance !

[5]

[4]

[3]

[2]

[1]

2016 (KAU AME)

, Chapter 6

12

Term Project Report (Due date: )

2-DOF or 3-DOF manipulator

Discussion

Presentation (PPT file = ) 12/?

1)

2) plot

3) , disturbance , torque plot

4) Inertia matrix element , Coriolis force, centrifugal

force plot

5) Disturbance

[]

1)

(Matlab, VC++ with OpenGL, solid model simulator )

2) Planar 3-DOF manipulator (end-effector )


2016 (KAU AME)

, Chapter 6

13

gravity term discussion

1) :

2) :

(= 0)

3) viscous friction damping :

damp out!

:

1 1 2 12

1 1 2 12

x l c l c

y l s l s

2 2 2 2

1 2 1 2 2

2 2 2 2

1 22

1 2 2 2 2

2

2 2

2

2 ( , )

1

x y l l l l c

x y l lc

l l s c

s c

atan2

,x y

1 1 2 1

1 1 2 2 2 2 2

1 1 2 1

1

1 1 2 1 2

2 2

1 2 1 2 11 2

1 1 1

, ,

1

( , )

x k c k sk l l c k l c

y k s k c

c k k k x k yx

s k k k x k yy k k

s c

atan2

1 2,c c

2016 (KAU AME)

, Chapter 6

14

Runge-Kutta (RK4 Numerical Integration Method for IVP of Simultaneous

ODEs)

:

(Ordinary differential equation, ODE)

(Partial differential equation, PDE)

: IVP(Initial Value Problem)

2016 (KAU AME)

, Chapter 6

15

Mass-Damper-Spring System

m

k

c

y

x

1 2

1 2

2 2 1

( ) ( (

( )

,

1

Mass-damper-spring system:

: ), )

(2 )

(2 ) 2

let

F t kx cx mx k c

mx cx kx F t

y y

x y x y F c ky y y

m m m

1

1

2

( ) ( )

( ) ( )

y t x t

y t x t

( )F t

m

( )F t

kxcx

mx

X-

2016 (KAU AME)

, Chapter 6

16

2

2

sin cos sin

cos sin cos

sin sin 0 (

sin ,cos 1)

Mass-damper-spring system:

Tangential

)

for small

x l x l l

y l y l l

gmg ml

l

(

sin 0 0 ( )g g

l l

Pendulum

(ODE)

1) (Linear ODE):

2) (Nonlinear ODE):

Pendulum()

my

x

l m

mg

T (tension)

mx

my

m= ml2ml

m=

1 2

1 2

2 1

1

2

1

,

( ) ( )

( ) ( )

let y y

y yg

y yl

y t t

y t t

:

2016 (KAU AME)

, Chapter 6

17

1

1

( , ) ( )

) ) ( )

1

data

1

(at (at

i i

i i

dyf x y y g x

dx

y y h

x x x x h

(x y ) .

2016 (KAU AME)

, Chapter 6

18

Euler Method

1

1 1

1

1

( , )

( , )

( , )

( , )

1

: Euler method

i i

i i i

i i i ii i i

i i i

i i i i

dyy f x y

dx

y y h

y f x y

y y y ydyy f x y

dx x x h

y y f x y h

:

iy

1iy

( , )i ih f x y h y

2016 (KAU AME)

, Chapter 6

19

Euler (RK2)

1

1

10

11

1

[1] ( , )

( , )

:

[2]

( , ) ( , )

2 2

(

i

i i i i

i i i

i i i

i i i

i

i ii

i i

y y f x y h

y f x y

y y y h

f x y f x yy yy

f xy y

Euler

Heun

:

:

:

[3] (Midpoint method):

1/ 2

1/ 2

1/ 2 1/ 2 1/

1/ 2 / 2

2

1

( , )2

)

, )

( ,

i

i i i i

i i

i

i

x

hy y f x y

h

y f x y

y

:

Midpoint:

2016 (KAU AME)

, Chapter 6

20

Euler (RK2)

2016 (KAU AME)

, Chapter 6

21

Runge-Kutta

1 1 2 2

1

( , )

( , , )

( , , )

( , , ) :

1

h)

Incremental function ( )

i i i i

i i n n

i i

dyf x y

dx

y y x y h h

x y h

x y h a k a k a k

(

1 1

1

2 1 11 1

3 2 21 1 22 2

1 1,1 1 1,2 2 1, 1 1

1

~ ~

( , )

( , )

( , )

( , )

1) : 1

RK

n n

i i

i i

i i

n i n i n n n n n

n

a a k k

k f x y

k f x p h y q k h

k f x p h y q k h q k h

k f x p h y q k h q k h q k h

,

2

3

4

RK (RK1) Euler

2) : 2 RK (RK2) Heun , Midpoint

3) : 3 RK (RK3)

4) : 4 RK (RK4)

n

n

n

2016 (KAU AME)

, Chapter 6

22

3 Runge-Kutta

1 2 3

1

1

2 1

2

1 3

1

2

3

1 4 1, ,

6 6 6

14

( , )

1 1( , )

2 22

6

( , )

[RK3]

, ,

i i

i

i i

i i

i

k k k

y y k k k

k f x y

k f

h

x h y k h

k f x h y k h k h

2016 (KAU AME)

, Chapter 6

23

4 Runge-Kutta :

1 2 3 4

1

2 1

1 1

3 2

4 3

2 3 4

1 2 2 1, , ,

6 6 6 6

( , )

1 1( , )

2 21 1

( , )2 2

( ,

6

)

12 2i i

i i

i i

i i

i i

k k k k

k f x y

k f x h y k h

k f

y y k k k k h

x h y k h

k f x h y k h

[RK4] , , ,

2016 (KAU AME)

, Chapter 6

24

n n 1

n n 1

n

11 1 2

22 1 2

1

1 2

( , )

( , , , , )

( , , , , ): ~

( , , , , )

1 :

n 1

n

nn

nn n

dyy f x y

dx

dyf x y y y

dxdy

f x y y yy ydx

dyf x y y y

dx

n

2016 (KAU AME)

, Chapter 6

25

n n 1

( 1)1 2

( ) ( 1)

( 1)

11 1 2

22 3

3

2

( , , , , ,

, ,

)

, ,

,

(

,

, ,

n :

)

Let

Then, n

nn n

n

n

n

n

d yy f x y y y y

dxy

y y y y y y y y

y y y

dyy f y

dxdy

y f ydx

n

1 2

0

0

1 1 2

2 2 1 2

1

2

( , , , , )

( , , )

(0)( ,

(0)

( , ,

1

:

) 2

2

Let

nn n n

dyy f f x y y y

dx

y y

y f x y y

y yy y

y y

y f y

y f f x y yy y

:n

1 0

2 0

(0)

) (0) with

y y

y y

2016 (KAU AME)

, Chapter 6

26

2 RK4

1

2 2

1

1,2 2,2 3,2

1,1 2,1 3,1 4,

4,2

,2

1

1 1 1,1 ,1

2 2 1,2 ,2

1

1,1 1 ,1

,2 2 21, ,

2 2

( , )

1:

61

:6

,

2 2

( , , )

i i

i i

i i

i

i

i i y

y

f

f

y f y y h

y f y y h

k k k k

k k k k

y

k yf y

f

x

k yx

for

for

For

For

RK4

:

1 1

1

2 2

1

2,1 1 ,1 1,1

,1 1,1

3,1

,2 1,2

2,2 2 ,2 1,2

,2 2,2

3,

1 ,1 2,1

2

0.5

( , 0.

( 0.5 , )

0.5

( , 0.5

0.5 ,

0.5 ,

0. , )

5

.55

)

0

i

i

i

i

i

i

i

i

i y f

y

y f

f

y k hk f y k h

y k h

k f y k

x h

xk f y k h

y k hh

h

x

k

h

f

f

o

or

for

r

2

2 2

1 1

2,1 2,1

4,1 1 ,1 3,1

,

2 ,2 2,2

,2 3,2

4,2 1 3,2 ,2 3,21

0.5 ,

( ,

( 0.5 )0.

)

, )

5 ,

,

,(

ii i

i

i i

i i

i

y f

y f

y f

f y k h

y k h

k f y k h

y k h

k

x

f y

h

x h h

x

k

y k hh

for

for

for

1

2 1 1 21 01

22 2 0

1 2 2 1 2

2

0

20

( , , )(0)

(0)( , , ) ( , , )

(0)( , , )

(0)

dyy f x y y

y yy y dxdyy y y y

f x y y f x y ydx

d y y yy f x y y

y ydx

Let with

with

2016 (KAU AME)

, Chapter 6

27

RK4

( )

(RK4 method h )

(xout h )

(xout )

xout

2016 (KAU AME)

, Chapter 6

28

MatLab Code (script m-file)

% main.m

% === Example of RK4 Integration algorithm

%------- 2nd order ODE ----------------------% y_ddot + a*y_dot + b*y = 0% with (I.C.) y(t=0) = , y_dot(t=0) =

clear;

%---------- System Parameters --------------a = 10.; b = 100.;

% (I.C.)x1(1) = 3.0; x2(1) = 0.0;

h = 0.01; % Integration time interval tf = 3.0; % Final time t = [ 0: h: tf]';

%--- RK4 integration start -----------------------------for i = 1: 1: size(t,1)-1

% Slopes k1, k2, k3, k4k11 = x2(i) ; k12 = -a*x2(i) - b*x1(i); k21 = x2(i) + 0.5*k12*h ; k22 = -a*( x2(i) + 0.5*k12*h ) - b*( x1(i) + 0.5*k11*h ); k31 = x2(i) + 0.5*k22*h ; k32 = -a*( x2(i) + 0.5*k22*h ) - b*( x1(i) + 0.5*k21*h ); k41 = x2(i) + k32*h ; k42 = -a*( x2(i) + k32*h ) - b*( x1(i) + k31*h );

% Updated value x1(i+1) = x1(i) + (k11 + 2*k21 + 2*k31 + k41)*h/6; x2(i+1) = x2(i) + (k12 + 2*k22 + 2*k32 + k42)*h/6;

end;

figure(1); subplot(211); plot(t, x1); xlabel('time(sec)'); ylabel('x1'); title('Position'); grid; subplot(212); plot(t, x2); xlabel('time(sec)'); ylabel('x2'); title('Velocity'); grid;

2

0

(0) 3, (0) 0

100 10 / sec

10 2

( , )

(I.C.) 3m

Natural freq.( ),

Damping coeff.( ),

.

n n

n

n

y ay by

y y Spring

kb rad

ma

2016 (KAU AME)

, Chapter 6

29

MatLab Code (function m-file)

% === Example of RK4 Integration algorithm

% It requires m-files with subfunctions rk4.m & dyn_eqn.m

%========================================

% : rk4_main.m or .m

function rk4_main()

% subfunction m-file main

%------- 2nd order ODE ----------------------

% y_ddot + a*y_dot + b*y = u(t) with (I.C.) y(0), y_dot(0)

clear;

%---------- System Parameters --------------

%global a b h;

a = 3.; % c/m

b = 100.; % k/m

h = 0.01; % Integration time interval

% (I.C.)

x(1,:) = [3.0 0.0]; %x1(1) = 3.0; x2(1) = 0.0;

tf = 3.0; % Final time

t = [ 0: h: tf]';

%--- RK4 integration start -----------------------------

for i = 1: 1: size(t,1)-1

% Input

u(i) = 0;

x(i+1,:) = rk4( x(i,:), u(i), a, b, h );

end;

figure(1);

subplot(211); plot(t, x(:,1)); xlabel('time(sec)'); ylabel('x1'); title('Position'); grid;

subplot(212); plot(t, x(:,2)); xlabel('time(sec)'); ylabel('x2'); title('Velocity'); grid;

% Subfunction 1

% rk4.m

function x = rk4( x, u, a, b, h )

% Slopes k1, k2, k3, k4

2016 (KAU AME)

, Chapter 6

30

MatLab Code (inline function )

% rk4_inline_fn.m

function rk4_inline_fn()

%---------- System Parameters --------------

a = 3.; % c/m

b = 100.; % k/m

h = 0.01; % Integration time interval

x(1,:) = [3.0 0.0]; %x1(1) = 3.0; x2(1) = 0.0; % (I.C.)

tf = 3.0; t = [ 0: h: tf]';

% inline functions for

% y_ddot + a*y_dot + b*y = u(t) with (I.C.) y(0), y_dot(0)

%-----------------------------------

f1 = inline('x2');

f2 = inline('-a*x2 - b*x1 + u', 'x1','x2','u','a','b');

%--- RK4 integration -----------------------------

for i = 1: 1: size(t,1)-1

u(i) = 0; % Input

k11 = f1(x(i,2));

k12 = f2(x(i,1), x(i,2), u(i), a, b);

k21 = f1(x(i,2)+0.5*k11*h);

k22 = f2(x(i,1)+0.5*k11*h, x(i,2)+0.5*k12*h, u(i), a, b);

k31 = f1(x(i,2)+0.5*k21*h);

k32 = f2(x(i,1)+0.5*k21*h, x(i,2)+0.5*k22*h, u(i), a, b);

k41 = f1(x(i,2)+k31*h);

k42 = f2(x(i,1)+k31*h, x(i,2)+0.5*k32*h, u(i), a, b);

x(i+1,1) = x(i,1) + h*( k11 + 2*k21 + 2*k31 + k41 )/6;

x(i+1,2) = x(i,2) + h*( k12 + 2*k22 + 2*k32 + k42 )/6;

end;

Documents

Chapter 6. Motion Control (Robot Trajectory Simulation)mercury.kau.ac.kr/sjkwon/Lecture/robotics/Robotics-ch… · · 2016-11-07Motion Control (Robot Trajectory Simulation)