KR20150041346A - 다관절 매니퓰레이터 제어 방법 및 제어 시스템 - Google Patents

다관절 매니퓰레이터 제어 방법 및 제어 시스템 Download PDF

Info

Publication number
KR20150041346A
KR20150041346A KR20130119762A KR20130119762A KR20150041346A KR 20150041346 A KR20150041346 A KR 20150041346A KR 20130119762 A KR20130119762 A KR 20130119762A KR 20130119762 A KR20130119762 A KR 20130119762A KR 20150041346 A KR20150041346 A KR 20150041346A
Authority
KR
South Korea
Prior art keywords
end effector
tension
control signal
sensor
joint
Prior art date
Application number
KR20130119762A
Other languages
English (en)
Inventor
형승용
곽노산
권웅
노경식
이광규
김지영
이주석
Original Assignee
삼성전자주식회사
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 삼성전자주식회사 filed Critical 삼성전자주식회사
Priority to KR20130119762A priority Critical patent/KR20150041346A/ko
Publication of KR20150041346A publication Critical patent/KR20150041346A/ko

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B25J19/021—Optical sensing devices
    • B25J19/023—Optical sensing devices including video camera means
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
    • B25J9/104—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons
    • B25J9/1045—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons comprising tensioning means
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Manipulator (AREA)

Abstract

일 실시 예에 따른 다관절 매니퓰레이터의 제어 방법은 입력되는 제어 신호를 상기 엔드 이펙터에 대한 영상좌표 상의 목표 위치로 변환하는 단계, 비전 센서를 이용하여 상기 엔드 이펙터에 대한 위치 정보를 검출하는 단계, 검출된 상기 위치 정보를 이용하여 상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 추정하는 단계, 상기 목표 위치와 상기 현재 위치의 차이 값을 연산하는 단계, 연산된 상기 차이 값을 이용하여 보정 신호를 생성하는 단계 및 생성된 상기 보정 신호를 상기 복수의 관절 각각에 대한 구동부로 인가하는 단계를 포함한다.

Description

다관절 매니퓰레이터 제어 방법 및 제어 시스템{Method and system for controlling articulated manipulator}
다관절 매니퓰레이터 제어 방법 및 제어 시스템이 개시된다. 더욱 상세하게는, 영상 정보를 이용하여 백래시를 보상하는 다관절 매니퓰레이터 제어 방법 및 제어 시스템이 개시된다.
다관절 매니퓰레이터는 동작 및 형태(shape) 특성으로 인해 재난지역에서 생존자를 탐색하는 구조(rescue) 로봇에서부터 내시경, 싱글 포트 수술 로봇, 능동형 카테터(Active Catheter) 등과 같은 의료용 로봇, 군사용 로봇, 우주에서의 다양한 작업을 수행하는 로봇, 위험물 처리 로봇 및 배관 청소 로봇과 같은 각종 산업용 로봇과 같은 로봇 분야는 물론 이외의 다양한 분야에서 최근 활발히 연구되고 있다.
이러한 다관절 매니퓰레이터를 이루는 각 관절은 일반적으로 케이블과 같은 장력 전달 수단과 연결되고, 케이블을 당김에 따라 각각의 관절이 회전하며, 각각의 관절이 회전함에 따라 다관절 매니퓰레이터는 다양한 자유도로 동작할 수 있다.
이와 같이 케이블과 같은 수단을 이용하여 구동되는 경우, 원격에서 다관절 매니퓰레이터로 구동력을 전달할 수 있는 이점이 있는 반면, 케이블에 발생하는 마찰력과 장력으로 인하여 케이블이 늘어짐에 따라 백래시(backlash) 증상이 발생하고, 이에 따라 각 관절을 정밀하게 제어하는 것이 용이하지 않다.
이러한 백래시 증상은 일반적으로는 각 관절 축에 설치된 엔코더(encoder)를 이용하여 측정된 관절 각을 피드백함으로써 해결할 수 있는데, 최근 소형화 되어가는 다관절 매니퓰레이터의 경우, 각 관절 축에 관절 각 측정을 위한 센서를 설치할 수 있는 공간이 없을 뿐 아니라, 센서로부터 신호를 받기 위한 전선이 통과할 수 있는 경로를 만들어 내는 것 역시 용이하지 않다.
관절 각을 직접 측정하지 않고 영상 정보 및 장력과 같은 간접 정보를 이용하여 엔드 이펙터의 위치를 정밀하게 제어할 수 있는 다관절 매니퓰레이터 제어 방법 및 제어 시스템을 제공하는 것이다.
일 실시 예에 따른 다관절 매니퓰레이터의 제어 방법은 입력되는 제어 신호를 상기 엔드 이펙터에 대한 영상좌표 상의 목표 위치로 변환하는 단계, 비전 센서를 이용하여 상기 엔드 이펙터에 대한 위치 정보를 검출하는 단계, 검출된 상기 위치 정보를 이용하여 상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 추정하는 단계, 상기 목표 위치와 상기 현재 위치의 차이 값을 연산하는 단계, 연산된 상기 차이 값을 이용하여 보정 신호를 생성하는 단계 및 생성된 상기 보정 신호를 상기 복수의 관절 각각에 대한 구동부로 인가하는 단계를 포함한다.
또한, 일 실시 예에 따른 다관절 매니퓰레이터 제어 시스템은 끝단에 엔드 이펙터가 결합된 복수의 관절을 갖는 다관절 매니퓰레이터, 상기 엔드 이펙터에 대한 영상을 획득하는 비전 센서 및 입력되는 제어 신호를 상기 엔드 이펙터에 대한 영상좌표 상의 목표 위치로 변환하고, 상기 비전 센서를 이용하여 상기 엔드 이펙터에 대한 위치 정보를 검출한 후, 검출된 위치 정보를 이용하여 상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 추정하고, 추정된 상기 현재 위치와 상기 변환된 목표 위치의 차이 값을 연산한 다음, 연산된 상기 차이 값을 이용하여 보정 신호를 생성한 후, 생성된 보정 신호를 상기 복수의 관절 각각에 대한 구동부로 인가하는 제어부를 포함한다.
도 1은 다관절 매니퓰레이터를 갖는 수술 로봇 구조를 개략적으로 도시한 도면이다.
도 2는 선형 구동기 양쪽으로 장력 센서가 설치된 일 예를 도시한 도면이다.
도 3은 다관절 매니퓰레이터를 갖는 수술 로봇 구성을 개략적으로 도시한 블럭도이다.
도 4는 비전 센서 및 장력 센서를 통해 검출된 상태 정보를 이용하여 추정된 엔드 이펙터의 현재 위치 및 외력을 피드백하여 다관절 매니퓰레이터의 동작을 제어하는 과정을 도시한 도면이다.
도 5는 도 3의 상태 추정부에서 추정필터 상태를 추정하는 동작을 도시한 도면이다.
도 6은 다관절 매니퓰레이터 제어 방법을 순차적으로 도시한 흐름도이다.
본 발명의 목적, 특정한 장점들 및 신규한 특징들은 첨부된 도면들과 연관되어지는 이하의 상세한 설명과 바람직한 실시 예들로부터 더욱 명백해질 것이다. 본 명세서에서 각 도면의 구성요소들에 참조번호를 부가함에 있어서, 동일한 구성 요소들에 한해서는 비록 다른 도면상에 표시되더라도 가능한 한 동일한 번호를 가지도록 하고 있음에 유의하여야한다. 또한, 본 발명을 설명함에 있어서, 관련된 공지 기술에 대한 구체적인 설명이 본 발명의 요지를 불필요하게 흐릴 수 있다고 판단되는 경우 그 상세한 설명은 생략한다. 본 명세서에서, 제1, 제2 등의 용어는 하나의 구성요소를 다른 구성요소로부터 구별하기 위해 사용되는 것으로, 구성요소가 상기 용어들에 의해 제한되는 것은 아니다.
이하, 첨부된 도면들을 참조하여 본 실시 예를 상세히 설명하기로 한다.
이후 설명에서는 수술 로봇을 예로 들어 설명할 것이나, 이는 하나의 실시 예에 불과하며, 본 발명의 적용이 특별히 수술 로봇 분야에 한정되는 것은 아니다. 예로써, 내시경, 능동형 카테터(Active Catheter) 등과 같은 의료용 로봇, 재난지역에서 생존자를 탐색하는 구조(rescue) 로봇, 군사용 로봇, 우주에서 다양한 작업을 수행하는 로봇, 위험물 처리 로봇, 배관 청소 로봇 등 각종 산업용 서비스 로봇을 포함하는 모든 로봇 분야에 적용 가능함을 물론, 이와 같은 로봇 분야뿐 아니라 케이블을 통해 구동력을 전달받는 다관절 매니퓰레이터가 사용되는 모든 분야에 적용 가능함은 자명할 것이다.
도 1은 다관절 매니퓰레이터를 갖는 수술 로봇 구조를 개략적으로 도시한 도면이고, 도 2는 선형 구동기 양쪽으로 장력 센서가 설치된 일 예를 도시한 도면이며, 도 3은 다관절 매니퓰레이터를 갖는 수술 로봇 구성을 개략적으로 도시한 블럭도이다.
도 1을 참조하면, 수술 로봇은 크게 마스터 콘솔(100)과 슬레이브 장치(200)를 포함한다. 마스터 콘솔(100)은 슬레이브 장치(200)를 원격 제어하는 장치이다. 마스터 콘솔(100)은 조작자의 조작에 따라 제어 신호를 생성하고, 생성된 제어 신호를 슬레이브 장치(200)로 전송한다. 슬레이브 장치(200)는 마스터 콘솔(100)로부터 제어 신호를 수신하고, 수신된 제어 신호에 따라 동작한다.
이때, 환자 측에는 조작자를 보조할 보조자가 한 명 이상 위치할 수 있다. 여기에서, 조작자를 보조한다는 것은 환자가 위치하는 실제 공간에서 조작자의 조작에 의해 이루어지는 수술 작업을 보조하는 것을 의미할 수 있으며, 예를 들어, 사용되는 수술 도구의 교체 등이 이에 해당될 수 있으나, 특별히 이에 한정되는 것은 아니다.
마스터 콘솔(100)과 슬레이브 장치(200)는 물리적으로 독립된 별도의 장치로 분리 구성될 수 있으나, 특별히 이에 한정되는 것은 아니며, 예로써 하나로 통합된 일체형 장치로 구성되는 것 역시 가능할 것이다.
도 1에 도시한 바와 같이, 마스터 콘솔(100)은 입력부(110) 및 표시부(120)를 포함할 수 있다.
입력부(110)는 수술 로봇의 동작 모드를 선택하는 명령, 슬레이브 장치(200)의 수술 도구(210)의 동작을 원격으로 제어하기 위한 명령 등을 조작자로부터 입력받을 수 있는 구성을 의미하며, 본 실시 예에서 입력부(110)로 햅틱 디바이스, 클러치 페달, 스위치, 버튼 등이 사용될 수 있으나, 특별히 이에 한정되는 것은 아니며, 예로써 음성 인식 디바이스 등과 같은 구성도 사용될 수 있다. 이하 설명에서는 입력부(110)로 햅틱 디바이스가 사용된 것을 예로 들어 설명하고 있으나, 이는 하나의 실시 예에 불과하며, 입력부(110)로는 상술한 다양한 수단이 사용될 수 있음은 자명하다.
도 1에서는 입력부(110)가 두 개의 핸들(111, 113)을 포함하는 것으로 도시하고 있으나, 이는 하나의 실시 예에 불과할 뿐, 본 발명이 이에 한정되는 것은 아니다. 예를 들어, 하나의 핸들을 포함할 수도 있고 또는 세 개 이상의 핸들을 포함하는 것 역시 가능할 것이다.
조작자는 양손으로 두 개의 핸들(111, 113)을 각각 움직임으로써, 슬레이브 장치(200)의 수술 도구(210)의 동작을 제어할 수 있다. 즉, 조작자가 입력부(110)를 조작하면, 마스터 콘솔(100)의 제어부(130)는 제어 신호 생성부(135)를 통해 조작된 입력부(110)의 상태 정보에 대응되는 제어 신호를 생성하고, 생성된 제어 신호를 통신부(140)를 통해 슬레이브 장치(200)로 전송할 수 있다.
마스터 콘솔(100)의 표시부(120)에는 후술될 슬레이브 장치(200)의 내시경(221)을 통해 수집된 환자 신체 내부에 대한 실제 영상 및 환자의 수술 전 의료 영상을 3차원 영상으로 변환한 가상 영상 등이 화상 이미지로 표시될 수 있다. 이를 위해, 마스터 콘솔(100)은 슬레이브 장치(200)로부터 전송되는 영상 데이터를 수신 및 처리하여 표시부(120)로 출력하기 위한 영상 처리부(미도시)를 포함할 수 있다. 여기에서, "영상 데이터"는 전술한 바와 같이, 내시경(221)을 통해 수집된 실제 영상, 환자의 수술 전 의료 영상을 3차원 영상으로 변환한 가상 영상 등을 포함할 수 있으나, 특별히 이에 한정되는 것은 아니다.
표시부(120)는 하나 이상의 모니터로 구성될 수 있으며, 각 모니터에 수술 시 필요한 정보들이 개별적으로 표시되도록 구현할 수 있다. 예를 들어, 표시부(120)가 세 개의 모니터로 구성된 경우, 이중 하나의 모니터에는 내시경(221)을 통해 수집된 실제 영상, 환자의 수술 전 의료 영상을 이용하여 생성한 3차원 영상 등이 표시되고, 다른 두 개의 모니터에는 각각 슬레이브 장치(200)의 동작 상태에 관한 정보 및 환자 정보 등이 표시되도록 구현할 수 있다. 다른 예로, 복수의 모니터 동일한 영상이 표시되도록 구현할 수 있으며, 이때, 각각의 모니터마다 동일한 영상이 표시될 수도 있고, 복수의 모니터 전체에 하나의 영상이 표시될 수도 있다. 또한, 모니터의 수량은 표시를 요하는 정보의 유형이나 종류 등에 따라 다양하게 결정될 수 있다. 상술한 표시부(120)는 예로써, LCD(Liquid Crystal Display), LED(Light Emitting Diode), OLED(Organic Light Emitting Diodes), PDP(Plasma Display Panel), 또는 이들의 조합으로 구현될 수 있으나, 특별히 이에 한정되는 것은 아니다.
여기에서, "환자 정보"는 환자의 상태를 나타내는 정보일 수 있으며, 예를 들어, 체온, 맥박, 호흡 및 혈압 등과 같은 생체 정보일 수 있다. 이러한 생체 정보를 마스터 콘솔(100)로 제공하기 위해 후술할 슬레이브 장치(200)는 체온 측정 모듈, 맥박 측정 모듈, 호흡 측정 모듈, 혈압 측정 모듈 등을 포함하는 생체 정보 측정 유닛을 더 포함할 수 있다. 이를 위해, 마스터 콘솔(100)은 슬레이브 장치(200)로부터 전송되는 생체 정보를 수신 및 처리하여 표시부(120)로 출력하기 위한 신호 처리부(미도시)를 더 포함할 수 있다.
슬레이브 장치(200)는 도 1에 도시한 바와 같이, 크게 수술 도구(210) 및 마스터 콘솔(100)로부터 제어 신호를 수신하여 수술 도구(210)의 동작을 제어하는 제어부(230)를 포함할 수 있다.
본 실시 예에서, 수술 도구(210)는 다관절 매니퓰레이터 형태일 수 있다. 예를 들어, 도 1과 같이, 수술 도구(210)는 샤프트(211) 및 샤프트(211) 끝단에 결합된 엔드 이펙터(213)를 포함할 수 있다. 여기에서, 샤프트(211)는 도 1과 같이, 복수의 관절을 포함할 수 있으며, 통상 원통형 등 관(管)형 부재가 사용될 수 있다. 이에 따라, 각 관절 및 엔드 이펙터(213)로 조작자의 조작에 따른 구동력이 전달되도록 각 관절 및 엔드 이펙터(213)에 연결된 케이블(250)이 샤프트(211) 내에 수용될 수 있다.
이때, 케이블(250)은 샤프트(211)에 포함된 관절의 개수와 대응되는 개수로 구비될 수 있다. 즉, 샤프트(211)에 포함된 각각의 관절마다 별도의 케이블(250)이 연결되고, 각각의 케이블(250)마다 케이블(250)이 일 방향 및 타 방향으로 당겨지도록 동작하는 선형 구동기(260)가 마련되며, 각각의 선형 구동기(260)를 동작시키는 복수의 모터(270)가 구비될 수 있으나, 특별히 이에 한정되는 것은 아니다.
예를 들어, 조작자가 수술 도구(210)의 엔드 이펙터(213)를 특정 위치(예: 수술 부위)로 이동시키도록 마스터 콘솔(100)의 입력부(110)를 조작하면, 마스터 콘솔(100)의 제어부(130)는 제어 신호 생성부(135)를 통해 조작자에 의해 조작된 입력부(110)의 상태 정보에 대응되는 제어 신호를 생성하여 슬레이브 장치(200)로 전송한다. 이때, 생성된 제어 신호는 슬레이브 장치(200)의 수술 도구(210)의 엔드 이펙터(213)의 위치를 제어하기 위한 정보를 갖는 신호일 수 있다.
이후, 슬레이브 장치(200)의 제어부(230)는 마스터 콘솔(100)로부터 전송된 제어 신호에 따라 샤프트(211)의 각 관절에 연결된 케이블(250)을 일 방향 또는 타 방향으로 당겨 샤프트(211)의 각 관절을 일정 각도로 회전시킴으로써, 샤프트(211) 끝단에 결합된 엔드 이펙터(213)가 해당 위치로 이동하도록 제어할 수 있다. 이를 위해, 본 실시 예에 따른 슬레이브 장치(200)는 도 1에 도시한 바와 같이, 케이블(250)에 마련되어 케이블(250)이 일 방향 또는 타 방향으로 당겨지도록 동작하는 선형 구동기(linear actuator)(260) 및 선형 구동기(260)와 연결되어 선형 구동기(260)를 구동시키는 모터(270)를 포함할 수 있으나, 이는 하나의 실시 예에 불과하며, 케이블(250)로 구동력을 전달하기 위한 구성이 특별히 이에 한정되는 것은 아니다.
또한, 도 1에 도시하지는 않았으나, 본 실시 예에 따른 슬레이브 장치(200)는 모터(270)에 마련된 엔코더(encoder)를 더 포함할 수 있다. 엔코더(encoder)는 모터(270)의 회전 수, 회전 속도, 회전 방향 및 회전 각도 등을 검출할 수 있다. 이에 따라, 제어부(230)는 엔코더(encoder)를 통해 검출된 신호를 이용하여 모터(270) 구동을 제어할 수 있다.
또한, 본 실시 예에 따른 슬레이브 장치(200)는 센서부(220, 도 2 참조)를 더 포함할 수 있다. 이때, 센서부(220)는 도 1 및 도 2에 도시한 바와 같이, 비전 센서(221) 및 장력 센서(223)를 포함할 수 있으나, 특별히 이에 한정되는 것은 아니다.
여기에서, 장력 센서(223)는 도 1과 같이, 선형 구동기(260)와 케이블(250)의 연결 부위에 설치되어 케이블(250)에 걸리는 장력을 측정할 수 있다.
본 실시 예에서와 같이 케이블을 이용하여 각 관절로 구동력을 전달하는 다관절 매니퓰레이터에서 일 방향으로 회전하던 관절을 타 방향으로 회전시키기 위해서는 관절에 연결되어 일 방향으로 당겨지던 케이블(250)을 반대 방향으로 당겨야 한다.
이때, 케이블(250)이 갖는 탄성과 케이블(250)에 발생하는 마찰력 및 장력으로 인한 늘어짐으로 인하여 선형 구동기(260)가 동작을 시작해도 케이블(250)이 당겨지는 쪽 텐션(tension)이 낮아 선형 구동기(260)의 동작에 케이블(250)이 즉각적으로 반응하여 타방향으로 당겨지지 않음으로써 일정 시간 동안 해당 관절이 움직이지 않는 백래시(backlash) 증상이 발생할 수 있다.
이러한 백래시 증상으로 인하여 장력 센서(223)를 한쪽에만 설치해서는 다른 쪽에 걸리는 힘을 측정할 수 없으므로, 본 실시 예에서는 도 2에 도시한 바와 같이, 장력 센서(223)를 선형 구동기(260)의 양쪽에 설치하였다. 그러나, 이는 하나의 실시 예에 불과하며, 선형 구동기(260)와 케이블(250)의 연결 부위에 설치되는 장력 센서(223)의 수가 특별히 이에 한정되는 것은 아니며, 필요에 따라 결정될 수 있다.
또한, 본 실시 예에서 장력 센서(223)는 도 1에 도시한 바와 같이, 복강 외부에 배치된 선형 구동기(260)에 근접하게 설치되어 복강 외부에 위치하므로, 설치 공간에 제약받지 않을 수 있다.
이와 같이, 선형 구동기(260)와 케이블(250)의 연결 부위에 장력 센서(223)를 설치함으로써, 장력 센서(223)를 통해 검출된 케이블(250)에 걸리는 장력을 이용하여 엔드 이펙터(213)의 위치 정보를 추정하는 동시에 엔드 이펙터(213)에 가해지는 외력을 추정할 수 있다. 이에 대해서는 추후 상세히 설명할 것이다.
또한, 비전 센서(221)는 엔드 이펙터(213)에 대한 영상을 획득하기 위한 구성이며, 본 실시 예에서 비전 센서(221)로는 내시경이 사용될 수 있으나, 특별히 이에 한정되는 것은 아니다.
이때, 내시경은 입체 영상을 획득하기 위한 스테레오 카메라 및 조명수단 등을 포함할 수 있으나, 특별히 이에 한정되는 것은 아니다. 이때, 카메라로는 CMOS(Complementary Metal-Oxide Semiconductor) 카메라 또는 CCD(Charge Coupled Device) 카메라 등이 사용될 수 있으나, 특별히 이에 한정되는 것은 아니다. 또한, 내시경으로는 로봇 수술에서 주로 사용되는 복강경뿐 아니라, 흉강경, 관절경, 비경, 방광경, 직장경, 십이지장경, 심장경 등 다양한 수술용 내시경이 사용될 수 있다.
엔드 이펙터(213)는 수술 도구(210)에서 환자의 수술 부위에 실제로 작용하는 부분을 의미하는 것으로, 예로써, 클램프, 그래스퍼, 가위, 스태플러, 바늘 홀더, 메스, 절단 블레이드 등을 포함할 수 있으나, 특별히 이에 한정되는 것은 아니며, 수술에 필요한 공지된 도구라면 어떤 것이든 사용 가능할 것이다.
도 1에 도시한 바와 같이, 본 실시 예에서 수술 도구(210) 및 비전 센서(221)는 환자 신체(예: 복부)에 형성된 하나의 침습구에 설치된 트로카(300)를 통해 복강 내부로 삽입될 수 있으나, 특별히 이에 한정되는 것은 아니다.
또한, 도 1에서는 하나의 모터(270), 하나의 선형 구동기(260) 및 하나의 케이블(250)을 도시하고 있으나, 이는 설명의 편의를 위하여 개략적으로 도시한 것일 뿐, 모터(270), 선형 구동기(260) 및 케이블(250)은 수술 도구(210)에 포함된 관절의 개수에 대응되는 개수만큼 구비되는 것 역시 가능할 것이다.
슬레이브 장치(200)의 제어부(230)는 상술한 바와 같이, 마스터 콘솔(100)로부터 전송되는 제어 신호에 따라 모터(270)를 제어하는 동작을 수행할 수 있다.
구체적으로 설명하면, 슬레이브 장치(200)의 제어부(230)는 마스터 콘솔(100)로부터 입력되는 제어 신호를 엔드 이펙터(213)에 대한 영상좌표 상의 목표 위치로 변환하고, 비전 센서(221) 및 장력 센서(223)를 이용하여 엔드 이펙터(213)의 위치 정보 및 케이블(250)에 걸리는 장력를 검출한 후, 검출된 위치 정보 및 장력를 기생성된 모델에 적용하여 엔드 이펙터(213)에 대한 영상좌표 상의 현재 위치 및 엔드 이펙터(213)에 가해지는 외력을 추정하고, 추정된 현재 위치와 변환된 목표 위치의 차이 값을 연산한 다음, 연산된 차이 값을 이용하여 보정 신호를 생성한 후, 생성된 보정 신호를 수술 도구(210)의 복수의 관절 각각에 대한 구동부 즉, 모터(270)로 인가하는 동작을 할 수 있다.
여기에서, "제어 신호"는 엔드 이펙터(213)가 조작자가 원하는 공간상의 위치로 이동시키기 위한 정보를 포함하는 신호일 수 있다.
또한, 본 실시 예에 따른 슬레이브 장치(200)의 제어부(230)는 도 2에 도시한 바와 같이, 상태 추정부(231) 및 보정 신호 생성부(233)를 포함할 수 있으나, 특별히 이에 한정되는 것은 아니다.
상태 추정부(231)는 엔드 이펙터(213)의 현재 상태 구체적으로, 영상좌표 상의 현재 위치 및 가해지는 외력을 추정하기 위한 구성으로서, 입력되는 제어 신호와 비전 센서(221) 및 장력 센서(223)를 통해 검출된 위치 정보 및 장력을 기생성된 모델 즉, 다관절 매니퓰레이터의 시스템 모델 및 센서 모델에 적용하여 엔드 이펙터(213)의 현재 상태를 추정할 수 있으나, 특별히 이에 한정되는 것은 아니다.
여기에서, "시스템 모델" 및 "센서 모델"은 다음과 같은 방법을 통해 생성될 수 있다.
예를 들어, 수술 도구(210)의 각 관절에는 마찰력과 외력이 작용하고 있으며, 각 관절을 구동하는 케이블(250)은 탄성이 있어서 힘이 작용하면 늘어나는 특성을 가질 수 있다. 이와 같은 수술 도구(210)의 관절 하나를 단순화하여 표현하면 아래의 [수학식 1]과 같다.
Figure pat00001
여기에서,
Figure pat00002
은 엔드 이펙터(213)의 질량,
Figure pat00003
는 감쇠 계수,
Figure pat00004
는 탄성 계수,
Figure pat00005
은 마스터 콘솔(100)로부터 입력되는 엔드 이펙터(213)의 목표 위치,
Figure pat00006
은 엔드 이펙터(213)의 현재 위치,
Figure pat00007
은 케이블(250)에 걸리는 힘,
Figure pat00008
는 엔드 이펙터(213)에 가해지는 외력,
Figure pat00009
는 케이블(250)에 작용하는 마찰력을 의미할 수 있다.
이러한 [수학식 1]을
Figure pat00010
를 추정 변수로 사용하여 행렬 형태를 갖는 시스템 모델 및 센서 모델로 변형하면 각각 아래의 [수학식 2] 및 [수학식 3]과 같다.
Figure pat00011
Figure pat00012
여기에서,
Figure pat00013
는 엔드 이펙터(213) 이동 속도 변화량,
Figure pat00014
은 영상좌표 상의 엔드 이펙터(213) 위치 변화량,
Figure pat00015
는 엔드 이펙터(213)에 가해지는 외력 변화량,
Figure pat00016
은 마스터 콘솔(100)로부터 입력되는 엔드 이펙터(213)의 이동 속도,
Figure pat00017
는 엔드 이펙터(213) 이동 속도에 대한 추정변수,
Figure pat00018
은 엔드 이펙터(213) 현재 위치에 대한 추정 변수,
Figure pat00019
는 엔드 이펙터(213)에 가해지는 외력에 대한 추정변수,
Figure pat00020
,
Figure pat00021
및
Figure pat00022
는 시스템 모델의 정확도를 나타내는 화이트 노이즈 레벨(white noise level),
Figure pat00023
는 비전 센서(221) 출력 값에 대한 상태변수,
Figure pat00024
는 장력 센서(223) 출력 값에 대한 상태변수,
Figure pat00025
는 비전 센서(221)의 정확도를 나타내는 화이트 노이즈 레벨(white noise level),
Figure pat00026
는 장력 센서(223)의 정확도를 나타내는 화이트 노이즈 레벨(white noise level)을 의미할 수 있다.
이때, 상술한 [수학식 3]에서는
Figure pat00027
이 장력 센서(223)에 의해 직접적으로 추정될 수 있는 것으로 표현하고 있으나, 실제로 본 실시 예에 따른 수술 도구(210)는 다관절 매니퓰레이터 형태이기 때문에 상술한 [수학식 3]에서의
Figure pat00028
는 아래의 [수학식 4]를 통해 간접적으로 얻어질 수 있다.
Figure pat00029
여기에서,
Figure pat00030
는 장력 센서(223) 출력 값이고,
Figure pat00031
및
Figure pat00032
은 각각 아래의 [수학식 5] 및 [수학식 6]에 대한 자코비안(Jacobian) 행렬식이다.
Figure pat00033
Figure pat00034
여기에서,
Figure pat00035
는 데카르트(Cartesian) 좌표계의 (x, y, z, q0, q1, q2, q3)으로 구성된 포즈(pose) 상태 변수이며,
Figure pat00036
은 관절 각을 생성하기 위한 케이블의 길이,
Figure pat00037
는 각 관절에 대한 관절 각 벡터를 의미할 수 있다.
즉, 케이블(240)을 통해 구동력이 전달되는 다관절 매니퓰레이터 형태의 수술 도구(210)는 엔드 이펙터(213)에 가해지는 외력이 하나의 관절에만 영향을 미치는 것이 아니라, 모든 관절로 분산될 수 있다. 이에 따라, 상술한 바와 같이, 기구학 정보를 갖는 자코비안(Jacobian) 행렬식을 이용함으로써, 엔드 이펙터(213)에 가해지는 외력을 정확하게 추정할 수 있다.
본 실시 예에서와 같이 케이블을 통해 구동력이 전달되는 다관절 매니퓰레이터 형태의 수술 도구는 기구학 정보만을 이용하여 생성한 제어 명령만으로는 상술한 백래시(backlash)로 인한 비선형적 특성에 따른 모델의 부정확성으로 인하여 오차가 발생하게 된다. 예를 들어, 멀티 포트 수술 로봇의 경우 대부분의 자유도를 복강 외에서 처리함으로써, 복강 내에서는 자유도가 많지 않아 모델이 간단한 반면, 싱글 포트 수술 로봇의 경우 모든 자유도를 복강 내에서 구현하게 되므로, 오차들이 누적되고, 이 오차가 병진 위치 오차뿐 아니라, 자세 각 오차에 파급되어 조작자를 혼란스럽게 할 수 있다. 이와 같은 병진 위치에 나타나는 오차에 비하여 자세 각 오차는 직관적인 수술 도구 조작을 방해하는 요소가 될 수 있다. 이에 따라, 능동적으로 마스터 콘솔(100)로부터 입력되는 목표 위치로 수술 도구(210)의 엔드 이펙터(213)를 정확하게 정렬시키기 위한 피드백 제어 과정이 반드시 필요하다.
마스터 콘솔(100)로부터 입력되는 제어 신호와 엔드 이펙터(213)의 위치 관계는 다양한 방법으로 정의될 수 있다. 일반적인 방법으로는 마스터 콘솔(100)로부터 조작자에 의해 조작된 입력부(110)에 대한 x, y, z, 롤(roll), 피치(pitch), 요(yaw)의 6 자유도(DOF) 정보를 갖는 제어 신호를 전달받아 사용할 수 있다. 이외의 다른 방법으로는 카메라 영상에서 일정 픽셀(pixel)을 선정하고, 선정된 픽셀의 (x, y)값을 전달하면 엔드 이펙터(213)가 카메라 상에서 전달된 (x, y) 위치에 배치되도록 하는 방법이 사용될 수 있으나, 특별히 이에 한정되는 것은 아니다.
이때, 상술한 제어 신호는 관절 각에 대한 함수로만 표현된다면 어떠한 형태로도 구현될 수 있으며, 예를 들어, 아래의 [수학식 7]과 같이 구현될 수 있다.
Figure pat00038
여기에서,
Figure pat00039
는 각 관절에 대한 관절 각 벡터이고,
Figure pat00040
은 관절 각 벡터를 제어 신호로 변환하는 함수를 의미할 수 있다. 이를 이용하여 마스터 콘솔(100)로부터 입력되는 제어 신호를 각 관절에 대한 관절 각 벡터로 변환할 수 있을 것이다.
또한, 엔드 이펙터(213)의 위치를 측정하는 비전 센서(221)의 출력 값에 대한 함수 역시 상술한 [수학식 7]의 형태와 유사하게 표현될 수 있으며, 이는 아래의 [수학식 8]과 같다.
Figure pat00041
여기에서,
Figure pat00042
는 관절 각 벡터를 영상 좌표 상의 위치로 변환하는 함수를 의미할 수 있다.
상술한 [수학식 7] 및 [수학식 8]을 각각 자코비안(Jacobian) 행렬 형태로 변환하면, 아래의 [수학식 9] 및 [수학식 10]과 같다.
Figure pat00043
Figure pat00044
이러한 [수학식 9] 및 [수학식 10]에서 매개 변수(
Figure pat00045
)를 제거하고, 의사역행렬을 통하여 계산하면 아래의 [수학식 11]과 같은 마스터 콘솔(100)로부터 입력되는 제어 신호와 비전 센서(221)의 출력 값과의 관계식을 구할 수 있다.
Figure pat00046
이를 다시 아래의 [수학식 12]와 같이 표현할 수 있다.
Figure pat00047
여기에서,
Figure pat00048
는 마스터 콘솔(100)로부터 입력되는 제어 신호에 해당하는 영상 좌표 상의 목표 위치 정보를 계산하는 자코비안(Jacobian) 관계식을 의미할 수 있다. 본 실시 예에 따른 제어부(230)는 이와 같은 [수학식 12]를 이용하여 마스터 콘솔(100)로부터 입력되는 제어 신호를 엔드 이펙터(213)의 영상좌표 상의 목표 위치로 변환할 수 있다.
단, 여기에서 획득된
Figure pat00049
의 차수(dimension)가
Figure pat00050
의 차수보다 크고,
Figure pat00051
의 랭크(rank)가
Figure pat00052
의 차수보다 큰 조건을 만족하는 것이 바람직할 것이다. 이는, 동일한 제어 신호에 대하여 2개 이상의
Figure pat00053
값을 갖게 되어 조종이 불가능한 경우를 방지하기 위함이다.
상술한 [수학식 1] 내지 [수학식 12]를 사용하여 구현된 제어부(230)의 구조를 도 4에 도시하였다.
도 4를 참조하면, 조작자가 원하는 엔드 이펙터(213) 위치에 대한 명령인 제어 신호(
Figure pat00054
)가 마스터 콘솔(100)로부터 입력되면, 제어부(230)는 자코비안(Jacobian) 관계식
Figure pat00055
를 이용하여 입력된 제어 신호(
Figure pat00056
)를 대응되는 엔드 이펙터(213)의 영상좌표 상의 목표 위치(
Figure pat00057
)로 변환한다.
이후, 제어부(230)는 변환된 영상좌표 상의 목표 위치(
Figure pat00058
)와 상태 추정부(231)를 통해 추정된 영상좌표 상의 현재 위치(
Figure pat00059
)를 비교하여 차이 값을 연산하고, 보정 신호 생성부(233)를 이용하여 연산된 차이 값에 따른 보정 신호를 생성하여 모터(270)로 전달한다.
이때, 보정 신호를 생성하는 과정에 대하여 구체적으로 설명하면 다음과 같다. 즉, 도 4에 도시한 바와 같이, 영상좌표 상의 목표 위치(
Figure pat00060
)와 추정된 영상좌표 상의 현재 위치(
Figure pat00061
)의 차이 값에 비례 상수(
Figure pat00062
)를 곱하고, 비례 상수(
Figure pat00063
)를 곱한 값에 자코비안(Jacobian)
Figure pat00064
의 의사역행렬을 곱하여 차이 값을 줄이는 방향의 관절 각 속도(
Figure pat00065
)를 산출하고, 산출된 관절 각 속도(
Figure pat00066
)를 적분하여 관절 각(
Figure pat00067
)을 생성한다. 제어부(230)는 이와 같이 생성된 관절 각(
Figure pat00068
)에 대응되는 전기신호를 모터(270)로 인가하여 모터(270) 구동을 제어한다.
이와 같이, 본 실시 예에서 상태 추정부(231)는 센서부(220)를 통해 검출된 다양한 정보, 구체적으로, 비전 센서(221) 및 장력 센서(223)를 통해 각각 검출된 엔드 이펙터(213)의 위치 정보 및 케이블(250)에 걸리는 장력과 기생성된 시스템 모델([수학식 2]) 및 센서 모델([수학식 3])을 최적으로 조합하여 엔드 이펙터(213)에 대한 영상좌표 상의 현재 위치 및 엔드 이펙터(213)에 가해지는 외력을 추정할 수 있다.
또한, 제어부(230)는 모터(270) 구동을 제어함으로써, 선형 구동기(260)의 위치를 결정하고, 이에 따라 선형 구동기(260)에 연결되어 있는 케이블(250)을 통하여 관절 자세 각이 결정될 수 있다. 이때, 선형 구동기(260)의 위치 제어는 정확하게 이루어질 수 있지만, 케이블(250)로 연결된 각 관절에는 케이블(250)이 거쳐가는 경로에 마찰이 발생하여 백래시(backlash)가 발생하며, 이와 같은 백래시는 수술 도구(210) 자세 각의 조합에 따라 변하기 때문에 간단한 상수 형태로 보상할 수 없다. 이와 같이, 입력되는 제어 신호와 출력 신호 사이에 히스테리시스(Histerisis)가 발생하는데 이를 정확하게 보상하지 않고, 비례 제어기를 사용하는 경우 떨림 현상이 발생할 수 있다.
이러한 문제를 해결하기 위해 본 실시 예에서는 상술한 바와 같이 관절 각(
Figure pat00069
)이 아닌 관절 각속도(
Figure pat00070
)를 이용한 제어부(230)를 구현하고 있으며, 이는 PID(Proportional Integral Derivative)제어에서 I(Integral) 제어를 강화하는 방법으로, 이에 따라 상술한 떨림 문제를 해결할 수 있다.
본 실시 예에서 상술한 구성의 상태 추정부(231)를 사용하는 이유는 다음과 같이 세 가지가 있다.
첫 번째는, 각 관절에 설치된 센서가 없기 때문에 엔드 이펙터(213)의 위치를 모터(270)의 입력 신호를 통하여 정확하게 추정하기 어려움에 따라, 수술 도구(210)의 각 관절에 대한 관절 각을 직접적으로 측정하지 않고, 엔드 이펙터(213)의 위치나 케이블(250)에 걸리는 장력 같은 다른 정보를 검출하여 각 관절에 대한 관절 각을 간접적으로 획득하는 방법이 요구되기 때문이다.
두 번째는, 제어 신호가 입력되는 주기에 비해 비전 센서(221)를 통해 영상을 획득하는 주기가 현저하게 크기 때문이다. 즉, 센서 정보가 획득되지 않는 동안에는 입력된 제어 신호를 이용하여 엔드 이펙터(213)의 상태를 예측하고, 센서 정보가 획득되면 획득된 센서 정보를 이용하여 제어 신호를 이용하여 예측된 엔드 이펙터(213)의 상태를 보정해가는 추정 방법을 사용함으로써, 입력되는 제어 신호와 센서 정보를 최적으로 사용하고자 하는 것이다.
이때, 비전 센서(221)를 통한 영상 획득을 수행할 때, 영상 획득이 수행되지 못하여 센서 정보가 제공되지 않는 경우, 제어부(230)는 입력된 제어 신호만을 사용하여 센서 정보가 제공되지 않는 구간은 개루프(Open loop) 형태로 동작하다가 센서 정보가 획득되면 획득된 센서 정보를 이용한 폐루프(Closed loop) 형태로 동작할 수 있다.
세 번째는, 위치 정보 및 장력과 같이 물리량이 다른 정보들을 이용하여 엔드 이펙터(213)의 모든 상태를 최적으로 추정하기 위함이다. 즉, 본 실시 예에서는 상술한 시스템 모델(수학식 2) 및 센서부 모델(수학식 3)과 같이 위치와 힘의 관계가 서로 연동될 수 있는데, 이러한 위치와 힘 각각에 대한 신뢰성은 다르기 때문에 신뢰성을 고려하여 획득하고자 하는 상태를 확률적으로 가장 신뢰가 가는 추정변수로 제안하게 되는 것이다.
본 실시 예에서는 수술 도구(210)의 각 관절에 대한 관절 각을 상태변수로 구현하는 대신, 센서부(220)를 통해 출력된 센서 값에 대한 상태변수를 사용하였다. 즉, 상태 추정부(231)는 관절 각에 대한 상태변수가 아닌 센서 값에 대한 상태변수를 사용하며, 이에 따라, 센서부(220)로부터 출력된 값을 직접적으로 입력되는 제어 신호에 맞출 수 있는 출력 피드백(output feedback) 형태의 제어가 가능하다.
즉, 본 실시 예에서는 도 4와 같이, 입력되는 제어 신호(
Figure pat00071
)를 자코비안(Jacobian) 관계식
Figure pat00072
를 이용하여 대응되는 엔드 이펙터(213)의 영상좌표 상의 목표 위치(
Figure pat00073
)로 변환하게 되는데, 상태 추정부(231)는 센서부(220)를 통해 영상좌표 상의 목표 위치(
Figure pat00074
)와 동일한 차수의 영상좌표 상의 현재 위치(
Figure pat00075
)를 추정하면 되고, 굳이 변환된 영상좌표 상의 목표 위치(
Figure pat00076
)보다 차수가 큰 모든 관절 각을 추정하지 않아도 되는 이점이 있다.
본 실시 예에서 상태 추정부(231)는 일반적으로 칼만 필터(Kalman Filter) 기반 추정기를 사용하여 구성될 수 있다. 이때, 사용되는 칼만 필터(Kalman Filter)로는 선형 칼만 필터(Linear Kalman Filter), 확장 칼만 필터(Extended Kalman Filter), UKF(Uncented Kalman Filter), 파티클 필터(Particle Filter), MCMC(Marcov Chain Monte Carlo) 등이 사용될 수 있으나, 특별히 이에 한정되는 것은 아니다.
일반적으로, 확률 기반 추정기로서 칼만 필터(Kalman Filter)는 예측(Prediction)과 업데이트(Update) 두 과정으로 분리될 수 있는데 본 실시 예에서는 우선, 마스터 콘솔(100)로부터 입력되는 제어 신호와 모델 오차에 대한 정보를 통하여 상태 예측(Prediction)을 수행할 수 있다. 이때, 예측(Prediction)의 정밀도는 모델링을 얼마나 정확히 했는가에 따라 결정될 수 있는데, 완벽하게 모델링을 수행하였다면 공분산(Covariance)이 영행렬이 될 것이다. 그러나, 본 실시 예와 같이 케이블(250)을 통해 구동력을 전달하는 경우, 비선형 특성을 가짐에 따라 완벽한 모델링은 불가능하므로 모델의 오차 범위를 크게 설정하게 된다. 여기에서, 오차 범위는 입력되는 제어 신호에 대한 예측치와 실제 값의 오차에 대한 데이터를 이용하여 오차 분산값을 구하여 산출할 수 있으며, 이와 같이 산출된 오차 범위가 곧 모델 오차가 될 수 있다.
즉, 케이블(250)을 구동력 전달 수단으로 사용하는 경우, 상술한 오차 범위는 가장 큰 오차 발생원인이 되는 백래시(backlash)의 데드존(deadzone)을 예측함으로써 산출될 수 있는데, 이때, 오차 범위는 수술 도구(210)의 자세, 케이블(250)의 마모 정도 및 엔드 이펙터(213)에 가해지는 외력에 의해 영향을 받을 수 있다. 특히, 외력의 경우 이를 측정하는 센서가 없으면 측정이 불가능하기 때문에 상당히 큰 공분산 분포를 가질 수 있다.
본 실시 예에 따른 상태 추정부(231)는 마스터 콘솔(100)로부터 입력되는 제어 신호(
Figure pat00077
)에 기초하여 수술 도구(210)의 복수의 관절 각각에 대한 관절 각을 예측하게 되며, 입력되는 제어 신호를 빠른 주기로 획득할 수 있으므로, 이에 대한 예측 과정은 매우 빨리 실행될 수 있다.
또한, 상태 추정부(231)는 센서부(220)를 통해 검출된 엔드 이펙터(213)에 대한 상태 정보를 이용하여 앞선 입력되는 제어 신호(
Figure pat00078
)에 대한 예측 과정을 통해 진행된 예측치를 보상하여 최종 상태를 추정할 수 있다. 이때, 보상되는 값은 예측치에 의해 계산된 오차 공분산과 센서부(220) 자체의 오차 공분산과의 관계를 통하여 가중치를 갖는 합으로 계산될 수 있다. 즉, 센서부(220)의 정밀도가 커지면 센서부(220)의 가중치가 커지고, 제어 신호에 대한 예측치의 정밀도가 더 크면 예측치에 가중치가 커지는 형태로 칼만 게인(Kalman Gain)이 산출될 수 있다.
이에 따라, 본 실시 예에 따른 상태 추정부(231)는 제어 신호에 대한 예측치를 이용하여 상태를 추정하면서 센서부(220)를 통해 검출된 정보를 이용하여 예측치에 의해 누적된 오차를 보정함으로써, 실제 값에 대한 정보를 정확하게 추정할 수 있다. 또한, 센서부(220)의 획득 주기와는 상관없이 고주파의 제어루틴을 수행할 수 있는 이점이 있다.
이와 같은 상태 추정부(231)의 동작을 도 5에 도시하였다.
도 5를 참조하면, 상태 추정부(231)에서는 센서부(220)에서 신호가 획득되지 않으면 제어 입력 정보와 모델을 사용하여 예측 과정만 수행하고, 센서부(220)에서 신호가 획득되면 업데이트 과정을 통하여 예측치를 보정할 수 있다. 상태 추정부(231)의 이러한 기능을 통하여 추정 주기와 센서 획득 주기를 동기화하지 않더라도 제어를 수행할 수 있다. 도 5에서
Figure pat00079
은 상태 변수 추정치를 나타내고,
Figure pat00080
은 상태 변수 추정 확률을 의미하는 공분산을 나타낸다.
상술한 바와 같이, 비전 센서(221) 및 장력 센서(223)를 통해 검출된 엔드 이펙터(213)에 대한 상태 정보를 이용하여 다관절 매니퓰레이터를 제어하는 방법을 도 6에 도시하였다.
도 6을 참조하면, 우선, 슬레이브 장치(200)의 제어부(230)는 마스터 콘솔(100)로부터 수술 도구(210)에 대한 제어 신호가 입력되는지를 판단한다(S601). 제어 신호가 입력되면, 입력된 제어 신호를 대응되는 엔드 이펙터(213)에 대한 영상좌표 상의 목표 위치로 변환한다(S603).
또한, 슬레이브 장치(200)의 제어부(230)는 비전 센서(221) 및 장력 센서(223)를 이용하여 엔드 이펙터(213)에 대한 상태 정보를 검출하고(S605), 마스터 콘솔(100)로부터 입력되는 제어 신호 및 검출된 상태 정보를 기생성된 센서 모델([수학식 3]) 및 시스템 모델([수학식 2])에 적용하여 엔드 이펙터(213)에 대한 영상좌표 상의 현재 위치 및 외력을 추정한다(S607).
여기에서, "상태 정보"는 비전 센서(221)를 통해 획득된 이미지를 이용하여 검출된 엔드 이펙터(213)의 위치 정보 및 장력 센서(223)를 통해 검출된 수술 도구(210) 각 관절에 연결된 케이블(250)에 걸리는 장력을 포함할 수 있다.
다음, 단계 S607을 통해 추정된 엔드 이펙터(213)에 대한 영상좌표 상의 현재 위치와 단계 S603을 통해 변환된 엔드 이펙터(213)에 대한 영상좌표 상의 목표 위치의 차이 값을 연산하고(S609), 연산된 차이 값을 이용하여 보정 신호를 생성한 후(S611), 생성된 보정 신호를 수술 도구(210)의 각 관절 구동부 즉, 모터(270)로 인가한다(S613).
이때, 슬레이브 장치(200)의 제어부(230)는 상술한 단계 S607을 통해 추정된 외력을 통신부(240)를 통해 마스터 콘솔(100)로 전송하고, 마스터 콘솔(100)은 슬레이브 장치(200)로부터 전송된 외력을 이용하여 조작자가 엔드 이펙터(213)에 가해지는 외력을 느낄 수 있도록 입력부(110)로의 포스 피드백(force feedback)을 수행할 수 있다.
이와 같이, 본 실시 예에서는 비전 센서(221)뿐 아니라 장력 센서(223)를 통해 검출된 엔드 이펙터(213)에 대한 위치 정보 및 케이블(250)에 걸린 장력을 이용함으로써, 각각의 관절에 대한 각도를 직접 측정하지 않아도 엔드 이펙터(213)에 대한 위치 정보를 더욱 정확하게 추정할 수 있고, 동시에 엔드 이펙터(213)에 가해지는 외력을 추정할 수 있다.
이상 본 발명을 구체적인 실시 예를 통하여 상세히 설명하였으나, 이는 본 발명을 구체적으로 설명하기 위한 것으로 본 발명은 이에 한정되지 않으며, 본 발명의 기술적 사상 내에서 당 분야의 통상의 지식을 가진 자에 의해 그 변형이나 개량이 가능함이 명백하다.
본 발명의 단순한 변형 내지 변경은 모두 본 발명의 영역에 속하는 것으로 본 발명의 구체적인 보호 범위는 첨부된 특허청구범위에 의하여 명확해질 것이다.
100 : 마스터 콘솔
110 : 입력부
111, 113 : 핸들
120 : 표시부
130 : 제어부
135 : 제어 신호 생성부
140 : 통신부
200 : 슬레이브 장치
210 : 수술 도구
220 : 센서부
221 : 비전 센서
223 : 장력 센서
230 : 제어부
231 : 상태 추정부
233 : 보정 신호 생성부
240 : 통신부
250 : 케이블
260 : 선형 구동기
270 : 모터

Claims (20)

  1. 끝단에 엔드 이펙터가 결합된 복수의 관절을 갖는 다관절 매니퓰레이터의 제어 방법으로서,
    입력되는 제어 신호를 상기 엔드 이펙터에 대한 영상좌표 상의 목표 위치로 변환하는 단계;
    비전 센서를 이용하여 상기 엔드 이펙터에 대한 위치 정보를 검출하는 단계;
    검출된 상기 위치 정보를 이용하여 상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 추정하는 단계;
    상기 목표 위치와 상기 현재 위치의 차이 값을 연산하는 단계;
    연산된 상기 차이 값을 이용하여 보정 신호를 생성하는 단계; 및
    생성된 상기 보정 신호를 상기 복수의 관절 각각에 대한 구동부로 인가하는 단계
    를 포함하는 다관절 매니퓰레이터 제어 방법.
  2. 제1항에 있어서,
    상기 엔드 이펙터에 대한 영상좌표 상의 목표 위치로 변환하는 단계는 하기 식(1)을 이용하여 수행되고,
    Figure pat00081
    (1)
    여기에서,
    Figure pat00082
    은 상기 입력되는 제어 신호에 대한 상기 엔드 이펙터의 목표 위치이고,
    Figure pat00083
    는 관절 각 벡터이며,
    Figure pat00084
    는 관절 각 벡터를 제어 신호로 변환하는 함수이고,
    Figure pat00085
    는 관절 각 벡터를 영상좌표 상의 위치로 변환하는 함수인 다관절 매니퓰레이터 제어 방법.
  3. 제2항에 있어서,
    상기 보정 신호를 생성하는 단계는,
    상기 차이 값에 기설정된 비례 상수를 곱하는 단계;
    상기 비례 상수가 곱해진 차이 값에 상기
    Figure pat00086
    의 의사역행렬을 곱하여 상기 차이 값을 감소시키는 방향의 관절 각속도를 연산하는 단계; 및
    연산된 상기 관절 각속도를 적분하여 관절 각을 산출하는 단계
    를 포함하는 다관절 매니퓰레이터 제어 방법.
  4. 제1항에 있어서,
    상기 엔드 이펙터에 대한 위치 정보를 검출하는 단계는,
    장력 센서를 이용하여 상기 복수의 관절 각각에 연결된 케이블에 걸리는 장력을 검출하는 단계를 더 포함하는 다관절 매니퓰레이터 제어 방법.
  5. 제4항에 있어서,
    상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 추정하는 단계에서,
    상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치는 상기 검출된 위치 정보 및 장력에 기초하여 추정되는 다관절 매니퓰레이터 제어 방법.
  6. 제4항에 있어서,
    상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 추정하는 단계는,
    상기 검출된 장력을 이용하여 상기 엔드 이펙터에 가해지는 외력을 추정하는 단계를 포함하는 다관절 매니퓰레이터 제어 방법.
  7. 제6항에 있어서,
    상기 엔드 이펙터에 대한 현재 위치 및 외력은 상기 검출된 엔드 이펙터에 대한 위치 정보와 상기 케이블에 걸리는 장력 및 상기 입력되는 제어 신호를 기생성된 모델에 적용하여 추정되는 다관절 매니퓰레이터 제어 방법.
  8. 제7항에 있어서,
    상기 기생성된 모델은 시스템 모델 및 센서 모델을 포함하는 다관절 매니퓰레이터 제어 방법.
  9. 제8항에 있어서,
    상기 시스템 모듈은 하기 식(2)와 같이 정의되고,
    Figure pat00087
    (2)
    여기에서,
    Figure pat00088
    은 상기 엔드 이펙터의 질량,
    Figure pat00089
    는 상기 엔드 이펙터 이동 속도 변화량,
    Figure pat00090
    은 영상좌표 상의 상기 엔드 이펙터 위치 변화량,
    Figure pat00091
    는 상기 엔드 이펙터에 가해지는 외력 변화량,
    Figure pat00092
    는 감쇠 계수,
    Figure pat00093
    는 탄성 계수,
    Figure pat00094
    은 입력되는 상기 엔드 이펙터의 이동 속도,
    Figure pat00095
    은 입력되는 상기 엔드 이펙터의 목표 위치,
    Figure pat00096
    는 상기 케이블에 걸리는 힘,
    Figure pat00097
    는 상기 케이블에 작용하는 마찰력,
    Figure pat00098
    는 상기 엔드 이펙터 이동 속도에 대한 추정변수,
    Figure pat00099
    은 영상좌표 상의 상기 엔드 이펙터 위치에 대한 추정변수,
    Figure pat00100
    는 상기 엔드 이펙터에 가해지는 외력에 대한 추정변수,
    Figure pat00101
    ,
    Figure pat00102
    및
    Figure pat00103
    는 시스템 모델의 정확도를 나타내는 화이트 노이즈 레벨(white noise level)인 다관절 매니퓰레이터 제어 방법.
  10. 제8항에 있어서,
    상기 센서 모델은 하기 식(3)과 같이 정의되고,
    Figure pat00104
    (3)
    여기에서,
    Figure pat00105
    는 상기 비전 센서 출력 값에 대한 상태변수,
    Figure pat00106
    는 상기 장력 센서 출력 값에 대한 상태변수,
    Figure pat00107
    는 상기 엔드 이펙터 이동 속도에 대한 추정변수,
    Figure pat00108
    은 영상좌표 상의 상기 엔드 이펙터 위치에 대한 추정변수,
    Figure pat00109
    는 상기 엔드 이펙터에 가해지는 외력에 대한 추정변수,
    Figure pat00110
    및
    Figure pat00111
    는 센서 모델의 정확도를 나타내는 화이트 노이즈 레벨(white noise level)인 다관절 매니퓰레이터 제어 방법.
  11. 제8항에 있어서,
    상기 제어 신호가 입력되는 주기와 상기 위치 정보 또는 장력이 검출되는 주기가 서로 다른 경우,
    상기 위치 정보 또는 장력이 검출되지 않는 동안에는 상기 입력되는 제어 신호를 상기 시스템 모델에 적용하여 상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 예측하고,
    상기 위치 정보 또는 장력이 검출되면 검출된 위치 정보 또는 장력과 상기 입력되는 제어 신호를 각각 상기 센서 모델과 상기 시스템 모델에 적용하여 상기 엔드 이펙터에 대한 보정된 영상좌표 상의 현재 위치 및 상기 엔드 이펙터에 가해지는 외력을 추정하는 다관절 매니퓰레이터 제어 방법.
  12. 끝단에 엔드 이펙터가 결합된 복수의 관절을 갖는 다관절 매니퓰레이터;
    상기 엔드 이펙터에 대한 영상을 획득하는 비전 센서; 및
    입력되는 제어 신호를 상기 엔드 이펙터에 대한 영상좌표 상의 목표 위치로 변환하고, 상기 비전 센서를 이용하여 상기 엔드 이펙터에 대한 위치 정보를 검출한 후, 검출된 위치 정보를 이용하여 상기 엔드 이펙터에 대한 영상좌표 상의 현재 위치를 추정하고, 추정된 상기 현재 위치와 상기 변환된 목표 위치의 차이 값을 연산한 다음, 연산된 상기 차이 값을 이용하여 보정 신호를 생성한 후, 생성된 보정 신호를 상기 복수의 관절 각각에 대한 구동부로 인가하는 제어부
    를 포함하는 다관절 매니퓰레이터 제어 시스템.
  13. 제12항에 있어서,
    상기 제어부는 상기 입력되는 제어 신호를 하기 식(1)을 이용하여 상기 엔드 이펙터에 대한 영상좌표 상의 목표 위치로 변환하고,
    Figure pat00112
    (1)
    여기에서,
    Figure pat00113
    은 상기 입력되는 제어 신호에 대한 상기 엔드 이펙터의 목표 위치이고,
    Figure pat00114
    는 관절 각 벡터이며,
    Figure pat00115
    는 관절 각 벡터를 제어 신호로 변환하는 함수이고,
    Figure pat00116
    는 관절 각 벡터를 영상좌표 상의 위치로 변환하는 함수인 다관절 매니퓰레이터 제어 시스템.
  14. 제13항에 있어서,
    상기 제어부는 상기 연산된 차이 값에 기설정된 비례 상수를 곱한 다음, 상기 비례 상수가 곱해진 차이 값에 상기
    Figure pat00117
    의 의사역행렬을 곱하여 상기 차이 값을 감소시키는 방향의 관절 각속도를 연산한 후, 연산된 상기 관절 각속도를 적분하여 관절 각을 산출함으로써 상기 보정 신호를 생성하는 다관절 매니퓰레이터 제어 시스템.
  15. 제12항에 있어서,
    상기 복수의 관절 각각에 구동력을 전달하는 복수의 케이블; 및
    상기 복수의 케이블 각각에 마련된 장력 센서
    를 더 포함하는 다관절 매니퓰레이터 제어 시스템.
  16. 제15항에 있어서,
    상기 제어부는 상기 장력 센서를 이용하여 상기 복수의 케이블 각각에 걸리는 장력을 검출하고, 검출된 장력을 이용하여 상기 엔드 이펙터에 대한 현재 위치 및 외력을 추정하는 다관절 매니퓰레이터 제어 시스템.
  17. 제16항에 있어서,
    상기 제어부는 상기 검출된 엔드 이펙터에 대한 위치 정보와 상기 케이블에 걸리는 장력 및 상기 입력되는 제어 신호를 기생성된 모델에 적용하여 상기 엔드 이펙터에 대한 현재 위치 및 외력을 추정하는 다관절 매니퓰레이터 제어 시스템.
  18. 제17항에 있어서,
    상기 기생성된 모델은 시스템 모델 및 센서 모델을 포함하는 다관절 매니퓰레이터 제어 시스템.
  19. 제18항에 있어서,
    상기 시스템 모듈은 하기 식(2)와 같이 정의되고,
    Figure pat00118
    (2)
    여기에서,
    Figure pat00119
    은 상기 엔드 이펙터의 질량,
    Figure pat00120
    는 상기 엔드 이펙터 이동 속도 변화량,
    Figure pat00121
    은 영상좌표 상의 상기 엔드 이펙터 위치 변화량,
    Figure pat00122
    는 상기 엔드 이펙터에 가해지는 외력 변화량,
    Figure pat00123
    는 감쇠 계수,
    Figure pat00124
    는 탄성 계수,
    Figure pat00125
    은 입력되는 상기 엔드 이펙터의 이동 속도,
    Figure pat00126
    은 입력되는 상기 엔드 이펙터의 목표 위치,
    Figure pat00127
    는 상기 케이블에 걸리는 힘,
    Figure pat00128
    는 상기 케이블에 작용하는 마찰력,
    Figure pat00129
    는 상기 엔드 이펙터 이동 속도에 대한 추정변수,
    Figure pat00130
    은 영상좌표 상의 상기 엔드 이펙터 위치에 대한 추정변수,
    Figure pat00131
    는 상기 엔드 이펙터에 가해지는 외력에 대한 추정변수, ,
    Figure pat00133
    및
    Figure pat00134
    는 시스템 모델의 정확도를 나타내는 화이트 노이즈 레벨(white noise level)인 다관절 매니퓰레이터 제어 시스템.
  20. 제18항에 있어서,
    상기 센서 모델은 하기 식(3)과 같이 정의되고,
    Figure pat00135

    여기에서,
    Figure pat00136
    는 상기 비전 센서 출력 값에 대한 상태변수,
    Figure pat00137
    는 상기 장력 센서 출력 값에 대한 상태변수,
    Figure pat00138
    는 상기 엔드 이펙터 이동 속도에 대한 추정변수,
    Figure pat00139
    은 영상좌표 상의 상기 엔드 이펙터 위치에 대한 추정변수,
    Figure pat00140
    는 상기 엔드 이펙터에 가해지는 외력에 대한 추정변수,
    Figure pat00141
    및
    Figure pat00142
    는 센서 모델의 정확도를 나타내는 화이트 노이즈 레벨(white noise level)인 다관절 매니퓰레이터 제어 시스템.
KR20130119762A 2013-10-08 2013-10-08 다관절 매니퓰레이터 제어 방법 및 제어 시스템 KR20150041346A (ko)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR20130119762A KR20150041346A (ko) 2013-10-08 2013-10-08 다관절 매니퓰레이터 제어 방법 및 제어 시스템

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR20130119762A KR20150041346A (ko) 2013-10-08 2013-10-08 다관절 매니퓰레이터 제어 방법 및 제어 시스템

Publications (1)

Publication Number Publication Date
KR20150041346A true KR20150041346A (ko) 2015-04-16

Family

ID=53034856

Family Applications (1)

Application Number Title Priority Date Filing Date
KR20130119762A KR20150041346A (ko) 2013-10-08 2013-10-08 다관절 매니퓰레이터 제어 방법 및 제어 시스템

Country Status (1)

Country Link
KR (1) KR20150041346A (ko)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109262614A (zh) * 2018-10-12 2019-01-25 清华大学深圳研究生院 一种机器人关节模组运动控制系统及其方法
KR101994588B1 (ko) 2018-06-18 2019-06-28 포항공과대학교 산학협력단 수중 로봇의 유압식 매니플레이터의 위치 추정 장치 및 그 방법

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101994588B1 (ko) 2018-06-18 2019-06-28 포항공과대학교 산학협력단 수중 로봇의 유압식 매니플레이터의 위치 추정 장치 및 그 방법
CN109262614A (zh) * 2018-10-12 2019-01-25 清华大学深圳研究生院 一种机器人关节模组运动控制系统及其方法

Similar Documents

Publication Publication Date Title
EP3684280B1 (en) Systems and methods for controlling a robotic manipulator or associated tool
CN107921623B (zh) 机器人系统
US11173597B2 (en) Systems and methods for controlling a robotic manipulator or associated tool
US9060796B2 (en) Surgical robot system and control method thereof
KR102222124B1 (ko) 수술 지원 장치, 그 제어 방법, 기록 매체 및 수술 지원 시스템
Zemiti et al. Mechatronic design of a new robot for force control in minimally invasive surgery
US9283049B2 (en) Control system configured to compensate for non-ideal actuator-to-joint linkage characteristics in a medical robotic system
KR102188100B1 (ko) 로봇 및 그 제어방법
US9788909B2 (en) Synthetic representation of a surgical instrument
US20160199984A1 (en) Method for the Alignment of a Multiaxial Manipulator with an Input Device
JP2018075121A (ja) 医療用支持アーム装置
US20190365489A1 (en) Medical support arm system and control device
KR20140066544A (ko) 로봇 및 로봇의 마찰 보상 방법
US11160633B2 (en) Medical observation apparatus, driving control method, medical observation system, and support arm apparatus
Ortmaier et al. Robot assisted force feedback surgery
US20200000535A1 (en) Control system, control method, and surgical arm system
KR20150033473A (ko) 로봇 및 그 제어방법
TWI544891B (zh) 用於腹腔鏡手術之俱有直覺式操控的輔助性機器人內視鏡系統及其方法
US20230028689A1 (en) System and method for inter-arm registration
KR20150041346A (ko) 다관절 매니퓰레이터 제어 방법 및 제어 시스템
US9254568B2 (en) Robot control methods
KR20140132113A (ko) 백래시 보상 방법 및 백래시 보상값 산출식 생성 방법
US20230123065A1 (en) Systems and methods of controlling instruments
KR20220095530A (ko) 수술 로봇 장치 및 수술 로봇 장치의 하중 보상 방법

Legal Events

Date Code Title Description
A201 Request for examination
E902 Notification of reason for refusal
E601 Decision to refuse application