移動体制御システム及び移動体の可動部の絶対位置算出方法
【課題】移動体の可動部の絶対位置を精度よく算出することができる移動体制御システムを提供する。
【解決手段】 移動体と、送信時間情報を含んだ信号を送信する装置とを備えた移動体制御システムであり、移動体は、本体と、可動部と、可動部を駆動するアクチュエータと、本体に取付けられ、信号を受信する信号受信部と、信号から信号受信部の絶対位置を算出する位置算出部と、アクチュエータの駆動量検出部と、位置算出部で算出された位置情報と検出された駆動量から可動部の絶対位置を算出する機能部を備えており、絶対位置算出部は、タイマと、検出される駆動量を検出時間と共に記憶する記憶部と、送信時間情報から信号が送信された時点の駆動量を記憶部の中から特定する特定部と、その駆動量と本体位置算出装置で算出した位置情報から、信号送信時の可動部の絶対位置を算出する算出部を備えている。
【解決手段】 移動体と、送信時間情報を含んだ信号を送信する装置とを備えた移動体制御システムであり、移動体は、本体と、可動部と、可動部を駆動するアクチュエータと、本体に取付けられ、信号を受信する信号受信部と、信号から信号受信部の絶対位置を算出する位置算出部と、アクチュエータの駆動量検出部と、位置算出部で算出された位置情報と検出された駆動量から可動部の絶対位置を算出する機能部を備えており、絶対位置算出部は、タイマと、検出される駆動量を検出時間と共に記憶する記憶部と、送信時間情報から信号が送信された時点の駆動量を記憶部の中から特定する特定部と、その駆動量と本体位置算出装置で算出した位置情報から、信号送信時の可動部の絶対位置を算出する算出部を備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、信号送信装置と、空間内を移動する移動体を備えた移動体制御システムに関する。
【背景技術】
【０００２】
ＧＰＳ等の位置検出システムによって、移動体の位置を認識しながら移動体を制御するする移動体制御システムが知られている。この種の移動体制御システムは、移動体と、移動体が移動する空間に信号を送信する信号送信装置とによって構成されている。移動体は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号送信装置が空間に送信する信号を受信する信号受信部と、信号受信部が受信した信号から信号受信部の絶対位置（以下では、本体の絶対位置という）を算出する本体位置算出部と、アクチュエータの駆動量を検出する駆動量検出部を有している。移動体は、空間内を移動しながらアクチュエータによって関節を駆動させ、可動部を運動させる。または、関節を駆動させ、可動部を運動させることによって空間内を移動する。
このような移動体では、可動部に所望の運動をさせるために、可動部の絶対位置が算出される。可動部の絶対位置の算出は、本体位置算出部が算出する本体の絶対位置と、駆動量検出部が検出する駆動量を用いて行われる。すなわち、アクチュエータの駆動量から本体に対する可動部の相対位置を算出し、算出した可動部の相対位置と本体の絶対位置から、可動部の絶対位置が算出される。
なお、上述した信号を利用して本体の絶対位置を検出する移動体については、特許文献１に開示されている。
【０００３】
【特許文献１】特開２００４−１７２４８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上述した移動体の制御システムでは、信号受信部で信号を受信してから本体の絶対位置が算出されるまでの間に、僅かな時間ｔを要する。すなわち、受信した信号から算出された本体の絶対位置は、算出された時点よりも時間ｔだけ前の時点の本体の絶対位置となっている。従って、本体が移動している場合には、本体の絶対位置を算出したタイミングにおける実際の本体の位置は、算出した本体の絶対位置とは僅かに異なる位置となる。このため、算出された絶対位置と算出した時点でのアクチュエータの駆動量に基づいて可動部の絶対位置を算出しても、その算出された絶対位置は正しい絶対位置とはならない。可動部の絶対位置を正確に算出することができないと、可動部を正確に運動させることができないという問題が生じる。
例えば、本体に関節を介して接続された可動部によって脚部が構成され、脚部を駆動することによって目標位置まで移動する移動体では、脚部の位置を基準として制御することがある。しかし、上述したように、本体の絶対位置の算出に時差が生じると、脚部の絶対位置が正確に算出されない。したがって、実際には脚部が目標位置に到達しているにもかかわらず、脚部が目標位置に到達していないと判定されるために、さらに脚部が駆動される。従って、移動体を目標位置で停止させることができない。
【０００５】
本発明は、上記した実情に鑑みてなされたものであり、移動体の可動部を精度良く運動させる移動体制御システムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明の移動体制御システムは、空間内を移動する移動体と、送信時間を示す時間情報を含んだ信号を前記空間内に対して送信する信号送信装置とを備えている。移動体は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、本体に取付けられ、信号送信装置が空間に送信する信号を受信する信号受信部と、信号受信部が受信した信号から信号受信部の絶対位置を算出する本体位置算出部と、アクチュエータの駆動量を検出する駆動量検出部と、本体位置算出部で算出した位置情報と駆動量検出部で検出されたアクチュエータの駆動量から可動部の絶対位置を算出する絶対位置算出部と、を備えている。その絶対位置算出部は、駆動量検出部でアクチュエータの駆動量を検出した時間を計時するタイマと、所定の時間間隔で駆動量検出部により検出されるアクチュエータの駆動量をタイマで計時された検出時間と共に順に記憶する記憶部と、本体位置算出部で算出した位置情報に対応する信号の送信時間情報に基づいて、信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定する駆動量特定部と、本体位置算出部で算出した位置情報と、駆動量特定部で特定した駆動量から、対応する信号の送信時における可動部の絶対位置を算出する可動部位置算出部を備えている。
この移動体制御システムでは、駆動量検出部で検出されるアクチュエータの駆動量は検出時間と共に順に記憶部に記憶される。また、信号送信装置は送信時間情報を含んだ信号を送信し、その信号は信号受信部で受信される。信号送信装置が送信する信号は、送信するのとほぼ同時に信号受信部に受信されるので、信号の送信時間情報は信号の受信時間を示している。本体位置算出部は、信号受信部が受信した信号に基づいて信号受信部の絶対位置を算出する。すると、駆動量特定部が、本体位置算出部が算出した位置情報に対応する信号の送信時間情報をキーとして、記憶部に記憶されている駆動量の中からそれらの信号を受信した時点におけるアクチュエータの駆動量を特定する。これによって、信号の受信時間と同じ時点におけるアクチュエータの駆動量が特定される。可動部位置算出部は、本体位置算出部が算出した位置情報と、駆動量特定部で特定した駆動量から、可動部の位置を特定する。従って、信号を受信した時点における可動部の絶対位置を精度良く算出することができる。
【０００７】
上述の移動体制御システムでは、絶対位置算出部が、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正する補正部をさらに有していることが好ましい。タイマで計時される時間と信号に含まれる時間情報とのズレを補正することで、より精度よく可動部の絶対位置を算出することができる。
【０００８】
タイマで計時される時間と信号に含まれる時間情報とのズレを補正する方法としては、例えば、次に記載する方法を採ることができる。
すなわち、補正部は、予め設定された動作パターンで本体に動作を行わせながら所定の時間間隔で本体位置算出部により信号受信部の絶対位置を算出し、本体に行わせた動作と本体位置算出部で算出した信号受信部の絶対位置軌道とのずれから、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正する。この方法によると、簡易な方法で両者のズレを補正することができる。
【０００９】
上述した絶対位置算出部によって算出される可動部の絶対位置は、位置情報が送信された時点の絶対位置（すなわち、現時点より所定の時間ｔだけ前の可動部の絶対位置）となる。したがって、絶対位置算出部は、可動部位置算出部によって算出された可動部の絶対位置に基づいて、現在の可動部の絶対位置を予測する予測位置算出部をさらに有することが好ましい。過去の正確な絶対位置に基づいて現在の絶対位置を予測することで、現在の絶対位置を精度良く予測することができる。
なお、現在の絶対位置を予測する方法としては、可動部位置算出部で算出された絶対位置と、その時点から現在までの本体及び可動部への移動指令値とに基づいて算出することができる。
【００１０】
また、本発明は、可動部の絶対位置を好適に算出することができる絶対位置算出方法を提供する。すなわち、本発明の絶対位置算出方法は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号を受信する信号受信部と、アクチュエータの駆動量を検出する駆動量検出部と、時間を計時するタイマと、データを記憶する記憶部を備えた空間内を自由に移動する移動体と、信号受信部に対して送信時間情報を含んだ信号を送信する信号送信装置とを備えた移動体制御システムにおいて、移動体の可動部の絶対位置を算出する。この方法は、駆動量検出部で検出されたアクチュエータの駆動量をその検出時間と共に所定の時間間隔で順に記憶部に記憶するステップと、信号送信装置から送信時間を示す時間情報を含んだ信号を送信するステップと、上記信号を信号受信部で受信し、受信した信号から信号受信部の絶対位置を算出するステップと、信号受信部で受信した信号の送信時間情報に基づいて信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定するステップと、算出した信号受信部の絶対位置と、特定した駆動量から、可動部の絶対位置を算出するステップを有する。
この方法によると、可動部の絶対位置を精度良く算出することができる。
【発明を実施するための最良の形態】
【００１１】
下記に詳細に説明する実施例の主要な特徴を最初に列記する。
（形態１）ロボット制御システムは、ロボットと、ＧＰＳ信号出力装置と、複数のＧＰＳアンテナによって構成されている。
（形態２）ＧＰＳ信号出力装置は、時刻をカウントする時刻カウント部を備えており、ＧＰＳ信号出力装置が時刻カウント部でカウントされた送信時刻情報を含んだ信号を各ＧＰＳアンテナに出力することによって、各ＧＰＳアンテナから同時に、送信時刻情報を含んだＧＰＳ電波が送信される。
（形態３）ロボットは、頭部と、頭部に取り付けられている第１可動部と、第１可動部に取り付けられている第２可動部と、・・、第ｎ−１可動部に取り付けられている第ｎ可動部を有している。頭部と第１可動部は関節によって接続されており、各可動部間も関節によって接続されている。各関節を駆動することによって、その関節によって接続されている各可動部が相対的に移動する。
（形態４）可動部群の一部によって脚部が構成されており、脚部の関節を駆動することによってロボットは歩行する。
（形態５）頭部には、ＧＰＳセンサ（ＧＰＳ受信装置）が設置されている。
（形態６）ＧＰＳセンサは、ＧＰＳ電波を受信し、受信したＧＰＳ電波を分析することでＧＰＳセンサ自体の絶対位置を検出する。ＧＰＳ電波によって伝送されるデータには、そのＧＰＳ電波を発信した時刻データが含まれている。ＧＰＳセンサは、検出された絶対位置と時刻データを出力する。
（形態７）各関節はモータを備えている。モータによって関節を駆動すると、その関節によって接続されている可動部が運動する。
（形態８）各モータはエンコーダを有しており、エンコーダはモータの回転角度を検出する。
（形態９）ロボットは制御装置を有している。制御装置は、ＧＰＳセンサと各エンコーダと各モータに電気的に接続されている。
（形態１０）制御装置は、足部の絶対位置を算出する座標算出部を有している。座標算出部は、ＧＰＳセンサから出力されたＧＰＳセンサの絶対位置及び時刻データと、各エンコーダの検出値から、足部の先端の絶対位置を算出する。
（形態１１）制御装置は、各エンコーダの検出値と、座標算出部が算出した足部の先端の絶対位置に基づいて、モータに制御指令値を出力し、ロボットを歩行させる。
（形態１２）座標算出部は、時刻をカウントするタイマと、ＧＰＳセンサから出力される本体の絶対位置と時刻データを読取る第１読取部と、各エンコーダの検出値を読取ると共にその検出時刻をタイマで計測する第２読取部と、第２読取部で読取った検出値をその検出時刻と関連付けて記憶する記憶部と、第１読取部で読取った時刻データをキーとして記憶部が記憶している検出値の中から、その時刻データと最も近い時刻データと関連付けられた検出値を特定する特定部と、第１読取部で読取った絶対位置と特定部で特定された検出値に基づいて可動部の絶対位置を算出する算出部を備えている。
【実施例】
【００１２】
本発明の一実施例に係るロボット制御システム１００について図面を参照しながら説明する。図１に示すように、ロボット制御システム１００は、ロボット１０と、ＧＰＳ信号出力装置１０２と、４つのＧＰＳアンテナ（いわゆる、室内ＧＰＳ衛星）１０４ａ〜１０４ｄによって構成されている。
【００１３】
図１に示すように、ロボット１０が移動するフィールドの周囲には、複数のＧＰＳアンテナ１０４ａ〜１０４ｄが取り付けられている。ＧＰＳアンテナ１０４ａ〜１０４ｄは、ＧＰＳ信号出力装置１０２に接続されている。ＧＰＳ信号出力装置１０２は、時刻ｔをカウントする時刻カウント部１０６を有している。ＧＰＳ信号出力装置１０２は時刻カウント部１０６がカウントする時刻情報ｔを読取り、時刻情報ｔを含んだ電気信号（ＧＰＳ信号）を各ＧＰＳアンテナ１０４ａ〜１０４ｄに出力する。すると、各ＧＰＳアンテナ１０４ａ〜１０４ｄは入力されたＧＰＳ信号を変換した電波（ＧＰＳ電波）をロボットが移動する空間に同時に送信する。従って、ロボットが移動する空間に、時刻情報ｔを含んだＧＰＳ電波がそれぞれ送信される。ＧＰＳ信号出力装置１０２はＧＰＳ信号を所定の周期で繰り返し出力するので、ロボットが移動する空間にはＧＰＳ電波が所定の周期で繰り返し送信される。また、ＧＰＳ信号出力装置１０２が時刻情報ｔを読取ってからＧＰＳアンテナ１０４ａ〜１０４ｄがＧＰＳ電波を送信するまでの処理はほぼ同時に行われるので、時刻情報ｔはそのＧＰＳ電波を送信した時刻を示している。
【００１４】
ロボット１０は、平坦な床面上に配置され、床面上を目標位置まで歩行して移動する自律移動ロボットである。
図１に示すように、ロボット１０は、頭部１２と、胴体部１４と、腿部１６、１８と、脛部２０、２２と、足部２４、２６によって構成されている。頭部１２と胴体部１４は、関節３０によって接続されている。胴体部１４と腿部１６は、関節３２、３４によって接続されている。腿部１６と脛部２０は、関節３６によって接続されている。脛部２０と足部２４は、関節３８によって接続されている。胴体部１４と腿部１８は、関節４０、４２によって接続されている。腿部１８と脛部２２は、関節４４によって接続されている。脛部２２と足部２６は、関節４６によって接続されている。腿部１６、脛部２０、足部２４、及び関節３２、３４、３６、３８によって、右脚部５０が形成されている。腿部１８、脛部２２、足部２６、及び関節４０、４２、４４、４６によって、左脚部５２が形成されている。
また、各関節には図示しないモータが配されている。モータを駆動することによって、各関節が駆動され、各関節の角度が変更される。各モータはエンコーダを有しており、エンコーダはモータの回転量を検出することによって、関節の角度φ１〜φ９を検出する。各モータ及び各エンコーダは、図３に示すように、後述する制御装置６０に接続されている。
【００１５】
図２は、各関節の位置と、各関節の回転軸を模式的に示した図である。図２では、各関節の位置が点３０〜４６によって示されている。また、足部２４、２６の先端部２４ａ、２６ａが点２４ａ、２６ａによって示されている。また、後述するＧＰＳセンサ５８が取り付けられている頭部１２の頂点１２ａの位置が点１２ａによって示されている。各点の相対位置がベクトルｌ１〜ｌ１１によって示されている。図２に示すように、関節３０〜４６は、それぞれ角度φ１〜φ９の方向に回転する。各関節のエンコーダは、各関角φ１〜φ９を検出する。
【００１６】
ロボット１０は、ＧＰＳ電波を受信するＧＰＳセンサ５８を備えている。ＧＰＳセンサ５８は、頭部１２の頂点１２ａに取り付けられている。ＧＰＳセンサ５８は、各ＧＰＳアンテナ１０４ａ〜１０４ｄから送信されるＧＰＳ電波（すなわち、ＧＰＳ信号）を受信し、受信したＧＰＳ信号に基づいてＧＰＳセンサ５８自体（すなわち、頭部１２の頂点１２ａ）の絶対位置Ａを検出する。また、ＧＰＳセンサ５８は受信したＧＰＳ信号に含まれる時刻情報ｔを読取る。上述したように、ＧＰＳ電波は所定周期で繰り返し送信されるので、ＧＰＳセンサ５８はＧＰＳ信号を所定周期で繰り返し受信し、絶対位置Ａと時刻情報ｔを検出する。ＧＰＳセンサ５８は、後述する制御装置６０と接続されており、検出した絶対位置Ａと時刻情報ｔを制御装置６０に出力する。
なお、ＧＰＳセンサ５８が、受信したＧＰＳ信号に基づいて絶対位置Ａを検出（算出）するには、各ＧＰＳアンテナ１０４ａ〜１０４ｄから送信されたＧＰＳ電波を分析して絶対位置Ａを演算する必要がある。従って、ＧＰＳ信号を受信してから絶対位置Ａが検出されるまでには、ある程度の時間を要する。受信したＧＰＳ信号から絶対位置Ａを演算する手順については、従来公知であるため、ここでは、その詳細な説明については省略する。
【００１７】
ロボット１０は、各関節３０〜４６のモータを制御する制御装置６０を有している。図３に示すように、制御装置６０は、ＧＰＳセンサ５８と、各関節３０〜４６を駆動するモータ及びそのモータのエンコーダにそれぞれ接続されている。制御装置６０は、ＣＰＵ，ＲＯＭ，ＲＡＭ等からならマイクロコンピュータによって構成されている。制御装置６０は、ＲＯＭに格納されているプログラムを実行することで、（１）各関節３０〜４６のモータにトルク指令値を出力して各関節３０〜４６を駆動させる制御部６２と、（２）ＧＰＳセンサ５８から出力される絶対位置Ａ及び時刻データｔと、各関節３０〜４６のエンコーダから入力される検出値φ１〜φ１１から、先端部２４ａ、２６ａの絶対位置を算出する座標算出部７０として機能する。
【００１８】
まず、座標算出部７０の構成について説明する。座標算出部７０は、時刻カウント部７２と、記憶部７６と、データ特定部７８と、算出部８０と、予測位置算出部８２と、軸足判定部８４を備えている。
【００１９】
軸足判定部８４は、右脚部５０と左脚部５２のいずれが軸足（すなわち、ロボット１０の重量がより多く加わっている方の脚部）かを判定する。軸足判定部８４には、後述する算出部８０から、足部２４、２６の先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉ及び頭部１２の頂点１２ａの絶対位置データＡｔｉが入力される。軸足判定部８４は、入力された先端部２４ａ、２６ａ及び頂点１２ａの絶対位置データから、いずれが軸足であるかを判定する。軸足判定部８４の判定値は、算出部８０に入力される。
【００２０】
時刻カウント部７２は、時刻をカウントする。時刻カウント部７２がカウントする時刻データｓは、上述したＧＰＳ信号出力装置１０２の時刻カウント部１０６がカウントする時刻データｔと略同一となるよう設定されている。時刻カウント部７２がカウントする時刻データｓは、記憶部７６に読取られる。
【００２１】
記憶部７６は、所定の周期で各エンコーダから各関節３０〜４６の関節角φ１〜φ９を読取り、記憶する。記憶部７６は、各エンコーダから関節角φ１〜φ９を読取る際に時刻カウント部７２の時刻データｓを読取り、読取った角度データφ１〜φ９を時刻データｓと関連付けて記憶する。
なお、エンコーダから各関節３０〜４６の関節角φ１〜φ９を読み取る周期は、ＧＰＳセンサ５８でＧＰＳ信号を受信してから絶対位置を出力するまでの周期と比較して短く設定されている。関節角φ１〜φ９を読み取る周期とＧＰＳセンサ５８で絶対位置を出力する周期との関係を図６に模式的に示している。図６に示すように、ＧＰＳセンサ５８から絶対位置を１回出力する間に、エンコーダの関節角φ１〜φ９が複数回読み取られ、記憶部７６に記憶されている。
【００２２】
データ特定部７８は、ＧＰＳセンサ５８から絶対位置Ａと時刻データｔが入力されたときに、その時刻データｔをキーとして、記憶部７６に記憶されている角度データの中から、ＧＰＳセンサ５８から出力された絶対位置Ａと対応する角度データを特定する。
すなわち、データ特定部７８には、ＧＰＳセンサ５８から時刻データｔｉと時刻ｔｉにおける頂点１２ａの絶対位置データＡｔｉが所定周期で入力される。データ特定部７８は、時刻データｔｉを入力されると、記憶部７６が記憶している時刻データｓを検索し、入力された時刻データｔｉと最も近い時刻データｓｎを特定する。そして、特定した時刻データｓｎと関連付けられている角度データφ１〜φ９を読取る。また、データ特定部７８は、記憶部７６から角度データφ１〜φ９を読取ると、その読取った角度データφ１〜φ９より前に記憶された角度データφ１〜φ９及び時刻データｓを消去する。そして、読取った角度データφ１〜φ９と、ＧＰＳセンサ５８から入力された絶対位置データＡｔｉを算出部８０に入力する。
【００２３】
算出部８０は、（１）データ特定部７８で特定された角度データφ１〜φ９から頭部１２に対する足部２４、２６（詳しくは、先端部２４ａ、２６ａ）の相対位置Ｍｔｉ、Ｐｔｉを算出し、（２）その算出した先端部２４ａ、２６ａの相対位置とＧＰＳセンサ５８から出力された頭部１２の絶対位置Ａｔｉから、先端部２４ａ、２６ａの絶対位置ＡＭｔｉ、ＡＰｔｉを算出する。
すなわち、算出部８０には、データ特定部７８から、絶対位置データＡｔｉと、角度データφ１〜φ９が所定周期で入力される。算出部８０は、データ特定部７８からデータが入力される毎に、入力された各データから時刻ｔｉにおける先端部２４ａ、２６ａの絶対位置ＡＭｔｉ、ＡＰｔｉ算出する。
【００２４】
（１）角度データφ１〜φ９から先端部２４ａ、２６ａの相対位置の算出
まず、角度データφ１〜φ９から先端部２４ａ、２６ａの相対位置を算出する方法について説明する。まず、ロボット内座標軸について説明し、その後に、相対位置を算出する手順について説明する。
【００２５】
（ロボット内座標軸の設定）
ロボット１０の基本姿勢は、図４に示す関節を伸ばした姿勢に設定されている。また、ロボット１０の正面方向をｘ′軸、垂直方向をｚ′軸、ｘ′軸及びｚ′軸と直行する方向をｙ′軸とするｘ′ｙ′ｚ′座標系（ロボット１０の座標系）が設定されている。
また、各点（関節３０〜４６、頂点１２ａ、先端部２４ａ、２６ａ）の位置は、相対位置ベクトルｌ１〜ｌ１１によって規定されている。ベクトルｌ１〜ｌ１１は、各点と隣接する下流側（頂点１２ａ側を上流とする）の点の相対位置を示している。ｌ１〜ｌ１１は、ロボット１０の各部の寸法によって定まる定数である。例えば、基本姿勢における点２６ａの点４６に対する相対位置は、ベクトルｌ１１＝（０，０，ｌ１１）によって規定する。また、各関節３０〜４６の角度φ１〜φ９は、基本姿勢に対する角度によって規定する。
以上のような設定の下で、点２４ａ、２６ａの時刻ｔｉにおける相対位置（相対位置データＭｔｉ、Ｐｔｉ）の算出が行われる。
【００２６】
（頭部１２の座標系における相対位置の算出）
図２における点２４ａの点３８に対する相対位置Ｍ６は、ベクトルｌ６方向をｚ６軸、関節３８の回転軸方向をｙ６軸、ｚ６軸及びｙ６軸と直行する方向をｘ６軸としたｘ６ｙ６ｚ６座標系（足部２４の座標系）によって規定する。従って、点２４ａの点３８に対する相対位置Ｍ６は、（０，０，ｌ６）となる。次に、ｘ６ｙ６ｚ６座標系で示されている点２４ａの相対位置Ｍ６を、ベクトルｌ５方向をｚ５軸、関節３８の回転軸方向をｙ５軸、ｚ５軸及びｙ５軸と直行する方向をｘ５軸としたｘ５ｙ５ｚ５座標系（脛部２０の座標系）における相対位置Ｍ５（ｘｍ５，ｙｍ５，ｚｍ５）に座標変換する。相対位置Ｍ５は、ベクトルＭ６を回転軸ｙ５周りに角度φ５回転させることで得られる。すなわち、
【００２７】
【数１】
【００２８】
ここで、Ｒｏｔ（ｙ５，φ５）は、所定の座標系をｙ５軸周りにφ５だけ回転させる回転ベクトルである。これによって、ｘ５ｙ５ｚ５座標系における点２４ａの点３８に対する相対位置Ｍ５が算出される。
次に、算出した相対位置Ｍ５を、同じｘ５ｙ５ｚ５座標系における点３６に対する相対位置Ｍ５′（ｘｍ５′，ｙｍ５′，ｚｍ５′）に座標変換する。相対位置Ｍ５′は、相対位置Ｍ５をベクトルｌ５分スライドさせた座標であるので、
【００２９】
【数２】
【００３０】
これによって、ｘ５ｙ５ｚ５座標系における点２４ａの点３６に対する相対位置Ｍ５′が算出される。このような座標変換を、上流側へ向かって各点毎に繰り返し実施することで、ベクトルｌ１方向をｚ１軸、関節３０の回転軸方向をｘ１軸、ｘ１軸及びｚ１軸に直行する方向をｙ１軸とするｘ１ｙ１ｚ１座標系（頭部１２の座標系）における点２４ａの点１２ａに対する相対位置Ｍ１′（ｘｍ１′，ｙｍ１′，ｚｍ１′）が算出される。すなわち、
【００３１】
【数３】
【００３２】
同様にして、点２６ａの点１２ａに対する相対位置P１′も算出される。
【００３３】
（頭部１２の姿勢角度の算出）
次に、算出部８０は、軸足判定部８４から、現在の軸足を読取る。そして、その軸足の各関節と関節３０の角度から頭部１２の姿勢角度を算出する。
すなわち、軸足側の足部（接地足）は、床面と平行となっている。従って、頭部１２の姿勢角度は、軸足側の足部から頭部１２までの関節角によって求めることができる。図５に示すように、頭部１２の姿勢角度は、ベクトルｌ１とｚ′軸とのｘ′軸周りでの傾斜角度φ０ｘ及びｙ′軸周りでの傾斜角度φ０ｙによって示される。例えば、図２に示すように、脚部５２が軸足である場合には、次の式で算出される。
【００３４】
【数４】
【００３５】
（ロボット１０の座標系における相対位置の算出）
次に、ベクトルＭ１′、ベクトルＰ１′と、角度φ０ｘ及び角度φ０ｙから、ｘ′ｙ′ｚ′座標系（ロボット１０の座標系）における点２４ａの点１２ａに対する相対位置Ｍ０（ｘｍ０，ｙｍ０，ｚｍ０）及びｘ′ｙ′ｚ′座標系における点２６ａの点１２ａに対する相対位置Ｐ０（ｘｐ０，ｙｐ０，ｚｐ０）を算出する。相対位置Ｍ０、Ｐ０の算出は、相対位置Ｍ１′、Ｐ１′を、角度φ０ｘ及び角度φ０ｙによって座標変換することで求められる。すなわち、
【００３６】
【数５】
【００３７】
算出された相対位置Ｍ０、Ｐ０が、頭部１２に対する先端部２４ａ、２６ａの相対位置Ｍｔｉ、Ｐｔｉである。
【００３８】
（２）先端部２４ａ、２６ａの絶対位置の算出
算出部８０は、まず、入力された絶対位置データＡｔｉ及び前回の処理周期で入力された絶対位置データＡｔｉ−１から、ロボット１０の進行方向を算出する。ロボット１０の進行方向は、ｘ軸との角度θによって表すことができる。
角度θを算出すると、算出部８０は、ｘ′ｙ′ｚ′座標系の相対位置データＭｔｉ，Ｐｔｉをｘｙｚ座標系における先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉに座標変換する。絶対位置データＡＭｔｉ、ＡＰｔｉは、相対位置データＭｔｉ，Ｐｔｉをｚ軸周りに角度θだけ回転し、絶対位置データＡｔｉだけスライドすることで得られる。すなわち、
【００３９】
【数６】
【００４０】
算出部８０が、算出した先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉは予測位置算出部８２に入力される。また、先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉと頂点１２ａの絶対位置データＡｔｉ軸足判定部８４に入力される。
【００４１】
予測位置算出部８２は、算出された絶対位置データＡＭｔｉ、ＡＰｔｉから現在の足部２４，２６の先端部２４ａ、２６ａの絶対位置を予測する。
すなわち、予測位置算出部８２には、絶対位置データＡＭｔｉ、ＡＰｔｉが所定周期で入力される。予測位置算出部８２は、先端部２４ａ，２６ａが絶対位置データＡＭｔｉ、ＡＰｔｉにあった時点から現在までに各モータに出力した移動指令値を制御部６２から読取る。そして、読取った移動指令値から予測される先端部２４ａ，２６ａの相対移動値と、入力された絶対位置データＡＭｔｉ、ＡＰｔｉから現在の先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０を予測する。
予測位置算出部８２が予測した絶対位置データＡＭ０、ＡＰ０は制御部６２に入力される。
【００４２】
時刻データ同期部８６は、制御部６２に制御データを入力してロボット１０に特定の動作を実行させるとともに、ＧＰＳセンサ５８から入力される絶対位置データＡと時刻データｔを読取る。そして、関節角から算出した頭部１２の頂点１２ａの運動軌道と、ＧＰＳセンサ５８から入力される絶対位置データＡから算出した頂点１２ａの運動軌道とに基づいて、ＧＰＳ信号出力装置１０２の時刻カウント部１０６がカウントする時刻データｔと、時刻カウント部７２がカウントする時刻データｓを同期させる。
【００４３】
制御部６２は、予測位置算出部８２が予測した先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０と、各エンコーダから入力される関節角φ１〜φ９と、別途入力される目標位置座標（すなわち、目標軌道データ）に基づいて、各モータに出力するトルク指令値を算出する。具体的には、制御部６２は、まず、予測位置算出部８２が予測した先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０と、目標軌道データによる先端部２４ａ，２６ａの目標位置とのずれから、次に出力する目標位置を修正する。そして、修正した目標位置と各エンコーダから入力される関節角φ１〜φ９に基づいて、各モータに出力するトルク指令値を算出する。算出されたトルク指令値は、各モータに出力される。
なお、制御部６２が各モータへ出力するトルク指令値を算出する手順については、従来公知の方法を用いることができるため、ここでは、その詳細な説明については省略する。
【００４４】
次に、上述した制御装置６０が実行する絶対位置算出処理について説明する。図７は、制御装置６０の絶対位置算出処理を示すフローチャートである。
図７に示すように制御装置６０は、まず、ＧＰＳセンサ５８から絶対位置データＡｔｉと時刻データｔｉが入力されたか否かを判定する（ステップＳ２）。ＧＰＳセンサ５８からのデータ入力がない場合（ステップＳ２でＮＯ）は、制御装置６０は処理をそのまま終了する。ＧＰＳセンサ５８からのデータ入力がある場合（ステップＳ２でＹＥＳ）は、ステップＳ４に進む。
【００４５】
ステップＳ４に進むと、制御装置６０は、ＧＰＳセンサ５８から入力した時刻データｔｉをキーとして、記憶部７６に記憶されている角度データφ１〜φ９のうち、入力された時刻データｔｉと対応する角度データφ１〜φ９を読み出す（ステップＳ４）。すなわち、入力された時刻データｔｉに最も近い時刻データｓｎを特定し、時刻データｓｎに関連付けられた角度データφ１〜φ９を読み出す。
角度データφ１〜φ９を読み出すと、制御装置６０は、入力された時刻データｔｉより前の時点の記憶部７６に記憶されている角度データφ１〜φ９を消去する（ステップＳ６）。これによって、記憶部７６から不要なデータが消去される。
【００４６】
次に、制御装置６０は、軸足判定部８４より軸足がどちらであるかという情報を取得し（ステップＳ８）、その取得した軸足情報とステップＳ４で入力された角度データφ１〜φ９より、頭部１２の頂点１２ａに対する足部２４，２６の先端部２４ａ，２６ａの相対位置データＭｔｉ、Ｐｔｉを算出する（ステップＳ１０）。相対位置データＭｔｉ、Ｐｔｉを算出する手順については、既に説明している。
【００４７】
相対位置データＭｔｉ、Ｐｔｉを算出すると、制御装置６０は、ステップＳ２で入力されたＧＰＳセンサ５８から入力された絶対位置データＡｔｉと、ステップＳ１０で算出した相対位置データＭｔｉ、Ｐｔｉに基づいて、足部２４，２６の先端部２４ａ，２６ａの絶対位置ＡＭｔｉ、ＡＰｔｉを算出する（ステップＳ１２）。次に、制御装置６０は、ステップＳ１２で算出された絶対位置ＡＭｔｉ、ＡＰｔｉと制御部６２から出力された移動指令値に基づいて、現在の足部２４，２６の先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０を予測する（ステップＳ１４）。これによって、制御装置６０の処理を終了する。
【００４８】
次に、制御装置６０で実行される時刻データ同期処理について説明する。図９は、座標算出部７０の時刻カウント部７２とＧＰＳ信号出力装置１０２の時刻カウント部１０６との同期を取るために、制御装置６０で実行される時刻データ同期処理を示すフローチャートである。
図８に示すように、時刻データ同期処理が開始されると、制御装置６０は、まず、ロボット１０を直立させて（ステップＳ３６）、ロボット１０の頭部１２の初期位置Ａｐ（ｘｐ，ｙｐ）取得する（ステップＳ３８）。すなわち、制御装置６０は、ＧＰＳセンサ５８から頭部１２の絶対位置データが出力されるまで待機し、ＧＰＳセンサ５８から絶対位置データが出力されると、その出力された絶対位置データを初期位置Ａｐとして記憶する。
【００４９】
ステップＳ４０に進むと、制御装置６０は、関節３０のモータに制御指令値を出力して、一定周期でロボット１０に首振り運動させる（ステップＳ４０）。ロボット１０に首振り運動をさせるのと平行して、制御装置６０は、ＧＰＳセンサ５８から出力される絶対位置データＡｔｉと時刻データｔｉを順次取得する（ステップＳ４２）。ステップＳ４２のデータ取得と平行して、制御装置６０は、関節３０の角度φ１を順次読み取るとともに、その読取り時刻ｓｎを取得する（ステップＳ４４）。
次に、制御装置６０は、ステップＳ４２で順次取得した絶対位置データＡｔｉのｙ座標ｙｔｉと、時刻データｔｉをグラフにプロットする（図１０参照、ステップＳ４６）。また、制御装置６０は、ステップＳ４４で順次取得した関節角φ１とステップＳ３８で取得した頭部１２の初期位置から、時刻ｓｎのときの頭部１２のｙ座標ｙｓｎを次の式より算出する。なお、次式のθは、上述したロボット１０の向きとｘ軸との角度である。算出された頭部１２のｙ座標ｙｓｎと時刻データｓｎはグラフにプロットされる（ステップＳ４８）。
【００５０】
【数７】
【００５１】
ステップＳ４６によってＧＰＳセンサ５８に基づく頭部１２の運動軌道がプロットされ、ステップＳ４８によってエンコーダの検出結果に基づく頭部１２の運動軌道がプロットされると、制御装置６０は、ｙ座標ｙｔｉと、ｙ座標ｙｓｎを比較し、時刻データｔと時刻データｓのズレを算出する（ステップＳ５０）。すなわち、ロボット１０は直立状態で首振り運動を行っているだけなので、ＧＰＳセンサ５８で検出されるロボット１０の頭部１２の運動軌道と、エンコーダで検出される関節角φ１に基づく頭部１２の運動軌道は、本来一致するはずである。したがって、ＧＰＳセンサ５８に基づく頭部１２のｙ座標ｙｔｉとエンコーダに基づく頭部１２のｙ座標ｙｓｎとの時間軸方向のズレ時間Δｔ（図１０のΔｔ）は、ＧＰＳ信号出力装置１０２の時刻カウント部１０６と制御装置６０内の時刻カウント部７２の時間ズレとなる。そこで、制御装置６０は、ステップＳ５０で算出されたズレ時間Δｔに応じて制御装置６０内の時刻カウント部７２の時刻データｓを補正する（ステップＳ５２）。これによって、ＧＰＳ信号出力装置１０２の時刻データｔと制御装置６０内の時刻データｓを正確に同期させることができる。
【００５２】
以上に説明したように、本実施例のロボット制御システム１００では、ＧＰＳセンサ５８がＧＰＳ信号を受信すると、ＧＰＳセンサ５８は、そのＧＰＳ信号に含まれる時刻情報ｔｉと、そのＧＰＳ信号から算出される絶対位置を制御装置６０に出力する。制御装置６０では、各エンコーダが検出した各関節の角度φ１〜φ９を読取るとともに、その読取り時刻ｓｎを読取り記憶部７６に記憶する。そして、制御装置６０が足部２４，２６の先端部２４ａ，２６ａの絶対位置を算出する際には、記憶部７６の角度データφ１〜φ９の中から、ＧＰＳセンサ５８で受信されたＧＰＳ信号に含まれる時刻情報ｔｉと対応する角度データφ１〜φ９を特定する。そして、制御装置６０は、ＧＰＳセンサ５８から出力された絶対位置データと、特定した角度データφ１〜φ９から、足部２４，２６の先端部２４ａ、２６ａの絶対位置を算出する。従って、制御装置６０は、先端部２４ａ、２６ａの絶対位置を正確に算出することができる。
また、本実施例のロボット制御システム１００では、ロボット１０の脚部５０、５２を床面に対して移動させない状態で、関節３０を駆動してロボット１０の頭部１２に首振り運動を行わせる。そして、首振り運動を行いながら所定周期でＧＰＳセンサ５８から出力される絶対位置を検出し、また、関節３０の角度φ１を読み取って頭部１２のｙ座標ｙｓｎを算出する。そして、ＧＰＳセンサ５８に基づく頭部１２のｙ座標ｙｓｎと、エンコーダの検出値に基づく頭部１２のｙ座標ｙｔｉを比較して、時刻データｔと時刻データｓのズレ時間Δｔを算出する。これによって、絶対位置を精度よく算出することができる。
また、本実施例のロボット制御システム１００では、制御装置６０で算出された先端部２４ａ，２６ａの絶対位置から現在の先端部２４ａ，２６ａの絶対位置を予測し、その予測した絶対位置に基づいて軌道データを修正する。このため、ロボット１０は、目標軌道を正確に歩行し、目標位置で正確に停止することができる。
【００５３】
なお、上述した実施例では、時刻データｔｉ及びｓｎに基づいて、ＧＰＳ電波が送信されたときの角度データφ１〜φ９を特定したが、本発明はこのような形態に限られない。例えば、ＧＰＳセンサ５８によるＧＰＳ電波の受信から絶対位置の出力までの遅れ時間を予め測定しておき、その遅れ時間に基づいて、ＧＰＳ電波が送信されたときの角度データφ１〜φ９を特定してもよい。
また、上述した実施例では、記憶部７６に角度データφ１〜φ９を記憶したが、本発明はこのような形態に限られない。例えば、制御装置６０は、検出した関節角φ１〜φ９から相対位置データＭｔｉ、Ｐｔｉを算出し、その算出した相対位置データＭｔｉ、Ｐｔｉを記憶部に記憶するようにしてもよい。
また、上述した実施例では、時刻データの同期処理において、ｙｓｎとｙｔｉを算出し、比較したが、本発明はこのような形態に限られない。例えば、関節角φ１と、ｙ座標ｙｔｉをそれぞれグラフにプロットすることによっても、それらの軌道の位相ずれからズレ時間Δｔを求めることができる。すなわち、頂点１２ａの位置変化を示す他の形式のデータをＧＰＳセンサの検出値及び関節角φ１から算出し、それらを比較することによって時刻データを同期させることができる。
また、上述の実施例ではロボット１０を直立させた状態で関節３０だけを駆動させて時刻データの同期処理を行ったが、足部２４、２６を移動させない状態で、他のさまざまな動作を行うことによっても時刻データの同期処理を行うことができる。
【００５４】
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例をさまざまに変形、変更したものが含まれる。
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【図面の簡単な説明】
【００５５】
【図１】本実施例のロボット制御システム１００の全体図。
【図２】本実施例のロボット１０の説明図。
【図３】本実施例のロボット１０のブロック図。
【図４】本実施例のロボット１０の説明図。
【図５】本実施例のロボット１０の頭部１２の姿勢角度の説明図。
【図６】頭部１２の絶対位置と関節角の検出タイミングの説明図。
【図７】足部２４、２６の絶対位置算出処理のフローチャート。
【図８】時刻データ同期処理のフローチャート。
【図９】時刻データ同期処理時の運動軌道のずれを示す図
【符号の説明】
【００５６】
１０：ロボット
１２：頭部
１２ａ：頂点
１４：胴体部
１６、１８：腿部
２０、２２：脛部
２４、２６：足部
２４ａ、２６ａ：先端部
３０〜４６：関節
５０：右脚部
５２：左脚部
５８：ＧＰＳセンサ
６０：制御装置
６２：制御部
７０：座標算出部
７２：時刻カウント部
７６：記憶部
７８：データ特定部
８０：算出部
８２：予測位置算出部
８４：軸足判定部
８６：時刻データ同期部
１００：ロボット制御システム
１０２：ＧＰＳ信号出力装置
１０４ａ〜１０４ｄ：ＧＰＳアンテナ
１０６：時刻カウント部
【技術分野】
【０００１】
本発明は、信号送信装置と、空間内を移動する移動体を備えた移動体制御システムに関する。
【背景技術】
【０００２】
ＧＰＳ等の位置検出システムによって、移動体の位置を認識しながら移動体を制御するする移動体制御システムが知られている。この種の移動体制御システムは、移動体と、移動体が移動する空間に信号を送信する信号送信装置とによって構成されている。移動体は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号送信装置が空間に送信する信号を受信する信号受信部と、信号受信部が受信した信号から信号受信部の絶対位置（以下では、本体の絶対位置という）を算出する本体位置算出部と、アクチュエータの駆動量を検出する駆動量検出部を有している。移動体は、空間内を移動しながらアクチュエータによって関節を駆動させ、可動部を運動させる。または、関節を駆動させ、可動部を運動させることによって空間内を移動する。
このような移動体では、可動部に所望の運動をさせるために、可動部の絶対位置が算出される。可動部の絶対位置の算出は、本体位置算出部が算出する本体の絶対位置と、駆動量検出部が検出する駆動量を用いて行われる。すなわち、アクチュエータの駆動量から本体に対する可動部の相対位置を算出し、算出した可動部の相対位置と本体の絶対位置から、可動部の絶対位置が算出される。
なお、上述した信号を利用して本体の絶対位置を検出する移動体については、特許文献１に開示されている。
【０００３】
【特許文献１】特開２００４−１７２４８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上述した移動体の制御システムでは、信号受信部で信号を受信してから本体の絶対位置が算出されるまでの間に、僅かな時間ｔを要する。すなわち、受信した信号から算出された本体の絶対位置は、算出された時点よりも時間ｔだけ前の時点の本体の絶対位置となっている。従って、本体が移動している場合には、本体の絶対位置を算出したタイミングにおける実際の本体の位置は、算出した本体の絶対位置とは僅かに異なる位置となる。このため、算出された絶対位置と算出した時点でのアクチュエータの駆動量に基づいて可動部の絶対位置を算出しても、その算出された絶対位置は正しい絶対位置とはならない。可動部の絶対位置を正確に算出することができないと、可動部を正確に運動させることができないという問題が生じる。
例えば、本体に関節を介して接続された可動部によって脚部が構成され、脚部を駆動することによって目標位置まで移動する移動体では、脚部の位置を基準として制御することがある。しかし、上述したように、本体の絶対位置の算出に時差が生じると、脚部の絶対位置が正確に算出されない。したがって、実際には脚部が目標位置に到達しているにもかかわらず、脚部が目標位置に到達していないと判定されるために、さらに脚部が駆動される。従って、移動体を目標位置で停止させることができない。
【０００５】
本発明は、上記した実情に鑑みてなされたものであり、移動体の可動部を精度良く運動させる移動体制御システムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明の移動体制御システムは、空間内を移動する移動体と、送信時間を示す時間情報を含んだ信号を前記空間内に対して送信する信号送信装置とを備えている。移動体は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、本体に取付けられ、信号送信装置が空間に送信する信号を受信する信号受信部と、信号受信部が受信した信号から信号受信部の絶対位置を算出する本体位置算出部と、アクチュエータの駆動量を検出する駆動量検出部と、本体位置算出部で算出した位置情報と駆動量検出部で検出されたアクチュエータの駆動量から可動部の絶対位置を算出する絶対位置算出部と、を備えている。その絶対位置算出部は、駆動量検出部でアクチュエータの駆動量を検出した時間を計時するタイマと、所定の時間間隔で駆動量検出部により検出されるアクチュエータの駆動量をタイマで計時された検出時間と共に順に記憶する記憶部と、本体位置算出部で算出した位置情報に対応する信号の送信時間情報に基づいて、信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定する駆動量特定部と、本体位置算出部で算出した位置情報と、駆動量特定部で特定した駆動量から、対応する信号の送信時における可動部の絶対位置を算出する可動部位置算出部を備えている。
この移動体制御システムでは、駆動量検出部で検出されるアクチュエータの駆動量は検出時間と共に順に記憶部に記憶される。また、信号送信装置は送信時間情報を含んだ信号を送信し、その信号は信号受信部で受信される。信号送信装置が送信する信号は、送信するのとほぼ同時に信号受信部に受信されるので、信号の送信時間情報は信号の受信時間を示している。本体位置算出部は、信号受信部が受信した信号に基づいて信号受信部の絶対位置を算出する。すると、駆動量特定部が、本体位置算出部が算出した位置情報に対応する信号の送信時間情報をキーとして、記憶部に記憶されている駆動量の中からそれらの信号を受信した時点におけるアクチュエータの駆動量を特定する。これによって、信号の受信時間と同じ時点におけるアクチュエータの駆動量が特定される。可動部位置算出部は、本体位置算出部が算出した位置情報と、駆動量特定部で特定した駆動量から、可動部の位置を特定する。従って、信号を受信した時点における可動部の絶対位置を精度良く算出することができる。
【０００７】
上述の移動体制御システムでは、絶対位置算出部が、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正する補正部をさらに有していることが好ましい。タイマで計時される時間と信号に含まれる時間情報とのズレを補正することで、より精度よく可動部の絶対位置を算出することができる。
【０００８】
タイマで計時される時間と信号に含まれる時間情報とのズレを補正する方法としては、例えば、次に記載する方法を採ることができる。
すなわち、補正部は、予め設定された動作パターンで本体に動作を行わせながら所定の時間間隔で本体位置算出部により信号受信部の絶対位置を算出し、本体に行わせた動作と本体位置算出部で算出した信号受信部の絶対位置軌道とのずれから、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正する。この方法によると、簡易な方法で両者のズレを補正することができる。
【０００９】
上述した絶対位置算出部によって算出される可動部の絶対位置は、位置情報が送信された時点の絶対位置（すなわち、現時点より所定の時間ｔだけ前の可動部の絶対位置）となる。したがって、絶対位置算出部は、可動部位置算出部によって算出された可動部の絶対位置に基づいて、現在の可動部の絶対位置を予測する予測位置算出部をさらに有することが好ましい。過去の正確な絶対位置に基づいて現在の絶対位置を予測することで、現在の絶対位置を精度良く予測することができる。
なお、現在の絶対位置を予測する方法としては、可動部位置算出部で算出された絶対位置と、その時点から現在までの本体及び可動部への移動指令値とに基づいて算出することができる。
【００１０】
また、本発明は、可動部の絶対位置を好適に算出することができる絶対位置算出方法を提供する。すなわち、本発明の絶対位置算出方法は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号を受信する信号受信部と、アクチュエータの駆動量を検出する駆動量検出部と、時間を計時するタイマと、データを記憶する記憶部を備えた空間内を自由に移動する移動体と、信号受信部に対して送信時間情報を含んだ信号を送信する信号送信装置とを備えた移動体制御システムにおいて、移動体の可動部の絶対位置を算出する。この方法は、駆動量検出部で検出されたアクチュエータの駆動量をその検出時間と共に所定の時間間隔で順に記憶部に記憶するステップと、信号送信装置から送信時間を示す時間情報を含んだ信号を送信するステップと、上記信号を信号受信部で受信し、受信した信号から信号受信部の絶対位置を算出するステップと、信号受信部で受信した信号の送信時間情報に基づいて信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定するステップと、算出した信号受信部の絶対位置と、特定した駆動量から、可動部の絶対位置を算出するステップを有する。
この方法によると、可動部の絶対位置を精度良く算出することができる。
【発明を実施するための最良の形態】
【００１１】
下記に詳細に説明する実施例の主要な特徴を最初に列記する。
（形態１）ロボット制御システムは、ロボットと、ＧＰＳ信号出力装置と、複数のＧＰＳアンテナによって構成されている。
（形態２）ＧＰＳ信号出力装置は、時刻をカウントする時刻カウント部を備えており、ＧＰＳ信号出力装置が時刻カウント部でカウントされた送信時刻情報を含んだ信号を各ＧＰＳアンテナに出力することによって、各ＧＰＳアンテナから同時に、送信時刻情報を含んだＧＰＳ電波が送信される。
（形態３）ロボットは、頭部と、頭部に取り付けられている第１可動部と、第１可動部に取り付けられている第２可動部と、・・、第ｎ−１可動部に取り付けられている第ｎ可動部を有している。頭部と第１可動部は関節によって接続されており、各可動部間も関節によって接続されている。各関節を駆動することによって、その関節によって接続されている各可動部が相対的に移動する。
（形態４）可動部群の一部によって脚部が構成されており、脚部の関節を駆動することによってロボットは歩行する。
（形態５）頭部には、ＧＰＳセンサ（ＧＰＳ受信装置）が設置されている。
（形態６）ＧＰＳセンサは、ＧＰＳ電波を受信し、受信したＧＰＳ電波を分析することでＧＰＳセンサ自体の絶対位置を検出する。ＧＰＳ電波によって伝送されるデータには、そのＧＰＳ電波を発信した時刻データが含まれている。ＧＰＳセンサは、検出された絶対位置と時刻データを出力する。
（形態７）各関節はモータを備えている。モータによって関節を駆動すると、その関節によって接続されている可動部が運動する。
（形態８）各モータはエンコーダを有しており、エンコーダはモータの回転角度を検出する。
（形態９）ロボットは制御装置を有している。制御装置は、ＧＰＳセンサと各エンコーダと各モータに電気的に接続されている。
（形態１０）制御装置は、足部の絶対位置を算出する座標算出部を有している。座標算出部は、ＧＰＳセンサから出力されたＧＰＳセンサの絶対位置及び時刻データと、各エンコーダの検出値から、足部の先端の絶対位置を算出する。
（形態１１）制御装置は、各エンコーダの検出値と、座標算出部が算出した足部の先端の絶対位置に基づいて、モータに制御指令値を出力し、ロボットを歩行させる。
（形態１２）座標算出部は、時刻をカウントするタイマと、ＧＰＳセンサから出力される本体の絶対位置と時刻データを読取る第１読取部と、各エンコーダの検出値を読取ると共にその検出時刻をタイマで計測する第２読取部と、第２読取部で読取った検出値をその検出時刻と関連付けて記憶する記憶部と、第１読取部で読取った時刻データをキーとして記憶部が記憶している検出値の中から、その時刻データと最も近い時刻データと関連付けられた検出値を特定する特定部と、第１読取部で読取った絶対位置と特定部で特定された検出値に基づいて可動部の絶対位置を算出する算出部を備えている。
【実施例】
【００１２】
本発明の一実施例に係るロボット制御システム１００について図面を参照しながら説明する。図１に示すように、ロボット制御システム１００は、ロボット１０と、ＧＰＳ信号出力装置１０２と、４つのＧＰＳアンテナ（いわゆる、室内ＧＰＳ衛星）１０４ａ〜１０４ｄによって構成されている。
【００１３】
図１に示すように、ロボット１０が移動するフィールドの周囲には、複数のＧＰＳアンテナ１０４ａ〜１０４ｄが取り付けられている。ＧＰＳアンテナ１０４ａ〜１０４ｄは、ＧＰＳ信号出力装置１０２に接続されている。ＧＰＳ信号出力装置１０２は、時刻ｔをカウントする時刻カウント部１０６を有している。ＧＰＳ信号出力装置１０２は時刻カウント部１０６がカウントする時刻情報ｔを読取り、時刻情報ｔを含んだ電気信号（ＧＰＳ信号）を各ＧＰＳアンテナ１０４ａ〜１０４ｄに出力する。すると、各ＧＰＳアンテナ１０４ａ〜１０４ｄは入力されたＧＰＳ信号を変換した電波（ＧＰＳ電波）をロボットが移動する空間に同時に送信する。従って、ロボットが移動する空間に、時刻情報ｔを含んだＧＰＳ電波がそれぞれ送信される。ＧＰＳ信号出力装置１０２はＧＰＳ信号を所定の周期で繰り返し出力するので、ロボットが移動する空間にはＧＰＳ電波が所定の周期で繰り返し送信される。また、ＧＰＳ信号出力装置１０２が時刻情報ｔを読取ってからＧＰＳアンテナ１０４ａ〜１０４ｄがＧＰＳ電波を送信するまでの処理はほぼ同時に行われるので、時刻情報ｔはそのＧＰＳ電波を送信した時刻を示している。
【００１４】
ロボット１０は、平坦な床面上に配置され、床面上を目標位置まで歩行して移動する自律移動ロボットである。
図１に示すように、ロボット１０は、頭部１２と、胴体部１４と、腿部１６、１８と、脛部２０、２２と、足部２４、２６によって構成されている。頭部１２と胴体部１４は、関節３０によって接続されている。胴体部１４と腿部１６は、関節３２、３４によって接続されている。腿部１６と脛部２０は、関節３６によって接続されている。脛部２０と足部２４は、関節３８によって接続されている。胴体部１４と腿部１８は、関節４０、４２によって接続されている。腿部１８と脛部２２は、関節４４によって接続されている。脛部２２と足部２６は、関節４６によって接続されている。腿部１６、脛部２０、足部２４、及び関節３２、３４、３６、３８によって、右脚部５０が形成されている。腿部１８、脛部２２、足部２６、及び関節４０、４２、４４、４６によって、左脚部５２が形成されている。
また、各関節には図示しないモータが配されている。モータを駆動することによって、各関節が駆動され、各関節の角度が変更される。各モータはエンコーダを有しており、エンコーダはモータの回転量を検出することによって、関節の角度φ１〜φ９を検出する。各モータ及び各エンコーダは、図３に示すように、後述する制御装置６０に接続されている。
【００１５】
図２は、各関節の位置と、各関節の回転軸を模式的に示した図である。図２では、各関節の位置が点３０〜４６によって示されている。また、足部２４、２６の先端部２４ａ、２６ａが点２４ａ、２６ａによって示されている。また、後述するＧＰＳセンサ５８が取り付けられている頭部１２の頂点１２ａの位置が点１２ａによって示されている。各点の相対位置がベクトルｌ１〜ｌ１１によって示されている。図２に示すように、関節３０〜４６は、それぞれ角度φ１〜φ９の方向に回転する。各関節のエンコーダは、各関角φ１〜φ９を検出する。
【００１６】
ロボット１０は、ＧＰＳ電波を受信するＧＰＳセンサ５８を備えている。ＧＰＳセンサ５８は、頭部１２の頂点１２ａに取り付けられている。ＧＰＳセンサ５８は、各ＧＰＳアンテナ１０４ａ〜１０４ｄから送信されるＧＰＳ電波（すなわち、ＧＰＳ信号）を受信し、受信したＧＰＳ信号に基づいてＧＰＳセンサ５８自体（すなわち、頭部１２の頂点１２ａ）の絶対位置Ａを検出する。また、ＧＰＳセンサ５８は受信したＧＰＳ信号に含まれる時刻情報ｔを読取る。上述したように、ＧＰＳ電波は所定周期で繰り返し送信されるので、ＧＰＳセンサ５８はＧＰＳ信号を所定周期で繰り返し受信し、絶対位置Ａと時刻情報ｔを検出する。ＧＰＳセンサ５８は、後述する制御装置６０と接続されており、検出した絶対位置Ａと時刻情報ｔを制御装置６０に出力する。
なお、ＧＰＳセンサ５８が、受信したＧＰＳ信号に基づいて絶対位置Ａを検出（算出）するには、各ＧＰＳアンテナ１０４ａ〜１０４ｄから送信されたＧＰＳ電波を分析して絶対位置Ａを演算する必要がある。従って、ＧＰＳ信号を受信してから絶対位置Ａが検出されるまでには、ある程度の時間を要する。受信したＧＰＳ信号から絶対位置Ａを演算する手順については、従来公知であるため、ここでは、その詳細な説明については省略する。
【００１７】
ロボット１０は、各関節３０〜４６のモータを制御する制御装置６０を有している。図３に示すように、制御装置６０は、ＧＰＳセンサ５８と、各関節３０〜４６を駆動するモータ及びそのモータのエンコーダにそれぞれ接続されている。制御装置６０は、ＣＰＵ，ＲＯＭ，ＲＡＭ等からならマイクロコンピュータによって構成されている。制御装置６０は、ＲＯＭに格納されているプログラムを実行することで、（１）各関節３０〜４６のモータにトルク指令値を出力して各関節３０〜４６を駆動させる制御部６２と、（２）ＧＰＳセンサ５８から出力される絶対位置Ａ及び時刻データｔと、各関節３０〜４６のエンコーダから入力される検出値φ１〜φ１１から、先端部２４ａ、２６ａの絶対位置を算出する座標算出部７０として機能する。
【００１８】
まず、座標算出部７０の構成について説明する。座標算出部７０は、時刻カウント部７２と、記憶部７６と、データ特定部７８と、算出部８０と、予測位置算出部８２と、軸足判定部８４を備えている。
【００１９】
軸足判定部８４は、右脚部５０と左脚部５２のいずれが軸足（すなわち、ロボット１０の重量がより多く加わっている方の脚部）かを判定する。軸足判定部８４には、後述する算出部８０から、足部２４、２６の先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉ及び頭部１２の頂点１２ａの絶対位置データＡｔｉが入力される。軸足判定部８４は、入力された先端部２４ａ、２６ａ及び頂点１２ａの絶対位置データから、いずれが軸足であるかを判定する。軸足判定部８４の判定値は、算出部８０に入力される。
【００２０】
時刻カウント部７２は、時刻をカウントする。時刻カウント部７２がカウントする時刻データｓは、上述したＧＰＳ信号出力装置１０２の時刻カウント部１０６がカウントする時刻データｔと略同一となるよう設定されている。時刻カウント部７２がカウントする時刻データｓは、記憶部７６に読取られる。
【００２１】
記憶部７６は、所定の周期で各エンコーダから各関節３０〜４６の関節角φ１〜φ９を読取り、記憶する。記憶部７６は、各エンコーダから関節角φ１〜φ９を読取る際に時刻カウント部７２の時刻データｓを読取り、読取った角度データφ１〜φ９を時刻データｓと関連付けて記憶する。
なお、エンコーダから各関節３０〜４６の関節角φ１〜φ９を読み取る周期は、ＧＰＳセンサ５８でＧＰＳ信号を受信してから絶対位置を出力するまでの周期と比較して短く設定されている。関節角φ１〜φ９を読み取る周期とＧＰＳセンサ５８で絶対位置を出力する周期との関係を図６に模式的に示している。図６に示すように、ＧＰＳセンサ５８から絶対位置を１回出力する間に、エンコーダの関節角φ１〜φ９が複数回読み取られ、記憶部７６に記憶されている。
【００２２】
データ特定部７８は、ＧＰＳセンサ５８から絶対位置Ａと時刻データｔが入力されたときに、その時刻データｔをキーとして、記憶部７６に記憶されている角度データの中から、ＧＰＳセンサ５８から出力された絶対位置Ａと対応する角度データを特定する。
すなわち、データ特定部７８には、ＧＰＳセンサ５８から時刻データｔｉと時刻ｔｉにおける頂点１２ａの絶対位置データＡｔｉが所定周期で入力される。データ特定部７８は、時刻データｔｉを入力されると、記憶部７６が記憶している時刻データｓを検索し、入力された時刻データｔｉと最も近い時刻データｓｎを特定する。そして、特定した時刻データｓｎと関連付けられている角度データφ１〜φ９を読取る。また、データ特定部７８は、記憶部７６から角度データφ１〜φ９を読取ると、その読取った角度データφ１〜φ９より前に記憶された角度データφ１〜φ９及び時刻データｓを消去する。そして、読取った角度データφ１〜φ９と、ＧＰＳセンサ５８から入力された絶対位置データＡｔｉを算出部８０に入力する。
【００２３】
算出部８０は、（１）データ特定部７８で特定された角度データφ１〜φ９から頭部１２に対する足部２４、２６（詳しくは、先端部２４ａ、２６ａ）の相対位置Ｍｔｉ、Ｐｔｉを算出し、（２）その算出した先端部２４ａ、２６ａの相対位置とＧＰＳセンサ５８から出力された頭部１２の絶対位置Ａｔｉから、先端部２４ａ、２６ａの絶対位置ＡＭｔｉ、ＡＰｔｉを算出する。
すなわち、算出部８０には、データ特定部７８から、絶対位置データＡｔｉと、角度データφ１〜φ９が所定周期で入力される。算出部８０は、データ特定部７８からデータが入力される毎に、入力された各データから時刻ｔｉにおける先端部２４ａ、２６ａの絶対位置ＡＭｔｉ、ＡＰｔｉ算出する。
【００２４】
（１）角度データφ１〜φ９から先端部２４ａ、２６ａの相対位置の算出
まず、角度データφ１〜φ９から先端部２４ａ、２６ａの相対位置を算出する方法について説明する。まず、ロボット内座標軸について説明し、その後に、相対位置を算出する手順について説明する。
【００２５】
（ロボット内座標軸の設定）
ロボット１０の基本姿勢は、図４に示す関節を伸ばした姿勢に設定されている。また、ロボット１０の正面方向をｘ′軸、垂直方向をｚ′軸、ｘ′軸及びｚ′軸と直行する方向をｙ′軸とするｘ′ｙ′ｚ′座標系（ロボット１０の座標系）が設定されている。
また、各点（関節３０〜４６、頂点１２ａ、先端部２４ａ、２６ａ）の位置は、相対位置ベクトルｌ１〜ｌ１１によって規定されている。ベクトルｌ１〜ｌ１１は、各点と隣接する下流側（頂点１２ａ側を上流とする）の点の相対位置を示している。ｌ１〜ｌ１１は、ロボット１０の各部の寸法によって定まる定数である。例えば、基本姿勢における点２６ａの点４６に対する相対位置は、ベクトルｌ１１＝（０，０，ｌ１１）によって規定する。また、各関節３０〜４６の角度φ１〜φ９は、基本姿勢に対する角度によって規定する。
以上のような設定の下で、点２４ａ、２６ａの時刻ｔｉにおける相対位置（相対位置データＭｔｉ、Ｐｔｉ）の算出が行われる。
【００２６】
（頭部１２の座標系における相対位置の算出）
図２における点２４ａの点３８に対する相対位置Ｍ６は、ベクトルｌ６方向をｚ６軸、関節３８の回転軸方向をｙ６軸、ｚ６軸及びｙ６軸と直行する方向をｘ６軸としたｘ６ｙ６ｚ６座標系（足部２４の座標系）によって規定する。従って、点２４ａの点３８に対する相対位置Ｍ６は、（０，０，ｌ６）となる。次に、ｘ６ｙ６ｚ６座標系で示されている点２４ａの相対位置Ｍ６を、ベクトルｌ５方向をｚ５軸、関節３８の回転軸方向をｙ５軸、ｚ５軸及びｙ５軸と直行する方向をｘ５軸としたｘ５ｙ５ｚ５座標系（脛部２０の座標系）における相対位置Ｍ５（ｘｍ５，ｙｍ５，ｚｍ５）に座標変換する。相対位置Ｍ５は、ベクトルＭ６を回転軸ｙ５周りに角度φ５回転させることで得られる。すなわち、
【００２７】
【数１】
【００２８】
ここで、Ｒｏｔ（ｙ５，φ５）は、所定の座標系をｙ５軸周りにφ５だけ回転させる回転ベクトルである。これによって、ｘ５ｙ５ｚ５座標系における点２４ａの点３８に対する相対位置Ｍ５が算出される。
次に、算出した相対位置Ｍ５を、同じｘ５ｙ５ｚ５座標系における点３６に対する相対位置Ｍ５′（ｘｍ５′，ｙｍ５′，ｚｍ５′）に座標変換する。相対位置Ｍ５′は、相対位置Ｍ５をベクトルｌ５分スライドさせた座標であるので、
【００２９】
【数２】
【００３０】
これによって、ｘ５ｙ５ｚ５座標系における点２４ａの点３６に対する相対位置Ｍ５′が算出される。このような座標変換を、上流側へ向かって各点毎に繰り返し実施することで、ベクトルｌ１方向をｚ１軸、関節３０の回転軸方向をｘ１軸、ｘ１軸及びｚ１軸に直行する方向をｙ１軸とするｘ１ｙ１ｚ１座標系（頭部１２の座標系）における点２４ａの点１２ａに対する相対位置Ｍ１′（ｘｍ１′，ｙｍ１′，ｚｍ１′）が算出される。すなわち、
【００３１】
【数３】
【００３２】
同様にして、点２６ａの点１２ａに対する相対位置P１′も算出される。
【００３３】
（頭部１２の姿勢角度の算出）
次に、算出部８０は、軸足判定部８４から、現在の軸足を読取る。そして、その軸足の各関節と関節３０の角度から頭部１２の姿勢角度を算出する。
すなわち、軸足側の足部（接地足）は、床面と平行となっている。従って、頭部１２の姿勢角度は、軸足側の足部から頭部１２までの関節角によって求めることができる。図５に示すように、頭部１２の姿勢角度は、ベクトルｌ１とｚ′軸とのｘ′軸周りでの傾斜角度φ０ｘ及びｙ′軸周りでの傾斜角度φ０ｙによって示される。例えば、図２に示すように、脚部５２が軸足である場合には、次の式で算出される。
【００３４】
【数４】
【００３５】
（ロボット１０の座標系における相対位置の算出）
次に、ベクトルＭ１′、ベクトルＰ１′と、角度φ０ｘ及び角度φ０ｙから、ｘ′ｙ′ｚ′座標系（ロボット１０の座標系）における点２４ａの点１２ａに対する相対位置Ｍ０（ｘｍ０，ｙｍ０，ｚｍ０）及びｘ′ｙ′ｚ′座標系における点２６ａの点１２ａに対する相対位置Ｐ０（ｘｐ０，ｙｐ０，ｚｐ０）を算出する。相対位置Ｍ０、Ｐ０の算出は、相対位置Ｍ１′、Ｐ１′を、角度φ０ｘ及び角度φ０ｙによって座標変換することで求められる。すなわち、
【００３６】
【数５】
【００３７】
算出された相対位置Ｍ０、Ｐ０が、頭部１２に対する先端部２４ａ、２６ａの相対位置Ｍｔｉ、Ｐｔｉである。
【００３８】
（２）先端部２４ａ、２６ａの絶対位置の算出
算出部８０は、まず、入力された絶対位置データＡｔｉ及び前回の処理周期で入力された絶対位置データＡｔｉ−１から、ロボット１０の進行方向を算出する。ロボット１０の進行方向は、ｘ軸との角度θによって表すことができる。
角度θを算出すると、算出部８０は、ｘ′ｙ′ｚ′座標系の相対位置データＭｔｉ，Ｐｔｉをｘｙｚ座標系における先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉに座標変換する。絶対位置データＡＭｔｉ、ＡＰｔｉは、相対位置データＭｔｉ，Ｐｔｉをｚ軸周りに角度θだけ回転し、絶対位置データＡｔｉだけスライドすることで得られる。すなわち、
【００３９】
【数６】
【００４０】
算出部８０が、算出した先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉは予測位置算出部８２に入力される。また、先端部２４ａ、２６ａの絶対位置データＡＭｔｉ、ＡＰｔｉと頂点１２ａの絶対位置データＡｔｉ軸足判定部８４に入力される。
【００４１】
予測位置算出部８２は、算出された絶対位置データＡＭｔｉ、ＡＰｔｉから現在の足部２４，２６の先端部２４ａ、２６ａの絶対位置を予測する。
すなわち、予測位置算出部８２には、絶対位置データＡＭｔｉ、ＡＰｔｉが所定周期で入力される。予測位置算出部８２は、先端部２４ａ，２６ａが絶対位置データＡＭｔｉ、ＡＰｔｉにあった時点から現在までに各モータに出力した移動指令値を制御部６２から読取る。そして、読取った移動指令値から予測される先端部２４ａ，２６ａの相対移動値と、入力された絶対位置データＡＭｔｉ、ＡＰｔｉから現在の先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０を予測する。
予測位置算出部８２が予測した絶対位置データＡＭ０、ＡＰ０は制御部６２に入力される。
【００４２】
時刻データ同期部８６は、制御部６２に制御データを入力してロボット１０に特定の動作を実行させるとともに、ＧＰＳセンサ５８から入力される絶対位置データＡと時刻データｔを読取る。そして、関節角から算出した頭部１２の頂点１２ａの運動軌道と、ＧＰＳセンサ５８から入力される絶対位置データＡから算出した頂点１２ａの運動軌道とに基づいて、ＧＰＳ信号出力装置１０２の時刻カウント部１０６がカウントする時刻データｔと、時刻カウント部７２がカウントする時刻データｓを同期させる。
【００４３】
制御部６２は、予測位置算出部８２が予測した先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０と、各エンコーダから入力される関節角φ１〜φ９と、別途入力される目標位置座標（すなわち、目標軌道データ）に基づいて、各モータに出力するトルク指令値を算出する。具体的には、制御部６２は、まず、予測位置算出部８２が予測した先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０と、目標軌道データによる先端部２４ａ，２６ａの目標位置とのずれから、次に出力する目標位置を修正する。そして、修正した目標位置と各エンコーダから入力される関節角φ１〜φ９に基づいて、各モータに出力するトルク指令値を算出する。算出されたトルク指令値は、各モータに出力される。
なお、制御部６２が各モータへ出力するトルク指令値を算出する手順については、従来公知の方法を用いることができるため、ここでは、その詳細な説明については省略する。
【００４４】
次に、上述した制御装置６０が実行する絶対位置算出処理について説明する。図７は、制御装置６０の絶対位置算出処理を示すフローチャートである。
図７に示すように制御装置６０は、まず、ＧＰＳセンサ５８から絶対位置データＡｔｉと時刻データｔｉが入力されたか否かを判定する（ステップＳ２）。ＧＰＳセンサ５８からのデータ入力がない場合（ステップＳ２でＮＯ）は、制御装置６０は処理をそのまま終了する。ＧＰＳセンサ５８からのデータ入力がある場合（ステップＳ２でＹＥＳ）は、ステップＳ４に進む。
【００４５】
ステップＳ４に進むと、制御装置６０は、ＧＰＳセンサ５８から入力した時刻データｔｉをキーとして、記憶部７６に記憶されている角度データφ１〜φ９のうち、入力された時刻データｔｉと対応する角度データφ１〜φ９を読み出す（ステップＳ４）。すなわち、入力された時刻データｔｉに最も近い時刻データｓｎを特定し、時刻データｓｎに関連付けられた角度データφ１〜φ９を読み出す。
角度データφ１〜φ９を読み出すと、制御装置６０は、入力された時刻データｔｉより前の時点の記憶部７６に記憶されている角度データφ１〜φ９を消去する（ステップＳ６）。これによって、記憶部７６から不要なデータが消去される。
【００４６】
次に、制御装置６０は、軸足判定部８４より軸足がどちらであるかという情報を取得し（ステップＳ８）、その取得した軸足情報とステップＳ４で入力された角度データφ１〜φ９より、頭部１２の頂点１２ａに対する足部２４，２６の先端部２４ａ，２６ａの相対位置データＭｔｉ、Ｐｔｉを算出する（ステップＳ１０）。相対位置データＭｔｉ、Ｐｔｉを算出する手順については、既に説明している。
【００４７】
相対位置データＭｔｉ、Ｐｔｉを算出すると、制御装置６０は、ステップＳ２で入力されたＧＰＳセンサ５８から入力された絶対位置データＡｔｉと、ステップＳ１０で算出した相対位置データＭｔｉ、Ｐｔｉに基づいて、足部２４，２６の先端部２４ａ，２６ａの絶対位置ＡＭｔｉ、ＡＰｔｉを算出する（ステップＳ１２）。次に、制御装置６０は、ステップＳ１２で算出された絶対位置ＡＭｔｉ、ＡＰｔｉと制御部６２から出力された移動指令値に基づいて、現在の足部２４，２６の先端部２４ａ，２６ａの絶対位置ＡＭ０、ＡＰ０を予測する（ステップＳ１４）。これによって、制御装置６０の処理を終了する。
【００４８】
次に、制御装置６０で実行される時刻データ同期処理について説明する。図９は、座標算出部７０の時刻カウント部７２とＧＰＳ信号出力装置１０２の時刻カウント部１０６との同期を取るために、制御装置６０で実行される時刻データ同期処理を示すフローチャートである。
図８に示すように、時刻データ同期処理が開始されると、制御装置６０は、まず、ロボット１０を直立させて（ステップＳ３６）、ロボット１０の頭部１２の初期位置Ａｐ（ｘｐ，ｙｐ）取得する（ステップＳ３８）。すなわち、制御装置６０は、ＧＰＳセンサ５８から頭部１２の絶対位置データが出力されるまで待機し、ＧＰＳセンサ５８から絶対位置データが出力されると、その出力された絶対位置データを初期位置Ａｐとして記憶する。
【００４９】
ステップＳ４０に進むと、制御装置６０は、関節３０のモータに制御指令値を出力して、一定周期でロボット１０に首振り運動させる（ステップＳ４０）。ロボット１０に首振り運動をさせるのと平行して、制御装置６０は、ＧＰＳセンサ５８から出力される絶対位置データＡｔｉと時刻データｔｉを順次取得する（ステップＳ４２）。ステップＳ４２のデータ取得と平行して、制御装置６０は、関節３０の角度φ１を順次読み取るとともに、その読取り時刻ｓｎを取得する（ステップＳ４４）。
次に、制御装置６０は、ステップＳ４２で順次取得した絶対位置データＡｔｉのｙ座標ｙｔｉと、時刻データｔｉをグラフにプロットする（図１０参照、ステップＳ４６）。また、制御装置６０は、ステップＳ４４で順次取得した関節角φ１とステップＳ３８で取得した頭部１２の初期位置から、時刻ｓｎのときの頭部１２のｙ座標ｙｓｎを次の式より算出する。なお、次式のθは、上述したロボット１０の向きとｘ軸との角度である。算出された頭部１２のｙ座標ｙｓｎと時刻データｓｎはグラフにプロットされる（ステップＳ４８）。
【００５０】
【数７】
【００５１】
ステップＳ４６によってＧＰＳセンサ５８に基づく頭部１２の運動軌道がプロットされ、ステップＳ４８によってエンコーダの検出結果に基づく頭部１２の運動軌道がプロットされると、制御装置６０は、ｙ座標ｙｔｉと、ｙ座標ｙｓｎを比較し、時刻データｔと時刻データｓのズレを算出する（ステップＳ５０）。すなわち、ロボット１０は直立状態で首振り運動を行っているだけなので、ＧＰＳセンサ５８で検出されるロボット１０の頭部１２の運動軌道と、エンコーダで検出される関節角φ１に基づく頭部１２の運動軌道は、本来一致するはずである。したがって、ＧＰＳセンサ５８に基づく頭部１２のｙ座標ｙｔｉとエンコーダに基づく頭部１２のｙ座標ｙｓｎとの時間軸方向のズレ時間Δｔ（図１０のΔｔ）は、ＧＰＳ信号出力装置１０２の時刻カウント部１０６と制御装置６０内の時刻カウント部７２の時間ズレとなる。そこで、制御装置６０は、ステップＳ５０で算出されたズレ時間Δｔに応じて制御装置６０内の時刻カウント部７２の時刻データｓを補正する（ステップＳ５２）。これによって、ＧＰＳ信号出力装置１０２の時刻データｔと制御装置６０内の時刻データｓを正確に同期させることができる。
【００５２】
以上に説明したように、本実施例のロボット制御システム１００では、ＧＰＳセンサ５８がＧＰＳ信号を受信すると、ＧＰＳセンサ５８は、そのＧＰＳ信号に含まれる時刻情報ｔｉと、そのＧＰＳ信号から算出される絶対位置を制御装置６０に出力する。制御装置６０では、各エンコーダが検出した各関節の角度φ１〜φ９を読取るとともに、その読取り時刻ｓｎを読取り記憶部７６に記憶する。そして、制御装置６０が足部２４，２６の先端部２４ａ，２６ａの絶対位置を算出する際には、記憶部７６の角度データφ１〜φ９の中から、ＧＰＳセンサ５８で受信されたＧＰＳ信号に含まれる時刻情報ｔｉと対応する角度データφ１〜φ９を特定する。そして、制御装置６０は、ＧＰＳセンサ５８から出力された絶対位置データと、特定した角度データφ１〜φ９から、足部２４，２６の先端部２４ａ、２６ａの絶対位置を算出する。従って、制御装置６０は、先端部２４ａ、２６ａの絶対位置を正確に算出することができる。
また、本実施例のロボット制御システム１００では、ロボット１０の脚部５０、５２を床面に対して移動させない状態で、関節３０を駆動してロボット１０の頭部１２に首振り運動を行わせる。そして、首振り運動を行いながら所定周期でＧＰＳセンサ５８から出力される絶対位置を検出し、また、関節３０の角度φ１を読み取って頭部１２のｙ座標ｙｓｎを算出する。そして、ＧＰＳセンサ５８に基づく頭部１２のｙ座標ｙｓｎと、エンコーダの検出値に基づく頭部１２のｙ座標ｙｔｉを比較して、時刻データｔと時刻データｓのズレ時間Δｔを算出する。これによって、絶対位置を精度よく算出することができる。
また、本実施例のロボット制御システム１００では、制御装置６０で算出された先端部２４ａ，２６ａの絶対位置から現在の先端部２４ａ，２６ａの絶対位置を予測し、その予測した絶対位置に基づいて軌道データを修正する。このため、ロボット１０は、目標軌道を正確に歩行し、目標位置で正確に停止することができる。
【００５３】
なお、上述した実施例では、時刻データｔｉ及びｓｎに基づいて、ＧＰＳ電波が送信されたときの角度データφ１〜φ９を特定したが、本発明はこのような形態に限られない。例えば、ＧＰＳセンサ５８によるＧＰＳ電波の受信から絶対位置の出力までの遅れ時間を予め測定しておき、その遅れ時間に基づいて、ＧＰＳ電波が送信されたときの角度データφ１〜φ９を特定してもよい。
また、上述した実施例では、記憶部７６に角度データφ１〜φ９を記憶したが、本発明はこのような形態に限られない。例えば、制御装置６０は、検出した関節角φ１〜φ９から相対位置データＭｔｉ、Ｐｔｉを算出し、その算出した相対位置データＭｔｉ、Ｐｔｉを記憶部に記憶するようにしてもよい。
また、上述した実施例では、時刻データの同期処理において、ｙｓｎとｙｔｉを算出し、比較したが、本発明はこのような形態に限られない。例えば、関節角φ１と、ｙ座標ｙｔｉをそれぞれグラフにプロットすることによっても、それらの軌道の位相ずれからズレ時間Δｔを求めることができる。すなわち、頂点１２ａの位置変化を示す他の形式のデータをＧＰＳセンサの検出値及び関節角φ１から算出し、それらを比較することによって時刻データを同期させることができる。
また、上述の実施例ではロボット１０を直立させた状態で関節３０だけを駆動させて時刻データの同期処理を行ったが、足部２４、２６を移動させない状態で、他のさまざまな動作を行うことによっても時刻データの同期処理を行うことができる。
【００５４】
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例をさまざまに変形、変更したものが含まれる。
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【図面の簡単な説明】
【００５５】
【図１】本実施例のロボット制御システム１００の全体図。
【図２】本実施例のロボット１０の説明図。
【図３】本実施例のロボット１０のブロック図。
【図４】本実施例のロボット１０の説明図。
【図５】本実施例のロボット１０の頭部１２の姿勢角度の説明図。
【図６】頭部１２の絶対位置と関節角の検出タイミングの説明図。
【図７】足部２４、２６の絶対位置算出処理のフローチャート。
【図８】時刻データ同期処理のフローチャート。
【図９】時刻データ同期処理時の運動軌道のずれを示す図
【符号の説明】
【００５６】
１０：ロボット
１２：頭部
１２ａ：頂点
１４：胴体部
１６、１８：腿部
２０、２２：脛部
２４、２６：足部
２４ａ、２６ａ：先端部
３０〜４６：関節
５０：右脚部
５２：左脚部
５８：ＧＰＳセンサ
６０：制御装置
６２：制御部
７０：座標算出部
７２：時刻カウント部
７６：記憶部
７８：データ特定部
８０：算出部
８２：予測位置算出部
８４：軸足判定部
８６：時刻データ同期部
１００：ロボット制御システム
１０２：ＧＰＳ信号出力装置
１０４ａ〜１０４ｄ：ＧＰＳアンテナ
１０６：時刻カウント部
【特許請求の範囲】
【請求項１】
空間内を移動する移動体と、送信時間を示す時間情報を含んだ信号を前記空間内に対して送信する信号送信装置とを備えた移動体制御システムであって、
移動体は、
本体と、
その本体に関節を介して結合された可動部と、
関節を駆動するアクチュエータと、
本体に取付けられ、信号送信装置が空間に送信する信号を受信する信号受信部と、
信号受信部が受信した信号から信号受信部の絶対位置を算出する本体位置算出部と、
アクチュエータの駆動量を検出する駆動量検出部と、
本体位置算出部で算出した位置情報と駆動量検出部で検出されたアクチュエータの駆動量から可動部の絶対位置を算出する絶対位置算出部と、を備えており、
その絶対位置算出部は、
駆動量検出部でアクチュエータの駆動量を検出した時間を計時するタイマと
所定の時間間隔で駆動量検出部により検出されるアクチュエータの駆動量をタイマで計時された検出時間と共に順に記憶する記憶部と、
本体位置算出部で算出した位置情報に対応する信号の送信時間情報に基づいて、信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定する駆動量特定部と、
本体位置算出部で算出した位置情報と、駆動量特定部で特定した駆動量から、対応する信号の送信時における可動部の絶対位置を算出する可動部位置算出部を備えていることを特徴とする移動体制御システム。
【請求項２】
絶対位置算出部は、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正する補正部をさらに有していることを特徴とする請求項１の移動体制御システム。
【請求項３】
補正部は、予め設定された動作パターンで本体に動作を行わせながら所定の時間間隔で本体位置算出部によって信号受信部の絶対位置を算出し、本体に行わせた動作と本体位置算出部で算出した信号受信部の絶対位置軌道とのズレから、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正することを特徴とする請求項２の移動体制御システム。
【請求項４】
絶対位置算出部は、可動部位置算出部によって算出された可動部の絶対位置に基づいて、現在の可動部の絶対位置を予測する予測位置算出部をさらに有することを特徴とする請求項１〜３のいずれかの移動体制御システム。
【請求項５】
本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号を受信する信号受信部と、アクチュエータの駆動量を検出する駆動量検出部と、時間を計時するタイマと、データを記憶する記憶部を備えた空間内を自由に移動する移動体と、信号受信部に対して送信時間情報を含んだ信号を送信する信号送信装置とを備えた移動体制御システムにおいて、移動体の可動部の絶対位置を算出する方法であって、
駆動量検出部で検出されたアクチュエータの駆動量をその検出時間と共に所定の時間間隔で順に記憶部に記憶するステップと、
信号送信装置から送信時間を示す時間情報を含んだ信号を送信するステップと、
上記信号を信号受信部で受信し、受信した信号から信号受信部の絶対位置を算出するステップと、
信号受信部で受信した信号の送信時間情報に基づいて信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定するステップと、
算出した信号受信部の絶対位置と、特定した駆動量から、可動部の絶対位置を算出するステップを有することを特徴とする算出方法。
【請求項１】
空間内を移動する移動体と、送信時間を示す時間情報を含んだ信号を前記空間内に対して送信する信号送信装置とを備えた移動体制御システムであって、
移動体は、
本体と、
その本体に関節を介して結合された可動部と、
関節を駆動するアクチュエータと、
本体に取付けられ、信号送信装置が空間に送信する信号を受信する信号受信部と、
信号受信部が受信した信号から信号受信部の絶対位置を算出する本体位置算出部と、
アクチュエータの駆動量を検出する駆動量検出部と、
本体位置算出部で算出した位置情報と駆動量検出部で検出されたアクチュエータの駆動量から可動部の絶対位置を算出する絶対位置算出部と、を備えており、
その絶対位置算出部は、
駆動量検出部でアクチュエータの駆動量を検出した時間を計時するタイマと
所定の時間間隔で駆動量検出部により検出されるアクチュエータの駆動量をタイマで計時された検出時間と共に順に記憶する記憶部と、
本体位置算出部で算出した位置情報に対応する信号の送信時間情報に基づいて、信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定する駆動量特定部と、
本体位置算出部で算出した位置情報と、駆動量特定部で特定した駆動量から、対応する信号の送信時における可動部の絶対位置を算出する可動部位置算出部を備えていることを特徴とする移動体制御システム。
【請求項２】
絶対位置算出部は、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正する補正部をさらに有していることを特徴とする請求項１の移動体制御システム。
【請求項３】
補正部は、予め設定された動作パターンで本体に動作を行わせながら所定の時間間隔で本体位置算出部によって信号受信部の絶対位置を算出し、本体に行わせた動作と本体位置算出部で算出した信号受信部の絶対位置軌道とのズレから、タイマで計時される時間と信号送信装置から送信される信号に含まれる時間情報とのズレを補正することを特徴とする請求項２の移動体制御システム。
【請求項４】
絶対位置算出部は、可動部位置算出部によって算出された可動部の絶対位置に基づいて、現在の可動部の絶対位置を予測する予測位置算出部をさらに有することを特徴とする請求項１〜３のいずれかの移動体制御システム。
【請求項５】
本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号を受信する信号受信部と、アクチュエータの駆動量を検出する駆動量検出部と、時間を計時するタイマと、データを記憶する記憶部を備えた空間内を自由に移動する移動体と、信号受信部に対して送信時間情報を含んだ信号を送信する信号送信装置とを備えた移動体制御システムにおいて、移動体の可動部の絶対位置を算出する方法であって、
駆動量検出部で検出されたアクチュエータの駆動量をその検出時間と共に所定の時間間隔で順に記憶部に記憶するステップと、
信号送信装置から送信時間を示す時間情報を含んだ信号を送信するステップと、
上記信号を信号受信部で受信し、受信した信号から信号受信部の絶対位置を算出するステップと、
信号受信部で受信した信号の送信時間情報に基づいて信号が送信された時点におけるアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定するステップと、
算出した信号受信部の絶対位置と、特定した駆動量から、可動部の絶対位置を算出するステップを有することを特徴とする算出方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００７−２４５２４４（Ｐ２００７−２４５２４４Ａ）
【公開日】平成１９年９月２７日（２００７．９．２７）
【国際特許分類】
【出願番号】特願２００６−６７７９７（Ｐ２００６−６７７９７）
【出願日】平成１８年３月１３日（２００６．３．１３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成１９年９月２７日（２００７．９．２７）
【国際特許分類】
【出願日】平成１８年３月１３日（２００６．３．１３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]