特許 7696818 位置推定システム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2025-06-13

(45)【発行日】2025-06-23

(54)【発明の名称】位置推定システム

(51)【国際特許分類】

   G01C 21/30 20060101AFI20250616BHJP

   G05D 1/43 20240101ALI20250616BHJP

【ＦＩ】

G01C21/30

G05D1/43

【請求項の数】 6

(21)【出願番号】P 2021205826

(22)【出願日】2021-12-20

(65)【公開番号】P2023091205

(43)【公開日】2023-06-30

【審査請求日】2024-06-07

(73)【特許権者】

【識別番号】000005108

【氏名又は名称】株式会社日立製作所

(74)【代理人】

【識別番号】110000350

【氏名又は名称】ポレール弁理士法人

(72)【発明者】

【氏名】金子 アレックス益男

(72)【発明者】

【氏名】橋爪 滋郎

【審査官】菅家 裕輔

(56)【参考文献】

【文献】特開２０２０－１８７６６４（ＪＰ，Ａ）

【文献】国際公開第２０２０／２１３２７５（ＷＯ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０８Ｇ １／００ － ９９／００

Ｇ０１Ｃ ２１／００ － ２５／００

Ｇ０５Ｄ １／４３

(57)【特許請求の範囲】

【請求項1】

移動体に搭載され、センサからの情報をもとに現在位置の候補を生成し、現在位置の候補から移動体の現在位置として最も確率の高い候補を抽出する位置推定システムであって、
位置推定システムは、
移動体に搭載されるセンサからの情報に基づいて、前記移動体が存在する空間に含まれる直線を認識する直線認識手段と、
前記移動体に搭載されるセンサからの情報に基づいて、前記移動体が存在する空間に含まれるランドマークを認識するランドマーク認識手段と、
記憶装置に登録されたランドマークの位置を取得するランドマーク位置取得手段と、
前記記憶装置に登録された前記ランドマークに対して、認識した前記直線のうち数の多い平行な線の向き及び、前記記憶装置に登録されたランドマークに対して、当該平行な線を９０度、１８０度、２７０度回転した向きであって、かつ認識した前記ランドマークと前記移動体との相対距離の位置に前記移動体の現在位置の候補を複数生成する生成手段と、
複数生成した前記移動体の現在位置の候補のそれぞれに対して前記センサが時系列に取得した情報に基づいて算出した移動量を加算する移動量加算手段と、
前記認識したランドマークの位置を移動量加算後の現在位置の候補の座標に変換した位置と、前記記憶装置に登録されたランドマークの位置と、の距離が最も近くなる前記移動体の現在位置の候補を前記移動体の現在位置として最も確率の高い候補として抽出する抽出手段を備えることを特徴とする位置推定システム。

【請求項2】

請求項１に記載の位置推定システムであって、
前記抽出手段は、前記移動体が取得したランドマークの情報と、予め定められた地図情報とに基づいて、前記移動体の現在位置を予測する予測手段と、複数の前記現在位置の候補と、前記予測手段の予測結果とを比較し、複数の前記現在位置の候補のうち、前記移動体の現在位置として最も確率の高い候補を抽出する抽出手段を備えることを特徴とする位置推定システム。

【請求項3】

請求項１または請求項２に記載の位置推定システムであって、
前記生成手段は、前記ランドマークと前記移動体との相対距離と、前記直線認識手段が認識した前記線の向きと、予め定められた地図情報によるランドマークの数に基づいて、現在位置の候補の数と位置を算出することを特徴とする位置推定システム。

【請求項4】

請求項２に記載の位置推定システムであって、
前記抽出手段は、前記予測手段が予測したそれぞれの位置候補から認識可能なランドマークの位置と、現在位置から実際にセンサで認識したランドマークの位置を比較し、それぞれの位置候補が現在位置である確率を算出することを特徴とする位置推定システム。

【請求項5】

請求項２に記載の位置推定システムであって、
前記予測手段は、それぞれの位置候補の過去の位置とセンサで算出した移動体の移動量に基づいて、それぞれの位置候補の最新位置を算出し、算出したそれぞれの位置候補と地図情報によるセンサで認識可能なランドマークの位置を予測することを特徴とする位置推定システム。

【請求項6】

請求項１から請求項５のいずれか１項に記載の位置推定システムであって、
現在位置の候補から移動体の現在位置として最も確率の高い候補を抽出する処理を、移動体がロストしたことをもって実行することを特徴とする位置推定システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は，移動体の位置推定システムに関する。

【背景技術】

【0002】

移動体として例えば搬送ロボットがその周囲の情報を収集し，移動体の現在位置を推定する技術が開発されている。現在位置推定は、移動体の仮の現在位置から収集した周囲の情報を事前に作成した走行環境の地図とマッチングすることにより、仮の現在位置を修正する。一方、仮の現在位置が不明であり、あるいは誤差が大きい場合、マッチングしても、仮の現在位置を修正できない（以下においてはこの事象を、「移動体がロストした」というものとする）ため、搬送ができなくなる。

【0003】

例えば，非特許文献１は，移動体がロストしたとき、Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒを用いて、事前に定められた数（Ｐ）に基づいてランダムな位置と方位を有する現在位置の候補を複数生成する。次に、移動体が走行しながら実際に収集した周囲の情報と地図情報による予測した周囲の情報に基づいて、それぞれの現在位置の候補の存在確率を算出し、確率の高い現在位置の候補付近では候補を新しく生成し、確率の低い現在位置の候補を削除することによって、現在位置の候補の数を調整していく。最終的に、最も確率の高い現在位置の候補を現在位置とする。

【先行技術文献】

【非特許文献】

【0004】

【文献】Ｖ．Ｅｌｖｉｒａ，ｅｔ ａｌ．“Ａｄａｐｔｉｎｇ ｔｈｅ Ｎｕｍｂｅｒ ｏｆ Ｐａｒｔｉｃｌｅｓ ｉｎ Ｓｅｑｕｅｎｔｉａｌ Ｍｏｎｔｅ Ｃａｒｌｏ ｔｈｒｏｕｇｈ ａｎ Ｏｎｌｉｎｅ Ｓｃｈｅｍｅ ｆｏｒ Ｃｏｎｖｅｒｇｅｎｃｅ Ａｓｓｅｓｓｍｅｎｔ”．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｉｎｇ．２０１７．

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかし，非特許文献１のように，走行環境の広さやランドマーク配置に合わせないで、ランダムで事前に定められた候補の数に基づいて現在位置の候補を生成する場合、移動体が周囲の情報を収集しても、最も確率の高い現在位置の候補が実際の現在位置（真値）に収束する保証はない。

【0006】

また、位置の候補が多い場合、処理時間が増えることになり、一方、候補が少ないと、正確な位置推定できないということになる。

【0007】

以上のことから本発明においては、実際の現在位置（真値）を導くに好適な現在位置の候補を多数生成可能な位置推定システムを提供することを目的とする。

【課題を解決するための手段】

【0008】

上記課題を解決するために，本発明の移動体の位置推定システムは，「移動体に搭載され、センサからの情報をもとに現在位置の候補を生成し、現在位置の候補から移動体の現在位置として最も確率の高い候補を抽出する位置推定システムであって、移動体に搭載されるセンサからの情報に基づいて、移動体が存在する空間に含まれる直線を認識する直線認識手段と、移動体に搭載されるセンサからの情報に基づいて、移動体が存在する空間に含まれるランドマークを認識するランドマーク認識手段と、認識したランドマークに対して、認識した直線のうち数の多い平行な線の向きと同方向及び逆方向並びに直交する方向であって、かつ認識したランドマークと移動体との相対距離の位置に移動体の現在位置の候補を複数生成する生成手段と、現在位置の候補から移動体の現在位置として最も確率の高い候補を抽出する抽出部を備えることを特徴とする位置推定システム。」としたものである。

【発明の効果】

【0009】

本発明によれば、実際の現在位置（真値）を導くに好適な現在位置の候補を多数生成できるので、位置推定システムを搭載した移動体は，ロストしたときの位置推定の正確さが向上できる。また本発明の実施例によれば、正確で低処理での位置推定が可能となり、位置の候補生成の最適化が図られる。

【図面の簡単な説明】

【0010】

【図1】本発明の実施例に係る移動体の位置推定システムの構成例を示す図。

【図2】本発明の実施例に係る移動体の位置推定システムにおける処理フロー例を示す図。

【図3】直線認識処理ステップＳ２０２の詳細処理内容を事例で説明するための図。

【図4a】センサ１２で取得した図３と同じ内容のフレーム例を示す図。

【図4b】座標４０４の下に管理されるランドマークの配置情報例を示す図。

【図5a】図４ｂの候補に対して移動量を加算した後の候補を示す図。

【図5b】センサと外部記憶装置のランドマークを比較することを示す図。

【図6a】移動体が工場内（走行環境内）で搬送作業している状態を示す図。

【図6b】センサ取得範囲６０２内に障害物が存在する状態を示す図。

【図6c】移動体１が位置推定できなくなった後の動作を示す図。

【図6d】移動体１が位置推定を回復するときの動作を示す図。

【発明を実施するための形態】

【0011】

以下，本発明の実施例に係る移動体の位置推定システムについて，図面を用いて説明する。

【実施例1】

【0012】

図１は，本発明の実施例１に係る移動体の位置推定システムの構成例を示す図である。図１において移動体１は、本発明の一実施例に係る位置推定システム１０を搭載している。計算機で構成された位置推定システム１０は、複数のセンサ１２（１２ａ，１２ｂ，・・・１２ｎ）、入力部（信号受付部）２，処理部１４，制御部１５、メモリ１６，出力部１７などがバス１８により共通接続されており、各種の情報が共有されている。なお、位置推定システム１０は図示せぬ通信部を介して外部記憶装置１９に接続されている。

【0013】

このうち信号受付部２は，外部から位置信号（この場合に位置信号とは、直接的に位置を示す信号は勿論、その加工により位置となりえる信号をも含むものである）を受付る。信号受付部２は，例えば，世界の絶対座標で現在位置を推定するＧＰＳである。また，信号受付部２は，ＧＰＳよりも精度よく現在位置を推定するＲＴＫ－ＧＰＳでもよい。また，信号受付部２は，準天頂衛星でもよい。また，信号受付部２は，既知の位置に固定されたビーコンでもよい。また，信号受付部２は，車輪エンコーダやＩＭＵやジャイロなど相対座標で位置を推定するセンサから受信してもよい。また，信号受付部２は，走行環境の立体物の形状，大きさ，高さなどの情報を受信してもよい。最終的に，移動体１の現在位置推定や制御や認知に利用できる情報を入手可能なものであればよい。

【0014】

センサ１２は，例えばスチルカメラまたはビデオカメラである。また，センサ１２は単眼カメラまたは複眼カメラでもよい。また，センサ１２はレーザセンサでもよい。センサ１２は，センサ１２の出力から最終的に、走行環境のランドマーク情報を抽出できるものであればよい。

【0015】

センサ１２ａは，例えば，移動体１の前方に設置されている。センサ１２ａの取得方向は移動体１の前方に向けられている。センサ１２ａは，例えば，移動体１の前方の遠景情報を取得する。他のセンサ１２ｂ，・・・センサ１２ｎは，センサ１２ａと異なる位置に設置され，センサ１２ａと異なる撮像方向または領域を撮像する。センサ１２ｂは，例えば，移動体１の後方で下方に向けて設置されていてもよい。センサ１２ｂは，移動体１後方の近景情報を取得するものでよい。

【0016】

センサ１２が単眼カメラの場合，床面や天井が平らであれば，画像上のピクセル位置と実際の地上位置関係（ｘ，ｙ）が一定になるため，センサ１２からランドマークまでの距離を幾何学的に計算できる。また、床面や天井が平らでない場合、画像上でのランドマークの時系列での移動量と信号受付部２から受信した移動体１の移動量に基づいて、ランドマークまでの距離を推定できる。センサ１２がステレオカメラの場合，画像上のランドマークまでの距離を正確に幾何学的に計測できる。また，センサ１２がレーザの場合，より正確で遠方の情報を取得できる。以下の説明では，カメラやレーザを採用した事例について説明するが，周囲の立体物への距離を算出できれば，これ以外のセンサ（広角レンズを有するカメラまたはＴＯＦカメラなど）でもよい。

【0017】

また，センサ１２ａ，センサ１２ｂ，・・・センサ１２ｎは，同時に障害物や日差しなどの環境外乱の影響を受けないような条件で移動体１に設置されるのがよい。例えば，センサ１２ａは移動体１の前方で前向きに設置されるのに対して，センサ１２ｂは移動体１の後方で後ろ向きまたは下向きに設置されてもよい。これにより，例えば，日差しの影響によりセンサ１２ａが正確な情報を取得できなかった場合でも、進行方向の逆向きまたは下向きのセンサ１２ｂには影響しにくいので、環境外乱の影響を受けずに距離測定が可能である。

【0018】

また，センサ１２ａ，センサ１２ｂ，・・・センサ１２ｎは，互いに異なる取得条件（絞り値，ホワイトバランス，周期，等）で取得してもよい。例えば，明るい場所用にパラメータを調整したセンサと，暗い場所用にパラメータを調整したセンサとを搭載することで，環境の明暗によらず撮像可能としてもよい。

【0019】

センサ１２ａ，センサ１２ｂ，・・・センサ１２ｎは，制御部１５から取得開始の指令を受けたとき，または一定の時間間隔で情報を取得する。取得された情報のデータ及び取得時刻は，メモリ１６に格納される。

【0020】

また、センサ１２ａ，センサ１２ｂ，・・・センサ１２ｎがそれぞれ取得した情報を用いて、異なるタスクを実施してもよい。例えば、センサ１２ａとセンサ１２ｂで取得した情報に基づいて移動体１の位置推定を行い、センサ１２ｃとセンサ１２ｄで障害物検知を行う。最終的に、それぞれのセンサ１２ａ，センサ１２ｂ，・・・センサ１２ｎが得られた情報に基づいた結果を融合し、制御部１５による移動体１の制御ができればよい。また、制御部１５のＣＰＵがシングルスレッドでしか処理できない場合、それぞれのセンサ１２ａ，センサ１２ｂ，・・・センサ１２ｎが得られた情報をセンサ１２ａ，センサ１２ｂ，・・・センサ１２ｎの順番で処理する。一方、制御部１５のＣＰＵがマルチスレッドで処理できる場合は、センサ１２ａ，センサ１２ｂ，・・・センサ１２ｎが得られた情報を同時に処理することができる。

【0021】

処理部１４は、信号受付部２やセンサ１２で取得した情報を処理して移動体１の位置、ランドマークの認識、またはランドマークへの距離を算出する。例えば，処理部１４は，センサ１２が時系列に取得した情報で移動体１の移動量を算出し，過去の位置に移動量を加算して現在位置を推定する。処理部１４は，時系列に取得した情報の各情報で特徴を抽出してもよい。

【0022】

処理部１４は，さらに，次以降の情報で同じ特徴を抽出する。そして，処理部１４は，特徴のトラッキングにより移動体１の移動量を算出する。また，処理部１４は，Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ（ＳＬＡＭ）やＳｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ技術を用いて，移動体１の走行環境の形状を算出する。また、処理部１４は，Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ（ＩＣＰ）技術を用いて、センサ１２で取得された情報を事前に作られた走行環境の形状情報と比較するマップマッチングを行い、移動体１の位置を推定する。

【0023】

また、処理部１４は、外部記憶装置１９から得られた情報を処理する。処理部１４は，算出された位置または移動量に応じた表示を行ってもよく，または移動体１の制御に関する信号を出力してもよい。処理部１４は，例えば，移動体１の走行中にセンサ１２が取得した情報を処理して，障害物検知する。また，処理部１４は，例えば，移動体１の走行中にセンサ１２が取得した情報を処理して，事前に定められたランドマークを認識する。また、処理部１４は現在位置の候補を生成する。また、処理部１４は、算出した移動体１の移動量に基づいて、それぞれの現在位置の候補を更新する。また、処理部１４は、更新したそれぞれの現在位置の候補と外部記憶装置１９から受信した情報に基づいて、ランドマークの位置を予測する。処理部１４の詳細は後述する。

【0024】

制御部１５は，処理部１４の情報処理の結果に基づいて，移動体１に対して移動速度に関する指令を出力する。例えば，制御部１５は，情報内の立体物の解像度，情報内の特徴のうちの外れ値の数または情報処理の種類等に応じて，移動体１の移動速度を増加させる指令，減少させる指令または維持させる指令を出力してもよい。

【0025】

メモリ１６は，移動体１の主記憶装置（メインメモリ）およびストレージなどの補助記憶装置を含む。処理部１４がメモリ１６に格納された情報データ及び取得時刻に基づいて，様々な情報処理を行う。この情報処理では，例えば，中間情報が作成されてメモリ１６に保存される。中間情報は，処理部１４による処理の他，制御部１５などの判断や処理に利用されてもよい。

【0026】

出力部１７は、移動体１の状態や次に行う動作を表示するなどの出力を行う。出力部１７は、例えば、センサ１２で得られた情報を出力する。また、出力部１７は、処理部１４の処理結果や制御部１５の指令を出力する。例えば、移動体１が次に移動する目的地や現在位置から認識可能なランドマークを出力する。また、出力部１７は、センサ１２で認識した周囲の障害物を出力する。また、出力部１７は、地図情報を出力する。なお出力部の機能は移動体１に搭載されるが、出力先としての例えばモニタは移動体１に搭載、或は非搭載のいずれであってもよい。

【0027】

バス１８は，ＩＥＢＵＳ（Ｉｎｔｅｒ Ｅｑｕｉｐｍｅｎｔ Ｂｕｓ）やＬＩＮ（Ｌｏｃａｌ Ｉｎｔｅｒｃｏｎｎｅｃｔ Ｎｅｔｗｏｒｋ）やＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ Ａｒｅａ Ｎｅｔｗｏｒｋ）などで構成できる。

【0028】

外部記憶装置１９は，移動体１が走行する環境の地図情報を有する。外部記憶装置１９の地図情報は，例えば，走行環境にある静止物（壁，柱，看板，照明、テーブル、椅子、など）の形状や位置である。外部記憶装置１９のそれぞれの地図情報を数式で表してもよい。例えば，線情報を複数点で構成せず，線の傾きと切片のみでよい。また，外部記憶装置１９の地図情報を区別せずに，点群で表してもよい。点群は３Ｄ（ｘ，ｙ，ｚ），４Ｄ（ｘ，ｙ，ｚ，色）などで表してもよい。最終的に，移動体１の現在位置から走行環境を検出し，マップマッチングができれば，外部記憶装置１９の地図情報をどんな形にしてもよい。

【0029】

制御部１５から取得開始の指令を受けたとき，外部記憶部１９が地図情報をメモリ１６に送る。外部記憶装置１９は，移動体１に搭載された場合，情報をバス１８で送受信する。一方，外部記憶装置１９は移動体１に搭載されていない場合，信号受付部２で移動体１と外部記憶装置１９を接続する。移動体１と外部記憶装置１９の接続は，例えば，Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ（ＬＡＮ）でする。また，移動体１と外部記憶装置１９の接続をＷｉｄｅ Ａｒｅａ Ｎｅｔｗｏｒｋ（ＷＡＮ）にしてもよい。また、制御部１５から指令を受けたとき，外部記憶装置１９が移動体１の走行環境の地図情報を処理部１４に送る。

【0030】

図２は，本発明の実施例に係る移動体の位置推定システムにおける処理フロー例を示している。ここでは、処理部１４における処理内容の一例を示している。なお、図２におけるフローは、移動体がロストしたことを前提として起動されるものであり、ロスト以前には、別途初期設定された位置情報に基づいた走行制御が行われていたものとする。

【0031】

まずこの一連の処理の最初に、情報取得処理ステップＳ２０１では、センサ１２において走行環境の情報を例えば所定時間間隔で取得する。

【0032】

次に直線認識処理ステップＳ２０２は、情報取得処理ステップＳ２０１で得られた情報から走行環境における直線を認識する。センサ１２がカメラの場合、直線認識処理ステップＳ２０２はカメラセンサ１２で得られた画像から直線を抽出する。抽出する方法は、例えば、Ｈｏｕｇｈ Ｔｒａｎｓｆｏｒｍが利用可能である。また、センサ１２がレーザの場合、取得したコーナーや平面の位置関係に基づいて直線を抽出する。

【0033】

なおここで、移動体１が配置される走行環境は、直線が存在する領域であれば、屋内、屋外を問わない。但し本発明の以下の説明では、走行環境として人間が構築した屋内環境を想定し例示している。特に屋内の場合には、長方形パターンを構成する直線が多数配置されていることから、長方形パターンを構成する直線を多数抽出しやすいといえる。

【0034】

走行環境から複数の線を抽出できる場合、それぞれ抽出した線の方向を算出し、最も多い方向の線を抽出する。また、センサ１２に誤差があるため、最も多い線の方向を抽出する前に、他の方向を有する線への相対角度を算出する。算出した相対角度が９０度になっていれば、長方形パターンを構成する線である確率が高いため、線を抽出し、９０度とは違う相対角度の場合は抽出しない。最終的に長方形パターンに対する移動体１の方位推定が必要のため、複数の線を抽出した場合、抽出した複数の線の平均や中央値を算出する。直線認識処理ステップＳ２０２の詳細処理内容は、図３を用いて後述する。

【0035】

図２の確認処理ステップＳ２０３では、直線認識処理ステップＳ２０２の処理により、走行環境の情報から直線認識ができたか否かを確認する。障害物や外乱などの影響で移動体１の現在位置から、線を認識できない場合、情報取得処理ステップＳ２０１に戻り、新たな環境の情報を抽出する。この場合の新たな環境の情報とは、移動体が移動した後の新たな環境での情報であるが、より積極的な意味合いにおいては、上方や前後左右に設置された複数のセンサを切り替えて、例えば前センサの確認後は左右あるいは後センサについての処理を行い、線を検知する可能性を高める処理を併せて実行することが望ましい。そのうえで、直線認識できた場合は、ランドマーク認識処理ステップＳ２０４に進む。

【0036】

ランドマーク認識処理ステップＳ２０４では、センサ１２により走行環境から取得した情報から、外部記憶装置１９に事前に登録した特定のランドマークを認識する。ランドマークは、例えば、看板、照明、テーブル、椅子、機器などの静止物、あるいはランドマークは走行環境のエッジやコーナーであり、その位置や形状が外部記憶装置１９に記憶されていることから、センサ１２で走行環境から取得した情報を用いて、外部記憶装置１９を参照することでランドマークの認識が可能である。

【0037】

またランドマーク認識処理ステップＳ２０４では、特定のランドマークを認識した後、移動体１と認識したランドマークの間の相対距離を測定する。センサ１２がステレオカメラやライダーのように、距離情報を算出できるセンサの場合、センサ１２が測定した距離情報を相対距離とすることができる。

【0038】

他方、センサ１２が直接的に距離を算出できない単眼カメラの場合、深層学習や幾何学的に認識したランドマークへの相対距離を測定することができる。ランドマークが点群以外の場合、相対距離の座標を事前に決める必要がある。例えば、ランドマーク認識処理ステップＳ２０４において画像からランドマークを認識した場合、画像上でのランドマークの中心を算出し、その中心を座標基準としてセンサ１２からの相対距離を測定する。また、中心ではなく、画像上でのコーナーを座標基準としてもよい。

【0039】

最終的に、センサ１２の出力を用いて、ランドマーク認識処理ステップＳ２０４で認識したランドマークの位置が、外部記憶装置１９に登録されたランドマークの位置と一致すればよい。

【0040】

確認処理ステップＳ２０５では、センサ１２で取得した走行環境の情報からランドマークを認識できたか、否かを確認する。障害物や外乱などの影響で移動体１の現在位置からランドマークを認識できない場合は、情報取得処理ステップＳ２０１に戻り、センサを変更するなどして新たな環境の情報を抽出する。ランドマーク認識できた場合は、候補生成処理ステップＳ２０６に進む。

【0041】

候補生成処理ステップＳ２０６では、直線認識処理ステップＳ２０２での処理と、ランドマーク認識処理ステップＳ２０４での処理と、外部記憶装置１９に事前に登録したランドマークに基づいて、現在位置の候補を生成する。候補生成処理ステップＳ２０６の詳細処理内容は、図４を用いて後述する。

【0042】

予測処理ステップＳ２０７では、候補生成処理ステップＳ２０６でそれぞれ生成した現在位置の候補の位置を更新し、それぞれの更新した位置でランドマークの位置を予測する。予測処理ステップＳ２０７の詳細処理内容は、図５を用いて後述する。

【0043】

現在位置抽出処理ステップＳ２０８では、予測処理ステップＳ２０７で得られたそれぞれの現在位置の候補から実際の現在位置を抽出する。

【0044】

図３は、直線認識処理ステップＳ２０２の詳細処理内容を事例で説明するための図である。この事例では、説明を簡単に行うという理由により、センサ１２をカメラとし、走行環境の天井を撮像した事例とする。この事例においてフレーム３００は、センサ１２で取得したフレーム（画像）であり、座標３０１はＵ軸－Ｖ軸で示したフレーム３００の座標である。

【0045】

フレーム３００内には多数の線が映りこまれており、このうち、線３０２ａ～３０２ｇは、フレーム３００に映っている平行線である。線３０３は、平行な線３０２ａ～３０２ｇとは、異なる方向を有する線である。線３０４ａ～３０４ｂは、平行な線３０２ａ～３０２ｇに対して９０度で配置された線である。また壁３０５ａ～３０５ｃは、フレーム３００に映っている走行環境の壁である。

【0046】

これら各種の線の中から走行環境の長方形パターンを構成する線を認識するには、まず、フレーム３００に映っている全ての線（線３０２ａ～３０２ｇ、線３０３、線３０４ａ～３０４ｂ）を抽出する。

【0047】

次に、抽出した線の向きをそれぞれ算出し、最も本数が多い線の向きを求める。この場合、平行な線３０２ａ～３０２ｇの向きが最も本数が多い線の向きとして抽出される。また、線３０３のように、向きが異なる線（外れ値）があっても、線３０２ａ～３０２ｇの本数に対して少ないため、直線認識処理ステップＳ２０２では認識されない。

【0048】

またこの処理では、障害物の影響で平行な線３０２ａ～３０２ｇを抽出するのが困難で、誤って線３０３を認識する場合があるため、線３０４ａ～３０４ｂの向きを用いて直線認識の信頼度を上げる。例えば、線３０３は線３０４ａ～３０４ｂに対して９０度で配置されていないため、長方形を構成する線である可能性が低い。一方、平行な線３０２ａ～３０２ｇは線３０４ａ～３０４ｂに対して９０度で配置されているため、長方形パターンを構成する線である確率が高い。従って、平行な線３０２ａ～３０２ｇの向きが認識される。また、センサ１２で取得した画像に線３０２ａ～３０２ｇに対して９０度で配置された線３０４ａ～３０４ｂが映っていなかった場合、時系列に認識した過去の線の向きと移動体１の過去の移動量に基づいて直線認識することができる。このようにして直線認識処理ステップＳ２０２では、最も本数が多い線の向きが抽出される。

【0049】

図４ａ，図４ｂは、候補生成処理ステップＳ２０６の詳細処理内容を事例で説明するための図である。この事例では、説明を簡単に行うという理由により、センサ１２をカメラとし、走行環境の天井を撮像しているものとする。またフレーム３００は、センサ１２で取得したフレームである。座標３０１はＵ軸－Ｖ軸で示したフレーム３００の座標である。

【0050】

図４ａは、センサ１２で取得した図３と同じ内容のフレームである。但しここでは、直線認識処理ステップＳ２０２で認識した線の向き４０２と、ランドマーク認識処理ステップＳ２０４で認識したランドマーク４０３に着目して表記している。この図でランドマーク４０３は、〇印で示されている。

【0051】

図４ｂは、外部記憶装置１９に登録された走行環境の事例であり、Ｘ軸－Ｙ軸で示した座標４０４の下に管理されるランドマークの配置情報例である。ここでは、外部記憶装置１９に登録された走行環境のランドマーク４０５ａ～ランドマーク４０５ｎと、その位置を示している。また図４ｂには、ランドマーク４０５ｎに仮着目した時の現在位置の候補４０７ａ～４０７ｄを例示している。なお、距離ｄ４０６は、ランドマーク認識処理ステップＳ２０４で算出したセンサ１２から認識したランドマーク４０５ｎへの相対距離である。

【0052】

図４ｂでの説明にあたり、確認処理ステップＳ２０５までの処理により確認できた事項は、ランドマークの存在と、直線の向きと、そのランドマークまでの相対距離が判明したことの３点のみである。図４ｂに例示する複数のランドマークのどれが存在確認したランドマークであり、またこの存在確認したランドマークに対する現在位置（方向、姿勢）までは判明していない。

【0053】

これらのことから、候補生成処理ステップＳ２０６では、以下のようにして現在位置の候補４０７ａ～４０７ｄを生成する。ここでは、図４ｂのランドマーク４０５ｎを用いて現在位置候補生成の考え方を説明する。

【0054】

まず、直線認識処理ステップＳ２０２で認識した線の向き４０２と、ランドマーク認識処理ステップＳ２０４で認識したランドマークへの距離ｄ４０６に基づいて、現在位置の候補４０７ａを算出する。この処理では、走行環境が屋内環境であり、長方形パターンが多く配置されているため、直線認識処理ステップＳ２０２で認識した線の向き４０２が長方形パターンの１方向であることを利用して、残りの３方向、つまり、線の向き４０２に対して、９０度、１８０度、２７０度回転した現在位置の候補４０７ｂ～４０７ｄを生成する。これは、認識した線の向き４０２に対して逆方向または直交する方向に現在位置の候補４０７ｂ～４０７ｄを追加配置したことである。

【0055】

なお、この説明では、図４ｂのランドマーク４０５ｎを用いて説明したが、その前提として、そもそもランドマーク認識処理ステップＳ２０４で認識したランドマークが実際に外部記憶装置１９に登録されたランドマーク４０５ｎか分からない。このため、外部記憶装置１９に登録されたランドマーク４０５ａ～ランドマーク４０５ｎの全てに対して現在位置の候補を生成する。また、長方形の４方向と外部記憶装置１９に登録されたランドマークの数ｎが既知のため、現在位置の候補の数（ｎ＊４）を正確に算出できる。つまり、ランダムに現在位置の候補を生成する公知例と違って、本発明は現在位置の候補の正確な位置と方位を含めて、候補の数も算出可能となる。

【0056】

なお、前述した現在位置の候補の数（ｎ＊４）は、位置推定するための最小限必要な数であるが、走行環境の外乱やセンサ１２の誤差が大きい場合、位置推定の信頼度を上げるために現在位置の候補を増やしてもよい。例えば、本発明で生成した現在位置の候補に加えて、従来技術のようにランダムに現在位置の候補を生成してもよい。また、最小限必要な現在位置の候補（ｎ＊４）を増やすためのパラメータｋを用いることができる。その場合、生成する現在位置の候補がｎ＊４＊ｋとなり、走行環境の外乱やセンサ１２の誤差に合わせてパラメータｋを調整する。また、ランダムではなく、センサ１２の誤差と生成した現在位置の候補（ｎ＊４）に基づいて現在位置の候補の数を調整してもよい。例えば、センサ１２の誤差が１メートルの場合、それぞれ生成した現在位置の候補（ｎ＊４）の中心から、１メートル以内他の現在の候補を生成してもよい。最終的に、移動体１の実際の現在位置を特定できる情報があれば、その情報に基づいてパラメータｋを調整してもよい。例えば、信号受信部２で受信した情報に基づいて、移動体１の実際の現在位置を地図の特定の領域に絞ることができた場合、ｎ＊４を生成する必要がなく、特定した領域周辺にｎ＊４より少ない現在位置の候補を生成する。さらには、９０度間隔に配置した候補の間にさらに４５度間隔で追加配置することもできる。

【0057】

図５ａ，図５ｂを用いて、予測処理ステップＳ２０７の処理内容を事例で説明する。図５ａは、図４ｂの現在位置の候補４０７ｂ～４０７ｄに対してそれぞれ移動量Ｄ５０１を加算した後の現在位置の候補５０２ａ～５０２ｄを示す図である。移動量Ｄ５０１の加算により、４０７ａは５０２ａの位置に、４０７ｂは５０２ｂの位置に、４０７ｃは５０２ｃの位置に、４０７ｄは５０２ｄの位置にそれぞれシフトしている。

【0058】

図５ｂにおいて、（移動量Ｄ５０１を加算した後の）現在位置の候補５０３ａと５０３ｂは、ランドマーク５０５をセンサ１２で実際に認識したときの最新現在位置の候補である。図５ｂでは、センサ１２で実際に認識したランドマーク５０５をそれぞれの現在位置の候補５０３ａ～５０３ｂの座標に変換した後のそれぞれの位置を示している。

【0059】

これに対し、ランドマーク５０４ａ～５０４ｂは、外部記憶装置１９の情報に基づいて、現在位置の候補５０３ａ～５０３ｂに対する最も近いランドマークの位置を示している。また、ランドマーク５０５は、移動体１の実際の現在位置で実際に認識したランドマークである。

【0060】

図５ｂの位置関係において、予測処理ステップＳ２０７では、実際に認識したランドマーク５０５を現在位置の候補５０３ａ～５０３ｂの座標から得られたランドマーク５０４ａ～５０４ｂと比較する。現在位置の候補５０３ａについて、その近傍の５０５と５０４ａの距離、現在位置の候補５０３ｂについて、その近傍の５０５と５０４ｂの距離をそれぞれ評価する。

【0061】

評価結果に応じて現在位置抽出処理ステップＳ２０８では、現在位置の候補を抽出する。このケースでは、ランドマーク５０５からランドマーク５０４ａへの距離の方がランドマーク５０５からランドマーク５０４ｂへの距離よりも近いため、移動体１の実際の現在位置を現在位置の候補５０３ａとし、現在位置抽出処理ステップＳ２０８で現在位置の候補として５０３ａを抽出する。

【0062】

一方、センサ１２の誤差が発生する可能性があるため、現在位置抽出処理ステップＳ２０８で抽出する現在位置のロバスト性を上げるための観察を増やしてもよい。例えば、時系列に認識認識したランドマーク５０４ａ，５０４ｂからランドマーク５０５への距離を算出して、算出したそれぞれの距離の平均を算出して、最も平均距離の小さい現在位置の候補を実際の現在位置とする。また、ｂａｙｅｓｉａｎ確率を用いて、算出したそれぞれの最新の現在位置の候補から認識可能なランドマークの予測と実際にセンサ１２で認識ランドマークに対して、それぞれの現在位置の候補の存在確率を算出してもよい。それで、それぞれの現在位置の候補の存在確率を時系列に算出し、事前に定められた存在確率の閾値より存在確率が大きくなった現在位置の候補があった場合、その現在位置の候補を現在位置とする。

【0063】

以上説明した位置推定システムは、走行環境に配置されたランドマークと直線にパターンがある想定で、抽出したランドマークからの相対位置と直線パターンに基づいて位置の候補を正確に生成することで、位置の候補を正確に算出できるため、最適な処理時間と正確さで位置推定ができる。特に、正確で低処理な位置推定と、位置の候補生成の最適化を図ることができる。

【実施例2】

【0064】

実施例２では、実施例１の位置推定システムを搭載した移動体１を走行環境内で実運用することに付いて説明する。

【0065】

図６ａは、移動体１が工場内（走行環境内）で搬送作業している状態を示している。ここで、Ｘ軸―Ｙ軸で示す座標６０１は、外部記憶装置１９に登録された走行環境のランドマーク情報の座標である。またセンサ取得範囲６０２はセンサ１２の情報取得範囲であり、簡単のため、センサ１２をカメラ１台とする。またランドマーク６０３ａ～６０３ｅは、工場内の天井に配置された照明であり、外部記憶装置１９に登録された情報である。この状態では、移動体１がセンサ１２で得られたランドマーク情報に基づいてマップマッチングを行い、位置推定している。この場合、移動体１は問題なく搬送作業ができているものとする。

【0066】

図６ｂは、センサ取得範囲６０２内に障害物が存在する状態を示している。この状態では、障害物６０４（例えば作業者）がセンサ取得範囲６０２に入り、移動体１は位置推定ができなくなった。従って、移動体１は座標６０１に対する現在位置が不明となった。

【0067】

図６ｃは、移動体１が位置推定できなくなった後の動作を示している。この時、図２の処理によれば、まず、情報取得処理ステップＳ２０１では、センサ１２により走行環境の画像を取得する。次に、直線認識処理ステップＳ２０２では、情報取得処理ステップＳ２０１で取得した画像から線認識を実施する。この認識処理により、工場内においてある箱６０４ａ～６０４ｎから直線６０５を認識したものとする。

【0068】

次にランドマーク認識処理ステップＳ２０４では、ランドマーク６０３ａを認識し、センサ１２からランドマーク６０３ａへの距離ｄ６０６が算出されたとする。そして、距離ｄ６０６と、認識した直線６０５の方向と、外部記憶装置１９に登録されたランドマークの数に基づいて、候補生成処理ステップＳ２０６では、現在位置の候補を生成する。この場合、外部記憶装置１９にランドマークが５つあるため、候補生成処理ステップＳ２０６では、４＊５＝２０の現在の位置の候補が生成される。なお、現在位置の候補６０７ａ～６０７ｎは、ステップ候補生成で生成された現在位置の候補である。

【0069】

図６ｄは、移動体１が位置推定を回復するときの動作を示している。図６ｄにおいて、最新現在位置の候補６０８ａ～６０８ｎは、予測手段２０７で算出したそれぞれの生成した現在位置の候補の最新位置である。

【0070】

あるとき、センサ１２でランドマーク６０９を認識したとする。センサ１２で実際に認識したランドマーク６０９をそれぞれの現在位置の候補６０７ａ～６０７ｎの座標に変換したあと、外部記憶装置１９に登録されたそれぞれのランドマーク情報と比較する。そこで、現在位置の候補６０８ｂの座標に対して変換したランドマーク６０９に最も近い外部記憶装置１９に登録された情報による予測がランドマーク６０３ｂのため、現在位置抽出手段２０８で現在位置の候補６０８ｂを現在位置とする。従って、移動体１が工場内での搬送作業を続けられる。

【符号の説明】

【0071】

１：移動体
２：信号受付部
１２：センサ
１４：処理部
１５：制御部
１６：メモリ
１７：出力部
１９：外部記憶装置

【図1】

【図2】

【図3】

【図4a】

【図4b】

【図5a】

【図5b】

【図6a】

【図6b】

【図6c】

【図6d】