KR101112500B1 - 이동 로봇의 제어장치 - Google Patents

Abstract

기체(53)와, 이것에 연결된 복수의 링크기구(52, 54)를 구비한다. 기체(53)와 링크기구(다리)(52)의 선단부(족평)(58) 사이의 소정의 부위(무릎)에 외력을 검출하는 센서(90)가 설치되어 있다. 로봇(61)이 소정의 부위(무릎)를 접지시킨 동작 자세에서, 센서(90)로 검출한 외력(상반력)을 목표 외력에 근접시키도록 적어도 기체(53)와 소정의 부위(무릎) 사이의 관절(55)의 변위를 제어한다. 로봇(61)의 다리나 팔의 선단부 이외의 부분이, 바닥 등과 접촉하여 외력을 받는 상태에서 다리나 팔의 선단부 이외의 부분에 작용하는 외력을 적절하게 제어하여 로봇(61)의 자세를 안정하게 유지한다.

링크기구, 관절, 다리식 이동 로봇의 제어장치, 외력 검출수단, 목표 외력 결정수단, 관절 변위 제어수단.

Description

이동 로봇의 제어장치{CONTROL DEVICE FOR MOBILE BODY}

본 발명은 2족이동 로봇 등의 이동 로봇의 제어장치에 관한 것으로, 보다 상세하게는, 무릎, 팔꿈치, 동체 등, 로봇의 다리나 팔의 선단부 이외의 부분을 접지시키는 경우 등에 이동 로봇에 작용하는 상반력(床反力)(외력)을 제어하는 장치에 관한 것이다.

2족이동 로봇 등의 다리식 이동 로봇을, 그 다리의 선단부를 접지시키거나 이상(離床)시키거나 하면서 이동시키는 경우에, 그들 다리의 선단부에 작용하는 상반력을 제어하는 기술로서는, 본원 출원인에 의한 일본 특개평5-305585호 공보(특허문헌 1), 일본 특개평10-277969호 공보(특허문헌 2), 일본 특개2001-322076호 공보(특허문헌 3)에 개시되는 기술이 알려져 있다.

그러나, 이들 기술은, 원래, 다리의 선단부를 바닥에 접지시키고 로봇의 이동을 행하는 것을 상정한 것으로, 상반력을 검출하는 센서를 다리의 선단부에 구비하고 있다. 또, 팔을 구비한 로봇에 어떤 작업을 행하게 하는 경우에, 그 팔의 선단부에 작용하는 힘을 제어하기 위해서, 이 팔의 선단부에 힘 센서를 구비한 것도 알려져 있다.

이들 종래의 로봇에서는, 원래, 다리나 팔의 선단부에 상반력 등의 외력이 작용하는 것을 상정하고 있고, 다리나 팔의 선단부 이외의 개소, 예를 들면 무릎에는, 외력을 검출하는 센서가 구비되어 있지 않다. 이 때문에, 무릎에 작용하는 외력(상반력 등)을 제어할 수는 없었다.

예를 들면 종래의 인간형 로봇을 무릎을 꿇게 한 상태에서는, 좌우의 무릎에 작용하는 상반력은 바닥의 요철에 의존(지배)되고, 또한 바닥의 요철에 따라, 다리의 관절각을 조절하는 것이 행해지고 있지 않았다. 이 때문에, 바닥의 요철에 의해, 로봇의 바닥과의 접지부위가 흔들리는 등, 그 접지부위의 접지상태가 불안정하게 되는 경우도 있었다. 그리고, 이러한 상태에서는, 로봇의 동작이나 작업을 행하려고 하면 로봇의 자세가 불안정하게 되는 경우가 있었다.

또, 예를 들면 로봇이 무릎을 꿇은 상태로부터 일어서는 경우에, 실제의 바닥의 경사가 목표 보용에서 상정하고 있던 경사와 상이한 경우에는, 로봇의 중심위치가 적절하게 제어되지 않아, 로봇의 상체의 자세가 기운 채 일어서려고 해버리기 때문에, 이 로봇이 전도되버릴 우려도 있었다.

또, 로봇이 의자에 앉은 채, 그 자세를 안정하게 유지하면서 동작이나 작업을 행하는 경우에서, 의자로부터 일어설 때, 또는 의자에 앉을 때에, 로봇의 자세를 적절한 자세로 복원할 수 있도록 하기 위해서, 로봇의 각 다리의 족평(足平)에 작용하는 상반력 뿐만 아니라, 의자로부터 로봇의 둔부에 작용하는 반력도 적절하게 제어할 수 있는 것이 바람직하다.

본 발명은 이러한 배경을 감안하여 이루어진 것으로, 이동 로봇의 무릎, 팔꿈치, 동체, 둔부 등, 다리나 팔의 선단부 이외의 부분이, 바닥 또는 바닥의 연장 물이라고 생각되는 대상물과 접촉하여 외력을 받는 상태에서, 로봇의 다리나 팔의 선단부에 작용하는 외력뿐만 아니라, 다리나 팔의 선단부 이외의 부분에 작용하는 외력도 적절하게 제어하여, 이동 로봇의 자세를 안정하게 유지하는 것을 가능하게 하는 이동 로봇의 제어장치를 제공하는 것을 목적으로 한다.

본 발명의 이동 로봇의 제어장치의 제 1 발명은, 기체(基體)와, 이 기체의 이동을 위해 이 기체에 연결되어 외부와 접촉하는 복수의 링크기구를 구비함과 동시에, 이 기체와 각 링크기구의 선단부 사이에, 이 기체에 대해 각 링크기구의 선단부를 가동으로 하는 복수의 관절이 설치되고, 상기 복수의 링크기구 중 1개 이상의 특정 링크기구의 선단부와 상기 기체 사이의 소정 부위에서 외부와 접촉하는 특정 동작 자세로 동작할 수 있는 다리식 이동 로봇의 제어장치에 있어서,

상기 특정 동작 자세에서 상기 소정 부위에 작용하는 외력을 검출 또는 추정하는 외력 검출수단과,

상기 특정동작 자세에서의 상기 소정 부위의 외력의 목표값인 목표 외력을 결정하는 목표 외력 결정수단과,

상기 검출 또는 추정된 외력이 상기 목표 외력에 근접하도록 적어도 상기 소정 부위와 상기 기체 사이에 존재하는 관절의 변위를 제어하는 관절 변위 제어수단을 구비한 것을 특징으로 하는 것이다.

이러한 제 1 발명에 의하면, 상기 특정 자세 동작에서, 상기 소정 부위가 외부와 접촉한다. 이 소정 부위는, 상기 기체와 상기 특정 링크기구의 선단부 사이에서 이 특정 링크기구에 설치되어 있는 부위이지만, 그 소정 부위에 작용하는 외력은 상기 외력 검출수단에 의해 검출 또 추정된다. 그리고, 이 검출 또는 추정된 외력이 상기 목표 외력 결정수단으로 결정한 목표 외력에 근접하도록, 적어도 상기 소정 부위와 상기 기체 사이에 존재하는 관절의 변위를 제어함으로써, 이 소정 부위에 작용하는 외력을 적절하게 제어할 수 있다. 그 결과, 로봇의 자세를 안정하게 유지하는 것이 가능하게 된다.

이러한 제 1 발명에서는, 상기 특정 링크기구는 예를 들면 다리체이다(제 2 발명). 이 경우에는, 상기 소정 부위는 이 다리체의 선단부와 기체 사이의 중간부위(예를 들면 무릎 등)가 된다. 따라서, 예를 들면 인간형 로봇의 무릎꿇기 자세에서, 그 무릎에 작용하는 외력을 제어할 수 있다. 또한, 로봇에 구비하는 상기 링크기구는, 다리체에만 한하지 않고, 팔체를 포함하고 있어도 된다. 그 경우, 상기 특정 링크기구는 다리체만이어도 되지만, 팔체만이어도 되고, 또는, 다리체와 팔체 양자를 포함하고 있어도 된다.

또, 상기 제 1 발명(또는 제 1, 제 2 발명)에서는, 예를 들면, 상기 특정 링크기구는, 상기 기체에 관절을 통하여 연결된 둔부로부터 연장하여 설치된 다리체이며, 상기 소정의 부위는 상기 둔부이다(제 3 발명). 이 경우, 로봇이 걸터앉는 경우 등에서, 둔부에 작용하는 외력을 제어하여 로봇의 자세(특히 상체 등의 기체의 자세)를 적절한 안정자세로 제어할 수 있다.

또한, 상기 제 1 발명(또는 제 1~제 3 발명)에서는, 상기 이동 로봇의 상기 기체 등의 제 2 소정 부위의 실 자세를 검출하는 실 자세 검출수단과, 이 제 2 소정 부위의 목표 자세를 결정하는 목표 운동 결정수단을 구비하고, 상기 목표 외력 결정수단은 적어도 상기 제 2 소정 부위의 실 자세와 목표 자세의 편차에 따라 상기 목표 외력을 결정하는 것이 바람직하다(제 4 발명).

이 제 4 발명에 의하면, 상기 제 2 소정 부위(예를 들면 기체)의 실 자세를 소요의 목표 자세에 복원하도록 상기 소정 부위에 작용하는 외력을 제어할 수 있으므로 로봇의 자세의 안정성을 높일 수 있다.

또한 제 1 발명(또는 제 1~제 4 발명)에서는, 상기 이동 로봇의 상기 기체 등의 제 2 소정 부위의 실 자세를 검출하는 실 자세 검출수단과, 이 제 2 소정 부위의 목표 자세를 결정하는 목표 운동 결정수단을 구비하고, 상기 관절 제어수단은, 상기 제 2 소정 부위의 실 자세와 목표 자세의 편차에 따라 외력의 조작량을 결정하는 수단을 구비하고, 상기 목표 외력과 상기 외력의 조작량의 합력에 상기 검출 또는 추정된 외력을 근접시키도록 상기 관절의 변위를 제어하는 것이 바람직하다(제 5 발명).

이 제 5 발명에 의하면, 상기 소정 부위에 작용하는 외력을 목표 외력에 근접시키면서, 상기 제 2 소정 부위(예를 들면 기체)의 실 자세를 목표 자세에 근접시키도록 관절의 변위를 제어하므로, 로봇의 자세를 안정화 함과 동시에, 소정 부위에 작용하는 외력을 적절하게 제어할 수 있다.

또한, 제 4 발명 및 제 5 발명에서의 제 2 소정 부위는, 상기 특정 동작 자세에서 외부와 접촉하는 소정 부위와 동일해도 된다.

도 1은, 본 발명의 제 1 참고예 및 제 2 참고예에 따른 로봇의 전체 외관도, 도 2는 제 1 참고예의 제어장치의 기능적 구성을 도시하는 블럭도, 도 3(a)~(c) 및 도 4(a)~(c)는, 제 1 참고예의 로봇(4다리 로봇)의 동작을 설명하기 위한 도면, 도 5(a)~(c)는 제 1 참고예에서의 가중값의 설정예를 도시하는 그래프, 도 5(d)는 제 1 참고예에서의 ZMP(목표 전체 상반력 중심점)의 설정예를 도시하는 그래프이다. 도 6은 제 1 참고예의 노드의 계층구조를 도시하는 도면, 도 7은 목표 노드 상반력 중심점과 가중값의 관계를 도시하는 도면, 도 8은 목표 노드 상반력과 가중값의 관계를 도시하는 도면, 도 9는 제 1 참고예의 제어장치의 메인루틴 처리를 도시하는 플로우차트이다. 도 10~도 14는 각각 제 1 참고예의 로봇(4다리 로봇)의 목표 노드 상반력 병진력 성분, 실 노드 상반력 병진력 성분, 실 노드 상반력 모멘트, 보상 전체 상반력 모멘트, 노드 보상 상반력 모멘트를 예시하는 도면이다. 도 15 및 도 16은 제 1 참고예에서의 노드 보상각에 의한 위치 수정(컴플라이언스 동작)을 설명하기 위한 도면, 도 17~도 21은 각각 제 1 참고예에서의 계층형 컴플라이언스 동작 결정부, 보상 전체 상반력 모멘트 분배기, θ1423 결정부(보상각 결정부), θ14 결정부(보상각 결정부), 기구변형 보상량 산출부의 기능적 구성을 나타내는 블럭도, 도 22는 보상각을 결정하는 처리를 도시하는 플로우차트, 도 23은 제 1 참고예의 로봇에 관계되는 계층구조의 다른 예를 설명하기 위한 도면이다. 도 24는 제 2 참고예의 로봇(6다리 로봇)의 계층구조를 설명하기 위한 도면, 도 25는 제 2 참고예에서의 계층형 컴플라이언스 동작 결정부의 기능적 구성을 나타내는 블럭도, 도 26~도 28은 각각 제 2 참고예의 로봇(6다리 로봇)의 목표 노드 상반력의 병진력 성분, 실 노드 상반력의 병진력 성분, 실 노드 상반력 모멘트를 예시하는 도면, 도 29(a), (b)는 제 2 참고예에서의 노드 보상 상반력 모멘트의 설정수법을 설명하기 위한 도면, 도 30 및 도 31은 제 2 참고예에서의 노드 보상각에 의한 위치 수정(컴플라이언스 동작)을 설명하기 위한 도면이다. 도 32 및 도 33은 각각 제 2 참고예에서의 θ145236 결정부(보상각 결정부), θ145 결정부(보상각 결정부)의 기능을 나타내는 블럭도, 도 34는 제 2 참고예의 로봇에 관계되는 계층구조의 다른 예를 설명하기 위한 도면이다. 도 35는 제 3 참고예에서의 계층형 컴플라이언스 동작 결정부의 기능적 구성을 나타내는 블럭도, 도 36은 제 3 참고예에서의 제어장치의 메인루틴 처리를 도시하는 플로우차트, 도 37~도 39는 제 3 참고예에서의 개념 및 용어를 설명하기 위한 도면, 도 40은 제 3 참고예에서의 바닥형상 추정기의 기능을 나타내는 블럭도, 도 41은 도 40에서의 바닥높이 편차 추정처리 서브루틴의 처리를 도시하는 플로우차트이다. 도 42 및 도 43은 각각 4다리 로봇, 6다리 로봇에서의 계층 상대화의 처리를 설명하기 위한 도면이다. 도 44는 바닥형상 추정에 관계되는 각 노드의 요구 모드의 설정예를 도시하는 도면, 도 45~도 55는 바닥형상 추정 처리를 도시하는 플로우차트이다. 도 56은 제 4 참고예에서의 바닥형상 추정기의 기능을 나타내는 블럭도, 도 57은 제 5 참고예에서의 바닥형상 추정기의 기능을 나타내는 블럭도이다. 도 58 및 도 59는 제 1 실시형태에서의 로봇(2족 이동 로봇)이 무릎을 꿇은 상태를 도시하는 도면, 도 60은 제 1 실시형태에서의 로봇의 무릎의 상반력 센서의 구성을 도시하는 도면, 도 61은 제 1 실시형태에서의 노드의 계층구조를 도시하는 도면, 도 62는 제 1 실시형태에서의 계층형 컴플라이언스 동작 결정부의 기능적 구성을 도시하는 블럭도이다. 도 63(a)~(c)는 제 1 실시형태에서의 로봇의 상체의 자세의 수정 수법을 설명하기 위한 도면, 도 64는 제 1 실시형태에서의 로봇의 상체의 위치 자세의 수정 수법을 설명하기 위한 도면, 도 65는 제 1 실시형태에서의 역키네마틱스 연산부의 기능을 나타내는 블럭도이다. 도 66은 제 2 실시형태에서의 역키네마틱스 연산부의 기능을 나타내는 블럭도, 도 67 및 도 68은 제 2 실시형태에서의 로봇의 자세의 수정 수법을 설명하기 위한 도면이다. 도 69는 제 3 실시형태에서의 로봇의 구성을 도시하는 도면, 도 70은 제 3 실시형태에서의 노드의 계층구조를 도시하는 도면이다.

발명을 실시하기 위한 최량의 형태

먼저, 본 발명의 실시형태의 설명에서 사용하는 주 용어에 대해 설명해 둔다.

본원 출원인이 앞서 제안한 일본 특개평10-277969호 공보의 복합 컴플라이언스 제어의 기술에서는, 로봇의 다리 선단부(족평)로부터 상반력을 받는 것을 전제로 하여, 다리 마다 상반력을 분별했다. 이에 반해, 본 명세서의 참고예에서는, 예를 들면 인간형 로봇이 꿇어앉은 상황이나 걸터앉은 상황, 팔을 접지시킨 상황 등, 다리 선단부 이외로부터도 상반력을 받는 상황도 고려한다. 이 때문에, 다리식 이동 로봇의 소요 동작에 있어서 바닥과 접하여 반력을 받는 이 로봇의 부위를 「접지부위」라고 부른다. 이 경우, 「바닥」은, 통상의 의미에서의 바닥(또는 지면)뿐만 아니라, 이 바닥(또는 지면)에 고정된 의자(로봇이 걸터앉을 의자) 등과 같이, 로봇이 그 동작에 있어서 접촉하여 반력을 받는 대상물도, 「바닥」에 포함되는 것으로 한다. 따라서, 예를 들면 2족 이동 로봇이 의자 등에 걸터앉을 때에는, 이 로봇의 허리부는 접지부위에 해당하는 것이 된다. 또한, 다리식 이동 로봇의 통상적인 이동시(2족 이동 로봇의 보행시 등)에는, 각 다리의 선단부(족평 등)가 접지부위에 상당하는 것이 되는 것은 물론이다.

접지부위를 분류(구별)할 때에는, 로봇의 동일한 링크(단일 강체에 상당하는 부분)상에서 접지부위가 복수 개소로 분리되어 분포하고 있을 경우(동일한 링크의, 서로 분리된 복수의 부분이 접지해 있는 경우)에서도, 즉 동일 링크상에서 복수의 접지면이 연결되어 있지 않아도, 이것들을 모아서 하나의 접지부위라고 정의한다. 예를 들면, 스파이크 핀이 4개 장착된 접지부위가 그들 스파이크 핀을 통하여 접지하는 경우에는, 4개의 스파이크 핀의 각각의 접지면을 모아서 하나의 접지부위로 간주한다. 단, 그들 접지면을 반드시 하나로 모을 필요는 없다.

제 n 접지부위란, 이러한 룰을 기초로 분류(구별)되어 식별 번호 n(n=1,2,3,…)이 배정된 접지부위의 식별 명칭이다.

접지부위 상반력이란, 접지부위에 작용하는 상반력으로 하고, 특히, 제 n 접지부위에 작용하는 상반력을 제 n 접지부위 상반력이라고 한다. 또, 전체 상반력이란 모든 접지부위에 작용하는 상반력의 합력으로 한다. 또한 상반력 중심점이란, 상반력이 그 점에 발생하는 모멘트의 수평성분이 0이 되는 것과 같은 작용점으로 한다.

또한, 접지부위 상반력, 전체 상반력 등의 상반력은, 일반적으로는, 힘의 작 용점과, 그 작용점에 걸리는 병진력 및 모멘트의 세트에 의해 표현되고, 동일한 상반력에 대해, 그것을 표현하는 작용점과, 병진력 및 모멘트의 세트는 무한에 있다. 그 중에는, 연직축 주위의 성분(연직성분)을 제외한 모멘트 성분(모멘트의 수평성분)이 0이 되는 점을 작용점으로서 사용하는 상반력의 표현이 존재한다. 이 표현에서의 작용점을 본 명세서에서는 상반력 중심점이라고 한다. 또, 상반력 중심점을 상반력이 그 점에 발생하는 모멘트의 바닥면 평행성분(바닥면에 평행한 성분)이 0이 되는 점으로서 정의해도 된다.

또한, 이 명세서에서 「바닥면」은, 본원 출원인이 앞서 제안한 일본 특개평5-318340호 공보에 기재되는 가상적인 바닥면(목표 보용상에서 상정되는 바닥면으로서, 실 바닥면과는 반드시 일치한다고는 할 수 없음)어어도 된다.

보충하면, 접지부위의 상반력 중심점은 그 접지부위가 접지하고 있는 상태에서는, 통상, 그 접지면(바닥과의 접촉면)상에 설정된다. 한편, 접지부위가 공중을 이동하고 있을 때의 접지부위 상반력은 항상 0이므로, 어느 점을 작용점으로 해도 이 접지부위 상반력의 모멘트 수평성분은 0이다. 즉, 상반력 중심점은 임의로 설정할 수 있다. 단, 로봇의 동작 제어를 원활하게 행하는 점에서는, 목표로 하는 상반력 중심점은 연속적으로 이동하는 것이 바람직하다. 그래서, 본 명세서의 실시형태에서는, 접지부위 상반력의 상반력 중심점은 접지부위가 공중을 이동할 때에, 접지부위와 함께 이동하는 작용점으로 한다.

본 명세서의 실시형태에서는, 적어도 실 상반력(로봇에 실제로 작용하는 상반력)에 따라 각 접지부위의 위치 자세의 수정을 행하는 컴플라이언스 제어동작에 서, 단순히, 각 접지부위 마다, 서로 무관하게(독립하여) 컴플라이언스 제어동작을 행하는 것은 아니다. 본 명세서의 실시형태는, 각 접지부위를 트리구조로 분류하고, 적어도 분류된 각 그룹에 작용하는 실 상반력(각 그룹에 포함되는 모든 접지부위에 작용하는 실 상반력 혹은 그 합력)에 기초하여, 접지부위의 위치 자세의 수정동작을 결정하는 것에 특징이 있다. 또한, 이후, 트리구조로 분류하는 것을 「계층화」라고 부르는 경우도 있다.

본 명세서의 실시형태에서의 다리식 이동 로봇의 보용생성장치가 생성하는 변수의 목표값에는, 변수명의 머리에 「목표」를 붙인다. 실제의 다리식 이동 로봇에 있어서의 당해 변수의 값(참 값은 모르므로, 실제로는, 검출값 또는 추정값)에는, 변수명의 머리에 「실」을 붙인다. 상기 「실 상반력」은 그 예이다.

후술하는 컴플라이언스 제어(상반력 제어)의 전체 상반력(로봇의 모든 접지부상에 작용하는 실 상반력의 합력)의 목표를 목표 전체 상반력이라고 부른다. 목표 전체 상반력의 모멘트 수평성분이 0이 되는 점을 목표 전체 상반력 중심점이라고 부른다.

이동 로봇의 목표 보용의 운동(목표 보용에서의 로봇의 각 부위의 운동)이 발생하는 관성력과 이 로봇에 작용하는 중력의 합력의 모멘트 수평성분이 0이 되는 바닥면상의 점을 목표 ZMP라고 부른다. 또한, 목표 보용의 운동은 목표 보용에서의 로봇의 각 부위의 목표 위치 자세의 시계열에 의해 정해지므로, 그 각 부위의 목표 위치 자세의 시계열을 총칭적으로 목표 보용의 운동 혹은 목표 운동이라고 한다. 보충하면, 이들 시계열을 규정할 수 있으면, 상기와 같이 정의한 목표 운동과 상이한 표현으로 목표 운동을 기술해도 된다. 예를 들면, 로봇의 각 관절의 목표 변위의 시계열과 기체 등의 특정 부위의 목표 위치 자세의 시계열의 세트를 목표 운동으로 해도 된다.

목표 전체 상반력은, 통상, 목표 보용의 운동 패턴(목표 운동의 시계열 패턴)에 대해 동역학적으로 평형을 이루는 전체 상반력이다. 따라서, 통상, 목표 전체 상반력 중심점은 목표 ZMP와 일치한다. 그래서, 이하의 설명에서는, 많은 경우, 목표 전체 상반력 중심점과 목표 ZMP를 구별하지 않고 사용한다. 단, 예외적으로, 본원 출원인이 앞서 제안한 특허 제3269852호 등에서 설명하고 있는 로봇의 제어의 경우에는 목표 전체 상반력 중심점과 목표 ZMP와는 일치한다고는 할 수 없다. 이하의 설명에서는, 목표 ZMP라는 단어를 사용하는 경우도 있는데, 엄밀하게는 목표 전체 상반력 중심점이라고 해야 할 개소도 있다.

또한, 이동 로봇의 목표 보용에서 이 로봇이 상반력 이외의 반력(외력)을 환경으로부터 받도록 상정해도 된다. 그리고, 그 상반력 이외의 반력(외력)을, 예를 들면 목표 대상물 반력이라고 칭하고, 앞서 기술한 목표 ZMP의 정의를 다음과 같이 확장해도 된다. 즉 로봇의 목표 보용의 운동 패턴에 의해 발생하는 관성력과 이 로봇에 작용하는 중력과 목표 대상물 반력과의 합력을 동역학적으로 구하고, 그 합력이 바닥면상의 어떤 점에 발생하는 모멘트가, 연직축 주위의 성분을 제외하고 0이면, 그 점을 다시 고쳐서 목표 ZMP로 하도록 해도 된다. 단, 목표 대상물 반력도, 상반력의 하나의 형태로서 취할 수 있으면, 목표 ZMP의 정의는, 앞서 기술한 정의와 같아도 된다.

이상을 전제로 하여, 우선, 본 발명에 관련되는 참고예 및 본 발명의 실시형태를 이하에 상세하게 설명한다. 또한, 본 명세서에서는, 설명의 편의상, 우선, 잠시동안, 참고예를 설명하고, 그 후에 본 명세서의 실시형태를 설명한다. 참고예는 제 1~제 5 참고예까지이다.

[제 1 참고예]

도 1은, 제 1 및 제 2 참고예에 따른 일반적인 복수 다리의 이동 로봇(다리식 이동 로봇)의 외관도이다. 단, 도 1에서는, 로봇(1)은, 제 1 다리(#1)~제 6 다리(#6)까지의 6개의 다리(다리체)를 갖는 것으로서 도시하고 있는데, 제 1 참고예에서는, 제 5 다리(#5) 및 제 6 다리(#6)는 없는 것으로 한다. 즉, 제 1 참고예에서는, 로봇(1)은 제 1 다리(#1)~제 4 다리(#4)의 4개의 다리(다리체)를 갖는 4다리 로봇인 것으로 한다. 도 1에서는, 제 2 참고예에 따른 로봇(1)의 구성요소에 대해서는, 괄호붙인 참조부호를 붙이고 있다.

도 1에 도시하는 바와 같이, 제 1 참고예의 로봇(1)(4다리 로봇)에서는, 이 로봇(1)의 기체인 상체(24)의 우측부로부터 2개의 다리(제 1 다리(#1), 제 3 다리(#3))가 전후 방향으로 줄지어서 연장되어 설치되고, 이와 마찬가지로 상체(24)의 좌측부로부터 2개의 다리(제 2 다리(#2), 제 4 다리(#4))가 전후 방향으로 줄지어서 연장되어 설치되어 있다. 각 다리(#1~#4)의 선단부에는, 프리 조인트인 볼 관절(12)을 통하여 원반 형상의 접지부위(10)가 부착되어 있다. 이후, 접지부위(10)를 각 다리(#1~#4) 마다 구별할 필요가 있을 때는, 각각을 제 1 접지부위, 제 2 접지부위, 제 3 접지부위, 제 4 접지부위라고 한다.

각 접지부위(10)는, 그 중심점을 볼 관절(12)의 중심점에 일치시켜 이 볼 관절(12)에 걸어맞추어져 있고, 이 접지부위(10)의 중심점(엄밀하게는 볼 관절(12))에는, 상반력 모멘트(상반력의 모멘트 성분)가 작용하지 않도록 되어 있다. 즉 접지부위(10)의 중심점의 상반력 모멘트(실 상반력 모멘트)는 항상 0이 된다.

도시한 로봇(1)에서는, 각 다리(#1~#4)는, 로봇(1)의 상체(24) 근처의 부분과 중간부에 각각 관절(14, 15)을 구비함과 동시에, 각 다리(#1~#4)의 선단부 근방(볼 관절(12)과 관절(14)을 연결하는 링크의 볼 관절(12) 근처의 부분)에, 스프링 등의 탄성부재로 구성된 컴플라이언스 기구(42)와, 접지부위(10)에 작용하는 실 상반력을 검출하는 상반력 검출수단(외력 검출수단)으로서의 6축력센서(34)가 개재되어 있다. 도시의 예에서는, 관절(14)은 2축 주위의 회전이 가능하게 되고, 관절(15)은 1축 주위의 회전이 가능하게 되어 있다. 또한, 컴플라이언스 기구로 하여, 접지부위의 밑바닥면에 스폰지나 고무 등의 탄성체를 형성해도 된다.

각 관절(14, 15)의 변위동작(각 축 주위의 회전동작)은, 도시를 생략하는 전동 모터 등의 액추에이터에 의해 행해진다. 그리고, 각 관절(14, 15)의 실제의 변위량(각 축 주위의 회전각)인 실 관절 변위가, 도시하지 않은 로터리 인코더 등의 센서에 의해 검출된다. 또한, 6축력센서(34)는, 3축방향의 병진력과 3축 주위의 모멘트를 검출가능한 것이지만, 제 1 참고예의 로봇(1)에서는, 상기한 바와 같이 접지부위(10)의 중심점에 실 상반력 모멘트가 작용하지 않는다. 따라서, 6축력센서(34) 대신에, 3축방향의 병진력을 검출하는 3축력센서, 또는, 실 상반력의 병진력의 연직성분만을 검출하는 힘 센서를 사용해도 된다.

상체(24)에는, 마이크로컴퓨터나 액추에이터 구동회로 등을 포함하는 전자회로 유닛으로 구성된 제어장치(50)와, 상체(24)의 자세를 검출하는 자세 센서(36)와, 도시하지 않은 전원(2차전지, 캐패시터 등)이 내장되어 있다. 자세 센서(36)는, 예를 들면 가속도 센서와 자이로 센서로 구성되어 있다. 또한, 본 명세서에서는, 「자세」는 일반적으로는 공간적인 방향을 의미한다(단, 로봇의 전체의 「자세」는 로봇의 운동의 순시값을 의미함). 그리고, 본 참고예에서는, 자세 센서(36)는 상체(24)의 자세 중, 예를 들면 연직방향에 대한 피칭 방향 및 롤링 방향의 자세 경사(경사각)를 검출한다. 즉, 자세 센서(36)는 상체(24)의 실 자세 경사를 검출하는 실 자세 경사 검출수단으로서 기능한다.

또한, 로봇(1)의 상체(24)의 요잉 방향의 자세(회전각)도 제어하는 경우에는, 상체(24)의 요잉 방향의 회전각도(환언하면 상체(24)의 3축방향의 자세를) 자세 센서(36)로 검출하도록 해도 된다.

도 2는 상기 제어장치(50)의 기능적 구성 및 동작을 도시하는 블럭도이다. 또한, 도 2에서는, 도 1의 로봇(1)으로부터, 제어장치(50), 자세 센서(36) 및 6축력센서(34)를 제외한 것을 실 로봇(1)이라고 칭하고 있다. 여기에서, 상기 도 1에 도시한 바와 같이, 로봇(1)의 대략 전방방향을 X축, 대략 좌측 횡방향을 Y축, 상방향을 Z축으로 한 바닥에 고정된 소정의 좌표계(XYZ 좌표계)를 「지지 다리 좌표계」 또는 「글로벌 좌표계」라고 부른다. 이후, 특별히 예고하지 않는 한, 위치, 자세, 병진력 및 모멘트는 이 지지 다리 좌표계(글로벌 좌표계)의 좌표성분의 세트에 의해 표현되는 것으로 한다. 지지 다리 좌표계(글로벌 좌표계)의 원점은, 정상 적으로 단일의 점에 고정되어 있을 필요는 없고, 로봇(1)의 이동 도중에 바닥에 대한 원점위치를 변경해도 된다. 예를 들면, 로봇(1)의 소정의 다리가 착지할 때마다 지지 다리 좌표계(글로벌 좌표계)의 원점위치를 변경해도 된다.

도 2에 도시하는 바와 같이, 제어장치(50)는, 그 기능적 구성요소(기능적 수단)로서, 보용생성장치(100), 목표 상반력 분배기(102), 로봇 기하학 모델(역키네마틱스 연산부)(110), 계층형 컴플라이언스 동작 결정부(114), 변위 콘트롤러(112), 실 상반력 검출기(108), 자세편차 연산부(103), 및 자세안정화 제어 연산부(104)를 구비하고 있다. 이하에, 제어장치(50)의 이들 요소의 개요를 설명한다.

보용생성장치(100)는, 본 발명에서의 목표 보용 결정수단 또는 목표 운동 결정수단으로서의 기능을 갖고, 로봇(1)의 동작을 규정하는 목표 보용을 생성(결정)하여 출력한다. 목표 보용은, 본 참고예에서는, 로봇의 목표 운동의 궤도(로봇의 각 부위의 목표 위치 자세의 시계열)와 목표 상반력의 궤도(로봇이 바닥으로부터 받는 반력의 작용점의 목표 위치와 그 작용점에 걸리는 병진력 및 모멘트의 목표값 세트의 시계열)로 구성된다. 또한, 본 명세서에서는, 「궤도」는 시계열 패턴(시간적 변화의 패턴)을 의미한다.

보용생성장치(100)가 출력하는 목표 운동의 궤도는, 로봇(1)의 각 접지부위(10)의 위치 및 자세의 목표값의 궤도인 목표 접지부위 궤도와, 로봇(1)의 상체(24)의 위치 및 자세의 목표값의 궤도인 목표 상체위치 자세 궤도로 구성된다. 또한, 후술하는 제 1 실시형태와 같이, 팔이나 머리부에 관련되는 관절을 구비하는 로봇에서의 보용생성장치는 팔이나 머리부의 목표 위치 자세 궤도도 목표 운동의 구성요소로서 결정하여 출력한다.

또, 보용생성장치(100)가 출력하는 목표 상반력의 궤도는, 본 참고예에서는, 로봇(1)의 전체 상반력 중심점의 목표 위치의 궤도인 목표 전체 상반력 중심점 궤도와, 이 목표 전체 상반력 중심점을 작용점으로 하는 전체 상반력의 목표값의 궤도인 목표 전체 상반력 궤도로 구성된다. 본 참고예에서는, 목표 전체 상반력 중심점 궤도는 ZMP의 목표 위치의 궤도인 목표 ZMP 궤도와 동일하게 되어 있다.

각 접지부위(10)의 위치는, 이 접지부위(10)의 어떤 대표점의 위치이며, 그 대표점은, 예를 들면 각 접지부위(10)의 중심점(볼 관절(12)의 중심점)의, 이 접지부위(10)의 접지면(밑바닥면)에의 연직방향의 투영점 또는 이 접지부위(10)의 중심점(볼 관절(12)의 중심점)으로 되어 있다. 이후, 각 접지부위(10)의 대표점의 위치를 R간단히 접지부위 위치라고 부른다. 이 접지부위 위치의 목표값의 궤도(목표 접지부위 위치궤도)가 보용생성장치(100)에 의해 결정되는 상기 목표 접지부위 궤도에 포함된다.

여기에서, 본 참고예의 로봇(1)에서는, 각 접지부위(10)는 프리 조인트인 볼 관절(12)에 걸어맞추어져 있으므로, 각 접지부위(10)의 자세를 제어할 수는 없다. 이 때문에, 본 참고예에서는, 보용생성장치(100)는 실제로는 각 접지부위(10)의 목표 자세의 궤도를 생성하지 않는다(생성할 필요가 없음). 따라서, 본 참고예에서는, 상기 목표 접지부위 궤도는 목표 접지부위 위치궤도와 동일한 것을 의미한다.

단, 후술하는 제 1 실시형태의 로봇과 같이, 각 접지부위가 각 다리의 선단부에 그 자세를 제어 가능하게 부착되어 있는 경우(각 다리의 선단부에 액추에이터 에 의해 동작가능한 관절을 통하여 접지부위가 부착되어 있는 경우)에는, 각 접지부위의 목표 자세의 궤도도 상기 목표 접지부위 궤도에 포함되어야 한다. 본 명세서에서는, 이러한 경우도 일반적으로 고려하기 위해서, 종종 「접지부위 위치 자세」라고 하는 용어를 사용하는데, 그것은, 본 참고예에서는, 실질적으로는 「접지부위 위치」에 대한 것을 의미한다.

도 3(a)~도 3(c) 및 도 4(a)~도 4(c)를 참조하여, 목표 접지부위 궤도(목표 접지부위 위치궤도) 및 목표 전체 상반력 중심점 궤도를 보다 구체적으로 설명한다. 제 1 참고예에서는, 다리(#1~#4) 중, 제 1 다리(#1) 및 제 4 다리(#4)의 세트를 지지 다리, 제 2 다리(#2) 및 제 3 다리(#3)의 세트를 미착지 다리로 하는 시기와, 모든 다리(#1~#4)를 지지 다리로 하는 시기와, 제 1 다리(#1) 및 제 4 다리(#4)의 세트를 미착지 다리, 제 2 다리(#2) 및 제 3 다리(#3)의 세트를 지지 다리로 하는 시기를 순서대로 반복하면서, 미착지 다리가 되는 다리의 세트를 이상시켜서 공중에서 이동시킨 후, 원하는 위치에 착지시킨다고 하는 다리(#1~#4)의 운동을 행함으로써 로봇(1)의 이동을 행하도록 하고 있다. 지지 다리는, 접지하여 로봇(1)의 자중을 지탱하는 다리(0이 아닌 상반력을 작용시켜야 할 다리)이며, 미착지 다리는 지지 다리가 아닌 다리이다.

도 3(a)~도 3(c) 및 도 4(a)~도 4(c)는, 상기의 바와 같이 로봇(1)을 이동시킬 때의 각 다리(#1~#4)의 선단의 목표 접지부위 위치(상세하게는 수평면(XY 평면)상에서의 위치)를 시계열적으로 순서대로(시각 t1~t6의 순으로) 도시한 것이다. 이들 도면 중의 참조부호 Q1~Q4에 대응하는 삼각형이, 각각 제 1~제 4 접지부 위(10)의 목표 접지부위 위치(제 1~제 4 접지부위(10)의 상기 대표점의 수평면(XY 평면)상에서의 위치)를 나타내고 있다.

보충하면, 도 3 및 도 4중의 참조부호 Q1~Q4를 붙인 삼각형은, 정확하게는, 각 접지부위(10)에 관계되는 후술의 목표 노드 상반력 중심점(목표 접지부위 상반력 중심점)의 위치를 나타내는 것이다. 단, 본 참고예에서는, 각 접지부위(10)의 대표점을 상기한 바와 같이 설정하고 있으므로, 제 1~제 4 접지부위(10)의 각각의 목표 접지부위 위치(대표점의 목표 위치)는, 후술하는 목표 노드 상반력 중심점 Qn(n=1,2,3,4)의 각각의 위치에 일치하거나, 혹은, 이 목표 노드 상반력 중심점 Qn(n=1,2,3,4)의 각각의 위치와 일정한 위치관계를 갖는다(수평면(XY 평면)상에서의 위치는 일치함). 이 때문에, 도 3 및 도 4중의 참조부호 Q1, Q2, Q3, Q4에 대응하는 삼각형은, 제 1~제 4 접지부위(10)의 각각의 후술의 목표 노드 상반력 중심점을 나타냄과 동시에, 제 1~제 4 접지부위(10)의 각각의 위치를 나타내고 있다.

도 3(a)는, 제 2 다리(#2) 및 제 3 다리(#3)의 세트를 지지 다리로 하고, 미착지 다리로서의 제 1 다리(#1) 및 제 4 다리(#4)의 세트를 착지시키는 순간(시각 t1)에서의 각 접지부위(10)의 목표 접지부위 위치를 도시하고, 도 3(b)는 모든 다리(#1~#4)가 지지 다리로 되어 있는 상태의 시각 t2에서의 목표 접지부위 위치를 도시하고, 도 3(c)는 제 1 다리(#1) 및 제 4 다리(#4)의 세트를 지지 다리로 하고, 미착지 다리로서의 제 2 다리(#2) 및 제 3 다리(#3)의 세트를 이상시키기(공중으로 들어 올리기) 직전의 시각 t3에서의 목표 접지부위 위치를 도시하고 있다. 또, 도 4(a)는 제 1 다리(#1) 및 제 4 다리(#4)의 세트를 지지 다리로 하고, 미착지 다리 로서의 제 2 다리(#2) 및 제 3 다리(#3)의 세트를 공중으로 들어 올린 상태의 시각 t4에서의 목표 접지부위 위치를 도시하고, 도 4(b)는 제 1 다리(#1) 및 제 4 다리(#4)의 세트를 지지 다리로 하고, 미착지 다리로서의 제 2 다리(#2) 및 제 3 다리(#3)의 세트를 착지시킨 순간(시각 t5)에서의 목표 접지부위 위치를 도시하고, 도 4(c)는 제 2 다리(#2) 및 제 3 다리(#3)의 세트를 지지 다리로 하고, 미착지 다리로서의 제 1 다리(#1) 및 제 4 다리(#4)의 세트를 이상시키기 직전의 시각 t6에서의 목표 접지부위 위치를 도시하고 있다. 또한, 도 4(a)에서는 미착지 다리인 제 2 다리(#2) 및 제 3 다리(#3)의 목표 접지부위 위치를 파선의 삼각형으로 나타내고 있다. 보충하면, 미착지 다리의 접지부위(10)의 연직방향(Z축방향)의 위치의 궤도는 바닥면으로부터 소정의 높이까지 상승한 후, 하강하고, 다시 착지하도록 결정된다.

이들 도 3 및 도 4에서의 점 P는, 목표 전체 상반력 중심점(목표 ZMP)을 나타내고 있다. 목표 전체 상반력 중심점 궤도는, ZMP의 존재가능 범위(소위 지지 다각형에 상당하는 바닥면상의 영역) 내에서 이 범위의 경계에 지나치게 근접하지 않는 위치(예를 들면 ZMP의 존재가능 범위의 거의 중앙위치)에 존재하면서, 연속적으로 이동하도록 결정된다. 제 1 참고예에서는, 2개의 다리(#1, #4 또는 #2, #3가 지지 다리가 될 때에는(도 3(a), (c), 도 4(a)~(c)를 참조), 그들 다리의 접지부위(10, 10)의 대표점을 연결하는 선분 상에서, 그 선분의 끝점에 지나치게 근접하지 않도록 목표 전체 상반력 중심점이 설정된다. 또, 모든 다리(#1~#4)가 지지 다리가 될 때에는(도 3(b)를 참조), 모든 접지부위(10)의 대표점을 꼭지각으로 하는 다각형 내에서, 이 다각형의 경계에 지나치게 접근하지 않도록 목표 전체 상반력 중심점이 설정된다. 도 5(d)는 이렇게 결정된 목표 전체 상반력 중심점(목표 ZMP)의 위치의 X축방향(로봇(1)의 진행방향)의 성분 ZMPx의 궤도를 예시한 그래프이다. 또한, 도 5(a)~(c)는 후술의 가중값의 설정예를 도시하는 그래프이다.

또, 보용생성장치(100)가 결정하는 목표 상체위치 자세 궤도는, 적어도, 로봇(1)의 목표 운동에 의해 발생하는 관성력과 로봇(1)에 작용하는 중력과의 합력이 목표 ZMP 주위에 작용하는 모멘트의 수평성분이 0이 되도록 로봇(1)의 동역학 모델 등을 사용하여 결정된다. 또한, 「상체위치」는 상체(24)의 어떤 대표점의 위치이다.

또, 보용생성장치(100)가 결정하는 목표 전체 상반력은, 목표 전체 상반력 중심점에 작용하는 병진력 및 모멘트의 목표값으로부터 구성되고, 본 참고예에서는, 로봇(1)의 목표 운동에 의해 발생하는 관성력과 로봇(1)에 작용하는 중력의 합력에 목표 전체 상반력 중심점에서 균형이 잡히도록 결정된다. 또한, 목표 전체 상반력 중심점(목표 ZMP) 주위의 목표 전체 상반력의 모멘트 수평성분은 0이다. 보충하면, 목표 전체 상반력 중심점에 작용하는 병진력 및 모멘트의 모든 성분을 목표값으로서 결정할 필요는 없다. 예를 들면 로봇(1)의 연직축 주위의 자세 혹은 상반력을 제어하지 않는 경우에는, 목표 전체 상반력의 모멘트의 연직축 주위의 성분(Z축방향 성분)을 결정할 필요는 없다.

또한, 보용생성장치(100)가 결정하는 목표 접지부위 궤도(목표 접지부위 위치궤도)는 후술하는 계층형 컴플라이언스 동작 결정부(114)에서 수정된다.

상기 목표 상반력 분배기(102)는 로봇(1)의 제 1~제 4 접지부위(10)를 트리구조 형상으로 그룹으로 나누고(즉 계층화 하고), 계층화 된 각 그룹에 트리구조의 노드를 대응시킨다. 따라서, 이하의 설명에서, 노드를 그룹으로 치환해서 표현해도 상관없다. 각 노드는 1개 이상의 접지부위(10)로 구성되는 그룹이다.

제 1 참고예에서는, 도 6에 도시하는 바와 같이 접지부위(10)가 계층화 된다. 즉, 제 n 접지부위(10)(n=1,2,3,4)를 제 n 노드에 대응시키고, 제 1 노드와 제 4 노드를 자 노드로서 갖는 노드를 제 14 노드로 하고, 제 2 노드와 제 3 노드를 자 노드로서 갖는 노드를 제 23 노드로 하고, 제 14 노드와 제 23 노드를 자 노드로서 갖는 노드를 제 1423 노드로 한다. 따라서, 제 1~제 4 노드는 각각 제 1, 제 2, 제 3, 제 4 접지부위(10)로 이루어지는 노드, 제 14 노드는 제 1 접지부위(10)와 제 4 접지부위(10)로 이루어지는 노드, 제 23 노드는 제 2 접지부위(10)와 제 3 접지부위(10)로 이루어지는 노드, 제 1423 노드는 모든 접지부위(10)로 이루어지는 노드이다.

트리구조에서의 일반적인 호칭법에 따라, 자 노드를 갖지 않는 노드를 잎 노드라고 부르고, 부모 노드를 갖지 않는 노드를 뿌리 노드라고 부른다. 따라서, 제 n 노드(n=1,2,3,4)는 잎 노드, 제 1423 노드는 뿌리 노드이다. 또한, 본 명세서에서는, 노드를 구별하기 위해서, 잎 노드에는, 그것에 대응하는 접지부위(혹은 다리)와 동일한 번호(1,2,3,…)를 할당하고, 잎 노드 이외의 노드에는, 잎 노드보다 큰 번호를 할당하는 것으로 한다. 또, 잎 노드 및 뿌리 노드 이외의 노드를 중간 노드라고 부른다. 제 1 참고예에서는, 중간 노드는 제 14 노드, 제 23 노드이다.

목표 상반력 분배기(102)에는 보용생성장치(100)에서 결정된 목표 보용 중, 목표 전체 상반력 중심점 궤도, 목표 전체 상반력 궤도 및 목표 접지부위 궤도가 입력된다. 또한, 보용생성장치(100)가 목표 보용을 결정하기 위해서 사용한 보용 패러미터(로봇(1)의 미착지 다리의 접지부위(10)의 착지 예정위치, 착지 예정시각 등)도 목표 상반력 분배기(102)에 입력해도 된다. 그리고, 목표 상반력 분배기(102)는, 이것들의 입력을 기초로, 상기한 바와 같이 계층화된 각 노드의 상반력 중심점의 목표 위치인 목표 노드 상반력 중심점(목표 제 n 노드 상반력 중심점)(Qn)(n=1,2,3,4,14,23,1423)과, 뿌리 노드를 제외한 각 노드의 가중값(Wn)(n=1,2,3,4,14,23)을 결정한다. 단, 각 가중값(Wn)은 1 이하의 양의 값으로 한다.

목표 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23) 및 가중값(Wn)(n=1,2,3,4,14,23)의 결정수법을 도 3(b), 도 5 및 도 7 등을 참조하면서 이하에 상세히 설명한다. 도 3(b)에는, 동 도시된 상태에서의 Qn과 Wn의 관계가 도시되고, 도 5(a)~(c)에는 Wn의 설정예가 도시되고, 도 7에는 가중값과 목표 노드 상반력 중심점의 관계가 도시되어 있다.

뿌리 노드인 제 1423 노드의 목표 노드 상반력 중심점(목표 제 1423 노드 상반력 중심점) Q1423은 목표 전체 상반력 중심점(도 3 및 도 4 중의 점 P)과 동일하게 결정된다(P=Q1423). 따라서, 이후의 설명에서는, 목표 제 1423 노드 상반력 중심점을 나타내는 부호로서, 대부분의 경우, 목표 전체 상반력 중심점과 동일한 참조부호 P를 사용한다.

또, 뿌리 노드의 가중값 W1423은 표현의 통일성을 유지하기 위해서, 편의상 「1」로 설정된다.

제 n 접지부위(10)(n=1,2,3,4)의 목표 상반력 중심점(목표 제 n 접지부위 상반력 중심점이라고도 함)은, 본 참고예에서는, 항상 제 n 접지부위(10)의 중심점(볼 관절(12)의 중심점)에 설정된다. 그리고, 제 n 접지부위(10)에 대응하는 잎 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4)은 제 n 접지부위(10)의 목표 상반력 중심점과 동일하게 결정된다. 따라서, 목표 노드 상반력 중심점(Qn)(n=1,2,3,4)도 항상 제 n 접지부위(10)의 중심점(이 점의 위치는 이 제 n 접지부위(10)의 목표 접지부위 위치로부터 일의적으로 정해짐)에 설정된다. 이렇게 결정한 잎 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4)의 위치는 제 n 접지부위(10)의 목표 접지부위 위치로부터 일의적으로 정해진다. 역으로 말하면, 잎 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4)은 제 n 접지부위(10)의 목표 위치를 규정하는 것이 된다. 또한, 이후의 설명에서는, 잎 노드의 목표 노드 상반력 중심점을 목표 접지부위 상반력 중심점이라고 하는 경우도 있다. 또, 각 목표 접지부위 궤도는 연속적으로 변화되도록 설정되므로, Qn(n=1,2,3,4)도 연속적으로 변화되게 된다.

보충하면, 본 참고예에서는, 제 n 잎 노드(제 n 접지부위(10))의 목표 상반력 중심점(Qn)(n=1,2,3,4)을 제 n 접지부위(10)의 중심점에 설정했지만, 접지부위의 자세를 제어할 수 있는 로봇에서는, 각 잎 노드(각 접지부위)의 목표 상반력 중심점은, 예를 들면 그 잎 노드에 대응하는 접지부위의 목표 위치 자세에 의해 정해 지는 이 접지부위의 접지면(목표 보용에서 상정되는 바닥면과의 접촉면) 내에 설정하면 된다.

이후, 일반적으로, 임의의 2개의 점 A, B를 연결하는 선분 또는 그 길이를 AB라고 나타낸다. 또, 연산자 「*」는 스칼라와 스칼라의 세트 혹은 스칼라와 벡터와의 세트에 대해서는, 승산을 의미하고, 벡터와 벡터의 세트에 대해서는, 외적(벡터 적)을 의미한다.

뿌리 노드 이외의 각 노드의 가중값(Wn)(n=1,2,3,4,14,23)과 각 중간 노드의 목표 노드 상반력 중심점 Q14, Q23은, 목표 제 14 노드 상반력 중심점(Q14)이, Q1Q14:Q14Q4=W4:W1=(1-W1):W1이라고 하는 관계식을 충족시키는 선분 Q1Q4의 내분점, 목표 제 23 노드 상반력 중심점(Q23)이 Q2Q23:Q23Q3=W3:W2=(1-W2):W2이라는 관계를 만족시키는 선분 Q2Q3의 내분점, 목표 제 1423 노드 상반력 중심점(P)(=목표 전체 상반력 중심점)이 Q14P:PQ23=W23:W14=(1-W14):W14를 만족시키는 선분 Q14Q23의 내분점이 되도록 결정된다.

바꿔 말하면, 적어도 다음 식 1, 2, 3을 만족하도록, Q14, Q23, Wn(n=1,2,3,4,14,23)이 결정된다. 또한, 이들 식 1~3중의 Qn(n=1,2,3,4,14,23)은 그 점의 위치(위치 벡터)를 의미하고 있다.

Q14=Q1*W1+Q4*W4, W1+W4=1 …식 1

Q23=Q2*W2+Q3*W3, W2+W3=1 …식 2

P=Q14*W14+Q23*W23, W14+W23=1 …식 3

Wn(n=1,2,3,4,14,23)이 1 이하의 양의 값이므로, 상기 식 1, 2, 3의 우변에 서의 Qn(n=1,2,3,4,14,23)의 계수(가중값)는 모두 양의 값이 된다.

상기 식 1~3은 자 노드를 갖는 각 노드(즉 잎 노드가 아닌 각 노드)의 목표 노드 상반력 중심점의 위치가, 소정의 양의 가중값을 사용하고, 그 노드의 자 노드의 목표 노드 상반력 중심점 위치의 가중평균의 위치에 설정되는 것을 의미한다. 바꿔 말하면, 상기 도 3(b) 및 도 7에 도시하는 바와 같이, 자 노드를 갖는 각 노드의 목표 노드 상반력 중심점(Qn)(n=14,23,1423)은 그 노드의 모든 자 노드의 목표 노드 상반력 중심점의 내분점에 설정된다. 도 7은 각 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23,1423)과 가중값(Wn)(n=1,2,3,4,14,23)의 관계를 도시하는 도면이다. 또한, 도 3(b)중의 L23, L14, L1423은 각각 선분 Q2Q3, Q1Q4, Q23Q14의 길이이다.

보충하면, Q1~Q4 및 P(=Q1423)는 상기한 바와 같이 결정되므로, Wn(n=1,2,3,4,14,23)을 결정하면, 식 1~3을 만족시키는 Q14, Q23은 일의적으로 정해지고, 반대로, Q14, Q23을 결정하면, 식 1~3을 만족시키는 Wn(n=1,2,3,4,14,23)은 일의적으로 정해진다. 따라서, 가중값(Wn)(n=1,2,3,4,14,23)을 결정한 뒤에, 상기 식 1~3에 기초하여 중간 노드의 목표 노드 상반력 중심점 Q14, Q23을 결정해도, 중간 노드의 목표 노드 상반력 중심점 Q14, Q23을 결정한 다음, 상기 식 1~3에 기초하여 가중값(Wn)(n=1,2,3,4,14,23)을 결정해도, 어느쪽이어도 좋다.

중간 노드의 목표 노드 상반력 중심점 Q14, Q23은 도 3(a)~도 3(c) 및 도 4(a)~도 4(c)에 시계열적으로 도시한 바와 같이, 각 접지부위(10)의 이동에 따라 이동한다. 이때의 가중값 W14(=1-W23), W1(=1-W4), W3(=1-W2)는, 각각 예를 들면 상기 도 5(a)~(c)의 그래프로 도시하는 바와 같이 연속적으로 변화되도록 결정된다. 제 n 노드(n=1,2,3,4)의 Qn의 궤도 및 가중값(Wn)이 연속적으로 변화되도록 설정되므로, 목표 노드 상반력 중심점 Q14, Q23도 연속적으로 이동하도록 결정된다. 즉, 모든 목표 노드 상반력 중심점(목표 제 n 노드 상반력 중심점(n=1,2,3,4,14,23,1423))은 연속적으로 이동하도록 설정된다.

목표 상반력 분배기(102)는 상기한 바와 같이 결정한 각 노드의 목표 노드 상반력 중심점을 출력한다. 또한, 뿌리 노드의 목표 상반력 중심점은 보용생성장치(100)가 결정하는 목표 전체 상반력 중심점과 동일하므로, 목표 상반력 분배기(102)로부터 출력할 필요는 없다.

목표 상반력 분배기(102)는, 또한, 각 노드의 목표 상반력 중심점에 작용하는 상반력의 목표값인 목표 노드 상반력을 결정하여 출력한다. 출력하는 목표 노드 상반력에는, 적어도 잎 노드인 제 n 노드(n=1,2,3,4)의 목표 노드 상반력 중심점(Qn)에 작용하는 목표 노드 상반력(목표 제 n 노드 상반력), 즉, 각 접지부위(10)의 목표 상반력이 포함된다. 이 목표 노드 상반력은, 후술하는 계층형 컴플라이언스 동작 결정부(114)의 처리에서 실행되는, 각 다리(#1~#4)의 컴플라이언스 기구(42) 등(도 1 참조)의 휨 보상(보다 상세하게는 컴플라이언스 기구(42) 및 각 다리의 링크기구의 휨에 의한 각 접지부위(10)의 위치 어긋남의 보상) 등을 위해 필요하다. 보충하면, 로봇(1)이 각 다리(#1~#4)의 선단부 이외에도 컴플라이언스 기구를 구비하는 경우에는, 컴플라이언스 기구의 변형을 구하기 위해서, 목표 제 14 노드 상반력, 목표 제 23 노드 상반력(중간 노드의 목표 노드 상반력)도 결정하 여 출력하는 것이 바람직하다.

일반적으로, 각 노드의 목표 노드 상반력 중심점에 작용하는 목표 상반력(목표 노드 상반력)은 목표 전체 상반력과 각 노드의 가중값으로부터 결정하면 된다. 구체적으로는, 임의의 노드의 목표 상반력은 그 노드의 가중값과 그 노드의 모든 선조 노드의 가중값의 적에 목표 전체 상반력을 승산함으로써 결정하면 된다. 즉, 다음 식 4a(또는 식 4b)에 의해 목표 제 n 노드 상반력이 산출된다.

목표 제 n 노드 상반력

=제 n 노드 가중값

*제 n 노드의 모든 선조 노드의 가중값의 적

*목표 전체 상반력 …식 4a

목표 제 n 노드 상반력

=제 n 노드 가중값

*제 n 노드의 모든 선조 노드(뿌리 노드를 제외한)의 가중값의 적

*목표 전체 상반력 …식 4b

또는, 잎 노드가 아닌 임의의 제 n 노드의 목표 상반력이 제 n 노드의 모든 자 노드의 목표 상반력의 합(합력)에 일치하고, 또한, 뿌리 노드의 목표 상반력이 목표 전체 상반력에 일치하도록, 목표 각 노드 상반력이 결정된다. 이 관계를 도시한 것이 도 8이다.

즉, 제 n 노드의 목표 상반력인 목표 제 n 노드 상반력을 Fn(n=1,2,3,4,14,23,1423), 목표 전체 상반력을 Ftotalref로 나타내면, Fn은, 도 8 중에 나타내는 식에 의해, Ftotalref와, 가중값(Wn)(n=1,2,3,4,14,23)으로부터 결정된다. 도 8 중의 식은 상기 식 4a 또는 4b와 등가이다.

이상과 같이 각 노드의 목표 상반력 중심점(목표 노드 상반력 중심점)과 각 노드의 목표 상반력(목표 노드 상반력)을 결정하는 것은, 제 n 노드의 모든 자 노드의 목표 상반력의 합력이 목표 제 n 노드 상반력 중심점 주위에 발생하는 모멘트의 수평성분이 0이 되도록 각 노드의 목표 상반력 중심점과 목표 상반력을 결정하고 있는 것이 된다. 따라서, 목표 노드 상반력의 모멘트 수평성분은, 어느 노드에 대해서도 0이다.

또한, 접지하고 있지 않은(비접지상태의) 접지부위(10)의 목표 상반력(접지하고 있지 않은 접지부위(10)에 대응하는 잎 노드의 목표 노드 상반력)이 0이 되도록, 각 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23), 각 가중값(Wn)(n=1,2,3,4,14,23) 및 각 목표 노드 상반력(Fn)(n=1,2,3,4,14,23)의 세트가 결정된다. 따라서, 제 1 참고예에서는, 도 5(a)에 도시한 바와 같이, 제 1 접지부위(10) 및 제 4 접지부위(10)가 미착지 다리가 되어 이상하는 시기(이 미착지 다리의 접지부위(10)가 공중을 이동하는 시기)에서는, 가중값 W14가 0(W23=1)이 되고, 또, 제 2 접지부위(10) 및 제 3 접지부위(10)가 미착지 다리가 되어 이상하는 시기에서는, 가중값 W23이 0(W14=1)이 된다. 이렇게, 자 노드를 갖는 중간 노드의 가중값은 그 중간 노드에 잎 노드로서 속하는 모든 접지부위가 접지하고 있지 않은 시기에서는 0으로 설정된다. 역으로 말하면, 중간 노드의 가중값은 그 중간 노드에 속하는 어느 하나의 접지부위가 접지하고 있는 시기(엄밀하게는 어느 하나의 접 지부위에 0이 아닌 상반력이 작용하고 있는 시기)에서는 0으로는 설정되지 않는다.

보충하면, 제 1 참고예에서는, 중간 노드 Q14에 속하는 제 1 접지부위(10) 및 제 4 접지부위(10)의 어느 한쪽만을 비접지상태로 하는 시기를 갖지 않기 때문에, 이들 접지부위(10, 10)에 대응하는 가중값 W1, W4는 0 또는 1이 되는 시기를 갖지 않는 것으로 되어 있다. 단, 제 1 접지부위(10) 및 제 4 접지부위(10)의 어느 한쪽이 비접지상태가 되는 시기를 갖도록, 목표 접지부위 궤도를 결정하도록 한 경우에는, 그 시기에서 비접지상태가 되는 접지부위(10)에 대응하는 가중값을 0으로 설정하고, 또한, 접지상태가 되는 접지부위(10)에 대응하는 가중값을 1로 설정하도록 하면 된다. 이 경우, 그 시기에서의 중간 노드 Q14의 가중값은, 0이 아닌 값으로 설정된다. 중간 노드 Q23, 및 이것에 속하는 잎 노드인 제 2 접지부위(10) 및 제 3 접지부위(10)에 관계되는 가중값에 대해서도 동일하다. 일반적으로 말하면, 자 노드를 갖는 노드의 가중값은 그 노드에 속하는 접지부위 중 어느 하나가 접지상태일 때에는, 0이 아닌 값으로 설정되고, 그 노드에 속하는 접지부위 모두가 비접지상태가 될 때에, 0으로 설정된다.

상기한 바와 같이 구한 각 목표 노드 상반력은 연속적으로 변화되므로, 로봇(1)의 충격이 적은 이동(보행)을 실현하기 위해 적합하다.

이상과 같이, 제 1 참고예에서는, 각 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23,1423), 각 가중값(Wn)(n=1,2,3,4,14,23) 및 각 목표 각 노드 상반력(Fn)(n=1,2,3,4,14,23,1423)은 목표 상반력 분배기(102)에 의해 다음 조건 A)~G)를 만족시키도록 결정된다.

A) 각 잎 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4)은 이 잎 노드에 대응하는 접지부위(10)의 중심점에 일치한다. 보다 일반적으로는, Qn(n=1,2,3,4)은 목표 보용(목표 접지부위 궤도 등의 목표 운동)에 따라 결정된다. 보충하면, 예를 들면 각 접지부위(10)의 목표 상반력 중심점을 보용생성장치(100)에서 결정하도록 해도 되고, 그 경우에는, 보용생성장치(100)가 결정하는 목표 상반력을 기초로, 목표 노드 상반력 중심점(Qn)(n=1,2,3,4)을 결정하면 된다.

B) 뿌리 노드의 목표 노드 상반력 중심점은 목표 전체 상반력 중심점(P)에 일치한다.

C) 뿌리 노드를 제외한 각 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23)과 가중값(Wn)(n=1,2,3,4,14,23)은 상기 식 1~3의 관계식을 충족시킨다. 바꾸어 말하면, 자 노드를 갖는 임의의 제 n 노드(n=14,23,1423)의 목표 노드 상반력 중심점(Qn)은 그 제 n 노드의 각 노드의 목표 노드 상반력 중심점의 가중평균의 점이 된다.

D) 자 노드를 갖는 임의의 제 n 노드(n=14,23,1423)의 목표 노드 상반력(Fn)은 이 제 n 노드의 모든 자 노드의 목표 노드 상반력의 합(합력)에 일치하고, 또한, 뿌리 노드(제 1423 노드)의 목표 노드 상반력(F1423)은 목표 전체 상반력(Ftotalref)에 일치한다. 바꾸어 말하면, 제 n 노드(n=1,2,3,4,14,23,1423)의 목표 상반력(Fn) 및 가중값(Wn)이 도 8중의 관계식을 만족시킨다.

E) 접지하고 있지 않은 접지부위(10)에 대응하는 잎 노드의 목표 노드 상반력은 0이 된다.

F) Qn, Wn, Fn(n=1,2,3,4,14,23,1423)은 연속적으로 변화된다.

G) 비접지상태의 접지부위(10)에 대응하는 잎 노드의 가중값, 또는, 그 잎 노드의 선조 노드의 어느 1개의 가중값은 0으로 된다.

또한, 목표 노드 상반력을 결정할 때에, 상기와 같이 가중값을 기초로 결정하는 대신, 각 목표 노드 상반력 중심점을 기초로 결정해도 된다. 즉, 상기 A)~C)의 조건을 만족시키도록 각 목표 노드 상반력 중심점을 결정한 뒤, 그 목표 노드 상반력 중심점과, 상기 식 1~3을 기초로, 각 가중값을 결정하고, 그 결정한 가중값을 사용하여 상기 식 4에 의해 목표 각 노드 상반력을 결정하도록 해도 된다.

도 2의 설명으로 되돌아와, 상기 자세편차 연산부(103)는 로봇(1)에 목표 상체자세에 대한 실 상체자세의 편차를 산출하여 출력하는 것이다. 본 참고예에서는, 이 자세편차 연산부(103)에는, 상기 자세 센서(36)로 검출된 상체(24)의 연직방향에 대한 경사각(이하, 실 상체자세 경사라고 함)과, 보용생성장치(100)에서 결정된 목표 상체위치 자세(상세하게는, 목표 상체자세 중 연직방향에 대한 경사각. 이하, 목표 상체자세 경사라고 함)가 입력되고, 이들 편차(θberr)(실 상체자세 경사 목표 상체자세 경사. 이하, 상체자세 경사 편차(θberr)라고 함)가 산출된다. 산출하는 θberr은 X축 주위 성분(롤링 방향 성분)(θberrx)과, Y축 주위 성분(피칭 방향 성분)(θberry)으로 구성된다. 또한, 목표 상체자세 경사가 예를 들면 정상적으로 0인 경우에는, θberr=실 상체자세 경사이므로, 자세 센서(36)의 검출값(실 상체자세 경사)을 그대로, 상체자세 경사 편차(θberr)로서 출력하면 된다.

상기 자세안정화 제어 연산부(104)는, 일반적으로 말하면, 상기 상체자세 경 사 편차 등, 로봇(1)에 구비된 센서의 정보에 기초하여 검출 또는 추정되는 로봇(1)의 상태에 따라 로봇(1)의 자세를 안정화하기 위한 전체 상반력의 보상량(목표 전체 상반력의 보정량)인 보상 전체 상반력을 산출하는 것이다.

로봇(1)의 자세를 장기적으로 안정화시키기 위해서는, 로봇(1)의 상체(24) 등의 소정 부위의 실제의 위치 자세를 바람직한 위치 자세로 복원시키기 위해서 필요한 병진력과 모멘트를 구하고, 이것을 목표 전체 상반력 중심점(목표 ZMP)을 작용점으로 하여 부가적으로 발생시킬 필요가 있다. 이 부가적인 병진력과 모멘트를 보상 전체 상반력이라고 한다. 또, 보상 전체 상반력의 모멘트 성분을 「보상 전체 상반력 모멘트(Mdmd)」(상세하게는 목표 전체 상반력 중심점(목표 ZMP) 주위의 보상 전체 상반력 모멘트(Mdmd))라고 부른다.

본 참고예에서는, 자세안정화 제어 연산부(104)는 실 상체자세 경사를 목표 상체자세 경사로 복원하도록(근접시키도록) 보상 전체 상반력 모멘트(Mdmd)를 산출한다. 이 때문에, 자세안정화 제어 연산부(104)에는, 상기 자세편차 연산부(103)에서 구해진 상체자세 경사 편차(θberr)(θberrx, θberry)가 입력된다.

자세안정화 제어 연산부(104)는 입력된 상체자세 경사 편차(θberr)를 기초로 상기 보상 전체 상반력 모멘트(Mdmd)를 산출한다. 산출하는 Mdmd는 X축 주위 성분(Mdmdx)과, Y축 주위 성분(Mdmdy)으로 구성된다.

Mdmdx 및 Mdmdy는, 구체적으로는, 예를 들면 다음 식 5, 6의 피드백 제어칙(여기에서는 PD 제어칙)에 의해 결정된다. 즉, Mdmdx 및 Mdmdy는 상체자세 경사 편차(θberrx, θberry)를 0에 근접시키도록 결정된다.

Mdmdx=-Kthx*θberrx-Kwx*(dθberrx/dt) …식 5

Mdmdy=-Kthy*θberry-Kwy*(dθberry/dt) …식 6

여기에서, Kthx, Kthy, Kwx 및 Kwy는 소정의 게인이다.

또, (dθberrx/dt),(dθberry/dt)는 각각 상체자세 경사 편차 θberrx, θberry의 시간미분값이다.

본 참고예에서는, 보상 전체 상반력 모멘트(Mdmd)의 Z축 주위 성분(요잉 방향 성분)(Mdmdz)를 사용하지 않으므로, Mdmdz의 결정을 행하지 않지만, 로봇(1)의 스핀(연직축 주위의 미끄러짐)을 방지하거나 하기 위해서 Mdmdz를 결정하도록 해도 된다. Mdmdz의 결정법에 관해서는, 본원 출원인이 먼저 제안한 일본 특원 2003-185613호나 일본 특원 2003-185930호에 상세하게 설명되어 있다. 또, 예를 들면 로봇(1)의 실제의 중심의 위치를 목표 보용에서의 중심의 위치로 복원하기 위해서, 그 중심의 위치편차에 따라 보상 전체 상반력의 병진력을 결정하도록 하는 것도 가능하다.

상기 상반력 검출기(108)는, 각 다리(#1~#4)의 6축력센서(34)의 출력을 기초로, 실제의 로봇(1)의 각 접지부위(10)에 작용하는 상반력의 실제값인 실 상반력(즉 잎 노드의 실 상반력(실 노드 상반력))을 검출한다. 또한, 상반력 검출기(108)는, 로봇(1)의 각 관절(14, 15)에 구비한 인코더 등의 센서(도시하지 않음)에 의해 검출되는 각 다리(#1~#4)의 각 관절(14, 15)의 실 관절 변위(각 관절(14, 15)의 각 회전축 주위의 실 회전각)에 기초하여, 상체(24)에 고정된 좌표계에 대한 각 접지부위(10)의 상대위치 자세(제 1 참고예에서는 상대위치)를 산출한다. 또 한, 이때, 실 관절 변위 대신에, 각 관절(14, 15)의 변위 지령값(회전각 지령값)인 관절 변위 지령을 사용하거나, 또는, 실 관절 변위와 관절 변위 지령 양자를 사용해도 된다. 그리고, 산출한 각 접지부위(10)의 상대위치 자세를 기초로, 각 다리(#1~#4) 마다의 6축력센서(34)의 검출값(이것은 6축력센서(34) 등에 고정된 로컬 좌표계에서의 값임)을 좌표변환 하고, 상체(24)에 고정된 좌표계로 표현된 실 상반력을 산출한 후, 이것을 지지 다리 좌표계(글로벌 좌표계)로 표현된 실 상반력으로 변환한다. 지지 다리 좌표계로의 좌표변환에는, 자세 센서(36)의 검출값, 또는 목표 상체자세 경사를 사용하면 된다. 보충하면, 제 1 참고예의 로봇(1)에서는, 상기한 바와 같이 접지부위(10)의 중심점에 상반력 모멘트가 작용하지 않으므로, 각 접지부위(10)의 실 상반력 중의 모멘트 성분을 검출할 필요는 없다. 이 경우, 상기한 바와 같이 6축력센서(34) 대신에, 3축력센서를 사용하여, 실 상반력의 3축방향의 병진력 성분을 검출하거나, 1축 상반력 센서를 사용하여 실 상반력의 병진력 연직성분만을 검출하도록 해도 된다.

상기 로봇 기하학 모델(역키네마틱스 연산부)(110)은 각 접지부위 위치 자세의 최종적인 목표 궤도(이것은 후술의 계층형 컴플라이언스 동작 결정부(114)에 의해 결정됨)와 목표 상체위치 자세 등을 기초로, 역키네마틱스의 연산에 의해, 그것들을 만족하는 로봇(1)의 각 관절(14, 15)의 변위(회전각)의 지령값인 각 관절 변위 지령을 산출한다. 본 참고예에서는, 역키네마틱스의 연산의 해의 식을 미리 구해 두고, 그 식에 목표 상체위치 자세와 각 접지부위의 최종적인 목표 위치를 대입하는 것만으로 각 관절 변위 지령을 산출하도록 했다. 즉, 로봇 기하학 모델(110) 은 보용생성장치(100)에서 결정된 목표 상체위치 자세 궤도와 계층형 컴플라이언스 동작 결정부(114)에서 후술하는 바와 같이 수정된 수정 목표 접지부위 궤도(기구변형 보상된 수정 목표 접지부위 궤도)가 입력되고, 그것들의 입력값으로부터 역키네마틱스의 연산에 의해, 각 다리(#1~#4)의 각 관절(14, 15)의 관절 변위 지령을 산출한다.

또한, 후술하는 제 1 실시형태와 같이 팔 관절이나 목 관절 등 다리 관절 이외의 관절이 있는 로봇의 경우에는, 손이나 머리부 등의 상체에 대한 상대위치 자세를 기초로, 역키네마틱스의 연산에 의해, 다리관절 이외의 관절의 변위가 결정된다.

상기 변위 콘트롤러(112)는, 관절 제어수단으로서의 기능을 갖고, 로봇(1)의 각 관절(14, 15)의 실 관절 변위(검출값)와, 로봇 기하학 모델(역키네마틱스 연산부)(110)에서 산출된 관절 변위 지령이 입력되고, 이 관절 변위 지령을 목표값으로 하여, 이 목표값에 실 관절 변위를 추종시키도록 각 관절(14, 15)의 액추에이터(도시하지 않음)를 제어(피드백 제어)한다.

계층형 컴플라이언스 동작 결정부(114)는, 목표 전체 상반력과 보상 전체 상반력의 합력에 실 전체 상반력을 근접시키도록 목표 접지부위 궤도를 수정하고, 그 수정 후의 목표 접지부위 궤도인 기구변형 보상된 수정 목표 접지부위 위치 자세 궤도를 출력하는 것이다. 또한, 본 참고예에서는, 각 접지부위(10)의 자세는 제어할 수 없으므로, 기구변형 보상된 수정 목표 접지부위 위치 자세 궤도는, 실제로는, 기구변형 보상된 수정 목표 접지부위 위치궤도이다.

계층형 컴플라이언스 동작 결정부(114)는, 일반적으로는, 이하의 3개의 요구를 가능한 한 만족시키도록 각 접지부위(10)의 목표 접지부위 궤도를 수정하는 것이다.

요구 1) 로봇(1)의 위치 자세를 안정화하기 위해서, 실 전체 상반력을 자세안정화 제어 연산부(104)가 출력하는 보상 전체 상반력(모멘트(Mdmd))과 목표 전체 상반력의 합력에 추종시킨다. 또한, 제 1 참고예에서는 로봇(1)의 상체(24)의 자세 경사(연직방향에 대한 경사)를 안정화하기 위해서, 목표 전체 상반력 중심점 주위의 실 전체 상반력 모멘트의 수평성분을 보상 전체 상반력 모멘트(Mdmd)의 수평성분(Mdmdx, Mdmdy)에 추종시킨다. 보충하면, 목표 전체 상반력 중심점 주위의 목표 전체 상반력 모멘트의 수평성분은 0이므로, 이것과 Mdmd의 합력은 Mdmd에 일치한다.

요구 2) 접지하고 있어야 할 복수의 접지부위(10)에 대하여, 그 일부의 접지부위(10)에 실 상반력이 집중하고, 그 이외의 어떤 일부의 접지부위(10)의 실 상반력이 지나치게 감소하여, 그 실 상반력이 감소한 접지부위(10)의 접지성이 극단적으로 저하되지 않도록, 가능한 한 잎 노드가 아닌 각 노드의 목표 상반력 중심점 주위의 실 상반력 모멘트의 절대값을 작게 한다. 또한, 제 1 참고예에서는 제 14 노드, 제 23 노드, 제 1423 노드의 목표 상반력 중심점 주위의 실 상반력 모멘트의 절대값을 가능한 한 작게 한다.

요구 3) 접지하고 있어야 할 각 접지부위(10)의 접지성을 확보하도록, 즉 각 접지부위(10)에서의 로컬 접지압 분포(실 상반력의 분포)가 치우쳐, 이 접지부 위(10)의 국소적인 접지성이 저하되지 않도록, 가능한 한 각 접지부위(10)(각 잎 노드)의 목표 상반력 중심점 주위의 실 상반력 모멘트의 절대값을 작게 한다. 단, 제 1 참고예의 로봇(1)에서는, 각 접지부위(10)의 목표 상반력 중심점 주위의 실 상반력 모멘트는 항상 0이 되므로, 이 요구 3)은 고려할 필요는 없다.

또한, 일반적으로, 요구 1)~3) 모두, 또는 요구 1), 2)를 완전하게 만족할 수는 없다. 예를 들면 실 전체 상반력을 보상 전체 상반력과 목표 전체 상반력의 합력에 일치시키면서 각 접지부위(10)의 목표 상반력 중심점 주위의 실 상반력 모멘트를 0으로 하는 것이 물리적으로 불가능한 경우가 많다. 따라서, 계층형 컴플라이언스 동작 결정부(114)는, 일반적으로는, 요구 1)~3), 또는 요구 1), 2)를 가능한 한 만족시키면서, 어떤 타협점에서, 각 접지부위(10)의 목표 접지부위 궤도를 수정하게 된다.

이상이 제어장치(50)의 각 기능적 수단(기능적 구성요소)의 개요이다.

보충하면, 상기 계층형 컴플라이언스 동작 결정부(114), 자세안정화 제어 연산부(104) 및 로봇 기하학 모델(역키네마틱스 연산부)(110)은 본 발명에서의 노드 동작 제어수단에 상당하는 것이다.

다음에 도 9의 플로우차트를 참조하면서, 제어장치(50)의 전체적인 동작(연산처리)을 보다 상세하게 설명한다. 도 9는 제어장치(50)의 메인루틴 처리를 도시하는 플로우차트(구조화 플로우차트)이다. 또한, 도 9의 좌단에는, 해당하는 처리를 행하는 제어장치(50)의 구성요소를 나타낸다.

우선, S10에서 제어장치(50)의 초기화가 행해진 후, S12를 경과하여 S14로 진행되고, 제어장치(50)의 연산처리는 이 제어장치(50)의 제어주기 마다의 타이머 인터럽션을 기다리는 상태가 된다. 제어주기는 예를 들면 50ms이다.

계속해서 S16으로 진행되고 보용의 전환차례인지 아닌지를 판단하고, 그 판단 결과가 NO일 때는 후술의 S22로 진행된다. 또, S16의 판단 결과가 YES일 때는 S18로 진행되어 시각 t를 0으로 초기화 한 후, S20으로 진행되어 보용 패러미터를 설정한다. 본 참고예에서는, 예를 들면 로봇(1)의 어떤 소정의 다리(예를 들면 #1)가 이상하고 나서 다음에 이상할 때까지(혹은 착지하고나서 다음에 착지할 때까지)의 소정 기간분의 목표 보용을 단위로 하여, 그 소정 기간분의 목표 보용을 규정하는 패러미터(목표 보용을 결정하기 위한 알고리즘에서 사용하는 패러미터)인 보용 패러미터가 S20에서 설정된다. 그리고, S16에서 말하는 「보용의 전환차례」는 상기 소정 기간분의 목표 보용의 전환차례이다. 또한, 목표 보용의 전환차례의 판단은, 시각이나, 상기 소정의 다리의 6축력센서(34)의 검출값 등을 기초로 행하면 된다.

S20에서 설정되는 보용 패러미터는, 로봇(1)의 목표 운동 궤도(상세하게는 목표 접지부위 궤도 및 목표 상체위치 자세 궤도)를 규정하는 운동 패러미터와, 목표 상반력 궤도(상세하게는 목표 전체 상반력 궤도 및 목표 전체 상반력 중심점 궤도)를 규정하는 상반력 패러미터로 구성된다. 보충하면, 로봇(1)의 목표 운동이 결정되면, 그 목표 운동에 따라 발생하는 관성력과 로봇(1)에 작용하는 중력의 합력의 부호를 반전시킨 것을 목표 전체 상반력으로 하면 되므로, 상반력 패러미터는, 목표 전체 상반력 중심점 궤도만을 규정하는 것이라도 좋다. 또, 목표 접지부 위 궤도(보다 일반적으로는 목표 상체위치 자세 이외의 목표 운동의 패러미터)와 목표 전체 상반력 중심점을 결정한 뒤, 로봇(1)의 동역학 모델을 사용하고, 로봇(1)의 목표 상체위치 자세를 포함하는 목표 운동이 발생하는 관성력과 로봇(1)에 작용하는 중력의 합력이 목표 전체 상반력 중심점(목표 ZMP) 주위에 발생하는 모멘트의 수평성분이 0이 되도록, 목표 상체위치 자세를 결정하는 경우에는, 운동 패러미터에는 목표 상체위치 자세 궤도를 규정하는 패러미터가 포함되어 있지 않아도 된다.

계속해서 S22로 진행되고, 상기 보용 패러미터를 기초로, 목표 보용의 순시값을 결정한다. 여기에서 「순시값」은 제어주기마다의 값을 의미하고, 목표 보용 순시값은 목표 상체위치 자세, 목표 접지부위 위치(목표 접지부위 궤도의 순시값), 목표 전체 상반력, 및 목표 전체 상반력 중심점 위치(목표 ZMP 위치)의 순시값으로부터 구성된다. 또한, 본 참고예에서는, 접지부위(10)의 자세를 제어할 수 없으므로, 각 접지부위(10)의 목표 자세의 순시값은 결정하지 않는다. 각 접지부위의 자세를 제어할 수 있는 경우에는, 상기 보용 패러미터에 각 접지부위의 목표 자세를 규정하는 패러미터를 포함시켜 두고, 그 패러미터를 기초로 각 접지부위의 목표 자세의 순시값을 결정하면 된다.

상기 S14~S22의 처리는 상기 보용생성장치(100)에 의해 실행되는 처리이다.

계속해서 S24로 진행되고, 각 노드의 가중값(Wn)(n=1,2,3,4,14,23)과 각 노드의 목표 상반력 중심점(목표 노드 상반력 중심점)(Qn)(n=1,2,3,4,14,23)이 구해진다. 이 처리는 목표 상반력 분배기(102)에 의해 상기한 바와 같이 실행되는 처 리이다.

계속해서 S26로 진행되고, 목표 노드 상반력(적어도 각 접지부위(10)(각 잎 노드)의 목표 상반력을 포함함)이 구해진다. 제 1 참고예에서는, 각 잎 노드의 목표 노드 상반력(각 목표 접지부위 상반력)이 구해진다. 이 S26 처리도 목표 상반력 분배기(102)에 의해 상기한 바와 같이 실행되는 처리이다. 또한, 상기한 바와 같이, 각 다리(#1~#4)의 선단부 이외에도 컴플라이언스 기구를 설치한 경우에는, 잎 노드가 아닌 각 중간 노드의 목표 상반력도 더 구하는 편이 좋다. 보충하면, 목표 노드 상반력의 모멘트 수평성분은 0이다.

계속해서 S28로 진행되고, 자세 센서(36) 등의 출력으로부터 상기 실 상체자세 경사 등의 로봇(1)의 상태를 검출한다. 제 1 참고예에서는, 상기 자세편차 연산부(103)에 의해, 자세 센서(36)에 의한 실 상체자세 경사의 검출값이 받아들여지고, 그 검출값과 목표 상체위치 자세중의 목표 상체자세 경사(현재 시각의 순시값)로부터, 상체자세 경사 편차(θberr)가 산출된다.

계속해서 S30로 진행되고, S28에서 검출한 로봇(1)의 상태 등으로부터 로봇(1)의 자세를 안정화하기 위한 보상 전체 상반력을 구한다. 제 1 참고예에서는, 상기 자세안정화 제어 연산부(104)에 의해, 상기 상체자세 경사 편차(θberr)로부터, 목표 전체 상반력 중심점(목표 ZMP) 주위의 보상 전체 상반력 모멘트(Mdmd)의 수평성분(Mdmdx, Mdmdy)이 상기 식 5 및 식 6에 따라서 산출된다.

계속해서 S32로 진행되어 각 접지부위(10)의 실 상반력을 검출한다. 이것은 상기 실 상반력 검출기(108)에 의해 실행되는 처리이며, 상기와 같이, 6축력센 서(34)에 의해 검출된 각 접지부위(10) 마다의 실 상반력을 지지 다리 좌표계(글로벌 좌표계)로 변환한 것이 구해진다. 이후, 각 접지부위(10)의 실 상반력을 실 접지부위 상반력이라고 하는 경우가 있다.

계속해서 S34로부터 S38까지, 상기 계층형 컴플라이언스 동작 결정부(114)의 처리가 실행된다.

이하, 잠시동안, 이들 S34~S38의 처리를 구체적으로 설명하기 전에, 계층형 컴플라이언스 동작 결정부(114)의 전체적인 처리를 설명한다. 또한, 이 설명은, 이해의 편의상, 주로 상기 도 3(b)에 도시하는 상태(로봇(1)의 모든 다리(#1~#4)가 지지 다리로 되어 있는 상태)를 예로 들어서 행한다.

계층형 컴플라이언스 동작 결정부(114)는, 목표 상반력 분배기(102)에서 결정된 각 잎 노드의 목표 노드 상반력 등을 기초로, 잎 노드를 제외한 각 노드의 목표 노드 상반력의 병진력 성분과 모멘트 성분(보다 상세하게는 목표 노드 상반력 중심점을 작용점으로 하는 병진력 성분과 모멘트 성분)을 결정한다.

도 3(b)의 상태에서의 각 노드의 목표 상반력(각 목표 노드 상반력)의 병진력 성분을 예시적으로 도 10에 도시한다. 이 도면에서, 벡터 Fn_ref(n=1,2,3,4,14,23)가 목표 제 n 노드 상반력의 병진력 성분을 나타내고 있다. 또, Ftotalref는 목표 전체 상반력(=목표 제 1423 노드 상반력)의 병진력 성분을 나타낸다. 상기 도 8에 도시한 바와 같이, 잎 노드가 아닌 임의의 제 n 노드(n=14,23,1423)의 목표 제 n 노드 상반력은 제 n 노드의 모든 자 노드의 목표 노드 상반력의 합력으로 결정된다. 따라서, F14ref=F1ref+F4ref, F23ref=F2ref+F3ref, Ftotalref(=F1423ref)=F14ref+F23ref이다. 계층형 컴플라이언스 동작 결정부(114)는, 목표 상반력 분배기(102)에서 결정된 각 잎 노드(각 접지부위(10))의 목표 노드 상반력의 병진력 성분 Fn_ref(n=1,2,3,4)로부터, 상기한 바와 같이, 각 중간 노드의 목표 노드 상반력의 병진력 성분 F14ref(=F1ref+F4ref), F23ref(=F2ref+F3ref)를 구한다. 또한, Ftotalref(=F1423ref)는 상기 보용생성장치(100)에서 결정된 목표 전체 상반력의 병진력 성분에 설정된다.

또한, 계층형 컴플라이언스 동작 결정부(114)는 목표 노드 상반력의 병진력 성분의 경우와 마찬가지로, 잎 노드를 제외한 각 노드의 목표 노드 상반력의 목표 노드 상반력 중심점을 작용점으로 하는 모멘트 성분을 결정한다. 단, 이 경우, 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23)의 정의로부터, 목표 제 n 노드 상반력의 모멘트 수평성분은 항상 0으로 설정된다. 목표 제 1423 노드 상반력(=목표 전체 상반력)의 모멘트 수평성분도 0으로 설정된다.

본 참고예의 로봇(1)에서는, 각 접지부위(10)는 각 다리(#1~#4)의 선단부의 볼 관절(12)(프리 조인트)에 걸어맞추어져 있으므로, 각 접지부위(10)(각 잎 노드)에 상반력 모멘트(수평성분 및 연직성분)를 발생시킬 수 없다. 이 때문에, 계층형 컴플라이언스 동작 결정부(114)에서는, 각 접지부위(10)(각 잎 노드)의 목표 노드 상반력의 모멘트 연직성분도 0으로 설정된다.

잎 노드가 아닌 노드의 목표 노드 상반력 모멘트 연직성분은, 로봇(1)의 목표 운동에 동역학적으로 균형이 잡히도록 결정하면, 일반적으로는 0이 아닌 값도 채택할 수 있지만, 본 참고예에서는, 로봇(1)의 자세의 연직축 주위의 회전(요잉 방향의 회전)에 관한 제어를 행하지 않는다. 이 때문에, 본 참고예에서는, 잎 노드가 아닌 노드의 목표 노드 상반력의 모멘트의 연직성분의 설정은 생략한다. 이러한 점에서, 도 3(b)의 상태에서의 각 목표 노드 상반력의 모멘트 성분의 도시는 생략한다. 또한, 로봇(1)의 자세의 연직축 주위의 회전에 관한 제어를 행하는 경우에는, 각 노드의 목표 상반력 모멘트 연직성분도 설정해야 한다.

보충하면, 잎 노드를 포함한 각 노드의 목표 노드 상반력의 병진력 성분 및 모멘트 성분의 결정을 상기 목표 상반력 분배기(102)에서 행하도록 한 경우에는, 그 결정을 계층형 컴플라이언스 동작 결정부(114)에서 행할 필요는 없다.

또, 계층형 컴플라이언스 동작 결정부(114)는 각 노드의 실 상반력인 실 노드 상반력의 병진력 성분 및 모멘트 성분도 결정한다.

도 3(b)의 상태에서의 각 노드의 실 상반력(실 노드 상반력)의 병진력 성분을 예시적으로 도 11에 도시한다. 이 도면에서, 벡터 Fn_act(n=1,2,3,4,14,23)가 실 제 n 노드 상반력의 병진력 성분을 나타내고 있다. 또한 Ftotalact는 실 전체 상반력(=실 제 1423 노드 상반력)의 병진력 성분을 나타낸다. 일반적으로, 잎 노드가 아닌 각 노드의 실 상반력의 병진력 성분은 그 노드의 모든 자 노드의 실 상반력의 병진력 성분의 합력으로 한다. 따라서, 제 14 노드, 제 23 노드 및 제 1423 노드의 실 상반력의 병진력 성분은 F14act=F1act+F4act, F23act=F2act+F3act, Ftotalact(=F1423act)=F14act+F23act이다. 또, 각 잎 노드의 실 상반력의 병진력 성분 F1act, F2act, F3act, F4act는 상기 실 상반력 검출기(108)에서 얻어지는 각 접지부위(10)의 실 상반력(실 접지부위 상반력)의 병진력 성분이다. 또한, 도 11의 점선으로 나타내는 벡터는, 상기 도 10에 도시한 목표 노드 상반력의 병진력 성분이다. 계층형 컴플라이언스 동작 결정부(114)는 상기한 바와 같이, 실 상반력 검출기(108)에서 얻어진 각 접지부위(10)의 실 상반력으로부터 각 노드의 실 노드 상반력의 병진력 성분을 결정한다.

또, 도 3(b)의 상태에서의 각 노드의 실 상반력의 모멘트 성분을 예시적으로 도 12에 나타낸다. 이 도면에서, 벡터 Mn_act(n=14,23,1423)는 실 제 n 노드 상반력의 모멘트 성분을 나타낸다. 이 경우, 각 노드의 실 상반력의 병진력 성분과 동일하게, 일반적으로는, 잎 노드가 아닌 노드의 실 상반력의 모멘트 성분 M14act, M23act, M1423act(=Mtotalact)는, 그 노드의 모든 자 노드의 실 상반력의 합력의 모멘트 성분(그 노드의 목표 상반력 중심점 Q14, Q23, Q1423을 작용점으로 하는 모멘트 성분)으로 한다.

본 참고예의 로봇(1)에서는, 상기한 바와 같이 각 접지부위(10)(각 잎 노드)에 상반력 모멘트를 발생시킬 수 없으므로, 각 잎 노드의 실 상반력(실 접지부위 상반력)의 모멘트 성분은 항상 0이 된다. 따라서, 각 잎 노드의 실 노드 상반력의 모멘트 성분의 도시는 생략한다.

잎 노드가 아닌 노드의 실 상반력 모멘트 성분(본 참고예에서는, M14act, M23act, M1423act)은 일반적으로 0이 되지 않는다. 예를 들면 목표 제 14 노드 상반력 중심점(Q14)의 주위에는, 일반적으로는, 제 1 접지부위(10)(제 1 노드) 및 제 4 접지부위(10)(제 4 노드)의 실 상반력의 병진력 성분의 수평성분에 의해 모멘트 가 발생한다. 단, 본 참고예의 로봇(1)에서는, 다리의 선단부에 프리 조인트인 볼 관절(12)을 구비하고 있으므로, M14act의 선분 Q1Q4와 동일한 방향의 성분, M23act의 선분 Q2Q3과 동일한 방향의 성분은 0이 된다.

또한, 각 다리의 선단부의 관절에 액추에이터를 구비한 로봇(각 다리의 선단부의 접지부위의 자세를 제어가능한 로봇)에 있어서는, 이 로봇의 자세의 연직축 주위 회전에 관한 제어를 행하지 않는 경우에도, 본원 출원인이 앞서 제안한 일본 특개평10-277969호 공보의 실시예에 개시하는 바와 같이, 각 접지부위의 목표 상반력 모멘트 수평성분도 설정하고, 또, 각 접지부위의 실 상반력 모멘트도 검출한다. 그리고, 각 접지부위의 실 상반력 모멘트 수평성분을 목표 상반력 모멘트 수평성분에 근접시키도록, 또는, 각 접지부위의 실 상반력 모멘트를 목표 상반력 모멘트 수평성분과 그 접지부위 보상 상반력 모멘트의 합(벡터 합)에 근접시키도록, 각 접지부위의 자세의 보정 동작을 행해야 한다.

계층형 컴플라이언스 동작 결정부(114)의 처리를 보다 상세하게 설명한다. 이 경우, 도 3(b)에 도시하는 상태에서, 로봇(1)의 상체(24)의 자세가 좌측 후방측으로 쓰러질 것 같은 상태로 되고, 자세안정화 제어 연산부(104)가 결정한 보상 전체 상반력 모멘트(Mdmd)가 도 13과 같이 되어 있는 상황을 상정한다.

로봇(1)의 상체(24)의 자세(연직방향에 대한 경사)를 복원시키기(목표 상체자세에서의 경사로 복원시키기) 위해서는, 목표 전체 상반력 중심점(목표 ZMP) 주위의 실 전체 상반력 모멘트의 수평성분을 목표 전체 상반력 모멘트(Mtotalref)(=M1423ref)와 보상 전체 상반력 모멘트(Mdmd)의 합의 수평성분에 추 종시키면 된다.

한편, 목표, 전체 상반력 중심점(목표 ZMP)에서는 목표 전체 상반력 모멘트(Mtotalref)의 수평성분은 0이다. 따라서, 로봇(1)의 상체(24)의 전후 좌우의 자세(경사)를 복원시키기 위해서는, 목표 전체 상반력 중심점(목표 ZMP) 주위의 실 전체 상반력 모멘트의 수평성분을 Mdmd의 수평성분(Mdmdx, Mdmdy)에 추종시키면 된다. 또, 본 참고예에서는, 각 접지부위(10)의 목표 상반력 중심점 주위의 실 상반력 모멘트는 0이다.

그래서, 제 1 참고예에서의 계층형 컴플라이언스 동작 결정부(114)는 상기한 요구 1), 2)의 요구를 가능한 한 만족하도록 보용생성장치(100)에서 결정한 각 접지부위(10)의 목표 접지부위 위치(특히 높이 방향의 위치)를 수정한다.

이 수정을 행하기 위해서, 계층형 컴플라이언스 동작 결정부(114)는 보상각이라고 하는 것을 결정한다. 이 보상각은 접지부위(10)의 위치 사이의 상대관계를 어떤 점 주위의 회전조작에 의해 수정하기(본 참고예에서는 상하방향에서 수정하기) 위한 조작량(회전량)이다. 본 참고예에서는, 그 보상각으로서는, 제 14 노드 보상각(θ14), 제 23 노드 보상각(θ23), 제 1423 노드 보상각(θ1423)이 있다. 즉, 잎 노드 이외의 노드의 보상각이다. 그리고, 계층형 컴플라이언스 동작 결정부(114)는 이들 보상각(θ14, θ23, θ1423)을 기초로, 목표 제 n 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23)을 수정하여 이루어지는 목표 제 n 노드 상반력 중심점(Qn')을 결정한다.

도 15에 도시하는 바와 같이, 제 14 노드 보상각(θ14)은 선분 Q1Q4와 선분 Q1'Q4'이 이루는 각도, 제 23 노드 보상각(θ23)은 선분 Q2Q3과 선분 Q2'Q3'이 이루는 각도이다. 또, 도 16에 도시하는 바와 같이, 제 1423 노드 보상각(θ1423)은 선분 Q14Q23과 선분 Q14'Q23'이 이루는 각도이다. 이들 노드 보상각(θ14, θ23, θ1423)의 결정수법은 후술한다.

도 15 및 도 16을 참조하고, 보상각(θ14, θ23, θ1423)을 기초로 한 목표 제 n 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23)의 수정은 다음과 같이 행해진다.

도 15를 참조하고, 목표 제 1 노드 상반력 중심점(Q1)(제 1 접지부위(10)의 목표 상반력 중심점)과 목표 제 4 노드 상반력 중심점(Q4)(제 2 접지부위(10)의 목표 상반력 중심점)을 포함하고 또한 수평면과 수직한 평면의 법선 벡터 V14를 구한다. V14의 크기는 1로 한다. 목표 제 1 노드 상반력 중심점(Q1)의 좌표(위치)를, 목표 제 14 노드 상반력 중심점(Q14)을 회전 중심으로 하여 법선 벡터 V14 주위(Q14를 통과하고 V14에 평행한 축 주위)로 상기 제 14 노드 보상각(θ14)만큼 회전이동 한다. 이 회전이동에 의한 Q1의 이동 후의 점을 Q1'으로 한다. 동일하게, 목표 제 4 노드 상반력 중심점(Q4)의 좌표(위치)를 목표 제 14 노드 상반력 중심점을 회전중심으로 하여 법선 벡터 V14 주위로 제 14 노드 보상각(θ14)만큼 회전이동 한다. 이 회전이동에 의한 Q4의 이동 후의 점을 Q4'으로 한다. 바꾸어 말하면, 선분 Q1Q4를 그 내분점인 Q14를 회전중심으로 하여 V14 주위로 θ14만큼 회전시켜서 얻어지는 선분의 끝점을 Q1', Q4'으로 한다. 따라서, 제 14 노드 보상각(θ14)은 제 14 노드의 목표 상반력 중심점(Q14)의 위치를 움직이지 않고, 제 14 노드의 자 노드인 제 1 노드 및 제 4 노드의 각각의 목표 상반력 중심점(Q1, Q4)의 위치의 상대관계를 움직이기 위한 조작량이다.

또, 목표 제 2 노드 상반력 중심점(Q2)과 목표 제 3 노드 상반력 중심점(Q3)을 포함하고 또한 수평면과 수직한 평면의 법선 벡터 V23을 구한다. V23의 크기는 1로 한다. 목표 제 2 노드 상반력 중심점(Q2)의 좌표(위치)를, 목표 제 23 노드 상반력 중심점(Q23)을 회전중심으로 하여 법선 벡터 V23 주위(Q23을 통과하고 V23에 평행한 축 주위)로, 제 23 노드 보상각(θ23)만큼 회전이동 한다. 도 15에 도시하는 바와 같이 이 회전이동에 의한 Q2의 이동 후의 점을 Q2'으로 한다. 마찬가지로, 목표 제 3 접지부위 상반력 중심점(Q3)의 좌표(위치)를, 목표 제 23 노드 상반력 중심점을 회전중심으로 하여 법선 벡터 V23 주위로, 제 23 노드 보상각(θ23)만큼 회전이동 한다. 이 회전이동에 의한 Q3의 이동 후의 점을 Q3'로 한다. 바꾸어 말하면, 선분 Q2Q3를 그 내분점인 Q23을 회전중심으로 하여 V23 주위로 θ23만큼 회전시켜서 얻어지는 선분의 끝점을 Q2', Q3'으로 한다. 따라서, 제 23 노드 보상각(θ23)은 제 23 노드의 목표 상반력 중심점(Q23)의 위치를 움직이지 않고, 제 23 노드의 자 노드인 제 2 노드 및 제 3 노드의 각각의 목표 상반력 중심점(Q2, Q3)의 위치의 상대관계를 움직이기 위한 조작량이다.

다음에, 도 16을 참조하여, 목표 제 14 노드 상반력 중심점(Q14)과 목표 제 23 노드 상반력 중심점(Q23)을 포함하고 또한 수평면과 수직한 평면의 법선 벡터 V1423을 구한다. V1423의 크기는 1로 한다. 목표 제 14 노드 상반력 중심점(Q14)의 좌표(위치)를 목표 전체 상반력 중심점(P)(=Q1423)을 회전중심으로 하여 법선 벡터 V1423 주위(P를 통과하고 V1423에 평행한 축 주위)로, 제 1423 보상각(θ 1423)만큼 회전이동 한다. 이 회전이동에 의한 Q14의 이동 후의 점을 Q14'으로 한다. 마찬가지로, 목표 제 23 접지부위 상반력 중심점(Q23)의 좌표(위치)를 목표 전체 상반력 중심점(P)을 회전중심으로 하여 법선 벡터 V1423 주위로 제 1423 보상각(θ1423)만큼 회전이동 한다. 이 회전이동에 의한 Q23의 이동 후의 점을 Q23'으로 한다. 바꾸어 말하면, 선분 Q14Q23을 그 내분점인 P(=Q1423)를 회전중심으로 하여 V1423 주위로 θ1423만큼 회전시켜서 얻어지는 선분의 끝점을 Q14', Q23'으로 한다. 따라서, 제 1423 노드 보상각(θ1423)은, 제 1423 노드의 목표 상반력 중심점(P)의 위치를 움직이지 않고, 제 1423 노드의 자 노드인 제 14 노드 및 제 23 노드의 각각의 목표 상반력 중심점(Q14, Q23)의 위치의 상대관계를 움직이기 위한 조작량이다.

또한, 이후, 일반적으로, 시점이 A, 종점이 B인 벡터를 벡터 A_B로 표기한다.

다음에, Q1'을 벡터 Q14_Q14'만큼 이동한 점Q1"을 구한다. 마찬가지로, Q4'을 벡터 Q14_Q14'만큼 이동한 점 Q4"을 구한다. 또, Q2'을 벡터 Q23_Q23'만큼 이동한 점 Q2"을 구한다. 마찬가지로, Q3'을 벡터 Q23_Q23'만큼 이동한 점 Q3"을 구한다.

이어서, 제 n 접지부위(n=1,2,3,4)의 목표 접지부위 위치를 벡터 Qn_Qn"만큼 평행이동(거의 상하이동) 시킨다. 이것에 의해, 각 접지부위(10)의 목표 접지부위 위치가 수정되게 된다. 보충하면, 각 노드의 목표 상반력 중심점과 마찬가지로, 각 잎 노드의 위치를 이 잎 노드에 대응하는 접지부위(10)의 위치로 하고, 자 노드 를 갖는 각 노드의 위치를, 그 노드의 모든 자 노드의 위치의, 상기한 바와 같이 설정된 가중값을 사용한 가중 평균의 위치로서 정의했을 때, 상기한 바와 같이, 제 14 노드, 제 23 노드, 제 1423 노드 각각의 각 노드의 목표 상반력 중심점을 보상각(θ14, θ23, θ1423)에 따라 움직이는 처리는, 제 14 노드, 제 23 노드, 제 1423 노드의 각각의 각 노드의 위치를 보상각(θ14, θ23, θ1423)에 따라 움직이는 것과 동일한 것이다.

또한, 각 다리의 선단부에 프리 조인트가 없는 로봇(접지부위의 자세를 제어가능한 로봇)에서는, 상기 평행이동을 각 접지부위의 자세(목표 자세)를 바꾸지 않고 행한 후에, 제 n 접지부위를 Qn"을 중심으로, 전후 방향축(X축) 주위로, 어떤 회전각 θn_x만큼 더 회전시킴과 동시에, 좌우방향 축(Y축) 주위로, 어떤 회전각 θn_y만큼 회전시킨다. 이것에 의해 각 접지부위의 목표 자세를 수정한다. 이 경우의 회전각θn(θn_x, θn_y로 이루어지는 2차원 양)을 제 n 접지부위 보상각이라고 부르고, θn_x를 제 n 접지부위 보상각 X성분, θn_y를 제 n 접지부위 보상각 Y성분이라고 부른다. 제 n 접지부위 보상각(θn)은, 본원 출원인이 앞서 제안한 일본 특개평10-277969호 공보에 기재된 수법에 따라서 결정하면 된다.

보충하면, 상기 각 노드 보상각은, 그 노드의 목표 노드 상반력 중심점을 작용점으로 하여, 거기에 원래 발생해야 할 목표 상반력 모멘트(그 수평성분은 0)와 노드 보상 상반력 모멘트(Mn_dmd)의 합력이 발생하도록 결정된다. 이 경우, 노드 보상 상반력 모멘트(Mn_dmd)의 기초가 되는 보상 전체 상반력 모멘트(Mdmd)는 실 자세 경사 편차를 0에 근접시키도록 결정된 것이다. 따라서, 각 노드 보상각은 목 표 전체 상반력 중심점 주위의 상반력 모멘트를 원하는 모멘트(이 경우에는 보상 전체 상반력 모멘트)에 근접시키면서, 실 자세 경사 편차를 0에 근접시키도록 각 접지부위(10)의 상대위치 관계를 조작하기 위한 조작량으로서 기능하게 된다.

이상과 같이, 각 접지부위(10)의 목표 접지부위 위치(상세하게는 그 위치의 상호의 상대관계)를 수정하는 처리가 제 1 참고예에서의 계층형의 컴플라이언스 동작이다. 이 컴플라이언스 동작에서는, 자 노드를 갖는 각 노드 마다, 그 노드의 자손 노드로 되어 있는 각 접지부위(10)의 목표 접지부위 위치 사이의 상대관계(상대적 위치관계)의 조작량(수정량)이 각 보상각(θ14, θ23, θ1423)에 따라 정해지고, 그것들의 조작량(수정량)을 합성함으로써, 각 접지부위(10)의 목표 접지부위 위치의 상호의 상대관계가 수정되게 된다. 상기와 같이 수정된 목표 접지부위 위치 자세를 수정 목표 접지부위 위치 자세라고 한다.

이러한 컴플라이언스 동작에서는, 일반적으로, 상기 보상각(보상 동작량)이 과대하면, 각 접지부위(10)의 접지 압력 분포는 바뀌어도, 접지영역(접지면의 압력이 정인 영역)은 바뀌지 않는다. 이러한 경우에는, 보상각에 비례하여 각 접지부위(10)에 장착된 컴플라이언스 기구(42) 등이 변형되고, 그 변형량에 따른 각 접지부위(10)의 실 상반력이 발생한다. 이 결과, 보상각과 보상각에 의해 발생하는 실 상반력의 변화량 사이의 관계는, 이하에 나타내는 양호한 특성, 즉 선형특성을 갖는다.

특성 1) 잎 노드가 아닌 노드의 목표 상반력 중심점 주위의 보상각 만큼을 조작하여 각 접지부위(10)의 목표 접지부위 위치를 이동시키면, 내려간 접지부 위(10)의 실 상반력의 병진력 성분이 증가하고, 올라간 접지부위(10)의 실 상반력의 병진력 성분이 감소한다. 이때, 각 접지부위(10)(각 잎 노드)의 수정된 목표 상반력 중심점 주위의 실 상반력 모멘트는 거의 변화되지 않는다.

특성 2) 제 n 접지부위 보상각 만큼을 조작하여 제 n 접지부위의 목표 자세를 회전시키면, 제 n 접지부위의 목표 상반력 중심점에 작용하는 실 상반력의 모멘트 성분이 변화되고, 병진력 성분은 그다지 변화되지 않는다.

특성 3) 잎 노드가 아닌 노드의 목표 상반력 중심점 주위의 보상각과 제 n 접지부위 보상각을 동시에 조작하면, 각 접지부위(10)의 실 상반력의 변화량은 각각을 단독으로 조작했을 때의 변화량의 합이 된다.

도 17은, 본 참고예에서의 계층형 컴플라이언스 동작 결정부(114)의 기능을 나타내는 블럭도이며, 동 도면을 참조하여 계층형 컴플라이언스 동작 결정부(114)의 처리를 더욱 상세하게 설명한다.

계층형 컴플라이언스 동작 결정부(114)는, 그 기능적 수단으로서, 보상 전체 상반력 모멘트 분배기(114a), 보상각 결정부(114b, 114c, 114d), 수정 목표 접지부위 위치 자세 산출부(114g), 기구변형 보상량 산출부(114n), 및 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)를 구비하고 있다.

보상 전체 상반력 모멘트 분배기(114a)는, 상기 보상 전체 상반력 모멘트(Mdmd)(Mdmdx, Mdmdy)를, 제 1423 노드 보상 상반력 모멘트(M1423dmd), 제 14 노드 보상 상반력 모멘트(M14dmd) 및 제 23 노드 보상 상반력 모멘트(M23dmd)로 분배하는 것이다.

제 1423 노드 보상 상반력 모멘트(M1423dmd)는 제 1423 보상각(θ1423)을 조작함으로써 발생하는 각 접지부위(10)(상세하게는 제 1~제 4 접지부위)의 상반력의 병진력 성분이 목표 제 1423 노드 상반력 중심점(=목표 전체 상반력 중심점(목표 ZMP)) 주위에 발생해야 할 모멘트의 목표값이다.

제 1423 노드 보상 상반력 모멘트(M1423dmd)의 상기 벡터 V1423 방향의 성분(V1423 방향의 축 주위의 성분)을 M1423dmdv라고 기술한다. 한편, 벡터 V1423은 계층형 컴플라이언스 동작 결정부(114)의 컴플라이언스 동작의 전체적인 설명에서 정의한 벡터이다(도 16 참조). V1423에 직교하고, 또한 연직방향에도 직교하는 벡터를 U1423으로 하면, 본 참고예에서는, 제 1423 노드 보상 상반력 모멘트(M1423dmd)의 U1423 방향의 성분(U1423 방향의 축 주위의 성분) M1423dmdu는 0으로 설정된다. 이것은, 본 참고예의 로봇(1)에서는, 제 1423 노드 보상각(θ1423)을 조작해도, 상반력 모멘트의 U1423 방향 성분을 발생할 수는 없기 때문이다. 또, 본 참고예에서는 연직축 주위의 로봇(1)의 자세의 회전을 제어하지 않으므로, M1423dmd의 연직방향 성분 M1423dmdz도 0으로 설정된다.

제 14 노드 보상 상반력 모멘트(M14dmd)는 제 14보상각(θ14)를 조작함으로써 발생하는 각 접지부위(10)(상세하게는 제 1 및 제 4 접지부위)의 상반력의 병진력 성분이 목표 제 14 노드 상반력 중심점 주위에 발생해야 할 모멘트의 목표값이다.

제 14 노드 보상 상반력 모멘트(M14dmd)의 벡터 V14 방향 성분을 M14dmdv라고 기술한다. 또한, 벡터 V14는 계층형 컴플라이언스 동작 결정부(114)의 컴플라 이언스 동작의 전체적인 설명에서 정의한 벡터이다(도 15를 참조). V14에 직교하고, 또한 연직방향에도 직교하는 벡터를 U14로 하면, 본 참고예에서는, 제 14 노드 보상 상반력 모멘트(M14dmd)의 U14 방향 성분(M14dmdu)은 0으로 설정된다. 이것은, 본 참고예의 로봇(1)에서는, 제 14 노드 보상각(θ14)을 조작해도, 상반력 모멘트의 U14 방향 성분을 발생할 수는 없기 때문이다. 또한, 본 참고예에서는, M14dmd의 연직방향 성분도 0으로 설정된다.

제 23 노드 보상 상반력 모멘트(M23dmd)는 제 23 보상각(θ23)을 조작함으로써 발생하는 각 접지부위(10)(상세하게는 제 2 및 제 3 접지부위)의 상반력의 병진력 성분이 목표 제 23 노드 상반력 중심점 주위에 발생해야 할 모멘트의 목표값이다.

제 23 노드 보상 상반력 모멘트(M23dmd)의 벡터 V23 방향 성분을 M23dmdv라고 기술한다. 또한, 벡터 V23은 계층형 컴플라이언스 동작 결정부(114)의 컴플라이언스 동작의 전체적인 설명에서 정의한 벡터이다(도 15를 참조). V23에 직교하고, 또한 연직방향에도 직교하는 벡터를 U23으로 하면, 본 참고예에서는, 제 23 노드 보상 상반력 모멘트(M23dmd)의 U23 방향 성분(M23dmdu)은 0으로 설정된다. 이것은, 본 참고예의 로봇(1)에서는, 제 23 노드 보상각(θ23)을 조작해도, 상반력 모멘트의 U23 방향 성분을 발생할 수는 없기 때문이다. 또한, 본 참고예에서는, M23dmd의 연직방향 성분도 0으로 설정된다.

제 1423 노드 보상 상반력 모멘트(M1423dmd), 제 14 노드 보상 상반력 모멘트(M14dmd) 및 제 23 노드 보상 상반력 모멘트(M23dmd)는, 예를 들면 다음과 같이 결정된다.

임의의 제 n 노드에 대해, 목표 제 n 노드 상반력 중심점에 제 n 노드 보상 상반력 모멘트를 더하는 것과 등가가 되도록, 제 n 노드 보상 상반력 모멘트의 수평성분을 0으로 한 채, 목표 제 n 노드 상반력 중심점을 수정한 경우에 있어서의 수정된 목표 제 n 노드 상반력 중심점 위치를 수정 목표 제 n 노드 상반력 중심점 위치라고 부른다.

수정 목표 제 1423 노드 상반력 중심점(Pmdfd)(=Q1423mdfd), 수정 목표 제 14 노드 상반력 중심점(Q14mdfd), 수정 목표 제 23 노드 상반력 중심점(Q23mdfd)과 각 노드 보상 상반력 모멘트의 관계를 다음 식 7~9에 나타낸다. 또한, 목표 제 1423 노드 상반력 중심점(Q1423)은 전체 상반력 중심점(P)과 일치하므로, 수정 목표 제 1423 노드 상반력 중심점(Pmdfd)은 수정 목표 전체 상반력 중심점이라고 하는 경우도 있다.

M1423dmd=(Pmdfd-P)*Ftotalref …식 7

M14dmd=(Q14mdfd-Q14)*F14ref …식 8

M23dmd=(Q23mdfd-Q23)*F23ref …식 9

또한, 식 7중의(Pmdfd-P) 등, 2개의 점의 차는, 그들 점의 위치 벡터의 차를 의미한다. 또, Ftotalref, F14ref, F23ref는 도 10에 도시한 바와 같이, 각각 제 1423 노드, 제 14 노드, 제 23 노드의 목표 노드 상반력의 병진력 성분이다.

로봇(1)의 접지성을 높게 유지하기 위해서는, 접지해야 할 접지부위(10)의 상반력이 0에 지나치게 근접하거나, 부의 값으로 되려고 하는 것과 같이 제어해서 는 안된다. 따라서, 이하의 조건 1)~3)을 만족해야 한다.

수정 노드 존재위치 조건 1)

Pmdfd는 전체 상반력 중심점(P)으로부터 선분 Q14Q23의 끝점에 지나치게 근접하지 않고, 또한, 선분 Q14Q23상에 있을 것. 이와 같이, 수정 목표 제 1423 노드 상반력 중심점(수정 목표 전체 상반력 중심점)(Pmdfd)가 존재해야 할 범위를 수정 목표 제 1423 노드 상반력 중심점(수정 목표 전체 상반력 중심점)의 존재 허용범위라고 부른다.

수정 노드 존재위치 조건 2)

Q14mdfd는 목표 제 14 노드 상반력 중심점(Q14)으로부터 선분 Q1Q4의 끝점에 지나치게 근접하지 않고, 또한, 선분 Q1Q4상에 있을 것. 이와 같이, 수정 목표 제 14 노드 상반력 중심점(Q14mdfd)이 존재해야 할 범위를 수정 목표 제 14 노드 상반력 중심점의 존재 허용범위라고 부른다.

수정 노드 존재위치 조건 3)

Q23mdfd는 목표 제 23 노드 상반력 중심점(Q23)으로부터 선분 Q2Q3의 끝점에 지나치게 근접하지 않고, 또한, 선분 Q2Q3상에 있을 것. 이와 같이, 수정 목표 제 23 노드 상반력 중심점(Q23mdfd)이 존재해야 할 범위를 수정 목표 제 23 노드 상반력 중심점의 존재 허용범위라고 부른다.

한편, 로봇(1)에 실제로 발생하는 자세복원력(상체(24)의 경사의, 목표 상체자세의 경사에의 복원력)을 적절하게 하기 위해서는, 제 1423 노드 보상 상반력 모멘트(M1423dmd), 제 14 노드 보상 상반력 모멘트(M14dmd) 및 제 23 노드 보상 상반 력 모멘트(M23dmd)의 합력이 보상 전체 상반력 모멘트(Mdmd)에 대략 일치해야 한다. 즉, 이하의 식 10을 대략 만족해야 한다.

Mdmd=M1423dmd+M14dmd+M23dmd …식 10

그래서, 본 참고예에서는, 제 1423 노드 보상 상반력 모멘트(M1423dmd), 제 14 노드 보상 상반력 모멘트(M14dmd) 및 제 23 노드 보상 상반력 모멘트(M23dmd)는, 그것들과 상기 식 7, 식 8, 식 9에 의해 정해지는 수정 목표 노드 상반력 중심점(Pmdfd, Q14mdfd, Q23mdfd)의 위치가 상기 수정 노드 존재위치 조건 1), 2) 및 3)을 만족하고 있는 한은, 다음 식 11~13에 의해 결정된다.

M1423dmd=Mat1423*Mdmd*V1423 …식 11

M14dmd=Mat14*Mdmd*V14 …식 12

M23dmd=Mat23*Mdmd*V23 …식 13

단, Mat1423, Mat14, Mat23은 게인 행렬(제 3 요소가 0인 1행 3열의 행렬)이며, 이것들은, 식 11, 식 12 및 식 13에 의해 따라서 구해지는 M1423dmd, M14dmd, M23dmd가 식 10을 만족하도록 설정된다.

보다 엄밀하게는, 식 10, 식 11, 식 12 및 식 13으로 이루어지는 연립방정식이 Mdmd의 값에 관계없이 항등적으로 성립하도록, 게인 행렬 Mat1423, Mat14, Mat23이 결정된다. 상기 연립방정식이 항등적으로 성립하기 위한 게인 행렬은 일의적으로 결정되는 것은 아니므로, 예를 들면 수정 노드 존재위치 조건 1), 2) 및 3)의 어느 것을 특별히 중시하는지에 따라, 그것에 적절한 게인 행렬을 결정하면 된다. 보충하면, 각 보상각(θ1423, θ14, θ23)을 연속적으로 변화시키기 위해 서, 게인 행렬은 연속적으로 변화시키는 것이 바람직하다. 또, 로봇(1)의 직립시인지의 여부나, 이동형태의 차이 등에 의해, 게인 행렬의 설정 방침을 바꾸어도 된다.

식 11, 식 12 및 식 13에 따라서 결정되는 제 1423 노드 보상 상반력 모멘트(M1423dmd), 제 14 노드 보상 상반력 모멘트(M14dmd) 및 제 23 노드 보상 상반력 모멘트(M23dmd)가 수정 노드 존재위치 조건 1), 2) 및 3)중 어느 하나를 만족하고 있지 않는 경우에는, 이 조건 1), 2), 3)을 만족하도록, 이것들을 수정한다. 보다 구체적으로는 수정 목표 제 n 노드 상반력 중심점(n=1423,14,23) 중, 상기 존재 허용범위를 초과한 수정 목표 노드 상반력 중심점을 그 존재 허용범위의 경계의 점에 설정하고, 그밖의 수정 목표 노드 상반력 중심점을 식 11, 식 12 및 식 13을 가능한 한 만족하도록(좌변과 우변의 차의 절대값이 가능한 한 작아지도록) 결정한다. 단, 일반적으로 피드백 제어에서는 피드백량이 조금 변해도, 제어대상의 거동이 크게 바뀌는 일은 없으므로, 식 11, 식 12 및 식 13을 엄밀하게 또는 억지로 만족시킬 필요는 없다.

이상과 같이, 보상 전체 상반력 모멘트 분배기(114a)는 각 노드 보상 상반력 모멘트(M1423dmd, M14dmd, M23dmd)를 결정한다. 도 14에는, 이렇게 하여 결정된 각 노드 보상 상반력 모멘트(M1423dmd, M14dmd, M23dmd)의 예를 나타냈다. 도면 중의 Mdmd는 상기 도 13에 도시한 것과 같다.

또, 보상 전체 상반력 모멘트 분배기(114a)는, 각 접지부위(10)의 목표 상반력(각 잎 노드의 목표 노드 상반력)인 목표 접지부위 상반력, 각 노드 보상 상반력 모멘트(M1423dmd, M14dmd, M23dmd)를 기초로, 목표 접지부위 상반력에 각 노드 보상 상반력 모멘트를 부가함으로써 수정되는 각 접지부위(10)의 목표 상반력인 수정 목표 접지부위 상반력(Fn_refmdfd)(n=1,2,3,4)을 구한다.

이때, 각 접지부위(10)의 목표 상반력 중심점(잎 노드의 목표 노드 상반력 중심점)(Qn)(n=1,2,3,4), 목표 전체 상반력 중심점(P), 각 목표 노드 상반력 중심점(Q14, Q23)과 목표 전체 상반력(Ftotalref)으로부터, 각 목표 노드 상반력을 구하는 수법과 동일한 수법에서 의해, 각 접지부위(10)의 목표 상반력 중심점(Qn)(n=1,2,3,4), 수정 전체 상반력 중심점(Pmdfd), 각 수정 목표 노드 상반력 중심점(Q14mdfd, Q23mdfd)과 목표 전체 상반력(Ftotalref)으로부터, 수정 목표 접지부위 상반력(Fn_refmdfd)(n=1,2,3,4)을 구하면 된다. 즉, 각 접지부위(10)의 목표 상반력 중심점(Qn)(n=1,2,3,4), 수정 전체 상반력 중심점(Pmdfd), 각 수정 목표 노드 상반력 중심점(Q14mdfd, Q23mdfd)으로부터, 상기 식 1~3에 기초하여 각 노드의 가중값을 결정하고, 그 가중값을 사용하여 상기 식 4에 기초하여 수정 목표 접지부위 상반력(Fn_refmdfd)(n=1,2,3,4)을 구하면 된다.

이것으로부터 명확한 바와 같이, 자 노드를 갖는 제 n 노드의 목표 제 n 노드 상반력 중심점에 노드 보상 상반력 모멘트를 발생시킨(목표 제 n 노드 상반력 중심점에 작용하는 목표 상반력의 모멘트 성분을 수정한)다는 것은, 그 제 n 노드의 자 노드의 가중값을 수정하는 것과 동등하다.

이상에서 설명한 보상 전체 상반력 모멘트 분배기(114a)의 처리는 도 18에 도시하는 기능 블럭도에 의해 표시된다. 즉, 목표 노드 상반력 중심 점(Qn)(n=1,2,3,4,14,23)을 기초로, 제 14 노드, 제 23 노드 및 수정 목표 노드 상반력 중심점(Qn_mdfd)(n=14,23,1423)의 존재 허용범위가 상기 수정 노드 존재위치 조건 1), 2), 3)에 따라서 결정된다. 또, 보상 전체 상반력 모멘트(Mdmd)와, 각 노드의 목표 상반력 중심점(Qn)(n=1,2,3,4,14,23,1423) 및 목표 상반력(Fn_ref)(n=1,2,3,4,14,23,1423)과, 상기 존재 허용범위를 기초로, 수정 목표 노드 상반력 중심점(Qn_mdfd)(n=14,23,1423) 및 노드 보상 상반력 모멘트 (Mn_dmd)(n=14,23,1423)가 결정된다. 또, 수정 목표 노드 상반력 중심점(Qn_mdfd)(n=14,23,1423)과, 각 접지부위(10)의 목표 상반력 중심점(Qn)(n=1,2,3,4)과, 목표 전체 상반력(Ftotalref)으로부터, 수정 목표 접지부위 상반력(Fn_refmdfd)(n=1,2,3,4)이 구해진다.

다음에, 상기 보상각 결정부(114b~114d) 중 보상각 결정부(θ1423 결정부)(114b)의 처리를 도 19의 블럭도를 참조하여 설명한다. 목표 제 14 노드 상반력 중심점(Q14)에 실 제 1 노드 상반력과 실 제 4 노드 상반력의 합력의 병진력 성분(F1act+F4act)이 작용함과 동시에, 목표 제 23 노드 상반력 중심점(Q23)에 실 제 2 노드 상반력과 실 제 3 노드 상반력의 합력의 병진력 성분(F2act+F3act)이 작용한 경우에, 그것들의 병진력 성분이 목표 전체 상반력 중심점(P)(목표 제 1423 노드 상반력 중심점)의 주위에 발생하는 모멘트(M1423act)를 다음 식 14에 의해 구한다.

M1423act=P_Q14*(F1act+F4act)+P_Q23*(F2act+F3act) …식 14

여기에서, P_Q14는 시점이 P, 종점이 Q14인 벡터, P_Q23은 시점이 P, 종점이 Q23인 벡터이다.

또한, 식 14 대신, 다음 식 15를 사용하여 M1423act를 산출해도 실제상은 거의 문제가 없다.

M1423act=P_Q1*F1act+P_Q2*F2act

+P_Q3*F3act+P_Q4*F4act …식 15

식 15의 우변은, 제 1~제 4 노드(각 잎 노드)의 실 상반력의 병진력 성분(Fn_act)(n=1,2,3,4)로부터 목표 전체 상반력 중심점(P) 주위에 작용하는 실 전체 상반력 모멘트(Mtotalact)를 산출하는 식으로 되어 있다. 보충하면, 식 14는, 목표 전체 상반력 중심점(P) 주위에 작용하는 실 전체 상반력 모멘트(Mtotalact)로부터, 목표 제 14 노드 상반력 중심점(Q14) 주위에 작용하는 실 상반력 모멘트와 목표 제 23 노드 상반력 중심점(Q23) 주위에 작용하는 실 상반력 모멘트를 뺀 것으로 되어 있다.

일반적으로, 잎 노드인 임의의 제 n 노드의 실 제 n 노드 상반력 모멘트(Mn_act)는 제 n 접지부위의 실 상반력 모멘트로 한다. 또, 잎 노드가 아닌 임의의 제 n 노드에 대하여, 그 모든 자 노드의 상반력(여기에서의 자 노드의 상반력은, 엄밀하게는, 이 자 노드의 목표 상반력 중심점에 작용하는 상반력)이 목표 제 n 노드 상반력 중심점에 작용하는 모멘트를 실 제 n 노드 상반력 모멘트 Mn_act라고 부른다. 식 14 및 식 15와 같이, 실 제 n 노드 상반력 모멘트는 자 노드의 실 상반력 모멘트를 포함하는 정의와 포함하지 않는 정의가 있지만 어느 것을 사용해도 상관없다. 특히, 자 노드의 실 상반력 모멘트의 수평성분을 제어하는 피드백 제어의 응답성이 높으면, 자 노드의 실 상반력 모멘트의 수평성분은 곧 0에 수렴하므로, 어느 쪽의 정의를 사용해도, 제 n 노드의 실 상반력의 제어의 거동은 그다지 바뀌지 않는다.

이하에 나타내는 식 16은, 식 14에 대응한 실 제 n 노드 상반력 모멘트의 산출의 일반식, 식 17은 식 15에 대응한 실 제 n 노드 상반력 모멘트의 산출의 일반식이다. 단, 식 16 및 식 17에서, Qn_Qm은 시점이 Qn, 종점이 Qm인 벡터이다. 또, 잎 노드인 제 m 노드의 실 상반력 모멘트(Mm_act)는 실 상반력 검출기에서 검출된 각 접지부의 실 상반력 모멘트로 한다.

m∈{제 n 노드의 자 노드의 번호의 집합}이라고 하면

Mn_act=Σ(Qn_Qm*Fm_act) …식 16

m∈{제 n 노드의 자손인 잎 노드의 번호의 집합}이라고 하면

Mn_act=Σ(Qn_Qm*Fm_act+Mm_act) …식 17

또한, 식 16, 17중 Σ는 m에 대한 총합을 의미한다. 보충하면, 제 1 참고예 형태에서는, 잎 노드(각 접지부위)의 목표 상반력 중심점 주위의 실 상반력 모멘트가 0이 되므로, 상기 식 15의 우변에서는, 식 17의 Mm_act의 성분이 포함되어 있지 않다. 접지부위의 자세를 제어가능한 로봇에서는, 일반적으로는, 식 17의 Mm_act는 0이 되지 않는다.

제 1423 노드 보상각(θ1423)은, 일반적으로는, 상기한 바와 같이 구해지는 M1423act와 먼저 보상 전체 상반력 모멘트 분배기(114a)에서 구해진 제 1423 노드 보상 상반력 모멘트(M1423dmd)와의 편차(M1423act-M1423dmd)를 0에 근접시키도록 피드백 제어칙 등에 의해 결정하면 된다. 예를 들면 이 편차에 소정의 게인 행렬(3차의 대각 행렬)을 승산함으로써 θ1423을 구하면 된다.

단, 본 참고예에서는, 상기 벡터 V1423의 방향의 축 주위의 제 1423 노드 보상각(θ1423)을 결정하면 되므로, M1423act의 벡터 V1423 방향의 성분 M1423actv와 M1423dmd의 벡터 V1423 방향의 성분(M1423dmdv)의 편차에 따라 θ1423을 결정하면 된다. 그리고, 이때, 본 참고예에서는, 상반력의 제어의 응답성이나 안정성을 높이기 위해서, M1423actv와 M1423dmdv를 필터에 통과시킨 뒤에, 그들 편차에 따라 θ1423을 결정하는 것으로 했다.

구체적으로는, 상기와 같이 구한 M1423act의 벡터 V1423 방향의 성분(M1423actv)을 추출한다. 이것은, 벡터의 내적 연산 「?」을 사용한 다음 식 18에 의해 얻어진다.

M1423actv=M1423act?V1423 …식 18

이상과 같이 M1423actv를 산출하는 처리가 도 19의 참조부호 114k를 붙인 연산부에 의해 실행된다.

다음에 상기 M1423actv를 로 패스 필터(114i)에 통과시키고, M1423actvfilt를 얻는다. 또, 상기 제 1423 노드 보상 상반력 모멘트(M1423dmd)의 V1423 방향의 성분(M1423dmdv)을 보상 필터(114j)에 통과시키고, M1423dmdvfilt를 얻는다. 또한, M1423dmdv는, 상기 식 18과 동일하게, M1423dmd와 V1423의 내적 연산에 의해 구해진다. 그리고, M1423dmdvfilt를 M1423actvfilt로부터 감산한 것을 편차 모멘트 V1423 방향 성분(M1423errv)으로서 얻는다.

또한, 상기 보상 필터(114j)는, 제어계에서의 M1423dmdv로부터 실 전체 상반력 모멘트까지의 전달함수의 주파수 응답특성을 개선하는 것이다.

최후에 다음 식 19의 피드백 제어칙(여기에서는 비례제어칙)의 연산에 의해 상기 제 1423 노드 보상각(θ1423)을 얻는다. 여기에서 K1423은 제어 게인이며, 통상, 이것은 정의 값으로 설정한다.

θ1423=K1423*M1423errv …식 19

즉, 편차 모멘트 V1423 방향 성분(M1423errv)에 제어 게인(K1423)을 승산함으로써, 제 1423 노드 보상각(θ1423)을 얻는다.

다음에 도 17의 보상각 결정부(θ14 결정부)(114c)의 처리를, 도 20을 참조하여 설명하면 목표 제 1 노드 상반력 중심점(Q1)에 실 제 1 노드 상반력(제 1 접지부위(10)의 실 상반력)의 병진력 성분(F1act)이 작용함과 동시에, 목표 제 4 노드 상반력 중심점(Q4)에 실 제 4 노드 상반력(제 4 접지부위(10)의 실 상반력)의 병진력 성분(F4act)과 작용한 경우에, 그들 병진력 성분이 목표 제 14 노드 상반력 중심점(Q14)의 주위에 발생하는 모멘트(M14act)를 일반식인 상기 식 16을 사용하여 구한다.

보다 구체적으로는, 다음 식 20에 의해 구한다.

M14act=Q14_Q1*F1act+Q14_Q4*F4act …식 20

여기에서, Q14_Q1은 시점이 Q14, 종점이 Q1인 벡터, Q14_Q4는 시점이 Q14, 종점이 Q4인 벡터이다.

또, 식 20 대신, 다음 식 21(상기 식 17을 구체화한 식)을 사용해도, 전술의 이유에 의해, 실제상은 거의 문제가 없다.

M14act=Q14_Q1*F1act+Q14_Q4*F4act+M1act+M4act …식 21

단, M1act는 실 제 1 노드 상반력 모멘트, M4act는 실 제 4 노드 상반력 모멘트이다. 또한, 제 1 참고예에서는 각 다리(#1~#4)의 선단부에 프리 조인트(볼 관절(12))를 구비하고 있으므로, M1act, M4act는 0이다.

식 21은 제 14 노드의 모든 잎 노드의 실 상반력의 합력이 목표 제 14 노드 상반력 중심점 주위에 작용하는 모멘트를 산출하는 식으로 되어 있다. 또한, 식 20은, 제 14 노드의 모든 잎 노드의 실 상반력의 합력이 목표 제 14 상반력 중심점 주위에 작용하는 모멘트로부터, 실 제 1 노드 상반력 모멘트와 실 제 4 노드 상반력 모멘트를 감산한 것으로 되어 있다.

제 14 노드 보상각(θ14)은, 일반적으로는, 상기와 같이 구해지는 M14act와 먼저 보상 전체 상반력 모멘트 분배기(114a)에서 구해진 제 14 노드 보상 상반력 모멘트(M1423dmd)의 편차(M14act-M14dmd)를 0에 근접시키도록 피드백 제어칙 등에 의해 구하면 된다.

단, 본 참고예에서는, θ1423의 경우와 동일한 이유에 의해, M14act의 상기 벡터 V14 방향의 성분(M14actv)과 M14dmd의 벡터 V14 방향의 성분(M14dmdv)을 각각 필터에 통과시킨 것의 편차에 따라 θ14를 결정하는 것으로 했다.

즉, 상기와 같이 구한 M14act의 벡터 V14 방향 성분(M14actv)을 추출한다. 이것은 벡터의 내적 연산을 사용한 다음 식 22에 의해 얻어진다.

M14actv=M14act?V14 …식 22

이상과 같이 M14actv를 산출하는 처리가 도 20의 참조부호(114k')를 첨부한 연산부에 의해 실행된다.

다음에, 상기 M14actv를 로 패스 필터(114i')에 통과시키고, M14actvfilt를 얻는다. 또, 상기 제 14 노드 보상 상반력 모멘트(M14dmd)의 V14 방향의 성분(M14dmdv)을 보상 필터(114j')에 통과시키고, M14dmdvfilt를 얻는다. 또한, M14dmdv는 M14dmd와 V14의 내적 연산에 의해 구해진다. 그리고, M14dmdvfilt를 M14actvfilt로부터 감산한 것을 편차 모멘트 V14 방향 성분(M14errv)으로서 얻는다. 또한, 보상 필터(114j')는, 제어계에서의 M14dmdv로부터 실 전체 상반력 모멘트까지의 전달함수의 주파수 응답특성을 개선하는 것이다.

최후에 다음 식 23의 피드백 제어칙(여기에서는 비례제어칙)의 연산에 의해 상기 제 14 노드 보상각(θ14)을 얻는다. 여기에서 K14는 제어 게인이며, 통상, 이것은 정의 값으로 설정한다.

θ14=K14*M14errv …식 23

도 17의 보상각 결정부(θ23 결정부)(114d)의 처리는 제 14 노드 보상각(θ14) 결정부(114c)의 처리와 동일하므로, 여기에서는 상세한 설명을 생략한다. 그 처리의 개요는 다음과 같다. 제 23 노드의 자 노드의 실 상반력에 의해 목표 제 23 노드 상반력 중심점(Q23)의 주위에 발생하는 모멘트(M23act)의 벡터 V23 방향의 성분(M23actv)이 상기 식 16 또는 17에 기초하여 산출된다. 그리고, 그 M23actv를 로 패스 필터에 통과시킨 것(M23actvfilt)을, 상기 제 23 노드 보상 상반력 모멘트(M23dmd)의 V23 방향의 성분(M23dmdv)을 보상 필터에 통과시킨 것(M23dmdvfilt) 으로부터 감산하여 이루어지는 편차 모멘트 V23 방향 성분(M23errv)으로부터 피드백 제어칙(비례제어칙)의 연산에 의해, 상기 제 23 노드 보상각(θ23)이 산출된다.

이상의 보상각 결정부(114b~114d)의 처리에 의해, 목표 전체 상반력 중심점(P)에 작용하는 실 모멘트(Mact)의 수평성분을 보상 전체 상반력 모멘트(Mdmd)에 근접시키도록 노드 보상각(θ1423, θ14, θ23)의 세트가 결정되게 된다. 또한, θ1423은, 본 참고예에서는, 상기 식 19에 의해 구했지만, 식 19의 우변의 M1423err 대신, M1423act+M14act+M23act(실 노드 상반력 모멘트의 총 합)와 M1423dmd와의 편차, 또는, 실 노드 상반력 모멘트의 총 합과 M1423dmd를 각각 필터에 통과시킨 것의 편차에 따라 결정하도록 해도 된다.

보충하면, 본 참고예에서는, 각 노드의 목표 노드 상반력 중심점(Qn)을 변경하지 않고, 그 Qn을 작용점으로 하여, 노드 보상 상반력 모멘트를 결정했다. 그리고, 이 노드 상반력 보상 모멘트(보다 정확하게는, 이 노드 보상 상반력 모멘트와 Qn을 작용점으로 하는 목표 노드 상반력의 모멘트 성분의 합력)와 Qn을 작용점으로 하는 실 노드 상반력 모멘트의 편차를 제어량으로 하고, 이 제어량을 0에 근접시키도록 노드 보상각을 결정하도록 했다. 이와 같이 노드 보상각을 결정하는 대신 다음과 같이 노드 보상각(θ1423, θ14, θ23)을 결정하도록 해도 된다. 즉, 자 노드를 갖는 각 제 n 노드(n=14,23,1423)에 대하여, 그 제 n 노드의 실 노드 상반력(제 n 노드의 모든 자 노드의 실 노드 상반력의 합력)의 모멘트의 수평성분이 0이 되는 상반력 중심점을 실 제 n 노드 상반력 중심점으로서 구한다. 또는, 제 n 노드(n=14,23,1423)의 실 노드 상반력의 모멘트로부터, 그 각 노드의 실 노드 상반력 이 이 자 노드의 목표 노드 상반력 중심점에 작용하는 모멘트를 감산하여 이루어지는 모멘트의 수평성분이 0이 되는 상반력 중심점을 실 제 n 노드 상반력 중심점으로서 구한다. 예를 들면 제 14 노드에 관한 실 제 14 노드 상반력 중심점은, 상기 식 16 또는 식 17에 의해 구한 M14act의 수평성분을, F1act와 F4act의 합력(실 제 14 노드 상반력의 병진력 성분)의 연직성분으로 나눈 값만큼, 목표 제 14 노드 상반력 중심점을 선분 Q1Q4 상에서 옮겨놓은 점으로서 구해진다. 제 23 노드, 제 1423 노드의 노드 상반력 중심점에 대해서도 동일하다. 그리고, 제 n 노드(n=14,23,1423)의 목표 노드 상반력 중심점과, 상기와 같이 구한 실 제 n 노드 상반력 중심점과의 편차, 또는, 그들 노드 상반력 중심점의 각각을 필터에 통과시킨 값의 편차를 제어량으로 하고, 그 제어량을 0에 근접시키도록, 이 제어량에 따라 노드 보상각(θ1423, θ14, θ23)을 결정한다(예를 들면 제어량에 어떤 게인을 승산함으로써, θ1423, θ14, θ23을 결정함).

도 17의 수정 목표 접지부위 위치 자세 산출부(114g)는, 제 1423 노드 보상각(θ1423), 제 14 노드 보상각(θ14), 제 23 노드 보상각(θ23)에 기초하여, 전술한 계층형 컴플라이언스 동작의 목표 접지부위 위치 자세의 수정 수법(도 15 및 도 16을 참조하여 설명한 수법)에 따라 각 접지부위(10)의 목표 위치 및 목표 자세인 목표 접지부위 위치 자세를 수정하고, 수정 목표 접지부위 위치 자세를 얻는다. 단, 본 참고예에서는, 각 다리(#1~#4)의 선단부에 프리 조인트(볼 관절(12))가 있어, 의도적으로 각 접지부위(10)의 자세를 변화시킬 수는 없으므로, 수정 목표 접지부위 위치 자세는, 실제로는, 수정 목표 접지부위 위치를 의미한다.

도 21은 도 17의 상기 기구변형 보상량 산출부(114n)의 처리를 나타내는 기능 블럭도이다. 도 21에 도시하는 바와 같이, 기구변형 보상량 산출부(114n)는, 상기 보상 전체 상반력 모멘트 분배기(114a)로부터 출력되는 각 접지부위(10)의 수정 목표 접지부위 상반력(Fn_refmdfd)(n=1,2,3,4)(목표 접지부위 상반력에, 제 1423 노드 보상 상반력 모멘트(M1423dmd), 제 14 노드 보상 상반력 모멘트(M14dmd) 및 제 23 노드 보상 상반력 모멘트(M23dmd)를 부가함으로써 수정된 목표 접지부위 상반력)에 의해 발생이 예상되는 각 다리(#1~#4)의 변형량(각 다리의 링크기구 및 컴플라이언스 기구(42)의 변형량)인 기구변형량(En_mdfd)(n=1,2,3,4)을 구한다. 기구변형량(En_mdfd)(n=1,2,3,4)은, 각 다리에 작용하는 힘(또는 상반력)과 이 다리의 변형과의 관계를 나타내는 기구 컴플라이언스 모델을 사용하여 구해진다. 그리고, 기구변형 보상량 산출부(114n)는, 기구변형량(En_mdfd)(n=1,2,3,4)을 없애기 위한 기구변형 보상량(En_cmpn)(n=1,2,3,4)을 구한다. 기구변형 보상량(En_cmpn)은 각 기구변형량 En_mdfd에 (-1)을 승산함으로써 구해진다.

도 17의 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)는, 산출된 기구변형량(En_mdfd)을 없애도록, 각 접지부위(10)의 수정 목표 접지부위 위치 자세(상기 수정 목표 접지부위 위치 자세 산출부(114g)에서 구해진 위치 자세)을 더욱 수정하여, 각 접지부위(10)의 기구변형 보상된 수정 목표 접지부위 위치 자세를 얻는다. 기구변형 보상된 수정 목표 접지부위 위치 자세는, 각 접지부위(10)의 수정 목표 접지부위 위치 자세에, 각각에 대응하는 기구변형 보상량(En_cmpn)을 부가함으로써 결정된다.

예를 들면, 수정 목표 접지부위 상반력에 의해, 어떤 접지부위(10)에 대응하는 다리의 컴플라이언스 기구(42) 등이 연직방향으로 z만큼 줄어들어 산출된 경우에는, 그 접지부위(10)의 목표 위치를 z만큼 내리도록 수정 목표 접지부위 상반력이 수정된다. 즉 기구변형 보상 후의 접지부위(10)의 접지면(밑바닥면)이 목표 접지부위 상반력을 받아 변형했을 때의 위치 자세가, 기구변형 보상전의 접지부위의 접지면의 목표 위치 자세에 일치하도록, 기구변형 보상된 수정 목표 접지부위 위치 자세를 산출한다. 또한, 이것에 대한 상세한 설명은, 일본 특개평10-277969호 공보에 상세하게 설명되어 있다. 또, 본 참고예의 로봇(1)에서는, 접지부위(10)의 자세를 제어할 수 없으므로, 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)에서 실제로 수정되는 것은, 각 접지부위(10)의 수정 목표 접지부위 위치이다.

상기와 같은 기구변형 보상은, 컴플라이언스 기구(42) 등의 변형에 의해 발생하는 실 접지부위 위치 자세의 어긋남을 피드포워드적으로 없애는 제어이며, 이 제어가 없는 경우에 비해, 보다 더한층, 목표 보용에 가까운 형태에서의 로봇(1)의 이동을 실현할 수 있다.

이상이, 계층형 컴플라이언스 동작 결정부(114)의 처리의 상세이다.

상기를 전제로 하여 도 9의 플로우차트의 설명으로 되돌아와, S34에서, 상기와 같이 보상각을 결정한다. 도 22는 그 보상각의 결정처리의 서브루틴을 일반화 하여 나타낸 플로우차트이다.

동 도면을 참조하여 설명하면, 우선 S100에서, 각 접지부위(10)의 실 상반 력(상기 실 상반력 검출기(108)에서 얻어지는 실 상반력)을 기초로, 실 제 n 노드 상반력의 병진력 성분(Fn_act)(n=1,2,3,4,14,23,1423)을 산출한다. 이 경우, 각 잎 노드의 실 노드 상반력의 병진력 성분(Fn_act)(n=1,2,3,4)은, 상기 실 상반력 검출기(108)에 의한 각 접지부위(10)의 실 상반력의 검출값의 병진력 성분이다. 또, 잎 노드가 아닌 각 노드의 실 노드 상반력의 병진력 성분(Fn_act)(n=14,23,1423)은, 상기한 바와 같이, 그 노드의 자 노드의 실 노드 상반력의 합력의 병진력 성분이다.

이어서 S102로 진행되고, 실 제 n 노드 상반력 모멘트 Mn_act(n=1,2,3,4,14,23,1423)을 산출한다. 제 1 참고예의 로봇(1)에서는, 각 잎 노드의 실 노드 상반력 모멘트(Mn_act)(n=1,2,3,4)는 0이다. 또, 잎 노드가 아닌 각 노드의 실 노드 상반력 모멘트(Mn_act)(n=14,23,1423)는, S100에서 얻어진 Fn_act(n=1,2,3,4)와, 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,14,23,1423)으로부터 상기 식 16 또는 식 17에 기초하여 산출된다.

이어서 S104로 진행되고, 상기 자세안정화 제어 연산부(104)에서 구해진 보상 전체 상반력 모멘트(Mdmd)를 기초로, 제 n 노드 보상 상반력 모멘트(Mn_dmd)(n=14,23,1423)를 결정한다. 이 처리는, 상기 보상 전체 상반력 모멘트 분배기(114a)에 의해 상기한 바와 같이 행해진다.

이어서 S106으로 진행되고, 계층형 컴플라이언스 동작의 설명에서 정의한 벡터 V1423, V14, V23 및 이것에 직교한 벡터 U1423, U14, U23을 구한다. 단, 제 1 참고예에서는, 벡터 U1423, U14, U23 방향의 상반력 모멘트를 발생할 수는 없으므 로, U1423, U14, U23을 구할 필요는 없다.

보충하면, 후술하는 제 2 참고예와 같이, 제 n 노드의 자 노드의 수가 3 이상인 경우에는, Vn은, 시간적으로 급격하게 변화되지 않으면, 어느 방향으로 잡아도 되므로, Vn의 방향은 지지 다리 좌표계의 X축의 방향이나, 로봇의 상체의 방향에 맞추는 등, 적당히 결정하면 된다. 또, Un은 Vn에 직교하고 있으면 된다.

이어서 S108로 진행되고, 실 제 n 노드 상반력 모멘트(Mn_act)(n=14,23,1423)의 Vn 방향 성분(Mn_actv)과 Un방향 성분(Mn_actu)을 추출한다. 이것은, Mn_act와 Vn, Un의 내적 연산을 행하면 된다. 또한, 제 1 참고예와 같이 잎 노드가 아닌 임의의 제 n 노드의 자 노드의 수가 2 이하인 경우에는, Un 방향 성분(Mn_actu)은 0이다. 따라서, Mn_actu는 구할 필요는 없다.

이어서 S110로 진행되고, 제 n 노드 보상 상반력 모멘트(Mn_dmd)(n=14,23,1423)의 Vn 방향 성분(Mn_dmdv)과 Un 방향 성분(Mn_dmdu)을 추출한다. 이것은, Mn_dmd와 Vn, Un과의 내적 연산을 행하면 된다. 또한, 제 1 참고예와 같이 잎 노드가 아닌 임의의 제 n 노드의 자 노드의 수가 2 이하인 경우에는, 제 n 노드 보상 상반력 모멘트 Mndmd의 Un방향 성분 Mn_dmdu는 0이 된다. 또는, Mn_dmdu를 구할 필요는 없다.

이어서 S112로 진행되고, Mn_actv를 필터에 통과시킨 값과 Mn_dmdv를 필터에 통과시킨 값의 차에 게인(Kn)을 승산함으로써(보다 일반적으로는, 당해 차로부터 피드백 제어칙에 의해) 제 n 노드 보상각(θn)의 Vn 성분을 결정한다. 제 1 참고예에서는, 이 처리는, 제 14 노드, 제 23 노드, 제 1423 노드에 대해, 상기 각 보 상각 결정부(114b, 114c, 114d)에 의해 상기한 바와 같이 실행된다.

이어서 S114로 진행되고, Mn_actu를 필터에 통과시킨 값과 Mn_dmdu를 필터에 통과시킨 값의 차에 게인(Kn)을 승산함으로써(보다 일반적으로는 당해 차로부터 피드백 제어칙에 의해) 제 n 노드 보상각(θn)의 Un 성분을 결정한다. 단, 제 1 참고예와 같이 잎 노드가 아닌 임의의 제 n 노드의 자 노드의 수가 2 이하인 경우에는, 제 n 노드 보상각 U성분은 0으로 한다. 또는, S114의 처리를 실행할 필요는 없다.

이상이, 도 9의 S34의 서브루틴 처리이다. 보충하면, S106~S114의 처리는, 목표 제 n 노드 상반력 중심점에 작용하는 실 제 n 노드 상반력 모멘트가 제 n 노드 보상 상반력 모멘트(보다 정확하게는 제 n 노드 보상 상반력 모멘트와 목표 제 n 노드 상반력 중심점에 작용하는 목표 노드 상반력 모멘트와의 합력)에 수렴하도록 제 n 노드 보상각을 결정하는 처리라고 할 수 있다.

이어서, 도 9의 플로우차트의 S36으로 진행되고, 상기 기구변형 보상량을 산출한다. 이 처리는, 상기 기구변형 보상량 산출부(114n)에 의해 상기한 바와 같이 행해진다.

이어서 S38로 진행되고 목표 접지부위 위치 자세를 S34에서 구한 보상각에 따라 수정하고, 이것을 S36에서 구한 기구변형 보상량에 따라 더 수정함으로써, 각 접지부위(10)의 기구변형 보상된 수정 목표 접지부위 위치 자세를 얻는다. 제 1 참고예에서는, 상기 수정 목표 접지부위 위치 자세 산출부(114g)에 의해, 상기한 바와 같이(상기 도 15 및 도 16을 참조하여 설명한 바와 같이) 보상각(θ1423, θ 14, θ23)에 기초하여 각 접지부위(10)의 수정 목표 접지부위 위치를 구한다. 그리고, 이 구한 수정 목표 접지부위 위치가 상기 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)에 의해, 상기한 바와 같이 상기 기구변형 보상량(En_cmpn)(n=1,2,3,4)에 따라 더 수정되고, 그것에 의해, 각 접지부위(10)의 기구변형 보상된 수정 목표 접지부위 위치가 얻어진다.

이상에서 설명한 도 9의 S32~S38의 처리가 계층형 컴플라이언스 동작 결정부(114)의 처리이다.

이어서 S40으로 진행되고, 목표 상체위치 자세와 기구변형 보상된 수정 접지부위 위치 자세(제 1 참고예에서는 기구변형 보상된 수정 접지부위 위치)로부터 로봇(1)의 관절 변위 지령을 산출한다. 이 처리는, 상기 로봇 기하학 모델(110)에 의해 상기한 바와 같이 실행된다.

이어서 S42로 진행되고, 실 관절 변위를 관절 변위 지령에 추종시킨다. 이 추종제어(서보제어)는 상기 변위 콘트롤러(112)에 의해 실행된다.

이어서 S44로 진행되고, 시각을 △t(제어장치(50)의 연산처리 주기)만큼 갱신하고, S14로 되돌아와서 상기의 처리를 반복한다.

이상에서 설명한 제 1 참고예와 같은 계층형의 컴플라이언스 동작에 의해, 접지부위가 3개 이상인 이동 로봇에서도, 각 노드 상반력의 제어가 서로 거의 간섭하지 않게 되어, 각 노드 상반력을 용이하고 또한 적절하게 제어할 수 있다. 따라서, 제어의 간섭이 없어, 각 노드의 실 상반력이 바람직한 값으로부터 벗어나거나 발진하지 않는다. 이 때문에, 바닥면의 대역적인 굴곡이나 경사뿐만 아니라, 국소 적인 요철이나 경사 등도 포함하는 예기치 못하는 바닥형상 변화가 있어도, 그 영향을 그다지 받지 않고 다리식 이동 로봇에 작용하는 상반력을 적절하게 제어할 수 있다. 또, 이동 로봇의 자세를 안정화하는 제어를 용이하게 실현할 수 있는 동시에, 이동 로봇이 받는 착지 충격을 저감할 수 있어, 이동 로봇의 접지성을 높여, 이동시의 슬립이나 스핀을 방지할 수 있다. 또한, 이동 로봇의 액추에이터의 부하를 저감할 수 있다. 따라서, 각 접지부위의 상반력을 적절하게 제어하여, 높은 자세안정성을 얻을 수 있다.

로봇의 자세의 안정화에 대한 효과를 보충하면, 로봇이 상정한 바닥면 위를 상정한 대로 보행하고 있는 상태로부터 로봇 전체가 기운 경우에, 경사각 편차(θberr)(θberrx, θberry)와 이것에 따라 발생하는 목표 전체 상반력 중심점 주위의 모멘트 수평성분의 증가량(△M)과의 관계가 비례관계에 있는 것이 바람직하다. 그렇지 않아도, 어떤 회전행렬(T), 어떤 대각 행렬(diag)(a,b)에 대하여, 다음 식 24가 성립하는 것이 바람직하다. 또한, T, diag(a,b)는 2차 정방 행렬이다.

T*△M=diag(a,b)*T*θberr …식 24

이것들의 관계를 만족하지 않을 경우에는, 로봇의 자세가 기운 상태로부터 되돌아올 때에, 직선적으로 경사각 편차(θberr)가 0에 수렴하지 않아, 세차운동을 일으킬 우려가 있다. 예를 들면 로봇의 상체가 전방으로 기운 상태에서는, 후방으로 되돌리는 복원력 이외에, 횡방향으로의 전도력(복원력)이 불필요하게 작용하여, 경사각 편차(θberr)가 직선적으로 0으로 되돌아오지 않고, 후방 횡방향으로 복원력이 작용하여, 경사각 편차(θberr)가 소용돌이 형상으로 0에 수렴해 간다.

또, 상기와 같은 이유로, 컴플라이언스 제어에서는, 로봇 전체의 경사각 편차(θberr)의 변화속도(dθberT/dt)와 이것에 따라 발생하는 모멘트의 증가량(△M_d)의 관계도, 비례관계에 있는 것이 바람직하다. 그렇지 않아도, 어떤 회전 행렬 T, 어떤 대각 행렬(diag)(e,f)에 대하여, 다음 식 25가 성립하는 것이 바람직하다. 또한, T, diag(e,f)는 2차 정방 행렬이다.

T*△M_d=diag(e,f)*T*dθberr/dt …식 25

일반적으로, 로봇의 각 다리에 독립적으로 컴플라이언스 제어를 걸면, 이것들의 관계를 만족시키지 않고, 세차운동을 일으킬 우려가 있다. 제 1 참고예에 나타낸 계층형 컴플라이언스 제어에서는, 상기 식 24, 25의 관계를 만족할 수 있으므로, 로봇의 자세 제어의 수렴성이 높고, 또, 발진이나 진동을 방지할 수 있다.

상기 제 1 참고예에서는, 목표 접지부위 위치를, 각 접지부위의 자세를 바꾸지 않고(자세를 제어하지 않고), 각 노드 상반력 중심점 주위로 회전이동 시키도록 보정(수정) 동작을 행함으로써, 각 접지부위(10) 사이의 상대높이를 목표 접지부위 위치 사이의 상대높이로부터 수정했다. 단, 목표 접지부위 위치를 연직방향으로만 이동시키도록 하여, 각 접지부위(10)의 높이만을 수정하도록 해도 된다. 구체적으로는, 이하의 수순에 의해 목표 접지부위 위치를 수정한다.

우선, 이하의 식 26, 27에 의해, 목표 제 14 노드 상반력 중심점의 연직위치 수정량(Z14)과 목표 제 23 노드 상반력 중심점의 연직위치 수정량(Z23)을 구한다.

Z14=-PQ14*θ1423 …식 26

Z23=PQ23*θ1423 …식 27

단, 여기에서, θ1423에는 상기 식 19에서 구해지는 값을 대입한다.

이어서, 각 접지부위의 목표 상반력 중심점(각 잎 노드의 목표 노드 상반력 중심점)의 연직위치 수정량(Zn)(n=1,2,3,4)을 다음 식에 의해 구한다.

Z1=-Q14Q1*θ14+Z14 …식 28

Z4=Q14Q4*θ14+Z14 …식 29

Z2=-Q23Q2*θ23+Z23 …식 30

Z3=Q23Q3*θ23+Z23 …식 31

단, 여기에서, θ14에는 상기 식 23에서 구해지는 값을 대입하고, θ23에는 θ14와 동일하게 하여 구해지는 값을 대입한다.

이상과 같이 구한 Z1, Z2, Z3, Z4를 목표 접지부위 위치에 연직방향으로 더함으로써, 수정 목표 접지부위 위치를 얻는다.

또, 상기 제 1 참고예에서는, 접지부위(10)를 상기 도 6에 도시한 바와 같이 계층화했지만, 그 계층구조는 반드시 1개의 계층구조에 미리 정해서 둘 필요는 없다. 예를 들면 트로트, 갤롭 등, 로봇(1)의 이동형태(이동시의 다리의 운동형태) 등 에 따라, 계층구조를 변경해도 된다. 예를 들면, 도 23에 도시하는 바와 같이 접지부위(10)를 계층화해도 된다. 도 23은, 예를 들면 로봇(1)의 모든 다리(#1~#4)를 지지 다리로 하는 시기에서, 제 1 접지부위(10)와 제 2 접지부위(10)의 세트를 제 12 노드로 하고, 제 3 접지부위(10)와 제 4 접지부위(10)의 세트를 제 34 노드로 한 경우의 예를 상기 도 3(b)와 동일하게 도시한 것이다. 이 예에서는, 각 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,12,34,1234)은 도시한 바와 같이 설정된다. 이렇게 함으로써, 경우에 따라서는, 상기 컴플라이언스 동작과 후술의 바닥형상 추정을 보다 적확하게 행하는 것이 가능하게 된다.

[제 2 참고예]

이하에 상기 도 1 및 도 24~도 34를 참조하면서, 본 발명의 제 2 참고예를 설명한다. 도 1을 참조하여, 제 2 참고예의 로봇(1)이 제 1 참고예의 로봇과 다른 점을 중심으로 설명하면, 제 2 참고예의 로봇(1)은 제 1~제 4 다리(#1~#4)에 더하여, 이들 다리(#1~#4)와 동일한 구조의 제 5 다리(#5) 및 제 6 다리(#6)를 구비한다. 즉, 제 2 참고예의 로봇(1)은 6다리 로봇이다. 제 5 다리(#5)는 제 3 다리(#3)의 후방측에서 로봇(1)의 상체(24)의 우측부로부터 연장하여 설치되고, 제 6 다리(#6)는 제 4 다리(#4)의 후방측에서 로봇(1)의 상체(24)의 좌측부로부터 연장하여 설치되어 있다. 이것 이외의 로봇(1)의 기구적 구조는, 제 1 참고예와 동일하므로, 제 1 참고예와 동일한 참조부호를 붙이고 설명을 생략한다.

이 제 2 참고예의 로봇(1)(6다리 로봇)의 계층형 컴플라이언스 제어의 수법의 기본적인 개념은 제 1 참고예의 것과 동일하다. 단, 제 2 참고예의 계층형 컴플라이언스 제어에서는, 3개의 자 노드를 갖는 노드가 있는 것과, 그것에 대응하여 제어처리가 확장되어 있는 것이 제 1 참고예의 것과 상이하다.

그래서, 이후, 그 차이점을 중심으로 제 2 참고예의 로봇(1)의 제어처리에 대해 설명한다. 또한, 제 2 참고예의 설명에서 사용하는 참조부호 및 용어는, 제 1 참고예와 동등한 의미를 갖는 것은, 제 1 참고예와 동일한 참조부호 및 용어를 사용하고, 상세한 설명을 생략한다.

도 24는, 제 2 참고예에서의 계층구조를 설명하기 위한 도면으로, 제 1 참고예에서의 도 3(b)에 대응하는 도면이다. 도 24에 도시하는 바와 같이, 제 2 참고예에서는, 6개의 접지부위(10)의 각각을 잎 노드(제 1~제 6 노드)로 하고, 그 6개의 모든 접지부위(10)의 세트를 뿌리 노드(제 145236 노드)로 하고 있는 동시에, 제 1 다리(#1), 제 4 다리(#4), 제 5 다리(#5)의 각각의 접지부위인 제 1, 제 4, 제 5 접지부위(10)의 세트를 제 145 노드, 제 2 다리(#2), 제 3 다리(#3), 제 6 다리(#6)의 각각의 접지부위인 제 2, 제 3, 제 6 접지부위(10)의 세트를 제 236 노드로 하고 있다. 즉, 제 145 노드는 제 1, 제 4, 제 5 노드(3개의 잎 노드)를 자 노드로서 갖는 중간 노드, 제 236 노드는 제 2, 제 3, 제 6 노드(3개의 잎 노드)를 자 노드로서 갖는 중간 노드이다.

제 2 참고예에서는, 예를 들면 제 145 노드의 3개의 접지부위(10)의 세트와, 제 236 노드의 3개의 접지부위(10)의 세트를 번갈아 들어올려서 착지시킴으로써, 로봇(1)의 이동을 행하도록 하고 있다. 도 24는, 양자의 세트를 착지시킨 상태(양자의 세트를 지지 다리로 한 상태)를 도시하고 있고, 도면 중의 참조부호 Qn(n=1,2,3,4,5,6)을 붙인 삼각형이 각 접지부위(10)의 위치에 대응하고 있다.

또한, Qn(n=1,2,3,4,5,6)은 잎 노드인 제 1~제 6 각 접지부위(10)의 목표 상반력 중심점(목표 노드 상반력 중심점), Q145, Q236은, 각각 제 145 노드, 제 236 노드의 목표 상반력 중심점(목표 노드 상반력 중심점), P는 뿌리 노드(제 145236 노드)의 목표 상반력 중심점(목표 노드 상반력 중심점)(Q145236)으로서의 목표 전체 상반력 중심점(=목표 ZMP)을 나타내는 것이다. 제 1 참고예와 동일하게, Qn(n=1,2,3,4,5,6)은 각각 대응하는 접지부위(10)의 중심점에 일치하고 있다.

제 2 참고예에서의 제어장치(50)의 전체적인 기능적 구성은 상기 제 1 참고예에 관하여 설명한 도 2의 것과 동일하다.

이 경우, 제 2 참고예에서의 보용생성장치(100)는, 제 1 참고예의 것과 동일하게, 로봇(1)의 목표 운동 궤도(목표 접지부위 궤도, 목표 상체위치 자세 궤도)와 목표 상반력 궤도(목표 전체 상반력 중심점 궤도, 목표 전체 상반력 궤도)를 결정하여 출력한다. 단, 제 2 참고예에서는, 목표 접지부위 궤도는 6개의 접지부위(10)의 각각의 목표 접지부위 위치의 궤도이다. 또한, 각 접지부위를 그 자세를 제어 가능하게 설치한 경우에는, 목표 접지부위 자세의 궤도도 목표 접지부위 궤도에 포함된다. 또, 목표 전체 상반력 중심점 궤도는, 제 2 참고예에서의 다리(#1~#6)의 운동형태(특히 지지 다리의 착지 예정위치)에 맞추어서, ZMP의 존재가능 범위 내에서 이 범위의 경계에 지나치게 근접하지 않는 위치(예를 들면 ZMP의 존재가능 범위의 거의 중앙위치)에 존재하면서, 연속적으로 이동하도록 결정된다.

또, 제 2 참고예에서의 목표 상반력 분배기(102)에서는, 제 1 참고예와 동일하게, 다음 조건 A')~G')을 만족시키도록, 목표 노드 상반력 중심점과 각 노드의 가중값과 목표 노드 상반력을 결정한다.

A') 각 잎 노드의 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,5,6)은 이 잎 노드에 대응하는 접지부위(10)의 중심점에 일치한다. 보다 일반적으로는, Qn(n=1,2,3,4,5,6)은 목표 보용(목표 접지부위 궤도 등의 목표 운동)에 따라 결정된다. 또한, 예를 들면 각 접지부위(10)의 목표 상반력 중심점을 보용생성장 치(100)에서 결정한 경우에는, 보용생성장치(100)가 결정하는 목표 상반력을 기초로, 목표 노드 상반력 중심점(Qn)(n=1,2,3,4,5,6)을 결정하면 된다.

B') 뿌리 노드의 목표 바닥 노드 반력 중심점은 목표 전체 상반력 중심점(P)에 일치한다.

C') 자 노드를 갖는 임의의 제 n 노드(n=145,236,145236)의 목표 노드 상반력 중심점(Qn)은 그 제 n 노드의 각 노드의 목표 노드 상반력 중심점의 가중평균의 점이 된다. 보다 구체적으로는, 제 145 노드의 목표 노드 상반력 중심점(Q145)은, 그 자 노드(잎 노드)인 제 1, 4, 5 노드의 목표 노드 상반력 중심점(Q1, Q4, Q5)을 정점으로 하는 삼각형의 내분점(삼각형의 경계의 점을 포함함)이 되고, 제 236 노드의 목표 노드 상반력 중심점(Q236)은 그 자 노드(잎 노드)인 제 2, 3, 6 노드의 목표 노드 상반력 중심점(Q2, Q3, Q6)을 정점으로 하는 삼각형의 내분점(삼각형의 경계의 점을 포함함)이 된다. 또, 제 145236 노드(뿌리 노드)의 목표 노드 상반력 중심점(Q145236)(=목표 전체 상반력 중심점(P))은 그 자 노드(중간 노드)인 제 145, 236 노드의 목표 노드 상반력 중심점(Q145, Q236)을 연결하는 선분의 내분점이 된다.

D') 자 노드를 갖는 임의의 제 n 노드(n=145,236,145236)의 목표 노드 상반력(Fn)은 이 제 n 노드의 모든 자 노드의 목표 노드 상반력의 합(합력)에 일치하고, 또한, 뿌리 노드(제 145236 노드)의 목표 노드 상반력(F145236)은 목표 전체 상반력(Ftotalref)에 일치한다. 또는, 각 노드의 목표 노드 상반력(Fn)(n=1,2,3,4,5,6,145,236,145236)은 각 노드의 가중값과 상기 식 4a 또는 4b 의 관계를 갖는다.

E') 접지하고 있지 않은 접지부위(10)에 대응하는 잎 노드의 목표 노드 상반력은 0이 된다.

F') 각 노드(제 n 노드(n=1,2,3,4,145,236,145236))의 목표 노드 상반력 중심점, 가중값, 목표 노드 상반력은 연속적으로 변화된다.

G') 비접지상태의 접지부위(10)에 대응하는 잎 노드의 가중값, 또는, 그 잎 노드의 선조 노드의 어느 1개의 가중값은 0으로 된다.

또한, 뿌리 노드의 가중값은 상기 제 1 참고예와 동일하게 편의적으로 「1」로 설정했다.

제 2 참고예에서의 자세편차 연산부(103) 및 자세안정화 제어 연산부(104)의 처리는, 제 1 참고예와 동일하고, 보상 전체 상반력 모멘트(Mdmd)(Mdmdx, Mdmdy)가 제 1 참고예에서 설명한 바와 같이 결정된다.

또, 제 2 참고예에서의 실 상반력 검출기(108)는, 제 1 참고예의 것과 동일하게, 각 접지부위(10)(본 참고예에서는 6개의 접지부위)에 작용하는 실 상반력을 각 다리(#1~#6)에 구비한 6축력센서(34)의 출력으로부터 검출하고, 그것을 지지 다리 좌표계(바닥에 고정된 도 1에 도시하는 글로벌 좌표계)로 표현하여 이루어지는 실 상반력으로 변환한다.

또, 제 2 참고예에서의 로봇 기하학 모델(역키네마틱스 연산부)(110)은 제 1 참고예의 것과 동일하게, 각 접지부위 위치의 최종적인 목표 궤도와 목표 상체위치 자세 등을 기초로, 역키네마틱스의 연산에 의해, 그것들을 만족하는 로봇(1)의 각 관절 변위 지령을 산출한다.

또, 제 2 참고예에서의 변위 콘트롤러(112)는, 제 1 참고예의 것과 동일하게, 상기 각 관절 변위 지령에 실 관절 변위를 추종시키도록 로봇(1)의 각 관절(14, 15)의 액추에이터(도시하지 않음)를 제어한다.

또, 제 2 참고예에서의 계층형 컴플라이언스 동작 결정부(114)는, 제 1 참고예의 것과 동일하게 상기 요구 1), 2)를 가능한 한 만족하도록 각 접지부위(10)의 목표 접지부위 궤도를 수정한다. 단, 본 참고예에서는, 3개의 자 노드를 갖는 중간 노드를 갖기 때문에, 계층형 컴플라이언스 동작 결정부(114)의 구체적인 처리는 제 1 참고예의 것보다 다소 복잡하게 된다. 또한, 각 접지부위(10)의 자세를 제어 가능하게 로봇을 구성한 경우에는, 계층형 컴플라이언스 동작 결정부(114)는, 상기 요구 3)도 가능한 한 만족하도록 각 접지부위(10)의 목표 접지부위 궤도를 수정해야 한다.

도 25는, 제 2 참고예의 계층형 컴플라이언스 동작 결정부(114)의 기능을 나타내는 블럭도이며, 제 1 참고예에서의 도 17에 대응하고 있다. 동 도 25를 참조하여, 제 2 참고예의 계층형 컴플라이언스 동작 결정부(114)도, 제 1 참고예와 동일하게, 보상 전체 상반력 모멘트 분배기(114a), 보상각 결정부(114b, 114c, 114d)와, 수정 목표 접지부위 위치 자세 산출부(114g)와, 기구변형 보상량 산출부(114n)와, 기구변형 보상된 수정 목표 접지부위 위치 자세 결정부(114h)를 구성요소(기능적 수단)로서 구비한다. 또한, 제 2 참고예에서는, 보상각은 제 145236 노드, 제 145 노드, 제 236 노드에 각각 관계되는 제 145236 보상각(θ145236), 제 145 보상 각(θ145) 및 제 236 보상각(θ236)이 있고, 보상각 결정부(114b, 114c, 114d)는 각각 θ145236, θ145, θ236을 결정하는 것이다.

이하에, 제 2 참고예에서의 계층형 컴플라이언스 동작 결정부(114)의 처리를 제 1 참고예의 것과 상이한 점을 중심으로 설명한다.

제 2 참고예에서의 계층형 컴플라이언스 동작 결정부(114)는 제 1 참고예의 것과 동일하게, 목표 상반력 분배기(102)의 출력 등을 기초로, 각 노드의 목표 노드 상반력의 병진력 성분 및 모멘트 성분을 결정함과 동시에, 실 상반력 검출기(108)의 출력을 기초로, 각 노드의 실 노드 상반력의 병진력 성분 및 모멘트 성분을 결정한다.

즉, 도 26을 참조하여, 각 잎 노드의 목표 노드 상반력의 병진력 성분(Fn_ref)(n=1,2,3,4,5,6)은 목표 상반력 분배기(102)에서 결정된 목표 노드 상반력의 병진력 성분과 동일하게 되고, 뿌리 노드의 목표 노드 상반력의 병진력 성분(F145236ref)은 보용생성장치(100)에서 결정된 목표 전체 상반력의 병진력 성분(Ftotalref)과 동일하게 된다. 또, 잎 노드 및 뿌리 노드 이외의 중간 노드의 목표 노드 상반력의 병진력 성분(Fn_ref)(n=145,236)은 그 중간 노드의 각 노드의 목표 노드 상반력의 합력의 병진력 성분으로 결정된다(F145ref=F1ref+F4ref+F5ref, F236ref=F2ref+F3ref+F6ref). 이 경우, 중간 노드의 부모 노드는 뿌리 노드이므로, F145ref+F236ref=Ftotalref이다. 또한, 도 26은, 제 2 참고예의 로봇(1)의 모든 접지부위(10)가 접지하고 있는 상태에서의 각 노드의 목표 노드 상반력의 병진력 성분(Fn_ref)을 예시하고 있다.

또, 각 노드의 목표 노드 상반력의 모멘트 성분(상세하게는 이 노드의 목표 노드 상반력 중심점을 작용점으로 하는 목표 노드 상반력의 모멘트 성분)은 모두 0으로 된다.

또, 도 30을 참조하여, 각 잎 노드의 실 노드 상반력의 병진력 성분(Fn_act)(n=1,2,3,4,5,6)은 실 상반력 검출기(108)에서 검출된 각 접지부위(10)의 실 상반력의 병진력 성분과 동일한 것으로 된다. 그리고, 자 노드를 갖는 각 노드의 실 노드 상반력의 병진력 성분(Fn_act)(n=145,236,145236)은 그 노드의 각 노드의 실 노드 상반력의 합력의 병진력 성분으로 결정된다. 따라서, F145act=F1act+F4act+F5act, F236act=F2act+F3act+F6act, F145236act(=Ftotalact)=F145act+F236act 이다. 또한, 도 30은, 제 2 참고예의 로봇(1)의 모든 접지부위(10)가 접지하고 있는 상태에서의 각 노드의 실 노드 상반력의 병진력 성분(Fn_act)을 예시하고 있다.

또, 도 31을 참조하여, 각 잎 노드의 실 노드 상반력의 모멘트 성분(이 잎 노드의 목표 노드 상반력 중심점을 작용점으로 하는 모멘트 성분)(Mn_act)(n=1,2,3,4,5,6)은, 기본적으로는, 실 상반력 검출기(108)에서 검출된 각 접지부위(10)의 실 상반력의 모멘트 성분과 동일하게 된다. 단, 본 참고예에서는, 각 접지부위(10)의 중심점, 즉 이 접지부위(10)의 목표 상반력 중심점에 실제로 모멘트는 발생하지 않으므로, 각 잎 노드의 실 노드 상반력의 모멘트 성분은 0으로 설정된다. 그리고, 자 노드를 갖는 각 제 n 노드(n=145,236,145236)의 실 노드 상반력의 모멘트 성분(Mn_act)은, 기본적으로는 그 제 n 노드의 각 노드의 실 노드 상반력의 합력이 이 제 n 노드의 목표 노드 상반력 중심점에 작용하는 모멘트 성분(이것은 일반적으로 0이 되지 않음)으로 결정된다.

제 2 참고예에서의 상기 보상 전체 상반력 모멘트 분배기(114a)는, 상기 보상 전체 상반력 모멘트(Mdmd)(Mdmdx, Mdmdy)를, 제 145236 노드 보상 상반력 모멘트(M145236dmd), 제 145 노드 보상 상반력 모멘트(M145dmd) 및 제 236 노드 보상 상반력 모멘트(M236dmd)에 분배하는 것이다.

제 145236 노드 보상 상반력 모멘트(M145236dmd)는, 제 145236 보상각(θ145236)을 조작함으로써(제 1, 제 4, 제 5 접지부위의 세트와, 제 2, 제 3, 제 6 접지부위의 세트를 목표 전체 상반력 중심점(P)(=Q145236) 주위로 θ145236만큼 회전시킴으로써) 발생하는 각 접지부위(10)의 상반력의 병진력 성분이 목표 전체 상반력 중심점(P)(목표 ZMP) 주위에 발생해야 할 모멘트의 목표값이다.

제 145 노드 보상 상반력 모멘트(M145dmd)는, 제 145 보상각(θ145)을 조작함으로써(제 145 노드에 속하는 제 1, 제 4 및 제 5 접지부위(10)를 목표 제 145 노드 상반력 중심점(Q145) 주위로 θ145만큼 회전시킴으로써) 발생하는 각 접지부위(10)(상세하게는 제 1, 제 4, 및 제 5 접지부위)의 상반력의 병진력 성분이 목표 제 145 노드 상반력 중심점 주위에 발생해야 할 모멘트의 목표값이다.

제 236 노드 보상 상반력 모멘트(M236dmd)는, 제 236 보상각(θ236)을 조작함으로써(제 236 노드에 속하는 제 2, 제 3 및 제 6 접지부위(10)를 목표 제 236 노드 상반력 중심점(Q236) 주위로 θ236만큼 회전시킴으로써) 발생하는 각 접지부위(10)(상세하게는 제 2, 제 3, 및 제 6 접지부위)의 상반력의 병진력 성분이 목표 제 236 노드 상반력 중심점 주위에 발생해야 할 모멘트의 목표값이다.

또한, 본 참고예에서는, 제 1 참고예와 동일하게, 로봇(1)의 연직축 주위의 자세제어를 행하지 않으므로(보상 전체 상반력 모멘트(Mdmd)의 연직축 주위의 성분이 0이므로), M145236dmd, M145dmd, M236dmd는 모두 연직축 주위의 성분이 0인 모멘트(수평 벡터)이며, 보상각(θ145236, θ145, θ236)은 수평한 축 주위의 회전각이다. 또, 특히, M145236dmd는 선분 Q145Q236에 수직한 수평축 주위의 모멘트이다.

이들 노드 보상 상반력 모멘트(M145236dmd, M145dmd, M236dmd)는, 기본적으로는, 다음 조건 11), 12)를 만족시키도록 결정된다.

11) 제 145 노드, 제 236 노드, 제 145236 노드에 대해, 상기 제 1 참고예에서 정의한 바와 같이, 다음 식 7a~9a의 관계식을 충족시키는 수정 목표 노드 상반력 중심점(Q145mdfd, Q236mdfd, Pmdfd)(=Q145236mdfd)을 도입했을 때, 이들 Q145mdfd, Q236mdfd, Pmdfd는 각각의 소정의 존재 허용범위 내에 있다.

M145236dmd=(Pmdfd-P)*Ftotalref …식 7a

M145dmd=(Q145mdfd-Q145)*F145ref …식 8a

M236dmd=(Q236mdfd-Q236)*F236ref …식 9a

Q145mdfd, Q236mdfd, Pmdfd의 존재가능 범위는, 예를 들면 제 2 참고예의 로봇(1)의 모든 접지부위(10)가 접지하고 있는 상태에서는, 도 29(a)에 도시하는 바와 같이 설정된다. 즉, Q145mdfd의 존재 허용범위는, 도면 중의 굵은 선의 삼각형상의 영역(그 삼각형의 변 및 내부 영역)이며, 이것은, 제 145 노드의 자 노드의 목표 노드 상반력 중심점(Q1, Q4, Q5)을 정점으로 하는 삼각형의 내부이고, 이 삼각형(Q1Q4Q5)의 경계에 지나치게 근접하지 않도록 설정된 영역이다. Q236mdfd의 존재 허용범위도 동일하다. 또, Pmdfd의 존재 허용범위는, 도면 중의 굵은 선의 선분상의 영역이며, 이것은 제 145236 노드(뿌리 노드)의 자 노드의 목표 상반력 중심점(Q145,Q236)을 연결하는 선분 Q145Q236상에서, 이 선분 Q145Q236의 끝점에 지나치게 근접하지 않도록 설정된 영역이다.

12) Mdmd=M145236dmd+M145dmd+M236dmd를 거의 만족한다.

또, 본 참고예에서는, 뿌리 노드(제 145236 노드)의 자 노드가 2개이므로, M145236dmd는, 상기 제 1 참고예에서의 각 노드 보상 상반력 모멘트와 동일하게, 선분 Q145Q236에 직교하는 수평한 단위 벡터(이것을 V145236으로 나타냄)와 동일 방향의 벡터로 제한된다. 그래서, 본 참고예에서는, 다음 조건 13)을 충족시키도록 M145236dmd, M145dmd, M236dmd를 결정한다.

13) M145236dmd+M145dmd+M236dmd의 벡터 V145236 방향의 성분은, 가능한 한, Mdmd의 벡터 V145236 방향의 성분에 가까운 값으로 한다.

이들 조건 11)~13)을 만족시키는 M145236dmd, M145dmd, M236dmd는, 예를 들면 다음과 같이 결정된다. 우선, M145236dmd를 Mdmd의 V145236 방향의 성분에 결정한다. 단, 상기 식 7a에 의해 정해지는 수정 목표 노드 상반력 중심점(Pmdfd)이 그 존재 허용범위에 들어가지 않는 경우에는, Pmdfd가 그 존재 허용범위의 경계의 점이 되도록, M145236dmd를 수정한다. 이어서, Mdmd로부터 상기한 바와 같이 결정한 M145236dmd를 뺀 벡터가, Mdmd145와 Mdmd236의 합에 거의 일치하고, 또한, Mdmd145+Mdmd23의 V145236 방향의 성분이 Mdmd-Mdmd145236의 V145236 성분에 가능한 한 가까운 것으로 되고, 또한 상기 조건 11)을 만족시키도록, Mdmd145, Mdmd236을 결정한다. 또한, 이 경우, Mdmd145, Mdmd236은 서로 평행한 벡터로 한다.

이렇게 결정된 M145236dmd, M145dmd, M236dmd의 예를 도 29(b)에 도시한다. 보충하면, M145236dmd는 상기한 바와 같이 선분 Q145Q236에 수직한 수평 벡터이다.

또한, 로봇(1)의 연직축 주위의 자세도 제어하는 경우에는, M145236dmd, M145dmd, M236dmd의 연직성분도 결정하도록 해도 된다.

제 2 참고예에서의 각 보상각 결정부(114b~114d)는, 기본적으로는, 각각 제 n 노드(n=145236,145,236)의 노드 보상 상반력 모멘트와 실 노드 상반력 모멘트(목표 노드 상반력 중심점을 작용점으로 하는 모멘트)의 편차에 따라, 그 편차를 0에 근접시키도록 노드 보상각(θ145236, θ145, θ236)을 결정한다. 도 30 및 도 31에 그 예를 도시한다. 이 경우, 본 참고예에서는, θ145는, 도 30에 도시하는 바와 같이, 편차 M145act-M145dmd와 동일한 방향에서, 목표 제 145 노드 상반력 중심점(Q145)을 통과하는 축 주위의 회전각, θ236은, 도 30에 도시하는 바와 같이, 편차 M236act-M236dmd와 동일한 방향에서, 목표 제 236 노드 상반력 중심점(Q236)을 통과하는 축 주위의 회전각이다. 또, θ145236은, 도 31에 도시하는 바와 같이, 편차 M145236act-M145236dmd와 동일한 방향(선분 Q145Q236에 수직한 수평방향)에서, 목표 전체 상반력 중심점(P)을 통과하는 축 주위의 회전각이다.

단, 본 참고예에서도, 제 1 참고예와 동일하게, 제 n 노드(n=145236,145,236)의, 노드 보상 상반력 모멘트를 필터에 통과시킨 것과, 실 노 드 상반력 모멘트를 필터에 통과시킨 것의 편차에 따라, 노드 보상각(θ145236, θ145, θ236)을 결정하도록 했다.

도 32는 상기한 바와 같이 노드 보상각(θ145236)을 결정하는 보상각 결정부(114b)(θ145236 결정부)의 기능을 나타내는 블럭도, 도 33은 상기한 바와 같이 노드 보상각(θ145)을 결정하는 보상각 결정부(114c)(θ145 결정부)의 기능을 나타내는 블럭도이다. 또한, 보상각 결정부(114d)(θ236 결정부)의 처리는 θ145 결정부(114b)의 처리와 동일하므로 상세한 설명 및 도시를 생략한다.

보상각 결정부(θ145236 결정부)(114b)에서는, 우선, 목표 제 145 노드 상반력 중심점(Q145)에 실 제 1, 제 4, 제 5 노드 상반력의 합력의 병진력 성분(F1act+F4act+F5act)이 작용하는 동시에, 목표 제 236 노드 상반력 중심점(Q236)에 실 제 2, 제 3, 제 6 노드 상반력의 합력의 병진력 성분(F2act+F3act+F6act)가 작용한 경우에, 목표 전체 상반력 중심점(P)(목표 제 145236 노드 상반력 중심점)의 주위에 발생하는 모멘트(M145236act)를 상기 식 16 또는 17에 기초하여 구한다. 그리고, 그 구한 M145236act의 벡터 V145236 방향의 성분(M145236actv)을 내적 연산에 의해 구한다. 여기에서 벡터 V145236은 선분 Q145Q236에 수직하고 또한 수평한 단위 벡터이다. 또한, 제 145236 노드에 관해서는, V145236에 수직하고 또한 수평한 단위 벡터(U145236)의 방향의 축 주위에 자 노드(제 145 노드 및 제 236 노드)의 목표 상반력 중심점의 위치를 조작할 수 없으므로, M145236act의 벡터(U145236) 방향의 성분은 구할 필요는 없다.

이어서, 상기 제 1 참고예와 동일하게, 이 M145236actv를 로 패스 필터에 통 과시킨 것(M145236actvfilt)과, 상기 제 145236 노드 보상 상반력 모멘트(M145236dmd)의 벡터 V방향의 성분(M145236dmdv)을 보상 필터에 통과시킨 것(M145236dmdvfilt)과의 편차(M145236errv)(=M145236actvfilt-M145236dmdvfilt)에 소정의 게인(K145236)을 승산함으로써, 제 145236 노드 보상각(θ145236)이 결정된다.

보상각 결정부(θ145 결정부)(114c)에서는, 우선, 목표 제 1, 제 4, 제 5 노드 상반력 중심점(Q1, Q4, Q5)에 각각 실 제 1, 제 4, 제 5 노드 상반력의 병진력 성분(F1act, F4act, F5act)가 작용한 경우에, 제 145 노드 상반력 중심점(Q145)의 주위에 발생하는 모멘트(M145act)를 산출한다. 이 경우, 산출하는 M145act는, 서로 직교하는 수평한 단위 벡터인 벡터 V145 및 U145의 각각의 방향의 성분으로 구성된다. 벡터 V145 또는 U145의 방향은 임의이어도 된다.

이어서, 이 M145act를 로 패스 필터에 통과시킨 것(M145actfilt)과, 상기 제 145 노드 보상 상반력 모멘트(M145dmd)(벡터 V방향의 성분 및 벡터 U방향의 성분)을 보상 필터에 통과시킨 것(M145dmdfilt)의 편차(M145err)(=M145actfilt-M145dmdfilt)에 소정의 게인 행렬(K145)(대각 행렬)을 승산함으로써, 제 145 노드 보상각(θ145)이 결정된다. θ145는 벡터 V의 축 주위 각도성분과 벡터 U의 축 주위의 각도성분으로 구성된다.

보상각 결정부(θ236 결정부)(114d)에 의한 제 236 노드 보상각(θ236)의 결정처리도 상기한 θ145 결정부(114c)의 처리와 동일하게 행해진다.

이상에서 설명한 보상각 결정부(114b~114d)의 처리에 의해, 목표 전체 상반 력 중심점(P)에 작용하는 실 모멘트(Mact)의 수평성분을 보상 전체 상반력 모멘트(Mdmd)에 근접시키도록 노드 보상각(θ145236, θ145, θ236)의 세트가 결정되게 된다.

또한, 제 1 참고예에 관해 보충 설명한 바와 같이, 다음과 같이 노드 보상각(θ145236, θ145, θ236)을 결정하도록 해도 된다. 즉, 자 노드를 갖는 각 제 n 노드(n=145,236,145236)에 대하여, 그 제 n 노드의 실 노드 상반력(제 n 노드의 모든 자 노드의 실 노드 상반력의 합력)의 모멘트의 수평성분이 0이 되는 상반력 중심점을 실 제 n 노드 상반력 중심점으로서 구한다. 또는, 제 n 노드(n=145,236,145236)의 실 노드 상반력의 모멘트로부터, 그 각 자 노드의 실 노드 상반력이 이 자 노드의 목표 노드 상반력 중심점에 작용하는 모멘트를 감산하여 이루어지는 모멘트의 수평성분이 0이 되는 상반력 중심점을 실 제 n 노드 상반력 중심점으로 하여 구한다. 그리고, 제 n 노드(n=145,236,145236)의 목표 노드 상반력 중심점과, 상기와 같이 구한 실 제 n 노드 상반력 중심점과의 편차, 또는, 그들 노드 상반력 중심점의 각각을 필터에 통과시킨 값의 편차를 제어량으로 하고, 그 제어량을 0에 근접시키도록, 이 제어량에 따라 노드 보상각(θ145236, θ145, θ236)을 결정한다(예를 들면 제어량에 어떤 게인을 승산함으로써, θ145236, θ145, θ236을 결정함).

도 25에 도시한 제 2 참고예에서의 수정 목표 접지부위 위치 자세 산출부(114g)는, 다음과 같이, 각 접지부위(10)의 목표 접지부위 위치 자세(도 1의 로봇에서는 실제로는 목표 접지부위 위치)를 수정하고, 수정 목표 접지부위 위치 자 세를 얻는다. 즉, 도 30 및 도 31을 참조하여, 제 145 노드의 자 노드인 제 1, 제 4 및 제 5 노드의 목표 상반력 중심점(Q1, Q4, Q5)을 각각, 제 145 노드의 목표 상반력 중심점(Q145)을 회전중심으로 하여, 제 145 노드 보상각(θ145)(수평 벡터)만큼 회전이동 한다. 이 회전이동 후의 Q1, Q4, Q5를 각각 Q1', Q4', Q5'이라고 한다. 따라서, 제 145 노드 보상각(θ145)은, 제 145 노드의 목표 상반력 중심점(Q145)의 위치를 움직이지 않고, 제 145 노드의 자 노드인 제 1, 제 4, 제 5 노드의 각각의 목표 상반력 중심점(Q1, Q4, Q5)의 위치의 상대관계를 움직이기 위한 조작량이다.

마찬가지로, 제 236 노드의 자 노드인 제 2, 제 3 및 제 6 노드의 목표 상반력 중심점(Q2, Q3, Q6)을 각각, 제 236 노드의 목표 상반력 중심점(Q236)을 회전중심으로 하여, 제 236 노드 보상각(θ236)(수평 벡터)만큼 회전이동 한다. 이 회전이동 후의 Q2, Q3, Q6를 각각 Q2', Q3', Q6'으로 한다. 따라서, 제 236 노드 보상각(θ236)은, 제 236 노드의 목표 상반력 중심점(Q236)의 위치를 움직이지 않고, 제 236 노드의 자 노드인 제 2, 제 3, 제 6 노드의 각각의 목표 상반력 중심점(Q2, Q3, Q6)의 위치의 상대관계를 움직이기 위한 조작량이다.

이들 회전이동을 시각적으로 도시한 것이 도 30이다.

이어서, 제 145236 노드의 자 노드인 제 145 및 제 236 노드의 목표 상반력 중심점(Q145, Q236)을 각각, 제 145236 노드의 목표 상반력 중심점(P)(목표 전체 상반력 중심점)을 회전중심으로 하여, 상기 편차(M145236errv)의 벡터(수평 벡터)와 동일한 방향(선분 Q145Q236과 직교하는 수평방향)의 축심 주위로 제 145236 노 드 보상각(θ145236)만큼 회전이동 한다. 이 회전이동 후의 Q145, Q236을 도 31에 도시하는 바와 같이 각각 Q145', Q236'으로 한다. 따라서, 제 145236 노드 보상각(θ145236)은, 제 145236 노드(뿌리 노드)의 목표 상반력 중심점(P)의 위치를 움직이지 않고, 제 145236 노드의 자 노드인 제 145, 제 236 노드의 각각의 목표 상반력 중심점(Q145, Q236)의 위치의 상대관계를 움직이기 위한 조작량이다.

이어서, 도 31을 참조하여, 앞의 회전이동 후의 목표 노드 상반력 중심점(Q1', Q4', Q5')을 벡터(Q145_Q145')만큼 평행이동하고, 이것에 의해, 제 1, 제 4 및 제 5 노드의 최종적인 수정 후의 목표 노드 상반력 중심점(Q1", Q4", Q5")을 얻는다. 마찬가지로, 앞의 회전이동 후의 목표 노드 상반력 중심점(Q2', Q3', Q6')을 벡터(Q236_Q236')만큼 평행이동 하고, 이것에 의해, 제 2, 제 3 및 제 6 노드의 최종적인 수정 후의 목표 노드 상반력 중심점(Q2", Q3", Q6")을 얻는다.

최후에, 제 n 접지부위(n=1,2,3,4,5,6)의 목표 접지부위 위치를 벡터(Qn_Qn")만큼 평행이동(거의 상하이동) 시킨다. 이것에 의해, 각 접지부위(10)의 목표 접지부위 위치(보다 정확하게는 각 접지부위(10)의 목표 접지부위 위치의 상대관계)가 수정되게 된다. 즉, 자 노드를 갖는 각 노드 마다, 그 노드의 자손 노드로 되어 있는 각 접지부위(10)의 목표 접지부위 위치 사이의 상대관계(상대적 위치관계)의 조작량(수정량)이 각 보상각(θ145, θ236, θ145236)에 따라 정해지고, 그것들의 조작량(수정량)을 합성함으로써 각 접지부위(10)의 목표 접지부위 위치의 상호의 상대관계가 수정되게 된다.

또한, 접지부위의 자세를 제어 가능하게 하고, 접지부위의 목표 상반력 중심 점 주위에 상반력 모멘트를 발생시킬 수 있는 로봇에서는, 각 접지부위의 목표 접지부위 상반력 중심점(목표 노드 상반력 중심점) 주위의 족평자세 회전동작을, 일본 특개평10-277969호 공보(복합 컴플라이언스 제어)에 개시한 수법으로 행하면 된다. 즉, 제 1 참고예에 관해 보충 설명한 바와 같이, 제 n 접지부위를 상기의 평행이동 후의 Qn"을 중심으로 하여, 이 제 n 접지부위의 목표 자세를 수정하도록 하면 된다.

도 25에 도시한 제 2 참고예에서의 기구변형 보상량 산출부(114n)는, 상기 제 1 참고예의 것과 동일하게, 각각 다리(#1~#6)의 컴플라이언스 기구(42) 등의 변형에 의한 영향을 보상하기 위한 기구변형 보상량(En_cmpn)(n=1,2,3,4,5,6)을 구한다. 즉, 제 2 참고예에서의 보상 전체 상반력 모멘트 분배기(114a)로부터 출력되는 각 접지부위(10)의 수정 목표 접지부위 상반력(Fn_refmdfd)(n=1,2,3,4,5,6,)(목표 접지부위 상반력에, 각 노드 보상 상반력 모멘트(M145236dmd, M145dmd, M236dmd)를 부가함으로써 수정된 목표 접지부위 상반력)에 의해 발생이 예상되는 각 다리(#1~#6)의 컴플라이언스 기구(42) 등의 기구변형량(En_mdfd)(n=1,2,3,4,5,6)을 기구 컴플라이언스 모델을 사용하여 구한다. 이 En_mdfd에 (-1)을 승산한 것을 기구변형 보상량(En_cmpn)으로서 구한다.

도 25에 도시한 제 2 참고예에서의 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)는, 제 1 참고예의 것과 동일하게, 기구변형 보상된 수정 목표 접지부위 위치 자세는, 각 접지부위(10)의 수정 목표 접지부위 위치 자세(상기 수정 목표 접지부위 위치 자세 산출부(114g)에서 구해진 위치 자세)에, 각각에 대 응하는 기구변형 보상량(En_cmpn)을 부가함으로써 결정된다.

이상이, 본 참고예(제 2 참고예)에서의 계층형 컴플라이언스 동작결정부(114)의 처리의 상세이다.

이상에서 설명한 이외의 제어장치(50)의 연산처리는 제 1 참고예와 동일하다.

또한, 제 2 참고예에서의 계층구조는, 제 1 참고예의 경우와 동일하게, 로봇(1)의 동작 형태(이동형태) 등에 따라 변경해도 된다. 예를 들면, 도 34에 도시하는 바와 같이, 계층구조를 설정해도 된다. 이 도시의 예에서는, 제 1 접지부위 및 제 2 접지부위를 자 노드(잎 노드)로서 갖는 제 12 노드와, 제 3 접지부위 및 제 4 접지부위를 자 노드(잎 노드)로서 갖는 제 34 노드와, 제 5 접지부위 및 제 6 접지부위를 자 노드(잎 노드)로서 갖는 제 56 노드와, 제 56 노드 및 제 34 노드를 자 노드로서 갖는 제 3456 노드를 중간 노드로서 구비하고, 뿌리 노드는 제 3456 노드와 제 12 노드를 자 노드로서 갖는 것으로 되어 있다. 이렇게 함으로써 컴플라이언스 동작 및 후술의 바닥형상 추정을 보다 적확하게 행하는 것이 가능하게 되는 경우가 있다. 또한, 도면 중의 부호의 의미는, 상기 도 3(b)나 도 24 등에 도시한 것과 동일하다.

[제 3 참고예]

다음에, 제 2 참고예의 로봇(1)(6다리 로봇)에 바닥형상 추정기능과 그 추정 결과에 따른 로봇(1)의 동작 보정 기능을 부가한 제 3 참고예에 대해 설명한다. 또한, 본 참고예에서는, 주로, 제 2 참고예에서 나타낸 6다리 로봇을 중심으로 해 서 설명하지만, 4다리 로봇에 대해서도 보충적으로 설명을 부가하는 경우가 있다. 또, 본 참고예의 이해의 편의상, 2다리 로봇에 대해서도 언급하는 경우가 있다.

본 참고예에서의 로봇(1)의 기구적 구성은, 제 1 또는 제 2 참고예에서 설명한 도 1에 도시한 것(단, 6개의 다리(#1~#6)를 갖는 로봇(1))과 동일하다. 따라서, 로봇(1)의 기구적 구성의 설명은 생략한다. 또, 본 참고예에 있어서의 로봇(1)에 구비한 제어장치(50)의 기능적 구성도, 상기 도 2에 도시한 것과 동일하다. 단, 본 참고예에서는, 도 2중의 계층형 컴플라이언스 동작 결정부(114)에는, 새로운 기능이 부가되어 있고, 그 점이 제 2 참고예의 것과 상이하다. 그리고, 계층형 컴플라이언스 동작 결정부(114) 이외의 제어장치(50)의 구성요소의 처리는 제 2 참고예와 동일하다. 그래서, 본 참고예에서의 설명에서는, 계층형 컴플라이언스 동작 결정부(114)의 처리를 중심으로 설명하고, 이외의 제어장치(50)의 처리에 대해서는, 상세한 설명을 생략한다.

도 35는 본 참고예에서의 계층형 컴플라이언스 동작 결정부(114)의 처리기능을 도시하는 블럭도이다. 이 중, 제 2 참고예의 것과 상이한 기능을 설명하면, 본 참고예에서의 계층형 컴플라이언스 동작 결정부(114)에는, 새롭게, 바닥형상 추정 수단으로서의 바닥형상 추정기(130)와, 바닥형상 추정기(130)가 출력(추정)하는 추정 바닥형상 편차(보다 상세하게는 각 접지부위(10)에 관계되는 후술의 바닥높이 편차의 추정값)을 수정 목표 접지부위 위치 자세에 가산하는 가산기(132)가 부가받고, 가산기(132)의 출력을, 수정 목표 접지부위 위치 자세 대신 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)에 입력하도록 하고 있다. 이외의 계 층형 컴플라이언스 동작 결정부(114)의 구성요소의 처리는 제 2 참고예의 것과 동일하다.

또, 이것에 따라, 본 참고예에서의 제어장치(50)의 메인루틴의 제어처리는 그 일부가 상기 도 9의 플로우차트에 나타낸 제어처리와 상이하다. 도 36은 본 참고예에서의 제어장치(50)의 메인루틴의 제어처리를 도시하는 플로우차트이다. 도시한 바와 같이, 본 참고예에서는, S36의 후에, 새롭게 S37에서 바닥형상 편차를 추정하는 처리(바닥형상 추정기(130)의 처리)가 추가된다. 또한, S38'에서는, 상기 제 2 참고예에서 설명한 각 보상각(θ145236, θ145, θ236)과 S37에서 추정된 바닥형상 편차에 따라 각 목표 접지부위 위치 자세가 수정되고, 그 수정 후의 목표 접지부위 위치 자세를 기구변형 보상량에 따라 더 수정함으로써, 최종적인 목표 접지부위 위치 자세인 기구변형 보상된 수정 목표 접지부위 위치 자세가 얻어진다. 이 경우, 보다 상세하게는, 각 보상각(θ145236, θ145, θ236)에 따라 제 2 참고예와 동일하게 수정 목표 접지부위 위치 자세를 구한 후, 그 수정 목표 접지부위 위치 자세가 바닥형상 편차에 따라 수정되고, 이것이 기구변형 보상량에 따라 더 수정됨으로써 기구변형 보상된 수정 목표 접지부위 위치 자세가 얻어진다. 상기한 사항 이외는 도 9의 처리와 동일하다.

이후, 본 참고예의 제 2 참고예와 상이한 점을 구체적으로 설명한다.

우선, 본 참고예의 상세한 설명에 들어가기 전에, 바닥형상 추정기(130)가 추정하는 점에서 사용하는 개념과 용어를 이하와 같이 정의한다. 또한, 여기에서의 설명은, 편의상, 본 참고예의 6다리 로봇(1)에 한하지 않고, 일반적인 로봇의 간략적인 도면을 사용하여 행한다.

도 37, 도 38 및 도 39에 도시하는 바와 같이, 목표 보용에서 상정된 바닥(또는 바닥면)을 「상정 바닥」이라고 부른다. 로봇이 이동하는 실제의 바닥을 「실 바닥」이라고 부른다. 또한, 설명의 편의상, 도 37에서는 제 1 참고예에서 설명한 4다리 로봇을 도시하고, 도 38 및 도 39에서는, 2다리 로봇을 도시하고 있지만, 이하에 설명하는 용어의 의미는, 본 참고예에서의 6다리 로봇(1)을 포함하는 임의의 복수 다리 로봇에서도 동일하다.

상기 제 1 및 제 2 참고예에서 설명한 계층형 컴플라이언스 제어에서 정의된 목표 제 n 접지부위 상반력 중심점(Qn)은, 제 n 접지부위의 중심점에 설정된 점이었는데, 그 상반력 중심점(Qn)은 이 제 n 접지부위의 접지면(밑바닥면)에 설정되어 있어도 된다. 이 경우, 목표 보용에서, 목표 제 n 접지부위 상반력 중심점(Qn)과 접하는 것이 상정된 상정 바닥면상의 점을 「상정 제 n 바닥 접점(Dn)」이라고 부른다.

이 정의로부터 명확한 바와 같이, 로봇의 목표 보용에서 제 n 접지부위가 접지하고 있는 시기에서는, 목표 제 n 접지부위 상반력 중심점(Qn)과 상정 제 n 바닥 접점(Dn)은, 지지 다리 좌표계(글로벌 좌표계)로 보아 동일 좌표가 된다. 이에 반해, 실제로 로봇(1)이 이동하고 있을 때에, 실 제 n 접지부위의 밑바닥면상에서의 목표 제 n 접지부위 상반력 중심점(Qn)에 대응하는 점이, 실 바닥에 접촉하는 점을 「실 제 n 바닥 접점(Dnact)」이라고 부른다.

이들 점의 관계를 의미하는 예를 도 37, 도 38 및 도 39에 도시한다. 또한, 도 37은 4다리 로봇을, 목표 제 1 접지부위 상반력 중심점(Q1)과 목표 제 2 접지부위 상반력 중심점(Q2)을 통과하는 수직면의 법선방향으로(즉 거의 측방으로부터) 본 도면, 도 38은 이동(보행)하고 있는 2다리 로봇을 목표 제 1 접지부위 상반력 중심점(Q1)과 목표 제 2 접지부위 상반력 중심점(Q2)을 통과하는 수직면의 법선방향으로(즉 거의 측방으로) 본 도면, 도 39는 거의 직립 자세의 2다리 로봇을 목표 제 1 접지부위 상반력 중심점(Q1)과 목표 제 2 접지부위 상반력 중심점(Q2)을 통과하는 수직면의 법선방향으로(즉 거의 배면으로) 본 도면이다.

이들 도 37~도 39에는, 상기 수직면에서의 상정 바닥의 단면이 세선으로 표시되고, 상기 수직면에서의 실 바닥의 단면이 굵은 선으로 표시되어 있다. 또한, 도 37에서는, 로봇의 목표 자세(목표 보용의 순시값에서의 로봇의 전체적인 자세)와, 실 자세가 각각 파선, 실선으로 표시되어 있다. 또, 도 38 및 도 39에서는, 로봇의 목표 자세(목표 보용의 순시값에서의 로봇의 전체적인 자세)와, 실 접지부위 위치 자세가 각각 세선, 굵은 선으로 표시되어 있다. 이들 상황에서의 실 제 n 바닥 접점은 실 바닥면상의 점으로, 도 37, 도 38 및 도 39에 도시하는 위치가 된다.

상정 바닥면에 대한 실 바닥면의 형상 편차를 바닥형상 편차라고 부른다. 바닥형상 편차를 정량적으로 표현하는 지표로서, 제 n 노드 바닥높이 편차, 제 n 노드 바닥경사 편차를 이하와 같이 정의한다.

제 n 바닥 접점에서의 바닥면의 높이를 「제 n 접지부위 바닥 높이」라고 부른다. 잎인 제 n 노드에 대해, 실 제 n 접지부위 바닥높이와 상정 제 n 접지부위 바닥높이의 차를 「제 n 접지부위 바닥높이 편차」 또는, 「제 n 노드 바닥높이 편차」라고 부른다. 제 n 바닥 접점에서의 바닥면의 경사를 「제 n 접지부위 바닥경사」라고 부른다. 잎인 제 n 노드에 대하여, 실 제 n 접지부위 바닥경사와 상정 제 n 접지부위 바닥경사의 차를 「제 n 접지부위 바닥경사 편차」 또는, 「제 n 노드 바닥경사 편차」라고 부른다. 접지부위 바닥경사 편차의 예를 도 39에 도시한다.

잎 노드인, 모든 제 j 노드에 대해, 목표 제 j 접지부위 위치 자세와 상정 제 j 바닥면의 높이와 경사(상세하게는, 상정 제 j 바닥 접점에서의 상정 바닥면의 높이와 경사)의 관계와, 노드 보상각의 세트를 사용한 컴플라이언스 동작에 의해 수정된 수정 목표 제 j 접지부위 위치 자세와 실 제 j 바닥면의 높이와 경사(상세하게는, 실 제 j 바닥 접점에서의 실 바닥면의 높이와 경사)의 관계가 일치하기 위한 노드 보상각의 세트를 「노드 바닥경사 편차(의 세트)」라고 부르고, 이 중의 제 n 노드 보상각에 대응하는 노드 바닥경사 편차의 성분을 「제 n 노드 바닥경사 편차」라고 부른다. 이렇게 정의한 「제 n 노드 바닥경사 편차」는, 제 n 노드가 잎 노드인 경우, 먼저 상기에서 잎 노드에 관해 정의한 「제 n 노드 바닥경사 편차」(=실 제 n 접지부위 바닥경사와 상정 제 n 접지부위 바닥경사의 차)에 일치하는 것이다.

결국, 노드 바닥경사 편차의 세트는, 목표 보용대로 이동하고 있는 로봇의 모든 접지부위를 목표 바닥면에 평행해져 되어 있는 상태로부터, 실 바닥면에 평행하게 되게 하기 위해 필요한 보상량에 상당한다.

따라서, 보행중에 바닥형상 편차를 추정하고, 추정한 바닥형상 편차를 목표 접지부위 위치 자세에 가하면, 바닥형상 편차가 있어도, 각 노드의 실 상반력 모멘트는 상정 바닥을 보행하고 있을 때와 동일하게 된다. 당연히, 잎 노드의 실 상반력 모멘트인 접지부위의 실 상반력 모멘트도 목표 접지부위 상반력 모멘트에 일치한다.

통상은, 상기와 같이 정의되는 「제 n 노드 바닥경사 편차」를 사용하여 바닥형상을 표현해도 상관없다. 어떤 노드가 3개의 자 노드를 갖고, 3개의 자 노드의 목표 상반력 중심점이 동일 직선상에 배열된 경우나, 어떤 노드가 4개 이상의 자 노드를 갖는 경우 등에서는, 바닥형상을 표현하는 것이 곤란하게 된다.

그래서, 이후의 설명에서는, 잎이 아닌 제 n 노드에 대해서는, 보다 일반적으로 적용할 수 있는 이하의 표현을 사용하기로 한다. 이하의 정의에서의 소정의 가중평균을 구하기 위해서 사용하는 가중값은 상기 목표 상반력 분배기(102)가 상기한 바와 같이 결정한 전술의 가중값과 동일한 것으로 한다.

정의:

모든 목표 접지부위의 높이와 경사를, 각각 대응하는 실 바닥면의 높이와 경사에 일치시키고, 또한, 임의의 노드의 목표 상반력 중심점을 그 모든 자 노드의 목표 상반력 중심점의 소정의 가중평균(즉 소정의 내분비에 의한 내분점)으로 나타냈을 때, 임의의 제 n 노드에 대하여, 제 n 노드의 목표 상반력 중심점의 높이(연직방향 위치)로부터 제 n 노드의 부모 노드의 목표 상반력 중심점의 높이(연직방향 위치)를 감산한 높이를 「실 제 n 노드 상대 바닥높이」라고 부른다.

정의:

모든 목표 접지부위의 높이와 경사를, 각각 대응하는 상정 바닥면의 높이와 경사에 일치시키고, 또한, 임의의 노드의 목표 상반력 중심점을 그 모든 자 노드의 목표 상반력 중심점의 소정의 가중평균(즉 소정의 내분비에 의한 내분점)으로 나타냈을 때, 임의의 제 n 노드에 대하여, 제 n 노드째 목표 상반력 중심점의 높이(연직방향 위치)로부터 제 n 노드의 부모 노드의 목표 상반력 중심점의 높이(연직방향 위치)를 감산한 높이를 「상정 제 n 노드 상대 바닥높이」라고 부른다.

실 제 n 노드 상대 바닥높이로부터 상정 제 n 노드 상대 바닥높이를 감산한 높이를 「제 n 노드 상대 바닥높이 편차」라고 부른다. 제 n 노드 상대 바닥높이 편차는 이하와 같이 정의해도, 동일한 값이 된다.

정의:

모든 목표 접지부위의 높이와 자세를, 각각 대응하는 접지부위 바닥높이 편차와 접지부위 바닥경사 편차에 일치시키고, 또한, 임의의 노드의 목표 상반력 중심점을 그 모든 자 노드의 목표 상반력 중심점의 소정의 가중평균(즉 소정의 내분비에 의한 내분점)으로 나타냈을 때, 임의의 제 n 노드에 대하여, 제 n 노드의 목표 상반력 중심점 높이로부터 제 n 노드의 부모 노드의 목표 상반력 중심 높이를 감산한 높이를 「제 n 노드 상대 바닥높이 편차」라고 부른다.

상기한 제 n 노드 상대 바닥높이 편차의 정의로부터 명확한 바와 같이, 자 노드를 갖는 각 노드에 대하여, 그 모든 자 노드의 제 n 노드 상대 바닥높이 편차는 바닥높이 편차의 상대관계를 나타내는 것이 된다. 또, 자 노드를 갖는 각 노드 에 대하여, 그 모든 자 노드의 가중평균값은 0이 된다.

본 참고예(제 3 참고예)에서는, 실 바닥면과 목표 보용상에서의 상정 바닥면과의 벗어남(즉 바닥형상 편차)을 보상하여, 접지해야 할 접지부위(10)를 적정하게 실 바닥면에 접지시키도록 각 접지부위(10)의 목표 위치 자세의 수정을 행한다. 그리고, 본 참고예에서는, 상기한 바와 같이 정의되는 제 n 노드 상대 바닥높이 편차를 바닥형상 편차를 나타내는 바닥형상 패러미터로서 사용하고, 이것을 기초로, 각 제 n 접지부위 바닥높이 편차(바닥형상 편차)를 추정한다. 그리고, 추정한 각 제 n 접지부위 바닥높이 편차(이하, 단지 제 n 바닥높이 편차라고 하는 경우가 있음)에 따라 제 n 접지부위(10)의 목표 위치를 수정하도록 했다.

이상을 전제로 하여, 이하에 본 참고예의 주요부의 상세를 설명한다.

도 35에 도시한 계층형 컴플라이언스 동작 결정부(114)의 바닥형상 추정기(130)에의 입력은, 일반적으로는 크게 나누어서 이하와 같이 된다.

1) 각 목표 상반력 중심점(목표 전체 상반력 중심점(P), 목표 제 n 접지부위 상반력 중심점(Qn)(n=1,2,3,4,5,6)).

2) 상정 바닥면 형상(상정 제 n 바닥 접점의 좌표, 상정 제 n 접지부위 바닥경사).

3) 최종적으로 실 로봇이 추종해야 할 목표 자세(기구변형 보상된 수정 목표 접지부위 위치 자세) 또는 실 관절 변위 또는 이것들의 주파수 가중평균(주파수 특성을 갖는 가중값에 의한 가중평균).

4) 상체자세 경사 편차

5) 실 상반력(실 제 n 접지부위 상반력의 병진력 성분과 모멘트 성분).

또한, 본 참고예에서의 로봇(1)에서는, 각 접지부위(10)의 자세를 제어할 수 없으므로, 상정 바닥면 형상중, 상정 제 n 접지부위 경사는 필요하지 않다.

또, 본 참고예에서의 로봇(1)에서는, 실 상반력중의 실 제 n 접지부위 상반력의 모멘트 성분은 0이 되므로, 이것도 바닥형상 추정기(130)에 입력할 필요는 없다. 단, 이후의 본 참고예의 설명에서는, 각 접지부위(10)의 자세를 제어할 수 있을 경우도 고려하고, 제 n 접지부위 상반력에는, 일반적으로, 실 제 n 접지부위 상반력의 모멘트 성분도 포함되는 것으로 한다. 또, 이후의 설명에서는, 접지부위의 개수가 4개나 6개 이외의 경우에도 용이하게 본 참고예를 확장적으로 적용할 수 있도록, 접지부위의 총 수를 종종 「최종 잎 노드 번호」라고 나타낸다. 예를 들면 도 1의 6다리 로봇(1)에서는, 최종 잎 노드 번호는 「6」이다.

도 40은 바닥형상 추정기(130)의 처리기능을 도시하는 블럭도이다. 도 40에 도시하는 바닥형상 추정기(130)를 구성하는 각 기능 요소에 대해 설명하면, 바닥형상 추정기(130)는 기구 컴플라이언스 모델(134)을 구비한다.

기구 컴플라이언스 모델(134)은, 각 접지부위(10)가 실 상반력(Fn_act)(n=1,2,…, 최종 잎 노드 번호)을 받았을 때의 각 다리(#1~#6)의 컴플라이언스 기구(42) 등(각 다리의 컴플라이언스 기구(42) 및 링크기구)의 변형량을 구하고, 구한 변형량을, 상기 로봇 기하학 모델(110)(도 2 참조)에의 입력인 각 기구변형 보상된 수정 목표 접지부위 위치 자세에 가함으로써, 기구변형 후의 제 n 접지부위(각 접지부위)의 위치 자세의 추정값(보다 상세하게는, 실 상체자세가 목표 상체자세에 일치하고 있다고 가정한 경우의 제 n 접지부위 위치 자세의 추정값)인 추정 제 n 접지부위 위치 자세(n=1,2, …, 최종 잎 노드 번호)를 구한다. 또한, 이 때에 사용하는 각 기구변형 보상된 수정 목표 접지부위 위치 자세는 전회 제어주기에서 구한 값 등의 과거값이다.

기구변형 후의 각 추정 접지부위 위치 자세를 구할 때에, 로봇(1)의 관절 변위의 제어의 추종 지연을 무시할 수 없는 경우에는, 기구변형 보상된 수정 목표 접지부위 위치 자세를 그대로 사용하는 대신, 기구변형 보상된 수정 목표 접지부위 위치 자세를 추종 지연에 상당하는 로 패스 필터에 통과시켜서 이루어지는 위치 자세를 사용하면 된다.

또는, 로봇(1)의 실 관절 변위의 검출값으로부터 로봇 기하학 모델(도 2의 로봇 기하학 모델(110)과 동일한 모델)을 통하여 기구변형이 없는 경우의 실 접지부위 위치 자세인 기구변형 없는 실 접지부위 위치 자세를 구하고, 구한 값을 기구변형 보상된 수정 목표 접지부위 위치 자세 대신 사용하면 된다.

또는, 주파수 가중값(주파수 특성을 갖는 가중값)을 사용하여, 상기 기구변형 없는 실 접지부위 위치 자세와 기구변형 보상된 수정 목표 접지부위 위치 자세의 가중평균을 구하고, 그것을 기구변형 보상된 수정 목표 접지부위 위치자세 대신 사용하면 된다.

엄밀하게는, 상기한 기구변형 없는 실 접지부위 위치 자세를 사용해야 하지만, 관절 변위 제어의 능력이 높으면, 상기의 어느 것을 사용해도 큰 차는 없다.

또한, 기구 컴플라이언스 모델(134)에 대해서는, 본 출원인이 앞서 제안한 일본 특개평10-277969호 공보에서 상세가 설명되어 있으므로, 여기에서는 더 이상의 설명을 생략한다.

보충하면, 도 2에서 로봇 기하학 모델(110)에의 입력에 기구변형 보상이 포함되지 않는 경우, 즉, 기구변형 보상이 없는 수정 목표 접지부위 위치 자세(상기 수정 목표 접지부위 위치 자세 산출부(114g)에서 구해지는 위치 자세)를 로봇 기하학 모델(110)에 입력하는 경우에는, 그 기구변형 보상이 없는 수정 목표 접지부위 위치 자세를 바닥형상 추정기(130)의 기구 컴플라이언스 모델(134)에 입력하면 된다.

도 40의 설명으로 되돌아오고, 이어서, 기구변형 후의 추정 각 접지부위 위치 자세(n=1,2,…,6)를, 목표 전체 상반력 중심점(P)을 회전중심으로 하여 상기 상체자세 경사 편차(θberr)만큼 회전이동 시킴으로써, 글로벌 좌표계(지지 다리 좌표계)로 본 기구변형 후의 제 n 추정 접지부위 위치 자세(n=1,2,…, 최종 잎 노드 번호)를 구한다. 이어서, 글로벌 좌표계로 본 기구변형 후의 제 n 추정 접지부위 위치 자세(n=1,2,…, 최종 잎 노드 번호)와 제 n 접지부위의 목표 접지부위 위치(제 n 접지부위의 대표점의 목표 위치)로 본 목표 제 n 접지부위 상반력 중심점의 위치를 기초로, 글로벌 좌표계로 본 기구변형 후의 목표 제 n 접지부위 상반력 중심점에 대응하는 점의 위치를 구한다. 그리고, 이 구한 점의 위치를 순간 추정 바닥 접점 위치(순간 추정 제 n 바닥 접점 위치)(Qn_estm')으로 한다. 이 Qn_estm'은 실 제 n 바닥 접점의 순시 위치의 추정값에 상당하는 것이다. 또한, 순간 추정 바닥 접점위치(Qn_estm')의 구하는 방법의 상세는, 일본 특개평10-277969호 공보중 의 식 21의 Q"을 구하는 것에 상당하므로, 여기에서는 더 이상의 설명을 생략한다.

이어서, 순간 추정 제 n 바닥 접점위치(Qn_estm')로부터 상정 제 n 바닥 접점위치(Dn)를 감산한 것을, 바이어스 함유 순간 제 n 접지부위 바닥높이 편차(Zfn_with_bias')로서 얻는다. 이 Zfn_with_bias'는, 제 n 접지부위 바닥높이 편차의 순시 추정값에 상당하는 것이지만, 본 참고예와 같이 상체의 실 높이를 검출하지 않는 경우에는, 일반적으로 바이어스 오차를 포함하고 있다. 그 때문에, Zfn_with_bias'를 바이어스 함유 순간 제 n 접지부위 바닥높이 편차라고 부른다.

여기에서, 상정 제 n 바닥 접점(Dn)은, 앞서 정의한 바와 같이, 제 n 접지부위가 접지하고 있는 시기에서는, 목표 제 n 접지부위 상반력 중심점(Qn)과 동일 위치로 한다. 목표 보용에서 제 n 접지부위가 접지하기 직전의 시기에서는, 다음에 접지할 때에 상정되어 있는 목표 제 n 접지부위 상반력 중심점(Qn)의 위치를 상정 제 n 바닥 접점(Dn)으로 한다. 목표 보용에서 제 n 접지부위가 이상한 직후의 시기에서는, 이상시에 상정되어 있던 목표 제 n 접지부위 상반력 중심점(Qn)의 위치를 상정 제 n 바닥 접점(Dn)으로 한다.

이어서, 바이어스 함유 순간 제 n 접지부위 바닥높이 편차(Zfn_with_bias')(n=1,2,…, 최종 잎 노드 번호)와 제 n 접지부위 상반력(Fn_act)(n=1,2,…,최종 잎 노드 번호)을 기초로, 도 41의 플로우차트에서 나타내는 바닥높이 편차 추정처리 서브루틴에 의해, 추정 제 n 노드 바닥높이 편차(Zfn_estm)(n=1,2,…,최종 잎 노드 번호)를 구한다.

여기에서, 바닥높이 편차 추정처리 서브루틴 등에 사용되는 「계층 상대화 처리」(또는 「계층 상대화」)에 대해 설명한다.

계층 상대화 처리란, 일반적으로 정의하면, 모든 잎 노드에 대한 입력값(어떤 소정 종류의 상태량의 값)의 세트에 대해, 모든 노드의 출력값을 결정하는 처리이다. 보다 상세하게는, 계층 상대화 처리는, 잎 노드가 아닌 임의의 노드의 모든 자 노드에 대응하는 출력값의 가중평균이 0이며, 또한, 임의의 잎 노드의 입력값(상태량)이 그 노드의 출력값과 그 노드의 모든 선조 노드의 출력값의 합에 일치하도록 각 노드 출력값을 결정하는 처리이다.

이후, 잎 노드에 대한 어떤 종류의 입력(상태량)(AA)으로부터, 계층 상대화 처리에 의해 어떤 종류의 출력(BB)의 값을 구하는 것을, 「AA를 계층 상대화하여 BB를 구한다」라고 한다.

이하에 계층 상대화 처리의 알고리즘을 설명한다. 일반적으로, 계층 상대화 처리의 입력을 제 n 접지부위 높이(Zfn)(n은 잎 노드 번호), 출력을 제 n 노드 상대높이(Zn_rel)(n=1,2,…, 최종 노드 번호)로 한다. 또한, 「최종 노드 번호」는 모든 노드 번호 중 최대 번호를 의미하고, 예를 들면 제 2 참고예에서 설명한 계층구조에서는, 최종 노드 번호=145236이다. 또, 이 알고리즘의 설명에서는, 「제 n 접지부위 높이」는, 상기 제 n 접지부위 바닥높이, 또는 제 n 접지부위 바닥높이 편차 등, 계층화 상대화 처리의 입력을 총칭적으로 나타낸 명칭이며, 「제 n 노드 상대높이」는, 후술하는 제 n 접지부위 상대 바닥높이 편차 등, 계층 상대화 처리의 출력을 총칭적으로 나타낸 명칭이다.

먼저, 바이어스 함유 제 n 노드 높이(Zn_with_bias)를 이하와 같이 결정한 다. 즉, 제 n 노드가 잎 노드(즉 접지부위)이면, 바이어스 함유 제 n 노드 높이(Zn_with_bias)에 제 n 접지부위 높이(Zfn)의 값을 대입한다. 제 n 노드가 잎 노드(즉 접지부위)가 아니면, 제 n 노드의 모든 자 노드의 바이어스 함유 제 n 노드 높이(Zn_with_bias)의 가중평균을 구하고, 이것을 가지고 바이어스 함유 제 n 노드 높이(Zn_with_bias)로 한다. 단, 각 노드에 대한 가중값에는, 상기 목표 상반력 분배기(102)가 결정한 가중값(Wj)(j=1,2,…)을 사용한다.

즉, 이하의 식 32에 의해, 바이어스 함유 제 n 노드 높이(Zn_with_bias)를 얻는다.

제 n 노드가 잎 노드인 경우,

Zn_with_bias=Zfn

제 n 노드가 잎 노드가 아닌 경우,

Zn_with_bias=Σ(Zj_with_bias*Wj)

단, Σ는 j∈{제 n 노드의 자 노드 번호의 집합}인 j에 대한 총 합. …식 32

이상의 룰에 따라, 모든 노드에 대해 바이어스 함유 노드 높이(Zn_with_bias)(n=1,2,…,최종 노드 번호)를 구한다.

최후에, 제 n 노드의 부모 노드(이것은 제 n 노드라고 함)의 바이어스 함유 높이(Zh_with_bias)를 바이어스 함유 제 n 노드 높이(Zn_with_bias)로부터 감산함으로써, 제 n 노드 상대높이(Zn_rel)를 구한다.

즉, 다음 식 33에 의해 제 n 노드 상대높이(Zn_rel)를 구한다. 단, 제 h 노 드는 제 n 노드의 부모 노드인 것으로 한다.

Zn_rel=Zn_with_bias-Zh_with_bias …식 33

이상의 룰에 따라, 모든 노드에 대해 노드 상대 바닥높이(Zn_rel)(n은 각 노드의 번호)를 구한다. 단, 뿌리 노드에 대한 노드 상대높이(Zk_rel)(k는 뿌리 노드의 번호)는 0으로 한다. 도 42에 제 1 참고예에서 나타낸 4다리 로봇의 노드 상대높이의 산출 예를 나타내고, 도 43에 본 참고예에서의 6다리 로봇의 노드 상대높이의 산출예를 나타낸다.

바이어스 함유 제 n 노드 높이(Zn_rel_with_bias)는 동일한 결과가 얻어지는 이하의 방법에 의해 구해도 된다.

제 n 노드가 잎 노드(즉 접지부위)이면, 바이어스 함유 제 n 노드 높이(Zn_with_bias)에 제 n 접지부위 높이(Zfn)의 값을 대입한다. 제 n 노드가 잎 노드(즉 접지부위)가 아니면, 제 n 노드의 모든 자손의 잎 노드의 바이어스 함유높이의 가중평균을 구하고, 이것을 가지고 바이어스 함유 제 n 노드 높이 Zn_with_bias로 한다. 단, 여기에서의 각 잎 노드 j에 대한 가중값(Wj')은 그 노드 j에 대해 상기 목표 상반력 분배기(102)가 결정한 가중값(Wj)과 그 노드 j의 선조 노드 또한 제 n 노드의 자손 노드인 모든 노드에 대해 상기 목표 상반력 분배기(102)가 결정한 가중값의 적으로 한다.

즉, 이하의 식 34에 의해, 바이어스 함유 제 n 노드 높이 Zn_with_bias를 얻는다.

제 n 노드가 잎 노드이면,

Zn_with_bias=Zfn

제 n 노드가 잎 노드가 아니면,

Zn_with_bias=Σ(Zj_with_bias*Wj')

단, Σ는 j∈{노드 n의 자손의 잎 노드 번호의 집합}인 j에 대한 총 합.

…식 34

보충하면, 이 경우의 가중값(Wj')은 목표 제 j 노드 상반력이 0이 아니면, 각 잎 노드의 목표 상반력을 목표 제 j 노드 상반력으로 나눈 값과 일치한다.

이상이 계층 상대화 처리이다.

이어서, 바닥높이 편차 추정 처리의 서브루틴 처리를, 그것을 도시하는 도 41의 플로우차트 등을 사용하여 이하에 설명한다. 또한, 이 처리에서는, 계층 상대화 처리의 입력으로서, 제 n 접지부위 바닥높이 편차를 사용하고, 출력을 제 n 노드 상대 바닥높이 편차로 한다. 그리고, 이들 입력 및 출력의 참조부호로서, 상기 식 32~34 등에 나타낸 것과 동일한 부호를 사용한다.

도 41을 참조하여, 우선, S50에서, 상기 바이어스 함유 순간 제 n 접지부위 바닥높이 편차(Zfn_with_bias')를 계층 상대화하여 순간 제 n 노드 상대 바닥높이 편차(Zn_rel')(n=1,2,…,최종 노드 번호)를 구한다.

이어서, S52에서, 제어장치(50)의 전회, 제어주기에서 구한, 제 n 접지부위 바닥높이 편차의 추정값인 추정 제 n 접지부위 바닥높이 편차(이후, 이것을 전회 추정 제 n 접지부위 바닥높이 편차(Zfn_estm_p)라고 부름)를 계층 상대화하여 얻어지는 출력을 전회 추정 제 n 노드 상대 바닥높이 편차(Zn_rel_estm_p)(n=1,2,…,최 종 노드 번호)로서 구한다. 단, 이 계층 상대화 처리에서 사용하는 각 노드의 가중값에는, 제어장치(50)의 금회의 제어주기에서 상기 목표 상반력 분배기(102)가 결정한 가중값(Wn)(n=1,2,…,최종 노드 번호)을 사용한다.

보충하면, 일반적으로, 금회의 제어주기에서 결정된 가중값(이하, 금회 가중값이라고 함)과 전회의 제어주기에서 결정된 가중값(이하, 전회 가중값이라고 함)이 상이한 경우에는, 실제 바닥의 형상에 변화가 없어도, 전회 가중값을 사용하여 구한 각 노드의 전회 추정 상대 바닥높이 편차의 값과 금회 가중값을 사용하여 구한 각 노드의 전회 추정 상대 바닥높이 편차의 값은 상이한 값으로 되어버린다. 그래서, 전회의 제어주기에서 구한 각 노드의 추정 상대 바닥높이 편차를 그대로 각 노드의 전회 추정 노드 상대 바닥높이 편차로서 사용하지 않고, 상기한 바와 같이, 금회 가중값을 사용하여 각 노드의 전회 추정 노드 상대 바닥높이 편차를 다시 계산하도록 했다.

이어서, S54에서, 다음 식 35에 의해, 제 n 노드 상대 바닥높이 편차 보정량 후보값(Zn_inc_cand)(n=1,2,…,최종 노드 번호)을 구한다. 이 Zn_inc_cand는 제 n 노드 상대 바닥높이 편차를 금회의 제어주기에서 갱신할 때의 이 편차의 보정량의 임시값을 의미하는 것이다.

이 경우, 제 n 노드 상대 바닥높이 편차 보정량 후보값(Zn_inc_cand)은, 다음 식 35에 나타내는 바와 같이, 순간 제 n 노드 상대 바닥높이 편차(Zn_rel')와 전회 추정 제 n 노드 상대 바닥높이 편차(Zn_rel_estm_p)의 차와 소정의 계수(△T/(Testm+△T))의 적으로 설정된다.

Zn_inc_cand=(Zn_rel'-Zn_rel_estm_p)

*△T/(Testm+△T) …식 35

여기에서, 식 35중의 Testm은 제 n 노드 바닥높이 편차의 추정(갱신)에 있어서의 시정수(1차 지연의 시정수)이고, △T는 제어장치(50)의 제어주기이다.

이와 같이, Zn_inc_cand는 설정했을 때, 제어장치(50)의 제어주기마다, Zn_inc_cand를 Zn_rel_estm_p에 가산해 감으로써 그 가산 결과의 값은, 서서히 Zn_rel'에 근접하도록 변화한다.

이어서, S56에서, 목표 보용의 시기에 따라 각 노드의 모드의 요구값인 노드 요구 모드(제 n 요구 모드(mdn_dmd))를 결정한다.

구체적으로는, 상기 6다리 로봇(1)에서는, 제 n 노드 요구 모드(n=1,2,3,4,5,6), 제 145 노드 요구 모드(md145dmd), 제 236 노드 요구 모드(md236dmd) 및 제 145236 노드 요구 모드(md145236dmd)를 목표 보용의 시간경과에 대해 도 44의 타이밍 차트로 나타내는 바와 같이 설정하고, 이것에 따라서 현재의 요구 모드를 결정한다. 단, 보용이 다르면, 그것에 따라 노드 요구 모드의 패턴도 바꾸어야 한다. 또한, 도 44의 제 1 단(최상단) 및 제 2 단의 타이밍 차트에서의 ON/OFF는, 접지부위가 접지하고 있는 상태가 ON에 대응하고 있고, 접지하고 있지 않은 상태가 OFF에 대응하고 있다.

보충하면, 본원 출원인이, 일본 특개평10-277969호 공보에 개시한 바와 같이, 접지부위로서 족평을 갖고, 접지부위에서의 바닥경사 편차를 추정할 수 있는 로봇의 경우에는 접지부위의 바닥경사 편차 추정에 대응하는 요구 모드도, 동 공보 에 설명되어 있는 바와 같이 설정해야 한다.

도 44에 도시하는 바와 같이, 각 노드의 모드(이하, 단순히 노드 모드라고 함)에는, 준비완료 모드, 홀딩 모드, 및 리셋 모드가 있다.

준비완료 모드는 바닥형상의 추정을 행하는 모드이다. 홀딩 모드는 바닥 형상의 추정값(추정 제 n 접지부위 바닥높이 편차)을 홀딩 하는(직전의 값을 유지하는) 모드이다. 홀딩 모드는 바닥형상 추정값이 발산할 우려가 있는 시기와, 바닥형상 추정값의 정밀도가 저하될 두려움이 있는 시기에 존재시킨다. 또, 리셋 모드는 바닥형상 추정값을 다음의 바닥형상의 추정을 개시(다음의 준비완료 모드의 개시)하기 까지 소정의 초기값으로 이행시키는 모드이다.

잎 노드인 제 n 노드(n=1,2,3,4,5,6)에 대해서는, 목표 보용상에서 제 n 접지부위가 접지하고 있으면(즉 목표 제 n 접지부위 상반력이 0이 아니면), 제 n 노드 요구 모드는, 준비완료 모드에 설정된다. 그 후, 목표 보용상에서 제 n 접지부위가 이상한(즉 목표 제 n 접지부위 상반력이 0으로 된) 것이라면, 그 직후는 제 n 노드 요구 모드가 홀딩 모드로 설정된다. 그리고, 그 후 잠시 후 제 n 노드 요구 모드가 리셋 모드로 설정된다. 또한, 목표 보용상에서 제 n 접지부위가 접지하기 직전부터, 제 n 노드 요구 모드가 준비완료 모드로 설정된다.

또, 잎 노드가 아닌 제 n 노드(n=145,236,145236), 즉, 자 노드를 갖는 제 n 노드에 대해서는, 그 제 n 노드에 속하는 적어도 1개의 접지부위가 목표 보용상에서 접지하고 있는 경우에는, 제 n 노드 요구 모드는 준비완료 모드로 설정된다. 그 후, 목표 보용상에서 제 n 노드에 속하는 모든 접지부위가 이상하면, 그 직후까 지는, 제 n 노드 요구 모드가 홀딩으로 설정된다. 또한, 제 n 노드에 속하는 모든 접지부위가 이상하기 직전부터 제 n 노드 요구 모드를 홀딩 모드로 설정하도록 해도 된다. 그리고, 그 후 잠시 후 제 n 노드 요구 모드가 리셋 모드로 설정된다. 또한, 목표 보용상에서 제 n 노드에 속하는 적어도 1개의 접지부위가 접지하기 직전부터, 제 n 노드 요구 모드가 준비완료 모드로 설정된다.

이어서, S58에서, 추정 허가조건을 만족하는지 아닌지를 판정하고, 그 판정 결과와 노드 요구 모드에 기초하여 각 노드의 모드를 최종적으로 결정한다. 결정되는 각 노드의 모드는 상기 준비완료 모드, 홀딩 모드, 및 리셋 모드 중 어느 하나이다. 상기 노드 요구 모드는, 목표 보용상에서의 각 접지부위의 접지, 비접지에 따라 결정한 것이었지만, S58에서는, 각 접지부위의 실제의 접지, 비접지 등을 고려하여, 각 노드의 모드가 결정된다.

추정 허가조건이란, 이하의 어느 하나의 식 36, 37을 만족하는 것이다. 이 경우, 식 36, 37의 모두 만족하지 않을 때는 추정 허가조건이 성립하지 않는다고 한다.

Fn_act_z>Fn_min …식 36

Zn_rel'<Zn_rel_estm_p …식 37

여기에서, Fn_act_z는 실 제 n 접지부위 상반력(n=1,2,…,최종 잎 노드 번호)의 병진력 연직성분이다.

추정 허가조건이란, 제 n 노드 상대 바닥높이 편차를 추정해도(바닥형상 편차를 추정해도), 그 추정값이 발산하지 않는 조건이다. 여기에서 발산이란, 후술 하는 바와 같이 추정된 추정 제 n 바닥높이 편차(Zfn_estm)(바닥형상 편차의 추정값)를 사용하여, 실재의 제 n 바닥높이 편차의 영향을 없애도록 수정 목표 접지부위 위치 자세를 수정하는 보정 동작을 행한 경우에, 추정 제 n 바닥높이 편차(Zfn_estm)가 계속 증가하고, 제 n 접지부위가 바닥으로부터 떨어져 가는(바닥으로부터 더욱 떠 가는) 상황을 의미한다.

Zfn_estm의 추정이 이상적으로 실행된다면, 소정의 허용값(Fn_min)은 0이면 되지만, 실제로는 Fn_act의 검출 오차가 발생하고, 그것에 의해 추정값이 발산하는 경우가 있다. 그것을 막기 위해서, Fn_min은 Fn_act의 검출오차보다도 충분하게 큰 값으로 설정한다.

본 참고예에서는 추정 허가조건이란, 이상에 기술한 발산이 생기지 않는 조건이다. 단, 추정 허가 조건은 바닥 형상의 소요의 추정 정밀도를 확보할 수 있는 조건이어도 된다. 따라서, 실 제 n 접지부위 상반력(Fn_act)의 병진력 연직성분 대신, 실 제 n 접지부위 상반력(Fn_act)의 병진력 성분 내의 상정 바닥면(또는 추정 바닥면(추정한 실 바닥면))에 수직한 성분을 사용해도 된다.

또한, 바닥형상 편차(실 바닥면과 상정 바닥면의 형상 편차)를 추정하는 것만으로 보행동작에 반영하지 않는 경우, 즉, 바닥형상 편차의 추정값을 사용하여 바닥형상 편차의 영향을 없애도록 수정 목표 접지부위 위치 자세를 수정하지 않는 경우에는 바닥형상 편차의 추정값이 발산하지 않는다.

추정 허가조건을 만족하는지 아닌지의 판정 결과와 노드 요구 모드에 기초하여 각 노드의 모드를 결정한다.

제 n 노드 요구 모드가 준비완료 모드이고, 또한, 추정 허가조건을 만족하면, 제 n 노드 모드를 준비완료 모드로 한다.

제 n 노드 요구 모드가 준비완료 모드이고, 또한, 추정 허가조건을 만족하지 않으면, 제 n 노드 모드를 홀딩 모드로 한다.

제 n 노드 요구 모드가 홀딩 모드이면, 제 n 노드 모드를 홀딩 모드로 한다.

제 n 노드 요구 모드가 리셋 모드이면, 제 n 노드 모드를 리셋 모드로 한다.

따라서, 제 n 노드의 모드는, 요구 모드가 준비완료 모드이고, 또한, 추정 허가조건을 만족하는 경우에 한하여, 최종적으로 준비완료 모드로 결정된다.

이어서, S60~S70에서, 모든 노드에 대하여, 노드에 대응하는 제 n 노드 상대 바닥높이 편차(n=1,2,…,최종 노드 번호)를 추정한다.

보충하면, 일본 특개평10-277969호 공보에 도시한 바와 같이, 자세를 제어가능한 접지부위(족평)를 가지고, 접지부위에서의 바닥경사 편차를 추정할 수 있는 로봇의 경우에는 접지부위에 대응한 바닥경사 편차도 추정하는 것이 바람직하다.

이 때문에, S60~S70의 처리는, 접지부위의 자세를 제어가능한 경우에도, 바닥형상 편차의 추정을 행할 수 있도록 구성되어 있다.

이하에 S60~S70의 처리를 구체적으로 설명하면, S62에서, 제 n 노드의 자 노드의 개수를 판정한다. 이때, 자 노드수(자 노드의 개수)가 2개인 경우에는, S64에서, 그 개수에 대응한 바닥형상 편차의 추정 처리인 2자 노드용 바닥형상 추정 처리가 행해진다. 또, 자 노드수가 3개인 경우에는, S66에서, 그 개수에 대응한 바닥형상 편차의 추정 처리인 3자 노드용 바닥형상 추정 처리가 행해진다. 또, 자 노드수가 0개인 경우에는, S68에서, 이 제 n 노드의 접지부위가 상반력 모멘트를 제어할 수 있는지 아닌지를 판정한다.

자 노드가 0인 경우의 처리에 대해서 더욱 설명하면, 제 n 노드의 자 노드수가 0인 경우, 제 n 노드는 잎 노드이고, 제 n 노드는 접지부위에 대응하고 있다. 이 경우, 상기와 같이 S68에서, 우선, 제 n 접지부위가 상반력 모멘트를 제어할 수 있는지 아닌지를 판정한다. 여기에서, 예를 들면, 통상의 2족보행 로봇과 같이, 접지부위로서 자세를 제어가능한 족평을 가지고, 다리에 프리 조인트를 포함하지 않는 로봇의 경우에는, 접지부위에서 상반력 모멘트를 발생할 수 있다. 단, 본 참고예의 로봇(1)(6다리 로봇)에서는, 각 접지부위(10)가 프리 조인트인 볼 관절(12) 걸어맞추어져 있기 때문에, 어떠한 접지부위(10)도 상반력 모멘트를 발생할 수 없다. 이 경우에는, 자 노드수가 0인 제 n 노드에 대한 처리는 실행되지 않는다.

한편, S68에서, 상반력 모멘트를 제어할 수 있다고 판정된 경우에는, S70에서, 접지부위 바닥경사 편차 추정처리가 행해진다. 이 처리는 각 접지부위에서의 바닥경사 편차를 추정하는 처리이다. 이 처리는, 본 출원인이 앞서 제안한 일본 특개평10-277969호 공보 중의 바닥경사 추정 처리에서의 족평 상반력 중심점을 본 참고예에서의 제 n 접지부위의 목표 상반력 중심점으로 치환한 처리이다. 따라서, 본 명세서에서는, 더 이상의 상세 설명을 생략한다.

다음에, 상기 2자 노드용 바닥형상 추정 처리를 도 45~도 48을 참조하여 설명한다. 도 45는 2자 노드용 바닥형상 추정 처리의 서브루틴 처리를 도시하는 플로우차트, 도 46, 도 47은 각각 도 45의 S6404, S6408의 서브루틴 처리를 도시하는 플로우차트, 도 48은 도 45의 S6406 및 S6410의 서브루틴 처리를 도시하는 플로우차트이다.

2자 노드용 바닥형상 추정 처리에서는, 도 45의 플로우차트에 도시되는 바와 같이, S6400에서, 2개의 자 노드를 갖는 제 n 노드의 모든 자 노드의 모드(도 41의 S58에서 결정된 모드)가 판단된다. 그 판단 결과는, 「모두 준비완료」, 「모두 리셋」, 및, 「기타(else)」의 3개로 나누어진다. 또한, 이후의 설명에서는, 제 n 노드의 2개의 자 노드를 제 i 노드, 제 j 노드로 한다.

여기에서, 자 노드의 모드가 「모두 준비완료」라고 판단하면, S6402에서, 제 n 노드의 자 노드의 실 노드 상반력의 병진력 연직성분(Fi_act_z, Fj_act_z)의 합력(Fn_z)(=Fi_act_z+Fj_act_z)이 소정의 값(Fn_min2)보다 큰지 아닌지를 판단한다. Fn_z는, 바꾸어 말하면, 제 n 노드에 속하는 모든 접지부위의 실 상반력의 합력의 병진력 연직성분이다.

S6402의 판단 결과가 YES인 경우에는 S6404에서, 2개의 자 노드를 갖는 노드용의 그룹 내 전체 추정 처리(제 n 노드가 갖는 2개의 자 노드의 각각의 노드 상대 바닥높이 편차를 실질적으로 추정하는 처리)를 행한다. 이 처리에서는, 도 46의 플로우차트 중에 나타내는 식에 따라 제 n 노드의 2개의 자 노드인 제 i 노드 및 제 j 노드의 각각의 추정 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm)가 구해진다(갱신된다). 즉, Zi_rel_estm에 대해 대표적으로 설명하면, Zi_el_estm의 전회 제어주기에서의 값(Zi_rel_estm_p)에, 상기 S54에서 구한 제 i 노드 상대 바닥높이 편차 보정량 후보값(Zi_inc_cand)을 첨가함으로써 새로운 추정 제 i 노드 상대 바닥높이 편차(Zi_rel_estm)가 구해진다. 제 j 노드에 대해서도 동일하다.

또, 제 n 노드의 2개의 자 노드의 상반력의 합력(Fn_z)이 소정의 값(Fn_min2)보다 작은 경우(S6402의 판단 결과가 NO인 경우)에는, 바닥형상 편차의 추정 정밀도가 지나치게 떨어지므로, 실질적인 추정 처리를 실행하지 않고, S6406에서, 2개의 자 노드를 갖는 노드용의 그룹 내 전체 홀딩 처리(제 n 노드가 갖는 2개의 자 노드의 각각의 추정 노드 상대 바닥높이 편차를 갱신하지 않고 홀딩하는 처리)가 행해진다. 그 홀딩 처리에서는, 도 48의 플로우차트중의 식으로 표시하는 바와 같이, 제 i 노드 및 제 j 노드 각각의 추정 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm)의 값이 전회 제어주기에서의 값(Zi_rel_estm_p, Zj_rel_estm_p)에 유지된다.

다음에, 도 45의 S6400에서, 2개의 자 노드의 모드가 「모두 리셋」이라고 판단하면, S6408에서, 2개의 자 노드를 갖는 노드용 그룹 내 전체 리셋 처리(제 n 노드가 갖는 2개의 자 노드의 각각의 추정 노드 상대 바닥높이 편차를 리셋하는 처리)가 행해진다. 그 리셋 처리에서는, 도 47의 플로우차트 중의 식에 따라서 노드 및 제 i 노드 및 제 j 노드 각각의 추정 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm)가 0에 서서히 근접해 가도록 갱신된다. 또한, 동 식중의 △T, Testm의 의미는 상기 식 35의 것과 동일하다.

이 리셋 처리는, 보다 일반적으로 말하면, Zi_rel_estm, Zj_rel_estm을, 그것들의 가중평균값이 0이라는 조건, 즉 Wi*Zi_rel_estm+Wj*Zj_rel_estm=0을 만족하면서, 각각 Zi_rel_estm_p, Zj_rel_estm_p, 보다도 0에 가까운 값으로 결정한다는 처리이다. 또한, 이 리셋 처리는, 일차 지연요소를 포함하기 때문에, 노드 상대 바닥높이 편차가 완전하게 0으로 되돌아오기 위해서는 무한시간을 요한다. 그래서, 본 출원인이 먼저 제안한 유한 정정함수 발생기(일본 특개평5-32411호 공보)를 사용하여, 노드 상대 바닥높이 편차를 서서히 0으로 리셋하도록 해도 된다.

또, 도 45의 S6400에서, 2개의 자 노드의 모드가 「기타」인 경우, 즉, 「모두 준비완료」, 「모두 리셋」의 어떤 것도 아닌 경우(예를 들면, 2개의 자 노드의 모드가 모두 홀딩 모드인 경우)에는, S6406과 동일한 처리(도 48을 참조)가 S6410에서 실행된다.

이상과 같이, 제 n 노드의 2개의 자 노드 모드가 모두 준비 완료인 경우에, 또한, 노드 상대 바닥높이 편차의 추정 처리의 발산이 생길 우려가 없는 경우에만, 그 2개의 자 노드의 노드 상대 바닥높이 편차의 실질적인 추정처리가 행해진다. 그리고, 자 노드의 모드가 모두 준비완료인 경우이라도, 추정처리의 발산이 생길 우려가 있는 경우에는, 2개의 자 노드의 추정 노드 상대 바닥높이 편차의 값이 홀딩 된다. 또, 2개의 자 노드의 실 노드 상반력이 모두 0이 되는 상황, 즉, 제 n 노드에 속하는 모든 접지부위(10)에 실 상반력이 작용하지 않는 상황에서는, 2개의 자 노드의 노드 상대 바닥높이 편차는 서서히 0이 되도록 리셋된다.

다음에 도 41의 S66의 3자 노드 바닥형상 추정 처리를 도 49~도 55를 참조하여 설명한다. 도 49는 3자 노드용 바닥형상 추정 처리의 서브루틴 처리를 도시하는 플로우차트, 도 50, 도 51은 각각 도 49의 S6604, S6608의 서브루틴 처리를 도시하는 플로우차트, 도 52는 도 49의 S6606, S6614 및 S6618의 서브루틴 처리를 도 시하는 플로우차트, 도 53, 도 54는 각각 도 49의 S6612, S6616의 서브루틴 처리를 도시하는 플로우차트이다.

3자 노드용 바닥형상 추정 처리에서는, 도 49의 플로우차트에 도시되는 바와 같이, S6600에서, 3개의 자 노드를 갖는 제 n 노드의 모든 자 노드의 모드(도 41의 S58에서 결정된 모드)가 판단된다. 그 판단 결과는, 「모두 준비완료」, 「모두 리셋」, 「2개의 자 노드만 준비완료」, 「1개의 자 노드만 홀딩이고, 나머지가 리셋」, 「2개의 자 노드만 홀딩이고, 나머지가 리셋」, 및 「기타(else)」 6개로 나누어진다.

여기에서, 도 49의 플로우차트에서는, 3자 노드용 바닥형상 추정 처리를 보다 일반화하여 설명하기 위해서, 상기 제 2 참고예에서 설명한 바와 같이, 각 중간 노드(제 145 노드, 제 236 노드)에 속하는 접지부위(10)의 모두가 동시에 접지하거나 이상하는 경우뿐만 아니라, 각 중간 노드의 접지부위 중 어느 하나가 접지하고, 그밖의 접지부위가 이상하도록 로봇의 목표 보용을 생성하는 경우에 대해서도 고려하고 있다. 이 경우, 자 노드가 2개일 때와 달리, 3개의 자 노드의 모드가 「2개의 자 노드만 준비완료」, 「1개의 자 노드만 홀딩이고, 나머지가 리셋」, 혹은 「2개의 자 노드만 홀딩이고, 나머지가 리셋」인 경우도 고려할 필요가 있다. 또한, 이후의 설명에서는, 제 n 노드의 3개의 자 노드를 제 i 노드, 제 j 노드, 제 k 노드로 한다.

여기에서, 자 노드의 모드가 「모두 준비완료」라고 판단하면, S6602에서, 제 n 노드의 자 노드의 실 노드 상반력의 병진력 연직성분(Fi_act_z, Fj_act_z, Fk_act_z)의 합력(Fn_z)(=Fi_act_z+Fj_act_z+Fk_act_z)이 소정의 값(Fn_min2)보다 큰지 아닌지를 판단한다. Fn_z는 바꾸어 말하면, 제 n 노드에 속하는 모든 접지부위의 실 상반력의 합력의 병진력 연직성분이다.

S6602의 판단 결과가 YES인 경우에는, S6604에서, 3개의 자 노드를 갖는 노드용 그룹내 전체 추정 처리(제 n 노드가 갖는 3개의 자 노드의 각각의 노드 상대 바닥높이 편차를 실질적으로 추정하는 처리)를 행한다. 이 처리에서는, 도 50의 플로우차트 중에 나타내는 식에 따라서 제 n 노드의 3개의 자 노드인 제 i 노드, 제 j 노드 및 제 k 노드 각각의 추정 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm, Zk_rel_estm)이 구해진다(갱신된다). 즉, Zi_rel_estm에 대해 대표적으로 설명하면, Zi_rel_estm의 전회 제어주기에서의 값 Zi_rel_estm_p에, 상기 S54에서 구한 제 i 노드 상대 바닥높이 편차 보정량 후보값(Zi_inc_cand)을 가함으로써, 새로운 추정 제 i 노드 상대 바닥높이 편차(Zi_rel_estm)가 구해진다. 제 j 노드, 제 k 노드에 대해서도 동일하다.

또, S6602의 판단 결과가 NO일 경우에는, 바닥형상 편차의 추정 정밀도가 지나치게 떨어지므로, 실질적인 추정 처리를 실행하지 않고, S6606에서, 3개의 자 노드를 갖는 노드용의 그룹 내 전체 홀딩 처리(제 n 노드가 갖는 3개의 자 노드의 각각의 추정 노드 상대 바닥높이 편차를 갱신하지 않고 홀딩하는 처리)가 행해진다. 그 홀딩 처리에서는, 도 52의 플로우차트 중의 식으로 표시하는 바와 같이, 제 i 노드, 제 j 노드 및 제 k 노드 각각의 추정 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm, Zk_rel_estm의 값이 전회 제어주기에서의 값 Zi_rel_estm_p, Zj_rel_estm_p, Zk_rel_estm_p에 유지된다.

또 도 49의 S6600에서 3개의 자 노드의 모드가 「모두 리셋」이라고 판단하면, S6608에서, 3개의 자 노드를 갖는 노드용 그룹 내 전체 리셋 처리(제 n 노드가 갖는 3개의 자 노드의 각각의 추정 노드 상대 바닥높이 편차를 리셋하는 처리)가 행해진다. 그 리셋 처리에서는, 도 51의 플로우차트중의 식에 따라서 제 i 노드, 제 j 노드 및 제 k 노드의 각각의 추정 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm, Zk_rel_estm)가 0에 서서히 근접해 가도록 갱신된다. 또한, 동 식중의 △T, Testm의 의미는 상기 식 35의 것과 동일하다.

이 리셋 처리는 보다 일반적으로 말하면, Zi_rel_estm, Zj_rel_estm, Zk_rel_estm을, 그것들의 가중평균값이 0이라는 조건, 즉 Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0을 만족하면서, 각각 Zi_rel_estm_p, Zj_rel_estm_p, Zk_rel_estm_p 보다도 0에 가까운 값으로 결정한다고 하는 처리이다. 또한, Zi_rel_estm_p, Zj_rel_estm_p, Zk_rel_estm_p를 0으로 리셋하기 위해서, 상기 유한 정정함수 발생기(일본 특개평5-324115호 공보)를 사용해도 된다.

또, 도 49의 S6600에서, 3개의 자 노드의 모드가 「2개의 자 노드만 준비완료」라고 판단하면, 제 n 노드의 자 노드의 실 노드 상반력의 합력(제 n 노드에 속하는 모든 접지부위(10)의 실 상반력의 합력(제 n 노드에 속하는 모든 접지부위(10)의 실 상반력의 합력)의 병진력 연직 성분(Fn_z)이 소정의 값(Fn_min2)보다 큰지 아닌지를 S6610에서 판단한다.

이 판단 결과가 YES인 경우에는, S6612에서, 3개의 자 노드를 갖는 노드용 그룹 내 부분 추정 처리(제 n 노드가 갖는 3개의 자 노드의 각각의 노드 상대 바닥높이 편차를 실질적으로 추정하는 처리)가 행해진다. 이 처리는 도 53의 플로우차트에 도시하는 바와 같이 실행된다. 또한, 여기에서는 제 i 노드가 준비완료 모드가 아니고, 제 j 노드 및 제 k 노드가 준비완료 모드인 것으로 한다.

우선, S66120에서 제 i 노드의 모드가 판단된다. 이 판단 결과가 홀딩 모드인 경우에는 S66122에서 새로운 제 i 노드 상대 바닥높이 편차 보정량 후보값(Zi_inc_cand')을 0으로 결정하고, 리셋 모드일 경우에는, S66124에서, 도면 중의 식에 의해, 새로운 제 i 노드 상대 바닥높이 편차 보정량 후보값(Zi_inc_cand')을 결정한다. S66124에서 결정되는 Zi_inc_cand'는 Zi_rel_estm_p을 서서히 0에 근접시키기 위한 제 i 노드 상대 바닥높이 편차 보정량 후보값이다. 또한, S66124의 식중의 △T, Testm의 의미는 상기 식 35의 것과 동일하다. 보충하면, S66124에서 Zi_inc_cand'을 결정할 때, 상기 유한 정정 함수발생기(일본 특개평5-324115호 공보)를 사용해도 된다.

이어서, S66126에서, 도면 중의 식에 따라, 새로운 제 j 노드 상대 바닥높이 편차 보정량 후보값(Zj_inc_cand') 및 새로운 제 k 노드 상대 바닥높이 편차 보정량 후보값(Zk_inc_cand')을 결정한다.

즉, Wi*Zi_inc_cand'+Wj*Zj_inc_cand'+Wk*Zk_inc_cand'=0이라는 조건(Zi_inc_cand', Zj_inc_cand', Zk_inc_cand'의 가중평균값이 0이라는 조건)과, Zj_inc_cand'-Zj_inc_cand=Zk_inc_cand'-Zk_inc_cand라는 조건을 만족하도록 Zj_inc_cand', Zk_inc_cand'를 결정한다.

다음에, S66128에서, 상기한 바와 같이 결정된 Zi_inc_cand', Zj_inc_cand', Zk_inc_cand'을 각각 제 i 노드, 제 j 노드, 제 k 노드의 상대 바닥높이 편차의 전회 제어주기에서의 값 Zi_rel_estm_p, Zj_rel_estm_p, Zk_rel_estm_p에 가함으로써, 새로운 Zi_rel_estm, Zj_rel_estm, Zk_rel_estm이 결정된다.

이와 같이 Zi_rel_estm, Zj_rel_estm, Zk_rel_estm을 결정함으로써, Wi*Zi_rel_estm+Wj*Zi_rel_estm+Wk*Zk_rel_estm=0을 만족하면서, Zj_rel_estm_Zk_rel_estm이 Zj_inc_cand-Zk_inc_cand에 근접하도록 Zi_rel_estm, Zj_rel_estm, Zk_rel_estm이 결정되게 된다.

또, 도 49의 S6610의 판단 결과가 NO일 경우에는, 바닥형상 편차의 추정 정밀도가 지나치게 떨어지므로, 실질적인 추정 처리를 실행하지 않고, S6614에서 상기 S6606과 동일한 처리(도 52를 참조)가 실행된다.

또 도 49의 S6600에서, 3개의 자 노드의 모드가 「1개의 자 노드만 홀딩이고, 나머지가 리셋」이라고 판단하면, S6616에서, 1개의 자 노드만 홀딩이고, 나머지가 리셋인 경우에 대응하는 처리가 실행된다. 그 처리에서, 도 54의 플로우차트 중에 나타내는 식에 의해, 새로운 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm, Zk_rel_estm)가 결정된다. 또한, 여기에서는, 제 i 노드의 모드가 홀딩 모드이고, 제 j 노드 및 제 k 노드의 모드가 리셋 모드로 한다. 또, 동 식중의, △T, Testm의 의미는, 상기 식 35의 것과 동일하다.

도 54의 처리는 보다 일반적으로는 Zi_rel_estm을 0으로 결정하고, Zj_rel_estm, Zk_rel_estm을 Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0(Zi_rel_estm, Zj_rel_estm, Zk_rel_estm의 가중평균이 0인 조건)을 만족하면서, Zj_rel_estm_p, Zk_rel_estm_p 보다도 0에 가까운 값으로 결정하는 처리이다. 보충하면, 제 j 노드 및 제 k 노드의 모드가 리셋 모드로 되는 시점까지, 그들 노드의 가중값(Wj, Wk)은 0이 되어 있을 것이다. 따라서, 가중값 Wi, Wj, Wk의 합이 1이라는 조건으로부터, 이 시점까지 Wi가 1이 되어 있고, 또한, Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0이므로, 이 시점까지, Zi_rel_estm은 0으로 되어 있다. 이 때문에, 제 i 노드의 Zi_rel_estm의 값을 홀딩한다고 하는 것은, 그 값을 0에 유지한다고 하는 것이다. 그래서, 도 54의 처리에서는, Zi_rel_estm의 값을 0에 결정하도록 하고 있다.

또한, Zj_rel_estm, Zk_rel_estm을 서서히 0에 리셋하는 처리에서는, 상기 유한 정정함수 발생기(일본 특개평5-324115호 공보)를 사용해도 된다.

또한 도 49의 S6600에서, 자 노드의 모드가 「2개의 자 노드만 홀딩이고, 나머지가 리셋」이라고 판단하면, S6618에서 2개의 자 노드만 홀딩이고, 나머지가 리셋인 경우의 처리가 행해진다. 그 처리에서는, 도 55의 플로우차트중의 식에 따라, 새로운 노드 상대 바닥높이 편차(Zi_rel_estm, Zj_rel_estm, Zk_rel_estm)가 결정된다. 또한, 여기에서는, 제 i 노드 및 제 j 노드의 모드가 모두 홀딩 모드이고, 제 k 노드의 모드가 리셋 모드인 것으로 한다. 또, 동 식중의 △T, Testm의 의미는 상기 식 35의 것과 동일하다.

도 55의 처리는, 보다 일반적으로는, Zi_rel_estm, Zj_rel_estm을 각각 전회 제어주기 에서의 값으로 홀딩하고, Zk_rel_estm을 Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0(Zi_rel_estm, Zj_rel_estm, Zk_rel_estm의 가중평균값이 0이라는 조건)을 만족하면서, Zk_rel_estm_p보다도 0에 가까운 값으로 결정하는 처리이다. 보충하면, 제 k 노드가 리셋 모드가 되는 시점까지, Wk는 0으로 되어 있다.

또한, Zk_rel_estm을 서서히 0에 리셋하는 처리에서는, 상기 유한 정정함수 발생기(일본 특개평5-324115호 공보)를 사용해도 된다.

또한 도 49의 S6620에서 」「기타」라고 판단한 경우(예를 들면 3개의 자 노드의 모드가 모두 홀딩 모드인 경우)에는, S6620에서 상기 S6606과 동일한 처리(도 52를 참조)가 실행된다.

이상과 같이, 제 n 노드의 3개의 자 노드의 모드 중, 2개 이상의 자 노드의 모드가 준비완료인 경우에서, 또한, 노드 상대 바닥높이 편차의 추정 처리의 발산이 생길 우려가 없는 경우에만, 그 2개 이상의 자 노드의 노드 상대 바닥높이 편차의 실질적인 추정 처리가 행해진다. 그리고, 2개 이상의 자 노드의 모드가 준비완료인 경우라도, 추정 처리의 발산이 생길 우려가 있는 경우에는, 3개의 자 노드의 추정 노드 상대 바닥높이 편차의 값이 홀딩된다.

이상과 같이, 도 41의 S60~S66에서는, 추정 제 n 노드 상대 바닥높이 편차(Zn_rel_estm)(n=1,2,…,최종 노드 번호)가 결정된다. 또한, 뿌리 노드의 상대 바닥높이 편차는 0이다.

도 41의 플로우차트의 처리에서는, 최후에, S72에서, 제 n 노드(n=1,2,…,최 종 잎 노드 번호)의 모든 선조 노드의 추정 상대 바닥높이 편차와 추정 제 n 노드 상대 바닥높이 편차의 합을 구하고, 구한 합을 추정 제 n 접지부위 바닥높이 편차(추정 제 n 바닥높이 편차)(Zfn_estm)으로 한다.

이상이, 도 40에서의 바닥높이 편차의 추정 처리의 서브루틴 처리이다.

다음에 도 35의 설명으로 되돌아오고, 전술한 바와 같이 바닥형상 추정기(130)로 차례차례로 구해진 추정 제 n 바닥높이 편차(Zfn_estm)(n=1,2,…,최종 잎 노드 번호)는, 가산기(132)에 의해, 각 접지부위(10)의 수정 목표 접지부위 위치 자세에 가산되고, 이것에 의해, 바닥형상 편차 보상된 수정 목표 접지부위 위치 자세가 구해진다. 그리고, 이 바닥형상 편차 보상된 수정 목표 접지부위 위치자세가 수정 목표 접지부위 위치 자세 대신, 상기 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)에 입력된다.

이상 설명한 바닥형상 추정기(130) 및 가산기(132)를 포함하는 제어장치(50)의 처리를 이 제어장치(50)의 제어주기마다 반복 실행함으로써, 추정 제 n 바닥높이 편차(Zfn_estm)가 실제의 제 n 바닥높이 편차에 수렴한다. 또, 각 목표 접지부위 위치 자세는 그 접지부위에 대응하는 추정 제 n 바닥높이 편차(Zfn_estm)에 따라 수정되어, 실제의 제 n 바닥높이 편차가 실 상반력에 미치는 영향을 흡수한다.

또한, 모든 제 n 접지부위(n=1,2,…,최종 잎 노드 번호)가 접지하고 있으면, 제 n 접지부위의 실제의 접지면상에서의 목표 제 n 접지부위 상반력 중심점(Qn)에 상당하는 점은, 실 제 n 바닥 접점(Dn_act)에 일치할 것이다. 따라서, 상기 순간 제 n 노드 상대 바닥높이 편차(Zn_rel')는 변동하지 않을 것이다. 그러나, 실제로 는 로봇(1)의 기계나 제어계의 진동 및 전기적인 노이즈에 의해, 상기 바닥형상 추정기(130)의 상기한 계산 알고리즘에 의해 얻어진 순간 제 n 노드 상대 바닥높이 편차(Zn_rel')는 격렬하게 변동한다. 따라서, 순간 제 n 노드 상대 바닥높이 편차를 그대로 추정 제 n 노드 상대 바닥높이 편차로서 사용하고, 이것을 기초로 추정 제 n 바닥높이 편차(Zfn_estm)를 구하고(즉 제 n 노드(n=1,2,…,최종 잎 노드 번호) 모든 선조 노드의 순간 상대 바닥높이 편차와 순간 제 n 노드 상대 바닥높이 편차의 합을 가지고 추정 제 n 바닥높이 편차(Zfn_estm으로 하고), 또한 도 35에 도시하는 바와 같이, 추정 제 n 바닥높이 편차(Zfn_estm)를 수정 목표 접지부위 위치자세에 가산한 것을 기구변형 보상된 수정 목표 접지부위 위치 자세 산출부(114h)에 입력시키면, 로봇(1)의 접지부위(10)는 발진하거나 또는 격렬하게 진동한다.

그런데, 상기 2개의 자 노드를 갖는 노드용 그룹 내 전체 추정 처리, 3개의 자 노드를 갖는 노드용 그룹내 전체 추정 처리, 3개의 자 노드를 갖는 노드용 그룹 내 부분추정 처리에서는, 순간 제 n 노드 상대 바닥높이 편차(Zn_rel')로부터 추정 제 n 노드 상대 바닥높이 편차(Zn_rel_estm)까지의 전달함수는, 1차 지연의 로 패스 필터로 된다. 즉, 추정 제 n 노드 상대 바닥높이 편차(Zn_rel_estm)는 순간 제 n 노드 상대 바닥높이 편차(Zn_rel')에 로 패스 필터를 통과시킨 것이 된다. 따라서, 추정 제 n 노드 상대 바닥높이 편차(Zn_rel_estm)을 기초로 구한 추정 제 n 바닥높이 편차(Zfn_estm)를 도 35에 도시하는 바와 같이 수정 목표 접지부위 위치 자세에 가산해도, 각 접지부위(10)의 발진이나 진동을 일으키기 어렵게 된다. 이것 에 의해, 각 접지부위(10)의 발진이나 진동을 방지할 수 있다.

덧붙여서, 본 참고예에서는, 상기 로 패스 필터의 시정수는 Testm이다. 또한, 본 참고예에서 설명한 바닥형상 추정기(130)의 처리 이외에도, 순간 제 n 노드 상대 바닥높이 편차(Zn_rel')로부터 추정 제 n 노드 상대 바닥높이 편차(Zn_rel_estm)까지의 전달함수가 로 패스 필터로 되도록 구성하는 것은 가능하다.

이어서, 이 실시형태에서의 바닥형상 추정기(130)의 특징을 설명한다.

로봇(1)의 자세안정화 제어를 위해 필요한 각 노드 보상 모멘트를 발생시키려고, 각 노드 보상각을 변화시켜, 최종적인 목표 접지부위 위치 자세인 기구변형 보상된 수정 목표 접지부위 위치 자세를 변동시켜도, 각 접지부위(10)가 실제로 접지하고 있는 한, 제 n 접지부위(10)의 실제의 접지면상에서의 목표 제 n 접지부위 상반력 중심점(Qn)에 상당하는 점은, 실 제 n 바닥 접점(Dnact)에 일치한 채 움직이지 않는다. 그 대신, 각 다리의 컴플라이언스 기구(42) 등이 변형되거나, 상체(24)의 위치 자세가 변화된다. 즉, 기구변형 보상된 수정 목표 접지부위 위치 자세를 변경시킨 분량을 없애도록, 컴플라이너스 기구(42) 등이 변형되거나, 상체(24)의 위치 자세가 변화된다.

본 참고예에서의 바닥형상 추정기(130)는 기구변형 보상된 수정 목표 접지부위 위치 자세로부터 추정 제 n 바닥높이 편차(Zfn_estm)(n=1,2,…,최종 잎 노드 번호)를 산출할 때까지, 실 상반력을 기초로 기구 컴플라이언스 모델(134)에 의해 산출된 컴플라이언스 기구(42) 등의 변형량과 상체자세 경사 편차(θberr)에 의한 추 정 제 n 바닥높이 편차(Zfn_estm)에의 영향을 없애고 있으므로, 상체자세 경사 편차가 발생하거나, 기구변형 보상된 수정 목표 접지부위 위치 자세가 변동해도, 추정 제 n 바닥높이 편차(Zfn_estm)는 그 영향을 받지 않는다.

이것은, 바닥형상의 추정 처리가 로봇(1)의 컴플라이언스 제어나 자세제어로부터 간섭을 받지 않는 것을 의미한다. 따라서, 추정 제 n 바닥높이 편차(Zfn_estm)를 도 35에 도시하는 바와 같이 수정 목표 접지부위 위치 자세에 가산해도, 로봇(1)의 제어계의 안정여유(발진의 발생곤란)는 거의 저하되지 않는다. 즉, 컴플라이언스 제어, 자세제어, 및 바닥형상의 추정처리와, 바닥형상 편차의 추정값을 사용한 로봇의 목표 보용의 보정 동작을 동시에 행해도, 각 제어?처리가 서로 간섭하여 발진하는 일은 거의 없다. 즉, 간단하게 말하면, 바닥형상 편차의 복수의 패러미터를 동시에 추정하면서, 바닥형상 편차가 상반력에 미치는 영향을 없애기 위한 다리의 보상동작(접지부위의 위치 자세의 수정동작)도 동시에 실행할 수 있다. 게다가, 자세제어를 위한 다리의 보상동작(접지부위의 위치 자세의 수정동작)도 동시에 실행할 수 있다.

또, 각 접지부위(10)의 추정 제 n 바닥높이 편차(Zfn_estm)를 도 35에 도시하는 바와 같이 수정 목표 접지부위 위치 자세에 가산하면, 실 제 n 바닥높이 편차가 존재하고 있어도, 그 영향을 없앨 수 있으므로, 실 전체 상반력은, 정상적으로는 로봇(1)이 상정 바닥을 이동하고 있는 경우와 동일하게 된다. 게다가, 추정 제 n 바닥높이 편차, Zfn_estm의 추정과, 추정 제 n 바닥높이 편차(Zfn_estm)를 사용한 수정 목표 접지부위 위치 자세의 수정은 제어주기마다 차례차례로 실행되므로, 바닥형상이 도중에 변화되어도, 변화 후의 제 n 바닥높이 편차를 추정하여, 그 추정값에 기초하여 바닥형상의 변화의 영향을 없앨 수 있다. 단, 바닥형상 추정기(130)에는 전술한 바와 같이 로 패스 필터가 포함되므로, 각 접지부위(10)가 접지한 직후나 바닥형상이 도중에 변화되었을 때에는, 과도적으로는, 실 전체 상반력은 바닥형상 편차의 영향을 받는다. 그러나, 그 후, 시정수(Testm)로 그 영향이 감쇠된다.

또, 이하의 특징도 있다.

1) 바닥형상 편차가 커도 제어가 파탄되기 어렵다. 이것은, 계층형 컴플라이언스 제어는 미소한 보상각에 한정된 근사를 행하고 있지 않기 때문이다.

2) 바닥형상 편차의 추정 정밀도가 높다.

3) 로봇의 자세안정화 제어와의 간섭에 의한 발진이 발생하기 어려우므로, 추정 처리의 시정수를 짧게 할 수 있다. 그것에 의해, 바닥형상 편차가 상반력에 미치는 영향을 없애기 위한 다리의 보상동작의 응답성을 높일 수 있으므로, 바닥형상 편차가 상반력에 미치는 과도적인 영향이 단시간에 해소된다.

로봇의 이동시의 1보 전의 착지기(어떤 접지부위(10)가 접지상태로 되는 시기)에서의 추정 제 n 바닥높이 편차와, 당해 1보 전의 착지기의 다음 착지기에서의 추정 제 n 바닥높이 편차와의 관계에 상관이 없는 경우에는, 본 참고예에 나타낸 바와 같이 추정 제 n 바닥높이 편차를 강제적으로 서서히 0에 수렴시키는 것이 좋다. 그러나 예를 들면, 상정 바닥에 고려되어 있지 않은 실 바닥면위 굴곡이 충분히 여유가 있다는 것을 알고 있다면, 상기 관계에 어느 정도의 상관이 있다고 생각 된다. 이 경우에는, 현재의 착지기에서의 추정 제 n 바닥높이 편차를, 강제적으로 서서히 1보 전의 착지기에 있어서의 추정 제 n 바닥높이 편차에 1보다 작은 양의 정수를 승산한 값에 수렴시키도록 해도 된다.

또한, 1보 전의 착지기에서의 추정 제 n 바닥높이 편차뿐만 아니라, 복수보 전의 착지기에서의 추정 제 n 바닥높이 편차도 사용하여 현재의 착지기에서의 추정 제 n 바닥높이 편차의 수렴 목표값을 결정해도 된다. 또, 현재나 1보 전이나 복수보 전의 착지기에서의 다른 노드의 추정 제 n 노드 바닥높이 편차 및 추정 제 n 노드 바닥경사 편차도 사용하여 수렴 목표값을 결정해도 된다.

본 참고예(제 3 참고예)는 상기한 바와 같이 구성했으므로, 종래 기술에서는 곤란했던 많은 접지부위를 갖는 로봇에서도, 바닥형상의 추정, 구체적으로는, 각 접지부위에 대응하는 바닥높이 편차를 동시에, 바꾸어 말하면 복합적으로, 정밀도 좋게 추정할 수 있다. 또한, 로봇의 접지부위를 그 자세를 제어 가능하게 설치하고, 바닥형상의 추정 처리에서 바닥높이 편차뿐만 아니라, 바닥경사 편차도 추정하는 경우에는, 바닥경사 편차도 포함하여 동시에, 정밀도 좋게 추정할 수 있다.

게다가, 바닥형상의 추정 결과를 기초로 목표 접지부위 위치 자세 궤도를 수정함으로써, 바닥형상이 상정하고 있던 형상과 상이해도, 그 영향을 흡수하여 바라는 바와 같은 상반력을 발생시킬 수 있다.

특히, 형상 편차의 추정값에 의한 보정이 아닌 계층형 컴플라이언스 제어로 완전히 제거할 수 없었던 실 상반력의 제어 목표값으로부터의 정상 편차를 가능한 한 0에 근접시킬 수 있다. 바꾸어 말하면 바닥형상의 편차에 기인하는 상반력의 정상 편차를 해소할 수 있다.

또, 바닥형상의 추정 정밀도가 저하될 우려가 있는 상황 또는 그 추정값이 발산할 우려가 있는 상황에서는 바닥형상의 추정을 중단하도록 구성했으므로, 상기한 작용효과에 더하여, 추정값이 부적절한 것으로 되는 경우가 없다.

또한, 본 참고예에 따른 로봇의 바닥형상 추정 장치(130)는 상기한 수법으로 바닥형상을 추정하면 충분하고, 추정값에 기초하여 운동을 보정하는 것은 필수적이지 않다.

[제 4 참고예]

다음에 본 발명의 제 4 참고예에 따른 이동 로봇의 제어장치(특히 바닥형상 추정 장치)를 설명한다. 제 4 참고예에서는, 그것을 간단하게 설명하면, 상기 제 3 참고예의 도 40에 도시한 바닥형상 추정기(130)의 블록선도를 등가변환한 것이다. 즉, 직전에 추정한 바닥형상의 추정값(전회 제어주기에서의 추정값 등의 이 추정값의 과거값)과, 최종적으로 수정되어 결정된 목표 보용(상세하게는 컴플라이언스 동작을 포함하는 기구변형 보상된 수정 목표 접지부위 위치 자세)(혹은 실 관절 변위)와, 실 상체자세 경사편차(θberr)를 기초로, 각 노드의 상반력을 추정하고, 그 추정값과 각 노드의 실 상반력의 차를 기초로, 이 차를 0에 근접시키도록 바닥형상의 추정값의 보정량 후보값을 결정하고, 바닥형상의 추정이 발산하지 않는다고 판단되면, 바닥형상의 추정값의 보정량 후보값을 기초로 직전에 추정한 바닥형상의 추정값을 보정하도록 했다. 이 처리는, 본 출원인이 앞서 제안한 일본 특개평10-277969호 공보에서, 2족이동 로봇의 각 다리의 족평의 상반력 중심점 주위 의 상정되는 모멘트와 실 모멘트의 차가 0에 근접하도록 각 족평에 관계되는 바닥경사를 추정하는 수단, 및, 전체 상반력 중심점 주위의 상정되는 모멘트와 실 모멘트의 차가 0에 근접하도록 양 다리 사이의 간섭각을 추정하는 수단을 발전시킨 계층형의 바닥형상 추정수단에 상당하는 것이다.

이와 같이 본 참고예는, 바닥형상 추정기(130)의 처리만이, 제 3 참고예와 상이한 것이고, 바닥형상 추정기(130) 이외의 처리에 대해서는 설명을 생략한다.

도 57은, 본 참고예에서의 바닥형상 추정기(130)의 기능적 수단을 도시하는 블럭도이다. 이 도 57을 참조하여 본 참고예에서의 바닥형상 추정기(130)를 이하에 설명한다.

우선, 각 접지부위(10)의 기구변형 보상된 수정 목표 제 n 접지부위 위치 자세(n=1,2,…,최종 잎 노드 번호)와, 각 접지부위(10)의 목표 접지부위 위치로부터 본 이 접지부위(10)의 목표 제 n 접지부위 상반력 중심점과, 상기 자세 경사 편차(θberr)를 기초로, 기구변형 보상된 수정 목표 제 n 접지부위 위치 자세를, 목표 전체 상반력 중심점을 회전중심으로 하여 θberr 만큼 회전이동시켜서 이루어지는, 회전 후 기구변형 보상된 수정 목표 제 n 접지부위 위치 자세(n=1,2,…,최종 잎 노드 번호)를 구한다. 구해진 회전후 기구변형 보상된 수정 목표 제 n 접지부위 위치 자세(n=1,2,…,최종 잎 노드 번호)로부터, 목표 보용상에서의 상정 제 n 바닥 접지위치(Dn)(n=1,2,…,최종 잎 노드 번호)를 감산하고, 제 n 접지부위 간섭 높이(Zn_int)를 구한다. 그것을 바닥형상 추정기(130)에 구비한 기구 컴플라이언스 모델(역 모델)에 입력함으로써, 각 접지부위(10)의 상반력의 추정값인 추정 제 n 접지부위 상반력을 구한다.

또한, 여기에서의 기구 컴플라이언스 모델은 각 접지부위 간섭 높이의 상호 상대관계에 따라 추정 접지부위 상반력의 상호의 상대관계를 구하는 것이며, 절대값에 의미를 갖지 않는다. 또, 제 n 접지부위의 실 상반력(Fn_act)의 검출값이 어떤 임계값(Fn_min3)보다 작을 경우에는, 상기 기구 컴플라이언스 모델에서도 제 n 접지부위가 접지하고 있지 않은 것도 상정한다.

또한, 제 n 접지부위(n=1,2,…,최종 잎 노드 번호)의 실 상반력(Fn_act)의 검출값으로부터, 상기 추정 제 n 접지부위 상반력을 감산하고, 제 n 접지부위 상반력 추정 오차(Ffn_estm_err)는 힘으로 표현되어 있기 때문에, 환산값(Cn)(예를 들면, 스프링 정수와 같은 것)에 의해 높이 편차로 환산하여, 이것을 제 n 접지부위 바닥높이 편차 보정량 후보값(Zfn_inc_cand)으로서 얻는다. 또한, 환산값(Cn)은 대각 행렬이라고는 할 수 없다.

이어서, 구해진 제 n 접지부위 바닥높이 편차 보정량 후보값(Zfn_inc_cand)으로부터, 상기한 「계층 상대화 처리」에 의해, 계층 상대화함으로써, 제 n 노드 상대높이 편차 보정량 후보값(Zn_inc_cand)(n=1,2,…,최종 노드 번호)을 구한다. 다음에, 바닥높이 편차의 추정 처리의 서브루틴 처리에 의해, 제 n 노드 상대 바닥높이 편차 보정량 후보값(Zn_inc_cand)으로부터, 추정 바닥높이 편차(추정 제 n 바닥높이 편차)(Zfn_estm)(n=1,2,…,최종 잎 노드 번호)를 구한다. 여기에서의 바닥높이 편차의 추정 처리의 서브루틴 처리는 도 41의 S56~S72까지의 처리와 동일하다.

또한, 일본 특개평10-277969호 공보에 개시한 바와 같이, 접지부위로서 자세를 제어할 수 있는 족평을 가지고, 접지부위에서의 바닥경사 편차를 추정할 수 있는 로봇의 경우에는, 도 41의 S70에 관해 설명한 바와 같이, 접지부위 바닥경사 편차 추정처리를 행하는 것이 바람직하다.

여기에서, 제 3 참고예와 제 4 참고예(본 참고예)를 비교한다. 제 4 참고예는, 간단히 말하면, 제 3 참고예의 바닥형상 추정기, 130의 블록선도를 등가변환한 것으로 되어 있다. 따라서, 제 4 참고예의 작용효과는 제 3 참고예의 작용효과와 동일하다. 또, 제 3 참고예와 동일하게, 바닥형상의 추정값에 기초하여 운동을 보정하는 것은 필수적이지는 않다.

또한, 제 3 및 제 4 참고예에서, 상반력 센서로서 분포압 센서를 사용해도 된다. 6축력센서(34) 등의 힘 센서보다도 분포압 센서 쪽이 실 상반력에 의한 접지부위의 변형을 보다 치밀하게 추정할 수 있으므로, 바닥형상 추정의 정밀도를 향상시킬 수 있다.

게다가, 본 출원인에 의한 일본 특허 제3035051호 공보의 도 1에 도시하는 바와 같이, 접지부위에 접촉센서를 복수개 배치(예를 들면 접지부의의 네 구석에 배치) 하고, 그 출력신호에 기초하여 접지부위가 어느 방향으로 뜨기 쉬운지를 판정해도 된다. 그리고, 접지하고 있지 않은 접촉 센서가 점점 더 뜨는 방향에서의 바닥 형상의 추정을 중단하거나, 또는, 모든 접촉 센서가 떠 있을 때에도 바닥형상의 추정을 중단하도록 해도 된다.

[제 5 참고예]

상기 제 1~제 4 참고예에서는, 자 노드를 갖는 노드의 이 자 노드의 총수가 3개 이하의 경우를 예로 들어서 설명했지만, 자 노드의 수가 4개 이상인 경우에도, 가중평균의 개념을 도입함으로써, 이하에 나타내는 바와 같이 모멘트 및 경사각의 개념을 확장할 수 있다.

목표값, 실제값, 편차의 어느 것에 관해서도 개념을 확장할 수 있으므로, 여기에서는, 목표값, 실제값, 편차의 구별을 하지 않고, 일반적인 접지부위 상반력의 세트(Fn)(n=1,2,...)에 대한 모멘트를 이하와 같이 확장하여 정의한다.

제 n 접지부위의 상반력의 세트(Fn)(n=1,2,...,최종 잎 노드 번호)를 계층 상대화하여, 제 n 노드 상대 상반력(Fn_rel(n=1,2,...,최종 노드 번호)을 구한다.

제 n 노드의 자 노드의 수를 r로 한다.

제 n 노드의 제 j 번째의 자 노드의 식별 번호가 aj(j=1,2,…,r)라고 한다. 제 n 노드의 제 j 번째의 자 노드의 가중값이 Wj(j=1,2,…,r)이라고 한다.

제 n 노드의 제 j 번째의 자 노드의 가중값을 제 j 요소로서 갖는 열 벡터를 UWn으로 한다.

즉, 하기 식 38과 같이 UWn을 정의한다. 여기에서 「T」는 전치(轉置)를 의미하고, 행 벡터의 전치에 의해 열 벡터를 표현하기로 한다.

UWn=(Wa1,Wa2,…,War)^T …식 38

벡터(UWn)에 직교하는(즉 벡터(UWn)와의 내적이 0이 되는) r-1개의 서로 독립한 벡터를 R(1),R(2),…,R(r-1)로 한다. R(j)(j=1,2,…,r-1)는 r행 1열의 열 벡 터이다. 또한, R(j)(j=1,2,…,r-1)는 연산의 용이함과 제어정밀도를 고려하면, 서로 직교하는 것이 바람직하다. R(j)를 제 j 열(j=1,2,…,r-1)로 하는 행렬을 Hn으로 한다. Hn 은 r행 r-1열의 행렬이다.

제 n 노드의 제 j 번째의 자 노드의 상대 상반력(Faj_rel)을 제 j 요소(j=1,2,…,r)로 하는 열 벡터를 제 n 그룹 상대 상반력 벡터(Fn_rel_c)로 한다.

즉, 다음 식 39에 의해, Fn_rel_c를 정의한다.

Fn_rel_c=(Fa1_rel,Fa2_rel,…,Far_rel)^T …식 39

Fn_rel_c는 r행 1열의 열 벡터이다.

다음 식 40을 만족하는 열 벡터(Mn_exp)를 제 n 노드 확장 상반력 모멘트라고 부른다.

Fn_rel_c=Hn*Mn_exp …식 40

Mn_exp는 r-1행 1열의 열 벡터이다.

이상과 같이, 접지부위 상반력의 세트(Fn)(n=1,2,…)에 대하여, 제 n 노드 확장 상반력 모멘트(Mn_exp)를 정의한다.

즉, 접지부위 상반력(Fn)(n=1,2,…)을 기초로, 계층화한 노드 상대 상반력(Fn_rel)(n=1,2,...)을 구하고, 또한 제 n 노드의 모든 자 노드(aj)(j=a1,a2,…,ar)의 상대 상반력을 요소로 하는 벡터(Fa1_rel,Fa2_rel,…,Far_rel)^T를, 제 n 노드의 모든 자 노드의 가중값을 요소로 하는 벡터(Wa1,Wa2,…,War)^T에 직교하는 어떤 소정의 서로 독립한 벡터(R(j))(j=1,2,…,r-1)의 선형결합으로 나타냈을 때의 선형결합의 계수를 요소로 하는 벡터를 제 n 노드 확장 상반력 모멘트(Mn_exp)라고 부른다.

또한, 제 n 노드 확장 상반력 모멘트(Mn_exp)의 제 j 번째 요소(제 j 행째 요소)를 제 n 노드 확장 상반력 모멘트 제 j 성분이라고 부른다.

또, 실 접지부위 상반력의 세트(Fn)(n=1,2,…)에 대한 제 n 노드 확장 상반력 모멘트를 실 제 n 노드 확장 상반력 모멘트(Mn_exp_act)라고 부른다.

또, 목표 접지부위 상반력의 세트(Fn)(n=1,2,…)에 대한 제 n 노드 확장 상반력 모멘트를 목표 제 n 노드 확장 상반력 모멘트(Mn_exp_ref)라고 부른다. 목표 제 n 노드 확장 상반력 모멘트는 통상 0벡터로 설정된다.

또, 접지부위 상반력 편차의 세트(Fn)(n=1,2,…)에 대한 제 n 노드 확장 상반력 모멘트를 제 n 노드 확장 상반력 모멘트 편차(Mn_exp_err)라고 부른다.

마찬가지로, 일반적인 접지부위 높이의 세트(Zn)(n=1,2,...)에 대한 경사각을 이하와 같이 확장하여 정의한다.

접지부위 높이의 세트(Zn)(n=1,2,…,최종 잎 노드 번호)를 계층 상대화하고, 제 n 노드 상대높이(Zn_rel)(n=1,2,…,최종 노드 번호)를 구한다.

제 n 노드의 제 j 번째의 자 노드의 상대높이(Zaj_rel)를 제 j 요소(j=1,2,…,r)로 하는 열 벡터를 제 n 노드 상대높이 벡터(Zn_rel_c)로 한다.

즉, 다음 식 41에 의해 Zn_rel_c를 정의한다.

Zn_rel_c=(Za1_rel,Za2_rel,…,Zar_rel)^T …식 41

다음 식 42를 만족하는 열 벡터(θn_exp)를 제 n 노드 확장 상반력 경사각이라고 부른다.

Zn_rel_c=Hn*θn_exp …식 42

θn_exp는 r-1행 1열의 열 벡터이다.

즉, 접지부위 높이(Zn)(n=1,2,…)를 기초로, 계층화 한 노드 상대높이(Zn_rel)(n=1,2,…)를 구하고, 또한 제 n 노드의 모든 자 노드(aj)(j=1,2,…,r)의 상대높이를 요소로 하는 벡터(Za1_rel,Za2_rel,…,Zar_rel)^T를 제 n 노드의 모든 자 노드의 가중값을 요소로 하는 벡터(Wa1,Wa2,…,War)^T에 직교하는 상기 소정의 상호 독립한 벡터 R(j)(j=1,2,…,r-1)의 선형결합으로 나타냈을 때의 선형결합의 계수를 요소로 하는 벡터를 제 n 노드 확장 경사각(θn)이라고 부른다.

상기한 바와 같이 정의된 확장 상반력 모멘트와 확장 경사각을 사용함으로써, 제 a 노드의 자 노드의 수가 4개 이상인 경우에도, 컴플라이언스 제어 및 바닥형상 추정의 처리는, 거의 그대로 확장된다. 이하에 그 예를 제 5 참고예로서 설명한다. 또한, 제 5 참고예는 로봇의 제어장치의 기능적 구성은, 상기 도 2에 도시한 것과 동일하게 한다. 또, 이 경우, 계층형 컴플라이언스 동작 결정부는 보상 전체 상반력 모멘트 분배기, 보상각 결정부, 및 바닥형상 추정기 이외의 기능적 구성은 상기 도 35를 참조하여 설명한 것과 동일해도 된다.

[보상 전체 상반력 모멘트 분배기의 처리의 확장에 대해]

예를 들면, 계층형 컴플라이언스 동작 결정부 중의 보상 전체 상반력 모멘트 분배기의 처리는 다음과 같이 확장된다.

로봇의 자세를 복원시키기 위해서 목표 제 n 노드 확장 상반력 모멘트(Mn_exp_rel)(통상 0)에 부가적으로 발생해야 할 확장 상반력 모멘트를 제 n 노드 보상 확장 상반력 모멘트(Mn_exp_dmd)라고 부른다.

제 n 노드 보상 확장 상반력 모멘트(Mn_exp_dmd)와 제 n 노드 보상 상반력 모멘트(Mn_dmd)의 사이에는, 다음 식 43의 관계가 있다. 단, Cn_mech는 각 목표 노드 상반력 중심점과 로봇의 기구의 컴플라이언스 특성에 의해 결정되는 소정의 계수 행렬(2행 r-1열의 행렬)이다.

Mn_dmd=Cn_mech*Mn_exp_dmd …식 43

한편, 상기 식 10을 일반화하면 다음 식 44가 된다.

Mdmd=ΣMn_dmd …식 44

또한, Σ는 모든 제 n 노드에 관한 총 합을 의미한다.

따라서, 보상 전체 상반력 모멘트 분배기에서는, 식 43과 식 44를 만족하도록, 제 n 노드 보상 확장 상반력 모멘트(Mn_exp_dmd)(n=1,2,…) 및 제 n 노드 보상 상반력 모멘트(Mn_dmd)를 결정하면 된다.

[보상각 결정부(θn 결정부)의 처리의 확장에 대해]

제 5 참고예에서의 각 제 n 노드의 보상각 결정부의 처리는, 예를 들면 제 3 참고예의 θ145 결정부(도 25 및 도 33을 참조)의 처리에서, M145act를 구하기 까지의 처리를 각 접지부위의 실 상반력을 기초로 상기의 실 제 n 노드 확장 상반력 모멘트(Mn_exp_act)를 구하는 처리로 치환하고, 145를 n으로 치환하고, 도 33중의 보상 필터, 로 패스 필터 및 게인(Kn)의 차원을 제 n 노드의 자 노드수로부터 1을 감산한 수로 확장하면 된다. 상기의 치환을 행한 처리에 의해, 제 n 노드의 보상각(θn)을 구할 수 있다.

[확장 상반력 모멘트를 사용한 바닥형상 추정에 대해]

아래와 같이, 바닥형상 추정기의 처리의 일부에 확장 상반력 모멘트의 개념을 사용한 수법을 포함시켜도 좋다.

즉, 상기 제 4 참고예에서 설명한 바닥형상 추정기의 도 56에 도시한 처리 대신에, 도 57의 블럭도로 나타내는 처리를 사용하여, 바닥형상 편차를 추정한다.

도 57을 참조하여, 이 처리를 설명하면 우선, 상기 도 56에서 설명한 바와 같이 산출되는 추정 제 n 접지부위 상반력과, 제 n 접지부위의 실 상반력의 차(각 노드마다의 차의 세트)로부터, 확장 모멘트의 정의에 따라서, 제 n 접지부위 상반력 추정 오차(Ffn_estm_err)(n=1,2,...)에 대응하는 제 n 노드 확장 상반력 모멘트 추정 오차(Mn_exp_estm_err)를 구한다. 또한, 제 n 노드 확장 상반력 모멘트 추정 오차를 결정하는 점에서 필요하게 되는 상기 벡터(R(j))는 각 노드의 가중값을 기초로 결정된다.

이어서, 다음 식 45와 같이, 소정의 계수 매트릭스(Kn_cmpl)를 제 n 노드 확장 상반력 모멘트 추정 오차(Mn_exp_estm_err)에 승산함으로써, 제 n 노드 확장 경사각의 보정량의 후보값인 제 n 노드 확장 경사각 보정량 후보값(θn_inc_cand)을 구한다.

θn_inc_cand=Kn_cmpl*Mn_exp_estm_err …식 45

또한, Kn_cmpl은 대각 행렬이라고는 할 수 없다.

이어서, 벡터(R(j))(j=1,2,…r-1)에 대한 계수를 θn_inc_cand의 제 j 요소로서, 벡터(R(j))를 선형결합한 벡터를 구하고, 이것을 가지고, 제 n 노드 상대 바닥높이 편차의 보정량의 후보값인 제 n 노드 상대 바닥높이 편차 보정량 후보값(Zn_inc_cand)으로 한다.

즉, 다음 식 46에 의해, Zn_inc_cand를 구한다.

Zn_inc_cand=Hn*θn_inc_cand …식 46

이상과 같이, 제 n 노드 상반력 추정 오차(Fn_estm_err)로부터 제 n 노드 상대 바닥높이 편차 보정량 후보값(Zn_inc_cand)을 구할 때까지의 처리를 도 56의 것으로부터 도 57의 것으로 치환하면 된다.

상기 이외의 도 57의 연산처리는 도 56의 연산처리와 동일하다.

또, 제 5 참고예에서의 바닥높이 편차의 추정 처리의 서브루틴 처리에서는, 제 n 노드의 자 노드 수가 4 이상인 경우의 처리가 필요하게 된다. 이 처리는, 상기 3자 노드용 바닥형상 추정 처리(도 41을 참조)를 확장하고, 이 3자 노드용 바닥형상 추정처리와 동일하게, 어느 하나의 자 노드(j)에 대응하는 접지부위가 떠서 발산해 버릴 것 같은 경우에는, 추정 제 j 노드 상대 바닥높이 편차(Zj_rel_estm)를 전회값(Zj_rel_estm_p)에 유지한다(제 j 노드 상대 바닥높이 편차 보정량 후보값(Zj_inc_cand)을 0으로 함). 또, 그 이외의 자 노드의 추정 노드 상대 바닥높이 편차(Zk_rel_estm)를 전회 추정 제 k 노드 상대 바닥높이 편차(Zk_rel_estm_p)와 Zn_inc_cand의 합에 어떤 소정의 바이어스 값(c)을 더한 값에 근접하도록 결정한 다. 단, 바이어스 값(c)은 제 n 노드의 모든 자 노드의 추정 노드 상대 바닥높이 편차의 가중평균이 0이 되도록 결정된다.

이상과 같이, 바닥형상의 추정 처리를 4개 이상의 자 노드를 갖는 노드가 있을 경우에도, 확장할 수 있다. 단, 이러한 처리를 행하는 것 보다는, 4개 이상의 자 노드를 갖지 않도록, 계층구조를 결정하는 쪽이, 연산처리는 용이하게 된다.

이상 설명한 제 1~제 5 참고예를 기초로 하여, 이하에 본 발명의 실시형태를 설명한다.

[제 1 실시형태]

다음에, 본 발명의 제 1 실시형태에 따른 이동 로봇의 제어장치를 도 58~도 65를 참조하여 설명한다. 도 58 및 도 59는, 제 1 실시형태에 따른 다리식 이동 로봇(51)(본 참고예에서는 2족 이동 로봇이라고 함)이 후술하는 무릎관절(56)의 부위를 바닥에 붙인 상황(무릎을 꿇은 상황)을 측면시로 도시하고 있다. 또한, 도 58에서는 로봇(51)을 선 형상의 링크의 연결체로서 모식화 하여 도시하고 있다.

본 실시형태에 따른 발명 목적은, 무릎, 팔꿈치, 동체 등, 로봇의 다리나 팔의 선단부 이외의 부분이 바닥 또는 밑바닥에 고정(설치)된 대상물(바닥의 연장물이라고 생각되는 대상물)과 접촉하는 상태에서, 이 대상물을 포함하는 바닥으로부터 받는 반력(외력)을 조작하여, 로봇의 자세를 안정하게 제어하는데 있다.

보다 구체적으로는, 로봇이 무릎을 꿇은 상태나 의자에 앉은 상태에서의 자세제어를 가능하게 하는 것에 있다.

종래의 공지의 인간형 로봇에는, 예를 들면 무릎에는, 상반력을 검출하는 센 서가 구비되어 있지 않고, 무릎에 작용하는 상반력을 제어할 수는 없었다. 예를 들면 종래의 인간형 로봇에서는, 도 58 및 도 59와 같이 로봇을 무릎을 꿇게 한 상태에서는, 좌우의 무릎에 작용하는 상반력은 바닥의 요철에 의존(지배)되고, 또, 바닥의 요철에 따라, 다리의 관절각을 조절하는 것이 행해지고 있지 않았다. 이 때문에, 바닥의 요철에 의해, 로봇의 바닥과의 접지부위가 흔들리는 등, 그 접지부위의 접지상태가 불안정학 되는 경우도 있었다. 그리고, 이러한 상태에서는, 로봇의 동작이나 작업을 행하려고 하면 로봇의 자세가 불안정하게 되는 경우가 있었다.

또, 예를 들면 로봇이 무릎을 꿇은 상태로부터 일어서는 경우에, 실제의 바닥의 경사가 목표 보용에서 상정하고 있던 경사와 상이한 경우에는, 로봇의 중심위치가 적절하게 제어되지 않아, 로봇의 상체의 자세가 기운채 일어서려고 해버리기 때문에 이 로봇이 전도될 우려가 있었다.

또, 로봇이 의자에 앉은 채, 그 자세를 안정하게 유지하면서 동작이나 작업을 행하는 경우에 있어서, 의자로부터 일어설 때, 또는 의자에 앉을 때에, 로봇의 자세를 적절한 자세로 복원할 수 있게 하기 위해서, 로봇의 각 다리의 족평에 작용하는 상반력뿐만 아니라, 의자로부터 로봇의 둔부에 작용하는 반력도 적절하게 제어할 수 있는 것이 바람직하다.

또한, 개념적으로는, 의자 등과 같이 밑바닥에 고정설치된 대상물도 바닥의 일부라고 생각하면, 상기 반력도 광의의 상반력이라고 할 수 있다.

제 1 실시형태는, 이상과 같이 종래 방식의 문제점을 감안하여, 상기 문제점을 해소하고, 이동 로봇의 무릎, 팔꿈치, 동체, 둔부 등, 다리나 팔의 선단부 이외 의 부분이, 바닥 또는 바닥의 연장물이라고 생각되는 대상물과 접촉하여 반력을 받는 상태에서, 상기한 계층형 컴플라이언스의 제어를 사용하면서, 로봇의 다리나 팔의 선단부에 작용하는 반력뿐만 아니라, 다리나 팔의 선단부 이외의 부분에 작용하는 반력도 적절하게 제어하여, 이동 로봇의 자세를 안정하게 유지하는 것을 가능하게 한 것이다.

이하에, 도 58 및 도 59에 도시하는 로봇(2족이동 로봇)(51)을 예로 들어, 제 1 실시형태를 보다 구체적으로 설명한다.

우선, 로봇(51)의 기구적 구성을 설명하면, 이 로봇(51)에서는 상체(기체)(53)의 하단부로부터 2개의 다리(링크 기구)(52)(한쪽 다리는 도시를 생략하고 있음)가 연장하여 설치되어 있다. 또, 상체(53)의 상부로부터 2개의 팔(링크기구)(54)(한쪽 팔은 도시를 생략하고 있음)이 연장하여 설치되어 있다. 각 다리(52)는 상체(53)측의 기단부(상체(53)와의 연결 개소)와, 중간부와, 선단부에 각각 전동 모터 등의 액추에이터에 의해 동작하는 고관절(55), 무릎관절(56), 발목관절(57)을 구비하고, 발목관절(57)을 통하여 각 다리(52)의 선단부에 족평(58)이 연결되어 있다. 마찬가지로, 각 팔(54)은 상체(53)측의 기단부(상체(53)와의 연결 개소)와, 중간부와, 선단부에 각각 전동 모터 등의 액추에이터에 의해 동작하는 견관절(59), 팔꿈치관절(60), 손목관절(61)을 구비하고, 손목관절(61)을 통하여 손(62)이 각 팔(54)의 선단부에 연결되어 있다. 이 예에서는, 각 다리(2)의 각 관절(55, 56, 57)은 각각, 예를 들면 3자유도, 1자유도, 2자유도를 갖는 관절로 되어 있고, 족평(58)은 상체(53)에 대해 6자유도를 갖는다. 또, 각 팔(54)의 견관 절(59), 팔꿈치관절(60), 손목관절(61)은 각각의 자유도가 상체(53)에 대해 손(62)이 6자유도 이상의 자유도를 갖도록 구성되어 있다.

또한, 로봇(51)의 상체(53)에는, 도시를 생략하지만, 상기 제 1 참고예 등과 동일한 제어장치(50)가 탑재되어 있다. 또, 상체(53)의 상단에는 머리부(63)도 설치되어 있다.

이 로봇(51)에서는, 도 58 및 도 59와 같이 로봇(51)을 무릎을 꿇게 한 상태(특정자세 상태)에서는, 각 다리(52)의 족평(58)과 각 무릎관절(56)의 부위(보다 상세하게는, 무릎관절(56)의 근방에서 무릎관절(56)과 발목관절(57)을 연결하는 링크(정강이 링크)의 표면부. 이하, 단지 무릎이라고 함)와, 각 팔(54)의 손(62)이 접지부위가 된다. 그리고, 본 실시형태에서는, 도 59에 도시하는 바와 같이, 접지부위인 무릎에 외력 검출장치인 상반력 센서(90)(하중 센서)를 구비하고 있다. 이 상반력 센서(90)의 본체(센서부)(92)와 스폰지 등의 유연한 부재(탄성체)(94)로 구성되고, 본체(92)를 무릎(다리의 링크)에 고정하고, 그 본체(92)의 외측을 유연한 부재(탄성체)(94)에 의해 덮고 있다. 또한, 무릎에서의 컴플라이언스 제어의 정밀도를 좋게 하기 위해서, 무릎을 유연한 부재(94)에 의해 덮을 뿐만 아니라, 그 유연한 부재(94)의 표면(접지면)을 둥글게 볼록면 형상으로 형성하는 것이 바람직하다. 이렇게 함으로써, 로봇(51)의 목표 운동의 수정동작과 상반력의 관계의 비선형성이 작아지므로, 컴플라이언스 제어의 제어특성이 좋아진다. 또한, 도시는 생략하지만, 족평(58)과 발목관절(57)은 6축력센서 등의 상반력 센서와 컴플라이언스 기구를 통하여 연결되어 있다. 동일하게 손(62)과 손목관절(61)은 도시를 생략하 는 6축력센서 등의 상반력 센서와 컴플라이언스 기구를 통하여 연결되어 있다. 그것들의 연결 구성은 공지의 것이어도 좋다.

무릎관절의 접지부위의 상반력 센서(90)로서는, 이하의 어떤 것이어도 좋다.

1) 접지부위의 바닥에의 접촉면(접지면)에 수직한 방향의 병진력만을 검출하는 센서

2) 접지부위의 바닥에의 접촉면(접지면)에 수직한 방향의 병진력뿐만 아니라 이 접촉면에 수직한 방향 이외의 방향의 병진력도 검출하는 센서

3) 분포압 센서

4) 병진력 이외에 모멘트도 검출하는 센서(예를 들면 6축력센서)

또, 하중을 직접적으로 검출하는 것이 아니어도, 상기 유연한 부재(94)의 변형(휨)을 검출하는 변위 센서를 사용해도 된다.

본 실시형태에서는 상반력 센서(90)로서, 상기 2)의 센서를 사용하는 것으로 한다. 컴플라이언스 제어의 정밀도를 보다 더한층 높이기 위해서는, 상반력 센서(90)로서, 상기 3) 또는 4)의 센서를 사용하면 된다. 또한, 무릎의 구조로서는, 본출원인이 앞서 제안한 일본 특개2000-62760호 공보에 개시하는 바와 같이, 스프링을 구비한 보호 패드(무릎의 보호를 위한 패드)에 상반력 센서를 추가한 것으로 해도 된다.

또, 도시는 생략하지만 각 다리(52)와 각 팔(54)이 선단부에는, 6축력 센서 등의 외력 검출수단이 설치되어 있다.

본 실시형태에서는, 상기한 바와 같이 무릎을 꿇은 로봇(51)에 대하여, 도 61에 도시하는 바와 같이 계층구조를 설정하고 있다. 즉, 접지부위로서의 우측 족평(58), 좌측 족평(58), 우측 무릎, 좌측 무릎, 우측 손(62), 좌측 손(62)을 각각 잎 노드인 제 1 노드, 제 2 노드, 제 3 노드, 제 4 노드, 제 5 노드, 제 6 노드에 대응시킨다. 또, 제 1 노드와 제 2 노드를 자 노드로서 갖는 제 12 노드, 제 3 노드와 제 4 노드를 자 노드로서 갖는 제 34 노드, 제 5 노드와 제 6 노드를 자 노드로서 갖는 제 56 노드, 및 제 12 노드와 제 34 노드를 자 노드로서 갖는 제 1234 노드를 중간 노드로서 설정했다. 또한, 제 1234 노드와 제 56 노드를 자 노드로서 갖는 제 123456 노드를 뿌리 노드로서 설정했다. 또, 각 노드에는, 상기 제 1 참고예 등과 동일한 방침에 의해, 도시한 바와 같이, 목표 상반력 중심점(Qn)(n=1,2,3,4,5,6,12,34,56,1234,123456)이 대응시켜진다. 또한, 뿌리 노드인 제 123456 노드의 목표 상반력 중심점(Q123456)은 목표 전체 상반력 중심점(P)과 동일하다.

본 실시형태에서는, 제어장치(50)는 상기 도 2에 도시한 것과 동일한 기능적 구성을 갖는 것으로 되어 있다.

이 경우, 본 실시형태에서의 보용생성장치(100)가 출력하는 목표 보용중 목표 운동의 목표 접지부위 궤도는, 각 손(62)의 목표 위치 자세 궤도와 각 족평(58)의 목표 위치 자세 궤도와 각 무릎의 목표 위치궤도로 구성된다. 이 경우, 보용생성장치(100)는, 상정한 바닥면상에서 족평(58)과 손(62)과 무릎이 보용에 대한 요구와 같이 접지하도록, 목표 족평위치 자세(목표 제 1, 2 접지부위 위치 자세)와 목표 손위치 자세(목표 제 5, 6 접지부위 위치 자세)와 목표 무릎위치(목표 제 3, 4 접지부위 위치)를 우선적으로 결정하고, 이어서 목표 ZMP(목표 전체 상반력 중심점)을 각 접지부위의 목표 접지점(또는 목표 접지선 혹은 목표 접지면)을 포함하는 최소 볼록 다각형인 지지 다각형 내에 결정하고, 이어서, 목표 상체위치 자세를 로봇(51)의 동역학 모델을 사용하여, 목표 족평위치 자세와 목표 손위치 자세와 목표 무릎위치와 목표 ZMP를 만족하도록 결정한다.

또한, 머리부(63)가 상체(53)에 대해 가동인 경우에는, 그 머리부(63)의 목표 위치 자세 궤도가 목표 운동에 포함된다.

또, 도 2의 로봇 기하학 모델(역키네마틱스 연산부)(110)의 연산처리는 본 실시형태에서는, 후술하는 바와 같이, 제 1~제 3 참고예와 상이하다.

또, 본 실시형태에서는 계층형 컴플라이언스 동작 결정부(114)는 그 일부의 처리 수법이 상기 제 1~제 3 참고예에서 설명한 것과 상이하다. 도 62는 본 실시형태의 계층형 컴플라이언스 동작 결정부(114)의 기능적 구성을 도시하고 있다. 이 기능적 구성의 처리에서, 제 1~제 3 참고예와 상이한 주요점을 설명하면, 보상 전체 상반력 모멘트 분배기(114a)는, 각 중간 노드 및 뿌리 노드의 노드 보상 상반력 모멘트뿐만 아니라, 잎 노드중 제 n 노드(n=1,2,5,6)의 노드 보상 상반력 모멘트(Mn_dmd), 즉, 목표 제 n 접지부위 상반력 중심점(n=1,2,5,6) 주위의 요구 상반력 모멘트(자세복원용의 요구 모멘트)도 결정하여 출력한다. 이것은, 로봇(51)에서는, 접지부위로서의 족평(58), 및 손(62)에 그 목표 상반력 중심점 주위에 모멘트가 발생할 수 있기 때문이다. 또한, 이들 요구 모멘트를 포함한 노드 보상 상반력 모멘트(Mn_dmd)의 결정수법은, 상기 제 1 및 제 2 참고예에서 설명한 수법과 동 일해도 된다. 또, 본 실시형태에서는, 보상 전체 상반력 모멘트 분배기(114a)는, 노드 보상 상반력 모멘트를 각각에 대응하는 각 노드의 목표 상반력 중심점 주위에 발생시킴으로써 수정되는, 목표 제 n 접지부위 상반력 중심점(n=1,2,5,6) 주위의 수정 목표 제 n 접지부위 상반력 모멘트(Mn_refmdfd)(n=1,2,5,6)를, 수정 목표 접지부위 상반력의 병진력 성분(Fn_refmdfd)(n=1,2,3,4,5,6)과 함께 결정하여 출력한다. 이것들은, 기구변형 보상량 산출부(114n)에서 기구변형 보상량을 산출하기 위해서 사용된다.

또, 보상각 결정부(도 62의 가상선으로 둘러싼 부분)는, 본 실시형태에서는, 각 중간 노드 및 뿌리 노드의 보상각뿐만 아니라, 잎 노드중 제 n 노드(n=1,2,5,6)의 노드 보상각(θn)도 결정하여 출력한다. 이들 제 n 노드 보상각(θn)(n=1,2,5,6)을 결정하는 θn 결정부는, 일본 특개평10-277969호 공보에 개시한 족평 보상각 결정부와 동일한 처리에 의해, 제 n 노드 보상각(θn)(n=1,2,5,6)을 결정한다. 또한, 이들 노드 이외의 각 중간 노드 및 뿌리 노드의 보상각(θn)(n=12,34,56,1234,123456)의 결정수법은 상기 제 1 및 제 2 참고예에서 설명한 수법과 동일해도 된다.

또, 본 실시형태에서의 수정 목표 접지부위 위치 자세 산출부(114g)는, 상기 제 1 및 제 2 참고예에서 설명한 수법에 의해, 각 제 n 노드 보상각(θn)(n=12,34,56,1234,123456)을 기초로 한 노드의 회전조작을 합성함으로써, 목표 접지부위 위치를 수정하고, 또한, 제 n 노드 보상각(θn)(n=1,2,5,6)을 기초로, 제 n 접지부위(n=1,2,5,6)의 목표 자세를 수정하고, 이것에 의해, 각 접지부위의 수정 목표 접지부위 위치 자세를 구한다.

또, 본 실시형태에서는, 바닥형상 추정기(130)는, 상기 제 3 참고예에서 설명한 수법과 동일한 수법으로 추정 제 n 바닥높이 편차(Zfn_estm)(n=1,2,3,4,5,6)를 구하는 것과 더불어, 제 n 접지부위의 접지부위 바닥경사 편차(θfn)(n=1,2,5,6)를 추정한다. 이 접지부위 바닥경사 편차(θfn)의 추정은, 일본 특개평10-277969호 공보에 개시한 바닥형상 추정기의 족평 바닥경사를 추정하는 처리와 동일한 처리에 의해 행해진다.

본 실시형태의 계층형 컴플라이언스 동작 결정부(114)의 상기한 것 이외의 기능적 구성의 처리수법은 제 3 참고예의 것과 동일하다.

이상 설명한 차이점을 제외하면, 본 실시형태에서의 제어장치(50)의 연산처리는, 제 1~제 3 참고예와 동일하다.

다음에, 본 실시형태에서의 상기 로봇 기하학 모델(110)의 처리에 관하여 상세히 설명한다.

본 실시형태의 로봇(51)에서는, 상체(53)로부터 무릎에 이르기까지의 관절의 자유도의 수가 3밖에 없으므로(고관절(55)에 3자유도만 있을 뿐이므로), 엄밀하게는 상체위치 자세와 족평위치 자세와 좌우 무릎의 수평위치와 좌우 무릎의 높이의 합을 그것들의 목표에 유지한 채, 좌우 무릎의 높이의 차만을 변경할 수는 없다. 바꿔 말하면, 억지로 좌우 무릎의 높이의 차만을 변경하려고 하면, 접지부위(즉 무릎 및 족평)와 바닥 사이에서 뒤틀림 또는 미끄러짐이 발생하고 만다.

마찬가지로, 엄밀하게 상체위치 자세와 양 무릎의 위치와 양 족평의 수평위 치와 좌우 족평의 높이의 합을 그것들의 목표에 유지한 채, 좌우 족평의 높이의 차만을 변경할 수는 없다. 바꿔 말하면, 억지로 좌우 족평의 높이의 차만을 변경하려고 하면, 접지부위(즉 무릎 및 족평)와 바닥 사이에서 뒤틀림 또는 미끄러짐이 발생하고 만다.

마찬가지로, 엄밀하게 상체위치 자세와 족평위치 자세와 좌우 무릎의 수평위치와 좌우 무릎의 높이의 차를 그것들의 목표에 유지한 채, 좌우 무릎의 높이의 합만을 변경할 수는 없다. 바꿔 말하면, 억지로 좌우 무릎의 높이의 합만을 변하게 하려면, 접지부위(즉 무릎 및 족평)와 바닥 사이에서 뒤틀림 또는 미끄러짐이 발생하지 않더라도, 상체위치가 목표 위치로부터 벗어난다. 그 결과, 로봇(51)의 전체 중심위치와 관성력이 목표값으로부터 벗어나, 로봇(51)의 안정성이 저하된다.

즉, 어떻든, 계층형 컴플라이언스 동작 결정부(114)에서 결정된 기구변형 보상된 수정 목표 접지부위 위치 자세와 목표 상체위치 자세를 동시에 엄밀하게 만족할 수는 없으므로, 로봇(51)의 도 58 및 도 59의 자세상태에서, 이 로봇(51)의 족평(58) 혹은 무릎과 바닥 사이에서 뒤틀림 또는 미끄러짐이 발생하거나, 로봇(51)의 전체의 중심위치와 관성력이 목표값으로부터 어긋나, 로봇(51)의 안정성이 저하되거나 한다. 또한, 접지부위 중 손(62)은, 상체(53)에 대해 6자유도 이상의 자유도를 가지므로, 팔(54)의 손(62) 이외의 부위로 바닥에 접지하는 것과 같은 자세를 로봇(51)에게 취하게 하지 않는 한, 손(62)과 바닥 사이에서 뒤틀림 등의 미끄러짐이 발생하지는 않는다.

그래서, 본 실시형태에서는, 로봇(51)의 접지부위 중 무릎 및 족평(58)과 바 닥 사이에서 최대한 뒤틀림 또는 미끄러짐이 발생하지 않고, 또한 상체(53)의 중심위치(특히 수평위치)가 가능한 한 바뀌지 않도록 하면서, 좌우 무릎의 높이의 차의 변화 등에 따라 상체(53)의 자세, 또는 자세와 위치를 수정하는 것으로 한다.

도 63(a), (b)에 그 상체(53)의 자세의 수정동작의 예를 도시한다. 또한, 도 63(c)에 대해서는 후술한다. 이들 도 63(a), (b), (c)에서는, 팔(54)과 머리부(63)는 도시를 생략하고 있다.

상체(53)의 자세의 수정동작의 하나는, 도 63(a)에 도시하는 바와 같이 로봇(51)이 무릎을 꿇은 상태로부터, 컴플라이언스 제어에 의한 좌우 무릎의 높이의 차의 변화에 따라, 도 63(b)에 도시하는 바와 같이, 대략 상체(53)의 중심(G)을 통과하는 체간축을 회전축으로 하여, 상체(53)를 회전시키는(화살표 y1과 같이 회전시키는) 동작이다.

또, 도 63과 같은 로봇(51)의 무릎꿇기 자세에서는, 좌우 족평(58, 58)의 높이의 차의 변화에 따라, 좌우 무릎의 높이의 차는 대략 좌우 족평(58, 58)의 높이의 차의 변화의 반 만큼 변화된다. 그래서, 컴플라이언스 제어에 의한 좌우 족평(58, 58)의 높이의 차의 변화에 따라, 좌우 무릎의 높이의 차의 변화에 따른 상체(53)의 자세수정과 동일한 동작을 좌우 족평의 높이의 차의 변화의 경우에 비해 반 분량의 양만큼 행하도록 한다.

또한, 이와 같이 상체(53)의 자세를 수정할 때, 접지부위(무릎 및 족평(58))와 바닥 사이에서 가능한 한 뒤틀림 혹은 미끄러짐이 발생하지 않고, 또한 상체(53)의 중심(G)위치(특히 그 수평위치), 또는 그 중심(G)과 전체 상반력 중심점 과 연결하는 선분의 경사가 가능한 한 바뀌지 않도록 하면서, 컴플라이언스 제어에 의한 좌우 무릎의 높이의 합의 변화에 따라 상체(53)의 위치와 자세를 수정하는 것으로 한다.

도 64에 좌우 무릎의 높이의 합의 변화에 따라 상체(53)의 위치와 자세를 수정하는 동작을 시각적으로 도시한다. 즉, 점선으로 나타내는 로봇(51)의 자세로부터, 실선으로 나타내는 로봇(51)의 자세와 같이, 컴플라이언스 제어에 의해 양 무릎을 내리는 동작에 따라, 상체(53)의 하단부(또는 허리부)를 화살표 y3로 나타내는 바와 같이 전방측으로 옮기고, 상체(53)의 경사를 화살표 y2로 나타내는 바와 같이, 후방측으로(상체(53)의 기립 방향으로) 옮기도록 한다. 즉, 상체(53)의 중심(G)위치(또는 상체(53)의 소정의 대표점의 위치), 특히 그 수평위치를 유지하면서 상체(53)를 후방측으로 경사지게 한다. 또는, 이 중심(G)과 목표 전체 상반력 중심점(P)을 연결하는 선분의 경사를 유지하면서, 상체(53)를 후방측으로 경사지게 한다. 또, 컴플라이언스 제어에 의해 양 무릎을 올리는 동작에 따라, 상기와 반대로, 상체(53)의 하단부(또는 허리부)를 후방측으로 옮기고, 상체의 경사를 전방측으로 옮기도록 한다. 즉, 상체 중심(G)위치(또는 상체의 소정의 대표점의 위치), 특히 그 수평위치를 유지하면서 상체(53)를 전방측으로 경사지게 한다. 또는, 이 중심(G)과 목표 전체 상반력 중심점(P)을 연결하는 선분의 경사를 유지하면서 상체(53)를 전방측으로 경사지게 한다. 또한, 도 64중 Q1", Q3"은 각각 상기한 바와 같이 상체(53)의 위치 자세를 수정한 후의 족평(58)의 목표 상반력 중심점, 무릎의 목표 상반력 중심점을 나타내고 있고, 이 예에서는, Q1"은 수정 전의 족평(58)의 목표 상반력 중심점(Q1)과 동일하다.

이상이 상체(53)의 자세나 위치의 수정동작의 개요이다.

본 실시형태에서의 로봇 기하학 모델(역키네마틱스 연산부)(110)의 처리기능을 도 65의 블럭도에 도시한다.

역키네마틱스 연산부(110)에서는, 우선, 기구변형 보상된 수정 목표 제 n 접지부위 위치 자세(n=1,2,…,6)와 보용생성장치(100)가 결정한 각 족평(58) 및 무릎의 목표 접지부위 위치 자세를 기초로, 보용생성장치(100)가 결정한 좌우 무릎의 목표 위치에 대한, 좌우 무릎의 높이의 차의 수정량(Zkneediffmdfd) 및 좌우 무릎의 높이의 합의 수정량(Zkneesummdfd)과, 보용생성장치(100)가 결정한 좌우 족평의 목표 위치에 대한, 좌우 족평(58, 58)의 높이의 차의 수정량(Zfootdiffmdfd)을 구한다.

구체적으로는, 이하의 식 47, 48, 49에 의해, 좌우 무릎의 높이의 차의 수정량(Zkneediffmdfd), 좌우 무릎의 높이의 합의 수정량(Zkneesummdfd), 좌우 족평의 높이의 차의 수정량(Zfootdiffmdfd)을 구한다.

Zkneediffmdfd

= 기구변형 보상된 수정 목표 제 4 접지부위 위치

- 목표 제 4 접지부위 위치

- 기구변형 보상된 수정 목표 제 3 접지부위 위치

+ 목표 제 3 접지부위 위치 …식 47

Zkneesummdfd

= 기구변형 보상된 수정 목표 제 4 접지부위 위치

- 목표 제 4 접지부위 위치

+ 기구변형 보상된 수정 목표 제 3 접지부위 위치

- 목표 제 3 접지부위 위치 …식 48

Zfootdiffmdfd

= 기구변형 보상된 수정 목표 제 2 접지부위 위치

- 목표 제 2 접지부위 위치

- 기구변형 보상된 수정 목표 제 1 접지부위 위치

+ 목표 제 1 접지부위 위치 …식 49

또한, 식 47~49의 「부위 위치」는 보다 정확하게는, 그 「부위 위치」의 높이 성분(연직방향 성분)이다.

이어서, 좌우 무릎의 높이의 차의 수정량(Zkneediffmdfd), 좌우 무릎의 높이의 합의 수정량(Zkneesummdfd) 및 좌우 족평의 높이의 차의 수정량(Zfootdiffmdfd)을 기초로, 상체위치 자세 수정량을 구한다.

상체위치 자세 수정량은, 무릎높이차용 상체위치 수정량(Xbkneediffmdfd), 무릎높이차용 상체자세 수정량(θbkneediffmdfd), 무릎높이합용 상체위치 수정량(Xbkneesummdfd), 무릎높이합용 상체자세 수정량(θbkneesummdfd) 및 족평높이차용 상체위치 수정량(Xbfootdiffmdfd), 족평높이차용 상체자세 수정량(θbfootdiffmdfd)로 이루어진다. 구체적으로 이것들의 값은 다음과 같이 구해진다.

우선, 좌우 무릎의 높이의 차의 수정량(Zkneediffmdfd)에 따라, 이것에 의해 접지부위(무릎 및 족평(58))와 바닥 사이에서 뒤틀림 또는 미끄러짐이 발생하지 않기 위한, 무릎높이차용 상체위치 수정량(Xbkneediffmdfd)과 무릎높이차용 상체자세 수정량(θbkneediffmdfd)을 좌우 무릎의 높이의 차의 수정량(Zkneediffmdfd)과 그 순간(현재 시각)에서의 로봇(51)의 목표 자세(목표 운동)를 기초로 기하학 연산에 의해 구한다.

보다 구체적으로는, 예를 들면 다음 식 50에 의해, Xbkneediffmdfd와 θbkneediffmdfd를 구한다. 단, Kxkneediff와 Kthkneediff는 로봇(51)의 목표 자세에 따른 비례계수이다.

Xbkneediffmdfd=Kxkneediff*Zkeediffmdfd

θbkneediffmdfd=Kthkneediff*Zkneediffmdfd …식 50

이것 이외에도, 미리, 몇개의 대표적인 로봇(51)의 목표 자세에 대해, 좌우 무릎의 높이의 차의 수정량(또는 제 34 노드 보상각(θ34))과 상체위치 자세의 수정량의 관계를 구해두고, 이것을 맵 또는 함수로서 기억하고, 이것과 좌우 무릎의 높이의 차의 수정량(Zkneediffmdfd)을 기초로 Xbkneediffmdfd 및 θbkneediffmdfd를 구해도 된다. 또한, 본 실시형태에서는, 좌우 무릎의 높이의 차의 수정량에 대해, 상체(53)를 그 체간축 주위로 회전시켜서 상체자세를 수정하므로, 무릎높이차용 상체위치 수정량(Xbkneediffmdfd)는 0이어도 된다.

이어서, 상기와 동일하게, 좌우 족평(58, 58)의 높이의 차의 수정량(Zfootdiffmdfd)에 따라, 이것에 의해 접지부위(무릎 및 족평(58))와 바닥 사이에서 뒤틀림 또는 미끄러짐이 발생하지 않기 위한, 족평높이차용 상체위치 수정 량(Xbfootdiffmdfd)과 족평높이차용 상체자세 수정량(θbfootdiffmdfd)을, 좌우 족평의 높이의 차의 수정량(Zfootdiffmdfd)과 그 순간(현재 시각)에 있어서의 로봇(51)의 목표 자세(목표 운동)를 기초로 기하학 연산에 의해 구한다.

보다 구체적으로는, 예를 들면, 다음 식 51에서 의해, Xbfootdiffmdfd와 θbfootdiffmdfd를 구한다. 단, Kxfootdiff와 Kthfootdiff는 로봇(51)의 목표 자세에 따른 비례계수이다. 전술한 바와 같이 족평(58)의 높이 차의 영향은 무릎의 높이의 차의 영향의 거의 반 분량이므로, Kxfootdiff와 Kthfootdiff는 각각, Kxkneediff와 Kthkneediff의 2분의 1로 한다.

Xbfootdiffmdfd=Kxfootdiff*Zfootdiffmdfd

θbfootdiffmdfd=Kthfootdiff*Zfootdiffmdfd …식 51

또한, 본 실시형태에서는, 좌우 족평(58, 58)의 높이의 차의 수정량에 대해, 상체(53)를 그 체간축 주위로 회전시켜서 상체자세를 수정하므로, 족평높이차용 상체위치 수정량(Xbfootdiffmdfd)는 0이어도 된다.

이어서, 상기와 동일하게, 좌우 무릎의 높이의 합의 수정량, Zsummdfd에 따라, 이것에 의해 접지부위(무릎 및 족평)과 바닥 사이에서 뒤틀림 또는 미끄러짐이 발생하지 않기 위한, 무릎높이합용 상체위치 수정량(Xbsummdfd)과 무릎높이합용 상체자세 수정량(θbsummdfd)을, 좌우 무릎의 높이의 합의 수정량(Zsummdfd)과 그 순간(현재 시각)에 있어서의 로봇(51)의 목표 자세를 기초로 기하학 연산에 의해 구한다.

이것 이외에도, 미리, 몇개의 대표적인 목표 자세에 대해, 좌우 무릎의 높이 의 합의 수정량과 상체자세의 수정량의 관계를 구해 두고, 이것을 맵 또는 함수로서 기억하고, 이것과 좌우 무릎의 높이의 합의 수정량(Zsummdfd)을 기초로 Xbsummdfd와 θbsummdfd를 구해도 된다.

또한, 다음과 같이, 상체위치 자세의 수정량을 결정해도 된다. 즉, 좌우 무릎의 높이의 차를, 그 차를 생기게 하는 제 34 노드 보상각(θ34)으로 환산하고, 좌우 무릎의 높이의 합을, 그 합을 생기게 하는 제 1234 노드 보상각(θ1234)으로 환산하고, 좌우 족평의 높이의 차를 그 차를 생기게 하는 제 12 노드 보상각(θ12)으로 환산한다. 그리고, 이들 환산된 보상각을 기초로, 기하학 연산에 의해 상체위치 자세 수정량을 결정하도록 해도 된다. 또는, 미리, 몇개의 대표적인 로봇(51)의 목표 자세에 대해, 환산된 보상각과 상체위치 자세 수정량의 관계를 구해 두고, 이것을 맵 또는 함수로서 기억하고, 이것과 환산된 보상각을 기초로, 상체위치 자세 수정량을 결정하도록 해도 된다.

이어서, 목표 상체위치 자세를 상체위치 자세 수정량만큼 이동(회전이동과 평행이동)하여, 뒤틀림 보정된 목표 상체위치 자세를 구한다. 구체적으로는, 체간축(또는 소정의 회전축(거의 연직면 내의 회전축)) 주위에 비교적 목표 상체위치 자세를 무릎높이차용 상체자세 수정량과 족평높이차용 상체자세 수정량의 합(θbkneediffmdfd+θbfootdiffmdfd)만큼 회전이동 하고, 이것을 좌우 축 주위로 무릎높이합용 상체자세 수정량(θbkneesummdfd)만큼 더 회전이동 하고, 또한 이것을 로봇(51)의 전후 방향에서 무릎높이차용 상체위치 수정량과 무릎높이합용 상체위치 수정량과 족평높이차용 상체위치 수정량의 합(Xbkneediffmdfd + Xbkneesummdfd + Xbfootdiffmdfd)만큼 평행이동 함으로써, 뒤틀림 보정된 목표 상체위치 자세를 구한다.

이어서, 뒤틀림 보정된 목표 상체위치 자세와 기구변형 보상된 목표 제 n 접지부위 위치 자세(n=1,2,…,6)를 기초로, 로봇(51)의 관절 변위 지령을 구한다.

이상과 같이 역키네마틱스 연산부(110)의 처리가 실행된다.

즉, 제 1 실시형태는, 로봇(51)의 접지부위 위치 자세의 수정동작을 위해, 기하학적으로 자유도가 부족한 경우에는, 계층형 컴플라이언스 동작에 의해 목표 접지부위 위치 자세를 수정할뿐만 아니라, 상체위치의 점(상체의 대표점) 또는 상체를 포함하는 복수 부위의 가중평균 위치(이 경우의 가중값은 각 부위의 질량비로 되어 있는 것이 바람직함)의 점(전체 중심 등), 특히 그 점의 수평위치를 유지하도록, 또는, 그 점과 목표 전체 상반력 중심점(목표 ZMP)을 연결하는 선의 각도를 목표 보용에 있어서의 각도로 가능한 한 유지하도록, 목표 상체위치 자세도 수정하도록 했다.

또, 달리 말하면, 로봇(51)의 전체의 자세제어의 안정성을 높이기 위해서, 목표 전체 상반력 중심점(목표 ZMP) 주위에 발생하는 실 상반력 모멘트의 제어성을 우선하고, 어떤 소정의 접지부위(A)(여기에서는 무릎)의 상대높이 또는 경사각도의 조작량, 또는 어떤 소정의 접지부위(A)를 자손 노드로서 갖는 노드의 상대높이 또는 경사각도의 조작량으로서의 보상 높이 및 보상각 중 적어도 어느 한쪽을 결정하는 수단과, 상기 보상 높이 및 보상각 중 적어도 어느 한쪽에 따라, 상체위치 또는 상체를 포함하는 복수 부위의 상기 가중평균 위치를 대략 목표 보용에서의 위치에 유지하면서, 상체의 자세 및 위치중 적어도 어느 한쪽의 수정량을 결정하는 수단과, 상기 소정의 접지부위(A)를 제외한 소정의 접지부위(B)(여기에서는 족평)의 위치 및 자세 중 적어도 어느 한쪽과, 상기 수정량에 의해 수정한 상체위치 자세에 기초하여, 관절 변위를 결정하는 역키네마틱스 연산수단을 구비하도록 했다.

또한, 제 1 실시형태에서는, 로봇(51)의 상체(53)의 체간축 주위로 상체자세를 회전시키는 대신, 도 64(c)와 같이, 연직축을 회전축으로 하여, 로봇(51)의 허리(상체(53)의 하단부)를 중심으로 상체(53)를 회전시키도록 해도 된다. 단, 이 경우에는 상체(53)의 중심(G)위치의 좌우측 방향으로의 어긋남이 커진다. 또는, 상체(53)의 체간축과 연직축의 중간적인 축을 회전축으로 하여 상체자세를 회전시켜도 된다. 또는 상체(53)의 위치와 자세를 양 무릎의 높이의 차의 수정량이나 양 족평(58, 58)의 높이의 차의 수정량에 따라 동시에 수정해도 된다. 또, 상체(53)의 중심(G)위치가 바뀌지 않도록 하는 대신, 로봇(51)의 전체중심 위치 또는 상체(53)의 대표점 위치가 변하지 않도록 상체(53)의 위치 자세를 수정해도 된다.

어떻든, 컴플라이언스 동작 등에 의한 로봇(51)의 접지부위 위치 자세의 수정동작을 위해, 기하학적으로 자유도가 부족한 경우에, 접지부위의 뒤틀림 등의 미끄러짐이 발생하지 않도록, 상체(53)의 위치 및 자세 중 적어도 어느 한쪽을 목표 보용의 위치 자세로부터 수정하도록 하면 된다. 또, 상체(53)의 위치 및 자세 중 적어도 어느 한쪽을 수정하는 대신 상체(53) 이외의 소정 부위의 위치 및 자세 중 적어도 어느 한쪽을 수정하도록 해도 된다.

[제 2 실시형태]

이어서, 본 발명의 제 2 실시형태에 따른 이동 로봇의 제어장치를 설명한다.

제 1 실시형태에서는, 접지부위의 뒤틀림 등의 미끄러짐을 방지하고, 또한 로봇(51)의 전체 중심위치와 관성력이 가능한 한 벗어나지 않도록 하기 위해서, 상체위치 자세를 수정하도록 했다. 본 실시형태(제 2 실시형태)에서는, 접지부위의 뒤틀림 등의 미끄러짐을 어느 정도까지는 허용하고, 우선적으로 로봇(51)의 전체중심 위치와 관성력이 벗어나지 않도록, 관절 변위(고관절(55)과 무릎관절(56)의 변위) 또는 접지부위 위치 자세를 수정하도록 했다.

즉, 본 실시형태에서는, 역키네마틱스 연산부(110)는, 제 1 실시형태에서 설명한 도 65의 처리 대신, 도 66의 블럭도에서 도시하는 처리를 행하도록 했다. 이 차이점을 제외하면, 본 실시형태는 제 1 실시형태와 동일하다.

본 실시형태의 역키네마틱스 연산부(110)를 도 66을 참조하여 설명하면, 우선, 각 기구변형 보상된 수정 목표 제 제 n 접지부위 위치 자세(n=1,2,…,6)(상세하게는 위치 자세 중의 위치)와 각 목표 제 n 접지부위 위치 자세(n=1,2,…,6)(상세하게는 위치 자세 중의 위치)를 기초로, 상기 식 47, 식 48 및 식 49에 따라 좌우 무릎의 높이의 차의 수정량(Zkneediffmdfd), 좌우 무릎의 높이의 합의 수정량(Zkneesummdfd), 및 좌우 족평의 높이의 차의 수정량(Zfootdiffmdfd)을 구하고, 또한, 좌우 무릎의 높이의 차를, 그 차를 생기게 하는 제 34 노드 보상각(θ34)으로 환산한다. 동일하게, 좌우 무릎의 높이의 합을 그 합을 생기게 하는 제 1234 노드 보상각(θ1234)으로 환산하고, 좌우 족평의 높이의 차를 그 차를 생기게 하는 제 12 노드 보상각(θ12)으로 환산한다.

이어서, 환산된 각 노드 보상각을 기초로 관절 변위 수정량을 결정한다. 이것은 다음과 같이 행해진다. 우선, 목표 보용의 상체위치 자세를 고정하고, 또한 목표 전체 상반력 중심점(목표 제 123456 노드 상반력 중심점(도 61의 점 P))과 로봇(51)의 전체중심(또는, 상체중심, 또는 상체(53)를 포함하는 복수 부위의 중심)을 연결하는 선분의 각도가 바뀌지 않도록 하면서, 관절 변위를 수정한 경우를 상정하고, 이 경우에, 각 접지부위(무릎 및 족평(58))에 대응한 제 n 노드(n=12,34,1234)의 경사각도(보상각)가 단위 섭동(攝動)량으로 되기 위한 제 j 관절 변위 수정량(j=1,2,…,관절의 총수)을 구하고, 이것을 제 n 노드 보상각에 대한 제 j 관절 변위의 감도(Ln_j)로 한다.

보다 일반적으로는, 목표 보용의 관절 변위를 수정한 경우에, 목표 전체 상반력 중심점(목표 제 123456 노드 상반력 중심점)과 로봇(51)의 전체중심(또는, 상체중심, 또는, 상체(53)를 포함하는 복수 부위의 중심)을 연결하는 선분과, 각 접지부위(무릎 및 족평(58))에 대응한 제 n 노드(n=12,34,1234)의 경사각도 사이의 상대각도가 단위섭동량으로 되기 위한 제 j 관절 변위 수정량(j=1,2,…,관절의 총수)을 구하고, 이것을 제 n 노드 보상각에 대한 제 j 관절 변위의 감도(Ln_j)로 한다.

구체적으로는, Ln_knee_r을 제 n 노드 보상각(n=12,34,1234)에 대한 우측 무릎관절 변위의 감도, Ln_knee_l을 제 n 노드 보상각에 대한 좌측 무릎관절 변위의 감도, Ln_hip_r을 제 n 노드 보상각에 대한 우측 가랑이 피치 관절 변위의 감도, Ln_hip_l을 제 n 노드 보상각에 대한 좌측 가랑이 피치 관절 변위의 감도로 하면, 도 58 및 도 59에 도시하는 로봇(51)의 무릎꿇기 자세에서는, 각 감도는 다음 식 52와 같이 설정한다.

L1234_hip_r=0, L1234_hip_l=0, L1234_knee_r=1,

L1234_knee_l=1, L12_hip_r=0, L12_hip_l=0

L12_knee_r=a12, L12_knee_l= -a12, L34_hip_r=a34

L34_hip_l=-a34, L34_knee_r=0, L34_knee_l=0 …식 52

단, a12, a34는 소정의 정수이다.

이어서, 다음 식 53~56에 의해 각 관절 변위 수정량을 결정한다. 단, θknee_r은 우측 무릎관절 변위 수정량, θknee_l은 좌측 무릎관절 변위 수정량, θhip_r은 우측 고관절 변위 수정량(보다 상세하게는, 우측 고관절의 피칭 방향의 관절 변위 수정량), θhip_l은 좌측 고관절 변위 수정량(보다 상세하게는, 좌측 고관절의 피칭 방향의 관절 변위 수정량)으로 한다.

θknee_r=L1234_knee_r*θ1234+L12_knee_r*θ12

+ L34_knee_r*θ34 …식 53

θknee_l=L1234_knee_l*θ1234+L12_knee_l*θ12

+ L34_knee_l*θ34 …식 54

θhip_r=L1234_hip_r*θ1234+L12_hip_r*θ12

+ L34_hip_r*θ34 …식 55

θhip_l=L1234_hip_l*θ1234+L12_hip_l*θ12

+ L34_hip_l*θ34 …식 56

또한, 다른 관절 변위 수정량도 마찬가지로 구하지만, 설명을 쉽게 하기 위해서, 여기에서는 생략한다.

즉, 제 1234 보상각(θ1234)에 대해서는, 도 67에 도시하는 바와 같이 양 무릎관절(56)의 변위(각도)를 θ1234와 동량만큼 수정한다. 또, 제 12 보상각(θ12)에 대해서는, 우측 무릎관절(56)의 변위(각도)를 θ12에 비례하여 수정하면서, 좌측 무릎관절(56)의 변위(각도)를 우측 무릎관절(56)의 변위의 수정량에 -1을 승산한 양만큼 수정한다. 즉 우측 무릎관절(56)의 변위(각도)를 도 67에 도시하는 바와 같이 수정하고, 좌측 무릎관절(56)의 변위(각도)는 그 반대로 수정한다. 또, 제 34 보상각(θ34)에 대해서는, 우측 고관절(55)의 피칭 방향의 변위(각도)를 θ34에 비례하여 수정하고, 좌측 고관절(55)의 피칭 방향의 변위(각도)를 우측 고관절(55)의 피칭 방향의 변위의 수정량에 -1을 승산한 양만큼 수정하도록 했다. 즉 우측 고관절(55)의 피칭 방향의 변위(각도)를 도 68에 도시하는 바와 같이 수정하고, 좌측 고관절(55)의 피칭 방향의 변위(각도)는 그 반대로 수정한다. 또한, 도 67의 Q1, Q1''은 각각 상기와 같은 관절 변위의 수정 전의 족평(58)의 목표 상반력 중심점, 수정 후의 족평(58)의 목표 상반력 중심점을 나타내고 있다. 또, 도 68의 Q3, Q3''은 각각 상기와 같은 관절 변위의 수정 전의 무릎의 목표 상반력 중심점, 수정 후의 무릎의 목표 상반력 중심점을 나타내고 있다.

이러한 관절 변위의 수정에 의해, 목표 전체 상반력 중심점(목표 제 123456 노드 상반력 중심점)과 로봇(51)의 전체중심(또는, 상체중심, 또는, 상체(53)를 포함하는 복수 부위의 중심)을 연결하는 선분과, 접지부위(무릎 및 족평(58))에 대응 한 제 n 노드(n=12,34,1234)의 경사각도와의 사이의 상대각도가 상기 환산된 각 노드 보상각(θ12,θ23,θ1234)만큼 변화된다. 따라서, 실 상반력이 충실하게 제어되어, 로봇(51)의 자세안정성 및 접지성이 향상된다.

상기한 바와 같이 관절 변위 지령(목표 관절 변위)을 직접 수정하는 대신, 대략 동일한 목표 관절 변위를 생기게 하기 위해 우선적으로 위치 자세를 수정하는 접지부위(구체적으로는, 족평(58), 손(62))의 수정 후의 목표 위치 자세인 우선수정 목표 접지부위 위치 자세를 결정하도록 하고, 결정된 우선수정 목표 접지부위 위치 자세를 기초로 역키네마틱스 연산에 의해 관절 변위 지령을 결정하도록 해도 된다.

또한, 관절의 자유도 부족 때문에, 목표 상체위치 자세와 모든 수정 목표 접지부위 위치 자세를 만족하도록 역키네마틱스 연산에 의해 관절 변위를 결정할 수는 없으므로, 역키네마틱스 연산을 행할 때에는, 모든 수정 목표 접지부위 위치 자세 중 일부의 수정 목표 접지부위 위치 자세를 사용하게 된다. 이 일부의 수정 목표 접지부위 위치 자세를 상기 우선수정 목표 접지부위 위치 자세라고 부른다.

예를 들면, 도 67의 상황에서는, 제 1234 노드 보상각(θ1234)에 대해서는, 무릎을 중심으로 하여 목표 족평위치를 회전이동 시킨 위치를 우선수정 목표 족평위치로 결정하면 된다.

또한, 제 1 실시형태 및 제 2 실시형태에서는, 1예로서, 로봇(51)의 무릎꿇기 상황에서의 컴플라이언스 동작에 대해 설명했지만, 로봇이 의자에 앉은 상황에서의 컴플라이언스 동작에 적응하는 경우에는, 다음과 같이 하면 된다. 이하에, 그것을 제 3 실시형태로서 설명한다.

[제 3 실시형태]

도 69은 본 실시형태의 로봇의 허리부의 구성을 도시하고 있다. 이 로봇(71)은 둔부(72)의 밑바닥면부의 좌우에 각각 상반력을 검출하는 상반력 센서(73, 73)(하중 센서. 예를 들면 6축력센서)를 구비한다. 또한, 좌우에 상반력 센서(73, 73)를 설치하는 대신, 둔부(72)의 밑바닥면부의 좌우에 가해지는 힘의 합력을 검출하는 1개의 상반력 센서를 구비하도록 해도 된다.

이 경우, 또, 상반력 센서(73, 73)의 외측은 도시한 바와 같이 스폰지 등의 유연한 부재(탄성체)(74)에 의해 덮혀 있다. 또한, 둔부(72)의 접지부위에서의 컴플라이언스 제어의 정밀도를 좋게 하기 위해서, 유연한 부재에 의해 덮을 뿐만 아니라, 도시한 바와 같이 유연한 부재(탄성체)(74)의 표면(접지면)을 둥글게 볼록면 형상으로 형성하는 것이 바람직하다. 특히, 로봇(71)이 그 둔부(72)를 통하여 의자 등에 앉은 상태에서 상반력 센서(73, 73)의 검지 위치(센서 본체의 위치)와 상기 부재(74)의 볼록면의 정점과의 수평위치를 일치시키도록, 부재(74)를 설치해 두는 것이 바람직하다. 이렇게 함으로써 로봇(71)의 접지부위의 위치 자세의 수정동작과 상반력의 관계의 비선형성이 작아지므로, 로봇(71)의 컴플라이언스 제어 등의 제어특성이 양호하게 된다.

또한, 로봇(71)에서는, 둔부(72)의 좌우의 측부로부터, 다리(링크기구)(52, 52)가 연장하여 설치되어 있다. 다리(52, 52)의 구조는, 예를 들면 그 관절을 포함하여 상기 제 1 실시형태의 것과 동일하게 되어 있다. 따라서, 다리(52, 52)에 대한 제 1 실시형태와 동일한 참조부호를 사용하여, 설명을 생략한다. 단, 본 실시형태에서는 다리(52)의 무릎에 상반력 센서를 구비하고 있지 않아도 된다.

또, 도시한 바와 같이, 둔부(72)의 상측에는, 로봇(71)이 기체인 동체(상체)(77)가 설치되고, 이 동체(77)의 상부의 양 측부로부터 견관절(78, 78)을 통하여 팔(79, 79)이 연장하여 설치되어 있다. 또한, 팔(79)은 상기 제 2 실시형태의 로봇(51)의 것과 동일 구조이어도 된다. 또한, 동체(77)는 관절(80)을 통하여 둔부(72)에 연결되어 있다. 이 경우, 관절(80)은 둔부(72)에 대해 동체(77)를 요잉 방향으로 선회시키는 동체 선회용 관절(80a)과, 동체(77)를 둔부(72)에 대해 전후 및 좌우측 방향으로 경사시키는 동체 굴곡용 관절(80b)로 구성되어 있다. 로봇(71)에 상술한 바와 같이 구비된 각 관절은 도시하지 않은 액추에이터에 의해 동작한다. 또, 둔부(72) 또는 동체(77)에는, 도시를 생략하지만, 상기 제 1 실시형태 등과 동일한 제어장치(50)가 탑재되어 있다. 또, 각 다리(55) 및 각 팔(79)의 선단부에는 6축력센서 등의 외력 검출수단이 설치되어 있다.

본 실시형태에서는, 둔부(72)를 통하여 의자 등에 걸터앉는 로봇(71)에 대해, 도 70에 도시하는 바와 같이 계층구조를 설정하면 된다. 즉, 접지부위로서의 우측 족평(58), 좌측 족평(58), 둔부(72)의 밑바닥면부의 우측부(우측 상반력 센서(73)의 설치 부위), 둔부(72)의 밑바닥면부의 좌측부(좌측 상반력 센서(73)의 설치 부위)를 각각 잎 노드인 제 1 노드, 제 2 노드, 제 3 노드, 제 4 노드에 대응시킨다. 또, 제 1 노드와 제 2 노드를 자 노드로서 갖는 제 12 노드, 제 3 노드와 제 4 노드를 자 노드로서 갖는 제 34 노드를 중간 노드로서 설정하고, 제 12 노드 와 제 34 노드를 자 노드로서 갖는 제 1234 노드를 뿌리 노드로서 설정했다. 또, 각 노드에는, 상기 제 1 참고예 등과 동일한 방침에 의해, 도시한 바와 같이, 목표 상반력 중심점(Qn)(n=1,2,3,4,12,34,1234)이 대응시켜진다. 또한, 뿌리 노드인 제 1234 노드의 목표 상반력 중심점(Q1234)은 목표 전체 상반력 중심점(P)과 동일하다.

본 실시형태에서는, 제어장치(50)는 상기 도 2에 도시한 것과 동일한 기능적 구성을 갖는 것으로 되어 있다.

이 경우, 본 실시형태에서의 보용생성장치(100)가 출력하는 목표 보용 중 목표 운동의 목표 접지부위 궤도는 각 족평(58)의 목표 위치 자세 궤도와 둔부(72)의 목표 위치 자세 궤도로 구성된다. 목표 운동 중 상체위치 자세 궤도는 본 실시형태에서는, 동체(77)의 위치 자세 궤도를 의미한다. 또한, 목표 운동에는 각 팔(79)의 선단부의 위치 자세 궤도도 포함된다. 또, 목표 전체 상반력 중심점(P)은 실제의 바닥면상에는 없고, 공중의 가상면상에 있다.

또, 본 실시형태에서는, 계층형 컴플라이언스 동작 결정부(114)는 그 기능적 구성요소는 상기 제 1 실시형태의 것과 동일하다(도 62를 참조). 단, 본 실시형태에서는, 보상 전체 상반력 모멘트 분배기는, 도 70에 도시한 계층구조에서의 각 중간 노드 및 뿌리 노드의 노드 보상 상반력 모멘트와, 각 족평(58)에 대응하는 잎 노드의 노드 보상 상반력 모멘트를 결정하여 출력한다. 또, 보상각 결정부는, 도 70에 도시한 계층구조에서의 각 중간 노드 및 뿌리 노드의 노드 보상각과, 각 족평(58)에 대응하는 잎 노드의 노드 보상각을 결정하여 출력한다. 이 경우, 이들 노드 보상 상반력 모멘트 및 노드 보상각의 기본적인 결정 수법은 상기 제 1~제 3 참고예나 제 1 실시형태에서 설명한 수법과 동일해도 된다. 또, 바닥형상 추정기는, 상기 제 1 실시형태의 경우와 동일하게, 상기 제 3 참고예에서 설명한 수법과 동일한 수법으로 추정 제 n 바닥높이 편차(Zfn_estm)(n=1,2,3,4)를 구하는 것과 더불어, 각 족평(58)의 접지부위 바닥경사 편차(θfn)(n=1,2)를 추정한다.

또, 제어장치(50)의 기능적 구성요소인 로봇 기하학 모델(역키네마틱스 연산부)는, 상기 제 1 실시형태와 동일한 수법에 의해, 기본적으로는, 상기 기구변형 보상된 수정 목표 접지부위 위치 자세와 목표 상체위치 자세를 기초로, 둔부(72)의 접지부위와 족평(58)(접지부위)에서 뒤틀림 등의 미끄러짐이 발생하지 않도록, 그들 접지부위의 위치 자세를 수정함과 동시에, 상체(동체)(77)의 위치 자세를 수정한다.

이상에서 설명한 것 이외의 제어장치(50)의 처리는 상기 제 1 실시형태와 동일해도 된다.

다음에, 이상에서 설명한 실시형태에 따른 몇개의 변형 태양을 설명한다.

상반력 센서에 의해 직접적으로 검출할 수 없는 실 노드 상반력은 상반력 센서에 의해 직접적으로 검출되는 다른 실 접지부위 상반력, 가속도 센서의 검출값, 경사계의 검출값 등을 사용하여, 옵저버에 의해 추정하거나, 혹은, 간이적인 대수관계에 의해 추정하면 된다. 예를 들면, 계층형 컴플라이언스 동작은, 목표 전체 상반력 중심점 주위에 접지부위를 회전시키는 회전형의 컴플라이언스 동작이므로, 컴플라이언스 동작을 행해도, 로봇의 전체중심의 연직 가속도(또는 목표 전체 상반 력 중심점과 전체 중심을 연결하는 선분 방향의 가속도 성분)는, 로봇의 목표 보용(목표 운동)에서의 전체중심의 연직가속도(또는 목표 전체 상반력 중심점과 전체 중심을 연결하는 선분 방향의 가속도 성분)에 거의 일치한다. 따라서, 모든 실 접지부위 상반력 연직성분의 합은, 로봇의 목표 보용(목표 운동)에서의 전체중심의 연직가속도와 중력가속도의 합에 로봇의 전체 질량을 승산한 값에 거의 일치한다.

따라서, 실 제 n 노드 상반력을 직접적으로 검출할 수 없는 경우, 우선, 제 n 노드를 선조 노드에 갖지 않고, 또한, 제 n 노드 그 자체가 아닌 모든 잎 노드의 실 상반력(실 접지부위 상반력)의 합(이후, 이것을 실 제 n 노드외 상반력이라고 부름)을 구한다.

이어서, 다음 식 57에 의해, 실 제 n 노드 상반력의 추정값인 추정 제 n 노드 상반력을 구한다.

추정 제 n 노드 상반력

=(목표 보용의 전체중심 가속도+중력가속도)*전체 질량

- 실 제 n 노드외 상반력 …식 57

상체 등에 가속도 센서를 구비하고 있는 경우에는, 가속도 센서의 검출값과, 목표 보용 및 실 관절 변위 중 적어도 어느 한쪽에 의해 로봇 전체의 중심가속도(이후, 추정 전체중심 가속도라고 부름)를 추정하고, 다음 식 58에 의해, 실 제 n 노드 상반력의 추정값인 추정 제 n 노드 상반력을 구해도 된다.

추정 제 n 노드 상반력

=(추정 전체 중심가속도+중력가속도)*전체 질량

- 실 제 n 노드외 상반력 …식 58

예를 들면, 상기 제 1 실시형태에서, 무릎의 상반력을 검지하는 상반력 센서가 구비되어 있지 않은 경우, 또는, 무릎의 상반력을 검지하는 상반력 센서가 고장난 경우에는, 추정 제 34 노드 상반력을 다음 식 59에 의해 구한다.

추정 제 34 노드 상반력

=(목표 보용의 전체중심 가속도+중력가속도)*전체 질량

- 실 제 34 노드외 상반력

=목표 보용의 전체중심 가속도*전체 질량

- (실 제 12 노드 상반력+실 제 56 노드 상반력) …식 59

또한, 추정 제 34 노드 상반력을 실 제 34 노드 상반력 대신 사용하여, 컴플라이언스 동작 처리에 의해 제 1234 노드 보상각(θ1234)을 결정하고, 또 바닥형상 추정 처리에 의해 추정 제 1234 노드 상대 바닥높이 편차(Z1234rel_estm)를 추정한다.

제 34 노드 보상각(θ34)의 결정에서는, 상정한 바닥대로라고 가정하고, 기구변형 보상된 수정 목표 제 3 접지부위 위치 자세, 기구변형 보상된 수정 목표 제 4 접지부위 위치 자세와 추정 제 34 노드 상반력을 기초로, 실 제 3 노드 상반력의 추정값인 추정 제 3 노드 상반력과 실 제 4 노드 상반력의 추정값인 추정 제 4 노드 상반력을 구한다.

또한, 추정 제 3 노드 상반력과 추정 제 4 노드 상반력을 실 제 3 노드 상반력과 실 제 4 노드 상반력 대신 사용하여, 컴플라이언스 동작 처리에 의해 제 34 노드 보상각(θ34)을 결정한다.

로봇의 자세의 복원 요구 모멘트(보상 전체 상반력 모멘트)를 기초로, 상기와 같이 복원 요구 모멘트를 더하는 것과 등가가 되도록 목표 노드 상반력 중심점 주위의 수정 목표 노드 상반력 모멘트를 결정하는 대신, 복원 요구 모멘트(보상 전체 상반력 모멘트)를 기초로, 목표 노드 상반력 중심점을 수정한 수정 목표 노드 상반력 중심점을 결정해도 된다. 이 경우, 목표 노드 상반력 중심점 주위의 목표 노드 상반력 모멘트는 수정하지 않고 0인 채로 한다.

또, 제 n 노드 상반력 내에서 제어할 수 없었던 성분(예상되는, 또는 검출된 제어편차 성분)을 보충하기 위해서, 이 성분에 따라, 제 n 노드의 부모 노드의 목표 상반력을 수정해도 된다. 즉, 실 제 n 노드 상반력과 목표 제 n 노드 상반력의 차의 전회 제어주기의 값 또는 상기 차를 로 패스 필터에 통과시킨 값을 제 n 노드의 목표 상반력에 부가해도 된다.

바닥형상의 추정 처리에서는 컴플라이언스 제어용의 트리구조와 다른 트리구조를 설정해도 된다. 그 트리구조는 뿌리 노드와 잎 노드로 이루어지는 2층의 것이어도 된다. 또, 각 노드의 가중값도 컴플라이언스 제어용의 것과 상이한 것으로 해도 된다. 노드 상반력이 0이 되는 시기(그 노드에 속하는 모든 접지부위, 또는 그 노드에 대응하는 접지부위가 공중을 이동하는 시기)에서 그 노드의 가중값이 0이면 된다.

또, 바닥형상의 추정 처리에서는, 전술의 수정 목표 노드 상반력 중심점에 기초하여, 각 수정 노드 가중값을 결정하고, 이것을 요소로 하는 벡터를 바닥형상 추정에 사용하는 가중값으로서 사용해도 된다.

바닥형상 추정의 처리에서, 각 점(각 접지부위)의 추정 상대 바닥높이 편차를 구할 때에, 실 상반력과 기구 컴플라이언스 모델과 자세 센서의 검출값을 사용하는 대신, 컴플라이언스 기구 등의 변형량을 검출하는 변형량 검출기를 구비하고, 그 변형량 검출기의 검출값과 자세 센서의 검출값을 사용하도록 해도 된다.

바닥형상을 상대적으로 추정할 뿐만 아니라, 글로벌로 추정(글로벌 좌표계에서의 바닥형상의 추정)하고 싶은 경우에는, 다음과 같이 해도 된다.

1) 로봇의 상체의 글로벌 좌표계에서의 높이(이하, 글로벌 높이라고 함)를 검지할 수 있는 경우에 있어서의 바닥의 글로벌 높이의 추정:

상체 등 로봇이 어떤 소정 부위의 글로벌 높이를 검출하는 검출기(가속도 센서를 사용한 상체높이 추정 장치(예를 들면 본원 출원인에 의한 PCT/JP03/05448호에 기재되어 있는 추정 장치) 또는 시각센서 등의 외계 센서 등)를 사용한다. 추정된 상체의 글로벌 높이와, 관절 변위(실 관절 변위 또는 목표 관절 변위), 실 상체자세 경사의 검출값 및 실 상반력의 검출값을 기초로, 추정 바닥높이 편차의 글로벌 좌표계에서의 값을 구한다. 따라서, 뿌리 노드의 추정 바닥높이 편차는, 모든 접지부위의 글로벌한 추정 바닥높이 편차의 가중평균값으로서의 의미를 갖게 된다.

2) 상체의 글로벌 높이를 모르는 상황에서의 바닥의 글로벌 높이의 추정:

어떤 추정 제 n 바닥높이 편차의 글로벌 값(그룹 내 상대값이 아니라 글로벌 좌표계에서의 편차)을 알고 있으면, 그것과 추정 각 상대 바닥높이 편차를 기초로, 모든 점(접지부위)의 추정 바닥높이 편차의 글로벌 값을 구한다. 로봇의 이동의 스타트 점에서의 추정 제 n 바닥높이 편차의 글로벌 값을 알고 있으면, 로봇의 이동을 계속함으로써 차례차례로 새롭게 접지한 접지부위의 바닥높이 편차의 글로벌 값이 구해지므로, 이것을 기억함으로써, 로봇의 이동궤적상에서의 바닥높이의 맵을 작성할 수 있다.

또, 바닥형상의 추정 처리에서는, 전술의 수정 목표 노드 상반력 중심점에 기초하여 각 수정 노드 가중값(각 노드의 가중값의 값을 수정한 것)을 결정하고, 이것을 바닥형상 추정에서, 실 제 n 노드 상대 바닥높이 등을 정의하기 위해서 사용하는 가중값으로 해도 된다. 또, 바닥형상 추정에서, 실 제 n 노드 상대 바닥높이 등을 정의하기 위해서 사용하는 가중값은 목표 상반력 분배기가 결정한 전술의 가중값과 반드시 동일한 것으로 할 필요는 없다.

보충하면, 실 제 n 노드 상대 바닥높이 등을 정의하기 위한 가중값은 목표 상반력 분배기가 결정하는 가중값에 반드시 일치시키지 않으면 안될 것은 없지만, 일치시키면, (이 경우, 자동적으로, 뿌리 노드의 목표 상반력 중심점은 목표 전체 상반력 중심점에 일치하는,) 컴플라이언스 동작용의 제 n 노드 보상각(n=1,2,…)에 제 n 노드 추정 바닥경사 편차를 부가하는 것만으로, 바닥형상 편차의 영향을 없앨 수 있으므로, 바닥형상 편차의 영향을 없애기 위한 연산량을 저감할 수 있다.

또한, 추정 바닥형상 편차를 바닥형상의 지도정보로서 기억해 두고, 다음회 동일 장소를 보행할 때에, 기억해 둔 바닥형상의 지도정보를 기초로 목표 보용을 생성하도록 해도 된다.

또한, 상기의 각 실시형태에 있어서의 연산처리 기능을 나타내는 블록선도는 혹은 플로우차트에서의 연산처리 순서를 등가변형을 해도 된다.

이상과 같이, 본 발명은 다리식 이동 로봇의 다리의 선단부 이외의 부위를 접지시키는 상황이어도 상반력을 적절하게 제어하여, 높은 안정성으로 원활한 동작을 가능하게 하는 다리식 이동 로봇을 제공할 수 있는 것으로서 유용하다.

Claims (5)

1. 기체와, 이 기체의 이동을 위해 이 기체에 연결되어 외부와 접촉하는 복수의 링크기구를 구비함과 동시에, 이 기체와 각 링크기구의 선단부 사이에, 이 기체에 대해 각 링크기구의 선단부를 가동으로 하는 복수의 관절이 설치되고, 상기 복수의 링크기구 중 1개 이상의 특정 링크기구의 선단부와 상기 기체 사이의 소정 부위에서 외부와 접촉하는 특정 동작 자세로 동작할 수 있는 다리식 이동 로봇의 제어장치에 있어서,

   상기 특정 동작 자세에서 상기 소정 부위에 작용하는 외력을 검출 또는 추정하는 외력 검출수단과,

   상기 특정동작 자세에서의 상기 소정 부위의 외력의 목표값인 목표 외력을 결정하는 목표 외력 결정수단과,

   상기 검출 또는 추정된 외력이 상기 목표 외력에 근접하도록 적어도 상기 소정 부위와 상기 기체 사이에 존재하는 관절의 변위를 제어하는 관절 제어수단을 구비한 것을 특징으로 하는 이동 로봇의 제어장치.
2. 제 1 항에 있어서, 상기 특정 링크 기구는 다리체인 것을 특징으로 하는 이동 로봇의 제어장치.
3. 제 1 항에 있어서, 상기 특정 링크 기구는 상기 기체에 관절을 통하여 연결 된 둔부로부터 연장하여 설치된 다리체 이며, 상기 소정 부위는 상기 둔부인 것을 특징으로 하는 이동 로봇의 제어장치.
4. 제 1 항에 있어서, 상기 이동 로봇의 상기 기체 등의 제 2 소정 부위의 실 자세를 검출하는 실 자세 검출수단과, 이 제 2 소정 부위의 목표 자세를 결정하는 목표 운동 결정수단을 구비하고, 상기 목표 외력 결정수단은 적어도 상기 제 2 소정 부위의 실 자세와 목표 자세의 편차에 따라 상기 목표 외력을 결정하는 것을 특징으로 하는 이동 로봇의 제어장치.
5. 제 1 항에 있어서, 상기 이동 로봇의 상기 기체 등의 제 2 소정 부위의 실 자세를 검출하는 실 자세 검출수단과, 이 제 2 소정 부위의 목표 자세를 결정하는 목표 운동 결정수단을 구비하고, 상기 관절 제어수단은 상기 제 2 소정 부위의 실 자세와 목표 자세의 편차에 따라 외력의 편차량을 결정하는 수단을 구비하고, 상기 목표 외력과 상기 외력의 조작량의 합력에 상기 검출 또는 추정된 외력을 근접시키도록 상기 관절의 변위를 제어하는 것을 특징으로 하는 이동 로봇의 제어장치.

Images