Chapter 3Chapter 3

Manipulator Kinematics머니퓰레이터 기구학머니퓰레이터 기구학

1

Introduction

Configuration of manipulatorLink + joint– Link + joint

Joint (관절): enable two links move relatively회전 관절 두 링크가 상대적인 회전운동을 하도록 연결– revolute or rotary joint (회전 관절): 두 링크가 상대적인 회전운동을 하도록 연결

– prismatic joint (직동 관절) : 상대적인 직선운동을 하도록 체결

– Representation of joint• Revolute joint :R• prismatic: P

2

머니퓰레이터 ( i l t )머니퓰레이터 (manipulator)– 목적에 따라 다수의 링크와 관절이 직렬 또는 병렬로 연결되어 다자유도의 운동

을 구현

말단장치(end-effector)– 머니퓰레이터의 마지막 링크에 연결되어 구체적인 임무를 수행

말단장치(end-effector)의 위치와 방위

– 작업자에게 제일 중요한 고려사항

– 직교 좌표계 값으로 쉽게 인지 및 설정

– 각 관절에 부착된 모터의 적절한 구동으로 구현

말단 장치의 위치 및 자세를 나타내는 직각 좌표( X )로 표현된 변수들과, 각 관절의회전량(또는 직동 관절의 경우 직선 이동거리)을 나타내는 관절변수( )들 사이의관계를 파악해야 함

θ관계를 파악해야 함

θ⇔x3

θ⇔x

Manipulator Kinematics

Kinematics : the science of motion which treats motion without regard to the forces (torques) that cause itthat cause it

: treats position, velocity, acceleration, jerk, etc and all higher order derivatives of position variables w.r.t. time.

Relationships between motions and the forces (torques) that cause themdynamics (Chapter 6)

4

Manipulator kinematics : relationships between end-effector position/orientation described in rectangular coordinate and

corresponding joint variables that implements them actually: 직교 좌표로 표현된 말단장치의 위치 및 방위 값들과 이를 실질적으로 구현하는 각 관

절변수들 사이의 관계

(정 기구학)– forward kinematics (정 기구학)

• 주어진 머니퓰레이터의 관절변수 값의 조합에 의해서 만들어지는 머니퓰레이터 말단장치의 위치와 방위를 구하여 직교좌표 값으로 표현하는 문제

– inverse kinematics (역 기구학)• 말단장치의 목표 위치와 방위를 만들어 내기 위해 필요한 관절변수 값의 조

합을 찾아내는 것합을 찾아내는 것

5

Manipulator Coordinate setup and link parameters

복잡한 형상과 다자유도 머니퓰레이터의 기구학

말단장치의 위치 및 방위를 기저좌표계에 대한 상대값으로 표현해야 함

한꺼번에 기저와 말단장치의 상대위치, 방위 구하기는 어려우므로 우선 각 링크간의상대위치와 상대방위 정보 필요

각 링크에 좌표계를 설정하고 좌표계 사이의 관계를 동차변환을 이용하여 각 링크간의 상대 위치와 상대 방위에 대한 정보 확보한 후 조합

말단장치의 위치 및 방위 정보 획득

6

Link Description

a minimum of six joints is required to position an end-effector generally in 3D space(ex. 처음 3개 position, 마지막 3 joints orientation)

Will find the relation between these six joint variables and e.e. position/orientationj p

A. Robot as a kinematic chainManipulator: set of bodies (links) connected in a chain by jointsp ( ) y jI. Link

• rigid bodies connected in a chain by joints• link numbering: 0, 1, 2, ... , ng

- links are numbered from the immobile base ⇒ link 0- First moving body link 1

II. Joint • forms a connection between a neighboring pair of links• i -th joint = the point in space where links i-1 and i are connected• joint numbering : 1, 2, 3, ... N

h j i i d h i l• each joint is assumed to have a single DOF

revolute joint: rotation angleprismatic joint: displacementJoint variable: p j p

Side View

joint 2 joint 3joint 1Revolute

Side View

link 2

link 3 joint 4Prismaticjoint

joint

li k 4link 1

jlink 4

Immobile baselink 0 joint i = connects (i-1)th and (i)th links

Top ViewTop View

link 1

link 3joint 3

link 2link 2

joint 2

link 0joint 1

B. Link connection descriptionHow to describe the location of each link relative to its neighbors? g

use frame attached to each link!Let frame a frame attached to link iTo describe relationship between two adjacent frames and in 3D space (thi

{}i{ }1−i {}i

nk of mapping in Ch.2), we need - 3 components for the relative position vector- 3 Euler angles for the orientation

{ } {}

need 6 parameters

By choosing the frame coordinate axes cleverly, any robot can be described kinematically using four or less quantities for each link (link parameters)

Denavit-Hartenberg notation

Link Parameters

Li k l th Di t b t t j i tLink length ai-1 Distance between two joint axesmutually perpendicular line segment to both axes

Link Twist angle relative location of two joint axesαLink Twist angle relative location of two joint axes

Link offset di distance along joint axis from one link to the next

1−iα(두 joint axes가 얼마나 비틀어 졌나)

Joint angle amount of rotation about joint axis iθ

a, describe the link itself (link 자체가 어떤 형태로 생겼는지 설명, 길이와 꼬인 정도)

d, describe the link’s connection to a neighboring link

α

θ(주위 link와 어떻게 연결되어 있나 설명)(주위 link와 어떻게 연결되어 있나 설명)

좌표계 설정방법

Denavit-Hartenberg 규약 가장 대표적

Denavit Hartenberg convention : frame attachment procedureDenavit-Hartenberg convention : frame attachment procedure

– 링크 번호를 고정된 머니퓰레이터 기저에서부터 마지막 링크까지 0부터 n까지할당한다할당한다.

– 모든 관절을 찾아내고 관절의 기준 축(회전 관절은 회전축, 직동 관절은 운동방향)을 따라 연장선을 그린 후 z축으로 설정한다 이때 링크 i 1 과 링크 i 를 연결향)을 따라 연장선을 그린 후 z축으로 설정한다. 이때 링크 i-1 과 링크 i 를 연결하는 관절 축을 zi로 설정한다. (관절 번호 1부터, 링크 번호 0부터)

– 인접한 두 개의 관절축 zi 와 zi 에 대한 공통 법선이 zi와 만나는 점 또는 zi 와인접한 두 개의 관절축 zi 와 zi+1에 대한 공통 법선이 zi와 만나는 점, 또는 zi 와zi+1가 교차하는 경우 교차점을 좌표계 { i }의 원점 Oi로 설정한다.

– zi 와 zi 1가 서로 교차하면 두 축이 만들어낸 평면에 수직(normal)한 방향을, 교zi 와 zi+1가 서로 교차하면 두 축이 만들어낸 평면에 수직(normal)한 방향을, 교차(intersect)하지 않으면(including paralle) 두 축에 공통으로 수직(perpendicular)인 방향을 xi로 설정한다.

– 오른손 좌표계 규정을 따라 yi를 설정한다.

12

• 링크 인자(link parameter)– 링크 자체의 형상과 인접 링크들간의 상대 관계를 나타냄

링크 길이 xi-1 축을 따라서 측정한 zi-1 축과 zi 축 사이의 거리1−ia링크 뒤틀림 각 xi-1 축을 중심으로 측정한 zi-1 축과 zi 축 사이의 각도

링크 오프셋 z i 축을 따라서 측정한 xi-1 축과 xi 축 사이의 거리

관절 각 z 축을 중심으로 측정한 x 축과 x 축 사이의 각도

1−iα

id

θ관절 각 zi 축을 중심으로 측정한 xi-1 축과 xi 축 사이의 각도iθ

관절변수(joint variable) iq관절 각

링크 오프셋

iθ

id(회전량과 직선 이동거리를 나타낼 수 있으므로)

13

Joint i-1

Joint i Joint i+1

link i-1

αiz

io x

iy

1−iα i ix

1−iz1−iy

ai-1iθ

1−ix

Link length the distance from to measured along

Link Twist angle the angle from to measured aboutα1−ia 1−iz iz 1−ix

z z xLink Twist angle the angle from to measured about

Link offset distance from to measured along

Joint angle the angle from to measured about

1−iαidiθ

1−iz iz 1−ix1−ix ix iz1−ix ix iz

Joint iJoint i+1

Joint i-1

link i-1

1−iα

ai-1 iαiθ i

Side View

joint 2, z2 joint 3, z3joint 1, z1Revolutejoint

3z2zSide View a2a1

link 2

link 3

d3

joint 4 , z4

Prismaticjoint

joint

d43x3y

2z

x1z

link 1

0210 === ααα

d2 a0=a3=0j

2x

1xd1

Immobile baselink 0 o903

210

−=αd1

Note.0z

Top View

Many different choices of {0} possibled1 varies accordingly

0z

0x

Top View

θ2θ

3θ3x

1θ

HW & Quiz at the next class!!!

F & li k t i D it H t b• Frames & link parameters using Denavit-Hartenberg

17

Multiple possible frame assignmentsMultiple possible frame assignments– Frame assignment not unique

– zi 와 zi+1가 서로 교차할 경우 좌표 xi의 설정은 두 축의 공통 평면에 수직한 방향이므로 좌표계의 설정은 여러 가지로 가능하다이므로 좌표계의 설정은 여러 가지로 가능하다

18

{}i { }1−iNow construct the transformation that defines frame in terms of frame

1−iα

1−ia

By intuition, we know these two frames are different by rotation displacement 1i

idiθ

displacement rotation

• 좌표계 { i-1}을– xi-1 축을 중심으로 각도 만큼 회전한 후– x 축을 따라서 거리 만큼 이동한 후

1−iα1ia– xi-1 축을 따라서 거리 만큼 이동한 후

– zi 축을 중심으로 각도 만큼 회전한 후– zi 축을 따라서 거리 만큼 이동

{ i }와 { i-1} 일치됨1−iaiθ

id

• 두 링크 사이의 상대 관계– 네 단계의 상대변환– { i }와 { i-1} 일치됨– 각 단계의 변환행렬을 앞에서부터 순차적으로 곱하면

19

• 좌표계 { i-1}을– xi-1 축을 중심으로 각도 만큼 회전한 후– xi-1 축을 따라서 거리 만큼 이동한 후– zi 축을 중심으로 각도 만큼 회전한 후

{ i }와 { i-1} 일치됨

1−iα1−ia

iθ– zi 축을 따라서 거리 만큼 이동

• 두 링크 사이의 상대 관계

i

id

두 링크 사이의 상대 관계– 네 단계의 상대변환– { i }와 { i-1} 일치됨– 각 단계의 변환행렬을 앞에서부터 순차적으로 곱하면

),(),(),(),( 11111

iiRiitransiitransiiRi

i zTdzTaxTxTT θα −−−−− =

동일한 축에 대한 변환

– 각 단계의 변환행렬을 앞에서부터 순차적으로 곱하면

),(),(),(),( 1111 iitransiiRiiRiitrans dzTzTxTaxT θα −−−−= (교환법칙 성립)

⎥⎥⎥⎤

⎢⎢⎢⎡

−−−

= −−−−

−0

1111

1

iiiiiii

iii

ddsscccs

asc

θθαααθαθ

θθ

⎥⎦

⎤⎢⎣

⎡=

10rR R과 r :

두 링크 i-1과 i 사이의 상대 회

20⎥⎥

⎦⎢⎢

⎣−−−−

10001111 iiiiiii dccscss αααθαθ ⎦⎣ 10

전량과 상대거리(변위)

Joint i-1Joint i

Joint i+1<Transformation frame {i} in terms of {i-1}>

link i-1

y

1−iαiz

ioix

iy

ai 1 θ

1−iz1−iy iθ

{A}{C}

i-1iθ

1−ix1−iα

{B}Using intermediate frames A, B, C{i 1} {A} {B} {C} {i}

)ˆ( axT)ˆ(xT α{i-1}⇒{A} {A}⇒{B} {B}⇒{C}

)ˆ(zT θ )ˆ( dzT{C}⇒{i}

=− Ti 1

{i-1} {A} {B} {C} {i}

),( 11 −− iitrans axT),( 11 −− iiR xT α ),( iiR zT θ ),( iitrans dzT=Ti),( 11 −− iix aScrew α ),( iiz dScrew θ

Note

),( 11 −− iix aScrew α

1−ia1−iα

combination of a translation along an axis x by a distance and a rotation about the same axis by an angle

)()()()( αα axTxTxTaxT =Screw ),(),(),(),(),(),(),(),(

θθαα

zTdzTdzTzTaxTxTxTaxT

RtranstransR

transRRtrans

==

⎤⎡⎤⎡⎤⎡⎤⎡⎤⎡ 00100010010010001

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

10000000

001

100000000001

100001000010

001

100001000010

001

100000000001

αααα

αααα

αααα

cssc

a

cssc

aa

cssc

⎦⎣⎦⎣⎦⎣⎦⎣⎦⎣ 10001000100010001000

22

인접한 두 링크간의 상대 위치 및 방위

⎥⎥⎤

⎢⎢⎡

−−− −0

1111

1

iiiiiii

iii

dsscccsascαααθαθ

θθ

i 1

인접한 두 링크간의 상대 위치 및 방위

Function of four link parameters

⎥⎥⎥

⎦⎢⎢⎢

⎣

=−−−−

−−−−

10001111

1111

iiiiiii

iiiiiii

dccscss αααθαθTii1− Function of four link parameters

( ) onlyiiii da θα ,,, 11 −−

: 머니퓰레이터의 설계인자 known1−iα1−ia

두 링크간 상대 운동량인 (회전관절경우)를 알면 를 완성iθ Tii1−

두 링크간의 상대 위치 및 방위를 온전히 알 수 있다

id (직동관절경우)

즉 는 관절변수 ( 또는 ) 만의 함수임을 알 수 있다Tii1−

iqiθ id

)(11 ii qTT −− = )( iii qTT=

만 알면 구성 가능iq Tii1−

23

Forward Kinematics

• N-링크 머니퓰레이터 말단부의 위치 및 방위를 결정하는 정기구학

– ⎥⎦

⎤⎢⎣

⎡== −

1)()()()( 1

3232

121

01

0

OrR

qTqTqTqTT NN

NN L

– 두 링크간 상대 운동량인 (회전 관절 경우) 또는 (직동 관절 경우)를알면 말단 장치의 머니퓰레이터 기저좌표계 {0} 에 대한 상대 위치 벡터r 및 방위를 나타내는 회전행렬 R를 직교 좌표계의 값으로 온전히 파악

iθ id

r 및 방위를 나타내는 회전행렬 R를 직교 좌표계의 값으로 온전히 파악할 수 있게 된다

α a d, , 로봇설계parameter (all known)α a d, 봇설계p e e ( ow )

θ 현재 로봇의 kinematic 상태Sensor (encorder) 에 의해 획득

if all ’s are known by sensors, Cartesian position and orientation of the last link (end-effector) can be computed by

θTN

0

24

( ) p y N

Transformation between frames using Screw’s

TTTT ii

ii

ii

ii 1

121

21 +

−−−

−+ =

),( 22 −− iix aScrew α ),( 11 −− iiz dScrew θ ),( 11 −− iix aScrew α ),( iiz dScrew θ ),( iix aScrew α ),( 11 ++ iiz dScrew θ

{ }2−i { }1−i { }i { }1+i

25

예제3. Determine the forward kinematics for the 3-axis manipulator in Fig 3.

i From To along

0α0z 1z 0x0

d ˆ ˆ ˆ

0a0z 1z 0

0z 1z 0x

0x 1x1θ

1d 0x

1z

1x 1z

D-H LinkLinkparameters

1. link 0, 1, 2, 3 결정2. 모든 joint를 찾아내서 로 설정 (1부터 numbering )3. 과 의 공통법선 (link 1의 길이방향)이 과 만나는 점을 frame 1의 원점으로

iz1z 2z

26

setting4. 공통법선을 으로 잡고 right-hand rule을 이용하면 자동 setting5. repeat till i=3

1x 1y

⎥⎥⎥⎤

⎢⎢⎢⎡

−−−

= −−−−

−0

1111

1111

1

iiiiiii

iiiiiii

iii

dccscssdsscccs

asc

αααθαθαααθαθ

θθ

Using D-H table and orTii1−

⎥⎦

⎤⎢⎣

⎡=−

11

OrR

Tii

⎥⎥⎤

⎢⎢⎡ −

=0000

11

11

0 θθθθ

cssc

T ⎥⎥⎤

⎢⎢⎡ −

=00

0

22

122

1 θθθθ

csLsc

T ⎥⎥⎤

⎢⎢⎡ −

=00

0

33

233

2 θθθθ

csLsc

T

⎥⎥

⎦⎢⎢

⎣−−−−

10001111 iiiiiii dccscss αααθαθ ⎦⎣ 1O

27⎥⎥⎥

⎦⎢⎢⎢

⎣

=

100001001T

⎥⎥⎥

⎦⎢⎢⎢

⎣

=

100001002T

⎥⎥⎥

⎦⎢⎢⎢

⎣

=

100001003T

특정한 관절 변수 값( )에 의해 얻어지는 머니퓰레이터 말단의321 ,, θθθ

위치와 방위를 나타내는 정 기구학

⎥⎤

⎢⎡ −⎥⎤

⎢⎡ −

⎥⎤

⎢⎡ −

000

000

0000 23312211

θθθθ

θθθθ

θθθθ LscLscsc

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

==

1000010000

1000010000

1000010000 3322112

312

01

03

θθθθθθ cscscsTTTT

⎦⎣⎦⎣⎦⎣

⎥⎥⎤

⎢⎢⎡

++−

00 12211123123

slslcsclclsc

)sin(),cos(sin,cos

21122112

1111

θθθθθθ

+=+===

scsc

⎥⎥⎥

⎦⎢⎢⎢

⎣

+=

10000100

0 12211123123 slslcs

)sin()cos(

321123

321123

θθθθθθ

++=++=

sc

)(),( 21122112

세 번째 링크에 부착된 좌표계 {3}의 원점의 위치와 방위를 나타냄세 번째 링크 끝점의 위치와 방위를 구하려면?

28

• 세 번째 링크 끝점의 위치와 방위를 구하려면?

⎤⎡ 001 l– 세 번째 링크 끝점에 좌표계 {4} 설정

• 좌표계 {3}과 동일한방위

• x3방향으로 l3만큼 병진 ⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

=01000010

001 3

34

l

T* 좌표계 {4}의 좌표축 설정은 중요하지않다x3방향 l3만큼 병진

• 세 번째 링크 끝점의 위치와 방위를 나타내는 정기구학

⎥⎦

⎢⎣ 1000

T0• 세 번째 링크 끝점의 위치와 방위를 나타내는 정기구학

= 34

23

12

01

04 TTTTT

T4

⎥⎥⎥⎤

⎢⎢⎢⎡

⎥⎥⎥⎤

⎢⎢⎢⎡ −

⎥⎥⎥⎤

⎢⎢⎢⎡ −

⎥⎥⎥⎤

⎢⎢⎢⎡ −

=0010

00100

000

00000 3

33

233

22

122

11

11 lcs

Lsccs

Lsccssc

θθθθ

θθθθ

θθθθ

⎥⎥

⎦⎢⎢

⎣⎥⎥

⎦⎢⎢

⎣⎥⎥

⎦⎢⎢

⎣⎥⎥

⎦⎢⎢

⎣ 10000100

10000100

10000100

10000100

29

3.4 Inverse Kinematics– The problem of determining the position and orientation of the end-effector for given

joint angles of the manipulator.– 작업자가 원하는 말단장치의 목표 위치와 방위를 만들어 낼 수 있는 관절변수 값 iq작업자가 원하는 말단장치의 목 위치와 방위를 만들어 낼 수 있는 관절변수 값

( 또는 )를 찾아내는 문제– 원하는 자세와 위치를 만들어 내기 위해 머니퓰레이터의 각 관절을 어떻게 제어

해야 할 것인가에 대한 답을 찾는 과정

iqiθ id

– 특정 관절변수 값에 의해서 만들어지는 머니퓰레이터 말단장치의 위치와 방위를결정하는 정 기구학의 반대 개념

역 기구학:

주어진 목표 위치 및 방위 를 구현하는

관절변수 값 을 찾는 문제

⎥⎦

⎤⎢⎣

⎡=

× 131

0

OrR

TN

Nqqq ,,, 21 L

역 기구학

관절변수 값 을 찾는 문제Nqqq ,,, 21

Solving 12 equations to find for giveniθ ⎥⎦

⎤⎢⎣

⎡1

&,,O

rRda α

⎥⎥⎤

⎢⎢⎡

−−−

= −−−−

−0

1111

1

iiiiiii

iii

dsscccsascαααθαθ

θθ

Ti 1−

⎦⎣

⎥⎤

⎢⎡

=rR

⎥⎥⎥⎤

⎢⎢⎢⎡

= 232221

131211

yrrrxrrr

30

⎥⎥⎥

⎦⎢⎢⎢

⎣

=−−−−

10001111 iiiiiii dccscss αααθαθ

Ti ⎥⎦

⎢⎣ 1O ⎥

⎥

⎦⎢⎢

⎣ 1000333231 zrrr

given

3.4.1 Inverse Kinematics for the desired position(목표 위치 구현을 위한 역기구학)

역기구학의 해법관절변수 를 찾기 위해 12개의 비선형 연립방정식 풀이 필요qqq

(목표 위치 구현을 위한 역기구학)

– 관절변수 를 찾기 위해 12개의 비선형 연립방정식 풀이 필요

TqTqTqTqTO

rRNN

NN

013

232

121

01 )()()()(

1==⎥

⎦

⎤⎢⎣

⎡ −L

Nqqq ,,, 21 L

– 일반적으로 방위행렬 R의 9개의 방정식 중 3개만이 독립적이므로 위치정보를 나타내는 3개의 방정식과 더불어 구성되는 6개의 방정식을 6개의 미지수에 대해 풀면 된다

O 1⎦⎣

는 3개의 방정식과 더불어 구성되는 6개의 방정식을 6개의 미지수에 대해 풀면 된다• 대단히 복잡한 비선형 함수들로 구성

대단히 복잡하고 어려운 해수치해석(n merical sol tion)을 고려가능

TN0

수치해석(numerical solution)을 고려가능실시간 적용 어려움머니퓰레이터의 구조나 형상에 따라 의 구성 역시 전혀 다른 형태모든 머니퓰레이터에 공통으로 적용될 수 있는 역기구학의 일반적이고

TN0

모든 머니퓰레이터에 공통으로 적용될 수 있는 역기구학의 일반적이고유일한 해법은 없다

31

일반적 해법

– 행렬구성요소들의 함수 형태를 최대한 이용하여 구하고자 하는 관절변수TN0

를 고립시켜 구한 후 순차적으로 나머지 관절변수(i= i+1)들의 값을 구한다

– 간단한 머니퓰레이터 경우 머니퓰레이터의 기하학적 특성을 이용하여 관절변수

N

iq

간단한 머니퓰레이터 경우 머니퓰레이터의 기하학적 특성을 이용하여 관절변수값을 쉽게 구할 수도 있다

32

예제 3.2간단한 2 링크 평면 머니퓰레이터의 말단 장치를 (x y)에 위치시키고 방위 φ를 만– 간단한 2-링크 평면 머니퓰레이터의 말단 장치를 (x,y)에 위치시키고 방위 φ를 만들어 낼 수 있는 역 기구학을 풀어보자. 즉 관절변수 q ( 과 )를 찾아보자1θ 2θ

Inverse kinematics equation

{= 2

312

01

03 TTTT

given

Obtained byforward kinematics orR & r information

⎥⎥⎥⎤

⎢⎢⎢⎡

++−

=⎥⎥⎥⎤

⎢⎢⎢⎡

010000

122111212

122111212

232221

131211

slslcsclclsc

zrrryrrrxrrr2θ

⎥⎥

⎦⎢⎢

⎣⎥⎥

⎦⎢⎢

⎣ 10000100

1000333231 zrrr

given444 3444 21

33

)sin(),cos(,sin,cos 211221121111 θθθθθθ +=+=== scsc

rij : 방위각 에 해당되는 좌표계 {3}의 주축들의방향 코사인 성분

φ

• 역기구학의 위치정보를 이용하면 12211 clclx += 12211 slsly +=

- Method I

양변을 제곱하여 더하면 22122

2112112121

22

21

22 2)(2 cllllssccllllyx ++=+++=+

2222 )( llyx ++2cosθ 에 대해 정리하면

21

212 2

)(cosll

llyx +−+=θ

관계를 이용하면 22

2 cos1sin θθ −±=1cossin 22

22 =+ θθ

)cos,cos1(2atan)cos,(sin2atan 222

222 θθθθθ −±==

Found ! 2θ

2sinθ 의 양, 음 부호에 따라

값도 두 개가 존재θ 값도 두 개가 존재2θ

역 기구학 해의 부호에 따라 가능한 두 가지 형상34

Matlab commandEx)

0 5 1/ t(2) l1 0 5 l2 0 5x = 0.5; y= 1/sqrt(2); l1=0.5; l2 = 0.5;pp= ( x^2 + y^2 - (l1^2 + l2^2))/(2*l1*l2);a_cos = acos(pp)* 180/pi

i i 1 1 ia_sin = asin(sqrt(1-pp^2)) * 180/pith2 = atan2(sqrt(1-pp^2), pp) * 180/pi

>> a cos =_

60.0000a_sin =

60.0000th2 =

60.0000

35

atan vs. atan21

• )/(tan)/(atan 12 xyxy −==θ

),(atan22 xy=θ

2/2/ 2 πθπ <≤− 한 개의 인수 사용

두 개의 인수 사용

예) 머니퓰레이터 링크의 끝점 위치 (1,1)과 (-1,-1)의 방위

( ) o451atan11atan

11atan2 ==⎟

⎠⎞

⎜⎝⎛ −=⎟

⎠⎞

⎜⎝⎛=θ Diff t !1 ( )

112⎠⎝ −⎠⎝

( ) ( ) oo 1351,1atan2,451,1atan22 −=−−==θDifferent !1

-11

-1

전 구간에서 정의전체 공간에서 머니퓰레이터의 관절변수 값을 정의

역기구학 atan2

머니퓰레이터의 관절변수 값에 제한머니퓰레이터의 관절변수 값에 제한

36

?1θ

만의 함수( now known)1221221212121112211

1221221212121112211

)()()()()()(cslscllcssclslslslysslccllsscclclclclx

++=++=+=−+=−+=+=

2θ1θQ

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡+−+

yx

sc

cllslslcll

1

1

22122

22221

행렬식으로 정리하면

⎦⎣⎦⎣⎦⎣ + yscllsl 122122

By Cramer’s rule

222

2221

2212212

222

221

222211 )()(

)(,)()(

)(slcll

ycllxslsslcll

yslxcllc++++−

=++++

=

)(θ θ 의 부호에 따라 도 달라짐θ),(2atan 111 cs=θ 2θ 의 부호에 따라 도 달라짐1θ

- 정기구학과 달리 역 기구학의 해는 유일하지 않을 수 있다!- 수학적으로 해가 얻어졌다 하더라도 현실적으로 머니퓰레이터가 만들어 낼

Comment on inverse kinematics

수학적 해가 얻어졌다 하더라 현실적 머니퓰레이터가 만들어 낼수 있는 자세가 아닐 수도 있어(관절운동 범위 제한으로 인해) 역 기구학의 해가 없을 수도 있다.

37

Method II• 링크의 끝점과 머니퓰레이터 기저를 연결한 삼각형의 기하하적 관계

)(22222 θllll )cos(2 22122

21

22 θπ −−+=+ llllyx

에 대해 정리하면2θ

21

22

21

22

2 2)(cos

llllyx +−+

=θ

22

2 cos1sin θθ −±= 이므로

)cos,cos1(2atan 222

2 θθθ −±=

2sinθ 의 양, 음 부호에 따라Note. 2 의 양 음 부 에 따라

값도 두 개가 존재2θ

38

각 에 대해 코사인 법칙을 적용하면β

( ) 221

2221

222 cos2 llyxlyx =+−++ β

( )⎥⎥⎦

⎤

⎢⎢⎣

⎡

+

−++= −

22

22

21

2221

2cos

yxlllyxβ

β 에 대해 정리하면

⎥⎦⎢⎣ +12 yxl

는 이미 부호가 고려된 두 가지 자세 각각에 대해 따로 따로 계산하므로 굳이 2가지 부호까지 동시에 고려하는 atan2 이용하여 구할 필요 없다

β

βαθ ±=1 ⎭⎬⎫

⎩⎨⎧

>−<+

0:0: 2

θθ

then

시에 고려하는 atan2 이용하여 구할 필요 없다

각 계산할때 cos, sin 단독⎭⎩ > 0: 2θ

),(atan2 xy=α여기서

각 계산할때 cos, sin 단독사용하지 않고 atan함수 쓰는 이유

39

예제 3.3– Solve the forward kinematics for the RR-type manipulator

{= 2

312

01

03 TTTTi 3z

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

+−−

−

=

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

0 22122

21212121

21212121

333231

232221

131211

sllcscslcsscscclssccc

zrrryrrrxrrr

given

(3.32)2z

3

⎥⎦

⎢⎣

⎥⎦

⎢⎣ 10001000

22122333231

given444 3444 21

위치정보로부터212

cslccl

yx= Function of only1θ212 csly y1

),(2atan1 xy=θhence Found !1θ

?2θ

식 (3.32)에서 만의 함수로 구성된 직접적인 식은 찾을 수가 없다.2θ(3.32) 양변에 만의 함수인 의 역행렬을 곱하면1θ T0

1

40

( )

⎥⎤

⎢⎡ −

⎥⎤

⎢⎡

=− )(

21212121131211

232

12

03

101

cclsscccxrrrTTTT θ

( ) ( )

⎤⎡⎤⎡

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

+−−

=

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

⇒−−

0

10000

100022122

21212121101

333231

232221101

l

sllcscslcsscs

Tzrrryrrr

T

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

−

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−+−+−+−+−++++

⇒

10000

01000

10002222

2222

1333231

11123131122121121111

11123113122112121111

slcs

clsc

lzrrrycxscrrscrrscrrs

ysxcsrcrsrcrsrcr

⎦⎣⎦⎣ 10001000

Function of only2θ

2211 clysxc =+

From the position information

1θ( x, y, z, l1, l2, : all known)221 sllz =−

),(2atan 1112 ysxclz +−=θ

1( , y, , 1, 2, )

역기구학의 유용한 해법특정 관절변수만으로 구성된 관계식을 유도하기 위해서 해당 관절변수를Note.포함하는 동차 변환행렬의 역행렬을 곱하는 방법 유용

Note.

41

3.4.2 Inverse kinematics for the desired orientation implementation

목표 방위 구현을 위한 역기구학– 임의의 위치에 임의의 방위를 갖도록 머니퓰레이터를 제어하기 위한 최소 자유도 : 6

처음 세 축: 위치 결정 나머지 세 축: 방위 결정– 처음 세 축: 위치 결정, 나머지 세 축: 방위 결정

– 일반적으로 방위결정을 위한 마지막 3축은 관절축이 한 점에서 교차하도록 구성

6자유도 머니퓰레이터 말단장치의 위치와 방위 표현식

5432100

),,(),,(

)()()()()()(

65436321

03

6565

454

343

232

121

01

06

θθθθθθ

θθθθθθ

TT

TTTTTTT

=

=

말단장치 위치를 결정하는 처음세 관절의 변수 의 함수

말단장치의 방위를 결정짓는 마지막세 관절의 관절변수 의 함수321 ,, θθθ 654 ,, θθθ

목표위치 구현을 위한 역기구학

How to find ?321 ,, θθθ How to find ?654 ,, θθθ

목표방위 구현을 위한 역기구학Done in 3.4.1.

42

목표방위 구현을 위한 역기구학?

마지막 세관절의 관절변수 값을 구하는 역 기구학마지막 세관절의 관절변수 값을 구하는 역 기구학

( ):

:0

103

T

T −목표 위치 역기구학으로부터

주어진 목표위치 방위 값),,(),,(

)()()()()()(

65436321

03

6565

454

343

232

121

01

06

θθθθθθ

θθθθθθ

TT

TTTTTTT

=

=

( ) TTT 06

103654

36 ),,( −

=θθθ

:06T 주어진 목표위치, 방위 값

Both known, hence can be solved for 654 ,, θθθ

방위에만 관련되므로 세 번의 연속회전으로 구현하고자 하는특정방위를 나타내는 회전행렬

( ) RRRRRR 06

103

56

45

34654

36 ),,( −

==θθθ

세 번의 연속 회전오일러 각도법과 롤-피치-요

특정방위를 나타내 회전행렬

세 번의 연속 회전일러 각 법과 롤 피치

각도법에 의해 구현 가능

특정방위를 구현해 내는 관절각 의 값을 구하는 역 기구학 문제654 ,, θθθ

결론

오일러 각도법과 롤-피치-요 각도법 적용

43

예제 3.4– 작업자가 원하는 머니퓰레이터 말단의 방위가 다음과 같이 주어져있을 때 이 목

표 방위를 구현하기 위해 필요한 손목 관절의 회전량을 Z-Y-Z 오일러 각도법을이용하여 구하라. 즉 목표방위 구현을 위한 역기구학을 풀어라.

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

333231

232221

13121136

rrrrrrrrr

R목표 방위⎥⎦⎢⎣ 333231 rrr

목표 방위 구현을 위한 Z-Y-Z 오일러 각도법

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

=36

10000

0010

0

10000

)()()(

cssc

cs

sccssc

RRRR ZYZ

γγγγ

ββ

ββαααα

γβα

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−+−+−−−

=

⎥⎦⎢⎣⎥⎦⎢⎣−⎥⎦⎢⎣ 1000100

csscsssccscsscccssccssccssccc

cs

βγβγββαγαγβαγαγβαβαγαγβαγαγβα

ββ

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎦⎢⎣

333231

232221

131211

rrrrrrrrr

βγβγβ

44

방위 역 기구학의 해는 위치 역 기구학에서와 마찬가지로 주어진 행렬 요소들의 형태를최대한 이용하여 풀어낼 수 있다

–최대한 이용하여 풀어낼 수 있다

β

?β만약 행렬의 를 이용하여 20≠βsA. 만약 ,행렬의 (3,3) 요소를 이용하여 ),1(2tan 33

233 rra −±=β

의 부호에 따라β

)(2tan)(2tan)1(2tan 2 rrarrarra === γαβ 경우인

),(2tan),,(2tan),1(2tan),(2tan),,(2tan),1(2tan

31321323332

33

313213233333

rrarrarrarrarrarra

−=−−=−−=

−==−=

γαβγαβ

경우인

경우인

B. 만약 ( )0=βs

⎤⎡⎤⎡ +−+⎤⎡ −−− 00)()(0 1211 rrsccsscsscc γαγαγαγαγαγα0=βB.1

πβ ,0=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡++++

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+−+

10000

1000)()(0)()(

10000

2221

1211

rrrr

cssc

ccsssccscsscsscc

γαγαγαγα

γαγαγαγαγαγαγαγα

회전행렬

)(2tan)(2tan rrarra+γα ),(2tan),(2tan 11121121 rrarra −==+γα

무수히 많은 해가 존재

1 eq. for 2 variables

⎪

⎪⎨

⎧==

00

αβ

무수히 많은 해가 존재0=α 가정

⎪⎩ −== ),(2tan),(2tan 11121121 rrarraγ

45

B.2 πβ =⎤⎡⎤⎡⎤⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−−−−−−

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−++−−−−

10000

1000)cos()sin(0)sin()cos(

10000

2221

1211

rrrr

ccsssccscsscsscc

γαγαγαγα

γαγαγαγαγαγαγαγα

회전행렬

),(2tan),(2tan 11121121 rrarra −−=−=−γα

0=α 가정

⎪⎩

⎪⎨

⎧==

)(2t)(2t0απβ

⎪⎩ −−−=−−= ),(2tan),(2tan 11121121 rrarraγ

46

예제 3.5예제 3 4과 동일한 목표 방위를 X Y Z 롤 피치 요 방법을 이용하여 구현하고자– 예제 3.4과 동일한 목표 방위를 X-Y-Z 롤-피치-요 방법을 이용하여 구현하고자할 때 필요한 롤-피치-요 각 를 구하시오

방법 I) 롤-피치-요 방법을 이용한 역기구학 식

γβα ,,

방법 I) 롤-피치-요 방법을 이용한 역기구학 식

⎥⎤

⎢⎡

⎥⎤

⎢⎡ +−

=

131211

36 ),(),(),(

rrrsscsccssscccxRotyRotzRotR

γαγβαγαγβαβαγβα

⎥⎥⎥

⎦⎢⎢⎢

⎣

=⎥⎥⎥

⎦⎢⎢⎢

⎣ −−+=

333231

232221

rrrrrr

ccscssccssccssscs

γβγββγαγβαγαγβαβα (3.51)

0≠βcA. 만약

( )ββα

β

crcr

rrr

//2atan

,2atan 221

21131

=

⎟⎠⎞⎜

⎝⎛ +±= m

( )( )ββγ

ββαcrcrcrcr

/,/2atan/,/2atan

3332

1121

==

의 경우 부호에 따라 두 개의 해를 가질 수 있으나 머니퓰레이터의 제β 의 경우 부호에 따라 두 개의 해를 가질 수 있으나 머니퓰레이터의 제어를 위해 둘 중 하나를 해로 지정하여야 한다. 이 때 해가 구간에 존재하도록 선택하는 것이 일반적이다

β

47

B 만약 ( )0=βc o90±=βB. 만약 ( )0βc 90±β

B.1. o90=β⎥⎤

⎢⎡

++−

00 sscccssc

ααααγαγαγαγα

⎥⎥⎤

⎢⎢⎡

=⎥⎥⎤

⎢⎢⎡

−−−−

=

⎥⎥⎥

⎦⎢⎢⎢

⎣−−+

00

)sin()cos(0)cos()sin(0

0010

2322

1312

rrrr

sccsccss

γαγαγαγα

γαγαγαγα회전행렬

⎥⎥⎦⎢

⎢⎣−⎥

⎥⎦⎢

⎢⎣− 001001

)()( 2322γγ

),(2tan))cos(),(sin(2tan 2212 rraa =−−=− γαγαγα

0=α 가정

무수히 많은 해가 존재

( )⎪⎩

⎪⎨

⎧

==

°=

2tan00.90

rraγα

β

( )⎩ = 2212 ,2tan rraγ

B.2. o90−=β⎧

90β

( )⎪⎩

⎪⎨

⎧

−==

°−=

2212 ,2tan0.0

90

rraγαβ

같은 방법으로

⎩

48

방법 II) 특정 관절변수를 포함하는 동차 변환행렬의 역행렬을 곱하는 방법

양변에 만의 함수인 의 역행렬을 곱하면α ),( αzRot⎥⎥⎤

⎢⎢⎡

=⎥⎥⎤

⎢⎢⎡

−++−

=

=

131211

36 ),(),(),(

rrrrrr

sccssccssscssscsccsssccc

xRotyRotzRotR

γαγβαγαγβαβαγαγβαγαγβαβα

γβα

⎥⎤

⎢⎡ 131211

11

rrr

⎥⎥⎦⎢

⎢⎣

=⎥⎥⎦⎢

⎢⎣ −

+=

333231

232221

rrrrrr

ccscssccssccssscs

γβγββγαγβαγαγβαβα

⎥⎥⎥

⎦⎢⎢⎢

⎣

= −−

333231

23222111 ),(),(),(),(),(

rrrrrrzRotxRotyRotzRotzRot αγβαα

⎥⎥⎤

⎢⎢⎡

⎥⎥⎤

⎢⎢⎡ −

=131211

0cossin0sincos

)()( rrrrrr

xRotyRot αααα

γβ⎥⎥⎦⎢

⎢⎣⎥⎥⎦⎢

⎢⎣

=

333231

232221

1000cossin),(),(

rrrrrrxRotyRot ααγβ

⎥⎥⎤

⎢⎢⎡

−−−+++

=⎥⎥⎤

⎢⎢⎡

− 132312221121

231322122111

0 srcrsrcrsrcrsrcrsrcrsrcr

sccsssc

αααααααααααα

γγγβγββ

⎥⎥⎦⎢

⎢⎣⎥

⎥⎦⎢

⎢⎣− 333231

132312221121

rrrccscs γβγββγγ

우변행렬의 (2 1)요소인 를 이용하면0=− αα srcr우변행렬의 (2,1)요소인 를 이용하면01121 =αα srcr

( )1121,2tan rra=α

( )ααβ srcrra 211131 ,2tan +−= Q행렬의 (1,1) 요소(3 1)

βcrs −=β( )β 211131

( )ααααγ srcrsrcra 12221323 ,2tan −+−=

(3,1) 31rs =β

((2,2)와 (2,3) 이용)

49

예제 3.6예제 3 4와 동일한 방위를 Z Y X 오일러 방법을 이용하여 구현하고자– 예제 3.4와 동일한 방위를 Z-Y-X 오일러 방법을 이용하여 구현하고자할 때 필요한 오일러 각 를 구하시오γβα ,,

Z-Y-X 오일러 방법에 의한 회전행렬은 X-Y-Z 롤-피치-요와 동일하므로역 기구학의 해는 예제 3.5의 해와 동일하다역 기구학의 해는 예제 3.5의 해와 동일하다

50

3.4.2 Inverse kinematics for the desired orientation implementation

Equivalent angle-axis representation of an orientation

- Rotation about an arbitrary axis k

θ

1222 =++ kkk1=k- let k = [kx, ky, kz], where k is a unit vector ( or )

- rotation about a vector k by

y

1++ zyx kkk1k[ x, y, z], ( )

k : axis of rotation: angle of rotation about kθ

1where

kz

z

),(),(),(),(),()( αβθβαθ −−= zRyRzRyRzRRk

θθ cv −=1

θβk

),(),(),(),(),()( ββ yyk

⎥⎥⎤

⎢⎢⎡ +−+ θθθθθθ

kkkkkkkskvkkskvkkcvk yzxzyxx

2

2

αkxky

⎥⎥

⎦⎢⎢

⎣ ++−−++=

θθθθθθ

θθθθθθ

cvkskvkkskvkkskvkkcvkskvkk

zxzyyzx

zzyyzyx2

2

51

x

y Rotation matrix obtained by rotation w.r.t. arbitrary axis

Equivalent angle-axis representation of an orientation

Any orientation that is represented as rotational matrix as follows

also can be achieved by a single rotation with respect to a suitable axis K in spaceby a suitable angleIn other words, by selecting a suitable k & , we can obtain

θ)(θKRR =θ

- then how to select suitable k & ? inverse kinematicsθ- then how to select suitable k & ? inverse kinematicsθ

52

Inverse kinematics for equivalent angle-axis representationAny orientation that is represented as rotational matrix as follows

also can be achieved by a single rotation with respect to a suitable axis k in space by ay g p p ysuitable angle such thatθ

: the axis of rotation (arbitrary axis)

: angle of rotation about k

Note. is characterized by only three parameters( , but one constraint )zyx KKK ,, θ 1=K

)(θKR

53

Remember rotation w r t arbitrary axis kProof)

),(),(),(),(),()( αβθβαθ −−= zRyRzRyRzRRkk

z

Remember rotation w.r.t. arbitrary axis kProof)

⎥⎥⎥⎤

⎢⎢⎢⎡

−+++−+

= θθθθθθ

θθθθθθ

kkkkkkkskvkkcvkskvkkskvkkskvkkcvk

zzyyzyx

yzxzyxx

2

2

2

θβk

kz

⎥⎦

⎢⎣ ++− θθθθθθ cvkskvkkskvkk zxzyyzx

2

whereα

k

kxky

θθ cv −=1⎥⎥⎤

⎢⎢⎡

232221

131211

yrrrxrrr

x

xy

θθ

222

⎥⎥⎥

⎦⎢⎢⎢

⎣

=

1000333231 zrrr

θθθθθθ

θθθθθθ

cckkkkkk

cckcckcck

cvkcvkcvkrrr

zyx

zyx

3)(

)1()1()1(222222

222

222332211

+++−++=

+−++−++−=

+++++=++

θskrr x22332 =− xks

rr=

−

θ22332

θ

θθ

θθ

ccc

cckkkkkk zyxzyx

2131

3)(

+=+−=

+++++=Similarly for ky & kz

54

⎟⎠

⎞⎜⎝

⎛ −++= −

21cos 3322111 rrr

θ

Ex) z축을 중심으로 α 만큼 회전하여 얻어지는 방위와 동일한 방위를 구현할 수 있Ex) z축을 중심으로 α 만큼 회전하여 얻어지는 방위와 동일한 방위를 구현할 수 있는 k 및 θ 를 구하고, 이것이 목표방위와 일치하는지 확인하시오.

• z축을 중심으로 θ 만큼 회전하여 얻어지는 방위• z축을 중심으로 θ 만큼 회전하여 얻어지는 방위

⎥⎤

⎢⎡ − 0sincos αα

A

⎥⎥⎥

⎦⎢⎢⎢

⎣

==1000cossin ααRA

B

= =

α

• 즉, k를 중심으로 a 만큼 회전하게 되면, 위와 동일한 회전행렬이 됨을 알 수 있다.

3.5 Velocity kinematics

Velocity Kinematics: Jacobian

– 운동중인 머니퓰레이터 말단부의 속도(linear/angular velocity)와 머니퓰레이터관절의 변화율(joint rate) 사이의 관계관절의 변화율(j ) 사이의 관계

선속도: 위치의 시간 변화율각속도: 방위(각위치)의 시간 변화율각속도: 방위(각위치)의 시간 변화율

)( θθθfclclx =+= ),,( 321112211 θθθfclclx =+=),,( 321212211 θθθfslsly =+=

),,( 3213321 θθθθθθφ f=++=말단부의 위치 x, y 와 방위

56

말단부의 위치 x,y와 방위

),,( 321112211 θθθfclclx =+=),,( 321212211 θθθfslsly =+=

)( θθθθθθφ f++

말단부의 위치 x,y와 방위

)f(Θu =⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

φyx

u⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡= 2

1

fff

fwhere

),,( 3213321 θθθθθθφ f=++= ⎥⎦⎢⎣φ ⎥⎦⎢⎣ 3f

관절변수 를 갖는 일반화된 6-자유도 머니퓰레이터로 확장하면),,1( niqi L=

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

65432122

65432111

qqqqqqfuqqqqqqfu

=⇒==

qfu

),,,,,(

)(),,,,,(

65432166

65432122

qqqqqqfu

qqqqqqfu

=

⇒M

qfu

⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

=⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

=4

3

2

1

uuuu

zyx

xφu

⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

=4

3

2

1

ffff

f⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

=4

3

2

1

qqqq

qwhere

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣ 6

5

4

uu

z

y

x

φ

φφ

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣ 6

5

4

fff

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣ 6

5

4

qqq

x, y, z: 머니퓰레이터 말단의 위치 좌표y: 특정 방위를 나타내는 오일러 회전각zyx φφφ ,,

57

u: 직교좌표계 값으로 표시된 절대량q: 각 링크간의 상대 변위인 상대량

)(qfu = 을 chain rule을 이용하여 시간에 대해 미분하면

222

66

12

2

11

1

16

6

12

2

11

1

111

fff

qqfq

qfq

qf

tq

qf

tq

qf

tq

qf

dtduu

&&&&

&L&&L&

∂∂∂∂∂

++∂∂

+∂∂

=∂∂

∂∂

++∂∂

∂∂

+∂∂

∂∂

==

66

26

16

6

66

22

2

21

1

22

qfqfqfu

qqfq

qfq

qfu

&L&&&

M

L

∂∂

++∂∂

+∂∂

=

∂++

∂+

∂=

66

22

11

6 qq

qq

qq ∂∂∂

벡터-행렬 형태로 나타내면

&

&

L

L

&

&

&

⎥⎥⎥⎤

⎢⎢⎢⎡

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

=⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

2

1

222

6

1

2

1

1

1

3

2

1

qq

qf

qf

qf

qf

qf

qf

vvv

uuu

z

y

x

J( && )

{

{q

J(qu&

&

M

4444 34444 21

L

MOMM

&

&

&

&⎥⎥

⎦⎢⎢

⎣⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢

⎣ ∂∂

∂∂

∂∂

∂∂∂

⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢

⎣⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢

⎣6

6

6

2

6

1

6

621

6

5

4

)

qqf

qf

qf

qqq

uuu

z

y

x

ωωω qJ(qu& )=

자코비안(Jacobian)J(qu )

: 위치 좌표 x, y, z의 시간 변화율인 선속도: 각 위치(angular position )의 시간 변화율인 각속도

zyx vvv ,,zyx φφφ ,,zyx ωωω ,,

58

Forward velocity kinematics (정방향 속도 기구학)Forward velocity kinematics (정방향 속도 기구학)

•• 자코비안 :관절변수들의 변화율 를 머니퓰레이터 말단부의 선속도 와 각

속도 로 변환시키는 역할q& v

qJ(qu && )=

속도 로 변환시키는 역할

• 머니퓰레이터 각 관절의 변화율 조합의 결과로 만들어지는 머니퓰레이터 말단부의 선속도 및 각속도를 얻을 수 있게한다

ω

Inverse velocity kinematics (역방향 속도 기구학)

•• 임의의 작업을 수행하기 위해 직교 좌표성분으로 주어진 머니퓰레이터 말단

uJ(qq && 1)−=• 임의의 작업을 수행하기 위해 직교 좌표성분으로 주어진 머니퓰레이터 말단

의 목표 선속도 및 각속도를 구현하기 위해 필요한 머니퓰레이터 관절변수들의 변화율 벡터를 찾아내는 문제

인간이 인지하기 쉬운 직교 좌표성분으로 표현된 목표인간이 인지하기 쉬운 직교 좌표성분으로 표현된 목표속도성분을 머니퓰레이터가 구현하기 위해 필요한 관절간의 상대 회전속도 성분으로 변화

59

특이점(singularities) 또는 특이형상 (singularity configuration)특이점(singularities) 또는 특이형상 (singularity configuration)

– 특정 형상(관절 각)에서 자코비안의 행렬식(determinant)이 영이 되어

q&

0≠J

uJ(q

JuJ(qq &&&))() 1 adj

== −를 만족하는 이 존재하지 않는 점

– 관절속도의 어떠한 조합으로도 만들어낼 수 없는 머니퓰레이터 말단부의 특정방향의 속도성분이 존재

0≠J

향의 속도성분이 존재

– 작업에 필요한 자유로운 운동을 구현해 내는데 어려움

– 특이점 근처에서는 머니퓰레이터 말단부의 작은 속도차도 관절속도에 큰 변화를가져와 작업환경과 작업자에게 큰 위험요소로 작용가져와 작업환경과 작업자에게 큰 위험요소로 작용

특이점을 미리 찾아내고 이를 회피하도록 작업을 설계하는 것은 대단히 중요– 특이점을 미리 찾아내고 이를 회피하도록 작업을 설계하는 것은 대단히 중요

0≈J q&If , then would be very large

60

예제 3.7– 평면 2-링크 머니퓰레이터(그림 3.5)의 특이점을 찾아보자

12211 clclx += 12211 slsly +=

⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡+

−−−=⎥

⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡

2

1

12212211

12212211

θθ&

&

&

&

clclclslslsl

vv

yx

y

x qJ(qu && )=

특이점

0221 == sllJ L,,02 πθ ±=

1. 두 번째 링크가 첫 번째 링크와 겹쳐진 ( ) 형상2. 두 링크가 나란히 일직선으로 서있을 경우 ( ) 02 =θ

πθ ±=2

두 경우 모두 두 번째 링크는 반경방향으로는 움직이지 못하고링크가 회전하는 원호의 접선 방향으로만 움직일 수 있음

Note.평면에서의 완전한 운동 표현

2DOF 필요But @ singular point, 1-DOF

drops

특이점에서는 운동의 자유도(Degree of freedom)가 줄어듬

61

회전 좌표계를 이용한 상대 운동 해석N

– 완전한 형태의 자코비안• 선속도에 관한 자코비안 +각속도에 관련된 자코비안

Note.그림 3.9는 와 사이 관계를 알수 있는 가 존재하는 예외적으로 간단한 경우임

ω q&qJ(qu && )=

• 선속도에 관한 자코비안 : 위치변수 r (즉 x,y,z)와 관절변수 사이의 관계식인위치 기구학 식만 있으면 미분으로 획득 가능

• 각속도에 관련된 자코비안 :방위와 관련된 행렬의 성 들을 미분하여 얻 방법을 려– 방위와 관련된 R행렬의 구성요소들을 미분하여 얻는 방법을 고려

– 그러나 동차 변환행렬의 회전행렬 R을 구성하는 어떤 방향 코사인 벡터도 미분하면 각속도 ( )가 직접 얻어지지 않는다즉 미분을 통해 각속도 와 관절변수 속도 를 직접 관계시켜 줄 수

ω zyx ωωω ,,&– 즉, 미분을 통해 각속도 와 관절변수 속도 를 직접 관계시켜 줄 수

있는 유용한 회전기구학 식이 존재하지 않는다– 따라서, 기구학의 직접 미분이 아닌 다른 방법으로 구해야 한다

ω q

회전좌표계 기반의 상대운동해석을 이용한완전한 형태의 자코비안 유도

62

{A}: 머니퓰레이터 기저에 부착된 고정 기준계{A} 머니퓰레이터 기저에 부착된 고정 기준계{B}: 임의의 축을 기준으로 각속도 로 회전하는 i 번째 링크에 부착되어 링크와 함께 회전하는 회전 좌표계{C}: 링크 i 내의 한 점 C에 부착된 좌표계

ω

O를 중심으로 한 C’

미소 시간 Δt동안 발생한 벡터 의 변위 Δr회전중심 O

링크 i 의 회전으로 C C’으로 이동 시

CB r

CB

CB

CB

CB

rttr

trrr

×Δ=Δ=

Δ==Δ

ωωφ

ωφθφ

sin

))(sin())(sin(

N t h tB

회전좌표계 {B} 내에 정의된 벡터 의 고정기준계{A}에서 본 시간에 대한 변화율 = 의 선속도

CB r

CB r

Note. why not ???trCB Δ×ω

= 점 C의 선속도

CBC

B

tt

CB

rt

rttr

dtrd

×=Δ×Δ

=Δ

=ΔΔ

ωω00

limΔlim (3.76)ttA ttdt ΔΔ →Δ→Δ 00

}{(3.76)

So far {B}: 회전만 하는 좌표

그림 3.10 즉 회전하는 좌표계 내의 한 점(벡터)의 절대 속도

So far, {B}: 회전만 하는 좌표

63

From now on, 회전 좌표계 {B}의 원점이 공간상에서 자유롭게 움직일(이동할) 수 있다고 가정정

좌표계 {B}는 회전하면서 동시에 병진하는 좌표계

CB

BA

CA rrr +=

고정 좌표계 {A}에서 본 각 항의 시간에 대한 변화율BAA +고정 좌표계 {A}에서 본 각 항의 시간에 대한 변화율CB

BA

CA rrr +=

}{}{}{ A

CB

A

BA

A

CA

dtrd

dtrd

dtrd

+=}{}{}{ AAA

CA v B

A v

회전 및 병진하는 좌표계 {B} 내에정의된 벡터 의 좌표계 {A}의 원점에서 본 절대속도

CB r

고정 좌표계 {A}에서 본 링크 i상의 한 점 C와 좌표계 {B}의 원점의 절대속도

64

kzjyixr BBBCB ++= i, j, k : 좌표계 {B}의 단위벡터

BBB zyx ,,C

B r로 정의된다면: 의 각 방향 성분 크기BBB y

C

( )

⎟⎞

⎜⎛

⎟⎞

⎜⎛

++=

kdjdidkdzjdyidx

kzjyixdtd

dtrd

BBB

ABBB

A

CB

}{}{

의 각 방향 성분 크기

⎟⎠⎞

⎜⎝⎛ +++⎟

⎠⎞

⎜⎝⎛ ++= k

dtzj

dtyi

dtxk

dtj

dtyi

dt BBBBBB

(3.79)

???

의 좌표계 {B}에 대한 시간 변화율을 좌표계 {B}의 좌표성분 i,j,k로 표현한 것BBB zyx ,,

= 좌표계 {B}의 원점에서 본 C점의 속도

???

좌 계 { }의 에서 의 속

}{B

CB

dtrd

= 0: 링크의 길이가 일정한 회전 관절(즉 C점이 고정돼있는 경우)의 경우관절 길이의 변화속도: 직동관절의 경우(즉 C점이 움직이는 경우)

65

고정 기준계 {A}에서 본 회전 및 병진하는 좌표계 {B}내에 정의된 벡터 의 시간에 대한 변화율C

B r CB

B

CB

A

CB

rdt

rddt

rd×+= ω

}{}{

회전하는 좌표계 내에 정의된 어떠한 벡터에도 동일하게 확장적용가능BB )()( dd

BA }{}{

CB

B

CB

A

CB

rdt

rddt

rd×+= ω

}{}{

•×+•

=• ω

}{}{

)()(

BA dtd

dtd

회전 운동하는 좌표계에 대해 정의된 임의의 벡터 의 고정)(•회전 운동하는 좌표계에 대해 정의된 임의의 벡터 의 고정기준 좌표계에서 본 시간 변화율

)(•

종합하면

A

CB

A

BA

A

CA

dtrd

dtrd

dtrd

+=}{}{}{

CB

CB

BA

CA rvvv ×++= ω

CB

B

CB

A

BA

rdt

rddt

rd×++= ω

}{}{

CCBC rvvv ×++ ω

BABAAA

좌표계 {A}와 같은 방위를갖는 좌표 성분으로 변환

CBA

BCBA

BBA

CA rRvRvv ×++= ω

66

CBA

BCBA

BBA

CA rRvRvv ×++= ω

– : 고정 좌표계에서 본 링크 i상의 한 점 C의 절대속도

– : 고정 좌표계에서 본 {B} 원점의 절대속도C

A v

BA v }{A

CA

dtrd

BA

dtrd

}{Adt

}{B

CB

dtrd

: {B}의 원점에서 본 링크상의 C의 상대속도

:{B}의 회전에 의해 발생하는 성분

CB v

BAA rRr ×× ωω :{B}의 회전에 의해 발생하는 성분CBC rRr ×=× ωω

CBA

BCBA

BBA

CA PRvRvv ×++= ω 의 또 다른 유도 방법

CB

BA

CA rrr +=

BAAA

를 미리 동일 좌표 성분으로 변환 후 적용•×+•

=• ω

}{}{

)()(

BA dtd

dtd

•• )()( dd적용

CBA

BBA

CA rRrr +=

회전좌표계 {B}에 대해 정의된 벡터 아님

•×+•• ω)()( dd

적용 불가!

•×+= ω}{}{

)()(

BA dtdt 적용

67

•×+= ω}{}{ BA dtdt 적용 불가!

고정 좌표계에서 본 링크 i상의 한 점 C의 절대가속도

•×+•

=• ω

}{}{

)()(

BA dtd

dtd

CBA

BCBA

BBA

CA rRvRvv ×++= ω 에 를 적용하면

( )BABABABABABAAA ( )

( )CBA

BCBA

BCBA

BCBA

BBA

CBA

BCBA

BCBA

BCBA

BCBA

BCBA

BBA

CA

rRrRvRvRv

rRvRrRrRvRvRvv

××+×+×++=

××+×+××+×+×++=

ωωωω

ωωωωωωω

&&&

&&&&

2

)(

( )CBCBCBCBB

코리올리(Coriolis) 가속도: 좌표계가 회전하기 때문에 생겨나는 가속도 성분

68

속도 전파식을 이용한 자코비안의 완성

머니퓰레이터의 속도, 가속도?그림 3.11 {i}, {i+1} 그림 3.10 {B}, {C} 대응 관계 이용

Definition링크 i의 선속도: 좌표계 {i} 원점의 선속도링크 i 의 각속도: 좌표계 {i}의 회전속도그림 3 11

선속도:질점의 속성각속도: 물체(강체)의 속성

링크 i 의 각속도: 좌표계 {i}의 회전속도그림 3.11

A. i +1번째 관절이 직동 관절인 경우

각속도

ii ωω =+1 iii

iii R ωω 1

11 +

++ =

B. i +1번째 관절이 회전 관절인 경우

111 ˆ +++ += iiii zθωω &1

11

11

1 ˆ ++

++

++ += i

iii

iiii

i zR θωω &

69

앞 링크에 대한상대 회전속도 추가

1+iθ&

1ˆ +iz: 각속도의 크기

:각속도의 방향

선속도 좌표계 {i} 원점의 속도 에 i+1번째 관절에 의해 추가되는 속도를 더하여 구한다iv

)( 11

11

++

++ ×+= i

ii

ii

iiii

i rvRv ω

11

111

11 ˆ)( +

+++

++

+ +×+= ii

iii

ii

iii

iii zdrvRv &ω

회전 관절

직동 관절

각가속도 및 선가속도 •×+•

=• ω

}{}{

)()(

BA dtd

dtd

각속도, 선속도에 적용하여 구한다

iv주의) 의 미분에는 적용X

( ) ( )11

11

11

11

11 ˆˆ +

++

++

++

++

+ ×++= ii

iiii

iii

iiii

iii zRzR θωθωω &&&&&

회전 관절

)()( 111

111

11

+++

+++

++ ××+×+= i

ii

iiii

iiii

ii

iiii

iiii

i rRRrRvRv ωωω&&&

직동 관절직동 관절

iii

iii R ωω && 1

11 +

++ =

( ))()(ˆ2ˆ 1111111 +++++++ iiiiiiiiiiiii RRZdZdR &&&&&& ( ))()(2 11

11

11

111

11

11

11

++

++

++

+++

++

++

++ ××+×+×++= i

ii

ii

iiii

ii

iiii

iii

ii

iii

iiii

i rRrRZdZdvRv ωωωω

70

Detailed derivationDetailed derivation

)(11 ++ iiiii R회전 관절

)( 11

11

++

++ ×+= i

ii

ii

iiii

i rvRv ω

)())(( 1111

1111

11

++++

++++

++ ×+×+××+×+= i

ii

ii

iiii

iiii

ii

ii

ii

ii

iiii

iiii

i rrRRrrRvRv ωωωωω &&&&

)()( 111

111

11

+++

+++

++ ××+×+= i

ii

iiii

iiii

ii

iiii

iiii

i rRRrRvRv ωωω&&&

111 ˆ)( +++ +×+ iiiiii zdrvRv &ω

직동 관절

d&1111 )( ++++ +×+= iiiiiii zdrvRv ω

( ) ( )111

1111

11 )()( ++

+++

+++

+ ××+×+××+×+= ii

ii

ii

ii

iii

iii

ii

ii

ii

iii

iiii

iii rrRrrRvRv &&&& ωωωωωω

1+id

( )

11

111

11

1ˆˆ+

+++

++

++ ×++ i

iii

ii

ii ZdZd &&& ω

71

( ))()(ˆ2ˆ1

11

11

111

11

11

11

1+

++

++

+++

++

++

++

+ ××+×+×++= ii

ii

iii

iii

iii

iii

iii

ii

iiii

iii rRrRZdZdvRv ωωωω &&&&&&

velocity propagation (속도 전파식)

특정 링크의 속도 및 각속도는 머니퓰레이터의 형태상 앞쪽의 링크들의 속도에– 특정 링크의 속도 및 각속도는 머니퓰레이터의 형태상 앞쪽의 링크들의 속도에종속

– 마지막 링크의 속도와 각속도를 구하기 위해서는 머니퓰레이터 기저의 속도부터바깥쪽의 링크의 속도 및 각속도를 차례로 구해야 함바깥쪽의 링크의 속도 및 각속도를 차례로 구해야 함

– 즉 속도 및 각속도 식의 i i+1

속도 전파식(velocity propagation)속도 전파식(velocity propagation)

완전한 형태의 자코비안 구성

72

예제 3.8&& 0

– 관절회전율 이 주어졌을 때 머니퓰레이터 말단의 속도 를 속도 전파식을 이용하여 구하고 이를 이용하여 자코비안을 구하라

21,θθ && 30v

i d θi

1 0 0 0

2 0 0

1−ia 1−iα id iθ

1θ T01

1l 2θ T123 0 0 0

2

2l T23

⎥⎥⎤

⎢⎢⎡ −

0000

11

11

θθθθ

cssc

⎥⎥⎤

⎢⎢⎡ −

000 122

θθθθ

csLsc

⎥⎥⎤

⎢⎢⎡

0010001 2L

T01 T1 T2

3= = =

⎥⎥⎥

⎦⎢⎢⎢

⎣ 10000100

11

⎥⎥⎥

⎦⎢⎢⎢

⎣ 100001000022 θθ cs

⎥⎥⎥

⎦⎢⎢⎢

⎣ 10000100

1 T2T3= =

Fixed manipulator base 00ω = 0

0v = 0

73

속도 전파식 적용

i = 0 ⎤⎡ 0 ⎤⎡0 iii R11 ++직동 관절

i 0

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

1

11 0

0

θω

& ⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

000

11v

2122 &

11

11

11 ˆ +

++

++

+ += ii

iiii

iii zR θωω &

iii

iii R ωω 1

11 +

++ =직동 관절

회전 관절

i = 1

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡+

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=

+=

22

22

22

2112

122

00

00

10000

θθθθθθ

θωω

&&

&

cssc

ZR)( 1

11

1+

++

+ ×+= ii

ii

iii

iii rvRv ω

11

111

11 ˆ)( +

+++

++

+ +×+= ii

iii

ii

iii

iii zdrvRv &ω

회전 관절

직동 관절

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+=

⎥⎦⎢⎣⎥⎦⎢⎣⎥⎦⎢⎣ 21

00

100

θθ

θθ

&& ⎥⎦⎢⎣ + 21 θθ

⎥⎤

⎢⎡ 1L

⎪

⎪⎬

⎫

⎪

⎪⎨

⎧+⎥

⎥⎤

⎢⎢⎡

⎥⎥⎤

⎢⎢⎡−=

×+=

0000

00

)(

122

22

21

11

112

122

θθθθθ

ω

&kji

cssc

rvRv

⎥⎥⎥

⎦⎢⎢⎢

⎣

=002

1r

⎥⎥⎤

⎢⎢⎡

=

⎪⎭

⎪⎩ ⎥

⎥⎦⎢

⎢⎣⎥

⎥⎦⎢

⎢⎣ 000100

121

121

1

θθθθ&

&

cLsL

L이므로

⎥⎥

⎦⎢⎢

⎣ 0121

74

i = 2 += 33

3223

233 θωω & ZR

0

i 2

⎤⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

21

00

100010001

θθ &&

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+=

21

00

θθ &&

⎪⎫

⎪⎧

⎥⎤

⎢⎡

⎥⎤

⎢⎡

×+=

001

)(

121

32

22

223

233

θθ

ω

& kjisL

rvRv

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

00

2

32

Lr 이므로

⎥⎤

⎢⎡

⎪⎭

⎪⎬

⎪⎩

⎪⎨ ++

⎥⎥⎥

⎦⎢⎢⎢

⎣⎥⎥⎥

⎦⎢⎢⎢

⎣

=00

000100

010

121

2

21121

θθ

θθθθ

&

&&&

sL

LcL

⎥⎦⎢⎣ 0

⎥⎥⎥

⎦⎢⎢⎢

⎣

++=0

)( 212111 θθθθ &&& LcL

02133 ⎤⎡⎤⎡ slv

Jacobian with respect to {3}

qqJu &&&

&)(

0000

0

331

2212

21

3,3

3,3

3,3

=⇒⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

+

=⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

θθ

ω

llclsl

vvv

z

y

x

75

110000 2

,33

,33

,3 ⎦⎣

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

θ

ωωω

z

y

x

좌표계 {3}에 대해 표현된 말단의 속도를 기준좌표인 {0} 좌표성분으로 변환하면

⎥⎥⎥⎤

⎢⎢⎢⎡

++⎥⎥⎥⎤

⎢⎢⎢⎡ −

=

==

)(00

212121

121

1212

1212

332

312

013

3033

0

θθθθθθ

&&&

&

LcLsL

cssc

vRRRvRv

⎥⎥⎥⎤

⎢⎢⎢⎡

⎥⎥⎥⎤

⎢⎢⎢⎡ −

=

=

1212

1212

330

330

00

00

ωω

&&cssc

R

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+++−−

=

⎥⎦⎢⎣⎥⎦⎢⎣

0)()(

0100

21122111

21122111

θθθθθθθθ&&&

&&&

cLcLsLsL

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+=

⎥⎦⎢⎣ +⎥⎦⎢⎣

21

21

00

100

θθ

θθ

&&⎥⎦⎢⎣ 0 ⎦⎣ 21

과 을 벡터-행렬 형태로 묶어서 정리하면30 v 3

0ω

0 ⎤⎡⎤⎡ LLL

qJ(qu &&&

&)

0000 0

2

1

12212211

12212211

30

30

30

30

=⇔⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

+−−−

=⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

θθ

ω

cLcLcLsLsLsL

vvv

z

y

x

1100

2

30

30

3 ⎦⎣

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

θ

ωωω

z

y

x

회전 좌표계에 대한 상대운동 해석을 이용하여 유도된 속도 전파식을 통해 선속도와 각속도를관절변수의 변화율과 직접 관계시키는 자코비안을 구할 수 있다.

76

자코비안의 구성

선속도와 각속도 성분을 모두 갖는 6행으로 구성된 행렬: 선속도와 각속도 성분을 모두 갖는 6행으로 구성된 행렬

: 선속도를 나타내는 상위 3행만으로도 자코비안을 구성하기도 함

77

Review on DynamicsReview on Dynamics

16.8 회전축을 이용한 상대운동해석(R l i M i A l i U i R i A )(Relative-Motion Analysis Using Rotating Axes)

78

16.8 회전축을 이용한 상대운동해석(R l i M i A l i U i R i A )(Relative-Motion Analysis Using Rotating Axes)

So far, 병진 기준계를 이용한 상대운동 해석으로 병진과 회전으로 분해하여 해석: 평면의 일반적 운동의 해석

, 병진 기준계를 이용한 상대운동 해석 병진과 회전 분해하여 해석

강체내의 두 점 사이의 상대운동 또는 핀으로 연결된 여러 강체 상의 점들의

해석에 유용

Both givesthe same result!

강체에 대해 상대운동(e.g.sliding motion)을 하는 질점의 해석: 병진과 회전을 겸하

는 강체에 고정된 기준계 (즉, 운동기준계, moving frame) 을 이용 more general기준계 OXYZ : 고정기준계 (fixed frame)

result!

ij 점 B: moving frame 에 대해 상대 운동하는 질점 또는Axyz

기준계 : 기준계 OXYZ에 대해 병진하면서 (각속도 와

각가속도 로) 회전하는 기준계(운동기준계

kΩ=ΩkΩ&& =Ω

Axyz

J

ij 점 B: moving frame 에 대해 상대 운동하는 질점 또는

moving frame 와 함께 운동하는 강체위에서 움직이는 질점

AxyzAxyz

ABAB /rrr += 고정기준좌표계에 부착된 OXY에서 본 점 B

I ABAB /rrr +=

jir BBAB yx +=/moving frame = local frame

고정기준좌표계에 부착된 OXY에서 본 점 B의 절대위치

회전, 병진 운동하는 강체에 부착된 Axy좌표

계에 대한 B점의 상대위치

기준계 의병진정보 : 원점 A의 속도( ) 와 가속도( )

회전정보 : 이 기준계의 각속도 와 각가속도kΩ=Ω kΩ&& =ΩAv Aa

Axyz

ABAB /vvv +=

ABAxyzABAB /// )( rvv ×+= Ωy

ABAxyzABAB // )( rvvv ×++=⇒ Ω

80

가속도

ABOXYZxyzABOXYZAOXYZBOXYZ dtd

dtd

dtd

dtd

// )( |||| rvvv ×++= Ω

dd ||

dd )()()( || vvv ×+= Ω

( ) ( ) ( )•×Ω+•=• AxyzOXYZ dtd

dtd ||Remembering

xyzABxyzABAxyzxyzABOXYZ dtdt)( )()( /// || vvv ×+= Ω

xyzAB )( /a=

ABOXYZABOXYZABOXYZ dtd

dtd

dtd

/// ||| ) ( rrr ×+×=× ΩΩΩ

d 0주의ΩΩΩ ×+= |Axyzdt

d 0ABxyzAB // )( rv ×+Ω기준계 Axyz의 각속도의 변화율

은 기준계에 무관하다

주의

Ω= &Ω

( ) ( ) ) ( )( ///// ABxyzABABxyzABxyzABAB rvrvaaa ×Ω×Ω+×Ω++×Ω+×Ω++=⇒ &

ABAB /aaa += AB/a

ABAB /aaa += …(16-27)

( )xyzABABABxyzABAB ///// 2) ( )( vrraa ×+××+×+= ΩΩΩΩ&

( )xyzABABABxyzABAB //// 2) ( )( vrraaa ×Ω+×Ω×Ω+×Ω++=⇒ &

: 고정기준계 (fixed frame) 에서 본 점 B의 가속도(점 B의 절대 가속도): 고정기준계에서 본 점 A 의 가속도(점 A의 절대 가속도): 고정기준계에서 본 점 B의 점 A에 대한 상대 가속도

BaAa

: 고정기준계에서 본 점 B의 점 A에 대한 상대 가속도

(고정기준계에 대한 상대 변위 rB/A의 2차 변화율): 운동기준계(moving frame) Axyz에서 본 점 B의 가속도

)( /a

AB /a

(상대변위 rB/A의 운동기준계에 대한 성분 xB , yB의

2차 변화율에 기인한 상대가속도 성분): 각가속도에 기인한 상대가속도 성분

xyzAB )( /a

AB /r×Ω&: 구심가속도 성분(Centripetal acceleration): 코리올리 가속도성분(Coriolis acceleration)

AB / r×Ω) ( / ABr×× ΩΩ

xyzAB )( 2 /v×Ω