intelligentrobotics-ch3 2011 student [호환...

Chapter 3머니퓰레이터 기구학(Manipulator Kinematics)

3 1 개요3.1 개요머니퓰레이터의 구성

링 관절링크 + 관절관절: 두 링크가 상대운동을 할 수 있게 함

회전 관절(revolute or rotary joint): 두 링크가 상대적인회전 관절(revolute or rotary joint): 두 링크가 상대적인회전운동을 하도록 연결직동 관절(prismatic joint) : 상대적인 직선운동을 하도록 체결관절의 표현관절의 표현

회전관절 :R직동관절: P직동관절:

지능형 로봇 공학(사이텍미디어)2

3 1 개요3.1 개요머니퓰레이터

목적에 따라 다수의 링 와 관절이 직렬 는 병렬 연결되어목적에 따라 다수의 링크와 관절이 직렬 또는 병렬로 연결되어다자유도의 운동을 구현

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

말단장치(end-effector)의 위치와 방위작업자에게 제일 중요한 고려사항작업자에게 제일 중요한 고려사항직교 좌표계 값으로 쉽게 인지 및 설정각 관절에 부착된 모터의 적절한 구동으로 구현각 관절에 부착된 모터의 적절한 구동으로 구현

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


3 1 개요3.1 개요머니퓰레이터 기구학직교 좌표로 표현된 말단장치의 위치 및 방위 값들과 이를직교 좌표로 표현된 말단장치의 위치 및 방위 값들과 이를실질적으로 구현하는 각 관절변수들 사이의 관계

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

역 기구학(i ki ti )역 기구학(inverse kinematics)말단장치의 목표 위치와 방위를 만들어 내기 위해 필요한관절변수 값의 조합을 찾아내는 것


3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자복잡한 형상과 다자유도 머니퓰레이터의 기구학

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

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

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

좌표계 설정방법Denavit-Hartenberg 규약 가장 대표적

5지능형 로봇 공학(사이텍미디어)

3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자Denavit-Hartenberg 규약 (좌표계 설정 방법 )

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

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

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

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

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


3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자링크 인자

링 자체의 형상과 인접 링 들과의 상대 관계를 나타냄링크 자체의 형상과 인접 링크들과의 상대 관계를 나타냄

링크 길이 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(회전량과 직선 이동거리를 나타낼 수 있으므로)


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 joint 3joint 1 joint 2 joint 3joint 1Revolutejoint d4Side View a2a1

link 2

link 3

d2

d3d3

joint 4Prismaticjoint

joint

link 1

0321 === αααd1 a0=0

j

Immobile baselink 0

321

Top View3θ

Top View

θ2θ

1θ

3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자Denavit-Hartenberg 규약을 이용한 좌표계와 링크 인자


3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자여러 가지 계 설정 가능성의 예

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


3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자좌표계 { i-1}을

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

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

1−iα

1−ia

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

두 링크 사이의 상대 관계

iθ

id

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

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

iiRiitransiitransiiRi

i zTdzTaxTxTT θα −−−−− =

)()()()( dzTzTxTaxT θα=동일한 축에 대한 변환(교환법칙 성립)),(),(),(),( 1111 iitransiiRiiRiitrans dzTzTxTaxT θα −−−−=

⎥⎥⎤

⎢⎢⎡

−−−

−−−−

−0

1111

1

iiiiiii

iii

dsscccsascαααθαθ

θθ

⎥⎤

⎢⎡

=rR

( 칙 )

R과 r :두 링크 i 1과 i 사이의 상대

지능형 로봇 공학(사이텍미디어)⎥⎥⎥

⎦⎢⎢⎢

⎣

=−−−−

10001111

1111

iiiiiii

iiiiiii

dccscss αααθαθ ⎥⎦

⎢⎣

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

회전량과 상대거리(변위)

13

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 θ

3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자인접한 두 링크간의 상대 위치 및 방위

⎥⎥⎥⎤

⎢⎢⎢⎡

−−−

= −−−−

−0

1111

1111

1

iiiiiii

iiiiiii

iii

dccscssdsscccs

asc

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

θθ

Tii1−

iiii da θα ,,, 11 −− 만의 함수⎥⎥

⎦⎢⎢

⎣−−−−

10001111 iiiiiii dccscss αααθαθ

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

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

idiθ Ti

i1−

(직동관절경우)

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

i (직동 절경우)

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

i1

iqiθ id

)(11i

ii

ii qTT −− =

지능형 로봇 공학(사이텍미디어)

)( iii q

15

만 알면 구성 가능iq Tii1−

3 3 정 기구학 (F d Ki ti )3.3. 정 기구학 (Forward Kinematics)N-링크 머니퓰레이터 말단부의위치 및 방위를 결정하는 정기구학위치 및 방위를 결정하는 정기구학

⎥⎤

⎢⎡

== − )()()()( 12100 rRqTqTqTqTT NL

두 링크간 상대 운동량인 (회전 관절 경우) 또는 (직동 관절경우)를 알면 말단 장치의 머니퓰레이터 기저좌표계 {0} 에

⎥⎦

⎢⎣

==1

)()()()( 332211 OqTqTqTqTT NNN

iθ id경우)를 알면 말단 장치의 머니퓰레이터 기저좌표계 {0} 에대한 상대 위치 벡터 r 및 방위를 나타내는 회전행렬 R를 직교좌표계의 값으로 온전히 파악할 수 있게 된다

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

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

if all ’s are known by sensors, Cartesian position and orientation of the θ


last link (end-effector) can be computed by TN0

3 3 정 기구학 (F d Ki ti )3.3. 정 기구학 (Forward Kinematics)예제3. 그림의 3축 머니퓰레이터의 정 기구학 식을 구하시오

D-H 링크 인자

⎥⎥⎥⎤

⎢⎢⎢⎡

−−−

= −−−−

−0

1111

1111

1

iiiiiii

iiiiiii

iii

dccscssdsscccs

asc


θθ

D-H table과 또는 이용하면Tii1−

⎥⎦

⎤⎢⎣

⎡=−

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 αααθαθi ⎦⎣ 1O


⎥⎥⎥

⎦⎢⎢⎢

⎣

=

100001001T

⎥⎥⎥

⎦⎢⎢⎢

⎣

=

100001002T

⎥⎥⎥

⎦⎢⎢⎢

⎣

=

100001003T

17

3 3 정 기구학 (F d Ki ti )3.3. 정 기구학 (Forward Kinematics)

특정한 관절 변수 값 에 의해 얻어지는 머니퓰레이터 말단의특정한 관절 변수 값( )에 의해 얻어지는 머니퓰레이터 말단의위치와 방위를 나타내는 정 기구학

321 ,, θθθ

⎤⎡⎤⎡⎤⎡ 0000 θθθθθθ LscLscsc

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡ −

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡ −

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡ −

==010000

0

010000

0

01000000

33

233

22

122

11

11

23

12

01

03

θθθθ

θθθθ

θθθθ

csLsc

csLsc

cssc

TTTT

⎥⎦

⎢⎣⎥⎦

⎢⎣

⎥⎦

⎢⎣ 100010001000

⎥⎤

⎢⎡ ++− 0 123312211123123 clclclsc sincos θθ == sc

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

++=

10000100

0 123312211123123

123312211123123

slslslcs

)sin()cos(

321123

321123

θθθθθθ

++=++=

sc

)sin(),cos(sin,cos

21122112

1111

θθθθθθ

+=+===

scsc

⎥⎦

⎢⎣ 1000

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

)sin( 321123 θθθ ++s


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

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

세 번째 링크 끝점에 좌표계 {4} 설정좌표계 {3}과 동일한방위 ⎥

⎥⎤

⎢⎢⎡

0010001 3

3

l

T* 좌표계 {4}의좌표축 설정은좌표계 {3}과 동일한방위

x3방향으로 l3만큼 병진

학

⎥⎥⎥

⎦⎢⎢⎢

⎣

=

10000100

34T

0

좌표축 설정은중요하지 않다

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

= 34

23

12

01

04 TTTTT

T04

⎥⎤

⎢⎡⎥⎤

⎢⎡ −⎥⎤

⎢⎡ −

⎥⎤

⎢⎡ − 0010000 323312211

43214

lLscLscsc θθθθθθ

⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

=

100001000010

1000010000

1000010000

1000010000 332211 cscscs θθθθθθ


⎦⎣⎦⎣⎦⎣⎦⎣ 1000100010001000

3 4 역 기구학 (I Ki ti )3.4. 역 기구학 (Inverse Kinematics)작업자가 원하는 말단장치의 목표 위치와 방위를 만들어 낼 수있는 관절변수 값 ( 또는 )를 찾아내는 문제q θ d있는 관절변수 값 ( 또는 )를 찾아내는 문제원하는 자세와 위치를 만들어 내기 위해 머니퓰레이터의 각관절을 어떻게 제어해야 할 것인가에 대한 답을 찾는 과정특정 관절변수 값에 의해서 만들어지 머니퓰레이터

iq iθ id

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

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

⎤⎢⎣

⎡=

× 131

0

OrR

TN

역 기구학:

관절변수 값 을 찾는 문제

⎦⎣

Nqqq ,,, 21 L

Solving 12 equations to find for givenθ ⎥⎤

⎢⎡

&rR

da α

⎥⎥⎥⎤

⎢⎢⎢⎡

−−−

= −−−−

−0

1111

1

iiiiiii

iii

dccscssdsscccs

asc


θθ

Tii1−

Solving 12 equations to find for giveniθ ⎥⎦

⎢⎣ 1

&,,O

da α

⎥⎦

⎤⎢⎣

⎡=

1OrR


⎥⎥

⎦⎢⎢

⎣−−−−

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

3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학역기구학의 해법

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

TqTqTqTqTrR N 01210 )()()()(⎥⎤

⎢⎡ −

Nqqq ,,, 21 L

일반적으로 방위행렬 R의 9개의 방정식 중 3개만이독립적이므로 위치정보를 나타내는 3개의 방정식과 더불어

TqTqTqTqTO NNN332211 )()()()(

1==⎥

⎦⎢⎣

L

독립적이므로 위치정보를 나타내는 3개의 방정식과 더불어구성되는 6개의 방정식을 6개의 미지수에 대해 풀면 된다

대단히 복잡한 비선형 함수들로 구성

대단히 복잡하고 어려운 해

TN0

대단히 복잡하고 어려운 해

수치해석(numerical solution)을 고려가능

실시간 적용 어려움실시간 적용 어려움

머니퓰레이터의 구조나 형상에 따라 의 구성 역시전혀 다른 형태

모든 머니퓰레이터에 공통으로 적용될 수 있는 역기구학의

TN0

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


3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학일반적 해법

행렬구성요소들의 함수 형태를 최대한 이용하여 구하고자하는 관절변수들 를 고립시켜 구한 후 순차적으로 나머지관절변수들의 값을 구한다

TN0

iq관절변수들의 값을 구한다

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


3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학예제 3.2

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

1θ 2θ

역 기구학 식

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


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

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

φ

3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학방법 I

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

양변을 제곱하여 더하면 22122

2112112121

22

21

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

cosθ 에 대해 정리하면22

21

22 )(cos llyx +−+=θ2cosθ 에 대해 정리하면

212 2

cosll

=θ

관계를 이용하면 22

2 cos1sin θθ −±=

)coscos1(2atan)cos(sin2atan 2 θθθθθ ±==

1cossin 22

22 =+ θθ

)cos,cos1(2atan)cos,(sin2atan 22222 θθθθθ −±==

Found ! 2θ

2sinθ 의 양, 음 부호에 따라값도 두 개가 존재2θ


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

24

Matlab commandE )Ex)

x = 0.5; y= 1/sqrt(2); l1=0.5; l2 = 0.5;pp= ( x^2 + y^2 - (l1^2 + l2^2))/(2*l1*l2);pp ( x 2 + y 2 (l1 2 + l2 2))/(2 l1 l2);a_cos = acos(pp)* 180/pia_sin = asin(sqrt(1-pp^2)) * 180/pith2 = atan2(sqrt(1-pp^2), pp) * 180/pi

>>>> aco =

60.0000asi =asi =

60.0000th2 =

60 0000


60.0000

3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학atan vs. atan2

)/()/( 1θ 한 개의 인수 사용)/(tan)/(atan 12 xyxy −==θ

),(atan22 xy=θ

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

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

두 개의 인수 사용

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

o4511atan

11atan2 =⎟

⎠⎞

⎜⎝⎛−−

=⎟⎠⎞

⎜⎝⎛=θ Different !1

1 ( ) ( ) oo 1351,1atan2,451,1atan22 −=−−==θ전 구간에서 정의전체 공간에서 머니퓰레이터의 관절변수 값을 정의

-11

역기구학 atan2역기구학 atan2


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

26

3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학?1θ

)()()( lllllll만의 함수

( now known)1221221212121112211

1221221212121112211

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

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

2θ1θQ

행렬식으로 정리하면

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡+−+

yx

sc

cllslslcll

1

1

22122

22221

행렬식으로 정리하면

222

2221

2212212

222

221

222211 )()(

)(,)()(

)(slcll

ycllxslsslcll

yslxcllc++++−

=++++

=

By Cramer’s rule

2222122221 )()()()(

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

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

Comment on inverse kinematics


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

27

3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학방법 II

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

)cos(2 22122

21

22 θπ −−+=+ llllyx

22

21

22

2 2)(cos

llllyx +−+

=θ

에 대해 정리하면2θ

212 ll

22

2 cos1sin θθ −±= 이므로

)cos,cos1(2atan 222

2 θθθ −±=

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

값도 두 개가 존재2θ


3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학각 에 대해 코사인 법칙을 적용하면β

( ) 221

2221

222 cos2 llyxlyx =+−++ β

( )⎥⎥⎦

⎤

⎢⎢⎣

⎡

+

−++= −

22

22

21

2221

2cos

yxlllyxβ

β 에 대해 정리하면

⎥⎦⎢⎣ +12 yxl

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

β

βαθ ±=1 ⎭⎬⎫

⎩⎨⎧

>−<+

0:0: 2

θθthen

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

⎭⎩ > 0: 2θ

),(atan2 xy=α여기서


3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학예제 3.3

림과 같은 RR형 머니퓰레이터의 역 기구학을 풀어 자그림과 같은 RR형 머니퓰레이터의 역 기구학을 풀어보자.

{= 2

312

01

03 TTTT

given 3z

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

+−−

−

=

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

0 22122

21212121

21212121

333231

232221

131211

sllcscslcsscscclssccc

zrrryrrrxrrr

(3.32)2z

⎥⎦

⎢⎣

⎥⎦

⎢⎣ 10001000

given444 3444 21

위치정보로부터212

212

cslccl

yx= Function of only1θ

),(2atan1 xy=θ따라서

?

Found !1θ

2θ ?2θ

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

1


( ) 양 에 의 수 의 역행렬을 곱하1 1

30

3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학( ) =

− )( 232

12

03

101 TTTT θ

( ) ( )⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

+−−

−

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⇒−−

10000

100022122

21212121

21212121

101

333231

232221

131211

101 sllcs

cslcsscscclssccc

Tzrrryrrrxrrr

T

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

−−

=

⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎡

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

⇒

⎦⎣⎦⎣

00100

0

10001000

2222

2222

1333231

11123131122121121111

11123113122112121111

slcs

clsc

lzrrrycxscrrscrrscrrs

ysxcsrcrsrcrsrcr

⎥⎥

⎦⎢⎢

⎣⎥⎥

⎦⎢⎢

⎣ 1000100022221333231

Function of only2θ위치정보로부터

221

2211

sllzclysxc

=−=+

),(2atan 1112 ysxclz +−=θ

1θ( x, y, z, l1, l2, : all known)

),(2atan 1112 ysxclz +θ

역기구학의 유용한 해법특정 관절변수만으로 구성된 관계식을 유도하기 위해서 해당 관절변수를Note.


포함하는 동차 변환행렬의 역행렬을 곱하는 방법 유용

31

3 4 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학목표 방위 구현을 위한 역기구학

임의의 위치에 임의의 방위를 갖 록 머니퓰레이터를 제어하기임의의 위치에 임의의 방위를 갖도록 머니퓰레이터를 제어하기위한 최소 자유도 : 6처음 세 축: 위치 결정, 나머지 세 축: 방위 결정일반적으로 방위결정을 위한 마지막 3축은 관절축이 한 점에서교차하도록 구성

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

),,(),,(

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

65436321

03

6565

454

343

232

121

01

06

θθθθθθ

θθθθθθ

TT

TTTTTTT

=

=

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

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

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


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

32

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

3 4 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학마지막 세관절의 관절변수 값을 구하는 역 기구학

( ) 0103 )( −θθθ

( ):

:06

103

T

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

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

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

65436321

03

6565

454

343

232

121

01

06

θθθθθθ

θθθθθθ

TT

TTTTTTT

=

=

( ) TTT 06

103654

36 ),,( =θθθ

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

방위에만 관련되므로

( ) 0105433 −

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

( ) RRRRRR 06

103

56

45

34654

36 ),,( ==θθθ

세 번의 연속 회전오일러 각도법과 롤-피치-요각도법에 의해 구현 가능

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

결론


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

33

3 4 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학예제 3.4

작업자가 원하는 머니퓰레이터 말단의 방위가 다음과 같이작업자가 원하는 머니퓰레이터 말단의 방위가 다음과 같이주어져있을 때 이 목표 방위를 구현하기 위해 필요한 손목관절의 회전량을 Z-Y-Z 오일러 각도법을 이용하여 구하라. 즉목표방위 구현을 위한 역기구학을 풀어라목표방위 구현을 위한 역기구학을 풀어라.

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

333231

232221

13121136

rrrrrrrrr

R목표 방위⎦⎣ 333231

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

⎤⎡⎤⎡⎤⎡

=36

000)()()(

scscscRRRR ZYZ

γγββααγβα

⎤⎡ −−−

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

10000

0010

0

10000

sccssccssccc

cssc

cs

sccssc

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

γγγγ

ββ

ββαααα

⎥⎤

⎢⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−+−+=

131211 rrr

csscsssccscsscccsβγβγββαγαγβαγαγβαβγγβγγβ

지능형 로봇 공학(사이텍미디어)⎥⎥⎥

⎦⎢⎢⎢

⎣

=

333231

232221

rrrrrr

34

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

0≠βs?β

A. 만약 ,행렬의 (3,3) 요소를 이용하여 ),1(2tan 332

33 rra −±=β의 부호에 따라β 의 부호에 따라β

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

31321323332

33

31321323332

33

rrarrarrarrarrarra

−=−−=−−=

−==−=

γαβγαβ

경우인

경우인

B. 만약 ( )0=βs

⎥⎥⎤

⎢⎢⎡

=⎥⎥⎤

⎢⎢⎡

+++−+

=⎥⎥⎤

⎢⎢⎡

++−−−

00

0)()(0)()(

00 1211

rrrr

cssc

ccsssccscsscsscc

γαγαγαγα

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

회전행렬

0=βB.1πβ ,0=

⎥⎥⎦⎢

⎢⎣

=⎥⎥⎦⎢

⎢⎣

++=⎥⎥⎦⎢

⎢⎣

+−+1000

1000)()(

1000 2221 rrcsccsssccs γαγαγαγαγαγα회전행렬

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

무수히 많은 해가 존재

1 eq. for 2 variables

0 가정⎪⎩

⎪⎨

⎧

−====

),(2tan),(2tan00

11121121 rrarraγαβ


0=α 가정 ⎩ ),(),( 11121121γ

35

3 4 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학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γ



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

γβα ,,

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

6 ),(),(),( xRotyRotzRotR γβα

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−++−

=

333231

232221

131211

rrrrrrrrr

ccscssccssccssscssscsccsssccc

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

(3.51)

0≠βcA. 만약

( )ββα

β

crcr

rrr

/,/2atan

,2atan

1121

221

21131

=

⎟⎠⎞⎜

⎝⎛ +±= m

( )( )ββγ

ββcrcr /,/2atan 3332

1121

=

의 경우 부호에 따라 두 개의 해를 가질 수 있으나 머니퓰레이터의제어를 위해 둘 중 하나를 해로 지정하여야 한다. 이 때 해가 구간에β


제어를 위해 둘 중 하나를 해 지정하여야 한다 이 때 해가 구간에존재하도록 선택하는 것이 일반적이다

37

3 4 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학B. 만약 ( )0=βc o90±=β

B.1. o90=β

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−++−

00100

sccsccsssscccsscγαγαγαγαγαγαγαγα

회전행렬

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−−−−

=

⎥⎦⎢⎣

00100

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

2322

1312

rrrr

γαγαγαγα

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

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

⎪⎧ °= 0.90β0=α 가정

( )⎪⎩

⎪⎨

⎧

==

2212 ,2tan0

rraγα

β

2B.2. o90−=β

( )⎪⎩

⎪⎨

⎧=

°−=

2t0.0

90αβ

같은 방법으로


( )⎪⎩ −= 2212 ,2tan rraγ

38

3 4 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학방법 II) 특정 관절변수를 포함하는 동차 변환행렬의 역행렬을 곱하는 방법

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

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−++−

=

=

333231

232221

131211

36 ),(),(),(

rrrrrrrrr

ccscssccssccssscssscsccsssccc

xRotyRotzRotR

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

γβα

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡= −−

333231

232221

13121111 ),(),(),(),(),(

rrrrrrrrr

zRotxRotyRotzRotzRot αγβαα

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

333231

232221

131211

1000cossin0sincos

),(),(rrrrrrrrr

xRotyRot αααα

γβ

⎤⎡⎤⎡ βββ

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−−−+++

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−

333231

132312221121

231322122111

0rrr

srcrsrcrsrcrsrcrsrcrsrcr

ccscssccsssc

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

γβγββγγγβγββ

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

( )1121,2tan rra=α

( ) Q행렬의 (1,1) 요소 βc


( )ααβ srcrra 211131 ,2tan +−=

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

Q행렬의 (1,1) 요소(3,1)

βc31rs −=β

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


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

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


3 5 속도 기구학3.5 속도 기구학자코비안(Velocity Kinematics: Jacobian)

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

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

),,( 321112211 θθθfclclx =+=),,( 321212211 θθθfslsly =+=말단부의 위치 x,y와 방위


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

41

3 5 속도 기구학3.5 속도 기구학

)( θθθfll말단부의 위치 x,y와 방위

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

),,( 3213321 θθθθθθφ f=++=)f(Θu =

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

φyx

u⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

3

2

1

fff

fwhere

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

)( qqqqqqfu =

)(

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

65432122

65432111

qqqqqqfu

qqqqqqfuqqqqqqfu

=

=⇒==

M

qfu

⎥⎥⎥⎤

⎢⎢⎢⎡

⎥⎥⎥⎤

⎢⎢⎢⎡

2

1

uu

yx

⎥⎥⎥⎤

⎢⎢⎢⎡

2

1

fff

⎥⎥⎥⎤

⎢⎢⎢⎡

2

1

qq

),,,,,( 65432166 qqqqqqfu =

⎥⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢⎢

⎣

=

⎥⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢⎢

⎣

=

6

5

4

3

uuuuz

z

y

x

φ

φφ

u

⎥⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢⎢

⎣

=

6

5

4

3

ffff

f

⎥⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢⎢

⎣

=

6

5

4

3

qqqq

qwhere

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


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

3 5 속도 기구학3.5 속도 기구학)(qfu = 을 chain rule을 이용하여 시간에 대해 미분하면

fffqfqfqfdu ∂∂∂∂∂∂∂∂∂

66

22

2

21

1

22

66

12

2

11

1

16

6

12

2

11

1

111

qqfq

qfq

qfu

qqfq

qfq

qf

tq

qf

tq

qf

tq

qf

dtduu

M

&L&&&

&L&&L&

∂∂

++∂∂

+∂∂

=

∂∂

++∂∂

+∂∂

=∂∂

∂∂

++∂∂

∂∂

+∂∂

∂∂

==

66

62

2

61

1

66 q

qfq

qfq

qfu &L&&&

M

∂∂

++∂∂

+∂∂

=

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

&

&

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 ωωω ,,


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

J( && )자코비안 :관절변수들의 변화율 를 머니퓰레이터말단부의 선속도 와 각속도 로 변환시키는 역할

q&v ω

qJ(qu )=

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

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

임의의 작업을 수행하기 위해 직교 좌표성분으로 주어진

uJ(qq && 1)−=임의의 작업을 수행하기 위해 직교 좌표성분으로 주어진머니퓰레이터 말단의 목표 선속도 및 각속도를 구현하기위해 필요한 머니퓰레이터 관절변수들의 변화율 벡터를찾아내는 문제찾아내는 문제

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


관절간의 상대 회전속도 성분으로 변화

44

3 5 속도 기구학3.5 속도 기구학특이점(singularities) 또는특이형상 ( i l it fi ti )특이형상 (singularity configuration)

특정 형상(관절 각)에서 자코비안의 행렬식(determinant)이영이 되어 를 만족하는 이 존재하지 않는 점uJ(qq && 1)−= q&

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

0≠J

말단부의 특정방향의 속도성분이 존재작업에 필요한 자유로운 운동을 구현해 내는데 어려움특이점 근처에서는 머니퓰레이터 말단부의 작은 속도차도관절속도에 큰 변화를 가져와 작업환경과 작업자에게 큰관절속도에 큰 변화를 가져와 작업환경과 작업자에게 큰위험요소로 작용특이점을 미리 찾아내고 이를 회피하도록 작업을 설계하는것은 대단히 중요것은 대단히 중요


3 5 속도 기구학3.5 속도 기구학예제 3.7

평면 2 링 머니퓰레이터( 림 3 5)의 특이점을 찾아 자평면 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

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


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

46

3 5 속도 기구학3.5 속도 기구학회전 좌표계를 이용한 상대 운동 해석

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

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

그러나 동차 변환행렬의 회전행렬 R을 구성하는 어떤 방향 코사인벡터도 미분하면 각속도 ( )가 직접 얻어지지 않는다ω zyx ωωω ,,

즉, 미분을 통해 각속도 와 관절변수 속도 를 직접 관계시켜 줄 수있는 유용한 회전기구학 식이 존재하지 않는다

따라서, 기구학의 직접 미분이 아닌 다른 방법으로 구해야 한다

ω q&

따라서, 기구학의 직접 미분이 아닌 다른 방법으로 구해야 한다

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


완전한 형태의 자코비안 유도

47

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

ω

O를 중심으로 한 C’

미소 시간 Δt동안 발생한 벡터 의 변위 Δr

CB

CB trrr Δ==Δ ωφθφ ))(sin())(sin(

회전중심 O

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

CB r

CB

CB

CC

rttr ×Δ=Δ= ωωφ

φφ

sin

))(())((

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

BBB rtrrd ×ΔωΔ

CB r

CB r

CBC

ttA

C rt

rttr

dtrd

×=Δ×Δ

=Δ

=→Δ→Δ

ωω00

}{

limΔlim

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

그림 3 10


, { } 회전만 하는 좌

48

그림 3.10

3 5 속도 기구학3.5 속도 기구학회전 좌표계 {B}의 원점이 공간상에서 자유롭게 움직일(이동할) 수 있다고 가정

좌표계 {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

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

B r

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


3 5 속도 기구학3.5 속도 기구학kzjyixr BBBC

B ++= i, j, k : 좌표계 {B}의 단위벡터: 의 각 방향 성분 크기BBB zyx ,,

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

Cr

( )

⎟⎞

⎜⎛

⎟⎞

⎜⎛

++=

kdjdidkdzjdyidx

kzjyixdtd

dtrd

BBB

ABBB

A

CB

}{}{

⎟⎠⎞

⎜⎝⎛ +++⎟

⎠⎞

⎜⎝⎛ ++= k

dtzj

dtyi

dtxk

dtj

dtyi

dt BBBBBB

(3.79)

CBr×ω

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

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

Cr×ω

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

}{B

CB

dtrd= 0: 링크의 길이가 일정한 회전 관절(즉 C점이 고정돼있는 경우)의 경우

관절 길이의 변화속도: 직동관절의 경우(즉 C점이 움직이는 경우)


???⎟⎞

⎜⎛ ++ kdzjdyidx

만약 {B}가 병진없이 회전만 하는 경우

???⎟⎠

⎜⎝

++ kdt

zjdt

yidt

x BBB

A. 식 (3.79)와 식(3.76)와 동일해야 함

( )++= kzjyixdtd

dtrd

ABBB

A

CB

}{}{

⎟⎠⎞

⎜⎝⎛ +++⎟

⎠⎞

⎜⎝⎛ ++= k

dtdzj

dtdyi

dtdxk

dtdzj

dtdyi

dtdx

dtdt

BBBBBB

AA }{}{

(3.79)

CBr×ω0

CBC

B

ttA

CB

rt

rttr

dtrd

×=Δ×Δ

=Δ

=→Δ→Δ

ωω00

}{

limΔlim (3.76)

B. ???,, kdtdj

dtdi

dtd


i, j, k의 방향의 변화

{B}가 각속도 로 회전함에 따라 생기는 항C

BBBB rk

dtdzj

dtdyi

dtdx ×=⎟

⎠⎞

⎜⎝⎛ ++ ω

ω


고정 기준계 {A}에서 본 회전 및 병진하는 좌표계 BCB

CB rdrd

×+

회전하 좌 계 내에 정의된 어떠한 벡터에 일하게 확장적 가

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

B r CB

B

C

A

C rdtdt

×+= ω}{}{

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

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 ×++= ω

52

CBA

BCBA

BBA

CA rRvRvv ×++= ω

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

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

CBCBBC

CA v

BA v

}{A

CA

dtrd

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

}{Adt

}{B

CB

dtrd

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

BAA}{

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

BCA rRr ×=× ωω

CBA

BCBA

BBA

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

CB

BA

CA rrr +=

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

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

=• ω

}{}{

)()(

BA dtd

dtd

CBA

BBA

CA rRrr +=

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

•×+•

=• ω

}{}{

)()(

BA dtd

dtd

적용


회전좌 계 { }에 대해 정의된 벡터 아님•×+

•=

• ω}{}{

)()(

BA dtd

dtd

적용 불가!

3 5 속도 기구학3.5 속도 기구학고정 좌표계에서 본 링크 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) 가속도: 좌표계가 회전하기 때문에 생겨나는 가속도 성분


3 5 속도 기구학3.5 속도 기구학속도 전파식을 이용한 자코비안의 완성

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

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

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

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

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

각속도

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 θωω &

55

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

1+iθ&

1ˆ +iz: 각속도의 크기

:각속도의 방향

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

)( 11

11

++

++ ×+= i

ii

ii

iiii

i rvRv ω

11

111

11 ˆ)( +

+++

++

+ +×+= ii

iii

ii

iii

iii zdrvRv &ω

회전 관절

직동 관절1111 )( ++++ ++ iiiiiii zdrvRv ω 직동 관절

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

=• ω

}{}{

)()(

BA dtd

dtd

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

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

( ) ( )11

11

11

11

11 ˆˆ +

++

++

++

++

+ ×++= ii

iiii

iii

iiii

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

회전 관절iv주의) 의 미분에는 적용X

)()( 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 ωωωω

56

3 5 속도 기구학3.5 속도 기구학속도 전파식(velocity propagation)

특정 링 의 속 및 각속 는 머니퓰레이터의 형태상 앞쪽의특정 링크의 속도 및 각속도는 머니퓰레이터의 형태상 앞쪽의링크들의 속도에 종속마지막 링크의 속도와 각속도를 구하기 위해서는 머니퓰레이터기저의 속 부터 바깥쪽의 링 의 속 및 각속 를 차례기저의 속도부터 바깥쪽의 링크의 속도 및 각속도를 차례로구해야 함즉 속도 및 각속도 식의 i i+1

속도 전파식(velocity propagation)

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


3 5 속도 기구학3.5 속도 기구학예제 3.8

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

21,θθ && 30v

i

1 0 0 01−ia 1−iα id iθ

1θ T01

2 0 0

3 0 0 0

1

1l 2θ T122l T2

3

⎥⎥⎤

⎢⎢⎡ −

0000

11

11

θθθθ

cssc

⎥⎥⎤

⎢⎢⎡ −

000 122

θθθθ

csLsc

⎥⎥⎤

⎢⎢⎡

0010001 2L

T01 T1 T2

3= = =⎥⎥⎥

⎦⎢⎢⎢

⎣ 10000100

11

⎥⎥⎥

⎦⎢⎢⎢

⎣ 100001000022 θθ cs

⎥⎥⎥

⎦⎢⎢⎢

⎣ 10000100

1 T2T3= =


머니퓰레이터 기저는 고정 00ω = 0

0v = 0

58

3 5 속도 기구학3.5 속도 기구학속도 전파식 적용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

59


i = 2⎤⎡⎤⎡

+= 33

3223

233 θωω & ZR

0

i 2

⎤⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

21

0

00

100010001

θθ &&

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+=

21

00

θθ &&

⎪⎬

⎫⎪⎨

⎧⎥⎤

⎢⎡

⎥⎤

⎢⎡

×+=

00010001

)(

121

32

22

223

233

θθθθθθ

ω

&&&

& kjiL

sL

rvRv

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

00

2

32

Lr 이므로

⎥⎤

⎢⎡

⎪⎭

⎪⎬

⎪⎩

⎪⎨ ++

⎥⎥⎥

⎦⎢⎢⎢

⎣⎥⎥⎥

⎦⎢⎢⎢

⎣

=00

000100

010

121

2

21121

θθ

θθθθ

&&&

&sL

LcL

⎦⎣

⎥⎥⎥

⎦⎢⎢⎢

⎣

++=0

)( 212111 θθθθ LcL

⎤⎡Jacobian with respect to {3}

qqJu &&&

&)(

0000

0

331

2212

21

3,3

3,3

3,3

3

=⇒⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

+

=⎥⎥⎥⎥⎥⎤

⎢⎢⎢⎢⎢⎡

θθ

llclsl

vvv

z

y

x


110000 2

,33

,33

,33 ⎥

⎦⎢⎣

⎥⎥⎥⎥⎥

⎦⎢⎢⎢⎢⎢

⎣⎥⎥⎥⎥

⎦⎢⎢⎢⎢

⎣

θ

ωωω

z

y

x


== 332

312

013

3033

0 vRRRvRv = 300 ωω R

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

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

++⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

0)(

10000

212121

121

1212

1212

3321333

θθθθθθ

&&&

&

LcLsL

cssc

vRRRvRv

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

=

21

1212

1212

333

00

10000

θθ

ωω

&&cssc

R

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+++−−

=0

)()(

21122111

21122111

θθθθθθθθ&&&

&&&

cLcLsLsL

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+=

21

00

θθ &&

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

0ω

12212211

12212211

30

30

⎥⎥⎤

⎢⎢⎡

+−−−

⎥⎥⎤

⎢⎢⎡

cLcLcLsLsLsL

vv x

qJ(qu &&&

&)

000000 0

2

1

12212211

30

30

30

3

=⇔⎥⎦

⎤⎢⎣

⎡

⎥⎥⎥⎥⎥⎥

⎢⎢⎢⎢⎢⎢ +

=

⎥⎥⎥⎥⎥⎥

⎢⎢⎢⎢⎢⎢

θθ

ωω

cLcLcLvv

y

x

z

y

1130 ⎥

⎥⎦⎢

⎢⎣⎥

⎥⎦⎢

⎢⎣ ω z

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


선속도와 각속도를 관절변수의 변화율과 직접 관계시키는 자코비안을 구할 수 있다.

61

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

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


intelligentrobotics-ch3 2011 student [호환...

Documents