Chapter 6. Motion Control Systems (Trajectory …mercury.hau.ac.kr/sjkwon/Lecture/robotics/Robotics-chap6...시뮬레이션결과정리 Discussion Presentation (PPT file = 보고서)

로봇공학 (KAU AME)

로봇공학, Chapter 6

1

Chapter 6. Motion Control Systems

(Trajectory Control Simulation)

로봇공학 (Robotics)

1) 2-link manipulator trajectory tracking control

2) RK4 Numerical Integration Algoruithm(연립 상미방 수치해)



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)

알고리즘 적용

위치 속도

시뮬레이션에서 각 의 는 어떻게 발생시키

제

는가

임의

어

의

입력



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



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

+−

++

( )n t+

외란 入力

센서 노이즈入力

기준 入力

모델링 오차



5

Independent Joint Control

▪ n-dof robot manipulator의 경우(multi-loop)

Joint controllers(ex. PID)

Joint spacetrajectory

Joint spacevariables

Robot

Dynamics

K1(s)1

Command

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



6

211 12 1 11 1 1 2

212 22 2 22 2 2 1

1

11 121

12 222

( )

( )

H H G t

H H G t

H H

H H

−

•

•

+ + + =

=

Robot dynamic equation of motion:

2 2

원 연립 차 미방 형태로 표현:

1 1 1 21 1 2 1 2 1 1 2 1 2 1

2 2 1 22 1 2 1 2 2 1 2 1 2 1

1 1

1 2

2 3

2 4

( , )( , , , ) ( , , , , )

( , )( , , , ) ( , , , , )

gc f

gc f

x

x

x

x

•

− − =

=

4 1 원 연립 차 미방 형태로 표현:

상태변수 정의

1 2

2 1 1 2 1 2 1

3 4

2 2 1 2 1 2 1

( , , , , )

( , , , , )

x x

x f

x x

x f

=

= =

=

[1] (function RK4) 2-DOF or 3-DOF Robot dynamics에 대한 RK4 수치적분 함수

(Input: joint torque → Output: 각 조인트 운동 (각도 및 속도)

Dynamic Simulation of Two-link Robot Term Project



7

1 2 1 2l l

→

•

→

:

link

(Ex.) RK4

RK4 step = sampling time,

link

(Ex.) 2-link 매니퓰레이터 모델

길이 ( 무게

미분방정식에 대한 수치적분 알고리즘 구현:

미분방정식을 적분하기 위

알고리즘

한 함수

(m

작성

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

분

kg)

적

( ) ( ) ( )t q t q t→

→

input torque

Plant

1)

2)

( )1ms = 0.001

주어진 에 대하여 조인트 각도 및 각속도 산출

출력값은 로봇제어에서 의 센서 출력값으로 취급

중력에 의한 진자운동 확인

일정한 torque 입력에 대한 출력을 확인

예 초



8

[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



9

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초



10

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 :

길이

무게



11

[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

떻게 변화하는지를 고찰

입력

제어 입력 제 예한



12

[6] (function Main) function [1]~[5]를 연결하는 simulation program 완성

1) Matlab 이용 → Simulink 보다는 m-file coding 권장

2) Visual C++ 이용


Robot

Dynamics

K1(s)1

Command

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 kinematicsTorque 입력 제한!(motor torque의 한계)

1

1d

2

2d

disturbance 입력!

[5]

[4]

[3]

[2]

[1]



13

▪ 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의 변화,

5) Coriolis force, centrifugal force의 변화 plot

6) Disturbance 입력에 따른 제어오차의 변화

[권장사항]

1) 로봇 제어 동작 애니메이션

(Matlab, VC++ with OpenGL, 기타 solid model simulator 이용)

2) Planar 3-DOF manipulator로 확장(end-effector 자세제어 포함)




14

유의 사항

▪ 보고서에 운동방정식에서 gravity term의 영향에 대한 discussion 첨가

1) 수평 평면 운동의 경우: 중력 항을 제거

2) 수직 평면 운동의 경우: 중력 항을 첨가

토크 입력이 없을 때(제어입력= 0) 중력에 의한 자유진자 운동 발생

3) 각 조인트에 viscous friction에 의한 damping 항 첨가:

→ 시간에 따라 진자운동 damp out!

▪ 2-link 역기구학 해:

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 s

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




15

Dynamic Simulation of 3-link Robot



16

연립 상미방 수치해를 위한 Runge-Kutta 법(RK4 Numerical Integration Method for IVP of Simultaneous

ODEs)

▪ 미분방정식:

• 미지함수와 미지함수의 도함수로 구성되어있는 방정식

▪ 상미분방정식(Ordinary differential equation, ODE)

• 함수가 한 개의 독립변수만 포함

▪ 편미분방정식(Partial differential equation, PDE)

• 함수가 한 개 이상의 독립변수를 포함

▪ 미방에 대한 초기치 문제: IVP(Initial Value Problem)



17

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-방향의 운동방정식



18

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

•

= =

= →

= −

→

= →

=

차미방에 대한

수치적분 알고리즘

수치해 구하기:



19

상미방에 대한 수치적분

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의 )를 찾음.

차 상미방에대한 수치적분 알고리즘의일반적인형태

새로운 기울기

기울

값 이전값 구간간격

각 알고리즘의차이 를 결정하는 방법의기 차이



20

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



21

개선된 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:

기울기

구간 중앙점에서의기울기



22

개선된 Euler법 (RK2법)



23

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

→

= →

=

=

차 법 법

차 법 법 법

차 법

차 법



24

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

+

=

= + +

= + +

= + − +

+

기울기 에 각각 가중치

구간 시작점의기울기

구간 중앙점의기울기

구간 끝점의기울기



25

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] , , , 기울기 에 각각 가중치

구간 시작점의기울기



구간 끝점의기울기



26

연립미분방정식

▪ 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개필요



27

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

= =



28

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



29

모듈화된 RK4 코드 작성

(미분방정식 계산)

(RK4 method에 의해h 구간 끝의 적분값 계산)

(xout 구간 내에서h 간격으로 적분)

(xout 간격으로적분값 저장)

xout 간격으로 출력→



30

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

+ + =

= =

= = = → =

= = → =

을 앞으로 당겼다 놓는 경우

고유진동수

감쇠계수

값을 변화시켜볼 것



31

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



32

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 Systems (Trajectory …mercury.hau.ac.kr/sjkwon/Lecture/robotics/Robotics-chap6...시뮬레이션결과정리 Discussion Presentation (PPT file = 보고서)