WO2016159346A1

WO2016159346A1 - 脚型機構体、歩行ロボット、姿勢制御方法及びプログラム

Info

Publication number: WO2016159346A1
Application number: PCT/JP2016/060915
Authority: WO
Inventors: 充典植村; 文夫宮崎; 平井　宏明
Original assignee: Osaka University NUC
Current assignee: University of Osaka NUC
Priority date: 2015-04-02
Filing date: 2016-04-01
Publication date: 2016-10-06
Anticipated expiration: 2017-10-02
Also published as: JPWO2016159346A1; US10589810B2; JP6407409B2; US20180093725A1

Abstract

歩行ロボット（１）は、２本の脚（１０）の上部に相対的に姿勢変更可能に連結された胴体（２０）と、脚（１０）及び胴体（２０）の姿勢に基づいて本体の姿勢を安定にする姿勢制御部（１０５，１０６，１０７）とを備えている。脚（１０）及び胴体（２０）の各角度情報から、立脚状態にある脚の足首周りの角運動量Ｐ_１及びその積分値Ｐ_Ｉ１を周期的に算出する状態算出部（１０１，１０２，１０３）と、算出された角運動量Ｐ_１及びその積分値Ｐ_Ｉ１が目標値に対して近づく状態であるか離れる状態であるかを判定し、判定結果を姿勢制御部（１０５，１０６，１０７）に出力する安定判定部（１０４）とを備えている。このように、足首周りの角運動量とその積分値を用いて転倒を回避する。

Description

脚型機構体、歩行ロボット、姿勢制御方法及びプログラム

　本発明は、複数の脚を備えて自立歩行（走行）し、あるいは歩行を補助・支援し、乃至は姿勢解析のための脚型機構体、歩行ロボット、及びその姿勢制御技術に関する。

　歩行する脚型ロボットは、不整地を含め人の生活環境での移動を得意とするため、その基礎技術が盛んに研究されている。しかし、盛んな研究競争が繰り広げられているわりに、脚型ロボットの制御技術やハードウェア技術の限界から、実用化に至っている脚型ロボットは殆ど存在しない現状にある。脚型ロボットの技術的問題は、ロボットの大型化や高重量化、高価格化、鈍速化、転倒の発生などがある。これらの問題は、制御技術の問題やロボットが多数のアクチュエータを持つことを原因とするものが多い。従来の脚型ロボットのバランス制御は、ＺＭＰ（Zero Moment Point）を規範としたものが多い。しかし、ＺＭＰが両脚の足裏から構成される平面外にあるときロボットの踵か爪先が浮いてしまい、そのとき接地している爪先や踵はアクチュエータを持たない関節と同等な状態になる。

　このような場合に有効なバランス制御法は、殆ど提案されてこなかった。また、従来のロボット制御技術の多くでは、ロボットの全自由度を制御するために足首に２個のアクチュエータを持つ。しかし、足先に２個のアクチュエータを取り付けると、足先が大型化、高重量化及び高価格化してしまう。また、その足先を動かすために膝や股関節のアクチュエータも高出力化する必要があるため、膝や股も大型化、高重量化及び高価格化する。結果として、ロボット全体が大型化、高重量化、高価格化及び鈍速化する。一方、足首にアクチュエータを持たない脚型ロボットは、少数ではあるが研究されている。それらの研究により、ロボットが特定の速度で歩行する制御アルゴリズムは確立されつつある。これにより、両脚足首の合計４個のアクチュエータが不要となり、ロボットの小型化、軽量化、低価格化及び高速化に貢献する。

　しかし、足首にアクチュエータを持たないロボットが一定の立位姿勢を保てることは、脚型ロボットの実用化には不可欠であるにも関わらず、一定の立位姿勢を保つための従来の制御技術は極めて不十分であり、課題となっている。

　特許文献１には、僅かな外力がロボットに作用したときに、元の立位姿勢に戻ることが可能な多関節ロボットの姿勢制御方法が記載されている。また、非特許文献１には、足首にアクチュエータを持つ脚型ロボットで、外力等によりロボットが押された場合に、足首トルクを使って元の姿勢に戻れる最大範囲を規定する技術が示されている。

特開２００６－７５９４５号公報

杉原知道、"最良重心－ＺＭＰ　レギュレータに基づく二脚運動の立位可安定性と踏み出し"、第14回ロボティクスシンポジア、PP.435-440、2009．

　特許文献１に記載の姿勢制御方法では、大きな外力が加わった場合にはロボットは転倒してしまうが、どのような状態であれば、元の立位姿勢に戻れるかという判定手法等の評価に関しては記載されていない。

　非特許文献１は、重心に外力が作用する場合に限定した制御技術を示したものであり、また、足首アクチュエータを備えたタイプの脚型ロボットである。足首アクチュエータが一定以上の足首トルクを出力した場合に、踵か爪先が地面から離れるため、地面に接地している爪先や踵部分はアクチュエータのない関節と同様な状態となるため、非特許文献１の制御方法では制御不能となる。さらに、ロボットの重心位置周りの角運動量を０と仮定しており、ロボットの重心位置周りに角運動量が発生した場合には対処できないため、現実に則したアルゴリズムとしては一定の限界がある。

　本発明は、上記に鑑みてなされたもので、足首周りの角運動量とその積分値を用いることで、足首アクチュエータの有無に関係なく転倒回避制御を可能とする脚型機構体、歩行ロボット、姿勢制御方法及びプログラムを提供するものである。

　本発明に係る脚型機構体は、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体と、前記脚及び本体の状態に基づいて前記本体の姿勢を安定にする姿勢制御手段とを備えた脚型機構体において、前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出手段と、算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるかを判定し、判定結果を前記姿勢制御手段に出力する判定手段とを備えたものである。

　また、本発明に係る姿勢制御方法は、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢を、前記脚及び本体の状態に基づいて安定にする姿勢制御手段を備えた脚型機構体の姿勢制御方法において、前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出工程と、算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるかを判定し、判定結果を前記姿勢制御手段に出力する判定工程とを備えたものである。

　また、本発明に係るプログラムは、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢を、前記脚及び本体の状態に基づいてコンピュータで安定に制御するプログラムにおいて、前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出手段、及び算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるかを判定し、判定結果を姿勢安定処理に供する判定手段、として前記コンピュータを機能させるものである。

　これらの発明によれば、姿勢制御手段によって、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢が、脚及び本体の状態に基づいて安定に制御される。状態算出手段によって、脚及び本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出され、判定手段によって、算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるかが判定され、判定結果が姿勢制御手段に出力される。従って、簡易な構成でありながら、本体の姿勢すなわち脚型機構体の姿勢のバランスが保たれ、外力を含む外乱によっても転倒が回避される。

　本発明によれば、足首周りの角運動量とその積分値を用いて転倒を回避する制御が可能となる。

本発明に係る歩行ロボットをモデル化した構成図である。歩行ロボットの機能構成図である。状態量である角運動量ｐ_１を縦軸に、その積分値ｐ_Ｉ１を横軸にとった相図である。外力の印加に対する姿勢安定処理のフローチャートである。図１に対応する図で、足首にアクチュエータを備えた実施形態の構成図である。歩行運動を生成するｐ_１－ｐ_Ｉ１相図である。歩行運動中で本体の姿勢を変更することにより、より少ない歩数で目標の歩行速度を実現することと、歩行運動中での静止を回避する制御のためでのｐ_１－ｐ_Ｉ１相図である。

　図１は、本発明に係る歩行ロボットをモデル化した構成図である。図２は、歩行ロボットの機能構成図である。以下では、単にロボット１という。

　ロボット１は、複数本のリンク、例えば図１の紙面奥行き方向で左右対称となる２本の脚１０（１０Ｌ，１０Ｒ）と、各脚１０Ｌ，１０Ｒの上部に連結された本体としての胴体２０とを有する。各脚１０Ｌ，１０Ｒの上部は、関節に対応する脚アクチュエータの一例としての脚モータ３１（左脚モータ３１Ｌ及び右脚モータ３１Ｒ）を介して、胴体２０の下部と連結されている。

　脚モータ３１によって、胴体２０は、脚１０に対して相対姿勢が変更可能となる。角度ｑ１は、脚１０と水平な地面ＥＡとのなす角度（重力方向に対する角度）であり、角度ｑ２は、脚１０と胴体２０とのなす角度である。角度ｑ１は、遊脚状態の脚１０の相対回転によって変更される。角度ｑ２は、胴体２０と脚１０の相対回転によって変更される。具体的には、角度ｑ２は、地面ＥＡに接地した一方の脚１０側の脚モータ３１の駆動によって、あるいは接地した両方脚の脚モータ３１の同期駆動によって変更される。角度ｑ１は、外乱によって変更される他、角度ｑ２の変更によってロボットの重心位置が変更されることによりロボット全体が足首周りに回転することで間接的に変更される。

　なお、脚１０を長さ方向の途中で分割し、膝関節用の膝アクチュエータである膝モータ３２（左膝モータ３２Ｌ及び右膝モータ３２Ｒ）を介して膝周りに相対回転可能に、又は膝部分が伸縮することで脚の長さが調節可能となるように連結された構成を採用してもよい。この場合、脚モータ３１及び膝モータ３２の各回転軸は互いに平行に設定されている。脚１０を分割し、膝モータ３２で連結する構成は必須ではないが、後述するように安定化可能範囲の拡大、また当該範囲外にある場合の踏み出し位置の精度を高める上で好ましい。

　各関節部分に相当する部分、例えば左右の脚１０Ｌ，１０Ｒの適所、及び胴体２０の適所、例えば下部には、それぞれ角度センサ４０が設けられている。角度センサ４０は、本実施形態では、地面ＥＡとの角度を検出する左脚角度センサ４１Ｌと右脚角度センサ４１Ｒ、及び脚１０Ｌ，１０Ｒとの角度を検出する胴体角度センサ４２を有する。なお、左脚角度センサ４１Ｌ及び右脚角度センサ４１Ｒと、胴体角度センサ４２との少なくとも一方には、例えば加速度センサを利用した重力センサが採用されている。それ以外のセンサは、関節部の相対回転量を角度情報として得るものでよい。いずれかに重力センサを採用することで、ロボット１の各部位について鉛直方向（図１、矢印ｇ参照）に対する傾斜角度に置換することが可能となり、これによって、後述するようにロボット１の状態を知るための計算が可能となる。

　なお、図１では示していないが、角度センサ４０は、ロボット１に採用される関節部位毎に設けられることで、胴体や各脚の質量分布の情報を元にロボットの重心位置や角運動量が算出可能となる。

　続いて、図１を参照しつつ、モデル化されたロボット１の姿勢制御方法を説明する。

（１）足首周りの角運動量に関する運動方程式
　図１において、紙面奥行き方向を左右方向、図の右方を前方とし、図の左方を後方として設定している。図１では、左右の脚１０Ｌ，１０Ｒは重なっている。また、ロボット１は、脚１０の下端である足首にアクチュエータを持つ態様でもよいが、本実施形態では、アクチュエータを持たない態様で説明する。また、ここでは、簡単化のため１関節すなわち脚１０と胴体２０との連結部である関節（以下、腰という）を備えたモデルで説明するが、以下の内容は任意の関節数を持つロボットにも適用できる。

　まず、ロボットの運動方程式は、ラグランジュの運動方程式を用いて、数１で表される。

のように、式(1)で示される。

　これらより、足首周りの角運動量ｐ_１の時間変化は、

　図１のように、足首にアクチュエータを持たない場合は、τ_１= 0となる。この場合、足首周りのロボット１の回転を制御するためには、重心位置ｘ_ｇを適切に制御する必要があることがわかる。

（２）立直姿勢を保つためのｐ_Ｉ１
　ロボット１が安定な姿勢（直立姿勢）を保つことができる関節角度ベクトル、つまりｘ_ｇ= 0となる姿勢を、ｑ_０と定義し、その近傍で足首周りの角運動量ｐ_１を積分した量の近似値（以下、単に、積分値又は状態量と呼ぶ）を、

と定義する。このとき、式(1)と慣性行列Ｍとの特性から、

　これらのｐ_１とｐ_Ｉ１とを０に収束させれば、ロボット１は足首周りに回転せず、かつ直立姿勢を保つことができる。

（３）胴体姿勢の調節によるｐ_１とｐ_Ｉ１の制御
　胴体の角度ｑ_ｔｒは、図１から、ｑ_ｔｒ＝ｑ_１＋ｑ_２―π／２と定義できる。ロボット１は腰の部位に脚モータ３１があるため、ｑ_ｔｒは制御可能である。角運動量に関する式を考察すると、次式が近似的に成立する。

ただし、ｃ_ｑｔｒは正の定数である。

を得る。式(5)と式(2)とから、状態量をｐ_１とｐ_Ｉ１とし、入力をｑ_ｔｒとした状態方程式

が得られる。ここで、ロボット１の胴体２０は脚１０との干渉のため、一定角度以上は曲げられないことを考えて、

という入力の制約を設定する。これらの状態方程式(6)と入力の制約(7)がある場合、状態量ｐ_１とｐ_Ｉ１とを０に収束させられる最大範囲は、

となる。以下、この式(8)の範囲を、胴体姿勢による安定化可能範囲と呼び、式(8)が満たされない範囲を、安定化不可能範囲と呼ぶ。安定化可能範囲を最大化する入力の一例は、

である。ただし、ｑ_ｔｒ＞＝ｑ_{ｔｒｍａｘ}のときは、ｑ_ｔｒ＝ｑ_{ｔｒｍａｘ}とし、ｑ_ｔｒ＜＝ｑ_{ｔｒｍｉｎ}のときは、ｑ_ｔｒ＝ｑ_{ｔｒｍｉｎ}とする。また、ｋ_１，ｋ_２は、一定の範囲で設定される定数である。よって、式(8)が満たされているときは、式(9)に従って胴体２０の角度を調節することで、最終的に姿勢を安定化し得、式(8)が満たされないときは、一方の脚１０を踏み出す必要があることが分かる。

　脚１０を踏み出す場合、次に脚１０を着いたときに安定化可能範囲にあれば、直立姿勢に戻ることができる。図１と式(3)とから、関節角度ベクトルｑは、立脚と遊脚とが交換した瞬間に値が変わり、積分値ｐ_Ｉ１の値も瞬間的に変わる。そこで、脚１０が着いたときに、式(8)が満たされるようにすれば、直立姿勢に戻ることが可能となる。そこで、

となるように踏み出した脚１０の足先を設定すれば、安定化不可能領域から最も離れること、すなわち安定化可能範囲の中央に入ることが可能となる。

　続いて、外力が作用した場合の姿勢安定化について、図３を参照しつつ説明する。図３は、状態量である角運動量ｐ_１を縦軸に、その積分値ｐ_Ｉ１を横軸にとった相図である。なお、角運動量ｐ_１は、回転の勢いを示すもので、ロボット１全体が足首周りに前回りに回転するとき正の値をとり、後ろ周りに回転するとき負の値をとる。また、積分値ｐ_Ｉ１は、ロボット１が前側に傾いているとき正の値をとり、ロボット１が後ろ側に傾いているとき負の値をとる。

　胴体２０の姿勢が立直な場合、すなわちｑ_ｔｒ＝０のときの、状態量ｐ_１とｐ_Ｉ１の推移を図３の矢印ＡＲで示す。また、ロボット１の転倒を防止するために、脚１０と胴体２０の連結位置（腰）の高さを一定以上に保つ必要があり、そのために、ｐ_Ｉ１の値を、図３の左右両側の線１０１，１０２で囲まれる領域内に留める必要がある。

　まず、矢印ＡＲの意味を説明する。ロボット１の状態は、任意の時点で図３の相図内のある点（状態位置）に対応しており、その状態を継続すると、ロボット１の状態は、矢印ＡＲで示す方向の状態位置に遷移する。例えば、ロボット１の現在の状態に対応する点が薄い網掛けを施した領域３０１内（いわゆる第２、第４象限）にある場合、時間経過に従って矢印ＡＲに沿って移動して、ｐ_１＝０、ｐ_Ｉ１＝０、すなわち立直静止（原点）側に近づくようになる。一方、ロボット１の現在の状態に対応する点が、領域３０２内（いわゆる第１、第３象限）にある場合、時間経過に従って矢印ＡＲに沿って移動して、ｐ_１，ｐ_Ｉ１共に絶対値が大きくなっていき、すなわち原点から離れて、最終的に線１０１，１０２で囲まれる領域外に飛び出してしまう。

　また、領域３０１内の、原点を通る線分２０１は以下の意味を有する。ある時点におけるロボット１の状態に対応する点が、この線分２０１上にある場合、時間経過に従って状態が遷移する結果、ｐ_１＝０、ｐ_Ｉ１＝０、すなわち原点に到達（復帰）し得る。

　さらに、線分２０１を含む両側には、所定の幅を有する、濃い目の網掛けが施された領域２０２が設定されている。前記したように、ある時点でロボット１の状態が線分２０１上に一致するときは、ロボット１に対する姿勢制御は不要であるが、線分２０１から離れていても、所定の範囲内では、ロボット１の姿勢を制御することで、原点に戻すことが可能である。領域２０２は、この範囲を示すもので、式(8)で示す胴体姿勢による安定化可能範囲に該当する。そして、式(8)で示す安定化可能範囲内で胴体姿勢を制御する、胴体角度ｑ_ｔｒの目標値を式(9)に示している。式(9)は、式(7)の制約の範囲内で、安定化可能範囲を最大化するものである。

　一方、安定化可能範囲２０２を除く領域３０１、及び領域３０２からは、そのままでは図３の原点（姿勢安定の目標値）に辿り着けない安定化不可能範囲であり、放置すれば最終的に転倒してしまうことになるため、一方の脚１０を適切な時点で踏み出す必要がある。領域３０１に位置する場合、原点に近づく状態にあることから、脚を踏み出す必要はない。また、領域３０２に位置する場合、原点から離れていくので、足を踏み出す必要がある。踏み出す脚１０は、次に着地によって状態量ｐ_Ｉ１が領域２０２に遷移する側である。脚１０を着いたとき、領域２０２に入れれば、最終的に安定化可能となる。また、式(10)を満たすように脚１０を着けば、状態量ｐ_１、ｐ_Ｉ１は領域２０２の中心位置にくるため、安定化不可能範囲から最も遠ざかることができる。一方、領域２０２に入れない場合（例えば状態量ｐ_１の値が大きすぎたり小さすぎたりした場合）、左右の線１０１，１０２の位置にくるように（図３中の左右に示す略図ロボットのように、遊脚側の脚を立脚側の脚から最大限離れた位置にするように）足を着くことで、次の歩で、状態量ｐ_１、ｐ_Ｉ１の値が最大限原点に近づくことになる。これを繰り返すと、状態量ｐ_１、ｐ_Ｉ１がどのような値であっても、最終的に安定化可能範囲に移行でき、式(9)を利用して原点に戻ることができる。

　図２において、ロボット１は、各モータ３０を制御する、制御部１００及び記憶部１１０を有する制御装置によって自立制御可能にされている。制御部１００は、例えばマイクロコンピュータで構成され、モータ３０、角度センサ４０及び記憶部１１０と接続されている。記憶部１１０は、制御プログラム、制御プログラムを実行する上で、また各演算を実行する際に参照される各種の参照データを記憶するエリア、及びワークエリアを有している。制御部１００は、制御プログラムを実行することで、角度情報取得部１０１、角度運動量算出部１０２、積分値算出部１０３、安定判定部１０４、姿勢目標設定部１０５、踏出指示部１０６、踏出目標設定部１０７、及び歩行制御部１０８として機能する。

　角度情報取得部１０１は、角度センサ４０からそれぞれの角度情報を所定周期、例えば０．０１秒～０．０００１秒程度、一般的には０．００１秒程度で取り込むものである。取り込み周期は、用途等に応じて適宜の範囲で設定可能である。角運動量算出部１０２は、各角度情報を取り込む毎に、立脚側の脚１０の足首周りの角運動量ｐ_１を、式(1)を実行して算出するものである。積分値算出部１０３は、式(3)を実行して、近似の積分値ｐ_Ｉ１を算出するものである。

　安定判定部１０４は、角運動量ｐ_１、積分値ｐ_Ｉ１が得られる毎に、各種の制約条件を用いて、式(8)の判定処理を実行し、判定条件を満たす場合には、安定化可能範囲にあると判定し、そうでない場合は、安定化不可能範囲にあると判定する。姿勢目標設定部１０５は、判定結果が安定可能範囲である場合に、式(9)から得られる目標値を胴体２０の姿勢制御信号として生成する。

　踏出指示部１０６は、判定結果が安定化不可能範囲である場合に、片方の脚１０、例えば不安定となる方向に近い側の脚１０に対する踏み出しの指示を行う。踏出目標設定部１０７は、踏み出し指示された側の脚１０に対して、次の着地時に式(10)を満たすように足先位置の指令値を算出する。踏み出された遊脚と立脚との関係は、遊脚側の脚１０が地面ＥＡに接触した瞬間に遊脚は立脚に切り替わり、同時に立脚は遊脚に切り替わるものとする。遊脚と立脚とが切り替わるとき、図１と式(3)とから、関節角度ベクトルｑは値が変わり、積分値ｐ_Ｉ１の値も瞬間的に切り替わる。この値は、脚１０を着く位置を調節することにより、線１０１，１０２で囲まれた領域の任意の値をとる。なお、ここでは、遊脚と立脚とが入れ替わる、脚の衝突の際に生じる撃力は０として、遊脚と立脚とが切り替わるときにｐ_１の値は変化しないものとして扱っている。ただし、ｐ_１の値が変化するとして扱ってもよい。

　一方の脚１０を踏み出す場合、当該脚が着地した次の判定時点で、安定化可能範囲に入った、すなわち式(8)が満たされると判定されると、以後は、式(9)によって直立静止姿勢に戻ることができる。なお、式(10)から得られる目標値を設定することで、線分２０１に載せることが可能となる。

　一方、当該脚が着地した次の判定時点でも安定化不可能範囲と判定されると、線１０１，１０２上に位置するように遊脚側の脚１０を立脚側の脚から最大限遠い位置に着地するように制御することで、その次の脚の踏み出しで、状態量ｐ_１，ｐ_Ｉ１を最大限、原点に近づけることができ、最終的に直立姿勢に戻ることができる。角度情報取得部１０１～踏出目標設定部１０７は、かかる処理を繰り返すことで、外乱により生じた姿勢不安定を、脚１０を移動せずに、あるいは移動させることによって安定姿勢に戻すことが可能となる。

　なお、歩行制御部１０８は、所定の速度で脚１０Ｌ，１０Ｒを交互に繰り出すように制御するものである。歩行時において転倒することなく交互に他方の脚１０を踏み出す処理の詳細については後述する。

　次に、図４のフローチャートを用いて、外力の印加に対する姿勢安定処理を説明する。まず、角度情報を取得し、脚１０の接地点周り（足首周り）の角運動量ｐ_１及びその積分値ｐ_Ｉ１が所定の周期毎に算出される（ステップＳ１）。次いで、算出された角運動量ｐ_１及びその積分値ｐ_Ｉ１に基づいて、式(8)による判定処理が実行される（ステップＳ３）。判定結果が安定化可能範囲であると、胴体姿勢の目標値ｑ_ｔｒｄが、式(9)に基づいて設定され（ステップＳ５）、脚モータ３１に同期駆動指令される（ステップＳ７）。この場合、ステップＳ１～Ｓ５を所定周期で繰り返すことで、最終的に原点に戻る。

　一方、ステップＳ３で、判定結果が安定化不可能範囲であると、式(10)に基づいて目標足先位置が設定され、片方の脚１０の脚モータ３１に対して踏み出し駆動の指示が行われる（ステップＳ９）。次いで、ステップＳ１に戻り、次の検出周期で、ステップＳ３で安定化可能範囲に入ったか否かが判断され、入ったのであれば、最終的に原点に戻り、一方、入らなかった場合には、入るまでステップＳ９を繰り返し、最終的に、ステップＳ３を満たすことになる。

　次に、（４）足首にアクチュエータがある態様の制御について説明する。

　前記（１）～（３）で説明した内容は、図５のように脚１０の下端に足首関節を介して足部１１を備え、また足首関節にアクチュエータである足首モータ３４が取り付けられたロボット１Ａにも適用できる。足首モータ３４は、地面ＥＡに接地している足部１１に対して脚１０の相対角度を変える。足部１１に足首モータ３４が設けられている場合、式(2)のτ_１は一定の範囲、

で発生させられる。足首関節から足部１１の爪先や踵までの距離が長いほど、この範囲は大きくなる。この範囲を超えると、爪先や踵が地面から離れる。地面から離れた爪先（又は踵）と反対側の地面に接触している踵（又は爪先）は、その接触点を中心に回転できる足首モータ３４を持たない関節と同等な状態となる。この場合でも、足首にアクチュエータを持たない場合の制御法が適用できる。式(11)の範囲で姿勢を安定化できる場合は、足首関節により姿勢を安定化できるので、胴体姿勢を調節する必要はない。ただし、胴体姿勢を調節する態様としてもよい。ここでは、胴体姿勢は鉛直となるように設定して、ｑ_ｔｒ＝０として説明を行う。このとき、式(1)と式(5)から、状態量をｐ_１とｐ_Ｉ１とし、入力をτ_１とした状態方程式

が得られる。本式(12)と式(11)とから、足首トルクによってｐ_１とｐ_Ｉ１とを０に収束させられる最大範囲は、

となる。本式(13)の範囲を足首トルクによる安定化可能範囲と呼び、本式(13)が満たされない範囲を安定化不可能範囲と呼ぶ。安定化可能範囲を最大化する入力の一例は、

である。ただし、τ_１＞＝τ_１ｍａｘのときは、τ_１＝τ_１ｍａｘとし、τ_１＜＝τ_１ｍｉｎのときは、τ_１＝τ_１ｍｉｎとする。また、ｋ_３，ｋ_４は、一定の範囲で設定する定数である。よって、式(13)が満たされているときは、式(14)を用いて姿勢を安定化する。式(13)が満たされない場合は、前述の胴体２０の姿勢を用いた安定化法を併用する。両方併用した場合、式(6)と式(12)の入力であるｑ_ｔｒとτ_１が両方作用することで、安定化可能範囲は、

に広がる。それでも安定化できない場合は、前述のように脚１０を踏み出し、次に足部１１が着いたときに安定化できるように足部１１の位置を制御すればよい。

（５）歩行運動への応用
　ここでは、以上の説明を応用して、さらに歩行運動を生成する制御法を説明する。足首にアクチュエータがない場合、τ_１＝０を考える。このとき、式(6)から現在の状態量ｐ_１(ｔ)，ｐ_Ｉ１(ｔ)と、Ｔ秒後の状態量ｐ_１(ｔ＋Ｔ)，ｐ_Ｉ１(ｔ＋Ｔ)の関係を解析的に求める。

である。これらの式(16)，(17)から、胴体姿勢が立直な場合すなわちｑ_ｔｒ＝０のｐ_１とｐ_Ｉ１の推移を図６の矢印ＡＲで示す。転倒を防止するために腰の高さを一定以上に保つ必要があり、そのためにはｐ_Ｉ１の値を、図６の線１０１，１０２で囲まれる範囲内に留める必要がある。図６中の右方向に歩行するためには、ｐ_Ｉ１が負の値をとっている初期姿勢から、ｐ_Ｉ１が正の値をとる姿勢まで推移し、同じ初期姿勢に戻れればよい。これを実現する軌道の一つを図６に歩行線４０１で示す。よって、一定の歩行を行うためには、図中の線１０１，１０２で囲まれた領域の任意の点から目標値としての歩行線４０１に辿り着けばよい。

　歩行制御部１０８は、両脚１０Ｌ，１０Ｒの交互踏み出し操作の制御信号を出力しつつ、さらに、角度情報取得部１０１～積分値算出部１０３及び踏出指示部１０６、踏出目標設定部１０７からの制御信号に従って、踏み出し及び着地位置を指示する。これを実現するためには、遊脚側の脚１０を適切な位置に着けばよい。ここでは、遊脚側の脚１０が地面ＥＡに接触した瞬間に遊脚は立脚に切り替わり、同時に立脚は遊脚に切り替わるものとする。前述のように、遊脚と立脚が切り替わるとき、ｐ_Ｉ１の値は瞬間的に切り替わる。この値は、脚１０を着く位置を調節することにより線１０１，１０２で囲まれた領域の任意の値に移動できる。なお、遊脚と立脚が入れ替わるとき、脚１０の衝突の際に生じる撃力に応じてｐ_１の値も影響を受けるが、ここでは、前記したように撃力は０として、遊脚と立脚が切り替わるときにｐ_Ｉ１の値は変化しないものとして扱う。ただし、ｐ_Ｉ１の値が変化する場合にも適用可能である。ｐ_１とｐ_Ｉ１とが歩行線４０１上の任意の点に辿り着くためには、例えば次のようなアルゴリズムを採用し、制御部１００に実行させればよい。

　図６の領域３０１内に、ｐ_１とｐ_Ｉ１の値があるとき、一定時間経過するとこれらの値は歩行線４０１に近づく。よって、この場合、現運動をそのまま継続すればよい。それに対し、領域３０２内にあるときは歩行線４０１から遠ざかるため、遊脚側の脚１０を適切な場所に着く必要がある。また、線１０１，１０２で囲まれた領域以外にある場合も、転倒を防止するために遊脚側の脚１０を適切な場所に着く必要がある。脚１０を着く位置は、次の１歩で、ｐ_１とｐ_Ｉ１の値が最も歩行線４０１に近づくことができる位置にする。これにより、図６中の線１０１，１０２で囲まれた任意の点から歩行線４０１に辿り着くことができる。外乱等の影響で、ｐ_１とｐ_Ｉ１が大きく変わったとしても、上記のアルゴリズムを継続して実行することで、自動的に目標の歩行状態（歩行線４０１）に収束することが可能となる。

　ただし、図６の原点に状態がある場合（ｐ_１＝０，ｐ_Ｉ１＝０）には、ロボット１は歩行中であるにも関わらず停止してしまう。これを避ける方法として、胴体２０を傾けることによって領域３０１に状態を推移させればよい。

　上記のアルゴリズムに胴体姿勢を調節する制御を加えることで、より少ない歩数で目標の歩行状態に辿り着くアルゴリズムが考えられる。すなわち、胴体姿勢が鉛直でないときｃ_２≠０となり，式(16)と式(17)から、ｐ_１とｐ_Ｉ１の推移は、図７の矢印ＡＲのようになる。このとき、腰の高さを一定以上に保つｐ_Ｉ１の値の範囲は、図７のように胴体２０を傾けた方に広げることができる。これにより、ｐ_１とｐ_Ｉ１の値は、一歩でより歩行線４０１に近づけるようになる。ｐ_１とｐ_Ｉ１の値が、歩行線４０１に十分近づいたときは、胴体姿勢を鉛直に戻せば、目標とする歩行が実現できることになる。

　なお、前記実施形態では、ロボット１に外力が印加された場合を想定したが、その他、外乱として不整地歩行時等における姿勢バランスを損なう乃至は転倒しかねないような力が作用する場合を含む。また、力が作用する態様として、歩行中での脚の踏み出し、歩行状態から静止する場合での姿勢バランス制御にも適用できる。

　前記実施形態では、歩行ロボットで説明したが、本発明は、歩行ロボットに限定されず、足首にアクチュエータを備える脚ロボット、またアクチュエータを備える１脚型あるいは２脚型の歩行補助ロボットや義足等の脚型機構体に適用可能である。これらに適用することで、人体を含めた本体の姿勢バランスを制御して転倒防止を図るようにすることが可能となる。また、脚は左右で２本の他、３本型、４本型乃至はそれ以上の本数を有するものでもよい。また、脚を前後方向に踏み出す態様に限定されず、その他の方向にも踏み出す、例えば前後左右方向に踏み出し可能なように胴体との連結部に球状ジョイントを設ける態様としてもよい。

　前記実施形態における外力に対する姿勢バランス制御技術は、他の用途にも適用可能である。例えば、コンピュータグラフィックス（ＣＧ）において、仮想のキャラクタに種々の箇所に外力が作用した時の臨場感のある姿勢バランスの挙動を再現するべく、従来では、実際に人体（その他の生物含む）の各箇所にセンサを取付けて種々の動きを検出するモーチョンキャプチャー技術を用いていたが、前記実施形態におけるロボットを採用することで各種の姿勢バランスの挙動情報を容易に取得でき、ＣＧへの利用に供することが可能となる。

　以上説明したように、本発明に係る脚型機構体は、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体と、前記脚及び本体の状態に基づいて前記本体の姿勢を安定にする姿勢制御手段とを備えた脚型機構体において、前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出手段と、算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるか、すなわち姿勢安定化の可能性の有無を判定し、判定結果を前記姿勢制御手段に出力する判定手段とを備えることが好ましい。

　また、本発明に係る姿勢制御方法は、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢を、前記脚及び本体の状態に基づいて安定にする姿勢制御手段を備えた脚型機構体の姿勢制御方法において、前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出工程と、算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるか、すなわち姿勢安定化の可能性の有無を判定し、判定結果を前記姿勢制御手段に出力する判定工程とを備えることが好ましい。

　また、本発明に係るプログラムは、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢を、前記脚及び本体の状態に基づいてコンピュータで安定に制御するプログラムにおいて、前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出手段、及び算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるか、すなわち姿勢安定化の可能性の有無を判定し、判定結果を姿勢安定処理に供する判定工程、として前記コンピュータを機能させることが好ましい。

　これらの発明によれば、姿勢制御手段によって、少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢が、脚及び本体の状態に基づいて安定に制御される。状態算出手段によって、脚及び本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出され、判定手段によって、算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるか、すなわち姿勢安定化の可能性の有無が判定され、判定結果が姿勢制御手段に出力される。従って、簡易な構成でありながら、本体の姿勢すなわち脚型機構体の姿勢のバランスが保たれ、外力を含む外乱によっても転倒が回避される。

　また、前記脚型機構体は、前記脚及び前記本体の角度情報を検出する角度センサを備え、前記角度センサの少なくとも１個が重力方向に対する角度を検出可能とすることが好ましい。かかる構成によれば、角度センサの少なくとも１個に重力方向に対する角度を検出可能なものを用いるので、脚の下端周りの角運動量、その積分値が算出可能となる。

　また、前記脚型機構体は、複数の脚の上部に前記本体の姿勢を相対変更可能に連結する脚アクチュエータを備え、前記姿勢制御手段は、前記判定結果が、前記算出された角運動量及びその積分値が目標値に対して近づく状態であるとした場合、前記各脚の下端位置を静止させた状態で前記脚アクチュエータを駆動して前記本体の姿勢を安定させる第１の姿勢制御手段と、前記判定結果が、前記算出された角運動量及びその積分値が目標値に対して離れる状態であるとした場合、前記脚アクチュエータのいずれかを駆動して対応する脚の踏み出し操作を行って前記本体の姿勢を安定させる第２の姿勢制御手段とを備えることが好ましい。かかる構成によれば、算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるか、すなわち姿勢安定化の可能性の有無に応じて、第１の姿勢制御手段による、脚の踏み替えを行うことなく姿勢バランスを取る態様と、第２の姿勢制御手段による、踏み替えを行って姿勢バランスを取る態様とを採用したので、確実に姿勢バランスが取れることになる。

　また、前記姿勢制御手段は、前記脚アクチュエータに対して、判定時点における前記角運動量及びその積分値に基づいて前記本体の姿勢の目標値を設定することが好ましい。かかる構成によれば、目標値が、判定時点で一旦算出した角運動量及びその積分値に基づいて設定されるので、処理が容易となる。

　また、歩行ロボットは、複数の脚を有する脚型機構体において、前記複数の脚の各脚アクチュエータを個別に操作して歩行を行わせる歩行制御手段を備えることが好ましい。かかる構成によれば、歩行ロボットにおいて不可欠な静止時の姿勢バランスの制御が容易に行われる。また、歩行時における外乱に対しても姿勢バランス処理を適用することが可能となる。

　また、前記歩行ロボットは、前記脚が、長手方向の途中で分割され、関節アクチュエータを介して分割部位が相対回転又は伸縮可能に連結されていることが好ましい。かかる構成によれば、関節アクチュエータを作動することで、各関節の相対角度又は脚の長さを制御できるので、姿勢バランス性能が向上し、また着地位置の精度を高めることが可能となる。

　１　歩行ロボット（脚型機構体）
　１０　脚
　２０　胴体（本体）
　３０　モータ（アクチュエータ）
　３１　脚モータ（脚アクチュエータ）
　４０　角度センサ
　１００　制御部（姿勢制御手段）
　１０２　角運動量算出部（状態算出手段）
　１０３　積分値算出部（状態算出手段）
　１０４　安定判定部（判定手段）
　１０５　姿勢目標設定部（第１の姿勢制御手段）
　１０６　踏出指示部（第２の姿勢制御手段）
　１０７　踏出目標設定部（第２の姿勢制御手段）

Claims

　少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体と、前記脚及び本体の状態に基づいて前記本体の姿勢を安定にする姿勢制御手段とを備えた脚型機構体において、
　前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出手段と、
　算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるかを判定し、判定結果を前記姿勢制御手段に出力する判定手段とを備えた脚型機構体。
　前記脚及び前記本体の角度情報を検出する角度センサを備え、前記角度センサの少なくとも１個が重力方向に対する角度を検出可能であることを特徴とする請求項１に記載の脚型機構体。
　複数の脚の上部に前記本体の姿勢を相対変更可能に連結する脚アクチュエータを備え、
　前記姿勢制御手段は、
　前記判定結果が、前記算出された角運動量及びその積分値が目標値に対して近づく状態であるとした場合、前記各脚の下端位置を静止させた状態で前記脚アクチュエータを駆動して前記本体の姿勢を安定させる第１の姿勢制御手段と、
　前記判定結果が、前記算出された角運動量及びその積分値が目標値に対して離れる状態であるとした場合、前記脚アクチュエータのいずれかを駆動して対応する脚の踏み出し操作を行って前記本体の姿勢を安定させる第２の姿勢制御手段とを備えた請求項１又は２に記載の脚型機構体。
　前記姿勢制御手段は、前記脚アクチュエータに対して、判定時点における前記角運動量及びその積分値に基づいて前記本体の姿勢の目標値を設定する請求項３に記載の脚型機構体。
　請求項３又は４に記載の脚型機構体に、前記複数の脚の各脚アクチュエータを個別に操作して歩行を行わせる歩行制御手段を備えた歩行ロボット。
　前記脚は、長手方向の途中で分割され、関節アクチュエータを介して分割部位が相対回転又は伸縮可能に連結されていることを特徴とする請求項５に記載の歩行ロボット。
　少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢を、前記脚及び本体の状態に基づいて安定にする姿勢制御手段を備えた脚型機構体の姿勢制御方法において、
　前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出工程と、
　算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるかを判定し、判定結果を前記姿勢制御手段に出力する判定工程とを備えた姿勢制御方法。
　少なくとも１個の脚の上部に相対的に姿勢変更可能に連結された本体の姿勢を、前記脚及び本体の状態に基づいてコンピュータで安定に制御するプログラムにおいて、
　前記脚及び前記本体の各角度情報から、立脚状態にある脚の下端周りの角運動量及びその積分値を周期的に算出する状態算出手段、
　及び算出された角運動量及びその積分値が目標値に対して近づく状態であるか離れる状態であるかを判定し、判定結果を姿勢安定処理に供する判定手段、として前記コンピュータを機能させるプログラム。