Robotics - Chapter 5. Velocity Kinematics
[4. Manipulator Inverse Kinematics]까지는 정지 상태에 있는 Manipulator에서 End-effector의 position 및 orientation과 관절 변수들의 관계를 이용하여 정/역 기구학으로 나타내는 방법에 대해 다루었다. 이 장에서는 운동 상태의 End-effector의 position과 orientation에 대한 시간 변화율인 선속도와 각속도라는 개념에 대해 알아볼 것이다. 그리고, Manipulator가 움직이는 상태에서 모양이나 크기에 약간의 변형이 있을 수 있지만, 그 정도가 무시할 수 있을 만큼 작기 때문에 강체(Rigid body)로 가정한다.
고정 축에 대한 강체의 회전 운동
선형 운동에서 정의되는 속도 및 가속도는 선속도(Linear velocity) 또는 선가속도(Linear acceleration)라고 지칭하기도 한다. 선형이 아닌 회전 운동에서의 속도와 가속도에 대해 알아보도록 하자.
어떤 물체가 한 축을 중심으로 회전을 하는 경우, 임의의 시각 $t$에서 물체의 운동 상태를 관찰하면 물체를 구성하는 각각의 부분은 서로 다른 선속도와 선가속도를 가지게 된다. 이는 물체를 구성하는 입자들의 거리가 변형되기 때문에 물체를 하나의 입자로 간주하여 운동 상태를 분석할 수 없게 된다. 그러므로 앞서 언급한 것처럼 강체의 개념을 도입하여 설명한다.
아래 그림과 같이 고정축 $z$를 중심으로 회전하는 강체에 대해서, 기준선인 $x$축으로부터 각 $\theta$만큼 이동하면 강체에 속한 모든 다른 입자들도 같은 각도 $\theta$만큼 회전한다. 각 입자와 마찬가지로 전체 강체에 각 $\theta$를 부여할 수 있으므로, 회전하는 강체의 각위치(Angular position)을 정의할 수 있다.
강체에 포함되며 회전축에 수직인 참조선(Reference line)을 위 그림의 (a)에서와 같이 설정할 수 있고, 이를 $x\text{-} y$ 평면에 정사영 시키면 (b)와 같이 나타낼 수 있다. 참조선이 $x$축과 이루는 각도를 $\theta$, 회전 반경을 $r$, 각도 $\theta$와 반지름 $r$에 의한 호의 길이를 $s$라 하면
\[\begin{align*} s &= r\theta \\ \theta &= {s \over r} \end{align*}\]이다. 여기에서 각도 $\theta$는 라디안(radian)이며, 1 라디안은 호의 길이 $s$가 반지름의 길이 $r$과 같을 때의 각도를 의미한다.
이때 각속도 $\omega$와 각가속도 $\alpha$는 다음과 같이 정의할 수 있다.
\[\begin{align*} w &= \lim_{\Delta t \to 0}{\Delta\theta \over \Delta t} = {d\theta \over dt} = \dot\theta \\ \alpha &= \lim_{\Delta t \to 0}{\Delta\omega \over \Delta t} = {d\omega \over dt} = \ddot\theta \end{align*}\]이때, $\theta$ 위에 붙어있는 점은 시간에 대한 미분을 의미한다. 즉, $\dot \theta$는 $\theta$를 시간 $t$에 대해 한번 미분한 것을 의미하고 $\ddot\theta$는 두번 미분한 것을 의미한다.
점질량($P$)과 단위 벡터($\hat r, \hat \theta, \hat k$)를 이용하여 위와 같은 [Polar coordinates]에서 $P$의 $\hat r$ 방향 및 $\hat \theta$ 방향의 속도와 가속도를 구할 수 있다.
\[\begin{align*} \vec r &= r\hat r \\ \vec v &= \dot r\hat r + r\dot{\hat r} = \dot r\hat r + r\dot\theta \hat\theta \\ \vec a &= (\ddot r-r\dot\theta^2)\hat r + (2\dot r\dot\theta + r\ddot\theta)\hat\theta \end{align*}\]Cartesian coordinates에서는 $x$축과 $y$ 축이 고정되어 있어, 단위 벡터($\hat x$, $\hat y$)의 시간에 대한 미분 값이 0이 된다. 하지만 Polar coordinates에서는 $P $의 위치가 변함에 따라 coordinate system도 변하기 때문에 위와 같은 식이 나오게 된다.
그리고 $P$가 회전하는 반경에 해당하는 $r$이 시간에 따라 변하지 않는다면, $\dot r$과 $\ddot r$ 모두 0이 된다. 그러므로 위 식은
\[\begin{align*} \vec r &= r\hat r \\ \vec v &= \dot r\hat r + r\dot{\hat r} = \dot r\hat r + r\dot\theta \hat\theta =r\dot\theta \hat\theta \\ \vec a &= (\ddot r-r\dot\theta^2)\hat r + (2\dot r\dot\theta + r\ddot\theta)\hat\theta = (-r\dot\theta^2)\hat r + ( r\ddot\theta)\hat\theta \end{align*}\]와 같이 다시 쓸 수 있다. 그러므로 그림에서 $\hat\theta$ 방향 속도에 해당하는 $v$는 $r\dot\theta$의 값을 가지게 된다. 그리고 그림에는 나타나있지 않지만, 원운동에서 회전 중심 방향으로 작용하는 구심 가속도(Centripetal acceleration)는 $-\hat r$ 방향의 가속도인 $r\dot\theta^2$가 된다. 또한 접선($\hat\theta$) 방향에 해당하는 접선 가속도(Tangential acceleration)는 $r\ddot\theta$가 된다.
단위 벡터 $\hat\theta$는 단위 벡터 $\hat k$와 $\hat r$의 외적($\hat\theta = \hat k \times \hat r$)이므로, $v$를 다음과 같이도 나타낼 수 있다.
\[\begin{align*} \vec v = r\dot\theta\hat\theta &= r\dot\theta\hat k\times\hat r \\ &=\dot\theta\hat k \times r\hat r \\ &= \vec w \times \vec r \end{align*}\]회전 좌표계를 이용한 상대 운동 해석
Manipulator 링크가 한 축에 대하여 회전 운동할 때, [2. Position and Orientation - 2]에서 정의한 링크계와 벡터 성분을 이용하여 공간상의 위치 벡터로 표현되는 한 질점(Mass of point)에 대한 선속도와 각속도에 대해 알아보자.
링크계 상의 선속도
좌표계 {$A$}에서 점 $P$에 대한 위치 벡터는 ${}^AP$라고 할 수 있으므로, 좌표계 {$A$} 상에서 점 $P$의 속도는 ${}^AV_P(=\dot{{}^AP})$로 표현할 수 있다.
링크계 상의 각속도
선속도가 한 점에 대한 속도를 의미하는 반면 각속도는 강체가 회전 운동할 때, 한 질점에 대한 속도를 의미한다. 링크계의 회전에 의한 각속도는 $\Omega$ 기호를 사용해 나타내며, 링크계를 부여하여 기준계에 대한 특정계의 회전을 표현한다. 위 그림의 ${}^A\Omega_B$는 기준계 {$A$}에 대한 링크계 {$B$}의 회전을 의미하며, 링크계 {$B$}의 원점을 기준계 {$A$}의 원점과 일치하도록 평행이동하여 나타내면 다음과 같다.
계를 이용한 상대 속도
기준계 {$A$}에서 정의된 공간상의 점 ${}^AP$는 링크계 {$B$}에 대한 위치 벡터 ${}^AP_{Borg}$와 회전 행렬 ${}^AR_B$를 이용하여 나타내면
\[{}^AP = {}^AP_{Borg} + {}^AR_B {}^BP\]와 같다. 이를 시간에 대해 미분하면 기준계 {$A$} 상에서 점 ${}^AP$의 속도인 ${}^AV_P$를 다음과 같이 구할 수 있다.
\[\begin{align*} {}^AV_P &= \dot {{}^AP} \\ &= \dot{{}^AP_{Borg}} + \dot{({}^AR_B {}^BP)} \\ &= {}^AV_{Borg} + \dot{{}^AR_B} {}^BP + {}^AR_B \dot{{}^BP} \\ &= {}^AV_{Borg} + [{}^A\Omega_B]_\times{}^AR_B {}^BP + {}^AR_B {}^BV_P \end{align*}\]\[\Omega = \begin{pmatrix} ~\Omega_x \\ ~\Omega_y \\ ~\Omega_z \end{pmatrix}, ~~ [\Omega]_\times = \begin{pmatrix} 0 & -\Omega_z & \Omega_y\\ \Omega_z & 0 & -\Omega_x \\ -\Omega_y & \Omega_x & 0 \end{pmatrix}\]회전 행렬 ${}^AR_B$을 시간에 대해 미분한 $\dot{{}^AR_B}$의 값이 \([{}^A\Omega_B]_\times{}^AR_B\)인 이유는 이후에 나온다. \([{}^A\Omega_B]_\times\)는 \({}^A\Omega_B\)의 Skew-symmetric matrix이고, $[{}^A\Omega_B]_\times{}^AR_B = {}^A\Omega_B \times{}^AR_B$ 를 의미한다. Skew-symmetric matrix의 공식은 다음과 같다.
각속도
방금 식으로 살펴보았던 회전 행렬의 시간에 대한 미분 값($\dot{{}^AR_B}$)을 구하기 위해 각속도에 대해 자세히 살펴보자. 기하학적인 방법과 대수적인 방법 두가지로 살펴보겠다.
기하학적 해석
위 그림과 같이 링크계 {$A$}와 {$B$}의 원점이 일치(${}^AP_{Borg}=0$)하고, 점 ${}^BP$가 고정 링크계 {$A$}에 대해서 각속도 ${}^A\Omega_B$로 회전하는 관계를 생각해보자. 링크계 {$B$} 상에서의 점 $P$인 ${}^BP$가 고정되어 있다고 가정하면, ${}^BV_P=\dot{{}^BP}=0$ 이다. 하지만 링크계 {$A$}에 대해서 링크계 {$B$}가 각속도 ${}^A\Omega_B$로 회전하기 때문에 ${}^AV_P \ne 0$ 이다. 이때, ${}^AV_P$를 결정하는 값이 $\dot{{}^AR_B} {}^BP$가 된다.
위 그림에서 링크계 {$A$}에 대해서 링크계 {$B$}가 ${}^A\Omega_B$축 으로 회전할 때, 시계 방향으로의 회전을 양의 방향으로 설정하면 다음과 같은 관계식을 설정할 수 있다.
\[\begin{align*} \Delta{}^AP_B &= ({}^A\Omega_B \Delta t) ({}^AP_B\sin\theta) \\ {\Delta{}^AP_B \over \Delta t} &= {}^A\Omega_B {}^AP_B\sin\theta \\ \end{align*}\]\[\begin{align*} {}^AV_P &= {}^A\Omega_B \times {}^AP_B = {}^A\Omega_B \times {}^AR_B {}^BP\\ &= \dot{{}^AR_B} {}^BP \end{align*}\]여기서 ${}^AP_B$는 좌표계 {$A$}에서 점 ${}^BP$의 위치 벡터를 의미한다. 즉, ${}^AP_B = {}^AP$ 이다.
위 식에서는 원점을 공유한다는 가정과 점 $P$가 링크계 {$B$} 상에서 고정되어 있다는 가정을 한 결과이다. 따라서 두 링크계의 원점 사이 속도(${}^AV_{Borg}$)와 링크계 {$B$} 상에서 점 $P$의 속도($\dot{{}^BP}$)를 고려하면, 앞서 구한 아래 식이 성립함을 알 수 있다.
\[\begin{align*} {}^AV_P &= \dot {{}^AP} \\ &= \dot{{}^AP_{Borg}} + \dot{({}^AR_B {}^BP)} \\ &= {}^AV_{Borg} + \dot{{}^AR_B} {}^BP + {}^AR_B \dot{{}^BP} \\ &= {}^AV_{Borg} + [{}^A\Omega_B]_\times{}^AR_B {}^BP + {}^AR_B {}^BV_P \end{align*}\]대수적 해석
[회전 변환된 좌표계의 변환]에서 살펴보았듯이, ${}^AR_B$는 (정규) 직교 행렬이다. ${}^AR_B$를 $R$이라 할 때, $RR^T=I$ 이므로 $\det(R)^2=1$ 을 만족한다. 만약 $\det(R)=-1$ 일 경우, 해당 행렬은 벡터 간의 orientation 관계를 반대로 바꿔버리기 때문에 Rotation matrix라고 할 수 없다(이유). 그러므로 Rotation matrix $R$은 $\det(R)=1$ 을 항상 만족하고, 이러한 행렬을 정상 (정규) 직교 행렬(Proper orthogonal matrix)이라고 한다.
$RR^T=I$ 이고 $R$은 $\theta$로 이루어진 식이므로 양변을 $\theta$에 대해 미분하면
\[\begin{align*} {dR\over d\theta}R^T + R{dR\over d\theta}^T &= {dR\over d\theta}R^T + R({dR\over d\theta})^T \\ &= {dR\over d\theta}R^T + ({dR\over d\theta}R^T)^T \\&=S+S^T \\&= 0 \end{align*}\]가 되고, $S(={dR\over d\theta}R^T)$ 는 비대칭 행렬(Skew symmetric matrix)의 특성을 만족하게 된다. 그리고
\[\begin{align*} SR &= {dR\over d\theta}R^T R \\&= {dR\over d\theta} \\ SRd\theta &= dR \end{align*}\]이다. 마지막 식의 양변을 시간에 대해 미분하기 위해 $dt$로 나누어주면
\[\begin{align*} SR{d\theta\over dt} &= {dR\over dt} \\ SR\dot\theta &= \dot R \end{align*}\]을 구할 수 있다. 그리고 간단하게 좌변과 우변이 같은지를 확인해보면 아래와 같이 성립함을 알 수 있다.
\[\begin{align*} SR\dot\theta &= ({dR\over d\theta}R^T)R({d\theta\over dt}) \\ &= {dR\over d\theta}R^TR{d\theta\over dt} \\ &= {dR\over d\theta}I{d\theta\over dt} \\ &= {dR\over d\theta}{d\theta\over dt} \\ &= {dR\over dt} \\ &= \dot R \end{align*}\]앞서 기하학적 방법으로 구한 \(\dot{{}^AR_B} = [{}^A\Omega_B]_\times {}^AR_B\) 를 위 식의 표기법으로 변환하면 \(\dot R = [\Omega]_\times R\) 이다. 해당 식과 위 식이 동일한 $\dot R$에 대해 표현하므로 \(SR\dot\theta = [\Omega]_\times R\) 이고, \(SR\dot\theta = (\dot\theta S)R\) 이므로 \(\dot\theta S = [\Omega]_\times\) 이다. 따라서 아래 식을 만족하게 된다.
\[\begin{align*} \dot\theta S &= [\Omega]_\times \\ &= [\dot\theta \hat\Omega]_\times \\ &= \dot\theta[\hat\Omega]_\times \\ \therefore S &= [\hat\Omega]_\times \end{align*}\]이때 \([\Omega]_\times = [\dot\theta \hat\Omega]_\times\) 이고 \(\dot\theta\)는 각속도 값(스칼라)이므로, $\hat\Omega$은 회전축의 방향을 가리키는 크기가 1인 단위벡터를 의미한다.
Manipulator의 링크 운동 해석
Manipulator의 이웃하는 두 링크와 관절에 대한 운동 해석을 위해서, 기준계인 베이스부(링크계 {$0$})를 기준으로 임의의 계(링크계 {$i$})에서의 속도와 각속도를 다음과 같이 표현하기로 한다.
\[\begin{align*} v_i &= {}^0v_i = {}^0V_i \\ \omega_i &= {}^0\omega_i = {}^0\Omega_i \end{align*}\]어떤 링크계 {$i$}의 기준계 {$0$}에 대한 속도를 임의의 좌표계 {$k$}에 대해서 나타내는 방법은 다음과 같다.
\[\begin{align*} {}^kv_i &= {}^kR_0({}^0V_i) = {}^kR_0{}^0v_i \\ {}^k\omega_i &= {}^kR_0({}^0\Omega_i) = {}^kR_0 {}^0\omega_i \end{align*}\]임의의 링크계 {$A$}에 대한 링크계 {$C$}의 각속도는 링크계 {$B$}를 매개로 하여 다음과 같이 나타낼 수 있다.
\[{}^A\Omega_C = {}^A\Omega_B + {}^AR_B{}^B\Omega_C\]대문자에 해당하는 $V, \Omega$ 는 임의의 두 좌표계 사이의 상대적인 속도를 의미하는 반면, 소문자에 해당하는 ${}^kv_i, {}^k\omega_i$ 는 기준계 {$0$}에서 측정한 어떤 링크계 {$i$}의 속도를 임의의 좌표계 {$k$}에서 표현한 것을 의미한다.
링크 간의 각속도
앞선 표현법들을 활용하면 다음과 같은 식을 유도할 수 있다.
\[\begin{align*} {}^0\Omega_{i+1} &= {}^0\Omega_i + {}^0R_i{}^i\Omega_{i+1} \\ {}^{i+1}R_0{}^0\Omega_{i+1} &= {}^{i+1}R_0({}^0\Omega_i + {}^0R_i{}^i\Omega_{i+1}) \\ {}^{i+1}\omega_{i+1} &= {}^{i+1}\omega_i + {}^{i+1}R_i{}^i\Omega_{i+1} \\ &= {}^{i+1}R_i ({}^i\omega_i + {}^i\Omega_{i+1}) \end{align*}\]이때 관절의 회전은 오직 $\hat Z$축에 대해서만 일어나기 때문에
\[{}^{i+1}R_i{}^i\Omega_{i+1} = \begin{pmatrix} 0 \\ 0 \\ \dot\theta_{i+1} \end{pmatrix}\]와 같이 표현할 수 있다. 그러므로 \({}^{i+1}\omega_{i+1}\) 를 다음과 같이 나타낼 수 있다.
\[\begin{align*} {}^{i+1}\omega_{i+1} &= {}^{i+1}R_i{}^i\omega_i + \begin{pmatrix} 0 \\ 0 \\ \dot\theta_{i+1} \end{pmatrix} \\ &= {}^{i+1}R_i{}^i\omega_i + \dot\theta_{i+1}{}^{i+1}\hat Z_{i+1} \end{align*}\]링크 간의 선속도
각속도에서와 마찬가지로 다음과 같은 식을 유도할 수 있다.
\[\begin{align*} {}^0V_{i+1} &={}^0V_i + \dot{{}^0R_i} {}^iP_{i+1} + {}^0R_i {}^iV_{i+1} \\ &={}^0V_i + [{}^0\Omega_i]_\times{}^0R_i {}^iP_{i+1} + {}^0R_i {}^iV_{i+1} \\ {}^{i+1}R_0{}^0V_{i+1} &= {}^{i+1}R_0({}^0V_i + [{}^0\Omega_i]_\times{}^0R_i {}^iP_{i+1} + {}^0R_i {}^iV_{i+1}) \\ {}^{i+1}v_{i+1}&= {}^{i+1}R_i({}^iR_0{}^0V_i + {}^iR_0[{}^0\Omega_i]_\times{}^0R_i {}^iP_{i+1}) + {}^{i+1}R_i {}^iV_{i+1} \\ \end{align*}\]${}^{i}P_{i+1}$ 는 링크계 {$i$}에서 링크계 {$i+1$}의 원점을 표현한 위치 벡터이다. ( ${}^{i}P_{i+1} = {}^{i}P_{(i+1)org}$ )
이때 마지막 식의 각 항을 다음과 같이 표현할 수 있다.
\[\begin{align*} {}^iR_0{}^0V_i = {}^iv_i \end{align*}\] \[\begin{align*} {}^iR_0[{}^0\Omega_i]_\times{}^0R_i &= [{}^iR_0{}^0\Omega_i]_\times \\ &= [{}^i\omega_i]_\times \end{align*}\] \[\begin{align*} {}^{i+1}R_i {}^iV_{i+1} &= \begin{pmatrix} 0 \\ 0 \\ \dot d_{i+1} \end{pmatrix} \\ &= \dot d_{i+1}{}^{i+1}\hat Z_{i+1} \end{align*}\]두번째 식에 대한 증명은 다음과 같다. (자세한 설명은 클릭)
임의의 회전 행렬 $R$ 과 임의의 벡터 $a, b$ 가 있고, $S(a) = [a]_\times$ 라 하자.
\(\begin{align*} RS(a)R^Tb &= R(a\times (R^Tb)) \\ &= (Ra) \times (RR^Tb) \\ &= (Ra) \times b \\ &= S(Ra)b \end{align*}\)
그러므로 $RS(a)R^T = S(Ra)$ 를 만족한다.
그러므로 ${}^{i+1}v_{i+1}$ 식을 다음과 같이 간단하게 표현할 수 있다.
\[\begin{align*} {}^{i+1}v_{i+1} &={}^{i+1}R_i({}^iv_i + [{}^i\omega_i]_\times{}^iP_{i+1}) + \begin{pmatrix} 0 \\ 0 \\ \dot d_{i+1} \end{pmatrix} \\ &={}^{i+1}R_i({}^iv_i + {}^i\omega_i\times{}^iP_{i+1}) + \dot d_{i+1}{}^{i+1}\hat Z_{i+1} \end{align*}\]자코비안과 Manipulator
Manipulator는 각 관절의 각도를 제어함으로써 End-effector를 직교 좌표 공간에 원하는 지점에 위치시킨다. Manipulator의 각 관절은 입력 변수이며, 이를 관절 공간(Joint space)라고 한다.
$n$개의 관절 변수를 가지는 Manipulator의 관절 공간은
\[\theta = \begin{bmatrix} \theta_1 \\ \theta_2 \\ \vdots \\ \theta_n \end{bmatrix}\]이고, Manipulator의 End-effector를 직교 좌표 공간상에
\[X = \begin{bmatrix} [{}^0P_n] \\[3pt] [{}^0R_n] \end{bmatrix}\]와 같이 나타낸다면, 두 공간 사이의 관계는 다음과 같다.
자코비안 행렬의 정의
$n=6$ 개의 관절 변수를 가지는 6자유도 로봇의 직교 좌표 변수 벡터 $X$와 관절 변수 벡터 $q$를 다음과 같다고 가정하자.
\[X = \begin{bmatrix} P_x\\P_y\\P_z\\\phi\\\theta\\\psi \end{bmatrix}~~, ~~~~~~~~~~ q = \begin{bmatrix} q_1\\q_2\\q_3\\q_4\\q_5\\q_6 \end{bmatrix}\][정기구학]에서는 관절 변수 벡터 $q$로 직교 위치 벡터 $X$를 구하는 것이 목적이었다. 따라서 다음과 같은 관계가 있다.
\[X = \boldsymbol F(q)\]여기서 $\boldsymbol F$는 정기구학에 해당한다. 위 식을 시간에 대해 미분하면 다음과 같은 시간에 따라 변화하는 속도 기구학(Velocity Kinematics)에 해당하는 식을 얻을 수 있다.
\[\frac{dX}{dt} = \frac{\partial \boldsymbol F(q)}{\partial q} \frac{\partial q}{\partial t}\]이때, 자코비안(Jacobian) 행렬은 다음과 같이 정의되며,
\[\def\arraystretch{1.5} \begin{align*} J &= \frac{\partial \boldsymbol F(q)}{\partial q} \\&= \begin{bmatrix} \frac{\partial \boldsymbol{F}_1}{\partial q_1} & \frac{\partial \boldsymbol{F}_1}{\partial q_2} & \cdots & \frac{\partial \boldsymbol{F}_1}{\partial q_n} \\ \frac{\partial \boldsymbol{F}_2}{\partial q_1} & \frac{\partial \boldsymbol{F}_2}{\partial q_2} & \cdots & \frac{\partial \boldsymbol{F}_2}{\partial q_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial \boldsymbol{F}_6}{\partial q_1} & \frac{\partial \boldsymbol{F}_6}{\partial q_2} & \cdots & \frac{\partial \boldsymbol{F}_6}{\partial q_n} \\ \end{bmatrix} \end{align*}\]이를 이용해 속도 기구학 식을 간단히 나타낼 수 있다.
\[\dot X = J \dot q\]그리고 이 식을 역으로 이용해 속도 $\dot X$가 주어지면, 관절 변수의 속도 $\dot q$를 구할 수 있다.
\[J^{-1}\dot X = J^{-1}J \dot q = \dot q\]이는 직교 좌표에서 Manipulator의 움직임을 관절 공간에서 관절의 움직임으로 나타내게 해주므로, 실제 제어에 매우 중요하다. 하지만 위 식을 이용하기 위해서는 $J$의 역행렬이 존재해야 한다. 즉, $\det(J) \ne0$ 이 성립해야 한다.
직교 좌표 변수 벡터 $X$와 관절 변수 벡터 $q$의 시간에 대한 미분을 구하면 다음과 같다.
\[\dot X = \begin{bmatrix} \dot P_x\\\dot P_y\\\dot P_z\\\dot \phi\\\dot \theta\\\dot \psi \end{bmatrix} = \begin{bmatrix} {}^0v_n \\ {}^0\omega_n \end{bmatrix}~~, ~~~~~~~~~~ \dot q = \begin{bmatrix} \dot q_1\\\dot q_2\\\dot q_3\\\dot q_4\\\dot q_5\\\dot q_6 \end{bmatrix}\]그리고 앞서 구했던 자코비안 행렬도 위와 같이 선속도와 각속도에 관한 값으로 분리할 수 있다.
\[J = \begin{bmatrix} J_v \\ J_\omega \end{bmatrix}\]따라서 다음 식을 만족하게 되고,
\[\displaylines{ {}^0v_n = J_v\dot q \\ {}^0\omega_n = J_\omega\dot q }\]이를 행렬 표현으로 바꾸면 다음과 같다.
\[\begin{bmatrix} {}^0v_n \\ {}^0\omega_n \end{bmatrix} = \begin{bmatrix} J_v \\ J_\omega \end{bmatrix} \dot q = J\dot q\]링크계의 각속도
먼저 각속도에 관한 자코비안인 $J_\omega$를 구해보자. 만약 관절 $i$가 회전 관절이라면, $q_i = \theta_i$ 이고 다음 식을 만족한다. ( ${}^i \hat z_i = k = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix} ^T$ )
\[{}^{i}\Omega_{i} = {}^iR_{i-1}{}^{i-1}\Omega_{i} = {}^iR_{i-1}({}^{i-1}\hat z_i \dot q_i) = \dot q_i {}^i\hat z_i = \dot q_i k =\begin{pmatrix} 0 \\ 0 \\ \dot q_i \end{pmatrix}\] \[{}^0R_{i}{}^{i}\Omega_{i} = \dot q_i {}^0R_{i}{}^i\hat z_i = \dot q_i {}^0\hat z_i\]이때 $\dot q_i$는 스칼라이고, ${}^0R_{i}{}^i\hat z_i$ 는 링크계 {$i$}의 $z$ 축(${}^i\hat z_i$)을 기준계 {$0$}에서 표현한 것이므로 ${}^0\hat z_i$이 된다. 따라서 기준계 {$0$}을 기준으로 마지막 링크계 {$n$}의 각속도는 다음과 같다.
\[\begin{align*} {}^0\Omega_n = {}^0\omega_n &= {}^0R_1 {}^1\Omega_1 + {}^0R_2 {}^2\Omega_2 + \cdots + {}^0R_{n} {}^{n}\Omega_n \\ &= {}^0R_1 \dot q_1k + {}^0R_2 \dot q_2k + \cdots + {}^0R_n \dot q_nk \\ &= \dot q_1 {}^0\hat z_1 + \dot q_2 {}^0\hat z_2 + \cdots + \dot q_n {}^0\hat z_n \\ &=\sum_{i=1}^n \dot q_i{}^0\hat z_i \end{align*}\]앞서 정의한 식에서 ${}^0\omega_n$은 다음과 같으므로,
\[{}^0\omega_n = J_\omega\dot q\]$J_\omega$는 다음과 같은 식을 만족한다. ( \(\dot q = \begin{bmatrix} \dot q_1 &\dot q_2 &\cdots &\dot q_n \end{bmatrix}^T\) )
\[J_\omega = \begin{bmatrix} ~{}^0\hat z_1 & {}^0\hat z_2 & \cdots & {}^0\hat z_n~ \end{bmatrix}_{3\times n}\]모든 관절이 회전 관절일 경우에 해당한다. 만약 관절 $i$가 직동 관절이라면, $J_\omega$의 $i$번째 열은 $0$ 벡터가 된다.
링크계의 선속도
$n$개의 관절 변수로 이루어진 Manipulator의 End-effector의 변환 행렬은 다음과 같이 표현할 수 있다.
\[\def\arraystretch{1.2} {}^0T_n(q) = \begin{bmatrix} {}^0R_n(q) & {}^0P_n(q)\\ 0 & 1 \end{bmatrix}\]이때 ${}^0P_n$은 기준계 {$0$}에서 링크계 {$n$}의 원점 $O_n$의 위치 벡터를 의미하고, 선속도는 다음과 같이 구할 수 있다.
\[\begin{align*} \dot{{}^0P_n} &= \frac{\partial{}^0P_n}{\partial q_1}\dot q_1 + \frac{\partial{}^0P_n}{\partial q_2}\dot q_2 + \cdots + \frac{\partial{}^0P_n}{\partial q_n}\dot q_n \\ &= \sum_{i=1}^n \frac{\partial{}^0P_n}{\partial q_i}\dot q_i \end{align*}\]이전까지는 ${}^AP_{Borg}$가 좌표계 {$A$}에서 좌표계 {$B$}의 원점의 위치 벡터를 의미했지만, 편의를 위해 원점을 뜻하는 $org$ 부분을 제거했다. 즉, ${}^AP_{B} = {}^AP_{Borg}$ 이다.
그러므로 $J_v$의 $i$번째 열성분은 $\frac{\partial{}^0P_n}{\partial q_i}$가 되고, 이는 $i$번째 관절만 움직이고 다른 관절은 고정되어 있다고 가정하게 된다.
관절 $i$가 직동 관절일 경우
직동 관절일 경우 $q_i = d_i$ 이고, 링크계 {$i-1$}에서 링크계 {$i$}의 원점 $O_i$의 위치 벡터는 ${}^{i-1}P_i$ 이므로
\[{}^{i-1}P_i = a_{i-1}{}^{i-1}\hat x_{i-1} + {}^{i-1}R_id_i{}^i\hat z_i\]가 된다. 그리고 앞서 정의한 ${}^0P_n$은 다음과 같다.
\[\begin{align*} {}^0P_n &= {}^0P_{i-1} + {}^0R_{i-1}{}^{i-1}P_n \\ &= {}^0P_{i-1} + {}^0R_{i-1}({}^{i-1}P_i + {}^{i-1}R_{i}{}^{i}P_n) \end{align*}\]관절 $i$만 움직인다고 가정하면, $q_i$로 이루어진 식만 변화하게 된다. 위 식에서 ${}^{i-1}P_i$를 제외한 나머지 항은 모두 $q_i$와 관련이 없기 때문에, $q_i$에 대해 미분하면 다음과 같다.
\[\begin{align*} \frac{\partial{}^0P_n}{\partial q_i} &= {}^0R_{i-1}\frac{\partial{}^{i-1}P_i}{\partial q_i} \\ &={}^0R_{i-1}\frac{\partial(a_{i-1}{}^{i-1}\hat x_{i-1} + {}^{i-1}R_iq_i{}^i\hat z_i)}{\partial q_i} \\ &={}^0R_{i-1}({}^{i-1}R_i{}^i\hat z_i) \\ &={}^{0}R_i{}^i\hat z_i \\ &={}^0\hat z_i \end{align*}\]관절 $i$가 회전 관절일 경우
회전 관절일 경우 $q_i = \theta_i$ 이고, 직동 관절일 때와 마찬가지로 다음 식을 만족한다.
\[\begin{align*} {}^0P_n &= {}^0P_{i-1} + {}^0R_{i-1}{}^{i-1}P_n \\ &= {}^0P_{i-1} + {}^0R_{i-1}({}^{i-1}P_i + {}^{i-1}R_{i}{}^{i}P_n) \end{align*}\]관절 $i$만 움직인다고 가정하면, $q_i$에 따라 변화하는 항은 ${}^{i-1}R_{i}$ 가 유일하다. 그러므로 가정을 유지한 채로 위 식을 시간 $t$에 대해 미분하면 다음과 같다.
\[\begin{align*} \dot{{}^0P_n}&= {}^0R_{i-1}({}^{i-1}\dot R_{i}){}^{i}P_n \\ &= {}^0R_{i-1} ([{}^{i-1}\Omega_{i}]_\times{}^{i-1}R_{i}) {}^{i}P_n \\ &= {}^0R_{i-1} [{}^{i-1}\Omega_{i}]_\times{}^{i-1}R_{0} {}^{0}R_{i} {}^{i}P_n \\ &= [{}^0R_{i-1}{}^{i-1}\Omega_{i}]_\times {}^{0}R_{i} {}^{i}P_n \\ &= [{}^0R_{i}{}^{i}\Omega_{i}]_\times {}^{0}R_{i} {}^{i}P_n \\ &= ({}^0R_{i}{}^{i}\Omega_{i})\times ({}^{0}R_{i} {}^{i}P_n) \\ &= \dot q_i {}^0\hat z_i \times ({}^{0}P_n - {}^{0}P_i) \end{align*}\]따라서 다음 식을 만족하게 된다.
\[\begin{align*} \frac{{}^0\dot P_n}{\dot q_i} = \frac{\partial{}^0P_n}{\partial q_i} = {}^0\hat z_i \times ({}^{0}P_n - {}^{0}P_i) \end{align*}\]자코비안 행렬
앞서 살펴본 링크계의 각속도와 선속도를 이용해 자코비안 행렬을 완성할 수 있다.
\[J = \begin{bmatrix} J_v \\ J_\omega \end{bmatrix} = \begin{bmatrix} J_1 & J_2 & \cdots & J_n \end{bmatrix}_{6\times n}\]관절 $i$가 회전 관절일 경우에는 다음과 같고,
\[J_i = \begin{bmatrix} {}^0\hat z_i \times ({}^{0}P_n - {}^{0}P_i) \\ {}^0\hat z_i \end{bmatrix}\]관절 $i$가 직동 관절일 경우에는 각속도가 없으므로 다음과 같다.
\[J_i = \begin{bmatrix} {}^0\hat z_i \\ 0 \end{bmatrix}\]${}^0\hat z_i$ 는 베이스부인 기준계 {$0$}에서 링크계 {$i$}의 $\hat z$축을 표현한 벡터이고, ${}^{0}P_i$ 는 베이스부인 기준계 {$0$}에서 링크계 {$i$}의 원점을 표현한 위치 벡터이다.
특이성
행렬의 특이성(singularity)은 역행렬이 존재하는지 판단할 때 사용된다.
위 그림과 같이 자코비안이 비특이 행렬(non-singular matrix)이면 역행렬이 존재하므로, 직교 좌표계의 End-effector에 대한 속도를 이용해서 관절 변수의 속도를 역으로 계산할 수 있다. 그리고 자코비안의 역행렬이 존재할 조건은 다음과 같다.
\[\det (J(\theta)) \ne 0\]물리적으로는 작업공간의 경계부근에서 대부분의 Manipulator가 기구적인 특이성을 가지고 있다. 특히 작업 반경 내에서 관절 축이 일렬로 나란해지는 경우에 주로 발생한다.
특이점이 되는 부분에서는 예상할 수 있듯이 End-effector의 임의의 운동이 제한되기 때문에 자유도의 감소가 발생한다. 또한, 역기구학 풀이 시 무한개의 해가 나타나기 때문에 적절한 해를 찾는데 어려움이 있게 된다. 그리고, 특이점 근방에서 속도는 Manipulator의 관절 공간에 매우 큰 속도 값을 파생하므로, Manipulator의 물리적인 기구 요소에 손상을 유발할 수도 있다.