JP5487756B2 - ロボット制御装置、ロボット制御プログラムおよびロボット制御方法 - Google Patents

Description

本発明は、ロボット制御装置、ロボット制御プログラムおよびロボット制御方法に関する。

近年、２輪駆動の移動ロボットが自律走行等を行う際、自己の位置を車輪の回転量によって推定するオドメトリの技術が利用されている。オドメトリとは、２つの車輪の各回転速度からロボットの併進速度およびロボット本体の角速度を求め、これらを積分してロボットの位置および姿勢を推定する技術である。

かかるオドメトリの技術では、２つの車輪の各半径および２つの車輪間の距離が、ロボットの自己位置推定の精度を左右する重要なパラメータとなる。

ところが、２つの車輪のどちらか一方若しくは両方の車輪の半径、または車輪間の距離が、ロボットの経年の運用または衝突等により変化する場合があり、自己位置推定の精度に影響を及ぼす。また、車輪の速度を制御するギア間の噛み合わせの隙間（バックラッシュ）も自己位置推定誤差の原因となる。

そこで、従来では、ロボット管理者が、オドメトリに関わるパラメータの誤差を定期的に点検して、誤差に応じたパラメータの補正や部品の交換を行っていた。

また、２輪駆動の移動ロボットをスタート位置から特定距離にあるゴール位置に向けて走行させて、走行後に撮像した目印の撮像画像とあらかじめ撮像した同一目印の画像とを用いて、当該移動ロボットのオドメトリに関わるパラメータの誤差を調整するパラメータ調整装置が開示されている。

特開２００７−１５６５７６号公報

しかしながら、移動ロボットの自己位置推定に関わる誤差を調整する従来の技術では、当該誤差を正確且つ効率的に調整することができないという問題がある。

すなわち、従来のパラメータ調整装置では、走行後に目印を撮像する場合、目印までの撮像距離等の撮像条件が変わると撮像精度も変わってしまい、撮像誤差によりオドメトリに関わるパラメータの誤差を正確に特定することができない。その結果、パラメータ調整装置は、オドメトリに関わるパラメータの誤差を正確に調整することができない。

また、ロボット管理者による誤差の調整では、作業時間を多く要し、コスト高となってしまう。

本発明は、上記に鑑みてなされたものであって、移動ロボットの自己位置推定に関わる誤差を正確且つ効率的に調整することができるロボット制御装置、ロボット制御プログラムおよびロボット制御方法を提供することを目的とする。

上述した問題を解決し、目的を達成するために、ロボット制御装置は、ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御部と、当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測部と、前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出部とを備える構成を採る。

以上により、ロボット制御装置は、ロボットの車輪に関わる誤差を正確且つ効率的に算出することができ、当該誤差の調整精度を向上させることができるという効果を奏する。

図１は、実施例１に係るロボット制御装置の構成を示す機能ブロック図である。 図２は、実施例１に係るロボット制御装置の計測を説明するための図である。 図３は、移動ロボット運動学モデルの説明を示す図である。 図４は、実施例２に係るロボット制御装置の構成を示す機能ブロック図である。 図５は、座標の定義を示す図である。 図６は、旋回後ロボット本体の絶対座標を算出する方法を示す図である。 図７は、実施例２に係るロボット制御装置の誤差補正手順を示すフローチャートである。 図８は、実施例３に係るロボット制御装置の構成を示す機能ブロック図である。 図９は、ガタ量を算出する方法を示す図である。 図１０は、実施例３に係るロボット制御装置のガタ量補正手順を示すフローチャートである。

以下に、本発明に係るロボット制御装置、ロボット制御プログラムおよびロボット制御方法の実施例を図面に基づいて詳細に説明する。なお、本実施例によりこの発明が限定されるものではない。

図１は、本実施例１に係るロボット制御装置の構成を示す機能ブロック図である。図１に示すように、ロボット制御装置１は、移動制御部１０、計測部２０および算出部３０を有している。

ロボット制御装置１は、車輪に関わるパラメータを用いてロボット本体の自己位置を推定する装置あり、特にオドメトリによる自己位置推定を行う。

移動制御部１０は、ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する。ここで、所定角度とは、例えば３６０度や３６０度の複数倍であるがこれに限定されず、１度や２度等旋回前後の位置が近似した位置となるような角度であれば良い。

計測部２０は、ロボット本体から離間した位置にある目印（以下、「ランドマーク」という。）を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する。ここで、ロボット本体の旋回前後に関わるロボット位置およびロボット方位の計測について、図２を参照して説明する。図２は、実施例１に係るロボット制御装置１のロボット位置およびロボット方位の計測を説明するための図である。

移動ロボットは、旋回前に所定のランドマークを抽出する。そして、移動ロボットは、ロボット本体の旋回開始位置から、抽出されたランドマークに対して相対位置および相対方位を計測して、ランドマークに対する自己のロボット位置およびロボット方位とする。なお、計測方法として、例えば２台のステレオカメラをロボットに搭載してステレオ視覚を用いて計測する方法があるが、これに限定されるものではない。また、ランドマークには、目印として特徴があり計測しやすい標的であれば良く、例えばロボットの充電機がある。

次に、移動ロボットは、ロボット本体の旋回開始位置から所定位置を中心にして、旋回前後の位置が近似した位置となる所定角度、例えば３６０度旋回する。そして、移動ロボットは、旋回後の旋回終了位置から、旋回前と同じランドマークに対して相対位置および相対方位を計測して、ランドマークに対する自己のロボット位置およびロボット方位とする。

ここで、移動ロボットは、例えば所定角度を３６０度とした場合、３６０度旋回指示に基づいて旋回しているため、車輪に関わる誤差がなければ、３６０度旋回後には旋回前の位置および方位に戻ることができる。ところが、移動ロボットは、車輪に関わる誤差があると、３６０度旋回後には旋回前の位置および方位に戻ることができない。すなわち、旋回後のランドマークに対する自己のロボット位置は、旋回前のランドマークに対する自己のロボット位置と同一にならない。そして、旋回後のロボット方位も同様に旋回前のロボット方位と同一にならない。

そこで、旋回前後のロボット位置およびロボット方位を用いて、車輪に関わる誤差を、後記する算出部３０によって算出する。

図１に戻って、算出部３０は、計測部２０にて計測した旋回前後に関わるロボット位置間の差分および旋回前後に関わるロボット方位間の差分に基づき、車輪に関わる誤差を算出する。

上記実施例１によれば、ロボット制御装置１は、ロボット本体を旋回前後の位置が近似した位置となるような所定角度旋回させてロボット本体の位置等を計測しているため、旋回前後のロボット本体の位置が近似し、旋回前後の計測条件の変動も小さく済むこととなる。そのため、ロボット制御装置１は、ロボットの車輪に関わる誤差を正確に算出することができ、当該誤差の調整精度を向上させることができる。

また、ロボット制御装置１は、ロボットの車輪に関わる誤差を自動的に算出することができ、その後、当該誤差を補正することにより効率的な調整を行うことができる。

ところで、移動ロボットのオドメトリによる自己位置推定は、運動学モデルを用いて行われる。この運動学モデルについて、図３を参照して説明する。図３は、移動ロボット運動学モデルの説明を示す図である。

まず、移動ロボットの運動学モデルを説明するための定義を行う。ロボットは、２つの車輪を有するものとする。そして、ロボットの位置および方位は、座標軸をＸ軸（Ｘ_Ｇ）およびＹ軸（Ｙ_Ｇ）とした絶対座標系と、座標軸をＸ軸（Ｘ_Ｒ）およびＹ軸（Ｙ_Ｒ）として且つロボットの中心を原点としたロボット座標系とから表される。ロボットの位置及び方位は、図３の例では、絶対座標系では（ｘ_Ｇ、ｙ_Ｇ、θ_Ｇ）で表され、ロボット座標系では（０、０、θ）で表される。ここで、θ_Ｇは、絶対座標系の原点からロボットに対する絶対方位であり、θは、ロボット座標系の原点からロボットが視ている相対方位、すなわちロボットの姿勢を指す。

また、ロボットの２つの車輪は、それぞれ右車輪の半径をｒ_１（単位：ｍ、以下同じ）、左車輪の半径をｒ_２とし、両車輪間の距離をl（単位：ｍ）とし、これらが運動学モデルでは重要な、車輪に関わるパラメータとなる。

さらに、車輪の回転速度は、それぞれ右車輪の回転速度をφ_１（単位：ｒａｄ／秒、以下同じ）、左車輪の回転速度をφ_２とする。また、ロボット本体の前進速度をｖ（単位：ｍ／秒）、ロボット本体の回転速度をω（単位：ｍ／秒）とする。

例えば、正転方向にφ_１の回転速度で右車輪を回転させ、正転方向と反対方向に−φ_１の回転速度で左車輪を回転させると、ロボット本体は（ｘ_Ｇ、ｙ_Ｇ）を中心にその場で旋回して、ロボット本体の回転速度ωが発生する。また、φ_１＝φ_２の回転速度で右車輪および左車輪を回転させると、ロボット本体の回転速度ωは発生しないが、ロボットの前進速度ｖが発生する。つまり、左右車輪の回転速度φ_１およびφ_２によってロボット本体の絶対座標が決定されることになる。

このような定義の下、移動ロボットの運動学モデルでは、絶対座標系で観測されるロボットの状態、すなわちロボットの絶対位置（ｘ、ｙ）および絶対方位θを式（１）および（２）のように表すことができる。なお、式（２）では、ロボットの状態をロボットの速度として表している。

式（２）により、ロボットの絶対位置および絶対方位（ｘ_Ｇ、ｙ_Ｇ、θ_Ｇ）は、左右車輪の回転速度φ_１およびφ_２以外に車輪に関わるパラメータとなる車輪の半径ｒ_１、ｒ_２および両車輪間の距離ｌによって決定される。

そして、ロボット座標系から絶対座標系へのロボットの速度に関する変換は、式（４）で表される回転変換の行列を用いて以下の式（３）のようになる。

さらに、ロボット座標系でのロボットの右車輪の回転速度ω_１は、右車輪の回転速度φ_１を用いて、以下の式（５）のようになる。また、ロボットの左車輪の回転速度ω_２は、左車輪の回転速度φ_２を用いて、以下の式（６）のようになる。

そして、車輪の回転速度から導出されロボットの自己位置を推定する際に使用される運動方程式は、回転変換の行列（８）を用いて式（７）のようになる。

上記した運動方程式によると、運動方程式に設定された車輪の半径ｒ_１、ｒ_２または両車輪間の距離ｌが、実際の車輪の半径または両車輪間の距離とそれぞれ異なると、移動後のロボットの絶対位置および方位（ｘ_Ｇ、ｙ_Ｇ、θ_Ｇ）が不正確となる。例えば、時間の経過による摩擦、さらには衝突等によって車輪の半径または車輪間の距離が変化することがある。その場合、運動学モデル（運動方程式）にパラメータとして設定された車輪の半径または車輪間の距離と実際の車輪の半径または車輪間の距離との間でそれぞれ誤差が生じることとなり、その結果、運動学モデルを用いた自己位置推定に誤差が生じることになる。したがって、車輪の半径または車輪間の距離が変化した場合には、運動学モデルにパラメータとして設定される車輪の半径または車輪間の距離を含む車輪に関わるパラメータを補正することが重要となる。

そこで、実施例２では、車輪に関わるパラメータのうち車輪の半径および車輪間の距離に関わる誤差を算出して、算出された誤差に基づき、当該車輪に関わるパラメータを補正する場合について説明する。

図４は、本実施例２に係るロボット制御装置２の構成を示す機能ブロック図である。なお、図１に示すロボット制御装置１と同一の構成については同一符号を付すことで、その重複する構成および動作の説明については省略する。

図４に示すロボット制御装置２と実施例１に係るロボット制御装置１とが異なるところは、車輪に関わる誤差を算出する算出部３０に旋回後絶対座標算出部３１および誤差算出部３２を追加し、指示部４０、記憶部５０、補正部６０および報知部７０を追加した点にある。

指示部４０は、車輪に関わる誤差を補正するよう指示する機能部であり、パラメータ誤差補正指示部４１を有する。

パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出後、車輪に関わるパラメータの誤差を補正部６０に補正させるために、所定位置を中心にして当該ロボット本体を３６０度旋回させる旋回指示を移動制御部１０に対して行う。なお、パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出できない場合には、場所を移動して計測可能なランドマークを抽出後、当該旋回指示を行う。なお、ランドマークは、ロボット本体から離間した位置にあるものとする。

移動制御部１０は、所定位置を中心にしてロボット本体を３６０度旋回させる旋回指示をパラメータ誤差補正指示部４１から取得すると、ロボット本体を旋回させるために必要な両車輪の回転速度を記憶部５０から読み出す。また、移動制御部１０は、記憶部５０から読み出された両車輪の回転速度に基づいて、所定位置を中心にロボット本体を３６０度旋回させる場合に要する動作時間を算出する。この動作時間の算出は、例えばノミナル運動モデルを用いて算出される。

また、移動制御部１０は、ロボット本体の旋回前に車輪内のギアによるバックラッシュを除去する動作を行う。具体的には、移動制御部１０は、ロボット本体の旋回を開始する前に、車輪内の第１ギア部および第２ギア部同士を噛合わせて第１ギア部および第２ギア部をロボット本体の旋回方向にゆっくりと回転させる。そして、移動制御部１０は、第１ギア部の旋回方向側の噛合部位と第２ギア部の噛合部位とが完全に接触した状態で、ロボット本体の旋回を開始する。

なお、バックラッシュとは、車輪内の噛合っている第１ギア部と第２ギア部との噛合わせの隙間（遊び部分）のことである。このように、ロボット本体の旋回前にバックラッシュを除去することにより、バックラッシュによる車輪の回転量の誤差を排除することができ、車輪に関わるパラメータの誤差をより正確に算出することができる。

また、移動制御部１０は、ロボット本体の旋回前（バックラッシュ除去後）およびロボット本体の旋回後に、ロボット本体の位置および方位を計測するように計測部２０に指示する。

さらに、移動制御部１０は、バックラッシュ除去後の旋回開始位置から所定位置を中心にして、算出された動作時間分、ロボット本体を旋回させる。

計測部２０は、パラメータ誤差補正指示部４１によって抽出されたランドマークを基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する。具体的には、計測部２０は、ロボット本体の旋回前に、ロボット本体の位置および方位を計測する旨の指示を移動制御部１０から取得すると、ロボット本体の旋回開始位置からランドマークに対して相対位置および相対方位を計測する。そして、計測部２０は、計測した相対位置および相対方位を、ランドマークに対する自己のロボット位置およびロボット方位として記憶部５０に格納する。

また、計測部２０は、ロボット本体の旋回後にロボット本体の位置および方位を計測する旨の指示を移動制御部１０から取得すると、ロボット本体の旋回終了位置からランドマークに対して相対位置および相対方位を計測する。そして、計測部２０は、計測した相対位置および相対方位を、ランドマークに対する自己のロボット位置およびロボット方位として記憶部５０に格納する。

記憶部５０は、車輪に関わるパラメータの誤差を算出するために必要な値を記憶する機能部である。具体的には、記憶部５０は、ロボット本体の旋回前後のロボット位置およびロボット方位を記憶する。また、記憶部５０は、車輪に関わるパラメータ内の複数の要素、すなわち、両車輪の各半径、両車輪間の距離および両車輪の各回転速度をあらかじめ記憶する。

旋回後絶対座標算出部３１は、計測部２０によって計測された旋回前後のランドマークに対する自己のロボット位置および旋回開始位置に基づき、旋回後のロボットの絶対位置（旋回終了位置）を算出する。なお、旋回開始位置は、絶対座標系上の絶対位置を指す。

また、旋回後絶対座標算出部３１は、計測部２０によって計測された旋回前後のランドマークに対する自己のロボット方位および旋回開始位置でのロボットの絶対方位に基づき、旋回終了位置でのロボットの絶対方位を算出する。なお、旋回終了位置でのロボット本体の絶対座標（絶対位置および絶対方位）を算出する方法は後に詳述するものとする。

誤差算出部３２は、旋回後絶対座標算出部３１によって算出されたロボットの旋回前後の絶対座標の差分に基づき、車輪の半径が変化したことによる誤差または車輪間の距離が変化したことによる誤差を算出する。例えば、誤差算出部３２は、車輪の半径に関する誤差として、実際の左右車輪の半径の比率を算出する。また、誤差算出部３２は、車輪間の距離に関する誤差として、パラメータに設定された車輪間距離に対する実際の車輪間距離の比率を算出する。

ここで、車輪に関わるパラメータ内の車輪の半径または車輪間の距離に関わる誤差の算出方法について、図５および図６を参照して説明する。図５は、座標の定義を示す図であり、図６は、旋回後のロボット本体の絶対座標を算出する方法を示す図である。

まず、図５では、ロボットから離間した位置にあるランドマークと、旋回中心に旋回した旋回前後のロボットが絶対座標系上に表されている。ロボット本体の旋回開始位置の絶対座標は、Ｇ_０（ｘ_０、ｙ_０、θ_０）である。

また、旋回前のロボット本体のランドマークに対する相対位置および相対方位は、相対座標Ｍ_０（ｘ_ｍ０、ｙ_ｍ０、θ_ｍ０）として定義される。旋回中心を中心に旋回開始位置から３６０度旋回後のロボット本体のランドマークに対する相対位置および相対方位は、相対座標Ｍ_１（ｘ_ｍ１、ｙ_ｍ１、θ_ｍ１）として定義される。

図５における座標の定義の下、相対座標Ｍ_０およびＭ_１および旋回前のロボット本体の絶対座標Ｇ_０（ｘ_０、ｙ_０、θ_０）に基づいて、旋回後のロボット本体の絶対座標Ｇ_１（ｘ_１、ｙ_１、θ_１）が算出される。

図６は、旋回後のロボット本体の絶対座標を算出する方法を示す図である。

まず、旋回前のロボット本体の絶対方位と、旋回前のロボット本体のランドマークに対する相対方位とに基づいて、式（９）に示すようにランドマークの絶対方位を算出する。

次に、ランドマークの絶対方位と、旋回後のロボット本体の絶対方位および旋回後のロボット本体のランドマークに対する相対方位との関係は、式（１０）のようになる。

そうすると、式（９）および式（１０）によって、式（１１）に示すように、旋回後のロボット本体の絶対方位は、式（１１）に示すように算出できる。

また、旋回前のロボット本体の絶対位置および旋回前のロボット本体のランドマークに対する相対位置に基づいて、式（１２）に示すようにランドマークの絶対位置を算出する。なお、相対位置を絶対座標系に変換するために、回転変換の行列（１３）を使用する。

次に、ランドマークの絶対位置と、旋回後のロボット本体の絶対位置および旋回後のロボット本体のランドマークに対する相対位置との関係は、式（１４）のようになる。

そうすると、式（１２）および式（１４）によって、式（１５）に示すように旋回後のロボット本体の絶対位置は、式（１５）に示すように算出できる。

ここで、移動ロボットの運動学モデルにより、車輪の回転速度が常数の場合、ロボットの動作時間ｔに応じてロボットの絶対方位θは、式（１６）より式（１７）に変換して表わすことができる。なお、旋回前のロボットの絶対座標は（０、０、０）であるものとする。また、φ_１およびφ_２はそれぞれ右車輪および左車輪の回転速度、ｒ_１およびｒ_２はそれぞれ右車輪および左車輪の半径、ｌは両車輪間の距離を指すものとする。

また、同様に、ロボットの絶対位置ｘは、式（１８）より式（１９）に変換して表わすことができる。

さらに、ロボットが３６０度旋回した後ではロボットの方位（姿勢）が２π付近となるため、ｓｉｎ（ｘ）をｘで代入すると、３６０度旋回後のロボットの絶対方位および絶対位置は、式（２０）および（２１）により表わすことができる。なお、Ｃは、２πを指すものとする。

ここで、両車輪の回転速度を常数にしてロボット本体を３６０度旋回させるという条件により、以下に示すように変数を定義することができる。

例えば、車輪の半径が変化したことによる誤差ｅ_１を、式（２２）のように、左右車輪の半径の比率で表わすものとする。ここでは、誤差ｅ_１は、右車輪の半径ｒ_１に対する左車輪の半径ｒ２の比率を指している。

また、車輪間の距離が変化したことによる誤差ｅ_２を、式（２３）のように、パラメータに設定された車輪間距離Ｌに対する実際の車輪間距離ｌの比率で表わすものとする。

また、車輪の回転速度φ_１、φ_２および旋回時間ｔは、それぞれ式（２４）および式（２５）のように表わされる。なお、φは、あらかじめ定められた回転速度であるものとする。また、旋回時間ｔは、ロボット本体が３６０度旋回するのに要する動作時間Ｔである。

これらの変数を式（２０）および（２１）に代入し、さらにロボット旋回前後の絶対方位の差分を式（２０）の左辺に代入し、ロボット旋回前後の絶対位置の差分を式（２１）の左辺に代入して、車輪の半径に関わる誤差ｅ_１および車輪間距離に関わる誤差ｅ_２を算出する。すなわち、式（１１）によって算出されたロボット旋回後の絶対方位θ_{R_new}（以降、θ_１とする。）を式（２０）の左辺に代入し、式（１５）によって算出されたロボット旋回後の絶対位置Ｐ_{R_new}（以降、ｘ_１とする。）を式（２１）の左辺に代入する。これにより、ロボット本体が３６０度旋回するのに要する動作時間Ｔだけ旋回した結果、旋回後の絶対座標が旋回前の絶対座標（０、０、０）との差分になり、差分に応じた誤差（ｅ_１およびｅ_２）を算出できる。

その結果、車輪の半径が変化したことによる誤差ｅ_１は、式（２６）によって表され、車輪間の距離が変化したことによる誤差ｅ_２は、式（２７）によって表わされる。

このようにして、車輪に関わるパラメータ内の車輪の半径に関する誤差ｅ_１および車輪間の距離に関する誤差ｅ_２をそれぞれ算出することができる。

図４に戻って、補正部６０は、車輪に関わるパラメータを補正する機能部であり、パラメータ補正部６１を有する。

パラメータ補正部６１は、誤差算出部３２にて算出された誤差に基づき、車輪に関わるパラメータを補正する。例えば、パラメータ補正部６１は、誤差算出部３２にて算出された車輪の半径に関わる誤差ｅ_１および車輪間の距離に関わる誤差ｅ_２を前述した運動モデルで使用される運動方程式のパラメータに反映させるように補正する。

具体的には、式（２６）による車輪の半径が変化したことによる誤差ｅ_１を用いて、ロボットが移動する際に使用する運動方程式（式（７））の車輪の半径を補正する。すなわち、運動方程式の左車輪の半径ｒ_２を、式（２２）を用いてｅ_１×ｒ_１に置換する。また、式（２７）による車輪間の距離が変化したことによる誤差ｅ_２を用いて、ロボットが移動する際に使用する運動方程式（式（７））の車輪間の距離を補正する。すなわち、運動方程式の両車輪間の距離ｌを、式（２３）を用いてｌ×ｅ_２に置換する。以下の式（２８）が補正後の運動方程式となる。

なお、パラメータ補正部６１は、移動制御部１０、計測部２０および算出部３０によって実行される誤差分析を、あらかじめ定められた回数分実行して、誤差分析結果の平均を用いて車輪に関わるパラメータを補正しても良い。

報知部７０は、誤差算出部３２にて算出された誤差が異常閾値を超えた場合、車輪の異常を報知する。具体的には、報知部７０は、車輪の半径に関わる誤差または車輪間の距離に関わる誤差のうちいずれか一方でも異常閾値を越えた場合、異常がある旨を、ロボットを管理する計算機システムに送信する。例えば、異常には、車輪の異常、計測機器の異常または組み立て不良による異常を含むものとする。

次に、実施例２に係るロボット制御装置の誤差補正手順を、図７を参照して説明する。図７は、実施例２に係るロボット制御装置の誤差補正手順を示すフローチャートである。

まず、パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出して（ステップＳ１１）、所定位置を中心にしてロボット本体を３６０度旋回させるように移動制御部１０に指示する。

すると、移動制御部１０は、ロボット本体の旋回前に車輪内のギアによるバックラッシュを除去する動作を行う（ステップＳ１２）。これにより、移動制御部１０は、バックラッシュによる車輪の回転量の誤差を排除することができるため、車輪に関わるパラメータの誤差をより正確に算出することができる。

バックラッシュ除去後、計測部２０は、ロボット本体の旋回開始位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ１３）、ランドマークに対する自己のロボット位置およびロボット方位とする。

そして、移動制御部１０は、所定位置を中心にして、旋回開始位置からロボット本体を３６０度旋回させる場合に要する動作時間分、ロボット本体を旋回させる（ステップＳ１４）。なお、移動制御部１０は、例えばノミナル運動モデルを使って、記憶部５０に記憶された両車輪の回転速度に基づいた動作時間を算出する。

ロボット本体の旋回後、計測部２０は、ロボット本体の旋回終了位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ１５）、ランドマークに対する自己のロボット位置およびロボット方位とする。

引き続き、旋回後絶対座標算出部３１は、旋回前後のランドマークに対する自己のロボット位置およびロボット方位と、旋回開始位置およびロボットの絶対方位とに基づき、旋回後のロボットの絶対位置および絶対方位を算出する。そして、誤差算出部３２は、ロボットの旋回前後の絶対座標（絶対位置、絶対方位）の差分に基づき、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差をそれぞれ算出する（ステップＳ１６）。

また、誤差算出部３２は、車輪に関わるパラメータに誤差があるか否かを判定する（ステップＳ１７）。具体的には、誤差算出部３２は、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差のうちいずれか一方でも誤差があるか否かを判定する。

そして、誤差算出部３２は、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差のうちいずれも誤差がない場合（ステップＳ１７Ｎｏ）、誤差補正処理を終了する。

一方、誤差算出部３２は、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差のうちいずれか一方でも誤差がある場合（ステップＳ１７Ｙｅｓ）、当該誤差が異常閾値を超えているか否かを判定する（ステップＳ１８）。

そして、誤差算出部３２は、誤差が異常閾値を超えていない場合（ステップＳ１８Ｎｏ）、車輪の半径に関わる誤差ｅ１および車輪間の距離に関わる誤差ｅ２を前述した運動モデルに用いられる運動方程式のパラメータに反映させるように補正する（ステップＳ１９）。

一方、誤差算出部３２は、誤差異常閾値を超えている場合（ステップＳ１８Ｙｅｓ）、異常がある旨を、ロボットを管理する計算機システムに報知する（ステップＳ２０）。

上記実施例２によれば、ロボット制御装置２は、ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始し、当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置及びロボット方位を計測し、計測した旋回前後に関わるロボット位置間の差分及び旋回前後に関わるロボット方位間の差分に基づき、車輪に関わるパラメータ内の車輪の半径に関する誤差および車輪間の距離に関する誤差を算出するようにした。さらに、ロボット制御装置２は、算出した誤差に基づき、車輪に関わるパラメータを補正するようにした。

かかる構成によれば、ロボット制御装置２は、ロボット本体を３６０度旋回させてロボット本体の位置等を計測しているため、旋回前後のロボット本体の位置が近似し、旋回前後の計測条件の変動も小さく済むこととなる。そのため、ロボット制御装置２は、ロボットの車輪に関わるパラメータの誤差を正確に算出することができ、オドメトリによる自己位置推定の調整精度を向上させることができる。

また、ロボット制御装置２は、ロボットの車輪に関わるパラメータの誤差を自動的に算出することができ、その後、当該パラメータを補正することによりオドメトリによる自己位置推定の調整を効率的に行うことができる。

さらに、ロボット制御装置２は、ロボットを３６０度旋回させる場所と目印とがあれば、車輪に関わるパラメータの誤差を調整することができるため、ロボットを直進させて当該誤差を調整するよりも場所を選ばないで容易に調整することができる。

なお、上記実施例２においては、移動制御部１０は、ロボットから離間した位置を中心にして、ロボット本体を３６０度旋回させるようにした。しかし、移動制御部１０は、ロボット本体の２つの車輪間の中央位置を中心にして、ロボット本体を３６０度旋回させるようにしても同様の効果が得られることは言うまでもない。

また、上記実施例２においては、パラメータ誤差補正指示部４１は、ロボット本体を３６０度旋回させる旋回指示を行うものとした。しかし、パラメータ誤差補正指示部４１は、誤差を拡大させるために、ロボット本体を３６０度複数回旋回させる旋回指示をしても同様の効果が得られることは言うまでもない。

ところで、実施例２では、車輪に関わるパラメータのうち車輪の半径および車輪間の距離に関わる誤差を算出して、算出された誤差に基づき当該車輪に関わるパラメータを補正して、オドメトリによる自己位置推定の調整精度を向上させる場合について説明した。しかし、車輪に関わるパラメータを補正した後、バックラッシュによる間隙距離（以降、「ガタ量」という。）を算出して、算出されたガタ量に基づき当該車輪の回転量を補正して、オドメトリによる自己位置推定の調整精度をさらに向上させるようにしても良く、実施例３として、以下に説明する。

図８は、本実施例３に係るロボット制御装置３の構成を示す機能ブロック図である。なお、図４に示すロボット制御装置２と同一の構成については同一符号を付すことで、その重複する構成および動作の説明については省略する。

図８に示すロボット制御装置３と実施例２に係るロボット制御装置２とが異なるところは、指示部４０にガタ補正指示部４２、算出部３０にガタ量算出部３３および補正部６０にガタ量補正部６２を追加した点にある。

ガタ補正指示部４２は、計測可能なランドマークを抽出後、ガタ量を補正部６０に補正させるために、所定位置を中心にしてロボット本体を所定角度旋回させる旋回指示を移動制御部１０に対して行う。なお、パラメータ誤差補正指示部４１は、計測可能なランドマークを抽出できない場合には、場所を移動して計測可能なランドマークを抽出後、当該旋回指示を行う。また、ガタ補正指示部４２は、車輪に関わるパラメータを補正した後、連続して旋回指示を行っても良いし、連続しないで改めて旋回指示を行っても良い。

移動制御部１０は、旋回指示をガタ補正指示部４２から取得すると、車輪内の第１ギア部および第２ギア部同士を噛合わせて第１ギア部および第２ギア部をロボット本体の任意方向にゆっくりと回転させて、第１ギア部の任意方向側の噛合部位と第２ギア部の噛合部位とが完全に接触した状態にする。

また、移動制御部１０は、旋回指示に応じてロボット本体を旋回させるために必要な両車輪の回転速度を記憶部５０から読み出す。また、移動制御部１０は、記憶部５０から読み出された両車輪の回転速度に基づいて、所定位置を中心にロボット本体を所定角度旋回させる場合に要する動作時間を算出する。この動作時間の算出は、例えばノミナル運動モデルを使って算出される。

なお、移動制御部１０はガタ量の算出を目的としてロボット本体を旋回させるため、所定角度は所定位置を中心に例えば１度から２度程度であれば良い。また、所定位置は、ロボット本体の２つの車輪間の中央を示すロボット中心とするが、これに限定されるものではない。

また、移動制御部１０は、ロボット本体の旋回前（任意方向にゆっくり回転させた後）および旋回後にロボット本体の位置および方位を計測するように計測部２０に指示する。

さらに、移動制御部１０は、任意方向にゆっくり回転させた後の旋回開始位置からロボット中心を中心にして、算出された動作時間分、ロボット本体を旋回させる。

ガタ量算出部３３は、計測部２０によって計測された旋回前後のランドマークに対する自己のロボット方位および旋回開始位置でのロボットの絶対方位に基づき、旋回後のロボットの絶対方位を算出する。

また、ガタ量算出部３３は、算出されたロボットの旋回前後の絶対方位の差分に基づき、ガタ量を算出する。

ここで、ガタ量の算出方法について、図９を参照して説明する。図９は、ガタ量を算出する方法を示す図である。

まず、移動制御部１０は、ロボット中心を旋回中心に、ロボット本体の車輪内の第１ギア部ａおよび第２ギア部ｂをＡ１方向にゆっくり回転させて、第１ギア部ａのＡ１方向側の噛合部位と第２ギア部ｂの噛合部位とが完全に接触した状態にする。この状態でのロボット本体の絶対座標Ｇ_０（ｘ_０、ｙ_０、θ_０）を、旋回開始位置の絶対座標（０、０、０）とする。

次に、旋回開始位置から旋回前のロボット本体のランドマークに対する相対位置および相対方位が、計測部２０によって計測され、相対座標Ｍ_０（ｘ_ｍ０、ｙｍ_０、θｍ_０）として定義される。

さらに、移動制御部１０は、ロボット中心を旋回中心に、旋回開始位置からＡ１方向と反対方向Ａ２に角度θ_ｃ旋回させる場合に要する動作時間分旋回させる。そして、旋回後のロボット本体のランドマークに対する相対位置および相対方位が、計測部２０によって計測され、相対座標Ｍ_１（ｘ_ｍ１、ｙｍ_１、θｍ_１）として定義される。

ロボット本体の旋回前後の相対座標Ｍ_０およびＭ_１および旋回前のロボット本体の絶対座標Ｇ_０に基づいて、旋回後のロボット本体の絶対座標Ｇ_１が算出される。なお、旋回後のロボット本体の絶対座標の算出方法は、前述した「旋回後のロボット本体の絶対座標を算出する方法」と同様であるため、その説明を省略する。

ここで、旋回後のロボット本体の絶対座標Ｇ_１の絶対方位は、旋回前の絶対座標Ｇ_０の絶対方位が「０」であることにより、旋回後の相対方位θ_ｍ１と旋回前の相対方位θ_ｍ０との差分と一致する。つまり、この旋回前後の相対方位の差（θ_ｍ１−θ_ｍ０）は、ロボット本体が実際に旋回した角度と一致することになる。その結果、旋回指示された角度θ_ｃと、ロボット本体が実際に旋回した角度との差分により、ガタ量が算出される。すなわち、ガタ量ｅ_３は、以下の式（２９）によって表される。
ｅ_３＝θ_ｃ−（θ_ｍ１−θ_ｍ０）・・・（２９）

図８に戻って、ガタ量算出部３３は、算出したガタ量を記憶部５０に格納する。

ガタ量補正部６２は、車輪の回転速度に変化があったとき、ガタ量算出部３３によって算出されたガタ量に基づき、車輪の回転量を補正する。具体的には、ガタ量補正部６２は、車輪の回転速度の回転方向が反対方向に変化したとき、記憶部５０によって記憶されたガタ量を車輪の回転量に加算してロボット本体の軌道を補正する。

次に、実施例３に係るロボット制御装置のガタ量補正手順を、図１０を参照して説明する。図１０は、実施例３に係るロボット制御装置のガタ量補正手順を示すフローチャートである。なお、ステップＳ２１〜Ｓ２５は、ガタ量を算出する手順を説明し、ステップＳ２６〜Ｓ２８は、ガタ量を補正する手順を説明する。

まず、ガタ補正指示部４２は、車輪に関わるパラメータを補正した後、計測可能なランドマークを抽出して（ステップＳ２１）、所定位置を中心にしてロボット本体を所定角度旋回させるように移動制御部１０に指示する。なお、所定角度は所定位置を中心に例えば１度から２度程度であれば良い。また、所定位置は、ロボット本体の２つの車輪間の中央を示すロボット中心とするが、これに限定されるものではない。

すると、移動制御部１０が所定角度の旋回前にロボット本体を任意方向にゆっくり回転させる。そして、計測部２０は、回転後の旋回開始位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ２２）、ランドマークに対する自己のロボット位置およびロボット方位とする。なお、所定角度の旋回前に任意方向にゆっくり回転させるのは、第１ギア部の任意方向側の噛合部位と第２ギア部の噛合部位とが完全に接触した状態にするためである。

引き続き、移動制御部１０は、所定位置を中心にして旋回開始位置から任意方向と反対方向に所定角度旋回させる場合に要する動作時間分、ロボット本体を旋回させる（ステップＳ２３）。

ロボット本体の旋回後、計測部２０は、ロボット本体の旋回終了位置からランドマークに対して相対位置および相対方位を計測して（ステップＳ２４）、ランドマークに対する自己のロボット位置およびロボット方位とする。

引き続き、ガタ量算出部３３は、旋回前後のランドマークに対する自己のロボット方位と旋回開始位置のロボットの絶対方位とに基づき、旋回後のロボットの絶対方位を算出する。さらに、ガタ量算出部３３は、ロボットの旋回前後の絶対方位の差分に基づき、ガタ量を算出して（ステップＳ２５）、記憶部５０に格納する。

その後、車輪の回転速度に変化があったとき、ガタ量補正部６２は、車輪の回転速度の回転方向に変化があるか否かを判定する（ステップＳ２６）。そして、ガタ量補正部６２は、車輪の回転速度の回転方向に変化がある場合には（ステップＳ２６Ｙｅｓ）、記憶部５０によって記憶されたガタ量を車輪の回転量に加算して、ロボット本体の軌道を補正して（ステップＳ２７）、ステップＳ２８に遷移する。

一方、ガタ量補正部６２は、車輪の回転速度の回転方向に変化がない場合には（ステップＳ２６Ｎｏ）、モータ制御部（図示せず）に速度指令を指示する（ステップＳ２８）。

上記実施例３によれば、ロボット制御装置３は、車輪に関わるパラメータの補正後、車輪内の第１ギア部および第２ギア部を任意方向にロボット本体を回転させて、回転後の旋回開始位置からロボット中心を中心としてロボット本体を任意方向と反対方向に所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット方位を計測し、計測した旋回前後に関わるロボット方位間の差分に基づき、第１ギア部の噛合部分および第２ギア部の噛合部分同士の間隙距離（ガタ量）を算出するようにした。さらに、ロボット制御装置３は、車輪の回転方向と反対方向に車輪を回転させた場合、算出されたガタ量を車輪の回転量に加算して補正するようにした。

かかる構成によれば、ロボット制御装置３は、オドメトリによる自己位置推定に関わる誤差となるガタ量を補正することにより、さらに自己位置推定の調整精度を向上させることができる。

また、ロボット制御装置３は、仮に所定角度がロボット中心を中心に０度に近い角度であれば旋回前後のロボット本体の位置が近似することになり、旋回前後の計測条件の変動も小さく済むことになる。そのため、ロボット制御装置３は、自己位置推定に関わる誤差となるガタ量を正確に算出することができ、オドメトリによる自己位置推定の調整精度を向上させることができる。

さらに、ロボット制御装置３は、ガタ量を自動的に算出することができ、その後、算出されたガタ量を補正することによりオドメトリによる自己位置推定の調整を効率的に行うことができる。

また、ロボット制御装置３は、ロボットをロボット中心に旋回させる場所と目印とがあれば、ガタ量を算出することができるため、仮にロボットを直進させてガタ量を算出するよりも場所を選ばないで容易に調整することができる。

以上、上記実施例によって本発明の技術的思想の範囲が限定されるものではなく、特許請求の範囲に記載した技術的範囲の範囲を逸脱しない限り、各種様々な実施例が実施可能であることは言うまでもない。また、本実施例に記載した効果は、これに限定されるものではない。

また、図示したロボット制御装置１〜３の各構成要素は機能概念的に記載したものであって、必ずしも物理的に図示のように構成されるものではなく、そのロボット制御装置１〜３の具体的な態様は図示のものに限縮されるものでは到底ないことは言うまでもない。

なお、ロボット制御装置１〜３にて行われる各処理機能は、その全部または任意の一部が、ＣＰＵ（Central Processing Unit）（またはＭＰＵ（Micro Processing Unit）、ＭＣＵ（Micro Controller Unit）などのマイクロ・コンピュータ）および当該ＣＰＵ（またはＭＰＵ、ＭＣＵなどのマイクロ・コンピュータ）にて解析実行されるプログラムにて実現され、あるいは、ワイヤードロジックによるハードウェアとして実現されても良い。

また、ロボット制御装置１〜３にて行われる各処理機能は、その全部または任意の一部が、ＣＰＵ（Central Processing Unit）（またはＭＰＵ（Micro Processing Unit）、ＭＣＵ（Micro Controller Unit）などのマイクロ・コンピュータ）および当該ＣＰＵ（またはＭＰＵ、ＭＣＵなどのマイクロ・コンピュータ）にて解析実行させるために、コンピュータ読み取り可能な記録媒体に記録されるものであっても良く、この場合でも、上記実施例と同様の効果が得られることは言うまでもない。

以上の実施例に係る実施形態に関し、さらに以下の付記を開示する。

（付記１）ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御部と、
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測部と、
前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出部と
を有することを特徴とするロボット制御装置。

（付記２）前記移動制御部は、
前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出部は、
前記計測部にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径または車輪間の距離に関わる誤差を算出することを特徴とする付記１に記載のロボット制御装置。

（付記３）前記算出部にて算出した前記誤差に基づき、前記車輪に関わるパラメータを補正する補正部を有することを特徴とする付記１または付記２に記載のロボット制御装置。

（付記４）前記移動制御部は、
前記旋回開始位置から前記所定位置を中心にして、前記ロボット本体を複数回３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始することを特徴とする付記１から付記３のいずれか１つに記載のロボット制御装置。

（付記５）前記算出部にて算出した前記誤差が異常閾値を超えた場合、前記車輪の異常を報知する報知部を有することを特徴とする付記１から付記４のいずれか１つに記載のロボット制御装置。

（付記６）前記移動制御部は、
前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする付記１から付記５のいずれか１つに記載のロボット制御装置。

（付記７）前記移動制御部は、
前記ロボット本体の旋回を開始する前に、前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出部は、
前記計測部にて計測した前記所定角度の旋回前後のロボット方位に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする付記６に記載のロボット制御装置。

（付記８）前記所定位置は、
前記ロボット本体の２つの車輪間の中央であることを特徴とする付記７に記載のロボット制御装置。

（付記９）前記正転方向と反対方向に前記車輪を回転させた場合、前記算出部にて算出した前記間隙距離を前記車輪の回転量に加算して補正する補正部を有することを特徴とする付記７または付記８に記載のロボット制御装置。

（付記１０）ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御手順と、
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測手順と、
前記計測手順にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出手順と
をコンピュータに実行させることを特徴とするロボット制御プログラム。

（付記１１）前記移動制御手順は、
前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出手順は、
前記計測手順にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径または車輪間の距離に関わる誤差を算出することを特徴とする付記１０に記載のロボット制御プログラム。

（付記１２）前記算出手順にて算出した前記誤差に基づき、前記車輪に関わるパラメータを補正する補正手順を含むことを特徴とする付記１０または付記１１に記載のロボット制御プログラム。

（付記１３）前記移動制御手順は、
前記旋回開始位置から前記所定位置を中心にして、前記ロボット本体を複数回３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始することを特徴とする付記１０から付記１２のいずれか１つに記載のロボット制御プログラム。

（付記１４）前記算出手順にて算出した前記誤差が異常閾値を超えた場合、前記車輪の異常を報知する報知手順を含むことを特徴とする付記１０から付記１３のいずれか１つに記載のロボット制御プログラム。

（付記１５）前記移動制御手順は、
前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする付記１０から付記１４のいずれか１つに記載のロボット制御プログラム。

（付記１６）前記移動制御手順は、
前記ロボット本体の旋回を開始する前に、前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出手順は、
前記計測手順にて計測した前記所定角度の旋回前後のロボット方位に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする付記１５に記載のロボット制御プログラム。

（付記１７）前記所定位置は、
前記ロボット本体の２つの車輪間の中央であることを特徴とする付記１６に記載のロボット制御プログラム。

（付記１８）前記正転方向と反対方向に前記車輪を回転させた場合、前記算出手順にて算出した前記間隙距離を前記車輪の回転量に加算して補正する補正手順を有することを特徴とする付記１６または付記１７に記載のロボット制御プログラム。

（付記１９）コンピュータが２輪駆動のロボットを制御するロボット制御方法であって、
ロボット本体の旋回開始位置から所定位置を中心にして、当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御工程と、
当該ロボット本体から離間した位置にある目印を基準にして、当該ロボット本体の旋回前後に関わるロボット位置およびロボット方位を計測する計測工程と、
前記計測工程にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わる誤差を算出する算出工程と
を含むことを特徴とするロボット制御方法。

（付記２０）前記移動制御工程は、
前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出工程は、
前記計測工程にて計測した前記旋回前後に関わるロボット位置間の差分および前記旋回前後に関わるロボット方位間の差分に基づき、前記車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径または車輪間の距離に関わる誤差を算出することを特徴とする付記１９に記載のロボット制御方法。

（付記２１）前記算出工程にて算出した前記誤差に基づき、前記車輪に関わるパラメータを補正する補正工程を含むことを特徴とする付記１９または付記２０に記載のロボット制御方法。

（付記２２）前記移動制御工程は、
前記旋回開始位置から前記所定位置を中心にして、前記ロボット本体を複数回３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始することを特徴とする付記１９から付記２１のいずれか１つに記載のロボット制御方法。

（付記２３）前記算出工程にて算出した前記誤差が異常閾値を超えた場合、前記車輪の異常を報知する報知工程を含むことを特徴とする付記１９から付記２２のいずれか１つに記載のロボット制御方法。

（付記２４）前記移動制御工程は、
前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする付記１９から付記２３のいずれか１つに記載のロボット制御方法。

（付記２５）前記移動制御工程は、
前記ロボット本体の旋回を開始する前に、前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
前記算出工程は、
前記計測工程にて計測した前記所定角度の旋回前後のロボット方位に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする付記２４に記載のロボット制御方法。

（付記２６）前記所定位置は、
前記ロボット本体の２つの車輪間の中央であることを特徴とする付記２５に記載のロボット制御方法。

（付記２７）前記正転方向と反対方向に前記車輪を回転させた場合、前記算出工程にて算出した前記間隙距離を前記車輪の回転量に加算して補正する補正工程を含むことを特徴とする付記２５または付記２６に記載のロボット制御方法。

１、２、３ ロボット制御装置
１０ 移動制御部
２０ 計測部
３０ 算出部
３１ 旋回後絶対座標算出部
３２ 誤差算出部
３３ ガタ量算出部
４０ 指示部
４１ パラメータ誤差補正指示部
４２ ガタ補正指示部
５０ 記憶部
６０ 補正部
６１ パラメータ補正部
６２ ガタ量補正部
７０ 報知部

Claims (7)

1. ロボット本体の絶対座標系における絶対位置を示す旋回開始位置から所定位置を中心にして、現時点で把握されている車輪の半径および車輪間の距離に関する情報に基づいて当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御部と、
   当該ロボット本体の前記旋回開始位置から当該ロボット本体から離間した位置にある目印に対して当該ロボット本体の旋回前の相対位置および相対方位を計測し、当該ロボット本体の旋回終了位置から前記目印に対して当該ロボット本体の旋回後の相対位置および相対方位を計測する計測部と、
   前記計測部にて計測した旋回前後の前記目印に対する相対方位および前記旋回開始位置での当該ロボット本体の絶対方位に基づき、前記旋回終了位置での当該ロボット本体の絶対方位を算出し、前記計測部にて計測した旋回前後の前記目印に対する相対位置、前記旋回開始位置および前記相対位置を前記絶対座標系における絶対位置に変換するために用いられる回転行列であって算出されたロボット本体の絶対方位を用いた回転行列に基づき、当該ロボット本体の旋回後の絶対位置を示す旋回終了位置を算出する第一の算出部と、
   前記第一の算出部にて算出した前記旋回前後の絶対位置間の差分および前記旋回前後の絶対方位間の差分に基づき、移動ロボットの運動学モデルを用いて、前記車輪の半径および車輪間の距離に関わる誤差を算出する第二の算出部と
   を有することを特徴とするロボット制御装置。
2. 前記移動制御部は、
   前記旋回開始位置から所定位置を中心にして、前記ロボット本体を３６０度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
   前記第二の算出部は、
   前記第一の算出部にて算出した前記旋回前後に関わる絶対位置間の差分および前記旋回前後に関わる絶対方位間の差分に基づき、前記運動学モデルで用いられる車輪に関わるパラメータ内の複数の要素のうち、少なくとも前記車輪の半径および車輪間の距離に関わる誤差を算出することを特徴とする請求項１に記載のロボット制御装置。
3. 前記第二の算出部にて算出した前記誤差に基づき、前記運動学モデルで用いられる前記車輪の半径および車輪間の距離を補正する補正部を有することを特徴とする請求項１または請求項２に記載のロボット制御装置。
4. 前記移動制御部は、
   前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向に回転させて、前記第１ギア部の前記正転方向側の噛合部位と当該第２ギアの噛合部位とが完全に接触した状態で、前記ロボット本体の旋回を開始することを特徴とする請求項１から請求項３のいずれか１つに記載のロボット制御装置。
5. 前記移動制御部は、
   さらに、前記ロボット本体の旋回を開始する前に、前記車輪内の第１ギア部および第２ギア部同士を噛合して前記第１ギア部および前記第２ギア部を正転方向と反対方向に回転させて、回転後の前記旋回開始位置から所定位置を中心として前記ロボット本体を前記正転方向に前記所定角度旋回させる旋回指示を検出すると、当該ロボット本体の旋回を開始し、
   前記第二の算出部は、
   さらに、前記第一の算出部にて算出した前記所定角度の旋回前後のロボット本体の絶対方位の差分に基づき、前記第１ギア部の噛合部分および前記第２ギア部の噛合部分同士の間隙距離を算出することを特徴とする請求項１に記載のロボット制御装置。
6. ロボット本体の絶対座標系における絶対位置を示す旋回開始位置から所定位置を中心にして、現時点で把握されている車輪の半径および車輪間の距離に関する情報に基づいて当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御手順と、
   当該ロボット本体の前記旋回開始位置から当該ロボット本体から離間した位置にある目印に対して当該ロボット本体の旋回前の相対位置および相対方位を計測し、当該ロボット本体の旋回終了位置から前記目印に対して当該ロボット本体の旋回後の相対位置および相対方位を計測する計測手順と、
   前記計測手順にて計測した旋回前後の前記目印に対する相対方位および前記旋回開始位置での当該ロボット本体の絶対方位に基づき、前記旋回終了位置での当該ロボット本体の絶対方位を算出し、前記計測手順にて計測した旋回前後の前記目印に対する相対位置、前記旋回開始位置および前記相対位置を前記絶対座標系における絶対位置に変換するために用いられる回転行列であって算出されたロボット本体の絶対方位を用いた回転行列に基づき、当該ロボット本体の旋回後の絶対位置を示す旋回終了位置を算出する第一の算出手順と、
   前記第一の算出手順にて算出した前記旋回前後の絶対位置間の差分および前記旋回前後の絶対方位間の差分に基づき、移動ロボットの運動学モデルを用いて、前記車輪の半径および車輪間の距離に関わる誤差を算出する第二の算出手順と
   をコンピュータに実行させることを特徴とするロボット制御プログラム。
7. コンピュータが２輪駆動のロボットを制御するロボット制御方法であって、
   ロボット本体の絶対座標系における絶対位置を示す旋回開始位置から所定位置を中心にして、現時点で把握されている車輪の半径および車輪間の距離に関する情報に基づいて当該ロボット本体を旋回前後の位置が近似した位置となるように所定角度旋回させる旋回指示を検出すると、当該ロボット本体を移動させる２つの車輪の回転動作に応じて、当該ロボット本体の旋回を開始する移動制御工程と、
   当該ロボット本体の前記旋回開始位置から当該ロボット本体から離間した位置にある目印に対して当該ロボット本体の旋回前の相対位置および相対方位を計測し、当該ロボット本体の旋回終了位置から前記目印に対して当該ロボット本体の旋回後の相対位置および相対方位を計測する計測工程と、
   前記計測工程にて計測した旋回前後の前記目印に対する相対方位および前記旋回開始位置での当該ロボット本体の絶対方位に基づき、前記旋回終了位置での当該ロボット本体の絶対方位を算出し、前記計測工程にて計測した旋回前後の前記目印に対する相対位置、前記旋回開始位置および前記相対位置を前記絶対座標系における絶対位置に変換するために用いられる回転行列であって算出されたロボット本体の絶対方位を用いた回転行列に基づき、当該ロボット本体の旋回後の絶対位置を示す旋回終了位置を算出する第一の算出工程と、
   前記第一の算出工程にて算出した前記旋回前後の絶対位置間の差分および前記旋回前後の絶対方位間の差分に基づき、移動ロボットの運動学モデルを用いて、前記車輪の半径および車輪間の距離に関わる誤差を算出する第二の算出工程と
   を含むことを特徴とするロボット制御方法。

Images