(57)【要約】      （修正有）【課題】人間のような動的物体が存在する環境において移動体の位置を出来る限り正確に推定する。【解決手段】位置推定システム１０は、移動体１２を含み、移動体は自律行動で環境内を移動する。このとき、移動体に設けられた距離センサ２２ａ、２２ｂによって距離データが検出され、移動体の左右の車輪１６ａ、１６ｂのそれぞれの回転数データも検出される。コンピュータは、回転数データに基づいて移動体の位置および向きを算出し、検出した距離データを、距離を計測された点の世界座標系における位置データに変換する。この位置データを所定のルールでクラスタリングし、各クラスタの特徴量に基づいて壁や床などの静的物体以外のクラスタに含まれる位置データに対応する距離データを消去する。したがって、静的物体についての距離データのみを用いて、移動体の位置が推定される。【選択図】図１
2014174683.tif 000002
【特許請求の範囲】
【請求項１】
或る環境についての地図に対応する地図データと、当該環境内を移動する移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定する位置推定装置であって、
前記距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出手段、
前記位置データ算出手段によって算出された位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリング手段、
前記クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断手段、および
前記条件判断手段によって所定の条件を満たさないことが判断されたクラスタに含まれる位置データに対応する距離データを消去する消去手段を備える、位置推定装置。
【請求項２】
前記所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることである、請求項１記載の位置推定装置。
【請求項３】
前記所定の条件は、前記クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含む、請求項１または２記載の位置推定装置。
【請求項４】
前記クラスタリング手段によってクラスタリングされた複数のクラスタの各々の特徴を抽出する抽出手段をさらに備え、
前記条件判断手段は、前記抽出手段によって抽出された特徴が前記所定の条件を満たすかどうかを判断する、請求項１ないし３のいずれかに記載の位置推定装置。
【請求項５】
前記抽出手段によって抽出された特徴は、前記クラスタに含まれる位置データが示す複数の点の分散についての特徴量である、請求項４記載の位置推定装置。
【請求項６】
前記特徴量は、前記クラスタに含まれる位置データが示す複数の点の分散行列についての固有値であり、
前記所定の条件は、前記固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい前記固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む、請求項５記載の位置推定装置。
【請求項７】
或る環境についての地図に対応する地図データと、当該環境内を移動する移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するコンピュータの位置推定プログラムであって、
前記コンピュータに、
前記距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出ステップ、
前記位置データ算出ステップにおいて算出した位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリングステップ、
前記クラスタリングステップにおいてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断ステップ、および
前記条件判断ステップにおいて所定の条件を満たさないことが判断されたクラスタに含まれる位置データに対応する距離データを消去する消去ステップを実行させる、位置推定プログラム。
【請求項８】
或る環境についての地図に対応する地図データと、当該環境内を移動する移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するコンピュータの位置推定方法であって、
前記コンピュータは、
（ａ）前記距離センサで検出された距離データから、距離を計測された点の位置データを算出し、
（ｂ）前記ステップ（ａ）において算出した位置データを所定のルールに従って複数のクラスタにクラスタリングし、
（ｃ）前記ステップ（ｂ）においてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断し、そして
（ｄ）前記ステップ（ｃ）において所定の条件を満たさないことが判断されたクラスタに含まれる位置データに対応する距離データを消去する、位置推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は位置推定装置、位置推定プログラムおよび位置推定方法に関し、特にたとえば、距離センサを備える移動体の位置を推定する、位置推定装置、位置推定プログラムおよび位置推定方法に関する。
【背景技術】
【０００２】
この発明の背景技術の一例が特許文献１に開示される。この特許文献１では、移動マップに壁や障害物などの存在情報のみならず物体の光反射率属性を示す情報を含めることにより、移動領域内にガラスなどの光学的に特殊な素材が存在する場合においても、レーザ光の反射を仮想的に描写しながら自己位置推定を実施する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００９−２２３７５７
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、この背景技術では、環境内を移動する人間等の動的物体について考慮していないため、動的物体が存在する環境下においては、移動体の位置を正しく推定するのが困難である。
【０００５】
それゆえに、この発明の主たる目的は、新規な、位置推定装置、位置推定プログラムおよび位置推定方法を提供することである。
【０００６】
この発明の他の目的は、動的物体が存在していても出来る限り正確に位置を推定できる、位置推定装置、位置推定プログラムおよび位置推定方法を提供することである。
【課題を解決するための手段】
【０００７】
第１の発明は、或る環境についての地図に対応する地図データと、当該環境内を移動する移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定する位置推定装置であって、距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出手段、位置データ算出手段によって算出された位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリング手段、クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断手段、および条件判断手段によって所定の条件を満たさないことが判断されたクラスタに含まれる位置データに対応する距離データを消去する消去手段を備える、位置推定装置である。
【０００８】
第１の発明では、位置推定装置は、或る環境についての地図に対応する地図データと、当該環境内を移動する移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定する。位置データ算出手段は、距離センサで検出された距離データから、距離を計測された点の位置データを算出する。たとえば、位置データは、距離を計測したときの移動体の位置を基準しても算出してもよいし、移動体の移動開始位置を基準として算出してもよい。クラスタリング手段は、位置データ算出手段によって算出された位置データを所定のルールに従って複数のクラスタにクラスタリングする。条件判断手段は、クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する。たとえば、所定の条件は、クラスタの特徴についての条件であり、より具体的には、人間のような動的物体であるか、壁や柱のような静的物体であるかを判別するための条件である。消去手段は、条件判断手段によって所定の条件を満たさないことが判断されたクラスタに含まれる位置データに対応する距離データを消去する。
【０００９】
第１の発明によれば、不要な距離データを消去するので、必要なデータのみに基づいて位置を推定することができる。したがって、出来る限り正しく位置を推定することができる。
【００１０】
第２の発明は、第１の発明に従属し、所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることである。
【００１１】
第２の発明では、所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることであり、近くに有る点が同じクラスタに分類される。
【００１２】
第２の発明によれば、点同士の距離を検出するだけなので、簡単にクラスタリングすることができる。
【００１３】
第３の発明は、第１または第２の発明に従属し、所定の条件は、クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含む。
【００１４】
第３の発明では、所定の条件は、クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含み、クラスタに含まれる点が少ない場合には、当該クラスタに含まれる点（位置データ）に対応する距離データは消去される。たとえば、クラスタに含まれる点の数が少ない場合には、動的物体であるか、静的物体であるかを判別することができないため、当該クラスタは消去されるのである。
【００１５】
第３の発明によれば、動的物体であるか、静的物体であるかを判別できない不要なデータを消去することができる。
【００１６】
第４の発明は、第１ないし第３の発明に従属し、クラスタリング手段によってクラスタリングされた複数のクラスタの各々の特徴を抽出する抽出手段をさらに備え、条件判断手段は、抽出手段によって抽出された特徴が所定の条件を満たすかどうかを判断する。
【００１７】
第４の発明では、抽出手段は、複数のクラスタの各々の特徴を抽出する。条件判断手段は、各クラスタの特徴が所定の条件を満たすかどうかを判断する。
【００１８】
第４の発明によれば、クラスタに含まれる点の個数以外の特徴によって所定の条件を満たすかどうかを判断することができる。
【００１９】
第５の発明は、第４の発明に従属し、抽出手段によって抽出された特徴は、クラスタに含まれる位置データが示す複数の点の分散についての特徴量である。
【００２０】
第５の発明では、クラスタの特徴は、当該クラスタに含まれる位置データが示す複数の点の分散についての特徴量であり、たとえば、クラスタの形状に関する特徴である。
【００２１】
第５の発明によれば、クラスタの形状に関する特徴が所定の条件を満たすかどうかで、動的物体であるか、静的物体であるかを判別することができる。
【００２２】
第６の発明は、第５の発明に従属し、特徴量は、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値であり、所定の条件は、固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む。
【００２３】
第６の発明では、特徴量は、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値である。たとえば、特徴量は、クラスタに含まれる複数の点の分散についての主成分であり、分散行列の固有値で表される。たとえば、所定の条件は、固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む。したがって、固有値の最大値が第１所定値以下である場合、または、２番目に大きい固有値が第２所定値以上である場合に、所定の条件を満たさないことが判断され、当該固有値を有するクラスタが消去される。
【００２４】
第６の発明によれば、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値に基づいて不要なデータを消去することができる。
【００２５】
第７の発明は、或る環境についての地図に対応する地図データと、当該環境内を移動する移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するコンピュータの位置推定プログラムであって、コンピュータに、距離センサで検出された距離データから、距離を計測された点の位置データを算出する位置データ算出ステップ、位置データ算出ステップにおいて算出した位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリングステップ、クラスタリングステップにおいてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断ステップ、および条件判断ステップにおいて所定の条件を満たさないことが判断されたクラスタに含まれる位置データに対応する距離データを消去する消去ステップを実行させる、位置推定プログラムである。
【００２６】
第８の発明は、或る環境についての地図に対応する地図データと、当該環境内を移動する移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するコンピュータの位置推定方法であって、コンピュータは、（ａ）距離センサで検出された距離データから、距離を計測された点の位置データを算出し、（ｂ）ステップ（ａ）において算出した位置データを所定のルールに従って複数のクラスタにクラスタリングし、（ｃ）ステップ（ｂ）においてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断し、そして（ｄ）ステップ（ｃ）において所定の条件を満たさないことが判断されたクラスタに含まれる位置データに対応する距離データを消去する、位置推定方法である。
【００２７】
第７および第８の発明においても、第１の発明と同様に、出来る限り正しく位置を推定することができる。
【発明の効果】
【００２８】
この発明によれば、移動体に設置した距離センサで距離を計測された点に対応する位置データを所定のルールに従って複数のクラスタにクラスタリングし、各クラスタが所定の条件を満たすかどうかを判別するだけで、静的物体についてのクラスタ以外のクラスタに含まれる位置データに対応する距離データを消去するので、人間のような動的物体が存在する環境においても、出来る限り正しく位置を推定することができる。
【００２９】
この発明の上述の目的，その他の目的，特徴および利点は、図面を参照して行う以下の実施例の詳細な説明から一層明らかとなろう。
【図面の簡単な説明】
【００３０】
【図１】図１はこの発明の位置推定システムの外観構成の一例を示す図解図である。
【図２】図２は図１に示す位置推定システムの電気的な構成の一例を示すブロック図である。
【図３】図３は図１および図２に示す距離センサの検出範囲を説明するための図解図である。
【図４】図４は移動体の位置および向きを算出するためのパラメータを説明するための図解図である。
【図５】図５はグリッドマップの一例を示す図解図である。
【図６】図６は動的物体と静的物体を判別する方法を説明するための図解図である。
【図７】図７は動的物体と静的物体を判別する方法を説明するための他の図解図である。
【図８】図８は動的物体と静的物体を判別する方法を説明するためのその他の図解図である。
【図９】図９は動的物体と静的物体を判別する方法を説明するためのさらに他の図解図である。
【図１０】図１０は静的物体を判定する具体的な場合を説明するための図解図である。
【図１１】図１１は動的物体と静的物体を判別した結果、静的物体以外の点を削除した場合の一例を示す図解図である。
【図１２】図１２は図２に示すＲＡＭのメモリマップの一例を示す図解図である。
【図１３】図１３は図２に示すＣＰＵの全体処理の一部を示すフロー図である。
【図１４】図１４は図２に示すＣＰＵの全体処理の他の一部であって、図１３に後続するフロー図である。
【図１５】図１５は図２に示すＣＰＵの不要データの消去処理の一部を示すフロー図である。
【図１６】図１６は図２に示すＣＰＵの不要データの消去処理の他の一部であって、図１５に後続するフロー図である。
【発明を実施するための形態】
【００３１】
図１（Ａ）を参照して、この実施例の位置推定システム１０は、電気車椅子のような移動体１２を含み、移動体１２は、その下部に、左右の前輪（キャスター）１４ａ、１４ｂおよび左右の後輪１６ａ、１６ｂを備えている。また、移動体１２の左側の枠の上部には操作レバー１８が設けられる。さらに、移動体１２の左側の枠であり、左足を置くステップ２０ａの左側に、距離センサ２２ａが取り付けられる。ただし、距離センサ２２ａは、移動体１２の右側の枠であり、右足を置くステップ２０ｂの右側に取り付けてもよい。また、移動体１２を上方から見た模式図である図１（Ｂ）に示すように、移動体１２の後方に、距離センサ２２ｂが設けられる。
【００３２】
なお、この実施例では、距離センサ２２ａおよび２２ｂを移動体１２の前後に取り付けるようにしてあるが、移動体１２の左右に取り付けるようにしてもよい。このように２つの距離センサ２２ａ、２２ｂを移動体１２の前後または左右に設けるのは、移動体１２の周りを全周（３６０°）に渡って、距離を検出（計測）するためである。ただし、これは単なる一例であり、距離センサは１つでもよく、または、３つ以上設けてもよい。
【００３３】
また、この実施例では、距離センサ２２ａおよび２２ｂは、或る環境内の壁、柱のような固定物（静的物体）のみならず、人間のような移動物（動的物体）を検出（判別）する。このため、後述するように、距離センサ２２ａ、２２ｂを、その検出位置が地面ないし床面から所定の高さになるように設置してある。この実施例では、所定の高さは１１ｃｍである。これは、人間の脚（足首辺り）を検出するためである。
【００３４】
図１（Ａ）に戻って、移動体１２の座席シートの下側であり、左の後輪１６ａと右の後輪１６ｂの間には、ボックス２４が設けられる。この中に、後述するコンピュータ３０、入出力インターフェイス（以下、単に「インターフェイス」という。）３２、モータドライバ３４ａ、３４ｂ、モータ３６ａ、３６ｂおよびエンコーダ３８ａ、３８ｂが設けられる（図２参照）。ただし、コンピュータ３０およびインターフェイス３２は、ボックス２４の外に設けるようにしてもよい。
【００３５】
図２は図１（Ａ）および（Ｂ）に示した位置推定システム１０の電気的な構成を示すブロック図である。図２に示すように、位置推定システム１０は、移動体１２の位置推定装置として機能するコンピュータ３０を含む。コンピュータ３０は、汎用のパーソナルコンピュータやワークステーションなどのコンピュータであり、ＣＰＵ３０ａ、ＲＡＭ３０ｂおよびＨＤＤ３０ｃなどのコンポーネントを備える。
【００３６】
また、コンピュータ３０には、上述した操作レバー１８および距離センサ２２ａ、２２ｂが接続されるとともに、インターフェイス３２が接続される。インターフェイス３２には、モータドライバ３４ａを介してモータ３６ａが接続されるとともに、モータドライバ３４ｂを介してモータ３６ｂが接続される。また、インターフェイス３２には、エンコーダ３８ａおよび３８ｂが接続される。
【００３７】
図示は省略するが、モータ３６ａの回転軸と左の後輪１６ａの回転軸がギアを用いて連結され、モータ３６ｂの回転軸と右の後輪１６ｂの回転軸がギアを用いて連結される。
【００３８】
コンピュータ３０は、この実施例の位置推定システム１０の全体的な制御を司る。この実施例では、ＣＰＵ３０ａは、操作レバー１８からの操作入力に応じてモータ３６ａ、３６ｂの駆動を制御したり、つまり移動体１２の移動を制御したり、距離センサ２２ａ、２２ｂおよびエンコーダ３８ａ、３８ｂで検出された距離データや回転数データを取得および記憶したりする。また、コンピュータ３０は、移動体１２が移動する（サービスを提供する）環境についての地図（地図データ）と、検出したデータに基づいて、移動体１２の位置を推定（決定）する。
【００３９】
操作レバー１８は、ユーザによって操作され、操作に応じた信号（操作入力）をコンピュータ３０に与える。たとえば、前進、後退、停止、左折、右折および旋回などの操作を行うことができる。
【００４０】
ただし、リモートコントローラ（図示せず）を用いて、遠隔操作により、操作入力をコンピュータ３０に与えることもできる。
【００４１】
距離センサ２２ａ、２２ｂは、たとえば、汎用のレーザレンジファインダ（ＬＲＦ）であり、レーザーを照射し、物体に反射して戻ってくるまでの時間から当該物体との距離を計測するものである。たとえば、トランスミッタ（図示せず）から照射したレーザーを回転ミラー（図示せず）で反射させて、扇状に一定角度（たとえば、０．２５度）ずつスキャンする。また、この実施例では、検出範囲（図３参照）の全体をスキャンする周期は、２５ｍｓｅｃである。
【００４２】
図３は、距離センサ２２ａ、２２ｂの検出範囲（計測範囲）を説明するための図解図である。この実施例の距離センサ２２ａ、２２ｂの計測範囲は、半径Ｒ（Ｒ≒１０〜１５ｍ）の扇形状で示される。扇の角度は、正面方向を中心として左側および右側のそれぞれに１３５°である。つまり、正面を中心とする２７０°の範囲について距離を検出することができる。
【００４３】
図２に戻って、モータドライバ３４ａ、３４ｂは、コンピュータ３０からの指示に基づいて、モータ３６ａ、３６ｂを駆動する。エンコーダ３８ａは、モータ３６ａの回転数（ｒｐｓ）を検出し、回転数についてのデータ（回転数データ）を、インターフェイス３２を介してコンピュータ３０に入力する。同様に、エンコーダ３８ｂは、モータ３６ｂの回転数を検出し、回転数についての回転数データを、インターフェイスを介してコンピュータ３０に入力する。
【００４４】
コンピュータ３０は、距離センサ２２ａ、２２ｂで検出される距離データと、エンコーダ３８ａ、３８ｂで検出される回転数データを取得して、ＲＡＭ３０ｂ（ＨＤＤ３０ｃでもよい。）に記憶する。ただし、距離センサ２２ａで検出される距離データと距離センサ２２ｂで検出される距離データは区別されるとともに、同じ時点において検出された距離データと回転数データは互いに関連付けられる。
【００４５】
たとえば、移動体１２は、身振り手振りのような身体動作および発話の少なくとも一方を用いたコミュニケーションを実行可能なロボットなどに適用され、当該ロボットは、自身が適用される環境（ショッピングモールやイベント会場など）において、道案内、接客、商品や展示品の説明（案内）などのサービスを提供する。たとえば、本件の出願人等が開発するとともに、製造販売するコミュニケーションロボット「Ｒｏｂｏｖｉｅ（ロボビー）（登録商標）」を用いることができる。ロボットは、自身が適用される環境内を移動し、移動した位置（場所）に応じたガイドを実行する。したがって、ロボット（移動体１２）は、自身の位置を知る必要がある。
【００４６】
ただし、ロボット（移動体１２）のスタート位置および向き（初期位置および初期の向き）、通過点および終点は外部から与えられ、初期位置から移動を開始する。たとえば、スタート位置、１または複数の通過点および終点は、時系列に従う順番で指示される。
【００４７】
したがって、位置推定システム１０では、移動体１２の位置を推定する。この実施例では、パーティクルフィルタを用いて移動体１２の位置が推定される。つまり、移動体１２が追跡される。パーティクルフィルタとは、多数の粒子を用いて、前時刻の状態からの予測と現在の観測情報から現在の状態を推定する手法である。パーティクルフィルタでは、予測、観測、リサンプリングを繰り返すことによって移動体１２を追跡する。
＜予測＞
時刻ｔ−１のパーティクルｉから時刻ｔにおけるパーティクルｉの位置（方向を含む“状態”）を予測する。この実施例では、移動体１２の状態ベクトルはｘ＝［ｘ，ｙ，θ］Ｔであり、位置（ｘ，ｙ）および向き（θ）で表される。また、パーティクルフィルタはｎ個（この実施例では、４００個）のパーティクルを含み、ｉ番目のパーティクルの状態ベクトルはｘｉ＝［ｘｉ，ｙｉ，θｉ］Ｔで表される。この実施例では、時刻ｔにおけるパーティクルｉの状態ベクトルｘitは、数１に従って算出される。
【００４８】
［数１］
ｘit＝ｆ（ｘit-1，ｕt-1，Δｔ）
ただし、数１に示すｕは移動体１２の動作情報ｕ＝［ｖ,ω］Tであり、数２に従って算出される。ここで、ｖは移動体１２の速度であり、ωは車輪１６ａおよび１６ｂの車軸の中心（図４参照）であり、車軸と垂直な軸周りの角度速である。また、ＶL、ＶRは移動体１２の左右の車輪（１６ａ、１６ｂ）の移動速度であり、Ｄは移動体１２の車輪（１６ａ、１６ｂ）間距離である。
【００４９】
［数２］
2014174683.tif 000003
【００５０】
左右の車輪（１６ａ、１６ｂ）の移動速度ＶL、ＶRは、左右の車輪（１６ａ、１６ｂ）の回転数から算出することができ、また、車輪間隔Ｄは予め知ることができる。ただし、回転数に応じた移動速度ＶL、ＶRのテーブルを用意しておき、計算せずに、テーブルを参照することにより移動速度ＶL、ＶRを取得してもよい。
【００５１】
そして、算出された動作ｕ＝［ｖ,ω］Tを積分することにより、前回算出した移動体１２の位置（ｘt-1,ｙt-1）および向きθt-1からの移動距離ｄと回転角度αを求めることができる。ただし、初期位置（ｘ0，ｙ0）および初期の向きθ0は外部から与えられる。したがって、上記の数１は数３のように表すことができる。
【００５２】
［数３］
ｘit＝ｘit-1＋ｖ・ｃｏｓ（θt-1）・Δｔ
ｙit＝ｙit-1＋ｖ・ｓｉｎ（θt-1）・Δｔ
θit＝θit-1＋ω・Δｔ
このように、前時刻ｔ−１における各パーティクルｉの状態ｘit-1から時間ｔにおける各パーティクルｉの状態ｘitが予測される。
＜観測＞
予測された位置における各パーティクルｉの尤度および重要度（重み）ｗitを算出する。この実施例では、尤度は図５に示すようなグリッドマップについての地図データを用いて算出する。ただし、図５では、移動体１２が移動する環境の一部についてのグリッドマップを示す。図５を参照して分かるように、グリッドマップは、複数のグリッド線が縦横に等間隔で描画されることにより生成される。詳細な説明は省略するが、グリッドマップは、移動体１２が移動する環境（現実空間）を真上方向から俯瞰的に見た場合の２次元のマップである。つまり、グリッドマップは、世界座標系で作成されている。
【００５３】
たとえば、グリッドマップでは、グリッド線の間隔は現実空間における所定の長さ（５〜１０ｃｍ）に相当する。また、壁や柱などの静的物体に相当する部分（距離センサ２２ａ、２２ｂで距離を計測可能な表面部分）が描画されたグリッドに対応して、静的物体であることの情報が記憶されている。図５では、静的物体であることの情報が記憶されているグリッドに黒色を付してある。
【００５４】
上述したように推定された時刻ｔにおける各パーティクルｉの状態ｘitにおけるグリッドマップ上の計測結果（シュミレーション結果）が、実際の距離センサ２２ａ、２２ｂの計測結果（計測値ｚt）と等しくなる確率（尤度）が数４に従って求められる。
【００５５】
［数４］
ｗit=(ｚit｜ｘit）
ｚit＝hc／Total
ただし、この実施例では、計測値ｚtは、１周期にスキャンするレーザーの総数Total（この実施例では、１０８０）のうち、実際に距離を計測したレーザーの数hcについての割合である。ただし、距離センサ２２ａ、２２ｂのそれぞれについて、計測値ｚtが求められ、尤度もそれぞれの距離センサ２２ａ、２２ｂについて求められる。
【００５６】
そして、数５に示すように、各パーティクルｉの重みｗitは、すべてのパーティクルｉの尤度合計で各パーティクルｉの尤度を正規化することにより、算出（決定）される。ただし、数５において、ｎはパーティクルｉの総数（この実施例では、４００）である。また、数５では、“＝”は代入を意味する。
【００５７】
［数５］
2014174683.tif 000004
【００５８】
この実施例では、尤度が重みｗitとして決定されるが、これに限定される必要はない。尤度の大きさに比例して重みｗitを決定してもよいし、尤度の最も高いパーティクルｉから順に重みｗitを線形的に変化させるように決定してもよいし、一定の閾値毎に重みｗitを段階的に変化するように決定してもよい。
【００５９】
ただし、各パーティクルｉについては、グリッドマップ上の状態ｘitが示す位置（ｘt，ｙt）において、距離センサ２２ａ、２２ｂと同様に、状態ｘitが示す向きθtを中心に左右１３５度の範囲で０．２５度ずつスキャンした場合に、レーザーが静的物体であることの情報が記憶されたグリッドに当った数が検出され、スキャンした総数（１０８０）における当った数の割合が算出される。ただし、レーザー光が届くのは距離Ｒである。
＜リサンプリング＞
観測により求めた各パーティクルｉの重みｗitに比例した確率で復元抽出によりパーティクルｉを選択し、時刻ｔでのパーティクルｉの集合を求める。したがって、重みｗit（尤度）の小さい（低い）パーティクルｉは選択されずに消滅し、重みｗit（尤度）の大きい（高い）パーティクルｉは何度も選択され仲間を増やす（分裂する）。リサンプリングされたパーティクルｉが次の推定に用いられる。ただし、この実施例では、各パーティクルｉの平均座標が移動体１２の位置(ｘt，ｙt）として推定され、各パーティクルｉの平均の向きが移動体１２の向き（θt）として推定される。
【００６０】
このように移動体１２の位置を推定（修正）するのは、回転数データに基づいてジオメトリ計算（数２、数３）により、初期位置（ｘ0，ｙ0）および初期の向き（θ0）から累積的に時間毎の移動体１２の位置および向きを求める場合には、車輪１６ａ、１６ｂの滑りの発生等の要因により、誤差が発生するからである。実験した環境下においては、移動体１２が３０ｃｍ進む毎に約５％のずれ（誤差）が発生した。
【００６１】
また、このような位置推定システム１０は、ショッピングモールやイベント会場など、人間、ショッピングカートや台車のような動的物体が存在する環境において移動する（或るサービスを提供する）移動体１２の位置を推定するため、距離センサ２２ａ、２２ｂで検出された距離データのうち、人間などの動的物体を計測した場合の距離データを消去する必要がある。このような動的物体の情報については、地図データ（グリッドマップ）に記憶されていないからである。
【００６２】
そこで、この実施例では、距離データに基づいて、壁、柱などの静的物体（環境）以外の動的物体などを計測した距離データを判別して、位置推定のために不要な距離データを消去するようにしてある。以下、人間または人間と思われる動的物体についての距離データを消去する場合について説明する。
【００６３】
たとえば、図６には、壁の前に２人の人間が存在し、そのような状況において、移動体１２が壁や人間に対面し、移動体１２に設けられた距離センサ２２ａで距離を検出する様子が示される。ただし、図６では、壁、人間および移動体１２を上方から見た図である。また、ここでは、簡単のため、距離センサ２２ａの計測範囲は、正面を中心とする約９０°の範囲であり、その計測範囲を約３°ずつスキャンした場合について示してある。さらに、図６では、検出（計測）される距離は点線で示してある。また、上述したように、距離センサ２２ａは、床ないし地面から所定の高さに設置され、したがって、人間については、その脚（足首辺り）との距離を計測する。
【００６４】
図６に示すような場合について、不要な距離データを消去する方法について説明する。この実施例では、まず、移動体１２に搭載された距離センサ２２ａ、２２ｂのそれぞれで計測された距離データから、当該距離データを検出したときの移動体１２の位置および向きを基準とする２次元の座標系（移動体座標系）の位置（点）データが算出される。このとき、２つの距離センサ２２ａ、２２ｂの取り付け位置が考慮される。つまり、移動体１２の位置と、距離センサ２２ａ、２２ｂの取り付け位置のずれが調整される。当然のことではあるが、距離データを検出したときの距離センサ２２ａ、２２ｂの計測範囲における角度も考慮される。ただし、この実施例では、世界座標系における移動体１２の位置（ｘt,ｙt）および向きθtは所定時間（たとえば、３０秒）毎に推定（更新）されるため、距離データは、推定された現実空間上の位置（ｘt,ｙt）および向きθtに基づいて、２次元の世界座標系の位置データに変換される。
【００６５】
この変換された位置データに対応する位置（点）を所定のルールに従ってクラスタリングし、クラスタリングされた点群（クラスタ）の特徴から人間を判別し、人間であることが判別されたクラスタに含まれる位置（点）データに対応する距離データを消去する。ただし、クラスタに含まれる点の数が所定数ＮＰ（この実施例では、５）よりも少ない場合には、動的物体であるか静的物体であるかを判別するのが困難であるため、このようなクラスタに含まれる位置データに対応する距離データについても消去するようにしてある。このクラスタに含まれる位置データ（点）の数もクラスタの特徴の１つである。
【００６６】
クラスタリングでは、距離が所定値ＴＨ（この実施例では、１０ｃｍ）未満である位置（点）同士が同じクラスタに分類される。具体的には、着目する点との距離が最も近い（短い）点を検出し、その距離が所定値ＴＨ未満であるかどうかを判断する。そして、距離が所定値ＴＨ未満である場合には、検出した点を着目する点と同じクラスタに分類する。一方、距離が所定値ＴＨ以上である場合には、検出した点を着目する点とは異なる新しいクラスタに分類する。そして、新しいクラスタに分類した点を着目する点として、上記のようなクラスタリングを行う。このようにして、クラスタリングはすべての点について行われる。
【００６７】
図７は、図６において移動体１２の距離センサ２２ａによって距離を計測された点（位置）を白丸印で示す。ただし、図７では、距離を計測された点（位置）を分かり易く示すために、壁と人間を省略してある。このような点が上述した方法により、クラスタリングされる。ただし、上述したように、実際には、世界座標系に座標変換された位置データについてクラスタリングが行われる。
【００６８】
図８は、図７に示す各点（位置）について、上述した方法により、クラスタリングを行った場合に生成されたクラスタを示す。図８から分かるように、たとえば、７つのクラスタが生成される。図８（図９および図１０も同じ。）では、同じクラスタに分類されていることを示すために点を囲んである。
【００６９】
クラスタリングを終えると、各クラスタについての特徴を抽出（算出）する。この実施例では、クラスタの特徴は、当該クラスタに含まれる複数の点の分散についての主成分であり、分散行列についての固有値λ１，λ２である。ただし、λ１は分散行列についての固有値の中で最も大きい固有値であり、λ２は固有値λ１の次に（２番目に）大きい固有値である。図９では、分かり易く示すために、クラスタの特徴（分散の主成分）を実線で示し、クラスタに含まれる点を省略してある。
【００７０】
上述したように、クラスタに含まれる点の数が所定数ＮＰ未満であるクラスタについては、動的物体か静的物体かを判別することが困難であるため、固有値を算出するまでも無く、消去される。したがって、図８および図９に示すように、点の数が４つであるクラスタ４は消去される。図９（図１１も同じ。）では、クラスタ４が消去されたことを、当該クラスタ４に含まれる点を囲む線を点線で記載することにより表現している。ただし、実際には、クラスタに含まれる点（位置データ）に対応する距離データが消去される。
【００７１】
固有値λ１，λ２を算出する場合には、まず、各クラスタでは、クラスタに含まれる複数の点についての重心（または平均）（μx，μy）が数６に従って算出される。ただし、クラスタにｎ個の点が含まれているものとする。
【００７２】
［数６］
2014174683.tif 000005
【００７３】
算出された重心（μx，μy）を基準とした場合についてのクラスタの分散行列Σは数７で示され、その固有値が数８に従って算出される。
【００７４】
［数７］
Σij = E[(Xi −μi)(Xj −μj)]=E(XiXj)−E(Xi)E(Xj)
ただし、μi=E(Xi)であり、この実施例では、２つの固有値λ１，λ２を算出するため、ｉ＝ｊ＝１，２である。
【００７５】
［数８］
Σｘ−λＩｘ＝(Σ−λＩ)ｘ＝０
det(Σ−λＩ)＝０
ただし、この実施例では、２つの固有値λ１，λ２を算出するため、Ｉは２行２列の単位行列である。
【００７６】
このようにして、固有値λ１，λ２が算出されると、この固有値λ１，λ２を用いて、動的物体であるか、静的物体であるかが判別される。具体的には、固有値λ１が第１閾値（たとえば、６５ｃｍ）よりも大きい場合、または、固有値λ２が第２閾値（たとえば、３ｃｍ）よりも小さい場合には、このような固有値λ１，λ２を有するクラスタを静的物体と判別する。ただし、第１閾値および第２閾値は、人間のような動的物体と壁や柱のような静的物体を判別するために実験等により経験的に得られた数値である。
【００７７】
このように、固有値λ２が第２閾値よりも小さい場合には、クラスタの形状が細長く、壁に対応するクラスタと考えられるからである。また、固有値λ１が第１閾値よりも大きい場合には、図１０に示すように、通路の角に相当する部分の壁や柱に対応するクラスタと考えられるからである。したがって、固有値λ１が第１閾値以下である場合、または、固有値λ２が第２閾値以上である場合には、人間の脚（足首辺り）に相当する物体すなわち人間または人間のような物体に対応するクラスタであると考え、このような固有値λ１，λ２を有するクラスタを動的物体と判別し、当該クラスタに含まれる位置データに対応する距離データを消去する。
【００７８】
ただし、この実施例では、固有値λ１，λ２の両方が第２閾値（たとえば、３ｃｍ）よりも小さい場合には、物体が小さ過ぎると判断し、当該固有値λ１，λ２を有するクラスタに含まれる位置データに対応する距離データを消去する。
【００７９】
図１１は、図９に示したクラスタ１−３、５−７について、それぞれの特徴に基づいて静的物体であるか動的物体であるかを判別し、動的物体であることが判別されたクラスタ（図１１では、クラスタ２、３、５、６）を消去した図である。したがって、図６−図９および図１１に示すように、壁や柱のような静的物体についてのクラスタに含まれる位置データに対応する距離データのみが残され、人間のような動的物体についてのクラスタおよび動的物体または静的物体を判別できないクラスタのような不要なクラスタに含まれる位置データに対応する距離データは消去される。
【００８０】
図１２は、図２に示したコンピュータ３０に内蔵されるＲＡＭ３０ｂのメモリマップ３００の一例を示す。図１２に示すように、ＲＡＭ３０ｂは、プログラム記憶領域３０２およびデータ記憶領域３０４を含む。
【００８１】
プログラム記憶領域３０２には、メイン処理プログラム３０２ａ、データ記憶プログラム３０２ｂ、位置データ算出プログラム３０２ｃ、不要データ消去プログラム３０２ｄ、位置推定プログラム３０２ｅおよび自律航行プログラム３０２ｆなどが記憶される。
【００８２】
メイン処理プログラム３０２ａは、位置推定システム１０のメインルーチンを処理するためのプログラムである。データ記憶プログラム３０２ｂは、距離データおよび回転数データを取得して、ＲＡＭ３０ｂのデータ記憶領域３０４に記憶するためのプログラムである。
【００８３】
位置データ算出プログラム３０２ｃは、検出した距離データから、移動体１２の位置および向きを基準として移動体座標系の位置データを算出し、算出した位置データを世界座標系の位置データに座標変換するためのプログラムである。不要データ消去プログラム３０２ｄは、移動体１２の位置推定に先立って、動的物体と判別されたクラスタと、動的物体または静的物体を判別できないクラスタに含まれる位置データに対応する距離データを消去するためのプログラムである。位置推定プログラム３０２ｅは、不要データ消去プログラム３０２ｄに従って削除された距離データを除く距離データと地図データ３０４ｅに基づき、パーティクルフィルタを用いて、移動体１２の位置（状態）を推定するためのプログラムである。自動航行プログラム３０２ｆは、地図データ３０４ｅを参照して、当該移動体１２を目的地（１または複数の通過点と終点）に向けて自動的に移動させるためのプログラムである。
【００８４】
図示は省略するが、位置推定に必要な他のプログラムもプログラム記憶領域３０２に記憶される。
【００８５】
また、データ記憶領域３０４には、距離データバッファ３０４ａおよび回転数データバッファ３０４ｂが設けられる。距離データバッファ３０４ａは、データ記憶プログラム３０２ｂに従って距離データを記憶する。同様に、回転数データバッファ３０４ｂは、データ記憶プログラム３０２ｂに従って回転数データを記憶する。
【００８６】
また、データ記憶領域３０４には、位置データ３０４ｃ、クラスタデータ３０４ｄ、地図データ３０４ｅおよび状態データ３０４ｆが記憶される。位置データ３０４ｃは、位置データ算出プログラム３０２ｃに従って距離データバッファ３０４ａに記憶された距離データから算出され、世界座標系に座標変換された位置（点）データである。詳細な説明は省略するが、距離データと対応する位置データとは互いに関連付けされている。クラスタデータ３０４ｄは、距離データから算出され、座標変換された位置データをクラスタリングしたクラスタ毎のデータである。地図データ３０４ｅは、移動体１２が移動する環境についてのグリッドマップのデータである。この地図データ３０４ｅでは、グリッドマップのみならず、上述したように、静的物体（の表面）に相当するグリッドに対応して静的物体であることを示す情報が記憶されている。状態データ３０４ｆは、位置推定プログラム３０２ｅに従って推定された移動体１２の状態（位置および向き）についてのデータである。
【００８７】
図示は省略するが、データ記憶領域３０４には、位置推定のために必要な他のデータが記憶されたり、位置推定のために必要なフラグやタイマ（カウンタ）などが設けられたりする。
【００８８】
図１３および図１４は図２に示したＣＰＵ３０ａの全体処理を示すフロー図である。ＣＰＵ３０ａは、外部からスタート位置、１または複数の通過点、および終点を与えられると、図１３に示すように全体処理を開始し、ステップＳ１で、距離センサ２２ａ、２２ｂを駆動する。このとき、ＣＰＵ３０ａは、距離センサ２２ａ、２２ｂからの距離データの取得および記憶を開始する。
【００８９】
次のステップＳ３では、最初の通過点を設定する。ここでは、指示された通過点のうち、最初の通過点の座標が設定される。そして、ステップＳ５で、移動体１２の自律航行を開始する。つまり、ＣＰＵ３０ａは、全体処理とは並行して、移動体１２の移動制御の処理を実行する。詳細な説明および図示は省略するが、移動制御の処理では、ＣＰＵ３０ａは、地図データ３０４ｅを参照して、指示された通過点ないし終点に向かうように、モータドライバ３４ａ、３４ｂを制御し、モータ３６ａ、３６ｂを駆動する。
【００９０】
続くステップＳ７では、距離データおよび回転数データを取得するとともに記憶する。つまり、ＣＰＵ３０ａは、距離センサ２２ａ、２２ｂからの距離データを取得するとともに、エンコーダ３８ａ、３８ｂからの回転数データを取得し、互いに関連付けて、それぞれ時系列に従ってＲＡＭ３０ｂに記憶する。
【００９１】
続いて、ステップＳ９で、移動体１２の位置および向きに応じて距離データから位置データを算出する。ここでは、上述したように、距離センサ２２ａ、２２ｂのそれぞれで検出された距離のデータから、移動体１２の位置および向きを基準として移動体座標系の位置データが算出される。このとき、上述したように、２つの距離センサ２２ａ、２２ｂの取り付け位置が考慮される。そして、ステップＳ１１で、ステップＳ９で算出された位置データを世界座標系の位置データに変換する。
【００９２】
図１４に示すように、続くステップＳ１３では、後述する不要データの消去処理（図１５および図１６参照）を実行し、ステップＳ１５で、前回位置推定（Ｓ１７）を行ってから所定時間を経過したかどうかを判断する。このとき、ＣＰＵ３０ａは、図示しないタイマを用いて時間をカウントする。また、この実施例では、所定時間が経過したかどうかを判断するようにしてあるが、所定距離移動したかどうかを判断するようにしてもよい。ただし、ステップＳ１５の処理は省略してもよい。
【００９３】
なお、全体処理を開始し、最初にステップＳ１５の判断処理が実行される場合には、位置推定を一度も行っていないため、“ＹＥＳ”と判断するようにしてある。
【００９４】
ステップＳ１５で“ＮＯ”であれば、つまり前回位置推定を行ってから所定時間を経過していなければ、そのまま図１３に示したステップＳ７に戻る。一方、ステップＳ１５で“ＹＥＳ”であれば、つまり前回位置推定を行ってから所定時間を経過すれば、ステップＳ１７で、地図を参照して位置および角度を推定する。ここでは、ＣＰＵ３０ａは、上述したように、不要データを消去した後の距離データを用いて、パーティクルフィルタにより移動体１２の位置および向きを推定（修正）する。したがって、ステップＳ１７の処理によって、状態データ３０４ｆが更新される。
【００９５】
続くステップＳ１９では、通過点に到達したかどうかを判断する。ここでは、ＣＰＵ３０ａは、ステップＳ１７で推定した位置が設定された通過点と一致するかどうかを判断する。ステップＳ１９で“ＹＥＳ”であれば、つまり通過点に到達すれば、ステップＳ２１で、次の通過点または終点を設定して、ステップＳ７に戻る。したがって、移動体１２が次の通過点または終点に向けて移動するように移動制御される。一方、ステップＳ１９で“ＮＯ”であれば、つまり通過点に到達していなければ、ステップＳ２３で、終点に到達したかどうかを判断する。ここでは、ＣＰＵ３０ａは、ステップＳ１７で推定した位置が終点と一致するかどうかを判断する。
【００９６】
ステップＳ２３で“ＮＯ”であれば、つまり終点に到達していなければ、ステップＳ７に戻る。一方、ステップＳ２３で“ＹＥＳ”であれば、つまり終点に到達すれば、全体処理を終了する。
【００９７】
なお、詳細な説明は省略するが、位置を推定する（ステップＳ１７の処理を実行する）ために用いられた距離データおよび回転数データは、バッファ３０４ａ、３０４ｂから消去される。
【００９８】
図１５および図１６は、図１４に示したステップＳ１３の不要データの消去処理を示すフロー図である。図１５に示すように、ＣＰＵ３０ａは、不要データの消去処理を開始すると、ステップＳ３３で、着目する点（位置データに対応する位置）についての新しいクラスタを生成する。たとえば、ステップＳ３３では、まだクラスタに分類されていない位置データが任意に選択され、新しいクラスタが生成される。このとき、着目する点についての位置データを含む新しいクラスタのデータが追加される。つまり、新しいクラスタのデータを追加することにより、クラスタデータ３０４ｄが更新される。
【００９９】
続くステップＳ３５では、次の点への距離が所定値ＴＨ（この実施例では、１０ｃｍ）未満であるかどうかを判断する。ここでは、ＣＰＵ３０ａは、着目する点に最も近い点との距離が所定値ＴＨ未満であるかどうかを判断するのである。ステップＳ３５で“ＮＯ”であれば、つまり次の点への距離が所定値ＴＨ以上であれば、新しいクラスタを生成するために、ステップＳ３３に戻る。一方、ステップＳ３５で“ＹＥＳ”であれば、つまり次の点への距離が所定値ＴＨ未満であれば、ステップＳ３７で、当該次の点を既存のクラスタに追加する。つまり、当該次の点が着目する点と同じクラスタに分類される。
【０１００】
続いて、ステップＳ３９では、すべての点についてクラスタリングを行ったかどうかを判断する。ここでは、ＣＰＵ３０ａは、クラスタに分類されていない位置データが残っていないかどうかを判断するのである。ステップＳ３９で“ＮＯ”であれば、つまりクラスタリングを行っていない点が残っていれば、ステップＳ３５に戻る。図示は省略するが、このとき、直前にクラスタに分類された点が着目する点に設定される。
【０１０１】
一方、ステップＳ３９で“ＹＥＳ”であれば、つまりすべての点についてクラスタリングを行えば、クラスタリングを終了したと判断し、ステップＳ４１で、１番目のクラスタを読み出し、図１６に示すステップＳ４３で、当該クラスタに含まれる点の数が所定数ＮＰ（この実施例では、５）未満かどうかどうかを判断する。
【０１０２】
なお、ステップＳ４１では、説明の便宜上、１番目のクラスタを読み出すように説明したが、ここでは、任意のクラスタが読み出される。後述するステップＳ５７においても同様であるが、このステップＳ５７では、静的物体であることが判別されていない任意のクラスタが読み出される。ただし、クラスタを生成した順番に従ってクラスタを読み出すようにしてもよい。
【０１０３】
ステップＳ４３で“ＹＥＳ”であれば、つまり当該クラスタに含まれる点の数が所定数ＮＰ未満であれば、静的物体か動的物体かの判別が出来ないと判断して、ステップＳ５３に進む。一方、ステップＳ４３で“ＮＯ”であれば、つまり当該クラスタに含まれる点の数が所定数ＮＰ以上であれば、ステップＳ４５で、数４−数６に従って、当該クラスタの固有値λ1、λ2を算出する。
【０１０４】
次のステップＳ４７では、固有値λ1およびλ2の両方が第２閾値Ｈmin（この実施例では、３ｃｍ）未満であるかどうかを判断する。上述したように、ここでは、静的物体または動的物体を判別することができるかどうかが判断されるのである。ステップＳ４７で“ＹＥＳ”であれば、つまり固有値λ1およびλ2の両方が第２閾値Ｈmin未満であれば、そのままステップＳ５３に進む。一方、ステップＳ４７で“ＮＯ”であれば、つまり固有値λ1およびλ2の少なくとも一方が第２閾値Ｈmin以上であれば、ステップＳ４９で、固有値λ1が第１閾値Ｈmaxよりも大きい、または、固有値λ2が第２閾値Ｈmin未満であるかどうかを判断する。上述したように、ここでは、人間のような動的物体であるか、壁や柱のような静的物体であるかが判断（判別）される。
【０１０５】
ステップＳ４９で“ＹＥＳ”であれば、つまり固有値λ1が第１閾値Ｈmaxよりも大きい、または、固有値λ2が第２閾値Ｈmin未満であれば、壁や柱のような静的物体であると判断し、ステップＳ５１で、当該クラスタを静的物体に対応するクラスタに決定して、ステップＳ５５に進む。
【０１０６】
一方、ステップＳ４９で“ＮＯ”であれば、つまり固有値λ1が第１閾値Ｈmax以下である、または、固有値λ2が第２閾値Ｈmin以上である場合には、人間のような動的物体であると判断して、ステップＳ５３で、当該クラスタに含まれる位置データに対応する距離データを距離データバッファ３０４ａから消去して、ステップＳ５５に進む。つまり、ステップＳ５３の処理を実行することにより、位置推定に不要な距離データが消去される。
【０１０７】
ステップＳ５５では、すべてのクラスタを判別したかどうかを判断する。ステップＳ５５で“ＮＯ”であれば、つまりまだ判別していないクラスタが存在する場合には、ステップＳ５７で、次のクラスタを読み出し、ステップＳ４３に戻る。一方、ステップＳ５５で“ＹＥＳ”であれば、つまりすべてのクラスタを判別すれば、図１４に示した全体処理にリターンする。
【０１０８】
この実施例によれば、距離センサの計測結果から静的物体および動的物体を判別し、静的物体以外の物体についての計測結果を消去し、静的物体についての計測結果のみを用いて移動体の位置を推定するので、動的物体の計測結果による影響を可及的少なくすることができる。つまり、人間などの動的物体が存在する環境においても、出来る限り正しく位置を推定することができる。
【０１０９】
また、この実施例では、距離センサで検出された距離データを位置データに変換するだけで不要なデータを消去することができるので、カメラ等の他のセンサを設ける必要が無い。つまり、コストが高くなったり、画像処理などの負荷が大きくなったりすることもない。
【０１１０】
なお、この実施例では、動的物体の例として人間ないし人間のような物体を判別する場合について詳細に説明したが、上述したように、ショッピングカートや台車などの他の動的物体を判別することもできる。ただし、ショッピングカートや台車などの他の動的物体を判別する場合には、クラスタに含まれる位置データ（点）の数やその分散についての主成分（分散行列の固有値）が満たす条件をそれぞれの場合について決定する必要がある。
【０１１１】
また、この実施例では、距離データを世界座標系の位置データに変換してクラスタリングするようにしたが、距離データを移動体を基準とした座標系（移動体座標系）の位置データに変換してクラスタリングするようにしてもよい。
【０１１２】
さらに、この実施例で示した具体的な数値は単なる一例であり、限定されるべきではなく、実施される製品等に応じて適宜変更可能である。また、この実施例では、所定値や閾値を用いて大小を判断するステップにおいて、当該所定値や当該閾値を含まないようにしたが、当該所定値や当該閾値を含んで大小を判断するようにしてもよい。
【符号の説明】
【０１１３】
１０  …位置推定システム
１２  …移動体
１６ａ、１６ｂ  …車輪
１８  …操作レバー
２２ａ、２２ｂ  …距離センサ
３０  …コンピュータ
３４ａ、３４ｂ  …モータドライバ
３６ａ、３６ｂ  …モータ
３８ａ、３８ｂ  …エンコーダ
【図１】
2014174683.tif 000006
【図２】
2014174683.tif 000007
【図３】
2014174683.tif 000008
【図４】
2014174683.tif 000009
【図５】
2014174683.tif 000010
【図６】
2014174683.tif 000011
【図７】
2014174683.tif 000012
【図８】
2014174683.tif 000013
【図９】
2014174683.tif 000014
【図１０】
2014174683.tif 000015
【図１１】
2014174683.tif 000016
【図１２】
2014174683.tif 000017
【図１３】
2014174683.tif 000018
【図１４】
2014174683.tif 000019
【図１５】
2014174683.tif 000020
【図１６】
2014174683.tif 000021