KR101986451B1

KR101986451B1 - 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템

Info

Publication number: KR101986451B1
Application number: KR1020180035321A
Authority: KR
Inventors: 조건래; 기건희; 이문직; 김민규; 이계홍
Original assignee: 한국로봇융합연구원
Priority date: 2018-03-27
Filing date: 2018-03-27
Publication date: 2019-06-10

Abstract

본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법은, 수중작업을 보조하기 위한 영상기반의 작업물 위치추정기법을 적용한 것으로서, 초기정보를 작업자로부터 입력 받으므로, 기존에 연구되고 있는 자동위치추정과 같은 연구들에 비교하여 더 높은 추정 결과의 신뢰성을 확보할 수 있으며, 추정된 위치정보를 활용함으로써 기존의 원격조작 보다 간편한 방법으로 로봇 팔을 조작할 수 있으며, 스테레오로 배치된 카메라에서 영상을 기반으로 물체의 위치정보를 추정하는 기법으로 작업자로부터 영상에서 물체의 점 정보에 대한 터치입력을 받아 물체의 6자유도 위치정보를 추정할 수 있다.

Description

수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템{MANIPULATOR CONTROL METHOD FOR WATER ROBOT}

본 발명은 수중로봇의 매니퓰레이터 제어방법에 관한 것으로서, 보다 상세하게는 수중 로봇의 터치스크린 입력을 통한 작업물의 6자유도 위치 추정 방법 및 이를 이용한 원격조종 보조 기법이 적용된 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템에 관한 것이다.

수중환경에서 에너지 채취 및 해저 케이블과 배관, 구조물의 설치 등 다양한 이슈들이 있다. 수중이라는 환경 특성상, 사람이 접근하기 힘들고 작업에 어려움이 많다.

수중작업을 위해 유인잠수정부터, 최근에는 무인잠수정 등과 같은 수중로봇이 투입되어 다양한 작업들을 수행한다. 수중로봇은 유인잠수정에 비해 인간이 투입된다는 위험요소가 없기 때문에 많이 연구되고 있다.

수중로봇은 정찰에 이용되기도 하며, 동시에 수중에서 케이블 등의 물체를 핸들링, 절단, 연결, 설치 등의 작업에 많이 사용된다.

수중로봇의 일련의 작업들은 일반적으로 선상에서 관제시스템을 두고 원격으로 제어한다. 작업자는 로봇에서 송신되는 영상 정보와 센서 정보만을 이용해 작업환경을 파악하여 로봇을 조작한다.

수중환경에서 획득할 수 있는 정보는 육상에 비해 제한적이기 때문에 그만큼 수중작업은 많은 어려움이 따른다. 특히, 수중로봇에 부착된 로봇 팔을 조작하는 것은 작업자의 숙련도와 많은 시간을 요구한다.

수중에서 로봇을 이용한 작업은 원격조작을 통해 이루어지는 것이 보통이다. 즉, 로봇을 수중으로 내려 보낸 후, 선박에 배치되어 있는 운영 룸에서 로봇을 조종하여 작업을 수행한다.

조종자는 카메라 및 센서의 정보를 통해 수중의 작업상황을 파악하고, 수중의 이동로봇 및 로봇 팔의 각 모션을 직접 조작한다. 이 경우, 조종자는 2D카메라 등 제한된 정보를 바탕으로 수중의 상황을 3차원으로 재구성하여 파악해야 하며, 로봇 팔의 조작 역시 조종창치를 통해 간접적으로 수행해야 한다. 따라는 조종자는 고도록 숙달될 필요가 있으며, 작업 시간도 상당히 소요된다.

이를 해결하기 위한 선행 연구로, 카메라 영상처리, 또는 소나 이미지 정보를 활용하여 자동으로 위치를 추정하는 연구가 진행된 바 있다. 이러한 위치 추정 연구는 로봇 팔을 이용한 작업자동화에 이용될 수 있다.

기존 연구 기술은, 작업의 신뢰도를 중시하는 업계 분위기 상 자동화 기법은 크게 호응을 얻지 못하고 있으며, 잘못된 동작으로 인해 작업을 실패할 경우, 그 피해가 크며 수중상황에서 복구 작업을 수행하는 것도 쉽지 않으며, 경우에 따라 거의 복구가 불가능할 수도 있다.

또 다른 연구로는 터치스크린을 이용한 위치 추정 기법이 제안되었다. 그러나 이 기법은 물체의 위치정보만 추정가능하며, 회전정보를 추정하지 못하는 단점이 있다. 즉, 물체가 점이나 공과 같이 회전 정보가 불필요한 경우에만 적용 가능하므로, 적용성이 제한되는 문제점이 있다.

본 발명의 일 실시예는, 수중작업의 효율성 향상 연구에 활용하고자, 수중작업을 보조하기 위한 영상기반의 작업물 위치추정기법을 적용한 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템을 제공하는 것이다.

상기의 본 발명의 일 실시예는, 초기정보를 작업자로부터 입력 받기 때문에, 기존에 연구되고 있는 자동위치추정과 같은 연구들에 비교하여 더 높은 추정 결과의 신뢰성을 확보할 수 있으며, 추정된 위치정보를 활용함으로써 기존의 원격조작 보다 간편한 방법으로 로봇 팔을 조작할 수 있는 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템을 제공하는 것이다.

상기의 본 발명의 일 실시예는, 스테레오로 배치된 카메라에서 영상을 기반으로 물체의 위치정보를 추정하는 기법으로 작업자로부터 영상에서 물체의 점 정보에 대한 터치입력을 받아 물체의 6자유도 위치정보를 추정하는 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템을 제공하는 것이다.

본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법은, 수중 작업을 위해 배치된 로봇 팔과 작업 대상을 촬영하도록 상기 로봇 팔과 함께 수중에 배치된 복수의 카메라에 의해 상기 로봇 팔과 작업 대상을 촬영하여 상기 복수의 카메라에 상응하는 터치스크린에 표시하는 단계; 상기 로봇 팔의 엔드부를 목표 위치로 이동시키기 위해 상기 로봇 팔이 표시된 상기 터치스크린의 입력에 의한 위치추정 원격조종과 직접 제어 가능한 마스터 입력에 의한 완전 원격조종 중 어느 하나를 선택하는 단계; 상기 터치스크린의 입력에 의한 위치추정 원격조종이 선택되면, 상기 로봇 팔의 촬영 영상을 나타내는 상기 터치스크린에 상기 로봇 팔의 엔드부를 목표 위치까지 이동시키기 위한 복수의 지점들을 입력하는 단계; 상기 터치스크린에 입력된 지점들을 입력받는 목표 위치 추정부가 상기 터치스크린으로부터 입력된 지점들을 기초로 상기 작업 대상에 대한 상기 엔드부의 목표 위치를 계산하는 단계; 상기 목표 위치 추정부가 계산한 목표 위치를 기초로 상기 로봇 팔을 상기 목표 위치까지 이동시키기 위한 상기 로봇 팔의 모션 경로를 모션 계획부가 수립하는 단계; 상기 모션 계획부에 의해 수립된 모션 경로를 기초로 역기구부가 역기구학 해석을 사용하여 상기 로봇 팔 관절들 각각의 움직임을 위한 동작 데이터를 생성하는 단계; 메인 컨트롤러가 상기 역기구부에 의한 상기 로봇 팔의 관절부들의 동작 데이터를 상기 로봇 팔의 관절들을 움직이도록 상기 로봇 팔에 마련된 서브 컨트롤러로 전송하는 단계; 및 상기 서브 컨트롤러가 상기 동작 데이터를 전송받고 상기 로봇 팔을 동작시켜 상기 로봇 팔의 엔드부를 목표 위치로 위치시키는 단계를 포함한다.

상기 터치스크린에 복수의 지점들을 입력하는 단계에서는, 상기 복수의 카메라에 의해 촬영된 영상 위에 상기 로봇 팔의 엔드부가 목표하는 목표 위치에 있는 작업 대상의 중심점, 상기 엔드부가 목표 위치까지 접근하는 경로 상의 점, 및 상기 엔드부가 목표 위치까지 접근하는 경로에 직교하는 점을 X, Y, Z 직교좌표계를 기준으로 입력하되, 상기 터치스크린에서 각각의 카메라에 대해 3개씩 점들을 입력할 수 있다.

상기 엔드부의 목표 위치를 계산하는 단계에서는, 상기 촬영된 영상의 전체 좌표계에서 각각의 상기 카메라의 카메라 좌표와 상기 카메라 좌표를 기준으로 상기 작업 대상에 대한 각각의 상기 카메라 초점거리에 따른 초점 좌표를 구하고, 각각의 상기 카메라의 초점 좌표의 원점과 상기 카메라 좌표의 원점을 연결하는 복수의 선분에 상응하는 복수의 좌표 원점 선분 식을 구하며, 상기 복수의 좌표 원점 선분 식을 기초로 상기 복수의 선분 상의 가장 인접한 점을 각각 구하여 상기 목표 위치를 계산할 수 있다.

상기 각각의 카메라에 대해 상기 카메라 좌표와 상기 초점 좌표의 원점을 연결하는 상기 복수의 선분에 수직인 법선 벡터를 구하고, 상기 법선 벡터와 상기 복수의 좌표 원점 선분 식으로부터 상기 법선 벡터와 상기 복수의 선분에 대한 각각의 교점을 구하며, 상기 각각의 교점들 거리의 중간 위치를 상기 목표 위치로 추정할 수 있다.

상기 엔드부의 목표 위치를 계산하는 단계에서는, 상기 작업 대상의 회전 양상을 추정하며, 상기 작업 대상의 회전 양상의 추정은, 각각의 상기 카메라의 상기 초점 좌표의 원점에서 상기 로봇 팔의 엔드부가 목표하는 목표 위치에 있는 작업 대상의 중심점과 상기 엔드부가 목표 위치까지 접근하는 경로 상의 점을 연결하는 다른 복수의 선분과 그 선분의 식을 각각 구하며, 상기 다른 복수의 선분에 수직인 벡터를 구하며, 각각의 상기 카메라에 관한 상기 다른 복수의 선분의 식과 상기 수직인 벡터를 사용하여 각각의 상기 복수의 선분을 포함하는 복수의 평면과 그 평면의 식을 구하며, 상기 복수의 평면에 대해 수직인 벡터를 상기 복수의 평면의 식에 관한 상호 교차 곱을 사용하여 구하며, 상기 복수의 평면에 수직인 벡터와 상기 초점 좌표의 원점에서 상기 로봇 팔의 엔드부가 목표하는 목표 위치에 있는 작업 대상의 중심점과 상기 엔드부가 목표 위치까지 접근하는 경로 상의 점을 사용하여 목표 위치로 접근하는 접근 벡터를 구하며, 상기 목표 위치로 접근하는 접근 벡터에 수직하는 법선 벡터를 구하고, 상기 접근 벡터와 상기 법선 벡터를 사용하여 상기 작업 대상의 회전 양상을 회전 행렬로 표현할 수 있다.

상기 작업 대상에 추정된 목표 위치로 상기 로봇 팔의 엔드부를 움직이는 경우, 상기 엔드부와 작업 대상의 충돌을 방지하기 위해서, 상기 작업 대상으로부터 상기 목표 위치를 일정거리 옵셋하여 설정할 수 있다.

상기 모션 계획부는 5차 폴리노미얼 궤적을 사용한 부드러운 이동 경로를 생성할 수 있다.

상기 역기구부는 상기 모션 계획부가 수립한 직교 좌표 위치 궤적에 자코비안을 사용하여 상기 로봇 팔 조인트의 위치 궤적을 구할 수 있다.

상기 서브 컨트롤러는 로봇 암의 속도 제어기로부터 상기 로봇 팔의 속도를 감지하여 비례 게인을 이용한 위치 루프 제어를 수행할 수 있다.

상기 수중로봇의 매니퓰레이터 제어방법에서는, 상기 직접 제어를 위한 마스터 입력에 의한 완전 원격조종을 선택하는 경우, 상기 모션 계획부는 5차 폴리노미얼 궤적을 사용한 부드러운 이동 경로를 생성하며, 상기 역기구부는 상기 모션 계획부가 수립한 직교 좌표 위치 궤적에 자코비안을 사용하여 상기 로봇 팔 조인트의 위치 궤적을 구하며, 상기 서브 컨트롤러는 로봇 암의 속도 제어기로부터 상기 로봇 팔의 속도를 감지하여 비례 게인을 이용한 위치 루프 제어를 수행할 수 있다.

본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어시스템은, 수중 작업을 위해 배치된 로봇 팔과 작업 대상을 촬영하도록 상기 로봇 팔과 함께 수중에 배치된 복수의 카메라; 상기 복수의 카메라에 상응하는 촬영 영상을 표시하는 터치스크린; 상기 로봇 팔의 엔드부를 목표 위치로 이동시키기 위해 상기 로봇 팔이 표시된 상기 터치스크린의 입력에 의한 위치추정 원격조종과 직접 제어을 위한 마스터 입력에 의한 완전 원격조종 중 어느 하나를 선택하도록 구성된 마스터 컨트롤러; 및 상기 로봇 팔의 관절들을 움직이도록 상기 로봇 팔에 마련된 서브 컨트롤러를 포함하며, 상기 마스터 컨트롤러는, 상기 터치스크린의 입력에 의한 위치추정 원격조종이 선택되어 상기 로봇 팔의 촬영 영상을 나타내는 상기 터치스크린에 상기 로봇 팔의 엔드부를 목표 위치까지 이동시키기 위한 지점들을 입력하는 경우, 상기 터치스크린에 입력된 지점들을 입력받으며 상기 터치스크린으로부터 입력된 지점들을 기초로 상기 작업 대상에 대한 상기 엔드부의 목표 위치를 계산하는 목표 위치 추정부; 상기 목표 위치 추정부가 계산한 목표 위치를 기초로 상기 로봇 팔을 상기 목표 위치까지 이동시키기 위한 상기 로봇 팔의 모션 경로를 수립하는 모션 계획부; 상기 모션 계획부에 의해 수립된 모션 경로를 기초로 상기 로봇 팔 관절들 각각의 움직임을 위한 동작 데이터를 생성하는 역기구부; 및 상기 역기구부에 의한 상기 로봇 팔의 관절부들의 동작 데이터를 상기 메인 컨트롤러로부터 전송받으며, 상기 동작 데이터를 기초로 상기 로봇 팔의 관절들을 움직이도록 상기 로봇 팔에 마련된 서브 컨트롤러를 포함하며, 상기 서브 컨트롤러가 상기 로봇 팔을 동작시켜 상기 로봇 팔의 엔드부를 목표 위치로 위치시킬 수 있다.

본 발명의 일 실시예는, 수중작업의 효율성 향상 연구에 활용하고자, 수중작업을 보조하기 위한 영상기반의 작업물 위치추정기법을 적용한 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템을 제공할 수 있다.

상기의 본 발명의 일 실시예는, 초기정보를 작업자로부터 입력 받기 때문에, 기존에 연구되고 있는 자동위치추정과 같은 연구들에 비교하여 더 높은 추정 결과의 신뢰성을 확보할 수 있으며, 추정된 위치정보를 활용함으로써 기존의 원격조작 보다 간편한 방법으로 로봇 팔을 조작할 수 있는 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템을 제공할 수 있다.

상기의 본 발명의 일 실시예는, 스테레오로 배치된 카메라에서 영상을 기반으로 물체의 위치정보를 추정하는 기법으로 작업자로부터 영상에서 물체의 점 정보에 대한 터치입력을 받아 물체의 6자유도 위치정보를 추정하는 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템을 제공할 수 있다.

상기와 같이 기술된 본 발명의 일 실시예는, 수중로봇 팔의 원격작업에서 현재까지 완전 원격조종(full teleoperation. 작업자가 로봇의 모든 자유도를 직접 원격조종)에 의존하고 있는 것보다, 위치추정기법 및 이를 이용한 원격조종 보조기법을 활용함으로써 작업효율을 크게 향상시킬 수 있는 수중로봇의 매니퓰레이터 제어방법 및 그 제어 시스템을 제공할 수 있다.

도 1은 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 원격 조종을 위한 시스템 구성도이다.
도 2는 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법에서 목표 위치 추정을 위해 터치스크린을 통한 입력을 받는 개념도이다.
도 3은 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법의 보조 원격조종을 위한 제어 구조도이다.
도 4는 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법의 목표 위치 추정의 개념도이다.
도 5는 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법의 목표 회전 양상 추정의 개념도이다.
도 6은 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 원격 조종을 위한 시스템의 시험도이다.
도 7은 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법의 테스트 결과의 박스형 플롯들이다.

이하, 첨부된 도면에 도시된 특정 실시예들에 의해 본 발명의 다양한 실시예들을 설명한다. 실시예들에 차이는 상호 배타적이지 않은 사항으로 이해되어야 하며, 본 발명의 기술 사상 및 범위를 벗어나지 않으면서, 일 실시예에 관련하여 기재되어 있는 특정 형상, 구조 및 특성은 다른 실시예로 구현될 수 있다.

본 발명의 실시예들에 따른 개별 구성요소의 위치 또는 배치는 변경 가능한 것으로 이해되어야 하며, 도면에서 유사한 참조부호는 다양한 측면에 걸쳐 동일하거나 유사한 기능을 가리킬 수 있으며, 길이 및 면적, 두께 등과 그 구체적인 형태는 설명 상의 편의를 위하여 과장되어 표현된 것일 수 있다.

사용되는 용어들은 특별히 정의된 용어를 제외하고는 통상적인 한자, 국어 혹은 영어의 사전적인 의미 혹은 해당 분야에서 사용되는 용어와 부합하는 속성을 가진 것으로 이해되어야 한다. "포함한다, 구성된다, 또는 구비한다"는 다른 구성요소들을 더 가질 수 있음을 의미한다.

본 발명의 일 실시예에서, 로봇 팔의 엔드부는 작업물인 작업 대상에 대한 작업을 수행하기 위한 도구나 집게가 설치된 로봇 팔의 단부이다. 복수의 카메라는 로봇 팔의 배후에서 로봇 팔과 작업 대상을 촬영하도록 설치된 것으로 상정한다. 로봇 팔은 6자유도 이상을 갖는 다관절 로봇을 상정한다.

도 1과 도 2를 참조하면, 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어 시스템은, 수중 작업을 위해 배치된 로봇 팔(100)과 작업 대상(W)을 촬영하도록 로봇 팔(100)과 함께 수중에 배치된 복수의 카메라(121, 122), 복수의 카메라(121, 122)에 상응하는 촬영 영상을 표시하는 터치스크린(130), 로봇 팔(100)의 엔드부(110)를 목표 위치로 이동시키기 위해 로봇 팔(100)이 표시된 터치스크린(130)의 입력에 의한 위치추정 원격조종과 직접 제어를 위한 마스터 입력에 의한 완전 원격조종 중 어느 하나를 선택하게 구성된 마스터 컨트롤러(140), 및 로봇 팔(100)의 관절들을 움직이도록 로봇 팔(100)에 마련된 서브 컨트롤러(180)를 포함한다.

상기의 마스터 컨트롤러(140)는, 작업 대상(W)에 대한 작업 위치를 산출하는 목표 위치 추정부(150)(Target position estimator), 목표 위치에 따른 경로를 수립하는 모션 계획부(155)(Motion planner), 로봇 팔(100) 관절들의 움직임을 계산하는 역기구부(160)(Inverse kinematics), 그리고 로봇 팔(100) 관절의 위치를 제어하기 위한 조인트 포지션 컨트롤러(170)(Joint position controller)를 포함한다.

목표 위치 추정부(150)는 터치스크린(130)의 입력에 의한 위치추정 원격조종이 선택되면 작동한다. 이러한 목표 위치 추정부(150)는 로봇 팔(100)의 촬영 영상을 나타내는 터치스크린(130)에 로봇 팔(100)의 엔드부(110)를 목표 위치까지 이동시키기 위한 지점들을 입력하는 경우, 터치스크린(130)을 통해 복수의 지점들을 입력받으며 터치스크린(130)으로부터 입력된 지점들을 기초로 작업 대상(W)에 대한 엔드부(110)의 목표 위치를 계산하는 것이다.

모션 계획부(155)는 목표 위치 추정부(150)가 계산한 목표 위치를 기초로 로봇 팔(100)을 목표 위치까지 이동시키기 위한 로봇 팔(100)의 모션 경로를 수립하는 것이다.

역기구부(160)는 모션 계획부(155)에 의해 수립된 모션 경로를 기초로 로봇 팔(100) 관절들 각각의 움직임을 위한 동작 데이터를 생성하는 것이다.

본 발명의 일 실시예에 따르면, 메인 컨트롤러의 조인트 포지션 컨트롤러(170)가 역기구부(160)에 의한 로봇 팔(100)의 관절부들의 동작 데이터를 로봇 팔(100)의 관절들을 움직이도록 로봇 팔(100)에 마련된 서브 컨트롤러(180)로 전송한다. 서브 컨트롤러(180)는 동작 데이터를 전송받고 로봇 팔(100)을 동작시켜 로봇 팔(100)의 엔드부(110)를 목표 위치로 위치시킬 수 있다.

도 1과 도 2에 도시된 바와 같이, 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법은, 수중로봇에 스테레오로 장착된 두 대의 카메라(121, 122)의 영상에 대해 터치스크린(130) 통해 작업자가 입력한 정보를 활용하여, 작업물의 6자유도(위치, 회전) 정보를 추정할 수 있는 방법이다.

도 1 내지 도 4에서와 같이, 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법은, 각 카메라(121, 122) 영상에 대해 세 점의 입력을 받고, 이를 두 대 카메라(121, 122) 영상에 대해 반복하여 총 여섯 점의 정보를 입력 받는다. 이 정보를 활용함으로써, 작업물의 위치정보 뿐만 아니라 회전 정보도 포함한 6자유도 위치를 추정할 수 있다. 각각 영상에 대해 입력받는 점의 정보는 다음과 같다.

① 작업물의 중심점,

② 접근방향 위의 점,

③ 법선방향 위의 점,

여기서,

는 카메라(121, 122)의 번호를 의미한다. 각 점은 각각의 카메라(121, 122) 이미지 좌표의 2D위치 정보를 나타낸다.

본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법은, 위에서 제안한 6자유도 위치추정 기법을 활용하여 수중로봇의 원격조종을 보조할 수 있도록 구현된다.

도 5는 원격조종 알고리즘의 구조를 도시한 그림이다.

도 5를 참조하면 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법에 따르면, 'full teleoperation mode(작업자가 모든 로봇자유도를 원격조종)'과 'assisted teleoperation mode(위치추정기법을 활용한 원격조종)'을 선택적으로 스위칭하여 사용할 수 있도록 두 조종 기능이 마스터 컨트롤러(140)에 구현된다.

마스터 컨트롤러(140)에는 full teleoperation mode(작업자가 모든 로봇자유도를 원격조종)'과 'assisted teleoperation mode(위치추정기법을 활용한 원격조종)'을 구현하는 기능이 탑재된다.

'assisted teleoperation mode'는 상기의 위치추정기법을 사용하는 것으로서, 목표 위치 추정부(150)(Target postion estimator)와 추정된 위치로 로봇을 이동시키는데 필요한 기능들 모션 계획부(155), 역기구부(160), 조인트 포지션 컨트롤러(170)(Motion planner, inverse kinematics, joint position controller)로 구현된다.

이하에서는, 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법에 대해서 좀 더 구체적으로 설명한다.

본 발명에서 제안한 위치추정 방법은 다음과 같다.

먼저, 도 2에서 도시한 바와 같이 두 대의 카메라(121, 122) 영상에 대해 각각 세 점씩, 총 여섯 점의 위치정보를 입력 받는다. 이 정보를 활용하여 작업물의 위치 및 회전 정보를 추정하는 방법은 다음과 같다.

1. 작업물의 위치 추정

도 2는 작업물의 위치 추정을 위한 개념도이다.

도 2를 보면 작업물의 위치 추정을 위해 하나의 좌표계를 찾을 수 있다. 월드좌표

와 이미지좌표

사이의 상대적인 위치는 로봇 팔(100)과 카메라(121, 122) 사이의 기하학적 관계에 의해 결정된다.

카메라(121, 122) 초점좌표의 위치

는 각 카메라(121, 122)의

와 초점거리

의 위치에 의해 결정된다. 초점거리는 캘리브레이션 툴을 통해 쉽게 얻을 수 있다. 본 발명의 일 실시예를 구현하기 위해 상정한 카메라(121, 122)는 HD-3000으로서, 초점거리는

,

이다.

본 발명의 일 실시예에서는 좌표계의 사이의 관계를 이용함으로써, 월드좌표계 상의 카메라(121, 122)초점 좌표의 원점인

를 결정할 수 있다. 작업물의 중심위치의 터치 포인트는

로 3차원 카메라(121, 122) 좌표계에 대해 다음과 같이 나타낼 수 있다.

(1)

월드좌표계상의 작업물의 위치는 다음과 표현할 수 있다.

(2)

여기서,

은 월드좌표계상

의 카메라(121, 122)초점 좌표

의 회전행렬이다. 그리고 두 점

와

를 잇는 직선의 공식을 이용하여 직선 위에 존재하는 점들의 좌표를 구할 수 있다.

(3)

여기서,

는 변위벡터,

는 독립변수이다. 카메라(121, 122) 1, 2에 대한 2개의 직선을 구할 수 있다.

두 직선의 가장 가까운 점을 구하는 것으로 작업물의 3차원 좌표를 추정할 수 있다. 먼저, 두 직선의 수직인 법선벡터 구한다. 법선벡터와 각 직선 사이의 교점을 도출한다. 두 교점을 평균함으로써 작업물의 위치를 추정할 수 있다.

두 직선을 수직인 법선벡터

은 다음과 같이 나타낼 수 있다.

(4)

식 (4)의 벡터와 평행한 직선 방정식은 다음과 같이 구해진다.

(5)

여기서,

는 독립변수이며,

는 오프셋벡터이다. (3)식과 (5)식의 교점을 구하는 식은 다음과 같이 나타낼 수 있다.

(6)

(7)

여기서,

,

는 교차점에서

와

를 나타낸다. 식 (7)에서 (6)을 빼면 다음

와 같이 다시 정리할 수 있다.

(8)

여기서,

이다. 식 (8)에서 식 (3)의

를 대입하면, 식 (3)과 식(5)의 직선들 간의 교점을 구할 수 있다. 마지막으로 교점사이의 중간점을 구하여 작업물의 3차원 좌표를 구한다.

(9)

[작업물의 회전자세 추정]

작업물의 자세는 도 1의 터치스크린(130)을 통해 입력받은 점

,

을 이용하여 추정할 수 있다. 회전행렬의 접근벡터는

와

, 회전행렬의 법선벡터는

와

를 이용하여 추정 가능하다. 접근벡터와 법선벡터를 구하는 방식은 동일하다.

도 4는 접근벡터 추정에 대한 모식도를 나타낸다.

먼저, 직선

와

를 구한다. 두 직선

를

이용하여 두 직선을 포함하는 평면

를 구할 수 있다. 접근벡터는 두 평면

,

의 교선으로부터 추정할 수 있다.

식 (3)을 이용하면

를 구할 수 있다.

와

를 지나는 직선은 다음과 같다.

(10)

여기서,

는 독립변수이다.

,

의 수직인 벡터는 다음과 같이 계산할 수 있다.

(11)

계산된

를 이용하여, 직선

,

를 포함하는 평면을 다음과 같이 구할 수 있다.

(12)

여기서,

이다. 식 (12)에서 구해진 두 평면에 수직인 벡터를 교차 곱을 하면 다음과 같이 두 평면을 교차하는 선의 방향을 알 수 있다.

(13)

식 (1)을 정규화하면 접근벡터를 다음과 같이 나타낼 수 있다.

(14)

식 (14)에서, 부호함수는 접근벡터가 반대방향을 나타내는 것을 막기 위해 포함되어 있다.

와

를 이용하여 법선벡터를 구하는 방식은 식 (10)에서 (14)까지의 동일한 방식으로 추정되며, 추정된 법선벡터는

로 나타낸다. 최종적으로 작업물의 자세는 다음의 회전행렬로 표현한다.

(15)

본 발명의 일 실시예에서, 추정된 작업물의 위치로 로봇을 움직일 경우, 로봇 팔(100)과 작업물 사이에 충돌이 일어날 수 있다. 이러한 충돌을 방지하기 위해, 작업물에서 일정 오프셋 거리만큼 띄워서 목표위치를 지정할 수 있다. 오프셋 거리를 고려하면 목표점의 위치는 다음과 같이 선정할 수 있다.

(16)

여기서, ^T L은 오프셋 거리 백터를 나타내며, 만일 도 2의 접근 방향(approach direction)으로 일정거리를 띄울 경우 다음과 같다:

.

상기의 작업물 위치/회전 추정 기법을 수중 로봇 팔(100)의 원격조종에 적용하려면, 도 1과 도 5에 도시한 바와 같이 메인 컨트롤러에 모션 계획부(155, motion planning), 역기구부(160, inverse kinematics), 조인트 포지션 컨트롤러(170, joint position controller) 등의 기능을 추가로 구현함으로써 그 적용이 가능하게 된다.

2. 모션 계획부에 의한 모션 계획(Motion planning) 수립

모션 계획부(155, Motion planning)는 로봇의 현재 위치에서 목표 작업 점까지 연결하는 부드러운 이동경로를 생성하는 역할을 한다. 이동 경로를 생성하는 방법은 여러 가지 기법을 사용할 수 있는데, 예를 들어 5차 폴리노미얼 궤적(5th order ploynomial trajectory)을 적용할 수 있다. 5차 폴리노미얼 궤적은 다음과 같이 주어진다.

(18)

여기서,

는 궤적의 계수 값들을 의미하며,

는 시간을 의미한다. 계수값

는 궤적의 끝단 조건들을 이용하여 결정할 수 있는데, 초기 위치 조건(

,

) 및 목표 위치 조건(

,

),그리고, 이동시간을 적절히 선정하여 결정할 수 있다.

3. 역기구부에 의한 역기구학(Inverse kinematics) 해석

역기구부(160)(Inverse kinematics)는 역기구학 해석을 사용하는 것으로서, 모션 계획부(155)를 통해 생성된 로봇 팔(100)의 카테시안 위치 궤적으로부터 로봇 팔(100) 조인트의 위치궤적을 구하는 과정을 수행한다.

이러한 역기구부(160)의 역기구학 해석 수행 과정 역시 다양한 역기구학 알고리즘을 적용할 수 있다. 예를 들어서 감쇠최소자승법[Damped Least Squares 기법]을 활용하면 다음과 같다.

감쇠최소자승법[減衰最小自乘法, damped least squares method]은, 비선형 목적함수(objective function)의 최적화(optimization) 방법 중의 하나이다. 최소자승법은 오차의 제곱합을 최소화하는 추정량을 구하는 방법이다.

먼저, 로봇 팔(100)의 카테시안 속도와 조인트 속도의 관계는 다음과 같이 자코비안 행렬을 이용하여 포현된다.

, (21)

Damped least squares 기법을 이용하여 로봇의 기구학을 구하면 다음과 같이 표현된다.

, (22)

여기서

는 자코비안의 슈도 역행렬로 다음과 같다.

. (23)

또한,

는 자코비안의 영공간(Null space)를 활용하여 추가적인 최적화를 시키기 위한 조건으로 다음과 같이 선정된다.

. (24)

(23)에서 자승 에러를 최소화하기 위한 최적 솔루션은 다음과 같다.[The optimal solution minimizing square errors in (23) is given as follows]:

, (25)

또한,

는 자코비안 행렬이 singular 할 경우, 역행렬 연산의 오류를 막기 위한 damping parameter로 다음과 같이 선정할 수 있다.

(28)

여기서

은 자코비안 행렬의 최소 특이 값을 의미한다.

4. 조인트 포지션 컨트롤러(Joint position controller)

로봇 팔(100)의 조인트 속도제어기를 내장하고 있을 경우, 그 외부에 다음과 같이 위치에 대한 비례게인을 이용하여 위치 제어 루프를 추가할 수 있다.

. (34)

여기서,

는 변환된 속도 지령값을 의미하며,

는 위치피드백 게인 값을 의미한다. 로봇 팔(100)에 내장된 속도제어기는 많은 경우 PI제어기로 구성되며 다음과 같이 표현될 수 있다.

. (35)

여기서,

와

는 각각 속도 제어기의 비례 게인과 적분 게인을 나타낸다. 위 두식으로부터 속도제어기가 내장된 로봇 팔(100)에 위치제어 루프를 추가할 경우 다음과 같이 PID 위치제어기로 동작하는 것을 확인할 수 있다.

,(36)

여기서,

.

이하에서는, 본 발명의 일 실시예에 따른 수중로봇의 매니퓰레이터 제어방법의 지상 시험 결과에 대해 설명한다.

본 발명은 수중로봇 팔(100)을 이용한 수중작업에서 원격조종 보조기술로 활용될 수 있다. 이를 통해, 작업자의 편의성을 확보하고 작업효율을 향상시킴으로써 작업시간 단축 및 비용절감의 효과를 얻을 수 있다. 이러한 효과를 검증하기 위해 수중로봇 팔(100)의 실험을 수행하였으며 그 내용은 다음과 같다.

도 6은 테스트를 위한 셋업을 도시한 것이다. 로봇 팔을 이용하여 그립핑 툴(gripping tool)을 잡아서 바닥의 케이블 위에 놓는 작업이다.

작업의 내용은 다음 표와 같이 세분화 될 수 있다. 즉, 그립핑 툴(Gripping tool)을 잡기 위해 로봇 팔을 이동시키는 접근(approaching) 과정, 그립핑 툴(Gripping tool)의 핸들을 로봇 팔의 엔드부로 붙잡는 시징(Seizing)과정, 그리고, 그립핑 툴(Gripping tool)을 케이블 위로 옮기고 내려놓는 움직임(Moving) 과정으로 구분할 수 있다. 이 때, 접근(Approaching)과 움직임(Moving) 과정 중 로봇 팔(100)을 적절한 위치로 옮기는 과정에 본 발명을 적용하였다.

표 1, 표 2와 도 7은 실험 결과를 나타낸 것이다. 실험결과로부터 특히 접근(Approaching) 과정의 작업시간을 47% 수준으로 크게 감소시킬 수 있으며, 전체 작업시간도 28% 수준으로 감소시킬 수 있는 것으로 확인했다.

테이블 1 작업의 세부 기술(Table I. Detailed description of the task)

Step of Task	Description	Full teleoperation	Assisted teleoperation
#0.Initial posture	Robot looks forward (samepostureinFig.6) Gripper is closed	-	-
#1.Approaching	Moving robot to the tool	Manual*	Assisted
#2.Seizing	Opening the gripper	Manual	Manual
	Delicate positioning of robot to seize the tool	Manual	Manual
	Closing the gripper (seizingthetool)	Manual	Manual
#3.Moving	Moving the the tool to the cable	Manual	Assisted
	Delicate positioning of gripping tool on the cable	Manual	Manual
	Open the gripper (layingthegrippingtool)	manual	Manual

수동모드는 완전 원격 조정과 같다('Manual' mode is same with full tele-operation).: 모든 로봇의 자유도는 원격 조종된다(every DOF of robot is teleoperated).

테이블 2 실험결과(Table II. Exprimental results: Averaged Time and P-value)

		Approaching	Seizing	Moving	Total
Avg. Time (sec)	①Full teleoperation	55.751	25.335	66.604	147.69
Avg. Time (sec)	②Assisted teleoperation	29.533	20.456	56.07	106.059
(①-②)/①*100		47.0%	19.3%	15.8%	28.2%
p-value		0.0013	0.0755	0.0462	0.0024

이상과 같이 본 발명의 일 실시예에 대하여 설명하였으나, 이를 기초로 해당 기술 분야에서 통상의 지식을 가진 자라면 청구범위에 기재된 본 발명의 사상으로부터 벗어나지 않는 범위 내에서, 구성 요소의 부가, 변경, 삭제 또는 추가 등에 의해 본 발명을 다양하게 수정 및 변경시킬 수 있을 것이며, 이 또한 본 발명의 권리범위 내에 포함된다 할 것이다.

W: 작업 대상
100: 로봇 팔
110: 엔드부
121, 122: 복수의 카메라
130: 터치스크린
140: 마스터 컨트롤러
150: 목표 위치 추정부
155: 모션 계획부
160: 역기구부
170: 조인트 포지션 컨트롤러
180: 서브 컨트롤러

Claims

수중 작업을 위해 배치된 로봇 팔과 작업 대상을 촬영하도록 상기 로봇 팔과 함께 수중에 배치된 복수의 카메라에 의해 상기 로봇 팔과 작업 대상을 촬영하여 상기 복수의 카메라에 상응하는 터치스크린에 표시하는 단계;
상기 로봇 팔의 엔드부를 목표 위치로 이동시키기 위해 상기 로봇 팔이 표시된 상기 터치스크린의 입력에 의한 위치추정 원격조종과 직접 제어가 가능한 마스터 입력에 의한 완전 원격조종 중 어느 하나를 선택하는 단계;
상기 터치스크린의 입력에 의한 위치추정 원격조종이 선택되면, 상기 터치스크린에 상기 로봇 팔의 엔드부를 목표 위치까지 이동시키기 위한 복수의 지점들을 입력하는 단계;
상기 터치스크린에 입력된 지점들을 입력받는 목표 위치 추정부가 상기 터치스크린으로부터 입력된 복수의 지점들을 기초로 상기 작업 대상에 대한 상기 엔드부의 목표 위치를 계산하는 단계;
상기 목표 위치 추정부가 계산한 목표 위치를 기초로 상기 로봇 팔을 상기 목표 위치까지 이동시키기 위한 상기 로봇 팔의 모션 경로를 모션 계획부가 수립하는 단계;
상기 모션 계획부에 의해 수립된 모션 경로를 기초로 역기구부가 역기구학 해석을 사용하여 상기 로봇 팔 관절들 각각의 움직임을 위한 동작 데이터를 생성하는 단계;
메인 컨트롤러가 상기 역기구부에 의한 상기 로봇 팔의 관절부들의 동작 데이터를 상기 로봇 팔의 관절들을 움직이도록 상기 로봇 팔에 마련된 서브 컨트롤러로 전송하는 단계; 및
상기 서브 컨트롤러가 상기 동작 데이터를 전송받고 상기 로봇 팔을 동작시켜 상기 로봇 팔의 엔드부를 목표 위치로 위치시키는 단계를 포함하는 수중로봇의 매니퓰레이터 제어방법.
제1 항에 있어서,
상기 터치스크린에 복수의 지점들을 입력하는 단계에서는, 상기 복수의 카메라에 의해 촬영된 영상 위에 상기 로봇 팔의 엔드부가 목표하는 목표 위치에 있는 작업 대상의 중심점, 상기 엔드부가 목표 위치까지 접근하기 위한 경로 상의 점, 및 상기 엔드부가 목표 위치까지 접근하기 위한 경로에 직교하는 점을 X, Y, Z 직교좌표계를 기준으로 입력하되, 상기 터치스크린에서 각각의 카메라에 대해 점들을 입력하는 수중로봇의 매니퓰레이터 제어방법.
제2 항에 있어서,
상기 엔드부의 목표 위치를 계산하는 단계에서는, 상기 촬영된 영상의 전체 좌표계에서 각각의 상기 카메라의 카메라 좌표와 상기 카메라 좌표를 기준으로 상기 작업 대상에 대한 각각의 상기 카메라 초점거리에 따른 초점 좌표를 구하고, 각각의 상기 카메라의 초점 좌표의 원점과 상기 카메라 좌표의 원점을 연결하는 복수의 선분에 상응하는 복수의 좌표 원점 선분 식을 구하며, 상기 복수의 좌표 원점 선분 식을 기초로 상기 복수의 선분 상의 가장 인접한 점을 각각 구하여 상기 목표 위치를 계산하는 수중로봇의 매니퓰레이터 제어방법.
제3 항에 있어서,
상기 각각의 카메라에 대해 상기 카메라 좌표와 상기 초점 좌표의 원점을 연결하는 상기 복수의 선분에 수직인 법선 벡터를 구하고, 상기 법선 벡터와 상기 복수의 좌표 원점 선분 식으로부터 상기 법선 벡터와 상기 복수의 선분에 대한 각각의 교점을 구하며, 상기 각각의 교점들 거리의 중간 위치를 상기 목표 위치로 추정하는 수중로봇의 매니퓰레이터 제어방법.
제3 항에 있어서,
상기 엔드부의 목표 위치를 계산하는 단계에서는, 상기 작업 대상의 회전 양상을 추정하되,
상기 작업 대상의 회전 양상의 추정은,
각각의 상기 카메라의 상기 초점 좌표의 원점에서 상기 로봇 팔의 엔드부가 목표하는 목표 위치에 있는 작업 대상의 중심점과 상기 엔드부가 목표 위치까지 접근하는 경로 상의 점을 연결하는 다른 복수의 선분과 그 선분의 식을 각각 구하며, 상기 다른 복수의 선분에 수직인 벡터를 구하며, 각각의 상기 카메라에 관한 상기 다른 복수의 선분의 식과 상기 수직인 벡터를 사용하여 각각의 상기 복수의 선분을 포함하는 복수의 평면과 그 평면의 식을 구하며, 상기 복수의 평면에 대해 수직인 벡터를 상기 복수의 평면의 식에 관한 상호 교차 곱을 사용하여 구하며,
상기 복수의 평면에 수직인 벡터와 상기 초점 좌표의 원점에서 상기 로봇 팔의 엔드부가 목표하는 목표 위치에 있는 작업 대상의 중심점과 상기 엔드부가 목표 위치까지 접근하는 경로 상의 점을 사용하여 목표 위치로 접근하는 접근 벡터를 구하며, 상기 목표 위치로 접근하는 접근 벡터에 수직하는 법선 벡터를 구하고, 상기 접근 벡터와 상기 법선 벡터를 사용하여 상기 작업 대상의 회전 양상을 회전 행렬로 표현하는 수중로봇의 매니퓰레이터 제어방법.
제5 항에 있어서,
상기 작업 대상에 추정된 목표 위치로 상기 로봇 팔의 엔드부를 움직이는 경우, 상기 엔드부와 작업 대상의 충돌을 방지하기 위해서, 상기 작업 대상으로부터 상기 목표 위치를 일정거리 옵셋하여 설정하는 수중로봇의 매니퓰레이터 제어방법.
제1 항에 있어서,
상기 모션 계획부는 5차 폴리노미얼 궤적을 사용한 부드러운 이동 경로를 생성하는 수중로봇의 매니퓰레이터 제어방법.
제7 항에 있어서,
상기 역기구부는 상기 모션 계획부가 수립한 직교 좌표 위치 궤적에 자코비안을 사용하여 상기 로봇 팔 조인트의 위치 궤적을 구하는 수중로봇의 매니퓰레이터 제어방법.
제8 항에 있어서,
상기 서브 컨트롤러는 로봇 암의 속도 제어기로부터 상기 로봇 팔의 속도를 감지하여 비례 게인을 이용한 위치 루프 제어를 수행하는 수중로봇의 매니퓰레이터 제어방법.
제1 항에 있어서,
상기 직접 제어를 위한 마스터 입력에 의한 완전 원격조종을 선택하는 경우,
상기 모션 계획부는 5차 폴리노미얼 궤적을 사용한 부드러운 이동 경로를 생성하며, 상기 역기구부는 상기 모션 계획부가 수립한 직교 좌표 위치 궤적에 자코비안을 사용하여 상기 로봇 팔 조인트의 위치 궤적을 구하며, 상기 서브 컨트롤러는 로봇 암의 속도 제어기로부터 상기 로봇 팔의 속도를 감지하여 비례 게인을 이용한 위치 루프 제어를 수행하는 수중로봇의 매니퓰레이터 제어방법.
제1 항 내지 제10 항 중 어느 한 항에 따른 수중로봇의 매니퓰레이터 제어방법이 적용된 제어 시스템으로서,
수중 작업을 위해 배치된 로봇 팔과 작업 대상을 촬영하도록 상기 로봇 팔과 함께 수중에 배치된 복수의 카메라;
상기 복수의 카메라에 상응하는 촬영 영상을 표시하는 터치스크린;
상기 로봇 팔의 엔드부를 목표 위치로 이동시키기 위해 상기 로봇 팔이 표시된 상기 터치스크린의 입력에 의한 위치추정 원격조종과 직접 제어을 위한 마스터 입력에 의한 완전 원격조종 중 어느 하나를 선택하도록 구성된 마스터 컨트롤러; 및
상기 로봇 팔의 관절들을 움직이도록 상기 로봇 팔에 마련된 서브 컨트롤러를 포함하며,
상기 마스터 컨트롤러는,
상기 터치스크린의 입력에 의한 위치추정 원격조종이 선택되어 상기 로봇 팔의 촬영 영상을 나타내는 상기 터치스크린에 상기 로봇 팔의 엔드부를 목표 위치까지 이동시키기 위한 지점들을 입력하는 경우, 상기 터치스크린에 입력된 지점들을 입력받으며 상기 터치스크린으로부터 입력된 지점들을 기초로 상기 작업 대상에 대한 상기 엔드부의 목표 위치를 계산하는 목표 위치 추정부;
상기 목표 위치 추정부가 계산한 목표 위치를 기초로 상기 로봇 팔을 상기 목표 위치까지 이동시키기 위한 상기 로봇 팔의 모션 경로를 수립하는 모션 계획부;
상기 모션 계획부에 의해 수립된 모션 경로를 기초로 상기 로봇 팔 관절들 각각의 움직임을 위한 동작 데이터를 생성하는 역기구부; 및
상기 역기구부에 의한 상기 로봇 팔의 관절부들의 동작 데이터를 상기 메인 컨트롤러로부터 전송받으며, 상기 동작 데이터를 기초로 상기 로봇 팔의 관절들을 움직이도록 상기 로봇 팔에 마련된 서브 컨트롤러를 포함하며,
상기 서브 컨트롤러가 상기 로봇 팔을 동작시켜 상기 로봇 팔의 엔드부를 목표 위치로 위치시키는 수중로봇의 매니퓰레이터 제어시스템.