JP7489014B2

JP7489014B2 - 位置推定システム

Info

Publication number: JP7489014B2
Application number: JP2020211082A
Authority: JP
Inventors: 祐太井上; 和孝小田
Original assignee: Toyota Industries Corp
Current assignee: Toyota Industries Corp
Priority date: 2020-03-31
Filing date: 2020-12-21
Publication date: 2024-05-23
Anticipated expiration: 2040-12-21
Also published as: JP2021163455A

Description

本発明は、位置推定システムに関する。

従来の位置推定システムとしては、例えば特許文献１に記載されているような技術が知られている。特許文献１に記載の位置推定システムは、レーザを用いて移動体と周囲にある物体との距離を計測するレーザ測長器と、移動体が走行する領域の地図を記憶した地図データ記憶部と、移動体と周囲にある物体との距離データと地図データとを基にして、移動体の自己位置を推定する同定装置とを備えている。

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

しかしながら、上記従来技術においては、移動体が自己位置を推定しながら走行している際に、移動体が廊下等のように同じ風景が続く走行路を走行すると、レーザ測長器によって同じような形状のレーザ点群が計測されることになる。その結果、移動体の自己位置の推定精度が低下してしまう。

本発明の目的は、移動体の周囲環境に関わらず、移動体の自己位置を高精度に推定することができる位置推定システムを提供することである。

本発明の一態様に係る位置推定システムは、移動体から周囲にレーザを照射し、レーザの反射光を受光することにより、移動体の周囲に存在する物体までの距離を検出するレーザセンサと、レーザセンサにより検出された物体までの距離データと移動体の周囲環境の地図データとを用いて、移動体の自己位置を推定する第１自己位置推定部と、移動体が走行する走行路に沿って設置され、レーザセンサから照射されるレーザを物体よりも高い輝度で反射させる複数の反射部材と、複数の反射部材のうち少なくとも２つの反射部材の位置情報を用いて、移動体の自己位置を推定する第２自己位置推定部とを備える。

このような位置推定システムにおいては、第１自己位置推定部によって、レーザセンサにより検出された移動体の周囲に存在する物体までの距離データと移動体の周囲環境の地図データとを用いて、移動体の自己位置が推定される。このとき、同じ風景が続く走行路を移動体が走行する場合には、第１自己位置推定部による移動体の自己位置の推定精度が低下しやすい。そこで、そのような同じ風景が続く周囲環境では、レーザセンサから照射されるレーザを移動体の周囲に存在する物体よりも高い輝度で反射させる複数の反射部材を、予め走行路に沿って設置しておく。そして、第２自己位置推定部によって、少なくとも２つの反射部材の位置情報を用いて、移動体の自己位置を推定することにより、移動体の自己位置の推定精度が確保される。これにより、移動体の周囲環境に関わらず、移動体の自己位置が高精度に推定される。

位置推定システムは、第１自己位置推定部による移動体の自己位置の推定信頼度を判定する信頼度判定部と、信頼度判定部により判定された第１自己位置推定部による移動体の自己位置の推定信頼度に基づいて、移動体の自己位置を決定する自己位置決定部とを更に備え、自己位置決定部は、信頼度判定部により移動体の自己位置の推定信頼度が高いと判定されたときは、第１自己位置推定部により推定された移動体の自己位置である第１自己位置推定値を移動体の自己位置として決定する第１推定モードを実施し、信頼度判定部により移動体の自己位置の推定信頼度が低いと判定されたときは、第２自己位置推定部により推定された移動体の自己位置である第２自己位置推定値を移動体の自己位置として決定する第２推定モードを実施してもよい。

このような構成では、第１自己位置推定部により移動体の自己位置を推定している際に、移動体の自己位置の推定信頼度が低いと判定されると、第２自己位置推定部により推定された移動体の自己位置である第２自己位置推定値が移動体の自己位置として決定される。従って、同じ風景が続く走行路を移動体が走行することで、第１自己位置推定部による移動体の自己位置の推定精度が低下しても、移動体の自己位置が確実に高精度に推定される。

位置推定システムは、レーザセンサの検出データに基づいて、移動体の周囲に反射部材が存在するかどうかを判断し、移動体の周囲に未登録の反射部材が存在するときに、第１自己位置推定値または第２自己位置推定値に基づいて、未登録の反射部材の位置情報を輝度ランドマークとして登録するランドマーク登録部を更に備えてもよい。

このような構成では、移動体の周囲に未登録の反射部材が存在するときには、第１自己位置推定値または第２自己位置推定値に基づいて、未登録の反射部材の位置情報が輝度ランドマークとして自動的に登録される。従って、複数の反射部材を走行路に沿って設置したときに、作業者が各反射部材の設置箇所の位置情報をいちいち設定登録しなくて済むため、作業者の負担が軽減される。

ランドマーク登録部は、信頼度判定部により移動体の自己位置の推定信頼度が高いと判定されたときは、第１自己位置推定値に基づいて、未登録の反射部材の位置情報を輝度ランドマークとして登録してもよい。

このような構成では、第１自己位置推定値に基づいて、反射部材の位置情報が輝度ランドマークとして自動的に登録される場合でも、移動体の自己位置の推定精度が確保されているため、反射部材の位置情報が適切な輝度ランドマークが得られる。

ランドマーク登録部は、移動体の周囲に輝度ランドマークとして登録済の反射部材が少なくとも２つ存在するときは、第２自己位置推定値に基づいて、反射部材の位置情報を輝度ランドマークとして登録してもよい。

このような構成では、同じ風景が続く走行路を移動体が走行する場合でも、第２自己位置推定部によって、輝度ランドマークとして登録済の少なくとも２つの反射部材の位置情報を用いて移動体の自己位置が推定されるため、反射部材の位置情報が適切な輝度ランドマークが得られる。

ランドマーク登録部は、レーザセンサの検出データに基づいて、輝度ランドマークとして登録済の反射部材からのレーザの反射光を受光しなくなったかどうかを判断し、登録済の反射部材からのレーザの反射光を受光しなくなったときは、登録済の反射部材に対応する輝度ランドマークを削除してもよい。

このような構成では、移動体が輝度ランドマークとして登録済の反射部材を通過することで、当該反射部材からのレーザの反射光がレーザセンサによりを受光されなくなると、当該反射部材に対応する輝度ランドマークが削除されることになる。従って、移動体が同じ走行路を再度走行する際に、走行路の周囲環境の変化によって反射部材の数や位置等が変わっていても、反射部材の位置情報を輝度ランドマークとして再度登録することで、第２自己位置推定部により移動体の自己位置が高精度に推定される。

位置推定システムは、第１自己位置推定値と第２自己位置推定値との距離が予め決められた規定値以上であるときに、第１自己位置推定値が異常であると検知する異常検知部を更に備えてもよい。

このような構成では、第１自己位置推定値が異常であると検知されたときに、その旨を作業者に知らせたり、移動体の走行を強制的に停止させることができる。

位置推定システムは、異常検知部により第１自己位置推定値が異常であると検知されたときに、第２自己位置推定値を第１自己位置推定値として再設定する自己位置補正部を更に備えてもよい。

このような構成では、第１自己位置推定値が異常であると検知されたときは、第２自己位置推定値が第１自己位置推定値として再設定される。このため、第１自己位置推定部により推定される移動体の自己位置が適切な値に初期化されることになる。従って、第１自己位置推定部による移動体の自己位置の推定精度が高くなる。

自己位置決定部は、第１推定モードの実施時に、信頼度判定部により移動体の自己位置の推定信頼度が低いと判定されると共に、レーザセンサの検出データに基づいて、移動体の周囲に反射部材が少なくとも２つ存在すると判断されたときは、第１推定モードから第２推定モードに移行してもよい。

このような構成では、第１推定モードの実施時に、移動体の自己位置の推定信頼度が低くなっても、移動体の周囲に反射部材が少なくとも２つ存在すると判断されたときは、第２自己位置推定部による移動体の自己位置推定が可能である。このため、第１推定モードから第２推定モードに移行するようになる。従って、移動体の自己位置推定の信頼性が確保される。

自己位置決定部は、第２推定モードの実施時に、信頼度判定部により移動体の自己位置の推定信頼度が高いと判定されると共に、第１自己位置推定値と第２自己位置推定値との距離が予め決められた閾値以下であるときは、第２推定モードから第１推定モードに移行してもよい。

このような構成では、第２推定モードの実施時に、第１自己位置推定部による移動体の自己位置の推定信頼度が高いと共に、第１自己位置推定値と第２自己位置推定値との距離が閾値以下であるときは、第２推定モードから第１推定モードに移行する。従って、移動体の自己位置推定の信頼性が確保される。

自己位置決定部は、第２推定モードの実施時に、移動体の自己位置の推定信頼度が高いと判定されると共に第１自己位置推定値と第２自己位置推定値との距離が閾値以下である状態が規定時間だけ継続したときに、第２推定モードから第１推定モードに移行してもよい。

このような構成では、第２推定モードから第１推定モードへの移行時に、ノイズ等の影響があっても、移動体の自己位置推定の信頼性が確保される。

位置推定システムは、信頼度判定部により移動体の自己位置の推定信頼度が低いと判定されると共に、レーザセンサの検出データに基づいて、移動体の周囲に反射部材が少なくとも２つ存在しないと判断されたときに、第１自己位置推定値及び第２自己位置推定値が異常であると検知する異常検知部を更に備えてもよい。

このような構成では、第１自己位置推定値及び第２自己位置推定値が異常であると検知されたときに、その旨を作業者に知らせたり、移動体の走行を強制的に停止させることができる。

異常検知部は、移動体の自己位置の推定信頼度が低いと判定されると共に移動体の周囲に反射部材が少なくとも２つ存在しないと判断される状態が規定時間だけ継続したときに、第１自己位置推定値及び第２自己位置推定値が異常であると検知してもよい。

このような構成では、ノイズ等の影響があっても、第１自己位置推定値及び第２自己位置推定値が異常であることを精度良く検知することができる。

位置推定システムは、信頼度判定部により移動体の自己位置の推定信頼度が低いと判定されると共に、レーザセンサの検出データに基づいて、移動体の周囲に反射部材が少なくとも２つ存在しないと判断されたときに、直前に推定または補完された第２自己位置推定値を第１自己位置推定値として再設定する自己位置補正部を更に備えてもよい。

このような構成では、第１自己位置推定部による移動体の自己位置の推定信頼度が低い状態で、第２自己位置推定部による移動体の自己位置推定が不可能になると、直前に推定または補完された第２自己位置推定値が第１自己位置推定値として再設定される。このため、第１自己位置推定部により推定される移動体の自己位置が適切な値に初期化されることになる。従って、第１自己位置推定部による移動体の自己位置の推定精度が高くなる。

本発明によれば、移動体の周囲環境に関わらず、移動体の自己位置を高精度に推定することができる。

本発明の第１実施形態に係る位置推定システムを備えた走行制御システムの概略構成を示すブロック図である。レーザセンサから移動体の周囲にレーザが照射される様子を示す概念図である。レーザセンサから照射されたレーザが反射板で反射される様子を示す概念図である。図１に示された３点測量自己位置推定部において、輝度ランドマークとして登録された３つの反射板の位置座標を用いた３点測量法によって、移動体の自己位置が推定される様子を示す概念図である。図１に示されたランドマーク登録部により実行されるランドマーク登録処理の手順の詳細を示すフローチャートである。移動体が前方に進んだときに、反射板からのレーザ反射点の位置が移動体の進行方向とは逆の方向に変化する様子を示す概念図である。３点測量自己位置推定部により推定された移動体の自己位置に基づいて、反射板の位置座標が輝度ランドマークとして新たに登録される様子を示す概念図である。図１に示された自己位置決定部により実行される自己位置決定処理の手順の詳細を示すフローチャートである。本発明の第２実施形態に係る位置推定システムを備えた走行制御システムの概略構成を示すブロック図である。図９に示された異常検知部により実行される異常検知処理の手順の詳細を示すフローチャートである。ＳＬＡＭ自己位置推定部により得られた移動体の第１自己位置推定値が異常であると検知される様子を示す概念図である。３点測量自己位置推定部により得られた移動体の第２自己位置推定値がＳＬＡＭ自己位置推定部により得られた移動体の第１自己位置推定値として再設定される様子を示す概念図である。本発明の第３実施形態に係る位置推定システムを備えた走行制御システムの概略構成を示すブロック図である。図１３に示された自己位置決定部により実行される自己位置決定処理の手順の詳細を示すフローチャートである。図１３に示された異常検知部により実行される異常検知処理の手順の詳細を示すフローチャートである。図１３に示された位置推定システムにおいて、３点測量モードからＳＬＡＭモードに移行する様子を示すタイミングチャートである。図１３に示された位置推定システムにおいて、第１自己位置推定値及び第２自己位置推定値が異常であると検知される様子を示すタイミングチャートである。輝度ランドマークとして登録された２つの反射板の位置座標を用いて、移動体の自己位置が推定される様子を示す概念図である。

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

図１は、本発明の第１実施形態に係る位置推定システムを備えた走行制御システムの概略構成を示すブロック図である。図１において、走行制御システム１は、例えばフォークリフトや無人搬送車等の移動体２（図２参照）を自動的に誘導走行させるシステムである。

走行制御システム１は、レーザセンサ３と、エンコーダ４と、複数の反射板５と、コントローラ６と、駆動部７とを備えている。レーザセンサ３、エンコーダ４、コントローラ６及び駆動部７は、移動体２に搭載されている。

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

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

エンコーダ４は、移動体２の車輪（図示せず）の回転角度を計測するオドメトリ用のセンサである。

反射板５は、図３に示されるように、レーザセンサ３から照射されるレーザＬを移動体２の周囲に存在する物体８よりも高い輝度で反射させる反射部材である。物体８は、例えば移動体２が走行する走行路９の左右両側に設置された壁である。複数の反射板５は、走行路９に沿って左右両側に間隔を隔てて設置されている。各反射板５は、物体８の同じ高さ位置に取り付けられている。

コントローラ６は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ６は、レーザセンサ３の検出データ及びエンコーダ４の計測値を取得し、所定の処理を実行し、駆動部７を制御する。なお、駆動部７は、特に図示はしないが、移動体２を走行させる走行モータと、移動体２を転舵させる操舵モータとを有している。

コントローラ６は、ＳＬＡＭ自己位置推定部１０と、オドメトリ自己位置推定部１１と、信頼度判定部１２と、ランドマーク登録部１３と、３点測量自己位置推定部１４と、自己位置決定部１５と、駆動制御部１６とを有している。

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

具体的には、ＳＬＡＭ自己位置推定部１０は、レーザセンサ３により検出された移動体２の周囲に存在する物体８までの距離データと移動体２の周囲環境の地図データとをマッチングさせて、移動体２の自己位置の推定演算を行う。なお、移動体２の自己位置は、例えば移動体２の中心の位置である。また、移動体２の自己位置は、位置座標（ＸＹ座標）及び向き（角度）で表される。

オドメトリ自己位置推定部１１は、エンコーダ４により計測された移動体２の車輪の回転角度から移動体２の移動量を求め、その移動体２の移動量に基づいて移動体２の自己位置を推定する。

信頼度判定部１２は、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置とオドメトリ自己位置推定部１１により推定された移動体２の自己位置とに基づいて、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度を判定する。

具体的には、信頼度判定部１２は、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置とオドメトリ自己位置推定部１１により推定された移動体２の自己位置とのずれ量が予め決められた閾値以上であるときは、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が低いと判定する。信頼度判定部１２は、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置とオドメトリ自己位置推定部１１により推定された移動体２の自己位置とのずれ量が閾値よりも少ないときは、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が高いと判定する。

このとき、信頼度判定部１２は、例えばＸ座標のずれ量、Ｙ座標のずれ量及び角度のずれ量のうちの少なくとも１つが対応する閾値以上であるときに、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が低いと判定する。

ランドマーク登録部１３は、レーザセンサ３の検出データに基づいて、移動体２の周囲に反射板５が存在するかどうかを判断し、移動体２の周囲に未登録の反射板５が存在するときに、ＳＬＡＭ自己位置推定部１０または３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置情報を輝度ランドマークとして登録する。なお、ランドマーク登録部１３の具体的な処理については、後で詳述する。

３点測量自己位置推定部１４は、ランドマーク登録部１３により輝度ランドマークとして登録された複数の反射板５のうちの３つの反射板５の位置情報を用いて、移動体２の自己位置を推定する第２自己位置推定部である。

具体的には、３点測量自己位置推定部１４は、図４に示されるように、三角測量法を用いて、輝度ランドマークＭとして登録された３つの反射板５の位置情報から移動体２の自己位置の推定演算を行う。このとき、移動体２に近い３つの輝度ランドマークＭの位置座標が使用される。３点測量自己位置推定部１４は、３つの輝度ランドマークＭ（反射板５）の位置座標と、移動体２と反射板５との相対距離とから、移動体２の自己位置の推定演算を行う。

自己位置決定部１５は、信頼度判定部１２により判定されたＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度に基づいて、移動体２の自己位置を決定する。なお、自己位置決定部１５の具体的な処理については、後で詳述する。

駆動制御部１６は、自己位置決定部１５により決定された移動体２の自己位置に基づいて、移動体２を目的地まで走行させるように駆動部７を制御する。

ここで、レーザセンサ３、エンコーダ４、複数の反射板５、コントローラ６のＳＬＡＭ自己位置推定部１０、オドメトリ自己位置推定部１１、信頼度判定部１２、ランドマーク登録部１３、３点測量自己位置推定部１４及び自己位置決定部１５は、本実施形態の位置推定システム１７を構成している。

図５は、ランドマーク登録部１３により実行されるランドマーク登録処理の手順の詳細を示すフローチャートである。本処理は、移動体２の走行が開始されると実行される。なお、本処理の実行前は、登録フラグが０に設定されている。登録フラグは、輝度ランドマークとして登録された反射板５があるかどうかを表すフラグである。輝度ランドマークとして登録された反射板５がないときは、登録フラグが０に設定される。

図５において、ランドマーク登録部１３は、まずレーザセンサ３の検出データを取得する（手順Ｓ１０１）。そして、ランドマーク登録部１３は、レーザセンサ３の検出データに基づいて、移動体２の周囲に反射板５が存在するかどうかを判断する（手順Ｓ１０２）。このとき、図６に示されるように、レーザセンサ３の検出データにおける複数のレーザ反射点Ｒ_Ｌのうち輝度値が予め決められた規定値よりも高いレーザ反射点Ｐ_Ｌが存在するときに、反射板５が存在すると判定される。

ランドマーク登録部１３は、移動体２の周囲に反射板５が存在すると判断したときは、登録フラグが１であるかどうかを判断する（手順Ｓ１０３）。輝度ランドマークとして登録された反射板５があるときは、登録フラグが１に設定される。

ランドマーク登録部１３は、登録フラグが１であると判断したときは、レーザセンサ３の検出データに基づいて、輝度ランドマークとして登録された反射板５からのレーザの反射光を受光したかどうかを判断する（手順Ｓ１０４）。

ここで、図６に示されるように、移動体２が前方に進むに従い、反射板５からのレーザ反射点Ｐ_Ｌは、移動体２から見て進行方向（Ａ方向）とは逆の方向（Ｂ方向）においてレーザセンサ３により計測される。そして、移動体２が更に前方に進むと、反射板５からのレーザ反射点Ｐ_Ｌがレーザセンサ３により計測されなくなる。従って、移動体２が輝度ランドマークとして登録された反射板５を通過すると、当該反射板５からのレーザＬの反射光がレーザセンサ３により受光されなくなる。

ランドマーク登録部１３は、輝度ランドマークとして登録された反射板５からのレーザの反射光を受光していないと判断したときは、当該反射板５に対応する輝度ランドマークを削除する（手順Ｓ１０５）。

ランドマーク登録部１３は、手順Ｓ１０３で登録フラグが０であると判断したとき、手順Ｓ１０４で輝度ランドマークとして登録された反射板５からのレーザの反射光を受光したと判断したとき、または手順Ｓ１０５を実行した後、信頼度判定部１２により得られた移動体２の自己位置の推定信頼度の判定結果を取得する（手順Ｓ１０６）。そして、ランドマーク登録部１３は、移動体２の自己位置の推定信頼度が高いかどうかを判断する（手順Ｓ１０７）。

ランドマーク登録部１３は、移動体２の自己位置の推定信頼度が高いと判断したときは、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置（第１自己位置推定値）を取得する（手順Ｓ１０８）。

そして、ランドマーク登録部１３は、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置座標を輝度ランドマークとして登録する（手順Ｓ１０９）。このとき、ランドマーク登録部１３は、移動体２の自己位置と、移動体２と反射板５との相対距離とから、未登録の反射板５の位置座標を求める。

ランドマーク登録部１３は、手順Ｓ１０７で移動体２の自己位置の推定信頼度が低いと判断したときは、輝度ランドマークが３つ以上あるかどうかを判断する（手順Ｓ１１０）。

ランドマーク登録部１３は、輝度ランドマークが３つ以上あると判断したときは、３点測量自己位置推定部１４により推定された移動体２の自己位置（第２自己位置推定値）を取得する（手順Ｓ１１１）。

そして、ランドマーク登録部１３は、３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置座標を輝度ランドマークとして登録する（手順Ｓ１１２）。このとき、ランドマーク登録部１３は、上記の手順Ｓ１０９と同様に、移動体２の自己位置と、移動体２と反射板５との相対距離とから、未登録の反射板５の位置座標を求める。これにより、図７に示されるように、新たな輝度ランドマークＭが登録される。

ランドマーク登録部１３は、手順Ｓ１０９または手順Ｓ１１２を実行した後、登録フラグを１に設定する（手順Ｓ１１３）。そして、ランドマーク登録部１３は、上記の手順Ｓ１０１を再度実行する。

また、ランドマーク登録部１３は、手順Ｓ１１０で輝度ランドマークが３つ以上ないと判断したとき、または移動体２の周囲に反射板５が存在しないと判断したときも、上記の手順Ｓ１０１を再度実行する。

図８は、自己位置決定部１５により実行される自己位置決定処理の手順の詳細を示すフローチャートである。本処理は、移動体２の走行が開始されると実行される。

図８において、自己位置決定部１５は、まず信頼度判定部１２により得られた移動体２の自己位置の推定信頼度の判定結果を取得する（手順Ｓ１２１）。そして、自己位置決定部１５は、移動体２の自己位置の推定信頼度が高いかどうかを判断する（手順Ｓ１２２）。

自己位置決定部１５は、移動体２の自己位置の推定信頼度が高いと判断したときは、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置（第１自己位置推定値）を最終的な移動体２の自己位置として決定するＳＬＡＭモードを実施する（手順Ｓ１２３）。ＳＬＡＭモードは、移動体２の自己位置として第１自己位置推定値を使用する第１推定モードである。そして、自己位置決定部１５は、上記の手順Ｓ１２１を再度実行する。

自己位置決定部１５は、移動体２の自己位置の推定信頼度が低いと判断したときは、３点測量自己位置推定部１４により推定された移動体２の自己位置（第２自己位置推定値）を最終的な移動体２の自己位置として決定する３点測量モードを実施する（手順Ｓ１２４）。３点測量モードは、移動体２の自己位置として第２自己位置推定値を使用する第２推定モードである。そして、自己位置決定部１５は、上記の手順Ｓ１２１を再度実行する。

以上において、移動体２を目的地に向かって誘導走行させる際には、レーザセンサ３から移動体２の周囲にレーザが照射され、移動体２の周囲に存在する物体８で反射したレーザの反射光がレーザセンサ３により受光されることで、移動体２の周囲に存在する物体８までの距離が検出される。そして、ＳＬＡＭ自己位置推定部１０において、レーザセンサ３の検出データと移動体２の周囲環境の地図データとのマッチングを行って、移動体２の自己位置が推定される。

ただし、同じ風景が続く走行路９を移動体２が走行する際には、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が低下する。この場合には、図４に示されるように、３点測量自己位置推定部１４において、輝度ランドマークＭとして登録された３つの反射板５の位置座標を用いた３点測量によって移動体２の自己位置が推定される。これにより、移動体２の自己位置の推定精度が確保される。

また、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が高いときは、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置座標が輝度ランドマークＭとして登録される。一方、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が低いときは、図７に示されるように、３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置座標が輝度ランドマークＭとして登録される。

以上のように本実施形態によれば、ＳＬＡＭ自己位置推定部１０によって、レーザセンサ３により検出された移動体２の周囲に存在する物体８までの距離データと移動体２の周囲環境の地図データとを用いて、移動体２の自己位置が推定される。このとき、同じ風景が続く走行路９を移動体２が走行する場合には、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定精度が低下しやすい。そこで、そのような同じ風景が続く周囲環境では、レーザセンサ３から照射されるレーザを移動体２の周囲に存在する物体８よりも高い輝度で反射させる複数の反射板５を、予め走行路９に沿って設置しておく。そして、３点測量自己位置推定部１４によって、３つの反射板５の位置情報を用いて、移動体２の自己位置を推定することにより、移動体２の自己位置の推定精度が確保される。これにより、移動体２の周囲環境に関わらず、移動体２の自己位置が高精度に推定される。その結果、移動体２の誘導走行のロバスト性を向上させることができる。

また、本実施形態では、移動体２の自己位置の推定信頼度が高いと判定されたときは、ＳＬＡＭ自己位置推定部１０により得られた第１自己位置推定値が移動体２の自己位置として決定され、移動体の自己位置の推定信頼度が低いと判定されたときは、３点測量自己位置推定部１４により得られた第２自己位置推定値が移動体２の自己位置として決定される。このため、ＳＬＡＭ自己位置推定部１０により移動体２の自己位置を推定している際に、移動体２の自己位置の推定信頼度が低いと判定されると、３点測量自己位置推定部１４により得られた第２自己位置推定値が移動体２の自己位置として決定される。従って、同じ風景が続く走行路９を移動体２が走行することで、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定精度が低下しても、移動体２の自己位置が確実に高精度に推定される。

また、本実施形態では、移動体２の周囲に未登録の反射板５が存在するときには、ＳＬＡＭ自己位置推定部１０または３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置情報が輝度ランドマークとして自動的に登録される。従って、複数の反射板５を走行路９に沿って設置したときに、作業者が各反射板５の設置箇所の位置情報をいちいち設定登録しなくて済むため、作業者の負担が軽減される。

また、本実施形態では、移動体２の自己位置の推定信頼度が高いと判定されたときは、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置情報が輝度ランドマークとして登録される。従って、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置情報が輝度ランドマークとして自動的に登録される場合でも、移動体２の自己位置の推定精度が確保されているため、反射板５の位置情報が適切な輝度ランドマークが得られる。

また、本実施形態では、移動体２の周囲に輝度ランドマークとして登録済の反射板５が３つ存在するときは、３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置情報が輝度ランドマークとして登録される。従って、同じ風景が続く走行路９を移動体２が走行する場合でも、３点測量自己位置推定部１４によって３つの登録済の反射板５の位置情報を用いて移動体２の自己位置が推定されるため、反射板５の位置情報が適切な輝度ランドマークが得られる。

また、本実施形態では、輝度ランドマークとして登録済の反射板５からのレーザ反射光を受光しなくなったときは、登録済の反射板５に対応する輝度ランドマークが削除される。このため、移動体２が登録済の反射板５を通過することで、当該反射板５からのレーザの反射光がレーザセンサ３により受光されなくなると、当該反射板５に対応する輝度ランドマークが削除されることになる。従って、移動体２が同じ走行路９を再度走行する際に、走行路９の周囲環境の変化によって反射板５の数や位置等が変わっていても、反射板５の位置情報を輝度ランドマークとして再度登録することで、３点測量自己位置推定部１４により移動体２の自己位置が高精度に推定される。

図９は、本発明の第２実施形態に係る位置推定システムを備えた走行制御システムの概略構成を示すブロック図である。図９において、本実施形態の位置推定システム１７は、上記のレーザセンサ３、エンコーダ４、複数の反射板５及び駆動部７と、報知部２０と、コントローラ２１とを備えている。

報知部２０は、移動体２に搭載されている。報知部２０としては、表示器及び警報器等が使用される。

コントローラ２１は、上記のコントローラ６の構成に加え、異常検知部２２と、自己位置補正部２３とを更に有している。

異常検知部２２は、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置（第１自己位置推定値）と３点測量自己位置推定部１４により推定された移動体２の自己位置（第２自己位置推定値）との距離が予め決められた規定値以上であるときに、第１自己位置推定値が異常であると検知する。

図１０は、異常検知部２２により実行される異常検知処理の手順の詳細を示すフローチャートである。本処理は、移動体２の走行が開始されると実行される。

図１０において、異常検知部２２は、まずＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置（第１自己位置推定値）と３点測量自己位置推定部１４により推定された移動体２の自己位置（第２自己位置推定値）とを取得する（手順Ｓ１３１）。

そして、異常検知部２２は、図１１に示されるように、移動体２の第１自己位置推定値Ｓ１と移動体２の第２自己位置推定値Ｓ２との距離Ｄが規定値以上であるかどうかを判断する（手順Ｓ１３２）。

異常検知部２２は、移動体２の第１自己位置推定値Ｓ１と移動体２の第２自己位置推定値Ｓ２との距離Ｄが規定値以上であると判断したときは、第１自己位置推定値Ｓ１が異常であると検知する（手順Ｓ１３３）。

そして、異常検知部２２は、第１自己位置推定値Ｓ１が異常である旨を報知信号として報知部２０に出力する（手順Ｓ１３４）。また、異常検知部２２は、移動体２の走行を強制的に停止させるように駆動部７を制御する（手順Ｓ１３５）。

異常検知部２２は、手順Ｓ１３２で移動体２の第１自己位置推定値Ｓ１と移動体２の第２自己位置推定値Ｓ２との距離Ｄが規定値以上でないと判断したときは、手順Ｓ１３３～Ｓ１３５を実行しない。

自己位置補正部２３は、異常検知部２２により移動体２の第１自己位置推定値が異常であると検知されたときに、３点測量自己位置推定部１４により得られた移動体２の第２自己位置推定値をＳＬＡＭ自己位置推定部１０により得られた移動体２の第１自己位置推定値として再設定する。

例えば図１２（ａ）に示されるように、移動体２の第１自己位置推定値Ｓ１と移動体２の第２自己位置推定値Ｓ２との距離Ｄが規定値以上である場合には、図１２（ｂ）に示されるように、第２自己位置推定値Ｓ２が第１自己位置推定値Ｓ１として再設定される。つまり、第１自己位置推定値Ｓ１が第２自己位置推定値Ｓ２に補正されることになる。その状態で、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定が継続して実施される。

以上のように本実施形態では、ＳＬＡＭ自己位置推定部１０により得られた移動体２の第１自己位置推定値と３点測量自己位置推定部１４により得られた移動体２の第２自己位置推定値との距離が規定値以上であるときは、第１自己位置推定値が異常であると検知される。従って、第１自己位置推定値が異常であると検知されたときに、その旨を作業者に知らせたり、移動体２の走行を強制的に停止させることができる。

また、本実施形態では、第１自己位置推定値が異常であると検知されたときは、３点測量自己位置推定部１４により得られた移動体２の第２自己位置推定値がＳＬＡＭ自己位置推定部１０により得られた移動体２の第１自己位置推定値として再設定される。このため、ＳＬＡＭ自己位置推定部１０により推定される移動体２の自己位置が適切な値に初期化（リセット）されることになる。従って、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定精度が高くなる。

図１３は、本発明の第３実施形態に係る位置推定システムを備えた走行制御システムの概略構成を示すブロック図である。図１３において、本実施形態の位置推定システム１７は、上記のレーザセンサ３、エンコーダ４、複数の反射板５、駆動部７及び報知部２０と、コントローラ３０とを備えている。

コントローラ３０は、上記のＳＬＡＭ自己位置推定部１０、オドメトリ自己位置推定部１１、信頼度判定部１２、ランドマーク登録部１３及び３点測量自己位置推定部１４と、自己位置決定部３１と、異常検知部３２と、自己位置補正部３３と、上記の駆動制御部１６とを有している。

自己位置決定部３１は、信頼度判定部１２により判定されたＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度とレーザセンサ３の検出データとに基づいて、移動体２の自己位置を決定する。自己位置決定部３１による自己位置決定処理については、後で詳述する。

異常検知部３２は、信頼度判定部１２により移動体２の自己位置の推定信頼度が低いと判定されると共に、レーザセンサ３の検出データに基づいて、移動体２の周囲に反射板５が３つ以上存在しないと判断されたときに、第１自己位置推定値及び第２自己位置推定値が異常であると検知する。

自己位置補正部３３は、信頼度判定部１２により移動体２の自己位置の推定信頼度が低いと判定されると共に、レーザセンサ３の検出データに基づいて、移動体２の周囲に反射板５が３つ以上存在しないと判断されたときに、直前に推定された第２自己位置推定値を第１自己位置推定値として再設定する。

このとき、自己位置補正部３３は、オドメトリ自己位置推定部１１により推定された移動体２の自己位置を用いて、直前に推定された第２自己位置推定値を補完することで、第１自己位置推定値を再設定してもよい。また、自己位置補正部３３は、移動体２の周囲に反射板５が３つ以上存在しないと判断される状態が継続している間、第１自己位置推定値の再設定を繰り返し実施してもよい。

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

図１４において、自己位置決定部３１は、まず現在実施されている推定モードがＳＬＡＭモードであるかどうかを判断する（手順Ｓ１５１）。自己位置決定部３１は、現在実施されている推定モードがＳＬＡＭモードであると判断したときは、信頼度判定部１２により得られた移動体２の自己位置の推定信頼度の判定結果を取得する（手順Ｓ１５２）。

そして、自己位置決定部３１は、移動体２の自己位置の推定信頼度が高いかどうかを判断する（手順Ｓ１５３）。自己位置決定部３１は、移動体２の自己位置の推定信頼度が高いと判断したときは、上記の手順Ｓ１５１を再度実行する。

自己位置決定部３１は、移動体２の自己位置の推定信頼度が低いと判断したときは、レーザセンサ３の検出データを取得する（手順Ｓ１５４）。そして、自己位置決定部３１は、レーザセンサ３の検出データに基づいて、移動体２の周囲に反射板５が３つ以上存在するかどうかを判断する（手順Ｓ１５５）。ここで、自己位置決定部３１は、輝度ランドマークとして登録済の３つ以上の反射板５からのレーザの反射光を受光したかどうかによって、移動体２の周囲に反射板５が３つ以上存在するかどうかを判断する。

自己位置決定部３１は、移動体２の周囲に反射板５が３つ以上存在すると判断したときは、３点測量自己位置推定部１４による移動体２の自己位置推定が可能であるとして、推定モードをＳＬＡＭモードから３点測量モードに移行し（手順Ｓ１５６）、上記の手順Ｓ１５１を再度実行する。これにより、３点測量モードが実施されることとなる。

自己位置決定部３１は、移動体２の周囲に反射板５が３つ以上存在していないと判断したときは、３点測量自己位置推定部１４による移動体２の自己位置推定が不可能であるとして、上記の手順Ｓ１５６を実行せずに、上記の手順Ｓ１５１を再度実行する。これにより、ＳＬＡＭモードがそのまま継続して実施されることとなる。

自己位置決定部３１は、手順Ｓ１５１で現在実施されている推定モードがＳＬＡＭモードでなく３点測量モードであると判断したときは、信頼度判定部１２により得られた移動体２の自己位置の推定信頼度の判定結果を取得する（手順Ｓ１５７）。

そして、自己位置決定部３１は、移動体２の自己位置の推定信頼度が高いかどうかを判断する（手順Ｓ１５８）。自己位置決定部３１は、移動体２の自己位置の推定信頼度が高いと判断したときは、第１自己位置推定値と第２自己位置推定値との距離が予め決められた閾値以下であるかどうかを判断する（手順Ｓ１５９）。

自己位置決定部３１は、第１自己位置推定値と第２自己位置推定値との距離が閾値以下であると判断したときは、両者の距離が閾値以下となってから規定時間が経過したかどうかを判断する（手順Ｓ１６０）。自己位置決定部３１は、規定時間が経過したと判断したときは、推定モードを３点測量モードからＳＬＡＭモードに移行し（手順Ｓ１６１）、上記の手順Ｓ１５１を再度実行する。これにより、ＳＬＡＭモードが実施されることとなる。

自己位置決定部３１は、手順Ｓ１５８で移動体２の自己位置の推定信頼度が低いと判断したとき、手順Ｓ１５９で第１自己位置推定値と第２自己位置推定値との距離が閾値以下でないと判断したとき、手順Ｓ１６０で規定時間が経過していないと判断したときは、上記の手順Ｓ１６１を実行せずに、上記の手順Ｓ１５１を再度実行する。これにより、３点測量モードがそのまま継続して実施されることとなる。

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

図１５において、異常検知部３２は、まず信頼度判定部１２により得られた移動体２の自己位置の推定信頼度の判定結果を取得する（手順Ｓ１７１）。そして、異常検知部３２は、移動体２の自己位置の推定信頼度が低いかどうかを判断する（手順Ｓ１７２）。異常検知部３２は、移動体２の自己位置の推定信頼度が低いと判断したときは、レーザセンサ３の検出データを取得する（手順Ｓ１７３）。

そして、異常検知部３２は、レーザセンサ３の検出データに基づいて、移動体２の周囲に反射板５が３つ以上存在しないかどうかを判断する（手順Ｓ１７４）。ここでの判断手法は、上記のＳ１５５と同様である。異常検知部３２は、移動体２の周囲に反射板５が３つ以上存在しないと判断したときは、３つ以上の反射板５が存在しないと判断されてから規定時間が経過したかどうかを判断する（手順Ｓ１７５）。

異常検知部３２は、規定時間が経過したと判断したときは、ＳＬＡＭ自己位置推定部１０及び３点測量自己位置推定部１４による移動体２の自己位置推定が不可能であるとして、第１自己位置推定値及び第２自己位置推定値が異常であると検知する（手順Ｓ１７６）。

そして、異常検知部３２は、第１自己位置推定値及び第２自己位置推定値が異常である旨を報知信号として報知部２０に出力する（手順Ｓ１７７）。また、異常検知部３２は、移動体２の走行を強制的に停止させるように駆動部７を制御する（手順Ｓ１７８）。

異常検知部３２は、手順Ｓ１７２で移動体２の自己位置の推定信頼度が高いと判断したとき、手順Ｓ１７４で移動体２の周囲に反射板５が３つ以上存在すると判断したとき、手順Ｓ１７５で規定時間が経過していないと判断したときは、手順Ｓ１７６～Ｓ１７８を実行しない。

図１６は、３点測量モードからＳＬＡＭモードに移行する様子を示すタイミングチャートである。図１６において、時間ｔ１では、移動体２の自己位置の推定信頼度が高いため、３点測量自己位置推定部１４による移動体２の自己位置推定が可能であっても、ＳＬＡＭモードが実施され、ＳＬＡＭ自己位置推定部１０により得られた第１自己位置推定値が採用される。

その後の時間ｔ２では、３点測量自己位置推定部１４による移動体２の自己位置推定が可能である状態で、移動体２の自己位置の推定信頼度が低くなるため、ＳＬＡＭモードから３点測量モードに移行し、３点測量自己位置推定部１４により得られた第２自己位置推定値が採用される。

その後の時間ｔ３では、移動体２の自己位置の推定信頼度が高くなっても、第１自己位置推定値と第２自己位置推定値との距離が閾値Ｓ以下でないため、３点測量モードが継続して実施される。

その後の時間ｔ４では、第１自己位置推定値と第２自己位置推定値との距離が閾値Ｓ以下となったが、両者の距離が閾値Ｓ以下となってから規定時間Ｔが経過していないため、３点測量モードが継続して実施される。

その後の時間ｔ５では、第１自己位置推定値と第２自己位置推定値との距離が閾値Ｓ以下となってから、規定時間Ｔが経過したため、３点測量モードからＳＬＡＭモードに移行し、ＳＬＡＭ自己位置推定部１０により得られた第１自己位置推定値が採用される。

図１７は、第１自己位置推定値及び第２自己位置推定値が異常であると検知される様子を示すタイミングチャートである。図１７において、時間ｔ１では、移動体２の自己位置の推定信頼度が高いため、３点測量自己位置推定部１４による移動体２の自己位置推定が可能であっても、ＳＬＡＭモードが実施され、ＳＬＡＭ自己位置推定部１０により得られた第１自己位置推定値が採用される。

その後の時間ｔ３では、３点測量自己位置推定部１４による移動体２の自己位置推定が不可能になる。このため、直前に推定または補完された第２自己位置推定値が第１自己位置推定値として再設定される。

その後の時間ｔ４では、３点測量自己位置推定部１４による移動体２の自己位置推定が不可能になってから規定時間Ｔが経過したため、第１自己位置推定値及び第２自己位置推定値が異常であると検知され、報知部２０によって異常発報される。

以上のような本実施形態では、ＳＬＡＭモードの実施時に、移動体２の自己位置の推定信頼度が低くなっても、移動体２の周囲に反射板５が３つ以上存在すると判断されたときは、３点測量自己位置推定部１４による移動体２の自己位置推定が可能である。このため、ＳＬＡＭモードから３点測量モードに移行するようになる。従って、移動体２の自己位置推定の信頼性が確保される。

また、本実施形態では、３点測量モードの実施時に、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が高いと共に、第１自己位置推定値と第２自己位置推定値との距離が閾値以下であるときは、３点測量モードからＳＬＡＭモードに移行する。従って、移動体２の自己位置推定の信頼性が更に確保される。

また、本実施形態では、移動体２の自己位置の推定信頼度が高いと共に第１自己位置推定値と第２自己位置推定値との距離が閾値以下である状態が規定時間だけ継続したときに、３点測量モードからＳＬＡＭモードに移行する。このため、３点測量モードからＳＬＡＭモードへの移行時に、ノイズ等の影響があっても、移動体２の自己位置推定の信頼性が確保される。

また、本実施形態では、移動体２の自己位置の推定信頼度が低いと判定されると共に、移動体２の周囲に反射板５が３つ以上存在しないと判断されたときに、第１自己位置推定値及び第２自己位置推定値が異常であると検知される。このため、第１自己位置推定値及び第２自己位置推定値が異常である旨を作業者に知らせたり、移動体２の走行を強制的に停止させることができる。

また、本実施形態では、移動体２の自己位置の推定信頼度が低いと判定されると共に移動体２の周囲に反射板５が３つ以上存在しないと判断された状態が規定時間だけ継続したときに、第１自己位置推定値及び第２自己位置推定値が異常であると検知される。このため、ノイズ等の影響があっても、第１自己位置推定値及び第２自己位置推定値が異常であることを精度良く検知することができる。

また、本実施形態では、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が低い状態で、３点測量自己位置推定部１４による移動体２の自己位置推定が不可能になると、直前に推定または補完された第２自己位置推定値が第１自己位置推定値として再設定される。このため、ＳＬＡＭ自己位置推定部１０により推定される移動体２の自己位置が適切な値に初期化されることになる。従って、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定精度が高くなる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が高いと判定されたときは、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置が採用され、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が低いと判定されたときは、３点測量自己位置推定部１４により推定された移動体２の自己位置が採用されているが、特にそのような形態には限られない。例えば、輝度ランドマークとして登録済の３つ以上の反射板５が設置されている環境下では、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度に関わらず、３点測量自己位置推定部１４により推定された移動体２の自己位置を採用してもよい。

また、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置と３点測量自己位置推定部１４により推定された移動体２の自己位置との平均値を、最終的な移動体２の自己位置として採用してもよい。

また、上記実施形態では、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が高いと判定されたときは、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置座標が輝度ランドマークとして新たに登録され、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が低いと判定されたときは、移動体２の周囲に輝度ランドマークとして登録済の反射板５が３つ以上存在する場合に、３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置座標が輝度ランドマークとして新たに登録されているが、特にそのような形態には限られない。例えば、移動体２の周囲に輝度ランドマークとして登録済の反射板５が３つ以上存在していれば、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度に関わらず、３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置座標を輝度ランドマークとして新たに登録してもよい。

また、上記実施形態では、オドメトリ自己位置推定部１１によって、エンコーダ４の計測値を用いて移動体２の自己位置が推定され、その移動体２の自己位置とＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置とが比較されることで、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が判定されているが、特にそのような形態には限られない。例えば、エンコーダ４に代えて、移動体２の角速度及び加速度を計測する慣性計測ユニットを使用し、慣性計測ユニットの計測値を用いて移動体２の自己位置を推定し、その移動体２の自己位置とＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置とを比較することで、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度を判定してもよい。また、路面にＡＲマーカまたはバーコードを設置し、ＡＲマーカの撮像画像またはバーコードの読込データを用いて移動体２の自己位置を推定し、その移動体２の自己位置とＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置とを比較することで、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度を判定してもよい。

また、上記実施形態では、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定信頼度が判定されているが、複数の反射板５が設置されている環境下では、特にそのような形態には限られない。例えば、移動体２の周囲に反射板５が３つ以上存在するときには、上記の第２実施形態のように、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置と３点測量自己位置推定部１４により推定された移動体２の自己位置とを比較してもよい。この場合、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置と３点測量自己位置推定部１４により推定された移動体２の自己位置とのずれ量が規定値よりも少ないときは、ＳＬＡＭ自己位置推定部１０により推定された移動体２の自己位置を優先して採用してもよい。

また、上記実施形態では、複数の反射板５が設置されている環境下において、輝度ランドマークとして登録された３つの反射板５の位置座標を用いた３点測量法によって、移動体２の自己位置が推定されているが、特に３点測量法には限られず、輝度ランドマークとして登録された２つの反射板５の位置座標を用いて、移動体２の自己位置を推定してもよい。

具体的には、図１８に示されるように、移動体２におけるレーザセンサ３の搭載位置と輝度ランドマークＭとして登録された２つの反射板５の位置座標とが予め分かっている。従って、レーザセンサ３によって反射板５までの距離と反射板５でのレーザ反射点Ｒ_Ｌとを検出すると共に、エンコーダ４等を用いて移動体２の移動量を検出することにより、移動体２の自己位置を推定することができる。

また、上記実施形態では、ＳＬＡＭ自己位置推定部１０または３点測量自己位置推定部１４により推定された移動体２の自己位置に基づいて、未登録の反射板５の位置情報が輝度ランドマークとして自動的に登録されているが、特にそのような形態には限られない。例えば、複数の反射板５が走行路９に沿って設置されたときに、作業者が各反射板５の設置箇所の位置情報を設定登録してもよい。

また、上記実施形態では、移動体２が走行する走行路９の左右両側に物体８としての壁が設置された環境となっているが、本発明は、例えば走行路９の左右片側に物体８としての壁が設置されることで同じ風景が続くような環境下でも適用可能である。また、本発明は、例えば荷物等の特徴物が変化することで、ＳＬＡＭ自己位置推定部１０による移動体２の自己位置の推定精度が低下するような環境下でも適用可能である。

また、上記実施形態は、移動体２を目的地まで自動的に誘導走行させる走行制御システム１であるが、本発明は、特にその形態には限られず、例えば移動体２を対象物に追尾して走行させるシステム等、移動体２の自己位置を推定する必要があれば適用可能である。

２…移動体、３…レーザセンサ、５…反射板（反射部材）、８…物体、９…走行路、１０…ＳＬＡＭ自己位置推定部（第１自己位置推定部）、１２…信頼度判定部、１３…ランドマーク登録部、１４…３点測量自己位置推定部（第２自己位置推定部）、１５…自己位置決定部、１７…位置推定システム、２２…異常検知部、２３…自己位置補正部、３１…自己位置決定部、３２…異常検知部、３３…自己位置補正部、Ｌ…レーザ、Ｍ…輝度ランドマーク、Ｓ１…第１自己位置推定値、Ｓ２…第２自己位置推定値。

Claims

移動体から周囲にレーザを照射し、前記レーザの反射光を受光することにより、前記移動体の周囲に存在する物体までの距離を検出するレーザセンサと、
前記レーザセンサにより検出された前記物体までの距離データと前記移動体の周囲環境の地図データとを用いて、前記移動体の自己位置を推定する第１自己位置推定部と、
前記移動体が走行する走行路に沿って設置され、前記レーザセンサから照射される前記レーザを前記物体よりも高い輝度で反射させる複数の反射部材と、
前記複数の反射部材のうち少なくとも２つの反射部材の位置情報を用いて、前記移動体の自己位置を推定する第２自己位置推定部と、
前記第１自己位置推定部による前記移動体の自己位置の推定信頼度を判定する信頼度判定部と、
前記信頼度判定部により判定された前記第１自己位置推定部による前記移動体の自己位置の推定信頼度に基づいて、前記移動体の自己位置を決定する自己位置決定部と、
前記レーザセンサの検出データに基づいて、前記移動体の周囲に前記反射部材が存在するかどうかを判断し、前記移動体の周囲に未登録の反射部材が存在するときに、前記未登録の反射部材の位置情報を輝度ランドマークとして登録するランドマーク登録部とを備え、
前記自己位置決定部は、前記信頼度判定部により前記移動体の自己位置の推定信頼度が高いと判定されたときは、前記第１自己位置推定部により推定された前記移動体の自己位置である第１自己位置推定値を前記移動体の自己位置として決定する第１推定モードを実施し、前記信頼度判定部により前記移動体の自己位置の推定信頼度が低いと判定されたときは、前記第２自己位置推定部により推定された前記移動体の自己位置である第２自己位置推定値を前記移動体の自己位置として決定する第２推定モードを実施し、
前記ランドマーク登録部は、前記信頼度判定部により前記移動体の自己位置の推定信頼度が高いと判定されたときは、前記第１自己位置推定値に基づいて、前記未登録の反射部材の位置情報を前記輝度ランドマークとして登録し、前記信頼度判定部により前記移動体の自己位置の推定信頼度が低いと判定されると共に、前記移動体の周囲に前記輝度ランドマークとして登録済の反射部材が少なくとも２つ存在するときは、前記第２自己位置推定値に基づいて、前記未登録の反射部材の位置情報を前記輝度ランドマークとして登録する位置推定システム。
移動体から周囲にレーザを照射し、前記レーザの反射光を受光することにより、前記移動体の周囲に存在する物体までの距離を検出するレーザセンサと、
前記レーザセンサにより検出された前記物体までの距離データと前記移動体の周囲環境の地図データとを用いて、前記移動体の自己位置を推定する第１自己位置推定部と、
前記移動体が走行する走行路に沿って設置され、前記レーザセンサから照射される前記レーザを前記物体よりも高い輝度で反射させる複数の反射部材と、
前記複数の反射部材のうち少なくとも２つの反射部材の位置情報を用いて、前記移動体の自己位置を推定する第２自己位置推定部と、
前記第１自己位置推定部による前記移動体の自己位置の推定信頼度を判定する信頼度判定部と、
前記信頼度判定部により判定された前記第１自己位置推定部による前記移動体の自己位置の推定信頼度に基づいて、前記移動体の自己位置を決定する自己位置決定部と、
前記レーザセンサの検出データに基づいて、前記移動体の周囲に前記反射部材が存在するかどうかを判断し、前記移動体の周囲に未登録の反射部材が存在するときに、前記未登録の反射部材の位置情報を輝度ランドマークとして登録するランドマーク登録部とを備え、
前記自己位置決定部は、前記信頼度判定部により前記移動体の自己位置の推定信頼度が高いと判定されたときは、前記第１自己位置推定部により推定された前記移動体の自己位置である第１自己位置推定値を前記移動体の自己位置として決定する第１推定モードを実施し、前記信頼度判定部により前記移動体の自己位置の推定信頼度が低いと判定されたときは、前記第２自己位置推定部により推定された前記移動体の自己位置である第２自己位置推定値を前記移動体の自己位置として決定する第２推定モードを実施し、
前記ランドマーク登録部は、前記レーザセンサの検出データに基づいて、前記輝度ランドマークとして登録済の反射部材からの前記レーザの反射光を受光しなくなったかどうかを判断し、前記登録済の反射部材からの前記レーザの反射光を受光しなくなったときは、前記登録済の反射部材に対応する輝度ランドマークを削除する位置推定システム。
前記第１自己位置推定値と前記第２自己位置推定値との距離が予め決められた規定値以上であるときに、前記第１自己位置推定値が異常であると検知する異常検知部を更に備える請求項１または２記載の位置推定システム。
前記異常検知部により前記第１自己位置推定値が異常であると検知されたときに、前記第２自己位置推定値を前記第１自己位置推定値として再設定する自己位置補正部を更に備える請求項３記載の位置推定システム。
前記自己位置決定部は、前記第１推定モードの実施時に、前記信頼度判定部により前記移動体の自己位置の推定信頼度が低いと判定されると共に、前記レーザセンサの検出データに基づいて、前記移動体の周囲に前記反射部材が少なくとも２つ存在すると判断されたときは、前記第１推定モードから前記第２推定モードに移行する請求項１または２記載の位置推定システム。
前記自己位置決定部は、前記第２推定モードの実施時に、前記信頼度判定部により前記移動体の自己位置の推定信頼度が高いと判定されると共に、前記第１自己位置推定値と前記第２自己位置推定値との距離が予め決められた閾値以下であるときは、前記第２推定モードから前記第１推定モードに移行する請求項１～５の何れか一項記載の位置推定システム。
前記自己位置決定部は、前記第２推定モードの実施時に、前記移動体の自己位置の推定信頼度が高いと判定されると共に前記第１自己位置推定値と前記第２自己位置推定値との距離が前記閾値以下である状態が規定時間だけ継続したときに、前記第２推定モードから前記第１推定モードに移行する請求項６記載の位置推定システム。
前記信頼度判定部により前記移動体の自己位置の推定信頼度が低いと判定されると共に、前記レーザセンサの検出データに基づいて、前記移動体の周囲に前記反射部材が少なくとも２つ存在しないと判断されたときに、前記第１自己位置推定値及び前記第２自己位置推定値が異常であると検知する異常検知部を更に備える請求項５～７の何れか一項記載の位置推定システム。
前記異常検知部は、前記移動体の自己位置の推定信頼度が低いと判定されると共に前記移動体の周囲に前記反射部材が少なくとも２つ存在しないと判断される状態が規定時間だけ継続したときに、前記第１自己位置推定値及び前記第２自己位置推定値が異常であると検知する請求項８記載の位置推定システム。
前記信頼度判定部により前記移動体の自己位置の推定信頼度が低いと判定されると共に、前記レーザセンサの検出データに基づいて、前記移動体の周囲に前記反射部材が少なくとも２つ存在しないと判断されたときに、直前に推定または補完された前記第２自己位置推定値を前記第１自己位置推定値として再設定する自己位置補正部を更に備える請求項５～９の何れか一項記載の位置推定システム。