KR100881608B1 - 다리식 이동로봇의 보용 생성 장치 - Google Patents

Abstract

다리식 이동 로봇(1)의 병진 상반력 연직성분 또는 로봇(1)의 전체 중심가속도 연직성분 또는 상체 가속도 연직성분의 목표 궤도를 결정하는 수단과, 이 목표 궤도를 만족하도록 로봇(1)의 전체 중심 또는 상체(24)의 목표 연직위치를 결정하는 수단과, 다리체(2)의 관절에 관한 기하학적 조건에 기초하여 전체 중심 또는 상체(24)의 목표 연직위치를 결정하는 수단을 구비한다. 보행, 주행 등의 보용형태에 따라, 이들 목표 연직위치로부터 선택하고, 또는 이들 목표 연직위치를 가중평균 등에 의해 합성한 것을 최종적인 목표 연직위치로서 얻는다.

다리식, 이동로봇, 관절, 연직위치, 상반력, 보용

Description

다리식 이동로봇의 보용 생성 장치{GAIT PRODUCING DEVICE FOR LEG TYPE MOVABLE ROBOT}

본 발명은, 다리식 이동로봇의 보행뿐만 아니라, 주행에도 적합한 보용(步容) 생성 장치에 관한 것이다.

다리식 이동로봇, 예를 들면 2족 이동로봇의 이동 동작을 행하게 하기 위한 보용(목표 보용)의 생성은, 종래, 주로 로봇에 원활한 보행 동작을 행하게 하는 보용(보행 보용)을 생성하는 것을 주목적으로 하여 행해져 왔다. 그러나, 최근에는, 다리식 이동로봇의 개발이 진행됨에 따라서, 이 로봇에 보행뿐만 아니라, 주행도 행하게 할 수 있는 보용을 생성하는 것이 요망되고 있다.

또한, 「보용」이라는 한자에 「보(步)」가 들어 있으므로 보행에 한정된다고 오해받기 쉽지만, 원래 「보용」이란, 트로트 등, 말의 주행 형태를 가리키는 말로서 사용되고 있는 바와 같이, 주행도 포함한 개념이다.

여기에서, 보행과 주행의 특징의 차이를 설명해 둔다.

동시에 모든 다리가 공중에 존재하는 순간이 있는 이동 형태를 주행이라고 정의하는 것이 일반적이다. 그러나, 이 정의에 의해 보행과 주행을 명확하게 구별할 수 있다고는 할 수 없다. 예를들면, 빠른 조깅에서는, 대부분의 인간에게 있어 서, 동시에 모든 다리가 공중에 존재하는 순간이 있지만, 느린 조깅에서는 상당한 인간에게 있어서, 항상 어느 하나의 다리가 접지해 있다. 빠른 조깅은 주행인데 느린 조깅은 보행이라고 정의하는 것은, 감각적으로 조금 무리가 있다.

도 46에, 전형적인 주행에 있어서의 상체 연직위치와 상반력(床反力) 연직성분(좌우의 다리에 작용하는 상반력 연직성분의 합)의 패턴을 도시하고, 도 49에는, 전형적인 보행에 있어서의 상체 연직위치와 상반력 연직성분의 패턴을 도시한다.

또한, 상체 연직위치 속도는, 상체 대표점의 연직위치와 그 속도를 의미한다. 상체 수평위치 속도는, 상체 대표점의 수평위치와 그 속도를 의미한다. 상체 연직위치 속도와 상체 수평위치 속도를 합하여, 상체 위치 속도라 부른다.

또, 「상반력 연직성분」은, 엄밀하게는, 상반력의 연직축 주위의 모멘트 성분과 구별하기 위해서, 「병진 상반력 연직성분」이라고 기술해야 하지만, 말이 길어지므로, 여기에서는 「병진」을 생략한다.

우선 상체의 움직임을 살펴 보면, 보행에서는, 지지다리의 위를 상체가 통과하는 순간에 상체는 가장 높고, 주행에서는, 이 순간에 가장 낮다. 즉, 보행과 주행에서는, 상체의 상하 이동 패턴의 위상이 반전하고 있다.

한편, 상반력에 관해서는, 보행에서는 비교적 일정한 반면에, 주행에서는, 크게 변동하여, 지지다리의 위를 상체가 통과하는 순간에 상반력은 가장 크다. 또, 당연한 것이지만, 동시에 모든 다리가 공중에 존재하는 순간에 상반력은 0이 되고 있다. 보다 상세하게 관찰하면, 주행에서는, 지지다리를 줄인 양에 거의 비례한 크기의 상반력이 발생한다. 바꿔 말하면, 주행에서는, 다리를 스프링과 같이 사용해서 튀면서 이동하고 있다고 할 수 있다.

느린 조깅에서는, 전형적인 주행과 동일한 상체 상하 이동의 위상을 갖는다. 또, 느린 조깅에서는, 동시에 모든 다리가 공중에 존재하는 순간이 없는 경우가 많지만, 그 경우에도, 지지다리와 미착지 다리의 전환이 행해지는 순간에, 상반력은 완전하게는 0이 되지 않지만 거의 0으로 되어 있다.

따라서, 이상과 같이, 상체의 상하 이동이나 상반력의 패턴의 상기 특징으로부터 보행과 주행을 구별하는 편이, 느린 조깅도 주행으로 간주되고, 감각과도 일치해서, 보다 적절하다고 말할 수 있을 것이다.

특히, 가장 특징적인 점으로 양자를 구별하면, 주행이란, 지지다리의 전환이 행해지는 순간에, 상반력이 0 또는 거의 0이 되는 이동 형태이며, 보행은 그 이외의 이동 형태(상반력 연직성분이 비교적 일정)라고 말할 수 있을 것이다.

한편, 본 출원인은 먼저 일본 특원 2000-352011호에서, 다리식 이동로봇의 상반력을 포함하는 보용을, 동역학적 평형 조건(목표 보용의 중력과 관성력과 상반력과의 균형 조건이다. 협의에서는, 목표 보용의 운동에 의한 중력과 관성력의 합력이 목표 ZMP주위에 작용하는 모멘트가 0인 것. 상세한 것은 후술한다.)을 거의 만족하면서, 자유롭고 또한 리얼타임으로 생성하는 기술을 앞에 제안했다. 이 기술이나, 일본 특개평 10-86081호 공보, 일본 특개평 10-277969호 공보 등에서 본 출원인이 제안해 온 일련의 다리식 이동로봇의 제어장치는, 보행뿐만 아니라 주행에도 적용할 수 있는 것이다.

그렇지만, 이들 기술에서는, 본 출원인이 먼저 일본 특개평 10-86080호 공보 에서 제안한 상체 높이 결정 수법을 사용하여, 상체 높이(이후, 상체 연직위치라 부름)를 족평(足平)과의 상대 위치 관계로부터 기하학적으로 결정하도록 하고 있다. 그리고 목표 상반력 연직성분을 명시적으로 설정하지 않고, 목표 상반력 연직성분은 상기한 바와 같이 기하학적으로 결정된 상체 연직위치(상체 높이)에 따라 결과적으로(종속적으로) 결정되고 있다. 따라서, 원하는 상반력 연직성분을 발생시킬 수 있다고는 할 수 없다. 이 때문에, 상반력 연직성분이 0 또는 거의 0으로 되는 순간을 의도적으로 발생시켜야 하는 로봇의 주행에 있어서는, 상반력 연직성분이 0 또는 거의 0이 되어야 할 순간에, 로봇의 목표 보용의 운동에 의해 발생하는 상반력 연직성분이 완전히 작아지지 않는 경우가 발생한다. 그리고, 이 경우에는, 로봇의 양 족평을 공중에 뜨게 하든지, 혹은 한쪽만의 족평을 바닥면에 가볍게 접촉시키는 순간을 실현할 수 없게 되어, 로봇에 주행을 원활하게 행하게 하는 것이 곤란하게 된다. 즉, 종래의 기술에서 생성된 보용에서는, 주행보용에 적합한 상반력 연직성분 패턴을 발생시킬 수 있다고 할 수 없어, 반드시 최적의 제어 수법이었다고는 할 수 없었다.

또, 로봇에 주행을 행하게 하는 경우, 주행뿐만 아니라, 보행도 원활하게 할 수 있고, 게다가, 그것들의 주행, 보행의 사이의 이행도 원활하게 할 수 있는 것이 요망된다. 그렇지만, 종래의 기술에서는, 상기한 바와 같이 주행에 적합한 상반력 연직성분을 발생할 수 있다고는 할 수 없으므로, 주행과 보행 사이의 이행을 원활하게 할 수 없을 우려가 있었다.

따라서, 본 발명의 목적은, 보행, 주행 등의 보용형태에 의하지 않고 로봇의 이동을 원활하게 할 수 있고, 게다가, 다른 2개의 보용형태 사이에서의 보용형태의 이행도 원활하게 하는 것이 가능한 목표 보용을 생성할 수 있는 보용 생성 장치를 제공하는 것에 있다.

본 발명의 다리식 이동로봇의 보용 생성 장치는, 상체로부터 뻗어 설치된 복수의 다리체를 운동시켜서 이동하는 다리식 이동로봇의 목표 보용을 생성하는 보용 생성 장치에 있어서, 상기 로봇에 작용하는 병진 상반력의 연직성분 또는 로봇의 전체 중심가속도의 연직성분 또는 로봇의 상체 가속도의 연직성분의 목표 궤도를 결정하는 목표 연직성분 궤도 결정 수단과, 이 목표 연직성분 궤도 결정 수단에 의해 설정된 목표 궤도에 병진 상반력 연직성분 또는 상체 중심가속도 연직성분 또는 상체 가속도 연직성분을 일치시키도록 상기 로봇의 전체 중심 또는 상체의 목표 연직위치를 결정하는 제 1 목표 연직위치 결정 수단과, 상기 로봇의 전체 중심 또는 상체의 목표 연직위치를 상기 다리체의 관절변위에 관한 기하학적 조건에 기초하여 결정하는 제 2 목표 연직위치 결정 수단을 구비하고, 생성하는 목표 보용의 형태에 따라, 상기 제 1 및 제 2 목표 연직위치 결정 수단에 의해 각각 결정된 목표 연직위치 중 어느 한쪽을 선택하고, 또는 이 목표 연직위치의 양자를 합성함으로써 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위치를 결정하도록 한 것을 특징으로 한다(제 1 발명).

또한, 이하의 본 발명(제 1 발명 이외의 발명을 포함함)의 설명에서는, 편의상, 병진 상반력의 연직성분 또는 로봇의 전체 중심가속도의 연직성분 또는 로봇의 상체 가속도의 연직성분을 병진 상반력(등) 연직성분이라고 하는 경우가 있다. 또, 로봇의 전체 중심 또는 상체를 전체 중심(등)이라고 하는 경우가 있다.

이러한 본 발명(제 1 발명)에 의하면, 전체 중심(등)의 목표 연직위치는, 2종류 결정되고, 보용 형태(보행, 주행 등의 보용 형태)에 따라 그것들 중 어느 하나를 선택하고, 또는, 그것들을 합성함으로써 최종적인 목표 연직위치가 결정된다.

이 경우, 1개의 목표 연직위치(제 1 목표 연직위치 결정 수단에 의한 목표 연직위치)는, 병진 상반력(등) 연직성분의 목표 궤도를 결정한 뒤에, 그 결정한 목표 궤도에 병진 상반력(등) 연직성분을 일치시키도록 로봇의 전체 중심(등)의 목표 연직위치가 결정된다.

여기에서, 병진 상반력 연직성분은, 로봇의 전체 중심가속도 연직성분과 중력가속도와의 합에 비례한다. 또, 예를 들면 로봇의 다리체의 질량이 상체의 질량에 비해 충분하게 작을 경우에는, 병진 상반력 연직성분은, 상체 가속도 연직성분과 중력가속도와의 합에 거의 비례한다. 따라서 원하는 병진 상반력 연직성분을 발생시키기 위해서는, 이 병진 상반력 연직성분 그 자체의 목표 궤도를 결정해도 좋은 것은 물론, 전체 중심가속도 연직성분 혹은 상체 가속도 연직성분의 목표 궤도를 결정해도 좋게 된다. 이 때문에, 본 발명에서는, 병진 상반력 연직성분, 전체 중심 가속도 연직성분, 상체 가속도 연직성분중 어느 목표 궤도를 설정해도 좋다. 또한, 전체 중심 연직위치의 궤도 혹은 상체 연직위치의 궤도를 2차 미분한 것에 중력가속도를 더한 것이, 병진 상반력(등) 연직성분에 거의 비례하게 된다.

따라서, 병진 상반력(등) 연직성분의 목표 궤도에 병진 상반력(등) 연직성분 으 일치시키도록 상기 로봇의 전체 중량 및 상체의 목표 연직위치 성분을 결정한다는 것은, 로봇의 전체 중심(등)의 연직운동(연직방향의 운동)에 의해 발생하는 병진 상반력(등) 연직성분이 목표 궤도에 합치하도록 전체 중심(등) 목표 연직위치의 궤도가 결정되게 된다. 이 때문에, 제 1 목표 연직위치 결정 수단이 결정하는 목표 연직위치는, 원하는 병진 상반력 연직성분 혹은 이것과 거의 동등한 병진 상반력 연직성분을 발생시킬 수 있는 것이 된다. 따라서 이 목표 연직위치는, 예를 들면 로봇의 주행보용에 적합하다.

한편, 다른 하나의 목표 연직위치(제 2 목표 연직위치 결정 수단에 의한 목표 연직위치)는, 상기 다리체의 관절변위에 관한 기하학적 조건에 기초하여 결정된다. 따라서 그 목표 연직위치는, 각 다리체의 관절변위를 적정한 것(예를 들면 각 다리체가 다 펴지지 않는 관절변위)에 유지할 수 있는 것이 된다. 이 때문에, 이 목표 연직위치는, 예를 들면 로봇의 보행 보용에 적합하다.

그리고 본 발명에서는, 이들 목표 연직위치가, 보행, 주행 등의 보용형태에 따라, 선택되고, 혹은 합성되므로, 각각의 보용형태, 또는 각각의 보용의 이행기의 형태에 적합한 목표 연직위치도 목표 보용을 얻을 수 있게 된다.

따라서, 제 1 발명에 의하면, 보행, 주행 등의 보용형태에 의하지 않고 로봇의 이동을 원활하게 행할 수 있고, 게다가, 다른 2개의 보용형태의 사이에서의 보용형태의 이행도 원활하게 행할 수 있는 목표 보용을 생성할 수 있다.

이러한 제 1 발명에서는, 상기 목표 보용의 형태가 로봇의 적어도 하나의 다리체를 착지시키는 착지기와, 모든 다리체를 공중에 뜨게하는 공중기를 번갈아 반 복하는 제 1 보용 형태일 때에는, 상기 제 1 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치를 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위치로서 선택하고, 상기 목표 보용의 형태가 로봇의 적어도 어느 하나의 다리체를 항상 착지시키면서, 각 다리체의 착지 및 기상을 번갈아 반복하는 제 2 보용형태일 때에는, 상기 제 2 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치를 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위치로서 선택하는 것이 바람직하다(제 2 발명).

이것에 의하면, 상기 착지기와 공중기를 번갈아 반복하는 제 1 보용형태, 즉 로봇의 주행과 같은 보용형태에서의 보용을 생성할 때에는, 상기 병진 상반력(등) 연직성분의 목표 궤도에 기초하여 목표 연직위치가 선택된다. 그리고, 적어도 어느 하나의 다리체를 항상 착지시키면서, 각 다리체의 착지 및 이상(離床)을 번갈아 반복하는 제 2 보용형태, 즉 로봇의 보행과 같은 보용형태에서의 보용을 생성할 때에는, 상기 기하학적 조건에 기초하는 목표 연직위치가 선택된다. 따라서, 그들 제 1 및 제 2 보용형태 어느곳에 두어도, 각각의 보용형태에 적합한 목표 보용을 생성할 수 있다.

이 제 2 발명에서는, 상기 목표 연직성분 궤도 결정 수단은, 상기 제 1 보용형태의 상기 착지기에서는 상기 목표 궤도에 대응하는 병진 상반력 연직성분이 그 증가측으로 볼록한 형상으로 변화되고, 또한 상기 제 1 보용형태의 상기 공중기에서는 상기 목표 궤도에 대응하는 병진 상반력 연직성분이 0으로 유지되는 패턴에서 이 목표 궤도를 결정하는 것이 바람직하다(제 3 발명).

이것에 의하면, 병진 상반력(등) 연직성분의 목표 궤도가 주행보용에 적합한 것으로 결정되는 것이 되고, 나아가서는, 주행보용에 적합한 로봇의 목표 보용의 운동(특히 연직운동)을 결정할 수 있는 것이 된다. 즉, 병진 상반력 연직성분을 0으로 해야 할 공중기, 또는 이 병진 상반력 연직성분을 충분하게 발생시켜야 할 착지기에, 그와 같이 할 수 없는 목표 보용을 생성해버리는 것을 확실하게 방지할 수 있다. 따라서 그 목표 보용에 의해, 공중기와 착지기를 반복하는 보용형태에서의 로봇의 동작을 원활하게 행하는 것이 가능하게 된다.

더욱이, 상기 제 2, 제 3 발명에서는, 상기 제 1 보용형태와 제 2 보용형태 사이의 이행기에서의 상기 목표 보용을 생성할 때에는, 상기 제 1 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치와 상기 제 2 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치와의 가중 평균에 의해 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위치를 결정하는 것이 바람직하다(제 4 발명).

이것에 의하면, 제 1 보용형태와 제 2 보용형태 사이의 이행기에서의 목표 연직위치를 양자의 보용형태에 적합하도록 결정할 수 있기 때문에, 그 이행기의 로봇의 동작을 원활하게 행할 수 있는 목표 보용을 생성할 수 있다. 또한, 이 경우, 상기 가중 평균에서의 중요도는, 기본적으로는, 제 1 보용형태에 의해 가까운 시기에서는, 상기 제 1 목표 연직위치 결정 수단에 의한 목표 연직위치(병진 상반력(등) 연직성분의 목표 궤도에 기초하는 목표 연직위치)에 관계되는 중요도가 보다 높아지도록 하고, 또, 제 2 보용형태에 의해 가까운 시기에서는, 상기 제 2 목표 연직위치 결정 수단에 의한 목표 연직위치(기하학적 조건에 기초하는 목표 연직위 치)에 관계되는 중요도가 보다 높아지게 되도록 설정하는 것이 바람직하다.

또, 상기 제 2∼제 4 발명에서는, 상기 제 1 보용형태는, 이 로봇의 주행동작의 보용형태이며, 상기 제 2 보용형태는, 이 로봇의 보행 동작의 보용형태인 것이 바람직하다(제 5 발명). 이것에 의해, 2족 이동로봇의 보행과 주행, 및 그것들의 이행을 원활하게 행할 수 있는 목표 보용을 생성할 수 있다.

이상에서 설명한, 제 1∼제 5 발명에서는, 상기 기하학적 조건은, 상기 다리식 이동로봇의 각 다리체를 상체에 연결하는 관절과 이 다리체의 선단부 사이에서 각 다리체가 갖는 특정 관절의 변위에 관한 기하학적 조건인 것이 바람직하다(제 6 발명). 즉, 로봇에 원하는 동작을 확실하게 행하게 하고, 또, 지지다리의 착지시 등의 충격을 완화하는 점에서는, 특히, 상기 특정 관절의 변위에 항상 어느 정도의 여유가 있는(어느 정도의 변위가 가능한 상태에서, 예를 들면 각 다리체가 다 펴지지 않은 상태) 것이 바람직하다. 이 경우, 제 6 발명에 의하면, 상기 기하학적 조건을, 상기 특정 관절의 변위에 관한 기하학적 조건으로 함으로써, 상기한 바람직한 상태를 확보할 수 있는 전체 중심(등)의 목표 연직위치를 결정할 수 있다. 또한, 각 다리체가 예를 들면 고관절, 무릎관절, 발목관절을 갖는 2족 이동로봇에서는, 상기 특정 관절은 무릎관절이다.

그리고 이 제 6 발명에서는, 상기 기하학적 조건은, 모든 특정 관절의 변위량을 변수성분으로 하고, 또한, 각 다리체가 펴지는 방향으로의 특정 관절의 변위에 대해 단조변화하는 연속한 함수의 값을 소정값에 대략 유지하도록 정한 조건인 것이 바람직하다(제 7 발명).

이것에 의하면, 각 다리체가 다 펴지는 일이 없는 전체 중심(등) 연직위치를 결정할 수 있다. 또한, 각 다리체가 예를 들면 고관절, 무릎관절, 발목관절을 갖는 2족 이동로봇에서는, 상기 함수로서는, 예를 들면 각 다리체의 무릎관절의 구부림 각도 혹은 그 정현값의 역수를 양 다리체에 대해 가산하여 이루어지는 함수 등을 들 수 있다.

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

도 2는 도 1의 로봇의 각 다리체의 족평 부분의 구성을 도시하는 측면도,

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

도 4는 도 3의 제어 유닛의 기능적 구성을 도시하는 블록도,

도 5는 도 1의 로봇의 주행보용을 예시하는 설명도,

도 6은 목표 상반력 연직성분의 설정예를 도시하는 선도,

도 7은 목표 ZMP의 설정예를 도시하는 선도,

도 8 및 도 9는 로봇의 운동 모드(상체 병진 모드 및 상체 회전 모드)의 설명도,

도 10 및 도 11은 로봇의 동역학 모델의 설명도,

도 12는 도 3의 제어 유닛에 구비한 보용 생성 장치의 메인 루틴 처리를 나타내는 플로차트,

도 13은 로봇의 상체 위치의 발산 상태를 도시하는 설명도,

도 14는 도 12의 플로차트 서브루틴 처리를 나타내는 플로차트,

도 15는 정상보용 족평 착지위치 자세 및 좌표계의 관계를 도시하는 설명도,

도 16은 정상보용의 목표 상반력 연직성분의 설정예를 도시하는 선도,

도 17은 정상보용의 상반력 수평성분 허용범위의 설정예를 도시하는 선도,

도 18은 정상보용의 목표 ZMP의 설정예를 도시하는 선도,

도 19는 도 12의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 20은 도 19의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 21은 도 20의 플로차트의 서브루틴 처리를 나타내는 플로우차트,

도 22는 도 21의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 23∼도 30은, 도 22의 플로차트의 주요부의 처리를 설명하기 위한 선도,

도 31은 정상보용에 있어서의 상체 위치의 궤도의 예를 나타내는 설명도,

도 32는 도 12의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 33은 금회(今回) 보용의 상반력 수평성분 허용범위의 설정예를 도시하는 선도,

도 34는 도 12의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 35는 도 34의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 36은 도 35의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 37은 도 36의 플로차트의 서브루틴 처리를 나타내는 플로차트,

도 38은 도 37의 플로차트의 주요부의 처리를 설명하기 위한 선도,

도 39는 금회 보용과 정상보용에 있어서의 상체 수평위치 궤도의 관계를 도 시하는 선도,

도 40은 로봇의 운동의 모드의 다른 예를 나타내는 설명도,

도 41은 정상보용에 수속(收束)하는 금회 보용의 종단에서의 상체 수평위치와 상체 수평속도와의 관계를 도시하는 선도,

도 42는 보행과 주행 사이의 보용의 전환에 관한 처리를 나타내는 플로차트,

도 43은 주행보용에서의 목표 상반력 연직성분의 설정예를 도시하는 선도,

도 44 및 도 45은 목표 상체 연직위치의 결정 처리를 예시하는 플로차트,

도 46은 주행보용에서의 상체 연직위치와 상반력 연직성분과의 관계를 도시하는 선도, 및

도 47은 보행 보용에서의 상체 연직위치와 상반력 연직성분과의 관계를 도시하는 선도.

이하, 첨부된 도면을 참조하여 본 발명의 1실시형태에 관계되는 다리식 이동로봇의 보용 생성 장치 및 제어 장치를 설명한다. 또한, 본 발명의 실시형태에서는 다리식 이동로봇으로서는 2족 이동로봇를 예로 든다.

도 1은, 본 실시형태에 관계되는 보용 생성 장치 및 제어 장치가 적용되는 다리식 이동로봇으로서의 2족 이동로봇의 전체적 구성의 개략을 도시하는 개략도이다.

도시하는 바와 같이, 2족 이동로봇(이하, 로봇이라고 함)(1)은 상체(24)로부터 하방으로 뻗어 설치된 좌우 한쌍의 다리체(다리부 링크)(2, 2)를 갖추고 있다. 양 다리체(2, 2)는 동일 구조이며, 각각 6개의 관절을 갖추고 있다. 그 6개의 관절은 상체(24)측으로부터 순서대로, 가랑이(주요부)의 선회(회전)용(상체(24)에 대한 요잉 방향의 회전용)의 관절(10R, 10L)(부호 R, L은 각각 우측 다리체, 좌측 다리체에 대응하는 것을 의미하는 부호이다. 이하 동일)과, 가랑이(주요부)의 롤링 방향(X축 주위)의 회전용의 관절(12R, 12L)과, 가랑이(주요부)의 피칭 방향(Y축 주위)의 회전용의 관절(14R, 4L)과, 무릎부의 피칭 방향의 회전용의 관절(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))을 통해서 상기 상체(기체)(24)가 부착되어 있다. 상체(24)의 내부에는, 상세를 후술하는 제어 유닛(26) 등이 격납되어 있다. 또한, 도 1에서는 도시의 편의상, 제어 유닛(26)을 상체(24)의 외부에 기재하고 있다.

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

또한, 도시는 생략하지만, 상체(24)의 상부의 양측부에는 좌우 한쌍의 팔체가 부착되는 동시에, 상체(24)의 상단부에는 머리부가 배치된다. 이들 팔체 및 머 리부는, 본 발명의 요지와 직접적인 관련을 갖지 않으므로 상세한 설명은 생략하지만, 각 팔체는, 그것에 구비하는 복수의 관절에 의해, 이 팔체를 상체(24)에 대해 전후로 흔드는 등의 운동을 행하는 것이 가능하게 되어 있다.

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

또한, 이 명세서에서 후술하는 상체(24)의 위치 및 속도는, 상체(24)의 소정 위치, 구체적으로는 상체(24)의 미리 정한 대표점(예를 들면 좌우의 고관절 사이의 중앙점 등)의 위치 및 그 이동 속도를 의미한다. 동일하게, 각 족평(22R, 22L)의 위치 및 속도는, 각 족평(22R, 22L)의 미리 정한 대표점의 위치 및 그 이동 속도를 의미한다. 이 경우, 본 실시형태에서는 각 족평(22R, 22L)의 대표점은, 예를 들면 각 족평(22R, 22L)의 바닥면상(보다 구체적으로는 각 다리체(2)의 발목관절의 중심으로부터 각 족평(22R, 22L)의 바닥면으로의 수선이 이 바닥면과 교차하는 점 등)에 설정된다.

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

도 2에 도시하는 바와 같이, 각 족평(22R(L))의 상방에는, 상기 6축력 센서(34)와의 사이에 스프링 기구(38)가 장비되는 동시에, 발바닥(각 족평(22R(L))의 바닥면)에는 고무 등으로 이루어지는 발바닥 탄성체(40)가 붙여져 있다. 이들 스프링 기구(38) 및 발바닥 탄성체(40)에 의해 컴플라이언스 기구(42)가 구성되어 있다. 스프링 기구(38)는 구체적으로는, 족평(22R(L))의 상면부에 부착된 사각 형상의 가이드 부재(도시 생략)와, 발목관절(18R(L))(도 2에서는 발목관절(20R(L))을 생략하고 있음) 및 6축력 센서(34)측에 부착되고, 상기 가이드 부재내에 탄성재(고무나 스프링)를 통하여 미동하도록 수납되는 피스톤 형상 부재(도시 생략)로 구성되어 있다.

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

더욱이, 도 1에서는 도시를 생략하지만, 로봇(1)의 외부에는, 이 로봇(1)의 조종용의 조이스틱(조작기)(44)(도 3 참조)이 설치되고, 그 조이스틱(44)을 조작함으로써, 직진이동 하고 있는 로봇(1)을 선회시키는 등, 로봇(1)의 보용에 대한 요구를 필요에 따라서 제어 유닛(26)에 입력할 수 있도록 구성된다. 이 경우, 입력할 수 있는 요구는, 예를 들면 로봇(1)의 이동시의 보용형태(보행, 주행 등), 미착지 다리의 착지 위치 자세나 착지 시각, 또는 이것들의 착지 위치 자세나 착지 시각을 규정하는 지령 데이터(예를 들면 로봇(1)의 이동 방향, 이동 속도 등)이다.

도 3은 제어 유닛(26)의 구성을 도시하는 블록도이다. 이 제어 유닛(26)은 마이크로컴퓨터에 의해 구성되어 있고, CPU로 이루어지는 제 1 연산장치(60) 및 제 2 연산장치(62), A/D변환기(50), 카운터(56), D/A변환기(66), RAM(54), ROM(64), 및 이들간의 데이터 수수를 행하는 버스 라인(52)을 구비하고 있다. 이 제어 유닛(26)에서는, 상기 각 다리체(2)의 6축력 센서(34), 경사 센서(36), 조이스틱(44) 등의 출력 신호는 A/D변환기(50)로 디지털 값으로 변환된 후, 버스 라인(52)을 통하여 RAM(54)에 입력된다. 또, 로봇(1)의 각 관절의 엔코더(33)(로터리 엔코더)의 출력은 카운터(56)를 통하여 RAM(54)에 입력된다.

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

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

이하 설명하면, 제어 유닛(26)은, 후술과 같이 목표 보용을 자유롭게 또한 리얼 타임으로 생성하여 출력하는 보용 생성 장치(100)를 구비하고 있다. 이 보용 생성 장치(100)는, 그 기능에 따라, 본원 발명의 각 수단을 구성하는 것이다. 이 보용 생성 장치(100)가 출력하는 목표 보용은, 목표 상체 위치 자세 궤도(상체(24)의 목표 위치 및 목표 자세의 궤도), 목표 족평위치 자세 궤도(각 족평(22)의 목표 위치 및 목표 자세의 궤도), 목표 팔 자세 궤도(각 팔체의 목표 자세의 궤도), 목표 전체 상반력 중심점(목표 ZMP) 궤도, 및 목표 전체 상반력 궤도로 구성된다. 또한, 다리체(2)나 팔체 이외에, 상체(24)에 대해 움직일 수 있는 부위를 구비할 경우에는, 그 가동 부위의 목표 위치 자세 궤도가 목표 보용에 더해진다.

여기에서, 상기 보용에서의 「궤도」는 시각적 변화의 패턴(시계열 패턴)을 의미하고, 이하의 설명에서는, 「궤도」 대신에 「패턴」이라고 부르는 일도 있다. 또, 「자세」는 공간적인 방향을 의미한다. 구체적으로는, 예를 들면 상체 자세는 Z축(연직축)에 대한 롤링 방향(X축 주위)의 상체(24)의 경사각(자세각)과 피칭 방향(Y축 주위)의 상체(24)의 경사각(자세각)으로 표시되고, 족평자세는 각 족평(22)에 고정적으로 설정된 2축의 공간적인 방위각으로 표시된다. 본 명세서에서는, 상체 자세는 상체 자세각이라고 하는 일도 있다.

또한, 이하의 설명에서는, 오해를 발생할 우려가 없는 경우에는, 종종 「목표」를 생략한다. 또, 보용중의, 상반력에 관계되는 구성 요소 이외의 구성 요소, 즉 족평위치 자세, 상체 위치 자세 등, 로봇(1)의 운동에 관계되는 보용을 총칭적으로 「운동」이라고 한다. 또, 각 족평(22)에 작용하는 상반력(병진력 및 모멘트로 이루어지는 상반력)을 「각 족평 상반력」이라고 부르고, 로봇(1)의 모든(2개) 족평(22R, 22L)에 대한 「각 족평 상반력」의 합력을 「전체 상반력」이라고 한다. 단, 이하의 설명에서는, 각 족평 상반력은 거의 언급하지 않으므로, 특별히 예고하지 않는 한, 「상반력」은 「전체 상반력」과 동일한 의미로서 취급한다.

목표 상반력은, 일반적으로는, 작용점과 그 점에 작용하는 병진력 및 모멘트에 의해 표현된다. 작용점은 어디에 잡아도 좋으므로, 동일한 목표 상반력이라도 무수한 표현을 생각할 수 있지만, 특별히 목표 상반력 중심점(전체 상반력의 중심점의 목표 위치)을 작용점으로 하여 목표 상반력을 표현하면, 목표 상반력의 모멘트 성분은, 연직성분(연직축(Z축) 주위의 모멘트)을 제외하고 0이 된다. 바꾸어 말하면, 목표 상반력 중심점 주위의 목표 상반력의 모멘트의 수평성분(수평축(X축 및 Y축) 주위의 모멘트)는 0이 된다.

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

이와 같은 배경으로부터, 상기 일본 특원 2000-352011호의 명세서에서는 목표 보용을, 다음과 같이 정의하고 있었다.

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

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

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

보행에서는, 본 출원인이 먼저 일본 특개평 10-86080호 공보에서 제안한 상체 높이 결정수법에 의해 로봇(1)의 상체(24)의 연직위치(상체 높이)가 결정되면, 병진 상반력 연직성분은 종속적으로 결정된다. 더욱이, 목표 보용의 운동에 의한 관성력과 중력과의 합력이 목표 ZMP 주위에 발생하는 모멘트의 수평 성분이 0이 되도록 로봇(1)의 상체 수평위치 궤도를 결정함으로써, 병진 상반력 수평성분도 결정 된다. 이 때문에, 일본 특원 2000-352011호의 명세서에서는, 목표 보용의 상반력에 관해서 명시적으로 설정해야 할 물리량으로서는, 목표 ZMP만으로 충분했다. 따라서, 협의의 목표 보용의 정의로서는, 상기의 b)로 충분했다. 그것에 대해, 본 실시형태에서 설명하는 로봇(1)의 주행보용(상세한 것은 후술함)에서는, 상반력 연직성분(병진 상반력 연직성분)도 제어상 중요하다. 이 때문에 본 발명에서는, 이 상반력 연직성분의 목표 궤도를 명시적으로 설정한 다음에, 로봇(1)의 목표 상체 연직위치 등의 궤도를 결정한다. 그래서, 본 명세서에서는, 협의의 목표 보용의 정의로서, 다음 b')를 사용한다.

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

이 명세서에서는 이후, 이해를 쉽게 하기 위해서, 특별히 예고하지 않는 한, 목표 보용은 상기 b')의 협의의 목표 보용의 의미로 사용한다. 이 경우, 목표 보용의 「1보」는, 로봇(1)의 한쪽의 다리체(2)가 착지하고나서 또 한쪽의 다리체(2)가 착지할 때까지의 의미로 사용한다. 또한, 이하의 설명에서는, 「상반력 연직성분」은, 「병진 상반력 연직성분」을 의미하는 것으로 하고, 상반력중의 모멘트의 연직성분(연직축 주위 성분)은, 「모멘트」라는 용어를 사용하여 「상반력 연직성분」과 구별을 한다. 동일하게, 「상반력 수평성분」은 「병진 상반력 수평성분」을 의미하는 것으로 한다.

또, 보용에서의 양 다리 지지기란 말할 필요도 없이, 로봇(1)이 그 자중을 양 다리체(2, 2)로 지지하는 기간, 편각지지기(片脚支持期)란 어느 한쪽의 다리체(2)로 로봇(1)의 자중을 지지하는 기간, 공중기란 양 다리체(2, 2)가 바닥으로부터 떨어져 있는(공중에 떠 있는) 기간을 말한다. 편각지지기에서 로봇(1)의 자중을 지지하지 않는 측의 다리체(2)를 「미착지 다리」라고 부른다. 또한, 본 실시형태에서 설명하는 주행보용에서는, 양 다리 지지기는 없고, 편각지지기(착지기)와 공중기가 번갈아 반복된다. 이 경우, 공중기에서는 양 다리(2, 2)란, 로봇(1)의 자중을 지지하지 않게 되는데, 이 공중기 직전의 편각지지기에서 미착지 다리였던 다리체(2), 지지다리였던 다리체(2)를 각각 이 공중기에서도 미착지 다리, 지지다리라고 부른다.

도 5에 도시하는 주행보용을 예로 하여, 보용 생성 장치(100)가 생성하는 목표 보용의 개요를 설명한다. 또한, 보용에 관한 그 밖의 정의 및 상세한 것은, 먼저 본원 출원인이 제안한 일본 특개평 10-86081호 공보에도 기재되어 있으므로, 이하에서는 일본 특개평 10-86081호 공보에 기재되어 있지 않은 내용을 주로 설명한다.

우선, 도 5에 도시하는 주행보용을 설명한다. 이 주행보용은, 인간의 통상적인 주행보용과 동일한 보용이다. 이 주행보용에서는, 로봇(1)의 좌우 어느 한쪽만의 다리체(2)(지지다리)의 족평(22)이 착지(접지)하는 편각지지기와, 양 다리체(2, 2)가 공중에 뜨는 공중기가 번갈아 반복된다. 도 5의 1번째의 상태는 편각지지기의 개시시(초기)의 상태, 2번째의 상태는 편각지지기의 중간시점의 상태, 3번째의 상태는 편각지지기에 이어지는 공중기의 개시시(편각지지기의 종료시)의 상태, 4번째의 상태는 공중기의 중간시점의 상태, 5번째의 상태는 공중기의 종 료시(다음의 편각지지기의 개시시)의 상태를 도시하고 있다.

이 주행보용에서는, 로봇(1)은, 도 5의 1번째의 상태에서 도시하는 바와 같이, 편각지지기의 개시시에서 지지다리(로봇(1)의 진행 방향 전방측의 다리체(2))측의 족평(22)의 발뒤꿈치로 착지한다. 이어서, 도 5의 2번째의 상태에서 도시하는 바와 같이, 로봇(1)은, 착지한 족평(22)(지지다리측의 족평(22))의 바닥면의 거의 전체면을 착지시킨 후, 도 5의 3번째의 상태에서 도시하는 바와 같이, 그 지지다리측의 족평(22)(도 5의 3번째의 상태에서의 로봇(1)의 진행 방향 후방측의 다리체(2)의 족평(22))의 발끝으로 바닥을 차고 공중으로 뛰어오른다. 이것에 의해 편각지지기가 종료되는 동시에 공중기가 개시된다. 또한, 편각지지기에서의 미착지 다리는, 도 5의 1번째 상태에서 도시하는 바와 같이, 편각지지기의 개시시에서는, 지지다리의 후방측에 존재하지만, 도 5의 2번째 및 3번째의 상태에서 도시하는 바와 같이 다음 착지 예정 위치를 향해서 지지다리의 전방측으로 나온다. 이어서, 도 5의 4번째의 상태에서 도시하는 공중기를 거친 후, 로봇(1)은, 미착지 다리(공중기의 개시 직전의 편각지지기에서 미착지 다리로 되어 있었던 다리체(2))의 족평(22)의 발뒤꿈치로 착지하고, 다음의 편각지지기가 개시된다.

도 5의 주행보용을 고려하면서, 상기 보용 생성 장치(100)가 생성하는 목표 보용의 기본적인 개요를 설명한다. 상세한 것은 후술하지만, 보용 생성 장치(100)가 목표 보용을 생성할 때, 미착지 다리측의 족평(22)의 착지 위치 자세(착지 예정 위치 자세)나 착지 시각(착지 예정시각) 등의 목표 보용 생성용의 기본적인 요구값(요구 파라미터)이, 상기 조이스틱(44)의 소요의 조작 등 에 따라 보용 생성 장치(100)에 주어진다. 그리고 보용 생성 장치(100)는, 그 요구 파라미터를 사용하여 목표 보용을 생성한다. 보다 상세하게 말하면, 보용 생성 장치(100)는, 상기 요구 파라미터에 따라, 목표 보용의 목표 족평위치 자세 궤도, 목표 상반력 연직성분 궤도 등, 목표 보용의 일부의 구성 요소를 규정하는 파라미터(보용 파라미터라고 함)를 결정한 다음에, 그 보용 파라미터를 사용하여 목표 보용의 순시값을 축차 결정하고, 이 목표 보용의 시계열 패턴을 생성한다.

이 경우, 목표 족평위치 자세 궤도(보다 상세하게는, 족평의 위치 및 자세의 공간적인 각 성분(X축 성분 등)의 목표 궤도)는, 예를 들면 본 출원인이 특허 제3233450호에서 제안한 유한시간 정정 필터를 사용하여 각 족평(22) 마다 생성된다. 이 유한시간 정정 필터는, 가변 시정수의 1차 지연 필터, 즉, 전달함수가 1/(1+τs)의 형태로 나타내어지는 필터(τ는 가변의 시정수. 이하, 이 필터를 단위 필터라고 함)를 복수단(본 실시형태에서는 3단 이상), 직렬로 접속한 것이며, 원하는 지정 시각에 지정값에 도달하는 궤도를 생성·출력할 수 있는 것이다. 이 경우, 각 단의 단위 필터의 시정수(τ)는, 모두, 유한시간 정정 필터의 출력 생성을 개시하고 나서, 상기 지정 시각까지의 잔여 시간에 따라 축차 가변적으로 설정된다. 보다 상세하게는, 이 잔여 시간이 짧아짐에 따라 τ의 값이 소정의 초기값(>0)으로부터 감소되어 가, 최종적으로는, 이 잔여 시간이 0이 되는 지정 시각에서, τ의 값이 0이 되도록 설정된다. 그리고, 유한시간 정정 필터에는, 상기 지정값(보다 상세하게는, 유한시간 정정 필터의 출력의 초기값으로부터 상기 지정값으로의 변화량)에 따른 높이의 스텝 입력이 주어진다. 이와 같은 유한시간 정정 필터는, 지정 시각에서 지정값에 도달하는 출력이 생성될 뿐만아니라, 지정 시각에서의 유한시간 정정 필터의 출력의 변화 속도를 0 혹은 거의 0으로 할 수 있다. 특히, 단위 필터를 3단 이상(3단으로 좋음), 접속한 경우에는, 유한시간 정정 필터의 출력의 변화 가속도(변화 속도의 미분값)도 0 혹은 거의 0으로 할 수 있다.

이러한 유한시간 정정 필터를 사용하는 족평위치 자세 궤도(족평(22)이 착지하고나서 다음에 착지할 때까지의 위치 자세 궤도)의 생성은, 예를 들면 다음과 같이 행해진다. 예를 들면 X축방향(전후방향)의 목표 족평위치 궤도는 다음과 같이 생성된다. 즉, 상기 요구 파라미터에 의해 정해지는 각 족평(22)의 다음 착지 예정 위치의 X축방향 위치(보다 상세하게는, 다음 착지 예정 위치의 하나 전의 착지 위치에 대한 X축방향의 변화량(이동량). 이것은 상기 지정값에 상당함)에 따라 유한시간 정정 필터에의 스텝 입력의 높이가 결정되는 동시에 상기 시정수(τ)가 소정의 초기값으로 초기화 된 후, 그 결정된 스텝 입력이 유한시간 정정 필터에 주어지고, 족평(22)의 X축방향 위치의 궤도생성이 개시된다. 그리고, 이 궤도생성시에는, 상기 시정수(τ)는, 족평(22)의 착지 예정 시각(이것은 상기 지정 시각에 상당함)까지 초기값으로부터 0까지 감소해 가도록, 축차 가변 설정된다. 이것에 의해 착지 예정시각에서 착지 예정 위치에 도달하는, 족평(22)의 X축방향의 위치의 궤도가 생성된다.

또, Z축방향(연직방향)의 목표 족평위치 궤도는, 예를 들면 다음과 같이 생성된다. 즉, 우선, 족평(22)의 다음 착지 예정 위치 및 착지 예정 시각에 따라서, 이 족평(22)의 높이(연직위치)가 최대로 될 때의 이 족평(22)의 Z축방향 위치(이 하, 최고점 위치라고 함)와 그 최고점 위치에의 도달 시각이 결정된다. 그리고 그 최고점 위치(이것은 상기 지정값에 상당함)에 따라 유한시간 정정 필터에의 스텝 입력의 높이가 결정되는 동시에 시정수(τ)가 초기화 된 후, 그 결정된 스텝 입력이 유한시간 정정 필터에 주어지고, 상기 최고점 위치까지의 Z축방향의 족평위치 궤도가 축차 생성된다. 이때, 시정수(τ)는, 최고점 위치에의 도달 시각(상기 지정 시각에 상당)까지 초기값으로부터 0까지 감소하도록 축차 가변 설정된다. 더욱이, 최고점 위치까지의 Z축방향 위치의 궤도의 생성이 종료하면, 시정수(τ)를 초기화 하는 동시에 지금까지의 스텝 입력과 역극성의 스텝 입력(보다 상세하게는, 최고점 위치로부터 다음 착지 예정 위치까지의 Z축방향의 변화량(이것은 상기 지정값에 상당함)에 따른 높이의 역극성의 스텝 입력)이 유한시간 정정 필터에 입력되고, 이 최고점 위치로부터 착지 예정 위치까지의 Z축방향의 족평위치의 궤도가 축차 생성된다. 이 때, 시정수(τ)는 족평(22)의 착지 예정시각까지 초기값으로부터 0까지 감소하도록 축차 가변 설정된다.

또한, Z축방향의 족평위치 궤도의 생성에서는, 시정수(τ)를 궤도생성 개시 시각으로부터 족평(22)의 착지 예정시각까지, 초기값으로부터 0까지 계속적으로 감소하도록 가변 설정하는 동시에, 최고점 위치에의 도달 시각 혹은 그 근방 시각에서, 스텝 입력의 극성을 역극성으로 전환함으로써, Z축방향의 족평위치 궤도를 생성하도록 해도 좋다. 이 경우에는, 족평(22)을 원하는 최고점 위치에 정밀도 좋게 도달시킬 수는 없지만, 착지 예정시각에서의 착지 예정 위치에의 도달은 문제 없이 행할 수 있다.

족평자세 궤도에 대해서도, 상기한 족평위치 궤도와 동일하게 유한시간 정정 필터를 사용하여 생성할 수 있다. 이 경우, 족평자세의 공간적인 각 성분중, 그 자세의 각도 변화가 단조적(단조 증가 혹은 단조 감소)인 것으로 되는 성분에 대해서는, 상기한 X축방향의 족평위치 궤도의 생성과 동일하게 족평자세 궤도를 생성하면 좋다. 또, 자세의 각도 변화가 극대값 혹은 극소값을 갖는 성분에 대해서는, 상기한 Z축방향의 족평위치 궤도의 생성과 동일하게 족평자세 궤도를 생성하면 좋다.

또한, 상기한 바와 같이 유한시간 정정 필터에 의해 생성되는 목표 족평위치 자세 궤도는, 바닥면에 고정된 후술의 지지다리 좌표계에서의 각 족평(22)의 목표 위치 자세 궤도이다.

상술한 바와 같이 생성되는 목표 족평위치 자세 궤도는, 각 족평(22)의 위치가, 그 초기 접지 상태(목표 보용의 초기 시각의 상태)로부터 착지 예정 위치를 향해서 서서히 가속화되면서 이동을 개시하도록 생성된다. 그리고, 이 목표 족평위치 자세 궤도는, 최종적으로 착지 예정시각까지 서서히 위치의 변화 속도를 0 또는 거의 0에까지 감속하고, 착지 예정시각에서 착지 예정 위치에 도달하여 정지하도록 생성된다. 이 때문에, 각 족평(22)의 착지 순간에 있어서의 대지(對地)속도(바닥에 고정된 지지다리 좌표계에서의 각 족평(22)의 위치의 변화 속도)가 0 또는 거의 0이 된다. 따라서, 주행보용에서 동시에 모든 다리체(2, 2)가 공중에 존재하는 상태(공중기에서의 상태)로부터 착지해도, 착지 충격이 작아진다.

상기 주행보용에서는, 로봇(1)에 작용하는 중력에 의해 공중기 후반부터 상 체(24)의 연직속도는 하향으로 되고, 착지시에도 하향인 대로이다. 따라서, 상기한 바와 같이 각 족평(22)의 착지 순간에서의 대지속도가 0 또는 거의 0이 되도록 목표 족평위치 자세 궤도를 생성하는 동시에, 후술하는 바와 같이 동역학적 평형 조건을 충족시키도록 상체(24)의 목표 위치 자세 궤도를 생성했을 때, 착지 직전에서, 상체(24)에 대한 미착지 다리측의 족평(22)의 상대 속도는, 상향으로 된다. 즉, 주행보용의 착지 순간에서는, 로봇(1)의 목표 보용은 미착지 다리측의 다리체(2)를 상체(24)측으로 당겨들이면서 착지하는 것과 같은 보용이 된다. 바꿔 말하면, 본 실시형태에서의 목표 보용에서는, 로봇(1)은, 착지 순간에서, 미착지 다리측의 족평(22)의 대지속도가 0 또는 거의 0이 되도록, 상체(24)로부터 보아서 이 족평(22)을 끌어올리도록 하여 착지한다. 이것에 의해, 착지 충격은 작아지고, 착지 충격이 과대해지는 것을 방지하도록 하고 있다.

또, 본 실시형태에서는 유한시간 정정 필터는, 단위 필터를 3단 이상(예를 들면 3단), 직렬로 접속한 것이기 때문에, 착지 예정시각까지 각 족평(22)의 속도(족평위치의 변화 속도)가 0 또는 거의 0이 될 뿐만아니라, 각 족평(22)은, 그 가속도도 착지 예정시각에서 0 또는 거의 0이 되어서 정지한다. 즉, 착지 순간에서의 대지가속도도 0 또는 거의 0이 된다. 따라서 착지 충격이 한층더 작아지게 된다. 특히, 실제의 로봇(1)의 착지 시각이 목표의 착지 시각으로부터 벗어나도, 충격이 그다지 증대하지 않게 된다. 보충설명하면, 착지 예정시각에서 각 족평(22)의 대지속도를 0 또는 거의 0으로 하는 점에서는, 유한시간 정정 필터의 단위 필터의 단수는 2단이어도 되지만, 이 경우에는, 착지 예정시각에서의 각 족평(22)의 가속도 는 일반적으로는 0이 되지 않는다.

또한, 족평자세에 관해서는, 각 족평(22)이 착지 예정시각에서 그 발뒤꿈치로 착지한 후, 이 족평(22)의 바닥면의 거의 전체면이 바닥에 접지할 때까지 계속해서 움직인다. 이 때문에, 이 족평(22)의 바닥면의 거의 전체면이 바닥에 접지하는 시각을 상기 지정 시각으로 설정하고, 상기 유한시간 정정 필터에 의해 족평자세 궤도가 생성된다.

또, 본 실시형태에서는 유한시간 정정 필터를 사용하여 족평위치 궤도를 생성했지만, 착지 예정시각에서의 족평위치의 변화 속도가 0 또는 거의 0이 되도록(족평위치의 시간 미분값이 0이 되도록), 게다가, 이 착지 예정시각에서의 족평위치의 변화 가속도(변화 속도의 시간 미분값)가 0 또는 거의 0이 되도록 설정된 다항식 등의 함수를 사용하여 목표 족평위치 궤도를 생성해도 좋다. 이것은, 목표 족평자세 궤도의 생성에 관해서도 동일하다. 단, 이 목표 족평자세 궤도의 생성에 관해서는, 상술과 동일하게, 각 족평(22)의 바닥면의 거의 전체면이 바닥에 설치하는 시각에서, 각 족평(22)의 자세의 변화 속도, 게다가 그 변화 가속도가 0 또는 거의 0이 되도록 다항식 등의 함수가 설정된다.

목표 상반력 연직성분 궤도는, 예를 들면 도 6과 같이 설정된다. 본 실시형태에서는 주행보용에서의 목표 상반력 연직성분 궤도의 형상(상세하게는 편각지지기에서의 형상)은, 사다리꼴 형상(상반력 연직성분의 증가측으로 볼록한 형상)으로 정해져 있고, 그 사다리꼴의 높이, 꺾임점의 시각을 목표 상반력 연직성분 궤도를 규정하는 보용 파라미터로서, 그것들의 보용 파라미터(상반력 연직성분 궤도 파라 미터)가 결정된다. 또한, 주행보용의 공중기에서는, 목표 상반력 연직성분은 정상적으로 0으로 설정된다. 이 예와 같이, 목표 상반력 연직성분 궤도는, 실질적으로 연속이 되도록(값이 불연속으로 되지 않도록) 설정하는 것이 좋다. 이것은 상반력을 제어할 때의 로봇(1)의 관절의 동작을 원활하게 하기 위해서이다. 또한, 「실질적으로 연속」이라는 것은, 아날로그적으로 연속한 궤도(참 의미에서의 연속한 궤도)를 이산시간계에서 디지털 표현했을 때에 필연적으로 발생하는 값의 튐은, 이 궤도의 연속성을 잃게 하는 것은 아니라고 하는 것을 의미하는 것이다.

목표 ZMP 궤도는 다음과 같이 설정된다. 도 5의 주행보용에서는, 상기한 바와 같이 지지다리측 족평(22)의 발뒤꿈치로 착지하고, 다음에 그 지지다리측 족평(22)의 발끝으로 차서 공중으로 뛰어오르고, 최후에 미착지 다리측 족평(22)의 발뒤꿈치로 착지한다. 따라서, 편각지지기에서의 목표 ZMP 궤도는, 도 7의 상단 도면에 도시하는 바와 같이 지지다리측 족평(22)의 발뒤꿈치를 초기위치로 하고, 다음에 지지다리측 족평(22)의 바닥면의 거의 전체면이 접지하는 기간에 이 족평(22)의 전후방향의 중앙으로 이동하고, 그 후에 이상시까지 지지다리측 족평(22)의 발끝으로 이동하도록 설정된다. 여기에서, 도 7의 상단 도면은, X축방향(전후방향)의 목표 ZMP 궤도를 도시하는 것이며, 도 7의 하단 도면은 Y축방향(좌우방향)의 목표 ZMP 궤도를 도시하는 것이다. 또한, 편각지지기에서의 Y축방향의 목표 ZMP 궤도는, 도 7의 하단 도면에 도시하는 바와 같이, Y축방향에서의 지지다리측 다리체(2)의 발목관절의 중심위치와 동일한 위치에 설정된다.

주행보용에서는, 또한 편각지지기의 종료후, 양 다리체(2, 2)가 바닥으로부 터 떨어지고, 상반력 연직성분이 0이 된다. 상반력 연직성분이 0일 때, 즉 공중기에는, 로봇(1)의 전체 중심은 자유낙하 운동을 하고, 전체 중심 주위의 각운동량 변화는 0이다. 이 때, 바닥의 임의의 점에서, 로봇(1)에 작용하는 중력과 관성력과의 합력의 능률은 0이므로, 목표 ZMP는 일정하지 않다. 즉, 바닥의 어느 점도, 「중력과 관성력과의 합력이 작용하는 모멘트의 수평성분이 0인 작용점」이라는 ZMP의 조건을 충족시킨다. 바꿔 말하면, 임의의 점에 목표 ZMP를 설정해도, 상기 합력이 목표 ZMP 주위에 작용하는 모멘트의 수평성분이 0이라는 동역학적 평형 조건을 만족한다. 따라서, 목표 ZMP를 불연속으로 설정해도 상관없다. 예를 들면 공중기에서는, 목표 ZMP를, 이상시(편각지지기의 종료시)의 목표 ZMP위치로부터 이동하지 않도록 설정하고, 공중기 종단에서, 착지시의 목표 ZMP위치에 불연속(스텝 형상)으로 이동하도록 이 목표 ZMP 궤도를 설정해도 상관없다. 그러나 본 실시형태에서는, 도 7의 상단 도면에 도시하는 바와 같이, 공중기에서의 목표 ZMP 궤도의 X축방향 위치는, 다음 미착지 다리측 다리체(2)의 착지까지 지지다리측 족평(22)의 발끝으로부터 미착지 다리측 족평(22)의 발뒤꿈치의 착지 위치까지 연속적으로 이동하도록 했다. 또, 도 7의 하단 도면에 도시하는 바와 같이, 공중기에서의 목표 ZMP 궤도의 Y축방향 위치는, 다음 미착지 다리측 다리체(2)의 착지까지 지지다리측 다리체(2)의 발목관절의 중심의 Y축방향 위치로부터 미착지 다리측 다리체(2)의 발목관절의 중심의 Y축방향 위치까지 연속적으로 이동하도록 했다. 즉, 보용의 전기간에 있어서 목표 ZMP 궤도를 연속(실질적으로 연속)으로 했다. 그리고 후술하는 바와 같이, 목표 ZMP주위의 중력과 관성력과의 합력의 모멘트(연직성분을 제외함) 가 0이 되도록 목표 보용을 생성하도록(보다 구체적으로는 목표 상체 위치 자세 궤도를 조정하도록) 했다.

이하에 목표 ZMP 궤도를 연속적으로 하는 이유를 기술한다. 공중기에서는, 엄밀한 동역학 모델을 사용하여, 중력과 관성력과의 합력이 목표 ZMP주위에 작용하는 모멘트가 0이 되고, 또한 목표 상반력 연직성분을 0으로 하여 목표 보용을 생성하면, 생성된 목표 보용에 의한 중력과 관성력과의 합력이, 바닥에 고정된 어느 좌표계(이것을 글로벌 좌표계라 부름)의 원점 주위에 작용하는 모멘트의 참값(엄밀한 동역학 모델에 의해 산출되는 중력과 관성력과의 합력이 상기 좌표계 원점 주위에 작용하는 모멘트이며, 이후, 원점 주위 참 모멘트라 부른다.)은 0이 된다.

그러나, 상기와 같이 생성할 때에 사용하는 동역학 모델이 엄밀한 모델이 아니라 근사 모델인 경우에는, 생성된 보용의 원점 주위 참 모멘트는 0이 되지 않는 경우가 있다. 게다가, 근사 모델이, 동역학을 표현하는데 일반적인 미분(차분) 방정식 또는 적분 방정식으로 기술되어 있다고 하면, 원점 주위 참 모멘트는 목표 ZMP의 위치에 의존하고, 목표 ZMP의 위치를 미소량만큼 변경하면, 원점 주위 참 모멘트도 미소량만큼 이동한다. 따라서 이 경우에, 목표 ZMP의 위치를 불연속으로 이동시키면, 원점 주위 참 모멘트도 불연속으로 변화된다.

이것은, 생성된 보용의 참 관성력(엄밀한 모델에서 산출된 관성력)이 불연속으로 되는 것을 의미한다. 또한 이것은, 로봇(1)의 어느 하나 또는 모든 관절변위의 가속도(각가속도)가 불연속으로 되는 것을 의미한다.

이상의 이유에 의해, 모델의 근사 오차를 고려하면, 생성된 보용이 원활하게 되게 하기 위해서, 공중기에서도 목표 ZMP 궤도를 연속(실질적으로 연속)으로 해 두는 것이 바람직하다. 단, 본 실시형태에서 사용하는 후술의 동역학 모델에서는, 목표 ZMP의 위치에 의하지 않고, 목표 ZMP주위 모멘트의 수평성분을 0으로 하는 것과 같은 목표 보용을 일의적으로 생성할 수 있으므로, 반드시 목표 ZMP를 연속으로 해 둘 필요는 없다.

또한, 본 실시형태에서는 도 7에 도시한 바와 같은 목표 ZMP 궤도의 꺾임점의 위치나 시각이, ZMP 궤도 파라미터(목표 ZMP 궤도를 규정하는 파라미터)로서 설정된다. 또, 상기한 ZMP 궤도의 「실질적으로 연속」의 의미는, 상기 상반력 연직성분 궤도의 경우와 동일하다.

ZMP 궤도 파라미터는, 안정 여유가 높고, 또한 급격한 변화를 하지 않도록 결정된다. 여기에서, 로봇(1)의 접지면을 포함하는 최소의 볼록 다각형(소위 지지 다각형)의 중앙부근에 목표 ZMP가 존재하는 상태를 안정 여유가 높다고 말한다(상세한 것은 일본 특개평 10-86081호 공보를 참조). 도 7의 목표 ZMP 궤도는 이러한 조건을 충족시키도록 설정한 것이다.

또, 목표 팔 자세는, 상체(24)에 대한 상대 자세로 나타낸다.

또, 목표 상체 위치 자세, 목표 족평위치 자세 및 후술의 기준 상체 자세는 글로벌 좌표계로 기술된다. 글로벌 좌표계는 상기와 같이 바닥에 고정된 좌표계이다. 글로벌 좌표계로서는, 보다 구체적으로는, 후술하는 지지다리 좌표계가 사용된다.

본 실시형태에서는 보용 생성 장치(100)는, 목표 상체 자세 뿐만아니라, 기 준 상체 자세도 생성한다. 이 기준 상체 자세는, 보용에 대한 요구(보용 생성 장치(100)에 대한 행동 계획부 등의 장치 또는 외부(상기 조이스틱(44) 등)로부터의 요구)에 그대로 따라서 생성되는 상체 자세이다.

목표 상체 자세(이후, 「기준」이 붙어 있지 않은 경우에는, 목표 상체 자세를 나타냄)는, 장기적으로는 기준 상체 자세에 추종하든지 또는 일치하도록 생성된다.

목표 상체 자세는, 보행에서는, 본원 출원인에 의한 일본 특원 2000-352011호의 명세서에 기재한 실시형태와 같이 통상, 항상 기준 상체 자세에 일치시켜 두면 좋다. 일본 특원 2000-352011호에서는, 기준 상체 자세라고 하는 개념은 기재되어 있지 않지만, 목표 상체 자세 패턴을, 명시적 또한 우선적으로 부여하고 있었으므로, 기준 상체 자세에 목표 상체 자세가 항상 일치하고 있는 것과 동일하다.

그러나, 주행 등 공중기가 있는 보용이나 저마찰 바닥면에서의 보행에 있어서는, 단지 상체 수평가속도를 조정하는 것만으로는, 목표 보용의 상반력 수평성분이 허용범위 이내(또는 마찰 한계 이내)에 존재하도록 하면서 동역학적 평형 조건을 만족할 수 없다.

그래서, 본 실시형태에서는, 목표 상체 자세를 필요에 따라서 기준 상체 자세로부터 고의로 비키어 놓는 것으로 했다. 보다 구체적으로는, 이하에 설명하는 두가지의 운동 모드를 복합적으로 발생시킴으로써, 목표 보용의 상반력 수평성분이 허용범위 이내(또는 마찰 한계 이내)에 존재하면서 동역학적 평형 조건을 만족시키도록 했다.

도 8과 같이, 로봇(1)의 어느 운동 상태로부터, 상체 수평가속도만을 섭동(攝動)(미소 변화)시키면, 로봇(1)의 전체 중심 수평가속도와 전체 중심 주위의 각운동량이 섭동한다. 즉, 상체 수평가속도의 섭동은, 그것에 의해서 발생하는 로봇(1)의 관성력과 중력과의 합력에 대해 동역학적으로 균형이 잡히는 상반력 연직성분을 섭동시키지 않고(로봇(1)의 전체 중심 연직가속도를 섭동시키지 않고), 목표 ZMP주위의 상반력 모멘트 성분(단 연직축 주위 성분을 제외함)과 상반력 수평성분을 섭동시킨다. 이와 같이 로봇(1)의 상체 수평가속도를 섭동시키는 운동 모드를 상체 병진 모드라 부른다.

바꿔 말하면, 상반력 연직성분을 변화시키지 않고, 목표 ZMP주위의 상반력 모멘트 성분(단 연직축 주위 성분을 제외함)과 상반력 수평성분을 변화시키는 운동을 상체 병진 모드라 부른다.

이 때의 단위가속도당의 상반력 모멘트 성분의 변화를 △Mp, 단위가속도당의 상반력 수평성분의 변화를 △Fp라 한다. 도 8에 도시하는 상황에서 상체(24)를 전방으로 수평 가속하면, △Mp와 △Fp는 도 8에 도시하는 화살표 방향으로 작용한다.

감각적으로 알기 쉽게 하기 위해서, 운동에 의해 발생하는 관성력과 중력과의 합력에 균형이 잡히는 상반력을 사용하여 표현했지만, 관성력과 중력과의 합력을 사용하여 표현한 쪽이, 이론적으로는 적확하다. 또한, 상기 합력과 상반력은, 서로 크기가 같고 방향이 반대로 되어 있다.

그것에 반해, 도 9와 같이, 로봇(1)의 어떤 운동 상태로부터, 어떤 점(Pr) 주위에 상체 자세각가속도(상체(24)의 자세각의 각가속도)를 섭동시키면, 로봇(1) 의 전체 중심은 섭동하지 않고, 전체 중심 주위의 각운동량이 섭동한다. 즉, 점(Pr) 주위의 상체 자세각가속도의 섭동은, 상반력 연직성분과 상반력 수평성분을 섭동시키지 않고, 목표 ZMP주위의 상반력 모멘트 성분(연직축 주위 성분을 제외함)을 섭동시킨다. 이와 같이 로봇(1)의 상체 자세각가속도를 섭동시키는 운동 모드를 상체 회전 모드라 부른다.

바꿔 말하면, 상반력 연직성분과 상반력 수평성분을 변화시키지 않고, 목표 ZMP주위의 상반력 모멘트 성분(연직축 주위 성분을 제외함)을 변화시키는 운동을 상체 회전 모드라 부른다.

이 때의 단위 각가속도당의 상반력 모멘트 성분의 변화를 △Mr, 단위 각가속도당의 상반력 수평성분의 변화를 △Fr이라고 한다. △Fr는 0이다. 도 9에 도시하는 상황에서 상체(24)가 앞으로 기울도록 상체 자세각의 각가속도를 발생시키면, △Mr는 도 9에 도시하는 화살표 방향으로 작용한다.

다음에 본 실시형태에 사용되는 로봇 동역학 모델에 대하여 설명한다. 본 실시형태에서는 단순화된(근사된) 이하에 나타내는 동역학 모델을 사용한다. 단, 이하에 나타내는 동역학 모델에 관해서는, 키네마탁스 모델(관절, 링크의 구조나 치수를 나타내는 모델이며, 바꿔 말하면, 관절변위와 링크의 위치 자세와의 관계를 나타내는 모델이다.)도 필요하다.

도 10은, 본 실시형태에 사용되는 로봇 동역학 모델이다. 도시하는 바와 같이, 이 동역학 모델은, 로봇(1)의 각 다리체(2)에 각각 대응하는 2개의 질점(2m, 2m), 및 상체(24)에 대응하는 질점(24m)으로 이루어지는 합계 3질점과, 이너시어가 만나서 질량이 없는 플라이 휠(FH)로 구성되는 모델이다. 이 동역학 모델에서는, 디커플드, 즉 다리체(2, 2)의 동역학(각 질점(2m, 2m)의 동역학)과 상체(24)의 동역학(질점(24m) 및 플라이 휠(FH)의 동역학)이 서로 비간섭으로 구성되는 동시에, 로봇(1) 전체의 동역학은 그것들의 선형결합으로 표시된다. 또, 더욱이, 상체(24)의 운동과 상반력과의 관계도, 상체(24)의 병진운동(상체 병진 모드)과 상반력과의 관계, 및 상체(24)의 회전운동(상체 회전 모드)과 상반력과의 관계로 분리된다. 구체적으로는, 상체 질점(24m)의 수평운동에 의해 발생하는 상반력은, 상체(24)의 수평방향 병진운동(상체 병진 모드)에 의해 발생하는 상반력에 대응하고, 플라이 휠의 회전운동에 의해 발생하는 상반력은, 상체(24)의 자세각의 회전운동(상체 회전 모드)에 의해 발생하는 상반력에 대응한다.

또한, 로봇(1)의 팔체의 질량은 상체(24)에 포함되는 것으로 하고, 상체 질점(24m)은 팔체의 질량을 포함하는 질량을 가진다. 본 실시형태에서는 목표 보용에서의 팔체의 운동(팔흔들기 운동)은, 후술하는 바와 같이, 로봇(1)의 팔흔들기 이외의 운동에 의해 로봇(1)에 발생하는 연직축 주위의 관성력의 모멘트를 없애면서도, 상체(24)에 대한 양 팔체의 전체의 중심의 상대 위치가 움직이지 않도록 행해지므로, 팔체의 팔흔들기 운동에 의한(연직축 주위 성분을 제외함) 상반력 모멘트에의 영향과 상반력 수평성분에의 영향은 무시한다.

이하에 상기 단순화 모델을 수식화한다. 단, 설명을 간단하게 하기 위해서, 사지탈 플레인(전후축(X축)과 연직축(Z축)을 포함하는 평면)에서의 운동방정식만을 기술하고, 레이터럴 플레인(좌우축(Y축)과 연직축(Z축)을 포함하는 평면)에서의 운 동방정식을 생략했다.

설명의 편의상, 동역학 모델에 관한 변수 및 파라미터를 이하와 같이 정의한다. 또한, 각 질점(2m, 2m, 24m)은 그것이 대응하는 부위의 대표점이든지, 또는 그 부위의 위치 자세로부터 기하학적으로 일의적으로 결정되는 점에 상당한다. 예를 들면, 지지다리측 다리체(2)의 질점(2m)의 위치는, 이 다리체(2)의 족평(22)의 바닥면의 상기 대표점으로부터 소정의 거리만큼 상방의 점으로 한다.

Zsup: 지지다리 질점 연직위치

Zswg: 미착지 다리 질점 연직위치

Zb: 상체 질점 연직위치(상체 연직위치와는 일반적으로는 다르다.)

ZGtotal: 전체 중심 연직위치

Xsup: 지지다리 질점 수평위치

Xswg: 미착지 다리 질점 수평위치

Xb: 상체 질점 수평위치(상기 점 Pr로부터 전후에 있는 소정의 거리만큼 오프셋한 점이다. 오프셋은, 직립시 등에서, 엄밀한 모델의 중심위치와, 본 동역학 모델의 중심위치가 되도록이면 일치하도록 결정된다. 상체 수평위치와는 일반적으로는 다르다.)

XGtotal: 전체 중심 수평위치

θby: 연직방향에 대한 Y축 주위의 상체 자세각(경사각)

mb: 상체 질점 질량

msup: 지지다리 질점 질량

mswg: 미착지 다리 질점 질량

mtotal: 로봇 총질량(=mtotal+msup+mswg)

J: 상체 관성모멘트(상체 회전 모드에서의 등가 관성모멘트이다. 실제 로봇(1)의 상체(24) 부위의 관성모멘트와는 일반적으로는 일치하지 않는다.)

Fx: 상반력 수평성분(상세하게는 병진 상반력의 전후방향(X축) 성분)

Fz: 상반력 연직성분(상세하게는 병진 상반력의 연직방향(Z축) 성분. 본 실시형태에서는, 이것은 목표 병진 상반력 연직성분과 동일함)

My: 목표 ZMP주위의 상반력 모멘트(상세하게는 상반력 모멘트의 좌우축(Y축) 주위 성분)

또한, 본 실시형태에서는 각 다리체(2)의 질점(2m)의 위치와, 그 다리체(2)의 족평(22)의 위치(족평(22)의 미리 정한 대표점의 위치)와의 위치 관계는 미리 정해져 있고, 일방의 위치가 결정되면, 타방의 위치는 일의적으로 정해지는 것으로 되어 있다. 또, 상체 질점(24m)과, 상체(24)의 위치(상체(24)의 미리 정한 대표점의 위치)와의 위치 관계는, 상체(24)의 자세각에 따라서 미리 정해져 있고, 일방의 위치와 자세각이 결정되면, 타방의 위치는 일의적으로 정해지는 것으로 되어 있다.

또, 임의의 변수 X에 대해, dX/dt는 X의 1차 미분을 나타내고, d2X/dt2 는 X의 2차 미분을 나타낸다. 따라서, 변수 X가 변위라면, dX/dt는 속도, d2X/dt2는 가속도를 의미한다. g는 중력가속도 정수를 나타낸다. 여기에서는, g는 플러스 값으로 한다.

상기 동역학 모델의 운동방정식(동역학적 평형 조건을 나타내는 식)은, 식01, 식02, 식03으로 나타내어진다.

Fz=mb*(g+d2Zb/dt2)+msup*(g+d2Zsup/dt2)+mswg*(g+d2Zswg/dt2)

……식01

Fx=mb*d2Xb/dt2+msup*d2Xsup/dt2+mswg*d2Xswg/dt2

……식02

My= -mb*(Xb-Xzmp)*(g+d2Zb/dt2)+mb*(Zb-Zzmp)*(d2Xb/dt2)

-msup*(Xsup-Xzmp)*(g+d2Zsup/dt2)

+msup*(Zsup-Zzmp)*(d2Xsup/dt2)

-mswg*(Xswg-Xzmp)*(g+d2Zswg/dt2)

+mswg*(Zswg-Zzmp)*(d2Xswg/dt2)+J*d2θby/dt2

……식03

또, 로봇 전체 중심위치에는 다음 관계식이 성립한다.

ZGtotal=(mb*Zb+msup*Zsup+mswg*Zswg)/mtotal ……식04

XGtotal=(mb*Xb+msup*Xsup+mswg*Xswg)/mtotal ……식05

이하에 상기 동역학 모델과 상기 △Fp, △Mp, △Fr, △Mr의 관계를 나타낸다.

상기 △Fp는, 식02에서, d2Xb/dt2를 단위량만큼 섭동시켰을 때의 Fx의 섭동량이므로, 다음 식에 의해 구해진다.

△Fp=mb ……식06

즉, 상체 병진 모드의 단위가속도당의 상반력의 수평성분의 변화 △Fp는, 상 기 동역학 모델의 상체 질점(24m)의 질량에 상당한다.

상기 △Mp는, 식03에서, d2Xb/dt2를 단위량만큼 섭동시켰을 때의 My의 섭동량이므로, 다음 식에 의해 구해진다.

△Mp=mb*(Zb-Zzmp) ……식07

즉, 상체 병진 모드의 단위가속도당의 상반력 모멘트 성분의 변화 △Mp는, 상기 동역학 모델의 상체 질점 질량에, 상체 질점(24m)의 목표 ZMP로부터의 높이(연직위치)를 곱한 것이 된다. 이 때의 상체 질점(24m) 및 목표 ZMP의 위치와, 상체 질점(24m)의 운동과의 관계는, 상체 질점(24m)을 도립 진자 질점, 목표 ZMP를 도립 진자 지지점에 대응시켰을 때의 도립 진자의 거동에 상당한다.

상기 △Fr는, 식02에서, d2θby/dt2를 단위량만큼 섭동시켰을 때의 Fx의 섭동량이기 때문에, 다음 식에 의해 구해진다.

△Fr=0 ……식08

즉, 상체 회전 모드의 단위가속도당의 병진 상반력 수평성분의 변화 △Fr는, 0이다.

상기 △Mr는, 식03에서, d2θby/dt2를 단위량만큼 섭동시켰을 때의 My의 섭동량이므로, 다음 식에 의해 구해진다.

△Mr=J ……식09

즉, 상체 회전 모드의 단위가속도당의 상반력 모멘트 성분의 변화 △Mr는, 플라이 휠의 관성모멘트에 상당한다.

본 실시형태에서의 보용 생성 장치(100)는, 로봇(1)의 한쪽 다리체(2)가 착 지하고나서 다른쪽 다리체(2)가 착지할 때까지의 1보분의 목표 보용(상기 협의의 의미에서의 목표 보용)을 단위로 하여, 그 1보분의 목표 보용을 순서대로 생성한다. 따라서, 본 실시형태에서 생성하는 도 5의 주행보용에서는, 이 목표 보용은 편각지지기의 개시시부터, 이것에 이어지는 공중기의 종료시(다음의 편각지지기의 개시시)까지의 목표 보용이 순서대로 생성된다. 여기에서, 새롭게 생성하고자 하고 있는 목표 보용을 「금회 보용」, 그 다음 목표 보용을 「다음회 보용」, 더욱이 그 다음 목표 보용을 「다음 다음회 보용」과 같이 부른다. 또, 「금회 보용」의 하나 전에 생성한 목표 보용을 「전회(前回) 보용」이라고 부른다.

또, 보용 생성 장치(100)가 금회 보용을 새롭게 생성할 때, 이 보용 생성 장치(100)에는, 로봇(1)의 2보 앞까지의 미착지 다리측 족평(22)의 착지 예정 위치 자세, 착지 예정시각의 요구값(요구)이 보용에 대한 요구 파라미터로서 입력된다(또는 보용 생성 장치(100)가 기억장치로부터 요구 파라미터를 읽어들임). 그리고, 보용 생성 장치(100)는, 이들 요구 파라미터를 사용하여, 목표 상체 위치 자세 궤도, 목표 족평위치 자세 궤도, 목표 ZMP 궤도, 목표 상반력 연직성분 궤도, 목표 팔자세 궤도 등을 생성한다. 이 때, 이들 궤도를 규정하는 보용 파라미터의 일부는, 보행의 계속성을 확보하도록 적당하게 수정된다.

이하에 도 5의 주행보용을 생성하는 것을 예로 하여, 보용 생성 장치(100)의 보용 생성 처리의 상세를 도 12∼도 38을 참조하면서 설명한다. 도 12는, 그 보용 생성 장치(100)가 실행하는 보용 생성 처리를 나타내는 플로차트(구조화 플로차트)이다.

우선 S010에서 시각 t를 0으로 초기화 하는 등 여러 초기화 작업이 행해진다. 이 처리는, 보용 생성 장치(100)의 기동시 등에 행해진다. 이어서, S012를 거쳐서 S014로 진행하고, 보용 생성 장치(100)는, 제어 주기(도 12의 플로차트의 연산처리 주기) 마다의 타이머 인터럽션을 기다린다. 제어 주기는 △t이다.

이어서, S016으로 진행하고, 보용을 전환할 차례인지 아닌지가 판단되고, 보용을 전환할 차례일 때는 S018로 진행하는 동시에, 전환할 차례가 아닐 때는 S030으로 진행한다. 여기에서, 상기 「보용을 전환할 차례」는, 전회보용의 생성이 완료되고, 금회 보용의 생성을 개시하는 타이밍을 의미하며, 예를 들면 전회보용의 생성을 완료한 제어 주기의 다음 제어 주기가 보용을 전환할 차례가 된다.

S018로 진행할 때는 시각 t가 0으로 초기화되고, 이어서 S020으로 진행하고, 다음회 보용 지지다리 좌표계, 다음 다음회 보용 지지다리 좌표계, 금회 보용 주기 및 다음회 보용 주기가 읽어들여진다. 이들 지지다리 좌표계 및 보용 주기는, 상기 요구 파라미터에 의해 정해지는 것이다. 즉, 본 실시형태에서는 보용 생성 장치(100)에 조이스틱(44) 등으로부터 부여되는 요구 파라미터는, 2보 앞까지의 미착지 다리측 족평(22)의 착지 예정위치 자세(족평(22)이 착지하고나서 발바닥을 바닥면에 거의 전면적으로 접촉시키는 것과 같이, 미끄러지지 않고 회전시킨 상태에서의 족평위치 자세), 착지 예정시각의 요구값을 포함하고 있고, 그 1보째의 요구값, 2보째의 요구값이 각각, 금회 보용, 다음회 보용에 대응하는 것으로서, 금회 보용의 생성 개시시(상기 S016의 보용을 전환할 차례) 이전에 보용 생성 장치(100)에 부여된 것이다. 또한, 이들 요구값은 금회 보용의 생성 도중에도 변경하는 것은 가능하다.

그리고, 상기 요구 파라미터에서의 1보째의 미착지 다리측 족평(22)(금회 보용에서의 미착지 다리측 족평(22))의 착지 예정 위치 자세의 요구값에 대응하여 다음회 보용 지지다리 좌표계가 정해진다.

예를 들면 도 15를 참조하여, 금회 보용(1보째)에 관계되는 미착지 다리측 족평(22)(도면에서는 22L)의 착지 예정 위치 자세의 요구값이, 금회 보용의 지지다리측 족평(22)(도면에서는 22R)의 착지 위치 자세에 대해, 금회 보용 지지다리 좌표계의 X축방향(금회 보용의 지지다리측 족평(22R)의 전후방향) 및 Y축방향(금회 보용의 지지다리측 족평(22R)의 좌우방향)에, 각각 xnext, ynext만큼 이동하고, 또한 Z축 주위(연직축 주위)에 θznext만큼 회전시킨 위치 자세인 것으로 한다. 여기에서, 지지다리 좌표계는, 지지다리측 족평(22)을 수평자세(보다 일반적으로는 바닥면에 평행한 자세)로 하여 이 지지다리측 족평(22)의 바닥면의 거의 전체면을 바닥면에 접촉(밀착)시킨 상태에서, 이 지지다리측 다리체(2)의 발목중심으로부터 바닥면에 연장시킨 수선이 이 바닥면과 교차하는 점(이 점은, 본 실시형태의 예에서는 지지다리측 족평(22)의 바닥면의 거의 전체면을 바닥면에 접촉시킨 상태에서는, 이 족평(22)의 대표점과 합치함)을 원점으로 하고, 그 원점을 지나는 수평면을 XY평면으로 하는 글로벌 좌표계(바닥에 고정된 좌표계)이다. 이 경우, X축방향, Y축방향은, 각각 지지다리측 족평(22)의 전후방향, 좌우방향이다. 또한, 지지다리 좌표계의 원점은, 반드시 지지다리측 족평(22)의 바닥면의 거의 전체면을 바닥면에 접촉시킨 상태에서의 이 족평(22)의 대표점(족평(22)의 위치를 대표하는 점)에 일 치할 필요는 없고, 이 대표점과 다른 바닥면상의 점에 설정되어도 좋다.

이 때, 다음회 보용 지지다리 좌표계는, 도시하는 바와 같이 금회 보용의 미착지 다리측 족평(22L)의 착지 예정 위치 자세의 요구값에 따라서 이 족평(22)을 착지시킨 경우(족평(22)의 대표점을 착지 예정 위치의 요구값에 일치시키고, 또한, 족평(22)의 자세(방향)를 착지 예정 자세의 요구값에 일치시킨 경우)에서의 이 족평(22L)의 대표점(보다 상세하게는 이 대표점에 합치하는 바닥상의 점)을 원점으로 하고, 이 원점을 지나는 수평면내에서의 이 족평(22L)의 전후방향, 좌우방향을 각각 X'축 방향, Y'축 방향으로 하는 좌표계이다.

상기와 마찬가지로, 2보째의 미착지 다리측 족평(22)의 착지 예정 위치 자세의 요구값에 따라 다음 다음회 보용 지지다리 좌표계(도 15의 X''Y''좌표를 참조)가 정해진다. 또, 금회 보용 주기는, 금회 보용의 지지다리측 족평(22)의 착지 예정시각(요구값)으로부터, 1보째(금회 보용)의 미착지 다리측 족평(22)의 착지 예정시각(요구값)까지의 시각으로서 정해지고, 다음회 보용 주기는, 1보째의 미착지 다리측 족평(22)의 착지 예정시각(요구값)으로부터 2보째의 미착지 다리측 족평(22)의 착지 예정시각(요구값)까지의 시간으로서 정한다.

또한, 상기 요구 파라미터는, 본 실시형태에서는 조이스틱(44)의 소요의 조작에 의해 보용 생성 장치(100)에 입력되는 것이지만, 미리 이 요구 파라미터 또는 이것에 대응하는 상기한 지지다리 좌표계의 위치 자세 및 보용 주기를 로봇(1)의 이동 스케줄로서 기억해 두어도 좋다. 또는, 조이스틱(44) 등의 조종 장치로부터의 지령(요구)과 그 때까지의 로봇(1)의 이동 이력을 기초로 상기 다음회 및 다음 다음회 보용 지지다리 좌표계, 및 금회 및 다음회 보용 주기를 결정해도 좋다.

이어서 S022로 진행하고, 보용 생성 장치(100)는, 금회 보용에 이어지는 가상적인 주기적 보용으로서의 정상 선회 보용의 보용 파라미터를 결정한다. 이 보용 파라미터는, 정상 선회 보용에서의 목표 족평위치 자세 궤도를 규정하는 족평궤도 파라미터, 기준으로 하는 상체 자세 궤도를 규정하는 기준 상체 자세 궤도 파라미터, 목표 팔자세 궤도를 규정하는 팔궤도 파라미터, 목표 ZMP 궤도를 규정하는 ZMP 궤도 파라미터, 목표 상반력 연직성분 궤도를 규정하는 상반력 연직성분 궤도 파라미터를 포함한다. 더욱이, 목표 상반력 수평성분 허용범위를 규정하는 파라미터도 보용 파라미터에 포함된다.

또한, 이 명세서에서 「정상 선회 보용」은, 그 보용을 반복했을 때에 보용의 경계(본 실시형태에서는 1보마다의 보용의 경계)에서 로봇(1)의 운동 상태(족평위치 자세, 상체 위치 자세 등의 상태)로 불연속이 생기지 않는 주기적 보용을 의미하는 것으로서 사용한다. 이후, 「정상 선회 보용」을 「정상보용」이라고 약칭하는 경우도 있다.

주기적 보용인 정상 선회 보용은, 본 실시형태에서는 로봇(1)의 2보분의 보용, 즉 금회 보용에 이어지는 제 1 선회 보용과 이 제 1 선회 보용에 이어지는 제 2 선회 보용으로 이루어지는 보용을 이 정상 선회 보용의 1주기분의 보용으로 하여, 그 1주기분의 보용을 반복하는 보용이다. 또한, 여기에서 「선회」라는 용어를 사용한 것은, 선회율을 0으로 할 때는 직진을 의미하므로, 직진도 광의의 의미에서 선회에 포함시킬 수 있기 때문이다. 또, 생성하는 목표 보용이 상기 도 5의 주행보용이면, 그 목표 보용의 금회 보용은, 편각지지기 및 공중기를 갖는 주행보용이므로, 정상 선회 보용의 제 1 선회 보용 및 제 2 선회 보용은, 모두 금회 보용과 동일하게 편각지지기와 공중기를 갖는 보용이다. 즉, 제 1 선회 보용 및 제 2 선회 보용의 기본적인 보용형태는 금회 보용과 동일하다.

정상 선회 보용에 대해서 보충하면, 2족 이동로봇에서는, 정상 선회 보용의 1주기분은, 상기한 협의의 의미에서의 보용이, 적어도 2보분 필요하다. 그리고 3보 이상의 보용을 1주기분의 보용으로 하는 복잡한 정상 선회 보용을 설정하는 것도 가능하다. 단, 정상 선회 보용은, 후술과 같이, 금회 보용의 종단(종료 시각)에 있어서의 발산 성분(상세한 것은 후술함)을 결정하기 위해서만 사용된다. 이 때문에, 3보 이상의 보용을 1주기로 하는 정상 선회 보용을 사용하는 것은, 보용 생성의 처리가 번잡하게 됨에도 불구하고, 효과는 적다. 그래서, 본 실시형태에서의 정상 선회 보용의 1주기분의 보용을 2보분의 보용(제 1 및 제 2 선회 보용)에 의해 구성하도록 하고 있다. 또한, 3족 이상의 다리식 이동로봇에 있어서는, 그것에 따라 정상 선회 보용을 정의할 만한 보용수가 증가한다. 이하에서는, 설명의 편의상, 복수의 협의의 보용(본 실시형태에서는 2보분의 보용)으로 이루어지는 정상 선회 보용을 1보의 보용으로 간주한다.

정상 선회 보용은, 보용 생성 장치(100)에서 금회 보용의 종단에서의 발산 성분이나 상체 연직위치 속도, 상체 자세각 및 그 각속도 등의 로봇(1)의 운동 상태를 결정하기 위해서 잠정적으로 작성되는 것이며, 보용 생성 장치(100)로부터 그대로 출력되는 것은 아니다.

또한, 「발산」이란, 도 13에 도시하는 바와 같이, 2족 이동로봇(1)의 상체(24)의 위치가 양 족평(22, 22)의 위치로부터 멀리 떨어진 위치로 벗어나버리는 것을 의미한다. 발산 성분의 값이란, 2족 이동로봇(1)의 상체(24)의 위치가 양 족평(22, 22)의 위치(보다 구체적으로는, 지지다리측 족평(22)의 접지면에 설정된 글로벌 좌표계(지지다리 좌표계)의 원점)로부터 멀리 떨어져 가는 상태를 나타내는 수치이다.

본 실시형태에서는 목표 보용이, 상기 발산을 발생하지 않고, 계속적으로 생성되도록, 발산 성분을 지표로 하여 보용을 생성하도록 했다. 단, 계속적인 보용의 대표예인 정상보용(보용의 궤도의 불연속을 발생하지 않고, 동일 패턴의 보용을 반복할 수 있는 주기적 보용이며, 원리상, 무한회 반복해도 발산하지 않는 보용)의 초기 발산 성분(정상보용의 초기 시각에서의 발산 성분)이어도, 단순하게 0이 아니라, 정상보용의 파라미터가 변하면, 그 초기발산 성분도 변한다. 즉, 걷는 방법 또는 달리는 방법 등의 보용형태에 의해, 적절한 발산 성분이 변한다. 그래서, 본 실시형태에서는 이제부터 생성하려고 하는 금회 보용의 뒤에 이어지는 정상보용이 상기 금회 보용에 관계되는 요구 파라미터에 따라 설정되고, 정상보용의 초기 발산 성분을 구하고나서, 금회 보용의 종단 발산 성분을 정상보용의 초기 발산 성분에 일치시키도록(보다 일반적으로는, 금회 보용을 정상보용에 연속시키도록 혹은 근접시키도록), 금회 보용을 생성한다. 이것과 같은 보용 생성의 기본적인 지침은 본 출원인이 먼저 제안한 일본 특원 2000-352011호의 것과 동일하다.

본 발명의 실시형태에서는 일본 특원 2000-352011호의 제 1 실시예에서 사용 한 것과 같은 선형의 3질점 동역학 모델을 사용하고 있지 않지만, 다음 식으로 정의되는 발산 성분 및 수속 성분의 개념은, 도 10에 도시한 모델과 같은 비선형의 동역학 모델의 거동의 섭동분에 대해서도, 근사적으로 충분한 정밀도로 적용할 수 있다.

발산 성분 = 상체 질점 수평위치 + 상체 질점 수평속도/ω0 ……식10

수속 성분 = 상체 질점 수평위치 - 상체 질점 수평속도/ω0 ……식11

단, 여기에서의 상체 질점 수평위치는, 도 10에 도시하는 상기 동역학 모델에서는, 상체 질점 수평위치(Xb)를 나타낸다.

또, ω0는 어느 소정의 값이며, 주행시에는, 일본 특원 2000-352011호의 보행 보용 생성시의 값에 대해 약간 변경할 필요가 있다.

발산 성분 및 수속 성분에 관한 그 밖의 상세한 것은, 일본 특원 2000-352011호에 기술되어 있으므로, 여기에서는 이것 이상 언급하지 않는다.

본 실시형태에 있어서는, 일본 특원 2000-352011호에 나타낸 방식에 더하여, 더욱, 후술하는 바와 같이, 목표 상반력 연직성분 궤도를 규정하는 보용 파라미터를 설정하고, 이 목표 상반력 연직성분을 동역학적으로 만족하도록 로봇(1)의 전체 중심 연직위치를 결정한다. 이 경우, 상반력 연직성분의 2차 적분값이 로봇(1)의 전체 중심 연직위치를 규정하는 것으로 되므로, 목표 상반력 연직성분의 설정이 나쁘면, 로봇(1)의 전체 중심 연직위치 혹은 상체 연직위치가 지나치게 높아지거나 지나치게 낮아지거나 한다. 따라서, 목표 상반력 연직성분의 설정 방법도 중요한 과제이다. 그러나, 상반력 연직성분과 상체 연직위치와의 관계는, ZMP와 상체 수 평위치와의 관계와 유사하므로, 상체 수평위치 속도를 적절하게 하기 위한 목표 ZMP 결정수법의 일부를, 이하의 본 실시형태에 도시하는 바와 같이, 조금 변경하는 것만으로, 상체 연직위치 속도를 적절하게 하기 위한 목표 상반력 연직성분의 결정수법에 적용할 수 있다.

본제로 되돌아가, S022에서는, 도 14에 도시하는 플로차트에 따라서, 이하의 처리가 행해진다.

우선, S100에서, 금회 보용, 제 1 선회 보용, 제 2 선회 보용의 순서로 족평위치 자세 궤도가 이어지도록, 정상보용의 보용 파라미터중의 족평궤도 파라미터가 결정된다. 이하에 구체적인 설정 방법을 도 15를 참조하여 설명한다. 또한, 이후의 설명에서는, 지지다리측의 다리체(2)의 족평(22)을 지지다리 족평이라 부르고, 미착지 다리측의 다리체(2)의 족평(22)을 미착지 다리 족평이라 칭한다. 또, 보용의 「초기」, 「종단」은 각각 보용의 개시시각, 종료시각 혹은 그것들의 시각에서의 순시 보용을 의미한다.

족평궤도 파라미터는, 제 1 선회 보용 및 제 2 선회 보용의 초기 및 종단의 각각에서의 지지다리 족평 및 미착지 다리 족평의 각각의 위치 자세, 각 선회 보용의 보용 주기 등으로 구성된다. 이 족평궤도 파라미터중, 제 1 선회 보용 초기 미착지 다리 족평위치 자세는, 다음회 보용 지지다리 좌표계에서 본 금회 보용 종단 지지다리 족평위치 자세로 된다. 이 경우, 주행보용에서는, 금회 보용 종단에서의 지지다리 족평(22)은, 공중으로 이동해 있다. 그리고, 금회 보용 종단 지지다리 족평위치 자세는, 금회 보용 초기 지지다리 족평위치 자세(=전회 보용 종단 미착지 다리 족평위치 자세)로부터, 상기 요구 파라미터에서의 2보째의 미착지 다리측 족평(22)의 착지 예정 위치 자세의 요구값(금회 보용의 지지다리 족평(22)의 다음회 보용에서의 착지 예정 위치 자세의 요구값) 혹은 이 요구값에 대응하는 다음 다음회 보용 지지다리 좌표계에 따라 정해지는 다음회 보용 종단 미착지 다리 족평위치 자세에 이르는 족평위치 자세 궤도(상세하게는 다음회 보용 지지다리 좌표계에서 본 궤도)를, 금회 보용 종단까지 상기 유한시간 정정 필터를 사용하여 생성함으로써 구해진다.

또한, 다음회 보용 종단 미착지 다리 족평위치 자세는, 그 위치 자세로부터 족평(22)을 접지시킨 채, 발끝을 내리도록 이 족평(22)을 수평자세까지 피칭 방향으로 소정 각도 회전시켰을 때의 이 족평의 위치 자세가 다음 다음회 보용 지지다리 좌표계의 위치 자세에 합치하도록 결정된다. 바꾸어 말하면, 다음회 보용 종단 미착지 다리 족평위치 자세는, 상기 요구 파라미터에서의 2보째의 미착지 다리측 족평(22)의 착지 위치 자세의 요구값으로부터, 이 족평(22)을 미끄러지지 않도록 접지시킨 채, 발끝을 들어 올리는 것과 같이 이 족평(22)을 피칭 방향으로 소정 각도 회전시킨 상태(발끝을 들고 발뒤꿈치를 착지시킨 상태)에서의 이 족평(22)의 위치 자세이다.

또, 제 1 선회 보용 초기 지지다리 족평위치 자세는, 다음회 보용 지지다리 좌표계에서 본 금회 보용 종단 미착지 다리 족평위치 자세로 된다. 이 경우, 금회 보용 종단 미착지 다리 족평위치 자세는, 상기 다음회 보용 종단 미착지 다리 족평위치 자세의 경우와 동일하게, 상기 다음회 보용 지지다리 좌표계 혹은 이것에 대 응하는 상기 요구 파라미터의 1보째(금회 보용)의 미착지 다리 착지 예정 위치 자세의 요구값에 따라 정해지는 것이다. 즉, 이 금회 보용 종단 미착지 다리 족평위치 자세는, 그 위치 자세로부터, 족평(22)을 접지시킨 채, 발끝을 내리는 것과 같이 이 족평(22)을 회전시키고 이 족평(22)의 바닥면의 거의 전체면을 바닥면에 접지시켰을 때의 이 족평의 대표점이 다음회 보용 지지다리 좌표계의 원점에 합치하도록 결정된다.

제 1 선회 보용 종단 미착지 다리 족평위치 자세는, 금회 보용 종단 미착지 다리 족평위치 자세나 다음회 보용 종단 미착지 다리 족평위치 자세의 결정수법과 마찬가지로, 다음회 보용 지지다리 좌표계에서 본 다음 다음회 보용 지지다리 좌표계의 위치 자세에 기초하여 결정된다. 보다 구체적으로는, 제 1 선회 보용 종단 미착지 다리 족평위치 자세는, 그 위치 자세로부터 족평(22)을 접지시킨 채, 미끄러지지 않도록 이 족평(22)을 수평자세까지 소정 각도 회전시켰을 때의 족평위치 자세가, 다음회 보용 지지다리 좌표계에서 본 다음 다음회 보용 지지다리 좌표계의 위치 자세에 합치하도록 설정된다.

제 1 선회 보용 종단에서는, 지지다리 족평(22)은 이상하여 공중에 있다. 지지다리 족평(22)이 이상하고나서 뒤의 궤도를 결정하기 위해서, 제 1 선회 보용 지지다리 족평 착지 예정 위치 자세가 설정된다. 제 1 선회 보용 지지다리 족평 착지 예정 위치 자세는, 다음회 보용 지지다리 좌표계에서 본 다음 다음 다음회 보용 지지다리 좌표계의 위치 자세에 기초하여 설정된다. 보다 구체적으로는, 제 1 선회 보용 지지다리 족평 착지 예정 위치 자세는, 다음회 보용 지지다리 좌표계에 서 본 다음 다음 다음회 보용 지지다리 좌표계의 위치 자세이다. 또한, 다음 다음 다음회 보용 지지다리 좌표계는, 다음 다음회 보용 지지다리 좌표계와 다음 다음 다음회 보용 지지다리 좌표계와의 상대적 위치 자세관계가, 금회 보용 지지다리 좌표계와 다음회 보용 지지다리 좌표계와의 상대적 위치 자세 관계와 일치하도록 설정된다.

제 1 선회 보용 종단 지지다리 족평위치 자세는, 제 1 선회 보용 초기 지지다리 족평위치 자세를 구한 경우와 마찬가지로, 제 1 선회 보용 초기 지지다리 족평위치 자세로부터, 상기 제 1 선회 보용 지지다리 족평 착지 예정 위치 자세에 이르는 족평위치 자세 궤도(보다 상세하게는 다음회 보용 지지다리 좌표계에서 본 궤도)를, 제 1 선회 보용 종단까지 상기 유한시간 정정 필터를 사용하여 생성함으로써 구해진다.

제 2 선회 보용 초기 미착지 다리 족평위치 자세는, 다음 다음회 보용 지지다리 좌표계에서 본 제 1 선회 보용 종단 지지다리 족평위치 자세로 된다. 제 2 선회 보용 초기 지지다리 족평위치 자세는, 다음 다음회 보용 지지다리 좌표계에서 본 제 1 선회 보용 종단 미착지 다리 족평위치 자세로 된다.

제 2 선회 보용 종단 미착지 다리 족평위치 자세는, 금회 보용 지지다리 좌표계에서 본 금회 보용 종단 미착지 다리 족평위치 자세로 된다. 제 2 선회 보용 종단 지지다리 족평위치 자세는, 금회 보용 지지다리 좌표계에서 본 금회 보용 종단 지지다리 족평위치 자세로 된다.

제 1 선회 보용 및 제 2 선회 보용의 보용 주기는, 다음회 보용 주기와 동일 하게 설정된다. 이것들의 제 1 선회 보용 및 제 2 선회 보용의 보용 주기는, 서로 동일하게 하는 것은 반드시 필요하지는 않지만, 어느 주기도, 적어도 다음회 보용 주기에 따라 결정하는 것이 바람직하다. 또한, 금회 보용, 제 1 선회 보용 및 제 2 선회 보용의 상기 이외의 운동 파라미터(양 다리 지지기 시간 등의 시간 파라미터를 포함함)는, 상기 결정된 파라미터에 따라, 보용의 조건(액추에이터의 속도가 허용범위에 들어있든지, 가동각를 넘지 않았는지, 바닥 등과 간섭하고 있지 않은지 등)을 만족하도록 적당하게 결정한다.

다음에, S102로 진행하고, 목표 상체 자세가 추종해야 할 기준 상체 자세 궤도를 규정하는 기준 상체 자세 궤도 파라미터가 결정된다. 기준 상체 자세는, 정상보용의 초기(제 1 선회 보용의 초기)와 종단(제 2 선회 보용의 종단)에서 이어지도록(정상보용의 초기, 종단에서의 기준 상체 자세의 자세각 및 그 각속도가 일치하도록) 설정되어 있는 한, 일정 자세일 필요는 없지만, 본 실시형태에서는 이해를 쉽게 하기 위해서, 기준 상체 자세는, 직립 자세(연직 자세)로 설정된다. 즉, 본 실시형태에서는, 기준 상체 자세는, 정상보용의 전체 기간에서 직립 자세로 설정된다. 따라서, 본 실시예에서는 기준 상체 자세의 자세각의 각속도 및 각가속도는 0이다.

다음에 S104로 진행하고, 팔자세 궤도 파라미터, 보다 상세하게는 연직축(혹은 상체 체간축) 주위의 양 팔체의 각 운동량 변화에 관한 것 이외의 팔자세 궤도 파라미터가 결정된다. 예를들면, 상체(24)에 대한 팔체의 손끝의 상대 높이나 팔 전체의 상대 중심위치 등의 팔자세 궤도 파라미터가 결정된다. 또한, 본 실시예에 서는 팔 전체의 상대 중심위치는, 상체에 대해 일정하게 유지되도록 설정된다.

다음에 S106으로 진행하고, 상반력 연직성분 궤도 파라미터가 설정된다. 이 경우, 이 파라미터에 의해 규정되는 상반력 연직성분 궤도가, 제 1 선회 보용 및 제 2 선회 보용중 어디에 있어도 도 6과 같이 실질적으로 연속한(값이 스텝 형상으로 튀지 않음) 것이 되도록 상반력 연직성분 궤도 파라미터가 설정된다. 즉, 정상 선회 보용의 목표 상반력 연직성분 궤도는 도 16과 같은 패턴으로 설정된다. 그 패턴에서는, 제 1 선회 보용 및 제 2 선회 보용중 어디에 있어도, 편각지지기에서는 상반력 연직성분이 사다리꼴 형상으로 변화되고, 공중기에서는 상반력 연직성분이 0에 유지된다. 그리고 이 패턴의 꺾임점의 시각이나, 사다리꼴 부분의 높이(피크값)가 상반력 연직성분 궤도 파라미터로서 설정된다.

이 상반력 연직성분 궤도 파라미터의 설정시에는, 상반력 연직성분의 보용 전체 기간(제 1 선회 보용과 제 2 선회 보용 양쪽의 기간을 합친 기간에서 정상보용의 1주기의 기간)에서의 평균값을 로봇(1)의 자중과 일치시킨다. 즉, 상반력 연직성분의 평균값이 로봇(1)에 작용하는 중력과 동일한 크기이고 반대 방향이 되도록 한다.

상기와 같이 상반력 연직성분 궤도를 설정하는 것은, 정상보용의 조건을 충족시키기 위해서 필요하다. 정상보용의 조건이란, 지지다리 좌표계(지지다리측 족평(22)의 접지면에 설정된 좌표계)에서 본 보용의 모든 상태변수(로봇(1)의 각 부의 위치, 자세, 속도 등)의 초기 상태(제 1 선회 보용의 초기 상태)와 다음 지지다리 좌표계(다음의 제 1 선회 보용의 지지다리 좌표계)에서 본 보용의 종단 상태(제 2 선회 보용의 종단상태)가 일치하고 있는 것이다(이하, 이 조건을 정상보용의 경계 조건이라고 하는 일이 있음). 따라서, 정상보용 종단에서의 로봇(1)의 전체 중심 연직속도와 정상보용 초기에서의 전체 중심 연직속도와의 차이(상세하게는, 제 2 선회 보용 종단의 전체 중심 연직속도와 제 1 선회 보용 초기의 전체 중심 연직속도와의 차이)도 0이 되어야 한다. 상기 차이는 상반력 연직성분과 중력과의 차이의 적분값(1차 적분값)이므로, 상기 차이를 0으로 하기 위해서는, 상기와 같이 상반력 연직성분 궤도를 설정할 필요가 있다.

또한, 본 실시예에서는 상반력 연직성분의 제 1 선회 보용, 제 2 선회 보용 각각의 기간에 있어서의 평균값을 로봇(1)의 자중과 일치시키도록 했다. 보다 구체적으로는, 예를 들면 제 1 선회 보용 및 제 2 선회 보용의 보용 주기에 따라 각 선회 보용에서의 상반력 연직성분 궤도의 사다리꼴 부분의 꺾임점의 시각을 설정한 뒤에, 그 사다리꼴 부분의 높이를, 상반력 연직성분의 제 1 선회 보용, 제 2 선회 보용 각각의 기간에서의 평균값이 로봇(1)의 자중과 일치하도록 결정했다(사다리꼴의 높이를 미지수로 하여, 상기 평균값 및 자중의 일치 조건을 나타내는 방정식을 풀어서 사다리꼴의 높이를 구함).

이렇게 함으로써, 제 1 선회 보용 종단의 전체 중심 연직속도와 제 1 선회 보용 초기의 전체 중심 연직속도와의 차이도 0, 제 2 선회 보용 종단의 전체 중심 연직속도와 제 2 선회 보용 초기의 전체 중심 연직속도와의 차이도 0이 된다. 단, 이와 같이 할 필연성은 없다. 예를 들면, 제 1 선회 보용과 제 2 선회 보용과의 경계근처에서 상체 연직위치가 지나치게 높아지거나 지나치게 낮아지게 되거나 해 서 무리한 자세가 될 것 같은 경우에는, 각 선회 보용에서 상기 평균값 및 자중을 일치시킨 상태로부터 각 선회 보용의 상반력 연직성분 궤도의 사다리꼴의 높이 등을 수정하도록 해도 좋다.

다음에 S108로 진행하고, 상기와 같이 도 16과 같이 설정된 상반력 연직성분 궤도에 따라, 상반력 수평성분의 허용범위[Fxmin, Fxmax](보다 상세하게는 이것을 규정하는 파라미터)가 도 17과 같이 설정된다. 도 17의 마이너스측의 꺾은선이 상반력 수평성분 허용 하한값(Fxmin), 플러스측의 꺾은선 수평성분 허용 상한값(Fxmax)을 나타낸다. 이것들의 설정 방법에 관해서 이하에 보충한다. 이하에서는, 바닥면이 수평인 경우에 대하여 설명한다.

상반력 수평성분은, 바닥과 족평(22) 사이의 마찰에 의해 발생하지만, 마찰은 얼마든지 발생할 수 있는 것은 아니고, 한계가 있다. 따라서, 생성된 목표 보용에 따라서 실제의 로봇(1)이 이동했을 때에 슬립하지 않도록 하기 위해서는, 목표 보용의 상반력 수평성분이, 항상 마찰 한계 이내로 되어야 한다. 그래서, 이 조건을 만족시키기 위해서, 상반력 수평성분 허용범위를 설정하고, 후술하는 바와 같이, 목표 보용의 상반력 수평성분이 이 허용범위내가 되도록, 목표 보용을 생성하는 것으로 했다.

바닥과 족평(22) 사이의 마찰계수를 μ라고 하면, Fxmin은, 항상 -μ* 상반력 연직성분 이상으로, Fxmax는 μ* 상반력 연직성분 이하로 설정되어야 한다. 가장 단순한 설정 방법은, 다음 식에 의한 설정이다. 단, ka는 1보다 작은 플러스의 정수이다.

Fxmin = -ka*μ*상반력 연직성분

Fxmax= ka*μ*상반력 연직성분 ……식12

도 17의 상반력 수평성분 허용범위는, 식 12에 따라서 설정한 예이다. 상반력 수평성분 허용범위를 규정하는 파라미터로서, 도 17의 사다리꼴 파형 등의 꺾임점에서의 값과 시각을 설정해도 좋지만, 식12에 의해 상반력 수평성분 허용범위를 결정할 때에는, 단지 식12에서의 (ka*μ)의 값을 파라미터로서 설정하는 것만으로도 좋다.

또한, 상기 조건(목표 보용의 상반력 수평성분을 항상 마찰 한계 이내로 한다는 조건)을 만족하고 있으면, 다른 설정 방법에서 상반력 수평성분 허용범위를 설정해도 상관없다.

다음에 S110로 진행하고, 제 1 선회 보용 및 제 2 선회 보용을 합친 정상보용의 ZMP 궤도를 규정하는 ZMP 궤도 파라미터가 설정된다. 이 경우, 목표 ZMP 궤도는, 상술한 바와 같이 안정 여유가 높고 또한 급격한 변화를 하지 않도록 설정된다.

더욱 상세하게는, 도 5의 주행보용에서는, 지지다리 족평(22)의 발뒤꿈치로 착지한 후, 잠시 뒤 이 지지다리 족평(22)의 바닥면의 거의 전체면이 접지하고, 또한 잠시 뒤, 지지다리 족평(22)의 발끝만이 접지한다. 그리고, 다음에 지지다리 족평(22)의 발끝으로 차서 공중으로 뛰어오르고, 최후에 미착지 다리 족평(22)의 발뒤꿈치로 착지한다. 또, 목표 ZMP는 접지면내에 존재해야 한다. 그래서, 본 실시예에서는 정상보용의 제 1 선회 보용 및 제 2 선회 보용의 각각의 목표 ZMP의 X 축방향의 위치는, 상기 도 7의 상단 도면에 도시한 바와 같이, 지지다리 족평(22)의 발뒤꿈치를 초기 위치로 하고, 이 족평(22)의 바닥면의 거의 전체가 접지할 때까지 그 위치에 머무르도록 설정된다. 다음에, 목표 ZMP는 지지다리 족평(22)의 중앙으로 이동하고, 이 족평(22)이 발끝 접지 상태가 될 때까지 발끝으로 이동하고, 그 후에 이상시까지 지지다리 족평(22)의 발끝에 머무르도록 설정된다. 더욱이 그 후는, 목표 ZMP는, 상기와 같이, 다음 미착지 다리 족평(22)의 착지까지 목표 ZMP가, 지지다리 족평(22)의 발끝으로부터 미착지 다리 족평(22)의 발뒤꿈치의 착지 위치까지 연속적으로 이동하도록 설정된다. 따라서, 제 1 선회 보용과 제 2 선회 보용으로 이루어지는 정상보용의 목표 ZMP 궤도(X축방향의 궤도)는, 도 18과 같이 된다. 그리고, 이 목표 ZMP 궤도의 꺾임점의 시각 및 위치가 ZMP 궤도 파라미터로서 설정된다. 이 경우, 꺾임점의 시각은, 상기 요구 파라미터에 따라 정한 제 1 선회 보용 및 제 2 선회 보용의 보용 주기에 따라 설정되고, 이 꺾임점의 위치는, 다음회 보용 지지다리 좌표계 및 다음 다음회 보용 지지다리 좌표계의 위치 자세 또는 이들 좌표계를 규정하는 요구 파라미터의 1보째 및 2보째의 미착지 다리측 족평 착지 예정 위치 자세의 요구값에 따라 설정된다. 또한, ZMP 궤도의 Y축 방향의 위치는, 상기 도 7의 하단 도면에 도시한 것과 동일하게 설정된다. 보다 상세하게는, 제 1 선회 보용에서의 목표 ZMP의 Y축방향 위치의 궤도는, 도 7의 하단 도면의 것과 동일 패턴으로 설정되고, 제 2 선회 보용에서의 목표 ZMP의 Y축방향 위치의 궤도는, 제 1 선회 보용의 것과 동일한 궤도에서, 이 궤도의 종단에 이어지는 것에 설정된다.

최후로, S112로 진행하고, 정상보용 1보(1주기)의 초기 시각, 종단 시각, 기간이 이하와 같이 재정의된다.

정상보용은, 그 초기와 종단에서의 상태변수가 연속적으로 이어지는 보용이어야 한다. 이와 같은 보용의 결정을 쉽게 하기 위해서, 본 실시예에서는 정상보용을 결정할 때까지는, 상술의 협의의 보용의 정의와는 달리, 정상보용의 1보의 초기, 종단, 기간을 편의상, 도 16과 같이 정한다. 즉, 제 1 선회 보용의 편각지지기의 후반에서 상반력 연직성분이 어느 정도 감소한 시각을 정상보용의 초기 시각 Ts로서 설정한다. 또한, 초기 시각 Ts는, 도 7에 도시하는 바와 같이 지지다리 족평(22)의 바닥면의 거의 전체면이 접지한 상태로부터 발끝접지로 변하는 순간 또는 그 직전의 시각(도 7의 발바닥 전체면 접지 기간의 종료 시각 혹은 그 직전의 시각)에 설정하는 것이 바람직하다. 또, S110에서 설정한 도 18(또는 도 7)에 도시하는 목표 ZMP와 시각 Ts와의 관계를 설명하면 제 1 선회 보용에서 지지다리 족평(22)의 바닥면의 거의 전체면이 접지한 후에, 목표 ZMP가 지지다리 족평(22)의 중앙으로 이동하고, 발끝접지 상태가 될 때까지 발끝에 이동을 완료한 순간이, 시각 Ts로 되어 있는 것이 바람직하다. 이와 같은 초기 시각 Ts는 예를 들면 먼저 설정한 목표 ZMP 궤도 파라미터에 따라 설정된다. 상기와 같이 초기 시각 Ts를 설정하는 이유는 후술한다.

또, 도 16에 도시하는 바와 같이 정상보용의 주기 Tcyc는 제 1 선회 보용과 제 2 선회 보용의 보용 주기의 합이다. 정상보용의 종단시각을 Te로 한다. Te는 Ts에 Tcyc를 더한 시각으로 설정된다.

정상보용이 결정된 시점(후술의 도 19의 S204의 루프를 빠져나온 시점)으로부터, 다시, 보용의 초기, 종단 등의 정의를, 전술의 협의의 보용의 정의에 되돌리는 것으로 한다. 이하의 설명에서는, 전술의 협의의 보용의 정의에 의한 초기 시각(지지다리측 족평(22)의 착지 당초의 시각)을 0으로 하고, 정상보용을 결정할 때까지 사용하는 상기 초기 시각 Ts를 참조 부호 Ts를 사용하여(단순히 「Ts」라고 생략하는 일도 있음) 본래의 초기 시각 0과 구별한다.

도 12의 S010으로부터 S022까지에 나타나 있는 처리가 행해진 후, S024로 진행하고, 정상보용의 초기 상태가 산출된다. 여기에서 산출되는 초기 상태는, 정상보용의 초기 상체 수평위치 속도(수평방향에서의 초기 상체 위치 및 초기 상체 속도), 초기 상체 연직위치 속도(연직방향에서의 초기 상체 위치 및 초기 상체 속도), 초기 발산 성분, 초기 상체 자세각 및 그 각속도이다. 이 초기 상태의 산출은, 도 19의 플로차트에 따라서, 탐색적으로 행해진다.

도 19 플로차트에서는, 우선, S200에서, 정상보용의 보용 파라미터(상기 도 12의 S022에서 설정한 파라미터)에 기초하여, 목표 족평위치 자세, 목표 팔자세, 및 목표 상체 자세각(자세각과 방향)의 초기 상태(초기 시각 Ts에서의 상태)가 결정된다. 여기에서 상태란, 위치나 자세각과 그 변화율(시간 미분)을 나타낸다.

이 경우, 지지다리측의 목표 족평위치 자세의 초기 상태는, 상기 도 14의 S100에서 결정한 족평 궤도 파라미터의 제 1 선회 보용 초기 지지다리 족평위치 자세로부터, 제 2 선회 보용 종단 미착지 다리 족평위치 자세에 달하는 족평위치 자세 궤도(다음회 보용 지지다리 좌표계에서 본 궤도)를 시각 Ts까지 유한시간 정정 필터에 의해 생성함으로써 결정된다. 미착지 다리측의 목표 족평위치 자세의 초기 상태는, 다음회 보용 지지다리 좌표계에서 본 금회 보용 초기 지지다리 족평위치 자세로부터, 제 1 선회 보용 종단 미착지 다리 족평위치 자세에 달하는 족평위치 자세 궤도를 시각 Ts까지 유한시간 정정 필터에 의해 생성함으로써 결정된다. 또, 목표 팔자세의 초기 상태는, 도 14의 S104에서 결정한 팔자세 궤도 파라미터에 기초하여 시각 Ts에서의 팔자세(상체(24)에 대한 양 팔체의 전체 중심위치 등)를 구함으로써 결정된다. 또, 목표 상체 자세각의 초기 상태는, 도 14의 S102에서 결정한 기준 상체 자세 궤도 파라미터에 의해 정해지는 시각 Ts에서의 기준 상체 자세(자세각) 및 그 각속도가 이 목표 상체 자세각의 초기 상태로서 결정된다. 또한, 본 실시예에서는, 기준상태 자세각은 연직자세이므로, 목표 상체 자세각의 초기 상태(자세각 및 그 각속도)는 0이다. 또, 본 실시형태에서는 정상보용의 목표 족평위치 자세 궤도, 상반력 연직성분 궤도, 및 목표 ZMP 궤도는, 각각, 도 14의 플로차트에서 결정한 족평궤도 파라미터, 상반력 연직성분 궤도 파라미터, ZMP 궤도 파라미터에 의해 서로 독립적으로 결정된다. 예를 들면 정상보용의 순시 순시의 목표 족평위치 자세는, 상반력 연직성분의 순시값에 의존하지 않고 족평궤도 파라미터에 따라 결정된다.

이어서 S202에서 초기 상체 수평위치 속도의 후보(즉 초기 시각 Ts에서의 상체 수평위치 속도 후보)인(Xs, Vxs)(Xs: 수평위치, Vxs: 수평속도)가 가결정된다. 여기에서 가결정되는 후보(Xs, Vxs)는, 임의여도 좋지만, 예를 들면 전회 보용의 생성시에 구한 정상보용의 초기 상태에서의 상체 수평위치 속도를 가결정 후보(Xs, Vxs)로 하면 좋다.

또한, 설명을 간략하게 하기 위해서, 사지탈 플레인상에서 X방향(전후방향)에서의 정상보용의 초기 상태를 탐색하는 경우를 예로 들지만, 실제로는 위치·속도모두 X방향(전후방향), Y방향(좌우방향)을 각각 또는 동시에 정상보용의 초기 상태(정상보용의 상기 경계조건을 충족시키는 초기 상태)를 탐색할 필요가 있다.

탐색적인 결정 수법으로서는, 유사 야코비안(감도 매트릭스)을 구하고, 최급강하법 등에 의해 다음 후보를 결정하는 방법이나, 심플렉스법 등을 사용하면 좋다. 본 실시형태에서는, 최급강하법을 사용한다.

이어서, S204를 거쳐서 S206에서, 초기(시각 Ts) 상체 연직위치 속도(Zs, Vzs)(Zs: 연직위치, Vzs: 연직속도)가 결정된다.

초기 상체 연직속도(Vzs)는, 본 실시형태에서는 이하와 같이 해석적으로 결정된다.

로봇(1)에는, 동역학적 관계로서, 다음 식이 성립한다.

종단 전체 중심 연직위치 - 초기 전체 중심 연직위치

=(상반력 연직성분/로봇의 전체 질량)의 2차 적분

+ 중력가속도의 2차 적분

+ 초기 전체 중심 연직속도*1보의 시간

……식13

(단, 중력가속도는 마이너스 값으로 한다.)

또, 정상보용에서는, 종단 전체 중심 연직위치와 초기 전체 중심 연직위치는 일치하므로, 상기 식13의 우변은 0이 되어야 한다. 따라서 이것들의 관계로부터 초기 전체 중심 연직속도를 구할 수 있다. 구체적으로는, 우선, 상기 도 14의 S104에서 설정한 상반력 연직성분 파라미터에 의해 산출되는 상반력 연직성분을 로봇(1)의 전체 질량으로 나눈 값을 1보의 기간(시각 Ts로부터 시각 Te까지) 2차 적분함으로써, 상반력 연직성분에 의한 전체 중심 이동량(식13의 우변 제 1 항)이 구해진다. 더욱이, 중력가속도를 1보의 기간 2차 적분함으로써, 중력에 의한 전체 중심 이동량(식13의 우변 제 2 항)이 구해진다. 그리고 상기 구한 상반력에 의한 전체 중심 이동량과 중력에 의한 전체 중심 이동량과의 합을 부호 반전하고, 이것을 1보의 시각(정상보용의 1주기(Tcycl)의 시각)으로 나눔으로써, 초기 전체 중심 연직속도가 구해진다.

다음에 초기 상체 연직위치(Zs)를 구하기 위해서, 다음 식을 사용하여, 시각 0의 전체 중심 연직속도가 구해진다. 이 때, 다음 식에서의 시각 Ts의 전체 중심 연직속도에는, 식13에 기초하여 상기한 바와 같이 구한 값을 대입하고, 적분기간은 시각 0으로부터 시각 Ts까지로 한다.

시각 Ts의 전체 중심 연직속도 - 시각 0의 전체 중심 연직속도

= (상반력 연직성분/로봇의 전체 질량)의 1차 적분

+ 중력가속도의 1차 적분

……식14

(단, 중력가속도는 마이너스의 값으로 한다.)

다음에, 본 출원인이 먼저 일본 특개평 10-86080호 공보에서 제안한 상체 높 이 결정수법을 사용하여 시각 0에서의(즉 착지 순간에서의) 상체 높이(상체 연직위치)가 결정된다. 이 때, 시각 0에서의 족평위치 자세(상기 도 14의 S100에서 결정한 제 1 선회 보용 초기(시각 0) 지지다리 족평위치 자세 및 제 1 선회 보용 초기(시각 0) 미착지 다리 족평위치 자세)와, 각 다리체(2)의 무릎부의 구부림각에 관한 소정의 기하학적 조건에 기초하여, 시각 0에서의 각 다리체(2, 2)의 무릎부가 다 펴지않는 상체 연직위치(상체 높이)가 결정된다. 구체적으로는, 지지다리측 다리체(2)의 무릎 구부림각를 θsup, 미착지 다리측 다리체(2)의 무릎 구부림각를 θswg로 했을 때, 예를 들면 그것들의 무릎 구부림각(θsup, θswg)의 정현값의 역수의 합이 어떤 소정값(유한값)이 되도록, 상체 연직위치가 결정된다. 여기에서, 무릎 구부림각(θsup, θswg)은, 각 다리체(2)의 대퇴부의 축심에 대한 하퇴부의 축심의 각도이며, 각 다리체(2)가 다 펴진 상태로부터 무릎부를 구부려감에 따라, 0으로부터 증가해 가는 각도이다. 또한, 이와 같은 상체(24)의 연직위치의 결정수법은, 상기 일본 특개평 10-86080호 공보에 상세하게 설명되어 있으므로, 여기에서는 이상의 설명에 멈춘다.

다음에, 이와 같이 결정한 시각 0에서의 상체 연직위치나, 족평위치 자세 등으로부터 시각 0에서의 전체 중심 연직위치가 로봇(1)의 모델을 사용하여 구해진다. 예를 들면 도 10의 모델에 대응하는 상기 식04의 키네매틱스 모델을 사용함으로써, 시각 0에서의 전체 중심 연직위치가 구해진다. 구체적으로는, 시각 0에서의 상체 연직위치와 상체 자세각(본 실시형태에서는 0)으로부터 상체 질점(24m)의 연직위치가 구해지고, 시각 0에서의 지지다리 족평위치 자세 및 미착지 다리 족평위 치 자세(이것들은, 상기 도 14의 S100에서 설정한 제 1 선회 보용 초기 지지다리 족평위치 자세 및 제 1 선회 보용 초기 미착지 다리 족평위치 자세임)로부터 각각 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직위치가 구해진다. 그리고 이들 구한 상체 질점(24m), 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직위치를, 상기 식04의 Zb, Zsup, Zswg에 대입함으로써, 전체 중심 연직위치(식04의 ZGtotal)가 구해진다. 또한, 로봇(1)의 보다 엄밀한 모델(예를 들면 도 11에 도시하는 바와 같은 다질점 모델)을 사용하고, 시각 0에서의 상체 수평위치나 팔자세도 고려하여, 시각 0에서의 전체 중심 연직위치를 구하도록 해도 좋다.

다음에 상기 식13에서, 상기 구한 시각 0에서의 전체 중심 연직위치를 동 식13의 좌변의 초기 전체 중심 연직위치에 대입하고, 상기 구한 시각 0의 전체 중심 연직속도를 동 식13의 우변의 초기 전체 중심 연직속도에 대입하고, 또한, 동 식13의 1보의 시각을 Ts로 하고, 적분기간을 시각 0로부터 시각 Ts까지로 함으로써, 식13의 좌변의 종단 전체 중심 연직위치의 값이 산출되고, 이 산출값이 초기(시각 Ts) 전체 중심 연직위치로서 얻어진다. 또한, 이 구한 초기(시각 Ts) 전체 중심 연직위치와 시각 Ts에서의 족평위치 자세(S200에서 구한 것) 등으로부터, 로봇(1)의 모델(예를 들면 식04의 키네매틱스 모델)을 사용하여, 초기(시각 Ts) 상체 연직위치(Zs)가 구해진다. 구체적으로는, 시각 Ts에서의 지지다리 및 미착지 다리의 각 평위치 자세로부터, 도 10의 모델의 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직위치가 구해지고, 이것들의 연직위치와, 상기 구한 초기(시각 Ts) 전체 중심 연직위치를 식04에 적용함으로써, 상체 질점(24m)의 연직위치(식04의 Zb)가 구해진다. 그리고, 이 상체 질점(24m)의 연직위치와 시각 Ts에서의 상체 자세(본 실시형태에서는 0)로부터 역산적으로 초기(시각 Ts) 상체 연직위치(Zs)가 구해진다. 또한, 이 경우도, 보다 엄밀한 모델을 사용하여, 상체 수평위치나 팔자세도 고려하여 초기 상체 연직위치(Zs)를 구하도록 해도 좋다.

최후에, 상기 S200에서 결정한 족평위치 자세의 초기 상태나, S206에서 구한 초기 전체 중심 연직속도 등으로부터, 로봇(1)의 모델(예를 들면 식04의 키네매틱스 모델)을 사용하여, 초기 상체 연직속도가 구해진다. 구체적으로는, 식04의 양변을 시간 미분하여 이루어지는 식에, 상기 식13에 기초하여 구한 초기 전체 중심 연직속도와, 지지다리측 및 미착지 다리측의 각 족평위치 자세의 초기 상태에 의해 정해지는 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직속도를 적용함으로써, 상체 질점(24m)의 연직속도가 구해진다. 그리고 이 상체 질점(24m)의 연직속도와 상체 자세각의 초기 상태(S200 혹은 후술의 S208에서 결정된 것)로부터, 상체 연직속도가 구해진다. 또한, 각 족평위치 자세의 초기 상태나, 초기 전체 중심 연직속도 이외에, 팔자세의 초기 상태(S200에서 결정된 것), 가결정한 상체 수평위치의 초기 상태(S202 혹은 후술의 S216 혹은 S218에서 가결정된 최신의 것), 상기 구한 초기 상체 연직위치를 고려하고, 상기 구한 초기 전체 중심 연직속도를 만족하는 초기 상체 연직속도를, 로봇(1)의 보다 엄밀한 모델을 사용하여 구하도록 해도 좋다.

보충하면, 로봇(1)의 모델이 도 11과 같은 복잡한 것(엄밀한 모델)이라도, 로봇(1)은, 한쪽 다리당 6자유도이므로, 족평위치 자세 궤도 및 상체 위치 자세 궤 도의 초기 상태(위치 자세와 그 변화율)이 주어지면, 로봇의 초기 상태는 일의적으로 모두 결정되게 된다. 따라서, 초기 전체 중심 연직속도도 이것들이 주어지면 일의적으로 결정된다. 역으로, 한쪽 다리당 6자유도이므로, 족평위치 자세 궤도 및 상체 위치 자세 궤도의 초기 상태의 내에서 속도에 관한 상태의 하나(예를 들면 초기 상체 연직속도)가 미정이더라도, 초기 전체 중심 연직속도가 주어지면, 미정의 초기 상태는 일의적으로 결정된다.

S206의 처리후, 이어서 S208로 진행하고, 정상 선회 보용이 임시로 생성된다. 더 구체적으로는, 상기 도 12의 S022에서 결정한 정상보용의 보용 파라미터를 기초로, 초기 시각 Ts로부터 종단시각 Te까지의 각 순간에서의 목표 ZMP, 목표 상반력 연직성분, 목표 족평위치 자세, 기준 상체 자세, 목표 팔자세 및 상반력 수평성분 허용범위가 축차 구해진다. 그리고, 구한 목표 ZMP와 목표 상반력 연직성분에 관한 동역학적 평형 조건, 및 상반력 수평성분 허용범위의 조건을 만족하도록, 상기 동역학 모델(도 10의 모델)을 사용하여, 상기 상체 수평위치 속도(Xs, Vxs), 상체 연직위치 속도(Zs, Vzs)를 상체(24)의 초기(시각 Ts) 상태로 하여, 상체 위치 자세를 축차 결정함으로써, 시각 Ts로부터 종단시각 Te까지의 보용을 생성한다. 또, 이 때, 상체 자세는, 기준 상체 자세로 되도록이면 일치하도록 생성된다.

또한, 이 정상보용의 보용 생성은 보용 생성 장치(100)의 내부에서 행해지는 것 뿐이며, 실제의 로봇(1)을 구동하기 위한 목표값으로서, 후술하는 복합 컴플라이언스 동작 결정부(104)에 출력되는 일은 없다.

이하에, S208의 처리인 축차 연산에 의한 정상보용 생성 처리의 상세를 설명 한다.

도 20은 그 처리를 나타내는 서브루틴 플로차트이다.

이하 설명하면, S300에서 여러가지 초기화가 행해진다. 구체적으로는, 가보용 생성용 시각 k에 초기 시각 Ts를 대입한다. 또한, 상체 수평위치 속도에, 현재 가결정되어 있는 (Xs, Vxs)(도 19의 S202 혹은 후술의 S216 혹은 S218에서 결정된 것)가 대입되고, 상체 연직위치 속도에, 상기 S206에서 구한 최신의 (Zs, Vzs)가 대입된다. 또, 목표 상체 자세각에는, 기준 상체 자세각 초기값(시각 Ts에서의 기준 상체 자세각), 목표 상체 자세각속도에는, 기준 상체 자세각속도 초기값(시각 Ts에서의 기준 상체 자세각속도)을 대입한다. 보충하면, 후술하는 바와 같이 최종적으로는 초기 목표 상체 자세각속도는 변경되므로, 기준 상체 자세각속도 초기값과 다른 값을 대입해도 좋다.

이어서, S302을 거쳐서 S304에서, 가보용 생성용 시각 k가 보용 종단 시각 이전인지 아닌지(k≤Ts+Tcyc인지 아닌지)가 판단되고, 그 판단 결과가 YES인 경우에는, S306의 보용 순시값 결정 서브루틴으로 진행하고, 보용 순시값이 결정된다. 이어서 보용 생성 장치(100)의 처리는, S308로 진행하고, 가보용 생성용 시각 k를 △k만큼 늘린 후, S304로 돌아간다.

여기에서, △k는, 가보용 생성의 인터벌이며, 통상, 제어 주기 △t와 일치시켜 두면 좋다. 또한, △k는, 가보용의 동역학적 정밀도를 떨어뜨려도 좋으면, 연산량을 저감하기 위해서, △k를 △t보다도 길게 설정해도 좋다.

S304의 판단 결과가 NO일 경우에는, S310로 진행한다. 이상의 처리에 의해, S310로 진행할 때까지 정상보용이 초기로부터 종단까지 생성된다.

이하에, S306의 보용 순시값 결정 서브루틴의 상세를 도 21을 참조하여 설명한다.

우선 도 21의 S400에서, 정상보용 파라미터(상반력 연직성분 궤도 파라미터)를 기초로, 상기 도 16에 나타낸 목표 상반력 연직성분의 시각 k에서의 값(금회값)이 구해진다. 더욱, S402에서, 정상보용 파라미터(ZMP 궤도 파라미터)를 기초로, 상기 도 18에 나타낸 목표 ZMP 궤도의 시각 k에서의 값(금회값)이 구해진다.

이어서 S404로 진행하고, 정상보용 파라미터(족평궤도 파라미터, 기준 상체 자세 궤도 파라미터, 및 팔자세 궤도 파라미터)를 기초로, 시각 k에서의 목표 양 족평위치 자세(지지다리측 및 미착지 다리측의 양자의 목표 족평위치 자세), 기준 상체 자세 및 목표 팔자세의 값(금회값)이 구해진다. 단, 목표 팔자세에 관해서는, 보다 상세하게는 양 팔체의 전체 중심위치가 결정되지만, 연직축(또는 상체 체간축) 주위의 각운동량을 변화시키는 팔체의 운동(팔흔들기 운동)은, 아직 결정되어 있지 않다. 또한, 목표 족평위치 자세의 금회값(시각 k에서의 값)은, 상기 도 19의 S200에서 초기 시각 Ts에서의 족평위치 자세를 구한 경우와 동일하게 구해진다.

이어서 S406으로 진행하고, 목표 상반력 연직성분을 만족하는(로봇(1)의 연직방향의 관성력과 중력과의 합을 목표 상반력 연직성분에 균형이 잡히게 함) 시각 k에서의 전체 중심 연직위치 속도의 값(금회값)이 산출된다. 구체적으로는, 예를 들면 상기 도 10의 동역학 모델에 관계되는 상기 식01과 식04에 기초하여, 전체 중 심 연직위치 속도가 산출된다. 즉, 식01과 식04에 의하면, 로봇(1)의 운동에 의한 전체 중심 연직가속도와 중력가속도의 합에 로봇(1)의 전체 질량을 승산한 것이 상반력 연직성분과 동일하다는 관계식(로봇(1)의 전체 중심의 연직방향에 관한 운동방정식)이 얻어진다. 따라서, 이 관계식과 목표 상반력 연직성분으로부터, 전체 중심 연직가속도가 구해진다.

또한, 이 관계식 자체는, 로봇(1)의 모델에 의존하지 않고 일반적으로 성립하는 것이다. 그리고 이 구한 전체 연직가속도를 적분함으로써, 전체 중심 연직속도가 산출되고, 또한 그 전체 중심 연직속도를 적분함으로써, 전체 중심 연직위치가 산출된다. 이것들의 산출은, 보다 일반적으로는, 다음 식 15 및 식16에서 나타내지는 동역학관계식(뉴턴의 운동방정식을 이산화한 식)을 이용해서 행해진다.

시각 k에서의 전체 중심 연직속도

= 시각 (k-△k)에서의 전체 중심 연직속도

+((상반력 연직성분/로봇의 전체 질량)+중력가속도)*△k

(단, 중력가속도는 음의 값으로 한다.)

……식15

시각 k에서의 전체 중심 연직위치

= 시각 (k-△k)에서의 전체 중심 연직위치

+ 시각 k에서의 전체 중심 연직속도*△k

……식16

이어서 S408로 진행하고, 전체 중심 연직위치를 만족하는 상체 연직위치가 산출된다. 구체적으로는, 예를 들면 상기 도 10의 모델에 관계되는 식04를 사용하여 상체 연직위치가 산출된다. 즉, 지지다리측 및 미착지 다리측의 목표 족평위치 자세의 금회값으로부터, 도 10의 모델의 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직위치가 구해진다. 그리고 이들 구한 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직위치와, S407에서 구한 전체 중심 연직위치의 금회값을 식04에 적용함으로써 상체 질점(24m)의 연직위치가 구해진다. 더욱이, 이 구한 상체 질점(24m)의 연직위치와 목표 상체 자세각의 현재값(S404에서 설정된 기준 상체 자세각 또는 후술의 S414에서 결정된 전회(시각 k-△k)의 목표 상체 자세각)로부터 상체 연직위치가 구해진다.

또한, 보다 엄밀한 모델(예를 들면 도 11에 도시하는 바와 같이 로봇(1)의 각 링크에 대응하는 질점을 가지게 한 모델)을 사용하여 상체 연직위치를 구해도 좋다. 이 경우에는, 상기 구한 목표 양 족평위치 자세의 금회값(시각 k의 값), 시각 (k-△k)의 목표 상체 자세(또는 상기 구한 기준 상체 자세의 금회값), 상기 구한 목표 팔자세(양 팔의 중심위치)의 금회값, 시각 (k-△k)의 상체 수평위치와, 구하고 싶은 상체 연직위치로부터 결정되는 전체 중심 연직위치가, 상기 구한 전체 중심 연직위치의 금회값에 일치하도록 상체 연직위치가 구해진다.

이 경우, 시각 k의 상체 자세와 상체 수평위치는, 아직 결정되어 있지 않으므로, 대신에 시각 (k-△k)의 값을 사용했다. 보다 정밀도를 높이기 위해서, 시각 k에서의 추정값을, 시각 (k-△k)에서의 보용상태로부터 외삽에 의해 구해도 좋다.

이어서 S410으로 진행하고, 상기 도 14의 S108에서 결정한 보용 파라미터(상 반력 수평성분 허용범위를 규정하는 파라미터)에 기초하여, 도 17에 나타낸 상반력 수평성분 허용범위〔Fxmin, Fxmax]의 시각 k에서의 값(금회값)이 구해진다.

이어서 S412로 진행하고, 목표 ZMP에 관한 동역학적 평형 조건(로봇(1)의 관성력과 중력과의 합력이 목표 ZMP 주위에 발생하는 모멘트의 수평성분이 0이라는 조건)을 만족하도록, 상체 수평가속도와 상체 자세각가속도와의 금회값이 결정된다. 단, 상반력 수평성분(Fx)이 [Fxmin, Fxmnax]를 초과하지 않고, 또한, 상체 자세각속도가 정상보용의 초기와 종단에서 일치하도록, 상체 수평가속도와 상체 자세각가속도가 결정된다. 이하에 이 상세를 설명한다.

이 시점에서, 상기와 같이 족평위치 자세 및 상체 연직위치의 순시값(금회값)이 결정되어 있으므로, 나머지 상체 수평위치와 상체 자세각을 결정하면, 연직축 주위의 각운동량을 변화시키는 팔체의 운동자유도를 제외하고, 로봇(1)의 목표 운동은 일의적으로 결정할 수 있다. 따라서, 상체 수평위치와 상체 자세각을 결정하면, 상반력의 연직축 주위의 모멘트를 제외하고, 모든 상반력도 일의적으로 결정된다. 또한, 본 실시형태에서는 정상보용의 목표 상반력 연직성분 및 목표 ZMP는, 각각 상기 도 12의 S022에서 결정한 상반력 연직성분 궤도 파라미터 및 목표 ZMP 궤도 파라미터에 의해 규정된다. 따라서, 상체 수평위치와 상체 자세각의 결정에 따라 종속적으로 결정되는 상반력은, 상반력 수평성분뿐이다. 또, 보충하면, 전술한 바와 같이, 로봇(1)은 한쪽 다리당 6자유도이므로, 목표 족평위치 자세와 목표 상체 위치 자세로부터 다리체(2)의 각 부의 위치 자세는 일의적으로 결정된다. 또한, 연직축 주위의 각운동량을 변화시키는 팔의 운동자유도는, 후술하는 바와 같 이, 스핀력을 없애기 위해서 사용된다.

그런데, 보용 생성에 있어서, 상기 상체 병진 모드를 그다지 사용하지 않고 주로 상체 회전 모드를 사용함으로써, 목표 ZMP를 만족시키면(목표 ZMP 주위의 상반력 모멘트의 수평성분을 0으로 하면), 상체 자세각이 커서 지나치게 흔들려버릴 우려가 있다. 따라서, 이것을 방지하기 위해서는, 되도록이면 상체 병진 모드를 사용해야 한다. 그러나, 상체 병진 모드는, 상반력 수평성분의 변화를 동반하므로, 상반력 수평성분 허용범위가 좁을 경우에는, 상체 병진 모드를 강하게 작용시키면 슬립할 우려가 있다. 이 경우에는, 상체 회전 모드에 의지할 수 밖에 없다. 특히, 상기 주행보용과 같이 상반력 수평성분 허용범위가 0이 되는 기간에서는, 상반력 수평성분을 발생시키는 것과 같은 보용을 생성할 수는 없으므로, 상체 회전 모드에 의지하지 않을 수 없다. 이상을 고려하여, 본 실시형태에서는 도 22에 도시하는 플로차트를 따라서, 상체 수평가속도와 상체 자세각가속도가 결정된다.

우선, S500에서, 초기 시각 Ts으로부터 제 1 선회 보용의 공중기를 거쳐서 제 2 선회 보용이 되고, 상반력 연직성분이 어떤 소정 크기까지 증가한 시각 Tm이 설정된다. 또한, 제 2 선회 보용의 편각지지기의 후반에서 상반력 연직성분이 어느 정도 감소한 시각 Ts2가 설정된다. 또한, 제 2 선회 보용의 공중기를 거쳐서 제 1 선회 보용이 되고, 상반력 연직성분이 어떤 소정의 크기까지 증가한 시각 Tm2가 설정된다.

도 16에 이들 시각을 나타낸다. 시각 Tm은, 지지다리 족평(22)의 바닥면의 거의 전체면이 접지한 순간 혹은 그 직후에 설정하는 것이 바람직하다. 시각 Tm2 도 동일하다. 또, 시각 Ts2는, 상기 초기 시각 Ts와 동일하게, 족평(22)의 바닥면의 거의 전체면이 접지한 상태로부터 발끝 접지로 바뀌는 순간 혹은 그 직전에 설정하는 것이 바람직하다.

또, 상기 도 14의 S110에서 설정한 도 18의 목표 ZMP와 이것들의 시각 Tm, Ts2, Tm2와의 관계를 설명하면, 제 2 선회 보용에서, 목표 ZMP가, 지지다리 족평(22)의 발뒤꿈치를 초기 위치로 하여, 이 족평(22)의 바닥면의 거의 전체가 접지할 때까지 그 위치에 머무르고, 다음에 지지다리 족평(22)의 중앙으로 이동하기 시작하는 순간을 시각 Tm로 하는 것이 바람직하다. 그 후, 지지다리 족평(22)의 발끝만 접지한 상태로 될 때까지, 목표 ZMP가 발끝으로 이동을 완료한 순간을, 시각 Ts2로 하는 것이 바람직하다. 또한 다음 제 1 선회 보용에서, 목표 ZMP가, 지지다리 족평(22)의 발뒤꿈치를 초기위치로 하여 이 족평(22)의 바닥면의 거의 전체가 접지할 때까지 그 위치에 지위소리에 멈추고, 다음에 지지다리 족평(22)의 중앙으로 이동하기 시작하는 순간을 시각 Tm2로 하는 것이 바람직하다.

상기와 같이 설정하는 이유는 후술한다.

다음에 S502에서 현재 시각(정상보용작성용 타이머의 값) k가, 시각 Ts부터 Tm의 사이 또는 시각 Ts2로부터 Tm2의 사이에 있는지 아닌지가 판정된다. S502의 판정 결과가 YES이면, S504로 진행하고, NO이면 S518로 진행된다.

S504에서는, 로봇(1)의 전회 순간 보용상태(시각 k-1의 보용상태)로부터, 임시로, 상체 회전 모드의 각가속도를 0으로 하여, 상체 병진 모드의 운동을 로봇(1)에게 시킨 경우에 금회(시각 k)의 목표 ZMP를 만족하기 위해서 필요한 상체 수평가 속도(αtmp)가 구해진다. 이 αtmp는, 예를 들면 상기 도 10의 동역학 모델에 관계되는 상기 식03을 사용하여 구해진다. 보다 구체적으로는, 예를 들면 현재시각 k까지 구해진 목표 족평위치 자세의 시계열값을 사용하여, 현재 시각 k에서의 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직가속도가 구해지는 동시에, 현재시각 k(금회)의 목표 족평위치 자세를 사용하여 지지다리 질점(2m) 및 미착지 다리 질점(2m)의 연직위치가 구해진다. 또, 현재시각 k(금회)의 상반력 연직위치를 사용하여 상체 질점(24m)의 연직위치가 구해지는 동시에, 현재시각 k까지에 구한 목표 상체 연직위치의 시계열값을 사용하여 현재시각 k에서의 상체 질점(24m)의 연직가속도가 구해진다. 그리고 이들 구한 값을 상기 식03에 대입하는 동시에, 동 식03의 My, d2θby/dt2를 0으로 하여 되는 식을, d2Xb/dt2에 대해 풀어서, 상체 질점 수평가속도 d2Xb/dt2가 상체 수평가속도(αtmp)로서 구해진다. 또한, 보다 엄밀한 동역학 모델을 사용하여, 목표 ZMP 주위의 상반력 모멘트의 수평성분을 0으로 하는 상체 수평가속도(αtmp)를 탐색적으로 구하도록 해도 좋다. 또, 본 실시형태에서는, 기준 상체 자세가 연직자세이고, 기준 상체 자세에 의한 상체 자세각가속도가 0이기 때문에, 상체 회전 모드의 각가속도를 0으로 하여 상체 수평가속도(αtmp)를 구하도록 했다. 단, 기준 상체 자세가 변화되도록 이 기준 상체 자세 궤도 파라미터를 설정한 경우에, 그것에 의해 정해지는 현재시각 k에서의 기준 상체 자세각가속도가 0이 아닌 경우에는, 상체 회전 모드의 각가속도를 그 0이 아닌 기준 상체 자세각가속도의 값으로 하고, 상체 수평가속도(αtmp)를 동역학 모델을 사용하여 구하도록(예를 들면 식03의 d2θby/dt2를 0이 아닌 기준 상체 자세각가속도로 설정하여 상기와 마찬가지로 상체 수평가속도(αtmp)를 구하도록) 하면 좋다.

다음에, S506으로 진행하고, 상체 수평가속도가 αtmp의 경우의 시각 k에서의 상반력 수평성분(Fxtmp)이 동역학 모델을 사용하여 구해진다. 본 실시형태에서는, 상기 동역학 모델의 식02를 사용하여 Fxtmp가 구해진다. 즉, 다음 식17에 의해 Fxtmp가 구해진다. 단, d2Xsup/dt2 및 d2Xswg/dt2는 각각 시각 k에서의 지지다리 족평 질점 수평가속도, 미착지 다리 족평 질점 수평가속도를 나타낸다.

Fxtmp=mb*αtmp+msup*d2Xsup/dt2

+mswg*d2Xswg/dt2 ……식17

이렇게 하여 구해지는 Fxtmp의 예를 도 23에 도시한다. 도 23에, Fxtmp가 상반력 수평성분 허용범위 [Fxmin, Fxmax]를 넘은 부분을 사선으로 나타낸다.

다음에 S508로 진행하고, 상체 병진 모드의 상체 수평가속도(α), 이것에 의해 발생하는 상반력 수평성분(Fx), 상체 회전 모드의 상체 각가속도(β)가 다음과 같이 결정된다(S508∼S516).

즉,

Fxtmp>Fxmax이면, S510으로 진행하고, 다음 식에 의해 Fx가 결정된다.

Fx=Fxmax ……식18

Fxtmp<Fxmin 이면, S512로 진행하고, 다음 식에 의해 Fx가 결정된다.

Fx=Fxmin ……식19

그 이외, 즉, Fxtmp가, 상반력 수평성분 허용범위 [Fxmin, Fxmax]내에 존재 하면, S514로 진행하고, 다음 식에 의해 Fx가 결정된다.

Fx=Fxtmp ……식20

어느쪽의 경우에도, 이어서 S516으로 진행하고, 다음 식에 의해 상체 수평가속도(α), 상체 자세각가속도(β)가 결정된다.

α = αtmp + (Fx-Fxtmp)/△Fp ……식21

β = (αtmp-α)*△Mp/△Mr ……식22

단, △Fp, △Mp 및 △Mr는 각각 상기 식06, 07, 및 식09에 의해 결정된다.

보충하면, 보다 동역학 연산의 정밀도를 높이려고 하면, 상기와 같이 상체 각가속도(β)을 구한 후, 상체 병진 모드와 상기 구한 상체 각가속도(β)의 상체 회전 모드를 합성한 운동이 목표 ZMP를 만족하도록, 상체 병진 모드의 상체 수평가속도(α)를, 보다 엄밀한 동역학 모델을 사용하여 해석적 또는 탐색적으로 결정한 편이 좋다. 또한, 탐색적인 결정법으로서는, 유사 야코비안(감도 매트릭스)을 구하고, 유사 뉴튼법 등에 의해 다음 후보를 결정하는 방법이나, 심플렉스법 등을 사용하면 좋다.

또한 상반력 수평성분(Fx)이 상반력 수평성분 허용범위 [Fxmin, Fxmax]를 엄밀하게 넘지 않도록 하기 위해서, S510에서는, Fx=Fxmax 또한 목표 ZMP 주위의 상반력 모멘트의 수평성분이 0이 되도록, S512에서는, Fx=Fxmin 또한 목표 ZMP 주위의 상반력 모멘트의 수평성분이 0이 되도록, 상체 수평가속도(α)와 상체 각가속도(β)와의 세트를 탐색적으로 구해도 좋다.

상기한 바와 같이 구해지는 Fx를 도 24에 나타낸다. Fx는, Fxtmp의 값을 상 반력 수평성분 허용범위 [Fxmin, Fxmax]로부터 넘지 않도록 제한(포화)시킨 것으로 되어 있다. 보다 상세하게는, 상체 병진 모드만에 의한 상체 수평가속도(αtmp)에 따른 Fxtmp가 허용범위 [Fxmin, Fxmax]내에 있을 경우에는, Fxtmp가 그대로 Fx가 된다. 또, 상체 병진 모드만에 의한 상체 수평가속도(αtmp)에 따른 Fxtmp가 허용범위 [Fxmin, Fxmax]의 상한을 넘고, 혹은 하한을 하회한 경우에는, 각각 Fx는, 각각 강제적으로 Fxmax, Fxmin에 제한된다. 특히 주행보용의 공중기에서는, 항상, Fxmax=Fxmin=0이므로, Fx=0이 된다.

또, 상기한 바와 같이 구해지는 상체 자세각가속도(β)를 도 25에 나타낸다. 상체 병진 모드에 의해 발생하는 Fx가 허용범위 [Fxmin, Fxmax]를 넘지 않도록, 상체 병진 모드의 가속도를 제한함으로써 부족한 상반력 모멘트(보다 상세하게는 목표 ZMP 주위의 상반력 모멘트의 수평성분을 0으로 하는데에 필요한 관성력 모멘트로부터, 제한된 상체 수평운동과 양 다리체(2, 2)의 운동에 의한 모멘트 성분을 뺀 모멘트)를, 상체 회전 모드에서 보충하게 된다. 또한, 주행보용의 공중기에서는, 상체 병진 모드에 의한 상체 수평가속도(α)는 항상 0으로 제한되는 것이 되므로, 상체 회전 모드에 의한 상체 자세각가속도(β)만에 의해, 상기 부족분의 상반력 모멘트가 보충되게 된다.

이상이, 시각 k가 시각 Ts부터 Tm 사이 또는 시각 Ts2로부터 Tm2 사이의 경우의 처리이다.

S502 판정 결과가 NO일 경우에는 이하의 처리가 행해진다. 우선, S518로 진행하고, 상체 자세각속도를, 현재시각 k가 시각 Ts2 이전이면 시각 Ts2까지, 그렇 지 않으면 시각 Te까지, 초기값(현재시각 k가 시각 Ts2 이전이면 시각 Ts에서의 값, 그렇지 않으면 시각 Ts2에서의 값)으로 되돌리기 위한 상체 자세각가속도를 발생시키는 상반력 모멘트의 ZMP 환산값(이후, 상체 자세 복원 모멘트 ZMP 환산값이라 부르고, ZMPrec라고 약칭함)의 패턴을 설정한다.

이것에 대하여 이하에 상세하게 설명한다. 현재시각 k가 시각 Tm2 이후의 시기의 처리는, 시각 Ts2 이전의 시기의 처리와 동일하므로, 현재시각 k가 시각 Ts2 이전까지의 시기의 처리만을 해설한다.

시각 Tm으로부터 시각 Ts2까지, 상체 회전 모드를 사용하여 상체 자세각가속도를 발생시킴으로써, 상체 자세각속도를 초기값(시각 Ts에서의 값)으로 되돌리는 것을 생각한다. 이것을 위한 상체 자세각가속도 패턴을, β(k)라고 한다.

상체 회전 모드에서는, 상체 자세각가속도 β(k)를 발생시키려고 하면, 상반력 모멘트 β(k)*△Mr이 발생한다. 이 결과, 그 순간에서의 상반력 연직성분이 Fz(k)이라고 하면, (목표 ZMP가 아니라) 운동으로부터 산출되는 ZMP(k)는, 다음 식으로 구해지는 △ZMP만큼 벗어난다.

△ZMP(k) = -β(k)*△Mr/Fz(k) ……식23

따라서, △Mr의 패턴과 Fz(k)의 패턴이 정해져 있으면(기지이면), △ZMP(k)의 패턴을 적당히 설정함으로써, 식23을 만족하는 상체 자세각가속도 패턴을 발생시켜서, 상체 자세각속도를 초기값(시각 Ts에서의 값), 즉 기준 상체 자세 궤도의 초기(시각 Ts) 상태에서의 상체 자세각속도로 되돌릴 수 있다.

상기 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec)은, 그렇게 적절하게 설정된 △ZMP(k)를 의미한다. 상기 식23을 사용하여 상체 자세 복원 모멘트 ZMP 환산값을 설정할 때에, △Mr는 엄밀하게는 변동하지만, 근사적으로 일정값으로 해도 좋다. 정상보용은 임시로 생성되는 것 뿐이며, 실제의 로봇을 이 보용에 추종시키는 것은 아니므로, 정상보용의 동역학적 정밀도는 그다지 높지 않아도 되기 때문이다.

도 26에 ZMPrec의 1예를 도시한다. 도 26에서는, 그 패턴은 사다리꼴 패턴으로 되어 있다. 그 사다리꼴부의 꺾임점의 시각은, 시각 Tm과 시각 Ts2 사이의 기간에서의 목표 ZMP 패턴의 꺾임점의 시각(도 18 참조)에 일치시켜 있다. 이것은, 후술하지만, 금회 보용의 목표 ZMP 패턴의 수정이 간단해지기 때문이다. 식23의 △ZMP(k)에 ZMPrec을 대입함으로써 다음 식이 얻어진다.

β(k) = -ZMPrec*Fz(k)/△Mr ……식24

따라서 이 식24에서 구해지는 β(k)는, 도 27의 실선으로 나타내는 바와 같이 된다. 또한, 도 27의 점선으로 나타내는 것은, 시각 Ts로부터 시각 Tm까지의 상체 자세각가속도(도 25의 실선으로 나타내는 것)이다.

그런데, 정상보용에서는, 시각 Ts2에서의 상체 자세각속도가 시각 Ts에서의 상체 자세각속도에 일치하고 있는 것이 바람직하다. 일치하고 있으면, 상체 자세각의 변동이 작아지기 때문이다.

그래서, 시각 Ts2에서의 상체 자세각속도를 시각 Ts에서의 상체 자세각속도에 일치시키기 위해서, 시각 Ts로부터 Ts2까지의 상체 자세각가속도(β)의 적분값이 0이 되도록, 상체 자세각가속도 패턴을 설정한다. 즉, 시각 Tm부터 시각 Ts2까지의 기간의 β(k)의 적분값(도 27의 실선부의 적분값)이, 시각 Ts부터 시각 Tm까 지의 상체 자세각가속도(이것은 도 22의 S504∼S516에서 전술한 바와 같이 구해짐)의 적분값(도 27의 점선부의 적분값, 바꿔 말하면 시각 Tm에서의 상체 자세각속도와 시각 Ts에서의 상체 자세각속도의 차이)에 -1을 곱한 값이 되도록 설정된다.

더 구체적으로는, 본 실시형태에서는 도 26의 ZMPrec의 사다리꼴 패턴에서, 사다리꼴의 꺾임점의 시각(이것은 상기한 바와 같이 정상보용의 목표 ZMP 궤도에 기초하여 결정됨)을 알고 있는 것으로 하고, 사다리꼴 높이를 미지수로 함으로써, 상기 관계를 만족하는 ZMPrec의 사다리꼴 높이(ZMPrec 패턴을 규정하는 파라미터)가 구해진다.

제 1 선회 보용에서의 ZMPrec의 사다리꼴 높이를 제 1 선회 보용 상체 자세 복원 모멘트 ZMP 환산값 사다리꼴 높이(acyc1)라고 부른다. 제 2 선회 보용에서의 ZMPrec의 사다리꼴 높이를 제 2 선회 보용 상체 자세 복원 모멘트 ZMP 환산값 사다리꼴 높이(acyc2)라고 부른다. 또한, 제 2 선회 보용에서의 ZMPrec는, 시각 Tm2부터 Te까지의 기간에서 설정되는 사다리꼴 패턴이다.

보충하면, 금회 보용을 1보 생성하고 있는 도중에 보용 파라미터가 변경되지 않는 한, 시각 Tm과 Ts2 사이의 기간, 또는 시각 Tm2와 Te 사이의 기간에 있어서 S518에서 결정되는 ZMPrec 패턴은 변하지 않으므로(동일 파라미터값이 산출되므로), S518의 처리는, 시각 Tm과 시각 Tm2에서만 실행해도 좋다.

이어서 S520으로 진행하고, 시각 k에서의 상체 자세 복원 모멘트 ZMP 환산값 ZMPrec의 순간값(금회값)이, 상기와 같이 결정한 파라미터(사다리꼴 높이)를 기초로 산출된다.

이어서 S522로 진행하고, 상기 식24를 사용하여, 시각 k에서의 상기 상체 자세각가속도 패턴의 순간값(금회값)(β)이 산출된다.

이어서 S524로 진행하고, 로봇의 전회 순간 목표 보용상태(시각 k-1의 목표 보용상태)로부터, 임시로 상체 회전 모드를 시키지 않고 상체 병진 모드의 운동만을 시킨 경우에, 이번(시각 k)의 목표 ZMP를 만족하기 위해서 필요한 상체 수평가속도(αtmp)가, 로봇(1)의 동역학 모델을 사용하여 구해진다. 이 산출은 S504와 동일하게 행해진다.

이어서 S526으로 진행하고, 다음 식을 사용하여, 상체 병진 모드의 상체 수평가속도(α)가 산출된다.

α = αtmp-(△Mr/△Mp)*β ……식25

더욱 S528로 진행하고, 상체 수평가속도가 상기한 바와 같이 구한 α일 경우의 시각 k에서의 상반력 수평성분(Fx)이, 상기 식17과 동일한 다음 식26에 의해 구해진다.

Fx = mb*α+msup*d2Xsup/dt2

+mswg*d2Xswg/dt2 …식26

보충하면, 보다 동역학 연산의 정밀도를 높이고자 한다면, 상기와 같이 상체 각가속도(β)을 구한 후, S526에서, 식25를 사용한 상체 수평가속도(α)의 산출을 실행하는 대신에, S516의 보충에서 해설한 수법과 동일하게, 상체 병진 모드와 상기 구한 상체 각가속도(β)의 상체 회전 모드를 합성한 운동이 목표 ZMP를 만족하도록, 상체 병진 모드의 상체 수평가속도(α)를, 보다 엄밀한 동역학 모델을 사용 하여, 해석적 또는 탐색적으로 결정하는 편이 좋다.

이상이, 시각 k가 시각 Ts부터 Tm 사이 또는 시각 Ts2부터 Tm2 사이의 어느 것에도 없는 경우의 처리이다.

이상의 처리를 시각 Ts2까지 완료했을 때의, 상반력 수평성분(Fx)을 도 28에 도시한다. 또한, 이상의 처리를 시각 Te까지 완료했을 때의, 상반력 수평성분(Fx)을 도 29에, 상체 자세각가속도(β)를 도 30에 나타낸다. 또한, 이상에 나타낸 S412의 처리(상체 수평가속도와 상체 자세각가속도를 결정하는 서브루틴)는, 시각 k에서의 상체 수평가속도와 상체 자세각가속도의 순간값이 결정되는 것으로서, 시각 Te까지의 이것들의 시계열이 한번에 생성되는 것은 아니다.

또한, 본 실시형태에서는 기준 상체 자세가 연직자세에서, 기준 상체 자세에 의한 상체 자세각가속도가 0이기 때문에, 상기한 바와 같이 구한 상체 회전 모드의 각가속도(β)를 S412의 처리에서 결정되는 상체 자세각가속도로 했다. 단, 기준 상체 자세가, 변화되도록 이 기준 상체 자세 궤도 파라미터를 설정한 경우에는, 상기한 바와 같이 구한 상체 회전 모드의 각가속도(β)와 기준 상체 자세각가속도의 합으로써, S412의 처리에서 결정되는 상체 자세각가속도로 해야 한다.

시각 Ts, Tm, Ts2 및 Tm2를 상기와 같이 설정한 이유는, 시각 Tm부터 Ts2까지의 기간과 시각 Tm2부터 Te까지의 기간에 있어서, 상체 자세각속도를 기준 상체 자세 궤도의 초기 각속도로 되돌리도록 상체 자세각가속도(β)를 발생시켜도, 상반력 수평성분(Fx)이 허용범위 [Fxmin, Fxmax]를 넘지 않도록 하기 위해서이다. 바꾸어 말하면, 시각 Tm부터 Ts2까지의 기간과 시각 Tm2로부터 Te까지의 기간에 있어 서는, 상반력 수평성분 허용범위가 충분하게 크기 때문에, 목표 ZMP를 만족하면서, 상체 자세각속도를 되돌리도록 상체 자세각가속도(β)을 발생시켜도, 상반력 수평성분(Fx)은 허용범위를 넘는 일이 없다.

또, 시각 Ts에서의 상체 자세각속도를 시각 Ts에서의 상체 자세각속도에 일치시키는 것을 생각하지 않고, acyc1과 acyc2를 동일한 값으로 하여 (acyc1=acyc2로 하여), 시각 Te에서의 상체 자세각속도와 시각 Ts에서의 상체 자세각속도에 일치시키도록 acyc1 및 acyc2의 값을 결정해도 좋다.

이어서 도 21의 S414로 진행하고, S412에서 구한 상체 수평가속도를 축차 적분(시각 Ts로부터 현재시각 k까지 누적 가산) 함으로써 상체 수평속도가 구해지고, 또한 그 상체 수평속도를 축차 적분(시각 Ts부터 현재시각 k까지 누적 가산) 함으로써, 상체 수평위치(금회값)이 구해진다. 또, S412에서 구한 상체 자세각가속도를 축차 적분(시각 Ts로부터 현재시각 k까지 누적 가산) 함으로써 상체 자세각속도가 구해지고, 또한 그 상체 자세각속도를 축차 적분(시각 Ts부터 현재시각 k까지 누적 가산) 함으로써, 상체 자세각(금회값)이 구해진다.

이상과 같이 도 20의 S306의 정상보용 순시값결정 서브루틴이 실행된 후, S308로 진행하고, 보용 생성용 시각 k의 값이 보용 생성 인터벌 △k만큼 증가된다. 이어서 S304로 되돌아가고, S304에 나타내는 조건이 성립하고 있는 한, S306과 S308의 처리가 반복되고, S304에 나타내는 조건이 성립하지 않게 되면, 즉 종단(시각 Te=Ts+Tcyc)까지 가정상보용의 생성이 완료되면, S310으로 진행한다. S310에서는, 다음 식을 따라서 초기(시각 Ts) 상체 자세각과 초기(시각 Ts) 상체 자세각속 도가, 상기 구한 가보용의 종단(시각 Te)상체 자세각 등을 기초로 수정된다. 이 수정은, 상체 자세각 및 그 각속도에 관한 정상보용의 경계조건(정상보용의 초기와 종단에서 상체 자세각 및 각속도를 일치시킴)을 충족시키도록 하기 위한 것인다. 또한, 여기에서의 설명에서는, 로봇(1)의 보용은, 이 로봇(1)을 직선적으로 이동시키는 보용이라고 한다.

이 경우, 다음 식의 새로운 값이 수정된 값이다.

새로운 초기 상체 자세각속도

= 가보용의 초기 상체 자세각속도

-(가보용의 종단 상체 자세각 - 가보용의 초기 상체 자세각)/Tcyc

……식30

새로운 초기 상체 자세각 = 초기(시각 Ts) 기준 상체 자세각

……식31

여기에서, 상기 식30에 의해 초기 상태 자세각속도를 수정하는 이유를 이하에 설명한다. 정상보용에서는, 그것을 반복했을 때에 경계에서 로봇(1)의 운동 상태가 불연속으로 되지 않도록(상기 경계조건을 충족시키도록) 초기 상태 자세각 및 그 각속도를 결정할 필요가 있다. 이 경우, 상술한 바와 같이 구한 가정상보용의 종단 상체 자세각속도는, 전술한 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec)에 대응하는 상체 자세각가속도 패턴의 설정에 의해, 초기 상체 자세각속도 초기값(기준 상체 자세각속도 초기값)에 일치하도록 생성되어 있으므로, 나머지 조건인 다음식을 충족시키면, 상체 자세각 및 각속도에 관한 상기 조건을 만족한다.

종단 상체 자세각 = 초기 상체 자세각 ……식32

또,

상체 자세각의 2차 미분 = 상체 자세각가속도 ……식33

이므로, 이 식33의 양변을 정상보용의 초기부터 종단까지 2차 적분함으로써, 다음 식의 관계를 얻는다.

종단 상체 자세각 = 초기 상체 자세각

+초기 상체 자세각속도*Tcyc

+상체 자세각가속도의 2차 적분값

……식34

한편, 정상보용 생성에서는, 상기와 같이 목표 ZMP를 만족하기 위해서, 기본적으로는 상체 병진운동 모드를 사용하여 상체(24)의 수평가속도(수평가감속)를 조정하지만, 목표 ZMP를 만족하기 위한 상체(24)의 수평가속도가, 충분하게 발생할 수 없는 상황(충분한 마찰력을 발생할 수 없는 상황)에서는, 상체 회전운동 모드를 사용하여 전체 중심 가속도가 변하지 않도록 하면서 상체 자세각의 각가속도를 조정함으로써 상체 병진운동 모드에 의한 상체 수평가속도의 부족분이 보충된다.

상기 상체 회전운동 모드를 사용하여 상체 자세각가속도를 조정하는 동작은, 도 10에 도시하는 플라이 휠(FH)이 갖추어진 로봇(1)에 있어서, 그 플라이 휠(FH)의 각가속도를 조정하는 것과 근사적으로 등가이다. 플라이 휠(FH)의 회전속도가 0에 가까우면, 자이로 효과는 발생하지 않고, 플라이 휠(FH)이 발생하는 힘은, 플라이 휠(FH)의 각가속도만으로 결정된다. 따라서, 상체(24)에 플라이 휠(FH)이 갖 추어진 로봇(1)에서는, 플라이 휠(FH)의 초기 각도·각속도가 다소 변해도, 이 플라이 휠(FH)을 동일한 패턴의 각가속도로 움직이면, 발생하는 힘은 거의 변하지 않는다. 그 때문에, 로봇 동역학 방정식인 식01, 식02, 식03에서는 자이로 효과를 무시하고 있다. 따라서, 본 실시형태의 로봇(1)에서, 식01, 식02, 식03, 또는, 보다 일반적이고 엄밀한 동역학 모델중 어느것을 사용한 경우에도, 전체 중심 가속도가 변하지 않도록 하면서 상체 자세각를 동일한 패턴의 각가속도로 움직이면(즉 상체 회전운동 모드를 사용하여, 상체 자세각을 동일한 패턴의 각가속도로 움직이면), 초기 상체 자세각도·각속도를 조금 바꾸어도, 발생하는 관성력과 중력과의 합력은 거의 동일하게 된다. 바꿔 말하면, 초기 상체 자세각도·각속도를 조금 바꾸어도, 상기한 플로차트를 따라서 생성되는 보용의 상체 자세각가속도는 동일하게 된다.

즉, 식34에서의 상체 자세각가속도의 2차 적분값은, 초기 상체 자세각도·각속도에 의존하지 않게 된다. 즉, 식34에서의 상체 자세각가속도의 2차 적분값은, 초기 상체 자세각도·각속도의 값에 관계없이, 일정값으로 취급할 수 있다.

초기 상체 자세각의 값이 가보용의 초기 상체 자세각, 또는 초기 상체 자세각속도의 값이 가보용의 초기 상체 자세각속도였을 경우에, 종단 상체 자세각이 가보용의 종단 상체 자세각으로 된 것이기 때문에, 이 관계를 식34에 대입하여 다음 식을 얻는다.

가보용의 종단 상체 자세각 = 가보용의 초기 상체 자세각

+상체 자세각가속도의 2차 적분값

+가보용의 초기 상체 자세각속도*Tcyc

……식35

이것으로부터,

상체 자세각가속도의 2차 적분값

= (가보용의 종단 상체 자세각 - 가보용의 초기 상체 자세각)

- 가보용의 초기 상체 자세각속도*Tcyc

……식36

상기 의론에 의해 상체 자세각가속도의 2차 적분값은 정수취급할 수 있으므로(초기 상체 자세각도·각속도에 의존하지 않으므로), 상기 식을 식34에 대입하여, 다음 식이 얻어진다.

종단 상체 자세각 = 초기 상체 자세각

+(가보용의 종단 상체 자세각 - 가보용의 초기 상체 자세각)

-가보용의 초기 상체 자세각속도*Tcyc

+초기 상체 자세각속도*Tcyc

……식37

식37과 식32로부터 상기 식30이 얻어진다.

또한, 초기 상체 자세각은, 본 실시형태에서는 식31과 같이 기준 상체 자세각 초기값에 일치시키도록 했지만, 정상보용의 상체 자세각의 평균값이 기준 상체 자세각 초기값에 일치하도록 초기 상체 자세각를 설정하도록 해도 좋다. 또는, 정상보용의 상체 자세각의 최대값과 최소값의 평균값이 기준 상체 자세각 초기값에 일치하도록 초기 상체 자세각를 설정해도 좋다.

또, 상기에서는 로봇(1)을 직진 이동시키는 경우에 한정한 예를 설명했지만, 선회 이동을 포함하는 정상보용을 결정하는 경우에는, 그 초기와 종단에서는 로봇(1)의 진행 방향이 변한다. 이 때문에 초기 상체 자세각속도는 초기 상체 자세각에 의존하게 된다. 따라서 이것을 고려하여 초기 상체 자세각 및 각속도를 결정할 필요가 있다. 이 경우에는, 예를 들면 다음 식37a, 37b를 충족시키도록 초기 상체 자세각 및 초기 상체 자세각속도를 갱신하도록 하면 좋다.

종단 상체 자세각 - 초기 상체 자세각

=상반력 수평성분 허용범위를 충족시키도록 결정된 상체 자세각가속도의 2차 적분

+ZMPrec에 의해 발생하는 상체 자세각가속도의 2차 적분

+초기 상체 자세각속도*정상보용의 주기

……식37a

종단 상체 자세각속도 - 초기 상체 자세각속도

=상반력 수평성분 허용범위를 충족시키도록 결정된 상체 자세각가속도의 1차 적분

+ZMPrec에 의해 발생하는 상체 자세각가속도의 1차 적분

…… 식37b

또한, 식37a, 37b의 각각의 우변 제 1 항의 적분기간은, 시각 Ts부터 Tm까지의 기간과, Ts2부터 Tm2까지의 기간을 합친 기간이며, 우변 제 2 항의 적분기간은, 시각 Tm부터 Ts2까지의 기간과 Tm2로부터 Te까지의 기간을 합친 기간이다.

보다 구체적으로 설명하면 정상보용에서는, 제 1 선회 보용의 지지다리 좌표계(다음회 보용 지지다리 좌표계)에서 본 초기 상태 자세각 및 각속도가, 각각, 다음 제 1 선회 보용의 지지다리 좌표계(다음 다음 다음회 보용 지지다리 좌표계)에서 본 종단 상체 자세각 및 각속도에 일치하고 있지 않으면 않된다. 그래서, 본 실시형태에서는, 새로운 초기 상체 자세각은, 상기 식31에 의해 결정하고, 이 새로운 초기 상체 자세각과, 이것을 정상보용에서의 로봇(1)의 토탈 선회각(연직축 주위의 선회각)에 따른 매트릭스(회전좌표 변환의 매트릭스)에 의해 다음회 보용 지지다리 좌표계에서 본 값으로 좌표변환 한 것과를 각각 식37a의 좌변의 초기 상체 자세각, 종단 상체 자세각에 대입한다. 또, 식37a, 37b의 각각의 우변 제 1 항의 적분에 관계되는 상체 자세각가속도는, 상기 도 22의 S516에서 축차 구해진 것이 사용된다.

그리고 식37a, 37b의 초기 상체 자세각속도와, 식37a, 37b의 우변 제 2 항의 적분에 관계되는 ZMPrec(도 26의 사다리꼴 형상의 패턴)의 사다리꼴 높이를 미지수로 하여 (단, ZMPrec의 사다리꼴 패턴의 꺾임점의 시각은 미리 정한다. 또, 제 1 선회 보용의 ZMPrec의 사다리꼴 높이 acyc1과 제 2 선회 보용의 ZMPrec의 사다리꼴 높이 acyc2를 동일값으로 한다.), 그것들의 미지수를 포함하는 식37a, 37b의 연립방정식을 풀어서 구해지는 초기 상체 자세각속도를 새로운 초기 상체 자세각속도로 하여 결정한다. 이 경우, 식37b의 종단 상체 자세각속도는, 미지수인 초기 상체 자세각속도를, 정상보용의 상기 토털의 선회각에 따른 매트릭스에 의해 다음회 지 지다리 좌표계에서 본 값으로 좌표변환한 것이다.

로봇(1)의 이동이 선회를 포함하는 이동일 경우에는, 상기한 바와 같이 하여, 새로운 초기 상체 자세각 및 각속도를 결정하면 좋다.

또한, 정상보용의 경계조건을 충족시키는 초기 상체 자세각속도를 탐색적으로 구하는 것도 가능하다.

도 20의 S310의 처리를 완료한 후, 도 19의 S210로 진행하고, 생성한 보용(가정상보용)의 종단 상체 수평위치·속도를, 그 순간의 지지다리에 대응하는 지지다리 좌표계(도 31의 X''', Y''', Z''' 좌표계)에서 본 값으로 변환하고, 그 값을(Xe, Vxe)로 한다(Xe: 종단 상체 수평위치, Vxe: 종단 상체 수평속도).

이어서 S212로 진행하고, 도시하는 바와 같이, 초기 상체 수평위치 속도(Xs, Vxs)와 종단 상체 수평위치 속도(Xe, Vxe)와의 차이를 산출한다. 이 차이(Xs-Xe, Vxs-Vxe)를 상체 수평위치 속도 경계조건 오차(errx, errvx)라고 부른다. 정상보용에서는, 상기 경계조건을 충족시킬 필요가 있으므로, (Xs, Vxs)와 (Xe, Vxe)가 일치하고 있어야 하다. 따라서 상체 수평위치 속도 경계조건 오차(errx, errvx)가 0 또는 거의 0이어야 한다. 본 실시형태에서는, 이하와 같이, 상체 수평위치 속도 경계조건 오차(errx, errvx)가 거의 0이 되는(Xs, Vxs)가 탐색적으로 구해진다.

이어서 S214로 진행하고, 산출한 상체 수평위치 속도 경계조건 오차(errx, errvx)가, 미리 적당하게 설정된 허용범위내에 있는지 아닌지가 판단된다. 또한, 이와 같이 상체 수평위치 속도 경계조건 오차의 허용범위를 설정하는 대신에, 초기 발산성분(Xs+Vxs/ω0)과 종단 발산성분(Xe+Vxe/ω0)과의 차, 및 초기 수속성분(Xs- Vxs/ω0)과 종단 수속성분(Xe-Vxe/ω0)과의 차가 각각 어떤 허용범위내에 있는지 아닌지 판단하도록 해도 좋다. 단 ω0는, 상술한 바와 같이, 어떤 소정의 값이다.

S214의 판단 결과가 NO일 때에는 S216으로 진행한다. 이 S216에서는, (Xs, Vxs)의 부근에 복수(본 실시형태에서는 2개)의 초기값 후보(Xs+△Xs, Vxs), (Xs, Vxs+△Vxs)이 결정된다. 여기에서, △Xs, △Vxs는, 각각 Xs, Vxs에 대한 소정의 미소변화량을 의미하고 있다. 그리고, 이것들의 초기값 후보의 각각을 상체 수평위치 속도의 초기 상태로서, 상기 S208과 동일한 처리에 의해 정상보용이 보용 파라미터를 사용하여 생성된다. 또한, 그 생성된 정상보용의 종단 상체 위치 속도를, 그 순간의 지지다리에 대응하는 지지다리 좌표계(도 31의 X''', Y''', Z''' 좌표계)에서 본 값으로 변환한 값(Xe+△Xe1, Vxe+△Vxe1), (Xe+△Xe2, Vxe+△Vxe2)가 구해진다. 여기에서, (Xe+△Xe1, Vxe+△Vxe1)은 (Xs+△Xs, Vxs)에 대응하는 종단 상체 위치 속도를 의미하고, (Xe+△Xe2, Vxe+△Vxe2)는 (Xs, Vxs+△Vxs)에 대응하는 종단 상체 위치 속도를 의미하고 있다. 또한, 이 경우의 정상보용(가보용)의 생성 처리에서는, 상체 수평위치 속도 이외의 변수의 초기 상태(시각 Ts에서의 상태)는, 예를 들면 상체 수평위치 속도의 초기값 후보를 (Xs, Vxs)으로 한 경우와 동일하게 설정하면 좋다. S216에서는 또한, 상기 S210과 동일한 처리에 의해, 각 초기값 후보와 이것에 대응하는 종단 상체 위치 속도와의 차이, 즉, 각 초기값 후보(Xs+△Xs, Vxs), (Xs, Vxs+△Vxs)의 각각에 대응하는 상체 수평위치 속도 경계조건 오차가 구해진다.

이어서 S218로 진행하고, (Xs, Vxs) 및 그 부근의 초기값 후보(Xs+△Xs, Vxs), (Xs, Vxs+△Vxs)의 각각에 대한 상체 수평위치 속도 경계조건 오차를 기초로, (Xs, Vxs)의 다음 초기값 후보가 탐색법(유사 야코비안(감도 행렬)을 구하고, 최급강하법 등에 의해 다음 후보를 결정하는 방법이나, 심플렉스법 등)에 의해 결정된다. 즉, (Xs, Vxs) 및 그 부근의 초기값 후보(Xs+△Xs, Vxs), (Xs, Vxs+△Vxs)의 각각에 대한 상체 수평위치 속도 경계조건 오차에 의해, 상체 수평위치 및 상체 수평속도를 각각 초기값 후보(Xs, Vxs)로부터 미소 변화시켰을 때의, 상체 수평위치 속도 경계조건 오차의 변화 정도를 나타내는 감도 행렬이 구해지고, 그 감도 행렬에 기초하여, 상체 수평위치 속도 경계조건 오차를 보다 작게 하는 초기값 후보(Xs, Vxs)가 새롭게 결정된다. 그리고, 이와 같이 상체 수평위치 속도의 새로운 초기값 후보(Xs, Vxs)가 결정된 후, S206로 돌아간다.

S214의 판단 결과가 NO로 되는 한은 상기한 처리(S206∼S218의 처리)가 반복된다. 또한, 이 경우, 상체 수평위치 속도의 새로운 초기값 후보(Xs, Vxs)에 대응하는 정상보용의 생성 처리(S208)에서의 S300(도 20 참조)에서는, 상체 자세각속도의 초기값은, 기준 상체 자세각속도의 초기값이 아니라, 상체 수평위치 속도의 전회의 초기값 후보(Xs, Vxs)에 대응하는 S208의 처리에서의 S310(도 20 참조)에서 결정한 것으로 설정된다. 그리고 S214의 판단 결과가 YES가 되면, 반복 루프(S204)를 빠져나와 S220로 진행한다. 또, S204의 반복 루프를 빠져나왔을 때의 직전에 생성된 가정상보용이, 경계조건을 충족시키는 정상보용으로서 얻어지게 된다.

S220에서는, 본래의 초기 시각 0(금회 보용의 종단시각)에서의 초기 상체 수 평위치 속도(X0, V0)와 초기 시각 0에서의 초기 상체 연직위치 속도(Z0, Vz0)와 초기 시각 0에서의 초기 상체 자세각 및 각속도가 구해진다.

구체적으로는, (X0, V0)와 (Z0, Vz0)는, 상기 도 21에 나타낸 정상보용 순시값결정 서브루틴의 S408 및 S414에서, 제 2 선회 보용으로부터 제 2 선회 보용으로 전환한 순간의 시각, 즉, 시각 k=Tcyc(시각 Te-Ts)일 때에 결정된 상체 수평위치 속도와 상체 연직위치 속도를, 각각 시각 Tcyc로부터 시작되는 1보(즉 2번째의 제 1 선회 보용)의 지지다리에 대응하는 지지다리 좌표계(도 31의 Xl''', Y''', Z''' 좌표계)에서 본 값으로 변환한 값으로 결정된다. 마찬가지로, 초기 상태 자세각 및 각속도는, 시각 k=Tcyc(시각 Te-Ts)일 때에 결정된 상체 자세각 및 각가속도를, 시각 Tcyc로부터 시작되는 1보(즉 2번째의 제 1 선회 보용)의 지지다리에 대응하는 지지다리 좌표계(도 31의 X''', Y''', Z''' 좌표계)에서 본 값으로 변환한 값으로 결정된다.

이어서 S222으로 진행하고, 정상보용 초기 발산 성분 q[0]가 다음 식으로부터 구해진다.

q[o] = X0+V0/ω0 ……식40

단 ω0는, 발산에 관한 해설에서 기술한 바와 같이, 어떤 소정의 값이다.

더욱, S224로 진행하고, 정상보용 초기발산 성분 q[0]이 금회 보용 지지다리 좌표계에서 본 값으로 변환되고, 이것이 q''[0]로서 구해진다. 또, 초기 상체 연직위치 속도(Z0, Vz0)가 금회 보용 지지다리 좌표계에서 본 값으로 변환되고, 이것이(Z0'', Vz0'')로서 구해진다.

보충하면, (Z0'', Vz0'')은, 제 2 선회 보용의 지지다리 좌표계(도 15의 X'', Y'', Z'' 좌표계)에서 본 제 2 선회 보용 종단 상체 연직위치 속도에 일치하고 있다. 또, q''[0]도 제 2 선회 보용의 지지다리 좌표계(도 15의 X'', Y'', Z'' 좌표계)에서 본 제 2 선회 보용 종단 발산 성분에 일치하고 있다. 따라서, 이것들의 성질을 이용하여, (Z0'', Vz0'') 및 q''[0]을 산출하게 해도 좋다.

이상에 의해, 도 12의 S024의 처리, 즉 정상보용의 초기 상태를 구하는 서브루틴 처리가 종료한다.

이어서, 도 12의 S026으로 진행하고, 금회 보용의 보용 파라미터를 결정(일부는 가결정) 한다. S026에서는, 더 구체적으로는, 도 32에 나타내는 플로차트에 따라서, 이하의 처리가 행해진다.

우선, S600에서, 금회 보용의 족평위치 자세 궤도가 정상보용의 족평위치 자세 궤도에 이어지도록, 금회 보용의 족평궤도 파라미터가 설정된다.

구체적으로는, 금회 보용 초기 미착지 다리 족평위치 자세(금회 보용 미착지 다리 족평위치 자세의 초기값)은, 금회 보용 지지다리 좌표계에서 본 현재 미착지 다리위치 자세(전회보용 종단 미착지 다리위치 자세)에 설정된다. 금회 보용 초기 지지다리 족평위치 자세(금회 보용 지지다리 족평위치 자세의 초기값)는, 금회 보용 지지다리 좌표계에서 본 현재 지지다리 족평위치 자세(전회 보용 종단 지지다리 족평위치 자세)에 설정된다. 또, 금회 보용 종단 미착지 다리 족평위치 자세는, 금회 보용 지지다리 좌표계에서 본 차회 보용 지지다리 좌표계(금회 보용에 관계되는 1보째의 미착지 다리 착상 위치 자세의 요구값)에 대응하여 결정된다. 즉, 금 회 보용 종단 미착지 다리 족평위치 자세로부터 미착지 다리 족평(22)을 바닥에 접촉시킨 채, 미끄러지지 않도록 이 족평(22)을 그 바닥면의 거의 전체면이 접지할 때까지 회전시켰을 때의, 이 족평(22)의 대표점이 금회 보용 지지다리 좌표계에서 본 다음회 보용 지지다리 좌표계의 원점에 합치하도록 금회 보용 종단 미착지 다리 족평위치 자세가 결정된다.

금회 보용 종단에서는, 지지다리 족평(22)은 이상하여 공중에 있다. 지지다리 족평(22)이 이상하고나서 후 궤도를 결정하기 위해서, 지지다리 족평착지 예정 위치 자세를 설정한다. 지지다리 족평착지 예정 위치 자세는, 금회 보용 지지다리 좌표로에서 본 다음 다음회 보용 지지다리 좌표(금회 보용에 관계되는 2보째의 미착지 다리 족평위치 자세의 요구값)에 대응하여 설정된다. 보다 구체적으로는, 지지다리 족평착지 예정 위치 자세는, 그 위치 자세로부터, 족평(22)을 바닥에 접촉시킨 채, 미끄러지지 않도록 이 족평(22)을 그 바닥면의 거의 전체면이 바닥에 접지할 때까지 회전시켰을 때의 이 족평(22)의 대표점이 금회 보용 지지다리 좌표에서 본 다음 다음회 보용 지지다리 좌표의 원점에 합치하도록 설정된다.

금회 보용 종단 지지다리 족평위치 자세는, 현재 지지다리 위치 자세(금회 보용 초기 지지다리 족평위치 자세)로부터, 다음회 보용 지지다리 좌표계에 대응하는 족평착지 예정 위치 자세(상기 요구 파라미터에서의 2보째의 미착지 다리측 족평착지 위치 자세의 요구값)에 이르는 족평위치 자세 궤도를, 금회 보용 종단까지 상기 유한시간 정정 필터에 의해 생성함으로써 구해진다.

이어서, S602으로 진행하고, 금회 보용의 기준 상체 자세 궤도 파라미터가, 정상보용의 제 1 선회 보용이나 제 2 선회 보용과 동일하게 결정된다. 단, 금회 보용의 기준 상체 자세 궤도가 상기 정상보용의 기준 상체 자세 궤도에 연속해서 이어지도록(금회 보용 종단에서의 기준 상체 자세각 및 각속도가 각각 정상보용 초기의 기준 상체 자세각 및 각속도에 일치하도록) 상기 파라미터가 설정된다. 또한, 본 실시형태에서는 기준 상체 자세는, 금회 보용 및 정상보용의 어느곳에서 정상적인 연직자세이다.

이어서, S604로 진행하고, 금회 보용의 팔자세 궤도 파라미터가, 정상보용의 제 1 선회 보용이나 제 2 선회 보용과 동일하게 결정된다. 단, 금회 보용의 팔자세 궤도가 상기 정상보용의 팔자세 궤도에 연속해서 이어지도록 상기 파라미터를 설정한다. 또한, 여기에서 결정되는 팔자세 궤도 파라미터는, 정상보용 파라미터의 결정(도 14의 S104)의 경우와 동일하게, 연직축(또는 상체 체간축) 주위의 양 팔체의 각운동량 변화에 관한 것 이외의 운동 파라미터이며, 양 팔체의 중심위치의 궤도를 규정하는 파라미터이다.

이어서, S606으로 진행하고, 금회 보용의 상반력 연직성분 궤도 파라미터가, 정상보용의 제 1 선회 보용이나 제 2 선회 보용과 동일하게, 이 파라미터에 의해 규정되는 상반력 연직성분 궤도가 상기 도 6과 같이 실질적으로 연속한(값이 스텝 형상으로 뛰지 않는) 궤도가 되도록 설정된다.

단, 상반력 연직성분 궤도 파라미터는, 금회 보용의 전체 중심 연직위치 속도와 상반력 연직성분 궤도의 모두가, 상기 정상보용과 연속하여 이어지도록 결정된다.

구체적으로는, 우선, 상기 도 12의 S024의 처리(정상보용 초기 상태의 결정 처리)에서 최종적으로 구한 정상보용의 초기 상체 연직위치 속도를 금회 보용 지지다리 좌표계에서 본 값으로 변환한 값(Z0'', Vz0''), 즉, 도 19의 S224에서 구한(Z0'', Vz0'') 등을 기초로, 예를 들면 상기 식04(또는 로봇(1)의 키네매틱스 모델)를 사용하여, 금회 보용 지지다리 좌표계에서 본 정상보용의 초기 전체 중심 연직위치 속도가 구해진다. 구체적으로는, 금회 보용 지지다리 좌표계에서 본 정상보용의 초기 전체 중심 연직위치는, S224에서 구한 정상보용의 상체 연직위치Z0''에 대응하는 도 10의 모델의 상체 질점 연직위치와, 정상보용 초기의 각 족평위치를 금회 보용 지지다리 좌표계에서 본 값으로 변환한 것에 대응하는 지지다리측 및 미착지 다리측의 다리체 질점 연직위치를 식04에 대입함으로써 구해진다. 또, 금회 보용 지지다리 좌표계에서 본 정상보용의 초기 전체 중심 연직속도는, S224에서 구한 정상보용의 상체 연직속도 Vz0''에 대응하는 도 10의 모델의 상체 질점 연직속도와, 정상보용 초기의 각 족평 연직속도를 금회 보용 지지다리 좌표계에서 본 값으로 변환한 것에 대응하는 지지다리측 및 미착지 다리측의 다리체 질점 연직속도를 식04의 양변을 미분하여 이루어지는 식에 대입함으로써 구해진다. 또한, 이 초기 전체 중심 연직위치 속도의 산출은, 보다 엄밀한 모델을 사용하여 행하도록 해도 좋다.

그리고, 이와 같이 하여 구한 정상보용의 초기 전체 중심 연직위치 속도를, 상기 식13과 하기 식41의 종단 전체 중심 연직위치 속도에 대입하는 동시에, 전회의 목표 보용 순시값(보다 엄밀하게는, 전회 목표 보용의 종단상태를 금회 지지다 리 좌표계로 변환한 것)의 전체 중심 연직위치 및 속도를, 상기 식13과 하기 식41의 초기 전체 중심 연직위치 및 속도에 대입하고, 식13과 식41의 관계를 만족하도록, 금회 보용의 상반력 연직성분 패턴(보다 구체적으로는 파라미터 값)이 결정된다. 단, 식13과 식41에서의 적분값은 금회 보용의 초기부터 종단까지의 기간의 적분값으로 한다.

종단 전체 중심 연직속도 - 초기 전체 중심 연직속도

= (상반력 연직성분/로봇의 질량)의 1차 적분

+중력가속도의 1차 적분

……식41

단, 중력가속도는 음의 값으로 한다.

보다 구체적으로는, 우선, 도 6과 같은 상반력 연직성분 패턴을 규정하는 상반력 연직성분 파라미터(꺾임점의 시각 등)중의, 적어도 2개의 파라미터를 독립한 미지변수로 하여, 그 미지변수의 값을, 식13과 식41로 이루어지는 연립방정식을 풀어서 결정한다.

미지변수로 하는 상반력 연직성분 파라미터에는, 예를 들면 도 6의 사다리꼴의 높이(상반력 연직성분의 피크값)와 폭(편각지지기 시간)을 선택하면 좋다. 이 경우, 도 6의 사다리꼴의 양측 부분의 경사는 금회 보용 주기 등에 따라 미리 정한 값으로 하고, 또는, 편각지지기로부터 공중기로 이행하는 시각을 제외한 상반력 연직성분 패턴의 꺾임점의 시각을 금회 보용 주기 등에 따라 미리 정한 값으로 한다. 보충하면, 미지변수를 하나로 하면, 일반적으로는 식13과 식41의 연립방정식을 만 족하는 답이 존재하지 않는다.

이어서, S608로 진행하고, 상반력 수평성분 허용범위 [Fxmin, Fxmax](구체적으로는 이 상반력 수평성분 허용범위의 패턴을 규정하는 파라미터)가, 정상보용의 제 1 선회 보용이나 제 2 선회 보용과 동일하게 설정된다. 예를들면 도 33에 도시하는 바와 같은 패턴으로 상반력 수평성분 허용범위가 설정된다. 본 실시형태에서는, 먼저 S606에서 결정한 상반력 연직성분 패턴에 따라 상기 식12 에 기초하여 상반력 수평성분 허용범위가 설정된다.

이어서, S610으로 진행하고, 금회 보용의 ZMP 궤도(구체적으로는 ZMP 궤도를 규정하는 파라미터로, 궤도의 꺾임점의 시각이나 위치)가, 정상보용의 제 1 선회 보용이나 제 2 선회 보용과 동일하게, 안정 여유가 높고 또한 급격한 변화를 하지 않도록, 상기 도 7에 도시하는 바와 같이 설정된다. 단, 금회 보용의 ZMP 궤도가 상기 정상보용의 ZMP 궤도에 연속해서 이어지도록 상기 파라미터가 설정된다. 즉, 금회 보용 종단에서의 ZMP의 위치가 정상보용 초기의 ZMP위치에 일치하도록, ZMP 궤도 파라미터가 결정된다. 이 경우, 주행보용에서는, 편각지지기에서의 ZMP 궤도의 꺾임점의 시각이나 위치의 설정의 방법은, 전술한 정상보용의 ZMP 궤도 파라미터의 설정의 방법과 동일해도 좋다. 그리고, 공중기에서의 목표 ZMP 궤도가, 공중기 개시시부터, 정상보용 초기의 ZMP위치까지, 직선적으로 연속해서 변화되도록 ZMP 궤도 파라미터를 설정하면 좋다.

더욱, S610에서 결정되는 금회 보용의 ZMP 궤도 파라미터는, 가결정된 것 뿐이며, 후술하는 바와 같이 수정된다. 그래서, 상기와 같이 설정한 금회 보용의 ZMP궤도를, 이후, 금회 보용의 가목표 ZMP 궤도라고 부르는 일이 있다.

최후에, S612으로 진행하고, 상체 자세각 복원 기간[Ta, Tb]이 설정된다. 상체 자세각 복원 개시시각 Ta는, 정상보용의 제 2 선회 보용에서의 Tm에 상당하고, 상체 자세각 복원 수료 시각 Tb는, 정상보용의 제 2 선회 보용에서의 Ts2에 상당하는 것이다. 이들 시각 Ta, Tb의 설정의 방법은, 각각 Tm 및 Ts2의 설정 방법과 동일하다.

도 12의 설명으로 되돌아가서, 상기와 같이 S026에 나타내는 처리(금회 보용의 보용 파라미터 결정 처리)를 행한 후, 이어서 S028로 진행하고, 금회 보용의 보용 파라미터(ZMP 궤도 파라미터)가 수정된다. 이 처리에서는, 상체 위치 자세 궤도를 정상보용으로 연속시키고, 혹은 근접시키기 위해 ZMP 궤도 파라미터가 수정된다.

도 34는 그 처리를 나타내는 서브루틴·플로차트이다

우선 S700을 거쳐서 S702로 진행하고, 가목표 ZMP 패턴과 그 밖의 금회 보용 파라미터를 기초로, 금회 보용 종단시각까지의 가금회 보용이 가생성된다.

S702에서는, 도 35에 나타내는 플로차트에 따라서, 이하의 처리가 행해진다.

이하 설명하면 S800에서, 여러 가지의 초기화가 행해진다. 구체적으로는, 가보용 생성용 시각 k에 0이 대입된다. 또, 전회 목표 보용의 종단상태(보다 구체적으로는, 상체 수평위치 속도, 상체 연직위치 속도, 상체 자세각 및 그 각속도, 목표 족평위치 자세, 목표 팔자세 등의 보용상태의 종단값)를 금회 지지다리 좌표계로 변환한 것을, 금회 보용 초기 상태로 한다.

이어서, S802를 거쳐서 S804에 있어서, 가보용 생성용 시각 k가 금회 보용 종단시각 Tcurr 이전인지 아닌지(k≤Tcurr인지 아닌지) 판단되고, 그 판단 결과가 YES일 경우에는, S806의 금회 보용 순시값결정 서브루틴으로 진행하고, 금회 보용의 시각 k의 순시값이 결정된다. 이어서 S808로 진행하고, 가보용 생성용 시각 k가 △k만큼 증가되고, S804로 돌아가다.

S804의 판단 결과가 NO일 경우에는, 도 35에 도시하는 플로차트의 처리가 완료한다.

이상의 처리에 의해, 임시의 금회 보용이 초기로부터 종단까지 생성된다.

S806의 보용 순시값 결정 서브루틴에서는, 도 36에 나타내는 처리가 행해진다. 이하에 이것을 설명한다.

우선 S900에서, 금회 보용 파라미터의 상반력 연직성분 궤도 파라미터를 기초로, 도 6에 나타낸 목표 상반력 연직성분의 시각 k에서의 값(금회값)이 구해진다. 이어서, S902에서, 금회 보용 파라미터의 ZMP 궤도 파라미터를 기초로, 도 7에 나타내는 목표 ZMP 궤도(가목표 ZMP 궤도)의 시각 k에서의 값(금회값)을 구한다.

이어서 S904로 진행하고, 금회 보용 파라미터(족평궤도 파라미터, 기준 상체 자세 궤도 파라미터 및 팔자세 궤도 파라미터)를 기초로, 시각 k에서의 목표 양 족평위치 자세, 기준 상체 자세 및 목표 팔자세의 값(금회값)이 각각 구해진다. 단, 목표 팔자세에 관해서는, 팔 전체 중심위치는 결정되지만, 연직축(또는 상체 체간축) 주위의 각운동량을 변화시키는 운동자유도는, 아직 결정되어 있지 않다. 또 한, 정상보용의 경우와 동일하게, 금회 보용의 목표 족평위치 자세 궤도 및 상반력 연직성분 궤도는, 각각, 도 32의 플로차트에서 결정한 족평궤도 파라미터, 상반력 연직성분 궤도 파라미터에 따라 서로 독립적으로 결정된다.

이어서 S906으로 진행하고, S900에서 구한 목표 상반력 연직성분의 금회값를 만족하도록(로봇(1)의 연직방향의 관성력과 중력과의 합을 목표 상반력 연직성분에 균형이 잡히게 하도록) 시각 k에서의 전체 중심 연직위치가 산출된다. 이 산출은, 구체적으로는 상기 도 21의 S406과 동일한 처리에 의해 행해진다.

이어서 S908로 진행하고, 상기와 같이 구한 전체 중심 연직위치를 만족하는 상체 연직위치의 금회값이 산출된다. 이 산출은, 구체적으로는, 상기 도 21의 S408과 동일한 처리에 의해 행해진다.

이어서 S910으로 진행하고, 상기 도 32의 S608에서 결정한 상반력 수평성분 허용범위의 파라미터에 기초하여, 도 33에 나타내는 상반력 수평성분 허용범위 [Fxmin, Fxmax]의 시각 k에서의 값(금회값)이 구해진다.

이어서 S912로 진행하고, 목표 ZMP를 만족하도록(목표 ZMP 주위에 발생하는 모멘트 수평성분이 0이 되도록), 금회 가보용의 상체 수평가속도와 상체 자세각가속도가 결정된다. 단, 이들 가속도는, 상반력 수평성분 Fx가 [Fxmin, Fxmax]를 넘지 않도록 결정된다. 이 처리는, 도 21의 S412의 상세를 나타내는 도 22의 플로차트의 처리와는 약간 다르다.

S912에서는, 구체적으로는, 도 37의 플로차트에 나타내는 처리가 행해진다. 도 37의 플로차트에서는, 우선 S1000에서, 시각 k가 상기 도 32의 S612에서 설정한 상체 자세각 복원 기간 [Ta, Tb]에 있는지 아닌지가 판단된다.

이 판단 결과가 NO일 경우(상반력 수평성분 허용범위가 0 혹은 좁을 경우)에는, S1002로부터 S1014까지의 처리가 실행된다. S1002부터 S1014까지의 처리는, 전술의 도 22의 S504부터 S516까지의 처리와 동일하므로, 설명을 생략한다.

S1000의 판단 결과가 YES일 경우에는, S1016으로 진행하고, 상체 자세 복원 모멘트 ZMP 환산값 패턴(ZMPrec)이 설정된다. 이 경우, 정상보용의 생성에 관계되는 상기 도 22의 S518과 달리, ZMPrec는, 일정값 0의 패턴(즉 도 26에 도시하는 사다리꼴 패턴의 높이가 0인 패턴)으로 설정된다. 다음에 S1018부터 S1026까지의 처리가 행해진다. S1018로부터 S1026까지의 처리는, 상기 도 22의 S520부터 S528까지의 처리와 동일하므로 설명을 생략한다. 더욱, S1016∼S1026의 처리에서는, 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec)의 순시값은 항상 0이 되므로, S1020에서 구해지는 상체 회전 모드의 상체 자세각가속도(β)도 0이 된다. 따라서 β=0으로 하고, S1022, S1024 및 S1026의 처리만을 행하도록 해도 좋다. S1000의 판단 결과가 YES일 경우, 즉, 상체 자세각 복원 기간[Ta, Tb]에서는, 상기한 바와 같이 β=0이 되므로, S912의 처리에서는, 상체 자세각 복원 기간 [Ta, Tb]의 기간(상반력 수평성분 허용범위가 충분히 큰 기간)에서, 정상보용 생성의 경우와 달리, 상체 자세각속도의 복원 방향으로의 상체 자세변화의 운동 궤도는 생성되지 않게 된다.

또한, 본 실시형태에서는 기준 상체 자세가 연직자세에서, 기준 상체 자세에 의한 상체 자세각가속도가 0이기 때문에, 상기와 같이 구한 상체 회전 모드의 각가속도(β)를 S912의 처리에서 결정되는 상체 자세각가속도로 했다. 단, 기준 상체 자세가 변화하도록 이 기준 상체 자세 궤도 파라미터를 설정한 경우에는, 상기와 같이 구한 상체 회전 모드의 각가속도(β)와 기준 상체 자세각가속도의 합으로써, S912의 처리에서 결정되는 상체 자세각가속도로 해야 한다.

이상과 같이 도 36의 S912의 처리를 완료한 후, 이어서 S914로 진행하고, 상기 S912에서 구한 상체 수평가속도를 축차 적분함(금회 보용 초기(시각 k=0)로부터 현재시각 k까지 누적 가산함)으로써 상체 수평속도가 구해지고, 또한 그 상체 수평속도를 축차 적분함(금회 보용 초기(시각 k=0)로부터 현재시각 k까지 누적 가산함)으로써, 상체 수평위치(금회값)가 구해진다. 또, 상기 S912에서 구한 상체 자세각가속도를 축차 적분함(금회 보용 초기(시각 k=0)로부터 현재시각 k까지 누적 가산함)으로써, 상체 자세각속도가 구해지고, 또한 그 상체 자세각속도를 축차 적분함(금회 보용 초기(시각 k=0)로부터 현재시각 k까지 누적 가산함)으로써, 상체 자세각의 금회값이 구해진다. 이 S914의 처리는 도 21의 S414와 동일한 처리이다.

이상과 같이 도 35의 S806의 금회 보용 순시값결정 서브루틴이 실행되고, 이어서 S808로 진행하고, 보용 생성용 시각 k의 값이 보용 생성 인터벌(△k)만큼 증가된다. 이어서 S804로 되돌아가고, S804에 나타내는 조건이 성립하고 있는 한, S806과 S808의 처리가 반복된다. 그리고 S804에 나타내는 조건이 성립하지 않게 되면, 즉 종단까지 가금회 보용의 생성이 완료하면, 도 34의 S704로 진행한다.

S704에서는, 상기와 같이 S702에서 구한 금회 보용 종단에서의 상체 수평위치 속도(Xe, Vxe)로부터, 종단 발산 성분(q0[k])(k=Tcurr)이 도시된 식(상기 식10)을 사용하여 구해진다.

이어서 S706으로 진행하고, 금회 보용 종단 발산 성분(q0[k])과 정상보용 초기 발산 성분(q'')(도 19의 S224에서 구한 것)과의 차이인 종단 발산 성분 오차(errq)가 도시된 식을 사용하여 구해진다. 더욱, S708로 진행하고, 그 구한 종단 발산 성분 오차(errq)가 허용범위내(0 근방의 범위)에 있는지 아닌지를 판단단한다.

S708의 판단 결과가 NO일 때는 S710로 진행하고, a=△a(△a는 소정의 미소량)로 하고, 도 38의 관계에 따라서 현재의 가목표 ZMP 패턴에 사다리꼴 형상의 수정을 가세한 목표 ZMP를 기초로, 상기 S702와 동일하게, 종단까지의 가금회 보용을 계산한다. 여기에서, 도 38을 참조하고, a는, 금회 보용 종단 발산 성분을 정상보용 초기 발산 성분에 가능한 한 일치시키기(금회 보용의 상체 수평위치 자세 궤도를 정상보용의 상체 수평위치 자세 궤도에 근접시키기) 위해서, 가목표 ZMP를 수정하기 위한 사다리꼴 패턴의 높이이다. 이 경우, 본 실시형태에서는 가목표 ZMP의 수정은, 지지다리 족평(22)의 바닥면의 거의 전체면이 접지하는 기간(발바닥 전체면 접지 기간), 즉, 상반력 수평성분 허용범위가 충분하게 넓은 것으로 되는 기간에 행하도록 하고 있고, 상기 사다리꼴 패턴의 꺾임점의 시각은, 발바닥 전체면 접지 기간에 있어서의 가목표 ZMP의 꺾임점의 시각에 맞추어 설정되어 있다. 또한, S710에서 a=△a라고 하는 것은, 현재의 가목표 ZMP 궤도를 상기 사다리꼴 패턴에 의해 미소량, 수정했을 때의 상기 종단 발산 성분 오차(errq)의 변화를 관측하기 위해서이다.

상기한 바와 같이 S710에서, a=△a로 하여 가목표 ZMP 궤도를 수정했을 때의 가금회 보용을 종단까지 생성한 후, 더욱 S712로 진행하고, S710에서 구한 가금회 보용 종단에서의 상체 수평위치 속도(Xe1, Vxe1)를 기초로, 이 가금회 보용에서의 종단 발산 성분 q1[k1가 도시된 식(상기 식10)을 사용하여 구해진다.

또한, S710에서 △a는, 본 실시형태에서는, 적당하게 설정되는 미소량의 정수로 했지만, 이하에 설명하는 반복 연산에 의해 종단 발산 성분 오차(errq)가 작아짐에 따라서 △a를 감소시키도록 이 △a를 설정하도록 해도 좋다. 단, 정수로 해 두어도, 수회의 반복 연산으로 종단 발산 성분 오차(errq)를 허용범위 내에 넣을 수 있다.

이어서 S714로 진행하고, 파라미터 감도(r)(△a에 대한 종단 발산 성분 오차의 변화의 비율)이 도시된 식으로부터 구해진다. 더욱, S716으로 진행하고, a=-errq/r, 즉 S706에서 구한 종단 발산 성분 오차(errq)를, S714에서 구한 파라미터 감도(r)로 나눈 값을 높이(a)로 하는 사다리꼴 패턴의 수정량을 도 38의 관계에 따라서 가목표 ZMP 패턴에 더함으로써, 가목표 ZMP 패턴이 수정됨(새로운 가목표 ZMP 패턴이 결정됨).

이어서 S702로 돌아간다. 그리고 S708의 판단 결과가 NO인 한은, 상기한 S702∼S716의 처리가 반복되고, S708의 판단 결과가 YES로 되었을 때에는, 그 반복 루프(S700)를 빠져나와 S718로 진행된다.

S718에서는, 가금회 보용의 종단 상체 자세각과 정상보용의 초기 상체 자세각과의 차이, 및 가금회 보용의 종단 상체 자세각속도와 정상보용의 초기 상체 자세각속도와의 차이 등을 기초로, 금회 보용의 상체 자세각이 정상보용의 상체 자세 각에 근접하도록, 금회 보용의 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec)의 패턴이 결정된다. 여기에서 결정하는 ZMPrec는, 후술하는 금회 보용 순시값의 생성 처리에서, 상반력 수평성분 허용범위가 충분하게 큰 것으로 되는 기간(편각지지기내의 기간)에서, 상체 자세각 궤도를 정상보용에 이어지게 하도록(근접시키도록) 상체 자세각가속도를 발생시켜도, 금회 보용 종단 발산 성분과 정상보용 초기 발산 성분과의 일치성(S708의 조건)을 유지할 수 있도록, 가목표 ZMP를 수정하기 위한 것이다.

이 ZMPrec는, 상기 정상보용 생성 처리에서 설명한 것과 동일하게 사다리꼴 형상의 패턴이며, 구체적으로는 다음과 같이 결정된다. 즉, 도 26에 나타낸 ZMPrec의 사다리꼴 패턴에 있어서, 사다리꼴의 정상점(꺾임점)의 시각은 알고 있는 것으로 하는(더 구체적으로는 사다리꼴의 꺾임점의 시각을 목표 ZMP의 꺾임점 시각에 맞추는) 동시에, 사다리꼴 높이를 미지수로 하고, 이하와 같이 ZMPrec의 사다리꼴 높이(파라미터)가 구해진다. 단, S718에서는 도 26에서의 Tm 및 Ts2를 각각 Ta, Tb로 치환하는 것으로 한다.

상기한 바와 같이 상체 자세 복원 모멘트 ZMP 환산값 패턴의 미지 파라미터가 1개인 경우에는, 금회 보용의 종단에서 상체 자세각와 상체 자세각속도 양쪽을 정상보용으로 연속으로 이어가는 것은 일반적으로는 할 수 없다. 그래서, 본 실시형태에서는 복수보에 걸쳐서 서서히, 생성되는 보용의 상태가 정상보용의 상태에 근접할 수 있도록, 미지 파라미터를 결정하는 것으로 한다.

보충하면, 1보용중에서의 ZMPrec 패턴을 복잡하게 하고, 미지 파라미터를 2 개 이상으로 하고, 금회 보용의 종단에서 상체 자세각과 상체 자세각속도 양자를 정상보용으로 연속으로 연결해도 좋지만, ZMPrec 패턴이 지그재그로 크게 지나치게 변동할 우려가 있다.

이하에서는, 산출 원리를 설명한 후, 산출 순서를 설명한다.

상기와 같이 S702에서 ZMPrec 패턴의 사다리꼴 높이를 0으로 하여 구한 가금회 보용의 종단 상체 자세각와 정상보용의 초기 상체 자세각과의 차이를 구하고, 이것을 θerr로 한다. 또, 가금회 보용의 종단 상체 자세각속도와 정상보용의 초기 상체 자세각속도와의 차이를 구하고, 이것을 vθerr로 한다.

여기에서, ZMPrec 패턴의 사다리꼴 높이를 어떤 값(bcurr)으로 하여 금회 보용이 생성되고, 잇따라서, 금회 보용과 동일한 알고리즘에 의해 제 1 선회 보용이 생성된다고 상정한다. 단, 제 1 선회 보용의 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec) 패턴은, 도 22의 S518에서 구해진 제 1 선회 보용의 ZMPrec 패턴(높이가 상기 acyc1인 도 26의 사다리꼴 패턴)과 어떤 값 b1의 합으로 한다.

이렇게 하여 생성된 보용을 ZMPrec 수정 보용이라 부르고, 그 종단(제 1 선회 보용의 종단) 상체 자세각과 각속도를 각각 θ1, vθ1로 한다.

S024에서의 정상보용의 초기 상태를 구하는 서브루틴의 처리가 완료된 시점에서 구해진 본래의 정상보용(최종적으로 S310의 식30, 식31에 의해 결정된 정상보용 초기 상체 자세각 및 각속도를 초기값으로 하고, ZMPrec 패턴을 S518에서 구해진 패턴(높이가 acyc1인 도 26의 사다리꼴 패턴)으로 한 경우의 정상보용)의 제 1 선회 보용 종단 상체 자세각과 각속도를 각각 θ1org, vθ1org로 한다.

여기에서, △θ1, △vθ1을 다음과 같이 정의한다.

△θ=θ1-θ1org ……식50

△vθ1=vθ1-vθ1org ……식51

△θ1 및 △vθ1은, ZMPrec수정 보용과 본래의 정상보용을 제 1 선회 보용의 종단까지 생성한 시점에서, 양자간에 있어서의 상체 자세각의 차이 및 각속도의 차이를 의미한다. 만약에 △θ1 및 △vθ가 0으로 되면, ZMPrec 수정 보용에 잇따라서, 금회 보용과 동일한 알고리즘에 의해, ZMPrec 패턴의 사다리꼴 높이를 상기 acyc2로 하고 제 2 선회 보용이 생성되면, 이 보용은, 본래의 정상보용에 일치한다.

따라서 △θ1 및 △vθ1이 0이 되는 금회 보용 사다리꼴 높이 bcurr, 제 1 선회 보용 사다리꼴 높이(b1)를 구하고, 구한 bcurr를 금회 보용의 사다리꼴 높이로서 최종적으로 결정하면 좋다.

그런데, 로봇(1)의 상체 자세각에 관한 동역학 모델이 도 10에 도시하는 플라이 휠(FH)과 같은 선형 특성을 가지므로, △θ1 및 △vθ1는, 금회 보용 사다리꼴 높이 bcurr, 제 1 선회 보용 사다리꼴 높이 b1, 가금회 보용의 종단 상체 자세각과 정상보용의 초기 상체 자세각의 차이 θerr, 가금회 보용의 종단 상체 자세각속도와 정상보용의 초기 상체 자세각속도의 차이 vθerr과 이하의 관계를 갖는다.

△θ1 = c11*bcurr+c12*b1+θerr+e1*vθerr ……식52

△vθ1 = c21*bcurr+c22*b1+e2*vθerr ……식53

단, c11, c12, c21, c22, e1, e2는, 금회 보용, 제 1 선회 보용의 보용 주 기, 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec) 패턴의 파라미터(특히 시각에 관한 파라미터) 등에 의해 일의적으로 결정되는 계수다.

이상의 원리에 기초하여, 산출 수순으로서, 우선, 가금회 보용과 정상보용과의 경계에서의 상체 자세각의 차이 θerr과 각속도의 차이 Vθerr이 구해진다.

이어서, 식52, 53의 계수인 c11, c12, c21, c22, e1, e2가, 금회 보용, 제 1 선회 보용의 보용 주기, 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec) 패턴의 파라미터(특히 시각에 관한 파라미터) 등을 기초로 구해진다.

이어서 식52, 식53의 우변이 0이 되도록 금회 보용 사다리꼴 높이 bcurr, 제 1 선회 보용 사다리꼴 높이 b1이 결정된다. 즉, 식52, 식53의 우변을 0으로 한 연립방정식을 풀어서, bcurr, b1이 구해진다.

최후에, 금회 보용의 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec)의 사다리꼴 패턴의 사다리꼴 높이가, 상기 구한 금회 보용 사다리꼴 높이 bcurr에 설정된다.

이어서 S720으로 진행하고, 현재의 가목표 ZMP 패턴(S700의 반복 루프를 빠졌나왔을 때의 가목표 ZMP 패턴)에, S718에서 상기와 같이 구한 상체 자세 복원 모멘트 ZMP 환산값 패턴을 가산하여 이루어지는 패턴이 금회 보용의 목표 ZMP 패턴으로서 결정된다. 또한, 이 처리는, S710에서 △a의 높이의 사다리꼴 패턴을 가목표 ZMP 패턴에 더하는 처리와 동일하다.

이하에 가목표 ZMP 패턴에 상체 자세 복원 모멘트 ZMP 환산값 패턴을 가산한 이유를 기술한다.

S700의 루프에서 생성되는 가금회 보용은, 상술과 같이, 상체 자세 복원 모 멘트 ZMP 환산값(ZMPrec)을 0(ZMPrec의 사다리꼴 패턴의 높이 파라미터를 0)으로 하여 생성된다. 이와 같이 하여 S700의 루프에서 최종적으로 생성된 가금회 보용에서는, 그 상체 위치 속도는, 정상보용에 연속하고, 혹은 근접하지만, 상체 자세각은, 정상보용의 상체 자세각으로부터 벗어나고, 경우에 따라서는 발산해버린다.

S718에서 구해진 상체 자세 복원 모멘트 ZMP 환산값 패턴은, 상체 자세각의 정상보용에 대한 어긋남을 0에 근접시키기 위한 상체 자세각가속도를 발생시키는 것이다.

그러나, S700의 루프에서 최종적으로 얻어진 가목표 ZMP 패턴을 수정하지 않고, S718에서 구해진 상체 자세 복원 모멘트 ZMP 환산값 패턴에 따른 상체 자세각가속도를 발생시키면, 동역학적 평형 조건(로봇의 중력과 관성력의 합력이 목표 ZMP에 작용하는 모멘트가 연직성분을 제외하고 0으로 되는 것)을 만족시키기 위해서, 상체 수평위치 궤도를 상기 가금회 보용의 상체 수평위치 궤도로부터 비키어 놓지 않으면 않되게 된다. 그래서, 본 실시형태에서는, 상체 수평위치 궤도를 S700의 루프에서 최종적으로 얻어지는 것으로부터 비키어 놓지 않아도 되도록, ZMPrec에 의해 가목표 ZMP 패턴을 수정하는 것으로 했다.

상기 가금회 보용의 운동에 더하여, S718에서 구한 상체 자세 복원 모멘트 ZMP 환산값 패턴에 대응하는 상체 자세각가속도를 발생시키면, ZMP(운동에 의해 발생하는 중력과 관성력의 합력의 연직성분을 제외한 모멘트가 0이 되는 점)는 상체 자세 복원 모멘트 ZMP 환산값만 벗어난다. 따라서 반대로, 가목표 ZMP 패턴에 상체 자세 복원 모멘트 ZMP 환산값 패턴을 가산한 패턴을 목표 ZMP 패턴으로 하고, S718에서 구한 상체 자세 복원 모멘트 ZMP 환산값 패턴에 대응하는 상체 회전 모드의 상체 자세각가속도를 발생시키면서, 목표 ZMP 패턴을 만족하는 금회 보용을 생성하면, 상기 가금회 보용과 동일한 상체 병진운동이 된다.

이상이, 가목표 ZMP 패턴에 상체 자세 복원 모멘트 ZMP 환산값 패턴을 가산한 패턴을 목표 ZMP 패턴으로 한 이유이다.

도 12로 돌아가서, 상기와 같이 S028에서 금회 보용 파라미터를 수정한 후, 또는 S016의 판단 결과가 NO일 경우에는, S030으로 진행하고, 금회 보용 순시값이 결정된다.

S030에서는, 도 36의 S900으로부터 S910까지와 동일한 처리가 행해지고, 다음에 S912의 서브루틴인 도 37의 S1000∼S1026의 처리가 행해진다. 단, 이 경우, 도 37의 S1016에서는, 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec) 패턴은, 0으로 설정되는 것이 아니라, 도 34의 S718에서 설정한 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec) 패턴으로 설정된다. 이어서 도 36의 S914와 동일한 처리가 실행되고, 이것에 의해 도 12의 S030의 처리가 완료된다.

이어서 S032로 진행하고, 스핀력을 캔슬하기(로봇(1)의 팔 이외의 운동에 의해 목표 ZMP 주위에 발생하는 상반력 모멘트 연직성분을 대략 0으로 하기) 위한 팔동작이 결정된다. 구체적으로는, 팔을 흔들지 않았을 경우의 목표 ZMP에서의 상반력 모멘트 연직성분 궤도(엄밀하게는, 팔을 흔들지 않고 보용을 생성한 경우에 있어서, 로봇의 중력과 관성력의 합력이 목표 ZMP에 작용하는 모멘트 연직성분 궤도의 각 순시값의 부호를 반전한 것)가 구해진다. 즉, S030의 처리에 의해 생성된 보용의 운동(이것에는 팔흔들기의 운동은 포함되어 있지 않음)의 순시값에 균형이 잡히는 목표 ZMP(순시값) 주위의 상반력 모멘트 연직성분으로 순시값이 구해진다. 그리고 이것을, 팔흔들기 운동의 등가 관성모멘트로 나눔으로써, 스핀력 캔슬에 필요한 팔흔들기 동작의 각가속도가 구해진다. 또한, 보충하면, 팔흔들기가 지나치게 클 경우에는, 등가 관성모멘트보다도 큰 값으로 나누면 된다.

다음에 이 각가속도를 2차 적분하고, 이것을 적분값이 과대로 되는 것을 방지하기 위한 로 컷 필터에 통과시켜서 얻은 각도를 팔흔들기 동작각으로 한다. 단, 팔흔들기 동작에서는, 좌우의 팔을 전후 역방향으로 흔들고, 양 팔체의 중심위치를 변화시키지 않도록 한다. 또한, 스핀력을 캔슬하기 위한 팔흔들기 운동을 정상보용에서도 생성해 두고, 이것에 이어지도록, 금회 보용에서의 팔흔들기 운동을 결정하도록 해도 좋다.

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

이상이, 보용 생성 장치(100)에서의 목표 보용 생성 처리다.

도 4를 참조하여 본 실시형태에 관계되는 장치의 동작을 더욱 설명하면, 보용 생성 장치(100)에서, 상기한 바와 같이 목표 보용이 생성된다. 생성된 목표 보용중, 목표 상체 위치 자세(궤도), 목표 팔자세(궤도)가, 로봇 기하학 모델(역키네매틱스 연산부)(102)로 송출된다.

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

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

로봇(1)에 발생한 상반력(상세하게는 실제 각 발 상반력)은 6축력 센서(34)에 의해 검출된다. 그 검출값은 상기 복합 컴플라이언스 동작 결정부(104)에 보내진다. 또, 로봇(1)에 발생한 자세 경사 편차 errx, θerry(상세하게는 목표 상체 자세각에 대한 실제 자세각의 편차로, 롤링 방향(X축 주위)의 자세각 편차가 θerrx이며, 피칭 방향(Y축 주위)의 자세각 편차가 θerry임)이 경사 센서(36)를 통하여 검출되고, 그 검출값은 자세안정화 제어 연산부(112)에 보내진다. 이 자세안정화 제어 연산부(112)에서, 로봇(1)의 상체 자세각을 목표 상체 자세각으로 복원하기 위한 목표 전체 상반력 중심점(목표 ZMP) 주위 보상 전체 상반력 모멘트가 산출되어 복합 컴플라이언스 동작 결정부(104)에 보내진다. 복합 컴플라이언스 동작 결정부(104)는, 입력값에 기초하여 목표 상반력을 수정한다. 구체적으로는, 목표 전체 상반력 중심점(목표 ZMP) 주위에 보상 전체 상반력 모멘트가 작용하도록 목표 상반력을 수정한다.

복합 컴플라이언스 동작 결정부(104)는, 수정된 목표 상반력에, 센서 검출값 등으로부터 산출되는 실제 로봇의 상태 및 상반력을 일치시키려고 상기 기구변형 보상식 수정 목표 족평위치 자세(궤도)를 결정한다. 단 모든 상태를 목표에 일치시키는 것은 사실상 불가능하므로, 이것들간에 트레이드오프 관계를 부여하여 타협적으로 가능한 한 일치시킨다. 즉, 각 목표에 대한 제어 편차에 중요도를 부여하고, 제어 편차(또는 제어 편차의 2승)의 중요도부가 평균이 최소가 되도록 제어한다. 이것에 의해 실제의 족평위치 자세와 전체 상반력이 목표 족평위치 자세와 목표 전체 상반력에 대강 따르도록 제어된다.

또한, 본 발명의 요지는 보용 생성 장치(100)에서의 로봇(1)의 보용 생성에 있고, 상기한 복합 컴플라이언스 동작 결정부(104) 등의 구성 및 동작은, 본 출원인이 먼저 출원한 일본 특개평 10-277969호 공보 등에 상세하게 기재되어 있으므로, 설명을 이상으로 그친다.

S028에서, 금회 보용은, 상기와 같이, 금회 보용의 종단 발산 성분이, 정상 선회 초기 발산 성분 q[0]을 금회 보용의 지지다리 좌표계에서 본 값인 q''에 일치하도록 수정된다.

실은, 발산 성분이란, 동역학 모델을 사용하여 금회 보용 파라미터에 따라서 보용을 생성하고, 그대로 연속하여 정상 선회 보용 파라미터에 따라서 보용을 반복 생성했을 때, 생성한 보용의 상체 수평위치가 정상 선회 보용에 수속하는지 아닌지를 평가하는 지표이다. 수속하는 경우에, 금회 보용의 종단 발산 성분이, 정상 선회 초기 발산 성분 q[0]을 금회 보용의 지지다리 좌표계에서 본 값인 q''에 정확하게 일치하도록, 본래, 발산 성분은 정의되어야 한다.

식10에서 정의한 발산 성분은, 실은, 상기한 성질을 근사적으로 만족하는 발산 성분이다.

따라서 본 실시형태에서는, 동역학 모델을 사용하여 금회 보용 파라미터에 따라 보용을 생성하고, 그대로 연속하여 정상 선회 보용 파라미터에 따라 보용을 반복 생성했을 때, 생성한 보용의 상체 수평위치가 정상 선회 보용의 상체 수평위치에 수속하도록(근접하도록), 금회 보용 파라미터를 수정했다고 할 수 있다.

이것은, 일본 특원 2000-352011호의 것과 동일하다.

특히, 본 실시형태에서는, 그 조건(금회 보용이 정상보용에 근접함)을 만족하도록, 금회 보용의 보용 파라미터중의 목표 ZMP 패턴을 수정하도록 했다. 도 39을 참조하여 설명하면 동 도면에 부호 B로 나타내는 궤도는, 상기한 바와 같이 보용의 경계에서 발산 성분이 일치하도록 생성한 상체 수평위치 궤도를 나타낸다.

이 도면에 부호 A로 나타내는 궤도는, 정상 선회 보용과의 경계에서의 상체 수평위치 속도가 일치하도록 금회 보용을 생성하고, 계속하여 정상보용을 반복 생성한 경우의 상체 수평위치 궤도를 나타낸다.

도시한 바와 같이, 부호 B로 나타내는 궤도는, 금회 보용과 최초의 정상 선회 보용과의 경계에서는, 일반적으로 부호 A로 나타내는 궤도로부터 벗어나 있지만, 그 후에 서서히 부호 A로 나타내는 궤도에 수속하고(근접하고), 다음 정상 선회 보용시기에서 부호 A로 나타내는 궤도에 거의 일치한다. 이와 같이, 보용경계에서 발산 성분만을 일치시키는 보용 생성 수법으로도, 보용경계에서 위치 속도 양쪽을 일치시키는 보용 생성 수법과 동일하게 보용의 발산을 방지할 수 있다. 또한, 이 도면에 부호 C로 나타내는 예는, 그것들을 고려하지 않고 궤도를 생성한 경우를 나타낸다. 이러한 경우에는, 생성한 궤도는 경시적으로 발산해버린다. 당연히, 목표 ZMP 패턴을 복잡한 것으로 하고, 복수의 파라미터를 조정하여, 위치 속도 양쪽을 일치시켜도 좋지만, 목표 ZMP 패턴이 지그재그로 될 우려가 있다. 또한, 위치 속도 양쪽을 일치시키면, 발산 성분도 일치하기 때문에, 위치 속도 양쪽을 일치시키는 방법도 발산 성분을 일치시키는 방법의 특수예라고 할 수 있다.

더욱이, 본 실시형태에서는, 동역학 모델을 사용하여 금회 보용 파라미터를 따라서 보용을 생성하고, 그대로 연속하여 정상 선회 보용 파라미터에 따라서 보용을 반복 생성했을 때, 생성한 보용의 상체 자세각이 정상 선회 보용의 상체 자세각에 수속하고(근접하고), 또는 일치하도록, 금회 보용 파라미터를 수정했다고 할 수 있다.

본 실시형태에 관계되는 다리식 이동로봇의 보용 생성 장치는 상기한 바와 같이 구성했으므로, 동역학 모델을 사용하여 금회 보용 파라미터에 따라서 보용을 생성하고, 그대로 연속하여 정상 선회 보용 파라미터에 따라서 보용을 반복 생성했을 때, 생성한 보용의 상체 수평위치가 정상 선회 보용에 수속하는 조건을 만족하는 금회 보용의 종단 상체 수평위치와 종단 상체 수평속도와의 관계는, 엄밀하게는, 도 41의 실선과 같이 곡선 관계가 된다. 발산 성분의 본래의 정의로부터 말하면, 도 41의 실선의 관계를 만족하는 종단 상체 수평위치와 종단 상체 수평속도와의 어떠한 세트로부터 결정되는 발산 성분의 값은 동일해야한다. 또, 발산 성분을 지표로 하여 물리적으로 알기 쉽게 하기 위해서는, 종단 상체 수평속도가 0일 때에 종단 상체 수평위치와 발산 성분의 값이 일치하고 있는 것이 바람직하다.

즉, 발산 성분을 구하는 함수를 f라고 하면, 도 41의 실선을 나타내는 식이 다음 식60의 형태로 나타내어지고, 또한, 다음 식61을 만족하는 것이 바람직하다.

f(종단 상체 수평위치, 종단 상체 수평속도) = C

(단지 C는 어떤 정수) ……식60

f(종단 상체 수평위치, 0) = 종단 상체 수평위치 ……식61

함수f(종단 상체 수평위치, 종단 상체 수평속도)의 값이 발산 성분의 값이 된다.

발산 성분 정의식인 전술의 식10은, 함수f의 근사식이다. 식10으로 정의되는 발산 성분이 어떤 정수일 경우, 종단 상체 수평위치와 종단 상체 수평속도의 관계는, 도 41의 점선이 되고, 상기 실선에 대한 근사 직선으로 되어 있다.

발산 성분을 나타내는 함수f는, 직선 근사하는 대신에, 다항식 등을 사용하 여 곡선으로 근사해도 좋다. 또는, 함수f를 맵화하여 기억해도 좋다.

또, 근사 정밀도를 높이기 위해서는, 정상보용 파라미터에 따라, 함수f를 바꾸어야 한다.

또한, 발산 성분의 정의식인 식10에서, 상체 위치 속도의 대신에, 로봇 전체 중심위치 속도를 사용해도 좋다.

본 실시형태에서는 이해를 쉽게 하기 위해서, 상반력 수평성분 허용범위는, 전후방향(X축방향) 성분과 좌우방향(Y축방향) 성분 각각 독립적으로 설정할 수 있도록 했지만, 전후방향과 좌우방향의 관계식으로 나타내는 편이, 보다 슬립하기 어려운 보용이 생성된다.

예를 들면, 다음식과 같이 소위 마찰원을 허용범위로 하여도 좋다.

(상반력 수평성분의 X성분)*(상반력 수평성분의 X성분)

+ (상반력 수평성분의 Y성분)*(상반력 수평성분의 Y성분)

≤(ka*μ*Fz)*(ka*μ*Fz)

……식59

단, Fz는, 목표 상반력 연직성분을 μ는 마찰계수, ka는 1이하의 양의 정수를 나타낸다.

단, 이상과 같이, 상반력 수평성분 허용범위를 전후방향과 좌우방향의 관계식으로 나타내는 경우, 허용범위를 동시에 혹은 교대로 만족하도록, 사지탈플레인상에서의 운동과 레이터럴 플레인상에서의 운동을 동시 혹은 번갈아 구할 필요가 있다.

또한, 로봇의 전체 중심 연직가속도와 중력가속도와의 합은 상반력 연직성분에 비례하므로, 보용 파라미터로서, 상반력 연직성분 궤도 대신에 로봇의 전체 중심 연직가속도 궤도를 규정하는 파라미터를 명시적으로 설정해도 상관없다. 본질적으로 동일한 것이다. 또, 로봇의 전체 중심궤도에 가까운 거동을 하는 부위의 연직가속도 궤도를 규정하는 파라미터를 명시적으로 설정해도 상관없다. 예를 들면, 다리체의 질량이 상체의 질량에 비해 충분히 작을 경우에는, 상체 연직가속도 궤도와 로봇의 전체 중심 연직가속도 궤도는, 거의 동일하든지 비례 관계로 되므로, 상반력 연직성분 궤도 대신에 상체 연직가속도 궤도를 사용해도 좋다. 동일하게, 상반력 수평성분과 로봇의 전체 중심 수평가속도는, 비례 관계에 있으므로, 본 실시형태에서의 상반력 수평성분 및 그 허용범위 대신에, 로봇 전체 중심 수평가속도 및 그 허용범위를 사용해도 상관없다. 또, 로봇의 전체 중심 수평궤도에 가까운 거동을 하는 부위의 수평가속도 궤도에 관한 파라미터를 명시적으로 설정해도 상관없다. 예를 들면, 다리체의 질량이 상체의 질량에 비해 충분하게 작을 경우에는, 상체 수평가속도 궤도와 로봇의 전체 중심 수평가속도 궤도는, 거의 동일하든지 비례 관계에 있으므로, 상반력 수평성분 및 그 허용범위 대신에 상체 수평가속도 및 그 허용범위를 사용해도 좋다.

상기 실시형태에서는, 상반력 수평성분과 목표 ZMP주위의 상반력 모멘트를 적절한 값으로 하기 위해서, 상체 회전 모드와 상체 병진 모드의 2개의 운동 모드를 사용했지만, 이것들 이외의 운동 모드를 사용해도 좋다.

예를 들면, 도 40과 같이, 고관절을 중심으로 상체 자세를 회전시키면, 전체 중심 주위의 각운동량이 변화되는 동시에 전체 중심도 변화된다. 이 운동과 상기 상체 병진 모드를 어떤 소정의 비율로 중첩하면(합성하면), 상기 상체 회전 모드와 거의 동일한 운동이 되어, 상반력 수평성분이 발생하지 않게 된다. 따라서, 이것을 변경하여 상기 상체 회전 모드라고 간주하면, 본 실시형태의 알고리즘에 따라서, 동일한 보용을 생성할 수 있다.

따라서 운동 모드중 하나가 상반력 수평성분을 발생하지 않는 운동 모드일 필요도 없다. 어떠한 모드의 조합이라도, 상반력 수평성분과 목표 ZMP주위의 상반력 모멘트의 발생의 비율이 다른 두가지의 운동 모드를 사용하기만 하면, 상기 예와 같이, 임의의 상반력 수평성분과 목표 ZMP주위의 상반력 모멘트를 발생시킬 수 있기 때문이다.

또, 상체 자세를 변화시키는 운동 모드 이외의 운동 모드를 사용해도 좋다. 단, 가능한 한 작은 변위로 큰 상반력 수평성분 또는 목표 ZMP주위의 상반력 모멘트를 발생할 수 있는 운동 모드를 선택하는 것이 바람직하다.

예를 들면, 좌우의 팔을 동일 회전 방향으로 흔드는 운동 모드, 접지하지 않은(공중에 존재하는) 족평의 위치를 섭동시키는 운동 모드라도 좋다. 단, 미착지 다리 궤도를 섭동시키는 경우에는, 착지 위치가 변하지 않도록 착지 직전까지는, 섭동량을 실질적으로 0으로 되돌려야 한다.

또, 세가지 이상의 운동 모드를 사용해도 좋다.

또, 선택한 운동 모드중의 적어도 두가지는, 그 운동 모드에 의한 상반력 수평성분과 목표 ZMP주위의 상반력 모멘트와의 발생 비율이, 서로 다른 것일 필요가 있다. 그렇지 않으면, 일반적으로 연립방정식의 해가 없어지기(각운동 모드의 거동을 일의적으로 결정할 수 없기) 때문이다.

또한 가능한 한, 상반력 수평성분을 그다지 변화시키지 않고 목표 ZMP주위의 상반력 모멘트를 충분히 크게 변화시킬 수 있는 운동 모드와, 목표 ZMP주위의 상반력 모멘트를 그다지 변화시키지 않고 상반력 수평성분을 충분히 크게 변화시킬 수 있는 운동 모드를 조합시키는 것이 바람직하다.

바꿔 말하면, 전체 중심을 그다지 변화시키지 않고 각운동량을 충분히 크게 변화시킬 수 있는 운동 모드와, 각운동량을 그다지 변화시키지 않고 전체 중심을 충분히 크게 변화시킬 수 있는 운동 모드를 조합시키는 것이 바람직하다. 이렇게 하면, 운동 모드의 변위가 작아지기 때문이다.

상기 실시형태에 사용한 동역학 모델 이외에도 이하의 모델을 사용해도 좋다.

1) 도 11에 도시한 바와 같이 복수의 링크에 질점을 설정한 비선형인 모델(다질점 모델)

2) 본원 출원인에 의한 일본 특원 200O-352011호에 개시된 3질점 모델

3) 상체에만 질량이 있는 1질점 모델

4) 전체 중심 주위의 각운동량 변화에 의해 발생하는 관성력의 모멘트를 무시한 모델

5) 중력과 관성력과의 합력(또는 이것과 균형이 잡히는 상반력)과 상체 병진운동과의 관계를 나타내는 부분 모델과, 상기 합력과 상체 회전운동과의 관계를 나 타내는 부분 모델을 분리하여 갖는 분리형 모델. 예를 들면, 도 10에 도시하는 질점은, 상기 합력과 상체 병진운동과의 관계를 나타내는 부분 모델이며, 도 10에 도시하는 플라이 휠은, 상기 합력과 상체 회전운동과의 관계를 나타내는 부분 모델이다.

또, 각 처리에서 사용되는 모델은 동일해도 좋고, 처리에 따라서 적당하게, 모델을 가려 사용해도 좋다. 예를 들면, 상기 정상보용은 금회 보용의 종단상태를 결정하기 위해서만 생성되는 것이므로, 그 정상보용의 동역학적인 정밀도에 대한 요구는, 금회 보용에 비해 작다. 그래서, 예를 들면 금회 보용의 생성 처리에서는, 도 10의 동역학 모델(3질점 + 플라이 휠의 모델)을 사용하는 한편, 정상보용의 생성 처리(특히 도 21의 S408, S412)에서는, 각 다리체(2)의 질량을 무시하고, 상체(24)에 대응하는 상체 질점(24m)과, 플라이 휠(FH)로 구성되는 동역학 모델(도 10의 모델에서 다리 질점(2m, 2m)을 제거한 1질점+플라이 휠의 모델)을 사용하여 정상보용을 생성하도록 해도 좋다. 이 경우의 정상보용의 생성 처리에서는, 상기 실시형태에서, 다리 질점(2m)의 질량을 0로 하여 상기 도 21의 S408, S412의 처리를 행하도록 좋다. 이렇게 하면, 정상보용의 생성 처리에서의 연산량을 대폭 삭감할 수 있다.

또한, 상기 실시형태에서, 블록선도, 플로차트, 및 알고리즘 등은, 연산 처리 순서를 바꾸는 등의 등가변형을 해도 좋다. 또, 적당하게 로 패스 필터를 삽입해도 좋다.

또, 상기 실시예를 2족 이동로봇에 관하여 설명해 왔지만, 3족 이상의 다족 로봇에도 응용할 수 있다.

상기 도 19의 S206에서 초기(시각 Ts) 상체 연직위치(Zs)를 결정하는 수법에 대해 보충하면, 본 출원인이 먼저 일본 특개평10-86080호 공보에서 제안한 상체 높이 결정 수법에서는, 양 다리체의 무릎 구부림각이 어떤 관계를 만족하도록, 상체 연직위치가 결정되는데, 시각 0에서의(즉 착지 순간에서의) 상체 연직위치를 결정하는 방법으로서는, 이것 이외에, 막 착지한 다리체의 무릎 구부림각이 어떤 소정의 각도 또는 각도 범위가 되도록 결정해도 좋다. 착지 충격흡수 능력은, 주로 착지하는 다리의 무릎 구부림각에 의존하기 때문이다. 보충하면, 무릎 구부림각이 0에 가까우면(지나치게 펴져 있으면) 착지 충격흡수 능력이 떨어지는 경향이 있다. 반대로 지나치게 구부리면, 무릎관절 액추에이터의 토크가 과대해질 우려가 있다.

더욱 보충하면, 본 실시형태에서는 도 19의 S206에서, 착지 순간에서는 상체 연직위치를 키네매틱스적(기하학적)으로 결정하고, 그 이외의 시기에서는 목표 연직 상반력을 기초로 동역학을 만족하도록 상체 연직위치를 결정했다. 단, 보다 일반적으로는, 착지 전후 또는 이상 전후 등, 가장 다리체가 펴질(바꿔 말하면, 무릎 구부림각이 가장 0도에 가까운, 또는 고관절과 발목의 사이의 거리가 가장 긴) 때에 있어서의 상체 연직위치를 키네매틱스적(기하학적)으로 결정하고, 그 이외는, 목표 연직 상반력을 기초로 동역학을 만족하도록 상체 연직위치를 결정해도 좋다.

실시예와 같이, 탐색 수법을 사용하거나 부분적으로 해석 수법을 사용하거나, 정상보용의 초기 상태(주로 초기 상체 수평위치 속도, 초기 상체 연직위치 속도)를 결정하는 수법 대신에, 미리, 상기 수법을 사용하여 여러가지 정상보용 파라 미터에 대해 계산해 두고, 정상보용 파라미터와 정상보용의 초기 상태와의 관계를 맵화 혹은 근사식화해서 기억해 두고, 실제로 이동시에는, 맵화 또는 근사식화된 상기 관계를 기초로, 정상보용의 초기값을 결정해도 좋다.

더욱이, 맵화 또는 근사식화된 상기 관계와 상기 함수f를 합성한 함수를 맵화 또는 근사식화 하여 기억해도 좋다. 즉, 상기한 족평궤도 파라미터, 상반력 연직궤도 파라미터 등으로 이루어지는 정상보용 파라미터로부터, 정상보용의 발산 성분을 직접 구하는 함수를 맵화 또는 근사식화 하여 기억해도 좋다. 예를 들면, 복수 종류의 대표적인 정상보용 파라미터 세트의 각각에 대해, 사전에 정상보용을 생성해 두고, 각 조의 정상보용 파라미터에 대한 정상보용의 초기 상태(도 12의 S024에서 구하는 것)를 구해 두고, 또한 그 각 세트의 정상보용 파라미터와, 정상보용 초기 상태와의 관계를 도시하는 맵을 미리 작성해 둔다. 그리고, 목표 보용을 생성할 때에, 결정된 정상보용 파라미터의 세트로부터, 상기 맵에 기초하여 선택 또는 보간에 의해 정상보용 초기 상태를 구하도록 하면 된다. 이렇게 하면, 금회 보용의 생성 마다, 정상보용을 생성할 필요가 없어져, 목표 보용 생성 처리의 연산량을 대폭 삭감할 수 있다.

정상보용으로 연결하기 위한 금회 보용의 수정 방법으로서, 본 실시형태에서는, 금회 보용의 목표 ZMP 파라미터를 수정했지만, 그 이외의 파라미터를 수정해도 좋다.

예를들면, 금회 보용의 미착지 다리의 공중에서의 궤도를 바꾸어도 좋다. 또, 예를들면, 상체 수평위치가 정상보용보다도 후방으로 벗어나버릴 것 같은 경우 에는, 미착지 다리를 이상후 재빠르게 전방으로 이동시켜서 미착지 다리 중심위치를 전방으로 비키어 놓는다. 이렇게 함으로써, 목표 ZMP를 만족하기 위한 상체 수평위치는, 전방으로 보다 한층더 가속되지 않을 수 없게 된다. 이 결과, 상체 수평위치가 금회 보용 종단에서, 보다 한층더 전방으로 이동하여, 정상보용에 일치시킬 수 있다.

또, 목표 ZMP 파라미터를 수정하는 대신에, 금회 보용의 주기를 수정해도 좋다. 예를들면, 상체 수평위치가 정상보용보다도 후방으로 벗어나버릴 것 같은 경우에는, 금회 보용의 주기를 길게 하면 좋다. 금회 보용의 주기를 길게 함으로써, 상체 수평위치가 이동하는 시각이 연장되고, 그만큼 전방으로 이동할 수 있기 때문이다.

단, 탐색 수법에 의해, 상체 수평위치 등의 적정값을 결정할 때에, 목표 ZMP 파라미터를 수정하는 경우에서는, 목표 ZMP의 수정량에 거의 비례하고, 금회 보용 종단 상체 수평위치가 변화되므로, 상기 적정값의 탐색 회수가 적어도 좋다. 이에 대해, 미착지 다리 중심궤도나 보용의 주기를 수정하는 경우에는, 그 수정에 대해, 금회 보용 종단 상체 수평위치가, 상당히 비선형으로 변화되므로, 상기 적정값의 탐색에 보다 많은 탐색 회수를 요한다.

정상보용으로 연결하기(근접시키기) 위한 금회 보용의 수정 방법으로서, 본 실시형태에서는, 금회 보용의 목표 ZMP 파라미터를 수정했다. 이 방법에서는, 경우에 따라서는, 목표 ZMP 파라미터의 수정량(상기 도 38의 수정량 a)이 과대하게 되는 일이 있다. 예를들면, 그자리에서 호핑하고 있는 보용으로부터 갑자기 고속 으로 이동하는 요구(주행요구)가 주어지면, 고속의 정상보용(주행의 정상보용)으로 연결하기(근접시키기) 위해, 목표 ZMP 파라미터는, 진행 방향에 대해 뒤쪽에 극단적으로 크게 비키어 놓을 필요가 생긴다. 이 경우에는, 상기한 바와 같이 목표 ZMP 파라미터이외의 보용 파라미터도 수정하는 것이 바람직하다. 단, 이 경우에서는, 급격한 가속을 요구하는 것 자체에 무리가 있었기 때문이므로, 요구값 자신을 수정해도 좋다.

요구값의 수정을 행하는 경우에는, 예를 들면 우선 본 실시형태에 나타낸 수순을 따라서, 요구(요구 파라미터)를 만족하는 정상보용을 구하고, 이 정상보용에 이어지도록 금회 보용 파라미터를 결정한 시점에서, 금회 보용의 목표 ZMP 궤도의 안정 여유가 지나치게 적어진 것인지 아닌지를 판정한다. 그리고, 안정 여유가 지나치게 적은 경우(소위 지지 다각형으로부터 목표 ZMP가 벗어나거나, 지지 다각형의 단부 부근에 목표 ZMP가 위치하는 경우)에는, 요구를 수정하도록 하면 된다.

또는, 미리 보용의 가감 속도(다음회 보용 초기속도-금회 보용 초기속도)/금회 보용의 주기)의 허용범위를 설정해 놓고, 요구(보용 주기에 관한 요구 파라미터)를 받은 시점에서, 이 요구에 대응하는 가감 속도를 구하고, 그 구한 가감 속도가 허용범위를 넘었으면, 허용범위가 되도록, 요구를 수정해도 좋다.

금회 보용의 종단 상체 연직위치 속도를 정상보용의 초기 상체 연직위치 속도에 일치시키기 위한 상반력 연직성분 파라미터가 로봇의 능력을 초과하고 있는 경우에는, 상기 위치 속도 양쪽을 일치시키는 대신에, 적어도 상체 연직위치만은 일치시키도록 상반력 연직성분 파라미터를 결정해도 좋다.

또는, 금회 보용의 종단 상체 연직위치를 정상보용의 초기 상체 연직위치보다도 낮게 하고, 금회 보용의 종단 상체 연직속도를 정상보용의 초기 상체 연직속도보다도 상향으로 커지도록 상반력 연직성분 파라미터를 결정해도 좋다.

또는, 어떤 소정의 중요도를 사용하여, 금회 보용의 종단 상체 연직위치와 금회 보용의 종단 상체 연직속도의 가중평균(중요도부가 평균)을 구하고, 구한 가중평균이, 정상보용의 초기 상체 연직위치와 초기 상체 연직속도의 가중평균과 일치하도록 상반력 연직성분 파라미터를 결정해도 좋다.

또는, 로봇의 위치에너지와 운동에너지의 합이, 금회 보용의 종단과 정상보용의 초기에서 일치하도록 상반력 연직성분 파라미터를 결정해도 좋다.

또는, 도 34의 S718에서 금회 보용의 상체 자세각이 정상보용의 상체 자세각에 근접하도록, 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec)의 패턴을 결정하는 수법을 사용해도 좋다. 즉, S718의 수법 또는 후술하는 금회 보용의 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec)을 결정하는 다른 수법에서, 상체 자세각을 상체 연직위치로, 상체 자세 복원 모멘트 ZMP 환산값을 상반력 연직성분으로 바꿔놓는 수법을 사용하여, 상반력 연직성분 파라미터를 결정해도 좋다.

상반력 수평성분 허용범위 [Fxmin, Fxmax]를, 식62와 식63을 만족하도록 설정해도 좋다.

Fxmax = kmax·μ·Fz+kc·(XG-ZMPx)/(ZG-ZMPz)·Fz

Fxmin = kmin·μ·Fz+kc·(XG-ZMPx)/(ZG-ZMPz)·Fz

(kmax≥0, kmin≤0)

……식62

단, Fxmax는, 식12에서 Ka=1로 한 경우의 Fxmax의 값보다 커지지 않도록 설정하고, Fxmin은, 식12에서 Ka=1로 한 경우의 Fxmin의 값보다 작아지지 않도록 설정한다.

여기에서, ZMPx는 목표 ZMP의 수평위치, ZMPz는 목표 ZMP의 연직위치를 나타낸다.

또 여기에서, (XG, ZG)는, 적어도 상체의 질점을 포함하는 질점집합의 중력과 관성력과의 합력을 근사적으로 발생하는 질점의 위치로 한다(XG는 수평위치, ZG는 연직위치를 나타냄). 바꿔 말하면, (XG, ZG)는, 적어도 상체의 질점을 포함하는 질점집합을 대표하는 질점의 위치로 한다. 예를 들면, 전체 중심위치, 또는 다리체의 일부를 제외한 전체 중심위치로 한다. 또는, 상기 실시예에 사용한 도 10에 도시하는 동역학 모델의 상체 질점(24m)의 위치(Xb, Xz)라도 좋다.

상기 식62에서 kc=0으로 설정한 경우에는, 상기 실시형태와 동일하게 된다(상기 식12를 참조).

kc=1, 또는 Fxmax와 Fxmin을 거의 0에 설정한 경우에는, 목표 ZMP(ZMPx, ZMPz)에 작용하는 목표 상반력(Fx, Fz)의 벡터가, 대략 (XG, ZG)를 가리키므로, 목표 상반력의 방향의 변동(특히 높은 주파수의 변동)을 작게 할 수 있다.

그 결과, 목표 상반력 수평성분(Fx)의 변화가 온화해져서, 접지성이 높아져 슬립을 방지할 수 있다.

또, 상체 수평위치 궤도의 가속도는 Fx에 비례하므로, 상체 수평위치 궤도의 가속도의 변화도 완만하게 되고, 목표 ZMP를 수정했을 때의 보용 종단 상체 수평위치의 변화가 선형적(직선적, 리니어)이 되고, 정상보용이나 금회 보용의 탐색에서의 수속이 빨라져, 탐색 회수를 감소시킬 수 있다.

더욱이, kc=1 또한, kmax=kmin=0으로 설정한 경우에는, 1질점 모델의 거동이 된다(특히, (XG, ZG)가 전체 중심인 경우). 이 경우, 정상보용의 상체 수평위치 궤도가 용이하게 탐색 가능하다.

또, 식62에서, (ZG-ZMPz) 대신에 어떤 소정의 정수로 해도 좋다. 보행이나 주행중에서는, (ZG-ZMPz)는, 크게 변동하지 않기(변동해도 10퍼센트 정도이기) 때문이다.

kc=1 또한, kmax=kmin=0, 또한 식62에서, (ZG-ZMPz)의 대신에 어떤 소정의 정수로 하고, (XG, ZG)를 전체 중심위치로 한 경우, 1질점 선형 도립 진자의 거동이 되므로, 거동을 해석적으로 풀 수 있어, 탐색 수법을 사용하지 않아도, 정상보용이나 금회 보용을 결정할 수 있다. 단, 상체 회전 모드의 각가속도의 변동은 커진다.

보충하면, 상기 △Mp, △Fp, △Mr 및 △Fr는, 상기와 같이 단순한 동역학 모델을 사용하는 경우에는, 동역학 연산에 의해 해석적으로 구해도 좋지만, 일반적인 보다 복잡한 동역학 모델을 사용하는 경우에는, 상체를 상체 병진 모드에서 미소량만큼 가속 또는 상체 회전 모드에서 미소량만큼 가속화시킨 경우의 상반력을 구하고, 가속시키지 않은 경우의 상반력과의 차이를 구하고, 이 차이를 상기 미소량으로 나눔으로써 구하면 된다.

또, 미리 표준적은 보용에서의 △Mp, △Fp, △Mr, △Fr 및 △Mp/△Mr 등의 각각의 평균값을 구해 두고, 그것을 사용해도 된다. △Mp, △Fp, △Mr, △Fr 및 △Mp/△Mr는, 상태(자세와 그 변화율)에 의해 변하므로, 각 순간의 상태마다 구하는 방법에 비해, 정밀도는 약간 저하되지만, 상기 모델보다도 복잡한 모델을 사용한 경우에는, 연산량을 대폭 저감할 수 있다.

상기 도 22의 S504로부터 S516까지의 처리 및 도 37의 S1002로부터 S1014까지의 처리에서, 상반력 수평성분(Fx)에 허용한계까지 여유가 있으면, 이 여유를 이용하여, 그 시점까지의 상체 자세각가속도(β)의 적분값, 즉 상체 자세각속도를, 서서히 원래로(초기 상태로) 되돌리도록 해도 좋다.

구체적으로는, S506 및 또는 S1004에서, 우선, 상체 자세각가속도(β)의 적분값을 서서히 되돌리기 위한 상체 자세각가속도(β)의 값(이것을 βrec라고 부름)을 그것까지의 상체 자세각가속도(β)의 적분값 등으로부터 결정한다. 예를 들면 상체 자세각가속도(β)의 적분값에 마이너스의 게인을 곱한 값을 βrec로 해도 좋다.

다음에 Fxtmp을 상기 식17 대신에 다음 식70으로 결정한다.

Fxtmp = △Fp*αtmp - βrec*△Fp*△Mr/△Mp

+ msup*d2Xsup/dt2 + mswg*d2Xswg/dt2

……식70

식70의 우변 제 2 항은, 상체 자세각가속도가 βrec인 상체 회전 모드의 운동을 시켰을 때에, 목표 ZMP 주위에 발생하는 모멘트(βrec*△Mr)를 없애기 위해서 필요한 상체 병진 모드의 가속도(-βrec*△Mr/△Mp)에 의해 발생하는 상반력 수평성분을 의미한다.

식70에 의해 결정되는 Fxtmp는, 로봇의 전회 순시 목표 보용상태(시각 k-1의 목표 보용상태)로부터, 임시로 상체 자세각가속도가 βrec인 상체 회전 모드의 운동을 시키면서 상체 병진 모드의 운동을 시킨 경우에, 금회(시각 k) 목표 ZMP를 만족하기 위해서 필요한 상체 병진 모드의 상체 수평가속도를 의미한다.

그것 이후의 처리는, 상기 실시형태대로 한다. 이상의 처리에 의해, 상반력 수평성분(Fx)의 여유를 이용하여, 어느 정도, 상체 자세각속도를 복원할 수 있으므로, 상체 자세각속도 변동을 보다 한층더 작게 억제할 수 있다.

도 37의 S1016에서 값이 0에 일정한 ZMPrec의 패턴을 설정한 경우에, 가금회 보용의 종단 상체 자세각이 정상보용의 초기 상체 자세각으로부터 크게 벗어나버리는 경우에는, 어긋남이 작아지도록 도 26에 도시하는 사다리꼴 패턴의 높이를 0 이외로 설정해도 상관없다. 그 대신에, 그 이후의 도 34의 S718에서, S718에서 구해지는 상체 자세 복원 모멘트 ZMP 환산값 패턴에, S1016에서 설정한 상체 자세 복원 모멘트 ZMP 환산값 패턴을 가산할 필요가 있다. 이렇게 하면, 최종적으로 얻어지는 보용은, S1016에서 값이 0으로 일정한 패턴을 설정한 경우와 동일 또는 거의 동일한 보용이 된다.

단, 도 34의 S700의 탐색 루프의 처리를 개시하고 나서 완료할 때까지의 사이는, 도 37의 S1016에서 설정하는 상체 자세 복원 모멘트 ZMP 환산값 패턴(도 26에 도시하는 ZMPrec의 사다리꼴 패턴의 높이)을 변경하지 않는 것이 바람직하다. 도중에 변경하면, 종단 발산 성분(errq)이 수속하지 않게 되어, 루프를 빠져 나갈 수 없게 될 우려가 있기 때문이다.

금회 보용의 상체 자세 복원 모멘트 ZMP 환산값(ZMPrec) 패턴의 사다리꼴높이 bcurr를 결정하는 방법으로서, 이하의 방법을 사용해도 좋다.

상기 ZMPrec 수정 보용(도 34의 S718의 설명을 참조)의 금회 보용 종단시점에서의 상체 자세각과 각속도를 각각 θcurr, vθcurr로 하고, 이것들과, 정상보용의 초기 상체 자세각, 각속도와의 차이를 △θcerr, △vθcerr로 한다.

보용 주기를 인터벌로 하고, 가금회 보용의 종단 상체 자세각, 각속도와 정상보용의 초기 상체 자세각, 각속도와의 차이 θerr, vθerr을 전회상태, bcurr를 입력, △θcerr, △vθcerr을 금회상태로 하는 이산계의 상태방정식을 세우고, △θcerr, △vθcerr를 0에 수속시키도록, 현대 제어이론 등을 사용하여 피드백칙을 결정하고, 이것에 기초하여 bcurr을 구해도 좋다.

금회 보용 및 또는 정상보용의 ZMPrec는, 사다리꼴 패턴으로 하지 않고, 각 순간에 있어서, 목표 상체 자세각·각속도와 기준 상체 자세각·각속도와의 차이 등을 기초로, 그 차이가 0에 수속하도록, 상태 피드백칙 등을 사용하여, 각 순간에 있어서의 ZMPrec의 값을 결정해도 좋다.

금회 보용의 ZMPrec는, 사다리꼴 패턴으로 하지 않고, 각 순간에서 금회 보용의 목표 상체 자세각·각속도를 기초로, 이것들이 제 1 선회 보용의 초기 상체 자세각·각속도에 근접하도록, 상태 피드백칙 등을 사용하여, 각 순간에 있어서의 ZMPrec를 결정해도 좋다.

다음에 로봇(1)의 보행 보용의 생성, 및, 보행과 주행과의 이행에 관하여 설명한다. 또한, 여기에서, 보행 보용은, 공중기가 없고, 편각지지기와 양쪽 다리 지지기가 번갈아 반복되는 보용이다.

본 실시예에서는 보행 보용을 생성하는 경우에는, 본 출원인이 먼저 제안한 상체 높이 결정법(일본 특개평 10-86080호 공보) 등을 사용하여, 적어도 기하학적 제약조건에 기초하여 상체 연직위치가 결정되고, 주행보용을 생성하는 경우에는, 상기 도 36의 S906, S908과 같이 상반력 연직성분으로부터 결정된다. 이 경우, 보행으로부터 주행 또는 주행으로부터 보행으로 이행할 때의 보용에 있어서의 최종적인 상체 연직위치는, 기하학적 제약조건에 기초하여 결정되는 보행용의 상체 연직위치(Zg)와, 상반력 연직성분으로부터 동역학적으로 결정되는 주행용의 상체 연직위치(Zf)를 합성함으로써 결정하는 것이 바람직하다. 보다 구체적으로는, 예를 들면 다음 식71과 같이, 그것들의 상체 연직위치(Zg, Zf)의 가중 평균에 의해 최종적인 상체 연직위치 Z를 결정한다. 이 경우, 중요도(w)를 1로부터 0 또는 0으로부터 1로 서서히 변화시킨다.

이것에 의해, 보행과 주행 사이의 이행시의 상체 연직위치 궤도를 원활하게 변화시킬 수 있다.

상체 연직위치(Z) = (1-w)*Zg+w*Zf ……식71

구체적인 처리의 1예를 도 42 등을 사용하여 이하에 설명한다. 또한, 이 경우의 처리는, 상기 도 12∼도 38을 참조하여 설명한 주행보용에 관계되는 처리의 일부를 변경하면 되므로, 그 변경 부분을 주체로 설명한다.

상기 정상보용의 생성에 관계되는 도 21의 S408과, 금회 보용의 생성에 관계되는 도 36의 S908 대신에, 도 42의 처리가 실행된다. 즉, 우선, S1500에서, 목표 상반력 연직성분 궤도에 따라 정해지는 전체 중심 연직위치 궤도를 만족하는 상체 연직위치(Zf)가 산출된다. 이 처리는, 도 21의 S408 또는 도 36의 S908의 처리와 동일해도 좋다. 이어서 S1502로 진행하고, 본 출원인이 먼저 제안한 상체 높이 결정법(일본국 특개평 10-86080호 공보) 등을 사용하고, 적어도 각 다리체(2)의 관절(무릎관절)의 변위에 관한 기하학적 제약조건에 기초하여 결정되는 상체 연직위치(Zg)를 구한다. 이 상체 연직위치(Zg)는, 예를 들면 목표 양 족평위치 자세의 금회값(시간 k의 값), 기준 상체 자세의 금회값, 목표 상체 수평위치의 전회값(시간 k-1의 값)을 사용하여 일본 특개평 10-86080호 공보의 도 6에 나타낸 처리를 실행함으로써 구해진다. 이 경우, 상기 기하학적 조건은, 예를 들면 주행보용에 관계되는 도 19의 S206에 관하여 설명한 경우와 동일하게, 지지다리 및 미착지 다리의 다리체(2, 2)의 무릎 구부림각(θsup, θswg)의 정현값의 역수의 합(이것은 각 다리체(2)가 성장하는 펴지는 방향으로의 무릎관절(특정 관절)의 변위에 대해 단조변화하는 연속한 함수의 값임)이 어떤 소정값(유한값)이 된다는 조건이다. 또한, 이와 같이 상체 연직위치(Zg)를 결정함으로써, 결과적으로는 전체 중심 연직위치도, 각 다리체(2)의 관절(무릎관절)의 변위에 관한 기하학적 제약조건에 기초하여 결정되게 된다.

이어서, S1504로 진행하고, 보용의 시기에 따라 중요도(w)를 결정한다. 예를 들면 보용의 초기로부터 종단에 걸쳐서, 0으로부터 1 또는 1로부터 0으로 서서 히 변화되도록 상기 식71의 중요도(w)를 결정한다. 이 경우, 보행으로부터 주행으로의 이행시에는, w를 0으로부터 1로 변화시키고, 주행으로부터 보행으로의 이행시에는, w를 1로부터 0으로 변화시키도록 중요도(w)를 결정한다. 또한, 이 때, 중요도(w)의 변화율(중요도의 1차 시간미분)도 연속적으로 변화하도록 설정하면 좋다. 게다가, 중요도(w)의 변화율의 변화율(중요도의 2차 시간미분)도 연속적으로 변화하도록 설정하면 좋다. 이렇게 함으로써, 최종적으로 생성되는 상체 연직위치 궤도가 원활하게 된다. 또, 보행의 계속중은 중요도(w)를 0으로 설정하면 좋고, 주행의 계속중은 중요도(w)를 1로 설정하면 좋다. w=0으로 했을 때에는, 상기 기하학적 조건에 기초하는 상체 연직위치가, 최종적인 상체 연직위치로서 선택되게 된다. 동일하게 w=1로 했을 때에는, 목표 상반력 연직성분에 대응하는 상체 연직위치가, 최종적인 상체 연직위치로서 선택되게 된다.

최후에, S1506으로 진행하고, 상기 식71에 의해, 최종적인 상체 연직위치를 결정한다.

또한, 상기와 같이, 보행으로부터 주행 또는 주행으로부터 보행으로 이행할 때에만, 중요도(w)가 0과 1의 중간의 값을 취하는 것이 아니라, 그 이외의 때에서도 중요도(w)가 0과 1의 중간의 값을 취하도록 해도 좋다. 또, 보행으로부터 주행, 또는 주행으로부터 보행으로의 이행은, 1보용으로 행할 필요는 없고, 복수 보용으로 행하도록 해도 좋다.

또, 보행의 계속중은, 도 14의 S106, 도 32의 S606에서는, 목표 상반력 연직성분 궤도 파라미터는, 예를 들면 그것에 의해 규정되는 목표 상반력 연직성분이, 보용의 1주기의 기간, 일정값(=로봇(1)의 자중) 또는 대강 일정값으로 되도록 설정된다. 또한, 이 경우, 예를 들면 도 43에 도시하는 바와 같은 패턴으로 목표 상반력 연직성분 궤도를 설정하도록 해도 좋다. 이 패턴에서는 편각지지기보다도 양쪽 다리 지지기 쪽이 약간, 목표 상반력 연직성분이 크게 된다. 따라서 상반력 수평성분의 허용범위는, 편각지지기보다도 양쪽 다리 지지기 쪽이 약간 커진다.

이상에서 설명한 이외의 처리는, 상술한 주행보용에 관계되는 처리와 동일해도 좋다.

또한, 보행의 계속중은, 상기 기하학적 조건(기하학적 제약조건)에 기초하여 상체 연직위치가 결정되므로, 도 21의 S406, 도 36의 S906의 처리 등, 보행시의 상체 연직위치를 결정하는 점에서 불필요한 처리는 생략해도 좋다.

사면이동시(경사진 바닥면에서 로봇(1)을 이동시킬 때)의 보용 생성에서는, 상반력 수평 성분 허용범위나 전체 중심 가속도 수평성분 대신에, 병진 상반력의 바닥면 평행성분(바닥면에 평행한 성분), 즉 마찰력의 허용범위, 또는 전체 중심 가속도의 바닥면 평행성분(이것은 마찰력에 비례함)의 허용범위를 설정하도록 해도 좋다. 예를 들면, 병진 상반력의 바닥면 평행성분(마찰력)의 허용범위를 설정할 경우에 관해서 설명(이 설명은 전체 중심 가속도의 바닥면 평행성분의 허용범위를 설정하는 경우라도 동일)하면, 이 마찰력은, 바닥면의 수평면에 대한 경사각도 θf(로봇(1)의 진행 방향을 향해서 전방으로 내려가는 사면의 경우를 정으로 한다)라고 하면, 다음 식72의 관계가 성립한다. 따라서, 상기 실시형태와 동일한 알고리즘으로 보용을 생성할 경우, 이 식72의 관계를 사용하여, 마찰력 허용범위를 상 반력 수평성분 허용범위로 변환함으로써, 이 상반력 수평성분 허용범위를 설정하도록 하면 된다. 또한, 이 경우, 식72의 상반력 연직성분에는, 목표 상반력 연직성분을 사용하면 된다.

마찰력 = 상반력 수평성분*cos(θf) - 상반력 연직성분*sin(θf)

……식72

금회 보용의 파라미터 결정은, 상기 실시형태와 같이 전회보용이 완료한 시점에서 행하는 이외에, 본원 출원인에 의한 일본 특원 2000-352011호에 기술되어 있는 바와 같이, 금회 보용의 생성 도중에, 재결정해도 좋다. 이렇게 함으로써, 연산량은 증가하지만, 보용에 대한 요구에 변경이 있었을 때에 즉응할 수 있기 때문이다.

또, 보용의 수정(금회 보용 파라미터를 재결정하는 것)이, 금회 제어주기에서 타이밍이 맞지 않을 경우에는, 우선 금회는, 수정하지 않는 보용 혹은 잠정적으로 수정한 보용(탐색 도중의 보용으로서, 완전하게는, 탐색 완료 조건(보용 경계조건의 어긋남이 허용값 이내에 들어가 있는 것)을 만족하고 있지 않은 보용)을 출력하고, 다음회 제어주기까지, 또는 복수 제어주기 후까지(잠정적이 아님) 적절한 수정 보용으로 해도 좋다. 수정된 목표 ZMP 궤도와 목표 상반력 연직성분 궤도는 연속이며, 단시간 후에는 이것들이 급격하게 변화되는 일은 없으므로, 금회 보용의 목표 ZMP 궤도와 목표 상반력 연직성분 궤도가 약간 깔쭉깔쭉하게 될 뿐이며, 거의 문제는 없다.

상기 도 12의 S032에서의, 스핀력을 캔슬하는 팔흔들기 동작 대신에, 상체(24)를 그 체간축(또는 연직축) 주위로 회전시키는(비트는) 동작을 행해도 좋다. 또는, 팔흔들기 동작과 상체(24)를 그 체간축(혹은 연직축) 주위로 회전시키는(비트는) 동작을 병용해도 좋다.

상기 보용의 생성에 관계되는 도 21의 S408 및 도 36의 S908, 또는 도 42의 S1500에서, 전체 중심 연직위치 속도를 만족하는 상체 연직위치를 결정할 때는, 상기와 같이 식04 등을 사용하여 해석적으로 상체 연직위치를 결정하는 대신에, 이하의 다른 수법 1 또는 2에 의해 상체 연직위치를 결정해도 좋다.

다른 수법 1

도 44에, 다른 수법 1의 연산 처리 플로차트를 나타낸다.

우선 S1100에서, 전회 순간 목표 보용상태(시각 k-1에 결정된 목표 보용상태) 등을 기초로 동역학 모델을 사용하여 전체 중심 연직위치·속도가 산출된다. 이후, 동역학 모델을 사용하여 산출한 전체 중심 연직위치·속도를 모델 전체 중심 연직위치·속도라 부른다. 즉, S1100에서는, 전회 모델 전체 중심 연직위치·속도가 산출된다. 또한, 상기 동역학 모델은 예를 들면 상기 도 11과 같은 다질점 모델 등을 사용하면 된다.

이어서, S1102로 진행하고, 상기와 같이 구한 전회 모델 전체 중심 연직위치·속도와, 도 21의 S406 또는 도 36의 S906에서 산출된 전체 중심 연직위치 속도, 즉 목표 상반력 연직성분의 금회값(시각 k의 값)을 만족하는 전체 중심 연직위치·속도와의 차이(위치의 차이와 속도의 차이의 양자. 이후 이 차이를 중심 연직위치·속도편차라고 부름)가 구해진다. 즉, 다음 식80에 의해 중심 연직위치·속도편 차가 결정된다.

중심 연직위치·속도편차

= 전회 모델 전체 중심 연직위치·속도

- 목표 상반력 연직성분을 만족하는 전체 중심 연직방향 위치·속도

……식80

이어서, S1104로 진행하고, 목표 상반력 연직성분을 만족하는 전체 중심 연직위치·속도에 상기 모델 전체 중심 연직위치·속도가 추종하도록, 적어도 상기 중심 연직방향 편차와 전회 상체 연직위치(시각 k-1에서 결정된 상체 연직위치)를 기초로, 금회 상체 연직위치(상체 연직위치의 금회값)를 결정한다. 예를 들면, 다음 식에 의해 금회 상체 연직위치가 결정된다.

금회 상체 연직위치

= 전회상체 연직위치

+ Kgz*중심 연직위치 편차

+ Kgvz*중심 연직속도 편차

……식81

단, Kgz, Kgvz는 게인이다.

Kgvz는 0이라도 상관없다. 이 경우, 모델 전체 중심 연직속도, 목표 상반력 연직성분을 만족하는 전체 중심 연직속도 및 중심 연직방향 속도 편차를 산출하는 필요는 없다.

이상과 같이, 금회 상체 연직위치가 산출된다. 이 경우, 식81에 의할 때에 는, 중심 연직위치 편차 및 중심 연직속도 편차에 따라 금회 상체 연직위치가 산출되게 된다.

또, 금회 상체 연직위치를 구하기 위해서, 중심 연직위치 편차에 따른 성분이나, 중심 연직속도 편차에 따른 성분 이외에, 더욱, 예를 들면 이하와 같이 로봇(1)의 모델(예를 들면 도 10의 모델로부터 플라이 휠(FH)을 제외한 3질점 모델)을 사용하여 구한 상체 연직위치에 따른 성분(피드 포워드 성분)을 전회 상체 연직위치에 가산함으로써, 금회 상체 연직위치를 구하도록 해도 좋다. 이 경우에는, 도 21의 S406 또는 도 36의 S906에서 산출된 전체 중심 연직위치 속도, 즉 목표 상반력 연직성분의 금회값(시각 k의 값)을 만족하는 전체 중심 연직위치 속도와, 도 21의 S404 또는 도 36의 S904에서 산출된 목표 양쪽 족평위치 자세의 금회값(시각 k의 값)으로부터, 예를 들면 상기 3질점 모델을 사용하여 가상체 연직위치(이것을 여기에서는, FF 상체 연직위치라고 함)를 구한다. 또한, 도 21의 S408 또는 도 36의 S908에서 상술과 같이 도 10의 모델을 사용하여 상체 연직위치를 구하고 있었을 경우에는, 그것을 그대로 상기 FF 상체 연직위치로서 이용하면 된다. 그리고 이와 같이 하여 구한 FF 상체 연직위치에 적당한 게인을 승산한 것을 피드 포워드 성분으로 하여, 상기 식81의 우변에 더욱 가산하고, 이것에 의해 금회 상체 연직위치를 구한다. 즉, 중심 연직위치 편차 및 중심 연직속도 편차와(중심 연직속도 편차는 생략해도 좋음), FF 상체 연직위치에 따라 전회 상체 연직위치를 보정함으로써, 금회 상체 연직위치가 구해진다. 이렇게 하면, 최종적으로 구해지는 금회 상체 연직위치의 변동을 억제할 수 있다. 또한, 이 수법에서 전회 상 체 연직위치를 구하기 위해서 사용하는 로봇(1)의 모델은, 보다 엄밀한 모델인 것이 바람직하다.

다른 수법 2

또, 이하 도시하는 바와 같이 반복 탐색 수법을 사용한 다른 수법 2에 의해 금회 상체 연직위치를 결정해도 좋다.

도 45에, 그 수법의 연산 처리 플로차트를 나타낸다.

우선 S1200에서, 상체 연직위치 속도(보다 상세하게는 상체 연직위치 속도의 금회값의 후보)가 가결정된다. 예를 들면, 전회목표 보용 순시값의 상체 연직위치 속도(전회값)가 상체 연직위치 속도의 후보값으로서 가결정된다. 또는, 상체 연직위치의 전회값에, 전회 목표 보용 순시값의 상체 연직속도와 제어주기(△t)와의 적을 가한 값을 상체 연직위치의 후보값으로서 가결정해도 좋다.

이어서, S1202을 거치고, S1204로 진행하고, 상체 연직위치 속도(후보)의 현재값(현재의 가결정값)과, 상기 구한 목표 양쪽 족평위치 자세의 금회값, 시각 (k-△k)의 목표 상체 자세(또는 상기 구한 기준 상체 자세의 금회값), 상기 구한 목표 팔자세의 금회값, 시각(k-△k)의 상체 수평위치 및 그것들의 변화율로부터 결정되는 로봇(1)의 자세상태로부터 금회 모델 전체 중심 연직위치·속도가 산출된다. 또한, 금회 모델 전체 중심 연직위치·속도의 산출에서 사용하는 일부의 변수에는 전회값이 사용되고 있다.

이어서, S1206로 진행하고, 다음 식82에 의해, 중심 연직방향 위치·속도 편차를 산출한다.

중심 연직위치·속도편차

= 금회 모델 전체 중심 연직위치·속도

-목표 상반력 연직성분을 만족하는 전체 중심 연직위치

……식82

이어서, S1208로 진행하고, 식82에서 구한 중심 연직위치 편차가 소정의 허용범위내에 있으면, 최종적으로 구해진 상체 연직위치(현재값)를 상체 연직위치의 금회값(금회 상체 연직위치)으로서 결정하고, 금회 상체 연직위치 결정의 처리를 완료한다. 또, S1208의 판단에서 중심 연직위치 편차가 소정의 허용범위내에 없는 경우에는, S1210으로 진행하고, 다음 식에 의해, 상체 연직위치의 다음 후보를 결정한다.

상체 연직위치 다음 후보

= 상체 연직위치

+ Kgz*중심 연직위치 편차

+ Kgvz*중심 연직속도 편차

……식83

이어서, S1212로 진행하고, 상체 연직위치 다음 후보를 고쳐서 상체 연직위치로 하고(상체 연직위치의 후보를 갱신함), 다시 S1204로 진행한다. 그리고, 이 새로운 상체 연직위치의 후보를 기초로 금회 모델 전체 중심 연직위치·속도가 구해지고, 이하, 중심 연직방향 위치·속도 편차가 소정의 허용범위내로 될 때까지, 상기한 처리가 반복된다.

이들 수법 1, 2는, 모델 전체 중심 연직위치가, 도 21의 S406 또는 도 36의 S908에서 산출된 전체 중심 연직위치, 즉 목표 상반력 연직성분의 금회값을 전체 질량으로 나눈 값을 2차 적분함으로써 얻어지는 전체 중심 연직위치에 추종 또는 거의 일치하도록, 상체 연직위치를 결정하는 것이다.

이 대신에, 모델 전체 중심 연직속도가, 목표 상반력 연직성분의 금회값를 전체 질량으로 나눈 값을 1차 적분함으로써 얻어지는 전체 중심 연직속도에 추종 또는 거의 일치하도록, 수법 1 또는 수법 2와 동일한 수법에 의해 상체 연직속도를 결정하고, 이것을 1차 적분함으로써 상체 연직위치를 결정해도 좋다.

또는, 모델 전체 중심 연직가속도가, 목표 상반력 연직성분의 금회값을 전체 질량으로 나눈 값에 추종 또는 거의 일치하도록, 수법 1 또는 수법 2와 동일한 수법에 의해 상체 연직가속도를 결정하고, 이것을 2차 적분함으로써 상체 연직위치를 결정해도 좋다.

단, 상기 다른 수법 1 및 2에 비해, 순간적인 동역학적 오차는 적지만, 적분 오차에 의해, 모델 전체 중심 연직위치가, 목표 상반력 연직성분의 금회값을 전체 질량으로 나눈 값을 2차 적분함으로써 얻어지는 전체 중심 연직위치로부터 벗어나도, 그 어긋남을 0에 수속되는 작용이 작동하지 않으므로, 보용의 동역학적 정밀도가 장기적으로는 저하하는 경향이 있다.

바꿔 말하면, 상기 다른 수법 1 및 2에서는, 순간적인 동역학적 오차는 약간 크지만, 장기적인 동역학적 오차는 적으므로, 로봇의 자세의 안정성이 높아진다.

상술한 실시형태에서는, 보행 보용을 생성하는 경우에도 주행 보용을 생성하 는 경우과 동일하게 목표 상반력 연직성분을 설정한 뒤에, 상체 연직위치를 결정하도록 했다. 단, 보행보용의 경우에는, 반드시 목표 상반력 연직성분을 명시적으로 설정할 필요는 없고, 예를 들면 본원 출원인이 일본 특개평 10-86080호 공보에서 제안한 상체 높이 결정 수법을 사용하여, 각 다리체의 관절에 관한 기하학적 조건(기하학적 제약조건)에 기초하여 상체 연직위치를 결정하도록 해도 좋다.

이상과 같이 본 발명은 2족 이동로봇 등의 다리식 이동로봇을, 여러 보용형태로 원활하게 이동시키는 것이 가능한 보용을 생성할 수 있는 것으로서 유용하다.

Claims (7)

1. 상체로부터 뻗어 설치된 복수의 다리체를 운동시켜서 이동하는 다리식 이동로봇의 목표 보용을 생성하는 보용 생성 장치에 있어서,

   상기 로봇에 작용하는 병진 상반력의 연직성분 또는 로봇의 전체 중심가속도의 연직성분 또는 로봇의 상체 가속도의 연직성분의 목표 궤도를 결정하는 목표 연직성분 궤도 결정 수단과,

   이 목표 연직성분 궤도 결정 수단에 의해 설정된 목표 궤도에 병진 상반력 연직성분 또는 전체 중심가속도 연직성분 또는 상체 가속도 연직성분을 일치시키도록 상기 로봇의 전체 중심 또는 상체의 목표 연직위치를 결정하는 제 1 목표 연직위치 결정 수단과,

   상기 로봇의 전체 중심 또는 상체의 목표 연직위치를 상기 다리체의 관절변위에 관한 기하학적 조건에 기초하여 결정하는 제 2 목표 연직위치 결정 수단을 구비하고,

   생성하는 목표 보용의 형태에 따라, 상기 제 1 및 제 2 목표 연직위치 결정 수단에 의해 각각 결정된 목표 연직위치의 어느 한쪽을 선택하거나, 또는 이 목표 연직위치의 양자를 합성함으로써 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위치를 결정하도록 한 것을 특징으로 하는 다리식 이동로봇의 보용 생성 장치.
2. 제 1 항에 있어서, 상기 목표 보용의 형태가 로봇의 적어도 하나의 다리체를 착지시키는 착지기와, 모든 다리체를 공중에 뜨게하는 공중기를 번갈아 반복하는 제 1 보용형태일 때에는, 상기 제 1 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치를 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위치로 하여 선택하고,

   상기 목표 보용의 형태가 로봇의 적어도 어느 하나의 다리체를 항상 착지시키면서, 각 다리체의 착지 및 이상을 번갈아 반복하는 제 2 보용형태일 때에는, 상기 제 2 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치를 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위치로서 선택하는 것을 특징으로 하는 다리식 이동로봇의 보용 생성 장치.
3. 제 2 항에 있어서, 상기 목표 연직성분 궤도 결정 수단은, 상기 제 1 보용형태의 상기 착지기에서는 상기 목표 궤도에 대응하는 병진 상반력 연직성분이 그 증가측으로 볼록한 형상으로 변화하고, 또한 상기 제 1 보용형태의 상기 공중기에서는 상기 목표 궤도에 대응하는 병진 상반력 연직성분이 0으로 유지되는 패턴으로 이 목표 궤도를 결정하는 것을 특징으로 하는 다리식 이동로봇의 보용 생성 장치.
4. 제 2 항에 있어서, 상기 제 1 보용형태와 제 2 보용형태 사이의 이행기에서의 상기 목표 보용을 생성할 때에는, 상기 제 1 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치와 상기 제 2 목표 연직위치 결정 수단에 의해 결정된 목표 연직위치의 가중 평균에 의해 로봇의 전체 중심 또는 상체의 최종적인 목표 연직위 치를 결정하는 것을 특징으로 하는 다리식 이동로봇의 보용 생성 장치.
5. 제 2 항에 있어서, 상기 제 1 보용형태는, 이 로봇의 주행동작의 보용형태이며, 상기 제 2 보용형태는, 이 로봇의 보행 동작의 보용형태인 것을 특징으로 하는 다리식 이동로봇의 보용 생성 장치.
6. 제 1 항에 있어서, 상기 기하학적 조건은, 상기 다리식 이동로봇의 각 다리체를 상체에 연결하는 관절과 이 다리체의 선단부 사이에서 각 다리체가 갖는 특정 관절의 변위에 관한 기하학적 조건인 것을 특징으로 하는 다리식 이동로봇의 보용 생성 장치.
7. 제 6 항에 있어서, 상기 기하학적 조건은, 모든 특정 관절의 변위량을 변수 성분으로 하고, 또한, 각 다리체가 펴지는 방향으로의 특정 관절의 변위에 대해 단조변화하는 연속한 함수의 값을 소정값으로 유지하도록 정한 조건인 것을 특징으로 하는 다리식 이동로봇의 보용 생성 장치.

Images