JP7275973B2 - 位置推定装置 - Google Patents

Description

本発明は、位置推定装置に関する。

従来の位置推定装置としては、例えば特許文献１に記載されているような技術が知られている。特許文献１に記載の位置推定装置は、レーザを発射し、その反射光を検知して物体までの距離を測定するレーザ距離センサと、無人搬送車が走行する走行エリアの番地と走行エリアに設定されている座標との対応情報を格納するデータメモリと、レーザ距離センサからの計測データと地図データとをマッチングさせて無人搬送車の現在位置を推定する処理部とを備えている。

特開２０１１－２５３４１４号公報

しかしながら、上記従来技術のように、レーザを使用して移動体の自己位置を推定する場合には、レーザを反射させる物体の数が少なくなると、移動体の自己位置の推定精度が低下してしまう。

本発明の目的は、移動体の自己位置を高精度に推定することができる位置推定装置を提供することである。

本発明の一態様は、移動体の自己位置を推定する位置推定装置において、移動体の周囲にレーザを照射し、レーザの反射光を受光することにより、移動体の周囲の物体までの距離を検出する距離検出部と、距離検出部の検出データに基づいて、移動体の自己位置の推定演算を行う推定演算部と、推定演算部による移動体の自己位置の推定精度を検知する推定精度検知部とを備え、推定演算部は、推定精度検知部により検知された移動体の自己位置の推定精度に応じて距離検出部の検出データの使用距離範囲を変更して、移動体の自己位置の推定演算を行う。

このような位置推定装置においては、距離検出部によって、移動体の周囲にレーザを照射することで、移動体の周囲の物体までの距離が検出される。そして、推定演算部によって、距離検出部の検出データに基づいて移動体の自己位置の推定演算が行われる。ここで、推定精度検知部によって、推定演算部による移動体の自己位置の推定精度が検知される。そして、推定演算部では、移動体の自己位置の推定精度に応じて距離検出部の検出データの使用距離範囲を変更して、移動体の自己位置の推定演算が行われる。このとき、移動体の自己位置の推定精度が低いほど、距離検出部の検出データの使用距離範囲を広くする。距離検出部の検出データの使用距離範囲が広くなると、検出データの使用距離範囲内においてレーザを反射させる物体の数が増えやすくなる。これにより、移動体の自己位置を高精度に推定することができる。

推定演算部は、移動体の自己位置の推定精度が低いときは、移動体の自己位置の推定精度が高いときに比べて距離検出部の検出データの使用距離範囲を広くして、移動体の自己位置の推定演算を行ってもよい。このような構成では、移動体の自己位置の推定精度が低いときは、距離検出部の検出データの使用距離範囲が広くなるため、検出データの使用距離範囲内においてレーザを反射させる物体の数が確実に増えやすくなる。

推定精度検知部は、推定演算部による移動体の自己位置の推定精度が低下しているかどうかを検知し、推定演算部は、移動体の自己位置の推定精度が低下していないときは、距離検出部の検出データの使用距離範囲として第１距離範囲を使用して、移動体の自己位置の推定演算を行い、移動体の自己位置の推定精度が低下しているときは、距離検出部の検出データの使用距離範囲として第１距離範囲よりも広い第２距離範囲を使用して、移動体の自己位置の推定演算を行ってもよい。このような構成では、距離検出部の検出データの使用距離範囲としては、第１距離範囲及び第２距離範囲という２つのみが使用される。従って、推定演算部により実行される演算処理を簡素化することができる。

位置推定装置は、移動体が移動する路面に設置されたマークを検出するマーク検出部を更に備え、推定演算部は、第１距離範囲を使用して移動体の自己位置の推定演算を行うことで、移動体の自己位置を第１位置推定値として推定し、推定精度検知部は、マーク検出部により検出されたマークに基づいて、移動体の自己位置を第２位置推定値として推定し、第１位置推定値と第２位置推定値とのずれ量が閾値以上であるときに、移動体の自己位置の推定精度が低下していると検知してもよい。このような構成では、距離検出部の検出データに基づいて得られた第１位置推定値と、路面に設置されたマークに基づいて得られた第２位置推定値とを比較することで、移動体の自己位置の推定精度が低下しているかどうかが検知される。従って、移動体の自己位置の推定精度が低下しているかどうかを正確に検知することができる。

位置推定装置は、移動体の状態を検出する状態検出部を更に備え、推定演算部は、第１距離範囲を使用して移動体の自己位置の推定演算を行うことで、移動体の自己位置を第１位置推定値として推定し、推定精度検知部は、状態検出部により検出された移動体の状態に基づいて、移動体の自己位置を第２位置推定値として推定し、第１位置推定値と第２位置推定値とのずれ量が閾値以上であるときに、移動体の自己位置の推定精度が低下していると検知してもよい。このような構成では、距離検出部の検出データに基づいて得られた第１位置推定値と、移動体の状態に基づいて得られた第２位置推定値とを比較することで、移動体の自己位置の推定精度が低下しているかどうかが検知される。従って、移動体の自己位置の推定精度が低下しているかどうかを正確に検知することができる。

推定精度検知部は、第１位置推定値と第２位置推定値とのずれ量が規定時間または規定距離だけ連続して閾値以上であるときに、移動体の自己位置の推定精度が低下していると検知してもよい。このような構成では、第１位置推定値と第２位置推定値とのずれ量が瞬間的に閾値以上になる状態が排除されるため、移動体の自己位置の推定精度が低下しているかどうかをより正確に検知することができる。

推定精度検知部は、距離検出部の検出データに基づいて、移動体の複数の自己位置候補点の分散値を算出し、分散値が閾値以上であるときに、移動体の自己位置の推定精度が低下していると検知してもよい。このような構成では、距離検出部の検出データに基づいた計算処理によって、移動体の自己位置の推定精度が低下しているかどうかが検知される。従って、簡単な構成で、移動体の自己位置の推定精度が低下しているかどうかを検知することができる。

推定精度検知部は、分散値が規定時間または規定距離だけ連続して閾値以上であるときに、移動体の自己位置の推定精度が低下していると検知してもよい。このような構成では、分散値が瞬間的に閾値以上になる状態が排除されるため、移動体の自己位置の推定精度が低下しているかどうかを正確に検知することができる。

本発明によれば、移動体の自己位置を高精度に推定することができる。

本発明の第１実施形態に係る位置推定装置を備えた走行制御装置を示す概略構成図である。 図１に示されたレーザセンサによるレーザの照射範囲を路面に設置されたＡＲマーカと共に示す概念図である。 図１に示された推定演算部により実行される演算処理手順の詳細を示すフローチャートである。 図１に示された推定演算部により移動体の自己位置の推定演算を行う際に作成される移動体の複数の自己位置候補点を示す概念図である。 図１に示された推定精度検知部により実行される検知処理手順の詳細を示すフローチャートである。 図１に示された推定演算部によりレーザセンサの検出データの使用距離範囲として通常距離範囲または長距離範囲を使用して、移動体の自己位置の推定演算を行う様子を示す概念図である。 本発明の第２実施形態に係る位置推定装置を備えた走行制御装置を示す概略構成図である。 図７に示された推定精度検知部により実行される検知処理手順の詳細を示すフローチャートである。 本発明の第３実施形態に係る位置推定装置を備えた走行制御装置を示す概略構成図である。 図９に示された推定精度検知部により実行される検知処理手順の詳細を示すフローチャートである。 移動体の自己位置候補点と地図とのマッチ度を計測する様子を示すモデル図である。 移動体の自己位置候補点として地図とのマッチ度が高い自己位置候補点が多い状態を示す概念図である。 移動体の自己位置候補点として地図とのマッチ度が低い自己位置候補点が多い状態を示す概念図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。

図１は、本発明の第１実施形態に係る位置推定装置を備えた走行制御装置を示す概略構成図である。図１において、走行制御装置１は、例えばフォークリフトや無人搬送車等の移動体２（図２参照）を走行経路に沿って目的地まで自動的に走行させる装置である。走行制御装置１は、移動体２に搭載されている。

走行制御装置１は、レーザセンサ３と、カメラ４と、駆動部５と、コントローラ６とを備えている。

レーザセンサ３は、移動体２の周囲にレーザを照射し、レーザの反射光を受光することにより、移動体２の周囲の物体までの距離を検出する距離検出部を構成している。レーザセンサ３としては、例えばレーザレンジファインダが用いられる。使用するレーザとしては、２Ｄレーザでもよいし、３Ｄレーザでもよい。

レーザセンサ３は、図２に示されるように、レーザＬを扇状に照射する。具体的には、レーザセンサ３は、移動体２の前方直進方向を中心した規定の角度範囲（ここでは２７０度）にレーザＬを照射する。レーザセンサ３から照射されたレーザＬは物体７（図６参照）に当たり、その物体７で反射したレーザ（反射光）がレーザセンサ３で受光される。物体７は、壁や柱等といった静止物体であり、地図データに登録されている。

カメラ４は、図２に示されるように、移動体２が移動する路面に設置されたマークであるＡＲ（augmentedreality）マーカ８を撮像する。カメラ４は、ＡＲマーカ８を検出するマーク検出部を構成している。

駆動部５は、特に図示はしないが、移動体２の走行輪を回転させる走行モータと、移動体２の操舵輪を転舵させる操舵モータとを有している。

コントローラ６は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ６は、推定演算部１１と、推定精度検知部１２と、駆動制御部１３とを有している。

推定演算部１１は、レーザセンサ３の検出データに基づいて、移動体２の自己位置の推定演算を行う。推定演算部１１は、例えばＳＬＡＭ（simultaneous localization andmapping）手法を用いて、移動体２の自己位置の推定演算を行う。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。ＳＬＡＭは、レーザセンサ等を利用して、自己位置推定と環境地図の作成とを同時に行う。

具体的には、推定演算部１１は、レーザセンサ３により検出された物体７までの距離データと移動体２の周囲環境の地図データとをマッチングさせて、移動体２の自己位置の推定演算を行う。このとき、推定演算部１１は、後述する推定精度検知部１２により検知された移動体２の自己位置の推定精度に応じてレーザセンサ３の検出データの使用距離範囲を変更して、移動体２の自己位置の推定演算を行う。より詳細には、推定演算部１１は、推定精度検知部１２により検知された移動体２の自己位置の推定精度が低いときは、推定精度検知部１２により検知された移動体２の自己位置の推定精度が高いときに比べてレーザセンサ３の検出データの使用距離範囲を広くして、移動体２の自己位置の推定演算を行う。なお、推定演算部１１の具体的な処理については、後で詳述する。

推定精度検知部１２は、カメラ４により撮像されたＡＲマーカ８の画像データに基づいて、推定演算部１１による移動体２の自己位置の推定精度を検知する。具体的には、推定精度検知部１２は、ＡＲマーカ８の画像データに基づいて、推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかを検知する。推定精度検知部１２の具体的な処理については、後で詳述する。

駆動制御部１３は、推定演算部１１により推定された移動体２の自己位置に基づいて、移動体２を走行経路に沿って目的地まで走行させるように駆動部５を制御する。

本実施形態の位置推定装置１５は、レーザセンサ３と、カメラ４と、コントローラ６の推定演算部１１及び推定精度検知部１２とによって構成されている。

図３は、推定演算部１１により実行される演算処理手順の詳細を示すフローチャートである。本処理は、移動体２の走行の開始が指示されると、実行される。

図３において、推定演算部１１は、まずレーザセンサ３の検出データを取得する（手順Ｓ１０１）。続いて、推定演算部１１は、レーザセンサ３の検出データの使用距離範囲として通常距離範囲Ｐ（図６（ａ），（ｂ）参照）を使用して、移動体２の自己位置の推定演算を行う（手順Ｓ１０２）。

通常距離範囲Ｐは、移動体２が自己位置の推定演算に必要な物体７が移動体２の走行経路の近傍に存在する場所を走行するときに使用される第１距離範囲である。通常距離範囲Ｐは、予め設定されている。

推定演算部１１は、レーザセンサ３から照射され物体７に当たって反射されたレーザであるレーザセンサ３の検出データのうち、通常距離範囲Ｐ内に配置された物体７に当たって反射されたレーザであるレーザセンサ３の検出データのみを使用して、移動体２の自己位置の推定演算を行う。

具体的には、推定演算部１１は、図４に示されるように、レーザセンサ３の検出データの通常距離範囲Ｐ内における移動体２の周囲の物体７までの距離データと移動体２の周囲環境の地図データとをマッチングさせることで、移動体２の自己位置候補点Ｓを複数作成する。推定演算部１１は、各自己位置候補点Ｓからレーザを照射したと仮定した場合における地図データとのマッチ度を演算することで、各自己位置候補点Ｓの重みを算出する。そして、推定演算部１１は、重みを持った複数の自己位置候補点Ｓの加重平均値を移動体２の自己位置に設定する。このとき、推定演算部１１は、移動体２の自己位置を第１位置推定値として推定する。なお、移動体２の自己位置は、２次元座標（ＸＹ座標）及び向きで表される。

続いて、推定演算部１１は、第１位置推定値を推定精度検知部１２に出力する（手順Ｓ１０３）。続いて、推定演算部１１は、推定精度検知部１２からの推定精度検知信号（後述）が入力されたかどうかを判断する（手順Ｓ１０４）。

推定演算部１１は、推定精度検知信号が入力されたと判断したときは、レーザセンサ９の検出データを取得する（手順Ｓ１０５）。そして、推定演算部１１は、推定精度検知信号に基づいて、推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかを判断する（手順Ｓ１０６）。

推定演算部１１は、推定演算部１１による移動体２の自己位置の推定精度が低下していないと判断したときは、上記の手順Ｓ１０２と同様に、レーザセンサ３の検出データの使用距離範囲として通常距離範囲Ｐを使用して、移動体２の自己位置の推定演算を行う（手順Ｓ１０７）。

推定演算部１１は、推定演算部１１による移動体２の自己位置の推定精度が低下していると判断したときは、レーザセンサ３の検出データの使用距離範囲として長距離範囲Ｑ（図６（ｃ）参照）を使用して、移動体２の自己位置の推定演算を行う（手順Ｓ１０８）。

長距離範囲Ｑは、移動体２が自己位置の推定演算に必要な物体７が移動体２の走行経路の近傍において足りない場所を走行するときに使用される第２距離範囲である。長距離範囲Ｑは、通常距離範囲Ｐよりも広い使用距離範囲である。長距離範囲Ｑは、自己位置の推定演算に必要な物体７の数を満たすように予め設定されている。

推定演算部１１は、レーザセンサ３から照射され物体７に当たって反射されたレーザであるレーザセンサ３の検出データのうち、長距離範囲Ｑ内に配置された物体７に当たって反射されたレーザであるレーザセンサ３の検出データのみを使用して、移動体２の自己位置の推定演算を行う。

具体的には、推定演算部１１は、レーザセンサ３の検出データの長距離範囲Ｑ内における移動体２の周囲の物体７までの距離データと移動体２の周囲環境の地図データとをマッチングさせることで、移動体２の自己位置候補点Ｓを複数作成する。推定演算部１１は、各自己位置候補点Ｓからレーザを照射したと仮定した場合における地図データとのマッチ度を演算することで、各自己位置候補点Ｓの重みを算出する。そして、推定演算部１１は、重みを持った複数の自己位置候補点Ｓの加重平均値を移動体２の自己位置に設定する。

続いて、推定演算部１１は、手順Ｓ１０７または手順Ｓ１０８で得られた移動体２の自己位置を駆動制御部１３に出力する（手順Ｓ１０９）。その後、推定演算部１１は、上記の手順Ｓ１０１を再度実行する。

なお、推定演算部１１は、手順Ｓ１０６において推定演算部１１による移動体２の自己位置の推定精度が低下していないと判断したときは、手順Ｓ１０７を実行せずに、手順Ｓ１０２で得られた移動体２の自己位置を駆動制御部１３に出力してもよい。

図５は、推定精度検知部１２により実行される検知処理手順の詳細を示すフローチャートである。本処理も、推定演算部１１と同様に、移動体２の走行の開始が指示されると、実行される。

図５において、推定精度検知部１２は、まずカメラ４により撮像されたＡＲマーカ８の画像データを取得する（手順Ｓ１１１）。そして、推定精度検知部１２は、ＡＲマーカ８の画像データに基づいて、移動体２の自己位置を第２位置推定値として推定する（手順Ｓ１１２）。

続いて、推定精度検知部１２は、推定演算部１１からの第１位置推定値が入力されたかどうかを判断する（手順Ｓ１１３）。推定精度検知部１２は、第１位置推定値が入力されたと判断したときは、第１位置推定値と第２位置推定値とのずれ量を算出する（手順Ｓ１１４）。

そして、推定精度検知部１２は、第１位置推定値と第２位置推定値とのずれ量が予め決められた閾値以上であるかどうかを判断する（手順Ｓ１１５）。閾値は、第１位置推定値と第２位置推定値とのＸ座標のずれ量、Ｙ座標のずれ量及び向きのずれ量について個別に設定されている。

推定精度検知部１２は、第１位置推定値と第２位置推定値とのずれ量が閾値以上であると判断したときは、第１位置推定値と第２位置推定値とのずれ量が閾値以上である状態が予め決められた規定回数（規定サイクル）連続しているかどうかを判断する（手順Ｓ１１６）。つまり、推定精度検知部１２は、第１位置推定値と第２位置推定値とのずれ量が規定時間だけ連続して閾値以上であるかどうかを判断する。このとき、推定精度検知部１２は、例えば第１位置推定値と第２位置推定値とのＸ座標のずれ量、Ｙ座標のずれ量及び向きのずれ量のうちの少なくとも１つが対応する閾値以上であるときは、手順Ｓ１１６を実行する。

推定精度検知部１２は、第１位置推定値と第２位置推定値とのずれ量が閾値以上である状態が規定回数連続していると判断したときは、推定演算部１１による移動体２の自己位置の推定精度が低下していると判定する（手順Ｓ１１７）。このとき、推定精度検知部１２は、推定演算部１１による移動体２の自己位置の推定精度が予め決められた基準値よりも低いと判定する。

推定精度検知部１２は、手順Ｓ１１５で第１位置推定値と第２位置推定値とのずれ量が閾値以上でないと判断したとき、または手順Ｓ１１６で第１位置推定値と第２位置推定値とのずれ量が閾値以上である状態が規定回数連続していないと判断したときは、推定演算部１１による移動体２の自己位置の推定精度が低下していないと判定する（手順Ｓ１１８）。このとき、推定精度検知部１２は、推定演算部１１による移動体２の自己位置の推定精度が基準値以上であると判定する。

続いて、推定精度検知部１２は、手順Ｓ１１７または手順Ｓ１１８で判定された結果を推定精度検知信号として推定演算部１１に出力する（手順Ｓ１１９）。その後、推定精度検知部１２は、上記の手順Ｓ１１１を再度実行する。

以上のように構成された走行制御装置１において、移動体２の走行時、レーザセンサ３によって移動体２の周囲にレーザを照射することで、移動体２の周囲の物体７までの距離が検出される。そして、コントローラ６の推定演算部１１によって、レーザセンサ３の検出データに基づいて移動体２の自己位置の推定演算が行われる。そして、コントローラ６の駆動制御部１３によって、移動体２の自己位置に基づいて移動体２が目的地に向かって走行するように制御される。

ここで、通常は、図６（ａ）に示されるように、レーザセンサ３の検出データの使用距離範囲として通常距離範囲Ｐを使用して、移動体２の位置の推定演算が行われる。このとき、レーザセンサ３の検出データの通常距離範囲Ｐ内に物体７が数多く存在する場合には、レーザセンサ３から照射されたレーザに物体７が容易に捉えられるため、移動体２の自己位置が正しく推定される。

しかし、図６（ｂ）に示されるように、レーザセンサ３の検出データの通常距離範囲Ｐ内に存在する物体７の数が少ない場合には、レーザセンサ３から照射されたレーザに物体７が捉えられにくいため、移動体２の自己位置の推定精度が低下する。

そこで、コントローラ６の推定精度検知部１２によって、推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかが検知される。そして、推定演算部１１による移動体２の自己位置の推定精度が低下していると検知されたときは、図６（ｃ）に示されるように、レーザセンサ３の検出データの使用距離範囲として長距離範囲Ｑを使用して、移動体２の自己位置の推定演算が行われる。すると、レーザセンサ３の検出データの長距離範囲Ｑ内に物体７が数多く存在するようになる。従って、レーザセンサ３から照射されたレーザに物体７が容易に捉えられるため、移動体２の自己位置が正しく推定される。

以上のように本実施形態にあっては、レーザセンサ３によって、移動体２の周囲にレーザを照射することで、移動体２の周囲の物体７までの距離が検出される。そして、推定演算部１１によって、レーザセンサ３の検出データに基づいて移動体２の自己位置の推定演算が行われる。ここで、推定精度検知部１２によって、推定演算部１１による移動体２の自己位置の推定精度が検知される。そして、推定演算部１１では、移動体２の自己位置の推定精度に応じてレーザセンサ３の検出データの使用距離範囲を変更して、移動体２の自己位置の推定演算が行われる。このとき、移動体２の自己位置の推定精度が低いほど、レーザセンサ３の検出データの使用距離範囲を広くする。レーザセンサ３の検出データの使用距離範囲が広くなると、検出データの使用距離範囲内においてレーザを反射させる物体７の数が増えやすくなる。これにより、移動体２の自己位置を高精度に推定することができる。

また、本実施形態では、移動体２の自己位置の推定精度が低いときは、移動体２の自己位置の推定精度が高いときに比べてレーザセンサ３の検出データの使用距離範囲を広くして、移動体２の自己位置の推定演算が行われる。従って、移動体２の自己位置の推定精度が低いときは、レーザセンサ３の検出データの使用距離範囲が広くなるため、検出データの使用距離範囲内においてレーザを反射させる物体７の数が確実に増えやすくなる。

また、本実施形態では、推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかが検知され、移動体２の自己位置の推定精度が低下していないときは、レーザセンサ３の検出データの使用距離範囲として通常距離範囲Ｐを使用して、移動体２の自己位置の推定演算が行われ、移動体２の自己位置の推定精度が低下しているときは、レーザセンサ３の検出データの使用距離範囲として長距離範囲Ｑを使用して、移動体２の自己位置の推定演算が行われる。このようにレーザセンサ３の検出データの使用距離範囲としては、通常距離範囲Ｐ及び長距離範囲Ｑという２つのみが使用される。従って、推定演算部１１により実行される演算処理を簡素化することができる。

また、本実施形態では、通常距離範囲Ｐを使用して推定演算を行うことで、移動体２の自己位置が第１位置推定値として推定されると共に、カメラ４により検出されたＡＲマーカ８に基づいて、移動体２の自己位置が第２位置推定値として推定される。そして、第１位置推定値と第２位置推定値とのずれ量が閾値以上であるときに、移動体２の自己位置の推定精度が低下していると検知される。このようにレーザセンサ３の検出データに基づいて得られた第１位置推定値と、路面に設置されたＡＲマーカ８に基づいて得られた第２位置推定値とを比較することで、移動体２の自己位置の推定精度が低下しているかどうかが検知される。従って、移動体２の自己位置の推定精度が低下しているかどうかを正確に検知することができる。

また、本実施形態では、第１位置推定値と第２位置推定値とのずれ量が規定時間だけ連続して閾値以上であるときに、移動体２の自己位置の推定精度が低下していると検知される。従って、第１位置推定値と第２位置推定値とのずれ量が瞬間的に閾値以上になる状態が排除されるため、移動体２の自己位置の推定精度が低下しているかどうかをより正確に検知することができる。

また、本実施形態では、移動体２の自己位置の推定精度に応じて、レーザセンサ３の検出データの使用距離範囲が自動的に変更されるので、移動体２の自己位置の推定精度が必要な場所をオペレータ等がいちいち探し出して指定しなくて済む。従って、オペレータ等の負担を軽減することができる。

なお、本実施形態では、路面に設置されたＡＲマーカ８がカメラ４により撮像され、ＡＲマーカ８の画像データに基づいて移動体２の自己位置が推定され、その推定結果が推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかの検知に利用されているが、特にその形態には限られない。

例えば、路面に設置されるマークとして、バーコードを使用してもよい。この場合には、バーコードリーダによりバーコードが読み込まれ、バーコードの読込データに基づいて移動体２の自己位置が推定され、その推定結果が推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかの検知に利用される。

図７は、本発明の第２実施形態に係る位置推定装置を備えた走行制御装置を示す概略構成図である。図７において、本実施形態の位置推定装置１５は、上記のカメラ４に代えて、移動体２の状態量を計測する計測器２０を備えている。

計測器２０としては、例えば移動体２の状態量として移動体２の車輪の回転角度を計測するエンコーダ、或いは移動体２の状態量として移動体２の角速度及び加速度を計測する慣性計測ユニット等が用いられる。計測器２０は、移動体２の状態を検出する状態検出部を構成している。

また、位置推定装置１５は、上記の推定精度検知部１２に代えて、推定精度検知部１２Ａを備えている。推定精度検知部１２Ａは、計測器２０により計測された移動体２の状態量に基づいて、推定演算部１１による移動体２の自己位置の推定精度を検知する。具体的には、推定精度検知部１２Ａは、移動体２の状態量に基づいて、推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかを検知する。

図８は、推定精度検知部１２Ａにより実行される検知処理手順の詳細を示すフローチャートであり、図５に対応している。図８において、推定精度検知部１２Ａは、まず計測器２０の計測値を取得する（手順Ｓ１１１Ａ）。そして、推定精度検知部１２Ａは、計測器２０の計測値に基づいて、移動体２の自己位置を第２位置推定値として推定する（手順Ｓ１１２Ａ）。その後、推定精度検知部１２Ａは、上記の手順Ｓ１１３～Ｓ１１９を順次実行する。

このような本実施形態においては、レーザセンサ３の検出データに基づいて得られた第１位置推定値と、移動体２の状態に基づいて得られた第２位置推定値とを比較することで、移動体２の自己位置の推定精度が低下しているかどうかが検知される。従って、移動体２の自己位置の推定精度が低下しているかどうかを正確に検知することができる。

図９は、本発明の第３実施形態に係る位置推定装置を備えた走行制御装置を示す概略構成図である。図９において、本実施形態の位置推定装置１５は、上記のカメラ４及び計測器２０を備えていない。

また、位置推定装置１５は、上記の推定精度検知部１２，１２Ａに代えて、推定精度検知部１２Ｂを備えている。推定精度検知部１２Ｂは、レーザセンサ３の検出データに基づいて、推定演算部１１による移動体２の自己位置の推定精度を検知する。具体的には、推定精度検知部１２Ｂは、レーザセンサ３の検出データに基づいて、推定演算部１１による移動体２の自己位置の推定精度が低下しているかどうかを検知する。

図１０は、推定精度検知部１２Ｂにより実行される検知処理手順の詳細を示すフローチャートであり、図５に対応している。図１０において、推定精度検知部１２Ｂは、まずレーザセンサ３の検出データを取得する（手順Ｓ１２１）。

続いて、推定精度検知部１２Ｂは、移動体２の複数の自己位置候補点Ｓ（図４参照）の分散値を算出する（手順Ｓ１２２）。具体的には、推定精度検知部１２Ｂは、まず複数の自己位置候補点Ｓの平均値を算出し、各自己位置候補点Ｓと平均値との差分を標準偏差として算出し、その標準偏差の二乗平均を分散値として算出する。

続いて、推定精度検知部１２Ｂは、複数の自己位置候補点Ｓの分散値が予め決められた閾値以上であるかどうかを判断する（手順Ｓ１２３）。推定精度検知部１２Ｂは、複数の自己位置候補点Ｓの分散値が閾値以上であると判断したときは、複数の自己位置候補点Ｓの分散値が閾値以上である状態が規定回数（規定サイクル）連続しているかどうかを判断する（手順Ｓ１２４）。つまり、推定精度検知部１２Ｂは、複数の自己位置候補点Ｓの分散値が規定時間だけ連続して閾値以上であるかどうかを判断する。

推定精度検知部１２Ｂは、複数の自己位置候補点Ｓの分散値が閾値以上である状態が規定回数連続していると判断したときは、推定演算部１１による移動体２の自己位置の推定精度が低下していると判定する（手順Ｓ１２５）。

推定精度検知部１２Ｂは、手順Ｓ１２３で複数の自己位置候補点Ｓの分散値が閾値以上でないと判断したとき、または手順Ｓ１２４で複数の自己位置候補点Ｓの分散値が閾値以上である状態が規定回数連続していないと判断したときは、推定演算部１１による移動体２の自己位置の推定精度が低下していないと判定する（手順Ｓ１２６）。

続いて、推定精度検知部１２Ｂは、手順Ｓ１２５または手順Ｓ１２６で判定された結果を推定精度検知信号として推定演算部１１に出力する（手順Ｓ１２７）。その後、推定精度検知部１２Ｂは、上記の手順Ｓ１２１を再度実行する。

このように複数の自己位置候補点Ｓの分散値を用いて、推定演算部１１による移動体２の自己位置の推定精度を検知する際には、例えば図１１に示されるように、自己位置候補点Ｓから見えているレーザを地図に投影して、自己位置候補点Ｓと地図とのマッチ度を計測する。図１１（ａ）に示される自己位置候補点Ｓ１は、地図とのマッチ度が高い。図１１（ｂ）に示される自己位置候補点Ｓ２は、地図とのマッチ度が低い。

図１２に示されるように、地図とのマッチ度が高い自己位置候補点Ｓ１が多い場合には、複数の自己位置候補点Ｓの分散値が小さくなる。このため、推定演算部１１による移動体２の自己位置の推定精度が低下していないと検知される。なお、図１２中の自己位置候補点Ｓ１は、地図とのマッチ度が高い自己位置候補点Ｓである。図１２中の自己位置候補点Ｓ２は、地図とのマッチ度が低い自己位置候補点Ｓである。

図１３（ａ）に示されるように、地図とのマッチ度が低い自己位置候補点Ｓ２が多い場合には、複数の自己位置候補点Ｓの分散値が大きくなる。このため、推定演算部１１による移動体２の自己位置の推定精度が低下していると検知される。なお、図１３中の自己位置候補点Ｓ１も、地図とのマッチ度が高い自己位置候補点Ｓである。図１３中の自己位置候補点Ｓ２も、地図とのマッチ度が低い自己位置候補点Ｓである。

地図とのマッチ度が低い自己位置候補点Ｓ２が多いときは、図１３（ｂ）に示されるように、自己位置候補点Ｓを作成する範囲を広げてもよい。自己位置候補点Ｓを作成する範囲を広げても、地図とのマッチ度が低い自己位置候補点Ｓ２が多い場合に、推定演算部１１による移動体２の自己位置の推定精度が低下していると検知してもよい。

以上のような本実施形態においては、レーザセンサ３の検出データに基づいて、移動体２の自己位置候補点Ｓの分散値が算出され、分散値が閾値以上であるときに、移動体２の自己位置の推定精度が低下していると検知される。このようにレーザセンサ３の検出データに基づいた計算処理によって、移動体２の自己位置の推定精度が低下しているかどうかが検知される。従って、上記のカメラ４及び計測器２０が不要となるため、簡単な構成で、移動体２の自己位置の推定精度が低下しているかどうかを検知することができる。

また、本実施形態では、自己位置候補点Ｓの分散値が規定時間だけ連続して閾値以上であるときに、移動体２の自己位置の推定精度が低下していると検知される。従って、分散値が瞬間的に閾値以上になる状態が排除されるため、移動体２の自己位置の推定精度が低下しているかどうかを正確に検知することができる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、第１位置推定値と第２位置推定値とのずれ量または移動体２の複数の自己位置候補点Ｓの分散値が規定時間だけ連続して閾値以上であるときに、移動体２の自己位置の推定精度が低下していると検知されているが、特にその形態には限られない。第１位置推定値と第２位置推定値とのずれ量または移動体２の複数の自己位置候補点Ｓの分散値が規定距離だけ連続して閾値以上であるときに、移動体２の自己位置の推定精度が低下していると検知してもよい。また、第１位置推定値と第２位置推定値とのずれ量または移動体２の複数の自己位置候補点Ｓの分散値が閾値以上になった時点で、移動体２の自己位置の推定精度が低下していると検知してもよい。

また、上記実施形態では、移動体２の自己位置の推定精度が低下しているか否かが検知され、移動体２の自己位置の推定精度が低下していないときは、レーザセンサ３の検出データの使用距離範囲として通常距離範囲Ｐが使用され、移動体２の自己位置の推定精度が低下しているときは、レーザセンサ３の検出データの使用距離範囲として長距離範囲Ｑが使用されているが、特にその形態には限られない。例えば、移動体２の自己位置の推定精度を「高」、「中」、「低」のように３つ以上の範囲で検知し、移動体２の自己位置の推定精度が低くなるほど、レーザセンサ３の検出データの使用距離範囲を３段階以上で広くしてもよい。

また、上記実施形態は、移動体２を走行経路に沿って目的地まで走行させる走行制御装置であるが、本発明は、特にそれには限られず、例えば移動体２を対象物に沿って走行させる場合など、移動体２の自己位置を推定する必要がある装置であれば、適用可能である。

また、上記実施形態では、自己位置推定技術としてレーザを利用したＳＬＡＭ手法を用いて移動体２の自己位置が推定されているが、本発明は、特にＳＬＡＭ手法には限られず、レーザを使用して距離を検出するセンサの検出データに基づいて、移動体２の自己位置を推定する手法であれば、適用可能である。

２…移動体、３…レーザセンサ（距離検出部）、４…カメラ（マーク検出部）、７…物体、８…ＡＲマーカ（マーク）、１１…推定演算部、１２，１２Ａ，１２Ｂ…推定精度検知部、１５…位置推定装置、２０…計測器（状態検出部）、Ｌ…レーザ、Ｐ…通常距離範囲（第１距離範囲）、Ｑ…長距離範囲（第２距離範囲）、Ｓ…自己位置候補点。

Claims (6)

1. 移動体の自己位置を推定する位置推定装置において、
   前記移動体の周囲にレーザを照射し、前記レーザの反射光を受光することにより、前記移動体の周囲の物体までの距離を検出する距離検出部と、
   前記距離検出部の検出データに基づいて、前記移動体の自己位置の推定演算を行う推定演算部と、
   前記推定演算部による前記移動体の自己位置の推定精度を検知する推定精度検知部とを備え、
   前記推定演算部は、前記推定精度検知部により検知された前記移動体の自己位置の推定精度が低いときは、前記移動体の自己位置の推定精度が高いときに比べて前記距離検出部の検出データの使用距離範囲を広くして、前記移動体の自己位置の推定演算を行い、
   前記推定精度検知部は、前記推定演算部による前記移動体の自己位置の推定精度が低下しているかどうかを検知し、
   前記推定演算部は、前記移動体の自己位置の推定精度が低下していないときは、前記距離検出部の検出データの使用距離範囲として第１距離範囲を使用して、前記移動体の自己位置の推定演算を行い、前記移動体の自己位置の推定精度が低下しているときは、前記距離検出部の検出データの使用距離範囲として前記第１距離範囲よりも広い第２距離範囲を使用して、前記移動体の自己位置の推定演算を行う位置推定装置。
2. 前記移動体が移動する路面に設置されたマークを検出するマーク検出部を更に備え、
   前記推定演算部は、前記第１距離範囲を使用して前記移動体の自己位置の推定演算を行うことで、前記移動体の自己位置を第１位置推定値として推定し、
   前記推定精度検知部は、前記マーク検出部により検出された前記マークに基づいて、前記移動体の自己位置を第２位置推定値として推定し、前記第１位置推定値と前記第２位置推定値とのずれ量が閾値以上であるときに、前記移動体の自己位置の推定精度が低下していると検知する請求項１記載の位置推定装置。
3. 前記移動体の状態を検出する状態検出部を更に備え、
   前記推定演算部は、前記第１距離範囲を使用して前記移動体の自己位置の推定演算を行うことで、前記移動体の自己位置を第１位置推定値として推定し、
   前記推定精度検知部は、前記状態検出部により検出された前記移動体の状態に基づいて、前記移動体の自己位置を第２位置推定値として推定し、前記第１位置推定値と前記第２位置推定値とのずれ量が閾値以上であるときに、前記移動体の自己位置の推定精度が低下していると検知する請求項１記載の位置推定装置。
4. 前記推定精度検知部は、前記第１位置推定値と前記第２位置推定値とのずれ量が規定時間または規定距離だけ連続して前記閾値以上であるときに、前記移動体の自己位置の推定精度が低下していると検知する請求項２または３記載の位置推定装置。
5. 前記推定精度検知部は、前記距離検出部の検出データに基づいて、前記移動体の複数の自己位置候補点の分散値を算出し、前記分散値が閾値以上であるときに、前記移動体の自己位置の推定精度が低下していると検知する請求項１記載の位置推定装置。
6. 前記推定精度検知部は、前記分散値が規定時間または規定距離だけ連続して前記閾値以上であるときに、前記移動体の自己位置の推定精度が低下していると検知する請求項５記載の位置推定装置。

Images