JP3674779B2 - Motion control device and motion control method for legged mobile robot, and robot device - Google Patents

Description

【０１０２】
また、脚式移動ロボット１００の腰部高さが一定（ｒ'_hz＋ｒ_qz＝const）で、且つ、膝部質点がゼロであることを前提とする。
【０１０３】
まず、足部（より具体的には足底）運動、足部運動から導出されるＺＭＰ軌道、体幹運動、上肢運動、腰部の姿勢や高さなど、各部の駆動・動作を実際に決定するためのパターンが設定される（ステップＳ１１）。より具体的には、まず足部運動パターン、次いでＺＭＰ軌道、体幹運動パターン、そして上肢運動パターンを設定する。また、後述するヨー軸モーメント補償に用いる未知変数（上肢運動に関する）を、ここで設定しておく。また、腰部の運動に関しては、Ｚ'方向のみ設定し、Ｘ'及びＹ'の各方向については未知とする。
【０１０４】
次に、線形・非干渉多質点近似モデルを用いて、足部、体幹、そして上肢運動により発生する設定ＺＭＰ上でのピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xを算出する（ステップＳ１２）。
【０１０５】
次いで、線形・非干渉多質点近似モデルを用いて、腰部水平面内運動（ｒ'_hx，ｒ'_hy）によって発生する設定ＺＭＰ上でのピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xを算出する（ステップＳ１３）。
【０１０６】
次いで、設定ＺＭＰ上におけるピッチ軸及びロール軸回りのモーメントに関する釣り合い式を、ロボット１００（あるいはその多質点近似モデル）とともに動く運動座標系Ｏ'−Ｘ'Ｙ'Ｚ'上で導出する（ステップＳ１４）。より具体的には、足部、体幹、そして上肢運動により発生するピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xを既知変数の項として右辺に、腰部質点の水平運動に関する項（ｒ'_hx，ｒ'_hy）を未知変数の項として左辺にまとめ、下式に示すような線形・非干渉なＺＭＰ方程式（１）を導出する。
【０１０７】
【数２】

【０１０８】
但し、以下が成立するものとする。
【０１０９】
【数３】

【０１１０】
次いで、上記のＺＭＰ方程式（１）を解いて、腰部水平面内軌道を算出する（ステップＳ１５）。例えば、オイラー法やルンゲ・クッタ法などの数値的解法（周知）を用いてＺＭＰ方程式（１）を解くことで、未知変数としての腰部の水平絶対位置（ｒ'_hx，ｒ'_hy）の数値解を求めることができる（ステップＳ１６）。ここで求められる数値解は、安定歩行可能な腰部運動パターンの近似解であり、より具体的にはＺＭＰが目標位置に入るような腰部水平絶対位置である。ＺＭＰ目標位置は、通常、着床した足底に設定される。
【０１１１】
ここで求められる近似解は、安定歩行可能な腰部運動パターンを規定する腰部の水平絶対位置の近似解（ｒ'_hx，ｒ'_hy）であり、より具体的にはＺＭＰが目標位置に入るような腰部水平絶対位置である。但し、足部、体幹、上肢、腰部などの運動により発生するピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xをキャンセルしたことになるが、まだヨー軸回りのモーメントＭ_zがキャンセルされずに残されている。そこで、後続のステップでは、ヨー軸回りのモーメントＭ_zのキャンセルするための上肢運動パターンの生成処理を行う。
【０１１２】
ステップＳ１７において、腰部運動の近似解によって生じる設定ＺＭＰ上のヨー軸モーメント（但し、腰部に発生するヨー軸モーメント）Ｍ_zhを、下式（２）を用いて算出する。
【０１１３】
【数４】

【０１１４】
次に、線形・非干渉多質点近似モデルを用いて、足部、体幹の運動によって発生する設定ＺＭＰ上でのヨー軸回りのモーメントＭ_zを算出する（ステップＳ１８）。
【０１１５】
次いで、設定ＺＭＰ上におけるヨー軸回りのモーメントＭ_zに関する釣り合い式を、ロボット１００（あるいはその多質点近似モデル）とともに動く運動座標系Ｏ'−Ｘ'Ｙ'Ｚ'上で導出する（ステップＳ１９）。より具体的には、足部、体幹の運動により発生するヨー軸回りのモーメントＭ_zと腰部運動の近似解によって生じるヨー軸モーメントＭ_zhを既知変数の項として右辺に、上肢質点の運動により発生するヨー軸回りのモーメントＭ_armを未知変数の項として左辺にまとめることで、下式に示すような線形・非干渉なＺＭＰ方程式（３）を導出する。
【０１１６】
【数５】

【０１１７】
さらに、オイラー法やルンゲ・クッタ法などの数値的解法（周知）を用いて、ＺＭＰ方程式（３）を解くことによって（ステップＳ２０）、ヨー軸モーメントＭ_zとＭ_zhを補償して安定歩行を可能とするのに必要な上肢運動パターンを算出する（ステップＳ２１）。数値的解法による上肢運動パターンの算出時には、あらかじめ与えられているヨー軸モーメント補償用の未知変数を適用する。
【０１１８】
上式（３）において、未知変数θの取り方として、上肢関節のどの関節角を未知変数として利用してもよい。より具体的に言えば、図３に示した関節自由度のうち、肩関節ピッチ軸８、肩関節ロール軸９、上腕ヨー軸１０、肘関節ピッチ軸１１、前腕ヨー軸１２、手首関節ピッチ軸１３、手首関節ロール軸１４のうち１以上の関節の組み合わせて上式（３）を成立させるようにしてもよい。
【０１１９】
また、上式（３）を成立させる上で、右上肢と左上肢とが対称又は逆位相で動かすことを前提としていなくてもよい。未知変数θの取り方としては、大きなモーメントの発生量が期待できるものとして、肩関節のピッチ軸回りの関節角（θ_{arm_shoulder}）を未知変数として用いることが挙げられる。あるいは、より人間の生体メカニズムに近いモーメント発生動作を期待できるものとしては、肩関節のピッチ軸回りの関節角（θ_{arm_shoulder}）を未知変数にし、さらに肘関節角（θ_{arm_elbow}）を（θ_{arm_elbow}＝Ｇ×θ_{arm_shoulder}：Ｇはゲイン）とすることで、未知変数を１つにしたままで（すなわち計算量を変えずに）肩関節と肘関節の協調動作によるヨー軸補償運動パターンの算出を行うことができる。
【０１２０】
勿論、ロボット１００の上体の一部である体幹ヨー軸７を利用して上式（３）を成立させることも、理論式上は可能であるが、可動角が広い肩関節や肘関節などの上肢の駆動を用いて上式（３）を成立させる方が機体での実現が容易であり、且つ、高精度な制御が可能となる。
【０１２１】
上記のステップＳ２１までで算出された近似解上ではあらかじめ設定した体幹・上肢運動が実現できない場合には、体幹・上肢運動パターンの再設定・修正を行う（ステップＳ２２）。この際、膝部の軌道を算出してもよい。
【０１２２】
次いで、上述のようにして得られた全身運動パターンを代入して、厳密モデル（すなわち、剛体、若しくは非常に多くの質点からなる脚式移動ロボット１００の精密なモデル）における設定ＺＭＰ上におけるロール軸、ピッチ軸、及びヨー軸の各軸回りのモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）を算出する（ステップＳ２３）。非厳密モデルでは上記の［数３］が成立することを前提としたが、厳密モデルにおいてはかかる前提を要しない（すなわち時間の変化に対して一定である必要はない）。
【０１２３】
厳密モデルにおけるモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）は、腰部運動においてロール、ピッチ、ヨーそれぞれの軸回りに発生するモーメント誤差である。続くステップＳ２４では、これら各軸回りのモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）が許容値（εＭ_x，εＭ_y，εＭ_z）未満か否かを判定する。許容値ε未満であれば、ロボット１００の機体上の腰、体幹、下肢の各部における安定運動パターンの厳密解、並びに機体（腰部）のヨー軸回りに発生するモーメントＭ_zとＭ_zhをキャンセルしたロボット１００の全身運動パターンを得ることができたことを意味するので（ステップＳ２５）、本処理ルーチン全体を終了する。
【０１２４】
他方、厳密モデルにおけるモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）が許容値（εＭ_x，εＭ_y，εＭ_z）以上であった場合には、厳密モデルにおけるロール軸並びにピッチ軸回りのモーメント（ｅＭ_x，ｅＭ_y）を用いて近似モデルにおけるロール軸並びにピッチ軸回りのモーメント（Ｍ_x，Ｍ_y）を修正して（ステップＳ２７）、再びＺＭＰ方程式の導出を行う。あるいは、厳密モデルにおけるヨー軸回りのモーメント既知発生モーメントｅＭ_zを用いて近似モデルにおけるヨー軸回りの既知発生モーメントＭ_zを修正して（ステップＳ２６）、再びＺＭＰ方程式の導出を行う。そして、許容値ε未満に収束するまで、上述したような腰、体幹、下肢の運動パターンの近似解の算出と修正を繰り返し実行することにより、腰部並びに上肢運動パターンの厳密解、並びに、安定歩行を実現できる全身運動パターンを得ることができる。
【０１２５】
すなわち、図７及び図８に示した処理ルーチンによれば、脚式作業時に印加されるロール、ピッチ、ヨー各軸回りのモーメントの影響をキャンセルしながら機体を安定且つ正確に正確に駆動することができる、脚式移動ロボットのための全身運動パターンを生成することができる。また、相互に干渉しているロール、ピッチ、ヨーの各軸回りの運動を安定に算出することができる。
【０１２６】
また、図７及び図８に示した処理ルーチンによれば、ＺＭＰ上のヨー軸モーメントがほぼゼロとなるヨー軸回りに極めて滑りにくい運動を前以って生成するため、機体はヨー軸回りにはほとんど滑らない。特に、動きそのものが重要なエンターティンメント用途のロボットには、なくてはならない技術である。なお、ロボットにヨー軸回りのモーメントが印加された場合には、ピッチ軸、ロール軸、ヨー軸の３面の運動を同時に解くため、ヨー軸回りのモーメントを打ち消すための動作によって、ピッチ軸、ロール軸回りのモーメントを発生させることもない。
【０１２７】
また、図７及び図８に示した処理ルーチンにおいて、ステップＳ２１において得られたヨー軸モーメントを補償する上肢運動とは、人間の場合における両腕の振りや腰の回転を利用した運動によって実現することができる。このような上肢運動は、特に高速な歩行動作において姿勢安定性の維持に大きく寄与する他、機体の上半身の動作を表現力豊かに見せるといった演出効果がある。
【０１２８】
ヒトやサルにおける動作上の「表現力」を考えた場合、腕や胴体のなどの上体の動きや姿勢は、作業の実現だけでなく、感情の体現という側面があり、非常に重要な意味を持つ。これが「身振り」、「手振り」と呼ばれる所以である。したがって、身振りや手振りという上半身主導の動作を活用してヨー軸モーメントを補償することにより、機体の動作表現力が豊かになり、とりわけエンターティンメント指向の高いロボットにおいては好ましい。
【０１２９】
また、図９及び図１０には、脚式移動ロボット１００において安定歩行可能な腰、体幹、下肢運動を生成するための処理手順の他の例をフローチャートの形式で示している。但し、上述と同様に、図５に示した線形・非干渉多質点近似モデルを用いてロボット１００の各関節位置や動作を記述するものとする。
【０１３０】
まず、足部（より具体的には足底）運動、足部運動から導出されるＺＭＰ軌道、体幹運動、上肢運動、腰部の姿勢や高さなど、各部の駆動・動作を実際に決定するためのパターンが設定される（ステップＳ３１）。より具体的には、まず足部運動パターン、次いでＺＭＰ軌道、体幹運動パターン、そして上肢運動パターンを設定する。また、後述するヨー軸モーメント補償に用いる未知変数（上肢運動に関する）を、ここで設定しておく。また、腰部の運動に関しては、Ｚ'方向のみ設定し、Ｘ'及びＹ'の各方向については未知とする。
【０１３１】
次に、線形・非干渉多質点近似モデルを用いて、足部、体幹、そして上肢運動により発生する設定ＺＭＰ上でのピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xを算出する（ステップＳ３２）。
【０１３２】
次いで、線形・非干渉多質点近似モデルを用いて、腰部水平面内運動（ｒ'_hx，ｒ'_hy）によって発生する設定ＺＭＰ上でのピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xを算出する（ステップＳ３３）。
【０１３３】
次いで、設定ＺＭＰ上におけるピッチ軸及びロール軸回りのモーメントに関する釣り合い式を、ロボット１００（あるいはその多質点近似モデル）とともに動く運動座標系Ｏ'−Ｘ'Ｙ'Ｚ'上で導出する（ステップＳ３４）。より具体的には、足部、体幹、そして上肢運動により発生するピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xを既知変数の項として右辺に、腰部質点の水平運動に関する項（ｒ'_hx，ｒ'_hy）を未知変数の項として左辺にまとめ、線形・非干渉なＺＭＰ方程式（１）を導出する（同上）。
【０１３４】
次いで、腰部水平面内運動（ｒ'_hx，ｒ'_hy）をフーリエ級数展開し、次いで、設定ＺＭＰ上でのピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xの各々についてもフーリエ級数展開し、次いで、腰部水平面内軌道のフーリエ係数を算出し、さらに逆フーリエ級数展開することで（ステップＳ３５）、腰部運動の近似解が求まる（ステップＳ３６）。
【０１３５】
当業界において既に周知のように、フーリエ級数展開することにより、時間軸成分を周波数成分に置き換えて演算することができる。すなわち、この場合には腰部の動きを周期的な動きとして捉えることができる。また、ＦＦＴ（高速フーリエ変換）を適用することができるので、計算速度を大幅に向上させることができる。
【０１３６】
ここで求められる近似解は、安定歩行可能な腰部運動パターンを規定する腰部の水平絶対位置の近似解（ｒ'_hx，ｒ'_hy）であり、より具体的にはＺＭＰが目標位置に入るような腰部水平絶対位置である。但し、足部、体幹、上肢、腰部などの運動により発生するピッチ軸回りのモーメントＭ_y、並びにロール軸回りのモーメントＭ_xをキャンセルしたことになるが、まだヨー軸回りのモーメントＭ_zがキャンセルされずに残されている。そこで、後続のステップでは、ヨー軸回りのモーメントＭ_zとＭ_zhをキャンセルするための上肢運動パターンの生成処理を行う。
【０１３７】
ステップＳ１７において、腰部運動の近似解によって生じる設定ＺＭＰモーメント上のヨー軸モーメント（但し、腰部に発生するヨー軸モーメント）Ｍ_zhを、上式（２）を用いて算出する。
【０１３８】
次に、線形・非干渉多質点近似モデルを用いて、足部運動と体幹運動によって発生する設定ＺＭＰ上でのヨー軸回りのモーメントＭ_zを算出する（ステップＳ３８）。
【０１３９】
次いで、設定ＺＭＰ上におけるヨー軸回りのモーメントに関する釣り合い式を、ロボット１００（あるいはその多質点近似モデル）とともに動く運動座標系Ｏ'−Ｘ'Ｙ'Ｚ'上で導出する（ステップＳ３９）。より具体的には、足部、体幹の運動により発生するヨー軸回りのモーメントＭ_zと腰部運動の近似解によって生じるヨー軸回りのモーメンチＭ_zhを既知変数の項として右辺に、上肢質点の運動により発生するヨー軸回りのモーメントＭ_armを未知変数の項として左辺にまとめることで、上式に示したような線形・非干渉なＺＭＰ方程式（３）を導出する。
【０１４０】
次いで、ステップＳ３５と同様に、ＺＭＰ方程式の周波数領域におけるフーリエ係数の比較法により（ステップＳ４０）、上肢運動パターンを算出する（ステップＳ４１）。上肢運動パターンの算出時には、あらかじめ与えられているヨー軸モーメント補償用の未知変数を適用する。
【０１４１】
上式（３）において、未知変数θの取り方として、上肢関節のどの関節角を未知変数として利用してもよい。より具体的に言えば、図３に示した関節自由度のうち、肩関節ピッチ軸８、肩関節ロール軸９、上腕ヨー軸１０、肘関節ピッチ軸１１、前腕ヨー軸１２、手首関節ピッチ軸１３、手首関節ロール軸１４のうち１以上の関節の組み合わせて上式（３）を成立させるようにしてもよい。
【０１４２】
また、上式（３）を成立させる上で、右上肢と左上肢とが対称又は逆位相で動かすことを前提としていなくてもよい。未知変数θの取り方の例としては、大きなモーメントの発生量が期待できるものとして、肩関節のピッチ軸回りの関節角（θ_{arm_shoulder}）を未知変数として用いることが挙げられる。あるいは、より人間の生体メカニズムに近いモーメント発生動作を期待できるものとしては、肩関節のピッチ軸回りの関節角（θ_{arm_shoulder}）を未知変数にし、さらに肘関節角（θ_{arm_elbow}）を（θ_{arm_elbow}＝Ｇ×θ_{arm_shoulder}：Ｇはゲイン）とすることで、未知変数を１つにしたままで（すなわち計算量を変えずに）肩関節と肘関節の協調動作によるヨー軸補償運動パターンの算出を行うことができる。
【０１４３】
勿論、ロボット１００の上体の一部である体幹ヨー軸７を利用して上式（３）を成立させることも、理論式上は可能であるが、可動角が広い肩関節や肘関節などの上肢の駆動を用いて上式（３）を成立させる方が機体での実現が容易であり、且つ、高精度な制御が可能となる。
【０１４４】
上記のステップＳ２１までで算出された近似解上ではあらかじめ設定した体幹・上肢運動が実現できない場合には、体幹・上肢運動パターンの再設定・修正を行う（ステップＳ４２）。この際、膝部の軌道を算出してもよい。
【０１４５】
次いで、上述のようにして得られた全身運動パターンを代入して、厳密モデル（すなわち、剛体、若しくは非常に多くの質点からなる脚式移動ロボット１００の精密なモデル）における設定ＺＭＰ上におけるロール軸、ピッチ軸、及びヨー軸の各軸回りのモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）を算出する（ステップＳ４３）。非厳密モデルでは上記の［数３］が成立することを前提としたが、厳密モデルにおいてはかかる前提を要しない（すなわち時間の変化に対して一定である必要はない）。
【０１４６】
厳密モデルにおけるモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）は、腰部運動においてロール、ピッチ、ヨーそれぞれの軸回りに発生するモーメント誤差である。続くステップＳ４４では、これら各軸回りのモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）が許容値（εＭ_x，εＭ_y，εＭ_z）未満か否かを判定する。許容値ε未満であれば、ロボット１００の機体上の腰、体幹、下肢の各部における安定運動パターンの厳密解、並びにロボットの機体のヨー軸回りに発生するモーメントをキャンセルしたロボット１００の全身運動パターンを得ることができたことを意味するので（ステップＳ４５）、本処理ルーチン全体を終了する。
【０１４７】
他方、厳密モデルにおけるモーメント（ｅＭ_x，ｅＭ_y，ｅＭ_z）が許容値（εＭ_x，εＭ_y，εＭ_z）以上であった場合には、厳密モデルにおけるロール軸並びにピッチ軸回りのモーメント（ｅＭ_x，ｅＭ_y）を用いて近似モデルにおけるロール軸並びにピッチ軸回りのモーメント（Ｍ_x，Ｍ_y）を修正して（ステップＳ４７）、再びＺＭＰ方程式の導出を行う。あるいは、厳密モデルにおけるヨー軸回りのモーメント既知発生モーメントｅＭ_zを用いて近似モデルにおけるヨー軸回りの既知発生モーメントＭ_zを修正して（ステップＳ４６）、再びＺＭＰ方程式の導出を行う。そして、許容値ε未満に収束するまで、上述したような腰、体幹、下肢の運動パターンの近似解の算出と修正を繰り返し実行することにより、腰部並びに上肢運動パターンの厳密解、並びに、安定歩行を実現できる全身運動パターンを得ることができる。
【０１４８】
すなわち、図９及び図１０に示した処理ルーチンによれば、脚式作業時に印加されるロール、ピッチ、ヨー各軸回りのモーメントの影響をキャンセルしながら機体を安定且つ正確に正確に駆動することができる、足式移動ロボットのための全身運動パターンを生成することができる。また、相互に干渉しているロール、ピッチ、ヨーの各軸回りの運動を安定に算出することができる。
【０１４９】
また、図９及び図１０に示した処理ルーチンによれば、ＺＭＰ上のヨー軸モーメントがほぼゼロとなるヨー軸回りに極めて滑りにくい運動を前以って生成するため、機体はヨー軸回りにはほとんど滑らない。特に、動きそのものが重要なエンターティンメント用途のロボットには、なくてはならない技術である。なお、ロボットにヨー軸回りのモーメントが印加された場合には、ピッチ軸、ロール軸、ヨー軸の３面の運動を同時に解くため、ヨー軸回りのモーメントを打ち消すための動作によって、ピッチ軸、ロール軸回りのモーメントを発生させることもない。
【０１５０】
また、図９及び図１０に示した処理ルーチンにおいて、ステップＳ４１において得られたヨー軸モーメントを補償する上肢運動とは、人間の場合における両腕の振りや腰の回転を利用した運動によって実現することができる。このような上肢運動は、特に高速な歩行動作において姿勢安定性の維持に大きく寄与する他、機体の上半身の動作を表現力豊かに見せるといった演出効果がある。
【０１５１】
ヒトやサルにおける動作上の「表現力」を考えた場合、腕や胴体のなどの上体の動きや姿勢は、作業の実現だけでなく、感情の体現という側面があり、非常に重要な意味を持つ。これが「身振り」、「手振り」と呼ばれる所以である。したがって、身振りや手振りという上半身主導の動作を活用してヨー軸モーメントを補償することにより、機体の動作表現力が豊かになり、とりわけエンターティンメント指向の高いロボットにおいては好ましい。
【０１５２】
［追補］
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１５３】
また、本発明の要旨は、必ずしも「ロボット」と称される製品には限定されない。すなわち、電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行う機械装置であるならば、例えば玩具などのような他の産業分野に属する製品であっても、同様に本発明を適用することができる。
【０１５４】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、冒頭に記載した特許請求の範囲の欄を参酌すべきである。
【０１５５】
【発明の効果】
以上詳記したように、本発明によれば、高速歩行などの脚式作業を安定且つ正確に実現することができる、脚式移動型ロボットのための優れた動作制御装置及び動作制御方法、並びにロボット装置を提供することができる。
【０１５６】
また、本発明によれば、上肢の運動を用いて機体のヨー軸回りに印加されるモーメントを低減することができる、脚式移動型ロボットのための優れた動作制御装置及び動作制御方法、並びにロボット装置を提供することができる。
【図面の簡単な説明】
【図１】本発明の実施に供される脚式移動ロボット１００を前方から眺望した様子を示た図である。
【図２】本発明の実施に供される脚式移動ロボット１００を後方から眺望した様子を示た図である。
【図３】本実施例に係る脚式移動ロボット１００が具備する自由度構成モデルを模式的に示した図である。
【図４】本実施例に係る脚式移動ロボット１００の制御システム構成を模式的に示した図である。
【図５】本実施例に係る歩行制御の計算のために導入される、脚式移動ロボット１００の線形且つ非干渉の多質点近似モデルを示した図である。
【図６】図５に示した多質点モデルの腰部周辺の拡大図である。
【図７】脚式移動ロボット１００において安定歩行可能な腰、体幹、下肢運動を生成するための処理手順の一例を示したフローチャートである。
【図８】脚式移動ロボット１００において安定歩行可能な腰、体幹、下肢運動を生成するための処理手順の一例を示したフローチャートである。
【図９】脚式移動ロボット１００において安定歩行可能な腰、体幹、下肢運動を生成するための処理手順の他の例を示したフローチャートである。
【図１０】脚式移動ロボット１００において安定歩行可能な腰、体幹、下肢運動を生成するための処理手順の他の例を示したフローチャートである。
【図１１】２足脚式移動ロボットの歩行速度［秒／歩］とヨー軸方向に発生するモーメントＮｍとの関係（例）を表した図である。
【符号の説明】
１…頭部，２…首関節ヨー軸
３…首関節ピッチ軸，４…首関節ロール軸
５…体幹ピッチ軸，６…体幹ロール軸
７…体幹ヨー軸，８…肩関節ピッチ軸
９…肩関節ロール軸，１０…上腕ヨー軸
１１…肘関節ピッチ軸，１２…前腕ヨー軸
１３…手首関節ピッチ軸，１４…手首関節ロール軸
１５…手部，１６…股関節ヨー軸
１７…股関節ピッチ軸，１８…股関節ロール軸
１９…膝関節ピッチ軸，２０…足首関節ピッチ軸
２１…足首関節ロール軸，２２…足部
３０…頭部ユニット，４０…体幹部ユニット
５０…腕部ユニット，５１…上腕ユニット
５２…肘関節ユニット，５３…前腕ユニット
６０…脚部ユニット，６１…大腿部ユニット
６２…膝関節ユニット，６３…脛部ユニット
８０…制御ユニット，８１…主制御部
８２…周辺回路
９１，９２…接地確認センサ
９３…姿勢センサ
１００…脚式移動ロボット[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a motion control device and a motion control method for a realistic robot having a structure simulating the mechanism and motion of a living body, and a robot device, and more particularly to an upright walking type body mechanism such as a human or a monkey, The present invention relates to a motion control device and a motion control method for a legged mobile robot having a structure simulating motion, and a robot device.
[0002]
More particularly, the present invention relates to a motion control device and a motion control method for a legged mobile robot that realizes legged work such as high-speed walking stably and accurately, and to a robot apparatus, and in particular, legged such as walking. The present invention relates to a motion control device and a motion control method for a legged mobile robot that stably and accurately drives while canceling the influence of moments around each axis of roll, pitch, and yaw applied to a machine body during work, and a robot device.
[0003]
[Prior art]
A mechanical device that uses an electrical or magnetic action to perform a movement resembling human movement is called a “robot”. It is said that the word “robot” comes from the Slavic word “ROBOTA (slave machine)”. Robots began to spread in Japan from the end of the 1960s, but most of them were industrial robots such as manipulators and transfer robots for the purpose of automating and unmanned production work in factories. there were.
[0004]
A stationary type robot such as an arm type robot that is implanted and used in a specific place operates only in a fixed / local work space such as an assembly / sorting operation of parts. In contrast, a mobile robot has a non-restricted working space, and can freely move on a predetermined route or a non-route to perform a predetermined or arbitrary human work, or a human or dog. Alternatively, various wide-ranging services that can replace other life forms can be provided. In particular, legged mobile robots are more unstable than crawler and tire type mobile robots, making posture control and walking control difficult, but they are difficult to control stairs and ladders, climb over obstacles, It is excellent in that it can realize flexible walking / running operation regardless of distinction.
[0005]
Recently, the model is based on the body mechanism and movement of a four-legged animal such as a dog or a cat, or a pet-type robot that mimics its movement, or a human-like animal that walks upright on two legs. Research and development on legged mobile robots such as the “humanoid” or “humanoid robot” has been progressed, and the expectation for practical use is also increasing.
[0006]
The significance of researching and developing a legged mobile robot with two legs standing upright, called a human form or a human form, can be understood from the following two viewpoints, for example.
[0007]
One is a human scientific viewpoint. In other words, through the process of creating a robot with a structure resembling human lower limbs and / or upper limbs, devising its control method and simulating human walking motion, the mechanism of human natural motion including walking Can be elucidated in engineering. Such research results can be greatly reduced to the progress of various other research fields dealing with human movement mechanisms, such as ergonomics, rehabilitation engineering, or sports science.
[0008]
The other is the development of a practical robot that supports life as a human partner, that is, supports human activities in various situations in the living environment and other daily lives. This kind of robot needs to learn how to adapt to a person or an environment with different personalities while learning from humans in various aspects of the human living environment, and needs to grow further in terms of functionality. At this time, it is considered that the robot having the “human shape”, that is, the same shape or the same structure as the human, functions effectively for smooth communication between the human and the robot.
[0009]
For example, when teaching the robot how to get through the room while avoiding obstacles that should not be stepped on, rather than having a completely different structure than the one you are teaching, such as a crawler type or quadruped type robot, A biped robot with the same appearance is much easier for the user (worker) to teach and for the robot (for example, “Control of biped robot” by Takanishi). (Refer to Automobile Engineering Society Kanto Branch <High Plastics> No.25, 1996 APRIL).
[0010]
Numerous techniques related to posture control and stable walking have been proposed for robots of the type that perform legged movement by biped walking. Stable “walking” as used herein can be defined as “moving with legs without falling down”.
[0011]
Robot posture stabilization control is very important in avoiding robot overturning. This is because a fall means that the robot is interrupting the work being executed, and a considerable amount of labor and time are spent to get up from the fall state and resume the work. Above all, it is because there is a danger of causing fatal damage to the robot body itself or the object on the other side colliding with the falling robot. Therefore, in the design and development of legged mobile robots, posture stability control during walking and other legged work is one of the most important technical issues.
[0012]
At the time of walking, gravity, inertial force, and these moments act on the road surface from the walking system due to gravity and acceleration caused by walking motion. According to the so-called “Dalambert principle”, they balance with the floor reaction force and the floor reaction force moment as a reaction from the road surface to the walking system. As a result of the dynamic reasoning, there is a point where the pitch and roll axis moment become zero, that is, “ZMP (Zero Moment Point)” on or inside the side of the support polygon formed by the sole contact point and the road surface.
[0013]
Many of the proposals related to posture stability control of legged mobile robots and prevention of falls during walking use this ZMP as a norm for determining the stability of walking. The biped walking pattern generation based on the ZMP norm has advantages such that a foot landing point can be set in advance and it is easy to consider the kinematic constraint conditions of the foot according to the road surface shape. In addition, using ZMP as a stability determination criterion means that a trajectory is treated as a target value in motion control instead of force, and thus technically feasible. Regarding the concept of ZMP and the application of ZMP to walking robot stability criteria, "LEGGED LOCOMOTION ROBOTS" written by Miomir Vukobratovic (Ichiro Kato's "Walking Robot and Artificial Feet" (Nikkan Kogyo Shimbun)) It is described in.
[0014]
In general, a biped walking robot such as a humanoid has a higher center of gravity and a smaller ZMP stability area during walking than a quadruped walking. Therefore, the problem of posture change accompanying such a change in road surface condition is particularly important in a biped robot.
[0015]
There are already some proposals using ZMP as a posture stability criterion for biped robots.
[0016]
For example, the legged mobile robot described in Japanese Patent Application Laid-Open No. 5-305579 is designed to perform stable walking so that a point on the floor surface where ZMP becomes zero coincides with a target value.
[0017]
In the legged mobile robot described in JP-A-5-305581, the ZMP has a support polyhedron (polygon) or a position where the ZMP has at least a predetermined margin from the end of the support polygon when landing or getting off the floor. It was configured to be. In this case, even if a disturbance or the like is received, there is a ZMP margin for a predetermined distance, and the stability of the aircraft during walking is improved.
[0018]
Japanese Patent Laid-Open No. 5-305583 discloses that the walking speed of a legged mobile robot is controlled by the ZMP target position. That is, using the walking pattern data set in advance, the leg joint is driven so that the ZMP matches the target position, the inclination of the upper body is detected, and the walking pattern set according to the detected value Change the data discharge rate. When the robot leans forward, for example, by stepping on unknown irregularities, the posture can be recovered by increasing the discharge speed. Further, since the ZMP is controlled to the target position, there is no problem even if the discharge speed is changed during the both-leg support period.
[0019]
Japanese Patent Laid-Open No. 5-305585 discloses that the landing position of a legged mobile robot is controlled by the ZMP target position. That is, the legged mobile robot described in the publication detects a deviation between the ZMP target position and the actually measured position and drives one or both of the legs so as to eliminate it, or a moment around the ZMP target position. Is detected, and the leg is driven so that it becomes zero, thereby realizing stable walking.
[0020]
Japanese Patent Application Laid-Open No. 5-305586 discloses that the tilting posture of a legged mobile robot is controlled by the ZMP target position. That is, a moment around the ZMP target position is detected, and when the moment is generated, the leg is driven so that the moment becomes zero, thereby performing stable walking.
[0021]
As described above, the posture stability control of a robot using ZMP as a stability determination criterion is such that the pitch and roll axis moment are zero on or inside the support polygon formed by the sole contact point and the road surface. The point is to search for points.
[0022]
However, as a result of a priori verification by the present inventors, when the robot is legged at high speed, not only the moment about the pitch axis and roll axis but also the yaw axis, that is, the Z axis It was also found that a moment was generated.
[0023]
FIG. 11 shows the relationship (example) between the walking speed [second / step] of the biped legged mobile robot and the moment Nm generated in the yaw axis direction. As can be seen from the figure, the time required for the legged mobile robot to take one step is shortened, that is, as the walking speed increases, the yaw moment increases significantly.
[0024]
Such a yaw-axis moment will eventually cause the aircraft to turn, causing slipping around the yaw axis between the bottom of the robot and the road surface, which greatly affects the stability of walking. This is an obstacle to realizing the formula work stably and accurately. Furthermore, if the influence of this yaw axis moment becomes significant, it may lead the robot to fall, leading to the expectation and damage of colliding objects.
[0025]
For example, in Japanese Patent Application No. 2000-206531, which has already been assigned to the present applicant, a lumbar motion pattern is derived based on an arbitrary foot motion pattern, ZMP trajectory, trunk motion pattern, and upper limb motion pattern. Discloses a whole body motion pattern that realizes stable walking of a bipedal mobile robot. According to the robot walking control device and the walking control method described in the specification, the gait of the lower limb is determined so as to stably walk even when the robot is in various motion states such as when the robot is not standing upright or during normal walking. be able to. In particular, when a gesture or hand gesture using the upper body is applied when the body is standing upright, the lower limb gait that enables stable walking can be determined according to the gait of the upper body.
[0026]
However, in this specification, the moment M about the roll axis and the pitch axis on the set ZMP generated in the robot body by the movement of the foot, trunk, and upper limbs. _x , M _y Is described in terms of deriving a whole body coordinated motion that can be stably walked by canceling, but the moment M about the yaw axis generated during the whole body coordinated motion is described _z Is not considered.
[0027]
A method of performing correction control for canceling the moment around the yaw axis, such as shaking the arm, after detecting slipping around the yaw axis of the airframe by a sensor or the like is also conceivable. However, in this case, since correction control is performed after the fact, there is a problem that it will slip somewhat.
[0028]
In addition, since an operation such as shaking an arm is a movement of a surface unrelated to gravity, it is necessary to control the arm to a desired position after canceling the moment.
[0029]
In addition, when using a humanoid arm composed of a rotary joint that shares movement in the Z direction, swinging the arm to counteract the moment around the yaw axis can simultaneously improve robot stability. It tends to generate moments around the pitch axis and roll axis that are damaged.
[0030]
That is, if the arm is swung afterward to suppress the slip of the yaw axis, the problem is that the walking itself becomes unstable although the slip around the yaw axis is cured.
[0031]
This phenomenon becomes particularly large as the robot moves at high speed, and is not preferable control for a robot that performs high-speed movement (running).
[0032]
In addition, using a humanoid arm to generate only a moment around the yaw axis is likely to be an inhuman and unnatural movement (for example, in a horizontal plane so that no movement occurs in the Z direction). This is a fatal robot for entertainment that emphasizes expressive power.
[0033]
[Problems to be solved by the invention]
An object of the present invention is to provide an excellent motion control device and motion control method, a robot device, and a robot device for a legged mobile robot having a structure simulating an upright walking type body mechanism and motion. is there.
[0034]
A further object of the present invention is to provide an excellent motion control device and motion control method for a legged mobile robot, and a robot device capable of stably and accurately realizing a legged work such as high-speed walking. is there.
[0035]
A further object of the present invention is to provide an excellent for a legged mobile robot that can stably and accurately drive the airframe while canceling the influence of moments around the roll, pitch, and yaw axes applied during legged work. Another object of the present invention is to provide an operation control apparatus, an operation control method, and a robot apparatus.
[0036]
[Means and Actions for Solving the Problems]
The present invention has been made in consideration of the above problems, and the first side of the present invention is composed of at least an upper limb, a trunk, and a lower limb, and is an operation control device or operation of a robot that performs legged movement by the lower limb. A control method,
Means or step for setting exercise on at least one of the upper limbs, trunk and lower limbs;
Means or step for calculating a yaw moment of the robot body on the set ZMP generated by the movement of the set upper limb, trunk, and lower limb;
Means or step for calculating upper limb movement to cancel the yaw moment;
Means or step for modifying the set upper limb, trunk, lower limb movements according to the calculated upper limb movements;
A motion control device or a motion control method for a legged mobile robot.
[0037]
As described in the “Prior Art” column, in particular, in a biped walking type mobile robot, the yaw moment applied to the airframe increases remarkably as the walking speed increases. Such a yaw-axis moment is an obstacle to realizing a stable and accurate leg work such as turning of the aircraft, slipping around the yaw axis with respect to the road surface, and falling of the aircraft due to loss of balance.
[0038]
According to the motion control apparatus and the motion control method of the legged mobile robot according to the first aspect of the present invention, when executing the motion pattern of the airframe composed of the motions of the lower limbs, the trunk, and the upper limbs, on the set ZMP The yaw axis moment generated in the aircraft can be canceled out by upper limb movement.
[0039]
According to the motion control apparatus and motion control method for a legged mobile robot according to the first aspect of the present invention, a motion that is extremely difficult to slip around the yaw axis where the yaw moment on the ZMP is almost zero is generated in advance. Therefore, the aircraft hardly slides around the yaw axis. In particular, it is an indispensable technology for entertainment robots where movement itself is important. In addition, when a moment around the yaw axis is applied to the robot, the pitch axis, roll axis, and yaw axis are solved simultaneously by the action to cancel the moment around the yaw axis. No moment around the roll axis is generated.
[0040]
Therefore, the stability during walking and other legged work can be maintained by the movement of the upper limb while continuing the legged work by the lower limbs and the trunk.
[0041]
More specifically, the movement of the upper limb is a movement using driving of a shoulder joint and an elbow joint. Of course, the left and right upper limbs generally move in opposite phases, but it is not necessary to limit to this in order to realize the present invention.
[0042]
According to the body design of a general upright biped robot, the shoulder joints and elbow joints have a wider movable angle than the trunk (eg trunk yaw axis). It is possible to achieve high attitude stability by canceling the yaw moment of the aircraft efficiently and with high accuracy.
[0043]
Further, such an upper limb movement has an effect of showing the motion of the upper body of the robot with rich expressive power.
[0044]
Further, the second aspect of the present invention is an operation control device or an operation control method for a robot that is composed of at least an upper limb, a trunk, and a lower limb, and performs a legged movement by the lower limb,
Means or step for setting exercise on at least one of the upper limbs, trunk and lower limbs;
Means or step for calculating the pitch axis and / or roll axis moment of the robot body on the set ZMP generated by the movement of the set upper limb, trunk, and lower limb;
Means or step for calculating the movement of the lower limbs and trunk to cancel the pitch axis and / or roll axis moments;
Means or step for calculating the yaw moment of the robot body on the set ZMP caused by the movement of the calculated lower limbs and trunk;
Means or step for calculating upper limb movement to cancel the yaw moment;
Means or step for modifying the set upper limb, trunk, lower limb movements according to the calculated upper limb, trunk, lower limb movements;
A motion control device or a motion control method for a legged mobile robot.
[0045]
When a bipedal legged mobile robot performs whole body movement consisting of upper limb, trunk, and lower limb movements, moments are generated around the pitch axis, roll axis, and yaw axis in the robot body. To do.
[0046]
According to the motion control apparatus or the motion control method for the legged mobile robot according to the second aspect of the present invention, the ZMP is set when executing the motion pattern of the airframe composed of the motions of the lower limbs, the trunk, and the upper limbs. After calculating the lower limb and trunk movements that cancel the moments around the pitch axis and roll axis generated in the aircraft above, the moments around the yaw axis generated in the aircraft on the set ZMP by the lower limb and trunk movements are further calculated. The motion of the upper limb that cancels out can be calculated. Accordingly, it is possible to realize a stable and accurate whole body cooperative movement while canceling the influence of moments around the roll, pitch and yaw axes applied to the body during leg-type work such as walking.
[0047]
In particular, in a bipedal legged mobile robot, the yaw axis moment applied to the aircraft increases remarkably as the walking speed increases. Such yaw axis moments are caused by the rotation of the aircraft and around the yaw axis relative to the road surface. Although it hinders the airframe operation such as slipping or overturning the airframe, according to the second aspect of the present invention, this can be suitably removed.
[0048]
In addition, according to the motion control device and motion control method for a legged mobile robot according to the second aspect of the present invention, a motion that is extremely difficult to slip around the yaw axis where the yaw axis moment on the ZMP is almost zero is obtained in advance. Therefore, the aircraft hardly slides around the yaw axis. When a moment around the yaw axis is applied to the robot, the pitch axis, roll axis, and roll axis are solved by the action to cancel the moment around the yaw axis, since the three axes of the pitch axis, roll axis, and yaw axis are solved simultaneously. No moment of rotation is generated.
[0049]
Therefore, the stability during walking and other legged work can be maintained by the movement of the upper limb while continuing the legged work by the lower limbs and the trunk.
[0050]
More specifically, the movement of the upper limb is a movement using driving of a shoulder joint and an elbow joint. Of course, the gist of the present invention need not be limited to movements in which the left and right upper limbs are in antiphase or symmetrical.
[0051]
According to the body design of a general upright biped robot, the shoulder joints and elbow joints have a wider movable angle than the trunk (eg trunk yaw axis). It is possible to achieve high attitude stability by canceling the yaw moment of the aircraft efficiently and with high accuracy. Further, such an upper limb movement has an effect of showing the motion of the upper body of the robot with rich expressive power.
[0052]
Further, the third aspect of the present invention is an operation control device or an operation control method for a robot that is composed of at least an upper limb, a trunk, and a lower limb, and performs a legged movement by the lower limb,
(A) means or step for setting at least one of foot movement, trunk movement, upper limb movement, posture of the waist and height for realizing the requested movement;
(B) means or step for setting a ZMP trajectory based on the foot movement set by the means or step (a);
(C) The pitch axis and roll axis of the legged mobile robot generated on the ZMP set by the means or step (b) by the movement of the foot, trunk, or upper limb set by the means or step (a) Means or step for calculating the moment of rotation;
(D) means or step for obtaining a solution of a waist motion that cancels out the moment about the pitch axis and roll axis calculated by the means or step (c);
(E) means or step for calculating a moment generated around the yaw axis of the legged mobile robot on the ZMP set by the means or step (b) by the waist movement obtained by the means or step (d); ,
(F) means or step for obtaining a solution of the upper limb motion that cancels the moment about the yaw axis calculated by the means or step (e);
(G) means or step for deriving the whole body movement of the legged mobile robot based on the lumbar motion and upper limb movement determined by the means or step (d) and the means or step (f);
A motion control device or a motion control method for a legged mobile robot.
[0053]
According to the motion control apparatus or the motion control method for the legged mobile robot according to the third aspect of the present invention, the ZMP is set when executing the motion pattern of the airframe composed of the motions of the lower limbs, the trunk, and the upper limbs. After calculating the movements of the lower limbs and trunk that cancel each moment around the pitch axis and roll axis generated in the aircraft above, the moment around the yaw axis generated in the aircraft on the set ZMP by the movement of the lower limbs and trunk is further calculated By calculating the movement of the upper limb that cancels out, stable and accurate whole body coordinated movement can be realized while canceling the influence of the moments around the roll, pitch, and yaw axes applied to the aircraft during legged work. .
[0054]
In addition, according to the motion control device and motion control method for a legged mobile robot according to the third aspect of the present invention, a motion that is extremely difficult to slip around the yaw axis where the yaw moment on the ZMP is almost zero is obtained in advance. Therefore, the aircraft hardly slides around the yaw axis. When a moment around the yaw axis is applied to the robot, the pitch axis, roll axis, and roll axis are solved by the action to cancel the moment around the yaw axis, since the three axes of the pitch axis, roll axis, and yaw axis are solved simultaneously. No moment of rotation is generated.
[0055]
Further, the fourth aspect of the present invention is an operation control device or an operation control method for a robot that is composed of at least an upper limb, a trunk, and a lower limb, and performs a legged movement by the lower limb,
(A) means or step for setting at least one of foot motion, trunk motion, upper limb motion, waist posture and height for realizing the requested motion;
(B) means or step for setting a ZMP trajectory based on the foot movement set by the means or step (A);
(C) Using an inexact model of the legged mobile robot, on the ZMP set by the means or step (B) by the movement of the foot, trunk, or upper limb set by the means or step (A) Means or step for calculating moments about the pitch axis and roll axis of the legged mobile robot generated in
(D) means or step for obtaining an approximate solution of waist motion that cancels the moments about the pitch axis and roll axis calculated by the means or step (C);
(E) Using the inexact model of the legged mobile robot, the legged formula on the ZMP set by the means or step (B) by the approximate solution of the waist movement obtained by the means or step (D) Means or step for calculating a moment generated around the yaw axis of the mobile robot;
(F) means or step for obtaining an approximate solution of upper limb movement that cancels the moment about the yaw axis calculated by the means or step (E);
(G) Using the exact model of the legged mobile robot, set by the means or step (B) when performing the whole body motion calculated by the means or step (D) and the means or step (F) Means or step for calculating moments around the pitch axis, roll axis and yaw axis of the legged mobile robot generated on the ZMP;
(H) a means or step for determining a whole body motion if the moments about the pitch axis, roll axis, and yaw axis calculated by the means or step (G) are less than a predetermined allowable value;
(I) If the moment about the pitch axis, roll axis, and yaw axis calculated by the means or step (G) is equal to or greater than a predetermined allowable value, the moment of the non-strict model on the set ZMP is corrected, Means or step for reintroducing said means or step (D) or said means or step (F);
A motion control device or a motion control method for a legged mobile robot.
[0056]
According to the motion control apparatus or the motion control method for the legged mobile robot according to the fourth aspect of the present invention, the ZMP is set when executing the motion pattern of the airframe composed of the motions of the lower limbs, the trunk, and the upper limbs. After calculating the lower limb and trunk movements that cancel the moments around the pitch axis and roll axis generated in the aircraft above, the moments around the yaw axis generated in the aircraft on the set ZMP by the lower limb and trunk movements are further calculated. By calculating the movement of the upper limb that cancels out, stable and accurate whole body coordinated movement can be realized while canceling the influence of the moments around the roll, pitch, and yaw axes applied to the aircraft during legged work. .
[0057]
In addition, according to the motion control apparatus and motion control method for a legged mobile robot according to the fourth aspect of the present invention, a motion that is extremely difficult to slip around the yaw axis where the yaw axis moment on the ZMP is almost zero is obtained in advance. Therefore, the aircraft hardly slides around the yaw axis. When a moment around the yaw axis is applied to the robot, the pitch axis, roll axis, and roll axis are solved by the action to cancel the moment around the yaw axis, since the three axes of the pitch axis, roll axis, and yaw axis are solved simultaneously. No moment of rotation is generated.
[0058]
In the motion control apparatus or the motion control method for a legged mobile robot according to the fourth aspect of the present invention, the inexact model may be a linear and / or non-interference multi-mass point approximate model related to the legged mobile robot. Good. The exact model may be a rigid model related to the robot or a multi-mass point approximation model of nonlinear and / or interference.
[0059]
(F ′) The means or the step (D) for obtaining an approximate solution for the waist movement and / or the approximate solution obtained in the means or the step (F) for obtaining an approximate solution for the upper limb movement is a trunk / upper limb set in advance. If the exercise cannot be realized, a means or step for resetting / correcting the trunk / upper limb exercise pattern may be further provided.
[0060]
In addition, the means or step (D) for obtaining an approximate solution of the lumbar motion includes the moment on the set ZMP caused by the foot motion, the trunk motion, and the upper limb motion, and the moment on the set ZMP caused by the motion of the lumbar in the horizontal plane. An approximate solution of the waist motion may be obtained by solving the balance equation.
[0061]
Further, the means or step (D) for obtaining the approximate solution of the waist motion may be calculated by replacing the time function with the frequency function. More specifically, the Fourier series expansion is applied to the moment on the set ZMP caused by the foot movement, trunk movement, and upper limb movement, and the Fourier series expansion is applied to the movement in the horizontal plane of the lumbar region. An approximate solution of the waist motion may be obtained by calculating the Fourier coefficient of the trajectory and further applying inverse Fourier series expansion.
[0062]
Further, the means or step (F) for obtaining the approximate solution of the upper limb motion obtains the approximate solution of the upper limb motion by solving the balance equation of the moment on the set ZMP generated by the foot motion and the trunk motion. May be.
[0063]
The means (F) for obtaining an approximate solution of the upper limb motion may be calculated by replacing the time function with the frequency function. More specifically, the means (F) for obtaining an approximate solution of the upper limb motion calculates a Fourier coefficient by applying a Fourier series expansion to the moment on the set ZMP generated by the foot motion and the trunk motion. Further, an approximate solution of upper limb motion may be obtained by applying inverse Fourier series expansion.
[0064]
Further, a fifth aspect of the present invention is a robot apparatus that is composed of at least an upper limb, a trunk, and a lower limb, and performs legged movement by the lower limb.
An exercise pattern setting means for setting an exercise pattern of the robot;
Moment calculating means for calculating a moment about the yaw axis of the robot body on the set ZMP generated by the set motion pattern;
An exercise pattern calculating means for calculating an upper limb exercise pattern for reducing the calculated moment about the yaw axis;
A movement pattern correcting means for correcting the set movement pattern based on the calculated upper limb movement pattern;
A robot apparatus comprising:
[0065]
Here, the movement pattern setting means sets at least one movement pattern of the upper limb, the trunk, or the lower limb of the robot.
[0066]
When a bipedal legged mobile robot performs whole body movement consisting of upper limb, trunk, and lower limb movements, moments are generated around the pitch axis, roll axis, and yaw axis in the robot body. To do.
[0067]
According to the robot apparatus of the fifth aspect of the present invention, the moment about the yaw axis generated in the airframe on the set ZMP is canceled when executing the motion pattern of the airframe composed of the motion of the lower limbs, the trunk, and the upper limb. Such an upper limb motion pattern can be calculated to correct the motion pattern. Accordingly, it is possible to realize a stable and accurate whole body cooperative movement while canceling the influence of the moment about the yaw axis applied to the airframe during legged work such as walking.
[0068]
In particular, in a bipedal legged mobile robot, the yaw axis moment applied to the aircraft increases remarkably as the walking speed increases. Such yaw axis moments are caused by the rotation of the aircraft and around the yaw axis relative to the road surface. Although it hinders the operation of the aircraft, such as slipping and falling, according to the fifth aspect of the present invention, this can be suitably removed.
[0069]
Further, according to the robot apparatus of the fifth aspect of the present invention, since the motion that is extremely difficult to slip around the yaw axis where the yaw axis moment on the ZMP is almost zero is generated in advance, Hardly slips.
[0070]
Other objects, features, and advantages of the present invention will become apparent from more detailed descriptions based on embodiments of the present invention described later and the accompanying drawings.
[0071]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
[0072]
FIG. 1 and FIG. 2 show a state in which the legged mobile robot 100 used for carrying out the present invention is viewed from the front and the rear. Further, FIG. 3 schematically shows the joint degree-of-freedom configuration of the legged mobile robot 100.
[0073]
As shown in FIG. 3, the legged mobile robot 100 connects an upper limb including two arms and a head 1, a lower limb including two legs that realize a moving operation, and an upper limb and a lower limb. Consists of the trunk.
[0074]
The neck joint that supports the head 1 has three degrees of freedom: a neck joint yaw axis 2, a neck joint pitch axis 3, and a neck joint roll axis 4.
[0075]
Each arm portion includes a shoulder joint pitch axis 8, a shoulder joint roll axis 9, an upper arm yaw axis 10, an elbow joint pitch axis 11, a forearm yaw axis 12, a wrist joint pitch axis 13, and a wrist joint roll. It comprises a shaft 14 and a hand portion 15. The hand portion 15 is actually a multi-joint / multi-degree-of-freedom structure including a plurality of fingers. However, since the movement of the hand portion 15 has little contribution or influence on the posture control or walking control of the robot 100, it is assumed in this specification that the degree of freedom is zero. Therefore, it is assumed that each arm portion has seven degrees of freedom.
[0076]
The trunk has three degrees of freedom: a trunk pitch axis 5, a trunk roll axis 6, and a trunk yaw axis 7.
[0077]
In addition, each leg part constituting the lower limb includes a hip joint yaw axis 16, a hip joint pitch axis 17, a hip joint roll axis 18, a knee joint pitch axis 19, an ankle joint pitch axis 20, and an ankle joint roll axis 21. , And the foot portion 22. In the present specification, the intersection of the hip joint pitch axis 17 and the hip joint roll axis 18 defines the hip joint position of the robot 100 according to the present embodiment. The human foot 22 is actually a structure including a multi-joint / multi-degree-of-freedom sole, but the bottom of the legged mobile robot 100 according to this embodiment has zero degrees of freedom. Accordingly, each leg is configured with 6 degrees of freedom.
[0078]
In summary, the legged mobile robot 100 according to the present embodiment has a total of 3 + 7 × 2 + 3 + 6 × 2 = 32 degrees of freedom. However, the legged mobile robot 100 for entertainment is not necessarily limited to 32 degrees of freedom. It goes without saying that the degree of freedom, that is, the number of joints, can be increased or decreased as appropriate in accordance with design and manufacturing constraints and required specifications.
[0079]
Each degree of freedom of the legged mobile robot 100 as described above is actually implemented using an actuator. It is preferable that the actuator be small and light in light of demands such as eliminating the appearance of extra bulges on the appearance and approximating the shape of a human body, and performing posture control on an unstable structure such as biped walking. . In this embodiment, a small AC servo actuator of a gear direct connection type and a servo control system of a single chip built in a motor unit is mounted. This type of AC servo actuator is disclosed in, for example, Japanese Patent Laid-Open No. 2000-299970 (Japanese Patent Application No. 11-33386) already assigned to the present applicant.
[0080]
FIG. 4 schematically shows a control system configuration of the legged mobile robot 100. As shown in the figure, the legged mobile robot 100 includes each of the mechanism units 30, 40, 50R / L, 60R / L expressing human limbs, and adaptive control for realizing a cooperative operation between the mechanism units. (Where R and L are suffixes indicating right and left, and so on).
[0081]
The entire operation of the legged mobile robot 100 is controlled by the control unit 80 in an integrated manner. The control unit 80 exchanges data and commands between a main control unit 81 including main circuit components (not shown) such as a CPU (Central Processing Unit) and a memory, and each component of the power supply circuit and the robot 100. The peripheral circuit 82 includes an interface (none of which is shown).
[0082]
In realizing the present invention, the installation location of the control unit 80 is not particularly limited. Although it is mounted on the trunk unit 40 in FIG. 4, it may be mounted on the head unit 30. Alternatively, the control unit 80 may be provided outside the legged mobile robot 100 so as to communicate with the body of the legged mobile robot 100 by wire or wirelessly.
[0083]
Each joint degree of freedom in the legged mobile robot 100 shown in FIG. 3 is realized by a corresponding actuator. That is, the head unit 30 includes a neck joint yaw axis actuator A that represents the neck joint yaw axis 2, the neck joint pitch axis 3, and the neck joint roll axis 4. ₂ , Neck joint pitch axis actuator A _Three , Neck joint roll axis actuator A _Four Is arranged.
[0084]
The trunk unit 40 includes a trunk pitch axis actuator A that represents the trunk pitch axis 5, the trunk roll axis 6, and the trunk yaw axis 7. _Five , Trunk roll axis actuator A ₆ , Trunk yaw axis actuator A ₇ Is deployed.
[0085]
Further, the arm unit 50R / L is subdivided into an upper arm unit 51R / L, an elbow joint unit 52R / L, and a forearm unit 53R / L, but the shoulder joint pitch axis 8, the shoulder joint roll axis 9, and the upper arm Shoulder joint pitch axis actuator A representing the yaw axis 10, the elbow joint pitch axis 11, the elbow joint roll axis 12, the wrist joint pitch axis 13, and the wrist joint roll axis 14. ₈ , Shoulder joint roll axis actuator A ₉ , Upper arm yaw axis actuator A _Ten , Elbow joint pitch axis actuator A ₁₁ , Elbow joint roll axis actuator A ₁₂ , Wrist joint pitch axis actuator A ₁₃ Wrist joint roll axis actuator A ₁₄ Is deployed.
[0086]
The leg unit 60R / L is subdivided into a thigh unit 61R / L, a knee unit 62R / L, and a shin unit 63R / L, but the hip joint yaw axis 16, the hip joint pitch axis 17, the hip joint Hip joint yaw axis actuator A representing each of roll axis 18, knee joint pitch axis 19, ankle joint pitch axis 20, and ankle joint roll axis 21 ₁₆ Hip joint pitch axis actuator A ₁₇ , Hip joint roll axis actuator A ₁₈ , Knee joint pitch axis actuator A ₁₉ , Ankle joint pitch axis actuator A ₂₀ , Ankle joint roll axis actuator A _{twenty one} Is deployed.
[0087]
Actuator A used for each joint ₂ , A _Three More preferably, it can be constituted by a small AC servo actuator (described above) of a gear direct coupling type and a servo control system that is mounted on a motor unit in a single chip.
[0088]
For each mechanism unit such as the head unit 30, trunk unit 40, arm unit 50, and leg unit 60, sub-control units 35, 45, 55, and 65 for actuator drive control are provided. Furthermore, grounding confirmation sensors 91 and 92 for detecting whether or not the soles of the legs 60R and L have landed are mounted, and the trunk unit 40 is equipped with a posture sensor 93 for measuring posture. ing.
[0089]
The grounding confirmation sensors 91 and 92 are configured by, for example, proximity sensors or micro switches installed on the soles of the feet. In addition, the posture sensor 93 is configured by, for example, a combination of an acceleration sensor and a gyro sensor.
[0090]
Based on the outputs of the ground contact confirmation sensors 91 and 92, it is possible to determine whether the left and right legs are currently standing or swinging during an operation period such as walking or running. Further, the inclination and posture of the trunk can be detected by the output of the posture sensor 93.
[0091]
The main control unit 80 can dynamically correct the control target in response to the outputs of the sensors 91 to 93. More specifically, an adaptive control is performed on each of the sub-control units 35, 45, 55, and 65, and a whole body motion pattern in which the upper limbs, trunk, and lower limbs of the legged mobile robot 100 are driven in cooperation. Can be realized.
[0092]
The whole body movement on the body of the robot 100 sets a foot movement, a ZMP (Zero Moment Point) trajectory, a trunk movement, an upper limb movement, a waist height, and the like, and instructs an operation according to these setting contents. The command is transferred to each sub-control unit 35, 45, 55, 65. And each sub-control part 35, 45 ... interprets the received command from the main control part 81, and each actuator A ₂ , A _Three A drive control signal is output to. Here, “ZMP” refers to a point on the floor where the moment due to floor reaction force during walking is zero, and “ZMP trajectory” refers to, for example, ZMP during the walking motion period of the robot 100. Means the trajectory that moves (see above).
[0093]
Next, in the legged mobile robot 100 according to the present embodiment, the procedure of posture stabilization processing at the time of legged work, that is, at the time of executing the whole body cooperative movement pattern including the foot, waist, trunk, lower limb movement, etc. will be described. .
[0094]
The legged mobile robot 100 according to the present embodiment is an infinite or continuous collection of mass points. However, here, the calculation amount for the stabilization processing is reduced by substituting with an approximate model composed of a finite number of discrete mass points. More specifically, the legged mobile robot 100 having the multi-joint degree-of-freedom configuration shown in FIG. 3 is handled by replacing it with a multi-mass point approximation model as shown in FIG. The approximate model shown is a linear and non-interfering multi-mass point approximate model.
[0095]
In FIG. 5, the O-XYZ coordinate system represents roll, pitch, and yaw axes in the absolute coordinate system, and the O′-X′Y′Z ′ coordinate system represents the roll, pitch, Each axis of yaw is represented. In the multi-mass model shown in the figure, i is a subscript representing the i-th given mass, m _i Is the mass of the i-th mass point, r ' _i Represents the position vector of the i-th mass point (however, the motion coordinate system). The mass of the lumbar mass point, which is particularly important in lumbar motion control described later, is m _h The position vector is r ′ _h (R ' _hx , R ' _hy , R ' _hz ) And the position vector of ZMP is r ′ _zmp (R ' _zmpx , R ' _zmpy , R ' _zmpz ).
[0096]
The multi-mass model is a representation of a robot in the form of a wire frame model. As can be seen from FIG. 5, the multi-mass point approximation model is set with each shoulder, both elbows, both wrists, trunk, waist, and both ankles as mass points. In the illustrated inexact multi-mass point approximation model, the moment formula is described in the form of a linear equation that does not interfere with the pitch and roll axes. The multi-mass point approximation model can be generated by the following processing procedure.
[0097]
(1) The mass distribution of the entire robot 100 is obtained.
(2) Set the mass point. The mass point setting method may be either manual input by the designer or automatic generation according to a predetermined rule.
(3) The center of gravity is obtained for each region i, the center of gravity position and the mass m _i Is assigned to the relevant mass point.
(4) Each mass point m _i , The mass position r _i Is displayed as a sphere with a radius proportional to its mass.
(5) The mass points that are actually connected, that is, the spheres are connected.
[0098]
Note that each rotation angle (θ in the waist information of the multi-mass model shown in FIG. _hx , Θ _hy , Θ _hz ) Regulates the posture of the waist in the legged mobile robot 100, that is, the rotation of the roll, pitch, and yaw axis (FIG. 6 shows an enlarged view of the multi-mass model around the waist). Wanna)
[0099]
Next, the procedure of the posture stabilization process in the legged mobile robot 100 according to the present embodiment, that is, the generation process of the whole body cooperative movement pattern in which the moments generated by the movements of the waist, trunk, and lower limbs are canceled will be described.
[0100]
7 and 8 show an example of a processing procedure for generating waist, trunk, and lower limb movements that can be stably walked by the legged mobile robot 100 in the form of a flowchart. However, in the following description, each joint position and operation of the legged mobile robot 100 are described using a linear / non-interference multi-mass point approximation model as shown in FIG. Will be used. However, it should be understood that symbols with a dash (') describe a motion coordinate system.
[0101]
[Expression 1]

[0102]
In addition, the waist height of the legged mobile robot 100 is constant (r ′ _hz + R _qz = Const) and the knee mass point is zero.
[0103]
First, actually determine the driving and movement of each part such as foot (more specifically, plantar) movement, ZMP trajectory derived from foot movement, trunk movement, upper limb movement, waist posture and height, etc. A pattern for this is set (step S11). More specifically, first, a foot movement pattern, then a ZMP trajectory, a trunk movement pattern, and an upper limb movement pattern are set. Also, an unknown variable (related to upper limb movement) used for yaw axis moment compensation described later is set here. Regarding the movement of the waist, only the Z ′ direction is set, and the X ′ and Y ′ directions are unknown.
[0104]
Next, the moment M around the pitch axis on the set ZMP generated by the foot, trunk, and upper limb movements using the linear / non-interference multi-mass approximation model _y , And moment M around the roll axis _x Is calculated (step S12).
[0105]
Next, the motion in the lumbar horizontal plane (r ′ _hx , R ' _hy ) Moment M about the pitch axis on the setting ZMP _y , And moment M around the roll axis _x Is calculated (step S13).
[0106]
Next, a balance equation regarding the moment about the pitch axis and the roll axis on the set ZMP is derived on the motion coordinate system O′-X′Y′Z ′ that moves with the robot 100 (or its multi-mass point approximate model) (step S14). ). More specifically, the moment M about the pitch axis generated by the foot, trunk, and upper limb movements _y , And moment M around the roll axis _x On the right side as a term of a known variable, a term (r ′ _hx , R ' _hy ) As unknown variable terms on the left side, and a linear and non-interfering ZMP equation (1) as shown in the following equation is derived.
[0107]
[Expression 2]

[0108]
However, the following shall hold.
[0109]
[Equation 3]

[0110]
Next, the above-mentioned ZMP equation (1) is solved to calculate the waist horizontal plane trajectory (step S15). For example, by solving the ZMP equation (1) using a numerical solution (well known) such as the Euler method or the Runge-Kutta method, the horizontal absolute position of the waist (r ′ _hx , R ' _hy ) Can be obtained (step S16). The numerical solution obtained here is an approximate solution of a waist motion pattern that enables stable walking, and more specifically, a waist horizontal absolute position where ZMP enters the target position. The ZMP target position is usually set at the planted sole.
[0111]
The approximate solution obtained here is an approximate solution (r ′ of the horizontal absolute position of the waist that defines the waist motion pattern capable of stable walking. _hx , R ' _hy More specifically, it is the waist horizontal absolute position where ZMP enters the target position. However, the moment M about the pitch axis generated by the movement of the foot, trunk, upper limbs, waist, etc. _y , And moment M around the roll axis _x Is canceled, but the moment M around the yaw axis is still _z Is left without cancellation. Therefore, in the subsequent steps, the moment M around the yaw axis _z An upper limb motion pattern generation process for canceling the above is performed.
[0112]
In step S17, the yaw axis moment on the set ZMP generated by the approximate solution of the waist motion (however, the yaw axis moment generated in the waist) M _zh Is calculated using the following equation (2).
[0113]
[Expression 4]

[0114]
Next, using a linear / non-interference multi-mass point approximation model, moment M about the yaw axis on the set ZMP generated by the motion of the foot and trunk _z Is calculated (step S18).
[0115]
Next, moment M about the yaw axis on the set ZMP _z Is derived on a motion coordinate system O′-X′Y′Z ′ that moves with the robot 100 (or its multi-mass point approximation model) (step S19). More specifically, the moment M about the yaw axis generated by the movement of the foot and trunk _z And yaw moment M generated by approximate solution of lumbar motion _zh Is the term of the known variable, and the moment M around the yaw axis generated by the motion of the upper limb mass point _arm Are combined on the left side as terms of unknown variables to derive a linear and non-interfering ZMP equation (3) as shown in the following equation.
[0116]
[Equation 5]

[0117]
Further, by solving the ZMP equation (3) using a numerical solution (well-known) such as the Euler method or the Runge-Kutta method (step S20), the yaw axis moment M _z And M _zh To calculate the upper limb movement pattern necessary to enable stable walking (step S21). When calculating the upper limb motion pattern by a numerical solution, an unknown variable for compensating the yaw axis moment given in advance is applied.
[0118]
In the above equation (3), any joint angle of the upper limb joint may be used as the unknown variable as a method of taking the unknown variable θ. More specifically, among the joint degrees of freedom shown in FIG. 3, the shoulder joint pitch axis 8, the shoulder joint roll axis 9, the upper arm yaw axis 10, the elbow joint pitch axis 11, the forearm yaw axis 12, the wrist joint pitch axis. 13. The above equation (3) may be established by combining one or more of the wrist joint roll shafts 14.
[0119]
In establishing the above equation (3), it is not necessary to assume that the upper right limb and the left upper limb move symmetrically or in opposite phases. As the method of taking the unknown variable θ, it is assumed that a large moment generation amount can be expected, and the joint angle (θ _{arm_shoulder} ) As an unknown variable. Alternatively, the moment generation operation closer to the human biological mechanism can be expected as the joint angle around the pitch axis of the shoulder joint (θ _{arm_shoulder} ) Is an unknown variable, and the elbow joint angle (θ _{arm_elbow} ) To (θ _{arm_elbow} = G x θ _{arm_shoulder} : G is a gain), it is possible to calculate the yaw axis compensation motion pattern by the cooperative operation of the shoulder joint and the elbow joint while keeping one unknown variable (that is, without changing the calculation amount).
[0120]
Of course, the above equation (3) can be established by using the trunk yaw axis 7 which is a part of the upper body of the robot 100, but it is theoretically possible, but a shoulder joint or elbow joint with a wide movable angle. It is easier to realize the above-described expression (3) using the driving of the upper limbs such as the above, and high-precision control is possible.
[0121]
If the preset trunk / upper limb movement cannot be realized on the approximate solution calculated up to step S21, the trunk / upper limb movement pattern is reset / corrected (step S22). At this time, the knee trajectory may be calculated.
[0122]
Next, the whole body motion pattern obtained as described above is substituted, and the roll axis on the setting ZMP in the exact model (that is, the precise model of the legged mobile robot 100 composed of a rigid body or a large number of mass points). , Moment about each axis of pitch axis and yaw axis (eM _x , EM _y , EM _z ) Is calculated (step S23). In the inexact model, it is assumed that the above [Equation 3] holds, but in the exact model, such an assumption is not required (that is, it is not necessary to be constant with respect to a change in time).
[0123]
Moment in exact model (eM _x , EM _y , EM _z ) Is a moment error generated around the roll, pitch, and yaw axes in the waist movement. In subsequent step S24, moments (eM _x , EM _y , EM _z ) Is the allowable value (εM _x , ΕM _y , ΕM _z ) It is determined whether it is less than. If it is less than the allowable value ε, the exact solution of the stable motion pattern in each part of the waist, trunk, and lower limbs on the body of the robot 100, and the moment M generated around the yaw axis of the body (waist) _z And M _zh This means that the whole body motion pattern of the robot 100 that has been canceled can be obtained (step S25), and the entire processing routine is terminated.
[0124]
On the other hand, the moment in the exact model (eM _x , EM _y , EM _z ) Is the allowable value (εM _x , ΕM _y , ΕM _z ) Or more, the moment (eM around the roll axis and pitch axis in the strict model) _x , EM _y ) To the moment about the roll axis and pitch axis in the approximate model (M _x , M _y ) Is corrected (step S27), and the ZMP equation is derived again. Alternatively, the moment moment eM about the yaw axis in the exact model _z The known generated moment M around the yaw axis in the approximate model using _z Is corrected (step S26), and the ZMP equation is derived again. Then, until the convergence to below the allowable value ε, by repeatedly executing the calculation and correction of the approximate solution of the waist, trunk, and lower limb motion patterns as described above, the strict solution of the waist and upper limb motion patterns and the stable A whole body movement pattern that can realize walking can be obtained.
[0125]
That is, according to the processing routines shown in FIGS. 7 and 8, the aircraft can be driven stably and accurately accurately while canceling the influence of the moments about the roll, pitch, and yaw axes applied during the legged work. A whole body motion pattern for a legged mobile robot can be generated. In addition, it is possible to stably calculate the movements around the roll, pitch, and yaw axes that interfere with each other.
[0126]
In addition, according to the processing routine shown in FIGS. 7 and 8, since the motion that is extremely non-slipable around the yaw axis where the yaw axis moment on the ZMP becomes almost zero is generated in advance, the aircraft moves around the yaw axis. Hardly slips. In particular, it is an indispensable technology for entertainment robots where movement itself is important. In addition, when a moment around the yaw axis is applied to the robot, the pitch axis, roll axis, and yaw axis are solved simultaneously by the action to cancel the moment around the yaw axis. No moment around the roll axis is generated.
[0127]
In the processing routines shown in FIGS. 7 and 8, the upper limb motion that compensates for the yaw moment obtained in step S21 is realized by a motion utilizing swinging of both arms or rotation of the waist in the case of a human being. be able to. Such an upper limb movement contributes greatly to maintaining posture stability, particularly in high-speed walking movements, and also has an effect of making the upper body movement of the aircraft more expressive.
[0128]
When considering the “expressive power” of movement in humans and monkeys, the movement and posture of the upper body, such as arms and torso, are not only the realization of work, but also the aspect of emotional manifestation, which is very important have. This is why it is called “gesture” or “hand gesture”. Therefore, by compensating for the yaw moment by utilizing the upper body-driven motions such as gestures and hand gestures, the motion expressive power of the aircraft is enriched, and is particularly preferable for a robot with high entertainment orientation.
[0129]
9 and 10 show other examples of processing procedures for generating waist, trunk, and lower limb motions that can be stably walked by the legged mobile robot 100 in the form of flowcharts. However, as described above, the joint positions and operations of the robot 100 are described using the linear / non-interfering multi-mass point approximation model shown in FIG.
[0130]
First, actually determine the driving and movement of each part such as foot (more specifically, plantar) movement, ZMP trajectory derived from foot movement, trunk movement, upper limb movement, waist posture and height, etc. A pattern for this is set (step S31). More specifically, first, a foot movement pattern, then a ZMP trajectory, a trunk movement pattern, and an upper limb movement pattern are set. Also, an unknown variable (related to upper limb movement) used for yaw axis moment compensation described later is set here. Regarding the movement of the waist, only the Z ′ direction is set, and the X ′ and Y ′ directions are unknown.
[0131]
Next, the moment M around the pitch axis on the set ZMP generated by the foot, trunk, and upper limb movements using the linear / non-interference multi-mass approximation model _y , And moment M around the roll axis _x Is calculated (step S32).
[0132]
Next, the motion in the lumbar horizontal plane (r ′ _hx , R ' _hy ) Moment M about the pitch axis on the setting ZMP _y , And moment M around the roll axis _x Is calculated (step S33).
[0133]
Next, a balance equation regarding the moment about the pitch axis and the roll axis on the set ZMP is derived on the motion coordinate system O′-X′Y′Z ′ that moves together with the robot 100 (or its multi-mass approximate model) (step S34). ). More specifically, the moment M about the pitch axis generated by the foot, trunk, and upper limb movements _y , And moment M around the roll axis _x On the right side as a term of a known variable, a term (r ′ _hx , R ' _hy ) As terms of unknown variables on the left side, and a linear and non-interfering ZMP equation (1) is derived (same as above).
[0134]
Next, exercise in the lumbar horizontal plane (r ′ _hx , R ' _hy ) In the Fourier series, and then the moment M around the pitch axis on the set ZMP _y , And moment M around the roll axis _x For each of these, a Fourier series expansion is performed, then a Fourier coefficient of the orbit in the lumbar horizontal plane is calculated, and further an inverse Fourier series expansion is performed (step S35), whereby an approximate solution of the lumbar motion is obtained (step S36).
[0135]
As already well known in the art, the time series component can be replaced with the frequency component for calculation by expanding the Fourier series. That is, in this case, the movement of the waist can be regarded as a periodic movement. Moreover, since FFT (Fast Fourier Transform) can be applied, the calculation speed can be greatly improved.
[0136]
The approximate solution obtained here is an approximate solution (r ′ of the horizontal absolute position of the waist that defines the waist motion pattern capable of stable walking. _hx , R ' _hy More specifically, it is the waist horizontal absolute position where ZMP enters the target position. However, the moment M about the pitch axis generated by the movement of the foot, trunk, upper limbs, waist, etc. _y , And moment M around the roll axis _x Is canceled, but the moment M around the yaw axis is still _z Is left without cancellation. Therefore, in the subsequent steps, the moment M around the yaw axis _z And M _zh The upper limbs movement pattern generation process for canceling is performed.
[0137]
In step S17, the yaw axis moment on the set ZMP moment generated by the approximate solution of the waist motion (however, the yaw axis moment generated in the waist) M _zh Is calculated using the above equation (2).
[0138]
Next, the moment M around the yaw axis on the set ZMP generated by the foot motion and the trunk motion using the linear / non-interference multi-mass point approximation model _z Is calculated (step S38).
[0139]
Next, a balance equation relating to the moment about the yaw axis on the set ZMP is derived on the motion coordinate system O′-X′Y′Z ′ that moves with the robot 100 (or its multi-mass approximate model) (step S39). More specifically, the moment M about the yaw axis generated by the movement of the foot and trunk _z Moment M around the yaw axis caused by the approximate solution of the lumbar movement _zh Is the term of the known variable, and the moment M around the yaw axis generated by the motion of the upper limb mass point _arm Are combined on the left side as terms of unknown variables to derive a linear and non-interfering ZMP equation (3) as shown in the above equation.
[0140]
Next, as in step S35, an upper limb motion pattern is calculated (step S41) by a Fourier coefficient comparison method in the frequency domain of the ZMP equation (step S40). When calculating the upper limb motion pattern, an unknown variable for compensating the yaw axis moment given in advance is applied.
[0141]
In the above equation (3), any joint angle of the upper limb joint may be used as the unknown variable as a method of taking the unknown variable θ. More specifically, among the joint degrees of freedom shown in FIG. 3, the shoulder joint pitch axis 8, the shoulder joint roll axis 9, the upper arm yaw axis 10, the elbow joint pitch axis 11, the forearm yaw axis 12, the wrist joint pitch axis. 13. The above equation (3) may be established by combining one or more of the wrist joint roll shafts 14.
[0142]
In establishing the above equation (3), it is not necessary to assume that the upper right limb and the left upper limb move symmetrically or in opposite phases. As an example of how to take the unknown variable θ, the joint angle around the pitch axis of the shoulder joint (θ _{arm_shoulder} ) As an unknown variable. Alternatively, the moment generation operation closer to the human biological mechanism can be expected as the joint angle around the pitch axis of the shoulder joint (θ _{arm_shoulder} ) Is an unknown variable, and the elbow joint angle (θ _{arm_elbow} ) To (θ _{arm_elbow} = G x θ _{arm_shoulder} : G is a gain), it is possible to calculate the yaw axis compensation motion pattern by the cooperative operation of the shoulder joint and the elbow joint while keeping one unknown variable (that is, without changing the calculation amount).
[0143]
Of course, the above equation (3) can be established by using the trunk yaw axis 7 which is a part of the upper body of the robot 100, but it is theoretically possible, but a shoulder joint or elbow joint with a wide movable angle. It is easier to realize the above-described expression (3) using the driving of the upper limbs such as the above, and high-precision control is possible.
[0144]
If the preset trunk / upper limb movement cannot be realized on the approximate solution calculated up to step S21, the trunk / upper limb movement pattern is reset / corrected (step S42). At this time, the knee trajectory may be calculated.
[0145]
Next, the whole body motion pattern obtained as described above is substituted, and the roll axis on the setting ZMP in the exact model (that is, the precise model of the legged mobile robot 100 composed of a rigid body or a large number of mass points). , Moment about each axis of pitch axis and yaw axis (eM _x , EM _y , EM _z ) Is calculated (step S43). In the inexact model, it is assumed that the above [Equation 3] holds, but in the exact model, such an assumption is not required (that is, it is not necessary to be constant with respect to a change in time).
[0146]
Moment in exact model (eM _x , EM _y , EM _z ) Is a moment error generated around the roll, pitch, and yaw axes in the waist movement. In subsequent step S44, moments (eM _x , EM _y , EM _z ) Is the allowable value (εM _x , ΕM _y , ΕM _z ) It is determined whether it is less than. If it is less than the allowable value ε, the robot 100's whole body motion in which the exact solution of the stable motion pattern in the waist, trunk, and lower limbs on the body of the robot 100 and the moment generated around the yaw axis of the body of the robot are canceled Since this means that the pattern has been obtained (step S45), the entire processing routine is terminated.
[0147]
On the other hand, the moment in the exact model (eM _x , EM _y , EM _z ) Is the allowable value (εM _x , ΕM _y , ΕM _z ) Or more, the moment (eM around the roll axis and pitch axis in the strict model) _x , EM _y ) To the moment about the roll axis and pitch axis in the approximate model (M _x , M _y ) Is corrected (step S47), and the ZMP equation is derived again. Alternatively, the moment moment eM about the yaw axis in the exact model _z The known generated moment M around the yaw axis in the approximate model using _z Is corrected (step S46), and the ZMP equation is derived again. Then, until the convergence to below the allowable value ε, by repeatedly executing the calculation and correction of the approximate solution of the waist, trunk, and lower limb motion patterns as described above, the strict solution of the waist and upper limb motion patterns and the stable A whole body movement pattern that can realize walking can be obtained.
[0148]
That is, according to the processing routines shown in FIGS. 9 and 10, the aircraft can be driven stably and accurately accurately while canceling the influence of the moments about the roll, pitch, and yaw axes applied during the legged work. A whole body motion pattern for a legged mobile robot can be generated. In addition, it is possible to stably calculate the movements around the roll, pitch, and yaw axes that interfere with each other.
[0149]
Further, according to the processing routines shown in FIGS. 9 and 10, since the movement that is extremely non-slipable around the yaw axis where the yaw axis moment on the ZMP becomes almost zero is generated in advance, the aircraft is rotated around the yaw axis. Hardly slips. In particular, it is an indispensable technology for entertainment robots where movement itself is important. In addition, when a moment around the yaw axis is applied to the robot, the pitch axis, roll axis, and yaw axis are solved simultaneously by the action to cancel the moment around the yaw axis. No moment around the roll axis is generated.
[0150]
In the processing routines shown in FIGS. 9 and 10, the upper limb motion that compensates for the yaw moment obtained in step S41 is realized by a motion using swinging of both arms or rotation of the waist in the case of a human. be able to. Such an upper limb movement contributes greatly to maintaining posture stability, particularly in high-speed walking movements, and also has an effect of making the upper body movement of the aircraft more expressive.
[0151]
When considering the “expressive power” of movement in humans and monkeys, the movement and posture of the upper body, such as arms and torso, are not only the realization of work, but also the aspect of emotional manifestation, which is very important have. This is why it is called “gesture” or “hand gesture”. Therefore, by compensating for the yaw moment by utilizing the upper body-driven motions such as gestures and hand gestures, the motion expressive power of the aircraft is enriched, and is particularly preferable for a robot with high entertainment orientation.
[0152]
[Supplement]
The present invention has been described in detail above with reference to specific embodiments. However, it is obvious that those skilled in the art can make modifications and substitutions of the embodiment without departing from the gist of the present invention.
[0153]
Further, the gist of the present invention is not necessarily limited to a product called “robot”. That is, as long as it is a mechanical device that performs a movement resembling human movement using an electrical or magnetic action, the present invention is similarly applied to a product belonging to another industrial field such as a toy. Can be applied.
[0154]
In short, the present invention has been disclosed in the form of exemplification, and the description of the present specification should not be interpreted in a limited manner. In order to determine the gist of the present invention, the claims section described at the beginning should be considered.
[0155]
【The invention's effect】
As described above in detail, according to the present invention, an excellent motion control device and motion control method for a legged mobile robot capable of stably and accurately realizing legged work such as high-speed walking, and A robotic device can be provided.
[0156]
Further, according to the present invention, an excellent motion control device and motion control method for a legged mobile robot capable of reducing the moment applied around the yaw axis of the airframe using the motion of the upper limbs, and A robotic device can be provided.
[Brief description of the drawings]
FIG. 1 is a view showing a state in which a legged mobile robot 100 used for carrying out the present invention is viewed from the front.
FIG. 2 is a view showing a state in which a legged mobile robot 100 used for carrying out the present invention is viewed from behind.
FIG. 3 is a diagram schematically illustrating a degree-of-freedom configuration model included in the legged mobile robot 100 according to the present embodiment.
FIG. 4 is a diagram schematically illustrating a control system configuration of a legged mobile robot 100 according to the present embodiment.
FIG. 5 is a diagram showing a linear and non-interfering multi-mass point approximation model of the legged mobile robot 100 introduced for calculation of walking control according to the present embodiment.
6 is an enlarged view around the waist of the multi-mass model shown in FIG.
FIG. 7 is a flowchart showing an example of a processing procedure for generating waist, trunk, and lower limb motions capable of stable walking in the legged mobile robot 100.
FIG. 8 is a flowchart showing an example of a processing procedure for generating waist, trunk, and lower limb movements capable of stable walking in the legged mobile robot 100.
FIG. 9 is a flowchart showing another example of a processing procedure for generating waist, trunk, and lower limb movements capable of stable walking in the legged mobile robot 100.
FIG. 10 is a flowchart showing another example of a processing procedure for generating waist, trunk, and lower limb motions in which the legged mobile robot 100 can stably walk.
FIG. 11 is a diagram illustrating a relationship (example) between a walking speed [second / step] of a biped legged mobile robot and a moment Nm generated in the yaw axis direction.
[Explanation of symbols]
1 ... head, 2 ... neck yaw axis
3 ... Neck joint pitch axis, 4 ... Neck joint roll axis
5 ... trunk pitch axis, 6 ... trunk roll axis
7 ... trunk yaw axis, 8 ... shoulder joint pitch axis
9 ... Shoulder joint roll axis, 10 ... Upper arm yaw axis
11 ... Elbow joint pitch axis, 12 ... Forearm yaw axis
13 ... wrist joint pitch axis, 14 ... wrist joint roll axis
15 ... hand, 16 ... hip yaw axis
17 ... Hip pitch axis, 18 ... Hip roll axis
19 ... Knee joint pitch axis, 20 ... Ankle joint pitch axis
21 ... Ankle joint roll axis, 22 ... Foot
30 ... head unit, 40 ... trunk unit
50 ... arm unit, 51 ... upper arm unit
52 ... Elbow joint unit, 53 ... Forearm unit
60 ... Leg unit, 61 ... Thigh unit
62 ... knee joint unit, 63 ... shin unit
80 ... control unit, 81 ... main control unit
82. Peripheral circuit
91, 92 ... Grounding confirmation sensor
93 ... Attitude sensor
100: Legged mobile robot

Claims (30)

It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control device for a type of robot that performs legged movement by the lower limb,
Means for setting at least leg movement and ZMP ;
Means for calculating a yaw-axis moment of the airframe of the legged mobile robot generated by the set motion on the set ZMP;
Means for calculating upper limb motion to cancel the yaw axis moment;
Means for correcting the set movement according to the calculated upper limb movement;
A motion control apparatus for a legged mobile robot, comprising: The upper limb has one or more degrees of freedom of rotation in each of the shoulder joint and the elbow joint,
The upper limb motion that cancels the yaw moment is a motion using the drive of the shoulder joint or elbow joint.
The motion control apparatus for a legged mobile robot according to claim 1. It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control device for a type of robot that performs legged movement by the lower limb,
Means for setting at least leg movement and ZMP ;
Means for calculating at least one of the pitch axis or roll axis moment of the airframe of the legged mobile robot generated by the set motion on the set ZMP;
First movement calculating means for calculating movement of at least one of a lower limb or a trunk that cancels the pitch axis or roll axis moment;
Means for calculating a yaw-axis moment of the body of the legged mobile robot generated by the calculated movement of the lower limbs or trunk on the set ZMP;
A second motion calculating means for calculating an upper limb motion that cancels the yaw axis moment;
Means for correcting the set movement according to the movement calculated by the first and second movement calculation means ;
A motion control apparatus for a legged mobile robot, comprising: The upper limb has one or more degrees of freedom of rotation in each of the shoulder joint and the elbow joint,
The upper limb motion that cancels the yaw moment is a motion using the drive of the shoulder joint or elbow joint.
The motion control apparatus for a legged mobile robot according to claim 3. It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control device for a type of robot that performs legged movement by the lower limb,
(A) means for setting a foot motion, trunk motion, upper limb motion, waist posture and height for realizing the requested motion ;
(B) means for setting a ZMP trajectory based on the foot movement set by the means (a);
(C) On the set ZMP, the moments about the pitch axis and roll axis of the legged mobile robot by the means (b) by the movement of the foot, trunk and upper limb set by the means (a) Means for calculating
(D) means for obtaining a solution of the waist motion that cancels the moments about the pitch axis and roll axis calculated by the means (c);
(E) means for calculating a moment generated around the yaw axis of the legged mobile robot by the means (b) on the set ZMP by the waist movement obtained by the means (d);
(F) means for obtaining a solution of the upper limb motion that cancels the moment about the yaw axis calculated by the means (e);
(G) means for deriving the whole body movement of the legged mobile robot based on the waist motion and upper limb movement obtained by the means (d) and the means (f);
A motion control apparatus for a legged mobile robot, comprising: It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control device for a type of robot that performs legged movement by the lower limb,
(A) means for setting a foot motion, trunk motion, upper limb motion, waist posture and height for realizing the requested motion ;
(B) means for setting a ZMP trajectory based on the foot movement set by the means (A);
(C) Moments about the pitch axis and roll axis of the legged mobile robot generated by the movement of the foot, trunk, and upper limb set by the means (A) on the ZMP set by the means (B) Means for calculating using an inexact model of the legged mobile robot ;
(D) means for obtaining an approximate solution of the motion in the horizontal plane of the waist that cancels out the moment about the pitch axis and roll axis calculated by the means (C);
(E) a moment generated about the yaw axis of the legged mobile robot by the approximate solution of the horizontal plane motion of the lumbar obtained by said means on set ZMP (D) of the said means (B), the legged Means for calculating using an inexact model of the mobile robot ;
(F) means for obtaining an approximate solution of upper limb movement that cancels the moment about the yaw axis calculated by the means (E);
(G) The pitch axis, roll axis, and yaw of the legged mobile robot that are generated when the whole body movement calculated by the means (D) and the means (F) is performed on the ZMP set by the means (B). Means for calculating a moment about an axis using an exact model of the legged mobile robot ;
(H) means for determining the whole body motion if the moment about the pitch axis, roll axis, and yaw axis calculated by the means (G) is less than a predetermined allowable value;
(I) If the moment about the pitch axis, roll axis, and yaw axis calculated by the means (G) is equal to or greater than a predetermined allowable value, the inexact model on the ZMP set by the means (B) Means for correcting the moment and re-injecting the means (D) or the means (F);
A motion control apparatus for a legged mobile robot, comprising: The inexact model is a linear or non-interfering multi-mass point approximation model for the legged mobile robot;
The exact model is a rigid body model for the robot, or a nonlinear or interference multi-mass point approximation model,
The motion control apparatus for a legged mobile robot according to claim 6. further,
(F ′) The trunk and upper limb movements set in the means (A) in the approximate solution of the posture and height of the waist set in the means (A) and the horizontal movement of the waist determined in the means (D) If but can not be achieved, or the in the approximate solution of the upper limb motion determined in means (F) when the trunk motion and upper limb motion set in the unit (a) can not be realized, the trunk motion and resetting the upper limb or Means for making corrections,
The motion control apparatus for a legged mobile robot according to claim 6. Said means (D) is the foot motion on said set by means (B) ZMP, trunk movement, a moment generated by the upper limb, said means (B) waist in the horizontal plane on set ZMP that by Obtain an approximate solution of the posture and height of the waist and the motion in the horizontal plane by solving the balance equation with the moment generated by the motion.
The motion control apparatus for a legged mobile robot according to claim 6. The means (D) is calculated by replacing a function of time with a function of frequency.
The motion control apparatus for a legged mobile robot according to claim 6. The means (D) applies Fourier series expansion to moments generated by foot movement, trunk movement, and upper limb movement on the ZMP set by the means (B) , and Fourier series expansion to the horizontal movement of the waist. To calculate the Fourier coefficient of the orbit in the lumbar horizontal plane, and further apply the inverse Fourier series expansion to obtain an approximate solution of the posture and height of the lumbar region and the motion in the horizontal plane ,
The motion control apparatus for a legged mobile robot according to claim 6. The means (F) for obtaining an approximate solution of the upper limb motion is an approximate solution of the upper limb motion by solving a balance equation of moments generated by foot motion and trunk motion on the ZMP set by the means (B). Seeking
The motion control apparatus for a legged mobile robot according to claim 6. The means (F) for obtaining an approximate solution of the upper limb movement is calculated by replacing a function of time with a function of frequency.
The motion control apparatus for a legged mobile robot according to claim 6. The means (F) for obtaining an approximate solution of the upper limb movement calculates a Fourier coefficient by applying a Fourier series expansion to the moment generated by the foot movement and the trunk movement on the ZMP set by the means (B). Further, an approximate solution of upper limb motion is obtained by applying inverse Fourier series expansion,
The motion control apparatus for a legged mobile robot according to claim 6. It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control method for a type of robot that performs legged movement by the lower limb,
Setting at least leg movement and ZMP ;
Calculating a yaw-axis moment of the body of the legged mobile robot generated by the set motion on the set ZMP;
Calculating an upper limb motion that cancels the yaw moment;
Modifying the set movement according to the calculated upper limb movement;
An operation control method for a legged mobile robot, comprising: The upper limb has one or more degrees of freedom of rotation in each of the shoulder joint and the elbow joint,
The upper limb motion that cancels the yaw moment is a motion using the drive of the shoulder joint or elbow joint.
The motion control method for a legged mobile robot according to claim 15. It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control method for a type of robot that performs legged movement by the lower limb,
Setting at least leg movement and ZMP ;
Calculating at least one of the pitch axis or roll axis moment of the airframe of the legged mobile robot caused by the set motion on the set ZMP;
A first motion calculating step for calculating a motion of at least one of a lower limb or a trunk that cancels the pitch axis or roll axis moment;
Calculating a yaw-axis moment of the body of the legged mobile robot generated by the calculated movement of the lower limb or trunk on the set ZMP;
A second motion calculating step for calculating an upper limb motion for canceling the yaw axis moment;
Correcting the set motion according to the motion calculated in the first and second motion calculation steps ;
An operation control method for a legged mobile robot, comprising: The upper limb has one or more degrees of freedom of rotation in each of the shoulder joint and the elbow joint,
The upper limb motion that cancels the yaw moment is a motion using the drive of the shoulder joint or elbow joint.
The motion control method for the legged mobile robot according to claim 17. It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control method for a type of robot that performs legged movement by the lower limb,
(A) setting a foot motion, trunk motion, upper limb motion, waist posture and height for realizing the requested motion ;
(B) setting a ZMP trajectory based on the foot movement set in step (a);
(C) On the ZMP set in step (b) , moments about the pitch axis and roll axis of the legged mobile robot generated by the movement of the foot, trunk, and upper limb set in step (a) Calculating steps,
(D) obtaining a waist motion solution that cancels the moments about the pitch axis and roll axis calculated in step (c);
(E) calculating a moment generated around the yaw axis of the legged mobile robot on the ZMP set in step (b) by the waist motion determined in step (d);
(F) obtaining an upper limb motion solution that cancels out the moment about the yaw axis calculated in step (e);
(G) deriving a whole body motion of the legged mobile robot based on the waist motion and upper limb motion determined in the steps (d) and (f);
An operation control method for a legged mobile robot, comprising: It is composed of at least an upper limb, a trunk, and a lower limb, and is a motion control method for a type of robot that performs legged movement by the lower limb,
(A) a step of setting a foot motion, trunk motion, upper limb motion, waist posture and height for realizing the requested motion ;
(B) setting a ZMP trajectory based on the foot movement set in step (A);
(C) On the ZMP set by the means (B) , the moments about the pitch axis and roll axis of the legged mobile robot generated by the movement of the foot, trunk, and upper limb set by the step (A) Calculating using an inexact model of the legged mobile robot ;
(D) obtaining an approximate solution of the motion in the horizontal plane of the waist that cancels out the moment about the pitch axis and roll axis calculated in step (C);
(E) On the ZMP set in step (B) , the moment generated around the yaw axis of the legged mobile robot by the approximate solution of the motion of the waist in the horizontal plane obtained in step (D) is Calculating using an inexact model of the mobile robot ,
(F) obtaining an approximate solution of upper limb movement that cancels the moment about the yaw axis calculated in step (E);
(G) On the ZMP set in the step (B) , the pitch axis, roll axis, and roll axis of the legged mobile robot that are generated when the whole body motion is calculated calculated in the steps (D) and (F), and Calculating a moment about the yaw axis using an exact model of the legged mobile robot ;
(H) If the moments about the pitch axis, roll axis, and yaw axis calculated in step (G) are less than a predetermined tolerance, the whole body motion is determined as a solution;
(I) If the moments about the pitch axis, roll axis, and yaw axis calculated in step (G) are equal to or greater than a predetermined allowable value, the inexact model on the ZMP set in step (B) Correcting the moment and re-entering step (D) or step (F);
An operation control method for a legged mobile robot, comprising: The inexact model is a linear or non-interfering multi-mass point approximation model for the legged mobile robot;
The exact model is a rigid body model for the robot, or a nonlinear or interference multi-mass point approximation model,
The motion control method for a legged mobile robot according to claim 20. further,
(F ′) In the approximate solution of the waist posture and height set in step (A) and the horizontal motion of the waist in the step (D), the trunk motion and upper limb motion set in step (A) If but can not be achieved, or the in the approximate solution of the upper limb motion obtained in step (F) when the trunk motion and upper limb motion set in the step (a) can not be realized, the trunk motion and resetting the upper limb or Steps to make corrections,
The operation control method for the legged mobile robot according to claim 20, comprising: Wherein step (D), the step (B) by the foot motion on set ZMP, trunk movement, the moment on the ZMP generated by the upper limb, lumbar on set ZMP that in the step (B) Approximate solution of posture and height of waist and motion in horizontal plane by solving balance equation with moment generated by horizontal motion of
The motion control method for a legged mobile robot according to claim 20. In the step (D) , the function of time is replaced with the function of frequency.
The motion control method for a legged mobile robot according to claim 20. In the step (D) , the Fourier series expansion is applied to the moment generated by the foot movement, the trunk movement, and the upper limb movement on the ZMP set in the step (B) , and the Fourier series expansion is applied to the horizontal movement of the waist. To calculate the Fourier coefficient of the orbit in the lumbar horizontal plane, and further apply the inverse Fourier series expansion to obtain an approximate solution of the posture and height of the lumbar region and the motion in the horizontal plane ,
The motion control method for a legged mobile robot according to claim 20. In the step (F) of obtaining an approximate solution of the upper limb motion, an approximate solution of the upper limb motion is solved by solving a balance equation of moments generated by foot motion and trunk motion on the ZMP set in the step (B). Seeking
The motion control method for a legged mobile robot according to claim 20. In the step (F) of obtaining an approximate solution of the upper limb movement, a time function is replaced with a frequency function, and calculation is performed.
The motion control method for a legged mobile robot according to claim 20. In the step (F) for obtaining an approximate solution of the upper limb movement, a Fourier coefficient is calculated by applying Fourier series expansion to the moment generated by the foot movement and the trunk movement on the ZMP set in the step (B). Further, an approximate solution of upper limb movement is obtained by applying inverse Fourier series expansion,
The motion control method for a legged mobile robot according to claim 20. In a robot apparatus that is composed of at least an upper limb, a trunk, and a lower limb, and performs a legged movement by the lower limb,
A motion setting means for setting the motion and ZMP of the robot;
Moment calculating means for calculating a moment about the yaw axis of the robot body on the set ZMP generated by the set motion ;
Movement calculating means for calculating upper limb movement for reducing the calculated moment about the yaw axis;
The calculated the issued on the basis of upper limb movement, a motion correction means for correcting a movement that is the set,
A robot apparatus comprising: The motion that is set by the motion setting means is any movement of the upper limbs or the trunk, or leg of at least the robot,
30. The robot apparatus according to claim 29.

Images