JP7347226B2 - 自己位置推定装置、及び自己位置推定方法 - Google Patents

Description

本発明は、自己位置推定装置、及び自己位置推定方法に関する。

移動体の自己位置を推定する自己位置推定装置として、例えば特許文献１に記載されているような技術が知られている。特許文献１に記載の自己位置推定装置は、移動体の前側に設けられたセンサを用いて周囲の環境を検出し、当該検出結果と地図データとを比較することで自己位置を推定している。自己位置推定装置は、上述のような自己位置推定を行いながら、当該自己位置に基づき、予め設定した走行経路上を移動体が走行するように制御を行っている。

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

ここで、移動体が走行する経路の周囲の環境は、必ずしも一定ではない。従って、地図データ作成時と、自己位置推定装置による自己位置推定時とで、移動体の周囲の環境が変化する場合がある。上述の自己位置推定装置は、移動体の前方の環境のみを検出しているため、当該移動体の前方の環境が変化した場合、自己位置推定に失敗してしまう場合がある。これに対し、移動体に多数の検出部を設けることで、移動体の周囲の検出範囲を拡大し、検出結果と地図データとのマッチング度を向上させる方法が挙げられる。しかし、当該方法は、必要な検出部の数が増加してしまうという問題を有する。その他、検出結果及び地図データに基づく自己位置推定に失敗した場合に、エンコーダや慣性センサなどで取得される走行情報に基づく自己位置推定（デッドレコニング）の結果を用いて移動体を自律走行させる方法が挙げられる。しかしながら、当該自己位置推定方法は、周囲の環境を考慮しない方法であるため、長距離の自律走行を行うと、自己位置の誤差が大きくなってしまう。その結果、自己位置推定装置の自己位置推定の性能が低下してしまうという問題が生じる。

本発明の目的は、検出部の数を抑制しつつ、自己位置推定の性能を向上できる自己位置推定装置、及び自己位置推定方法を提供することである。

本発明の一態様に係る自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置において、移動体の周囲の検出範囲における環境の状態を検出する検出部と、検出部による検出結果、及び予め取得された地図データに基づいて、移動体の自己位置を推定する自己位置推定部と、を備え、自己位置推定部は、検出結果及び地図データに基づく第１の自己位置推定に失敗したと判定した場合、移動体の走行情報に基づく第２の自己位置推定を行い、第２の自己位置推定に基づいて移動体が所定の距離を走行しても、第１の自己位置推定に失敗したと判定した場合、検出部の検出範囲を走行時からずらした前記検出結果に基づいて前記第１の自己位置推定を行う。

このような自己位置推定装置においては、自己位置推定部は、移動体の周囲の環境の状態の検出結果、及び予め取得された地図データに基づいて、移動体の自己位置を推定する。従って、環境が変化することで、検出範囲の環境の状態が、地図データを取得したときと異なる場合がある。従って、自己位置推定部は、検出結果及び地図データに基づく第１の自己位置推定に失敗する場合がある。これに対し、自己位置推定部は、検出結果及び地図データに基づく第１の自己位置推定に失敗したと判定した場合、移動体の走行情報に基づく第２の自己位置推定を行う。これにより、移動体は、第１の自己位置推定を行えない状態でも、第２の自己位置推定に基づく自律走行を行うことができる。これにより、移動体が第１の自己位置推定が可能な位置まで到達することができれば、移動体は、精度の良い第１の自己位置推定に基づく自律走行に復帰することができる。一方、移動体が第２の自己位置推定に基づく自律走行を行っても、第１の自己位置推定が可能な位置まで到達できない状況もある。当該状況に対し、自己位置推定部は、第２の自己位置推定に基づいて移動体が所定の距離を走行しても、第１の自己位置推定に失敗したと判定した場合、検出部の検出範囲を走行時からずらした検出結果に基づいて第１の自己位置推定を行う。検出範囲を走行時からずらすことで、検出部の数を増やさなくとも、検出部が移動体の周囲の環境を広範囲にわたって検出することが可能となる。当該タイミングで自己位置推定部が第１の自己位置推定に成功した場合、第２の自己位置推定に基づく自律走行によって蓄積された自己位置の誤差を解消することができる。その結果、自己位置推定部は自己位置推定の精度の低下を抑制しつつ、第２の自己位置推定による自律走行の距離を伸ばすことができる。すなわち、自己位置推定装置は、第１の自己位置推定が可能な位置まで、移動体２が到達できる可能性を高めることができる。以上より、検出部の数を抑制しつつ、自己位置推定の性能を向上できる。

自己位置推定部は、移動体の旋回に伴って検出範囲をずらすように制御してよい。この場合、検出範囲をずらすための特別な回転機構などを追加することを不要とすることができる。

自己位置推定部は、検出範囲をずらしている途中で第１の自己位置推定を行い、第１の自己位置推定が成功した時点で、検出範囲をずらすことを停止してよい。この場合、検出範囲を３６０°回転させるまで待たずに、移動体が速やかに自律走行に復帰することができる。

自己位置推定部は、検出範囲が移動体の周囲を３６０°回転しても第１の自己位置推定が成功しない場合、移動体の停止を行ってよい。このように、検出部が移動体の周囲の環境を全周にわたって検出しても、自己位置推定部が第１の自己位置推定を行えない場合、異常停止することで、当該状況に対する処置を行うことができる。

本発明の一態様に係る自己位置推定方法は、移動体の自己位置を推定する自己位置推定方法において、移動体の周囲の検出範囲における環境の状態を検出する検出ステップと、検出ステップによる検出結果、及び予め取得された地図データに基づいて、移動体の自己位置を推定する自己位置推定ステップと、を備え、自己位置推定ステップは、検出結果及び地図データに基づく第１の自己位置推定に失敗したと判定した場合、移動体の走行情報に基づく第２の自己位置推定を行う第１のステップと、第２の自己位置推定に基づいて移動体が所定の距離を走行しても、第１の自己位置推定に失敗したと判定した場合、検出ステップでの検出範囲を走行時からずらした検出結果に基づいて第１の自己位置推定を行う第２のステップと、を有する。

このような自己位置推定方法によれば、上述の自己位置推定装置と同趣旨の作用・効果を得ることができる。

本発明によれば、検出部の数を抑制しつつ、自己位置推定の性能を向上できる自己位置推定装置、及び自己位置推定方法を提供することができる。

本発明の一実施形態に係る自己位置推定装置を備える走行制御装置を示す概略構成図である。 レーザセンサの移動体における取付位置を示す概略図である。 自己位置推定部による処理内容を示すフローチャートである。 移動体の挙動を示す概念図である。 移動体の挙動を示す概念図である。 比較例に係る自己位置推定装置を備える移動体の挙動を示す概念図である。 変形例に係る自己位置推定装置を備える移動体を示す概略図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。

図１は、本発明の一実施形態に係る自己位置推定装置１００を備える走行制御装置１を示す概略構成図である。走行制御装置１は、移動体２を自動的に走行させる装置である。図２は、レーザセンサ３の移動体２における取付位置を示す概略図である。移動体２は、例えばフォークリフト、無人搬送車などである。走行制御装置１は、移動体２に搭載されている。

走行制御装置１は、レーザセンサ３（検出部）と、駆動部４と、走行状態取得センサ６と、コントローラ５とを備えている。

レーザセンサ３は、移動体２の周囲の検出範囲における環境の状態を検出する。具体的に、レーザセンサ３は、移動体２の周囲の物体を検出し、検出した物体との位置関係を計測する。レーザセンサ３は、移動体２の周囲にレーザを照射し、レーザの反射光を受光することにより、レーザセンサ３から移動体２の周囲の物体を検出し、当該物体までの距離を計測する。これにより、レーザセンサ３は、移動体２周囲の検出範囲ＤＥ内（図２参照）に存在する物体を検出し、検出した物体との距離を測定できる。レーザセンサ３としては、例えばレーザレンジファインダが用いられる。使用するレーザとしては、２Ｄレーザでもよいし、３Ｄレーザでもよい。

ここで、図２に示すように、レーザセンサ３は、移動体２の車体の所定箇所に設けられている。ここでは、レーザセンサ３は、移動体２の前部に設けられている。レーザセンサ３の検出範囲ＤＥは、レーザセンサ３から移動体２の前側へ向かって延びる基準軸ＳＬを基準として左右に所定の角度をなす範囲に設定される。また、検出範囲ＤＥは、レーザセンサ３を基準として一定の距離をなす範囲に設定される。従って、検出範囲ＤＥは、レーザセンサ３を中心とする扇状をなす。

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

走行状態取得センサ６は、移動体２の走行状態に関する走行情報を取得する。走行状態取得センサ６は、駆動部４に設けられて走行輪などに設けられたエンコーダ、及び慣性センサなどによって構成される。走行状態取得センサ６は、取得した検出結果をコントローラ５へ送信する。

コントローラ５は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ５は、経路設定部１０と、自己位置推定部１１と、走行制御部１２と、記憶部１３と、を有している。

経路設定部１０は、移動体２が走行するための経路を設定する。経路設定部１０は、スタート地点とゴール地点を取得し、当該スタート地点とゴール地点との間において、移動体２が走行する経路を設定する。経路設定部１０は、経路設定時には、地図データを読み出し、地図データ中のスタート地点とゴール地点との間に、どのような物体が存在しているかを把握した上で、移動体２がこれらの物体と干渉せずに速やかにゴール地点まで到達できる経路を設定する。

自己位置推定部１１は、レーザセンサ３による検出結果、及び予め取得された地図データに基づいて、移動体２の自己位置を推定する。自己位置推定部１１は、例えばＳＬＡＭ（simultaneous localization andmapping）手法を用いて、移動体２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。ＳＬＡＭは、自己位置推定と環境地図の作成とを同時に行う。具体的には、自己位置推定部１１は、レーザセンサ３により検出された物体までの距離データと移動体２の周囲環境の地図データとをマッチングさせて、移動体２の自己位置の推定演算を行う。更に、自己位置推定部１１は、補助的な自己位置推定として、走行状態取得センサ６からの走行情報に基づく相対的な自己位置推定（デッドレコニング）を行う。なお、自己位置推定装置１００は、自己位置推定部１１と、上述のレーザセンサ３と、を備えて構成される。自己位置推定部１１は、情報取得部１４と、演算部１６と、デッドレコニング部１７を備える。

情報取得部１４は、自己位置の推定に必要な各種情報を所得する。情報取得部１４は、レーザセンサ３による検出結果を取得する。情報取得部１４は、自己位置推定のために必要な地図データを記憶部１３から読み出す。情報取得部１４は、走行状態取得センサ６から移動体２の走行情報を取得する。また、情報取得部１４は、経路設定部１０が設定した経路も取得する。

演算部１６は、レーザセンサ３の検出結果から、移動体２の自己位置を推定するための演算を行う。演算部１６は、レーザセンサ３で検出された周囲の物体、及び地図データに基づいて、移動体２の自己位置の推定演算を行う。演算部１６による自己位置推定は、ＳＬＡＭ手法を用いて行われてよい。演算部１６は、移動体２の周囲の物体を検出し、検出した物体と移動体２との間の距離データと、移動体周囲環境の地図データとをマッチングすることで自己位置を推定してよい。例えば、演算部１６は、図４（ａ）～（ｃ）に示すような移動体２の周囲の環境の状態と、地図データが含む環境の状態（図４（ａ）～（ｃ）の壁部５１の手前から荷物５２を取り除いた状態）とを比較することで自己位置を推定する。なお、以降の説明では、レーザセンサ３の検出結果及び地図データに基づく自己位置推定を「第１の自己位置推定」と称する場合がある。

デッドレコニング部１７は、走行状態取得センサ６で取得された走行情報に基づいて、移動体２の自己位置推定を行う。デッドレコニング部１７は、エンコーダ値や慣性センサ値などの走行情報に基づき、移動体２が所定位置からどの程度の距離をどの方向へ走行したかを把握することで、自己位置推定を行う。なお、当該自己位置推定方法は、移動体２の周辺環境を考慮することなく、移動体２の相対的な位置を推定する方法であるため、走行距離が長くなるほど（例えば走行輪のスリップなどにより）自己位置推定の誤差が蓄積される。なお、以降の説明では、走行情報に基づく自己位置推定を「第２の自己位置推定」と称する場合がある。

ここで、演算部１６は、第１の自己位置推定が成功したか失敗したかの判定を行う。例えば、演算部１６は、レーザセンサ３で検出された物体と地図データに登録された物体とのマッチング度合いなどを演算し、当該マッチング度が所定の閾値を超えているか否かに基づいて、判定を行う。演算部１６は、マッチング度が閾値を超えていれば第１の自己位置推定が成功したと判定し、マッチング度が閾値以下であれば、第１の自己位置推定が失敗したと判定する。

デッドレコニング部１７は、演算部１６が第１の自己位置推定が失敗したと判定した場合、第２の自己位置推定を行う。また、演算部１６は、第２の自己位置推定に基づいて移動体２が所定の距離を走行しても、第１の自己位置推定に失敗したと判定した場合、レーザセンサ３の検出範囲ＤＥを走行時からずらすように制御を行い、レーザセンサ３の検出範囲ＤＥをずらした検出結果に基づいて第１の自己位置推定を行う。

演算部１６が、検出範囲ＤＥを走行時からずらすように制御を行う内容について説明する。検出範囲ＤＥを走行時からずらす事とは、検出範囲ＤＥの基準軸ＳＬの向きを、走行時における検出範囲ＤＥの基準軸ＳＬとは異なるようにすることである。具体的には、演算部１６は、図４（ｃ）に示すように、移動体２の旋回に伴って検出範囲ＤＥをずらすように制御する。移動体２が旋回を行うと、レーザセンサ３の基準軸ＳＬの向きが、走行時のもの（図４（ａ）（ｂ）のもの）から変わる。従って、演算部１６は、走行制御部１２に制御信号を送信して移動体２を旋回させることで、検出範囲ＤＥをずらす。

演算部１６は、レーザセンサ３が検出範囲ＤＥをずらしている途中で第１の自己位置推定を行う。演算部１６は、第１の自己位置推定が成功した時点で、検出範囲ＤＥをずらすことを停止する。演算部１６は、検出範囲ＤＥが移動体２の周囲を３６０°回転しても自己位置を推定できない場合、移動体２の停止を行う。

走行制御部１２は、自己位置推定部１１により推定された移動体２の自己位置に基づいて、移動体２の走行を制御する。走行制御部１２は、自己位置とゴール地点の位置とを比較することによって、移動体２の移動方向や移動距離を演算する。また、走行制御部１２は、当該演算結果に基づいて、移動体２がゴール地点へ向かうように駆動部４へ制御信号を出力する。記憶部１３は、各種データを記憶する。記憶部１３は、地図データを記憶する。

次に、図３～図５を参照して、自己位置推定部１１によって実行される処理内容について説明する。図３は、自己位置推定部１１による処理内容を示すフローチャートである。なお、図３の各ステップは、特段の注意書きが無くとも、請求項における「自己位置推定ステップ」に属するものとする。図４及び図５は、移動体２の挙動を示す概念図である。図４及び図５は、移動体２が一対の壁部５１間の通路５３を走行している時の様子を示す。壁部５１の手前には、地図データ作成時には存在していない荷物５２が多数積まれている。

図３に示すように、自己位置推定部１１は、レーザセンサ３から検出結果を取得すると共に、当該検出結果及び地図データに基づいて移動体２の第１の自己位置推定を行う（ステップＳ１００：検出ステップ、自己位置推定ステップ）。次に、自己位置推定部１１は、第１の自己位置推定が成功したか否かを判定する（ステップＳ１１０）。ステップＳ１１０において第１の自己位置推定が成功したと判定された場合、自己位置推定部１１は、第１の自己位置推定に基づく自律走行を行うように制御を行う（ステップＳ１１５）。これにより、図３に示す処理が終了し、再びステップＳ１００から処理が繰り返される。

一方、自己位置推定部１１は、ステップＳ１１０において第１の自己位置推定が失敗したと判定した場合、移動体２の走行情報に基づく第２の自己位置推定を行い、当該第２の自己位置推定の結果に基づいて移動体２の自律走行を実行する（ステップＳ１２０：第１のステップ）。例えば、図４（ａ）に示すように、検出範囲ＤＥ内に荷物５２が増えることによって、検出された環境の状態が地図データから変化した場合、自己位置推定部１１は、第１の自己位置推定が失敗したと判定する。この場合、図４（ｂ）に示すように、自己位置推定部１１は、デッドレコニングによる自律走行で移動体２が荷物５２の多い領域を抜けるように、第２の自己位置推定を実行する。

次に、図３に示すように、自己位置推定部１１は、第２の自己位置推定に基づきデッドレコニングによる自律走行を行っている最中も、第１の自己位置推定を行い、第１の自己位置推定が失敗したか否かを判定する（ステップＳ１３０）。自己位置推定部１１は、ステップＳ１３０において、第１の自己位置推定が成功したと判定した場合、第１の自己位置推定結果に基づく自律走行を行うように制御を行う（ステップＳ１１５）。これにより、図３に示す処理を終了し、再びステップＳ１００から処理が繰り返される。例えば、荷物５２の多い領域が小さい場合、移動体２は第１の自己位置推定に失敗しても、第２の自己位置推定に基づきデッドレコニングによる自律走行をすれば、速やかに第１の自己位置推定による自律走行に復帰することができる。

一方、自己位置推定部１１は、ステップＳ１３０において第１の自己位置推定が失敗したと判定した場合、デッドレコニングで一定距離を走行したか否かを判定する（ステップＳ１４０）。一定距離は、第２の自己位置推定による自己位置推定の誤差の大きさに基づいて、予め設定される。自己位置推定部１１は、移動体２がデッドレコニングにて一定距離を走行していないと判定した場合、ステップＳ１２０に戻り、デッドレコニングによる自律走行を継続する。

一方、自己位置推定部１１は、ステップＳ１４０において、移動体２がデッドレコニングにて一定距離を走行したと判定した場合、移動体２がその場で旋回するように制御を行う（ステップＳ１５０）。これにより、移動体２が通路５３を進行している状態（図４（ａ）（ｂ）参照）に対し、レーザセンサ３の検出範囲ＤＥがずれる（図４（ｃ）参照）。自己位置推定部１１は、検出範囲ＤＥをずらしている途中で第１の自己位置推定を行い、第１の自己位置推定が失敗したか否かを判定する（ステップＳ１６０：第２のステップ）。例えば、図４（ｃ）に示すように、検出範囲ＤＥが進行方向と反対側に向いたとき（検出範囲ＤＥが走行時から１８０°ずれた状態）に、当該検出範囲ＤＥの荷物５２が少ない場合、自己位置推定部１１は、第１の自己位置推定が成功したと判定する。

図３に戻り、自己位置推定部１１は、ステップＳ１６０において第１の自己位置推定が成功したと判定した場合、第１の自己位置推定の推定結果により、自己位置を補正する（ステップＳ１８０）。これにより、自己位置推定部１１は、デッドレコニングによる自律走行で蓄積された自己位置の誤差を解消することができる。次に、自己位置推定部１１は、検出範囲ＤＥが進行方向を向くように、その場で旋回するように制御を行う（ステップＳ１９０）。例えば、図４（ｃ）に示すように検出範囲ＤＥが進行方向と反対側を向いた状態から、図５（ａ）に示すように検出範囲ＤＥが進行方向を向くように、移動体２がその場で旋回する。自己位置推定部１１は、検出範囲ＤＥが進行方向を向いたら、ステップＳ１２０に戻り、再びデッドレコニングによる自律走行を繰り返す。例えば、図５（ｂ）に示すように、移動体２がデッドレコニングによる自律走行を行うことで荷物５２が多い領域を抜けた場合、ステップＳ１３０において自己位置推定部１１が第１の自己位置推定が成功したと判定するため、図５（ｃ）に示すように移動体２が第１の自己位置推定の結果に基づく自律走行を再開する。

自己位置推定部１１は、ステップＳ１６０において第１の自己位置推定が失敗したと判定した場合、レーザセンサ３が移動体２の周囲を３６０°回転したか否かを判定する（ステップＳ１７０）。自己位置推定部１１は、ステップＳ１７０においてレーザセンサ３が移動体２の周囲を３６０°回転していないと判定した場合、ステップＳ１５０に戻り、移動体２によるその場での旋回を継続するように制御する。一方、自己位置推定部１１は、レーザセンサ３が移動体２の周囲を３６０°回転したと判定した場合、第１の自己位置推定が成功しないと判断する。この場合、自己位置推定部１１は、移動体２の停止を行う（ステップＳ２００）。ステップＳ２００の後、所定の処置（例えば、地図データの更新、荷物５２の除去などの処置）を施した後、第１の自己位置推定が可能な状態にした後、再びステップＳ１００から処理が繰り返される。

次に、本実施形態に係る自己位置推定装置１００、及び自己位置推定方法の作用・効果について説明する。

例えば、比較例として、移動体２に多数のレーザセンサ３を設けた自己位置推定装置について説明する。例えば、比較例に係る自己位置推定装置は、移動体２の前側のレーザセンサ３のみならず、後側にもレーザセンサ３を有する。この場合、移動体２の周囲の検出範囲を拡大することで、検出結果と地図データとのマッチング度を向上させることはできるが、必要なレーザセンサ３の数が増加してしまうという問題が生じる。

これに対し、本実施形態に係る自己位置推定装置１００の自己位置推定部１１は、検出結果及び地図データに基づく第１の自己位置推定に失敗したと判定した場合（図４（ａ）参照）、移動体２の走行情報に基づく第２の自己位置推定を行う。これにより、移動体２は、第１の自己位置推定を行えない状態でも、第２の自己位置推定に基づく自律走行を行うことができる（図４（ｂ）参照）。これにより、移動体２が第１の自己位置推定が可能な位置まで到達することができれば、移動体２は、精度の良い第１の自己位置推定に基づく自律走行に復帰することができる。

ここで、他の比較例として、第１の自己位置推定を失敗したときに第２の自己位置推定に基づいて自律走行させるだけの自己位置推定装置について説明する。比較例に係る自己位置推定装置は、検出範囲ＤＥをずらすような処理を行わない。当該比較例に係る自己位置推定装置は、第１の自己位置推定が行えない場合（図６（ａ）参照）、第２の自己位置推定に基づいて自律走行を行わせる（図６（ｂ）参照）。しかしながら、当該自己位置推定方法は、周囲の環境を考慮しない方法であるため、長距離の自律走行を行うと、自己位置の誤差が大きくなってしまう。従って、移動体２が所定の距離を走行しても第１の自己位置推定を可能な位置まで到達できない場合（図６（ｃ）参照）、自己位置推定部は、周辺環境に基づく補正を行わずに走行を続けることでエンコーダ等の誤差が徐々に加算されることにより、誤った場所に自己位置があると推定してしまうことになり得る。その結果、比較例に係る自己位置推定装置は、図６に示すように第１の自己位置推定を行えない場所（通路５３に荷物５２が多く置かれているような場所）の距離が長くなる場合などに、自己位置推定の性能が低下してしまうという問題が生じる。

他の比較例が有する問題に対し、本実施形態に係る自己位置推定装置１００の自己位置推定部１１は、第２の自己位置推定に基づいて移動体２が所定の距離を走行しても、第１の自己位置推定に失敗したと判定した場合、レーザセンサ３の検出範囲ＤＥを走行時からずらした検出結果に基づいて第１の自己位置推定を行う（図４（ｃ）参照）。検出範囲ＤＥを走行時からずらすことで、レーザセンサ３の数を増やさなくとも、レーザセンサ３が移動体２の周囲の環境を広範囲にわたって検出することが可能となる。当該タイミングで自己位置推定部１１が第１の自己位置推定に成功した場合、第２の自己位置推定に基づく自律走行によって蓄積された自己位置の誤差を解消することができる。従って、移動体２は、自己位置を見失うことなく、引き続き第２の自己位置推定による自律走行を継続できる（図５（ａ）（ｂ）参照）。その結果、自己位置推定部１１は自己位置推定の精度の低下を抑制しつつ、第２の自己位置推定による自律走行の距離を伸ばすことができる。これにより、自己位置推定装置１００は、第１の自己位置推定が可能な位置（図５（ｂ）参照）まで、移動体２が到達できる可能性を高めることができる。そして、移動体２は、精度のよい第１の自己位置推定に基づく自律走行に復帰することができる（図５（ｃ）参照）。以上より、レーザセンサ３の数を抑制しつつ、自己位置推定の性能を向上できる。

自己位置推定部１１は、移動体２の旋回に伴って検出範囲ＤＥをずらすように制御してよい。この場合、検出範囲ＤＥをずらすための特別な回転機構（例えば図７参照）などを追加することを不要とすることができる。

自己位置推定部１１は、検出範囲ＤＥをずらしている途中で第１の自己位置推定を行い、第１の自己位置推定が成功した時点で、検出範囲ＤＥをずらすことを停止してよい。この場合、検出範囲ＤＥを３６０°回転させるまで待たずに、移動体２が速やかに自律走行に復帰することができる。

自己位置推定部１１は、検出範囲ＤＥが移動体２の周囲を３６０°回転しても第１の自己位置推定が成功しない場合、移動体２の停止を行ってよい。このように、レーザセンサ３が移動体２の周囲の環境を全周にわたって検出しても、自己位置推定部１１が第１の自己位置推定を行えない場合、異常停止することで、当該状況に対する処置を行うことができる。

本実施形態に係る自己位置推定方法は、移動体２の自己位置を推定する自己位置推定方法において、移動体２の周囲の検出範囲ＤＥにおける環境の状態を検出する検出ステップと、検出ステップによる検出結果、及び予め取得された地図データに基づいて、移動体２の自己位置を推定する自己位置推定ステップと、を備え、自己位置推定ステップは、検出結果及び地図データに基づく第１の自己位置推定に失敗したと判定した場合、移動体２の走行情報に基づく第２の自己位置推定を行う第１のステップと、第２の自己位置推定に基づいて移動体２が所定の距離を走行しても、第１の自己位置推定に失敗したと判定した場合、検出ステップでの検出範囲を走行時からずらした検出結果に基づいて第１の自己位置推定を行う第２のステップと、を有する。

このような自己位置推定方法によれば、上述の自己位置推定装置１００と同趣旨の作用・効果を得ることができる。

本発明は、上述の実施形態に限定されるものではない。

例えば、図４や図５で示す作業現場の様子、検出される物体、経路などは一例にすぎず、作業現場に合わせて適宜変更されてよい。

上述の実施形態では、移動体２の周囲の物体を検出する検出部として２Ｄレーザ、３Ｄレーザなどのレーザセンサが例示されていたが、検出部の具体的構成は特に限定されるものではない。例えば、検出部としてカメラが採用され、ＶｉｓｕａｌＳＬＡＭ（カメラを用いた自己位置推定）の技術が用いられてもよい。カメラは、取得した画像の画像処理を行うことで、物体を検出すると共に、当該物体までの距離などの情報を検出することができる。例えば、画像中に写された物体のエッジを検出することで、物体の検出が可能である。その他、検出部は、超音波などのセンサによって構成されてもよい。

検出範囲ＤＥをずらす方法は、移動体２の旋回に限定されるものではない。例えば、図７に示すように、移動体２がレーザセンサ３を回転させる回転機構６０を有してもよい。回転機構６０は、通常時には移動体２の前側にレーザセンサ３を配置し、検出範囲ＤＥをずらす必要がある時は、レーザセンサ３の位置及び検出範囲ＤＥを移動体２周りに回転させる。このような回転機構６０は、移動体２が正面を向いたままで検出範囲ＤＥをずらすことができる。

なお、デッドレコニング部１７は、必ずしも自己位置推定部１１が第１の自己位置推定を失敗したときにのみ、第２の自己位置推定を行う必要はなく、第１の自己位置推定が成功している最中にも、補助的に第２の自己位置推定を行ってよい。

２…移動体、３…レーザセンサ（検出部）、１１…自己位置推定部、ＤＥ…検出範囲、１００…自己位置推定装置。

Claims (4)

1. 移動体の自己位置を推定する自己位置推定装置において、
   前記移動体の周囲の検出範囲における環境の状態を検出する検出部と、
   前記検出部による検出結果、及び予め取得された地図データに基づいて、前記移動体の自己位置を推定する自己位置推定部と、を備え、
   前記自己位置推定部は、
   前記検出結果及び前記地図データに基づく第１の自己位置推定に失敗したと判定した場合、前記移動体の走行情報に基づく第２の自己位置推定を行い、
   前記第２の自己位置推定に基づいて前記移動体が所定の距離を走行しても、前記第１の自己位置推定に失敗したと判定した場合、前記検出部の前記検出範囲を走行時からずらした前記検出結果に基づいて前記第１の自己位置推定を行い、
   前記自己位置推定部は、前記移動体の旋回に伴って前記検出範囲をずらすように制御し、
   前記自己位置推定部は、前記検出範囲をずらすことで前記第１の自己位置推定が成功した場合、前記第１の自己位置推定の推定結果で前記移動体の自己位置を補正し、前記検出範囲が進行方向を向くようにその場で旋回を行うように制御し、
   前記自己位置推定部は、前記第２の自己位置推定に基づいて走行を行うと共に第１の自己位置推定による自己位置の推定が可能か判断を行う、自己位置推定装置。
2. 前記自己位置推定部は、前記検出範囲をずらしている途中で前記第１の自己位置推定を行い、前記第１の自己位置推定が成功した時点で、前記検出範囲をずらすことを停止する、請求項１に記載の自己位置推定装置。
3. 前記自己位置推定部は、前記検出範囲が前記移動体の周囲を３６０°回転しても前記第１の自己位置推定が成功しない場合、前記移動体の停止を行う、請求項１又は２に記載の自己位置推定装置。
4. 移動体の自己位置を推定する自己位置推定方法において、
   前記移動体の周囲の検出範囲における環境の状態を検出する検出ステップと、
   前記検出ステップによる検出結果、及び予め取得された地図データに基づいて、前記移動体の自己位置を推定する自己位置推定ステップと、を備え、
   前記自己位置推定ステップは、
   前記検出結果及び前記地図データに基づく第１の自己位置推定に失敗したと判定した場合、前記移動体の走行情報に基づく第２の自己位置推定を行う第１のステップと、
   前記第２の自己位置推定に基づいて前記移動体が所定の距離を走行しても、前記第１の自己位置推定に失敗したと判定した場合、前記検出ステップでの前記検出範囲を走行時からずらした前記検出結果に基づいて前記第１の自己位置推定を行う第２のステップと、を有し、
   前記自己位置推定ステップにおいて、前記移動体の旋回に伴って前記検出範囲をずらすように制御し、
   前記自己位置推定ステップにおいて、前記検出範囲をずらすことで前記第１の自己位置推定が成功した場合、前記第１の自己位置推定の推定結果で前記移動体の自己位置を補正し、前記検出範囲が進行方向を向くようにその場で旋回を行うように制御し、
   前記自己位置推定ステップにおいて、前記第２の自己位置推定に基づいて走行を行うと共に第１の自己位置推定による自己位置の推定が可能か判断を行う、自己位置推定方法。
   

Images