Inverse Kinematic Analysis of a 6-DOF Collaborative Robot with Offset Wrist

Offset Wrist를 갖는 6자유도 협동로봇의 역기구학 해석

Abstract

In this paper, the numerical inverse kinematics analysis is presented for a collaborative robot with an offset wrist. Robot manipulators with offset wrist are widely used in industrial applications, due to many advantages over those with wrist center and those with three parallel axes such as simple mechanical design, light weight, and so on. There may not exist a closed-form solution for a robot manipulator with offset wrist. A simple numerical method is applied to solve the inverse kinematics with offset wrist. Singularity is analyzed using Jacobian matrix and the numerical inverse kinematics algorithm is implemented on the real-time controller.

1. 서론

직렬 매니퓰레이터(manipulator)의 역기구학[1]을 효율적으로 계산하는 것은 로봇 공학에 가장 중요한 기본적인 과제이고 산업 로봇시스템의 제어 및 응용을 위한 필수 요구사항이다. 일반적인 제조용 다관절 로봇과 같이 Spherical wrist를 갖는 직렬 매니퓰레이터는 세 개의 연속적인 조인트축이 한 점에서 교차하여 해석적 역기구학 해가 존재하지만, 추가의 링크 또는 조인트 offset이 존재하는 경우에는 해석적 역기구학 해가 존재하지 않을 수 있다. 이러한 로봇 기구부 해석의 복잡성에도 불구하고 다양한 응용예 에서 필요한 성능을 제공하므로 추가의 offset이 포함된 로봇 기구부 사용이 산업용 로봇에서 증가되고 있다[2]. 관절 축이 평행하지 않고 offset wrist가 있는 로봇 매니퓰레이터는 높은 payload를 제공하지만 [3], 교차하거나 평행한 3개의 인접한 조인트 축이 존재하지 않으면 역기구학 해가 존재하지 않을 수 있다[4,5]. Offset wrist를 갖는 메커니즘에 관한 많은 연구가 진행되었다[6-8]. 로봇의 성능을 제한하는 특이점을 제거하기 위해 유니버설 조인트(universal joint) 형태의 offset wrist를 갖는 연구도 제시되었다[9]. Offset wrist 메커니즘에 대해 ADAMS 시뮬레이션을 수행하여 적절한 offset 수정 방법에 관한 연구도 제시되었다[10].

본 논문에서는 Universal robot[11] 구조에서 wrist 부에 위치한 4축을 3축에 이동 배치함으로써 wrist부의 중량 및 관성을 감소시킬 수 있어 가반하중 증가가 가능하고 보다 고속운동이 가능한 로봇기구 메커니즘을 제안한다. Offset wrist를 갖는 로봇 기구부에 대한 수치적 방법을 포함하는 역기구학 해석방법을 제시한다. 또한, 자코비안 행 렬식을 이용한 특이성 해석결과를 제시하고 수치 적 역기구학 해석알고리즘을 실시간 제어기에 적용하여 이의 유효성을 확인하였다.

2. 로봇의 기구학적 모델

제안하는 offset wrist를 갖는 6자유도 협동로봇의 기구학적 모델링을 Fig. 1과 같이 제시한다. 기구학적 모델에 대한 D-H 매개변수를 Table 1과 같이 나타낼 수 있다. Fig. 1과 같은 로봇 형상에 대한 조인트 변수(\(\theta_i\)) 및 기준 좌표계 O-x₀y₀z₀에 대한 엔드이펙터(6번 조인트) 좌표계 Q-x₆y₆z₆의 동차변환 행렬은 다음과 같다.

Table 1. D-H parameters for robot of kinematics modeling with offset wrist

Fig. 1 Kinematic modeling of the robot with offset wrist

\(\begin{gathered} \boldsymbol{\theta}=[0, \pi / 2, \pi / 2,0,0,0]^{T}, \\ { }^{0} A_{6}=\left[\begin{array}{cccc} -1 & 0 & 0 & 0 \\ 0 & -1 & 0 & -d_{5} \\ 0 & 0 & 1 & d_{1}+a_{2}+d_{4}+d_{6} \\ 0 & 0 & 0 & 1 \end{array}\right] \end{gathered}\)

3. 역기구학 해석

Offset wrist 구조의 협동로봇은 d₅ ≠ 0인 wrist center를 갖지 않는 구조이다. Wrist center를 갖는 경우, Fig. 2와 같이 P 와 P’ 가 일치하여 로봇 암 3축과 손목 3축을 분리하여 해석적 역기구학 해를 유도할 수 있다. 본 연구에서는 θ₆만을 포함하는 비선형 방정식에 대하여 θ6를 수치적 방법으로 구하고, 기존 wrist center를 갖는 구조와 유사하게 P점을 이용하여 로봇 암을 구성하는 θ₁, θ₂, θ₃ 을 해석적으로 구하고 이 결과를 이용하여 로봇 손목을 구성하는 θ₄, θ₅를 해석적으로 연산할 수 있는 알고리즘을 제시하고자 한다.

Fig. 2 Orthogonality condition for the robot with offset wrist

첫 번째로 θ₆를 수치적 방법으로 구한다고 하면, 두 번째로 로봇암에 대한 역기구학 해석을 유도한다. θ₆를 이용하여 기준 좌표계 및 3번 조인트 좌표계에 대하여 P점을 다음과 같이 구할 수 있다.

\(\begin{aligned} { }^{0} \boldsymbol{p} &=\overrightarrow{O P}=\left[p_{x}, p_{y}, p_{z}\right]^{T}=\boldsymbol{q}-d_{6} \boldsymbol{w}+d_{5} \boldsymbol{z}_{4}, \\ { }^{3} \boldsymbol{p} &=\overrightarrow{B P}=\left[0,0, d_{4}, 1\right]^{T} \end{aligned}\)       (1)

여기서 기준 좌표계에서 표현된 u=x₆, v=y₆, w=z₆로 정의하고 Fig. 2와 같이 z₄를 다음과 같이 구할 수 있다.

\(\boldsymbol{z}_{4}=s \theta_{6} \boldsymbol{u}+c \theta_{6} \boldsymbol{v}\)       (2)

동차변환행렬을 이용하여 다음 관계를 유도할 수 있다.

\(\left({ }^{0} A_{1}\right)^{-1}{ }^{0} \boldsymbol{p}={ }^{1} A_{3}{ }^{3} \boldsymbol{p}\)       (3)

위 식을 정리하면 다음과 같은 3개 식을 구할 수 있다.

\(p_{x} c \theta_{1}+p_{y} s \theta_{1}=a_{2} c \theta_{2}+d_{4} s \theta_{23}\)       (4)

\(p_{z}-d_{1}=a_{2} s \theta_{2}-d_{4} c \theta_{23}\)       (5)

\(p_{x} s \theta_{1}-p_{y} c \theta_{1}=0\)       (6)

식 (1)로부터 θ₁은 다음과 같이 구할 수 있다.

\(\theta_{1}=\operatorname{Atan} 2\left(p_{y}, p_{x}\right)\)       (7)

여기서, θ₁=θ₁^* 또는 \(\theta_{1}=\theta_{1}^{*}+\pi\)로 2개의 해가 가능하다. \({ }^{0} \boldsymbol{p}=\left[0,0, p_{z}\right]^{T}\)인 경우 θ₁은 정의할 수 없는 특이점을 갖는다.

식 (4), (5), (6)의 양변을 제곱하여 더하면 θ₃만의 수식을 얻을 수 있다.

\(2 a_{2} d_{4} s \theta_{3}=p_{x}^{2}+p_{y}^{2}+\left(p_{z}-d_{1}\right)^{2}-a_{2}^{2}-d_{4}^{2}\)       (8)

θ₃는 다음과 같이 구할 수 있다.

\(\theta_{3}=\sin ^{-1} \kappa\)       (9)

여기서, \(\kappa=\frac{p_{x}^{2}+p_{y}^{2}+\left(p_{z}-d_{1}\right)^{2}-a_{2}^{2}-d_{4}^{2}}{2 a_{2} d_{4}}\)이고, \(\theta_{3}=\theta_{3}^{*}\) 또는 \(\theta_{3}=\pi-\theta_{3}^{*}\)로 2개의 해가 가능하다. 식 (4), (5)를 θ₂에 대하여 정리하면 다음과 같다.

\(\left[\begin{array}{cc} a_{2}+d_{4} s \theta_{3} & d_{4} c \theta_{3} \\ -d_{4} c \theta_{3} & a_{2}+d_{4} s \theta_{3} \end{array}\right]\left[\begin{array}{c} c \theta_{2} \\ s \theta_{2} \end{array}\right]=\left[\begin{array}{c} p_{x} c \theta_{1}+p_{y} s \theta_{1} \\ p_{z}-d_{1} \end{array}\right]\)       (10)

식 (10)으로부터 θ₂ 는 다음과 같이 구할 수 있다.

\(\theta_{2}=\operatorname{Atan} 2\left(s \theta_{2}, c \theta_{2}\right)\)       (11)

세 번째로 로봇 손목에 역기구학 해석을 수행한다. 로봇 손목에 대한 회전행렬 ³R₅은 다음과 같이 나타낼 수 있다.

\({ }^{3} R_{5}\left(\theta_{4}, \theta_{5}\right)={ }^{0} R_{3}{ }^{T}\left(\theta_{1}, \theta_{2}, \theta_{3}\right){ }^{0} R_{6}{ }^{5} R_{6}{ }^{T}\left(\theta_{6}\right)\)        (12)

위 식을 정리하면 다음과 같다.

\(\left[\begin{array}{ccc} c \theta_{4} c \theta_{5} & -s \theta_{4} & c_{\theta_{4}} s \theta_{5} \\ s \theta_{4} c \theta_{5} & c \theta_{4} & s \theta_{4} s \theta_{5} \\ -s \theta_{5} & 0 & c \theta_{5} \end{array}\right]=\left[\begin{array}{ccc} * & a_{12} & * \\ * & a_{22} & * \\ a_{31} & a_{32} & a_{33} \end{array}\right]\)       (13)

여기서 \(\boldsymbol{u}=\left[u_{x}, u_{y}, u_{z}\right]^{T},\) \(\boldsymbol{v}=\left[v_{x}, v_{y}, v_{z}\right]^{T}\), \(\boldsymbol{w}=\left[w_{x}, w_{y}, w_{z}\right]^{T}\)이고 \(a_{12}, a_{22}, a_{31}, a_{33}\)은 다음과 같다.

\(\begin{aligned} a_{12}=& s \theta_{6}\left(u_{x} c \theta_{1} c \theta_{23}+u_{y} s \theta_{1} c \theta_{23}+u_{z} s \theta_{23}\right) \\ &+c \theta_{6}\left(v_{x} c \theta_{1} c \theta_{23}+v_{y} s \theta_{1} c \theta_{23}+v_{z} s \theta_{23}\right) \\ a_{22}=&-s \theta_{6}\left(-u_{x} s \theta_{1}+u_{y} c \theta_{1}\right) \\ &-c \theta_{6}\left(-v_{x} s \theta_{1}+v_{y} c \theta_{1}\right) \\ a_{31}=& c \theta_{6}\left(u_{x} c \theta_{1} c \theta_{23}+u_{y} s \theta_{1} c \theta_{23}-u_{z} s \theta_{23}\right) \\ &-s \theta_{6}\left(v_{x} c \theta_{1} c \theta_{23}+v_{y} s \theta_{1} c \theta_{23}-v_{z} c \theta_{23}\right) \\ a_{33}=& w_{x} c \theta_{1} s \theta_{23}+w_{y} s \theta_{1} s \theta_{23}-w_{z} c \theta_{23} \end{aligned}\)       (14)

식 (13)의 (3,1), (3,3) 원소로부터 θ₅를 (1,2), (2,2) 원소로부터 θ₄를 다음과 같이 구할 수 있다.

\(\theta_{5}=\operatorname{Atan} 2\left(s \theta_{5}, c \theta_{5}\right)=\operatorname{Atan} 2\left(-a_{31}, a_{33}\right)\)       (15)

\(\theta_{4}=\operatorname{Atan} 2\left(s \theta_{4}, c \theta_{4}\right)=\operatorname{Atan} 2\left(-a_{12}, a_{22}\right)\)       (16)

한편, θ₆는 Fig. 2와 같이 z₄는 \(\overline{B P}\)와 직교조건 (\(\overline{B P} \cdot \boldsymbol{z}_{4}=0\))으로부터 수치적으로 구할 수 있다. θ₆값을 가정하면 P점을 구할 수 있고 로봇암에 대한 역기구학 연산을 통하여 \(\theta_{1}, \theta_{2}, \theta_{3}\)을 계산할 수 있다. 직교조건을 이용하여 θ₆만의 변수를 갖는 수식을 다음과 같이 얻을 수 있다.

\(\begin{aligned} &\overline{B P} \cdot \boldsymbol{z}_{4} \\ &=d_{4}\left[\begin{array}{c} c \theta_{1} s \theta_{23} \\ s \theta_{1} s \theta_{23} \\ -c \theta_{23} \end{array}\right] \cdot\left(s \theta_{6} \boldsymbol{u}+c \theta_{6} \boldsymbol{v}\right)=0 \end{aligned}\)       (17)

식 (17)의 직교조건은 식 (13)의 (3,2) 원소로부터도 구할 수 있다.

\(\begin{aligned} a_{32}=& s \theta_{6}\left(u_{x} c \theta_{1} s \theta_{23}+u_{y} s \theta_{1} s \theta_{23}-u_{x} c \theta_{23}\right) \\ &+c \theta_{6}\left(v_{x} c \theta_{1} s \theta_{23}+v_{y} s \theta_{1} s \theta_{23}-v_{z} c \theta_{23}\right) \\ =& 0 \end{aligned}\)       (18)

Fig. 3의 오른쪽 그래프에 θ₆에 대한 식 (17)의 연산값을 나타내었다. 직교조건을 만족하는 θ₆는 \(-n \pi \leq \theta_{6} \leq+n \pi\) 범위에서 일반적으로 2개가 존재하고 2개 해 중에서 이전 수치해와 근접한 θ₆의 해를 선택한다.

Fig. 3 Numerical analysis for θ₆

4. 자코비안 해석

직렬로봇의 속도 관계는 다음과 같이 구할 수 있다.

\(\dot{\boldsymbol{x}}=J \dot{\boldsymbol{\theta}}\)        (19)

\(\dot x\)와 \(\dot \theta\)는 각각 엔드이펙터의 속도벡터와 조인트 각속도 벡터이고 자코비안 행렬 J는 다음과 같이 정의할 수 있다.

\(J=\left[\begin{array} \boldsymbol{z}_{0} \times{ }^{0} \boldsymbol{p}_{6}^{*} \boldsymbol{z}_{1} \times{ }^{1} \boldsymbol{p}_{6}^{*} \ldots \boldsymbol{z}_{5} \times{ }^{5} \boldsymbol{p}_{6}^{*} \\ \quad z_0 \qquad \quad z_1 \qquad \qquad z_5 \end{array}\right]\)       (20)

여기서, \(\boldsymbol{z}_{i-1}={ }^{0} R_{i-1}{ }^{i-1}[0, 0, 1]^{T}\)이고 \(i-1_{\boldsymbol{p}_{6}}^{*}\) 는 기준 좌표계에서 표현된 i-1번째 조인트 좌표계 의 원점에서 6번째 조인트 좌표계 원점까지의 위치벡터이다. 제안하는 offset wrist를 갖는 협동로봇의 특이점을 구하기 위해 자코비안 행렬식 det(J)는 다음과 같이 구할 수 있다.

\(\begin{aligned} \operatorname{det}(J)=& a_{2} d_{4} d_{5} c \theta_{2} c^{2} \theta_{3} s \theta_{4} s \theta_{5}-a_{2} d_{4}^{2} c^{2} \theta_{3} s \theta_{2} s \theta_{5} \\ &-a_{2} d_{4}^{2} c \theta_{2} c \theta_{3} s \theta_{3} s \theta_{5}-a_{2}^{2} d_{5} c \theta_{2} s \theta_{3} s \theta_{4} s \theta_{5} \\ &-a_{2} d_{4} d_{5} c \theta_{2} s \theta_{4} s \theta_{5}-a_{2}^{2} d_{4} c \theta_{2} c \theta_{3} s \theta_{5} \\ &+a_{2}^{2} d_{5} c \theta_{2} c \theta_{3} c \theta_{4} c \theta_{5} s \theta_{4} \\ &-a_{2} d_{4} d_{5} c \theta_{3} s \theta_{2} s \theta_{3} s \theta_{4} s \theta_{5} \end{aligned}\)       (21)

여기서, det(J)=0인 경우, 특이점이 발생하므로 로봇은 해당 위치 및 자세를 회피해야 한다. det(J)=0인 offset wrist를 갖는 로봇의 대표적인 특이점 형상을 Fig. 4에 나타내었다. 여기서, \(\beta=0 \text { or } \pm \pi, \gamma=\pm \pi / 2\)이다. wrist center를 갖는 로봇과 같이 조인트 각도 1개가 특정한 조건(\(\theta_{3}=\beta\) or \(\theta_{5}=\beta\))을 만족할 때 특이점은 존재하지 않지만, offset wrist 로봇은 다수의 조인트 각도들의 조합에 대한 특이점이 존재하는 것을 알 수 있다.

Fig. 4 Typical singular configurations

5. 역기구학 연산속도

수치적 알고리즘을 포함하는 역기구학 알고리즘의 실시간 연산성을 검증하기 위하여 엔드 이펙터 이동방향에 따른 역기구학 알고리즘 연산 TET(Task Execution Time)를 실시간 제 어기에서 실험하였다. 역기구학 알고리즘을 실시간 제어기인 xPC Target에 적용하여 Fig. 5 와 같이 TET를 연산하였다. 여기서, 사용된 PC 사양은 Intel Atom 1.86GHz dual core이 다. Fig. 5와 같이 엔드이펙터를 각각 x, y, z축 을 따라 직선운동하는 경우, x, y, z축에 대하여 회전운동하는 경우와 Fig. 6과 같이 복합운동하는 경우의 TET를 순차적으로 연산하였다.

Fig. 5 TET of the inverse kinematics on the real-time controller for the specified motions

Fig. 6 Trajectory for combined motions

각 축방향의 운동 범위는 다음과 같다.

\(\begin{gathered} |\Delta x|<100,|\Delta y|<50,|\Delta z|<50 \\ 0^{\circ}<\Delta \theta_{x}, \Delta \theta_{y}, \Delta \theta_{z}<90^{\circ} \end{gathered}\)

복합운동은 Fig. 6과 같은 20개의 위치에 대하여 자세도 10º간격으로 변화하면서 계산하였다.

Fig. 5의 TET와 같이 역기구학 연산 TET 는 엔드이펙터 이동방향에 관계없이 일정함을 알 수 있고 최대 0.4msec임을 알 수 있다. 실시간 제어기의 제어 루프는 1msec이므로 수치적 알고리즘을 포함하는 역기구학의 실시간 연산성을 검증하였다.

6. 결론

본 논문에서는 가반하중 증가 및 고속이동이 가능한 offset wrist를 갖는 협동로봇의 역기구학 해석방법을 제시하였다. θ₆만의 변수를 포함하는 비선형 방정식의 수치적 해석으로 θ₆를 구하면 나머지 조인트 각도들은 로봇 암과 손목 기구로 나누어 해석적으로 구할 수 있는 역기구학 알고리즘을 제시하였다. 또한, 자코비안의 행렬식(det(J))을 이용하여 특이성을 해석하였고 offset wrist 로봇의 경우 조인트 각도 1개가 특정한 조건을 만족하는 경우의 특이점은 존재하지 않지만, 다수의 조인트 각도들의 조합에 대한 특이점이 존재함을 제시하였다. 수치적 방법을 포함하는 역기구학 알고리즘의 실시간 연산성을 실시간 제어기에서 검증하였다. 향후 연구로 offset wrist를 갖는 협동로봇의 충돌감지 및 직접교시에 대한 연구를 수행하고자 한다.

감사의 글

본 논문은 중소벤처기업부의 월드클래스 300프로젝트 R&D지원사업(과제번호: S26 41371)을 통해 개발된 성과이고 이에 관계자 여러분께 감사드립니다.