포크레인용 다관절 매니퓰레이터 어태치먼트 운동학 해석 및 모션제어

A Study on Motion Control and Kinematics Analysis of Articulated Manipulator Attachment for Excavator

Abstract

In this paper, it is proposed a new approach to motion control and kinematics analysis of articulated manipulator attachment with five degree of freedom for excavator. Unlike the well-established theory for the control of linear systems, there is little general control theory relatively for a robust control of nonlinear systems. The control technique is essential for providing a stable and robust performance for application of articulated manipulator control. The proposed control algorithm is one of robust control methods based on error informations of the position and velocity error informations using stability analysis of dynamic model. Through simulation test, the proposed control scheme is illustrated to be a efficient control technique for real-time control.

1. 서론

본 논문에서 채택한 제어 기법은 컴퓨터 토오크 제어 기법에서 부하 변동이나 알 수 없는 외란에도 변화하는 관성이나 마찰항 등의 파라미터변화에도 유연하게 적응할 수 있는 제어 기법을 사용하고 있다. 비선형 동적 시스템을 표현하고 제어할 수 있는 비선형시스템에 적용할 때 요구되는 것들중에 하나는 공학적인 문제에 직면하게 되는 동적 시스템을 얼마나 정확하게 모델링 하는가 하는 것이다. 그리고 동적 시스템의 입 ⋅ 출력 변수를 어떻게 선정 할 것이냐 하는 문제가 따르게 된다. 일반적으로 동적 시스템 거동을 근사화시키는 작업을 하기 위하여 폐루프제어 방식에 의한 피드백 에 의한 가중치 보정이 필요하다는 것은 자명한 사실이다. 학습제어에 관한 학회지에서 그러한 네트워크는 오차 역전파라 불리우는 세포 모델에 의한 학습방법을 제안하고 있다. 따라서, 학습제어 방식의 입출력 관계는 가중치와 관련된 파라미터의 조합의 값으로 결정된다. 위에서 언급한 네트워크의 능력은 바로 동적 시스템의 거동을 표현할 수 있는 가중치의 조합이 존재하는냐에 달려있다고 해도 과언이 아니므로, 고부하가 작용하는 산법 현장에의 적용에는 많은 한계점이 존재하고 있다고 할 수 있다.. [1]

본 논문에서 제안하는 제어기는 오부환경의 외란 및 고부하 하중이 작용하는 상황에서도 강인한 제어기법이라 할 수 있는 게산 토오크 방법 (computed torque method)을 기본구조로 하는 다관절 매니풀레이터 제어기를 기본구조로 하는 제어알고리즘을 개발하고, 이를 바탕으로 안정된 제어입력을 공급할 수 있는 제어기를 설계하고, 외부외란에 능동적으로 적응되면서 항상 우수한 제어성능이 유지될 수 있는 견실 제어기를 구성하여, 5축 다관절 매니퓰레이터의 위치 및 속도제어에 적용함으로써 그 효율성을 입증한다.

2. 매니퓰레이터 어태치먼트 운동 해석

매니퓰레이터는 [Fig 1] 에서 보는 바와 같이 5개의 회전관절로 이루어졌다.

Fig. 1 The structure of articulated manipulator attachment

Table 1. The Link Parameters

작업 로봇의 정기구학 방정식은 주어진 조인트각 θ_1∼6에 근거한 동차변환행렬 ⁰T₆이다. du여기서 6번째 Joint는 회전관절l 아닌 작업 tool에 대한 정보를 나타내기 위한 것이다. 따라서, ⁰T₆를 구하기 위해서 다음과 같이 각 링크 사이의 관계를 이용하여 계산할 수 있다.

^{i -1} T_i = T(z,d)T(z,θ) T(x,a)T(x,α)

for i = 1,2,3,4,5,6

#

⁰T₆ (θ₁,θ₂,θ₃,θ₄,θ₅,θ₆ )

= ⁰T₁(θ₁) ¹T₂(θ₂)²T₃(θ₃)³T₄(θ₄)⁴T₅(θ₅ )⁵T₆(θ₆ )       (2-1)

여기서 ⁵T₆ 는 툴(tool)의 좌표변환을 나타낸다. 동차변환행렬은 다음과 같이 정의된다.[2]

여기서 sin(θ₂ +θ₃ +θ₄ )는 s_234, cos(θ₂ +θ₃ +θ₄ )는 c₂₃₄ 로 나타냈다.

#

#(2-2)

역기구학 방정식은 동차변환행렬 ⁰T₆으로 지정된 위치와 방향을 기준으로, 조인트 각 θ_1∼6을 구하는 것이다. 여기서 모든 관절의 운동범위 각도를 150도이내로 제한한다. 먼저 θ₁을 찾으려면 먼저 0번 좌표계에서 바라본 P점의 위치 ⁰p를 결정한다. Fig.2 에서 볼 수 있듯이 ⁰p는 Q점에서 P 점으로 z₆을 따라 역으로 변환하여 찾을 수 있다. Q점과 tool 정보 d₆ 모두 알려진 값이다.[3]

Fig. 2 The kinematics structure of joints

⁰p 는 다음과 같이 작성할 수 있다.

⁰p=⁰q-d₆⁰w

#(2-3)

⁰p : 0번 좌표계에서 바라본 P점의 위치벡터

⁰q : 0번 좌표계에서 바라본 Q점의 위치벡터

⁰w : 0번 좌표계에서 바라본 z₆ 단위방향벡터

그리고 각도 Φ₁ 과 Φ₂를 구한다. Φ₁는 변이 ⁰p_x와 ⁰p_y인 삼각형을 계산하여 찾는다.

 Φ₁ = Atan2(⁰p_y , ⁰p_x )       (2-4)

 Φ₂는 변이 d₄와 #인 삼각형을 계산하여 찾는다.

 #

#

#(2-5)

Φ₂는 두 가지 솔루션이 존재하게 되는데, 상기 두 솔루션은 “Shoulder left”와 “Shoulder right”에 해당한다. 각도 θ₁은 다음과 같이 구할 수 있다.

θ₁ = Φ₁ -Φ₂       (2-6)

θ₅는 0번 좌표에서 바라본 Q점(⁰q)을 통해서 계산할 수 있다.

#

#

#(2-7)

그리고 ¹q_z = d₄ +d₆cosθ₅으로 식(7)과 연립하여 θ₅를 계산할 수 있다.

#(2-8)

이때, θ₅는 두 가지 솔루션을 가지며 “bucket up”과 “bucket down” 형상으로 나타난다.

관절 좌표가 아닌 tool 좌표정보를 나타내는 θ₆를 결정하기 위해서는 6번 좌표에서 바라본 z₁을 분석한다. [Fig.3]와 같이 축은 이동을 무시하고 회전만을 고려하면, 이에 대한 좌표계(coordinate systems)가 다음과 같이 표현될 수 있다.

Fig. 3 The 3D coordinates of revolution joint

#(2-9)

#(2-10)

식(2-9)와 식(2-10)는 같고 sinθ₅ ≠ 0 (θ₅ ≠ 0 or π) 일 때, θ₆의 솔루션이 결정된다.

θ₆ = Atan2(sinθ₆, cosθ₆ )

#(2-11)

θ₆를 정의함으로 나머지 각 θ₂,θ₃,θ₄는 3축 평면 로봇 역기구학 해석과 같으므로 손쉽게 계산할 수 있다.[5]

⁰T₆ = ⁰T₁ (θ₁ )¹ T₄ (θ₂,θ₃,θ₄ )⁴ T₅(θ₅ )⁵T₆ (θ₆ )

⁰T₁^-10T₆ (⁴T₅⁵T₆) ^-1 = ¹T₄ (θ₂,θ₃,θ₄)

#(2-12)

도출한 ¹T₄의 위치값인 ¹r_x과 ¹r_y를 제곱 후 더하여 θ₃을 식(2-13)과 같이 구한다.

#

θ₃=±cos^-1k

#(2-13)

여기서도 θ₃는 두 가지 솔루션을 가지게 되데, “Elbow up”과 “Elbow down”에 해당한다. θ₂는 앞서 구한 θ₃와 ¹r_x, ¹r_y를 이용하여 계산한다.

(a₂ +a₃cθ₃)cθ₂ - (a₃sθ₃)sθ₂ = ¹r_x

(a₃sθ₃)cθ₂ - (a₂ +a₃cθ₃ )sθ₂ = ¹r_y       (2-14)

#

#

#

θ₂ = Atan2(sinθ₂,cosθ₂)       (2-15)

마지막 남은 각도 θ₄는 θ₂와 θ₃를 이용하여 계산할 수 있다.

θ₄ = θ₂₃₄ - (θ₂ +θ₃)

θ₂₃₄ = Atan2(¹x_4y , ¹x_4x )       (2-16)

3. 매니퓰레이터 어태치먼트 모션제어

3.1 매니퓰레이터의 관절속도

다관절 매니퓰레이터의 관절속도를 구하기 위하여 링크 I 위의 임의의 고정된 점을 ⁱr_i 이라고 정의 하면, 점 ⁱr_i 은 ⅰ번째 링크 좌표계에 대하여 다음과 같은 동차좌표(homogeneous coordinate) 로 표현할 수 있다.[5]

#(2.17)

링크 ⅰ위의 점 ⁱr_i 을 기준좌표계(base coordinate frame)에 대하여 표시하면 다음과 같이 표현될 수 있다.

^or_i = ^oA_i ⁱr_i       (2.18)

여기서 ^or_i 는 기준좌표계에 대한 링크 ⅰ위의 고정된 점 ⁱr_i의 상대적 위치를 나타내고 있고, ^oA_i는 ⅰ번째 링크좌표계와 기본좌표계 사이의 관계를 나타내는 동차좌표변환행렬로서 다음과 같이 정의된다.

^oA_i = ⁰A₁ ¹A₂ ... ^i-1 A_i       (2.19)

일반적으로 관절 ⅰ의 관절 변수는 회전관절의 경우에는 θ_i 가 변수이고 미끄럼관절의 경우에는 d_i 가 변수 이므로 회전관절 및 미끄럼관절에 대하여 동시에 적용 가능한 동적방정식을 유도하기 위하여 관절 ⅰ에 대한 일반화된 좌표(generalized coordinate)를 나타내는 변수를 q_i로 정의하면, 기준 좌표계에 대하여 표시된 링크 ⅰ의 점 ⁱr_i 의 속도는 다음과 같이 표현될 수 있다.

#(2-20)

위의 식 (2.20)에서의 ^oA_i 의 q_i 에 대한 편미분 (ə^oA_i /əq_i) 은 회전관절의 경우 (q_i = θ_i )를 고려하면, 식 (2.17)의 θ_i 에 대한 미분으로부터 다음과 같은 관계식을 얻을 수 있다.[4.,5]

그러므로 식 (2.9)를 식 (2.5)에 대입하면, 링크ⅰ위의 점 ⁱr_i 의 속도는 다음과 같이 표현된다.

#(2.21)

3.2 매니퓰레이터의 운동에너지

K_i 를 링크 ⅰ의 운동에너지라고 정의하면, 링크 ⅰ위의 미소질량 dm_i의 운동에너지 dK_i 는다음 식과 같이 표현된다.

#(2.22)

위의 식 (2.21), (2.22)로부터 식 (2.22)의 미소 질량 dm_i의 운동에너지는 다음과 같이 표현될 수 있다.

#(2.23)

링크 ⅰ의 전체 운동에너지는 다음과 같이 표현된다.

#(2.24)

그러므로 식 (2.24)을 정리하여, # 정의되는 관성텐서(inertia tensor)를 사용하면, J_i는 다음의 식으로 표현될 수 있다.

여기서 ⁱr_i = (x_i , y_i , z_i , 1)는 ⅰ번째 링크 좌표계로부터의 링크 ⅰ의 질량중심벡터를 나타낸다. 위의 식 (2.24)에서 기본좌표계에 대한 각 링크의 회전반경을 사용하면 J_i는 다음과 같이 정리된다.

#(2.25)

여기서 K_i23는 yz축에 대한 링크 ⅰ의 회전 반경을 나타낸다.

그러므로 로봇 매니퓰레이터의 전체 운동에너지는 다음과 같다.

#(2.26)

위의 식에서 마지막 항은 액추에이터에 의한 운동에너지를 나타내고, I_ai는 액추에이터의 관성을 나타내고 있다.

3.3 매니퓰레이터의 위치에너지

매니퓰레이터의 링크 ⅰ의 위치에너지를 P_i로 정의하면, 링크 ⅰ의 위치에너지 P_i 는 다음과 같이 정의된다.

P_i = -m_i g ^o r_i = -m_i g (^o A_i ⁱr_i) ,i = 1,2,...,n       (2.27)

로봇 매니퓰레이터의 전체 위치에너지는 다음과 같이 표현된다.

#(2.28)

여기서 g^T = (g_x , g_y , g_z , 0)^T는 기준좌표계에서 표현되어지는 중력벡터이며, ⁱr_i = (x_i , y_i , z_i , 1)^T는 링크 ⅰ의 질량중심 벡터를 나타낸다.

3.4 매니퓰레이터의 운동방정식

로봇 매니퓰레이터의 운동방정식을 구하기 위하여 Lagrange-Euler의 운동방정식을 기술하면 다음과 같이 표현된다.

#(2.29)

여기서 q_i 는 로봇의 관절변수를 나타내고, τ_i 는 로봇 시스템에 인가된 힘(혹은 토크)을 나타내며, L은 매니퓰레이터의 운동에너지와 위치에너지의 차이로 정의되는 라그랑지안(Lagrangian)을 나타낸다. 그러므로 n 자유도를 갖는 로봇 매니퓰레이터에 적용하면, 식 (2.28), (2.29)로부터 라그랑지안 L은 다음과 같이 표현된다.

#(2.30)

따라서 식 (2.31)으로부터 다음의 관계식을 얻을 수 있다.

#(2.31)

그리고 식 (2.22)을 시간에 대해 미분하면 식 (2.20)의 우변의 첫째항은 다음과 같이 표현된다.

#(2.32)

식 (2.32)에서 우변의 두 번째 항은 식 (2.31)으로부터,

#(2.33)

으로 표현된다. 그리고 식 (2.32)과 (2.33)으로부터 다음의 관계식이 정의된다.

#(2.34)

그러므로 로봇 매니퓰레이터의 동적방정식은 식 (2.34)로부터 다음 식으로 표현된다.

#(2.35)

식 (2.35)의 로봇 매니퓰레이터의 동적방정식을 행렬식으로 표현하면 다음 식으로 정의된다.

#(2.36)

여기서 D_ij는 symmetric positive definite 관성행렬 (n×n)을 나타내고, C_ijk 는 원심력과 코리올리스력에 의한 비선형행렬 (n×1)을 나타내며, G_i는 중력하중행렬 (n×1)을 나타내고 있는데, 각각 다음과 같이 정의된다.

#

3.5 제어알고리즘 개발

본 연구애서는 컴퓨터-토크 제어알고리즘 기법을 적용하여 파라미터 즉, 관성모우멘트나 코리울리 원심력항을 추정해주는 갱신 법칙을 부가시킴으로써 파라미터 오차벡터가 제로가 되도록 하여 점근적인 안정을 꾀하려는 것이다. 본 논문에서 접근한 적응제어는 질량이나 마찰계수와 같은 것을 추정하는 것이 아니기 때문에 정확한 제어 플랜트의 모델링을 요구하지 않는 다는 것이 큰 특징이라 할 수 있다. [12]

다음에 나오는 식이 로봇 동역학과 서보 모터와 기어비로써 결합된 최종식을 다음과 같다.

#(3-1)

여기서, R(#)는 알려진 시간함수의 n×r 행렬이고, ν는 알 수 없는 파라미터 벡터이며, H, V _m , G, F는 각각 관성, 코리올리 원심력, 중력, 그리고 마찰항을 나타낸다.[13]

#(3-2)

적응 제어 토크는 다음과 같다.

#(3-3)

추적오차의 정의로부터 다음과 같이 쓸 수 있다.

#(3-4)

(3-1)식과 (3-4)식으로 부터 다음과 같이 쓸 수 있다.

#(3-5)

여기서, #은 알 수 없는 파라미터의 시변추정을 나타내는 n×1 벡터이다.

오차 방정식의 형태로 식을 정리하면 다음과 같이 된다.

#(3-6)

파라미터 오차는 다음과 같이 정의한다.

#(3-7)

오차 방정식을 상태 공간 형태로 다시 쓰면 다음과 같은 형태로 된다.

#(3-8)

추종 오차 벡터는 다음과 같이 정의한다.

#(3-9)

#(3-10)

다음은 Lyapunov 안정성 해석을 통하여 추종 오차 벡터 e가 점근적인 안정을 하도록 하는 적응 제어칙을 선택하기 위하여 양의 한정인 리아프노브 함수를 다음과 같이 선정한다.[14]

#(3-11)

여기서, P는 2n×2n의 양의 한정 상수 대칭행렬이고, Γ는 대각이며, 양의 한정인 r×r 행렬이다.

#(3-12)

리아프노브 함수를 시간에 대하여 미분함으로써 다음과 같이 된다.

#(3-13)

식(3-8)를 식(3-13)에 대입하여 정리하면 다음과 같이 된다.

#(3-14)

전치의 특성을 이용하여 항별로 조합하면 다음과 같은 식이 표현된다.

#(3-15)

여기서, Q는 리아프노브 함수를 만족하는 양의 한정 행렬이다.

A^TP+PA=-Q       (3-16)

#는 항상 최소한 음의 반한정이어야 안정하다는 사실에서 적응 갱신 법칙을 다음과 같이 유도 할 수 있다.

#(3-17)

#(3-18)

#가 0이기 떄문에 파라미터 추종 벡터 #를 적응갱신 법칙에 대입하면 다음과 같은 최종 제어칙을 정의할 수 있다.

#(3-19)

#는 (A > 0) 항상 음이면 오차와 접근적으로 영으로 간다.

4. 시뮬레이션

본 연구에서는 다관절 매니퓰레이터 어태치먼터의 관절 운동제어의 정밀도 및 신뢰성을 입증하기 위하여 컴퓨터 시물레이션을 통하여 그 성능을 예증하였다. 시뮬레이션 방법은 Matlab과 같은 제어 언어구조에 기반한 CEMTOOL을 이용하여 부하변동에 대한 성능 실험을 수행하고, 그 결과를 비교 분석하였다. Fig.4는 2%의 부하하중에 대한 관절 1과 관절2에 대한 위치 및 속도 궤적제어 정밀도를 실험한 결과를 나타내고 있다. 실험결과 부하하중이 존재하여도 매우 우수한 제어성능을 보여주고 있다.

Fig. 4 The control performance of neuro controller for the position trajectory of Link1 and Link2 (payload 2%)

Fig.4는 2%의 부하하중에 대한 관절 1과 관절2에 대한 위치 및 속도 궤적제어 정밀도를 실험한 결과를 나타내고 있다. 실험결과 부하하중이 존재하여도 매우 우수한 제어성능을 보여주고 있다. Fig.5는 4%의 부하하중에 대한 관절 1과 관절2에 대한 위치 및 속도 궤적제어 정밀도를 실험한 결과를 나타내고 있다. 실험결과 부하하중의 증가에도 매우 강인한 제어성능을 보여주고 있다. Fig.6과 Fig.7은 각각 부하하중이 6% 및 8%의 부하하중 증가에 대한 관절 1과 관절2의 위치 및 속도 궤적제어 성능실험결과를 나타내고 있다. 성능실험결과 부하하중을 6% 및 8%로 증가하여도 부하하중에 아주 견실한 제어성능을 보여주고 있다. 그리고 Fig.8은 10%의 부하하중에 대한 관절 1과 관절2에 대한 위치 및 속도 궤적제어 정밀도 성능실험의 결과를 나타내고 있다. 실험한 결과 부하하중이 증가하여도 부하하중에 매우 강인한한 제어성능을 보여주고 있음을 확인할 수 있다.

Fig. 5 Control performance of neuro controller for the position trajectory of Link1 and Link2 (payload 4%)

Fig. 6 Control performance of neuro controller for the position trajectory of Link1 and Link2 (payload 6%)

Fig. 7 Control performance of neuro controller for the position trajectory of Link1 and Link2 (payload 8%)

Fig. 8 Control performance of neuro controller for the position trajectory of Link1 and Link2 (payload 10%)

5. 결론

본 논문은 포크레인구조 5자유도 다관절 매니퓰레이터 운동학 해석 및 모션제어 설계 방법을 제시하였다. 논문에서는 기존의 다관절 시스템의 모션제어기법에서 많이 적용한 외란에 대한 학습 능력이 우수한 지능제어밥법의 장점을 충분히 살리려는 시도보다는 고부하의 하중에 강인하고 정밀하게 적응할 수 있는 계산토크기법(computed torque method)을 적용한 프레인 구조의 다관절 매니퓰레이터 어태치먼트의 제어방법을 제시하였다. 포크레인용 다관절 어태치먼트는 최고 3.5톤의 고하중의 부하가 걸리는 구조이므로 미세한 정밀도 보다는 반복적인 고하중에 강인한 제어특성을 유지할 수 있는 장점을 이용하여 견실하고 실시간 제어에 적합한 제어성능이 유지됨을 모의실험을 통하여 입증하였다.