JP5765163B2 - 自己位置推定装置、方法、及びプログラム - Google Patents

Description

本発明は、所定のエリア内を移動する移動体の自己位置を推定するための技術に関する。

自律移動体等には、エリア内の状況と自己位置とを正確に推定する技術が必要とされる。この種の技術として、光波測距儀（レーザレンジファインダ）等を用いて環境地図の作成と自己位置の推定とを同時に行うことができるＳＬＡＭ(Simultaneously Localization and Mapping)等が知られている。

特許文献１は、自律移動体の自己位置を推定するシステムにおいて、環境に置かれた物体の位置・形状、自律移動体の自己位置等の推定精度を簡単且つ安価に向上させることを目的として、水平方向に照射線を走査することにより自律移動体と物体との相対位置及び物体の表面形状を取得し、当該取得された情報と予めデータベースに記憶された物体の表面形状等に関する情報とを照合させ、当該照合が一致した場合にデータベースに記憶された物体の情報を走行計画手段に送信し、当該照合が不一致の場合に走査により取得された物体の情報を走行計画手段に送信する構成を開示している。

また、同種の技術として、特許文献２，３が開示されている。

特開２００６−１４６３８１号公報 特開２００９−２０５２２６号公報 特開２００５−３１００４３号公報

図２０は、従来の自己位置推定装置において生じ得る課題を示している。移動体１０１は、計測センサ部、例えばレーザレンジファインダのレーザ照射部１０２が一箇所に固定されたものである。レーザ照射部１０２からのレーザ光の照射により取得された物体との距離等の環境データに基づいて、ＳＬＡＭ等の技術により環境地図の作成、自己位置の推定等が行われる。

上記構成においては、レーザ照射部１０２が一箇所に固定されているため、レーザ光の走査エリアは同一平面上に限定される。従って、環境地図の作成等の処理は、唯一の走査エリアから得られる環境データに基づいて行われなければならない。しかしながら、当該走査エリアに、例えば格子状のもの、変形しやすいもの、ガラス等の測距処理が困難な障害物１０３が存在する場合、自己位置の推定精度が著しく低下する可能性が高い。

そこで、本発明は、環境中に測距処理が困難な障害物が存在しても、自己位置の推定精度を高く維持できるようにすることを目的とする。

本発明の一態様は、移動体の複数の視点から取得される環境データに基づいて、前記視点毎に二次元マップを作成するマップ作成部と、複数の前記二次元マップの中から直線部が比較的多い前記二次元マップを抽出するマップ抽出部と、前記抽出された二次元マップと、当該抽出された二次元マップと同一の視点から取得される環境データとの照合結果に基づいて、前記移動体の自己位置を推定する位置推定部とを備える自己位置推定装置である。

また、前記マップ作成部は、前記各視点から照射線を平面状に走査させることにより前記環境データを取得するものであって、隣り合う２つの計測点を結んだ線と所定の基準線とがなす角度に基づいて前記直線部の多さを判定するものであることが好ましい。

また、前記位置推定部は、前記抽出された二次元マップと同一の視点から取得される環境データのうち、前記直線部であることを示す度数が一定値に満たないデータ、及び他のデータとの連続性が認められないデータを、前記照合の対象から除外するものであることが好ましい。

また、本発明は、上記装置と同様の技術思想に基づく自己位置推定方法及びプログラムである。

本発明によれば、異なる視点から取得される複数の二次元マップのうち、直線部が最も多い二次元マップを選択的に使用して移動体の自己位置が推定される。これにより、推定精度を大きく向上させることができる。

本発明の実施の形態１に係る自己位置推定装置の構成を示す図である。 実施の形態１に係る移動体の構成を示す図である。 実施の形態１に係る移動体が利用される状況を例示する図である。 実施の形態１における環境地図を作成する際の処理の流れを示すフローチャートである。 第１のレーザ照射部に対応する二次元マップを例示する図である。 第２のレーザ照射部に対応する二次元マップを例示する図である。 第３のレーザ照射部に対応する二次元マップを例示する図である。 実施の形態１における自己位置を推定する際の処理の流れを示すフローチャートである。 第１のレーザ照射部に対応する環境データを例示する図である。 第２のレーザ照射部に対応する環境データを例示する図である。 第３のレーザ照射部に対応する環境データを例示する図である。 環境データにおける測定点及び角度を例示する図である。 図１２の角度に基づいて作成されるヒストグラムを例示するグラフである。 第１のレーザ照射部に対応するヒストグラムを示すグラフである。 第２のレーザ照射部に対応するヒストグラムを示すグラフである。 第３のレーザ照射部に対応するヒストグラムを示すグラフである。 第２のレーザ照射部に対応するヒストグラムにおける閾値を例示するグラフである。 連結性がないデータを例示する図である。 本発明の実施の形態２に係る移動体の構成を示す図である。 従来の自己位置推定装置において生じ得る課題を示す図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施の形態１に係る自己位置推定装置１の構成を示している。自己位置推定装置１は、所定のエリア内を自律的に移動する移動体に備えられる。自己位置推定装置１により取得された自己位置を示す情報は、当該移動体の自律移動制御に用いられる。自己位置推定装置１は、マップ作成部２、マップ抽出部３、及び位置推定部４を有する。

マップ作成部２は、移動体の複数の視点から取得される環境データに基づいて、これらの視点毎に二次元マップを作成する。当該視点とは、移動体の周囲の環境に関する情報を取得するレンジファインダ等の機構の一部であり、例えばレーザレンジファインダのレーザ照射部、撮像装置のレンズ部等である。各視点に対応する複数の二次元マップは、マージ等されることなく個別にデータベースに保持される。

マップ抽出部３は、マップ作成部２により取得された複数の二次元マップの中から、直線部が比較的多い二次元マップを抽出する。二次元マップは、エリア内に存在する障害物等の形状を直線及び曲線の組み合わせにより表現する。マップ抽出部３は、このような二次元マップを構成する線のうち直線部を認識し、直線部の全線に対する比率等が多い二次元マップを抽出する。

位置推定部４は、マップ抽出部３により抽出された二次元マップと、当該抽出された二次元マップと同一の視点から取得される環境データとの照合結果に基づいて、移動体の自己位置を推定する。これにより、直線部を多く含む二次元マップを基準として移動体の自己位置が推定される。

図２は、実施の形態１に係る移動体１１の構造を概略的に示している。移動体１１は、レーザレンジファインダの一部を構成する３つのレーザ照射部１２Ａ，１２Ｂ，１２Ｃを備える。各レーザ照射部１２Ａ，１２Ｂ，１２Ｃは、垂直方向に所定の間隔を空けて配置され、それぞれスキャン用のレーザ光を水平方向に照射する。

図３は、移動体１１が利用される状況を例示している。本例においては、移動体１１は壁面２１に囲まれた室内で利用され、当該室内には、幾つかの障害物２２，２３，２４，２５が存在する。

図４は、自己位置推定装置１が環境地図を作成する際の処理の流れを示している。環境地図の作成時には、全てのレーザ照射部１２Ａ，１２Ｂ，１２Ｃが同時刻にそれぞれ環境データを取得する（Ｓ１０１）。次いで、各レーザ照射部１２Ａ，１２Ｂ，１２Ｃにより取得された各環境データに基づいて、各レーザ照射部１２Ａ，１２Ｂ，１２Ｃに対応する３つの二次元マップを作成する（Ｓ１０２）。これら３つの二次元マップは、それぞれ個別に記憶される（Ｓ１０３）。

図５はレーザ照射部１２Ａに対応する二次元マップ３０Ａ、図６はレーザ照射部１２Ｂに対応する二次元マップ３０Ｂ、そして図７はレーザ照射部１２Ｃに対応する二次元マップ３０Ｃを例示している。

図８は、自己位置推定装置１が自己位置を推定する際の処理の流れを示している。先ず、各レーザ照射部１２Ａ，１２Ｂ，１２Ｃによりそれぞれ環境の環境データが取得される（Ｓ２０１）。図９はレーザ照射部１２Ａに対応する環境データ３１Ａ、図１０はレーザ照射部１２Ｂに対応する環境データ３１Ｂ、そして図１１はレーザ照射部１２Ｃに対応する環境データ３１Ｃを例示している。

次いで、各環境データ３１Ａ，３１Ｂ，３１Ｃについて、隣り合う計測点（Ｚ_ｎ，Ｚ_ｎ＋１）を結んだ線と所定の基準線との成す角度θ_ｎを算出し、当該角度θ_ｎに基づいてヒストグラムを作成する（Ｓ２０２）。図１２は、測定点（Ｚ_ｎ，Ｚ_ｎ＋１）及び角度θ_ｎを例示している。図１３は、角度θ_ｎに基づいて作成されるヒストグラムを例示している。本例では、θ_ｎ＝ａｎｇｌｅ（Ｚ_ｎ）＝ａｔａｎ（（Ｚ_{ｎ＋１．ｙ}−Ｚ_ｎ．ｙ）／（Ｚ_{ｎ＋１．ｘ}−Ｚ_ｎ．ｘ））、度数＝ｈｉｓｔｇｒａｍ（θ_ｎ）の関係が成り立つ。当該度数が大きい程、二次元マップ３０Ａ，３０Ｂ，３０Ｃを構成する線の当該度数に対応する部分が直線に近いということになる。

また、図１４はレーザ照射部１２Ａ（環境データ３１Ａ）に対応するヒストグラム３２Ａ、図１５はレーザ照射部１２Ｂ（環境データ３１Ｂ）に対応するヒストグラム３２Ｂ、そして図１６はレーザ照射部１２Ｃ（環境データ３１Ｃ）に対応するヒストグラム３２Ｃを示している。

次いで、各ヒストグラム３２Ａ，３２Ｂ，３２Ｃにおいて、閾値Ｔを超える度数に対応する角度の個数を算出し、当該個数が最も多いものに対応する二次元マップ３０Ａ，３０Ｂ，３０Ｃを抽出する（Ｓ２０３）。本例においては、当該閾値Ｔを超す度数となる角度の個数は、図１５に示すヒストグラム３２Ｂが最も多いため、これに対応する二次元マップ３０Ｂが抽出される。

次いで、抽出された二次元マップ３０Ｂに対応するヒストグラム３２Ｂから、閾値Ｌ以下のデータを削除する（Ｓ２０４）。図１７は、ヒストグラム３２Ｂにおける閾値Ｌを例示している。

次いで、残ったデータのうち連結性がないデータを削除する（Ｓ２０５）。図１８は、連結性がないデータ３３を例示している。

次いで、抽出された二次元マップ３０ＢとステップＳ２０４，Ｓ２０５によりフィルタリングされたデータとをマッチングさせ（Ｓ２０６）、移動体１１の自己位置（ｘ，ｙ，θ）を推定する（Ｓ２０７）。

上記自己位置推定装置１によれば、異なる視点（レーザ照射部１２Ａ，１２Ｂ，１２Ｃ）から取得される複数の二次元マップ（３０Ａ，３０Ｂ，３０Ｃ）のうち、直線部が最も多い二次元マップを用いて移動体の自己位置が推定される。これにより、推定精度を大きく向上させることができる。

実施の形態２
図１９は、実施の形態２に係る自己位置推定装置に係る移動体５１の構造を概略的に示している。移動体５１は、レーザレンジファインダの一部を構成する１つのレーザ照射部５２を備える。

レーザ照射部５２は、レーザ光の照射角度を上下に変化させる機構を有する。このような構成によっても、複数の二次元マップを作成することができ、これらの中から直線部の多い二次元マップを、上記実施の形態１と同様の手法により、選択的に使用することができる。本実施の形態によれば、レーザ照射部５２を複数設けることなく、上記実施の形態１と同様に、推定精度を向上させることができる。

尚、本発明は上記実施の形態に限られるものではなく、趣旨を逸脱しない範囲で適宜変更することが可能なものである。

１ 自己位置推定装置
２ マップ作成部
３ マップ抽出部
４ 位置推定部
１１，５１ 移動体
１２Ａ，１２Ｂ，１２Ｃ，５２ レーザ照射部
２１ 壁面
２２，２３，２４，２５ 障害物
３０Ａ，３０Ｂ，３０Ｃ 二次元マップ
３１Ａ，３１Ｂ，３１Ｃ 環境データ
３２Ａ，３２Ｂ，３２Ｃ ヒストグラム

Claims (7)

1. 移動体の複数の視点から取得される環境データに基づいて、前記視点毎に二次元マップを作成するマップ作成手段と、
   複数の前記二次元マップの中から直線部が比較的多い前記二次元マップを抽出するマップ抽出手段と、
   前記抽出された二次元マップと、当該抽出された二次元マップと同一の視点から取得される環境データとの照合結果に基づいて、前記移動体の自己位置を推定する位置推定手段と、
   を備える自己位置推定装置。
2. 前記マップ作成手段は、前記各視点から照射線を平面状に走査させることにより前記環境データを取得するものであって、隣り合う２つの計測点を結んだ線と所定の基準線とがなす角度に基づいて前記直線部の多さを判定する、
   請求項１に記載の自己位置推定装置。
3. 前記位置推定手段は、前記抽出された二次元マップと同一の視点から取得される環境データのうち、前記直線部であることを示す度数が一定値に満たないデータ、及び他のデータとの連続性が認められないデータを、前記照合の対象から除外する、
   請求項２に記載の自己位置推定装置。
4. 移動体の複数の視点から取得される環境データに基づいて、前記視点毎に二次元マップを作成するマップ作成ステップと、
   複数の前記二次元マップの中から直線部が比較的多い前記二次元マップを抽出するマップ抽出ステップと、
   前記抽出された二次元マップと、当該抽出された二次元マップと同一の視点から取得される環境データとの照合結果に基づいて、前記移動体の自己位置を推定する位置推定ステップと、
   を備える自己位置推定方法。
5. 前記マップ作成ステップは、前記各視点から照射線を平面状に走査させることにより前記環境データを取得するものであって、隣り合う２つの計測点を結んだ線と所定の基準線とがなす角度に基づいて前記直線部の多さを判定する、
   請求項４に記載の自己位置推定方法。
6. 前記位置推定ステップは、前記抽出された二次元マップと同一の視点から取得される環境データのうち、前記直線部であることを示す度数が一定値に満たないデータ、及び他のデータとの連続性が認められないデータを、前記照合の対象から除外する、
   請求項５に記載の自己位置推定方法。
7. コンピュータに、請求項４〜６のいずれか１項に記載の方法を実行させるための自己位置推定プログラム。

Images