KR101083414B1 - 다리식 이동 로봇의 제어장치 - Google Patents

Abstract

목표 보용에 추종하여 동작하고 있는 로봇의 미끄러짐의 발생을 판단하고, 로봇에 작용시키는 상반력 수평성분이나 상반력 모멘트 연직성분 등의 제한대상량의 허용범위를 미끄러짐의 판단 결과에 따라 가변적으로 설정한다. 목표 보용의 가운동을 동역학 모델을 사용하여 결정하고, 그 가운동에 의해 정해지는 제한대상량이 허용범위를 일탈했을 때, 동역학적 평형조건을 만족시키면서, 제한대상량을 허용범위내에 제한하도록 로봇의 각 운동량 변화율을 가운동으로부터 변화시킴으로써 이 가운동을 수정하여 목표 보용의 운동을 결정한다.

로봇, 상체, 다리체, 선단부, 제어장치

Description

다리식 이동 로봇의 제어장치{CONTROLLER OF LEGGED MOBILE ROBOT}

본 발명은 다리식 이동 로봇의 보행뿐만 아니라, 주행에도 적합한 제어장치에 관한 것이다.

다리식 이동 로봇, 예를 들면 2족이동 로봇의 이동동작을 행하게 하기 위한 보용(목표 보용)의 생성은, 종래, 주로 로봇에 부드러운 보행동작을 행하게 하는 보용(보행 보용)을 생성하는 것을 주목적으로 하여 행해져 왔다. 그러나, 최근에는, 다리식 이동 로봇의 개발이 진행됨에 따라서, 이 로봇에 보행 뿐만아니라, 주행도 행하게 할 수 있는 보용을 생성하는 것이 요망되고 있다. 게다가, 충분한 마찰력을 발생할 수 없는 미끄러지기 쉬운 바닥(소위 스키드 패드) 위에서도 로봇을 지장 없이 이동시킬 수 있는 보용을 생성하는 것이 요망되고 있다.

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

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

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

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

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

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

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

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

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

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

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

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

그렇지만, 이 기술에 있어서는, 목표 보용의 병진 상반력 수평성분이나 목표 보용의 ZMP 주위의 상반력 모멘트 연직성분의 크기를 고려하고 있지 않았다. 이 때문에, 마찰의 한계를 초과하여, 로봇의 지지 다리 족평(足平)의 미끄러짐(바닥면에 병행한 방향의 슬립이나 스핀)이 발생할 우려가 있었다. 또한, 스핀이란 실제 로봇의 요잉각(연직축 주변 회전각)속도가 목표 요잉각속도로부터 벗어나는 것을 말한다.

마찰계수가 높은 바닥면을 보행하는 경우(이 경우도 적어도 1개의 다리체는 항상 접지하고 있음)에는 상반력 연직성분이 항상 거의 로봇의 자중에 상당하므로, 마찰력의 한계가 높다. 이 때문에, 로봇의 미끄러짐은 발생하기 어렵다.

그런데, 주행에 있어서는, 상반력 연직성분이 0으로 되거나, 또는 0에 가까깝게 될 때가 있으므로, 그 때에는, 가령 마찰계수가 높더라도, 바닥면의 마찰력의 한계가 0이 되거나 또는 0에 가깝게 된다. 따라서, 목표 보용의 병진 상반력 수평성분이나 상반력 모멘트 연직성분이 한계를 초과하여, 로봇이 미끄러져 전도(轉倒)할 우려가 있었다.

또, 보행의 경우에도, 바닥의 마찰 계수가 낮을 경우에는, 로봇의 미끄러짐이 발생하여 전도될 우려가 있었다.

한편, 본 출원인은 앞서, 예를 들면 PCT출원 PCT/JP02/13596 등에서, 목표 보용의 병진 상반력 수평성분이 허용범위를 넘지 않도록, 목표 보용을 생성하거나, 목표 보용에서 팔 이외가 발생하는 모멘트 연직성분을 상쇄하도록, 팔을 흔들도록 하는 기술을 제안했다. 이 기술에 의하면, 로봇의 미끄러짐의 발생을 억제할 수 있다.

그런데, 바닥면의 상태 등에 따라서는, 병진 상반력 수평성분의 허용범위가 실제의 바닥면의 마찰력의 한계와 정합되지 않는 경우도 있으며, 그 경우에는 로봇의 미끄러짐이 발생할 우려가 있었다. 그리고, 이것을 회피하기 위해서, 병진 상반력 수평성분의 허용범위를 좁게 설정하면, 상체의 자세(경사각)가 크게 변동하기 쉽다. 또, 다리를 격렬하게 흔들어서 이동하는 경우에는 모멘트 연직성분을 상쇄하기 위해서 팔 흔들기도 심하게 된다.

따라서, 본 발명의 목적은 상기의 문제점을 해소하고, 보행, 주행 등의 보용형태나 바닥면의 마찰상태에 의존하지 않고, 로봇의 미끄러짐의 발생을 보다 확실하게 방지하여, 한층더 바람직한 보용을 생성할 수 있는 제어장치를 제공하는 것에 있다.

본 발명의 다리식 이동 로봇의 제어장치의 제 1 발명은,

상체로부터 연장하여 설치된 복수의 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 보용을 생성하고, 그 목표 보용에 추종하도록 로봇의 동작을 제어하는 제어장치에 있어서,

상기 목표 보용에 추종하여 동작하고 있는 상기 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

상기 로봇에 작용시키는 병진 상반력의 수평성분 또는 이 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 수평성분 또는 이 전체 중심 가속도의 바닥면 평행성분을 제한대상량으로 하여, 이 제한대상량의 허용범위를 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정하는 허용범위 설정수단과,

소정의 동역학 모델상에서 상기 로봇에 작용하는 중력과 관성력의 합력이 소정의 동역학적 평형조건을 만족시키도록 상기 목표 보용의 가(假)운동을 결정하는 가운동 결정수단과,

상기 목표 보용의 가운동에 의해 정해지는 상기 제한대상량이 상기 허용범위를 일탈했을 때, 상기 동역학적 평형조건을 만족시키면서, 이 제한대상량을 상기 허용범위내로 제한하도록 로봇의 중심 주위의 각운동량 변화율을 상기 가운동으로부터 변화시킴으로써 이 가운동을 수정하여 목표 보용의 운동을 결정하는 가운동 수정수단을 구비한 것을 특징으로 한다.

이 제 1 발명에 의하면, 목표 보용의 가운동에 의해 정해지는 상기 제한대상량(병진 상반력의 수평성분 또는 이 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 수평성분 또는 이 전체 중심 가속도의 바닥면 평행성분)이 상기 허용범위를 일탈했을 때, 이 가운동을 수정하여 목표 보용의 운동을 결정한다. 이 경우, 로봇의 중심 주위의 각운동량 변화율을 상기 가운동으로부터 변화시키므로, 동역학적 평형조건(예를 들면 목표 ZMP 주위의 상반력 모멘트 수평성분이 0이 되는 등의 조건)을 만족시키면서, 제한대상량을 허용범위내로 제한할 수 있다. 또한, 변화시키는 각운동량 변화율은 기본적으로는 롤링 방향, 또는 피칭 방향의 각운동량 변화율이면 된다. 그리고, 이 경우, 상기 허용범위는 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정되므로, 실제 로봇의 미끄러짐의 발생의 유무에 따라 즉응적으로 목표 운동을 수정할 수 있다. 이 결과, 실제 로봇의 미끄러짐이 발생해도, 즉시 그 미끄러짐을 억제하도록 로봇의 운동이 수정된다. 따라서, 제 1 발명에 의하면, 실제 로봇의 미끄러짐의 발생을 억제할 수 있고, 로봇의 자세의 안정성을 유지할 수 있다. 또한, 미끄러짐의 발생이 있다고 판단되었을 때에는, 상기 허용범위는 그것을 좁히도록 설정되어야 하고, 이것은 이하에 설명하는 제 2∼제 4 발명에서도 동일하다.

다음에 본 발명의 다리식 이동 로봇의 제 2 발명은,

상체로부터 연장하여 설치된 복수의 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 보용을 생성하고, 그 목표 보용에 추종하도록 로봇의 동작을 제어하는 제어장치에 있어서,

상기 목표 보용에 추종하여 동작하고 있는 상기 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

상기 로봇에 작용시키는 상반력 모멘트의 연직성분 또는 이 상반력 모멘트의 바닥면 법선방향 성분 또는 이 로봇의 각운동량 변화율의 연직성분 또는 이 각운동량 변화율의 바닥면 법선방향 성분을 제한대상량으로 하여, 이 제한대상량의 허용범위를 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정하는 허용범위 설정수단과,

소정의 동역학 모델상에서 상기 로봇에 작용하는 중력과 관성력의 합력이 소정의 동역학적 평형조건을 만족시키도록 상기 목표 보용의 가운동을 결정하는 가운동 결정수단과,

상기 목표 보용의 가운동에 의해 정해지는 상기 제한대상량이 상기 허용범위를 일탈했을 때, 상기 동역학적 평형조건을 만족시키면서, 이 제한대상량을 상기 허용범위내로 제한하도록 로봇의 각운동량 변화율을 상기 가운동으로부터 변화시킴으로써 이 가운동을 수정하여 목표 보용의 운동을 결정하는 가운동 수정수단을 구비한 것을 특지이으로 한다.

이 제 2 발명에 의하면, 목표 보용의 가운동에 의해 정해지는 상기 제한대상량(상반력 모멘트의 연직성분 또는 이 상반력 모멘트의 바닥면 법선방향 성분 또는 이 로봇의 각운동량 변화율의 연직성분 또는 이 각운동량 변화율의 바닥면 법선방향 성분)이 상기 허용범위를 일탈했을 때, 이 가운동을 수정하여 목표 보용의 운동을 결정한다. 이 경우, 로봇의 각운동량 변화율을 상기 가운동으로부터 변화시키므로, 동역학적 평형조건(예를 들면 목표 ZMP 주위의 상반력 모멘트 수평성분이 0이 되는 등의 조건)을 만족시키면서, 제한대상량을 허용범위내로 제한할 수 있다. 또한, 변화시키는 각운동량 변화율은 기본적으로는 요잉 방향의 각운동량 변화율이면 된다. 그리고, 이 경우, 상기 허용범위는 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정되므로, 실제의 로봇의 미끄러짐의 발생의 유무에 따라 즉응적으로 목표 운동을 수정할 수 있다. 이 결과, 실제의 로봇의 미끄러짐이 발생해도, 즉시 그 미끄러짐(특히 스핀)을 억제하도록 로봇의 운동이 수정된다. 따라서, 제 1 발명에 의하면, 실제의 로봇의 미끄러짐의 발생을 억제할 수 있고, 로봇의 자세의 안정성을 유지할 수 있다.

또한, 제 1 발명과 제 2 발명은 병용해도 좋다. 이 경우에는, 병진 상반력의 수평성분 또는 이 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 수평성분 또는 이 전체 중심가속도의 바닥면 평행성분을 1개의 제한대상량으로 하는 동시에, 상반력 모멘트의 연직성분 또는 이 상반력 모멘트의 바닥면 법선방향 성분 또는 이 로봇의 각운동량 변화율의 연직성분 또는 이 각운동량 변화율의 바닥면 법선방향 성분을 다른 1개의 제한대상량으로 하고, 이들의 허용범위를 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정하면 된다. 이 경우에는, 어느 한쪽의 제한대상량이 허용범위를 일탈했을 때에, 목표 운동의 가운동을 수정하는 것이 바람직하다.

다음에, 본 발명에 다리식 이동 로봇의 제어장치의 제 3 발명은,

상체로부터 연장하여 설치된 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 운동의 순시값을 적어도 이 로봇의 운동과 상반력의 관계를 나타내는 동역학 모델을 사용하여 축차 결정하면서, 그 결정한 목표 운동의 순시값에 추종시키도록 상기 로봇의 동작을 제어하는 다리식 이동 로봇의 제어장치에 있어서,

상기 목표 운동에 추종하여 동작하고 있는 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

적어도 상기 로봇에 작용시키는 병진 상반력의 수평성분 또는 이 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 수평성분 또는 이 전체 중심가속도의 바닥면 평행성분을 제한대상량으로 하여, 이 제한대상량의 허용범위를 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정하는 허용범위 설정수단과,

적어도 상기 결정된 목표 운동의 순시값에 대응하는 로봇의 자세의 목표 상태량과 이 로봇의 자세의 실제 상태량의 편차에 기초하여 상기 목표 운동의 새로운 순시값을, 이 새로운 순시값에 대응해서 상기 동역학 모델에 기초하여 정해지는 상기 제한대상량이 상기 허용범위내에 들어가는 동시에 상기 편차가 0에 근접하도록 결정하는 목표 순시값 결정수단을 구비한 것을 특징으로 한다.

이 제 3 발명에 의하면, 적어도 로봇의 자세의 상태량에 관한 상기 편차 에 기초하여, 상기 목표 운동의 새로운 순시값을, 이 새로운 순시값에 대응해서 상기 동역학 모델에 기초하여 정해지는 상기 제한대상량(병진 상반력의 수평성분 또는 이 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 수평성분 또는 이 전체 중심가속도의 바닥면 평행성분)이 상기 허용범위내에 들어가는 동시에 상기 편차가 0에 근접하도록 결정한다. 즉, 로봇의 자세를 원하는 자세로 유지하도록 하면서, 제한대상량이 허용범위를 일탈하지 않도록(이하, 이 조건을 여기에서의 설명에서는 자세/제한대상량 조건이라고 하는 경우가 있음) 목표 운동의 순시값이 결정된다. 그리고, 이 경우, 상기 허용범위는 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정되므로, 실제의 로봇의 미끄러짐의 발생의 유무에 따라 즉응적으로, 상기 자세/제한대상량 조건을 만족시키도록 목표 운동의 순시값을 결정할 수 있다. 이 결과, 실제의 로봇의 미끄러짐이 발생해도, 이 로봇의 자세를 적절한 자세로 안정하게 유지하면서, 즉시 그 미끄러짐을 억제할 수 있다.

또, 본 발명의 다리식 이동 로봇의 제어장치의 제 4 발명은,

상체로부터 연장하여 설치된 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 운동 및 목표 상반력의 순시값을 적어도 이 로봇의 운동과 상반력의 관계를 나타내는 동역학 모델을 사용하여 축차 결정하면서, 그 결정한 목표 운동 및 목표 상반력의 순시값에 추종시키도록 상기 로봇의 동작을 제어하는 다리식 이동 로봇의 제어장치에 있어서,

상기 목표 운동 및 목표 상반력에 추종하여 동작하고 있는 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

적어도 상기 로봇에 작용시키는 병진 상반력의 수평성분 또는 이 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 수평성분 또는 이 전체 중심가속도의 바닥면 평행성분을 제한대상량으로 하여, 이 제한대상량의 허용범위를 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정하는 허용범위 설정수단과,

적어도 상기 결정된 목표 운동 및 목표 상반력의 순시값에 대응하는 로봇의 자세의 목표 상태량과 이 로봇의 자세의 실제 상태량과의 편차에 기초하여 상기 목표 운동 및 목표 상반력의 새로운 순시값을, 이 새로운 목표 운동의 순시값에 대응해서 상기 동역학 모델에 기초하여 정해지는 상기 제한대상량이 상기 허용범위내에 들어가는 동시에 상기 편차가 0에 근접하도록 결정하는 목표 순시값 결정수단을 구비한 것을 특징으로 한다.

이 제 4 발명에 의하면, 적어도 로봇의 자세의 상태량에 관한 상기 편차 에 기초하여, 상기 목표 운동 및 목표 상반력의 새로운 순시값을 이 새로운 목표 운동의 순시값에 대응하여 상기 동역학 모델에 기초하여 정해지는 상기 제한대상량(병진 상반력의 수평성분 또는 이 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 수평성분 또는 이 전체 중심 가속도의 바닥면 평행성분)이 상기 허용범위내에 들어가는 동시에 상기 편차가 0에 근접하도록 결정한다. 즉, 로봇의 자세를 원하는 자세로 유지하도록 하면서, 제한대상량이 허용범위를 일탈하지 않도록(이하, 이 조건을, 제 3 발명과 동일하게, 자세/제한대상량 조건이라고 하는 경우가 있음) 목표 운동 및 목표 상반력의 순시값이 결정된다. 그리고, 이 경우, 상기 허용범위는 상기 미끄러짐 판단 수단의 판단 결과에 따라 가변적으로 설정되므로, 상기 제 3 발명과 동일하게, 실제의 로봇의 미끄러짐의 발생의 유무에 따라 즉응적으로, 상기 자세/제한대상량 조건을 만족시키도록 목표 운동 및 목표 상반력의 순시값을 결정할 수 있다. 이 결과, 실제의 로봇의 미끄러짐이 발생해도, 이 로봇의 자세를 적절한 자세로 안정하게 유지하면서, 즉시 그 미끄러짐을 억제할 수 있다. 또, 이 경우, 목표 운동과 목표 상반력의 양자가 상기 자세/제한대상량 조건에 의해 규제되므로, 효과적으로 미끄러짐을 억제할 수 있다.

상기 제 1∼제 4 발명에서는, 예를 들면 다음과 같이 하여, 미끄러짐의 발생을 판단할 수 있다.

즉, 상기 미끄러짐 판단 수단은 적어도 접지하고 있는 다리체의 선단부의 대지속도에 기초하여 미끄러짐의 발생을 판단한다(제 5 발명).

이 경우, 예를 들면 이 대지속도의 절대값이 소정값보다도 클 때에는, 미끄러짐의 발생이 있다고 판단할 수 있다.

또는, 상기 미끄러짐 판단 수단은, 적어도 접지하고 있는 다리체에 작용하는 실제 상반력의 시간적 변화율과 이 다리체의 선단부의 대지속도에 기초하여, 이 다리체의 겉보기 스프링정수를 구하는 수단을 구비하고, 적어도 이 겉보기 스프링정수에 기초하여 미끄러짐의 발생을 판단한다(제 6 발명). 이 경우, 예를 들면 이 겉보기 스프링정수가 소정값보다도 작을 때에는, 미끄러짐의 발생이 있다고 판단할 수 있다.

또는, 상기 미끄러짐 판단 수단은, 적어도 접지하고 있는 다리체에 작용하는 실제 상반력을 소정 주파수의 근방 범위에 주파수 통과특성을 갖는 밴드패스 필터에 통과시킨 것에 기초하여 미끄러짐의 발생을 판단한다(제 7 발명). 이 경우, 상기 실제 상반력을 밴드패스 필터에 통과시킨 것은, 소위 미끄러짐 진동이 발생하고 있을 때의, 실제 상반력의 진동성분에 상당하는 것이 된다. 그리고, 예를 들면 이 진동성분의 크기(절대값)가 소정값보다도 클 때에는, 미끄러짐 진동의 발생이 있다고 판단할 수 있다.

또한, 미끄러짐의 발생의 판단은 제 5∼제 7 발명의 어떤 것도 행할 수 있지만, 이들 제 5∼제 7 발명중 2개 이상을 조합시켜서 미끄러짐의 발생의 판단을 행하도록 해도 좋다.

도 1은 본 발명의 실시형태 및 이것에 관련되는 참고예에 있어서의 다리식 이동 로봇으로서의 2족이동 로봇의 전체 구성을 개략적으로 도시하는 도면,

도 2는 도 1의 로봇의 다리체의 선단부의 구조를 도시하는 도면,

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

도 4는 제 1 참고예에 있어서의 제어 유닛의 기능적 구성을 도시하는 블럭도,

도 5는 실시형태 및 참고예에서 생성하는 주행 보용을 설명하기 위한 도면,

도 6은 목표 상반력 연직성분 궤도의 예를 도시하는 그래프,

도 7은 목표 ZMP 궤도의 X성분 및 Y성분의 예를 도시하는 그래프,

도 8은 로봇의 상체 병진 모드를 설명하기 위한 도면,

도 9는 로봇의 상체 경사 모드를 설명하기 위한 도면,

도 10은 로봇의 상체 요잉 회전 모드를 설명하기 위한 도면,

도 11a는 로봇의 역위상 팔 흔들기 모드를 평면시로 설명하기 위한 도면,

도 11b는 로봇의 역위상 팔 흔들기 모드를 측면시로 설명하기 위한 도면,

도 12는 실시형태에서 사용하는 동역학 모델을 설명하기 위한 도면이다.

도 13은 제 1 참고예에서의 보용생성 장치의 메인루틴 처리를 도시하는 플로우차트,

도 14는 로봇의 발산 상태를 설명하기 위한 도면,

도 15는 도 13의 S022의 서브루틴 처리를 도시하는 플로우차트,

도 16은 정상 보용과 지지다리 좌표계를 설명하기 위한 도면,

도 17은 정상 보용의 상체궤도와 지지다리 좌표계를 예시하는 도면,

도 18은 기준 역위상 팔 흔들기 각의 예를 도시하는 그래프,

도 19는 정상 보용에 있어서의 목표 상반력 연직성분 궤도의 설정예를 도시하는 그래프,

도 20은 정상 보용에 있어서의 상반력 수평성분 허용범위의 설정예를 도시하는 그래프,

도 21은 정상 보용에 있어서의 상반력 모멘트 연직성분 허용범위의 설정예를 도시하는 그래프,

도 22는 정상 보용에 있어서의 목표 ZMP 궤도의 설정예를 도시하는 그래프이다.

도 23은 도 13의 S024의 서브루틴 처리를 도시하는 플로우차트,

도 24는 도 23의 S208의 서브루틴 처리를 도시하는 플로우차트,

도 25는 도 24의 S306의 서브루틴 처리를 도시하는 플로우차트,

도 26은 도 25의 S412의 서브루틴 처리를 도시하는 플로우차트,

도 27은 허용범위를 고려하지 않은 상반력 수평성분의 예를 도시하는 그래프,

도 28은 허용범위를 고려한 상반력 수평성분의 예를 도시하는 그래프,

도 29는 상체 경사 각가속도의 예를 도시하는 그래프,

도 30은 로봇의 상체 경사각을 복원하기 위한 상체 경사 복원 모멘트 ZMP 환산값의 예를 도시하는 그래프,

도 31은 상체 경사 복원 모멘트 ZMP 환산값을 가미한 상체 경사각 가속도의 예를 도시하는 그래프,

도 32는 허용범위를 고려하지 않은 상반력 모멘트 연직성분의 예를 도시하는 그래프,

도 33은 허용범위를 고려한 상반력 모멘트 연직성분의 예를 도시하는 그래프,

도 34는 역위상 팔 흔들기 모멘트의 예를 도시하는 그래프,

도 35는 도 34의 역위상 팔 흔들기 모멘트에 대응하는 역위상 팔 흔들기 각가속도를 도시하는 그래프,

도 36은 역위상 팔 흔들기 각을 복원하기 위한 역위상 팔 흔들기 복원 각가속도의 예를 도시하는 그래프,

도 37은 도 36의 역위상 팔 흔들기 복원 각가속도에 대응하는 역위상 팔 흔들기 복원 모멘트를 도시하는 그래프,

도 38은 도 33의 상반력 모멘트 연직성분과 도 37의 역위상 팔 흔들기 복원 모멘트를 합성하여 이루어지는 상반력 모멘트 연직성분을 도시하는 그래프,

도 39는 도 13의 S026의 서브루틴 처리를 도시하는 플로우차트,

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

도 41은 금회 보용의 상반력 모멘트 연직성분 허용범위의 설정예를 도시하는 그래프,

도 42는 도 13의 S028의 서브루틴 처리를 도시하는 플로우차트,

도 43은 도 42의 S702의 서브루틴 처리를 도시하는 플로우차트,

도 44는 금회 보용의 가목표 ZMP와 ZMP 수정량과 수정후의 목표 ZMP의 예를 도시하는 그래프,

도 45는 도 13의 S030의 서브루틴 처리를 도시하는 플로우차트,

도 46은 도 45의 S1412의 서브루틴 처리를 도시하는 플로우차트,

도 47은 정상 보용과 목표 보용의 상체위치 궤도와의 관계를 도시하는 그래프,

도 48은 상체 경사 모드(허리중심의 상체 경사)의 다른 예를 도시하는 도면,

도 49는 동역학 모델의 다른 예를 설명하기 위한 도면,

도 50은 보행 보용에 있어서의 목표 상반력 연직성분의 설정예를 도시하는 도면,

도 51은 로봇의 주행 보용에 있어서의 상체 연직방향 위치와 상반력 연직성분의 관계를 도시하는 도면,

도 52는 로봇의 보행 보용에 있어서의 상체 연직방향 위치와 상반력 연직성분의 관계를 도시하는 도면,

도 53은 제 2 참고예에 있어서의 제어유닛의 기능적 구성을 도시하는 블럭도,

도 54는 도 53에 도시하는 보상 전체 상반력 모멘트 수평성분 분배기의 처리를 도시하는 블럭도,

도 55는 도 53에 도시하는 모델 조작 상반력 모멘트 연직성분 결정기의 처리를 도시하는 블럭도,

도 56은 제 2 참고예에 있어서의 보용생성 장치의 메인루틴 처리를 도시하는 플로우차트,

도 57은 도 56의 S3032의 서브루틴 처리를 도시하는 플로우차트,

도 58은 도 57의 S3414의 서브루틴 처리를 도시하는 플로우차트,

도 59는 제 3 참고예에 있어서의 제어 유닛의 기능적 구성을 도시하는 블럭도,

도 60은 제 2 실시형태에 있어서의 보용생성 장치의 메인루틴 처리를 도시하는 플로우차트,

도 61은 도 60의 S2034의 서브루틴 처리를 도시하는 플로우차트,

도 62는 도 61의 S2114의 서브루틴 처리를 도시하는 플로우차트,

도 63은 본 발명의 제 4 참고예에 있어서의 보용생성 장치의 기능적 구성을 도시하는 블럭도,

도 64는 제 4 참고예에 있어서의 ZMP 허용범위의 설정예를 도시하는 도면,

도 65는 제 4 참고예에 있어서의 보용생성 장치의 메인루틴 처리를 도시하는 플로우차트,

도 66은 도 65의 S3536의 처리를 도시하는 블럭도,

도 67은 도 66에 도시하는 상체 수평위치 섭동용 모델을 설명하기 위한 도면,

도 68은 도 66에 도시하는 상체자세각 보정용 섭동 모델을 설명하기 위한 도면,

도 69는 도 66에 도시하는 역위상 팔 흔들기 각 보정용 섭동 모델을 설명하기 위한 도면,

도 70은 도 66에 도시하는 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부의 처리를 도시하는 블럭도,

도 71은 제 5 참고예에 있어서의 도 65의 S3536의 처리를 도시하는 블럭도,

도 72는 도 71에 도시하는 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부의 처리를 도시하는 블럭도,

도 73은 제 6 참고예에 있어서의 보용생성 장치의 기능적 구성을 도시하는 블럭도,

도 74는 도 73에 도시하는 의사 풀 모델의 처리를 도시하는 블럭도,

도 75는 제 7 참고예에 있어서의 도 65의 S3536의 처리를 도시하는 블럭도,

도 76은 본 발명의 제 1 실시형태에 있어서의 제어 유닛의 기능적 구성을 도시하는 블럭도,

도 77은 제 1 실시형태에 있어서의 보용생성 장치의 메인루틴 처리를 도시하는 플로우차트,

도 78은 도 77의 S2334의 처리를 도시하는 플로우차트,

도 79는 도 76에 도시하는 미끄러짐 판정부의 처리를 도시하는 플로우차트,

도 80∼도 82는 각각 도 79의 S5210, S5212, S5214의 서브루틴 처리를 도시하는 플로우차트,

도 83은 미끄러짐 판정부의 판정 결과와, 허용범위의 저감율과, 상반력 허용 범위의 예를 도시하는 그래프,

도 84는 제 5∼제 7 참고예에 관한 변형 태양의 예를 도시하는 블럭도,

도 85는 제 4 참고예에 관한 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부의 처리의 변형 태양의 예를 도시하는 블럭도.

이하, 첨부 도면을 참조하여 본 발명의 실시형태에 따른 다리식 이동 로봇의 제어장치를 설명한다. 또한, 다리식 이동 로봇으로서는 2족이동 로봇을 예로 든다.

먼저, 도 1∼도 47을 참조하여, 본 발명의 다리식 이동 로봇의 제어장치에 관련되는 제 1 참고예를 설명해 둔다. 또한, 제 53∼도 58을 참조하여 제 2 참고예를, 도 59∼도 62를 참조하여 제 3 참고예를, 도 63∼도 70을 참조하여 제 4 참고예를, 도 71 및 도 72를 참조하여 제 5 참고예를, 도 73 및 도 74를 참조하여 제 6 참고예를, 도 75를 참조하여 제 7 참고예를 설명해 둔다. 후술하는 본 발명의 실시형태는, 그 기계적 구성은 제 1 참고예와 동일하고, 또, 로봇의 보용생성처리 및 제어처리의 일부(구체적으로는 허용범위 설정처리)만이 제 1∼제 7 참고예중 어느 하나와 상이한 것이다. 따라서, 후술하는 실시형태의 설명에서는, 이 제 1∼제 7 참고예의 설명을 자주 원용한다. 보충하면, 도 1과 후술의 도 2, 도 3 및 도 5∼도 12를 참조하여 설명하는 사항은 후술의 실시형태에서도 동일하다.

먼저, 제 1 참고예를 설명한다. 도 1은, 이 제 1 참고예에 따른 다리식 이동 로봇을 전체적으로 도시하는 개략도이다.

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

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

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

상체(3)의 상부의 양측부에는 좌우 한쌍의 팔체(5, 5)가 부착되는 동시에, 상체(3)의 상단부에는 머리부(4)가 배치된다. 각 팔체(5)는, 3개의 관절30R(L), 32R(L), 34R(L)로 구성된 어깨 관절과, 관절 36R(L)로 구성된 팔꿈치관절과, 관절 38R(L)로 구성된 손목관절과, 이 손목관절에 연결된 손끝부(40)(L)를 구비하고 있다. 또한, 머리부(4)는 본 발명의 요지와 직접적인 관련을 갖지 않기 때문에 상세한 설명을 생략한다.

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

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

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

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

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

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

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

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

이하 설명하면, 제어 유닛(60)은 로봇(1)의 목표 보용을 자유자재로 또한 리얼타임으로 생성하여 출력하는 보용생성 장치(100)를 구비하고 있다. 이 보용생성 장치(100)는, 그 기능에 의해, 본원발명의 각 수단을 구성하는 것이다. 이 보용생성 장치(100)가 출력하는 목표 보용은, 목표 상체위치 자세 궤도(상체(3)의 목표 위치 및 목표 자세의 궤도), 목표 족평위치 자세 궤도(각 족평(22)의 목표 위치 및 목표 자세의 궤도), 목표 팔자세 궤도(각 팔체(5)의 목표 자세의 궤도), 목표 전체 상반력 중심점(목표 ZMP) 궤도, 목표 전체 상반력 궤도로 구성된다. 또한, 다리체(2)나 팔체(5) 이외에 상체(3)에 대해 가동 부위를 갖는 경우에는, 그 가동 부위의 목표 위치 자세 궤도가 목표 보용에 부가된다.

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

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

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

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

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

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

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

c) 일련의 보용은 몇개의 보용이 연결된 것으로 한다.

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

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

본 명세서에서는 이후, 이해를 쉽게 하기 위해서, 특별히 예고하지 않는 한, 목표 보용은 상기 b')의 협의의 목표 보용의 의미로 사용한다. 이 경우, 목표 보용의 「1보」는 로봇(1)의 한쪽의 다리체(2)가 착지하고 나서 또 한쪽의 다리체(2)가 착지할 때까지의 의미로 사용한다. 보충하면, 본 명세서의 참고예 및 실시형태에서는 목표 보용은 1보의 기간분의 보용의 의미로 사용하지만, 그것은 반드시 1보의 기간분의 것일 필요는 없고, 복수보분, 또는 1보보다도 짧은(예를 들면 반보 등) 기간분의 보용이어도 좋다. 또한, 이하의 설명에서는, 「상반력 연직성분」은 「병진 상반력 연직성분」을 의미하는 것으로 하고, 상반력중 모멘트의 연직성분( 연직축 주위 성분)은 「모멘트」라는 용어를 사용하여 「상반력 연직성분」과 구별을 한다. 마찬가지로, 「상반력 수평성분」은 「병진 상반력 수평성분」을 의미하는 것으로 한다.

또, 보용에 있어서의 양다리 지지기란 말할 필요도 없이, 로봇(1)이 그 자중을 양 다리체(2, 2)로 지지하는 기간, 한쪽다리 지지기란 어느 한쪽만의 다리체(2)로 로봇(1)의 자중을 지지하는 기간, 공중기란 양 다리체(2, 2)가 바닥으로부터 떨어져 있는(공중에 떠 있음) 기간을 말한다. 한쪽다리 지지기에 있어서 로봇(1)의 자중을 지지하지 않은 측의 다리체(2)를 「미착지 다리」라고 부른다. 또한, 본 제 1 참고예에서 설명하는 주행 보용에서는, 양다리 지지기는 없고, 한쪽다리 지지기(착지기)와 공중기가 번갈아 반복된다. 이 경우, 공중기에서는 양 다리체(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)의 착지 위치 자세(착지 예정위치 자세)나 착지 시각(착지 예정시각) 등의 목표 보용생성용의 기본적인 요구값(요구 패러미터)이, 상기 조이스틱(73)의 소요의 조작 등에 따라 보용생성 장치(100)에 주어진다. 그리고, 보용생성 장치(100)는 그 요구 패러미터를 사용하여 목표 보용을 생성한다. 보다 상세하게 말하면, 보용생성 장치(100)는, 상기 요구 패러미터에 따라, 목표 보용의 목표 족평위치 자세 궤도, 목표 상반력 연직성분 궤도 등, 목표 보용의 일부의 구성요소를 규정하는 패러미터(보용 패러미터라고 함)를 결정한 다음, 그 보용 패러미터를 사용하여 목표 보용의 순시값을 축차 결정하고, 이 목표 보용의 시계열 패턴을 생성한다.

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

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

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

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

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

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

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

목표 상반력 연직성분 궤도는, 예를 들면 도 6과 같이 설정된다. 본 제 1 참고예에서는, 주행 보용에 있어서의 목표 상반력 연직성분 궤도의 형상(상세하게는 한쪽다리 지지기에서의 형상)은, 사다리꼴 형상(상반력 연직성분의 증가측으로 볼록한 형상)으로 정해져 있고, 그 사다리꼴의 높이, 꺽임점의 시각을 목표 상반력 연직성분 궤도를 규정하는 보용 패러미터로 하여, 그것들의 보용 패러미터(상반력 연직성분 궤도 패러미터)가 결정된다. 또한, 주행 보용의 공중기에서는, 목표 상반력 연직성분은 정상적으로 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 궤도를 설정해도 상관없다. 그러나 본 제 1 참고예에서는, 도 7의 상단 도면에 도시하는 바와 같이, 공중기에 있어서의 목표 ZMP 궤도의 X축 방향 위치는, 다음 미착지 다리측 다리체(2)의 착지까지 지지다리측 족평(22)의 발끝에서부터 미착지 다리측 족평(22)의 발뒤꿈치의 착지 위치 까지 연속적으로 이동하도록 했다. 또, 도 7의 하단 도면에 도시하는 바와 같이, 공중기에 있어서의 목표 ZMP 궤도의 Y축 방향 위치는, 다음 미착지 다리측 다리체(2)의 착지까지 지지다리측 다리체(2)의 발목관절의 중심의 Y축 방향 위치로부터 미착지 다리측 다리체(2)의 발목관절의 중심의 Y축 방향 위치까지 연속적으로 이동하도록 했다. 즉, 보용의 전체 기간에 있어서 목표 ZMP 궤도를 연속(실질적으로 연속)으로 했다. 그리고, 후술하는 바와 같이, 목표 ZMP 주위의 중력과 관성력의 합력의 모멘트(연직성분을 제외함)이 0이 되도록 목표 보용을 생성하도록(보다 구체적으로는 목표 상체위치 자세 궤도를 조정하도록) 했다. 모델의 근사 오차를 고려하면, 생성된 보용이 순조롭게 되도록 하기 위해서, 공중기에서도 목표 ZMP 궤도를 연속(실질적으로 연속)으로 해 두는 것이 바람직하다. 단, 본 제 1 참고예에서 사용하는 후술의 동역학 모델에서는, 목표 ZMP의 위치에 의하지 않고, 목표 ZMP 주위의 모멘트의 수평성분을 어떤 값(그 값은 본 제 1 참고예에서는 0이지만, 후술의 실시형태에서는 반드시 0인 것은 아님)으로 하는 목표 보용을 일의적으로 생성할 수 있으므로, 반드시 목표 ZMP를 연속으로 해 둘 필요는 없다.

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

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

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

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

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

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

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

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

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

바꿔 말하면, 상반력 연직성분을 변화시키지 않고, 목표 ZMP 주위의 상반력 모멘트 수평성분과 상반력 수평성분을 변화시키는 운동을 상체 병진 모드라고 부른다. 또한, 상체 병진 모드에서는, 상반력 모멘트 연직성분(연직축 주위 성분)도 섭동하지만, 여기에서는 이 점에 주목하지 않는다.

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

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

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

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

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

또, 도 10과 같이, 로봇(1)의 어떤 운동상태로부터, 어떤 점 Pq주위에 상체 요잉 각가속도(상체(3)의 연직축 주위의 회전 각가속도)를 섭동시키면, 로봇(1)의 전체 중심은 섭동하지 않고, 전체 중심 주위의 각운동량 연직성분이 섭동한다. 그런데, 로봇(1)의 전체 중심이 섭동하지 않으면, 각운동량 연직성분의 섭동은, 작용점에 의존하지 않는다. 따라서, 점 Pq 주위의 상체 요잉 각가속도의 섭동은, 상반력 연직성분, 상반력 수평성분 및 상반력 모멘트 수평성분을 섭동시키지 않고, 목표 ZMP 주위의 상반력 모멘트 연직성분을 섭동시킨다. 이와 같이 로봇(1)의 상체 요잉 각가속도를 섭동시키는 운동 모드를 상체 요잉 회전 모드라고 부른다.

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

이때의 단위 각가속도당의 상반력 모멘트 연직성분의 변화를 △Mbz, 단위 각가속도당의 상반력 수평성분의 변화를 △Fb라고 한다. △Fb는 0이다. 도 10에 도시하는 상황에서 상체(3)를 화살표 방향으로 단위 각가속도로 회전시키면(각가속도 βb=1으로 회전시키면), △Mbz는 도 10에 도시하는 화살표 방향으로 작용한다.

또한, 도 10에 도시하는 운동에서는, 양 팔체(5, 5)의 손끝위치가 지지다리 좌표계(바닥에 고정된 좌표계)로 보아서 바뀌지 않도록, 상체(3)를 회전시켰지만, 상체(3)에 대한 팔체(5)의 상대위치 자세를 바꾸지 않고, 상체(3)와 함께 팔체(5)를 회전시키는 운동을 상체 요잉 회전 모드로 정의해도 좋다. 단, 이 경우에는, 후술하는 운동방정식에 약간 변경이 필요하다.

또, 도 11(a), (b)와 같이, 로봇(1)의 어떤 운동상태로부터, 양 팔체(5, 5)의 선단을 서로 전후로 역방향으로 섭동시키면, 로봇(1)의 전체 중심은 섭동하지 않고, 전체 중심 주위의 각운동량 연직성분이 섭동한다. 이후, 이 운동 모드를 역위상 팔 흔들기 모드라고 부른다. 바꿔 말하면, 상반력 연직성분, 상반력 수평성분 및 상반력 모멘트 수평성분을 섭동시키지 않고, 목표 ZMP 주위의 상반력 모멘트 연직성분을 섭동시키는 팔 흔들기 운동 모드를 역위상 팔 흔들기 모드라고 부른다.

우측 팔체(5R)를 전방측으로 단위량 이동시키고, 좌측 팔체(5L)를 후방측으로 단위량 이동시키는 동작을 단위 각도의 역위상 팔 흔들기라고 부른다. 도 11(a), (b)는 역위상 팔 흔들기 각도가 θaz의 상태를 나타낸다.

역위상 팔 흔들기 모드의 단위 각가속도당의 상반력 모멘트 연직성분의 변화를 △Maz, 단위 각가속도당의 상반력 수평성분의 변화를 △Fa로 한다. △Fa는 0이다. 도 11(a), (b)에 도시하는 상황에서 우측 팔체(5R)를 전방측으로 가속하고, 좌측 팔체(5L)를 후방측으로 가속하면(각가속도 βa>0으로 흔들면), 도 11(a)에 도시하는 화살표 방향(연직축의 정의 방향)으로 상반력 모멘트 연직성분 Maz가 작용한다.

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

도 12는 본 제 1 참고예에 사용되는 로봇(1)의 동역학 모델이다. 도시와 같이, 이 동역학 모델은, 로봇(1)의 각 다리체(2)에 각각 대응하는 2개의 질점(質點) 2m, 2m, 및 상체(3)에 대응하는 질점 3m으로 이루어지는 합계 3질점과, 이너셔가 있고 질량이 없는 4개의 플라이휠 FHx, FHy, FHbz, FHaz로 구성되는 모델이다. 플라이휠 FHx, FHy, FHbz, FHaz는 각각 X축(전후방향축), Y축(좌우방향축), Z축(연직축), Z축(연직축)의 주위로 회전가능한 것이다. 이 동역학 모델에서는, 디커플드, 즉 다리체(2, 2)의 동역학(각 질점 2m, 2m의 동역학)과 상체(3)의 동역학(질점 3m 및 플라이휠 FHx, FHy 및 FHbz의 동역학)과 팔체(5, 5)의 동역학(플라이휠 FHaz의 동역학)이 서로 비간섭으로 구성되는 동시에, 로봇(1) 전체의 동역학은 그것들의 선형 결합으로 표시된다. 또, 더욱이, 상체(3)의 운동과 상반력의 관계도, 상체(3)의 병진 운동(상체 병진 모드)과 상반력의 관계, 상체(3)의 경사운동(상체 경사 모드)과 상반력의 관계, 상체(3)의 요잉 회전운동(상체 요잉 회전 모드)과 상반력의 관계, 및 양 팔체(5, 5)의 역위상 팔 흔들기 운동(역위상 팔 흔들기 모드)과 상반력의 관계로 분리된다. 구체적으로는, 상체 질점 3m의 수평운동에 의해 발생하는 상반력은 상체(3)의 수평방향 병진 운동(상체 병진 모드)에 의해 발생하는 상반력에 대응하고, 플라이휠 FHx 및 FHy의 회전운동에 의해 발생하는 상반력은 상체(3)의 경사각의 회전운동(상체 경사 모드)에 의해 발생하는 상반력에 대응한다. 또한, 플라이휠 FHx의 회전운동은 상체(3)의 경사각의 롤링 방향(X축 주위)의 회전운동에 대응하고, 플라이휠 FHy의 회전운동은 상체(3)의 경사각의 피칭 방향(Y축 주위)의 회전운동에 대응하고 있다. 또, 플라이휠 FHbz의 회전운동에 의해 발생하는 상반력은 상체(3)의 요잉 회전운동(상체 요잉 회전 모드)에 의해 발생하는 상반력에 대응한다. 또, 플라이휠 FHaz의 회전운동에 의해 발생하는 상반력은 역위상 팔 흔들기(역위상 팔 흔들기 모드)에 의해 발생하는 상반력에 대응한다.

또한, 로봇(1)의 팔체의 질량은 상체(3)에 포함되는 것으로 하고, 상체 질점 3m은 팔체(5, 5)의 질량을 포함하는 질량을 갖는다.

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

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

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

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

ZGtotal: 전체 중심 연직위치

Xsup: 지지다리 질점 X위치

Ysup: 지지다리 질점 Y위치

Xswg: 미착지 다리 질점 X위치

Yswg: 미착지 다리 질점 Y위치

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

Yb: 상체 질점 Y위치

XGtotal: 전체 중심 수평 X위치

YGtotal: 전체 중심 수평 Y위치

θbx: 연직방향에 대한 X축 주위의 상체 경사각

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

θbz: 상체 요잉 회전각

θaz: 역위상 팔 흔들기 각

mb: 상체 질점 질량

msup: 지지다리 질점 질량

mswg: 미착지 다리 질점 질량

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

J: 상체 관성 모멘트(상체 경사 모드에 있어서의 등가 관성 모멘트이다. 즉, FHx 및 FHy의 관성 모멘트이다. 실제 로봇(1)의 상체(3)부위의 관성 모멘트는 일반적으로는 일치하지 않는다.)

Jbz: 연직축 주위 상체 관성 모멘트(상체 요잉 회전 모드에 있어서의 등가 관성 모멘트이다. 실제 로봇(1)의 상체(3)부위의 관성 모멘트와는 일반적으로는 일치하지 않는다.)

Jaz: 팔 흔들기 연직축 주위 관성 모멘트(스핀 캔슬을 위한 역위상 팔 흔들기에 있어서의 등가 관성 모멘트이다. 즉, FHz의 관성 모멘트이다.)

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

Fy: 상반력 Y성분(상세하게는 병진 상반력의 좌우 방향(Y축) 성분)

Fz: 상반력 연직성분(상세하게는 병진 상반력의 연직방향(Z축) 성분.

본 제 1 참고예에서는, 이것은 목표 병진 상반력 연직성분과 동일함)

Mx: 목표 ZMP 주위의 상반력 모멘트 X성분(상세하게는 상반력 모멘트의 전후축(X축) 주위 성분)

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

Mz: 목표 ZMP 주위의 상반력 모멘트 Z성분(상세하게는 상반력 모멘트의 연직축(Z축) 주위 성분)

또한, 각 질점 2m, 3m의 X위치, Y위치는 각각 전후 방향(X축 방향)의 위치, 좌우 방향(Y축 방향)의 위치를 의미하고 있다. 또, 본 제 1 참고예에서는, 각 다리체(2)의 질점 2m의 위치와, 그 다리체(2)의 족평(22)의 위치(족평(22)의 미리 정한 대표점의 위치)와의 위치관계는 미리 정해져 있어, 일방의 위치가 결정되면, 타방의 위치는 일의적으로 정해지는 것으로 되어 있다. 또, 상체 질점 3m과, 상체(3)의 위치(상체(3)의 미리 정해진 대표점의 위치)와의 위치관계는, 상체(3)의 자세각(이후, 상체에 관해서는, 자세각이란 경사각과 요잉각을 의미하는 것으로 한다.)에 따라 미리 정해져 있어, 일방의 위치와 자세각이 정해지면, 타방의 위치는 일의적으로 정해지는 것으로 되어 있다.

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

상기 동역학 모델의 운동방정식(동역학적 평형조건을 나타내는 식)은, 식01, 식02x, 식02y, 식03x, 식03y, 식03z로 표시된다.

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

+mswg*(g+d2Zswg/dt2) ……식01

Fx=mb*d2Xb/dt2+msup*d2Xsup/dt2+mswg*d2Xswg/dt2 ……식02x

Fy=mb*d2Yb/dt2+msup*d2Ysup/dt2+mswg*d2Yswg/dt2 ……식02y

Mx=mb*(Yb-Yzmp)*(g+d2Zb/dt2)

-mb*(Zb-Zzmp)*(d2Yb/dt2)

+msup*(Ysup-Yzmp)*(g+d2Zsup/dt2)

-msup*(Zsup-Zzmp)*(d2Ysup/dt2)

+mswg*(Yswg-Yzmp)*(g+d2Zswg/dt2)

-mswg*(Zswg-Zzmp)*(d2Yswg/dt2)+J*d2θbx/dt2

……식03x

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

……식03y

Mz=mb*(Xb-Xzmp)*(d2Yb/dt2)-mb*(Yb-Yzmp)*(d2Xb/dt2)

+msup*(Xsup-Xzmp)*(d2Ysup/dt2)

-msup*(Ysup-Yzmp)*(d2Xsup/dt2)

+mswg*(Xswg-Xzmp)*(d2Yswg/dt2)

-mswg*(Yswg-Yzmp)*(d2Xswg/dt2)

+Jbz*d2θbz/dt2+Jaz*d2θaz/dt2 ……식03z

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

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

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

YGtotal=(mb*Yb+msup*Ysup+mswg*Yswg)/mtotal ……식05y

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

상기 △Fp는 식02x 또는 식02y에 있어서, d2Xb/dt2 또는 d2Yb/dt2를 단위량만큼 섭동시켰을 때의 Fx 또는 Fy의 섭동량이기 때문에, 다음 식에 의해 정해진다.

△Fp=mb ……식06

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

상기 △Mp는, 식03y 또는 식03x에 있어서, d2Xb/dt2 또는 d2Yb/dt2을 단위량만큼 섭동시켰을 때의 My 또는 Mx의 섭동량이기 때문에, 다음 식에 의해 구해진다.

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

즉, 상체 병진 모드의 각 수평축(X축, Y축) 방향의 단위 가속도당의 상반력 모멘트 수평성분의 변화 △Mp는, 상기 동역학 모델의 상체 질점 질량에, 상체 질점 3m의 목표 ZMP로부터의 높이(연직위치)를 곱한 것이 된다. 이때의 상체 질점 3m 및 목표 ZMP의 위치와, 상체 질점 3m의 운동의 관계는, 상체 질점 3m을 도립진자 질점, 목표 ZMP를 도립진자 지점에 대응시켰을 때의 도립진자의 거동에 상당한다. 또한, Y축 방향의 △Mp는, 보다 정확하게는, 식07의 우변 부호를 반전시킨 것이다.

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

△Fr=0 …식08

즉, 상체 경사 모드의 각 수평축(X축, Y축) 방향의 단위 가속도당의 상반력 수평성분의 변화 △Fr은 0이다.

상기 △Mr은 식03x 또는 식03y에 있어서, d2θbx/dt2 또는 d2θby/dt2를 단위량만큼 섭동시켰을 때의 Mx 또는 My의 섭동량이기 때문에, 다음 식에 의해 구해진다.

△Mr=J …식09

즉, 상체 경사 모드의 각 수평축(X축, Y축) 방향의 단위 가속도당의 상반력 모멘트 수평성분의 변화 △Mr은 수평축 플라이휠(FHx 및 FHy)의 관성 모멘트에 상당한다.

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

△Mbz=Jbz …식09b

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

상기 △Maz는 식03z에 있어서, d2θaz/dt2를 단위량만큼 섭동시켰을 때의 Mz의 섭동량이므로, 다음 식에 의해 구해진다.

△Maz=Jaz …식09a

즉, 역위상 팔 흔들기의 단위 각가속도당의 상반력 모멘트 성분의 변화 △Maz는 팔 흔들기에 대응하는 플라이휠 FHaz의 관성 모멘트에 상당한다.

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

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

이하에 도 5의 주행 보용을 생성하는 것을 예로 하여, 본 제 1 참고예에서의 보용생성 장치(100)의 보용생성 처리의 상세를 도 13∼도 46을 참조하면서 설명한다. 도 13은 그 보용생성 장치(100)가 실행하는 보용생성 처리의 메인루틴을 도시하는 플로우차트(구조화 플로우차트)이다.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

본 발명의 제 1 참고예에서는, PCT 공개 공보 WO/03/40224의 제 1 실시형태에서 사용한 것과 같은 선형의 3질점 동역학 모델을 사용하고 있지 않지만, 다음 식에서 정의되는 발산성분 및 수렴성분의 개념은, 도 12에 도시한 모델과 같은 비선형의 동역학 모델의 거동의 섭동분에 대해서도, 근사적으로 충분한 정밀도로 적용할 수 있다.

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

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

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

또, ω0, ω0'은 어떤 소정의 값이다. 이들 ω0, ω0'값은 거의 동일한 값이지만, 엄밀하게는 일치하지 않는다. 그리고, 주행시에서는, PCT 공개 공보 WO/02/40224의 보행 보용생성시의 값에 대해 약간 변경할 필요가 있다.

발산성분 및 수렴성분에 관한 그 밖의 상세한 것은, PCT 공개 공보 WO/02/40224에 기술되어 있으므로, 여기에서는 이것 이상 언급하지 않는다.

본 제 1 참고예에서는, PCT 공개 공보 WO/02/40224에 개시한 방식에 더하여, 더욱이, 후술하는 바와 같이, 목표 상반력 연직성분 궤도를 규정하는 보용 패러미터를 설정하고, 이 목표 상반력 연직성분을 동역학적으로 만족하도록 로봇(1)의 전체 중심 연직위치를 결정한다. 이 경우, 상반력 연직성분의 2차적분값이 로봇(1)의 전체 중심 연직위치를 규정하는 것이 되므로, 목표 상반력 연직성분의 설정이 나쁘면, 로봇(1)의 전체 중심 연직위치 혹은 상체 연직위치가 지나치게 높아지거나 지나치게 낮아지거나 한다. 따라서, 목표 상반력 연직성분의 설정방법도 중요한 과제이다. 그러나, 상반력 연직성분과 상체 연직위치의 관계는 ZMP와 상체 수평위치와의 관계와 유사하므로, 상체 수평위치 속도를 적절하게 하기 위한 목표 ZMP 결정수법의 일부를, 이하의 본 제 1 참고예에 나타내는 바와 같이, 약간 변경하는 것만으로 상체 연직위치 속도를 적절하게 하기 위한 목표 상반력 연직성분의 결정수법에 적용할 수 있다.

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

먼저, S100에서, 금회 보용, 제 1 선회 보용, 제 2 선회 보용의 순서로 족평위치 자세 궤도가 이어지도록, 정상 보용의 보용 패러미터중의 족평궤도 패러미터가 결정된다. 이하에 구체적인 설정방법을 도 16을 참조하여 설명한다. 또한, 이후의 설명에서는, 지지다리측의 다리체(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 선회 보용의 종단)로 이어지도록(정상 보용의 초기, 종단에서의 기준 상체자세의 자세각 및 그 각속도가 일치하도록) 설정되어 있는 한, 일정한 자세일 필요는 없지만, 본 제 1 참고예에서는 이해를 쉽게 하기 위해서, 기준 상체 자세중의 경사각(연직방향에 대한 경사각)에 관계되는 자세는, 직립 자세(연직자세)로 설정된다. 즉, 본 제 1 참고예에서는, 상체(3)의 경사각에 관한 기준 상체자세는 정상 보용의 전체 기간에 있어서 직립 자세로 설정된다. 따라서, 본 제 1 참고예에서는 기준 상체자세중의 경사각의 각속도 및 각가속도는 0이다. 기준 상체자세중의 요잉각 궤도(이후, 기준 요잉각 궤도라고도 부름) θbz는, 예를 들면 일정 각속도(정상 보용의 평균 선회 속도)의 운동으로 하면 된다. 또는, 후술의 기준 역위상 팔 흔들기 궤도의 예(도 18)과 같이 싸인파 형상으로 해도 좋다. 단, 정상 보용을 반복했을 때에 기준 요잉각 및 그 각속도가 연속이 되도록 설정한다.

또한, 본 제 1 참고예에서는, 목표 상체자세중의 요잉각 궤도(이후, 목표 요잉각 궤도라고도 부름)는 기준 요잉각 궤도에 일치시키는 것으로 한다.

다음에 S104로 진행하고, 기준 팔자세 궤도 패러미터가 결정된다. 구체적으로는, 양 팔체(5, 5)의 전체의 중심위치(상체(3)에 대한 상대적인 중심위치), 좌우 의 손끝(양 팔체(5, 5)의 선단)의 좌우 방향의 간격, 및 역위상 팔 흔들기 각에 관한 패러미터가 결정된다. 기준 역위상 팔 흔들기 각은, 예를들면 도 17과 같이 좌선회 하는 경우에는, 도 18과 같이 설정하면 된다. 도 18과 같이, 기준 역위상 팔 흔들기 각 θazref는, 정상 보용을 반복했을 때에 보용의 경계(제 2 선회 보용의 종단과 다음 제 1 선회 보용의 경계)에 있어서 역위상 팔 흔들기 각 및 각속도가 모두 연속으로 되고, 또한 제 1 선회 보용 초기에 있어서의 지지다리와 역위상 팔 흔들기 각의 상대관계가, 다음 제 1 선회 보용 초기에 있어서의 지지다리와 역위상 팔 흔들기 각의 상대관계에 일치하도록 설정된다. 즉, 제 1 선회 보용의 초기 역위상 팔 흔들기 각속도와 제 2 선회 보용의 종단 역위상 팔 흔들기 각속도가 일치하고, 제 2 선회 보용의 종단 역위상 팔 흔들기 각은, 정상 보용의 선회각(제 1 선회 보용과 제 2 선회 보용의 선회각의 합)에 제 1 선회 보용 초기 역위상 팔 흔들기 각을 더한 값으로 설정된다. 도 18에서는 기준 역위상 팔 흔들기 각 θazref를 싸인파 형상의 파형으로 했지만, 일정 각속도로 설정해도 좋다. 또는, 지지다리의 요잉각과 미착지 다리의 요잉각의 평균의 값으로 해도 좋다.

또한, 본 제 1 참고예에서는, 목표 팔자세의 양 팔체(5, 5)의 전체의 중심위치(상체(3)에 대한 상대위치)는 상체(3)에 대해 일정하게 유지되도록 설정된다.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

바닥과 족평(22) 사이의 마찰 계수를 μ, 모멘트 연직성분을 발생하기 위한 바닥과 족평(22) 사이의 접촉면의 유효반경(또는 바닥과 족평(22) 사이의 접촉면의 목표 ZMP 주위의 단면 2차 모멘트의 평방근)을 r로 하면, Mzmin은, 항상 -μ*r *상반력 연직성분이상으로, Mzmax는 μ*r* 상반력 연직성분 이하로 설정되지 않으면 안된다. 가장 단순한 설정방법은, 다음 식에 의한 설정이다. 단, ka는 1보다 작은 플러스의 정수이다.

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

Mzmax=ka*μ*r*상반력 연직성분 …식1012

도 21의 상반력 모멘트 연직성분 허용범위는, 식10l2에 따라서 설정한 예이다. 상반력 모멘트 연직성분 허용범위를 규정하는 패러미터로서, 도 21의 사다리꼴 파형 등의 꺾임점에서의 값과 시각을 설정해도 좋지만, 식1012에 의해 상반력 모멘트 연직성분 허용범위를 결정할 때에는, 단지 식1012에서의 (ka*μ)의 값을 패러미터로서 설정하는 것만으로도 된다. r은 각 순간에 있어서 목표 ZMP와 접지면으로부터 산출하는 것이 좋지만, 정수로 해도 상관없다.

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

게다가, 상반력 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위를 독립적으로 설정하는 것이 아니고, 상반력 수평성분과 상반력 연직성분 모멘트의 조합의 허용범위를 설정하도록 해도 좋다. 왜냐하면, 상반력 수평성분이 커지면, 상반력 모멘트 연직성분의 허용범위가 좁아지고, 또한 상반력 모멘트 연직성분이 커지면, 상반력 수평성분의 허용범위가 좁아지기 때문이다.

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

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

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

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

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

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

최후에 S114로 진행하고, 정상 보용의 상체 자세각·역위상 팔 흔들기 각 복원기간 [Tm, Ts2] 및 [Tm2, Te]를 설정한다. 보충하면, 정상 보용을 반복했을 때에 보용의 경계에 있어서 상체 자세각 및 역위상 팔 흔들기 각은 연속이어야 한다. 그것을 위해서는, 정상 보용의 초기 상체자세 각속도와 종단 상체자세 각속도가 일 치하고, 정상 보용의 초기 역위상 팔 흔들기 각속도와 종단 역위상 팔 흔들기 각속도가 일치하지 않으면 안된다. 상기 기간은, 그렇게 하기 위한, 상체 자세각 궤도와 역위상 팔 흔들기 각 궤도를 조정하는 기간이다.

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

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

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

상기와 같이 설정하는 이유는 후술한다. 또한, 상체 자세각을 복원하는 기간과 역위상 팔 흔들기 각을 복원(조정)하는 기간을 각각 설정해도 좋다.

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

도 23의 플로우차트에서는, 먼저, S200에 있어서, 정상 보용의 보용 패러미터(상기 도 13의 S022에서 설정한 패러미터)에 기초하여 목표 족평위치 자세, 목표 팔자세, 및 목표 상체자세각(경사각과 요잉각)의 초기 상태(초기 시각 Ts에서의 상태)가 결정된다. 여기에서 상태란 위치나 자세각과 그 변화율(시간 미분)을 나타낸다.

이 경우, 지지다리측의 목표 족평위치 자세의 초기 상태는, 상기 도 15의 S100에서 결정한 족평궤도 패러미터의 제 1 선회 보용 초기 지지다리 족평위치 자세로부터, 제 2 선회 보용 종단 미착지 다리 족평위치 자세에 이르는 족평위치 자 세 궤도(다음회 보용 지지다리 좌표계로 본 궤도)를 시각 Ts까지 유한시간 정정 필터에 의해 생성함으로써 결정된다. 미착지 다리측의 목표 족평위치 자세의 초기 상태는 다음회 보용 지지다리 좌표계로 본 금회 보용 초기 지지다리 족평위치 자세로부터, 제 1 선회 보용 종단 미착지 다리 족평위치 자세에 이르는 족평위치 자세 궤도를 시각 Ts까지 유한시간 정정 필터에 의해 생성함으로써 결정된다. 또, 목표 팔자세의 초기 상태는, 도 15의 S104에서 결정한 기준 팔자세 궤도 패러미터에 기초하여 구해지는 시각 Ts에 있어서의 기준 팔자세로 정해진다. 보다 구체적으로는, 목표 팔자세의 양 팔체(5, 5)의 전체 중심위치(상체(3)에 대한 상대위치), 좌우의 손끝(양 팔체(5, 5)의 선단부)의 좌우 방향의 간격이 역위상 팔 흔들기 각·각속도가 결정된다. 단, 역위상 팔 흔들기 각·각속도는, 후술하는 바와 같이, 정상 보용을 반복했을 때에 보용의 경계에서 연속하도록 수정되므로, 여기에서는 아직 임시로 결정된 것 뿐이다.

또, 목표 상체 자세각의 초기 상태는, 도 15의 S102에서 결정한 기준 상체자세 궤도 패러미터에 의해 정해지는 시각 Ts에서의 기준 상체자세(경사각 및 요잉각) 및 그 각속도가 이 목표 상체 자세각의 초기 상태로서 결정된다. 또한, 본 제 1 참고예에서는, 상체(3)의 경사각에 관계되는 기준 상체자세는 연직자세이므로, 목표 상체자세중의 경사각의 초기 상태(경사각 및 그 각속도)는 0이다.

또, 본 제 1 참고예에서는, 정상 보용의 목표 족평위치 자세 궤도, 상반력 연직성분 궤도, 및 목표 ZMP 궤도는, 각각, 도 15의 플로우차트에서 결정한 족평궤도 패러미터, 상반력 연직성분 궤도 패러미터, ZMP 궤도 패러미터에 의해 서로 독 립적으로 결정된다. 예를 들면, 정상 보용의 순시 순시의 목표 족평위치 자세는, 상반력 연직성분의 순시값에 의존하지 않고 족평궤도 패러미터에 따라 결정된다.

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

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

보충하면, 사지탈 플레인상에서는, 요잉 회전이나 모멘트 연직성분 등 연직축 주위에 관한 개념은 존재하지 않으므로, 적어도 요잉 회전이나 모멘트 연직성분에 관해서는, 3차원 공간에서 연산된다.

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

이어서, S204를 거쳐서 S206에서, 정상 보용을 반복했을 때에 상체 연직위치 속도가 연속이 되고, 또한, 무릎 등의 관절각이 너무 늘어나거나 너무 줄어들거나 하지 않도록, 초기(시각 Ts) 상체 연직위치 속도 (Zs, Vzs)(Zs: 연직위치, Vzs: 연직속도)가 결정된다. 이것에 대한 상세한 것은, 예를 들면 본원 출원인이 앞서 출원한 PCT/JP02/13592에 기재되어 있으므로, 여기에서는 생략한다.

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

또, 상반력 모멘트 연직성분에 관한 조건, 즉, 상반력 모멘트 연직성분 허용범위를 만족하도록, 역위상 팔 흔들기 동작이 결정된다.

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

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

도 24는 그 처리를 도시하는 서브루틴 플로우차트이다.

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

역위상 팔 흔들기 각에는, 기준 초기 역위상 팔 흔들기 각(초기 시각 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의 보용 순시값 결정 서브루틴의 상세를 도 25를 참조하여 설명한다.

먼저 도 25의 S400에 있어서, 정상 보용 패러미터(상반력 연직성분 궤도 패러미터)를 기초로, 상기 도 19에 도시한 목표 상반력 연직성분의 시각 k에 있어서의 값(금회값)이 구해진다. 또한, S402에서 정상 보용 패러미터(ZMP 궤도 패러미터)를 기초로, 상기 도 22에 도시한 목표 ZMP 궤도의 시각 k에 있어서의 값(금회값)이 구해진다.

이어서 S404로 진행하고, 정상 보용 패러미터(족평궤도 패러미터, 기준 상체자세 궤도 패러미터, 및 팔자세 궤도 패러미터)를 기초로, 시각 k에 있어서의 목표 양 족평위치 자세(지지다리측 및 미착지 다리측 양자의 목표 족평위치 자세), 기준 상체자세 및 기준 팔자세의 값(금회값)이 구해진다. 기준 팔자세에 관해서는, 구체적으로는, 양 팔체(5, 5)의 전체 중심위치(상체(3)에 대한 상대위치), 좌우의 손끝(양 팔체(5, 5)의 선단부)의 좌우 방향의 간격 및 역위상 팔 흔들기 각의 값(금회값)이 구해진다.

또한, 목표 족평위치 자세의 금회값(시각 k에서의 값)은, 상기 도 23의 S200에서 초기 시각 Ts에서의 족평위치 자세를 구한 경우와 동일하게 구해진다.

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

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

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

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

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

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

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

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

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

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

이어서 S410로 진행하고, 상기 도 15의 S108에서 결정한 보용 패러미터(상반력 수평성분 허용범위를 규정하는 패러미터)에 기초하여 도 20에 도시한 상반력 수평성분 허용범위 [Fxmin, Fxmax]의 시각 k에서의 값(금회값)이 구해진다.

이어서 S411로 진행하고, 상기 도 15의 S109에서 결정한 보용 패러미터(상반력 모멘트 연직성분 허용범위를 규정하는 패러미터)에 기초하여 도 21에 도시한 상반력 모멘트 연직성분 허용범위 [Mzmin, Mzmax]의 시각 k에서의 값(금회값)이 구해진다.

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

또한, 상체 자세각중, 요잉각은 기준 상체자세각중 요잉각에 일치하도록 결정된다. 또, 목표 팔자세에 관해서는, 역위상 팔 흔들기 각 이외의 성분은 기준 팔자세에 일치하도록 결정된다. 또, 이때, 상기 조건을 만족하면서도, 목표 상체 경사각과 목표 역위상 팔 흔들기 각과는, 각각 기준 상체 경사각과 기준 역위상 팔 흔들기 각에 가능한 한 추종하도록 결정된다. 이하에 이 상세를 설명한다.

이 시점에서, 상기와 같이 족평위치 자세 및 상체 연직위치의 순시값(금회값)이 결정되어 있고, 또, 팔자세에 관해서, 역위상 팔 흔들기 각 이외의 성분은 기준 팔자세의 그것에 일치하도록 결정되어 있다. 이 때문에, 나머지 상체 수평위치와 상체자세각과 역위상 팔 흔들기 각을 결정하면, 로봇(1)의 목표 운동은 일의적으로 결정할 수 있다. 따라서, 모든 상반력도 일의적으로 결정된다. 또한, 본 제 1 참고예에서는, 정상 보용의 목표 상반력 연직성분 및 목표 ZMP는 각각 상기 도 13의 S022에서 결정한 상반력 연직성분 궤도 패러미터 및 목표 ZMP 궤도 패러미터에 의해 규정된다.

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

한편, 역위상 팔 흔들기 운동은 목표 ZMP 주위의 상반력 모멘트의 수평성분 및 상반력 수평성분 어느것도 변화시키지 않고, 상반력 모멘트 연직성분만을 변화시킬 수 있으므로, 상반력 모멘트 연직성분이 상기 상반력 모멘트 연직성분 허용범위를 초과하지 않도록 하기 위해서 사용할 수 있다. 이상을 고려하고, 본 제 1 참고예에서는, 도 26에 도시하는 플로우차트에 따라서 상체 수평가속도와 상체자세 각가속도와 역위상 팔 흔들기 가속도가 결정된다. 또한, 이해의 편의상, 상체 수평가속도와 상체자세 각가속도(상체(3)의 경사각의 각가속도)의 결정에 관해서는 사지탈 플레인상에서 X방향(전후 방향)에서의 상체 수평가속도 및 상체자세 각가속도를 결정하는 경우를 예로 들어서 설명하지만, 실제로는 Y방향(좌우 방향)의 상체 수평가속도 및 상체자세 각가속도도 X방향과 동일하게 결정된다.

먼저 S500에 있어서, 목표 상체 요잉각에 시각 k에서의 기준 상체 요잉각의 값을 대입한다. 또, 팔자세의 역위상 팔 흔들기 각·각속도 성분을 제외하고, 목표 팔자세에 시각 k에서의 기준 팔자세의 값을 대입한다.

다음에 S502에서 현재 시각(정상 보용 작성용 타이머의 값) k가, 상체자세각·역위상 팔 흔들기 각 복원기간(정상 보용의 경우, 상체자세각·역위상 팔 흔들기 각 복원기간은 시각 Tm부터 시각 Ts2의 기간 및 시각 Tm2부터 Te까지의 기간이다.) 사이에 있는지 없는지가 판정된다. S502의 판정 결과가 NO이면, S504로 진행하고, YES이면 S530으로 진행한다.

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

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

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

+mswg*d2Xswg/dt2 …식17

이렇게 하여 구해지는 Fxtmp의 예를 도 27에 도시한다. 도 27에, 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를 도 28에 도시한다. Fx는 Fxtmp의 값을 상반력 수평성분 허용범위 [Fxmin, Fxmax]로부터 초과하지 않도록 제한(포화)시킨 것으로 되어 있다. 보다 상세하게는, 상체 병진 모드만에 의한 상체 수평가속도 αtmp에 따른 Fxtmp가 허용범위 [Fxmin, Fxmax]내에 있을 경우에는, Fxtmp가 그대로 Fx가 된다. 또, 상체 병진 모드만에 의한 상체 수평가속도 αtmp에 따른 Fxtmp가 허용범위 [Fxmin, Fxmax]의 상한을 초과하거나, 혹은 하한을 하회한 경우에는, 각각 Fx는 각각 강제적으로 Fxmax, Fxmin에 제한된다. 특히 주행 보용의 공중기에서는, 항상, Fxmax=Fxmin=0이므로, Fx=0이 된다.

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

다음에 S518로 진행하고, 임시로 상체 병진 모드의 상체 수평가속도가 α, 상체 경사 모드의 상체 각가속도(상체 경사 각가속도)가 β, 상체 요잉 회전 모드의 상체가속도(상체 요잉 각가속도)가 기준 요잉 각가속도 d2θbzref/dt2, 역위상 팔 흔들기 각가속도 βa가 기준 역위상 팔 흔들기 각가속도 d2θazref/dt2인 운동을 시킨 경우의 상반력 모멘트 연직성분 Mztmp를 구한다. 이후, d2θbzref/dt2를 βbref, d2θazref/dt2를 βaref로 한다.

구체적으로는, 식03z에, 식1001부터 식1004을 대입함으로써 얻어지는 Mz가 Mztmp이다.

d2Xb/dt2=αx …식1001

d2Yb/dt2=αy …식1002

d2θbz/dt2=βbref …식1003

d2θaz/dt2=βaref …식1004

단, αx는 상체 수평가속도 α의 X성분, αy는, 상체 수평가속도 α의 Y성분 을 나타낸다. 또, Xb, Yb에는 시각 k-1에서의 상체 수평위치를 대입하고, Xzmp, Yzmp, Xsup, d2Ysup/dt2, Xswg, d2Yswg/dt2에는, 시각 k의 값을 대입하는 것으로 한다.

이와 같이 하여 구해지는 Mztmp의 예를 도 32에 도시한다. 도 32에 Mztmp가 상반력 모멘트 연직성분 허용범위 [Mzmin, Mzmax]를 초과해 있는 부분을 사선으로 나타낸다.

다음에, S520으로 진행하고, 역위상 팔 흔들기 각가속도 βa가 다음과 같이 결정된다(S520∼S528).

즉,

Mztmp>Mzmax이면, S522로 진행하고, 다음 식에 의해 Mz가 결정된다.

Mz=Mzmax …식1018

Mztmp<Mzmin이면, S524로 진행하고, 다음 식에 의해 Mz가 결정된다.

Mz=Mzmin …식1019

그 이외, 즉, Mztmp가 상반력 수평성분 허용범위 [Mzmin, Mzmax]내에 존재하면, S526으로 진행하고, 다음 식에 의해 Mz가 결정된다.

Mz=Mztmp …식1020

어느 경우에 있어서도, 이어서 S528로 진행하고, 다음 식에 의해 역위상 팔 흔들기 각가속도 βa가 결정된다.

βa=βaref+(Mztmp-Mz)/△Maz …식1021

단, △Maz는 식09a에 의해 결정된다.

이하에 S518로부터 S528까지의 처리를 해설한다.

상기한 바와 같이 구해지는 Mz는 역위상 팔 흔들기를 포함시킨 로봇 전체의 운동에 의한 상반력 모멘트 연직성분을 나타내고 있다.

상기 처리에서는, 이 Mz가 상반력 모멘트 연직성분 허용범위 [Mzmin, Mzmax]를 초과하지 않도록 역위상 팔 흔들기 각가속도 βa가 결정되어 있다. 구체적으로는, Mz는, 도 33에 도시하는 바와 같이, Mztmp의 값을 상반력 수평성분 허용범위 [Mzmin, Mzmax]로부터 초과하지 않도록 제한(포화)시킨 것으로 결정되어 있다. 보다 상세하게는, Mztmp가 허용범위 [Mzmin, Mzmax]내에 있을 경우에는, Mztmp가 그대로 Mz로 되고, Mztmp가 허용범위 [Mzmin, Mzmax]의 상한을 초과하거나, 혹은 하한을 하회한 경우에는, 각각 Mz는, 각각 강제적으로 Mzmax, Mzmin으로 제한된다. 특히 주행 보용의 공중기에서는, 항상, Mzmax=Mzmin=0이므로, Mz=0이 된다.

Mz가 상반력 모멘트 연직성분 허용범위 [Mzmin, Mzmax]를 초과하지 않도록 하기 위해서, 역위상 팔 흔들기에 의해 발생해야 할 모멘트 연직성분 Maz는 (Mz-Mztmp)이다. Maz(=Mz-Mztmp)를 도 34에 도시한다.

역위상 팔 흔들기 각가속도 βa는 Maz를 역위상 팔 흔들기 등가 관성 모멘트 △Maz로 나눈 것을 기준 역위상 팔 흔들기 각가속도 βaref(기준 역위상 팔 흔들기 각을 2차미분한 값)에 더함으로써 얻어진다. 즉, βa는 상기 식1021에서 구해진다. 역위상 팔 흔들기 각가속도βa를 도 35에 도시한다.

이상과 같이 S504부터 S528까지의 처리에서는, 역위상 팔 흔들기를 포함시킨 로봇 전체의 운동에 의해 발생하는 상반력 모멘트 연직성분 Mz가 허용범위 [Mzmin, Mzmax]를 초과하지 않도록(역위상 팔 흔들기 각가속도를 기준 역위상 팔 흔들기 각가속도 βaref에 일치시킨 경우에 발생하는 상반력 모멘트 연직성분 Mztmp가 허용범위를 초과한 분량을 상쇄하도록(캔슬하도록)), 역위상 팔 흔들기 각가속도 βa가 결정된다.

보충하면, 상반력 모멘트 연직성분 Mz가 상반력 모멘트 연직성분 허용범위 [Mzmin, Mzmax]를 엄밀하게 초과하지 않도록 하기 위해서, S504부터 S528까지의 처리 대신에, 역위상 팔 흔들기 각가속도 βa를 보다 엄밀한 동역학 모델을 사용하여 해석적 또는 탐색적으로 결정하는 편이 좋다. 또한, 탐색적인 결정법으로서는, 의사 자코비안(감도 매트릭스)을 구하고, 의사 뉴튼법 등에 의해 다음 후보를 결정하는 방법이나, 심플렉스법 등을 사용하면 된다.

이상이, 시각 k가 상체자세각·역위상 팔 흔들기 각 복원기간 사이에 없을 경우의 처리이다.

S502의 판정 결과가 YES인 경우에는 이하의 처리가 행해진다. 먼저, S530으로 진행하고, 로봇(1)의 전회 순간 보용상태(시각 k-1의 보용상태)로부터, 상체 경사 모드의 각가속도를 0으로 하고, 상체 병진 모드의 운동을 로봇(1)에게 시킨 경우에 금회(시각 k)의 목표 ZMP를 만족하기 위해서 필요한 상체 수평가속도 α를 구하고, 이것을 최종적인 상체 수평가속도로 결정한다.

다음에 S532로 진행하고, 상기 경우에 상반력 수평성분 Fx를 구한다.

다음에 S534로 진행하고, 상체자세 각가속도(상체 경사 각가속도) β를 0으로 결정한다. 또한, 상체 요잉 각가속도는 기준 상체 요잉 각가속도 βbref(기준 상체 요잉각을 2차미분한 값)로 결정된다.

이어서 최후에, S536으로 진행하고, 역위상 팔 흔들기 각가속도 βa에, 기준 역위상 팔 흔들기 각가속도 βaref(기준 역위상 팔 흔들기 각을 2차미분한 값)을 대입한다.

이상이, S502의 판정 결과가 YES인 경우의 처리이다. 즉, 이 경우에는, 상체자세 각가속도(상체 경사 각가속도 및 상체 요잉 각가속도)를 기준 상체자세 각가속도에, 역위상 팔 흔들기 각가속도를 기준 역위상 팔 흔들기 각가속도에 일치시킨다. 이와 같이 해도 운동에 의해 발생하는 상반력이, 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위중 어느 것도 초과하지 않았다고 예상되고 있으므로, 이와 같이 결정해도 문제 없다.

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

다음에 S416으로 진행하고, S412에서 구한 역위상 팔 흔들기 가속도 βa를 축차 적분함으로써(시각 Ts부터 현재 시각 k까지 누적 가산함으로써) 역위상 팔 흔 들기 속도가 구해지고, 또한 이것을 축차 적분함으로써(시각 Ts부터 현재 시각 k까지 누적 가산함으로써), 역위상 팔 흔들기 각 θaz(금회값)가 구해진다.

이상과 같이 도 24의 S306의 정상 보용 순시값 결정 서브루틴이 실행된 후, S308로 진행하고, 보용생성용 시각 k의 값이 보용생성 인터벌 △k만큼 증가된다. 이어서 S304로 되돌아가고, S304에 나타내는 조건이 성립하고 있는 한, S306과 S308의 처리가 반복되고, S304에 도시하는 조건이 성립하지 않게 되면, 즉 종단(시각 Te=Ts+Tcyc)까지 가보용의 생성이 완료하면, S310으로 진행한다.

정상 보용에서는, 그것을 반복했을 때에 경계에서 로봇(1)의 운동상태가 불연속으로 되지 않도록, 초기 상체자세각 및 그 각속도를 결정할 필요가 있다.

그 때문에, S310에서는, 상체자세 각속도를, 시각 Te까지, 초기값(시각 Ts에서의 값)으로 되돌리기 위한 상체자세 각가속도를 발생시키는 상반력 모멘트의 ZMP 환산값(이후, 상체자세 복원 모멘트 ZMP 환산값이라 부르고, ZMPrec 로 약칭함)의 패턴을 설정한다.

이것에 대해 이하에 상세하게 설명한다.

상체자세각·역위상 팔 흔들기 각 복원기간(시각 Tm부터 시각 Ts2의 기간 및 시각 Tm2부터 Te까지의 기간)에, 상체 경사 모드를 사용하여 상체자세 각가속도를 발생시킴으로써, 상체자세 각속도를 초기값(시각 Ts에서의 값)으로 되돌리는 것을 생각한다. 이것을 위한 상체자세 각가속도 패턴을 β(k)라고 한다. 또한, 상기 기간 이외에서는 β(k)=0으로 한다.

상체 경사 모드에서는, 상체자세 각가속도 β(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는 엄밀하게는 변동하지만, 근사적으로 일정값으로 해도 좋다. 정상보용은 임시로 생성될 뿐이며, 실제의 로봇을 이 보용에 추종시키는 것은 아니므로, 정상보용의 동력학적 정밀도는 그다지 높지 않아도 되기 때문이다.

도 30에 ZMPrec의 1예를 도시한다. 도 30에서는, ZMPrec의 패턴으로서, 시각 Tm부터 시각 Ts2의 기간 및 시각 Tm2부터 시각 Te까지의 기간에에 사다리꼴 패턴이 형성되어 있다. 그 사다리꼴부의 꺾임점의 시각은 시각 Tm과 시각 Ts2 사이의 기간 및 시각 Tm2부터 시각 Te까지의 기간에 있어서의 목표 ZMP 패턴의 꺾임점의 시각(도 22 참조)에 일치시켜져 있다. 이것은, 후술하지만, 금회 보용의 목표 ZMP 패턴의 수정이 간단해지기 때문이다.

식23의 △ZMP(k)에 ZMPrec(k)를 대입함으로써 다음 식이 얻어진다.

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

따라서, 이 식24에서 구해지는 β(k)는, 도 31의 실선으로 도시하는 바와 같이 된다. 또한, 도 31의 점선으로 도시하는 것은, 시각 Ts부터 시각 Tm까지의 기간 및 시각 Tm2부터 Te까지의 기간의 상체자세 각가속도(도 29의 실선으로 도시한 것)이다. (이후, 시각 k에서의 값인 것이 명확한 경우에는, (k)를 생략하는 경우가 있다.)

초기(시각 Ts) 상체자세각은, 초기(시각 Ts) 기준 상체자세각에 일치시킨다.

또, 초기 상체자세 각속도는, 식37a, 37b를 만족시키도록 결정한다.

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

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

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

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

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

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

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

……식37b

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

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

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

이어서 S312로 진행하고, 상체 경사 복원 모멘트 ZMP 환산값(ZMPrec) 패턴을 기초로, 이것에 의한 상체 수평위치·속도에의 영향량을 구하고, 종단 상체 수평위치·속도에 가산한다.

이 처리에 대해 해설한다. 또한, 그 상세한 것은, 본원 출원인의 PCT/JP02/13592에 설명되어 있으므로, 여기에서는 간략한 설명에 그친다.

시각 Ts부터 Tm의 기간 및 시각 Ts2부터 Te의 기간에 있어서, 상기와 같이 상체 경사 복원 모멘트 ZMP 환산값 (ZMPrec) 패턴을 발생하도록 상체자세 각가속도 β를 변화시키면, 상체자세 각가속도 β는 다음 식과 같이 된다.

β= -ZMPrec*Fz/△Mr …식1025

상체 경사 복원 모멘트를 발생시키지 않는 경우에 목표 ZMP를 만족하는 상체 수평가속도는, S532에서 구한 것과 같이 αtmp이지만, 상기와 같이 상체자세 각가속도 β를 변화시키면, 목표 ZMP를 만족하기 위해서 필요한 상체 수평가속도 α는 다음 식과 같이 된다.

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

식1025 및 1026으로부터,

α= αtmp+ZMPrec*Fz/△Mp …식1027

즉, 상체 경사 복원 모멘트 ZMP 환산값(ZMPrec)에 의해, 가속도가 식1027의 우변 제 2 항분만큼 증가한다.

식이 선형인 것을 이용하면, 상기와 같이 상체 경사 복원 모멘트 ZMP 환산값 (ZMPrec) 패턴을 발생하도록 상체자세 각가속도 β를 변화시킨 경우의 종단 상체 수평속도는, 상체 경사 복원 모멘트 ZMP 환산값(ZMPrec) 패턴을 발생시키지 않았을 경우의 종단 상체 수평속도, 즉 S414에서 구한 상체 수평속도의 종단값에, 시각 Ts부터 Te까지의 (ZMPrec*Fz/△Mp)의 1차적분을 가산한 것으로서 구해진다. 또, 상기와 같이 상체 경사 복원 모멘트 ZMP 환산값(ZMPrec) 패턴을 발생하도록 상체자세 각가속도 β를 변화시킨 경우의 종단 상체 수평위치는, 상체 경사 복원 모멘트 ZMP 환산값(ZMPrec) 패턴을 발생시키지 않은 경우의 종단 상체 수평위치, 즉 S414에서 구한 상체 수평위치의 종단값에, 시각 Ts부터 Te까지의 (ZMPrec*Fz/△Mp)의 2차적분을 가산한 것으로서 구해진다.

S312의 처리를 완료한 후, S314로 진행하고, 역위상 팔 흔들기 각속도가 초기와 종단에서 일치하도록 역위상 팔 흔들기 복원 각가속도(βarec)패턴을 결정한다.

구체적으로는, 역위상 팔 흔들기 복원 각가속도 패턴을 도 36과 같이 사다리꼴 형상으로 설정하고, 시각 Tm부터 Ts2까지의 기간의 사다리꼴 높이 azcyc2과 시각 Tm2부터 Te까지의 기간의 사다리꼴 높이 azcyc1을 동일하게 하고, 시각 Ts부터 Te까지의 βarec의 적분값과 상반력 모멘트 연직성분 Mz가 허용범위를 초과하지 않도록 하기 위한 상기 구한 역위상 팔 흔들기 가속도 βa의 적분값과의 합이 0이 되도록 사다리꼴 높이 azcyc1, azcyc2를 결정한다. 또한, 양 구간의 사다리꼴 높이는 동일하지 않아도 상관없다.

보충하면, 이와 같이 결정한 역위상 팔 흔들기 복원 각가속도 패턴에 의해 발생하는 상반력 모멘트 연직성분(Mazrec)은, 도 37과 같이 된다. 따라서, 역위상 팔 흔들기를 포함하는 로봇의 운동에 의해 발생하는 상반력 모멘트 연직성분 Mz는, 최종적으로는, 도 38에 도시하는 바와 같이, 도 32의 Mztmp와 도 34의 Maz와 도 37의 Mazrec와의 합, 즉, 도 33의 Mz와 도 37의 Mazrec의 합이 된다. 시각 Tm부터 Ts2까지의 기간 및 시각 Tm2부터 Te까지의 기간에 있어서는, 사다리꼴 형상의 복원 모멘트가 가산되는데, 이들 기간은 허용범위가 충분하게 넓은 기간으로 설정되어 있으므로, 역위상 팔 흔들기를 포함하는 로봇의 운동에 의해 발생하는 상반력 모멘트 연직성분은 허용범위를 초과하는 일은 없다.

다음에 S316으로 진행하고, 정상 보용의 초기(시각 Ts) 역위상 팔 흔들기 각 및 각속도를 결정한다.

구체적으로는, 초기 역위상 팔 흔들기 각속도는 이하의 식에 의해 결정한다.

초기 역위상 팔 흔들기 각속도

= 기준 초기 역위상 팔 흔들기 각속도

-(βarec가 0인 경우의 역위상 팔 흔들기 각

+βarec 패턴의 2차적분)/Tcyc …식1030

단, 상기 식에서, βarec가 0인 경우의 역위상 팔 흔들기 각이란 S416에서 구한 역위상 팔 흔들기 각(시각 Te에서의 역위상 팔 흔들기 각)이다. 또, βarec의 2차적분이란 도 36과 같이 설정한 역위상 팔 흔들기 복원 각가속도의 시각 Ts부터 Te까지의 2차적분값이다. 또, 기준 초기 역위상 팔 흔들기 각속도는 상기 기준 역위상 팔 흔들기 각속도(기준 역위상 팔 흔들기 각 θaref의 1차미분값)의 시각 Ts에서의 값이다.

초기 역위상 팔 흔들기 각은 기준 초기 역위상 팔 흔들기 각에 일치시키면 좋다. 또는, 최종적으로 결정된 역위상 팔 흔들기 각가속도(즉, 상반력 모멘트 연직성분 Mz가 허용범위를 초과하지 않게 하기 위한 상기 구한 역위상 팔 흔들기 가속도 βa와 복원 각가속도 βarec의 합)와 상기 구한 초기 역위상 팔 흔들기 각속도를 기초로, 임시로 초기 역위상 팔 흔들기 각을 기준 초기 역위상 팔 흔들기 각에 일치시킨 경우에 산출되는 팔 흔들기 각과 기준 역위상 팔 흔들기 각과의 차의 평균값 또는 상기 차의 최대값과 최소값의 평균값을 구하고, 구한 평균값의 2분의 1을 기준 초기 역위상 팔 흔들기 각으로부터 뺀 값을 최종적인 초기 역위상 팔 흔들기 각으로서 결정해도 좋다. 이렇게 함으로써, 산출되는 팔 흔들기 각과 기준 역위상 팔 흔들기 각과의 차의 절대값이 지나치게 커지는 것을 방지할 수 있다.

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

또 시각 Ts, Tm, Ts2 및 Tm2를 상기와 같이 설정한 또 하나의 이유는, 시각 Tm부터 Ts2까지의 기간과 시각 Tm2부터 Te까지의 기간에 있어서, 역위상 팔 흔들기 각속도를 기준 역위상 팔 흔들기 각 궤도의 초기 각속도로 되돌리도록 역위상 팔 흔들기 각가속도 βa를 발생시켜도, 상반력 모멘트 연직성분 Mz가 허용범위 [Mzmin, Mzmax]를 초과하지 않도록 하기 위해서이다. 바꾸어 말하면, 시각 Tm부터 Ts2까지의 기간과 시각 Tm2부터 Te까지의 기간에 있어서는, 상반력 모멘트 연직성분 허용범위가 충분히 크기 때문에, 역위상 팔 흔들기 각속도를 되돌리도록 역위상 팔 흔들기 각가속도 βa를 발생시켜도, 상반력 모멘트 연직성분 Mz는 허용범위를 초과하지 않는다.

이상과 같이 도 24의 S316의 처리가 완료된 후, 도 23의 S210으로 진행하고, 생성한 보용(가정상보용)의 종단 상체 수평위치·속도를, 그 순간의 지지다리에 대응하는 지지다리 좌표계(도 17의 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이 아니면 안된다. 본 제 1 참고예에서는, 이하와 같이, 상체 수평위치 속도 경계조건 오차(errx, errvx)가 거의 0이 되는(Xs, Vxs)가 탐색 적으로 구해진다.

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

S214의 판단 결과가 NO일 때에는 S216으로 진행한다. 이 S216에서는 (Xs, Vxs)의 부근에 복수(본 제 1 참고예에서는 2개)의 초기값 후보(Xs+△Xs, Vxs), (Xs, Vxs+△Vxs)가 결정된다. 여기에서, △Xs, △Vxs는 각각 Xs, Vxs에 대한 소정의 미소변화량을 의미하고 있다. 그리고, 이들 초기값 후보의 각각을 상체 수평위치 속도의 초기 상태로 하고, 상기 S208과 동일한 처리에 의해 정상 보용이 보용 패러미터를 사용하여 생성된다. 또한, 그 생성된 정상 보용의 종단 상체위치 속도를 그 순간의 지지다리에 대응하는 지지다리 좌표계(도 17의 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(도 24 참조)에서는, 상체자세 각속도의 초기값은 기준 상체자세 각속도의 초기값이 아니라, 상체 수평위치 속도의 전회 의 초기값 후보(Xs, Vxs)에 대응하는 S208의 처리에서의 S310(도 24 참조)에서 결정한 것으로 설정된다. 그리고, S214의 판단 결과가 YES로 되면, 반복 루프(S204)를 빠져나와 S220으로 진행한다. 또한, S204의 반복 루프를 빠져나왔을 때의 직전에 생성된 가정상보용이 경계 조건을 만족시키는 정상 보용으로서 얻어지게 된다.

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

구체적으로는, (X0, V0)와 (Z0, Vz0)는, S310에서 결정된 상체 경사 복원 모멘트 ZMP 환산값 패턴, 시각 Ts에 있어서의 정상 보용의 초기 상체자세각 및 각속도와, S204의 루프를 빠져나온 후의 시각 Ts에 있어서의 상체 수평위치 속도(Xs, Vxs)를 기초로, 보용의 조건을 만족하도록 보용을 생성한 경우에 있어서의, 제 2 선회 보용으로부터 제 1 선회 보용으로 전환된 순간의 시각, 즉, 시각 k=Tcyc(시각 Te-Ts)일 때에 결정되는 상체 수평위치 속도와 상체 연직위치 속도를, 각각 시각 Tcyc로부터 시작되는 1보(즉 2번째의 제 1 선회 보용)의 지지다리에 대응하는 지지다리 좌표계(도 17의 X''', Y''', Z''' 좌표계)로부터 본 값으로 변환한 값으로 결정된다. 마찬가지로, 초기 상태 자세각 및 각속도는, 시각 k=Tcyc(시각 Te-Ts)일 때에 결정되는 상체자세각 및 각가속도를, 시각 Tcyc로부터 시작되는 1보(즉 2번째의 제 1 선회 보용)의 지지다리에 대응하는 지지다리 좌표계(도 17의 X''', Y''', Z''' 좌표계)로부터 본 값으로 변환한 값으로 결정된다.

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

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

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

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

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

또한, S226으로 진행하고, 본래의 초기 시각 0(금회 보용의 종단시각)에 있어서의 초기 역위상 팔 흔들기 각 및 각속도(θaz0, ωaz0)를 구하고, 또한, 이것을 금회 보용의 지지다리 좌표계로 본 값인 (θaz0'', ωaz0'')을 구한다. 구체적으로는, (θaz0, ωaz0)는 S314 및 S316에서 결정된 역위상 팔 흔들기 복원 각가속도 패턴, 정상 보용의 초기(시각 Ts) 역위상 팔 흔들기 각 및 각속도를 기초로, 보용의 조건을 만족하도록 보용을 생성한 경우에 있어서의(보다 구체적으로는, 상체자세각·역위상 팔 흔들기 각 복원기간 이외에서는, 상반력 모멘트 연직성분이 허용범위를 초과하지 않도록, 역위상 팔 흔들기 각 궤도를 결정하고, 상체자세각·역 위상 팔 흔들기 각 복원기간에서는, 상기 기준 역위상 팔 흔들기 각가속도 βaref와 역위상 팔 흔들기 복원 각가속도 βarec의 합이 발생하도록 역위상 팔 흔들기 각 궤도를 결정한 경우에 있어서의) 제 2 선회 보용으로부터 제 1 선회 보용으로 전환된 순간의 시각, 즉, 시각 k=Tcyc(시각 Te-Ts)일 때에 결정되는 역위상 팔 흔들기 각 및 각속도를, 시각 Tcyc로부터 시작되는 1보(즉 2번째의 제 1 선회 보용)의 지지다리에 대응하는 지지다리 좌표계(도 17의 X''', Y''', Z''' 좌표계)로부터 본 값으로 변환한 값으로 결정된다.

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

이어서, 도 13의 S026으로 진행하고, 금회 보용의 보용 패러미터를 결정(일부는 가결정)한다. S026에서는, 보다 구체적으로는, 도 39에 도시하는 플로우차트에 따라서, 이하의 처리가 행해진다.

먼저, S600에서, 금회 보용의 족평위치 자세 궤도가 정상 보용의 족평위치 자세 궤도로 이어지도록, 금회 보용의 족평궤도 패러미터가 설정된다.

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

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

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

이어서, S602로 진행하고, 금회 보용의 기준 상체자세 궤도 패러미터가 정상 보용의 제 1 선회 보용이나 제 2 선회 보용과 동일하게 결정된다. 단, 금회 보용의 기준 상체자세 궤도가 상기 정상 보용의 기준 상체자세 궤도에 연속하여 이어지도록(금회 보용 종단에서의 기준 상체자세각 및 각속도가 각각 정상 보용 초기의 기준 상체자세각 및 각속도에 일치하도록) 상기 패러미터가 설정된다. 또한, 본 제 1 참고예에서는 경사각에 관한 기준 상체자세는 금회 보용 및 정상 보용 어떤 것이라도 정상적인 연직자세이다.

이어서, S604로 진행하고, 금회 보용의 기준 팔자세 궤도 패러미터가 정상 보용의 제 1 선회 보용이나 제 2 선회 보용과 마찬가지로 결정된다. 단, 금회 보용의 초기 기준 팔자세 및 그 변화율이 기준 팔자세 및 변화율의 현재 순간값에 일치하고, 또한, 금회 보용의 팔자세 궤도가 상기 정상 보용의 팔자세 궤도에 연속하여 연결되도록 상기 패러미터를 설정한다. 또한, 여기에서 결정되는 팔자세 궤도 패러미터는 정상 보용 패러미터의 결정(도 15의 S104)의 경우와 동일하고, 예를들면, 양 팔체(5, 5)의 전체의 중심위치(상체(3)에 대한 상대위치), 좌우의 손끝(양 팔체(5, 5)의 선단)의 좌우 방향의 간격 및 역위상 팔 흔들기 각에 관한 패러미터가 결정된다. 또한, 본 제 1 참고예에서는, 양 팔체(5, 5)의 전체의 중심위치는 상체(3)에 대해 일정하게 유지되도록 설정된다.

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

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

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

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

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

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

+중력가속도의 2차적분

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

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

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

+중력가속도의 1차적분 …식41b

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

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

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

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

이어서, S610으로 진행하고, 상반력 모멘트 연직성분 허용범위 [Mzmin, Mzmax](구체적으로는 이 상반력 모멘트 연직성분 허용범위의 패턴을 규정하는 패러미터)가 정상 보용의 제 1 선회 보용이나 제 2 선회 보용과 동일하게 설정된다. 예를들면 도 41에 도시하는 바와 같은 패턴으로 상반력 모멘트 연직성분 허용범위가 설정된다. 본 제 1 참고예에서는, 먼저 S606에서 결정한 상반력 연직성분 패턴 에 따라 상기 식1012에 기초하여 상반력 모멘트 연직성분 허용범위가 설정된다.

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

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

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

도 13의 설명으로 되돌아와, 상기와 같이 S026에 도시하는 처리(금회 보용의 보용 패러미터 결정처리)를 행한 후, 이어서 S028로 진행하고, 금회 보용의 보용 패러미터(ZMP 궤도 패러미터)가 수정되고, 또 역위상 팔 흔들기 각에 관한 패러미터가 결정된다. 이 처리에서는, 상체위치 자세 궤도를 정상 보용에 연속시키거나, 혹은 근접시키기 위해 ZMP 궤도 패러미터가 수정되고, 또한, 역위상 팔 흔들기 각이 정상 보용의 역위상 팔 흔들기 각 궤도에 수렴하도록 금회 보용의 역위상 팔 흔들기 각에 관한 패러미터가 결정된다.

도 42는 그 처리를 도시하는 서브루틴·플로우차트이다.

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

S702에서는, 도 43에 도시하는 플로우차트에 따라, 이하의 처리가 행해진다.

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

보충하면, 목표 팔자세는 목표 역위상 팔 흔들기 각·각속도를 포함한다.

이어서, S802를 거쳐서 S804에서, 가보용생성용 시각 k가 금회 보용 종단시각 Tcurr 이전 인지 아닌지(k≤Tcurr 인지 아닌지)가 판단되고, 그 판단 결과가 YES일 경우에는, S806의 금회 보용 순시값 결정 서브루틴으로 진행하고, 금회 보용의 시각 k의 순시값이 결정된다. S806의 보용 순시값 결정 서브루틴에서는, 전술의 S306과 동일하게 도 25에 도시하는 바와 같이 가보용을 생성한다. 단, 보용 패러미터로서 정상 보용 패러미터 대신에 금회 보용 패러미터를 사용할 수 있다.

이어서 S808로 진행하고, 가보용생성용 시각 k가 △k만큼 증가되고, S804로 되돌아온다.

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

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

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

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

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

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

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

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

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

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

이 ZMPrec는 상기 정상 보용생성처리에서 설명한 것과 동일하게 사다리꼴 형상의 패턴이며, 구체적으로는 다음과 같이 결정된다. 즉, 도 30에서의 제 2 선회 보용의 기간의 ZMPrec의 사다리꼴 패턴과 동일하게, 금회 보용의 ZMPrec의 사다리꼴 패턴을 설정하고, 사다리꼴의 정점(꺾임점)의 시각은 알고 있는 것으로 하는(보다 구체적으로는 사다리꼴의 꺾임점의 시각을 목표 ZMP의 꺾임점 시각에 맞추는) 동시에, 사다리꼴 높이를 미지수로 하여, 이하와 같이 ZMPrec의 사다리꼴 높이(패러미터)가 구해진다. 단, ZMPrec의 사다리꼴 패턴이 상승하기 시작하는 시각을 Ta로 하고, 사다리꼴 패턴으로부터 0으로 되돌아오는 시각을 Tb로 한다.

상기한 바와 같이 상체자세 복원 모멘트 ZMP 환산값 패턴의 미지 패러미터가 하나인 경우에는, 금회 보용의 종단에서 상체자세각과 상체자세 각속도 양쪽을 정상 보용으로 연속하여 연결시키는 것은 일반적으로는 불가능하다. 그래서, 본 제 1 참고예에서는, 복수보에 걸쳐서 서서히, 생성되는 보용의 상태가 정상 보용의 상태에 근접하도록, 미지 패러미터를 결정하기로 한다.

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

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

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

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

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

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

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

△θ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)의 상체 자세각에 관한 동력학 모델이 도 12에 도시하는 플라이 휠 FHx, FHy와 같은 선형 특성을 가지므로, △θ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 패턴으로 한 이유이다.

이어서 S722로 진행하고, 가금회 보용의 종단 역위상 팔 흔들기 각과 정상 보용의 초기 역위상 팔 흔들기 각과의 차 및 가금회 보용의 종단 역위상 팔 흔들기 각속도와 정상 보용의 초기 역위상 팔 흔들기 각속도의 차를 기초로, 금회 보용의 역위상 팔 흔들기 각 궤도가 정상 보용의 역위상 팔 흔들기 각 궤도에 근접하도록, 역위상 팔 흔들기 복원 각가속도 패턴이 결정된다. 이 패턴의 결정방법은, S718에서 상체자세 복원 모멘트 ZMP 환산값 패턴을 결정하는 방법과, 변수 이름이 다음과 같이 상이한 것 이외는, 거의 동일하다.

상체자세 복원 모멘트 ZMP 환산값 패턴→역위상 팔 흔들기 복원 각가속도 패턴

수평성분→모멘트 연직성분

이하에 상세를 설명한다. 여기에서 결정하는 역위상 팔 흔들기 복원 각가속도 패턴은, 후술하는 금회 보용 순시값의 생성처리에서, 상반력 모멘트 연직성분 허용범위가 충분히 큰 것으로 되는 기간(한쪽다리 지지기내의 기간)에 있어서, 역위상 팔 흔들기 각 궤도를 정상 보용으로 연결시키도록(근접시키도록) 수정하기 위한 것이다.

이 역위상 팔 흔들기 복원 각가속도 패턴은 상기 정상 보용생성 처리에서 설명한 것과 동일하게 사다리꼴 형상의 패턴으로, 구체적으로는 다음과 같이 결정된다. 즉, 도 36에서의 제 2 선회 보용의 기간의 역위상 팔 흔들기 복원 각가속도의 사다리꼴 패턴과 동일하게, 금회 보용의 역위상 팔 흔들기 복원 각가속도의 사다리꼴 패턴을 설정하고, 사다리꼴의 정점(꺾임점)의 시각은 알려진 것으로 하는(보다 구체적으로는 사다리꼴의 꺾임점의 시각을 목표 ZMP의 꺾임점 시각에 맞추는) 동시에, 사다리꼴 높이를 미지수로 하여 이하와 같이 역위상 팔 흔들기 복원 각가속도의 사다리꼴 높이(패러미터)가 구해진다. 단, 역위상 팔 흔들기 복원 각가속도의 사다리꼴 패턴이 상승하기 시작하는 시각을 Ta로 하고, 사다리꼴 패턴으로부터 0으 로 되돌아오는 시각을 Tb로 한다.

상기한 바와 같이 역위상 팔 흔들기 복원 각가속도 패턴의 미지 패러미터가 하나인 경우에는, 금회 보용의 종단에 있어서 역위상 팔 흔들기 각과 역위상 팔 흔들기 각속도 양쪽을 정상 보용에 연속으로 연결시키는 것은 일반적으로는 불가능하다. 그래서, 본 제 1 참고예에서는, 복수보에 걸쳐서 서서히, 생성되는 보용의 상태가 정상 보용의 상태에 근접하도록, 미지 패러미터를 결정하는 것으로 한다.

보충하면, 1보용중에서의 역위상 팔 흔들기 복원 각가속도 패턴을 복잡하게 하고, 미지 패러미터를 2개 이상으로 하여, 금회 보용의 종단에서 역위상 팔 흔들기 각과 역위상 팔 흔들기 각속도 양자를 정상 보용에 연속으로 연결시켜도 좋지만, 역위상 팔 흔들기 복원 각가속도 패턴이 지그재그로 크게 지나치게 변동할 우려가 있다.

상기와 같이 S702에서 역위상 팔 흔들기 복원 각가속도 패턴의 사다리꼴 높이를 0으로 하여 구한 가금회 보용의 종단 역위상 팔 흔들기 각과 정상 보용의 초기 역위상 팔 흔들기 각의 차를 구하고, 이것을 θazerr이라고 한다. 또, 가금회 보용의 종단 역위상 팔 흔들기 각속도와 정상 보용의 초기 역위상 팔 흔들기 각속도의 차를 구하고, 이것을 vθzerr이라고 한다.

여기에서, 역위상 팔 흔들기 복원 각가속도 패턴의 사다리꼴 높이를 어떤 값 bzcurr로 하여 금회 보용이 생성되고, 계속 이어서, 금회 보용과 동일한 알고리즘에 의해 제 1 선회 보용이 생성된다고 상정한다. 단, 제 1 선회 보용의 역위상 팔 흔들기 복원 각가속도 패턴은 도 24의 S314에서 구해진 제 1 선회 보용의 역위상 팔 흔들기 복원 각가속도 패턴(높이가 상기 azcyc1인 도 36의 사다리꼴 패턴)과 어떤 값 bz1의 합인 것으로 한다.

이렇게 하여 생성된 보용을 역위상 팔 흔들기 복원 각가속도 수정보용이라고 부르고, 그 종단(제 1 선회 보용의 종단) 역위상 팔 흔들기 각과 각속도를 각각 θz1, vθz1 이라고 한다.

S024에 있어서의 정상 보용의 초기 상태를 구하는 서브루틴의 처리가 완료한 시점에서 구해진 본래의 정상 보용(최종적으로 S314에서 결정된 정상 보용 초기 역위상 팔 흔들기 각 및 각속도를 초기값으로 하고, 역위상 팔 흔들기 복원 각가속도 패턴을 S314에서 구해진 패턴(높이가 azcyc1인 도 36의 사다리꼴 패턴)으로 한 경우의 정상 보용)의 제 1 선회 보용 종단 역위상 팔 흔들기 각과 각속도를 각각 θz1org, vθz1org 라고 한다.

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

△θz1=θz1-θz1org …식1050

△vθz1=vθz1-vθz1org …식1051

△θz1 및 △vθz1은 역위상 팔 흔들기 복원 각가속도 수정보용과 본래의 정상 보용을 제 1 선회 보용의 종단까지 생성한 시점에서, 양자간에 있어서의 역위상 팔 흔들기 각의 차 및 각속도의 차를 의미한다. 만약, △θz1 및 △vθz1이 0이 되면, 역위상 팔 흔들기 복원 각가속도 수정보용에 이어서, 금회 보용과 동일한 알고리즘에 의해, 역위상 팔 흔들기 복원 각가속도 패턴의 사다리꼴 높이를 상기 azcyc2로 하여 제 2 선회 보용이 생성되면, 이 보용은 본래의 정상 보용에 일치한 다.

따라서, △θz1 및 △vθz1이 0이 되는 금회 보용 사다리꼴 높이 bzcurr, 제 1 선회 보용 사다리꼴 높이 bz1을 구하고, 구한 bzcurr을 금회 보용의 사다리꼴 높이로서 최종적으로 결정하면 된다.

그런데, 로봇(1)의 역위상 팔 흔들기 각에 관한 동역학 모델이 도 12에 도시하는 플라이휠 FHaz와 같은 선형 특성을 가지므로, △θz1 및 △vθz1은 금회 보용 사다리꼴 높이 bzcurr, 제 1 선회 보용 사다리꼴 높이 bz1, 가금회 보용의 종단 역위상 팔 흔들기 각과 정상 보용의 초기 역위상 팔 흔들기 각의 차 θzerr, 가금회 보용의 종단 역위상 팔 흔들기 각속도와 정상 보용의 초기 역위상 팔 흔들기 각속도의 차 vθzerr과 이하의 관계를 갖는다.

△θz1 = cz11*bzcurr+cz12*bz1+θzerr+ez1*vθzerr …식1052

△vθz1 = cz21*bzcurr+cz22*bz1+ez2*vθzerr …식1053

단, cz11, cz12, cz21, cz22, ez1, ez2는 금회 보용, 제 1 선회 보용의 보용 주기, 역위상 팔 흔들기 복원 각가속도 패턴의 패러미터(특히 시간에 관한 패러미터) 등에 의해 일의적으로 결정되는 계수이다.

이상의 원리에 기초하여, 산출 수순으로서, 먼저, 가금회 보용과 정상 보용의 경계에서의 역위상 팔 흔들기 각의 차 θzerr와 각속도의 차 vθzerr이 구해진다.

이어서, 식1052, 1053의 계수인 cz11, cz12, cz21, cz22, ez1, ez2가 금회 보용, 제 1 선회 보용의 보용 주기, 역위상 팔 흔들기 복원 각가속도 패턴의 패러 미터(특히 시간에 관한 패러미터) 등을 기초로 구해진다.

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

최후에, 금회 보용의 역위상 팔 흔들기 복원 각가속도의 사다리꼴 패턴의 사다리꼴 높이가, 상기 구한 금회 보용 사다리꼴 높이 bzcurr로 설정된다.

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

S030에서는 그 서브루틴인 도 45의 처리가 실행된다.

도 45의 S1400부터 S1411까지는, 도 25의 S400부터 S411까지와 동일한 처리가 행해지고, 다음에 S1412의 서브루틴인 도 46의 S1000∼S1018의 처리가 행해진다.

구체적으로는, 먼저 S1000에서, 목표 상체 요잉각에 현재 시각에서의 기준 상체 요잉각의 값을 대입한다. 또, 팔자세 역위상 팔 흔들기 각·각속도를 제외하고, 목표 팔자세에 현재 시각에서의 기준 팔자세의 값을 대입한다.

이어서 S1002로 진행하고, 현재 시각이, 상체자세각·역위상 팔 흔들기 각 복원기간(시각 Ta부터 시각 Tb의 기간)의 사이에 있는지 아닌지가 판정된다. S1002의 판정 결과가 NO이면, S1004로 진행하고, YES이면 S1006으로 진행한다.

S1004에서는, 상체 경사각·역위상 팔 흔들기 각 복원기간이 아닌 경우의 상 체 수평가속도 α, 상체 각가속도 β, 역위상 팔 흔들기 각가속도 βa 산출 처리(도 26의 S504부터 S528)와 동일한 처리를 행한다.

S1006으로 진행하는 경우에서는, 먼저, S1006에서, 임시로 상체 병진 모드의 운동을 시킨 경우에 금회(시각 k) 목표 ZMP를 만족하기 위해서 필요한 상체 수평가속도 αtmp를 구한다.

이어서 S1008로 진행하고, 현재 시각에 있어서의 상체 경사 복원 모멘트 ZMP 환산값 패턴의 순간값 ZMPrec를 S718에서 결정한 상체 경사 복원 모멘트 ZMP 환산값 패턴에 관한 패러미터를 기초로 산출한다.

이어서 S1010으로 진행하고, 현재 시각에 있어서의 역위상 팔 흔들기 복원 각가속도 패턴의 순간값 βarec를 S722에서 결정한 역위상 팔 흔들기 복원 각가속도 패턴에 관한 패러미터를 기초로 산출한다.

이어서 S1012로 진행하고, 도시하는 식에 의해, 상체 경사 모드의 상체 각가속도(상체 경사 각가속도) β가 결정된다.

이어서 S1014로 진행하고, 도시하는 식에 의해, 상체 수평가속도 α가 결정된다.

이어서 S10l6으로 진행하고, S1010에서 산출한 역위상 팔 흔들기 복원 각가속도 패턴의 순간값 βarec와 기준 역위상 팔 흔들기 각가속도 βaref(기준 역위상 팔 흔들기 각을 2차미분한 값)의 합을 목표 역위상 팔 흔들기 각가속도 βa에 대입한다.

이어서 S1018로 진행하고, 상체 수평가속도가 α의 경우의 상반력 수평성분 Fx를 구한다.

이어서 S1414로 진행하고, 상체 수평가속도와 상체자세 각가속도를 적분하여 상체 수평속도와 상체자세 각속도(상체 경사 각속도)를 산출한다. 이것을 더 적분하고, 상체 수평위치와 상체자세(상체 경사각)을 결정한다. 또한, 상체자세중의 상체 요잉각은 본 제 1 참고예에서는 기준 상체 요잉각으로 결정된다.

이어서 S1416으로 진행하고, 역위상 팔 흔들기 가속도를 적분하여 역위상 팔 흔들기 각속도를 산출한다. 이것을 더 적분하여 역위상 팔 흔들기 각을 결정한다.

이상과 같이, 도 13의 S030의 처리가 완료된다.

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

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

도 4를 참조하여 본 제 1 참고예에 관계되는 장치의 동작을 더욱 설명하면, 보용생성 장치(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축력 센서(50)에 의해 검출된다. 그 검출값은 상기 복합 컴플라이언스 동작 결정부(104)에 보내진다.

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

보다 구체적으로는, 본 제 1 참고예에서는, 보상 전체 상반력 모멘트 수평 성 분 Mdmdxy는 예를 들면 PD 제어칙을 사용하여 다음 식에 의해 결정된다.

보상 전체 상반력 모멘트 수평성분 Mdmdxy

=Kθb*상체자세 경사각 편차

+Kωb*상체자세 경사각속도 편차 …d25

여기에서, Kθb, Kωb는 소정의 게인이다. 또, 상체자세 경사각속도 편차는 상체자세 경사각 편차의 시간 미분값이고, 목표 상체자세 경사각속도에 대한 실제 상체자세 경사각속도의 편차를 의미한다. 또, 상체자세 경사각 편차는 보다 상세하게는, 로봇(1)의 상체(3)의 롤링 방향(X축 주위)의 자세 경사각 편차와 피칭 방향(Y축 주위)의 자세 경사각 편차로 이루어지는 벡터이다.

또, 로봇(1)에 발생한 상기 실제 상체자세각 편차중의 요잉각 편차 θerrz(상세하게는 실제 상체자세각 편차중의 요잉 방향(Z축 주위)의 자세각 편차가 θerrz임)가 자세 센서(54)를 통하여 검출되고, 그 검출값은 요잉 안정화 제어 연산부(113)에 보내진다. 이 요잉 안정화 제어 연산부(113)에서, 로봇(1)의 실제 상체 요잉각 및/또는 각속도를 목표 상체 요잉각 및/또는 각속도에 수렴시키기 위한 목 표 전체 상반력 중심점(목표 ZMP) 주위 보상 전체 상반력 모멘트 연직성분이 산출되어 복합 컴플라이언스 동작 결정부(104)에 보내진다. 복합 컴플라이언스 동작 결정부(104)는 입력값에 기초하여 목표 상반력을 수정한다. 구체적으로는, 목표 전체 상반력 중심점(목표 ZMP) 주위에 보상 전체 상반력 모멘트가 작용하도록 목표 상반력을 수정한다.

보다 구체적으로는, 본 제 1 참고예에서는, 보상 전체 상반력 모멘트 연직성분 Mdmdz는 예를 들면 PD제어칙을 사용하여 다음 식에 의해 결정된다.

보상 전체 상반력 모멘트 연직성분 Mdmdz=Kθbz*상체 요잉각편차

+Kωbz*상체 요잉각 속도편차 ……d26

여기에서, Kθbz, Kωbz는 소정의 게인이다. 또, 상체 요잉각속도 편차는, 상체 요잉각 편차의 시간 미분값이고, 목표 상체 요잉각속도에 대한 실제 상체 요잉각속도의 편차를 의미한다.

보충하면, 보상금 상반력 모멘트 연직성분 Mdmdz를 상기 식d26에 의해 구할 때, 족평(22)과 바닥 사이의 연직축 주위 회전 미끄러짐 또는 연직축 주위 회전 진동을 방지하고 싶을뿐이라면, Kθbz를 0으로 하면 된다. 상체 요잉각 편차도 0에 근접시키고자 한다면, 실제 상반력 모멘트 연직성분이 커지는 경향이 있기 때문이다.

복합 컴플라이언스 동작 결정부(104)는 입력값에 기초하여 목표 상반력을 수정한다. 구체적으로는, 목표 전체 상반력 중심점(목표 ZMP) 주위에 보상 전체 상반력 모멘트 수평성분이 작용하도록 목표 상반력 모멘트 수평성분을 수정하고, 또 한, 목표 보용에 동역학적으로 균형을 이루는 목표 전체 상반력 중심점(목표 ZMP)주위의 목표 상반력 연직성분에 보상 전체 상반력 모멘트 연직성분을 부가적으로 추가함으로써 목표 상반력 모멘트 연직성분을 수정한다.

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

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

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

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

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

따라서, 본 제 1 참고예에서는, 동역학 모델을 사용하여 금회 보용 패러미터에 따라서 보용을 생성하고, 그대로 연속하여 정상 선회 보용 패러미터에 따라서 보용을 반복하여 생성했을 때, 생성한 보용의 상체 수평위치가 정상 선회 보용의 상체 수평위치에 수렴하도록(근접하도록), 금회 보용 패러미터를 수정했다고 할 수 있다. 단, 엄밀하게는, 금회 보용 직후의 제 1 선회 보용만은, 상기와 같이 구한 제 1 선회 보용 사다리꼴 높이 b1, bz1에 의해 수정된 보용으로 할 필요가 있다. 달리 말하면, 금회 보용과 제 1 선회 보용을 합친 보용을 금회 보용으로 간주하면, 본 참고예에서는, 상기와 같이 보용을 반복 생성했을 때, 생성한 보용의 상체자세각이 제 2 선회 봉용과 제 1 선회 보용으로 이루어지는 정상 보용의 상체자세각에 수렴하거나(근접하거나), 혹은 일치하도록, 금회 보용 패러미터를 수정했다고 할 수 있다.

이것은, PCT 공개 공보 WO/02/40224의 것과 동일하다.

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

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

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

더욱이 또, 본 제 1 참고예에서는, 동역학 모델을 사용하여 금회 보용 패러미터에 따라서 보용을 생성하고, 그대로 연속하여 정상 선회 보용 패러미터에 따라 서 보용을 반복 생성했을 때, 생성한 보용의 상체자세각이 정상 선회 보용의 상체자세각에 수렴하거나(근접하거나), 혹은 일치하도록, 금회 보용 패러미터를 수정했다고 할 수 있다. 단, 엄밀하게는, 금회 보용 직후의 제 1 선회 보용만은, 상기와 같이 구한 제 1 선회 보용 사다리꼴 높이 b1, bz1에 의해 수정된 보용으로 할 필요가 있다.

다음에 본 제 1 참고예의 변형 태양을 이하에 몇개 설명한다. 또한, 이하에 설명하는 변형 태양은 후술의 각 실시형태에서도 동일하다.

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

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

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

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

≤(ka*μ*Fz)*(ka*μ*Fz) …식59

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

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

또, 상반력 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위를 상기와 같이 각각 설정하는 대신에, 상반력 수평성분과 상반력 모멘트 연직성분과의 세트의 허용범위를 설정하도록 해도 좋다. 상반력 수평성분이 커짐에 따라 상반력 모멘트 연직성분의 마찰한계는 작아진다. 또, 상반력 모멘트 연직성분이 커짐에 따라, 상반력 수평성분의 마찰한계는 작아진다. 따라서, 이것을 감안하여 상반력 수평성분과 상반력 모멘트 연직성분의 세트의 허용범위를 설정하도록 하는 편이, 보다 현실의 마찰한계 특성에 가까운 허용범위를 설정할 수 있다. 구체적으로는, 상반력 수평성분의 절대값과 상반력 모멘트 연직성분의 절대값의 가중평균에 대해 허용범위를 설정해도 좋다.

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

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

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

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

예를 들면 좌우의 팔체의 선단위치를 동일한 방향으로 휘젓는 운동 모드, 접지하고 있지 않은(공중에 존재하는) 족평의 위치를 섭동시키는 운동 모드이어도 좋다. 단, 미착지 다리 궤도를 섭동시키는 경우에는, 착지 위치가 바뀌지 않도록 착지 직전까지는, 섭동량을 실질적으로 0으로 되돌려야 한다. 또한, 좌우의 팔체의 선단위치를 동일한 방향으로 휘젓는 운동 모드와 역위상 팔 흔들기 모드를 병용해도 되는 것은 물론이다.

또, 3개 이상의 운동 모드를 사용해도 좋다.

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

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

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

또, 상반력 모멘트 연직성분이 허용범위를 초과하지 않도록 하기 위해서(스핀력을 캔슬하기 위해서), 역위상 팔 흔들기 모드 대신에, 상체 요잉 회전 모드를 사용해도 좋다. 상체 요잉 회전 모드를 사용하는 경우에는, 로봇(1)의 상체(3)를 허리근처의 부분(예를 들면 도 1의 54의 부재의 하측의 부분)과 그 상측의 부분(예를 들면 도 1의 54의 부재의 상측의 부분)으로 분할하고, 상측의 부분이 허리근처의 부분에 대해 요잉 방향으로(예를 들면 상체(3)의 체간축 주위로) 회전할 수 있도록 만들어 두는 것이 바람직하다. 이렇게 하면, 다리체(2, 2)의 자세에 영향을 주지 않고, 상체(3)의 상측의 부분을 회전시켜서, 상반력 모멘트 연직성분을 조정할 수 있다. 이렇게 한 경우의 참고예는, 예를 들면 상기 제 1 참고예에서 역위상 팔 흔들기 각, 그 각속도 및 그 각가속도를 결정하는 대신에, 상체(3)의 상측의 부분의 회전각, 그 각속도 및 그 각가속도를 결정하도록 하면 된다. 이 경우, 양 팔체(5, 5)는 예를들면 상체(3)의 상측의 부분 에 대한 상대위치를 일정하게 하면 된다. 단, 상체(3)의 상측의 부분의 요잉 회전과 더불어, 양 팔체(5, 5)의 역위상 팔 흔들기 동작을 부가해도 되는 것은 물론이다.

또, 상반력 모멘트 연직성분이 발생하는 것이라면, 팔, 상체이외의 부위를 변위시키는 운동 모드를 사용해도 좋다.

예를 들면, 공중기에서, 양 다리 선단을 전후 역방향으로 움직이는 모드를 사용해도 좋다.

또는, 상반력 모멘트 연직성분이 발생하는 몇개의 운동 모드를 병용해도 좋다. 예를 들면, 역위상 팔 흔들기 모드와 상체 요잉 회전 모드를 병용해도 좋다.

또, 상체 요잉 회전 모드 및 역위상 팔 흔들기 모드는 전체 중심위치가 바뀌지 않도록(바꿔 말하면 상반력 수평성분을 발생시키지 않고) 상반력 모멘트 연직성분을 발생시키는 모드이지만, 전체 중심위치가 바뀌어버리는(바꿔 말하면 상반력 수평성분을 발생해버리는) 운동이어도 좋다. 상체 병진 모드와 조합함으로써, 상반력 수평성분을 조정할 수 있기 때문이다.

상기 제 1 참고예에 사용한 동역학 모델(도 12의 동역학 모델) 이외에도 이하의 모델을 사용해도 좋다.

1) 도 49에 도시한 바와 같이 복수의 링크에 질점을 설정한 비선형 모델(다질점 모델). 이 모델의 각 링크에는, 이너셔(관성 모멘트)가 설정되어 있어도 좋다.

2) 본원 출원인에 의한 PCT 공개 공보 WO/02/40224에 개시되는 3질점 모델

3) 상체 요잉 회전 모드 혹은 역위상 팔 흔들기 모드에 의해 발생하는 관성모멘트를 무시한 모델.

4) 중력과 관성력과의 합력(또는 이것과 균형을 이루는 상반력)과 상체 병진 운동의 관계를 나타내는 부분 모델과, 상기 합력과 상체자세 회전운동(상체 경사운동과 상체 요잉 회전운동)의 관계를 나타내는 부분 모델으로 분리하여 갖는 분리형 모델. 예를 들면, 도 12에 도시하는 질점은 상기 합력과 상체 병진운동의 관계를 나타내는 부분 모델이며, 도 12에 도시하는 플라이휠은 상기 합력과 상체자세 회전운동의 관계를 나타내는 부분 모델이다.

또한, 상기 1)∼4) 어느 경우에도, 관성모멘트 연직성분을 발생시키는 운동 모드는 필요하다.

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

또한, 상기 제 1 참고예에 있어서, 블록선도, 플로우차트, 및 알고리즘 등은 연산처리 순서를 바꾸는 등의 등가변형을 해도 좋다. 또, 적당히 로 패스 필터를 삽입해도 좋다.

또, 상기 제 1 참고예를 2족이동 로봇에 관해서 설명해 왔지만, 3족 이상의 다족 로봇에도 응용할 수 있다.

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

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

정상 보용으로 연결시키기(근접시키기) 위한 금회 보용의 수정방법으로서, 본 제 1 참고예에서는, 금회 보용의 목표 ZMP 패러미터를 수정했지만, 그 이외의 패러미터를 수정해도 좋다.

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

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

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

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

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

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

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

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

금회 보용의 역위상 팔 흔들기 복원 각가속도 패턴의 사다리꼴 높이 bzcurr을 결정하는 방법으로서, 이하의 방법을 사용해도 좋다.

상기 역위상 팔 흔들기 복원 각가속도 수정 보용(도 42의 S722의 설명을 참조)의 금회 보용 종단시점에서의 역위상 팔 흔들기 각과 각속도를 각각 θzcurr, vθzcurr이라 하고, 이것들과 정상 보용의 역위상 팔 흔들기 각, 각속도와의 차를 △θzcerr, △vθzcerr이라고 한다.

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

금회 보용 및 또는 정상 보용의 역위상 팔 흔들기 복원 각가속도 βarec는, 사다리꼴 패턴으로 하지 않고, 각 순간에 있어서, 목표 역위상 팔 흔들기 각·각속도와 기준 역위상 팔 흔들기 각·각속도와 차 등을 기초로, 그 차가 0에 수렴하도록, 상태 피드백칙 등을 사용하여, 각 순간에 있어서의 역위상 팔 흔들기 복원 각가속도 βarec의 값을 결정해도 좋다.

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

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

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

…식72

또, 사면 이동시(경사진 바닥면에서 로봇(1)을 이동시킬 때)의 보용생성에 있어서는, 상반력 모멘트 연직성분은, 식1072에 의해 바닥면 마찰력 법선방향 모멘트로 변환할 수 있으므로, 상반력 모멘트 연직성분 허용범위 대신에, 상반력 모멘트의 바닥면 법선방향 성분, 즉 바닥면 마찰력 법선방향 모멘트의 허용범위의 허용범위를 설정하도록 해도 좋다.

바닥면 마찰력 법선방향 모멘트=상반력 모멘트 연직성분*C0S(θf)

…식1072

금회 보용의 패러미터 결정은, 상기 제 1 참고예와 같이 전회 보용이 완료된 시점에서 행하는 이외에, 본원 출원인에 의한 PCT 공개 공보 W0/02/40224에 기술되 어 있는 바와 같이, 금회 보용의 생성 도중에 재결정 해도 좋다. 이렇게 함으로써, 연산량은 증가하지만, 보용에 대한 요구에 변경이 있었을 때에 즉각 대응할 수 있기 때문이다.

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

또, 상기 제 1 참고예에서는, 도 5의 주행 보용을 생성하는 경우를 예로 들어서 설명했지만, 로봇(1)의 보행 보용을 생성하는 경우에도, 상기 제 1 참고예와 동일하게 목표 보용을 생성할 수 있다. 이 경우에는, 예를 들면 상기 제 1 참고예에 있어서, 목표 상반력 연직성분을 상기 도 6에 도시한 것 대신에, 예를 들면 도 50에 도시하는 바와 같은 패턴으로 설정하면 된다. 이 예에서는, 상반력 연직성분 궤도는, 양다리 지지기에서는 상반력 연직성분의 증가측에 볼록한 사다리꼴 형상으로 설정되고, 한쪽다리 지지기에서는 상반력 연직성분의 감소측에 볼록한 사다리꼴 형상으로 설정된다. 또한, 사다리꼴 부분의 높이 C1, C2의 설정방법 등의 상세한 것은, 예를 들면 본원 출원인에 의한 PCT 공개 공보 WO/03/057425/A1에 상세하게 설명되어 있으므로 여기에서의 설명은 생략한다.

다음에, 이상 설명한 제 1 참고예 및 그 변형 태양을 기초로 하여, 본 발명의 제 2 참고예를 도 53∼도 58을 참조하여 설명한다. 또한, 본 제 2 참고예의 설명에 있어서, 상기 제 1 참고예와 동일 구성 부분 혹은 동일 기능 부분에 대해서는 상기 제 1 참고예와 동일한 참조부호를 사용하고 설명을 생략한다. 특히, 제 2 참고예는, 앞에서도 기술한 바와 같이, 상기 도 1∼도 3 및 도 5∼도 12를 참조하여 설명한 사항은 상기 제 1 참고예와 동일하다.

본 제 2 참고예가 상기 제 1 참고예와 상이한 점의 개요를 설명하면, 본 제 2 참고예에서는 목표 상체자세각과 실제 상체자세각의 차인 실제 상체자세각 편차(상체(3)의 연직방향에 대한 경사각의 편차 및 요잉각의 편차), 및/또는 그 변화율을 0에 근접시키기 위해, 컴플라이언스 제어로의 목표 상반력을 조작함과 더불어, 목표 보용도 수정하도록 했다. 특히, 실제 상체자세 편차중의 요잉각 성분 및/또는 그 각속도에 따라, 목표 보용에 동역학적으로 균형을 이루는(목표 보용의 운동의 관성력과 중력과의 합력이 목표 ZMP 주위에 발생하는 모멘트의 연직성분에 균형을 이루는) 목표 ZMP 주위의 상반력 모멘트의 연직성분도 수정하도록 했다.

본 제 2 참고예에 있어서의 제어 유닛(60)의 기능적 구성을 나타내는 블럭도를 도 53에 도시한다. 이하, 제 2 참고예에서의 제어 유닛(60)의 기능적 구성이 상기 참고예의 것(도 4의 것)과 상이한 점을 설명한다.

본 제 2 참고예에 있어서는, 자세 경사안정화 제어 연산부(112)에서 산출되 는 보상 전체 상반력 모멘트 수평성분 Mdmdxy는, 보상 전체 상반력 모멘트 수평성분 분배기(110)에 입력된다. 보상 전체 상반력 모멘트 수평성분 분배기(110)는 보상 전체 상반력 모멘트 수평성분 Mdmdxy를 컴플라이언스 제어용 목표 상반력 모멘트 수평성분과 모델 조작 상반력 모멘트 수평성분으로 분배한다. 환언하면, 실제 상체자세 경사각 편차를 기초로, 자세 경사안정화 제어 연산부(112) 및 보상 전체 상반력 모멘트 수평성분 분배기(110)에 의해, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분과 모델 조작 상반력 모멘트 수평성분이 결정된다.

구체적으로는, 보상 전체 상반력 모멘트 수평성분 분배기(110)에 있어서는, 먼저, 모델 조작 상반력 모멘트 수평성분이 다음 식으로 결정된다. 또한, 상반력 모멘트 수평성분 허용범위는 후술하는 바와 같이 보용생성 장치(100)에서 결정된다.

Mdmdxy>상반력 모멘트 수평성분 허용범위 상한값일 경우

모델 조작 상반력 모멘트 수평성분 = -Mdmdxy

+상반력 모멘트 수평성분 허용범위 상한값

Mdmdxy<상반력 모멘트 수평성분 허용범위 하한값일 경우

모델 조작 상반력 모멘트 수평성분 = -Mdmdxy

+상반력 모멘트 수평성분 허용범위 하한값

상반력 모멘트 수평성분 허용범위 하한값≤Mdmdxy, 또한,

Mdmdxy≤상반력 모멘트 수평성분 허용범위 상한값일 경우

모델 조작 상반력 모멘트 수평성분 = 0

……식d27a

다음에 컴플라이언스 제어용 목표 상반력 모멘트 수평성분이 다음 식으로 결정된다.

컴플라이언스 제어용 목표 상반력 모멘트 수평성분

= Mdmdxy+모델 조작 상반력 모멘트 수평성분 ……식d27b

따라서, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분과 모델 조작 상반력 모멘트 수평성분과의 차가 Mdmdxy와 동일하게 되도록 그것들의 상반력 모멘트 수평성분이 결정된다.

상기와 같은 연산을 행하는 보상 전체 상반력 모멘트 수평성분 분배기(110)를 블럭도로 나타내면, 도 54에 도시하는 바와 같다.

또, 요잉 안정화 제어 연산부(113)에서 상기 제 1 참고예와 동일하게 결정된 보상 전체 상반력 모멘트 연직성분 Mdmdz(상기 식d26을 참조)는 모델 조작 상반력 모멘트 연직성분 결정기(111)에 입력된다. 모델 조작 상반력 모멘트 연직성분 결정기(111)는 보상 전체 상반력 모멘트 연직성분 Mdmaz를 기초로 모델 조작 상반력 모멘트 연직성분을 결정한다. 바꾸어 말하면, 실제 상체자세각 편차중의 상체 요잉각 편차를 기초로, 요잉 안정화 제어 연산부(113) 및 모델 조작 상반력 모멘트 연직성분 결정기(111)에 의해, 보상 전체 상반력 모멘트 연직성분 Mdmdz와 모델 조작 상반력 모멘트 연직성분이 결정된다.

구체적으로는, 모델 조작 상반력 모멘트 연직성분 결정기(111)에 있어서, 모델 조작 상반력 모멘트 연직성분이 다음 식으로 결정된다. 또한, 상반력 모멘트 연직성분 보상량 허용범위는 후술하는 바와 같이 보용생성 장치(100)에서 결정된다.

Mdmdz>상반력 모멘트 연직성분 보상량 허용범위 상한값일 경우

모델 조작 상반력 모멘트 연직성분 = -Mdmdz

+상반력 모멘트 연직성분 보상량 허용범위 상한값

Mdmdz<상반력 모멘트 연직성분 보상량 허용범위 하한값일 경우

모델 조작 상반력 모멘트 연직성분 = -Mdmdz

+상반력 모멘트 연직성분 보상량 허용범위 하한값

상반력 모멘트 연직성분 보상량 허용범위 하한값≤Mdmdz, 또한, Mamdz≤상반력 모멘트 연직성분 보상량 허용범위 상한값일 경우 모델 조작 상반력 모멘트 연직성분 = 0 ……식d26b

상기한 바와 같은 연산을 행하는 모델 조작 상반력 모멘트 연직성분 결정기(111)를 블럭도로 나타내면, 도 55에 도시하는 바와 같이 된다. 따라서, 모델 조작 상반력 모멘트 연직성분은 보상 전체 상반력 모멘트 연직성분 Mdmdz중 상반력 모멘트 연직성분 보상량 허용범위를 일탈한 분량의 부호를 반전시킨 것으로 설정된다.

컴플라이언스 제어용 목표 상반력 모멘트 수평성분 및 보상 전체 상반력 모멘트 연직성분 Mdmdz는 복합 컴플라이언스 동작 결정부(104)에 보내진다.

모델 조작 상반력 모멘트 수평성분 및 연직성분은 보용생성 장치(100)에 보내진다.

또한, 보상 전체 상반력 모멘트 연직성분 Mdmdz 대신에, 보상 전체 상반력 모멘트 연직성분 Mdmdz와 모델 조작 상반력 모멘트 연직성분과의 합을, 컴플라이언스 제어용 목표값으로 하여, 복합 컴플라이언스 동작 결정부(104)에 보내도 좋다.

복합 컴플라이언스 동작 결정부(104)는 보용생성 장치(100)가 생성하는 목표 보용의 운동에 로봇(1)의 운동을 추종시키면서, 보용생성 장치(100)가 생성하는 목표 전체 상반력에 컴플라이언스 제어용 목표 상반력 모멘트 수평성분과 보상 전체 상반력 모멘트 연직성분 Mdmdz를 부가함으로써 수정되는 목표 전체 상반력에 실제 상반력이 근접하도록, 목표 족평위치 자세를 수정함으로써, 기구변형 보상된 수정 목표 족평위치 자세(궤도)를 결정한다.

이 경우, 로봇(1)의 족평위치 자세나 상반력의 모든 상태를 목표에 일치시키는 것은 사실상 불가능하므로, 상기 제 1 참고예와 동일하게, 이것들 사이에 트레이드오프 관계를 주어서 타협적으로 가능한 한 일치시킨다.

상세한 것은 후술하지만, 보용생성 장치(100)는 이 보용생성장치(100)에서 결정되는 목표 ZMP 주위의 상반력 모멘트 수평성분이 모델 조작 상반력 모멘트 수평성분이 되도록 동역학 모델을 사용하여 목표 보용의 운동(특히 상체위치 자세 궤도)을 생성한다. 또한, 보용생성 장치(100)는 모델 조작 상반력 모멘트가 0으로 하여 생성된 목표 보용(가목표 보용)에 동역학적으로 균형을 이루는 목표 전체 상반력 중심점(목표 ZMP) 주위의 목표 상반력 모멘트 연직성분에 모델 조작 상반력 모멘트 연직성분이 부가적으로 발생하도록 목표 보용의 운동(특히 팔 흔들기 궤도)을 수정한다.

이것들 이외의, 제어 유닛(60)의 기능적 구성은 상기 제 1 참고예와 동일하다. 보충하면, 상기 제 1 참고예에서 생성한 목표 보용은 본 제 2 참고예에서 상기 모델 조작 상반력 모멘트 수평성분과 모델 조작 상반력 모멘트 연직성분을 정상적으로 0으로 설정한 경우에 생성되는 목표 보용과 동일하다.

제 2 참고예에서의 보용생성 장치(100)의 동작(보용생성처리)을 그 메인 플로우차트인 도 56을 사용하여 이하에 상세히 기술한다. S3010부터 S3028까지는 상기 참고예의 도 13의 S010부터 S028과 동일한 처리를 행한다.

이어서, S3030으로 진행하고, 컴플라이언스 제어용의 목표 ZMP 주위의 상반력 모멘트 수평성분과 상반력 모멘트 연직성분 보상량의 허용범위를 규정하는 패러미터가 결정된다.

또한, 상반력 모멘트 수평성분을 상반력 연직성분으로 나눈 것이 ZMP(상반력 중심점)의 목표 ZMP로부터의 벗어남량을 의미하는 것이 된다. 따라서, 상반력 모멘트 수평성분 허용범위를 상반력 연직성분으로 나눔으로써, 상반력 중심점으로 변환한 ZMP 허용범위(상반력 중심점 허용범위)의 패러미터를 설정하도록 해도 좋다.

보충하면, S3030에서 결정하는 컴플라이언스 제어용의 상반력 모멘트 수평성분 허용범위와 상반력 모멘트 연직성분 보상량 허용범위의 패러미터를 기초로, 후술하는 금회 보용 순시값 결정 서브루틴(S3032의 서브루틴)에서, 그것들의 허용범위의 순시값이 결정되고, 그것이, 상기 보상 전체 상반력 모멘트 수평성분 분배기(110) 및 모델 조작 상반력 모멘트 연직성분 결정기(111)에 있어서의 상기의 처리에 사용된다.

이 상반력 모멘트 수평성분 허용범위에 대해서는, 본원 출원인에 의한 PCT출원 PCT/JP03/00435에 상반력 모멘트 허용범위로서 설정방법이 상세하게 기재되어 있으므로, 본 명세서에서는 더 이상의 설명을 생략한다.

상기 상반력 모멘트 연직성분 보상량은, 보용생성 장치(100)에 있어서, 가령, 상반력 모멘트 연직성분이 보용생성용의 상반력 모멘트 연직성분 허용범위(이것은 S3026에서 설정됨)내에 제한된 목표 보용을 생성한 경우에, 목표 보용의 운동이 발생하는 상반력 모멘트에 부가할 수 있다 상반력 모멘트 연직성분의 보상량을 의미한다. 따라서, 보용생성용의 상반력 모멘트 연직성분 허용범위가 실제의 마찰한계보다도 충분하게 좁게 설정되어 있지 않으면, 상반력 모멘트 연직성분 보상량 허용범위를 넓게 설정할 수는 없다.

상반력 모멘트 연직성분 보상량 허용범위는 보용생성용의 상반력 모멘트 연직성분 허용범위(상기 도 41을 참조)와 상사형으로 설정하면 된다. 또, 상기 도 5의 주행 보용의 공중기에서는 컴플라이언스 제어용의 상반력 모멘트 연직성분 보상량 허용범위는 상한값이 0, 하한값이 0의 영역으로 설정된다.

도 56으로 되돌아오고, 상기와 같이 S3030에서 컴플라이언스 제어용의 목표 ZMP 주위의 상반력 모멘트 수평성분과 상반력 모멘트 연직성분 보상량의 허용범위를 규정하는 패러미터를 결정한 후, 또는 S3016의 판단 결과가 NO일 경우에는 S3032로 진행하고, 금회 보용 순시값이 결정된다. S3032에서는, 목표 ZMP 주위에 상기 식d27a에 따라서 결정되는 모델 조작 상반력 모멘트 수평성분이 발생하도록 금회 보용 순시값이 결정된다. 단, 금회 보용에 균형을 이루는(금회 보용의 운동 의 관성력과 중력과의 합력이 목표 ZMP 주위에 발생하는 관성력 모멘트의 연직성분에 균형을 이루는) 상반력 모멘트 연직성분이 상반력 모멘트 연직성분 허용범위를 초과하지 않도록 금회 보용 순시값이 결정된다.

구체적으로는, 도 57, 도 58의 플로우차트에 따라서 보용 순시값이 결정된다. 즉, S3030에서는 먼저, 도 57의 S3400부터 S3411까지의 처리가 실행된다. 이들 S3400부터 S3411까지의 처리는 전술한 도 45의 S1400부터 S1411까지의 처리와 완전히 동일하다.

이어서, S3412로 진행하고, 상기 도 56의 S3030에서 결정한 컴플라이언스 제어용의 상반력 모멘트 수평성분 허용범위 및 상반력 모멘트 연직성분 보상량 허용 범위의 패러미터를 기초로, 현재 시각에서의 상반력 모멘트 수평성분 허용범위 [Mxymin, Mxymax]와 상반력 모멘트 연직성분 보상량 허용범위 [Mzcmin, Mzcmax]의 순시값(현재 시각 t의 금회값)이 구해진다.

구해진 상반력 모멘트 수평성분 허용범위는 상기 보상 전체 상반력 모멘트 수평성분 분배기(110)(도 53 참조)로 보내진다. 그리고, 이 분배기(110)에서 상기 식d27a에 의해 산출된 모델 조작 상반력 모멘트의 금회값(현재 시각 t의 값)이 보용생성 장치(100)에 주어진다.

또, 구해진 상반력 모멘트 연직성분 보상량 허용범위는 상기 모델 조작 상반력 모멘트 연직성분 결정기(111)(도 53 참조)로 보내진다. 그리고, 이 모델 조작 상반력 모멘트 연직성분 결정기(111)에서 상기 식d26b에 의해 산출된 모델 조작 상반력 모멘트 연직성분의 금회값(현재 시각 t의 값)이 보용생성 장치(100)에 주어진 다.

이어서, 보용생성 장치(100)의 처리는 S3414로 진행하고, 보상 전체 상반력 모멘트 분배기(110)로부터 주어진 모델 조작 상반력 모멘트 수평성분이 목표 ZMP 주위에 발생하도록, 금회 보용의 상체 수평가속도와 상체자세 경사 각가속도가 결정된다. 단, 상반력 수평성분 Fx가 S3410에서 결정된 상반력 수평성분 허용범위 [Fxmin, Fxmax]를 초과하지 않도록 상체 수평가속도 및 상체자세 각가속도(상체 경사 각가속도)가 결정된다.

바꿔 말하면, 로봇(1)의 운동의 관성력과 중력과의 합력이 목표 ZMP 주위에 작용하는 모멘트 수평성분이 모델 조작 상반력 모멘트 수평성분의 부호를 반전한 모멘트가 되도록, 금회 보용의 상체 수평가속도와 상체자세 각가속도(상체 경사 각가속도)가 결정된다. 단, 관성력 수평성분의 부호를 반전하는 힘이 상반력 수평성분 허용범위 [Fxmin, Fxmax]를 초과하지 않도록 상체 수평가속도 및 상체자세 경사 각가속도가 결정된다.

S3414에서는 구체적으로는, 도 58에 도시하는 플로우차트에 따라, 상체 수평가속도와 상체자세 각가속도가 결정된다. 본 플로우차트는 S3104 및 S3130을 제외하고, 상기 도 26과 동일한 처리가 행해진다. S3104 및 S3130에서는, 도 26의 S504 및 S530과 달리, 로봇(1)의 전회 순간 보용상태(시각 k-1의 보용상태)로부터, 상체 경사 모드의 각가속도를 0으로 하고(보다 정확하게는 상체 경사 모드의 각가속도를 기준 상체자세 각가속도에 일치시키고), 상체 병진 모드의 운동을 로봇(1)에게 시킨 경우에 금회(시각 k)의 목표 ZMP 주위에 상기 모델 조작 상반력 모멘트 수평성분이 발생하기 때문에 필요한 상체 수평가속도(S3104에서는 αtmp, S3130에서는 α)가 구해진다.

이것 이외의 처리는 도 26의 처리와 동일하다.

이상과 같이 S3414의 처리를 행한 후, S3416으로 진행하고, 도 45의 S1414와 동일한 처리가 실행되고, 금회 보용의 상체 수평위치 및 상체자세 경사각(상세하게는 그것들의 현재 시각 t에서의 금회값)이 결정된다.

이어서 S3418로 진행하고, 도 45의 S1416과 동일한 처리가 실행되고, 금회 보용의 역위상 팔 흔들기 각 및 각속도(상세하게는 그것들의 현재 시각 t에서의 금회값)이 결정된다.

이것에 의해, 도 56의 S3032의 처리가 완료된다.

이어서 도 56의 S3034로 진행하고, 목표 ZMP 주위에 모델 조작 상반력 모멘트 연직성분이 부가적으로 발생하도록, S3032에서 생성한 금회 보용 순시값을 수정한다.

구체적으로는, 모델 조작 상반력 모멘트 연직성분에 대응하는 역위상 팔 흔들기 각가속도의 수정량 βaadd를 다음 식에 의해 구한다.

역위상 팔 흔들기 각가속도의 수정량 βaadd

= 모델 조작 상반력 모멘트 연직성분

/역위상 팔 흔들기 운동의 등가 관성 모멘트 △Maz

또한, βaadd를 제어주기의 기간 적분하고, 역위상 팔 흔들기 각속도의 수정량을 구하고, 더욱 이것을 적분하여 역위상 팔 흔들기 각의 수정량을 구한다.

최후에, S3032에서 생성한 금회 보용의 역위상 팔 흔들기 각 및 각속도에 각각, 역위상 팔 흔들기 각속도의 수정량 및 역위상 팔 흔들기 각의 수정량을 부가함으로써, 금회 보용의 역위상 팔 흔들기 각 및 각속도를 수정한다.

이어서 S3036으로 진행하고, 시각 t에 제어주기 △t를 더하고, 다시, S3014로 되돌아오고, 제어주기마다의 타이머 인터럽션를 기다린다.

제 2 참고예에서는 이상과 같이, 보용생성 장치(100)에서의 목표 보용생성처리가 행해지고, 목표 상체위치 자세, 목표 족평위치 자세, 목표 팔자세(역위상 팔 흔들기 각을 포함함), 목표 ZMP 및 목표 전체 상반력의 순시값이 축차 결정되어서 출력된다.

이 경우, 목표 전체 상반력은 컴플라이언스 제어에 필요한 성분만을 출력해도 좋다. 또한, 목표 ZMP는 목표 전체 상반력에 포함되지만 특히 중요하므로 굳이 출력으로서 거론했다. 또, 복합 컴플라이언스 제어장치(도 53의 점선으로 둘러싼 부분)에 대해서는, 모델 조작 상반력 모멘트 수평성분은 목표 상반력으로서 출력되지 않는다. 즉, 컴플라이언스 제어에 대해서는, 목표 ZMP 주위의 상반력 모멘트 수평성분이 0이 되는 것을 목표로 하는 목표 상반력(본래의 의미에서의 목표 ZMP를 만족하는 목표 상반력)이 보용생성 장치(100)로부터 출력된다.

또, 복합 컴플라이언스 제어장치에 대해서는, S3034에서 수정된 금회 보용의 상반력 모멘트 연직성분이 목표값으로서 보용생성 장치(100)로부터 출력된다.

이상 설명한 제 2 참고예의 작용 1로서, 목표 보용의 운동은 모델 조작 상반력 모멘트 수평성분이 목표 ZMP 주위에 발생하도록 생성되면서, 실제 로봇(1)의 상 반력은 모델 조작 상반력 모멘트 수평성분이 부가되지 않도록 제어된다. 따라서, 차감 모델 조작 상반력 모멘트 수평성분의 분만, 목표 보용의 운동과 상반력의 불평형을 발생한다. 이것은, 실제 로봇(1)의 상체자세 경사각과 목표 보용의 상체자세 경사각의 차를 0에 수렴시키는 효과에 관하여 말하면, 모델 조작 상반력 모멘트 수평성분의 부호를 반전한 상반력 모멘트 수평성분을 실제 로봇(1)에 작용시키는 것과 등가이다.

즉, 모델 조작 상반력 모멘트 수평성분을 적당하게 결정함으로써, 실제 로봇(1)을 수정 목표 보용(실제 로봇(1)의 상체자세 경사각과 목표 보용의 상체자세 경사각과의 차를 0에 수렴시키는 보용)에 수렴시킬 수 있다. 즉, 실제 로봇(1)의 자세 경사를 안정화 시킬 수 있다.

작용 2로서, 모델 조작 상반력 모멘트 수평성분의 부호를 반전한 모멘트와 컴플라이언스 제어용 목표 상반력 모멘트 수평성분과의 합이 토털의 경사 복원력(로봇(1)의 실제 상체자세 경사각을 목표 상체자세 경사각에 복원시키는 힘)이 된다. 즉, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분과 모델 조작 상반력 모멘트 수평성분과의 차가 토털의 자세 경사 복원력이 된다.

또, 작용 3으로서, 모델 조작 상반력 모멘트 수평성분은 ZMP 존재가능 범위를 무시하고 어떠한 값으로도 취할 수 있으므로, 대단히 높은 자세 경사 복원력을 발생할 수 있다.

또, 작용 4로서, 상반력 수평성분이 상반력 수평성분 허용범위를 초과하지 않도록 상체 병진 모드의 상체 병진 가속도와 상체 경사 모드의 상체자세 경사 각 가속도가 결정되므로, 주행 보용에서의 지지다리측 다리체(2)의 이상 직전이나 착상 직후 등, 큰 상반력 수평성분을 발생할 수 없는 시기나, 마찰계수가 작은 바닥을 로봇(1)이 이동하는 경우에 있어서도 로봇(1)의 슬립을 방지할 수 있다.

또, 작용 5로서, 상반력의 병진력 연직성분이 0의 시기, 즉, 양 다리가 모두 접지해 있지 않은 시기에서는, 상반력 수평성분의 허용범위가 0으로 설정됨으로써, 상기한 제 2 참고예의 알고리즘에 의해 자동적으로, 상체 병진 모드에 의존하지 않고 상체 경사 모드에 의존한 자세 경사 복원이 행해져, 바닥과 발밑바닥 사이의 마찰력에 의존하지 않고 자세 복원이 행해진다. 따라서, 이 시기(공중기)에서도, 단순히, 상체 병진 모드를 수정하는 것만의 방식과 달리, 유효하게 자세 경사 복원작용이 작용한다. 또한, 이때에는 상반력 수평성분이 0이 되도록 생성되기 때문에, 보용의 전체 중심 수평가속도는 0이 된다.

또, 작용 6으로서, 상기한 바와 같이, 컴플라이언스 제어에 대해서는 모델 조작 상반력 모멘트 수평성분은 목표 상반력으로서 출력되지 않는다. 즉, 목표 ZMP 주위에 모델 조작 상반력 모멘트 수평성분을 발생시키도록 보용생성이 행해져도, 컴플라이언스 제어에 대해서는, 목표 ZMP 주위의 상반력 모멘트 수평성분이 0이 되는 것을 목표로 하는 목표 상반력이 보용생성 장치(100)로부터 주어진다. 따라서, 컴플라이언스 제어에 의한 상반력 제어를 방해하지 않고 컴플라이언스 제어에 의한 상반력 제어를 적절하게 행할 수 있다. 보다 구체적으로는 족평(22)의 본래의 접지성이 저하되거나, 족평(22)의 밑바닥면이 떠버리는 것과 같은 문제가 발생하는 것을 방지 또는 억제할 수 있다.

또한, 후술하는 바와 같이, 제 3 참고예 이후에서도, 목표 ZMP 주위의 컴플라이언스 제어용 목표 상반력 모멘트 수평성분은 상반력 모멘트 수평성분 허용범위를 초과하지 않도록 결정된다.

또한, 작용 1부터 6은 본원 출원인이 먼저 제안한 PCT/JP03/00435의 기술과 동일하다.

작용 7로서, 목표 보용의 운동은 모델 조작 상반력 모멘트 연직성분이 목표 ZMP 주위에 부가적으로 발생하도록 생성되면서, 실제의 로봇(1)의 상반력은 보용생성 장치(100)에 의해 모델 조작 상반력 모멘트 연직성분이 부가된 목표 보용에 균형을 이루는 목표 상반력 모멘트 연직성분에 보상 전체 상반력 모멘트 연직성분 Mdmdz를 더한 것을 목표값으로 하여, 복합 컴플라이언스 제어에 의해 상기 목표값에 근접하도록 실제 상반력이 제어된다. Mdmdz가 커져가면, Mdmdz와 역방향의 모델 조작 상반력 모멘트 연직성분이 목표 보용에 부가되므로, 복합 컴플라이언스 제어에 의해 상기 목표값에 근접하도록 실제 상반력을 제어하도록 해도, 실제 상반력의 모멘트의 연직성분이 과대하게 되지 않고, 그 결과, 실제 로봇(1)의 스핀을 발생하지 않아, 실제 로봇(1)의 상체자세 요잉각 및/또는 상체자세 요잉각속도와 목표 보용의 상체자세 요잉각 및/또는 요잉각속도와의 차를 0에 수렴시키는 효과를 이룰 수 있다.

즉, 모델 조작 상반력 모멘트 연직성분을 적당히 결정함으로써, 실제 로봇(1)을 그 스핀이 발생하지 않도록 하면서 수정 목표 보용(실제 로봇(1)의 상체자세 요잉각 및/또는 상체자세 요잉각속도와 목표 보용의 상체자세 요잉각 및/또는 상체 자세 요잉각속도와의 차를 0에 수렴시키는 보용)에 수렴시킬 수 있다. 즉, 실제 로봇(1)의 요잉 회전을 안정화시킬 수 있다.

작용 8로서, 보상 전체 상반력 모멘트 연직성분 Mdmdz가 토털 요잉 회전 복원력이 된다. 보상 전체 상반력 모멘트 연직성분 Mdmdz는 요잉각 편차 및/또는 요잉각속도 편차를 0에 근접시키도록 피드백제어칙에 기초하여 결정되므로, 요잉각 편차의 제어적인 안정성이 보증되면서, 요잉각 편차 및/또는 요잉각속도 편차를 0에 근접시킬 수 있다.

또, 작용 9로서, 모델 조작 상반력 모멘트 연직성분은 상반력 모멘트 연직성분의 허용범위(또는 마찰한계)를 무시하고 어떠한 값으로도 취할 수 있으므로, 대단히 높은 자세 요잉 회전 복원력을 발생할 수 있다.

또, 작용 10으로서, 컴플라이언스 제어에 대해서는, 최종적으로 목표 상반력 모멘트 연직성분이 상반력 모멘트 연직성분 허용범위와 상반력 모멘트 연직성분 보상량 허용범위의 합의 범위를 초과하지 않도록 결정되므로, 주행 보용에서의 지지다리측 다리체(2)의 이상 직전이나 착상 직후 등, 큰 상반력 모멘트 연직성분을 발생할 수 없는 시기나, 마찰 계수가 작은 바닥을 로봇(1)이 이동하는 경우에 있어서도 컴플라이언스 제어에 의한 상반력 제어를 적절하게 행할 수 있어, 로봇(1)의 스핀을 방지할 수 있다. 보다 구체적으로는, 족평(22)의 본래의 접지성이 저하되거나, 족평(22)의 밑바닥면이 떠버리는 것과 같은 문제가 발생하는 것을 방지 또는 억제할 수 있다.

또, 작용 11로서, 상반력의 병진력 연직성분이 0의 시기, 즉, 양 다리가 모 두 접지하고 있지 않은 시기에서는, 상반력 모멘트 연직성분 허용범위와 상반력 모멘트 연직성분 보상량 허용범위가 0으로 설정됨으로써, 상기한 본 제 2 참고예의 알고리즘에 의해 자동적으로, 실제 상반력 모멘트 연직성분에 의존하지 않고 역위상 팔 흔들기 모드에 의존한 요잉 회전 복원이 행해져, 바닥과 발밑바닥 사이의 마찰력에 의존하지 않고 요잉 회전 복원이 행해진다. 따라서, 이 시기(공중기)에서도, 단순히, 컴플라이언스 제어의 목표 상반력 모멘트 연직성분을 수정하는 것만의 방식과 달리, 유효하게 요잉 회전 복원작용이 작용한다.

작용 12로서, 목표 ZMP 주위에 발생하는 모멘트 수평성분이 0이 되도록 생성했다고 한 경우의 보용을 원보용, 상기 제 2 참고예와 같이 목표 ZMP 주위에 발생하는 모멘트 수평성분이 모델 조작 상반력 모멘트 수평성분이 되고, 또한, 목표 ZMP 주위에 발생하는 모멘트 연직성분에 부가적으로 모델 조작 상반력 모멘트 연직성분이 발생하도록 생성되는 보용을 수정 보용이라고 부르면, 통상, 원보용과 수정 보용과는 상이한 보용이 된다. 원보용은 정상 보용에 점근하도록 설정되어 있으므로, 수정 보용은 통상 정상 보용에 점근하지 않는 보용이 된다.

그러나, 금회 보용(수정보용)의 생성이 완료된 직후에, 다시 S3020부터 S3028이 실행되고, 수정 보용의 종단 상태를 새로운 초기 상태로 한 새로운 금회 보용이, 새롭게 설정되는 정상 보용에 점근하도록, 새로운 금회 보용 패러미터가 결정되므로, 계속적으로(장기적으로) 안정성이 보증된 보용을 계속 생성할 수 있다.

상기한 작용 12는 본원 출원인이 먼저 제안한 PCT/JP03/60435의 기술과 거의 동일하다. 단, 이것에 더하여 본 제 2 참고예에서는 다음과 같은 작용도 발생한다. 즉, 요잉각 회전 복원을 위해 수정된 역위상 팔 흔들기 각 및 각속도의 종단 상태를 새로운 초기 상태로 한 새로운 금회 보용의 역위상 팔 흔들기 각 궤도가 새롭게 설정되는 정상 보용의 역위상 팔 흔들기 각 궤도에 점근하도록, 새로운 금회 보용의 역위상 팔 흔들기 각 궤도에 관한 패러미터가 결정되므로, 계속적으로(장기적으로) 역위상 팔 흔들기 각의 안정성이 보증된 보용을 계속 생성할 수 있다.

또한, 본 제 2 참고예에서는 보상 전체 상반력 모멘트 수평성분 Mdmdxy가 상반력 모멘트 수평성분 허용범위내의 값일 때에는, 모델 조작 상반력 모멘트 수평성분은 0이 되지만, 이때의 모델 조작 상반력 모멘트 수평성분을 도 12의 동역학 모델의 상태량(예를 들면 동역학 모델상에서의 로봇의 중심위치나, 상체 질점 3m의 위치 등)에 따라 설정하도록 해도 좋다.

또, 본 제 2 참고예에서는 보상 전체 상반력 모멘트 연직성분 Mdmdz가 상반력 모멘트 연직성분 보상량 허용범위내의 값일 때에는, 모델 조작 상반력 모멘트 연직성분 보상량은 0이 되지만, 이때의 모델 조작 상반력 모멘트 연직성분 보상량을 도 12의 동역학 모델의 상태량(예를 들면 동역학 모델상에서의 로봇(1)의 역위상 팔 흔들기 각 및 각속도, 상체 요잉각 및 각속도 등)에 따라 설정하도록 해도 좋다.

다음에 본 발명의 제 3 참고예를 도 59∼도 62를 참조하여 설명한다. 또한, 제 3 참고예의 설명에서, 상기 제 1 참고예 혹은 상기 제 2 참고예와 동일구성 부분 혹은 동일기능 부분에 대해서는 상기 제 1 참고예 혹은 제 2 참고예와 동일한 참조부호를 사용하고 설명을 생략한다. 제 3 참고예는, 상기 제 1 참고예에서 상기 도 1∼도 3 및 도 5∼도 12를 참조하여 설명한 사항은 상기 제 1 참고예와 동일하다.

제 3 참고예가 상기 제 1 참고예 및 제 2 참고예와 상이한 점의 개요를 설명하면, 원보용과 수정보용이 동시에 생성된다. 수정보용은, 실제 로봇(1)의 상체자세(경사각 및 요잉각)의 안정화를 위해 원보용으로부터 수정된다. 또한, 이 수정보용은 컴플라이언스 제어에 의해 자세 복원에 필요한 상반력 모멘트를 발생해도 아직 여유가 있는(목표 ZMP 주위에 발생가능한 상반력 모멘트에 여유가 있는) 경우에는 이 여유를 사용하여, 가능한 범위에서 원보용에 수렴하도록 결정된다.

제 3 참고예에서의 제어 유닛(60)의 기능적 구성을 나타내는 블럭도를 도 59에 도시한다. 제 3 참고예에서는 자세 경사 안정화 제어 연산부(112)가 구하는 보상 전체 상반력 모멘트 수평성분 Mdmdxy는, 보용생성 장치(100)에 입력된다.

또, 요잉 안정화 제어 연산부(113)가 구하는 보상 전체 상반력 모멘트 연직성분 Mdmdz도 보용생성 장치(100)에 입력된다.

그리고, 이 Mdmdxy 및 Mdmdz를 기초로 모델 조작 상반력 모멘트(수평성분 및 연직성분)와 컴플라이언스 제어용 목표 상반력 모멘트(수평성분 및 연직성분)를 결정하는 보상 전체 상반력 모멘트 분배기(120)가 보용생성 장치(100)에 편입되고, 이 보용생성 장치(100)로부터 컴플라이언스 제어용 목표 상반력 모멘트가 복합 컴플라이언스 동작 결정부(104)에 출력된다. 그리고, 보용생성 장치(100)내의 보상 전체 상반력 모멘트 분배기(120)는 후술하는 바와 같이, 상기 제 2 참고예의 보상 전체 상반력 모멘트 수평성분 분배기(110) 및 모델 조작 상반력 모멘트 연직성분 결정기(111) 보다도 복잡한 처리를 행한다. 이것 이외의 제어 유닛(60)의 기능적 구성은 상기 제 2 참고예와 동일하다.

도 60에 제 3 참고예에서의 보용생성 장치(100)의 메인루틴 처리의 플로우차트를 도시한다.

동 도 60에서, S2010부터 S2028까지는, 상기 제 1 참고예의 메인 플로우차트(도 13)의 S010부터 S028까지의 처리와 동일한 처리가 행해진다. 또한, S028(본 실시형태에서는 S2028)의 서브루틴인 도 43의 플로우차트의 S800에서의 초기화에 있어서는, 금회 보용 초기 상태는 전회 수정보용(보용생성 장치(100)가 최종적으로 출력하는 보용)의 종단상태를 금회 보용 지지다리 좌표계로 변환한 것이며, 후술의 S2032에서 결정되는 원보용의 종단상태는 S2028의 서브루틴 S800에서는 사용할 수 없다.

이어서 S2030으로 진행하고, 컴플라이언스 제어용의 상반력 모멘트 수평성분 허용범위를 결정한다. 이 상반력 모멘트 수평성분 허용범위의 결정방법은 제 2 참고예의 S3032(도 56)와 동일하다.

S2030의 처리를 완료한 후, 또는 S2016의 판단 결과가 NO일 경우에는, S2032로 진행하고, 원보용의 순시값(시각 t의 금회값)이 결정된다. 원보용은 목표 ZMP 주위의 상반력 모멘트 수평성분이 0이 되도록 생성되는 보용이다.

이 원보용은, 상기 제 2 참고예의 도 56의 S3032의 서브루틴 처리의 일부를 변경한 알고리즘에 의해 생성된다. 즉, S3032내의 서브루틴 처리(상세하게는 S3032의 서브루틴 처리인 도 57의 S3414의 서브루틴 처리)인 도 58의 S3104 및 S3130에 있어서, 모델 조작 상반력 모멘트 수평성분을 0으로 하여(목표 ZMP 주위의 목표 상반력 모멘트 수평성분을 0으로 하여), 상체 수평가속도 αtmp가 구해진다. 이것 이외의 처리는 도 56의 S3032의 처리와 동일해도 좋다.

이어서, S2034로 진행하고, 수정보용의 순시값을 결정한다. 또한, 수정보용은 보용생성 장치(100)로부터 최종적으로 출력되는 목표 보용이다.

S2034의 처리는, 도 61의 플로우차트에서 나타내는 서브루틴 처리에 의해 행해지고, 이하에 그 상세를 설명한다.

먼저 S2100으로부터 S2111까지는, 제 2 참고예에서 설명한 도 57의 S3400부터 S3411과 동일한 처리가 실행된다.

이어서, S2112로 진행하고, 보용 패러미터를 기초로 현재 시각에서의 상반력 모멘트 수평성분 허용범위 [Mxymin, Mxymax]를 구한다. 이것은 도 57의 S3412에서의 상반력 모멘트 수평성분 허용범위 [Mxymin, Mxymax]의 결정과 동일하게 행해진다.

이어서, S2114로 진행하고, 상반력 모멘트 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위와 상반력 수평성분 허용범위의 조건을 만족하도록, 모델 조작 상반력 모멘트(수평성분 및 연직성분), 컴플라이언스 제어용 목표 상반력 모멘트, 상체 수평가속도, 상체자세 경사 각가속 및 역위상 팔 흔들기 각가속도가 결정된다.

S2114의 상세를 그 처리를 도시하는 도 62의 플로우차트를 사용하여 이하에 설명한다.

먼저, S2200에서, 수정보용의 상체 수평위치와 원보용의 상체 수평위치와의 차인 모델간 상체 수평위치 차가 구해진다. 이 시점에서는, 수정보용의 상체 수평위치의 금회값(시각 t에서의 값)은 아직 정해져 있지 않다. 그래서, S2200에서는, 수정보용의 상체 수평위치의 전회값(시각 t-△t의 제어주기에서 최종적으로 구해진 값)과, 원보용의 상체 수평위치의 전회값(시각 t-△t의 제어주기에서 S2032에서 구해진 값) 또는 금회값(시각 t의 제어주기에서 S2032에서 구해진 값)을 사용하여, 모델간 상체 수평위치 차가 산출된다.

이어서, S2202로 진행하고, 수정보용의 상체자세 경사각과 원보용의 상체자세 경사각과의 차인 모델간 상체자세 경사각 차가 구해진다. 이 S2202에서는 S2200의 모델간 상체 수평위치 차의 산출 처리의 경우와 동일하게, 수정보용의 상체자세 경사각의 전회값과, 원보용의 상체자세 경사각의 전회값 또는 금회값을 사용하여 모델간 상체자세 경사각 차가 구해진다.

이어서, S2204로 진행하고, 수정보용의 역위상 팔 흔들기 각과 원보용의 역위상 팔 흔들기 각과의 차인 모델간 역위상 팔 흔들기각 차가 구해진다. 이 S2204에서는, S2200의 모델간 상체 수평위치 차의 산출 처리의 경우와 동일하게, 수정보용의 역위상 팔 흔들기 각의 전회값과, 원보용의 역위상 팔 흔들기 각의 전회값 또는 금회값을 사용하여 모델간 역위상 팔 흔들기 각 차가 구해진다.

이어서, S2206으로 진행하고, 모델간 상체 수평위치 차를 기초로, 그 차를 0에 수렴시키기 위해서 필요한 모델 상체 수평위치 안정화 상반력 모멘트 요구값 Mpfdmd가 결정된다. 단지 수정보용의 상체 병진 모드의 상체 수평가속도를 발생시키는 상반력 모멘트를 원보용의 상체 병진 모드의 상체 수평가속도를 발생시키는 상반력 모멘트에 일치시키면, 모델간 상체 수평위치 차는 발산한다. 모델 상체 수평위치 안정화 상반력 모멘트 요구값 Mpfdmd는, 상기 상체 병진 모드에 의해 수정보용의 상체 수평위치를 원보용의 상체 수평위치로 되돌아가도록 동작을 행했을 때에, 이것에 따라 발생하는 상반력 모멘트로부터 원보용의 상체 병진 모드의 상체 수평가속도를 발생시키는 상반력 모멘트를 뺀 모멘트로서의 의미를 갖는다.

구체적으로는, 모델 상체 수평위치 안정화 상반력 모멘트 요구값 Mpfdmd는 예를 들면 다음 식의 피드백제어칙에 의해 결정된다. 이 예에서는, 피드백제어칙으로서, PD제어칙을 사용하고 있지만, PID 등, 다른 피드백제어칙을 사용해도 좋다.

Mpfdmd = Kmp*모델간 상체 수평위치 차

+Kmpv*모델간 상체 수평위치 차의 시간 미분값

……식d28

단, Kmp, Kmpv는 피드백 게인(비례 게인, 미분 게인)이다.

이어서, S2208로 진행하고, 모델간 상체자세 경사각 차를 기초로, 그 차를 0에 수렴시키기 위해서 필요한 모델 상체자세 경사각 안정화 상반력 모멘트 요구값 Mrfdmd가 결정된다. 단지 수정보용의 상체 경사 모드의 상체자세 경사 각가속도를 발생시키는 상반력 모멘트를 원보용의 상체 경사 모드의 상체자세 경사 각가속도를 발생시키는 상반력 모멘트에 일치시키면, 모델간 상체자세 경사각 차는 0에 수렴하 지 않는다. 모델 상체자세 경사각 안정화 상반력 모멘트 요구값 Mrfdmd는 상체 경사 모드에 의해 수정보용의 상체자세 경사각을 원보용의 상체자세 경사각으로 되돌리도록 동작을 행했을 때에, 이것에 따라 발생하는 상반력 모멘트로부터 원보용의 상체 경사 모드의 상체자세 경사 각가속도를 발생시키는 상반력 모멘트를 뺀 모멘트로서의 의미를 갖는다.

구체적으로는, 모델 상체자세 경사각 안정화 상반력 모멘트 요구값 Mrfdmd는 예를 들면 다음 식의 피드백제어칙에 의해 결정된다. 이 예에서는, 피드백제어칙으로서, PD제어칙을 사용하고 있지만, PID 등, 다른 피드백제어칙을 사용해도 된다.

Mrfdmd = Kmr*모델간 상체자세 경사각 차

+Kmrv*모델간 상체자세 경사각 차의 시간 미분값

……식d29

단, Kmr, Kmrv는 피드백 게인(비례 게인, 미분 게인)이다.

이어서, S2210으로 진행하고, 모델간 역위상 팔 흔들기 각 차를 기초로, 그 차를 0에 수렴시키기 위해 필요한 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트 요구값 Mafdmd가 결정된다. 단지 수정보용의 역위상 팔 흔들기 모드의 역위상 팔 흔들기 각가속도를 발생시키는 상반력 모멘트를 원보용의 역위상 팔 흔들기 모드의 역위상 팔 흔들기 각가속도를 발생시키는 상반력 모멘트에 일치시키면, 모델간 역위상 팔 흔들기 각 차이는 0으로 수렴하지 않는다. 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트 요구값 Mafdmd는, 역위상 팔 흔들기 모드에 의해 수정보용의 역위상 팔 흔들기 각을 원보용의 역위상 팔 흔들기 각으로 되돌리도록 동작을 행했을 때에, 이것에 따라 발생하는 상반력 모멘트로부터 원보용의 역위상 팔 흔들기 모드의 역위상 팔 흔들기 각가속도를 발생시키는 상반력 모멘트를 뺀 모멘트로서의 의미를 갖는다.

구체적으로는, 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트 요구값 Mafdmd는 예를 들면 다음 식의 피드백제어칙에 의해 결정된다. 이 예에서는, 피드백제어칙으로서, PD제어칙을 사용하고 있지만, PID 등, 다른 피드백제어칙을 사용해도 좋다.

Mafdmd = Ka* 모델간 역위상 팔 흔들기 각 차

+Kav*모델간 역위상 팔 흔들기 각 차의 시간 미분값

……식d29b

단, Kar, Kav는 피드백 게인(비례 게인, 미분 게인)이다.

그런데, 최종적으로 결정된 수정보용의 상체 병진 모드에 따라 발생하는 상반력 모멘트 수평성분으로부터 원보용의 상체 병진 모드의 상체 수평가속도를 발생시키는 상반력 모멘트 수평성분을 뺀 모멘트를, 모델 상체 수평위치 안정화 상반력 모멘트라고 부른다. 또, 최종적으로 결정된 수정보용의 상체 경사운동 모드에 따라 발생하는 상반력 모멘트 수평성분으로부터 원보용의 상체 경사운동 모드의 상체자세 경사 각가속도를 발생시키는 상반력 모멘트 수평성분을 뺀 모멘트를 모델 상체자세 경사각 안정화 상반력 모멘트라고 부른다. 또, 최종적으로 결정된 수정보용의 역위상 팔 흔들기 모드에 따라 발생하는 상반력 모멘트 연직성분으로부터 원 보용의 역위상 팔 흔들기 모드의 역위상 팔 흔들기 각가속도를 발생시키는 상반력 모멘트 연직성분을 뺀 모멘트를 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트라고 부른다.

한편, 섭동운동과 섭동 상반력과는 선형성이 근사적으로 성립한다, 즉, 상이한 섭동운동을 원보용의 운동에 부가한 운동의 상반력은 원보용의 상반력에 각각의 섭동운동이 발생하는 섭동 상반력을 부가한 것과 거의 일치한다. 또, 역위상 팔 흔들기 모드에서는 상반력 모멘트 수평성분은 변화하지 않는다. 따라서, 다음 식이 근사적으로 성립한다.

모델 조작 상반력 모멘트 수평성분

= 모델 상체 수평위치 안정화 상반력 모멘트

+ 모델 상체자세 경사각 안정화 상반력 모멘트

……식d30

이 식d30이 근사적으로 성립하는 것과, 역위상 팔 흔들기 각가속도에 비례하여 상반력 모멘트 연직성분이 변화하는 것을 고려하면서, 모델 상체 수평위치 안정화 상반력 모멘트를 모델 상체 수평위치 안정화 상반력 모멘트 요구값 Mpfdmd에 일치 또는 가능한 한 근접하도록 결정하고, 또한, 모델 상체자세 경사각 안정화 상반력 모멘트를 모델 상체자세 경사각 안정화 상반력 모멘트 요구값Mrfdmd에 일치 또는 가능한 한 근접하도록 결정하면서, 또한, 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트를 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트 요구값 Mafdmd에 일치 또는 가능한 한 근접하도록 결정하면, 수정보용에 적절한 모델 조작 상반력 모멘트를 발생시켜서, 이하에 나타내는 복원조건을 만족하면서, 수정보용의 상체 수평가속도와 상체자세 경사 각가속도를, 각각 원보용의 상체 수평가속도와 상체자세 경사 각가속도에 가능한 범위에서 수렴시킬 수 있다.

그래서, S2210의 후, S2212로 진행하고, 이하의 조건(이것들을 복원조건이라고 부름)을 가능한 한 만족하도록 모델 상체 수평위치 안정화 상반력 모멘트(상체 병진 모드의 상반력 모멘트)와 모델 상체자세 경사각 안정화 상반력 모멘트(상체 경사 모드의 상반력 모멘트)와 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트를 결정한다. 또한, 상기의 모델 상체 수평위치 안정화 상반력 모멘트와 모델 상체자세 경사각 안정화 상반력 모멘트와 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트의 정의를 만족하도록, 수정보용의 상체 수평가속도와 상체자세 경사 각가속도와 역위상 팔 흔들기 각가속도를 결정한다. 또한, 이하에 도시하는 복원조건은 번호가 작은 조건일 수록 우선도가 높은 것으로 한다. 즉 상반하여 양립할 수 없는 조건이 있는 경우에는 번호가 작은 조건을 우선하여 만족(성립)시킨다. 단, 복원조건 1, 2 및 3은 반드시 만족(성립)시킨다.

복원조건 1) 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 모델 조작 상반력 모멘트의 합(이것은, 상기 식d27b가 성립하는 경우에는, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분에 상당함)이 상반력 모멘트 수평성분 허용범위를 초과하지 않을 것.

복원조건 2) 수정보용의 상반력 수평성분이 상반력 수평성분 허용범위를 초과하지 않을 것.

복원조건 3) 수정보용의 상반력 모멘트 연직성분과 보상 전체 상반력 모멘트 연직성분 Mdmdz의 합(이것은, 컴플라이언스 제어용 목표 상반력 모멘트 연직성분에 상당함)이 상반력 모멘트 연직성분 허용범위를 초과하지 않을 것.

복원조건 4) 모델 상체자세 경사각 안정화 상반력 모멘트가 모델 상체자세 경사각 안정화 상반력 모멘트 요구값 Mrfdmd에 일치 또는 가능한 한 가까울 것. 이 조건은, 수정보용의 상체자세 경사각이 원보용(당초 예정한 보용)의 상체자세 경사각에 수렴하기 위한 조건이다.

복원조건 5) 모델 상체 수평위치 안정화 상반력 모멘트가 모델 상체 수평위치 안정화 상반력 모멘트 요구값 Mpfdmd에 일치 또는 가능한 한 가까울 것. 이 조건은 수정보용의 상체 수평위치가 원보용(당초 예정한 보용)의 상체 수평위치에 수렴하기 위한 조건이다.

복원조건 6) 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트가 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트 요구값 Mafdmd에 일치 또는 가능한 한 가까울 것. 이 조건은 수정보용의 역위상 팔 흔들기 각이 원보용(당초 예정한 보용)의 역위상 팔 흔들기 각에 수렴하기 위한 조건이다.

복원조건 7) 모델 상체자세 경사각 안정화 상반력 모멘트, 모델 상체 수평위치 안정화 상반력 모멘트 및 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트가 각각 연속일 것.

상기와 같은 복원조건 1∼6을 만족하는 상체 수평가속도, 상체자세 경사 각가속도, 역위상 팔 흔들기 각가속도 등을 결정하는 S2212의 처리는 구체적으로는 예를 들면 다음과 같이 실행된다.

먼저, 상기 복원조건 1, 2, 4 및 5를 만족하도록, 모델 상체 수평위치 안정화 상반력 모멘트 및 모델 상체자세 경사각 안정화 상반력 모멘트가 결정되고, 또한 상체 수평가속도 및 상체자세 경사 각가속도가 결정된다. 이 처리의 상세는, 본원 출원인이 먼저 제안한 PCT/JP03/00435의 기술에 기재되어 있으므로 여기에서는 설명을 생략한다.

이어서, 상기 복원조건 3 및 6을 만족하도록 모델 역위상 팔 흔들기 안정화 상반력 모멘트가 결정되고, 또한 역위상 팔 흔들기 각가속도가 결정된다.

구체적으로는, 상기와 같이 결정된 상체 수평가속도 및 상체자세 경사 각가속도와 원보용의 역위상 팔 흔들기 각가속도 βaorg로 가령 운동한 경우에 발생하는(운동에 동역학적으로 균형을 이루는) 목표 ZMP 주위의 상반력 모멘트 연직성분을 구한다. 이후, 이것을 무보정시 상반력 모멘트 연직성분이라 부른다.

이어서, 다음 식과 같이, 무보정시 상반력 모멘트 연직성분과 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트 요구값 Mafdmd와 보상 전체 상반력 모멘트 연직성분 Mdmdz와의 합 Msumz를 구한다.

Msumz = 무보정시 상반력 모멘트 연직성분

+Mafdmd+Mdmdz

이어서, 다음 식에 의해, 모델 역위상 팔 흔들기 안정화 상반력 모멘트가 결정된다.

Msumz>상반력 모멘트 연직성분 허용범위 상한값일 경우

모델 역위상 팔 흔들기 안정화 상반력 모멘트 = -Mdmdz

-무보정시 상반력 모멘트 연직성분

+상반력 모멘트 연직성분 허용범위 상한값

Msumz<상반력 모멘트 연직성분 허용범위 하한값일 경우

모델 역위상 팔 흔들기 안정화 상반력 모멘트 = -Mdmdz

-무보정시 상반력 모멘트 연직성분

+상반력 모멘트 연직성분 허용범위 하한값

상반력 모멘트 연직성분 허용범위 하한값≤Msumz, 또한, Msumz≤

상반력 모멘트 연직성분 허용범위 상한값일 경우

모델 역위상 팔 흔들기 안정화 상반력 모멘트

= 모델 역위상 팔 흔들기 각 안정화 상반력 모멘트 요구값 Mafdmd

……식d26c

이어서, 수정보용의 역위상 팔 흔들기 각가속도가 다음 식에 의해 구해진다.

수정보용의 역위상 팔 흔들기 각가속도

=원보용의 역위상 팔 흔들기 각가속도 βaorg

+모델 역위상 팔 흔들기 안정화 상반력 모멘트

/팔 흔들기 운동의 등가 관성 모멘트 △Maz

수정보용의 상반력 모멘트 연직성분은 원보용의 역위상 팔 흔들기 각가속도 βaorg와 모델 역위상 팔 흔들기 안정화 상반력 모멘트의 합이므로, 상기 식과 같이 모델 역위상 팔 흔들기 안정화 상반력 모멘트를 결정함으로써, 상기 복원조건 3 이 만족된다.

상기한 바와 같이 S2212의 처리를 실행한 후, S2214로 진행하고, 상기 식d30에 의해, 모델 조작 상반력 모멘트 수평성분이 결정된다. 즉, S2208에서 구한 모델 상체 수평위치 안정화 상반력 모멘트와 모델 상체자세 경사각 안정화 상반력 모멘트의 합이 모델 조작 상반력 모멘트 수평성분으로서 결정된다. 또한, 최종적으로 결정되는 수정보용의 운동의 금회 순시값에 기초하여 목표 ZMP 주위의 상반력 모멘트를 직접 산출하고, 이것을 가지고 모델 조작 상반력 모멘트로 해도 좋다.

이어서, S2216으로 진행하고, 상기 식d27b에 의해, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분이 결정된다. 즉, 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 S2214에서 구한 모델 조작 상반력 모멘트 수평성분과의 합이 컴플라이언스 제어용 목표 상반력 모멘트 수평성분으로서 결정된다.

이어서, S2218로 진행하고, 도시하는 식에 의해, 컴플라이언스 제어용 목표 상반력 모멘트 연직성분을 결정한다. 또한, 도시하는 식에서의 수정보용에 균형을 이루는(수정보용의 운동에 동역학적으로 균형을 이루는) 상반력 모멘트 연직성분은 무보정시 상반력 모멘트 연직성분과 모델 역위상 팔 흔들기 안정화 상반력 모멘트의 합이지만, 최종적으로 결정되는 수정보용의 운동의 금회 순시값에 기초하여 목표 ZMP 주위의 상반력 모멘트 연직성분을 직접 산출해도 좋다.

이상에 의해, 도 61의 S2114의 처리가 종료되고, 다음에 S2116으로 진행한다. 이 S2116의 처리는, 상기 제 2 참고예에서의 도 57의 S3416과 동일하고, 상체 수평가속도의 2차적분에 의해 상체 수평위치의 금회값이 결정되는 동시에, 상체자 세 경사 각가속도의 2차적분에 의해 상체자세 경사각의 금회값이 결정된다.

다음에 S2118로 진행한다. 이 S2118의 처리는, 상기 제 1 실시형태에서의 도 57의 S3418과 동일하고, 역위상 팔 흔들기 각가속도의 2차적분에 의해 역위상 팔 흔들기 각의 금회값이 결정된다.

이어서, 도 60의 S2036으로 진행하고, 시각 t에 제어주기 △t를 더하고, 다시, S2014로 되돌아와, 제어주기마다의 타이머 인터럽션를 기다린다.

보충하면, 제 3 참고예에서의 동역학 모델에 의한 보용 순시값 결정에서는 동역학 모델의 상태량(혹은 전회, 전전회의 보용 순시값)도 필요하므로, 수정보용 생성용과 원보용 생성용 2개의 동역학 모델이 필요하다. 본 실시형태에서는, 그것들의 동역학 모델은 도 12에 도시한 동역학 모델이다.

본 실시형태에서는, 상기와 같이, 원보용과 수정보용이 병렬적으로 생성되고, 수정보용은 실제 로봇(1)의 자세(경사각 및 요잉각) 안정화를 위해 수정되는 동시에, 컴플라이언스 제어에 의해 자세 복원에 필요한 상반력 모멘트(수평성분 및 연직성분)를 발생해도 아직 여유가 있을 경우에는 이 여유를 사용하여, 가능한 범위에서 원보용에 수렴하도록 하고 있다. 이것 때문에, 상기 제 2 참고예의 작용효과와 더불어, 당초 설정한 원보용에 가까운, 즉, 당초의 요구대로의 보용에 가까운 보용을 생성할 수 있다. 따라서, 미리 설정된 이동경로가 있을 경우에는, 이동경로로부터 크게 벗어나는 것을 방지하는 것이 가능하게 된다. 또, 수정보용의 상체자세 경사각이 원보용(당초 결정한 보용)의 상체자세 경사각에 수렴하는 것을, 수정보용의 상체 수평위치가 원보용(당초 결정한 보용)의 상체 수평위치에 수렴하는 것보다도 우선했(상반력 수평성분 허용범위를 만족시키는 범위에서 가능한 한 상체 병진 모드의 운동을 조정했)으므로, 상체자세 경사각이 크게 변동하는 것을 억제할 수 있다.

다음에 제 4 참고예를 도 63∼도 70을 참조하여 설명한다. 또한, 본 제 4 참고예의 설명에서, 상기 제 1∼제 3 참고예와 동일구성 부분 혹은 동일기능 부분에 대해서는 이 제 1∼제 3 참고예와 동일한 참조부호를 사용하고 상세한 설명을 생략한다.

본 제 4 참고예에서는 제어 유닛(60)의 기능적 구성은 상기 제 3 참고예의 것, 즉 상기 도 59에 도시한 것과 동일하다. 단, 제 4 참고예에서는 보용생성 장치(100)가 실행하는 보용생성의 알고리즘은 상기 제 3 참고예의 것과 다르다. 그리고, 보용생성 장치(100) 이외의 각 부의 처리는 상기 제 3 참고예와 동일하다.

도 63은 본 실시형태에서의 보용생성 장치(100)의 처리의 개요를 도시하는 블럭도이다. 이 도 63을 참조하여, 보용생성 장치(100)의 처리의 개요를 이하에 설명한다. 또한, 도 63을 사용하여 이하에 설명하는 처리의 개요는, 후술하는 제 5∼제 7 참고예에 대해서도 동일하다. 또, 제 4 참고예 및 후술하는 제 5∼제 7 참고예에서는 상기 도 12의 동역학 모델을 단순화 모델이라고 칭한다.

도시하는 바와 같이, 보용생성 장치(100)는 보용 패러미터 결정부(100a)를 구비한다. 보용 패러미터 결정부(100a)는 목표 보용의 패러미터(목표 보용을 규정하는 패러미터)의 값 또는 시계열 테이블을 결정한다. 이것은, 후술의 도 65의 플로우차트의 S3520부터 S3530까지의 처리에 상당한다.

상세한 것은 후술하지만, 보용 패러미터 결정부(100a)가 결정하는 패러미터에는, 목표 족평위치 자세 궤도, 목표 팔자세 궤도, 기준 상체자세 궤도, 목표 ZMP 궤도, 목표 상반력 연직성분 궤도 등을 규정하는 패러미터가 포함되는 이외에, 상반력 수평성분 허용범위를 규정하는 패러미터와, ZMP 허용범위(혹은 상반력 모멘트 수평성분 허용범위)를 규정하는 패러미터와, 상반력 모멘트 연직성분 허용범위를 규정하는 패러미터가 포함된다. 이 경우, 제 4 참고예에서 설정하는 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위는 후술의 S3526의 처리에서 설정되는 단순화 모델 보용용의 것과, S3530에서 설정되는 보용 보정용의 것 2종류가 있다. 한편, ZMP 허용범위(혹은 상반력 모멘트 수평성분 허용범위)는 S3530의 처리에서 설정되는 풀 모델 보정용(보용 보정용)의 것뿐이다. 또한, 제 4 참고예에서는, S3530에서는 ZMP 허용범위를 규정하는 패러미터를 설정하지만, 이것은 상반력 모멘트 수평성분 허용범위를 규정하는 패러미터로 설정하는 것과 동등하다. 이것은 상기 제 2 참고예의 도 56의 S3030에 관하여 설명한 바와 같이, 상반력 모멘트 수평성분을 목표 상반력 연직성분으로 나눈 것이, ZMP(상반력 중심점)의 목표 ZMP로부터의 벗어남량을 의미하는 것이 되기 때문이다.

여기에서, 제 4 참고예에서 설정하는 ZMP 허용범위는, 도 64와 같이 설정된다. 상세는 PCT/JP03/00435에 상세하게 기재되어 있으므로 여기에서는, 더 이상의 설명을 생략한다.

보용 패러미터 결정부(100a)에서 결정된 보용 패러미터는 목표 순시값 발생부(100b)에 입력된다. 목표 순시값 발생부(100b)는 입력된 보용 패러미터에 기초 하여, 목표 족평위치 자세, 목표 ZMP, 목표 상반력 연직성분, 목표 팔자세, 목표 전체 중심 연직위치, 목표 상체 연직위치, 상반력 수평성분 허용범위, ZMP 허용범위, 기준 상체자세각 및 기준 역위상 팔 흔들기 각의, 현재 시각 t에 있어서의 순시값을 축차 산출(발생)한다(도 63에서는 일부의 목표 순시값만을 기재하고 있음). 이 목표 순시값 발생부(100b)의 처리는, 후술의 도 65의 플로우차트 S3532의 처리중에서 실행되는 도 57의 S3400∼S3412의 처리, 및, 도 65의 S3534의 처리에 상당한다. 또한, 제 4 참고예에서는, 목표 순시값 발생부(100b)가 산출하는 목표 순시값중, 일부의 순시값(구체적으로는 목표 상체 연직위치의 순시값)은 임시값이고, 후에 수정된다. 또, 목표 순시값 발생부(100b)가 산출하는 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위의 순시값은 단순화 모델 보용용의 순시값과 보용 보정용의 순시값이 있다.

목표 순시값 발생부(100b)에서 산출(발생)된 목표 순시값(일부는 가순시값)은 풀 모델 보정부(100c)에 입력된다. 또, 풀 모델 보정부(100c)에는, 상기자세 경사안정화 제어 연산부(112)(도 59를 참조)에서 구해지는 보상 전체 상반력 모멘트 수평성분 Mdmaxy 및 상기 요잉 안정화 제어 연산부(113)(도 59를 참조)에서 구해지는 보상 전체 상반력 모멘트 연직성분 Mdmdz도 입력된다. 풀 모델 보정부(100c)는 동역학 모델로서, 단순화 모델(100c1)과 풀 모델(100c2)을 구비한다. 이 풀 모델 보정부(100c)는 단순화 모델(100c1)에 기초하여 입력값으로부터 목표 상체위치 자세 및 역위상 팔 흔들기 각의 가순시값 등을 결정하는 동시에, 또한 결정된 상체위치 자세 및 역위상 팔 흔들기 각의 가순시값 등을 풀 모델(100c2)을 사용하 여 수정한다.

또한, 단순화 모델(100c1)을 풀 모델 보정부(100c)에 포함시키지 않는 구성도 가능하다. 또, 풀 모델(100c2)은 후술하는 바와 같이, 역(逆) 풀 모델(역동역학 풀 모델)과 순(順) 풀 모델(순동역학 풀 모델)중 어느 하나를 포함한다.

풀 모델 보정부(100c)는 기본적으로는, 다음 A1∼A4의 조건을 만족하도록, B의 처리를 실행한다. 즉, 풀 모델 보정부(100c)는

A1) 풀 모델 보정부(100c)에 의해 생성되는 보정 보용의 운동에 균형을 이루는 상반력 모멘트에 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 보상 전체 상반력 모멘트 연직성분 Mdmdz를 더한 것이 풀 모델 보정부(100c)로부터 출력하는 컴플라이언스 제어용 상반력 모멘트에 일치한다.

A2) 참 ZMP(목표 ZMP 주위에 컴플라이언스 제어용 목표 상반력 모멘트를 발생시킴으로써 수정된 본래의 정의를 만족하는 ZMP)는 ZMP 허용범위(안정여유를 충분히 유지할 수 있는 허용범위)에 존재한다.

A3) 상반력 수평성분은 보용 보정용의 상반력 수평성분 허용범위내에 있다.

A4) 목표 ZMP의 주위에 발생할 컴플라이언스 제어용 목표 상반력 모멘트 연직성분이 상반력 모멘트 연직성분 허용범위내에 있다. 라고 하는 조건을 만족하도록,

B) 단순화 모델을 사용하여 결정된 단순화 모델 보용의 상체위치 자세를 보정하는 동시에, 목표 ZMP 주위의 컴플라이언스 제어용 목표 상반력 모멘트를 출력한다.

또한, 상기 조건 A2는 목표 ZMP 주위에 발생시키는 상반력 모멘트를 ZMP 허용범위에 대응하는 상반력 모멘트 수평성분 허용범위내에 넣는 것과 동등하다.

여기에서, 상기 단순화 모델(100c1)과 풀 모델(100c2)에 대하여 설명해 둔다. 단순화 모델(100c1)은 동역학적 정밀도보다도, 연산량 저감이나 거동해석의 용이함에 중점을 둔 동역학 모델이며, 동역학적으로 일부 무시가 있거나(예를 들면, 중심 주위의 각운동량 변화를 무시하거나), 모순이 있는 것(엄밀함이 결여된 것)이어도 좋다. 제 4 참고예에서는 상기 참고예에서 설명한 도 12의 동역학 모델(상기 식01∼05에서 기술되는 동역학 모델)이 단순화 모델(100c1)로서 사용된다.

또, 풀 모델(100c2)은 단순화 모델(100c1)과는 상이한 로봇 동역학 모델을 의미한다. 이것은, 단순화 모델(100c1)보다도 근사 정밀도가 높은 로봇 동역학 모델인 것이 바람직하다. 도시예로 설명하면, 제 4 참고예에서는 앞에 기술한 바와 같이 상기 도 12에 도시한 동역학 모델을 단순화 모델(100c1)로서 사용하므로, 그것보다도 근사 정밀도가 높은 동역학 모델, 예를 들면 상기 도 49에 도시한 다질점 모델(로봇(1)의 각 링크에 각각 질점을 갖는 모델)과 같은 로봇 동역학 모델을 풀 모델(100c2)로서 사용하는 것이 바람직하다. 이경우, 이 풀 모델(100c2)은 질점의 주위에 관성 모멘트를 설정하는 것이어도 좋다.

그러나, 단순화 모델(100c1)과 풀 모델(100c2)은 반드시 모델의 근사 정밀도가 상이할 필요는 없다. 단순화 모델(100c1)과 풀 모델(100c2)은, 동역학방정식이 동일하고, 상반력 수평성분 허용범위 및 또는 상반력 모멘트 연직성분 허용범위가 상이한, 즉, 단순화 모델 보용용의 허용범위와, 보용 보정용(풀 모델 보정용)의 허 용범위가 상이할 뿐이어도 좋다. 예를들면, 단순화 모델(100c1)을 사용한 보용을 생성할 때의 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위를 크게 잡아(마찰한계를 초과해도 좋음), 풀 모델(100c2)을 사용하여 보용을 수정할 때의 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위를 로봇(1)의 슬립 혹은 스핀이 잘 발생하지 않도록 설정할 뿐이어도 좋다.

또한, 본 명세서에 있어서, 목표 족평위치 자세, 목표 상반력(특히, 목표 ZMP와 목표 상반력 연직성분)에 기초하여(입력하여) 상체위치 자세 및 역위상 팔 흔들기 각을 산출하는(출력시키는)데 사용하는 모델을 「순동역학 모델」이라고 부르고, 목표 족평위치 자세, 목표 상체자세, 목표 상체위치 및 역위상 팔 흔들기 각에 기초하여(입력하여) 상반력(특히, 목표 ZMP 혹은 목표 ZMP 주위의 상반력 모멘트(수평성분 및 연직성분)와 상반력 수평성분)을 산출하는(출력시키는)데 사용하는 모델을 「역동역학 모델」이라고 부른다. 순동역학 모델의 입력에는 적어도 목표 상반력이 포함되고, 역동역학 모델의 입력에는 적어도 목표 운동이 포함된다.

풀 모델 보정부(100c)가 갖는 풀 모델(100c2)은 역동역학 풀 모델(종종 「역 풀 모델」이라고 약칭) 또는 순동역학 풀 모델(종종 「순 풀 모델」이라고 약칭)을 갖는다. 일반적으로는, 역동역학 모델의 연산에 비해, 순동역학 모델의 연산은 연산량이 많아지는 경향이 있다.

이상이 제 4 참고예에서의 보용생성 장치(100)에 있어서의 처리의 개요이다.

다음에, 제 4 참고예에서의 보용생성 장치(100)의 처리를 상세히 설명한다. 제 4 참고예에서의 보용생성 장치(100)는 도 65의 플로우차트에 도시하는 처리를 실행하여 보용을 생성한다.

먼저, S3510부터 S3528까지에서, 상기 제 2 참고예에서 설명한 도 56의 S3010부터 S3028까지의 처리와 동일한 처리가 실행된다. 또한, S3026의 서브루틴인 도 39의 S608에서 결정하는 금회 보용의 상반력 수평성분 허용범위 및 S610에서 결정하는 상반력 모멘트 연직성분 허용범위는 상기 제 1 참고예∼제 3 참고예의 경우보다도 엄밀하게 마찰력 한계를 고려하지 않더라도 좋고, 마찰력 한계를 초과하는 범위로정해도 좋다. 이것은, 후술하는 풀 모델 보정에 의해, 상반력 수평성분 및 상반력 모멘트 연직성분은 최종적으로 풀 모델 보정용의 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위에 의해 제한되게 되기 때문이다.

이어서, S3530으로 진행하고, 풀 모델 보정용(보용 보정용)의 상반력 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위와 ZMP 허용범위(상반력 중심점 허용범위)를 각각 규정하는 패러미터를 결정한다. 이 경우, 풀 모델 보정용의 상반력 수평성분 허용범위는, 예를 들면 상기 단순화 모델 보용용의 상반력 수평성분 허용범위와 동일하고, X축 방향(전후 방향), Y축 방향(좌우 방향)의 각각에 대해, 상반력 연직성분 궤도와 상기 식12에 기초하여 상기 도 30에 도시하는 바와 같은 패턴으로 설정된다. 그리고, 예를 들면, 상기 식12의 ka*μ의 값이 풀 모델 보정용의 상반력 수평성분 허용범위를 규정하는 패러미터로서 설정된다. 단, 이 상반력 수평성분 허용범위는, 식12의 계수 ka의 값을, 단순화 모델 보용용의 상반력 수평성분 허용범위보다도 약간 작게 설정하는 등 하여, 확실하게 마찰한계내의 범위에 설정하는 것이 바람직하다.

상반력 모멘트 연직성분 허용범위에 관해서도 상반력 수평성분 허용범위 와 동일하게 설정된다. 즉, 상기 식1012에 기초하여 상기 도 41에 도시하는 바와 같은 패턴으로 설정된다.

또, ZMP 허용범위는, 상기 제 2 참고예에서의 도 56의 S3030에서 상반력 모멘트 수평성분 허용범위의 설정에 관하여 설명한 경우와 동일하게 설정된다. 또한, ZMP 허용범위에 목표 상반력 연직성분을 곱함으로써 ZMP 허용범위를 이것과 등가인 상반력 모멘트 수평성분 허용범위로 변환해도 되는 것은 물론이다.

이어서, S3532로 진행하고 금회 보용 순시값(현재 시각 t의 값)이 상기 단순화 모델(도 12의 동역학 모델)을 사용하여 결정된다. 이 S3532의 처리는 상기 제 1 참고예에서의 도 13의 S030의 처리와 동일하다.

이상 설명한 S3532까지의 처리에 의해 생성된 현재 시각 t에서의 보용의 순시값을 이후 단순화 모델 보용 순시값이라고 한다. 제 4 참고예에서는, 이 단순화 모델 보용 순시값은 단순화 모델(도 12의 동역학 모델)을 사용하여, 로봇(1)에 그 운동에 의해 발생하는 관성력과 중력과의 합력이 목표 ZMP 주위에 발생하는 상반력 모멘트 수평성분이 0가 되도록(목표 ZMP에 관한 동역학적 평형조건을 만족시키도록) 결정된다.

이 경우, 단순화 모델 보용 순시값중, 상체 수평위치 및 상체자세 경사각의 순시값과, 상체 연직위치의 순시값과, 역위상 팔 흔들기 각의 순시값은 가순시값이며, 후술하는 풀 모델 보정에 의해 보정되는 것이다. 또, 본 실시형태에서의 단순화 모델 보용 순시값중, 목표 ZMP 주위의 목표 상반력 모멘트 수평성분의 순시값은 정상적으로 0이지만, 후술하는 풀 모델 보정에 의해 목표 ZMP 주위에 발생시키는 상반력 모멘트 수평성분의 목표값으로서의 컴플라이언스 제어용 목표 상반력 모멘트 수평성분이 생성된다.

다음에, S3534로 진행하고, 보용 보정용(풀 모델 보정용)의 상반력 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위와 ZMP 허용범위를 규정하는 패러미터(S3530에서 설정한 것)를 기초로, 보용 보정용의 상반력 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위와 ZMP 허용범위와의 순시값(현재 시각 t의 값)이 구해진다.

이어서, S3536으로 진행하고, 풀 모델을 사용하여 보정 보용의 발생(보용의 보정)을 행하고, 최종적인 목표 보용의 순시값이 결정된다. 즉, 상기 도 63을 참조하여 설명한 바와 같이, 보정 목표 상체위치 자세, 보정 목표 역위상 팔 흔들기 각이나, 목표 ZMP 주위의 목표 상반력 모멘트(수평성분 및 연직성분)로서의 컴플라이언스 제어용 목표 상반력 모멘트(수평성분 및 연직성분)의 산출(결정) 등이 실행된다.

이어서, S3538로 진행하고, 시각 t를 △t만큼 늘리고, 다시 S3514로 되돌아와, S3514부터 S3538까지의 처리가 반복된다.

상기 S3536의 처리는 본 실시형태의 특징을 이루는 부분이며, 이하에 그 처리를 상세히 설명한다. 제 4 참고예에 따른 장치의 보용 보정수법은, 풀 모델 피드 포워드 보정형이다. 또, 역동역학 풀 모델(역 풀 모델)을 사용하는 수법이고, 또한 단순화 모델 보용의 입력을 보정하지 않는 수법이며, 또한 섭동 모델을 사용 하는 수법이다.

도 66은, 제 4 참고예에 따른 보용생성 장치(100)의 동작, 구체적으로는, 도 65의 플로우 챠트의 S3536의 보용의 수정수법을 설명하는 기능 블럭도이다. 단, 도 66의 단순화 모델(200)은 단지 동역학 모델뿐만 아니라, 전술의 S3510부터 S3532의 처리, 즉 단순화 모델 보용 순시값의 산출(결정)처리를 나타낸다. 따라서, 도 66에서, 단순화 모델(200)로부터 앞 부분이 S3536의 처리에 상당한다.

또한, 보용 보정용(풀 모델 보정용)의 상반력 수평성분 허용범위와 ZMP 허용범위의 순시값을 결정하는 처리는 도 65의 플로우차트의 참조부호 S3534를 사용하여 나타내고 있다.

실제의 처리는 하나의 컴퓨터로 실행되므로, 블럭도를 이산화 한 후, 블럭도의 상류측에서 하류측(보용출력측)을 향해서 순차적으로 실행된다. 단, 상류측으로 되돌아오는 피드백량은 전회 제어주기(현재 시각 t에 대해 시각 t-△t)에 산출한 값(상태량)을 사용하는 것으로 한다. 또한, 이후, 전회 제어주기(시각 t-△t)에 산출한 값을 전회값으로 약칭한다.

S3536의 처리가 실행될 때마다, 블럭도의 1제어주기분의 연산이 실행된다.

S3536에서는, 먼저, 상기와 같이 얻어진 단순화 모델 보용의 목표 상체자세각(이후, 단순화 모델 상체자세각이라고 부른다. 또, 이중의 경사각 성분을 단순화 모델 상체자세 경사각이라고 부른다.), 목표 상체 수평위치(이후, 단순화 모델 상체 수평위치라고 부름), 목표 중심위치, 목표 족평위치 자세, 목표 팔자세(목표 역위상 팔 흔들기 각을 포함함) 등의 운동을 나타내는 변수(이것을 운동변수라고 부름)의 순시값과, 목표 ZMP의 순시값이 상기 역동역학 풀 모델(역 풀 모델)(201)에 입력된다. 입력된 운동변수로 표시되는 운동에 균형을 이루는(즉 운동에 의해 풀 모델이 발생하는) 상반력 수평성분과 목표 ZMP 주위의 상반력 모멘트(수평성분 및 연직성분)이, 역 풀 모델(201)의 연산처리에 의해 산출된다. 또한, 제 4 참고예에서는 단순화 모델 보용에 있어서의 목표 ZMP 주위의 상반력 모멘트 수평성분은 0이므로, 역 풀 모델(201)에 의해 산출되는 목표 ZMP주위의 상반력 모멘트 수평성분은 단순화 모델 보용의 오차로서의 의미를 갖는 것이다.

또한, 역 풀 모델(201)에 의해 구해진 상반력 수평성분과 상반력 모멘트 수평성분과 상반력 모멘트 연직성분을 각각 「풀 모델 상반력 수평성분」, 「풀 모델 상반력 모멘트 수평성분」, 「풀 모델 상반력 모멘트 연직성분」이라고 부른다. 또, 이후, 종종, 풀 모델 상반력 수평성분을 Ffull, 풀 모델 상반력 모멘트 수평성분을 Mfullxy, 풀 모델 상반력 모멘트 연직성분을 Mfullz로 약칭한다.

또, 상기 역 풀 모델(201)은 목표 중심위치를 만족하는 상체 연직위치를 산출한다. 또한, 도시는 생략했지만, 역 풀 모델(201)은 중심 수평위치도 산출한다.

보충하면, 풀 모델에는 목표 전체 중심 연직위치를 입력하고 있고, 또 목표 상반력 연직성분은 목표 전체 중심 연직위치의 2차미분값으로부터 구해지므로, 풀 모델에는 특별히 목표 상반력 연직성분을 입력할 필요는 없다. 길더라도 연산을 줄이기 위한 등의 이유에서, 풀 모델에 목표 상반력 연직성분을 입력해도 좋다.

여기에서, 보용 보정에 사용되는 섭동 모델에 대해 설명한다.

섭동 모델은 상체 수평위치 보정용 섭동 모델(202), 상체자세 경사각 보정용 섭동 모델(203) 및 역위상 팔 흔들기 각 보정용 섭동 모델(231)로 이루어진다. 또한, 섭동 모델은 이와 같이 3개의 모델로 분리하지 않고, 도 12와 같은 하나의 모델로 해도 좋다. 또, 상체자세 경사각 보정용 섭동 모델(203)은 도면에서는 상체자세각 보정용 섭동 모델(203)이라 약칭한다.

상체 수평위치 보정용 섭동 모델(202)은 상기 상체 병진 모드에서의 상반력의 섭동과 상체 수평위치의 섭동의 관계를 나타낸다.

상체 수평위치 보정용 섭동 모델(202)은 목표 상반력 모멘트의 보정량을 입력하고, 이것에 동역학적으로 균형을 이루는 목표 상체 수평위치의 보정량을 산출한다. 이 입력(목표 상반력 모멘트의 보정량)을 상체 수평위치 보정용 섭동 모델 모멘트(Mp)라고 부른다. 또, 상체 수평위치 보정용 섭동 모델(202)의 출력(목표 상체 수평위치의 보정량)을 보정용 섭동 모델 상체 수평위치 Xc라고 부른다. 또, 상체 수평위치 보정용 섭동 모델(202)이 발생하는 상반력 수평성분을 상체 수평위치 보정용 섭동 모델 상반력 수평성분 Fp라고 부른다.

상체 수평위치 보정용 섭동 모델(202)은 도 67과 같이, 지지점과 도립진자 질점(상체 질점)과 이것들을 결합하는 신축 자유로운 지지봉으로 이루어지는 도립진자로 표시된다. 지지점의 수평위치는 상기한 금회 보용의 지지다리 좌표계의 원점의 수평위치에 일치하고, 지지점의 연직위치는 목표 ZMP의 연직위치에 일치하도록 지지점의 위치가 설정되어 있다. 도립진자 질점의 질량 mb는, 도 12에 도시하는 상기 단순화 모델(3질점+플라이휠의 모델)의 상체 질점 질량과 같다. 도립진자 질점의 연직위치 Zc는 단순화 보용에 대응하는 도 12에 도시하는 단순화 모델의 상 체 질점위치의 연직위치 Zb와 동일한 것으로 한다.

이 상체 수평위치 보정용 섭동 모델(202)은 상기 단순화 모델에 있어서, 상반력 모멘트의 섭동 △My와 상체 질점 수평지위의 섭동 △Xb의 관계를 나타낸다.

그래서, My, Xb, Zb 이외는 정수로 간주하고, 상반력 모멘트의 섭동 △My와 상체 질점 수평위치의 섭동 △Xb와의 관계를 상기 식03y로부터 구하면, 다음 식이 얻어진다.

△My = -mb*△Xb*(g+d2Zb/dt2)+mb*(Zb-Zzmp)*d2△Xb/dt2 …식a12

동일하게, Fx, Xb 이외는 정수로 간주하고, 상반력 수평성분의 섭동 △Fx와 상체 질점 수평위치의 섭동 △Xb의 관계를 상기 식02x로부터 구하면, 다음 식이 얻어진다.

△Fx=mb*d2△Xb/dt2 …식a13

상체 수평가속도에 의해 발생하는 △Mp와 △Fp의 비인 상체 병진 모드 상반력 비율 h는 식a12의 우변중 상체 수평가속도에 의해 발생하는 항(즉 제 2 항)과 식a13과의 비이기 때문에, 다음 식이 얻어진다.

h=(Zb-Zzmp) …식a14

즉, 상체 병진 모드 상반력 비율 h는, 단순화 모델의 상체 질점(도립진자 질점)의 지점으로부터의 높이에 상당한다.

따라서, 식a12과 식a14로부터 다음 식이 얻어진다.

△My = -mb*△Xb*(g+d2Zb/dt2)+mb*h*d2△Xb/dt2 …식a15

한편, 상체 질점(도립진자 질점)에 작용하는 중력과 관성력과의 합력의 병진 력 연직성분에 균형을 이루는 상반력 연직성분을 상체 상반력 연직성분 Fbz라고 부른다. 즉, 상체 상반력 연직성분 Fbz는 다음 식으로 정의된다.

Fbz=mb*(g+d2Zb/dt2) …식a16

그리고, 이 식a16과 상기 식01로부터, 상체 상반력 연직성분 Fbz는 다음 식으로 된다.

Fbz=Fz-msup*(g+d2Zsup/dt2)-mswg*(g+d2Zswg/dt2) …식a17

바꿔 말하면, 상체 상반력 연직성분은 상반력 연직성분 Fz와 도 12에 도시하는 상기 단순화 모델(3질점+플라이휠의 모델)의 양다리 질점에 작용하는 중력과 관성력과의 합력의 병진력 연직성분과의 합과 동등하다.

식a16을 식a15에 대입함으로써, 다음 식이 얻어진다.

△My = -Fbz*△Xb+mb*h*d2△Xb/dt2 …식a18

식a18의 △My를 상체 수평위치 보정용 섭동 모델 모멘트(Mp), △Xb를 보정용 섭동 모델 상체 수평위치 Xc에 대응시킴으로써(식a18의 △My에 상체 수평위치 보정용 섭동 모델 모멘트(Mp), △Xb에 보정용 섭동 모델 상체 수평위치 Xc를 대입함으로써), 다음 식이 얻어진다.

Mp = -Fbz*Xc+mb*h*d2Xc/dt2 …식a19

즉, 상체 수평위치 보정용 섭동 모델(202)은 식a14에 의해 구해지는 상체 병진 모드 상반력 비율 h와 식a17에 의해 구해지는 상체 상반력 연직성분 Fbz를 사용하고, 식a19로 표시된다.

또, 식a13의 △Fx를 상체 수평위치 보정용 섭동 모델 상반력 수평성분 Fp에 대응시킴으로써, 다음 식이 얻어진다.

Fp=mb*d2Xc/dt2 …식a20

즉, 식a14, 식a17, 식a19, 식a20에 의해 상체 수평위치 보정용 섭동 모델(202)이 기술된다.

보충하면, 여기에서는, 상체 질점위치의 섭동과 상체위치(상체 대표점위치)의 섭동은 일치하는 것으로 간주하고 있지만, 엄밀하게는, 일치한다고는 할 수 없다. 따라서, Mp, Fp와 Xc의 관계를 구하기 위해서는, 또한 상체 질점 수평위치와 상체위치의 기하학적 관계를 나타내는 모델이 필요하다.

한편, 상체 자세 경사각 보정용 섭동 모델(203)은 상기 상체 경사 모드에서의 상반력의 섭동과 상체자세 경사각의 섭동의 관계를 나타낸다.

상체자세 경사각 보정용 섭동 모델(203)은 상반력 모멘트 수평성분의 보정량이 입력되고, 이것에 동역학적으로 균형을 이루는 목표 상체자세 경사각의 보정량을 산출한다. 이 입력(상반력 모멘트의 보정량)을 상체자세 경사각 보정용 섭동 모델 모멘트(Mr)(상체자세각 보정용 섭동 모델 Mr이라고 약칭하기도 함)이라고 부른다. 또, 상체자세 경사각 보정용 섭동 모델(203)의 출력(목표 상체자세 경사각의 보정량)을 보정용 섭동 모델 상체자세 경사각 θc라고 부른다. 또, 상체자세 경사각 보정용 섭동 모델(203)이 발생하는 상반력 수평성분을 상체 수평위치 보정용 섭동 모델 상반력 수평성분 Fr이라고 부른다. Fr은 전술한 바와 같이 0이다. 즉, 다음 식이 항상 성립한다.

Fr=0 …식a21

상체자세 경사각 보정용 섭동 모델(203)은 도 68과 같이, 플라이휠로 표시된다. 보충하면, 도 68에서는, Y축 주위 회전의 플라이휠만을 도시했지만, 상체자세 경사각 보정용 섭동 모델(203)은 실제로는, Y축 주위 회전의 플라이휠뿐만 아니라, X축 주위 회전의 플라이휠도 필요로 한다. 그들 플라이휠의 이너셔는, 도 12에 도시한 상기 단순화 모델(3질점 모델+플라이휠)의 플라이휠 FHx, FHy와 동일하다. 이 상체자세 경사각 보정용 섭동 모델(203)의 플라이휠의 회전각도가 보정용 섭동 모델 상체자세 경사각 θc, 그 플라이휠에 의해 발생하는 상반력 모멘트 수평성분이 상체자세 경사각 보정용 섭동 모델 모멘트(Mr)에 상당한다. 또한, 이하의 설명에서는 이해의 편의상, Y축 주위 회전의 플라이휠을 중심으로 설명하지만, X축 주위 회전의 플라이휠에 대해서도 동일하다.

이 상체자세 경사각 보정용 섭동 모델(203)(보다 상세하게는 사지탈 플레인에 관한 모델)은, 상기 단순화 모델(3질점+플라이휠의 모델)의 동역학 방정식의 상기 식03y에 있어서, 상반력 모멘트의 섭동 △My와 상체자세 경사각의 섭동 △θby의 관계를 나타내고 있다.

그래서, My, θby 이외는 정수로 간주하고, 상반력 모멘트 수평성분의 섭동 △My와 상체자세 경사각의 섭동 △θby의 관계를 식03y로부터 구하면, 다음 식이 얻어진다.

△My = J*d2△θby/dt2 …식a22

식a22의 △My를 상체자세 경사각 보정용 섭동 모델 모멘트(Mr), △θby를 보정용 섭동 모델 상체자세 경사각 θc에 대응시킴으로써, 다음 식이 얻어진다.

Mr = J*d2△θc/dt2 …식a23

즉, 상체자세 경사각 보정용 섭동 모델(203)은 식a23으로 표시된다. 또, 상체 수평위치 보정용 섭동 모델 상반력 수평성분 Fa는 상기한 바와 같이 식a21과 같이 된다(Fr=0). 또한, 상기에서는, 상체자세 경사각 보정용 섭동 모델(203)의 동역학을 사지탈 플레인상에서 설명했지만, 래터럴 플레인상에서의 동역학도, 식a23과 동일한 형태의 식으로 표시된다.

또, 역위상 팔 흔들기 각 보정용 섭동 모델(231)은 상기 역위상 팔 흔들기 모드에서의 상반력의 섭동과 역위상 팔 흔들기 각의 섭동의 관계를 나타낸다.

역위상 팔 흔들기 각 보정용 섭동 모델(231)은 상반력 모멘트 연직성분의 보정량이 입력되고, 이것에 동역학적으로 균형을 이루는 목표 역위상 팔 흔들기 각의 보정량을 산출한다. 이 입력(상반력 모멘트 연직성분의 보정량)을 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)라고 부른다. 또, 역위상 팔 흔들기 각 보정용 섭동 모델(203)의 출력(목표 역위상 팔 흔들기 각의 보정량)을 보정용 섭동 모델 역위상 팔 흔들기 각 θca라고 부른다. 또, 역위상 팔 흔들기 각 보정용 섭동 모델이 발생하는 상반력 수평성분을 역위상 팔 흔들기 각 보정용 섭동 모델 상반력 수평성분 Fa라고 부른다. Fa는 전술한 바와 같이 0이다. 즉, 다음 식이 항상 성립한다.

Fa=0 …식a21c

역위상 팔 흔들기 각 보정용 섭동 모델(231)은 도 69와 같이, 플라이휠로 표시된다. 플라이휠의 이너셔는 도 12에 도시한 상기 단순화 모델(3질점 모델+플라 이휠)의 플라이휠 FHaz와 동일하다. 이 역위상 팔 흔들기 각 보정용 섭동 모델(231)의 플라이휠의 회전각도가 보정용 섭동 모델 역위상 팔 흔들기 각 θca, 그 플라이휠에 의해 발생하는 상반력 모멘트 연직성분이 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)에 상당한다.

이 역위상 팔 흔들기 각 보정용 섭동 모델(231)은 상기 단순화 모델(3질점+플라이휠의 모델)의 동역학 방정식의 상기 식03z에서, 상반력 모멘트의 섭동△Mz와 역위상 팔 흔들기 각의 섭동 △θaz와의 관계를 나타내고 있다.

그래서, Mz, θaz 이외는 정수로 간주하고, 상반력 모멘트의 섭동 △Mz와 역위상 팔 흔들기 각의 섭동 △θaz의 관계를 식03z로부터 구하면, 다음 식이 얻어진다.

△Mz=Jaz*d2△θaz/dt2 …식a22c

식a22c의 △Mz를 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma), △θaz를 보정용 섭동 모델 역위상 팔 흔들기 각 θca에 대응시킴으로써, 다음 식이 얻어진다.

Ma=Jaz*d2△θca/dt2 …식a23c

즉, 역위상 팔 흔들기 각 보정용 섭동 모델(231)은 식a23c로 표시된다. 또, 역위상 팔 흔들기 각 보정용 섭동 모델 상반력 수평성분 Fr은 상기한 바와 같이 식a21c와 같이 된다(Fa=0).

후술하는 바와 같이, S3536에서는, 최종적으로, 보정 보용(보다 상세하게는, 단순화 모델 보용의 일부의 순시값을 보정한 목표 순시값)이 생성(출력)된다. 보 정 보용의 목표 상체자세 경사각(이것을 이후, 보정 목표 상체자세 경사각이라고 부름)은, 상기 구한 단순화 모델 상체자세 경사각의 순시값(S3532에서 구한 금회 보용의 현재 시각 t에서의 목표 상체자세 경사각의 순시값)에 상기 보정용 섭동 모델 상체자세 경사각 θc(현재 시각 t의 제어주기에서 구해지는 값)를 연산부(204)에서 더함으로써 얻어진다. 보정 보용의 목표 상체 수평위치(이것을 이후, 보정 목표 상체 수평위치라고 부름)는 상기 구한 단순화 모델 상체 수평위치의 순시값(S3532에서 구한 금회 보용의 현재 시각 t에서의 목표 상체 수평위치의 순시값)에 보정용 섭동 모델 상체 수평위치 Xc(현재 시각 t의 제어주기에서 구해지는 값)를 연산부(205)에서 더함으로써 얻어진다. 또, 보정 보용의 목표 역위상 팔 흔들기 각(이것을 이후, 보정 목표 역위상 팔 흔들기 각이라고 부름)은, 상기 구한 단순화 모델 역위상 팔 흔들기 각의 순시값(S3532에서 구한 금회 보용의 현재 시각 t에서의 목표 역위상 팔 흔들기 각의 순시값)에 보정용 섭동 모델 역위상 팔 흔들기 각 θca(현재 시각 t의 제어주기에서 구해지는 값)를 연산부(232)에서 더함으로써 얻어진다.

보정 보용의 목표 상반력도 수정된다. 구체적으로는, 목표 ZMP 주위의 상반력 모멘트 수평성분이 0이 아니게 되고, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분이 목표값으로서 출력된다. 또, 목표 상반력 수평성분도 보정 목표 상반력 수평성분으로 수정되고, 출력된다. 또, 목표 상반력 모멘트 연직성분도 보정 목표 상반력 모멘트 연직성분으로 수정되고, 출력된다.

그런데, 상기와 같이, 보정 보용의 운동은 단순화 모델 보용의 운동에 섭동 모델의 운동(상세하게는 상체 수평위치 보정용 섭동 모델(202)과 상체자세 경사각 보정용 섭동 모델(203)의 운동과 역위상 팔 흔들기 각 보정용 섭동 모델(231)의 운동)을 더한(합성한) 운동이다.

일반적으로, 어떤 기준운동에 어떤 섭동운동을 더한 운동이 발생하는 상반력은 기준운동이 발생하는 상반력(운동에 의해 발생하는 중력과 관성력에 균형을 이루는 상반력)과 섭동운동에 의해 발생하는 상반력의 섭동분의 합으로 근사된다.

또, 상체 경사 모드가 발생하는 상반력 수평성분, 상체 경사 모드가 발생하는 상반력 모멘트 연직성분, 역위상 팔 흔들기 모드가 발생하는 상반력 수평성분 및 역위상 팔 흔들기 모드가 발생하는 상반력 모멘트 수평성분이 모두 0인 것을 고려하면, 보정 보용의 운동에 역 풀 모델(201)상에서 균형을 이루는 상반력 모멘트에 보상 전체 상반력 모멘트 수평성분 Mamdxy와 보상 전체 상반력 모멘트 연직성분 Mdmdz를 더한 것이 보정 목표 상반력 모멘트에 일치하기(상기 A1의 조건을 만족시키기) 위해서는, 이하의 3개의 식을 만족하지 않으면 안된다.

풀 모델 상반력 모멘트 수평성분 Mfullxy

+상체 수평위치 보정용 섭동 모델 모멘트(Mp)

+상체자세 경사각 보정용 섭동 모델 모멘트(Mr)

+보상 전체 상반력 모멘트 수평성분 Mdmdxy

=보정 목표 상반력 모멘트 수평성분 …식h5

풀 모델 상반력 수평성분 Ffull

+상체 수평위치 보정용 섭동 모델 상반력 수평성분 Fp

=보정 목표 상반력 수평성분 …식h6

풀 모델 상반력 모멘트 연직성분 Mfulllz

+상체 수평위치 보정용 섭동 모델 모멘트 연직성분 Mpz

+역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 연직성분 Maz

+보상 전체 상반력 모멘트 연직성분 Mdmdz

=보정 목표 상반력 모멘트 연직성분 …식h1006

단, 상체 수평위치 보정용 섭동 모델 모멘트 연직성분 Mpz는 상체 수평위치 보정용 섭동 모델의 운동에 의해 역 풀 모델(201)상에서 발생하는 상반력 모멘트 연직성분이다. 또, 식h5, 식h1006의 모멘트는 원래의(단순화 모델의) 목표 ZMP 주위의 모멘트이다.

또, 보정 보용의 참 ZMP는 보정 목표 상반력 모멘트를 목표 상반력 연직성분으로 나는 값만큼, 단순화 모델 보용의 목표 ZMP(이상 목표 ZMP)로부터 벗어난 점으로 변경된다.

보정 보용의 참 ZMP=목표 ZMP

+보정 목표 상반력 모멘트/목표 상반력 연직성분 …식h7

또한, 보정 보용의 참 ZMP의 X방향(전후방향) 성분을 산출하는 경우에는 보정 목표 상반력 모멘트의 Y축(좌우방향축) 주위 성분을 사용한다. 또, 보정 보용의 참 ZMP의 Y방향 성분을 산출하는 경우에는, 보정 목표 상반력 모멘트의 X축(전후축) 주위 성분을 사용한다. 단, 보정 보용의 참 ZMP의 Y방향 성분을 산출하는 경우에는, 식h7의 우변의 「+」는 「-」로 하지 않으면 안된다.

보충하면, 보정 보용의 참 ZMP 주위의 보정 목표 상반력 모멘트 연직성분을 산출하도록 해도 좋다.

보정 보용의 참 ZMP 주위의 보정 목표 상반력 모멘트 연직성분은 원래의 목표 ZMP 주위의 모멘트에 보정 보용의 참 ZMP와 목표 ZMP의 차와 식h6에서 구한 보정 목표 상반력 수평성분과의 외적을 더한 것이 된다.

식h7로부터 구해지는 보정 보용의 참 ZMP는 ZMP 허용범위내에 없으면 안된다. 이것을 ZMP 제약조건이라고 부른다.

또, 보정 목표 상반력 수평성분은 보용 보정용(풀 모델 보정용)의 상반력 수평성분 허용범위내에 없으면 안된다. 이것을 상반력 수평성분 제약조건이라고 부른다.

또, 보정 목표 상반력 모멘트 연직성분은 보용 보정용(풀 모델 보정용)의 상반력 모멘트 연직성분 허용범위내에 없으면 안된다. 이것을 상반력 모멘트 연직성분 제약조건이라고 부른다.

이상과 같이, 보정 보용은 식h5, 식h6, 식h1006, ZMP 제약조건(식h7로부터 얻어지는 보정 보용의 참 ZMP의 존재범위 조건) 상반력 수평성분 제약조건 및 상반력 모멘트 연직성분 제약조건을 만족하지 않으면 안된다.

그러나, 단지, 이것들의 식이나 조건을 만족하는 것 만으로는, 상기 보정용 섭동 모델 상체위치와 상기 보정용 섭동 모델 상체자세 경사각과 상기 보정용 섭동 모델 역위상 팔 흔들기 각이 발산해 버린다.

그래서, 상기 상체 수평위치 보정용 섭동 모델(202)과 상기 상체자세 경사각 보정용 섭동 모델(203)과 상기 역위상 팔 흔들기 각 보정용 섭동 모델(231)의 상태량(보다 구체적으로는, 보정용 섭동 모델 상체 수평위치 속도, 보정용 섭동 모델 상체자세 경사각·각속도, 보정용 섭동 모델 역위상 팔 흔들기 각·각속도 등)을 기초로, 이것들의 상태량이 어떤 소정의 상태에 수렴하도록(안정하도록), 보정용 섭동 모델(202, 203, 231)의 안정화 제어를 행한다.

먼저, 상체 수평위치 보정용 섭동 모델(202)의 안정화 제어에 대해 상세를 설명한다.

상체 수평위치 보정용 섭동 모델(202)을 목표 정정위치에 수렴(안정)시키기 위한 제어칙을 상체 수평위치 보정용 섭동 모델 제어칙(206)이라고 부르고, 이 제어칙이 결정하는 피드백량(조작량)을 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd라고 부른다. 여기에서 「요구값」을 붙인 것은, 후술하는 바와 같이, 참 ZMP가 상기 ZMP 허용범위내에 존재하고, 또한, 상반력 수평성분이 상반력 수평성분 허용범위내로 되도록, 상기 제어칙에서 결정된 값에 제한이 가해져 수정되기 때문이다. 제한이 가해져 수정된 모멘트를 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)라고 부른다.

구체적으로 상체 수평위치 보정용 섭동 모델 제어칙(206)으로서는, 식h10을 사용한다. 단, 목표 정정위치는 식h8로 주어진다. 또, mtotal은 상기 로봇 총중량, mb는 상기 상체 질점 질량(도립진자 질량)이며, XGf는 단순화 모델 보용의 순간 자세를 기초로 풀 모델을 사용하여 계산되는 중심 수평위치, 즉, 상기 역 풀 모델이 산출하는 중심 수평위치이다. 또, Kpp, Kpv는 피드백제어의 게인이다.

목표 정정위치 = -mtotal/mb*(XGf-XGs) …식h8

상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd

=Kpp*(보정용 섭동 모델 상체 수평위치 Xc - 목표 정정위치)

+Kpv*보정용 섭동 모델 상체 수평속도 dXc/at

-보정용 섭동 모델 상체 수평위치 Xc*상체 상반력 연직성분 Fbz

…식h10

상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd의 X축(전후방향축)주위 성분을 구하는 경우에는, 보정용 섭동 모델 상체 수평위치 속도 및 목표 정정위치는 Y축 방향(좌우 방향) 성분을 사용한다.

상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd의 Y축(좌우방향축)주위 성분을 구하는 경우에는, 보정용 섭동 모델 상체 수평위치 속도 및 목표 정정위치는 X축 방향(전후 방향) 성분을 사용하고, 우변 제 3 항의 「-」를 「+」로 바꾼다.

상세한 것은, 앞에 본 출원인이 제안한 PCT/JP03/00435에 기재되어 있으므로, 여기에서는 더 이상의 설명을 생략한다.

다음에 상체자세 경사각 보정용 섭동 모델(203)의 안정화 제어에 대해 상세를 설명한다.

목표 순시값 발생부(100b)가 출력하는 기준 상체자세 경사각(도 57의 S3404에서 구해진 것) 또는 단순화 모델에 의한 목표 상체자세 경사각(도 57의 S3414에서 구해진 것)에, 보정 목표 상체자세 경사각, 즉, 단순화 모델에 의한 목표 상체 자세 경사각에 보정용 섭동 모델 상체자세 경사각을 더한 것이 정정 또는 추종하도록, 상체자세 경사각 보정용 섭동 모델(203)의 상태에 따라, PI제어 등의 피드백제어칙에 의해 피드백량(조작량)을 결정하고, 이것을 상체자세 경사각 보정용 섭동 모델(203)에 부가적으로 입력한다.

이 제어칙을 상체자세 경사각 보정용 섭동 모델 제어칙(207)(상체자세각 보정용 섭동 모델 제어칙(207)이라 약칭하기도 함)이라 부르고, 이 피드백량(조작량)을 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd라고 부른다. 여기에서 「요구값」을 붙인 것은 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd와 동일한 이유때문이다. 제한이 가해져 수정된 모멘트를 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)라고 부른다.

상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd를 결정하는 상체자세 경사각 보정용 섭동 모델 제어칙(207)은 구체적으로는, 다음 식과 같이 하면 된다.

상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd

=Krp*(보정용 섭동 모델 상체자세 경사각 θc

-(기준 상체자세 경사각 - 단순화 모델에 의한 목표 상체자세 경사각))

+Krv*보정용 섭동 모델 상체자세 경사각속도 dθc/dt …식h11

단, Krp, Krv는 피드백제어의 게인이다.

또한, 식h11에서, (기준 상체자세 경사각 - 단순화 모델에 의한 목표 상체자 세 경사각) 대신에 0으로 해도 좋다.

다시, 도 66의 기능 블럭도를 사용하여 S3536의 처리를 설명하면, 상기와 같이, 상기 상체 수평위치 보정용 섭동 모델 제어칙(206)(식h10)에 의해 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd가 구해진다. 또, 상기 상체자세 경사각 보정용 섭동 모델 제어칙(207)(식h11)에 의해 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd가 구해진다.

이어서, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)가 0이었다라고 가정한 경우의 상체위치 보정용 섭동 모델의 상반력 추정(산출)값 F0가 F0 연산부(208)에 의해 구해진다. 후술하는 바와 같이, 상체 수평위치 보정용 섭동 모델(202)에는, 풀 모델 상반력 모멘트 수평성분 Mfullxy에, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 더한 것이 입력된다. 따라서, F0는 상체 수평위치 보정용 섭동 모델(202)에 풀 모델 상반력 모멘트 수평성분 Mfullxy의 부호를 반전시킨 것만이 입력된 경우에 상체 수평위치 보정용 섭동모델(202)이 발생하는 상반력이다.

구체적으로는, 다음 식에 의해 F0가 구해진다.

F0=mb*d2Xc/dt2-1/h*Mpf …식h12

우변 제 1항은 전회(시각 t-△t)의 상체 수평위치 보정용 섭동 모델(202)의 상반력 수평성분을 나타낸다.

우변 제 2항은, 전회의 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)에 의해, 상체 수평위치 보정용 섭동 모델(202)에 직접적으로 발생한(즉 직달항의) 상반력 수평성분을 나타낸다.

즉, 상체 수평위치 보정용 섭동 모델(202)의 전회의 상체 질점 수평가속도에 상체 질점의 질량 mb를 곱한 값으로부터, 전회의 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 상체 병진 모드 상반력 비율 h로 나눈 값을 뺌으로써, Mpf가 0이었다라고 가정한 경우의 상체위치 보정용 섭동 모델의 상반력 F0의 추정값이 구해진다.

다음에, 상기 제약(상반력 수평성분 제약조건 및 ZMP 제약조건)을 무시하고, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd에 일치시키고, 또한, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)를, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd에 일치시킴과 동시에, 목표 ZMP 주위의 목표 상반력 모멘트 수평성분으로서의 컴플라이언스 제어용 목표 상반력 모멘트를 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 Mpf와 Mrf와의 총 합에 일치시켰다고 가정한 경우에, 목표 ZMP 주위에 발생하는 상반력 모멘트 수평성분 Min이 Min 연산부(209)에 의해 구해진다. 이 상반력 모멘트 수평성분을 무제한시 보정 목표 상반력 모멘트 수평성분 Min이라 부른다. 무제한시 보정 목표 상반력 모멘트 수평성분 Min은 다음 식에 의해 구해진다.

Min=Mpfdmd+Mrfdmd+Mdmdxy …식h13

즉, 무제한시 보정 목표 상반력 모멘트 수평성분 Min은 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd와 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd와 보상 전체 상반력 모멘트 수평성분 Mdmdxy를 가산함으로써 얻어진다.

이어서, 상기 제약(상반력 수평성분 제약조건 및 ZMP 제약조건)을 무시하고, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd에 일치시키고, 또한, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)를 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd에 일치시킴과 동시에, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분을 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 Mpf와 Mrf와의 총합에 일치시켰다고 가정한 경우에, 발생하는 상반력 수평성분 Fin이 Fin 연산부(210)에 의해 구해진다. 이 상반력 수평성분을 무제한시 보정 목표 상반력 수평성분 Fin이라고 부른다.

보정 목표 상반력 수평성분은, 상기 식h6에 의해 얻어지는데 상기와 같이, 상체자세 경사각 보정용 섭동 모델(303)의 거동에 의해 상체자세 경사각 보정용 섭동 모델(203)에는 상반력 수평성분은 발생하지 않는, 즉 Fr은 0이다. 따라서, 무제한시 보정 목표 상반력 수평성분 Fin은 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)가 0이었다고 가정한 경우의 보정 목표 상반력 수평성분에, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 0으로부터 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd로 변경함으로써 증가하는 상반력 수평성분을 더한 것이 된다.

그런데, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 0으로부터 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd로 변경함으로써 증가하는 상반력 수평성분은 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd를 상체 병진 모드 상반력 비율 h로 나눈 값이 된다.

따라서, 무제한시 보정 목표 상반력 수평공분 Fin은 식h15에 도시하는 바와 같이 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd를 상체 병진 모드 상반력 비율 h로 나눈 값에, 상기 구한 풀 모델 상반력 수평성분 Ffull을 가산하고, 또한, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)가 0이었다고 가정한 경우의 상체위치 보정용 섭동 모델의 상반력의 산출값 F0를 가산함으로써 얻어진다.

Fin=1/h*Mpfdmd+Ffull+F0 …식h15

이어서, 제한수단(제한처리부)(211)에 의해, 무제한시 보정 목표 상반력 모멘트 수평성분 Min과 무제한시 보정 목표 상반력 수평성분 Fin으로부터, 이것들에 제한을 가한 값인(목표 ZMP 주위의) 제한 보정 목표 상반력 모멘트 수평성분 Mltd와 제한 보정 목표 상반력 수평성분 Fltd가 결정된다. 본 실시형태에서는 컴플라이언스 제어용 목표 상반력 모멘트 수평성분은 제한 보정 목표 상반력 모멘트 수평성분 Mltd에 일치하고, 보정 보용의 상반력 수평성분은 제한 보정 목표 상반력 수평성분Fltd에 일치한다.

제한 보정 목표 상반력 모멘트 수평성분 Mltd와 제한 보정 목표 상반력 수평성분 Fltd란, 보정 보용(컴플라이언스 제어용 목표 상반력 모멘트 수평성분을 포함함)의 참 ZMP가 상기 ZMP 허용범위내에 존재하고, 또한, 보정 보용의 상반력 수평 성분이 상반력 수평성분 허용범위내로 되도록 결정된다. 즉, Mltd 및 Fltd는 ZMP 제약조건 및 상반력 수평성분 제약조건을 만족하도록 결정된다.

또, 상기 제약 조건하에서, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mp)는 가능한 한, 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값Mpfdmd에 일치 또는 가까운 값이 되도록 결정된다. 마찬가지로, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mr)는 가능한 한 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd에 일치 또는 가까운 값이 되도록 결정된다. 이것에 의해, 상기 보정용 섭동 모델 상체위치 Xc와 상기 보정용 섭동 모델 상체자세 경사각 θc를 안정화하여, 발산하는 것을 방지한다.

제한수단(제한처리부)(211)의 상세는, 앞에 본 출원인이 제안한 PCT/JP03/00435에 기재되어 있으므로, 여기에서는 더 이상의 설명을 생략한다.

이어서, 이하의 식에 의해, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf), 및 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)가 각각 Mpf 연산부(212), Mrf 연산부(213)에 의해 구해진다.

Mpf=(Fltd-Ffull-F0)*h …식h20

Mrf=Mltd-Mpf-Mdmdxy …식h21

즉, Mpf 연산부(212)에서는, 제한 보정 목표 상반력 수평성분 Fltd로부터, 풀 모델 상반력 수평성분 Ffull을 빼고, Mp가 0이었다고 가정한 경우의 상체위치 보정용 섭동 모델(202)의 상반력의 산출값 F0를 뺀 값에, 상체 병진 모드 상반력 비율 h를 곱함으로써, 상기 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)가 얻어진다. 또, Mrf 연산부(213)에서는, 목표 ZMP 주위의 제한 보정 목표 상반력 모멘트 수평성분 Mltd로부터 상기 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)와 보상 전체 상반력 모멘트 수평성분 Mdmdxy를 뺌으로써, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)가 얻어진다.

이어서, 이하의 식에 의해, 상체 수평위치 보정용 섭동 모델 상반력 모멘트(Mp), 및 상체자세 경사각 보정용 섭동 모델 상반력 모멘트(Mr)가 구해진다.

Mp=Mpf-Mfull …식h22

Mr=Mrf …식h23

즉, Mp 연산부(214)에서, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)로부터 풀 모델 상반력 모멘트 수평성분 Mfullxy를 뺌으로써, 상체 수평위치 보정용 섭동 모델 상반력 모멘트(Mp)가 얻어진다. 또, 상체자세 경사각 보정용 섭동 모델 상반력 모멘트(Mr)는 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)와 동일한 값이 된다.

이어서, 상체 수평위치 보정용 섭동 모델 상반력 모멘트(Mp)가 상체위치 보정용 섭동 모델(202)에 입력되고, 그 입력된 상반력 모멘트에 균형을 이루는 보정용 섭동 모델 상체 위치 Xc가 산출된다.

또, 상체자세 경사각 보정용 섭동 모델 상반력 모멘트(Mr)가 상체자세 경사각 보정용 섭동 모델(203)에 입력되고, 그 입력된 상반력 모멘트 수평성분에 균형을 이루는 보정용 섭동 모델 상체자세 경사각 θc가 산출된다.

이어서, 연산부(205, 204)에서, 각각 다음 식h24, 식h25에 따라, 보정 목표 상체 수평위치와 보정 목표 상체자세 경사각이 결정되고, 이것들이 각각 상체 수평위치 자세의 최종적인 목표 순시값으로서 출력된다.

보정 목표 상체 수평위치

=단순화 모델 상체 수평위치+보정용 섭동 모델 상체위치 Xc …식h24

보정 목표 상체자세 경사각

=단순화 모델 상체자세 경사각+보정용 섭동 모델 상체자세 경사각 θc

…식h25

즉, 단순화 모델 상체 수평위치에 보정용 섭동 모델 상체위치 Xc를 더함으로써 보정 목표 상체 수평위치가 얻어지고, 이것이 최종적인 목표 상체 수평위치로서 출력된다. 또, 단순화 모델 상체자세 경사각에 보정용 섭동 모델 상체자세 경사각 θc를 더함으로써 보정 목표 상체자세 경사각이 얻어지고, 이것이 최종적인 목표 상체자세 경사각으로서 출력된다.

또, 목표 ZMP 주위의 컴플라이언스 제어용 목표 상반력 모멘트 수평성분으로서 제한 보정 목표 상반력 모멘트 수평성분 Mltd가 출력되는 동시에, 보정 목표 상반력 수평성분으로서 제한 보정 목표 상반력 수평성분 Fltd가 출력된다.

즉, 다음 식h26, 식h27에 따라, 보정 목표 상반력 수평성분과 목표 ZMP 주위의 보정 목표 상반력 모멘트 수평성분이 각각, 상반력 수평성분 및 상반력 모멘트 수평성분(목표 ZMP 주위의 모멘트 수평성분)의 최종적인 목표 순시값으로서 결정되고, 이것들이 출력된다.

컴플라이언스 제어용 목표 상반력 모멘트 수평성분

=제한 보정 목표 상반력 모멘트 수평성분 Mltd …식h26

보정 목표 상반력 수평성분

=제한 보정 목표 상반력 수평성분 Fltd …식h27

S3536에서의 이상까지의 처리는, 본원 출원인에 의한 PCT/JP03/00435 대로이므로, 여기에서의 설명은 이상으로 마친다.

제 4 참고예에서는 또한, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)에서, 역위상 팔 흔들기 각 보정용 섭동 모델(231)에의 입력인 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)와 목표 상반력 모멘트 연직성분이 결정된다. 이어서, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)는 역위상 팔 흔들기 각 보정용 섭동 모델(231)에 입력되고, 보정용 섭동 모델 역위상 팔 흔들기 각 θca가 결정된다.

보충하면, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)는, 목표 순시값 발생부(100b)가 출력하는 기준 역위상 팔 흔들기 각(도 57의 S3404에서 구해진 것) 또는 단순화 모델에 의한 목표 역위상 팔 흔들기 각(도 57의 S3414에서 구해진 것)에, 보정 목표 역위상 팔 흔들기 각, 즉, 단순화 모델에 의한 목표 역위상 팔 흔들기 각에 보정용 섭동 모델 역위상 팔 흔들기 각을 더한 것이 정정 또는 추종하도록, 역위상 팔 흔들기 각 보정용 섭동 모델(231)의 상태에 따라, PI 제어 등의 피드백제어칙에 의해 피드백량(조작량)을 결정하고, 이것을 역위상 팔 흔들기 각 보정용 섭동 모델(231)에 부가적으로 입력한다.

이 제어칙을 역위상 팔 흔들기 각 보정용 섭동 모델 제어칙이라 부르고, 이 피드백량(조작량)을 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd라고 부른다. 여기에서 「요구값」을 붙인 것은 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd와 동일한 이유 때문이다. 제한이 가해져 수정된 모멘트를 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트(Maf)라고 부른다. 또한, 본 실시형태에서는, 이 Maf가 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)로서, 역위상 팔 흔들기 각 보정용 섭동 모델(231)에 입력된다.

역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd를 결정하는 역위상 팔 흔들기 각 보정용 섭동 모델 제어칙은, 구체적으로는, 다음 식과 같이 하면 된다.

역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd

=Kap*(보정용 섭동 모델 역위상 팔 흔들기 각 θca

-(기준 역위상 팔 흔들기 각 - 단순화 모델에 의한 목표 역위상 팔 흔들기 각))

+Kav*보정용 섭동 모델 역위상 팔 흔들기 각속도 dθca/dt

…식h30

단, Kap, Kav는 피드백제어칙의 게인이다.

또한, 식h30에 있어서, (기준 역위상 팔 흔들기 각 - 단순화 모델에 의한 목표 역위상 팔 흔들기 각) 대신에 0으로 해도 좋다.

이하에, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)의 동작에 대해 그 기능 블럭도인 도 70을 사용하여 상세를 설명한다.

먼저, 상기와 같이, 상기 역위상 팔 흔들기 각 보정용 섭동 모델 제어칙(230a)(식h30)에 의해 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd가 구해진다.

이어서, 상체 수평위치 보정용 섭동 모델(202)을 사용한 상체 수평위치 보정에 기인하는 상반력 모멘트 연직성분 섭동량 Mpz가, 상체 수평위치 보정에 기인하는 상반력 모멘트 연직성분 섭동량 산출부(230b)에 의해 구해진다.

구체적으로는, 다음 식에 의해 구해진다.

Mpz=(단순 모델 상체 질점위치

+보정용 섭동 모델 상체위치 Xc - 목표 ZMP)

*(단순 모델 상체 질점 수평가속도

+보정용 섭동 모델 상체가속도 d2Xc/dt2)

-(단순 모델 상체 질점위치-목표 ZMP)

*(단순 모델 상체 질점 수평가속도) …식h31

또는, 상기 식을 근사한 이하의 식을 사용해도 좋다.

Mpz≒보정용 섭동 모델 상체위치Xc

*(단순 모델 상체 질점 수평가속도

+보정용 섭동 모델 상체가속도 d2Xc/dt2)

+(단순 모델 상체 질점위치 - 목표 ZMP)

*보정용 섭동 모델 상체가속도 d2Xc/dt2) …식h32

다음에 상기 상반력 모멘트 연직성분 제약조건을 무시하고, 역위상 팔 흔들 기 각 보정용 섭동 모델 안정화 모멘트(Maf)를, 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd에 일치시킴과 동시에, 목표 ZMP 주위의 목표 상반력 모멘트 연직성분으로서의 컴플라이언스 제어용 목표 상반력 모멘트 연직성분을 보상 전체 상반력 모멘트 연직성분 Mdmdz와 Maf와 Mfullz와 Mpz와의 총합에 일치시켰다고 가정한 경우에, 목표 ZMP 주위에 발생하는 상반력 모멘트 연직성분 Minz가 연산부(230c)에 의해 구해진다. 이 상반력 모멘트 연직성분을 무제한시 보정 목표 상반력 모멘트 연직성분 Minz라고 부른다. 무제한시 보정 목표 상반력 모멘트 연직성분 Minz는 다음 식에 의해 구해진다.

Minz=Mafdmd+Mdmdz+Mfullz+Mpz …식h33

즉, 무제한시 보정 목표 상반력 모멘트 연직성분 Minz는 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd와 보상 전체 상반력 모멘트 연직성분 Mdmdz와 풀 모델 상반력 모멘트 연직성분 Mfullz와 상체 수평위치 보정용 섭동 모델 모멘트 연직성분 Mpz를 가산함으로써 얻어진다.

이어서, 제한수단(제한처리부)(230d)에 의해, 무제한시 보정 목표 상반력 모멘트 연직성분 Minz로부터, 이것에 제한을 부가한 값인(목표 ZMP 주위의) 제한 보정 목표 상반력 모멘트 연직성분 Mltdz가 결정된다. 제 4 참고예에서는, 컴플라이언스 제어용 목표 상반력 모멘트 연직성분은 제한 보정 목표 상반력 모멘트 연직성분 Mltdz에 일치한다.

제한 보정 목표 상반력 모멘트 연직성분 Mltdz는 상반력 모멘트 연직성분 허용범위내가 되도록 결정된다. 즉, Mltdz는 상반력 모멘트 연직성분 제약조건을 만 족하도록 결정된다.

또, 상기 상반력 모멘트 연직성분 제약 조건하에서, 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트(Maf)는, 가능한 한, 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd에 일치 또는 가까운 값이 되도록 결정된다. 이것에 의해, 상기 보정용 섭동 모델 역위상 팔 흔들기 각 θca를 안정화하고, 발산하는 것을 방지한다.

제한수단(제한처리부)(230d)는 다음 식에 도시하는 포화 특성을 가진 함수이다.

Minz>상반력 모멘트 연직성분 허용범위 상한값일 경우

Mltdz=상반력 모멘트 연직성분 허용범위 상한값

Minz<상반력 모멘트 연직성분 허용범위 하한값일 경우

Mltdz=상반력 모멘트 연직성분 허용범위 하한값

상반력 모멘트 연직성분 허용범위 하한값≤Minz, 또한, Minz≤상반력 모멘트 연직성분 허용범위 상한값일 경우

Mltdz=Minz ……식h34

이어서, 이하의 식에 의해 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)(=역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트(Maf))가 Maf 연산부(230e)에 의해 구해진다.

Ma=Mltdz-(Mdmdz+Mfullz+Mpz) …식h35

즉, Maf 연산부(230e)에서는, 제한 보정 목표 상반력 모멘트 연직성분 Mltdz 로부터, 보상 전체 상반력 모멘트 연직성분 Mdmdz와 풀 모델 상반력 모멘트 연직성분 Mfullz와 상체 수평위치 보정용 섭동 모델 모멘트 연직성분 Mpz를 뺌으로써, 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트(Maf)로서의 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)가 얻어진다.

한편, 목표 ZMP 주위의 컴플라이언스 제어용 목표 상반력 모멘트 연직성분으로서 제한 보정 목표 상반력 모멘트 연직성분 Mltdz가 출력된다.

즉, 다음 식h36에 따라, 목표 ZMP 주위의 보정 목표 상반력 모멘트 연직성분이 상반력 모멘트 연직성분(목표 ZMP 주위의 모멘트 연직성분)의 최종적인 목표 순시값으로서 결정되고, 이것들이 출력된다.

컴플라이언스 제어용 목표 상반력 모멘트 연직성분

=제한 보정 목표 상반력 모멘트 연직성분 Mltdz …식h36

이상과 같이, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)의 처리가 행해진 후, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)는 도 66의 역위상 팔 흔들기 각 보정용 모델(231)에 입력되고, 그 입력된 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)에 균형을 이루는 보정용 섭동 모델 역위상 팔 흔들기 각 θca가 식a23c를 사용하여(적분함으로써) 산출된다.

이어서, 연산부(232)에서, 다음 식h37에 따라, 보정 목표 역위상 팔 흔들기 각이 결정되고, 이것이 역위상 팔 흔들기 각의 최종적인 목표 순시값으로서 출력된다.

보정 목표 역위상 팔 흔들기 각

=단순화 모델 역위상 팔 흔들기 각

+보정용 섭동 모델 역위상 팔 흔들기 각 θca …식h37

즉, 단순화 모델 역위상 팔 흔들기 각에 보정용 섭동 모델 역위상 팔 흔들기 각 θca를 더함으로써 보정 목표 역위상 팔 흔들기 각이 얻어지고, 이것이 최종적인 목표 역위상 팔 흔들기 각으로서 출력된다.

이상과 같이, S3536의 보용 보정이 행해진다.

또한, 보충하면, 제 4 참고예는 피드포워드형의 보정이며, 또, 섭동 동역학 모델은 엄밀한 모델은 아니다. 이 때문에, 상기와 같이 식h5, 식h6 및 식h1006을 만족하도록 보용을 보정해도, 엄밀하게는, 동역학 평형조건을 만족하는 것이 아니라, 근사적으로 동역학 평형조건을 만족한다.

또, 제 4 참고예에서는 예를 들면 1보의 보용의 종단(금회 보용 종단)에 있어서, 상체 수평위치 보정용 섭동 모델(202)과 상체자세 경사각 보정용 섭동 모델(203)과 역위상 팔 흔들기 각 보정용 섭동 모델(231)과의 상태량, 예를 들면 상체 수평위치 보정용 섭동 모델(202)의 상체 질점(도립진자 질점)의 수평위치나, 상체자세 경사각 보정용 섭동 모델(203)의 플라이휠의 회전각이나, 역위상 팔 흔들기 각 보정용 섭동 모델(231)의 플라이휠의 회전각은 단순화 모델(200)의 상태량으로서 추가된다. 즉, 금회 보용 종단에 있어서의 단순화 모델(200)의 상태량이 상체 수평위치 보정용 섭동 모델(202)의 상체운동과 상체자세 경사각 보정용 섭동 모델(203)과 역위상 팔 흔들기 각 보정용 섭동 모델(231)과의 상태량을 더한 것으로 수정된다. 또한, 각 섭동 모델(202, 203, 231)의 상태량은 초기화 된다(예를 들면 상체 수평위치 보정용 섭동 모델(202)의 상체 질점(도립진자 질점)의 수평위치나, 상체자세 경사각 보정용 섭동 모델(203)의 플라이휠의 회전각이나, 역위상 팔 흔들기 각 보정용 섭동 모델(231)의 플라이휠의 회전각이 0으로 리셋됨). 그리고, 상기한 바와 같이 수정한 단순화 모델(200)의 상태량을 다음회 보용의 초기값으로 하고, 다음회 보용의 생성이 행해지는 동시에, 각 섭동 모델(202, 203, 231)에 의한 보정용 섭동 모델 상체 수평위치 Xc, 보정용 섭동 모델 상체 자세 경사각 θc 및 보정용 섭동 모델 역위상 팔 흔들기 각 θca의 산출이 행해진다. 이것에 의해 각 섭동 모델(202, 203, 231)의 거동의 안정성을 더한층 향상시킬 수 있다. 또한, 상기와 같은 단순화 모델(200)의 상태량의 수정은 보용의 생성도중에 적당하게 행해도 좋다. 반대로, 상기와 같은 단순화 모델(200)의 상태량의 수정이나 섭동 모델의 리셋을 보용의 생성도중에 행하지 않아도 좋다.

또, 상기와 같은 단순화 모델(200)의 상태량의 수정은 후술하는 제 5, 제 6 참고예에서도 동일하게 행해진다.

제 4 참고예에서는 상기 복원조건 모두를 만족한다. 따라서, 상기 제 3 참고예와 동일한 작용효과를 이룰 수 있다.

또한, 제 3 참고예의 작용효과와 더불어, 상기 복원조건을 만족하도록 상체 수평위치, 상체자세 경사각 및 역위상 팔 흔들기 각을 결정할 때에, 연산량을 비교적 적은 것으로 할 수 있다.

다음에 본 발명의 제 5 참고예를 이하에 도 71 및 도 72를 참조하여 설명한다. 제 5 참고예에 따른 장치의 보정수법은, 상기 제 4 참고예의 것과 보용생성 장치(100)의 처리(도 65의 S3536의 처리)만이 다른 것이며, 풀 모델 피드백 보정형이다. 또, 역동역학 풀 모델(역 풀 모델)을 사용하는 수법이고, 또한 단순화 모델 보용의 입력을 보정하지 않는 수법이며, 또한 섭동 모델을 사용하는 수법이다.

도 71은, 이 제 5 참고예에 따른 장치의 동작, 구체적으로는, 도 65의 플로우차트의 S3536의 보용의 수정수법을 설명하는 기능 블럭도이다. 단, 도 71의 단순화 모델(200)은, 상기 제 4 참고예와 동일하게 단지 동역학 모델뿐만 아니라, 도 65의 S3510부터 S3532의 처리, 즉 단순화 모델 보용 순시값의 산출(결정) 처리를 나타낸다. 따라서, 도 71에 있어서, 단순화 모델(200)로부터 앞의 부분이 S3536의 처리에 상당한다. 또한, 도 71의 기능부분중, 제 4 참고예에 따른 도 66과 동일한 기능분에 대해서는, 도 66과 동일한 참조부호를 사용하고, 상세한 설명을 생략한다.

S3536의 처리 이외의 구성은 제 4 참고예와 동일하므로 설명을 생략하고, 이하에 도 71을 사용하여 S3536의 처리를 상세하게 설명한다.

S3536에서는, 먼저, 상기와 같이 도 65의 S3532에서 구한 단순화 모델 보용의 목표 상체 수평위치의 순시값(현재 시각 t의 제어주기에서 구한 가순시값)에, 전회 제어주기(시각 t-△t)에서 후술하는 바와 같이 산출한 보정용 섭동 모델 상체 수평위치 Xc를 연산부(205)에 의해 더함으로써, 최종적인 목표 상체 수평위치(보정 목표 상체 수평위치)가 구해진다. 또, 도 65의 S3532에서 구한 단순화 모델 보용의 목표 상체자세 경사각의 순시값(현재 시각 t의 제어주기에서 구한 가순시값)에, 전회 제어주기(시각 t-△t)에서 산출한 후술하는 보정용 섭동 모델 상체자세 경사 각 θc를 연산부(204)에 의해 더함으로써, 최종적인 목표 상체자세 경사각(보정 목표 상체자세 경사각)이 구해진다. 또, 도 65의 S3532에서 구한 단순화 모델 보용의 목표 역위상 팔 흔들기 각의 순시값(현재 시각 t의 제어주기에서 구한 가순시값)에, 전회 제어주기(시각 t-△t)에서 산출한 후술하는 보정용 섭동 모델 역위상 팔 흔들기 각 θca를 연산부(232)에 의해 더함으로써, 최종적인 목표 역위상 팔 흔들기 각(보정 목표 역위상 팔 흔들기 각)이 구해진다.

그리고, 이들 보정 목표 상체 수평위치, 보정 목표 상체자세 경사각 및 보정 목표 역위상 팔 흔들기 각이, 각각 상체 수평위치, 상체자세 경사각 및 역위상 팔 흔들기 각의 최종적인 목표 순시값으로서 출력된다.

즉, 상기 식h24, 식h25, 식h37에 의해, 보정 목표 상체 수평위치와 보정 목표 상체자세 경사각과 보정 목표 역위상 팔 흔들기 각이 구해진다.

이어서, 상기와 같이 단순화 모델 보용을 보정하여 얻어진 목표 상체 수평위치(보정 목표 상체 수평위치), 목표 상체자세 경사각(보정 목표 상체자세 경사각), 목표 역위상 팔 흔들기 각(보정 목표 역위상 팔 흔들기 각), 및 상기와 같이 얻어진 단순화 모델 보용의 목표 중심위치, 목표 족평위치 자세, 목표 팔자세 등의 운동변수의 순시값과 목표 ZMP의 순시값을, 상기 역동역학 풀 모델(201)에 입력하고, 그 입력된 운동변수로 표시되는 운동에 균형을 이루는(즉 운동에 의해 역 풀 모델(201)이 발생하는) 상반력 수평성분과 목표 ZMP 주위의 상반력 모멘트(수평성분 및 연직성분)가 산출된다. 따라서, 본 제 5 참고예에서는, 역 풀 모델(201)에는, 단순화 모델 상체 수평위치, 상체자세 경사각, 역위상 팔 흔들기 각에 더하여, 보정 용 섭동 모델 상체 수평위치 Xc와 보정용 섭동 모델 상체자세 경사각 θc와 보정용 섭동 모델 역위상 팔 흔들기 각 θca가 추가적으로 입력된다. 이후, 제 4 참고예와 같이 역 풀 모델(201)에 의해 산출된 상반력 수평성분, 상반력 모멘트 수평성분, 상반력 모멘트 연직성분을 각각 풀 모델 상반력 수평성분 Ffullxy, 풀 모델 상반력 모멘트 수평성분 Mfullxy, 풀 모델 상반력 모멘트 연직성분 Mfullz라고 부른다.

풀 모델 상반력 수평성분 Ffull은 보정 목표 상반력 수평성분(현재 시각 t에서의 상반력 수평성분의 최종적인 목표 순시값)으로서 출력된다.

즉, 이하의 식에 의해, 보정 목표 상반력 수평성분이 결정되어 출력된다.

보정 목표 상반력 수평성분=풀 모델 상반력 수평성분 Ffull

…식h48

이상의 처리로부터 알 수 있는 바와 같이, 제 5 참고예에서는 풀 모델 보정 보용은 단순화 모델 보용에 상체 수평위치 보정용 섭동 모델(202)의 거동과 상체자세 경사각 보정용 섭동 모델(203)의 거동과 역위상 팔 흔들기 각 보정용 섭동 모델(231)의 거동을 더한 것이 된다. 또한, 상체 경사 모드가 발생하는 상반력 수평성분 및 역위상 팔 흔들기 모드가 발생하는 상반력 수평성분이 모두 0인 것을 고려하면, 다음 식이 성립한다. 단, 단순화 모델 상반력 수평성분은 단순화 모델 보용의 운동에 의해 발생하는 상반력을 역 풀 모델(201)을 사용하여 산출한 것의 병진력 수평성분이다.

풀 모델 상반력 수평성분 Ffull

=단순화 모델 상반력 수평성분

+상체 수평위치 보정용 섭동 모델 상반력 수평성분 Fp

…식h51

이어서, 상체 수평위치 보정용 섭동 모델 제어칙(206)에 의해, 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd가 결정된다. 본 실시형태에서의 상체 수평위치 보정용 섭동 모델 제어칙(206)은 일본 특원 2001-133621호에서 본원 출원인이 제안한 바와 같이 설정된다. 예를 들면, 다음 식과 같이 이 제어칙(206)이 결정된다.

상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd

=Kpg*중심차+Kvg*보정용 섭동 모델 상체 수평속도 dXc/dt …식h52

단, 중심차는 다음 식으로 구해진다.

중심차 = 풀 모델 중심 수평위치 - 단순화 모델 중심 수평위치

…식h53

식h52에서의 Kpg, Kvg는 피드백제어칙의 게인이며, 풀 모델 중심 수평위치, 단순화 모델 중심 수평위치 및 보정용 섭동 모델 상체 수평속도 dXc/dt는, 각각, 전회(시각 t-△t)에 후술하는 바와 같이 산출한 풀 모델 보용 순시값의 중심 수평위치, 단순화 모델 보용 순시값의 중심 수평위치(단순화 모델 보용의 순간자세를 기초로 단순화 모델을 사용하여 계산되는 중심 수평위치 XGs), 및 보정용 섭동 모델 상체 수평속도 dXc/dt이다.

즉, 풀 모델 중심 수평위치로부터 단순화 모델 중심 수평위치를 감산하여 얻 은 중심차와 섭동 모델의 상태량의 하나인 섭동 모델 상체속도에 기초하여 섭동 모델 제어용 피드백량(조작량)을 연산하도록 했다. 이러한 섭동 모델 제어칙에 의해, 상기 중심차의 시간적 평균값을 거의 0으로 제어할 수 있다.

이어서, 상체자세 경사각 보정용 섭동 모델 제어칙(207)에 의해, 상체자세 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd가 결정된다. 이것은, 제 4 참고예와 동일한 제어칙을 사용하면 된다. 따라서, 이 제어칙(207)으로서는, 예를 들면 상기 식h11이 사용된다.

이어서, 무제한시 보정 목표 상반력 모멘트(Min)가 Min 연산부(209)에 의해 구해진다(추정됨). 무제한시 보정 목표 상반력 모멘트(Min)은, 제 4 참고예와 동일하게 상기 제약(ZMP 제약조건 및 상반력 수평성분 제약조건)을 무시하고, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd에 일치시키고, 또한, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)를, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd에 일치시키는 동시에, 목표 ZMP 주위의 목표 상반력 모멘트로서의 컴플라이언스 제어용 목표 상반력 모멘트 수평성분을 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 Mpf와 Mrf와의 총합에 일치시킨 경우에, 목표 ZMP 주위에 발생하는 상반력 모멘트 수평성분이다.

무제한시 보정 목표 상반력 모멘트 수평성분 Min은, 제 4 참고예와 동일하게, 상기 식h13의 연산에 의해 구해진다. 즉, 무제한시 보정 목표 상반력 모멘트 수평성분 Min은 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd와 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd와 보상 전체 상반력 모멘트 수평성분 Mdmdxy를 가산함으로써 얻어진다.

또한, Fin 연산부(210)에 의해, 무제한시 보정 목표 상반력 수평성분 Fin도 구해진다(추정됨). 무제한시 보정 목표 상반력 수평성분 Fin은 제 4 참고예와 동일하게, 상기 제약(ZMP 제약조건 및 상반력 수평성분 제약조건)을 무시하고, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)를 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd에 일치시키고, 또한, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)를, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트 요구값 Mrfdmd에 일치시킴과 동시에, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분을 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 Mpf와 Mrf와의 총합에 일치시킨 것으로 한 경우에, 역 풀 모델(201)이 발생하는 상반력 수평성분(Ffull에 상당)이다.

무제한시 보정 목표 상반력 수평성분 Fin은 제 4 참고예와는 달리, 다음 식에 의해 구해진다.

무제한시 보정 목표 상반력 수평성분 Fin

=풀 모델 상반력 수평성분 Ffull

+1/h*(상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd

-상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)) …식h54

단, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)는 전회값(시각 t-△t에서의 값)이 사용된다. 즉, 상체 수평위치 보정용 섭동 모델 안정화 모멘트 요구값 Mpfdmd와 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)의 차를 구하고, 상체 수평위치 보정용 섭동 모델의 입력을 상기 차만큼 증가시키는 것에 기인하는 풀 모델 상반력 수평성분 Ffull의 증가량을 상기 차를 상체 병진 모드 상반력 비율 h로 나눔으로써 추정한다. 또한, 이것에 풀 모델 상반력 수평성분 Ffull을 더함으로써, 무제한시 보정 목표 상반력 수평성분 Fin이 추정된다.

이어서, 제 4 참고예와 동일한 제한수단(제한처리부(211))에 의해, 무제한시 보정 목표 상반력 모멘트 수평성분 Min과 무제한시 보정 목표 상반력 수평성분 Fin으로부터, 상기 제약(ZMP 제약조건 및 상반력 수평성분 제약조건)을 만족하도록, 이것들에 제한을 가한 값인(목표 ZMP 주위의) 제한 보정 목표 상반력 모멘트 수평성분 Mltd와 제한 보정 목표 상반력 수평성분 Fltd가 결정된다. 이 처리수법은 제 4 참고예와 동일하다.

제 5 참고예에서도, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분은 제한 보정 목표 상반력 모멘트 수평성분 Mltd에 일치되고, 또, 보정 목표 상반력 수평성분은 제한 보정 목표 상반력 수평성분 Fltd에 거의 일치하므로, 상기와 같이 제한 보정 목표 상반력 모멘트 수평성분 Mltd와 제한 보정 목표 상반력 수평성분 Fltd를 결정함으로써, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분 및 보정 목표 상반력 수평성분은 각각 ZMP 제약조건 및 상반력 수평성분 제약조건을 거의 만족한다.

이어서, Mpf 연산부(215)에 의해 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)가 구해진다. 더욱 상세하게는, 제한 보정 목표 상반력 수평성분 Fltd로 부터 풀 모델 상반력 수평성분 Ffull을 뺀 값에 게인 Kc를 곱한 것을 적분기(215a)로 적분하고, 또한 얻어진 적분값에 상체 병진 모드 상반력 비율 h를 곱함으로써, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)가 구해진다. 즉, 다음 식에 의해, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)가 얻어진다.

Mpf=h*∫Kc(Fltd-Ffull)dt …식h55

이어서, Mrf 연산부(214)에 의해, 제한 보정 목표 상반력 모멘트 수평성분 Mltd로부터 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)와 보상 전체 상반력 모멘트 수평성분 Mdmdxy를 뺌으로써, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)가 구해진다. 즉, 상기 식h21에 의해, 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)가 얻어진다.

또한, 상기 식h23에 의해, 상체자세 경사각 보정용 섭동 모델 상반력 모멘트(Mr)가 결정된다. 즉, Mrf 연산부(214)의 출력인 상체자세 경사각 보정용 섭동 모델 안정화 모멘트(Mrf)가 그대로 상체자세 경사각 보정용 섭동 모델 상반력 모멘트(Mr)로서 결정된다.

이어서, 다음 식으로 정의되는 풀 모델 상반력 모멘트 오차 Merr이 Merr 연산부(216)에 의해 산출된다.

풀 모델 상반력 모멘트 오차 Merr

=풀 모델 상반력 모멘트 수평성분 Mfullxy

-제한 보정 목표 상반력 모멘트 수평성분 Mltd …식h56

이어서 Mp 연산부(217)에 의해, 다음 식에 따라, 상체 수평위치 보정용 섭동 모델 상반력 모멘트(Mp)가 구해진다.

Mp=Mpf-∫Km*Merr dt …식h57

즉, 풀 모델 상반력 모멘트 오차 Merr에 적분 게인 Km을 곱한 값을 적분기(217a)로 적분하는 동시에, 그 적분값의 부호를 반전한다. 또한, 이 적분기(217a)의 출력을 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)에 부가함으로써, 상체 수평위치 보정용 섭동 모델 상반력 모멘트(Mp)가 구해진다.

이어서, 상체 수평위치 보정용 섭동 모델 상반력 모멘트(Mp)를 상체위치 보정용 섭동 모델(202)에 입력하고, 입력된 상반력 모멘트 수평성분에 균형을 이루는 보정용 섭동 모델 상체위치 Xc가 산출된다.

또, 상체자세 경사각 보정용 섭동 모델 상반력 모멘트(Mr)를 상체자세 경사각 보정용 섭동 모델(203)에 입력하고, 입력된 상반력 모멘트 수평성분에 균형을 이루는 보정용 섭동 모델 상체자세 경사각 θc가 산출된다.

S3536에 있어서의 이상까지의 처리는, PCT/JP03/00435 대로이므로, 여기에서의 설명은 이상으로 마친다.

제 5 참고예에서는 또한, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)에 있어서, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 Ma가 결정된다.

또한 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)가 역위상 팔 흔들기 각 보정용 섭동 모델에 입력되어, 보정용 섭동 모델 역위상 팔 흔들기 각 θca가 결정된다.

이하에, 제 5 참고예에서의 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)의 동작에 대해 그 기능 블럭도인 도 72를 사용하여 상세를 설명한다.

먼저, 역위상 팔 흔들기 각 보정용 섭동 모델 제어칙에 의해, 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd가 결정된다. 이것은 제 4 참고예와 동일한 제어칙을 사용하면 된다. 따라서, 이 제어칙으로서는, 예를 들면 상기 식h30이 사용된다.

이어서, 연산부(230g)에서, 풀 모델 상반력 모멘트 연직성분 Mfullz와 보상 전체 상반력 모멘트 연직성분 Mdmdz와 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd와의 합으로부터 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)의 전회값(후술하는 적분기의 출력)을 뺌으로써 무제한시 보정 목표 상반력 모멘트 연직성분 Minz를 구한다. 이어서, 제한수단(제한처리부)(230h)에서, 무제한시 보정 목표 상반력 모멘트 연직성분 Minz를 상반력 연직성분 모멘트 허용범위를 초과하지 않도록 제한을 가함으로써(즉 도시하는 포화특성 함수에 통과시켜서), 제한시 보정 목표 상반력 모멘트 연직성분 Mltdz를 구한다. 이어서, 풀 모델 상반력 모멘트 연직성분 Mfullz와 보상 전체 상반력 모멘트 연직성분 Mdmdz와 역위상 팔 흔들기 각 보정용 섭동 모델 안정화 모멘트 요구값 Mafdmd와의 합을 제한시 보정 목표 상반력 모멘트 연직성분 Mltdz로부터 뺀 값을 적분기(230i)에 의해, 적분 게인 Ka로 적분함으로써 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)를 구하고, 이것을 출력한다. 또, 제한시 보정 목표 상반력 모멘트 연직성분 Mltdz를 컴플라이언스 제어용 목표 상반력 모멘트 연직성분으로서 출력한다.

이상과 같이, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)에 있어서, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)와 컴플라이언스 제어용 목표 상반력 모멘트 연직성분이 결정된다.

구한 보정용 섭동 모델 상체위치 Xc, 보정용 섭동 모델 상체자세 경사각 θc, 보정용 섭동 모델 역위상 팔 흔들기 각 θca, 상체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf) 및 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)는 상기와 같이 다음회 제어주기(시각 t+△t)에 있어서, 전회값으로서 사용된다.

잔여 구성 및 처리는 제 4 참고예와 동일하다. 이 제 5 참고예에 의하면, 제 4 참고예와 동일한 작용효과를 이룰 수 있다.

다음에, 본 발명의 제 6 참고예를 도 73 및 도 74를 참조하여 설명한다. 제 6 참고예는 상기 제 4 및 제 5 참고예에서 사용한 역동역학 풀 모델(역 풀 모델)(201) 대신에, 순동역학 모델(보다 정확하게는 의사 순동역학 모델)을 사용하여 보용을 수정하면서 생성하는 수법이다.

도 73은 이 제 6 참고예에 따른 장치의 동작을 설명하는 기능 블럭도이다. 동 도 73에 도시하는 바와 같이, 본 제 6 참고예에서는 의사 순 풀 모델(의사 순동역학 풀 모델)(222)을 갖추고 있다.

이 의사 순 풀 모델(222)은 상체 수평위치 안정화 모멘트 요구값 Mpfdmd, 상체자세 경사각 안정화 모멘트 요구값 Mrfdmd, 역위상 팔 흔들기 각 안정화 모멘트 요구값 Mafdmd, 목표 ZMP, 목표 상반력 연직성분, 보상 전체 상반력 모멘트 수평성분 Mdmdxy 및 보상 전체 상반력 모멘트 연직성분 Mdmdz와, 목표 족평위치 자세 및 목표 팔자세 등 상체(3)를 제외한 부위의 운동상태를 입력으로 하고, 목표 상체 연직위치, 목표 상체 수평위치, 목표 상체자세 경사각, 목표 역위상 팔 흔들기 각, 목표 ZMP 주위의 목표 상반력 모멘트로서의 컴플라이언스 제어용 목표 상반력 모멘트(수평성분 및 연직성분), 및 목표 상반력 수평성분을 출력하는 모델이다. 또한, 의사 순 풀 모델(222)의 목표값 입력은, 상기 도 63을 참조하여 설명한 바와 같이, 보용 패러미터 결정부(100a)에서 결정되는 보용 패러미터를 기초로, 목표 순시값 발생부(100b)에 의해 생성된다.

상기 의사 순 풀 모델(222)은 구체적으로는 도 74의 기능 블럭도로 표시되고, 도 74의 파선으로 둘러싼 부분이 의사 순 풀 모델(222)에 상당한다. 또한, 이 기능 블럭도에서 상기 제 5 참고예의 도 71과 동일기능 부분에 대해서는 도 71과 동일한 참조부호를 사용하고 있다.

여기에서, 동 도 74의 단순화 모델(200)은 단지 동역학 모델뿐만 아니라, 전술의 도 65의 S3510부터 S3532의 처리, 즉 단순화 모델 보용 순시값의 산출(결정)처리를 나타낸다. 또, S3532의 금회 보용 순시값(단순화 모델 보용 순시값)의 산출(결정)처리에 있어서는, 상기 제 4 참고예에서 설명한 바와 같이 목표 ZMP 주위의 모델 조작 상반력 모멘트 수평성분을 0으로 하고 보용의 순시값을 생성한 뒤에, 또한 제 4 참고예에서 기술한 상체자세 경사각 보정용 섭동 모델 모멘트(Mr)에 상당하는 단순화 보용 상체자세 경사각 보정용 모멘트(Mr)(전회값)을 발생하는 상체 경사 모드의 섭동운동과 제 4 참고예에서 기술한 상체 수평위치 보정용 섭동 모델 모멘트(Mp)에 상당하는 단순화 모델 상체 수평위치 보정용 모멘트(Mp)(전회값)를 발생하는 상체 병진 모드의 섭동운동과 제 4 참고예에서 기술한 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)에 상당하는 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Ma)(전회값)를 발생하는 역위상 팔 흔들기 모드의 섭동운동을 부가한다. 이것에 의해, 단순화 모델(200)이 출력하는 보용의 순시값을 보정한다.

보다 구체적으로 설명하면 본 제 6 참고예에서의 도 65의 S3532의 처리에 있어서는, 그 서브루틴 처리인 도 57의 S3414와 S3416에 있어서, 단순화 모델(200)에서 구한 상체 수평가속도에, 상기 식03y의 우변 제 2 항을 Mp(전회값)와 동일하게 한 식, 즉 Mp=mb*(Zb-Zzmp)*(d2Xb/dt2)라는 식에 기초하여 구해지는 상체 수평가속도(d2Xb/dt2)를 섭동분으로서 더하여 이루어지는 상체 수평가속도를 금회 보용의 초기부터 현재 시각 t까지 2차적분 함으로써, 현재 시각 t에 있어서의 상체 수평위치의 순시값이 구해진다. 또, 도 57의 S3414에 있어서, 단순화 모델(200)에서 구한 상체자세 경사 각가속도에, 상기 식03y의 우변 제 7 항을 Mr(전회값)과 동일한 것으로 한 식, 즉, Mr=J*d2θby/dt2라는 식에 기초하여 구해지는 상체자세 경사 각가속도(d2θby/dt2)를 섭동분으로서 더하여 이루어지는 상체자세 경사 각가속도를 금회 보용의 초기부터 현재 시각 t까지 2차적분 함으로써, 현재 시각 t에서의 상체자세 경사각의 순시값이 구해진다. 또, 도 57의 S3414에서, 단순화 모델(200)에서 구한 역위상 팔 흔들기 각가속도에, 상기 식03z의 우변 제 8 항을 Ma(전회값)과 동일한 것으로 한 식, 즉, Ma=Jaz*d2θaz/dt2라는 식에 기초하여 구해지는 역위상 팔 흔들기 각가속도(d2θaz/dt2)를 섭동분으로서 더하여 이루어지는 역위상 팔 흔들기 각가속도를 금회 보용의 초기로부터 현재 시각 t까지 2차적분 함으로써, 현재 시각 t에 있어서의 역위상 팔 흔들기 각의 순시값이 구해진다.

보충하면, 상체 수평위치와 상체자세 경사각에 대해서는, 사지탈 플레인상의 순시값에 대해 기술했는데, 래터럴 플레인상의 순시값도 마찬가지로 구한다.

도 74에 있어서, 단순화 모델(200)로부터 앞의 부분이 S3536의 처리에 상당하는 처리를 실행하는 부분이다. 이하에 도 74를 사용하여 S3536의 처리를 상세하게 설명한다.

S3536에서는, 먼저, S3032에서 상기한 바와 같이 단순화 모델 상체 수평위치 보정용 모멘트(Mp)(상세하게는 시각(t-△t)의 제어주기에서의 전회값)를 기초로 보정된 단순화 모델 수평 상체위치가 목표 상체 수평위치(시각 t에서의 상체 수평위치의 최종적인 목표 순시값)로서 출력된다. 또, 단순화 모델 상체자세 경사각 보정용 모멘트(Mr)(상세하게는 시각(t-△t)의 제어주기에서의 전회값)을 기초로 보정된 단순화 모델 상체자세 경사각이 목표 상체자세 경사각(시각 t에서의 상체자세 경사각의 최종적인 목표 순시값)으로서 출력된다. 또, 단순화 모델 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)(상세하게는 시각(t-△t)의 제어주기에서의 전회값)를 기초로 보정된 단순화 모델 역위상 팔 흔들기 각이 목표 역위상 팔 흔들기 각(시각 t에서의 역위상 팔 흔들기 각의 최종적인 목표 순시값)으로서 출력된다.

즉, 식h100, 식h101, 식h102에 의해, 최종적인 목표 상체 수평위치와 목표 상체자세 경사각이 결정된다.

목표 상체 수평위치=단순화 모델 상체 수평위치 …식h100

목표 상체자세 경사각=단순화 모델 상체자세 경사각 …식h101

목표 역위상 팔 흔들기 각=단순화 모델 역위상 팔 흔들기 각 …식h102

이어서 목표 상체 수평위치(즉 단순화 모델 상체 수평위치), 목표 상체자세 경사각(즉 단순화 모델 상체자세 경사각), 목표 역위상 팔 흔들기 각(즉 단순화 모델 역위상 팔 흔들기 각) 및 상기와 같이 얻어진 단순화 모델 보용의 목표 전체 중심위치, 목표 족평위치 자세, 목표 팔자세 등의 운동변수의 순시값과 목표 ZMP의 순시값을, 상기 역동역학 풀 모델(역 풀 모델)(201)에 입력하고, 입력된 운동변수로 표시되는 운동에 균형을 이루는(즉 운동에 의해 역 풀 모델(201)이 발생하는) 상반력 수평성분과 목표 ZMP 주위의 상반력 모멘트(수평성분 및 연직성분)이 산출된다. 이후, 제 5 참고예와 동일하게, 이들 산출된 상반력 수평성분, 상반력 모멘트 수평성분 및 상반력 모멘트 연직성분을 각각 풀 모델 상반력 수평성분 Ffull, 풀 모델 상반력 모멘트 수평성분 Mfullxy, 상반력 모멘트 연직성분 Mfullz라고 부른다.

제 5 참고예와 동일하게, 풀 모델 상반력 수평성분 Ffull은 목표 상반력 수평성분(시각 t에서의 상반력 수평성분의 최종적인 목표 순시값)으로서 출력된다.

본 제 6 참고예에서는 상체자세 경사각 보정용 섭동 모델과 상체 수평위치 보정용 섭동 모델과 역위상 팔 흔들기 각 보정용 섭동 모델을 갖지 않으므로, 상체 수평위치 보정용 섭동 모델 제어칙과 상체자세 경사각 보정용 섭동 모델 제어칙과 역위상 팔 흔들기 각 보정용 섭동 모델 제어칙에 상당하는 처리는, 후술하는 바와 같이, 제 5 참고예와는 다르다.

이것을 제외하면, 상기 처리 이후, 단순화 모델 상체자세 경사각 보정용 모멘트(Mr)와 단순화 모델 상체 수평위치 보정용 모멘트(Mp)와 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Ma)를 결정할 때까지는, 제 5 참고예에서, 상체자세 경사각 보정용 섭동 모델 모멘트(Mr)와 상체 수평위치 보정용 섭동 모델 모멘트(Mp)와 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)를 결정하는 처리와 동일한 처리가 실행된다. 즉, Min 연산부(209), Fin 연산부(210), 제한처리부(211)(제한수단), Mpf 연산부(215), Merr 연산부(216), Mrf 연산부(217)(=Mr 연산부), Mp 연산부(214)의 처리는 제 5 참고예와 동일하다. 또한, 역위상 팔 흔들기 각 보정용 섭동 모델 제어칙에 상당하는 처리를 제외하면, 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트 결정부(230)의 처리는, 상기 제 5 참고예에서의 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)의 처리와 동일하다. 또한, 본 제 6 참고예에서는 Mr, Mp 및 Ma는, 각각 제 5 참고예에 있어서의 상체자세 경사각 보정용 섭동 모델 모멘트(Mr)와 상체 수평위치 보정용 섭동 모델 모멘트(Mp)와 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)에 각각 대응하는 것이지만, 제 5 참고예와 같이 섭동 모델에 입력하는 것이 아니라, 단순화 모델(200)에 입력하는 것이다. 이 때문에, Mr, Mp 및 Ma의 명칭을 본 제 6 참고예에서는, 단순화 모델 상체자세 경사각 보정용 모멘트, 단순화 모델 상체 수평위치 보정용 모멘트, 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트라고 칭하고 있다.

상기와 같이 구해진 단순화 모델 상체자세 경사각 보정용 모멘트(Mr)와 단순화 모델 상체 수평위치 보정용 모멘트(Mp)와 단순화 모델 역위상 팔 흔들기 각 보 정용 모멘트(Ma)는, 상기와 같이 다음회 제어주기(시각 t+△t)에, 단순화 모델 보용 순시값의 결정(생성)시에 전회값으로서 사용된다.

잔여 구성 및 처리는 제 5 참고예와 동일하다.

이하에, 상체자세 경사각 안정화 모멘트 요구값 Mrfdmd와 상체 수평위치 안정화 모멘트 요구값 Mpfdmd와 역위상 팔 흔들기 각 안정화 모멘트 요구값 Mafdmd와의 결정처리에 대해 도 73을 참조하여 설명한다.

도 73에 도시하는 바와 같이, 제 6 참고예에서는, 전술한 바와 같이 의사 순 풀 모델(222)에 구비하는 단순화 모델(200)과는 다른 단순화 모델(223)을 구비하고 있다. 이 단순화 모델(223)은 본 제 6 참고예에서의 그 기능은 전술한 단순화 모델(200)과 동일하고, 단지 동역학 모델뿐만 아니라, 전술의 도 65의 S3510부터 S3532의 처리, 즉 단순화 모델 보용 순시값의 산출(결정)처리를 나타내는 것이다. 또한, 단순화 모델(223)은 실제상은, 상체자세 경사각의 순시값과, 상체 수평위치의 순시값을 결정할 수 있으면 좋으며, 반드시 도 65의 S3510부터 S3532의 처리 모두를 행할 필요는 없다.

이하에 도 73을 사용하여 제 6 참고예에서의 Mpfdmd 및 Mrfdmd의 결정처리를 상세하게 설명한다.

본 제 6 참고예에서는, 단순화 모델(223)을 사용하여 생성되는 보용과 상기 의사 순동역학 풀 모델(222)을 사용하여 상기한 바와 같이 산출되는 보용과의 사이의 상체 수평위치, 상체자세 경사각 및 역위상 팔 흔들기 각의 각각의 차가 연산부(224, 225, 228)에 의해 구해진다. 그리고, 이들 차에 따라, 이들 차가 0에 수렴 하도록 PID 등의 피드백제어칙에 의해 상체 수평위치 안정화 모멘트 요구값 Mpfdmd, 상체자세 경사각 안정화 모멘트 요구값 Mrfdmd 및 역위상 팔 흔들기 각 안정화 모멘트 요구값 Mafdmd가 결정된다. 즉, 단순화 모델(223)에 의한 상체 수평위치와 의사 순 풀 모델(222)에 의한 상체 수평위치와의 차에 따라, 피드백제어칙으로 이루어지는 상체 수평위치 안정화 제어칙(226)에 의해, Mpfdmd가 구해진다. 또, 단순화 모델(223)에 의한 상체자세 경사각과 의사 순 풀 모델(222)에 의한 상체자세 경사각과의 차에 따라, 피드백제어칙으로 이루어지는 상체자세 경사각 안정화 제어칙(227)에 의해 Mrfdmd가 구해진다. 또, 단순화 모델(223)에 의한 역위상 팔 흔들기 각과 의사 순 풀 모델(222)에 의한 역위상 팔 흔들기 각과의 차에 따라, 피드백제어칙으로 이루어지는 역위상 팔 흔들기 각 안정화 제어칙에 의해 Mafdmd가 구해진다. 그리고, 이 결정된 Mpfdmd, Mrfdmd 및 Mafdmd가 상기 의사 순동역학 풀 모델에 피드백되어서 입력된다.

또한, 제 6 참고예에서는 보용생성 장치(100)는, 상기 의사 순동역학 풀 모델(222)에의 입력의 일부인 목표 ZMP, 목표 상반력 연직성분, 목표 족평위치 자세 및 목표 팔자세 등과, 상기 의사 순동역학 풀 모델(222)로부터의 출력인 목표 상체 연직위치, 목표 상체 수평위치, 목표 상체자세 경사각, 목표 역위상 팔 흔들기 각, 목표 상반력 수평성분 및 컴플라이언스 제어용 목표 상반력 모멘트(수평성분 및 연직성분)를 최종적인 금회 보용의 목표 순시값으로서 출력한다.

이상 설명한 제 6 참고예에 의하면, 상기 제 5 참고예와 동일한 작용효과를 이룰 수 있다.

또한, 상기 제 6 참고예에서는 상체자세 경사각 안정 모멘트 요구값 Mrfdmd, 상체 수평위치 안정화 모멘트 요구값 Mpfdmd 및 역위상 팔 흔들기 각 안정화 모멘트 요구값 Mafdmd를 의사 순 풀 모델(222)에만 입력하도록 했지만, Mrfdmd, Mpfdmd 및 Mafdmd를 단순화 모델(223)에 입력하도록 해도 좋고, 또는, 단순화 모델(223)과 의사 순 풀 모델(222)에 분배·공급하도록 해도 좋다. 또, 보상 전체 상반력 모멘트 연직성분 Mdmdz를 단순화 모델(223)에 입력하고, 이 Mdmdz가 부가적으로 발생하도록 단순화 모델 역위상 팔 흔들기 각을 결정하도록 해도 좋다.

다음에 본 발명의 제 7 참고예를 도 75를 참조하여 설명한다. 도 75는 본 발명의 제 7 참고예에 따른 장치의 동작, 구체적으로는 도 65의 플로우차트의 S3536의 보용의 수정 수법을 설명하는 기능 블럭도이다. 또한, 도 75에서는, 제 4 참고예 혹은 제 5 참고예와 동일기능 부분에 대해서는, 도 66 또는 도 71과 동일한 참조부호를 사용한다.

제 7 참고예에서는, 상체 수평위치 보정용 섭동 모델(202)과 상체자세 경사각 보정용 섭동 모델(203)과 역위상 팔 흔들기 각 보정용 섭동 모델(231)을 구비한다. 또, 3개의 분배기(220, 221, 241)를 구비한다.

분배기(220, 221, 241)를 정의하면, 모두, 1입력2출력의 전달 블록이고, 입력에 따라, 일방의 출력을 결정하고(예를 들면 입력에 주파수특성, 불감대 특성, 포화 특성 등의 신호처리를 행하여 일방의 출력을 결정하고), 2개의 출력의 합이 입력에 일치 또는 거의 일치하도록, 타방의 출력을 결정하는 전달 블록이다.

분배기(220)에는, 제한 보정 목표 상반력 모멘트 수평성분 Mltdxy로부터, 상 체 수평위치 보정용 섭동 모델 안정화 모멘트(Mpf)와 보상 전체 상반력 모멘트 수평성분 Mdmdxy를 뺀 것(Mr 연산부(214)의 출력)인 상체자세 경사각 보정용 모멘트(Mr)가 입력되고, 상체자세 경사각 보정용 섭동 모델(203)에 입력하는 상체자세 경사각 보정용 섭동 모델 입력(Mri)과, 단순화 모델(200)에 입력하는 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)에 분배된다. 이때, 상체자세 경사각 보정용 섭동 모델 입력(Mri)과 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)의 합이 상체자세 경사각 보정용 모멘트(Mr)에 일치하도록, 상체자세 경사각 보정용 섭동 모델 입력(Mri)과 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)가 결정된다(출력됨).

보다 구체적으로는, 상체자세 경사각 보정용 모멘트(Mr)에 따라, 상체자세 경사각 보정용 섭동 모델 입력(Mri)가 결정된다. 예를 들면, 상체자세 경사각 보정용 모멘트(Mr)를 불감대 특성, 포화 특성, 혹은 주파수특성을 갖는 신호처리를 통하여, 상체자세 경사각 보정용 섭동 모델 입력(Mri)가 결정된다. 또, 상체자세 경사각 보정용 모멘트(Mr)로부터 상체자세 경사각 보정용 섭동 모델 입력(Mri)를 뺀 것이, 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)로서 결정된다. 더욱 구체적으로 설명하면 본 실시형태에서는 분배기(220)는 예를 들면, 입력(상체자세 경사각 보정용 모멘트(Mr=Mltd-Mpf-Mdmdxy)를 로 패스 필터에 통과시켜서 이루어지는 저주파 성분(직류분)을 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)로서 출력하는 동시에, 그 입력(상체자세 경사각 보정용 모멘트(Mr))으로부터 Mrs를 뺀 성분이 상체자세 경사각 보정용 섭동 모델 입력(Mri)로서 출력된다. 이 경우, 저 주파 성분(직류분)인 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)에 관해서는, 불감대 특성을 갖게 해 두고, 상기 로 패스 필터의 출력이 어떤 소정값을 중심으로 하는 소정범위내에 존재하는 상태에서는, Mrs가 이 소정값(예를 들면 0)에 유지되도록 해 둔다.

분배기(220)의 출력인 상체자세 경사각 보정용 섭동 모델 입력(Mri)는 상기 상체자세 경사각 보정용 섭동 모델(203)에 입력되고, 상체자세 경사각 보정용 섭동 모델(203)에 의해 보정용 섭동 모델 상체자세 경사각 θc가 결정된다.

분배기(220)의 또하나의 출력인 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)는 단순화 모델(200)에 입력된다. 이것은, 상기 제 6 참고예에 있어서의 도 74에서, 단순화 모델 상체자세 경사각 보정용 모멘트(Mr)를 단순화 모델(200)에 입력하는 것에 해당하는 것이다.

분배기(221)에서는, 풀 모델 상반력 모멘트 수평성분 오차 Merr에 게인 Km을 곱한 값을 적분기(217a)에 의해 적분하고, 또한 부호를 반전한 값을 입력으로 한다.

분배기(221)의 입력은 분배기(220)와 동일하게, 단순화 모델(200)에 입력하는 단순화 모델 상체 수평위치 보정용 모멘트(Mps)와, 상체 수평위치 보정용 섭동 모델(202)에 입력하는 오차보정 모멘트(Me)에 분배된다. 보다 구체적으로는, 적분기(217a)의 출력에 따라, 오차보정 모멘트(Me)가 결정된다. 예를 들면 적분기(217a)의 출력(분배기(220)의 입력)을 불감대 특성, 포화 특성, 또는 주파수특성을 갖는 신호처리를 통하여, 오차보정 모멘트(Me)가 결정된다. 또, 적분기(217a)의 출력으로부터, 오차보정 모멘트(Me)를 뺀 것이 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)로서 결정된다. 더욱 구체적으로 설명하면 본 제 7 참고예에서는 분배기(221)는 예를 들면, 입력(적분기(217a)의 출력)을 로 패스 필터에 통과시켜서 이루어지는 저주파 성분(직류분)을 단순화 모델 상체 수평위치 보정용 모멘트(Mps)로서 출력하는 동시에, 그 입력(적분기(217a)의 출력)으로부터 Mps를 뺀 성분을 오차보정 모멘트(Me)로서 출력한다. 이 경우, 저주파 성분(직류분)인 단순화 모델 상체 수평위치 보정용 모멘트(Mps)에 관해서는, 불감대 특성을 갖게 해 두고, 상기 로 패스 필터의 출력이, 어떤 소정값을 중심으로 하는 소정범위내에 존재하는 상태에서는, Mps가 이 소정값(예를 들면 0)에 유지되도록 해 둔다.

분배기(221)의 출력인 오차보정 모멘트(Me)에 상체 수평위치 보정용 모델 안정화 모멘트(Mpf)를 Mp 연산부(217b)에서 가산함으로써 상체 수평위치 보정용 섭동 모델 모멘트(Mp)가 구해진다. 그리고, 상체 수평위치 보정용 섭동 모델 모멘트(Mp)는, 상체 수평위치 보정용 섭동 모델(202)에 입력되고, 상체 수평위치 보정용 섭동 모델(202)에 의해 보정용 섭동 모델 상체 수평위치 Xc가 결정된다.

분배기(221)의 또 하나의 출력인 단순화 모델 상체 수평위치 보정용 모멘트(Mps)는 단순화 모델(200)에 입력된다. 이것은, 상기 제 6 참고예의 도 74에서, 단순화 모델 상체 수평위치 보정용 모멘트(Mp)를 단순화 모델(200)에 입력하는 것에 상당하는 것이다.

역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)는 제 5 참고예에서 사용한 도 72에 도시하는 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부 와 동일하다. 단, 도 72의 출력인 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)는 역위상 팔 흔들기 각 보정용 섭동 모델(231)에 직접 입력되지 않고, 분배기(241)에 입력된다. 그래서, 본 제 7 참고예에서는 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트(Ma)는 명칭을 고쳐서 역위상 팔 흔들기 각 보정용 모델 모멘트(Ma)라고 부른다.

분배기(241)에서는, 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(230)가 결정한 역위상 팔 흔들기 각 보정용 모델 모멘트(Ma)를 입력으로 한다.

분배기(241)의 입력은 분배기(220)와 동일하게 단순화 모델(200)에 입력하는 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)와, 역위상 팔 흔들기 각 보정용 섭동 모델(231)에 입력하는 역위상 팔 흔들기 각 보정용 섭동 모델 입력(Mai)에 분배된다.

이때, 역위상 팔 흔들기 각 보정용 섭동 모델 입력(Mai)와 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)의 합이 역위상 팔 흔들기 각 보정용 모델 모멘트(Ma)에 일치하도록, 역위상 팔 흔들기 각 보정용 섭동 모델 입력(Mai)와 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)가 결정된다(출력됨).

보다 구체적으로는, 역위상 팔 흔들기 각 보정용 모델 모멘트(Ma)에 따라, 역위상 팔 흔들기 각 보정용 섭동 모델 입력(Mai)이 결정된다. 예를 들면, 역위상 팔 흔들기 각 보정용 모델 모멘트(Ma)를 불감대 특성, 포화 특성, 또는 주파수 특성을 갖는 신호처리를 통하여, 역위상 팔 흔들기 각 보정용 섭동 모델 입력(Mai)이 결정된다. 또, 역위상 팔 흔들기 각 보정용 모델 모멘트(Ma)로부터 역위상 팔 흔 들기 각 보정용 섭동 모델 입력(Mai)을 뺀 것이, 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)로서 결정된다. 보다 구체적으로 설명하면 본 실시형태에서는 분배기(241)는, 예를 들면 입력(역위상 팔 흔들기 각 보정용 모델 모멘트(Ma))을 로 패스 필터에 통과시켜서 이루어지는 저주파 성분(직류분)을 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)로서 출력함과 동시에, 그 입력(역위상 팔 흔들기 각 보정용 모델 모멘트(Ma))로부터 Mas를 뺀 성분이 역위상 팔 흔들기 각 보정용 섭동 모델 입력(Mai)으로서 출력된다. 이 경우, 저주파 성분(직류분)인 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)에 관해서는, 불감대 특성을 갖게 해 두고, 상기 로 패스 필터의 출력이, 어떤 소정값을 중심으로 하는 소정 범위내에 존재하는 상태에서는, Mas가 이 소정값(예를 들면 0)에 유지되도록 해 둔다.

분배기(241)의 출력인 역위상 팔 흔들기 각 보정용 섭동 모델 입력(Mai)은 상기 역위상 팔 흔들기 각 보정용 섭동 모델(231)에 입력되고, 역위상 팔 흔들기 각 보정용 섭동 모델(231)에 의해 보정용 섭동 모델 역위상 팔 흔들기 각 θca가 결정된다.

분배기(241)의 또 하나의 출력인 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)는 단순화 모델(200)에 입력된다. 이것은, 상기 제 6 참고예에 있어서의 도 74에서, 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Ma)를 단순화 모델(200)에 입력하는 것에 상당하는 것이다.

단순화 모델(200)은 상기 제 6 참고예와 동일하게, 단순화 모델 보용 순시값의 산출(결정)처리에 있어서, 목표 ZMP 주위에 상반력 모멘트 수평성분이 발생하지 않도록(모델 조작 상반력 모멘트를 0으로 하여) 보용의 순시값을 생성한 뒤에, 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)(전회값)를 발생하는 상체 경사 모드의 섭동운동을 부가하고, 단순화 모델 상체 수평위치 보정용 모멘트(Mps)(전회값)을 발생하는 상체 병진 모드의 섭동운동을 부가하고, 또한 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)를 발생하는 역위상 팔 흔들기 모드의 섭동운동을 부가하고, 보용의 순시값을 보정한다.

또한, 본 제 7 참고예에서는 도 65의 S3528의 처리의 일부인 도 43의 S800에 있어서, 전회 보용의 종단상태로서, 전회 보용 종단에 있어서의 단순화 모델의 상태량을 사용한다. 따라서, 분배기(220, 221, 241)로부터 단순화 모델에 출력되는 Mrs, Mps 및 Mas가 0 이외의 값이 될 때가 있으면, 단순화 모델은 당초의 거동으로부터 벗어나므로, 이것에 따라 S3528에서 보용 패러미터가 적당하게 수정된다. 또, 상기 불감대의 영역을 크게 설정하면, Mrs, Mps 및 Mas의 절대값이 작아지므로, 보용 패러미터의 수정량의 절대값도 작아진다.

그 밖의 구성 및 처리는 제 5 참고예와 동일하다. 보다 상세하게는, 연산부(204, 205), 목표 상체 수평위치를 연산하는 연산부, Merr 연산부(216), 상체자세 경사각 보정용 섭동 모델 제어칙(207), 상체 수평위치 보정용 섭동 모델 제어칙(206), Min 연산부(209), Fin 연산부(210), 제한처리부(211), Mpf 연산부(215), 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트 결정부(240)의 처리는 상기 제 5 참고예와 동일하다.

또한, 본 제 7 참고예에서는 상기 제 4 참고예에서 설명한 바와 같이 금회 보용의 종단 등에서 각 섭동 모델(202, 203, 231)의 상태량에 의해 단순화 모델(200)의 상태량을 수정하는 것과 같은 처리는 행할 필요는 없다. 이것은, 분배기(220, 221, 241)로부터 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs), 단순화 모델 상체 수평위치 보정용 모멘트(Mps) 및 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)가 추가 입력되기 때문이다.

이러한 본 실시형태에서는 상기 제 5 참고예 또는 제 6 참고예와 동일한 작용효과를 이룰 수 있다.

또한, 본 제 7 참고예에서는 각 분배기(220, 221, 241)의 2개의 출력중 일방을 0으로 하고, 타방을 입력에 일치시켜도 좋다.

이 경우, 예를 들면 분배기(220)의 출력인 단순화 모델 상체자세 경사각 보정용 모멘트(Mrs)를 0으로 하고, 분배기(221)의 출력인 단순화 모델 상체 수평위치 보정용 모멘트(Mps)를 0으로 하고, 분배기(241)의 출력인 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Mas)를 0으로 하면, 제 5 참고예와 동일한 작용효과를 이루게 된다(사실상, 제 5 참고예의 것과 동일한 구성이 됨).

또, 오차보정 모멘트(Me)에 상체 수평위치 보정용 모델 안정화 모멘트(Mpf)를 가산한 값을 도시하지 않은 제 3 분배기에 입력하고, 그 출력의 일방을 상체 수평위치 보정용 섭동 모델에 입력하고, 또하나의 일방의 출력을 단순화 모델 상체 수평위치 보정용 모멘트(Mps)에 가산해도 좋다. 이 경우, 분배기(221)의 출력인 단순화 모델 상체 수평위치 보정용 모멘트(Mps)를 0으로 해도 좋다. 즉, 분배기(221)를 생략하고, 풀 모델 상반력 모멘트 오차 Merr에 게인 Km을 곱한 값을 적분 하고 부호를 반전한 값을 상체 수평위치 보정용 모델 안정화 모멘트(Mpf)에 가산한 값을 제 3 분배기에 입력해도 좋다.

또한, 이상 설명한 제 4∼제 7 참고예에서는 도 65의 S3532에 있어서, 목표 ZMP를 만족하도록, 즉, 목표 ZMP 주위의 상반력 모멘트 수평성분이 0이 되도록, 금회 보용 순시값(목표 운동의 가순시값)을 결정하도록 했지만, 이것 대신에, 제 2 참고예에 있어서의 도 56의 S3032와 같이, 목표 ZMP 주위에 모델 조작 상반력 모멘트 수평성분이 발생하도록 금회 보용 순시값을 결정하도록 해도 좋다. 이 경우에는, 도 53에 도시하는 바와 같이, 보용생성 장치(100)에 보상 전체 상반력 모멘트 수평성분 분배기(110)로부터 모델 조작 상반력 모멘트 수평성분을 부여하도록 하면 된다.

또, 이상 설명한 제 1∼제 7 참고예에서는, 보상 전체 상반력 모멘트 수평성분 Mdmdxy는, 상체자세각·각속도 대신에, 전체 중심 수평위치·속도 등, 로봇(1)의 다른 자세에 관한 상태량에 기초하여 결정해도 좋다.

다음에, 본 발명의 제 1 실시형태를 도 76부터 도 83을 참조하여 설명한다.

본 실시형태에 있어서의 제어 유닛(60)의 기능 블럭도를 도 76에 도시한다. 또한, 본 실시형태에서는 상기 제 3 참고예와 동일구성 부분 혹은 동일기능 부분에 대해서는 제 3 참고예와 동일한 참조부호를 사용한다.

본 실시형태에서는 제 3 참고예(도 59 참조)에 비교하여, 실제 로봇(1)의 미끄러짐의 유무를 판정하는 미끄러짐 판정부(114)가 추가되어 있다. 또, 미끄러짐 판정부(114)로부터 출력되는 미끄러짐 판정 결과가 보용생성 장치(100)에 입력된 다. 보용생성 장치(100)에 있어서는, 미끄러짐 판정 결과가 미끄러짐 있음으로 판정되면, 보용 패러미터를 기초로 결정되는 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위를 좁힌다. 또, 보용생성 장치(100)는 미끄러짐 판정 결과가 미끄러짐 없음으로 판정되면, 보용 패러미터를 기초로 결정되는 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위로 되돌린다. 다른 구성 및 처리에 관해서는, 상기 제 3 참고예와 동일하다.

미끄러짐 판정부(114)를 그 처리를 나타내는 플로우차트인 도 79를 사용하여 상세하게 설명하면 먼저, S5210에서, 스핀 판정 서브루틴이 실행된다. 스핀 판정 서브루틴에서는, 그 플로우차트인 도 80의 S5310으로 진행하고, 실제 상체자세 각속도 및 관절각 지령(검출값)을 기초로 지지다리 족평(22)의 대지각속도 연직성분 ωsupz를 구한다. 이어서 S5312로 진행하고, 족평(22)의 6축력 센서(50)의 검출값(실제 상반력)을 기초로, 지지다리에 작용하고 있는 실제 상반력 모멘트 연직성분의 시간적인 변화율인 지지다리 상반력 모멘트 연직성분의 변화율 dMsupactz/dt를 구한다. 이어서 S5314로 진행하고, 상기 지지다리 족평(22)의 대지각속도 연직성분 ωsupz의 절대값이 어떤 소정의 값 ωe를 초과했는지 아닌지를 판정한다. 초과한 경우에는, S5316으로 진행하고, 상기 지지다리 상반력 모멘트 연직성분의 변화율 dMsupactz/dt를 상기 지지다리 족평의 대지각속도 연직성분 ωsupz로 나누고 부호를 반전한 값인, 지지다리의 겉보기 비틀림 스프링 정수 Ksupt를 구한다. 이어서 S5318로 진행하고, Ksupt가 어떤 소정의 값 Ksuptmin보다도 작은지 아닌지를 판정하고, 작은 경우에는, S5320으로 진행하고, 스핀 판정 결과를 스핀 있음으로 판 정하고, 그렇지 않을 경우에는, S5322로 진행하고 스핀 판정 결과를 스핀 없음으로 판정한다. 또, S5314에서, 상기 지지다리 족평의 대지각속도 연직성분 ωsupz의 절대값이 상기 소정의 값 ωe를 초과하지 않았다고 판정된 경우에는, S5324로 진행하고, 스핀 판정 결과를 스핀 없음으로 판정한다. 이와 같이 스핀의 유무를 판정함으로써, 족평(22) 등의 휨(탄성 변형)에 의해, ωsupz의 절대값이 어떤 소정의 값 ωe를 초과했을 때, 스핀 있음으로 오판정되는 것과 같은 사태가 회피된다.

이상과 같이 S5210의 처리를 완료한 후, S5212로 진행하고, 병진 미끄러짐 판정 서브루틴이 실행된다.

병진 미끄러짐 판정 서브루틴에서는, 그 플로우차트인 도 81의 S5410으로 진행하고, 실제 상체자세 각속도, 가속도 검출값 및 관절각 지령(검출값)을 기초로 지지다리 족평(22)의 대지병진속도 수평성분 Vsupxy를 구한다. 이어서 S5412로 진행하고, 족평(22)의 6축력 센서(50)의 검출값(실제 상반력)을 기초로, 지지다리에 작용하고 있는 실제 상반력 수평성분의 시간적인 변화율인 지지다리 상반력 수평성분의 변화율 dFsupactxy/dt를 구한다. 이어서 S5414로 진행하고, 상기 지지다리 족평의 대지병진속도 수평성분 Vsupxy의 절대값이 어떤 소정의 값 Ve를 초과했는지 아닌지를 판정한다. 초과한 경우에는 S5416으로 진행하고, 상기 지지다리 상반력 수평성분의 변화율 dFsupactxy/dt를 상기 지지다리 족평(22)의 대지병진속도 수평성분 Vsupxy로 나눈 부호를 반전한 값이다, 지지다리의 겉보기 전단 스프링 정수 Ksups(=(-dFsupactxy/dt)/Vsupxy)를 구한다. 이어서 S5418로 진행하고, Ksups가 어떤 소정의 값 Ksupsmin보다도 작은지 아닌지를 판정하고, 작을 경우에는, S5420 으로 진행하고, 병진 미끄러짐 판정 결과를 병진 미끄러짐 있음으로 판정하고, 그렇지 않은 경우에는, S5422로 진행하고, 병진 미끄러짐 판정결과를 병진 미끄러짐 없음으로 판정한다. 또, S5414에서, 상기 지지다리 족평의 대지병진속도 수평성분 Vsupxy의 절대값이 상기 소정의 값 Ve를 초과하지 않았다고 판정된 경우에는, S5424로 진행하고, 병진 미끄러짐 판정 결과를 병진미끄러짐 없음으로 판정한다. 이와 같이 병진 미끄러짐의 유무를 판정함으로써 족평(22) 등의 휨(탄성 변형)에 의해, Ksups의 절대값이 어떤 소정의 값 Ksupsmin을 초과했을 때에, 병진미끄러짐 있음으로 오판정되는 것과 같은 사태가 회피된다.

이상과 같이 S5212의 처리를 완료한 후, S5214로 진행하고 미끄러짐 진동 판정 서브루틴이 실행된다.

미끄러짐 진동 판정 서브루틴에서는, 그 플로우차트인 도 82의 S5510으로 진행하고, 족평(22)의 6축력 센서(50)의 검출값(병진력 수평성분 및/또는 모멘트 연직성분)을 밴드패스 필터에 통과시킴으로써, 미끄러짐 진동성분을 추출한다. 또한, 족평(22)의 6축력 센서(50)의 검출값 대신에, 족평(22)에 요잉레이트 센서, 가속도 센서 등을 구비하고, 그 검출값을 사용해도 좋다. 또, 밴드패스 필터의 통과주파수대는, 족평(22)의 발밑바닥이나 바닥면의 재질 등에 따라 미리 설정된다. 이어서 S5512로 진행하고, 검출된 미끄러짐 진동 성분의 크기의 절대값이 어떤 소정의 값 Ampe를 초과했는지 아닌지를 판정한다. 초과한 경우에는, S5514로 진행하고, 미끄러짐 진동 판정 결과를 미끄러짐 진동 있음으로 판정하고, 그렇지 않은 경우에는, S5516으로 진행하고, 미끄러짐 진동 판정 결과를 미끄러짐 진동 없음으로 판정한다.

이상과 같이 S5214의 처리를 완료한 후, S5216으로 진행하고, 스핀 판정 결과가 스핀 있음, 또는 병진 미끄러짐 판정 결과가 병진 미끄러짐 있음, 또는 미끄러짐 진동 판정 결과가 미끄러짐 진동 있음이면, S5218로 진행하고, 종합 판정 결과인 미끄러짐 판정 결과를 미끄러짐 있음으로 결정하고, 그렇지 않으면 S5220으로 진행하고, 종합 판정 결과인 미끄러짐 판정 결과를 미끄러짐 없음으로 결정한다. 이상이 미끄러짐 판정부(114)의 처리이다.

도 77에 본 실시형태에 있어서의 보용생성 장치(100)의 메인루틴 처리의 플로우차트를 도시한다.

동 도 77에서, S2310부터 S2332까지는, 제 3 참고예의 플로우차트(도 60)의 S2010부터 S2032까지의 처리와 동일한 처리가 행해진다.

이어서 S2334로 진행하고, 수정보용 순시값 결정 서브루틴이 실행된다.

본 실시형태에서는 수정보용 순시값 결정 서브루틴에서는, 원보용에 근접하도록 목표 ZMP 및 역위상 팔 흔들기 복원 각가속도 패턴을 수정하면서, 수정된 목표 ZMP 주위에 모델 조작 상반력 모멘트(수평성분 및 연직성분)이 부가적으로 발생하도록 수정보용 순시값이 결정된다. 단 상반력 허용범위를 미끄러짐 판정 결과에 따라 변경하고, 이 점이 제 3 참고예와 상이하다.

도 78에 수정보용 순시값 결정 서브루틴의 플로우차트를 도시한다. 수정보용 순시값 결정 서브루틴에서는, 먼저 S5100부터 S5114까지의 처리가 제 3 참고예의 수정보용 순시값 결정 서브루틴인 도 61의 S2100부터 S2112과 동일하게 실행된 다. 이어서, S5116으로 진행하고, 상기 미끄러짐 판정부(114)에 의한 미끄러짐 판정 결과가 미끄러짐 있음인지 아닌지를 판정하고, 상기 미끄러짐 판정 결과가 미끄러짐 있음인 경우에는, S5118로 진행하고, 허용범위 저감율 att를 서서히 0에 근접시킨다(실질적으로 연속적으로 감소시킴). 또한, 허용범위 저감율 att는 0부터 1까지의 값을 취하는 것으로 한다. S5116에서 상기 미끄러짐 판정 결과가 미끄러짐 없음으로 판정된 경우에는, S5120으로 진행하고 허용범위 저감율 att를 서서히 1에 근접시킨다(실질적으로 연속적으로 증가시킴).

이어서 S5122로 진행하고, S5110 및 S5112에서 결정한 상반력 수평성분과 상반력 모멘트 연직성분의 허용범위의 상한값 및 하한값인 Fxmin, Fxmax, Mzmin 및 Mzmax에 각각 허용범위 저감율 att를 곱함으로써, 상반력 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위를 좁힌다. 이후, 이 S5122에서 결정되는 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위를 총칭적으로 최종적 상반력 허용범위라고 한다. 또한, 당연한 것이지만 저감율이 1인 경우에는, 최종적 상반력 허용범위는 S5110 및 S5112에서 결정한 상반력 수평성분과 상반력 모멘트 연직성분의 허용범위(이하, 이것들을 원래의 상반력 허용범위라고 함)에 일치하고, 이 원래의 상반력 허용범위로부터 좁혀지지 않는다.

도 83에, 미끄러짐 판정 결과에 따라, 허용범위 저감율 att, 최종적 상반력 허용범위(구체적으로는, 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위)가 변화되는 양상의 예를 도시한다.

미끄러짐이 발생한 경우에는, 그립 상태로 재빨리 회복하지 않으면 로봇(1) 이 전도될 우려가 발생하므로, 미끄러짐이 발생했다고 판정되면, 최종적 상반력 허용범위를 재빨리 좁히도록 하는 편이 좋다. 단, 너무 급격하면, 보용의 가속도 변화가 지나치게 커져, 충격 등이 생기므로 지나치게 급격하지 않은 적절한 값으로 해야 한다.

미끄러짐이 없어졌다고 판정되면, 최종적 상반력 허용범위를 원래의 상반력 허용범위로 재빨리 되돌리도록 하는 편이 좋다. 최종적 상반력 허용범위가 좁은 상태대로이면, 수정보용이 원보용으로부터 크게 어긋나버려 있기 때문이다. 단, 지나치게 급격하면, 보용의 운동의 가속도변화가 지나치게 커져, 충격 등이 발생하여 다시 미끄러져버리므로, 지나치게 급격하지 않는 적절한 값으로 해야 한다.

이어서 S5124로 진행하고, 도 61의 S2114와 동일하게, 상반력 모멘트 수평성분 허용범위와 상반력 모멘트 연직성분 허용범위와 상반력 수평성분 허용범위의 조건을 만족하도록, 모델 조작 상반력 모멘트 수평성분·컴플라이언스 제어용 목표 상반력 모멘트(수평성분 및 연직성분), 상체 수평가속도 및 상체자세 경사 각가속도 및 역위상 팔 흔들기 각가속도를 결정한다.

이어서 S5126부터 S5128까지 진행하고, 도 61의 S2116부터 S2118까지와 동일한 처리가 행해진다.

상기와 같이 수정보용 순시값 결정 서브루틴이 실행되고, 이어서 도 77의 S2336으로 진행하고, 현재 시각 t를 △t만큼 증가하고, 다시 S2314로 되돌아와 보용생성을 계속한다.

이상이 본 실시형태에 있어서의 보용생성 장치(100)의 처리이다.

상기와 같이 본 실시형태에서는, 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위를 미끄러짐 판정 결과에 따라 변경하도록 했지만, 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위중 어느 한쪽만을 변경하도록 해도 좋다.

일반적으로, 족평(22)이 바닥면상에서 평행으로 미끄러지고 있는 경우(전단방향으로 미끄러지고 있을 경우)에는, 스핀 방향(요잉 방향)의 그립력도 저하된다. 또 반대로, 스핀 방향(요잉 방향)으로 미끄러지고 있는 경우에는, 족평(22)이 바닥면상에서 평행하게 미끄러지기 쉬워진다. 따라서, 어느 하나의 미끄러짐이 검지되면, 상반력 수평성분 허용범위 및 상반력 모멘트 연직성분 허용범위 양쪽을 좁히는(0으로 하는) 편이 미끄러짐으로부터 회복하는 효과는 높다.

이상에서 설명한 제 1 실시형태는 본 발명의 제 3∼제 7 발명의 실시형태로 되고 있다. 이 경우, 제 1 실시형태에서의 상반력 수평성분이 제한대상량, 상체자세 경사각 편차 및/또는 상체자세 경사각 속도편차가 로봇(1)의 상태량의 편차에 상당한다. 또, 제 1 실시형태에서의 보용생성용의 상반력 수평성분 허용범위 [Fxmin, Fxmax]가 제한대상량의 허용범위에 상당한다. 또, 발명에 있어서의 동역학 모델은 도 12의 동역학 모델에 상당한다. 또, 도 77의 S2334에서 결정되는 수정 보용 순시값의 운동성분과 상반력 성분이 각각 목표 운동의 순시값 및 목표 상반력의 순시값에 상당한다.

상기 제 1 실시예에서는 보용생성장치(100)로서 상기 제 3 참고예의 것을 사용했지만, 제 3 참고예 대신에 제 1, 제 2, 제 4∼제 7 참고예중 어느것을 사용해 도 좋다.

제 1 참고예를 사용하는 경우의 실시형태에서는. 예를 들면 도 13의 S030에서 금회 보용 순시값을 결정할 때에, 그 서브루틴인 도 45의 S1411과 S1412 사이에 도 78의 S5116∼S5122의 처리를 삽입하면 된다. 이와같이 한 실시형태(제 2 실시형태)는 본 발명중 제 1 발명, 제 2 발명 및 제 5∼제 7 발명의 실시형태가 된다. 이 경우, 제 1 실시형태에 있어서의 상반력 수평성분이 제 1 발명에서의 제한대상량에 상당하고, 상반력 연직성분이 제 2 발명에 있어서의 제한대상량에 상당한다. 또, 제 1 실시형태에 있어서의 보용생성용의 상반력 수평성분 허용범위[Fxmin, Fxmax]가 제 1 발명에서의 제한대상량의 허용범위에 상당하고, 보용생성용의 상반력 모멘트 연직성분 허용범위[Mzmin, Mzmax]가 제 2 발명에서의 제한대상량의 허용범위에 상당한다. 또, 발명에 있어서의 동역학 모델은 도 12의 동역학 모델이 상당한다. 또, 도 13의 S030에서 결정되는 금회 보용 순시값의 운동성분이 목표 운동에 상당한다. 또, 도13의 S030중에서 실행되는 도 46의 S1004에 있어서, S504, S518(도 26을 참조)에서 임시로 설정되는 운동이 상기 제 1 발명 또는 제 2 발명에 있어서의 가운동에 상당한다.

또, 제 2 참고예를 사용하는 경우의 실시형태에서는 예를 들면 도 56의 S3032에서 금회 보용 순시값을 결정할 때에, 그 서브루틴인 도 57의 S3411과 S3412 사이에 도 78의 S5116∼S5122의 처리를 삽입하면 된다. 이와 같이 한 실시형태(제 3 실시형태)는 본 발명중 제 3∼제 7 발명의 실시형태가 된다. 이 경우, 제 3 실시형태에서의 상반력 수평성분이 제한대상량에 상당하고, 상체자세 경사각 편차 및 /또는 상체자세 경사각 속도편차가 제 3 및 제 4 발명에 있어서의 로봇(1)의 상태량의 편차에 상당한다. 또, 제 3 실시형태에서의 보용생성용의 상반력 수평성분 허용범위[Fxmin, Fxmax]가 제한대상량의 허용범위에 상당한다. 또, 발명에 있어서의 동역학 모델은 도 12의 동역학 모델이 상당한다. 또, 도 56의 S3034에서 결정되는 금회 보용 순시값의 운동성분과 상반력 성분이 목표 운동의 순시값, 목표 상반력의 순시값에 상당한다. 또한, 제 3 실시형태에서는 상반력 모멘트 연직성분 보상량 허용범위를 미끄러짐의 발생의 판단 결과에 따라 가변적으로 설정하도록 해도 좋다.

또, 제 4 참고예를 사용하는 경우의 실시형태에서는 예를 들면 도 65의 S3536의 처리(풀 모델을 사용한 보용의 수정처리)를 실행할 때에, 도 78의 S5116∼S5122의 처리를 실행하고, 허용범위를 가변적으로 설정하도록 하면 된다. 이와 같이 한 실시형태(제 4 실시형태)는 본 발명중 제 3∼제 7 발명의 실시형태가 된다. 이 경우, 제 4 실시형태에 있어서의 상반력 수평성분이 제한대상량에 상당하고, 상체 자세 경사각 편차 및/또는 상체자세 경사각 속도편차가 로봇(1)의 상태량의 편차에 상당한다. 또, 제 4 실시형태에 있어서의 보용생성용의 상반력 수평성분 허용범위[Fxmin, Fxmax]가 제한대상량의 허용범위에 상당한다. 또, 도 65의 S3536에서 수정된 보용의 운동성분 및 상반력 성분이 각각 제 3 발명 및 제 4 발명에 있어서의 목표 운동의 순시값, 목표 상반력의 순시값에 상당한다. 또, 발명에 있어서의 동역학 모델은 역 풀 모델(각 섭동 모델(202, 203, 231)을 포함함)이 상당한다.

또, 제 5 참고예를 사용하는 경우의 실시형태에서는 예를 들면 도 65의 S3536의 처리(풀 모델을 사용한 보용의 수정처리)를 실행할 때에, 도 78의 S5116∼S5122의 처리를 실행하고, 허용범위를 가변적으로 설정하도록 하면 된다. 이와 같이 한 실시형태(제 5 실시형태)는 본 발명중 제 3∼제 7 발명의 실시형태가 된다. 이 경우, 제 5 실시형태에 있어서의 상반력 수평성분이 제한대상량에 상당하고, 상체자세 경사각 편차 및/또는 상체자세 경사각 속도편차가 로봇(1)의 상태량의 편차에 상당한다. 또한, 제 5 실시형태에서의 보용생성용의 상반력 수평성분 허용범위[Fxmin, Fxmax]이 제한대상량의 허용범위에 상당한다. 또, 도 65의 S3536에서 수정된 보용의 운동성분 및 상반력 성분이 각각 제 3 발명 및 제 4 발명에 있어서의 목표 운동의 순시값, 목표 상반력의 순시값에 상당한다. 또, 발명에 있어서의 동역학 모델은 풀 모델(각 섭동 모델(202, 203, 231)을 포함함)이 상당한다.

또한, 제 6 및 제 7 참고예를 사용하는 경우의 실시형태는 상기 제 5 실시형태와 동일하고, 제 3∼제 7 발명의 실시형태가 된다.

또한 미끄러짐의 판정 방법으로서는, 상기 제 1 실시형태에서 거론한 방식 이외에도, 이하와 같은 어느것을 사용해도 좋다. 또는 이들 결과를 종합적으로(논리합, 적 등의 논리연산을 사용하여) 판단해도 좋다.

1) 각속도 검출기 등을 기초로 밑바닥에 대한 족평(22)의 각속도를 구하고, 이 값의 절대치값이 어떤 소정의 값을 초과하면, 미끄러짐이 발생했다고 판정한다.

2) 어느 하나의 다리체(2)의 상반력 모멘트의 절대값이 어떤 소정의 값을 초과하면, 미끄러짐이 발생했다고 판정한다.

3) 전체 상반력 모멘트(양 다리체(2, 2)의 상반력의 합력의 모멘트)의 절대 값이 어떤 소정의 값을 초과하면, 미끄러짐이 발생했다고 판정한다.

4) 양다리 지지기에서, 전체 상반력 모멘트의 변화율과 상체자세 각속도편차와의 비에 (-1)을 곱한 값이 어떤 소정의 값(양다리 접지시 비틀림 강성)보다도 작으면, 미끄러짐이 발생했다고 판정한다.

5) 시각센서나 공간필터 등을 사용한 비접촉형의 대지속도, 각속도 검출기, 또는 접촉형의 대지속도, 각속도 검출기의 검출값(추정값)의 절대값이 어떤 소정의 값을 초과하면, 미끄러짐이 발생했다고 판정한다.

다음에 상기 제 1∼제 7 참고예 및 이것을 사용하는 실시형태(상기 제 1, ∼제 5 실시형태를 포함함)에 관계되는 변형 태양을 이하에 설명한다.

상기 제 2∼제 7 참고예 및 이것을 사용하는 실시형태에서는 보상 전체 상반력 모멘트 수평성분 및 보상 전체 상반력 모멘트 연직성분을 결정하도록 했지만, 보상 전체 상반력 모멘트 수평성분 및 보상 전체 상반력 모멘트 연직성분중 어느 하나, 혹은 양자를 0으로 설정해도 좋다. 단 이 경우에는, 풀 모델 보정은, 단지 단순화 모델에 의한 목표 보용의 정밀도를 높이기 위해서 사용되고, 풀 모델을 사용하여 보용을 수정하는 것에 의한 상체자세 경사 복원 효과 및/또는 상체 요잉각(각속도) 복원 효과는 없어진다.

상기 제 4∼제 7 참고예 및 이것을 사용하는 실시형태에서는 단순화 모델(200)에 의한 보용생성(단순화 모델 보용생성)에는, 본원 출원인이 먼저 제안한 일본 특개평 5-337849호 공보의 제 1 실시예, 제 2 실시예의 보용생성장치를 사용해도 좋다. 풀 모델에 의한 보정량을 작게 할 수 있으므로, 보정된 보용이 단순화 모델 보용으로부터 크게 벗어나는 것을 방지할 수 있다.

상기 제 2 참고예 및 이것을 사용하는 실시형태에서는, 보상 전체 상반력 모멘트 수평성분 Mdmdxy가 상반력 모멘트 허용범위 수평성분을 초과하면(제 4 참고예 이후 및 그것을 사용하는 실시형태에 있어서는, 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 모델 조작 상반력 모멘트 수평성분과의 합이 상반력 모멘트 허용범위를 초과하면), 컴플라이언스 제어용 목표 상반력 모멘트 수평성분은 상반력 모멘트 수평성분 허용범위의 상한값 또는 하한값으로 된다. 단, 보상 전체 상반력 모멘트 수평성분 Mdmdxy(제 4 참고예 이후 및 그것을 사용하는 실시형태에 있어서는, 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 모델 조작 상반력 모멘트 수평성분과의 합)이 상반력 모멘트 수평성분 허용범위를 초과해 있어도, 보상 전체 상반력 모멘트 수평성분 Mdmdxy(제 4 참고예 이후 및 그것을 사용하는 실시형태에서는, 보상 전체 상반력 모멘트 수평성분 Mdmdxy와 모델 조작 상반력 모멘트 수평성분과의 합)의 증가 혹은 감소에 따라 컴플라이언스 제어용 목표 상반력 모멘트 수평성분도 증가 혹은 감소하도록 해도 좋다. 컴플라이언스 제어용 목표 상반력 모멘트 수평성분이 상반력 모멘트 수평성분 허용범위에 접근하면, 컴플라이언스 제어에 의해 제어되는 실제 상반력 모멘트 수평성분이 목표보다도 적게 되는 경향이 있고, 이 때문에 컴플라이언스 제어용 목표 상반력 모멘트 수평성분이 조금 허용범위를 초과해도, 곧바로는, 족평(22)의 본래의 접지감을 저하시키거나, 족평(22)의 밑바닥면이 떠버리는 것과 같은 문제가 발생하지 않는 경우가 많기 때문이다.

동일한 이유로부터, 상반력 모멘트 수평성분 허용범위를 소위 지지다각형으 로 표시되는 ZMP 존재 가능범위(엄밀한 표현으로는, 실제 상반력 작용점의 존재 허용범위)를 모멘트 수평성분으로 환산하여 얻어지는 허용범위를 초과하여 설정해도 좋다.

컴플라이언스 제어에 의해 발생하는 상반력 모멘트 수평성분에 과도하게 기대하면, 상기와 같이 족평(22)의 본래의 접지감을 저하시키거나, 족평(22)의 밑바닥면이 떠버리는 것과 같은 문제가 발생한다. 따라서, 상반력 모멘트 수평성분 허용범위는 자세제어가 컴플라이언스 제어에 기대하는 실제 상반력 모멘트 수평성분의 허용범위라고 할 수 있다.

상반력 모멘트 수평성분 허용범위는 보용 패러미터로부터 뿐만아니라, 검출된 실제 상반력에도 기초하여 결정해도 좋다. 또, 계단에서의 에지 위치 검출값 등, 족평(22)의 접지영역의 검출값에도 기초하여 결정해도 좋다.

한편, 컴플라이언스 제어에 의해 발생하는 상반력 모멘트 연직성분에 과도하게 기대하면, 스핀되어 버리는 것과 같은 문제가 발생한다. 따라서, 상반력 모멘트 연직성분 허용범위는 자세제어가 컴플라이언스 제어에 기대하는 실제 상반력 모멘트 연직성분의 허용범위라고 할 수 있다.

상기 제 2 참고예 및 이것을 사용하는 실시형태의 도 56의 S3028에서의 금회 보용 패러미터의 수정(목표 ZMP의 수정)을 제어주기 마다 실행하도록, 메인 플로우 차트의 처리를 변경해도 좋다.

수정보용(목표 보용)이 원보용보다 크게 벗어나버릴 경우(발산해버릴 경우)에는, 다음회 보용의 보용 패러미터 수정을 할 때에 이미, 너무 크게 벗어나버려( 지나치게 발산해버려), 다음회 보용의 보용 패러미터 수정만으로는 계속적장기적으로 안정한 목표 보용을 생성하는 것이 곤란하게 된다. 금회 보용 패러미터의 수정(목표 ZMP의 수정)을 제어주기 마다 실행함으로써, 이 문제를 상당히 해결할 수 있다.

게다가, 원보용으로부터의 벗어남 상태에 의해, 금회 보용의 족평 착지 위치나 착지 시각 등을 제어주기 마다 변경해도 좋다.

구체적으로는, S3020부터 S3030까지의 처리를 제어주기 마다 실행하도록 처리 플로우를 변경해 두고, S3020에서, 다음회 보용 지지다리 좌표계(다음회 보용 지지다리 좌표계는, 다음 족평 착지 위치 자세에 대응), 다음다음회 보용 지지다리 좌표계(다음다음회 보용 지지다리 좌표계는 다음 다음 족평 착지 위치 자세에 대응), 금회 보용 주기(금회 보용 주기는 다음의 족평 착지 시각에 대응) 및 다음회 보용 주기(다음회 보용 주기는 정상 보용의 주기에 상당)의 적어도 어느 하나를, S3028에서의 금회 보용 패러미터의 수정(특히 목표 ZMP의 수정)이 작아지도록(즉, 금회 보용의 안정여유가 높게 유지되도록), 적당하게 변경하면 된다.

게다가, 상기 이외의 보용 패러미터를 변경해도 좋다.

또, 수정보용의 원보용으로부터의 벗어남은 단순화 모델 상체자세 경사각 보정용 모멘트(Mr), 단순화 모델 상체 수평위치 보정용 모멘트(Mp) 및 단순화 모델 역위상 팔 흔들기 각 보정용 모멘트(Ma) 등으로부터 동역학 모델을 사용하여 추정할 수 있으므로, Mr, Mp 및 Ma에 따라 모델의 거동 벗어남을 추정하고, 추정된 거동 벗어남을 기초로, 보용 패러미터를 수정해도 좋다. 또는, Mr, Mp 및 Ma와 보용 패러미터의 수정량의 적정값의 관계를 미리 구하여 맵화 해 두고, Mr, Mp 및 Ma에 따라 맵을 기초로 보용 패러미터의 수정량을 결정해도 좋다.

또, 다른 실시형태(제 4∼제 7 참고예를 사용하는 실시형태)에 대해서도 상기와 동일하게, 처리 플로우를 변경해도 좋다.

복원조건에는, 상기한 조건 이외에도, 관절각이 허용범위를 초과했는지 아닌지, 다리 등의 간섭이 없는지, 관절각속도, 토크가 과대하지 않은지 등의 키네마틱스 조건, 다이나믹스 조건을 부가해도 좋다.

또, 이것에 따라, 전술하는 바와 같이, 금회 보용 패러미터의 수정(목표 ZMP 또는 착지 위치, 시각 등의 수정)을 제어주기 마다 실행하도록 메인 플로우차트의 처리를 변경한 경우에는, 금회 보용의 안정여유가 높게 유지되도록 적당하게 변경되는 보용 패러미터의 값이, 적절한 값이 되도록(어떤 제약조건을 만족하도록) 하는 것도, 복원조건의 하나에 부가해도 좋다.

보용의 전환시기에서 결정된(읽어들여진) 착지 위치, 착지 시각은 상위의 제어장치로부터의 지시(보행계획 결정부나 오퍼레이터로부터의 지시 등. 이것을 당초의 요구라고 부른다.)에 의해 결정되므로, 수정보용의 착지 위치, 착지 시각은, 가능한 한 보용의 전환시기에서 결정된(읽어들여진) 착지 위치, 착지 시각으로 되돌려야 한다. 그래서, 보용의 전환시기에서 결정된(읽어들여진) 착지 위치, 착지 시각을 기억하고, 수정보용의 착지 위치, 착지 시각이 기억된 착지 위치, 착지 시각에 가능한 한 일치 또는 근접하는 것도 복원조건에 부가해도 좋다. 단, 실제로는, 상기 복원조건 4, 5 및 6에 의해, 수정보용의 착지 위치, 착지 시각은 가능한 한 보용의 전환시기에서 결정된(읽어들여진) 착지 위치, 착지 시각으로 서서히 되돌아오는 작용이 작동하므로, 굳이 추가할 필요는 없다.

또, 상황의 변화에 대응하여, 당초의 요구를 변경하고, 변경한 요구를 만족하는 보용 패러미터에, 상기와 같이 보용의 안정여유를 높게 유지하기 위해서 적당하게 변경되는 보용 패러미터가, 가능한 한 일치 또는 근접하는 것을 복원조건에 부가해도 좋다. 이 경우, 복원조건 4, 5 및 6은 삭제해야 한다.

전술한 각종의 복원조건을 만족하는 모델 상체 수평위치 안정화 상반력 모멘트와 모델 상체자세 경사각 안정화 상반력 모멘트의 결정법에는, 제약 조건하에서 최적값을 구하기 위한 선형 계획법(심플렉스법 등)이나 탐색법을 사용하면 된다. 또는, 퍼지 추론을 사용해도 좋다.

또, 착지 위치를 변경하는 경우에는, 보행환경의 장해물 등도 고려하지 않으면 안되는 상황도 생각할 수 있다. 이러한 상황에도 대처할 수 있게 하기 위해서는, 환경인식이나 행동결정 등의 인공 지능의 분야에 속하는 처리도 부가하여, 수정보용을 결정해야 한다.

모델 역위상 팔 흔들기 각 안정화 상반력 모멘트의 결정법도 동일하다.

상기 제 4∼제 6 참고예에서의 제한수단(제한처리부)의 전후의 블럭도는 예를 들면, 본원 출원인에 의한 PCT/JP02/13593에서 게시하는 바와 같이, 등가변환 또는 근사변환 해도 좋다.

이 경우, 상체 수평위치 보정용 모델 안정화 모멘트(Mpf)를 h로 나눈 값을 로 패스 필터에 통과시킨 값을 보정 목표 상반력 수평성분(풀 모델 상반력 수평성 분 Ffull)으로부터 뺀 값이 상기 PCT 출원에서의 바이어스 추정값 destm에 상당한다. 따라서, 예를 들면, 상기 제 5∼제 7 참고예에서의 게인 Kc의 적분을 시정수 1/Kc의 1차 지연을 피드백 요소로 한 포지티브 피드백계로 치환하거나 하여, 도 84와 같이, 근사변환을 해도 좋다. 이 예에서는, 제 5∼제 7 참고예 및 이것을 사용하는 실시형태에서의 Mpf 연산부(215)가 도시한 바와 같이 근사 변환되어 있다.

역위상 팔 흔들기 각 보정용 섭동 모델 모멘트도 동일하게 등가변환 또는 근사변환 하여 구하도록 해도 좋다.

상기 제 4 참고예 및 이것을 사용하는 실시형태에서는, 상기 역위상 팔 흔들기 각 보정용 섭동 모델 모멘트는, 도 70에 도시하는 기능 블럭도 대신에, 도 85에 도시하는 기능 블럭도의 처리를 행하도록 해도 좋다. 이것을 제 8 실시형태로서 이하에 설명하면, 도 70에서는, 역위상 팔 흔들기 각 보정용 모델이 복원하는 동작에 의해 발생하는 모멘트 연직성분도 포함시키고, 상반력 모멘트 연직성분 허용범위를 초과하는지 아닌지가 판정되고, 역위상 팔 흔들기 각 보정용 모델의 거동이 결정되었지만, 도 85에 도시하는 기능 블럭도의 처리에서는, 역위상 팔 흔들기 각 보정용 모델이 복원하는 동작에 의해 발생하는 모멘트 연직성분은 상반력 모멘트 허용범위를 초과하는지 아닌지를 판정하는 점에서 무시된다.

도 85에 도시하는 기능 블럭도의 처리를 상세하게 설명하면 풀 모델 상반력 모멘트 연직성분 Mfullz와 상체 수평위치 보정에 기인하는 상반력 모멘트 연직성분 섭동량 Mpz와 보상 전체 상반력 모멘트 연직성분 Mdmdz의 합을 무제한시 보정 목표 상반력 모멘트 연직성분 Minz로 하고, 이것에 도 70과 동일하게 제한을 가함으로 써, 제한시 보정 목표 상반력 모멘트 연직성분 Mltdz를 구한다. 제한시 보정 목표 상반력 모멘트 연직성분 Mltdz는 컴플라이언스 제어용 목표 상반력 모멘트 연직성분으로서 출력된다. 또한, 제한시 보정 목표 상반력 모멘트 연직성분 Mltdz로부터 무제한시 보정 목표 상반력 모멘트 연직성분 Minz를 뺀 값, 즉, 무제한시 보정 목표 상반력 모멘트 연직성분 Minz중 상반력 모멘트 연직성분 허용범위를 초과한 분인 Maa를 구한다. 이어서, 보정용 섭동 모델 역위상 팔 흔들기 각 θca의 전회값을 기초로, PD 제어 등을 사용한 역위상 팔 흔들기 각 보정용 섭동 모델 제어칙에 의해 역위상 팔 흔들기 각 보정용 모델 안정화 모멘트 요구값 Mafdmd를 구하고, 이것을 Maa로부터 뺀 값(모멘트)을 역위상 팔 흔들기 각 보정용 섭동 모델에 입력하고, 보정용 섭동 모델 역위상 팔 흔들기 각 θca를 얻는다.

Maa로부터 보정용 섭동 모델 역위상 팔 흔들기 각 θca까지의 전달함수는 로 패스 필터의 전달함수로 된다.

즉, 바꿔 말하면, 역위상 팔 흔들기 각 보정용 모델에 의한 상반력 모멘트 연직성분의 캔슬(초과방지 동작)을 행하지 않을 경우에 발생하는 상반력 모멘트 연직성분(무제한시 상반력 모멘트 연직성분)을, 상반력 모멘트 연직성분 허용범위를 초과하지 않도록 제한하는 제한수단(포화수단)에 통과시킴으로써, 컴플라이언스 제어용 목표 상반력 모멘트 연직성분(제한시 보정 목표 상반력 모멘트 연직성분 Mltdz)을 얻는 동시에, 무제한시 상반력 모멘트 연직성분을 상반력 모멘트 연직성분 허용범위를 초과한 분을 구하는 불감대 수단에 통과시킨 값을 로 패스 필터(즉 하이 컷 필터)에 통과시킴으로써, 보정용 섭동 모델 역위상 팔 흔들기 각 θca를 얻는다.

상기 제 1 및 제 2 참고예 그리고 이것을 사용하는 실시형태에서는, 본원 출원인이 제안한 일본 특개평 5-337849호 공보에 기재된 구성에 더하여, 상반력의 병진력 수평성분이 상반력 수평성분의 허용범위를 초과하지 않도록, 상반력 수평성분과 목표 ZMP 주위의 상반력 모멘트 수평성분과의 발생의 비율이 상이한 2개의 운동 모드의 거동, 예를 들면 상체 병진운동 모드의 상체 병진가속도와 상체 경사운동 모드의 상체자세 경사 각가속도가 결정되므로, 실제 로봇(1)을 수정 목표 보용(최종적으로 보용생성 장치(100)가 출력하는 보용)에 수렴시킬 수 있다. 즉, 실제 로봇(1)의 자세를 안정화시킬 수 있다.

또, 컴플라이언스 제어용 목표 상반력 모멘트 수평성분과 모델 조작 상반력 모멘트 수평성분의 차가 토털의 복원력으로 된다.

또, 모델 조작 상반력 모멘트 수평성분은 ZMP 존재가능 범위를 무시하고 어떠한 값이라도 취할 수 있으므로, 대단히 높은 자세 복원력을 발생할 수 있다.

또, 상반력의 병진력 수평성분이 상반력 수평성분의 허용범위를 초과하지 않으므로, 로봇(1)의 슬립을 방지할 수 있다.

또, 상반력 모멘트 연직성분(컴플라이언스 제어용 목표 상반력 모멘트 연직성분)은 실제 상반력 모멘트 연직성분이 상반력 모멘트 연직성분의 허용범위를 초과하지 않으므로, 로봇(1)의 슬립을 더한층 방지할 수 있다.

또, 상반력 연직성분이 0의 시기, 즉, 양 다리체(2, 2)가 모두 접지하고 있지 않은 시기에서는, 상체 병진운동 모드에 의존하지 않고 상체 회전운동 모드에 의존한 자세 복원이 행해져, 바닥과 족평(22) 사이의 마찰력에 의존하지 않고 효과적으로 자세 복원이 행해진다.

또, 실제 상반력 모멘트가 과대해지는 것을 방지하므로, 족평(22)의 본래의 접지성이 저하되거나, 족평(22)의 밑바닥면이 떠버리는 것과 같은 문제가 발생하는 것을 방지 또는 억제할 수 있다.

또, 1보분의 수정보용의 종단상태를 새로운 초기 상태로 한 새로운 금회 보용이 정상 보용에 점근하도록, 금회 보용 패러미터가 결정 또는 변경되므로, 계속적으로(장기적으로) 안정성이 보증된 보용을 계속하여 생성할 수 있다.

상기 제 3 참고예 및 이것을 사용하는 상기 제 1 실시형태에서는, 상기와 같이, 원보용과 수정보용이 동시에 생성되고, 수정보용은 실제 로봇(1)의 자세안정화를 위해 수정됨과 동시에, 컴플라이언스 제어에 의해 자세 복원에 필요한 상반력 모멘트(수평성분 및 연직성분)을 발생해도 아직 여유가 있는 경우에는, 이 여유를 사용하여, 가능한 범위에서 원보용에 수렴하도록 하고 있다. 이 때문에, 제 1 실시형태의 효과에 더하여, 당초 설정한 원보용에 가까운, 즉, 당초의 요구대로의 보용에 가까운 보용을 생성할 수 있다. 따라서, 미리 설정된 이동경로가 있을 경우에는, 이동경로로부터 크게 벗어나는 것을 방지할 수 있다. 또, 수정보용의 상체자세각이 원보용(당초 결정한 보용)의 상체자세각에 수렴하는 것을, 수정보용의 상체 수평위치가 원보용(당초 결정한 보용)의 상체 수평위치에 수렴하는 것보다도 우선했으므로, 상체자세각이 크게 변동하는 것을 억제할 수 있다.

상기 제 1∼제 7 참고예 및 이것을 사용하는 실시형태에서는 상반력 수평성 분 허용범위를 설정하도록 했지만, 상반력 수평성분과 로봇 전체 중심 수평가속도는 비례관계에 있으므로, 상기 각 실시형태에 있어서의 상반력 수평성분 및 그 허용범위 대신에, 로봇 전체 중심 수평가속도 및 그 허용범위를 사용해도 상관없다. 또, 로봇 전체 중심 수평궤도에 가까운 거동을 하는 부위의 수평가속도 궤도에 관한 패러미터를 명시적으로 설정해도 상관없다. 예를 들면, 다리체(2, 2)의 질량이 상체(3)의 질량에 비해 충분하게 작을 경우에는, 상체 수평가속도 궤도와 로봇(1)의 전체 중심 수평가속도 궤도는, 거의 동일하거나 비례관계에 있으므로, 상반력 수평성분 및 그 허용범위 대신에 상체 수평가속도 및 그 허용범위를 사용해도 좋다.

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

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

또, 사면이동시(경사진 바닥면에서 로봇(1)을 이동시킬 때)의 보용생성에 있어서는, 상반력 모멘트 연직성분 허용범위 대신에, 상반력 모멘트의 바닥면 법선방향 성분, 즉 마찰력 모멘트의 허용범위를 설정하도록 해도 좋다. 예를 들면, 이 마찰력 모멘트는 바닥면의 수평면에 대한 경사각도를 θf(로봇(1)의 진행방향을 향하여 전방하향 사면의 경우를 정으로 함)라고 하면, 다음 식c73의 관계가 성립한다. 따라서, 상기 실시형태와 동일한 알고리즘에 보용을 생성하는 경우, 이 식c73의 관계를 사용하여, 상반력 모멘트 연직성분과 상반력 모멘트 수평성분으로부터 마찰력 모멘트를 산출하고, 이 값이 마찰력 모멘트의 허용범위를 초과하지 않도록 처리를 변경하면 된다.

마찰력 모멘트=상반력 모멘트 연직성분*COS(θf)

+상반력 모멘트 수평성분*sin(θf) …식c73

또, 상기 제 1 참고예에 관한 변형 태양에서 설명한 바와 같이, 상반력 수평성분과 목표 ZMP 주위의 상반력 모멘트 수평성분을 적절한 값으로 하기 위해서, 상기의 실시형태에서는 상체 경사 모드와 상체 병진 모드 2개의 운동 모드를 사용했지만, 이것들 이외의 운동 모드를 사용해도 좋다. 이 경우, 운동 모드중 하나가 상반력 수평성분을 발생하지 않는 운동 모드일 필요도 없다. 어떠한 모드의 조합이어도, 상반력 수평성분과 목표 ZMP 주위의 상반력 모멘트의 발생의 비율이 상이 한 2개의 운동 모드를 사용할 수만 있다면, 상기 예와 같이, 임의의 상반력 수평성분과 목표 ZMP 주위의 상반력 모멘트를 발생시킬 수 있기 때문이다.

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

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

또, 3개 이상의 운동 모드를 사용해도 좋다.

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

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

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

상반력 모멘트 연직성분을 적절한 값으로 하기 위해서, 상기의 실시형태에서는 역위상 팔 흔들기 모드를 사용했지만, 이것들 이외의 운동 모드를 사용해도 좋다. 예를 들면, 상기 제 1 참고예의 변형 태양에 대해 설명한 바와 같이, 상체 요잉 회전 모드를 사용하거나, 상체 요잉 회전 모드와 역위상 팔 흔들기 모드를 병용해도 좋다.

또, 상반력 모멘트 연직성분을 적절한 값으로 하기 위해서 사용되는 운동 모드가 상반력 수평성분 및 상반력 모멘트 수평성분을 발생하지 않는 운동 모드일 필요도 없다. 예를 들면, 미착지 다리를 전후로 흔드는 운동 모드를 사용해도 좋다. 그 운동 모드에 의해 발생한 상반력 수평성분 및 상반력 모멘트 수평성분은 상체 경사 모드와 상체 병진 모드 2개의 운동 모드를 조정함으로써, 상쇄할 수 있기 때문이다.

풀 모델을 사용하는 참고예 및 실시형태에서는 상기 단순화 모델로서, 상기 실시형태에 사용한 동역학 모델(도 12의 모델) 이외에도 이하의 모델을 사용해도 좋다.

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

2) 본원출원인에 의한 PCT 공개 공보 WO/02/40224에 도시된 3질점 모델

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

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

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

단, 단순화 모델에 단순화 모델 상체자세 경사각 보정용 모멘트가 더해지는 참고예 및 실시형태에 있어서는, 상기 2), 3), 및 4)의 모델을 사용할 수는 없다.

또한, 풀 모델은 기본적으로는 단순화 모델보다도 보다 근사 정밀도가 높은 동역학 모델을 사용하는 것이 바람직하지만, 단순화 모델과 동등한 근사 정밀도의 동역학 모델을 사용해도 좋다.

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

또, 상기 각 실시형태를 2족이동 로봇에 관하여 설명해 왔는데, 1족 혹은 3족 이상의 다족 로봇에도 본 발명을 적용할 수 있다.

이상과 같이 본 발명은, 2족이동 로봇 등의 다리식 이동 로봇과 바닥면과의 마찰력이 작아지는 것과 같은 상황에서도, 로봇의 미끄러짐의 발생을 억제하면서, 이 로봇의 자세를 안정아게 유지하여, 이 로봇을 원활하게 이동시킬 수 있는 것으 로서 유용하다.

Claims (7)

1. 상체로부터 연장하여 설치된 복수의 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 보용을 생성하고, 그 목표 보용에 추종하도록 로봇의 동작을 제어하는 제어장치에 있어서,

   상기 목표 보용에 추종하여 동작하고 있는 상기 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

   상기 로봇에 작용시키는 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 바닥면 평행성분을 제한대상량으로 하고, 상기 미끄러짐 판단 수단에 의해 미끄러짐이 있다고 판단된 경우, 제한대상량의 허용범위가 좁아지도록 당해 허용범위를 설정하는 허용범위 설정수단과,

   적어도 상기 로봇의 운동과 상반력과의 관계를 나타내는 동역학 모델상에서 상기 로봇에 작용하는 중력과 관성력과의 합력이 바닥면상의 미리 정해져 있는 점 주위에 발생하는 모멘트의 수평 성분이 미리 정해져 있는 목표 상반력 모멘트 수평 성분과 균형을 이루는 조건, 또는, 당해 합력의 병진력 법선방향 성분이 미리 정해진 목표 병진 상반력 법선방향 성분과 균형을 이룬다고 하는 조건인 동역학적 평형조건을 만족시키도록 로봇의 질량중심을 중심으로 하는 각운동량 변화율을 포함하는 상기 목표 보용의 가운동을 결정하는 가운동 결정 수단과,

   상기 목표 보용의 가운동에 의해 정해지는 상기 제한대상량이 상기 허용범위를 일탈했을 때, 상기 동역학적 평형조건을 만족시키면서, 이 제한대상량을 상기 허용범위내에 제한하도록 로봇의 질량중심을 회전중심으로 하는 각운동량 변화율을 상기 가운동으로부터 변화시킴으로써 이 가운동을 수정하여 목표 보용의 운동을 결정하는 가운동 수정수단을 구비한 것을 특징으로 하는 다리식 이동 로봇의 제어장치.
2. 상체로부터 연장하여 설치된 복수의 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 보용을 생성하고, 그 목표 보용에 추종하도록 로봇의 동작을 제어하는 제어장치에 있어서,

   상기 목표 보용에 추종하여 동작하고 있는 상기 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

   상기 로봇에 작용시키는 상반력 모멘트의 바닥면 법선방향 성분 또는 이 로봇의 각운동량 변화율의 바닥면 법선방향 성분을 제한대상량으로 하고, 상기 미끄러짐 판단 수단에 의해 미끄러짐이 있다고 판단된 경우, 제한대상량의 허용범위가 좁아지도록 당해 허용범위를 설정하는 허용범위 설정수단과,

   적어도 상기 로봇의 운동과 상반력과의 관계를 나타내는 동역학 모델상에서 상기 로봇에 작용하는 중력과 관성력과의 합력이 바닥면상의 미리 정해져 있는 점 주위에 발생하는 모멘트의 수평 성분이 미리 정해져 있는 목표 상반력 모멘트 수평 성분과 균형을 이루는 조건, 또는 당해 합력의 병진력 법선방향 성분이 미리 정해진의 목표 병진 상반력 법선방향 성분과 균형을 이룬다고 하는 조건인 동역학적 평형조건 을 만족시키도록 로봇의 질량중심을 중심으로 하는 각운동량 변화율을 포함하는 상기 목표 보용의 가운동을 결정하는 가운동 결정수단과,

   상기 목표 보용의 가운동에 의해 정해지는 상기 제한대상량이 상기 허용범위를 일탈했을 때, 상기 동역학적 평형조건을 만족시키면서, 이 제한대상량을 상기 허용범위내에 제한하도록 로봇의 각운동량 변화율을 상기 가운동으로부터 변화시킴으로써 이 가운동을 수정하여 목표 보용의 운동을 결정하는 가운동 수정수단을 구비한 것을 특징으로 하는 다리식 이동 로봇의 제어장치.
3. 상체로부터 연장하여 설치된 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 운동의 순시값을, 적어도 이 로봇의 운동과 상반력과의 관계를 나타내는 동역학 모델을 사용하여 축차 결정하면서, 그 결정한 목표 운동의 순시값에 추종시키도록 상기 로봇의 동작을 제어하는 다리식 이동 로봇의 제어장치에 있어서,

   상기 목표 운동에 추종하여 동작하고 있는 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

   적어도 상기 로봇에 작용시키는 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 바닥면 평행성분을 제한대상량으로 하고, 상기 미끄러짐 판단 수단에 의해 미끄러짐이 있다고 판단된 경우, 제한대상량의 허용범위가 좁아지도록 당해 허용범위를 설정하는 허용범위 설정수단과,

   적어도 상기 결정된 목표 운동의 순시값에 대응하는 로봇의 자세의 목표상태량과 이 로봇의 자세의 실제 상태량과의 편차에 기초하여, 상기 목표 운동의 새로운 순시값을, 이 새로운 순시값에 대응하고 상기 동역학 모델에 기초하여 정해지는 상기 제한대상량이 상기 허용범위내에 들어가는 동시에 상기 편차가 0에 근접하도록 결정하는 목표 순시값 결정수단을 구비한 것을 특징으로 하는 다리식 이동 로봇의 제어장치.
4. 상체로부터 연장하여 설치된 다리체를 운동시켜서 이동하는 다리식 이동 로봇의 목표 운동 및 목표 상반력의 순시값을, 적어도 이 로봇의 운동과 상반력과의 관계를 나타내는 동역학 모델을 사용하여 축차 결정하면서, 그 결정한 목표 운동 및 목표 상반력의 순시값에 추종시키도록 상기 로봇의 동작을 제어하는 다리식 이동 로봇의 제어장치에 있어서,

   상기 목표 운동 및 목표 상반력에 추종하여 동작하고 있는 로봇의 미끄러짐의 발생을 판단하는 미끄러짐 판단 수단과,

   적어도 상기 로봇에 작용시키는 병진 상반력의 바닥면 평행성분 또는 이 로봇의 전체 중심 가속도의 바닥면 평행성분을 제한대상량으로 하고, 상기 미끄러짐 판단 수단에 의해 미끄러짐이 있다고 판단된 경우, 제한대상량의 허용범위가 좁아지도록 당해 허용범위를 설정하는 허용범위 설정수단과,

   적어도 상기 결정된 목표 운동 및 목표 상반력의 순시값에 대응하는 로봇의 자세의 목표 상태량과 이 로봇의 자세의 실제 상태량과의 편차에 기초하여, 상기 목표 운동 및 목표 상반력의 새로운 순시값을 이 새로운 목표 운동의 순시값에 대응하여 상기 동역학 모델에 기초하여 정해지는 상기 제한대상량이 상기 허용범위내에 들어가는 동시에 상기 편차가 0에 근접하도록 결정하는 목표 순시값 결정수단을 구비한 것을 특징으로 하는 다리식 이동 로봇의 제어장치.
5. 제 1 항 내지 제 4 항중 어느 한 항에 있어서, 상기 미끄러짐 판단 수단은 적어도 접지하고 있는 다리체의 선단부의 대지속도의 절대값이 미리 정해진 값보다도 클 때는 미끄러짐의 발생이 있다고 판단하는 것을 특징으로 하는 다리식 이동 로봇의 제어장치.
6. 제 1 항 내지 제 4 항중 어느 한 항에 있어서, 상기 미끄러짐 판단 수단은 적어도 접지하고 있는 다리체에 작용하는 실제 상반력의 시간적 변화율을, 이 다리체의 선단부의 대지속도로 나눈 것의 부호를 반전시킴으로써, 이 다리체의 겉보기 스프링정수를 구하는 수단을 구비하고,

   적어도 이 겉보기 스프링 정수가 미리 정해진 값보다도 작을 때는 미끄러짐의 발생이 있다고 판단하는 것을 특징으로 하는 다리식 이동 로봇의 제어장치.
7. 제 1 항 내지 제 4 항중 어느 한 항에 있어서, 상기 미끄러짐 판단 수단은 적어도 접지하고 있는 다리체에 작용하는 실제 상반력을 미리 정해진 주파수의 근방범위에 주파수 통과특성을 갖는 밴드패스 필터에 통과시킨 것의 절대값이 소정값보다도 클 때는 미끄러짐 진동의 발생이 있다고 판단하는 것을 특징으로 하는 다리식 이동 로봇의 제어장치.

Images