폐루프 역기구학(Closed-Loop Inverse Kinematics, CLIK) 은 로봇의 각 관절 속도(q˙)와 말단 장치의 위치 및 방위 속도(x˙)와의 선형 관계식인 자코비안 행렬을 이용한 제 어 기법 중 하나이다^[14-16]. 이 절에서는 본 논문에서 특이 점 회피 알고리즘을 구현하기 위해 사용한 로봇 제어기 인 폐루프 역기구학에 대해 간단히 소개한다.

우선 n 자유도 로봇의 관절변수 벡터와 말단 장치의 위치 및 방위를 각각 q=[q1q2⋯qn]∈Rn 와 x=[x1x2⋯xm]∈Rm 로 정의하자. 또한, 로봇 제어 시스템에 대한 가정 으로, (A1) 로봇의 제어시스템은 연속적이고, (A2) 로봇 의 기구적 물성치(Kinematic Property)를 이상적으로 알고 있다고 가정한다. 그렇다면, Jk=∂ xk∂ q 의 관계식인 자코 비안 행렬 J(q)∈Rm×n 을 이용하여 각 관절 속도에서 말단 장치의 위치 및 방위 속도를 계산하는 정기구학 (Forward Kinematics)을 표현할 수 있다.

한편, 식 (1)과 상응하는 역기구학(Inverse Kinematics)은

로 표현할 수 있다. 식 (2)에서 J∗(q)∈Rn×m 의 역연산자(Inverse Operator)로, 일반적으로 관절의 수(n) 가 말단 장치의 제어 자유도(m)보다 큰 여유자유도 시스 템(Redundant System)의 경우, 식 (3)과 같이 우-의사역 행렬(Right Pseudo Inverse, PI)로 표현한다.

수학적으로 우-의사역행렬은 각 관절 에너지의 합의 최소값을 도출하여 주며, 로봇이 특이점(Singular Point) 에 위치하지 않는 경우에 한해 항상 존재함이 증명되었 다^[3,14].

결과적으로 식 (2)와 (3)을 통하여 말단 장치의 속도를 이용해 관절들의 각속도를 구할 수 있음을 알 수 있다. 하지만, 실제 로봇을 식 (2)와 (3)을 이용하여 제어하기에 는 무리가 있다. 이는 앞서 언급한 가정들(A1, A2)이 실 제 시스템에서는 적용되지 않기 때문에 말단 장치의 추종 오차(Tracking Error, e = x_d - x)가 발생하기 때문이다.

이러한 추종 에러를 줄이기 위하여, 폐루프 역기구학 에서는 식 (2)에 추종 에러에 비례하는 피드백(Feedback) 을 추가한다. 이는 다음의 식 (4)와 같이 표현 가능하다.

식 (4)에서 x_d는 말단 장치의 목표 위치 및 방위를 의 미하고, K=kI∈R^m는 피드백 게인 행렬(Feedback Gain) 로 대각양행렬(Diagonal Positive-definite Matrix)이다. k 의 크기는 추종 에러의 수렴 속도와 관계가 있다. 구체적 으로, k의 크기가 커지면 커질수록 추종 에러의 수렴속 도가 빨라짐을 알 수 있다. 하지만 너무 큰 k의 값은 말단 장치의 진동을 유발할 수 있다. 따라서 k의 값은 실험을 통해 말단 장치의 진동을 유발하지 않으면서 최 적의 추종 에러를 빠르게 줄일 수 있는 값으로 설정한다. 결과적으로, 식 (4)를 이용한 폐루프 역기구학의 블록 선도(Block Diagram)는 Fig. 1과 같다.

Fig. 1 

Block diagram of closed-loop inverse kinematics. F.K means forward kinematic eR^n×nuation to obtain end-effector’s position and orientation

특이값 분해(Singular Value Decomposition, SVD)는 행렬을 다음과 같은 3개의 요소 행렬로 분해하여 행렬의 특이값을 찾는 방법으로,

U ∈R^m×m과 V∈R^n×n 는 직교행렬(Orthogonal Matrix) 이며, S ∈R^m×n은 대각선상의 원소들을 제외한 원소들 이 0인 행렬로 자코비안 행렬과 같이 행렬의 행이 열보 다 큰 경우 (m > n), 식 (6)과 같이 표현할 수 있다.

식 (6)에서 σ를 특이값(Singular Value)라 부르고, 0이 아닌 특이값의 개수(r=max(i∣σ_i > 0))는 본행렬(J)의 랭크와 같다. 그러므로, 식 (5)는 다음과 같은 벡터 형태 로 표현이 가능하다.

위의 식에서 u_i와 υ_i는 각각 U와 V의 i번째 열을 의미 한다.

한편, 식 (5)와 (7)은 U와 V가 직교행렬이라는 성질을 이용하여, 쉽게 식 (8)과 같은 의사역행렬(Moore-Penrose Pseudo Inverse)을 구할 수 있다.

식 (8)의 의사역행렬은 특이값(σ)의 역수형태를 포함 하고 있기 때문에, i번째 특이값(σ_i)이 0과 가까워지면 가까워질수록, 의사역행렬의 요소들의 값이 커짐을 알 수 있다(limσi→01σi=∞). 즉, 로봇이 특이점에 가까이 위 치할 경우, 자코비안행렬의 특이값이 작아지게 되고, 이 는 J^*의 크기의 발산을 야기하기 때문에, 식 (4)의 폐루 프 역기구학 제어를 방해하는 것이다.

로봇의 자세가 특이점 근처에 있는지 판단하는 방법 으로는 크게 조작성 지수(Manipulability Measure)과 조 건수(Condition Number)가 쓰이고 있다^[5,23-24].

먼저, 조작성 지수는

와 같이 표현한다. 식 (9)에서 보듯이, 로봇이 특이점 자 세에 가까워질수록 조작성의 값은 작아지며, 로봇이 완벽 한 특이점 자세에 돌입한다면 조작성 지수는 0이 된다.

한편, 조건수(κ)는 자코비안 행렬의 가장 큰 특이값 (σ_max )과 0이 아닌 가장 작은 특이값(σ_max )의 비로 나타 낼 수 있으며 이를 식으로 표현하면 다음과 같다.

그러므로, 조건수는 조작성과 반대로 특이점 근처에 서 큰 값으로 증가하여 발산하게 된다.

Damped Pseudo Inverse 방법은 Damped Least Squares 혹은 Singularity Robust Pseudo Inverse라고도 말하며, 식 (7)에서의 특이값에 조정항(λ)을 추가하여 의사역행 렬이 항상 존재하도록 하는 방법을 말한다^[6,21].

식 (11)과 (12)의 형태에서 알 수 있듯이, JD∗ 의 특이값 은 σn이 0에 가까워진다 하여도 발산하지 않고 0으로 수렴함을 알 수 있다(limσn→0σnσn2+λ2=0). 또한 JD∗의 조건 수는 식 (13)과 같으며, 식 (10)과 달리 로봇이 특이점에 위치해 있어도 항상 수렴함을 알 수 있다.

이처럼, Damped Pseudo Inverse의 조정항(λ)은 제어 의 특이점을 극복하는 요소이지만, 로봇의 정밀한 움직 임 제어를 방해하는 요소이기도 하다. 그러므로, 일반적 인 로봇 제어에서의 조정항은 로봇의 움직임을 방해하 지 않으면서 특이점을 극복할 수 있게 작은 크기(σn ≫λ) 를 넣어주거나^[5], 식 (14)와 같이 조작성 지수(w)를 이용 하여 조정항의 크기를 조절하는 방법(이하 DPI*)을 사용 한다^[21].

식 (14)에서 w₀와 λ_m은 각각 조작성 지수에 대한 임계 값(Threshold)과 조정항의 스케일링 게인(Scaling Gain) 이다.

한편, Sugihara는 조정항의 크기를 로봇 말단장치의 현재 위치와 목적 위치와의 추종 오차(e)로 크기를 정하는 Error Damped Pseudo Inverse(E-DPI)를 제안하였다^[8,17].

식 (15)에서 ζ는 12eTe 로 추종 오차의 내적에 비례하 며, W∈R^n×n 는 w_i를 대각성분의 원소로 가지는 대각 행렬이다. 이 때, w_i는 i번째의 관절을 포함하는 로봇 링크의 길이를 적당한 비율로 나눈 값으로, 실험적으로 링크 길이의 1000분의 1배로 대입한다^[17]. 그러므로, Error Damped Pseudo Inverse의 조정항은 각 관절마다 조정항 의 크기가 다르게 되므로, 제어 수렴 시간이 일반적인 Damped Pseudo Inverse보다 단축될 수 있다.

Jacobian Transpose 방법은 폐루프 역기구학 식인 식 (4)의 우-의사역행렬(J^*) 대신 자코비안의 전치행렬(J^T) 을 대입하여 제어하는 것을 말한다^[7,25].M16

자코비안의 전치행렬로 대신했을 때 기대할 수 있는 장점으로는 크게 다음과 같다. 먼저, 자코비안 행렬과 같은 정기구학 정보만을 이용하므로 특이점이 존재하지 않는다. 또한, 제어 과정 중, 특이값 분해와 같은 연산 시간이 무거운 계산이 포함되지 않아 고속 제어에 유리 하다. 마지막으로, 르야프노프 이론(Lyapunov Theory) 을 통해 연속시스템에서의 안정성(Stability)가 검증되었 다^[26]. 하지만, 자코비안 역행렬 대신 자코비안의 전치행 렬을 사용하기 때문에, 추종 오차가 다른 알고리즘보다 크게 발생하며, 제어 주기가 낮거나 외란(Disturbance)이 발생하는 경우에서는 말단 장치의 제어 오차로 인해 로 봇이 진동하는 현상(Chattering)이 발생할 수 있다는 단 점이 있다.

이를 극복하기 위해, Lee등은 식 (17)과 같이 자코비안 의 전치행렬에 게인 대각행렬(D∈R^m×m)을 곱하여 행 렬의 크기를 낮춰주는 Scaled Jacobian Transpose(SJT) 를 제안하여, 고속 제어에 유리하면서 진동 현상을 최소 화하고자 하였다^[27].

이 때, D의 i번째 대각성분 는 Jacobian 행렬의 i번째 열의 크기의 역수로 설정하며 이를 식으로 표현하면 식 (18)과 같다.

식 (4)과 (8)에서 보듯이 특이점 근처에서 로봇의 자코 비안의 역행렬은 발산하므로 적은 추종 오차(e)가 많은 관절 각도 변화를 야기함을 알 수 있다. 이 점을 이용하 여 Selectively Damped Pseudo Inverse는 자코비안의 고 유 벡터(u_i)를 이용한 고유 벡터에 따른 관절 각도의 변화 량을 계산한다^[9]. 이는 식 (5)와 (7)에서 보듯이, i번째 고유 벡터는 i번째 특이값을 반영하는 움직임을 나타내 기 때문이다^[30]. 이 때 계산된 관절 각도의 변화량이 로봇 관절의 허용 각속도보다 클 경우, 관절 각도의 변화량을 제한한다. 그러므로, 로봇이 특이점 근처에 있을 경우 특이점 방향으로의 움직임이 제한된다. 구체적인 알고 리즘의 전개과정은 다음과 같다.

먼저, 2.1절의 일반적인 의사역행렬을 사용할 때, 위치 오차에 따른 관절 각 변화율은 식 (19)과 같다.

또한, 자코비안 행렬이 0에 가까운 특이점을 포함하지 않는다면, 다음의 조건을 만족하게 된다.

이 때, 만약 위치 오차가 자코비안 행렬의 고유 벡터라 면(e=ui, uiTui=1), i번째 고유벡터에 의한 j번째 관절각 의 순간 변화량(q˙j)은 다음과 같이 간단해진다.M21

이 때, υ_j,i는 υ_i의 j번째 성분을 의미한다. 따라서, j번 째 관절각의 순간 변화량(q˙j)이 말단 장치의 위치에 주는 영향(m_i)은M22M23

와 같다. J_l,j는 자코비안 행렬의 l번째 행의 j번째 성분을 의미하고, m_i,l 은 |q˙j| 에 의한 l번째 말단 장치의 위치 변 화를 의미한다. 따라서 식 (20)에 의해, 특이점 밖에서의 m_i는 i번째 고유벡터의 1-norm(ni=‖ui‖1)과 같다.

한편, m_i의 크기는 각각의 조인트에 의한 말단장치의 움직임이 서로 상쇄(Cancellation) 될 때 커지게 된다. 즉, 로봇이 특이점에 가까워질 경우 q˙j 이 발산하여 m_i가 n_i 보다 커지게 된다. 그러므로, nimi 가 1보다 작을 때 로봇 이 특이점 영역을 판별하는 조건으로 설정하여 i번째 고 유벡터에 의한 허용 관절 속도(γ_i)를 식 (24)와 같이 정의 할 수 있다.

식 (24)에서 γ_max는 관절의 최대 허용 각속도를 의미한 다. 최종적으로, 식 (24)의 조정항을 이용하여 실제 추종 오차(e)에 의한 관절각 변화(wi=1σiυiuiTe)를 제한하는 식은 식 (25)과 (26)이다.

Filtered Inverse는 자코비안 행렬의 역행렬을 특이값 분해 없이 최적화기법을 통하여 계산하는 방법으로, 우- 의사역행렬 오차와 좌-의사역행렬 투영 오차를 최소화 하는 역행렬을 얻는다^[10,11].

Fig. 2에서 알 수 있듯이, 우-의사역행렬 오차(S_r)와 좌-의사역행렬 오차(S_l)의 합을 피드백하여 자코비안의 역행렬을 구하게 된다. 이처럼, Filtered Inverse 또한 Jacobian Transpose와 같이 연산과정 중 특이값 분해를 하지 않아, 계산 속도가 용이할 수 있으나, 반복 연산 (Iteration)의 수(k)에 따라 수렴 속도가 지연 될 수 있다 는 단점이 있다.

Fig. 2 

Pseudo code offiltered inverse

Task Transition 방법은 로봇에게 부여된 여러 업무를 연속적이고 안전하게 바꿀 수 있는 제어 프레임워크이 다. Lee는 역기구학 제어 기반 Task Transition 프레임워 크를 제시하여 관절 각도 제한 회피 등을 극복할 수 있음 을 보였고^[28,29], Han은 Lee의 후속연구로 역동역학(Inverse Dynamics) 제어기반 Task Transition 프레임워크를 제시 하여 특이점 회피 알고리즘을 구현하였다^[12,13]. 본 논문 에서는 위의 선행 연구들을 종합하여 역기구학 제어 기 반 Task Transition 프레임워크를 이용한 특이점 회피 알고리즘을 기술하겠다.

먼저 2개의 업무(x˙1d, x˙2d)를 수행할 때 1번 업무를 우선 적으로 수행한 뒤 나머지 여유자유도로 2번 업무를 수행 하는 역기구학 제어기는 다음과 같이 설계할 수 있다.

여기서 J₁, J₂, 그리고 N1=I−J1∗J1 은 각각 1번, 2번 업무의 자코비안 행렬, 그리고 1번 업무의 영공간(Nullspace) 이다. 또한 식 (27)에서 1번 업무에 영향을 주지 않는 2번 업무의 관절 각속도(q˙(2|1))는 식 (28)과 같다.

하지만, 식 (27)과 (28)는 각 업무의 독립적인 제어는 보장하나, 두 업무의 우선 순위를 바꿀 때 불연속적인 관절각의 변화를 유발한다. 이러한 문제점을 해결하기 위하여, Lee는 활성화 지표(h)를 이용한 Task Transition 방법을 제안하였고, 이는 식 (29)에서 (31)과 같다.

식 (30)과 (31)에서 h₁과 h₂는 각각 1번 업무, 2번 업무 의 활성화 지표이고 0에서 1 사이의 값을 가진다. 만약 활성화 지표가 1이라면 해당 업무를 최우선적으로 수행 하고, 0이라면 해당 업무를 수행하지 않는다. 식 (30)과 (31)에서 보듯이 활성화 지표를 이용하여 각 업무를 동 일한 과정을 통해 배분할 수 있다.

이러한 Task Transition 방법을 이용하여 특이점 회피 알고리즘을 설계한다면 다음과 같다. 특이값 분해의 식 (5)에서 직교행렬 U는 특이값(σ_i)에 따른 방향벡터들로 이루어져 있으므로, 특이값의 크기에 따라 0이 아닌 특 이값을 위한 방향벡터들(U_n)과 0인 특이값을 위한 방향 벡터들(U_s)로 나눌 수 있다.M32

그러므로, 어떠한 업무 x˙d 는 다음과 같이 0인 특이값 에 영향을 받지 않는 업무(x˙1d)와 영향을 받는 업무(x˙2d)로 분할 가능하다.M33M34

또한 이 업무와 상응하는 자코비안 행렬은 각각M35

로 나눌 수 있다. 이 때, 1번 업무는 특이점에 영향을 받지 않는 업무이므로 활성화 지표(h₁)는 항상 1로 지정 한다. 한편, 2번 업무는 특이점에 영향을 받는 업무이므 로 활성화 지표(h₂)는 1로 시작하여, 제일 작은 특이값 (σmin)이 작아짐에 따라 0으로 감소하게 정한다. 예를 들 어 Fig. 3은 특이값이 0.001과 0.01 사이일 때 활성화 지표 (h₂)가 변하는 그래프를 나타낸 그림이다. 결과적으로 Task Transition을 통한 특이점 회피 알고리즘은 특이점 에 영향을 받지 않은 1번 업무를 우선적으로 수행 한 뒤, 특이점에 영향을 받는 2번 업무를 차선적으로 수행 함을 알 수 있다.

Fig. 3 

Activation parameters w.r.t singular value

Fig. 4와 같이 각 알고리즘의 구현을 위해 사용한 로봇 은 휴머노이드 로봇 THORMANG의 7자유도 팔을 사용 하였다^[19]. 이 로봇을 가상현실에서 제어하기 위해 로봇 시뮬레이터 V-REP을 사용하였고, 물리엔진으로는 Bullet 엔진을 선택하였다^[20]. 로봇의 제어주기는 200 Hz이고 시뮬레이션을 수행한 컴퓨터의 사양은 Intel i7 3.90GHz, RAM 32GB이다.

Fig. 4 

Experimental environment with THORMANG

한편, 각 알고리즘을 테스트하기 위해 THORMANG 의 오른 손목의 위치 목표 경로(p_d)는 초기 로봇의 손목 위치(p_i)를 기준으로 5초동안 식 (37)와 같이 움직이도록 정의하고, 방향 목표 경로(o_d)는 초기 THORMAGN의 방향(o_i)과 동일하게 설정하였다.

식 (37)의 경로는 말단장치의 목표가 기구적으로 닿을 수 없거나 자코비안 행렬의 랭크가 작업의 차원보다 작 아질 때 발생하는 기구적 특이점(Kinematic Singularity) 이 존재하는 경로이며, 조작성 지수(w)가 0.001 미만인 곳을 기구적 특이점의 근처라고 간주하였다. 이를 도식 화하면 Fig. 5와 같다.

Fig. 5 

Reference desired trajectory

마지막으로, 각 알고리즘의 특이점 회피를 위한 게인은 Table 1과 같다. Table 1의 DPI는 식 (12)를 이용한 Damped Pseudo Inverse이며, DPI*는 식 (14) 기반 Damped Pseudo Inverse이다. 각 알고리즘의 게인 값들은 특이점 회피 능력 과 추종 능력을 최대한 발휘 할 수 있는 값들로 설정하였다.