特許 7382910 自己位置推定装置及びプログラム

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

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

(11)【特許番号】

(24)【登録日】2023-11-09

(45)【発行日】2023-11-17

(54)【発明の名称】自己位置推定装置及びプログラム

(51)【国際特許分類】

   G01C 21/30 20060101AFI20231110BHJP

   G05D 1/02 20200101ALI20231110BHJP

   G01C 11/06 20060101ALI20231110BHJP

   G01C 15/00 20060101ALI20231110BHJP

   B60W 40/02 20060101ALI20231110BHJP

【ＦＩ】

G01C21/30

G05D1/02 J

G01C11/06

G01C15/00 101

G01C15/00 104C

B60W40/02

【請求項の数】 9

(21)【出願番号】P 2020130479

(22)【出願日】2020-07-31

(65)【公開番号】P2022026832

(43)【公開日】2022-02-10

【審査請求日】2023-02-03

(73)【特許権者】

【識別番号】509186579

【氏名又は名称】日立Ａｓｔｅｍｏ株式会社

(74)【代理人】

【識別番号】110000925

【氏名又は名称】弁理士法人信友国際特許事務所

(72)【発明者】

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

(72)【発明者】

【氏名】腰塚 久洋

(72)【発明者】

【氏名】筒井 隆

(72)【発明者】

【氏名】池田 道彦

【審査官】上野 博史

(56)【参考文献】

【文献】特開２０１１－２１５０５２（ＪＰ，Ａ）

【文献】特開２０１７－０５３７９５（ＪＰ，Ａ）

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

【文献】特開２０１９－１３３３１８（ＪＰ，Ａ）

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

Ｇ０１Ｃ ２１／００－２１／３６

２３／００－２５／００

Ｇ０５Ｄ １／００－１／１２

Ｇ０１Ｃ １／００－１／１４

５／００－１５／１４

Ｂ６０Ｗ １０／００－１０／３０

３０／００－６０／００

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

【請求項1】

センサで収集した走行環境の情報と、地図情報とを比較することで自己の位置を推定する自己位置推定装置であって、
自己の現在の位置を仮に推定する現在位置推定部と、
前記地図情報に含まれる点群データのうち、前記現在位置推定部で推定した現在の仮位置に基づき前記センサが前記情報を取得可能な範囲を算出し、前記取得可能な範囲の点群データを選択する点群データ選択部と、
前記地図情報から選択した点群データの点数と、位置を推定する際の許容可能な最大の処理時間で処理できる最大の点数である最大処理可能点数とを比較し、比較結果に基づいて、選択した前記点群データの点数を前記最大処理可能点数以下の範囲で調整する点数調整部と、
調整後の前記点群データと前記センサで取得した前記情報の点群データとのマッチングを行うマッチング部と、
前記マッチング部のマッチング結果に基づいて、前記現在位置推定部で推定した自己の現在の仮位置を修正する現在位置修正部と、を備え、
前記点数調整部において、選択した点群データの点数が前記最大処理可能点数よりも少ない場合には、選択した点群データの点数を前記最大処理可能点数まで増やす
自己位置推定装置。

【請求項2】

前記点数調整部において、選択した点群データの点数が前記最大処理可能点数よりも多い場合には、選択した点群データの点数を前記最大処理可能点数に減らす
請求項１に記載の自己位置推定装置。

【請求項3】

前記マッチング部において、マッチングできなかった点群データを記憶し、記憶した点群データを、次回の位置推定の際に前記地図情報に含まれる点群データから選択しないようにする
請求項１に記載の自己位置推定装置。

【請求項4】

前記点数調整部において、前記選択した点群データの点数を前記最大処理可能点数に減らす際に、前記選択した点群データのうち現在の仮位置からより遠い点群データを優先的に減らす
請求項２に記載の自己位置推定装置。

【請求項5】

前記点数調整部において、前記選択した点群データの点数を前記最大処理可能点数まで増やす際に、前記選択した点群データのうち現在の仮位置により近い点群データを優先的に増やす
請求項１に記載の自己位置推定装置。

【請求項6】

前記最大処理可能点数は、予め求めておいた異なる点数と当該点数での処理時間との対応関係に基づいて算出される点数である
請求項１に記載の自己位置推定装置。

【請求項7】

前記マッチング部は、マッチングできなかった点群データを含む領域の位置と形状を算出して記憶する
請求項３に記載の自己位置推定装置。

【請求項8】

前記マッチング部は、時系列に得られた前記マッチング結果に応じて、前記地図情報に対して時系列にマッチングできなかった領域の大きさ又は点群データの点数を調整して記憶する
請求項３又は７に記載の自己位置推定装置。

【請求項9】

センサで収集した走行環境の情報と、地図情報とを比較することで自己の位置を推定する自己位置推定装置が備えるコンピューターに、
絶対座標における自己の現在の位置又は相対座標における自己の現在の位置を仮に推定する手順と、
前記地図情報に含まれる点群データのうち、推定した現在の仮位置に基づき前記センサが前記情報を取得可能な範囲を算出し、前記取得可能な範囲の点群データを選択する手順と、
前記地図情報から選択した点群データの点数と、位置を推定する際の許容可能な最大の処理時間で処理できる最大の点数である最大処理可能点数とを比較し、比較結果に基づいて、選択した前記点群データの点数を前記最大処理可能点数以下の範囲で調整する手順と、
調整後の前記点群データと前記センサで取得した前記情報の点群データとのマッチングを行う手順と、
マッチング結果に基づいて、推定した自己の現在の仮位置を修正する手順と、
選択した前記点群データの点数を前記最大処理可能点数以下の範囲で調整する手順において、選択した点群データの点数が前記最大処理可能点数よりも少ない場合には、選択した点群データの点数を前記最大処理可能点数まで増やす手順
を実行させるためのプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定装置及びプログラムに関し、特にロボットや自動車などの移動体の位置を推定する技術に関する。

【背景技術】

【0002】

ロボット及び自動車などの移動体がその周囲の情報を収集し、移動体の現在位置及び走行状態を推定し、移動体の走行を制御する自律走行技術及び運転支援技術が開発されている。現在位置推定の信頼度は、収集した周囲の情報の精度や量に依存する。一方、収集した情報量が大きい場合、処理負荷が高くなり、位置推定の信頼度が低くなる。

【0003】

例えば、特許文献１には、移動体の位置推定に要する処理の負荷を低減することができるようにした移動体位置推定装置及び移動体位置推定方法が開示されている。特許文献１では、所定の基準を満たす所定の特徴点を規定数だけ確保し、それら規定数の所定の特徴点だけを追跡することで、移動体の位置を比較的少ない処理負荷で推定できるようにしている。

【先行技術文献】

【特許文献】

【0004】

【文献】国際公開第１５／０４９７１７号

【発明の概要】

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

【0005】

しかしながら、特許文献１のように、所定の基準を満たす所定の特徴点の数が多い場合、処理負荷を低減できない。また、所定の基準を満たす所定の特徴点が少ない場合、精度の高い位置推定ができない。

【0006】

上記の状況から、位置推定の精度と処理負荷を最適化した位置推定を可能にする手法が要望されていた。

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

【0007】

上記課題を解決するために、本発明の一態様の自己位置推定装置は、センサで収集した走行環境の情報と、地図情報とを比較することで自己の位置を推定する自己位置推定装置であって次の構成をとる。
自己位置推定装置は、自己の現在の位置を仮に推定する現在位置推定部と、地図情報に含まれる点群データのうち、現在位置推定部で推定した現在の仮位置に基づきセンサが情報を取得可能な範囲を算出し、取得可能な範囲の点群データを選択する点群データ選択部と、地図情報から選択した点群データの点数と、位置を推定する際の許容可能な最大の処理時間で処理できる最大の点数である最大処理可能点数とを比較し、比較結果に基づいて、選択した点群データの点数を最大処理可能点数以下の範囲で調整する点数調整部と、調整後の点群データとセンサで取得した情報の点群データとのマッチングを行うマッチング部と、マッチング部のマッチング結果に基づいて、現在位置推定部で推定した自己の現在の仮位置を修正する現在位置修正部と、を備える。

【発明の効果】

【0008】

本発明の少なくとも一態様によれば、位置推定の精度と処理負荷を最適化した位置推定が可能となる。
上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

【図面の簡単な説明】

【0009】

【図1】本発明の一実施形態に係る移動体に搭載された位置推定装置の全体構成図である。

【図2】図１のセンサ処理部の内部構成例を示すブロック図である。

【図3】センサ処理部が実行する情報処理の手順例を示すフローチャートである。

【図4】ステップＳ２の最大点数記録処理における最大点数算出の詳細を示すグラフである。

【図5】ステップＳ１２のマッチング不可領域算出処理と、ステップＳ１３のマッチング不可領域記録処理の例（障害物なし）を示す図である。

【図6】ステップＳ１２のマッチング不可領域算出処理と、ステップＳ１３のマッチング不可領域記録処理の例（障害物あり）を示す図である。

【図7】ステップＳ９の点数低減処理の詳細を示す図である。

【図8】ステップＳ８の点数増加処理の詳細を示す図である。

【図9】本発明の一実施形態に係る自己位置推定処理の第１の例を示す図である。

【図10】本発明の一実施形態に係る自己位置推定処理の第２の例を示す図である。

【図11】本発明の一実施形態に係る自己位置推定処理の第３の例を示す図である。

【図12】本発明の一実施形態に係る自己位置推定処理の第４の例を示す図である。

【図13】本発明の一実施形態に係る自己位置推定処理の第５の例を示す図である。

【図14】本発明の一実施形態に係る自己位置推定処理の第６の例を示す図である。

【図15】点数低減を実施する場合の点群データの選択基準を説明する図である。

【図16】図１５で選択された点群を用いた自己位置推定処理の例を示す図である。

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

【0010】

以下、本発明を実施するための形態の例について、添付図面を参照して説明する。本明細書及び添付図面において実質的に同一の機能又は構成を有する構成要素については、同一の符号を付して重複する説明を省略する。

【0011】

なお、本明細書においては、「三次元点」は、形状を有する物体の表面及び内部に位置する空間上の座標を表すものであり、センサにより得られた点、マップに含まれる点等を含むものとする。また、複数の三次元点を「点群」と呼ぶ。なお、三次元点データ（点群データ）に色の情報が含まれていてもよい。

【0012】

［位置推定装置の全体構成］
図１は、本発明の一実施形態に係る位置推定装置の構成図である。
位置推定装置１（自己位置推定装置の一例）は、自動車又はロボットなどの移動体１００に搭載される。位置推定装置１は、位置推定装置１の外部に存在する外部記憶装置１９と通信可能となっている。この通信手段は無線が望ましい。

【0013】

位置推定装置１は、信号受付部２と、一台以上のセンサ１２ａ，１２ｂ，・・・，１２ｎと、情報処理装置１３とを有する。これらの構成要素は、バス１８により相互に接続されている。本明細書では、センサ１２ａ，１２ｂ，・・・，１２ｎを特に区別する必要がない場合には、センサ１２と記載する。

【0014】

情報処理装置１３は、例えば一般的なコンピューター（電子計算機）であって、センサ１２によって取得した情報を処理するセンサ処理部１４と、センサ処理結果に基づく処理を行う制御部１５（例えばＣＰＵ）と、メモリ１６と、ディスプレイなどの表示部１７と、を含む。情報処理装置１３においては、センサ処理部１４及び制御部１５が、メモリ１６又は図示しない記憶媒体に記録されたコンピュータープログラムを読み出して実行することにより、本実施形態に係る各機能が実現される。

【0015】

信号受付部２は、外部からの信号を受信する。例えば、信号受付部２は、世界の絶対座標で現在位置を推定する全地球測位システム（Global Positioning System : GPS）の受信機である。また、信号受付部２は、ＧＰＳよりも精度よく現在位置を推定するＲＴＫ－ＧＰＳ（Real Time Kinematic GPS）の受信機でもよい。また、信号受付部２は、準天頂衛星システムの受信機でもよい。また、信号受付部２は、既知の位置に固定されたビーコンからの信号を受信するものでもよい。また、信号受付部２は、車輪エンコーダ、慣性計測装置（Inertial Measurement Unit : IMU）、ジャイロなど、相対座標で位置を推定するセンサから信号を受信してもよい。また、信号受付部２は、走行環境の車線、標識、交通状態、立体物の形状、大きさ、高さなどの情報を受信してもよい。最終的に、位置推定装置１が搭載された移動体１００の現在位置推定や制御や認知に利用できれば、どのような方式でもよい。

【0016】

センサ１２は、例えばスチルカメラ又はビデオカメラである。また、センサ１２は、単眼カメラ又は複眼カメラでもよい。また、センサ１２は、レーザセンサでもよい。最終的に、走行環境（移動体１００の周囲）の形状情報を抽出できればどのようなセンサでもよい。

【0017】

情報処理装置１３は、センサ１２で取得した情報を処理して、移動体１００の位置又は移動量を算出する。情報処理装置１３は、算出された位置又は移動量に応じた表示を行ってもよい。また、情報処理装置１３は、移動体１００の制御に関する信号を出力してもよい。

【0018】

次に、センサ１２の詳細について説明する。
センサ１２ａは、例えば、移動体１００の前方に設置されている。センサ１２ａは、レンズを有し、移動体１００の前方の遠景情報を取得する。この場合、遠景から取得した情報は、立体物、又は位置推定のためのランドマーク（所定の静止物）などの特徴が抽出されるようにしてもよい。

【0019】

他のセンサ１２ｂ、…、１２ｎは、センサ１２ａと異なる位置に設置され、センサ１２ａと異なる方向又は領域を撮像する。センサ１２ｂは、例えば、移動体１００の後方で下方に向けて設置されていてもよい。この場合、センサ１２ｂは、移動体１００の後方の近景情報を取得する。近景情報は、移動体１００周辺の路面などでもよく、移動体１００の周囲の白線、又は路面ペイントなどが検出されるようにしてもよい。

【0020】

センサ１２が単眼カメラの場合、路面が平らであれば、画像上のピクセル位置と実際の地上位置関係（ｘ，ｙ）が一定になるため、センサ１２から特徴点までの距離を幾何学的に計算できる。路面が平ではない場合、画像上の特徴点の時系列での移動量と信号受付部２から受信した移動体１００の移動量とに基づいて、移動体１００から特徴点に相当する物体までの距離を推定できる。また、センサ１２がステレオカメラの場合、画像上の特徴点までの距離をより正確に計測できる。また、センサ１２がレーザセンサの場合、より正確に遠方の情報を取得できる。

【0021】

以下の説明では、単眼カメラ、ステレオカメラ又はレーザセンサを採用した事例について説明するが、周囲の立体物への距離を算出できれば、これ以外のセンサ（広角レンズを有するカメラ又はＴＯＦカメラなど）でもよい。

【0022】

また、センサ１２ａ，１２ｂ，・・・，１２ｎは、同時に雨や日差しなどの環境外乱の影響を受けないように配置されていることが望ましい。これにより、例えば、降雨時にセンサ１２ａのレンズに雨滴が付着した場合でも、進行方向の逆向き又は下向きのセンサ１２ｂのレンズには雨滴が付着しにくい。このため、センサ１２ａが取得した情報が雨滴の影響で不鮮明であっても、センサ１２ｂが取得した情報は雨滴の影響を受けにくい。また、日差しの影響でセンサ１２ａの情報が不鮮明であっても、センサ１２ｂが取得した情報は鮮明である可能性がある。

【0023】

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

【0024】

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

【0025】

メモリ１６は、情報処理装置１３の主記憶装置（メインメモリ）及びストレージなどの補助記憶装置を含む。メモリ１６には、本実施形態に係る各機能を実現するコンピュータープログラム、テーブル、ファイル等の情報が記録されている。メモリ１６として、半導体メモリやハードディスク、ＳＳＤ（Solid State Drive）等の記録装置、又はＩＣカード、光ディスク等の記録媒体を用いることができる。センサ処理部１４は、メモリ１６に格納された情報データ及び取得時刻に基づいて、様々な情報処理を行う。この情報処理では、例えば、中間情報が作成され、メモリ１６に保存される。中間情報は、センサ処理部１４による処理の他、制御部１５などの判断や処理に利用してもよい。

【0026】

バス１８は、ＩＥＢＵＳ（Inter Equipment Bus）やＬＩＮ（Local Interconnect Network）やＣＡＮ（Controller Area Network）などで構成できる。

【0027】

外部記憶装置１９は、地図情報を含む移動体１００が走行する環境の情報を記憶する。外部記憶装置１９に記憶される情報は、例えば、走行環境にある静止物（木、建物、道路、車線、信号、標識、路面ペイント、路端など）の形状や位置である。外部記憶装置１９のそれぞれの情報を数式で表してもよい。例えば、線情報を複数点で構成せず、線の傾きと切片のみでよい。また、外部記憶装置１９の情報（例えば静止物の種類など）を区別せずに、点群で表してもよい。点群は、３Ｄ（ｘ，ｙ，ｚ）、４Ｄ（ｘ，ｙ，ｚ，色）などで表してもよい。最終的に、移動体１００の現在位置から走行環境（周囲の情報）を検出し、マップマッチングができれば、外部記憶装置１９の情報は、どのような形式で保存しておいてもよい。

【0028】

制御部１５から取得開始の指令を受けたとき、外部記憶装置１９が情報をメモリ１６に送る。外部記憶装置１９は、移動体１００に設置されている場合には、バス１８を介して情報をメモリ１６に送受信する。一方、外部記憶装置１９が移動体１００に設置されていない場合、位置推定装置１と外部記憶装置１９との間の情報の送受信は、信号受付部２で行う。この通信は、例えば、ＬＡＮ（Local Area Network）、ＷＡＮ（Wide Area Network）等により行うことができる。

【0029】

センサ処理部１４は、センサ１２で取得された情報を処理する。センサ処理部１４は、例えば、移動体１００の走行中にセンサ１２が取得した情報を処理して、障害物を検知する。また、センサ処理部１４は、例えば、移動体１００の走行中にセンサ１２が取得した情報を処理して、事前に定められたランドマークを認識する。

【0030】

センサ処理部１４は、外部記憶装置１９及びメモリ１６に格納された移動体１００の現在位置とセンサ１２の内部パラメータとに基づいて、センサ１２で取得された情報を基に外部記憶装置１９の情報（例えば点群データ）を取得する。

【0031】

センサ処理部１４は、センサ１２で取得された情報に基づいて移動体の複数の位置候補を特定し、その複数の位置候補と移動体１００の移動速度に基づいて移動体１００の位置を推定する。

【0032】

センサ処理部１４は、移動体１００の走行中にセンサ１２が取得した情報を処理して、移動体１００の位置を推定してもよい。例えば、センサ処理部１４は、センサ１２が時系列に取得した情報で移動体１００の移動量を算出し、過去の位置に移動量を加算して現在位置を推定する。センサ処理部１４は、時系列に取得した情報のそれぞれから特徴を抽出してもよい。センサ処理部１４は、さらに、次回以降の情報で同じ特徴を抽出する。そして、センサ処理部１４は、特徴のトラッキングにより移動体１００の移動量を算出する。

【0033】

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

【0034】

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

【0035】

［センサ処理部の内部構成］
次に、移動体１００に設置されたセンサ処理部１４の構成を説明する。
図２は、センサ処理部１４の内部構成例を示すブロック図である。
センサ処理部１４は、入出力部１４１、現在位置推定部１４２、点群データ取得部１４３、フィルター部１４４、データ点数調整部１４５、マッチング部１４６、障害物検知部１４７、及び現在位置修正部１４８を有する。

【0036】

入出力部１４１は、センサ処理部１４にて移動体１００の位置修正を行うための必要な情報の入出力をする。例えば、入出力部１４１は、メモリ１６に格納されたセンサ１２ａ，１２ｂ，…，１２ｎで取得した走行環境の情報を読み出す。また、入出力部１４１は、メモリ１６に登録された情報抽出処理（外部記憶装置）（図３のＳ５）で外部記憶装置１９から情報を取得する。外部記憶装置１９から取得する情報量が大きく、メモリ１６に格納できない場合、現在位置推定処理（図３のＳ３）で推定した移動体１００の現在位置に近い情報だけを取得してもよい。また、現在位置推定処理で推定した移動体１００の現在位置から見たときのある領域に入っている情報だけを取得してもよい。

【0037】

現在位置推定部１４２は、自己の現在の位置を仮に推定する。現在位置推定部１４２が推定する位置は、絶対座標における位置（緯度、経度）に限らず、相対座標における位置でもよい。

【0038】

点群データ取得部１４３（点群データ選択部の一例）は、入出力部１４１が取得した情報に基づいて三次元点を取得し、一時的に記録する。センサ１２ａ、１２ｂ、…、１２ｎがカメラであり、入出力部１４１が取得した情報が画像の場合、二次元画像を三次元点に変換する。変換方法は、例えば、視差画像に基づいて画像のそれぞれのピクセルを三次元点にするものであってもよい。また、センサ１２ａ～１２ｎが単眼カメラの場合、ＳＦＭ（Structure From Motion）、Ｆｌａｔ ｓｕｒｆａｃｅ ｍｏｄｅｌ、ＯＲＢ－ＳＬＡＭ、ＬＳＤ－ＳＬＡＭなどの技術を用いてもよい。点群データ取得部１４３は、これらＳＦＭやＳＬＡＭ等の技術を用いて、移動体１００の走行環境の形状を算出する。なお、センサ１２ａ～１２ｎがレーザセンサの場合、走行環境の情報を三次元点で抽出するため、他の処理は不要となる。

【0039】

フィルター部１４４は、点群データ取得部１４３で取得した三次元点をフィルタリングする。なお、処理時間を軽減させるために、フィルター部１４４において、あるエリア以内の三次元点の平均値を算出し、一つの三次元点（ボクセル）に絞る処理をしてもよい。また、コーナーやエッジなどの特徴が少ない走行環境の領域の三次元点を削除してもよい。

【0040】

また、フィルター部１４４においては、現在位置推定処理（図３のステップＳ３）で推定した移動体１００の仮位置における外部記憶装置１９の可視三次元点をフィルタリングする。例えば、フィルター部１４４は、外部記憶装置１９に保存された地図情報に含まれる情報（点群データ）のうち、現在位置推定部１４２で推定した現在の仮位置に基づきセンサ１２が走行環境の情報を取得可能な範囲を算出し、外部記憶装置１９からその取得可能な範囲内の点群データを選択する。フィルター部１４４は、点群データ選択部の一例である。

【0041】

後述するマッチング処理（図３のステップＳ１０）において、センサ１２ａ、１２ｂ、…、１２ｎで取得した情報及び外部記憶装置１９から取得した情報の数や形状が大きく異なると、マッチング（照合）が失敗する可能性が高くなる。したがって、マッチング確率の高い移動体１００の仮位置における外部記憶装置１９の可視三次元点を抽出することにより、マッチング処理において精度よくマッチングができる。なお、このように所定の精度でマッチングができた場合を、「マッチングが成功した」という。

【0042】

データ点数調整部１４５は、地図情報から選択した点群データの点数と最大処理可能点数Ｎｍａｘ（図４参照）とを比較し、比較結果に基づいて、上記選択した点群データの点数を最大処理可能点数以下の範囲で調整する。ここで、最大処理可能点数とは、位置を推定する際の許容可能な最大の処理時間で処理できる最大の点数である。

【0043】

マッチング部１４６は、点群データ取得部１４３で取得した三次元点と、フィルター部１４４で取得した可視三次元点とのマッチングを行う。ここで、マッチング部１４６は、データ点数調整部１４５で点数を調整後の点群データと、センサ１２で取得した情報の点群データとのマッチングを行う。例えばマッチング部１４６は、ＩＣＰ（Iterative Closest Point）技術を用いて、センサ１２で取得した情報を、事前に作られた走行環境の形状情報と比較するマップマッチングを行う。

【0044】

障害物検知部１４７は、移動体１００の走行中にセンサ１２が取得した情報を処理して、センサ１２の周囲の障害物を検知する。上述したフィルター部１４４は、障害物検知部１４７における障害物の検知結果を考慮して、外部記憶装置１９から点群データを選択する。

【0045】

現在位置修正部１４８は、移動体１００の現在の仮位置を修正する。現在位置修正部１４８は、マッチング部１４６のマッチング結果（センサ１２で得た点群データと外部記憶装置１９の点群データとのずれ量）に基づいて、現在位置推定部１４２で推定された自己の現在の仮位置を修正する。そして、現在位置修正部１４８は、修正した現在位置の情報を制御部１５等へ出力する。

【0046】

現在位置修正部１４８は、修正した現在位置情報を入出力部１４１に送信し、入出力部１４１は、制御部１５の指令により修正した現在位置情報をメモリ１６に送信する。また、入出力部１４１は、現在位置修正部１４８で修正した現在位置情報を制御部１５の指令で直接制御部１５に送信してもよい。

【0047】

なお、上述したマッチング部１４６が、現在位置推定部１４２及び／又は障害物検知部１４７を兼ねてもよい。

【0048】

さらに、上述の説明においては、図２の記載に従って、入出力部１４１、現在位置推定部１４２、点群データ取得部１４３、フィルター部１４４、データ点数調整部１４５、マッチング部１４６、障害物検知部１４７、及び現在位置修正部１４８がセンサ処理部１４に含まれるものとしたが、本発明は、これに限定されるものではない。入出力部１４１、現在位置推定部１４２、点群データ取得部１４３、フィルター部１４４、データ点数調整部１４５、マッチング部１４６、障害物検知部１４７、及び現在位置修正部１４８がセンサ処理部１４に含まれない独立の構成要素であってもよい。

【0049】

［センサ処理部による情報処理］
次に、センサ処理部１４が実行する情報処理の手順について図３を参照して説明する。
図３は、センサ処理部１４が実行する情報処理の手順例を示すフローチャートである。

【0050】

まず、センサ処理部１４は、センサ１２の取得範囲をメモリ１６に記録するセンサ取得範囲記録処理を実行する（Ｓ１）。例えば、センサ１２の中心から横方向と縦方向の取得可能な最大角度（画角）をメモリ１６に記録する。また、センサ１２で取得する情報の精度が対象物までの距離に依存するため、精度が所定値よりも悪化しない最大距離（レンジ）をメモリ１６に記録する。この処理は、センサ処理部１４が本フローチャートを開始する前に予め実施しておいてもよい。

【0051】

次いで、センサ処理部１４は、情報処理装置１３の演算速度に基づいて算出された最大点数をメモリ１６に記録する最大点数記録処理を実行する（Ｓ２）。この最大点数記録処理の詳細については図４により後述する。

【0052】

次いで、現在位置推定部１４２は、信号受付部２から受信された情報に基づいて現在の仮位置を推定する現在位置推定処理を実行する（Ｓ３）。また、現在位置推定処理は、オドメトリやランドマークマッチングなどのセンサ１２で取得した情報に基づいて現在の仮位置を推定してもよい。ランドマークマッチングでは、センサ１２で取得した情報と外部記憶装置１９から取得したランドマークの情報とを照合する。さらに、現在位置推定処理は、前述した様々な位置推定方式（オドメトリ、ランドマークマッチング、信号受付部２から受信された情報など）の融合結果でもよい。例えば、様々な位置推定方式をカルマンフィルタで融合した結果に基づいて、現在位置を予測してもよい。最終的に、移動体１００の現在の仮位置を推定できれば、どんなセンサや方式を用いてもよい。

【0053】

次いで、点群データ取得部１４３は、センサ１２で移動体１００の走行環境の情報（周囲の情報）を抽出する情報抽出処理（センサ）を実行する（Ｓ４）。簡単のため、本実施形態では、この情報抽出処理（センサ）で抽出する情報を点群とする。なお、この例に限らず、情報として数式（関数、曲線）を用いたり、色（画像マッチング）を用いたりする方法も考えられる。

【0054】

次いで、点群データ取得部１４３は、現在位置推定処理（Ｓ３）で得られた現在の仮位置に基づいて、外部記憶装置１９から移動体１００の周囲の情報を抽出する情報抽出処理（外部記憶装置）を実行する（Ｓ５）。簡単のため、本実施形態では、この情報抽出処理（外部記憶装置）で抽出した情報を点群とし、抽出した点数をＮとする。

【0055】

次いで、センサ処理部１４は、最大点数記録処理（Ｓ２）とセンサ取得範囲記録処理（Ｓ１）で記録した最大点数（最大処理可能点数）とセンサ取得範囲を、メモリ１６から抽出するメモリ参照処理を実行する（Ｓ６）。ここで、抽出した最大処理可能点数をＮｍａｘとする。

【0056】

次いで、データ点数調整部１４５は、点群の点数Ｎと最大処理可能点数Ｎｍａｘを比較する点数確認処理を実行する（Ｓ７）。ここで、データ点数調整部１４５は、Ｎ＜Ｎｍａｘの場合に、ステップＳ８の点数増加処理に進み、Ｎ≧Ｎｍａｘの場合に、ステップＳ９の点数低減処理に進む。

【0057】

次いで、データ点数調整部１４５は、Ｎ＜Ｎｍａｘの状態のとき、点数Ｎを最大処理可能点数Ｎｍａｘまで増加させる（Ｓ８）。この点数増加処理の詳細については図８により後述する。

【0058】

また、データ点数調整部１４５は、Ｎ≧Ｎｍａｘの状態のとき、点数Ｎを最大処理可能点数Ｎｍａｘまで低減させる（Ｓ９）。この点数低減処理の詳細については図７により後述する。

【0059】

次いで、ステップＳ８又はＳ９の処理後、マッチング部１４６は、ステップＳ４の情報抽出処理（センサ）においてセンサ１２で抽出した情報（点群データ）と、ステップＳ５の情報抽出処理（外部記憶装置）において外部記憶装置１９から抽出した情報（点群データ）をマッチングする（Ｓ１０）。

【0060】

次いで、障害物検知部１４７は、移動体１００の周囲の障害物（他車、歩行者、移動の妨げになるものなど）の３次元形状とその位置を検知する障害物検知処理を実行する（Ｓ１１）。

【0061】

障害物検知処理では、センサ１２で取得された走行環境の情報に基づいて障害物を検知する。例えば、センサ１２が画像取得装置（カメラ）の場合、障害物検知部１４７は、画像処理技術や深層学習技術を用いて障害物を検知する。また、障害物検知処理では、信号受付部２が受信した情報に基づいて検知してもよい。例えば、移動体１００の走行環境に設置された監視カメラで障害物を検知し、検知結果と移動体１００に対する監視カメラの位置を信号受付部２に送信する。最終的に、ステップＳ４の情報抽出処理（センサ）で抽出した情報と、ステップＳ５の情報抽出処理（外部記憶装置）で抽出した情報とのマッチングの妨げになりそうな障害物を検知すればよい。

【0062】

次いで、マッチング部１４６は、ステップＳ１０のマッチング処理を実行した後、障害物等によってマッチングできなかった情報と領域を算出するマッチング不可領域算出処理を実行する（Ｓ１２）。このマッチング不可領域算出処理の詳細については図５及び図６により後述する。

【0063】

次いで、マッチング部１４６は、ステップＳ１２のマッチング不可領域算出処理で算出したマッチングできなかった領域をメモリ１６に記録するマッチング負荷領域記録処理を実行する（Ｓ１３）。

【0064】

次いで、現在位置修正部１４８は、ステップＳ１０のマッチング処理の結果を用いて、ステップＳ３の現在位置推定処理で仮に推定した現在位置を修正する現在位置修正処理を実行する（Ｓ１４）。

【0065】

次いで、センサ処理部１４は、本フローチャートの一連の処理を終了するかどうかを判定する（Ｓ１５）。判定基準は、例えば、事前に定められた位置推定の回数や、移動体１００の走行距離や、移動体１００の現在位置や、信号受付部２が受信した終了指令である。センサ処理部１４は、終了判定した場合（Ｓ１５のＹＥＳ）、本フローチャートの一連の処理を終了する。また、センサ処理部１４は、終了判定しなかった場合（Ｓ１５のＮＯ）、ステップＳ３の現在位置推定処理に戻る。

【0066】

［最大点数記録処理（最大点数算出）］
ここで、ステップＳ２の最大点数記録処理における最大点数算出の詳細について図４を参照して説明する。

【0067】

図４は、ステップＳ２の最大点数記録処理における最大点数算出方法を示すグラフである。図４において、関数Ｆ４００は、点群の点数と、その点数での処理時間（ｍｓ）との関係を表す関数である。

【0068】

情報処理装置１３の演算速度は、ステップＳ４の情報抽出処理（センサ）で抽出した点数と、ステップＳ５の情報抽出処理（外部記憶装置）で抽出した点数に依存する。関数Ｆ４００の求め方は、まず複数の異なる点数でステップＳ１０のマッチング処理を実行し、それぞれの点数で要する時間（以下「処理時間」）を算出し、それぞれの点数と処理時間の組み合わせ４０１（プロットデータ）を求める。次に、求められた複数の組み合わせ４０１に多項式やｓｐｌｉｎｅを適用して、補間処理を行う。正確に関数Ｆ４００を求めるために、本実施形態の位置推定方法を実際に実行するときに用いる情報処理装置１３で組み合わせ４０１を算出することが望ましい。

【0069】

なお、本実施形態の位置推定方法を実際に実行するときの情報処理装置１３で組み合わせ４０１を算出することが難しい場合、別の情報処理装置で関数（Ｆ’）を算出する。そして、本実施形態の位置推定方法を実際に実行するときの情報処理装置１３と、別の情報処理装置との間の演算速度の差Ｓに基づいて、関数Ｆ＝ｆ（Ｆ’，Ｓ）を求めてもよい。

【0070】

最大処理時間Ｔｍａｘ４０２は、位置推定を行う際に許容可能な最大の処理時間である。高精度な位置推定を実現するには、所定の最大周期以内に位置推定の実施が必要であり、所定の最大周期に基づいて最大処理時間Ｔｍａｘ４０２を算出する。例えば、本実施形態の位置推定方法を実行するために必要な最大周期が１０Ｈｚの場合、Ｔｍａｘ＝１００ｍｓとする。最大処理可能点数Ｎｍａｘ４０３は、最大処理時間Ｔｍａｘ４０２を関数Ｆ４００に代入したときに得られる点数である。なお、図４では、予め関数Ｆを求めて記憶しておいたが、この例に限らない。例えば、点群の点数とその点数での処理時間との関係を登録した参照テーブルを作成しておき、参照テーブルにない点数については補間により処理時間を求めるようにしてもよい。

【0071】

［マッチング不可領域算出処理とマッチング不可領域記録処理］
次に、ステップＳ１２のマッチング不可領域算出処理と、ステップＳ１３のマッチング不可領域記録処理について、図５及び図６を参照して説明する。

【0072】

（障害物がない場合）
まず、移動体１００の周囲に障害物がない場合について図５を参照して説明する。
図５は、マッチング不可領域算出処理とマッチング不可領域記録処理の例（障害物なし）を示す図である。一点鎖線は、移動体１００に設置されたセンサ１２が情報を取得可能な範囲（例えばカメラの撮影範囲（画角））を示している。なお、図５では、センサ１２の情報取得可能範囲を、移動体１００、ランドマーク５００ａ、及びランドマーク５００ｂを俯瞰した状態で表している。

【0073】

図５において、ランドマーク５００ａとランドマーク５００ｂは、移動体１００の走行環境（周囲）にある。対象物が走行環境にあるとは、対象物がセンサ１２の情報取得範囲に含まれると言い替えることもできる。

【0074】

情報５０１は、移動体１００の現在位置（仮）において情報抽出処理（センサ）（図３のＳ４）でランドマーク５００ａとランドマーク５００ｂから抽出した情報（例えば点群）である。
情報５０２は、移動体１００の現在位置（仮）に基づいて情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した走行環境の情報（例えば点群）である。

【0075】

マッチング結果５０３は、センサ１２で取得した情報５０１と、外部記憶装置１９から抽出した情報５０２とを用いてマッチング処理（Ｓ１０）を実行して得られた結果である。図５の例では、移動体１００の周囲に他車や歩行者のような障害物がないため、情報５０１と情報５０２のマッチングが良好に実行できている。そのため、マッチング不可領域が存在しない。

【0076】

（障害物がある場合）
次に、移動体１００の周囲に障害物がある場合について図６を参照して説明する。
図６は、マッチング不可領域算出処理とマッチング不可領域記録処理の例（障害物あり）を示す図である。

【0077】

図６の例では、一点鎖線で示すセンサ１２の情報取得範囲内に存在する障害物６１０（例えば通行人）の影響でランドマーク５００ｂの情報抽出ができなくなり、情報抽出処理（センサ）（Ｓ４）で抽出した情報が情報５０１ｂとなる。情報５０１ｂには、ランドマーク５００ａから得られた情報及び障害物６１０の情報が含まれる。

【0078】

したがって、センサ１２で取得した情報５０１ｂと、外部記憶装置１９から抽出した情報５０２とを用いてマッチング処理（Ｓ１０）した場合、マッチング結果５０３ｂが出力される。外部記憶装置１９から情報抽出処理（外部記憶装置）（Ｓ５）で抽出した情報５０２の中には、マッチングできなかった情報がある。マッチング部１４６は、マッチング不可領域算出処理（Ｓ１２）において、マッチングできなかった情報の複数の３次元位置を、一点鎖線で示した３次元領域５０４ｂ（マッチング不可領域）で表す。そして、マッチング部１４６は、その３次元領域５０４ｂの位置（例えば移動体１００からの距離、又は絶対位置）、長さ（奥行）、高さ（Ｈ）、幅（Ｗ）の情報を、マッチング不可領域記録処理（Ｓ１３）においてメモリ１６に記録する。

【0079】

なお、簡単のため、３次元領域５０４ｂを立方体として説明したが、３次元領域５０４ｂを球や円筒で表してもよい。最終的に、マッチングできなかった情報が入っていれば、３次元領域５０４ｂはどのような３次元形状でもよい。また、障害物検知部１４７による障害物検知処理（Ｓ１１）を用いて、３次元領域５０４ｂを求めてもよい。障害物検知処理から出力された障害物６１０の形状と位置に基づいて、３次元領域５０４ｂの３次元形状と位置を設定する。障害物検知処理によるマッチング不可領域算出処理（Ｓ１２）の具体例は、図１０及び図１１により後述する。

【0080】

簡単のため、移動物（障害物６１０）を用いてマッチング不可領域算出処理（Ｓ１２）を説明したが、障害物以外の原因でマッチング処理（Ｓ１０）が失敗することがある。例えば、工事や季節で環境が変わり、外部記憶装置１９に情報が登録されたときとマッチング時とで環境が異なる場合がある。したがって、情報抽出処理（センサ）（Ｓ４）で抽出した情報と情報抽出処理（外部記憶装置）（Ｓ５）で抽出した情報をマッチング処理（Ｓ１０）でマッチングを行う際、マッチングできない領域があるが、上記説明したとおり本発明によりマッチングできなかった領域の検出が可能である。

【0081】

［点数低減処理］
次に、ステップＳ９の点数低減処理の詳細について図７を参照して説明する。
図７は、データ点数調整部１４５による点数低減処理の詳細を示す図である。
図７において、ランドマーク７０１ａ，７０１ｂ，７０１ｃは、移動体１００の走行環境（周囲）にあるランドマークであり、各ランドマークの情報（点群データ）が外部記憶装置１９に登録されている。

【0082】

情報７０２は、移動体１００の現在位置に基づいて外部記憶装置１９から抽出した情報である。ここでは、簡単のため情報７０２を点群とし、情報７０２に含まれる点群の点数Ｎを、最大処理可能点数Ｎｍａｘ４０３（図４参照）よりも大きいとする（Ｎ＞Ｎｍａｘ）。

【0083】

情報７０２の点数Ｎは、最大処理可能点数Ｎｍａｘ４０３よりも大きいため、最大処理時間Ｔｍａｘ４０２以内に処理を完了できない。したがって、情報７０２の点数Ｎを最大処理可能点数Ｎｍａｘ４０３まで減らす必要がある。ここで、ランダムに点数を減らすか、又はボクセルを用いて点数を減らす。また、移動体１００の現在位置でマッチングできそうな情報だけを残し、他の情報を減らしてもよい。最終的に、点数Ｎが、最大処理可能点数Ｎｍａｘ４０３より小さくなれば、どのような減らし方でもよい。

【0084】

情報７０３は、外部記憶装置１９から抽出した情報７０２の点数Ｎを、最大処理可能点数Ｎｍａｘ４０３まで減らした後の情報（Ｎ＝Ｎｍａｘ）を示している。情報７０３では、情報７０２に含まれていたランドマーク７０１ａ，７０１ｂ，７０１ｃの点群の点数が減少している。

【0085】

［点数増加処理］
次に、ステップＳ８の点数増加処理の詳細について図８を参照して説明する。
図８は、データ点数調整部１４５による点数増加処理の詳細を示す図である。
図８において、情報７０２ｂは、移動体１００の現在位置に基づいて外部記憶装置１９から抽出した情報である。ここでは、簡単のため情報７０２ｂを点群とし、情報７０２ｂに含まれる点群の点数Ｎｂを、最大処理可能点数Ｎｍａｘ４０３（図４参照）よりも大きいとする（Ｎｂ＞Ｎｍａｘ）。

【0086】

領域７０４は、ステップＳ１１のマッチング不可領域算出処理で算出されたマッチング不可領域である。
情報７０３ｂは、外部記憶装置１９から抽出した情報７０２ｂの点数Ｎｂを、最大処理可能点数Ｎｍａｘ４０３まで減らした後の情報であり、その点数をＮｂ’（Ｎｂ’＝Ｎｍａｘ）とする。

【0087】

一方、情報７０２ｂには、ステップＳ１２のマッチング不可領域算出処理で算出された領域７０４が含まれている。この領域７０４の中に入っている情報は、マッチング処理（Ｓ１０）を実施するときに使わない。このため、情報７０３ｂに含まれていた領域７０４の中の情報（点群）が削除され、点数Ｎｂ’が点数Ｎｃになる（Ｎｃ＜Ｎｂ’）。Ｎｃ＜Ｎｍａｘのため、マッチング処理（Ｓ１０）を行う際、処理時間が最大処理時間Ｔｍａｘ４０２よりも短くなる。

【0088】

それゆえ、逆に点数Ｎｃを最大処理可能点数Ｎｍａｘまで増やすことが可能になる。そこで、点数増加処理（Ｓ８）で情報７０２ｂから領域７０４外で選択されていない点を選択し、情報７０３ｂから点数を増やす。情報７０５は、点数Ｎｃを最大処理可能点数Ｎｍａｘ４０３まで増やした後の情報である。また、点数増加処理（Ｓ８）で点群を増やす場合、データ点数調整部１４５は、点数低減処理（Ｓ９）と同じ基準で、ランダムやボクセル、マッチングできそうな点群の検索によって点群を増やす。情報７０５内の斜線を付した点群が、情報７０３ｂから増加した点群である。

【0089】

［自己位置推定処理の例］
次に、センサ処理部１４による自己位置推定処理の具体例について図９～図１４を参照して説明する。

【0090】

図９は、センサ処理部１４による自己位置推定処理の第１の例を示す図である。説明を簡単にするため、移動体１００が動いていないとする。

【0091】

図９において、ランドマーク９０１ａとランドマーク９０１ｂは、移動体１００の走行環境（周囲）にあるランドマークである。各ランドマークの情報（点群データ）が外部記憶装置１９に登録されている。
情報９０２ａは、移動体１００の現在の仮位置において情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク９０１ａとランドマーク９０１ｂの情報である。簡単のため、情報９０２ａを点群とする。

【0092】

情報９０３ａは、移動体１００の現在の仮位置に基づいて情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した、移動体１００の走行環境の情報である。情報９０３ａに含まれる点群の点数を、点数Ｎａとする。マッチング部１４６は、メモリ参照処理（Ｓ６）で事前にメモリ１６に登録された最大処理可能点数Ｎｍａｘ４０３を参照し、点数確認処理（Ｓ７）において点数Ｎａと最大処理可能点数Ｎｍａｘ４０３を比較する。説明のため、図９の例ではＮａ＜Ｎｍａｘとする。

【0093】

マッチング結果９０４ａは、マッチング処理（Ｓ１０）においてセンサ１２で取得した情報９０２ａと、外部記憶装置１９から抽出した情報９０３ａとをマッチングした結果である。現在位置修正部１４８は、マッチング結果９０４ａに基づいて、現在位置修正処理（Ｓ１４）において仮に推定した現在位置を修正する。また、図９の例では、マッチングできなかった領域がなかったとする（マッチングＯＫ）。

【0094】

図１０は、センサ処理部１４による自己位置推定処理の第２の例を示す図であり、図９の次のタイミングにおける状態を示している。このタイミングの間隔は、例えばセンサ処理部１４が情報処理を実行する周期である。

【0095】

図１０において、障害物１００５は、移動体１００に設置されたセンサ１２の取得範囲内にある障害物である。
情報９０２ｂは、移動体１００の現在の仮位置において情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク９０１ａと障害物１００５の情報である。

【0096】

情報９０３ｂは、移動体１００の現在の仮位置に基づいて情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した、移動体１００の走行環境の情報である。情報９０３ｂに含まれる点群の点数Ｎｂを、最大処理可能点数Ｎｍａｘ４０３よりも小さいとする（Ｎｂ＜Ｎｍａｘ）。

【0097】

マッチング結果９０４ｂは、マッチング処理（Ｓ１０）においてセンサで取得した情報９０２ｂと、外部記憶装置１９から抽出した情報９０３ｂをマッチングした結果である。この場合、障害物１００５の影響でマッチングできなかった領域があり、マッチング不可領域算出処理（Ｓ１２）で該当する領域９０６ｂが算出される。したがって、領域９０６ｂの形状と位置などの情報が、マッチング不可領域記録処理（Ｓ１３）においてメモリ１６に登録される（マッチングＮＧ）。

【0098】

図１１は、センサ処理部１４による自己位置推定処理の第３の例を示す図であり、図１０の次のタイミングにおける状態を示している。

【0099】

図１１において、情報９０２ｃは、移動体１００の現在の仮位置において情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク９０１ａと障害物１００５の情報である。すなわち、情報９０２ｃは、情報９０２ｂと同じである。

【0100】

情報９０３ｃは、移動体１００の現在の仮位置に基づいて情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した、移動体１００の走行環境の情報である。情報９０３ｃに含まれる点群の点数Ｎｃを、最大処理可能点数Ｎｍａｘ４０３よりも小さいとする（Ｎｃ＜Ｎｍａｘ）。

【0101】

データ点数調整部１４５は、メモリ参照処理（Ｓ６）においてメモリ１６に登録された領域９０６ｂ（マッチング不可領域）を参照する。マッチング部１４６が、一つ前のタイミングでマッチング処理（Ｓ１０）を行った後、マッチング不可領域算出処理（Ｓ１２）において領域９０６ｂを出力している。そのため、データ点数調整部１４５は、今回のマッチング処理を実施する前に、点数低減処理（Ｓ９）で領域９０６ｂの中にある情報を削除する。それにより、情報９０３ｃの点数Ｎｃが点数Ｎｃ’に減少する。Ｎｃ’＜Ｎｍａｘであるため、領域９０６ｂ外の点群を最大処理可能点数Ｎｍａｘ４０３まで増やし、マッチング処理を実施する。したがって、マッチング結果９０４ｃが示すとおり、障害物１００５による影響が軽減され、精度よくかつ最大処理時間Ｔｍａｘ４０２以内にマッチング処理が完了できる。

【0102】

図１２は、センサ処理部１４による自己位置推定処理の第４の例を示す図であり、図１１の次のタイミングにおける状態を示している。このタイミングでは、障害物１００５としての通行人が、センサ１２の情報取得可能範囲の内側から外側に移動している。

【0103】

図１２において、情報９０２ｄは、移動体１００の現在の仮位置において情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク９０１ａとランドマーク９０１ｂの情報である。すなわち、情報９０２ｄは、図９の情報９０２ａと同じである。

【0104】

情報９０３ｄは、移動体１００の現在の仮位置に基づいて情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した、移動体１００の走行環境の情報である。情報９０３ｄに含まれる点群の点数Ｎｄを、最大処理可能点数Ｎｍａｘ４０３よりも小さいとする（Ｎｄ＜Ｎｍａｘ）。

【0105】

データ点数調整部１４５は、図１１のタイミングと同じく、マッチング処理（Ｓ１０）を実施する前に、点数低減処理（Ｓ９）で領域９０６ｂ（マッチング不可領域）の中にある情報を削除し、情報９０３ｄの点数Ｎｄが点数Ｎｄ’に減少する。また、Ｎｄ’＜Ｎｍａｘであるため、領域９０６ｂ外の点群を最大処理可能点数Ｎｍａｘ４０３まで増やし、マッチング処理を実施する。この場合、マッチング結果９０４ｄに示すとおり、情報９０２ｄにおけるランドマーク９０１ｂの情報は、情報９０３ｄから領域９０６を削除した情報と一致しない。

【0106】

一方、このタイミングでは障害物１００５がセンサ１２の情報取得範囲外にあるため、領域９０６ｂが不要となる。以下、図１３を参照して外部記憶装置１９の情報から領域９０６ｂを削除する方法について説明する。

【0107】

図１３は、センサ処理部１４による自己位置推定処理の第５の例を示す図であり、図１２の次のタイミングにおける状態を示している。障害物１００５としての通行人は、センサ１２の情報取得可能範囲外のままである。

【0108】

図１３において、情報９０２eは、移動体１００の現在の仮位置において情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク９０１ａとランドマーク９０１ｂの情報である。すなわち、情報９０２ｅは、図１２の情報９０２ｄと同じである。

【0109】

情報９０２ｅは、図１２の情報９０２ｄと同じである。
情報９０３eは、移動体１００の現在の仮位置に基づいて情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した、移動体１００の走行環境の情報である。情報９０３eに含まれる点群の点数Ｎｅを、最大処理可能点数Ｎｍａｘ４０３よりも小さいとする（Ｎｅ＜Ｎｍａｘ）。

【0110】

センサ１２の情報取得範囲内に入っていた障害物１００５が、時系列にセンサ１２の情報取得範囲外となる確率がある。一方、障害物１００５がセンサ１２の情報取得範囲内に留まっている確率もあるため、このタイミングで領域９０６ｂ（マッチング不可領域）を削除せず、時系列（段階的）にマッチング不可領域算出処理（Ｓ１２）で領域９０６ｂの大きさを小さくする。図１３の場合、障害物１００５が図１２から引き続きセンサ１２の情報取得範囲外である。

【0111】

領域９０６ｅは、領域９０６ｂを縮小した後の領域を表す。点数低減処理（Ｓ９）で領域９０６ｅ（マッチング不可領域）の中にある情報が削除され、情報９０３ｅの点数Ｎｅが点数Ｎｅ’に減少する。そして、データ点数調整部１４５により、領域９０６ｅ（含まれる点群の点数）を考慮し、情報９０３ｅの点数Ｎｅを点数増加処理（Ｓ８）で最大処理可能点数Ｎｍａｘ４０３まで増やした上で、マッチング部１４６でマッチング処理（Ｓ１０）を実施する。その後、マッチング結果９０４ｅが得られる。これにより、領域９０６ｅの外側では、領域９０６ｂであった場所でもマッチングが可能となる。

【0112】

マッチング不可領域算出処理（Ｓ１２）で領域を小さくする場合、時系列に一定のパラメータＫ（０＜Ｋ＜１）を用いて小さくする。例えば、領域９０６ｂの長さ、高さ、幅を、Ｌｂ、Ｈｂ、Ｗｂとすると、マッチング不可領域算出処理（Ｓ１２）で領域９０６ｅの大きさを、Ｌｂ＊Ｋ、Ｈｂ＊Ｋ、Ｗｂ＊Ｋとする。また、時系列に領域９０６ｅが小さくなり、領域９０６ｅに情報９０３ｅの１点も入っていない場合、領域９０６ｅを削除する。なお、領域９０６ｂの形状は、矩形に限らない。

【0113】

図１３では、想定どおり障害物１００５がセンサ１２の情報取得範囲外となった例を示したが、障害物９０５がセンサ１２の情報取得範囲内に残った場合を説明する。この場合、マッチング処理（Ｓ１０）を行った後、マッチング不可領域算出処理（Ｓ１２）でマッチングできなかった領域９０６ｅが検出されるので、マッチング不可領域算出処理で領域９０６ｅを新たに大きくする。一例として、領域９０６ｅを領域６０６程度に大きくしてもよい。

【0114】

図１４は、センサ処理部１４による自己位置推定処理の第６の例を示す図であり、図１３の次のタイミングにおける状態を示している。

【0115】

図１４において、情報９０２ｆは、移動体１００の現在の仮位置において情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク９０１ａとランドマーク９０１ｂの情報である。すなわち、情報９０２ｆは、図１３の情報９０２ｅと同じである。

【0116】

情報９０３ｆは、移動体１００の現在の仮位置に基づいて情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した、移動体１００の走行環境の情報である。情報９０３ｆに含まれる点群の点数Ｎｆを、最大処理可能点数Ｎｍａｘ４０３よりも小さいとする（Ｎｆ＜Ｎｍａｘ）。

【0117】

このタイミングで、マッチング処理の対象から除外する領域９０６ｅ（図１３）が時系列に小さくなり、完全になくなったとする。したがって、ランドマーク９０１ａとランドマーク９０１ｂに相当する点群を含んだ情報９０３ｆと、センサ１２で取得した情報９０２ｆとをステップＳ１０のマッチング処理においてマッチングし、マッチング結果９０４ｆが出力される。

【0118】

また、マッチング処理で得られた結果の表示は必須でないが、デバッグやユーザーの参考のために表示部１７により表示してもよい。また、マッチング処理の結果のみならず、マッチング処理を行う際の点数、メモリ１６に登録された最大処理可能点数Ｎｍａｘ４０３、マッチング不可領域算出処理の結果などを表示してもよい。

【0119】

［点数低減時の点群選択基準］
次に、ステップＳ９の点数低減処理を実施する場合の点群の選択基準について図１５及び図１６を参照して説明する。

【0120】

図１５は、データ点数調整部１４５が点数低減処理（Ｓ９）を実施する場合の点群データの選択基準を説明する図である。図１５上側は、センサ１２とランドマークの位置関係の例を示し、図１５下側は、外部記憶装置１９から抽出する点群を示す。説明を簡単にするため、移動体１００が動いていないとする。

【0121】

座標１５００は、移動体１００に設置されたセンサ１２を基準とする座標（ｘ、ｙ、ｚ）である。座標１５００において、センサ１２が向いている方向（この例では移動体１００の進行方向と同じ）をｚ軸、ｚ軸と直交する方向であってランドマークの高さ方向をｙ軸、ｙ軸及びｚ軸と直交する方向をｘ軸とする。

【0122】

ランドマーク１５０１は、移動体１００の走行環境にあるランドマークである。
ランドマーク１５０２は、移動体１００の走行環境にあるランドマークであり、ランドマーク１５０１に比べ、ｚ軸上において移動体１００との距離が遠い。簡単のため、図１５の移動体１００、ランドマーク１５０１、及びランドマーク１５０２のｘ軸に対する位置を一定とする。ランドマーク１５０１，１５０２が、センサ１２の情報取得範囲に含まれている。

【0123】

点群１５０３は、外部記憶装置１９に登録されたランドマーク１５０１とランドマーク１５０２の情報（点群）である。
点群１５０４は、移動体１００の現在の仮位置に基づいて情報抽出処理（外部記憶装置）（図３のＳ５）により外部記憶装置１９から抽出した、移動体１００の走行環境の情報（点群）である。点群１５０４に含まれる点群の点数をＮｔとし、点数Ｎｔは最大処理可能点数Ｎｍａｘ４０３よりも大きいとする（Ｎｔ＞Ｎｍａｘ）。

【0124】

ここで、点数増加処理（Ｓ８）と点数低減処理（Ｓ９）を実施するときの点群の選択基準として、距離に応じた優先度について説明する。

【0125】

まず、移動体１００の現在位置からセンサ１２で抽出した場合の、外部記憶装置１９に登録された点群とマッチングできそうな点群を優先的に選択する。センサ１２は、例えば、カメラやレーザセンサの場合、立体物の表面（境界線）から情報を抽出（取得）するため、立体物の裏側に情報があっても抽出できない。したがって、点数低減処理では、移動体１００の現在位置から抽出（参照）が容易なランドマーク１５０１の表側（現在位置側）の点群１５０５（黒丸で表示）と、ランドマーク１５０２の表側の点群１５０６（黒丸で表示）を抽出し、抽出が難しい点群１５０５ｂと点群１５０６ｂ（グレー表示）を削除する。これにより、点数Ｎｔが点数Ｎｔ’に減少する（Ｎｔ’＜Ｎｍａｘ）。それゆえ、ランドマーク１５０１とランドマーク１５０２の境界線を代表する点群１５０５と点群１５０６を用いて、精度よく低負荷でマッチング処理ができる。

【0126】

一方、情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した走行環境の点群１５０４を点数低減処理で減らしても、ランドマーク１５０１とランドマーク１５０２の境界線を代表する点群１５０５と点群１５０６を合わせた点数Ｎｔ’が最大処理可能点数Ｎｍａｘ４０３よりも大きい場合（Ｎｔ’＞Ｎｍａｘ）がある。このような場合の対応について説明する。

【0127】

移動体１００に設置されたセンサ１２の精度は、解像度やキャリブレーションに依存する。例えば、レーザセンサの場合、ＴＯＦ原理でレーザセンサから対象物までの距離を測定するため、対象物の反射率や環境の反射（ノイズ）の影響により、測定した距離の精度が悪化する。また、レーザセンサは、解像度や情報取得範囲が限られているため、遠方の対象物、又はあるレンジ（距離）以上の対象物の測定精度が悪くなる。カメラの場合は、画像の解像度や視差画像を作成するときのアルゴリズムにより、測定精度が大きく変わる。基本的に、対象物がカメラから遠ければ遠いほど測定精度が悪くなる。

【0128】

なお、キャリブレーションには、外部パラメータと内部パラメータがある。外部パラメータは、センサの固定位置である。内部パラメータは、センサ内のレンズ配列（歪み）、視差画像の調整パラメータなどである。

【0129】

したがって、点数Ｎｔ’をさらに減らす必要がある場合、点数低減処理においてｚ軸上で移動体１００の現在位置から遠い情報を削除していく。例えば、ランドマーク１５０２の表面の点群１５０６を削除し、移動体１００により近いランドマーク１５０１の表面の点群１５０５を残してマッチング処理を実施する。簡単のため、点群１５０５の点数を、最大処理可能点数Ｎｍａｘ４０３よりも小さいとしたが、さらに点群１５０５の点数を減らす必要があれば、前述した説明と同じように点群１５０５のうちより移動体１００から遠い点群を優先的に削除する。

【0130】

また、点数低減処理の場合を説明したが、点数増加処理で点数の調整が必要な場合でも、移動体１００により近い点群を優先的に増やし、マッチング処理を行う。

【0131】

図１６は、図１５で選択された点群を用いた自己位置推定処理の例を示す図である。
点群１５０８は、タイミングｔ０において移動体１００の現在位置から情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク１５０２の点群である。
点群１５０９は、タイミングｔ０において移動体１００の現在位置から情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク１５０１の点群である。

【0132】

情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から抽出した走行環境の点群１５０４の点数Ｎｔが、最大処理可能点数Ｎｍａｘ４０３よりも大きいとする。また、点数低減処理（Ｓ９）でランドマーク１５０１とランドマーク１５０２の境界線を代表する点群１５０５と点群１５０６を抽出したとする。また、点群１５０５と点群１５０６を合わせた点数Ｎｔ’が、まだ最大処理可能点数Ｎｍａｘ４０３よりも大きいとする。この場合、点数低減処理で優先的に移動体１００により近い点群１５０５を残し、移動体１００から点群１５０５よりも遠方にある点群１５０６を削除し、マッチング処理を行う。

【0133】

次に、タイミングｔ１で障害物１６０１（例えば通行人）がセンサ１２の情報取得範囲内に入り、移動体１００の現在位置から情報抽出処理（センサ）（Ｓ４）で、ランドマーク１５０２の点群１５０８ｂと、障害物１６０１の点群１６０１ｂを抽出する。そして、情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から走行環境の情報を抽出する前に、障害物検知処理（Ｓ１１）で障害物１６０１を検知し、マッチング不可領域算出処理（Ｓ１２）で障害物１６０１を代表する領域１５１０（マッチング不可領域）が算出されたとする。次に、情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から走行環境の点群１５０４を抽出する。

【0134】

ここで、点数Ｎｔが最大処理可能点数Ｎｍａｘ４０３よりも大きいため、点数低減処理（Ｓ９）で点数Ｎｔを最大処理可能点数Ｎｍａｘ４０３に減らす。ただし、メモリ１６に登録された障害物１６０１を代表する領域１５１０を参照し、移動体１００に近い点群１５０５（図１６下側）が領域１５１０の中に入っているため、点群１５０５の選択が不可能である。したがって、より遠方のランドマーク１５０２の境界線を代表する点群１５０６を選択し、マッチング処理を行う。

【0135】

図１６下段に、次のタイミングｔ２で、障害物１６０１がセンサ１２の情報取得範囲に入っていない場合を示す。情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から走行環境の情報を抽出する前に、障害物検知処理（Ｓ１１）で障害物検知を行うが、障害物１６０１がなくなったため障害物検知処理の出力はない。したがって、マッチング不可領域算出処理（Ｓ１２）でマッチング不可領域を出力しない。

【0136】

情報抽出処理（外部記憶装置）（Ｓ５）で外部記憶装置１９から走行環境の点群１５０４を抽出し、点数Ｎｔが最大処理可能点数Ｎｍａｘ４０３よりも大きいため、点数低減処理（Ｓ９）で点数を減らす。ただし、タイミングｔ１のときと違って、メモリ１６に登録されたマッチング不可領域はないため、移動体１００に近いランドマーク１５０１の点群１５０５を残し、より遠方のランドマーク１５０２の点群１５０６を削除する。

【0137】

最後に、外部記憶装置１９から抽出したランドマーク１５０１の点群１５０５と、移動体１００の現在位置から情報抽出処理（センサ）（Ｓ４）で抽出したランドマーク１５０２の点群１５０８ｃ及びランドマーク１５０１の点群１５０９ｃを用いて、マッチング処理を行う。

【0138】

以上のとおり、一実施形態に係る自己位置推定装置（位置推定装置１）は、センサ（センサ１２）で収集した走行環境の情報と、地図情報とを比較することで自己の位置を推定する自己位置推定装置である。この自己位置推定装置は、自己の現在の位置を仮に推定する現在位置推定部（現在位置推定部１４２）と、地図情報に含まれる点群データのうち、現在位置推定部で推定した現在の仮位置に基づきセンサが情報を取得可能な範囲を算出し、取得可能な範囲の点群データを選択する点群データ選択部（フィルター部１４４）と、地図情報から選択した点群データの点数と、位置を推定する際の許容可能な最大の処理時間で処理できる最大の点数である最大処理可能点数とを比較し、比較結果に基づいて、選択した点群データの点数を最大処理可能点数以下の範囲で調整する点数調整部（データ点数調整部１４５）と、調整後の点群データとセンサで取得した情報の点群データとのマッチングを行うマッチング部（マッチング部１４６）と、マッチング部のマッチング結果に基づいて、現在位置推定部で推定した自己の現在の仮位置を修正する現在位置修正部（現在位置修正部１４８）と、を備える。

【0139】

上記のように構成された自己位置推定装置によれば、地図情報から選択する点群データの点数を、最大処理時間に対応する最大処理可能点数以下の範囲で動的に調整できるため、位置推定の精度と処理負荷を最適化した位置推定が可能となる。

【0140】

また、本実施形態の自己位置推定装置において、点数調整部（データ点数調整部１４５）は、選択した点群データの点数が最大処理可能点数Ｎｍａｘよりも多い場合には、選択した点群データの点数を最大処理可能点数Ｎｍａｘに減らすように構成されている。

【0141】

上記構成によれば、選択した点群データの点数を最大処理可能点数Ｎｍａｘに減らすことで、処理負荷を軽減できる。

【0142】

また、本実施形態の自己位置推定装置において、点数調整部（データ点数調整部１４５）は、選択した点群データの点数が最大処理可能点数Ｎｍａｘよりも少ない場合には、選択した点群データの点数を最大処理可能点数Ｎｍａｘまで増やすように構成されている。

【0143】

上記構成によれば、選択した点群データの点数を最大処理可能点数Ｎｍａｘに増やすことで、位置推定の精度を向上させることができる。

【0144】

また、本実施形態の自己位置推定装置において、マッチング部（マッチング部１４６）は、マッチングできなかった点群データ（例えば領域９０６ｂの情報）を記憶し、記憶した点群データを、次回の位置推定の際に地図情報に含まれる点群データから選択しないように構成されている。

【0145】

上記構成によれば、マッチングできなかった点群データを、次回の位置推定の際に地図情報に含まれる点群データから選択しないため、マッチングできなかった点群データの影響を排除し、ロバスト性を向上させることができる。

【0146】

また、本実施形態の自己位置推定装置において、点数調整部（データ点数調整部１４５）は、選択した点群データの点数を最大処理可能点数Ｎｍａｘに減らす際に、選択した点群データのうち現在の仮位置からより遠い点群データ（例えば点群１５０６）を優先的に減らすように構成されている。

【0147】

上記構成によれば、現在の仮位置からより遠い点群データを優先的に減らすことにより、処理負荷を軽減しつつ、位置推定の精度の低下を最小限に抑えることができる。

【0148】

また、本実施形態の自己位置推定装置において、点数調整部（データ点数調整部１４５）は、選択した点群データの点数を最大処理可能点数Ｎｍａｘまで増やす際に、選択した点群データのうち現在の仮位置により近い点群データ（例えば点群データ１５０５）を優先的に増やすように構成されている。

【0149】

上記構成によれば、現在の仮位置からより近い点群データを優先的に増やすことにより、処理負荷の増大を最小限に抑えつつ、位置推定の精度を向上させることができる。

【0150】

また、本実施形態の自己位置推定装置において、最大処理可能点数Ｎｍａｘは、予め求めておいた異なる点数と当該点数での処理時間との対応関係（例えば関数Ｆ）に基づいて算出される点数である。このように、点数と処理時間との関係を予め求めておくことで、時系列に変化する最大処理可能点数Ｎｍａｘを随時算出することができる。

【0151】

また、本実施形態の自己位置推定装置において、マッチング部（マッチング部１４６）は、マッチングできなかった点群データを含む領域（例えば領域９０６ｂ）の位置と形状を算出して記憶するように構成されている。このように、マッチングできなかった点群データを含む領域の位置と形状を記憶しておくことで、次回のマッチングにおいて該当する点群データを排除することができる。

【0152】

また、本実施形態の自己位置推定装置において、マッチング部（マッチング部１４６）は、時系列に得られたマッチング結果に応じて、地図情報に対して時系列にマッチングできなかった領域の大きさ又は点群データの点数を調整（例えば領域９０６ｂから領域９０６ｅに縮小）して記憶するように構成されている。

【0153】

上記構成によれば、時系列にマッチングできなかった領域又は点群データを調整し、時系列のマッチング処理に反映することで、時系列の現在位置の連続性が維持される。

【0154】

なお、本発明は上述した一実施形態に限られるものではなく、特許請求の範囲に記載した本発明の要旨を逸脱しない限りにおいて、その他種々の応用例、変形例を取り得ることは勿論である。例えば、上述した一実施形態は本発明を分かりやすく説明するために位置推定装置及び移動体の構成を詳細かつ具体的に説明したものであり、必ずしも説明した全ての構成要素を備えるものに限定されない。また、一実施形態の構成の一部について、他の構成要素の追加又は置換、削除をすることも可能である。

【0155】

また、上記の各構成、機能、処理部等は、それらの一部又は全部を、例えば集積回路で設計するなどによりハードウェアで実現してもよい。ハードウェアとして、ＦＰＧＡ（Field Programmable Gate Array）やＡＳＩＣ（Application Specific Integrated Circuit）などを用いてもよい。

【0156】

また、図３に示すフローチャートにおいて、処理結果に影響を及ぼさない範囲で、複数の処理を並列的に実行したり、処理順序を変更したりしてもよい。

【0157】

また、上述した実施形態において、制御線や情報線は説明上必要と考えられるものを示しており、製品上必ずしも全ての制御線や情報線を示しているとは限らない。実際には殆ど全ての構成要素が相互に接続されていると考えてもよい。

【0158】

また、本明細書において、「平行」及び「直交」などの用語を使用する場合、各々の用語は、厳密な「平行」及び「直交」のみを意味する用語ではなく、厳格な意味での「平行」及び「直交」を含み、さらにその機能を発揮し得る範囲にある「略平行」及び「略直交」の意味をも含むものである。

【符号の説明】

【0159】

１…位置推定装置、１２，１２ａ～１２ｎ…センサ、１３…情報処理装置、１４…センサ処理部、１５…制御部、１６…メモリ、１７…表示部、１９…外部記憶装置、１００…移動体、１４１…入出力部、１４２…現在位置推定部、１４３…点群データ取得部、１４４…フィルター部、１４５…データ点数調整部、１４６…マッチング部、１４７…障害物検知部、１４８…現在位置修正部、４００…関数、４０２…最大処理時間、４０１…組み合わせ、４０３…最大処理可能点数

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】