younghwanjoo1608

ㅎㅇ

5 minute read


3D robot arm을 생각하고자 한다. 다시 한 번 serial link manipulator의 성질을 생각.

  (1) 모든 joint는 link에 부착되어 있다. Link는 rigid이며, joint는 움직일 수 있다.

  (2) 모든 joint는 두 link를 연결한다.

  (3) 모든 link는 두 joint를 연결한다. (첫 번째와 마지막 link를 제외하고)

  (4) $N$개의 joint와 $N+1$개의 link(ground link 포함)으로 이루어진다.


2D case와는 달리 end effector의 위치를 결정하기 위해서는 link 길이와 joint 각도만으로 부족하다.




Link에 arm의 immobile base부터 차례로 번호를 붙이자. 즉 base는 link $0$이고, 첫 번째 움직이는 body는 link $1$이다. End effector는 link $n$이 될 것이다.

3D-space 상에서 end effector의 pose를 결정하기 위해서는 최소한 6개의 joint가 필요하다.1

Joint axis는 공간 상의 직선 또는 벡터 방향으로 정의된다. Joint axis $i$는 link $i-1$을 회전시킨다.

그러므로, link는 공간 상의 두 axis들의 relative location을 나타내는 두 수($a_{i-1}, \alpha_{i-1}$)로 정의될 수 있다.


3D-space 상의 임의의 두 axis 사이의 distance는 두 axis 모두에 수직인 선(common normal)을 따라 측정한다.2 이로써 link length $a_{i-1}$이 측정된다.

다른 하나의 parameter는 link twist. 위에서 정의한 common normal을 normal vector로 갖는 plane을 생각한다면, 이 plane 위로 axis $i-1$과 $i$를 투영시킬 수 있다. 이때 둘 사이의 각도가 바로 link twist $\alpha_{i-1}$이다. Link twist는 axis $i-1$에서 $i$로 right-hand sense로 측정된다.


이제 link를 어떻게 연결할까? Link들의 연결을 특정하는 parameter는 2개이다.



이웃한 link는 둘 사이에 공통의 joint를 갖는다. link offset은 이 joint axis를 따라 측정한 두 link 사이의 거리를 의미한다. Joint axis $i$에 대한 link offset을 $d_i$로 나타낸다.

다른 parameter는 이 공통 axis에 대해 측정한, 이웃한 두 link 사이의 상대적인 각도. Joint angle이라고 하며 $\theta_{i}$로 쓴다.


이상의 내용을 토대로 link frame $i$를 정의하자.



  (1) 각 link의 끝부분에 좌표계를 부착한다.

  (2) Joint axis들 사이의 common normal 또는 교차점을 identify.

  (3) Joint $i+1$의 축을 따르는 축 $z_i$를 선택한다.

  (3) 축 $z_{i-1}$과 $z_i$의 common normal을 따라 축 $z_i$의 origin $O_i$를 위치시킨다.

  (4) 또한 common normal을 따라 축 $z_{i-1}$의 origin $O’_i$를 위치시킨다.

  (5) 축 $z_{i-1}$과 $z_i$의 common normal을 따라 joint $i$에서 $i+1$ 방향의 축 $x_i$를 assign. 만약 축이 교차한다면 두 축을 포함하는 plane에 대해 normal한 방향으로 $x_j$를 assign.

  (6) 축 $y_i$를 assign하여 right-handed frame을 완성한다.


2.2.3 Denavit-Hatenberg Parameters


Robot의 각 frame은 위에서 소개한 네 DH parameter만으로 서술할 수 있다!


Corke’s Convection


이전 link frame에 대한 다음 link frame의 pose는

\[{}^{j-1}\mathbf{T}_j = \mathbf{R}_z(\theta_j) \mathbf{T}_z(d_j) \mathbf{T}_x(a_j) \mathbf{R}_x(\alpha_j)\]

$a_i$ : $O_i$와 $O’_i$ 사이의 거리. Common normal의 길이.

$d_i$ : $z_{i-1}$을 따라서 측정한 좌표계 $O’_i$의 거리.

$\alpha_i$ : 축 $x_i$에 대한 축 $z_{i-1}$과 $z_i$ 사이의 각도. 양의 방향은 ccw.

$\theta_i$ : 축 $x_{i-1}$과 $x_i$ 사이의 각도. 양의 방향은 ccw.

위 식을 link transformation이라고 하며, matrix representation으로 쓰면 다음 형태.

\[\begin{align*} {}^{j-1}\mathbf{T}_j &= \mathbf{R}_z(\theta_j) \mathbf{T}_z(d_j) \mathbf{T}_x(a_j) \mathbf{R}_x(\alpha_j) \\ \\ &= \begin{bmatrix} \cos \theta_j & -\sin \theta_j \cos \alpha_j & \sin \theta_j \sin \alpha_j & a_j \cos \theta_j \\ \sin \theta_j & \cos \theta_j \cos \alpha_j & -\cos \theta_j \sin \alpha_j & a_j \sin \theta_j \\ 0 & \sin \alpha_j & \cos \alpha_j & d_j \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{align*}\]


Revolute joint의 경우, $\theta_i$를 joint variable, 나머지 세 parameter를 fixed link parameter라고 부른다.

Prismatic joint의 경우, $d_i$가 joint variable이고 나머지가 fixed link parameter.


어떻게 4개 parameter만으로 설명이 가능할까? position과 orientation을 결정하려면 6개의 parameter가 필요한데?

DH notation에서는 좌표계끼리 constraint(common normals)가 존재하기 때문. $x_j$는 $z_{j-1}$과 교차하며 수직이기 때문에 2개의 DOF가 사라진다.


이제 6-jointed robot의 kinematics를 서술하는데 18개의 parameter만 있으면 된다. 모두 revolute joint로 이루어진 로봇(RRRRRR)이라면, 여섯 개의 $(a_i, \alpha_i, d_i)$가 필요.

\[\mathbf{T}_E = {}^{0}\mathbf{T}_1(\theta_1){}^{1}\mathbf{T}_2(\theta_2) \cdots {}^{N-1}\mathbf{T}_N(\theta_N)\]

Prismatic joint가 섞여 있으면(RPRRRR),

\[\mathbf{T}_E = {}^{0}\mathbf{T}_1(\theta_1){}^{1}\mathbf{T}_2(d_2) \cdots {}^{N-1}\mathbf{T}_N(\theta_N)\]


또, link frame은 꼭 link 위에 있지 않아도 됨을 명심!


Miscellaneous

 (1) Frame $\{O \}$에 대해, 축 $z_0$의 방향만이 결정된다. 그리고 $O_0$와 $x_0$는 임의로 선택된다.

 (2) 마지막 frame $\{n \}$에 대해, joint $n+1$는 존재하지 않으므로 $x_n$이 $z_{n-1}$에 수직이라고 할지라도 $z_n$은 유일하게 정의되지 않는다. 보통 joint $n$은 revolute이므로, $z_n$은 $z_{n-1}$의 방향으로 놓인다.

 (3) 두 연속된 축이 평행이라면 둘 사이의 common normal은 유일하게 정의되지 않는다.

 (4) 두 연속된 축이 intersect하면, $x_i$의 방향은 임의로 정해진다.

 (5) Joint $i$가 prismatic이면, $z_{i-1}$의 방향은 임의로 정해진다.


Craig’s Convention

다른 방식으로 DH parameter를 정의할 수 있다. oint $i$의 축을 따르는 축 $z_i$를 선택하자. 이렇게 해도 계산 결과는 같다.

이때는 ${}^{i-1}\textbf{T}_i$의 형태가 다음과 같다.

\[{}^{i-1}\textbf{T}_i = \begin{bmatrix} \cos \theta_i & -\sin \theta_i & 0 & a_{i-1} \\ \sin \theta_i \cos \alpha_{i-1} & \cos \theta_i \cos \alpha_{i-1} & -\sin \alpha_{i-1} & -\sin \alpha_{i-1} d_i \\ \sin \theta_i \sin \alpha_{i-1} & \cos \theta_i \sin \alpha_{i-1} & \cos \alpha_{i-1} & \cos \alpha_{i-1} d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}\]


2.2.4 2 DOF Planar Manipulator



$i$ $\theta_j$ $d_j$ $a_j$ $\alpha_j$
1 $q_1$ 0 $a_1$ 0
2 $q_2$ 0 $a_2$ 0


위 DH paramenter 표로부터 robot의 kinematics를 완전히 표현할 수 있다.


Robotics Toolbox에서는 SerialLink 함수로 해당 DH parameter를 갖는 로봇을 묘사할 수 있다.

dh = [ 0 0 1 0 ; 0 0 1 0];
r = SerialLink(dh)

---

r = 
 
noname:: 2 axis, RR, stdDH, slowRNE                              
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          1|          0|          0|
|  2|         q2|          0|          1|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+


DH parameter를 통해 결정된 로봇 r을 plotting.


r.plot([0.2 0.3])



물론 teach 명령어를 통해 joint를 마음대로 움직일 수도 있다.

fkine 명령어는 joint input을 이용해 forward kinematics를 진행한다.

r.fkine([0.2 0.3])

---

ans = 
    0.8776   -0.4794         0     1.858
    0.4794    0.8776         0    0.6781
         0         0         1         0
         0         0         0         1


2.2.5 PUMA 560


$i$ $\theta_j$ $d_j$ $a_j$ $\alpha_j$
1 $q_1$ 0 0 90
2 $q_2$ 0 $a_2$ 0
3 $q_3$ $d_3$ $a_3$ -90
4 $q_4$ $d_4$ $a_4$ 90
5 $q_5$ 0 0 -90
6 $q_6$ 0 0 0


Robotics Toolbox에서 mdl을 이용해 로봇 모델을 불러올 수 있다.

% load puma560 model
mdl_puma560

p560
---

p560 = 
 
Puma 560 [Unimation]:: 6 axis, RRRRRR, stdDH, slowRNE            
 - viscous friction; params of 8/95;                             
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|     1.5708|          0|
|  2|         q2|          0|     0.4318|          0|          0|
|  3|         q3|    0.15005|     0.0203|    -1.5708|          0|
|  4|         q4|     0.4318|          0|     1.5708|          0|
|  5|         q5|          0|          0|    -1.5708|          0|
|  6|         q6|          0|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+

마찬가지로 joint value를 집어넣어 forward kinematics를 진행할 수 있다. 여기서는 6개의 joint 변수가 필요함에 유의.

qz
p560.plot(qz)

---

qz =

     0     0     0     0     0     0



  1. 직관적으로 생각하면 position에 3개, orientation에 3개의 parameter가 필요하니까. ↩

  2. 이러한 'mutual perpendicular'는 항상 존재. ↩

You may also enjoy