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

Description

本発明は、移動ロボットの制御装置に関する。

基体及び該基体から延設される脚リンク機構等を構成する複数のリンクを関節を介して相互に連結した構造を有する人型ロボット等の移動ロボットにおいては、該移動ロボットの目標とする運動（移動動作）に応じて逆運動学の演算処理等により各関節の目標変位量を逐次決定し、その目標変位量に実際の変位量を追従させるように各関節の変位量を関節アクチュエータを介して制御する、所謂、位置制御が一般に知られている（例えば特許文献１を参照）。

このような位置制御は、各関節の目標変位量が移動ロボットの目標とする運動に応じて規定されるため、移動ロボットに作用する外力の変化等に対する柔軟性が乏しい。

このため、近年では、所謂、力制御方式によって関節アクチュエータによる各関節の駆動力を制御することで、移動ロボットの移動動作を行わせる技術の研究が進められている。

上記力制御方式は、移動ロボットの目標とする運動から逆動力学モデルを用いて各関節（脚リンク機構の動作用の各関節）の目標駆動力が決定され、該目標駆動力に応じて関節アクチュエータによる各関節の駆動力が制御される。

特開平５−３０５５７９号公報

ところで、力制御方式によって、移動ロボットの運動を制御する場合、該移動ロボットに作用する外力（外界物からの接触反力）の変化に応じて各関節の変位量が変動し易い。このため、移動ロボットの実際の運動が、目標とする運動から乖離し、あるいは、移動ロボットの運動の安定性が損なわれる虞がある。

そこで、移動ロボットの基体を構成するリンク等、特定のリンクの実際の位置及び姿勢（移動ロボットの外界から見た位置及び姿勢）をなんらかの手段により推定し、その推定値と目標値（目標とする運動における目標値）との偏差を低減するように該偏差に応じて関節アクチュエータによる関節の駆動力を適宜調整することが望ましいと考えられる。

そして、この場合、特定のリンクの実際の位置及び姿勢を簡易な手法で適切に推定し得ることが望まれる。

本発明はかかる背景に鑑みてなされたものであり、簡易な手法で移動ロボットの特定リンクの位置及び姿勢を適切に推定し、その推定値を力制御方式による移動ロボットの動作制御に利用することができる制御装置を提供することを目的とする。

本発明の移動ロボットの制御装置は、かかる目的を達成するために、基体を構成するリンクと該基体から延設される複数の脚リンク機構を構成するリンクとを含み、関節を介して相互に連結された複数のリンクと、各関節を駆動する関節アクチュエータとを備える移動ロボットに対し、前記関節アクチュエータにより前記脚リンク機構の動作用の各関節に作用させる駆動力を力制御方式で制御することで該移動ロボットの移動動作を行わせる制御装置であって、
前記脚リンク機構のうち、接地状態のリンクである接地リンクを含む少なくとも１つの脚リンク機構の接地リンクの目標姿勢と前記移動ロボットの複数のリンクのうちのあらかじめ定められた第１特定リンクの実際の姿勢の観測値とのうちの少なくともいずれか一方と、前記接地リンクの目標位置と、前記接地リンクと前記第１特定リンクとの間の各関節の実際の変位量の観測値とを少なくとも含む入力パラメータを逐次取得し、該入力パラメータを用いて、順運動学の演算処理を少なくとも含む処理を実行することにより前記第１特定リンクの実際の位置姿勢を逐次推定するように構成されたリンク位置姿勢推定手段と、
前記リンク位置姿勢推定手段による前記第１特定リンクの実際の位置姿勢の推定値と該第１特定リンクの目標位置姿勢との偏差と、前記移動ロボットの複数のリンクのうちの前記第１特定リンクと異なる非接地状態の第２特定リンクの実際の位置姿勢の推定値であって、前記第１特定リンクの実際の位置姿勢の推定値、並びに、前記第１特定リンクと前記第２特定リンクとの間の各関節の変位量の観測値から順運動学の演算処理によって算出される該第２特定リンクの実際の位置姿勢の推定値と該第２特定リンクの目標位置姿勢との偏差とのうちの少なくともいずれか一方の偏差に応じて、当該偏差を低減するように前記脚リンク機構の動作用の各関節に作用させる駆動力を前記関節アクチュエータを介して調整するように構成された関節駆動力制御手段とを備えることを特徴とする（第１発明）。

なお、本発明において、接地リンク、第１特定リンク及び第２特定リンクについての「位置姿勢」は、位置及び姿勢の組を意味する。

また、前記接地リンクについての「接地」は、前記移動ロボットに作用する重力に抗する接触反力を受けるように外界物に接触させた状態を意味する。この場合、外界物は、床もしくは地面に限らず、任意の構造物でもよい。

また、本発明において、関節の変位量、荷重等、任意の状態量の「観測値」は、前記移動ロボットに搭載された適宜のセンサによる該状態量の検出値、あるいは、該状態量と相関性を有する他の１つ以上の状態量の検出値（前記移動ロボットに搭載された適宜のセンサによる検出値）から、該相関性に基づいて推定してなる推定値、あるいは、該状態量と一致もしくはほぼ一致するとみなすことができる擬似的な推定値を意味する。この場合、「擬似的な推定値」としては、例えば該状態量の目標値を使用し得る。

また、本発明における「力制御方式」は、前記移動ロボットの目標とする運動から逆動力学モデルを用いて前記脚リンク機構の動作用の各関節の目標駆動力を決定し、少なくとも該目標駆動力に応じて、前記関節アクチュエータにより脚リンク機構の動作用の各関節に作用させる駆動力を制御する制御方式である。

上記第１発明において、前記接地リンクから前記第１特定リンクまでのリンクの連結構造体（リンク群）に着目した場合、該連結構造体における第１特定リンク以外の１つのリンクの位置と、該連結構造における１つのリンクの実際の姿勢と、該連結構造体における各関節（前記接地リンクと前記第１特定リンクとの間の各関節）の実際の変位量とが判れば、順運動学の演算処理等により前記第１特定リンクの実際の位置を特定できる。

ここで、前記脚リンク機構のうち、接地状態のリンクである接地リンクの実際の位置は、該接地リンクの動きが当該接地によって拘束を受けた状態での該接地リンクの位置である。また、前記関節駆動力制御手段によって、前記第１特定リンク及び第２特定リンクの少なくともいずれか一方についての前記偏差に応じて、該偏差を低減するように前記脚リンク機構の動作用の各関節に作用させる駆動力が調整される。

このため、前記接地リンクの実際の位置姿勢は、該接地リンクの接地対象の外界物（地面等）の実際の形状及び配置が移動ロボット１の目標運動で想定されたものとほぼ一致するような場合には、一般に、目標位置姿勢（移動ロボットの目標とする運動における目標位置姿勢）との一致度合いが比較的高い。

このため、前記接地リンクの目標位置及び目標姿勢は、それぞれ擬似的に、該接地リンクの実際の位置、実際の姿勢にほぼ一致するものとみなすことができる。従って、該接地リンクの目標位置を、上記連結構造体における１つのリンクの実際の位置を示すものとして用いることが可能である。同様に、該接地リンクの目標姿勢を、上記連結構造体における１つのリンクの実際の姿勢を示すものとして用いることも可能である。

また、前記第１特定リンクの実際の姿勢の観測値は、移動ロボットに搭載した適宜のセンサ（例えばジャイロセンサ等により構成される姿勢センサ）の検出信号に基づいて容易に取得することもできる。そして、この場合には、該第１特定リンクの実際の姿勢の観測値を、上記連結構造体における１つのリンクの実際の姿勢を示すものとして用いることもできる。

さらに、前記接地リンクと第１特定リンクとの間の各関節の実際の変位量の観測値は、移動ロボットに搭載した適宜のセンサ（例えばロータリエンコーダ、ポテンショメータ等により構成されるセンサ）によって容易に精度よく取得することができる。

そこで、前記リンク位置姿勢推定手段は、前記入力パラメータを用いて、順運動学の演算処理を少なくとも含む処理を実行することにより、前記第１特定リンクの実際の位置姿勢を逐次推定する。

これにより第１特定リンクの実際の位置姿勢を簡易な手法により推定することができる。

そして、関節駆動力制御手段は、かかる第１特定リンクの実際の位置姿勢の推定値に基づく前記偏差に応じて、前記脚リンク機構の動作用の各関節に作用させる駆動力を調整する。

従って、第１発明によれば、簡易な手法で移動ロボットの特定リンクの位置を適切に推定し、その推定値を力制御方式による移動ロボットの動作制御に利用することができる。

上記第１発明では、前記第２特定リンクは、前記脚リンク機構のそれぞれの先端部のリンクであり、前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において前記先端部のリンクが空中移動を行っているときに、前記第１特定リンクの実際の位置姿勢を逐次推定することに加えてさらに、該第１特定リンクの実際の位置姿勢の推定値、並びに、該第１特定リンクと前記先端部のリンクとの間の各関節の変位量の観測値から順運動学の演算処理によって、該先端部のリンクの実際の位置姿勢を逐次推定するように構成されており、前記関節駆動力制御手段は、前記移動ロボットの移動動作において前記先端部のリンクが空中移動を行っているときに、少なくとも該先端部のリンクの実際の位置姿勢の推定値と該先端部のリンクの目標位置姿勢との偏差に応じて、当該偏差を低減するように前記脚リンク機構の動作用の各関節に作用させる駆動力を前記関節アクチュエータを介して調整するように構成されていることが好ましい（第２発明）。

かかる第２発明によれば、前記移動ロボットの移動動作において脚リンク機構の前記先端部のリンクが空中移動を行っているときに、少なくとも該先端部のリンクの実際の位置姿勢の推定値と該先端部のリンクの目標位置姿勢との偏差に応じて、当該偏差を低減するように前記脚リンク機構の動作用の各関節に作用させる駆動力が前記関節アクチュエータを介して調整される。

このため、前記先端部のリンクが接地したときの該先端部のリンク（接地リンク）の実際の位置姿勢と目標位置姿勢との一致度合（特に実際の位置と目標位置との一致度合）が高まる。ひいては、前記入力パラメータの１つである前記接地リンクの目標位置が、該接地リンクの実際の位置を示すものとしての信頼性が高まる。

さらに前記接地リンクの目標姿勢が前記入力パラメータの１つである場合においては、該接地リンクの目標姿勢が、該接地リンクの実際の姿勢を示すものとしての信頼性が高まる。

その結果、前記第１特定リンクの位置姿勢の推定値、あるいは、前記第２特定リンクの位置姿勢の推定値の信頼性を高めることができる。

上記第１発明又は第２発明では、前記入力パラメータは、前記第１特定リンクの実際の姿勢の観測値を含むことができる。この場合には、前記リンク位置姿勢推定手段は、より具体的には、例えば次のような形態を採用し得る。

すなわち、前記リンク位置姿勢推定手段は、前記第１特定リンクの実際の位置の推定値を新たに決定するとき、前記第１特定リンクの現在の実際の姿勢の観測値と、過去に決定した前記第１特定リンクの実際の位置の推定値のうちの最新値と、前記接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって、前記第１特定リンクの現在の実際の位置が前記最新値に一致すると仮定した場合における前記接地リンクの位置の暫定値を算出する第１処理と、該接地リンクの位置の暫定値と該接地リンクの目標位置との偏差により前記最新値を補正することによって前記第１特定リンクの実際の位置の新たな推定値を決定する第２処理とを実行し、さらに、前記第１特定リンクの実際の姿勢の観測値を該実際の姿勢の推定値とするように構成されている（第３発明）。

この第３発明によれば、前記第１処理により算出される前記接地リンクの位置の暫定値と、該接地リンクの目標位置との偏差は、前記第１特定リンクの実際の位置の推定値のうちの最新値と、該第１特定リンクの実際の位置とのずれ量に相当する。

従って、前記第２処理において、当該偏差により前記最新値を補正することによって前記第１特定リンクの実際の位置の新たな推定値を適切に決定できる。

また、第１特定リンクの実際の姿勢の観測値を、そのまま該第１特定リンクの実際の姿勢の推定値として採用できる。

従って、第１特定リンクの実際の位置姿勢を簡易な手法で適切に推定できる。

ところで、前記接地リンクは、接地対象の外界物から受ける接触反力よる荷重が作用するため、弾性変形を生じやすい。

そこで、上記第３発明では、前記入力パラメータは、前記接地リンクに作用する実際の荷重の観測値をさらに含んでおり、前記リンク位置姿勢推定手段は、前記荷重の観測値を用いて前記接地リンクの弾性変形量を推定する第３処理と、前記第１処理で算出した前記接地リンクの位置の暫定値を、前記弾性変形量の推定値により補正する第４処理とをさらに実行し、前記第２処理では、前記接地リンクの位置の暫定値と該接地リンクの目標位置との偏差の代わりに、前記第４処理による前記接地リンクの位置の暫定値の補正後の値と該接地リンクの目標位置との偏差により前記最新値を補正することによって前記第１特定リンクの実際の位置の新たな推定値を決定するように構成されていることが好ましい（第４発明）。

この第４発明によれば、前記接地リンクの弾性変形に起因する該接地リンクの位置変化の影響を補償するように、前記第１特定リンクの実際の位置の推定値を逐次決定することができる。その結果、該第１特定リンクの実際の位置の推定値の信頼性をより一層高めることができる。

前記第３発明あるいは第４発明では、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間においては、一例の態様として、いずれか１つの接地リンクについての位置の暫定値もしくは前記補正後の暫定値と、目標位置とを用いて第１特定リンクの実際の位置を推定することが可能である。

ただし、前記第３発明では、前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンク毎に前記第１処理を実行し、前記第２処理では、各接地リンクの位置の暫定値と該接地リンクの目標位置との偏差により前記最新値を補正することによって各接地リンクに対応して得られる前記第１特定リンクの実際の位置の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置の新たな推定値を決定するように構成されていることが好ましい（第５発明）。

同様に、上記第４発明では、前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンク毎に前記第１処理、第３処理及び第４処理を実行し、前記第２処理では、各接地リンクの位置の暫定値の前記補正後の値と該接地リンクの目標位置との偏差により前記最新値を補正することによって各接地リンクに対応して得られる前記第１特定リンクの実際の位置の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置の新たな推定値を決定するように構成されていることが好ましい（第６発明）。

これらの第５発明又は第６発明によれば、複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンクに対応して得られる前記第１特定リンクの実際の位置の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置の新たな推定値が決定される。このため、上記期間において、いずれか１つの接地リンクについての位置の暫定値もしくは前記補正後の暫定値と、目標位置とを用いて第１特定リンクの実際の位置を推定する場合に較べて、第１特定リンクの実際の位置の推定値の信頼性あるいは安定性を高めることができる。

さらに、上記フィルタリング処理を行う場合には、第１特定リンクの実際の位置の推定値を滑らかに変化させるようにすることができる。

上記第１発明又は第２発明では、前記入力パラメータは、前記接地リンクの目標姿勢を含むこともできる。この場合には、前記リンク位置姿勢推定手段は、より具体的には、例えば次のような形態を採用し得る。

すなわち、前記リンク位置姿勢推定手段は、前記第１特定リンクの実際の位置姿勢の推定値を新たに決定するとき、前記接地リンクの目標位置姿勢の現在値と、前記接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって、前記第１特定リンクの実際の位置姿勢の新たな推定値を決定する第６処理を実行するように構成されている（第７発明）。

この第７発明によれば、前記第６処理では、前記接地リンクの目標位置及び目標姿勢をそれぞれ、該接地リンクの実際の位置、姿勢を示すものとして用いて、前記第１特定リンクの実際の位置姿勢の新たな推定値が決定されることとなる。従って、第１特定リンクの実際の位置姿勢を簡易な手法で適切に推定できる。

また、前記した如く、前記接地リンクは、接地対象の外界物から受ける接触反力よる荷重が作用するため、弾性変形を生じやすい。

そこで、上記第７発明では、前記入力パラメータは、前記接地リンクに作用する実際の荷重の観測値をさらに含んでおり、前記リンク位置姿勢推定手段は、前記荷重の観測値を用いて前記接地リンクの弾性変形量を推定する第７処理と、前記接地リンクの目標位置姿勢の現在値を前記弾性変形量の推定値により補正する第８処理とをさらに実行し、前記第６処理では、前記接地リンクの目標位置姿勢の現在値の代わりに、前記第８処理による補正後の値を用いて、前記第１特定リンクの実際の位置姿勢の新たな推定値を決定するように構成されていることが好ましい（第８発明）。

この第８発明によれば、前記接地リンクの弾性変形に起因する該接地リンクの位置姿勢の変化の影響を補償するように、前記第１特定リンクの実際の位置姿勢の推定値を逐次決定することができる。その結果、該第１特定リンクの実際の位置姿勢の推定値の信頼性をより一層高めることができる。

前記第７発明あるいは第８発明では、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間においては、一例の態様として、いずれか１つの接地リンクについて目標位置姿勢もしくはその補正後の値（弾性変形量の推定値による補正後の値）を用いて第１特定リンクの実際の位置を推定することが可能である。

ただし、前記第７発明では、前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間での前記第６処理では、各接地リンクの目標位置姿勢の現在値と、該接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって各接地リンクに対応して算出される前記第１特定リンクの位置姿勢の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置姿勢の新たな推定値を決定するように構成されていることが好ましい（第９発明）。

同様に、前記第８発明では、前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンク毎に前記第７処理及び第８処理を実行し、さらに前記第６処理では、各接地リンクの目標位置姿勢の現在値の前記補正後の値と、該接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって各接地リンクに対応して算出される前記第１特定リンクの位置姿勢の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリングを施すことによって、該第１特定リンクの実際の位置姿勢の新たな推定値を決定するように構成されていることが好ましい（第１０発明）。

これらの第９発明又は第１０発明によれば、複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンクに対応して得られる前記第１特定リンクの実際の位置の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置の新たな推定値が決定される。このため、上記期間において、いずれか１つの接地リンクについての目標位置姿勢もしくはその補正後の値（弾性変形量の推定値による補正後の値）を用いて第１特定リンクの実際の位置を推定する場合に較べて、第１特定リンクの実際の位置の推定値の信頼性あるいは安定性を高めることができる。

さらに、上記フィルタリング処理を行う場合には、第１特定リンクの実際の位置の推定値を滑らかに変化させるようにすることができる。

本発明の実施形態における移動ロボットの構成を示す図。 実施形態の移動ロボットの動作制御に関する構成を示すブロック図。 図２に示すリンク位置姿勢推定部の、第１実施形態における処理を示すブロック図。 図２に示すリンク位置姿勢推定部の、第２実施形態における処理を示すブロック図。

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

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

なお、本実施形態の説明では、脚リンク機構３Ｌ，３Ｒ等に関する参照符号Ｌ、Ｒは、それぞれロボット１の基体２の正面に向かって左側、右側を示す参照符号である。ただし、左右を区別する必要がないときは、参照符号Ｌ、Ｒを省略する。

各脚リンク機構３は、大腿部、下腿部、足平部にそれぞれ相当する３つのリンク１１，１２，１３を、基体２側から順番に、股関節機構部１４、膝関節機構部１５、足首関節機構部１６を介して連結して構成されている。以降、リンク１１，１２，１３をそれぞれ大腿部１１、下腿部１２、足平部１３ということがある。

各脚リンク機構３の関節機構部１４，１５，１６は、該脚リンク機構３の先端部のリンクである足平部１３が、基体２に対して例えば６自由度の運動自由度で動くように構成されている。

例えば、股関節機構部１４は、ヨー軸、ピッチ軸及びロール軸の総計３軸の回転自由度を有するように３つの関節（図示省略）により構成される。膝関節機構部１５は、例えばピッチ軸（１軸）の回転自由度を有するように単一の関節（図示省略）により構成される。足首関節機構部１６は、例えばピッチ軸及びロール軸の総計２軸の回転自由度を有するように２つの関節（図示省略）により構成される。

なお、本実施形態の説明では、特にことわらない限り、「関節」は、１軸の回転自由度を有する回転型の関節を意味する。このような関節は、１つの軸周りに相対回転し得る２つの部材を備える構造のものであり、公知の構造のものを採用できる。

また、本実施形態の説明では、ヨー軸、ピッチ軸、ロール軸は、それぞれ図１に示すようにロボット１が起立した状態（基体２、各脚リンク機構３及び各腕リンク機構４をほぼ上下方向に伸ばした状態）での該ロボット１の上下方向の軸（Ｚ軸）、左右方向の軸（Ｙ軸）、前後方向の軸（Ｘ軸）を意味する。

各腕リンク機構４は、上腕部、前腕部、ハンド部にそれぞれ相当する３つのリンク２１，２２，２３を、基体２側から順番に、肩関節機構部２４、肘関節機構部２５、手首関節機構部２６を介して連結して構成されている。以降、リンク２１，２２，２３をそれぞれ、上腕部２１、前腕部２２、ハンド部２３ということがある。

各腕リンク機構４の関節機構部２４，２５，２６は、脚リンク機構３の場合と同様に、該腕リンク機構４の先端部のリンクであるハンド部２３が、基体２に対して例えば６自由度の運動自由度で動くように構成されている。

例えば、肩関節機構部２４は、ヨー軸、ピッチ軸及びロール軸の総計３軸の回転自由度を有するように３つの関節（図示省略）により構成される。肘関節機構部２５は、例えばピッチ軸又はロール軸（１軸）の回転自由度を有するように単一の関節（図示省略）により構成される。手首関節機構部２６は、例えばピッチ軸及びロール軸の総計２軸の回転自由度を有するように２つの関節（図示省略）により構成される。

なお、ハンド部２３は、該ハンド部２３による作業用の開閉機構等を備えていてもよい。

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

以上が本実施形態のロボット１の機構的な構造の概要である。かかる構造のロボット１は、基本的には、左右の脚リンク機構３Ｌ，３Ｒの動作（各足平部１３の空中移動及びそれに続く接地を繰り返す２脚歩容の動作）によって移動できる。

あるいは、腕リンク機構４Ｌ，４Ｒを脚リンク機構３Ｌ，３Ｒとは別の脚リンク機構として動作させてロボット１の移動を行うことも可能である。例えば、脚リンク機構３Ｌ，３Ｒ及び腕リンク機構４Ｌ，４Ｒの４つのリンク機構のそれぞれの先端部のリンク（足平部１３、ハンド部２３）の空中移動及びそれに続く接地を繰り返す４脚歩容の動作によってロボット１の移動を行うことも可能である。この移動動作では、梯子、脚立等の急勾配の構造物の昇降を行うことも可能である。

補足すると、各脚リンク機構３、あるいは、各腕リンク機構４は、例えば６自由度よりも大きい運動自由度を有するように構成されていてもよい。

また、基体２は、関節を介して連結された２つ以上のリンクにより構成されていてもよい。例えば、基体を、１つ又は複数の関節を介して連結した上部側のリンクと下部側のリンクとの２つのリンクで構成してもよい。

また、各脚リンク機構３及び各腕リンク機構４のそれぞれは、回転型の関節に限らず、直動型の関節を含んでいてもよい。

また、ロボット１は、頭部５及び首関節機構部３１を持たない構造のもの、あるいは腕リンク機構４Ｒ，４Ｌの一方又は両方を備えないものであってもよい。

また、ロボット１は、３つ以上の脚リンク機構を備えるものであってもよい。なお、脚リンク機構３Ｌ，３Ｒに加えて、腕リンク機構４Ｌ，４Ｒをも脚リンク機構として動作させてロボット１の移動を行う場合には、ロボット１は、実質的に４つの脚リンク機構を有するロボットとみなすことができる。

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

図２に示すように、ロボット１には、該ロボット１の動作制御を行う制御装置４０と、各関節をそれぞれ駆動する関節アクチュエータ４１と、所要の各種センサとが搭載されている。

関節アクチュエータ４１は、各関節毎に備えられ、対応する関節に図示しない減速機等を介して駆動力を伝達することで、該関節を駆動する。各関節アクチュエータ４１は、本実施形態では電動モータにより構成される。

センサとしては、ロボット１の１つのリンク、例えば基体２の姿勢（空間的な向き）を検出するための姿勢センサ４２と、ロボット１の各関節の変位量（回転角）を検出するための関節変位センサ４３と、各脚リンク機構３の足平部１３が接触対象の外界物から受ける外力（並進力及びモーメント）を検出するための力センサ４４と、各腕リンク機構４のハンド部２３が接触対象の外界物から受ける外力（並進力及びモーメント）を検出するための力センサ４５とがロボット１に搭載されている。

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

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

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

なお、ロボット１には、上記以外のセンサ（外界環境の認識用のセンサ等）が搭載されていてもよい。例えば基体２もしくは頭部５にカメラもしくは測距センサ等が搭載されていてもよい。

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

この制御装置４０は、実装されるプログラムを実行することにより実現される機能、又は、ハードウェア構成により実現される機能として、各関節の目標駆動力（該関節に対応する関節アクチュエータ４１から該関節に付与する駆動力の目標値）を所定の制御処理周期で逐次決定する関節駆動力決定部５１と、該目標駆動力に応じて関節アクチュエータ４１を制御する関節制御部５２とを備える。

また、関節駆動力決定部５１は、ロボット１の所定のリンクの実際の位置及び姿勢を推定するリンク位置姿勢推定部５３を含む。なお、括弧内に記載した参照符号５４は後述の第２実施形態に関するものである。

補足すると、本実施形態では、関節駆動力決定部５１及び関節制御部５２により本発明における関節駆動力制御手段が実現される。また、リンク位置姿勢推定部５３は、本発明におけるリンク位置姿勢推定手段に相当する。

次に、前記制御装置４０の制御処理をより具体的に説明する。なお、以降の説明は、脚リンク機構３Ｌ，３Ｒの動作（２脚歩容の動作）によって、ロボット１の移動動作を行う場合を主要例として行う。この移動動作は、人の歩行動作と同様の形態で、各脚リンク機構３の先端部のリンクである足平部１３の空中移動とこれに続く接地とを繰り返すことでロボット１の移動を行う動作である。

制御装置４０は、外部のサーバ等から与えられるロボット１の動作指令、あるいは、あらかじめティーチングされた移動計画等に応じて、ロボット１の移動動作のための基準の目標運動を生成する。この基準の目標運動は、ロボット１の各リンクの運動、あるいは、各関節の変位量を規定し得るものである。該基準の目標運動は、例えば、各脚リンク機構３の足平部１３の目標位置姿勢の軌道と、基体２の目標位置姿勢の軌道と、各腕リンク機構４のハンド部２３の基体２に対する相対的な目標位置姿勢の軌道と、頭部５の基体２に対する相対的な目標姿勢の軌道とから構成される。あるいは、基準の目標運動は、これらの軌道を規定するパラメータにより構成される。

ここで、足平部１３等、ロボット１の任意の部位（リンク）の「目標位置姿勢」は、該部位の位置（詳しくは、該部位のあらかじめ定めた代表点の位置）の目標値と、該部位の姿勢（空間的な向き）の目標値との組を意味する。また、「軌道」は瞬時値の時系列を意味する。

また、基準の目標運動のうちの足平部１３の目標位置姿勢と、基体２の目標位置姿勢とは、外界に対して任意に設定されるグローバル座標系（ワールド座標系）で見た位置及び姿勢として表現される。これらの目標位置姿勢は、例えば、ＺＭＰ（Zero Moment Point）等に関する動力学的な制約条件（ＺＭＰが所謂、支持多角形内に存在する等の制約条件）、各脚リンク機構３の各関節の可動範囲等の機構的な制約条件、並びに、ロボット１の移動経路及び移動速度等の要求条件を満たし得るように生成される。その生成手法は公知の手法でよい。

また、ハンド部２３の目標位置姿勢は、基体２に対して設定されるローカル座標系で見た位置及び姿勢として表現され、頭部５の目標姿勢は、該ローカル座標系で見た姿勢として表現される。基体２に対する相対的なハンド部２３の目標位置姿勢及び頭部５の目標姿勢は、それぞれ、例えば一定に保つように生成され得る。ただし、ロボット１の移動時に、ハンド部２３又は頭部５が基体２に対して動くように、該ハンド部２３の目標位置姿勢、あるいは、頭部５の目標姿勢を生成するようにしてもよい。

補足すると、基準の目標運動の全部又は一部を、外部のサーバで生成し、それを該サーバからロボット１の制御装置４０に無線通信により転送することも可能である。この場合には、制御装置４０での基準の目標運動の全部又は一部の生成処理は不要である。

制御装置４０は、次に、上記基準の目標運動を基に、前記関節駆動力決定部５１の処理を実行することでロボット１の各関節の目標駆動力を所定の制御処理周期で逐次決定する。

この場合、関節駆動力決定部５１は、まず、上記基準の目標運動から、ロボット１の逆動力学モデルを用いて（力制御の手法によって）、ロボット１の各関節の基準の目標駆動力を算出する。

なお、２脚歩容の動作によりロボット１の移動動作を行う場合には、各腕リンク機構４の各関節の目標駆動力と、首関節機構部３１の各関節の目標駆動力とは、例えば位置制御の手法によって決定してもよい。すなわち、基体２に対する相対的なハンド部２３の目標位置姿勢に応じて逆運動学の演算処理より各腕リンク機構４の各関節の目標変位量を決定し、その目標変位量に各腕リンク機構４の各関節の実際の変位量（観測値）を一致させるように該関節の目標駆動力を決定してもよい。このことは、首関節機構部３１の各関節についても同様である。

次いで、関節駆動力決定部５１は、各関節の基準の目標駆動力を適宜修正することで、各関節アクチュエータ４１を実際に制御するために用いる各関節の目標駆動力を決定する。

この場合、本実施形態では、関節駆動力決定部５１は、各脚リンク機構３の各関節の基準の目標駆動力を修正するための操作量を決定するために、前記リンク位置姿勢推定部５３の処理を実行する。

ここで、本実施形態では、リンク位置姿勢推定部５３は、基体２の実際の位置姿勢（グローバル座標系で見た位置及び姿勢）と、脚リンク機構３Ｌ，３Ｒの足平部１３，１３のうちの非接地状態の足平部１３（空中移動を行う足平部１３）の実際の位置姿勢（グローバル座標系で見た位置及び姿勢）とを推定する。

この場合、基体２、及び非接地状態の足平部１３（以降、非接地足平部１３という）はそれぞれ、本発明における第１特定リンク、第２特定リンクに相当する。また、接地状態の足平部１３（以降、接地足平部１３という）は本発明における接地リンクに相当する。

なお、２脚歩容の動作によるロボット１の移動動作では、脚リンク機構３Ｌ，３Ｒの両方の足平部１３，１３が接地足平部１３，１３となる期間である両脚支持期の状態と、脚リンク機構３Ｌ，３Ｒのいずれか一方の足平部１３だけが接地足平部１３となる期間である片脚支持期の状態とが交互に切替わる。

リンク位置姿勢推定部５３の具体的な処理を図３を参照して詳説する。なお、以降の説明では、基体２の位置及び姿勢、接地足平部１３の位置及び姿勢、非接地足平部１３の位置及び姿勢、各関節の変位量、接地足平部１３に作用する荷重（並進力及びモーメント）を示す参照符号として、それぞれ、Ｘb、Ｑb、Ｘsup、Ｑsup、Ｘswg、Ｑswg、θi（ｉ：各関節の識別符号）、Ｆcに適宜の添え字（_ref、_act等）を付加した符号を使用する。

リンク位置姿勢推定部５３は、各制御処理周期において、各接地足平部１３の目標位置Ｘsup_refと、基体２の現在の実際の姿勢の観測値Ｑb_actと、各脚リンク機構３の各関節の現在の実際の変位量の観測値θi_actと、各接地足平部１３に作用する現在の実際の荷重の観測値Ｆc_actとを入力パラメータとして取得する。

この場合、リンク位置姿勢推定部５３が取得する各接地足平部１３の目標位置Ｘsup_refは、前記基準の目標運動のうち、現在の制御処理周期の時刻での該接地足平部１３の目標位置（瞬時値）である。また、基体２の現在の実際の姿勢の観測値Ｑb_actは、現在の制御処理周期において、前記姿勢センサ４２の検出信号を用いてストラップダウン方式の演算処理により算出された最新値である。また、各脚リンク機構３の各関節の現在の実際の変位量の観測値θi_actは、現在の制御処理周期において、前記関節変位センサ４３の検出信号により示される最新値である。また、各接地足平部１３に作用する現在の荷重の観測値Ｆc_actは、該接地足平部１３に対応する力センサ４４の検出信号により示される最新値である。

そして、リンク位置姿勢推定部５３は、図３に示す順運動学演算部６１及び弾性変形量推定部６２の処理を実行する。

順運動学演算部６１の処理では、リンク位置姿勢推定部５３は、前回の制御処理周期で決定した基体２の位置の推定値Ｘb_estm_p（すなわち前回値）と、基体２の実際の姿勢の観測値Ｑb_act（現在値）と、ロボット１の関節のうちの各接地足平部１３を有する脚リンク機構３（以降、接地脚リンク機構３という）の各関節の実際の変位量の観測値θi_act（現在値）とから、順運動学の演算処理により、各接地脚リンク機構３の接地足平部１３の位置の暫定値Ｘsup_temp1を算出する。

この暫定値Ｘsup_temp1は、基体２の位置及び姿勢、並びに、各接地脚リンク機構３の各関節の変位量が、それぞれ、Ｘb_estm_p（前回値）、Ｑb_act（現在値）、θi_act（現在値）に一致すると仮定して（みなして）、順運動学の演算処理（幾何学的な演算処理）により算出される値である。

なお、２つの脚リンク機構３Ｌ，３Ｒの足平部１３，１３が接地する両脚支持期では、Ｘsup_temp1は、両方の接地足平部１３，１３のそれぞれについて算出され、一方の脚リンク機構３Ｌ又は３Ｒの足平部１３だけが接地する片脚支持期では、Ｘsup_temp1は、当該一方の接地足平部１３についてのみ、算出される。

補足すると、基体２の位置の推定値Ｘb_estm_p（前回値）は、リンク位置姿勢推定部５３が過去に（現在の制御処理周期よりも前に）決定した推定値Ｘb_estmのうちの最新値を意味する。また、各接地足平部１３を有する接地脚リンク機構３の各関節の実際の変位量の観測値θi_actは、換言すれば、基体２（第１特定リンク）と、該接地足平部１３（接地リンク）との間の各関節の実際の変位量の観測値である。

弾性変形量推定部６２の処理では、リンク位置姿勢推定部５３は、各接地足平部１３に作用する荷重の観測値Ｆc_actから、該接地足平部１３に対応してあらかじめ設定された剛性係数（所謂、ばね定数）を用いて、該接地足平部１３の位置に関する弾性変形量ｄＸsupの推定値を算出する。

ここで、各足平部１３は、一般に、ある程度の弾性を有するため、その接地時に作用する荷重によって弾性変形を生じる。そして、その弾性変形によって、該足平部１３（接地足平部１３）の代表点の位置あるいは姿勢が、該足平部１３の弾性変形が生じていない場合に比して若干変化する。弾性変形量推定部６２の処理は、このような接地足平部１３の弾性変形による該接地足平部１３の位置の変化量の推定値を上記弾性変形量ｄＸsupの推定値として算出する。

なお、前記両脚支持期では、ｄＸsupは、両方の接地足平部１３，１３のそれぞれについて算出され、前記片脚支持期では、ｄＸsupは、当該一方の接地足平部１３についてのみ、算出される。

次いで、リンク位置姿勢推定部５３は、演算部６３，６４，６５の処理を順次実行する。

演算部６３の処理では、リンク位置姿勢推定部５３は、順運動学演算部６１で算出した各接地足平部１３の位置の暫定値Ｘsup_temp1を、該接地足平部１３に対応して弾性変形量推定部６２で算出した弾性変形量ｄＸsupの推定値で補正する（具体的には、Ｘsup_temp1からｄＸsupを減算する）ことで、補正後の暫定値Ｘsup_temp2を算出する。

この補正後の暫定値Ｘsup_temp2は、接地足平部１３の弾性変形による該接地足平部１３の位置の変化量（順運動学演算部６１で算出した接地足平部１３の位置の暫定値Ｘsup_temp1からの変化量）が、弾性変形量推定部６２で算出した弾性変形量ｄＸsupに一致すると仮定した場合における該接地足平部１３の位置である。

従って、補正後の暫定値Ｘsup_temp2は、基体２の実際の位置が、前記推定値Ｘb_estm_p（前回値）に一致しておれば、接地足平部１３の実際の位置に相当するものとなる。

そして、リンク位置姿勢推定部５３は、各接地足平部１３について、上記補正後の暫定値Ｘsup_temp2と基準の目標運動における該接地足平部１３の目標位置Ｘsup_refとの偏差（＝Ｘsup_temp2−Ｘsup_ref）を算出する処理を演算部６４で実行する。

ここで、接地によって外界物に対して拘束される各接地足平部１３の実際の位置は、一般に、目標位置Ｘsup_refに対する一致度合が比較的高いとみなすことができる。従って、上記偏差（＝Ｘsup_temp2−Ｘsup_ref）は、順運動学演算部６１の処理で使用した基体２の位置の推定値Ｘb_estm_p（前回値）と、基体２の現在の実際の位置との間のずれ量（差分）に相当するとみなすことができる。

そこで、リンク位置姿勢推定部５３は、次に演算部６５において、基体２の位置の推定値Ｘb_estm_p（前回値）を、演算部６４で算出した各接地足平部１３毎の上記偏差により補正する処理（具体的には、各接地足平部１３毎に、Ｘb_estm_pから当該偏差を減算する処理）を実行する。

次いで、リンク位置姿勢推定部５３は、演算部６５の算出値に平均化処理部６６の処理を施す。この平均化処理部６６は、前記両脚支持期においては、２つの接地足平部１３，１３のそれぞれについての演算部６５の算出値の平均値（加算平均値）を求める。また、前記片脚支持期においては、平均化処理部６６は、一方の接地足平部１３についての演算部６５の算出値をそのまま出力する。

そして、リンク位置姿勢推定部５３は、かかる平均化処理部６６の処理により得られた出力値に、ローパスフィルタ６７によりローパス特性のフィルタリング処理を施すことで、基体２の現在の実際の位置の推定値Ｘb_estmを求め、それを出力する。

また、リンク位置姿勢推定部５３は、基体２の実際の姿勢の観測値Ｑb_act（現在値）をそのまま、基体２の現在の実際の位置の推定値Qb_estmとして出力する。

さらに、リンク位置姿勢推定部５３は、片脚支持期においては、前記順運動学演算部６１とは別の順運動学演算部６８の処理を実行する。この順運動学演算部６８の処理では、リンク位置姿勢推定部５３は、上記の如く求めた基体２の現在の実際の位置の推定値Ｘb_estmと、基体２の実際の姿勢の観測値Ｑb_act（現在値）と、ロボット１の関節のうちの非接地足平部１３を有する脚リンク機構３（以降、非接地脚リンク機構３という）の各関節の実際の変位量の観測値θi_act（現在値）とから、順運動学の演算処理により、該非接地脚リンク機構３の足平部１３（非接地足平部１３）の現在の実際の位置及び姿勢のそれぞれの推定値Ｘswg_estm，Ｑswg_estmを算出し、それを出力する。

補足すると、非接地脚リンク機構３の各関節の実際の変位量の観測値θi_actは、換言すれば、基体２（第１特定リンク）と非接地足平部１３（第２特定リンク）との間の各関節の実際の変位量の観測値である。

なお、図３で、参照符号６９を付したものは、Ｚ変換の形式で表した遅延要素を示している。

本実施形態におけるリンク位置姿勢推定部５３は、以上説明した処理によって、基体２の実際の位置及び姿勢のそれぞれの推定値Ｘb_estm，Ｑb_estmと、片脚支持期における非接地足平部１３（空中移動を行う足平部１３）の実際の位置及び姿勢のそれぞれの推定値Ｘswg_estm，Ｑswg_estmとが所定の制御処理周期で逐次求められる。

補足すると、本実施形態では、順運動学演算部６１の処理、弾性変形量推定部６２の処理、演算部６３の処理が、それぞれ、本発明における第１処理、第３処理、第４処理に相当し、演算部６５、平均化処理部６６及びローパスフィルタ６７の処理が、本発明における第２処理に相当する。

関節駆動力決定部５１は、上記の如く求めた推定値Ｘb_estm，Ｑb_estm，Ｘswg_estm，Ｑswg_estmを用いて、各脚リンク機構３の各関節の基準の目標駆動力を修正するための操作量を決定する。

例えば、関節駆動力決定部５１は、基体２の実際の位置及び姿勢の推定値Ｘb_estm，Ｑb_estmと基準の目標運動における目標位置Ｘb_ref及び目標姿勢Ｑb_refとの偏差（位置偏差及び姿勢偏差）、並びに、非接地足平部１３の実際の位置及び姿勢の推定値Ｘswg_estm，Ｑswg_estmと基準の目標運動における目標位置Ｘswg_ref及び目標姿勢Ｑswg_refとの偏差（位置偏差及び姿勢偏差）を算出する。そして、関節駆動力決定部５１は、これらの偏差に応じて、各偏差を低減する（ゼロに近づける）ように、各脚リンク機構３の各関節の基準の目標駆動力（現在の制御処理周期での目標駆動力）を修正するための操作量をフィードバック制御則等により決定する。そして、関節駆動力決定部５１は、各制御処理周期において、上記の如く決定した操作量により、各脚リンク機構３の各関節の基準の目標駆動力を修正することで、各脚リンク機構３の目標駆動力を決定する。

これにより、基体２及び非接地足平部１３についての上記位置偏差及び姿勢偏差を低減し得るように、各脚リンク機構３の各関節の目標駆動力が調整される。

なお、推定値Ｘb_estm，Ｑb_estm，Ｘswg_estm，Ｑswg_estmの微分演算（時間的変化率を算出する演算）等により、基体２及び非接地足平部１３のそれぞれの並進速度及び角速度の推定値をさらに算出し、上記位置偏差及び姿勢偏差に加えて、さらに、基体２及び非接地足平部１３のそれぞれの並進速度及び角速度の推定値と、基準の目標運動における目標並進速度及び目標角速度との偏差（並進速度偏差及び角速度偏差）をも低減するように、前記操作量を決定することも可能である。

また、本実施形態では、２脚歩容の動作によるロボット１の移動動作では、関節駆動力決定部５１は、各腕リンク機構４の各関節の目標駆動力と、首関節機構部３１の各関節の目標駆動力とは、基準の目標駆動力に一致する駆動力に決定する。

制御装置４０は、各制御処理周期において、以上の如く関節駆動力決定部５１によりロボット１の各関節の目標駆動力を決定した後、関節制御部５２の処理を実行する。

この関節制御部５２は、各関節に関節アクチュエータ４１により付与する駆動力を目標駆動力に一致させるための該関節アクチュエータ４１の出力トルクの目標値を決定し、その出力トルクの目標値に応じて該関節アクチュエータ４１としての電動モータの通電電流を制御する。

これにより、各関節に目標駆動力に一致する駆動力が付与されるように、該関節に対応する関節アクチュエータ４１が制御させる。

以上が、本実施形態における制御装置４０の処理の詳細である。

本実施形態によれば、リンク位置姿勢推定部５３は、各制御処理周期において、各接地足平部１３の目標位置Ｘsup_ref、基体２の実際の姿勢の観測値Ｑb_act、各脚リンク機構３の各関節の実際の変位量に観測値θi_act、及び各接地足平部１３に作用する荷重の観測値Ｆc_actを入力パラメータとして取得し、これらの入力パラメータから前記した処理によって、基体２及び非接地足平部１３のそれぞれの実際の位置姿勢の推定値Ｘb_estm，Ｑb_estm，Ｘswg_estm，Ｑswg_estmを求める。

この場合、基体２の実際の姿勢の観測値Ｑb_actと、各脚リンク機構３の各関節の実際の変位量の観測値θi_actは、それぞれ、姿勢センサ４２、関節変位センサ４３の検出信号から比較的精度良く得ることができる。このため、基体２及び非接地足平部１３のそれぞれの姿勢の推定値Ｑb_estm，Ｑswg_estmを高い信頼性で求めることができる。

また、基体２の実際の位置の推定処理では、接地によって外界物に対して拘束される各接地足平部１３の目標位置Ｘsup_refが実際の位置に一致するものと仮定して行われる。

そして、この場合、非接地足平部１３の位置姿勢の推定値Ｘswg_estm，Ｑswg_estmと目標位置姿勢Ｘswg_ref，Ｑswg_refとの偏差（位置及び姿勢のそれぞれの偏差）を各脚リンク機構３の各関節の目標駆動力の調整に反映させるので、該非接地足平部１３が接地によって接地足平部１３となった状態での該接地足平部１３の実際の位置姿勢と目標位置姿勢Ｘsup_ref，Ｑsup_refとの一致度合を高めることができる。

また、各接地足平部１３に作用する荷重の観測値Ｆc_actに基づいて算出した該接地足平部１３の弾性変形量ｄＸsupを、基体２の位置の推定値Ｘb_estmに反映させるので、該接地足平部１３の弾性変形の影響を適切に補償できる。

また、２つの脚リンク機構３Ｌ，３Ｒが接地リンク１３を有することとなる前記両脚支持期では、各接地リンク１３に対応して算出される基体２の位置が平均化される。

このため、基体２及び非接地足平部１３のそれぞれの実際の位置の推定値Ｘb_estm，Ｘswg_estmを高い信頼性で求めることができる。

このようにリンク位置姿勢推定部５３は、信頼性の高い推定値Ｘb_estm，Ｑb_estm，Ｘswg_estm，Ｑswg_estmを、複雑な演算処理を必要としない簡易な処理で逐次求めることができる。

ひいては、各脚リンク機構３の各関節を力制御方式で制御しつつ、ロボット１の実際の運動が、ロボット１に作用する外力の変動等に応じて基準の目標運動から乖離するのを適切に防止できる。

補足すると、本実施形態における上記の説明は、２脚歩容の動作によってロボット１の移動動作を行う場合を主要例として行ったが、例えば、脚リンク機構３Ｌ，３Ｒに加えて腕リンク機構４Ｌ，４Ｒをも脚リンク機構として動作させる４脚歩容の移動動作においても上記のリンク位置姿勢推定部５３の処理と同様の処理によって、基体２及び各非接地足平部１３、並びに、非接地状態の各ハンド部２３のそれぞれの位置及び姿勢の推定値を求めることができる。

この場合には、接地状態の各ハンド部２３と、接地足平部１３とが本発明における接地リンクに相当するものとなり、非接地状態の各ハンド部２３と、非接地足平部１３とがそれぞれ本発明における第２特定リンクに相当するものとなる。

なお、この場合には、脚リンク機構３Ｌ，３Ｒ及び腕リンク機構４Ｌ，４Ｒのそれぞれの各関節の基準の目標駆動力が、前記基準の目標運動から逆動力学モデルによって決定される。そして、脚リンク機構３Ｌ，３Ｒ及び腕リンク機構４Ｌ，４Ｒのそれぞれの各関節の基準の目標駆動力が、例えば、基体２並びに、非接地状態の各足平部１３及びハンド部２３のそれぞれの位置及び姿勢の推定値と、目標値との偏差（位置偏差及び姿勢偏差）に応じて、あるいは、該位置偏差及び姿勢偏差と、基体２並びに、非接地状態の各足平部１３及びハンド部２３のそれぞれの並進速度偏差及び角速度偏差に応じて決定される操作量により修正される。

［第２実施形態］
次に、本発明の第２実施形態を図４を参照して説明する。なお、本実施形態は、リンク位置姿勢推定部の処理だけが前記第１実施形態と相違するものである。このため、第１実施形態と同一の事項については説明を省略する。

本実施形態では、制御装置４０は、図２の括弧付きの参照符号５４で示すリンク位置姿勢推定部５４を、第１実施形態で説明したリンク位置姿勢推定部５３の代わりに備える。

図４を参照して、本実施形態におけるリンク位置姿勢推定部５４の処理を説明する。なお、本実施形態の説明は、第１実施形態と同様に、２脚歩容の動作によって、ロボット１の移動動作を行う場合を主要例として行う。

本実施形態におけるリンク位置姿勢推定部５４は、各制御処理周期において、各接地足平部１３の目標位置Ｘsup_ref及び目標姿勢Ｑsup_refと、各脚リンク機構３の各関節の現在の実際の変位量の観測値θi_actと、各接地足平部１３に作用する現在の実際の荷重の観測値Ｆc_actとを入力パラメータとして取得する。

この場合、各接地足平部１３の目標位置Ｘsup_refと、各脚リンク機構３の各関節の現在の実際の変位量の観測値θi_actと、各接地足平部１３に作用する現在の実際の荷重の観測値Ｆc_actは、前記第１実施形態におけるリンク位置姿勢推定部５３が取得するものと同じである。

また、各接地足平部１３の目標姿勢Ｑsup_refは、前記基準の目標運動のうち、現在の制御処理周期の時刻での該接地足平部１３の目標姿勢（瞬時値）である。

そして、リンク位置姿勢推定部５４は、図４に示す弾性変形量推定部７１の処理を実行する。

この弾性変形量推定部７１の処理では、リンク位置姿勢推定部５４は、各接地足平部１３に作用する荷重の観測値Ｆc_actから、該接地足平部１３に対応してあらかじめ設定された剛性係数（ばね定数）を用いて、該接地足平部１３の位置に関する弾性変形量ｄＸsup（接地足平部１３の弾性変形による位置の変化量）の推定値と、姿勢に関する弾性変形量ｄＱsup（接地足平部１３の弾性変形による姿勢の変化量）の推定値とを算出する。

次いで、リンク位置姿勢推定部５４は、演算部７２，７３の処理を実行する。演算部７２の処理では、リンク位置姿勢推定部５４は、基準の目標運動における各接地足平部１３の目標位置を、該接地足平部１３に対応して弾性変形量推定部７１で算出した弾性変形量ｄＸsupの推定値で補正する（具体的には、Ｘsup_refにｄＸsupを加算する）。

また、演算部７３の処理では、リンク位置姿勢推定部５４は、基準の目標運動における各接地足平部１３の目標姿勢を、該接地足平部１３に対応して弾性変形量推定部７１で算出した弾性変形量ｄＱsupの推定値で補正する（具体的には、Ｑsup_refにｄＱsupを加算する）。

ここで、接地によって外界物に対して拘束される各接地足平部１３の実際の位置及び姿勢は、それぞれ、目標位置Ｘsup_ref、目標姿勢Ｑsup_refに対する一致度合が比較的高いとみなすことができる。

このため、演算部７２，７３の処理によって、各接地足平部１３の実際の位置及び姿勢を、それぞれ、該接地足平部１３の弾性変形量ｄＸsup，ｄＱsupだけ補正してなる位置及び姿勢（該接地足平部１３の弾性変形が無いと仮定した場合の位置及び姿勢）が算出される。

なお、弾性変形量推定部７１及び演算部７２，７３の処理は、ロボット１の両脚支持期では、２つの接地足平部１３，１３のそれぞれについて実行され、ロボット１の片脚支持期では、一方の接地足平部１３についてのみ実行される。

次いで、リンク位置姿勢推定部５４は、順運動学演算部７４の処理を実行する。この順運動学演算部７４の処理では、リンク位置姿勢推定部５４は、演算部７２，７３による各接地足平部１３の補正後の目標位置及び目標姿勢と、各接地足平部１３を有する接地脚リンク機構３の各関節の実際の変位量の観測値θi_act（現在値）とから、順運動学の演算処理により、基体２の位置及び姿勢の暫定推定値Ｘb_temp，Ｑb_tempを算出する。

なお、各接地足平部１３を有する接地脚リンク機構３の各関節の実際の変位量の観測値θi_actは、換言すれば、基体２（第１特定リンク）と該接地足平部１３（接地リンク）との間の各関節の実際の変位量の観測値である。

上記順運動学演算部７４は、前記両脚支持期では、２つの接地足平部１３のそれぞれ毎に、各接地足平部１３の位置及び姿勢の算出値を用いて基体２の位置及び姿勢の暫定推定値Ｘb_temp，Ｑb_tempを算出する。また、前記片脚支持期では、順運動学演算部７４は、一方の接地足平部１３の位置及び姿勢の算出値を用いて基体２の位置及び姿勢の暫定推定値Ｘb_temp，Ｑb_tempを算出する。

次いで、リンク位置姿勢推定部５４は、基体２の位置及び姿勢の上記暫定推定値Ｘb_temp，Ｑb_tempに、それぞれ平均化処理部７５の処理を施す。この平均化処理部７５は、前記両脚支持期においては、２つの接地足平部１３，１３のそれぞれに対応して順運動学演算部７４で算出したＸb_tempの平均値（加算平均値）及びＱb_tempの平均値（加算平均値）を求める。また、前記片脚支持期においては、平均化処理部７５は、順運動学演算部７４で算出したＸb_temp，Ｑb_tempをそのまま出力する。

そして、リンク位置姿勢推定部５４は、かかる平均化処理部７５の処理により得られた基体２の位置及び姿勢のそれぞれの出力値に、ローパスフィルタ７６によりローパス特性のフィルタリング処理を施すことで、基体２の現在の実際の位置及び姿勢のそれぞれの推定値Ｘb_estm，Ｑb_estmを求め、それを出力する。

さらに、リンク位置姿勢推定部５４は、片脚支持期においては、前記順運動学演算部７４とは別の順運動学演算部７７の処理を実行する。この順運動学演算部７７の処理では、リンク位置姿勢推定部５４は、上記の如く求めた基体２の現在の実際の位置及び姿勢の推定値Ｘb_estm，Ｑb_estmと、非接地脚リンク機構３の各関節の実際の変位量の観測値θi_act（現在値）とから、順運動学の演算処理により、該非接地脚リンク機構３の足平部１３（非接地足平部１３）の現在の実際の位置及び姿勢のそれぞれの推定値Ｘswg_estm，Ｑswg_estmを算出し、それを出力する。

補足すると、非接地脚リンク機構３の各関節の実際の変位量の観測値θi_actは、換言すれば、基体２（第１特定リンク）と非接地足平部１３（第２特定リンク）との間の各関節の実際の変位量の観測値である。

本実施形態は、以上説明したリンク位置姿勢推定部５４の処理以外は、前記第１実施形態と同じである。なお、本実施形態では、姿勢センサ４２を省略してもよい。

補足すると、本実施形態では、弾性変形量推定部７１の処理、演算部７２，７３の処理、がそれぞれ、本発明における第７処理、第８処理に相当し、順運動学演算部７４、平均化処理部７５及びローパスフィルタの処理が、本発明における第６処理に相当する。

なお、各接地足平部１３の目標位置姿勢Ｘsup_ref，Ｑsup_refと、弾性変形量ｄＸsup，ｄＱsupと、順運動学演算部７４の処理とをそれぞれ、例えば同次変換行列の形式で表現することもできる。この場合、それぞれの同次変換行列を掛け合わせる（乗算する）ことにより、基体２の位置姿勢の暫定推定値Ｘb_tmp，Ｑb_tempを求めることもできる。

本実施形態によれば、リンク位置姿勢推定部５４は、各制御処理周期において、各接地足平部１３の目標位置Ｘsup_ref及び目標姿勢Ｑsup_ref、各脚リンク機構３の各関節の実際の変位量に観測値θi_act、及び各接地足平部１３に作用する荷重の観測値Ｆc_actを入力パラメータとして取得し、これらの入力パラメータから前記した処理によって、基体２及び非接地足平部１３のそれぞれの実際の位置及び姿勢の推定値Ｘb_estm，Ｑb_estm，Ｘswg_estm，Ｑswg_estmを求める。

このリンク位置姿勢推定部５３の処理は、接地によって外界物に対して拘束される各接地足平部１３の目標位置Ｘsup_ref及び目標姿勢Ｑsup_refがそれぞれ実際の位置、姿勢に一致するものと仮定して行われる。

この場合、非接地足平部１３の位置姿勢の推定値Ｘswg_estm，Ｑswg_estmと目標位置姿勢Ｘswg_ref，Ｑswg_refとの偏差（位置及び姿勢のそれぞれの偏差）を各脚リンク機構３の各関節の目標駆動力の調整に反映させるので、該非接地足平部１３が接地によって接地足平部１３となった状態での該接地足平部１３の実際の位置姿勢と目標位置姿勢Ｘsup_ref，Ｑsup_refとの一致度合を高めることができる。

また、各接地足平部１３に作用する荷重の観測値Ｆc_actに基づいて算出した該接地足平部１３の弾性変形量ｄＸsup，ｄＱsupを、基体２の位置姿勢の推定値Ｘb_estm，Ｑb_estmに反映させるので、該接地足平部１３の弾性変形の影響を適切に補償できる。

また、２つの脚リンク機構３Ｌ，３Ｒが接地リンク１３を有することとなる前記両脚支持期では、各接地リンク１３に対応して算出される基体２の位置姿勢の暫定推定値Ｘb_temp，Ｑb_tempが平均化される。

さらに、各脚リンク機構３の各関節の実際の変位量の観測値θi_actは、関節変位センサ４３の検出信号から比較的精度良く得ることができる。

この結果、基体２及び非接地足平部１３のそれぞれの実際の位置及び姿勢の推定値Ｘb_estm，Ｑb_estm，Ｘswg_estm，Ｑswg_estmを高い信頼性で得ることができる。

このようにリンク位置姿勢推定部５４は、信頼性の高い推定値Ｘb_estm，Ｑb_estm，Ｘswg_estm，Ｑswg_estmを、複雑な演算処理を必要としない簡易な処理で逐次求めることができる。

ひいては、各脚リンク機構３の各関節を力制御方式で制御しつつ、ロボット１の実際の運動が、ロボット１に作用する外力の変動等に応じて基準の目標運動から乖離するのを適切に防止できる。

補足すると、本実施形態における上記の説明は、２脚歩容の動作によってロボット１の移動動作を行う場合を主要例として行ったが、第１実施形態に関して補足説明した場合と同様に、脚リンク機構３Ｌ，３Ｒに加えて腕リンク機構４Ｌ，４Ｒをも脚リンク機構として動作させる４脚歩容の移動動作においても上記のリンク位置姿勢推定部５４の処理と同様の処理によって、基体２及び各非接地足平部１３、並びに、非接地状態の各ハンド部２３のそれぞれの位置及び姿勢の推定値を求めることができる。

［変形態様について］
次に、本発明の実施形態に関する変形態様をいくつか説明する。

前記各実施形態では、基体２、非接地足平部１３をそれぞれ、本発明の第１特定リンク、第２特定リンクとした場合を例にとって説明したが、第１特定リンクは、基体２以外のリンク（例えば頭部５、腕リンク機構４のいずれかのリンク等）であってもよい。また、第２特定リンクは、非接地足平部１３以外のリンク（例えば、非接地脚リンク機構３の足平部１３以外のリンク、腕リンク機構４のハンド部２３等）であってもよい。

また、前記各実施形態では、弾性変形量推定部６２，７１を備えたが、各足平部１３（接地リンク）の剛性が比較的高いような場合には、該弾性変形量推定部６２，７１を省略してもよい。

例えば第１実施形態において、弾性変形量推定部６２及び演算部６３を省略し、演算部６３の算出値の代わりに、順運動学演算部６１により算出した暫定値Ｘsup_temp1をそのまま演算部６４に入力してもよい。

また、例えば第２実施形態において、弾性変形量推定部７１及び演算部７２，７３を省略し、演算部７２，７３の算出値の代わりに、各接地足平部１３の目標位置Ｘsup_ref及び目標姿勢Ｑsup_refをそのまま順運動学演算部７４に入力してもよい。

また、前記各実施形態では、平均化処理部６６，７５を省略することも可能である。例えば、第１実施形態において、ロボット１の片脚支持期だけでなく、両脚支持期においても、１つの接地足平部１３に対応して演算部６５により算出される基体２の位置を、ローパスフィルタ６７に入力して、基体２の位置の推定値Ｘb_estmを求めるようにしてもよい。

また、同様に、第２実施形態において、ロボット１の片脚支持期だけでなく、両脚支持期においても、１つの接地足平部１３に対応して順運動学演算部７４により算出される基体２の位置の暫定推定値Ｘb_tempをローパスフィルタ７６に入力して、基体２の位置の推定値Ｘb_estmを求めるようにしてもよい。

また、前記各実施形態では、ローパスフィルタ６７，７６を備えたが、該ローパスフィルタ６７，７６を省略した構成を採用してもよい。

また、前記各実施形態では、基体２及び非接地足平部１３のそれぞれの実際の位置及び姿勢を推定したが、例えばいずれか一方のリンクの位置及び姿勢、あるいは、位置を推定し、その推定値と目標値との偏差に応じて各脚リンク機構３の各関節の目標駆動力を調整する（基準の目標駆動力から補正する）ようにしてもよい。

また、前記各実施形態では、接地足平部１３に作用する荷重を力センサ４４により検出するようにしたが、例えば逆動力学モデルを用いて、該荷重を推定するようにしてもよい。このことは、ロボット１の移動時にハンド部２３を接地させる場合についても同様である。

１…移動ロボット、２…基体（第１特定リンク）、３Ｌ，３Ｒ…脚リンク機構、１１，１２，１３…脚リンク機構を構成するリンク、１３…足平部（接地リンク又は第２特定リンク）、４０…制御装置、５３，５４…リンク位置姿勢推定部（リンク位置姿勢推定手段）、５１…関節駆動力決定部（関節駆動力制御手段）、５２…関節制御部（関節駆動力制御手段）。

Claims (10)

1. 基体を構成するリンクと該基体から延設される複数の脚リンク機構を構成するリンクとを含み、関節を介して相互に連結された複数のリンクと、各関節を駆動する関節アクチュエータとを備える移動ロボットに対し、前記関節アクチュエータにより前記脚リンク機構の動作用の各関節に作用させる駆動力を力制御方式で制御することで該移動ロボットの移動動作を行わせる制御装置であって、
   前記脚リンク機構のうち、接地状態のリンクである接地リンクを含む少なくとも１つの脚リンク機構の接地リンクの目標姿勢と前記移動ロボットの複数のリンクのうちのあらかじめ定められた第１特定リンクの実際の姿勢の観測値とのうちの少なくともいずれか一方と、前記接地リンクの目標位置と、前記接地リンクと前記第１特定リンクとの間の各関節の実際の変位量の観測値とを少なくとも含む入力パラメータを逐次取得し、該入力パラメータを用いて、順運動学の演算処理を少なくとも含む処理を実行することにより前記第１特定リンクの実際の位置姿勢を逐次推定するように構成されたリンク位置姿勢推定手段と、
   前記リンク位置姿勢推定手段による前記第１特定リンクの実際の位置姿勢の推定値と該第１特定リンクの目標位置姿勢との偏差と、前記移動ロボットの複数のリンクのうちの前記第１特定リンクと異なる非接地状態の第２特定リンクの実際の位置姿勢の推定値であって、前記第１特定リンクの実際の位置姿勢の推定値、並びに、前記第１特定リンクと前記第２特定リンクとの間の各関節の変位量の観測値から順運動学の演算処理によって算出される該第２特定リンクの実際の位置姿勢の推定値と該第２特定リンクの目標位置姿勢との偏差とのうちの少なくともいずれか一方の偏差に応じて、当該偏差を低減するように前記脚リンク機構の動作用の各関節に作用させる駆動力を前記関節アクチュエータを介して調整するように構成された関節駆動力制御手段とを備えることを特徴とする移動ロボットの制御装置。
2. 請求項１記載の移動ロボットの制御装置において、
   前記第２特定リンクは、前記脚リンク機構のそれぞれの先端部のリンクであり、
   前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において前記先端部のリンクが空中移動を行っているときに、前記第１特定リンクの実際の位置姿勢を逐次推定することに加えてさらに、該第１特定リンクの実際の位置姿勢の推定値、並びに、該第１特定リンクと前記先端部のリンクとの間の各関節の変位量の観測値から順運動学の演算処理によって、該先端部のリンクの実際の位置姿勢を逐次推定するように構成されており、
   前記関節駆動力制御手段は、前記移動ロボットの移動動作において前記先端部のリンクが空中移動を行っているときに、少なくとも該先端部のリンクの実際の位置姿勢の推定値と該先端部のリンクの目標位置姿勢との偏差に応じて、当該偏差を低減するように前記脚リンク機構の動作用の各関節に作用させる駆動力を前記関節アクチュエータを介して調整するように構成されていることを特徴とする移動ロボットの制御装置。
3. 請求項１又は２記載の移動ロボットの制御装置において、
   前記入力パラメータは、前記第１特定リンクの実際の姿勢の観測値を含んでおり、
   前記リンク位置姿勢推定手段は、前記第１特定リンクの実際の位置の推定値を新たに決定するとき、前記第１特定リンクの現在の実際の姿勢の観測値と、過去に決定した前記第１特定リンクの実際の位置の推定値のうちの最新値と、前記接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって、前記第１特定リンクの現在の実際の位置が前記最新値に一致すると仮定した場合における前記接地リンクの位置の暫定値を算出する第１処理と、該接地リンクの位置の暫定値と該接地リンクの目標位置との偏差により前記最新値を補正することによって前記第１特定リンクの実際の位置の新たな推定値を決定する第２処理とを実行し、さらに、前記第１特定リンクの実際の姿勢の観測値を該実際の姿勢の推定値とするように構成されていることを特徴とする移動ロボットの制御装置。
4. 請求項３記載の移動ロボットの制御装置において、
   前記入力パラメータは、前記接地リンクに作用する実際の荷重の観測値をさらに含んでおり、
   前記リンク位置姿勢推定手段は、前記荷重の観測値を用いて前記接地リンクの弾性変形量を推定する第３処理と、前記第１処理で算出した前記接地リンクの位置の暫定値を、前記弾性変形量の推定値により補正する第４処理とをさらに実行し、前記第２処理では、前記接地リンクの位置の暫定値と該接地リンクの目標位置との偏差の代わりに、前記第４処理による前記接地リンクの位置の暫定値の補正後の値と該接地リンクの目標位置との偏差により前記最新値を補正することによって前記第１特定リンクの実際の位置の新たな推定値を決定するように構成されていることを特徴とする移動ロボットの制御装置。
5. 請求項３記載の移動ロボットの制御装置において、
   前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンク毎に前記第１処理を実行し、前記第２処理では、各接地リンクの位置の暫定値と該接地リンクの目標位置との偏差により前記最新値を補正することによって各接地リンクに対応して得られる前記第１特定リンクの実際の位置の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置の新たな推定値を決定するように構成されていることを特徴とする移動ロボットの制御装置。
6. 請求項４記載の移動ロボットの制御装置において、
   前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンク毎に前記第１処理、第３処理及び第４処理を実行し、前記第２処理では、各接地リンクの位置の暫定値の前記補正後の値と該接地リンクの目標位置との偏差により前記最新値を補正することによって各接地リンクに対応して得られる前記第１特定リンクの実際の位置の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置の新たな推定値を決定するように構成されていることを特徴とする移動ロボットの制御装置。
7. 請求項１又は２記載の移動ロボットの制御装置において、
   前記入力パラメータは、前記接地リンクの目標姿勢を含んでおり、
   前記リンク位置姿勢推定手段は、前記第１特定リンクの実際の位置姿勢の推定値を新たに決定するとき、前記接地リンクの目標位置姿勢の現在値と、前記接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって、前記第１特定リンクの実際の位置姿勢の新たな推定値を決定する第６処理を実行するように構成されていることを特徴とする移動ロボットの制御装置。
8. 請求項７記載の移動ロボットの制御装置において、
   前記入力パラメータは、前記接地リンクに作用する実際の荷重の観測値をさらに含んでおり、
   前記リンク位置姿勢推定手段は、前記荷重の観測値を用いて前記接地リンクの弾性変形量を推定する第７処理と、前記接地リンクの目標位置姿勢の現在値を前記弾性変形量の推定値により補正する第８処理とをさらに実行し、前記第６処理では、前記接地リンクの目標位置姿勢の現在値の代わりに、前記第８処理による補正後の値を用いて、前記第１特定リンクの実際の位置姿勢の新たな推定値を決定するように構成されていることを特徴とする移動ロボットの制御装置。
9. 請求項７記載の移動ロボットの制御装置において、
   前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間での前記第６処理では、各接地リンクの目標位置姿勢の現在値と、該接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって各接地リンクに対応して算出される前記第１特定リンクの位置姿勢の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリング処理を施すことによって、該第１特定リンクの実際の位置姿勢の新たな推定値を決定するように構成されていることを特徴とする移動ロボットの制御装置。
10. 請求項８記載の移動ロボットの制御装置において、
    前記リンク位置姿勢推定手段は、前記移動ロボットの移動動作において複数の脚リンク機構がそれぞれ接地リンクを有する期間では、各接地リンク毎に前記第７処理及び第８処理を実行し、さらに前記第６処理では、各接地リンクの目標位置姿勢の現在値の前記補正後の値と、該接地リンクと前記第１特定リンクとの間の各関節の現在の変位量の観測値とから順運動学の演算処理によって各接地リンクに対応して算出される前記第１特定リンクの位置姿勢の推定値を平均化することによって、又は、当該平均化により算出される値にローパス特性のフィルタリングを施すことによって、該第１特定リンクの実際の位置姿勢の新たな推定値を決定するように構成されていることを特徴とする移動ロボットの制御装置。