(57)【特許請求の範囲】
【請求項１】
移動体の周辺にある地物における複数の計測点までの距離を取得する工程と、
前記移動体の周辺を撮像して前記複数の計測点の色情報を取得する工程と、
前記複数の計測点と予め登録される地物における位置情報及び色情報を有する複数の登録点とを対応付ける工程と、
計測点の色情報と該計測点に対応する登録点の色情報とが一致又は類似する場合に、前記移動体の位置の推定に用いる有効計測点として該計測点を抽出する工程と、
有効計測点を用いて移動体の位置を推定する工程と、
を有する移動体位置推定方法。
【請求項２】
前記計測点は、最も近い位置にある登録点に対応付けられる、
請求項１に記載の移動体位置推定方法。
【請求項３】
前記計測点は、該計測点と登録点との間の距離が閾値未満の場合に、該登録点に対応付けられる、
請求項１又は２に記載の移動体位置推定方法。
【請求項４】
前記移動体の位置は、前記移動体と前記有効計測点との間の距離及び方向と、前記有効計測点と前記有効計測点に対応する登録点との間の距離及び方向と、前記登録点の位置情報とに基づいて推定される、
請求項１乃至３の何れか一項に記載の移動体位置推定方法。
【請求項５】
移動体の周辺にある地物における複数の計測点までの距離を取得する距離検出装置と、
前記移動体の周辺を撮像して前記複数の計測点の色情報を検出する色情報検出装置と、
前記移動体の位置を推定する制御装置と、を備え、
前記制御装置は、前記複数の計測点と予め登録される地物における位置情報及び色情報を有する複数の登録点とを対応付け、計測点の色情報と該計測点に対応する登録点の色情報とが一致又は類似する場合に、前記移動体の位置の推定に用いる有効計測点として該計測点を抽出し、該有効計測点を用いて移動体の位置を推定する、
移動体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体に搭載されるセンサを用いて取得する地物に関する情報と予め登録された地物に関する情報とに基づいて移動体の位置を推定する移動体位置推定方法及びその移動体位置推定方法を実行する移動体に関する。
【背景技術】
【０００２】
従来、デッドレコニングにより、予め登録された地図における自己位置の推定を行う車輪型移動ロボットが知られている（例えば、非特許文献１参照。）。このロボットは、また、ＩＣＰ（Iterative  Closest  Point）アルゴリズムにより、移動体に搭載されるレーザレンジファインダの計測点と地図上の登録点とを対応付けて自己位置を補正する。
【０００３】
また、車載レーザレンジファインダの出力と車載カメラの出力とに基づいて検出される車両の前方上部にある地物の形状及び色に基づいて車両の位置を特定する車両位置測定装置が知られている（例えば、特許文献１参照。）。この装置は、検出した地物の形状及び色と予め登録される基準地物の形状及び色とが一致する場合に、その基準地物に関連付けて記憶された位置情報と車載レーザレンジファインダの測定結果とに基づいて車両の位置を算出する。すなわち、レーザレンジファインダ及びカメラを用いて検出される地物が基準地物である場合に車両の位置を算出することができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１０−１９０６４７号公報
【非特許文献】
【０００５】
【非特許文献１】中本琢実、外２名、「レーザレンジファインダ搭載移動ロボットによる動的環境の３次元地図作成」、電子情報通信学会技術研究報告、Vol.106,  No.144,  pp.25-30.  2006.
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、非特許文献１のロボットは、地図に登録されていない地物における点をレーザレンジファインダが計測した場合、その計測点を地図上の登録点に誤って対応付け、自己位置の推定精度を低下させるおそれがある。
【０００７】
また、特許文献１の車両は、予め登録された地物の形状及び色と、レーザレンジファインダ及びカメラが検出する地物の形状及び色とを照合することによって車両の位置を特定するため、車両の位置の特定に時間がかかる。さらに、特許文献１の車両は、レーザレンジファインダ及びカメラを用いて検出される地物が予め登録された地物でない場合には車両の位置を算出できない。
【０００８】
上述の点に鑑み、本発明は、移動体に搭載されるセンサを用いて取得する地物に関する情報と予め登録された地物に関する情報とに基づいて、未登録の地物が存在する場合にもより高精度に且つより迅速に移動体の位置を推定できる移動体位置推定方法及びその移動体位置推定方法を実行する移動体を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上述の目的を達成するために、本発明の実施例に係る移動体位置推定方法は、移動体の周辺にある地物における複数の計測点までの距離を取得する工程と、前記移動体の周辺を撮像して前記複数の計測点の色情報を取得する工程と、前記複数の計測点と予め登録される地物における位置情報及び色情報を有する複数の登録点とを対応付ける工程と、計測点の色情報と該計測点に対応する登録点の色情報とが一致又は類似する場合に、前記移動体の位置の推定に用いる有効計測点として該計測点を抽出する工程と、有効計測点を用いて移動体の位置を推定する工程と、を有する。
【００１０】
また、本発明の実施例に係る移動体は、移動体の周辺にある地物における複数の計測点までの距離を取得する距離検出装置と、前記移動体の周辺を撮像して前記複数の計測点の色情報を検出する色情報検出装置と、前記移動体の位置を推定する制御装置と、を備え、前記制御装置は、前記複数の計測点と予め登録される地物における位置情報及び色情報を有する複数の登録点とを対応付け、計測点の色情報と該計測点に対応する登録点の色情報とが一致又は類似する場合に、前記移動体の位置の推定に用いる有効計測点として該計測点を抽出し、該有効計測点を用いて移動体の位置を推定する。
【発明の効果】
【００１１】
上述の手段により、本発明は、移動体に搭載されるセンサを用いて取得する地物に関する情報と予め登録された地物に関する情報とに基づいて、未登録の地物が存在する場合にもより高精度に且つより迅速に移動体の位置を推定できる移動体位置推定方法及びその移動体位置推定方法を実行する移動体を提供することができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施例に係る移動体の構成例を示す概略図である。
【図２】位置推定処理の流れを示すフローチャートである。
【図３】既登録の地物における計測点に基づく位置推定処理の様子を示す図である。
【図４】未登録の地物における計測点が存在する場合の位置推定処理の様子を示す図である。
【発明を実施するための形態】
【００１３】
以下、図面を参照しつつ、本発明を実施するための最良の形態の説明を行う。
【００１４】
図１は、本発明の実施例に係る移動体１００の構成例を示す概略図である。本実施例では、移動体１００は、主に、制御装置１、暫定位置検出装置２、距離検出装置３、色情報検出装置４、記憶装置５、及び駆動装置６を含む。
【００１５】
移動体１００は、周囲の地物の位置に基づいて自己位置を推定可能な装置であり、例えば、掃除ロボット、ＡＧＶ(Automated  Guided  Vehicle)等である。
【００１６】
具体的には、移動体１００は、暫定位置検出装置２が検出する自身の暫定的な位置と、距離検出装置３が検出する周囲の地物の移動体１００からの距離及び方向とに基づいて周囲の地物の位置を算出する。そして、移動体１００は、色情報検出装置４の出力に基づいてその地物の色情報を取得する。その後、移動体１００は、記憶装置５に予め登録される地物の位置及び色情報に基づいて、検出した地物と登録済みの地物とを対応付ける。そして、移動体１００は、対応付けられた登録済みの地物の位置と、検出した地物とその登録済みの地物との間の距離及び方向と、距離検出装置３が検出した距離及び方向から移動体１００のより正確な位置を推定する。
【００１７】
次に、移動体１００の構成要素の詳細について説明する。
【００１８】
制御装置１は、移動体１００の動作を制御するための装置であり、例えば、ＣＰＵ（Central  Processing  Unit）、ＲＡＭ(Random  Access  Memory)、ＲＯＭ（Read  Only  Memory）等を備えたコンピュータである。
【００１９】
本実施例では、制御装置１は、走行制御部１０、データ対応付け部１１、有効計測点抽出部１２、及び位置推定部１３の各機能要素に対応するプログラムを有する。制御装置１は、各機能要素に対応するプログラムをＲＯＭから読み出してＲＡＭに展開しＣＰＵに実行させる。
【００２０】
暫定位置検出装置２は、移動体１００の暫定的な位置を検出するための装置であり、例えば、ＧＰＳ(Global  Positioning  System)信号に基づいて移動体１００の位置（緯度、経度、高度）を測定するＧＰＳ受信機である。また、暫定位置検出装置２は、移動体１００が車輪を用いて移動する場合には、オドメトリ法により移動体１００の暫定的な位置を検出してもよい。具体的には、暫定位置検出装置２は、車輪に取り付けられるエンコーダの出力に基づいて既知の所定位置に対する移動距離及び移動方向を認識することによって移動体１００の暫定的な位置を検出してもよい。また、暫定位置検出装置２は、ランドマーク法により、壁や床に描かれたマークをカメラで認識することによって移動体１００の暫定的な位置を検出してもよい。
【００２１】
距離検出装置３は、移動体１００の周囲にある地物までの距離を検出するための装置である。本実施例では、距離検出装置３は、移動体１００の前部に取り付けられるレーザレンジファインダであり、例えば、１８０度の走査角度と０．５度の角度分解能とを有し、水平面を走査する。距離検出装置３は、照射したレーザが周囲の地物における計測点で反射して帰ってくるまでの時間に基づいて移動体１００とその計測点との間の距離を検出する。また、距離検出装置３は、レーザを照射した方向に基づいて、移動体１００から見た計測点の方向を認識する。
【００２２】
色情報検出装置４は、移動体１００の周囲にある地物の色情報を検出するための装置である。本実施例では、色情報検出装置４は、移動体１００の前部に取り付けられるカメラである。
【００２３】
本実施例では、距離検出装置３及び色情報検出装置４は、所定の固定的な位置関係で移動体１００に取り付けられている。そのため、距離検出装置３の各計測点は、色情報検出装置４が撮像する画像における１又は複数の画素に予め対応付けられている。
【００２４】
なお、１つの計測点に複数の画素が対応付けられている場合、その計測点の色情報には、例えば、それら複数の画素のそれぞれの色情報に基づく統計値（例えば、平均値、最大値、最小値、中央値、最頻値等である。）が採用される。
【００２５】
記憶装置５は、各種情報を記憶するための装置であり、例えば、ハードディスク、半導体メモリ、光学記憶媒体等の不揮発性記憶媒体である。
【００２６】
本実施例では、記憶装置５は、移動体１００が走行可能な領域内又はその領域の周囲にある地物に関する情報を予め登録した地物データベースを記憶する。地物は、壁、柱、机等を含み、地物データベースは、地物の表面における予め設定された登録点の位置情報及び色情報を含むレコードを一単位とするデータベースである。位置情報は、例えば、所定の座標系における座標値であり、色情報は、例えば、ＲＧＢ空間における座標値である。
【００２７】
駆動装置６は、移動体１００を動かすための装置であり、例えば、車輪、無限軌道等を駆動して移動体１００を移動させる。また、駆動装置６による移動体１００の移動は、ホロノミックであってもよく、非ホロノミックであってもよい。
【００２８】
次に、制御装置１が有する各種機能要素について説明する。
【００２９】
走行制御部１０は、移動体１００の自律走行を制御するための機能要素である。本実施例では、走行制御部１０は、位置推定部１３が推定する移動体１００の現在位置に応じた制御信号を駆動装置６に対して出力し、移動体１００が地物と接触することなく走行できるように駆動装置６を制御する。
【００３０】
なお、移動体１００は、オペレータの指示に応じて走行する構成であってもよい。例えば、移動体１００は、オペレータが事前に設定した経路に沿って走行する構成であってもよく、オペレータがある時点において指示した一方向に沿って走行し続ける構成であってもよい。
【００３１】
データ対応付け部１１は、距離検出装置３の計測点と、記憶装置５に予め登録される登録点とを対応付けるための機能要素である。
【００３２】
本実施例では、データ対応付け部１１は、暫定位置検出装置２が検出する移動体１００の暫定位置を所定の座標系における座標値に変換する。そして、データ対応付け部１１は、移動体１００の暫定位置の座標値と、距離検出装置３の各計測点までの距離及び方向とに基づいて、所定の座標系における各計測点の座標値を算出する。所定の座標系は、例えば、移動体１００が走行する平面上に拡がる二次元直交座標系であり、その平面上の任意の１点を原点とする。
【００３３】
その上で、データ対応付け部１１は、計測点に最も近い位置にある登録点をその計測点に関する対応登録点として対応付ける。
【００３４】
また、データ対応付け部１１は、計測点とその計測点に最も近い位置にある登録点との間の距離が閾値未満の場合に対応付けを行うようにしてもよい。すなわち、計測点とその計測点に最も近い位置にある登録点との間の距離が閾値以上の場合、データ対応付け部１１は、対応付けを行わずにその計測点を棄却してもよい。なお、対応付けが行われずに棄却される計測点に関する情報は、後の処理で用いられることはない。
【００３５】
有効計測点抽出部１２は、距離検出装置３の複数の計測点から有効計測点を抽出するための機能要素である。有効計測点は、位置推定部１３が移動体１００の位置を推定するために用いる計測点である。すなわち、有効計測点として抽出されずに棄却される計測点に関する情報は、移動体１００の位置を推定する際に、位置推定部１３によって用いられることはない。
【００３６】
本実施例では、有効計測点抽出部１２は、計測点の色情報と対応登録点の色情報との類似度を算出し、算出した類似度が閾値以上の場合に、すなわち、色が一致或いは類似している場合に、その計測点を有効計測点として抽出する。一方で、有効計測点抽出部１２は、算出した類似度が閾値未満の場合、すなわち、色が類似しない場合には、移動体１００の位置を推定するためにその計測点が用いられることがないよう、その計測点を棄却する。なお、計測点の色情報には、色情報検出装置４が撮像する画像における、その計測点に対応する画素の色情報が用いられる。また、類似度は、例えば、ＲＧＢ空間等の色空間におけるユークリッド距離の逆数として算出される。
【００３７】
位置推定部１３は、移動体１００の位置を推定するための機能要素である。本実施例では、位置推定部１３は、有効計測点抽出部１２が抽出した有効計測点と、その有効計測点に対応する対応登録点との間の距離である補正距離を算出する。また、位置推定部１３は、有効計測点から見た対応登録点の方向と所定方向との間の角度である補正角度を算出する。所定方向は、例えば、現時点における移動体１００の向き、すなわち、距離検出装置３の正面方向である。
【００３８】
そして、位置推定部１３は、各有効計測点に関する補正距離及び補正角度を算出した後、全体としての補正距離（以下、「集合的補正距離」とする。）及び全体としての補正角度（以下、「集合的補正角度」とする。）を算出する。集合的補正距離は、例えば、各有効計測点に関して算出した補正距離に基づく統計値（例えば、平均値、最大値、最小値、中央値、最頻値等である。）である。集合的補正角度についても同様である。
【００３９】
そして、位置推定部１３は、暫定位置検出装置２が検出した移動体１００の暫定位置を集合的補正角度の方向に集合的補正距離だけ補正することによって、移動体１００の位置を推定する。すなわち、位置推定部１３は、移動体１００の暫定位置に対応する所定の座標系における座標値に基づいて、移動体１００の補正後の位置に対応する所定の座標系における座標値を導き出す。
【００４０】
次に、図２を参照しながら、制御装置１が移動体１００の位置を推定する処理（以下、「位置推定処理」とする。）について説明する。なお、図２は、位置推定処理の流れを示すフローチャートであり、制御装置１は、所定周期で繰り返しこの位置推定処理を実行する。
【００４１】
最初に、制御装置１は、暫定位置検出装置２の出力に基づいて移動体１００の暫定位置を取得する（ステップＳ１）。
【００４２】
また、制御装置１は、距離検出装置３の出力に基づいて移動体１００の周囲の地物における計測点までの距離及び方向を取得する（ステップＳ２）。
【００４３】
また、制御装置１は、色情報検出装置４が撮像した、距離検出装置３の計測点を含む画像に基づいて各計測点の色情報を取得する（ステップＳ３）。
【００４４】
なお、ステップＳ１〜Ｓ３の処理は順不同であり、３つの処理が同時に行われてもよい。
【００４５】
その後、制御装置１は、データ対応付け部１１により、各計測点の座標値を算出する（ステップＳ４）。具体的には、データ対応付け部１１は、暫定位置検出装置２が検出する移動体１００の暫定位置を所定の座標系における座標値に変換する。そして、データ対応付け部１１は、移動体１００の暫定位置の座標値と、距離検出装置３の各計測点までの移動体１００からの距離及び方向とに基づいて、所定の座標系における各計測点の座標値を算出する。
【００４６】
その後、制御装置１は、データ対応付け部１１により、計測点に最も近い登録点をその計測点に関する対応登録点として対応付ける（ステップＳ５）。
【００４７】
その後、制御装置１は、有効計測点抽出部１２により、計測点の色情報と対応登録点の色情報とに基づいて有効計測点を抽出する（ステップＳ６）。
【００４８】
具体的には、有効計測点抽出部１２は、計測点の色情報とその計測点に対応する対応登録点の色情報との間の類似度を算出する。そして、有効計測点抽出部１２は、類似度が閾値以上の場合に、その計測点を有効計測点として抽出し、類似度が閾値未満の場合に、その計測点を棄却する。
【００４９】
その後、制御装置１は、位置推定部１３により、移動体１００の位置を推定する（ステップＳ７）。
【００５０】
具体的には、位置推定部１３は、有効計測点抽出部１２が抽出した有効計測点と、その有効計測点に対応する対応登録点との間の距離である補正距離を算出する。また、位置推定部１３は、有効計測点から見た対応登録点の方向と所定方向との間の角度である補正角度を算出する。そして、位置推定部１３は、各有効計測点に関する補正距離及び補正角度に基づいて、集合的補正距離及び集合的補正角度を算出する。そして、位置推定部１３は、暫定位置検出装置２が検出した移動体１００の暫定位置を集合的補正角度の方向に集合的補正距離だけ補正することによって、移動体１００の位置を推定する。
【００５１】
その後、制御装置１は、位置推定部１３が推定した移動体１００の位置を走行制御部１０に対して出力する（ステップＳ８）。走行制御部１０は、位置推定部１３が推定する移動体１００の位置に応じて駆動装置６に対して制御信号を出力し、移動体１００が地物と接触することなく走行できるように駆動装置６を制御する。
【００５２】
次に、図３を参照しながら、位置推定処理の具体例について説明する。なお、図３は、既登録の地物における計測点に基づく位置推定処理の様子を示す図であり、図３（Ａ）は、移動体１００の暫定位置ＰＰと既登録の地物である壁との位置関係を示す。また、図３（Ｂ）は、移動体１００の暫定位置ＰＰと、位置推定部１３によって推定された移動体１００の推定位置ＣＰとの位置関係を示す。
【００５３】
なお、図３（Ａ）の白丸は、記憶装置５に予め設定される登録点の位置及び色（白色）を示し、図３（Ａ）の白三角形は、距離検出装置３の計測点の位置及び色（白色）を示す。すなわち、図３では、各計測点の色と各計測点に対応する対応登録点の色が同じであり、計測点の全てが有効計測点として抽出される。
【００５４】
また、図３（Ａ）において白三角形から白丸に伸びる矢印は、各有効計測点に関する補正距離及び補正角度を有する補正ベクトルを表す。また、図３（Ｂ）において移動体１００の暫定位置ＰＰから推定位置ＣＰに伸びる矢印は、集合的補正距離及び集合的補正角度を有する集合的補正ベクトルを表す。
【００５５】
図３に示すように、既登録の地物のみが移動体１００の周囲に存在する場合、制御装置１は、計測点と登録点との誤った対応付けの影響を過度に受けることなく、移動体１００の位置を正確に推定できる。
【００５６】
次に、図４を参照しながら、位置推定処理の別の具体例について説明する。なお、図４は、未登録の地物における計測点が存在する場合の位置推定処理の様子を示す図である。
【００５７】
なお、図４の白丸は、記憶装置５に予め設定される登録点の位置及び色（白色）を示す。また、図４の白三角形は、既登録の地物ＲＦに関する距離検出装置３の計測点の位置及び色（白色）を示し、図４の黒三角形は、未登録の地物ＵＦ１、ＵＦ２に関する距離検出装置３の計測点の位置及び色（黒色）を示す。すなわち、図４では、既登録の地物ＲＦに関する計測点の色とそれらの計測点に対応する対応登録点の色とが同じであり、地物ＲＦに関する計測点の全てが有効計測点として抽出される。一方で、未登録の地物ＵＦ１、ＵＦ２に関する計測点の色とそれらの計測点に対応する対応登録点の色とが異なり、地物ＵＦ１、ＵＦ２に関する計測点の全てが棄却される。
【００５８】
また、図４において白三角形から白丸に伸びる実線の矢印ＡＲ１は、各有効計測点、すなわち、地物ＲＦにおける各計測点に関する補正距離及び補正角度を有する補正ベクトルを表す。また、図４において黒三角形から白丸に伸びる点線の矢印ＡＲ２は、棄却された計測点が仮に有効計測点として抽出されていた場合の各有効計測点、すなわち、地物ＵＦ１、ＵＦ２における各計測点に関する補正距離及び補正角度を有する補正ベクトルを表す。
【００５９】
また、図４は、移動体１００の暫定位置ＰＰ、推定位置ＣＰ１、及び推定位置ＣＰ２の位置関係を示す。推定位置ＣＰ１は、有効計測点、すなわち、地物ＲＦに関する計測点のみに基づいて推定される移動体１００の位置を示す。推定位置ＣＰ２は、棄却された計測点が仮に有効計測点として抽出されていた場合の全有効計測点、すなわち、地物ＵＦ１、ＵＦ２に関する有効計測点を含む全有効計測点に基づいて推定される移動体１００の位置を示す。
【００６０】
また、図４において暫定位置ＰＰから推定位置ＣＰ１に伸びる実線の矢印ＡＲ１ｃは、有効計測点、すなわち、地物ＲＦに関する計測点に基づいて算出される集合的補正距離及び集合的補正角度を有する集合的補正ベクトルを表す。また、図４において暫定位置ＰＰから推定位置ＣＰ２に伸びる点線の矢印ＡＲ２ｃは、棄却された計測点が仮に有効計測点として抽出されていた場合の全有効計測点、すなわち、地物ＵＦ１、ＵＦ２に関する有効計測点を含む全有効計測点に基づいて算出される集合的補正距離及び集合的補正角度を有する集合的補正ベクトルを表す。
【００６１】
図４に示すように、制御装置１は、既登録の地物ＲＦと未登録の地物ＵＦ１、ＵＦ２との色の違いに基づいて未登録の地物ＵＦ１、ＵＦ２に関する計測点を棄却する。そのため、制御装置１は、未登録の地物ＵＦ１、ＵＦ２が移動体１００の位置の推定に与える影響を排除することができる。その結果、制御装置１は、移動体１００の暫定位置ＰＰが推定位置ＣＰ２に補正されてしまうのを防止することができる。
【００６２】
以上の構成により、移動体１００は、未登録の地物ＵＦ１、ＵＦ２が移動体１００の周囲に存在する場合であっても、未登録の地物ＵＦ１、ＵＦ２に関する計測点を棄却するため、移動体１００の位置をより正確に推定できる。
【００６３】
また、移動体１００は、未登録の地物ＵＦ１、ＵＦ２に関する計測点を棄却するため、有効計測点の数を少なくすることができ、より迅速に移動体１００の位置を推定できる。
【００６４】
以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなしに上述した実施例に種々の変形及び置換を加えることができる。
【００６５】
例えば、上述の実施例において、有効計測点抽出部１２は、各計測点に関する色情報に基づいて有効計測点を抽出するが、色情報の代わりに明るさ（輝度）情報に基づいて有効計測点を抽出するようにしてもよい。
【符号の説明】
【００６６】
１・・・制御装置  ２・・・暫定位置検出装置  ３・・・距離検出装置  ４・・・色情報検出装置  ５・・・記憶装置  ６・・・駆動装置  １０・・・走行制御部  １１・・・データ対応付け部  １２・・・有効計測点抽出部  １３・・・位置推定部
【図１】
0005819257.tif 000002
【図２】
0005819257.tif 000003
【図３】
0005819257.tif 000004
【図４】
0005819257.tif 000005