KR101667031B1

KR101667031B1 - 로봇의 경로 계획 장치 및 그 방법

Info

Publication number: KR101667031B1
Application number: KR1020090105087A
Authority: KR
Inventors: 김명희; 임산; 노경식; 임복만
Original assignee: 삼성전자 주식회사
Priority date: 2009-11-02
Filing date: 2009-11-02
Publication date: 2016-10-17
Anticipated expiration: 2029-11-02
Also published as: US8483874B2; KR20110048330A; US20110106306A1

Abstract

제약 조건(Constraint)을 만족하면서 모션 경로를 매끄럽게(smoothing) 하는 로봇의 경로 계획 장치 및 그 방법을 개시한다. 로봇 머니퓰레이터가 작업을 수행하는 형상 공간에서 초기 시작점에서부터 트리를 확장하여 최종 목표점에 도달하는 RRT 경로를 기구학적 제약 조건을 만족하면서 매끄럽게(smoothing) 해줌으로써 안정적인 모션을 생성하며, 이를 통해 경로 계획의 성능을 개선하여 빠른 시간 내에 기구학적 제약 조건을 만족시키는 최적의 경로를 찾을 수 있게 된다.

Description

로봇의 경로 계획 장치 및 그 방법{PATH PLANNING APPARATUS OF ROBOT AND METHOD THEREOF}

제약 조건(Constraint)을 만족하면서 모션 경로를 매끄럽게(smoothing) 하는 로봇의 경로 계획 장치 및 그 방법에 관한 것이다.

일반적으로, 전기적 또는 자기적인 작용을 이용하여 인간의 동작과 닮은 운동을 행하는 기계장치를 로봇이라고 한다. 초기의 로봇은 생산 현장에서의 작업 자동화ㆍ무인화 등을 목적으로 한 머니퓰레이터나 반송 로봇 등의 산업용 로봇으로 인간을 대신하여 위험한 작업이나 단순한 반복 작업, 큰 힘을 필요로 하는 작업을 수행하였으나, 최근에는 인간과 유사한 관절체계를 가지고 인간의 작업 및 생활공간에서 인간과 공존하며 다양한 서비스를 제공하는 인간형 로봇(humanoid robot)의 연구 개발이 활발하게 진행되고 있다.

이러한 인간형 로봇은 전기적ㆍ기계적 메카니즘에 의해서 팔이나 손의 동작에 가깝게 운동할 수 있도록 만들어진 머니퓰레이터(manipulator)를 이용하여 작업을 수행한다. 현재 사용되고 있는 대부분의 머니퓰레이터는 여러 개의 링크(link)들이 서로 연결되어 구성된다. 각 링크들의 연결 부위를 관절(joint)이라 하는데 머니퓰레이터는 이러한 링크와 관절들 사이의 기하학적인 관계에 따라 운동 특성이 결정된다. 이 기하학적인 관계를 수학적으로 표현한 것이 기구학(Kinematics)이며 대부분의 머니퓰레이터는 이러한 기구학적 특성(kinematics characteristic)을 가지고 작업을 수행하기 위한 방향(목표점)으로 로봇 선단(end-effector;이하 엔드 이펙터라 한다)을 이동시킨다.

머니퓰레이터가 주어진 작업(예를 들어, 물체를 잡는 작업)을 수행하기 위해서는 머니퓰레이터가 작업을 수행하기 전의 초기 위치(시작점)에서부터 작업을 수행할 수 있는 즉, 물체를 잡을 수 있는 최종 위치(목표점)까지 머니퓰레이터의 이동 경로를 생성하는 것이 중요하다. 머니퓰레이터가 작업 영역 내의 장애물과 충돌하지 않는 충돌 회피 등의 제약 조건을 만족하면서 시작점과 목표점을 연결하는 경로를 계획하는 경로 계획(Sampling Based Path Planning) 방법 중 하나로 RRT(Rapidly-exploring Random Tree) 알고리즘이 있다.

RRT 알고리즘은 머니퓰레이터 또는 휴머노이드가 작업을 수행하는 형상 공간(Configuration Space;C-Space)에서 랜덤하게 샘플링한 형상을 이용한다. 그 중 한 실시예는 초기 시작점에서부터 가장 가까운 노드를 선택하는 과정을 반복하여 트리(tree)를 확장해 나가면서 최종 목표점까지 이동 경로를 찾는 방법으로, 목표점에서 엔드 이펙터까지의 거리와 방향 벡터의 함수로 이루어진 목표 함수(Goal Function)를 이용하여 트리 중에서 목표 점수(Goal Score)가 가장 작은 노드를 선택하여 트리를 확장해 나간다.

그러나, 종래의 RRT 알고리즘은 가장 가까운 노드를 선택하여 최종 목표점에 도달하는 트리를 확장해 나갈 때 장애물 회피 등의 제약 조건과 주어진 Goal Score 조건을 만족하면 트리를 확장하는 것이기 때문에 트리가 수직에 가까운 형태로 옆으로 뻗치는 경우가 많아 불필요하게 우회하거나 또는 지그재그한 형태의 경로를 생성할 수 있다. 이에 따라 머니퓰레이터 또는 휴머노이드의 이동 거리가 길어지고 머니퓰레이터 또는 휴머노이드의 모션이 급하게 꺾여지는 현상이 발생할 수 있다.

RRT 알고리즘에서 생성된 경로를 제약 조건(Constraint)을 만족하면서 매끄럽게(smoothing) 하는 로봇의 경로 계획 장치 및 그 방법을 제시하고자 한다.

본 발명의 일 실시예에 의한 로봇의 경로 계획 방법은, 로봇 머니퓰레이터의 이동 경로를 생성하기 위해 시작점, 목표점, 제약 조건을 가지는 형상 공간을 형성하고; 형상 공간상에서의 제약 조건을 만족하면서 시작점과 목표점을 연결하는 RRT 경로를 생성하고; RRT 경로에서 임의의 두 점을 선택하여 연결하는 중간 경로를 생성하고; 중간 경로를 프로젝션시켜 제약 조건을 만족하는지 판단하고; 제약 조건을 만족하면 임의의 두 점을 RRT 경로에 저장하여 RRT 경로를 매끄럽게 하는 것을 특징으로 한다.

시작점은 로봇 머니퓰레이터가 작업을 수행하기 전 초기 위치에서의 형상을 형상 공간상에 형성한 노드인 것을 특징으로 한다.

목표점은 로봇 머니퓰레이터가 작업을 수행하기 위한 목표 위치에서의 형상을 형상 공간상에 형성한 노드인 것을 특징으로 한다.

중간 경로를 프로젝션시키는 것은, 로봇 머니퓰레이터의 작업 공간상에서의 제약 조건을 얻고; 작업 공간상에서의 제약 조건과 형상 공간상에서의 제약 조건을 비교하여 작업 공간상에서의 에러를 계산하고; 작업 공간상에서의 에러를 보정하기 위해 중간 경로의 각 노드를 프로젝션시키는 것을 특징으로 한다.

중간 경로의 각 노드는 중간 경로를 일정 간격으로 나누어 얻은 복수의 노드 인 것을 특징으로 한다.

제약 조건을 만족하는지 판단하는 것은, 중간 경로의 각 노드가 제약 조건을 만족하는지 판단하는 것을 특징으로 한다.

그리고, 본 발명의 일 실시예에 의한 로봇의 경로 계획 방법은, 로봇 머니퓰레이터의 이동 경로를 생성하기 위해 시작점, 목표점, 제약 조건을 가지는 형상 공간을 형성하고; 형상 공간상에서의 제약 조건을 만족하면서 시작점과 목표점을 연결하는 RRT 경로를 생성하고; RRT 경로 전체를 로우 패스 필터링하고; 로우 패스 필터링한 경로를 프로젝션시켜 제약 조건을 만족하는지 판단하고; 제약 조건을 만족하면 프로젝션된 경로를 RRT 경로에 저장하여 RRT 경로를 매끄럽게 하는 것을 특징으로 한다.

로우 패스 필터링한 경로를 프로젝션시키는 것은, 로봇 머니퓰레이터의 작업 공간상에서의 제약 조건을 얻고; 작업 공간상에서의 제약 조건과 형상 공간상에서의 제약 조건을 비교하여 상기 작업 공간상에서의 에러를 계산하고; 작업 공간상에서의 에러를 보정하기 위해 로우 패스 필터링한 경로의 각 노드를 프로젝션시키는 것을 특징으로 한다.

로우 패스 필터링한 경로의 각 노드는 로우 패스 필터링한 경로에 있는 모든 노드인 것을 특징으로 한다.

또한, 본 발명의 일 실시예에 의한 로봇의 경로 계획 장치는, 로봇 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하는 인식부; 로봇 머니퓰레이터의 이동 경로를 생성하기 위한 형상 공간을 형성하고, 형상 공간상에서 제약 조건을 만족하면서 시작점과 목표점을 연결하는 RRT 경로를 생성하고, 로봇 머니퓰레이터의 작업 공간상에 RRT 경로를 프로젝션하여 RRT 경로를 매끄럽게 하는 경로 계획 생성부를 포함한다.

경로 계획 생성부는 RRT 경로에서 임의의 두 점을 선택하여 연결하는 중간 경로를 생성하고, 중간 경로를 일정 간격으로 나누어 얻은 각 노드를 제약 조건을 고려하여 프로젝션시키고, 중간 경로의 각 노드가 제약 조건을 만족하면 임의의 두 점을 RRT 경로에 저장하여 RRT 경로를 매끄럽게 하는 것을 특징으로 한다.

경로 계획 생성부는 RRT 경로 전체를 로우 패스 필터링하고, 로우 패스 필터링한 경로를 제약 조건을 고려하여 프로젝션시키고, 프로젝션된 경로가 제약 조건을 만족하면 프로젝션된 경로를 RRT 경로에 저장하여 RRT 경로를 매끄럽게 하는 것을 특징으로 한다.

개시된 로봇의 경로 계획 장치 및 그 방법에 의하면, 로봇 머니퓰레이터가 작업을 수행하는 형상 공간에서 초기 시작점에서부터 트리를 확장하여 최종 목표점에 도달하는 RRT 경로를 기구학적 제약 조건을 만족하면서 매끄럽게(smoothing) 해줌으로써 안정적인 모션을 생성하며, 이를 통해 경로 계획의 성능을 개선하여 빠른 시간 내에 기구학적 제약 조건을 만족시키는 최적의 경로를 찾을 수 있게 된다.

이하, 본 발명의 일 실시예를 첨부된 도면을 참조하여 상세히 설명한다.

도 1은 본 발명의 일 실시예에 의한 로봇의 일례를 나타낸 외관도이다.

도 1에서, 본 발명의 일 실시예에 의한 로봇(100)은 인간과 마찬가지로 두 개의 다리(110R, 110L)에 의해 직립 이동하는 이족 보행 로봇으로, 몸통(120)과, 몸통(120)의 상부에 두 개의 팔(130R, 130L)과 머리(140)를 구비하며, 두 개의 다리(110R, 110L)와 팔(130R, 130L) 선단에는 각각 발(111R, 111L)과 핸드(131R, 131L)를 구비한다.

참조부호에서 R과 L은 로봇(100)의 오른쪽(Right)과 왼쪽(Left)을 나타낸다.

도 2는 도 1에 나타낸 로봇의 주요 관절 구조를 나타낸 도면이다.

도 2에서, 두 개의 팔(130R, 130L)은 로봇(100)의 어깨, 팔꿈치, 손목에 해당하는 부분이 회전할 수 있도록 어깨 관절(132R, 132L), 팔꿈치 관절(133R, 133L), 손목 관절(134R, 134L)을 각각 구비하고, 어깨 관절(132R, 132L)은 몸통(120) 상부의 양 쪽 끝에 위치한다.

각 팔(130R, 130L)의 어깨 관절(132R, 132L)은 x축(roll axis)과 y축(pitch axis), z축(yaw axis)로 움직임이 가능하고, 팔꿈치 관절(133R, 133L)은 y축(pitch axis)로 움직임이 가능하며, 손목 관절(134R, 134L)은 x축(roll axis)과 y축(pitch axis), z축(yaw axis)로 움직임이 가능하다.

또한, 두 개의 팔(130R, 130L)에는 어깨 관절(132R, 132L)과 팔꿈치 관절(133R, 133L)을 연결하는 상부 링크(135R, 135L)와, 팔꿈치 관절(133R, 133L)과 손목 관절(134R, 134L)을 연결하는 하부 링크(136R, 136L)를 각각 포함하여 각 관절((132R, 132L), (133R, 133L), (134R, 134L))의 가동각 범위에 따라 일정 수준의 자유도를 가지고 이동이 가능하도록 한다.

그리고, 두 개의 다리(110R, 110L)와 연결되는 몸통(120)에는 로봇(100)의 허리에 해당하는 부분이 회전할 수 있도록 허리 관절(121)을 구비하며, 몸통(120)에 연결되는 머리(140)에는 로봇(100)의 목에 해당하는 부분이 회전할 수 있도록 목 관절(141)을 구비한다.

본 발명의 일 실시예에서 두 개의 팔(130R, 130L)은 모션이 가능한 작업을 수행하는 머니퓰레이터(130; manipulator)로, 머니퓰레이터(130)의 선단에 마련되는 두 개의 핸드(131R, 131L)가 작업 대상 물체를 잡는 엔드 이펙터(131; end effector)에 해당한다. 이를 도 3에 간략하게 도식화하였다.

도 3은 본 발명의 일 실시예에 의한 로봇의 기구학적 여유구동 머니퓰레이터의 형상을 간략하게 도식화한 구성도이다.

도 3에서, 머니퓰레이터(130)는 전기적ㆍ기계적 메카니즘에 의해서 인간의 팔이나 손의 동작에 가깝게 운동할 수 있도록 만들어진 것으로, 현재 사용되고 있는 대부분의 머니퓰레이터(130)는 여러 개의 링크(135, 136; 구체적으로, 상부 링크 또는 하부 링크)가 여러 개의 관절(132, 133, 134; 구체적으로, 어깨 관절, 팔꿈치 관절 또는 손목 관절)을 통해 서로 연결되어 구성된다. 머니퓰레이터(130)는 이러한 링크들(135, 136)과 관절들(joint; 132, 133, 134) 사이의 기하학적인 관계에 따라 운동 특성이 결정된다. 이 기하학적인 관계를 수학적으로 표현한 것이 기구학(Kinematics)이며 대부분의 머니퓰레이터(130)는 이러한 기구학적 특성(kinematics characteristic)을 가지고 작업을 수행하기 위한 방향으로 엔드 이펙터(131)를 이동시킨다. 본 발명의 일 실시예에 의한 머니퓰레이터(130)는 위치와 방향 조절이 가능한 링크들(135, 136)을 이용하여 작업 대상 물체를 잡기 위한 목표점으로 엔드 이펙터(131)를 이동시킨다.

도 3에서 알 수 있듯이, 동일한 물체를 잡기 위하여 목표점으로 이동하는 머니퓰레이터(130)의 형상은 (a) 또는 (b)와 같이 여러 가지 형상으로 변화될 수 있다.

도 4는 도 3의 머니퓰레이터가 물체를 잡는 모습을 모델링한 도면이다.

도 4에서, 머니퓰레이터(130)가 작업 대상 물체(A)를 잡는다고 가정했을 때, 만족해야 하는 Goal Function은 아래의 [식 1]과 같다.

[식 1]

Goal Function = II G - H II + α * I (G-H) ㅇ x_H- 1 I

여기서, G는 머니퓰레이터(130)가 작업 대상 물체(A)를 잡기 위한 목표점이고, H는 엔드 이펙터(131)의 중심이며, II G - H II는 엔드 이펙터(131)에서 목표점(G)까지의 거리이고, I (G-H) ㅇ x_H- 1 I 는 방향 벡터이며, α는 상수이다.

도 5는 본 발명의 일 실시예에 의한 로봇에서 머니퓰레이터의 이동 경로를 계획하기 위한 제어 블록도로서, 사용자 인터페이스부(200), 경로 계획 생성부(210), 인식부(220), 로봇 제어부(230) 및 구동부(240)를 포함하여 구성된다.

사용자 인터페이스부(200)는 로봇(100) 특히, 머니퓰레이터(130)에서 수행하기 위한 작업 명령(예를 들어, 테이블 위에 놓여진 물체를 잡기 위한 파지 명령)을 사용자가 스위치 조작이나 음성 등을 통해 입력한다.

경로 계획 생성부(210)는 사용자 인터페이스부(200)를 통해 입력되는 작업 명령에 따라 제약 조건을 만족하면서 머니퓰레이터(130)의 이동을 제어하기 위한 경로 계획을 생성하여 이를 로봇 제어부(230)에 전달한다. 경로 계획 생성부(210)에서 경로 계획을 생성하는 방법은 크게 공간 형성 단계와, 그래프 탐색 단계와, 경로 생성 단계를 포함한다.

공간 형성 단계는 장애물에 대한 충돌 회피, 관절들(joint; 132, 133, 134)의 가동각 범위(조인트 리미트) 등의 제약 조건을 만족하면서 경로를 매끄럽게(smoothing) 생성하기 위한 형상 공간(Configuration Space; 이하, C-Space라 한다)을 찾아내는 과정으로, 여기에서 형상이란 움직이는 머니퓰레이터(130)의 위치 및 방향을 표현할 수 있는 변수의 집합을 말하며, 형상들이 점유할 수 있는 모든 공간을 형상 공간(C-Space)이라 한다.

그래프 탐색 단계는 최적 경로를 생성하기 위한 탐색 가능 경로를 나타내는 연결 망(connected network)을 생성하는 과정으로, 움직이는 머니퓰레이터(130)에 대해서 트리와 같은 셀 분해(cell decompositon) 방식으로 형성된 형상 공간(C-Space)을 바탕으로 랜덤하게 형상을 추출하여 장애물 공간과 충돌을 일으키는 형상은 제외시키는 방법으로 노드를 생성하고, 생성된 노드들 사이를 연결하는 탐색 트리를 생성하여 경로 생성을 위한 탐색 그래프를 찾아내는 것이다.

경로 생성 단계는 주어진 탐색 공간의 연결 망에서 충돌을 일으키지 않도록 장애물 공간을 회피함은 물론, 제약 조건을 만족하면서 시작점과 목표점을 연결하는 경로를 매끄럽게(smoothing) 생성하는 과정이다.

인식부(220)는 머니퓰레이터(130)가 작업 명령을 수행하기 위해 주어진 정보 즉, 작업 명령을 수행하기 전의 머니퓰레이터(130)의 초기 위치에서의 형상(시작점)과 작업 명령을 수행할 수 있는 머니퓰레이터(130)의 목표 위치에서의 형상(목표점), 그리고 시작점과 목표점 사이에 있는 장애물들을 형상 공간(C-Space)에서 인식하여 경로 계획 생성부(210)에 전달하고, 이 인식 정보는 경로 계획 생성부(210)에서 머니퓰레이터(130)의 이동 경로를 계획하는데 근거가 된다.

로봇 제어부(230)는 경로 계획 생성부(210)에서 전달받은 경로 계획에 따라 구동부(240)를 제어하여 머니퓰레이터(130)를 구동시킴으로서 머니퓰레이터(130)의 이동을 제어한다.

이하, 상기와 같이 구성된 로봇 및 그 경로 계획 방법의 동작과정 및 작용효과를 설명한다.

도 6은 본 발명의 일 실시예에 의한 형상 공간을 나타낸 도면이다.

도 6에서, 형상 공간(C-Space)은 머니퓰레이터(130)가 작업을 수행하는 동적 공간으로, S는 머니퓰레이터(130)가 작업을 수행하기 전 초기 위치에서의 형상을 형상 공간(C-Space)에서 하나의 노드로 형성한 시작점을 나타내고, G는 머니퓰레이터(130)가 작업을 수행할 수 있는 즉, 물체(A)를 잡을 수 있는 목표 위치에서의 형상을 형상 공간(C-Space)에서 하나의 노드로 형성한 목표점을 나타내며, K는 시작점(S)과 목표점(G) 사이에 있는 장애물의 형상 공간을 나타낸다.

장애물(K)에 대한 충돌 회피, 조인트 리미트 등의 제약 조건을 만족하면서, 경로를 계획하는 방법 중 하나로 샘플링을 이용한 동작 계획(Sampling Based Motion Planning)이 있다. 이 방법 중의 하나로 RRT(Rapidly Random Tree) 알고리즘이 있다.

RRT 알고리즘은 머니퓰레이터(130)가 하나의 노드로 형성된 트리(T)로 시각화되며, 초기 시작점(S)에서 시작하여 최종 목표점(G)에 이르기까지 장애물(K)에 대한 충돌 회피, 조인트 리미트 등의 제약 조건을 만족하게 하는 경로를 찾는 알고리즘이다. 여기서 트리(T)의 확장(extend)은 형상 공간(C-Space)에서 랜덤하게 샘플링한 임의의 점(g)에서 트리(T)에 속하는 노드들 중 가장 가까운 노드를 선택하고, 가상선을 이은 후 가상선 안의 샘플(g_new)이 제약 조건을 만족하면 그 샘플(g_new)을 트리에 포함시키고, 샘플(g_new)이 목표점(G)에 도달할 때까지 계속 트리(T)를 확장해 나간다. 이러한 RRT 알고리즘을 이용한 본 발명의 경로 계획 생성을 도 7 내지 도 16을 참조하여 상세히 설명한다.

도 7 내지 도 12는 본 발명의 일 실시예에 의한 형상 공간에서 트리를 생성하는 과정을 시각화한 도면이다.

초기 시작점(S), 최종 목표점(G), 장애물(K)이 표시된 형상 공간(C-Space)에서 임의의 점(g)을 랜덤하게 샘플링하고(도 7 참조), 랜덤하게 샘플링된 점(g)과 Goal Score가 가장 작은 노드를 점선의 가상선으로 연결한 후 일정 간격으로 떨어진 새로운 샘플(g_new)을 생성하고(도 8 참조), 이 새로운 샘플(g_new)에 대하여 제약 조건(충돌 회피, 조인트 리미트 등) 및 Goal Score를 체크하여 제약 조건 및 Goal Score 조건을 만족하면 새로운 샘플(g_new)을 트리 T1에 추가한다(도 9 참조). 이러 한 방식으로 새로운 샘플(g_new)을 일정하게 확장될 때까지 반복해서 트리 T1에서 트리 T2로 확장한다(도 10 참조).

도 7 내지 도 10에 도시한 트리 확장 과정을 반복해서 새로운 샘플(g_new)들을 트리에 추가함으로써 트리를 확장한다(도 11 및 도 12 참조). 이러한 과정을 반복하여 확장된 샘플(g_new)이 목표점(G)을 향하여 진행하게 되는데, 이때 확장된 샘플(g_new)이 Goal Function을 만족하면 그래프 탐색 단계를 수행하여 머니퓰레이터(130)의 이동 경로를 만들어 준다.

이러한 RRT 알고리즘의 트리 확장 시, 노드가 최종 목표점(G)에 도달하는 트리를 확장해 나갈 때 샘플(g_new)이 제약 조건(충돌 회피, 조인트 리미트 등)과 Goal Score 조건을 만족하면 샘플(g_new)을 트리에 포함시켜 트리를 확장하는 것이기 때문에 트리가 도 12에 도시한 바와 같이, 수직에 가까운 형태로 옆으로 뻗치는 경우가 많아 불필요하게 우회하거나 또는 지그재그한 형태의 경로를 생성할 수 있다.

따라서, 본 발명의 일 실시예는 제약 조건(충돌 회피, 조인트 리미트 등)을 만족하면서 얻은 RRT 경로를 매끄럽게(smoothing) 하는 방법을 제시한다. 이를 위해 RRT 경로에서 임의의 두 점을 선택하여 연결하는 중간 경로(P1)를 생성하고, 이 중간 경로(P1)를 일정 간격으로 나누어 나누어진 각 노드(n₁, n₂, n₃)가 제약 조건을 만족하는지 판단하여 각 노드(n₁, n₂, n₃)가 제약 조건을 만족하면 임의의 두 점을 RRT 경로에 집어넣고 Short Cut의 중간 경로(P1)를 저장한다(도 13 참조).

그러나, RRT 경로에서 임의의 두 점을 선택하여 연결한 중간 경로(P2)를 일정 간격으로 나눈 각 노드(n₁, n₂, n₃, n₄)가 제약 조건을 만족하지 못하는 경우가 발생할 수 있다(도 14 참조). 이 경우 종래에는 연결하려고 했던 임의의 두 점을 버렸으나, 본 발명의 일 실시예에서는 임의의 두 점을 연결한 중간 경로(P2)를 프로젝션(projection; 투영)시켜 제약 조건을 만족시키도록 한다.

도 15는 본 발명의 일 실시예에 의한 RRT Path Smoothing을 위한 중간 경로의 프로젝션 과정을 시각화한 도면이다.

도 15에서, 임의의 두 점을 선택하여 연결한 중간 경로(P2)에서 일정 간격으로 나누어진 각 노드들(n₁, n₂, n₃, n₄) 중 하나의 노드(n₄)를 작업 공간상의 제약 조건을 만족하도록 프로젝션시킨 것을 나타낸 것이다.

노드(n₄)가 작업 공간상의 제약 조건을 만족하도록 프로젝션을 하는 방법은 아래의 [식 2]와 같다.

[식 2]

n_projected = n₄ - J^-1△χ_error

△χ_error = J△n₄

[식 2]에서, J는 Jacobian이다. △χ_error는 작업 공간상에서의 에러로, 머니퓰레이터(130)의 작업 공간상에서의 제약 조건을 얻어 작업 공간상에서의 제약 조건과 형상 공간상에서의 제약 조건을 비교하여 작업 공간상에서의 에러를 계산한 것이다. 또한 △n₄는 노드(n₄)에서 조인트들의 벡터로, 자유도에 따라 3~7개의 조인트가 있을 수 있다.

[식 2]에 의해 작업 공간상에서의 에러를 보정하기 위해 노드(n₄)를 프로젝션시킨 후에 프로젝션 노드(n_projected)가 제약 조건을 만족하는지 체크하여 프로젝션 노드(n_projected)가 제약 조건을 만족하면 임의의 두 점을 경로에 집어넣고 Short Cut 경로를 저장한다.

도 15에서는 제약 조건을 만족하도록 중간 경로(P2)의 각 노드들(n₁, n₂, n₃, n₄) 중 하나의 노드(n₄)를 프로젝션시킨 것을 예로 들어 설명하였으나, 중간 경로(P2)의 모든 노드들(n₁, n₂, n₃, n₄)을 프로젝션시켜 저장함은 물론이다.

이러한 중간 경로(P2)의 프로젝션을 통해 제약 조건을 만족하면서 RRT 경로를 매끄럽게 하는 과정을 도 16을 참조하여 설명한다.

도 16은 본 발명의 일 실시예에 의한 로봇에서 RRT Path Smoothing 방법을 나타낸 동작 순서도이다.

도 16에서, 경로 계획 생성부(210)는 인식부(220)를 통해 머니퓰레이터(130)가 작업 명령을 수행하기 위해 주어지는 정보 즉, 작업 명령을 수행하기 전의 머니퓰레이터(130)의 초기 위치에서의 형상(시작점; S)과 작업 명령을 수행할 수 있는 머니퓰레이터(130)의 목표 위치에서의 형상(목표점; G), 그리고 시작점(S)과 목표점(G) 사이에 있는 장애물(K)들을 회피하기 위한 제약 조건(충돌 회피, 조인트 리 미트 등)을 형상 공간(C-Space)에서 인식한다(300; 도 6 참조).

또한, 경로 계획 생성부(210)는 초기 시작점(S)과 최종 목표점(G)의 거리와 방향 차이에 따른 함수인 Goal Function을 [식 1]을 이용하여 설정한다(310).

이어서, 경로 계획 생성부(210)는 초기 시작점(S), 최종 목표점(G), 제약 조건이 인식된 형상 공간(C-Space)에서 제약 조건을 만족하는 이동 경로를 계획하기 위해 트리를 생성하는 경우에 있어서 임의의 점(g)을 랜덤하게 샘플링한다(320; 도 7 참조).

랜덤하게 샘플링된 임의의 점(g)에서 트리(T)에 속하는 노드들 중 Goal Score가 가장 작은 노드를 선택한다(330). 랜덤하게 샘플링된 임의의 점(g)과 Goal Score가 가장 작은 선택 노드를 점선의 가상선으로 연결한 후 일정 간격으로 떨어진 새로운 샘플(g_new)을 얻는다(340; 도 8 참조).

그리고, 이 새로운 샘플(g_new)에 대하여 제약 조건(충돌 회피, 조인트 리미트 등) 및 Goal Score를 체크하여 제약 조건 및 Goal Score 조건을 만족하는가를 판단한다(350).

단계 350의 판단 결과, 샘플(g_new)이 제약 조건 및 Goal Score 조건을 만족하면 새로운 샘플(g_new)을 트리에 추가한다(360; 도 9 참조). 이러한 방식으로 새로운 샘플(g_new)을 일정하게 확장될 때까지 반복해서 확장한다(도 10 참조).

이러한 트리 확장 과정을 반복해서 새로운 샘플(g_new)들을 트리에 추가함으로 써 도 12에 도시한 바와 같이, 머니퓰레이터(130)의 RRT 경로를 생성한다(370). 생성된 RRT 경로는 트리가 수직에 가까운 형태로 옆으로 뻗치는 경우가 많아 불필요하게 우회하거나 또는 지그재그한 형태의 경로를 갖게 된다.

따라서, 경로 계획 생성부(210)는 제약 조건을 만족하면서 RRT 경로를 매끄럽게(smoothing) 하기 위해 종료 조건(최대 반복 횟수 및 최대 시간)을 만족하는지 판단한다(380).

단계 380의 판단 결과, 종료 조건을 만족하지 않으면 RRT 경로에서 임의의 두 점을 선택하여(390), 임의의 두 점을 연결하는 중간 경로(P2)를 생성한다(400; 도 14 참조).

그리고, 이 중간 경로(P2)를 일정 간격으로 나누어 나누어진 각 노드(n₁, n₂, n₃, n₄)를 프로젝션시키고(410; 도 15 참조), 프로젝션 노드(n_1-projected, n_2-projected, n_3-projected, n_4-projected)가 기구학적인 제약 조건(충돌 회피, 조인트 리미트, 작업 공간상에서의 제약 , 토크 리미트 등)을 만족하는지 판단한다(420).

일정 간격으로 나누어진 각 노드(n₁, n₂, n₃, n₄)를 프로젝션시키는 방법은, 위에서 설명한 [식 2]을 참조한다.

단계 420의 판단 결과, 프로젝션 노드(n_1-projected, n_2-projected, n_3-projected, n_4-projected)가 제약 조건을 만족하지 않으면 단계 390로 피드백하여 이후의 동작을 진행한다.

한편, 단계 420의 판단 결과, 프로젝션 노드(n_1-projected, n_2-projected, n_3-projected, n_4-projected)가 제약 조건을 만족하면 임의의 두 점을 경로에 집어넣고 Short Cut의 RRT 경로를 저장하고(430), 단계 380로 피드백하여 종료 조건을 만족할 때까지 이후의 동작을 진행한다.

한편, 단계 380의 판단 결과, 종료 조건이면 단계 390에서 430의 과정을 통해 제약 조건을 만족하면서 매끄러워진 RRT 경로를 생성한다(440).

따라서, 경로 계획 생성부(210)는 이러한 방식을 통하여 머니퓰레이터(130)의 RRT 경로를 매끄럽게 한 후 그래프 탐색 단계를 통해 머니퓰레이터(130)의 경로를 생성한다. 이후 로봇 제어부(230)는 생성된 경로에 따라 구동부(240)를 제어하여 머니퓰레이터(130)의 이동을 제어한다.

본 발명의 일 실시예를 통하여 RRT 알고리즘에서 생성된 지그재그한 경로를 제약 조건을 만족하면서 매끄럽게 할 수 있다.

도 17 내지 도 19는 본 발명의 일 실시예에서 제시한 RRT Path smoothing 방법에 대한 시뮬레이션 동작을 작업 공간에 나타낸 도면이다.

도 17은 머니퓰레이터(130)가 작업을 수행하기 전 초기 위치에서의 형상(시작점; S)을 나타낸 것으로, 테이블 위에는 작업 대상 물체(A)와 장애물(K)이 놓여져 있다.

도 18은 머니퓰레이터(130)가 본 발명의 일 실시예에서 제시한 RRT 경로 smoothing 방법을 통해 작업 대상 물체(A)를 잡기 위한 과정을 나타낸 것이고, 도 19는 머니퓰레이터(130)가 본 발명의 일 실시예에서 제시한 RRT 경로 smoothing 방법을 통해 작업 대상 물체(A)를 z방향(높이 방향)으로 이동시키기 위한 과정을 나타낸 것이다.

이러한 시뮬레이션 동작의 결과를 종래와 비교해 보면 도 20 내지 도 22에 도시한 바와 같다.

도 20 내지 도 22는 본 발명의 일 실시예에서 제시한 RRT Path smoothing 방법의 시뮬레이션 결과와 종래 방법의 시뮬레이션 결과를 나타낸 엔드 이펙터의 궤적도이다.

도 20은 RRT 경로를 매끄럽게 하지 않은 경우, 엔드 이펙터(131)의 궤적을 나타낸 그래프로서, z방향의 제약 조건은 만족하나, x, y방향으로 왕복하는 경향이 있으며, 이로 인해 엔드 이펙터(131)에서 작업을 수행하는데 있어서도 시간이 15초 이상 걸림을 알 수 있다.

도 21은 RRT 경로를 종래의 방법으로 매끄럽게 한 경우, x, y 방향으로 왕복하는 현상이 줄고 시간 역시 1/3이상 줄었으나, 가장 중요한 z방향의 제약 조건을 만족하지 못함을 알 수 있다.

도 22는 RRT 경로를 본 발명의 일 실시예에서 제시한 방법으로 매끄럽게 한 경우, x, y 방향으로 왕복하는 현상이 줄고 시간 역시 1/2이상 줄었으며, 가장 중요한 z방향의 제약 조건도 만족함을 알 수 있다.

도 23은 본 발명의 다른 실시예에 의한 로봇에서 RRT Path Smoothing 방법을 나타낸 동작 순서도로서, 도 16과 동일한 부분에 대해서는 중복되는 설명을 생략하 기로 한다.

도 23에서, 경로 계획 생성부(210)는 시작점(S)과 목표점(G), 그리고 시작점(S)과 목표점(G) 사이에 있는 장애물(K)들을 회피하기 위한 제약 조건(충돌 회피, 조인트 리미트 등)을 형상 공간(C-Space)에서 인식한다(500; 도 6 참조).

또한, 경로 계획 생성부(210)는 시작점(S)과 목표점(G)의 거리와 방향 차이에 따른 함수인 Goal Function을 [식 1]을 이용하여 설정한다(510).

이어서, 경로 계획 생성부(210)는 시작점(S), 목표점(G), 제약 조건이 인식된 형상 공간(C-Space)에서 임의의 점(g)을 랜덤하게 샘플링한다(520; 도 7 참조).

랜덤하게 샘플링된 임의의 점(g)에서 트리(T)에 속하는 노드들 중 Goal Score가 가장 작은 노드를 선택한다(530). 랜덤하게 샘플링된 임의의 점(g)과 Goal Score가 가장 작은 선택 노드를 연결하여 새로운 샘플(g_new)을 얻는다(540; 도 8 참조). 그리고 이 새로운 샘플(g_new)이 제약 조건 및 Goal Score 조건을 만족하는가를 판단한다(550).

단계 550의 판단 결과, 샘플(g_new)이 제약 조건 및 Goal Score 조건을 만족하면 새로운 샘플(g_new)을 트리에 추가한다(560; 도 9 참조). 이러한 방식으로 새로운 샘플(g_new)을 일정하게 확장될 때까지 반복해서 확장한다(도 10 참조).

이러한 트리 확장 과정을 반복해서 도 12에 도시한 바와 같이, 머니퓰레이터(130)의 RRT 경로를 생성한다(570). 생성된 RRT 경로는 트리가 수직에 가까운 형 태로 옆으로 뻗치는 경우가 많아 불필요하게 우회하거나 또는 지그재그한 형태의 경로를 갖게 된다.

따라서, 경로 계획 생성부(210)는 제약 조건을 만족하면서 RRT 경로의 고주파적인 특성을 완화하여 RRT 경로를 매끄럽게(smoothing) 하기 위해 종료 조건(최대 반복 횟수 및 최대 시간)을 만족하는지 판단한다(580).

단계 580의 판단 결과, 종료 조건을 만족하지 않으면 RRT 경로 전체 즉, 모든 노드를 선택하여(590), 이동할 평균 경로를 생성한다(600). 이 방법은 전체 RRT 경로를 필터링없이 수정하는 로우 패스 필터링(Low Pass Filtering) 방법으로, 제약 조건과 충돌이 발생할 수 있다.

따라서, 평균 경로(구체적으로, 이동할 평균 경로에 있는 복수의 노드)를 프로젝션시켜(610), 기구학적 제약 조건(충돌 회피, 조인트 리미트, 작업 공간상에서의 제약 , 토크 리미트 등)을 만족하는지 판단한다(620).

단계 620의 판단 결과, 제약 조건을 만족하지 않으면 단계 580로 피드백하여 이후의 동작을 진행한다.

한편, 단계 620의 판단 결과, 제약 조건을 만족하면 프로젝션된 평균 경로를 집어넣어 Short Cut의 매끄러워진 RRT 경로(RRT Path Smoothing)를 저장하고(630), 단계 580로 피드백하여 종료 조건을 만족할 때까지 이후의 동작을 진행한다.

한편, 단계 580의 판단 결과, 종료 조건이면 단계 590에서 630의 과정을 통해 기구학적 제약 조건을 만족하면서 매끄러워진 RRT 경로(RRT Path Smoothing)를 생성한다(640).

본 발명의 다른 실시예를 통하여 RRT 알고리즘에서 생성된 지그재그한 경로를 제약 조건을 만족하면서 매끄럽게 할 수 있다.

도 5는 본 발명의 일 실시예에 의한 로봇에서 머니퓰레이터의 이동 경로를 계획하기 위한 제어 블록도이다.

도 13은 본 발명의 일 실시예에 의한 RRT Path Smoothing을 위한 중간 경로의 생성 과정의 일례를 시각화한 도면이다.

도 14는 본 발명의 일 실시예에 의한 RRT Path Smoothing을 위한 중간 경로의 생성 과정의 다른 예를 시각화한 도면이다.

도 17 내지 도 19는 본 발명의 일 실시예에서 제시한 RRT Path smoothing 방 법에 대한 시뮬레이션 동작을 작업 공간에 나타낸 도면이다.

도 23은 본 발명의 다른 실시예에 의한 로봇에서 RRT Path Smoothing 방법을 나타낸 동작 순서도로서, 도 16과 동일한 부분에 대해서는 중복되는 설명을 생략하기로 한다.

*도면의 주요 부분에 대한 부호의 설명*

10 : 로봇 130 : 머니퓰레이터

131 : 엔드 이펙터 132, 133, 134 : 관절

135, 136 : 링크 210 : 경로 계획 생성부

220 : 인식부 230 : 로봇 제어부

240 : 구동부

Claims

로봇 머니퓰레이터의 이동 경로를 생성하기 위해 시작점, 목표점, 제약 조건을 가지는 형상 공간을 형성하고;

상기 형상 공간 상에서의 제약 조건을 만족하면서 상기 시작점과 상기 목표점을 연결하는 RRT 경로를 생성하고;

상기 RRT 경로에서 임의의 두 점을 선택하여 연결하는 중간 경로를 생성하고;

상기 중간 경로를 프로젝션시켜 상기 제약 조건을 만족하는지 여부를 판단하고;

상기 제약 조건을 만족하면 상기 임의의 두 점을 상기 RRT 경로에 저장하여 상기 RRT 경로를 매끄럽게 하는 로봇의 경로 계획 방법.
제1항에 있어서,

상기 시작점은 상기 로봇 머니퓰레이터가 작업을 수행하기 전 초기 위치에서의 형상을 상기 형상 공간 상에 형성한 노드인 로봇의 경로 계획 방법.
제1항에 있어서,

상기 목표점은 상기 로봇 머니퓰레이터가 작업을 수행하기 위한 목표 위치에서의 형상을 상기 형상 공간 상에 형성한 노드인 로봇의 경로 계획 방법.
제1항에 있어서,

상기 중간 경로를 프로젝션시키는 것은,

상기 로봇 머니퓰레이터의 작업 공간 상에서의 제약 조건을 얻고;

상기 작업 공간 상에서의 제약 조건과 상기 형상 공간 상에서의 제약 조건을 비교하여 상기 작업 공간 상에서의 에러를 계산하고;

상기 작업 공간 상에서의 에러를 보정하기 위해 상기 중간 경로의 각 노드를 프로젝션시키는 로봇의 경로 계획 방법.
제4항에 있어서,

상기 중간 경로의 각 노드는 상기 중간 경로를 일정 간격으로 나누어 얻은 복수의 노드인 로봇의 경로 계획 방법.
제5항에 있어서,

상기 제약 조건을 만족하는지 여부를 판단하는 것은,

상기 중간 경로의 각 노드가 상기 제약 조건을 만족하는지 여부를 판단하는 로봇의 경로 계획 방법.
로봇 머니퓰레이터의 이동 경로를 생성하기 위해 시작점, 목표점, 제약 조건을 가지는 형상 공간을 형성하고;

상기 형상 공간 상에서의 제약 조건을 만족하면서 상기 시작점과 상기 목표점을 연결하는 RRT 경로를 생성하고;

상기 RRT 경로 전체를 로우 패스 필터링하고;

상기 로우 패스 필터링한 경로를 프로젝션시켜 상기 제약 조건을 만족하는지 여부를 판단하고;

상기 제약 조건을 만족하면 상기 프로젝션된 경로를 상기 RRT 경로에 저장하여 상기 RRT 경로를 매끄럽게 하는 로봇의 경로 계획 방법.
제7항에 있어서,

상기 로우 패스 필터링한 경로를 프로젝션시키는 것은,

상기 로봇 머니퓰레이터의 작업 공간 상에서의 제약 조건을 얻고;

상기 작업 공간 상에서의 제약 조건과 상기 형상 공간 상에서의 제약 조건을 비교하여 상기 작업 공간 상에서의 에러를 계산하고;

상기 작업 공간 상에서의 에러를 보정하기 위해 상기 로우 패스 필터링한 경로의 각 노드를 프로젝션시키는 로봇의 경로 계획 방법.
제8항에 있어서,

상기 로우 패스 필터링한 경로의 각 노드는 상기 로우 패스 필터링한 경로에 있는 모든 노드인 로봇의 경로 계획 방법.
로봇 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하는 인식부; 및

상기 로봇 머니퓰레이터의 이동 경로를 생성하기 위해 상기 시작점, 상기 목표점 및 제약 조건을 가지는 형상 공간을 형성하고, 상기 형상 공간 상에서의 제약 조건을 만족하면서 상기 시작점과 상기 목표점을 연결하는 RRT 경로를 생성하고, 상기 RRT 경로에서 임의의 두 점을 선택하여 연결하는 중간 경로를 생성하고, 상기 중간 경로를 프로젝션시켜 상기 제약 조건을 만족하는지 여부를 판단하고, 상기 제약 조건을 만족하면 상기 임의의 두 점을 상기 RRT 경로에 저장하여 상기 RRT 경로를 매끄럽게 하는 경로 계획 생성부를 포함하는 로봇의 경로 계획 장치.
삭제
제10항에 있어서,

상기 경로 계획 생성부는 상기 로봇 머니퓰레이터의 작업 공간 상에서의 제약 조건을 얻고, 상기 작업 공간 상에서의 제약 조건과 상기 형상 공간 상에서의 제약 조건을 비교하여 상기 작업 공간 상에서의 에러를 계산하고, 상기 작업 공간 상에서의 에러를 보정하기 위해 상기 중간 경로의 각 노드를 프로젝션시키는 로봇의 경로 계획 장치.
로봇 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하는 인식부; 및

상기 로봇 머니퓰레이터의 이동 경로를 생성하기 위해 상기 시작점, 상기 목표점 및 제약 조건을 가지는 형상 공간을 형성하고, 상기 형상 공간 상에서의 제약 조건을 만족하면서 상기 시작점과 상기 목표점을 연결하는 RRT 경로를 생성하고, 상기 RRT 경로 전체를 로우 패스 필터링하고, 상기 로우 패스 필터링한 경로를 프로젝션시켜 상기 제약 조건을 만족하는지 여부를 판단하고, 상기 제약 조건을 만족하면 상기 프로젝션된 경로를 상기 RRT 경로에 저장하여 상기 RRT 경로를 매끄럽게 하는 경로 계획 생성부를 포함하는 로봇의 경로 계획 장치.
제13항에 있어서,

상기 경로 계획 생성부는 상기 로봇 머니퓰레이터의 작업 공간 상에서의 제약 조건을 얻고, 상기 작업 공간 상에서의 제약 조건과 상기 형상 공간 상에서의 제약 조건을 비교하여 상기 작업 공간 상에서의 에러를 계산하고, 상기 작업 공간 상에서의 에러를 보정하기 위해 상기 로우 패스 필터링한 경로의 각 노드를 프로젝션시키는 로봇의 경로 계획 장치.