JP7302966B2 - 移動体 - Google Patents

Description

本発明は、移動体に関する。

ロボット、自動車などの移動体がその周囲の情報を収集し、移動体の現在位置及び走行状態を推定し、移動体の走行を制御する自律走行技術及び運転支援技術が開発されている。

例えば、特許文献１には、自己位置の推定の精度を向上することを目的として、複数の三次元点のうち、距離センサからの距離が所定の距離閾値未満である複数の三次元点と、距離センサからの距離が距離閾値以上であっても特徴量が特徴量閾値以上である複数の三次元点が所定数以上となる距離区間に含まれる複数の三次元点とを抽出し、抽出した複数の三次元点と、環境地図が示す複数の三次元点とをマッチングすることで自己位置を推定する方法が開示されている。

特許文献２には、誤差の累積を抑制し、目標物になりそうなものがない単調な道路でも適切に真の現在位置を算出することを目的として、移動体の現在位置における２次元画像である観測画像と、予め用意された仮現在位置に対応する２次元画像である参照画像と、を画像マッチングにより照合し、その照合結果に基づいて、移動体の真の現在位置を算出する技術が開示されている。

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

特許文献１においては、複数の三次元点を抽出する際、適切な三次元点を必ずしも抽出できるとは限らない。

特許文献２においては、処理が必要なデータ量が多い画像マッチングを行うため、移動体に内蔵された演算装置の限られた計算能力では、十分に短い時間内に結果を示すことが困難となる場合がある。

移動体自体に内蔵された情報処理装置の処理能力等には限界があるため、処理するデータ数を減らす必要性があることが求められている。

本発明の目的は、計算能力が限られている移動体の情報処理装置を用いて、短い処理時間で、移動体の現在位置の存在確率分布の範囲が狭い状態を維持する計算をし、移動体の位置を高精度で推定することにある。

上記課題を解決するために、本発明の移動体は、周囲の情報を収集するセンサと、センサによって取得した情報を処理するセンサ処理部と、三次元点取得部と、フィルター部と、マッチング部と、自己位置補正部と、を有し、三次元点取得部は、センサから得られた三次元点を取得し、フィルター部は、マップに含まれる三次元点のうち移動体の現在の仮位置から参照可能な三次元点を求め、マッチング部は、三次元点取得部に記録されている三次元点とフィルター部が求めた参照可能な三次元点とのマッチングを行い、自己位置補正部は、マッチングに成功した三次元点を基にマップにおける移動体の現在の位置を求める。

本発明によれば、計算能力が限られている移動体の情報処理装置を用いて、短い処理時間で、移動体の現在位置の存在確率分布の範囲が狭い状態を維持する計算をし、移動体の位置を高精度で推定することができる。

本発明の一実施形態に係る移動体を示す構成図である。 図１のセンサ処理部１４における情報処理プロセスを示すフロー図である。 図１のセンサ処理部１４を示す構成図である。 図２Ａの三次元点区別ステップＳ２０４におけるセンサの検出範囲の例を示す模式図である。 図２Ａの三次元点区別ステップＳ２０４の検出誤差を示す模式図である。 本発明の実施例を示す上面図である。 移動体が２台の場合の実施例を示す上面図である。 移動体が２台であり、固定センサを用いる場合の実施例を示す上面図である。 通行人がいる場合の実施例を示す上面図である。 マッチングステップ及び位置補正ステップの条件を示す上面図である。 マッチングステップ及び位置補正ステップの手順を示すフロー図である。

本発明は、ロボットや自動車などの移動体の位置を推定する技術に関する。

以下、本発明の実施形態に係る移動体の位置推定装置について、図面を用いて説明する。

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

図１は、本発明の一実施形態に係る移動体を示す構成図である。

本図において、移動体１００は、位置推定装置１を備えている。位置推定装置１は、移動体１００の外部に存在する外部記憶装置１９と通信可能となっている。この通信は、無線が望ましい。移動体１００は、自動車、ロボットなどである。

位置推定装置１は、信号受付部２と、一台以上のセンサ１２（１２ａ、１２ｂ、…、１２ｎ）と、情報処理装置１３と、を有する。これらの構成要素は、バス１８により相互に接続されている。

情報処理装置１３は、例えば一般的なコンピュータ（電子計算機）であって、センサ１２によって取得した情報を処理するセンサ処理部１４と、センサ処理結果に基づく処理を行う制御部１５（ＣＰＵ）と、メモリ１６と、ディスプレイなどの表示部１７と、を含む。情報処理装置１３においては、センサ処理部１４及び制御部１５が所定のコンピュータプログラムを実行する。実行する内容については後述する。

信号受付部２においては、外部からの信号を受信する。信号受付部２は、例えば、世界の絶対座標で現在位置を推定するＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ：全地球測位システム）である。また、信号受付部２は、ＧＰＳよりも精度よく現在位置を推定するＲＴＫ－ＧＰＳ（Ｒｅａｌ Ｔｉｍｅ Ｋｉｎｅｍａｔｉｃ ＧＰＳ）でもよい。また、信号受付部２は、準天頂衛星システムでもよい。また、信号受付部２は、既知の位置に固定されたビーコンからの信号を受信するものでもよい。また、信号受付部２は、車輪エンコーダ、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）、ジャイロなど、相対座標で位置を推定するセンサから受信してもよい。また、信号受付部２は、走行環境の車線、標識、交通状態、立体物の形状、大きさ、高さなどの情報を受信してもよい。最終的に、移動体１００の現在位置推定や制御や認知に利用できれば、どのような方式でもよい。

センサ１２は、例えばスチルカメラまたはビデオカメラである。また、センサ１２は、単眼カメラまたは複眼カメラでもよい。また、センサ１２は、レーザセンサでもよい。

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

つぎに、センサ１２の詳細について説明する。

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

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

センサ１２が単眼カメラの場合、路面が平らであれば、画像上のピクセル位置と実際の地上位置関係（ｘ，ｙ）が一定になるため、センサ１２から特徴点までの距離を幾何学的に計算できる。センサ１２がステレオカメラの場合、画像上の特徴点までの距離をより正確に計測できる。また、センサ１２がレーザセンサの場合、より正確で遠方の情報を取得できる。

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

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

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

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

メモリ１６は、情報処理装置１３の主記憶装置（メインメモリ）およびストレージなどの補助記憶装置を含む。

センサ処理部１４は、メモリ１６に格納された情報データ及び取得時刻に基づいて、様々な情報処理を行う。この情報処理では、例えば、中間情報が作成され、メモリ１６に保存される。中間情報は、センサ処理部１４による処理の他、制御部１５などの判断や処理に利用してもよい。

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

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

制御部１５から取得開始の指令を受けたとき、外部記憶装置１９が情報をメモリ１６に送る。外部記憶装置１９は、移動体１００に設置されている場合、情報をバス１８で送受信する。

一方、外部記憶装置１９が移動体１００に設置されていない場合、移動体１００と外部記憶装置１９との接続は、信号受付部２で行う。この接続は、例えば、Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ（ＬＡＮ）、Ｗｉｄｅ Ａｒｅａ Ｎｅｔｗｏｒｋ（ＷＡＮ）等により行うことができる。

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

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

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

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

図２Ａは、図１のセンサ処理部１４における情報処理プロセスを示すフロー図である。

図２Ａにおいて、現在位置推定ステップＳ２０１は、移動体１００の仮現在位置を推定するステップである。現在位置推定ステップＳ２０１は、例えば、絶対位置を推定するＧＰＳで構成する。また、現在位置推定ステップＳ２０１は、ある基準点から相対位置を推定するオドメトリで構成してもよい。前述のＧＰＳやオドメトリに基づく位置推定手法には、位置誤差が発生するが、後述する位置補正ステップＳ２０６で補正する。

環境情報抽出ステップＳ２０２は、移動体１００に設置されているセンサ１２ａ、１２ｂ、…、１２ｎで移動体１００の周囲の環境情報を抽出するステップである。ここで、環境情報の抽出は、センサ１２ａ、１２ｂ、…、１２ｎによる移動体１００の周囲の環境のグレイスケール画像、カラー画像、三次元点等の取得を含む。また、環境情報の抽出は、センサ１２ａ、１２ｂ、…、１２ｎで取得した画像からの視差画像作成であってもよい。また、環境情報の抽出は、センサ１２ａ、１２ｂ、…、１２ｎで取得した情報に含まれる特徴の抽出であってもよい。また、環境情報の抽出は、センサ１２ａ、１２ｂ、…、１２ｎで取得した情報に含まれる線情報の抽出であってもよい。

外部記憶装置情報抽出ステップＳ２０３は、制御部１５の指令に基づいて、外部記憶装置１９から情報を取得するステップである。制御部１５の指令で、外部記憶装置１９にある情報を全て取得してもよい。また、移動体１００の周囲の情報のみを取得してもよい。

三次元点区別ステップＳ２０４は、外部記憶装置情報抽出ステップＳ２０３で抽出した外部記憶装置情報の中からマッチングの最も確率が高い三次元点を区別する。三次元点区別ステップＳ２０４については、後述する。

マッチングステップＳ２０５は、環境情報抽出ステップＳ２０２でセンサ１２ａ、１２ｂ、…、１２ｎで取得した情報を三次元点区別ステップＳ２０４で区別した三次元点とマッチングするステップである。マッチングステップＳ２０５については、後述する。

位置補正ステップＳ２０６は、マッチングステップＳ２０５の結果を用い、現在位置推定ステップＳ２０１で推定した現在位置を補正するステップである。位置補正ステップＳ２０６については、後述する。

図２Ｂは、図１のセンサ処理部１４の構成を示したものである。

本図において、センサ処理部１４は、入出力部２１０と、三次元点取得部２１１と、フィルター部２１２と、マッチング部２１３と、自己位置補正部２１４と、を有する。

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

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

フィルター部２１２においては、三次元点取得部２１１で取得三次元点を三次元点区別ステップＳ２０４でフィルタリングする。処理時間を軽減させるために、あるエリア以内の三次元点の平均値を算出し、一つの三次元点に絞る。また、コーナーやエッジなどの特徴が少ない走行環境の領域の三次元点を削除してもよい。また、フィルター部２１２においては、現在位置推定ステップＳ２０１で推定した移動体１００の仮位置における外部記憶装置１９の可視三次元点をフィルタリングする。

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

マッチング部２１３は、三次元点取得部２１１で得た三次元点とフィルター部２１２で得た可視三次元点とのマッチングをマッチングステップＳ２０５で行う。

自己位置補正部２１４では、位置補正ステップＳ２０６を行い、移動体１００の現在位置を補正する。

自己位置補正部２１４で補正した現在位置情報を入出力部２１０に送信し、制御部１５の指令で補正した現在位置情報をメモリ１６に送信する。また、制御部１５の指令で入出力部２１０から直接制御部１５に自己位置補正部２１４で補正した現在位置情報を送信してもよい。

なお、上述の説明においては、図２Ｂの記載に従って、入出力部２１０、三次元点取得部２１１、フィルター部２１２、マッチング部２１３及び自己位置補正部２１４がセンサ処理部１４に含まれるものとしたが、本発明は、これに限定されるものではなく、入出力部２１０、三次元点取得部２１１、フィルター部２１２、マッチング部２１３及び自己位置補正部２１４がセンサ処理部１４に含まれない独立の構成要素であってもよい。

つぎに、図３Ａ及び３Ｂを用いて、三次元点区別ステップＳ２０４の詳細を説明する。

図３Ａは、図２Ａの三次元点区別ステップＳ２０４におけるセンサの検出範囲の例を示す模式図である。

図３Ｂは、図２Ａの三次元点区別ステップＳ２０４の検出誤差を示す模式図である。

図３Ａにおいて、符号１００１は、外部記憶装置１９に登録された移動体１００の走行環境の座標（ｘ，ｙ）を指す。

仮現在位置（Ｘ，Ｙ）１００２は、現在位置推定ステップＳ２０１において推定された座標１００１における移動体１００の位置である。

移動体１００が室内で走行しているとする。移動体１００が仮現在位置１００２にある時、外部記憶装置１９に登録された移動体１００の周囲の情報が三次元点群１００４と三次元点群１００５と三次元点群１００６Ｌと三次元点群１００６Ｒとなるとする。簡単のため、三次元点群１００４と三次元点群１００５と三次元点群１００６Ｌと三次元点群１００６Ｒの走行環境の地面に対する高さは一定とする。図中、○印は「参照可」であることを表し、●印は「参照不可」であることを表している。

センサ１２ａ、１２ｂ、…、１２ｎの取得角度をＦＯＶとする。移動体１００の仮現在位置１００２と取得角度ＦＯＶに基づいて、三次元点群１００４は、センサ１２ａ、１２ｂ、…、１２ｎの取得角度ＦＯＶ外にあるため、参照不可となる。また、三次元点群１００５と三次元点群１００６Ｌと三次元点群１００６Ｒは、センサ１２ａ、１２ｂ、…、１２ｎの取得角ＦＯＶ内に入っているが、センサ１２ａ、１２ｂ、…、１２ｎは、それぞれの方向の最も近い情報しか得られないため、三次元点群１００５は参照可能であり、三次元点群１００６Ｌと三次元点群１００６Ｒは参照不可となってしまう。

したがって、マッチング部２１３のマッチングステップＳ２０５で移動体１００の現在位置１００２から参照不可能な三次元点１００６Ｌと三次元点１００６Ｒを用いても、マッチングができない。このため、三次元点１００６Ｌと三次元点１００６Ｒをマッチングに用いられない。

よって、フィルター部２１２は、参照不可能な三次元点１００６Ｌ、１００６Ｒを除外する。

なお、簡単のため、三次元点群１００４、三次元点群１００５、三次元点群１００６Ｌ及び三次元点群１００６Ｒについては、室内の壁、椅子、テーブル等を想定して説明したが、どのような立体物で構成してもよい。

図３Ｂにおいて、存在確率分布３０１は、移動体１００の現在位置の存在確率分布を表している。存在確率分布３０１は、例えば、Ｋａｌｍａｎ Ｆｉｌｔｅｒ、Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ、Ｂａｙｅｓｉａｎ Ｆｉｌｔｅｒのような時系列フィルターを用いて推定する。

三次元点３０２は、外部記憶装置情報抽出ステップＳ２０３で抽出した複数の三次元点である。簡単のため、三次元点３０２の複数の三次元点の地面からの高さを一定とする。

三次元点３０３は、移動体１００の現在位置から取得した走行環境にある柱の情報である。存在確率分布３０４は、柱の現在位置の存在確率分布である。

角度θ_Ｎは、移動体１００のセンサ１２の一つからの視野の中心線からの方向である。

抽出した三次元点３０６は、三次元点区別ステップＳ２０４で抽出された参照可能な三次元点であり、マッチングステップＳ２０５で利用する三次元点である。

参照不可能な三次元点３０７は、三次元点区別ステップＳ２０４で参照不可能な三次元点として区別され、マッチングステップＳ２０５で利用しない三次元点である。
抽出した三次元点３０３の存在確率分布３０４は、その範囲が移動体１００と相似形の存在確率分布３０１となるとする。

センサ１２ａ、１２ｂ、…、１２ｎの情報取得誤差が０でない場合、抽出した三次元点３０３の存在確率分布３０４が移動体１００の存在確率分布３０１より大きくなる。移動体１００のセンサ１２ａ、１２ｂ、…、１２ｎの情報取得誤差が既知であれば、存在確率分布３０１に合わせて存在確率分布３０４を算出する。例えば、センサ１２ａ、１２ｂ、…、１２ｎがステレオカメラの場合、情報取得誤差は取得した情報の距離に比例するため、その誤差を含めて存在確率分布３０４を算出する。

したがって、抽出した三次元点３０３を三次元点３０２のうち、三次元点３０２Ｌ又は三次元点３０２Ｒのいずれとマッチングすればよいか不明となる。

そのため、角度θ_Ｎと現在位置の存在確率分布３０１で三次元点３０２Ｌと三次元点３０２Ｒと区別する。

従来センサは、センサに最も近い立体物を検出し、その立体物の裏に他の立体物があっても検出できない。

そこで、本発明においては、角度θ_Ｎの方向においてセンサに最も近い立体物を探索し、抽出すると、マッチングができる確率が高くなる。

つぎに、角度θ_Ｎの設定について説明する。

所定の角度の範囲θ_Ａからθ_Ｂまでの間で角度θ_Ｎを調整し、マッチング確率が最も高い三次元点を探索する。例えば、センサの取得角度がＦＯＶの場合、θ_Ａ＝－ＦＯＶ、θ_Ｂ＝＋ＦＯＶとする。θ_Ａからθ_Ｂまでの範囲において所定の間隔ｄθ（＝θ_Ｎ－θ_Ｎ－１）を設定する。ここで、ｄθは固定値とする。ｄθは、センサの解像度に合わせて設定してもよい。

また、ｄθを移動体１００の存在確率分布３０１に合わせてもよい。例えば、自己位置の信頼性が低ければ、現在位置推定ステップＳ２０１で推定した仮の現在位置誤差が大きいため、ｄθを小さく設定する。一方、自己位置の信頼性が高ければ、現在位置推定ステップＳ２０１で推定した仮の現在位置誤差が小さいため、ｄθを大きく設定する。

また、存在確率分布３０１の大きさに基づいて、θ_Ａ及びθ_Ｂを変更してもよい。例えば、存在確率分布３０１が大きい場合、推定した現在位置の方位誤差が大きくなっている確率が高いため、θ_Ａをθ_Ａ’（＝θ_Ａ－α）とし、θ_Ｂをθ_Ｂ’（＝θ_Ｂ＋α）とする。

図４は、本発明の実施例を示したものである。

本図において、三次元点４０１は、外部記憶装置１９に登録された移動体１００の走行環境の三次元点である。

存在確率分布４０２は、移動体１００の存在確率分布である。移動体１００が走行を開始した時点においては、存在確率分布４０２が小さいものとする。

範囲４０３は、移動体１００が三次元点区別ステップＳ２０４で探索する範囲である。

この場合に、外部記憶装置情報抽出ステップＳ２０３でθ_Ａ＝－ＦＯＶ、θ_Ｂ＝＋ＦＯＶと設定し、範囲４０３内の移動体１００の周囲の外部記憶装置１９の情報を抽出する。そして、三次元点区別ステップＳ２０４において参照可と参照不可の三次元点を区別する。

移動体１００が廊下のような特徴の少ない環境を走行する場合、マッチングステップＳ２０５を行っても、位置の補正ができない。外部記憶装置１９に登録された情報に基づいて移動体１００が特徴の少ない環境を走行していることを制御部１５が認識した場合、制御部１５の指令で移動体１００が移動し、特徴の探索を開始する。特徴の探索は、例えば、外部記憶装置１９に登録された情報に基づいて、移動体１００の現在位置に最も近い特徴の位置を抽出する。次に、制御部１５の指令で移動体１００がその特徴の検出可能な位置に移動し、マッチングステップＳ２０５と位置補正ステップＳ２０６で位置補正する。

移動体１００が前述の抽出した特徴の位置に移動する場合、現在位置推定ステップＳ２０１で移動体１００の推定した位置誤差が大きくなり、移動体１００の存在確率分布が存在確率分布４０４になる。そこで、外部記憶装置情報抽出ステップＳ２０３でθ_Ａをθ_Ａ’（＝θ_Ａ－α）とし、θ_Ｂをθ_Ｂ’（＝θ_Ｂ＋α）とし、移動体１００の周囲の外部記憶装置１９の情報を抽出する。そして、三次元点区別ステップＳ２０４でθ_Ａ’（＝－ＦＯＶ－α）からθ_Ｂ’ （＝＋ＦＯＶ＋α）までの範囲４０５で探索を行い、参照可能な三次元点を区別する。

位置補正ステップＳ２０６において移動体１００の現在位置を補正した後、次にマッチングステップＳ２０５を行う際には、外部記憶装置情報抽出ステップＳ２０３においてθ_Ａ＝－ＦＯＶ、θ_Ｂ＝＋ＦＯＶと設定する。この新しく設定したθ_Ａ及びθ_Ｂを用いて三次元点区別ステップＳ２０４で参照可と参照不可の三次元点を区別する。

また、移動体１００が特徴の少ない環境を走行し、最も近い特徴の位置に移動する場合、現在位置推定ステップＳ２０１で推定する位置誤差が大きくならない経路を制御部１５が選定する。例えば、現在位置推定ステップＳ２０１で移動体１００の現在位置を車輪エンコーダで推定する場合、誤差は走行した距離に比例するため、なるべく最短距離を算出する。また、移動体１００の推定した方位誤差を避けるために、なるべく回転が少なく、直進で移動できる経路を算出する。したがって、特徴の少ない環境を走行している場合、動的障害物があっても、移動体１００が避けずに、一時停止し、動的障害物が存在しなくなった後に直進移動を続ける。

図５は、２台の移動体が走行する場合である。

本図において、三次元点５０１は、外部記憶装置１９に登録された移動体１００の走行環境の三次元点である。

存在確率分布５０２は、移動体１００の現在位置の存在確率分布である。

三次元点５０３は、移動体１００が現在位置からセンサで抽出した三次元点である。

存在確率分布５０４は、三次元点５０３の存在確率分布で、存在確率分布５０２と同じ大きさとする。この場合、センサで抽出した三次元点５０３を三次元点５０１の三次元点５０５Ｒか三次元点５０５Ｌどれとマッチングすればよいか不明となり、誤ってマッチングする可能性が高い。

存在確率分布５０６は、移動体１００が走行中環境に存在する２台目の移動体１００ｂの存在確率分布である。存在確率分布５０６は、存在確率分布５０４より小さいとする。移動体１００と移動体１００ｂとは、互いの位置及び存在確率分布の情報を共有する。情報共有は、例えば、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）に接続して情報の送信及び受信をする。

移動体１００と移動体１００ｂとが情報を共有するタイミングは、三次元点区別ステップＳ２０４の直前とする。また、移動体１００と移動体１００ｂとが情報を共有するタイミングを三次元点区別ステップＳ２０４の周期より短い周期で実施してもよい。最終的に、存在確率分布の情報を必要としている三次元点区別ステップＳ２０４の周期に合わせて、最新の情報を共有できれば、周期をどのように設定してもよい。

移動体１００ｂの現在位置からセンサで移動体１００への距離５０７を算出する。存在確率分布５０６があるため、センサで算出した移動体１００への距離５０７に誤差がある。このため、移動体１００は、距離５０８と距離５０９との間に存在する。

距離５０８及び距離５０９を存在確率分布５０２と融合することにより、移動体１００の存在確率分布は、更新され、符号５１０で表される範囲となる。そして、センサで抽出した三次元点５０３の存在確率分布５０４も、更新され、符号５１１で表される狭い範囲となる。言い換えると、他の移動体である移動体１００ｂが有するセンサから周囲の情報を取得し、移動体１００が有するセンサからの周囲の情報と組み合わせて、移動体１００の現在の位置（存在確率分布５１０）を求める。

これにより、存在確率分布５１１は、存在確率分布５０４より小さくなる。そして、センサで抽出した三次元点５０３を存在確率分布５０４の範囲に入っている三次元点５０５Ｌとマッチングし、移動体１００の位置を修正する。

簡単のため、移動体１００と移動体１００ｂが止まった状態で説明したが、一台又は二台とも走行しながら上記の位置修正を実施してもよい。また、簡単のため、移動体２台を用いて説明したが、２台以上であってもよい。

図６は、２台の移動体及び固定カメラの実施例である。

本図において、存在確率分布６０１は、移動体１００の現在位置の存在確率分布である。

存在確率分布６０２は、移動体１００ｂの現在位置の存在確率分布である。存在確率分布６０２を存在確率分布６０１より大きいとする。

この場合、移動体１００ｂに設置したセンサで移動体１００への距離を算出しても、距離６０３と距離６０４との間が存在確率分布６０１より大きくなるため、２台の移動体１００、１００ｂによる位置補正が効かない。

本図においては、移動体１００の走行環境に設置された固定カメラ６０５（固定センサ）を用いることができ、固定カメラ６０５から移動体１００及び移動体１００ｂまでの距離６０６及び距離６０７が得られる。この場合に、距離６０６及び距離６０７の誤差が発生しても、移動体１００ｂは、距離６０８と距離６０９との間に存在し、移動体１００は、距離６１０と距離６１１との間に存在するはずである。よって、存在確率分布６０１が存在確率分布６１２となり、存在確率分布６０２が存在確率分布６１３となり、ともに狭い範囲となる。

存在確率分布６１２及び存在確率分布６１３はそれぞれ、存在確率分布６０１、存在確率分布６０２より小さいため、三次元点区別ステップＳ２０４での参照可能な三次元点の区別ができ、マッチングステップＳ２０５で精度よく移動体１００の位置を修正できる。

また、移動体１００ｂに設置したセンサで移動体１００への距離６１４を算出し、距離６１５と距離６１６との間で存在確率分布６１７を算出し、三次元点区別ステップＳ２０４～位置補正ステップＳ２０６で位置補正をしてもよい。存在確率分布６１７は、存在確率分布６１２よりも狭い範囲となる。言い換えると、移動体１００の走行環境に設置された固定センサから周囲の情報を取得し、移動体１００が有するセンサからの周囲の情報と組み合わせて、移動体１００の現在の位置（存在確率分布６１７）を求める。

簡単のため、移動体２台と固定カメラ１台を用いて説明したが、２台以上の移動体と複数の固定カメラを用いて位置補正を行ってもよい。

また、簡単のため、固定カメラ６０５で説明したが、移動体１００を推定し、距離を算出できるセンサであれば、カメラ以外でもよい。

図７は、通行人がいる場合である。

本図において、固定カメラ７０１は、移動体１００が走行する環境に固定されたセンサである。

存在確率分布７０２は、移動体１００の現在位置の存在確率分布である。

存在確率分布７０３は、移動体１００ｂの現在位置の存在確率分布である。

移動体１００ｂと固定カメラ７０１との間に通行人７０４等がいる場合、固定カメラ７０１による移動体１００ｂの検出は、不可能となる。言い換えると、通行人７０４等は、検出の障害となり得る。

固定カメラ７０１で移動体１００への距離７０５を算出し、図５及び図６で説明したとおり、移動体１００の存在確率分布７０２を小さくし、存在確率分布７０２が存在確率分布７０２ｂとする。

一方、移動体１００の現在位置からは、移動体１００ｂの検出が可能であり、距離７０６を算出した後、移動体の存在確率分布７０３を小さくし、存在確率分布７０３ｂを得ることができる。

次に、移動体１００ｂの現在位置を修正するために三次元点区別ステップＳ２０４、マッチングステップＳ２０５及び位置補正ステップＳ２０６に進む。

三次元点区別ステップＳ２０４で現在位置とセンサの取得範囲に基づいて、移動体１００ｂの周囲の三次元点７０７の全点を抽出できるが、通行人７０４の影響で固定カメラ７０１が移動体１００ｂを検出できなくなっている。このため、固定カメラ７０１から通行人７０４の存在と移動体１００ｂに対する通行人の方向αを移動体１００ｂに送信する。

これにより、三次元点区別ステップＳ２０４でマッチングできる確率が低い方向α以外の三次元点を抽出し、マッチングステップＳ２０５と位置補正ステップＳ２０６で位置推定を実施する。

本図においては、簡単のため、通行人一人の場合について説明したが、複数の通行人が存在してもよい。また、複数の通行人の影響により、他の移動体や固定センサを検出することや、位置補正をすることができない場合、一時的に停止し、通行人がいなくなるまで待機する。また、通行人がしばらくいなくならない場合、移動体が通行人を回避して、他の移動体や固定センサを検出できる位置まで移動してもよい。

なお、ここでは通行人について説明したが、一般に、人間以外の動物、物体等をまとめて「障害物」と呼ぶ。

まとめると、三次元点取得部が取得した三次元点に障害物の三次元点が含まれている場合には、マッチング部は、障害物の三次元点を除いた三次元点を用いてマッチングを行う。

つぎに、図８Ａ及び８Ｂを用いて、図２ＡのマッチングステップＳ２０５及び位置補正ステップＳ２０６の詳細について説明する。

図８Ａは、マッチングステップ及び位置補正ステップの条件を示す上面図である。

図８Ｂは、マッチングステップ及び位置補正ステップの手順を示すフロー図である。

図８Ａにおいて、三次元点８０１（●印）は、外部記憶装置１９に登録された移動体１００の走行環境である。

三次元点８０２（○印）は、移動体１００の現在位置からセンサで抽出した三次元点である。三次元点８０１と三次元点８０２とのマッチング方法は、例えば、ＩＣＰ Ｍａｔｃｈｉｎｇ（ＩＣＰは、Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔの略称である。）を用いる。また、三次元点８０１及び三次元点８０２を線に変換して、線マッチングを行ってもよい。最終的に、三次元点８０１及び三次元点８０２の相対位置（Ｘ，Ｙ，Ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）を出力すれば、ＩＣＰ以外のマッチング方法を適用してもよい。ここで、ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗはそれぞれ、移動体１００が前後の軸（進行方向の軸）に対して、その軸の周りに回転する角度、移動体１００の先端部が上下する角度、移動体１００の先端部が左右に振動する角度を表している。

本図において破線で示す層８０３Ｒ、８０３Ｌは、移動体１００の周囲の状態を数値化するための区分である。ここでは、簡単のため、層８０３Ｒ、８０３Ｌは、走行環境を距離と位置とに分け、それぞれ、三次元点の点群が含まれる領域として区分したものであり、それぞれの領域の特徴を数値の集合として表したものである。なお、層８０３Ｒは、移動体１００の進行方向右側に位置するものであり、層８０３Ｌは、移動体１００の進行方向左側に位置するものである。また、「位置」は、走行面の座標であり、二次元座標である。

具体的には、層８０３Ｒ、８０３Ｌは、下記式（１）に示すとおり、距離、位置、高さ、色などで構成されるものである。

層（ｎ）＝ｆ（距離，位置，高さ，色，…） …（１）
式中、右辺のかっこ内は、それぞれの層８０３Ｒ、８０３Ｌの属性を記載したものである。それぞれの層８０３Ｒ、８０３Ｌの属性は、それぞれの層８０３Ｒ、８０３Ｌに含まれる三次元点の点群を構成するそれぞれの点における距離、位置、高さ、色などに対応するそれぞれの特徴の数値（特徴量）についてその点群の平均値を算出して示したものである。言い換えると、層８０３Ｒ、８０３Ｌは、属性を変数とする。

上記式（１）についての更に具体的な例は、下記式（２）で表される。

層（ｉ）＝ｆ（距離ｄ，位置ｐ）＝ｆ（ｄ_ｉ－１＜ｄ＜ｄ_ｉ，ｐ＝Ｒ） …（２）
式中、ｉは、１以上の整数であり、それぞれの領域を表す符号である。ｄ_ｉは、移動体１００から隣の領域（例えばｉ＋１）との境界までの距離（所定の規則に従って特定される値）である。Ｒは、移動体１００の右側を表す数値である。

上記式（２）においては、属性を距離及び位置に限定して示している。また、この式は、層８０３Ｒに関するものである。

同様に、層８０３Ｌに関するものは、下記式（３）で表される。

層（ｊ）＝ｆ（距離ｄ，位置ｐ）＝ｆ（ｄ_ｊ－１＜ｄ＜ｄ_ｊ，ｐ＝Ｌ） …（３）
式中、ｊは、１以上の整数であり、それぞれの領域を表す符号である。ｄ_ｊは、移動体１００から隣の領域（例えばｊ＋１）との境界までの距離（所定の規則に従って特定される値）である。Ｌは、移動体１００の左側を表す数値である。

外部記憶装置情報抽出ステップＳ２０３においては、外部記憶装置情報を抽出し、前述のそれぞれの層に距離及び位置に基づいて外部記憶装置情報を入力する。三次元点区別ステップＳ２０４においては、それぞれの層のマッチングができる参照可能な三次元点を抽出し、環境情報抽出ステップＳ２０２においてセンサにより得られた情報をそれぞれの層に入力する。

したがって、層８０３Ｒ、８０３Ｌのそれぞれは、外部記憶装置情報とセンサで抽出した情報とを有する。このため、マッチングステップＳ２０５でそれぞれの層にＩＣＰ Ｍａｔｃｈｉｎｇをかけることにより、修正した属性の候補であって、下記式（４）で表されるものが得られる。

層（ｋ）＝（Ｘ_ｋ，Ｙ_ｋ，Ｚ_ｋ，ｒｏｌｌ_ｋ，ｐｉｔｃｈ_ｋ，ｙａｗ_ｋ，…） …（４）
式中、ｋは、１以上の整数であり、それぞれの領域を表す符号である。

位置補正ステップＳ２０６でそれぞれの層から修正した位置の候補を融合し、移動体１００の修正した位置を出力する。融合した位置は、例えば、それぞれの層で得られた位置の平均である。また、融合した位置をそれぞれの層で得られた位置の中央値にしてもよい。また、融合した位置をそれぞれの層で得られた位置の最頻値にしてもよい。

また、それぞれの層にあった点数Ｔ_ｋ（ｋは、１以上の整数であり、それぞれの領域を表す符号である。）を重みにして融合してもよい。

それぞれの属性は、下記式（５）～（１０）で表される。

Ｘ＝Σ（Ｘ_ｋ・Ｔ_ｋ）／ΣＴ_ｋ …（５）
Ｙ＝Σ（Ｙ_ｋ・Ｔ_ｋ）／ΣＴ_ｋ …（６）
Ｚ＝Σ（Ｚ_ｋ・Ｔ_ｋ）／ΣＴ_ｋ …（７）
ｒｏｌｌ＝Σ（ｒｏｌｌ_ｋ・Ｔ_ｋ）／ΣＴ_ｋ …（８）
ｐｉｔｃｈ＝Σ（ｐｉｔｃｈ_ｋ・Ｔ_ｋ）／ΣＴ_ｋ …（９）
ｙａｗ＝Σ（ｙａｗ_ｋ・Ｔ_ｋ）／ΣＴ_ｋ …（１０）
また、センサがカメラの場合、算出した距離誤差が距離に比例するため、層の距離ｄ_ｋ（ｋは、１以上の整数であり、それぞれの領域を表す符号である。）を重みにしてもよい。また、ＩＣＰ Ｍａｔｃｈｉｎｇが出力するスコア（センサで抽出した情報と外部記憶装置情報との差）を重みにして、それぞれの層から修正した位置の候補を融合してもよい。

図８ＢにマッチングステップＳ２０５と位置補正ステップＳ２０６の手順を示す。

図８Ｂに示すプロセスにおいては、属性を用いてマッチングを行う。

本図において、層分けステップＳ８１０は、移動体１００の現在位置からセンサで抽出した三次元点８０２と、外部記憶装置１９に登録された三次元点８０１とを上記式（１）を用いて層に分けるステップである。

各層マッチングステップＳ８２０は、層分けステップＳ８１０で分けられた、それぞれの層のセンサで検出した三次元点８０２と、外部記憶装置１９から取得した三次元点８０１と、をマッチングするステップである。

マッチング結果融合ステップＳ８３０は、それぞれの層で得られた各層マッチングステップＳ８２０の結果を融合する。融合の際は、上記式（４）～（１０）を用いる。

１：位置推定装置、２：信号受付部、１２、１２ａ、１２ｂ、１２ｎ：センサ、１３：情報処理装置、１４：センサ処理部、１５：制御部、１６：メモリ、１７：表示部、１９：外部記憶装置、１００：移動体、２１０：入出力部、２１１：三次元点取得部、２１２：フィルター部、２１３：マッチング部、２１４：自己位置補正部、３０１、３０４：存在確率分布、３０２、３０３：三次元点、６０５：固定カメラ。

Claims (6)

1. 周囲の情報を収集するセンサと、
   前記センサによって取得した情報を処理するセンサ処理部と、
   三次元点取得部と、
   フィルター部と、
   マッチング部と、
   自己位置補正部と、を有する移動体において、
   前記三次元点取得部は、前記センサから得られた三次元点を取得し、
   前記フィルター部は、マップに含まれる三次元点のうち前記移動体の現在の仮位置から参照可能な三次元点を求め、
   前記マッチング部は、前記三次元点取得部に記録されている前記三次元点と前記フィルター部が求めた参照可能な前記三次元点とのマッチングを行い、
   前記自己位置補正部は、前記マッチングに成功した三次元点を基に前記マップにおける前記移動体の現在の位置を求め、
   前記三次元点取得部は、前記移動体の走行環境に設置された固定センサ、又は他の移動体が有するセンサから周囲の情報を取得し、前記移動体が有する前記センサからの周囲の情報と組み合わせて現在の位置を求め、
   前記自己位置補正部は、前記移動体の前記現在の仮位置における存在確率分布と、前記固定センサ又は前記他の移動体が求めた前記移動体の位置情報とを用いて、前記移動体の存在確率分布の範囲が狭い範囲に補正をし、
   前記移動体の前記位置情報は、前記固定センサ又は前記他の移動体が求めた前記移動体までの距離の誤差範囲であり、
   前記補正は、前記誤差範囲と前記移動体の前記存在確率分布とを融合して行う、移動体。
2. 前記参照可能な三次元点は、前記センサが取得可能なセンシング角度の範囲内の前記マップ上の三次元点かつ前記マップ上で前記センシング角度の同一方向に複数の三次元点が存在する場合、前記移動体に最も近傍にある三次元点を抽出することで求められる、請求項１記載の移動体。
3. 前記他の移動体の存在確率分布は、前記移動体の前記現在の仮位置における存在確率分布より小さい、請求項１記載の移動体。
4. 前記三次元点取得部が取得した前記三次元点に、移動する障害物の三次元点が含まれている場合には、
   前記マッチング部は、前記障害物の三次元点を除いた三次元点を用いて、前記マッチングを行う、請求項１又は２に記載の移動体。
5. 前記マッチング部は、属性を変数とする層を用いて、前記マッチングを行う、請求項１又は２に記載の移動体。
6. 前記マッチング部は、前記層の前記属性に重みを設定し、前記マッチングを行う、請求項５記載の移動体。

Images