WO2003090981A1

WO2003090981A1 - Systeme permettant d'estimer l'attitude d'un robot mobile monte sur des jambes

Info

Publication number: WO2003090981A1
Application number: PCT/JP2003/005449
Authority: WO
Inventors: Toru Takenaka; Takashi Matsumoto
Original assignee: Honda Giken Kogyo Kabushiki Kaisha
Priority date: 2002-04-26
Filing date: 2003-04-28
Publication date: 2003-11-06
Also published as: EP1502712A4; KR100956537B1; KR20050007346A; EP1502712A1; US20050156551A1; JP4181113B2; EP2110210B1; EP2106886B1; KR20050000410A; US6963185B2; EP1508408B1; DE60336518D1; KR100956539B1; US7145305B2; KR20050000412A; EP2110210A1; EP1502712B1; DE60332224D1; JP4246696B2; EP1504855A4

Description

明細書脚式移動ロポットの自己姿勢推定装置技術分野

本発明は、脚式移動ロポットの自己姿勢を推定する装置に関する。背景技術

一般的に、 2足移動ロボット等の脚式移動口ポットには、上体に傾斜計が搭載されており、傾斜計の出力（上体の鉛直方向に対する傾斜角）を目標歩容の上体の傾斜角に収束させるようにロボットの姿勢制御が行われる。

傾斜計は、上体の角速度を検知するためのジャイロセンサと、鉛直方向（すなわち重力方向）を検知するための加速度センサ（または振り子等の鉛直指示器）とから構成される。傾斜計では、基本的にジャイロセンサが検知した角速度を積分することによって上体の傾斜角を推定するが、そのままでは積分誤差が蓄積する（所謂ドリフトが発生する）ので加速度センサの検出値を用いて、積分によって得られた推定傾斜角を補正していた。具体的には、加速度センサによって検出される加速度方向に対する傾斜角と、ジャィ口センサの検出値を積分器に通して得られた傾斜角との差を求め、その差に応じた補正量（差にある所定のゲインを乗じた値と差を積分してある所定のゲインを乗じた値の和）を前記積分器に追加的に入力することにより、傾斜角を補正していた。

しかし、この方式では、口ポットの鉛直軸回り（ョ一方向）のジャィ口センサのドリフトを補償することはできないので、ロポットの鉛直軸回りの向きを精度よく推定することは困難であった。また、口ポットの走行などのように上体姿勢が激しく加速ないしは減速する場合には、上体の傾斜角や向きの推定誤差が拡大しやすい。

なお、以降、上体などのようなある代表的な部位の傾きと向きを総称して「姿勢」と呼ぶ。「傾き」は、鉛直方向となす角度である。「向き」は、代表的な部位の前方向を示すベクトルを水平面に投影したベクトルの向きである。

一般的に代表的な部位とは、ジャィ口センサと加速度センサとから構成される傾斜計が搭載されている部位であるが、他の部位であつても、その部位と傾斜計が搭載されている部位との間の各関節にエンコーダのような関節変位検出器や変位制御ァクチユエ一夕があれば、その部位の傾斜を算出することができるので、その部位を代表的な部位としても構わない。例えば、関節を持つ首で連結された頭部にジャイロセンサおよびまたは加速度センサを備える場合でも、代表部位を上体としても良い, なお、後述する実施形態においては、上体を代表的部位としている。

また一般的には全関節変位の組も姿勢と呼ばれるが、特に断りがない限り、本実施例においては、「姿勢」をこの意味では用いない。

一方、脚式移動口ポットでは、移動中に脚体を振ることによる反力によって、足平（脚体先端部）と床との間の摩擦力が限界となって、足平と床の間で回転滑り（スピン）が生じ、口ポット全体が鉛直軸まわりに姿勢回転して、目標歩容の向きからずれることがある。

補足すると、上体が常に鉛直（直立）を維持して、直線歩行する目標歩容だけが生成されるとは限らない。目標歩容においても、口ポット全体または上体は旋回したり、前後左右に傾いたりする。すなわち、目標歩容においても、全体の姿勢回転（または上体等の代表部位の姿勢回転）が存在する。そこで、本明細書では、目標歩容における姿勢回転を目標姿勢回転と呼ぶ。本明細書で主に問題とする現象は、実際のロポットの全体の姿勢回転（または上体等の代表部位の姿勢回転）が、前記目標姿勢回転からずれることである。この現象を、厳密に表現するならば，「目標姿勢回転からの摂動」あるいは「姿勢回転摂動」と呼ぶべきであるが、目標姿勢回転と混同する恐れがない場合には、以降、これを「姿勢回転」と省略する。

なお、以降、特に、口ポット全体が鉛直軸まわりに姿勢回転して、目標歩容の向きからずれる現象をスピンと呼ぶ。

上記の如く発生するロポットの姿勢回転によって、口ポットの向きが目標歩容の向きからずれてしまうと、口ポットの移動経路は、目標とする移動経路からもずれてしまう。そして、このような場合には、ロポッ卜の移動経路を目標経路に誘導してやる必要があり、そのためには、口ポッ卜の上体等の代表部位の姿勢、特に向きを精度よく推定することが必要となる。しかし、前述したように、従来のロボットの上体姿勢の推定技術では、前記ドリフトの影響で、特にョー方向の口ポットの姿勢を精度よく推定することができないものとなっていた。

本発明は以上説明した背景に鑑みてなされたものであり、脚式移動口ボットの自己姿勢、特にョ一方向の姿勢を精度良く推定することができる自己姿勢推定装置を提供することを目的とする。発明の開示

本発明の脚式移動ロボットの自己姿勢推定装置の第 1発明は、前記の目的を達成するために、決定された目標歩容に追従するように制御される脚式移動ロポッ卜において、前記ロポットの所定の部位の姿勢角速度を検出する姿勢角速度検出手段と、少なくとも前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちのいずれかを含む該ロポットの運動状態量を基に、前記口ポットと床との接触面に滑りが無いと仮定して、前記所定の部位の姿勢角速度を推定する無滑り姿勢角速度推定手段と、少なくとも前記姿勢角速度検出手段の検出値と前記無滑り姿勢角速度推定手段が推定した姿勢角速度とに応じて前記姿勢角速度検出手段の検出値に対するドリフト補正値を決定するドリフト補正値決定手段と、少なくとも前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度を積分することにより前記所定の部位の姿勢角を推定する積分手段とを備え、前記ドリフト補正値決定手段は、前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度と前記無滑り姿勢角速度推定手段が推定した姿勢角速度との偏差を 0に近づけるように新たなドリフト補正値を決定することを特徴とするものである。

かかる第 1発明では、少なくとも前記目標歩容の目標運動と前記ロボットの関節の変位検出値と該関節の変位目標値とのうちのいずれかを含む該ロポッ卜の運動状態量を基に、前記ロポットと床との接触面に滑りが無いと仮定して、前記所定の部位の姿勢角速度が推定される。すなわち、上記滑りが無いと仮定することで、前記運動状態量を基に、幾何学的な演算によって、前記所定の部位の姿勢角を推定することがすることができるので、その姿勢角の時間的変化量として該所定の部位の姿勢角速度を推定することができる。そして、少なくとも、この推定した姿勢角速度（姿勢角速度推定値）と、姿勢角速度検出手段の検出値（姿勢角速度検出値）とに応じて前記ドリフト補正値が決定される。より詳しくは、姿勢角速度検出値をドリフト補正値（既に決定したドリフト補正値）により補正してなる姿勢角速度と上記姿勢角速度推定値との偏差を 0に近づけるように新たなドリフト補正値が決定される（ドリフト補正値が更新される）。なお、どのドリフト補正値を決定するときには、例えば前記偏差から適宜のフィ一ドバック制御則によりドリフト補正値を求めるようにすればよい。そして、少なくとも上記決定したドリフト補正値により姿勢角速度検出値を補正してなる姿勢角速度を積分することで、前記所定の部位の姿勢角が推定される。これにより、第 1発明によれば、姿勢角速度の積分に伴うドリフトの影響を補償して、口ポットの自己姿勢としての前記所定の部位の姿勢角を精度よく推定することが可能となる。

かかる第 1発明では、前記ドリフ卜補正値決定手段は、前記口ポットと床との接触面に回転滑りが生じているか否かを判断する手段を備え、該回転滑りが生じていると判断したときには、前記ドリフト補正値の値を保持することが好ましい（第 2発明）。

すなわち、第 1発明は、口ポットと床との接触面に滑りがないと仮定した上で推定した姿勢角速度（以下の本発明の説明では、滑りなし推定姿勢角速度という）を用いて前記ドリフト補正値を決定するものであるが、該滑りなし推定姿勢角速度は、回転滑りを生じている状況では、信頼性が低下する。そこで、第 2発明では、回転滑りが生じていると判断される状況では、ドリフト補正値の値を保持する（更新しない）。これにより、該ドリフト補正値の信頼性を確保し、ひいては、口ポットの自己姿勢としての前記所定の部位の姿勢角の推定値の精度を高い精度に確保することが可能となる。

また、本発明の脚式移動口ポットの自己姿勢推定装置の第 3発明は、脚式移動ロポットの所定の部位の姿勢角速度を検出する姿勢角速度検出手段と、少なくとも前記ロボットの運動停止状態における前記姿勢角速度検出手段の検出値を基に、該検出値に対するドリフト補正値を決定するドリフト補正値決定手段と、前記ロポッ卜の運動中に少なくとも前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度を積分することにより前記所定の部位の姿勢角を推定する積分手段とを備えたことを特徴とするものである。

かかる第 3発明は、ロポットの運動停止状態における姿勢角速度検出手段の検出値（姿勢角速度検出値）、すなわち、実姿勢角速度が原理的に 0となる状態での姿勢角速度検出値を基に、ドリフト補正値を決定すので、信頼性の高いドリフト補正値を決定できる。そして、口ポットの運動中に、少なくともそのドリフト補正値により、姿勢角速度検出値を補正してなる姿勢角速度を積分することで、前記所定の部位の姿勢角を推定するので、姿勢角速度の積分に伴うドリフトの影響を適正に補償して、ロボットの自己姿勢としての前記所定の部位の姿勢角を精度よく推定することが可能となる。

前記した第 1又は第 2発明では、前記ドリフ卜補正値決定手段は、前記姿勢角速度検出手段の検出値のうちのョ一方向の成分と前記無滑り姿勢角速度推定手段が推定した姿勢角速度のうちのョー方向の成分とに応じて前記ドリフト補正値を決定することが好ましい（第 4発明）。

同様に、前記第 3発明では、前記ドリフト補正値決定手段は、前記姿勢角速度検出手段の検出値のうちのョー方向の成分に応じて前記ドリフト補正値を決定することが好ましい（第 5発明）。

これらの第 4発明及び第 5発明によれば、姿勢角速度検出手段のョー方向のドリフトを適正に捕償することができるので、特に前記所定の部位の向きを精度よく推定することができ、ひいては、口ポットの移動方向を精度よく推定することが可能となる。

なお、前記第 1〜第 5発明では、前記所定の部位は、前記口ポットの上体であることが好ましい（第 6発明）。上体の姿勢を精度よく推定できるので、ロポッ卜の姿勢安定化等の制御を的確に行うことが可能となる。図面の簡単な説明

図 1は、本発明の実施形態における脚式移動ロボットとしての 2足移動ロポッ卜の全体的構成の概略を示す概略図、図 2は図 1の各脚体の足平部分の構成を概略的に示す概略図、図 3及び図 4はそれぞれ各脚体の足平部分の詳細構成を示す側面視の断面図及び底面図、図 5は図 1の口ポットに備えた制御ュニットの構成を示すブロック図、図 6は図 5の制御ュニットの機能的構成を示すプロック図である。図 7は口ポットの走行歩容を示す説明図、図 8 ( a), (b), ( c ) はそれぞれ目標歩容の床反力鉛直成分、目標 Z MP、自己位置姿勢推定用のゲインの設定例を示すグラフ、図 9は第 1実施形態における制御ユニットの要部の処理を示すフローチャート、図 1 0は図 9のフ口一チヤ一卜の自己位置姿勢推定処理を示すフローチャート、図 1 1及び図 1 2は図 9のフ口一チャートの自己位置姿勢推定処理を説明するための図である。図 1 3は図 9のフローチャートの自己位置姿勢推定処理の要部の処理を示すブロック図、図 1 4 ( a), (b) は図 1 3の処理を説明するための図、図 1 5は図 1 3の処理で用いるゲインの設定例を示すグラフである。

発明を実施するための最良の形態

以下、添付図面を参照してこの発明の実施形態に係る脚式移動ロポットの自己位置推定装置を説明する。尚、脚式移動口ポットとしては 2足移動ロポッ卜を例にとる。

図 1は、この実施形態に係る脚式移動ロポットとしての 2足移動ロボットを全体的に示す概略 ¾である。

図示の如く、 2足移動口ポット（以下、口ポットという） 1 は上体 (口ポット 1の基体） 3から下方に延設された左右一対の脚体（脚部リンク） 2 , 2を備える。両脚体 2 , 2は同一構造であり、それぞれ 6個の関節を備える。その 6個の関節は上体 3側から順に、股（腰部）の回旋（回転）用（上体 3に対するョ一方向の回転用）の関節 1 0 R, 1 0 L (符号 R, Lはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ）と、股（腰部）の口一ル方向（X 軸まわり）の回転用の関節 1 2 R， 1 2 Lと、股（腰部）のピッチ方向 (Y軸まわり）の回転用の関節 1 4 R, 1 4 L、膝部のピッチ方向の回転用の関節 1 6 R， 1 6 Lと、足首のピッチ方向の回転用の関節 1 8 R, 1 8 Lと、足首の口一ル方向の回転用の関節 2 0 R， 2 0 Lとから構成される。

各脚体 2の足首の 2つの関節 1 8 R (L ), 2 O R (L) の下部には, 各脚体 2の先端部を構成する足平（足部） 2 2 R (L) が取着されると共に、両脚体 2， 2の最上位には、各脚体 2の股の 3つの関節 1 O R (L), 1 2 R (L)， 1 4 R (L) を介して前記上体 3が取り付けられている。上体 3の内部には、詳細を後述する制御ユニット 6 0などが格納される。なお、図 1では図示の便宜上、制御ユニット 6 0を上体 3の外部に記載している。

上記構成の各脚体 2においては、股関節（あるいは腰関節）は関節 1 O R (L), 1 2 R (L)， 1 4 R (L) から構成され、膝関節は関節 1 6 R (L) から構成され、足首関節は関節 1 8 R (L), 2 0 R (L ) から構成される。また股関節と膝関節とは大腿リンク 2 4 R (L) で連結され、膝関節と足首関節とは下腿リンク 2 6 R (L) で連結される。尚、上体 3の上部の両側部には左右一対の腕体 5， 5が取り付けられると共に、上体 3の上端部には頭部 4が配置される。これらの腕体 5， 5及び頭部 4は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。

上記の構成により、各脚体 2の足平 2 2 R (L) は、上体 3に対して 6つの自由度を与えられている。そして、ロボット 1の歩行等の移動中に、両脚体 2 , 2を合わせて 6 * 2 = 1 2個（この明細書で「 *」はスカラに対する演算としては乗算を、べクトルに対する演算としては外積を示す）の関節を適宜な角度で駆動することで、両足平 2 2 R， 2 2 L の所望の運動を行うことができる。これにより、ロボット 1は任意に 3 次元空間を移動することができる。

図 1に示す如く、各脚体 2の足首関節 1 8 R ( L ) , 2 0 R ( L ) の下方には足平 2 2 R ( L ) との間に公知の 6軸力センサ 5 0が介装されている。該 6軸力センサ 5 0は、各脚体 2の足平 2 2 R ( L ) の着地の有無、および各脚体 2に作用する床反力（接地荷重）等を検出するためのものであり、該床反力の並進力の 3方向成分 F X , F y , F z並びにモーメントの 3方向成分 M X , M y， M z の検出信号を制御ユニット 6 0に出力する。また、上体 3には、 Z軸（鉛直方向（重力方向））に対する上体 3の傾き（姿勢角）およびその角速度等を検出するための傾斜センサ 5 4が備えられ、その検出信号が該傾斜センサ 5 4から制御ュニット 6 0に出力される。この傾斜センサ 5 4は、図示を省略する 3軸方向の加速度センサおよび 3軸方向のジャィ口センサを備え、これらのセンサの検出信号が上体 3の傾きおよびその角速度を検出するために用いられると共に、ロボット 1の自己位置姿勢を推定するために用いられる, また、詳細構造の図示は省略するが、口ポット 1の各関節には、それを駆動するための電動モータ 6 4 (図 5参照）と、その電動モー夕 6 4の回転量（各関節の回転角）を検出するためのエンコーダ（ロータリエンコーダ） 6 5 (図 5参照）とが設けられ、該エンコーダ 6 5の検出信号が該エンコーダ 6 5から制御ュニット 6 0に出力される。

さらに、図 1では図示を省略するが、口ポット 1の適宜な位置にはジョィスティック（操作器） 7 3 (図 5参照）が設けられ、そのジョイスティック 7 3を操作することで、直進移動している口ポット 1を旋回させるなど、口ポット 1の歩容に対する要求を必要に応じて制御ュニット 6 0に入力できるように構成されている。

図 2は本実施形態における各脚体 2の先端部分（各足平 2 2 R (L) を含む）の基本構成を概略的に示す図である。同図に示すように、各足平 2 2 R (L) の上方には、前記 6軸力センサ 5 0 との間にばね機構 7 0が装備されると共に、足底（各足平 2 2 R, Lの底面）にはゴムなどからなる足底弾性体 7 1が貼られている。これらのばね機構 7 0及び足底弾性体 7 1によりコンプライアンス機構 7 2が構成されている。ばね機構 7 0は詳細は後述するが、足平 2 2 R (L) の上面部に取り付けられた方形状のガイド部材（図 2では図示省略）と、足首関節 1 8 R (L) (図 2では足首関節 2 O R (L) を省略している）および 6軸力センサ 5 0側に取り付けられ、前記ガイド部材に弾性材（ゴムやばね）を介して微動自在に収納されるピストン状部材（図 2では図示省略）とから構成されている。

図 2に実線で表示された足平 2 2 R (L) は、床反力を受けていないときの状態を示している。各脚体 2が床反力を受けると、コンプライアンス機構 7 2のばね機構 7 0と足底弾性体 Ί 1とがたわみ、足平 2 2 R (L) は図中に点線で例示したような位置姿勢に移る。このコンプラインァス機構 7 2の構造は、例えば本出願人が先に提案した特開平 5 — 3 0 5 5 8 4号公報に詳細に説明されている如く、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。

上記コンプライアンス機構 7 2を含めた足平 2 2 R (L) (以下、足平機構 2 2 R (L) と称することがある。）のより詳細な構成を図 3及び図 4を参照してさらに説明する。図 3は足平機構 2 2 R (L) の側面示の断面図、図 4は該足平機構 2 2 R (L) の底面側から見た平面図である。

足平機構 2 2 R ( L ) は、大略平板状の足平プレート部材 1 0 2を骨格部材として備えている。この足平プレート部材 1 0 2は、その前端部 (つま先部）と後端部（踵部）とが若干上方に湾曲されているが、他の部分は平坦な平板状になっている。また、足平プレート部材 1 0 2の上面部には、横断面方形状のガイド部材 1 0 3がその軸心を上下方向に向けて固設されている。このガイド部材 1 0 3の内部には、該ガイド部材 1 0 3の内周面に沿うようにして略上下方向に移動可能に設けられた可動板（ピストン状部材） 1 0 4が設けられ、該可動板 1 0 4が足首関節 1 8 R ( L )， 2 0 R ( L ) に 6軸力センサ 5 0を介して連結されている。

また、可動板 1 0 4は、その下面の周縁部がばね、ゴム等の弾性材からなる複数の弾性部材 1 0 6 (図ではばねとして記載している）を介して足平プレート部材 1 0 2の上面部に連結されている。従って、足平プレート部材 1 0 2は、弹性部材 1 0 6、可動板 1 0 4及び 6軸力センサ 5 0を介して足首関節 1 8 R ( L ) に連結されている。尚、ガイド部材 1 0 3の内部（可動板 1 0 4の下側の空間）は、図示を省略する穴や隙間を介して大気側に開放されており、大気中の空気がガイド部材 1 0 3 の内部に入出自在となっている。また、上記ガイド部材 1 0 3、可動板 1 0 4、及び弾性部材 1 0 6は前記図 2に示したばね機構 7 0を構成するものである。

足平プレート部材 1 0 2の底面（下面）には、前記図 2に示した足底弾性体 7 1 としての接地部材 7 1が取着されている。該接地部材 7 1は足平機構 2 2 R ( L ) の接地状態で、該足平プレート部材 1 0 2と床面との間に介在させる弾性部材（床面に直接的に接触する弾性部材）であり、本実施形態では、足平プレート部材 1 0 2の接地面の四隅（足平プレート部材 1 0 2のつま先部の両側部並びに踵部の両側部）に固着されている。

また、接地部材 7 1は、本実施形態では、比較的軟質のゴム材から成る軟質層 1 0 7 aと、比較的硬質のゴム材から成る硬質層 1 0 7 bとを上下に重合してなる 2層構造に形成され、硬質層 1 0 7 bが、脚体 2の着床時に直接的に床面に接触する接地面部として最下面側に設けられている。

足平機構 2 2 R ( L ) には、上記の構成の他、着地衝撃緩衝装置 1 0 8が備えられている。この着地衝撃緩衝装置 1 0 8は、足平プレート部材 1 0 2の底面に取着された袋状部材 1 0 9と、該袋状部材 1 0 9の内部に対して圧縮性流体としての空気（大気中の空気）を入出させるための流通路 1 1 0とを備えている。

袋状部材 1 0 9は、その周囲に前記接地部材 7 1が存するようにして. 足平プレート部材 1 0 2の底面の大略中央部に設けられている。この袋状部材 1 0 9は、ゴム等の弾性材により変形自在に構成されており、外力による弾性変形が生じていない自然状態では、図 3に実線で示すように、上方に開口した円筒容器形状を呈する。そして、該袋状部材 1 0 9 は、その開口端部が全周にわたって足平プレー卜部材 1 0 2の底面に固着され、該足平プレート部材 1 0 2により閉蓋されている。また、袋状部材 1 0 9は、円筒容器形状を呈する自然状態では、該袋状部材 1 0 9 の底部が前記接地部材 7 1よりも下方に突出するように設けられている, つまり、該袋状部材 1 0 9の高さ（足平プレート部材 1 0 2の下面から袋状部材 1 0 9の底部までの距離）は、接地部材 7 1の厚さよりも大きいものとされている。従って、足平プレート部材 1 0 2が接地部材 7 1 を介して接地した状態（脚部 2の着床状態）では、袋状部材 1 0 9は、図 3に仮想線で示すように、床反力により袋状部材 1 0 9の高さ方向に圧縮される。

尚、本実施形態では、袋状部材 1 0 9が円筒容器形状を呈する自然状態は該袋状部材 1 0 9の膨張状態である。そして、袋状部材 1 0 9は、弹性材により構成されているため、圧縮されたとき、自然状態の形状 (円筒容器形状）への形状復元力を有する。

前記流通路 1 1 0は、袋状部材 1 0 9に対する空気の流入 · 流出を行う流入 · 流出手段を構成するものであり、本実施形態では、袋状部材 1 0 9の内部と前記ガイド部材 1 0 3の内部とを連通させるように足平プレート部材 1 0 2に穿設された流通孔である。この場合、前述のように、ガイド部材 1 0 3の内部は大気側に開放されているので、該流通路 1 1 0は、袋状部材 1 0 9の内部を大気側に連通させていることとなる。従つて、袋状部材 1 0 9の内部には、大気中の空気が流通路 1 1 0を介して入出自在となっており、該袋状部材 1 0 9の膨張状態（自然状態）では、該袋状部材 1 0 9内には空気が充填され、その内部の圧力は大気圧と同等になる。また、流通路 1 1 0は絞り通路となっており、袋状部材 1 0 9の内部に空気が入出する際には流体抵抗を生じるようになつている。

図 5は制御ュニット 6 0の構成を示すブロック図である。該制御ュニット 6 0はマイクロコンピュータにより構成されており、 C P Uからなる第 1の演算装置 9 0及び第 2の演算装置 9 2、 A / D変換器 8 0、力ゥンタ 8 6 、 D / A変換器 9 6 、 R A M 8 4 , R〇M 9 4、並びにこれらの間のデ一夕授受を行うバスライン 8 2を備えている。この制御ュニット 6 0では、各脚体 2の 6軸力センサ 5 0、傾斜センサ 5 4 (加速度センサおよびレートジャィ口センサ）、ジョイスティック 7 3等の出力信号は A Z D変換器 8 0でデジタル値に変換された後、バスライン 8 2 を介して R A M 8 4に送られる。またロポット 1の各関節のエンコーダ P T/JP03/05449

1 4

6 5 (ロータリーエンコーダ）の出力は、カウンタ 8 6を介して R A M 8 4に入力される。

前記第 1の演算装置 9 0は後述の如く目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モー夕 6 4の回転角の指令値）を算出し、 R A M 8 4に送出する。また第 2の演算装置 9 2は R A M 8 4から関節角変位指令と、前記エンコーダ 6 5の出力信号とに基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出して D / A変換器 9 6とサーポアンプ 6 4 aとを介して各関節を駆動する電動モータ 6 4に出力する。

図 6は、この実施形態に係る脚式移動口ポットの制御装置の機能的構成を全体的に示すブロック図である。この図 6中の「実口ポット」の部分以外の部分が制御ュニット 6 0が実行する処理機能（主として第 1の演算装置 9 0及び第 2の演算装置 9 2の機能）によって構成されるものである。なお、以下の説明では、脚体 2の左右を特に区別する必要がないときは、前記符号 R , Lを省略する。

以下説明すると、制御ユニット 6 0は、口ポット 1の目標歩容を自在かつリアルタイムに生成して出力する歩容生成装置 2 0 0、自己位置姿勢推定部 2 1 4等を備えている。自己位置姿勢推定部 2 1 4は、詳細は後述するが、本発明の特徴部分に係わる処理を実行し、口ポット 1の位置 ·姿勢（グローバル座標系での位置 '姿勢）を推定するものである。歩容生成装置 2 0 0が出力する目標歩容は、目標上体位置姿勢軌道 (上体 3の目標位置及び目標姿勢の軌道）、目標足平位置姿勢軌道（各足平 2 2の目標位置及び目標姿勢の軌道）、目標腕姿勢軌道（各腕体 5 の目標姿勢の軌道）、目標全床反力中心点（目標 Z M P ) 軌道、目標全床反力軌道から構成される。なお、脚体 2や腕体 5以外に上体 3に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。

ここで、本発明の実施形態での用語の意味あるいは定義について補足しておく。上記歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味し、以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、各部位の「姿勢」は、該部位の傾きと向きとを総称したものである。ここで、「傾き」は該部位の鉛直方向となす角度であり、「向き」は該部位の前方向を示すベクトルを水平面に投影したベクトルの向きである。例えば上体姿勢のうちの傾きは、 Z軸 (鉛直軸）に対するロール方向（X軸回り）の上体 3の傾斜角（姿勢角）と、 Z軸に対するピッチ方向（Y軸回り）の上体 3の傾斜角（姿勢角）とからなる。また、上体 3の向きは、上体 3の前方向を示すべクトルを水平面に投影したベクトルのョ一方向（Z軸回り）の回転角で表される。なお、足平姿勢は各足平 2 2に固定的に設定された 2軸の空間的な方位角で表される。また、特に、足平 2 2の着地姿勢に関しては、その着地姿勢は、基本的には着地した足平 2 2の向きを表し、具体的には着地した足平 2 2のかかとからつま先に向かうべクトルを水平面に投影したベクトルの向きを表す。また、目標腕姿勢は、腕体 5のすベての部位に関する上体 3に対する相対的な姿勢で表される。

上体位置は、上体 3の所定位置、具体的には上体 3のあらかじめ定めた代表点の位置を意味する。同様に、足平位置は、各足平 2 2 R , 2 2 Lのあらかじめ定めた代表点の位置を意味する。なお、上体速度は、上体 3の上記代表点の移動速度を意味し、足平速度は、各足平 2 2 R , 2 2 Lの上記代表点の移動速度を意味する。

目標上体位置姿勢等の目標歩容に関し、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。また、歩容のうちの、床反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、ロボット 1 の運動に係わる歩容を総称的に「運動」という。

各足平 2 2 R， Lの床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、口ポット 1の全ての（ 2本の）足平 2 2 R : 2 2 Lの床反力の合力を「全床反力」と呼ぶ。ただし、以下の説明においては、各足平床反力に関してはほとんど言及しないので、断らない限り、「床反力」は「全床反力」と同義として扱う。

目標床反力は、一般的には、作用点とその点に作用する力（並進力）と力のモーメントによつて表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に前述の目標床反力中心点を作用点にして目標床反力を表現すると、力のモーメントは、鉛直軸成分を除けば、 0になる。

尚、動力学的平衡条件を満足する歩容では、目標運動軌道から算出される Z M P (目標運動軌道から算出されるロポット 1の慣性力と重力との合力がその点まわりに作用するモーメントが、鉛直軸成分を除いて 0 になる点）と目標全床反力中心点は一致することから、目標全床反力中心点軌道の代わりに目標 Z M P軌道を与えると言っても同じことである (詳細は、例えば本出願人による P C T公開公報 WO/ 02/ 4022 を参照）。

このような背景から、 P C T公開公報 WO/ 02Z 40224の明細書では目標歩容を次のように定義していた。

a ) 広義の目標歩容とは、 1歩ないしは複数歩の期間の目標運動軌道とその目標床反力軌道との組である。

b ) 狭義の目標歩容とは、 1歩の期間の目標運動軌道とその Z M P軌道との組である。

c )一連の歩容は、いくつかの歩容がつながったものとする。口ポット 1の歩行を行う場合においては、本出願人が先に特開平 10- 86080号公報で提案した上体高さ決定手法によって上体鉛直位置（上体高さ）が決定されると、床反力の並進力成分は従属的に決定されるので. 目標歩容の床反力に関して明示的に設定すべき物理量としては、 Z M P だけで十分であった。したがって、 P C T公開公報 WOZ 02/ 40224の明細書では、狭義の目標歩容としては、上記の b ) で十分であった。それヒ対し、口ポット 1の走行を行う楊合には、床反力鉛直成分も制御上重要であるので、該床反力鉛直成分を明示的に設定することが好ましい, そこで、本願出願人が先に提案した P C T出願（PCT Z JP02 / 13596) 等では、狭義の目標歩容として、次の b ' ) を採用した。

b ' ) 狭義の目標歩容とは、 1歩の期間の目標運動軌道とその Z M P軌道と床反力鉛直成分軌道の組である。

この明細書では以降、特にことわらない限り、目標歩容は狭義の目標歩容の意味で使用する。また、目標歩容の「 1歩」は、口ポット 1の片方の脚体 2が着地してからもう一方の脚体 2が着地するまでの意味で使用する。

歩容における両脚支持期とは言うまでもなく、口ポット 1がその自重を両脚体 2， 2で支持する期間、片脚支持期とはいずれか一方のみの脚体 2で口ポット 1の自重を支持する期間、空中期とは両脚体 2 , 2が床から離れている（空中に浮いている）期間を言う。

片脚支持期においてロボット 1の自重を支持しない側の脚体 2を「遊脚」と呼び、自重を支持する側の脚体 2を「支持脚」と呼ぶ。口ポット 1の歩行では、両脚支持期と片脚支持期とが交互に繰り返され、ロポット 1の走行では片脚支持期と空中期とが交互に繰り返される。この場合. 走行の空中期では、両脚体 2 , 2とも、口ポット 1の自重を支持しないこととなるが、該空中期の直前の片脚支持期において遊脚であった脚体 2、支持脚であった脚体 2をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。

また、目標上体姿勢、目標上体位置、目標足平位置姿勢、目標腕姿勢等、目標歩容におけるロポット 1の各部の位置姿勢は支持脚座標系で記述される。支持脚座標系とは、支持脚の足平 2 2の接地面辺りに原点を持つ床面に固定された座標系である。より詳細には、支持脚座標系は、本出願人の特許 3273443 号に記載されているように、支持脚の足平 2 2を接地面との間で滑らさないで、水平姿勢になるまで回転させた時の. 該支持脚の足首関節の中心から接地面への垂直投影点を原点とし、該支持脚足平 2 2のつま先に向かう水平軸（足平 2 2の前後方向の軸）を X 軸として、鉛直軸を Z軸、これらの X軸、 Z軸に直交する座標軸（足平 2 2の左右方向の軸）を Y軸とする座標系である。

本発明の実施形態に係る歩容生成装置 2 0 0は、 2歩先までの遊脚の足平 2 2の着地位置姿勢、着地時刻の要求値（目標値）を入力として、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標 Z M P軌道、目標床反力鉛直成分軌道、及び目標腕姿勢軌道から構成される目標歩容を生成する。このとき、これらの軌道を規定するパラメータ（これを歩容パラメ一夕と呼ぶ）の一部は、歩容の継続性を満足するように修正される, 目標歩容の生成にはロポット 1の動力学モデルが用いられる。その動力学モデルとしては、例えば、前記 P C T公開公報 WO/ 02/ 40224号に記載の単純化モデルあるいは、本出願人が提案した特開 2002- 326173 号公報に記載の多質点モデル（フルモデル）などを用いれば良い。

また、歩容生成装置 2 0 0は、口ポット 1の片方の脚体 2が着地してから他方の脚体 2が着地するまでの 1歩分の目標歩容（前記狭義の意味での目標歩容）を単位として、その 1歩分の目標歩容を順番に生成する, ここで、現在あるいはこれから生成しょうとしている歩容を「今回歩容」、その次の歩容を「次回歩容」、さらにその次の歩容を「次次回歩容」と呼ぶ。また、「今回歩容」の 1つ前に生成した目標歩容を「前回歩容」と呼ぶ。

歩容生成装置 2 0 0が生成する目標歩容の一部を例示的に概説すると. 例えば目標足平位置姿勢軌道は、本出願人による特許 3233450 号に開示した有限時間整定フィル夕を用いて生成される。この有限時間整定フィル夕による足平位置姿勢軌道の生成処理では、例えば足平位置軌道は. 目標着地位置（着地位置の要求値）に向かって足平 2 2を徐々に加速しながら移動を開始し、目標着地時刻（着地時刻の要求値）までに徐々に速度を 0またはほぼ 0にまで減速し、該目標着地時刻に目標着地位置に到達して停止するように生成される。足平姿勢軌道についても同様である。これにより生成される目標足平位置姿勢軌道は、着地瞬間における対地速度が 0またはほぼ 0になるため、特にロボット 1 の走行を行う場合に、前記空中期からの着地時における着地衝撃を小さくできる。

また、図 7に示すように人間が走行を行う場合と同様の形態でロポット 1の走行を行う場合には、例えば目標床反力鉛直成分軌道および目標 Z M P軌道（詳しくは支持脚座標系の X軸方向（支持脚足平 2 2の前後方向）での目標 Z M P軌道）は、それぞれ図 8 ( a )、図 8 ( b ) に実線で示すようなパターンで設定される。なお、図 7の第 1番目〜第 3番目の図は、それぞれ片脚支持期の開始時、中間時点、終了時における口ポット 1 の両脚体 2 , 2の運動状態を模式的に示し、第 4番目及び第 5 番目の図は、それぞれ空中期の中間時点、空中期の終了時（次の片脚支持期の開始時）における口ポット 1の両脚体 2 , 2の運動状態を模式的に示している。

ロボット 1 の走行を行う場合には、目標床反力鉛直成分軌道は、基本的には、片脚支持期で.は上に凸のパターンとなり、空中期では 0に維持される。また、口ポット 1の歩行を行う場合には、目標床反力鉛直成分軌道は、例えば図 8 ( a ) に二点鎖線で示すように設定される。この場合、二点鎖線のうちの上に凸の部分が両脚支持期に対応し、下に凸の部分が片脚支持期に対応する。また、目標 Z M Pは走行、歩行のいずれであっても、基本的には、口ポット 1の脚体 2の接地面内（より詳しくは所謂、支持多角形内）の中央付近に設定される。

図 9は、前記歩容生成装置 2 0 0の歩容生成処理、ならびに、図 6に示す自己位置姿勢推定部 2 1 4の自己位置姿勢推定処理を示すフローチャ一ト（構造化フローチャート）である。

まず S 0 1 0において時刻 t を 0に初期化するなど種々の初期化作業を行う。

次いで S 0 1 2を経て S 0 1 4に進み、制御周期毎のタイマ割り込みを待つ。制御周期は A tである。

次いで S O 1 6に進み、自己位置姿勢推定部 2 1 4による自己位置姿勢推定を行う。 S 0 1 6における処理がこの出願に係る脚式移動ロポットの自己姿勢推定装置の特徴をなすが、この説明は後述することとする, 次いで S 0 1 8に進み、歩容切り替わり目（前回歩容の生成が終了し. 新たな今回歩容の生成を開始すべき時刻）であるか否かが判断される。そして、その判断結果が Y E Sであるときは S 0 2 0に進み、 N Oであるときは S 0 3 2に進む。なお、以下に説明する S 0 2 0以降の処理は. 本願出願人が先に提案した P C T公開公報 WOノ 02/ 40224あるいは前記 P C T出願 PCTZJP02/ 13596 に詳細に説明されているので、本明細書では簡略的な説明に留める。

S 0 2 0に進むときは時刻 t を 0に初期化する。次いで S O 2 2に進み、次回歩容支持脚座標系（詳しくはその位 #および向き）、次次回歩容支持脚座標系（詳しくはその位置および向き）、今回歩容周期および次回歩容周期を読み込む。

上記次回歩容支持脚座標系および次次回歩容支持脚座標系は、それぞれ、前記ジョイスティック 7 3の操作等によって指定される 1歩目の遊脚足平 2 2 (今回歩容の遊脚足平 2 2 ) の着地位置姿勢の要求値（目標着地位置姿勢）、 2歩目の遊脚足平 2 2 (次回歩容の遊脚足平 2 2 ) の着地位置姿勢の要求値（目標着地位置姿勢）に応じて、前記した支持脚座標系の定義に従って決定される。

また、今回歩容周期、次回歩容周期は、それぞれ、 1歩目の遊脚足平 2 2の着地時刻の要求値（目標着地時刻）、 2歩目の遊脚足平 2 2の着地時刻の要求値（目標着地時刻）に応じて決定される。

上記した遊脚足平 2 2の着地位置姿勢の要求値並びに着地時刻の要求値、あるいは支持脚座標系の位置および向き並びに歩容周期は、あらかじめ歩行スケジュールとして記憶しておいても良く、あるいはジョイスティック 7 3などの操縦装置からの指令（要求）とそのときまでの歩行履歴を基に決定しても良い。

次いで S 0 2 4に進み、今回歩容につながる定常旋回歩容の歩容パラメータが、 S 0 2 2で決定された次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期等に基づいて決定される < 主に、目標足平位置姿勢軌道を規定する足平軌道パラメ一夕、目標上体姿勢の基準軌道を規定する基準上体姿勢軌道パラメータ、目標腕姿勢軌道を規定する腕姿勢軌道パラメ一夕、目標 Z M P軌道を規定する Z M P 軌道パラメ一夕、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメ一夕が決定される。例えば床反力鉛直成分軌道パラメ一夕に関して例示すると、前記図 8 ( a ) に示したパターンの折れ点の時刻や値が床反力鉛直成分軌道パラメータとして決定される。ここで、前記定常旋回歩容は、その歩容を繰り返したときに歩容の境界において口ポット 1の運動状態に不連続が生じないような周期的歩容を意味する（以降、「定常旋回歩容」を「定常歩容」と略す場合もある）, -定常旋回歩容の 1周期分の歩容は、第 1旋回歩容と第 2旋回歩容とからなる。第 1旋回歩容は、今回歩容の支持脚座標系に対応する支持脚足平 2 2を次次回歩容支持脚座標系に対応する位置姿勢まで動かすときの歩容に相当し、第 2旋回歩容は、次回歩容支持脚座標系に対応する支持脚足平 2 2を次次次回支持脚座標系に対応する位置姿勢まで動かすときの歩容に相当する。この場合、次次次回歩容支持脚座標系は、第 2旋回歩容の遊脚足平 2 2の目標着地位置姿勢に対応するものである。そして. 該次次次回歩容支持脚座標系は、次次回歩容支持脚座標系（第 2旋回歩容の支持脚座標系）から見た該次次次回歩容支持脚座標系の位置姿勢 (位置及び向き）が、今回歩容支持脚座標系から見た次回歩容支持脚座標系（今回歩容の遊脚足平 2 2の着地位置姿勢）の位置姿勢（位置及び向き）に一致するように設定される。尚、定常旋回歩容に関して「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。

定常旋回歩容は、歩容生成装置 2 0 0で今回歩容の終端における発散成分や上体鉛直位置速度を決定するために暫定的に作成される仮想的な周期的歩容であり、口ポット 1を実際に制御するために歩容生成装置 2 0 0からそのまま出力されるものではない。

尚、「発散」とは、上体の位置が両足部（足平）の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、 2足移動口ポットの上体の位置が両足部（足平）の位置（厳密には、支持脚接地面に設定された支持脚座標系の原点からかけ離れていく具合を表す数値であり、上体 3の水平方向の位置及びその速度の関数で表される。本実施形態では、これから生成する今回歩容の後につながる定常歩容を移動要求（前記 2歩先までの遊脚の足平 2 2の着地位置姿勢、着地時刻などの要求値）に応じて設定し、定常歩容の初期発散成分を求めてから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致するように、今回歩容を生成するようにした。 S O 24の詳細は、本出願人が提案した前記 P C T公開公報 WOZ02Z40224、あるいは PCTZJP02/ 13596に説明されているので、これ以上の説明を省略する。

S 0 1 0から S 0 2 4までに示す処理を行って後、 S 0 2 6に進み、定常旋回歩容の初期状態（初期上体水平位置速度成分、初期上体鉛直位置速度、初期発散成分、初期上体姿勢角および角速度）を決定する。 S 0 2 6の詳細は、 P C T公開公報 WO/02/40224、あるいは PCTZ JP02Z13596 に説明しているので、ここでは、これ以上の説明を省略する。

次いで、 S 0 2 8に進み、今回歩容の歩容パラメ一夕を決定（一部仮決定）する。この場合、決定される今回歩容の歩容パラメ一夕は、定常旋回歩容の歩容パラメータと同様、主に、足平軌道パラメ一夕、基準上体姿勢軌道パラメータ、腕姿勢軌道パラメ一夕、目標 ZMP軌道パラメ一夕、目標床反力鉛直成分軌道パラメータであり、それぞれのパラメ一夕により規定される軌道が、定常旋回歩容の軌道に連続するように決定される。ただし、これらのパラメ一夕のうち、目標 Z MP軌道パラメ一タは暫定的なものである。この S 0 2 8の処理の詳細は、前記 P C T公開公報 WOZ02Z40224、あるいは PCT/JP02Z 13596 号等に説明されているので、ここではこれ以上の説明を省略する。

次いで S 0 3 0に進み、今回歩容の終端発散成分が定常歩容の初期発散成分に一致するように、今回歩容の歩容パラメ一夕を修正する。ここで修正される歩容パラメ一夕は、目標 Z M P軌道パラメータである。 S 0 3 0において今回歩容パラメ一夕を修正した後、あるいは S 0 1 8の判断結果が N Oである場合には、 S O 3 2に進み、今回歩容瞬時値を決定する。この処理の詳細は、前記 P C T公開公報 WO/ 02/ 40224, あるいは PCTZJP02Z 13596 号等に説明しているので、ここでは、これ以上の説明を省略する。

次いで S O 3 4に進み、 P C T出願 PCTZJP02Z 13596 号の実施形態の S 0 3 2の処理と同様、スピン力をキヤンセルするための腕振り動作を決定する。

次いで S 0 3 6に進み、歩容生成用時刻 tを Δ tだけ増やし、 S 0 1 4に戻り、以上のごとく歩容生成を続ける。

以上が、歩容生成装置 2 0 0における目標歩容生成処理、ならびに、自己位置姿勢推定部 2 1 4の自己位置姿勢推定処理である。

図 6を参照してこの実施形態に係る制御ュニット 6 0の制御処理をさらに説明すると、歩容生成装置 2 0 0において、上記したように目標歩容が生成される。生成された目標歩容のうち、目標上体位置姿勢（軌道）および目標腕姿勢軌道は、口ポット幾何学モデル（逆キネマテイクス演算部） 2 0 2に直接送られる。

また、目標足平位置姿勢（軌道）、目標 Z M P軌道（目標全床反力中心点軌道）、および目標全床反力（軌道）（目標床反力水平成分と目標床反力鉛直成分）は、複合コンプライアンス動作決定部 2 0 4に直接送られる一方、目標床反力分配器 2 0 6にも送られる。目標床反力分配器 2 0 6では、目標全床反力は各足平 2 2 R， 2 2 Lに分配され、目標各足平床反力中心点および目標各足平床反力が決定される。その決定された目標各足平床反力中心点および目標各足平床反力が複合コンプライアンス動作決定部 2 0 4に送られる。

複合コンプライアンス動作決定部 2 0 4では、機構変形補償付き修正目標足平位置姿勢軌道が生成され、それがロボッ卜幾何学モデル 2 0 2 に送られる。口ポット幾何学モデル 2 0 2は、目標上体位置姿勢（軌道）と機構変形補償付き修正目標足平位置姿勢（軌道）が入力されると，それらを満足する脚体 2 , 2の 1 2個の関節（ 1 O R ( L ) など）の関節変位指令（値）を算出して変位コントローラ 2 0 8に送る。変位コントロ一ラ 2 0 8は、ロボット幾何学モデル 2 0 2で算出された関節変位指令（値）を目標値としてロボッ卜 1の 1 2個の関節の変位を追従制御する。

ロボット 1 に生じた床反力（詳しくは実各足平床反力）は 6軸力センサ 5 0によって検出される。その検出値は前記複合コンプライアンス動作決定部 2 0 4に送られる。また、前記図 9の S 0 1 6で後述する如く求められた推定上体姿勢と歩容生成装置 2 0 0が生成した目標上体姿勢との差のうちの傾き成分、すなわち実上体姿勢角偏差 errx , Θ erry が姿勢安定化制御演算部 2 1 2に送られる。なお、 0 errx はロール方向（X軸回り）の傾き成分であり、 0 erry はピッチ方向（Y軸回り）の傾き成分である。この姿勢安定化制御演算部 2 1 2で、ロボット 1の上体姿勢の傾きを目標歩容の上体姿勢の傾きに復元するための目標全床反力中心点（目標 Z M P ) まわりの補償全床反力モーメント Mdmd が算出され、この補償全床反力モーメント Mdmd が複合コンプライアンス動作決定部 2 0 4に与えられる。複合コンプライアンス動作決定部 2 0 4は、入力値に基づいて目標足平位置姿勢を修正する。具体的には、複合コンプライアンス動作決定部 2 0 4では、実全床反力（すべての実足平床反力の合力で、並進力およびモーメントの両者を含む）が、目標各足平床反力の合力である目標全床反力と補償全床反力モーメント Mdmd との合力に一致するように、歩容生成装置 2 0 0から与えられた目標足平位置姿勢を修正して、機構変形補償付き修正目標足平位置姿勢（軌道）を決定する。ただしすベての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるベく一致させる。すなわち、足平位置姿勢及び床反力の各目標に対する制御偏差に重みを与えて、制御偏差（あるいは制御偏差の 2 乗）の重み付き平均が最小になるように制御する。

補足すると、機構変形補償付き修正目標足平位置姿勢（軌道）は、複合コンプライアンス動作決定部 2 0 4によって修正された床反力の目標値を発生させるために必要な足平の変形機構（円柱状ゴム、足底スポンジおよび衝撃吸収用の袋状のエアダンパー）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求めて、その変形量が発生するように修正した、目標足平位置姿勢（軌道）である。

以下に、本実施形態における S 0 1 6の自己位置姿勢推定処理について、そのフローチャートである図 1 0を用いて詳説する。

まず図 1 0の S 2 2 0 0において、上体 3の前記傾斜センサ 5 4に備えたジャイロセンサの検出値、すなわち上体 3の角速度（ 3軸方向の角速度）の検出値を積分器により積分して推定上体姿勢（以下、慣性航法的推定上体姿勢ということもある）を求める。この推定上体姿勢はグロ一パル座標系で記述される。なお、この処理では、前制御周期（前回以前の制御周期）で、後述する S 2 2 0 2から S 2 2 1 4の処理により求めた上体位置の推定値たる推定上体位置等を用いてジャィ口センサの検出値の積分誤差の蓄積によるドリフトを補正するドリフト補正も行われる。その詳細については後述する。

次いで、 S 2 2 0 2に進み、制御周期の間（前回制御周期と今回制御周期との間）におけるグローバル座標系から見た推定上体姿勢（S 2 2 0 0で求まる推定上体姿勢）の変化量と、該制御周期の間におけるグロ一バル座標系から見た目標歩容の上体姿勢の変化量との差を姿勢回転偏差変化量として算出する。換言すれば、グローバル座標系から見た推定上体姿勢の変化速度と、目標上体姿勢の変化速度との差を姿勢回転偏差変化量として算出する。

なお、グロ一バル座標系から見た目檩歩容の上体姿勢とは、現在の推定支持脚座標系上（より詳しくは、前回の制御周期に決定された推定支持脚座標系上）で、制御周期の間、上体 3の姿勢傾きやスピン（目標上体姿勢からのずれ）がなく口ポット 1が目標歩容通りに運動したと仮定した場合のグローバル座標系から見た上体姿勢のことである。ただし、推定支持脚座標系とは、実際のロボット 1の支持脚足平 2 2の推定位置姿勢に対応した支持脚座標系である。すなわち、推定支持脚座標系は、前記した支持脚座標系の定義によって、より具体的には、実際のロポット 1の支持脚足平 2 2を、その推定位置姿勢から、接地面との間で滑らさないで水平まで回転させた時の、該支持脚の足首中心から接地面への垂直投影点を原点とし、該支持脚足平 2 2のつま先に向かう水平軸を X 軸にとり、鉛直軸を Z軸、これらに直交する座標軸を Y軸にとった座標系である。

次いで S 2 2 0 4に進み、姿勢回転中心を決定する。具体的には、その瞬間の目標 Z M P (目標 Z M Pの現在値）を姿勢回転中心として決定する。なお、口ポット 1の走行における空中期では、例えば目標歩容における口ポット 1の重心を姿勢回転中心として決定する。

次いで S 2 2 0 6に進み、現在の推定支持脚座標系（前回の制御周期で決定された推定支持脚座標系で、図 1 1 に示す時刻 t _△ tにおける推定支持脚座標系）を、前記姿勢回転偏差変化量だけ、前記姿勢回転中心まわりに回転させた位置 · 姿勢を改めて現在の推定支持脚座標系（図 1 1に示す時刻 t における推定支持脚座標系）と決定する。

なお、推定支持脚座標系の原点および座標軸の向きは、グローバル座檩系によって表されるものとする。また、 S 0 1 0の初期化処理において、推定支持脚座標系の初期値（グローバル座標系での初期位置姿勢）がセッ卜されているものとする。

また、推定支持脚座標系の回転中心は、空中期のない通常の歩行においては、支持脚足平 2 2の滑り回転の回転中心を意味する。なお、走りなど、 1歩の途中に支持脚も床から離れている空中期においては、狭義の意味では支持脚足平 2 2の滑りが存在するわけではないので、空中での姿勢回転（あるいはスピン）は、推定支持脚座標系の回転中心まわりの回転で表現するのが望ましい。しかし、広義の意味で「支持脚足平の滑り」を推定支持脚座標系の回転中心まわりの姿勢回転と定義しても良いことである。

補足すると、支持脚座標系は、支持脚足平 2 2の接地面に対して設定するもので、その原点は、前述のように支持脚の足首中心から接地面への垂直投影点にしなくても良い。すなわち、支持脚座標系は、ロボット 1の運動を記述するための支持脚足平 2 2の近傍の仮想の床に設定されたローカル座標系である。結局、口ポット 1の姿勢回転（あるいはスピン）現象は、口ポット 1が前記仮想の床上で相対的には姿勢回転（あるいはスピン）しないで運動している状態を保ったまま、口ポット 1 ごと前記仮想の床を、グローバル座標系のなかである所定の点を回転中心として姿勢回転（あるいはスピン）させた現象とみなされる。

さらに言い替えると、口ポット 1の運動は、口一カル座標系において目標歩容あるいは関節変位検出値に従って運動しているロポット 1の全体が、ローカル座標系ごと、グローバル座標系に対して、ある所定の点 (姿勢回転中心）を回転中心として摂動回転をしているとみなすことができる。

したがって、姿勢回転中心は、推定支持脚座標系の回転中心であり、また、前記摂動回転の回転中心であるとも言える。

また、 S 2 2 0 6における姿勢回転後の現在の推定支持脚座標系の Z 軸の向きは一般には、鉛直方向を向かない。そこで、姿勢回転後に、現在の推定支持脚座標系の Z軸を鉛直方向に戻す（該 Z軸の鉛直方向に対する傾斜角度分だけ、次回歩容推定支持脚座標系をその原点回りに回転させる）ようにしてもよい。あるいは、 S 2 2 0 6の姿勢回転処理の際に、前記姿勢回転偏差変化量のうち、鉛直軸回りの成分（スピン成分）だけ、推定支持脚座標系を回転させるようにしてもよい。

次いで S 2 2 0 8に進み、着地時刻であるか否か、すなわち歩容の切り替わり目であるか否かを判定する。

S 2 2 0 8の判定結果が Y E Sである場合には、以下の処理を行う。すなわち、まず S 2 2 1 0に進み、図 1 2に示す如く、現在の推定支持脚座標系（S 2 2 0 6での姿勢回転後の推定支持脚座標系）に対する次回歩容推定支持脚座標系の相対位置姿勢関係が、目標歩容（前回歩容）における支持脚座標系に対する次回歩容支持脚座標系の相対位置姿勢関係と同一関係になるように、次回歩容推定支持脚座標系を決定する。なお、ここで言う次回歩容支持脚座標系は、これから生成しょうとしている今回歩容に対する次回歩容支持脚座標系ではなく、前回歩容の次の歩容（つまり今回歩容）の支持脚座標系のことである。また、目標歩容の支持脚座標系はその Z軸が鉛直方向を向く座標系であるので、姿勢回転後の現在の推定支持脚座標系に対して上記の相対位置姿勢関係を満たす次回歩容推定支持脚座標系の Z軸は、一般には鉛直方向を向かない。そこで、本実施形態では、姿勢回転後の現在の推定支持脚座標系に対して上記の相対位置姿勢関係を満たす次回歩容推定支持脚座標系の Z軸を鉛直方向に戻す（該 Z軸の鉛直方向に対する傾斜角度分だけ、次回歩容推定支持脚座標系をその原点回りに回転させる）ことで、最終的に次回歩容推定支持脚座標系を決定するようにしている。なお、 S 2 2 0 6において、前述の如く推定支持脚座標の Z軸を鉛直方向に向けるようにした場合には、 S 2 2 1 0で次回歩容推定支持脚座標系の Z軸を鉛直方向に戻す処理は不要である。

次いで S 2 2 1 2に進み、 S 2 2 1 0で決定した次回歩容推定支持脚座標系の位置姿勢（原点の位置及び座標軸の向き）を現在の推定支持脚座標系の位置姿勢に代入する。つまり、現在の推定支持脚座標系の位置姿勢を次回歩容推定支持脚座標系の位置姿勢に置き換える。

S 2 2 1 2の後、あるいは S 2 2 0 8の判定結果が N〇である場合には、 S 2 2 1 4に進み、現在の推定支持脚座標系の位置姿勢に対応して. グローバル座標系での上体位置の推定値たる推定上体位置を決定する。ただし、現在の推定支持脚座標系に対する推定上体位置の関係は、目標歩容（前回の制御周期で決定された目標歩容）の支持脚座標系に対する上体位置（目標上体位置）の関係に一致させる。

次いで、 S 2 2 1 6に進み、前記傾斜センサ 5 4に備えた加速度センザとジャィ口センサを基に慣性航法により慣性航法的推定上体位置姿勢を求めつつ、幾何学的推定上体位置と慣性航法的推定上体位置との差が 0に収束するように慣性航法的上体位置を修正する。この処理の詳細についても後述する。

以上が図 9の S 0 1 6の自己位置姿勢推定処理の概要である。

なお、 S 2 2 1 0では、次回歩容推定支持脚座標系を決定するために目標歩容を用いたが、口ポット 1の歩行を行なう場合には、次のようにして次回歩容推定支持脚座標系を決定するようにしてもよい。

すなわち、まず、現在（着地瞬間）の口ポット 1の関節変位検出値を基にキネマテイクス演算によって、現在の推定支持脚座標系における着地時の実遊脚足平位置 '姿勢（図 1 2の例では口ポット 1の前側の足平 2 2の位置姿勢）を推定する。以降、この推定された着地時の実遊脚足平位置 ·姿勢を推定着地時遊脚足平位置姿勢と呼ぶ。

ただし、この時、ロボット 1が、現在の推定支持脚座標系上で、目標歩容通りの上体姿勢を維持しつつ、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平 2 2が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。

補足すると、前記姿勢回転偏差変化量だけ前記姿勢回転中心まわりに回転させた現在推定支持脚座標系上で、口ポット 1が、目標歩容通りの上体姿勢を維持している姿を想定した理由は、この瞬間において、想定した姿における上体姿勢を、 · グローバル座標系において、推定上体姿勢に一致させるためである。したがって、現在推定支持脚座標系を、前記差だけ姿勢回転させる代わりに、前記姿勢回転偏差変化量の内の鉛直軸まわり成分（スピン成分）だけ回転させる場合には、口ポット 1が、現在の推定支持脚座標系上で、目標歩容通りの上体向きを維持しつつ、上体傾きは、前記姿勢回転偏差変化量の傾き成分に一致し、現在の推定支持脚座標系上の仮想の床に対して支持脚の足平 2 2が滑ることなく、関節変位検出値の通りに運動しているものと仮定する。

また、この時、 6軸力センサ 5 0による床反力検出値と目標歩容の床反力とのうちの少なくともいずれかを基に、足平 2 2の変形機構（前記図 3及び図 4に示した弾性部材 1 0 6、接地部材（足底弾性体） 7 1、袋状部材 1 0 9等）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求め、その変形量を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めても良い。より具体的には、図 1 2に示すロボット 1の姿のように、現在の推定支持脚座標系において上体姿勢を目標上体姿勢とし、関節変位を関節変位検出値（第 j 関節変位検出値 0 j 、 j = l， 2 - ) とし、変形機構の変形量を床反力検出値を基に推定した変形量とした場合の全体姿（その瞬間のロポット 1の全体の姿）を求め. この全体姿における支持脚（図 1 2ではロボット 1の後ろ側の脚体 2 ) に対する遊脚足平位置姿勢を求め、これをもって推定着地時遊脚足平位置姿勢とするようにしても良い。また、モ一夕電流指令あるいは検出値を基に外乱オブザーバを用いて、減速機やリンクに作用する負荷を推定し、推定した負荷を基に前記減速機やリンクの変形を推定して、前記減速機やリンクの変形を含めて（考慮して）、推定着地時遊脚足平位置姿勢を求めるようにしても良い。

または、少なくとも目標歩容の関節変位（関節変位目標値）とコンプライアンス補償量（本出願人が先に提案した特開平 10-277969 号を参照のこと）を基に、キネマテイクス演算によって推定着地時遊脚足平位置姿勢を求めるようにしても良い。

これらにより、より一層精度良く推定着地時遊脚足平位置姿勢を求めることができる。

また、口ポット 1の歩行を行っている場合には、 S 2 2 1 4の処理では、前記推定着地時遊脚足平位置姿勢を求めた手法と同様の手法を用い. 現在のロポット 1の各関節の関節変位検出値を基に、キネマティク演算演算によって支持脚座標系に対する上体位置を求め、この位置関係に、現在の推定支持脚座標系に対する推定上体位置の位置関係が一致するように、推定上体位置を決定するようにしてもよい。この場合、推定上体位置、推定支持脚座標系および関節変位検出値の関係は、図 1 2に示すような関係になる。

または、少なくとも目標歩容の関節変位（関節変位目標値）とコンプライアンス補償量（本出願人が先に提案した特開平 10-277969 号を参照のこと）を基に、キネマテイクス演算によって支持脚座標系に対する上体位置を求め、これに推定支持脚座標系に対する推定上体位置の関係を一致させても良い。

これらにより、より一層精度良く推定上体位置を決定することができる。

次に、 S 2 2 0 0の処理及び S 2 2 1 6の処理を図 1 3を参照しつつ詳説する。なお、以降の説明では、 S 2 2 1 4で前述の如く決定される推定上体位置を幾何学的推定上体位置という。

S 2 2 0 0の処理を詳説すると、まず、前回の制御周期およびそれ以前に、前述の如く決定した幾何学的推定上体位置の運動を基に、幾何学的推定上体位置の 2階微分である幾何学的推定上体加速度を算出する。この処理は、図 1 3のブロック 3 0 1で実行される。なお、幾何学的推定上体位置を求める処理は、図 1 7のブロック 3 0 0で実行されるものとする。補足すると、上体代表点と加速度センサの位置が一致していれば、幾何学的推定上体加速度（厳密には幾何学的に推定される上体 3の中の加速度センサ位置の加速度）を算出する上で、推定上体姿勢は必要ない。

次いで、加速度センサ検出値（上体加速度検出値）を、推定上体姿勢を用いてグロ一バル座標系に変換してなる加速度センサ検出値グローバル座標系換算値を求める。この処理は、図 1 3のブロック 3 0 2で実行される。なお、説明を簡単にするために、ここでは、加速度センサの位置と上体代表点の位置は一致しているものとする。異なる場合には、キネマティクス演算によって、加速度センサ検出値およびジャィ口センサ検出値を上体代表点における加速度 · 角速度に変換するか、あるいは、上体代表点における加速度 · 角速度を加速度センサの位置における加速度、ジャィ口センサの位置における角速度に変換すれば良い。

次いで、図 1 4 ( a ) および図 1 4 ( b ) に示すように、加速度センサ検出値グロ一バル座標系換算値から幾何学的推定上体加速度を減じる (ベクトルの減算を行う）ことにより、推定重力加速度を求める。加速度センサ検出値グローバル座標系換算値には、加速度センサに作用する重力による加速度成分が含まれるが、幾何学的推定上体加速度には、重力による加速度成分が含まれない。従って、加速度センサ検出値グロ一バル座標系換算値から幾何学的推定上体加速度を減じることで、推定重力加速度が求められる。この処理は、図 1 3のブロック 3 0 3で実行される。なお、図 1 4 ( a ) は、推定上体姿勢に誤差がない場合、図 1 4

( b ) は、推定上体姿勢に誤差（以降、これを推定上体姿勢誤差、あるいは推定上体姿勢誤差角度と呼ぶ）がある場合を示す。

一方、図 1 4 ( a ) および図 1 4 ( b ) に示すように、推定上体姿勢を基に口ポット 1が推定したグロ一バル座標系の鉛直方向（Z軸方向）に発生すると想定される重力加速度を想定重力加速度と呼ぶ。

前述したように、幾何学的推定上体加速度はグローバル座標系で記述される。また、実際の口ポット 1は、推定上体姿勢が実際の上体姿勢を正しく推定しているとみなした上で、目標歩容に追従するように運動している。言い換えると、推定上体位置姿勢を基に推定されるグロ一バル座標系が正しいとみなした上で、前記推定されたグローバル座標系上で目標歩容に追従するように運動している。より厳密に表現するならば、口ポット 1の自己位置姿勢推定処理においては、推定上体位置姿勢を基に推定されるグローバル座標系が正しいと信じ、実際の口ポット 1の挙動を、正しいと信じているグローバル座標系に設定されるローカル座標系である推定支持脚座標系上で目標歩容通りに運動している口ポット 1 が、各瞬間に、姿勢検出手段によって得られた（検出あるいは推定された）上体姿勢の変化速度と目標歩容の上体姿勢変化速度の差としての姿勢回転偏差変化量で、姿勢回転中心決定手段によって決定されたその瞬間の所定の姿勢回転中心まわりに、前記推定支持脚座標系と共に、回転したものであるとみなしている。

したがって、推定上体姿勢が正しく、かつ加速度センサの検出値に誤差がなければ、図 1 4 ( a ) に示すように、前記想定重力加速度と前記推定重力加速度とは一致する。

これに対し、加速度センサの検出値に誤差がなくても、推定上体姿勢に誤差があれば、図 1 4 ( b ) に示すように、前記想定重力加速度の向きと前記推定重力加速度の向きとには、推定上体姿勢誤差角度だけずれが生じる。すなわち、式 4 2が成立する。推定上体姿勢誤差角度 = 想定重力加速度と推定重力加速度との間の角度

…式 4 2 図 1 4 ( a ) および図 1 4 ( b ) に示すように、推定重力加速度と想定重力加速度との差を重力加速度推定誤差と呼ぶと、上記の関係から、重力加速度推定誤差は、加速度センサの検出値に誤差がなければ、推定上体姿勢の誤差によって発生し、推定上体姿勢を基にロポット 1が推定したグローバル座標系から見た重力加速度推定誤差の水平成分（推定重力加速度の想定重力加速度に直交する成分）と推定上体姿勢誤差角度とには、式 4 3の関係があることがわかる。ただし、式 4 3は、重力加速度推定誤差の前後方向成分（X成分）と推定上体姿勢誤差角度 Y軸まわり成分との関係を表す。重力加速度推定誤差の左右方向成分（Y成分）と推定上体姿勢誤差角度 X軸まわり成分の関係を表す場合においては、右辺のマイナス記号を削除すれば良い。ここで、重力加速度は、正である。重力加速度推定誤差の水平成分

= - tan( 推定上体姿勢誤差角度）*重力加速度

…式 4 3 本実施形態においては、式 4 2あるいは式 4 3から算出される推定上体姿勢誤差角度を用いて、推定上体姿勢誤差が 0に収束するように推定上体姿勢を補正するようにした。

S 2 2 0 0の処理の説明に戻ると、式 4 2を用いて、現在の瞬間（厳密には、 1制御周期前の瞬間）における想定重力加速度と推定重力加速度との間の角度から推定上体姿勢誤差角度を算出する。あるいは、式 4 3を用いて、現在の瞬間（厳密には、 1制御周期前の瞬間）における重力加速度推定誤差の水平成分から推定上体姿勢誤差角度を算出しても良い。推定上体姿勢誤差角度の算出処理は、図 1 3のブロック 3 0 4で実行される。

次いで、前記推定上体姿勢誤差角度を、図 1 3のブロック 3 0 5にて. センサ座標系（座標軸をジャイロセンサの検出軸に合わせた座標系）に変換した後、ブロック 3 0 6にて、その変換後の値に積分ゲイン Ka を乗じた値を積分することにより、推定ジャイロセンサドリフト（ジャィ口センサのドリフトの推定値）を求める。さらに、この推定ジャイロセンサドリフトを、ブロック 3 0 7にて、ジャィ口センサ検出値 o in (上体角速度検出値）から減じることにより、ドリフトを補正した角速度を求める。なお、ブロック 3 0 7では、ョ一レ一トのドリフトも適宜減算されるが、これについては後述する。さらに、このドリフトを補正した角速度を、ブロック 3 0 8にて、推定上体姿勢を用いてグローバル座標系に変換することにより、グローバル上体角速度 o glを求める。

次いで、前記推定上体姿勢誤差角度にゲイン Kb を図 1 3のブロック 3 0 9で乗じた値を、グロ一バル上体角速度から減じた値をプロック 3 1 0 (積分器）で積分することにより、新たな推定上体姿勢 0 estm を求める。

なお、推定上体姿勢 0 estm や角速度などは、クォータ二オン、回転行列、あるいはオイラー角などで表現される。

ジャイロセンサのドリフト（推定ジャイロセンサドリフト）を上記のごとき構成により推定する場合、推定が効果的に働くためには、推定上体姿勢誤差角度のセンサ座標系への変換値（ベクトル）のそれぞれの要素（それぞれのセンサ検出軸まわりの推定上体姿勢誤差角度、言い換えるとセンサ口一カル推定上体姿勢誤差角度）が、その要素に対応するジャイロセンサのドリフトの影響のみを受けて、他のジャイロセンサのドリフトの影響を受けない、あるいは、ほとんど受けないことが前提条件となる。

言い換えると、推定上体姿勢誤差角度の X ( Y ) 軸まわり成分の誤差は、 X ( Y ) 軸用ジャイロセンサのドリフトの影響を受けるが、 Y ( X ) 軸用ジャイロセンサのドリフトの影響を受けないことが前提条件となる。

もし、 X軸用ジャイロセンサのドリフトの影響により、推定上体姿勢誤差角度の X軸まわり成分が発生した後、上体をセンサ座標系の Z軸まわりに急に 9 0度回転させると、推定上体姿勢誤差角度は、グローバル座標系において蓄積されたままであるので、結果的に推定上体姿勢誤差角度のセンサローカル X軸まわり成分が、推定上体姿勢誤差角度のセンサローカル Y軸まわり成分に移る。したがって、前記前提条件が成立するためには、センサ座標系 Z軸まわりの回転速度の絶対値が十分に小さいことが必要条件となる。

よって、 Z軸（上体の上下方向軸）用ジャイロセンサ検出値の絶対値が大きい時には、積分ゲイン Ka を小さくするか、あるいは 0にする (すなわちジャイロセンサのドリフト補正を行わない）ようにするのが望ましい。

また、通常、ロボット 1の移動時や作業時には、上体 3の上下軸（体幹軸）は鉛直姿勢あるいはその近傍の姿勢になっているので、ジャイロセンサの角速度検出値をグロ一バル座標系に変換したグローバル上体角速度 o glz は、 Z軸用ジャイロセンサの角速度検出値の値に近い値となる。

よって、図 1 5に示すように、ジャイロセンサの角速度検出値をグロ一バル座標系に変換したグロ一バル上体角速度の鉛直軸まわり角速度 ω glz の絶対値が大きい時には、積分ゲイン Ka を小さくするか、あるいは 0にする（すなわちジャイロセンサのドリフト補正を行わない）ようにしても良い。

また、ジャイロセンサの角速度検出値の代わりに、上体 3の目標歩容の上体回転速度の鉛直軸まわり成分あるいは上体 3の上下方向軸まわり成分の絶対値が大きい時には、積分ゲイン Ka を小さくするか、あるいは 0にする（すなわちジャイロセンサのドリフト補正を行わない）ようにしても良い。

以上をまとめて言い換えると、ジャィ口センサの角速度検出値および目標歩容の少なくともいずれかを基に求められる上体回転速度の鉛直軸まわり成分あるいは上体 3の上下方向軸まわり成分の絶対値が大きい時には、積分ゲイン Kaを小さくするか、あるいは 0にする（すなわちジャイロセンサのドリフト補正を行わない）と良い。

また、加速度センサ検出値グローバル座標系換算値から想定重力加速度を減じた値の絶対値あるいは、幾何学的推定上体加速度の絶対値が大きい場合には、センサの検出誤差やロポッ卜 1本体の剛性不足などにより、推定重力加速度の誤差が大きくなる傾向があるので、上記ゲイン Kaを小さく、あるいは 0に設定するのが良い。

また、前述したごとく、実際のロボット 1は、推定上体位置姿勢を基に推定されるグローバル座標系が正しいとみなした上で、前記推定されたグローバル座標系上で目標歩容に追従するように運動している。したがって、支持脚が接地している状態においては、推定上体姿勢に大きな誤差があっても、実際の上体加速度は、実際の重力加速度に支配されず、前記推定されたグロ一バル座標系上での幾何学的推定上体加速度に、ほぼ一致する。一方、空中期においては、実際の上体加速度は、実際の重力加速度に支配されて加速するので、前記推定されたグローバル座標系上での幾何学的推定上体加速度の方向と大きくずれ、図 1 4 ( b ) の関係が成立しなくなる。したがって、空中期においては、幾何学的推定上体位置の精度は、比較的高精度の加速度センサとジャィ口センサを基に慣性航法によって求めた上体位置の精度よりも低下しがちとなる。したがって、空中期においても、上記ゲイン Ka を小さく、あるいは 0に設定するのが良い。

また、前述のように足平 2 2の変形を考慮して上体位置を推定する場合には、空中期でなくとも、脚体先端部の足平 2 2が床と良好に接触していない状態では、足底付近の変形量推定の誤差が大きくなる傾向があるので、幾何学的推定上体加速度の精度は低下する。したがって、支持脚の足平 2 2と床との間の接触面積が小さい場合にも、上記ゲイン Ka を小さく、あるいは 0に設定するのが良い。

なお、脚体 2の足平 2 2が床と良好に接触している状態とは、具体的には、以下のような状態の少なくともいずれかが満足する状態、あるいは以下のうちの複数が同時に満足する状態を指す。

a)足平 2 2の底面と床との間の接触面積が大きい状態、例えば、足平 2 PC菌雇 49

4 0

2が床にベたに接触している状態（いわゆるベた足状態）

b)床反力の分布圧が比較的均一である状態

c)接地箇所が多い状態

d)目標 Z M P (あるいは実床反力中心点）が足平 2 2の中央あるいは中央に近い状態

e)接地圧または床反力が高い状態

f)足平 2 2の位置姿勢変化率が小さい状態、厳密には、足平 2 2の底面と床の相対位置姿勢関係の変化が小さい状態

上記状態の判定には、例えば目標歩容の時期（フェーズ）、目標 Z M P、床反力検出値を基に行えばよい。あるいは、本実施例では備えていないが、分布圧センサや接触センサの検出値を基に上記状態の判定を行つてもよい。

以上のゲイン設定法をまとめると、ジャィ口センサの角速度検出値および目標歩容の少なくともいずれかを基に求められる上体回転速度の鉛直軸まわり成分あるいは上体 3の上下方向軸まわり成分の絶対値が大きい状態、加速度センサ検出値グロ一バル座標系換算値から想定重力加速度を減じた値の絶対値あるいは、幾何学的推定上体加速度の絶対値が大きい状態、空中期、あるいは、脚先端部（足平）が床と良好に接触していない状態では、上記ゲイン Ka を小さく、あるいは 0に設定するのが良い。

すなわち、 'これらの状態を基に総合的に判断して積分ゲイン Ka を決定するのが良い。

またこれらの状態の瞬時値、あるいは長期的傾向に応じて積分ゲイン Kaを決定しても良い。

同じ理由により、積分ゲイン Kb も前記ゲイン Ka と同様に設定することが好ましい。 03 05449

4 1 本実施形態での S 2 2 0 0においては、以上の処理の他に、以下のように、状況によりョ一レート補正（ョ一方向のドリフト補正）も行われる。この補正処理が、本願発明の特徴に係わる処理である。

すなわち、まず、ョーレート補正を行うか否かを、図 1 3のブロック 3 1 6で判定する。

具体的には、以下の条件の少なくともいずれかひとつあるいは複数を判断条件として用意し、この判断条件を満足している場合には、ョーレ一ト補正を行うと判断する。

a)検出された床反力の鉛直軸まわりモーメント（あるいはその微分値）の絶対値がある所定の値以内である状況

b)目標動作がほぼ静止である状況

c)足底と床の間の接触状況が良好で滑りが生じていないと考えられる状況

d)目標歩容の姿勢角速度とジャイロ検出値（姿勢角速度検出値）の差の絶対値がある所定の値以内である状況

e)目標歩容の姿勢角速度の絶対値がある所定の値以内である状況 f)ジャイロ検出値（姿勢角速度検出値）の絶対値がある所定の値以内である状況

基本的には、支持脚足平 2 2 と床との接地面で滑り（回転滑り）が生じないかもしくは生じ難いという状況で、ョーレート補正を行うと判靳する。

なお、 d ) および f ) におけるジャイロ検出値は、ジャイロセンサの検出値そのもの（ジャィ口センサの生の出力そのものが表す姿勢角速度検出値）をョ一レ一ト補正した値であることが望ましい。

次いで、ョ一レート補正を行うと判定された場合には、図 1 3に示すように、前記 6軸力センサ 5 0 (床反力センサ）による床反力検出値を PC確 3/05449

4 2 基に、ブロック 3 1 1 にて、足平 2 2の変形機構（前記図 3及び図 4に示した弾性部材 1 0 6、接地部材（足底弾性体） 7 1、袋状部材 1 0 9 等）の変形量を変形機構の力学モデル（ばねダンパーモデル等）を用いて求める。そして、ブロック 3 1 2にて、この求めた変形量（機構変形量）と、目標歩容（目標運動）とを基に、口ポット 1 の最新の着地時 (着地瞬間）に記憶した推定支持脚座標系（図 1 0の最新の S 2 2 1 2 で決定した推定支持脚座標系）に対応する足平 2 2と床との間に滑りが生じていないと想定した場合の上体姿勢（以降、滑りなし推定上体姿勢と呼ぶ）を算出する。つまり、最新の着地瞬間以降、姿勢回転（姿勢回転偏差の変化）を生じなかったと仮定して（着地瞬間以降、推定支持脚座標系の位置姿勢が変化していないと仮定して）、前記機構変形量を考慮しつつ、目標歩容（目標運動）を基に、現在の滑りなし推定上体姿勢を求める。なお、目標歩容（目標運動）の代わりに、ロボット 1の関節の変位検出値もしくは変位目標値を用いて、滑りなし推定上体姿勢を求めてもよい。また、モータ電流指令あるいは検出値を基に外乱ォブザーバを用いて、減速機やリンクに作用する負荷を推定し、推定した負荷を基に前記減速機ゃリンクの変形を推定して、前記減速機ゃリンクの変形を含めて（考慮して）、滑りなし推定上体姿勢を求めるようにしても良い。または、少なくとも目標歩容の関節変位とコンプライアンス補償量 (本出願人が先に提案した特開平 10-277969 号を参照）を基に、キネマティクス演算によって滑りなし推定上体姿勢を求めるようにしても良い。これらにより、より一層精度良く滑りなし推定上体姿勢を求めることができる。

次いで、上記の如く求めた滑りなし推定上体姿勢の現在値及び前制御周期（前回以前の制御周期）の値を用いて該滑りなし推定上体姿勢の微分値たる滑りなし推定上体姿勢角速度をプロック 3 1 3で求める。さらに、前記推定上体姿勢を出力するブロック（積分器） 3 1 0の入力、すなわちブロック 3 0 8から出力される上体姿勢角速度と上記滑りなし推定上体姿勢角速度との差をブロック 3 1 4で求め、この差のうちの、ョ一レートセンサ（ョ一レ一ト検出用ジャイロセンサ）の検出方向成分 (以下、ョーレートセンサ検出方向角速度偏差成分という）をブロック 3 1 5で抽出する。なお、このョ一レートセンサ検出方向角速度偏差成分の抽出には、推定上体姿勢（前回の制御周期で求めた値）が用いられる。該推定上体姿勢の代わりに、目標歩容の上体姿勢を用いてもよい。なお、上体姿勢が鉛直または鉛直に近い場合には、前記差（ブロック 3 1 4の出力）の鉛直方向成分の値をョーレートセンサ検出方向角速度偏差成分として用いてもよい。

次いで、ョーレートセンサ検出方向角速度偏差成分を、フィードバック制御則のブロック 3 1 8に入力することで、ョ一レ一トドリフトを求める（推定する）。ブロック 3 1 8のフィードバック制御則としては、例えば P I制御則が用いられ、前記ョ一レートセンサ検出方向角速度偏差成分（ブロック 3 1 5の出力）にゲイン Kf を乗じた値と、該ョーレ一トセンサ検出方向角速度偏差成分の積分値にゲイン Kg を乗じた値との和を求めることによりョーレ一トドリフトが求められる。そして、この求めたョーレ一トドリフトを、前記ブロック 3 0 7で前記角速度センサ検出値 co in から減じる。また、ョーレート補正を行わないと判定された場合には、フィードバック制御則のブロック 3 1 8への入力を遮断して（図 1 3のスィッチ 3 1 7をブロック 3 1 5の出力側から切り離し. 「 0」入力側に閉成する）、直前のョーレ一トドリフト値を保持し、その値を前記角速度センサ検出値 ω inから減じるようにする。

以上が図 1 0の S 2 2 0 0におけるョ一レ一ト補正の処理の詳細である。 4 4 次に、図 1 0の S 2 2 1 6の処理を図 1 3を参照して詳説する。 S 2 2 1 6 の処理では、図 1 3 に示すごとく慣性航法的推定上体位置 Xinertestm の前回値と前記幾何学的推定上体位置との差をプロック 3 1 9で求め、この差の時間微分値（または制御周期の間の前記差の変化量）にゲイン Kc を乗じた値と、該差にゲイン Kd を乗じた値との和をブロック 3 2 0で求める。つまり、上記差からフィードバック制御則としての P D制御則より、上記和を求める。また、前記ブロック 3 0 2の出力である前記加速度センサ検出値グローバル座標系換算値と想定重力加速度 G の差をブロック 3 2 1で求める。そして、この差（ブロック 3 2 1の出力）から上記和（ブロック 3 2 0の出力）を減じた値をプロック 3 2 2にて 2階積分することにより、新たな慣性航法的推定上体位置 Xinertestmを決定する。

なお、ゲイン Kcおよび Kd は、幾何学的推定上体位置の誤差が大きく発生し易い状況では小さく、あるいは 0に設定するのが良い。支持脚の足平 2 2の底面と床との間の接触面積が小さい場合、幾何学的推定上体位置の誤差は、大きくなる。したがって、支持脚の足平 2 2の底面と床との間の接触面積が小さい場合には、上記ゲイン Kc および Kd を小さく、あるいは 0に設定するのが良い。また、特に、足平 2 2が床にベたに接触している状態（いわゆるベた足状態）の時に、上記ゲイン Kc および Kdを大きくするのが良い。

以上より、例えば、走行の場合には、補正ゲイン Ka、 Kb、 Kc および Kd は、図 8 ( c ) に示す補正ゲイン Kのグラフのように、足底全面接地期間では高く、空中期には、 0または概ね 0にすれば良い。なお図 8 ( c ) の補正ゲイン Kのグラフは、 Ka、 Kb、 Kcおよび Kdの大きさの変化の傾向を表してはいるが、厳密な値を表しているわけではない。また、図 8 ( c ) では補正ゲイン Kの最大値が 1 となるように規格化されている。したがって、 Kは、補正ゲイン Ka、 Kb、 Kc、 Kd の絞り (減衰器）を意味すると考えても良い。

本実施例においては、前記式 4 2あるいは式 4 3を用いて算出される推定上体姿勢誤差を基に推定上体姿勢を補正するようにしたが、式 4 2 および式 4 3を用いず、直接、重力加速度推定誤差の水平成分を基に推定上体姿勢を補正するようにしても良い。すなわち、式 4 3の代わりに次の式 4 4を用いても良い。重力加速度推定誤差の水平成分 =一推定上体姿勢誤差角度 *重力加速度

…式 4 4 なお、ゲイン Ka の設定について補足すると、空中期においては、推定上体姿勢の誤差がいかなる値であろうとも、ロボット 1 とともに放物運動をする座標系から見ればロポット 1全体は無重力状態にいるのと同一であって、加速度センサの出力は、推定上体姿勢の誤差の影響を受けない。したがって、加速度センサの検出精度が高く、目標歩容に対する実際の口ポット 1の追従性が高く、口ポット 1の剛性が高く、且つ目標歩容を生成するために用いられるロポット動力学モデルのパラメ一夕が実際の口ポット 1 とほぼ一致しているという条件（以下、ここでは条件 Aという）が満足されるならば、推定重力加速度と想定重力加速度とは常にほぼ一致する。厳密には、推定上体姿勢の誤差に応じて、推定重力加速度と想定重力加速度とは同じ量だけ真値からずれるので、結果的に推定重力加速度と想定重力加速度とは常にほぼ一致する。したがって、本質的に、空中期においては、重力方向を推定することはできない。しかし、推定重力加速度と想定重力加速度とは常にほぼ一致するので、その瞬間の推定上体姿勢誤差はほぼ 0となり、上記ゲイン Ka を小さく設定しなくても、推定上体姿勢誤差にゲイン Ka を乗じて求められる補正量もほぼ 0となり、推定上体姿勢に大きく悪影響を与える恐れは低い。

また、上記条件 Aを満足する場合には、ゲイン Ka を一定にしておいた方が、むしろ推定上体姿勢の低周波数成分（D C成分）の精度が高くなる場合もある。なぜなら、上記のごとく、ゲイン Ka を乗じた値を積分した値を 0にするように補正した構成は、ゲイン Ka を一定にすると, 理論上、重力加速度推定誤差の水平成分の長時間平均値を 0にするように作用し、また、推定上体姿勢誤差が 0の時には、重力加速度推定誤差の水平成分の長時間平均値は理論上 0になるからである。しかし、通常は、上記条件 Aを満足することは難しいので、図 8 ( c ) のようにゲイン K aを設定する方が望ましい。

また、補正ゲイン Ka, Kb, Kc もしくは Kd の値を、定常カルマンフィルタあるいは非定常カルマンフィル夕の手法を用いて決定しても良い。ただし、本実施形態の口ポット 1においては、特に移動中においては、システム雑音（外乱）と観測雑音の性質が、カルマンフィル夕の前提条件を十分に満足してはいないので、十分な効果を生むとは限らない, 次に、前記実施形態で決定した姿勢回転中心について、以下に補足する。いずれかの脚体 2が接地し、十分大きい床反力が発生している時期においては、いわゆる支持多角形（接地面を含む最小凸多角形、 Z M P 存在可能範囲あるいは全床反力中心点存在可能範囲）の中に、姿勢回転中心があると考えられる。

この理由を以下に述べると、姿勢回転中心が支持多角形の外にあると足平 2 2の底面のあらゆる点が床に対して相対移動していることになるので、足平 2 2の底面と床との間のあらゆる接触点には、動摩擦が働く , 動摩擦は、口ポット 1が床反力を制御しょうとして、脚体 2を動かして PC確 3/05449

4 7 も、一定値のままで、任意の値に制御することはできない。すなわち、氷の上でつるつる滑ってうまく歩けない状態と同じである。したがって- ロボット 1が床反力を制御しつつ安定に移動している場合には、足平 2 2の底面と床との間のある領域においては、滑りが生じないで静摩擦が作用していると考えれる。すなわち、支持多角形の中に、姿勢回転中心があると考えられる。補足すると、口ポット 1の足平 2 2が完全剛体であるならば、姿勢回転中心点以外のあらゆる接触点は滑りが生じるが、実際の足平 2 2の底面は、ゴムなどの弾性体でできているので、姿勢回転中心の近傍でも滑りは生じないと考えれる。

また、空中期においては、口ポットは、重心を中心として摂動運動をすると考えられる。

また、上記以外の時期、すなわちいずれかの脚体 2が接地しているが- 床反力が十分に発生していない時期においては、運動の連続性を考慮すると、姿勢回転中心は、支持多角形と全体重心位置（あるいは上体代表点位置）との間に存在すると考えられる。

以上の考察から、前記図 1 0の S 2 2 0 4において決定する姿勢回転中心としては、より一般的には、以下のいずれかであることが望ましい ₍ 現在時刻 t における

a)目標 Z M P、

b)実 Z M P (すなわち、実床反力中心点、あるいは床反力の圧力中心点）、

c)支持脚座標系の所定の点、例えば原点

d)全体重心点、

e)上体代表点

f)上記のいずれかの複数の点の内分点

いずれかの脚が接地し、十分大きい床反力が発生している時期におい 03 05449

4 8 ては、姿勢回転中心は、支持多角形の中に設定するのが良い。具体的には、上記 a)および b)に設定すれば良い。あるいは、姿勢回転中心が支持多角形の中に含まれるように、上記 c)における所定の点を設定すれば良い。例えば支持脚座標系の原点（通常、足首関節の下）に設定すれば良い。

空中期においては、姿勢回転中心は、上記 d)のように設定するのが良いが、全体重心位置は上体代表点位置の近くに存在するので、上記 e)のように設定しても良い。

上記以外の時期、すなわちいずれかの脚体 2が接地しているが、床反力が十分に発生していない時期においては、上記 f)のように設定するのが良い。

さらに、全期間での運動の連続性を考慮すると、姿勢回転中心点は、連続的に変化するように設定するのが望ましい。

いずれにしても、概ねすベての各瞬間において、口ポット 1の全体を含む最小凸面体の面上あるいは内部に姿勢回転中心点を設定するのが望ましい。

なお、以上説明した実施形態では、加速度センサおよびまたはジャィ口センサは、上体 3以外の部位、例えば頭部 4に搭載（内蔵）されていても良い。さらに頭部 4と上体 3との間に首関節がある場合、首関節の変位指令（目標変位）または変位検出値を基に、加速度センサおよびまたはジャィ口センサの検出値をキネマティクス演算によって、上体代表点の加速度と角加速度とに変換しさえすれば、残りは、前記各実施形態と同様に自己位置推定を行えば良い。

また、推定支持脚座標系および幾何学的推定上体位置を決定する際に関節変位を用いる場合においては、その関節変位を、目標歩容の関節変位または、関節変位検出値としたが、これらの重み付き平均でも良い。 03 05449

4 9 また、このときの重みは周波数特性を持たせても良い。

推定支持脚座標系や推定上体位置姿勢など、推定される位置姿勢を表現する時には、前記実施形態のごとくグローバル座標系を基準として表現する代わりに、目標位置姿勢からの摂動分で表現しても良い。

また、前記実施形態においては、前記図 1 3に示したごとく、グロ一バル座標系における推定重力加速度を基に、グローバル座標系における推定上体姿勢誤差角度を求め、グローバル座標系における推定上体姿勢誤差角度にゲイン Kbを乗じた値を、グローバル上体角速度 c glを積分する積分器（図 1 3のブロック 3 1 0 ) に付加的に入力することによつて、推定上体姿勢を補正していた。すなわち、グロ一バル座標系において、推定上体姿勢を補正していたが、この代わりに、ジャイロセンサのローカル座標系（上体 3に固定された座標系）上で補正しても良い。具体的には、図 1 3において、ゲイン Kb のブロック 3 0 9と、その出力をグロ一バル上体角速度 c gl から減じる加算器（ブロック 3 0 8 ) を削除し、積分器 KaZ S (ブロック 3 0 6 ) を、 Ka/S + Kb、すなわち、 P I制御則のブロックに変更しても良い。

原理上、このようにローカル座標系で補正しても、ョーレ一卜が高い旋回（その場旋回含む）時に、 Kb によって傾斜ドリフトの収束が悪くなることはない。

推定上体姿勢の補正において、加速度センサ検出値グロ一バル座標系換算値から幾何学的推定上体加速度を減じることにより推定重力加速度を求める代わりに、加速度センサ検出値グローバル座標系換算値から目標歩容の上体加速度を減じることにより推定重力加速度を求めるようにしても良い。

実際には、目標歩容通りに運動しょうとしている口ポットには、姿勢回転中心まわりの姿勢回転が発生しているので、姿勢回転によって、上 P 漏菌 49

5 0 体加速度は、目標歩容からずれる。しかし、姿勢回転の内の姿勢傾斜は. 前後左右に振動しても平均的には、 0である。また、通常、姿勢回転の内のスピンに関しては、 1歩毎にスピンの向きが逆に切り替わるので、平均的には 0に近い。したがって、遠心力など、姿勢回転の回転方向によらずほぼ同一方向に作用する力を除けば、姿勢回転による上体加速度への影響は、正の影響と負の影響とが相殺し、長期的には、ほぼ 0になる。なお、ここで、「長期」とは、推定上体姿勢の補正の整定時間以上の時間を指す。

よって、推定上体姿勢の補正のために、幾何学的推定上体加速度を用いる代わりに、目標歩容の上体加速度を用いても、補正の効果が大きく低下することはない。

以上説明した如く、前記実施形態においては、慣性航法によって推定上体位置（慣性航法的推定上体位置）と推定上体姿勢とを求めると共に. 幾何学的推定上体位置を用いて慣性航法的な推定上体位置及び推定上体姿勢を補正するようにしたので、口ポット 1の自己位置姿勢ならびに着地位置 · 向き（推定支持脚座標系の位置 · 向き）を精度良く推定することができる。また、特に、口ポット 1の停止時等、足平 2 2と床との間の滑りが生じていないと判断される状況では、ョ一レート補正を行うようにしたことによって、ロボット 1の自己姿勢（上体姿勢）を精度よく推定することができる。産業上の利用可能性

以上の如く、本発明は、 2足移動ロボット等の脚式移動口ポットの上体等の所定の部位の姿勢を精度よく推定することができる技術を提供するものとして有用である。

Claims

請求の範囲

1 . 決定された目標歩容に追従するように制御される脚式移動ロボッ卜において、

前記ロボットの所定の部位の姿勢角速度を検出する姿勢角速度検出手段と、

少なくとも前記目標歩容の目標運動と前記ロポットの関節の変位検出値と該関節の変位目標値とのうちのいずれかを含む該ロポットの運動状態量を基に、前記口ポットと床との接触面に滑りが無いと仮定して、前記所定の部位の姿勢角速度を推定する無滑り姿勢角速度推定手段と、少なくとも前記姿勢角速度検出手段の検出値と前記無滑り姿勢角速度推定手段が推定した姿勢角速度とに応じて、前記姿勢角速度検出手段の検出値に対するドリフト補正値を決定するドリフト補正値決定手段と、少なくとも前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度を積分することにより前記所定の部位の姿勢角を推定する積分手段とを備え、

前記ドリフト補正値決定手段は、前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度と前記無滑り姿勢角速度推定手段が推定した姿勢角速度との偏差を 0に近づけるように新たなドリフト補正値を決定することを特徴とする脚式移動ロポッ卜の自己姿勢推定装置。

2 . 前記ドリフト補正値決定手段は、前記口ポットと床との接触面に回転滑りが生じているか否かを判断する手段を備え、該回転滑りが生じていると判断したときには、前記ドリフト補正値の値を保持することを特徵とする請求の範囲第 1項に記載の脚式移動ロボットの自己姿勢推定装置。

3 . 脚式移動ロポットの所定の部位の姿勢角速度を検出する姿勢角速度検出手段と、

少なくとも前記ロポッ卜の運動停止状態における前記姿勢角速度検出手段の検出値を基に、該検出値に対するドリフト補正値を決定するドリフ卜補正値決定手段と、

前記ロポットの運動中に少なくとも前記姿勢角速度検出手段の検出値を前記ドリフト補正値により補正してなる姿勢角速度を積分することにより前記所定の部位の姿勢角を推定する積分手段とを備えたことを特徴とする脚式移動ロポットの自己姿勢推定装置。

4 . 前記ドリフト補正値決定手段は、前記姿勢角速度検出手段の検出値のうちのョー方向の成分と前記無滑り姿勢角速度推定手段が推定した姿勢角速度のうちのョ一方向の成分とに応じて前記ドリフト補正値を決定することを特徴とする請求の範囲第 1項に記載の脚式移動ロポットの自己姿勢推定装置。

5 . 前記ドリフト補正値決定手段は、前記姿勢角速度検出手段の検出値のうちのョ一方向の成分に応じて前記ドリフ卜補正値を決定することを特徴とする請求の範囲第 3項に記載の脚式移動ロポッ卜の自己姿勢推定

6 . 前記所定の部位は、前記口ポットの上体であることを特徴とする請求の範囲第 1項又は第 3項に記載の脚式移動ロボットの自己姿勢推定装置。