JP2009107030A - 制御装置、制御方法、及び２足歩行ロボット - Google Patents

Abstract

【課題】搭乗者によって加えられる外力に対応することができる制御装置、制御方法、及び２足歩行ロボットを提供する。
【解決手段】制御装置５は、腰部補償軌道算出手段に加え、設定ＺＭＰ軌道変更量算出手段と、着地位置変更量算出手段とを備えることとした。これにより、制御装置５は、設定ＺＭＰ軌道を変更すると共に、着地位置を変更することにより、腰部３の軌道の発散を遅延又は収束させることができる。従って、制御装置５は、設定ＺＭＰ軌道変更量算出手段、及び着地位置変更量算出手段のうちいずれか一方を用いる従来に比べ、大きい外乱に対応でき、安定性を向上することができる。
【選択図】図１

Description

本発明は、制御装置、制御方法、及び２足歩行ロボットに関するものである。

従来、２足歩行ロボットとして、例えば、パラレルリンク機構や、シリアルリンク型脚機構を用いた人間搭乗型ロボットが開示されている。パラレルリンク機構を採用した早稲田大学のＷＬ−１６ＲＩＶは軽量、かつ小型化を実現しており、積載重量比は、１．２５（最大搭乗者体重７５ｋｇ、ロボット重量６０ｋｇ）である。これに対し、シリアルリンク型脚機構を採用したトヨタ自動車株式会社のｉ−ｆｏｏｔは、０．３（最大搭乗者体重６０ｋｇ、ロボット重量２００ｋｇ）、韓国科学技術院（ＫＡＩＳＴ）のＨＵＢＯは、０．６７（最大搭乗者体重１００ｋｇ、ロボット重量１５０ｋｇ）となっており、パラレルリンク機構の方が、シリアルリンク型脚機構に比べ、ロボットの積載重量比が高い。

ロボットの積載重量比が高いことは、人間の住環境で稼動する２足歩行車椅子の実現のためには非常に優位性の高いことであるが、積載重量比が高いことはそれだけ搭乗者の運動がロボットの安定性に及ぼす影響が大きくなることを意味する。

積載重量比の高い人間搭乗型２足歩行ロボットの安定化手法としては、ロボットの歩行中において、搭乗者がロボットの腰部の揺れによって受動的に揺動する運動を事前にモデル（受動的力学モデル）化しておき、その搭乗者の受動的運動を考慮した上で安定な歩行パターンをオフラインで生成するものが開示されている（例えば、非特許文献１）。

ところが、この安定化手法は、人間が搭乗した状態における２足歩行の安定化に有効であるものの、事前に予測することができない搭乗者の運動や外力には対応することができないという欠点があった。

一方、人間搭乗型でない通常の２足歩行ロボットの外力に対する安定化手法としては、外力に応じて腰部を加速し、これによる腰部軌道の発散を防ぐために目標ＺＭＰ軌道を変更する手法が開示されている（例えば、非特許文献２及び３）。

上記非特許文献２は、外力を腰部に取り付けた力センサで取得し、着地位置を変更することにより目標ＺＭＰ軌道を変更するものである。

また、上記非特許文献３は、実測ＺＭＰから仮想外力を算出し、立脚足部内で仮想的に目標ＺＭＰ軌道を変更するものである。
橋本健二、菅原雄介、川瀬正幹、太田章博、田中智明、沢戸瑛昌、林昭宏、遠藤信綱、林憲玉、高西淳夫、「脚部にパラレルリンク機構を用いた２足ロコモータの開発（第１２報：搭乗者の力学モデルを用いた歩行パターン生成）」、日本ロボット学会第２４回学術講演会予稿集、2006 A.Takanishi, et al., A control method for dynamic biped walking under unknown external force, Proc. of the IEEE International Workshop on Intelligent Robots and Systems, pp. 795-801,1990 長坂他、「体幹位置コンプライアンス制御を用いた人間型ロボットの歩行安定化」、日本ロボット学会第１７回学術講演会予稿集、1999

しかしながら、上記非特許文献２では、継続する外力には適応することができず、また、上記非特許文献３では、比較的小さな外力にしか適応できないという問題があった。また、非特許文献２及び３のいずれにおいても、外力はロボットの外部から加わる力のみを想定しており、人間搭乗型２足歩行ロボットにみられるような、搭乗者によって加えられる力に対して有効ではないという問題があった。

そこで本発明は上記した問題点に鑑み、搭乗者によって加えられる外力に対応することができる制御装置、制御方法、及び２足歩行ロボットを提供することを目的とする。

上記目的を達成するために、請求項１に係る発明は、足部を有する脚部、及び腰部を有する２足歩行ロボットの制御装置において、前記足部の軌道と、前記腰部の軌道とを設定すると共に、前記足部の軌道と前記腰部の軌道とに基いて設定ＺＭＰ軌道を設定する歩行パターン生成手段と、歩行中の前記２足歩行ロボットに加わった外力に応じて前記腰部を加速させるための腰部補償軌道を算出する腰部補償軌道算出手段と、所定の範囲内において前記設定ＺＭＰ軌道を変更する設定ＺＭＰ軌道の変更量を算出する設定ＺＭＰ軌道変更量算出手段と、前記脚部が着地する位置の変更量を算出する着地位置変更量算出手段とを備えることを特徴とする。

また、請求項２に係る発明は、前記足部に生じる足部反力に基づいて実測ＺＭＰ軌道を算出する実測ＺＭＰ算出手段を備え、前記腰部補償軌道算出手段は、前記設定ＺＭＰ軌道を変更した目標ＺＭＰ軌道を、前記実測ＺＭＰ軌道から減じたＺＭＰ偏差に基いて、腰部補償軌道を算出することを特徴とする。

また、請求項３に係る発明は、搭乗者が発生した力を前記腰部における座面反力として計測し、前記外力は、前記座面反力を用いることを特徴とする。

また、請求項４に係る発明は、前記歩行パターン生成手段は、理想的な搭乗者が前記腰部周りに発生する理論座面反力を設定し、前記座面反力から前記理論座面反力を減じて、外乱成分を抽出する外乱成分抽出手段を備え、前記外乱成分に基づいて前記腰部補償軌道を算出することを特徴とする。

また、請求項５に係る発明は、足部を有する脚部、及び腰部を有する２足歩行ロボットの制御方法において、前記足部の軌道と、前記腰部の軌道とを設定すると共に、前記足部の軌道と前記腰部の軌道とに基いて設定ＺＭＰ軌道を設定する歩行パターン生成ステップと、歩行中の前記２足歩行ロボットに加わった外力に応じて前記腰部を加速させるための腰部補償軌道を算出する腰部補償軌道算出ステップと、所定の範囲内において前記設定ＺＭＰ軌道を変更する設定ＺＭＰ軌道の変更量を算出する設定ＺＭＰ軌道変更量算出ステップと、前記脚部が着地する位置の変更量を算出する着地位置変更量算出ステップとを備えることを特徴とする。

また、請求項６に係る発明は、前記足部に生じる足部反力に基づいて実測ＺＭＰ軌道を算出する実測ＺＭＰ算出ステップを備え、前記腰部補償軌道算出ステップは、前記設定ＺＭＰ軌道を変更した目標ＺＭＰ軌道を、前記実測ＺＭＰ軌道から減じたＺＭＰ偏差に基づいて、腰部補償軌道を算出することを特徴とする。

また、請求項７に係る発明は、搭乗者が発生した力を前記腰部における座面反力として計測し、前記外力は、前記座面反力を用いることを特徴とする。

また、請求項８に係る発明は、前記歩行パターン生成ステップは、理想的な搭乗者が前記腰部周りに発生する理論座面反力を設定する理論座面反力設定ステップを有し、前記座面反力から前記理論座面反力を減じて、外乱成分を抽出する外乱成分抽出ステップを備え、前記外乱成分に基づいて前記腰部補償軌道を算出することを特徴とする。

また、請求項９に係る発明は、足部を有する脚部、腰部、及び制御装置を有する２足歩行ロボットにおいて、前記制御装置は、前記足部の軌道と、前記腰部の軌道とを設定すると共に、前記足部の軌道と前記腰部の軌道とに基いて設定ＺＭＰ軌道を設定する歩行パターン生成手段と、歩行中の前記２足歩行ロボットに加わった外力に応じて前記腰部を加速させる腰部補償軌道を算出するための腰部補償軌道算出手段と、所定の範囲内において前記設定ＺＭＰ軌道を変更する設定ＺＭＰ軌道の変更量を算出する設定ＺＭＰ軌道変更量算出手段と、前記脚部が着地する位置の変更量を算出する着地位置変更量算出手段とを備えることを特徴とする。

また、請求項１０に係る発明は、前記足部に足部反力センサを設け、前記足部反力センサによって計測した足部反力に基いて実測ＺＭＰを算出する実測ＺＭＰ算出手段を備え、前記腰部補償軌道算出手段は、前記設定ＺＭＰ軌道を変更した目標ＺＭＰを、前記実測ＺＭＰから減じたＺＭＰ偏差に基いて、腰部補償軌道を算出することを特徴とする。

また、請求項１１に係る発明は、前記腰部の上部に搭乗者用シートを有し、前記搭乗者用シートと前記腰部との間に座面反力計測センサを設け、前記座面反力計測センサにより前記外力を計測することを特徴とする。

また、請求項１２に係る発明は、前記歩行パターン生成手段は、理想的な搭乗者が前記腰部周りに発生する理論座面反力を設定し、前記座面反力から前記理論座面反力を減じて、外乱成分を抽出する外乱成分の抽出手段を備え、前記外乱成分に基づいて前記腰部補償軌道を算出することを特徴とする。

本発明の請求項１，５及び９に記載の発明によれば、制御装置は、設定ＺＭＰ軌道を変更すると共に、着地位置を変更することにより、腰部の軌道の発散を遅延又は収束させることができる。従って、制御装置は、設定ＺＭＰ軌道変更量算出手段、及び着地位置変更量算出手段のうちいずれか一方を用いる従来に比べ、大きい外乱に対応でき、安定性を向上することができる。

また、請求項２，６及び１０に記載の発明によれば、搭乗者の運動による外乱以外の外乱、すなわち、路面の細かな凹凸やロボット各部のたわみなどのモデル誤差による不安定性に関しても補償できるので、安定性をより向上することができる。

また、請求項３，７及び１１に記載の発明によれば、搭乗者用シートに着座した搭乗者が発生する外力と、その他の外力とを分離して測定することができるので、フィードバック量を減らすことができ、より安定性を向上することができる。

また、請求項４，８及び１２に記載の発明によれば、さらにフィードバック量を減らすことができる。

以下図面を参照して、本発明の好適な実施形態について説明する。
１．全体構成
（１）ロボットの構成
図１に示す２足歩行ロボット１は、脚部２、腰部３、前記腰部３の上部に設けられた搭乗者用シート４、及び制御装置５を備え、前記搭乗者用シート４に搭乗したユーザ（以下、搭乗者という）が想定外の外力を発生した場合でも、前記制御装置５で前記脚部２の動作を制御することにより、安定的に２足歩行し得るように構成されている。

脚部２は、脚部としての右足ユニット６及び左足ユニット７からなる。右足ユニット６及び左足ユニット７は、それぞれ、脚機構８、足部としての足底部９、及び固定板10を有し、左右対称に構成されている。

脚機構８は、３組の脚構成部11を有する。３組の脚構成部11は、平面視三角形の頂点に上端及び下端をそれぞれ配設するように設けられている。脚構成部11は、２本の直動リンク12と、該直動リンク12の両端にそれぞれ設けられたベース部側受動ジョイント13（本実施形態では合計１２個）及び足底側受動ジョイント14（本実施形態では合計６個）とにより構成されている。この脚構成部11は、２本の直動リンク12をＶ字状に配置し、上端が２つのベース部側受動ジョイント13を介してそれぞれ腰部３に連結されている。一方、脚構成部11は、２本の直動リンク12の下端を１つの足底側受動ジョイント14に連結した上で、固定板10に連結されている。

ベース部側受動ジョイント13と、足底側受動ジョイント14とは、ボールジョイントとユニバーサルジョイントとを有する。このようにして、脚機構８は、パラレルリンク機構の一種であるスチュアートプラットフォームにより構成されている。

直動リンク12は、送り螺子機構からなり、特に図示しないが、主にアウタチューブ、該アウタチューブ内を進退可能に設けられたインナーロッド、及び、長手方向軸を中心として前記インナーロッドを回転させるモータで構成され、インナーロッドの外形に形成された雄ネジがインナーロッドの内周に形成された雌ネジに対し螺合されている。また、このモータは後述するモータドライバにより制御される。尚、直動リンク12は特に限定されるものではなく、油圧、水圧、及び空気圧などを用いたシリンダや直動型アクチュエータを有する機構としてもよい。

腰部３は、ベース部15を備え、前記ベース部側受動ジョイント13、搭乗者用シート４、及びバッテリなどを搭載し得るように構成されている。腰部３の背面側には制御装置５が取り付けられている。

制御装置５は、図示しないが、内部のメモリに予め記憶している動作制御プログラムなどの各種プログラムに従って各種処理を実行する。そして、制御装置５は、電源スイッチがオンされるなどによって電源が投入されると、メモリから基本プログラム等の各種プログラムを適宜読み出して展開することにより、これら各種プログラムによって２足歩行ロボット１全体を統括的に制御し、各種機能、例えば外乱補償制御処理を実現し得るように構成されている。

図２に示すように、腰部３と搭乗者用シート４の間には、座面反力計測センサ20が設けられている。また、足底部９と固定板10との間には、足部反力計測センサ21が設けられている。座面反力計測センサ20は、図３に示すように、６軸力覚センサであって、各軸方向の力３成分と各軸周りのモーメント３成分を同時に且つ逐次連続的に高精度で検出することができる。

尚、所定の範囲としての支持多角形とは、路面と２足歩行ロボット１とが接地している部分である固定板10を凹にならないように囲んで形成される面積最大の凸多角形をいう。

また、２足歩行ロボットが歩行中に安定な支持状態を維持するために必要な条件を考えた場合、２足歩行ロボットが安定な支持状態を保てなくなる、つまり転倒するということは、足底部と路面との接点（３点以上）が形成する支持多角形のある辺または点を通る路面上の直線を軸に回転運動をしていると考えられる。すなわち、支持多角形から見て外向きのモーメントが作用していると考えられる。逆に言えば、支持多角形上の全ての辺および点のまわりに外向きのモーメントが発生せず、内向きのモーメントのみ発生していれば、２足歩行ロボットは安定な支持状態を維持できる。このときの２足歩行ロボットの支持状態を考えると、足底の接地点が浮かないため、全ての接地点において２足歩行ロボットから路面に作用する重力および慣性力による力は路面を押す向きであり、これらの合力が作用する点は足底の支持多角形内になければならない。この点回りの合力によるモーメントは明らかにゼロであり、この点をＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ、ゼロモーメントポイント）と呼ぶ。

（２）システム構成
次に、制御装置５のシステム構成について、図４を参照して説明する。図４に示す制御装置５は、歩行パターン生成手段25、外乱成分抽出手段26、実測ＺＭＰ算出手段27、腰部補償軌道算出手段28、設定ＺＭＰ軌道変更量算出手段29、着地位置変更量算出手段30、足部軌道生成手段31、座標変換手段32及び逆運動学演算手段33からなり、座面反力計測センサ20及び足部反力センサ21から計測値が入力され得るように接続されていると共に、上述したモータドライバ34へ駆動信号を出力し得るように構成されている。

制御装置５は、２足歩行ロボット１の右足ユニット６及び左足ユニット７に対する上述した一連の動作制御処理（図示しない）を内部のメモリに予め記憶している動作制御プログラムに従って実行する。

まず、制御装置５は、例えば、右足ユニット６を動作させる場合、予め歩行パターン生成手段25で設定された足部設定パターン軌道（^ｍＸ_ｐｆ）に基いて、足部軌道生成手段31により足部出力軌道（^ｍＸ_ｆ）を算出して、算出した足部出力軌道（^ｍＸ_ｆ）信号を座標変換手段32を介して逆運動学演算手段33に出力する。そうすると、制御装置５は、逆運動学演算手段33において、右足ユニット６の逆運動学を計算し、各リンク長さ（Ｉ_{ｆｉｎａｌ}）を算出する。そして制御装置５は算出された各リンク長さ（Ｉ_{ｆｉｎａｌ}）に対応して図示しないモータの出力軸を回転させるための例えば電圧値でなる駆動信号を生成すると共に、モータドライバ34へ出力する。これによりモータドライバ34は、駆動信号に基づいて、各直動リンク12のモータを駆動させることにより、アウタチューブに対しインナーロッドを進退させ、直動リンク12を伸縮させる。

直動リンク12と腰部３または足底部９との連結部分に配設されているベース部側受動ジョイント13、及び、足部側受動ジョイントは、直動リンク12の伸縮に追従してこれを妨げることなく円滑に従動する。直動リンク12は、モータの駆動が図示しないロータリーエンコーダ等の検出器により検出され、取得された検出値は角度信号としてフィードバックされることにより、フィードバック制御される。これにより、右足ユニット６は、一歩踏み出す動作や、その場で足踏みする動作等を行うことができる。さらに、このような右足ユニット６と同様の動作を左足ユニット７に行わせ、交互に連続して行わせることにより、歩行動作を行うことができる。

また、本発明に係る制御装置では、腰部補償軌道算出手段28、設定ＺＭＰ軌道変更量算出手段29、及び、着地位置変更量算出手段30によって算出した着地位置変更量（^ｍΔＸ_ｌｐ）に基づいて、足部軌道生成手段31において足部出力軌道（^ｍＸ_ｆ）を算出することにより、搭乗者用シートに搭乗した搭乗者が発生した外力に対応し得るように構成されている。以下、各手段について詳細に説明する。尚、前記座標変換手段32及び逆運動学演算手段33については、既に公知の技術（例えば、特開２００６−８２１５５号公報参照）であり、特に限定されるものではないので、簡単のため説明を省略する。

（ａ）歩行パターン生成手段
まず、歩行パターン生成手段25について説明する。歩行パターン生成手段25は、足部設定パターン軌道（^ｍＸ_ｐｆ）、腰部設定パターン軌道（^ｍＸ_ｐｗ）、設定ＺＭＰ軌道（^ｍＸ_ｐＺＭＰ）、搭乗者が座面反力計測センサ20周りに発生する理論的な外乱成分としての理論外力（^ｍＦ_ｔｈ）、及び搭乗者が座面反力計測センサ20周りに発生する理論的な外乱成分としての理論モーメント（^ｍＭ_ｔｈ）を算出し、足部設定パターン軌道（^ｍＸ_ｐｆ）に対応した足部設定パターン信号、腰部設定パターン軌道（^ｍＸ_ｐｗ）に対応した腰部設定パターン信号、設定ＺＭＰ軌道（^ｍＸ_ｐＺＭＰ）に対応した設定ＺＭＰ信号、理論外力（^ｍＦ_ｔｈ）に対応した理論外力信号、及び、理論モーメント（^ｍＭ_ｔｈ）に対応した理論モーメント信号を生成し出力する。

尚、ここで、理論的な外乱成分としての理論外力（^ｍＦ_ｔｈ）及び理論モーメント（^ｍＭ_ｔｈ）とは、理想的な搭乗者を想定し、この理想的な搭乗者が二足歩行ロボット１の腰部３周りに発生する外力を計算によって予め設定した理論値をいう。また、理想的な搭乗者とは、この場合、ロボットの揺れに体をまかせてシートに自然に着座している搭乗者のことをいう。

（ｂ）外乱成分抽出手段
次に、外乱成分抽出手段26について説明する。外乱成分抽出手段26は、座面反力計測センサ20で搭乗者が発生する外力を検出た検出値としての実測外力（^ｗｓＦ_{ｗｒｅａｌ}）、及び、上記外力の実測モーメント（^ｗｓＭ_{ｗｒｅａｌ}）を得る。そして、外乱成分抽出手段26は、実測外力（^ｗｓＦ_{ｗｒｅａｌ}）から理論外力（^ｍＦ_ｔｈ）、及び、上記外力の実測モーメント（^ｗｓＭ_{ｗｒｅａｌ}）から理論モーメント（^ｍＭ_ｔｈ）をそれぞれ差し引いて、搭乗者が発生する実質外力（^ｍＦ_ｅｘ）、及び搭乗者が発生する実質モーメント（^ｍＭ_ｅｘ）を算出する。このようにして、搭乗者が発生した外力から、理想的な搭乗者が二足歩行ロボット１の腰部３周りに発生する外力を減算することにより、想定外の外力としての実質外力（^ｍＦ_ｅｘ）、及び実質モーメント（^ｍＭ_ｅｘ）を抽出する。そして、この実質外力（^ｍＦ_ｅｘ）、及び実質モーメント（^ｍＭ_ｅｘ）に基づいて腰部補償軌道（^ｍＸ_ｗ）を算出することにより、全体としての制御量を減らすことができる。さらに、外乱成分抽出手段26は、外乱成分としての実質外力（^ｍＦ_ｅｘ）に対応した実質外力信号、及び、外乱成分としての実質モーメント（^ｍＭ_ｅｘ）に対応した実質モーメント信号を生成し、腰部補償軌道算出手段28に出力する。

（ｃ）実測ＺＭＰ算出手段
次に、実測ＺＭＰ算出手段27について説明する。実測ＺＭＰ算出手段27は、足部反力計測センサ21で計測した力（^ｗｓＦ_{ｆｒｅａｌ}）、及び足部反力計測センサ21で計測した力のモーメント（^ｗｓＭ_{ｆｒｅａｌ}）から、実測ＺＭＰ軌道（^ｍＸ_ａＺＭＰ）を算出し、実測ＺＭＰ軌道（^ｍＸ_ａＺＭＰ）に対応した実測ＺＭＰ信号を生成し出力する。

（ｄ）腰部補償軌道算出手段
次に、腰部補償軌道算出手段28について説明する。予め、制御装置５は、実測ＺＭＰ軌道（^ｍＸ_ａＺＭＰ）に対応した実測ＺＭＰ信号から、目標ＺＭＰ軌道（^ｍＸ_ｖＺＭＰ）に対応した目標ＺＭＰ信号を差し引いてＺＭＰ偏差信号を得て、ＺＭＰ偏差信号を腰部補償軌道算出手段28に入力する。腰部補償軌道算出手段28は、さらに、実質外力信号、実質モーメント信号、目標ＺＭＰ信号、足部出力信号を受け取り、ＺＭＰ偏差（^ｍＸ_ａＺＭＰ−^ｍＸ_ｖＺＭＰ）、実質外力（^ｍＦ_ｅｘ）、実質モーメント（^ｍＭ_ｅｘ）、足部出力軌道（^ｍＸ_ｆ）、及び目標ＺＭＰ軌道（^ｍＸ_ｖＺＭＰ）から、腰部補償軌道（^ｍＸ_ｗ）を求める。

尚、ここで、目標ＺＭＰ軌道（^ｍＸ_ｖＺＭＰ）とは、設定ＺＭＰ軌道（^ｍＸ_ｐＺＭＰ）から、後述する設定ＺＭＰ軌道変更量（^ｍΔＸ_ＺＭＰ）を加算して得た、変更した後の設定ＺＭＰ軌道をいう。

歩行中、２足歩行ロボット１に外力Ｆが加わった場合（図５（Ａ））、ロボットの転倒を防ぐため、実測ＺＭＰ軌道（^ｍＸ_ａＺＭＰ）を変更した後の設定ＺＭＰ軌道（^ｍＸ_ｖＺＭＰ）に一致させるように腰部３を加速させる必要がある（図５（Ｂ））。この腰部３を加速させる場合の腰部補償軌道（^ｍＸ_ｗ）の算出には、搭乗者も含めた腰部質点が、腰部３の重心の一質点に集中し、２足歩行ロボット１全体として、両足底部９の２点を含む三質点近似モデルを用いる。このモデルのＺＭＰ周りのモーメントのつりあい式は、数１となる。尚、２足歩行ロボット１に対する外力は、座面反力計測センサ20及び足部反力計測センサ21により検知するので、外乱入力点もこの場合、座面反力計測センサ20及び足部反力計測センサ21の配置点のみとする。

この数１を運動座標系について考えると、数２のようになる。尚、運動座標系の原点は、歩行パターンの腰部原点と同じ軌道をとる。

但し、運動座標系の回転は考慮しないものとし、簡単のため、Ｘ軸方向の運動のみ記載することとする。

腰部３の加速度を左辺に、それ以外を右辺にまとめると、数３となる。数３中のＢは、下肢質点が設定ＺＭＰ周りに発生するモーメント、Ｃは外乱が設定ＺＭＰ周りに発生するモーメントを表す。この数３を４次のルンゲクッタ法を用いて解き、次制御周期における腰部補償軌道（^ｍＸ_ｗ）を算出する。

さらに、歩行安定性を向上するため、腰部補償軌道（^ｍＸ_ｗ）を算出する数３に実測ＺＭＰ軌道（^ｍＸ_ａＺＭＰ）のフィードバックを追加することとした。実際には、数４のように修正し、実測ＺＭＰ軌道（^ｍＸ_ａＺＭＰ）から目標ＺＭＰ軌道（^ｍＸ_ｖＺＭＰ）を差し引いたＺＭＰ偏差（^ｍＸ_ａＺＭＰ−^ｍＸ_ｖＺＭＰ）にゲインをかけたものを数３に追加した。

（ｅ）設定ＺＭＰ軌道変更量算出手段
次に、設定ＺＭＰ軌道変更量算出手段29について説明する。設定ＺＭＰ軌道変更量算出手段29は、腰部補償軌道（^ｍＸ_ｗ）に対応した腰部補償信号から、腰部設定パターン軌道（^ｍＸ_ｐｗ）に対応した腰部設定パターン信号を差し引いた偏差信号を受け取り、設定ＺＭＰ軌道変更量（^ｍΔＸ_ＺＭＰ）を求める。

設定ＺＭＰ軌道変更量（^ｍΔＸ_ＺＭＰ）は、数５を用いて求める。変更された設定ＺＭＰ周りに補償軌道を生成することで、腰軌道の発散を遅延又は収束させることができる（図５（Ｃ））。

但し、設定パターンにおける設定ＺＭＰは、支持多角形のほぼ中央に位置するようになっており、設定ＺＭＰの変更量には制限を設けた。本実施形態では、Ｘ方向は０．０４ｍ、Ｙ方向は０．０３ｍを変更量の最大値とした。

（ｆ）着地位置変更量算出手段
次に、着地位置変更量算出手段30について説明する。着地位置変更量算出手段30は、腰部補償軌道（^ｍＸ_ｗ）に対応した腰部補償信号から、腰部設定パターン軌道（^ｍＸ_ｐｗ）に対応した腰部設定パターン信号を差し引いた偏差信号と、設定ＺＭＰ軌道変更量（^ｍΔＸ_ＺＭＰ）に対応した設定ＺＭＰ軌道変更信号とを受け取り、前記偏差信号に対応した偏差と、設定ＺＭＰ軌道変更量（^ｍΔＸ_ＺＭＰ）とにより、着地位置変更量（^ｍΔＸ_ｌｐ）算出し、着地位置変更量（^ｍΔＸ_ｌｐ）に対応した着地位置変更信号を生成し出力する。

着地位置変更量（^ｍΔＸ_ｌｐ）は、設定ＺＭＰ軌道の変更によってもなお腰部軌道を収束できない場合、着地位置を変更することにより、設定ＺＭＰを変更できる範囲を発散していく方向に広げることができる。着地位置変更量（^ｍΔＸ_ｌｐ）の算出には、腰部質点が、腰部３の重心の一質点に集中した一質点近似モデルを用いる。このモデルにおける設定ＺＭＰ周りのモーメントのつり合い式は、数６となる。

また、補償軌道の生成により、時刻Ｔ_０における設定パターンからの腰部偏差を（^ｍΔＸ_ｗ）、設定ＺＭＰ軌道変更量を（^ｍΔＸ_ＺＭＰ）とすると、数７の偏差に関する微分方程式を得る。

この微分方程式は数８のように表すことができる。尚、この解はｔ秒後の腰部偏差を表す。

このようにして数８をＴ_０から数えて三歩目で腰部偏差（位置・速度）が０となるように境界値問題として解き、次の一歩の着地位置変更量を求める。すなわち、着地位置を前方へずらすなどして制御量を収束させる（図５（Ｄ））。

（ｇ）足部軌道生成手段
次に、足部軌道生成手段31について説明する。足部軌道生成手段31は、着地位置変更量（^ｍΔＸ_ｌｐ）に対応した着地位置変更信号と、足部設定パターン軌道（^ｍＸ_ｐｆ）に対応した足部設定パターン信号を受け取り、着地位置変更量（^ｍΔＸ_ｌｐ）と足部設定パターン軌道（^ｍＸ_ｐｆ）とにより、足部出力軌道（^ｍＸ_ｆ）算出し、足部出力軌道（^ｍＸ_ｆ）に対応した足部出力信号を生成し出力する。

足部軌道生成手段31は、着地位置変更量（^ｍΔＸ_ｌｐ）の算出を受けて足部軌道の生成を行う。本実施形態では、Ｚ軸の軌道は設定パターンを用いており、Ｘ軸及びＹ軸についてのみ、毎制御周期５次関数補間を用いて、足部出力軌道（^ｍＸ_ｆ）を算出し、足部出力軌道（^ｍＸ_ｆ）に対応した足部出力信号を生成して座標変換手段32に出力している。尚、上記した式に使用された記号の説明を数９に示す。

２．動作及び作用
次に、外乱補償制御処理手順について説明する。尚、以下の説明において、ｉは、相数であり、ｊは、補間点である。

制御装置５は、動作制御処理を開始すると、図６に示す外乱補償制御手順ＲＴ１を開始する。制御装置５は、この外乱補償制御手順ＲＴ１を開始すると、ステップＳＰ１において、ｉが全相数より小さいか否かを判別する。このステップＳＰ１において肯定結果が得られると、次のステップＳＰ２に移る。ステップＳＰ２において制御コンピュータは、ｊが補間点数より小さいか否かを判別する。このステップＳＰ２において肯定結果が得られると、次のステップＳＰ３に移る。ステップＳＰ３において制御装置５は、設定ＺＭＰ軌道変更量算出処理を実行して、次のステップＳＰ４へ移る。ステップＳＰ４において制御装置５は、外乱成分抽出処理を実行して、次のステップＳＰ５へ移る。ステップＳＰ５において制御装置５は、腰部補償軌道算出処理を実行して、次のステップＳＰ６へ移る。ステップＳＰ６において制御装置５は、着地位置変更量算出処理を実行して、次のステップＳＰ７へ移る。ステップＳＰ７において制御装置５は、足部軌道生成処理を実行して、次のステップＳＰ８へ移る。このステップＳＰ８を終了した段階で、制御装置５は、上記ステップＳＰ３〜ステップＳＰ６までの処理によって算出した制御量としての着地位置変更量（^ｍΔＸ_ｌｐ）を、足部出力軌道（^ｍＸ_ｆ）に反映させることにより、上記動作制御処理にフィードバックさせることができる。

ステップＳＰ８において制御装置５は、ｊを１だけ増加させてステップＳＰ２へ戻る。ステップＳＰ２で否定結果がえられると、このことはステップＳＰ３〜ステップＳＰ８を補間点数繰り返したことを表し、ステップＳＰ９へ移る。ステップ９において制御装置５は、ｉを１だけ増加させてステップＳＰ１へ戻る。制御装置５は、ステップＳＰ１において否定結果が得られると、このことは全相数分だけ外乱補償制御処理を終了したことを表している。従って、制御装置５はステップＳＰ１０に移って、かかる外乱補償制御手順ＲＴ１を終了する。

制御装置５は、歩行中に２足歩行ロボット１に外乱が加わった場合、転倒を防止するため、腰部補償軌道算出手段28により算出した腰部補償軌道変更量に基づき２足歩行ロボット１の腰部３を加速させる。

因みに、腰部補償軌道算出手段28で求まる解は、初期値問題の解となるので、必ず発散解となるので、２足歩行ロボットの制御量は、そのままでは発散してしまうこととなる。

これに対し、本願発明に係る制御装置５は、腰部補償軌道算出手段28に加え、設定ＺＭＰ軌道変更量算出手段29と、着地位置変更量算出手段30とを備えることとした。これにより、制御装置５は、設定ＺＭＰ軌道を変更すると共に、着地位置を変更することにより、腰部３の軌道の発散を遅延又は収束させることができる。従って、制御装置５は、設定ＺＭＰ軌道変更量算出手段29、及び着地位置変更量算出手段30のうちいずれか一方を用いる従来に比べ、大きい外乱に対応でき、安定性を向上することができる。

また、制御装置５は、前記脚部２に生じる足部反力に基いて実測ＺＭＰ軌道を算出する実測ＺＭＰ算出手段27を備え、前記腰部補償軌道算出手段28は、前記実測ＺＭＰ軌道から前記設定ＺＭＰ軌道を変更した目標ＺＭＰ軌道を減じたＺＭＰ偏差に基いて、腰部補償軌道を算出する。これにより、制御装置５は、搭乗者の運動による外乱以外の外乱、すなわち、路面の細かな凹凸やロボット各部のたわみなどのモデル誤差による不安定性に関しても補償できるので、安定性をより向上することができる。

さらに、制御装置５は、前記外力として前記２足歩行ロボット１の前記腰部３における座面反力を用いる。これにより、搭乗者用シート４に着座した搭乗者が発生する外力と、その他の外力とを分離して測定することができるので、フィードバック量を減らすことができ、より安定性を向上することができる。

また、制御装置５は、前記歩行パターン生成手段25は、搭乗者が理論どおりにふるまった場合の理論座面反力を設定する理論座面反力設定手段を有し、前記外力から前記理論座面反力設定手段によって算出した理論座面反力を減じて、外乱成分を抽出する外乱成分抽出手段26を備える。これにより、制御装置５は、さらにフィードバック量を減らすことができる。尚、理論座面反力の算出に用いるモデルには、上記従来技術の欄で説明した搭乗者の受動的力学モデルを用いることにより、より精度の高い理論座面反力を計算することができる。

本発明は、本実施形態に限定されるものではなく、本発明の要旨の範囲内で種々の変形実施が可能である。例えば、上記２足歩行ロボット１は、腰部３に搭乗者用シート４を備えた人間搭乗型の２足方向ロボットについて説明したが、本発明はこれに限らず、搭乗者用シート４を備えていない、例えば、荷物搭載型２足歩行ロボット１に適用することもできる。

本発明は、上述したように搭乗者による外乱を補償する制御を実現することができるので、より安定性の高い人間搭乗型２足歩行ロボット１を提供することができる。これにより階段を昇降することができる２足歩行型車椅子など有用なデバイスを実現できる。

また、本発明の一部は通常の人間型２足歩行ロボット１の安定化にも有効であり、その場合にもより安定的に２足歩行させることができる。

本実施形態に係る２足歩行ロボットを示す全体斜視図である。 同上、（Ａ）正面図、（Ｂ）側面図である。 同上、搭乗者用シートを取り外した状態を示す斜視図である。 同上、制御装置のシステム構成を示すブロック図である。 同上、２足歩行ロボットの動作を段階的に示す図であり、（Ａ）外力が加わった状態、（Ｂ）腰部補償軌道を変更して腰部を加速させた状態、（Ｂ）設定ＺＭＰ軌道を変更した状態、（Ｄ）着地位置を変更した状態を示す図である。 同上、外乱補償制御の処理手順を示すフローチャートである。

符号の説明

１ ２足歩行ロボット
２ 脚部
３ 腰部
４ 搭乗者用シート
５ 制御装置
９ 足底部（足部）
20 座面反力計測センサ
21 足部反力計測センサ
25 歩行パターン制御手段
26 外乱成分抽出手段
27 実測ＺＭＰ算出手段
28 腰部補償軌道算出手段
29 設定ＺＭＰ軌道変更量算出手段
30 着地位置変更量算出手段
31 足部軌道生成手段

Claims (12)

1. 足部を有する脚部、及び腰部を有する２足歩行ロボットの制御装置において、
   前記足部の軌道と、前記腰部の軌道とを設定すると共に、前記足部の軌道と前記腰部の軌道とに基いて設定ＺＭＰ軌道を設定する歩行パターン生成手段と、
   歩行中の前記２足歩行ロボットに加わった外力に応じて前記腰部を加速させるための腰部補償軌道を算出する腰部補償軌道算出手段と、
   所定の範囲内において前記設定ＺＭＰ軌道を変更する設定ＺＭＰ軌道の変更量を算出する設定ＺＭＰ軌道変更量算出手段と、
   前記脚部が着地する位置の変更量を算出する着地位置変更量算出手段と
   を備えることを特徴とする制御装置。
2. 前記足部に生じる足部反力に基づいて実測ＺＭＰ軌道を算出する実測ＺＭＰ算出手段を備え、
   前記腰部補償軌道算出手段は、前記設定ＺＭＰ軌道を変更した目標ＺＭＰ軌道を、前記実測ＺＭＰ軌道から減じたＺＭＰ偏差に基いて、前記腰部補償軌道を算出する
   ことを特徴とする請求項１記載の制御装置。
3. 搭乗者が発生した力を前記腰部における座面反力として計測し、
   前記外力は、前記座面反力を用いることを特徴とする請求項１又は２記載の制御装置。
4. 前記歩行パターン生成手段は、理想的な搭乗者が前記腰部周りに発生する理論座面反力を設定し、
   前記座面反力から前記理論座面反力を減じて、外乱成分を抽出する外乱成分抽出手段
   を備え、
   前記外乱成分に基づいて前記腰部補償軌道を算出する
   ことを特徴とすることを特徴とする請求項３記載の制御装置。
5. 足部を有する脚部、及び腰部を有する２足歩行ロボットの制御方法において、
   前記足部の軌道と、前記腰部の軌道とを設定すると共に、前記足部の軌道と前記腰部の軌道とに基いて設定ＺＭＰ軌道を設定する歩行パターン生成ステップと、
   歩行中の前記２足歩行ロボットに加わった外力に応じて前記腰部を加速させるための腰部補償軌道を算出する腰部補償軌道算出ステップと、
   所定の範囲内において前記設定ＺＭＰ軌道を変更する設定ＺＭＰ軌道の変更量を算出する設定ＺＭＰ軌道変更量算出ステップと、
   前記脚部が着地する位置の変更量を算出する着地位置変更量算出ステップと
   を備えることを特徴とする制御方法。
6. 前記足部に生じる足部反力に基いて実測ＺＭＰ軌道を算出する実測ＺＭＰ算出ステップを備え、
   前記腰部補償軌道算出ステップは、前記設定ＺＭＰ軌道を変更した目標ＺＭＰ軌道を、前記実測ＺＭＰ軌道から減じたＺＭＰ偏差に基いて、腰部補償軌道を算出する
   ことを特徴とする請求項５記載の制御方法。
7. 搭乗者が発生した力を前記腰部における座面反力として計測し、
   前記外力は、前記座面反力を用いることを特徴とする請求項５又は６記載の制御方法。
8. 前記歩行パターン生成ステップは、理想的な搭乗者が前記腰部周りに発生する理論座面反力を設定する理論座面反力設定ステップを有し、
   前記座面反力から前記理論座面反力を減じて、外乱成分を抽出する外乱成分抽出ステップを備え、
   前記外乱成分に基づいて前記腰部補償軌道を算出する
   ことを特徴とする請求項７記載の制御方法。
9. 足部を有する脚部、腰部、及び制御装置を有する２足歩行ロボットにおいて、
   前記制御装置は、
   前記足部の軌道と、前記腰部の軌道とを設定すると共に、前記足部の軌道と前記腰部の軌道とに基いて設定ＺＭＰ軌道を設定する歩行パターン生成手段と、
   歩行中の前記２足歩行ロボットに加わった外力に応じて前記腰部を加速させるための腰部補償軌道を算出する腰部補償軌道算出手段と、
   所定の範囲内において前記設定ＺＭＰ軌道を変更する設定ＺＭＰ軌道の変更量を算出する設定ＺＭＰ軌道変更量算出手段と、
   前記脚部が着地する位置の変更量を算出する着地位置変更量算出手段と
   を備えることを特徴とする２足歩行ロボット。
10. 前記足部に足部反力センサを設け、
    前記足部反力センサによって計測した足部反力に基いて実測ＺＭＰを算出する実測ＺＭＰ算出手段を備え、
    前記腰部補償軌道算出手段は、前記設定ＺＭＰ軌道を変更した目標ＺＭＰを、前記実測ＺＭＰから減じたＺＭＰ偏差に基いて、腰部補償軌道を算出する
    ことを特徴とする請求項９記載の２足歩行ロボット。
11. 前記腰部の上部に搭乗者用シートを有し、
    前記搭乗者用シートと前記腰部との間に座面反力計測センサを設け、
    前記座面反力計測センサにより前記外力を計測する
    ことを特徴とする請求項９又は１０記載の２足歩行ロボット。
12. 前記歩行パターン生成手段は、理想的な搭乗者が前記腰部周りに発生する理論座面反力を設定し、
    前記座面反力から前記理論座面反力を減じて、外乱成分を抽出する外乱成分の抽出手段を備え、
    前記外乱成分に基づいて前記腰部補償軌道を算出する
    ことを特徴とする請求項１１記載の２足歩行ロボット。