JP4513773B2 - 移動体制御システム及び移動体の可動部の絶対位置算出方法 - Google Patents

Description

本発明は、信号送信装置と、空間内を移動する移動体を備えた移動体制御システムに関する。

ＧＰＳ等の位置検出システムによって、移動体の位置を認識しながら移動体を制御するする移動体制御システムが知られている。この種の移動体制御システムは、移動体と、移動体が移動する空間に信号を送信する信号送信装置とによって構成されている。移動体は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号送信装置が空間に送信する信号を受信する信号受信部と、信号受信部が受信した信号から信号受信部の絶対位置（以下では、本体の絶対位置という）を算出する本体位置算出部と、アクチュエータの駆動量を検出する駆動量検出部を有している。移動体は、空間内を移動しながらアクチュエータによって関節を駆動させ、可動部を運動させる。または、関節を駆動させ、可動部を運動させることによって空間内を移動する。
このような移動体では、可動部に所望の運動をさせるために、可動部の絶対位置が算出される。可動部の絶対位置の算出は、本体位置算出部が算出する本体の絶対位置と、駆動量検出部が検出する駆動量を用いて行われる。すなわち、アクチュエータの駆動量から本体に対する可動部の相対位置を算出し、算出した可動部の相対位置と本体の絶対位置から、可動部の絶対位置が算出される。
なお、上述した信号を利用して本体の絶対位置を検出する移動体については、特許文献１に開示されている。

特開２００４−１７２４８号公報

上述した移動体の制御システムでは、信号受信部で信号を受信してから本体の絶対位置が算出されるまでの間に、僅かな時間ｔを要する。すなわち、受信した信号から算出された本体の絶対位置は、算出された時点よりも時間ｔだけ前の時点の本体の絶対位置となっている。従って、本体が移動している場合には、本体の絶対位置を算出したタイミングにおける実際の本体の位置は、算出した本体の絶対位置とは僅かに異なる位置となる。このため、算出された絶対位置と算出した時点でのアクチュエータの駆動量に基づいて可動部の絶対位置を算出しても、その算出された絶対位置は正しい絶対位置とはならない。可動部の絶対位置を正確に算出することができないと、可動部を正確に運動させることができないという問題が生じる。
例えば、本体に関節を介して接続された可動部によって脚部が構成され、脚部を駆動することによって目標位置まで移動する移動体では、脚部の位置を基準として制御することがある。しかし、上述したように、本体の絶対位置の算出に時差が生じると、脚部の絶対位置が正確に算出されない。したがって、実際には脚部が目標位置に到達しているにもかかわらず、脚部が目標位置に到達していないと判定されるために、さらに脚部が駆動される。従って、移動体を目標位置で停止させることができない。

本発明は、上記した実情に鑑みてなされたものであり、移動体の可動部を精度良く運動させる移動体制御システムを提供することを目的とする。

本発明の移動体制御システムは、空間内を移動する移動体と、前記空間内に対して信号を送信する信号送信装置とを備えている。信号送信装置は、信号の出力時刻情報を含む信号を繰り返し出力する信号出力装置と、信号出力装置と接続されており、信号出力装置が出力した信号を前記空間内に送信する複数のアンテナを備えている。移動体は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、本体に取付けられ、信号送信装置の各アンテナが空間に送信する信号を受信する信号受信部と、各アンテナからの複数の信号から信号受信部の絶対位置を算出するとともに、信号の受信時刻情報を出力する本体位置算出部と、アクチュエータの駆動量を検出する駆動量検出部と、本体位置算出部で算出した絶対位置と駆動量検出部で検出されたアクチュエータの駆動量から可動部の絶対位置を算出する絶対位置算出部と、を備えている。その絶対位置算出部は、駆動量検出部でアクチュエータの駆動量を検出した時刻を計時するタイマと、所定の時間間隔で駆動量検出部により検出されるアクチュエータの駆動量をタイマで計時された検出時刻と共に順に記憶する記憶部と、本体位置算出部から出力された受信時刻情報に最も近い時刻に検出されたアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定する駆動量特定部と、本体位置算出部で算出した絶対位置と、駆動量特定部で特定した駆動量から、可動部の絶対位置を算出する可動部位置算出部を備えている。
この移動体制御システムでは、駆動量検出部で検出されるアクチュエータの駆動量は検出時刻と共に順に記憶部に記憶される。また、本体位置算出部は、信号受信部が受信した各アンテナからの複数の信号に基づいて信号受信部の絶対位置を算出するとともに、信号の受信時刻情報を出力する。すると、駆動量特定部が、受信時刻情報をキーとして、記憶部に記憶されている駆動量の中からその受信時刻情報に最も近い時刻に検出されたアクチュエータの駆動量を特定する。これによって、信号の受信時刻と同じ時点におけるアクチュエータの駆動量が特定される。可動部位置算出部は、本体位置算出部が算出した絶対位置と、駆動量特定部で特定した駆動量から、可動部の位置を特定する。従って、信号を受信した時点における可動部の絶対位置を精度良く算出することができる。

上述の移動体制御システムでは、絶対位置算出部が、タイマで計時される時刻と本体位置算出部で出力される受信時刻情報とのズレを補正する補正部をさらに有していることが好ましい。タイマで計時される時刻と受信時刻情報とのズレを補正することで、より精度よく可動部の絶対位置を算出することができる。

タイマで計時される時刻と受信時刻情報とのズレを補正する方法としては、例えば、次に記載する方法を採ることができる。
すなわち、補正部は、予め設定された動作パターンで本体に動作を行わせながら所定の時間間隔で本体位置算出部により信号受信部の絶対位置を算出し、本体に行わせた動作と本体位置算出部で算出した信号受信部の絶対位置軌道とのずれから、タイマで計時される時刻と本体位置算出部で出力される受信時刻情報とのズレを補正する。この方法によると、簡易な方法で両者のズレを補正することができる。

上述した絶対位置算出部によって算出される可動部の絶対位置は、信号の受信時刻における絶対位置（すなわち、現時点より所定の時間ｔだけ前の可動部の絶対位置）となる。したがって、絶対位置算出部は、可動部位置算出部によって算出された可動部の絶対位置に基づいて、現在の可動部の絶対位置を予測する予測位置算出部をさらに有することが好ましい。過去の正確な絶対位置に基づいて現在の絶対位置を予測することで、現在の絶対位置を精度良く予測することができる。
なお、現在の絶対位置を予測する方法としては、可動部位置算出部で算出された絶対位置と、その時点から現在までの本体及び可動部への移動指令値とに基づいて算出することができる。

また、本発明は、可動部の絶対位置を好適に算出することができる絶対位置算出方法を提供する。すなわち、本発明の絶対位置算出方法は、本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号を受信する信号受信部と、アクチュエータの駆動量を検出する駆動量検出部と、時刻を計時するタイマと、データを記憶する記憶部を備えた空間内を自由に移動する移動体と、信号の出力時刻情報を含む信号を出力する信号出力装置と、信号出力装置と接続されており、信号出力装置が出力した信号を前記空間内に送信する複数のアンテナを備えた信号送信装置とを備えた移動体制御システムにおいて、移動体の可動部の絶対位置を算出する。この方法は、駆動量検出部で検出されたアクチュエータの駆動量をその検出時刻と共に所定の時間間隔で順に記憶部に記憶するステップと、信号送信装置の各アンテナから出力時刻情報を含む信号を送信するステップと、信号送信装置の各アンテナから送信された信号を信号受信部で受信し、受信した各アンテナからの複数の信号から信号受信部の絶対位置を算出するとともに、信号の受信時刻情報を出力するステップと、受信時刻情報に最も近い時刻に検出されたアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定するステップと、算出した信号受信部の絶対位置と、特定した駆動量から、可動部の絶対位置を算出するステップを有する。
この方法によると、可動部の絶対位置を精度良く算出することができる。

下記に詳細に説明する実施例の主要な特徴を最初に列記する。
（形態１）ロボット制御システムは、ロボットと、ＧＰＳ信号出力装置と、複数のＧＰＳアンテナによって構成されている。
（形態２）ＧＰＳ信号出力装置は、時刻をカウントする時刻カウント部を備えており、ＧＰＳ信号出力装置が時刻カウント部でカウントされた送信時刻情報を含んだ信号を各ＧＰＳアンテナに出力することによって、各ＧＰＳアンテナから同時に、送信時刻情報を含んだＧＰＳ電波が送信される。
（形態３）ロボットは、頭部と、頭部に取り付けられている第１可動部と、第１可動部に取り付けられている第２可動部と、・・、第ｎ−１可動部に取り付けられている第ｎ可動部を有している。頭部と第１可動部は関節によって接続されており、各可動部間も関節によって接続されている。各関節を駆動することによって、その関節によって接続されている各可動部が相対的に移動する。
（形態４）可動部群の一部によって脚部が構成されており、脚部の関節を駆動することによってロボットは歩行する。
（形態５）頭部には、ＧＰＳセンサ（ＧＰＳ受信装置）が設置されている。
（形態６）ＧＰＳセンサは、ＧＰＳ電波を受信し、受信したＧＰＳ電波を分析することでＧＰＳセンサ自体の絶対位置を検出する。ＧＰＳ電波によって伝送されるデータには、そのＧＰＳ電波を発信した時刻データが含まれている。ＧＰＳセンサは、検出された絶対位置と時刻データを出力する。
（形態７）各関節はモータを備えている。モータによって関節を駆動すると、その関節によって接続されている可動部が運動する。
（形態８）各モータはエンコーダを有しており、エンコーダはモータの回転角度を検出する。
（形態９）ロボットは制御装置を有している。制御装置は、ＧＰＳセンサと各エンコーダと各モータに電気的に接続されている。
（形態１０）制御装置は、足部の絶対位置を算出する座標算出部を有している。座標算出部は、ＧＰＳセンサから出力されたＧＰＳセンサの絶対位置及び時刻データと、各エンコーダの検出値から、足部の先端の絶対位置を算出する。
（形態１１）制御装置は、各エンコーダの検出値と、座標算出部が算出した足部の先端の絶対位置に基づいて、モータに制御指令値を出力し、ロボットを歩行させる。
（形態１２）座標算出部は、時刻をカウントするタイマと、ＧＰＳセンサから出力される本体の絶対位置と時刻データを読取る第１読取部と、各エンコーダの検出値を読取ると共にその検出時刻をタイマで計測する第２読取部と、第２読取部で読取った検出値をその検出時刻と関連付けて記憶する記憶部と、第１読取部で読取った時刻データをキーとして記憶部が記憶している検出値の中から、その時刻データと最も近い時刻データと関連付けられた検出値を特定する特定部と、第１読取部で読取った絶対位置と特定部で特定された検出値に基づいて可動部の絶対位置を算出する算出部を備えている。

本発明の一実施例に係るロボット制御システム１００について図面を参照しながら説明する。図１に示すように、ロボット制御システム１００は、ロボット１０と、ＧＰＳ信号出力装置１０２と、４つのＧＰＳアンテナ（いわゆる、室内ＧＰＳ衛星）１０４ａ〜１０４ｄによって構成されている。

図１に示すように、ロボット１０が移動するフィールドの周囲には、複数のＧＰＳアンテナ１０４ａ〜１０４ｄが取り付けられている。ＧＰＳアンテナ１０４ａ〜１０４ｄは、ＧＰＳ信号出力装置１０２に接続されている。ＧＰＳ信号出力装置１０２は、時刻ｔをカウントする時刻カウント部１０６を有している。ＧＰＳ信号出力装置１０２は時刻カウント部１０６がカウントする時刻情報ｔを読取り、時刻情報ｔを含んだ電気信号（ＧＰＳ信号）を各ＧＰＳアンテナ１０４ａ〜１０４ｄに出力する。すると、各ＧＰＳアンテナ１０４ａ〜１０４ｄは入力されたＧＰＳ信号を変換した電波（ＧＰＳ電波）をロボットが移動する空間に同時に送信する。従って、ロボットが移動する空間に、時刻情報ｔを含んだＧＰＳ電波がそれぞれ送信される。ＧＰＳ信号出力装置１０２はＧＰＳ信号を所定の周期で繰り返し出力するので、ロボットが移動する空間にはＧＰＳ電波が所定の周期で繰り返し送信される。また、ＧＰＳ信号出力装置１０２が時刻情報ｔを読取ってからＧＰＳアンテナ１０４ａ〜１０４ｄがＧＰＳ電波を送信するまでの処理はほぼ同時に行われるので、時刻情報ｔはそのＧＰＳ電波を送信した時刻を示している。

ロボット１０は、平坦な床面上に配置され、床面上を目標位置まで歩行して移動する自律移動ロボットである。
図１に示すように、ロボット１０は、頭部１２と、胴体部１４と、腿部１６、１８と、脛部２０、２２と、足部２４、２６によって構成されている。頭部１２と胴体部１４は、関節３０によって接続されている。胴体部１４と腿部１６は、関節３２、３４によって接続されている。腿部１６と脛部２０は、関節３６によって接続されている。脛部２０と足部２４は、関節３８によって接続されている。胴体部１４と腿部１８は、関節４０、４２によって接続されている。腿部１８と脛部２２は、関節４４によって接続されている。脛部２２と足部２６は、関節４６によって接続されている。腿部１６、脛部２０、足部２４、及び関節３２、３４、３６、３８によって、右脚部５０が形成されている。腿部１８、脛部２２、足部２６、及び関節４０、４２、４４、４６によって、左脚部５２が形成されている。
また、各関節には図示しないモータが配されている。モータを駆動することによって、各関節が駆動され、各関節の角度が変更される。各モータはエンコーダを有しており、エンコーダはモータの回転量を検出することによって、関節の角度φ_１〜φ_９を検出する。各モータ及び各エンコーダは、図３に示すように、後述する制御装置６０に接続されている。

図２は、各関節の位置と、各関節の回転軸を模式的に示した図である。図２では、各関節の位置が点３０〜４６によって示されている。また、足部２４、２６の先端部２４ａ、２６ａが点２４ａ、２６ａによって示されている。また、後述するＧＰＳセンサ５８が取り付けられている頭部１２の頂点１２ａの位置が点１２ａによって示されている。各点の相対位置がベクトルｌ_１〜ｌ_１１によって示されている。図２に示すように、関節３０〜４６は、それぞれ角度φ_１〜φ_９の方向に回転する。各関節のエンコーダは、各関角φ_１〜φ_９を検出する。

ロボット１０は、ＧＰＳ電波を受信するＧＰＳセンサ５８を備えている。ＧＰＳセンサ５８は、頭部１２の頂点１２ａに取り付けられている。ＧＰＳセンサ５８は、各ＧＰＳアンテナ１０４ａ〜１０４ｄから送信されるＧＰＳ電波（すなわち、ＧＰＳ信号）を受信し、受信したＧＰＳ信号に基づいてＧＰＳセンサ５８自体（すなわち、頭部１２の頂点１２ａ）の絶対位置Ａを検出する。また、ＧＰＳセンサ５８は受信したＧＰＳ信号に含まれる時刻情報ｔを読取る。上述したように、ＧＰＳ電波は所定周期で繰り返し送信されるので、ＧＰＳセンサ５８はＧＰＳ信号を所定周期で繰り返し受信し、絶対位置Ａと時刻情報ｔを検出する。ＧＰＳセンサ５８は、後述する制御装置６０と接続されており、検出した絶対位置Ａと時刻情報ｔを制御装置６０に出力する。
なお、ＧＰＳセンサ５８が、受信したＧＰＳ信号に基づいて絶対位置Ａを検出（算出）するには、各ＧＰＳアンテナ１０４ａ〜１０４ｄから送信されたＧＰＳ電波を分析して絶対位置Ａを演算する必要がある。従って、ＧＰＳ信号を受信してから絶対位置Ａが検出されるまでには、ある程度の時間を要する。受信したＧＰＳ信号から絶対位置Ａを演算する手順については、従来公知であるため、ここでは、その詳細な説明については省略する。

ロボット１０は、各関節３０〜４６のモータを制御する制御装置６０を有している。図３に示すように、制御装置６０は、ＧＰＳセンサ５８と、各関節３０〜４６を駆動するモータ及びそのモータのエンコーダにそれぞれ接続されている。制御装置６０は、ＣＰＵ，ＲＯＭ，ＲＡＭ等からならマイクロコンピュータによって構成されている。制御装置６０は、ＲＯＭに格納されているプログラムを実行することで、（１）各関節３０〜４６のモータにトルク指令値を出力して各関節３０〜４６を駆動させる制御部６２と、（２）ＧＰＳセンサ５８から出力される絶対位置Ａ及び時刻データｔと、各関節３０〜４６のエンコーダから入力される検出値φ_１〜φ_１１から、先端部２４ａ、２６ａの絶対位置を算出する座標算出部７０として機能する。

まず、座標算出部７０の構成について説明する。座標算出部７０は、時刻カウント部７２と、記憶部７６と、データ特定部７８と、算出部８０と、予測位置算出部８２と、軸足判定部８４を備えている。

軸足判定部８４は、右脚部５０と左脚部５２のいずれが軸足（すなわち、ロボット１０の重量がより多く加わっている方の脚部）かを判定する。軸足判定部８４には、後述する算出部８０から、足部２４、２６の先端部２４ａ、２６ａの絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉ及び頭部１２の頂点１２ａの絶対位置データＡ_ｔｉが入力される。軸足判定部８４は、入力された先端部２４ａ、２６ａ及び頂点１２ａの絶対位置データから、いずれが軸足であるかを判定する。軸足判定部８４の判定値は、算出部８０に入力される。

時刻カウント部７２は、時刻をカウントする。時刻カウント部７２がカウントする時刻データｓは、上述したＧＰＳ信号出力装置１０２の時刻カウント部１０６がカウントする時刻データｔと略同一となるよう設定されている。時刻カウント部７２がカウントする時刻データｓは、記憶部７６に読取られる。

記憶部７６は、所定の周期で各エンコーダから各関節３０〜４６の関節角φ_１〜φ_９を読取り、記憶する。記憶部７６は、各エンコーダから関節角φ_１〜φ_９を読取る際に時刻カウント部７２の時刻データｓを読取り、読取った角度データφ_１〜φ_９を時刻データｓと関連付けて記憶する。
なお、エンコーダから各関節３０〜４６の関節角φ_１〜φ_９を読み取る周期は、ＧＰＳセンサ５８でＧＰＳ信号を受信してから絶対位置を出力するまでの周期と比較して短く設定されている。関節角φ_１〜φ_９を読み取る周期とＧＰＳセンサ５８で絶対位置を出力する周期との関係を図６に模式的に示している。図６に示すように、ＧＰＳセンサ５８から絶対位置を１回出力する間に、エンコーダの関節角φ_１〜φ_９が複数回読み取られ、記憶部７６に記憶されている。

データ特定部７８は、ＧＰＳセンサ５８から絶対位置Ａと時刻データｔが入力されたときに、その時刻データｔをキーとして、記憶部７６に記憶されている角度データの中から、ＧＰＳセンサ５８から出力された絶対位置Ａと対応する角度データを特定する。
すなわち、データ特定部７８には、ＧＰＳセンサ５８から時刻データｔ_ｉと時刻ｔ_ｉにおける頂点１２ａの絶対位置データＡ_ｔｉが所定周期で入力される。データ特定部７８は、時刻データｔ_ｉを入力されると、記憶部７６が記憶している時刻データｓを検索し、入力された時刻データｔ_ｉと最も近い時刻データｓ_ｎを特定する。そして、特定した時刻データｓ_ｎと関連付けられている角度データφ_１〜φ_９を読取る。また、データ特定部７８は、記憶部７６から角度データφ_１〜φ_９を読取ると、その読取った角度データφ_１〜φ_９より前に記憶された角度データφ_１〜φ_９及び時刻データｓを消去する。そして、読取った角度データφ_１〜φ_９と、ＧＰＳセンサ５８から入力された絶対位置データＡ_ｔｉを算出部８０に入力する。

算出部８０は、（１）データ特定部７８で特定された角度データφ_１〜φ_９から頭部１２に対する足部２４、２６（詳しくは、先端部２４ａ、２６ａ）の相対位置Ｍ_ｔｉ、Ｐ_ｔｉを算出し、（２）その算出した先端部２４ａ、２６ａの相対位置とＧＰＳセンサ５８から出力された頭部１２の絶対位置Ａ_ｔｉから、先端部２４ａ、２６ａの絶対位置ＡＭ_ｔｉ、ＡＰ_ｔｉを算出する。
すなわち、算出部８０には、データ特定部７８から、絶対位置データＡ_ｔｉと、角度データφ_１〜φ_９が所定周期で入力される。算出部８０は、データ特定部７８からデータが入力される毎に、入力された各データから時刻ｔ_ｉにおける先端部２４ａ、２６ａの絶対位置ＡＭ_ｔｉ、ＡＰ_ｔｉ算出する。

（１）角度データφ_１〜φ_９から先端部２４ａ、２６ａの相対位置の算出
まず、角度データφ_１〜φ_９から先端部２４ａ、２６ａの相対位置を算出する方法について説明する。まず、ロボット内座標軸について説明し、その後に、相対位置を算出する手順について説明する。

（ロボット内座標軸の設定）
ロボット１０の基本姿勢は、図４に示す関節を伸ばした姿勢に設定されている。また、ロボット１０の正面方向をｘ′軸、垂直方向をｚ′軸、ｘ′軸及びｚ′軸と直行する方向をｙ′軸とするｘ′ｙ′ｚ′座標系（ロボット１０の座標系）が設定されている。
また、各点（関節３０〜４６、頂点１２ａ、先端部２４ａ、２６ａ）の位置は、相対位置ベクトルｌ_１〜ｌ_１１によって規定されている。ベクトルｌ_１〜ｌ_１１は、各点と隣接する下流側（頂点１２ａ側を上流とする）の点の相対位置を示している。ｌ_１〜ｌ_１１は、ロボット１０の各部の寸法によって定まる定数である。例えば、基本姿勢における点２６ａの点４６に対する相対位置は、ベクトルｌ_１１＝（０，０，ｌ_１１）によって規定する。また、各関節３０〜４６の角度φ_１〜φ_９は、基本姿勢に対する角度によって規定する。
以上のような設定の下で、点２４ａ、２６ａの時刻ｔ_ｉにおける相対位置（相対位置データＭ_ｔｉ、Ｐ_ｔｉ）の算出が行われる。

（頭部１２の座標系における相対位置の算出）
図２における点２４ａの点３８に対する相対位置Ｍ_６は、ベクトルｌ_６方向をｚ_６軸、関節３８の回転軸方向をｙ_６軸、ｚ_６軸及びｙ_６軸と直行する方向をｘ_６軸としたｘ_６ｙ_６ｚ_６座標系（足部２４の座標系）によって規定する。従って、点２４ａの点３８に対する相対位置Ｍ_６は、（０，０，ｌ_６）となる。次に、ｘ_６ｙ_６ｚ_６座標系で示されている点２４ａの相対位置Ｍ_６を、ベクトルｌ_５方向をｚ_５軸、関節３８の回転軸方向をｙ_５軸、ｚ_５軸及びｙ_５軸と直行する方向をｘ_５軸としたｘ_５ｙ_５ｚ_５座標系（脛部２０の座標系）における相対位置Ｍ_５（ｘ_ｍ５，ｙ_ｍ５，ｚ_ｍ５）に座標変換する。相対位置Ｍ_５は、ベクトルＭ_６を回転軸ｙ_５周りに角度φ_５回転させることで得られる。すなわち、

ここで、Ｒｏｔ（ｙ_５，φ_５）は、所定の座標系をｙ_５軸周りにφ_５だけ回転させる回転ベクトルである。これによって、ｘ_５ｙ_５ｚ_５座標系における点２４ａの点３８に対する相対位置Ｍ_５が算出される。
次に、算出した相対位置Ｍ_５を、同じｘ_５ｙ_５ｚ_５座標系における点３６に対する相対位置Ｍ_５′（ｘ_ｍ５′，ｙ_ｍ５′，ｚ_ｍ５′）に座標変換する。相対位置Ｍ_５′は、相対位置Ｍ_５をベクトルｌ_５分スライドさせた座標であるので、

これによって、ｘ_５ｙ_５ｚ_５座標系における点２４ａの点３６に対する相対位置Ｍ_５′が算出される。このような座標変換を、上流側へ向かって各点毎に繰り返し実施することで、ベクトルｌ_１方向をｚ_１軸、関節３０の回転軸方向をｘ_１軸、ｘ_１軸及びｚ_１軸に直行する方向をｙ_１軸とするｘ_１ｙ_１ｚ_１座標系（頭部１２の座標系）における点２４ａの点１２ａに対する相対位置Ｍ_１′（ｘ_ｍ１′，ｙ_ｍ１′，ｚ_ｍ１′）が算出される。すなわち、

同様にして、点２６ａの点１２ａに対する相対位置P_１′も算出される。

（頭部１２の姿勢角度の算出）
次に、算出部８０は、軸足判定部８４から、現在の軸足を読取る。そして、その軸足の各関節と関節３０の角度から頭部１２の姿勢角度を算出する。
すなわち、軸足側の足部（接地足）は、床面と平行となっている。従って、頭部１２の姿勢角度は、軸足側の足部から頭部１２までの関節角によって求めることができる。図５に示すように、頭部１２の姿勢角度は、ベクトルｌ_１とｚ′軸とのｘ′軸周りでの傾斜角度φ_０ｘ及びｙ′軸周りでの傾斜角度φ_０ｙによって示される。例えば、図２に示すように、脚部５２が軸足である場合には、次の式で算出される。

（ロボット１０の座標系における相対位置の算出）
次に、ベクトルＭ_１′、ベクトルＰ_１′と、角度φ_０ｘ及び角度φ_０ｙから、ｘ′ｙ′ｚ′座標系（ロボット１０の座標系）における点２４ａの点１２ａに対する相対位置Ｍ_０（ｘ_ｍ０，ｙ_ｍ０，ｚ_ｍ０）及びｘ′ｙ′ｚ′座標系における点２６ａの点１２ａに対する相対位置Ｐ_０（ｘ_ｐ０，ｙ_ｐ０，ｚ_ｐ０）を算出する。相対位置Ｍ_０、Ｐ_０の算出は、相対位置Ｍ_１′、Ｐ_１′を、角度φ_０ｘ及び角度φ_０ｙによって座標変換することで求められる。すなわち、

算出された相対位置Ｍ_０、Ｐ_０が、頭部１２に対する先端部２４ａ、２６ａの相対位置Ｍ_ｔｉ、Ｐ_ｔｉである。

（２）先端部２４ａ、２６ａの絶対位置の算出
算出部８０は、まず、入力された絶対位置データＡ_ｔｉ及び前回の処理周期で入力された絶対位置データＡ_ｔｉ−１から、ロボット１０の進行方向を算出する。ロボット１０の進行方向は、ｘ軸との角度θによって表すことができる。
角度θを算出すると、算出部８０は、ｘ′ｙ′ｚ′座標系の相対位置データＭ_ｔｉ，Ｐ_ｔｉをｘｙｚ座標系における先端部２４ａ、２６ａの絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉに座標変換する。絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉは、相対位置データＭ_ｔｉ，Ｐ_ｔｉをｚ軸周りに角度θだけ回転し、絶対位置データＡ_ｔｉだけスライドすることで得られる。すなわち、

算出部８０が、算出した先端部２４ａ、２６ａの絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉは予測位置算出部８２に入力される。また、先端部２４ａ、２６ａの絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉと頂点１２ａの絶対位置データＡ_ｔｉ軸足判定部８４に入力される。

予測位置算出部８２は、算出された絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉから現在の足部２４，２６の先端部２４ａ、２６ａの絶対位置を予測する。
すなわち、予測位置算出部８２には、絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉが所定周期で入力される。予測位置算出部８２は、先端部２４ａ，２６ａが絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉにあった時点から現在までに各モータに出力した移動指令値を制御部６２から読取る。そして、読取った移動指令値から予測される先端部２４ａ，２６ａの相対移動値と、入力された絶対位置データＡＭ_ｔｉ、ＡＰ_ｔｉから現在の先端部２４ａ，２６ａの絶対位置ＡＭ_０、ＡＰ_０を予測する。
予測位置算出部８２が予測した絶対位置データＡＭ_０、ＡＰ_０は制御部６２に入力される。

時刻データ同期部８６は、制御部６２に制御データを入力してロボット１０に特定の動作を実行させるとともに、ＧＰＳセンサ５８から入力される絶対位置データＡと時刻データｔを読取る。そして、関節角から算出した頭部１２の頂点１２ａの運動軌道と、ＧＰＳセンサ５８から入力される絶対位置データＡから算出した頂点１２ａの運動軌道とに基づいて、ＧＰＳ信号出力装置１０２の時刻カウント部１０６がカウントする時刻データｔと、時刻カウント部７２がカウントする時刻データｓを同期させる。

制御部６２は、予測位置算出部８２が予測した先端部２４ａ，２６ａの絶対位置ＡＭ_０、ＡＰ_０と、各エンコーダから入力される関節角φ_１〜φ_９と、別途入力される目標位置座標（すなわち、目標軌道データ）に基づいて、各モータに出力するトルク指令値を算出する。具体的には、制御部６２は、まず、予測位置算出部８２が予測した先端部２４ａ，２６ａの絶対位置ＡＭ_０、ＡＰ_０と、目標軌道データによる先端部２４ａ，２６ａの目標位置とのずれから、次に出力する目標位置を修正する。そして、修正した目標位置と各エンコーダから入力される関節角φ_１〜φ_９に基づいて、各モータに出力するトルク指令値を算出する。算出されたトルク指令値は、各モータに出力される。
なお、制御部６２が各モータへ出力するトルク指令値を算出する手順については、従来公知の方法を用いることができるため、ここでは、その詳細な説明については省略する。

次に、上述した制御装置６０が実行する絶対位置算出処理について説明する。図７は、制御装置６０の絶対位置算出処理を示すフローチャートである。
図７に示すように制御装置６０は、まず、ＧＰＳセンサ５８から絶対位置データＡ_ｔｉと時刻データｔ_ｉが入力されたか否かを判定する（ステップＳ２）。ＧＰＳセンサ５８からのデータ入力がない場合（ステップＳ２でＮＯ）は、制御装置６０は処理をそのまま終了する。ＧＰＳセンサ５８からのデータ入力がある場合（ステップＳ２でＹＥＳ）は、ステップＳ４に進む。

ステップＳ４に進むと、制御装置６０は、ＧＰＳセンサ５８から入力した時刻データｔ_ｉをキーとして、記憶部７６に記憶されている角度データφ_１〜φ_９のうち、入力された時刻データｔ_ｉと対応する角度データφ_１〜φ_９を読み出す（ステップＳ４）。すなわち、入力された時刻データｔ_ｉに最も近い時刻データｓ_ｎを特定し、時刻データｓ_ｎに関連付けられた角度データφ_１〜φ_９を読み出す。
角度データφ_１〜φ_９を読み出すと、制御装置６０は、入力された時刻データｔ_ｉより前の時点の記憶部７６に記憶されている角度データφ_１〜φ_９を消去する（ステップＳ６）。これによって、記憶部７６から不要なデータが消去される。

次に、制御装置６０は、軸足判定部８４より軸足がどちらであるかという情報を取得し（ステップＳ８）、その取得した軸足情報とステップＳ４で入力された角度データφ_１〜φ_９より、頭部１２の頂点１２ａに対する足部２４，２６の先端部２４ａ，２６ａの相対位置データＭ_ｔｉ、Ｐ_ｔｉを算出する（ステップＳ１０）。相対位置データＭ_ｔｉ、Ｐ_ｔｉを算出する手順については、既に説明している。

相対位置データＭ_ｔｉ、Ｐ_ｔｉを算出すると、制御装置６０は、ステップＳ２で入力されたＧＰＳセンサ５８から入力された絶対位置データＡ_ｔｉと、ステップＳ１０で算出した相対位置データＭ_ｔｉ、Ｐ_ｔｉに基づいて、足部２４，２６の先端部２４ａ，２６ａの絶対位置ＡＭ_ｔｉ、ＡＰ_ｔｉを算出する（ステップＳ１２）。次に、制御装置６０は、ステップＳ１２で算出された絶対位置ＡＭ_ｔｉ、ＡＰ_ｔｉと制御部６２から出力された移動指令値に基づいて、現在の足部２４，２６の先端部２４ａ，２６ａの絶対位置ＡＭ_０、ＡＰ_０を予測する（ステップＳ１４）。これによって、制御装置６０の処理を終了する。

次に、制御装置６０で実行される時刻データ同期処理について説明する。図９は、座標算出部７０の時刻カウント部７２とＧＰＳ信号出力装置１０２の時刻カウント部１０６との同期を取るために、制御装置６０で実行される時刻データ同期処理を示すフローチャートである。
図８に示すように、時刻データ同期処理が開始されると、制御装置６０は、まず、ロボット１０を直立させて（ステップＳ３６）、ロボット１０の頭部１２の初期位置Ａ_ｐ（ｘ_ｐ，ｙ_ｐ）取得する（ステップＳ３８）。すなわち、制御装置６０は、ＧＰＳセンサ５８から頭部１２の絶対位置データが出力されるまで待機し、ＧＰＳセンサ５８から絶対位置データが出力されると、その出力された絶対位置データを初期位置Ａ_ｐとして記憶する。

ステップＳ４０に進むと、制御装置６０は、関節３０のモータに制御指令値を出力して、一定周期でロボット１０に首振り運動させる（ステップＳ４０）。ロボット１０に首振り運動をさせるのと平行して、制御装置６０は、ＧＰＳセンサ５８から出力される絶対位置データＡ_ｔｉと時刻データｔ_ｉを順次取得する（ステップＳ４２）。ステップＳ４２のデータ取得と平行して、制御装置６０は、関節３０の角度φ_１を順次読み取るとともに、その読取り時刻ｓ_ｎを取得する（ステップＳ４４）。
次に、制御装置６０は、ステップＳ４２で順次取得した絶対位置データＡ_ｔｉのｙ座標ｙ_ｔｉと、時刻データｔ_ｉをグラフにプロットする（図１０参照、ステップＳ４６）。また、制御装置６０は、ステップＳ４４で順次取得した関節角φ_１とステップＳ３８で取得した頭部１２の初期位置から、時刻ｓ_ｎのときの頭部１２のｙ座標ｙ_ｓｎを次の式より算出する。なお、次式のθは、上述したロボット１０の向きとｘ軸との角度である。算出された頭部１２のｙ座標ｙ_ｓｎと時刻データｓ_ｎはグラフにプロットされる（ステップＳ４８）。

ステップＳ４６によってＧＰＳセンサ５８に基づく頭部１２の運動軌道がプロットされ、ステップＳ４８によってエンコーダの検出結果に基づく頭部１２の運動軌道がプロットされると、制御装置６０は、ｙ座標ｙ_ｔｉと、ｙ座標ｙ_ｓｎを比較し、時刻データｔと時刻データｓのズレを算出する（ステップＳ５０）。すなわち、ロボット１０は直立状態で首振り運動を行っているだけなので、ＧＰＳセンサ５８で検出されるロボット１０の頭部１２の運動軌道と、エンコーダで検出される関節角φ_１に基づく頭部１２の運動軌道は、本来一致するはずである。したがって、ＧＰＳセンサ５８に基づく頭部１２のｙ座標ｙ_ｔｉとエンコーダに基づく頭部１２のｙ座標ｙ_ｓｎとの時間軸方向のズレ時間Δｔ（図１０のΔｔ）は、ＧＰＳ信号出力装置１０２の時刻カウント部１０６と制御装置６０内の時刻カウント部７２の時間ズレとなる。そこで、制御装置６０は、ステップＳ５０で算出されたズレ時間Δｔに応じて制御装置６０内の時刻カウント部７２の時刻データｓを補正する（ステップＳ５２）。これによって、ＧＰＳ信号出力装置１０２の時刻データｔと制御装置６０内の時刻データｓを正確に同期させることができる。

以上に説明したように、本実施例のロボット制御システム１００では、ＧＰＳセンサ５８がＧＰＳ信号を受信すると、ＧＰＳセンサ５８は、そのＧＰＳ信号に含まれる時刻情報ｔ_ｉと、そのＧＰＳ信号から算出される絶対位置を制御装置６０に出力する。制御装置６０では、各エンコーダが検出した各関節の角度φ_１〜φ_９を読取るとともに、その読取り時刻ｓ_ｎを読取り記憶部７６に記憶する。そして、制御装置６０が足部２４，２６の先端部２４ａ，２６ａの絶対位置を算出する際には、記憶部７６の角度データφ_１〜φ_９の中から、ＧＰＳセンサ５８で受信されたＧＰＳ信号に含まれる時刻情報ｔ_ｉと対応する角度データφ_１〜φ_９を特定する。そして、制御装置６０は、ＧＰＳセンサ５８から出力された絶対位置データと、特定した角度データφ_１〜φ_９から、足部２４，２６の先端部２４ａ、２６ａの絶対位置を算出する。従って、制御装置６０は、先端部２４ａ、２６ａの絶対位置を正確に算出することができる。
また、本実施例のロボット制御システム１００では、ロボット１０の脚部５０、５２を床面に対して移動させない状態で、関節３０を駆動してロボット１０の頭部１２に首振り運動を行わせる。そして、首振り運動を行いながら所定周期でＧＰＳセンサ５８から出力される絶対位置を検出し、また、関節３０の角度φ_１を読み取って頭部１２のｙ座標ｙ_ｓｎを算出する。そして、ＧＰＳセンサ５８に基づく頭部１２のｙ座標ｙ_ｓｎと、エンコーダの検出値に基づく頭部１２のｙ座標ｙ_ｔｉを比較して、時刻データｔと時刻データｓのズレ時間Δｔを算出する。これによって、絶対位置を精度よく算出することができる。
また、本実施例のロボット制御システム１００では、制御装置６０で算出された先端部２４ａ，２６ａの絶対位置から現在の先端部２４ａ，２６ａの絶対位置を予測し、その予測した絶対位置に基づいて軌道データを修正する。このため、ロボット１０は、目標軌道を正確に歩行し、目標位置で正確に停止することができる。

なお、上述した実施例では、時刻データｔ_ｉ及びｓ_ｎに基づいて、ＧＰＳ電波が送信されたときの角度データφ_１〜φ_９を特定したが、本発明はこのような形態に限られない。例えば、ＧＰＳセンサ５８によるＧＰＳ電波の受信から絶対位置の出力までの遅れ時間を予め測定しておき、その遅れ時間に基づいて、ＧＰＳ電波が送信されたときの角度データφ_１〜φ_９を特定してもよい。
また、上述した実施例では、記憶部７６に角度データφ_１〜φ_９を記憶したが、本発明はこのような形態に限られない。例えば、制御装置６０は、検出した関節角φ_１〜φ_９から相対位置データＭ_ｔｉ、Ｐ_ｔｉを算出し、その算出した相対位置データＭ_ｔｉ、Ｐ_ｔｉを記憶部に記憶するようにしてもよい。
また、上述した実施例では、時刻データの同期処理において、ｙ_ｓｎとｙ_ｔｉを算出し、比較したが、本発明はこのような形態に限られない。例えば、関節角φ_１と、ｙ座標ｙ_ｔｉをそれぞれグラフにプロットすることによっても、それらの軌道の位相ずれからズレ時間Δｔを求めることができる。すなわち、頂点１２ａの位置変化を示す他の形式のデータをＧＰＳセンサの検出値及び関節角φ_１から算出し、それらを比較することによって時刻データを同期させることができる。
また、上述の実施例ではロボット１０を直立させた状態で関節３０だけを駆動させて時刻データの同期処理を行ったが、足部２４、２６を移動させない状態で、他のさまざまな動作を行うことによっても時刻データの同期処理を行うことができる。

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例をさまざまに変形、変更したものが含まれる。
本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

本実施例のロボット制御システム１００の全体図。 本実施例のロボット１０の説明図。 本実施例のロボット１０のブロック図。 本実施例のロボット１０の説明図。 本実施例のロボット１０の頭部１２の姿勢角度の説明図。 頭部１２の絶対位置と関節角の検出タイミングの説明図。 足部２４、２６の絶対位置算出処理のフローチャート。 時刻データ同期処理のフローチャート。 時刻データ同期処理時の運動軌道のずれを示す図

符号の説明

１０：ロボット
１２：頭部
１２ａ：頂点
１４：胴体部
１６、１８：腿部
２０、２２：脛部
２４、２６：足部
２４ａ、２６ａ：先端部
３０〜４６：関節
５０：右脚部
５２：左脚部
５８：ＧＰＳセンサ
６０：制御装置
６２：制御部
７０：座標算出部
７２：時刻カウント部
７６：記憶部
７８：データ特定部
８０：算出部
８２：予測位置算出部
８４：軸足判定部
８６：時刻データ同期部
１００：ロボット制御システム
１０２：ＧＰＳ信号出力装置
１０４ａ〜１０４ｄ：ＧＰＳアンテナ
１０６：時刻カウント部

Claims (5)

1. 空間内を移動する移動体と、前記空間内に対して信号を送信する信号送信装置とを備えた移動体制御システムであって、
   信号送信装置は、
   信号の出力時刻情報を含む信号を繰り返し出力する信号出力装置と、
   信号出力装置と接続されており、信号出力装置が出力した信号を前記空間内に送信する複数のアンテナ、
   を備えており、
   移動体は、
   本体と、
   その本体に関節を介して結合された可動部と、
   関節を駆動するアクチュエータと、
   本体に取付けられ、信号送信装置の各アンテナが空間に送信する信号を受信する信号受信部と、
   信号受信部が受信した各アンテナからの複数の信号から信号受信部の絶対位置を算出するとともに、信号の受信時刻情報を出力する本体位置算出部と、
   アクチュエータの駆動量を検出する駆動量検出部と、
   本体位置算出部で算出した絶対位置と駆動量検出部で検出されたアクチュエータの駆動量から可動部の絶対位置を算出する絶対位置算出部と、を備えており、
   その絶対位置算出部は、
   駆動量検出部でアクチュエータの駆動量を検出した時刻を計時するタイマと
   所定の時間間隔で駆動量検出部により検出されるアクチュエータの駆動量をタイマで計時された検出時刻と共に順に記憶する記憶部と、
   本体位置算出部から出力された受信時刻情報に最も近い時刻に検出されたアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定する駆動量特定部と、
   本体位置算出部で算出した絶対位置と、駆動量特定部で特定した駆動量から、可動部の絶対位置を算出する可動部位置算出部を備えていることを特徴とする移動体制御システム。
2. 絶対位置算出部は、タイマで計時される時刻と本体位置算出部で出力される受信時刻情報とのズレを補正する補正部をさらに有していることを特徴とする請求項１の移動体制御システム。
3. 補正部は、予め設定された動作パターンで本体に動作を行わせながら所定の時間間隔で本体位置算出部によって信号受信部の絶対位置を算出し、本体に行わせた動作と本体位置算出部で算出した信号受信部の絶対位置軌道とのズレから、タイマで計時される時刻と本体位置算出部で出力される受信時刻情報とのズレを補正することを特徴とする請求項２の移動体制御システム。
4. 絶対位置算出部は、可動部位置算出部によって算出された可動部の絶対位置に基づいて、現在の可動部の絶対位置を予測する予測位置算出部をさらに有することを特徴とする請求項１〜３のいずれかの移動体制御システム。
5. 本体と、その本体に関節を介して結合された可動部と、関節を駆動するアクチュエータと、信号を受信する信号受信部と、アクチュエータの駆動量を検出する駆動量検出部と、時刻を計時するタイマと、データを記憶する記憶部を備えた空間内を自由に移動する移動体と、
   信号の出力時刻情報を含む信号を出力する信号出力装置と、信号出力装置と接続されており、信号出力装置が出力した信号を前記空間内に送信する複数のアンテナを備えた信号送信装置、
   を備えた移動体制御システムにおいて、移動体の可動部の絶対位置を算出する方法であって、
   駆動量検出部で検出されたアクチュエータの駆動量をその検出時刻と共に所定の時間間隔で順に記憶部に記憶するステップと、
   信号送信装置の各アンテナから出力時刻情報を含む信号を送信するステップと、
   信号送信装置の各アンテナから送信された信号を信号受信部で受信し、受信した各アンテナからの複数の信号から信号受信部の絶対位置を算出するとともに、信号の受信時刻情報を出力するステップと、
   受信時刻情報に最も近い時刻に検出されたアクチュエータの駆動量を、記憶部に記憶されている駆動量の中から特定するステップと、
   算出した信号受信部の絶対位置と、特定した駆動量から、可動部の絶対位置を算出するステップを有することを特徴とする算出方法。

Images