JP2017113847A - 移動ロボットの制御装置 - Google Patents

Abstract

【課題】外界物に接触させる可動リンクの個数、あるいは、各可動リンクの接触面の位置及び姿勢状態によらずに、各可動リンクを外界物に適切に接触させ得るように、移動ロボットの動作制御を行うことを可能とする。
【解決手段】制御装置４０の変位操作量決定部５３は、複数の可動リンク３，４のそれぞれの接触対象部１３，２３毎に、外界物から作用する実際の接触反力の観測値と目標値との偏差を所定の条件下で積分することで、接触対象部１３，２３の目標位置／姿勢を変位させる変位操作量を決定する。決定した変位操作量と移動ロボット１の基準の動作目標とに応じて各関節を駆動するアクチュエータ４１を制御する。
【選択図】図３

Description

本発明は、基体から延設された複数の可動リンクの運動によって移動する移動ロボットの制御装置に関する。

従来、例えば２つの脚リンクを有する移動ロボットに関し、特許文献１，２に見られるように、床形状（床の傾き）を推定しながら、移動ロボットの動作制御を行う技術が知られている。

特許第３０２４０２８号公報 特許第３６３４２３８号公報

近年、人型ロボット等、複数の可動リンクが基体から延設された移動ロボットを平坦な床面上だけでなく、不整地等の種々様々な環境下で移動させ得る技術の開発又は研究が進められている。

不整地等を含む種々様々な環境下で、移動ロボットを移動させる場合、各可動リンクと地面等の外界物との接触面は、それぞれの接触面の高さ、傾斜度合、向きが種々様々なものとなりやすい。

特に、移動ボットの動作環境によって、３つ以上の可動リンクを地面等の外界物に適宜接触させつつ、移動ロボットを移動させることが必要となる場合が多々ある。例えば、人型の移動ロボットを、凹凸の多い不整地等で移動させる場合に、該移動ロボットの姿勢の安定性を確保するために、２つの脚リンクと２つの腕リンクとを併せた４つの可動リンクを外界物に適宜接触させながら、該移動ロボットの移動を行うことが必要となる場合が多い。

そして、このように３つ以上の可動リンクを外界物に接触させる状況では、各可動リンクの接触面は、それぞれの高さ、傾斜度合、向きのいずれかが、互いに大きく相違するものとなりやすい。

従って、不整地等を含む種々様々な環境下で移動ロボットを動作させる場合、外界物に接触させる可動リンクの個数、あるいは、各可動リンクの接触面の位置及び姿勢状態によらずに、各可動リンクを外界物に適切に接触させ得るように、移動ロボットの動作制御を行うことが望まれる。

ここで、例えば前記特許文献１、２に見られる技術は、各脚リンクの接地面（接触面）が、概ね、同一平面上あるとみなし得ることを前提として床面の傾きを推定するようにしている。このため、特許文献１，２に見られる技術を、各可動リンクの接触面の位置及び姿勢状態が種々様々なものとなる不整地等を含む種々様々な環境下で移動ロボットを動作させる場合に適用して、移動ロボットの動作制御を適切に行うことが困難であった。

そこで、本発明は、外界物に接触させる可動リンクの個数、あるいは、各可動リンクの接触面の位置及び姿勢状態によらずに、各可動リンクを外界物に適切に接触させ得るように、移動ロボットの動作制御を行うことを可能とする制御装置を提供することを目的とする。

本発明の移動ロボットの制御装置は、上記の目的を達成するために、基体から延設された複数の可動リンクを備えると共に、各可動リンクが、その先端部と前記基体との間に設けられた１つ以上の関節の作動により前記基体に対して動くように構成されており、１つ以上の可動リンクを外界物に接触させずに空中で動かす動作と該動作に続いて該可動リンクの所定の部位である接触対象部を外界物に接触させる動作とを含む運動によって移動する移動ロボットの制御装置であって、
前記複数の可動リンクのそれぞれの接触対象部毎に、該接触対象部に外界物から作用する実際の接触反力の観測値と、該接触反力の目標値とを逐次取得すると共に、該接触対象部が外界物に実際に接触しているという第１条件と、前記移動ロボットの動作目標により示される該接触対象部の目標の運動状態が該接触対象部を外界物に接触させるべき状態であるという第２条件とのうちの少なくともいずれか一方の条件が満たされる状況であるか否かを逐次判断し、当該判断結果が肯定的となる各接触対象部毎に、該接触対象部に作用する接触反力の観測値と目標値との偏差である第１偏差を積分する処理と、各接触対象部に対応する前記第１偏差と当該判断結果が肯定的となる全ての接触対象部のそれぞれに対応する前記第１偏差の平均値との偏差である第２偏差を積分する処理とのうちのいずれか一方の積分処理を実行することによって、該接触対象部に作用する接触反力の観測値を目標値に近づけるように該接触対象部の目標位置／姿勢を前記移動ロボットの動作目標により示される位置／姿勢から変位させる第１変位操作量を逐次決定する接触対象部変位操作量決定手段と、
少なくとも前記移動ロボットの動作目標と前記決定された第１変位操作量とに応じて前記移動ロボットの各関節の目標変位量を逐次決定して、該目標変位量に応じて各関節を駆動するアクチュエータを制御するアクチュエータ制御手段とを備えることを特徴とする（第１発明）。

なお、本発明において、移動ロボットの「動作目標」は、該移動ロボットの全体の動作を規定する目標を意味する。また、接触対象部の「位置／姿勢」は、該接触対象部の「位置」及び「姿勢」の一方又は両方を意味し、「目標位置／姿勢」は、「位置」の目標値（目標位置）と「姿勢」の目標値（目標姿勢）との一方又は両方を意味する。

また、可動リンクの接触対象部を外界物に「接触」させるということは、該接触対象部を、接触反力を受け得るように外界物に接触させることを意味する。

また、接触反力等の状態量の「観測値」は、適宜のセンサによる該状態量の検出値、あるいは、該状態量と一定の相関関係を有する１つ以上の他の状態量の検出値から該相関関係に基づいて推定してなる推定値を意味する。

上記第１発明によれば、各接触対象部毎に、前記第１条件及び第２条件のいずれか一方の条件が満たされる状況において、前記第１偏差又は第２偏差を積分する処理を実行することによって、該接触対象部に作用する接触反力の観測値を目標値に近づけるように該接触対象部の目標位置／姿勢を前記移動ロボットの動作目標により示される位置／姿勢から変位させる第１変位操作量を逐次決定される。

そして、少なくとも前記移動ロボットの動作目標と前記決定された第１変位操作量とに応じて移動ロボットの各関節の目標変位量が逐次決定される。さらに、該目標変位量に応じて各関節を駆動するアクチュエータが制御される。

このため、移動ロボットの元々の動作目標において、想定されている外界物に対する各接触対象部の接触面の傾きもしくは位置が実際の接触面の傾きもしくは位置とずれが生じていても、そのずれの影響を各接触対象部毎に補償される。すなわち、各接触対象部毎に、該接触対象部が、動作目標でのタイミングからずれたタイミングもしくは位置で外界物に接触したり、あるいは、外界物に接触すべきタイミングで接触しない状況で、該接触対象部に、目標値の接触反力またはそれに近い接触反力を作用させ得るように、該接触対象部の位置／姿勢が、移動ロボットの元々の動作目標により示される位置／姿勢から調整されることとなる。

また、特に、接触対象部変位操作量決定手段が、各接触対象部毎に、前記第２偏差を積分する処理によって第１変位操作量を決定する場合には、複数の接触対象部を地面に接触させて移動ロボットが起立する状況等において、該複数の接触対象部のそれぞれの接触反力の観測値の誤差もしくはオフセット成分の影響によって、該複数の接触対象部のいずれかの接触対象部に対する変位操作量が過大になったりするのを防止して、該複数の接触対象部のそれぞれの変位操作量の相互のバランスをとることも可能となる。

よって、本発明によれば、外界物に接触させる可動リンクの個数、あるいは、各可動リンクの接触面の位置及び姿勢状態によらずに、各可動リンクを外界物に適切に接触させ得るように、移動ロボットの動作制御を行うことが可能となる。

上記第１発明では、前記接触対象部変位操作量決定手段は、前記判断結果が肯定的となる各接触対象部のうち、前記第１条件を満たさず、且つ前記第２条件を満たす各接触対象部に対応する前記第１変位操作量を決定する処理においては、該第１変位操作量のうちの該接触対象部の目標姿勢を変位させる成分を更新せずに一定値に維持し、該第１変位操作量のうちの該接触対象部の目標位置を変位させる成分を前記積分処理により逐次決定するように構成されていることが好ましい（第２発明）。

これによれば、移動ロボットの動作目標において外界物に接触すべき状態であるのに、実際には、外界物に接触していない状況となっている各接触対象部については、該接触対象部の姿勢の変動（動作目標より示される姿勢に対する変動）が抑制された状態で、該接対象部が外界物に近づくように移動して、該外界物に接触することとなる。

このため、該接触対象部を、前記動作目標に則した安定した姿勢に維持しつつ、滑らかに外界物に近づけることができる。

上記第１発明又は第２発明では、前記接触対象部変位操作量決定手段は、前記判断結果が肯定的となる各接触対象部のうち、前記第１条件及び第２条件のうちの少なくとも第１条件を満たす各接触対象部に対応する前記第１変位操作量を決定する処理においては、該第１変位操作量のうちの該接触対象部の目標位置を変位させる成分と該接触対象部の目標姿勢を変位させる成分との両方を前記積分処理により逐次決定するように構成されていることが好ましい（第３発明）。

これによれば、外界物に実際の接触している接触対象部については、前記第１変位操作量のうちの該接触対象部の目標位置を変位させる成分と該接触対象部の目標姿勢を変位させる成分との両方が前記積分処理により逐次決定される。このため、該接触対象部に作用する接触反力の観測値を目標値に近づけるように該接触対象部の位置／姿勢を調整することを、外界物の形状等によらずに速やかに実現できる。

上記第１〜第３発明では、前記接触対象部変位操作量決定手段は、前記判断結果が肯定的となる状況から否定的となる状況に変化した各接触対象部に対応する前記第１変位操作量を、前記判断結果が肯定的となる状況で決定した値からゼロまで逐次減衰させるように決定するように構成されていることが好ましい（第４発明）。

これによれば、各接触対象部を前記動作目標に従って、外界物から離反させている状況では、該接触対象部の位置／姿勢を動作目標により示される本来の位置／姿勢に収束させることができる。

上記第１〜第４発明では、前記接触対象部変位操作量決定手段は、各接触対象部に作用する接触反力の観測値を目標値に近づけるように該接触対象部の目標位置／姿勢を前記移動ロボットの動作目標により示される位置／姿勢から変位させる変位操作量として、さらに第２変位操作量及び第３変位操作量を、前記判断結果によらずに逐次決定する機能と、前記第１偏差又は第２偏差のうちの低周波成分と該低周波成分よりも高周波側の高周波成分とを逐次抽出する機能とをさらに有しており、各接触対象部に対応する前記第２変位操作量及び第３変位操作量を決定する処理では、前記低周波成分に比例する値と前記高周波成分に比例する値とのそれぞれを該第２変位操作量及び第３変位操作量として逐次決定し、前記判断結果が肯定的になる各接触対象部に対応する前記第１変位操作量を決定する処理では、前記低周波成分を積分する積分処理により該第１変位操作量を逐次決定するように構成されており、
前記アクチュエータ制御手段は、前記移動ロボットの動作目標と前記決定された第１変位操作量、第２変位操作量及び第３変位操作量との合成操作量に応じて前記移動ロボットの各関節の目標変位量を逐次決定するように構成されていることが好ましい（第５発明）。

これによれば、前記合成操作量が前記第２変位操作量をさらに含むことで、各接触対象部を外界物になじませるように接触させることをより素早く行うことが可能となる。

さらに、前記合成操作量が前記第３変位操作量をさらに含むことで、特に、各接触対象部が外界物に接触すべき状況でないのに、外界物に接触した場合に、該接触対象部に作用する接触反力を低減させるように該接触対象部の位置／姿勢を調整することをより素早く行うことが可能となる。

上記第１〜第５発明では、前記基体の実際の姿勢の観測値と、前記移動ロボットの動作目標により示される前記基体の目標姿勢との偏差である基体姿勢偏差を逐次取得し、該基体姿勢偏差をゼロに近づける外力を前記移動ロボットに付加するように、各接触対象部に付加すべき接触反力調整量を該基体姿勢偏差に応じて逐次決定する接触反力調整量決定手段をさらに備えており、前記接触対象部変位操作量決定手段は、前記移動ロボットの実際の運動が前記動作目標に従って行われると仮定した場合に各接触対象部に動力学的に作用すべき接触反力である基準接触反力に、該接触対象部に対応して決定された前記接触反力調整量を付加してなる接触反力の値を該接触対象部に対応する接触反力の前記目標値として用いるように構成されていることが好ましい（第６発明）。

これによれば、各接触対象部の接触反力の目標値には、前記基体姿勢偏差をゼロに近づける外力を前記移動ロボットに付加するように、各接触対象部に付加すべき接触反力調整量が含まれる。このため、基体の実際の姿勢を目標姿勢に近づけ得る適切な接触反力が各接触対象部に作用するように、各接触対象部を外界物に接触させることができる。

このため、外界物の形状等によらずに、ロボットの基体の姿勢の安定性を保ちつつ、各接触対象部を外界物に接触させることができる。

上記第１〜第６発明では、前記各接触対象部毎の前記第１変位操作量のうち、該接触対象部の目標位置を変位させる成分と該接触対象部の目標姿勢を変位させる成分とのうちの少なくともいずれか一方の成分の絶対値が所定の閾値以上になった場合に、前記移動ロボットの動作を停止させる機能を有するように構成されていることが好ましい（第７発明）。

ここで、接触対象部の目標位置を変位させる成分と該接触対象部の目標姿勢を変位させる成分とのうちの少なくともいずれか一方の成分の絶対値が所定の閾値以上になる状況は、外界物の位置もしくは形状等が前記動作目標で想定されているものと大きく乖離している可能性が高い状況である。

従って、第７発明によれば、かかる状況で、移動ロボットの動作を停止させることで、移動ロボットの姿勢が大きく崩れてしまうのを防止できる。

本発明の実施形態における移動ロボットを示す図。 第１実施形態の移動ロボットの動作制御に関する構成を示すブロック図。 図２に示す変位操作量決定部の処理を示すブロック線図。 図３に示す作動モード設定部６５の処理を示すフローチャート。 図５Ａ、図５Ｂ及び図５Ｃは移動ロボットの第１の動作例を順番に示す図。 図６Ａ、図６Ｂ及び図６Ｃは移動ロボットの第２の動作例を順番に示す図。 図７及び図７Ｂは移動ロボットの第３の動作例を順番に示す図。 図８Ａ及び図８Ｂはそれぞれ、接触対象部に作用する接触反力と該接触対象部に変位操作量との経時変化の一例を示すグラフ。 第２実施形態における変位操作量決定部の処理を示すブロック線図。

［第１実施形態］
本発明の第１実施形態を図１〜図８を参照して以下に説明する。

図１を参照して、本実施形態の移動ロボット１は、一例として、人型のロボットである。この移動ロボット１（以降、単にロボット１ということがある）は、上体に相当する基体２と、基体２から延設された複数の可動リンクとしての左右一対の（２つの）脚リンク３Ｌ，３Ｒ及び左右一対の（２つの）腕リンク４Ｌ，４Ｒと、頭部５とを備える。

なお、本実施形態の説明では、参照符号に付加する「Ｒ」，「Ｌ」は、それぞれロボット１の基体２の正面に向かって右側の要素、左側の要素を意味する。ただし、左右を特に区別する必要が無いときは、符号「Ｒ」，「Ｌ」を省略する。

また、以降の説明では、脚リンク３Ｌ，３Ｒ及び腕リンク４Ｌ，４Ｒのそれぞれを単に可動リンクということがある。

各脚リンク３は、基体２の下部から延設されている。各脚リンク３は、大腿部１１、下腿部１２、足部１３にそれぞれ相当する要素リンクを、基体２側から順番に、股関節１４、膝関節１５、足首関節１６を介して連結して構成されている。各脚リンク３の先端部たる足部１３は、ロボット１の移動時等に外界物（地面、床面、壁面等）に適宜接触させる接触対象部となる部位である。

各脚リンク３の足部１３と基体２との間の関節１４，１５，１６は、本実施形態では、該脚リンク３の足部１３が、基体２に対して例えば６自由度の運動自由度を有するように構成されている。

例えば、股関節１４は、総計３軸の回転自由度を有するように３つの要素関節（図示省略）により構成される。膝関節１５は、1軸の回転自由度を有するように単一の要素関節（図示省略）により構成される。足首関節１６は、総計２軸の回転自由度を有するように２つの要素関節（図示省略）により構成される。なお、要素関節は、１軸の回転自由度を有する関節である。該要素関節は、１軸周りに相対回転自在に係合された２つの部材により構成される。

各腕リンク４は、基体２の上部から延設されている。各腕リンク４は、上腕部２１、前腕部２２、ハンド部２３にそれぞれ相当する要素リンクを、基体２側から順番に、肩関節２４、肘関節２５、手首関節２６を介して連結して構成されている。

本実施形態では、各腕リンク４は、脚リンク３，３以外で、必要に応じて外界物に接触させる脚リンクとして機能し得る可動リンクである。この場合、各腕リンク４の先端部たるハンド部２３が、ロボット１の移動時の動作形態等に応じて、外界物（地面、床面、壁面等）に適宜接触させる接触対象部となる部位である。

各腕リンク４のハンド部２３と基体２との間の関節２４，２５，２６は、本実施形態では、各腕リンク４のハンド部２３が、基体２に対して例えば６自由度を有するように構成されている。

例えば、肩関節２４は、総計３軸の回転自由度を有するように３つの要素関節（図示省略）により構成される。肘関節２５は、1軸の回転自由度を有するように単一の要素関節（図示省略）により構成される。手首関節２６は、総計２軸の回転自由度を有するように２つの要素関節（図示省略）により構成される。

また、各腕リンク４のハンド部２３は、本実施形態では、物体の把持を行うことができるように構成されている。例えば、各ハンド部２３は、適宜のクランプ機構、あるいは、人の手指と同様の動作を行い得る複数の指機構等により構成される。

頭部５は、基体２の上端部に首関節３１を介して取り付けられている。首関節３１は、１軸、２軸、又は３軸の回転自由度を有するように構成される。

以上がロボット１の機構的な構成の概要である。

上記の如く構成されたロボット１は、４つの可動リンク３，３，４，４のうちの１つ以上の可動リンク３又は４を外界物に接触させずに空中で動かす動作と該動作に続いて該可動リンク３又は４を外界物に接触させる動作とを含む運動によって移動することができる。

例えば、人の歩行動作における脚の運動と同様の運動パターン（２脚歩容の運動パターン）で２つの脚リンク３，３を動かすことで、ロボット１の歩行動作を行うことができる。

この場合、腕リンク４を壁、手すり等に適宜接触させながら、脚リンク３，３を動かすことで、ロボット１の移動を行うこともできる。

また、例えば、４つの可動リンク３，３，４，４を脚として使用して、該４つの可動リンク３，３，４，４を、トロット、クロール、もしくはペース等の運動パターン（４脚歩容の運動パターン）で動かすことで、ロボット１の移動を行うこともできる。

さらに、例えば４つの可動リンク３，３，４，４を使用して、梯子等の昇降を行うこともできる。

補足すると、ロボット１の各脚リンク３あるいは各腕リンク４は、７自由度以上の運動自由度を有するように構成されていてもよい。また、各脚リンク３及び各腕リンク４のそれぞれは、回転型の関節に限らず、直動型の関節を含んでいてもよい。

また、ロボット１がハンド部２３により把持動作を必要としないロボットである場合には、各腕リンク４の先端部は、把持動作のための機構を備えない構造のものであってもよい。また、ロボット１は、頭部５を備えないものであってもよい。

また、基体２は関節を含んでいてもよい。例えば、基体２は、脚リンク３，３が連結された下側基体と、腕リンク４，４が連結された上側基体と、これらの上側基体及び下側基体を相対変位可能に連結する関節とにより構成されたものであってもよい。

次に、ロボット１の動作制御に関する構成を説明する。なお、以降の説明では、各脚リンク３の足部１３及び各腕リンク４のハンド部２３を総称的に接触対象部（１３又は２３）ということがある。

図２に示すように、ロボット１には、該ロボット１の動作制御を行う制御装置４０と、各関節（要素関節）をそれぞれ駆動する関節アクチュエータ４１と、所要の各種センサとが搭載されている。なお、図示は省略するが、本実施形態では、各ハンド部２３の把持動作を行わせるためのアクチュエータもロボット１に搭載されている。

センサとしては、ロボット１の基体２の姿勢を検出するための姿勢センサ４２と、ロボット１の各関節（要素関節）の変位量（回転角）を検出するための関節変位センサ４３と、ロボット１の視覚センサとしてのカメラ４４と、各脚リンク３の足部１３が接触対象の外界物から受ける外力（並進力及びモーメント）を検出するための力センサ４５と、各腕リンク４のハンド部２３が接触対象の外界物から受ける外力（並進力及びモーメント）を検出するための力センサ４６とが搭載されている。

姿勢センサ４２は、例えばストラップダウン方式で基体２の姿勢（空間的な向き）を検出し得るように基体２に搭載されたセンサである。該姿勢センサ４２は、３軸の角速度を検出するジャイロセンサと３軸の並進加速度を検出する加速度センサとから構成される。

関節変位センサ４３は、要素関節毎に備えられている。各関節変位センサ４３は、ロータリエンコーダ、ポテンショメータ等の回転角センサにより構成される。

カメラ４４は、例えばステレオカメラにより構成される。該カメラ４４は、例えば図１に示す如く頭部５に搭載されている。なお、カメラ４４の代わりに、又はカメラ４４に加えて、走査型のレーザ式測距センサ等の測距センサ（例えば、レーザ・レンジ・ファインダ）がロボット１に搭載されていてもよい。また、カメラ４４は、基体２に搭載されていてもよい。

力センサ４５は、各脚リンク３毎に備えられている。各力センサ４５は、例えば図１に示す如く各脚リンク３の足首関節１６と足部１３との間に介装された６軸力センサにより構成される。

また、力センサ４６は、各腕リンク４毎に備えられている。各力センサ４６は、図１に示す如く各腕リンク４の手首関節２６とハンド部２３との間に介装された６軸力センサにより構成される。

関節アクチュエータ４１は、要素関節毎に備えられている。各関節アクチュエータ４１は、電動モータあるいは油圧アクチュエータにより構成される。なお、関節アクチュエータ４１は、本発明におけるアクチュエータに相当する。

制御装置４０は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットにより構成される。そして、該制御装置４０には、上記の各センサの検出信号が入力される。なお、制御装置４０は、相互に通信可能な複数の電子回路ユニットにより構成されていてもよい。

この制御装置４０は、実装されるプログラムを実行することにより実現される機能、又は、ハードウェア構成により実現される機能として、ロボット１の各関節の作動を規定する基準の動作目標を生成する動作目標生成部５１と、基体２の実際の姿勢（以降、実姿勢という）を基準の動作目標により示される目標姿勢に近づけるように各接触対象部（１３又は２３）に外界物から作用する接触反力を調整するための接触反力調整量を決定する接触反力調整量決定部５２と、各接触対象部（１２又は２３）に外界物から作用する実際の接触反力（以降、実接触反力という）を目標接触反力（接触反力の目標値）に近づけるように各接触対象部（１３又は２３）を変位させる変位操作量を決定する変位操作量決定部５３と、ロボット１の各関節アクチュエータ４１を制御する関節アクチュエータ制御部５４とを備える。

なお、接触反力調整量決定部５２、変位操作量決定部５３、及び関節アクチュエータ制御部５４は、それぞれ本発明における接触反力調整量決定手段、接触対象部変位操作量決定手段、アクチュエータ制御手段に相当する機能部である。また、本実施形態では、変位操作量決定部５３が決定する変位操作量は、本発明における第１変位操作量に相当する。

ここで、本明細書での用語に関して補足すると、ロボット１の各接触対象部等、任意の部位の「位置姿勢」は、該部位の「位置」及び「姿勢」の組を意味する。そして、任意の部位の「目標位置姿勢」は、該部位の位置の目標値（目標位置）と該部位の姿勢の目標値（目標姿勢）との組を意味する。

この場合、任意の部位の「位置」は、該部位に対して任意に設計的に設定された該部位の代表点の空間的な位置を意味し、該部位の「姿勢」は該部位の空間的な向きを意味する。

また、任意の部位の「位置／姿勢」は、「位置」及び「姿勢」のうちの一方又は両方を意味する。同様に、任意の部位の「目標位置／姿勢」は、「目標位置」及び「目標姿勢」のうちの一方又は両方を意味する。

次に、制御装置４０の処理をより具体的に説明する。なお、以降の説明では、脚リンク３Ｒ，３Ｌ、及び腕リンク４Ｒ，４Ｌをそれぞれ順番に第１可動リンク３Ｒ、第２可動リンク３Ｌ、第３可動リンク４Ｒ、第４可動リンク４Ｌと称することがある。また、第１〜第４可動リンク３Ｒ，３Ｌ，４Ｒ，４Ｌのうちの任意の１つを第ｉ可動リンクと称し、該第ｉ可動リンクの接触対象部を第ｉ接触対象部と称することがある。

制御装置４０は、カメラ４４等を介して取得した外界情報と、外部のサーバ等から与えられ、もしくはあらかじめティーチングされた動作指令とを基に、動作目標生成部５１によりロボット１の基準の動作目標を逐次生成する。

この基準の動作目標は、取得した外界情報から認識される環境下で、上記動作指令に則した所要の運動をロボット１に行わせるための基本指針の動作目標である。該動作目標は、ＺＭＰ（Zero Moment Point）等に関する動力学的な制約条件をみたしつつ、ロボット１の各関節の作動（換言すれば、ロボット１の全体の運動）を規定し得るように適宜の動力学モデルを用いて生成される。その生成手法は公知の手法でよい。

本実施形態では、上記基準の動作目標は、各脚リンク３の足部１３及び各腕リンク４のハンド部２３のそれぞれの（すなわち、各可動リンク３，４のそれぞれの接触対象部の）基準の目標位置姿勢の軌道と、基体２の基準の目標位置姿勢の軌道と、頭部５の基準の目標姿勢の軌道とを含む。なお、「軌道」は、瞬時値（所定の制御処理周期毎の瞬時値）の時系列を意味する。

この場合、本実施形態では、各足部１３、各ハンド部２３及び基体２の目標位置姿勢は、ロボット１の外界に対して設計的に設定されるグローバル座標系で見た位置及び姿勢の目標値として表現される。また、頭部５の目標姿勢は、基体２に対して設計的に設定されるローカル座標系で見た頭部５の相対的な姿勢の目標値として表現される。

上記グローバル座標系としては、例えば図１に示すように、鉛直方向をＺ軸、互いに直交する２つの水平軸をＸ軸、Ｙ軸とする３軸直交座標系が用いられる。以降、特にことわらない限り、Ｘ軸、Ｙ軸、Ｚ軸は、当該グローバル座標系の各座標軸を意味する。

また、動作目標生成部５１が生成する基準の動作目標には、さらに、ＺＭＰの目標位置である目標ＺＭＰの軌道と、ロボット１を基準の動作目標に従って動作させた場合に各接触対象部（１３又は２３）に外界物から作用する接触反力である基準接触反力の軌道とが含まれる。

各接触対象部の基準接触反力は、それらの合力が、基準の動作目標により規定されるロボット１全体の運動によって発生する慣性力とロボット１に作用する重力との合力に釣り合うように適宜の動力学モデルを用いて決定される。なお、基準の動作目標において、外界物に対して非接触状態となる接触対象部（１３又は２３）に対応する基準接触反力はゼロである。

ＺＭＰは、詳しくは、ロボット１全体の運動によって発生する慣性力とロボット１に作用する重力との合力が、その点周りに発生するモーメントの水平軸周り方向（Ｘ軸周り方向及びＹ軸周り方向）の成分がゼロとなる点である。

かかるＺＭＰの目標位置である目標ＺＭＰは、本実施形態では、次式（１ａ），（１ｂ），（１ｃ）により算出される。

Ｐ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]はそれぞれ、当該合力のうちの並進力のＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分である。

なお、ロボット１の目標運動において、第ｉ可動リンクを外界物に接触させない状態では、上記式（１ａ）におけるＦ_i[Z]はゼロ（ひいては、Ｐcop_i[Z]・Ｆ_i[Z]＝０）である。また、Ｆa[Z]の極性を反転させてなる値（＝−Ｆa[Z]）は、式（１ａ）の分母の値に一致する。

上記式（１ａ）により、目標ＺＭＰのＺ軸方向位置（鉛直方向位置）は、外界物に接触させる全ての接触対象部（１３又は２３）のそれぞれの接触点のＺ軸方向位置を加重平均してなる位置として算出される。この場合、各接触対象部（１３又は２３）の接触点のＺ軸方向位置に係る重み係数は、該接触対象部（１３又は２３）に作用させる接触反力（並進力）のＺ軸方向成分Ｆ_i[Z]の分担割合（ロボット１に作用するトータルの接触反力（並進力）のＺ軸方向成分（＝式（１ａ）の分母の値）に対する割合）である。

そして、式（１ｂ），（１ｃ）によりそれぞれ算出される目標ＺＭＰのＸ軸方向位置及びＹ軸方向位置は、上記合力が、式（１ａ）により算出されるＺ軸方向の位置Ｐzmp[Z]において発生するＸ軸周り方向のモーメント成分とＹ軸周り方向のモーメント成分とがゼロとなる点のＸ軸方向位置及びＹ軸方向位置として算出される。

上記の如く算出される目標ＺＭＰ（基準の動作目標により規定される目標ＺＭＰ）は、ロボット１が単一平面状の床面を移動する場合には、該床面上の点となる。一方、ロボット１の複数の接触対象部（１３又は２３）のそれぞれの外界物との接触面が単一の平面上に無い場合には、目標ＺＭＰは、各接触対象部（１３又は２３）の接触面を含む平面からずれた点となる。

上記の如く目標ＺＭＰを算出する処理は、ロボット１が床面上を移動する場合に限らず、ロボット１が腕リンク４を壁等に接触させて移動する場合、ガレキ等がある不整地を移動する場合、あるいは、梯子を昇降する場合等でも同じである。

制御装置４０は、以上の如く動作目標生成部５１によりロボット１の基準の動作目標と目標ＺＭＰと各接触対象部（１３又は１４）の基準接触反力とを生成する。なお、かかる動作目標生成部５１の処理は、外部のサーバ等で実行してもよい。その場合には、制御装置４０は、該サーバ等で生成された基準の動作目標、目標ＺＭＰ、及び基準接触反力を、随時、無線通信等により取得すればよい。

制御装置４０は、さらに、接触反力調整量決定部５２、変位操作量決定部５３及び関節アクチュエータ制御部５４の処理を所定の制御処理周期で逐次実行する。

接触反力調整量決定部５２は、基準の動作目標における基体２の目標姿勢と、前記姿勢センサ４２の出力に基づき制御装置４０が推定した基体２の実姿勢の観測値とを逐次取得する。そして、接触反力調整量決定部５２は、基体２の実姿勢を目標姿勢に近づけるためにロボット１に付加すべき外力モーメント↑τ_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をロボット１に付加するために、基準の動作目標において外界物に接触させる各接触対象部（１３又は２３）を目標ＺＭＰ周りに回転させるべき回転量（回転角度）としての回転操作量↑Δθを算出する。該回転操作量↑Δθは、次式（３）で示す如く、式（４）により定義される行列Ａの逆行列Ａ^-1を↑τ_dmdに乗じる（逆行列Ａ^-1により↑τ_dmdを線形変換する）ことで算出される。

上記式（４）におけるＮは、可動リンク３，４の総数（本実施形態ではＮ＝４）、Ｉ3は３次の単位行列、Ｋ_iは、基準の動作目標において、第ｉ接触対象部が外界物に接触していることが要求されている状態（以降、接触要求有りの状態という）で「１」、外界物に接触していないことが要求されている状態（以降、接触要求無しの状態という）で「０」に設定される変数である。この変数Ｋ_iの値は、本実施形態では、第ｉ接触対象部が接触要求有り状態及び接触要求無し状態の一方から他方に切替わるときには、「１」及び「０」の一方の値から他方の値に所定の時間的変化率で（又は所定の経時変化パターンで）徐々に変化するように設定される。

また、Ｋτ_iは、第ｉ接触対象部の回転変位（目標ＺＭＰ周りの回転力による弾性変形）に関する剛性度合、Ｋｆ_iは、第ｉ接触対象部の並進変位（並進力による弾性変形）に関する剛性度合である。これらのＫτ_i、Ｋｆ_iの値はあらかじめ設定された所定値である。

なお、Ｋτ_iは、各座標軸周り方向の回転変位に関する剛性度合の設定値を対角成分とする対角行列であってもよい。同様に、Ｋｆ_iは、各座標軸方向の並進変位に関する剛性度合の設定値を対角成分とする対角行列であってもよい。

また、↑ｒc_iは、第ｉ接触対象部の接触点（接地圧中心点）を目標ＺＭＰから見た位置ベクトルである。なお、↑ｒc_i^Tは、↑ｒc_iの転置ベクトル（横ベクトル）である。

上記位置ベクトル↑ｒc_iは、基準の動作目標に対応する目標ＺＭＰと、基準の動作目標により示される第ｉ接触対象部の目標位置姿勢により規定される第ｉ接触対象部の接触点（接地圧中心点）の目標位置とから算出される。

次いで、接触反力調整量決定部５２は、各接触対象部（１３又は２３）について接触反力調整量を決定する。該接触反力調整量は、グローバル座標系の各座標軸周り方向の回転力から成る回転力成分↑τ_iと、各座標軸方向の並進力から成る並進力成分↑ｆ_iとの組により構成される。この場合、接触反力調整量決定部５２は、基準の動作目標において接触要求無しの状態の各接触対象部（１３又は２３）については、接触反力調整量の回転力成分↑τ_i及び並進力成分↑ｆ_iは、いずれもゼロベクトルとされる。

また、接触要求有りの状態の各接触対象部（１３又は２３）については、接触反力調整量の回転力成分↑τ_iと並進力成分↑ｆ_iとがそれぞれ、前記式（３）により算出した回転操作量↑Δθを用いて、次式（５ａ），（５ｂ）により算出される。

↑τ_i＝Ｋτ_i・↑Δθ ……（５ａ）
↑ｆ_i＝Ｋｆ_i・(↑Δθ×↑ｒc_i) ……（５ｂ）

なお、式（５ｂ）の上辺の↑Δθ×↑ｒc_iは、接触要求有りの状態の各接触対象部（１３又は２３）としての第ｉ接触対象部を、目標ＺＭＰ周りに基準の動作目標により示される目標位置姿勢から、回転操作量↑Δθの角度だけ回転させたときの並進変位量である。

従って、本実施形態では、接触要求有りの状態の各接触対象部（１３又は２３）については、該接触対象部（１３又は２３）を、基準の動作目標により示される目標位置姿勢から、回転操作量↑Δθの角度だけ回転させたときの回転変位量（＝↑Δθ）に応じて弾性的に発生し得る回転力と、該回転に伴う並進変位量（＝↑Δθ×↑ｒc_i）に応じて弾性的に発生し得る並進力とが、それぞれ、該接触対象部（１３又は２３）の接触反力調整量の回転力成分↑τ_i、並進力成分↑ｆ_iとして決定される。

これにより、各接触対象部（１３又は２３）の接触反力調整量は、それらの合力によって、目標ＺＭＰ周りに付加されるモーメントが前記外力モーメント↑τ_dmdに一致し、且つ、接触要求有りの状態の各接触対象部（１３又は２３）の接触反力調整量を発生するための該接触対象部（１３又は２３）の位置姿勢の変位が、接触要求有りの状態の各接触対象部（１３又は２３）の相互の位置姿勢関係を基準の動作目標での位置姿勢関係に維持し得る変位となるように決定される。

本実施形態では、接触反力調整量決定部５２の処理は、以上の如く実行される。なお、基体２の実姿勢を目標姿勢に近づけるための各接触対象部（１３又は２３）の接触反力調整量の決定手法は、上記の手法に限られない。すなわち、基体２の実姿勢を目標姿勢に近づけるための各接触対象部（１３又は２３）の接触反力調整量を、それらの合力によって目標ＺＭＰ周りに付加されるモーメントが、前記外力モーメント↑τ_dmdに一致するように決定する処理を、上記の手法と異なる手法によって実行することも可能である。

制御装置４０は、各制御処理周期において、次に変位操作量決定部５３の処理を実行する。該変位操作量決定部５３は、図３に示す処理によって、各接触対象部（１３又は２３）毎に、変位操作量を決定する。

具体的には、変位操作量決定部５３は、各接触対象部（１３又は２３）毎に、接触反力調整量決定部５２により決定された接触反力調整量と、動作目標生成部５１により決定された基準接触反力と、力センサ４５又は４６の出力により示される実接触反力の観測値とを取得する。

そして、変位操作量決定部５３は、各接触対象部（１３又は２３）の接触反力調整量と、基準接触反力とを演算部６１で加え合わせることで、該接触対象部（１３又は２３）の目標接触反力を取得する。

さらに、変位操作量決定部５３は、各接触対象部（１３又は２３）の実接触反力の観測値と目標接触反力との偏差（以降、接触反力偏差という）を演算部６２で算出する。なお、接触反力偏差は、より詳しくは、接触反力のうちの並進力に関する偏差（以降、並進接触反力偏差という）と回転力に関する偏差（以降、回転接触反力偏差という）とからなる。また、該接触反力偏差は、本発明における第１偏差に相当する。

そして、変位操作量決定部５３は、上記接触力偏差に演算部６３で所定値のゲインＧ１を乗じてなる値を積分処理部６４に入力し、該積分処理部６４の処理を実行する。なお、ゲインＧ１の値は、並進接触反力偏差に乗じる値と、回転接触反力偏差に乗じる値とで各別に設定されていてもよい。

積分処理部６４は、入力値（演算部６３の出力値）を所定の時定数Ｔaで積分する処理（伝達関数が１／(Ｔa・ｓ)により表される積分処理）を実行する作動モードである積分実行モードと、入力値の積分処理を実行せずに、積分処理部６４の出力値を一定に保持する作動モードであるホールドモードと、入力値の積分処理を実行せずに、該積分処理部６４の出力値を所定の減衰速度でゼロまで減衰させる作動モードであるリセットモードとの３つの作動モードを有する。

そして、変位操作量決定部５３は、作動モード設定部６５により、ロボット１の動作状況に応じて積分処理部６４の作動モードを設定し、その作動モードで積分処理部６４を作動させる。なお、積分処理部６４の作動モードは、前記接触反力偏差のうちの並進接触反力偏差に対する積分処理に係る作動モードである並進力用の作動モードと、回転接触反力偏差に対する積分処理に係る作動モードである回転力用の作動モードとを各別に設定可能である。

作動モード設定部６５による作動モードの設定処理は、図４のフローチャートに示す如く実行される。

ＳＴＥＰ１において、ロボット１が正常に動作中であるか否かが判断される。この判断結果が肯定的である場合には、さらに、ＳＴＥＰ２において、接触対象部（１３又は２３）が外界物に実際に接触しているか否かが判断される。なお、ＳＴＥＰ２の判断処理は、本発明における第１条件が成立するか否かを判断する処理に相当する。

このＳＴＥＰ２の判断は、例えば、該接触対象部（１３又は２３）に対応する力センサ４５又は４６の出力に基づいて判断される。具体的には、例えば、力センサ４５又は４６の出力により示される並進力の絶対値が所定の閾値以上である場合に、該接触対象部（１３又は２３）が外界物に実際に接触していると判断される。なお、ＳＴＥＰ２の判断は、力センサ４５又は４６の代わりに、適宜の接触検知センサを用いて行うことも可能である。

上記ＳＴＥＰ２の判断結果が肯定的である場合には、ＳＴＥＰ３において、並進力用の作動モードと回転力用の作動モードとの両方が積分実行モードに設定される。

また、ＳＴＥＰ２の判断結果が否定的である場合には、さらにＳＴＥＰ４において、基準の動作目標により規定される接触対象部（１３又は１４）の現在時刻での状態が、外界物に接触していることが要求されている状態（すなわち、前記接触要求有りの状態）であるか否かが判断される。この判断は、例えば、基準の動作目標により示される接触対象部の目標位置姿勢、あるいは、前記基準接触反力等に基づき行われる。あるいは、例えば、前記式（４）の係数Ｋ_iの値が所定値以上であるか否かによって、ＳＴＥＰ４の判断を行うことも可能である。なお、ＳＴＥＰ４の判断処理は、本発明における第２条件が成立するか否かを判断する処理に相当する。

上記ＳＴＥＰ４の判断結果が肯定的となる状況は、接触対象部（１３又は２３）が外界物に接触しているべき状態であるのに、実際には、該接触対象部（１３又は２３）が外界物から離れているという状況である。

この状況では、ＳＴＥＰ５において、並進力用の作動モードが積分実行モードに設定される一方、回転力用の作動モードはホールドモードに設定される。

また、ＳＴＥＰ１の判断結果が否定的である場合（ロボット１が正常動作中でない場合）、あるいは、ＳＴＥＰ４の判断結果が否定的となる場合（接触対象部（１３又は２３）が前記接触要求無しの状態であり、且つ、外界物に実際に接触していない状態である場合）には、ＳＴＥＰ６において、並進力用の作動モードと回転力用の作動モードとの両方がリセットモードに設定される。

積分処理部６４は、以上の如く作動モード設定部６５で設定された作動モードでの処理を実行する。

具体的には、前記ＳＴＥＰ３において、並進力用の作動モードと回転力用の作動モードとの両方が積分実行モードに設定されている状況（換言すれば、接触対象部（１３又は２３）が外界物に実際に接触している状況）では、積分処理部６４は、並進接触反力偏差に対応する入力値と、回転接触反力偏差に対応する入力値とをそれぞれ、所定の時定数Ｔａで逐次積分し、その積分値を出力する。

この場合、並進接触反力偏差に応じて、積分処理部６４が積分処理により算出する積分値は、該並進接触力偏差をゼロに近づけるように接触対象部（１３又は２３）を並進変位させるフィードバック操作量である。また、回転接触力偏差に応じて、積分処理部６４が積分処理により算出する積分値は、該回転接触力偏差をゼロに近づけるように接触対象部（１３又は２３）を回転変位させるフィードバック操作量である。

また、前記ＳＴＥＰ５において、並進力用の作動モードが積分実行モードに設定され、且つ、回転力用の作動モードがホールドモードに設定されている状況（換言すれば、接触対象部（１３又は２３）が外界物に接触しているべき状態であるのに、実際には、該接触対象部（１３又は２３）が外界物から離れているという状況）では、積分処理部６４は、並進接触反力偏差に対応する入力値を所定の時定数Ｔａで逐次積分する。この積分処理により、並進接触力偏差をゼロに近づけるように接触対象部（１３又は２３）を並進変位させるフィードバック操作量が算出される。

一方、積分処理部６４は、回転接触反力偏差に対応する入力値の積分処理を実行せずに、該回転接触反力偏差に対応する積分処理部６４の出力値を、前回の制御処理周期での値に維持する。従って、回転接触反力偏差に対応する積分処理部６４の出力値は、作動モードがホールドモードに設定されている期間中は、該ホールドモードの期間の開始直前における値に保持される。

また、前記ＳＴＥＰ６において、並進力用の作動モードと回転力用の作動モードとの両方がリセットモードに設定されている状況（換言すれば、接触対象部（１３又は２３）が前記接触要求無しの状態であり、且つ、外界物に実際に接触していない状況）では、積分処理部６４は、入力値の積分処理を実行せずに、並進接触反力偏差に対応する出力値と、回転接触反力偏差に対応する出力値との両方をそれぞれ、所定の減衰速度でゼロまで逐次減衰させるように決定する。この場合、積分処理部６４の出力値の減衰速度は、接触対象部（１３又は２３）が次に外界物に接触する目標タイミング（基準の動作目標でのタイミング）の前までに、該出力値がゼロまで減衰し得るように該基準の動作目標に応じて設定される。

変位操作量決定部５３は、以上の如く積分処理部６４の処理を実行した後、該積分処理部６４の出力値に対してリミット処理部６６によりリミット処理を施したものを変位操作量として決定する。

この場合、リミット処理部６６は、積分処理部６４の出力値（並進接触反力偏差に対応する出力値と、回転接触反力偏差に対応する出力値とのそれぞれ）があらかじめ定められた所定の許容範囲内に収まっている場合には、該積分処理部６４の出力値をそのまま変位操作量として決定する。

また、並進接触反力偏差に対応する出力値又は回転接触反力偏差に対応する出力値が所定の許容範囲を逸脱している場合には、リミット処理部６６は、該出力値を、強制的に該許容範囲の限界値に制限してなる値を変位操作量として決定する。

変位操作量決定部５３の処理は、各接触対象部（１３又は２３）毎に、以上の如く実行される。これにより、各接触対象部（１３又は２３）毎に、並進接触反力偏差に応じた変位操作量と、回転接触反力偏差に応じた変位操作量とが所定の制御処理周期で逐次決定される。

この場合、並進接触反力偏差に応じた変位操作量は、該並進接触反力偏差をゼロに近づけるように接触対象部（１３又は２３）を並進変位させる操作量（接触対象部（１３又は２３）の目標位置を基準の動作目標により示される位置から変更する操作量）である。また、回転接触反力偏差に応じた変位操作量は、該回転接触反力偏差をゼロに近づけるように接触対象部（１３又は２３）を回転変位させる操作量（接触対象部（１３又は２３）の目標姿勢を基準の動作目標により示される姿勢から変更する操作量）である。

制御装置４０は、各制御処理周期において、次に関節アクチュエータ制御部５４の処理を実行する。該関節アクチュエータ制御部５４は、基準の動作目標と、各接触対象部（１３又は２３）毎の変位操作量とに応じて、ロボット１の各関節のロボット１の各関節（要素関節）の目標変位量（回転角の目標値）を逐次決定する。

この場合、関節アクチュエータ制御部５４は、基準の動作目標のうちの各接触対象部（１３又は２３）の目標位置姿勢のうちの目標位置を並進接触反力偏差に応じた変位操作量により修正すると共に、目標姿勢を回転接触反力偏差に応じた変位操作量により修正する。

そして、関節アクチュエータ制御部５４は、各接触対象部（１３又は２３）の修正後の目標位置姿勢と、基準の動作目標により示される基体２の目標位置姿勢及び頭部５の目標姿勢とから、ロボット１の逆運動学の演算処理によって、ロボット１の各関節（要素関節）の目標変位量を決定する。

さらに、関節アクチュエータ制御部５４は、各関節の実際の変位量（関節変位センサ４３による検出値）を目標変位量に一致させるように関節アクチュエータ４１をフィードバック制御する。これにより、ロボット１は、修正後の動作目標に応じて動作することとなる。

なお、いずれかの接触対象部（１３又は２３）についての前記積分処理部６４の積分値の絶対値（詳しくは、並進接触反力偏差に応じた積分値の絶対値、又は回転接触反力偏差に応じた積分値の絶対値）が、過剰に大きなものとなる状況では、該接触対象部（１３又は２３）を接触させようとする外界物の形状が、基準の動作目標で想定されている形状から大きく崩れている可能性が高い。

そこで、本実施形態では、制御装置４０は、対本実施形態では、いずれかの接触対象部（１３又は２３）についての前記積分処理部６４の積分値の絶対値が所定の閾値以上の過大なものとなった場合には、ロボット１の非常停止処理を実行する。この非常停止処理では、制御装置４０は、例えば、空中に移動させた各接触対象部（１３又は２３）を、該空中移動前における外界物への接触箇所に戻した後、ロボット１の動作を停止させる。

以上が制御装置４０の制御処理によるロボット１の動作制御の詳細である。かかる動作制御によるロボット１の作動及びその利点を図５〜図８を参照して説明する。

図５Ａ、図５Ｂ及び図５Ｃは、第１〜第４可動リンク３Ｒ，３Ｌ，４Ｒ，４Ｌの全ての接触対象部１３Ｒ，１３Ｌ，２３Ｒ，２３Ｌ（足部１３Ｒ，１３Ｌ及びハンド部２３Ｒ，２３Ｌ）が接触要求有りの状態で、ロボット１の１つの接触対象部、例えばハンド部２３Ｌが実際には外界物が接触していない場合におけるロボット１の動作を順番に示している。

なお、図５Ａ、図５Ｂ及び図５Ｃではロボット１を模式化して記載していると共に、頭部５の図示を省略している。また、図５Ａ、図５Ｂ及び図５Ｃに示す例では足部１３Ｒ，１３Ｌを接触させる外界物は地面であり、ハンド部２３Ｒ，２３Ｌを接触させる外界物は壁面である。これらのことは、図６Ａ〜図６Ｃ、並びに図７Ａ、図７Ｂについても同様である。

図５Ａに示す状況では、壁面に接触していないハンド部２３Ｌに対応する積分処理部６４の作動モードが、前記ＳＴＥＰ５で設定される。このため、ロボット１は、ハンド部２３Ｌの姿勢を基準の動作目標での姿勢に維持したまま、該ハンド部２３Ｌを、矢印Ｙ１で示すように、接触させるべき壁面に近づける（ひいては、該ハンド部２３Ｌに対応する並進接触反力偏差をゼロに近づける）ように、該ハンド部２３Ｌを動かす。

なお、ここでは、図５Ａの時刻の前に、ハンド部２３Ｌに対応する積分処理部６４の出力値は、ゼロに減衰されているものとしている。

次いで、図５Ｂに示すように、ハンド部２３Ｌが壁面に接触して、該ハンド部２３Ｌに接触反力が作用するようになる。この状況では、ハンド部２３Ｌに対応する積分処理部６４の作動モードが、前記ＳＴＥＰ３で設定される。このため、図５Ｂに示すように、ハンド部２３Ｌに作用する接触反力のうちの並進力（観測値）が目標接触反力のうちの並進力に近づくように、ハンド部２３Ｌが壁面に押し付けられつつ、ハンド部２３Ｌに作用する接触反力のうちの回転力（観測値）が目標接触反力のうちの回転力に近づくように、ハンド部２３Ｌの姿勢が矢印Ｙ２で示す如く回転される。

この結果、最終的に、図５Ｃに示すように、ハンド部２３Ｌが壁面になじむように接触することとなる。

次に、図６Ａ、図６Ｂ及び図６Ｃは、第１〜第４可動リンク３Ｒ，３Ｌ，４Ｒ，４Ｌの全ての接触対象部１３Ｒ，１３Ｌ，２３Ｒ，２３Ｌ（足部１３Ｒ，１３Ｌ及びハンド部２３Ｒ，２３Ｌ）が接触要求有りの状態で、ロボット１の１つの接触対象部、例えば足部１３Ｒが実際には外界物が接触していない場合におけるロボット１の動作を順番に示している。

なお、図６Ａ、図６Ｂ及び図６Ｃでは、足部１３Ｒを接触させようとする地面と該足部１３Ｒとの間に凸部が突き出ている状況が想定されている。

図６Ａに示す状況では、地面に接触していない足部１３Ｒに対応する積分処理部６４の作動モードが、前記ＳＴＥＰ５で設定される。このため、ロボット１は、足部１３Ｒの姿勢を基準の動作目標での姿勢に維持したまま、該足部１３Ｒを、矢印Ｙ３で示すように、接触させるべき地面に近づける（ひいては、該足部１３Ｒに対応する並進接触反力偏差をゼロに近づける）ように、該足部１３Ｒを動かす。

なお、ここでは、図６Ａの時刻の前に、足部１３Ｒに対応する積分処理部６４の出力値は、ゼロに減衰されているものとしている。

次いで、足部１３Ｒが凸部に接触して、該足部１３Ｒに接触反力が作用するようになると（図６Ｂに示す状況）、足部１３Ｒに対応する積分処理部６４の作動モードが、前記ＳＴＥＰ３で設定される。このため、図６Ｂに示すように、ハンド部２３Ｌに作用する接触反力のうちの並進力（観測値）が目標接触反力のうちの並進力に近づくように、ハンド部２３Ｌが凸部に押し付けられつつ、足部１３Ｒに作用する接触反力のうちの回転力（観測値）が目標接触反力のうちの回転力に近づくように、足部１３Ｒの姿勢が矢印Ｙ４で示す如く回転される。

この結果、最終的に、図６Ｃに示すように、足部１３Ｒが地面と凸部とになじむように接触する。

次に、図７Ａ及び図７Ｂは、第１〜第４可動リンク３Ｒ，３Ｌ，４Ｒ，４Ｌのうちの第２〜第４可動リンク３Ｌ，４Ｒ，４Ｌのそれぞれの接触対象部１３Ｌ，２３Ｒ，２３Ｌ（足部１３Ｌ及びハンド部２３Ｒ，２３Ｌ）が接触要求有りの状態であると共に、第１可動リンクである右側脚リンク３Ｒの足部１３Ｒが、接触要求有りの状態となる直前の接触要求無しの状態で、地面上の凸部に接触し、その後、全ての接触対象部１３Ｒ，１３Ｌ，２３Ｒ，２３Ｌが接触要求有りの状態となった場合におけるロボット１の動作を順番に示している。

図７Ａに示す状況では、接触要求無しの状態で地面の凸部に接触した足部１３Ｒに対応する積分処理部６４の作動モードが、前記ＳＴＥＰ３で設定される。このため、ロボット１は、矢印Ｙ５，Ｙ６で示す如く、足部１３Ｒを凸部から浮き上がらせると共に回転させる（ひいては、該足部１３Ｒに対応する並進接触反力偏差及び回転接触反力偏差をゼロに近づける）ように、該足部１３Ｒを動かす。これにより、接触要求無しの状態の足部１３Ｒに作用する接触反力がゼロに近づくように、該足部１３Ｒの位置姿勢が調整される。

そして、最終的に、全ての接触対象部１３Ｒ，１３Ｌ，２３Ｒ，２３Ｌが接触要求有りの状態となると、図７Ｂに示すように（図６Ｃの場合と同様に）、足部１３Ｒが地面と凸部とになじむように接触する。

次に、図８Ａ及び図８Ｂはそれぞれ、ロボット１の２脚歩容での移動時に、一方の足部、例えば足部１３Ｌが、接触要求有りの状態の直前の接触要求無しの状態で、地面に接触してしまった場合における該足部１３Ｌに作用する接触反力のうちのＺ軸方向の並進力と、変位操作量のうちのＺ軸方向の並進変位成分との経時変化の一例を示すグラフである。

図８Ａに示すように、足部１３Ｌに作用する実接触反力（ここでは、Ｚ軸方向の実並進力）が目標接触反力に追従するように足部１３Ｌの位置姿勢が調整される。この場合、足部１３Ｌに対応する変位相操作量のうちのＺ軸方向の並進変位成分は、図８Ｂに示す如く、該足部１３Ｌの接触要求有りの状態の期間において、足部１３Ｌが接触する実際の地面のＺ軸方向位置と、基準の動作目標で想定されているＺ軸方向位置との偏差Ｚerrに近い値になるように算出される。

以上のように、本実施形態によれば、各接触対象部（１３又は２３）毎に、実接触反力の観測値が、目標接触反力に近づくように、各接触対象部（１３又は２３）の目標位置姿勢を、基準の動作目標における位置姿勢から変位させる変位操作量が決定される。そして、各接触対象部（１３又は２３）の目標位置姿勢を、基準の動作目標における位置姿勢から該接触対象部に対応する変位操作量により修正した目標位置姿勢を実現するように、ロボット１の関節アクチュエータ４１が制御される。

これにより、ロボット１の各接触対象部（１３又は２３）を接触させる外界物の形状等によらずに、各接触対象部（１３又は２３）を、目標接触反力又はこれに近い接触反力を受け得るように外界物に接触させることができる。

この場合、目標接触反力は、基体２の実姿勢を目標姿勢に近づけ得るように調整されている。このため、外界物の形状等によらずに、基体２の姿勢を安定な姿勢に保ちつつ、各接触対象部（１３又は２３）を外界物に接触させることができる。

また、各接触対象部（１３又は２３）の接触要求無しの状態で、該接触対象部（１３又は２３）が外界物に接触した場合に、並進接触反力偏差及び回転接触反力偏差のそれぞれに応じて、積分処理により変位操作量を決定するので、接触要求有りの状態の直前の接触要求無しの状態で、該接触対象部（１３又は２３）の位置姿勢を外界部の形状に則した位置姿勢に調整できる。このため、接触要求無し状態から接触要求有りの状態に変化したときに速やかに、該接触対象部（１３又は２３）を、外界物の形状になじんだ形態で、目標接触反力又はこれに近い接触反力を受け得るように外界物に接触させることができる。

また、各接触対象部（１３又は２３）の接触要求有りの状態で、該接触対象部（１３又は２３）が外界物に接触していない場合に、並進接触反力偏差及び回転接触反力偏差のうちの並進接触反力偏差に応じて、積分処理により変位操作量を決定され、回転接触反力偏差に対応する積分処理部６４の出力値は一定に保持される。

このため、接触対象部（１３又は２３）の姿勢変動を抑制しつつ、該接触対象部（１３又は２３）を、滑らかに外界物に近づけて、該外界物に接触させることができる。

そして、接触対象部（１３又は２３）が外界物に接触した状態では、並進接触反力偏差及び回転接触反力偏差のそれぞれに応じて、積分処理により変位操作量を決定するので、実接触反力を目標接触反力に速やかに近づけるように、接触対象部（１３又は２３）の位置姿勢を調整できる。

［第２実施形態］
次に、本発明の第２実施形態を図９を参照して説明する。なお、本実施形態は、制御装置４０の一部の処理だけが第１実施形態と相違するものである。このため、本実施形態では、第１実施形態と相違する事項を中心に説明し、第１実施形態と同一の事項については説明を省略する。

図９を参照して、本実施形態では、変位操作量決定部７１は、各接触対象部（１３又は２３）毎に、同図に示す処理によって、第１変位操作量、第２変位操作量及び第３変位操作量の組を、該接触対象部（１３又は２３）の目標位置姿勢を変位させる変位操作量として決定するように構成されている。

この場合、変位操作量決定部７１は、第１実施形態と同様に、演算部６１，６２の処理によって、接触反力偏差（並進接触反力偏差及び回転接触反力偏差）を算出する。

さらに、変位操作量決定部７１は、算出した接触反力偏差（並進接触反力偏差及び回転接触反力偏差のそれぞれ）をローパスフィルタ７２に入力することで、並進接触反力偏差及び回転接触反力偏差のそれぞれ毎に、接触反力偏差のうちの低周波成分を逐次抽出すると共に、該低周波成分を接触反力偏差から減じる処理を演算部７４で実行することで、接触反力偏差のうちの高周波成分を逐次抽出する。

なお、高周波成分は、接触反力偏差から、ハイパスフィルタにより抽出してもよい。また、低周波成分は、接触反力偏差から高周波成分を減じる処理により抽出してもよい。

そして、変位操作量決定部７１は、接触反力偏差の低周波成分に所定値のゲインＧＬを演算部７３で乗じてなる値を、第２変位操作量として求めると共に、接触反力偏差の高周波成分に所定値のゲインＧＨを演算部７５で乗じてなる値を、第３変位操作量として求める。

従って、上記第２変位操作量は、接触反力偏差の低周波成分に応じて、フィードバック制御則としての比例則により決定されるフィードバック操作量である。同様に、第３変位操作量は、接触反力偏差の高周波成分に応じて、フィードバック制御則としての比例則により決定されるフィードバック操作量である。

なお、第２変位操作量及び第３変位操作量のそれぞれは、並進接触反力偏差に応じた並進変位成分と、回転接触反力偏差に応じた回転変位成分とから成る。

また、変位操作量決定部７１は、第２変位操作量（演算部７３の出力値）から、前記第１実施形態と同じ処理を行う積分処理部６４及びリミット処理部６６により算出される値を第１変位操作量として決定する。この場合、積分処理部６４の作動モードは、第１実施形態と同様に作動モード設定部６５で設定される。

本実施形態では、各接触対象部（１３又は２３）毎に、上記の如く変位操作量決定部７１で決定される第１〜第３変位操作量が、基準の動作目標と共に、関節アクチュエータ制御部５４に与えられる。

そして、本実施形態では、関節アクチュエータ制御部５４は、基準の動作目標のうちの各接触対象部（１３又は２３）の目標位置姿勢のうちの目標位置を、並進接触反力偏差に応じた第１〜第３変位操作量を加え合わせた変位操作量により修正すると共に、目標姿勢を回転接触反力偏差に応じた第１〜第３変位操作量を加え合わせた変位操作量（合成操作量）により修正する。

さらに、関節アクチュエータ制御部５４は、各接触対象部（１３又は２３）の修正後の目標位置姿勢と、基準の動作目標により示される基体２の目標位置姿勢及び頭部５の目標姿勢とに応じて、第１実施形態と同様に、関節アクチュエータ４１を制御する。

本実施形態は、以上説明した事項以外は、前記第１実施形態と同じである。

かかる本実施形態では、各接触対象部（１３又は２３）の実接触反力を目標接触反力に近づけるように各接触対象部（１３又は２３）の目標位置姿勢を変位させる変位操作量として、前記第１実施形態における変位操作量に相当する第１変位操作量に加えて、接触反力偏差の低周波成分に比例する第２変位操作量と、高周波成分に比例する第３変位操作量とが使用される。

この場合、第２変位操作量をさらに含むことで、各接触対象部（１３又は２３）を外界物になじませるように接触させることをより素早く行うことが可能となる。

また、第３変位操作量をさらに含むことで、特に、各接触対象部（１３又は２３）の接触要求無しの状態で、該接触対象部（１３又は２３）が外界物に接触した場合に、接触反力を低減させるように該接触対象部（１３又は２３）の位置姿勢を調整することをより素早く行うことが可能となる。

なお、以上説明した各実施形態では、各接触対象部（１３又は２３）毎に、接触反力偏差に対する積分処理を積分処理部６４で実行するようにした。ただし、例えば各制御処理周期において、前記ＳＴＥＰ２又はＳＴＥＰ４の判断結果が肯定的となる全ての接触対象部（１３又は２３）（すなわち、実際に外界物に接触しているか、又は接触要求有りの状態となっている全ての接触対象部（１３又は２３））のそれぞれの接触反力偏差の平均値（現在時刻での平均値）を算出し、その平均値を各接触対象部（１３又は２３）の現在の接触反力偏差から差し引いてなる偏差（これは本発明における第２偏差に相当する）、あるいは、該第２偏差に所定値のゲインを乗じてなる値に対して、積分処理部６４で積分処理を実行するようにしてもよい。このようにした場合でも、前記各実施形態と同様の効果を奏することができる。

加えて、このようにした場合には、例えば、複数の接触対象部（１３又は２３）を地面に接触させてロボット１が起立する状況等において、該複数の接触対象部（１３又は２３）のそれぞれの接触反力の観測値の誤差もしくはオフセット成分の影響によって、該複数の接触対象部（１３又は２３）のいずれかの接触対象部に対する変位操作量が過大になったりするのを防止して、該複数の接触対象部（１３又は２３）のそれぞれの変位操作量の相互のバランスをとることができる。

また、前記各実施形態では、２つの脚リンク３Ｒ，３Ｌと２つの腕リンク４Ｒ，４Ｌとを可動リンクとして備えるロボット１を例にとって説明した。ただし、本発明における移動ロボットは、３つ以上の脚リンクを可動リンクとして備えるロボットであってもよい。また、移動ロボットは、腕リンクを備えないロボットであってもよい。

１…移動ロボット、２…基体、３…脚リンク（可動リンク）、４…腕リンク（可動リンク）、１３…足部（接触対象部）、２３…ハンド部（接触対象部）、４０…制御装置、５２…接触反力調整量決定部（接触反力調整量決定手段）、５３，７１…変位操作量決定部（接触対象部変位操作量決定手段）、５４…関節アクチュエータ制御部（アクチュエータ制御手段）、４１…関節アクチュエータ（アクチュエータ）。

Claims (7)

1. 基体から延設された複数の可動リンクを備えると共に、各可動リンクが、その先端部と前記基体との間に設けられた１つ以上の関節の作動により前記基体に対して動くように構成されており、１つ以上の可動リンクを外界物に接触させずに空中で動かす動作と該動作に続いて該可動リンクの所定の部位である接触対象部を外界物に接触させる動作とを含む運動によって移動する移動ロボットの制御装置であって、
   前記複数の可動リンクのそれぞれの接触対象部毎に、該接触対象部に外界物から作用する実際の接触反力の観測値と、該接触反力の目標値とを逐次取得すると共に、該接触対象部が外界物に実際に接触しているという第１条件と、前記移動ロボットの動作目標により示される該接触対象部の目標の運動状態が該接触対象部を外界物に接触させるべき状態であるという第２条件とのうちの少なくともいずれか一方の条件が満たされる状況であるか否かを逐次判断し、当該判断結果が肯定的となる各接触対象部毎に、該接触対象部に作用する接触反力の観測値と目標値との偏差である第１偏差を積分する処理と、各接触対象部に対応する前記第１偏差と当該判断結果が肯定的となる全ての接触対象部のそれぞれに対応する前記第１偏差の平均値との偏差である第２偏差を積分する処理とのうちのいずれか一方の積分処理を実行することによって、該接触対象部に作用する接触反力の観測値を目標値に近づけるように該接触対象部の目標位置／姿勢を前記移動ロボットの動作目標により示される位置／姿勢から変位させる第１変位操作量を逐次決定する接触対象部変位操作量決定手段と、
   少なくとも前記移動ロボットの動作目標と前記決定された第１変位操作量とに応じて前記移動ロボットの各関節の目標変位量を逐次決定して、該目標変位量に応じて各関節を駆動するアクチュエータを制御するアクチュエータ制御手段とを備えることを特徴とする移動ロボットの制御装置。
2. 請求項１記載の移動ロボットの制御装置において、
   前記接触対象部変位操作量決定手段は、前記判断結果が肯定的となる各接触対象部のうち、前記第１条件を満たさず、且つ前記第２条件を満たす各接触対象部に対応する前記第１変位操作量を決定する処理においては、該第１変位操作量のうちの該接触対象部の目標姿勢を変位させる成分を更新せずに一定値に維持し、該第１変位操作量のうちの該該接触対象部の目標位置を変位させる成分を前記積分処理により逐次決定するように構成されていることを特徴とする移動ロボットの制御装置。
3. 請求項１又は２記載の移動ロボットの制御装置において、
   前記接触対象部変位操作量決定手段は、前記判断結果が肯定的となる各接触対象部のうち、前記第１条件及び第２条件のうちの少なくとも第１条件を満たす各接触対象部に対応する前記第１変位操作量を決定する処理においては、該第１変位操作量のうちの該接触対象部の目標位置を変位させる成分と該接触対象部の目標姿勢を変位させる成分との両方を前記積分処理により逐次決定するように構成されていることを特徴とする移動ロボットの制御装置。
4. 請求項１〜３のいずれか１項に記載の移動ロボットの制御装置において、
   前記接触対象部変位操作量決定手段は、前記判断結果が肯定的となる状況から否定的となる状況に変化した各接触対象部に対応する前記第１変位操作量を、前記判断結果が肯定的となる状況で決定した値からゼロまで逐次減衰させるように決定するように構成されていることを特徴とする移動ロボットの制御装置。
5. 請求項１〜４のいずれか１項に記載の移動ロボットの制御装置において、
   前記接触対象部変位操作量決定手段は、各接触対象部に作用する接触反力の観測値を目標値に近づけるように該接触対象部の目標位置／姿勢を前記移動ロボットの動作目標により示される位置／姿勢から変位させる変位操作量として、さらに第２変位操作量及び第３変位操作量を、前記判断結果によらずに逐次決定する機能と、前記第１偏差又は第２偏差のうちの低周波成分と該低周波成分よりも高周波側の高周波成分とを逐次抽出する機能とをさらに有しており、各接触対象部に対応する前記第２変位操作量及び第３変位操作量を決定する処理では、前記低周波成分に比例する値と前記高周波成分に比例する値とのそれぞれを該第２変位操作量及び第３変位操作量として逐次決定し、前記判断結果が肯定的になる各接触対象部に対応する前記第１変位操作量を決定する処理では、前記低周波成分を積分する積分処理により該第１変位操作量を逐次決定するように構成されており、
   前記アクチュエータ制御手段は、前記移動ロボットの動作目標と前記決定された第１変位操作量、第２変位操作量及び第３変位操作量との合成操作量に応じて前記移動ロボットの各関節の目標変位量を逐次決定するように構成されていることを特徴とする移動ロボットの制御装置。
6. 請求項１〜５のいずれか１項に記載の移動ロボットの制御装置において、
   前記基体の実際の姿勢の観測値と、前記移動ロボットの動作目標により示される前記基体の目標姿勢との偏差である基体姿勢偏差を逐次取得し、該基体姿勢偏差をゼロに近づける外力を前記移動ロボットに付加するように、各接触対象部に付加すべき接触反力調整量を該基体姿勢偏差に応じて逐次決定する接触反力調整量決定手段をさらに備えており、
   前記接触対象部変位操作量決定手段は、前記移動ロボットの実際の運動が前記動作目標に従って行われると仮定した場合に各接触対象部に動力学的に作用すべき接触反力である基準接触反力に、該接触対象部に対応して決定された前記接触反力調整量を付加してなる接触反力の値を該接触対象部に対応する接触反力の前記目標値として用いるように構成されていることを特徴とする移動ロボットの制御装置。
7. 請求項１〜６のいずれか１項に記載の移動ロボットの制御装置において
   前記各接触対象部毎の前記第１変位操作量のうち、該接触対象部の目標位置を変位させる成分と該接触対象部の目標姿勢を変位させる成分とのうちの少なくともいずれか一方の成分の絶対値が所定の閾値以上になった場合に、前記移動ロボットの動作を停止させる機能を有するように構成されていることを特徴とする移動ロボットの制御装置。