KR101145243B1 - 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법 - Google Patents

Abstract

본 발명은 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법에 관한 것으로, 더 상세하게는, 매니퓰레이터에 위치 센서를 부착하여 조인트 각도 정보를 산출하고, 외부 장애물 또는 매니퓰레이터 자체의 링크 충돌 등으로 인해 매니퓰레이터의 움직임이 제한되는 공간 정보를 산출하는 제한 공간 산출 방법에 관한 것이다. 본 발명의 일 실시예에 따른 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법은 다관절 매니퓰레이터의 위치센서를 통해 움직임이 제한되는 공간을 산출하여, 상기 매니퓰레이터의 움직임을 제어하기 위한 방법으로서, 마스터 장치에서 상기 매니퓰레이터의 목표 위치를 생성하여, 상기 매니퓰레이터를 구동시키는 단계; 상기 위치 센서를 통해 상기 매니퓰레이터의 조인트 각도를 산출한 후에, 상기 조인트 각도를 상기 매니퓰레이터 말단의 자세 벡터로 변환하고, 상기 매니퓰레이터의 목표 자세 벡터와 비교하여 자세 오차를 산출함으로써, 상기 구동된 매니퓰레이터가 갖는 부족한 자유도로 인해 움직임이 제한되는 공간을 산출하는 단계; 상기 위치 센서를 통해 상기 매니퓰레이터의 조인트 각도를 산출한 후에, 상기 조인트 각도를 상기 매니퓰레이터의 목표 조인트 각도와 비교하여, 조인트 오차를 산출하고, 상기 산출된 자세 오차와 합산함으로써, 상기 구동된 매니퓰레이터가 외부 장애물 또는 매니퓰레이터의 링크 충돌로 인해 움직임이 제한되는 공간을 산출하는 단계; 및 상기 산출된 제한공간의 정보를 이용하여 매니퓰레이터의 관절 배치를 최적화하는 단계를 포함한다.

Description

다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법 {RESTRICTION SPACE CALCULATION METHOD USING POSITION SENSORS OF MULTI DEGREES-OF-FREEDOM MANIPULATOR}

본 발명은 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법에 관한 것으로, 더 상세하게는, 매니퓰레이터에 위치 센서를 부착하여 조인트 각도 정보를 산출하고, 외부 장애물 또는 매니퓰레이터 자체의 링크 충돌 등으로 인해 매니퓰레이터의 움직임이 제한되는 공간 정보를 산출하는 제한 공간 산출 방법에 관한 것이다.

원격제어 로봇의 기술의 발전에 따라 종래에 인간이 접근하기 어려웠던 환경에서의 원격 작업이 가능해지고, 최소 침습 수술 로봇과 같은 정밀한 작업과 손상된 신체의 보완 등의 목적으로 의수 등이 현실에 적용되고 있다. 로봇 등과 같은 매니퓰레이터를 인간의 의도대로 움직이기 위해서는 햅틱 디바이스 등과 같은 인간-로봇 상호 작용에 의해 움직이는 마스터 장치를 통해서 조작 신호를 입력하거나, 최근에는 근전도(EMG, Electromyogram)나 뇌전도(EEG, Electroencephalogram)와 같은 장비를 이용한 인간의 의도 파악을 통해 매니퓰레이터를 구동할 수 있다.

매니퓰레이터가 인위적으로 가공한 환경이 아닌 일상적인 주변 환경 내에서 움직이는 경우, 주변 환경 내의 장애물 또는 매니퓰레이터의 링크 충돌(link collision) 등으로 인해, 마스터 장치를 통한 조작자의 의도에 따라 움직일 수 없는 공간에 진입하는 경우가 발생한다. 이러한 제한 공간(restriction space)에 매니퓰레이터가 진입할 수 없음에도 불구하고, 조작자가 마스터 장치를 통해 매니퓰레이터를 무리하게 이동시키는 신호를 전달하면, 마스터 장치와 매니퓰레이터 사이에 위치 오차로 인해 전체 시스템의 안정성을 보장할 수 없게 된다.

따라서, 종래에는 힘 센서 또는 위치 센서를 통해 반발력의 형태로 매니퓰레이터가 제한 공간에 진입하는 것을 막고, 제한 공간에 진입하였는지 여부에 대한 정보를 조작자에게 전달하였다.

힘 센서에 의한 제한 공간의 산출은 정확도의 측면에서 우수하나, 힘 센서의 가격이 비싸므로, 주변 환경 내의 가능한 모든 충돌을 감지하기 위해 충분한 개수의 힘 센서를 매니퓰레이터에 부착하는 것은 비용 면에서 비효율적이다. 그러나, 만약 적은 수의 힘 센서만을 부착하여 감지할 필요성이 있는 충돌을 감지하지 못한다면, 매니퓰레이터가 적용되는 환경에서 안정성을 해치게 된다. 예를 들어, 원격제어 시스템에서 로봇과 같은 매니퓰레이터가 주변 장애물과 충돌하였으나, 조작자에게 마스터 장치를 통해 반발력이 제공되지 않는다면, 조작자는 마스터 장치를 통해 조작 신호를 계속 전달하게 되고, 매니퓰레이터는 장애물로 인해 움직이지 않으므로, 위치 오차가 커지게 되어, 전체 시스템의 안정성이 보장되지 않는다.

종래의 위치 센서를 사용한 제한 공간 산출의 경우, 매니퓰레이터에 위치 센서, 각도 센서, 엔코더, 홀(hall) 센서 등을 부착하여, 매니퓰레이터의 조인트의 각도나 위치 정보를 지속적으로 파악하고, 조작자가 마스터 장치로 입력하는 목표 위치에 대한 정보와 비교하여, 마스터 장치에 반발력의 형태로 제한 공간의 존재 여부를 전달한다. 그러나, 이러한 방법은 마스터 장치에 발생하는 반발력으로 인해 조작자가 제한 공간의 존재 여부를 파악할 수 있도록 하지만, 다관절 매니퓰레이터의 경우 제한 공간의 방향을 산출할 수 없다는 문제점을 갖는다.

본 발명의 목적은 매니퓰레이터에 부착된 위치 센서를 이용하여, 매니퓰레이터의 움직임이 제한되는 공간에 대한 제한 공간의 존재 여부 및 제한 공간의 방향 정보를 산출하고, 이를 벡터인 반발력의 형태로 조작자에게 전달할 뿐만 아니라, 제한공간을 고려하여 매니퓰레이터의 관절 배치를 최적화하는 것이다.

전술한 목적을 달성하기 위한 본 발명의 일 실시예에 따른 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법은 다관절 매니퓰레이터의 위치센서를 통해 움직임이 제한되는 공간을 산출하여, 상기 매니퓰레이터의 움직임을 제어하기 위한 방법으로서, 마스터 장치에서 상기 매니퓰레이터의 목표 위치를 생성하여, 상기 매니퓰레이터를 구동시키는 단계; 상기 위치 센서를 통해 상기 매니퓰레이터의 조인트 각도를 산출한 후에, 상기 조인트 각도를 상기 매니퓰레이터 말단의 자세 벡터로 변환하고, 상기 매니퓰레이터의 목표 자세 벡터와 비교하여 자세 오차를 산출함으로써, 상기 구동된 매니퓰레이터가 갖는 부족한 자유도로 인해 움직임이 제한되는 공간을 산출하는 단계; 상기 위치 센서를 통해 상기 매니퓰레이터의 조인트 각도를 산출한 후에, 상기 조인트 각도를 상기 매니퓰레이터의 목표 조인트 각도와 비교하여, 조인트 오차를 산출하고, 상기 산출된 자세 오차와 합산함으로써, 상기 구동된 매니퓰레이터가 외부 장애물 또는 매니퓰레이터의 링크 충돌로 인해 움직임이 제한되는 공간을 산출하는 단계; 및 상기 산출된 제한공간의 정보를 이용하여 매니퓰레이터의 관절 배치를 최적화하는 단계를 포함한다.

상기 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법은, 상기 구동된 매니퓰레이터가 갖는 부족한 자유도로 인해 움직임이 제한되는 공간을 제1 반발력(F_RG)으로 변환하는 단계; 상기 구동된 매니퓰레이터가 외부 장애물 또는 매니퓰레이터의 링크 충돌로 인해 움직임이 제한되는 공간을 제2 반발력(F_RE)으로 변환하는 단계; 및 상기 제1 반발력(F_RG)과 제2 반발력(F_RE)을 합산하여, 상기 마스터 장치로 전달하는 단계를 더 포함할 수 있다.

상기 제1 반발력(F_RG)의 산출은 하기 수학식 1에 따라 이루어질 수 있다.

[수학식 1]

F_RG = R_G e_sx = -K_G(I-J_sJ_s ^#)e_sx

여기서, R_G는 매니퓰레이터의 부족한 자유도로 인한 일시적 제한 공간(IRS)을 반발력으로 매핑시키는 제1 매핑 행렬이며, e_sx는 매니퓰레이터 말단의 자세 오차이며, K_G는 반발력 F_RG의 크기를 조정하기 위한 스칼라 힘 이득이며, I는 단위 행렬이며, 자코비안 J_s = ∂x_s / ∂q_s 이며, x_s 는 매니퓰레이터 말단의 자세 벡터이며, q_s 는 매니퓰레이터의 조인트 각도 벡터이며, J_s ^#는 J_s의 의사 역행렬(pseudo-inverse)이다.

상기 제2 반발력(F_RE)의 산출은 하기 수학식 2에 따라 이루어질 수 있다.

[수학식 2]

F_RE = R_E e_sx = -K_E {(I-J_{s θ}J^# _sθ)J_sJ^# _s} e_sx

여기서, R_E는 매니퓰레이터의 외부적 제한에 의한 일시적 제한 공간(IRS)을 반발력으로 매핑시키는 제2 매핑 행렬이며, e_sx는 매니퓰레이터 말단의 자세 오차이며, K_E 는 반발력 F_RE 의 크기를 조정하기 위한 스칼라 힘 이득이며, I는 단위 행렬이며, 자코비안 J_s = ∂x_s / ∂q_s 이며, x_s 는 매니퓰레이터 말단의 자세 벡터이며, q_s 는 매니퓰레이터의 조인트 각도 벡터이며, J^# _s 는 J_s의 의사 역행렬이며, J_{s θ}는 J_s의 유전 자코비안(inherited Jacobian)이며, J^# _sθ는 J_{s θ}의 의사 역행렬이다.

상기 매니퓰레이터에는 복수의 힘 센서가 부착되며, 상기 힘 센서에서 외부 장애물을 감지하여, 상기 매니퓰레이터의 움직임이 제한되는 공간을 제3 반발력(F_EF) 으로 변환하는 단계와, 상기 제3 반발력(F_EF)을 상기 제1 반발력(F_RG) 및 제2 반발력(F_RE)과 합산하여, 상기 마스터 장치로 전달하는 단계를 더 포함할 수 있다.

상기 매니퓰레이터의 목표 조인트 각도는 상기 매니퓰레이터의 목표 조인트 속도()를 적분기를 통해 적분하여 획득하며, 상기 목표 조인트 속도(

)의 산출은 제한공간의 정보를 이용하여 매니퓰레이터의 관절 배치를 최적화하기 위하여 하기 수학식 5에 따라 이루어질 수 있다.

[수학식 5]

여기서,

은 단위 행렬이며,

이며,

,

이며, e_sq는 조인트 오차이며, q_sd는 목표 조인트 각도이며, q_s는 조인트 각도이며, k₁ 및 k₂는 외력에 대한 매니퓰레이터의 감도(sensitivity) 및 안정성(stability)을 결정하는 상수들이며, 자코비안 J_s = ∂x_s / ∂q_s 이며, x_s 는 매니퓰레이터 말단의 자세 벡터이며, q_s 는 매니퓰레이터의 조인트 각도 벡터이며, J^# _s 는 J_s의 의사 역행렬이며,

는 매니퓰레이터 말단의 목표 속도 명령이다.

본 발명의 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법은 햅틱 장치(haptic device), 원격제어(teleoperation), 휴먼-로봇 상호작용(human-robot interaction), 의수 등에 적용하여, 꾸며지지 않은 미지의 실제 환경(unstructured environment)에서 장애물이나 매니퓰레이터 자체의 링크 충돌(link collision)이 있을 때, 위치 센서만으로 제한 공간(restriction space 또는 constraint space), 또는 움직일 수 없는 공간, 예를 들면, 매니퓰레이터가 장애물을 접하여 더 이상 움직일 수 없는 공간을 산출하여, 반발력의 형태로 조작자에게 전달하고 제한공간의 정보를 이용하여 매니퓰레이터의 관절 배치를 최적화함으로써, 매니퓰레이터의 무리한 조작으로 인한 오작동 내지 고장을 예방할 수 있는 효과가 있다.

또한, 본 발명의 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법은 원격지에 있는 마스터 장치와 매니퓰레이터의 형태 및 자유도가 다르더라도 추가적인 조작없이 적용할 수 있는 효과가 있다.

또한, 본 발명의 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법은 매니퓰레이터의 안정된 휴먼-로봇(human-robot) 상호 작용을 보장할 수 있는 효과가 있다. 예를 들어, 본 발명을 의수에 적용하여 의수의 움직임이 제한되는 공간을 산출하는 경우, 조작자의 의도와 무관하게 의수가 장애물이나 다른 사람과 충돌하는 것을 방지할 수 있어서, 힘 센서 없이도 안전한 동작이 가능하게 된다.

또한, 본 발명의 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법은 제한 공간을 산출하여 반발력으로 변환 시, 반발력이 벡터로 구성되므로, 제한 공간의 존재 여부 뿐만 아니라, 제한 공간의 방향에 대한 정보도 제공할 수 있는 효과가 있다.

도 1a 내지 1c는 다관절 매니퓰레이터의 일시적 제한 공간(IRS)과 일시적 이동 공간(IMS)의 개념을 설명하기 위한 도면이다.
도 2는 본 발명의 일 실시예에 따른 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법을 설명하기 위한 블록도이다.
도 3은 도 2의 방법에 의해 산출되는 제한 공간에 관한 정보를 반발력으로 변환시키는 과정을 설명하기 위한 도면이다.

이하에서는 첨부한 도면을 참조하여 본 발명의 바람직한 실시예들에 따른 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법에 대하여 상세하게 설명한다.

도 1a 내지 1c는 다관절 매니퓰레이터의 일시적 제한 공간(IRS)과 일시적 이동 공간(IMS)의 개념을 설명하기 위한 도면이다.

자유 공간에서 이동하는 동안, 매니퓰레이터의 이동에는 제한이 없으며, 따라서, 조작자는 반발력을 전혀 느끼지 않는다. 그러나, 매니퓰레이터의 이동이 제한되면, 조작자가 원격 조작 장치를 제한된 공간으로 이동시키지 않도록 조작자가 반발력을 감지할 필요가 있다.

따라서, 양방향 원격 조작 시스템의 목적은, 위치 오차 피드백 방법에 사용되는 방법과 같은 매니퓰레이터의 말단 위치와 마스터 장치의 말단 위치를 일치시키기 보다는, 조작자에게 매니퓰레이터 측의 제한된 공간과 동일한 제한 공간을 생성하는데 있다.

도 1a 내지 1c를 참조하면, 2차원 x-y 카테시안 공간에서 매니퓰레이터의 일시적 제한 공간의 예시를 도시한다.

도 1a에서, 장애물(20)은 x축 방향으로 매니퓰레이터(10)의 말단 움직임을 제한한다. 따라서, 매니퓰레이터(10)는 순간적으로 y축 방향으로만 움직일 수 있고, 이를 일시적 이동 공간(Instantaneous Motion Space, IMS)이라 칭한다. IMS의 대수적 보완은 일시적 제한 공간(IRS)과 같이 정의한다. 따라서, 도 1a에서 x축 방향은 IRS 내에 있다.

도 1b에서, 제1 조인트(30)의 y축 양의 방향 이동이 장애물(50)에 의해 제한되어, 이동 공간은 제2 조인트(40)에 의해 x축 방향과 제1 조인트(30)의 y축 음의 방향으로 연장된다. 따라서, 일시적으로 생성된 IRS는 양의 y축 방향이다.

도 1c는 주어진 태스크(task) 공간의 차원에 비해 매니퓰레이터(60)의 자유도가 부족한 것으로 인해 생성된 IRS를 나타낸다. 이 경우에, 비록 태스크 공간이 2차원 x-y 공간이라도, 매니퓰레이터(60)는 1 자유도만을 가지며, 따라서, 매니퓰레이터(60)의 이동 공간은 전체 태스크 공간에 걸쳐 연장될 수 없다. 도 1c에서의 IRS는 링크(62)와 접하는 방향, 즉, y축 방향이다.

도 1a와 1b에서, IRS는 장애물(20, 50) 및 링크(32) 충돌과 같은 외부적 제한에 의해 생성된다. 도 1c에서, IRS는 주어진 태스크 공간에 비해 부족한 자유도에 의해 생성된다. 따라서, IRS는 다음과 같이 정의될 수 있다.

IMS: 매니퓰레이터가 운동학적으로 달성할 수 있는 모든 가능한 속도 벡터의 컨벡스 콘(convex cone)

IRS: IMS의 대수적 보완

IRS_G: 매니퓰레이터의 부족한 자유도에 의해 생성되는 IRS

IRS_E: 외부적 제한에 의해 생성되는 IRS

IMS와 IRS는 조인트가 양방향으로 제한될 때, 하기와 같이 수학적으로 정의될 수 있다.

IMS = R(J_{s θ})

IRS_G = R(J_s)^⊥

IRS_E = R(J_s)∩ R(J_{s θ})^⊥

여기서, R(?)는 (?)의 공간 범위를 나타내며, (?)^⊥는 직교하는(orthogonal) 상보적인 공간을 나타낸다. 또한, 자코비안 J_s = ∂x_s / ∂q_s 를 나타낸다. 여기서, x_s 는 매니퓰레이터 말단의 자세(pose) 벡터를 나타내며, q_s 는 매니퓰레이터의 조인트 각도 벡터를 나타낸다.

J_{s θ}는 J_s의 유전 자코비안(inherited Jacobian)으로 정의되며, 제한된 조인트에 대응하는 J_{s θ}의 컬럼들은 0 컬럼 벡터들로 대체된다.

제한된 조인트들을 탐지하기 위해, 조인트 각도 오차가 장애물에 의한 이동 제한에 기인하는 것인지 또는 자유 이동 제어 오차에 기인하는지 결정하도록 소정의 문턱값 ε을 도입한다. 즉, 조인트 각도 오차가 문턱값 ε보다 크다면, J_{s θ}의 컬럼은 0 벡터로 대체된다. 문턱값 ε은 예상되는 제한공간이 없는 자유 이동 공간에서의 제어 오차보다 커야 한다.

R(J_s)^⊥는, 외부적 제한이 없더라도, 매니퓰레이터의 부족한 자유도, 즉, IRS_G 때문에 도달할 수 없는 제한된 공간을 나타낸다. 만약 매니퓰레이터의 이동이 제한되면, IMS는 R(J_{s θ})이 되며, IRS_E 는 R(J_s)∩ R(J_{s θ})^⊥ 이 된다.

이하에서는, 제한 공간의 산출 방법에 대해 설명한다.

도 2는 본 발명의 일 실시예에 따른 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법을 설명하기 위한 블록도이며, 도 3은 도 2의 방법에 의해 산출되는 제한 공간에 관한 정보를 반발력으로 변환시키는 과정을 설명하기 위한 도면이다.

도 2를 참조하면, 조작자(110)는 매니퓰레이터(140)를 목표 위치로 이동시키기 위한 조작력을 마스터 장치(120)로 입력한다. 마스터 장치(120)는 상기 목표 위치에 관한 정보를 원격으로 매니퓰레이터 제어기(130)로 전달하며, 매니퓰레이터 제어기(130)의 제어 신호에 따라 매니퓰레이터(140)가 구동하게 된다.

구동하는 매니퓰레이터(140)는 조인트와 링크의 위치에 따라 주변 환경과 작용/반작용을 일으키며, 제한 공간으로 진입한 경우, 이동이 제한된다.

이어서, 매니퓰레이터(140)에 부착된 위치 센서는 조인트의 상대적인 위치를 감지하여, 제한 공간 산출부(160)로 전달한다. 제한 공간 산출부(160)는 매니퓰레이터(140)의 자세 및 매니퓰레이터(140)의 조인트 각도 등에 관한 정보를 통합하여, 매니퓰레이터(140)의 움직임이 제한되는 공간을 산출한다. 제한 공간 산출부(160)에서 산출된 제한 공간에 관한 정보는, 예를 들어, 반발력(force reflection) 형태로 변환될 수 있다. 이 때 매니퓰레이터 제어기(130)는 산출된 제한 공간을 고려하여 매니퓰레이터의 관절 배치를 최적화하는 방향으로 제어 신호를 만든다.

이후, 제한 공간 산출부(160)는 상기 변환된 반발력을 마스터 장치(120) 내지 조작자(110)로 전달하며, 조작자(110)는 반발력을 감지하여, 매니퓰레이션(140)이 제한 공간에 진입했는지 여부와 제한 공간의 방향을 확인할 수 있다.

한편, 제한 공간 산출부(160)에서 산출된 제한 공간에 관한 정보는 상기 언급된 반발력 형태 이외에 다양한 알고리즘에 따라 변환되어 사용될 수 있다. 예를 들어, 제한 공간에 관한 정보가 의수에 적용되는 경우, 의수가 외부 장애물과 충돌하거나 또는 외부 물체가 의수에 접근하여 접촉하면, 의수의 움직임을 정지시키는 알고리즘이 사용될 수 있다.

상기 제한 공간 산출부(160)가 제한 공간에 관한 정보를 반발력 형태로 변환하는 과정은 도 3을 참조하여, 상세히 후술하기로 한다.

본 발명은 정확한 IRS를 나타내는 새로운 위치-센서-기반 반발력 방법에 관한 것으로, 반발력의 형태로 IRS를 산출하도록 RSP(restriction space projection) 행렬들을 도입한다.

전술한 바와 같이, IRS_G는 매니퓰레이터(140)의 자코비안, 즉, J_S로부터 산출될 수 있다. IRS_E는 측정된 조인트 오차 형태와 J_S를 사용하여 J_{s θ}로부터 산출될 수 있다.

목표 위치가 IRS 내에 있을 때, 매니퓰레이터(140)의 이동 제한은 제어 오차를 일으킨다. 본 발명의 RSP 방법에서, 태스크 오차는 반발력을 생성하도록 IRS에 투사된다(projected). IRS_G와 IRS_E를 산출하도록, R_G와 R_E 같은 2개의 RSP 행렬이 형성된다.

IRS_G의 정의로부터, 매니퓰레이터(140)의 부족한 자유도로 인한 IRS를 반발력으로 매핑시키는 제1 매핑 행렬 R_G는 하기와 같이 정의된다.

R_G: e_sx → F_RG

R_G = -K_G(I-J_SJ_S ^#)

따라서, 매니퓰레이터(140)의 부족한 자유도로 인한 반발력 F_RG 는 하기 수학식과 같다.

[수학식 1]

F_RG = R_G e_sx = -K_G(I-J_sJ_s ^#)e_sx

여기서, (?)^#은 (?)의 의사 역행렬(pseudo-inverse)이며, e_sx(= x_d - x_s)는 매니퓰레이터 말단의 자세 오차이다. 여기서, x_d 는 목표 자세 벡터이며, x_s 는 매니퓰레이터 말단의 자세 벡터이다. 또한, K_G는 반발력 F_RG의 크기를 조정하기 위한 스칼라 힘 이득이며, I는 단위 행렬이다.

상기와 유사하게, IRS_E의 정의로부터, 매니퓰레이터(140)의 외부적 제한에 의한 IRS를 반발력으로 매핑시키는 제2 매핑 행렬 R_E는 하기와 같이 정의될 수 있다.

R_E: e_sx → F_RE

R_E = -K_E(I-J_{s θ}J^# _sθ)J_sJ^# _s

따라서, 매니퓰레이터(140)의 외부적 제한에 의한 반발력 F_RE 는 하기 수학식과 같다.

[수학식 2]

F_RE = R_E e_sx = -K_E {(I-J_{s θ}J^# _sθ)J_sJ^# _s} e_sx

여기서, K_E는 스칼라 힘 이득이다.

또한, 상기 수학식 1과 2에서의 J_S 는 하기 수학식에 의해 구할 수 있다.

[수학식 3]

J_{s θ}= J_s D

여기서,

이며,

이다.

따라서, 전체 반발력 F_R은 하기와 같이 산출될 수 있다.

[수학식 4]

F_R = F_RG + F_RE = R_Ge_sx + R_Ee_sx

= -{K_G(I-J_sJ_s ^#)+K_E(I-J_{s θ}J^# _sθ)J_sJ^# _s}e_sx

비록 제안된 RSP 방법이 제한 공간의 방향을 산출하더라도, 크기를 추정할 수는 없다. 따라서, 조작자는 F_RG와 F_RE 사이의 가중치를 고려하여 응용분야에 적합하도록 K_E와 K_G를 변경해야 한다. F_RG와 F_RE는 직교하는 상보적인 공간에 존재하므로, 힘 이득인 K_E와 K_G는 독립적으로 조정될 수 있다.

도 3에서, 마스터 장치(120)로부터 매니퓰레이터 말단의 목표 속도 명령

가 전달되면, 매니퓰레이터(140)의 공간 범위, 즉, R(I-R_G)로 투사된다. 다음으로, 목표 조인트 속도가 투사된 목표 명령

로부터 산출되며, 매니퓰레이터(140)의 목표 조인트 각도 q_sd가 역기구학(inverse kinematics, IK)을 통해 산출된다. 매니퓰레이터 제어기(130), 즉, K_s는 제한공간을 고려하여 매니퓰레이터(140)의 관절 배치를 최적화하면서 목표 위치를 추종하도록 한다. 동시에, 장애물 충돌로 인해 외부 토크 τ_ext가 매니퓰레이터(140)의 조인트로 분배된다.

P_S는 매니퓰레이터(140)의 역학(dynamics)을 나타낸다. 자세 오차 e_sx는 목표 자세 벡터 x_d와 매니퓰레이터 말단의 자세 벡터 x_s로부터 산출된다. 변수 q_s는 매니퓰레이터(140)의 조인트 각도이다. FK는 조인트 각도 벡터 q_s로부터 매니퓰레이터 말단의 자세 벡터 x_s에 이르는 포워드 동적 매핑(Forward Kinematic mapping)을 나타낸다. R_G는 자코비안 J_s로부터 산출될 수 있다.

다음으로, F_RG = R_Ge_sx이다. J_s와 조인트 오차 e_sq로부터, J_{s θ}가 산출될 수 있다. F_RE = R_Ee_sx이다. F_EF는 힘 센서에 의해 탐지되는 힘 신호이다.

가 결정되면, 역기구학(IK)을 통해 출력되는 목표 조인트 속도

를 하기와 같이 산출할 수 있다.

[수학식 5]

여기서,

은 단위 행렬이며,

이다. 또한,

,

이며, k₁ 및 k₂는 외력에 대한 매니퓰레이터의 감도(sensitivity)와 안정성(stability)을 결정하는 상수들이다. 또한, I_n-J_s ^#J_s 는 멱등 행렬(indempotent matrix)이라는 것을 이용하여, p는 감소함수 임을 증명할 수 있고, 이는 매니퓰레이터가 상기의 역기구학(IK)을 이용하면, 제한공간을 고려하여 관절 배치를 최적화 또는 관절의 제어 오차를 최소화하는 방향으로 목표 조인트 속도를 산출한다는 것을 의미한다.

역기구학(IK)은 조인트 이동 제한을 회피하기 위해 조인트 각도 오차를 감소시키도록 목표 조인트 각도 배치를 변경시킨다.

IRS는 반발력의 형태로 마스터 장치(120)로 전달되므로, 매니퓰레이터에 부착된 힘 센서가 있어서 충돌을 감지할 수 있을 때, 힘 센서 신호를 계산된 반발력과 함께 사용할 수 있다. 만약, 힘 센서로부터 0이 아닌 힘 신호가 있다면(F_EF ≠ 0), F_EF 가 F_RE 를 대체하여 반발력 F_R = F_RG + F_EF이 되거나_, 혹은 F_R = F_RG + F_RE + F_EF 가 된다. 만약, 힘 센서가 충돌을 피했으나(F_EF = 0), 산출된 제한 공간이 있는 경우에는, F_R = F_RG + F_RE 가 된다.

상기한 본 발명의 바람직한 실시예는 단지 예시의 목적을 위해 개시된 것이고, 본 발명에 대한 통상의 지식을 가지는 당업자라면 본 발명의 사상과 범위 안에서 다양한 수정, 변경, 부가가 가능할 것이며, 이러한 수정, 변경 및 부가는 하기의 특허청구범위에 속하는 것으로 보아야 할 것이다.

10, 60, 140: 매니퓰레이터
20, 50: 장애물
30: 제1 조인트
32, 62: 링크
40: 제2 조인트
110: 조작자
120: 마스터 장치
130: 매니퓰레이터 제어기
140: 매니퓰레이터
150: 주변 환경
160: 제한 공간 산출부

Claims (6)

1. 다관절 매니퓰레이터의 위치센서를 통해 움직임이 제한되는 공간을 산출하여, 상기 매니퓰레이터의 움직임을 제어하기 위한 방법으로서,
   마스터 장치에서 상기 매니퓰레이터의 목표 위치를 생성하여, 상기 매니퓰레이터를 구동시키는 단계;
   상기 위치 센서를 통해 상기 매니퓰레이터의 조인트 각도를 산출한 후에, 상기 조인트 각도를 상기 매니퓰레이터 말단의 자세 벡터로 변환하고, 상기 매니퓰레이터의 목표 자세 벡터와 비교하여 자세 오차를 산출함으로써, 상기 구동된 매니퓰레이터가 갖는 부족한 자유도로 인해 움직임이 제한되는 공간을 산출하는 단계;
   상기 위치 센서를 통해 상기 매니퓰레이터의 조인트 각도를 산출한 후에, 상기 조인트 각도를 상기 매니퓰레이터의 목표 조인트 각도와 비교하여, 조인트 오차를 산출하고, 상기 산출된 자세 오차와 합산함으로써, 상기 구동된 매니퓰레이터가 외부 장애물 또는 매니퓰레이터의 링크 충돌로 인해 움직임이 제한되는 공간을 산출하는 단계; 및
   상기 산출된 제한 공간의 정보를 이용하여 매니퓰레이터의 관절 배치를 최적화하는 단계를 포함하는 것을 특징으로 하는 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법.
2. 제 1 항에 있어서,
   상기 구동된 매니퓰레이터가 갖는 부족한 자유도로 인해 움직임이 제한되는 공간을 제1 반발력(F_RG)으로 변환하는 단계;
   상기 구동된 매니퓰레이터가 외부 장애물 또는 매니퓰레이터의 링크 충돌로 인해 움직임이 제한되는 공간을 제2 반발력(F_RE)으로 변환하는 단계; 및
   상기 제1 반발력(F_RG)과 제2 반발력(F_RE)을 합산하여, 상기 마스터 장치로 전달하는 단계를 더 포함하는 것을 특징으로 하는 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법.
3. 제 2 항에 있어서, 상기 제1 반발력(F_RG)의 산출은 하기 수학식 1에 따라 이루어지는 것을 특징으로 하는 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법:
   [수학식 1]
   F_RG = R_G e_sx = -K_G(I-J_sJ_s ^#)e_sx
   여기서, R_G는 매니퓰레이터의 부족한 자유도로 인한 일시적 제한 공간(IRS)을 반발력으로 매핑시키는 제1 매핑 행렬이며,
   e_sx는 매니퓰레이터 말단의 자세 오차이며,
   K_G는 반발력 F_RG의 크기를 조정하기 위한 스칼라 힘 이득이며,
   I는 단위 행렬이며,
   자코비안 J_s = ∂x_s / ∂q_s 이며,
   x_s 는 매니퓰레이터 말단의 자세 벡터이며,
   q_s 는 매니퓰레이터의 조인트 각도 벡터이며,
   J_s ^#는 J_s의 의사 역행렬(pseudo-inverse)이다.
4. 제 2 항에 있어서, 상기 제2 반발력(F_RE)의 산출은 하기 수학식 2에 따라 이루어지는 것을 특징으로 하는 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법:
   [수학식 2]
   F_RE = R_E e_sx = -K_E {(I-J_{s θ}J^# _sθ)J_sJ^# _s} e_sx
   여기서, R_E는 매니퓰레이터의 외부적 제한에 의한 일시적 제한 공간(IRS)를 반발력으로 매핑시키는 제2 매핑 행렬이며,
   e_sx는 매니퓰레이터 말단의 자세 오차이며,
   K_E 는 반발력 F_RE 의 크기를 조정하기 위한 스칼라 힘 이득이며,
   I는 단위 행렬이며,
   자코비안 J_s = ∂x_s / ∂q_s 이며,
   x_s 는 매니퓰레이터 말단의 자세 벡터이며,
   q_s 는 매니퓰레이터의 조인트 각도 벡터이며,
   J^# _s 는 J_s의 의사 역행렬이며,
   J_{s θ}는 J_s의 유전 자코비안(inherited Jacobian)이며,
   J^# _sθ는 J_{s θ}의 의사 역행렬이다.
5. 제 2 항에 있어서, 상기 매니퓰레이터에는 복수의 힘 센서가 부착되며,
   상기 힘 센서에서 외부 장애물을 감지하여, 상기 매니퓰레이터의 움직임이 제한되는 공간을 제3 반발력(F_EF) 으로 변환하는 단계와,
   상기 제3 반발력(F_EF)을 상기 제1 반발력(F_RG) 및 제2 반발력(F_RE)과 합산하여, 상기 마스터 장치로 전달하는 단계를 더 포함하는 것을 특징으로 하는 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법.
6. 제 1 항에 있어서, 상기 매니퓰레이터의 목표 조인트 각도는 상기 매니퓰레이터의 목표 조인트 속도(

   

   )를 적분기를 통해 적분하여 획득하며, 상기 목표 조인트 속도(

   

   )의 산출은 제한 공간의 정보를 이용하여 매니퓰레이터의 관절 배치를 최적화하기 위하여 하기 수학식 5에 따라 이루어지는 것을 특징으로 하는 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법:
   [수학식 5]
   

   

   
   여기서,

   

   은 단위 행렬이며,
   

   

   이며,
   

   

   ,

   

   이며,
   e_sq는 조인트 오차이며,
   q_sd는 목표 조인트 각도이며,
   q_s는 조인트 각도이며,
   k₁ 및 k₂는 외력에 대한 매니퓰레이터의 감도(sensitivity) 및 안정성(stability)을 결정하는 상수들이며,
   자코비안 J_s = ∂x_s / ∂q_s 이며,
   x_s 는 매니퓰레이터 말단의 자세 벡터이며,
   q_s 는 매니퓰레이터의 조인트 각도 벡터이며,
   J^# _s 는 J_s의 의사 역행렬이며,
   

   

   는 매니퓰레이터 말단의 목표 속도 명령이다.

Images