KR20050007346A

KR20050007346A - 다리식 이동 로봇의 자기자세 추정 장치

Info

Publication number: KR20050007346A
Application number: KR10-2004-7017278A
Authority: KR
Inventors: 다케나카도루; 마츠모토다카시
Original assignee: 혼다 기켄 고교 가부시키가이샤
Priority date: 2002-04-26
Filing date: 2003-04-28
Publication date: 2005-01-17
Also published as: JP4181114B2; EP1504855B1; DE60336518D1; EP1508408B1; US7337040B2; WO2003090981A1; EP1508408A4; KR100956520B1; US7145305B2; DE60332233D1; US20050126833A1; JPWO2003090981A1; DE60336519D1; KR20050000410A; EP2106886B1; KR100956539B1; EP2110210A1; EP1504855A1; KR100956537B1; EP2106886A1

Abstract

상체(3) 등에 자이로 센서(각속도 센서), 가속도 센서를 탑재한 로봇(1)의 목표 보용의 목표운동, 관절의 변위 검출값, 관절의 변위 목표값 등을 포함하는 로봇의 운동 상태량을 사용하여, 로봇(1)의 운동에 의해 발생하는 가속도 센서 자신의 운동 가속도를 추정하고, 그 운동 가속도와 가속도 센서의 가속도 검출값과 각속도 센서의 각속도 검출값을 기초로 상체(3) 등의 소정의 부위의 실제 자세를 추정한다. 가속도 검출값과 운동 가속도와의 차이를 기초로 소정의 부위의 실제 자세의 추정값의 오차를 추정하고, 그 오차를 0에 근접시키도록 각속도 검출값을 적분하는 적분수단의 입력을 보정하면서, 적분수단의 출력을 실제 자세의 추정값으로서 얻는다. 이것에 의해 소정의 부위의 실제 자세를 정밀도 좋게 추정한다.

Description

다리식 이동 로봇의 자기자세 추정 장치{SYSTEM FOR ESTIMATING ATTITUDE OF LEG TYPE MOVING ROBOT ITSELF}

일반적으로, 2족이동 로봇 등의 다리식 이동 로봇에는 상체에 경사계가 탑재되어 있고, 경사계의 출력(상체의 연직방향에 대한 경사각)을 목표 보용의 상체의 경사각에 수속시키도록 로봇의 자세 제어가 행해진다.

경사계는 상체의 각속도를 검지하기 위한 자이로 센서와, 연직방향(즉 중력방향)을 검지하기 위한 가속도 센서(또는 진자 등의 연직 지시기)로 구성된다. 경사계에서는 기본적으로 자이로 센서가 검지한 각속도를 적분함으로써 상체의 경사각을 추정하지만 그대로는 적분오차가 축적(소위 드리프트가 발생함)되므로, 가속도 센서의 검출값을 사용하여 적분에 의해 얻어진 추정 경사각을 보정하고 있었다. 구체적으로는 가속도 센서에 의해 검출되는 가속도 방향에 대한 경사각과, 자이로 센서의 검출값을 적분기에 통과시켜서 얻어진 경사각과의 차를 구하고, 그 차이에 따른 보정량(차에 어떤 소정의 게인을 곱한 값과 차를 적분하고 어떤 소정의 게인을 곱한 값의 합)을 상기 적분기에 추가적으로 입력함으로써 경사각을 보정하고 있었다.

그러나, 이 방식에서는 로봇의 주행 등과 같이 상체가 격렬하게 가속 또는은 감속하는 경우에는 가속도 센서에 의해 검출되는 가속도에는 중력가속도 이외에 상체의 운동에 의한 가속도가 가해지기 때문에, 중력가속도 방향를 정확하게 검출할 수 없게 되어 상기의 보정에 의해 오히려 추정 경사각의 오차가 확대될 우려가 있었다.

이후, 상체 등과 같은 어떤 대표적인 부위의 경사와 방향을 총칭하여 「자세」라고 부른다. 「경사」는 연직방향과 이루는 각도이다. 「방향」은 대표적인 부위의 전방방향을 나타내는 벡터를 수평면에 투영한 벡터의 방향이다.

일반적으로 대표적인 부위란, 자이로 센서와 가속도 센서로 구성되는 경사계가 탑재되어 있는 부위이지만, 다른 부위라도 그 부위와 경사계가 탑재되어 있는 부위 사이의 각 관절에 엔코더와 같은 관절변위 검출기나 변위 제어 액추에이터가 있으면, 그 부위의 경사를 산출할 수 있으므로 그 부위를 대표적인 부위로 해도 상관없다. 예를 들면 관절을 갖는 목에 연결된 머리부에 자이로 센서 및 또는 가속도 센서를 구비하는 경우에도 대표 부위를 상체로 해도 좋다. 또한, 후술하는 실시형태에서는 상체를 대표적 부위로 하고 있다.

또 일반적으로는 전체 관절변위의 세트도 자세라고 불리지만, 특별히 예고가 없는 한, 본 실시예에서는 「자세」를 이 의미로는 사용하지 않는다.

종래의 다리식 이동 로봇에서는 상기와 같은 문제에 더하여 이동중에 다리체를 흔드는 것에 의한 반력에 의해, 족평(足平)(다리체 선단부)와 바닥과의 사이의 마찰력이 한계가 되어, 족평과 바닥 사이에서 회전 미끄러짐(스핀)이 생겨 로봇 전체가 연직축 주위로 자세 회전하여 목표 보용의 방향으로부터 벗어난다는 문제도 있었다.

보충하면, 상체가 항상 연직(직립)을 유지하고 직선보행하는 목표 보용만이 생성된다고는 할 수 없다. 목표 보용에서도 로봇 전체 또는 상체는 선회하거나 전후 좌우로 기울거나 한다. 즉, 목표 보용에서도 전체의 자세 회전(또는 상체 등의 대표 부위의 자세 회전)이 존재한다. 그래서, 본 명세서에서는 목표 보용에 있어서의 자세 회전을 목표 자세 회전이라 부른다. 본 명세서에서 주로 문제로 하는 현상은 실제의 로봇의 전체의 자세 회전(또는 상체 등의 대표 부위의 자세 회전)이 상기 목표 자세 회전으로부터 벗어나는 것이다. 이 현상을 엄밀하게 표현하면, 「목표 자세 회전으로부터의 섭동(攝動)」 또는 「자세 회전 섭동」이라고 불러야 하지만, 목표 자세 회전과 혼동할 우려가 없는 경우에는, 이후 이것을 「자세 회전」이라고 생략한다.

또한, 이후 특히 로봇 전체가 연직축 주위로 자세 회전하여, 목표 보용의 방향으로부터 벗어나는 현상을 스핀이라 부른다.

본 발명은 이상에서 설명한 배경을 감안하여 이루어진 것으로서, 다리식 이동 로봇의 자기자세를 정밀도 좋게 추정할 수 있는 자기자세 추정 장치를 제공하는 것을 목적으로 한다. 특히 가속도가 수평방향이나 연직방향으로 격렬하게 변동하고 있는 상태나 로봇의 발바닥과 바닥 사이의 회전 미끄러짐 등에 의해 로봇 전체에 자세 회전(또는 스핀)이 발생하고 있는 상태에서도, 자기자세를 정밀도 좋게 추정할 수 있는 자기자세 추정 장치를 제공하는 것을 목적으로 한다.

본 발명은 다리식 이동 로봇의 자기자세를 추정하는 장치에 관한 것이다.

도 1은 본 발명의 실시형태에서의 다리식 이동 로봇으로서의 2족이동 로봇의 전체적 구성의 개략을 도시하는 개략도,

도 2는 도 1의 각 다리체의 족평부분의 구성을 개략적으로 도시하는 개략도,

도 3 및 도 4는 각각 각 다리체의 족평부분의 상세 구성을 도시하는 측면에서 본 단면도 및 저면도,

도 5는 도 1의 로봇에 구비한 제어 유닛의 구성을 도시하는 블록도,

도 6은 도 5의 제어 유닛의 기능적 구성을 도시하는 블록도,

도 7은 로봇의 주행보용을 도시하는 설명도,

도 8(a), (b), (c)는 각각 목표 보용의 상반력 연직성분, 목표 ZMP, 자기위치 자세 추정용의 게인의 설정예를 도시하는 그래프,

도 9는 제 1 실시형태에서의 제어 유닛의 주요부의 처리를 도시하는 플로차트,

도 10은 도 9의 플로차트의 자기위치 자세 추정 처리를 도시하는 플로차트,

도 11 및 도 12는 도 9의 플로차트의 자기위치 자세 추정 처리를 설명하기 위한 도면,

도 13은 도 9의 플로차트의 자기위치 자세 추정 처리의 주요부의 처리를 도시하는 블록도,

도 14(a), (b)는 도 13의 처리를 설명하기 위한 도면, 그리고

도 15는 도 13의 처리에서 사용하는 게인의 설정예를 도시하는 그래프이다.

본 발명의 다리식 이동 로봇의 자기자세 추정 장치의 제 1 발명은 상기의 목적을 달성하기 위해서 결정된 목표 보용에 추종하도록 제어되는 다리식 이동 로봇에 있어서, 상기 로봇의 소정의 부위의 실제 자세각 속도를 검출 또는 추정하는 자세각 속도 검출 수단과, 상기 로봇에 탑재된 가속도 센서와, 적어도 상기 목표 보용의 목표운동과 상기 로봇의 관절의 변위 검출값과 이 관절의 변위 목표값과중 어느 하나를 포함하는 이 로봇의 운동 상태량을 사용하여 이 로봇의 운동에 의해 발생하는 상기 가속도 센서 자신의 가속도의 추정값으로서의 추정 운동 가속도를 결정하는 운동 가속도 추정 수단과, 적어도 상기 가속도 센서에 의한 가속도 검출값과, 상기 추정 운동 가속도와, 상기 검출 또는 추정된 실제 자세 각속도를 기초로 상기 소정의 부위의 실제 자세를 추정하는 자세 추정 수단을 구비한 것을 특징으로 하는 것이다.

이 제 1 발명에서는, 적어도 상기 목표 보용의 목표운동과 상기 로봇의 관절의 변위 검출값과 이 관절의 변위 목표값중 어느 하나를 포함하는 이 로봇의 운동 상태량을 사용하여 이 로봇의 운동에 의해 발생하는 상기 가속도 센서 자신의 가속도의 추정값으로서의 추정 운동 가속도가 결정된다. 즉, 상기 운동 상태량을 사용하여 가속도 센서의 위치의 운동 궤도를 기하학적으로 구할 수 있으므로, 그 운동 궤도로부터 가속도 센서 자신의 운동 가속도가 추정된다. 그리고, 그 추정 운동 가속도와, 가속도 센서에 의한 가속도 검출값(이것은 중력가속도와 운동 가속도를 합성한 것이 됨)과 상기 자세각속도 검출 수단에 의해 검출 또는 추정된 실제 자세각속도를 기초로 상기 소정의 부위의 실제 자세가 추정된다. 이 때문에, 상기 가속도 검출값에 포함되는 운동 가속도의 영향을 상기 추정 운동 가속도에 의해 보상하면서, 상기 검출 또는 추정된 실제 자세각속도, 혹은 그 적분값을 보정하고, 상기 소정의 부위의 실제 자세를 추정하는 것이 가능하게 된다. 그 결과, 로봇의 자기자세로서의 이 소정의 부위의 실제 자세를 정밀도 좋게 추정할 수 있다.

또, 본 발명의 다리식 이동 로봇의 자기자세 추정 장치의 제 2 발명은 결정된 목표 보용에 추종하도록 제어되는 다리식 이동 로봇에 있어서, 상기 로봇에 탑재된 가속도 센서와, 상기 로봇의 소정의 부위의 실제 자세각속도를 검출 또는 추정하는 자세각속도 검출 수단과, 적어도 상기 검출 또는 추정된 실제 자세각속도를 기초로, 상기 소정의 부위의 실제 자세의 추정값으로서의 자세각 추정값을 구하는 자세 추정 수단과, 상기 자세각 추정값과 상기 목표 보용에서의 상기 소정의 부위의 목표 자세각과의 차인 자세 회전 편차의 시간적 변화량을 자세 회전 편차 변화량으로서 구하는 자세 회전 편차 산출 수단과, 상기 자세 회전 편차의 변화의 회전중심을 결정하는 회전중심 결정 수단과, 상기 로봇이 상기 회전중심 주위로 상기 자세 회전 편차 변화량으로 회전했다고 상정하고, 상기 로봇의 운동에 의해 발생하는 상기 가속도 센서 자신의 가속도의 추정값으로서의 추정 운동 가속도를 결정하는 운동 가속도 추정 수단을 구비하고, 상기 자세 추정 수단은, 적어도 상기 가속도 센서에 의한 가속도 검출값과 상기 추정 운동 가속도를 기초로 상기 자세각 추정값을 보정하면서 구하는 것을 특징으로 하는 것이다.

이러한 제 2 발명에서는, 상기 추정 운동 가속도를 결정할 때 상기 자세각추정값과 상기 목표 보용에서의 상기 소정의 부위의 목표 자세각과의 차인 자세 회전 편차의 변화는, 어떤 회전중심의 주위로 로봇의 전체가 상기 자세 회전 편차 변화량으로 회전한 것과 동등하다고 상정된다. 실제로, 예를 들면 로봇의 족평(다리체 선단부)과 바닥과의 사이에서 회전 미끄러짐(스핀)이 발생한 경우에는, 그것이 상기 자세 회전 편차의 변화가 되어서 나타난다. 그리고 제 2 발명에서는, 상기 상정을 기초로 로봇의 운동에 의해 발생하는 상기 가속도 센서 자신의 가속도의 추정값으로서의 추정 운동 가속도를 결정하므로, 상기 회전 미끄러짐 등에 의한 상기 가속도 센서 자신의 실제의 운동을 정량적으로 파악하고, 이 가속도 센서 자신의 운동 가속도의 추정값인 추정 운동 가속도를 결정할 수 있다. 그 결과, 정밀도가 좋은 추정 운동 가속도를 결정하는 것이 가능하게 된다.

또한, 이 때 추정 운동 가속도를 결정하려면, 예를 들면 상기 상정을 기초로 가속도 센서의 위치(가속도 센서의 탑재부의 위치)를 차례로 추정하고, 그 위치의 운동 궤도에 기초하여 추정 운동 가속도를 결정하면 된다. 또는, 예를 들면 가속도 센서의 탑재부와 상이한 로봇의 어떤 대표 장소의 위치(로봇의 어떤 부위의 위치나, 착지 동작에서 착지하는 다리체의 접지 위치 등)를 상기 상정을 기초로 추정하고, 그 대표 장소의 위치의 추정값과, 로봇의 목표 보용 혹은 관절의 변위 검출값 혹은 이 관절의 변위 목표값을 사용하여, 기하학적 연산(키네마틱스 연산)에 의해 가속도 센서의 위치의 운동 궤도를 구하고, 그 운동 궤도로부터 상기 추정 운동 가속도를 결정하도록 해도 좋다. 보충하면 로봇의 접지 위치(바닥과의 접촉 개소의 위치) 또는 로봇의 하나의 부위의 위치를 추정할 수 있으면, 로봇의 임의의 위치의 운동 궤도는 상기와 같은 기하학적 연산에 의해 구할 수 있다.

제 2 발명에서는, 또한, 적어도 상기 가속도 센서에 의한 가속도 검출값과 상기와 같이 결정한 상기 추정 운동 가속도를 기초로, 상기 자세각 추정값이 보정되면서 결정된다. 이것에 의해, 상기 제 1 발명과 동일하게, 상기 가속도 검출값에 포함되는 운동 가속도의 영향을 상기 추정 운동 가속도에 의해 보상하면서, 상기 자세 측정값을 보정하는 것이 가능하게 된다. 그 결과, 이 소정의 부위의 실제 자세의 추정값인 자세각 추정값의 정밀도를 높일 수 있다. 즉, 로봇의 자기자세로서의 상기 소정의 부위의 실제 자세를 정밀도 좋게 추정할 수 있다.

상기 제 2 발명에서는, 더 구체적으로는 상기 운동 가속도 추정 수단은 상기 목표 보용을 기술하는 좌표계인 제 1 좌표계를 상기 자세 회전 편차 변화량만큼 상기 회전중심 주위로 회전시켜서 이루어지는 제 2 좌표계를 구하는 수단을 구비하고, 상기 목표 보용의 목표운동과 상기 로봇의 관절의 변위 검출값과 이 관절의 변위 목표값중 적어도 어느 하나로부터 파악되는, 상기 제 1 좌표계로부터 본 상기 가속도 센서 자신의 가속도와, 상기 추정 운동 가속도로 운동하는 가속도 센서를 상기 제 2 좌표계로부터 본 경우의 상기 가속도 센서 자신의 가속도가 동일하게 되도록 상기 추정 운동 가속도를 결정하는 것이 바람직하다(제 3 발명).

이러한 제 3 발명에 의하면, 상기 제 1 좌표계로부터 본 가속도 센서 자신의 가속도, 즉, 제 1 좌표계상에서 로봇이 회전 미끄러짐 등을 일으키지 않아, 목표 보용의 목표 운동 혹은 관절변위 검출값 혹은 관절변위 목표값 대로 운동하고 있다고 한 경우에 이 목표 운동, 관절변위 검출값 및 관절변위 목표값중 적어도 어느하나로부터 파악되는 가속도 센서 자신의 가속도와, 상기 추정 운동 가속도로 운동하는 가속도 센서를, 이 제 1 좌표계를 상기 회전중심 주위로 상기 자세 회전 편차 변화량으로 회전시켜서 이루어지는 제 2 좌표계로부터 본 경우의 가속도 센서 자신의 가속도가 동일하게(각 좌표계상에서의 가속도 센서 자신의 가속도의 각 좌표성분이 양 쪽좌표계에서 동일하게) 되도록, 상기 추정 운동 가속도가 결정된다. 그 결과, 자세 회전 편차의 변화를 적절하게 반영시켜서 가속도 센서 자신의 가속도의 추정값인 추정 운동 가속도를 결정할 수 있어, 정밀도가 좋은 추정 운동 가속도를 얻을 수 있다. 나아가서는, 상기 자세각 추정값의 보정을 적정하게 행하여, 이 자세각 추정값의 정밀도를 높일 수 있다.

상기 제 1∼제 3 발명에서는, 상기 자세 추정 수단은 적어도 상기 자세각속도의 검출값을 포함하는 입력을 적분함으로써 상기 소정의 부위의 실제 자세의 추정값으로서의 자세각 추정값을 구하는 적분수단과, 상기 가속도 센서에 의한 가속도 검출값과 상기 추정 운동 가속도와의 차에 가초하여, 상기 자세각 추정값의 오차를 추정하는 자세각 오차 추정 수단과, 상기 추정된 오차를 0에 근접시키도록 상기 적분수단의 입력을 보정하는 입력 보정 수단으로 구성되어 있는 것이 바람직하다(제 4 발명).

즉, 상기 가속도 센서에 의한 가속도 검출값과 상기 추정 운동 가속도와의 차(벡터)는 중력가속도에 상당하는 것이 되는데, 이 차이의 벡터는 상기 자세각 추정값에 오차가 있을 경우에는 이 자세각 추정값에 의해 파악되는 글로벌 좌표계(이 자세각 추정값이 정확하다고 가정하여 파악되는 글로벌 좌표계)상에서 연직방향을향하지 않는다. 여기에서, 글로벌 좌표계는 로봇이 이동하는 환경의 바닥(지면)에 대해 고정된 좌표계이다. 따라서, 상기 차(벡터)를 기초로 자세각 추정값(과거값)의 오차를 추정할 수 있다. 그리고 제 4 발명에서는, 그 추정된 오차를 0에 근접시키도록 상기 적분수단의 입력을 보정함으로써 이 적분수단의 출력인 자세각 추정값이 보정된다. 이것에 의해 자세각 추정값의 정밀도를 높일 수 있다. 또한, 상기한 바와 같이 적분수단의 입력을 보정하려면, 예를 들면 상기 오차로부터 적당한 피드백 제어칙에 의해 조작량을 구하고, 그 조작량을 적분수단에 부가적으로 입력하면 된다.

또, 상기 제 1∼제 4 발명에서는, 상기 소정의 부위는 상기 로봇의 상체(다리체가 뻗어 설치되는 기체)인 것이 바람직하다(제 5 발명). 상체의 자세각를 정밀도 좋게 파악하는 것은 로봇의 안정한 이동을 행하는 점에서 중요하기 때문이다.

또한, 제 1∼제 4 발명에서 상기 소정의 부위가 상기 로봇의 상체일 때에는 상기 자세 추정 수단이 보정하는 상기 자세각 추정값은, 이 상체의 연직방향에 대한 경사각도의 추정값인 것이 특히 바람직하다(제 6 발명). 상체의 연직방향에 대한 경사각도는 특히 로봇의 안정한 이동을 행하는 점에서 중요하기 때문이다.

이하, 첨부된 도면을 참조하여 본 발명의 실시형태에 관계되는 다리식 이동 로봇의 자기위치 추정 장치를 설명한다. 또한, 다리식 이동 로봇으로서는 2족이동 로봇을 예로 든다.

도 1은, 이 실시형태에 관계되는 다리식 이동 로봇으로서의 2족이동 로봇을전체적으로 도시하는 개략도이다.

도시한 바와 같이, 2족이동 로봇(이하, 로봇이라고 함)(1)은 상체(로봇(1)의 기체)(3)로부터 하방으로 뻗어 설치된 좌우 한쌍의 다리체(다리부 링크)(2, 2)를 구비한다. 양 다리체(2, 2)는 동일 구조이며, 각각 6개의 관절을 구비한다. 그 6개의 관절은 상체 3측으로부터 순차적으로, 다리 가랑이(허리부)의 선회(회전)용(상체(3)에 대한 요잉 방향의 회전용)의 관절 10R, 10L(부호 R, L은 각각 우측다리체, 좌측다리체에 대응하는 것을 의미하는 부호이다. 이하 동일함)과, 다리 가랑이(허리부)의 롤링 방향(X축 둘레)의 회전용의 관절 12R, 12L과, 다리 가랑이(허리부)의 피칭 방향(Y축 둘레)의 회전용의 관절 14R, 14L, 무릎부의 피칭 방향의 회전용의 관절 16R, 16L과, 발목의 피칭 방향의 회전용의 관절 18R, 18L과, 발목의 롤링 방향의 회전용의 관절 20R, 20L로 구성된다.

각 다리체(2)의 발목의 2개의 관절 18R(L), 20R(L)의 하부에는, 각 다리체(2)의 선단부를 구성하는 족평(족부)(22R(L))가 부착되는 동시에, 양 다리체(2, 2)의 최상위에는, 각 다리체(2)의 다리 가랑이의 3개의 관절10R(L), 12R(L), 14R(L)을 통하여 상기 상체(3)가 부착되어 있다. 상체(3)의 내부에는, 상세를 후술하는 제어 유닛(60) 등이 격납된다. 또한, 도 1에서는 도시의 편의상, 제어 유닛(60)을 상체(3)의 외부에 기재하고 있다.

상기 구성의 각 다리체(2)에서는, 고관절(또는 허리관절)은 관절 10R(L), 12R(L), 14R(L)로 구성되고, 무릎관절은 관절 16R(L)로 구성되고, 발목관절은 관절 18R(L), 20R(L)로 구성된다. 또 고관절과 무릎관절과는 대퇴 링크(24R(L))로 연결되고, 무릎관절과 발목관절과는 하퇴 링크(26R(L))로 연결된다.

또한, 상체(3)의 상부의 양 측부에는 좌우 한쌍의 팔체(5, 5)가 부착되는 동시에, 상체(3)의 상단부에는 머리부(4)가 배치된다. 이들 팔체(5, 5) 및 머리부(4)는, 본 발명의 요지와 직접적인 관련을 갖지 않기 때문에 상세한 설명을 생략한다.

상기의 구성에 의해, 각 다리체(2)의 족평(22R(L))은, 상체(3)에 대해 6개의 자유도가 주어져 있다. 그리고 로봇(1)의 보행 등의 이동중에, 양 다리체(2, 2)를 합하여 6*2=12개(이 명세서에서 「*」은 스칼라에 대한 연산으로서는 승산을, 벡터에 대한 연산으로서는 외적을 나타냄)의 관절을 적당한 각도로 구동함으로써 양 족평(22R, 22L)의 원하는 운동을 행할 수 있다. 이것에 의해, 로봇(1)은 임의로 3차원 공간을 이동할 수 있다.

도 1에 도시하는 바와 같이, 각 다리체(2)의 발목관절 18R(L), 20R(L)의 하방에는 족평(22R(L))과의 사이에 공지의 6축력 센서(50)가 개장되어 있다. 이 6축력 센서(50)는, 각 다리체(2)의 족평(22R(L))의 착지의 유무, 및 각 다리체(2)에 작용하는 상반력(접지 하중) 등을 검출하기 위한 것이고, 이 상반력의 병진력의 3방향 성분 Fx, Fy, Fz 및 모멘트의 3방향 성분 Mx, My, Mz의 검출 신호를 제어 유닛(60)에 출력한다. 또, 상체(3)에는, Z축(연직방향(중력방향))에 대한 상체(3)의 경사(자세각) 및 그 각속도 등을 검출하기 위한 경사 센서(54)가 구비되고, 그 검출 신호가 이 경사 센서(54)로부터 제어 유닛(60)에 출력된다. 이 경사 센서(54)는, 도시를 생략하는 3축 방향의 가속도 센서 및 3축방향의 자이로 센서를 구비하고, 이들 센서의 검출 신호가 상체(3)의 경사 및 그 각속도를 검출하기 위해서 사용되는 동시에, 로봇(1)의 자기 위치 자세를 추정하기 위해서 사용된다. 또, 상세구조의 도시는 생략하지만, 로봇(1)의 각 관절에는, 그것을 구동하기 위한 전동 모터(64)(도 5 참조)와, 그 전동 모터(64)의 회전량(각 관절의 회전각)을 검출하기 위한 엔코더(로터리엔코더)(65)(도 5 참조)가 설치되고, 이 엔코더(65)의 검출 신호가 이 엔코더(65)로부터 제어 유닛(60)에 출력된다.

또한, 도 1에서는 도시를 생략하지만, 로봇(1)의 적당한 위치에는 조이스틱(조작기)(73)(도 5 참조)이 설치되고, 그 조이스틱(73)을 조작함으로써, 직진 이동하고 있는 로봇(1)을 선회시키는 등, 로봇(1)의 보용에 대한 요구를 필요에 따라서 제어 유닛(60)에 입력할 수 있게 구성되어 있다.

도 2는 본 실시형태에서의 각 다리체(2)의 선단부(각 족평(22R(L))을 포함함)의 기본 구성을 개략적으로 도시하는 도면이다. 동 도면에 도시하는 바와 같이, 각 족평(22R(L))의 상방에는, 상기 6축력 센서(50) 사이에 스프링 기구(70)가 장착되는 동시에, 발바닥(각 족평(22R,L)의 바닥면)에는 고무 등으로 이루어지는 발바닥 탄성체(71)가 붙여져 있다. 이들 스프링 기구(70) 및 발바닥 탄성체(71)에 의해 컴플라이언스 기구(72)가 구성되어 있다. 스프링 기구(70)는 상세한 것은 후술하지만, 족평(22R(L))의 상면부에 부착된 방형 형상의 가이드 부재(도 2에서는 도시 생략)와, 발목관절(18R(L))(도 2에서는 발목관절(20R(L))을 생략하고 있음) 및 6축력 센서(50)측에 부착되고, 상기 가이드 부재에 탄성재(고무나 스프링)를 통하여 미동하도록 수납되는 피스톤 형상 부재(도 2에서는 도시생략)로 구성되어 있다.

도 2에 실선으로 표시된 족평(22R(L))은, 상반력을 받고 있지 않을 때의 상태를 나타내고 있다. 각 다리체(2)이 상반력을 받으면, 컴플라이언스 기구(72)의 스프링 기구(70)와 발바닥 탄성체(71)가 휘고, 족평(22R(L))은 도면중에 점선으로 예시한 위치 자세로 이동한다. 이 컴플라이언스 기구(72)의 구조는, 예를 들면 본 출원인이 앞서 제안한 일본 특개평 5-305584호 공보에 상세하게 설명되어 있는 바와 같이, 착지 충격을 완화하기 위해서 뿐만아니라, 제어성을 높이기 위해서도 중요한 것이다.

상기 컴플라이언스 기구(72)를 포함한 족평(22R(L))(이하, 족평기구(22R(L))라 부르는 일이 있다.)의 보다 상세한 구성을 도 3 및 도 4를 참조하여 더욱 설명한다. 도 3은 족평기구(22R(L))의 측면에서 본 단면도, 도 4는 이 족평기구(22R(L))의 바닥면측에서 본 평면도이다.

족평기구(22R(L))는, 대략 평판 형상의 족평 플레이트 부재(102)를 골격부재로서 구비하고 있다. 이 족평 플레이트 부재(102)는, 그 전단부(발끝부)와 후단부(발뒤꿈치부)가 약간 상방으로 만곡되어 있는데, 다른 부분은 평탄한 평판 형상으로 되어 있다. 또, 족평 플레이트 부재(102)의 상면부에는, 횡단면 방형 형상의 가이드 부재(103)가 그 축심을 상하 방향을 향하여 고정 설치되어 있다. 이 가이드 부재(103)의 내부에는, 이 가이드 부재(103)의 내주면을 따르도록 하여 대략 상하방향으로 이동할 수 있게 설치된 가동판(피스톤 형상 부재)(104)이 설치되고, 이 가동판(104)이 발목관절(18R(L), 20R(L))에 6축력 센서(50)를 통하여 연결되어 있다.

또, 가동판(104)은, 그 하면의 둘레 가장자리부가 스프링, 고무 등의 탄성재로 이루어지는 복수의 탄성부재(106)(도면에서는 스프링으로 기재되어 있음)를 통하여 족평 플레이트 부재(102)의 상면부에 연결되어 있다. 따라서, 족평 플레이트 부재(102)는, 탄성부재(106), 가동판(104) 및 6축력 센서(50)를 통하여 발목관절(18R(L))에 연결되어 있다. 또한, 가이드 부재(103)의 내부(가동판(104)의 하측의 공간)은, 도시 생략하는 구멍이나 간극을 통하여 대기측으로 개방되어 있고, 대기중의 공기가 가이드 부재(103)의 내부에 입출 자유롭게 되어 있다. 또, 상기 가이드 부재(103), 가동판(104), 및 탄성부재(106)는 상기 도 2에 도시한 스프링 기구(70)를 구성하는 것이다.

족평 플레이트 부재(102)의 바닥면(하면)에는, 상기 도 2에 도시한 발바닥 탄성체(71)로서의 접지 부재(71)가 부착되어 있다. 이 접지 부재(71)는, 족평기구(22R(L))의 접지 상태에서, 이 족평 플레이트 부재(102)와 바닥면 사이에 개재시키는 탄성부재(바닥면에 직접적으로 접촉하는 탄성부재) 이며, 본 실시형태에서는 족평 플레이트 부재(102)의 접지면의 네모퉁이(족평 플레이트 부재(102)의 발끝부의 양 측부 및 발뒤꿈치부의 양 측부)에 고정되어 있다.

또, 접지 부재(71)는, 본 실시형태에서는 비교적 연질의 고무재로 이루어지는 연질층(107a)과, 비교적 경질인 고무재로 이루어지는 경질층(107b)을 상하로 중합하여 이루어지는 2층구조로 형성되고, 경질층(107b)이, 다리체(2)의 착상시에 직접적으로 바닥면에 접촉하는 접지면부로서 최하면측에 설치되어 있다.

족평기구(22R(L))에는, 상기의 구성의 것 이외에, 착지 충격 완충장치(108)가 구비되어 있다. 이 착지 충격 완충장치(108)는, 족평 플레이트 부재(102)의 바닥면에 부착된 백 형상 부재(109)와, 이 백 형상 부재(109)의 내부에 대해 압축성 유체로서의 공기(대기중의 공기)을 출입시키기 위한 유통로(110)를 구비하고 있다.

백 형상 부재(109)는, 그 주위에 상기 접지 부재(71)가 존재하도록 하고, 족평 플레이트 부재(102)의 바닥면의 대략 중앙부에 설치되어 있다. 이 백 형상 부재(109)는, 고무 등의 탄성재에 의해 변형 자유롭게 구성되어 있어, 외력에 의한 탄성변형이 생기지 않은 자연상태에서는, 도 3에 실선으로 나타내는 바와 같이, 상방으로 개구한 원통 용기형상을 보인다. 그리고 이 백 형상 부재(109)는, 그 개구단부가 전체 주위에 걸쳐 족평 플레이트 부재(102)의 바닥면에 고정되고, 이 족평 플레이트 부재(102)에 의해 덮개씌워져 있다. 또, 백 형상 부재(109)는, 원통 용기형상을 보이는 자연상태에서는, 이 백 형상 부재(109)의 바닥이 상기 접지 부재(71)보다도 하방으로 돌출하도록 설치되어 있다. 즉, 이백 형상 부재(109)의 높이(족평 플레이트 부재(102)의 하면으로부터 백 형상 부재(109)의 바닥부까지의 거리)는, 접지 부재(71)의 두께보다도 큰 것으로 되어 있다. 따라서 족평 플레이트 부재(102)가 접지 부재(71)를 통하여 접지한 상태(다리부(2)의 착상 상태)에서는, 백 형상 부재(109)는, 도 3에 가상선으로 도시하는 바와 같이, 상반력에 의해 백 형상 부재(109)의 높이 방향으로 압축된다.

또한, 본 실시형태에서는 백 형상 부재(109)가 원통 용기형상을 보이는 자연상태는 이 백 형상 부재(109)의 팽창 상태이다. 그리고, 백 형상 부재(109)는, 탄성재에 의해 구성되어 있기 때문에, 압축되었을 때, 자연상태의 형상(원통 용기형상)으로의 형상복원력을 갖는다.

상기 유통로(110)는, 백 형상 부재(109)에 대한 공기의 유입·유출을 행하는 유입·유출 수단을 구성하는 것이며, 본 실시형태에서는 백 형상 부재(109)의 내부와 상기 가이드 부재(103)의 내부를 연통시키도록 족평 플레이트 부재(102)에 뚫어 설치된 유통 구멍이다. 이 경우, 상기한 바와 같이, 가이드 부재(103)의 내부는 대기측으로 개방되어 있으므로, 이 유통로(110)는, 백 형상 부재(109)의 내부를 대기측으로 연통시키고 있는 것이 된다. 따라서, 백 형상 부재(109)의 내부에는, 대기중의 공기가 유통로(110)를 통하여 출입 자유롭게 되어 있고, 이 백 형상 부재(109)의 팽창 상태(자연상태)에서는, 이 백 형상 부재(109)내에는 공기가 충전되고, 그 내부의 압력은 대기압과 동등하게 된다. 또, 유통로(110)는 좁힘 통로로 되어 있어, 백 형상 부재(109)의 내부에 공기가 출입할 때는 유체저항을 발생시키게 되어 있다.

도 5는 제어 유닛(60)의 구성을 도시하는 블록도이다. 이 제어 유닛(60)은 마이크로컴퓨터에 의해 구성되어 있고, CPU로 이루어지는 제 1 연산장치(90) 및 제 2 연산장치(92), A/D 변환기(80), 카운터(86), D/A변환기(96), RAM(84), ROM(94), 및 이들간의 데이터 수수를 행하는 버스 라인(82)을 구비하고 있다. 이 제어 유닛(60)에서는, 각 다리체(2)의 6축력 센서(50), 경사 센서(54)(가속도 센서 및 레이트 자이로 센서), 조이스틱(73) 등의 출력 신호는 A/D 변환기(80)에서 디지털 값으로 변환된 후, 버스라인(82)을 통하여 RAM(84)에 보내진다. 또 로봇(1)의 각 관절의 엔코더(65)(로터리 엔코더)의 출력은, 카운터(86)을 통하여 RAM(84)에 입력된다.

상기 제 1 연산장치(90)는 후술하는 바와 같이 목표 보용을 생성하는 동시에, 관절각 변위지령(각 관절의 변위각 혹은 각 전동 모터(64)의 회전각의 지령값)을 산출하고, RAM(84)에 송출한다. 또 제 2 연산장치(92)는 RAM(84)으로부터 관절각 변위지령과, 상기 엔코더(65)의 출력신호에 기초하여 검출된 관절각의 실측값을 읽어 내고, 각 관절의 구동에 필요한 조작량을 산출하여 D/A 변환기(96)와 서보 앰프(64a)를 통하여 각 관절을 구동하는 전동 모터(64)에 출력한다.

도 6은, 이 실시형태에 관계되는 다리식 이동 로봇의 제어장치의 기능적 구성을 전체적으로 도시하는 블록도이다. 이 도 6중의 「실제 로봇」의 부분이외의 부분이 제어 유닛(60)이 실행하는 처리 기능(주로 제 1 연산장치(90) 및 제 2의 연산장치(92)의 기능)에 의해 구성되는 것이다. 또한, 이하의 설명에서는, 다리체(2)의 좌우를 특별히 구별할 필요가 없을 때는, 상기 부호 R, L을 생략한다.

이하 설명하면, 제어 유닛(60)은 로봇(1)의 목표 보용을 자유롭고 또한 리얼타임으로 생성하여 출력하는 보용 생성 장치(200), 자기위치 자세 추정부(214) 등을 구비하고 있다. 자기위치 자세 추정부(214)는 상세하게는 후술하지만, 본 발명의 특징부분에 관계되는 처리를 실행하여, 로봇(1)의 위치·자세(글로벌 좌표계에서의 위치·자세)를 추정하는 것이다.

보용 생성 장치(200)이 출력하는 목표 보용은 목표 상체위치 자세 궤도(상체(3)의 목표 위치 및 목표 자세의 궤도), 목표 족평위치 자세 궤도(각 족평(22)의목표 위치 및 목표 자세의 궤도), 목표 팔자세 궤도(각 팔체(5)의 목표 자세의 궤도), 목표 전체 상반력 중심점(목표 ZMP) 궤도, 목표 전체 상반력 궤도로 구성된다. 또한, 다리체(2)나 팔체(5) 이외에 상체(3)에 대해 가동의 부위를 구비하는 경우에는, 그 가동 부위의 목표 위치 자세 궤도가 목표 보용에 가해진다.

여기에서, 본 발명의 실시형태에서의 용어의 의미 또는 정의에 대해 보충해 둔다. 상기 보용에서의 「궤도」는 시간적 변화의 패턴(시계열 패턴)을 의미하고, 이하의 설명에서는, 「궤도」 대신에 「패턴」이라고 부르는 경우도 있다. 또, 각 부위의 「자세」는, 이 부위의 경사와 방향을 총칭한 것이다. 여기에서, 「경사」는 이 부위의 연직방향과 이루는 각도이며, 「방향」은 이 부위의 전방 방향을 나타내는 벡터를 수평면에 투영한 벡터의 방향이다. 예를 들면 상체자세중의 경사는, Z축(연직축)에 대한 롤링 방향(X축 둘레)의 상체(3)의 경사각(자세각)과, Z축에 대한 피칭 방향(Y축 둘레)의 상체(3)의 경사각(자세각)으로 이루어진다. 또, 상체(3)의 방향은, 상체(3)의 전방 방향을 나타내는 벡터를 수평면에 투영한 벡터의 요잉 방향(Z축 둘레)의 회전각으로 나타내어진다. 또한, 족평자세는 각 족평(22)에 고정적으로 설정된 2축의 공간적인 방위각으로 나타내어진다. 또, 특히, 족평(22)의 착지 자세에 관해서는, 그 착지 자세는, 기본적으로는 착지한 족평(22)의 방향을 나타내고, 구체적으로는 착지한 족평(22)의 발뒤꿈치로부터 발끝을 향하는 벡터를 수평면에 투영한 벡터의 방향을 나타낸다. 또, 목표 팔자세는, 팔체(5)의 모든 부위에 관한 상체(3)에 대한 상대적인 자세로 나타내어진다.

상체 위치는, 상체(3)의 소정 위치, 구체적으로는 상체(3)의 미리 정한 대표점의 위치를 의미한다. 마찬가지로, 족평위치는, 각 족평(22R, 22L)의 미리 정한 대표점의 위치를 의미한다. 또한, 상체 속도는, 상체(3)의 상기 대표점의 이동 속도를 의미하고, 족평 속도는, 각 족평(22R, 22L)의 상기 대표점의 이동 속도를 의미한다.

목표 상체위치 자세 등의 목표 보용에 관하여, 이하의 설명에서는, 오해를 일으킬 우려가 없는 경우에는, 종종 「목표」를 생략한다. 또, 보용중의, 상반력에 관계되는 구성 요소 이외의 구성 요소, 즉 족평위치 자세, 상체위치 자세 등, 로봇(1)의 운동에 관계되는 보용을 총칭적으로 「운동」이라고 한다.

각 족평(22R, L)의 상반력(병진력 및 모멘트로 이루어지는 상반력)을 「각 족평 상반력」이라 부르고, 로봇(1)의 모든(2개의) 족평(22R, 22L)의 상반력의 합력을 「전체 상반력」이라고 부른다. 단, 이하의 설명에서는 각 족평 상반력에 관해서는 거의 언급하지 않으므로, 예고하지 않는 한, 「상반력」은 「전체 상반력」과 동일한 의미로서 취급한다.

목표 상반력은, 일반적으로는, 작용점과 그 점에 작용하는 힘(병진력)과 힘의 모멘트에 의해 표현된다. 작용점은 어디에 잡아도 좋으므로, 동일한 목표 상반력에서도 무수한 표현을 생각할 수 있지만, 특별히 전술의 목표 상반력 중심점을 작용점으로 하여 목표 상반력을 표현하면, 힘의 모멘트는 연직축 성분을 제외하면 0이 된다.

또한, 동력학적 평형 조건을 만족하는 보용에서는, 목표 운동 궤도로부터 산출되는 ZMP(목표 운동 궤도로부터 산출되는 로봇(1)의 관성력과 중력과의 합력이그 점 주위에 작용하는 모멘트가, 연직축 성분을 제외하고 0이 되는 점)와 목표 전체 상반력 중심점은 일치하므로, 목표 전체 상반력 중심점 궤도 대신에 목표 ZMP 궤도를 부여한다고 해도 동일하다(상세한 것은, 예를 들면 본 출원인에 의한 PCT 공개 공보 WO/02/40224를 참조).

이러한 배경으로부터, PCT 공개 공보 WO/02/40224의 명세서에서는 목표 보용을 다음과 같이 정의하고 있었다.

a) 광의의 목표 보용이란, 1보 내지는 복수보의 기간의 목표 운동 궤도와 그 목표 상반력 궤도와의 세트이다.

b) 협의의 목표 보용이란, 1보의 기간의 목표 운동 궤도와 그 ZMP 궤도와의 세트이다.

c) 일련의 보용은, 몇개의 보용이 이어진 것으로 한다.

로봇(1)의 보행을 행하는 경우에 있어서는, 본 출원인이 앞서 일본 특개평 10-86080호 공보에서 제안한 상체 높이 결정 수법에 의해 상체 연직위치(상체 높이)가 결정되면, 상반력의 병진력 성분은 종속적으로 결정되므로, 목표 보용의 상반력에 관하여 명시적으로 설정해야 할 물리량으로서는, ZMP만으로 충분했다. 따라서, PCT 공개 공보 WO/02/40224의 명세서에서는, 협의의 목표 보용으로서는, 상기의 b)로 충분했다. 그것에 반해, 로봇(1)이 주행을 행하는 경우에는, 상반력 연직성분도 제어상 중요하므로, 이 상반력 연직성분을 명시적으로 설정하는 것이 바람직하다. 그래서, 본원 출원인이 앞서 제안한 PCT 출원(PCT/JP02/13596) 등에서는, 협의의 목표 보용으로서, 다음 b')를 채용했다.

b') 협의의 목표 보용이란, 1보의 기간의 목표 운동 궤도와 그 ZMP 궤도와 상반력 연직성분 궤도의 세트이다.

이 명세서에서는 이후, 특별히 예고하지 않는 한, 목표 보용은 협의의 목표 보용의 의미로 사용한다. 또, 목표 보용의 「1보」는, 로봇(1)의 한쪽의 다리체(2)가 착지하고나서 또 한쪽의 다리체(2)가 착지할 때까지의 의미로 사용한다.

보용에서의 양다리 지지기란 말할 필요도 없이, 로봇(1)이 그 자체 중량을 양 다리체(2, 2)로 지지하는 기간, 한쪽 다리 지지기란 어느 한쪽만의 다리체(2)로 로봇(1)의 자체 중량을 지지하는 기간, 공중기란 양 다리체(2, 2)가 바닥으로부터 떨어져 있는(공중에 떠 있는) 기간을 말한다.

한쪽 다리 지지기에서 로봇(1)의 자체 중량을 지지하지 않는 측의 다리체(2)를 「미착지 다리」라고 부르고, 자체 중량을 지지하는 쪽의 다리체(2)을 「지지 다리」라고 부른다. 로봇(1)의 보행에서는, 양다리 지지기와 한쪽 다리 지지기가 번갈아 반복되고, 로봇(1)의 주행에서는 한쪽 다리 지지기와 공중기가 번갈아 반복된다. 이 경우, 주행의 공중기에서는, 양다리체(2, 2) 모두, 로봇(1)의 자체 중량을 지지하지 않는 것이 되지만, 이 공중기의 직전의 한쪽 다리 지지기에서 미착지 다리였던 다리체(2), 지지 다리였던 다리체(2)를 각각 이 공중기에서도 미착지 다리, 지지 다리라고 부른다.

또, 목표 상체자세, 목표 상체위치, 목표 족평위치자세, 목표 팔자세 등, 목표 보용에서의 로봇(1)의 각 부의 위치 자세는 지지 다리 좌표계로 기술된다. 지지 다리 좌표계란, 지지 다리의 족평(22)의 접지면 근처에 원점을 갖는 바닥면에고정된 좌표계이다. 보다 상세하게는, 지지 다리 좌표계는, 본 출원인의 특허 3273443호에 기재되어 있는 바와 같이, 지지 다리의 족평(22)을 접지면과의 사이에서 미끄러뜨리지 않고, 수평자세가 될 때까지 회전시켰을 때의, 이 지지 다리의 발목관절의 중심으로부터 접지면에의 수직 투영점을 원점으로 하고, 이 지지 다리 족평(22)의 발끝을 향하는 수평축(족평(22)의 전후방향의 축)을 X축으로 하고, 연직축을 Z축, 이들 X축, Z축에 직교하는 좌표축(족평(22)의 좌우 방향의 축)을 Y축으로 하는 좌표계이다.

본 발명의 실시형태에 관계되는 보용 생성 장치(200)는, 2보 앞까지의 미착지 다리의 족평(22)의 착지위치자세, 착지 시간의 요구값(목표값)을 입력으로 하여, 목표 상체위치자세 궤도, 목표 족평위치자세 궤도, 목표 ZMP 궤도, 목표 상반력 연직성분 궤도, 및 목표 팔자세 궤도로 구성되는 목표 보용을 생성한다. 이때, 이것들의 궤도를 규정하는 패러미터(이것을 보용 패러미터라 부름)의 일부는, 보용의 계속성을 만족하도록 수정된다.

목표 보용의 생성에는 로봇(1)의 동력학 모델이 사용된다. 그 동력학 모델로서는, 예를 들면, 상기 PCT 공개공보 WO/02/40224호에 기재된 단순화 모델 또는, 본 출원인이 제안한 일본 특개 2002-326173호 공보에 기재된 다질점 모델(풀 모델) 등을 사용하면 좋다.

또, 보용 생성 장치(200)는, 로봇(1)의 한쪽의 다리체(2)가 착지하고나서 다른쪽의 다리체(2)가 착지할 때까지의 1보분의 목표 보용(상기 협의의 의미에서의 목표 보용)을 단위로 하여, 그 1보분의 목표 보용을 순서로 생성한다. 여기에서,현재 또는 이제부터 생성하고자 하는 보용을 「금회 보용」, 그 다음 보용을 「다음회 보용」, 또한 그 다음 보용을 「다음다음회 보용」이라고 부른다. 또, 「금회 보용」의 하나 전에 생성한 목표 보용을 「전회 보용」이라고 부른다.

보용 생성 장치(200)가 생성하는 목표 보용의 일부를 예시적으로 개략적으로 설명하면, 예를 들면 목표 족평위치자세 궤도는, 본 출원인에 의한 일본 특허 3233450호에 개시한 유한시간 정정 필터를 사용하여 생성된다. 이 유한시간 정정 필터에 의한 족평위치자세 궤도의 생성 처리에서는, 예를 들면 족평위치 궤도는, 목표 착지위치(착지위치의 요구값)을 향하여 족평(22)을 서서히 가속하면서 이동을 개시하고, 목표 착지 시간(착지 시간의 요구값)까지 서서히 속도를 0 또는 거의 0에까지 감속하고, 이 목표 착지 시간에 목표 착지위치에 도달하여 정지하도록 생성된다. 족평자세 궤도에 대해서도 동일하다. 이것에 의해 생성되는 목표 족평위치자세 궤도는, 착지 순간에 있어서의 대지속도가 0 또는 거의 0이 되기 때문에, 특히 로봇(1)이 주행을 행하는 경우에, 상기 공중기로부터의 착지시에 있어서의 착지 충격을 작게 할 수 있다.

또, 도 7에 도시하는 바와 같이 인간이 주행을 행하는 경우와 동일한 형태로 로봇(1)이 주행을 행하는 경우에는, 예를 들면 목표 상반력 연직성분 궤도 및 목표 ZMP 궤도(상세하게는 지지 다리 좌표계의 X축방향(지지 다리 족평(22)의 전후방향)에서의 목표 ZMP 궤도)는, 각각 도 8(a), 도 8(b)에 실선으로 도시하는 바와 같은 패턴으로 설정된다. 또한, 도 7의 제 1 번째∼제 3 번째의 도면은, 각각 한쪽 다리 지지기의 개시시, 중간시점, 종료시에 있어서의 로봇(1)의 양 다리체(2, 2)의운동 상태를 모식적으로 도시하고, 제 4 번째 및 제 5 번째의 도면은, 각각 공중기의 중간 시점, 공중기의 종료시(다음의 한쪽 다리 지지기의 개시시)에 있어서의 로봇(1)의 양 다리체(2, 2)의 운동 상태를 모식적으로 도시하고 있다.

로봇(1)의 주행을 행하는 경우에는, 목표 상반력 연직성분 궤도는, 기본적으로는, 한쪽 다리 지지기에서는 위로 볼록한 패턴이 되고, 공중기에서는 0에 유지된다. 또, 로봇(1)의 보행을 행하는 경우에는, 목표 상반력 연직성분 궤도는, 예를 들면 도 8(a)에 2점 쇄선으로 도시하는 바와 같이 설정된다. 이 경우, 2점 쇄선중 위에 볼록한 부분이 양다리 지지기에 대응하고, 아래로 볼록한 부분이 한쪽 다리 지지기에 대응한다. 또, 목표 ZMP는 주행, 보행 어느쪽에 있어도, 기본적으로는, 로봇(1)의 다리체(2)의 접지면내(보다 상세하게는 소위, 지지 다각형내)의 중앙부근에 설정된다.

도 9는 상기 보용 생성 장치(200)의 보용 생성 처리, 및, 도 6에 도시하는 자기위치 자세 추정부(214)의 자기위치 자세 추정 처리를 도시하는 플로차트(구조화 플로차트)이다.

먼저 S010에서 시각 t를 0으로 초기화하는 등 여러 초기화 작업을 행한다.

이어서 S012를 거쳐서 S014로 진행하고, 제어 주기마다의 타이머 인터럽션을 기다린다. 제어 주기는 △t이다.

이어서 S016으로 진행하고, 자기위치 자세 추정부(214)에 의한 자기위치 자세 추정을 행한다. S016에서의 처리가 본 출원에 관계되는 다리식 이동 로봇의 자기자세 추정 장치의 특징을 이루지만, 이 설명은 후술하기로 한다.

이어서 S018로 진행하고, 보용 전환 차례(전회 보용의 생성이 종료되고, 새로운 금회 보용의 생성을 개시해야 할 시각)인지 아닌지가 판단된다. 그리고 그 판단 결과가 YES일 때는 S020으로 진행하고, NO일 때는 S032로 진행한다. 또한, 이하에 설명하는 S020 이후의 처리는, 본원 출원인이 앞서 제안한 PCT 공개 공보 WO/02/40224 또는 상기 PCT 출원 PCT/JP02/13596에 상세하게 설명되어 있으므로, 본 명세서에서는 간략적인 설명에 그친다.

S020으로 진행할 때는 시각 t를 0으로 초기화한다. 이어서 S022로 진행하고, 다음회 보용 지지다리 좌표계(상세하게는 그 위치 및 방향), 다음다음회 보용 지지다리 좌표계(상세하게는 그 위치 및 방향), 금회 보용주기 및 다음회 보용주기를 읽어들인다.

상기 다음회 보용 지지다리 좌표계 및 다음다음회 보용 지지다리 좌표계는, 각각, 상기 조이스틱(73)의 조작 등에 의해 지정되는 1보째의 미착지 다리 족평(22)(금회 보용의 미착지 다리 족평(22))의 착지 위치 자세의 요구값(목표 착지 위치 자세), 2보째의 미착지 다리 족평(22)(다음회 보용의 미착지 다리 족평(22))의 착지 위치 자세의 요구값(목표 착지 위치 자세)에 따라, 상기한 지지다리 좌표계의 정의에 따라서 결정된다.

또, 금회 보용주기, 다음회 보용주기는, 각각, 1보째의 미착지 다리 족평(22)의 착지 시각의 요구값(목표 착지 시각), 2보째의 미착지 다리 족평(22)의 착지 시각의 요구값(목표 착지 시각)에 따라 결정된다.

상기한 미착지 다리 족평(22)의 착지 위치 자세의 요구값 및 착지 시각의 요구값, 또는 지지다리 좌표계의 위치 및 방향 및 보용주기는, 미리 보행 스케줄로서 기억해 두어도 좋고, 또는 조이스틱(73) 등의 조종 장치로부터의 지령(요구)과 그때까지의 보행 이력을 기초로 결정해도 좋다.

이어서 S024로 진행하고, 금회 보용에 이어지는 정상 선회 보용의 보용 패러미터가, S022에서 결정된 다음회 보용 지지다리 좌표계, 다음다음회 보용 지지다리 좌표계, 금회 보용주기 및 다음회 보용주기 등에 기초하여 결정된다. 주로, 목표 족평위치 자세 궤도를 규정하는 족평궤도 패러미터, 목표 상체자세의 기준궤도를 규정하는 기준 상체자세 궤도 패러미터, 목표 팔자세 궤도를 규정하는 팔자세 궤도 패러미터, 목표 ZMP 궤도를 규정하는 ZMP 궤도 패러미터, 목표 상반력 연직성분 궤도를 규정하는 상반력 연직성분 궤도 패러미터가 결정된다. 예를 들면 상반력 연직성분 궤도 패러미터에 관해서 예시하면, 상기 도 9(a)에 도시한 패턴의 꺽임점의 시각이나 값이 상반력 연직성분 궤도 패러미터로서 결정된다.

여기에서, 상기 정상 선회 보용은 그 보용을 반복했을 때에 보용의 경계에서 로봇(1)의 운동 상태에 불연속이 생기지 않는 주기적 보용을 의미한다(이후, 「정상 선회 보용」을 「정상 보용」이라고 생략하는 경우도 있음).

정상선회 보용의 1주기분의 보용은, 제 1 선회 보용과 제 2 선회 보용으로 이루어진다. 제 1 선회 보용은 금회 보용의 지지다리 좌표계에 대응하는 지지다리 족평(22)을 다음다음회 보용 지지다리 좌표계에 대응하는 위치 자세까지 움직일 때의 보용에 상당하고, 제 2 선회 보용은 다음회 보용 지지다리 좌표계에 대응하는 지지다리 족평(22)을 다음다음다음회 지지다리 좌표계에 대응하는 위치 자세까지움직일 때의 보용에 상당한다. 이 경우, 다음다음다음회 보용 지지다리 좌표계는, 제 2 선회 보용의 미착지 다리 족평(22)의 목표 착지 위치 자세에 대응하는 것이다. 그리고 이 다음다음다음회 보용 지지다리 좌표계는, 다음다음회 보용 지지다리 좌표계(제 2 선회 보용의 지지다리 좌표계)로부터 본 이 다음다음다음회 보용 지지다리 좌표계의 위치 자세(위치 및 방향)가, 금회 보용 지지다리 좌표계로부터 본 다음회 보용 지지다리 좌표계(금회 보용의 미착지 다리 족평(22)의 착지 위치 자세)의 위치 자세(위치 및 방향)에 일치하도록 설정된다. 또한, 정상선회 보용에 관해서 「선회」라는 용어를 사용한 것은, 선회율을 0으로 할 때는 직진을 의미하므로, 직진도 광의의 의미에서 선회에 포함시킬 수 있기 때문이다.

정상 선회 보용은, 보용 생성 장치(200)에서 금회 보용의 종단에서의 발산 성분이나 상체 연직위치 속도를 결정하기 위해서 잠정적으로 작성되는 가상적인 주기적 보용이며, 로봇(1)을 실제로 제어하기 위해서 보용 생성 장치(200)로부터 그대로 출력되는 것은 아니다.

또한, 「발산」이란 상체의 위치가 양 다리부(족평)의 위치로부터 멀리 떨어진 위치로 어긋나버리는 것을 의미한다. 발산 성분의 값이란, 2족이동 로봇의 상체의 위치가 양 족부(족평)의 위치(엄밀하게는, 지지다리 접지면에 설정된 지지다리 좌표계의 원점으로부터 멀리 떨어져 가는 상태를 나타내는 수치이며, 상체(3)의 수평방향의 위치 및 그 속도의 함수로 나타내어진다.

본 실시예에서는 이제부터 생성하는 금회 보용의 뒤에 이어지는 정상보용을 이동 요구(상기 2보 앞까지의 미착지 다리의 족평(22)의 착지 위치 자세, 착지 시각 등의 요구값)에 따라 설정하고, 정상 보용의 초기 발산 성분을 구하고나서, 금회 보용의 종단 발산 성분을 정상 보용의 초기 발산 성분에 일치하도록, 금회 보용을 생성하도록 했다. S024의 상세는 본 출원인이 제안한 상기 PCT 공개 공보 WO/02/40224, 또는 PCT/JP02/13596에 설명되어 있으므로, 더 이상의 설명을 생략한다.

S010부터 S024까지에 나타나 있는 처리를 행한 후, S026으로 진행하고, 정상 선회 보용의 초기 상태(초기 상체 수평위치 속도 성분, 초기 상체 연직위치 속도, 초기 발산 성분, 초기 상체자세각 및 각속도)를 결정한다. S026의 상세는, PCT 공개 공보 WO/02/40224, 또는 PCT/JP02/13596에 설명하고 있으므로 여기에서는, 더 이상의 설명을 생략한다.

이어서, S028로 진행하고, 금회 보용의 보용 패러미터를 결정(일부 가결정)한다. 이 경우, 결정되는 금회 보용의 보용 패러미터는, 정상 선회 보용의 보용 패러미터와 같이 주로, 족평 궤도 패러미터, 기준 상체자세 궤도 패러미터, 팔자세 궤도 패러미터, 목표 ZMP 궤도 패러미터, 목표 상반력 연직성분 궤도 패러미터이며, 각각의 패러미터에 의해 규정되는 궤도가, 정상 선회 보용의 궤도에 연속하도록 결정된다. 단, 이들 패러미터중, 목표 ZMP 궤도 패러미터는 잠정적인 것이다. 이 S028의 처리의 상세는, 상기 PCT 공개 공보 WO/02/40224, 또는 PCT/JP02/13596호 등에 설명되어 있으므로, 여기에서는 더 이상의 설명을 생략한다.

이어서 S030으로 진행하고, 금회 보용의 종단 발산 성분이 정상보용의 초기 발산 성분에 일치하도록, 금회 보용의 보용 패러미터를 수정한다. 여기에서 수정되는 보용 패러미터는, 목표 ZMP 궤도 패러미터이다.

S030에서 금회 보용 패러미터를 수정한 후, 또는 S018의 판단 결과가 NO인 경우에는, S032로 진행하고 금회 보용 순시값을 결정한다. 이 처리의 상세는, 상기 PCT 공개 공보 WO/02/40224, 또는 PCT/JP02/13596호 등에 설명하고 있으므로, 여기에서는 더 이상의 설명을 생략한다.

이어서 S034로 진행하고, PCT 출원 PCT/JP02/13596호의 실시형태의 S032의 처리와 동일하게, 스핀력을 캔슬하기 위한 팔흔들기 동작을 결정한다.

이어서 S036으로 진행하고, 보용 생성용 시각 t를 △t만큼 늘리고, S014 로 되돌아가, 이상과 같이 보용 생성을 계속한다.

이상이, 보용 생성 장치(200)에서의 목표 보용 생성 처리, 및, 자기위치 자세 추정부(214)의 자기위치 자세 추정 처리이다.

도 6을 참조하여 이 실시형태에 관계되는 제어 유닛(60)의 제어 처리를 더 설명하면 보용 생성 장치(200)에서, 상기한 바와 같이 목표 보용이 생성된다. 생성된 목표 보용중, 목표 상체위치 자세(궤도) 및 목표 팔자세 궤도는 로봇 기하학 모델(역키네마틱스 연산부)(202)에 직접 보내진다.

또, 목표 족평위치 자세(궤도), 목표 ZMP 궤도(목표 전체 상반력 중심점 궤도), 및 목표 전체 상반력(궤도)(목표 상반력 수평성분과 목표 상반력 연직성분)은 복합 컴플라이언스 동작 결정부(204)에 직접 보내지는 한편, 목표 상반력 분배기(206)에도 보내진다. 목표 상반력 분배기(206)에서는, 목표 전체 상반력은 각 족평(22R, 22L)에 분배되어, 목표 각 족평 상반력 중심점 및 목표 각 족평 상반력이결정된다. 그 결정된 목표 각 족평 상반력 중심점 및 목표 각 족평 상반력이 복합 컴플라이언스 동작 결정부(204)에 보내진다.

복합 컴플라이언스 동작 결정부(204)에서는, 기구변형 보상식 수정 목표 족평위치 자세 궤도가 생성되고, 그것이 로봇 기하학 모델(202)에 보내진다. 로봇 기하학 모델(202)은, 목표 상체위치 자세(궤도)와 기구변형 보상식 수정 목표 족평위치 자세(궤도)가 입력되면, 그것들을 만족하는 다리체(2, 2)의 12개의 관절(10R(L) 등)의 관절 변위지령(값)을 산출하여 변위 콘트롤러(208)에 보낸다. 변위 콘트롤러(208)는 로봇 기하학 모델(202)에서 산출된 관절 변위지령(값)을 목표값으로 하여 로봇(1)의 12개의 관절의 변위를 추종 제어한다.

로봇(1)에 생긴 상반력(상세하게는 실제 각 족평 상반력)은 6축력센서(50)에 의해 검출된다. 그 검출값은 상기 복합 컴플라이언스 동작 결정부(204)에 보내진다. 또, 상기 도 9의 S016에서 후술하는 바와 같이 구해진 추정 상체자세와 보용 생성 장치(200)가 생성한 목표 상체자세와의 차이중 경사 성분, 즉 실제 상체자세각 편차(θerrx, θerry)가 자세 안정화 제어 연산부(212)에 보내진다. 또, θerrx는 롤링 방향(X축 둘레)의 경사 성분이며, θerry는 피칭 방향(Y축 둘레)의 경사 성분이다. 이 자세안정화 제어 연산부(212)에서, 로봇(1)의 상체자세의 경사를 목표 보용의 상체자세의 경사로 복원하기 위한 목표 전체 상반력 중심점(목표 ZMP) 주위의 보상 전체 상반력 모멘트(Mdmd)가 산출되고, 이 보상 전체 상반력 모멘트(Mdmd)가 복합 컴플라이언스 동작 결정부(204)에 주어진다. 복합 컴플라이언스 동작 결정부(204)는 입력값에 기초하여 목표 족평위치 자세를 수정한다. 구체적으로는, 복합 컴플라이언스 동작 결정부(204)에서는, 실제 전체 상반력(모든 실제 족평 상반력의 합력으로, 병진력 및 모멘트의 양자를 포함함)이, 목표 각 족평 상반력의 합력인 목표 전체 상반력과 보상 전체 상반력 모멘트(Mdmd)와의 합력에 일치하도록, 보용 생성 장치(200)로부터 주어진 목표 족평위치 자세를 수정하고, 기구변형 보상식 수정 목표 족평위치 자세(궤도)를 결정한다. 단 모든 상태를 목표에 일치시키는 것은 사실상 불가능하므로, 이들 사이에 트레이드오프 관계를 주어서 타협적으로 되도록이면 일치시킨다. 즉, 족평위치 자세 및 상반력의 각 목표에 대한 제어 편차에 가중도를 부여하여, 제어 편차(또는 제어 편차의 2승)의 가중평균이 최소가 되도록 제어한다.

보충하면, 기구변형 보상식 수정 목표 족평위치 자세(궤도)는, 복합 컴플라이언스 동작 결정부(204)에 의해 수정된 상반력의 목표값을 발생시키기 위해서 필요한 족평의 변형 기구(원주 형상 고무, 발바닥 스폰지 및 충격흡수용의 백 형상의 에어 댐퍼)의 변형량을 변형 기구의 역학 모델(스프링 댐퍼 모델 등)을 사용하여 구하고, 그 변형량이 발생하도록 수정한, 목표 족평위치 자세(궤도)이다.

이하에, 본 실시예에서의 S016의 자기위치 자세 추정 처리에 대하여, 그 플로차트인 도 10을 사용하여 상세하게 설명한다.

우선 도 10의 S2200에서, 상체(3)의 상기 경사 센서(54)에 구비한 자이로 센서의 검출값, 즉 상체(3)의 각속도(3축방향의 각속도)의 검출값을 적분기에 의해 적분하여 추정 상체자세(이하, 관성항법적 추정 상체자세라고 하는 경우도 있음)를 구한다. 이 추정 상체자세는 글로벌 좌표계로 기술된다. 또한, 이 처리에서는,전제어 주기(전회 이전의 제어 주기)에서, 후술하는 S2202부터 S2214의 처리에 의해 구한 상체 위치의 추정값인 추정 상체위치 등을 사용하여 자이로 센서의 검출값의 적분 오차의 축적에 의한 드리프트를 보정하는 드리프트 보정도 행해진다. 그 상세에 대해서는 후술한다.

이어서, S2202로 진행하고, 제어 주기의 사이(전회 제어 주기와 금회 제어 주기와의 사이)에서의 글로벌 좌표계로부터 본 추정 상체 자세(S2200에서 구하는 추정 상체자세)의 변화량과, 이 제어 주기의 사이에서의 글로벌 좌표계로부터 본 목표 보용의 상체자세의 변화량과의 차이를 자세 회전 편차 변화량으로서 산출한다. 바꾸어 말하면, 글로벌 좌표계로부터 본 추정 상체자세의 변화 속도와, 목표 상체자세의 변화 속도와의 차이를 자세 회전 편차 변화량으로서 산출한다.

또한, 글로벌 좌표계로부터 본 목표 보용의 상체자세란, 현재의 추정 지지다리 좌표계상(보다 상세하게는, 전회의 제어 주기에 결정된 추정 지지다리 좌표계상)에서, 제어 주기의 사이, 상체(3)의 자세경사나 스핀(목표 상체자세로부터의 벗어남)이 없게 로봇(1)이 목표 보용대로 운동했다고 가정한 경우의 글로벌 좌표계로부터 본 상체자세이다. 단, 추정 지지다리 좌표계란 실제의 로봇(1)의 지지다리 족평(22)의 추정 위치 자세에 대응한 지지다리 좌표계이다. 즉, 추정 지지다리 좌표계는, 상기한 지지다리 좌표계의 정의에 의해, 보다 구체적으로는, 실제의 로봇(1)의 지지다리 족평(22)을 그 추정 위치 자세로부터 접지면과의 사이에서 미끄러뜨리지 않고 수평까지 회전시켰을 때의, 이 지지다리의 발목중심으로부터 접지면으로의 수직 투영점을 원점으로 하고, 이 지지다리 족평(22)의 발끝을 향하는 수평축을 X축으로 잡고, 연직축을 Z축, 이것들에 직교하는 좌표축을 Y축에 잡은 좌표계이다.

이어서 S2204로 진행하고, 자세 회전중심을 결정한다. 구체적으로는, 그 순간의 목표 ZMP(목표 ZMP의 현재값)를 자세 회전중심으로서 결정한다. 또한, 로봇(1)의 주행에 있어서의 공중기에서는, 예를 들면 목표 보용에서의 로봇(1)의 중심을 자세 회전중심으로서 결정한다.

이어서 S2206으로 진행하고, 현재의 추정 지지다리 좌표계(전회의 제어 주기에서 결정된 추정 지지다리 좌표계에서, 도 11에 도시하는 시각 t-△t에서의 추정 지지다리 좌표계)를, 상기 자세 회전 편차 변화량만큼, 상기 자세 회전중심 주위로 회전시킨 위치·자세를 고쳐서 현재의 추정 지지다리 좌표계(도 11에 도시하는 시각 t에서의, 추정 지지다리 좌표계)로 결정한다.

또한, 추정 지지다리 좌표계의 원점 및 좌표축의 방향은 글로벌 좌표계에 의해 나타내어지는 것으로 한다. 또, S010의 초기화 처리에서, 추정 지지다리 좌표계의 초기값(글로벌 좌표계에서의 초기 위치 자세)이 세팅되어 있는 것으로 한다.

또, 추정 지지다리 좌표계의 회전중심은 공중기가 없는 통상의 보행에서는 지지다리 족평(22)의 미끄러짐 회전의 회전중심을 의미한다. 또, 달리기 등, 1보의 도중에 지지다리도 바닥으로부터 떨어져 있는 공중기에서는, 협의의 의미에서는 지지다리 족평(22)의 미끄러짐이 존재하는 것은 아니므로, 공중에서의 자세 회전(혹은 스핀)은 추정 지지다리 좌표계의 회전중심 주위의 회전으로 표현하는 것이 바람직하다. 그러나, 광의의 의미에서 「지지다리 족평의 미끄러짐」을 추정 지지다리 좌표계의 회전중심 주위의 자세 회전으로 정의해도 좋을 것이다.

보충하면, 지지다리 좌표계는 지지다리 족평(22)의 접지면에 대해 설정하는 것으로, 그 원점은 상기한 바와 같이 지지다리의 발목중심으로부터 접지면으로의 수직 투영점으로 하지 않아도 좋다. 즉, 지지다리 좌표계는 로봇(1)의 운동을 기술하기 위한 지지다리 족평(22)의 근방의 가상의 바닥에 설정된 로컬 좌표계이다. 결국, 로봇(1)의 자세 회전(또는 스핀) 현상은 로봇(1)이 상기 가상의 바닥상에서 상대적으로는 자세 회전(또는 스핀) 하지 않고 운동하고 있는 상태를 유지한 채, 로봇(1) 마다 상기 가상의 바닥을 글로벌 좌표계중에서 어떤 소정의 점을 회전중심으로 하여 자세 회전(또는 스핀)시킨 현상으로 간주된다.

또한 바꿔 말하면, 로봇(1)의 운동은 로컬 좌표계에서 목표 보용 또는 관절변위 검출값을 따라서 운동하고 있는 로봇(1)의 전체가 로컬 좌표계마다 글로벌 좌표계에 대해 어떤 소정의 점(자세 회전중심)을 회전중심으로 하여 섭동회전을 하고 있다고 간주할 수 있다.

따라서, 자세 회전중심은 추정 지지다리 좌표계의 회전중심이며 또, 상기 섭동회전의 회전중심이라고도 할 수 있다.

또, S2206에서의 자세 회전후의 현재의 추정 지지다리 좌표계의 Z축의 방향은 일반적으로는 연직방향을 향하지 않는다. 그래서, 자세 회전후에 현재의 추정 지지다리 좌표계의 Z축을 연직방향으로 되돌리도록(이 Z축의 연직방향에 대한 경사각도분 만큼, 다음회 보용추정 지지다리 좌표계를 그 원점주위로 회전시키도록) 해도 좋다. 또는, S2206의 자세 회전 처리시에 상기 자세 회전 편차 변화량중 연직축 주위의 성분(스핀 성분)만 추정 지지다리 좌표계를 회전시켜도 좋다.

이어서 S2208로 진행하고 착지 시각인지 아닌지, 즉 보용의 전환 차례인지 아닌지를 판정한다.

S2208의 판정 결과가 YES일 경우에는 이하의 처리를 행한다. 즉, 우선 S2210으로 진행하고, 도 12에 도시하는 바와 같이, 현재의 추정 지지다리 좌표계(S2206에서의 자세 회전후의 추정 지지다리 좌표계)에 대한 다음회 보용추정 지지다리 좌표계의 상대 위치 자세관계가, 목표 보용(전회보용)에서의 지지다리 좌표계에 대한 다음회 보용 지지다리 좌표계의 상대 위치 자세관계와 동일관계가 되도록, 다음회 보용추정 지지다리 좌표계를 결정한다. 또한, 여기에서 말하는 다음회 보용 지지다리 좌표계는, 이제부터 생성하려고 하는 금회 보용에 대한 다음회 보용 지지다리 좌표계가 아니라, 전회 보용의 다음 보용(즉 금회 보용)의 지지다리 좌표계이다. 또, 목표 보용의 지지다리 좌표계는 그 Z축이 연직방향을 향하는 좌표계이므로, 자세 회전후의 현재의 추정 지지다리 좌표계에 대해 상기의 상대 위치 자세관계를 중촉시키는 다음회 보용추정 지지다리 좌표계의 Z축은 일반적으로는 연직방향을 향하지 않는다. 그래서, 본 실시예에서는 자세 회전후의 현재의 추정 지지다리 좌표계에 대해 상기의 상대 위치 자세관계를 충족시키는 다음회 보용추정 지지다리 좌표계의 Z축을 연직방향으로 되돌림(이 Z축의 연직방향에 대한 경사각도분 만큼, 다음회 보용추정 지지다리 좌표계를 그 원점주위로 회전시킴)으로써 최종적으로 다음회 보용추정 지지다리 좌표계를 결정하도록 하고 있다. 또한, S2206에서, 전술과 같이 추정 지지다리 좌표의 Z축을 연직방향을 향하도록 한 경우에는S2210에서 다음회 보용추정 지지다리 좌표계의 Z축을 연직방향으로 되돌리는 처리는 불필요하다.

이어서 S2212로 진행하고, S2210에서 결정한 다음회 보용추정 지지다리 좌표계의 위치 자세(원점의 위치 및 좌표축의 방향)을 현재의 추정 지지다리 좌표계의 위치 자세에 대입한다. 즉, 현재의 추정 지지다리 좌표계의 위치 자세를 다음회 보용추정 지지다리 좌표계의 위치 자세로 치환한다.

S2212의 후, 또는 S2208의 판정 결과가 NO인 경우에는, S2214로 진행하고, 현재의 추정 지지다리 좌표계의 위치 자세에 대응하여 글로벌 좌표계에서의 상체 위치의 추정값인 추정 상체위치를 결정한다. 단, 현재의 추정 지지다리 좌표계에 대한 추정 상체위치의 관계는 목표 보용(전회의 제어 주기에서 결정된 목표 보용)의 지지다리 좌표계에 대한 상체위치(목표 상체위치)의 관계에 일치시킨다.

이어서, S2216으로 진행하고 상기 경사 센서(54)에 구비한 가속도 센서와 자이로 센서를 기초로 관성항법에 의해 관성항법적 추정 상체위치 자세를 구하면서, 기하학적 추정 상체위치와 관성항법적 추정 상체위치와의 차이가 0에 수속하도록 관성항법적 상체 위치를 수정한다. 이 처리의 상세에 대해서도 후술한다.

이상이 도 9의 S016의 자기위치 자세 추정 처리의 개요이다. 또한, S2210에서는 다음회 보용추정 지지다리 좌표계를 결정하기 위해서 목표 보용을 사용했는데, 로봇(1)이 보행을 행하는 경우에는, 다음과 같이 하여 다음회 보용추정 지지다리 좌표계를 결정하도록 해도 좋다.

즉, 먼저, 현재(착지 순간)의 로봇(1)의 관절변위 검출값을 기초로 키네마틱스 연산에 의해, 현재의 추정 지지다리 좌표계에서의 착지시의 실제 미착지 다리 족평위치·자세(도 12의 예에서는 로봇(1) 전방측의 족평(22)의 위치 자세)를 추정한다. 이후, 이 추정된 착지시의 실제 미착지 다리 족평위치·자세를 추정 착지시 미착지 다리 족평위치 자세라 부른다.

단, 이때, 로봇(1)이 현재의 추정 지지다리 좌표계상에서, 목표 보용대로의 상체 자세를 유지하면서, 현재의 추정 지지다리 좌표계상의 가상의 바닥에 대해 지지다리의 족평(22)이 미끄러지지 않고, 관절변위 검출값 대로 운동하고 있는 것으로 가정한다.

보충하면, 상기 자세 회전 편차변화량 만큼 상기 자세 회전 중심 주위로 회전시킨 현재 추정 지지다리 좌표계상에서, 로봇(1)이 목표 보용대로의 상체자세를 유지하고 있는 모습을 상정한 이유는, 이 순간에 상정한 모습에 있어서의 상체자세를 글로벌 좌표계에서, 추정 상체자세와 일치시키기 위함이다. 따라서, 현재 추정 지지다리 좌표계를 상기 차이만큼 자세 회전시키는 대신에, 상기 자세 회전 편차 변화량내의 연직축 주위 성분(스핀 성분)만 회전시키는 경우에는, 로봇(1)이 현재의 추정 지지다리 좌표계상에서, 목표 보용대로의 상체 방향을 유지하면서, 상체 경사는 상기 자세 회전 편차 변화량의 경사 성분에 일치하고, 현재의 추정 지지다리 좌표계상의 가상의 바닥에 대해 지지다리의 족평(22)이 미끄러지는 않고, 관절변위 검출값 대로 운동하고 있는 것으로 가정한다.

또, 이 때, 6축력 센서(50)에 의한 상반력 검출값과 목표 보용의 상반력중 적어도 어느 하나를 기초로, 족평(22)의 변형 기구(상기 도 3 및 도 4에 도시한 탄성부재(106), 접지 부재(발바닥 탄성체)(71), 백 형상 부재(109) 등)의 변형량을 변형 기구의 역학모델(스프링 댐퍼 모델 등)을 사용하여 구하고, 그 변형량을 포함하여(고려하여), 추정 착지시 미착지 다리 족평위치 자세를 구해도 좋다. 보다 구체적으로는, 도 12에 도시하는 로봇(1)의 모습과 같이, 현재의 추정 지지다리 좌표계에서 상체자세를 목표 상체자세로 하고, 관절변위를 관절변위 검출값(제 j 관절변위 검출값 θj, j=1, 2…)으로 하고, 변형 기구의 변형량을 상반력 검출값을 기초로 추정한 변형량으로 한 경우의 전체 모습(그 순간의 로봇(1)의 전체의 모습)을 구하고, 이 전체 모습에서의 지지다리(도 12에서는 로봇(1)의 후방측의 다리체(2))에 대한 미착지 다리 족평위치 자세를 구하고, 이것으로써 추정 착지시 미착지 다리 족평위치 자세로 하도록 해도 좋다. 또, 모터 전류지령 또는 검출값을 기초로 외란 옵저버를 사용하여, 감속기나 링크에 작용하는 부하를 추정하고, 추정한 부하를 기초로 상기 감속기나 링크의 변형을 추정하고, 상기 감속기나 링크의 변형을 포함하여(고려하여), 추정 착지시 미착지 다리 족평위치 자세를 구하도록 해도 좋다.

또는, 적어도 목표 보용의 관절변위(관절변위 목표값)와 컴플라이언스 보상량(본 출원인이 앞서 제안한 일본 특개평 10-277969호를 참조)을 기초로, 키네마틱스 연산에 의해 추정 착지시 미착지 다리 족평위치 자세를 구하도록 해도 좋다.

이것들에 의해, 더한층 정밀도 좋게 추정 착지시 미착지 다리 족평위치 자세를 구할 수 있다.

또, 로봇(1)의 보행을 행하고 있는 경우에는, S2214의 처리에서는 상기 추정착지시 미착지 다리 족평위치 자세를 구한 수법과 동일한 수법을 사용하여, 현재의 로봇(1)의 각 관절의 관절변위 검출값을 기초로, 키네마틱 연산 연산에 의해 지지다리 좌표계에 대한 상체위치를 구하고, 이 위치 관계에, 현재의 추정 지지다리 좌표계에 대한 추정 상체위치의 위치 관계가 일치하도록, 추정 상체위치를 결정하도록 해도 좋다. 이 경우, 추정 상체위치, 추정 지지다리 좌표계 및 관절변위 검출값의 관계는 도 12에 도시하는 바와 같은 관계가 된다.

또는, 적어도 목표 보용의 관절변위(관절변위 목표값)와 컴플라이언스 보상량(본 출원인이 앞서 제안한 일본 특개평 10-277969호를 참조)을 기초로, 키네마틱스 연산에 의해 지지다리 좌표계에 대한 상체위치를 구하고, 이것에 추정 지지다리 좌표계에 대한 추정 상체위치의 관계를 일치시켜도 좋다.

이것들에 의해, 더한층 정밀도 좋게 추정 상체위치를 결정할 수 있다.

다음에 S2200의 처리 및 S2216의 처리를 도 13을 참조하면서 상세히 설명한다. 또한, 이후의 설명에서는, S2214에서 전술한 바와 같이 결정되는 추정 상체위치를 기하학적 추정 상체위치라고 한다.

S2200의 처리를 상세하게 설명하면, 우선, 전회의 제어 주기 및 그 이전에, 전술과 같이 결정한 기하학적 추정 상체위치의 운동을 기초로, 기하학적 추정 상체위치의 2차미분인 기하학적 추정 상체가속도를 산출한다. 이 처리는 도 13의 블록301에서 실행된다. 또한, 기하학적 추정 상체위치를 구하는 처리는 도 13의 블록300에서 실행되는 것으로 한다. 보충하면, 상체 대표점과 가속도 센서의 위치가 일치해 있으면, 기하학적 추정 상체가속도(엄밀하게는, 기하학적으로 추정되는 상체(3)중의 가속도 센서 위치의 가속도)를 산출하는 점에서, 추정 상체자세는 필요없다.

이어서, 가속도 센서 검출값(상체가속도 검출값)을 추정 상체자세를 사용하여 글로벌 좌표계로 변환하여 이루어지는 가속도 센서 검출값 글로벌 좌표계환산값을 구한다. 이 처리는, 도 13의 블록302에서 실행된다. 또한, 설명을 간단하게 하기 위해서, 여기에서는, 가속도 센서의 위치와 상체 대표점의 위치는 일치하고 있는 것으로 한다. 다른 경우에는, 키네마틱스 연산에 의해, 가속도 센서 검출값 및 자이로 센서 검출값을 상체 대표점에서의 가속도·각속도로 변환하든지, 또는, 상체 대표점에서의 가속도·각속도를 가속도 센서의 위치에서의 가속도, 자이로 센서의 위치에서의 각속도로 변환하면 된다.

이어서, 도 14(a) 및 도 14(b)에 도시하는 바와 같이 가속도 센서 검출값 글로벌 좌표계 환산값으로부터 기하학적 추정 상체가속도를 뺌(벡터의 감산을 행함)으로써, 추정 중력가속도를 구한다. 가속도 센서 검출값 글로벌 좌표계 환산값에는, 가속도 센서에 작용하는 중력에 의한 가속도 성분이 포함되지만, 기하학적 추정 상체가속도에는 중력에 의한 가속도 성분이 포함되지 않는다. 따라서 가속도 센서 검출값 글로벌 좌표계 환산값으로부터 기하학적 추정 상체가속도를 뺌으로써 추정 중력가속도가 구해진다. 이 처리는, 도 13의 블록303에서 실행된다. 또한, 도 14(a)는 추정 상체자세에 오차가 없을 경우, 도 14(b)는 추정 상체자세에 오차(이후, 이것을 추정 상체자세 오차, 또는 추정 상체자세 오차각도라 부름)가 있는 경우를 도시한다.

한편, 도 14(a) 및 도 14(b)에 도시하는 바와 같이, 추정 상체자세를 기초로 로봇(1)이 추정한 글로벌 좌표계의 연직방향(Z축방향)에 발생하는 상정되는 중력가속도를 상정 중력가속도라 부른다.

전술한 바와 같이, 기하학적 추정 상체가속도는 글로벌 좌표계로 기술된다. 또, 실제의 로봇(1)은 추정 상체자세가 실제의 상체자세를 정확하게 추정하고 있다고 간주한 뒤에, 목표 보용에 추종하도록 운동하고 있다. 바꿔 말하면, 추정 상체위치 자세를 기초로 추정되는 글로벌 좌표계가 정확하다고 간주한 뒤에, 상기 추정된 글로벌 좌표계상에서 목표 보용에 추종하도록 운동하고 있다. 보다 엄밀하게 표현하면, 로봇(1)의 자기위치 자세 추정 처리에 있어서는, 추정 상체위치 자세를 기초로 추정되는 글로벌 좌표계가 정확하다고 믿고, 실제의 로봇(1)의 거동을, 옳다고 믿고 있는 글로벌 좌표계에 설정되는 로컬 좌표계인 추정 지지다리 좌표계상에서 목표 보용대로 운동하고 있는 로봇(1)이, 각 순간에, 자세 검출 수단에 의해 얻어진(검출 혹은 추정된) 상체자세의 변화 속도와 목표 보용의 상체자세변화 속도의 차이로서의 자세 회전 편차 변화량에서 자세 회전중심 결정 수단에 의해 결정된 그 순간의 소정의 자세 회전중심 주위로, 상기 추정 지지다리 좌표계와 함께, 회전한 것이라고 간주하고 있다.

따라서, 추정 상체 자세가 정확하고 또한 가속도 센서의 검출값에 오차가 없으면, 도 14(a)에 도시하는 바와 같이 상기 상정 중력가속도와 상기 추정 중력가속도와는 일치한다.

이에 대하여 가속도 센서의 검출값에 오차가 없어도, 추정 상체자세에 오차가 있으면, 도 14(b)에 도시한 바와 같이, 상기 상정 중력가속도의 방향과 상기 추정 중력가속도의 방향에는 추정 상체자세 오차각도 만큼 벗어남이 생긴다. 즉, 식42가 성립한다.

추정 상체자세 오차각도 = 상정 중력가속도와 추정 중력가속도 사이의 각도 …식42

도 14(a) 및 도 14(b)에 도시하는 바와 같이, 추정 중력가속도와 상정 중력가속도와의 차이를 중력가속도 추정 오차라 부르면, 상기의 관계로부터, 중력가속도 추정 오차는, 가속도 센서의 검출값에 오차가 없으면, 추정 상체자세의 오차에 의해 발생하고, 추정 상체 자세를 기초로 로봇(1)이 추정한 글로벌 좌표계로부터 본 중력가속도 추정 오차의 수평성분(추정 중력가속도의 상정 중력가속도에 직교하는 성분)과 추정 상체자세 오차각도에는, 식43의 관계가 있는 것을 알 수 있다. 단, 식43은 중력가속도 추정 오차의 전후방향 성분(X성분)과 추정 상체자세 오차각도 Y축 주위 성분과의 관계를 나타낸다. 중력가속도 추정 오차의 좌우방향 성분(Y성분)과 추정 상체자세 오차각도 X축 주위 성분의 관계를 나타내는 경우에는, 우변의 마이너스 기호를 삭제하면 좋다. 여기에서, 중력가속도는 플러스이다.

중력가속도 추정 오차의 수평성분

= -tan(추정 상체자세 오차각도)*중력가속도

…식43

본 실시예에서는, 식42 또는 식43으로부터 산출되는 추정 상체자세 오차각도를 사용하여, 추정 상체자세 오차가 0에 수속하도록 추정 상체자세를 보정하도록했다.

S2200의 처리의 설명으로 돌아가면, 식42를 사용하여, 현재의 순간(엄밀하게는, 1제어 주기전의 순간)에서의 상정 중력가속도와 추정 중력가속도 사이의 각도로부터 추정 상체 자세 오차각도를 산출한다. 또는, 식43을 사용하여, 현재의 순간(엄밀하게는, 1제어 주기전의 순간)에서의 중력가속도추정 오차의 수평성분으로부터 추정 상체자세 오차각도를 산출해도 좋다. 추정 상체자세 오차각도의 산출 처리는, 도 13의 블록304에서 실행된다.

이어서, 상기 추정 상체자세 오차각도를, 도 13의 블록305에서, 센서 좌표계(좌표축을 자이로 센서의 검출축에 맞춘 좌표계)로 변환한 후, 블록306에서, 그 변환후의 값으로 적분 게인 Ka을 곱한 값을 적분함으로써, 추정 자이로센서 드리프트(자이로 센서의 드리프트의 추정값)를 구한다. 또한, 이 추정 자이로센서 드리프트를, 블록307에서 자이로 센서 검출값(ωin)(상체각속도 검출값)으로부터 뺌으로써, 드리프트를 보정한 각속도를 구한다. 또한, 블록307에서는, 요잉레이트의 드리프트도 적당하게 감산되지만, 이것에 대해서는 후술한다. 또한, 이 드리프트를 보정한 각속도를, 블록350에서, 추정 상체자세를 사용하여 글로벌 좌표계로 변환함으로써, 글로벌 상체각속도(ωgl)를 구한다.

이어서, 상기 추정 상체자세 오차각도에 게인(Kb)을 도 13의 블록309에서 곱한 값을 블록308에서 글로벌 상체각속도(ωgl)로부터 빼고, 그 감산후의 값(블록308의 출력)을 블록310(적분기)에서 적분함으로써 새로운 추정 상체자세(θestm)를 구한다.

또한, 추정 상체자세(θestm)나 각속도 등은, 쿼터니온, 회전 행렬, 또는 오일러 각 등으로 표현된다.

자이로 센서의 드리프트(추정 자이로센서 드리프트)를 상기와 같은 구성에 의해 추정하는 경우, 추정이 효과적으로 작용하기 위해서는, 추정 상체자세 오차각도의 센서 좌표계에의 변환값(벡터)의 각각의 요소(각각의 센서 검출축 주위의 추정 상체자세 오차각도, 바꿔 말하면 센서 로컬 추정 상체자세 오차각도)가, 그 요소에 대응하는 자이로센서의 드리프트의 영향만을 받고, 다른 자이로 센서의 드리프트의 영향을 받지 않거나, 또는, 거의 받지 않는 것이 전제조건이 된다.

바꿔 말하면, 추정 상체자세 오차각도의 X(Y)축 주위 성분의 오차는, X(Y)축용 자이로 센서의 드리프트의 영향을 받지만, Y(X)축용 자이로 센서의 드리프트의 영향을 받지 않는 것이 전제조건이 된다.

만약, X축용 자이로 센서의 드리프트의 영향에 의해 추정 상체자세 오차각도의 X축 주위 성분이 발생한 후, 상체를 센서 좌표계의 Z축 주위로 급히 90도 회전시키면, 추정 상체자세 오차각도는 글로벌 좌표계에서 축적된 채이므로, 결과적으로 추정 상체자세 오차각도의 센서 로컬 X축 주위 성분이, 추정 상체자세 오차각도의 센서 로컬 Y축 주위 성분으로 이동한다. 따라서, 상기 전제조건이 성립하려면, 센서 좌표계 Z축 주위의 회전속도의 절대값이 충분하게 작은 것이 필요조건이 된다.

따라서, Z축(상체의 상하방향 축)용 자이로 센서 검출값의 절대값이 큰 때에는, 적분 게인(Ka)을 작게 하든지, 또는 0으로 하는(즉 자이로 센서의 드리프트 보정을 행하지 않는) 것이 바람직하다.

또, 통상, 로봇(1)의 이동시나 작업시에는, 상체(3)의 상하 축(체간축)은 연직자세 또는 그 근방의 자세로 되어 있으므로, 자이로 센서의 각속도 검출값을 글로벌 좌표계로 변환한 글로벌 상체각속도(ωglz)는 Z축용 자이로 센서의 각속도 검출값의 값에 가까운 값으로 된다.

따라서, 도 15에 도시하는 바와 같이, 자이로 센서의 각속도 검출값을 글로벌 좌표계로 변환한 글로벌 상체각속도의 연직축 주위 각속도(ωglz)의 절대값이 클 때에는, 적분 게인(Ka)을 작게 하든지, 또는 0으로 하도록(즉 자이로 센서의 드리프트 보정을 행하지 않도록) 해도 좋다.

또, 자이로 센서의 각속도 검출값 대신에, 상체(3)의 목표 보용의 상체회전속도의 연직축 주위 성분 또는 상체(3) 상하방향 축 주위 성분의 절대값이 클 때에는, 적분 게인(Ka)을 작게 하든지, 또는 0으로 하도록(즉 자이로 센서의 드리프트 보정을 행하지 않도록) 해도 좋다.

이상을 정리하여 바꿔 말하면, 자이로 센서의 각속도 검출값 및 목표 보용중 적어도 어느 하나를 기초로 구해지는 상체 회전속도의 연직축 주위 성분 또는 상체(3) 상하방향축 주위 성분의 절대값이 큰 때에는, 적분 게인(Ka)을 작게 하거나, 또는 0으로 하면(즉 자이로 센서의 드리프트 보정을 행하지 않으면) 좋다.

또, 가속도 센서 검출값 글로벌 좌표계 환산값으로부터 상정 중력가속도를 뺀 값의 절대값 또는, 기하학적 추정 상체가속도의 절대값이 큰 경우에는, 센서의 검출 오차나 로봇(1) 본체의 강성 부족 등에 의해, 추정 중력가속도의 오차가 커지는 경향이 있으므로, 상기 게인(Ka)를 작게, 또는 0으로 설정하는 것이 좋다.

또, 전술한 바와 같이, 실제의 로봇(1)은 추정 상체위치 자세를 기초로 추정되는 글로벌 좌표계가 정확하다고 간주한 다음에, 상기 추정된 글로벌 좌표계상에서 목표 보용에 추종하도록 운동하고 있다. 따라서, 지지다리가 접지하고 있는 상태에서는, 추정 상체자세에 큰 오차가 있어도, 실제의 상체가속도는 실제의 중력가속도에 지배되지 않고, 상기 추정된 글로벌 좌표계상에서의 기하학적 추정 상체가속도에, 거의 일치한다. 한편, 공중기에서는, 실제의 상체가속도는, 실제의 중력가속도에 지배되어서 가속되므로, 상기 추정된 글로벌 좌표계상에서의 기하학적 추정 상체가속도의 방향과 크게 벗어나, 도 14(b)의 관계가 성립하지 않게 된다. 따라서 공중기에서는, 기하학적 추정 상체위치의 정밀도는 비교적 고정밀도의 가속도 센서와 자이로 센서를 기초로 관성항법에 의해 구한 상체위치의 정밀도보다도 저하되기 쉽게 된다. 따라서, 공중기에서도, 상기 게인(Ka)을 작게, 또는 0으로 설정하는 것이 좋다.

또, 상기한 바와 같이 족평(22)의 변형을 고려하여 상체위치를 추정하는 경우에는, 공중기가 아니더라도, 다리체 선단부의 족평(22)이 바닥과 양호하게 접촉하고 있지 않은 상태에서는, 발바닥 부근의 변형량 추정의 오차가 커지는 경향이 있으므로, 기하학적 추정 상체가속도의 정밀도는 저하된다. 따라서, 지지다리의 족평(22)과 바닥 사이의 접촉 면적이 작은 경우에도, 상기 게인(Ka)을 작게, 또는 0으로 설정하는 것이 좋다.

또한, 다리체(2)의 족평(22)이 바닥과 양호하게 접촉하고 있는 상태란, 구체적으로는, 이하와 같은 상태중 적어도 어느 하나가 만족되는 상태, 또는 이하중 복수가 동시에 만족되는 상태를 가리킨다.

a) 족평(22)의 바닥면과 바닥과의 사이의 접촉 면적이 큰 상태, 예를 들면 족평(22)이 바닥에 완전히 접촉해 있는 상태(소위 편평족 상태)

b) 상반력의 분포압이 비교적 균일한 상태

c) 접지 개소가 많은 상태

d) 목표 ZMP(또는 실제 상반력 중심점)가 족평(22)의 중앙 또는 중앙에 가까운 상태

e) 접지압 또는 상반력이 높은 상태

f) 족평(22)의 위치 자세 변화율이 작은 상태, 엄밀하게는, 족평(22)의 바닥면과 바닥의 상대 위치 자세관계의 변화가 작은 상태

상기 상태의 판정에는, 예를 들면 목표 보용의 시기(페이즈), 목표 ZMP, 상반력 검출값을 기초로 행하면 좋다. 또는, 본 실시예에서는 구비하고 있지 않지만, 분포압 센서나 접촉 센서의 검출값을 기초로 상기 상태의 판정을 행해도 좋다.

이상의 게인 설정법을 정리하면, 자이로 센서의 각속도 검출값 및 목표 보용중 적어도 어느 하나를 기초로 구해지는 상체 회전속도의 연직축 주위 성분 또는 상체(3) 상하방향축 주위 성분의 절대값이 큰 상태, 가속도 센서 검출값 글로벌 좌표계 환산값으로부터 상정 중력가속도를 뺀 값의 절대값 또는, 기하학적 추정 상체가속도의 절대값이 큰 상태, 공중기, 또는, 다리 선단부(족평)가 바닥과 양호하게 접촉해 있지 않은 상태에서는, 상기 게인(Ka)을 작게, 또는 0에 설정하는 것이 좋다.

즉, 이것들의 상태를 기초로 종합적으로 판단하여 적분 게인(Ka)을 결정하는 것이 좋다.

또 이것들의 상태의 순시값, 또는 장기적 경향에 따라 적분 게인(Ka)을 결정해도 좋다.

동일한 이유에 의해, 적분 게인(Kb)도 상기 게인(Ka)과 동일하게 설정하는 것이 바람직하다.

본 실시예에서의 S2200에서는, 이상의 처리 이외에, 이하와 같이, 상황에 따라 요잉레이트 보정(요잉 방향의 드리프트 보정)도 행해진다.

즉, 우선, 요잉레이트 보정을 행할지 아닐지를, 도 13의 블록311에서 판정한다.

구체적으로는, 이하의 조건중 적어도 어느 하나 또는 복수를 판단 조건으로서 준비하고, 이 판단 조건을 만족하고 있는 경우에는, 요잉레이트 보정을 행한다고 판단한다.

a) 검출된 상반력의 연직축 주위 모멘트(또는 그 미분값)의 절대값이 어떤 소정의 값 이내인 상황

b) 목표동작이 거의 정지인 상황

c) 발바닥과 바닥의 접촉 상황이 양호하여 미끄러짐이 생기고 있지 않다고 생각되는 상황

d) 목표 보용의 자세각속도와 자이로 검출값(자세각속도 검출값)의 차이의절대값이 어떤 소정의 값 이내인 상황

e) 목표 보용의 자세각속도의 절대값이 어떤 소정의 값 이내인 상황

f) 자이로 검출값(자세각속도 검출값)의 절대값이 어떤 소정의 값 이내인 상황

기본적으로는, 지지다리 족평(22)과 바닥과의 접지면에서 미끄러짐(회전 미끄러짐)이 생기지 않거나 혹은 생기기 어려운 상황에서, 요잉레이트 보정을 행한다고 판단한다.

또한, d) 및 f)에서의 자이로 검출값은, 자이로 센서의 검출값 바로 그것(자이로 센서의 출력상태 그대로가 나타내는 자세각속도 검출값)을 요잉레이트 보정한 값인 것이 바람직하다.

이어서, 요잉레이트 보정을 행한다고 판정된 경우에는, 도 13에 도시하는 바와 같이 블록312에서, 목표 보용, 목표 상체자세, 목표 관절변위 또는 관절변위 검출값중 적어도 어느 하나와 최신의 착지시에 기억한 추정 지지다리 좌표계(이후, 착지시 추정 지지다리 좌표계라 부름)르 기초로, 추정 지지다리 좌표계에 대응하는 족평(22)과 바닥 사이에 미끄러짐이 발생하고 있지 않으면 상정한 경우의 상체자세(이후, 미끄러짐 없는 추정 상체자세라 부름)를 산출한다. 그리고, 상기 추정 상체자세와 상기 미끄러짐 없는 추정 상체자세와의 차를 블록313에서 구하고, 이 차를 블록314에서 센서 좌표계로 변환한 것을, 피드백 제어칙의 블록315에 입력함으로써 요잉레이트 드리프트를 구한다. 블록315의 피드백 제어칙으로서는, 예를 들면 PID 제어칙이 사용되고, 상기 차(블록314의 출력)의 시간 미분값(또는 제어 주기의 사이의 상기 차의 변화량)에 게인(Ke)을 곱한 값과, 상기 차(블록314의 출력)에 게인(Kf)을 곱한 값과, 상기 차(블록314의 출력)의 적분값에 게인(Kg)를 곱한 값과의 합을 구함으로써 요잉레이트 드리프트가 구해진다. 그리고, 이 구한 요잉레이트 드리프트를 상기 블록307에서 상기 각속도 센서 검출값(ωin)으로부터 뺀다. 또, 요잉레이트 보정을 행하지 않는다고 판정된 경우에는, 피드백 제어칙의 블록315에의 입력을 차단하여(도 13의 스위치(316)을 개방함), 직전의 요잉레이트 드리프트 값을 유지하고, 그 값을 상기 각속도 센서 검출값(ωin)으로부터 빼도록 한다. 또한, 미끄러짐 없는 추정 상체자세를 구할 때에는, 도 10의 S2210에서 착지시의 미착지 다리 족평위치·자세를 추정하는 경우와 동일하게, 상반력 검출값 및/또는 목표 보용의 상반력을 기초로, 족평(22)의 변형 기구(상기 도 3 및 도 4에 도시한 탄성부재(106), 접지 부재(발바닥 탄성체)(71), 자루형상 부재(109) 등)의 변형량을 변형 기구의 역학모델(스프링 댐퍼 모델 등)을 사용하여 구하고, 상기 변형량을 포함하여(고려항), 미끄러짐 없는 추정 상체자세를 구해도 좋다. 또, 모터 전류지령 또는 검출값을 기초로 외란 옵저버를 사용하여, 감속기나 링크에 작용하는 부하를 추정하고, 추정한 부하를 기초로 상기 감속기나 링크의 변형을 추정하고, 상기 감속기나 링크의 변형을 포함하여(고려하여), 미끄러짐 없는 추정 상체자세를 구하도록 해도 좋다.

또는, 적어도 목표 보용의 관절변위와 컴플라이언스 보상량(본 출원인이 앞서 제안한 일본 특개평 10-277969호를 참조)을 기초로, 키네마틱스 연산에 의해 미끄러짐 없는 추정 상체자세를 구하도록 해도 좋다.

이것들에 의해, 더한층 정밀도 좋고 미끄러짐 없는 추정 상체자세를 구할 수 있다.

다음에 도 10의 S2216의 처리를 도 13을 참조하여 상세하게 설명한다. S2216의 처리에서는, 도 13에 도시하는 바와 같이 관성항법적 추정 상체위치(Xinertestm)의 전회 값과 상기 기하학적 추정 상체위치와의 차를 블록317에서 구하고, 이 차의 시간미분값(또는 제어 주기의 사이의 상기 차의 변화량)에 게인(Kc)을 편승한 값과, 이 차에 게인(Kd)을 곱한 값과의 합을 블록318에서 구한다. 즉, 상기 차로부터 피드백 제어칙로서의 PD 제어칙으로부터, 상기 합을 구한다. 또, 상기 블록302의 출력인 상기 가속도 센서 검출값 글로벌 좌표계 환산값과 상정 중력가속도(G)의 차를 블록319에서 구한다. 그리고 이 차(블록319의 출력)로부터 상기 합(블록318의 출력)을 블록360에서 뺀 값을, 블록320에서 2차 적분함으로써 새로운 관성항법적 추정 상체위치(Xinertestm)를 결정한다.

또한, 게인(Kc 및 Kd)는 기하학적 추정 상체위치의 오차가 크게 발생하기 쉬운 상황에서는 작고, 또는 0으로 설정하는 것이 좋다. 지지다리의 족평(22)의 바닥면과 바닥과의 사이의 접촉 면적이 작은 경우, 기하학적 추정 상체위치의 오차는 커진다. 따라서 지지다리의 족평(22)의 바닥면과 바닥과의 사이의 접촉 면적이 작은 경우에는, 상기 게인(Kc 및 Kd)를 작게, 또는 0으로 설정하는 것이 좋다. 또, 특히 족평(22)이 바닥에 완전히 접촉해 있는 상태(소위 편평족 상태)시에, 상기 게인(Kc 및 Kd)을 크게하는 것이 좋다.

이상으로부터, 예를 들면 주행의 경우에는, 보정 게인(Ka, Kb, Kc 및 Kd)는도 8(c)에 도시하는 보정 게인(K)의 그래프와 같이, 발바닥 전체면 접지기간에서는 높고, 공중기에는 0 또는 대강 0으로 하면 좋다. 또한 도 8(c)의 보정 게인(K)의 그래프는 Ka, Kb, Kc 및 Kd중 크기의 변화의 경향을 나타내고는 있지만, 엄밀한 값을 나타내고 있는 것은 아니다. 또, 도 8(c)에서는 보정 게인(K)의 최대값이 1이 되도록 규격화 되어 있다. 따라서 K는 보정 게인(Ka, Kb, Kc, Kd)의 좁힘부(감쇠기)를 의미한다고 생각해도 좋다.

본 실시예에서는, 상기 식42 또는 식43을 사용하여 산출되는 추정 상체자세 오차를 기초로 추정 상체자세를 보정하도록 했는데, 식42 및 식43을 사용하지 않고, 직접, 중력가속도 추정 오차의 수평성분을 기초로 추정 상체자세를 보정해도 좋다. 즉, 식43 대신에 다음 식44를 사용해도 좋다.

중력가속도 추정 오차의 수평성분 = -추정 상체자세 오차각도*중력가속도

…식44

또, 게인(Ka)의 설정에 대하여 보충하면, 공중기에서는 추정 상체자세의 오차가 어떠한 값이더라도, 로봇(1)과 함께 포물운동을 하는 좌표계로부터 보면 로봇(1)전체는 무중력상태에 있는 것과 동일하고, 가속도 센서의 출력은 추정 상체자세의 오차의 영향을 받지 않는다. 따라서 가속도 센서의 검출 정밀도가 높고, 목표 보용에 대한 실제의 로봇(1)의 추종성이 높고, 로봇(1)의 강성이 높고, 또한 목표 보용을 생성하기 위해 사용되는 로봇 동력학 모델의 패러미터가 실제의 로봇(1)과 거의 일치하고 있다는 조건(이하, 여기에서는 조건 A라고 함)이 만족되면, 추정 중력가속도와 상정 중력가속도와는 항상 거의 일치한다. 엄밀하게는, 추정 상체자세의 오차에 따라 추정 중력가속도와 상정 중력가속도와는 동일한 양만큼 참값으로부터 벗어나므로, 결과적으로 추정 중력가속도와 상정 중력가속도와는 항상 거의 일치한다. 따라서 본질적으로, 공중기에서는 중력방향을 추정할 수 없다. 그러나, 추정 중력가속도와 상정 중력가속도와는 항상 거의 일치하므로, 그 순간의 추정 상체자세 오차는 거의 0이 되고, 상기 게인(Ka)을 작게 설정하지 않아도, 추정 상체자세 오차에 게인(Ka)을 곱하여 구해지는 보정량도 거의 0이 되어, 추정 상체자세에 크게 악영향을 끼칠 우려는 낮다.

또, 상기 조건 A를 만족하는 경우에는, 게인(Ka)을 일정하게 해 두는 편이 오히려 추정 상체자세의 저주파수 성분(DC 성분)의 정밀도가 높아지는 경우도 있다. 왜냐하면, 상기와 같이, 게인(Ka)을 곱한 값을 적분한 값을 0으로 하도록 보정한 구성은, 게인(Ka)를 일정하게 하면 이론상, 중력가속도 추정 오차의 수평성분의 장시간 평균값을 0으로 하도록 작용하고, 또, 추정 상체자세 오차가 0일 때에는, 중력가속도 추정 오차의 수평성분의 장시간 평균값은 이론상 0이 되기 때문이다. 그러나, 통상은 상기 조건 A를 만족하는 것은 어려우므로 도 8(c)와 같이 게인(Ka)을 설정하는 편이 바람직하다.

또, 보정 게인(Ka, Kb, Kc 혹은 Kd)의 값을 정상 칼만 필터 또는 또는 비정상 칼만 필터의 수법을 사용하여 결정해도 좋다. 단, 본 실시형태의 로봇(1)에서는, 특히 이동중에 있어서는 시스템 잡음(외란)과 관측 잡음의 성질이 칼만 필터의 전제조건을 충분하게 만족하고 있지는 않으므로, 충분한 효과를 낳는다고는 할 수 없다.

다음에 상기 실시형태에서 결정한 자세 회전중심에 대해, 이하에 보충한다. 어느 하나의 다리체(2)가 접지하고, 충분히 큰 상반력이 발생하고 있는 시기에 있어서는, 소위 지지 다각형(접지면을 포함하는 최소 볼록 다각형, ZMP 존재가능 범위 또는 전체 상반력 중심점 존재가능 범위)중에, 자세 회전중심이 있다고 생각된다.

이 이유를 이하에 기술하면, 자세 회전중심이 지지 다각형의 밖에 있으면 족평(22)의 바닥면의 모든 점이 바닥에 대해 상대적으로 이동하고 있게 되므로, 족평(22)의 바닥면과 바닥 사이의 모든 접촉점에는 동마찰이 작용한다. 동마찰은 로봇(1)이 상반력을 제어하려고 하여, 다리체(2)를 움직여도 일정한 값인 채이고 임의의 값으로 제어할 수는 없다. 즉, 얼음위에서 주르르 미끄러져서 잘 걸을 수 없는 상태와 같다. 따라서 로봇(1)이 상반력을 제어하면서 안정하게 이동하고 있는 경우에는, 족평(22)의 바닥면과 바닥과의 사이의 어떤 영역에서는, 미끄러짐이 생기지 않고 정마찰이 작용하고 있다고 생각할 수 있다. 즉, 지지 다각형중에 자세 회전중심이 있다고 생각할 수 있다. 보충하면, 로봇(1)의 족평(22)이 완전 강체이면 자세 회전중심점 이외의 모든 접촉점은 미끄러짐이 생기지만, 실제의 족평(22)의 바닥면은 고무 등의 탄성체로 되어 있으므로, 자세 회전중심의 근방에서도 미끄러짐은 생기지 않는다고 생각할 수 있다.

또, 공중기에서는 로봇은 중심(重心)을 중심으로하여 섭동운동을 한다고 생각할 수 있다.

또, 상기 이외의 시기 즉 어느 하나의 다리체(2)가 접지해 있지만, 상반력이충분히 발생하고 있지 않은 시기에서는, 운동의 연속성을 고려하면 자세 회전중심은 지지 다각형과 전체 중심위치(혹은 상체 대표점 위치)와의 사이에 존재한다고 생각할 수 있다.

이상의 고찰로부터, 상기 도 10의 S2204에서 결정하는 자세 회전중심으로서는 보다 일반적으로는 이하중 어느 하나인 것이 바람직하다.

현재시각 t에 있어서

a) 목표 ZMP,

b) 실제 ZMP(즉, 실제 상반력 중심점, 또는 상반력의 압력중심점),

c) 지지다리 좌표계의 소정의 점, 예를 들면 원점

d) 전체 중심점,

e) 상체 대표점

f) 상기중 어느 하나의 복수의 점의 내분점

어느 하나의 다리가 접지하고, 충분히 큰 상반력이 발생하고 있는 시기에는, 자세 회전중심은 지지 다각형중에 설정하는 것이 좋다. 구체적으로는, 상기 a) 및 b)에 설정하면 좋다. 또는, 자세 회전중심이 지지 다각형중에 포함되도록, 상기 c)에서의 소정의 점을 설정하면 좋다. 예를 들면 지지다리 좌표계의 원점(보통, 발목관절 아래)에 설정하면 좋다.

공중기에서는 자세 회전중심은 상기 d)와 같이 설정하는 것이 좋지만, 전체 중심위치는 상체 대표점 위치의 근처에 존재하므로, 상기 e)와 같이 설정해도 좋다.

상기 이외의 시기, 즉 어느 하나의 다리체(2)가 접지하고 있지만, 상반력이 충분하게 발생하고 있지 않는 시기에 있어서는, 상기 f)와 같이 설정하는 것이 좋다.

또한, 전체기간에서의 운동의 연속성을 고려하면, 자세 회전중심점은 연속적으로 변화되도록 설정하는 것이 바람직하다.

결국, 대강 모든 각 순간에 있어서, 로봇(1)의 전체를 포함하는 최소 볼록면체의 면위 또는 내부에 자세 회전중심점을 설정하는 것이 바람직하다.

또한, 이상에서 설명한 실시예에서는 가속도 센서 및 또는 자이로 센서는 상체(3)이외의 부위, 예를 들면 머리부(4)에 탑재(내장)되어 있어도 좋다. 또한, 머리부(4)와 상체(3) 사이에 목관절이 있는 경우, 목관절의 변위지령(목표변위) 또는 변위 검출값을 기초로, 가속도 센서 및 또는 자이로 센서의 검출값을 키네마틱스 연산에 의해, 상체 대표점의 가속도와 각 가속도로 변환하기만 하면 나머지는, 상기 각 실시형태와 동일하게 자기위치 추정을 행하면 된다.

또, 추정 지지다리 좌표계 및 기하학적 추정 상체위치를 결정할 때에 관절변위를 사용하는 경우에는, 그 관절변위를 목표 보용의 관절변위 또는, 관절변위 검출값으로 했지만, 이것들의 가중평균이라도 좋다. 또, 이때의 가중도는 주파수특성을 갖게 해도 좋다.

추정 지지다리 좌표계나 추정 상체위치 자세 등, 추정되는 위치 자세를 표현할 때에는, 상기 실시형태와 같이 글로벌 좌표계를 기준으로 하여 표현하는 대신에, 목표위치 자세로부터의 섭동분으로 표현해도 좋다.

또, 상기 실시형태에서는, 상기 도 13에 도시하는 바와 같이, 글로벌 좌표계에서의 추정 중력가속도를 기초로, 글로벌 좌표계에서의 추정 상체자세 오차각도를 구하고, 글로벌 좌표계에서의 추정 상체자세 오차각도에 게인(Kb)을 곱한 값을, 글로벌 상체각속도(ωgl)을 적분하는 적분기(도 13의 블록310)에 부가적으로 입력함으로써 추정 상체자세를 보정하고 있었다. 즉, 글로벌 좌표계에서, 추정 상체자세를 보정하고 있었지만, 이 대신에, 자이로 센서의 로컬 좌표계(상체(3)에 고정된 좌표계)상에서 보정해도 좋다. 구체적으로는, 도 13에서, 게인(Kb)의 블록309와, 그 출력을 글로벌 상체각속도(ωgl)로부터 빼는 가산기(블록308)를 삭제하고, 적분기(Ka/S)(블록306)를, Ka/S+Kb, 즉, PI제어칙의 블록으로 변경해도 좋다.

원리상, 이와 같이 로컬 좌표계에서 보정해도 요잉레이트가 높은 선회(그자리 선회 포함)시에, Kb에 의해 경사 드리프트의 집속이 악화되는 일은 없다.

추정 상체자세의 보정에서, 가속도 센서 검출값 글로벌 좌표계 환산값으로부터 기하학적 추정 상체가속도를 뺌으로써 추정 중력가속도를 구하는 대신에, 가속도 센서 검출값 글로벌 좌표계 환산값으로부터 목표 보용의 상체가속도를 뺌으로써 추정 중력가속도를 구하도록 해도 좋다.

실제로는, 목표 보용대로 운동하고 하는 로봇에는, 자세 회전중심 주위의 자세 회전이 발생하고 있으므로, 자세 회전에 의해, 상체가속도는 목표 보용으로부터 벗어난다. 그러나, 자세 회전 내의 자세경사는 전후 좌우로 진동해도 평균적으로는 0이다. 또, 통상, 자세 회전 내의 스핀에 관해서는, 1보마다 스핀의 방향이 반대로 전환되므로, 평균적으로는 0에 가깝다. 따라서 원심력 등, 자세 회전의 회전방향에 의하지 않고 거의 동일방향으로 작용하는 힘을 제외하면, 자세 회전에 의한 상체가속도에의 영향은, 플러스의 영향과 마이너스의 영향이 상쇄되어, 장기적으로는 거의 0이 된다. 또한, 여기에서, 「장기」란 추정 상체자세의 보정의 정정시간 이상의 시간을 가리킨다.

따라서 추정 상체자세의 보정을 위해, 기하학적 추정 상체가속도를 사용하는 대신에, 목표 보용의 상체가속도를 사용해도, 보정의 효과가 크게 저하하는 일은 없다.

이상 설명한 바와 같이, 상기 실시형태에서는, 관성항법에 의해 추정 상체위치(관성항법적 추정 상체위치)와 추정 상체자세를 구하는 동시에, 기하학적 추정 상체위치를 사용하여 관성항법적인 추정 상체위치 및 추정 상체자세를 보정하도록 했으므로, 로봇(1)의 자기위치 자세 및 착지 위치·방향(추정 지지다리 좌표계의 위치·방향)을 정밀도 좋게 추정할 수 있다.

이상과 같이, 본 발명은 2족이동 로봇 등의 다리식 이동 로봇의 상체 등의 소정의 부위의 자세를 정밀도 좋게 추정할 수 있는 기술을 제공하는 것으로서 유용하다.

Claims

결정된 목표 보용에 추종하도록 제어되는 다리식 이동 로봇에 있어서,

상기 로봇의 소정의 부위의 실제 자세각속도를 검출 또는 추정하는 자세각속도 검출 수단과,

상기 로봇에 탑재된 가속도 센서와,

적어도 상기 목표 보용의 목표 운동과 상기 로봇의 관절의 변위 검출값과 이 관절의 변위 목표값중 어느 하나를 포함하는 이 로봇의 운동 상태량을 이용하여 이 로봇의 운동에 의해 발생하는 상기 가속도 센서 자신의 가속도의 추정값으로서의 추정 운동 가속도를 결정하는 운동 가속도 추정 수단과,

적어도 상기 가속도 센서에 의한 가속도 검출값과, 상기 추정 운동 가속도와, 상기 검출 또는 추정된 실제 자세각속도를 기초로, 상기 소정의 부위의 실제 자세를 추정하는 자세 추정 수단을 구비한 것을 특징으로 하는 다리식 이동 로봇의 자기자세 추정 장치.
결정된 목표 보용에 추종하도록 제어되는 다리식 이동 로봇에 있어서,

상기 로봇에 탑재된 가속도 센서와,

상기 로봇의 소정의 부위의 실제 자세각속도를 검출 또는 추정하는 자세각속도 검출 수단과,

적어도 상기 검출 또는 추정된 실제 자세각속도를 기초로, 상기 소정의 부위의 실제 자세의 추정값으로서의 자세각 추정값을 구하는 자세 추정 수단과,

상기 자세각 추정값과 상기 목표 보용에서의 상기 소정의 부위의 목표 자세각과의 차인 자세 회전 편차의 시간적 변화량을 자세 회전 편차 변화량으로서 구하는 자세 회전 편차 산출 수단과,

상기 자세 회전편차의 변화의 회전중심을 결정하는 회전중심 결정 수단과,

상기 로봇이, 상기 회전중심 주위로 상기 자세 회전편차 변화량으로 회전했다고 상정하고, 상기 로봇의 운동에 의해 발생하는 상기 가속도 센서 자신의 가속도의 추정값으로서의 추정 운동 가속도를 결정하는 운동 가속도 추정 수단을 구비하고,

상기 자세 추정 수단은, 적어도 상기 가속도 센서에 의한 가속도 검출값과 상기 추정 운동 가속도를 기초로, 상기 자세각 추정값을 보정하면서 구하는 것을 특징으로 하는 다리식 이동 로봇의 자기자세 추정 장치.
제 2 항에 있어서, 상기 운동 가속도 추정 수단은, 상기 목표 보용을 기술하는 좌표계인 제 1 좌표계를 상기 자세 회전 편차 변화량만큼 상기 회전중심 주위로 회전시켜서 이루어지는 제 2 좌표계를 구하는 수단을 구비하고, 상기 목표 보용의 목표 운동과 상기 로봇의 관절의 변위 검출값과 이 관절의 변위 목표값중 적어도 어느 하나로부터 파악되는, 상기 제 1 좌표계로부터 본 상기 가속도 센서 자신의 가속도와, 상기 추정 운동 가속도로 운동하는 가속도 센서를 상기 제 2 좌표계로부터 본 경우의 상기 가속도 센서 자신의 가속도가 동일하게 되도록 상기 추정 운동가속도를 결정하는 것을 특징으로 하는 다리식 이동 로봇의 자기자세 추정 장치.
제 1 항 내지 제 3 항중 어느 한 항에 있어서, 상기 자세 추정 수단은, 적어도 상기 자세각속도의 검출값을 포함하는 입력을 적분함으로써 상기 소정의 부위의 실제 자세의 추정값으로서의 자세각 추정값을 구하는 적분수단과,

상기 가속도 센서에 의한 가속도 검출값과 상기 추정 운동 가속도와의 차에 기초하여, 상기 자세각 추정값의 오차를 추정하는 자세각 오차 추정 수단과,

상기 추정된 오차를 0에 근접시키도록 상기 적분수단의 입력을 보정하는 입력 보정 수단으로 구성되어 있는 것을 특징으로 하는 다리식 이동 로봇의 자기자세 추정 장치.
제 1 항 또는 제 2 항에 있어서, 상기 소정의 부위는 상기 로봇의 상체인 것을 특징으로 하는 다리식 이동 로봇의 자기자세 추정 장치.
제 2 항에 있어서, 상기 자세 추정 수단이 보정하는 상기 자세각 추정값은 상기 소정의 부위의 연직방향에 대한 경사각도의 추정값인 것을 특징으로 하는 다리식 이동 로봇의 자기자세 추정 장치.