JP6410174B2 - 移動ロボット - Google Patents

Description

本発明は、基体から延設された複数の可動リンクを有する移動ロボットに関する。

基体から延設された複数の可動リンクを有する移動ロボットとしては、例えば２つの脚リンクと２つの腕リンクとを可動リンクとして備える人型のロボットが従来より一般に知られている（例えば特許文献１を参照）。

この種の移動ロボットに高所での作業を行わせる場合、梯子もしくは脚立等、急峻な傾きを有する構造物（以降、急勾配構造物ということがある）の昇降を移動ロボットに行わせることが必要となる。

このため、近年では、例えば非特許文献１に見られるように、人型のロボットに脚立等の急勾配構造物を昇降させる技術の研究が進められている。

特許第５２２１６８８号公報

「身体環境保持負荷指標に基づく脚腕協調段差昇降行動の実現」／野田晋太朗、野沢峻一、室岡雅樹、岡田慧、稲葉雅幸／No.13-2 Proceedings of the 2013 JSME Conference on Robotics and Mechatronics, Tsukuba, Japan, May 22-25,2013／2P1-B03

梯子もしくは脚立等の急勾配構造物の昇降をヒューマノイドロボットの如き移動ロボットに行わせる場合、基体の上部から延設された上方側支持対象可動リンクの先端部（例えば腕リンクのハンド部）と基体の下部から延設された下方側支持対象可動リンクの先端部（例えば脚リンクの足平部）とを急勾配構造物に接触させて支持させ、且つ、上記上方側支持対象可動リンク及び下方側支持対象可動リンクの以外の１つ以上の移動対象可動リンク（例えば、他の腕リンク及び脚リンクの一方又は両方）の先端部を急勾配構造物から離反させた状態で、該移動対象可動リンクの先端部を目標支持位置姿勢に向かって移動させ、該目標支持位置姿勢にて該移動対象可動リンクの先端部を該急勾配構造物に接触させて支持させるという昇降用動作が行われる。

ここで、急勾配構造物での移動ロボットの昇降時においては、移動ロボットの全体重心が、通常、急勾配構造物から横方向に離れた位置に存在するため、重力に起因するモーメント（主にピッチ方向のモーメント等）が移動ロボットに作用しやすい。

そのため、上記昇降用動作の実行中に、急勾配構造物に支持させた可動リンクの先端部の滑りが発生し、ひいては、移動ロボットの基体の実際の姿勢が、目標とする本来の姿勢からずれた姿勢になってしまう場合が多々ある。

そして、このような場合には、急勾配構造物に支持させた下方側支持対象可動リンクの中間部、あるいは、移動対象可動リンク（特に基体の下部から延設される移動対象可動リンク）と急勾配構造物との間の干渉が発生しやすい。あるいは、移動対象可動リンクの先端部を、目標支持位置姿勢にて急勾配構造物に適切に支持させることができなくなる（例えば脚リンクの先端部の踏み外しを生じる）場合もある。

本発明はかかる背景に鑑みてなされたものであり、梯子、脚立等の構造物の昇降を移動ロボットに行わせる場合に、可動リンクと構造物との干渉等を回避し得るように移動ロボットの運動を行うことを可能とする制御装置を提供することを目的とする。

本発明の移動ロボットの制御装置は、上記目的を達成するために、基体と、該基体から延設された複数の可動リンクとを備え、各可動リンクの先端部と前記基体との間に設けられた関節の作動によって各可動リンクの先端部が前記基体に対して動くように構成された移動ロボットの制御装置であって、
前記移動ロボットに昇降対象の構造物を昇降させるとき、前記複数の可動リンクのうち、前記基体の上部から延設された１つ以上の可動リンクである上方側支持対象可動リンクの先端部と前記基体の下部から延設された１つ以上の可動リンクである下方側支持対象可動リンクの先端部とをそれぞれ該構造物に接触させて支持させ、且つ、前記上方側支持対象可動リンク及び下方側支持対象可動リンク以外の１つ以上の可動リンクである移動対象可動リンクの先端部をそれぞれ前記構造物から離反させた状態で、各移動対象可動リンクの先端部を目標支持位置姿勢に向かって移動させ、該目標支持位置姿勢にて該移動対象可動リンクの先端部を該構造物に接触させて支持させるという昇降用動作を含むように、該移動ロボットの各関節の作動を規定する動作目標を逐次決定する動作目標決定手段と、
前記決定された動作目標に応じて前記移動ロボットの各関節の作動を制御する関節制御手段と、
前記移動ロボットの前記昇降用動作における１つ以上の下方側支持対象可動リンクのそれぞれの先端部の実際の位置姿勢と前記動作目標における該下方側支持対象可動リンクの先端部の目標位置姿勢との偏差である位置姿勢偏差と、前記移動ロボットの前記昇降用動作における１つ以上の移動対象可動リンクのそれぞれの先端部の実際の位置姿勢と前記動作目標における該移動対象可動リンクの先端部の目標位置姿勢との偏差である位置姿勢偏差とのうちの少なくともいずれか一方を推定する偏差推定手段とを備え、
前記動作目標決定手段は、前記移動ロボットの前記昇降用動作における前記動作目標を逐次決定するとき、１つ以上の下方側支持対象可動リンクの先端部のそれぞれの前記基体に対する相対的な目標位置／姿勢と、１つ以上の移動対象可動リンクのそれぞれの先端部を前記構造物に支持させようとする目標支持位置／姿勢とのうちの少なくともいずれかを前記推定された位置姿勢偏差に応じて調整するように前記動作目標を決定するように構成されていることを特徴とする（第１発明）。

なお、本発明において、「位置姿勢」は、「位置」及び「姿勢」の組を意味する。また、「位置／姿勢」は、「位置」及び「姿勢」の少なくともいずれか一方（換言すれば、いずれか一方又は両方）を意味する。

前記移動ロボットの昇降用動作においては、前記移動ロボットに作用する重力に起因するモーメントが該移動ロボットに作用しやすいために、前記構造物に支持させた前記下方側支持対象可動リンクの先端部、あるいは前記上方側支持対象可動リンクの先端部の滑りが発生しやすい。

このような滑りが発生した場合に、前記下方側支持対象可動リンク又は移動対象可動リンクの先端部についての前記位置姿勢偏差（位置及び姿勢の少なくともいずれか一方についての偏差がゼロでない偏差）が発生し、該位置姿勢偏差が前記偏差推定手段により推定される。

そして、前記動作目標決定手段は、前記移動ロボットの前記昇降用動作における前記動作目標を逐次決定するとき、１つ以上の下方側支持対象可動リンクの先端部のそれぞれの前記基体に対する相対的な目標位置／姿勢と、１つ以上の移動対象可動リンクのそれぞれの先端部を前記構造物に支持させようとする目標支持位置／姿勢とのうちの少なくともいずれかを前記推定された位置姿勢偏差に応じて調整するように前記動作目標を決定する。

この場合、１つ以上の下方側支持対象可動リンクの先端部のそれぞれの前記基体に対する相対的な目標位置／姿勢を調整することで、結果的に、構造物に対する基体の位置／姿勢を調整できる。ひいては、構造物に対する基体の位置／姿勢が本来の適切な位置／姿勢から乖離するのを防止することが可能となる。

また、１つ以上の移動対象可動リンクのそれぞれの先端部を前記構造物に支持させようとする目標支持位置／姿勢を調整することで、該移動対象可動リンクの先端部が、構造物に対する本来の適切な支持位置／姿勢から乖離した位置／姿勢で該構造物に支持されること、もしくは、該該移動対象可動リンクの先端部が、目標支持位置／姿勢に達したときに、構造物に対して非接触状態となってしまうことを防止することが可能となる。あるいは、該移動対象可動リンクの先端部が目標支持位置姿勢に達するまでの該先端部の移動軌道が、本来の適切な軌道から乖離するのを防止することが可能となる。

従って、第１発明によれば、梯子、脚立等の構造物の昇降を移動ロボットに行わせる場合に、可動リンクと構造物との干渉等を回避し得るように移動ロボットの運動を行うことが可能となる。

かかる第１発明では、前記偏差推定手段は、種々様々な手法で前記位置姿勢偏差を推定することが可能である。

例えば、前記移動ロボットの基体の姿勢を検出するための姿勢センサが該移動ロボットに搭載されている場合には、前記偏差推定手段は、前記姿勢センサの出力により示される前記基体の姿勢の検出値と、前記動作目標における前記基体の姿勢の目標値との偏差だけ、前記動作目標の移動ロボットを、前記上方側支持対象可動リンクの先端部のそれぞれに前記構造物から作用する荷重の全体の作用中心点を回転中心として仮想的に回転させることにより得られる移動ロボットの各下方側支持対象可動リンクの先端部又は各移動対象可動リンクの先端部の位置姿勢を該下方側支持対象可動リンクの先端部又は該移動対象可動リンクの先端部の実際の位置姿勢として推定する手段を含む構成を採用できる（第２発明）。

前記移動ロボットの昇降用動作において、重力に起因して移動ロボットに作用するモーメントに応じて、前記構造物に支持させた下方側支持対象可動リンクの先端部、あるいは、前記上方側支持対象可動リンクの先端部の滑りが発生した場合、該移動ロボットは、前記上方側支持対象可動リンクの先端部のそれぞれに前記構造物から作用する荷重の全体の作用中心点を回転中心として回転することで、下方側支持対象可動リンクの先端部、あるいは、前記上方側支持対象可動リンクの先端部の滑りが発生したものとみなすことができる。

そして、この場合の移動ロボットの回転量は、前記姿勢センサの出力により示される前記基体の姿勢の検出値と、前記動作目標における前記基体の姿勢の目標値との偏差に一致もしくはほぼ一致する。

そこで、第２発明では、前記偏差推定手段は、上記の如く前記下方側支持対象可動リンクの先端部又は前記移動対象可動リンクの先端部の実際の位置姿勢を推定する。この場合、前記下方側支持対象可動リンクの先端部又は前記移動対象可動リンクの先端部の実際の位置姿勢の推定値と目標位置姿勢とから前記位置姿勢偏差を推定できる。

従って、第２発明によれば、前記位置姿勢偏差を推定するために必要となる前記下方側支持対象可動リンクの先端部又は前記移動対象可動リンクの先端部の実際の位置姿勢を、前記姿勢センサの出力を用いて適切に推定することができる。

なお、前記上方側支持対象可動リンクの先端部のそれぞれに前記構造物から作用する荷重の全体の作用中心点の位置（前記回転中心の位置）は、任意の手法で特定すればよい。例えば、各上方側支持対象可動リンクの先端部に作用する荷重を検出するための力センサ（６軸力センサ等）が該上方側支持対象可動リンクに搭載されている場合には、該力センサの出力に基づいて、上記作用中心点（回転中心）の位置を特定できる。

補足すると、前記位置姿勢偏差を推定するために必要となる前記下方側支持対象可動リンクの先端部又は前記移動対象可動リンクの先端部の実際の位置姿勢を推定する手法は、上記第２発明の手法に限られない。例えば、各下方側支持対象可動リンクの先端部に作用する荷重を検出するための力センサ（６軸力センサ等）が該下方側支持対象可動リンクに搭載されている場合には、該力センサの出力に基づいて、前記下方側支持対象可動リンクの先端部の実際の位置姿勢を推定することも可能である。

さらに、例えば、いずれか１つの下方側支持対象可動リンクの先端部の実際の位置姿勢の推定値と、移動ロボットの各関節の変位量の検出値とから各移動対象可動リンクの先端部又は他の下方側支持対象可動リンクの先端部の実際の位置姿勢を推定することも可能である。

上記第１発明又は第２発明は、前記移動ロボットが、前記基体の下部から延設された２つ以上の可動リンクと、前記基体の上部から延設された２つ以上の可動リンクとを備えており、前記昇降用動作が、前記基体の下部から延設された可動リンクのうちの一部の可動リンクを前記下方側支持対象可動リンク、前記基体の下部から延設された可動リンクのうちの残りの可動リンクを前記移動対象可動リンク、前記基体の上部から延設された可動リンクのうちの少なくとも１つの可動リンクを前記上方側支持対象可動リンクとして、前記移動対象可動リンクの先端部を移動させる昇降用動作である場合に好適である（第３発明）。

すなわち、第３発明における前記昇降用動作では、前記基体の下部から延設された可動リンクのうちの一部の可動リンクだけが下方側支持対象可動リンクとなるため、該下方側支持対象可動リンクの先端部の滑りが発生しやすい。

なお、上記「一部の可動リンク」というのは、基体の下部から延設された可動リンクの総数よりも少ない個数（１つ又は複数）の可動リンクを意味する。

しかるに、前記動作目標決定部が、前記第１発明に関して説明した如く動作目標を決定することで、可動リンクと構造物との干渉等を回避し得るように移動ロボットの運動（構造物の昇降）を行うことを高い信頼性で行うことが可能となる。

かかる第３発明では、前記動作目標決定手段は、前記推定された位置姿勢偏差が前記基体の下部が前記構造物に近づくように該基体の姿勢が傾斜することで発生する偏差である場合に、前記基体の下部から延設された可動リンクのうちの１つ以上の前記下方側支持対象可動リンクのそれぞれの前記基体に対する連結部と該下方側支持対象可動リンクの先端部との間の距離が、該下方側支持対象可動リンクについての前記位置姿勢偏差がゼロである場合よりも長くなるように前記基体に対する該下方側支持対象可動リンクの先端部の相対的な目標位置／姿勢を調整し、さらに、前記基体の下部から延設された可動リンクのうちの１つ以上の前記基体の下部から延設された可動リンクのうちの１つ以上の前記移動対象可動リンクのそれぞれの先端部の目標支持位置／姿勢が、該移動対象可動リンクについての前記位置姿勢偏差がゼロである場合よりも前記基体に近づくように、該目標支持位置／姿勢を調整するように構成されていることが好ましい（第４発明）。

なお、前記位置姿勢偏差がゼロである場合というのは、前記偏差推定手段により推定した全ての可動リンク（下方側支持対象可動リンク又は移動対象可動リンク）についての位置及び姿勢の偏差がいずれもゼロである場合を意味する。

この第４発明によれば、前記基体に対する下方側支持対象可動リンクの先端部の相対的な目標位置／姿勢を上記の如く調整することで、基体の下部が構造物から離反する方向に動かされることとなるため、構造物に対する基体の姿勢を本来の適切な姿勢に近づけることができる。

さらに、前記基体の下部から延設された前記移動対象可動リンクの先端部の目標支持位置／姿勢を調整することで、該移動対象可動リンクの先端部が構造物に到達するときの該先端部の位置／姿勢を本来の適切な位置／姿勢に近づけることができる。また、該移動対象可動リンクの先端部が構造物に到達するまでの該先端部の移動軌道が、構造物に近づき過ぎないようにすることも可能となる。

従って、可動リンクと構造物との干渉等を回避することを高い信頼性で適切に行うことができる。

本発明の一実施形態における移動ロボットの構成を示す図。 実施形態の移動ロボットの動作制御に関する構成を示すブロック図。 図３（ａ），（ｂ）は実施形態の移動ロボットの昇降用動作の形態例を示す図。 図２に示す動作目標決定部の要部処理を示すブロック線図。 図２に示す動作目標決定部の要部処理を示すブロック線図。

本発明の一実施形態を図１〜図４を参照して以下に説明する。

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

各脚リンク３は、基体２の下部から延設されている。各脚リンク３は、大腿部１１、下腿部１２、足平部１３にそれぞれ相当する要素リンクを、基体２側から順番に、股関節１４、膝関節１５、足首関節１６を介して連結して構成されている。

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

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

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

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

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

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

補足すると、本実施形態では、各脚リンク３は６自由度の運動自由度を有するように構成されているが、７自由度以上の運動自由度を有するように構成されていてもよい。このことは、各腕リンク４についても同様である。また、各脚リンク３及び各腕リンク４のそれぞれは、回転型の関節に限らず、直動型の関節を含んでいてもよい。

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

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

次に、ロボット１に動作制御に関する構成を説明する。

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

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

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

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

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

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

関節アクチュエータ４１は、要素関節毎に備えられ、電動モータあるいは油圧アクチュエータにより構成される。

ハンド部駆動アクチュエータ４９は、ハンド部２３毎に備えられ、電動モータあるいは油圧アクチュエータにより構成される。なお、ハンド部駆動アクチュエータ４９は、各ハンド部２３毎に、複数のアクチュエータにより構成されていてもよい。

制御装置４０は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットにより構成され、上記の各センサの検出信号が入力される。

この制御装置４０は、実装されるプログラムを実行することにより実現される機能、又は、ハードウェア構成により実現される機能として、ロボット１の各関節の作動を規定する動作目標を逐次決定する動作目標決定部５１と、該動作目標に応じて関節アクチュエータ４１を制御する関節制御部５２とを備える。

動作目標決定部５１が逐次決定する動作目標は、本実施形態では、各脚リンク３の足平部１３及び各腕リンク４のハンド部２３のそれぞれの目標位置姿勢と、基体２の目標位置姿勢と、頭部５の目標姿勢とから構成される。

ここで、各足平部１３等、ロボット１の任意の部位の位置は、該部位のあらかじめ定められた代表点の位置、該部位の姿勢は、該部位の空間的な向きを意味する。

そして、ロボット１の任意の部位の目標位置姿勢は、該部位の位置の目標値（目標位置）と該部位の姿勢の目標値（目標姿勢）との組を意味する。

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

また、動作目標決定部５１が逐次決定する動作目標は、ロボット１の実際の動作状況に応じて基準の動作目標を適宜修正することで決定される。

ここで、基準の動作目標は、ロボット１に所要の運動を行わせるための基本指針の動作目標である。この基準の動作目標は、本実施形態では、各脚リンク３の足平部１３及び各腕リンク４のハンド部２３のそれぞれの基準の目標位置姿勢の軌道と、基体２の基準の目標位置姿勢の軌道と、頭部５の基準の目標姿勢の軌道とから構成される。なお、軌道は、瞬時値（所定の制御処理周期毎の瞬時値）の時系列を意味する。

該基準の動作目標は、ロボット１の実際の動作が、想定された環境下で該基準の動作目標通りに行われると仮定した場合に、該ロボット１が所要の運動を適切に行い得るように作成される。

関節制御部５２は、動作目標決定部５１が決定した動作目標（瞬時値）から、ロボット１の逆運動学の演算処理によって、ロボット１の各関節（要素関節）の目標変位量（回転角の目標値）を逐次決定する。そして、各関節の実際の変位量（関節変位センサ４３による検出値）を目標変位量に一致させるように関節アクチュエータ４１をフィードバック制御する。

さらに、関節制御部５２は、各腕リンク４のハンド部２３を後述する構造物に支持させる場合等に、該ハンド部２３の把持動作を行わせるように、ハンド部駆動アクチュエータ４９を制御する。

なお、動作目標決定部５１及び関節制御部５２は、それぞれ本発明における動作目標決定手段、関節制御手段に相当する。また、動作目標決定部５１は、本発明における偏差推定手段に相当する偏差推定部５３としての機能を含んでいる。

次に、本実施形態のロボット１の作動制御を具体的に説明する。以降の説明では、ロボット１に昇降対象の構造物の一例としての梯子を昇降させる（昇らせる又は降らせる）場合を主要例としてロボット１の作動制御を説明する。

ロボット１に梯子を昇降させる場合、制御装置４０は、カメラ４４の撮像画像等を基に、昇降対象の梯子の位置及び姿勢等に関する構造物情報を取得する。この構造物情報は、梯子の位置及び姿勢、梯子の長さ及び幅、梯子における各踏ざんの位置、各踏ざんの形状、外径等を特定し得る情報である。

なお、当該構造物情報の全部又は一部を、ロボット１の外部のサーバとの通信によって取得し、あるいは、ロボット１にあらかじめ記憶保持された地図データ等から取得することも可能である。

そして、制御装置４０は、動作目標決定部５１により、ロボット１の基準の動作目標を生成する。

この場合、動作目標決定部５１は、ロボット１が梯子を昇降する動作を開始する前のタイミング、あるいは、梯子の昇降途中の所定のタイミング等において、取得した構造物情報に基づいて認識される梯子を、ロボット１に昇降させるための基準の動作目標（現在時刻以後の所定の期間分の基準の動作目標）を、あらかじめ定められた規則に従って生成する。

この場合、動作目標決定部５１が生成する基準の動作目標は、例えば、両方の腕リンク４，４のハンド部２３，２３と、一方の脚リンク３の足平部１３とを梯子に接触させて支持させ、且つ、他方の脚リンク３の足平部１３を梯子から離反させた状態（空中に浮かせた状態）で、該他方の脚リンク３の足平部１３を目標支持位置姿勢に向かって移動させ、該目標支持位置姿勢にて該足平部１３を梯子の踏ざんに接触させて支持させるという昇降用動作（以降、脚リンク移動昇降用動作という）と、両方の脚リンク３，３の足平部１３，１３と、一方の腕リンク４のハンド部２３とを梯子に接触させて支持させ、且つ、他方の腕リンク４のハンド部２３を梯子から離反させた状態（空中に浮かせた状態）で、該他方の腕リンク４のハンド部２３を目標支持位置姿勢に向かって移動させ、該目標支持位置姿勢にて該ハンド部２３を梯子の踏ざん（又は支柱）に接触させて支持させるという昇降用動作（以降、腕リンク移動昇降用動作という）とを含むように生成される。

ここで、各足平部１３の目標支持位置姿勢は、該足平部１３を梯子に接触させて支持させるタイミングでの該足平部１３の目標位置姿勢を意味する。このことは、各ハンド部２３の目標支持位置姿勢についても同様である。

本実施形態では、各足平部１３の目標支持位置姿勢は、より詳しくは、該足平部１３の底面を梯子の踏ざんに接地させた状態（載せた状態）での該足平部１３の目標位置姿勢である。また、各ハンド部２３の目標支持位置姿勢は、より詳しくは、該ハンド部２３により梯子の踏ざん（又は支柱）を把持した状態での該ハンド部２３の目標位置姿勢を意味する。

以降の説明では、梯子に支持させる足平部１３を有する脚リンク３を支持対象脚リンク３、移動させる足平部１３を有する脚リンク３を移動対象脚リンク３という。そして、支持対象脚リンク３の足平部１３と移動対象脚リンク３の足平部１３とをそれぞれ、支持対象足平部１３、移動対象足平部１３ということがある。

同様に、梯子に支持させるハンド部２３を有する腕リンク４を支持対象腕リンク４、移動させるハンド部２３を有する腕リンク４を移動対象腕リンク４という。そして、支持対象腕リンク４のハンド部２３と移動対象腕リンク４のハンド部２３とをそれぞれ、支持対象ハンド部２３、移動対象ハンド部２３ということがある。

また、基準の動作目標における基体２の目標位置姿勢は、例えば、梯子の厚さ方向（梯子の支柱と踏みざんとにほぼ直交する方向）での基体２と梯子との間の距離がほぼ一定となり、また、基体２の体幹軸が、梯子の厚さ方向の表面（ロボット１の基体２寄りの面）とほぼ平行となる一定姿勢に保たれるように生成される。

ただし、基体２の目標位置姿勢は、基体２と梯子との間の距離、あるいは、梯子に対する基体２の姿勢等が時間的に変化するように生成してもよい。

また、基準の動作目標における頭部５の目標姿勢（基体２に対する相対的な目標姿勢）は、例えば基体２に対して一定の姿勢に維持されるように決定される。ただし、頭部５の目標姿勢は、基体２に対して頭部５の姿勢が時間的に変化するように生成してもよい。

図３（ａ）は、ロボット１が基準の動作目標に従って梯子Ａを昇る途中における脚リンク移動昇降用動作の一例形態を示している。図示例では、ロボット１は、両方の腕リンク４，４のハンド部２３，２３により梯子Ａの１つの踏ざんａ１を把持すると共に、ロボット１の一方の脚リンク３の足平部１３を１つの踏ざんａ２上に支持させた状態で、梯子から離反させた他方の脚リンク３の足平部１３を踏ざんａ２の１つ上の踏ざんａ３上の目標支持位置姿勢に向かって移動させ、該他方の脚リンク３の足平部１３を踏ざんａ３上に支持させようとする状態を表している。

また、図３（ａ）では、基体２の基準の目標姿勢は、例えば基体２の体幹軸が鉛直方向を向く姿勢とされている。

なお、図３（ａ）（あるいは、後述の図３（ｂ））において、参照符号ｂを付したものは、梯子Ａの両側の支柱のうちの片側の支柱を示している。また、図３（ａ），（ｂ）では、ロボット１の頭部５の記載を省略している。

補足すると、ロボット１に梯子Ａ等の構造物を昇降させる場合の基準の動作目標は、前記脚リンク移動昇降用動作及び腕リンク移動昇降用動作の代わりに、あるいは、脚リンク移動昇降用動作及び腕リンク移動昇降用動作に加えて、一方の腕リンク４のハンド部２３と、一方の脚リンク３の足平部１３とを梯子に接触させて支持させ、且つ、他方の腕リンク４のハンド部２３と他方の脚リンク３の足平部１３とを梯子から離反させた状態で、当該他方の腕リンク４のハンド部２３と当該他方の脚リンク３の足平部１３とをそれぞれの目標支持位置姿勢に向かって移動させ、各目標支持位置姿勢にて該ハンド部２３及び該足平部１３を梯子に接触させて支持させるという昇降用動作（以降、脚リンク・腕リンク移動昇降用動作という）を含んでいてもよい。

また、基準の動作目標は、あらかじめ作成したものをロボット１の記憶装置に記憶保持しておいてもよい。あるいは、外部のサーバで基準の動作目標を作成し、その動作目標を、外部のサーバから随時、無線通信によってロボット１に転送するようにしてもよい。

また、基準の動作目標は、ロボット１による梯子Ａ等の構造物の昇降を行いながら、リアルタイムで逐次生成するようにしてもよい。

なお、以降の本実施形態の説明は、脚リンク移動昇降用動作と腕リンク移動昇降用動作とをロボット１に行わせる場合を主要例として説明するが、脚リンク・腕リンク移動昇降用動作を行う場合についても補足的な説明を行う。

上記脚リンク移動昇降用動作、腕リンク移動昇降用動作、及び脚リンク・腕リンク移動昇降用動作は、本発明における昇降用動作に相当する。なお、前記第３発明における昇降用動作に相当するものは、脚リンク移動昇降用動作、腕リンク移動昇降用動作、及び脚リンク・腕リンク移動昇降用動作である。

そして、脚リンク移動昇降用動作では、両方の腕リンク４，４が本発明における上方側支持対象可動リンクに相当し、脚リンク３，３のうちの一方である支持対象脚リンク３と他方である移動対象脚リンク３とがそれぞれ本発明における下方側支持対象可動リンク、移動対象可動リンクに相当する。

また、腕リンク移動昇降用動作では、両方の脚リンク３，３が本発明における下方側支持対象可動リンクに相当し、腕リンク４，４のうちの一方である支持対象腕リンク４と他方である移動対象腕リンク４とがそれぞれ本発明における上方側支持対象可動リンク、移動対象可動リンクに相当する。

また、脚リンク・腕リンク移動昇降用動作では、両方の腕リンク４，４の一方である支持対象腕リンク４と両方の脚リンク３，３の一方である支持対象脚リンク３とがそれぞれ本発明における上方側支持対象可動リンク、下方側支持対象可動リンクに相当し、両方の腕リンク４，４の他方である移動対象腕リンク４と両方の脚リンク３，３の他方である移動対象脚リンク３とが本発明における移動対象可動リンクに相当する。

次に動作目標決定部５１は、基準の動作目標を用いて、ロボット１の実際の動作制御のための動作目標（関節制御部５２に出力する動作目標）を所定の制御処理周期で逐次決定する。このとき、関節制御部５２が、当該動作目標に従ってロボット１の各関節アクチュエータ４１を制御する（ひいては各要素関節の変位量を制御する）と共に各ハンド部駆動アクチュエータ４９を制御する（ひいては各ハンド部２３の把持動作を制御する）ことで、ロボット１の実際の動作（梯子の昇降）が行われる。

この場合、動作目標決定部５１が各制御処理周期における動作目標を決定する処理は、以下に説明する如く実行される。

ここで、ロボット１が動作目標に従って梯子の昇降を行っている状況を想定する。この状況において、図３（ａ）に例示されるように、ロボット１の全体重心Ｇは、梯子Ａから水平方向に離反した状態となる。このため、ロボット１には、その全体重心Ｇに作用する重力に起因するモーメント（主にピッチ方向のモーメント）が作用し易い。

なお、本実施形態の説明において、ピッチ方向は、ロボット１のほぼ左右方向に延在する水平軸周りの方向、ロール方向は、ロボット１のほぼ前後方向に延在する水平軸周りの方向、ヨー方向はほぼ上下方向（鉛直方向）に延在する軸周りの方向である。

そして、特に、ロボット１の両方の脚リンク３，３のうちの一方の脚リンク３（支持対象脚リンク３）の足平部１３だけを梯子に支持させる前記脚リンク移動昇降用動作（又は前記脚リンク・腕リンク移動昇降用動作）においては、上記モーメントに起因して、支持対象ハンド部２３及び支持対象足平部１３の梯子に対する滑りが発生しやすい。

なお、ロボット１の両方の脚リンク３，３の足平部１３，１３を梯子に支持させる前記腕リンク移動昇降用動作においても、上記モーメントに起因して、支持対象ハンド部２３及び支持対象足平部１３の梯子に対する滑りが生じる場合もある。

そして、かかる滑りが発生すると、梯子に支持させたハンド部２３（支持対象ハンド部２３）に該梯子から作用する荷重の全体の作用中心点を支点（回転中心）として、ロボット１の全体の回転（主に、ピッチ方向の回転）が発生する。ひいては、ロボット１の全体の姿勢が動作目標の姿勢からずれることとなる。

図３（ｂ）は、例えば脚リンク移動昇降用動作の実行中に、支持対象ハンド部２３，２３及び支持対象足平部１３の滑りが発生し、ひいては、ロボット１の回転が発生した状況を例示している。この図示例は、角度θだけ、ロボット１が動作目標の状態からピッチ方向に回転した状態である。ロボット１に梯子Ａの如き構造物を昇降させる場合には、上記の如くロボット１が動作目標の状態から回転する状況がしばしば発生する。

また、腕リンク４，４のうちの一方の腕リンク４，４のハンド部２３だけを梯子Ａ等の構造物に支持させる腕リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中には、ロボット１が動作目標の状態からロール方向あるいはヨー方向等に回転する状況が発生する場合もある。

なお、脚リンク移動昇降用動作、腕リンク移動昇降用動作、脚リンク・腕リンク移動昇降用動作のいずれの昇降用動作でも、梯子Ａ等の構造物に支持させたハンド部２３もしくは足平部１３等の弾性変形に応じて、ロボット１が動作目標の状態から回転する状況が発生する場合もある。

このようにロボット１の回転が生じると、基体２の下部が梯子に近づくために、支持対象脚リンク３あるいは移動対象脚リンク３あるいは移動対象腕リンク４と梯子との干渉が生じやすくなる。

そこで、本実施形態では、動作目標決定部５１は、脚リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中に、支持対象足平部１３の実際の位置姿勢と目標位置姿勢との偏差を逐次推定し、それに応じて、動作目標を基準の動作目標から適宜修正するように、各制御処理周期での動作目標を決定する。

また、本実施形態では、動作目標決定部５１は、腕リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中に、移動対象ハンド部２３の実際の位置姿勢と目標位置姿勢との偏差を推定し、それに応じて動作目標を基準の動作目標から適宜修正するように、各制御処理周期での動作目標を決定する。

具体的には、動作目標決定部５１は、脚リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中の各制御処理周期において、図４のブロック図に示す処理を実行することで、ロボット１の動作目標を基準の動作目標から修正するための操作量（以降、回転対応操作量という）を決定する。

また、動作目標決定部５１は、腕リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中の各制御処理周期において、図５のブロック図に示す処理を実行することで、回転対応操作量を決定する。

なお、脚リンク・腕リンク移動昇降用動作では、動作目標決定部５１は、図４及び図５のブロック図に示す処理の一方又は両方を実行する。図４及び図５のブロック図に示す処理の両方を実行する場合には、それぞれの処理により決定した回転対応操作量を合成してなる操作量に応じて動作目標の修正が行われる。

まず、図４のブロック図に示す処理を説明する。図４を参照して、動作目標決定部５１は、脚リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中の各制御処理周期において、姿勢センサ４２の出力により示される基体２の現在の姿勢の検出値と、各腕リンク４の力センサ４６の出力により示される各ハンド部２３の現在の荷重（各ハンド部２３がこれを支持させた梯子から受ける並進力及びモーメント）の検出値と、前回の制御処理周期で決定した支持対象足平部１３の目標位置姿勢である支持対象足平部前回目標位置姿勢と、前回の制御処理周期で決定した各ハンド部２３の目標位置姿勢であるハンド部前回目標位置姿勢と、前回の制御処理周期で決定した基体２の目標姿勢である基体前回目標姿勢とを取得する。

上記支持対象足平部前回目標位置姿勢と、ハンド部前回目標位置姿勢と、基体前回目標姿勢とは、現在時刻以前の最新の目標値を意味する。

なお、２つの腕リンク４，４のうちの一方の腕リンク４のハンド部２３だけを梯子に支持させる脚リンク・腕リンク移動昇降用動作を実行する場合には、ハンド部２３の荷重の検出値と、ハンド部前回目標位置姿勢とに関しては、梯子に支持させている一方の腕リンク４についてのみ、ハンド部２３の荷重の検出値と、ハンド部前回目標位置姿勢とを取得するようにしてもよい。

そして、動作目標決定部５１は、基体姿勢偏差算出部６１の処理と回転中心決定部６２の処理とを実行する。

基体姿勢偏差算出部６１の処理では、動作目標決定部５１は、基体前回目標姿勢と基体２の現在の姿勢の検出値との偏差（回転角偏差）である基体姿勢偏差を算出する。

なお、本実施形態では、各制御処理周期での基体の目標位置姿勢は、該制御処理周期の時刻での基体２の基準の目標位置姿勢に一致するように決定される。従って、基体姿勢偏差は、基体前回目標姿勢の代わりに、前回の制御処理周期の時刻での基体２の基準の目標姿勢を用いて算出してもよい。

回転中心決定部６２の処理では、動作目標決定部５１は、各ハンド部２３の荷重の検出値と各ハンド部２３についてのハンド部前回目標位置姿勢とから、両ハンド部２３，２３にそれぞれ作用する荷重の全体（合成荷重）の作用中心点（荷重分布の重心点）の位置を算出し、その位置を、ロボット１の回転中心として決定する。

この場合、脚リンク移動昇降用動作では、梯子に対する一方のハンド部２３の接触面（把持箇所）と、他方のハンド部２３の接触面（把持箇所）との間の点が回転中心として決定される。また、脚リンク・腕リンク移動昇降用動作では、両ハンド部２３のうち、梯子に支持させたハンド部２３の接触面内の点が回転中心として決定される。

次に、動作目標決定部５１は、支持対象足平部１３の実際の位置姿勢である支持対象足平部実位置姿勢を推定する支持対象足平部実位置姿勢推定部６３の処理を実行する。

この支持対象足平部実位置姿勢推定部６３の処理では、動作目標決定部５１は、支持対象足平部前回目標位置姿勢を、回転中心決定部６２の処理で決定した回転中心の周りに、基体姿勢偏差の回転角だけ、仮想的に回転させてなる位置姿勢を、支持対象足平部実位置姿勢として推定する。

これにより、基体姿勢偏差が、回転中心決定部６２の処理で決定した回転中心の周りのロボット１の回転によって発生したものとみなして、支持対象足平部実位置姿勢が推定される。

次いで、動作目標決定部５１は、支持対象足平部前回目標位置姿勢と支持対象足平部実位置姿勢の推定値との偏差（以降、支持対象足平部位置姿勢偏差という）を偏差演算部６４で算出する。

以上の基体姿勢偏差算出部６１、回転中心決定部６２、支持対象足平部実位置姿勢推定部６３及び偏差演算部６４の処理によって、支持対象足平部位置姿勢偏差の推定値が所定の制御処理周期で逐次算出される。

なお、図４のブロック図の処理では、基体姿勢偏差算出部６１、回転中心決定部６２、支持対象足平部実位置姿勢推定部６３及び偏差演算部６４によって、本発明における偏差推定手段に相当する偏差推定部５３が構成される。そして、上記支持対象足平部位置姿勢偏差が本発明における位置姿勢偏差に相当する。

動作目標決定部５１は、以上の如く算出した支持対象足平部位置姿勢偏差の推定値を、ノイズ等に起因する高周波成分を除去するためのフィルタリング処理を実行するローパスフィルタ６５に入力し、さらに、該ローパスフィルタ６５の出力を、リミッタ６６に入力することで、支持対象足平部位置姿勢偏差の推定値を所定の上限値及び下限値の間の範囲内に制限するリミット処理を実行する。

そして、動作目標決定部５１は、このようにローパスフィルタ６５及びリミッタ６６の処理を施した後の支持対象足平部位置姿勢偏差の推定値に応じて、ロボット１の動作目標を基準の動作目標から修正するための回転対応操作量（補正量）を決定する処理を操作量決定部６７により実行する。

操作量決定部６７は、本実施形態では、各脚リンク３又は各腕リンク４と梯子との干渉が発生するのを防止しつつ、基体２の実際の姿勢が目標姿勢から乖離するのを抑制するように、１つ以上の可動リンクの先端部（足平部１３又はハンド部２３）の目標位置／姿勢（目標位置及び目標姿勢のうちの少なくともいずれか一方）を調整する（基準の動作目標から修正する）ための操作量を回転対応操作量として決定する。

この場合、操作量決定部６７は、支持対象足平部位置姿勢偏差の推定値に応じた量だけ、基体２に対する支持対象足平部１３の相対位置／姿勢（相対位置及び相対姿勢のうちの少なくともいずれか一方）と、移動対象足平部１３の目標支持位置／姿勢（目標支持位置及び目標支持姿勢のうちの少なくともいずれか一方）とのうちの少なくともいずれか一方を、基準の動作目標における位置／姿勢からずらすにように、回転対応操作量を決定する。

そのため、操作量決定部６７は、支持対象足平部１３の目標位置／姿勢の補正量（現在の制御処理周期で決定する目標位置／姿勢の補正量）と、移動対象足平部１３の到達目標の位置たる目標支持位置／姿勢の補正量とのうちの少なくともいずれか一方を、上記回転対応操作量として決定する。

なお、かかる回転対応補正量は、支持対象足平部位置姿勢偏差の推定値から、あらかじめ定められた演算式又はマップ等により決定される。また、支持対象足平部位置姿勢偏差の推定値がゼロ（もしくはほぼゼロ）である場合には、該回転対応操作量はゼロとされる。

例えば、図３（ｂ）に示すように、支持対象ハンド部２３，２３及び支持対象足平部１３の滑りによって、基体２の下端部が、梯子Ａにより近づくように、基体２が動作目標の姿勢からピッチ方向に傾いた状態を想定する。この状態では、支持対象足平部１３の実際の位置は、目標位置よりも梯子Ａの厚さ方向で奥側（ロボット１から見て奥側）にずれた状態となる。

このような状態では、操作量決定部６７は、支持対象脚リンク３と基体２との連結部（股関節１４）と、支持対象足平部１３との間の距離が基準の動作目標における当該距離よりも大きくなるように（ひいては、基体２の下端部を梯子Ａから離すように）、支持対象足平部１３の目標位置／姿勢の補正量（現在の制御処理周期での補正量）を決定する。該補正量は、支持対象足平部位置姿勢偏差の推定値の大きさ（位置偏差及び姿勢偏差の各成分の大きさ）の度合に応じて決定される。

さらに操作量決定部６７は、移動対象足平部１３の到達目標の位置姿勢たる目標支持位置姿勢での足平部１３を、基準の動作目標における目標支持位置姿勢での足平部１３よりも基体２に近づけるように、移動対象脚リンク３の足平部１３の目標支持位置／姿勢の補正量を決定する。該補正量は、支持対象足平部位置姿勢偏差の推定値の大きさ（位置偏差及び姿勢偏差の各成分の大きさ）の度合に応じて決定される。

このように、支持対象足平部１３の目標位置／姿勢の補正量と、移動対象足平部１３の目標支持位置／姿勢の補正量とが回転対応操作量として、支持対象足平部位置姿勢偏差の推定値に応じて決定される。

なお、支持対象足平部位置姿勢偏差の推定値の大きさ（位置偏差及び姿勢偏差の各成分の大きさ）の度合等によって、支持対象足平部１３の目標位置／姿勢と、移動対象足平部１３の目標支持位置／姿勢とのいずれか一方だけの補正量を決定する（他方の補正量をゼロとする）ようにしてもよい。

また、脚リンク・腕リンク移動昇降用動作においては、支持対象足平部１３の目標位置／姿勢と、移動対象足平部１３の目標支持位置／姿勢とのうちの一方又は両方の補正量に加えて、あるいは、該一方又は両方の補正量の代わりに、移動対象腕リンク４の目標支持位置／姿勢の補正量を回転対応操作量として、支持対象足平部位置姿勢偏差の推定値に応じて決定することも可能である。

次に、図５のブロック図に示す処理を説明する。なお、この処理は、図４のブロック図の処理と一部の処理だけが相違するので、図４のブロック図の処理と同一の事項については、詳細な説明を省略する。

図５を参照して、動作目標決定部５１は、腕リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中の各制御処理周期において、姿勢センサ４２の出力により示される基体２の現在の姿勢の検出値と、支持対象腕リンク４の力センサ４６の出力により示される支持対象ハンド部２３の現在の荷重の検出値と、前回の制御処理周期で決定した移動対象ハンド部２３の目標位置姿勢である移動対象ハンド部前回目標位置姿勢と、前回の制御処理周期で決定した支持対象ハンド部２３の目標位置姿勢である支持対象ハンド部前回目標位置姿勢と、前回の制御処理周期で決定した基体２の目標姿勢である基体前回目標姿勢とを取得する。

上記支持対象ハンド部前回目標位置姿勢と、移動対象ハンド部前回目標位置姿勢と、基体前回目標姿勢とは、現在時刻以前の最新の目標値を意味する。

そして、動作目標決定部５１は、基体姿勢偏差算出部７１の処理と回転中心決定部７２の処理とを実行する。

基体姿勢偏差算出部７１の処理は、図４の基体姿勢偏差算出部６１の処理と同じである。

また、回転中心決定部７２の処理では、動作目標決定部５１は、支持対象ハンド部２３の荷重の検出値と支持対象ハンド部前回目標位置姿勢とから、支持対象ハンド部２３に作用する荷重の全体の作用中心点（荷重分布の重心点）の位置を算出し、その位置を、ロボット１の回転中心として決定する。この場合、支持対象ハンド部２３の接触面内の点が回転中心として決定される。

次に、動作目標決定部５１は、移動対象ハンド部２３の実際の位置姿勢である移動対象ハンド部実位置姿勢を推定する移動対象ハンド部実位置姿勢推定部７３の処理を実行する。

この移動対象ハンド部実位置姿勢推定部７３の処理では、動作目標決定部５１は、移動対象ハンド部前回目標位置姿勢を、回転中心決定部７２の処理で決定した回転中心の周りに、基体姿勢偏差の回転角だけ、仮想的に回転させてなる位置姿勢を、移動対象ハンド部実位置姿勢として推定する。

これにより、基体姿勢偏差が、回転中心決定部７２の処理で決定した回転中心の周りのロボット１の回転によって発生したものとみなして、移動対象ハンド部実位置姿勢が推定される。

次いで、動作目標決定部５１は、移動対象ハンド部前回目標位置姿勢と移動対象ハンド部実位置姿勢の推定値との偏差（以降、移動対象ハンド部位置姿勢偏差という）を偏差演算部７４で算出する。

以上の基体姿勢偏差算出部７１、回転中心決定部７２、移動対象ハンド部実位置姿勢推定部７３及び偏差演算部７４の処理によって、移動対象ハンド部位置姿勢偏差の推定値が所定の制御処理周期で逐次算出される。

なお、図５のブロック図の処理では、基体姿勢偏差算出部７１、回転中心決定部７２、移動対象ハンド部実位置姿勢推定部７３及び偏差演算部７４によって、本発明における偏差推定手段に相当する偏差推定部５３が構成される。そして、上記移動対象ハンド部位置姿勢偏差が本発明における位置姿勢偏差に相当する。

動作目標決定部５１は、以上の如く算出した移動対象ハンド部位置姿勢偏差の推定値を、ノイズ等に起因する高周波成分を除去するためのフィルタリング処理を実行するローパスフィルタ７５に入力し、さらに、該ローパスフィルタ７５の出力を、リミッタ７６に入力することで、移動対象ハンド部位置姿勢偏差の推定値を所定の上限値及び下限値の間の範囲内に制限するリミット処理を実行する。

そして、動作目標決定部５１は、このようにローパスフィルタ７５及びリミッタ７６の処理を施した後の移動対象ハンド部位置姿勢偏差の推定値に応じて、ロボット１の動作目標を基準の動作目標から修正するための回転対応操作量（補正量）を決定する処理を操作量決定部７７により実行する。

操作量決定部７７は、図４の操作量決定部６７と同様に、各脚リンク３又は各腕リンク４と梯子との干渉が発生するのを防止しつつ、基体２の実際の姿勢が目標姿勢から乖離するのを抑制するように、１つ以上の可動リンクの先端部（足平部１３又はハンド部２３）の目標位置／姿勢（目標位置及び目標姿勢のうちの少なくともいずれか一方）を調整する（基準の動作目標から修正する）ための操作量を回転対応操作量として決定する。

この場合、操作量決定部７７は、移動対象ハンド部位置姿勢偏差の推定値に応じた量だけ、基体２に対する一方又は両方の支持対象脚リンク３の足平部１３の相対位置／姿勢（相対位置及び相対姿勢のうちの少なくともいずれか一方）と、移動対象腕リンク４のハンド部２３の目標支持位置／姿勢（目標支持位置及び目標支持姿勢のうちの少なくともいずれか一方）とのうちの少なくともいずれか一方を、基準の動作目標における位置／姿勢からずらすにように、回転対応操作量を決定する。

なお、かかる回転対応補正量は、移動対象ハンド部位置姿勢偏差の推定値から、あらかじめ定められた演算式又はマップ等により決定される。また、移動対象ハンド部位置姿勢偏差の推定値がゼロ（もしくはほぼゼロ）である場合には、該回転対応操作量はゼロとされる。

例えば腕リンク移動昇降用動作において、基体２が、移動対象腕リンク４側の下部が梯子に近づくようにヨー方向に回転することで、移動対象ハンド部位置姿勢偏差が発生したような状況で、移動対象腕リンク４と同じ側（右側又は左側）の片方の支持対象脚リンク３と基体２との連結部（股関節１４）と、支持対象足平部１３との間の距離が基準の動作目標における当該距離よりも大きくなるように（ひいては、基体２と、当該片方の支持対象脚リンク３との連結部を梯子Ａから離すように）、支持対象足平部１３の目標位置／姿勢の補正量（現在の制御処理周期での補正量）を決定する。該補正量は、移動対象ハンド部位置姿勢偏差の推定値の大きさ（位置偏差及び姿勢偏差の各成分の大きさ）の度合に応じて決定される
さらに操作量決定部６７は、移動対象ハンド部２３の到達目標の位置姿勢たる目標支持位置姿勢でのハンド部２３を、基準の動作目標における目標支持位置姿勢でのハンド部２３よりも基体２に近づけるように、移動対象ハンド部２３の目標支持位置／姿勢の補正量を決定する。該補正量は、移動対象ハンド部位置姿勢偏差の推定値の大きさ（位置偏差及び姿勢偏差の各成分の大きさ）の度合に応じて決定される。

このように、支持対象足平部１３の目標位置／姿勢の補正量と、移動対象ハンド部２３の目標支持位置／姿勢の補正量とが回転対応操作量として、移動対象ハンド部位置姿勢偏差の推定値に応じて決定される。

なお、移動対象ハンド部位置姿勢偏差の推定値の大きさ（位置偏差及び姿勢偏差の各成分の大きさ）の度合等によって、支持対象足平部１３の目標位置／姿勢と、移動対象ハンド部２３の目標支持位置／姿勢とのいずれか一方だけの補正量を決定する（他方の補正量をゼロとする）ようにしてもよい。

また、両方の脚リンク３，３の足平部１３，１３を梯子に支持させる腕リンク移動昇降用動作においては、両方の支持対象足平部１３，１３の目標位置／姿勢の補正量を回転対応操作量として決定することも可能である。

また、脚リンク・腕リンク移動昇降用動作においては、支持対象足平部１３の目標位置／姿勢と、移動対象ハンド部２３の目標支持位置／姿勢とのうちの一方又は両方の補正量に加えて、あるいは、該一方又は両方の補正量の代わりに、移動対象脚リンク３の目標支持位置／姿勢の補正量を回転対応操作量として、移動対象ハンド部位置姿勢偏差の推定値に応じて決定することも可能である。

補足すると、脚リンク・腕リンク移動昇降用動作においては、図４及び図５の両方の処理により回転対応操作量を決定する態様と、図４及び図５のいずれか一方の処理のみにより回転対応操作量を決定する態様とを採用できる。この場合、いずれの態様で回転対応操作量を決定するかは、例えば、支持対象足平部位置姿勢偏差もしくは移動対象ハンド部位置姿勢偏差の推定値の大きさ（位置偏差及び姿勢偏差の各成分の大きさ）の度合等に応じて選択することが可能である。

動作目標決定部５１は、脚リンク移動昇降用動作及び腕リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中に、上記のように操作量決定部６７又は７７により決定した回転対応操作量（補正量）を用いて基準の動作目標を修正することで、各制御処理周期における動作目標を逐次決定する。

この場合、脚リンク移動昇降用動作、腕リンク移動昇降用動作及び脚リンク・腕リンク移動昇降用動作のいずれの昇降用動作においても、動作目標決定部５１は、各制御処理周期での基体２の目標位置姿勢と頭部５の目標姿勢とを基準の動作目標に一致させる。

また、動作目標決定部５１は、脚リンク移動昇降用動作においては、各制御処理周期での両方の腕リンク４，４のハンド部２３，２３（支持対象ハンド部２３，２３）のそれぞれの目標位置姿勢を現状の位置姿勢（換言すれば、該ハンド部２３，２３を梯子に支持（把持）させたときの目標支持位置姿勢）に保持する。

そして、脚リンク移動昇降用動作においては、動作目標決定部５１は、各制御処理周期での支持対象足平部１３の目標位置姿勢を、操作量決定部６７で決定した回転対応操作量（補正量）によって、基準の動作目標における位置姿勢から補正してなる位置姿勢に決定する。

さらに、脚リンク移動昇降用動作においては、動作目標決定部５１は、現在時刻以後の移動対象足平部１３の目標位置姿勢の軌道を、操作量決定部６７で決定した回転対応操作量（補正量）によって、基準の動作目標における目標支持位置姿勢から補正してなる目標支持位置姿勢に向かって変化させるように各制御処理周期での移動対象脚リンク３の足平部１３の目標位置姿勢を決定する。

また、腕リンク移動昇降用動作においては、動作目標決定部５１は、各制御処理周期での支持対象ハンド部２３の目標位置姿勢を現状の位置姿勢（換言すれば、該ハンド部２３を梯子に支持（把持）させたときの目標支持位置姿勢）に保持する。

そして、腕リンク移動昇降用動作においては、動作目標決定部５１は、現在時刻以後の移動対象腕リンク４のハンド部２３の目標位置姿勢の軌道を、操作量決定部７７で決定した回転対応操作量（補正量）によって、基準の動作目標における目標支持位置姿勢から補正してなる目標支持位置姿勢に向かって変化させるように各制御処理周期での移動対象ハンド部２３の目標位置姿勢を決定する。

さらに、腕リンク移動昇降用動作においては、動作目標決定部５１は、各制御処理周期での補正対象の１つ又は２つの支持対象足平部１３の目標位置姿勢を、操作量決定部７７で決定した回転対応操作量（補正量）によって、基準の動作目標における位置姿勢から補正してなる位置姿勢に決定する。また、動作目標決定部５１は、腕リンク移動昇降用動作において補正対象でない支持対象脚リンク３の足平部１３の目標位置姿勢を、基準の動作目標における目標位置姿勢に一致させる。

また、脚リンク・腕リンク移動昇降用動作においては、動作目標決定部５１は、両方の腕リンク４，４のハンド部２３，２３うち、梯子に支持（把持）させた支持対象ハンド部２３の目標位置姿勢を現状の位置姿勢（換言すれば、該ハンド部２３を梯子に支持（把持）させたときの目標支持位置姿勢）に保持する。

また、動作目標決定部５１は、脚リンク・腕リンク移動昇降用動作において、図５の操作量決定部７７により移動対象ハンド部２３の目標支持位置姿勢についての回転対応操作量を決定した場合には、現在時刻以後の移動対象ハンド部２３の目標位置姿勢の軌道を、操作量決定部７７で決定した回転対応操作量（補正量）によって、基準の動作目標における目標支持位置姿勢から補正してなる目標支持位置姿勢に向かって変化させるように各制御処理周期での移動対象ハンド部２３の目標位置姿勢を決定する。

また、動作目標決定部５１は、脚リンク・腕リンク移動昇降用動作において、図４の操作量決定部６７により移動対象足平部１３の目標支持位置姿勢についての回転対応操作量を決定した場合には、現在時刻以後の移動対象足平部１３の目標位置姿勢の軌道を、操作量決定部６７で決定した回転対応操作量（補正量）によって、基準の動作目標における目標支持位置姿勢から補正してなる目標支持位置姿勢に向かって変化させるように各制御処理周期での移動対象足平部１３の目標位置姿勢を決定する。

また、動作目標決定部５１は、脚リンク・腕リンク移動昇降用動作において、図４の操作量決定部６７又は図５の操作量決定部７７により支持対象足平部１３の目標位置姿勢についての回転対応操作量を決定した場合には、各制御処理周期における支持対象足平部１３の目標位置姿勢を、操作量決定部６７又は７７で決定した回転対応操作量（補正量）によって、基準の動作目標における目標位置姿勢から補正してなる位置姿勢に決定する。

また、脚リンク・腕リンク移動昇降用動作において、図４の操作量決定部６７及び図５の操作量決定部７７の両方により、支持対象足平部１３の目標位置姿勢についての回転対応操作量を決定した場合には、動作目標決定部５１は、支持対象足平部１３の目標位置姿勢を、操作量決定部６７，７７の両方の回転操作量を合成してなる操作量によって、基準の動作目標における目標位置姿勢から補正してなる位置姿勢に決定する。

なお、脚リンク移動昇降用動作、腕リンク移動昇降用動作及び脚リンク・腕リンク移動昇降用動作のいずれの昇降用動作においても、支持対象足平部１３の目標位置姿勢についての回転操作量がゼロである場合には、該支持対象足平部１３の目標位置姿勢は、基準の動作目標における目標位置姿勢に一致するように決定される。

同様に、いずれの昇降用動作においても、移動対象足平部１３の目標支持位置姿勢についての回転操作量がゼロである場合、あるいは、移動対象ハンド部２３の目標支持位置姿勢についての回転操作量がゼロである場合には、それぞれ、移動対象足平部１３の目標支持位置姿勢、移動対象ハンド部２３の目標支持位置姿勢が、基準の動作目標における目標支持位置姿勢に一致するように決定される。

また、ロボット１による梯子の昇降時に、脚リンク移動昇降用動作又は腕リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中の期間とそれ以外の期間とのいずれにおいても、前記回転対応操作量とは別の操作量（例えばコンプライアンス制御用の操作量等）をさらに算出し、当該別の操作量に応じた基準の動作目標の補正をさらに行うことで、各制御処理周期での動作目標を決定するようにしてもよい。

本実施形態では、以上の如く決定される動作目標に従って、関節制御部５２によりロボット１の各関節（要素関節）の変位量が関節アクチュエータ４１を介して制御される。さらに、各ハンド部２３を梯子に支持させる場合には、該ハンド部２３により梯子を把持するように該ハンド部２３の把持動作がハンド部駆動アクチュエータ４９を介して制御される。

この場合、脚リンク移動昇降用動作及び腕リンク移動昇降用動作（又は脚リンク・腕リンク移動昇降用動作）の実行中に、支持対象ハンド部２３及び支持対象足平部１３の滑り、あるいは、支持対象腕リンク４もしくは支持対象脚リンク３の弾性変形等によって、基体姿勢偏差が発生しても、１つ又は２つの支持対象足平部１３の目標位置／姿勢と、移動対象足平部１３もしくは移動対象ハンド部２３の目標支持位置／姿勢とのうちの少なくともいずれか一方の補正量が回転対応操作量として、支持対象足平部位置姿勢偏差の推定値もしくは移動対象ハンド部位置姿勢偏差の推定値に応じて可変的に決定されることで、各脚リンク３又は各腕リンク４と梯子との干渉を防止することができる。

例えば図３（ｂ）に示す状況で、基体２の姿勢の目標姿勢からの乖離を低減する（ひいては、基体２の下部が梯子に近づき過ぎるのが抑制される）ように支持対象脚リンク３が伸展されることとなる。

さらに、目標支持位置姿勢に到達したときの移動対象足平部１３が基体２に近づくように、該移動対象足平部１３の目標支持位置／姿勢が修正される。ひいては、移動対象足平部１３の実際の支持位置姿勢は、基準の動作目標で予定されていた適切な目標支持位置から乖離するのが防止される。あるいは、目標支持位置姿勢に移動した移動対象足平部１３が梯子に対して非接地状態となる（踏み外す）ようなことが防止される。

このため、各脚リンク３の下腿部１２等が梯子の踏ざんに干渉することを防止することができる。ひいては、ロボット１が梯子を昇降することを円滑に行うことができる。

なお、図３（ｂ）に示す状況に限らず、支持対象ハンド部２３及び支持対象足平部１３の滑り、あるいは、支持対象腕リンク４もしくは支持対象脚リンク３の弾性変形等によって、基体姿勢偏差が発生した場合には、各脚リンク３又は各腕リンク４と梯子との干渉を防止しつつ、ロボット１に梯子を昇降させることができる。

また、本実施形態では、支持対象ハンド部２３にそれぞれ作用する荷重の全体の作用中心点をロボット１の回転中心とし、この回転中心と基体姿勢偏差とに応じて支持対象足平部実位置姿勢あるいは移動対象ハンド部実位置姿勢を推定するので、その推定を簡易な演算処理で適切に行うことができる。

次に、以上説明した実施形態に関連する変形態様をいくつか説明する。

前記実施形態では、図４のブロック図の処理において、前記回転中心と基体姿勢偏差とに応じて支持対象足平部実位置姿勢を推定するようにしたが、各脚リンク３に備えた力センサ４５による足平部１３の荷重検出値（並進力及びモーメントの検出値）に基づいて、支持対象足平部実位置姿勢を推定することも可能である。すなわち、支持対象脚リンク３の力センサ４５による支持対象足平部１３の荷重検出値から、該支持対象足平部１３の梯子に対する接触位置及び姿勢が判るので、該支持対象足平部１３の実際の位置及び姿勢を推定できる。

さらに、該支持対象足平部１３の実際の位置及び姿勢の推定値と、前記関節変位センサ４３による各関節（要素関節）の変位量の検出値とから、移動対象足平部１３もしくは移動対象ハンド部２３の実際の位置及び姿勢を推定することも可能である。

また、前記実施形態では、ロボット１が昇降する構造物として梯子を例にとって説明したが、該構造物は、脚立、急峻な傾斜を有する階段、凹凸を有する壁等であってもよい。

また、前記実施形態では、各腕リンク４の先端部たるハンド部２３を構造物（梯子）に支持させる場合に、該構造物の部分（前記実施形態では踏ざん）をハンド部２３に把持させるようにしたが、例えば、各腕リンク４の先端部を構造物の部分に引っかけるようして該構造物に支持させるようにすることも可能である。

また、前記実施形態では、図４のブロック図の処理において、支持対象足平部位置姿勢偏差の推定値に応じて回転対応操作量を決定したが、移動対象ハンド部２３又は移動対象足平部１３についての位置姿勢偏差の推定値に応じて、あるいは、これらの位置姿勢偏差の推定値と、支持対象足平部位置姿勢偏差の推定値とに応じて回転対応操作量を決定してもよい。

また、前記実施形態では、図５のブロック図の処理において、移動対象ハンド部位置姿勢偏差の推定値に応じて回転対応操作量を決定したが、移動対象足平部１３又は支持対象足平部１３についての位置姿勢偏差の推定値に応じて、、又は、支持対象足平部１３についての位置姿勢偏差の推定値に応じて、あるいは、これらの位置姿勢偏差の推定値と移動対象ハンド部位置姿勢偏差の推定値とに応じて回転対応操作量を決定してもよい。

また、前記実施形態では、脚リンク移動昇降用動作、腕リンク移動昇降用動作、又は脚リンク・腕リンク移動昇降用動作において、構造物に支持させた支持対象ハンド部２３の目標位置／姿勢を修正することは行わないものとしたが、その修正を、支持対象足平部位置姿勢偏差又は移動対象ハンド部位置姿勢偏差等に応じてさらに行うようにしてもよい。

また、前記実施形態では、ロボット１として、２つの脚リンク３，３及び２つの腕リンク４，４を可動リンクとして有する人型のロボットを例示したが、本発明は、種々様々な形態のロボットに適用できる。例えば、平地の移動時に脚リンクとして機能する３つ以上の可動リンクを備えるロボット、あるいは、３つ以上の腕リンクを備えるロボット等につじても本発明を適用できる。

１…移動ロボット、２…基体、３…脚リンク（可動リンク）、４…腕リンク（可動リンク）、１３…足平部（脚リンクの先端部）、２６…ハンド部（腕リンクの先端部）、４０…制御装置、４２…姿勢センサ、５１…動作目標決定部（動作目標決定手段）、５２…関節制御部（関節制御手段）、５３…偏差推定部（偏差推定手段）。

Claims (4)

1. 基体と、該基体から延設された複数の可動リンクとを備え、各可動リンクの先端部と前記基体との間に設けられた関節の作動によって各可動リンクの先端部が前記基体に対して動くように構成された移動ロボットの制御装置であって、
   前記移動ロボットに昇降対象の構造物を昇降させるとき、前記複数の可動リンクのうち、前記基体の上部から延設された１つ以上の可動リンクである上方側支持対象可動リンクの先端部と前記基体の下部から延設された１つ以上の可動リンクである下方側支持対象可動リンクの先端部とをそれぞれ該構造物に接触させて支持させ、且つ、前記上方側支持対象可動リンク及び下方側支持対象可動リンク以外の１つ以上の可動リンクである移動対象可動リンクの先端部をそれぞれ前記構造物から離反させた状態で、各移動対象可動リンクの先端部を目標支持位置姿勢に向かって移動させ、該目標支持位置姿勢にて該移動対象可動リンクの先端部を該構造物に接触させて支持させるという昇降用動作を含むように、該移動ロボットの各関節の作動を規定する動作目標を逐次決定する動作目標決定手段と、
   前記決定された動作目標に応じて前記移動ロボットの各関節の作動を制御する関節制御手段と、
   前記移動ロボットの前記昇降用動作における１つ以上の下方側支持対象可動リンクのそれぞれの先端部の実際の位置姿勢と前記動作目標における該下方側支持対象可動リンクの先端部の目標位置姿勢との偏差である位置姿勢偏差と、前記移動ロボットの前記昇降用動作における１つ以上の移動対象可動リンクのそれぞれの先端部の実際の位置姿勢と前記動作目標における該移動対象可動リンクの先端部の目標位置姿勢との偏差である位置姿勢偏差とのうちの少なくともいずれか一方を推定する偏差推定手段とを備え、
   前記動作目標決定手段は、前記移動ロボットの前記昇降用動作における前記動作目標を逐次決定するとき、１つ以上の下方側支持対象可動リンクの先端部のそれぞれの前記基体に対する相対的な目標位置／姿勢、１つ以上の移動対象可動リンクのそれぞれの先端部を前記構造物に支持させようとする目標支持位置／姿勢とのうちの少なくともいずれかを前記推定された位置姿勢偏差に応じて調整するように前記動作目標を決定するように構成されていることを特徴とする移動ロボットの制御装置。
2. 請求項１記載の移動ロボットの制御装置において、
   前記移動ロボットの基体の姿勢を検出するための姿勢センサが該移動ロボットに搭載されており、
   前記偏差推定手段は、前記姿勢センサの出力により示される前記基体の姿勢の検出値と、前記動作目標における前記基体の姿勢の目標値との偏差だけ、前記動作目標の移動ロボットを、前記上方側支持対象可動リンクの先端部のそれぞれに前記構造物から作用する荷重の全体の作用中心点を回転中心として仮想的に回転させることにより得られる移動ロボットの各下方側支持対象可動リンクの先端部又は各移動対象可動リンクの先端部の位置姿勢を該下方側支持対象可動リンクの先端部又は該移動対象可動リンクの先端部の実際の位置姿勢として推定する手段を含むことを特徴とする移動ロボットの制御装置。
3. 請求項１又は２記載の移動ロボットの制御装置において、
   前記移動ロボットは、前記基体の下部から延設された２つ以上の可動リンクと、前記基体の上部から延設された２つ以上の可動リンクとを備えており、
   前記昇降用動作は、前記基体の下部から延設された可動リンクのうちの一部の可動リンクを前記下方側支持対象可動リンク、前記基体の下部から延設された可動リンクのうちの残りの可動リンクを前記移動対象可動リンク、前記基体の上部から延設された可動リンクのうちの少なくとも１つの可動リンクを前記上方側支持対象可動リンクとして、前記移動対象可動リンクの先端部を移動させる昇降用動作であることを特徴とする移動ロボットの制御装置。
4. 請求項３記載の移動ロボットの制御装置において、
   前記動作目標決定手段は、前記推定された位置姿勢偏差が、前記基体の下部が前記構造物に近づくように該基体の姿勢が傾斜することで発生する偏差である場合に、前記基体の下部から延設された可動リンクのうちの１つ以上の前記下方側支持対象可動リンクのそれぞれの前記基体に対する連結部と該下方側支持対象可動リンクの先端部との間の距離が、前記位置姿勢偏差がゼロである場合よりも長くなるように前記基体に対する該下方側支持対象可動リンクの先端部の相対的な目標位置／姿勢を調整し、さらに、前記基体の下部から延設された可動リンクのうちの１つ以上の前記基体の下部から延設された可動リンクのうちの１つ以上の前記移動対象可動リンクのそれぞれの先端部の目標支持位置／姿勢が、該移動対象可動リンクについての前記位置姿勢偏差がゼロである場合よりも前記基体に近づくように、該目標支持位置／姿勢を調整するように構成されていることを特徴とする移動ロボットの制御装置。
   

Images