(57)【要約】    【構成】  地図作成システム１０は、移動体１２を含み、移動体はユーザの操作に従って環境内を移動される。このとき、移動体に設けられた距離センサ２２ａ、２２ｂによって距離データが検出され、移動体の左右の車輪１６ａ、１６ｂのそれぞれの回転数データも検出される。コンピュータ（３０）は、回転数データに基づいて移動体の位置および向きを算出し、距離データを、計測した物体の位置に対応する位置データに変換する。この位置データを所定のルールでクラスタリングし、各クラスタの特徴量に基づいて壁や床などの静的物体以外のクラスタ（位置データ）を消去する。したがって、静的物体についての位置データを用いて、移動体の位置が推定されるとともに、環境についての地図が作成される。    【効果】  距離データに基づく位置データから地図作成に不要なデータを消去するので、地図を簡単に作成することができる。【選択図】  図１
2014174275.tif 000002
【特許請求の範囲】
【請求項１】
移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するとともに、当該移動体が移動した環境についての地図を作成する地図作成装置であって、
前記距離センサで検出された距離データを、当該距離データの計測時における前記移動体の位置および向きを基準とする位置データに変換するデータ変換手段、
前記データ変換手段によって変換された位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリング手段、
前記クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断手段、および
前記条件判断手段によって所定の条件を満たさないことが判断されたクラスタを消去する消去手段を備える、地図作成装置。
【請求項２】
前記所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることである、請求項１記載の地図作成装置。
【請求項３】
前記所定の条件は、前記クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含む、請求項１または２記載の地図作成装置。
【請求項４】
前記クラスタリング手段によってクラスタリングされた複数のクラスタの各々の特徴を抽出する抽出手段をさらに備え、
前記条件判断手段は、前記抽出手段によって抽出された特徴が前記所定の条件を満たすかどうかを判断する、請求項１ないし３のいずれかに記載の地図作成装置。
【請求項５】
前記抽出手段によって抽出された特徴は、前記クラスタに含まれる位置データが示す複数の点の分散についての特徴量である、請求項４記載の地図作成装置。
【請求項６】
前記特徴量は、前記クラスタに含まれる位置データが示す複数の点の分散行列についての固有値であり、
前記所定の条件は、前記固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい前記固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む、請求項５記載の地図作成装置。
【請求項７】
移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するとともに、当該移動体が移動した環境についての地図を作成するコンピュータの地図作成プログラムであって、
前記コンピュータに、
前記距離センサで検出された距離データを、当該距離データを計測時の前記移動体の位置および向きを基準とする位置データに変換するデータ変換ステップ、
前記データ変換ステップにおいて変換した位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリングステップ、
前記クラスタリングステップにおいてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断ステップ、および
前記条件判断ステップにおいて所定の条件を満たさないことが判断されたクラスタを消去する消去ステップを実行させる、地図作成プログラム。
【請求項８】
移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するとともに、当該移動体が移動した環境についての地図を作成するコンピュータの地図作成方法であって、
前記コンピュータは、
（ａ）前記距離センサで検出された距離データを、当該距離データを計測時の前記移動体の位置および向きを基準とする位置データに変換し、
（ｂ）前記ステップ（ａ）において変換した位置データを所定のルールに従って複数のクラスタにクラスタリングし、
（ｃ）前記ステップ（ｂ）においてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断し、そして
（ｄ）前記ステップ（ｃ）において所定の条件を満たさないことが判断されたクラスタを消去する、地図作成方法。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は地図作成装置、地図作成プログラムおよび地図作成方法に関し、特にたとえば、距離センサを備える移動体を用いて或る環境についての地図を作成する、地図作成装置、地図作成プログラムおよび地図作成方法に関する。
【背景技術】
【０００２】
この発明の背景技術の一例が非特許文献１に開示される。この非特許文献１では、壁など動くことの無い環境の静的な部分に関する情報を含む占有グリッド地図と、一度でも動いた動的物体に関する情報を含む占有グリッド地図を作成および更新し、これらの地図の情報を統合することにより、SLAM(Simultaneous Localization and Mapping)の技術を用いて、地図を作成するとともに、移動ロボットの位置を推定する。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】Denis Wolf, et al.: “Online simultaneous localization and mapping in dynamic environments”, In Proc. of the Intl. Conf. on Robotics and Automation (ICRA), 2004.
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、この背景技術では、２つの地図を作成する必要があり、作成処理の負荷が大きい。また、２つの地図中の情報を統合するのにも手間がかかる。
【０００５】
それゆえに、この発明の主たる目的は、新規な、地図作成装置、地図作成プログラムおよび地図作成方法を提供することである。
【０００６】
この発明の他の目的は、地図を簡単に作成することができる、地図作成装置、地図作成プログラムおよび地図作成方法を提供することである。
【課題を解決するための手段】
【０００７】
第１の発明は、移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するとともに、当該移動体が移動した環境についての地図を作成する地図作成装置であって、距離センサで検出された距離データを、当該距離データの計測時における移動体の位置および向きを基準とする位置データに変換するデータ変換手段、データ変換手段によって変換された位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリング手段、クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断手段、および条件判断手段によって所定の条件を満たさないことが判断されたクラスタを消去する消去手段を備える、地図作成装置である。
【０００８】
第１の発明では、地図作成装置は、移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するとともに、当該移動体が移動した環境についての地図を作成する。データ変換手段は、距離センサで検出された距離データを、当該距離データの計測時における移動体の位置および向きを基準とする位置データに変換する。クラスタリング手段は、データ変換手段によって変換された位置データを所定のルールに従って複数のクラスタにクラスタリングする。条件判断手段は、クラスタリング手段によってクラスタリングされた複数のクラスタの各々が所定の条件を満たすかどうかを判断する。たとえば、所定の条件は、クラスタの特徴についての条件であり、より具体的には、人間のような動的物体であるか、壁や柱のような静的物体であるかを判別するための条件である。消去手段は、条件判断手段によって所定の条件を満たさないことが判断されたクラスタを消去する。つまり、不要な位置データが消去される。
【０００９】
第１の発明によれば、不要な位置データを消去するので、必要なデータのみに基づいて地図を作成することができる。したがって、地図を簡単に作成することができる。
【００１０】
第２の発明は、第１の発明に従属し、所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることである。
【００１１】
第２の発明では、所定のルールは、位置データが示す点同士の距離が所定距離以下または所定距離未満であることであり、近くに有る点が同じクラスタに分類される。
【００１２】
第２の発明によれば、点同士の距離を検出するだけなので、簡単にクラスタリングすることができる。
【００１３】
第３の発明は、第１または第２の発明に従属し、所定の条件は、クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含む。
【００１４】
第３の発明では、所定の条件は、クラスタに含まれる位置データが示す点の数が所定数以下または所定数未満であることを含み、クラスタに含まれる点が少ない場合には、当該クラスタは消去される。たとえば、クラスタに含まれる点の数が少ない場合には、動的物体であるか、静的物体であるかを判別することができないため、当該クラスタは消去されるのである。
【００１５】
第３の発明によれば、動的物体であるか、静的物体であるかを判別できない不要なクラスタを消去することができる。
【００１６】
第４の発明は、第１ないし第３の発明に従属し、クラスタリング手段によってクラスタリングされた複数のクラスタの各々の特徴を抽出する抽出手段をさらに備え、条件判断手段は、抽出手段によって抽出された特徴が所定の条件を満たすかどうかを判断する。
【００１７】
第４の発明では、抽出手段は、複数のクラスタの各々の特徴を抽出する。条件判断手段は、各クラスタの特徴が所定の条件を満たすかどうかを判断する。
【００１８】
第４の発明によれば、クラスタに含まれる点の個数以外の特徴によって所定の条件を満たすかどうかを判断することができる。
【００１９】
第５の発明は、第４の発明に従属し、抽出手段によって抽出された特徴は、クラスタに含まれる位置データが示す複数の点の分散についての特徴量である。
【００２０】
第５の発明では、クラスタの特徴は、当該クラスタに含まれる位置データが示す複数の点の分散についての特徴量であり、たとえば、クラスタの形状に関する特徴である。
【００２１】
第５の発明によれば、クラスタの形状に関する特徴が所定の条件を満たすかどうかで、動的物体であるか、静的物体であるかを判別することができる。
【００２２】
第６の発明は、第５の発明に従属し、特徴量は、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値であり、所定の条件は、固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む。
【００２３】
第６の発明では、特徴量は、クラスタに含まれる位置データが示す複数の点の分散行列についての固有値である。たとえば、特徴量は、クラスタに含まれる複数の点の分散についての主成分であり、分散行列の固有値で表される。たとえば、所定の条件は、固有値の最大値が第１所定値よりも大きいこと、または、２番目に大きい固有値が当該第１所定値よりも小さい第２所定値未満であることをさらに含む。したがって、固有値の最大値が第１所定値以下である場合、または、２番目に大きい固有値が第２所定値以上である場合に、所定の条件を満たさないことが判断され、当該固有値を有するクラスタが消去される。
【００２４】
第６の発明によれば、クラスタに含まれる複数の点の分散行列についての固有値に基づいて不要なデータを消去することができる。
【００２５】
第７の発明は、移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するとともに、当該移動体が移動した環境についての地図を作成するコンピュータの地図作成プログラムであって、コンピュータに、距離センサで検出された距離データを、当該距離データを計測時の移動体の位置および向きを基準とする位置データに変換するデータ変換ステップ、データ変換ステップにおいて変換した位置データを所定のルールに従って複数のクラスタにクラスタリングするクラスタリングステップ、クラスタリングステップにおいてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断する条件判断ステップ、および条件判断ステップにおいて所定の条件を満たさないことが判断されたクラスタを消去する消去ステップを実行させる、地図作成プログラムである。
【００２６】
第８の発明は、移動体に設置した距離センサの検出結果に基づいて当該移動体の位置を推定するとともに、当該移動体が移動した環境についての地図を作成するコンピュータの地図作成方法であって、コンピュータは、（ａ）距離センサで検出された距離データを、当該距離データを計測時の移動体の位置および向きを基準とする位置データに変換し、（ｂ）ステップ（ａ）において変換した位置データを所定のルールに従って複数のクラスタにクラスタリングし、（ｃ）ステップ（ｂ）においてクラスタリングした複数のクラスタの各々が所定の条件を満たすかどうかを判断し、そして（ｄ）ステップ（ｃ）において所定の条件を満たさないことが判断されたクラスタを消去する、地図作成方法である。
【００２７】
第７および第８の発明においても、第１の発明と同様に、地図を簡単に作成することができる。
【発明の効果】
【００２８】
この発明によれば、移動体に設置した距離センサで距離を計測された位置を所定のルールに従って複数のクラスタにクラスタリングし、各クラスタが所定の条件を満たすかどうかを判別するだけで、静的物体についてのクラスタ以外を消去するので、地図を簡単に作成することができる。
【００２９】
この発明の上述の目的，その他の目的，特徴および利点は、図面を参照して行う以下の実施例の詳細な説明から一層明らかとなろう。
【図面の簡単な説明】
【００３０】
【図１】図１はこの発明の地図作成システムの外観構成の一例を示す図解図である。
【図２】図２は図１に示す地図作成システムの電気的な構成の一例を示すブロック図である。
【図３】図３は図１および図２に示す距離センサの検出範囲を説明するための図解図である。
【図４】図４は移動体の位置および向きを算出するためのパラメータを説明するための図解図である。
【図５】図５は動的物体と静的物体を判別する方法を説明するための図解図である。
【図６】図６は動的物体と静的物体を判別する方法を説明するための他の図解図である。
【図７】図７は動的物体と静的物体を判別する方法を説明するためのその他の図解図である。
【図８】図８は動的物体と静的物体を判別する方法を説明するためのさらに他の図解図である。
【図９】図９は静的物体を判定する具体的な場合を説明するための図解図である。
【図１０】図１０は静的物体と判定されたクラスタ以外のクラスタを消去した状態を説明するための図解図である。
【図１１】図１１は作成された地図の一例を示す図解図である。
【図１２】図１２は作成された地図に動的物体について距離を計測された点をさらに表示した一例を示す図解図である。
【図１３】図１３は図２に示すＲＡＭのメモリマップの一例を示す図解図である。
【図１４】図１４は図２に示すＣＰＵの全体処理を示すフロー図である。
【図１５】図１５は図２に示すＣＰＵの不要データの消去処理の一部を示すフロー図である。
【図１６】図１６は図１に示すＣＰＵの不要データの消去処理の他の一部であって、図１５に後続するフロー図である。
【発明を実施するための形態】
【００３１】
図１（Ａ）を参照して、この実施例の地図作成システム１０は、電気車椅子のような移動体１２を含み、移動体１２は、その下部に、左右の前輪（キャスター）１４ａ、１４ｂおよび左右の後輪１６ａ、１６ｂを備えている。また、移動体１２の左側の枠の上部には操作レバー１８が設けられる。さらに、移動体１２の左側の枠であり、左足を置くステップ２０ａの左側に、距離センサ２２ａが取り付けられる。ただし、距離センサ２２ａは、移動体１２の右側の枠であり、右足を置くステップ２０ｂの右側に取り付けてもよい。また、移動体１２を上方から見た模式図である図１（Ｂ）に示すように、移動体１２の後方に、距離センサ２２ｂが設けられる。
【００３２】
なお、この実施例では、距離センサ２２ａおよび２２ｂを移動体１２の前後に取り付けるようにしてあるが、移動体１２の左右に取り付けるようにしてもよい。このように２つの距離センサ２２ａ、２２ｂを移動体１２の前後または左右に設けるのは、移動体１２の周りを全周（３６０°）に渡って、距離を検出（計測）するためである。ただし、これは単なる一例であり、距離センサは１つでもよく、または、３つ以上設けてもよい。
【００３３】
また、この実施例では、距離センサ２２ａおよび２２ｂは、或る環境内の壁、柱のような固定物（静的物体）のみならず、人間のような移動物（動的物体）を検出（判別）する。このため、後述するように、距離センサ２２ａ、２２ｂを、その検出位置が地面ないし床面から所定の高さになるように設置してある。この実施例では、所定の高さは１１ｃｍである。これは、人間の脚（足首辺り）を検出するためである。
【００３４】
図１（Ａ）に戻って、移動体１２の座席シートの下側であり、左の後輪１６ａと右の後輪１６ｂの間には、ボックス２４が設けられる。この中に、後述するコンピュータ３０、入出力インターフェイス（以下、単に「インターフェイス」という。）３２、モータドライバ３４ａ、３４ｂ、モータ３６ａ、３６ｂおよびエンコーダ３８ａ、３８ｂが設けられる（図２参照）。ただし、コンピュータ３０およびインターフェイス３２は、ボックス２４の外に設けるようにしてもよい。
【００３５】
図２は図１（Ａ）および（Ｂ）に示した地図作成システム１０の電気的な構成を示すブロック図である。図２に示すように、地図作成システム１０は、地図作成装置や移動体１２の位置推定装置として機能するコンピュータ３０を含む。コンピュータ３０は、汎用のパーソナルコンピュータやワークステーションなどのコンピュータであり、ＣＰＵ３０ａ、ＲＡＭ３０ｂおよびＨＤＤ３０ｃなどのコンポーネントを備える。
【００３６】
また、コンピュータ３０には、上述した操作レバー１８および距離センサ２２ａ、２２ｂが接続されるとともに、インターフェイス３２が接続される。インターフェイス３２には、モータドライバ３４ａを介してモータ３６ａが接続されるとともに、モータドライバ３４ｂを介してモータ３６ｂが接続される。また、インターフェイス３２には、エンコーダ３８ａおよび３８ｂが接続される。
【００３７】
図示は省略するが、モータ３６ａの回転軸と左の後輪１６ａの回転軸がギアを用いて連結され、モータ３６ｂの回転軸と右の後輪１６ｂの回転軸がギアを用いて連結される。
【００３８】
コンピュータ３０は、この実施例の地図作成システム１０の全体的な制御を司る。この実施例では、ＣＰＵ３０ａは、操作レバー１８からの操作入力に応じてモータ３６ａ、３６ｂの駆動を制御したり、つまり移動体１２の移動を制御したり、距離センサ２２ａ、２２ｂおよびエンコーダ３８ａ、３８ｂで検出された距離データや回転数データを取得および記憶したりする。また、コンピュータ３０は、SLAMの技術を用いて、検出したデータに基づいて、地図を作成したり、移動体１２の位置を推定（決定）したりする。
【００３９】
操作レバー１８は、ユーザによって操作され、操作に応じた信号（操作入力）をコンピュータ３０に与える。たとえば、前進、後退、停止、左折、右折および旋回などの操作を行うことができる。
【００４０】
ただし、リモートコントローラ（図示せず）を用いて、遠隔操作により、操作入力をコンピュータ３０に与えることもできる。
【００４１】
距離センサ２２ａ、２２ｂは、たとえば、汎用のレーザレンジファインダ（ＬＲＦ）であり、レーザーを照射し、物体に反射して戻ってくるまでの時間から当該物体との距離を計測するものである。たとえば、トランスミッタ（図示せず）から照射したレーザーを回転ミラー（図示せず）で反射させて、扇状に一定角度（たとえば、０．２５度）ずつスキャンする。また、この実施例では、検出範囲（図３参照）の全体をスキャンする周期は、２５ｍｓｅｃである。
【００４２】
図３は、距離センサ２２ａ、２２ｂの検出範囲（計測範囲）を説明するための図解図である。この実施例の距離センサ２２ａ、２２ｂの計測範囲は、半径Ｒ（Ｒ≒１０〜１５ｍ）の扇形状で示される。扇の角度は、正面方向を中心として左側および右側のそれぞれに１３５°である。つまり、正面を中心とする２７０°の範囲について距離を検出することができる。
【００４３】
図２に戻って、モータドライバ３４ａ、３４ｂは、コンピュータ３０からの指示に基づいて、モータ３６ａ、３６ｂを駆動する。エンコーダ３８ａは、モータ３６ａの回転数（ｒｐｓ）を検出し、回転数についてのデータ（回転数データ）を、インターフェイス３２を介してコンピュータ３０に入力する。同様に、エンコーダ３８ｂは、モータ３６ｂの回転数を検出し、回転数についての回転数データを、インターフェイスを介してコンピュータ３０に入力する。
【００４４】
コンピュータ３０は、距離センサ２２ａ、２２ｂで検出される距離データと、エンコーダ３８ａ、３８ｂで検出される回転数データを取得して、ＲＡＭ３０ｂ（ＨＤＤ３０ｃでもよい。）に記憶する。ただし、距離センサ２２ａで検出される距離データと距離センサ２２ｂで検出される距離データは区別されるとともに、同じ時点において検出された距離データと回転数データは互いに関連付けられる。
【００４５】
このような構成の地図作成システム１０では、SLAMの技術を用いて、上述したように、地図を作成するとともに、移動体１２の位置を推定する。ただし、SLAMの適用に先立って、距離データが回転数データに基づいて位置データに変換される。具体的には、移動体１２に搭載された距離センサ２２ａ、２２ｂのそれぞれで計測された距離データが、当該距離データを検出したときの移動体１２の位置および向きを基準とする２次元の座標系（移動体座標系）の位置（点）データに変換される。このとき、２つの距離センサ２２ａ、２２ｂの取り付け位置が考慮される。つまり、移動体１２の位置と、距離センサ２２ａ、２２ｂの取り付け位置のずれが調整される。当然のことではあるが、距離データを検出したときの距離センサ２２ａ、２２ｂの計測範囲における角度も考慮される。さらに、この実施例では、移動体座標系における位置データが、２次元の世界座標系の位置データに変換される。つまり、移動体座標系では、環境についての距離データの計測（移動体１２の移動）を開始したときの位置および向きを基準とするため、それを現実空間についての世界座標系に変換するようにしてある。
【００４６】
この実施例では、パーティクルフィルタを用いたSLAMが適用される。簡単に説明すると、SLAMでは、移動体１２の位置を推定することと地図を作成することを同時に行うため、先に観測した目印を用いて移動体１２の位置を推定し、その移動体１２の位置に従って新しい目印を配置して地図を成長（作成）する処理を繰り返し行う。
【００４７】
また、SLAMではベイズフィルタが用いられる。したがって、SLAMでは、上述したように、距離センサ２２ａ、２２ｂで検出された距離データを世界座標系に変換した位置データｚ1:tと移動体１２の動作ａ1:tに基づいて、移動体１２の軌跡ｒ1:tと地図ｍの同時確率密度の推定問題として定式化される。ただし、移動体１２の動作ａは、［ｖ,ω］Tである。
【００４８】
ここで、上述したように、この実施例では、計測された距離データを移動体１２の位置および向きを基準として移動体座標系の位置データに変換するようにしてある。ここで、移動体１２の位置および向きは、オドメトリによる計測値に基づいて求められる。具体的には、図４に示すように、移動体１２の位置および向きは、左右の車輪（１６ａ、１６ｂ）の移動速度ＶL、ＶRおよび車輪間距離Ｄを用いて算出される動作［ｖ,ω］Tを追跡することにより、求められる。
【００４９】
具体的には、移動体１２の動作［ｖ,ω］Tは数１に従って算出される。ただし、ｖは移動体１２の速度であり、ωは車輪１６ａおよび１６ｂの車軸の中心（図４参照）であり、車軸と垂直な軸周りの角度速である。
【００５０】
［数１］
2014174275.tif 000003
【００５１】
ただし、左右の車輪（１６ａ、１６ｂ）の移動速度ＶL、ＶRは、左右の車輪（１６ａ、１６ｂ）の回転数から算出することができ、また、車輪間隔Ｄは予め知ることができる。ただし、回転数に応じた移動速度ＶL、ＶRのテーブルを用意しておき、計算せずに、テーブルを参照することにより移動速度ＶL、ＶRを取得してもよい。
【００５２】
そして、算出された動作［ｖ,ω］Tを積分することにより、前回算出した移動体１２の位置および向きからの移動距離ｄと回転角度θを求めることができる。したがって、移動体１２の初期位置および初期の向き（角度θ＝０とする。）に、時系列に従って算出される移動距離ｄと回転角度θを累積的に加算することにより、時間毎の移動体１２の位置および向きを知ることができる。
【００５３】
なお、上記の移動体１２の軌跡ｒ1:tと地図ｍの同時確率密度の推定問題については、「S. Thrun, W. Burgard and D. Fox: Probabilistic Robotics. MIT Press, 2005. （邦訳） 上田隆一訳：確率ロボティクス， 毎日コミュニケーションズ，２００７．」（文献１）に開示されている。
【００５４】
また、状態ベクトルは、移動体１２の軌跡ｒ1:tと目印の集合ｍ1，…，ｍnからなる。このため、パーティクルフィルタをそのまま適用するのは困難である。そこで、Rao-Blackwellizationという操作により、パーティクルのサンプル空間を移動体１２の軌跡に限定することにより、パーティクルの次元を減らし、目印の位置の分布については、パーティクル毎に解析的に計算するという方法が「K. Murphy and S. Russell: ‘Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks,’ A. Doucet ed.: Sequential Monte Carlo Methods in Practice. Springer, 2001.」（文献２）において提案されている。これは、Rao-Blackwellised Particle Filter(RBPF) SLAMと呼ばれている。その実用的な実装として、FastSLAMがある（文献１）。
【００５５】
このFastSLAMでは、移動体１２の軌跡ｒ1:tと地図ｍの同時確率密度を数２のように分解し、移動体の軌跡の分布ｐ（ｒ1:t,ｍ|ｚ1:t，ａ1:t）をパーティクルフィルタで計算し、目印の位置の分布ｐ（ｍj｜ｒ1:t，ｚ1:t）をカルマンフィルタで計算する。
【００５６】
［数２］
2014174275.tif 000004
【００５７】
また、制御データ（ここでは、車輪１６ａ、１６ｂの回転数データ）を用いて、時刻ｔ−１のときに、時刻ｔにおける各パーティクルｉの状態（状態ベクトル）が予想される。次に予測した状態と計測値に基づく実際の状態の類似度（尤度）が算出され、この類似度に基づいて重要度ｗ(i)が計算される（リサンプリング）。たとえば、パーティクルｉ総数は固定であり、上記の類似度が低いパーティクルｉは消去され、類似度が高いパーティクルｉは分裂される。また、各パーティクルｉの重要度ｗ(i)は１で正規化される。
【００５８】
なお、この実施例では、パーティクルフィルタを用いたSLAMを適用するが、拡張カルマンフィルタを用いたSLAM(EKF-SLAM)を適用することもできる。このEKF-SLAMでは、移動体１２の位置ｒtと地図ｍの同時確率密度を正規分布で表し、ベイズ推定とマルコフ性仮定を使うことにより、数３に示すような漸化式を構成する。
【００５９】
［数３］
2014174275.tif 000005
【００６０】
ここで、ｕt＝（ｒt，ｍ1，…，ｍn）とすると、求める同時確率はｐ（ｕt｜ｚ1:t，ａ1:t）となり、拡張カルマンフィルタを実装することができる。ただし、動作モデルｐ（ｒt｜ｒt-1，ａt）は位置ｒtに対して計算されるが、計測モデルはｕtに対して計算される。
【００６１】
しかし、このような地図作成システム１０は、ショッピングモールやイベント会場など、人間、ショッピングカートや台車のような動的物体が存在する環境についての地図を作成するため、距離センサ２２ａ、２２ｂで検出された距離データを座標変換した位置データ（世界座標系の位置データ）のうち、人間などの動的物体についての位置データを消去する必要がある。
【００６２】
そこで、この実施例では、世界座標系の位置データから、壁、柱などの静的物体（環境）以外の動的物体などを判別して、環境についての地図作成のために不要な位置データを消去するようにしてある。以下、人間または人間と思われる動的物体についての位置データを消去する場合について説明する。
【００６３】
たとえば、図５には、壁の前に２人の人間が存在し、そのような状況において、移動体１２が壁や人間に対面し、移動体１２に設けられた距離センサ２２ａで距離を検出する様子が示される。ただし、図５では、壁、人間および移動体１２を上方から見た図である。また、ここでは、簡単のため、距離センサ２２ａの計測範囲は、正面を中心とする約９０°の範囲であり、その計測範囲を約３°ずつスキャンした場合について示してある。さらに、図５では、検出（計測）される距離は点線で示してある。また、上述したように、距離センサ２２ａは、床ないし地面から所定の高さに設置され、したがって、人間については、その脚（足首辺り）との距離を計測する。
【００６４】
図５に示すような場合について、不要な位置データを消去する方法について説明する。簡単に説明すると、位置データに対応する位置（点）を所定のルールに従ってクラスタリングし、クラスタリングされた点群（クラスタ）の特徴から人間を判別し、人間であることが判別されたクラスタ（位置（点）データの集合）を消去する。ただし、クラスタに含まれる点の数が所定数ＮＰ（この実施例では、５）よりも少ない場合には、動的物体であるか静的物体であるかを判別するのが困難であるため、このようなクラスタについても消去するようにしてある。このクラスタに含まれる点の数もクラスタの特徴の１つである。
【００６５】
クラスタリングでは、距離が所定値ＴＨ（この実施例では、１０ｃｍ）未満である位置（点）同士が同じクラスタに分類される。具体的には、着目する点との距離が最も近い（短い）点を検出し、その距離が所定値ＴＨ未満であるかどうかを判断する。そして、距離が所定値ＴＨ未満である場合には、検出した点を着目する点と同じクラスタに分類する。一方、距離が所定値ＴＨ以上である場合には、検出した点を着目する点とは異なる新しいクラスタに分類する。そして、新しいクラスタに分類した点を着目する点として、上記のようなクラスタリングを行う。このようにして、クラスタリングはすべての点について行われる。
【００６６】
図６は、図５において移動体１２の距離センサ２２ａによって距離を計測された点（位置）を白丸印で示す。ただし、図６では、距離を計測された点（位置）を分かり易く示すために、壁と人間を省略してある。このような点が上述した方法により、クラスタリングされる。ただし、上述したように、実際には、世界座標系に座標変換された位置データについてクラスタリングが行われる。
【００６７】
図７は、図６に示す各点（位置）について、上述した方法により、クラスタリングを行った場合に生成されたクラスタを示す。図７から分かるように、たとえば、７つのクラスタが生成される。図７（図８および図９も同じ。）では、同じクラスタに分類されていることを示すために点を囲んである。
【００６８】
クラスタリングを終えると、各クラスタについての特徴を抽出（算出）する。この実施例では、クラスタの特徴は、当該クラスタに含まれる複数の点の分散についての主成分であり、分散行列についての固有値λ１，λ２である。ただし、λ１は分散行列についての固有値の中で最も大きい固有値であり、λ２は固有値λ１の次に（２番目に）大きい固有値である。図８では、分かり易く示すために、クラスタの特徴（分散の主成分）を実線で示し、クラスタに含まれる点を省略してある。
【００６９】
上述したように、クラスタに含まれる点の数が所定数ＮＰ未満であるクラスタについては、動的物体か静的物体かを判別することが困難であるため、固有値を算出するまでも無く、消去される。したがって、図７および図８に示すように、点の数が４つであるクラスタ４は消去される。図８（図１０も同じ。）では、クラスタ４が消去されたことを、当該クラスタ４に含まれる点を囲む線を点線で記載することにより表現している。
【００７０】
固有値λ１，λ２を算出する場合には、まず、各クラスタでは、クラスタに含まれる複数の点についての重心（または平均）（μx，μy）が数４に従って算出される。ただし、クラスタにｎ個の点が含まれているものとする。
【００７１】
［数４］
2014174275.tif 000006
【００７２】
算出された重心（μx，μy）を基準とした場合についてのクラスタの分散行列Σは数５で示され、その固有値が数６に従って算出される。
【００７３】
［数５］
Σij = E[(Xi −μi)(Xj −μj)]=E(XiXj)−E(Xi)E(Xj)
ただし、μi=E(Xi)であり、この実施例では、２つの固有値λ１，λ２を算出するため、ｉ＝ｊ＝１，２である。
【００７４】
［数６］
Σｘ−λＩｘ＝(Σ−λＩ)ｘ＝０
det(Σ−λＩ)＝０
ただし、この実施例では、２つの固有値λ１，λ２を算出するため、Ｉは２行２列の単位行列である。
【００７５】
このようにして、固有値λ１，λ２が算出されると、この固有値λ１，λ２を用いて、動的物体であるか、静的物体であるかが判別される。具体的には、固有値λ１が第１閾値（たとえば、６５ｃｍ）よりも大きい場合、または、固有値λ２が第２閾値（たとえば、３ｃｍ）よりも小さい場合には、このような固有値λ１，λ２を有するクラスタを静的物体と判別する。ただし、第１閾値および第２閾値は、人間のような動的物体と壁や柱のような静的物体を判別するために実験等により経験的に得られた数値である。
【００７６】
このように、固有値λ２が第２閾値よりも小さい場合には、クラスタの形状が細長く、壁に対応するクラスタと考えられるからである。また、固有値λ１が第１閾値よりも大きい場合には、図９に示すように、通路の角に相当する部分の壁や柱に対応するクラスタと考えられるからである。したがって、固有値λ１が第１閾値以下である場合、または、固有値λ２が第２閾値以上である場合には、人間の脚（足首辺り）に相当する物体すなわち人間または人間のような物体に対応するクラスタであると考え、このような固有値λ１，λ２を有するクラスタを動的物体と判別し、当該クラスタを消去する。
【００７７】
ただし、この実施例では、固有値λ１，λ２の両方が第２閾値（たとえば、３ｃｍ）よりも小さい場合には、物体が小さ過ぎると判断し、当該固有値λ１，λ２を有するクラスタを消去する。
【００７８】
図１０は、図８に示したクラスタ１−３、５−７について、それぞれの特徴に基づいて静的物体であるか動的物体であるかを判別し、動的物体であることが判別されたクラスタ（図１０では、クラスタ２、３、５、６）を消去した図である。したがって、図５−図８および図１０に示すように、壁や柱のような静的物体についてのクラスタのみが残され、人間のような動的物体についてのクラスタおよび動的物体または静的物体を判別できないクラスタのような不要なクラスタ（位置データの集合）は消去される。
【００７９】
図１１は、上述したように、不要なクラスタを消去した後に、SLAMの技術を用いて作成した地図の一部を示す図解図である。ただし、図１１（図１２も同様である。）において、一本の実線で記載されているのが移動体１２の移動経路であり、それ以外の複数の小さな黒点で示される領域が壁や柱などの静的物体である。
【００８０】
また、図１２は、図１１に示す地図において、人間のような動的物体と判別されたクラスタについても描画した地図である。図１２において、複数の灰色の点で示される領域が人間などの動的物体である。図１１に示す地図では、このような動的物体が無いため、たとえば、コミュニケーションロボットのような移動体１２が道案内などのサービスを提供する場合に、移動体１２自身の位置を推定するために用いる地図として使用することができる。
【００８１】
図１３は、図２に示したコンピュータ３０に内蔵されるＲＡＭ３０ｂのメモリマップ３００の一例を示す。図１３に示すように、ＲＡＭ３０ｂは、プログラム記憶領域３０２およびデータ記憶領域３０４を含む。
【００８２】
プログラム記憶領域３０２には、メイン処理プログラム３０２ａ、データ記憶プログラム３０２ｂ、データ変換プログラム３０２ｃ、不要データ消去プログラム３０２ｄおよび地図作成プログラム３０２ｅなどが記憶される。
【００８３】
メイン処理プログラム３０２ａは、地図作成システム１０のメインルーチンを処理するためのプログラムである。データ記憶プログラム３０２ｂは、距離データおよび回転数データを取得して、ＲＡＭ３０ｂのデータ記憶領域３０４に記憶するためのプログラムである。ただし、移動体１２を移動させている場合には、距離データおよび回転数データをＨＤＤ３０ｃに記憶しておき、地図作成時に、ＨＤＤ３０ｃからＲＡＭ３０ｂに読み込んでもよい。
【００８４】
データ変換プログラム３０２ｃは、距離データを移動体１２の位置および向きを基準として移動体座標系の位置データに座標変換し、さらに、世界座標系の位置データに座標変換するためのプログラムである。不要データ消去プログラム３０２ｄは、地図作成に先立って、動的物体と判別されたクラスタ（位置データの集合）と、動的物体または静的物体を判別できないクラスタを消去するためのプログラムである。地図作成プログラム３０２ｅは、SLAMを適用することにより、地図を作成するためのプログラムである。
【００８５】
図示は省略するが、地図作成に必要な他のプログラムもプログラム記憶領域３０２に記憶される。
【００８６】
また、データ記憶領域３０４には、距離データバッファ３０４ａおよび回転数データバッファ３０４ｂが設けられる。距離データバッファ３０４ａは、データ記憶プログラム３０２ｂに従って距離データを記憶する。同様に、回転数データバッファ３０４ｂは、データ記憶プログラム３０２ｂに従って回転数データを記憶する。
【００８７】
また、データ記憶領域３０４には、位置データ３０４ｃ、クラスタデータ３０４ｄおよび地図データ３０４ｅが記憶される。位置データ３０４ｃは、データ変換プログラム３０２ｃに従って世界座標系に座標変換された位置（点）データである。クラスタデータ３０４ｄは、不要データ消去プログラム３０２ｄに従って不要なクラスタ（位置データの集合）を消去するに際してクラスタリングされた位置データの集合（クラスタ）毎のデータである。地図データ３０４ｅは、地図作成プログラム３０２ｅに従って作成された地図データである。
【００８８】
図示は省略するが、データ記憶領域３０４には、地図作成のために必要な他のデータが記憶されたり、地図作成のために必要なフラグやタイマ（カウンタ）などが設けられたりする。
【００８９】
図１４は図２に示したＣＰＵ３０ａの全体処理を示すフロー図である。図１４に示すように、ＣＰＵ３０ａは、全体処理を開始すると、ステップＳ１で、距離センサ２２ａ、２２ｂを駆動する。そして、ＣＰＵ３０ａは、距離センサ２２ａ、２２ｂからの距離データの取得および記憶を開始する。
【００９０】
次のステップＳ３では、移動体１２の移動制御を開始する。つまり、ＣＰＵ３０ａは、全体処理とは並行して、移動体１２の移動制御の処理を実行する。詳細な説明および図示は省略するが、移動制御の処理では、ＣＰＵ３０ａは、操作レバー１８から入力される操作入力に従ってモータドライバ３４ａ、３４ｂを制御し、モータ３６ａ、３６ｂを駆動する。
【００９１】
続くステップＳ５では、距離データおよび回転数データを取得するとともに記憶する。つまり、ＣＰＵ３０ａは、距離センサ２２ａ、２２ｂからの距離データを取得するとともに、エンコーダ３８ａ、３８ｂからの回転数データを取得し、互いに関連付けて、それぞれ時系列に従ってＲＡＭ３０ｂに記憶する。
【００９２】
続いて、ステップＳ７では、環境の全範囲について移動したかどうかを判断する。ここでは、ユーザによって移動が終了されたかどうかが判断される。具体的には、ＣＰＵ３０ａは、操作レバー１８からの操作入力が無い状態が一定時間（たとえば、３０秒）を経過したり、ユーザから移動終了の指示が入力されたりしたかどうかを判断する。
【００９３】
ステップＳ７で“ＮＯ”であれば、つまり環境の全範囲の移動を終了していない場合には、そのままステップＳ３に戻る。一方、ステップＳ７で“ＹＥＳ”であれば、つまり環境の全範囲を移動すれば、ステップＳ９で、移動体１２の位置および向きに応じて距離データを位置データに変換する。ここでは、上述したように、距離センサ２２ａ、２２ｂのそれぞれで検出された距離のデータが、移動体１２の位置および向きを基準として移動体座標系の位置データに座標変換される。このとき、上述したように、２つの距離センサ２２ａ、２２ｂの取り付け位置が考慮される。
【００９４】
続くステップＳ１１では、後述する不要データの消去処理（図１５および図１６参照）を実行し、ステップＳ１３で、消去処理により残ったクラスタ（位置データ）にSLAMを適用して地図を作成する。そして、ステップＳ１５で、作成した地図に対応する地図データ３０４ｅをＲＡＭ３０ｂに記憶して、全体処理を終了する。
【００９５】
なお、上述したように、距離センサ２２ａ、２２ｂの検出周期は、２０ｍｓｅｃであるため、ステップＳ５−Ｓ７のスキャンタイムは、２０ｍｓｅｃよりも短く設定される。
【００９６】
図１５および図１６は、図１４に示したステップＳ１１の不要データの消去処理を示すフロー図である。図１５に示すように、ＣＰＵ３０ａは、不要データの消去処理を開始すると、ステップＳ３１で、移動体座標系における位置データを世界座標系の位置データに変換する。
【００９７】
次のステップＳ３３では、着目する点（位置データに対応する位置）についての新しいクラスタを生成する。たとえば、ステップＳ３３では、まだクラスタに分類されていない位置データが任意に選択され、新しいクラスタが生成される。このとき、着目する点についての位置データを含む新しいクラスタのデータが追加される。つまり、新しいクラスタのデータを追加することにより、クラスタデータ３０４ｄが更新される。
【００９８】
続くステップＳ３５では、次の点への距離が所定値ＴＨ（この実施例では、１０ｃｍ）未満であるかどうかを判断する。ここでは、ＣＰＵ３０ａは、着目する点に最も近い点との距離が所定値ＴＨ未満であるかどうかを判断するのである。ステップＳ３５で“ＮＯ”であれば、つまり次の点への距離が所定値ＴＨ以上であれば、新しいクラスタを生成するために、ステップＳ３３に戻る。一方、ステップＳ３５で“ＹＥＳ”であれば、つまり次の点への距離が所定値ＴＨ未満であれば、ステップＳ３７で、当該次の点を既存のクラスタに追加する。つまり、当該次の点が着目する点と同じクラスタに分類される。
【００９９】
続いて、ステップＳ３９では、すべての点についてクラスタリングを行ったかどうかを判断する。ここでは、ＣＰＵ３０ａは、クラスタに分類されていない位置データが残っていないかどうかを判断するのである。ステップＳ３９で“ＮＯ”であれば、つまりクラスタリングを行っていない点が残っていれば、ステップＳ３５に戻る。図示は省略するが、このとき、直前にクラスタに分類された点が着目する点に設定される。
【０１００】
一方、ステップＳ３９で“ＹＥＳ”であれば、つまりすべての点についてクラスタリングを行えば、クラスタリングを終了したと判断し、ステップＳ４１で、１番目のクラスタを読み出し、図１６に示すステップＳ４３で、当該クラスタに含まれる点の数が所定数ＮＰ（この実施例では、５）未満かどうかどうかを判断する。
【０１０１】
なお、ステップＳ４１では、説明の便宜上、１番目のクラスタを読み出すように説明したが、ここでは、任意のクラスタが読み出される。後述するステップＳ５７においても同様であるが、このステップＳ５７では、静的物体であることが判別されていない任意のクラスタが読み出される。ただし、クラスタを生成した順番に従ってクラスタを読み出すようにしてもよい。
【０１０２】
ステップＳ４３で“ＹＥＳ”であれば、つまり当該クラスタに含まれる点の数が所定数ＮＰ未満であれば、静的物体か動的物体かの判別が出来ないと判断して、ステップＳ５３に進む。一方、ステップＳ４３で“ＮＯ”であれば、つまり当該クラスタに含まれる点の数が所定数ＮＰ以上であれば、ステップＳ４５で、数４−数６に従って、当該クラスタの固有値λ1、λ2を算出する。
【０１０３】
次のステップＳ４７では、固有値λ1およびλ2の両方が第２閾値Ｈmin（この実施例では、３ｃｍ）未満であるかどうかを判断する。上述したように、ここでは、静的物体または動的物体を判別することができるかどうかが判断されるのである。ステップＳ４７で“ＹＥＳ”であれば、つまり固有値λ1およびλ2の両方が第２閾値Ｈmin未満であれば、そのままステップＳ５３に進む。一方、ステップＳ４７で“ＮＯ”であれば、つまり固有値λ1およびλ2の少なくとも一方が第２閾値Ｈmin以上であれば、ステップＳ４９で、固有値λ1が第１閾値Ｈmaxよりも大きい、または、固有値λ2が第２閾値Ｈmin未満であるかどうかを判断する。上述したように、ここでは、人間のような動的物体であるか、壁や柱のような静的物体であるかが判断（判別）される。
【０１０４】
ステップＳ４９で“ＹＥＳ”であれば、つまり固有値λ1が第１閾値Ｈmaxよりも大きい、または、固有値λ2が第２閾値Ｈmin未満であれば、壁や柱のような静的物体であると判断し、ステップＳ５１で、当該クラスタを静的物体に対応するクラスタに決定して、ステップＳ５５に進む。
【０１０５】
一方、ステップＳ４９で“ＮＯ”であれば、つまり固有値λ1が第１閾値Ｈmax以下である、または、固有値λ2が第２閾値Ｈmin以上である場合には、人間のような動的物体であると判断して、ステップＳ５３で、当該クラスタを消去して、ステップＳ５５に進む。つまり、ステップＳ５３の処理を実行することにより、地図作成に不要な位置データが消去される。
【０１０６】
ステップＳ５５では、すべてのクラスタを判別したかどうかを判断する。ステップＳ５５で“ＮＯ”であれば、つまりまだ判別していないクラスタが存在する場合には、ステップＳ５７で、次のクラスタを読み出し、ステップＳ４３に戻る。一方、ステップＳ５５で“ＹＥＳ”であれば、つまりすべてのクラスタを判別すれば、図１４に示した全体処理にリターンする。
【０１０７】
この実施例によれば、距離センサの計測結果から静的物体および動的物体を判別し、静的物体についての計測結果を用いて地図を作成するので、動的物体の存在しない地図を簡単に作成することができる。
【０１０８】
また、この実施例では、距離センサで検出された距離データを位置データに変換して不要なデータを消去するので、カメラ等の他のセンサを設ける必要が無い。つまり、コストが高くなったり、画像処理などの負荷が大きくなったりすることもない。
【０１０９】
なお、この実施例では、移動体として電動車椅子を用いるようにしたが、これに限定される必要はない。たとえば、本件の出願人等が開発するとともに、製造販売するコミュニケーションロボット「Ｒｏｂｏｖｉｅ（ロボビー）（登録商標）」を用いることができる。
【０１１０】
また、この実施例では、距離センサで検出された距離データを移動体座標系の位置データに変換し、さらに、世界座標系の位置データに変換するようにしたが、環境についての距離データの計測（移動体の移動）を開始したときの現実空間における移動体の位置と向きを知ることができれば、移動体座標系の位置データを世界座標系の位置データに変換しなくてもよい。
【０１１１】
さらに、この実施例では、移動体座標系の位置データを世界座標系の位置データに変換してからクラスタリングするようにしたが、移動体座標系の位置データをクラスタリングした後に世界座標系に変換するようにしてもよい。
【０１１２】
さらにまた、この実施例では、動的物体の例として人間ないし人間のような物体を判別する場合について詳細に説明したが、上述したように、ショッピングカートや台車などの他の動的物体を判別することもできる。ただし、ショッピングカートや台車などの他の動的物体を判別する場合には、クラスタに含まれる位置データ（点）の数やその分散についての主成分（分散行列の固有値）が満たす条件をそれぞれの場合について決定する必要がある。
【０１１３】
また、この実施例で示した具体的な数値は単なる一例であり、限定されるべきではなく、実施される製品等に応じて適宜変更可能である。また、この実施例では、所定値や閾値を用いて大小を判断するステップにおいて、当該所定値や当該閾値を含まないようにしたが、当該所定値や当該閾値を含んで大小を判断するようにしてもよい。
【符号の説明】
【０１１４】
１０  …地図作成システム
１２  …移動体
１６ａ、１６ｂ  …車輪
１８  …操作レバー
２２ａ、２２ｂ  …距離センサ
３０  …コンピュータ
３４ａ、３４ｂ  …モータドライバ
３６ａ、３６ｂ  …モータ
３８ａ、３８ｂ  …エンコーダ
【図１】
2014174275.tif 000007
【図２】
2014174275.tif 000008
【図３】
2014174275.tif 000009
【図４】
2014174275.tif 000010
【図５】
2014174275.tif 000011
【図６】
2014174275.tif 000012
【図７】
2014174275.tif 000013
【図８】
2014174275.tif 000014
【図９】
2014174275.tif 000015
【図１０】
2014174275.tif 000016
【図１１】
2014174275.tif 000017
【図１２】
2014174275.tif 000018
【図１３】
2014174275.tif 000019
【図１４】
2014174275.tif 000020
【図１５】
2014174275.tif 000021
【図１６】
2014174275.tif 000022