intelligentrobotics-ch3 2011 student [호환...
TRANSCRIPT
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
3 1 개요3.1 개요머니퓰레이터 기구학직교 좌표로 표현된 말단장치의 위치 및 방위 값들과 이를직교 좌표로 표현된 말단장치의 위치 및 방위 값들과 이를실질적으로 구현하는 각 관절변수들 사이의 관계
정 기구학(forward kinematics)주어진 머니퓰레이터의 관절변수 값의 조합에 의해서만들어지는 머니퓰레이터 말단장치의 위치와 방위를만들어지는 머니퓰레이터 말단장치의 위치와 방위를구하여 직교좌표 값으로 표현하는 문제
역 기구학(i ki ti )역 기구학(inverse kinematics)말단장치의 목표 위치와 방위를 만들어 내기 위해 필요한관절변수 값의 조합을 찾아내는 것
지능형 로봇 공학(사이텍미디어)4
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를 설정한다.
지능형 로봇 공학(사이텍미디어)6
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(회전량과 직선 이동거리를 나타낼 수 있으므로)
지능형 로봇 공학(사이텍미디어)7
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
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 규약을 이용한 좌표계와 링크 인자
지능형 로봇 공학(사이텍미디어)11
3 2 머니퓰레이터 좌표계 설정 및 링크 인자3.2. 머니퓰레이터 좌표계 설정 및 링크 인자여러 가지 계 설정 가능성의 예
좌 계의 설정 유일하지만은 않다좌표계의 설정 유일하지만은 않다zi 와 zi+1가 서로 교차할 경우 좌표 xi의 설정은 두 축의 공통평면에 수직한 방향이므로 좌표계의 설정은 여러 가지로가능하다가능하다
지능형 로봇 공학(사이텍미디어)12
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 θ
지능형 로봇 공학(사이텍미디어)16
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
지능형 로봇 공학(사이텍미디어)18
세 번째 링크 끝점의 위치와 방위를 구하려면?
세 번째 링크 끝점의 위치와 방위를 구하려면?
세 번째 링크 끝점에 좌표계 {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 θθθθθθ
19지능형 로봇 공학(사이텍미디어)
⎦⎣⎦⎣⎦⎣⎦⎣ 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
지능형 로봇 공학(사이텍미디어)20
⎥⎥
⎦⎢⎢
⎣−−−−
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
모든 머니퓰레이터에 공통으로 적용될 수 있는 역기구학의일반적이고 유일한 해법은 없다
지능형 로봇 공학(사이텍미디어)21
3 4 1 목표 위치 구현을 위한 역 기구학3.4.1 목표 위치 구현을 위한 역 기구학일반적 해법
행렬구성요소들의 함수 형태를 최대한 이용하여 구하고자하는 관절변수들 를 고립시켜 구한 후 순차적으로 나머지관절변수들의 값을 구한다
TN0
iq관절변수들의 값을 구한다
간단한 머니퓰레이터 경우 머니퓰레이터의 기하학적 특성을이용하여 관절변수 값을 쉽게 구할 수도 있다
지능형 로봇 공학(사이텍미디어)22
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
지능형 로봇 공학(사이텍미디어)23
)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
25지능형 로봇 공학(사이텍미디어)
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θ
지능형 로봇 공학(사이텍미디어)28
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=α여기서
지능형 로봇 공학(사이텍미디어)29
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γ
지능형 로봇 공학(사이텍미디어)36
3 4 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학예제 3.5
예제 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 2 목표 방위 구현을 위한 역기구학3.4.2 목표 방위 구현을 위한 역기구학예제 3.6
예제 3 4와 동일한 방위를 Z Y X 일러 방법을 이용하여예제 3.4와 동일한 방위를 Z-Y-X 오일러 방법을 이용하여구현하고자 할 때 필요한 오일러 각 를 구하시오γβα ,,
Z-Y-X 오일러 방법에 의한 회전행렬은 X-Y-Z 롤-피치-요와동일하므로 역 기구학의 해는 예제 3.5의 해와 동일하다
지능형 로봇 공학(사이텍미디어)40
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
),,,,,( 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 φφφ ,,
지능형 로봇 공학(사이텍미디어)42
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
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 ωωω ,,
지능형 로봇 공학(사이텍미디어)43
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
말단부의 특정방향의 속도성분이 존재작업에 필요한 자유로운 운동을 구현해 내는데 어려움특이점 근처에서는 머니퓰레이터 말단부의 작은 속도차도관절속도에 큰 변화를 가져와 작업환경과 작업자에게 큰관절속도에 큰 변화를 가져와 작업환경과 작업자에게 큰위험요소로 작용특이점을 미리 찾아내고 이를 회피하도록 작업을 설계하는것은 대단히 중요것은 대단히 중요
지능형 로봇 공학(사이텍미디어)45
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}의원점의 절대속도
지능형 로봇 공학(사이텍미디어)49
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점이 움직이는 경우)
지능형 로봇 공학(사이텍미디어)50
???⎟⎞
⎜⎛ ++ 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
51지능형 로봇 공학(사이텍미디어)
i, j, k의 방향의 변화
{B}가 각속도 로 회전함에 따라 생기는 항C
BBBB rk
dtdzj
dtdyi
dtdx ×=⎟
⎠⎞
⎜⎝⎛ ++ ω
ω
3 5 속도 기구학3.5 속도 기구학
고정 기준계 {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
적용
53지능형 로봇 공학(사이텍미디어)
회전좌 계 { }에 대해 정의된 벡터 아님•×+
•=
• ω}{}{
)()(
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) 가속도: 좌표계가 회전하기 때문에 생겨나는 가속도 성분
지능형 로봇 공학(사이텍미디어)54
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)
완전한 형태의 자코비안 구성
지능형 로봇 공학(사이텍미디어)57
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
3 5 속도 기구학3.5 속도 기구학
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
지능형 로봇 공학(사이텍미디어)60
110000 2
,33
,33
,33 ⎥
⎦⎢⎣
⎥⎥⎥⎥⎥
⎦⎢⎢⎢⎢⎢
⎣⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
θ
ωωω
z
y
x
3 5 속도 기구학3.5 속도 기구학
== 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