(57)【特許請求の範囲】
【請求項１】
自身の移動に関して自身の位置データを少なくとも含む状態データを出力できるロボットを、それが存在する空間内において自己位置同定する、ロボット自己位置同定システムであって、
前記ロボットから順次取得した複数の状態データを記憶する状態データ記憶手段、
前記空間内に存在する実体をセンシングする環境センサ、
前記環境センサの出力に基づいて前記実体の位置を検出する位置検出手段、
前記位置検出手段によって検出された実体の複数の位置データを記憶する位置データ記憶手段、
前記状態データ記憶手段によって記憶された前記ロボットの複数の位置データおよび前記位置データ記憶手段によって記憶された前記実体の複数の位置データに基づいて、前記ロボットと前記実体とを関連付ける関連付手段、および
前記ロボットと関連付けられた前記実体の位置データに基づいて、前記状態データに含まれる位置データを補正する補正データを、前記ロボットに送信する送信手段を備え、
前記ロボットは、前記補正データに従って、自身の位置データを補正する、ロボット自己位置同定システム。
【請求項２】
前記状態データは、自身の角度データをさらに含み、
前記状態データ記憶手段は、前記角度データを含む状態データを記憶し、
前記状態データ記憶手段によって記憶された前記ロボットの複数の状態データおよび前記位置データ記憶手段によって記憶された前記実体の複数の位置データに基づいて、次の位置における角度を推定する角度推定手段、および
前記角度推定手段によって推定された角度および状態データ記憶手段によって記憶された状態データに含まれる角度データに基づいて、前記状態データに含まれる角度データを補正する角度補正データを算出する角度補正算出手段をさらに備え、
前記送信手段は、前記補正データおよび前記角度補正データを、前記ロボットに送信する第１送信手段を含み、
前記ロボットは、前記補正データおよび前記角度補正データに従って、自身の位置データおよび角度データを補正する、請求項１記載のロボット自己位置同定システム。
【請求項３】
前記角度推定手段は、角度を推定するときに、推定角度誤差をあわせて算出し、
前記推定角度誤差を補正精度として記憶する記憶手段をさらに備え、
前記送信手段は、前記記憶手段によって記憶された補正精度が低いとき、前記補正データのみを前記ロボットに送信する第２送信手段を含む、請求項２記載のロボット自己位置同定システム。
【請求項４】
前記角度推定手段は、前記状態データ記憶手段によって記憶された前記ロボットの複数の状態データおよび前記位置データ記憶手段によって記憶された前記実体の位置データに基づいて、前記ロボットの角度を観測する角度観測手段、および前記角度観測手段によって観測された角度の観測角度誤差を算出する観測角度誤差算出手段を含み、
前記角度推定手段は、前記観測された角度、前記観測角度誤差、前回推定された角度および前回算出された推定角度誤差に基づいて、次の位置の角度を推定し、推定角度誤差を算出する、請求項３記載のロボット自己位置同定システム。
【請求項５】
前記ロボットと前記実体との関連付けを維持するかを判断する判断手段、および
前記判断手段によって維持しないと判断されたとき、前記ロボットと前記実体との関連付けを解除する解除手段をさらに備える、請求項１ないし４のいずれかに記載のロボット自己位置同定システム。
【請求項６】
前記空間には、少なくとも２つ以上の実体が存在し、
前記関連付手段は、前記ロボットと前記実体との関連付けが解除されてから所定時間以内のとき、前記ロボットと各実体との第１速度差を算出する第１速度差算出手段、前記第１速度差算出手段によって算出された第１速度差が第１所定速度より大きいかを判断する第１速度判定手段、前記ロボットと各実体との距離が所定距離より離れているかを判定する距離判定手段、および前記第１速度判定手段によって前記ロボットとの第１速度差が前記第１所定速度以下と判定され、かつ前記距離判断手段によって前記ロボットとの距離が所定距離以内と判定された実体のうち、前記ロボットに最も近い実体と前記ロボットとを関連付ける第１関連付手段を含む、請求項５記載のロボット自己位置同定システム。
【請求項７】
前記空間には、少なくとも２つ以上の実体が存在し、
前記関連付手段は、前記ロボットと前記実体との関連付けが解除されてから所定時間が経過したとき、前記ロボットと各実体との第２速度差を算出する第２速度差算出手段、前記第２速度差算出手段によって算出された第２速度差が第２所定速度より大きいかを判定する第２速度判定手段、および前記第２速度判定手段によって前記ロボットの第２速度差が前記第２所定速度以下と判定された実体が１つだけであるとき、その実体と前記ロボットとを関連付ける第２関連付手段を含む、請求項５または６記載のロボット自己位置同定システム。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、ロボット自己位置同定システムに関し、特にたとえば、ロボットが存在する空間においてそのロボットを自己位置同定する、ロボット自己位置同定システムに関する。
【背景技術】
【０００２】
非特許文献１には、個別のＩＤが割り振られたRFIDタグが発する電波を受信し、電波強度を検出することで、RFIDタグを付けた人物の位置を推定し、人物を識別する技術が開示されている。
【０００３】
また、特許文献１には、位置検出装置が検出する位置情報を、適正に補正する位置補正装置が開示されている。位置検出装置は、GPSを利用しており、たとえば車両に取り付けられる。そして、位置補正装置は、GPSの測位に基づいて推定される推定車両速度と車両速度とから、車両の位置情報を補正する。
【非特許文献１】神田崇行,塩見昌裕,野村竜也,石黒浩,荻田紀博, "RFIDタグを用いた科学館来館者の移動軌跡の分析"（情報処理学会論文誌 Vol.49 No.5 P.1727-1742）
【特許文献１】特開２００２−３５０１５７号公報［G01C 21/00, G01S 5/14, G08G, 1/0969, G09B 29/00, G09B 29/10］
【発明の概要】
【発明が解決しようとする課題】
【０００４】
近年、様々なロボット自己位置同定システムが開発されている。そして、その一例として、非特許文献１の技術を応用し、RFIDタグをロボットに取り付けることでロボットを自己位置同定するシステムが開発されている。
【０００５】
ところが、上記したシステムではロボットにRFIDタグを取り付け、さらには環境にRFIDタグを受信するアンテナを設けなければならず、容易にロボットを自己位置同定することができない。また、広く一般的に利用されているGPSを利用すれば位置を検出することができるが、GPSの位置検出精度は低く、特許文献１で開示された技術を利用して位置補正を行ったとしても、ロボット自己位置同定システムには利用できない。
【０００６】
それゆえに、この発明の主たる目的は、新規な、ロボット自己位置同定システムを提供することである。
【０００７】
この発明の他の目的は、容易にロボットを自己位置同定することができる、ロボット自己位置同定システムを提供することである。
【課題を解決するための手段】
【０００８】
この発明は、上記の課題を解決するために、以下の構成を採用した。なお、括弧内の参照符号および補足説明等は、この発明の理解を助けるために記述する実施形態との対応関係を示したものであって、この発明を何ら限定するものではない。
【０００９】
第１の発明は、自身の移動に関して自身の位置データを少なくとも含む状態データを出力できるロボットを、それが存在する空間内において自己位置同定する、ロボット自己位置同定システムであって、ロボットから順次取得した複数の状態データを記憶する状態データ記憶手段、空間内に存在する実体をセンシングする環境センサ、環境センサの出力に基づいて実体の位置を検出する位置検出手段、位置検出手段によって検出された実体の複数の位置データを記憶する位置データ記憶手段、状態データ記憶手段によって記憶されたロボットの位置データおよび位置データ記憶手段によって記憶された実体の位置データに基づいて、ロボットと実体とを関連付ける関連付手段、およびロボットと関連付けられた実体の位置データに基づいて、状態データに含まれる位置データを補正する補正データを、ロボットに送信する送信手段を備え、ロボットは、補正データに従って、自身の位置データを補正する、ロボット自己位置同定システムである。
【００１０】
第１の発明では、ロボット自己位置同定システム（１００）は、空間（検出領域Ｆ）に存在するロボット（１８ａ，１８ｂ，１８ｃ）を、その空間内で自己位置同定する。また、ロボットは、自身の移動に関して自身の位置データを少なくとも含む状態データを出力できる。
【００１１】
状態データ記憶手段（２０，Ｓ９）は、たとえばメモリ（２２）のバッファにロボットから順次取得した状態データを記憶する。環境センサ（１２）は、たとえばＬＲＦ（Laser Range Finder）などであり、空間内に存在する人間やロボットなどの実体（Ｅａ，Ｅｂ，Ｅｃ，Ｅｄ）をセンシングする。位置検出手段（２０，Ｓ８９）は、環境センサの出力に対して、たとえばパーティクルフィルタを利用することで実体の位置を検出する。位置データ記憶手段（２０，Ｓ９１）は、たとえば状態データ記憶手段と同様、メモリのバッファに、検出された実体の位置データを記憶する。関連付手段（２０，Ｓ４９）は、バッファに記憶されたロボットの複数の位置データおよび実体の複数の位置データから、たとえば速度を求めることで、ロボットと実体とを関連付ける。そして、ロボットの位置データおよび実体の位置データは、空間における同じ平面座標系の値で示される。
【００１２】
送信手段（２０，Ｓ１５，Ｓ１９）は、ロボットと関連付けられた実体の位置データに基づいて、たとえば空間の位置を示す座標データを、補正データとしてロボットに送信する。そして、ロボットは、たとえば補正データを受け取ると、自身の位置データを補正する。
【００１３】
第１の発明によれば、容易に設置可能な環境センサと、ロボットが一般的に出力可能な状態データとを利用することで、ロボットの現在位置を容易に検出できるようになる。そのため、検出された実体の位置をロボットに送信することで、容易にロボットを自己位置同定することができるようになる。
【００１４】
第２の発明は、第１の発明に従属し、状態データは、自身の角度データをさらに含み、状態データ記憶手段は、角度データを含む状態データを記憶し、状態データ記憶手段によって記憶されたロボットの複数の状態データおよび位置データ記憶手段によって記憶された実体の複数の位置データに基づいて、次の位置における角度を推定する角度推定手段、および角度推定手段によって推定された角度および状態データ記憶手段によって記憶された状態データに含まれる角度データに基づいて、状態データに含まれる角度データを補正する角度補正データを算出する角度補正算出手段をさらに備え、送信手段は、補正データおよび角度補正データを、ロボットに送信する第１送信手段を含み、ロボットは、補正データおよび角度補正データに従って、自身の位置データおよび角度データを補正する。
【００１５】
第２の発明では、状態データ記憶手段は、ロボット自身の角度データをさらに含む、状態データを記憶する。角度推定手段（２０，Ｓ２６１−Ｓ２７７）は、たとえば実体の複数の位置データおよびロボットの複数の状態データからロボットの位置を予測することで、次の位置のロボットの角度を推定する。角度補正算出手段（２０，Ｓ２７９）は、たとえば現在の状態データに含まれる角度データと推定された角度（θ＾ｋ）との角度差を、角度補正データとして算出する。第１送信手段（２０，Ｓ１９）は、補正データおよび角度補正データを、ロボットに送信する。
【００１６】
第２の発明によれば、ロボットの角度も補正することで、自己位置同定の精度を高めることができる。
【００１７】
第３の発明は、第２の発明に従属し、角度推定手段は、角度を推定するときに、推定角度誤差をあわせて算出し、推定角度誤差を補正精度として記憶する記憶手段をさらに備え、送信手段は、記憶手段によって記憶された補正精度が低いとき、補正データのみをロボットに送信する第２送信手段を含む。
【００１８】
第３の発明では、角度推定手段は、角度を推定すると共に、推定角度誤差（σ２θ^ｋ）を算出する。記憶手段（２０，Ｓ２８１）は、推定角度誤差を補正精度として、バッファに記憶する。推定された角度の誤差が大きければ、第２送信手段（２０，Ｓ１５）は補正データのみをロボットに送信する。
【００１９】
第３の発明によれば、推定された角度の誤差を、補正精度として利用することで、ロボットの角度の補正が可能であるかを、判断できるようになる。
【００２０】
第４の発明は、第３の発明に従属し、角度推定手段は、状態データ記憶手段によって記憶されたロボットの複数の状態データおよび位置データ記憶手段によって記憶された実体の位置データに基づいて、ロボットの角度を観測する角度観測手段、および角度観測手段によって観測された角度の観測角度誤差を算出する観測角度誤差算出手段を含み、角度推定手段は、観測された角度、観測角度誤差、前回推定された角度および前回算出された推定角度誤差に基づいて、次の位置の角度を推定し、推定角度誤差を算出する。
【００２１】
第４の発明では、たとえば、角度観測手段（２０，Ｓ２６１−Ｓ２６５）は、複数の状態データから次の位置を予測して予測角度を算出し、実体の位置データから検知角度を算出する。そして、角度観測手段は、検知角度から予測角度を減算することで、ロボットの角度を観測（算出）する。観測角度誤差算出手段（２０，Ｓ２６７）は、たとえば、観測における位置の誤差（σ２ｐｏ）に対する実体の位置データの商に近似することで、観測角度誤差を算出する。
【００２２】
たとえば、角度推定手段は、観測された角度、観測角度誤差、前回推定された角度および前回算出された推定角度誤差に基づいて、カルマンゲイン（Ｋｋ）を求めることで、次の位置の角度を推定し、推定角度誤差を算出する。
【００２３】
第４の発明によれば、カルマンフィルタを用いて、次の位置の角度を推定することができる。
【００２４】
第５の発明は、第１の発明ないし第４の発明のいずれかに従属し、ロボットと実体との関連付けを維持するかを判断する判断手段、および判断手段によって維持しないと判断されたとき、ロボットと実体との関連付けを解除する解除手段をさらに備える。
【００２５】
第５の発明では、判断手段（２０，Ｓ１１７，Ｓ１１９，Ｓ１２１）は、たとえばロボットと実体との速度差や距離などに基づいて、関連付けを維持するかを判断する。解除手段（２０，Ｓ１２５）は、たとえばロボットと実体との速度差が大きかったり、ロボットと実体との距離が離れていたりすると、ロボットと実体との関連付けを解除する。
【００２６】
第５の発明によれば、エラーの要因となるロボットと実体との関連付けを、解除できるようになる。
【００２７】
第６の発明は、第５の発明に従属し、空間には、少なくとも２つ以上の実体が存在し、関連付手段は、ロボットと実体との関連付けが解除されてから所定時間以内のとき、ロボットと各実体との第１速度差を算出する第１速度差算出手段、第１速度差算出手段によって算出された第１速度差が第１所定速度より大きいかを判断する第１速度判定手段、ロボットと各実体との距離が所定距離より離れているかを判定する距離判定手段、および第１速度判定手段によってロボットとの第１速度差が第１所定速度以下と判定され、かつ距離判断手段によってロボットとの距離が所定距離以内と判定された実体のうち、ロボットに最も近い実体とロボットとを関連付ける第１関連付手段を含む。
【００２８】
第６の発明では、空間には、たとえば４つの実体が存在することがある。第１速度差算出手段（２０，Ｓ１９３）は、関連付けが解除されてから所定時間（５秒）以内のとき、たとえば５秒分のロボットの位置データおよび５秒分の実体の位置データから、第１速度差（ＲＭＳ（Root Mean Square）速度差）を算出する。第１速度判定手段（２０，Ｓ１９５）は、第１速度差が第１所定速度より大きいかを判断する。また、距離判定手段（２０，Ｓ１９９）は、ロボットの位置データおよび実体の位置データから距離を求め、ロボットと各実体とが所定距離より離れているかを判定する。たとえば、第１関連付手段（２０，Ｓ２０９）は、ロボットとの第１速度差が第１所定速度以下であり、かつロボットとの距離が所定距離以下の実体が複数ある場合、ロボットに最も近い実体とロボットとを関連付ける。
【００２９】
第６の発明によれば、関連付けが解除されて間もなければ、ロボットと実体とを容易に関連付け直すことができる。
【００３０】
第７の発明は、第５の発明または第６の発明に従属し、空間には、少なくとも２つ以上の実体が存在し、関連付手段は、ロボットと実体との関連付けが解除されてから所定時間が経過したとき、ロボットと各実体との第２速度差を算出する第２速度差算出手段、第２速度差算出手段によって算出された第２速度差が第２所定速度より大きいかを判定する第２速度判定手段、および第２速度判定手段によってロボットの第２速度差が第２所定速度以下と判定された実体が１つだけであるとき、その実体とロボットとを関連付ける第２関連付手段を含む。
【００３１】
第７の発明では、第６の発明と同様、空間には４つの実体が存在することがある。たとえば、第２速度差算出手段（２０，Ｓ２３３）は、関連付けが解除されてから所定時間が経過していれば、３００秒分のロボットの位置データおよび３００秒分の実体の位置データから、第２速度差（ＲＭＳ速度差）を算出する。第２速度判定手段（２０，Ｓ２３５）は、第２速度差が第２所定速度より大きいかを判定する。たとえば、第２関連付手段（２０，Ｓ２４７）は、ロボットとの第２速度差が第２所定速度以下の実体が１つだけであれば、その実体とロボットとを関連付ける。
【００３２】
第７の発明によれば、関連付けが解除されて所定時間が経過したとしても、ロボットと実体とを関連付け直すことができる。
【発明の効果】
【００３３】
この発明によれば、容易に設置可能な環境センサおよびロボットが一般的に出力可能な状態データを利用して、ロボットの位置を検出することができるため、容易にロボットを自己位置同定することができる。
【００３４】
この発明の上述の目的、その他の目的、特徴および利点は、図面を参照して行う以下の実施例の詳細な説明から一層明らかとなろう。
【図面の簡単な説明】
【００３５】
【図１】図１はこの発明の一実施例のロボット自己位置同定システムの概要を示す図解図である。
【図２】図２は図１に示す追跡サーバの電気的な構成の一例を示す図解図である。
【図３】図３は図１に示すロボットの外観を正面から見た図解図である。
【図４】図４は図１に示すロボットの電気的な構成の一例を示すブロック図である。
【図５】図５は図１および図２に示すＬＲＦの計測領域を示す図解図である。
【図６】図６は図１および図２に示すＬＲＦを利用して取得された実体の移動軌跡の一例を示す図解図である。
【図７】図７は図２に示すメモリに記憶された実体の位置履歴データの構成の一例を示す図解図である。
【図８】図８は図２に示すメモリに記憶されたロボットの状態履歴データの構成の一例を示す図解図である。
【図９】図９は図１に示す追跡サーバのメモリに記憶される、ＲＴＭ（Robot Tracking Model）データの一例を示す図解図である。
【図１０】図１０は図６に示す実体の運動履歴および図１に示すロボットの運動履歴の変化を示すグラフである。
【図１１】図１１は図１０に示す実体の運動履歴およびロボットの運動履歴のＲＭＳ誤差のグラフである。
【図１２】図１２は図２に示すメモリに一時記憶される追跡実体リストの構成の一例を示す図解図である。
【図１３】図１３は図２に示すメモリに記憶される関連付テーブルの構成の一例を示す図解図である。
【図１４】図１４は図１に示すプロセッサによって実行される角度修正処理で利用される各数値の関係を示す図解図である。
【図１５】図１５は図１に示す人間とロボットとの関連付けおよび関連付け解除の一例を示す図解図である。
【図１６】図１６は図２に示すメモリのメモリマップの一例を示す図解図である。
【図１７】図１７は図１６に示すデータ記憶領域の一例を示す図解図である。
【図１８】図１８は図１に示す追跡サーバのプロセッサの通信処理の一部を示すフロー図である。
【図１９】図１９は図１に示す追跡サーバのプロセッサの通信処理の一部であって、図１８に後続するフロー図である。
【図２０】図２０は図１に示す追跡サーバのプロセッサの補正処理を示すフロー図である。
【図２１】図２１は図１に示す追跡サーバのプロセッサの追跡処理の一部を示すフロー図である。
【図２２】図２２は図１に示す追跡サーバのプロセッサの追跡処理の一部であって、図２１に後続するフロー図である。
【図２３】図２３は図１に示す追跡サーバのプロセッサの関連付解除処理を示すフロー図である。
【図２４】図２４は図１に示す追跡サーバのプロセッサの関連付処理を示すフロー図である。
【図２５】図２５は図１に示す追跡サーバのプロセッサのローカル関連付処理を示すフロー図である。
【図２６】図２６は図１に示す追跡サーバのプロセッサのグローバル関連付処理を示すフロー図である。
【図２７】図２７は図１に示す追跡サーバのプロセッサの角度補正処理を示すフロー図である。
【発明を実施するための形態】
【００３６】
図１を参照して、この実施例のロボット自己位置同定システム１００は、たとえば自律移動型のロボット（以下、単に「ロボット」と言う。）１８ａ，１８ｂ，１８ｃを、それらのロボットが存在する空間内で自己位置同定（Localization（ローカライゼーション）と言うこともある。）するためのシステムである。なお、本実施例で言う「自己位置同定」とは、ロボット１８ａ，１８ｂ，１８ｃ（以下、区別しない場合には「ロボット１８」と言う。）が、その空間内における自身の位置、角度および速度などのデータを、自身のセンサおよび環境センサなどを利用して取得することを意味する。
【００３７】
ロボット自己位置同定システム１００は、追跡サーバ１０、遠隔操作装置１４、計画サーバ１６およびロボット１８から構成されている。そして、ロボット１８は、ネットワーク４００を介して、追跡サーバ１０、遠隔操作装置１４および計画サーバ１６に接続される。
【００３８】
追跡サーバ１０は、環境センサである、ＬＲＦ１２ａ，１２ｂを含む複数のＬＲＦを備える。ＬＲＦ１２ａ，１２ｂは人間およびロボット１８が行動できる場所（環境）に設置される。そして、追跡サーバ１０はＬＲＦ１２ａ，１２ｂを利用して、ロボット１８および人間を含む実体をセンシングすることで、各実体の位置を検出する。たとえば、人間およびロボット１８が行動できる場所とは、会社のフロア、博物館、ショッピングモールまたはアトラクション会場などである。なお、システムの管理者は、様々な場所において、ＬＲＦ１２ａ，１２ｂを、容易に設置することができる。また、追跡サーバ１０によって得られた位置データは、遠隔操作サーバ１４および計画サーバ１６に出力される。
【００３９】
さらに、追跡サーバ１０は、ローカライゼーション機能（ローカライゼーションモジュールと言うこともある。）を備えている。そのため、追跡サーバ１０は、ロボット１８との通信を確立し、ロボット１８が出力する状態データを受信（取得）するとともに、ロボット１８に対して位置および角度（向き）を補正するための補正データを送信する。状態データは、ロボット１８自身の位置データ、角度データおよび速度データを含み、ロボット１８が備えるセンサの出力に基づいて作成される。
【００４０】
遠隔操作装置１４は、追跡サーバ１０から得た位置データを表示する操作端末を含み、オペレータによって操作される。また、オペレータは、操作端末に表示されるロボット１８や人間の位置に基づいて、同時に複数のロボット１８ａ，１８ｂ，１８ｃを監視することができる。そして、オペレータは、必要に応じてロボット１８を遠隔操作したり、動作命令を発行したりする。また、計画サーバ１６は、追跡サーバ１０から得た位置データを利用して、ロボット１８が空間内に存在する人間とのコミュニケーション行動を行うように、経路を計算する。
【００４１】
ロボット１８は、相互作用指向のロボットでもあり、人間のようなコミュニケーションの対象との間で、身振り手振りのような身体動作および音声の少なくとも一方を含むコミュニケーション行動を実行する機能を備えている。そのため、ロボット１８は、遠隔操作装置１４または計画サーバ１６の命令に基づいて、人間に接近したり、コミュニケーション行動を行ったりする。
【００４２】
また、ロボット１８は、先述した状態データを０．１秒〜０．３秒毎に出力する。さらに、各ロボット１８は、文字列で構成されるロボットＩＤでそれぞれが識別（区別）される。
【００４３】
なお、ここでは簡単のため人間を１人、ロボット１８を３台しか示していないが、追跡サーバ１０は２人以上の人間および３台以上のロボット１８を同時に検出することができる。また、追跡サーバ１０、遠隔操作装置１４および計画サーバ１６と、ネットワーク４００との接続は無線接続であってもよいし、有線接続であってもよい。さらに、各サーバおよび装置間の接続も、有線接続であってもよいし、無線接続であってもよい。
【００４４】
図２は、追跡サーバ１０の電気的な構成を示すブロック図である。図２を参照して、追跡サーバ１０は、ＬＲＦ１２ａ−１２ｆおよびプロセッサ２０を含む。プロセッサ２０は、ＲＴＣ(Real Time Clock)２０ａを含んでおり、検出した位置データに対して、ＲＴＣ２０ａから得られた時刻データを対応付けて、メモリ２２のバッファに記憶する。
【００４５】
プロセッサ２０は、マイクロコンピュータ或いはCPUとも呼ばれ、先述したＬＲＦ１２ａおよびＬＲＦ１２ｂに加えて、ＬＲＦ１２ｃ，ＬＲＦ１２ｄ，ＬＲＦ１２ｅおよびＬＲＦ１２ｆともそれぞれ接続される。さらに、プロセッサ２０は、メモリ２２、通信ＬＡＮボード２４、無線通信装置２６および外部出力２８とも接続される。なお、ＬＲＦ１２ａ−１２ｆを区別する必要がない場合には、まとめて「ＬＲＦ１２」と言う。
【００４６】
ＬＲＦ１２は、レーザーを照射し、物体（実体も含む）に反射して戻ってくるまでの時間から当該物体との距離を計測するものである。たとえば、トランスミッタ（図示せず）から照射したレーザーを回転ミラー（図示せず）で反射させて、前方を扇状に一定角度（たとえば、０．５度）ずつスキャンする。ここで、ＬＲＦ１２としては、SICK社製のレーザーレンジファインダ（型式  LMS200）を用いることができる。このレーザーレンジファインダを用いた場合には、距離８mを±１５mm程度の誤差で計測可能である。
【００４７】
メモリ２２は、図示は省略をするが、ROM，HDDおよびRAMを含み、ROMおよびHDDには、追跡サーバ１０の動作を制御するための制御プログラムが予め記憶される。たとえば、ＬＲＦ１２による人間の検出に必要なプログラムなどが記録される。また、RAMは、ワークメモリやバッファメモリとして用いられる。
【００４８】
通信ＬＡＮボード２４は、たとえばDSPで構成され、プロセッサ２０から与えられた送信データを無線通信装置２６に与え、無線通信装置２６は送信データを、ネットワーク４００を介してロボット１８に送信する。また、送信データは補正データなどである。一方、通信ＬＡＮボード２４は、無線通信装置２６を介してデータを受信し、受信したデータをプロセッサ２０に与える。また、受信データはロボット１８が出力する状態データなどである。なお、受信した状態データは、メモリ２２のバッファに記憶される。
【００４９】
外部接続２８は、遠隔操作装置１４および計画サーバ１６と接続するためのポートであり、たとえばＬＡＮケーブル、SCSIケーブルまたはUSBケーブルなどの有線ケーブルが接続される。そして、プロセッサ２０は、実体の位置データを外部出力２８に与え、外部出力２８は位置データを遠隔操作装置１４および計画サーバ１６に出力する。
【００５０】
図３はこの実施例のロボット１８の外観を示す正面図である。図３を参照して、ロボット１８は台車３０を含み、台車３０の下面にはロボット１８を自律移動させる右車輪３２ａ、左車輪３２ｂ（特に区別しない場合には、「車輪３２」と言う）および１つの従輪３４が設けられる。右車輪３２ａおよび左車輪３２ｂは、右車輪モータ３６ａおよび左車輪モータ３６ｂ（図４参照）によってそれぞれ独立に駆動され、台車３０すなわちロボット１８を前後左右の任意方向に動かすことができる。また、従輪３４は車輪３２を補助する補助輪である。したがって、ロボット１８は、配置された空間内を自律制御によって移動可能である。
【００５１】
台車３０の上には、円柱形のセンサ取り付けパネル３８が設けられ、このセンサ取り付けパネル３８には、多数の赤外線距離センサ４０が取り付けられる。これらの赤外線距離センサ４０は、センサ取り付けパネル３８すなわちロボット１８の周囲の物体（人間や障害物など）との距離を測定するものである。
【００５２】
なお、この実施例では、距離センサとして、赤外線距離センサを用いるようにしてあるが、赤外線距離センサに代えて、小型のＬＲＦや、超音波距離センサおよびミリ波レーダなどを用いることもできる。
【００５３】
センサ取り付けパネル３８の上には、胴体４２が直立するように設けられる。また、胴体４２の前方中央上部（人の胸に相当する位置）には、上述した赤外線距離センサ４０がさらに設けられ、ロボット１８の前方の主として人間との距離を計測する。また、胴体４２には、その側面側上端部のほぼ中央から伸びる支柱４４が設けられ、支柱４４の上には、全方位カメラ４６が設けられる。全方位カメラ４６は、ロボット１８の周囲を撮影するものであり、後述する眼カメラ７０とは区別される。この全方位カメラ４６としては、たとえばCCDやCMOSのような固体撮像素子を用いるカメラを採用することができる。なお、これら赤外線距離センサ４０および全方位カメラ４６の設置位置は、当該部位に限定されず適宜変更されてもよい。
【００５４】
胴体４２の両側面上端部（人の肩に相当する位置）には、それぞれ、肩関節４８Ｒおよび肩関節４８Ｌによって、上腕５０Ｒおよび上腕５０Ｌが設けられる。図示は省略するが、肩関節４８Ｒおよび肩関節４８Ｌは、それぞれ、直交する３軸の自由度を有する。すなわち、肩関節４８Ｒは、直交する３軸のそれぞれの軸廻りにおいて上腕５０Ｒの角度を制御できる。肩関節４８Ｒの或る軸（ヨー軸）は、上腕５０Ｒの長手方向（または軸）に平行な軸であり、他の２軸（ピッチ軸およびロール軸）は、その軸にそれぞれ異なる方向から直交する軸である。同様にして、肩関節４８Ｌは、直交する３軸のそれぞれの軸廻りにおいて上腕５０Ｌの角度を制御できる。肩関節４８Ｌの或る軸（ヨー軸）は、上腕５０Ｌの長手方向（または軸）に平行な軸であり、他の２軸（ピッチ軸およびロール軸）は、その軸にそれぞれ異なる方向から直交する軸である。
【００５５】
また、上腕５０Ｒおよび上腕５０Ｌのそれぞれの先端には、肘関節５２Ｒおよび肘関節５２Ｌが設けられる。図示は省略するが、肘関節５２Ｒおよび肘関節５２Ｌは、それぞれ１軸の自由度を有し、この軸（ピッチ軸）の軸回りにおいて前腕５４Ｒおよび前腕５４Ｌの角度を制御できる。
【００５６】
前腕５４Ｒおよび前腕５４Ｌのそれぞれの先端には、人の手に相当する球体５６Ｒおよび球体５６Ｌがそれぞれ固定的に設けられる。ただし、指や掌の機能が必要な場合には、人間の手の形をした「手」を用いることも可能である。また、図示は省略するが、台車３０の前面、肩関節４８Ｒと肩関節４８Ｌとを含む肩に相当する部位、上腕５０Ｒ、上腕５０Ｌ、前腕５４Ｒ、前腕５４Ｌ、球体５６Ｒおよび球体５６Ｌには、それぞれ、接触センサ５８（図４で包括的に示す）が設けられる。台車３０の前面の接触センサ５８は、台車３０への人間や他の障害物の接触を検知する。したがって、ロボット１８は、その自身の移動中に障害物との接触が有ると、それを検知し、直ちに車輪３２の駆動を停止してロボット１８の移動を急停止させることができる。また、その他の接触センサ５８は、当該各部位に触れたかどうかを検知する。なお、接触センサ５８の設置位置は、当該部位に限定されず、適宜な位置（人の胸、腹、脇、背中および腰に相当する位置）に設けられてもよい。
【００５７】
胴体４２の中央上部（人の首に相当する位置）には首関節６０が設けられ、さらにその上には頭部６２が設けられる。図示は省略するが、首関節６０は、３軸の自由度を有し、３軸の各軸廻りに角度制御可能である。或る軸（ヨー軸）はロボット１８の真上（鉛直上向き）に向かう軸であり、他の２軸（ピッチ軸、ロール軸）は、それぞれ、それと異なる方向で直交する軸である。
【００５８】
頭部６２には、人の口に相当する位置に、スピーカ６４が設けられる。スピーカ６４は、ロボット１８が、それの周辺の人間に対して音声ないし音によってコミュニケーションを取るために用いられる。また、人の耳に相当する位置には、マイク６６Ｒおよびマイク６６Ｌが設けられる。以下、右のマイク６６Ｒと左のマイク６６Ｌとをまとめてマイク６６と言うことがある。マイク６６は、周囲の音、とりわけコミュニケーションを実行する対象である人間の音声を取り込む。さらに、人の目に相当する位置には、眼球部６８Ｒおよび眼球部６８Ｌが設けられる。眼球部６８Ｒおよび眼球部６８Ｌは、それぞれ眼カメラ７０Ｒおよび眼カメラ７０Ｌを含む。以下、右の眼球部６８Ｒと左の眼球部６８Ｌとをまとめて眼球部６８と言うことがある。また、右の眼カメラ７０Ｒと左の眼カメラ７０Ｌとをまとめて眼カメラ７０と言うことがある。
【００５９】
眼カメラ７０は、ロボット１８に接近した人間の顔や他の部分ないし物体などを撮影して、それに対応する映像信号を取り込む。また、眼カメラ７０は、上述した全方位カメラ４６と同様のカメラを用いることができる。たとえば、眼カメラ７０は、眼球部６８内に固定され、眼球部６８は、眼球支持部（図示せず）を介して頭部６２内の所定位置に取り付けられる。図示は省略するが、眼球支持部は、２軸の自由度を有し、それらの各軸廻りに角度制御可能である。たとえば、この２軸の一方は、頭部６２の上に向かう方向の軸（ヨー軸）であり、他方は、一方の軸に直交しかつ頭部６２の正面側（顔）が向く方向に直行する方向の軸（ピッチ軸）である。眼球支持部がこの２軸の各軸廻りに回転されることによって、眼球部６８ないし眼カメラ７０の先端（正面）側が変位され、カメラ軸すなわち視線方向が移動される。なお、上述のスピーカ６４、マイク６６および眼カメラ７０の設置位置は、当該部位に限定されず、適宜な位置に設けられてよい。
【００６０】
このように、この実施例のロボット１８は、車輪３２の独立２軸駆動、肩関節４８の３自由度（左右で６自由度）、肘関節５２の１自由度（左右で２自由度）、首関節６０の３自由度および眼球支持部の２自由度（左右で４自由度）の合計１７自由度を有する。
【００６１】
図４はロボット１８の電気的な構成を示すブロック図である。この図４を参照して、ロボット１８は、プロセッサ８０を含む。プロセッサ８０は、マイクロコンピュータ或いはCPUとも呼ばれ、バス８２を介して、メモリ８４、モータ制御ボード８６、センサ入力／出力ボード８８および音声入力／出力ボード９０に接続される。プロセッサ８０は、ＲＴＣ８０ａを含んでおり、作成した状態データに対して時刻データを対応付ける。
【００６２】
メモリ８４は、図示は省略をするが、SSD(Solid State Drive),ROMおよびRAMを含む。SSD,ROMには、ロボット１８の動作を制御するための制御プログラムが予め記憶される。たとえば、各センサの出力（センサ情報）を検知するための検知プログラムや、外部コンピュータ（追跡サーバ１０、遠隔操作装置１４および計画サーバ１６）との間で必要なデータやコマンドを送受信するための通信プログラムなどが記録される。また、RAMは、ワークメモリやバッファメモリとして用いられる。
【００６３】
モータ制御ボード８６は、たとえばDSPで構成され、各腕や首関節および眼球部などの各軸モータの駆動を制御する。すなわち、モータ制御ボード８６は、プロセッサ８０からの制御データを受け、右眼球部６８Ｒの２軸のそれぞれの角度を制御する２つのモータ（図４では、まとめて「右眼球モータ９２」と示す。）の回転角度を制御する。同様に、モータ制御ボード８６は、プロセッサ８０からの制御データを受け、左眼球部６８Ｌの２軸のそれぞれの角度を制御する２つのモータ（図４では、まとめて「左眼球モータ９４」と示す。）の回転角度を制御する。
【００６４】
また、モータ制御ボード８６は、プロセッサ８０からの制御データを受け、肩関節４８Ｒの直交する３軸のそれぞれの角度を制御する３つのモータと肘関節５２Ｒの角度を制御する１つのモータとの計４つのモータ（図４では、まとめて「右腕モータ９６」と示す。）の回転角度を制御する。同様に、モータ制御ボード８６は、プロセッサ８０からの制御データを受け、肩関節４８Ｌの直交する３軸のそれぞれの角度を制御する３つのモータと肘関節５２Ｌの角度を制御する１つのモータとの計４つのモータ（図４では、まとめて「左腕モータ９８」と示す。）の回転角度を制御する。
【００６５】
さらに、モータ制御ボード８６は、プロセッサ８０からの制御データを受け、首関節６０の直交する３軸のそれぞれの角度を制御する３つのモータ（図４では、まとめて「頭部モータ１１０」と示す。）の回転角度を制御する。そして、モータ制御ボード８６は、プロセッサ８０からの制御データを受け、右車輪３２ａを駆動する右車輪モータ３６ａ、左車輪３２ｂを駆動する左車輪モータ３６ｂの回転角度を、それぞれ個別に制御する。なお、この実施例では、車輪モータ３６を除くモータは、制御を簡素化するためにステッピングモータ（すなわち、パルスモータ）を用いる。ただし、車輪モータ３６と同様に直流モータを用いるようにしてもよい。また、ロボット１８の身体部位を駆動するアクチュエータは、電流を動力源とするモータに限らず適宜変更されてもよい。たとえば、他の実施例では、エアアクチュエータなどが適用されてもよい。
【００６６】
センサ入力／出力ボード８８は、モータ制御ボード８６と同様に、DSPで構成され、各センサからの信号を取り込んでプロセッサ８０に与える。すなわち、赤外線距離センサ４０のそれぞれからの反射時間に関するデータがこのセンサ入力／出力ボード８８を通じてプロセッサ８０に入力される。また、全方位カメラ４６からの映像信号が、必要に応じてセンサ入力／出力ボード８８で所定の処理を施してからプロセッサ８０に入力される。眼カメラ７０からの映像信号も、同様に、プロセッサ８０に入力される。また、上述した複数の接触センサ５８（図４では、まとめて「接触センサ５８」と示す。）からの信号がセンサ入力／出力ボード８８を介してプロセッサ８０に与えられる。
【００６７】
また、センサ入力／出力ボード８８には、右車輪３２ａの回転速度（車輪速）を検出する右車輪速センサ１１２ａ、左車輪３２ｂの回転速度を検出する左車輪速センサ１１２ｂがさらに接続される。そして、右車輪速センサ１１２ａおよび左車輪速センサ１１２ｂからのそれぞれから、一定時間における右車輪３２ａおよび左車輪３２ｂの回転数（車輪速）に関するデータが、センサ入力／出力ボート８８を通じて、プロセッサ８０に入力される。
【００６８】
音声入力／出力ボード９０も、他の入力／出力ボードと同様にDSPで構成され、プロセッサ８０から与えられる音声合成データに従った音声または声がスピーカ６４から出力される。また、マイク６６からの音声入力が、音声入力／出力ボード９０を介してプロセッサ８０に与えられる。
【００６９】
また、プロセッサ８０は、バス８２を介して通信ＬＡＮボード１１４に接続される。通信ＬＡＮボード１１４は、たとえばDSPで構成され、プロセッサ８０から与えられた送信データを無線通信装置１１６に与え、無線通信装置１１６は送信データを、ネットワーク４００を介して外部コンピュータ（追跡サーバ１０、遠隔操作装置１４、計画サーバ１６）に送信する。また、通信ＬＡＮボード１１４は、無線通信装置１１６を介してデータを受信し、受信したデータをプロセッサ８０に与える。たとえば、送信データとしては、全方位カメラ４６および目カメラ７０によって撮影された周囲の映像データや、状態データであったりする。
【００７０】
ここで、状態データに含まれる位置データは、右車輪速センサ１１２ａおよび左車輪速センサ１１２ｂが出力する車輪３２の回転数に基づいて求められる。また、状態データに含まれる角度データは、右車輪速センサ１１２ａおよび左車輪速度センサ１１２ｂの回転比率に基づいて求められる。さらに、状態データに含まれる速度データは、右車輪速センサ１１２ａが出力する車輪速データおよび左車輪速センサ１１２ｂが出力する車輪速データから構成される。つまり、ロボット１８は、状態データに含まれる速度データとして、右車輪３２ａの車輪速データおよび左車輪３２ｂの車輪速データを追跡サーバ１０に送信する。そして、追跡サーバ１０では、受信した左右の車輪速データに基づいて、ロボット１８の速度を算出する。
【００７１】
次にＬＲＦ１２について詳細に説明する。図５を参照して、ＬＲＦ１２の計測範囲は、半径Ｒ（Ｒ≒８m）の半円形状（扇形）で示される。つまり、ＬＲＦ１２は、その正面方向を中心とした場合に、左右９０°の方向を所定の距離（Ｒ）以内で計測可能である。
【００７２】
また、使用しているレーザーは、日本工業規格  JIS C 6802「レーザー製品の安全基準」におけるクラス１レーザーであり、人の眼に対して影響を及ぼさない安全なレベルである。また、この実施例では、ＬＲＦ１２のサンプリングレートを３７Hzとした。これは、移動したり、停止したりする実体の位置を連続して検出するためである。
【００７３】
さらに、先述したように、ＬＲＦ１２は、様々な場所に配置される。具体的には、ＬＲＦ１２ａ−１２ｆの各々は、計測領域が重なるように配置され、図示は省略するが、床面から約９０cmの高さに固定される。この高さは、人間の胴体と腕（両腕）とを検出可能とするためであり、たとえば、日本人の成人の平均身長から算出される。したがって、遠隔操作装置１４を設ける場所（地域ないし国）や人間の年齢ないし年代（たとえば、子供，大人）に応じて、ＬＲＦ１２を固定する高さを適宜変更するようにしてよい。なお、本実施例では、設定されるＬＲＦ１２は６台としたが、２台以上であれば、任意の台数のＬＲＦ１２を設置してもよい。
【００７４】
このような構成の追跡サーバ１０では、プロセッサ２０がＬＲＦ１２からの出力（距離データ）に基づいて、パーティクルフィルタを用いて、人間およびロボット１８の現在位置の変化を推定する。なお、本実施例では、人間とロボット１８とでは速度や形状が異なるため、人形状のモデル（人形状モデル）またはロボット形状のモデル（ロボット形状モデル）を採用して、パーティクルフィルタの尤度の計算量を軽減している。
【００７５】
たとえば、ＬＲＦ１２によってスキャンされると、人間などの実体が存在しない可視領域（ｏｐｅｎ）、実体が存在する陰領域（ｓｈａｄｏｗ）および実体のエッジ（ｅｄｇｅ）が検出される。
【００７６】
ここで、空間に均等にばら撒くパーティクルを識別する変数をｍとし、パーティクルの状態（位置と速度）をＸとし、或る時刻ｔの状態Ｘｔのときに観測される観測値ベクトルをＺｔとしたとき、各ＲＬＦ１２の尤度ｐｉ（Ｚｔ｜Ｘｔ[ｍ]）は数１に従って算出される。ただし、[ｍ]はパーティクルを個別に識別するための添え字である。また、表現の都合上、数式以外では[ｍ]を上付きにしないで記載する。以下、同様である。
【００７７】
[数１]
0005493097.tif 000002
【００７８】
つまり、尤度ｐｉ（Ｚｔ｜Ｘｔ[ｍ]）は、可視領域では一定値（ｐｏｐｅｎ）とし、陰領域では一定値（ｐｓｈａｄｏｗ）とエッジの尤度ｐｅｄｇｅ（Ｚｔ｜Ｘｔ[ｍ]）との和となる。
【００７９】
さらに、ＬＲＦ１２の数をｎｓｅｎｓｏｒｓとしたとき、数１で求めた各ＬＲＦ１２の尤度を統合した統合尤度ｐ（Ｚｔ｜Ｘｔ[ｍ]）は、数２に従って算出される。なお、陰領域に２人の人間が存在することがあるので、２つのパーティクルが同じ実体を追跡しないようにするために、各ＲＬＦ１２の尤度ｐｉ（Ｚｔ｜Ｘｔ[ｍ]）から補正値Ｐｃｏｌｌｏｃａｔｉｏｎが減算される。
【００８０】
[数２]
0005493097.tif 000003
【００８１】
このようにして求められた統合尤度ｐ（Ｚｔ｜Ｘｔ[ｍ]）に基づいて、各パーティクルが更新され、人間またはロボット１８の現在位置の変化が推定される。そして、推定された現在位置の変化に基づいて、ロボット１８または人間の位置データを求め、その位置データがバッファに記憶される。また、本実施例では、バッファに記憶された全ての位置データをまとめて「位置履歴データ」と言う。
【００８２】
なお、パーティクルフィルタを利用した人物追跡については、特開２００８−６１０５号公報に詳細が開示されている。
【００８３】
図６は、ＬＲＦ１２ａ−１２ｆが設置された或る環境の地図を示す図解図である。図６を参照して、地図が表す場所は或るショッピングモールである。ＬＲＦ１２ａ，１２ｃ，１２ｄの３台は地図の上側の位置に対応して設置されており、ＬＲＦ１２ｂ，１２ｅ，１２ｆの３台は地図の下側の位置に対応して設定されている。そして、２台以上のＬＲＦ１２の計測領域が重なる領域は、検出領域Ｆとして示され、図６では斜線の領域である。なお、検出領域Ｆにおける位置は、図６における左下を原点とする平面座標系で示される。
【００８４】
検出領域Ｆ（空間）内では、ロボット１８ａ、ロボット１８ｂ、ロボット１８ｃおよび人間は、実体Ｅａ、実体Ｅｂ、実体Ｅｃおよび実体Ｅｄ（区別しない場合は「実体Ｅ」と言う。）として検出される。また、図６では、各実体Ｅに対応する位置履歴データに基づいて、移動軌跡Ｍを示す。つまり、実体Ｅａの位置履歴データは移動軌跡Ｍａで示され、実体Ｅｂの位置履歴データは移動軌跡Ｍｂで示され、実体Ｅｃの位置履歴データは移動軌跡Ｍｃで示され、実体Ｅｄの位置履歴データは移動軌跡Ｍｄで示される。
【００８５】
図７は追跡サーバ１０によって検出された複数の位置データ、つまり位置履歴データを示す図解図である。図７を参照して、各位置データは、位置を検出した時刻（Ｔ）および検出領域Ｆの平面座標を示す数値（Ｘ，Ｙ）から構成される。そして、各位置履歴データは、実体Ｅ毎に分けて記憶される。たとえば、追跡サーバ１０のメモリ２２のバッファには、実体Ｅａの位置履歴データ、実体Ｅｂの位置履歴データ、実体Ｅｃの位置履歴データおよび実体Ｅｄの位置履歴データが記憶される。
【００８６】
図８は追跡サーバ１０が受信した複数の状態データ、つまり状態履歴データを示す図解図である。図８を参照して、各状態データは、受信した時刻（ｔ）、検出領域Ｆにおける平面座標を示す数値（ｘ，ｙ）、ロボット１８の向きを示す角度（ｄ）、ロボット１８の右車輪３２ａの速度（ｒｖ）およびロボット１８の左車輪３２ｂの速度（ｌｖ）から構成される。そして、位置履歴データと同様に、各状態履歴データは、ロボット１８毎に分けて記憶される。たとえば、メモリ２２のバッファには、ロボット１８ａの状態履歴データ、ロボット１８ｂの状態履歴データおよびロボット１８ｃの状態履歴データが記憶される。
【００８７】
なお、位置データおよび状態データは、一定時間（たとえば、３６０秒）が経つと、古いデータから削除（上書き）される。つまり、位置履歴データおよび状態履歴データがメモリ２２のメモリ容量を圧迫することは無い。
【００８８】
また、上記した平面座標を示す数値（Ｘ，Ｙ）および（ｘ，ｙ）において、１座標の変化は１cmの距離変化に対応する。たとえば、実体Ｅの位置データが（Ｘ，Ｙ）である場合に、実体Ｅが右に１cm移動すると、実体Ｅの座標が（Ｘ＋１，Ｙ）に変化する。
【００８９】
図９は、各ロボット１８のＲＴＭから構成されたＲＴＭデータの図解図である。この実施例では、追跡サーバ１０が状態データを受信するために各ロボット１８との通信を確立すると、ロボット１８毎にＲＴＭを作成する。そして、各ロボット１８に対して送信される補正データがこのＲＴＭに基づいて決められる。そのため、各ＲＴＭは現在のロボット１８の位置、角度および速度の値を含む。
【００９０】
図９を参照して、ＲＭＴデータは、各ロボット１８に割り振られたロボットＩＤに対応づけて記憶される。たとえば、ロボットＩＤが「００１」であるロボット１８ａのＲＴＭは、（ＲＸ１，ＲＹ１）が位置を示し、ＲＤ１が角度を示し、ＲＶ１が速度を示す。
【００９１】
ここで、追跡サーバ１０がロボット１８から状態データを受信すると、そのロボット１８に対応するＲＴＭの位置、角度および速度が更新される。たとえば、ロボット１８ａから受信した状態データにおいて、位置が（ｘ１，ｙ１）、角度がｄ１、速度がｒｖ１，ｌｖ１であれば、ロボットＩＤ「００１」のＲＴＭにおいて、（ＲＸ１，ＲＹ１），ＲＤ１，ＲＶ１が、受信した状態データの数値に更新される。なお、速度ＲＶ１は、ｒｖ１およびｒｖ１に基づいて算出された速度に更新される。
【００９２】
ところが、ロボット１８の車輪３２は、ロボット１８が停止したり、曲がったりする度に、地面に対して僅かに滑るため、車輪３２の回転から求める位置および角度と、実際の位置および角度とに誤差が生じる。そして、ロボット１８が移動するにつれて、その誤差は増大するため、ロボット１８が移動するにつれて、状態データに含まれる位置データおよび角度データの信頼性は低くなる。そのため、ロボット１８が出力する状態データだけに基づいて、そのロボット１８に対するＲＴＭを更新すると、ＲＴＭが正しいロボット１８の状態（位置、角度および速度）を示せなくなってしまう。
【００９３】
そこで、本実施例では、検出領域Ｆ内で検出された実体Ｅと検出領域Ｆ内を移動するロボット１８とを関連付け、ＬＲＦ１２を利用して検出された実体Ｅの位置データおよび関連付けられたロボット１８の状態データに基づいてＲＴＭを更新する。これにより、各ＲＴＭは、ロボット１８の正しい状態を示すことができるようになる。
【００９４】
まず、ロボット１８と実体Ｅとの関連付けについて説明する。本願発明では、実体Ｅおよびロボット１８の１秒毎の移動距離、つまり速度に着目する。なお、各実体Ｅおよびロボット１８ａの速度は、三平方の定理に基づいて、或る時刻の座標と１秒後の座標とから距離（mm）を求めることで、速度（mm/s）を求めることができる。
【００９５】
図１０の運動履歴グラフは、縦軸に距離（mm）をとり、横軸に時間（秒：s）をとるグラフである。そして、運動履歴グラフは、実体Ｅａ−実体Ｅｄの１５秒間の速度の変化と、ロボット１８ａの速度の変化とを示す。この運動履歴グラフにおいて、実体Ｅａ、実体Ｅｂ、実体Ｅｃおよび実体Ｅｄの速度に対応するグラフは、実線Ｌａ、実線Ｌｂ、実線Ｌｃおよび実線Ｌｄで示される。また、ロボット１８ａの速度に対応するグラフは点線ＤＬで示される。
【００９６】
たとえば、運動履歴グラフからは、点線ＤＬ（ロボット１８ａの速度）の変化は、実線Ｌａ（実体Ｅａの速度）、実線Ｌｃ（実体Ｅａの速度）および実線Ｌｄ（実体Ｅａの速度）の変化とは異なっているが、実線Ｌｂ（実体Ｅｂの速度）の変化とは類似していることが分かる。
【００９７】
そこで、ロボット１８ａの速度と各実体Ｅの速度とのＲＭＳ（Root Mean Square：二乗平均）誤差を、以下の数３に示す式に基づいて求めると、図１１に示すＲＭＳ誤差グラフを求めることができる。なお、本実施例におけるＲＭＳ誤差は、ＲＭＳ速度差と言う場合もある。
【００９８】
[数３]
0005493097.tif 000004
【００９９】
図１１を参照して、ＲＭＳ誤差グラフは、縦軸がＲＭＳ誤差、横軸が実体ＩＤのグラフであり、各ＲＭＳ誤差は数３のＲＴを１５（秒）として算出されている。まず、ＲＭＳ誤差グラフにおいて、実体Ｅａ、実体Ｅｃおよび実体ＥｄのＲＭＳ誤差に着目すると、実体ＩＤが「００１」の実体Ｅａとロボット１８ａとのＲＭＳ誤差は３３０mmである。また、実体ＩＤが「００３」の実体Ｅｃとロボット１８ａとのＲＭＳ誤差は５００mmであり、実体ＩＤが「００４」の実体Ｅｄとロボット１８ａとのＲＭＳ誤差は６００mmである。これに対して、実体ＩＤが「００２」の実体Ｅｂとロボット１８ａとのＲＭＳ誤差は３０mmである。つまり、速度の変化が類似するロボット１８ａと実体ＥｂとのＲＭＳ誤差は、他の実体Ｅａ，Ｅｃ，ＥｄとのＲＭＳ誤差に比べて小さいことが分かる。
【０１００】
そのため、本実施例では、或るロボット１８に対して、ＲＭＳ誤差、つまりＲＭＳ速度差（第１速度差）が３５０mm/s（第１所定速度）以下であり、かつ現在の速度差が３５０mm/s以下の実体Ｅが１つだけであれば、或るロボット１８とその実体Ｅとを関連付ける。
【０１０１】
たとえば、図１１を参照して、ロボット１８ａにおいて、ＲＭＳ速度差が３５０mm/s以下の実体Ｅは、実体Ｅａおよび実体Ｅｂである。さらに、図１０を参照して、１５秒の時点を現在の速度として考えると、現在の速度差が３５０mm/s以下の実体Ｅは、実体Ｅｂだけである。そのため、このような場合にはロボット１８ａと実体Ｅｂとが関連付けられる。そして、プロセッサ２０は、各ロボット１８に対して上述した判断を行うことで、他のロボット１８と他の実体Ｅとを関連付ける。
【０１０２】
また、プロセッサ２０が上述した判断を実行する際には、図１２に示す追跡実体リストを用いる。図１２を参照して、追跡実体リストは、実体ＩＤおよびフラグの列から構成されている。そして、フラグの列には、先述した条件を満たしていれば「１」が設定され、満たしていなければ「０」が設定される。
【０１０３】
たとえば、プロセッサ２０は、ロボット１８ａに対して、実体Ｅ毎に条件を満たすか否かを判断し、その判断結果をフラグの列に記録する。そして、全ての実体Ｅに対して判断が終了すれば、プロセッサ２０は、追跡実体リストのフラグの列において「１」が設定された実体ＩＤが１つだけであるか否かを判断する。このとき、フラグが「１」に設定された実体ＩＤが「００２」、つまり実体Ｅｂだけであれば、プロセッサ２０はロボット１８ａと実体Ｅｂとを関連付ける。
【０１０４】
また、プロセッサ２０は、関連付けた結果を図１３に示す関連付テーブルに記録する。関連付けテーブルは、ロボットＩＤを示す列と実体ＩＤを示す列とから構成される。たとえば、プロセッサ２０は、関連付テーブルにおいて、ロボット１８ａのロボットＩＤ「００１」に対して、実体Ｅｂの実体ＩＤ「００２」を設定する。
【０１０５】
本実施例では、このような処理を経てロボット１８と実体Ｅとを関連付ける処理を、「グローバル関連付処理」と言う。なお、上記説明では、簡単のためにＲＴを１５としてＲＳＭ速度差を算出していたが、実際のグローバル関連付処理では、ＲＴが３００に設定される。
【０１０６】
次に、ＲＴＭの更新について説明する。ＲＴＭの位置は、関連付けられた実体の位置データに基づいて更新される。たとえば、ロボット１８ａに関連付けられた実体Ｅｂの最新の位置データが、（３０，４０）であれば、ロボット１８ａに対応するＲＴＭの位置は（３０，４０）に更新される。
【０１０７】
また、ＲＴＭの角度は、実体Ｅの位置データおよびロボット１８の状態データから、カルマンフィルタを利用して次の位置で推定される角度（推定角度）および推定角度誤差量を求めることで、更新できる。具体的には、図１４を参照して、受信したロボット１８の状態データから次の位置を予測し、予測された位置（予測位置）と前回位置とから予測角度を計算する。また、追跡サーバ１０から現在の位置を取得し、前回位置と現在位置とから検出角度を計算する。そして、検出角度および予測角度から、観測角度Ｚｋを算出（観測）する。ただし、「Ｚｋ」が下付き添え字である場合については、表現の都合上、数式以外では「ｋ」を「Ｚ」の添え字とせず、まとめて下付きの添え字にして記載する。以下、同様である。
【０１０８】
さらに、追跡サーバ１０の観測には常に位置誤差量σ２ｐｏが含まれている。そのため、観測角度誤差量σ２Ｚｋは、数４に従って近似することができる。
【０１０９】
[数４]
0005493097.tif 000005
【０１１０】
ここで、数４において、変位量ｄｓは位置履歴データにおける前回位置から現在位置までの距離であり、角度誤差量σ２Ｚｋはその変位量ｄｓが大きい場合に小さくなる。つまり、実体Ｅの移動距離が大きいほど、角度誤差量σ２Ｚｋが小さくなる。
【０１１１】
また、前回の推定角度誤差量σ２θ-ｋおよび角度誤差量σ２Ｚｋから、カルマンゲインＫｋを数５に従って算出する。そして、そのカルマンゲインＫｋ、観測角度Ｚｋおよび前回の推定角度θ−ｋを、数６に従う式に代入することで、推定角度θ＾ｋを算出できる。
【０１１２】
ただし、「θ−ｋ」が下付きの添え字である場合については、表現の都合上、数式以外では「−」を「θ」の添え字とせず、まとめて下付きの添え字にして記載する。さらに、添え字「＾」は、数式では「θ」の上に付されるが、表現の都合上、数式以外では「θ」の上付き添え字として記載する。以下、同様である。
【０１１３】
[数５]
0005493097.tif 000006
【０１１４】
[数６]
0005493097.tif 000007
【０１１５】
さらに、カルマンゲインＫｋ、前回の推定角度θ−ｋおよび時間毎のプロセスノイズσ２ｐｒを数７に従う式に代入することで、推定角度誤差量σ２θ＾ｋを算出することができる。なお、本実施例ではノイズσ２ｐｒを０．０００１ラジアンとして計算する。
【０１１６】
[数７]
0005493097.tif 000008
【０１１７】
このようにして、推定角度θ＾ｋが算出されると、現在のＲＴＭの角度との角度差を算出して、ＲＴＭの角度を推定角度に補正し、算出した角度差を補正角度データとして記憶する。
【０１１８】
なお、前回値が存在しない場合、たとえば１回目の角度補正処理では、前回の推定角度θ−ｋおよび推定角度誤差量σ２θ＾ｋの代わりに所定値を、数５−７の数式に代入して推定角度θ＾ｋおよび推定角度誤差量σ２θ＾ｋを算出する。また、所定値は、「０」であってもよいし、ロボット１８の初期状態に応じた値であってもよい。
【０１１９】
そして、本実施例では、ロボット１８から状態データを受信（取得）したときに、追跡サーバ１０はロボット１８に対して記憶した補正角度データとＲＴＭが示す位置データを、補正データとしてロボット１８に送信する。そして、ロボット１８は、受信した位置データおよび補正角度データに従って、自身の位置および角度を補正する。これにより、追跡サーバ１０は、ロボット１８を、検出領域Ｆ内において自己位置同定することができる。
【０１２０】
このように、ロボット自己位置同定システム１００は、位置だけでなく、ロボット１８の角度も補正することで、自己位置同定の精度を高めることができる。また、本実施例では、カルマンフィルタを用いて、推定角度θ＾ｋを求めることができる。
【０１２１】
なお、追跡サーバ１０は、算出した推定角度誤差量σ２θ＾ｋを補正精度として記憶して置き、この補正精度が低い場合、つまり推定角度誤差量σ２θ＾ｋが大きい場合には、角度の補正データをロボット１８に送信しない。つまり、ロボット１８には、位置を補正するための補正データのみが送信される。このように、推定角度誤差量σ２θ＾ｋを補正精度として利用することで、角度の補正が可能であるかを判断できるようになる。
【０１２２】
また、本実施例では、ロボット１８と実体Ｅとを関連付けた後に、ローカル追跡エラーまたはグローバル追跡エラーが生じることが有る。ローカル追跡エラーは、追跡サーバ１０に問題が起きたり、実体Ｅを追跡するパーティクルフィルタが少しの間、消えてしまったりしたときに生じる。また、グローバル追跡エラーは、特定の環境下で、ロボット１８と実体Ｅとが誤って関連付けられたときに生じる。
【０１２３】
そこで、本実施例では、ローカル追跡エラーおよびグローバル追跡エラーを除去するために、次の３つの条件のうち１つでも満たされる場合に、１組のロボット１８と実体Ｅとの関連付けを解除する。すなわち、３つの条件は、（１）関連付けられた実体Ｅがすでに追跡されていない、（２）ロボット１８と関連付けられた実体Ｅとが離れている（たとえば、２m）、（３）ロボット１８の速度と関連付けられた実体Ｅの速度が異なる（たとえば、０．３５mm/s）である。
【０１２４】
つまり、ロボット自己位置同定システム１００は、エラーの要因となるロボット１８と実体Ｅとの関連付けを解除できる。
【０１２５】
そして、ロボット１８と実体との関連付けが解除されて、所定時間（たとえば、５秒）以内であれば、追跡サーバ１０は「ローカル関連付処理」を実行して、関連付けが解除されたロボット１８を、他の実体Ｅと関連付ける。
【０１２６】
ローカル関連付処理は、ＲＭＳ速度差（第２速度差）が３５０mm/s（第２所定速度）以下であり、かつ現在の速度差が３５０mm/s以下であり、かつロボット１８と実体Ｅとの距離が１m（所定距離）以下の実体Ｅが複数存在していれば、ロボット１８と最も近い実体Ｅが、ロボット１８と関連付けられる。
【０１２７】
また、ローカル関連付処理は、数３に示すＲＴを５（秒）として計算するため、先述したグローバル関連付け処理に比べて処理の速度が速い。以下、関連付けの解除およびローカル関連付け処理について、具体例を示して説明する。
【０１２８】
図１５（Ａ）は、人間とロボット１８との位置関係を示す図解図である。また、図１５（Ａ）に示す状態では、グローバル関連付処理が実行されていない。検出領域Ｆ内において、ロボット１８は人間に対してコミュニケーション行動を行っている。このとき、追跡サーバ１０は、（２００，３００）の位置でロボット１８を検出し、（２５０，３００）の位置で人間を検出する。そして、ロボット１８は、位置データ（２４０，３００）および速度データ（０mm/s）を含む、状態データを出力する。
【０１２９】
この状態でグローバル関連付処理が実行されると、図１５（Ｂ）に示すように、人間に対応する実体Ｅとロボット１８とが誤って関連付けられる。つまり、コミュニケーション行動が行われていれば、ロボット１８と人間とは動かないため、停止している人間またはロボット１８が他に存在しなければ、図１５（Ｂ）に示すように、人間に対応する実体Ｅとロボット１８とが誤って関連付けられる。そして、ロボット１８におけるＲＴＭの位置は、人間の位置（２５０，３００）に補正される。
【０１３０】
ところが、コミュニケーション行動が終了して、人間がロボット１８の前から立ち去ると、図１５（Ｃ）に示すような状態になる。つまり、ロボット１８が出力する状態データは位置データが（２５０，３００）となるが、人間に対応する実体Ｅの位置データが（５００，３００）となり、ＲＴＭの位置と人間の位置とで、距離が２５０cm（＝５００−２５０）離れていることなる。このとき、上記した関連付け解除の条件（２）を満たすことになるため、人間に対応する実体Ｅとロボット１８との関連付けは解除される。
【０１３１】
そして、関連付けが解除され、所定時間以内であれば、ローカル関連付処理が実行される。たとえば、図１５（Ｃ）に示す状態では、ロボット１８に対応する実体Ｅがローカル関連付けの条件（１）−（３）を満たすため、その実体Ｅとロボット１８とが正しく関連付けられる。このように、関連付けが解除されて間もなければ、ロボット１８と実体Ｅとを容易に関連付け直すことができる。
【０１３２】
また、ローカル関連付処理を実行しても、５秒以内にロボット１８と実体Ｅとを関連付けられなければ、グローバル関連付処理が実行される。つまり、関連付けが解除されて所定時間が経過したとしても、ロボット１８と実体Ｅとを関連付け直すことができる。
【０１３３】
図１６は、図２に示すメモリ２２のメモリマップ３００の一例を示す図解図である。図１６に示すように、メモリ２２はプログラム記憶領域３０２およびデータ記憶領域３０４を含む。プログラム記憶領域３０２には、追跡サーバ１０を動作させるためのプログラムとして、通信プログラム３１２および補正プログラム３１４などが記憶される。なお、通信プログラム３１２および補正プログラム３１４などの処理によって、ローカライゼーション機能が有効になる。
【０１３４】
通信プログラム３１２は、ロボット１８から状態データを受信し、補正データをロボット１８に送信するためのプログラムである。補正プログラム３１４は、ＲＴＭデータを補正するためのプログラムであり、追跡プログラム３１４ａ、関連付解除プログラム３１４ｂ、関連付プログラム３１４ｃおよび角度補正プログラム３１４ｄのサブルーチンを含む。
【０１３５】
追跡プログラム３１４ａは、パーティクルフィルタの尤度を更新し、実体Ｅの位置を検出するためのプログラムである。関連付解除プログラム３１４ｂは、ロボット１８と実体Ｅとの関連付けを解除するためのプログラムである。関連付プログラム３１４ｃは、ロボット１８と実体Ｅとを関連付けるためのプログラムであり、さらにローカル関連付プログラム３１６およびグローバル関連付プログラム３１８のサブルーチンを含む。そして、角度補正プログラム３１４ｄは、上述したカルマンフィルタによって角度を推定し、角度の補正データを求めるためのプログラムである。
【０１３６】
なお、図示は省略するが、追跡サーバ１０を動作させるためのプログラムとしては、関連付けなどが解除されたときに時間をカウントするプログラム、追跡サーバ１０とロボット１８との時刻を同期するためのプログラムなどを含む。
【０１３７】
また、図１７を参照して、データ記憶領域３０４には、ＬＲＦバッファ３３０、位置履歴バッファ３３２、状態履歴バッファ３３４、追跡実体リストバッファ３３６、補正角度バッファ３３８、補正精度バッファ３４０および前回値バッファ３４２が設けられる。また、データ記憶領域３４０には、関連付テーブルデータ３４４およびＲＴＭデータ３４６が記憶されるとともに、初回フラグ３４８、初期起動カウンタ３５０および解除カウンタ３５２も設けられる。
【０１３８】
ＬＲＦバッファ３３０は、各ＬＲＦ１２によって計測された距離データが一時記憶されるバッファである。位置履歴バッファ３３２は、図７に示す位置履歴データが一時的に記憶されるバッファである。状態履歴バッファ３３４は、図８に示す状態履歴データが一時的に記憶されるバッファである。追跡実体リストバッファ３３６は、図１２に示す追跡実体リストのデータが一時的に記憶されるバッファである。補正角度バッファ３３８は、推定角度θ＾ｋとＲＴＭの角度との角度差が、補正角度データとして一時的に記憶されるバッファである。補正精度バッファ３４０は、推定角度誤差量σ２θ＾ｋを補正精度データとして一時的に記憶するためのバッファである。前回値バッファ３４２は、角度補正処理で算出された推定角度θ＾ｋおよび推定角度誤差量σ２θ＾ｋを、前回の推定角度θ−ｋおよび推定角度誤差量σ２θ＾ｋとして一時的に記憶するためのバッファである。
【０１３９】
関連付けテーブルデータ３４４は、図１３に示す関連付テーブルのデータである。ＲＴＭデータ３４６は、図９に示すように、各ロボット１８に対応する複数のＲＴＭから構成されるデータである。
【０１４０】
初回フラグ３４８は、追跡サーバ１０が実行されてから一定時間が経過したか否かを判断するためのフラグである。たとえば初回フラグ３４８は１ビットのレジスタで構成される。初回フラグ３４８がオン（成立）されると、レジスタにはデータ値「１」が設定される。一方、初回フラグ３４８がオフ（不成立）されると、レジスタにはデータ値「０」が設定される。また、初回フラグ３４８は、追跡サーバ１０が起動されるとオンになり、追跡サーバ１０が起動してから一定時間が経過するとオフになる。
【０１４１】
初期起動カウンタ３５０は、追跡サーバ１０が起動してから一定時間が経過するまで時間をカウントするためのカウンタである。解除カウンタ３５２は、ロボット１８と実体とＥとの関連付けが解除されてからの時間をカウントするためのカウンタである。
【０１４２】
なお、図示は省略するが、データ記憶領域３０４には、遠隔操作装置１４および計画サーバ１６とのデータ通信の結果を一時的に記憶するバッファや、カルマンフィルタの計算結果が一時的に記憶されるバッファなどが設けられると共に、追跡サーバ１０の動作に必要な他のカウンタやフラグなども設けられる。
【０１４３】
以下、追跡サーバ１０によって実行される本願発明のフロー図について説明する。また、図１８および図１９のフロー図は、通信プログラム３１２による処理を示し、図２０のフロー図は補正プログラム３１４による処理を示す。また、図２１および図２２のフロー図は追跡プログラム３１４ａによる処理を示し、図２３のフロー図は関連付解除プログラム３１４ｂによる処理を示す。さらに、図２４のフロー図は関連付プログラム３１４ｃによる処理を示し、図２５のフロー図はローカル関連付プログラム３１６による処理を示し、図２６のフロー図はグローバル関連付プログラム３１８による処理を示し、図２７のフロー図は角度補正プログラム３１４ｄによる処理を示す。
【０１４４】
図１８を参照して、たとえば、追跡サーバ１０の電源がオンにされると、通信処理が実行される。そして、プロセッサ２０は、ステップＳ１では各ロボット１８との通信を確立する。たとえば、検出領域Ｆ内にロボット１８ａ−１８ｃが存在していれば、それらのロボット１８との通信を確立する。続いて、ステップＳ３では、通信を確立しているロボット１８の数を変数Ｒｎに設定する。たとえば、３台のロボット１８（１８ａ，１８ｂ，１８ｃ）との通信が確立されていれば、変数Ｒｎには「３」が設定される。なお、変数Ｒｎは、通信が確立されているロボット１８の総数を示す変数として定義されている。続いて、ステップＳ５では、Ｒｎ個のＲＴＭを作成する。たとえば、変数Ｒｎに「３」が設定されていれば、３つのＲＴＭを作成する。つまり、ＲＴＭデータ３４６には、作成された３つのＲＴＭが含まれる。
【０１４５】
続いて、図１９を参照して、ステップＳ７では、ロボット１８が出力した状態データを取得する。たとえば、ロボット１８ａが状態データを発信していれば、その状態データを受信する。続いて、ステップＳ９では、ロボットＩＤに基づいて状態データをバッファに記憶する。たとえば、ロボットＩＤが「００１」である、ロボット１８ａの状態データを取得した場合、その状態データを状態履歴バッファ３３４に記憶する。また、ロボット１８ａの状態データは、ロボット１８ａの状態履歴データ（図８参照）に追加される。なお、ステップＳ９の処理を実行するプロセッサ２０は、状態データ記憶手段として機能する。
【０１４６】
続いて、ステップＳ１１では、補正できるか否かを判断する。つまり、ロボット１８と実体Ｅとが関連付けられているか否かを判断する。また、本実施例では、追跡サーバ１０は起動してから一定時間が経過しなければ、ロボット１８と実体Ｅとをまだ関連付けることができない。そのため、ステップＳ１１では、起動してから一定時間が経過したことを示す初回フラグ３４８がオフであるか否かを判断する。ステップＳ１１で“ＮＯ”であれば、つまり追跡サーバ１０が起動してから一定時間が経過していなければ、ステップＳ１７に進む。
【０１４７】
また、ステップＳ１１で“ＹＥＳ”であれば、つまり追跡サーバ１０が起動してから一定時間が経過し、ロボット１８と実体Ｅとが関連付けられていれば、ステップＳ１３で補正精度が高いか否かを判断する。つまり、補正精度バッファ３４０に記憶された推定角度誤差量σ２θ＾ｋが閾値（たとえば、２度）以下であるか否かを判断する。ここで、ステップＳ１３で“ＮＯ”であれば、つまり補正精度バッファ３４０に一時記憶された推定角度誤差量σ２θ＾ｋが閾値以下でなければ、ステップＳ１５でＲＴＭの位置データをロボットＩＤに基づいて送信する。たとえば、ロボットＩＤ「００１」のロボット１８ａから状態データを取得していれば、同じロボットＩＤのロボット１８ａに対して、ＲＴＭの位置データを送信する。続いて、ステップＳ１７では、ロボットＩＤに基づいてＲＴＭを更新する。つまり、取得された状態データに含まれる角度データに基づいて、ＲＴＭの角度および速度を更新する。たとえば、ロボット１８ａの状態データにおいて、角度データが３０度であれば、ロボット１８ａのＲＴＭの角度が３０度に更新される。
【０１４８】
また、ステップＳ１３で“ＹＥＳ”であれば、つまり補正精度バッファ３４０に一時記憶された推定角度誤差量σ２θ＾ｋが閾値以下であれば、ステップＳ１９でＲＴＭの位置データおよび補正角度データをロボットＩＤに基づいて送信する。たとえば、状態データを受信したロボット１８のロボットＩＤに基づいて、ＲＴＭの位置データおよび補正角度データが送信される。なお、ロボット１８に送信される補正角度データは、補正角度バッファ３３８から読み出される。
【０１４９】
続いて、ステップＳ２１では、取得した角度および補正角度データから現在の角度を計算する。たとえば、状態データの角度データが２５度であり、補正角度データが−２度であれば、現在の角度が２３度（＝２５−２）になる。そして、ステップＳ１７では、ＲＴＭの角度はステップＳ２１で計算された角度に更新される。たとえば、ＲＴＭの角度が３０度であれば、ステップＳ２１で算出された２３度に更新される。
【０１５０】
なお、ステップＳ１５またはステップＳ１９の処理を実行するプロセッサ２０は、送信手段として機能する。また、特にステップＳ１５の処理を実行するプロセッサ２０は第２送信手段として機能し、ステップＳ１９の処理を実行するプロセッサ２０は第１送信手段として機能する。
【０１５１】
ここで、ステップＳ１１で“ＮＯ”と判断された場合には、ステップＳ１７では、状態データに含まれる位置データ、角度データおよび速度データに基づいて、ＲＴＭの位置、角度および速度が更新される。つまり、ロボット１８と実体Ｅとが関連付けられていない状態では、ロボット１８が出力する状態データに基づいてＲＴＭが更新される。
【０１５２】
ステップＳ２３では、各ロボット１８との通信が継続しているか否かを判断する。たとえば、ロボット１８ａ−１８ｄとの通信が継続しているか否かを判断する。ステップＳ２３で“ＮＯ”であれば、つまりロボット１８との通信が途切れていれば、ステップＳ２５で通信が途切れたロボットＩＤに基づいてＲＴＭを削除する。たとえば、ロボットＩＤが「００４」のロボット１８ｄとの通信が途切れていれば、ＲＴＭデータ３４６を構成する、ロボットＩＤ「００４」のＲＴＭが削除される。そして、ステップＳ２５の処理が終了すれば、ステップＳ３に戻る。つまり、変数Ｒｎをディクリメントして、ＲＴＭデータ３４６に含まれるＲＴＭの数と変数Ｒｎの数値とを一致させる。
【０１５３】
また、ステップＳ２３で“ＹＥＳ”であれば、つまり各ロボット１８との通信が継続していれば、ステップＳ２７で新しいロボット１８との通信が確立されたか否かを判断する。たとえば、５台目のロボット１８が検出領域Ｆに入ったか否かを判断する。ステップＳ２７で“ＮＯ”であれば、つまり新たなロボット１８との通信が確立されなければ、ステップＳ７に戻る。一方、ステップＳ２７で“ＹＥＳ”であれば、たとえば５台目のロボット１８との通信が確立されれば、ステップＳ３に戻る。つまり、変数Ｒｎを更新して、ＲＴＭデータ３４６に新しいＲＴＭを追加する。
【０１５４】
図２０には補正プログラム３１４の処理を示すフロー図が示される。たとえば、追跡サーバ１０の電源がオンにされると、プロセッサ２０は、ステップＳ４１で追跡処理を実行する。この追跡処理については、図２１および図２２に示すフロー図を用いて後述するため、ここでの詳細な説明は省略する。
【０１５５】
続いて、ステップＳ４３では、１度目の処理か否かを判断する。つまり追跡サーバ１０が起動してから、補正処理が実行されるのが１度目であるか否かを判断する。また、具体的には、初回フラグ３４８がオンであるか否かを判断する。ステップＳ４３で“ＮＯ”であれば、つまり初回フラグ３４８がオフであれば、ステップＳ４７に進む。一方、ステップＳ４３で“ＹＥＳ”であれば、つまり初回フラグ３４８がオンであれば、ステップＳ４５で一定時間が経過したか否かを判断する。たとえば、一定時間は３００秒であり、ステップＳ４５では追跡サーバ１０が起動してから３００秒経過したか否かを判断する。また、具体的には、追跡サーバ１０が起動してからの時間をカウントする初期起動カウンタ３５０の値が３００秒を示す値を超えているか否かを判断する。
【０１５６】
ステップＳ４５で“ＮＯ”であれば、つまり追跡サーバ１０が起動してから一定時間が経過していなければ、ステップＳ４１に戻る。つまり、位置データおよび状態データが記憶されるまでは、ロボット１８と実体Ｅとを関連付けることができないため、ステップＳ４１−Ｓ４５の処理が繰り返し実行される。
【０１５７】
また、ステップＳ４５で“ＹＥＳ”であれば、つまり追跡サーバ１０が起動してから一定時間が経過すれば、初回フラグ３４８をオフにしてステップＳ４７で関連付解除処理を実行する。なお、ステップＳ４７の関連付解除処理については、図２３に示すフロー図を用いて後述するため、ここでの詳細な説明は省略する。また、１度目の処理では、関連付解除処理が実行されたとしても、ロボット１８と実体Ｅとの関連付けが解除されることはない。
【０１５８】
続いて、ステップＳ４９では関連付処理が実行される。この関連付処理については、図２４に示すフロー図を用いて後述するため、ここでの詳細な説明は省略する。また、ステップＳ４９の処理を実行するプロセッサ２０は、関連付手段として機能する。
【０１５９】
続いて、ステップＳ５１では、変数ｉｘを初期化する。この変数ｉｘは、ロボットＩＤを指定するための変数である。そのため、変数ｉｘが初期化されると「１」が設定される。なお、以下の説明では、ロボットＩＤが「ｉｘ」のロボット１８は「ロボットｉｘ」と記述する。たとえば、ロボットＩＤが「００１」のロボット１８は「ロボット１」と記述される。これは、図面でも同じである。
【０１６０】
続いて、ステップＳ５３では、ロボットｉｘに対応するＲＴＭの位置を更新する。たとえば、変数ｉｘが「１」であれば、ロボットＩＤが「００１」のロボット１８ａのＲＴＭの位置が更新される。また、更新するための位置データは、位置履歴バッファ３３２に記憶される、ロボット１の位置履歴データから読み出される。従って、ロボット１の位置履歴データにおいて最新の位置が（３００，４００）であれば、ロボット１のＲＴＭの位置が（３００，４００）に更新される。続いて、ステップＳ５５では、角度補正処理を実行する。なお、この角度補正処理については、図２７に示すフロー図を用いて後述するため、ここでの詳細な説明は省略する。
【０１６１】
続いて、ステップＳ５７では、変数ｉｘをインクリメントする。つまり、次のロボットＩＤを指定するために、変数ｉｘはインクリメントされる。続いて、ステップＳ５９では、変数ｉｘが変数Ｒｎより大きいか否かを判断する。つまり、ロボット１８の総数を示す変数Ｒｎよりも、ロボットＩＤを指定する変数ｉｘが大きいか否かを判断する。これにより、全てのロボット１８に対して、ＲＴＭの位置および角度が更新されたか否かを判断することができる。ステップＳ５９で“ＮＯ”であれば、つまり変数ｉｘが変数Ｒｎ以下であれば、ステップＳ５３に戻る。つまり、他のロボット１８のＲＴＭの位置および角度を更新するためにステップＳ５３に戻る。一方、ステップＳ５９で“ＹＥＳ”であれば、つまり変数ｉｘが変数Ｒｎより大きければ、ステップＳ４１に戻る。つまり、全てのロボット１８のＲＴＭの位置および角度が更新されると、補正処理が再び実行される。
【０１６２】
図２１には追跡プログラム３１４ａの処理を示すフロー図が示される。プロセッサ２０は、ステップＳ４１で追跡処理が実行されると、ステップＳ７１でＬＲＦデータを取得する。つまり、ＬＲＦバッファ３３０に一時記憶される、各ＬＲＦ１２によって計測された距離のデータを取得する。続いて、ステップＳ７３では、新しい実体Ｅを検出したか否かを判断する。たとえば、検出領域Ｆ内で、実体Ｅａ−Ｅｄとは別の実体Ｅが検出されたか否かを判断する。ステップＳ７３で“ＮＯ”であれば、つまり新たな実体Ｅが検出されていなければ、ステップＳ７９に進む。一方、ステップＳ７３で“ＹＥＳ”であれば、つまり検出領域Ｆ内で新たな実体Ｅが検出されれば、ステップＳ７５で実体Ｅの数を変数Ｅｎに設定する。たとえば、検出領域Ｆ内で、４つの実体Ｅａ，Ｅｂ，Ｅｃ，Ｅｄが検出されていれば、変数Ｅｎには「４」が設定される。なお、変数Ｅｎは、検出されている実体Ｅの総数を示す変数として定義されている。
【０１６３】
続いて、ステップＳ７７では、Ｅｎ個の位置履歴データを作成する。たとえば、変数Ｅｎに「４」が設定されていれば、図７に示すように、位置履歴バッファ３３２に４つの位置履歴データが作成される。続いて、ステップＳ７９では、変数ＩＸを初期化する。この変数ＩＸは、実体ＩＤを指定するための変数である。そのため、変数ＩＸが初期化されると「１」が設定される。なお、以下の説明では、実体ＩＤが「ＩＸ」の実体Ｅは「実体ＩＸ」と記述する。たとえば、実体ＩＤが「００１」の実体Ｅは、「実体１」と記述される。これは、図面でも同じである。
【０１６４】
続いて、図２２を参照して、ステップＳ８１では、実体ＩＸにロボット１８が関連付けられているか否かを判断する。たとえば、変数ＩＸに「１」が設定されていれば、関連付けテーブルを参照し、実体ＩＤが「００１」の実体Ｅａにロボット１８が関連付けられているか否かを判断する。ステップＳ８１で“ＹＥＳ”であれば、つまり実体ＩＸにロボッ１８が関連付けられていれば、ロボット形状モデルを利用して、パーティクルフィルタの尤度を更新する。つまり、ステップＳ８３では、ロボット１８に適したモデルを使用して、数１および数２に示す式に従って、パーティクルフィルタの尤度を更新する。一方、ステップＳ８１で“ＮＯ”であれば、つまり実体ＩＸにロボット１８が関連付けられていなければ、ステップＳ８５で人形状モデルを利用して、パーティクルフィルタの尤度を更新する。つまり、ステップＳ８５では、人間に適したモデルを利用して、数１および数２に示す式に従って、パーティクルフィルタの尤度を更新する。
【０１６５】
続いて、ステップＳ８７では、パーティクルの位置を更新する。つまり、ステップＳ８３およびステップＳ８５の処理で、算出されたパーティクルフィルタの尤度に基づいてパーティクルの位置が更新される。続いて、ステップＳ８９では、実体ＩＸの位置を検出する。つまり、移動したパーティクルの位置に基づいて、実体ＩＸの位置、つまり検出領域Ｆ内の平面座標を検出する。なお、ステップＳ８９の処理を実行するプロセッサ２０は、位置検出手段として機能する。
【０１６６】
続いてステップＳ９１では、検出された実体ＩＸの位置をバッファに記憶する。つまり、位置データは、位置履歴バッファ３３２に記憶される位置履歴データにおいて、実体ＩＤ「ＩＸ」に基づいて追加される。たとえば、変数ＩＸに「１」が設定されていれば、実体ＩＤが「００１」の位置履歴データに、実体１の位置データが追加される。なお、ステップＳ９１の処理を実行するプロセッサ２０は、位置データ記憶手段として機能する。
【０１６７】
続いて、ステップＳ９３では、変数ＩＸをインクリメントする。つまり、次の実体ＩＤを指定するために、変数ＩＸはインクリメントされる。続いて、ステップＳ９５では、変数ＩＸが変数Ｅｎよりも大きいか否かを判断する。つまり、実体Ｅの総数を示す変数Ｅｎよりも、実体ＩＤを指定する変数ＩＸが大きいか否かを判断する。これにより、全ての実体Ｅに対して位置が検出されたか否かを判断することができる。ステップＳ９５で“ＮＯ”であれば、つまり全ての実体Ｅに対して位置が検出されていなければ、ステップＳ８１に戻る。一方、ステップＳ９５で“ＹＥＳ”であれば、つまり全ての実体Ｅに対して位置が検出されれば、追跡処理を終了して、補正処理に戻る。
【０１６８】
図２３には、関連付解除プログラム３１４ｂの処理を示すフロー図が示される。ステップＳ４７で関連付処理が実行されると、プロセッサ２０は、ステップＳ１１１でロボット１８が関連付けられているか否かを判断する。つまり、関連付テーブルデータ３４４が読み出され、少なくとも１台のロボット１８が実体Ｅと関連付けられているか否かを判断する。ステップＳ１１１で“ＮＯ”であれば、つまり全てのロボット１８が実体Ｅと関連付けられていなければ、関連付解除処理を終了して、補正処理に戻る。たとえば、追跡サーバ１０が起動し、ステップＳ４９の関連付処理が一度も実行されていない状態では、ステップＳ１１１で“ＮＯ”と判断される。
【０１６９】
一方、ステップＳ１１１で“ＹＥＳ”であれば、つまり少なくとも１台のロボット１８が実体Ｅと関連付けられていれば、ステップＳ１１３で関連付けられたロボット１８の数を変数ＡＲｎに設定する。たとえば、実体Ｅと関連付けられているロボット１８が３台であれば、変数ＡＲｎには「３」が設定される。また、変数ＡＲｎは、実体Ｅと関連付けられているロボット１８の総数を示す変数として定義されている。
【０１７０】
続いて、ステップＳ１１５では、変数Ｄｘを初期化する。たとえば、関連付け解除処理では、関連付けられているロボット１８には、第１ローカルＩＤが付与される。そして、変数Ｄｘは、その第１ローカルＩＤを指定するための変数である。そのため、変数Ｄｘが初期化されると、第１ローカルＩＤの最初のＩＤを示す「１」が設定されている。
【０１７１】
なお、第１ローカルＩＤは、関連付解除処理が実行される毎に付与される。つまり、同じロボット１８であっても、必ずしも同じ第１ローカルＩＤが付与されるとは限らない。また、ロボットＩＤと第１ローカルＩＤとも必ずしも一致するとは限らない。そして、変数ｉｘと同様に、以下の説明では、第１ローカルＩＤが「Ｄｘ」のロボット１８は、「ロボットＤｘ」と記述する。そして、この記述は図２３でも同じである。
【０１７２】
次のステップＳ１１７では、ロボットＤｘと関連付けられた実体Ｅがまだ追跡されている否かを判断する。つまり、関連付テーブルデータ３４４に基づいて、ロボットＤｘに対応する実体Ｅの実体ＩＤを読み出す。そして、位置履歴バッファ３３２を参照して、読み出した実体ＩＤに対応する位置履歴データが存在するか否かを判断する。ステップＳ１１７で“ＮＯ”であれば、つまりロボットＤｘと関連付けられた実体Ｅが検出領域Ｆ内に存在していなければ、ステップＳ１２５に進む。
【０１７３】
また、ステップＳ１１７で“ＹＥＳ”であれば、つまりロボットＤｘと関連付けられた実体Ｅが検出領域Ｆ内に存在していれば、ステップＳ１１９で、ロボットＤｘと関連付けられた実体Ｅが離れているか否かを判断する。たとえば、ステップＳ１１７と同様、ロボットＤｘに対応する実体ＩＤを読み出し、読み出した実体ＩＤに対応する位置履歴データの最新の位置データを、位置履歴バッファ３３２から取得する。さらに、ロボットＩＤが「Ｄｘ」の状態データから最新の状態データに含まれる位置データを、状態履歴バッファ３３４から取得する。そして、読み出された状態データの位置データと、位置履歴データの位置データとを比較して、たとえば２m以上離れているか否かを判断する。ステップＳ１１９で“ＹＥＳ”であれば、つまりロボットＤｘと関連付けられた実体Ｅが離れていれば、ステップＳ１２５に進む。
【０１７４】
また、ステップＳ１１９で“ＮＯ”であれば、たとえばロボットＤｘと関連付けられた実体Ｅが２m以内に存在していれば、ステップＳ１２１でロボットＤｘと関連付けられた実体Ｅとの速度が異なるか否かを判断する。ステップＳ１１９と同様に、位置履歴データから現在の位置および１秒前の位置を取得して、実体Ｅの速度を算出する。また、ステップＳ１１９と同様に、状態履歴データから現在の位置および１秒前の位置を取得して、ロボットＤｘの速度を算出する。そして、ロボットＤｘの速度と、関連付けられた実体Ｅの速度との差が、たとえば０．３５mm/s以上であるか否かを判断する。ステップＳ１２１で“ＹＥＳ”であれば、たとえば０．３５mm/s以上の速度差があれば、ステップＳ１２５に進む。また、ステップＳ１２１で“ＮＯ”であれば、たとえば０．３５mm/s以上の速度差がなければ、ステップＳ１２３でロボットＤｘの関連付けを維持する。たとえば、ステップＳ１２３では、ロボットＤｘの関連付けを解除する必要がないことを示すフラグをオンにする。そして、ステップＳ１２３の処理が終了すると、ステップＳ１２７に進む。
【０１７５】
ここで、ステップＳ１１７で“ＮＯ”、ステップＳ１１９で“ＹＥＳ”またはステップＳ１２１で“ＹＥＳ”と判断され、ステップＳ１２５の処理が実行されると、ロボットＤｘの関連付けを解除する。つまり、ステップＳ１２５では、関連付テーブルデータ３４４において、ロボットＤｘと関連付けられている実体ＩＤを削除する。また、ステップＳ１２５で関連付けが解除されると、解除カウンタ３５２によって、関連付けが解除されてからの時間が計測される。
【０１７６】
なお、ステップＳ１１７，Ｓ１１９，Ｓ１２１の処理を実行するプロセッサ２０は判断手段として機能する。また、ステップＳ１２５の処理を実行するプロセッサ２０は、解除手段として機能する。
【０１７７】
続いて、ステップＳ１２７では、変数Ｄｘをインクリメントする。つまり次の第１ローカルＩＤを指定するために、変数Ｄｘはインクリメントされる。続いて、ステップＳ１２９では、変数Ｄｘが変数ＡＲｎより大きいか否かを判断する。つまり、実体Ｅと関連付けられたロボット１８の総数を示す変数ＡＲｎよりも、第１ローカルＩＤを指定する変数Ｄｘが大きいか否かを判断する。これにより、実体Ｅと関連付けられた全てのロボット１８に対して、関連付けの解除が必要であるか否かを判断することができる。
【０１７８】
ステップＳ１２９で“ＮＯ”であれば、つまり変数Ｄｘが変数ＡＲｎ以下であれば、ステップＳ１１７に戻る。つまり、他のロボット１８において、関連付けの解除が必要であるか否かを判断するためにステップＳ１１７に戻る。一方、ステップＳ１２９で“ＹＥＳ”であれば、つまり変数Ｄｘが変数ＡＲｎより大きければ、関連付解除処理を終了して、補正処理に戻る。
【０１７９】
なお、解除カウンタ３５２は、関連付けが解除される度にリセットされて、時間が計測される。たとえば、関連付解除処理において、２つのロボット１８の関連付けが解除された場合には、後に関連付けが解除されたロボット１８に対してのみ、関連付けが解除されてからの時間が計測される。
【０１８０】
図２４には関連付プログラム３１４ｃの処理を示すフロー図が示される。ステップＳ４９で関連付処理が実行されると、プロセッサ２０は、ステップＳ１５１で関連付けられていないロボット１８があるか否かを判断する。つまり、関連付テーブルデータ３４４が読み出され、実体Ｅが関連付けられていないロボット１８があるか否かを判断する。ステップＳ１５１で“ＮＯ”であれば、つまり全てのロボット１８が実体Ｅと関連付けられていれば、関連付処理を終了して、補正処理に戻る。
【０１８１】
一方、ステップＳ１５１で“ＹＥＳ”であれば、つまり実体Ｅと関連付けられていないロボット１８があれば、ステップＳ１５３で関連付けられていないロボット１８の数を変数ＤＲｎに設定する。たとえば、実体Ｅと関連付けが解除されたロボット１８が１台であれば、変数ＤＲｎには「１」が設定される。なお、変数ＤＲｎは、実体Ｅと関連付けが解除されたロボット１８の総数を示す変数として定義されている。
【０１８２】
続いて、ステップＳ１５５では、変数ａｘを初期化する。たとえば、関連付処理では、関連付けられていないロボット１８には、第２ローカルＩＤが付与される。そして、変数ａｘは、第２ローカルＩＤを指定するための変数である。そのため、変数ａｘが初期化されると、第２ローカルＩＤの最初のＩＤを示す「１」が設定される。
【０１８３】
なお、第２ローカルＩＤは、第１ローカルＩＤと同様に、関連付処理が実行される毎に付与される。つまり、同じ第２ローカルＩＤが、同じロボット１８に付与されるとは限らない。また、変数Ｄｘと同様に、以下の説明では、第２ローカルＩＤが「ａｘ」のロボット１８を「ロボットａｘ」と記述する。そして、この記述は図面でも同じである。
【０１８４】
続いて、ステップＳ１５７では、追跡実体リストを作成する。たとえば、図１２に示す追跡実体リストを、実体Ｅの総数を示す変数Ｅｎに基づいて作成する。そして、作成した追跡実体リストを追跡実体リストバッファ３３６に格納する。続いて、ステップＳ１５９では、５秒（所定時間）以内に関連付けが解除されたか否かを判断する。つまり、５秒以内に実体Ｅとロボット１８との関連付けが解除されたか否かを判断する。また、具体的には、解除カウンタ３５２の値が５秒を示す値以下であるか否かを判断する。ステップＳ１５９で“ＮＯ”であれば、つまり実体Ｅとロボット１８との関連付けが解除されて５秒を超えていれば、ステップＳ１７１に進む。
【０１８５】
一方、ステップＳ１５９で“ＹＥＳ”であれば、つまり実体Ｅとロボット１８との関連付けが解除されてから５秒以内であれば、ステップＳ１６１で変数ＩＸを初期化する。つまり、後述するローカル関連付処理で、全ての実体Ｅを識別する必要があるため、変数ＩＸを初期化する。なお、初期化された変数ＩＸには「１」が設定される。続いて、ステップＳ１６３では、追跡実体リストを初期化する。つまり、追跡実体リストのフラグの列の全てを「１（オン）」に設定する。
【０１８６】
続いて、ステップＳ１６５では、ローカル関連付処理を実行する。なお、ステップＳ１６５のローカル関連付処理については、図２５に示すフロー図を用いて後述するため、ここでの詳細な説明は省略する。続いて、ステップＳ１６７では、変数ａｘをインクリメントする。つまり、次の第２ローカルＩＤを指定するために、変数ａｘはインクリメントされる。続いて、ステップＳ１６９では、変数ａｘが変数ＤＲｎより大きいか否かを判断する。つまり、関連付けられていないロボット１８の総数を示す変数ＤＲｎよりも、第２ローカルＩＤを指定する変数ａｘの方が大きいか否かを判断する。これにより、関連付けられていない全てのロボット１８に対して、ローカル関連付処理が実行されたか否かを判断することができる。
【０１８７】
ステップＳ１６９で“ＮＯ”であれば、つまり変数ａｘが変数ＤＲｎ以下であれば、ステップＳ１６１に戻る。つまり、関連付けられていない他のロボット１８に対してローカル関連付け処理を実行するために、ステップＳ１６１に戻る。一方、ステップＳ１６９で“ＹＥＳ”であれば、つまり変数ａｘが変数ＤＲｎより大きければ、関連付け処理を終了して、補正処理に戻る。
【０１８８】
ここで、ロボット１８と実体Ｅとの関連付けが５秒以内に解除されていなければ、ステップＳ１７１−Ｓ１７９の処理を実行する。なお、ステップＳ１７１−Ｓ１７９で、ステップＳ１６１−Ｓ１６９と同じ処理については、詳細な説明を省略する。
【０１８９】
ステップＳ１７１では、変数ＩＸを初期化し、ステップＳ１７３では追跡実体リストを初期化する。続いて、ステップＳ１７５では、グローバル関連付処理を実行する。このグローバル関連付け処理については、図２６に示すフロー図を用いて後述するため、ここでの詳細な説明は省略する。
【０１９０】
続いて、ステップＳ１７７では、変数ａｘをインクリメントし、ステップＳ１７９では、変数ａｘが変数ＤＲｎより大きいか否かを判断する。つまり、ステップＳ１７９では、関連付けられていない全てのロボット１８に対して、グローバル関連付処理が実行されたか否かを判断する。ステップＳ１７９で“ＮＯ”であれば、つまり実体Ｅと関連付けられていない全てのロボット１８に対してグローバル関連付処理が実行されていなければ、ステップＳ１７１に戻る。一方、ステップＳ１７９で“ＹＥＳ”であれば、つまり実体Ｅと関連付けられていない全てのロボット１８に対してグローバル関連付処理が実行されていれば、関連付処理を終了する。
【０１９１】
図２５にはローカル関連付プログラム３１６の処理を示すフロー図が示される。ステップＳ１６５でローカル関連付処理が実行されると、プロセッサ２０は、ステップＳ１９１で実体ＩＸが他のロボット１８に関連付けられているか否かを判断する。つまり、関連付テーブル３４４が読み出され、実体ＩＸと他のロボット１８とが関連付けられているか否かを判断する。ステップＳ１９１で“ＹＥＳ”であれば、つまり実体ＩＸが他のロボット１８と関連付けられていれば、ステップＳ２０１に進む。一方、ステップＳ１９１で“ＮＯ”であれば、つまり実体ＩＸが他のロボット１８と関連付けられていなければ、ステップＳ１９３で実体ＩＸとロボットａｘとの５秒間（第１時間）のＲＭＳ速度差（第１速度差）を算出する。つまり、ステップＳ１９３では、位置履歴バッファ３３２から、５秒分の実体ＩＸの位置履歴データを読み出し、状態履歴バッファ３３４から、５秒分のロボットａｘの状態履歴データを読み出す。そして、数３においてＲＴを５として、ＲＭＳ速度差を算出する。なお、ステップＳ１９３の処理を実行するプロセッサ２０は、第１速度差算出手段として機能する。
【０１９２】
続いて、ステップＳ１９５では、実体ＩＸとロボットａｘとのＲＭＳ速度差が大きいか否かを判断する。たとえば、ステップＳ１９３で算出されたＲＭＳ速度差が第１所定速度（３５０mm/s）より大きいか否かを判断する。ステップＳ１９５で“ＹＥＳ”であれば、つまりＲＭＳ速度差が第１所定速度より大きければ、ステップＳ２０１に進む。なお、ステップＳ１９５の処理を実行するプロセッサ２０は第１速度判定手段として機能する。
【０１９３】
一方、ステップＳ１９５で“ＮＯ”であれば、つまりＲＭＳ速度差が第１所定速度以下であれば、ステップＳ１９７で実体ＩＸとロボットａｘとの現在の速度差が大きいか否かを判断する。つまり、ステップＳ１９３で読み出された実体ＩＸの位置履歴データおよびロボットａｘの状態履歴データから、実体ＩＸの速度およびロボットａｘの速度を算出して、現在の速度差を求める。そして、現在の速度差が、たとえば３５０mm/sより大きいか否かを判断する。ステップＳ１９７で“ＹＥＳ”であれば、つまり現在の速度差が３５０mm/sより大きければ、ステップＳ２０１に進む。
【０１９４】
また、ステップＳ１９７で“ＮＯ”であれば、つまり現在の速度差が３５０mm/s以下であれば、ステップＳ１９９で実体ＩＸとロボットａｘとが離れているか否かを判断する。つまり、ステップＳ１９９では、ステップＳ１９３で読み出された実体ＩＸの位置履歴データおよびロボットａｘの状態履歴データから、実体ＩＸの位置データおよびロボットａｘの位置を取得する。そして、実体ＩＸとロボットａｘとの距離が、たとえば１mより大きいか否かをステップＳ１９９で判断する。ステップＳ１９９で“ＮＯ”であれば、つまり実体ＩＸとロボットａｘとが離れていなければ、ステップＳ２０３に進む。なお、ステップＳ１９９の処理を実行するプロセッサ２０は、距離判定手段として機能する。
【０１９５】
一方、ステップＳ１９９で“ＹＥＳ”であれば、つまり実体ＩＸとロボットａｘとが離れていれば、ステップＳ２０１で追跡実体リストにおける、実体ＩＸのフラグをオフにする。たとえば、変数ＩＸが「１」であれば、実体Ｅａに対応するフラグの欄において、「０（オフ）」が設定される。
【０１９６】
続いて、ステップＳ２０３では、変数ＩＸをインクリメントする。つまり、次の実体ＩＤを指定するために、変数ＩＸはインクリメントされる。続いて、ステップＳ２０５では、変数ＩＸが変数Ｅｎより大きいか否かを判断する。つまり、全ての実体Ｅに対して、ステップＳ１９１からステップＳ２０１の処理が実行されたか否かを判断する。ステップＳ２０５で“ＮＯ”であれば、つまり全ての実体Ｅに対して、ステップＳ１９１からステップＳ２０１の処理が実行されていなければステップＳ１９１に戻る。一方、ステップＳ２０５で“ＹＥＳ”であれば、つまり全ての実体Ｅに対して、ステップＳ１９１からステップＳ２０１の処理が実行されていれば、ステップＳ２０７でフラグがオンの実体Ｅがあるか否かを判断する。つまり、追跡実体リストにおいて、フラグの列に「１」が設定された実体Ｅがあるか否かを判断する。
【０１９７】
ステップＳ２０７で“ＹＥＳ”であれば、つまり追跡実体リストにおいて、フラグの列に「１」が設定された実体Ｅがあれば、ステップＳ２０９で最も近い実体Ｅをロボットａｘに関連付ける。つまり、位置履歴バッファ３３２から、追跡実体リストにおいてフラグがオンの実体Ｅの現在の位置データを読み出す。また、状態履歴バッファ３３４から、ロボットａｘの最新の位置データを読み出す。そして、フラグがオンの実体Ｅのそれぞれと、ロボットａｘの距離を算出し、最も距離が短い実体Ｅとロボットａｘとを関連付ける。また、ステップＳ２０９で関連付けられた結果が関連付テーブルに記録される。
【０１９８】
たとえば、実体Ｅａおよび実体Ｅｂのフラグがオンであり、ロボットａｘがロボット１８ａであれば、実体Ｅａおよび実体Ｅｂのそれぞれと、ロボット１８ａとの距離が算出される。そして、ロボット１８ａと実体Ｅｂとの距離の方が短ければ、ロボット１８ａと実体Ｅｂとが関連付けられる。これにより、図１３に示す関連付テーブルのように、ロボット１８ａのロボットＩＤに対して、実体Ｅｂの実体ＩＤが記録される。なお、ステップＳ２０９の処理を実行するプロセッサ２０は第１関連付手段として機能する。
【０１９９】
一方、ステップＳ２０７で“ＮＯ”であれば、つまりフラグがオンの実体Ｅが追跡実体リストになければ、ステップＳ２１１でロボットａｘに実体Ｅを関連付けない。つまりステップＳ２１１が実行されると、関連付テーブルに対して実体ＩＤが記録されない。
【０２００】
そして、ステップＳ２０９またはステップＳ２１１の処理が終了すれば、ローカル関連付処理を終了して、関連付処理に戻る。
【０２０１】
図２６にはグローバル関連付プログラム３１８の処理を示すフロー図が示される。ここで、グローバル関連付処理において、ローカル関連付処理と同じ処理については、詳細な説明を省略する。
【０２０２】
ステップＳ１７５でグローバル関連付け処理が実行されると、ステップＳ２３１では、実体ＩＸが他のロボット１８と関連付けられているか否かを判断する。ステップＳ２３１で“ＹＥＳ”であれば、つまり実体ＩＸが他のロボット１８と関連付けられていれば、ステップＳ２３９に進む。一方、ステップＳ２３１で“ＮＯ”であれば、つまり実体ＩＸが他のロボット１８と関連付けられていなければ、ステップＳ２３３で実体ＩＸとロボットａｘとの３００秒間（第２時間）のＲＭＳ速度差（第２速度差）を算出する。なお、ステップＳ２３３の処理を実行するプロセッサ２０は第２速度差算出手段として機能する。
【０２０３】
続いて、ステップＳ２３５では実体ＩＸとロボットａｘとのＲＭＳ速度差が大きいか否かを判断する。たとえば、実体ＩＸとロボットａｘとのＲＭＳ速度差が第２所定速度（３５０mm/s）より大きいか否かを判断する。ステップＳ２３５で“ＹＥＳ”であれば、つまり実体ＩＸとロボットａｘとのＲＭＳ速度差が第２所定速度より大きければ、ステップＳ２３９に進む。なお、ステップＳ２３５の処理を実行するプロセッサ２０は、第２速度判定手段として機能する。
【０２０４】
一方、ステップＳ２３５で“ＮＯ”であれば、つまり実体ＩＸとロボットａｘとのＲＭＳ速度差が第２所定速度以下であれば、ステップＳ２３７で実体ＩＸとロボットａｘとの現在の速度差が大きいか否かを判断する。たとえば、現在の速度差が３５０mm/sより大きいか否かを判断する。
【０２０５】
ステップＳ２３７で“ＹＥＳ”であれば、つまり現在の速度差が３５０mm/sより大きければ、ステップＳ２３９で追跡実体リストにおける実体ＩＸのフラグをオンにして、ステップＳ２４１に進む。一方、ステップＳ２３７で“ＮＯ”であれば、つまり現在の速度差が３５０mm/s以下であれば、ステップＳ２４１で変数ＩＸをインクリメントし、ステップＳ２４３で変数ＩＸが変数Ｅｎより大きいか否かを判断する。ステップＳ２４３で“ＮＯ”であれば、つまり変数ＩＸが変数Ｅｎ以下であれば、ステップＳ２３１に戻る。一方、ステップＳ２４３で“ＹＥＳ”であれば、つまり変数ＩＸが変数Ｅｎよりも大きければ、ステップＳ２４５でフラグがオンの実体Ｅが１つだけか否かを判断する。つまり、ステップＳ２４５では、追跡実体リストのフラグの列において、「１」が設定された実体Ｅが１つだけであるか否かを判断する。
【０２０６】
ステップＳ２４５で“ＮＯ”であれば、ステップＳ２４７でフラグがオンの実体Ｅにロボットａｘを関連付ける。たとえば、ロボットａｘがロボット１８ｂを示し、追跡実体リストで実体Ｅｃのフラグのみがオンであれば、図１３に示す関連付テーブルのように、ロボットＩＤ「００２」に対して、実体ＩＤ００３が記録される。なお、ステップＳ２４７の処理を実行するプロセッサ２０は第２関連付手段として機能する。
【０２０７】
一方、ステップＳ２４５で“ＮＯ”であれば、つまり、フラグがオンの実体Ｅが１つだけでなければ、ステップＳ２１１と同様に、ステップＳ２４９でロボットａｘに実体Ｅを関連付けない。
【０２０８】
そして、ステップＳ２４７またはステップＳ２４９の処理が終了すれば、グローバル関連付け処理を終了して、関連付け処理に戻る。
【０２０９】
図２７には角度補正プログラム３１４ｄの処理を示すフロー図が示される。ステップＳ５５の角度補正処理が実行されると、プロセッサ２０は、ステップＳ２６１で次の位置を予測する。具体的には、ロボットｉｘの状態履歴データから、最新の位置および１秒前の位置を読み出し、縦（Ｙ）軸および横（Ｘ）軸方向の位置変位量を算出する。そして、ロボットｉｘのＲＴＭにおいて、前回の位置（前回位置）に対して、算出した位置変位量を加えることで、図１４に示すような予測位置を求める。
【０２１０】
続いて、ステップＳ２６３では、予測角度および検出角度を算出する。具体的には、予測角度は、ステップＳ２６１で求めた予測位置と前回位置との角度を、横軸を基準（０度）にして算出する。また、検出角度については、まず位置履歴バッファ３３２から、ロボットｉｘに関連付けられた実体Ｅにおける、現在の位置（データ）を読み出す。次に、ロボットｉｘに関連付けられた実体Ｅにおける、現在の位置と前回位置との角度を検出角度とし、横軸を基準にして算出する。続いて、ステップＳ２６５では、検出角度および予測角度から観測角度Ｚｋを算出する。たとえば、図１４に示すように、検出角度から予測角度を減算することで、観測角度Ｚｋを算出（観測）する。なお、ステップＳ２６１−Ｓ２６５の処理を実行するプロセッサ２０は角度観測手段として機能する。
【０２１１】
続いて、ステップＳ２６７では、観測における位置誤差量σ２ｐｏおよび変位量ｄｓに基づいて、観測角度誤差量σ２Ｚｋを近似する。つまり、数４に従って、角度誤差量σ２Ｚｋを、位置誤差量σ２ｐｏおよび変位量ｄｓの商から近似する。なお、ステップＳ２６７の処理を実行するプロセッサ２０は観測角度誤差算出手段として機能する。
【０２１２】
続いて、ステップＳ２６９では、前回値があるか否かを判断する。つまり、前回値バッファ３４２に前回の推定角度θ−ｋおよび前回の推定角度誤差量σ２θ-ｋが格納されているか否かを判断する。ステップＳ２６９で“ＮＯ”であれば、たとえば追跡サーバ１０が起動してから角度補正処理が実行されるのが１度目であれば、ステップＳ２７１で観測角度誤差量σ２Ｚｋと所定値とに基づいてカルマンフィルタを更新する。つまり、数５において、前回の推定角度誤差量σ２θ-ｋにかえて所定値（たとえば、０）を代入して、カルマンゲインＫｋを算出する。
【０２１３】
続いて、ステップＳ２７３では、所定値を利用して、推定角度誤差量σ２θ＾ｋおよび推定角度θ＾ｋを算出し、ステップＳ２７９に進む。つまり、ステップＳ２７３では、数６に示す式において、前回の推定角度θ−ｋに代えて所定値を代入して、推定角度θ＾ｋを算出する。また、数７に示す式において、前回の推定角度誤差量σ２θ-ｋに代えて所定値を代入して、推定角度誤差量σ２θ＾ｋを算出する。
【０２１４】
また、ステップＳ２６９で“ＹＥＳ”であれば、つまり前回値バッファ３４２に前回の推定角度θ−ｋおよび前回の推定角度誤差量σ２θ-ｋが格納されていれば、ステップＳ２７５で観測角度誤差量σ２Ｚｋと前回値とに基づいてカルマンフィルタを更新する。つまり、数５において、前回値バッファ３４２から読み出された前回の推定角度誤差量σ２θ-ｋを代入して、カルマンゲインＫｋを算出する。続いて、ステップＳ２７７では、前回値を利用して推定角度誤差量σ２θ＾ｋおよび推定角度θ＾ｋを算出する。つまり、ステップＳ２７７では、数６に示す式において、前回値バッファ３４２から読み出された前回の推定角度θ−ｋを代入して推定角度θ＾ｋを算出する。また、数７に示す式において、前回値バッファ３４２から読み出された前回の推定角度誤差量σ２θ-ｋを代入して推定角度誤差量σ２θ＾ｋを算出する。
【０２１５】
なお、ステップＳ２６１−Ｓ２７７の処理を実行するプロセッサ２０は、角度推定手段として機能する。
【０２１６】
続いて、ステップＳ２７９では、推定角度θ＾ｋと、ロボットｉｘのＲＴＭの角度との角度差を算出する。たとえば、ロボットｉｘのＲＴＭの角度が３２度であり、推定角度θ＾ｋが３０度であれば、角度差として−２度（＝３０−３２）が算出される。なお、ステップＳ２７９の処理を実行するプロセッサ２０は、角度補正算出手段として機能する。
【０２１７】
続いて、ステップＳ２８１では、推定角度誤差量σ２θ＾ｋを補正精度とし、算出された角度差を補正角度データとして一時記憶する。つまり、推定角度誤差量σ２θ＾ｋは補正精度バッファ３４０に格納され、算出された角度差は補正角度バッファ３３８に格納される。なお、ステップＳ２８１の処理を実行するプロセッサ２０は記憶手段として機能する。
【０２１８】
続いて、ステップＳ２８３では、推定角度θ＾ｋに基づいて、ロボットｉｘのＲＴＭの角度を更新する。たとえば、推定角度θ＾ｋが３０度であれば、ロボットｉｘのＲＴＭの角度が３０度にされる。そして、ステップＳ２８３の処理が終了すれば、角度補正処理を終了する。
【０２１９】
なお、角度差および推定角度誤差量σ２θ＾ｋには、ロボットｉｘのロボットＩＤが対応付けられる。つまり、補正角度バッファ３３８および補正精度バッファ３４０には複数の角度補正データおよび補正精度が格納されるため、それぞれを識別可能にするため、ロボットＩＤが対応付けられる。
【０２２０】
また、角度補正処理では、前回の補正角度データが今回の状態データに含まれる角度データに反映される。たとえば、前回の角度補正処理で補正角度データが−２度と算出され、かつロボット１８に送信されていなければ、今回の処理では、状態データに含まれる角度データから−２度が反映されたうえで実行される。ただし、補正角度データがロボット１８に送信された場合には、補正角度バッファ３３８はリセットされる。つまり、補正角度データがロボット１８に送信された場合に限り、前回の補正角度データが今回の状態データに含まれる角度データに反映される。
【０２２１】
この実施例によれば、ロボット自己位置同定システム１００は、ネットワーク４００を介して接続された追跡サーバ１０およびロボット１８を含む。追跡サーバ１０はＬＲＦ１２によって検出領域Ｆ内の実体Ｅをセンシングすることで実体Ｅを追跡し、ロボット１８は状態データを出力する。
【０２２２】
追跡サーバ１０は、ロボット１８が出力する状態データおよび検出された実体Ｅの位置データをメモリ２２のバッファに記憶する。また、追跡サーバ１０は関連付処理を実行し、記憶した位置履歴データおよび状態履歴データに基づいて、各ロボット１８と各実体Ｅとを関連付ける。さらに、追跡サーバ１０は、各ロボット１８に対応するＲＴＭを、位置履歴データおよび状態履歴データに基づいて更新し、そのＲＴＭに基づく補正データをロボット１８に送信する。そして、ロボット１８は、追跡サーバ１０が送信した補正データに基づいて、自身の位置および角度を補正する。
【０２２３】
このように、本実施例では、容易に設置可能なＬＲＦ１２およびロボット１８が一般的に出力可能な状態データを利用して、ロボット１８と実体Ｅとを関連付けることができるため、容易にロボット１８を自己位置同定することができる。
【０２２４】
なお、ロボット自己位置同定システム１００では、各サーバおよびロボット１８に設定される時刻が一定時間（たとえば、２０秒）毎に同期される。つまり、ロボット１８が出力する状態データおよび追跡サーバ１０が記憶する位置データに対応付けられる時刻データは、同時刻を示す。
【０２２５】
また、遠隔操作装置１４のオペレータが、ロボット１８の位置データを修正してもよい。この場合、ロボット１８は、追跡サーバ１０に対してリセット信号を送信する。そして、ロボット１８は、リセット信号を送信したときには、追跡サーバ１０が送信した補正データに基づいて位置および角度を補正しない。さらに、追跡サーバ１０は、リセット信号を受信した場合には、リセット信号が途絶した直後に取得した状態データに基づいてＲＴＭを補正する。つまり、オペレータによってロボット１８の位置が修正された場合には、追跡サーバ１０が送信する補正データが、一時的に無効にされる。
【０２２６】
また、ロボット自己位置同定システム１００には、ローカライゼーション機能専用のサーバが含まれていてもよい。さらに、ロボット自己位置同定システム１００は、遠隔操作装置１４および計画サーバ１６を含んでいなくてもよい。
【０２２７】
また、本実施例のロボット自己位置同定システム１００は、本実施例のロボット１８だけに限らず、様々な種類のロボットを同時に自己位置同定することができる。そのため、ロボット１８が出力する状態データの位置および角度などは、車輪速センサ１１２だけでなく、様々なセンサを利用して検出されてもよい。
【０２２８】
また、本実施例の数値（所定時間、第１所定速度、第２所定速度、第１速度差、第２速度差、第１時間、第２時間、所定距離など）は、ロボット自己位置同定システム１００が設置される環境に応じて、任意に変更されてもよい。
【０２２９】
また、カルマンフィルタ以外にも、パーティクルフィルタおよび拡張カルマンフィルタなど、ロボット１８の状態を推定することができるフィルタ（状態推定フィルタ）を利用して角度を補正することができる。
【０２３０】
また、他の実施例では、ＲＭＳの代わりに相関係数を利用して、ロボット１８と実体Ｅとを関連付けてもよい。
【符号の説明】
【０２３１】
１０  …追跡サーバ
１２ａ−１２ｆ  …ＬＲＦ
１８  …ロボット
２０  …プロセッサ
２２  …メモリ
２４  …通信ＬＡＮボード
２６  …無線通信装置
３２ａ，３２ｂ  …車輪
３６ａ，３６ｂ  …車輪モータ
１００  …ロボット自己位置同定システム
１１２  …車輪速センサ
１１４  …通信ＬＡＮボード
１１６  …無線通信装置
４００  …ネットワーク
【図１】
0005493097.tif 000009
【図２】
0005493097.tif 000010
【図３】
0005493097.tif 000011
【図４】
0005493097.tif 000012
【図５】
0005493097.tif 000013
【図６】
0005493097.tif 000014
【図７】
0005493097.tif 000015
【図８】
0005493097.tif 000016
【図９】
0005493097.tif 000017
【図１０】
0005493097.tif 000018
【図１１】
0005493097.tif 000019
【図１２】
0005493097.tif 000020
【図１３】
0005493097.tif 000021
【図１４】
0005493097.tif 000022
【図１５】
0005493097.tif 000023
【図１６】
0005493097.tif 000024
【図１７】
0005493097.tif 000025
【図１８】
0005493097.tif 000026
【図１９】
0005493097.tif 000027
【図２０】
0005493097.tif 000028
【図２１】
0005493097.tif 000029
【図２２】
0005493097.tif 000030
【図２３】
0005493097.tif 000031
【図２４】
0005493097.tif 000032
【図２５】
0005493097.tif 000033
【図２６】
0005493097.tif 000034
【図２７】
0005493097.tif 000035