KR101667030B1

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

Info

Publication number: KR101667030B1
Application number: KR1020090073153A
Authority: KR
Inventors: 김명희; 노경식; 임산; 임복만; 곽천슈
Original assignee: 삼성전자 주식회사
Priority date: 2009-08-10
Filing date: 2009-08-10
Publication date: 2016-10-17
Anticipated expiration: 2029-08-10
Also published as: US9044862B2; KR20110015764A; US20110035051A1

Abstract

본 발명은 로봇의 머니퓰레이터가 초기 시작점에서부터 최종 목표점까지 이동하기 위한 최적의 경로를 계획하는 로봇의 경로 계획 장치 및 그 방법에 관한 것이다.

이를 위해 본 발명은 초기 시작점과 최종 목표점을 잇는 직선에서 일정한 각도 내에 있는 장애물들을 중간점으로 인식하여 중간점에서 일정 거리 떨어진 임의의 점들을 선택하고, 선택된 임의의 점들 중에서 장애물을 거치지 않고 시작점과 목표점을 바로 연결할 수 있는 임의의 점을 경유점으로 선택하여 새로운 중간 노드로 맵핑하고, 중간 노드 점을 경유하여 경로를 확장하여 Goal Function에 크게 의존하지 않아도 트리가 잘못된 방향으로 확장되는 것을 최소화하여 Local Minima에 빠지지 않도록 하며, 이를 통해 경로 계획의 성능을 개선하여 빠른 시간 내에 경로를 찾을 수 있게 된다.

Description

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

본 발명은 로봇의 경로 계획 장치 및 그 방법에 관한 것으로, 더욱 상세하게는 로봇의 머니퓰레이터가 시작점에서부터 목표점까지 이동하기 위한 최적의 경로를 계획하는 로봇의 경로 계획 장치 및 그 방법에 관한 것이다.

일반적으로, 전기적 또는 자기적인 작용을 이용하여 인간의 동작과 닮은 운동을 행하는 기계장치를 로봇이라고 한다. 초기의 로봇은 생산 현장에서의 작업 자동화ㆍ무인화 등을 목적으로 한 머니퓰레이터나 반송 로봇 등의 산업용 로봇으로 인간을 대신하여 위험한 작업이나 단순한 반복 작업, 큰 힘을 필요로 하는 작업을 수행하였으나, 최근에는 인간과 유사한 관절체계를 가지고 인간의 작업 및 생활 공간에서 인간과 공존하며 다양한 서비스를 제공하는 인간형 로봇(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)를 확장해 나가면서 최종 목표점까지 이동 경로를 찾는 방법이다. 이 경우 가장 가까운 노드(Node)를 선택하는 방법은 크게 2가지로 나눌 수 있는데 한가지는 최종 목표점의 Goal Configuration의 정보를 모두 아는 상태에서 목표점의 노드와 현재의 노드의 차이를 이용하는 방법과, 최종 목표점의 Goal Configuration의 정보를 알거나 혹은 모르는 상태에서, 현재의 엔드 이펙터에서 목표점의 엔드 이펙터까지의 거리와 방향 벡터의 함수로 Goal Function을 만들어 Goal Function이 가장 작은 노드를 선택하여 트리를 확장해 나가는 방법이 있다.

종래의 RRT 알고리즘 중 골 형상을 모두 알지 못하는 경우, Goal Function을 이용하는 RRT 알고리즘이 제안되었으나, Goal Function에 크게 의존하는 경우, Goal Function이 정확히 정해지지 않았으면 Local Minima에 빠져 해(Solution)를 찾지 못하게 되고, Local Minima에 빠졌을 경우 Goal Function을 변화시켜 Local Minima에서 빠져 나오도록 하고 있으나 Local Minima을 인지하기 전까지는 잘못된 방향으로 트리가 확장되기 때문에 해(Solution)를 찾는 속도가 그만큼 느려져 경로를 찾는데 많은 시간이 소요된다.

본 발명은 Goal Score에 크게 의존하지 않아도 트리가 잘못된 방향으로 확장되는 것을 최소화함으로써 경로 계획의 성능을 개선한 로봇의 경로 계획 장치 및 그 방법을 제시하고자 한다.

이를 위해 본 발명의 실시예에 의한 로봇의 경로 계획 장치는, 머니퓰레이터의 이동 경로를 생성하기 위한 형상 공간을 형성하고; 상기 형상 공간의 시작점과 목표점 사이에 장애물이 있는 경우, 상기 장애물을 중간점으로 인식하여 새로운 경유점을 선정하고; 상기 시작점에서 생성된 노드가 상기 경유점을 경유하여 상기 목표점에 도달하도록 트리를 확장하여 탐색 그래프를 생성하고; 상기 탐색 그래프를 통해 상기 시작점과 목표점을 연결하여 상기 장애물을 회피하는 최적 경로를 생성하는 것을 특징으로 한다.

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

상기 목표점은 상기 머니퓰레이터가 작업을 수행하기 위한 목표 위치에서의 형상을 상기 형상 공간상에 형성한 노드이거나, 역기구학의 해가 존재하지 않는 경우, 목표점의 Position과 Orientation의 정보를 가지고 있는 점인 것을 특징으로 한다.

상기 중간점은 상기 시작점과 상기 목표점을 잇는 직선에서 일정한 각도 내에 있는 장애물인 것을 특징으로 한다.

상기 경유점을 선정하는 것은, 상기 중간점에서 일정 거리 떨어진 임의의 점들을 선택하여 상기 임의의 점들을 상기 시작점과 목표점에 각각 연결하고, 그 사이에 다른 장애물이 존재하지 않는 임의의 점들을 상기 경유점으로 선정하는 것을 특징으로 한다.

상기 경유점을 선정하는 것은, 상기 시작점에서 무작위로 샘플링한 형상에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 선정하는 것을 특징으로 한다.

상기 트리를 확장하는 것은, 상기 시작점에서 무작위로 샘플링한 형상에서 확률에 따라 변화된 Goal Function에 의거한 가장 가까운 노드를 찾아서 상기 샘플링한 형상과 최근접 노드를 연결하여 트리를 확장하는 것을 특징으로 한다.

그리고, 본 발명의 실시예에 의한 로봇의 경로 계획 방법은, 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하고; 상기 시작점과 목표점 사이에 장애물이 있는 경우, 상기 장애물을 중간점으로 인식하여 새로운 경유점을 맵핑하고; 형상 공간에서 임의의 한 점을 무작위로 샘플링하여 가장 가까운 노드를 찾아 상기 샘플링한 임의의 점과 상기 노드를 연결하고; 상기 노드가 상기 경유점을 경유하여 상기 목표점에 도달하도록 트리를 확장하여 탐색 그래프를 생성하고; 상기 탐색 그래프를 통해 상기 시작점과 목표점을 연결하여 상기 장애물을 회피하는 최적 경로를 생성하는 것을 특징으로 한다.

또한, 본 발명의 실시예에 의한 로봇의 경로 계획 방법은, 상기 맵핑 시 Goal Function을 고려하여 중간점을 선정하는 것을 특징으로 한다.

상기 경유점을 선정하는 것은, 상기 시작점에서 무작위로 샘플링한 형상에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 선정하여 상기 트리를 확장해 나가는 것을 특징으로 한다.

그리고, 본 발명의 실시예에 의한 로봇의 경로 계획 장치는, 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점, 상기 시작점과 목표점 사이의 장애물을 인식하는 인식부; 상기 머니퓰레이터의 이동 경로를 생성하기 위한 형상 공간을 형성하고, 상기 형상 공간을 바탕으로 상기 장애물을 중간점으로 하는 새로운 경유점을 선정하고, 상기 경유점을 이용하여 상기 시작점과 목표점을 연결하는 최적 경로를 생성하는 경로 계획 생성부를 포함한다.

상기 경로 계획 생성부는 상기 중간점에서 일정 거리 떨어진 임의의 점들을 선택하여 상기 임의의 점들을 상기 시작점과 목표점에 각각 연결하고, 그 사이에 다른 장애물이 존재하지 않는 임의의 점들을 상기 경유점으로 선정하는 것을 특징으로 한다.

상기 경로 계획 생성부는 상기 형상 공간에서 임의의 한 점을 무작위로 샘플링하여 가장 가까운 노드를 찾아 상기 샘플링한 임의의 점과 상기 노드를 연결하고, 상기 노드가 상기 경유점을 경유하여 상기 목표점에 도달하도록 트리를 확장하여 탐색 그래프를 생성하는 것을 특징으로 한다.

상기 경로 계획 생성부는 상기 트리에서 새로 샘플링하여 얻은 점과 최근접 노드를 연결하여 상기 트리를 확장해 나가는 과정에서 상기 얻은 점에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 선정하는 것을 특징으로 한다.

이러한 본 발명의 실시예에 의하면 초기 시작점과 최종 목표점을 잇는 직선 에서 일정한 각도 내에 있는 장애물들을 중간점으로 인식하여 중간점에서 일정 거리 떨어진 임의의 점들을 선택하고, 선택된 임의의 점들 중에서 장애물을 거치지 않고 시작점과 목표점을 바로 연결할 수 있는 임의의 점을 경유점으로 선택하여 새로운 중간 노드로 맵핑하고, Goal Function을 고려하여 트리를 확장해 나감으로써 트리가 잘못된 방향으로 확장되는 것을 최소화하여 Local Minima에 빠지지 않도록 하며, 이를 통해 경로 계획의 성능을 개선하여 빠른 시간 내에 경로를 찾을 수 있게 된다.

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

도 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)과 관절들(132, 133, 134) 사이의 기하학적인 관계에 따라 운동 특성이 결정된다. 이 기하학적인 관계를 수학적으로 표현한 것이 기구학(Kinematics)이며 대부분의 머니퓰레이터(130)는 이러한 기구학적 특성(kinematics characteristic)을 가지고 작업을 수행하기 위한 방향으로 엔드 이펙터(131)를 이동시킨다. 본 발명의 실시예에 의한 머니퓰레이터(130)는 위치와 방향 조절이 가능한 링크들(135, 136)을 이용하여 작업 대상 물체를 잡기 위한 목표점으로 엔드 이펙터(131)를 이동시킨다.

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

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

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

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

공간 형성 단계는 충돌 회피 경로를 생성하기 위한 형상 공간(C-Space)을 찾아내는 과정으로, 여기에서 형상이란 움직이는 머니퓰레이터(130)의 위치 및 방향을 표현할 수 있는 변수의 집합을 말하며, 형상들이 점유할 수 있는 모든 공간을 형상 공간(C-Space)이라 한다.

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

경로 생성 단계는 주어진 탐색 공간의 연결 망에서 충돌을 일으키지 않도록 장애물 공간을 회피하여 시작점과 목표점을 연결하는 최적 경로를 생성하는 과정이다.

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

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

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

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

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

도 6은 도 5에서 중간점을 인식하는 과정을 시각화한 도면이다.

도 6에서, 시작점(S)과 목표점(G)을 잇는 직선에서 일정한 각도(α) 내에 있는 장애물을 중간점(K)으로 인식한다.

도 7 및 도 8은 도 6에서 인식된 중간점을 기준으로 하여 경유점을 선택하는 과정을 시각화한 도면이다.

도 7에서, 인식된 중간점(K)에서 일정 거리 떨어진 임의의 점들(K-1, K-2)을 선택하여 선택된 임의의 점들(K-1, K-2)과 시작점(S)을 연결하고, 선택된 임의의 점들(K-1, K-2)과 목표점(G)을 연결한다.

도 7에서 선택된 임의의 점들(K-1, K-2)은 중간점(K)을 거치지 않고 시작점(S)과 목표점(G)을 바로 연결할 수 있기 때문에 거쳐야 하는 경유점으로 선택한다. 경유점으로 선택되는 점들은 시작점(S)과 목표점(G)을 모두 볼 수 있는 지점에 위치한 중간 노드이다.

도 8에서, 인식된 중간점(K)에서 일정 거리 떨어진 임의의 점들(K-3, K-4)을 선택하여 선택된 임의의 점들(K-3, K-4)과 시작점(S)을 연결하고, 선택된 임의의 점들(K-3, K-4)과 목표점(G)을 연결한다.

도 8에서 선택된 임의의 점들(K-3, K-4)은 시작점(S)과 목표점(G)을 연결하는 직선상에 중간점(K)이 존재하여 시작점(S)과 목표점(G)을 바로 연결할 수 없기 때문에 경유점으로 선택하지 않는다. 즉, 임의의 점(K-3)은 목표점(G)은 볼 수 있으나 시작점(S)을 볼 수 없고, 임의의 점(K-4)은 시작점(S)은 볼 수 있으나 목표점(G)을 볼 수 없기 때문에 경유점으로 선택할 수 없다.

이와 같이, 경유점들이 선택되면, 시작점(S)에서 무작위로 샘플링한 형상에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 최종 선정하며, 이 과정에서 샘플링한 형상이 여러 개 존재할 경우 Goal Score에 근거하여 우선 순위를 둘 수 있다. Goal Score에 근거하여 우선 순위를 두는 방식은 종래의 RRT 알고리즘과 동일하다.

본 발명의 실시예에 의한 경로 계획 생성부(210)는 시작점(S)과 목표점(G)을 잇는 직선에서 일정한 각도(α) 내에 있는 장애물을 중간점(K)으로 인식하고, 인식된 중간점(K)에서 일정 거리 떨어진 임의의 점들(K-1, K-2)을 선택하여 시작점(S)과 목표점(G)을 볼 수 있는 임의의 점들(K-1, K-2)을 경유점1, 2로 선택한다.

경유점1, 2(K-1, K-2)가 선택되면, RRT 알고리즘을 이용한 트리 확장 방법으로 머니퓰레이터(130)의 이동 경로를 계획한다.

RRT 알고리즘은 형상 공간(C-Space)에서 머니퓰레이터(130)가 하나의 노드로 형성된 트리(T)로 시각화되며, 초기 시작점(S)에서 시작하여 최종 목표점(G)에 이르기까지 장애물 회피 등의 제약 조건을 만족하게 하는 경로를 찾는 알고리즘이다. 여기서 트리(T)의 확장(extension)은 형상 공간(C-Space)에서 무작위로 샘플링한 형상(g)에서 트리(T)에 속하는 노드들 중 가장 가까운 노드를 정하고 그 최근접 노드에서 일정 거리만큼 떨어진 새로운 노드를 선정하는 과정의 반복이다. 이러한 RRT 알고리즘을 이용한 본 발명의 경로 계획 생성을 도 9 내지 도 12를 참조하여 상세히 설명한다.

도 9는 본 발명의 실시예에 의한 형상 공간에서 시작점과 목표점 사이에 중간점이 존재하는 경우, 경유점을 선택하여 트리를 생성하는 과정을 시각화한 도면이다.

도 9에서, 경유점1, 2(K-1, K-2)가 선택된 형상 공간(C-Space)에서 제약 조건을 만족하는 이동 경로를 계획하기 위해 트리를 생성하는 경우에 있어서 무작위 로 샘플링된 임의의 형상(g)에서 가장 가까운 노드(g_near)를 찾아서 샘플링한 형상(g)과 최근접 노드(g_near)를 연결하여 트리1(T1)를 확장한다.

도 10은 본 발명의 실시예에 의한 경유점이 선택된 형상 공간에서 트리가 경유점까지 도달하는 과정을 시각화한 도면이다.

도 10에서, 도 9에서 확장된 트리1(T1)는 새로 샘플링한 형상(g_new)과 최근접 노드(g_near)를 연결하여 트리2(T2)를 확장하는데 이때 새로 샘플링한 형상(g_new)에서 얻은 점이 경유점1(K-1)에 가깝기 때문에 선택된 경유점1, 2(K-1, K-2) 중에서 경유점1(K-1)을 거쳐야 하는 목표 경유점으로 최종 선정하여 경유점1(K-1)까지 트리2(T2)를 확장한다.

도 11 및 도 12는 본 발명의 실시예에 의한 경유점이 선택된 형상 공간에서 트리가 경유점을 경유하여 확장해 나가는 과정을 시각화한 도면이다.

도 11 및 도 12에서, 도 10에서 확장된 트리2(T2)는 경유점1(K-1)을 경유하여 목표점(G)으로 가는 방향으로 진행하게 되는데, 이때 도 10과 마찬가지로 새로 샘플링한 형상(g_new')과 최근접 노드(g_near')를 연결하여 트리3(T3)을 확장한 후 이러한 과정을 반복하여 다시 목표점(G)까지 트리4(T4)를 확장한다.

목표점(G)까지 트리4(T4)를 확장해 나가면서 목표점(G)과 머니퓰레이터(130)의 엔드 이펙터(131) 사이의 거리가 일정 거리 미만이 되면 경로를 찾았다고 인정하고 그래프 탐색 단계를 수행하여 머니퓰레이터(130)의 이동 경로를 찾아 준다.

도 13은 본 발명의 실시예에 의한 형상 공간에서 시작점과 목표점 사이에 다수의 장애물이 존재하는 경우, 경유점을 선택하는 과정을 시각화한 도면이다.

도 13에서, 시작점(S)과 목표점(G)을 잇는 직선에서 일정한 각도(α) 내에 있는 장애물들을 중간점1, 2, 3(K1, K2, K3)으로 인식하고, 인식된 중간점1, 2, 3(K1, K2, K3)에서 일정 거리 떨어진 임의의 점들((K1-1, K1-2), (K2-1, K2-2), (K3-1, K3-2))을 선택한다.

중간점1(K1)에서 선택된 점들(K1-1, K1-2)은 시작점(S)과 중간점2(K2)에서 선택된 점들(K2-1, K2-2)을 볼 수 있고, 중간점2(K2)에서 선택된 점들(K2-1, K2-2)은 중간점1(K1)에서 선택된 점들(K1-1, K1-2)과 중간점3(K3)에서 선택된 점들(K3-1, K3-2)을 볼 수 있고, 중간점3(K3)에서 선택된 점들(K3-1, K3-2)은 중간점2(K2)에서 선택된 점들(K2-1, K2-2)과 목표점(G)을 볼 수 있는 점들을 경유점으로 선택한다.

이와 같이, 경유점들이 선택되면, 시작점(S)에서 무작위로 샘플링한 형상(g)에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 최종 선정하며, 이 과정에서 샘플링한 형상(g)이 여러 개 존재할 경우 Goal Score에 근거하여 우선 순위를 둘 수 있다. Goal Score에 근거하여 우선 순위를 두는 방식은 종래의 RRT 알고리즘과 동일하다.

본 발명의 실시예에 의한 경로 계획 생성부(210)는 시작점(S)과 목표점(G)을 잇는 직선에서 일정한 각도(α) 내에 있는 장애물들을 중간점1, 2, 3(K1, K2, K3)으로 인식하고, 인식된 중간점1, 2, 3(K1, K2, K3)에서 일정 거리 떨어진 임의의 점들((K1-1, K1-2), (K2-1, K2-2), (K3-1, K3-2))을 선택하여 시작점(S)과 목표점(G)을 볼 수 있는 임의의 점들((K1-1, K1-2), (K2-1, K2-2), K3-1)을 경유점1-1, 1-2, 2-1, 2-2, 3-1로 선택한다.

경유점1-1, 1-2, 2-1, 2-2, 3-1((K1-1, K1-2), (K2-1, K2-2), K3-1)가 선택되면, RRT 알고리즘을 이용한 트리 확장 방법으로 머니퓰레이터(130)의 이동 경로를 계획한다.

도 14는 본 발명의 실시예에 의한 형상 공간에서 시작점과 목표점 사이에 다수의 중간점이 존재하는 경우, 경유점을 선택하여 트리를 생성하는 과정을 시각화한 도면이다.

도 14에서, 경유점1-1, 1-2, 2-1, 2-2, 3-1((K1-1, K1-2), (K2-1, K2-2), K3-1)가 선택된 형상 공간(C-Space)에서 제약 조건을 만족하는 이동 경로를 계획하기 위해 트리를 생성하는 경우에 있어서 샘플링된 형상(g)에서 가장 가까운 노드(g_near)를 찾아서 샘플링한 형상(g)과 최근접 노드(g_near)를 연결하여 트리5(T5)를 확장한 후, 새로 샘플링한 형상(g_new)과 최근접 노드(g_near)를 연결하여 트리5(T5)를 확장하는데 이때 새로 샘플링한 형상(g_new)에서 얻은 점이 경유점1-1(K1-1)에 가깝기 때문에 선택된 경유점1-1, 1-2(K1-1, K1-2) 중에서 경유점1-1(K1-1)을 거쳐야 하는 목표 경유점으로 최종 선정하여 경유점1-1(K1-1)까지 트리5(T5)를 확장한다.

확장된 트리5(T5)는 경유점1-1(K1-1)을 경유하여 목표점(G)으로 가는 방향으로 진행하게 되는데, 이때에도 새로 샘플링한 형상(g_new')과 최근접 노드(g_near')를 연결하여 트리5(T5)를 확장한 후 새로 샘플링한 형상(g_new)과 최근접 노드(g_near)를 연결하여 트리5(T5)를 확장하는데 이때 새로 샘플링한 형상(g_new)에서 얻은 점이 경유점2-1(K2-1)에 가깝기 때문에 선택된 경유점2-1, 2-2(K2-1, K2-2) 중에서 경유점2-1(K2-1)을 거쳐야 하는 목표 경유점으로 최종 선정하여 경유점2-1(K2-1)까지 트리5(T5)를 확장한다.

이어서, 확장된 트리5(T5)는 경유점1-1, 2-1(K1-1, K2-1)을 경유하여 목표점(G)으로 가는 방향으로 진행하게 되는데, 이때에도 새로 샘플링한 형상(g_new')과 최근접 노드(g_near')를 연결하여 트리5(T5)를 확장한 후 새로 샘플링한 형상(g_new)과 최근접 노드(g_near)를 연결하여 트리5(T5)를 확장하는데 이때 샘플링한 형상(g_new)에서 얻은 점이 경유점3-1(K3-1)에 가깝기 때문에 경유점3-1(K3-1)을 거쳐야 하는 목표 경유점으로 최종 선정하여 경유점3-1(K3-1)까지 트리5(T5)를 확장한다.

마찬가지로, 확장된 트리5(T5)는 경유점1-1, 2-1, 3-1(K1-1, K2-1, K3-1)을 경유하여 목표점(G)으로 가는 방향으로 다시 진행하게 되는데, 이러한 과정을 반복하여 목표점(G)까지 트리5(T5)를 확장한다.

목표점(G)까지 트리5(T5)를 확장해 나가면서 목표점(G)과 머니퓰레이터(130)의 엔드 이펙터(131) 사이의 거리가 일정 거리 미만이 되면 경로를 찾았다고 인정하고 그래프 탐색 알고리즘을 수행하여 머니퓰레이터(130)의 이동 경로를 찾아 준다.

도 15 내지 도 18은 본 발명의 실시예에서 제시한 경로 계획에 대한 시뮬레이션 동작을 작업 공간에 나타낸 도면이다.

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

도 16은 머니퓰레이터(130)가 작업을 수행하기 위한 목표 위치에서의 형상(목표점; G)을 나타낸 것이다.

도 17은 머니퓰레이터(130)가 기존의 Goal Function 변화를 통해 해를 찾는 과정을 나타낸 것으로, 노드의 생성 수가 많고 장애물(실린더)을 회피하기 위한 경로가 작업 대상 물체(공)로부터 멀어지게 생성되었음을 알 수 있다. 이는 해(Solution)를 찾는 속도가 그만큼 느려져 경로를 찾는데 많은 시간이 소요된다.

도 18은 머니퓰레이터(130)가 본 발명에서 제시한 중간점의 경유를 통해 해를 찾는 과정을 나타낸 것으로, 도 17에 비해 노드의 생성 수가 적고 장애물(실린더)을 회피하기 위한 경로가 작업 대상 물체(공)로부터 도 17에 비해 가깝게 생성되었음을 알 수 있다. 이는 해(Solution)를 찾는 속도가 그만큼 빨라져 경로를 찾는데 소요되는 시간이 짧아진다.

도 19는 본 발명의 실시예에 의한 로봇의 경로 계획 방법을 나타낸 동작 순서도이다.

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

따라서, 경로 계획 생성부(210)는 시작점(S)과 목표점(G)을 잇는 직선에서 일정한 각도(α) 내에 있는 장애물을 중간점(K)으로 인식한 후(302; 도 6 참조), 인식된 중간점(K)에서 일정 거리 떨어진 임의의 점들(K-1, K-2)을 선택하여 선택된 임의의 점들(K-1, K-2)과 시작점(S)을 연결하고, 선택된 임의의 점들(K-1, K-2)과 목표점(G)을 연결하여 거쳐야 하는 경유점으로 선택한다(304; 도 7 참조).

이어서, 경로 계획 생성부(210)는 경유점1, 2(K-1, K-2)가 선택된 형상 공간(C-Space)에서 제약 조건을 만족하는 이동 경로를 계획하기 위해 트리를 생성하는 경우에 있어서 무작위로 샘플링된 임의의 한 점 즉, 형상(g)에서 가장 가까운 노드(g_near)를 찾아서 샘플링한 형상(g)과 최근접 노드(g_near)를 연결하여 트리1(T1)를 확장한다. 이 경우 Goal Function은 목표점(G)에서 엔드 이펙터(131)까지의 거리와 방향 벡터의 함수로 이루어져 트리 중에서 Goal Score가 가장 작은 노드를 선택하여 트리(T1)를 확장해 나가는 것이다(306~310; 도 9 참조).

이후, 확장된 현재 트리(T1)의 Goal Score가 경유점1, 2(K-1, K-2)의 Goal Score보다 작은가를 판단하여(312), 현재 트리(T1)의 Goal Score가 경유점1, 2(K-1, K-2)의 Goal Score보다 작지 않은 경우 단계 306으로 피드백하여 새로운 샘플을 저장한다.

단계 312의 판단 결과, 현재 트리(T1)의 Goal Score가 경유점1, 2(K-1, K-2)의 Goal Score보다 작은 경우 확장된 트리1(T1)는 새로 샘플링한 형상(g_new)과 최근접 노드(g_near)를 연결하여 트리2(T2)를 확장하는데 이때 새로 샘플링한 형상(g_new)에서 얻은 점이 경유점1(K-1)에 가깝기 때문에 선택된 경유점1, 2(K-1, K-2) 중에서 경유점1(K-1)을 거쳐야 하는 목표 경유점으로 최종 선정하여 경유점1(K-1)까지 트리2(T2)를 확장한다(314; 도 10 참조).

이후, 확장된 트리2(T2)는 경유점1(K-1)을 경유하여 목표점(G)으로 가는 방향으로 진행하게 되는데, 이때 도 10과 마찬가지로 새로 샘플링한 형상(g_new')과 최근접 노드(g_near')를 연결하여 트리3(T3)을 확장한 후 이러한 과정을 반복하여 다시 목표점(G)까지 트리4(T4)를 확장한다(316; 도 11 참조).

이와 같이, 목표점(G)까지 트리4(T4)를 확장해 나가면서 목표점(G)과 머니퓰레이터(130)의 엔드 이펙터 사이의 거리가 일정 거리 미만이 되면 경로를 찾았다고 인정하고 그래프 탐색 단계를 통해 머니퓰레이터(130)의 이동 경로를 찾아 경로 계획을 생성한다(318; 도 12 참조). 이후 로봇 제어부(230)가 생성된 경로 계획에 따라 구동부(240)를 제어하여 머니퓰레이터(130)의 이동을 제어한다.

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

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

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

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

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

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

240 : 구동부

Claims

머니퓰레이터의 이동 경로를 생성하기 위한 형상 공간을 형성하고;

상기 형상 공간의 시작점과 목표점 사이에 장애물이 있는 경우, 상기 장애물을 중간점으로 인식하여 경유점을 선정하고;

상기 시작점에서 생성된 노드가 상기 경유점을 경유하여 상기 목표점에 도달하도록 트리를 확장하여 탐색 그래프를 생성하고;

상기 탐색 그래프를 통해 상기 시작점과 목표점을 연결하여 상기 장애물을 회피하는 최적 경로를 생성하되,

상기 경유점을 선정하는 것은,

상기 중간점에서 일정 거리 떨어진 임의의 점들을 선택하여 상기 임의의 점들을 상기 시작점과 상기 목표점에 각각 연결하고, 상기 선택된 임의의 점들 중에서 상기 시작점과의 사이 및 상기 목표점과의 사이에 상기 장애물이 존재하지 않는 임의의 점들을 상기 경유점으로 선정하는 로봇의 경로 계획 방법.
제1항에 있어서,

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

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

상기 중간점은 상기 시작점과 상기 목표점을 잇는 직선에서 일정한 각도 내에 있는 장애물인 로봇의 경로 계획 방법.
삭제
제1항에 있어서,

상기 경유점을 선정하는 것은,

상기 시작점에서 무작위로 샘플링한 형상에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 선정하는 로봇의 경로 계획 방법.
제1항에 있어서,

상기 트리를 확장하는 것은,

상기 시작점에서 무작위로 샘플링한 형상에서 확률에 따라 변화된 Goal Function에 의거한 가장 가까운 노드를 찾아서 상기 샘플링한 형상과 최근접 노드를 연결하여 트리를 확장하는 로봇의 경로 계획 방법.
제7항에 있어서,

상기 확장된 트리의 Goal Score가 상기 경유점의 Goal Score보다 작은 경우 상기 경유점으로 상기 트리를 확장해 나가는 로봇의 경로 계획 방법.
제7항에 있어서,

상기 확장된 트리의 Goal Score가 상기 경유점의 Goal Score보다 계속하여 작지 않은 경우 Goal Function의 변화를 통해 새로운 노드를 찾아 상기 트리를 확장해 나가는 로봇의 경로 계획 방법.
머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하고;

상기 시작점과 상기 목표점 사이에 장애물이 있는 경우, 상기 장애물을 중간점으로 인식하여 경유점을 선정하고;

형상 공간에서 임의의 한 점을 무작위로 샘플링하여 가장 가까운 노드를 찾아 상기 샘플링한 임의의 점과 상기 노드를 연결하고;

상기 노드가 상기 경유점을 경유하여 상기 목표점에 도달하도록 트리를 확장하여 탐색 그래프를 생성하고;

상기 탐색 그래프를 통해 상기 시작점과 상기 목표점을 연결하여 상기 장애물을 회피하는 최적 경로를 생성하되,

상기 경유점을 선정하는 것은,

상기 중간점에서 일정 거리 떨어진 임의의 점들을 선택하여 상기 임의의 점들을 상기 시작점과 상기 목표점에 각각 연결하고, 상기 선택된 임의의 점들 중에서 상기 시작점과의 사이 및 상기 목표점과의 사이에 상기 장애물이 존재하지 않는 임의의 점들을 상기 경유점으로 선정하는 로봇의 경로 계획 방법.
삭제
제10항에 있어서,

상기 중간점은 상기 시작점과 상기 목표점을 잇는 직선에서 일정한 각도 내에 있는 장애물인 로봇의 경로 계획 방법.
삭제
제10항에 있어서,

상기 경유점을 선정하는 것은,

상기 시작점에서 무작위로 샘플링한 형상에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 선정하여 상기 트리를 확장해 나가는 로봇의 경로 계획 방법.
머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점, 상기 시작점과 상기 목표점 사이의 장애물을 인식하는 인식부; 및

상기 머니퓰레이터의 이동 경로를 생성하기 위한 형상 공간을 형성하고, 상기 형상 공간을 바탕으로 상기 장애물을 중간점으로 하는 새로운 경유점을 선정하고, 상기 경유점을 이용하여 상기 시작점과 상기 목표점을 연결하는 최적 경로를 생성하는 경로 계획 생성부를 포함하되,

상기 경로 계획 생성부는 상기 중간점에서 일정 거리 떨어진 임의의 점들을 선택하여 상기 임의의 점들을 상기 시작점과 상기 목표점에 각각 연결하고, 상기 선택된 임의의 점들 중에서 상기 시작점과의 사이 및 상기 목표점과의 사이에 상기 장애물이 존재하지 않는 임의의 점들을 상기 경유점으로 선정하는 로봇의 경로 계획 장치.
제15항에 있어서,

상기 중간점은 상기 시작점과 상기 목표점을 잇는 직선에서 일정한 각도 내에 있는 장애물인 로봇의 경로 계획 장치.
삭제
제15항에 있어서,

상기 경로 계획 생성부는 상기 형상 공간에서 임의의 한 점을 무작위로 샘플링하여 가장 가까운 노드를 찾아 상기 샘플링한 임의의 점과 상기 노드를 연결하고, 상기 노드가 상기 경유점을 경유하여 상기 목표점에 도달하도록 트리를 확장하여 탐색 그래프를 생성하는 로봇의 경로 계획 장치.
제18항에 있어서,

상기 경로 계획 생성부는 상기 트리에서 새로 샘플링하여 얻은 점과 최근접 노드를 연결하여 상기 트리를 확장해 나가는 과정에서 상기 얻은 점에서 가장 가까운 경유점을 거쳐야 하는 목표 경유점으로 선정하는 로봇의 경로 계획 장치.