(57)【特許請求の範囲】
【請求項１】
基体から延設された複数の可動リンクを備えると共に、各可動リンクが、その先端部と前記基体との間に設けられた１つ以上の関節の作動により前記基体に対して動くように構成されており、１つ以上の可動リンクを外界物に接触させずに空中で動かす動作と該動作に続いて該可動リンクの所定の部位である接触対象部を外界物に接触させる動作とを含む運動によって移動する移動ロボットの制御装置であって、
前記移動ロボットの動作目標により規定される所定の状態量の目標値と該状態量の観測値との偏差をゼロに近づけるように該移動ロボットに付加すべき外力モーメントを表すモーメント操作量を前記偏差に応じて決定するモーメント操作量決定手段と、
前記複数の可動リンクのうち、前記外界物に接触させる１つ以上の可動リンクである接触状態可動リンクのそれぞれの接触対象部と前記外界物との接触点の、前記動作目標により規定される目標ＺＭＰから見た位置ベクトルを前記目標ＺＭＰの周りに回転させる回転量を表す回転操作量であって、全ての接触状態可動リンクのそれぞれの接触対象部に対して共通の回転操作量を前記モーメント操作量に応じて決定する回転操作量決定手段と、
前記接触状態可動リンクの各接触対象部を、前記動作目標における目標位置姿勢である修正前目標位置姿勢から前記回転操作量により示される回転量だけ目標ＺＭＰ周りに回転させた場合に実現される該接触対象部の並進変位量と該回転量とを該接触対象部の位置姿勢の修正量として、該修正量に応じて、前記接触状態可動リンクの各接触対象部の修正前目標位置姿勢を修正する接触対象部位置姿勢修正手段と、
該接触対象部位置姿勢修正手段による修正後の各接触対象部の目標位置姿勢を実現するように前記移動ロボットの各関節の目標変位量を決定して、該目標変位量に応じて各関節を駆動するアクチュエータを制御するアクチュエータ制御手段とを備えており、
前記回転操作量決定手段は、各接触対象部の姿勢の回転に伴い発生する弾性回転力と、各接触対象部の並進変位に伴い発生する弾性並進力とによって、前記目標ＺＭＰ周りに発生するモーメントが前記モーメント操作量に一致するように、各接触対象部の前記修正前目標位置姿勢と前記目標ＺＭＰとから決定される各接触対象部の接触点の前記位置ベクトルと、各接触対象部の姿勢の回転に関する該接触対象部の剛性度合のあらかじめ定められた設定値と、各接触対象部の並進変位に関する該接触対象部の剛性度合のあらかじめ定められた設定値とから決定した係数行列により前記モーメント操作量を線形変換することにより前記回転操作量を決定するように構成されていることを特徴とする移動ロボットの制御装置。
【請求項２】
請求項１記載の移動ロボットの制御装置において、
前記回転操作量決定手段は、係数行列を次式（Ａ）により決定するように構成されていることを特徴とする移動ロボットの制御装置。
【数１】
0006498597.tif 000008
ただし、
ｉ：各可動リンクの接触対象部の識別番号
Ｋτ_i：第ｉ接触対象部の回転変位に関する剛性度合の設定値
Ｋｆ_i：第ｉ接触対象部の並進変位に関する剛性度合の設定値
↑ｒc_i：目標ＺＭＰから見た第ｉ接触対象部の位置ベクトル
Ｉ3：単位行列
Ｋ_i：第ｉ接触対象部の接触状態で「１」、非接触状態で「０」、接触状態及び非接触状態の一方から他方への移行時に「１」及び「０」の一方の値から他方の値に所定の時間的変化率で徐々に変化するように設定される変数
【請求項３】
請求項１又は２記載の移動ロボットの制御装置において、
前記所定の状態量は、前記基体の姿勢と、前記目標ＺＭＰ周りに発生するモーメントとのうちの少なくともいずれか一方を含むことを特徴とする移動ロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、基体から延設された複数の可動リンクの運動によって移動する移動ロボットの制御装置に関する。
【背景技術】
【０００２】
従来、人型ロボット等の脚式移動ロボットの動作制御に関し、基体の姿勢の観測値を目標姿勢に近づけるように、各脚リンクの足部の目標位置姿勢（位置及び姿勢の目標値）を修正する技術が知られている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−５２９６８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
近年、人型ロボット等、複数の可動リンクが基体から延設された移動ロボットを平坦な床面上だけでなく、不整地等の種々様々な環境下で移動させ得る技術の開発又は研究が進められている。
【０００５】
種々様々な環境下で、移動ロボットを移動させる場合、環境によっては、３つ以上の可動リンクを地面等の外界物に適宜接触させつつ、移動ロボットを移動させることが必要となる場合が多々ある。例えば、人型の移動ロボットを、凹凸の多い不整地等で移動させる場合に、該移動ロボットの姿勢の安定性を確保するために、２つの脚リンクと２つの腕リンクとを併せた４つの可動リンクを外界物に適宜接触させながら、該移動ロボットの移動を行うことが必要となる場合が多い。
【０００６】
このように、３つ以上の可動リンクを地面等の外界物に接触させながら移動ロボットの移動を行う場合であっても、該移動ロボットの姿勢の安定化等のために、基体の姿勢等の状態量の目標値からのずれに応じて、外界物に接触させる可動リンクの部位（以降、接触対象部という）の目標位置姿勢を適宜修正することが望ましい。
【０００７】
特に、不整地等の環境下では、移動ロボットの動作目標の生成処理で想定されている地面の形状等の環境情報と、実際の環境とのずれが生じ易いために、外界物に接触させる可動リンクの接触対象部の目標位置姿勢を適宜修正することの必要性が高い。
【０００８】
この場合、基体の姿勢等の状態量を目標値に近づけ得る適切な外力モーメントを、移動ロボットに付加し得るように、各接触対象部の目標位置姿勢を修正する必要がある。また、各接触対象部の目標位置姿勢の修正は、地面等の外界物に接触させる可動リンクの個数に依存しない仕方で行うことが望ましい。
【０００９】
ここで、前記特許文献１には、２つの脚リンクの足部（接触対象部）を平坦な床面上に接地させる場合における該足部の目標位置姿勢の修正手法については記載されている。
【００１０】
しかるに、前記特許文献１には、腕リンク等を含めた３つ以上の可動リンクを外界物の種々様々な箇所に接触させる場合に、各接触対象部の目標位置姿勢を修正する手法については記載されていない。
【００１１】
そして、３つ以上の可動リンクを外界物に接触させる場合を含めて、複数個の可動リンクを外界物に接触させる場合についての各接触対象部の目標位置姿勢の修正処理が不適切であると、複数の接触対象部の相互の位置姿勢関係が、修正前の目標位置姿勢により規定される相互の位置姿勢関係から比較的大きくずれてしまう虞がある。
【００１２】
そして、このような場合には、移動ロボットの移動のために動かす複数の可動リンク同士の干渉が生じ易くなったり、あるいは、移動ロボットが、その姿勢の安定性を保ち得る余裕度が低い動作状態となってしまいやすい。ひいては、移動ロボットの継続的な移動が困難となる状況が発生しやすくなる。
【００１３】
本発明はかかる背景に鑑みてなされたものであり、外界物に接触させる複数の可動リンクのそれぞれの接触対象部の目標位置姿勢を、移動ロボットに所要の外力モーメントが付加されるように修正する場合に、外界物の形状あるいは接触対象部の個数等によらずに、各接触対象部の相互位置姿勢関係を崩さないように行うことを可能とする制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１４】
本発明の移動ロボットの制御装置は、かかる目的を達成するために、基体から延設された複数の可動リンクを備えると共に、各可動リンクが、その先端部と前記基体との間に設けられた１つ以上の関節の作動により前記基体に対して動くように構成されており、１つ以上の可動リンクを外界物に接触させずに空中で動かす動作と該動作に続いて該可動リンクの所定の部位である接触対象部を外界物に接触させる動作とを含む運動によって移動する移動ロボットの制御装置であって、
前記移動ロボットの動作目標により規定される所定の状態量の目標値と該状態量の観測値との偏差をゼロに近づけるように該移動ロボットに付加すべき外力モーメントを表すモーメント操作量を前記偏差に応じて決定するモーメント操作量決定手段と、
前記複数の可動リンクのうち、前記外界物に接触させる１つ以上の可動リンクである接触状態可動リンクのそれぞれの接触対象部と前記外界物との接触点の、前記動作目標により規定される目標ＺＭＰから見た位置ベクトルを前記目標ＺＭＰの周りに回転させる回転量を表す回転操作量であって、全ての接触状態可動リンクのそれぞれの接触対象部に対して共通の回転操作量を前記モーメント操作量に応じて決定する回転操作量決定手段と、
前記接触状態可動リンクの各接触対象部を、前記動作目標における目標位置姿勢である修正前目標位置姿勢から前記回転操作量により示される回転量だけ目標ＺＭＰ周りに回転させた場合に実現される該接触対象部の並進変位量と該回転量とを該接触対象部の位置姿勢の修正量として、該修正量に応じて、前記接触状態可動リンクの各接触対象部の修正前目標位置姿勢を修正する接触対象部位置姿勢修正手段と、
該接触対象部位置姿勢修正手段による修正後の各接触対象部の目標位置姿勢を実現するように前記移動ロボットの各関節の目標変位量を決定して、該目標変位量に応じて各関節を駆動するアクチュエータを制御するアクチュエータ制御手段とを備えており、
前記回転操作量決定手段は、各接触対象部の姿勢の回転に伴い発生する弾性回転力と、各接触対象部の並進変位に伴い発生する弾性並進力とによって、前記目標ＺＭＰ周りに発生するモーメントが前記モーメント操作量に一致するように、各接触対象部の前記修正前目標位置姿勢と前記目標ＺＭＰとから決定される各接触対象部の接触点の前記位置ベクトルと、各接触対象部の姿勢の回転に関する該接触対象部の剛性度合のあらかじめ定められた設定値と、各接触対象部の並進変位に関する該接触対象部の剛性度合のあらかじめ定められた設定値とから決定した係数行列により前記モーメント操作量を線形変換することにより前記回転操作量を決定するように構成されていることを特徴とする（第１発明）。
【００１５】
なお、本発明において、移動ロボットの「動作目標」は、該移動ロボットの全体の動作を規定する目標を意味する。また、接触対象部の「位置姿勢」は、該接触対象部の「位置」及び「姿勢」の組を意味し、「目標位置姿勢」は、「位置」の目標値（目標位置）と「姿勢」の目標値（目標姿勢）との組を意味する。
【００１６】
また、目標ＺＭＰは、所謂ＺＭＰ（Zero Moment Point）の目標位置を意味する。ＺＭＰは、移動ロボットの全体の運動によって発生する慣性力と、移動ロボットに作用する重力との合力が、その点周りに発生するモーメントの水平軸周り成分がゼロとなる点を意味する。
【００１７】
なお、目標ＺＭＰは、複数の接触対象部が種々様々な外界物に接触する状況において、後述の式（１ａ）〜（１ｃ）の関係式を満たすように決定され得る。
【００１８】
また、可動リンク又は接触対象部を外界物に「接触」させるということは、該可動リンク又は接触対象部を、接触反力を受け得るように外界物に接触させることを意味する。
【００１９】
また、接触対象部と外界物との「接触点」は、接触対象部と外界物との接触面における接触圧中心点を意味する。
【００２０】
また、前記状態量の「観測値」は、適宜のセンサによる該状態量の検出値、あるいは、該状態量と一定の相関関係を有する１つ以上の他の状態量の検出値から該相関関係に基づいて推定してなる推定値を意味する。
【００２１】
上記第１発明によれば、前記回転操作量決定手段は、前記係数行列により前記モーメント操作量を線形変換することにより前記回転操作量を決定する。
【００２２】
そして、前記接触対象部位置姿勢修正手段は、前記接触状態可動リンクの各接触対象部を、前記動作目標における目標位置姿勢である修正前目標位置姿勢から前記回転操作量により示される回転量だけ目標ＺＭＰ周りに回転させた場合に実現される該接触対象部の並進変位量と該回転量とを該接触対象部の位置姿勢の修正量として、該修正量に応じて、前記接触状態可動リンクの各接触対象部の修正前目標位置姿勢を修正する。
【００２３】
これにより、前記により規定される各接触対象部の相互の位置姿勢関係（相互の位置関係及び相互の姿勢関係）を維持したままで、前記モーメント操作量により示される外力モーメントを移動ロボットに付加させ得るように、接触状態可動リンクの各接触対象部の修正前目標位置姿勢を修正することが可能となる。すなわち、各接触対象部の修正前目標位置姿勢により規定される各接触対象部の相互の位置姿勢関係を、修正前の相互の位置姿勢関係に一致させることが可能となる。
【００２４】
また、前記係数行列は、前記回転操作量決定手段の処理により、前記の如く決定されるので、前記接触状態可動リンクの接触対象部のトータルの個数に合わせて決定されることとなる。また、接触状態可動リンクのそれぞれの接触対象部が接触する外界物が該接触対象部を接触させ得るものである限り、該外界物の形状等によらずに、前記回転操作量決定手段の処理により、前記係数行列を決定できる。
【００２５】
従って、第１発明によれば、外界物に接触させる複数の可動リンク（接触状態可動リンク）のそれぞれの接触対象部の目標位置姿勢を、移動ロボットに所要の外力モーメント（モーメント操作量により示される外力モーメント）が付加されるように修正する場合に、外界物の形状あるいは接触対象部の個数等によらずに、各接触対象部の相互位置姿勢関係を崩さないように行うことが可能となる。ひいては、移動ロボットの継続的な移動が困難となる状況が発生するのを抑制できる。
【００２６】
上記第１発明では、前記回転操作量決定手段は、係数行列を次式（Ａ）により決定するように構成されていることが好ましい（第２発明）。
【００２７】
【数１】
0006498597.tif 000002
【００２８】
ただし、
ｉ：各可動リンクの接触対象部の識別番号
Ｋτ_i：第ｉ接触対象部の回転変位に関する剛性度合の設定値
Ｋｆ_i：第ｉ接触対象部の並進変位に関する剛性度合の設定値
↑ｒc_i：目標ＺＭＰから見た第ｉ接触対象部の位置ベクトル（縦ベクトル）
Ｉ3：単位行列
Ｋ_i：第ｉ接触対象部の接触状態で「１」、非接触状態で「０」、接触状態及び非接触状態の一方から他方への移行時に「１」及び「０」の一方の値から他方の値に所定の時間的変化率で徐々に変化するように設定される変数
上記第２発明によれば、前記モーメント操作量により示される外力モーメントを、前記回転操作量に変換する係数行列を好適に決定できる。この場合、各可動リンクの接触対象部が接触状態（外界物に接触した状態）及び非接触状態（外界物に接触していない状態）の一方から他方に切替わる際に、前記回転操作量が不連続的に変化するのを確実に防止できる。その結果、各接触対象部の目標位置姿勢の修正を滑らかに行うことができる。ひいては、移動ロボットの移動動作を滑らかに行うことができる。
【００２９】
上記第１発明又は第２発明では、前記所定の状態量は、前記基体の姿勢と、前記目標ＺＭＰ周りに発生するモーメントとのうちの少なくともいずれか一方を含むことが好ましい（第３発明）。
【００３０】
これによれば、移動ロボットの全体の姿勢の安定性を高めることができる。
【図面の簡単な説明】
【００３１】
【図１】本発明の実施形態における移動ロボットを示す図。
【図２】実施形態の移動ロボットの動作制御に関する構成を示すブロック図。
【図３】図２に示す動作目標修正部の処理を示すブロック線図。
【図４】図４Ａは動作目標の修正前の移動ロボットの動作状態の一例を示す図、図４Ｂは動作目標の修正後の移動ロボットの動作状態の一例を示す図。
【発明を実施するための形態】
【００３２】
本発明の一実施形態を図１〜図４を参照して以下に説明する。
【００３３】
図１を参照して、本実施形態の移動ロボット１は、一例として、人型のロボットである。この移動ロボット１（以降、単にロボット１ということがある）は、上体に相当する基体２と、基体２から延設された複数の可動リンクとしての左右一対の（２つの）脚リンク３Ｌ，３Ｒ及び左右一対の（２つの）腕リンク４Ｌ，４Ｒと、頭部５とを備える。
【００３４】
なお、本実施形態の説明では、参照符号に付加する「Ｒ」，「Ｌ」は、それぞれロボット１の基体２の正面に向かって右側の要素、左側の要素を意味する。ただし、左右を特に区別する必要が無いときは、符号「Ｒ」，「Ｌ」を省略する。
【００３５】
また、以降の説明では、脚リンク３Ｌ，３Ｒ及び腕リンク４Ｌ，４Ｒのそれぞれを単に可動リンクということがある。
【００３６】
各脚リンク３は、基体２の下部から延設されている。各脚リンク３は、大腿部１１、下腿部１２、足部１３にそれぞれ相当する要素リンクを、基体２側から順番に、股関節１４、膝関節１５、足首関節１６を介して連結して構成されている。各脚リンク３の先端部たる足部１３は、ロボット１の移動時等に外界物（地面、床面、壁面等）に適宜接触させる接触対象部となる部位である。
【００３７】
各脚リンク３の足部１３と基体２との間の関節１４，１５，１６は、本実施形態では、該脚リンク３の足部１３が、基体２に対して例えば６自由度の運動自由度を有するように構成されている。
【００３８】
例えば、股関節１４は、総計３軸の回転自由度を有するように３つの要素関節（図示省略）により構成される。膝関節１５は、1軸の回転自由度を有するように単一の要素関節（図示省略）により構成される。足首関節１６は、総計２軸の回転自由度を有するように２つの要素関節（図示省略）により構成される。なお、要素関節は、１軸の回転自由度を有する関節である。該要素関節は、１軸周りに相対回転自在に係合された２つの部材により構成される。
【００３９】
各腕リンク４は、基体２の上部から延設されている。各腕リンク４は、上腕部２１、前腕部２２、ハンド部２３にそれぞれ相当する要素リンクを、基体２側から順番に、肩関節２４、肘関節２５、手首関節２６を介して連結して構成されている。
【００４０】
本実施形態では、各腕リンク４は、脚リンク３，３以外で、必要に応じて外界物に接触させる脚リンクとして機能し得る可動リンクである。この場合、各腕リンク４の先端部たるハンド部２３が、ロボット１の移動時の動作形態等に応じて、外界物（地面、床面、壁面等）に適宜接触させる接触対象部となる部位である。
【００４１】
各腕リンク４のハンド部２３と基体２との間の関節２４，２５，２６は、本実施形態では、各腕リンク４のハンド部２３が、基体２に対して例えば６自由度を有するように構成されている。
【００４２】
例えば、肩関節２４は、総計３軸の回転自由度を有するように３つの要素関節（図示省略）により構成される。肘関節２５は、1軸の回転自由度を有するように単一の要素関節（図示省略）により構成される。手首関節２６は、総計２軸の回転自由度を有するように２つの要素関節（図示省略）により構成される。
【００４３】
また、各腕リンク４のハンド部２３は、本実施形態では、物体の把持を行うことができるように構成されている。例えば、各ハンド部２３は、適宜のクランプ機構、あるいは、人の手指と同様の動作を行い得る複数の指機構等により構成される。
【００４４】
頭部５は、基体２の上端部に首関節３１を介して取り付けられている。首関節３１は、１軸、２軸、又は３軸の回転自由度を有するように構成される。
【００４５】
以上がロボット１の機構的な構成の概要である。
【００４６】
上記の如く構成されたロボット１は、脚リンク３及び腕リンク４のうちの１つ以上の可動リンク３又は４を外界物に接触させずに空中で動かす動作と該動作に続いて該可動リンク３又は４を外界物に接触させる動作とを含む運動によって移動することができる。
【００４７】
例えば、人の歩行動作における脚の運動と同様の運動パターン（２脚歩容の運動パターン）で２つの脚リンク３，３を動かすことで、ロボット１の歩行動作を行うことができる。
【００４８】
この場合、腕リンク４を壁、手すり等に適宜接触させながら、脚リンク３，３を動かすことで、ロボット１の移動を行うこともできる。
【００４９】
また、例えば、４つの可動リンク３，３，４，４を脚として使用して、該４つの可動リンク３，３，４，４を、トロット、クロール、もしくはペース等の運動パターン（４脚歩容の運動パターン）で動かすことで、ロボット１の移動を行うこともできる。
【００５０】
さらに、例えば４つの可動リンク３，３，４，４を使用して、梯子等の昇降を行うこともできる。
【００５１】
補足すると、ロボット１の各脚リンク３あるいは各腕リンク４は、７自由度以上の運動自由度を有するように構成されていてもよい。また、各脚リンク３及び各腕リンク４のそれぞれは、回転型の関節に限らず、直動型の関節を含んでいてもよい。
【００５２】
また、ロボット１がハンド部２３により把持動作を必要としないロボットである場合には、各腕リンク４の先端部は、把持動作のための機構を備えない構造のものであってもよい。また、ロボット１は、頭部５を備えないものであってもよい。
【００５３】
また、基体２は関節を含んでいてもよい。例えば、基体２は、脚リンク３，３が連結された上側基体と、腕リンク４，４が連結された下側基体と、これらの上側基体及び下側基体を相対変位可能に連結する関節とにより構成されたものであってもよい。
【００５４】
次に、ロボット１の動作制御に関する構成を説明する。なお、以降の説明では、各脚リンク３の足部１３及び各腕リンク４のハンド部２３を総称的に接触対象部（１３又は２３）ということがある。
【００５５】
図２に示すように、ロボット１には、該ロボット１の動作制御を行う制御装置４０と、各関節（要素関節）をそれぞれ駆動する関節アクチュエータ４１と、所要の各種センサとが搭載されている。なお、図示は省略するが、本実施形態では、各ハンド部２３の把持動作を行わせるためのアクチュエータもロボット１に搭載されている。
【００５６】
センサとしては、ロボット１の基体２の姿勢を検出するための姿勢センサ４２と、ロボット１の各関節（要素関節）の変位量（回転角）を検出するための関節変位センサ４３と、ロボット１の視覚センサとしてのカメラ４４と、各脚リンク３の足部１３が接触対象の外界物から受ける外力（並進力及びモーメント）を検出するための力センサ４５と、各腕リンク４のハンド部２３が接触対象の外界物から受ける外力（並進力及びモーメント）を検出するための力センサ４６とが搭載されている。
【００５７】
姿勢センサ４２は、例えばストラップダウン方式で基体２の姿勢（空間的な向き）を検出し得るように基体２に搭載されたセンサである。該姿勢センサ４２は、３軸の角速度を検出するジャイロセンサと３軸の並進加速度を検出する加速度センサとから構成される。
【００５８】
関節変位センサ４３は、要素関節毎に備えられている。各関節変位センサ４３は、ロータリエンコーダ、ポテンショメータ等の回転角センサにより構成される。
【００５９】
カメラ４４は、例えばステレオカメラにより構成される。該カメラ４４は、例えば図１に示す如く頭部５に搭載されている。なお、カメラ４４の代わりに、又はカメラ４４に加えて、走査型のレーザ式測距センサ等の測距センサ（例えば、レーザ・レンジ・ファインダ）がロボット１に搭載されていてもよい。また、カメラ４４は、基体２に搭載されていてもよい。
【００６０】
力センサ４５は、各脚リンク３毎に備えられている。各力センサ４５は、例えば図１に示す如く各脚リンク３の足首関節１６と足部１３との間に介装された６軸力センサにより構成される。
【００６１】
また、力センサ４６は、各腕リンク４毎に備えられている。各力センサ４６は、図１に示す如く各腕リンク４の手首関節２６とハンド部２３との間に介装された６軸力センサにより構成される。
【００６２】
関節アクチュエータ４１は、要素関節毎に備えられている。各関節アクチュエータ４１は、電動モータあるいは油圧アクチュエータにより構成される。
【００６３】
制御装置４０は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットにより構成される。そして、該制御装置４０には、上記の各センサの検出信号が入力される。なお、制御装置４０は、相互に通信可能な複数の電子回路ユニットにより構成されていてもよい。
【００６４】
この制御装置４０は、実装されるプログラムを実行することにより実現される機能、又は、ハードウェア構成により実現される機能として、ロボット１の各関節の作動を規定する基準の動作目標を生成する動作目標生成部５１と、ロボット１の実際の動作状態等に応じて基準の動作目標を適宜修正する動作目標修正部５２と、修正後の動作目標に応じて各関節アクチュエータ４１を制御する関節アクチュエータ制御部５３とを備える。
【００６５】
そして、動作目標修正部５２は、図３に示すように、ロボット１の基体２の姿勢等の状態量を調整するためにロボット１に付加すべき外力モーメントであるモーメント操作量↑τ_dmdを決定するモーメント操作量決定部６１と、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢を、基準の動作目標（修正前の動作目標）における目標位置姿勢から修正するために使用する回転操作量↑Δθを決定する回転操作量決定部６５と、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢を回転操作量↑Δθを用いて修正する接触対象部位置姿勢修正部６６とを備える。
【００６６】
上記関節アクチュエータ制御部５３、モーメント操作量決定部６１、回転操作量決定部６５、及び接触対象部位置姿勢修正部６６は、それぞれ、本発明におけるアクチュエータ制御手段、モーメント操作量決定手段、回転操作量決定手段、及び接触対象部位置姿勢修正手段に相当する機能部である。
【００６７】
ここで、本明細書での用語に関して補足すると、ロボット１の各接触対象部等、任意の部位の「位置姿勢」は、該部位の「位置」及び「姿勢」の組を意味する。そして、任意の部位の「目標位置姿勢」は、該部位の位置の目標値（目標位置）と該部位の姿勢の目標値（目標姿勢）との組を意味する。
【００６８】
この場合、任意の部位の「位置」は、該部位に対して任意に設計的に設定された該部位の代表点の空間的な位置を意味し、該部位の「姿勢」は該部位の空間的な向きを意味する。
【００６９】
また、↑τ_dmd等、「↑」を先頭に付した参照符号は、ベクトルを表す参照符号である。この場合、本明細書では、ベクトルは、複数の成分を並べた縦ベクトルである。
【００７０】
次に、制御装置４０の処理をより具体的に説明する。なお、以降の説明では、脚リンク３Ｒ，３Ｌ、及び腕リンク４Ｒ，４Ｌをそれぞれ順番に第１可動リンク３Ｒ、第２可動リンク３Ｌ、第３可動リンク４Ｒ、第４可動リンク４Ｌと称することがある。また、第１〜第４可動リンク３Ｒ，３Ｌ，４Ｒ，４Ｌのうちの任意の１つを第ｉ可動リンクと称することがある。
【００７１】
制御装置４０は、カメラ４４等を介して取得した外界情報と、外部のサーバ等から与えられ、もしくはあらかじめティーチングされた動作指令とを基に、動作目標生成部５１によりロボット１の基準の動作目標を逐次生成する。
【００７２】
この基準の動作目標は、取得した外界情報から認識される環境下で、上記動作指令に即した所要の運動をロボット１に行わせるための基本指針の動作目標である。該動作目標は、ＺＭＰ（Zero Moment Point）等に関する動力学的な制約条件をみたしつつ、ロボット１の各関節の作動（換言すれば、ロボット１の全体の運動）を規定し得るように適宜の動力学モデルを用いて生成される。その生成手法は公知の手法でよい。
【００７３】
本実施形態では、上記基準の動作目標は、各脚リンク３の足部１３及び各腕リンク４のハンド部２３のそれぞれの（すなわち、各可動リンク３，４のそれぞれの接触対象部の）基準の目標位置姿勢の軌道と、基体２の基準の目標位置姿勢の軌道と、頭部５の基準の目標姿勢の軌道とを含む。なお、「軌道」は、瞬時値（所定の制御処理周期毎の瞬時値）の時系列を意味する。
【００７４】
この場合、本実施形態では、各足部１３、各ハンド部２３及び基体２の目標位置姿勢は、ロボット１の外界に対して設計的に設定されるグローバル座標系で見た位置及び姿勢の目標値として表現される。また、頭部５の目標姿勢は、基体２に対して設計的に設定されるローカル座標系で見た頭部５の相対的な姿勢の目標値として表現される。
【００７５】
上記グローバル座標系としては、例えば図１に示すように、鉛直方向をＺ軸、互いに直交する２つの水平軸をＸ軸、Ｙ軸とする３軸直交座標系が用いられる。以降、特にことわらない限り、Ｘ軸、Ｙ軸、Ｚ軸は、当該グローバル座標系の各座標軸を意味する。
【００７６】
また、動作目標生成部５１が生成する基準の動作目標には、さらに、ＺＭＰの目標位置である目標ＺＭＰの軌道も含まれる。
【００７７】
ＺＭＰは、詳しくは、ロボット１の運動によって発生する慣性力とロボット１に作用する重力との合力が、その点周りに発生するモーメントの水平軸周り方向（Ｘ軸周り方向及びＹ軸周り方向）の成分がゼロとなる点である。
【００７８】
ここで、ロボット１が単一平面状の床面を移動する場合には、ＺＭＰは該床面上の点となるものの、ロボット１の複数の接触対象部（足部１３又はハンド部２３）のそれぞれの外界物との接触面が単一の平面上に無い場合（例えば図４Ａに例示する場合）には、ＺＭＰは、各接触対象部の接触面を含む平面からずれた点となる。
【００７９】
かかるＺＭＰの目標位置である目標ＺＭＰは、本実施形態では、次式（１ａ），（１ｂ），（１ｃ）により算出される。
【００８０】
【数２】
0006498597.tif 000003
【００８１】
Ｐzmp[Z]、Ｐzmp[X]、Ｐzmp[Y]はそれぞれ目標ＺＭＰのＺ軸方向位置、Ｘ軸方向位置、Ｙ軸方向位置である。また、Ｎは可動リンクの総数（本実施形態ではＮ＝４）、Ｐcop_i[Z]は、第ｉ可動リンクの接触対象部（足部１３又はハンド部２３）の外界物に対する接触点（詳しくは、該接触対象部と外界物との接触面における接地圧中心点）の目標位置のうちのＺ軸方向位置、Ｘ軸方向位置、Ｙ軸方向位置、Ｆ_i[Z]は、第ｉ可動リンクの接触対象部に外界物から作用する接触反力（並進力）のＺ軸方向成分の目標値である。
【００８２】
また、Ｐg[Z]、Ｐg[X]、Ｐg[Y]はそれぞれ、ロボット１の全体重心の目標位置のうちのＺ軸方向位置、Ｘ軸方向位置、Ｙ軸方向位置である。また、Ｍa[X]、Ｍa[Y]はそれぞれ、ロボット１全体の目標運動（基準の動作目標により規定される目標運動）によって発生する慣性力とロボット１に作用する重力との合力がロボット１の全体重心周りに発生するモーメントのＸ軸周り方向成分、Ｙ軸周り方向成分、Ｆa[X]、Ｆa[Y]、Ｆa[Z]はそれぞれ、当該合力のうちの並進力のＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分である。
【００８３】
なお、ロボット１の目標運動において、第ｉ可動リンクを外界物に接触させない状態では、上記式（１ａ）におけるF_i[Z]はゼロ（ひいては、Ｐcop_i[Z]・F_i[Z]＝０）である。また、Ｆa[Z]の極性を反転させてなる値（＝−Ｆa[Z]）は、式（１ａ）の分母の値に一致する。
【００８４】
上記式（１ａ）により、目標ＺＭＰのＺ軸方向位置（鉛直方向位置）は、外界物に接触させる全ての接触対象部（足部１３又はハンド部２３）のそれぞれの接触点のＺ軸方向位置を加重平均してなる位置として算出される。この場合、各接触対象部の接触点のＺ軸方向位置に係る重み係数は、該接触対象部に作用させる接触反力（並進力）のＺ軸方向成分Ｆ_i[Z]の分担割合（ロボット１に作用するトータルの接触反力（並進力）のＺ軸方向成分（＝式（１ａ）の分母の値）に対する割合）である。
【００８５】
そして、式（１ｂ），（１ｃ）によりそれぞれ算出される目標ＺＭＰのＸ軸方向位置及びＹ軸方向位置は、上記合力が、式（１ａ）により算出されるＺ軸方向の位置Ｐzmp[Z]において発生するＸ軸周り方向のモーメント成分とＹ軸周り方向のモーメント成分とがゼロとなる点のＸ軸方向位置及びＹ軸方向位置として算出される。
【００８６】
上記の如く算出される目標ＺＭＰ（基準の動作目標により規定される目標ＺＭＰ）は、ロボット１が単一平面状の床面を移動する場合には、該床面上の点となる。一方、ロボット１の複数の接触対象部（足部１３又はハンド部２３）のそれぞれの外界物との接触面が単一の平面上に無い場合には、例えば図４に例示する如く、目標ＺＭＰは、各接触対象部の接触面を含む平面からずれた点となる。
【００８７】
ここで、図４Ａに示す例は、基準の動作目標において、ロボット１の脚リンク３Ｒ，３Ｌのそれぞれの足部１３Ｒ，１３Ｌ、並びに腕リンク４Ｒのハンド部２３Ｒ（換言すれば、第１〜第３可動リンク３Ｒ，３Ｌ，４Ｒのそれぞれの接触対象部（１３Ｒ，１３Ｌ，２３Ｒ）を、破線Ｓａで示す段差形状の床面（外界物）に接触（接地）させた状態のロボット１を例示している。なお、図４Ａでは、ロボット１の頭部５と左側の腕リンク４Ｌの図示を省略している。
【００８８】
上記の如く目標ＺＭＰを算出する処理は、ロボット１が床面上を移動する場合に限らず、ロボット１が腕リンク４を壁等に接触させて移動する場合、ガレキ等がある不整地を移動する場合、あるいは、梯子を昇降する場合等でも同じである。
【００８９】
制御装置４０は、以上の如く動作目標生成部５１によりロボット１の基準の動作目標を生成する。なお、かかる基準の動作目標の生成処理は、外部のサーバ等で実行してもよい。その場合には、制御装置４０は、該サーバ等で生成された動作目標を、随時、無線通信等により取得すればよい。
【００９０】
制御装置４０は、上記基準の動作目標を適宜修正する処理を、動作目標修正部５２により所定の制御処理周期で逐次実行する。この修正処理は、制御装置４０がカメラ４４等を介して取得した外界情報により示される外界状態（床面形状等）と、実際の外界状態とのずれ、あるいは、基準の動作目標の生成のために使用した動力学モデルのモデル化誤差等に起因して、ロボット１の所定の状態量の実際の値が、基準の動作目標により規定される目標値からずれるのを極力防止するための処理である。
【００９１】
本実施形態では、上記所定の状態量として、例えば、基体２の姿勢（詳しくは、Ｘ軸周り方向及びＹ軸周りの方向の傾斜姿勢）と、目標ＺＭＰ周りに発生するモーメント（詳しくは、Ｘ軸周り方向及びＹ軸周り方向のモーメント）との２種類の状態量を用いられる。
【００９２】
そして、動作目標修正部５２は、ロボット１の基体２の実際の姿勢と、目標ＺＭＰ周りに実際に発生するモーメントとが、それぞれの目標値からずれるのを極力抑制するように、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢を修正する。
【００９３】
かかる動作目標修正部５２の処理を図３を参照して以下に詳説する。
【００９４】
動作目標修正部５２は、各制御処理周期において、図３のブロック図で示す処理を実行する。具体的には、動作目標修正部５２は、まず、基体２の実際の姿勢と目標ＺＭＰ周りの実際のモーメントとをそれぞれの目標値に近づけるためのモーメント操作量↑τ_dmdをモーメント操作量決定部６１により決定する。
【００９５】
該モーメント操作量決定部６１は、基体２の実際の姿勢を基準の動作目標における目標姿勢に近づけるためにロボット１に付加すべき目標ＺＭＰ周りの外力モーメントの操作量成分である第１操作量成分↑τ１_dmdと、目標ＺＭＰ周りの実際のモーメントを目標値に近づけるためにロボット１に付加すべき目標ＺＭＰ周りの外力モーメントの操作量成分である第２操作量成分↑τ２_dmdとをそれぞれ、姿勢安定化操作量決定部６２とモーメント誤差補償操作量決定部６３とにより決定する。第１操作量成分↑τ１_dmd及び第２操作量成分↑τ２_dmdは、それぞれ、目標ＺＭＰ周りに作用する外力モーメント（外界物からロボットに作用する接触反力によるモーメント）の増減量の要求値を表すものである。
【００９６】
そして、モーメント操作量決定部６１は、これらの第１操作量成分↑τ１_dmdと、第２操作量成分↑τ２_dmdとを演算部６４で合成する（加え合わせる）ことで、モーメント操作量↑τ_dmdを決定する。
【００９７】
上記姿勢安定化操作量決定部６２及びとモーメント誤差補償操作量決定部６３の処理は、次のように実行される。
【００９８】
姿勢安定化操作量決定部６２は、基準の動作目標における基体２の目標姿勢と、前記姿勢センサ４２の出力に基づき制御装置４０が推定した基体２の実際の姿勢の観測値とを逐次取得する。そして、姿勢安定化操作量決定部６２は、基体２の目標姿勢と姿勢の観測値との偏差から、フィードバック制御則により該偏差をゼロに近づけるように第１操作量成分↑τ１_dmdを算出する。この場合のフィードバック制御則としては、本実施形態では、例えばＰＤ則（比例・微分則）が使用される。
【００９９】
具体的には、↑τ１_dmdのＸ軸周り方向の成分τ１_dmd[X]と、Ｙ軸周り方向の成分τ１_dmd[Y]とが、それぞれ、ＰＤ則に基づいて、次式（２ａ），（２ｂ）により算出される。
【０１００】
τ１_dmd[X]＝Ｋpx・Ｄθ[X]＋Ｋvx・ｄＤθ[X]    ……（２ａ）
τ１_dmd[Y]＝Ｋpy・Ｄθ[Y]＋Ｋvy・ｄＤθ[Y]    ……（２ｂ）
ここで、Ｄθ[X]は、Ｘ軸周り方向における基体２の目標姿勢と実際の姿勢の観測値との傾斜角偏差、ｄＤθ[X]は、傾斜角偏差Ｄθ[X]の時間的変化率（微分値）、Ｄθ[Y]は、Ｙ軸周り方向における基体２の目標姿勢と実際の姿勢の観測値との傾斜角偏差、ｄＤθ[Y]は、傾斜角偏差Ｄθ[Y]の時間的変化率（微分値）、Ｋpx，Ｋvx，Ｋpy，Ｋvyは、それぞれ所定値のゲインである。
【０１０１】
また、本実施形態では、↑τ１_dmdのＺ軸周り方向の成分τ１_dmd[Z]は、定常的にゼロに設定される。
【０１０２】
なお、↑τ１_dmdを算出するためのフィードバック制御則として、ＰＤ則以外のフィードバック制御則を使用することも可能である。
【０１０３】
モーメント誤差補償操作量決定部６３は、外界物に接触させる各接触対象物（１３又は２３）に対応する力センサ４５又は４６の出力により示される接触反力（並進力及びモーメント）の観測値に基づいて、目標ＺＭＰ周りに発生する実際のモーメントの観測値を取得する。
【０１０４】
この場合、目標ＺＭＰ周りのモーメントのうちのＸ軸周り方向の成分とＹ軸周り方向の成分とのそれぞれの目標値はゼロであるから、該モーメントのＸ軸周り方向の成分及びＹ軸周り方向の成分のそれぞれの観測値が、そのまま目標値との偏差（目標値に対する誤差）を示すものとなる。
【０１０５】
そこで、モーメント誤差補償操作量決定部６３は、目標ＺＭＰ周りの実際のモーメントのうちのＸ軸周り方向の成分τzmp_act[X]及びＹ軸周り方向の成分τzmp_act[Y]のそれぞれの観測値の極性を反転させた値を、それぞれ、第２操作量成分↑τ２_dmdのＸ軸周り方向の成分τ２_dmd[X]（＝−τzmp_act[X]）及びＹ軸周り方向の成分、τ２_dmd[Y]（＝−τzmp_act[Y]）として設定する。
【０１０６】
なお、−τzmp_act[X]、−τzmp_act[Y]のそれぞれに１以下の所定値のゲインを乗じてなる値を、τ２_dmd[X]、τ２_dmd[Y]として求めるようにしてもよい。
【０１０７】
動作目標修正部５２は、次に、上記の如く決定したモーメント操作量↑τ_dmdをロボット１に付加するように、各接触対象部（１３又は２３）の目標位置姿勢（基準の動作目標における目標位置姿勢）を修正するための回転操作量↑Δθを回転操作量決定部６５により決定する。
【０１０８】
ここで、モーメント操作量↑τ_dmdをロボット１に付加するように各接触対象部（１３又は１４）の目標位置姿勢を修正する処理は、本実施形態では、基準の動作目標において外界物に接触させる全ての接触対象部の接触点（接地圧中心点）の位置を、基準の動作目標における目標位置（修正前の位置）からそれぞれの接触対象部について共通の回転量だけ目標ＺＭＰ周りに回転させた位置に修正するように行われる。この場合の当該共通の回転量が上記回転操作量↑Δθである。なお、外界物に接触させる接触対象部（１３又は２３）は、本発明における接触状態可動リンクの接触対象部に相当する。
【０１０９】
かかる回転操作量↑Δθは、以下に説明する如く構築された後述の式（８）により算出される。なお、以降の説明では、第ｉ可動リンク３又は４の接触対象部（１３又は２３）を第ｉ接触対象部という。また、任意のベクトル（縦ベクトル）↑ａ，↑ｂに対して、↑ａ×↑ｂはベクトル積（外積）を意味し、↑ａ・↑ｂは内積を意味する。また、↑ａTは、↑ａの転置ベクトル（横ベクトル）を意味する。
【０１１０】
モーメント操作量↑τ_dmdを目標ＺＭＰ周りでロボット１に付加するために第ｉ接触対象部に付加的に作用させる接触反力のうちのモーメント及び並進力をそれぞれ↑τ_i、↑ｆ_i、該第ｉ接触対象部の接触点（接地圧中心点）を目標ＺＭＰから見た位置ベクトルを↑ｒc_iと表記すると、これらの↑τ_i、↑ｆ_i、↑ｒc_iと、↑τ_dmdとの関係は次式（３）により表すことができる。
【０１１１】
【数３】
0006498597.tif 000004
【０１１２】
ここで、Ｎは可動リンク３，４の総数（本実施形態ではＮ＝４）、Ｋ_iは第ｉ接触対象部が外界物に接触している状態（接触状態）で「１」、外界物に接触していない状態（非接触状態）で「０」となる変数である。そして、本実施形態では、変数Ｋ_iの値は、第ｉ接触対象部が接触状態及び非接触状態の一方から他方に切替わるときには、「１」及び「０」の一方の値から他方の値に所定の時間的変化率で（又は所定の経時変化パターンで）徐々に変化するように設定される。
【０１１３】
なお、単一の可動リンク３又は４の接触対象部（１３又は１４）だけが外界物に接触している状態では、その単一の接触対象部（１３又は１４）に対応する接触点と目標ＺＭＰは一致する。従って、当該単一の接触対象部（１３又は１４）に対応する↑ｒc_iはゼロとなる。
【０１１４】
さらに、第ｉ接触対象部の回転変位（目標ＺＭＰ周りの回転力による弾性変形）に関する剛性度合をＫτ_i、並進変位（並進力による弾性変形）に関する剛性度合をＫｆ_iと表記し、第ｉ接触対象部を↑Δθの回転量だけ目標ＺＭＰ周りに回転させた場合の該第ｉ接触対象部の接触点の並進変位量を↑ΔＰc_iと表記すると、↑τ_iと↑Δθとの間の関係、↑ｆ_iと↑ΔＰc_iとの間の関係、及び↑ΔＰc_iと↑Δθとの間の関係は、それぞれ次式（４ａ）、（４ｂ）、（４ｃ）により表される。
【０１１５】
↑Δθ＝↑τ_i／Ｋτ_i    ……（４ａ）
↑ΔＰc_i＝↑ｆ_i／Ｋf_i    ……（４ｂ）
↑ΔＰc_i＝↑Δθ×↑ｒc_i    ……（４ｃ）
なお、上記剛性度合Ｋτ_iは、より詳しくは第ｉ接触対象部の各座標軸（Ｘ軸、Ｙ軸、Ｚ軸）周り方向の単位回転量当たりに弾性的に発生するモーメント↑τ_iの各成分の変化量を表すものである。同様に、上記剛性度合Ｋf_iは、より詳しくは第ｉ接触対象部の各座標軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の単位変位量当たりに弾性的に発生する並進力↑ｆ_iの各成分の変化量を表すものである。また、↑τ_i、↑ｆ_iは、それぞれ本発明における弾性回転力、弾性並進力に相当するものである。
【０１１６】
補足すると、Ｋτ_iは、各座標軸周り方向の回転変位に関する剛性度合の設定値を対角成分とする対角行列であってもよい。同様に、Ｋｆ_iは、各座標軸方向の並進変位に関する剛性度合の設定値を対角成分とする対角行列であってもよい。
【０１１７】
前記式（３）に式（４ａ）〜（４ｃ）を適用すると、次式（５）が得られる。
【０１１８】
【数４】
0006498597.tif 000005
【０１１９】
また、ベクトル積（外積）に関する公式によって、次式（６）が成立する。
【０１２０】
↑ｒc_i×(↑Δθ×↑ｒc_i)
＝(↑ｒc_i・↑ｒc_i)・↑Δθ−(↑ｒc_i・↑Δθ)・↑ｒc_i
＝(↑ｒc_iT・↑ｒc_i・Ｉ3−↑ｒc_i・↑ｒc_iT)・↑Δθ    ……（６）
なお、Ｉ3は、３次の単位行列である。また、上付きの添え字Ｔは転置を意味する。
【０１２１】
この式（６）を式（５）に適用することで、次式（７）が得られる。
【０１２２】
【数５】
0006498597.tif 000006
【０１２３】
従って、↑Δθは、次式（８）の如く、ただし書きの式（９）で定義される行列Ａ（３行３列の行列）の逆行列Ａ-1を↑τ_dmdに乗じる（↑τ_dmdを逆行列Ａ-1により線形変換する）ことによって算出することができる。
【０１２４】
【数６】
0006498597.tif 000007
【０１２５】
そこで、回転操作量決定部６５は、モーメント操作量↑τ_dmdから、上記式（８）により回転操作量↑Δθを算出する。この場合、↑τ_dmdに乗じる逆行列Ａ-1を決定するために必要となる変数Ｋ_iの値は、第ｉ接触対象部が、外界物に接触する状態であるか、接触しない状態状態であるか、あるいは、これらの状態の移行時であるかに応じて前記した如く「１」、あるいは「０」、「１」と「０」との一方から他方の値に経時変化する値に設定される。
【０１２６】
また、剛性度合Ｋτ_i、Ｋｆ_iの値は、各可動リンク３又は４の接触対象部（１３又は２３）のそれぞれ毎にあらかじめ設定された設定値である。
【０１２７】
また、↑ｒc_iは、基準の動作目標における目標ＺＭＰ（↑Ｐzmp）と、第ｉ接触対象部の目標位置姿勢（修正前の目標位置姿勢）により規定される第ｉ接触対象部の接触点（接地圧中心点）の目標位置とから算出される。
【０１２８】
なお、本実施形態では、↑τ_dmdのＺ軸回り方向の成分はゼロであるので、↑ΔθのＺ軸周り方向の成分もゼロである。
【０１２９】
補足すると、上記逆行列Ａ-1は、本発明における係数行列に相当するものである。そして、上記式（９）は、前記式（Ａ）と同じ式である。
【０１３０】
以上のように回転操作量↑Δθを算出することによって、↑τ_dmdを実現し得るように各接触対象部を目標ＺＭＰの周りに回転させる回転量としての回転操作量↑Δθを決定することができる。
【０１３１】
動作目標修正部５２は、次に、接触対象部位置姿勢修正部６６により、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢を、回転操作量↑Δθに応じて修正する処理を実行する。
【０１３２】
この場合、接触対象部位置姿勢修正部６６は、外界物に接触させる各接触対象部（１３又は２３）のそれぞれに対して、回転操作量↑Δθを該接触対象部（１３又は２３）のそれぞれの姿勢の修正量として設定する。
【０１３３】
また、接触対象部位置姿勢修正部６６は、外界物に接触させる各接触対象部（１３又は２３）のそれぞれに対して、該接触対象部（１３又は２３）を、基準の動作目標における目標位置（修正前の目標位置）から目標ＺＭＰ周りに↑Δθの回転量だけ回転させた場合の並進変位量↑ΔＰ_iを、該接触対象部（１３又は２３）の目標位置の修正量として設定する。
【０１３４】
この場合、外界物に接触させる各接触対象部（１３又は２３）の並進変位量↑ΔＰ_iは、基準の動作目標における該接触対象部（１３又は２３）の目標位置を目標ＺＭＰから見た位置ベクトル↑ｒ_iと、回転操作量↑Δθとから、次式（１０）により算出される。
【０１３５】
↑ΔＰ_i＝↑Δθ×↑ｒ_i    ……（１０）
なお、↑ｒ_iは、基準の動作目標における接触対象部（１３又は２３）の目標位置と目標ＺＭＰとから算出される。
【０１３６】
そして、接触対象部位置姿勢修正部６６は、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢（基準の動作目標における目標位置姿勢）のうちの目標位置を修正量↑ΔＰ_iにより変位させた位置に修正すると共に、目標姿勢を修正量↑Δθにより回転させた姿勢に修正する。
【０１３７】
この結果、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢は、いずれも、基準の動作目標における目標位置姿勢から、目標ＺＭＰ周りに↑Δθの回転量だけ回転させた位置姿勢に修正されることとなる。
【０１３８】
なお、単一の可動リンク３又は４の接触対象部（１３又は１４）だけが外界物に接触している状態では、その単一の接触対象部（１３又は１４）の目標位置姿勢が、目標ＺＭＰ周りに↑Δθの回転量だけ回転させた位置姿勢に修正されることとなる。
【０１３９】
上記の如く動作目標修正部５２は、基準の動作目標のうち、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢を修正する。なお、動作目標修正部５２は、外界物に接触させる各接触対象部（１３又は２３）以外の各部の目標位置姿勢（詳しくは、外界物に接触させない可動リンク３，４の接触対象部（１３又は２３）の目標位置姿勢、基体２の目標位置姿勢、及び頭部５の目標姿勢）は、基準の動作目標における目標位置姿勢に維持する。ただし、外界物に接触させない可動リンク３，４の接触対象部（１３又は２３）の目標位置姿勢を、外界物に接触させる接触対象部（１３又は２３）と同様に、目標ＺＭＰ周りに↑Δθの回転量だけ回転させた位置姿勢に修正してもよい。
【０１４０】
以上の如く動作目標修正部５２の処理を実行することで、モーメント操作量↑τ_dmdがゼロでない状態では、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢が、基準の動作目標における目標位置姿勢から、目標ＺＭＰ周りに回転操作量↑Δθの回転量だけ回転させてなる位置姿勢に修正される。
【０１４１】
この場合、外界物に接触させる全ての接触対象部（１３又は２３）が、同じ回転量（↑Δθ）で目標ＺＭＰ周りに回転される。このため、外界物に接触させる全ての接触対象部（１３又は２３）の相互の相対的な位置及び姿勢の関係は、目標位置姿勢の修正前と修正後とで一定に維持される。
【０１４２】
従って、外界物に接触させる全ての接触対象部（１３又は２３）の相互の相対的な位置及び姿勢の関係を、基準の動作目標により規定される関係に維持したまま、モーメント操作量↑τ_dmdにより示される外力モーメントをロボット１に付加し得るように、当該各接触対象部（１３又は１４）の目標位置姿勢を修正することができることとなる。
【０１４３】
例えば、図４Ａに例示するロボット１の動作状態において、図示の如くモーメンント操作量↑τ_dmdが算出された場合を想定する。
【０１４４】
図４Ａに示す例は、基準の動作目標において、外界物に接触させる接触対象部としてのロボット１の足部１３Ｒ，１３Ｌ、及びハンド部２３Ｒを、破線Ｓａで示す段差形状の床面（外界物）に接触（接地）させた状態のロボット１を例示している。
【０１４５】
なお、図４Ａにおいて、↑ｒc_1，↑ｒc_2，↑ｒc_3は、それぞれ、目標ＺＭＰから見た足部１３Ｒの接触点Ｐc_1の位置ベクトル、足部１３Ｌの接触点Ｐc_2の位置ベクトル、ハンド部２３Ｒの接触点Ｐc_3の位置ベクトルを表している。
【０１４６】
図４Ａに例示する状況では、上記位置ベクトル↑ｒc_1，↑ｒc_2，↑ｒc_3を、モーメンント操作量↑τ_dmdの向きと逆向き方向（図示例では、反時計周り方向）に、目標ＺＭＰ周りに回転させるように回転操作量↑Δθが算出される。
【０１４７】
この回転操作量↑Δθに応じて、足部１３Ｒ，１３Ｌ、及びハンド部２３Ｒの目標位置姿勢を修正した状態のロボット１が、図４Ｂに示すロボット１である。この場合、図４Ａ及び図４Ｂに示されるように、外界物に接触させる接触対象部としての足部１３Ｒ，１３Ｌ、及びハンド部２３Ｒの相互の相対的な位置及び姿勢の関係は、修正前（図４Ａ）と修正後（図４Ｂ）とで同じ関係に維持される。なお、図４Ｂの２点鎖線Ｓｂは、床面Ｓａを目標ＺＭＰ周りに↑Δθの回転量で回転させた仮想的な床面を示している。
【０１４８】
また、図４Ｂに示す例では、足部１３Ｒ，１３Ｌを、基準の動作目標で想定されていた床面Ｓａに押し付ける力をさらに増加させ、また、ハンド部２３Ｒを床面Ｓａに押し付ける力を緩和するように、足部１３Ｒ，１３Ｌ、及びハンド部２３Ｒの目標位置姿勢が修正される。これにより、モーメンント操作量↑τ_dmdにより示されるモーメントが、目標ＺＭＰ周りでロボット１に付加されるように、足部１３Ｒ，１３Ｌ、及びハンド部２３Ｒの目標位置姿勢が修正されることとなる。
【０１４９】
制御装置４０は、各制御処理周期において、上記の如く動作目標修正部５２による修正処理を施してなる修正後の動作目標を用いて関節アクチュエータ制御部５３の処理を実行する。
【０１５０】
この関節アクチュエータ制御部５３は、動作目標修正部５２による修正後の動作目標における基体２の目標位置姿勢、各足部１３の目標位置姿勢、各ハンド部２３の目標位置姿勢、及び頭部５の目標姿勢（基体２に対する相対姿勢）から、ロボット１の逆運動学の演算処理によって、ロボット１の各関節（要素関節）の目標変位量（回転角の目標値）を逐次決定する。
【０１５１】
そして、関節アクチュエータ制御部５３は、各関節の実際の変位量（関節変位センサ４３による検出値）を目標変位量に一致させるように関節アクチュエータ４１をフィードバック制御する。これにより、ロボット１は、修正後の動作目標に応じて動作することとなる。
【０１５２】
本実施形態では、制御装置４０の制御処理は、以上説明した如く実行される。
【０１５３】
かかる本実施形態によれば、基体２の実際の姿勢と、目標ＺＭＰ周りの実際のモーメントとが、基準の動作目標における目標値からずれないようにするためのモーメント操作量↑τ_dmdにより示される外力モーメントがロボット１に作用するように、外界物に接触させる各接触対象部（１３又は２３）の目標位置姿勢が、基準の動作目標における目標位置姿勢から適宜修正される。
【０１５４】
この場合、当該修正は、外界物に接触させる全ての接触対象部（１３又は２３）の相互の相対的な位置及び姿勢の関係が、基準の動作目標における関係に維持されるように行われる。
【０１５５】
このため、外界物に接触させる可動リンク３，４の個数によらずに、ロボット１の移動時に外界物に接触させる各可動リンク３又は４の実際の運動パターンが、基準の動作目標で予定されていた最適な運動パターン又はそれに近い運動パターンに継続的に保たれるようにして、ロボット１の移動動作を行うことができる。
【０１５６】
ひいては、外界物に接触させる可動リンク３，４の個数によらずに、ロボット１の動作状態が、可動リンク３，４同士の干渉等が生じ易い動作状態となったり、あるいは、ロボット１の姿勢の安定性を保ち得る余裕度が低い動作状態となって、ロボット１の継続的な移動が困難となる状況が発生するのを防止できる。
【０１５７】
また、本実施形態では、式（８）により回転操作量↑Δθを算出するために使用する変数Ｋ_iの値は、第ｉ接触対象部が、外界物に接触する状態と、接触しない状態との一方から他方に切替わるときに、「１」と「０」との一方から他方の値に徐々に変化するように設定される。
【０１５８】
このため、回転操作量↑Δθを、不連続的な変化を生じないように算出することができる。その結果、外界物に接触させる各接触対象部１３又は１４の目標位置姿勢の修正を滑らかに行うことができる。ひいては、ロボット１の動作の安定性を高めることができる。
【０１５９】
なお、以上説明した実施形態では、２つの脚リンク３Ｒ，３Ｌと２つの腕リンク４Ｒ，４Ｌとを可動リンクとして備えるロボット１を例にとって説明した。ただし、本発明における移動ロボットは、３つ以上の脚リンクを可動リンクとして備えるロボットであってもよい。また、移動ロボットは、腕リンクを備えないロボットであってもよい。
【０１６０】
また、前記実施形態では、モーメント操作量↑τ_cmdを決定するために参照するロボット１の状態量として、基体２の姿勢と、目標ＺＭＰ周りのモーメントとを用いた。ただし、これらの状態量のうちの一方だけに応じて、モーメント操作量↑τ_cmdを決定するようにしてもよい。
【０１６１】
また、基体２の姿勢及び目標ＺＭＰ周りのモーメント以外の状態量（例えばロボット１の全体重心の位置、運搬物を支持する腕リンクの姿勢等）に応じてモーメント操作量↑τ_dmdを決定するようにしてもよい。モーメント操作量↑τ_dmdを決定するために参照する状態量は、ロボット１の作業種別等に応じて、適宜変更してもよい。
【符号の説明】
【０１６２】
１…移動ロボット、２…基体、３…脚リンク（可動リンク）、４…腕リンク（可動リンク）、１３…足部（接触対象部）、２３…ハンド部（接触対象部）、４０…制御装置、５３…関節アクチュエータ制御部（アクチュエータ制御手段）、６１…モーメント操作量決定部（モーメント操作量決定手段）、６５…回転操作量決定部（回転操作量決定手段）、６６…接触対象部位置姿勢修正部（接触対象部位置姿勢修正手段）。
【図１】
0006498597.tif 000009
【図２】
0006498597.tif 000010
【図３】
0006498597.tif 000011
【図４】
0006498597.tif 000012