JP2001328083A - 脚式移動ロボットの床形状推定装置 - Google Patents

Abstract

(57)【要約】 【課題】 脚式移動ロボット、特に２足歩行ロボットが
接地する床の形状を精度良く推定する。さらに、推定精
度が低下する恐れがある状況では床形状の推定を中断す
る。 【解決手段】 床形状推定器１３０において、少なくと
も全床反力モーメント（補償全床反力モーメントＭｄｍ
ｄ）の制御偏差に基づいて推定両脚間床傾斜偏差θｆｄ
ｂｅｓｔｍｖを推定すると共に、少なくとも目標各足平
床反力中心点まわりの各足平床反力モーメントの制御偏
差に基づいて推定各足平床傾斜偏差θｆｎｅｓｔｍｘ，
ｙを推定する。さらに、推定中断器（両脚間床傾斜偏差
推定中断器１４０、足平床傾斜偏差推定中断器１４６）
において、少なくともロボットの歩行時期に基づき、推
定精度が低下しそうな状況か否か判断し、推定精度が低
下する恐れがある状況にあると判断するときは床形状の
推定を中断する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は脚式移動ロボット
の床形状推定装置、より具体的には脚式移動ロボットが
接地する床の傾斜などの形状を推定すると共に、推定精
度が低下する恐れがある状況では床形状の推定を中断す
る装置に関し、さらには推定した床形状に基づいて床反
力を制御することを可能とする装置に関する。

【０００２】

【従来の技術】床形状を推定する技術としては、本出願
人が先に特開平６−３１６５８号公報で提案した技術、
および論文「路面形状に偏差のある環境に対する適応能
力を持つ２足歩行ロボットの開発」日本ロボット学会
誌、第１４巻第４号、１９９６年５月号）記載の技術が
知られている。

【０００３】以下に後者（ロボット学会論文記載技術）
の概要を説明し、その問題点を指摘する。

【０００４】この従来技術では、足平部が、足平上面
部、クッション部、足底の上下３層構造となっていて、
足平上面部と足底の間の相対角度と間隔を検出するため
にポテンショメータを足平の４隅に備えると共に、足平
上面部の絶対傾斜を検出するための傾斜計を備える。ま
た、足底の四隅にはスパイクを備える。尚、この明細書
において『足平』は足部を意味するものとして使用す
る。

【０００５】この従来技術にあっては、遊脚の足平の着
地時に、ポテンショメータと傾斜計の情報を基に、なら
い動作によって足平を床面にならわせようとする。その
結果、足底の四隅のスパイクが床に接地した状態にな
り、足底は床面に平行になる。このとき、傾斜計により
得られる足平上面部の絶対位置姿勢と、ポテンショメー
タから得られる足平上面部と足底の間の相対角度と間隔
を基に、幾何学的演算により床の高さと傾きが検出され
る。

【０００６】

【発明が解決しようとする課題】しかしながら、この従
来技術には以下のような問題点があった。即ち、ならい
動作によって足平が床面にならうまで、床反力を操作す
る姿勢安定化制御を行うことができなかった。またグリ
ップを増すために足底にゴム等の柔らかい材料を貼る
と、推定誤差が増えてしまう不都合があった。

【０００７】さらに、足平がべたに着地しない（即ち、
足底が着地の瞬間に床に面接触しない）歩行では、両脚
支持期の間に床の高さを正確に推定することができなか
った。また、直立時など両足平が床にべたに接地してい
る状態では、各足平の接地面傾斜と両足平接地面間の高
低差を同時には推定することができなかった。また、接
地の瞬間しか推定することができなかったので、着地し
た後に床が変形したときに変形後の床形状を推定するこ
とができなかった。

【０００８】他方、前記した従来技術の前者、即ち、特
開平６−３１６５８号公報記載技術の場合、床反力を操
作する姿勢安定化制御を行いながら、床の傾斜を推定す
ることは可能であったが、両脚支持期では両足平の接地
面と交差または接する平面の傾きだけを推定すると共
に、片脚支持期では支持脚の接地面の傾斜だけを推定す
るに止まっていた。即ち、これらの傾きを同時に推定す
ることはできなかった。

【０００９】

【発明が解決しようとする課題】従って、この発明の目
的は、従来技術では困難であった床形状の推定、具体的
には各足平の接地面傾斜と両足平接地面間の高低差を同
時に、換言すれば複合的に、精度良く推定すると共に、
推定精度が低下する恐れがあるときは、床形状の推定を
中断するようにした脚式移動ロボットの床形状推定装置
を提供することにある。

【００１０】

【課題を解決するための手段】上記の目的を達成するた
めに、請求項１項にあっては、少なくとも上体と、前記
上体に第１の関節を介して連結されると共に、その先端
に第２の関節を介して連結される足部を備えた複数本の
脚部を備えると共に、前記足部に接地する床面から作用
する床反力に応じて変形するコンプライアンス機構を備
え、少なくとも前記上体に対する前記足部の実位置姿勢
を検出し、前記足部の目標位置姿勢に追従させるように
前記関節を駆動する関節駆動手段を備えた脚式移動ロボ
ットの床形状推定装置において、前記上体の鉛直軸方向
に対する傾斜を検出する傾斜検出手段、前記足部の実位
置姿勢および／または前記足部の目標位置姿勢と前記検
出された傾斜に基づき、前記足部が接地する床面を含む
歩行環境における前記足部の位置姿勢を算出する対歩行
環境足部位置姿勢算出手段、前記床面から作用する床反
力による前記コンプライアンス機構の変形が加えられた
前記歩行環境における足部の変形位置姿勢を、前記コン
プライアンス機構をモデル化して得た機構コンプライア
ンスモデルを用いて算出する変形足部位置姿勢算出手
段、および前記算出された足部の変形位置姿勢に基づ
き、前記足部が接地するそれぞれの床面の形状およびそ
れらの相対位置関係を示す床形状推定値を算出する床形
状推定値算出手段、を備えると共に、前記床形状推定値
算出手段は、前記床形状推定値の算出が中断すべき状況
にあるか否か判定し、中断すべき状況にないと判定する
ときは前記床形状推定値の算出を続行する推定中断手
段、を備える如く構成した。

【００１１】これにより、従来技術では困難であった床
形状の推定、具体的には各足平の接地面傾斜と両足平接
地面間の高低差を同時に、換言すれば複合的に、精度良
く推定することができると共に、推定精度が低下する恐
れがある状況では床形状推定を中断するので、推定値が
不適正なものとなることがない。

【００１２】請求項２項にあっては、前記推定中断手段
は、中断すべき状況にあると判定するときは、前記床形
状推定値を保持するか、あるいは所定の値に設定する如
く構成した。

【００１３】これにより、推定精度が低下する恐れがあ
る状況においても、各足平の接地面傾斜と両足平接地面
間の高低差を同時に、換言すれば複合的に、精度良く推
定することができる。

【００１４】請求項３項にあっては、前記推定中断手段
は、少なくとも前記ロボットの歩行時期に基づいて中断
すべき状況にあるか否か判定する如く構成した。

【００１５】これにより、より的確に中断すべき状況に
あるか否かを判定することができ、よって一層推定値が
不適正なものとなることがない。

【００１６】請求項４項にあっては、前記推定中断手段
は、少なくとも前記足部と床面との接触状況に基づいて
中断すべき状況にあるか否か判定する如く構成した。

【００１７】これにより、より的確に中断すべき状況に
あるか否かを判定することができ、よって一層推定値が
不適正なものとなることがない。

【００１８】請求項５項にあっては、前記推定中断手段
は、少なくとも前記足部に作用する前記床反力に基づい
て中断すべき状況にあるか否か判定する如く構成した。

【００１９】これにより、より的確に中断すべき状況に
あるか否かを判定することができ、よって一層推定値が
不適正なものとなることがない。

【００２０】請求項６項にあっては、前記推定中断手段
は、少なくとも前記足部に作用する前記床反力の中心点
位置に基づいて前記中断すべき状況にあるか否か判定す
る如く構成した。

【００２１】これにより、より的確に中断すべき状況に
あるか否かを判定することができ、よって一層推定値が
不適正なものとなることがない。

【００２２】請求項７項にあっては、さらに、少なくと
も前記算出された床形状推定値に基づいて前記足部の目
標位置姿勢を修正する修正手段、を備えると共に、前記
推定中断器は、前記床形状推定値が発散する恐れがある
ときは、中断すべき状況にあると判定し、前記床形状推
定値の算出を中断する如く構成した。

【００２３】これにより、前記した効果に加え、複合コ
ンプライアンス制御で除去しきれなかった実床反力の制
御目標値からの定常偏差を可能な限り零に近づける、換
言すれば床形状のずれに起因する床反力の定常偏差を解
消することができる。

【００２４】

【発明の実施の形態】以下、添付図面を参照してこの発
明の一つの実施の形態に係る脚式移動ロボットの床形状
推定装置を説明する。尚、脚式移動ロボットとしては２
足歩行ロボットを例にとる。

【００２５】図１は、この実施の形態に係る床形状推定
装置を適用した脚式移動ロボット、より具体的には２足
歩行ロボットを全体的に示す概略図である。

【００２６】図示の如く、２足歩行ロボット（以下「ロ
ボット」という）１は左右それぞれの脚部（脚部リン
ク）２に６個の関節を備える（理解の便宜のために各関
節をそれを駆動する電動モータで示す）。６個の関節は
上から順に、股（腰部）の脚部回旋用の関節１０Ｒ，１
０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰
部）のロール方向（Ｘ軸まわり）の関節１２Ｒ，１２
Ｌ、股（腰部）のピッチ方向（Ｙ軸まわり）の関節１４
Ｒ，１４Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、
足首のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向
の関節２０Ｒ，２０Ｌから構成される。

【００２７】関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部に
は足平（足部）２２Ｒ，２２Ｌが取着されると共に、最
上位には上体（基体）２４が設けられ、その内部にマイ
クロコンピュータからなる制御ユニット２６（後述）な
どが格納される。上記において股関節（あるいは腰関
節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）
から、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ
（Ｌ）から構成される。また股関節と膝関節とは大腿リ
ンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３
０Ｒ，３０Ｌで連結される。

【００２８】上記の構成により、脚部２は左右の足につ
いてそれぞれ６つの自由度を与えられ、歩行中にこれら
の６＊２＝１２個の関節を適宜な角度で駆動すること
で、足全体に所望の動きを与えることができ、任意に３
次元空間を歩行させることができる（この明細書で
「＊」はスカラに対する演算としては乗算を、ベクトル
に対する演算としては外積を示す）。

【００２９】尚、この明細書で後述する上体２４の位置
およびその速度は、上体２４の所定位置、具体的には上
体２４の重心位置などの代表点の位置およびその移動速
度を意味する。

【００３０】図１に示す如く、足関節の下方には公知の
６軸力センサ３４が取着され、力の３方向成分Ｆｘ，Ｆ
ｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚと
を測定し、足部の着地の有無および床反力（接地荷重）
などを検出する。また、上体２４には傾斜センサ３６が
設置され、Ｚ軸（鉛直方向（重力方向））に対する傾き
とその角速度を検出する。また各関節の電動モータに
は、その回転量を検出するロータリエンコーダが設けら
れる。

【００３１】図２に示すように、足平２２Ｒ（Ｌ）の上
方には、ばね機構３８が装備されると共に、足底にはゴ
ムなどからなる足底弾性体４０が貼られてコンプライア
ンス機構４２を構成する。ばね機構３８は具体的には、
足平２２Ｒ（Ｌ）に取り付けられた方形状のガイド部材
（図示せず）と、足首関節１８Ｒ（Ｌ）および６軸力セ
ンサ３４側に取り付けられ、前記ガイド部材に弾性材を
介して微動自在に収納されるピストン状部材（図示せ
ず）とからなる。

【００３２】図中に実線で表示された足平２２Ｒ（Ｌ）
は、床反力を受けていないときの状態を示す。床反力を
受けると、コンプライアンス機構４２においてバネ機構
３８と足底弾性体４０がたわみ、足平は図中に点線で表
示された位置姿勢に移る。この構造は、着地衝撃を緩和
するためだけでなく、制御性を高めるためにも重要なも
のである。尚、その詳細は前記した特開平５−３０５５
８４号に記載されているので、詳細な説明は省略する。

【００３３】さらに、図１では図示を省略するが、ロボ
ット１の適宜な位置にはジョイスティック４４が設けら
れ、外部から必要に応じて直進歩行しているロボット１
を旋回させるなど歩容に対する要求を入力できるように
構成される。

【００３４】図３は制御ユニット２６の詳細を示すブロ
ック図であり、マイクロ・コンピュータから構成され
る。そこにおいて傾斜センサ３６などの出力はＡ／Ｄ変
換器５０でデジタル値に変換され、その出力はバス５２
を介してＲＡＭ５４に送られる。また各電動モータに隣
接して配置されるエンコーダの出力はカウンタ５６を介
してＲＡＭ５４内に入力される。

【００３５】制御ユニット２６の内部にはＣＰＵからな
る第１、第２の演算装置６０，６２が設けられており、
第１の演算装置６０は後述の如く、ロボット１が接地す
る床（床面）の形状を推定すると共に、ＲＯＭ６４に格
納されている歩容に基づいて後述の如く関節角変位指令
を算出し、ＲＡＭ５４に送出する。また第２の演算装置
６２はＲＡＭ５４からその指令と検出された実測値とを
読み出し、各関節の駆動に必要な制御値を算出してＤ／
Ａ変換器６６とサーボアンプを介して各関節を駆動する
電動モータに出力する。

【００３６】この実施の形態に係る脚式移動ロボットの
床形状推定装置は、概説すると、本出願人が先に提案し
た前記の特開平６−３１６５８号公報記載の技術の改良
である。より具体的には、同様に本出願人が先に提案し
た前記の特開平１０−２７７９６９号公報記載技術を基
礎とし、その上に特開平６−３１６５８号記載技術の手
法を拡張しながら適応させたものである。

【００３７】即ち、その特開平６−３１６５８号記載技
術においては、少なくともＺＭＰまわりの全床反力モー
メントの制御偏差を基に、両脚支持期では両脚間傾斜偏
差を推定して両脚補償角を修正し、片脚支持期では各足
平床傾斜偏差を推定して各足平補償角を修正していた
（特開平６−３１６５８号公報記載技術では足首角を修
正していたので、厳密には足平補償角を修正することと
は異なるが、ここではほぼ同じと考える）。

【００３８】尚、上記で、ＺＭＰ（Zero Moment Point)
とは、運動パターン（軌道）によって発生する慣性力と
重力の合力の床（面）上の作用点まわりのモーメント
が、鉛直軸まわりの成分を除き、０である点を意味す
る。

【００３９】それに対し、この実施の形態に係る脚式移
動ロボットの床形状推定装置においては、少なくとも全
床反力モーメントの制御偏差（あるいはそれから目標各
足平床反力中心点まわりの各足平床反力モーメントの制
御偏差を減じたもの）を基に床形状、より具体的には両
脚間床傾斜偏差を推定する。さらには、必要に応じて推
定値に基づいて両脚補償角を修正する。また、少なくと
も目標各足平床反力中心点まわりの各足平床反力モーメ
ントの制御偏差を基に床形状、より具体的には各足平床
傾斜偏差を推定する。さらには、必要に応じて推定値に
基づいて各足平補償角を修正するようにした。この結
果、両脚支持期であっても各足平床傾斜偏差を精度良く
推定することができ、各足平補償角を修正することがで
きる。

【００４０】さらに、両脚間床傾斜偏差および各足平床
傾斜偏差の推定において、前記したコンプライアンス機
構４２をモデル化した、床反力と脚部（脚部リンク）２
の変形の関係を表わす機構コンプライアンスモデルを用
いることにより、制御則に単なる積分を加えて定常制御
偏差を低減する従来手法では得られない良好な制御特性
が得られるようにした。即ち、制御則に単なる積分を加
えると位相が遅れるが、この実施の形態に係る装置では
オブザーバを用いた場合と同様に、制御の遅れが生じる
ことがない。

【００４１】ところで、その先に提案した前記特開平１
０−２７７９６９号公報記載技術は、予期しない凹凸形
状の床面に遭遇して姿勢の安定を失ったとき、床反力を
適切に制御して安定した姿勢を回復させることを意図し
ていた。

【００４２】即ち、平坦な床面を想定して生成された歩
容に基づいて歩行制御されていたロボット１が予期しな
い凹凸を踏むと、過大な床反力が生じてロボット１は傾
くが、そのような状況に対処すべく、本出願人は別の提
案技術（特開平５−３０５５８６号公報記載技術）にお
いて前記したコンプライアンス機構４２を用いると共
に、傾斜センサにより上体傾斜を検出して必要な復元モ
ーメント要求量を求め、それに目標全床反力中心点（目
標ＺＭＰ）まわりの実全床反力モーメント成分が一致す
るように足平軌道（パターン）を修正して各足平を上下
させていた。

【００４３】しかしながら、その制御は、長い距離でゆ
ったりと変化する大域（広域）的な傾斜やうねりには有
効であるが、局所的な傾きや段差には十分対応しきれな
かった。

【００４４】本出願人は、さらに、別の提案技術（特開
平５−３０５５８４号公報記載技術）において、同様に
前記したコンプライアンス機構４２を用いて予期しなか
った床反力モーメントを打ち消す方向に足関節１８Ｒ
（Ｌ），２０Ｒ（Ｌ）を駆動する制御も提案していた。

【００４５】従って、予期しない凹凸形状の床面に遭遇
して姿勢の安定を失ったときには、上記した２種の提案
技術を併用することも考えられるが、そうすると、制御
が干渉する不都合があった。

【００４６】そのような制御の干渉を回避すべく、先に
提案した前記特開平１０−２７７９６９号公報記載技術
では、局所的な凹凸や傾斜などを含む予期しない床形状
の変化に起因して姿勢の安定を失ったとき、床反力を適
切に制御するようにしている。

【００４７】この実施の形態に係る脚式移動ロボットの
床形状推定装置は、その先に提案した前記特開平１０−
２７７９６９号公報記載技術を前提としているので、理
解の便宜上、最初に、その先に提案した前記特開平１０
−２７７９６９号公報記載技術を説明する。尚、以下、
その先に提案した前記特開平１０−２７７９６９号公報
記載技術を「複合コンプライアンス制御」と呼ぶ。

【００４８】図４は、その複合コンプライアンス制御の
装置の構成および動作を機能的に示すブロック図であ
る。

【００４９】同図の説明に入る前に、その複合コンプラ
イアンス制御で使用した定義を説明する（尚、定義しな
い用語に関しては、本出願人が前記した技術とは別に提
案した出願（特開平１０−８６０８１号）で使用した定
義に従う）。

【００５０】『歩容』は、ロボット工学における一般的
な定義と異なり、目標運動パターンと床反力パターンを
合わせたものを指称する意味で使用する。但し、床反力
パターンとしては、例えば『ＺＭＰ軌跡だけ』というよ
うに、部分情報であっても良い。そのため、目標運動パ
ターンだけを出力して床反力パターンに関する情報を出
力しない装置に対して「歩容生成装置」と言う言葉を用
いない。

【００５１】各脚には、通し番号をつける。第ｎ脚に作
用する床反力を第ｎ足平床反力という（ｎ：１または
２。以下同じ）。全脚に作用する床反力を合成したもの
を全床反力という（ロボット工学では一般的には床反力
と呼ばれるが、足平床反力と区別するためにここでは
『全床反力』という）。

【００５２】足平床反力は、作用点と、そこにかかる力
と力のモーメントによって表現され、同一の足平床反力
に対して、表現の組み合わせは無限通りある。その中に
は、鉛直軸まわりの成分を除くモーメント成分が０でか
つ作用点が足底上にある表記が存在する。この表現にお
ける作用点を、ここでは足平床反力中心点という（本出
願人が別途提案した後述する特開平６−７９６５７号で
は『接地圧重心点』と称した）。

【００５３】同様に、全床反力は、作用点と、そこにか
かる力と力のモーメントによって表現され、同一の全床
反力に対して表現の組み合わせは無限通りある。その中
には、鉛直軸まわりの成分を除くモーメント成分が０で
かつ作用点が床面上にある表現が存在する。この表現に
おける作用点を、ここでは全床反力中心点という。尚、
この明細書で「床面」は、本出願人が先に提案した特開
平５−３１８３４０号公報に記載される仮想的な床面で
あっても良い。

【００５４】全床反力の目標値を目標全床反力という。
目標全床反力は、通常、目標運動パターンに対して動力
学的に平衡する全床反力である。従って、通常、目標全
床反力中心点は、目標ＺＭＰに一致する。

【００５５】尚、前記したＺＭＰの目標値を目標ＺＭＰ
という。目標ＺＭＰは、合力の垂直方向力成分が０でな
い限り、一義的に求められる。以下の説明では、理解し
やすくするために、目標ＺＭＰという言葉を用いる場合
もあるが、厳密には目標全床反力中心点と言うべき箇所
が多い。

【００５６】各足平床反力の目標値を目標各足平床反力
という。但し、目標全床反力とは異なり、目標運動パタ
ーンが決まっていても目標各足平床反力は一義的には決
定されない。実際のロボットに作用する全床反力を実全
床反力という。実際のロボットに作用する各足平床反力
を実各足平床反力という。

【００５７】上記した定義を前提とし、以下、図４を参
照して複合コンプライアンス制御装置の全体構成を概説
する。

【００５８】図示の如く、複合コンプライアンス制御装
置は歩容生成器１００を備え、歩容生成器１００は目標
歩容を生成し、出力する。目標歩容は、前述の定義の通
り、目標運動パターンと目標床反力パターン、より具体
的には目標上***置姿勢軌道、目標足平位置姿勢軌道、
目標全床反力中心点（目標ＺＭＰ）軌道および目標全床
反力軌道（あるいはパターン）からなる。目標床反力パ
ターンは、このように、目標全床反力中心点軌跡を含む
（後述する機構変形補償を行わないならば、目標床反力
パターンとしては目標全床反力中心点軌跡だけでも良
い）。

【００５９】歩容生成器１００が出力する目標全床反力
は、目標運動パターンに対して動力学的に平衡する全床
反力である。従って、目標全床反力中心点は、目標ＺＭ
Ｐに一致する。

【００６０】図５にロボット１が平地を歩行するときの
目標運動パターンの一例を示す。これに対応する目標Ｚ
ＭＰ軌道の床面上軌跡を図６に、タイム・チャートを図
７に示す。この歩容の期間に床に接触したままの足平を
「第１足平」、もう一方を「第２足平」と呼ぶ。

【００６１】図４の説明に戻ると、この装置は目標床反
力分配器１０２を備え、目標床反力分配器１０２は、目
標全床反力中心点（目標ＺＭＰ）と目標足平位置姿勢を
主な入力とし、目標各足平床反力中心点を決定して出力
する。実際には、歩容生成器１００から歩容のパラメー
タ（例えば、両脚支持期の時間や遊脚足平の目標着地位
置など）や、歩容の時期・時刻（例えば、現在時刻が両
脚支持期の初めから0.1secであるなど）などの情報も必
要に応じて取り込む。

【００６２】図５に示すような歩容に対して、目標床反
力分配器１０２は、目標各足平床反力中心点が以下の条
件を満足するように設定する。 条件１）目標各足平床反力中心点軌跡は連続である。 条件２）両脚支持期では、目標第１足平床反力中心点は
かかとに、目標第２足平床反力中心点はつまさきに存在
する。 条件３）このとき目標第１足平床反力中心点と目標第２
足平床反力中心点を結ぶ線分上に、目標全床反力中心点
が存在する。 条件４）片脚支持期では、目標第１足平床反力中心点
は、目標全床反力中心点に一致する。 条件５）片脚支持期の間に、目標第２足平床反力中心点
は、つまさきからかかとに移動する。

【００６３】これら条件を満足する目標第１足平床反力
中心点軌跡のタイム・チャートを図８に、目標第２足平
床反力中心点軌跡のタイム・チャートを図９に示す。
尚、この図では足首（関節１８Ｒ（Ｌ），２０Ｒ
（Ｌ））から足平２２Ｒ（Ｌ）への垂直投影点、より具
体的には足底への垂直投影点を原点とし、図１に示すよ
うに足平前方向をＸ軸の正の向き、足平左方向をＹ軸の
正の向きにとる。

【００６４】目標床反力分配器１０２は、さらに、付随
的ではあるが、目標各足平床反力も決定して出力する。
目標各足平床反力は、コンプライアンス機構４２のたわ
み補償のために必要である。

【００６５】次式を用いて上記のように設定された目標
各足平床反力中心点に対応する目標各足平床反力を決定
すれば、目標各足平床反力の合力は目標全床反力に一致
しなければならないと言う条件を満足する。

【００６６】 目標第１足平床反力＝目標全床反力＊（目標第２足平床反力中心点と目標ＺＭ Ｐの距離）／（目標第１足平床反力中心点と目標第２足平床反力中心点の距離） 目標第２足平床反力＝目標全床反力＊（目標第１足平床反力中心点と目標ＺＭ Ｐの距離）／（目標第１足平床反力中心点と目標第２足平床反力中心点の距離） ・・・式１

【００６７】このように求めた目標各足平床反力は連続
的に変化するので、衝撃の少ない歩行を実現するために
適している。尚、上記の詳細は本出願人が別途提案した
技術（特開平６−７９６５７号）に記述されている。

【００６８】図４の説明に戻ると、この装置は姿勢安定
化制御演算部１０４を備え、姿勢安定化制御演算部１０
４はセンサ情報に基づいてロボット１の状態を推定し、
補償全床反力を算出する。即ち、実際にロボット１が歩
行あるいは直立しているときなどには後述する変位コン
トローラによって実関節変位を目標関節変位に完全に追
従させることができたとしても、ロボット１の位置姿勢
は必ずしも望ましい位置姿勢にならない。

【００６９】ロボット１の姿勢を長期的に安定化させる
ためには、ロボットを望ましい位置姿勢に復元させるた
めに必要な力とモーメントを求め、これを目標全床反力
中心点（目標ＺＭＰ）を作用点として付加的に発生させ
る必要がある。この付加的な力とモーメントを補償全床
反力という。また、補償全床反力のモーメント成分を補
償全床反力モーメントという。

【００７０】尚、脚式移動ロボットの目標歩容が床反力
以外の反力を環境から受けるように想定し、それを例え
ば、目標対象物反力と称し、先に述べた目標ＺＭＰの定
義を次のように拡張しても良い。即ち、目標運動パター
ンによって発生する慣性力と重力と目標対象物反力の合
力を動力学的に求め、床面上のある点に作用するモーメ
ントが、鉛直軸まわりの成分を除いて零であるならば、
その点を改めて目標ＺＭＰとするようにしても良い。

【００７１】もし、ロボット１が完全剛体であって、変
位コントローラによって実関節変位を目標関節変位に完
全に追従させることができたと仮定すると、コンプライ
アンス機構４２のたわみによって生じるロボット全体の
位置姿勢の摂動的な運動は、以下の６自由度に分解でき
る。

【００７２】モード１）目標全床反力中心点（目標ＺＭ
Ｐ）を中心とした前後軸まわり回転（即ち、左右傾き） モード２）目標全床反力中心点（目標ＺＭＰ）を中心と
した左右軸まわり回転（即ち、前後傾き） モード３）目標全床反力中心点（目標ＺＭＰ）を中心と
した鉛直軸まわり回転（即ち、スピン） モード４）前後平行移動揺れ モード５）左右平行移動揺れ モード６）上下平行移動揺れ

【００７３】この内で、モード４とモード５は、コンプ
ライアンス機構４２が前後左右方向の剪断力を受けてた
わむことによって発生するものである。コンプライアン
ス機構４２の剪断方向の剛性が高いように製作するの
で、この揺れは極めて少なく、歩行に及ぼす悪影響はほ
とんどない。

【００７４】残り４自由度の内、モード３とモード６は
ここでの説明と直接の関連を有しないので、モード１と
モード２に対する制御を説明する。モード１とモード２
に対する制御は、これがないとほとんどの場合ロボット
が転倒するので、重要度は極めて高い。

【００７５】モード１を制御するための操作量は、補償
全床反力の前後軸（Ｘ軸）まわりモーメント成分であ
る。モード２を制御するための操作量は、補償全床反力
の左右軸（Ｙ軸）まわりモーメント成分である。従っ
て、補償全床反力の成分の内、前後軸方向モーメント成
分と左右軸方向モーメント成分だけを求めれば良い。他
の成分は、この実施の形態（および第２の実施の形態）
では用いないので０で良い。

【００７６】尚、以降は次の定義に従う。即ち、補償全
床反力のモーメント成分を「補償全床反力モーメントＭ
ｄｍｄ」（詳しくは目標全床反力中心点（目標ＺＭＰ）
まわりの補償全床反力モーメントＭｄｍｄ）と呼ぶ。図
５および図６に示す如く、ロボットの前方向をＸ軸、左
横方向をＹ軸、上方向をＺ軸にとり、第１足平の足首直
下の床面上の点を原点とした座標系を「支持脚座標系」
と呼び、断らない限り、位置、力およびモーメントはこ
の座標系で表現されるものとする。また、ＭｄｍｄのＸ
成分をＭｄｍｄｘ、Ｙ成分をＭｄｍｄｙ、Ｚ成分をＭｄ
ｍｄｚと記述する。上体２４の傾斜偏差（即ち、実上体
傾斜−目標上体傾斜）θｅｒｒのＸ成分をθｅｒｒｘ，
Ｙ成分をθｅｒｒｙ、これらの時間微分値を（ｄθｅｒ
ｒｘ／ｄｔ），（ｄθｅｒｒｙ／ｄｔ）と記述する。

【００７７】ＭｄｍｄｘおよびＭｄｍｄｙは、例えば次
式の制御則によって決定される。 Mdmdx = - Kthxθerrx - Kwx (dθerrx / dt) Mdmdy = - Kthyθerry - Kwy (dθerry / dt) ・・・式２ ここで、Ｋｔｈｘ，Ｋｔｈｙ，ＫｗｘおよびＫｗｙは、
上体傾斜安定化制御ゲインである。

【００７８】後述する複合コンプライアンス動作決定部
は、目標全床反力と補償全床反力の合力に実全床反力を
一致させようと働く。

【００７９】図４の説明に戻ると、この装置は実各足平
床反力検出器１０８を備え、実各足平床反力検出器１０
８は、６軸力センサ３４によって実各足平床反力（その
合力が実全床反力）を検出する。さらに、関節のエンコ
ーダによって検出される実変位（および／または変位指
令）に基づき、上体２４に固定された座標系に対する各
足平２２Ｒ（Ｌ）の相対位置姿勢を算出し、それによっ
て６軸力センサ３４の検出値を座標変換し、上体に固定
された座標系で表現された実各足平床反力を算出した
後、支持脚座標系に変換する。

【００８０】この装置はロボット幾何学モデル（逆キネ
マティクス演算部）１１０を備え、ロボット幾何学モデ
ル１１０は、上***置姿勢と足平位置姿勢を入力される
と、それらを満足する各関節変位を算出する。この実施
の形態におけるロボット１のような１脚あたりの関節自
由度が６である場合には、各関節変位は一義的に求ま
る。

【００８１】ここでは逆キネマティクスの解の式を直接
的に求めておき、式に上***置姿勢と足平位置姿勢を代
入するだけで各関節変位を得るようにした。即ち、ロボ
ット幾何学モデル１１０は、目標上***置姿勢と複合コ
ンプライアンス動作決定部で修正された修正目標足平位
置姿勢軌道（機構変形補償入り修正目標足平位置姿勢軌
道）を入力し、それらから１２個の関節（１０Ｒ（Ｌ）
など）の関節変位指令（値）を算出する。

【００８２】この装置は変位コントローラ１１２（前記
した第２の演算装置６２に同じ）を備え、変位コントロ
ーラ１１２は、ロボット幾何学モデル（逆キネマティク
ス演算部）１１０で算出された関節変位指令（値）を目
標値としてロボット１の１２個の関節の変位を追従制御
する。

【００８３】この装置は前記した複合コンプライアンス
動作決定部１１４を備え、複合コンプライアンス動作決
定部１１４は以下の２つの要求を満足させようと、目標
足平位置姿勢軌道を修正する。

【００８４】要求１）ロボットの位置姿勢制御のため
に、実全床反力を姿勢安定化制御演算部１０４が出力す
る補償全床反力（モーメントＭｄｍｄ）と目標全床反力
の合力に追従させる。ロボットの姿勢傾きだけを制御し
たい場合には、目標全床反力中心点まわりの実全床反力
水平方向モーメント成分だけを補償全床反力モーメント
Ｍｄｍｄに追従させる。

【００８５】要求２）各足平の接地性を確保するため
に、できるかぎり目標各足平床反力中心点まわりの実各
足平床反力モーメントの絶対値を小さくする。

【００８６】尚、通常は実全床反力を補償全床反力と目
標全床反力の合力に一致させながら目標各足平床反力中
心点まわりの実各足平床反力モーメントを０にすること
が、物理的に不可能な場合が多い。従って、要求１）と
要求２）は完全に両立させることはできず、ある点で妥
協しなくてはならない。

【００８７】上記を前提として図１０フロー・チャート
（構造化フロー・チャート）を参照してこの装置の動作
を説明する。尚、図の左端に該当する処理を行う図４装
置の構成要素を示す。

【００８８】先ずＳ１０において装置を初期化し、Ｓ１
２を経てＳ１４に進み、タイマ割り込みを待機する。タ
イマ割り込みは５０ｍｓごとになされ、即ち、制御周期
は５０ｍｓである

【００８９】続いてＳ１６に進んで歩容の切り替わり
目、即ち、支持脚の切り替わり目か否か判断し、否定さ
れるときはＳ２２に進むと共に、肯定されるときはＳ１
８に進んでタイマｔをイニシャライズし、Ｓ２０に進ん
で目標歩容パラメータを設定する。前記の如く、歩容パ
ラメータは、運動パラメータと床反力パラメータ（ＺＭ
Ｐ軌道パラメータ）から構成される。

【００９０】続いてＳ２２に進み、目標歩容の瞬時値を
決定する。ここで『瞬時値』は制御周期ごとの値を意味
し、目標歩容瞬時値は、目標上***置姿勢、目標各足平
位置姿勢、および目標ＺＭＰ位置から構成される。尚、
ここで『姿勢』はＸ，Ｙ，Ｚ空間における『向き』を意
味する。

【００９１】続いてＳ２４に進んで目標各足平床反力中
心点を求める。これは目標床反力分配器の説明で述べた
ように行う。具体的には、図８および図９に示すよう
に、設定した目標各足平床反力中心点軌跡の現在時刻ｔ
における値を求めることで行う。

【００９２】続いてＳ２６に進んで目標各足平床反力を
求める。これは目標床反力分配器の説明で述べた式１を
用いて目標各足平床反力を演算することで行う。

【００９３】続いてＳ２８に進み、前記した傾斜センサ
３６などの出力から上体２４の傾斜などロボット１の状
態を検出する。

【００９４】続いてＳ３０に進み、ロボット１の状態な
どから姿勢を安定化するための（目標全床反力中心点
（目標ＺＭＰ）まわりの）補償全床反力モーメントＭｄ
ｍｄｘ，Ｍｄｍｄｙを求める。具体的には、前記した式
２に従って補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍ
ｄｙを演算する。

【００９５】続いてＳ３２に進んで実各足平床反力を検
出する。これは前記の如く、６軸力センサ３４の出力か
ら検出する。

【００９６】続いてＳ３４に進み、両脚補償角θｄｂｖ
および各足平補償角θｎｘ（ｙ）を決定する。これは、
前記した複合コンプライアンス動作決定部１１４が行う
作業である。

【００９７】その複合コンプライアンス動作決定部１１
４の作業について説明する。説明の便宜のため、両脚支
持期において図１１に示すように第１足平２２Ｒ（Ｌ）
と第２足平２２Ｌ（Ｒ）に実各足平床反力が作用してい
る状況と仮定する。

【００９８】ここでベクトルＦｎａｃｔは第ｎ足平床反
力の力成分を表す。ベクトルＭｎａｃｔは第ｎ足平床反
力のモーメント成分を表す。ベクトルＭｎａｃｔの向き
は、向きに対して時計回りのモーメントが床から足平に
作用していることを表す。

【００９９】この瞬間の目標全床反力は、図１２に示す
ようになっていると仮定する。ちなみに、目標全床反力
中心点（目標ＺＭＰ）における目標全床反力モーメント
ベクトルＭｓｕｍｒｅｆは垂直である（定義により、目
標ＺＭＰは目標全床反力モーメントの水平方向成分が０
である点であるから）。

【０１００】これを式１に従って目標各足平床反力に分
配すると、図１３に示すようになる。同図において、ベ
クトルＦｎｒｅｆは目標第ｎ足平床反力の力成分を表
す。ベクトルＭｎｒｅｆは目標第ｎ足平床反力のモーメ
ント成分を表す。ベクトルＭｎｒｅｆの向きの表現は、
Ｍｎａｃｔと同様である。

【０１０１】説明のため、上体姿勢が左後ろに倒れそう
な状態を想定する。

【０１０２】前述の姿勢安定化制御演算部１０４では、
ロボット１の上体傾斜偏差検出値θｅｒｒｘ，θｅｒｒ
ｙに基づいて補償全床反力モーメントＭｄｍｄを算出す
る。この実施の形態では鉛直軸（Ｚ軸）まわりのスピン
を制御しないので、補償全床反力モーメントＭｄｍｄの
鉛直軸成分は０である。上***置の揺れも制御しないの
で、補償全床反力の力成分（Ｆｄｍｄ）も０である。こ
の状態に対応する補償全床反力モーメントＭｄｍｄを図
１４に示す。

【０１０３】姿勢を復元させるためには、目標全床反力
中心点（目標ＺＭＰ）まわりの実全床反力モーメントの
水平成分を、目標全床反力モーメントＭｓｕｍｒｅｆと
補償全床反力モーメントＭｄｍｄの和の水平成分に追従
させれば良い。

【０１０４】一方、目標全床反力中心点（目標ＺＭＰ）
では目標全床反力モーメントＭｓｕｍｒｅｆの水平方向
成分は０である。従って、前後左右の姿勢傾きを復元さ
せるためには、目標ＺＭＰまわりの実全床反力モーメン
トの水平成分を、Ｍｄｍｄの水平成分に追従させれば良
い。

【０１０５】複合コンプライアンス動作決定部１１４
は、前記した以下の要求をできる限り満足するように足
平の位置姿勢を修正する。 要求１）ロボットの姿勢傾斜を安定化制御するために、
目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力
モーメントの水平方向（Ｘ，Ｙ軸方向）成分を、補償全
床反力モーメントＭｄｍｄの水平方向成分に追従させ
る。 要求２）各足平の接地性を確保するために、できるかぎ
り目標各足平床反力中心点まわりの実各足平床反力モー
メントの絶対値を小さくする。

【０１０６】但し、前述の通り、要求１）と要求２）
は、完全に両立させることはできず、ある点で妥協しな
くてはならない。

【０１０７】足平の位置姿勢の修正は次のように行う。 １）目標第１足平床反力中心点Ｑ１と目標第２足平床反
力中心点Ｑ２を含み、かつ水平面と垂直な平面の法線ベ
クトルＶを求める。Ｖの大きさは１とする。Ｖを図１５
に示す。

【０１０８】２）目標第１足平床反力中心点Ｑ１の座標
を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法
線ベクトルＶまわりに、ある回転角θｄｂｖだけ回転移
動する。移動した後の点をＱ１’とする。同様に、目標
第２足平床反力中心点Ｑ２の座標を、目標全床反力中心
点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに
回転角θｄｂｖだけ回転移動する。移動した後の点をＱ
２’とする。

【０１０９】この回転角θｄｂｖを両脚補償角という。
始点がＱ１、終点がＱ１’のベクトルをベクトルＱ１Ｑ
１’とする。同様に、始点がＱ２、終点がＱ２’のベク
トルをベクトルＱ２Ｑ２’とする。図１６にＱ１’とＱ
２’を示す。

【０１１０】３）目標第１足平を、姿勢は変えずにベク
トルＱ１Ｑ１’だけ平行移動（ほぼ上下移動）させる。
同様に、目標第２足平を、姿勢は変えずにベクトルＱ２
Ｑ２’だけ平行移動させる。移動後の目標各足平を図１
６に太線で示す。

【０１１１】４）次に、目標第１足平をＱ１’を中心
に、前後方向軸（Ｘ軸）まわりに回転角θ１ｘ、左右方
向軸（Ｙ軸）まわりに回転角θ１ｙだけ回転させる。同
様に、目標第２足平をＱ２’を中心に前後方向軸（Ｘ
軸）まわりに回転角θ２ｘ、左右方向軸（Ｙ軸）まわり
に回転角θ２ｙだけ回転させる。回転角θｎｘ，θｎｙ
をそれぞれ第ｎ足平Ｘ補償角、第ｎ足平Ｙ補償角とい
う。回転後の目標各足平を図１７に太線で示す。

【０１１２】以上の補償動作量が過大でなければ、接地
圧力分布は変わっても、接地領域（足底面の圧力が正の
領域）は変わらない。このような場合には、補償動作量
に比例して各足平に装着されたコンプライアンス機構４
２が変形し、変形量に応じた実各足平床反力が発生す
る。この結果、補償動作量と補償動作によって発生する
実床反力の変化量との間の関係は、以下に示す良好な特
性を持つ。

【０１１３】特性１）両脚補償角θｄｂｖだけを操作し
て目標各足平位置を移動させると、下がった足平の実足
平床反力の力成分が増加し、上がった足平の実足平床反
力の力成分が減少する。このとき、修正目標各足平床反
力中心点まわりの実各足平床反力モーメントは、ほとん
ど変化しない。

【０１１４】特性２）第ｎ足平Ｘ補償角だけを操作して
目標第ｎ足平姿勢を回転させると、目標第ｎ足平床反力
中心点に作用する実第ｎ足平床反力のモーメントのＸ成
分だけが変化し、その他の床反力成分は少ししか変化し
ない。同様に、第ｎ足平Ｙ補償角だけを操作して目標第
ｎ足平姿勢を回転させると、実第ｎ足平床反力のモーメ
ントのＹ成分だけが変化し、その他の床反力成分は少し
しか変化しない。

【０１１５】特性３）両脚補償角θｄｂｖ、各足平Ｘ補
償角および各足平Ｙ補償角を同時に操作すると、実各足
平床反力の変化量は、それぞれを単独に操作したときの
変化量の和になる。

【０１１６】特性１および特性２は、これらの操作に独
立性があることを示し、特性３はこれらの操作に線形性
があることを示していると言える。

【０１１７】図１８は複合コンプライアンス動作決定部
１１４の演算処理を示すブロック図であり、同図を参照
してその処理を説明する。

【０１１８】概説すると、補償全床反力モーメント分配
器１１４ａにおいては補償全床反力モーメントＭｄｍｄ
の分配を行う。次に、実各足平床反力と分配された補償
全床反力モーメントなどから、両脚補償角決定部１１４
ｂおよび第ｎ足平Ｘ（Ｙ）補償角決定部１１４ｃ，１１
４ｄ，１１４ｅ，１１４ｆにおいて前述の補償角θｄｂ
ｖおよびθｎｘ（ｙ）を決定する。

【０１１９】次に、決定された各種補償角に基づいて修
正目標足平位置姿勢算出部１１４ｇは、補償された足平
位置姿勢（これを修正目標足平位置姿勢という）を幾何
学演算によって求める。最後に、機構変形補償入り修正
目標足平位置姿勢算出部１１４ｈは、目標各足平床反力
によって発生が予想されるコンプライアンス機構４２の
変形量を求め、それらを打ち消すように修正目標足平位
置姿勢をさらに修正する。

【０１２０】以下詳説すると、補償全床反力モーメント
分配器１１４ａは、補償全床反力モーメントＭｄｍｄ
を、両脚補償モーメントＭｄｍｄｄｂ、各足平補償モー
メントＭｄｍｄ１ｘ，ｙ，Ｍｄｍｄ２ｘ，ｙに分配す
る。両脚補償モーメントＭｄｍｄｄｂは、両脚補償角
（足平上下量）θｄｂｖを操作することによって目標全
床反力中心点（目標ＺＭＰ）まわりに各足平床反力の力
成分が作るモーメントの目標値である。

【０１２１】両脚補償モーメントＭｄｍｄｄｂのＶ方向
まわりの成分をＭｄｍｄｄｂｖと記述する。尚、ベクト
ルＶは複合コンプライアンス動作決定部１１４の説明で
定義したベクトルである。Ｖに直交し、鉛直方向にも直
交するベクトルをＵとすると、両脚補償モーメントＭｄ
ｍｄｄｂのＵ方向成分Ｍｄｍｄｄｂｕは０に設定され
る。両脚補償角θｄｂｖを操作しても、床反力のＵ方向
モーメント成分を発生することはできないからである。

【０１２２】ここでは補償全床反力モーメントＭｄｍｄ
の鉛直方向成分が０なので、Ｍｄｍｄｄｂの鉛直方向成
分Ｍｄｍｄｄｂｚも０に設定される。

【０１２３】第１足平補償モーメントＭｄｍｄ１は、第
１足平補償角θ１ｘ，θ１ｙを操作することによって目
標第１足平床反力中心点まわりに発生させたいモーメン
トである。第１足平補償モーメントＭｄｍｄ１のＸ成分
をＭｄｍｄ１ｘ、Ｙ成分をＭｄｍｄ１ｙと記述する。第
２足平補償モーメントＭｄｍｄ２は、第２足平補償角θ
２ｘ，θ２ｙを操作することによって目標第２足平床反
力中心点まわりに発生させたいモーメントである。第２
足平補償モーメントＭｄｍｄ２のＸ成分をＭｄｍｄ２
ｘ、Ｙ成分をＭｄｍｄ２ｙと記述する。

【０１２４】分配は、例えば次のように行う。 Mdmddbv ＝ Wdbx * Mdmdx + Wdby * Mdmdy ・・・式３ Mdmd1x ＝ W1x * (Mdmdx - Wint * Vx * Mdmddbv) Mdmd1y ＝ W1y * (Mdmdy - Wint * Vy * Mdmddbv) Mdmd2x ＝ W2x * (Mdmdx - Wint * Vx * Mdmddbv) Mdmd2y ＝ W2y * (Mdmdy - Wint * Vy * Mdmddbv) ・・・式４

【０１２５】ここで、Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ
１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔは分配用重み変数
である。ＶｘはベクトルＶのＸ成分の値、Ｖｙはベクト
ルＶのＹ成分の値である。この中で、Ｗｉｎｔは、両脚
補償角を操作することによって発生した全床反力モーメ
ントを各足平補償角を操作することによって打ち消すた
めのものである。

【０１２６】式３と式４の演算処理を行う補償全床反力
モーメント分配器１１４ａのブロック図を図１９に示
す。

【０１２７】歩行時の分配用重み変数Ｗｄｂｘ，Ｗｄｂ
ｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔの
設定例を図２０に示す。図２０のパターンは、以下の注
意点を考慮して決定することが望ましい。

【０１２８】注意点１）両脚補償角と各足平補償角が不
連続的に変化すると、関節に過大なトルクが発生する。
そこで、両脚補償角と各足平補償角を連続的に変化させ
るために、分配用重み変数は連続的に変化させる。

【０１２９】注意点２）両脚補償角および各足平補償角
を操作することによって発生する実床反力モーメント
が、なるべく補償全床反力モーメントＭｄｍｄに近い値
になるように、分配用重み変数を決定する。

【０１３０】この際、直立時や歩行時など状況に応じて
以下に示すように設定方針を変えた方が良い。直立時な
どのように、両脚補償モーメントのＶ方向成分Ｍｄｍｄ
ｄｂｖ、各足平補償モーメントＭｄｍｄ１，Ｍｄｍｄ２
を忠実に実各足平床反力に発生させることができる状況
では以下のように設定する。

【０１３１】この状況では目標全床反力中心点（目標Ｚ
ＭＰ）まわりの実全床反力モーメントの水平方向成分
を、補償全床反力モーメントＭｄｍｄの水平方向成分に
一致させるため（即ち、前述の複合コンプライアンス動
作決定部に対する要求１を満足するため）に、式５と式
６の両方をなるべく満足するように重みを設定すべきで
ある。

【０１３２】 Mdmddbv*Vx + Mdmd1x + Mdmd2x = Mdmdx・・・式５ Mdmddbv*Vy + Mdmd1y + Mdmd2y = Mdmdy・・・式６

【０１３３】これに式３、式４を代入すると、式５は式
７に、式６は式８に変換される。 (Wdbx * Mdmdx + Wdby * Mdmdy)*Vx + W1x * (Mdmdx - Wint * Vx * (Wdbx * Mdmdx + Wdby * Mdmdy)) + W2x * (Mdmdx - Wint * Vx * (Wdbx * Mdmdx + Wdby * Mdmdy))= Mdmdx ・・・式７ (Wdbx * Mdmdx + Wdby * Mdmdy)*Vy + W1y * (Mdmdy - Wint * Vy * (Wdbx * Mdmdx + Wdby * Mdmdy)) + W2y * (Mdmdy - Wint * Vy * (Wdbx * Mdmdx + Wdby * Mdmdy)) = Mdmdy ・・・式８

【０１３４】ＭｄｍｄｘとＭｄｍｄｙが任意の値を取っ
ても、式７と式８が恒等的に成立するためには、式９、
式１０、および式１１を同時に満足すれば良い。 Wint = 1 ・・・式９ W1x + W2x =1 ・・・式１０ W1y + W2y =1 ・・・式１１ 即ち、以上の状況では式９、式１０および式１１を同時
に満足するように、重みを決定すれば良い。

【０１３５】歩行時ではＭｄｍｄｄｂｖを目標にして両
脚補償角θｄｂｖを操作して足平の位置を修正しても、
実全床反力モーメントの発生量がＭｄｍｄｄｂｖに較べ
て不足する場合がある。例えば図２１のように両脚支持
期の初期にロボットが後傾して第１足平が未だ着地して
いない状況では、θｄｂｖによって第１足平の位置を下
げても、実床反力は変化しない。

【０１３６】同様に、Ｍｄｍｄ２を目標にして第２足平
補償角θ２を操作して第２足平の角度を修正しても、実
床反力モーメントの増加量がＭｄｍｄ２に較べて不足す
る場合がある。例えば、図２２のように両脚支持期の後
半にロボットが後傾している状況では、θ２によって第
２足平のかかとを下げても実床反力は変化しない。

【０１３７】従って、式５、式６を満足するように各重
みを設定しても、複合コンプライアンス制御によって発
生する実全床反力の増加量が補償全床反力モーメントＭ
ｄｍｄに届かない場合がある。このようなことが生じる
可能性が高い状況では、式５、式６の左辺の値を右辺の
値で割った値が１より大きくなるようにすべきである。

【０１３８】歩行時の分配用重み変数設定例である図２
０では、Ｗｉｎｔを０に設定することによって、図２１
の状況のように、両脚補償角θdbv を操作しても実全床
反力モーメントが発生できなくなっても、各足平補償角
を操作して不足分を補うようにした。

【０１３９】好都合なことに、図２１のように後傾する
と第２足平のかかとが結果的に下がって床に接地しやす
くなるので、第２足平補償角を操作することによって実
全床反力モーメントを発生させることができるようにな
る。

【０１４０】また、後傾していないときには両脚補償角
θｄｂｖを操作することによる実全床反力モーメントが
発生するが、第２足平のかかとが床に接地しないので、
第２足平補償角を操作しても実全床反力モーメントは発
生しない。

【０１４１】つまり、両脚補償角θｄｂｖが有効に働く
ときには各足平補償角が有効に働かず、各足平補償角が
有効に働くときには両脚補償角θｄｂｖが有効に働かな
いので、結果的に両脚補償角および各足平補償角を操作
することによって発生する実床反力モーメントの総量
は、ほぼ補償全床反力モーメントＭｄｍｄに等しくな
る。

【０１４２】状況によっては、両脚補償角および各足平
補償角を操作することによって発生する実床反力モーメ
ントの総量が補償全床反力モーメントＭｄｍｄよりも大
きくなってしまう場合がある。

【０１４３】しかし、この場合でも、Ｍｄｍｄが姿勢安
定化のためのフィードバック操作量であるならば、あま
り問題にならない。何故ならば、Ｍｄｍｄの大きさが多
少違っていても、一般的に制御系に言えることである
が、制御系のオープンループゲインが多少変化するだけ
で、クローズドループ特性はほとんど変わらないからで
ある。

【０１４４】注意点３）片脚支持期では、両脚補償角用
の分配用重み変数であるＷｄｂｘ，Ｗｄｂｙの絶対値を
小さくする。片脚支持期では両脚補償角を変化させて
も、接地していない足平が無駄に上下するだけで、実各
足平床反力は変化しないからである。

【０１４５】注意点４）足平の接地性を確保するため
に、目標足平床反力の力成分が小さいときには、その足
平の足平補償角のための分配用重み変数の絶対値を小さ
くする。特に、足平が床から遠く離れているときには、
その足平の足平補償角を動かしても、その足平の実足平
床反力は変化しないので、不要な動きをさせないために
も、その足平の足平補償角のための分配用重み変数の絶
対値を小さくすべきである。

【０１４６】注意点５）両脚補償角を操作することによ
って制御できる実全床反力モーメントの方向と、各足平
補償角を操作することによって制御できる実全床反力モ
ーメントの方向は通常異なる。

【０１４７】例えば、両脚補償角θｄｂｖを操作するこ
とによって発生する実全床反力モーメントの向きは必ず
Ｖ方向であり、Ｖ方向に直交する成分を発生させること
はできない。一方、各足平補償角を操作することによっ
て発生できる実全床反力モーメントの向きは、足平の接
地状況によって制約を受ける。

【０１４８】例えば、足平２２Ｒ（Ｌ）のつまさきのエ
ッジだけ、またはかかとのエッジだけが接地している場
合には、エッジ線方向にモーメントを発生することはで
きない。両脚支持期では、この特性を考慮して、なるべ
く無駄なく両脚補償角および各足平補償角を操作する。

【０１４９】例えば、両脚補償角を操作するための分配
重みＷｄｂｘ，Ｗｄｂｙは次のように決定する。

【０１５０】Ｘ成分がＷｄｂｘ、Ｙ成分がＷｄｂｙ、Ｚ
成分が０のベクトルをＷｄｂとすると、式３はベクトル
ＷｄｂとＭｄｍｄの内積になっている。従って、Ｍｄｍ
ｄをベクトルＷｄｂ方向成分とその直交成分に分解し、
ベクトルＷｄｂ方向成分だけを抽出して、ベクトルＷｄ
ｂの大きさを乗じたものが、式３によって求められるＭ
ｄｍｄｄｂｖであると言える。

【０１５１】この場合のＭｄｍｄｄｂｖを図２３に示
す。これは、両脚補償角を操作することによって実全床
反力モーメントのＷｄｂ方向成分を制御するフィードバ
ック制御系を構成することを意味する。もし、Ｗｄｂ方
向がベクトルＶと直交していたら、両脚補償角をいくら
操作しても実全床反力モーメントのＷｄｂ方向成分は発
生しないから、このフィードバック制御系はただ無駄に
両脚補償角を操作するだけになる。

【０１５２】従って、無駄な動きを減らしたい場合に
は、Ｗｄｂ方向をベクトルＶ方向に一致させるか、また
はなるべく近づけるべきである。また、補償全床反力モ
ーメントＭｄｍｄのＷｄｂ方向成分を、各足平補償角に
頼らずに両脚補償角を操作するだけで発生させたいなら
ば、ＷｄｂとＶの内積が１になるように設定する。一部
を各足平補償角に頼らせたいならば、ＷｄｂとＶの内積
が１より小さくなるように設定する。

【０１５３】ところで、足平の横幅が狭い場合には、各
足平補償角を操作することによって発生し得る実各足平
床反力モーメントのＸ成分は小さくなる。この場合に
は、Ｗｄｂｘを大きめに設定する。このように設定する
と、Ｗｄｂ方向とベクトルＶ方向は一致しなくなり、両
脚補償角の変動が増加するが、安定性が増す。

【０１５４】両脚補償角決定部１１４ｂの処理について
さらに詳説すると、図２４は両脚補償角決定部１１４ｂ
の演算処理のブロック図であり、両脚補償角θｄｂｖは
図示の如く演算される。

【０１５５】図２４を参照して説明すると、目標第１足
平床反力中心点Ｑ１に作用するＦ１ａｃｔと目標第２足
平床反力中心点Ｑ２に作用するＦ２ａｃｔが、目標全床
反力中心点Ｐのまわりに発生させるモーメントＭｆ１ｆ
２ａｃｔを、次式により求める。

【０１５６】 Mf1f2act = PQ1*F1act + PQ2*F2act ・・・式１２ ここで、ＰＱ１は始点がＰ、終点がＱ１のベクトル、Ｐ
Ｑ２は始点がＰ、終点がＱ２のベクトルである。

【０１５７】また、式１２の代わりに、次式を用いても
実際上はほとんど問題がない。 Mf1f2act = PQ1*F1act + PQ2*F2act+ M1act+ M2act ・・・式１２ａ 式１２ａは、目標全床反力中心点まわりに作用する実全
床反力モーメントＭｓｕｍａｃｔを算出する式になって
いる。尚、式１２は、目標全床反力中心点まわりに作用
する実全床反力モーメントから、目標各足平床反力中心
点まわりに作用する実各足平床反力モーメントを減じた
ものになっている。

【０１５８】次に、Ｍｆ１ｆ２ａｃｔのベクトルＶ方向
成分Ｍｆ１ｆ２ａｃｔｖを抽出する。これは、ベクトル
の内積演算を用いた次式によって得られる。尚、ベクト
ルＶは前述の動作説明において図１５に示したＶであ
る。 Mf1f2actv = Mf1f2act・V ・・・式１３

【０１５９】次に、Ｍｆ１ｆ２ａｃｔｖをローパスフィ
ルタ１１４ｉに通し、Ｍｆ１ｆ２ａｃｔｖｆｉｌｔを得
る。

【０１６０】次に、両脚補償モーメントＶ方向成分Ｍｄ
ｍｄｄｂｖを補償フィルタ１１４ｊに通し、それを、Ｍ
ｆ１ｆ２ａｃｔｖｆｉｌｔから減じ、偏差モーメントＶ
方向成分Ｍｄｉｆｆｖを得る。

【０１６１】尚、補償フィルタ１１４ｊは、Ｍｄｍｄｄ
ｂｖから実全床反力モーメントまでの伝達関数の周波数
応答特性を改善するものである。

【０１６２】次に、コンプライアンス機構４２の変形に
よる両脚補償モーメントＶ方向成分への影響を打ち消す
ための両脚機構変形補償角θｆｆｄｂｖを求める。これ
は、いわゆるフィードフォワード補償である。

【０１６３】具体的には、両脚補償モーメントＶ方向成
分Ｍｄｍｄｄｂｖと変形量との関係を表す機構コンプラ
イアンスモデルを用い、目標第１足平床反力中心点Ｑ１
と目標第２足平床反力中心点Ｑ２を結ぶ線分の変形角度
を求め、それの極性を反転したものを両脚機構変形補償
角θｆｆｄｂｖとすれば良い。

【０１６４】両脚機構変形補償角θｆｆｄｂｖは、近似
的には次式により求めれば良い。 θffdbv ＝−α＊Mdmddbv ・・・式１４ ここでαは所定の定数である。

【０１６５】最後に次式によって両脚補償角θｄｂｖを
得る。ここでＫｄｂは制御ゲインであり、通常、これは
正の値に設定する。 θdbv = Kdb * Mdiffv＋θffdbv ・・・式１５

【０１６６】第ｎ足平補償角決定部について説明する
と、図２５はその中の第１足平Ｘ補償角決定部１１４ｃ
の演算処理を示すブロック図であり、第１足平Ｘ補償角
決定部１１４ｃは第１足平Ｘ補償角θ１ｘを図示の如く
演算する。説明は省略するが、第１足平Ｙ補償角θ１
ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙ
も同様に求める。ここでは第１足平Ｘ補償角θ１ｘを求
めるアルゴリズムだけを説明する。

【０１６７】第１足平床反力モーメントＸ成分Ｍ１ａｃ
ｔｘをローパスフィルタ１１４ｋに通してＭ１ａｃｔｆ
ｉｌｔｘを得る。第１足平補償モーメントＸ成分Ｍｄｍ
ｄ１ｘを補償フィルタ１１４ｍに通し、それを、Ｍ１ａ
ｃｔｆｉｌｔｘから減じ、偏差モーメントＭｄｉｆｆ１
ｘを得る。両脚補償角決定と同様、補償フィルタ１１４
ｍは、Ｍｄｍｄ１ｘから実全床反力までの伝達関数の周
波数応答特性を改善するものである。

【０１６８】次に、両脚補償角決定と同様、コンプライ
アンス機構４２の変形による第１足平補償モーメントＸ
成分への影響を打ち消すための第１足平Ｘ機構変形補償
角θｆｆ１ｘを求める。これは、いわゆるフィードフォ
ワード補償である。

【０１６９】具体的には、第１足平補償モーメントＸ方
向成分Ｍｄｍｄ１ｘと変形量との関係を表す機構コンプ
ライアンスモデルを用い、第１足平の変形角度を求め、
それの極性を反転したものを第１足平Ｘ機構変形補償角
θｆｆ１ｘとすれば良い。

【０１７０】第１足平Ｘ機構変形補償角θｆｆ１ｘは、
近似的には次式により求めれば良い。 θff1x＝−α1x＊Mdmd1x ・・・式１６ ここでα１ｘは所定の定数である。

【０１７１】最後に次式によって第１足平Ｘ補償角θ１
ｘを得る。ここでＫ１ｘは制御ゲインであり、通常、こ
れも正の値に設定する。 θ1x = K1x * Mdiff1x＋θff1x ・・・式１７ 尚、図示のブロック線図は、演算処理順序を変えるなど
の等価変形をしても良い。

【０１７２】図１８に戻って説明を続けると、修正目標
足平位置姿勢算出部１１４ｇは、両脚補償角θｄｂｖ、
第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第
２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙに基づ
き、前述の複合コンプライアンス動作の足平位置姿勢修
正手法に従って目標足平位置姿勢を修正し、修正目標足
平位置姿勢を得る。

【０１７３】機構変形量算出部１１４ｎは、目標各足平
床反力によって発生が予想されるコンプライアンス機構
４２の変形量を求める。

【０１７４】機構変形補償入り修正目標足平位置姿勢算
出部１１４ｈは、算出された機構変形量を打ち消すよう
に、修正目標足平位置姿勢をさらに修正し、機構変形補
償入り修正目標足平位置姿勢を得る。

【０１７５】例えば、図２６に示すような機構変形量が
予想されるときには、機構変形補償入り修正目標足平位
置姿勢は、図２７に実線で示す位置姿勢に修正される。
即ち、図２７に示す機構変形補償後の足平２２Ｒ（Ｌ）
が目標足平床反力を受けて変形したときの位置姿勢が、
図２６に示す機構変形補償前の足平位置姿勢に一致する
ように、機構変形補償入り修正目標足平位置姿勢を算出
する。

【０１７６】機構変形補償は、コンプライアンス機構４
２の変形によって生じる実足平位置姿勢のずれをフィー
ドフォワード的に打ち消す制御であり、この制御がない
場合に比較し、より一層、目標歩容に近い歩行を実現す
ることができる。

【０１７７】上記を前提として図１０フロー・チャート
の説明に戻ると、前記の如く、Ｓ３４において上記した
補償角を決定する。

【０１７８】図２８はその作業を示すサブルーチン・フ
ロー・チャートである。

【０１７９】同図を参照して説明すると、先ずＳ１００
において前記したベクトルＶを求め、Ｓ１０２に進んで
分配用重み変数を図２０に示すように設定し、現在時刻
ｔでのこれらの値を求める。続いてＳ１０４に進み、式
３および式４によって補償全床反力モーメントＭｄｍｄ
を両脚補償モーメントＭｄｍｄｄｂｖと各足平補償モー
メントＭｄｍｄｎｘ（ｙ）に分配し、Ｓ１０６に進んで
既述の如く両脚補償角θｄｂｖを求め、Ｓ１０８に進ん
で各足平補償角θｎｘ（ｙ）を求める。

【０１８０】次いで図１０フロー・チャートのＳ３６に
進み、目標各足平床反力に基づいて機構変形補償量を算
出し、Ｓ３８に進んで目標足平位置姿勢を補償角θｄｂ
ｖ，θｎｘ（ｙ）に応じて修正し、さらにこれを機構変
形補償量に応じて修正し、機構変形補償入り修正目標足
平位置姿勢を得る。

【０１８１】次いでＳ４０に進み、上***置姿勢と機構
変形補償入り修正足平位置姿勢から関節変位指令（値）
を算出し、Ｓ４２に進んで実関節変位を算出された関節
変位指令（値）に追従させるようサーボ制御し、Ｓ４４
に進んで時刻をΔｔ更新し、Ｓ１４に戻って上記の処理
を繰り返す。

【０１８２】上記の如く構成したので、これによって、
概括すれば、実全床反力の制御と実各足平床反力の制御
が殆ど干渉しないようになり、それらを容易に制御する
ことができる。

【０１８３】即ち、この複合コンプライアンス制御装置
は、先に提案した技術に対して以下の点を改良した。即
ち、特開平５−３０５５８４号公報で提案した足首コン
プライアンス制御では、足首または足底の基準点などの
足平に固定された点における実床反力モーメントを検出
し、それに基づいて前記固定された点を中心に足平を回
転させていたが、この複合コンプライアンス制御装置で
は、移動する目標足平床反力中心点における実各足平床
反力モーメントを算出し、それに基づいて目標足平床反
力中心点を中心に足平を回転させるように変更し、その
点まわりのモーメントを望ましい値に制御するようにし
た。

【０１８４】この結果、実全床反力と実各足平床反力が
ほとんど干渉することなく、容易に制御することが可能
となった。より干渉を少なくするために、各瞬間におけ
る想定していた足底接地領域内にもっと適切な点を選定
しても良い。

【０１８５】さらには、ロボットに作用する床反力、よ
り具体的には目標全床反力中心点（目標ＺＭＰ）まわり
の実全床反力モーメントと目標各足平中心点まわりの実
各足平床反力モーメントを容易かつ適切に制御すること
ができる。換言すれば、先に提案した技術を併用する場
合に比較し、制御の干渉がなく、実全床反力と実各足平
床反力が望ましい値からずれたり発振することがない。

【０１８６】従って、大域的なうねりや傾斜だけでな
く、局所的な凹凸や傾斜なども含む予期しない床形状変
化があっても、その影響をあまり受けずに脚式移動ロボ
ットに作用する床反力を適切に制御することができる。

【０１８７】また、脚式移動ロボットの姿勢安定化制御
を容易に実現できると共に、脚式移動ロボットが受ける
着地衝撃を低減することができ、脚式移動ロボットの接
地性を高め、歩行時のスリップやスピンを防止すること
ができる。さらに、脚式移動ロボットのアクチュエータ
の負荷を低減することができる。

【０１８８】また、特開平５−３０５５８６号公報で提
案した技術では実全床反力（各足平床反力の合力）の目
標全床反力中心点（目標ＺＭＰ）まわりのモーメント成
分を検出し、その値が望ましい値になるように制御して
いたが、この複合コンプライアンス制御装置において
は、目標各足平床反力中心点に作用する足平床反力の内
のモーメント成分を除いた並進力成分の合成が目標全床
反力中心点（目標ＺＭＰ）まわりに作用するモーメント
を検出し、その値を望ましい値になるように制御するよ
うに変更した（尚、この点は先に提案した制御手法であ
っても良い）。

【０１８９】図２９は複合コンプライアンス制御の変形
例を示す、図１６と同様な説明図である。

【０１９０】変形例にあっては補償動作を簡易化して同
様の作用効果を得るようにした。即ち、各足平の床反力
の力成分を操作する足平位置修正動作の手法としては、
図１６に示した手法に代え、図２９に示すように、鉛直
方向にだけ移動させるようにした。このとき、第１足平
鉛直方向移動量Ｚ１と第２足平鉛直方向移動量Ｚ２は、
次式によって求める。 Z1 = -線分PQ1 の長さ *θdbv Z2 = 線分PQ2 の長さ *θdbv ・・・式１８ 但し、ここで、θｄｂｖには式１５で求められる値を代
入する。

【０１９１】以上を前提としてこの発明の一つの実施の
形態に係る脚式移動ロボットの床形状推定装置を説明す
る。

【０１９２】この実施の形態に係る脚式移動ロボットの
床形状推定装置の構成は、図４に示した複合コンプライ
アンス制御の構成、より具体的には図１８に示した構成
と同様である。ただし、図３０に示す如く、補償全床反
力モーメント分配器１１４ａなどで示される複合コンプ
ライアンス動作決定部は、図１８に示した構成と異な
り、新たに床形状推定器１３０と、床形状推定器１３０
が出力する各種の床形状推定値をそれぞれの補償角に加
算する加算器１３２（１３２ａ，１３２ｂ）を備えるよ
うにした。

【０１９３】同図の説明に入る前に、床形状推定装置１
３０が推定する上で使用する概念と用語を以下のように
定義する。

【０１９４】図３１に示すように、目標歩容において想
定された床（あるいは床面）を「想定床」と呼ぶ。ロボ
ットが歩行している実際の床を「実床」と呼ぶ。また想
定床形状に対する実床形状のずれを「床形状偏差」と呼
ぶ。

【０１９５】前記した複合コンプライアンス制御におい
て定義された目標第ｎ足平床反力中心点Ｑｎは、第ｎ足
平の足底（床との接触面）に設定された点であった。従
って、目標第ｎ足平床反力中心点Ｑｎは、足平２２Ｒ
（Ｌ）と共に移動する。これに対応して、目標第ｎ足平
床反力中心点Ｑｎと接することが想定された想定床面上
の点を「想定第ｎ床接点Ｄｎ」と呼ぶ。

【０１９６】この定義から明らかなように、第ｎ足平が
接地している時期では、目標第ｎ足平床反力中心点Ｑｎ
と想定第ｎ床接点Ｄｎは、支持脚座標系から見て同一座
標になる。これに対し、実際にロボット１が歩行してい
るときに、実第ｎ足平の足底面上における目標第ｎ足平
床反力中心点Ｑｎに相当する点が、実床に接触する点を
「実第ｎ床接点Ｄｎａｃｔ」と呼ぶ。

【０１９７】これらの点の関係を表す一例を図３１に示
す。図３１は、目標第１足平床反力中心点Ｑ１と目標第
２足平床反力中心点Ｑ２を通る垂直面の法線方向（複合
コンプライアンス制御におけるベクトルＶの方向）に向
いて側方から歩行しているロボット１を見たところであ
る。

【０１９８】ロボット１の目標姿勢（目標歩容のこの瞬
間における姿勢）および前記垂直面における想定床の断
面を細線で示す。ロボット１の実足平位置姿勢と前記垂
直面における実床の断面を太線で示す。この状況におけ
る実第ｎ床接点は、実床面上の点であり、図３１に示す
位置になる。

【０１９９】さらに、床形状偏差を定量的に表現する指
標として、両脚間床高低差偏差、両脚間床傾斜偏差、足
平床傾斜偏差を以下のように定義する。

【０２００】第２床接点の高さと第１床接点の高さの差
を「両脚間床高低差」と呼び、以下のように算出する。 両脚間床高低差＝第２床接点の高さ−第１床接点の高さ ・・・式１９

【０２０１】実両脚間床高低差と想定両脚間床高低差と
の差を「両脚間床高低差偏差」と呼ぶ。始点を第１床接
点、終点を第２床接点とする線分の傾斜を「両脚間床傾
斜」と呼び、以下のように算出する。 両脚間床傾斜＝atan（（第２床接点の高さ−第１床接点の高さ） ／（第２床接点と第１床接点の間の水平距離）） ・・・ 式２０ ここでatanは、三角関数であるタンジェントの逆関数を
表す。

【０２０２】実両脚間床傾斜と想定両脚間床傾斜との差
を「両脚間床傾斜偏差」と呼ぶ。それを図３１に示す。
この状況において、両脚間床傾斜偏差は正である。第ｎ
床接点における床面の傾斜を「第ｎ足平床傾斜」と呼
ぶ。

【０２０３】実第ｎ足平床傾斜と想定第ｎ足平床傾斜と
の差を「第ｎ足平床傾斜偏差」と呼ぶ。これを図３２に
示す。図３２はロボット１を後ろから見た図である。ロ
ボット１の目標姿勢（目標歩容のこの瞬間における姿
勢）および想定床の断面を細線で示す。ロボット１の実
足平位置姿勢と実床の断面を太線で示す。この状況にお
いて、第１足平床傾斜偏差は負である。

【０２０４】尚、両脚間床傾斜および両脚間床傾斜偏差
は、前述のベクトルＶ方向まわりの回転角で表される。
第ｎ足平床傾斜および第ｎ足平床傾斜偏差は、Ｘ方向の
回転角とＹ方向の回転角で表される。

【０２０５】また、両脚間床傾斜偏差および第ｎ足平床
傾斜偏差と複合コンプライアンス制御の各種補償角との
間には、以下の関係がある。即ち、両脚間床傾斜偏差
は、両脚支持期において、目標歩容通りに歩行している
ロボット１の目標第１足平床反力中心点Ｑ１と目標第２
足平床反力中心点Ｑ２を結ぶ線分を、目標床面に平行に
なっている状態から、実床面に平行にさせるために必要
な両脚補償角に相当する。

【０２０６】また、第ｎ足平床傾斜偏差は、目標歩容通
りに歩行しているロボットの第ｎ足平を、目標床面に平
行になっている状態から、実床面に平行にさせるために
必要な第ｎ足平補償角に相当する。

【０２０７】従って、歩行中に両脚間床傾斜偏差を推定
し、両脚間床傾斜偏差の推定値を両脚補償角に加えれ
ば、両脚間床傾斜偏差があっても、実全床反力は想定床
を歩行しているときと同一になる。また、歩行中に第ｎ
足平床傾斜偏差を推定し、第ｎ足平補償角の推定値を第
ｎ補償角に加えれば、第ｎ足平床傾斜偏差があっても、
実第ｎ足平床反力は想定床を歩行しているときと同一に
なる。

【０２０８】尚、複合コンプライアンス制御の両脚補償
動作として変形例で示した鉛直方向にのみ移動させる手
法を用いる場合には、両脚間床傾斜偏差の推定値の代わ
りに、両脚間床高低差偏差の推定値を用いれば良い。本
質的な違いはないので、以降の説明では両脚間床傾斜偏
差の推定値を用いる場合のみを説明する。

【０２０９】図３０に示す構成における床形状推定器１
３０への入力は、大きく分けて以下のようになる。 １）各種目標床反力中心点（目標全床反力中心点、目標
第ｎ足平床反力中心点）、 ２）想定床面形状（想定第ｎ床接点の座標、想定第ｎ足
平床傾斜）、 ３）最終的に実ロボットが追従すべき目標姿勢（機構変
形入り修正目標足平位置姿勢）または実関節変位または
これらの周波数重み付き平均、 ４）上体傾斜偏差、 ５）実床反力（実第ｎ足平床反力の力成分とモーメント
成分）

【０２１０】尚、床形状推定器１３０への入力は、図３
０に示すものを用いると、最も推定精度を高くすること
ができるが、それ以外のものを選んでも良い。それに関
しては第２の実施の形態で述べる。

【０２１１】床形状推定器１３０の処理機能ブロック図
を図３３に示す。

【０２１２】図３３に示す床形状推定器１３０を構成す
る各機能要素について説明すると、床形状推定器１３０
は機構コンプライアンスモデル１３４を備える。

【０２１３】機構コンプライアンスモデル１３４は、ロ
ボット幾何学モデル１１０（図４に示す）への入力、即
ち、機構変形補償入り修正目標足平位置姿勢が、実床反
力を受けたときのコンプライアンス機構４２の変形量を
求め、求めた変形量を機構変形補償入り修正目標足平位
置姿勢に加えることにより、機構変形後の推定各足平位
置姿勢を求める。ここで、機構変形後の推定各足平位置
姿勢は、上体２４に対する相対位置姿勢で表現される。

【０２１４】ロボットの関節変位制御の追従遅れが無視
できない場合、図３４に示すように、機構変形補償入り
修正目標足平位置姿勢に、追従遅れに相当するローパス
フィルタ１３６を通して機構コンプライアンスモデル１
３４に入力すれば良い。

【０２１５】あるいは、図３５に示すように、実関節変
位からロボット幾何学モデル１３７（図４に１１０と示
すのと同様のモデル）を介して機構変形なし実足平位置
姿勢を求め、求めた値を機構変形補償入り修正目標足平
位置姿勢の代わりに機構コンプライアンスモデル１３４
に入力すれば良い。

【０２１６】あるいは、周波数重みを用い、前記機構変
形なし実足平位置姿勢と機構変形補償入り修正目標足平
位置姿勢の重みつき平均を求め、それを機構変形補償入
り修正目標足平位置姿勢の代わりに機構コンプライアン
スモデル１３４に入力すれば良い。

【０２１７】厳密には、前記した機構変形なし実足平位
置姿勢を用いるべきであるが、関節変位制御の能力が高
ければ、上記のいずれでも大差はない。

【０２１８】また、図４においてロボット幾何学モデル
１１０への入力に機構変形補償が含まれない場合、機構
変形補償のない修正目標足平位置姿勢を機構コンプライ
アンスモデル１３４に入力すれば良い。

【０２１９】図３３の説明に戻ると、床形状推定器１３
０は両脚間床傾斜偏差推定部１３８を備える。即ち、同
図において、機構変形後の推定各足平位置姿勢、目標各
足平床反力中心点の目標足平に対する相対座標、想定各
床接点および上体傾斜偏差から推定両脚間床傾斜偏差を
推定するまでのブロックを「両脚間床傾斜偏差推定部」
と呼ぶ。以下にこの部分の処理を説明する。

【０２２０】先ず、機構変形後の推定各足平位置姿勢と
目標各足平床反力中心点Ｑｎの目標足平に対する相対座
標とから、機構変形後の目標各足平床反力中心点Ｑｎ”
の座標を算出する。ここで、機構変形後の目標各足平床
反力中心点Ｑｎ”は、実上体に対する相対位置で表わさ
れる。

【０２２１】より具体的には、目標第ｎ足平床反力中心
点Ｑｎの足平に対する相対位置をベクトルＲｎ、機構変
形後の推定第ｎ足平位置をベクトルＵｎ、機構変形後の
推定第ｎ足平姿勢をマトリックスＡｎで表わすと、機構
変形後の目標第ｎ足平床反力中心点Ｑｎ”は次式によっ
て計算される。 Qn”＝AnRn＋Un・・・式２１

【０２２２】次に、Ｑ１”を始点、Ｑ２”を終点とする
ベクトルＱ１”Ｑ２”の実上体に対するベクトルＶまわ
りの傾きθｆｄｂｖ”を算出する。ここでベクトルＶ
は、複合コンプライアンス制御で定義されたベクトルで
ある。

【０２２３】一方、想定第ｎ床接点Dnの目標上体に対す
る相対座標から、Ｄ１を始点、Ｄ２を終点とするベクト
ルＤ１Ｄ２の目標上体に対するベクトルＶまわりの傾き
θｆｄｂｖを算出する。

【０２２４】ここで、想定第ｎ床接点Ｄｎは、先に定義
したように、第ｎ足平が接地している時期においては、
目標第ｎ足平床反力中心点Ｑｎと同一位置とする。目標
歩容において第ｎ足平が接地する直前の時期では、次に
接地する時に想定されている目標第ｎ足平床反力中心点
Ｑｎの位置を想定第ｎ床接点Ｄｎとする。目標歩容にお
いて第ｎ足平が離床した直後の時期では、離床時に想定
されていた目標第ｎ足平床反力中心点Ｑｎの位置を想定
第ｎ床接点Ｄｎとする。

【０２２５】次に、前記のθｆｄｂｖ”に、上体傾斜偏
差のＶ方向成分θｅｒｒｖを加えることにより、Ｑ１”
を始点、Ｑ２”を終点とするベクトルＱ１”Ｑ２”の目
標上体に対するベクトルＶまわりの傾きを求め、それか
ら前記のθｆｄｂｖを減じることにより、Ｖ方向瞬間両
脚間床傾斜偏差計算値θｆｄｂｅｒｒｖを得る。

【０２２６】次に、それから直前の推定両脚間床傾斜偏
差θｆｄｂｅｓｔｍｖを減じることにより、θｆｄｂｅ
ｒｒｄｉｆｆｖを得る。次に、両脚間床傾斜偏差推定中
断器１４０において、両脚間床傾斜偏差の推定を実行す
べきか中断すべきかを判断する。

【０２２７】両脚間床傾斜偏差推定中断器１４０は、両
脚間床傾斜偏差の推定を実行（続行）すべきと判断した
ならば、両脚間床傾斜偏差推定中断器１４０への入力で
ある前記θｆｄｂｅｒｒｄｉｆｆｖをそのまま出力し、
中断すべきと判断したならば、０を出力する。両脚間床
傾斜偏差推定中断器１４０に関する詳細は後述する。

【０２２８】次に、両脚間床傾斜偏差推定中断器１４０
の出力を積分定数がＫｄｂｅｓｔｍである積分器１４２
で積分することにより、推定両脚間床傾斜偏差θｆｄｂ
ｅｓｔｍｖを得る。推定両脚間床傾斜偏差θｆｄｂｅｓ
ｔｍｖは、図３１に示す両脚間床傾斜偏差の推定値であ
り、Ｖまわりの角度で表わされる。以上が、両脚間床傾
斜偏差推定部１３８の処理である。

【０２２９】さらに、推定両脚間床傾斜偏差θｆｄｂｅ
ｓｔｍｖは、図３０に示すように、両脚補償角θｄｂｖ
に加算され、床形状推定入り両脚補償角θｄｂｖ’とし
て修正目標足平位置姿勢算出部１１４ｇへ入力される。

【０２３０】以上の処理を毎制御周期に繰り返し実行す
ることにより、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍ
ｖが実際の両脚間床傾斜偏差に収束する。また、足平位
置姿勢は推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖに従
って修正され、両脚間床傾斜偏差が実床反力に及ぼす影
響を吸収する。

【０２３１】尚、両実足平が接地しているならば、前記
したＱｎ”は図３１の実第ｎ床接点Ｄｎａｃｔに一致す
るはずである。従って、前記ベクトルＱ１”Ｑ２”と前
記ベクトルＤ１Ｄ２の傾きの差θｆｄｂｅｒｒｖは、図
３０における両脚間床傾斜偏差に一致するはずである。

【０２３２】しかし、実際にはロボット１の機械系や制
御系の振動および電気的なノイズにより、上記計算アル
ゴリズムによって得られたθｆｄｂｅｒｒｖは、実際の
両脚間床傾斜偏差を中心値として激しく変動する。従っ
て、θｆｄｂｅｒｒｖをそのまま両脚間床傾斜偏差の推
定値として用いて、これを図３０に示すように両脚補償
角に加算すると、ロボット１の足平２２Ｒ（Ｌ）は発振
するかあるいは激しく振動する。

【０２３３】ところで、図３３の両脚間床傾斜偏差推定
中断器１４０が常に推定実行（続行）と判断すれば、θ
ｆｄｂｅｒｒｖから推定両脚間床傾斜偏差θｆｄｂｅｓ
ｔｍｖまでの伝達関数は、１次遅れのローパスフィルタ
になる。即ち、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍ
ｖはθｆｄｂｅｒｒｖにローパスフィルタを通したもの
となる。従って、推定両脚間床傾斜偏差θｆｄｂｅｓｔ
ｍｖを図３０に示すように両脚補償角に加算しても、足
平２２Ｒ（Ｌ）の発振や振動を生じ難くなる。

【０２３４】ちなみに、この例では、ローパスフィルタ
の時定数は１／Ｋｄｂｅｓｔｍである。図３３に示す例
以外にも、θｆｄｂｅｒｒｖからθｆｄｂｅｓｔｍｖま
での伝達関数がローパスフィルタになるように構成すれ
ば良い。これにより、足平２２Ｒ（Ｌ）の発振や振動を
防止することができる。

【０２３５】次いで、両脚間床傾斜偏差推定部１３８の
特徴を説明する。

【０２３６】姿勢安定化制御のために必要な両脚補償モ
ーメントＶ方向成分Ｍｄｍｄｄｂｖを発生させようと、
図３０における両脚補償角θｄｂｖを変化させ、最終的
な目標足平位置姿勢である機構変形補償入り修正目標足
平位置姿勢を変更させても、両実足平が接地している限
り、機構変形後の目標各足平床反力中心点Ｑｎ”の実際
の位置は、実第ｎ床接点Ｄｎａｃｔに一致したまま動か
ず、その代わりに、コンプライアンス機構４２が変形し
たり、上体２４の位置姿勢が変化する。即ち、機構変形
補償入り修正目標足平位置姿勢を変更させた分を打ち消
すように、コンプライアンス機構４２が変形したり、上
体２４の位置姿勢が変化する。

【０２３７】この実施の形態における両脚間床傾斜偏差
推定部１３８は、機構変形補償入り修正目標足平位置姿
勢から推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを算出
するまでに、実床反力を基に機構コンプライアンスモデ
ル１３４によって算出された機構部変形量と、傾斜セン
サ３６によって検出された実上体姿勢傾斜偏差を加えて
いるので、機構変形補償入り修正目標足平位置姿勢が変
動しても、推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは
その影響を受けない。

【０２３８】これは、姿勢制御と両脚間床傾斜偏差推定
が非干渉化されていることを意味する。従って、推定両
脚間床傾斜偏差θｆｄｂｅｓｔｍｖを図３０に示すよう
に両脚補償角に加算しても、制御系の安定余裕（発振の
し難さ）はほとんど低下しない。この点は、前記したロ
ボット学会論文に示す従来技術では得られない利点であ
る。

【０２３９】また、推定両脚間床傾斜偏差θｆｄｂｅｓ
ｔｍｖを図３０に示すように両脚補償角に加算すると、
両脚間床傾斜偏差が存在していても、その影響を打ち消
すことができるので、実全床反力は、定常的には想定床
を歩行している場合と同一になる。しかも、両脚間床傾
斜偏差の推定と補償角の修正は毎制御周期に実行される
ので、床形状が途中で変化しても、変化後の両脚間床傾
斜偏差を推定し、推定値に基づいてその影響を打ち消す
ことができる。

【０２４０】ただし、両脚間床傾斜偏差推定部１３８に
は前述したようにローパスフィルタが含まれるので、両
脚が接地したときや床形状が途中で変化したときには、
過渡的には、実全床反力は、両脚間床傾斜偏差の影響を
受ける。しかし、その後、時定数１／Ｋｄｂｅｓｔｍで
その影響が減衰する。

【０２４１】さらに、図３３に示すように、床形状推定
器１３０は足平床傾斜偏差推定部１４４を備える。即
ち、同図において、機構変形後の推定第１足平位置姿
勢、想定第１足平床傾斜および上体傾斜偏差Ｘ成分から
推定第１足平床傾斜偏差Ｘ成分を推定するまでのブロッ
クを「足平床傾斜偏差推定部」１４４、より具体的には
「第１足平床傾斜偏差Ｘ成分推定部」と呼ぶ。この部分
の処理を説明する。

【０２４２】先ず、機構変形後の推定第１足平位置姿勢
と想定第１足平床傾斜から、想定第１足平床傾斜に対す
る機構変形後の推定第１足平位置姿勢の傾斜のＸ成分θ
ｆ１ｅｒｒｘ”を算出する。

【０２４３】これは、目標第１足平姿勢傾斜と想定第１
足平床傾斜との差にコンプライアンス機構４２のたわみ
角（図２）を実床反力から推定した値を加算した値に相
当する。ただし、図２ではＹ成分を示す。

【０２４４】次に、前記のθｆ１ｅｒｒｘ”に、上体傾
斜偏差のＸ方向成分θｅｒｒｘを加えて瞬間第１足平床
傾斜偏差Ｘ成分計算値θｆ１ｅｒｒｘを得る。

【０２４５】次に、それから直前の推定第１足平床傾斜
偏差θｆ１ｅｓｔｍｘを減じることにより、θｆ１ｅｒ
ｒｄｉｆｆｘを得る。

【０２４６】次に、足平床傾斜偏差推定中断器１４６、
より具体的には第１足平床傾斜偏差Ｘ成分推定中断器に
おいて、第１足平床傾斜偏差Ｘ成分の推定を実行すべき
か中断すべきかを判断する。

【０２４７】第１足平床傾斜偏差Ｘ成分推定中断器は、
第１足平床傾斜偏差Ｘ成分の推定を実行すべきと判断し
たならば、第１足平床傾斜偏差Ｘ成分推定中断器への入
力である前記θｆ１ｅｒｒｄｉｆｆｘをそのまま出力
し、中断すべきと判断したならば、０を出力する。尚、
足平床傾斜偏差推定中断器１４６、より具体的には第１
足平床傾斜偏差Ｘ成分推定中断器に関する詳細は後述す
る。

【０２４８】次に、第１足平床傾斜偏差Ｘ成分推定中断
器の出力を積分定数がＫ１ｅｓｔｍｘである積分器１４
８で積分することにより、推定第１足平床傾斜偏差Ｘ成
分θｆ１ｅｓｔｍｘを得る。

【０２４９】以上が、第１足平床傾斜偏差Ｘ成分推定部
の処理である。

【０２５０】推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓ
ｔｍｘは、図３０に示すように、第１足平Ｘ補償角θ１
ｘに加算され、床形状推定入り第１足平Ｘ補償角θ１
ｘ’として修正目標足平位置姿勢算出部１１４ｇへ入力
される。

【０２５１】以上の処理を毎制御周期に繰り返し実行す
ることにより、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅ
ｓｔｍｘが実際の第１足平床傾斜偏差Ｘ成分に収束す
る。また、足平位置姿勢は、推定第１足平床傾斜偏差Ｘ
成分θｆ１ｅｓｔｍｘに従って修正され、第１足平床傾
斜偏差Ｘ成分が実床反力に及ぼす影響を吸収する。

【０２５２】尚、図３３では第１足平床傾斜偏差Ｘ成分
推定部１４４のみ図示したが、それ以外にも第１足平床
傾斜偏差Ｙ成分推定部、第２足平床傾斜偏差Ｘ成分推定
部、第２足平床傾斜偏差Ｙ成分推定部が設けられ、第１
足平床傾斜偏差Ｙ成分、第２足平床傾斜偏差Ｘ成分およ
び第２足平床傾斜偏差Ｙ成分に関しても同様の処理が行
われる。

【０２５３】足平床傾斜偏差推定に関しても、両脚間床
傾斜偏差推定と同様に以下のことが言える。

【０２５４】実第１足平がべたに接地しているならば
（より厳密には、実第１足平と実床が面接触している
か、あるいはつまさきエッジやかかとエッジで接触して
いるようにＹ方向に線接触しているならば）、瞬間第１
足平床傾斜偏差Ｘ成分計算値θｆ１ｅｒｒｘは、実際の
第１足平床傾斜偏差Ｘ成分に一致するはずである。

【０２５５】しかし、実際には、ロボット１の機械系や
制御系の振動および電気的なノイズにより、上記計算ア
ルゴリズムによって得られたθｆ１ｅｒｒｘは、実際の
第１足平床傾斜偏差Ｘ成分を中心値として激しく変動す
る。

【０２５６】従って、θｆ１ｅｒｒｘをそのまま第１足
平床傾斜偏差Ｘ成分の推定値として用い、それを図３０
に示すように第１足平Ｘ補償角に加算すると、ロボット
１の足平２２Ｒ（Ｌ）は、発振するかあるいは激しく振
動する。

【０２５７】ところで、図３３の第１足平床傾斜偏差Ｘ
成分推定中断器が常に推定実行（続行）と判断すれば、
θｆ１ｅｒｒｘから推定第１足平床傾斜偏差Ｘ成分θｆ
１ｅｓｔｍｘまでの伝達関数は、１次遅れのローパスフ
ィルタになる。即ち、推定第１足平床傾斜偏差Ｘ成分θ
ｆ１ｅｓｔｍｘはθｆ１ｅｒｒｘにローパスフィルタを
通したものとなる。

【０２５８】従って、推定第１足平床傾斜偏差Ｘ成分θ
ｆ１ｅｓｔｍｘを図３０に示すように第１足平補償角に
加算しても、足平の発振や振動を生じ難くなる。ちなみ
に、この例では、ローパスフィルタの時定数は１／Ｋ１
ｅｓｔｍｘである。

【０２５９】図３３に示す例以外にも、両脚間床傾斜偏
差推定と同様に、θｆ１ｅｒｒｘからθｆ１ｅｓｔｍｘ
までの伝達関数がローパスフィルタになるように構成す
れば良い。これにより、足平２２Ｒ（Ｌ）の発振や振動
を防止することができる。

【０２６０】次いで、足平床傾斜偏差推定部１４４の特
徴を説明する。

【０２６１】両脚間床傾斜偏差推定と同様に以下のこと
が言える。

【０２６２】姿勢安定化制御のために必要な第１足平補
償モーメントＸ成分Ｍｄｍｄ１ｘを発生させようと、図
３０における第１足平補償角θ１ｘを変化させて、最終
的な目標足平位置姿勢である機構変形補償入り修正目標
足平位置姿勢を変更させても、実第１足平がべたに接地
している限り、実足底面の姿勢は変わらず、その代わり
に、コンプライアンス機構４２が変形したり、上体２４
の位置姿勢が変化する。即ち、両脚間床傾斜偏差推定と
同様に、機構変形補償入り修正目標足平位置姿勢を変更
させた分を打ち消すように、コンプライアンス機構４２
が変形したり、上体２４の位置姿勢が変化する。

【０２６３】この実施の形態における第１足平床傾斜偏
差Ｘ成分推定部は、機構変形補償入り修正目標足平位置
姿勢から推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍ
ｘを算出するまでに、実床反力を基に機構コンプライア
ンスモデル１３４によって算出されたコンプライアンス
機構変形量と、傾斜センサ３６によって検出された実上
体姿勢傾斜偏差を加えているので、機構変形補償入り修
正目標足平位置姿勢が変動しても、推定第１足平床傾斜
偏差Ｘ成分θｆ１ｅｓｔｍｘはその影響を受けない。

【０２６４】これは、姿勢制御と第１足平床傾斜偏差推
定が非干渉化されていることを意味する。従って、推定
第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを図３０に
示すように第１足平補償角に加算しても、両脚間床傾斜
偏差推定と同様に、制御系の安定余裕はほとんど低下し
ない。

【０２６５】また、推定第１足平床傾斜偏差Ｘ成分θｆ
１ｅｓｔｍｘを図３０に示すように第１足平補償角に加
算すると、第１足平床傾斜偏差が存在していても、その
影響を打ち消すことができるので、実第１足平床反力
は、定常的には想定床を歩行している場合と同一にな
る。

【０２６６】しかも、第１足平床傾斜偏差の推定と補償
角の修正は毎制御周期に実行されるので、床形状が途中
で変化しても、変化後の第１足平床傾斜偏差を推定し、
その影響を打ち消すことができる。

【０２６７】ただし、第１足平床傾斜偏差推定器には前
記のローパスフィルタが含まれるので、第１足平が接地
したときや床形状が途中で変化したときには、過渡的に
は、第１足平床傾斜偏差の影響を受け、その後、時定数
１／Ｋ１ｅｓｔｍｘでその影響が減衰する。

【０２６８】上記したこの実施の形態における床形状推
定の特徴を総合的に説明する。

【０２６９】この実施の形態における床形状推定が前提
とする複合コンプライアンス制御においては、各補償角
を操作して実全床反力および各足平床反力を制御して
も、その特徴として、それぞれの制御は非干渉化されて
いる。

【０２７０】従って、推定両脚間床傾斜偏差θｆｄｅｓ
ｔｍｖを求めることと、それを両脚補償角θｄｂｖに加
算することと、推定各足平床傾斜偏差θｆ１ｅｓｔｍ
ｘ，θｆ１ｅｓｔｍｙ，θｆ２ｅｓｔｍｘ，θｆ２ｅｓ
ｔｍｙを求めることと、それらをそれぞれ各足平補償角
θ１ｘ，θ１ｙ，θ２ｘ，θ２ｙに加算することを同時
に行っても、干渉し合って発振することはほとんどな
い。

【０２７１】その特徴と前述した両脚間床傾斜偏差推定
部１３８と足平床傾斜偏差推定部１４４の各特徴により
結果的に以下の利点が得られる。

【０２７２】１）姿勢復元力制御のために両脚補償角θ
ｄｂｖと各足平補償角θ１ｘ，θ１ｙ，θ２ｘ，θ２ｙ
を操作して実床反力を制御することと、両脚間床傾斜偏
差を推定することと、これを両脚補償角θｄｂｖに加算
することと、各足平床傾斜偏差を推定することと、それ
らを各足平補償角に加算することを同時に行うことがで
きる。

【０２７３】簡単に言えば、床形状偏差の複数のパラメ
ータを同時に推定しながら、床形状偏差が床反力に及ぼ
す影響を打ち消すための脚の補償動作も同時に実行する
ことができる。さらには、姿勢制御のための脚の補償動
作も同時に実行することができる。

【０２７４】２）床形状偏差が大きくても制御が破綻し
難い（複合コンプライアンス制御は微小角に限定した近
似を行っていないから）。 ３）床形状偏差の推定精度が高い。

【０２７５】４）姿勢安定化制御との干渉による発振が
生じにくいので、推定部１３８，１４４のゲイン（Ｋｄ
ｂｅｓｔｍ，Ｋ１ｅｓｔｍｘ，Ｋ１ｅｓｔｍｙ，Ｋ２ｅ
ｓｔｍｘ，Ｋ２ｅｓｔｍｙ）を大きくすることができ、
その結果、推定時定数を短くすることができる。それに
よって、床形状偏差が床反力に及ぼす影響を打ち消すた
めの脚の補償動作の応答性を高めることができるので、
床形状偏差が床反力に及ぼす過渡的な影響が短時間で解
消される。 ５）床形状偏差推定と床形状偏差が床反力に及ぼす影響
を打ち消すための脚の補償動作が常時実行されるので、
途中で床形状が変化しても、過渡的にはその影響を受け
るが、その後、推定の時定数でその影響が減衰する。

【０２７６】次いで、この実施の形態における前記した
推定中断器１４０，１４６について説明する。

【０２７７】先ず、第１足平床傾斜偏差Ｙ成分推定中断
器の役割について説明する。

【０２７８】第１足平床傾斜偏差推定Ｙ成分中断器は、
第１足平床傾斜偏差Ｙ成分の推定が正常に実行できるか
否かを判断し、正常に実行できないと判断したときには
推定を中断する。

【０２７９】より具体的には、原則的に、推定第１足平
床傾斜偏差Ｙ成分の精度が低下する恐れがある状況、お
よび推定第１足平床傾斜偏差Ｙ成分が発散する恐れがあ
る状況では、推定を中断するために、０を出力し（即
ち、積分入力に０を入れて積分値をホールドする）、そ
れ以外の状況では、推定を実行するために、第１足平床
傾斜偏差Ｙ成分推定中断器への入力をそのまま出力す
る。

【０２８０】もし、第１足平床傾斜偏差Ｙ成分推定中断
器が動作せず、常に第１足平床傾斜偏差Ｙ成分推定中断
器への入力をそのまま出力し続けると、ある状況では推
定第１足平床傾斜偏差Ｙ成分が発散する。

【０２８１】以下にこの実施の形態において推定第１足
平床傾斜偏差Ｙ成分が発散する状況を説明する。

【０２８２】例として、図３６に示すように、ロボット
１が片脚支持期に前傾したために、姿勢安定化制御によ
って第１足平Ｙ補償角を操作して大きな姿勢復元力を得
ようと試みた結果、本来べたに接地すべき実第１足平の
かかとが床から浮いてしまった状況を仮定する。また、
このとき実床形状は想定床形状と一致していると仮定す
る。

【０２８３】この状況では、図３６に示される想定第１
足平床傾斜Ｙ成分に対する機構変形後の推定第１足平姿
勢の傾斜Ｙ成分θｆ１ｅｒｒｙ”に上体傾斜偏差Ｙ成分
θｅｒｒｙを加えたθｆ１ｅｒｒｙは、同図に示すよう
に、正の値になる。

【０２８４】第１足平床傾斜偏差Ｙ成分推定中断器が、
常に入力であるθｆ１ｅｒｒｄｉｆｆｙをそのまま出力
し続ける場合には、前述したように、θｆ１ｅｒｒｙか
ら推定両脚間床傾斜偏差θｆ１ｅｓｔｍｙまでの伝達関
数は、１次遅れになる。

【０２８５】即ち、θｆ１ｅｓｔｍｙは、θｆ１ｅｒｒ
ｙにやや遅れて追従する。θｆ１ｅｓｔｍｙは、第１足
平Ｙ補償角θ１ｙに加算され、床形状推定入り第１足平
Ｙ補償角θ１ｙ’になる。図３０ではθ１ｘ’のみ図示
しているが、θ１ｙも同様な構成になる。

【０２８６】次に、目標第１足平位置姿勢に床形状推定
入り第１足平Ｙ補償角θ１ｙ’が加えられ、さらに目標
床反力（目標歩容の床反力）から算出される機構変形量
が減じられ（即ち、機構変形量を打ち消す機構変形補償
量が加えられ）、機構変形補償入り修正目標第１足平位
置姿勢が算出される。

【０２８７】さらに機構コンプライアンスモデル１３４
によって、実床反力から計算される機構変形量が求めら
れ、機構変形補償入り修正目標第１足平位置姿勢に加え
られて機構変形後の推定第１足平位置姿勢が求められ
る。この傾きＹ成分から想定第１足平傾斜のＹ成分が減
じられ、さらに上体傾斜偏差Ｙ成分θｅｒｒｙが加えら
れることにより、前記θｆ１ｅｒｒｙの値が更新され
る。

【０２８８】以上の関係から、θｆ１ｅｒｒｙとこれに
１次遅れで追従するθｆ１ｅｓｔｍｙの関係は以下のよ
うになる。尚、目標床反力による第１足平機構変形の補
償角は、先に段落０１３５で述べたコンプライアンス機
構４２の変形量を打ち消す補償量である。 θf1erry＝θf1estmy ＋θ1y＋目標床反力による第１足平機構変形の補償角Y成分＋θerry ＋実床反力から計算される第１足平機構変形角Y成分 ＋目標第１足平姿勢傾斜Y成分−想定第１足平傾斜Y成分 ・・・式２２

【０２８９】目標歩容の片足支持期においては、（目標
第１足平姿勢傾斜Ｙ成分−想定第１足平傾斜のY成分）
は０である。従って、式２２は次式に変形される。 θf1erry＝θf1estmy ＋θ1y＋目標床反力による第１足平機構変形の補償角Y成分＋θerry ＋実床反力から計算される第１足平機構変形角Y成分 ・・・式２３

【０２９０】第１足平が床にべたに接地しているなら
ば、（θｆ１ｅｓｔｍｙ＋θ１ｙ＋目標床反力による第
１足平機構変形の補償角Ｙ成分＋θｅｒｒｙ）がいかな
る値であっても、実床反力から計算される機構変形角Ｙ
成分はそれを打ち消すので、式２３の右辺は０になる。
従って、θｆ１ｅｒｒｙは常に０となり、また、θｆ１
ｅｓｔｍｙは、θｆ１ｅｒｒｙに１次遅れで追従するの
で、０に徐々に収束する。これは、実床形状が想定床形
状と一致している状況を推定器が正確に推定しているこ
とを意味する。

【０２９１】ところが、図３６の状況では（θｆ１ｅｓ
ｔｍｙ＋θ１ｙ＋目標床反力による第１足平機構変形の
補償角Ｙ成分＋θｅｒｒｙ）は大きな正の値となるが、
かかとが浮いてしまったために実床反力が十分に大きく
なれず、実床反力から計算される機構変形角Ｙ成分は、
前記の値を打ち消すだけの十分な大きさの負の値を持つ
ことができない。即ち、式２３の右辺は０にならない。
このような状況では、θｆ１ｅｒｒｙは０になれず、θ
ｆ１ｅｓｔｍｙは０に収束しない。これは、推定値が間
違っていることを意味する。

【０２９２】このような状況の中でも、特に次の式２４
を満足している状況が続く場合には、θｆ１ｅｒｒｙに
近づこうとしてθｆ１ｅｓｔｍｙが増大しても、θｆ１
ｅｒｒｙは、式２３と次の式２４から導かれるように、
θｆ１ｅｒｒｙ＞θｆ１ｅｓｔｍｙの関係を保つので、
θｆ１ｅｓｔｍｙから離れてしまう。

【０２９３】これを繰り返すうちに、θｆ１ｅｓｔｍｙ
は正の方向にますます発散する。これは、推定値が発散
しつつあることを意味する。 θ1y＋目標床反力による第１足平機構変形の補償角Y成分＋θerry ＋実床反力から計算される第１足平機構変形角Y成分 > 0 ・・・式２４

【０２９４】次いで、図３７を参照して第１の実施の形
態における第１足平床傾斜偏差Ｙ成分推定中断器の処理
を説明する。この処理は毎制御周期に実行される。

【０２９５】以下に説明すると、先ずＳ３００において
目標歩容の時期に応じて第１足平床傾斜Ｙ成分推定モー
ドを決定する。

【０２９６】具体的には、第１足平床傾斜Ｙ成分推定モ
ードは、リセットモード、ホールドモードおよび準備完
了モードの３つのモード状態を取りうるものとし、目標
歩容の時間経過に対して、図３８に示す如く遷移するよ
うに決定する。ただし、これは図５に示す歩容に対する
モード遷移パターンである。歩容が異なれば、それに応
じてモード遷移パターンも変えるべきである。

【０２９７】従ってＳ３０２に進み、第１足平床傾斜Ｙ
成分推定モードに応じて以下の処理を行う。

【０２９８】第１足平床傾斜Y成分推定モードがリセッ
トモードである場合はＳ３０４に進み、推定第１足平床
傾斜偏差Ｙ成分θｆ１ｅｓｔｍｙを強制的に徐々に０に
収束させ、Ｓ３０６に進み、第１足平床傾斜偏差Ｘ成分
推定中断器の出力を０にする。

【０２９９】尚、リセットモードは、図３８に示す如
く、第１足平を動かしても床反力に影響がないように、
第１足平が床から完全に離れている期間に存在させる。
１歩前に着地していた第２床接点の足平床傾斜偏差と次
に着地する第１床接点の足平床傾斜偏差との関係に相関
がない場合には、上記のように推定第１足平床傾斜偏差
Ｙ成分θｆ１ｅｓｔｍｙを強制的に徐々に０に収束させ
るのが良い。

【０３００】しかし、例えば、想定床に考慮されていな
い実床面のうねりがゆったりとしていることがわかって
いるならば、前記関係にある程度の相関があると考えら
れるので、推定第１足平床傾斜偏差Ｙ成分θｆ１ｅｓｔ
ｍｙを、強制的に徐々に１歩前に着地していた第２床接
点での推定第２足平床傾斜偏差に１より小さい正の定数
を乗じた値に収束させるようにしても良い。

【０３０１】さらに、１歩前の推定足平床傾斜偏差Y成
分だけでなく、複数歩前の推定足平床傾斜偏差Y成分も
用いて収束目標値を決定しても良い。また、１歩前や複
数歩前の推定両脚間床傾斜偏差も用いて収束目標値を決
定しても良い。

【０３０２】第１足平床傾斜Ｙ成分推定モードがホール
ドモードである場合はＳ３０８に進み、第１足平床傾斜
偏差Ｙ成分推定中断器の出力を０にする。従って、推定
第１足平床傾斜偏差Ｙ成分θｆ１ｅｓｔｍｙはホールド
モードに遷移する直前の値に保持される。

【０３０３】尚、ホールドモードは、推定第１足平床傾
斜偏差Ｙ成分θｆ１ｅｓｔｍｙが発散する恐れがある時
期およびその精度が低下する恐れがある時期に存在させ
る。例えば、図３８に示すように、第１足平のかかとま
たはつまさきが床から離れ始める時期から第１足平が床
から完全に離れてしばらく後までの期間に存在させる。

【０３０４】上記以外の時期、即ち、第１足平床傾斜偏
差Ｙ成分の推定が正常に実行できる時期では、第１足平
床傾斜Ｙ成分推定モードは準備完了モードになる。

【０３０５】準備完了モードでは、以下の処理を行う。

【０３０６】即ち、Ｓ３１０に進み、実第１足平床反力
モーメントＭ１ａｃｔと実第１足平床反力の力成分Ｆ１
ａｃｔから実第１足平床反力中心点位置Ｑ１ａｃｔを求
める。ここで、実第１足平床反力中心点位置Ｑ１ａｃｔ
は、その点における実第１足平床反力モーメントの内の
鉛直方向を除く方向の成分が０となる足底上の作用点で
ある。実第１足平床反力中心点Ｑ１ａｃｔのＸ座標をＱ
１ａｃｔｘとする。

【０３０７】次いでＳ３１２に進み、あらかじめ設定し
ておいた第１足平推定許容最小床反力Ｆ１ｍｉｎ、第１
足平推定許容領域最大値Ｑ１ｍａｘｘおよび第１足平推
定許容領域最小値Ｑ１ｍｉｎｘに対し、実第１足平床反
力Ｚ成分（鉛直成分）Ｆ１ａｃｔｚと前記Ｑ１ａｃｔｘ
が以下の条件を満たすか否かを調べる。

【０３０８】条件１）F1actz < F1min 条件２）Q1actx > Q1maxx かつθf1errdiffy > 0 条件３）Q1actx < Q1minx かつθf1errdiffy < 0

【０３０９】条件１に関して補足すると、第１足平推定
許容最小床反力Ｆ１ｍｉｎは、第１足平にそれ以下の床
反力しか作用していない場合には第１足平床傾斜が推定
できないと判断される足平床反力設定値であり、ロボッ
ト１の自重より小さな値に設定しておく。換言すれば、
条件１は、第１足平と床との間の圧力が小さ過ぎて第１
足平床傾斜Ｙ成分が精度良く推定できない状況を意味す
る。

【０３１０】一方、条件２および条件３に関しては、足
平の座標系から見て、つまさきのＸ座標をｘｔｏｅ、か
かとのX座標をｘｈｅｅｌとすると、あらかじめ、第１
足平推定許容領域最大値Ｑ１ｍａｘｘの値を、ｘｔｏｅ
よりもやや小さめに設定しておき、第１足平推定許容領
域最小値Ｑ１ｍｉｎｘの値を、ｘｔｏｅよりもやや大き
めに設定しておく。これらの関係を図３９に示す。

【０３１１】推定が理想的に実行されるならば、第１足
平推定許容領域最大値Ｑ１ｍａｘｘと第１足平推定許容
領域最小値Ｑ１ｍｉｎｘは、エッジに一致させても良
い。しかし実際には検出誤差が生じ、これにより推定値
が発散する場合がある。そこでこれを防ぐために、図３
９のようにエッジからやや内側に第１足平推定許容領域
最大値Ｑ１ｍａｘｘと第１足平推定許容領域最小値Ｑ１
ｍｉｎｘを設定しておく。

【０３１２】条件２は、かかとが浮きかかっているの
に、積分器入力θｆ１ｅｒｒｄｉｆｆｙが正、即ち、推
定第１足平床傾斜がかかとが浮く方向に変化しつつある
状況を意味する。このまま推定を続行すると、床形状推
定入り第１足平Ｙ補償角が、ますますかかとが浮く方向
に変化する。

【０３１３】条件３は、つまさきが浮きかかっているの
に、積分器入力θｆ１ｅｒｒｄｉｆｆｙが負、即ち、推
定第１足平床傾斜がつまさきが浮く方向に変化しつつあ
る状況を意味する。このまま推定を続行すると、床形状
推定入り第１足平Ｙ補償角が、ますますつまさきが浮く
方向に変化する。

【０３１４】図３７フローチャートの説明に戻ると、Ｓ
３１２で肯定されて条件１、条件２または条件３のいず
れかを満足する場合には、推定を中断すべきであるの
で、Ｓ３１４に進んで第１足平床傾斜偏差Ｙ成分推定中
断器の出力を０にし、それ以外の場合には、推定を実行
しても問題がないので、Ｓ３１６に進んで前記出力をθ
ｆ１ｅｒｒｄｉｆｆｙにする。

【０３１５】条件１、条件２および条件３の式は、以上
に述べた状況を表わしていれば良い。従って、条件１で
は、実第１足平床反力Ｚ成分（鉛直成分）Ｆ１ａｃｔｚ
の代わりに、実第１足平床反力の力成Ｆ１ａｃｔの内の
足平に垂直な成分（この成分を「Ｆ１ａｃｔｚ’」と呼
ぶ）を用いても良い。

【０３１６】また、条件２および条件３では、前記実第
１足平床反力中心点位置Ｑ１ａｃｔの代わりに、実第１
足平床反力モーメントの内の足平に垂直な方向を除く方
向の成分が０となる足底上の作用点（この点を「Ｑ１ａ
ｃｔ’」と呼ぶ）を用いても良い。

【０３１７】また、この実施の形態に係る第１足平床傾
斜偏差Ｘ成分推定中断器の処理について説明すると、第
１足平床傾斜偏差Ｘ成分推定中断器も、第１足平床傾斜
偏差Ｙ成分推定中断器とほぼ同様の処理を行う。

【０３１８】相違点は、前記条件２および条件３におい
て、つまさきまたはかかとが浮く条件の代わりに足平の
内エッジ、外エッジが浮く条件を用いることである。即
ち、条件２と条件３の代わりに、以下の条件２’と条件
３’を用いる。

【０３１９】ここでＱ１ｍａｘｙおよびＱ１ｍｉｎｙ
は、図３９のように足平内エッジと外エッジの足平中心
側に設定される。 条件２’）Q1acty > Q1maxy かつθf1errdiffx < 0 条件３’）Q1acty < Q1miny かつθf1errdiffx > 0

【０３２０】尚、座標系の向きの関係上、不等号が、条
件２、条件３の場合と一部異なる。また、第１足平床傾
斜偏差Ｘ成分推定モードの遷移タイミングは、第１足平
床傾斜偏差Ｙ成分推定モードの場合と較べ、準備完了モ
ードの終了時期をもう少し遅らせても良い。つまさきエ
ッジが接地していれば、まだ第１足平床傾斜偏差Ｘ成分
が推定できるからである。

【０３２１】また、第２足平床傾斜偏差推定中断器の処
理も、第１足平床傾斜偏差推定中断器の処理と同様であ
る。

【０３２２】次いで、図４０を参照して両脚間床傾斜偏
差推定中断器１４０の処理を説明する。この処理は毎制
御周期に実行される。

【０３２３】この処理は、第１足平床傾斜偏差Ｙ成分推
定中断器と基本的には同じであるが、細部が異なる。

【０３２４】以下説明すると、先ずＳ４００において目
標歩容の時期に応じて両脚間床傾斜推定モードを決定す
る。

【０３２５】具体的には、両脚間床傾斜推定モードは、
目標歩容の時間経過に対して、図４１のように決定す
る。ただし、これも図５に示す歩容に対するモードパタ
ーンであり歩容が異なれば、それに応じてモードパター
ンも変えるべきである。

【０３２６】次いでＳ４０２に進み、両脚間床傾斜推定
モードに応じて以下のように処理する。

【０３２７】両脚間床傾斜推定モードがリセットモード
である場合はＳ４０４に進み、推定両脚間床傾斜偏差θ
ｆｄｂｅｓｔｍｖを強制的に徐々に０に収束させ、Ｓ４
０６に進んで両脚間床傾斜偏差推定中断器の出力を０に
する。尚、リセットモードは、図４１に示すごとく、床
形状推定入り両脚補償角を動かしても全床反力に影響が
ないように、片脚支持期に存在させる。

【０３２８】１歩前の両脚間床傾斜偏差と次の１歩の両
脚間床傾斜偏差との関係に相関がない場合には、このよ
うに推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖを強制的
に徐々に０に収束させるのが良い。しかし、例えば、想
定床に考慮されていない実床面のうねりがゆったりとし
ていることが分かっているならば、前記関係にある程度
の相関があると考えられるので、推定両脚間床傾斜偏差
θｆｄｂｅｓｔｍｖを、強制的に徐々に１歩前の両脚間
床傾斜偏差に１より小さい正の定数を乗じた値に収束さ
せるようにしても良い。

【０３２９】さらに、１歩前の推定両脚間床傾斜偏差だ
けでなく、複数歩前の推定両脚間床傾斜偏差も用いて収
束目標値を決定しても良い。また、１歩前や複数歩前の
推定足平床傾斜偏差も用いて収束目標値を決定しても良
い。

【０３３０】両脚間床傾斜推定モードがホールドモード
である場合はＳ４０８に進み、両脚間床傾斜偏差推定中
断器の出力を０にする。従って、推定両脚間床傾斜偏差
θｆｄｂｅｓｔｍｖは、ホールドモードに遷移する直前
の値に保持される。

【０３３１】尚、ホールドモードは、推定両脚間床傾斜
偏差θｆｄｂｅｓｔｍｖが発散する恐れがある時期およ
びその精度が低下する恐れがある時期に存在させる。例
えば、図４１に示すように、両脚支持期の終了時点を始
めとしてそれからしばらく後までの期間に存在させる。

【０３３２】上記以外の時期、即ち、両脚間床傾斜偏差
の推定が正常に実行できる時期では、両脚間床傾斜偏差
モードは準備完了モードになる。準備完了モードでは、
以下の処理を行う。

【０３３３】先ずＳ４１０に進み、あらかじめ設定して
おいた両脚間床傾斜推定許容最小床反力Ｆｍｉｎ、両脚
間床傾斜推定許容比ｒｍｉｎに対し、実各足平床反力が
以下の条件を満たすか否かを調べる。 条件４）F1actz < Fmin または F2actz < Fmin 条件５）F2actz / ( F1actz ＋F2actz ) < rmin かつθ
fdberrdiffv > 0 条件６）F1actz / ( F1actz ＋F2actz ) < rmin かつθ
fdberrdiffv < 0

【０３３４】条件４に関して補足すると、両脚間床傾斜
推定許容最小床反力Ｆｍｉｎは、第１足平または第２足
平にこれ以下の床反力しか作用していない場合には、両
脚間床傾斜が推定できないと判断される足平床反力設定
値であり、通常、ロボット１の自重より小さな値に設定
しておく。換言すれば、条件４は、第１足平または第２
足平と床との間の圧力が小さ過ぎて両脚間床傾斜が精度
良く推定できない状況を意味する。

【０３３５】一方、条件５および条件６に関しては、推
定が理想的に実行されるならば、両脚間床傾斜推定許容
比ｒｍｉｎは０でも良い。しかし実際には検出誤差が生
じ、それにより推定値が発散する場合がある。それを防
ぐために、ｒｍｉｎには、１よりかなり小さい正の値を
設定しておく。

【０３３６】条件５は、第２足平が浮きかかっているの
に、積分器入力θｆｄｂｅｒｒｄｉｆｆｖが正、即ち、
推定両脚間床傾斜が第２足平が浮く方向に変化しつつあ
る状況を意味する。このまま推定を続行すると、床形状
推定入り両脚補償角は、ますます第２足平が浮く方向に
変化する。

【０３３７】条件６は、第１足平が浮きかかっているの
に、積分器入力θｆｄｂｅｒｒｄｉｆｆｖが負、即ち、
推定両脚間床傾斜が第１足平が浮く方向に変化しつつあ
る状況を意味する。このまま推定を続行すると、床形状
推定入り両脚補償角は、ますます第１足平が浮く方向に
変化する。

【０３３８】図４０フローチャートの説明に戻ると、条
件４、条件５または条件６のいずれかを満足する場合に
は推定を中断すべきであるので、Ｓ４１２に進んで両脚
間床傾斜偏差推定中断器の出力を０にし、それ以外の場
合には推定を実行しても問題がないので、Ｓ４１４に進
んで前記出力をθｆｄｂｅｒｒｄｉｆｆｖにする。

【０３３９】条件４、条件５および条件６の式は、以上
に述べた状況を表わしていれば良い。従って、実第ｎ足
平床反力Ｚ成分（鉛直成分）Ｆｎａｃｔｚの代わりに、
実第ｎ足平床反力の力成分Ｆｎａｃｔの内の足平に垂直
な成分Ｆｎａｃｔ’を用いても良い。

【０３４０】この実施の形態は上記の如く構成したの
で、従来技術では困難であった床形状の推定、具体的に
は各足平の接地面傾斜と両足平接地面間の高低差を同時
に、換言すれば複合的に、精度良く推定することができ
る。

【０３４１】さらには、床形状の推定結果を基に足平軌
道を修正することにより、床形状が想定していた形状と
異なっていても、その影響を吸収して望み通りの床反力
を発生させることができる。

【０３４２】特に、複合コンプライアンス制御で除去し
きれなかった実床反力の制御目標値からの定常偏差を可
能な限り零に近づける、換言すれば床形状のずれに起因
する床反力の定常偏差を解消することができる。

【０３４３】さらに、推定精度が低下する恐れがある状
況あるいは推定値が発散する恐れがある状況では床形状
推定を中断するように構成したので、前記した作用効果
に加え、推定値が不適正なものとなることがない。

【０３４４】尚、この実施の形態に係る脚式移動ロボッ
トの床形状推定装置は上記した手法で床形状を推定すれ
ば足り、推定値に基づいて床反力を操作して目標位置姿
勢などを制御することは必須ではない。

【０３４５】次いで、この発明の第２の実施の形態に係
る脚式移動ロボットの床形状推定装置を説明する。

【０３４６】第２の実施の形態は、第１の実施の形態を
簡易化したものであり、その全体構成を図４２に示す。
第１の実施の形態とは、床形状推定器への入力と床形状
推定器内での処理が異なる。それ以外は第１の実施の形
態と同様である。

【０３４７】第２の実施の形態においては、詳細は以下
に説明するが、簡単に言えば、第１の実施の形態の機構
コンプライアンスモデル１３４を近似してから、ブロッ
ク線図を等価変換するようにした。

【０３４８】第２の実施の形態に係る床形状推定器の処
理を説明する前に、先ず、用語の定義と、第２の実施の
形態で用いられるコンプライアンスモデルの説明を行
う。

【０３４９】図４３の上部に示すように、機構変形補償
入り修正目標足平位置姿勢と目標上***置姿勢の相対関
係を満足するように関節角を変位制御し、同図中部に示
すように、目標上体姿勢から上体傾斜偏差θｅｒｒだけ
傾けてロボット１の上体２４を空中に支持し、さらに同
図下部に示すように、目標各足平床反力に相当する外力
を足平に作用させたと仮定したときに、そのような状況
にある足平内の目標第１足平床反力中心点Ｑ１"'を始
点、目標第２足平床反力中心点Ｑ２"'を終点とするベク
トルの前記Ｖ方向まわりの傾きを「理想変形時両脚間傾
斜」と呼ぶ。

【０３５０】また、この仮定における第ｎ足平の姿勢傾
斜を「理想変形時第ｎ足平傾斜」と呼ぶ。上体傾斜偏
差、両脚補償角、各足平補償角、推定両脚間床傾斜偏
差、推定第１足平床傾斜偏差および推定第２足平床傾斜
偏差がすべて０であるならば、理想変形時両脚間傾斜は
想定両脚間床傾斜に一致し、理想変形時第ｎ足平傾斜は
想定第ｎ足平床傾斜に一致する。

【０３５１】想定両脚間床傾斜に推定両脚間床傾斜偏差
を加算したものを「推定両脚間床傾斜」と呼ぶ。これ
は、Ｖ方向まわりの角度で表わされる。また、想定第ｎ
足平床傾斜に推定第ｎ足平床傾斜偏差を加算したものを
「推定第ｎ足平床傾斜」と呼ぶ。理想変形時両脚間傾斜
と推定両脚間床傾斜との差を「推定両脚干渉角θｄｂｉ
ｎｔｖ」と呼ぶ。これも、Ｖ方向まわりの角度で表わさ
れる。

【０３５２】また、理想変形時第１足平傾斜と推定第１
足平床傾斜との差を「推定第１足平干渉角θ１ｉｎｔ」
と呼ぶ。同様に理想変形時第２足平傾斜と推定第２足平
床傾斜との差を「推定第２足平干渉角θ２ｉｎｔ」と呼
ぶ。

【０３５３】推定両脚間床傾斜が実両脚間床傾斜に一致
していて、かつ実ロボットの足平の目標各足平床反力中
心点が接地していると仮定すると、推定両脚干渉角θｄ
ｂｉｎｔｖを打ち消すように、実ロボットのコンプライ
アンス機構４２が変形しているはずである。

【０３５４】さらにこの機構変形に応じて、目標全床反
力中心点まわりに各足平床反力の力成分が作るモーメン
トの和のＶ方向成分が発生するはずである。この成分を
「Ｍｄｂｅｓｔｍｖ」と呼ぶ。

【０３５５】そこで、推定両脚干渉角θｄｂｉｎｔｖと
Ｍｄｂｅｓｔｍｖとの関係をあらかじめ調べておき、記
憶しておく。この関係は、実ロボットのコンプライアン
ス機構４２の特性の内で、両脚干渉角に関する特性だけ
を抽出したものであり、それ故に、「両脚間機構コンプ
ライアンスモデル」１３４ａ（図４４に示す）と呼ぶ。

【０３５６】推定第ｎ足平床傾斜が実第ｎ足平床傾斜に
一致していて、かつ実ロボットの第ｎ足平がべたに接地
していると仮定すると、推定第ｎ足平干渉角θｎｉｎｔ
を打ち消すように、実ロボットのコンプライアンス機構
４２が変形しているはずである。

【０３５７】さらにこの機構変形に応じて、目標第１足
平床反力中心点まわりに第１足平床反力モーメントが発
生するはずである。このモーメントを「Ｍ１ｅｓｔｍ」
と呼ぶ。同様に目標第２足平床反力中心点まわりに第２
足平床反力モーメントが発生するはずである。このモー
メントを「Ｍ２ｅｓｔｍ」と呼ぶ。

【０３５８】そこで、推定第ｎ足平干渉角のＸ成分θｎ
ｉｎｔｘと前記モーメントのＸ成分Ｍｎｅｓｔｍｘとの
関係をあらかじめ調べておき、記憶しておく。この関係
は、実ロボットのコンプライアンス機構４２の特性の内
で、足平干渉角に関する特性だけを抽出したものであ
り、それ故に、「足平機構コンプライアンスモデル」と
呼ぶ。第１足平Ｘ成分用足平機構コンプライアンスモデ
ルを図４５に記載の１３４ｂに示す。

【０３５９】次に、第２の実施の形態における床形状推
定器１３０ａの構成を説明する。

【０３６０】第２の実施の形態における床形状推定器１
３０ａは、両脚間床傾斜偏差推定部１３８ａと足平床傾
斜偏差推定部１４４ａとから構成される。両脚間床傾斜
偏差推定部１３８ａの処理機能ブロック図を図４４に示
す。足平床傾斜偏差推定部１４４ａの処理機能ブロック
図を図４５に示す。

【０３６１】図４５では、推定第１足平床傾斜偏差Ｘ成
分に関する処理機能ブロック図のみを示すが、推定第１
足平床傾斜偏差Ｙ成分、推定第２足平床傾斜偏差Ｘ成
分、推定第２足平床傾斜偏差Ｙ成分についても同様の処
理を行う。

【０３６２】先ず、第２の実施の形態に係る両脚間床傾
斜偏差推定部１３８ａの処理を図４４を参照して説明す
る。

【０３６３】先ず、上体傾斜偏差θｅｒｒをＶ方向成分
とそれと直交する方向の成分に分解したときのＶ方向成
分θｅｒｒｖを求める。

【０３６４】次に、目標歩容の目標第１足平床反力中心
点Ｑ１を始点、目標第２足平床反力中心点Ｑ２を終点と
するベクトルの傾斜を求め、求めた傾斜から想定両脚間
床傾斜を減じたものを想定両脚干渉角θｄｂｉｎｔｃｍ
ｄｖとする。

【０３６５】次に、次式により、前述の推定両脚干渉角
θｄｂｉｎｔｖを求める。 θdbintv＝θdbv’＋θerrv−θfdbestmv＋θdbintcmdv ・・・式２５

【０３６６】ここで、θｆｄｂｅｓｔｍｖは、第１の実
施の形態でも述べたように、推定両脚間床傾斜偏差であ
る。また、θｄｂｖ’は、床形状推定入り両脚補償角で
あり、図４３に示すように、両脚補償角θｄｂｖと推定
両脚間床傾斜偏差θｆｄｂｅｓｔｍｖの和である。

【０３６７】床形状推定入り両脚補償角θｄｂｖ’およ
び推定両脚間床傾斜偏差θｆｄｂｅｓｔｍｖは、それか
ら今回値を求めるものであるから、この処理では記憶し
ておいた直前の値を用いる。

【０３６８】尚、両脚が接地する想定時刻よりも前から
両脚間床傾斜偏差を推定したい場合において、より精度
を高めるために、想定両脚干渉角θｄｂｉｎｔｃｍｄｖ
を加算することが望ましいが、想定両脚干渉角θｄｂｉ
ｎｔｃｍｄｖの加算および想定両脚干渉角θｄｂｉｎｔ
ｃｍｄｖを算出する構成（図４４において点線で囲まれ
る構成）は必ずしも必須ではない。

【０３６９】次に両脚間機構コンプライアンスモデル１
３４ａを用いて、推定両脚干渉角θｄｂｉｎｔｖによっ
て発生すると予想される前記Ｍｄｂｅｓｔｍｖを求め
る。

【０３７０】一方、目標全床反力中心点まわりに実各足
平床反力の力成分が作るモーメントの和のＶ方向成分Ｍ
ｆ１ｆ２ａｃｔｖ（複合コンプライアンス制御の式１２
および式１３で求められるモーメントのＶ方向成分）を
求める。

【０３７１】次に、前記Ｍｆ１ｆ２ａｃｔｖと前記Ｍｄ
ｂｅｓｔｍｖとの差を求めることにより、両脚モーメン
ト推定偏差Ｍｄｂｅｒｒを得る。もし、推定両脚間床傾
斜偏差θｆｄｂｅｓｔｍｖが実両脚間床傾斜偏差に一致
しているならば、両脚モーメント推定偏差Ｍｄｂｅｒｒ
は０になる。逆に、もし両脚モーメント推定偏差Ｍｄｂ
ｅｒｒが０ならば、推定両脚間床傾斜偏差θｆｄｂｅｓ
ｔｍｖは実両脚間床傾斜偏差に一致する。

【０３７２】次に、両脚間床傾斜偏差推定中断器１４０
ａにおいて、両脚間床傾斜偏差の推定を実行すべきか中
断すべきかを判断する。両脚間床傾斜偏差推定中断器１
４０ａは、両脚間床傾斜偏差の推定を実行すべきと判断
したならば、両脚間床傾斜偏差推定中断器１４０ａへの
入力である前記Ｍｄｂｅｒｒをそのまま出力し、中断す
べきと判断したならば、０を出力する。両脚間床傾斜偏
差推定中断器１４０ａに関する詳細は後述する。

【０３７３】次に、両脚間床傾斜偏差推定中断器１４０
ａの出力を積分定数がＫｄｂｅｓｔｍ’である積分器１
４２ａで積分し、推定両脚間床傾斜偏差θｆｄｂｅｓｔ
ｍｖを得る。

【０３７４】以上が、第２の実施の形態における両脚間
床傾斜偏差推定部１３８ａの処理である。

【０３７５】さらに、推定両脚間床傾斜偏差θｆｄｂｅ
ｓｔｍｖは、図４２に示すように、両脚補償角θｄｂｖ
に加算され、床形状推定入り両脚補償角θｄｂｖ’とし
て修正目標足平位置姿勢算出部１１４ｇへ入力される。

【０３７６】このような処理を毎制御周期に繰り返し実
行することにより、両脚モーメント推定偏差Ｍｄｂｅｒ
ｒが０に収束するように、積分器出力である推定両脚間
床傾斜偏差θｆｄｂｅｓｔｍｖが変化する。

【０３７７】ところで前述したように、両脚モーメント
推定偏差Ｍｄｂｅｒｒが０になると、推定両脚間床傾斜
偏差θｆｄｂｅｓｔｍｖが実際の両脚間床傾斜偏差に一
致する。従って、しばらく経つと、推定両脚間床傾斜偏
差θｆｄｂｅｓｔｍｖは、実両脚間床傾斜偏差に一致す
る。即ち、この処理によって、両脚間床傾斜偏差が推定
される。

【０３７８】尚、両脚間機構コンプライアンスモデル１
３４ａが線形であるならば、その処理系の実両脚間床傾
斜偏差から推定両脚間床傾斜偏差までの閉ループ伝達関
数は、１次遅れになる。

【０３７９】その時定数Ｔは、両脚間機構コンプライア
ンスモデル１３４ａにおける入力（推定両脚干渉角）に
対する出力（前記モーメントＭｄｂｅｓｔｍｖ）の比
（絶対値）をＣとすると、次式で求められる。 T ＝ 1 / ( C× Kdbestm’)・・・式２６

【０３８０】第１の実施の形態の積分ゲインＫｄｂｅｓ
ｔｍに対して、第２の実施の形態の積分ゲインＫｄｂｅ
ｓｔｍ’を Kdbestm’＝ Kdbestm / C・・・式２７ とすることにより、第１の実施の形態とほぼ同一の特性
を持つ両脚間床傾斜偏差推定器１３８ａが得られる。

【０３８１】次に、足平床傾斜偏差推定部１４４ａの処
理を図４５を参照して説明する。

【０３８２】図４５では、推定第１足平床傾斜偏差Ｘ成
分に関する処理機能ブロック図のみを示すが、推定第１
足平床傾斜偏差Ｙ成分、推定第２足平床傾斜偏差Ｘ成
分、推定第２足平床傾斜偏差Ｙ成分についても同様の処
理を行う。

【０３８３】先ず、目標歩容の第１足平傾斜角を求め、
これから想定第１足平床傾斜傾斜を減じたもののＸ成分
を、想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘとす
る。次に、次式により前述の推定第１足平干渉角のＸ成
分θ１ｉｎｔｘを求める。 θ1intx ＝θ1x’＋θerrx−θf1estmx ＋θ1intcmdx ・・・式２８

【０３８４】ここで、θｆ１ｅｓｔｍｘは、第１の実施
の形態でも述べたように、推定第１足平床傾斜偏差Ｘ成
分である。また、θ１ｘ’は、床形状推定入り第１足平
Ｘ補償角であり、図４２に示すように、第１足平Ｘ補償
角θ１ｘと推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔ
ｍｘの和である。

【０３８５】床形状推定入り第１足平Ｘ補償角θ１ｘ’
および推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘ
は、それから今回値を求めるものであるから、この処理
では記憶しておいた直前の値を用いる。

【０３８６】尚、第１足平が床にべたに接地する想定時
刻よりも前から第１足平床傾斜偏差を推定したい場合に
おいて、より精度を高めるために、想定第１足平干渉角
Ｘ成分θ１ｉｎｔｃｍｄｘを加算することが望ましい
が、想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘの加
算および想定第１足平干渉角Ｘ成分θ１ｉｎｔｃｍｄｘ
を算出する構成（図４５において点線で囲われる構成）
は必ずしも必須ではない。

【０３８７】次に、前記の足平機構コンプライアンスモ
デル１３４ｂを用いて、推定第１足平干渉角Ｘ成分θ１
ｉｎｔｘによって発生すると予想される前記Ｍ１ｅｓｔ
ｍのＸ成分Ｍ１ｅｓｔｍｘを求める。

【０３８８】次に、検出された実第１足平床反力モーメ
ントＭ１ａｃｔｘと前記Ｍ１ｅｓｔｍｘとの差を求める
ことにより、第１足平床反力モーメント推定偏差Ｘ成分
Ｍ１ｅｒｒｘを得る。

【０３８９】もし、推定第１足平床傾斜偏差Ｘ成分θｆ
１ｅｓｔｍｘが実第１足平床傾斜偏差に一致しているな
らば、第１足平床反力モーメント推定偏差Ｘ成分Ｍ１ｅ
ｒｒｘは０になる。逆に、もし第１足平床反力モーメン
ト推定偏差Ｘ成分Ｍ１ｅｒｒｘが０ならば、推定第１足
平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘは実第１足平床傾
斜偏差に一致する。

【０３９０】次に、第１足平床傾斜偏差推定中断器にお
いて第１足平床傾斜偏差の推定を実行（続行）すべきか
中断すべきかを判断する。

【０３９１】第１足平床傾斜偏差推定中断器は、第１足
平床傾斜偏差の推定を実行すべきと判断したならば、第
１足平床傾斜偏差推定中断器への入力である第１足平床
反力モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘをそのまま
出力し、中断すべきと判断したならば、０を出力する。
尚、足平床傾斜偏差推定中断器１４６ａに関する詳細は
後述する。

【０３９２】次いで、第１足平床傾斜偏差推定中断器の
出力を積分定数がＫ１ｅｓｔｍｘ’である積分器で積分
し、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘを
得る。

【０３９３】以上が、第２の実施の形態における第１足
平床傾斜偏差X成分推定部の処理である。

【０３９４】さらに、推定第１足平床傾斜偏差Ｘ成分θ
ｆ１ｅｓｔｍｘは、図４２に示すように、第１足平Ｘ補
償角θ１ｘに加算され、床形状推定入り第１足平Ｘ補償
角θ１ｘ’として修正目標足平位置姿勢算出部１１４ｇ
へ入力される。

【０３９５】このような処理を毎制御周期に繰り返し実
行することにより、第１足平床反力モーメント推定偏差
Ｘ成分Ｍ１ｅｒｒｘが０に収束するように、積分器出力
である推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘ
が変化する。

【０３９６】ところで前述したように、第１足平床反力
モーメント推定偏差Ｘ成分Ｍ１ｅｒｒｘが０になると、
推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍｘが実第
１足平床傾斜偏差Ｘ成分に一致する。従って、しばらく
経つと、推定第１足平床傾斜偏差Ｘ成分θｆ１ｅｓｔｍ
ｘは、実第１足平床傾斜偏差Ｘ成分に一致する。即ち、
この処理によって、第１足平床傾斜偏差Ｘ成分が推定さ
れる。

【０３９７】尚、足平機構コンプライアンスモデル１３
４ｂが線形であるならば、その処理系の実第１足平床傾
斜偏差Ｘ成分から推定第１足平床傾斜偏差Ｘ成分までの
閉ループ伝達関数は、１次遅れになる。

【０３９８】その時定数Ｔは、足平機構コンプライアン
スモデル１３４ｂにおける入力（推定第１足平干渉角Ｘ
成分θ１ｉｎｔｘ）に対する出力（前記モーメントＭ１
ｅｓｔｍｘ）の比（絶対値）をＣ１とすると、次式で求
められる。 T ＝ 1 / ( C1 × K1estmx’)・・・式２９

【０３９９】第１の実施の形態の積分ゲインＫ１ｅｓｔ
ｍｘに対し、第２の実施の形態の積分ゲインＫ１ｅｓｔ
ｍｘ’を K1estmx’＝ K1estmx / C1・・・式３０ とすることにより、第１の実施の形態とほぼ同一の特性
を持つ第１足平床傾斜偏差推定器が得られる。

【０４００】尚、目標足平の向きが目標上体の向きと異
なる場合には、上体傾斜を各足平座標系で表わせばよ
い。

【０４０１】ここで、第１の実施の形態と第２の実施の
形態を比較する。

【０４０２】第１の実施の形態では床形状推定器１３０
への入力として、最終的に修正された目標足平位置姿勢
を用いていたが、第２の実施の形態では、それを決定す
るために中間的に生成される変数を用いていると言え
る。

【０４０３】第２の実施の形態は、簡単に言えば、第１
の実施の形態のコンプライアンスモデルを、両脚干渉角
に関するコンプライアンスモデルである両脚間機構コン
プライアンスモデル１３４ａと、足平干渉角に関するコ
ンプライアンスモデルである足平機構コンプライアンス
モデル１３４ｂに近似してから、ブロック線図を等価変
換したものになっている。

【０４０４】第２の実施の形態は機構コンプライアンス
モデルが簡素化されているので、第１の実施の形態に比
較すると、やや推定精度が低下するが、機構コンプライ
アンスの演算が簡単であるので、制御ユニット２６の負
荷を軽減することができる。それ以外は、第２の実施の
形態は、第１の実施の形態と異ならない。作用効果も同
様である。床形状の推定のみでも良いことも、第１の実
施の形態と同様である。

【０４０５】尚、上記において、両脚支持期になる想定
時刻（即ち、遊脚の着地想定時刻）よりも前から両脚間
床傾斜偏差の推定を開始することがない場合には、式２
５から想定両脚干渉角θｄｂｉｎｔｃｍｄｖの項を削除
した次式を用いても良い。なぜなら、目標歩容の両脚支
持期には、想定両脚干渉角θｄｂｉｎｔｃｍｄｖは０で
あるからである。 θdbintv＝θdbv’＋θerrv−θfdbestmv・・・式３１

【０４０６】目標歩容の第１足平のＹ軸に沿ったエッジ
（かかとエッジまたはつまさきエッジ）が接地する時刻
よりも前から第１足平床傾斜偏差X成分の推定を開始す
ることがない場合には、式２８から想定第１足平干渉角
Ｘ成分θ１ｉｎｔｃｍｄｘの項を削除した次式を用いて
も良い。なぜなら、目標歩容の第１足平のＹ軸に沿った
エッジが接地しているときには、想定第１足平干渉角Ｘ
成分θ１ｉｎｔｃｍｄｘは０であるからである。 θ1intx ＝θ1x’＋θerrx−θf1estmx・・・式３２

【０４０７】目標歩容の第１足平のＸ軸に沿ったエッジ
（即ち、内エッジまたは外エッジ）が接地する時刻より
も前から第１足平床傾斜偏差Ｙ成分の推定を開始するこ
とがない場合には、式２８のｘをｙに置き換えた式の代
わりに、その式から想定第１足平干渉角Ｙ成分θ１ｉｎ
ｔｃｍｄｙの項を削除した次式を用いても良い。なぜな
ら、目標歩容の第１足平のＹ軸に沿ったエッジ（即ち、
内エッジまたは外エッジ）が接地しているときには、想
定第１足平干渉角Ｙ成分θ１ｉｎｔｃｍｄｙは０である
からである。 θ1inty ＝θ1y’＋θerry−θf1estmy・・・式３３

【０４０８】さらに、前記した式１２で求められるモー
メントＭｆ１ｆ２ａｃｔｖの代わりに、式１２ａで求め
られるモーメントＭｆ１ｆ２ａｃｔｖを用いる場合に
は、前記Ｍｄｂｅｓｔｍｖの代わりに、推定両脚干渉角
を打ち消す機構変形によって発生するはずの目標全床反
力中心点まわりの全床反力モーメントを用いる。

【０４０９】さらに、推定床形状偏差を床形状地図情報
として記憶しておき、次回同一場所を歩行するときの目
標歩容を生成する際の参考にしても良い。

【０４１０】次いで、第２の実施の形態に係る第１足平
床傾斜偏差Ｙ成分推定中断器の処理を説明する。

【０４１１】第２の実施の形態に係る第１足平床傾斜偏
差Ｙ成分推定中断器の処理は第１の実施の形態のそれと
ほとんど同様であり、具体的には、図３７のθｆ１ｅｒ
ｒｄｉｆｆｙを第１足平床反力モーメント推定偏差Ｙ成
分Ｍ１ｅｒｒｙに置き換えれば良い。

【０４１２】また、両脚間床傾斜偏差推定中断器１４０
ａの処理も同様であって、図４０フロー・チャートに示
す処理においてθｆｄｂｅｒｒｄｉｆｆｖを両脚モーメ
ント推定偏差Ｍｄｂｅｒｒに置き換えれば良い。

【０４１３】ここで、両脚間床傾斜偏差推定中断器１４
０ａの役割を説明すると、両脚間床傾斜偏差推定中断器
１４０ａは、第１足平床傾斜偏差推定中断器と同様に、
両脚間床傾斜偏差の推定が正常に実行できるか否かを判
断し、正常に実行できないと判断したときには推定を中
断する。

【０４１４】より具体的には、推定両脚間床傾斜偏差の
精度が低下する恐れがある状況、および推定両脚間床傾
斜偏差が発散する恐れがある状況では、推定を中断する
ために０を出力し、それ以外の状況では、推定を継続す
るために両脚間床傾斜偏差推定中断器への入力をそのま
ま出力する。

【０４１５】推定両脚間床傾斜偏差は、各足平と床との
間の接触圧力が十分にあるときには発散しないが、ある
足平が浮きかかっているにもかかわらず、両脚補償角
が、その足平が浮く方向に変化しつつある状況で発生す
る。その発生メカニズムは推定第１足平床傾斜偏差Ｙ成
分が発散する場合とほぼ同一なので、これ以上の説明は
省略する。

【０４１６】上記の如く、第２の実施の形態において
も、床形状推定値、具体的には各足平の接地面傾斜と両
脚接地面間の高低差を同時に、精度良く推定することが
できる。

【０４１７】さらに、床形状が想定していた形状と異な
っていても、望み通りの床形状を発生されることができ
ると共に、床形状のずれに起因する床反力の定常偏差を
解消することができる。

【０４１８】また、床形状推定精度が低下する恐れがあ
る状況あるいは推定値が発散する恐れがある状況では床
形状推定を中断するように構成したので、前記した作用
効果に加え、推定値が不適正なものとなることがない。

【０４１９】尚、第１および第２の実施の形態におい
て、足平２２Ｒ（Ｌ）が浮きそうな状態の判断を、６軸
力センサ３４によって検出された実第１足平床反力から
前記実床反力中心点位置Ｑ１ａｃｔまたは前記Ｑ１ａｃ
ｔ’を算出し、それに基づいて足平２２Ｒ（Ｌ）がどち
らの方向に浮きやすいか（かかとが浮きやすいか、つま
さきが浮きやすいか、足平内エッジが浮きやすいか、ま
たは足平外エッジが浮きやすいか）を判定した。

【０４２０】しかし、それに限られるものではなく、図
４６に示す如く、足底弾性体４０と足平本体２２ａの間
に分布圧センサ２００を配置し、その出力信号に基づい
て足平２２Ｒ（Ｌ）がどちらの方向に浮きやすいかを判
定しても良い。尚、６軸力センサ３４よりも分布圧セン
サ２００の方が実床反力による足平の変形をより細緻に
推定することができるので、床形状推定の精度を向上さ
せることができる。

【０４２１】さらには、図４７に示す如く、足平２２Ｒ
（Ｌ）に接触センサ２０２を複数個配置（例えば足平の
四隅に配置）し、その出力信号に基づいて足平２２Ｒ
（Ｌ）がどちらの方向に浮きやすいかを判定しても良
い。尚、判定のアルゴリズムとして、接地していない接
触センサが増々浮く方向への推定を中断する、あるい
は、全ての接触センサが浮いているときも推定を中断す
ることなどが考えられる。

【０４２２】第１、第２の実施の形態では上記の如く、
請求項１項にあっては、少なくとも上体（基体）２４
と、前記上体２４に第１の関節（股（腰部）関節１０、
１２、１４Ｒ，Ｌ）を介して連結されると共に、その先
端に第２の関節（足（足首）関節１８、２０Ｒ，Ｌ）を
介して連結される足部（足平）２２Ｒ，Ｌを備えた複数
本の脚部（脚部リンク）２を備えると共に、前記足部２
２Ｒ，Ｌに接地する床面から作用する床反力に応じて変
形するコンプライアンス機構４２を備え、少なくとも前
記上体２４に対する前記足部２２Ｒ，Ｌの実位置姿勢を
検出し、前記足部２２Ｒ，Ｌの目標位置姿勢に追従させ
るように前記関節（１０、１２Ｒ，Ｌなど）を駆動する
関節駆動手段（電動モータ）を備えた脚式移動ロボット
の床形状推定装置において、前記上体２４の鉛直軸方向
に対する傾斜を検出する傾斜検出手段（傾斜センサ３
６）、前記足部２２Ｒ，Ｌの実位置姿勢および／または
前記足部２２Ｒ，Ｌの目標位置姿勢と前記検出された傾
斜に基づき、前記足部２２Ｒ，Ｌが接地する床面を含む
歩行環境における前記足部２２Ｒ，Ｌの位置姿勢（機構
変形補償入り修正目標足平位置姿勢）を算出する対歩行
環境足部位置姿勢算出手段（制御ユニット２６、複合コ
ンプライアンス動作決定部１１４、機構変形補償入り修
正目標足平位置姿勢算出部１１４ｈ、両脚間床傾斜偏差
推定部１３８内の上体傾斜偏差のＶ方向成分θｅｒｒｖ
の加算器、足平床傾斜偏差推定部１４４内の上体傾斜偏
差のＸ成分θｅｒｒｘの加算器）、前記床面から作用す
る床反力による前記コンプライアンス機構４２の変形が
加えられた前記歩行環境における足部の変形位置姿勢
（機構変形後の推定各足平位置姿勢）を、前記コンプラ
イアンス機構４２をモデル化して得た機構コンプライア
ンスモデル１３４を用いて算出する変形足部位置姿勢算
出手段（制御ユニット２６、複合コンプライアンスモデ
ル１３４、床形状推定器１３０（特に両脚間床傾斜偏差
推定部１３８内のθｆｄｂｖ’を算出するまでの部分、
足平床傾斜偏差偏差推定部１４４内のθｆ１ｅｒｒｘ’
を算出するまでの部分））、および前記算出された足部
の変形位置姿勢に基づき、前記足部２２Ｒ，Ｌが接地す
るそれぞれの床面の形状（推定第ｎ足平床傾斜（偏
差））およびそれらの相対位置関係（推定両脚間床傾斜
（偏差））を示す床形状推定値を算出する床形状推定値
算出手段（制御ユニット２６、複合コンプライアンス動
作決定部１１４、床形状推定器１３０、両脚間床傾斜偏
差推定部１３８（特にθｆｄｂｅｒｒｖ算出後の部
分）、足平床傾斜偏差推定部１４４（特にθｆ１ｅｒｒ
ｘ算出後の部分））、を備えると共に、前記床形状推定
値算出手段は、前記床形状推定値の算出が中断すべき状
況にあるか否か判定し、中断すべき状況にないと判定す
るときは前記床形状推定値の算出を続行する推定中断手
段（制御ユニット２６、両脚間床傾斜偏差推定中断器１
４０、１４０ａ、足平床傾斜偏差推定部１４４、１４４
ａ、足平床傾斜偏差中断器１４６、１４６ａ、Ｓ３００
からＳ３１６，Ｓ４００からＳ４１４）、を備える如く
構成した。

【０４２３】また、前記推定中断手段は、中断すべき状
況にあると判定するときは、前記床形状推定値を保持す
るか、あるいは所定の値に設定する（Ｓ３０４，Ｓ３０
６，Ｓ３０８，Ｓ３１４，Ｓ４０４，Ｓ４０６，Ｓ４０
８，Ｓ４１２）如く構成した。

【０４２４】また、前記推定中断手段は、少なくとも前
記ロボットの歩行時期に基づいて中断すべき状況にある
か否か判定する（Ｓ３００，Ｓ４００）如く構成した。

【０４２５】また、前記推定中断手段は、少なくとも前
記足部と床面との接触状況に基づいて中断すべき状況に
あるか否か判定する（Ｓ３１２，Ｓ４１０）如く構成し
た。

【０４２６】また、前記推定中断手段は、少なくとも前
記足部に作用する前記床反力に基づいて中断すべき状況
にあるか否か判定する（Ｓ３１２，Ｓ４１０）如く構成
した。

【０４２７】また、前記推定中断手段は、少なくとも前
記足部に作用する前記床反力の中心点位置に基づいて前
記中断すべき状況にあるか否か判定する（Ｓ３１２，Ｓ
４１０）如く構成した。

【０４２８】さらに、少なくとも前記算出された床形状
推定値に基づいて前記足部の目標位置姿勢を修正する修
正手段（制御ユニット２６、加算器１３２（１３２ａ，
１３２ｂ）、修正足平位置姿勢算出部１１４ｇ、機構変
形補償入り修正目標足平位置姿勢算出部１１４ｈ、機構
変形量算出部１１４ｎ）、を備えると共に、前記推定中
断手段は、前記床形状推定値が発散する恐れがあるとき
は、前記中断すべき状況にあると判定し（Ｓ３１２，Ｓ
４１０）、前記床形状推定値の算出を中断する如く構成
した。

【０４２９】尚、上記した第１、第２の実施の形態にお
いて、ブロック線図は演算処理順序を変えるなどの等価
変形をしても良い。

【０４３０】また、この発明を２足歩行ロボットに関し
て説明してきたが、２足歩行ロボットに限らず、多脚ロ
ボットにも応用することができる。

【０４３１】

【発明の効果】請求項１項にあっては、従来技術では困
難であった床形状の推定、具体的には各足平の接地面傾
斜と両足平接地面間の高低差を同時に、換言すれば複合
的に、精度良く推定することができると共に、推定精度
が低下する恐れがある状況では床形状推定を中断するの
で、推定値が不適正なものとなることがない。

【０４３２】請求項２項にあっては、推定精度が低下す
る恐れがある状況においても、各足平の接地面傾斜と両
足平接地面間の高低差を同時に、換言すれば複合的に、
精度良く推定することができる。

【０４３３】請求項３項にあっては、より的確に中断す
べき状況にあるか否かを判定することができ、よって一
層推定値が不適正なものとなることがない。

【０４３４】請求項４項にあっては、より的確に中断す
べき状況にあるか否かを判定することができ、よって一
層推定値が不適正なものとなることがない。

【０４３５】請求項５項にあっては、より的確に中断す
べき状況にあるか否かを判定することができ、よって一
層推定値が不適正なものとなることがない。

【０４３６】請求項６項にあっては、より的確に中断す
べき状況にあるか否かを判定することができ、よって一
層推定値が不適正なものとなることがない。

【０４３７】請求項７項にあっては、前記した効果に加
え、複合コンプライアンス制御で除去しきれなかった実
床反力の制御目標値からの定常偏差を可能な限り零に近
づける、換言すれば床形状のずれに起因する床反力の定
常偏差を解消することができる。

【図面の簡単な説明】

【図１】この発明の一つの実施の形態に係る床形状推定
装置を適用した脚式移動ロボット、より具体的には２足
歩行ロボットを全体的に示す概略図である。

【図２】図１に示す脚式移動ロボットの足部の構造を示
す説明側面図である。

【図３】図１に示す脚式移動ロボットの制御ユニットの
詳細を示すブロック図である。

【図４】先に提案した脚式移動ロボットの制御装置およ
びこの発明の一つの実施の形態に係る脚式移動ロボット
の床形状推定装置の構成および動作を機能的に示すブロ
ック図である。

【図５】図１に示す脚式移動ロボットが平地を歩行する
ときの運動パターンの一例を示す説明図である。

【図６】図５の運動パターンに対応する目標全床反力中
心点（目標ＺＭＰ）軌跡の床面上軌跡を示す説明図であ
る。

【図７】図５の運動パターンに対応する目標全床反力中
心点（目標ＺＭＰ）軌跡のタイム・チャートである。

【図８】図５の運動パターンに対応する所定の条件を満
たすように設定した目標第１足平床反力中心点軌跡のタ
イム・チャートである。

【図９】図５の運動パターンに対応する所定の条件を満
たすように設定した目標第２足平床反力中心点軌跡のタ
イム・チャートである。

【図１０】図４と同様に、先に提案した脚式移動ロボッ
トの制御装置の動作を示すフロー・チャートである。

【図１１】図１０フロー・チャートの内の両脚補償角な
どの演算処理を行う、図４に示す複合コンプライアンス
動作決定部の動作を説明するための、両脚支持期に第１
足平と第２足平に実各足平床反力が作用している状況を
示す説明図である。

【図１２】図１１に示す状況における目標全床反力の設
定を示す説明図である。

【図１３】図１１に示す状況における目標各足平床反力
の分配を示す説明図である。

【図１４】図１１に示す状況における補償全床反力モー
メントを示す説明図である。

【図１５】図１１に示す状況における、各足平床反力中
心点を含み、水平面に垂直な平面の法線ベクトルＶを示
す説明図である。

【図１６】図１１に示す状況における、目標各足平床反
力中心点を目標全床反力中心点（目標ＺＭＰ）まわり
に、所定角度θｄｂｖだけ回転させたときの状態を示す
説明図である。

【図１７】図１１に示す状況における、各足平を前後方
向軸および左右方向軸まわりに所定角度θｎｘ，θｎｙ
だけ回転させたときの状態を示す説明図である。

【図１８】図４の複合コンプライアンス動作決定部の演
算処理を示すブロック図である。

【図１９】図１８に示す補償全床反力モーメント分配器
の演算処理を示すブロック図である。

【図２０】図１８に示す補償全床反力モーメント分配器
の、両脚補償角などを操作するための分配重み変数の設
定例を示すタイム・チャートである。

【図２１】図２０の補償全床反力モーメント分配器の分
配重み変数の設定を説明するための、ロボットの姿勢を
示す説明図である。

【図２２】図２１と同様に、補償全床反力モーメント分
配器の分配重み変数の設定を説明するための、ロボット
の姿勢を示す説明図である。

【図２３】両脚補償角を操作するための分配重みを所定
の条件で決定したときの両脚補償モーメントＶ方向成分
Ｍｄｍｄｄｂｖを示す説明図である。

【図２４】図１８に示す両脚補償角決定部の演算処理を
示すブロック図である。

【図２５】図１８に示す各足平の補償角決定部の演算処
理を示すブロック図である。

【図２６】図１８に示す機構変形補償入り修正目標足平
位置姿勢算出部の演算処理を説明するための説明図であ
る。

【図２７】図２６と同様に、図１８に示す機構変形補償
入り修正目標足平位置姿勢算出部の演算処理を説明する
ための説明図である。

【図２８】図１０フロー・チャートの内の両脚補償角な
どの決定作業を示すサブルーチン・フロー・チャートで
ある。

【図２９】先に提案した制御装置の変形例を示す図１６
と同様の説明図で、足平位置の修正動作の別の例を示す
説明図である。

【図３０】この発明の一つの実施の形態の係る脚式移動
ロボットの床形状推定装置の構成を示す、図１８と同様
なブロック図である。

【図３１】図３０に示す装置が床形状を推定するときに
使用する定義の説明図である。

【図３２】同様に図３０に示す装置が床形状を推定する
ときに使用する定義の説明図である。

【図３３】図３０に示す床形状推定器の処理を機能的に
示すブロック図である。

【図３４】図３３に示す床形状推定器の機構コンプライ
アンスモデルの処理を機能的に示すブロック図である。

【図３５】同様に図３３に示す床形状推定器の機構コン
プライアンスモデルの処理を機能的に示すブロック図で
ある。

【図３６】この発明の一つの実施の形態における足平床
傾斜偏差推定値の発散を説明するための説明図である。

【図３７】この発明の一つの実施の形態における足平床
傾斜偏差推定中断器の処理を示すフロー・チャートであ
る。

【図３８】図３７の処理を説明するタイム・チャートで
ある。

【図３９】図３７の処理を説明する説明図である。

【図４０】この発明の一つの実施の形態における両脚間
床傾斜偏差推定中断器の処理を示すフロー・チャートで
ある。

【図４１】図４０の処理を説明するタイム・チャートで
ある。

【図４２】この発明の第２の実施の形態に係る脚式移動
ロボットの床形状推定装置の構成を示す、図３０と同様
なブロック図である。

【図４３】図４２に示す第２の実施の形態で使用する定
義の説明図である。

【図４４】第２の実施の形態における床形状推定器の両
脚間床傾斜偏差推定部の処理を機能的に示すブロック図
である。

【図４５】第２の実施の形態における床形状推定器の足
平床傾斜偏差推定部の処理を機能的に示すブロック図で
ある。

【図４６】第１および第２の実施の形態に係る脚式移動
ロボットの床形状推定装置で使用するセンサの別の例を
示す説明図である。

【図４７】第１および第２の実施の形態に係る脚式移動
ロボットの床形状推定装置で使用するセンサのさらに別
の例を示す説明図である。

【符号の説明】

１ ２足歩行ロボット（脚
式移動ロボット） ２ 脚部（脚部リンク） １０，１２，１４Ｒ，Ｌ 股関節 １６Ｒ，Ｌ 膝関節 １８，２０Ｒ，Ｌ 足関節 ２２Ｒ，Ｌ 足平（足部） ２４ 上体 ２６ 制御ユニット ３４ ６軸力センサ ３６ 傾斜センサ ４２ コンプライアンス機構 ６０ 第１の演算装置 ６２ 第２の演算装置 １００ 歩容生成器 １０２ 目標床形状分配器 １０４ 姿勢安定化制御演算部 １０８ 実各足平床反力検出器 １１０ ロボット幾何学モデル １１２ 変位コントローラ １１４ 複合コンプライアンス
動作決定部 １３０，１３０ａ 床形状推定器 １３２（１３２ａ，１３２ｂ） 加算器 １３４，１３４ａ 機構コンプライアンス
モデル １３８，１３８ａ 両脚間床傾斜偏差推定
部 １４０，１４０ａ 両脚間床傾斜偏差推定
中断器 １４４，１４４ａ 足平床傾斜偏差推定部 １４６，１４６ａ 足平床傾斜偏差推定中
断器 ２００ 分布圧センサ ２０２ 接触センサ

───────────────────────────────────────────────────── フロントページの続き (72)発明者 松本 隆志 埼玉県和光市中央１丁目４番１号 株式会 社本田技術研究所内 Ｆターム(参考） 3F059 AA00 BA02 BB06 BC07 CA05 CA08 DA02 DA08 DC01 DC04 DD01 DD06 FA03 FA08 FB01 FB17 FB21 FB29 FC02 FC03 FC06 FC07 FC13 FC14 5H303 AA30 BB03 BB09 BB15 DD01 DD26 JJ05 JJ10 LL03 MM05

Claims (7)

【特許請求の範囲】
1. 【請求項１】 少なくとも上体と、前記上体に第１の関
   節を介して連結されると共に、その先端に第２の関節を
   介して連結される足部を備えた複数本の脚部を備えると
   共に、前記足部に接地する床面から作用する床反力に応
   じて変形するコンプライアンス機構を備え、少なくとも
   前記上体に対する前記足部の実位置姿勢を検出し、前記
   足部の目標位置姿勢に追従させるように前記関節を駆動
   する関節駆動手段を備えた脚式移動ロボットの床形状推
   定装置において、 ａ．前記上体の鉛直軸方向に対する傾斜を検出する傾斜
   検出手段、 ｂ．前記足部の実位置姿勢および／または前記足部の目
   標位置姿勢と前記検出された傾斜に基づき、前記足部が
   接地する床面を含む歩行環境における前記足部の位置姿
   勢を算出する対歩行環境足部位置姿勢算出手段、 ｃ．前記床面から作用する床反力による前記コンプライ
   アンス機構の変形が加えられた、前記歩行環境における
   前記足部の変形位置姿勢を、前記コンプライアンス機構
   をモデル化して得た機構コンプライアンスモデルを用い
   て算出する変形足部位置姿勢算出手段、および ｄ．前記算出された足部の変形位置姿勢に基づき、前記
   足部が接地するそれぞれの床面の形状およびそれらの相
   対位置関係を示す床形状推定値を算出する床形状推定値
   算出手段、を備えると共に、前記床形状推定値算出手段
   は、 ｅ．前記床形状推定値の算出が中断すべき状況にあるか
   否か判定し、中断すべき状況にないと判定するときは前
   記床形状推定値の算出を続行する推定中断手段、を備え
   ることを特徴とする脚式移動ロボットの床形状推定装
   置。
2. 【請求項２】 前記推定中断手段は、中断すべき状況に
   あると判定するときは、前記床形状推定値を保持する
   か、あるいは所定の値に設定することを特徴とする請求
   項１項記載の脚式移動ロボットの床形状推定装置。
3. 【請求項３】 前記推定中断手段は、少なくとも前記ロ
   ボットの歩行時期に基づいて中断すべき状況にあるか否
   か判定することを特徴とする請求項１項または２項記載
   の脚式移動ロボットの床形状推定装置。
4. 【請求項４】 前記推定中断手段は、少なくとも前記足
   部と床面との接触状況に基づいて中断すべき状況にある
   か否か判定することを特徴とする請求項１項から３項の
   いずれかに記載の脚式移動ロボットの床形状推定装置。
5. 【請求項５】 前記推定中断手段は、少なくとも前記足
   部に作用する前記床反力に基づいて中断すべき状況にあ
   るか否か判定することを特徴とする請求項１項から３項
   のいずれかに記載の脚式移動ロボットの床形状推定装
   置。
6. 【請求項６】 前記推定中断手段は、少なくとも前記足
   部に作用する前記床反力の中心点位置に基づいて前記中
   断すべき状況にあるか否か判定することを特徴とする請
   求項５項記載の脚式移動ロボットの床形状推定装置。
7. 【請求項７】 さらに、 ｆ．少なくとも前記算出された床形状推定値に基づいて
   前記足部の目標位置姿勢を修正する修正手段を備えると
   共に、前記推定中断手段は、前記床形状推定値が発散す
   る恐れがあるときは、前記中断すべき状況にあると判定
   し、前記床形状推定値の算出を中断することを特徴とす
   る請求項１項から６項のいずれかに記載の脚式移動ロボ
   ットの床形状推定装置。