KR20080088513A - 사영변환 수렴연산 처리 방법 - Google Patents

Abstract

2개의 카메라의 촬상화상 내의 물체의 평탄부를 관측하기 위하여, 그들 촬상화상 사이의 사영변환을 이용한 수렴연산 처리를 행하는 방법에 있어서, 그 수렴연산 처리의 수렴성을 확보하면서, 연산부하를 경감한다.

수렴연산 처리에서의 사영변환 행렬의 소정 종류의 패러미터(n(i), d(i))의 초기값(n0(i), d0(i))에 의해 규정되는 평면(πa(i))이 관측대상의 물체의 평탄부를 포함하는 실제의 평면에 대하여 경사진 평면으로 되는 것을 이 초기값(n0(i), d0(i))이 충족시켜야 할 제약조건으로 하고, 이 초기값(n0(i), d0(i))을 이 제약조건을 충족시키는 값으로 설정한다.

카메라, 촬상화상, 변환화상, 사영변환 행렬, 수렴연산 처리

Description

사영변환 수렴연산 처리 방법{PROJECTIVE TRANSFORMATION CONVERGENCE CALCULATION METHOD}

본 발명은 2개의 카메라의 촬상화상 간의 사영변환을 이용하여, 이 촬상화상 중의 물체의 평탄부(평면 영역)를 관측하기 위한 수렴연산 처리 방법에 관한 것이다.

2개의 촬상 카메라의 촬상화상 중에, 평탄부를 갖는 물체가 존재하는 경우, 일방의 촬상화상(참조화상)을, 어떤 적당한 사영변환 행렬에 의해 변환했을 때, 그 변환 후의 화상 내에서의 물체의 평탄부의 화상을, 타방의 촬상화상(기준화상) 내에어서의 물체의 평탄부의 화상에 합치시킬 수 있다. 이 경우, 이러한 화상변환을 행할 수 있는 사영변환 행렬은 물체의 평탄부를 포함하는 평면의 공간적인 자세(예를 들면 법선방향)와 위치(예를 들면 카메라의 광학 중심으로부터의 거리)에 의존한다. 바꾸어 말하면, 이 사영변환 행렬은 촬상화상 내의 물체의 평탄부를 포함하는 평면의 공간적인 위치 및 자세를 규정한다.

이것을 이용하여, 2개의 촬상화상 중의 물체의 평탄부를 관측하는(평탄부의 자세 등을 측정함) 기술로서, 종래, 예를 들면, 「스테레오 동화상을 이용한 평면 영역 추출에 의한 장해물 검출」/세키 아키히토, 오쿠토미 마사토시/정보처리학회 논문지: 컴퓨터 비전과 이미지 미디어/vol.45, No.SIG13(CVIM10), Dec.2004(문헌 1)에 개시되는 기술이 알려져 있다. 이 기술에서는, 차량을 주행시키는 도로면으로서의 평면을 추출하기 위하여, 2개의 차량 탑재 카메라의 촬상화상 간의 사영변환 행렬의 적당한 템플릿를 복수 준비해 두고, 그것들의 템플릿를 사영변환 행렬의 초기값으로 하는 수렴연산 처리에 의해, 상기한 화상변환을 행할 수 있는 사영변환 행렬을 추정한다. 또한, 사영변환 행렬의 초기값으로서의 템플릿를 복수 준비한다고 하는 것은, 바꾸어 말하면, 이 사영변환 행렬에 의해 규정되는 평면을 나타내는 패러미터의 초기값을 복수 준비한다고 하는 것을 의미한다.

이 문헌 1에 개시되는 기술은, 수렴연산 처리에서의 사영변환 행렬의 초기값, 또는, 측정하려고 하는 평면의 초기값을 복수 준비해 둠으로써, 측정하려고 하는 평면에 대응하는 사영변환 행렬의 추정 처리의 로버스트성을 높이고, 또한 그 추정 처리의 발산이나 부적절한 사영변환 행렬이 추정되는(실제의 평면과는 상이한 평면이 추정되는) 것을 방지하고자 하는 것이다.

그렇지만, 비특허문헌 1의 기술에서는, 각 초기값에 의해 규정되는 평면과, 측정하려고 하는 실제의 평면과의 위치의 오차(카메라의 광학 중심으로부터의 거리의 오차)가 충분히 작은 것을 전제로 하고 있다. 이 때문에, 그것들의 위치의 오차가 비교적 큰 경우에는, 사영변환 행렬이 충분히 수렴될 때까지 필요한 반복연산의 회수가 대단히 많아지거나, 또는, 사영변환 행렬을 수렴시킬 수 없게 될 우려가 있었다.

또, 복수의 초기값에 대하여 수렴연산 처리를 행할 필요가 있으므로, 최종적으로 사영변환 행렬을 확정할 때까지의 연산부하가 과대한 것으로 되기 쉽다고 하는 문제가 있었다.

본 발명은 이러한 배경을 감안하여 이루어진 것으로, 2개의 카메라의 촬상화상 내의 물체의 평탄부를 관측하기 위하여, 그들 촬상화상 간의 사영변환을 이용한 수렴연산 처리를 행하는 사영변환 수렴연산 처리 방법에 있어서, 그 수렴연산 처리의 수렴성을 확보하면서, 연산부하를 경감할 수 있는 방법을 제공하는 것을 목적으로 한다.

본 발명의 사영변환 수렴연산 처리 방법은, 이러한 목적을 달성하기 위하여, 2개의 카메라의 촬상화상 중에 포착되어 있는 물체의 평탄부를 관측하기 위하여, 상기 2개의 카메라 중 일방의 카메라의 촬상화상 내의 물체의 화상을 양 카메라 간 의 사영변환 행렬에 의해 변환하여 이루어지는 변환화상과, 타방의 카메라의 촬상화상 내의 물체의 화상을 비교하면서, 이 사영변환 행렬의 소정 종류의 패러미터의 값에 의해 규정되는 평면을 상기 물체의 평탄부를 포함하는 평면에 수렴시키도록, 이 패러미터의 값을 갱신해 가는 수렴연산 처리를 실행하는 사영변환 수렴연산 처리 방법에 있어서, 상기 수렴연산 처리에서의 상기 패러미터의 초기값에 의해 규정되는 평면이 상기 물체의 평탄부를 포함하는 실제의 평면에 대하여 경사진 평면으로 되는 것을 이 초기값이 충족시킬 제약조건으로 하여, 이 초기값을 이 제약조건을 충족시키는 값으로 설정한 것을 특징으로 한다.

여기에서, 본원 발명자는 사영변환 수렴연산 처리 방법에 관하여 다양한 검토를 행한 결과, 다음의 것을 알았다. 즉, 구체적인 예는 후술하지만, 상기 수렴연산 처리에서 갱신해 가는 사영변환 행렬의 패러미터의 초기값은 그것에 의해 규정되는 평면(이하, 초기 평면이라고 하는 경우가 있음)이, 실제의 물체의 평탄부를 포함하는 평면(이하, 실평면이라고 하는 경우가 있음)과 평행하게 되는 경우에는, 초기 평면과 실평면의 공간적인 위치의 오차가 커짐에 따라, 수렴연산 처리의 반복 연산의 회수가 급격하게 증대하는 경향이 있다. 또한, 초기 평면과 실평면의 공간적인 위치의 오차가 어느 정도 커지면, 상기 패러미터의 값을 어떤 값에 수렴시킬 수 없게 된다. 즉, 사영변환 행렬의 패러미터의 초기값을 초기 평면과 실평면이 평행하게 되는 것과 같은 값으로 설정한 경우에는, 그들 초기 평면과 실평면의 공간적인 위치도 큰 오차가 생기지 않도록, 이 초기값을 설정하지 않으면, 수렴연산 처리의 연산 처리 부하가 급격하게 증대하고, 또는, 패러미터의 값을 수렴시킬 수 없게 된다.

한편, 초기 평면이, 실평면에 대하여 어느 정도 경사지도록, 사영변환 행렬의 패러미터의 초기값을 설정한 경우에는, 수렴연산 처리의 반복 연산의 회수는 그들 초기 평면과 실평면의 공간적인 위치의 오차가 비교적 커도, 급격하게 증가하지는 않아, 비교적 적은 반복 연산의 회수로 상기 패러미터의 값을 수렴시킬 수 있다.

그래서, 본 발명은, 상기한 바와 같이, 상기 수렴연산 처리에서의 상기 패러미터의 초기값에 의해 규정되는 평면(초기 평면)이 상기 물체의 평탄부를 포함하는 실제의 평면(실평면)에 대하여 경사진 평면이 되는 것을 이 초기값이 충족시킬 제약조건으로 하고, 이 초기값을 이 제약조건을 충족시키는 값으로 설정했다.

이와 같이, 상기 패러미터의 초기값을 설정함으로써, 이 초기값에 의해 규정되는 초기 평면이 실평면에 대하여 경사진 평면으로 되므로, 그것들의 평면의 위치의 오차가 비교적 커도, 수렴연산 처리의 수렴성을 확보하면서, 연산부하를 경감하는 것이 가능하게 된다.

이러한 본 발명에서는, 상기 패러미터의 초기값에 의해 규정되는 평면은, 예를 들면, 이 평면의 수평면에 대한 경사각도가 상기 물체의 평탄부를 포함하는 실제의 평면의 수평면에 대한 경사각도가 취할 수 있는 값의 범위로부터 일탈하도록 미리 정해진 소정 각도가 되는 평면이다.

즉, 일반적으로, 관측 대상으로 하는 물체의 평탄부를 포함하는 평면의 경사각도(수평면에 대한 경사각도)가 취할 수 있는 값은 어떤 범위 내의 값으로 된다. 따라서, 상기 소정 각도를, 당해 범위를 일탈한 값에 정해 두고, 상기 초기 평면의 경사각도가 이 소정 각도로 되도록 이 초기 평면을 규정하는 상기 패러미터의 초기값을 설정함으로써 상기 제약조건을 확실하게 충족시키는 초기값을 설정할 수 있다.

(발명을 실시하기 위한 최선의 형태)

본 발명의 1실시형태를 도 1∼도 11을 참조하여 설명한다. 또한, 본 실시형태는 본 발명의 사영변환 수렴연산 처리를 다리식 이동 로봇을 이동시키는 바닥의 바닥면 형상을 추정하는 처리에 적용한 경우의 실시형태이다. 따라서, 그 바닥이 본 발명에서의 물체에 상당하는 것이다.

우선, 도 1및 도 2를 참조하여 본 실시형태에서의 다리식 이동 로봇의 개요구성을 설명한다. 도 1은 본 실시형태에서의 다리식 이동 로봇으로서의 2족 보행 로봇의 외관 사시도, 도 2는 이 로봇의 제어 시스템의 개요를 도시하는 블럭도이다.

이 2족 보행 로봇(1)은 인간형 로봇이며, 상체(기체)(3), 좌우 1쌍의 다리체(5L, 5R), 좌우 1쌍의 팔체(7L, 7R), 및 머리부(9)를 구비한다. 또한, 본 실시형태의 설명에서는, 부호 「L」, 「R」은 각각 로봇(1)의 전방을 향하여 좌측의 요소, 우측 요소를 나타내는 부호로서 사용한다. 단, 좌우를 특별히 구별할 필요가 없을 때에는, 부호 「L」, 「R」을 생략한다.

다리체(5L, 5R)는 서로 동일한 구조의 것이며, 각각 상체(3)의 하단부의 왼 쪽, 오른쪽으로부터 고관절(11)을 통하여 연장하여 설치되어 있다. 그리고, 각 다리체(5)는 그 길이 방향의 중간부에 무릎관절(13)을 갖는다. 또, 각 다리체(5)의 선단부(하단부)에는, 발목관절(15)을 통하여 접지부로서의 족평(足平)(17)이 장착되어 있다. 본 실시형태에서는, 각 다리체(5)의 고관절(11), 무릎관절(13), 및 발목관절(15)은, 각각 3자유도, 1자유도, 2자유도를 갖는다. 따라서, 족평(17)은 상체(3)에 대하여 6자유도를 갖는다.

팔체(7L, 7R)는, 서로 동일한 구조의 것이며, 각각 상체(3)의 상부의 좌측부, 우측부로부터 견관절(19)을 통하여 연장하여 설치되어 있다. 그리고, 각 팔체(7)는 그 길이 방향의 중간부에 팔꿈치관절(21)을 갖는다. 또, 각 팔체(7)의 선단부에는, 손목관절(23)을 통하여 핸드(손끝부)(25)가 장착되어 있다. 본 실시형태에서는, 견관절(19), 팔꿈치관절(21) 및 손목관절(23)은 각각 3자유도, 1자유도, 2자유도를 갖는다. 따라서, 핸드(25)는 상체(3)에 대하여 6자유도를 갖는다.

머리부(9)는 상체(3)의 상단부에 탑재되어 있다. 이 머리부(9)에는, 좌우 1쌍의 카메라(27L, 27R)가 내장되어 있다. 이들 카메라(27L, 27R)는 예를 들면 CCD 카메라에 의해 구성되어 있다. 이들 카메라(27L, 27R)는 머리부(9)의 정면(대략 로봇(1)의 전방)의 외계의 영상을 촬상하는 것이다. 그리고, 카메라(27L, 27R)는 그것들의 광축이 머리부(9)의 정면을 향하여 거의 동일한 방향을 향하도록 머리부(9)에 고정되어 있다. 또, 머리부(9)에는, 상체(3)에 대하여 이 머리부(9)를 움직일 수 있게 하는 관절기구(도시 생략)가 내장되어 있다. 그 관절기구에 의해, 머리부(9)는 카메라(27L, 27R)와 일체로 상체(3)에 대하여 움직일 수 있게 되어 있 다. 예를 들면, 머리부(9)는 상체(3)의 체간축(대략 상하방향의 축) 주위의 회전동작과, 좌우 방향의 축 주위의 회전동작이 가능하게 되어 있다. 또한, 머리부(9)는 상체(3)에 고정되어 있어도 된다.

또, 상체(3)의 배면부에는 박스(29)가 장착되어 있고, 이 박스(29)에는 후술하는 제어 장치(33) 등이 수용되어 있다.

로봇(1)에는, 그 동작(보행 등)을 제어하기 위하여 도 2에 도시하는 제어 시스템이 구비되어 있다. 이 제어 시스템은, 각 다리체(5), 각 팔체(7) 및 머리부(9)의 상기한 각 관절을 각각 구동하는 관절 액추에이터(31)와, 이 관절 액추에이터(31)를 통하여 로봇(1)의 동작(운동)을 제어하는 제어 장치(33)와, 로봇(1)의 각종 내부 상태량을 검출하는 센서(35)를 갖는다. 또한, 도 2에서는, 관절 액추에이터(31)를 1개만 기재하고 있는데, 이 관절 액추에이터(31)는 각 다리체(5), 각 팔체(7) 및 머리부(9)의 상기한 각 관절마다 구비되어 있다. 또, 각 관절 액추에이터(31)는 전동 모터 등에 의해 구성된다.

센서(35)는, 상세한 도시는 생략하지만, 로봇(1)의 소정의 부위, 예를 들면 상체(3)의 3축방향의 가속도를 검출하는 가속도 센서, 이 상체(3)의 3축 주위의 각속도를 검출하는 레이트 센서, 이 상체(3)의 경사각도(연직방향에 대한 경사각도)를 검출하는 경사계, 로봇(1)의 각 족평(17)에 바닥측으로부터 작용하는 상반력(3축방향의 병진력 및 3축 주위의 모멘트)을 검출하는 힘 센서(6축력 센서) 등으로 구성된다.

제어 장치(33)는 CPU, RAM, ROM, 인터페이스 회로, 관절 액추에이터(31)의 구동회로 등을 포함하는 전자회로 유닛이다. 이 제어 장치(33)에는, 상기 카메라(27L, 27R)의 각각의 촬상화상이 입력됨과 아울러, 상기 센서(35)의 검출신호가 입력된다. 또한, 제어 장치(33)에는, 로봇(1)의 원격조종 장치(도시 생략) 등으로부터 로봇(1)의 이동속도 및 이동방향(진행방향)을 규정하는 지령(이하, 속도·방향 지령이라고 함)이 입력된다. 그리고, 제어 장치(33)는 이들 입력된 데이터를 사용하여, 소요의 연산 처리를 실행하고, 로봇(1)의 목표 보용을 생성한다. 또한, 제어 장치(33)는 생성된 목표 보용에 따라 각 다리체(5)의 고관절(11), 무릎관절(13), 발목관절(15) 등, 로봇(1)에 구비한 각 관절의 목표 동작을 결정한다. 그리고, 제어 장치(33)는 그 목표 동작에 따라 각 관절 액추에이터(31)를 제어하여, 로봇(1)을 이동시킨다.

또, 본 실시형태에서는, 제어 장치(33)는 로봇(1)을 이동시키고자 하는 바닥에 대하여 고정된 글로벌 좌표계(월드 좌표계)에서의 이 로봇(1)의 자기 위치를, 상기 센서(35)(예를 들면 가속도 센서 및 레이트 센서)의 검출신호 등에 기초하여 차례차례 인식하도록 하고 있다. 또한, 로봇(1)의 자기 위치는 적당한 외계 센서를 사용하여 인식하도록 해도 된다. 예를 들면, 로봇(1)을 이동시키려고 하는 바닥 등에 랜드마크를 부설해 두고, 카메라(27L, 27R)의 촬상화상 중의 랜드마크에 기초하여 로봇(1)의 자기위치를 인식하도록 해도 된다.

또, 본 실시형태에서는, 제어 장치(33)는 카메라(27L, 27R)의 촬상화상을 사용하여, 로봇(1)을 이동시키려고 하는 바닥면의 형상을 추정하는 처리도 실행한다.

또한, 본 실시형태에서는, 로봇(1)의 속도·방향 지령을 원격조종 장치로부 터 제어 장치(33)에 입력하도록 하고 있지만, 이 속도·방향 지령, 또는 이것을 규정하는 로봇(1)의 이동 계획을 제어 장치(33)에 미리 인지시켜 두어도 된다.

다음에 로봇(1)의 제어 장치(33)가 실행하는 제어 처리 중, 본 발명에 관련된 제어 처리를 도 3∼도 11을 참조하여 상세히 설명한다. 제어 장치(33)는 로봇(1)을 상기 속도·방향 지령에 따라 이동시키는 제어 처리(로봇(1)의 동작 제어 처리)를 실행하면서, 카메라(27L, 27R)의 촬상화상을 기초로, 로봇(1)의 진행방향 전방에서의 바닥면의 형상을 차례차례 추정한다.

도 3은 이 바닥면의 형상의 추정 처리(이하, 바닥면 형상 추정 처리라고 함)를 나타내는 플로우차트이다. 이 바닥면 형상 추정 처리는 로봇(1)의 동작 제어와 병행되고, 소정의 연산 처리 주기에서 차차 실행되는 처리이다.

여기에서, 이후의 설명에서 사용하는 용어에 관하여 보충해 둔다. 바닥면 형상 추정 처리에서의 각 연산 처리 주기를 프레임, 현재의 연산 처리 주기를 금회 프레임, 그 1개 전의 연산 처리 주기를 전회 프레임이라고 한다. 또, 제어 장치(33)가 로봇(1)의 동작 제어를 위해 현시점에서 상정하고 있는 바닥면(로봇(1)의 현재 위치를 그 영역 내에 포함하는 바닥면)을 현상 상정 바닥면, 실제의 바닥면을 실바닥면, 바닥면 형상 추정 처리로 추정된 바닥면을 추정 바닥면이라고 한다.

현상 상정 바닥면은 로봇(1)의 동작 제어를 위해 필요한 바닥면 형상의 정보에 의해 규정되는 바닥면이다. 그 바닥면 형상의 정보는 글로벌 좌표계에서의 경사면이나 수평면의 위치, 경사면의 경사 정도 등이며, 로봇(1)의 이동 개시 전에, 제어 장치(33)에 입력된다. 제어 장치(33)는, 기본적으로는, 로봇(1)을 이동시키 려고 하는 바닥면이 현상 상정 바닥면에 일치하고 있다고 간주하고 이 로봇(1)의 동작 제어를 행한다. 이 경우, 본 실시형태에서는, 로봇(1)의 진행방향 전방에서의 현상 상정 바닥면은, 추정 바닥면과의 차이(예를 들면, 그들 바닥면의 경사각도의 차)가 어느 정도 큰 경우에, 추정 바닥면에 따라 갱신된다. 또한, 로봇(1)의 이동 개시 시에 있어서의 로봇(1)의 접지 개소의 부근에서의 현상 상정 바닥면은 수평면으로, 실바닥면에 거의 일치하고 있는 것으로 한다.

또, 로봇(1)의 양 족평(17, 17) 중, 로봇(1)의 이동시에, 공중으로 들어올리는(이상(離床)시키는) 족평(17)을 미착지다리(遊脚) 족평(17), 타방의 족평(17)(로봇(1)의 자중을 바닥에 유지하는 다리체(5)의 족평(17))을 지지다리 족평(17)이라고 한다. 로봇(1)을 연속적으로 이동시킬 때, 미착지다리 족평(17) 및 지지다리 족평(17)은 번갈아 전환된다.

또, 본 실시형태에서는, 카메라(27L, 27R) 중 일방의 카메라의 촬상화상을 스테레오 화상에서의 기준화상으로 하고, 타방의 카메라의 촬상화상을 스테레오 화상에서의 참조화상으로 한다. 이후의 설명에서는, 편의상, 예를 들면, 카메라(27L)의 촬상화상을 기준화상으로 하고, 이 카메라(27L)를 기준 카메라(27L)로 하는 경우가 있다.

이상을 전제로 하여, 바닥면 형상 추정 처리를 설명한다. 도 3을 참조하여, 이 바닥면 형상 추정 처리에서는, 우선, STEP 1에서, 제어 장치(33)는 카메라(27L, 27R)로부터 좌우의 각 촬상화상(기준화상 및 참조화상)을 취득한다.

이어서, STEP 2에서, 제어 장치(33)는 입력된 상기 속도·방향 지령과, 현상 상정 바닥면의 형상과, 로봇(1)의 목표 보용을 생성하기 위하여 이 제어 장치(33)가 사용하고 있는 보용 패러미터에 기초하여 로봇(1)의 미착지다리 족평(17)의 장래의 착지 예정 위치를 1보째로부터 N보째까지 복수보분 결정한다. 1보째의 착지 예정 위치는 로봇(1)의 현재의 미착지다리 족평(17)의 착지 예정 위치이다.

본 실시형태에서는, STEP 2에서 결정하는 착지 예정 위치는, 예를 들면 1보째로부터 7보째까지의 7보분의 착지 예정 위치이다(N=7임). 그들 착지 예정 위치는 상기 글로벌 좌표계에서의 위치이다. 또, 그들 착지 예정 위치는 족평(17)의 대표점의 위치를 의미한다.

여기에서, 본 실시형태에서는, 제어 장치(33)가 로봇(1)의 목표 보용을 생성하기 위해 사용하는 보용 패러미터에는 1보째의 보용(이하, 금회 보용이라고 하는 경우가 있음)에서의 미착지다리 족평(17)의 착지 예정 위치(현재의 미착지다리 족평(17)의 착지 예정 위치)와, 그 다음 보용인 2보째의 보용(이하, 다음회 보용이라고 하는 경우가 있음)에서의 미착지다리 족평(17)의 착지 예정 위치(현재의 미착지다리 족평(17)의 착지 예정 위치)와, 금회 보용에 이어지는 가상적인 주기적 보용(2보분의 보용을 제 1 주기분의 보용으로 하는 주기적인 보용)인 정상 보용에서의 1보마다의 족평(17)의 착지 위치를 규정하는 보용 패러미터를 포함한다. 이들 보용 패러미터에 의해 규정되는 금회 보용 및 다음회 보용의 미착지다리 족평(17)의 착지 예정 위치 및 정상 보용에서의 착지 위치는 로봇(1)에 대하여 고정된 좌표계(예를 들면, 로봇(1)의 현재의 지지다리 족평(17)의 접지면 내의 소정의 대표점을 원점으로 하는 좌표계)에서의 위치이다.

이들 보용 패러미터에 의해 규정되는 금회 보용 및 다음회 보용의 미착지다리 족평(17)의 착지 예정 위치는, 제어 장치(33)에 입력된 상기 속도·방향 지령과, 현상 상정 바닥면(로봇(1)의 현재의 접지 개소 부근의 현상 상정 바닥면)의 형상 정보로부터, 미리 정해진 소정의 규칙에 따라 결정된다. 이 경우, 금회 보용 및 다음회 보용의 미착지다리 족평(17)의 착지 예정 위치는, 그들 위치에서 이 미착지다리 족평(17)을 현상 상정 바닥면에 접지시키도록 설정된다. 또, 금회 보용에 이어지는 가상적인 주기적 보용인 정상보용에 관해서는, 그 각 주기(2보분의 기간)의 1보째의 보용에서의 지지다리 족평(17)의 착지 위치와 미착지다리 족평(17)의 착지 위치는, 그것들 사이의 상대적인 위치관계가, 다음회 보용의 지지다리 족평(17)의 착지 위치(즉, 금회 보용의 미착지다리 족평(17)의 착지 예정 위치)와 다음회 보용의 미착지다리 족평(17)의 착지 예정 위치 사이의 상대적 위치관계에 합치하도록 결정된다. 또, 정상보용의 각 주기의 2보째의 보용에서의 지지다리 족평(17)의 착지 위치(즉, 정상보용의 1보째의 보용에 있어서의 미착지다리 족평(17)의 착지 예정 위치)와 미착지다리 족평(17)의 착지 위치는 그것들 사이의 상대적인 위치관계가, 금회 보용의 지지다리 족평(17)의 착지 위치와 금회 보용의 미착지다리 족평(17)의 착지 예정 위치 사이의 상대적인 위치관계에 합치하도록 결정된다.

또한, 정상보용은 금회 보용의 생성조건(로봇(1)의 이동을 안정화시키는 점에서 바람직한 조건)을 결정하기 위하여 생성되는 가상적인 보용이며, 로봇(1)의 현실의 동작 제어에 사용되는 보용은 아니다. 또, 정상보용의 1주기분의 보수는 2보분일 필요는 없고, 3보 이상의 보수를 1주기분의 보수로 해도 된다. 이러한 정 상보용에 관해서는, 예를 들면 본원 출원인에 의한 PCT 국제공개공보 WO/02/40224 등에 상세하게 설명되어 있으므로, 본 명세서에서의 상세한 설명은 생략한다.

그리고, STEP 2에서는, 제어 장치(33)는, 3보째까지의 착지 예정 위치를, 이 제어 장치(33)가 로봇(1)의 목표 보용(금회 보용)의 생성을 위해 사용하고 있는 상기 보용 패러미터에 기초하여 결정한다. 구체적으로는, 이 보용 패러미터에 의해 규정되는 1보째의 착지 예정 위치(금회 보용의 미착지다리 족평(17)의 착지 예정 위치) 및 2보째의 착지 예정 위치(다음회 보용의 미착지다리 족평(17)의 착지 예정 위치)를 글로벌 좌표계로 변환한 것을, 그대로, 2보째까지의 족평(17)의 착지 예정 위치로서 결정한다. 또, 상기 보용 패러미터에 의해 규정되는 정상보용의 1주기째의 2보째의 착지 위치를 글로벌 좌표계로 변환한 것을 3보째의 착지 예정 위치로서 결정한다.

또한, 제어 장치(33)는 4보째로부터 N보째(본 실시형태에서는 N=7)까지의 착지 예정 위치에 대해서는, 입력된 상기 속도·방향 지령과, 현상 상정 바닥면의 형상정보로부터, 소정의 규칙에 따라서 결정한다. 그 규칙은, 1보째 및 2보째의 착지 예정 위치를 결정하기 위한 규칙과 동일하다. 또한, 이들 착지 예정 위치를 결정할 때에는, 현재의 속도·방향 지령과, 현상 상정 바닥면이, 그대로 일정하게 유지되는 것으로 가정된다.

STEP 2의 처리에 의한 1보째부터 N보째까지의 착지 예정 위치의 설정예를 도 5, 도 6(a), (b), 도 7, 도 8(a), (b)를 참조하여 설명한다. 도 5는 로봇(1) 및 실바닥면 등을 로봇(1)의 시상면(측면시)으로 나타낸 설명도, 도 6(a), (b)는, 도 5에 도시하는 실바닥면 등을 연직방향 상방에서 본 도면, 도 7은, 로봇(1) 및 실바닥면 등을 로봇(1)의 시상면(측면시)으로 나타낸 설명도, 도 8(a), (b)는 도 7에 도시하는 실바닥면 등을 연직방향 상방에서 본 도면이다. 이 경우, 도 5에서는, 로봇(1)이 현시점에서 접지하고 있는 실바닥면이 수평면임과 아울러, 로봇(1)의 진행방향 전방의 어떤 위치(P1)로부터 원방측의 실바닥면이 수평면으로부터 경사면(도면에서는 상승 경사면)으로 변화되고 있는 예를 도시하고 있다. 또, 도 7에서는, 로봇(1)이 현 시점으로 접지하고 있는 실바닥면이 경사면(도면에서는 상승 경사면)임과 아울러, 로봇(1)의 진행방향 전방의 어떤 위치(P2)로부터 원방측의 실바닥면이 경사면으로부터 수평면으로 변화되고 있는 예를 도시하고 있다. 또한, 「사면」은 수평면에 대하여 경사진 바닥면을 의미한다. 또, 본 실시형태의 설명에서는, 로봇(1)의 진행경로상의 어떤 구간에서의 바닥면(실바닥면 또는 현상 상정 바닥면 또는 추정 바닥면)이, 그 구간 내에서 실질적으로 일정한 경사각도(수평면에 대한 경사각도)를 갖는 경사면으로 되어 있는 경우에 있어서, 로봇(1)의 진행경로상에서의 이 경사면의 양단을 경사면 에지라고 한다. 그리고, 그들 경사면 에지 중, 로봇(1)의 이동개시 지점에 보다 가까운 측의 경사면 에지를 경사면 개시 에지, 타방의 경사면 에지를 경사면 종료 에지라고 한다. 예를 들면, 도 5에 도시하는 실바닥면에서는, 이 실바닥면이 수평면으로부터 경사면으로 변화되는 위치(P1)에서의 이 수평면과 경사면의 교선이 경사면 개시 에지가 된다. 또, 도 7에 도시하는 실바닥면에서는, 이 실바닥면이 경사면으로부터 수평면으로 변화되는 위치(P2)에서의 이 경사면과 수평면의 교선이 경사면 종료 에지가 된다.

실바닥면이 도 5에 도시하는 바와 같은 바닥면일 경우에 있어서, 현상 상정 바닥면이 수평면(로봇(1)이 현시점에 접지하고 있는 실바닥면(수평면)에 연속하는 수평면)인 것으로 한다. 이 상황에서는, 예를 들면, 도 5 및 도 6(a)에서 파선으로 나타내는 바와 같이, 1보째부터 N보째(N=7)까지의 미착지다리 족평(17(1)∼17(N))의 착지 예정 위치가 설정된다. 이 경우, 로봇(1)의 1보마다의 보폭은 거의 일정하다. 또한, 본 실시형태의 설명에서는, 첨자(i)(i=1, 2, ……, N)는 i보째의 착지 예정 위치에 대응하는 것을 의미한다.

또, 실바닥면이 도 5에 도시하는 바와 같은 바닥면일 경우에 있어서, 현상 상정 바닥면이 실바닥면에 거의 일치하고 있는 것으로 한다. 이 경우에는, 예를 들면 도 6(b)에 파선으로 나타내는 바와 같이, 각 보수째의 착지 예정 위치에서의 로봇(1)의 미착지다리 족평(17(1)∼17(N))(N=7)이 경사면 개시 에지에 놓이지 않고, 또한, 이 경사면 개시 에지에 지나치게 근접하지 않도록, 1보째부터 N보째까지의 착지 예정 위치가 설정된다.

또, 실바닥면이 도 7에 도시하는 바와 같은 바닥면일 경우에 있어서, 현상 상정 바닥면이 경사면(로봇(1)이 현시점에서 접지하고 있는 실바닥면(경사면)에 연속하는 경사면)인 것으로 한다. 이 상황에서는, 예를 들면, 도 7 및 도 8(a)에서 파선으로 나타내는 바와 같이, 1보째부터 N보째(N=7)까지의 미착지다리 족평(17(1)∼17(N))의 착지 예정 위치가 설정된다. 이 경우, 로봇(1)의 1보마다의 보폭은 거의 일정하다. 단, 그 보폭은 도 5 및 도 6(a)의 경우(현상 상정 바닥면이 수평면일 경우)보다도 작은 보폭으로 된다.

또, 실바닥면이 도 7에 도시하는 바와 같은 바닥면일 경우에 있어서, 현상 상정 바닥면이 실바닥면에 거의 일치하고 있는 것으로 한다. 이 경우에는, 예를 들면 도 8(b)에 파선으로 나타내는 바와 같이, 각 보수째의 착지 예정 위치에서의 로봇(1)의 미착지다리 족평(17(1)∼17(N))(N=7)이 경사면 종료 에지에 놓이지 않고, 또한, 이 경사면 종료 에지에 지나치게 근접하지 않도록, 1보째부터 N보째까지의 착지 예정 위치가 설정된다.

이상과 같이 N보째까지의 각 착지 예정 위치는 미착지다리 족평(17)이 경사면 개시 에지 혹은 경사면 종료 에지에 놓이는 것을 피하도록 설정된다.

보충하면, 본 실시형태에서는, 3보째의 착지 예정 위치를 정상보용의 보용 패러미터에 대응시켜 결정했지만, 4보째 이후의 착지 예정 위치의 결정 방법과 마찬가지로 3보째의 착지 예정 위치를 결정해도 된다. N보째까지의 각 착지 예정 위치는, 기본적으로는, 속도·방향 지령 및 현상 상정 바닥면이 장래에도 현상으로 유지된다고 가정하고, N보째까지의 로봇(1)의 평균적인 이동속도와 이동방향이 속도·방향 지령에 합치하고, 또한 양다리체(5, 5) 상호간의 간섭 등에 관한 기하학적인 제약조건을 만족하면서, 현상 상정 바닥면상에서 로봇(1)을 안정하게 이동시킬 수 있도록 설정하면 된다.

또한, 로봇(1)의 현재의 미착지다리 족평(17)이 착지할 때까지는, 속도·방향 지령과, 현상 상정 바닥면이 일정하게 유지되고 있는 한, 어느 프레임에서도, STEP 2에서 설정되는 1보째부터 N보째까지의 각 착지 예정 위치는 동일하게 된다. 한편, 현재의 미착지다리 족평(17)이 착지하기까지, 속도·방향 지령이 변화되거 나, 또는, 현상 상정 바닥면이 갱신된 경우에는, STEP 2에서 설정되는 1보째부터 N보째까지의 각 착지 예정 위치에서 설정되는 각 착지 예정 위치는 경시적으로 변화된다. 또, 현재의 미착지다리 족평(17)이 착지한 후에는, 적어도 N보째의 착지 예정 위치는 신규로 설정되게 된다.

상기한 바와 같이 STEP 2의 처리를 실행한 후, 제어 장치(33)는 다음에 STEP 3의 처리를 실행한다. 이 STEP 3의 처리에서는, 제어 장치(33)는 STEP 2에서 결정한 1보째부터 N보째(N=7)까지의 각 착지 예정 위치 부근에 화상투영 영역(A(i))(i=1, 2, ……, N)을 설정한다. 각 화상투영 영역(A(i))은 후술하는 바와 같이 카메라(27L, 27R)의 각 촬상화상에 투영되는 영역이다. 그리고, 이 화상투영 영역(A(i))은 현상 상정 바닥면 상에 설정되는 평면 영역이다.

도 9는 각 화상투영 영역(A(i))의 설정예를 도시하는 도면이다. 이 도 9에는, 현상 상정 바닥면에 수직한 방향에서 본 화상투영 영역(A(i))(i=1, 2, ……, N)과, i보째의 착지 예정 위치에서 현상 상정 바닥면에 접지시킨 족평(17(i))을 도시하고 있다. 도시하는 바와 같이, 화상투영 영역(A(i))은, 그것에 대응하는 착지 예정 위치에서 현상 상정 바닥면에 족평(17(i))(도시예에서는 우측 족평(17R))의 밑바닥면의 평탄면의 전체를 접지시켰을 때의 이 족평(17(i))의 접지면의 부근에서 이 접지면을 포함하도록 설정된다.

보다 구체적으로는, 본 실시형태에서는, 화상투영 영역(A(i))은, 예를 들면 장방형 형상으로 설정된다. 이 경우, 로봇(1)의 진행방향에서의 화상투영 영역(A(i))의 길이(L1)는 이 진행방향에서의 족평(17(i))의 접지면의 길이(L3)보다도 약간 큰 것으로 되어 있다. 또, 로봇(1)의 진행방향과 직교하는 방향에서의 화상투영 영역(A(i))의 길이(L2)는, 현상 상정 바닥면에 대한 족평(17(i))의 접지상태를 유지한 채, 타방의 족평(17)(도 9에 도시하는 예에서는, 2점 쇄선으로 나타내는 좌측 족평(17L))을 족평(17(i))과 좌우 방향으로 병렬시켜 현상 상정 바닥면에 접지시켰다고 한 경우에, 양 족평(17, 17)의 접지면이 양자 모두, 화상투영 영역(A(i))에 포함되는 것과 같은 길이(로봇(1)의 진행방향에서 본 좌측 족평(17L)의 좌측 가장자리와, 우측 족평(17R)의 우측 가장자리의 간격(L4)보다도 약간 큰 길이)로 설정된다.

또한, 도 9에 도시하는 예에서는, 설명의 편의상, 족평(17)의 밑바닥면의 전체가 접지면(평탄면)이도록 기재하고 있는데, 족평(17)의 밑바닥면의 전체가 평탄면일 필요는 없다. 예를 들면, 족평(17)의 밑바닥면 중의 전단부 및 후단부가 위를 향해 만곡해 있고, 이 전단부 및 후단부를 제외한 밑바닥면이 평탄면으로 이루어져 있어도 된다. 또는, 족평(17)의 밑바닥면에 복수의 면 일치된 평탄면이 형성되어 있어도 된다. 이것들의 경우에는, 족평(17)의 접지면은 이 족평(17)의 밑바닥면의 일부분이 된다. 그리고, 그 경우에는, i보째의 화상투영 영역은 족평(17(i))의 전체를 현상 상정 바닥면에 투영한 영역 중, 이 족평(17(i))의 접지면 이외의 영역이 튀어 나오도록 설정되어 있어도 된다.

또, 화상투영 영역(A(i))은 반드시 장방형 모양으로 설정할 필요는 없다. 또, 예를 들면, 도 9에 가상선으로 나타내는 A(i)'과 같이 , 족평(17(i))의 접지면을 포함하고, 또한, 이 접지면보다도 약간 큰 면적을 갖는 영역(그 영역 내에 1개 의 족평(17(i))의 접지면만을 포함하는 것과 같은 영역)을 i보째의 화상투영 영역으로서 설정하도록 해도 된다.

상기한 바와 같이 화상투영 영역(A(i))을 설정했을 때, 예를 들면, 도 5에 도시하는 상황에서, 현상 상정 바닥면이 수평면일 경우에는, 도 5 및 도 6(a)에 도시하는 바와 같이, 1보째부터 N보째(N=7)까지의 화상투영 영역(A(1)∼A(N))이 설정된다. 또, 현상 상정 바닥면이, 도 5의 실바닥면에 거의 일치하고 있는 경우에는, 도 6(b)에 도시하는 바와 같이, 1보째부터 N보째까지의 화상투영 영역(A(1)∼A(N))이 설정된다.

또, 예를 들면, 도 7에 도시하는 상황에서, 현상 상정 바닥면이 경사면일 경우에는, 도 7 및 도 8(a)에 도시하는 바와 같이, 1보째부터 N보째(N=7)까지의 화상투영 영역(A(1)∼A(N))이 설정된다. 또, 현상 상정 바닥면이 도 7의 실바닥면에 거의 일치하는 경우에는, 도 8(b)에 도시하는 바와 같이, 1보째부터 N보째까지의 화상투영 영역(A(1)∼A(N))이 설정된다.

이어서, 제어 장치(33)는 STEP 4의 처리를 실행한다. 이 STEP 4의 처리에서는, 제어 장치(33)는 STEP 3에서 설정한 각 화상투영 영역(A(i))(i=1, 2, ……, N)을 카메라(27L, 27R)의 각 촬상화상에 투영한다. 또한, 이 경우, 각 화상투영 영역(A(i))을 각 카메라(27)의 촬상화상에 투영하기 위하여, 제어 장치(33)는 글로벌 좌표계에서의 카메라(27L, 27R)의 위치 및 자세, 또는, 카메라(27L 또는 27R)에 대하여 고정된 좌표계에서의 각 화상투영 영역(A(i))의 위치 및 자세를, 글로벌 좌표계에서의 로봇(1)의 현재 위치와, 이 로봇(1)의 각 관절의 변위량(검출값 또는 목 표값)에 기초하여 인식한다. 또한, 「자세」는 공간적인 방향을 의미한다.

STEP 4의 처리에 의해, 예를 들면 도 10에 예시하는 바와 같이, 각 촬상화상 중에, 각 화상투영 영역(A(i))을 이 촬상화상에 투영하여 이루어지는 부분화상 영역(Ac(i))이 얻어진다. 또한, 도 10은 우측 카메라(27R)의 촬상화상을 도시하고 있다.

각 부분화상 영역(Ac(i)) 내의 화상은 로봇(1)의 진행방향 전방에 존재하는 실바닥면의 부분영역의 화상이다. 이후, 각 부분화상 영역(Ac(i)) 내에 화상으로서 포착되어 있는 실바닥면의 부분영역을 실바닥면 부분영역(Ar(i))이라고 한다. 이 실바닥면 부분영역(Ar(i))은 본 발명에서의 물체(본 실시형태에서는 바닥)의 평탄부에 상당하는 것이다.

보충하면, 화상투영 영역(A(i))에서의 현상 상정 바닥면이 실바닥면에 일치하고 있는 경우에는, 이 화상투영 영역(A(i))에 대응하는 실바닥면 부분영역(Ar(i))은 화상투영 영역(A(i))에 일치한다. 한편, 화상투영 영역(A(i)) 내의 현상 상정 바닥면이 실바닥면에 일치하고 있지 않은 경우에는, 이 화상투영 영역(A(i))에 대응하는 부분화상 영역(Ac(i)) 내의 화상, 즉, 실바닥면 부분영역(Ar(i))의 화상은 실바닥면 중, 화상투영 영역(A(i)) 내의 각 점과 각 카메라(27)의 광학 중심을 연결하는 직선 상에 위치하는 부분영역의 화상이 된다. 예를 들면, 도 5에 도시하는 상황에서, 현상 상정 바닥면이 수평면일 경우에는, 6보째 및 7보째의 화상투영 영역(A(6), A(7))에 각각 대응하는 실바닥면 부분영역(Ar(6), Ar(7))은, 각각 도 5에 도시하는 바와 같이, 실바닥면 중 경사면 내의 부분영역이 된다. 그리고, 이들 실바닥면 부분영역(Ar(6), Ar(7))의 화상이 각각 부분화상 영역(Ac(6), Ac(7))내의 화상이 된다. 마찬가지로, 도 7에 도시하는 상황에서, 현상 상정 바닥면이 경사면일 경우에는, 6보째 및 7보째의 화상투영 영역(A(6), A(7))에 각각 대응하는 실바닥면 부분영역(Ar(6), Ar(7))은, 각각 도 7에 도시하는 바와 같이, 실바닥면 중 수평면 내의 부분영역이 된다.

또한, 1보째부터 N보째(N=7)까지의 모든 화상투영 영역(A(i))에 대응하는 부분화상 영역(Ac(i))이 각 카메라(27)의 실제의 촬상화상 내에 포함된다고는 할 수 없다.

이어서, 제어 장치(33)는 STEP 5의 처리를 실행한다. 이 처리는 각 화상투영 영역(A(i))에 대응하는 부분화상 영역(Ac(i)) 내에 포착되어 있는 실바닥면, 즉, 실바닥면 부분영역(Ar(i))의 형상을 나타내는 형상 패러미터를 추정하는 처리이다.

여기에서, 이 추정 처리의 개요를 설명해 둔다. 본 실시형태에서는, 각 화상투영 영역(A(i))에 대응하는 실바닥면 부분영역(Ar(i))이 어떤 1개의 평면에 포함되는 영역이라고 간주하고, 이 실바닥면 부분영역(Ar(i))의 공간적(3차원적)인 자세를 의미하는 바닥면 자세 패러미터와, 이 실바닥면 부분영역(Ar(i))의 법선방향에서의 이 실바닥면 부분영역(Ar(i))의 공간적인 위치를 나타내는 바닥면 위치 패러미터와의 세트를 이 실바닥면 부분영역(Ar(i))의 형상 패러미터로서 사용한다.

일반적으로, 공간 내의 임의의 평면은 그 법선방향과, 그 법선방향에서의 이 평면의 위치로부터 일의적으로 특정된다. 그래서, 본 실시형태에서는, 실바닥면 부분영역(Ar(i))의 바닥면 자세 패러미터로서, 이 실바닥면 부분영역(Ar(i))을 포함하는 평면의 법선벡터(단위 벡터)(n(i))를 사용한다. 또, 본 실시형태에서는, 로봇(1)의 어떤 대표점과 실바닥면 부분영역(Ar(i))을 포함하는 평면의 거리(d(i))(이하, 평면 거리(d(i))라고 함)를 바닥면 위치 패러미터로서 사용한다. 이 경우, 평면 거리(d(i))에 관한 로봇(1)의 대표점으로서, 예를 들면, 카메라(27R, 27L) 중의 기준 카메라(27L)의 광학 중심을 사용한다.

따라서, STEP 5의 처리는 각 실바닥면 부분영역(Ar(i))마다, 그것을 포함하는 평면의 법선벡터(n(i))와 평면 거리(d(i))의 세트(n(i), d(i))를 실바닥면 부분영역(Ar(i))의 형상 패러미터로서 추정하는 처리이다. 또한, 본 실시형태에서는, STEP 5에서 추정되는 법선벡터(n(i))는, 각 프레임에서, 예를 들면 카메라(27L, 27R)에 대하여 고정된 3차원 좌표계(이하, 카메라 좌표계라고 함)에서의 각 좌표축 성분의 세트로서 기술된다. 그 카메라 좌표계는, 예를 들면, 기준 카메라(27L)의 광학 중심을 원점으로 하는 3차원 직교 좌표계이다. 이 경우, 평면 거리(d(i))는 카메라 좌표계의 원점으로부터 실바닥면 부분영역(Ar(i))을 포함하는 평면까지의 거리가 된다.

상기 도 5 및 도 7에는, 상기와 같이 정의되는 형상 패러미터의 예를 도시하고 있다. 도 5 중의 n(2), d(2)는 2보째의 화상투영 영역(A(2))에 대응하는 실바닥면 부분영역(Ar(2))(도시예에서는, Ar(2)는 A(2)에 거의 일치하고 있음)의 형상 패러미터로서의 법선벡터(n(2))와, 평면 거리(d(2))를 예시하고 있다. 마찬가지 로, 도 5중의 n(6), d(6)은 6보째의 화상투영 영역(A(6))에 대응하는 실바닥면 부분영역(Ar(6))의 형상 패러미터로서의 법선벡터(n(6))와, 평면 거리(d(6))를 예시하고 있다. 또, 도 7 중의 n(3), d(3)은 3보째의 화상투영 영역(A(3))에 대응하는 실바닥면 부분영역(Ar(3))(도시예에서는, Ar(3)은 A(3)에 거의 일치하고 있다)의 형상 패러미터로서의 법선벡터(n(3))와, 평면 거리(d(3))를 예시하고 있다. 마찬가지로, 도 7중의 n(6), d(6)은 6보째의 화상투영 영역(A(6))에 대응하는 실바닥면 부분영역(Ar(6))의 형상 패러미터로서의 법선벡터(n(6))와, 평면 거리(d(6))를 예시하고 있다.

또한, STEP 5의 처리에서는, 각 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))를 카메라(27L, 27R)의 각각의 촬상화상 간의 사영변환을 이용한 수렴연산 처리에 의해 추정한다. 그 수렴연산 처리에서는, 참조화상(카메라(27R)의 촬상화상) 중의 부분화상 영역(Ac(i)) 내의 화상을, 형상 패러미터(n(i), d(i))의 값에 따라 정해지는 사영변환 행렬에 의해 변환하여 이루어지는 화상을, 기준화상(카메라(27L)의 촬상화상) 중의 부분화상 영역(Ac(i)) 내의 화상과 비교하면서, 그들 화상이 거의 일치하도록, (n(i), d(i))의 값을 어떤 초기값으로부터 서서히 갱신해 간다. 바꾸어 말하면, (n(i), d(i))를 변수 패러미터로 하여 그 변수 패러미터의 값에 의해 규정되는 평면을 실바닥면 부분영역(Ar(i))을 포함하는 평면에 수렴시키도록, 이 변수 패러미터의 값을 어떤 초기값으로부터 갱신해 간다(나아가서는, 이 변수 패러미터의 값에 의해 정해지는 사영변환 행렬을 갱신해 간다). 이 수렴연산 처리에 의한 변수 패러미터의 최종적인 수렴값이 실바닥면 부분영 역(Ar(i))의 형상 패러미터(n(i), d(i))의 추정값으로서 얻어진다. 또한, 그 변수 패러미터의 수렴값으로부터 정해지는 사영변환 행렬은, 참조화상 중의 부분화상 영역(Ac(i)) 내의 화상을, 기준화상 중의 부분화상 영역(Ac(i)) 내의 화상에 일치하는 화상으로 변환할 수 있는 사영변환 행렬로서의 의미를 갖는다.

도 4는 이 STEP 5의 처리의 상세를 나타내는 플로우차트이다. 이 처리는 STEP 3에서 설정된 각 화상투영 영역(A(i))마다 실행되는 처리이다. 이후, 이 STEP 5의 처리의 설명에서는, 착지 예정 위치의 보수째를 나타내는 참조부호 i는 1∼N(N=7) 중 임의의 1개의 값을 의미하는 것으로서 사용한다. 또, 전회 프레임에서 설정된 각 화상투영 영역(A(1)∼A(N))을 전회 화상투영 영역(A_p(1)∼A_p(N))으로 칭한다. 첨자 「_p」는 전회 프레임에 관한 것이라고 하는 의미로 사용한다.

STEP 5의 처리에서는, 제어 장치(33)는, 우선, STEP 501에서, N개의 전회 화상투영 영역(A_p(1)∼A_p(N)) 중에서, 금회 프레임에서의 ｉ보째의 착지 예정 위치에 대응하는 화상투영 영역(A(i))에 가장 가까운 전회 화상투영 영역(A_px)(A_px는 A_p(1)∼A_p(N) 중 1개)을 탐색한다. 여기에서, 「가장 가깝다」라고 하는 것은 공간적인 위치가 가장 가까운 것을 의미한다. 보다 구체적으로는, 제어 장치(33)는 화상투영 영역(A(i))의 중심점과, N개의 전회 화상투영 영역(A_p(1)∼A_p(N))의 각각의 중심점 사이의 거리를, 그것들의 중심점의 글로벌 좌표계에서의 위치에 기초하여 산출하고, 그 거리가 최소가 되는 1개의 전회 화상투영 영역을 A(i)에 가장 가까운 전회 화상투영 영역(A_px)으로 한다. 이후, 이 전회 화상투영 영역(A_px)을 근접 전회 화상투영 영역(Ap_x)이라고 한다.

이어서, STEP 502으로 진행되고, 제어 장치(33)는 화상투영 영역(A(i))과, 근접 전회 화상투영 영역(A_px)의 위치 차가 소정의 임계값 이상인지 아닌지를 판단한다. 또한, A(i)와 A_px의 위치차는 STEP 501에서 산출한 A(i)의 중심점과 A_px의 중심점의 거리이다. 그리고, 제어 장치(33)는, 이 판단 결과가 부정적인 경우에는, STEP 503, 504의 처리를 실행하고, 긍정적인 경우에는, STEP 505∼510의 처리를 실행한다.

보충하면, STEP 501∼510의 처리는 후술하는 수렴연산 처리에서의 (n(i), d(i))의 초기값을 설정하기 위한 처리이다.

여기에서, STEP 502의 판단 결과가 부정적으로 되는 상황은, 화상투영 영역(A(i))과 근접 전회 화상투영 영역(A_px)이 겹치는 부분을 갖고, A(i)에 대응하는 실바닥면 부분영역(Ar(i))을 포함하는 평면과, A_px에 대응하는 실바닥면 부분영역(Ar_px)을 포함하는 평면이 일치, 혹은 거의 일치하는 것이 높은 정확도로 예측되는 상황이다. 반대로, STEP 502의 판단 결과가 긍정적으로 되는 상황은 화상투영 영역(A(i))과 근접 전회 화상투영 영역(A_px)이 겹치는 부분이 겹치는 부분을 갖지 않고(환언하면 전회 프레임에서 이미 형상 패러미터의 추정이 이루어진 실바닥면 부분영역(Ar_px)의 관련성이 없고), A(i)에 대응하는 실바닥면 부분영역(Ar(i))을 포함하는 평면의 공간적인 자세 및 위치를 현시점에서는 높은 정확도로 예측하는 것이 곤란한 상황을 의미한다. 또한, STEP 502의 판단 결과가 긍정적으로 되는 상황은, 금회 프레임으로 새로운 보수째(전회 프레임에서의 N보째의 다음 보수째)의 착지 예정 위치에 대응하는 화상투영 영역이 설정된 경우나, 전회 프 레임과 금회 프레임 사이에서 로봇(1)의 진행방향 전방의 현상 상정 바닥면이 갱신된 경우 등에 발생한다.

STEP 502의 판단 결과가 부정적일 경우의 STEP 503, 504의 처리에서는, 화상투영 영역(A(i))에 대응하는 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))을 수렴연산 처리에 의해 추정하기 위하여, 그 초기값(n0(i), d0(i))이 설정된다. 보다 구체적으로는, STEP 503에서는, 제어 장치(33)는 근접 전회 화상투영 영역(A_px)에 대응하여, 전회 프레임으로 추정된 형상 패러미터(n_px, d_px)(이것은 전회 프레임에서의 카메라 좌표계로 기술됨)를 금회 프레임에서의 카메라 좌표계로 변환하고, 그 변환 후의 형상 패러미터를(n_px', d_px')로 한다. 또한, 전회 프레임의 카메라 좌표계와 금회 프레임의 카메라 좌표계 사이의 관계는, 전회 프레임과 금회 프레임 사이의 기간에서의 로봇(1)의 각 관절의 변위량에 기초하여 인식된다.

이어서, STEP 504에서, 제어 장치(33)는 상기와 같이 구한 (n_px', d_px')을 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 초기값(n0(i), d0(i))으로서 설정한다. 이와 같이, (n0(i), d0(i))를 설정한다고 하는 것은 실바닥면 부분영역(Ar(i))을 포함하는 평면의 초기값을 근접 전회 화상투영 영역(A_px)에 대응하는 실바닥면 부분영역(Ar_px)을 포함하는 평면에 일치시킨다고 하는 것을 의미한다.

한편, STEP 502의 판단 결과가 긍정적인 경우의 STEP 505∼510의 처리에서는, (n(i), d(i))의 초기값(n0(i), d0(i))이 다음과 같이 설정된다. 즉, STEP 505 에서, 제어 장치(33)는 근접 전회 화상투영 영역(A_px)에 대응하여, 전회 프레임으로 추정된 형상 패러미터(n_px, d_px)에 의해 규정되는 평면(πx)을 글로벌 좌표계에서 구한다(글로벌 좌표계에서의 평면(πx)의 위치 및 자세를 구한다). 보다 구체적으로는, 예를 들면, 전회 프레임의 카메라 좌표계로 기술된 형상 패러미터(n_px, d_px)를, 상기 STEP 503과 같은 처리에 의해, 금회 프레임에서의 카메라 좌표계로 변환한다. 그리고, 그 변환 후의 형상 패러미터(n_px'，d_px')와, 금회 프레임에서의 카메라 좌표계와 글로벌 좌표계 사이의 관계에 기초하여 평면(πx)을 글로벌 좌표계로 기술한다. 또는, 전회 프레임의 카메라 좌표계로 기술된 형상 패러미터(n_px, d_px)와, 전회 프레임에서의 카메라 좌표계와 글로벌 좌표계 사이의 관계에 기초하여 평면(πx)을 글로벌 좌표계로 기술한다. 또한, 각 프레임에서의 카메라 좌표계와 글로벌 좌표계 사이의 관계는, 각 프레임에서의 로봇(1)의 글로벌 좌표계상의 위치와, 이 프레임에서의 이 로봇(1)의 각 관절의 변위량에 기초하여 인식할 수 있다.

이어서, STEP 506으로 진행되고, 제어 장치(33)는 화상투영 영역(A(i))의 중심점과 기준 카메라(27L)의 광학 중심을 통과하는 직선(L(i))을 글로벌 좌표계에서 구한다. 즉, 이 직선(L(i))을 글로벌 좌표계로 기술한다.

이어서, STEP 507로 진행되고, 제어 장치(33)는 평면(πx)과 직선(L(i))의 교점(P(i))을 구한다. 즉, 교점(P(i))의 글로벌 좌표계에서의 위치를 구한다. 또한, 예를 들면, 금회 프레임에서의 카메라 좌표계에서의 교점(P(i))의 위치를 구해 두고, 그것을 글로벌 좌표계로 변환하도록 해도 된다.

이어서, STEP 508로 진행되고, 상기와 같이 구한 교점(P(i))을 통과하고, 수평면으로부터 소정 각도(θ)만큼 기운 평면(πa(i))을 글로벌 좌표계로 구한다. 이 소정 각도(θ)는 본 실시형태에서는, 예를 들면 45도이다. 이 경우, 교점(P(i))을 통과하고, 소정 각도(θ)만큼 기운 평면은 무수히 존재하지만, 그 어느 평면을 πa(i)로서 구해도 된다. 일례로서, 본 실시형태에서는, 교점(P(i))을 통과하고 글로벌 좌표계의 Y축방향 혹은 로봇(1)의 좌우 방향에 평행한 수평직선을 포함하고, 수평면으로부터 소정 각도(θ)만큼 기운 평면(로봇(1)에서 보아 상승 경사면 또는 하강 경사면이 되는 평면)을 πa(i)로서 구하도록 했다. 또한, 평면(πa(i))이 통과하는(평면(πa(i)) 상에 존재함) 점(P(i))은, 반드시 상기한 바와 같이 설정하지 않아도 되고, 예를 들면, 화상투영 영역(A(i)) 내의 임의의 점을 P(i)로서 설정해도 된다. 또, 예를 들면, 근접 전회 화상투영 영역(A_px)에 대응하여 전회 프레임에서 추정된 형상 패러미터(n_px, d_px)를 금회 프레임에서의 카메라 좌표계로 변환하여 이루어지는 (n_px'，d_px')의 d_px'을 평면(πa(i))의 평면 거리로서 설정하도록 해도 된다. 이 경우에는, 평면(πa(i))이 통과하는 점(P(i))을 결정할 필요는 없다.

이어서, S509로 진행되고, 금회 프레임에서의 카메라 좌표계에서 평면(πa(i))을 나타내는 형상 패러미터(na(i), da(i))를 구한다. na(i)는 πa(i)의 카메라 좌표계에서의 법선벡터(단위 벡터)이며, da(i)는 금회 프레임에서의 기준 카메라(27L)의 광학 중심과 평면(πa(i))의 거리이다.

이어서, STEP 510으로 진행되고, (na(i), da(i))를 실바닥면 부분영 역(Ar(i))의 형상 패러미터(n(i), d(i))의 초기값(n0(i), d0(i))으로서 설정한다.

이와 같이, STEP 502의 판단 결과가 긍정적으로 되는 경우, 즉, A(i)에 대응하는 실바닥면 부분영역(Ar(i))을 포함하는 평면의 공간적인 자세 및 위치를 현시점에서는 높은 정확도로 예측하는 것이 곤란한 상황인 경우에는, 형상 패러미터(n(i), d(i))의 초기값(n0(i), d0(i))은 그것에 의해 규정되는 평면이 수평면에 대하여 소정 각도(θ)만큼 기운 평면으로 되도록 설정된다.

이 경우의 초기값(n0(i), d0(i))의 설정예를 도 11을 참조하여 설명한다. 도 11은 그 설명도이며, 로봇(1)이나 실바닥면 등을 시상면(측면시)으로 나타내고 있다. 이 예에서는, 금회 프레임에서의 6보째의 화상투영 영역(A(6))에 가장 가까운 근접 전회 화상투영 영역(A_px)이 전회 프레임에서의 5보째의 화상투영 영역(A_p(5))인 것으로 한다. 또, 이 예에서는, 전회 프레임에서의 5보째의 착지 예정 위치의 부근에서의 실바닥면과 현상 상정 바닥면이 거의 일치하고 있고, 나아가서는, 근접 전회 화상투영 영역(A_px)(=A_p(5))에 대응하는 실바닥면 부분영역(Ar_p(5))이 A_px에 거의 일치하고 있는 것으로 하고 있다.

이 경우에는, 금회 프레임에서의 6보째의 화상투영 영역(A(5))에 관하여, 도면 중에 나타내는 평면(πx)(실바닥면 부분영역(Ar_p(5))을 포함하는 평면)이 STEP 505에서 구해진다. 그리고, 도면 중에 나타내는 직선(L(6))이 STEP 506에서 구해진다. 또한, 도면 중에 나타내는 점(P(6))이 STEP 507에서 구해진다. 또한, 이 예에서는, 점(P(6))은 6보째의 화상투영 영역(A(6))의 중심점에 일치하고 있다. 그리고, 도면 중에 나타내는 평면(πa(6))이 STEP 508에서 구해진다. 이 평면(π a(6))은 점(P(6))을 통과하고, 수평면에 대하여 소정 각도(θ)(예를 들면, 45도)만큼 기울어 있다. 또한, 이 평면(πa(6))을 나타내는 형상 패러미터(na(6), da(6)), 즉, 평면(πa(6))의 법선벡터(na(6))과, 금회 프레임에 있어서의 기준 카메라(27L)의 광학 중심으로부터 평면(πa(6))까지의 평면 거리(da(6))의 세트가 STEP 509에서 구해진다. 그리고, 이 형상 패러미터(na(6), da(6))가 6보째의 화상투영 영역(A(6))에 대응하는 실바닥면 부분영역(Ar(6))의 형상 패러미터(n(6), d(6))의 초기값(n0(6), d0(6))으로서 설정된다.

또한, STEP 502의 판단 결과가 긍정적으로 되는 경우에 실행하는 STEP 505∼510의 처리는 본 발명의 주요부 기술에 상당하는 처리이다. 이 처리에 의해 형상 패러미터(n(i), d(i))의 초기값(n0(i), d0(i))를 상기한 바와 같이 설정하는 이유에 대해서는, 후술한다.

이상에서 설명한 STEP 501∼510의 처리가, (n(i), d(i))의 초기값(n0(i), d0(i))을 설정하기 위한 처리이다.

이상과 같이 초기값(n0(i), d0(i))을 설정한 후, STEP 512로 진행되고, 제어 장치(33)는 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 임시값으로서, 초기값(n0(i), d0(i))을 설정한다. 또한, 이 STEP 512로부터의 처리가 형상 패러미터(n(i), d(i))를 추정하기 위한 수렴연산 처리이다. 이 수렴연산 처리는 본 실시형태에서는 가우스 뉴튼법의 수렴연산 처리이다.

이어서, STEP 513로 진행되고, 갱신 전 오차량(e(k-1))을 초기화한다. 여기에서 초기화되는 갱신 전 오차량(e(k-1))은 후술하는 루프 처리에서의 최초의 STEP 516의 처리에서 구해지는 오차량(e(k))과 비교하는 것이며, 그 오차량(e(k))보다도 확실하게 큰 오차량이 되는 것과 같은 과대한 오차량으로 설정된다.

이어서, STEP 514부터 STEP 522까지의 처리를 소요의 조건이 충족시켜질 때까지 반복하는 루프 처리가 실행된다. 이 루프 처리에서는, 형상 패러미터(n(i), d(i))는 변수 패러미터로서의 의미를 가지며, 적당하게, 그 값이 갱신된다. 즉, (n(i), d(i))는 본 발명에서의 패러미터에 상당한다. 이후, 그 루프 처리에서의 각 회의 STEP 514∼522의 처리를 루프 내 연산 처리, 현재 실행 중의 루프 내 연산 처리를 금회 루프 내 연산 처리, 그 1회 전의 루프 내 연산 처리를 전회 루프 내 연산 처리라고 한다. 또, 금회 루프 내 연산 처리에서 구해지는 값과, 전회 루프 내 연산 처리에서 구해지는 값을 구별하기 위하여, 필요에 따라 전자의 값에 (k)를 붙이고, 후자의 값에 (k-1)을 붙인다. 또, (n(i), d(i))는 그 값이 확정될 때까지는, 변수 패러미터(n(i), d(i))라고 한다.

STEP 514에서, 제어 장치(33)는 변수 패러미터(n(i), d(i))의 현재값으로부터, 카메라(27L, 27R) 간의 사영변환 행렬(H(i))을 산출한다. 이 사영변환 행렬(H(i))은 다음 식 (1)에 의해 주어지는 행렬이다. 또한, 여기에서는, H(i)는 정규화된 카메라 좌표계로 표현되어 있다.

H(i)=R+t·m(i)T ……（1)

단, m(i)≡n(i)/d(i)

여기에서, R은 카메라(27L, 27R) 간의 회전행렬, t는 카메라(27L, 27R)의 각각의 광학 중심 간의 병진 벡터(여기에서는 종벡터)이다. 이것들은 카메라(27L, 27R)의 상대적인 위치 및 자세관계 등의 내부 패러미터에 의존하는 것으로, 미리 캘리브레이션에 의해 특정되어 있다. 또한 m(i)는 식 (1)의 단서에서 정의되는 바와 같이, 법선벡터(n(i))를 평면 거리(d(i))로 나눈 벡터(여기에서는 종벡터)이다. 또한 식 (1) 중의 「T」는 전치를 의미한다.

식 (1)로부터 명확한 바와 같이, 사영변환 행렬(H(i))은 변수 패러미터(n(i), d(i))의 값에 의해 규정된다.

이어서, STEP 515로 진행되고, 제어 장치(33)는 카메라(27L, 27R)의 촬상화상 중 참조화상(카메라(27R)의 촬상화상)에서의 화상투영 영역(A(i))의 투영화상, 즉 이 참조화상에서의 부분화상 영역(Ac(i)) 내의 화상을, 상기와 같이 구한 사영변환 행렬(H(i))에 의해 변환하고, 그 변환에 의해 얻어진 화상을 변환화상으로서 얻는다.

이어서, STEP 516으로 진행되고, 제어 장치(33)는 기준화상(카메라(27L)의 촬상화상)에서의 화상투영 영역(A(i))의 투영화상, 즉 이 기준화상에서의 부분화상 영역(Ac(i)) 내의 화상과, 상기 STEP 515에서 구한 변환화상 사이의 오차량(e(k))을 구한다. 오차량(e(k))은 기준화상에서의 부분화상 영역(Ac(i)) 내의 화상과, 상기 STEP 515에서 구한 변환화상의 휘도 분포의 차나 형상의 차에 따른 양이다.

여기에서, 변수 패러미터(n(i), d(i))의 현재값에 의해 규정되는 평면이 실바닥면 부분영역(Ar(i))을 포함하는 평면과 정밀도 좋게 합치하고 있을 경우에는, STEP 515에서 구한 변환화상은 기준화상에서의 부분화상 영역(Ac(i)) 내의 화상에 일치 또는 거의 일치한다. 이 경우에는 오차량(e(k))은 0 혹은 미소한 것으로 된 다. 따라서, 오차량(e(k))은 (n(i), d(i))의 현재값에 의해 규정되는 평면(환언하면, (n(i), d(i))의 현재값에 의해 정해지는 사영변환 행렬(H(i))에 의해 규정되는 평면)과, 실바닥면 부분영역(Ar(i))을 포함하는 평면 사이의 차이의 정도를 나타내는 지표이다. 바꾸어 말하면, 오차량(e(k))은 (n(i), d(i)) 현재값의 참값에 대한 오차를 나타내는 지표이다.

이어서, STEP 517로 진행되고, 제어 장치(33)는 현재의 오차량(e(k))이 전회 루프 내 연산 처리에서의 STEP 516에서 구한 오차량인 갱신 전 오차량(e(k-1))보다도 작은 것인지 아닌지를 판단한다. 또한, 금회 루프 내 연산 처리가, 1회째일 경우에는, 오차량(e(k))과 비교하는 갱신 전 오차량(e(k-1))은 상기 STEP 513에서 초기화된 값이다. 따라서, 이 경우에는, STEP 517의 판단 결과는 항상 긍정적으로 된다.

보충하면, 금회 루프 내 연산 처리가 1회째인 경우에는, STEP 517의 판단 처리를 생략하고, STEP 518부터의 처리를 실행하도록 해도 된다. 그 경우에는, STEP 513의 처리는 불필요하다.

STEP 517의 판단 결과가 긍정적인 경우에는, STEP 518로 진행되고, 제어 장치(33)는 n(i), d(i)의 각각의 수정량(Δn(i), Δd(i))을 구한다. 또한, 이들 수정량(Δn(i), Δd(i))은 오차량(e(k)) 등에 따라 결정된다.

이어서, STEP 519로 진행되고, 현재의 e(k)이 소정량보다도 작은지 아닌지, 즉, e(k)가 충분히 작은지 아닌지를 판단한다.

그리고, 이 판단 결과가 부정적일 경우에는, STEP 520으로 진행되고, 제어 장치(33)는 STEP 518에서 구한 수정량(Δn(i), Δd(i))이 각각 소정량보다도 작은지 아닌지를 판단한다.

이 판단 결과가 부정적일 경우에는, STEP 521로 진행되고, 제어 장치(33)는 변수 패러미터(n(i), d(i))의 현재값을 (ns(i), ds(i))의 값으로서 설정함(기억 유지함)과 아울러, 갱신 전 오차량(e(k-1))에 현재의 오차량(e(k))을 대입한다(현재의 오차량(e(k))을 다음회의 루프 내 연산 처리에 관한 갱신 전 오차량으로서 설정함).

또한, STEP 522로 진행되고, 제어 장치(33)는 변수 패러미터(n(i), d(i))의 현재값을 STEP 518에서 구한 수정량(Δn(i), Δd(i))에 의해 갱신한다. 이것에 의해, 금회 루프 내 연산 처리가 종료한다. 그리고, 제어 장치(33)는 STEP 514부터의 루프 내 연산 처리를 재개한다.

한편, STEP 517의 판단 결과가 부정적일 때, 또는, STEP 519의 판단 결과가 긍정적일 때, 또는 STEP 520의 판단 결과가 긍정적일 때에는, 루프 내 연산 처리는 종료되고(STEP 514∼522의 루프 처리로부터 빠져나감), 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 추정값이 확정된다. 바꾸어 말하면, 참조화상에서의 실바닥면 부분영역(Ar(i))의 화상(부분화상 영역(Ac(i)) 내의 화상)을 사영변환 행렬(H(i))에 의해 변환하여 이루어지는 변환화상을, 기준화상에서의 실바닥면 부분영역(Ar(i))의 화상(부분화상 영역(Ac(i)) 내의 화상)에 합치시킬 수 있는 것과 같은 사영변환 행렬(H(i))을 규정하는 변수 패러미터(n(i), d(i))의 값이 확정된다.

구체적으로는 금회 루프 내 연산 처리에서의 STEP 520의 판단 결과가 긍정적으로 되는 상황은, 변수 패러미터(n(i), d(i))가 거의, 어떤 값에 수렴하고 있고, (n(i), d(i))의 갱신을 더 반복해도, (n(i), d(i))의 값이 거의 변화되지 않는다고 생각되는 상황이다. 이 경우에는, STEP 523로 진행되고, 제어 장치(33)는 변수 패러미터(n(i), d(i))의 현재값을 금회 루프 내 연산 처리의 STEP 518에서 구한 수정량(Δn(i), Δd(i))에 의해 갱신한다. 그리고, 제어 장치(33)는, STEP 524에서, 이 갱신 후의 변수 패러미터(n(i), d(i))의 값을 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 추정값으로서 확정한다. 또한, 이 경우, STEP 523의 처리를 생략하고, 금회 루프 내 연산 처리의 STEP 514에서 사용한 변수 패러미터(n(i), d(i))의 값을 그대로, 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 추정값 으로 해도 된다.

또, 금회 루프 내 연산 처리에서의 STEP 519의 판단 결과가 긍정적으로 되는 상황은 금회 루프 내 연산 처리에서의 STEP 514에서 사용한 변수 패러미터(n(i), d(i))에 의해 규정되는 평면이 실바닥면 부분영역(Ar(i))을 포함하는 평면에 충분히 일치하고 있다고 생각되는 상황이다. 이 경우에는, 상기 STEP 524로 진행되고, 제어 장치(33)는 변수 패러미터(n(i), d(i))의 현재값, 즉, 금회 루프 내 연산 처리의 STEP 514에서 사용한 (n(i), d(i))를 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 추정값으로서 확정한다.

또, 금회 루프 내 연산 처리에서의 STEP 517의 판단 결과가 부정적일 경우에는, 루프 내 연산 처리를 더 반복해도, 변수 패러미터(n(i), d(i))가 발산할 가능 성이 있다. 그래서, 이 경우에는, STEP 525로 진행되고, 제어 장치(33)는, 전회 루프 내 연산 처리에서의 STEP 521에서 설정한 (ns(i), ds(i)), 즉 전회 루프 내 연산 처리에서의 STEP 514에서 사용한 변수 패러미터(n(i), d(i))의 값을 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 추정값으로 한다.

이상이, STEP 5의 처리의 상세이다. 이 STEP 5의 처리에 의해, 각 화상투영 영역(A(i))에 대응하는 부분화상 영역(Ac(i)) 내에 화상으로서 포착되어 있는 실바닥면, 즉, 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))의 추정값이 얻어지게 된다.

여기에서, 상기 STEP 502의 판단 결과가 긍정적일 경우에, (n(i), d(i))의 초기값(n0(i), d0(i))에 대응하는 평면(πa(i))을 상기한 바와 같이 설정한 이유를 이하에 설명한다.

본원 발명자는 다음과 같은 실험을 행했다. 즉, 형상 패러미터(n(i), d(i))가 알고 있는 실바닥면 부분영역(Ar(i))(로봇(1)의 전방의 어떤 위치에서의 영역)에 대하여, 초기값(n0(i), d0(i)) 중의, n0(i)를 실바닥면 부분영역(Ar(i))의 실제의 법선벡터(n(i))에 일치시킨 경우(평면(πa(i))를 실바닥면 부분영역(Ar(i))의 평면과 평행하게 한 경우)와, n0(i)를 실바닥면 부분영역(Ar(i))의 실제의 법선벡터(n(i))에 대하여 기울인 경우(평면(πa(i))를 실바닥면 부분영역(Ar(i))의 평면에 대하여 기울인 경우)로, d0(i)의 값을 복수의 값으로 설정하고, 상기 STEP 512∼524의 수렴연산 처리를 실행했다. 그리고, 각각의 경우에서, 수렴연산 처리가 완료될 때까지의 상기 루프 내 연산 처리의 실행 회수를 계측했다.

그 계측결과의 일례를 도 12(a), (b)에 도시한다. 도 12(a)는 초기값(n0(i), d0(i))에 대응하는 평면(πa(i))을 실바닥면 부분영역(Ar(i))의 평면과 평행하게 한 경우에 있어서의, d0(i)의 설정값의 오차(실제의 d(i)에 대한 오차)와, 루프 내 연산 처리의 실행 회수의 관계를 도시하는 그래프, 도 12(b)는 초기값(n0(i), d0(i))에 대응하는 평면(πa(i))을 실바닥면 부분영역(Ar(i))에 대하여 45도만큼 기울인 경우에 있어서의, d0(i)의 설정값의 오차(실제의 d(i)에 대한 오차)와, 루프 내 연산 처리의 실행 회수의 관계를 나타내는 그래프이다. 또한, 이들 예에서는, 실바닥면 부분영역(Ar(i))은 수평면이다. 또, 어느 도면에서도, 횡축은 d0(i)의 설정값의 오차, 종축은 루프 내 연산 처리의 실행 회수이다.

도 12(a)에 도시하는 바와 같이, 평면(πa(i))을 실바닥면 부분영역(Ar(i))의 평면과 평행하게 한 경우에는, d0(i)의 설정값의 오차가 커짐에 따라, 루프 내 연산 처리의 회수가 급격하게 증가한다. 또, d0(i)의 설정값의 오차가 어느 정도 커지면, 변수 패러미터(n(i), d(i))를 어떤 값에 수렴시킬 수 없게 된다. 이에 반해, 도 12(b)에 도시하는 바와 같이, 평면(πa(i))을 실바닥면 부분영역(Ar(i))의 평면에 대하여 기울인 경우에는, d0(i)의 설정값의 오차가 커져도, 루프 내 연산 처리의 회수는 그다지 증가하지 않고, 게다가, 변수 패러미터(n(i), d(i))를 어떤 값에 수렴시킬 수 있다. 이것으로부터, 루프 내 연산 처리의 실행 회수를 가능한 한 적게 하면서, 효율적으로 실제의 (n(i), d(i))를 추정하기 위해서는, 초기값(n0(i), d0(i))에 대응하는 평면(πa(i))은 실바닥면 부분영역(Ar(i))의 평면에 대하여 기울여 두는 것이 바람직하다고 생각된다. 그리고, 이와 같이 한 경우에 는, d0(i)의 설정값은 실제의 d(i)의 값에 충분히 가까운 값일 필요가 없이, 비교적 대략적인 값이며 된다.

한편, 본 실시형태에서의 로봇(1)과 같은 다리식 이동 로봇에서는, 일반적으로, 이 로봇을 이동시키려고 하는 바닥면의 경사각도(수평면에 대한 경사각도)는, 이 로봇의 관절 등의 기구적인 제약이나 사양상의 제약에 의해, 어떤 허용 범위 내의 경사각도로 제한된다. 또, 로봇의 동작 환경이 미리 결정된 환경으로 제한되어 있는 경우에는, 그 환경 내의 바닥면의 경사각도의 범위는 미리 정해져 있다. 이와 같이, 로봇을 이동시키려고 하는 바닥면의 경사각도가 취할 수 있는 값은, 어떤 범위 내의 값이며, 그 범위는, 일반적으로 이미 알고 있다. 그리고, 로봇을 이동시키려고 하는 바닥면의 경사각도는, 일반적으로는, 겨우, 10도 정도이다.

따라서, 초기값(n0(i), d0(i))에 대응하는 평면(πa(i))을 로봇을 이동시키려고 하는 바닥면의 경사각도가 취할 수 있는 값의 범위를 일탈하는 것과 같은 각도로 기울이도록 하면, 평면(πa(i))을, 사실상, 항상 실바닥면 부분영역(Ar(i))의 평면에 대하여 기울일 수 있다. 예를 들면, 평면(πa(i))을 수평면에 대하여 45도 기울이도록 하면, 평면(πa(i))을, 사실상, 항상 실바닥면 부분영역(Ar(i))의 평면에 대하여 기울일 수 있다.

이러한 것으로부터, 본 실시형태에서는, STEP 502의 판단 결과가 긍정적일 경우에, 초기값(n0(i), d0(i))에 대응하는 평면(πa(i))이 실바닥면 부분영역(Ar(i))의 평면에 대하여 경사지는 것을 초기값(n0(i), d0(i))이 충족시켜야 할 제약조건으로 하고, 그 제약조건을 충족시키도록, 평면(πa(i))을 수평면에 대하여 소정 각도(θ)만큼 기울이는 것으로 했다. 그리고, 그 소정 각도(θ)의 예로서 45도를 채용했다.

또한, θ는 상기한 바와 같이 바닥면의 경사각도가 취할 수 있는 값의 범위를 일탈하고 있으면 되는데, 바람직하게는, 이 범위 내의 임의의 경사각도와의 각도차의 절대값이, 어떤 소정값 이상으로 되는 경사각도인 것이 바람직하다. 본 실시형태에서는, θ=45도라고 했으므로, 실바닥면의 경사각도가 겨우, 10도라고 한 경우, 평면(πa(i))의 경사각도와 실바닥면 부분영역(Ar(i))의 평면의 경사각도와의 차는 35도 이상으로 된다. 단, 평면(πa(i))의 경사각도와 실바닥면 부분영역(Ar(i))의 평면의 경사각도의 차는, 반드시, 35도 이상일 필요는 없으며, 예를 들면, 20도 이상이어도 된다.

이상에서 설명한 STEP 5의 처리를 각 실바닥면 부분영역(Ar(i))마다 실행함으로써 각 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))를 효율적으로 단시간에 추정할 수 있다.

또한, 본 실시형태에서는, STEP 502의 판단 결과가 긍정적일 경우에만, STEP 505∼STEP 510의 처리에 의해 초기값(n0(i), d0(i))을 설정하도록(πa(i)를 설정하도록) 했는데, STEP 502의 판단 결과가 부정적으로 되는 경우에도, STEP 505∼STEP 510과 동일한 처리를 실행하여, 초기값(n0(i), d0(i))을 설정하도록 해도 된다. 바꾸어 말하면, STEP 502∼STEP 504의 처리를 생략해도 된다.

도 3의 설명으로 되돌아와, 제어 장치(33)는 다음에 STEP 6의 처리를 실행한다. 이 STEP 6에서는, 제어 장치(33)는, 각 실바닥면 부분영역(Ar(i))의 형상 패 러미터(n(i), d(i))의 추정값에 필터링(1차 지연 필터 등)을 시행하여, 노이즈 성분을 제거한다. 이것에 의해, 형상 패러미터(n(i), d(i))의 값이 노이즈 등의 영향으로 과잉하게 변동하는 것을 방지한다. 또한, 이후의 설명에서는, 각 실바닥면 부분영역(Ar(i)) 마다의 형상 패러미터(n(i), d(i))는 이 STEP 6의 필터링 처리를 시행한 추정값을 의미하는 것으로 한다.

이어서, 제어 장치(33)는 STEP 7∼16의 처리를 실행하고, 금회 프레임의 처리를 종료한다. 이 STEP 7∼16의 처리에서는, 로봇(1)의 진행방향 전방의 실바닥면의 경사면 에지(경사면 개시 에지 또는 경사면 종료 에지)의 특정 등이 행해진다.

구체적으로는, 우선, STEP 7에서, 제어 장치(33)는 각 실바닥면 부분영역(Ar(i))(i=1, 2, ……, N)을, 이하에 설명하는 평가함수가 최소가 되도록, 앞쪽 그룹과 안쪽 그룹의 2개의 그룹으로 분류한다. 앞쪽 그룹은 로봇(1)에 의해 가까운 쪽의 그룹을 의미하고, 안쪽 그룹은 앞쪽 그룹보다도 로봇(1)으로부터 먼 쪽의 그룹을 의미한다. STEP 7의 처리는, 달리 말하면, STEP 2에서 착지 예정 위치를 결정한 1보째로부터 N보째까지 중에서, 어떤 보수째(K보째)를 이하에 설명하는 평가함수가 최소가 되도록 결정하고, 실바닥면 부분영역(Ar(i))(i=1, 2, ……, N)을 1보째부터 K보째까지의 착지 예정 위치에 대응하는 실바닥면 부분영역(Ar(1)∼Ar(K))으로 이루어지는 앞쪽 그룹과, K+1보째부터 N보째까지의 착지 예정 위치에 대응하는 실바닥면 부분영역(Ar(K+1)∼Ar(N))으로 이루어지는 안쪽 그룹으로 분류하는 처리이다.

그 분류를 위해 사용하는 평가함수로서, 본 실시형태에서는, 다음 식 (2)에 의해 표시되는 평가함수를 사용한다.

또한, 식 (2) 중의 mavf는, 보다 정확하게는, 1보째부터 K보째까지의 실바닥면 부분영역의 형상 패러미터(n(i), d(i))(i=1∼K)의 법선벡터(n(1)∼n(K))로부터, 구면선형보간(slerp)에 의해 구해지는 중심 벡터(법선벡터(n(1)∼n(K))의 평균의 방향을 갖는 단위 벡터)를 평면 거리(d(1)∼d(K))의 가중평균값에 의해 나누어서 이루어지는 벡터이다. 마찬가지로, mavb는 K보째부터 N보째까지의 실바닥면 부분영역의 형상 패러미터(n(i), d(i))(i=K∼N)의 법선벡터(n(K+1)∼n(N))로부터, 구면선형보간(slerp)에 의해 구해지는 중심 벡터(법선벡터(n(K+1)∼n(N))의 평균의 방향을 갖는 단위 벡터)를 평면 거리(d(K+1)∼d(N))의 가중평균값에 의해 나누어서 이루어지는 벡터이다.

이 식 (2)에서의 우변의 제1항은 Ar(1)∼Ar(K)의 각각 형상 패러미터의 편차로서의 의미를 가지며, 우변의 제2항은 Ar(K+1)∼Ar(N)의 각각의 형상 패러미터의 분산(편차 정도)으로서의 의미를 갖는다. 그리고, STEP 7의 처리에서는, 이 평가함수의 값이 최소가 되도록 K의 값을 정하고, 상기한 바와 같이 실바닥면 부분영 역(Ar(i))(i=1, 2, ……, N)을 앞쪽 그룹과 안쪽 그룹 2개의 그룹으로 분류한다. 이와 같이, Ar(1)∼Ar(N)을 앞쪽 그룹과, 안쪽 그룹으로 분류함으로써, 각각의 그룹에서의 형상 패러미터의 분산이 최소가 되도록, 바꾸어 말하면, 각각의 그룹에서의 형상 패러미터의 상호의 근사정도가 가장 높아지도록, Ar(1)∼Ar(N)이 분류되게 된다.

이와 같이 실바닥면 부분영역(Ar(i))(i=1, 2, ……, N)을 앞쪽 그룹과 안쪽 그룹으로 분류했을 때, 예를 들면 도 5에 도시하는 상황에서는, 식 (2)의 평가함수가 최소가 되는 K의 값은 「5」가 되고, 1보째부터 5보째까지의 실바닥면 부분영역(Ar(1)∼Ar(5))이 앞쪽 그룹, 6보째부터 7보째까지의 실바닥면 부분영역(Ar(6), Ar(7))이 안쪽 그룹으로서, 실바닥면 부분영역이 분류되게 된다. 또, 예를 들면 도 7에 도시하는 상황에서는, 5보째의 화상투영 영역(A(5))에 대응하는 실바닥면 부분영역(Ar(5)) 내에 경사면 에지(경사면 종료 에지)가 존재하므로, 식 (2)의 평가함수가 최소가 되는 K의 값은 「4」 또는 「5」가 된다. 이 경우에는, 5보째의 실바닥면 부분영역(Ar(5))은 앞쪽 그룹에 속하는 경우와, 안쪽 그룹에 속하는 경우가 있다. 단, 1보째부터 4보째까지의 실바닥면 부분영역(Ar(1)∼Ar(4))이 앞쪽 그룹에 속하고, 6보째부터 7보째까지의 실바닥면 부분영역(Ar(6), Ar(7))은 안쪽 그룹에 속한다.

이어서, STEP 8로 진행되고, 제어 장치(33)는, 상기한 바와 같이 분류한 각 그룹에 속하는 실바닥면 부분영역의 형상 패러미터의 대표값을 구하고, 그 대표값으로부터, 앞쪽 그룹에 대응하는 평면(이하, 앞쪽 대표평면이라고 함)과 안쪽 그룹 에 대응하는 평면(이하, 안쪽 대표평면이라고 함)의 각도차를 산출한다. 또한, 앞쪽 대표평면은, Ar(1)∼Ar(K)의 각각을 포함하는 평면을 대표적으로 근사하는 평면으로서의 의미를 가지며, 안쪽 대표평면은 Ar(K+1)∼Ar(N)의 각각을 포함하는 평명을 대표적으로 근사하는 평면으로서의 의미를 갖는다.

구체적으로는, STEP 8에서는, 앞쪽 그룹에 속하는 Ar(1)∼Ar(K)의 각각의 형상 패러미터에서의 법선벡터(n(1)∼n(K))의 중심 벡터(navf)와, 평면 거리(d(1)∼d(K))의 가중평균값(davf)의 세트(navf, davf)가 앞쪽 그룹의 형상 패러미터의 대표값으로서 구해진다. 또한, 이 경우, navf/davf는 식 (2)에서의 mavf와 동일하다.

마찬가지로, 안쪽 그룹에 속하는 Ar(K+1)∼Ar(N)의 각각의 형상 패러미터에서의 법선벡터(n(K+1)∼n(N))의 중심 벡터(navb)와, 평면 거리(d(K+1)∼d(N))의 가중평균값(davb)의 세트(navb, davb)가 안쪽 그룹의 형상 패러미터의 대표값으로서 구해진다. 또한, 이 경우, navb/davb는 식 (2)에서의 mavb와 동일하다.

그리고, 제어 장치(33)는 상기한 바와 같이 구한(navf, davf), (navb, davb)에 의해 각각 규정되는 평면을 앞쪽 대표평면, 안쪽 대표평면으로 하고, 이들 평면의 각도차를 산출한다.

보충하면, STEP 8에서 구하는 각도차는 앞쪽 대표평면과 안쪽 대표평면의 차이의 정도를 의미하는 지표로서의 의미를 갖는다. 그 지표로서는, 양 평면의 각도차 이외에, 상기 가중평균값(davf, davb)의 차나, 상기 식 (2)에 관하여 설명한 mavf와 mavb의 벡터공간에서의 거리 등을 사용해도 된다.

이어서, STEP 9로 진행되고, 제어 장치(33)는 STEP 8에서 구한 각도차가 소정의 임계값 이상인지 아닌지를 판단한다. 이 판단 결과가 긍정적일 경우에는, STEP 10으로 진행되고, 제어 장치(33)는 앞쪽 대표평면과 안쪽 대표평면의 교차 개소에 경사면 에지가 존재하고 있는 것으로 가정하여, 임시 경사면 에지를 산출한다. 구체적으로는, 제어 장치(33)는 앞쪽 대표평면을 규정하는 형상 패러미터의 대표값(navf, davf)과, 안쪽 대표평면을 규정하는 형상 패러미터의 대표값(navb, davb)으로부터, 그것들의 평면의 교선을 산출하고, 그것을 임시 경사면 에지로 한다.

또한, 이 임시 경사면 에지는 본 발명에서의 임시 경사 변화부에 상당하는 것이다.

이어서, STEP 11로 진행되고, 제어 장치(33)는 앞쪽 대표평면과 안쪽 대표평면에 관한 형상 패러미터의 대표값과, 경사면 에지를 재계산한다. 구체적으로는, 제어 장치(33)는 앞쪽 그룹에 속하는 실바닥면 부분영역(Ar(1)∼Ar(K))으로부터, 임시 경사면 에지와의 거리가 소정 거리보다도 짧은 실바닥면 부분영역(임시 경사면 에지에 근접해 있는 실바닥면 부분영역)을 제외한 것을 고쳐서 앞쪽 그룹으로 한다. 마찬가지로, 안쪽 그룹에 속하는 Ar(K+1)∼Ar(N)으로부터, 임시 경사면 에지의 거리가 소정 거리보다도 짧은 실바닥면 부분영역을 제외한 것을 고쳐서 안쪽 그룹으로 한다. 이렇게 하는 것은, 임시 경사면 에지에 근접해 있는 실바닥면 부분영역은 실바닥면 중의 경사가 상이한 2개의 평면에 걸쳐 있을 가능성이 있기 때문이다. 그리고, 이와 같이 고쳐서 정한 앞쪽 그룹의 각 실바닥면 부분영역의 형 상 패러미터로부터, 상기 STEP 8과 동일한 수법으로 그것들의 형상 패러미터의 대표값(navf, davf)을 구하고, 그것을 고쳐서, 앞쪽 대표평면의 형상 패러미터의 대표값으로 한다. 마찬가지로, 고쳐서 정한 안쪽 그룹의 각 실바닥면 부분영역의 형상 패러미터로부터, 상기 STEP 8과 동일한 수법으로 그것들의 형상 패러미터의 대표값(navb, davb)을 구하고, 그것을 바꾸어, 안쪽 대표평면의 형상 패러미터의 대표값으로 한다. 그리고, 제어 장치(33)는, 고쳐서 정한 (navf, davf), (navb, davb)에 의해 각각 규정되는 앞쪽 대표평면과 안쪽 대표평면의 교선을 경사면 에지로서 구한다.

또한, 이렇게 하여 구해지는 앞쪽 대표평면과, 안쪽 대표평면은 추정 바닥면으로서의 의미를 갖는다. 보다 상세하게는, 로봇(1)에서 보아, 경사면 에지보다도 앞쪽의 추정 바닥면이 앞쪽 대표평면에 의해 표시되며, 경사면 에지보다도 안쪽의 추정 바닥면이 안쪽 대표평면에 의해 표시된다. 이 경우, STEP 8의 처리에서, 임시 경사면 에지에 근접한 실바닥면 부분영역을 제외하고, 앞쪽 대표평면과 안쪽 대표평면을 규정하는 형상 패러미터 (navf, davf), (navb, davb)를 고쳐서 구하므로, 경사면 에지의 앞쪽 및 안쪽의 추정 바닥면의 정밀도를 높일 수 있음과 아울러, 이 경사면 에지를 정밀도 좋게 특정할 수 있다.

이어서, STEP 12로 진행되고, 제어 장치(33)는 경사면 패러미터를 갱신한다. 이 경사면 패러미터는 로봇(1)의 진행 경로에 따른 추정 바닥면의 거시적인 형상을 나타내는 패러미터이다. STEP 12에서는, 보다 구체적으로는, STEP 11에서 구해진 경사면 에지에 따라, 로봇(1)의 현재 위치에서의 추정 바닥면(로봇(1)의 현재의 접 지 개소 부근의 추정 바닥면에서 평면상의 바닥면. 이하, 현재 접지 추정 바닥면이라고 함)의 종료 위치(로봇(1)의 진행방향 전방의 종료 위치)를 나타내는 경사면 패러미터가 갱신된다.

또, 로봇(1)에서 보아, 현재 접지 추정 바닥면의 종료 위치보다 안쪽의 추정 바닥면(현재 접지 추정 바닥면과 경사 정도가 상이한 바닥면)의 개시 위치 및 그 경사 정도를 나타내는 경사면 패러미터가 STEP 11에서 구해진 안쪽 대표평면을 규정하는 형상 패러미터(navb, davb)에 따라 갱신된다.

한편, STEP 9의 판단 결과가 부정적일 경우에는, STEP 13∼STEP 16의 처리가 실행된다. 여기에서, STEP 9의 판단 결과가 부정적으로 되는 상황은, 주로, 각 실바닥면 부분영역(Ar(i))(i=1, 2, ……, N)이 거의 동일 평면 상에 존재하고 있는 것과 같은 상황이다. 단, 로봇(1)의 진행방향 전방의 실바닥면의 경사 정도가 서서히 변화되고 있는 것과 같은 경우에는, STEP 9에 판단 결과가 부정적으로 되는 것과 같은 경우도 있다. 이러한 상황에 대응하기 위하여, STEP 9에 판단 결과가 부정적인 경우에는, 제어 장치(33)는 이하에 설명하는 것과 같은 처리를 실행한다.

즉, 우선, STEP 13에서, 제어 장치(33)는 각 실바닥면 부분영역(Ar(i))(i=1, 2, ……, N)을 소정의 위치에서 앞쪽 그룹과 안쪽 그룹으로 분류한다. 구체적으로는, 제어 장치(33)는, 예를 들면, 1보째부터 미리 정한 보수째(Kx보째. 예를 들면, 3보째)까지의 실바닥면 부분영역(Ar(1)∼Ar(Kx))을 앞쪽 그룹, Kx+1보째부터 N보째까지의 실바닥면 부분 영역(Ar(Kx+1)∼Ar(N))을 안쪽 그룹으로 한다. 또한, 보수로 분류하는 대신, 예를 들면, 로봇(1)의 진행방향에서, 이 로봇(1)의 현재 위 치로부터의 거리가 소정 거리 이하가 되는 화상투영 영역(A(1)∼A(Kx))에 대응하는 실바닥면 부분영역을 앞쪽 그룹, 다른 실바닥면 부분영역(Ar(Kx+1)∼Ar(N))을 안쪽 그룹으로 하도록 해도 된다.

이어서, STEP 14로 진행되고, 제어 장치(33)는 로봇(1)의 현재의 동작 제어를 위해 사용하고 있는 이 로봇(1)의 현재의 접지 개소 부근의 현상 상정 바닥면(이하, 현재 접지 상정 바닥면이라고 함)과, STEP 13에서 결정된 안쪽 그룹에 대응하는 평면(안쪽 대표평면)의 각도차를 산출한다. 구체적으로는, STEP 14에서는, 안쪽 그룹에 속하는 Ar(Kx+1)∼Ar(N)의 각각의 형상 패러미터에서의 법선벡터(n(Kx+1)∼n(N))의 중심 벡터(navb)와, 평면 거리(d(Kx+1)∼d(N))의 가중평균값(davb)의 세트(navb, davb)에 의해 규정되는 평면을 안쪽 대표평면으로서 구한다. 그리고, 이 안쪽 대표평면과, 현재 접지 상정 바닥면의 각도차를 산출한다.

보충하면, STEP 14에서 구하는 각도차는 현재 접지 상정 바닥면과, 안쪽 대표평면의 차이의 정도를 나타내는 지표로서의 의미를 갖는다. 그 지표는 STEP 8의 경우와 마찬가지로, 각도차 이외의 지표를 사용해도 된다.

또, 예를 들면 본원 출원인이 일본 특허공개 2001-322076호 공보에 제안한 것과 같이, 로봇의 접지 개소 부근의 바닥면 형상을 상반력의 검출값 등을 기초로 추정하도록 한 경우에는, 그 추정된 바닥면 형상에 의해 표시되는 바닥면을 STEP 14의 처리에서 현재 접지 상정 바닥면으로서 사용하도록 해도 된다.

이어서, STEP 15로 진행되고, 제어 장치(33)는 STEP 14에서 구한 각도차의 절대값이 소정의 임계값 이상인지 아닌지의 여부를 STEP 9와 마찬가지로 판단한다.

그리고, 이 판단 결과가 부정적일 경우에는, 안쪽 대표평면은 현재 접지 상정 바닥면과 거의 동등한 경사 정도를 갖고, 이 현재 접지 상정 바닥면에 연속한 평면이라고 생각된다. 그리고, 이 경우에는, 제어 장치(33)는 추정 바닥면에 관한 경사면 패러미터를 갱신하지 않고, 금회 프레임의 처리를 종료한다.

또, STEP 15의 판단 결과가 긍정적일 경우에는, 제어 장치(33)는 안쪽 대표평면이 현재 접지 상정 바닥면에 대하여 경사진 평면이라고 간주하고, 경사면 에지를 산출한다. 이 경우, 예를 들면, Kx보째의 착지 예정 위치에서의 족평(17)의 발뒤꿈치의 위치와, Kx+1보째의 착지 예정 위치에서의 족평(17)의 발뒤꿈치의 위치와의 중점을 통과하고 로봇(1)의 진행방향에 직교하는 선을 경사면 에지로 한다.

이어서, STEP 12로 진행되고, 추정 바닥면에 관한 경사면 패러미터를 갱신한다. 보다 구체적으로는, STEP 16에서 구해진 경사면 에지에 따라, 현재 접지 추정 바닥면의 종료 위치(로봇(1)의 진행방향 전방의 종료 위치)를 나타내는 경사면 패러미터가 갱신된다. 또, 로봇(1)에서 보아, 현재 접지 추정 바닥면의 종료 위치보다 안쪽의 추정 바닥면(현재 접지 추정 바닥면과 경사 정도가 상이한 바닥면)의 개시 위치 및 경사 정도를 나타내는 경사면 패러미터가, STEP 14에서 구해진 안쪽 대표평면을 규정하는 형상 패러미터(navb, davb)에 따라 갱신된다. 이 경우, 안쪽 대표평면은 로봇(1)에서 보아, 경사면 에지보다도 안쪽의 추정 바닥면으로서의 의미를 갖는다.

이상이 본 실시형태에서의 바닥면 형상 추정 처리의 상세하다.

본 실시형태에서는, 로봇(1)의 동작 제어에 사용하는 현상 상정 바닥면은 상 기한 바와 같이 결정되는 추정 바닥면의 경사면 패러미터에 따라 적당하게 갱신된다. 이 경우, 본 실시형태에서는, 로봇(1)의 동작 제어의 안정성을 높이기 위하여, 현재 접지 상정 바닥면의 경사 정도에 관한 패러미터는 현상에 유지된다. 그리고, 현재 접지 상정 바닥면보다도 안쪽(현재 접지 추정 바닥면의 종료 위치보다도 안쪽)의 현상 상정 바닥면에 대해서는, 이 현상 상정 바닥면과 추정 바닥면의 경사 정도의 차가 소정값 이상(예를 들면, 1.5도 이상)일 경우에, 당해 안쪽의 현상 상정 바닥면의 경사면 에지의 위치나 경사 정도에 관한 패러미터가, 추정 바닥면에 합치하도록 갱신된다. 또한, 현재 접지 상정 바닥면의 경사 정도에 관한 경사면 패러미터를, 현상에 유지하는 것은 필수는 아니며, 현재 접지 추정 바닥면의 경사 정도에 맞추어 갱신하도록 해도 된다.

이상에서 설명한 본 실시형태의 로봇(1)에서는, 로봇(1)의 복수보분의 착지 예정 위치에 대응하는 실바닥면 부분영역(Ar(i))마다, 카메라(27L, 27R)의 촬상화상을 사용하여 각 실바닥면 부분영역(Ar(i))의 형상 패러미터(n(i), d(i))를 추정한다. 즉, 카메라(27L, 27R)의 촬상화상 내의 실바닥면 중, 로봇(1)의 이동동작에 필요한 부분영역에 한정하여, 실바닥면의 형상의 추정이 행해진다. 이 때문에, 카메라(27L, 27R)의 촬상화상에 의한 바닥면의 형상을 인식하기 위한 연산 처리를 경감하면서, 로봇(1)의 적절한 이동 제어를 행할 수 있다.

또한, 이상에서 설명한 실시형태에서는, 2족 보행 로봇(1)을 예로 들어 설명했지만, 3개 이상의 다리체를 갖는 다리식 이동 로봇에 대해서도 본 발명을 적용할 수 있는 것은 물론이다.

또, 상기 실시형태에서는, 물체가 카메라를 탑재한 로봇을 이동시키는 바닥일 경우를 예로 들어 설명했지만, 본 발명의 적용 대상은 이것에 한정되는 것은 아니다. 예를 들면, 카메라를 탑재한 차량이 주행하는 도로를 물체로 하고, 그 도로의 노면 형상을 추정하는 것과 같은 경우에도, 본 발명을 적용할 수 있다.

도 1은 본 발명의 1실시형태에서의 다리식 이동 로봇인 2족 보행 로봇의 외관 사시도.

도 2는 도 1의 로봇에 구비한 제어 시스템의 개요 구성을 도시하는 블럭도.

도 3은 실시형태에서의 바닥면 형상의 추정 처리를 나타내는 플로우차트.

도 4는 도 3의 STEP 5의 처리의 상세를 나타내는 플로우차트.

도 5는 로봇의 족평의 복수보분의 착지 예정 위치와 화상투영 영역과의 설정예를 설명하기 위한 도면.

도 6(a), (b)는 로봇의 족평의 복수보분의 착지 예정 위치와 화상투영 영역의 설정예를 설명하기 위한 도면.

도 7은 로봇의 족평의 복수보분의 착지 예정 위치와 화상투영 영역의 설정예를 설명하기 위한 도면.

도 8(a), (b)는 로봇의 족평의 복수보분의 착지 예정 위치와 화상투영 영역의 설정예를 설명하기 위한 도면.

도 9는 로봇의 족평과 화상투영 영역의 관계를 도시하는 도면.

도 10은 화상투영 영역을 촬상화상에 투영한 상태를 도시하는 도면.

도 11은 실바닥면 부분영역의 형상 패러미터를 추정하기 위한 수렴연산 처리에서의 형상 패러미터의 초기값의 설정예를 설명하기 위한 도면.

도 12(a), (b)는 실바닥면 부분영역의 형상 패러미터를 추정하기 위한 수렴연산 처리에서의 형상 패러미터의 초기값의 설정 수법을 설명하기 위한 그래프.

Claims (4)

1. 2개의 카메라의 촬상화상 중에 포착되어 있는 물체의 평탄부를 관측하기 위하여, 상기 2개의 카메라 중 일방의 카메라의 촬상화상 내의 물체의 화상을 양 카메라 사이의 사영변환 행렬에 의해 변환하여 이루어지는 변환화상과, 타방의 카메라의 촬상화상 내의 물체의 화상을 비교하면서, 이 사영변환 행렬의 소정 종류의 패러미터의 값에 의해 규정되는 평면을 상기 물체의 평탄부를 포함하는 평면에 수렴시키도록, 이 패러미터의 값을 갱신해 가는 수렴연산 처리를 실행하는 사영변환 수렴연산 처리 방법에 있어서,

   상기 수렴연산 처리에서의 상기 패러미터의 초기값에 의해 규정되는 평면이 상기 물체의 평탄부를 포함하는 실제의 평면에 대하여 경사진 평면으로 되는 것을 이 초기값이 충족시켜야 할 제약조건으로 하고, 이 초기값을 이 제약조건을 충족시키는 값으로 설정한 것을 특징으로 하는 사영변환 수렴연산 처리 방법.
2. 제 1 항에 있어서, 상기 패러미터의 초기값에 의해 규정되는 평면은, 이 평면의 수평면에 대한 경사각도가, 상기 물체의 평탄부를 포함하는 실제의 평면의 수평면에 대한 경사각도가 취할 수 있는 값의 범위로부터 일탈하도록 미리 정해진 소정 각도가 되는 평면인 것을 특징으로 하는 사영변환 수렴연산 처리 방법.
3. 2개의 카메라와, 당해 2개의 카메라의 촬상화상 중에 포착되어 있는 물체의 평탄부를 관측하기 위하여, 상기 2개의 카메라 중 일방의 카메라의 촬상화상 내의 물체의 화상을 양 카메라 사이의 사영변환 행렬에 의해 변환하여 이루어지는 변환화상과, 타방의 카메라의 촬상화상 내의 물체의 화상을 비교하면서, 이 사영변환 행렬의 소정 종류의 패러미터의 값에 의해 규정되는 평면을 상기 물체의 평탄부를 포함하는 평면에 수렴시키도록, 이 패러미터의 값을 갱신해 가는 수렴연산 처리를 실행하는 사영변환 수렴연산 처리 장치를 구비하고 있는 로봇에 있어서,

   상기 수렴연산 처리에서의 상기 패러미터의 초기값에 의해 규정되는 평면이 상기 물체의 평탄부를 포함하는 실제의 평면에 대하여 경사진 평면으로 되는 것을 이 초기값이 충족시켜야 할 제약조건으로 하고, 이 초기값을 이 제약조건을 충족시키는 값으로 설정한 것을 특징으로 하는 로봇.
4. 제 3 항에 있어서, 상기 패러미터의 초기값에 의해 규정되는 평면은, 이 평면의 수평면에 대한 경사각도가, 상기 물체의 평탄부를 포함하는 실제의 평면의 수평면에 대한 경사각도가 취할 수 있는 값의 범위로부터 일탈하도록 미리 정해진 소정 각도가 되는 평면인 것을 특징으로 하는 로봇.

Images