KR20110047844A

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

Info

Publication number: KR20110047844A
Application number: KR1020090104627A
Authority: KR
Inventors: 김명희; 임복만; 노경식; 임산
Original assignee: 삼성전자주식회사
Priority date: 2009-10-30
Filing date: 2009-10-30
Publication date: 2011-05-09
Also published as: US20110106307A1; US8818555B2; KR101667032B1

Abstract

동적 제약 조건(Dynamic Constraint)을 만족하면서 최적의 경로를 계획하는 로봇의 경로 계획 장치 및 그 방법을 개시한다. 로봇 머니퓰레이터가 작업을 수행하기 위해 이동하는 경로를 생성하는 형상 공간의 초기 시작점에서부터 트리를 확장해 나가면서 최종 목표점까지 이동 경로를 찾는 과정에서 로봇 머니퓰레이터의 동적 제약 조건을 고려하여 최적의 경로를 생성함으로써 모멘텀 및 ZMP 조건을 만족시키는 안정적인 모션을 생성하며, 이를 통해 경로 계획의 성능을 개선하여 빠른 시간 내에 기구학적 제약 조건뿐만이 아니라 동적 제약 조건을 만족시키는 경로를 찾을 수 있게 된다.

Description

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

동적 제약 조건(Dynamic Constraint)을 만족하면서 최적의 경로를 계획하는 로봇의 경로 계획 장치 및 그 방법에 관한 것이다.

일반적으로, 전기적 또는 자기적인 작용을 이용하여 인간의 동작과 닮은 운동을 행하는 기계장치를 로봇이라고 한다. 초기의 로봇은 생산 현장에서의 작업 자동화ㆍ무인화 등을 목적으로 한 머니퓰레이터나 반송 로봇 등의 산업용 로봇으로 인간을 대신하여 위험한 작업이나 단순한 반복 작업, 큰 힘을 필요로 하는 작업을 수행하였으나, 최근에는 인간과 유사한 관절체계를 가지고 인간의 작업 및 생활공간에서 인간과 공존하며 다양한 서비스를 제공하는 인간형 로봇(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 알고리즘은 시간에 따른 동적 제약 조건(Dynamic Constraint) 즉, 속도나 밸런스 등을 고려하지 않고 가장 가까운 노드를 선택하여 트리를 확장해 나가는 경로를 먼저 생성한 후, 오토 밸런서(Auto Balancer)를 이용하여 속도와 밸런스를 조정하였다. 결과적으로 경로 생성 시, 밸런싱을 고려하지 않다보니 오토 밸런서에서 속도와 밸런스 등을 조정할 수 없는 경우, 다시 경로를 찾아야 하므로 경로를 찾는 속도가 그만큼 느려져 최종 경로를 계획하는데 시간이 오래 걸렸다.

이러한 문제를 해결하기 위해 경로 생성 시, 동적 상태를 고려하는 방법으로 선택된 노드의 토크를 계산하여 정해진 토크 제약 조건을 만족시키지 못하면 선택된 노드를 버리는 방법이 제안되었다. 그러나 이 방법 역시 시간을 고려하지 않은 것으로 Dimension이 커지면 수렴 속도가 느려지고 또한 속도나 밸런스 등이 중요한 동적 상태에서는 제약 조건을 만족시키지 못한다는 단점이 있다.

동적 제약 조건(Dynamic Constraint)을 고려한 샘플에 근거하여 경로를 계획함으로써 경로 계획의 성능을 개선한 로봇의 경로 계획 장치 및 그 방법을 개시한다.

본 발명의 일 실시예에 의한 로봇의 경로 계획 방법은, 로봇 머니퓰레이터의 이동 경로를 생성하기 위해 시작점, 목표점, 장애물 정보를 가지는 형상 공간을 형성하고; 형상 공간에서 랜덤하게 샘플링한 임의의 점과 목표 점수가 가장 작은 노 드를 연결하여 장애물 회피 등의 구속 조건을 만족하는 샘플(g_new)을 얻고; 샘플(g_new)을 동적 제약 조건을 고려하여 프로젝션시키고; 프로젝션 샘플(g_projected)을 트리에 추가하여 트리를 확장하고; 트리 확장을 통해 시작점과 목표점을 연결하여 동적 제약 조건을 고려한 경로를 생성하는 것을 특징으로 한다.

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

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

트리 확장은 동적 제약 조건을 만족하도록 확장 구간의 간격과 시간을 설정하는 것을 특징으로 한다.

또한, 본 발명의 일 실시예에 의한 로봇의 경로 계획 방법은, 로봇 머니퓰레이터의 동적 제약 조건을 추정하는 것을 더 포함하고, 동적 제약 조건은 로봇의 무게 중심(COG)과 ZMP 또는 실험적으로 얻은 모멘터 값으로 설정하는 것을 특징으로 한다.

샘플(g_new)을 프로젝션시키는 것은, 로봇의 무게 중심(COG)이 Supporting Polygon안에 있는지 체크하여 샘플(g_new)이 정적 안정성을 만족하는지 판단하는 과정과, 로봇의 ZMP가 Supporting Polygon안에 들어오게 하여 로봇의 동적 안정성을 만족시키는 과정을 포함한다.

로봇의 동적 안정성은 로봇의 무게 중심(COG)과 ZMP가 Supporting Polygon안 에 들어오게 하여 로봇의 밸런스를 유지하는 것을 특징으로 한다.

그리고, 본 발명의 일 실시예에 의한 로봇의 경로 계획 방법은, 로봇 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하고; 로봇 머니퓰레이터의 동적 제약 조건을 추정하고; 시작점에서 목표점에 이르기까지 장애물 회피 등의 구속 조건을 만족하는 경로를 탐색하고; 경로 탐색 시, 동적 제약 조건을 만족하는 경로를 생성하는 것을 특징으로 한다.

경로를 탐색하는 것은, 시작점과 목표점을 가지는 형상 공간에서 랜덤하게 샘플링한 임의의 점과 목표 점수가 가장 작은 노드를 연결하여 구속 조건을 만족하는 샘플(g_new)을 얻고; 샘플(g_new)을 트리에 추가하여 샘플(g_new)이 목표점에 도달할 때까지 트리를 확장하고; 트리 확장을 통해 시작점과 목표점을 연결하는 경로를 탐색하는 것을 특징으로 한다.

경로를 생성하는 것은, 샘플(g_new)을 동적 제약 조건을 고려하여 프로젝션시키고; 프로젝션 샘플(g_projected)을 트리에 추가하여 트리를 확장하는 탐색 그래프를 생성하고; 탐색 그래프를 통해 시작점과 목표점을 연결하여 동적 제약 조건을 고려한 경로를 생성하는 것을 특징으로 한다.

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

경로 계획 생성부는 형상 공간에서 랜덤하게 샘플링한 임의의 점과 목표 점수가 가장 작은 노드를 연결하여 장애물 회피 등의 구속 조건을 만족하는 샘플(g_new)을 얻고, 샘플(g_new)을 동적 제약 조건을 고려하여 프로젝션시키고, 프로젝션 샘플(g_projected)을 트리에 추가하여 트리를 확장하는 것을 특징으로 한다.

경로 계획 생성부는 로봇의 무게 중심(COG)이 Supporting Polygon안에 있는지 체크하여 샘플(g_new)이 정적 안정성을 만족하는지 판단하고, 샘플(g_new)이 정적 안정성을 만족하면 로봇의 동적 안정성을 만족시키도록 샘플(g_new)을 프로젝션시키는 것을 특징으로 한다.

개시된 로봇의 경로 계획 장치 및 그 방법에 의하면, 로봇 머니퓰레이터가 작업을 수행하는 형상 공간에서 초기 시작점에서부터 트리를 확장해 나가면서 최종 목표점까지 이동 경로를 찾는 과정에서 로봇 머니퓰레이터의 동적 제약 조건을 고려하여 최적의 경로를 생성함으로써 모멘텀 및 ZMP 조건을 만족시키는 안정적인 모션을 생성하며, 이를 통해 경로 계획의 성능을 개선하여 빠른 시간 내에 기구학적 제약 조건뿐만이 아니라 동적 제약 조건을 만족시키는 경로를 찾을 수 있게 된다.

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

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

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

참조부호에서 R과 L은 로봇(100)의 오른쪽(Right)과 왼쪽(Left)을 나타내고, COG(Center Of Gravity)는 로봇(100)의 무게 중심을 나타내며, ZMP(Zero Moment Position)는 로봇(100)의 발(111R, 111L)과 지면의 접촉면에서 롤 방향(roll; 로봇의 보행 진행방향인 x축 방향)과 피치 방향(pitch; 로봇의 좌우 보폭 방향인 y축 방향)의 모멘트가 0이 되는 점을 나타낸다.

도 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)는 로봇(100)의 발목, 무릎, 힙에 해당하는 부분이 회전할 수 있도록 발목 관절(112R, 112L), 무릎 관절(113R, 113L), 힙 관절(114R, 114L)을 각각 구비하고, 힙 관절(114R, 114L)은 몸통(120) 하부의 양 쪽 끝에 위치한다.

각 다리(110R, 110L)의 발목 관절(112R, 112L)은 x축(roll axis)과 y축(pitch axis)로 움직임이 가능하고, 무릎 관절(113R, 113L)은 y축(pitch axis)로 움직임이 가능하며, 힙 관절(114R, 114L)은 x축(roll axis)과 y축(pitch axis), z축(yaw axis)로 움직임이 가능하다.

또한, 두 개의 다리(110R, 110L)에는 힙 관절(114R, 114L)과 무릎 관절(113R, 113L)을 연결하는 대퇴 링크(115R, 115L)와, 무릎 관절(113R, 113L)과 발목 관절(112R, 112L)을 연결하는 하퇴 링크(116R, 116L)를 각각 포함하여 각 관절((112R, 112L), (113R, 113L), (114, 114L))의 움직임에 따라 일정 수준의 자유도를 가지고 보행이 가능하도록 한다.

또한, 두 개의 발(111R, 111L)과 발목 관절(112R, 112L) 사이에는 힘/토크 측정센서(117R, 117L;Force and Torque sensor;이하 F/T센서라 한다)가 각각 설치되어 발(111R, 111L)로부터 전달되는 힘의 3방향 성분(Fx, Fy, Fz)과 모멘트의 3방 향 성분(Mx, My, Mz)을 측정하여 ZMP 정보를 제공한다.

그리고, 두 개의 다리(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)에서 경로 계획을 생성하는 방법은 크게 공간 형성 단계와, 그래프 탐색 단계와, 경로 생성 단계를 포함한다.

공간 형성 단계는 장애물에 대한 충돌 회피 등의 구속 조건을 만족하면서 경로를 생성하기 위한 형상 공간(Configuration Space; 이하, 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)의 이동을 제어한다.

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

도 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)에 이르기까지 장애물에 대한 충돌 회피 등의 구속 조건을 만족하게 하는 경로를 찾는 알고리즘이다. 여기서 트리(T)의 확장(extend)은 형상 공간(C-Space)에서 랜덤하게 샘플링한 형상(g)에서 트리(T)에 속하는 노드들 중 가장 가까운 노드를 선택하고, 가상선을 이은 후 가상선 안의 샘플(g_new)이 구속 조건을 만족하면 그 샘플(g_new)을 트리에 포함시키고, 샘플(g_new)이 목표점(G)에 도달할 때까지 계속 트리(T)를 확장해 나간다. 이러한 RRT 알고리즘을 이용한 본 발명의 경로 계획 생성을 도 7 내지 도 16을 참조하여 상세히 설명한다.

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

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

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

이러한 RRT 알고리즘의 트리 확장 시, 동적 제약 조건(Dynamic Constraint) 즉, 속도나 밸런스 등을 고려하지 않은 경우, 작업 모션을 빠르고 안정되게 생성할 수 없다.

따라서, 본 발명의 일 실시예는 장애물(K)에 대한 충돌 회피 등의 구속 조건을 만족시키는 기존의 RRT 알고리즘에서 동적 제약 조건(모멘텀 제약 조건)을 만족시키는 RRT 경로 계획 알고리즘을 제시한다. 이를 위해 경로 계획 시, 모멘텀을 만족하도록 샘플(g_new)을 프로젝션(projection)시키는 방법을 사용한다. 프로젝션(projection) 방법을 사용하면 어떤 상태의 모멘텀 리미트가 주어지더라도 그 상태에 맞추어 RRT 경로를 계획할 수 있으며, 또한 모멘트를 이용하여 밸런싱 제약 조건을 줄 수 있고 이 밸런싱 제약 조건을 만족시키는 모션은 빠른 속도로 움직일 수 있게 된다.

도 17은 본 발명의 일 실시예에 의한 형상 공간에서 동적 제약 조건을 고려 한 샘플의 프로젝션 과정을 시각화한 도면이다.

초기 시작점(S), 최종 목표점(G), 장애물(K)이 표시된 형상 공간(C-Space)에서 임의의 점(g)을 랜덤하게 샘플링하고(도 7 참조), 랜덤하게 샘플링된 점(g)과 Goal Score가 가장 작은 노드를 점선의 가상선으로 연결한 후 일정 간격으로 떨어진 새로운 샘플(g_new)을 생성한다(도 8 참조).

이 새로운 샘플(g_new)이 동적 제약 조건을 만족하도록 프로젝션(projection)시켜 프로젝션 샘플(g_projected)을 생성하는데, 동적 제약 조건을 만족하도록 샘플(g_new)을 프로젝션시키는 방법은 다음과 같다.

먼저, 샘플(g_new)이 정적 안정성(Static Stability)을 만족하는지 체크하여 정적 안정성을 만족하면 샘플(g_new)을 선택한다. 이때 정적 안정성을 체크하는 방법은 로봇(100)의 무게 중심(COG)을 계산하여 무게 중심(COG)이 로봇(100)의 Supporting Polygon안에 있는지 체크하는 것이다. Supporting Polygon은 로봇(100)이 바닥에 지지하고 있는 발(111R, 111L;구체적으로, 발바닥)로 계산한다.

무게 중심(COG)이 로봇(100)의 Supporting Polygon안에 있으면, 로봇(100)의 동적 안정성(Dynamic Stability)을 만족시키도록 샘플(g_new)을 프로젝션시킨다. 로봇(100)의 동적 안정성을 만족시키는 방법은 ZMP가 Supporting Polygon안에 들어오게 하는 것이다.

ZMP는 로봇(100)의 발(111R, 111L;구체적으로, 발바닥)과 지면의 접촉면에 서 롤 축(x축 방향)과 피치 축(y축 방향)의 모멘트가 0이 되는 점을 나타낸다.

도 18은 본 발명의 일 실시예에 의한 로봇의 ZMP 궤적을 나타낸 도면으로서, 로봇(100)의 보행진행 방향(x축 방향)과 좌우 보폭 방향(y축 방향)에 대한 ZMP 궤적을 x-y 평면상에 나타낸 것이다.

도 18에서, B는 보폭의 절반을 나타낸다.

도 18에서 알 수 있듯이, 로봇(100)이 안정된 보행 모션을 수행하기 위해서는 지면에서 모멘트의 합이 0이 되는 지점 즉, 양 발(111R, 111L)을 내딛을 지점들을 미리 결정하여야 하며, 또한 양발 지지 위상(Double Support Phase;이하 DSP라 한다)/한발 지지 위상(Single Support Phase;이하 SSP라 한다)과 같은 지지상태를 미리 결정하는 ZMP 궤적을 설정해야 한다.

로봇(100)의 발(111R, 111L)을 내딛을 지점들은 일반적으로 주기함수로 기술되며, 지지상태들은 ZMP를 옮기는데 이용된다. SSP 상태에서 이동다리가 걸음을 만드는 동안에 ZMP가 지지다리의 발바닥 내에 머물러 있어야 한다. DSP 상태에서는 ZMP가 지지다리 발바닥 내부에서 이동다리 발바닥 내부로 빠르게 옮겨져야 한다. 연속적이고 안정된 로봇(100)의 보행을 만들기 위해서는 이러한 과정들이 반복되어야 한다.

이와 같이, ZMP가 Supporting Polygon안에 들어오게 하려면, 모멘텀이 주어진 기준 모멘텀을 만족해야 하므로 로봇(100)의 선형 모멘텀(linear momentum)과 각 모멘텀(angular momentum)을 아래의 [식 2]를 이용하여 계산한다.

[식 2]

[식 2]에서, P=[P_x, P_y, P_z]^T는 로봇(100)의 선형 모멘텀(linear momentum)이고, L=[L_x, L_y, L_z]^T는 로봇(100)의 각 모멘텀(angular momentum)이다.

[식 2]의 첫 번째 항에서 m은 mass이고, E는 (3X3) 단위 행렬(identity matrix)이며, I는 (3X3) 관성 행렬(Inertia matrix)이고, B는 Base of mass이며, C는 Center of mass이고, r_b→c는 Base로부터 Center of mass의 위치 벡터(position vector)이며, ν _B 와 w _B 는 Base의 속도와 각속도이다.

[식 2]의 두 번째 항은 제약 조건(constrainted)이 주어진 조인트(예를 들어, 다리나 하나의 팔)로, 주어진 입력을 따라야 하는 조인트이다. 제약 조건이 주어진 조인트의 모멘텀은 각 조인트의 정보가 아니라 발(111R, 111L) 또는 핸드(131R, 131L)의 구속 조건을 알고 있다면 계산할 수 있다.

[식 2]의 세 번째 항은 주어진 모멘텀을 만족시킬 수 있도록 자유롭게 움직일 수 있는 조인트(예를 들어, 하나의 팔이나 머리, 허리)이다.

이러한 방법으로 선형 모멘텀(linear momentum)과 각 모멘텀(angular momentum)을 계산하기 위해서는 속도와 관성 정보가 필요하나, RRT의 탐색은 형상 공간(C-Space)에서 행해지기 때문에 속도 정보는 가지고 있지 않다. 하지만 두 노드 사이의 시간 간격(△t)이 충분히 작다면 두 노드 사이를 선형으로 가정할 수 있다. 따라서 두 노드 연결 시, 연결하는 거리의 최대 값이 정해져 있다면 속도는 두 노드의 상태를 차분(difference)하여 근사한 값으로 얻을 수 있다. 즉, 도 17의 가장 가까운 노드와 현재 노드를 이용하여 제약 조건이 주어지는 조인트의 속도를 계산한 후, [식 2]를 이용하여 자유롭게 움직일 수 있는 조인트의 각속도를 구한다. 이후 다시 선형 근사화를 하여 자유롭게 움직일 수 있는 조인트를 제약 조건을 만족시키도록 프로젝션시킨다.

이러한 과정을 통하여 샘플링 후 확장된 트리를 모멘텀을 만족시키는 상태로 프로젝션시킬 수 있다.

한편, 로봇(100)의 밸런스를 유지하기 위해서는 COG뿐만 아니라 ZMP역시 Supporting Polygon안에 존재하여야 한다. 로봇(100)의 발(111R, 111L)이 움직이는 경우 로봇(100)의 모션은 로봇(100)의 발(111R, 111L)이 움직이지 않을 때와 비교하여 더 동적이며 Supporting Polygon 역시 달라진다. 따라서 ZMP를 고려해야 하는 필요성이 더 커진다.

현재 COG의 위치를 알고, 로봇(100)의 정보로부터 최대 힘(Force)을 근사화시킬 수 있다면, ZMP를 만족시키는 모멘텀의 범위를 찾아낼 수 있다. 또한 두 노드 사이의 시간 간격(△t)이 충분히 작다면 모멘텀의 최소, 최대 값을 알 수 있다.

이러한 동적 제약 조건을 고려하여 경로를 생성하는 과정을 도 19를 참조하여 설명한다.

도 19는 본 발명의 일 실시예에 의한 로봇에서 동적 제약 조건을 고려한 샘플에 근거하여 경로를 계획하는 방법을 나타낸 동작 순서도이다.

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

또한, 경로 계획 생성부(210)는 형상 공간(C-Space)상에서 동적 제약 조건을 추정한다(310). 동적 제약 조건을 추정하기 위해서는 두 노드 사이를 연결하는 거리를 속도 관계를 고려하여 연결한다. 즉, 최대 속도가 1rad/sec이고, 두 노드 사이의 시간 스텝이 0.05sec라면 각 노드의 최대 연결 스텝의 간격은 0.05rad이 되도록 연결한다.

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

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

이 새로운 샘플(g_new)이 동적 제약 조건을 만족하도록 프로젝션(projection)시켜 프로젝션 샘플(g_projected)을 얻는데(350; 도 17 참조), 동적 제약 조건을 만족하도록 샘플(g_new)을 프로젝션시키는 방법은 도 20을 참조하여 이후에 설명한다.

이 프로젝션 샘플(g_projected)에 대하여 동적 제약 조건을 체크하여 동적 제약 조건을 만족하면 프로젝션 샘플(g_projected)을 트리(T1')에 추가한다(360; 도 17 참조). 이러한 방식으로 프로젝션 샘플(g_projected)을 일정하게 확장될 때까지 반복해서 트리 T1'를 확장한다.

프로젝션 샘플(g_projected)을 트리(T1')에 추가한 후 프로젝션 샘플(g_projected)이 목표점에 도달하였는지를 알기 위해 프로젝션 샘플(g_projected)의 Goal Score와 목표점의 Goal Score를 비교하여 프로젝션 샘플(g_projected)의 Goal Score가 목표점의 Goal Score보다 작은지를 판단한다(370).

단계 370의 판단 결과, 프로젝션 샘플(g_projected)의 Goal Score가 목표점의 Goal Score보다 작으면 프로젝션 샘플(g_projected)이 목표점에 도달한 것으로 판단하여 트리 상의 여러 노드들로부터 로봇(100)의 머니퓰레이터(130)가 이동할 경로를 생성한다(380).

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

도 20은 본 발명의 일 실시예에 의한 로봇에서 동적 제약 조건을 고려한 샘플의 프로젝션 방법을 나타낸 동작 순서도이다.

도 20에서, 로봇(100)의 무게 중심(COG)이 Supporting Polygon안에 있는지 판단하여(351), 로봇(100)의 무게 중심(COG)이 Supporting Polygon안에 있지 않으면 단계 320로 진행하여 이후의 동작을 계속한다.

단계 351의 판단 결과, 로봇(100)의 무게 중심(COG)이 Supporting Polygon안에 있으면 현재 노드와 선택 노드를 이용하여 제약 조건이 주어지는 조인트의 속도를 계산한다(352).

이어서, 모멘텀 제약 조건을 이용하여 프로젝션 각을 얻고(353), ZMP 구속 조건에 근거하여 각 모멘텀(angular momentum)을 얻는다(354).

이러한 방법으로 구해진 현재 모멘텀이 ZMP 조건을 만족하는가 판단하여(355), 현재 모멘텀이 ZMP 조건을 만족하지 않으면 단계 320로 진행하여 이후의 동작을 계속한다.

단계 355의 판단 결과, 현재 모멘텀이 ZMP 조건을 만족하면 다른 제약 조건을 만족하는가 판단하여(356), 다른 제약 조건을 만족하지 않으면 단계 320로 진행하여 이후의 동작을 계속한다. 다른 제약 조건이란 충돌 또는 작업 공간 상의 원하는 제약 조건(예를 들어, z평면을 움직이는 것) 등이 있을 수 있다.

단계 356의 판단 결과, 다른 제약 조건을 만족하면 단계 360로 진행하여 트리에 프로젝션 샘플(g_projected)을 추가하는 트리 확장을 수행한다.

본 발명의 일 실시예를 통하여 모멘텀 및 ZMP 조건을 만족시키는 안정적인 모션을 빠르게 생성할 수 있다.

도 21 내지 도 24는 동적 제약 조건을 고려하지 않은 경우와 고려한 경우에 있어서, ZMP 위치를 나타낸 시뮬레이션 그래프이다.

도 21은 동적 제약 조건을 고려하지 않은 경우, X성분의 ZMP 위치를 나타낸 시뮬레이션 그래프이고, 도 22는 동적 제약 조건을 고려하지 않은 경우, Y성분의 ZMP 위치를 나타낸 시뮬레이션 그래프이다.

도 21 및 도 22에서 보듯이, X성분의 ZMP 위치는 Supporting Polygon의 Upper 및 Lower Bound안에 들어가나, Y성분의 ZMP 위치는 Supporting Polygon의 Upper 및 Lower Bound를 벗어나는 것(도 22의 점선 표기)을 알 수 있다.

도 23은 동적 제약 조건을 고려한 경우, X성분의 ZMP 위치를 나타낸 시뮬레이션 그래프이고, 도 24는 동적 제약 조건을 고려한 경우, Y성분의 ZMP 위치를 나타낸 시뮬레이션 그래프이다.

도 23 및 도 24에서 보듯이, X성분과 Y성분의 ZMP 위치는 Supporting Polygon의 Upper 및 Lower Bound안에 들어가면서 제약 조건을 만족시키며, 또한 동작 중 발이 움직이지 않아 Supporting Polygon도 초기 형상과 거의 동일함을 알 수 있다.

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

도 17은 본 발명의 일 실시예에 의한 형상 공간에서 동적 제약 조건을 고려한 샘플의 프로젝션 과정을 시각화한 도면이다.

도 18은 본 발명의 일 실시예에 의한 로봇의 ZMP 궤적을 나타낸 도면이다.

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

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

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

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

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

240 : 구동부

Claims

로봇 머니퓰레이터의 이동 경로를 생성하기 위해 시작점, 목표점, 장애물 정보를 가지는 형상 공간을 형성하고;

상기 형상 공간에서 랜덤하게 샘플링한 임의의 점과 목표 점수가 가장 작은 노드를 연결하여 상기 장애물 회피 등의 구속 조건을 만족하는 샘플(g_new)을 얻고;

상기 샘플(g_new)을 동적 제약 조건을 고려하여 프로젝션시키고;

상기 프로젝션 샘플(g_projected)을 트리에 추가하여 상기 트리를 확장하고;

상기 트리 확장을 통해 상기 시작점과 목표점을 연결하여 상기 동적 제약 조건을 고려한 경로를 생성하는 로봇의 경로 계획 방법.
제1항에 있어서,

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

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

상기 트리 확장은 상기 동적 제약 조건을 만족하도록 상기 확장 구간의 간격과 시간을 설정하는 로봇의 경로 계획 방법.
제1항에 있어서,

상기 로봇 머니퓰레이터의 동적 제약 조건을 추정하는 것을 더 포함하고,

상기 동적 제약 조건은 로봇의 무게 중심(COG)과 ZMP 또는 실험적으로 얻은 모멘터 값으로 설정하는 로봇의 경로 계획 방법.
제5항에 있어서,

상기 샘플(g_new)을 프로젝션시키는 것은,

상기 로봇의 무게 중심(COG)이 Supporting Polygon안에 있는지 체크하여 상기 샘플(g_new)이 정적 안정성을 만족하는지 판단하는 과정과,

상기 로봇의 ZMP가 Supporting Polygon안에 들어오게 하여 상기 로봇의 동적 안정성을 만족시키는 과정을 포함하는 로봇의 경로 계획 방법.
제6항에 있어서,

상기 로봇의 동적 안정성은 상기 로봇의 무게 중심(COG)과 ZMP가 Supporting Polygon안에 들어오게 하여 상기 로봇의 밸런스를 유지하는 로봇의 경로 계획 방법.
로봇 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하고;

상기 로봇 머니퓰레이터의 동적 제약 조건을 추정하고;

상기 시작점에서 상기 목표점에 이르기까지 장애물 회피 등의 구속 조건을 만족하는 경로를 탐색하고;

상기 경로 탐색 시, 상기 동적 제약 조건을 만족하는 경로를 생성하는 로봇의 경로 계획 방법.
제8항에 있어서,

상기 동적 제약 조건은 로봇의 무게 중심(COG)과 ZMP 또는 실험적으로 얻은 모멘터 값으로 설정하는 로봇의 경로 계획 방법.
제8항에 있어서,

상기 경로를 탐색하는 것은,

상기 시작점과 목표점을 가지는 형상 공간에서 랜덤하게 샘플링한 임의의 점과 목표 점수가 가장 작은 노드를 연결하여 상기 구속 조건을 만족하는 샘플(g_new)을 얻고;

상기 샘플(g_new)을 트리에 추가하여 상기 샘플(g_new)이 상기 목표점에 도달할 때까지 트리를 확장하고;

상기 트리 확장을 통해 상기 시작점과 목표점을 연결하는 경로를 탐색하는 로봇의 경로 계획 방법.
제10항에 있어서,

상기 경로를 생성하는 것은,

상기 샘플(g_new)을 상기 동적 제약 조건을 고려하여 프로젝션시키고;

상기 프로젝션 샘플(g_projected)을 트리에 추가하여 상기 트리를 확장하는 탐색 그래프를 생성하고;

상기 탐색 그래프를 통해 상기 시작점과 목표점을 연결하여 상기 동적 제약 조건을 고려한 경로를 생성하는 로봇의 경로 계획 방법.
제11항에 있어서,

상기 샘플(g_new)을 프로젝션시키는 것은,

상기 로봇의 무게 중심(COG)이 Supporting Polygon안에 있는지 체크하여 상기 샘플(g_new)이 정적 안정성을 만족하는지 판단하는 과정과,

상기 로봇의 ZMP가 Supporting Polygon안에 들어오게 하여 상기 로봇의 동적 안정성을 만족시키는 과정을 포함하는 로봇의 경로 계획 방법.
제12항에 있어서,

상기 로봇의 동적 안정성은 상기 로봇의 무게 중심(COG)과 ZMP가 Supporting Polygon안에 들어오게 하여 상기 로봇의 밸런스를 유지하는 로봇의 경로 계획 방법.
로봇 머니퓰레이터의 초기 형상과 목표 형상에 각각 해당하는 시작점과 목표점을 인식하는 인식부;

상기 로봇 머니퓰레이터의 이동 경로를 생성하기 위한 형상 공간을 형성하고, 상기 형상 공간에서 상기 로봇 머니퓰레이터의 동적 제약 조건을 추정하고, 상기 동적 제약 조건을 고려하여 상기 시작점과 목표점을 연결하는 경로를 생성하는 경로 계획 생성부를 포함하는 로봇의 경로 계획 장치.
제14항에 있어서,

상기 경로 계획 생성부는 상기 형상 공간에서 랜덤하게 샘플링한 임의의 점과 목표 점수가 가장 작은 노드를 연결하여 장애물 회피 등의 구속 조건을 만족하는 샘플(g_new)을 얻고, 상기 샘플(g_new)을 동적 제약 조건을 고려하여 프로젝션시키고, 상기 프로젝션 샘플(g_projected)을 트리에 추가하여 상기 트리를 확장 하는 로봇의 경로 계획 장치.
제15항에 있어서,

상기 동적 제약 조건은 로봇의 무게 중심(COG)과 ZMP 또는 실험적으로 얻은 모멘터 값으로 설정하는 로봇의 경로 계획 장치.
제15항에 있어서,

상기 경로 계획 생성부는 상기 로봇의 무게 중심(COG)이 Supporting Polygon안에 있는지 체크하여 상기 샘플(g_new)이 정적 안정성을 만족하는지 판단하고, 상기 샘플(g_new)이 정적 안정성을 만족하면 상기 로봇의 동적 안정성을 만족시키도록 상기 샘플(g_new)을 프로젝션시키는 로봇의 경로 계획 장치.
제17항에 있어서,

상기 로봇의 동적 안정성은 상기 로봇의 무게 중심(COG)과 ZMP가 Supporting Polygon안에 들어오게 하여 상기 로봇의 밸런스를 유지하는 로봇의 경로 계획 장치.