JP2014178789A - 追跡装置、追跡プログラムおよび追跡方法 - Google Patents

Abstract

【構成】 追跡システム１０は、移動体１２を含み、移動体は或る環境内を移動する。このとき、移動体に設けられた距離センサ２２ａ、２２で距離データが検出され、移動体の各車輪１６ａ、１６ｂの回転数データも検出される。コンピュータ（３０）は、回転数データに基づいて移動体の位置および向きを算出し、検出した距離データを、距離を計測された点の位置データに変換する。この位置データを所定のルールでクラスタリングし、各クラスタの特徴量に基づいて各クラスタが静的物体または動的物体に対応するかどうかを判別する。したがって、静的物体であることが判別されたクラスタに含まれる位置データに対応する距離データを用いて移動体の位置が推定される。
【効果】 環境における移動体の位置を正しく推定して、当該移動体が適用されるロボットに位置に応じた所定のサービスを正しく提供させることができる。
【選択図】 図１

Description

この発明は追跡装置、追跡プログラムおよび追跡方法に関し、特にたとえば、距離センサを備える移動体を用いて人を追跡する、追跡装置、追跡プログラムおよび追跡方法に関する。

この発明の背景技術の一例が非特許文献１に開示される。この非特許文献１では、距離センサ（レーザレンジファインダ）を用いるとともに、センサ空間における足の見え方をテンプレートとして格納しておき、テンプレート群とセンサデータとのブロックマッチングにより人の追跡を行う。

奥迫伸一，坂根茂幸：レーザレンジファインダを用いた移動ロボットによる人の追跡, 日本ロボット学会誌，Vol.24, No.5, pp.605-613, 2006

しかし、この背景技術では、移動ロボットを基準にロボット座標系において人を追跡するため、現実空間（世界座標系）における移動ロボットおよび人間の位置を正しく知ることができない。たとえば、移動ロボットに或る環境において道案内のようなサービスを提供させる場合には、移動ロボットや人間の位置を正しく知る必要があるが、背景技術の追跡方法では、このようなサービスを移動ロボットに提供させることは考慮されていないため、正しくサービスを提供することができない。

それゆえに、この発明の主たる目的は、新規な、追跡装置、追跡プログラムおよび追跡方法を提供することである。

この発明の他の目的は、現実空間において人を追跡する物体の位置を正しく推定することができる、追跡装置、追跡プログラムおよび追跡方法を提供することである。

第１の発明は、距離センサを設置し環境内を移動する移動体を用いて当該環境内に存在する人間を追跡する追跡装置であって、距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出手段、位置データ算出手段によって算出された位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリング手段、クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断手段、条件判断手段の判断結果に応じて複数のクラスタの各々が動的物体または静的物体に対応するかどうかを判別する判別手段、環境についての地図データと、判別手段によって静的物体に対応することが判別されたクラスタに含まれる位置データに対応する距離データを用いて、移動体の位置を推定する移動体位置推定手段、判別手段の判別結果に基づいて環境内に存在する１または複数の人間を検知する検知手段、検知手段によって検知された１または複数の人間のうち、１人の人間を選択する選択手段、および選択手段によって選択された人間に移動体を追従させる追従手段、追跡装置である。

第１の発明では、追跡装置は距離センサを設置し環境内を移動する移動体を用いて当該環境内に存在する人間を追跡する。位置データ算出手段は、距離センサで検出された距離データから、距離を計測された点の位置データを算出する。たとえば、位置データは、距離を計測したときの移動体の位置を基準しても算出してもよいし、移動体の移動開始位置を基準として算出してもよい。クラスタリング手段は、位置データ算出手段によって算出された位置データを所定のルールに従って複数のクラスタにクラスタリングする。条件判断手段は、クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する。たとえば、所定の条件は、クラスタの特徴についての条件であり、より具体的には、人間のような動的物体であるか、壁や柱のような静的物体であるかを判別するための条件である。判別手段は、条件判断手段の判断結果に応じて複数のクラスタの各々が動的物体または静的物体に対応するかどうかを判別する。移動体位置推定手段は、環境についての地図データと、判別手段によって静的物体に対応することが判別されたクラスタに含まれる位置データに対応する距離データを用いて、移動体の位置を推定する。検知手段は、前記判別手段の判別結果に基づいて環境内に存在する１または複数の人間を検知する。つまり、検知手段は、動的物体であることが判別されたクラスタを人間として検知する。選択手段は、検知手段によって検知された１または複数の人間のうち、１人の人間を選択する。そして、追従手段は、選択手段によって選択された人間に移動体を追従させる。

第１の発明によれば、移動体に設置した距離センサで距離を計測された点に対応する位置データを所定のルールに従って複数のクラスタにクラスタリングし、各クラスタが所定の条件を満たすかどうかを判別するだけで、静的物体についてのクラスタに含まれる位置データに対応する距離データを用いて移動体の位置を推定するので、人間のような動的物体が存在する環境においても、移動体の位置を出来る限り正しく推定することができる。したがって、移動体が適用されるロボットに、正しい位置に基づいて所定のサービスを提供させることができる。

第２の発明は、第１の発明に従属し、検知手段によって検知された１または複数の人間の位置を推定する人間位置推定手段をさらに備える。

第２の発明では、人間位置推定手段は、検知された１または複数の人間の位置を例えばパーティクルフィルタを用いて推定する。

第２の発明によれば、追従する人間の位置を正しく推定することにより、移動体をより正確に追従させることができる。

第３の発明は、第１または第２の発明に従属し、所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることである。

第３の発明では、所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることであり、近くに有る点が同じクラスタに分類される。

第３の発明によれば、点同士の距離を検出するだけなので、簡単にクラスタリングすることができる。

第４の発明は、第１ないし第３の発明のいずれかに従属し、所定の条件は、クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含む。

第４の発明では、所定の条件は、クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含み、クラスタに含まれる点が少ない場合には、当該クラスタに含まれる点（位置データ）に対応する距離データは消去される。たとえば、クラスタに含まれる点の数が少ない場合には、動的物体であるか、静的物体であるかを判別することができないため、当該クラスタは消去されるのである。

第４の発明によれば、動的物体であるか、静的物体であるかを判別できない不要なデータを消去することができる。

第５の発明は、第１ないし第４の発明のいずれかに従属し、クラスタリング手段によってクラスタリングされた複数のクラスタの各々の特徴を抽出する抽出手段をさらに備え、条件判断手段は、抽出手段によって抽出された特徴が所定の条件を満たすかどうかを判断する。

第５の発明では、抽出手段は、複数のクラスタの各々の特徴を抽出する。条件判断手段は、各クラスタの特徴が所定の条件を満たすかどうかを判断する。

第５の発明によれば、クラスタに含まれる点の個数以外の特徴によって所定の条件を満たすかどうかを判断することができる。

第６の発明は、第５の発明に従属し、抽出手段によって抽出された特徴は、クラスタに含まれる位置データが示す複数の点の分散についての特徴量である。

第６の発明では、クラスタの特徴は、当該クラスタに含まれる位置データが示す複数の点の分散についての特徴量であり、たとえば、クラスタの形状に関する特徴である。

第６の発明によれば、クラスタの形状に関する特徴が所定の条件を満たすかどうかで、動的物体であるか、静的物体であるかを判別することができる。

第７の発明は、第６の発明に従属し、特徴量は、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値であり、所定の条件は、固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む。

第７の発明では、特徴量は、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値である。たとえば、特徴量は、クラスタに含まれる複数の点の分散についての主成分であり、分散行列の固有値で表される。たとえば、所定の条件は、固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む。したがって、固有値の最大値が第１所定値以下である場合、または、２番目に大きい固有値が第２所定値以上である場合に、所定の条件を満たさないことが判断され、当該固有値を有するクラスタが消去される。

第７の発明によれば、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値に基づいて不要なデータを消去することができる。

第８の発明は、距離センサを設置し環境内を移動する移動体を用いて当該環境内に存在する人間を追跡するコンピュータの追跡プログラムであって、コンピュータに、距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出ステップ、位置データ算出ステップにおいて算出した位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリングステップ、クラスタリングステップにおいてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断ステップ、条件判断ステップの判断結果に応じて複数のクラスタの各々が動的物体または静的物体に対応するかどうかを判別する判別ステップ、環境についての地図データと、判別ステップにおいて静的物体に対応することを判別したクラスタに含まれる位置データに対応する距離データを用いて、移動体の位置を推定する移動体位置推定ステップ、判別ステップの判別結果に基づいて環境内に存在する１または複数の人間を検知する検知ステップ、検知ステップにおいて検知した１または複数の人間のうち、１人の人間を選択する選択ステップ、および選択ステップにおいて選択した人間に移動体を追従させる追従ステップを実行させる、追跡プログラムである。

第９の発明は、距離センサを設置し環境内を移動する移動体を用いて当該環境内に存在する人間を追跡するコンピュータの追跡方法であって、コンピュータは、（ａ）距離センサで検出された距離データから、距離を計測された点の位置データを算出し、（ｂ）ステップ（ａ）において算出した位置データを所定のルールに従って複数のクラスタにクラスタリングし、（ｃ）ステップ（ｂ）においてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断し、（ｄ）ステップ（ｃ）の判断結果に応じて複数のクラスタの各々が動的物体または静的物体に対応するかどうかを判別し、（ｅ）環境についての地図データと、ステップ（ｄ）において静的物体に対応することを判別したクラスタに含まれる位置データに対応する距離データを用いて、移動体の位置を推定し、（ｆ）ステップIｄ）の判別結果に基づいて環境内に存在する１または複数の人間を検知し、（ｇ）ステップ（ｆ）において検知した１または複数の人間のうち、１人の人間を選択し、そして（ｈ）ステップ（ｇ）において選択した人間に移動体を追従させる、追跡方法である。

第８および第９の発明においても、第１の発明と同様に、人間を追跡する移動体の位置を出来る限り正しく推定することができる。

この発明によれば、移動体に設置した距離センサで距離を計測された点に対応する位置データを所定のルールに従って複数のクラスタにクラスタリングし、各クラスタが所定の条件を満たすかどうかを判別するだけで、静的物体についてのクラスタに含まれる位置データに対応する距離データを用いて移動体の位置を推定するので、人間のような動的物体が存在する環境においても、移動体の位置を出来る限り正しく推定することができる。したがって、移動体が適用されるロボットに、正しい位置に基づいて所定のサービスを提供させることができる。

この発明の上述の目的，その他の目的，特徴および利点は、図面を参照して行う以下の実施例の詳細な説明から一層明らかとなろう。

図１はこの発明の追跡システムの外観構成の一例を示す図解図である。 図２は図１に示す追跡システムの電気的な構成の一例を示すブロック図である。 図３は図１および図２に示す距離センサの検出範囲を説明するための図解図である。 図４は移動体の位置および向きを算出するためのパラメータを説明するための図解図である。 図５はグリッドマップの一例を示す図解図である。 図６は動的物体と静的物体を判別する方法を説明するための図解図である。 図７は動的物体と静的物体を判別する方法を説明するための他の図解図である。 図８は動的物体と静的物体を判別する方法を説明するためのその他の図解図である。 図９は動的物体と静的物体を判別する方法を説明するためのさらに他の図解図である。 図１０は静的物体を判定する具体的な場合を説明するための図解図である。 図１１は動的物体と静的物体を判別した結果、静的物体以外の点を削除した場合の一例を示す図解図である。 図１２は人間の位置を推定する方法を説明するための図解図である。 図１３は図１２に後続し、人間の位置を推定する方法を説明するための図解図である。 図１４は図２に示すＲＡＭのメモリマップの一例を示す図解図である。 図１５は図２に示すＣＰＵの全体処理の一部を示すフロー図である。 図１６は図２に示すＣＰＵの全体処理の他の一部であって、図１５に後続するフロー図である。 図１７は図２に示すＣＰＵのデータ判別処理の一部を示すフロー図である。 図１８は図２に示すＣＰＵのデータ判別処理の他の一部であって、図１７に後続するフロー図である。 図１９は図２に示す人間位置推定処理を示すフロー図である。 図２０は図２に示す追従処理を示すフロー図である。

図１（Ａ）を参照して、この実施例の追跡システム１０は、電気車椅子のような移動体１２を含み、移動体１２は、その下部に、左右の前輪（キャスター）１４ａ、１４ｂおよび左右の後輪１６ａ、１６ｂを備えている。また、移動体１２の左側の枠の上部には操作レバー１８が設けられる。さらに、移動体１２の左側の枠であり、左足を置くステップ２０ａの左側に、距離センサ２２ａが取り付けられる。ただし、距離センサ２２ａは、移動体１２の右側の枠であり、右足を置くステップ２０ｂの右側に取り付けてもよい。また、移動体１２を上方から見た模式図である図１（Ｂ）に示すように、移動体１２の後方に、距離センサ２２ｂが設けられる。

なお、この実施例では、距離センサ２２ａおよび２２ｂを移動体１２の前後に取り付けるようにしてあるが、移動体１２の左右に取り付けるようにしてもよい。このように２つの距離センサ２２ａ、２２ｂを移動体１２の前後または左右に設けるのは、移動体１２の周りを全周（３６０°）に渡って、距離を検出（計測）するためである。ただし、これは単なる一例であり、距離センサは１つでもよく、または、３つ以上設けてもよい。

また、この実施例では、距離センサ２２ａおよび２２ｂは、或る環境内の壁、柱のような固定物（静的物体）のみならず、人間のような移動物（動的物体）を検出（判別）する。このため、後述するように、距離センサ２２ａ、２２ｂを、その検出位置が地面ないし床面から所定の高さになるように設置してある。この実施例では、所定の高さは１１ｃｍである。これは、人間の脚（足首辺り）を検出するためである。

図１（Ａ）に戻って、移動体１２の座席シートの下側であり、左の後輪１６ａと右の後輪１６ｂの間には、ボックス２４が設けられる。この中に、後述するコンピュータ３０、入出力インターフェイス（以下、単に「インターフェイス」という。）３２、モータドライバ３４ａ、３４ｂ、モータ３６ａ、３６ｂおよびエンコーダ３８ａ、３８ｂが設けられる（図２参照）。ただし、コンピュータ３０およびインターフェイス３２は、ボックス２４の外に設けるようにしてもよい。

図２は図１（Ａ）および（Ｂ）に示した追跡システム１０の電気的な構成を示すブロック図である。図２に示すように、追跡システム１０は、移動体１２の追跡装置として機能するコンピュータ３０を含む。コンピュータ３０は、汎用のパーソナルコンピュータやワークステーションなどのコンピュータであり、ＣＰＵ３０ａ、ＲＡＭ３０ｂおよびＨＤＤ３０ｃなどのコンポーネントを備える。

また、コンピュータ３０には、上述した操作レバー１８および距離センサ２２ａ、２２ｂが接続されるとともに、インターフェイス３２が接続される。インターフェイス３２には、モータドライバ３４ａを介してモータ３６ａが接続されるとともに、モータドライバ３４ｂを介してモータ３６ｂが接続される。また、インターフェイス３２には、エンコーダ３８ａおよび３８ｂが接続される。

図示は省略するが、モータ３６ａの回転軸と左の後輪１６ａの回転軸がギアを用いて連結され、モータ３６ｂの回転軸と右の後輪１６ｂの回転軸がギアを用いて連結される。

コンピュータ３０は、この実施例の追跡システム１０の全体的な制御を司る。この実施例では、ＣＰＵ３０ａは、操作レバー１８からの操作入力に応じてモータ３６ａ、３６ｂの駆動を制御したり、つまり移動体１２の移動を制御したり、距離センサ２２ａ、２２ｂおよびエンコーダ３８ａ、３８ｂで検出された距離データや回転数データを取得および記憶したりする。また、コンピュータ３０は、移動体１２が移動する（所定のサービスを提供する）環境についての地図（地図データ）と、検出したデータに基づいて、移動体１２の位置を推定（決定）する。

操作レバー１８は、ユーザによって操作され、操作に応じた信号（操作入力）をコンピュータ３０に与える。たとえば、前進、後退、停止、左折、右折および旋回などの操作を行うことができる。

ただし、リモートコントローラ（図示せず）を用いて、遠隔操作により、操作入力をコンピュータ３０に与えることもできる。また、移動体１２は、自律航行（自律移動）可能である。

距離センサ２２ａ、２２ｂは、たとえば、汎用のレーザレンジファインダ（ＬＲＦ）であり、レーザーを照射し、物体に反射して戻ってくるまでの時間から当該物体との距離を計測するものである。たとえば、トランスミッタ（図示せず）から照射したレーザーを回転ミラー（図示せず）で反射させて、扇状に一定角度（たとえば、０．２５度）ずつスキャンする。また、この実施例では、検出範囲（図３参照）の全体をスキャンする周期は、２５ｍｓｅｃである。

図３は、距離センサ２２ａ、２２ｂの検出範囲（計測範囲）を説明するための図解図である。この実施例の距離センサ２２ａ、２２ｂの計測範囲は、半径Ｒ（Ｒ≒１０〜１５ｍ）の扇形状で示される。扇の角度は、正面方向を中心として左側および右側のそれぞれに１３５°である。つまり、正面を中心とする２７０°の範囲について距離を検出することができる。

図２に戻って、モータドライバ３４ａ、３４ｂは、コンピュータ３０からの指示に基づいて、モータ３６ａ、３６ｂを駆動する。エンコーダ３８ａは、モータ３６ａの回転数（ｒｐｓ）を検出し、回転数についてのデータ（回転数データ）を、インターフェイス３２を介してコンピュータ３０に入力する。同様に、エンコーダ３８ｂは、モータ３６ｂの回転数を検出し、回転数についての回転数データを、インターフェイスを介してコンピュータ３０に入力する。

コンピュータ３０は、距離センサ２２ａ、２２ｂで検出される距離データと、エンコーダ３８ａ、３８ｂで検出される回転数データを取得して、ＲＡＭ３０ｂ（ＨＤＤ３０ｃでもよい。）に記憶する。ただし、距離センサ２２ａで検出される距離データと距離センサ２２ｂで検出される距離データは区別されるとともに、同じ時点において検出された距離データと回転数データは互いに関連付けられる。

たとえば、移動体１２は、身振り手振りのような身体動作および発話の少なくとも一方を用いたコミュニケーションを実行可能なロボットなどに適用され、当該ロボットは、自身が適用される環境（ショッピングモール、空港やイベント会場など）において、特定の人間の買い物を手伝ったり、道案内、接客、商品や展示品を説明（案内）したりなどの所定のサービスを提供する。つまり、買い物用や荷物運搬用のカートを押すロボットやガイドロボットとして機能する。また、特定の人間が車椅子を使用する場合には、当該車椅子を押すロボットとして機能する場合もある。

たとえば、本件の出願人等が開発するとともに、製造販売するコミュニケーションロボット「Ｒｏｂｏｖｉｅ（ロボビー）（登録商標）」を用いることができる。ロボットは、自身が適用される環境内において特定の人間を追従したり、移動した位置（場所）に応じた所定のサービスを提供したりする。したがって、ロボット（移動体１２）は、環境内における自身の位置および特定の人間の位置を知る必要がある。

また、ロボットが上記の所定のサービスを提供する場合には、たとえば、ロボット（移動体１２）のスタート位置および向き（初期位置および初期の向き）、通過点および終点は外部のコンピュータから与えられ、ロボット（移動体１２）は初期位置から移動を開始する。ただし、スタート位置、通過点および終点は時系列に従って指示される。また、外部のコンピュータは、ロボット（移動体１２）の管理者ないし遠隔操作を行う者が操作するコンピュータであって、ロボット（移動体１２）と通信可能に設けられる。以下、この実施例において、同じである。

したがって、この実施例の追跡システム１０では、環境内（世界座標系）における移動体１２の位置を推定し、また移動体１２の距離センサ２２a、２２ｂで検知可能な人間の位置を推定する。また、追跡システム１０では、知された１または複数の人間から１の特定の人間を選択する。そして、選択した１人の人間を追従するように移動経路を決定（算出）し、算出した移動経路に従って（沿って）移動体１２を移動させる。そして、移動体１２（ロボット）は、移動中または／および通過点や終点において選択した１人の人間に所定のサービスを提供する。

この実施例では、パーティクルフィルタを用いて、移動体１２の位置および環境内に存在し、検知された１または複数の人間の位置が推定される。

ここで、パーティクルフィルタとは、多数の粒子を用いて、前時刻の状態からの予測と現在の観測情報から現在の状態を推定する手法である。パーティクルフィルタでは、予測、観測、リサンプリングを繰り返すことによって移動体１２および人間を追跡する。ただし、移動体１２を追跡するためのパーティクルフィルタと、人間を追跡するためのパーティクルフィルタはそれぞれ独立している。また、複数の人間を追跡する場合には、各人間用のパーティクルフィルタが設けられる。

なお、この実施例では、検知した人間の位置を推定することと、選択された１人の人間に追従して移動体１２を移動させることを、それぞれ追跡ということがある。つまり、パーティクルフィルタを用いたソフトウェアにより人間を追跡するとともに、移動体１２の移動により選択された１人の人間を追跡するのである。

まず、移動体１２を追跡する場合について説明する。
＜予測＞
時刻ｔ−１のパーティクルｉから時刻ｔにおけるパーティクルｉの位置（方向を含む“状態”）を予測する。この実施例では、移動体１２の状態ベクトルはｘ＝［ｘ，ｙ，θ］^Ｔであり、位置（ｘ，ｙ）および向き（θ）で表される。また、パーティクルフィルタはｎ個（この実施例では、４００個）のパーティクルを含み、ｉ番目のパーティクルの状態ベクトルはｘ^ｉ＝［ｘ^ｉ，ｙ^ｉ，θ^ｉ］^Ｔで表される。この実施例では、時刻ｔにおけるパーティクルｉの状態ベクトルｘⁱ _tは、数１に従って算出される。

［数１］
ｘⁱ _t＝ｆ（ｘⁱ _t-1，ｕ_t-1，Δｔ）
ただし、数１に示すｕは移動体１２の動作情報ｕ＝［ｖ,ω］^Tであり、数２に従って算出される。ここで、ｖは移動体１２の速度であり、ωは車輪１６ａおよび１６ｂの車軸の中心（図４参照）であり、車軸と垂直な軸周りの角度速である。また、Ｖ_L、Ｖ_Rは移動体１２の左右の車輪（１６ａ、１６ｂ）の移動速度であり、Ｄは移動体１２の車輪（１６ａ、１６ｂ）間距離である。

［数２］

左右の車輪（１６ａ、１６ｂ）の移動速度Ｖ_L、Ｖ_Rは、左右の車輪（１６ａ、１６ｂ）の回転数から算出することができ、また、車輪間隔Ｄは予め知ることができる。ただし、回転数に応じた移動速度Ｖ_L、Ｖ_Rのテーブルを用意しておき、計算せずに、テーブルを参照することにより移動速度Ｖ_L、Ｖ_Rを取得してもよい。

そして、算出された動作ｕ＝［ｖ,ω］^Tを積分することにより、前回算出した移動体１２の位置（ｘ_t-1,ｙ_t-1）および向きθ_t-1からの移動距離ｄと回転角度αを求めることができる。ただし、初期位置（ｘ₀，ｙ₀）および初期の向きθ₀は外部から与えられる。したがって、上記の数１は数３のように表すことができる。

［数３］
ｘⁱ _t＝ｘⁱ _t-1＋ｖ・ｃｏｓ（θ_t-1）・Δｔ
ｙⁱ _t＝ｙⁱ _t-1＋ｖ・ｓｉｎ（θ_t-1）・Δｔ
θⁱ _t＝θⁱ _t-1＋ω・Δｔ
このように、前時刻ｔ−１における各パーティクルｉの状態ｘⁱ _t-1から時間ｔにおける各パーティクルｉの状態ｘⁱ _tが予測される。
＜観測＞
予測された位置における各パーティクルｉの尤度および重要度（重み）ｗⁱ _tを算出する。この実施例では、尤度は図５に示すようなグリッドマップについての地図データを用いて算出する。ただし、図５では、移動体１２が移動する環境の一部についてのグリッドマップを示す。図５を参照して分かるように、グリッドマップは、複数のグリッド線が縦横に等間隔で描画されることにより生成される。詳細な説明は省略するが、グリッドマップは、移動体１２が移動する環境（現実空間）を真上方向から俯瞰的に見た場合の２次元のマップである。つまり、グリッドマップは、世界座標系で作成されている。

たとえば、グリッドマップでは、グリッド線の間隔は現実空間における所定の長さ（５〜１０ｃｍ）に相当する。また、壁や柱などの静的物体に相当する部分（距離センサ２２ａ、２２ｂで距離を計測可能な表面部分）が描画されたグリッドに対応して、静的物体であることの情報が記憶されている。図５では、静的物体であることの情報が記憶されているグリッドに黒色を付してある。

上述したように推定された時刻ｔにおける各パーティクルｉの状態ｘⁱ _tにおけるグリッドマップ上の計測結果（シュミレーション結果）が、実際の距離センサ２２ａ、２２ｂの計測結果（計測値ｚ_t）と等しくなる確率（尤度）が数４に従って求められる。

［数４］
ｗⁱ _t=(ｚⁱ _t｜ｘⁱ _t）
ｚⁱ _t＝hc／Total
ただし、この実施例では、計測値ｚ_tは、１周期にスキャンするレーザーの総数Total（この実施例では、１０８０）のうち、実際に距離を計測したレーザーの数hcについての割合である。ただし、距離センサ２２ａ、２２ｂのそれぞれについて、計測値ｚ_tが求められ、尤度もそれぞれの距離センサ２２ａ、２２ｂについて求められる。

そして、数５に示すように、各パーティクルｉの重みｗⁱ _tは、すべてのパーティクルｉの尤度合計で各パーティクルｉの尤度を正規化することにより、算出（決定）される。ただし、数５において、ｎはパーティクルｉの総数（この実施例では、４００）である。また、数５では、“＝”は代入を意味する。

［数５］

この実施例では、尤度が重みｗⁱ _tとして決定されるが、これに限定される必要はない。尤度の大きさに比例して重みｗⁱ _tを決定してもよいし、尤度の最も高いパーティクルｉから順に重みｗⁱ _tを線形的に変化させるように決定してもよいし、一定の閾値毎に重みｗⁱ _tを段階的に変化するように決定してもよい。

ただし、各パーティクルｉについては、グリッドマップ上の状態ｘⁱ _tが示す位置（ｘ_t，ｙ_t）において、距離センサ２２ａ、２２ｂと同様に、状態ｘⁱ _tが示す向きθ_tを中心に左右１３５度の範囲で０．２５度ずつスキャンした場合に、レーザーが静的物体であることの情報が記憶されたグリッドに当った数が検出され、スキャンした総数（１０８０）における当った数の割合が算出される。ただし、レーザー光が届くのは距離Ｒである。
＜リサンプリング＞
観測により求めた各パーティクルｉの重みｗⁱ _tに比例した確率で復元抽出によりパーティクルｉを選択し、時刻ｔでのパーティクルｉの集合を求める。したがって、重みｗⁱ _t（尤度）の小さい（低い）パーティクルｉは選択されずに消滅し、重みｗⁱ _t（尤度）の大きい（高い）パーティクルｉは何度も選択され仲間を増やす（分裂する）。リサンプリングされたパーティクルｉが次の推定に用いられる。ただし、この実施例では、各パーティクルｉの平均座標が移動体１２の位置(ｘ_t，ｙ_t）として推定され、各パーティクルｉの平均の向きが移動体１２の向き（θ_t）として推定される。

このように移動体１２の位置を推定（修正）するのは、回転数データに基づいてジオメトリ計算（数２、数３）により、初期位置（ｘ₀，ｙ₀）および初期の向き（θ₀）から累積的に時間毎の移動体１２の位置および向きを求める場合には、車輪１６ａ、１６ｂの滑りの発生等の要因により、誤差が発生するからである。実験した環境下においては、移動体１２が３０ｃｍ進む毎に約５％のずれ（誤差）が発生した。

また、このような追跡システム１０は、ショッピングモール、空港やイベント会場など、人間、カートや台車のような動的物体が存在する環境において移動する（所定のサービスを提供する）移動体１２の位置を推定するため、この場合には、距離センサ２２ａ、２２ｂで検出された距離データのうち、人間などの動的物体を計測した場合の距離データは使用されない。このような動的物体の情報については、地図データ（グリッドマップ）に記憶されていないからである。

そこで、この実施例では、距離データに基づいて、壁、柱などの静的物体（環境）を計測した距離データおよび環境に存在する人間または人間と思われる動的物体を計測した距離データを判別する。以下、静的物体についての距離データおよび動的物体についての距離データを判別する方法について説明する。

たとえば、図６には、壁の前に２人の人間が存在し、そのような状況において、移動体１２が壁や人間に対面し、移動体１２に設けられた距離センサ２２ａで距離を検出する様子が示される。ただし、図６では、壁、人間および移動体１２を上方から見た図である。また、ここでは、簡単のため、距離センサ２２ａの計測範囲は、正面を中心とする約９０°の範囲であり、その計測範囲を約３°ずつスキャンした場合について示してある。さらに、図６では、検出（計測）される距離は点線で示してある。また、上述したように、距離センサ２２ａは、床ないし地面から所定の高さに設置され、したがって、人間については、その脚（足首辺り）との距離を計測する。

図６に示すような場合について、静的物体についての距離データおよび動的物体についての距離データを判別する方法について説明する。この実施例では、まず、移動体１２に搭載された距離センサ２２ａ、２２ｂのそれぞれで計測された距離データから、当該距離データを検出したときの移動体１２の位置および向きを基準とする２次元の座標系（移動体座標系）の位置（点）データが算出される。このとき、２つの距離センサ２２ａ、２２ｂの取り付け位置が考慮される。つまり、移動体１２の位置と、距離センサ２２ａ、２２ｂの取り付け位置のずれが調整される。当然のことではあるが、距離データを検出したときの距離センサ２２ａ、２２ｂの計測範囲における角度も考慮される。つまり、距離センサ２２ａ、２２ｂで検出された位置（点）について、移動体１２を基準とした極座標のデータが取得されるのである。ただし、この実施例では、世界座標系における移動体１２の位置（ｘ_t,ｙ_t）および向きθ_tは所定時間（たとえば、３０秒）毎に推定（更新）されるため、距離データは、推定された現実空間上の位置（ｘ_t,ｙ_t）および向きθ_tに基づいて、２次元の世界座標系の位置データに変換される。

この変換された位置データに対応する位置（点）を所定のルールに従ってクラスタリングし、クラスタリングされた点群（クラスタ）の特徴から壁や柱のような静的物体および人間のような動的物体を判別する。そして、静的物体であることが判別されたクラスタに含まれる位置（点）データに対応する距離データを用いて移動体１２の位置が推定される。一方、動的物体であることが判別されたクラスタに含まれる位置（点）データに対応する距離データは移動体１２の位置を推定する場合には使用されない。ただし、クラスタに含まれる点の数が所定数ＮＰ（この実施例では、５）よりも少ない場合には、動的物体であるか静的物体であるかを判別するのが困難であるため、このようなクラスタに含まれる位置データに対応する距離データについては消去するようにしてある。このクラスタに含まれる位置データ（点）の数もクラスタの特徴の１つである。

クラスタリングでは、距離が所定値ＴＨ（この実施例では、１０ｃｍ）未満である位置（点）同士が同じクラスタに分類される。具体的には、着目する点との距離が最も近い（短い）点を検出し、その距離が所定値ＴＨ未満であるかどうかを判断する。そして、距離が所定値ＴＨ未満である場合には、検出した点を着目する点と同じクラスタに分類する。一方、距離が所定値ＴＨ以上である場合には、検出した点を着目する点とは異なる新しいクラスタに分類する。そして、新しいクラスタに分類した点を着目する点として、上記のようなクラスタリングを行う。このようにして、クラスタリングはすべての点について行われる。

図７は、図６において移動体１２の距離センサ２２ａによって距離を計測された点（位置）を白丸印で示す。ただし、図７では、距離を計測された点（位置）を分かり易く示すために、壁と人間を省略してある。このような点が上述した方法により、クラスタリングされる。ただし、上述したように、実際には、世界座標系に座標変換された位置データについてクラスタリングが行われる。

図８は、図７に示す各点（位置）について、上述した方法により、クラスタリングを行った場合に生成されたクラスタを示す。図８から分かるように、たとえば、７つのクラスタが生成される。図８（図９および図１０も同じ。）では、同じクラスタに分類されていることを示すために点を囲んである。

クラスタリングを終えると、各クラスタについての特徴を抽出（算出）する。この実施例では、クラスタの特徴は、当該クラスタに含まれる複数の点の分散についての主成分であり、分散行列についての固有値λ_１，λ_２である。ただし、λ_１は分散行列についての固有値の中で最も大きい固有値であり、λ_２は固有値λ_１の次に（２番目に）大きい固有値である。図９では、分かり易く示すために、クラスタの特徴（分散の主成分）を実線で示し、クラスタに含まれる点を省略してある。

上述したように、クラスタに含まれる点の数が所定数ＮＰ未満であるクラスタについては、動的物体か静的物体かを判別することが困難であるため、固有値を算出するまでも無く、消去される。したがって、図８および図９に示すように、点の数が４つであるクラスタ４は消去される。図９（図１１も同じ。）では、クラスタ４が消去されたことを、当該クラスタ４に含まれる点を囲む線を点線で記載することにより表現している。ただし、実際には、クラスタに含まれる点（位置データ）に対応する距離データが消去される。

固有値λ_１，λ_２を算出する場合には、まず、各クラスタでは、クラスタに含まれる複数の点についての重心（または平均）（μ_x，μ_y）が数６に従って算出される。ただし、クラスタにｎ個の点が含まれているものとする。

［数６］

算出された重心（μ_x，μ_y）を基準とした場合についてのクラスタの分散行列Σは数７で示され、その固有値が数８に従って算出される。

［数７］
Σ_ij = E[(X_i −μ_i)(X_j −μ_j)]=E(X_iX_j)−E(X_i)E(X_j)
ただし、μ_i=E(X_i)であり、この実施例では、２つの固有値λ_１，λ_２を算出するため、ｉ＝ｊ＝１，２である。

［数８］
Σｘ−λＩｘ＝(Σ−λＩ)ｘ＝０
det(Σ−λＩ)＝０
ただし、この実施例では、２つの固有値λ_１，λ_２を算出するため、Ｉは２行２列の単位行列である。

このようにして、固有値λ_１，λ_２が算出されると、この固有値λ_１，λ_２を用いて、動的物体であるか、静的物体であるかが判別される。具体的には、固有値λ_１が第１閾値（たとえば、６５ｃｍ）よりも大きい場合、または、固有値λ_２が第２閾値（たとえば、３ｃｍ）よりも小さい場合には、このような固有値λ_１，λ_２を有するクラスタを静的物体と判別する。ただし、第１閾値および第２閾値は、人間のような動的物体と壁や柱のような静的物体を判別するために実験等により経験的に得られた数値である。

このように、固有値λ_２が第２閾値よりも小さい場合には、クラスタの形状が細長く、壁に対応するクラスタと考えられるからである。また、固有値λ_１が第１閾値よりも大きい場合には、図１０に示すように、通路の角に相当する部分の壁や柱に対応するクラスタと考えられるからである。したがって、固有値λ_１が第１閾値以下である場合、または、固有値λ_２が第２閾値以上である場合には、人間の脚（足首辺り）に相当する物体すなわち人間または人間のような物体に対応するクラスタであると考え、このような固有値λ_１，λ_２を有するクラスタを動的物体と判別する。

ただし、この実施例では、固有値λ_１，λ_２の両方が第２閾値（たとえば、３ｃｍ）よりも小さい場合には、物体が小さ過ぎると判断し、当該固有値λ_１，λ_２を有するクラスタに含まれる位置データに対応する距離データを消去する。

図１１は、図９に示したクラスタ１−３、５−７について、それぞれの特徴に基づいて静的物体であるか動的物体であるかを判別し、動的物体であることが判別されたクラスタ（図１１では、クラスタ２、３、５、６）を消去（非表示）した図である。上述したように、移動体１２の位置を推定する場合には、図６−図９および図１１に示す、壁や柱のような静的物体についてのクラスタに含まれる位置データに対応する距離データのみが用いられる。

次に、人間を追跡する場合について説明する。この実施例では、移動体１２が適用される環境内に存在し、移動体１２に設けられる距離センサ２２ａ、２２ｂの検出範囲に存在する１または複数の人間が検出（検知）され、検知された１または複数の人間の位置が推定される。また、検知された１または複数の人間のうち、１人の人間が選択され、選択された１人の人間に移動体１２を追従させる。人間の位置の推定には、パーティクルフィルタが用いられる。ただし、上述したように、１人の人間に対して１つのパーティクルフィルタが用いられる（生成される）。以下、人間を検知し、検知した１または複数の人間の位置を推定する方法と、選択した１人の人間を追跡する方法について具体的に説明する。

上述したように、移動体１２の位置を推定する際に、クラスタの特徴から動的物体であることが判別されたクラスタは人間に対応する。つまり、人間が検知される。人間が検知されると、当該人間に対応するクラスタに含まれる複数の点の重心（または平均）の位置を当該人間の位置として決定し、識別情報（人間ＩＤ）を付して管理する。以下、識別情報を付した人間の位置を人間情報という。ただし、後述するように、人間情報には、人間の速度（移動速度）も含まれる。

１または複数の人間が検知されると、上述したように、各人間についてパーティクルフィルタを用いて位置が推定される。パーティクルフィルタについては、上述したため、移動体１２の位置を推定する場合と異なる点についてのみ説明することにする。

人間の状態ベクトルは、ｘ＝［ｘ，ｙ，ｖ_x，ｖ_y］^Tであり、位置（ｘ，ｙ）および速度（ｖ_x，ｖ_y）で表される。また、パーティクルフィルタはｍ個（この実施例では、５０個）のパーティクルを含み、ｍ番目のパーティクルの状態ベクトルはｘ^m＝＝［ｘ^m，ｙ^m，ｖ^m _x，ｖ^m _y］^Tで表される。また、この実施例では、時刻ｔにおけるパーティクルｍの状態ベクトルｘ^m _tは、数９に従って算出される。ただし、Δｔはフレーム間（現状態と次の状態の間）に時間であり、ω_x，ω_yはそれぞれ平均０であり、標準偏差σ_x，σ_yのガウシアンノイズである。

［数９］

なお、このように、ガウシアンノイズにより拡散させて、現在の状態ｘ^m _t-1から次の状態ｘ^m _tを予測するのは、人間が歩行する場合に、その進行方向が一直線上とは限らないからである。

また、実際の人間の位置（測定される位置）は、移動体１２の位置を推定する際に、動的物体であることが判別されたクラスタの重心を算出することにより知ることができる。実際の人間の速度は、実際の人間の位置の時間毎の変化（Δｔ）から算出することができる。そして、各パーティクルｍについて予測した位置および速度と、実際の人間の位置および速度の尤度を求めて、各パーティクルｍの重み（重要度）が更新される。

なお、前回検出した人間と同じ人間についてのクラスタであるかどうかは、前回検出したクラスタの重心からの距離が所定距離（たとえば、時間Δｔの間に人間が進むと考えられる距離）内であるかどうかで判断される。

また、人間の速度は、重心のｘ成分およびｙ成分のそれぞれについての時間Δｔの間における変化量で算出される。

図１２および図１３を用いて、パーティクルフィルタを用いて、２人の人間のそれぞれについての位置を推定（追跡）する方法について説明する。図１２（Ａ）は、時間ｔにおいて、検知した２人の人間についてパーティクルフィルタを生成した状態を示す。ただし、図１２（Ａ）において（図１２（Ｃ）、図１３（Ｂ）および図１３（Ｄ）も同じ。）、実線の丸印は移動体１２に設けられた距離センサ２２ａで距離を検出された点（人間の脚の周囲）を示し、この丸印よりも大きい実線の楕円は初期分布またはリサンプリングされたパーティクルの範囲（分布範囲）を規定する。

図１２（Ａ）に示すように、移動体１２（ロボット）は、２人の人間の後方であり、当該２人の人間の一方に追従するように移動しているものとする。図１２（Ａ）では、移動体１２（ロボット）は白抜き矢印の方向に移動する。ただし、図１２（Ｂ）、（Ｃ）および図１３（Ａ）−図１３（Ｄ）は簡単のため移動体１２を省略してある。また、図１２（Ａ）（図１２（Ｃ）、図１３（Ｂ）および図１３（Ｄ）も同じ。）では、２人の人間もそれぞれ白抜き矢印で示す方向に移動し、移動体１２に設けられた距離センサ２２ａで各人間の脚との距離を背面側から検出している様子が示される。

図１２（Ｂ）に示すように、時間ｔにおいて、時間ｔ＋１の状態ｘ^m _t+1が予測される。ただし、数９に従って予測される。以下、状態を予測する場合について同じ。この図１２（Ｂ）では（図１３（Ａ）および（Ｃ）も同じ。）、点線の丸印は移動体１２に設けられた距離センサ２２ａで距離を検出された点（人間の脚の周囲）を示し、この点線の丸印よりも大きい点線の楕円は予測（分散）されたパーティクルｍの範囲（予測範囲）を規定する。

そして、図１２（Ｃ）に示すように、時間ｔ＋１の測定値（実際の位置および速度）に応じて各パーティクルｍの尤度が求められ、尤度から各パーティクルｍの重みが算出され、パーティクルｍがリサンプリングされる。

次に、図１３（Ａ）に示すように、時間ｔ＋１において、時間ｔ＋２の状態ｘ^m _t+2が予測される。図示は省略するが、上述したように、時間の経過に従って、ロボット（移動体１２）は一方の人間を追従するように移動されている。ただし、追従する人間が立ち止まっている場合には、ロボット（移動体１２）の移動が停止されることもある。

図１３（Ｂ）に示すように、時間ｔ＋２の測定値に応じて各パーティクルｍの尤度が求められ、尤度から各パーティクルｍの重みが算出され、パーティクルｍがリサンプリングされる。

図１３（Ｃ）に示すように、時間ｔ＋２において、時間ｔ＋３の状態ｘ^m _t+3が予測される。そして、図１３（Ｄ）に示すように、時間ｔ＋３の測定値に応じて各パーティクルｍの尤度が求められ、尤度から各パーティクルｍの重みが算出され、パーティクルｍがリサンプリングされる。

このように、予測、計測、リサンプリングが繰り返し行われ、人間の位置が推測（追跡）されるのである。ただし、この実施例では、予測されたパーティクルｍの分散の主成分（固有値）λ_aが所定値Ｌ（この実施例では、０．４ｍ）を超えた場合には、該当する人間についての位置の推定を終了するようにしてある。このような場合には、人間はロボット（移動体１２）から離れる方向にまたは離れる速度で移動していると考えられるからである。詳細な説明は省略するが、固有値λ_aは、パーティクルｍについての分散行列の固有値のうちの最大値であり、上記の固有値λ₁と同じ方法で算出される。

次に、選択した人間を追従する方法について説明する。この実施例では、検知された１または複数の人間のうち、１人の人間がロボット（移動体１２）によって自動的に選択されたり、外部のコンピュータからの指示に従って選択されたりする。詳細な説明は省略するが、たとえば、ロボット（移動体１２）は、検知された１または複数の人間のうちから、ランダムに１人の人間を選択したり、自身の前方であり最も距離が近い人間を選択したり、話掛けられた人間を選択したりする。また、たとえば、外部のコンピュータでは、マップ（グリッドマップ）上に検知した人間を表示し、当該コンピュータの操作者が所望の人間を選択すると、選択された人間の位置がロボット（移動体１２）に送信される。

１人の人間が選択されると、当該人間を追従するように、ロボット（移動体１２）は移動する。この実施例では、ロボット（移動体１２）の移動経路が算出され、算出された移動経路に従って移動するようにロボット（移動体１２）のモータ３６ａ、３６ｂが駆動される。

具体的には、ロボット（移動体１２）の現在位置と、追従する人間の現在位置を結ぶ直線が移動経路として算出される。ただし、移動経路上に、壁や柱以外の静的物体、追従する人間以外の動的物体または動的物体の移動経路が重なっている場合には、これらの物体や移動経路を避けるべく、算出された移動経路が変更される。たとえば、グリッドマップ上に、障害となる物体、すなわち、壁や柱以外の静的物体、追従する人間以外の動的物体または動的物体の移動経路を描画し、これらが描画されていないグリッド上を移動して選択された人間を追従するように、移動経路を再計算する。

なお、移動経路の算出方法は一例であり、他の算出方法が採用されてもよい。たとえば、人間は基本的には直線的に移動しないため、追従する人間の移動方向（速度ベクトルの向き）と、ロボット（移動体１２）の移動方向（θ）とが所定角度（たとえば、１５°〜３０°）以内であれば、ロボット（移動体１２）を現在の移動方向であり、追従する人間に所定距離近づくように、移動経路を算出してもよい。かかる場合にも、障害となる物体が存在する場合には、移動経路が再計算される。

たとえば、追従する人間の速度と同じ速度で、算出した移動経路に従って移動するように、モータ３６ａ、３６ｂが駆動される。ただし、ロボット（移動体１２）と追従する人間の距離が所定距離（たとえば、１００ｃｍ）を超えて離れている場合には、追従する人間の速度よりも速い速度で移動するようにモータ３６ａ、３６ｂが駆動される。詳細な説明は省略するが、モータ３６ａ、３６の回転速度を異ならせることにより、ロボット（移動体１２）を、左右の斜め方向への移動、左折、右折、左旋回または右旋回させることができる。

図１４は、図２に示したコンピュータ３０に内蔵されるＲＡＭ３０ｂのメモリマップ３００の一例を示す。図１４に示すように、ＲＡＭ３０ｂは、プログラム記憶領域３０２およびデータ記憶領域３０４を含む。

プログラム記憶領域３０２には、メイン処理プログラム３０２ａ、データ記憶プログラム３０２ｂ、位置データ算出プログラム３０２ｃ、データ判別プログラム３０２ｄ、移動体位置推定プログラム３０２ｅ、人間位置推定プログラム３０２ｆ、移動経路算出プログラム３０２ｇおよび追従プログラム３０２ｈなどが記憶される。

メイン処理プログラム３０２ａは、追跡システム１０のメインルーチンを処理するためのプログラムである。データ記憶プログラム３０２ｂは、距離データおよび回転数データを取得して、ＲＡＭ３０ｂのデータ記憶領域３０４に記憶するためのプログラムである。

位置データ算出プログラム３０２ｃは、検出した距離データから、移動体１２の位置および向きを基準として移動体座標系の位置データを算出し、算出した位置データを世界座標系の位置データに座標変換するためのプログラムである。データ判別プログラム３０２ｄは、移動体１２の位置推定に先立って、位置データ算出プログラム３０２ｃに従って算出（座標変換）された位置データをクラスタリングし、静的物体に対応するクラスタまたは動的物体に対応するクラスタに判別するためのプログラムである。

移動体位置推定プログラム３０２ｅは、データ判別プログラム３０２ｄに従って静的物体に対応することが判別されたクラスタに含まれる位置データに対応する距離データと地図データ３０４ｅに基づき、パーティクルフィルタを用いて、移動体１２の位置（状態）を推定するためのプログラムである。

人間位置推定プログラム３０２ｆは、データ判別プログラム３０２ｄに従って動的物体に対応することが判別されたクラスタを人間として検知し、パーティクルフィルタを用いて、検知した人間の位置（状態）を推定するためのプログラムである。

移動経路算出プログラム３０２ｇは、データ判別プログラム３０２ｄの判別結果から障害となる物体を考慮するとともに、地図データ３０４ｅを参照して、選択した１人の人間に移動体１２を追従させるための移動経路を算出（再計算）するためのプログラムである。追従プログラム３０２ｈは、移動経路算出プログラム３０２ｇに従って算出された移動経路に従って沿って移動体１２を移動させるためのプログラムである。

図示は省略するが、人間の追跡（追従）や所定のサービスの提供に必要な他のプログラムもプログラム記憶領域３０２に記憶される。

また、データ記憶領域３０４には、距離データバッファ３０４ａおよび回転数データバッファ３０４ｂが設けられる。距離データバッファ３０４ａは、データ記憶プログラム３０２ｂに従って距離データを記憶する。同様に、回転数データバッファ３０４ｂは、データ記憶プログラム３０２ｂに従って回転数データを記憶する。

また、データ記憶領域３０４には、位置データ３０４ｃ、クラスタデータ３０４ｄ、地図データ３０４ｅおよび状態データ３０４ｆが記憶される。計測位置データ３０４ｃは、位置データ算出プログラム３０２ｃに従って距離データバッファ３０４ａに記憶された距離データから算出され、世界座標系に座標変換された位置（点）データである。詳細な説明は省略するが、距離データと対応する位置データとは互いに関連付けされている。

クラスタデータ３０４ｄは、距離データから算出され、座標変換された位置データをクラスタリングしたクラスタ毎のデータである。地図データ３０４ｅは、移動体１２が移動する環境についてのグリッドマップのデータである。この地図データ３０４ｅでは、グリッドマップのみならず、上述したように、静的物体（の表面）に相当するグリッドに対応して静的物体であることを示す情報が記憶されている。

移動体状態データ３０４ｆは、移動体位置推定プログラム３０２ｅに従って推定された移動体１２の状態（位置および向き）についてのデータである。人間情報データ３０４ｇは、人間位置推定プログラム３０２ｇに従って推定された人間の状態（位置および速度）に、人間の識別情報が付されたデータである。上述したように、各人間の状態（位置および速度）は、それぞれ、パーティクルフィルタを用いて推定され、各人間について推定された状態が時系列に従って記憶される。

図示は省略するが、データ記憶領域３０４には、人間の追跡（追従）や所定のサービスの提供に必要な他のデータが記憶されたり、上記のプログラムの実行のために必要なフラグやタイマ（カウンタ）などが設けられたりする。

図１５および図１６は図２に示したＣＰＵ３０ａの全体処理を示すフロー図である。ＣＰＵ３０ａは、外部からスタート位置、１または複数の通過点、および終点を与えられると、図１５に示すように全体処理を開始し、ステップＳ１で、距離センサ２２ａ、２２ｂを駆動する。このとき、ＣＰＵ３０ａは、距離センサ２２ａ、２２ｂからの距離データの取得および記憶を開始する。

次のステップＳ３では、最初の通過点を設定する。ここでは、指示された通過点のうち、最初の通過点の座標が設定される。そして、ステップＳ５で、移動体１２の自律航行（自律移動）を開始する。つまり、ＣＰＵ３０ａは、全体処理とは並行して、移動体１２の移動制御の処理（この実施例では、図２０に示す追従処理）を実行する。

続くステップＳ７では、距離データおよび回転数データを取得するとともに記憶する。つまり、ＣＰＵ３０ａは、距離センサ２２ａ、２２ｂからの距離データを取得するとともに、エンコーダ３８ａ、３８ｂからの回転数データを取得し、互いに関連付けて、それぞれ時系列に従ってＲＡＭ３０ｂに記憶する。

続いて、ステップＳ９で、移動体１２の位置および向きに応じて距離データから位置データを算出する。ここでは、上述したように、距離センサ２２ａ、２２ｂのそれぞれで検出された距離のデータから、移動体１２の位置および向きを基準として移動体座標系の位置データが算出される。このとき、上述したように、２つの距離センサ２２ａ、２２ｂの取り付け位置が考慮される。そして、ステップＳ１１で、ステップＳ９で算出された位置データを世界座標系の位置データに変換する。

図１６に示すように、続くステップＳ１３では、後述するデータ判別処理（図１７および図１８参照）を実行し、ステップＳ１５で、前回位置推定（Ｓ１７）を行ってから所定時間を経過したかどうかを判断する。このとき、ＣＰＵ３０ａは、図示しないタイマを用いて時間をカウントする。また、この実施例では、所定時間が経過したかどうかを判断するようにしてあるが、所定距離移動したかどうかを判断するようにしてもよい。ただし、ステップＳ１５の処理は省略してもよい。

なお、全体処理を開始し、最初にステップＳ１５の判断処理が実行される場合には、位置推定を一度も行っていないため、“ＹＥＳ”と判断するようにしてある。

ステップＳ１５で“ＮＯ”であれば、つまり前回位置推定を行ってから所定時間を経過していなければ、そのまま図１５に示したステップＳ７に戻る。一方、ステップＳ１５で“ＹＥＳ”であれば、つまり前回位置推定を行ってから所定時間を経過すれば、ステップＳ１７で、地図を参照して位置および角度を推定する。ここでは、ＣＰＵ３０ａは、上述したように、地図データ３０４ｅ、回転数データ、および静的物体と判別されたクラスタに含まれる位置データに対応する距離データを用いて、パーティクルフィルタにより移動体１２の位置および向きを推定する。したがって、ステップＳ１７の処理によって、移動体状態データ３０４ｆが更新される。

続くステップＳ１９では、通過点に到達したかどうかを判断する。ここでは、ＣＰＵ３０ａは、ステップＳ１７で推定した位置が設定された通過点と一致するかどうかを判断する。ステップＳ１９で“ＹＥＳ”であれば、つまり通過点に到達すれば、ステップＳ２１で、次の通過点または終点を設定して、ステップＳ７に戻る。一方、ステップＳ１９で“ＮＯ”であれば、つまり通過点に到達していなければ、ステップＳ２３で、終点に到達したかどうかを判断する。ここでは、ＣＰＵ３０ａは、ステップＳ１７で推定した位置が終点と一致するかどうかを判断する。

なお、詳細な説明は省略するが、ロボット（移動体１２）は、設定された通過点や終点に移動させるように、追従する人間をコミュニケーションにより誘導する。たとえば、音声により、道順を説明する。また、音声および身振り手振りのような身体動作の両方で道順を説明する。この実施例では、ロボット（移動体１２）を人間の後方を追従するように移動させるため、音声または音声と身体動作で道順を案内するようにしてあるが、ロボット（移動体１２）が人間を先導する場合には、身体動作（たとえば、指差し）だけで誘導してもよい。

ステップＳ２３で“ＮＯ”であれば、つまり終点に到達していなければ、ステップＳ７に戻る。一方、ステップＳ２３で“ＹＥＳ”であれば、つまり終点に到達すれば、全体処理を終了する。

なお、詳細な説明は省略するが、位置を推定する（ステップＳ１７の処理を実行する）ために用いられた距離データおよび回転数データは、バッファ３０４ａ、３０４ｂから消去される。

図１７および図１８は、図１６に示したステップＳ１３のデータ判別処理を示すフロー図である。図１７に示すように、ＣＰＵ３０ａは、データ判別処理を開始すると、ステップＳ３３で、着目する点（位置データに対応する位置）についての新しいクラスタを生成する。たとえば、ステップＳ３３では、まだクラスタに分類されていない位置データが任意に選択され、新しいクラスタが生成される。このとき、着目する点についての位置データを含む新しいクラスタのデータが追加される。つまり、新しいクラスタのデータを追加することにより、クラスタデータ３０４ｄが更新される。

続くステップＳ３５では、次の点への距離が所定値ＴＨ（この実施例では、１０ｃｍ）未満であるかどうかを判断する。ここでは、ＣＰＵ３０ａは、着目する点に最も近い点との距離が所定値ＴＨ未満であるかどうかを判断するのである。ステップＳ３５で“ＮＯ”であれば、つまり次の点への距離が所定値ＴＨ以上であれば、新しいクラスタを生成するために、ステップＳ３３に戻る。一方、ステップＳ３５で“ＹＥＳ”であれば、つまり次の点への距離が所定値ＴＨ未満であれば、ステップＳ３７で、当該次の点を既存のクラスタに追加する。つまり、当該次の点が着目する点と同じクラスタに分類される。

続いて、ステップＳ３９では、すべての点についてクラスタリングを行ったかどうかを判断する。ここでは、ＣＰＵ３０ａは、クラスタに分類されていない位置データが残っていないかどうかを判断するのである。ステップＳ３９で“ＮＯ”であれば、つまりクラスタリングを行っていない点が残っていれば、ステップＳ３５に戻る。図示は省略するが、このとき、直前にクラスタに分類された点が着目する点に設定される。

一方、ステップＳ３９で“ＹＥＳ”であれば、つまりすべての点についてクラスタリングを行えば、クラスタリングを終了したと判断し、ステップＳ４１で、１番目のクラスタを読み出し、図１８に示すステップＳ４３で、当該クラスタに含まれる点の数が所定数ＮＰ（この実施例では、５）未満かどうかどうかを判断する。

なお、ステップＳ４１では、説明の便宜上、１番目のクラスタを読み出すように説明したが、ここでは、任意のクラスタが読み出される。後述するステップＳ５７においても同様であるが、このステップＳ５７では、静的物体であることが判別されていない任意のクラスタが読み出される。ただし、クラスタを生成した順番に従ってクラスタを読み出すようにしてもよい。

ステップＳ４３で“ＹＥＳ”であれば、つまり当該クラスタに含まれる点の数が所定数ＮＰ未満であれば、静的物体か動的物体かの判別が出来ないと判断して、ステップＳ４８に進む。一方、ステップＳ４３で“ＮＯ”であれば、つまり当該クラスタに含まれる点の数が所定数ＮＰ以上であれば、ステップＳ４５で、数４−数６に従って、当該クラスタの固有値λ₁、λ₂を算出する。

次のステップＳ４７では、固有値λ₁およびλ₂の両方が第２閾値Ｈ_min（この実施例では、３ｃｍ）未満であるかどうかを判断する。上述したように、ここでは、静的物体または動的物体を判別することができるかどうかが判断されるのである。ステップＳ４７で“ＹＥＳ”であれば、つまり固有値λ₁およびλ₂の両方が第２閾値Ｈ_min未満であれば、ステップＳ４８で、当該クラスタに含まれる位置データに対応する距離データを消去して、ステップＳ５５に進む。一方、ステップＳ４７で“ＮＯ”であれば、つまり固有値λ₁およびλ₂の少なくとも一方が第２閾値Ｈ_min以上であれば、ステップＳ４９で、固有値λ₁が第１閾値Ｈ_maxよりも大きい、または、固有値λ₂が第２閾値Ｈ_min未満であるかどうかを判断する。上述したように、ここでは、人間のような動的物体であるか、壁や柱のような静的物体であるかが判断（判別）される。

ステップＳ４９で“ＹＥＳ”であれば、つまり固有値λ₁が第１閾値Ｈ_maxよりも大きい、または、固有値λ₂が第２閾値Ｈ_min未満であれば、壁や柱のような静的物体であると判断し、ステップＳ５１で、当該クラスタを静的物体に対応するクラスタに決定して、ステップＳ５５に進む。たとえば、ステップＳ５１では、静的物体であることが当該クラスタにラベリングされる。

一方、ステップＳ４９で“ＮＯ”であれば、つまり固有値λ₁が第１閾値Ｈ_max以下である、または、固有値λ₂が第２閾値Ｈ_min以上である場合には、人間のような動的物体であると判断して、ステップＳ５３で、当該クラスタを動的物体に対応するクラスタに決定して、ステップＳ５５に進む。たとえば、ステップＳ５３では、動的物体であることが討議あクラスタにラベリングされる。

ステップＳ５５では、すべてのクラスタを判別したかどうかを判断する。ステップＳ５５で“ＮＯ”であれば、つまりまだ判別していないクラスタが存在する場合には、ステップＳ５７で、次のクラスタを読み出し、ステップＳ４３に戻る。一方、ステップＳ５５で“ＹＥＳ”であれば、つまりすべてのクラスタを判別すれば、図１４に示した全体処理にリターンする。

図１９は、図２に示したＣＰＵ３０ａの人間位置推定処理を示すフロー図である。この人間位置推定処理は、ＣＰＵ３０ａによって、上述した全体処理と並行して実行される。図１９に示すように、ＣＰＵ３０ａは、人間位置推定処理を開始すると、ステップＳ７１で、人間を検知する。ここでは、データ判別処理の判別結果を参照して、動的物体と判別されたクラスタを人間に対応するクラスタとして検知する。このとき、人間に対応するクラスタに含まれる位置データに対応する複数の位置（点）の重心が算出され、算出された重心が人間の位置に決定される。

次のステップＳ７３では、新しい人間を検知したかどうかを判断する。ここでは、現在パーティクルフィルタで位置を推定（追跡）している人間以外の人間を検知したかどうかを判断する。つまり、人間情報データ３０４ｇに記憶されていない人間を検知したかどうかを判断するのである。

ステップＳ７３で“ＮＯ”であれば、つまり新しい人間を検知しなければ、そのままステップＳ７７に進む。一方、ステップＳ７３で“ＹＥＳ”であれば、つまり新しい人間を検知すれば、ステップＳ７５で、新しく検知された人間の位置に識別情報を付した人間情報を記憶して、ステップＳ７７に進む。このステップＳ７５では、上述したように決定された人間の位置に人間ＩＤが付され、ＲＡＭ３０ｂに記憶される。したがって、人間情報データ３０４ｇが更新される。なお、人間情報には、人間の速度も含まれるが新しく検知された時点においては、位置のみが記述される。

ステップＳ７７では、人間の位置および速度を推定する。ここでは、ＣＰＵ３０ａは、上述したように、パーティクルフィルタにより人間の位置および速度を推定する。したがって、ステップＳ７７の処理によって、人間情報データ３０４ｇが更新される。ただし、ステップＳ７７では、人間の状態が上書きされるのではなく、時系列に従って追加的に記憶される。

次のステップＳ７９では、予測したパーティクルｍにおける分散行列の固有値λ_aが所定距離Ｌ（この実施例では、０．４ｍ）を超えたかどうかを判断する。ステップＳ７９で“ＮＯ”であれば、つまり固有値λ_aが所定距離Ｌ以下であれば、そのままステップＳ８３に進む。一方、ステップＳ７９で“ＹＥＳ”であれば、つまり固有値λ_aが所定距離Ｌを超えれば、ステップＳ８１で、該当するパーティクルｍに対応する人間についての人間情報を削除して、ステップＳ８３に進む。

ただし、パーティクルフィルタは、人間毎に生成され、人間毎に状態（位置および速度）を推定するため、ステップＳ７７−Ｓ８１の処理は、各人間について実行される。

ステップＳ８３では、人間位置推定処理を終了するかどうかを判断する。ここでは、ＣＰＵ３０ａは、移動体１２が終点に移動し、全体処理を終了したかどうかを判断する。ステップＳ８３で“ＮＯ”であれば、つまり人間位置推定処理を終了しない場合には、そのままステップＳ７１に戻る。一方、ステップＳ８３で“ＹＥＳ”であれば、そのまま人間位置推定処理を終了する。

図２０は、図２に示したＣＰＵ３０ａの追従処理のフロー図である。この追従処理もまた、ＣＰＵ３０ａによって、全体処理や人間位置推定処理と並行して実行される。図２０に示すように、ＣＰＵ３０ａは、追従処理を開始すると、ステップＳ１０１で、追従する人間を選択する。ここでは、上述したように、ロボットまたは移動体１２（ＣＰＵ３０ａ）が追従する人間を選択したり、外部のコンピュータから指示された人間を選択したりする。

次のステップＳ１０３では、選択された人間の位置および速度を取得する。つまり、ＣＰＵ３０ａは、人間情報データ３０４ｇを参照して、選択された人間に対応する人間情報から位置および速度を取得する。

さらに、ステップＳ１０５では、静的物体と選択された人間以外の動的物体を検出する。ここでは、ＣＰＵ３０ａは、地図データ３０４ｅを参照するとともに、全体処理におけるクラスタの判別結果から、静的物体および動的物体と判別されたクラスタのうち、グリッドマップに載っていない静的物体と、選択された人間以外の動的物体を検出する。つまり、障害となり得る物体が検出される。

次のステップＳ１０７では、上述したように、移動経路を算出する。そして、ステップＳ１０９では、移動経路に従って移動体１２を移動させる。つまり、ＣＰＵ３０ａの指示の下、移動経路に沿って移動体１２を移動させるように、モータドライバ３４ａ、３４ｂによって、モータ３６ａ、３６ｂが駆動される。

そして、ステップＳ１１１では、追従処理を終了するかどうかを判断する。ここでは、ＣＰＵ３０ａは、移動体１２が終点に移動し、全体処理を終了したかどうかを判断する。ステップＳ１１１で“ＹＥＳ”であれば、そのまま追従処理を終了する。一方、ステップＳ１１１で“ＮＯ”であれば、つまり追従処理を終了しない場合には、ステップＳ１１３で、追従する人間の変更かどうかを判断する。ここでは、ＣＰＵ３０ａは、人間位置推定処理において追従している人間の人間情報が削除されたり、追従する人間の変更指示が外部のコンピュータから与えられたりしたかどうかを判断する。ステップＳ１１３で“ＮＯ”であれば、つまり追従する人間を変更しない場合には、ステップＳ１０３に戻るが、ステップＳ１１３で“ＹＥＳ”であれば、つまり追従する人間を変更する場合には、ステップＳ１０１に戻る。

この実施例によれば、距離センサの計測結果から静的物体および動的物体を判別し、静的物体についての計測結果のみを用いて移動体の位置を推定するので、動的物体の計測結果による影響を可及的少なくすることができる。つまり、人間などの動的物体が存在する環境においても、出来る限り正しく位置を推定することができる。つまり、移動体の位置を正しく推定し、検知した人間のうちから選択した１人の人間に移動体を追従させるので、人間を正確に追跡することができる。したがって、移動体を適用するロボットに、正しい位置に基づいた所定のサービスを提供させることができる。

また、この実施例によれば、人間の位置をパーティクルフィルタを用いて推定（追跡）するので、人間の位置を正しく推定し、より正確に移動体を追従させることができる。

さらに、この実施例では、距離センサで検出された距離データを位置データに変換するだけで静的物体および動的物体を判別できるので、カメラ等の他のセンサを設ける必要が無い。つまり、コストが高くなったり、画像処理などの負荷が大きくなったりすることもない。

さらにまた、この実施例では、位置データ（点）の数が所定数ＮＰ未満のクラスタや固有値λ1およびλ2の両方が第２閾値Ｈmin未満であるクラスタに含まれる位置データに対応する距離データを消去するようにしたが、これらのクラスタについても障害となる物体として移動経路の計算において考慮するようにしてもよい。

なお、この実施例では、動的物体の例として人間ないし人間のような物体を判別する場合について詳細に説明したが、上述したように、ショッピングカートや台車などの他の動的物体を判別することもできる。ただし、ショッピングカートや台車などの他の動的物体を判別する場合には、クラスタに含まれる位置データ（点）の数やその分散についての主成分（分散行列の固有値）が満たす条件をそれぞれの場合について決定する必要がある。

また、この実施例では、距離データを世界座標系の位置データに変換してクラスタリングするようにしたが、距離データを移動体を基準とした座標系（移動体座標系）の位置データに変換してクラスタリングするようにしてもよい。

さらに、この実施例では、パーティクルフィルタを用いて移動体や人間の位置を推定するようにしたがカルマンフィルタを用いてもよい。カルマンフィルタを用いた位置推定の手法は既に周知であるため、その説明は省略する。

さらにまた、この実施例で示した具体的な数値は単なる一例であり、限定されるべきではなく、実施される製品等に応じて適宜変更可能である。また、この実施例では、所定値や閾値を用いて大小を判断するステップにおいて、当該所定値や当該閾値を含まないようにしたが、当該所定値や当該閾値を含んで大小を判断するようにしてもよい。

１０ …追跡システム
１２ …移動体
１６ａ、１６ｂ …車輪
１８ …操作レバー
２２ａ、２２ｂ …距離センサ
３０ …コンピュータ
３４ａ、３４ｂ …モータドライバ
３６ａ、３６ｂ …モータ
３８ａ、３８ｂ …エンコーダ

Claims (9)

1. 距離センサを設置し環境内を移動する移動体を用いて当該環境内に存在する人間を追跡する追跡装置であって、
   前記距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出手段、
   前記位置データ算出手段によって算出された位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリング手段、
   前記クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断手段、
   前記条件判断手段の判断結果に応じて前記複数のクラスタの各々が動的物体または静的物体に対応するかどうかを判別する判別手段、
   前記環境についての地図データと、前記判別手段によって静的物体に対応することが判別されたクラスタに含まれる位置データに対応する距離データを用いて、前記移動体の位置を推定する移動体位置推定手段、
   前記判別手段の判別結果に基づいて前記環境内に存在する１または複数の人間を検知する検知手段、
   前記検知手段によって検知された１または複数の人間のうち、１人の人間を選択する選択手段、および
   前記選択手段によって選択された人間に前記移動体を追従させる追従手段を備える、追跡装置。
2. 前記検知手段によって検知された１または複数の人間の位置を推定する人間位置推定手段をさらに備える、請求項１記載の追跡装置。
3. 前記所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることである、請求項１または２記載の追跡装置。
4. 前記所定の条件は、前記クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含む、請求項１ないし３のいずれかに記載の追跡装置。
5. 前記クラスタリング手段によってクラスタリングされた複数のクラスタの各々の特徴を抽出する抽出手段をさらに備え、
   前記条件判断手段は、前記抽出手段によって抽出された特徴が前記所定の条件を満たすかどうかを判断する、請求項１ないし４のいずれかに記載の追跡装置。
6. 前記抽出手段によって抽出された特徴は、前記クラスタに含まれる位置データが示す複数の点の分散についての特徴量である、請求項５記載の追跡装置。
7. 前記特徴量は、前記クラスタに含まれる位置データが示す複数の点の分散行列についての固有値であり、
   前記所定の条件は、前記固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい前記固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む、請求項６記載の追跡装置。
8. 距離センサを設置し環境内を移動する移動体を用いて当該環境内に存在する人間を追跡するコンピュータの追跡プログラムであって、
   前記コンピュータに、
   前記距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出ステップ、
   前記位置データ算出ステップにおいて算出した位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリングステップ、
   前記クラスタリングステップにおいてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断ステップ、
   前記条件判断ステップの判断結果に応じて前記複数のクラスタの各々が動的物体または静的物体に対応するかどうかを判別する判別ステップ、
   前記環境についての地図データと、前記判別ステップにおいて静的物体に対応することを判別したクラスタに含まれる位置データに対応する距離データを用いて、前記移動体の位置を推定する移動体位置推定ステップ、
   前記判別ステップの判別結果に基づいて前記環境内に存在する１または複数の人間を検知する検知ステップ、
   前記検知ステップにおいて検知した１または複数の人間のうち、１人の人間を選択する選択ステップ、および
   前記選択ステップにおいて選択した人間に前記移動体を追従させる追従ステップを実行させる、追跡プログラム。
9. 距離センサを設置し環境内を移動する移動体を用いて当該環境内に存在する人間を追跡するコンピュータの追跡方法であって、
   前記コンピュータは、
   （ａ）前記距離センサで検出された距離データから、距離を計測された点の位置データを算出し、
   （ｂ）前記ステップ（ａ）において算出した位置データを所定のルールに従って複数のクラスタにクラスタリングし、
   （ｃ）前記ステップ（ｂ）においてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断し、
   （ｄ）前記ステップ（ｃ）の判断結果に応じて前記複数のクラスタの各々が動的物体または静的物体に対応するかどうかを判別し、
   （ｅ）前記環境についての地図データと、前記ステップ（ｄ）において静的物体に対応することを判別したクラスタに含まれる位置データに対応する距離データを用いて、前記移動体の位置を推定し、
   （ｆ）前記ステップ（ｄ）の判別結果に基づいて前記環境内に存在する１または複数の人間を検知し、
   （ｇ）前記ステップ（ｆ）において検知した１または複数の人間のうち、１人の人間を選択し、そして
   （ｈ）前記ステップ（ｇ）において選択した人間に前記移動体を追従させる、追跡方法。

Images