JP2023173688A - 移動体、事前地図生成方法、位置推定システム、位置推定方法、事前地図生成方法、およびプログラム - Google Patents

Abstract

【課題】移動体の位置の推定を短時間で精度よく行うことができる移動体、事前地図生成方法、位置推定システム、位置推定方法、事前地図生成方法、およびプログラムを提供する。【解決手段】特定の領域内を移動する移動体であって、前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を記憶する事前地図記憶部と、前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成する現在地図生成部と、前記事前地図および前記現在地図に基づいて、自己の位置を推定する自己位置推定部と、を備える。【選択図】図３

Description

本開示は、移動体、事前地図生成方法、位置推定システム、位置推定方法、事前地図生成方法、およびプログラムに関する。

移動体を自律移動させるとき、移動体の位置を推定することが行われている。移動体の位置を推定するために、移動体が移動する領域内の対象物（障害物など）の情報をあらかじめ記述した環境地図が使用されることがある。環境地図は、移動体が移動領域を自律移動するときに自己位置を推定したり、事前に移動経路を計画したりするためなどに利用される。

特許文献１には、三次元空間の距離情報を取得できる三次元距離センサを用いたり、二次元距離センサを移動体の移動方向に対して角度を付けて搭載し走行の時系列情報を蓄積したりすることで三次元空間の距離情報を取得し、環境地図を生成することが記載されている。

また、非特許文献１には、移動体が三次元空間の情報を記述した環境地図を用いて自己位置を推定する技術が記載されている。

国際公開第２０１６／１５７４２８号

Ｐｅｔｅｒ Ｂｉｂｅｒ ａｎｄ Ｗｏｌｆｇａｎｇ Ｓｔｒａβｅｒ， "Ｔｈｅ Ｎｏｒｍａｌ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ Ｔｒａｎｓｆｏｒｍ： Ａ Ｎｅｗ Ａｐｐｒｏａｃｈ ｔｏ Ｌａｓｅｒ Ｓｃａｎ Ｍａｔｃｈｉｎｇ"， Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ２００３ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ， （２００３）， ｐｐ． ２７４３－２７４８．

三次元空間の情報を記述した環境地図を用いて移動体の位置を推定しようとした場合、三次元の情報を処理するために、計算量が膨大になってしまい、処理に時間が掛かる。短時間で膨大な計算を処理するためには高性能な演算装置を移動体に搭載すればよいが、コストが掛かる、または装置が大型となり、搭載できる移動体が限られる、と言った課題が生じる。

本開示は、移動体の位置の推定を短時間で精度よく行うことができる移動体、事前地図生成方法、位置推定システム、位置推定方法、事前地図生成方法、およびプログラムを提供することを目的とする。

本開示に係る移動体は、特定の領域内を移動する移動体であって、前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を記憶する事前地図記憶部と、前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成する現在地図生成部と、前記事前地図および前記現在地図に基づいて、自己の位置を推定する自己位置推定部と、を備える。

本開示に係る事前地図生成装置は、特定の領域内を移動する移動体の自己位置推定に用いられる事前地図を生成する事前地図生成装置であって、前記領域内の対象物の計測位置を示す点群を含む点群地図を生成する点群地図生成部と、前記領域を複数の大領域に分割した場合の大領域毎に対象物が存在する確率密度を算出し、前記複数の大領域のそれぞれを複数の小領域にさらに分割して前記確率密度を前記小領域毎に離散化し、前記小領域毎に離散化した前記確率密度を示す前記事前地図を生成する事前地図生成部と、を備える。

本開示に係る位置推定システムは、特定の領域内を移動する移動体の位置を推定する位置推定システムであって、前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を生成する事前地図生成装置と、前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成し、前記事前地図および前記現在地図に基づいて、自己の位置を推定する移動体と、を備える。

本開示に係る位置推定方法は、特定の領域内を移動する移動体の位置推定方法であって、前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を記憶し、前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成し、前記事前地図および前記現在地図に基づいて、自己の位置を推定する。

本開示に係る事前地図生成方法は、特定の領域内を移動する移動体の自己位置推定に用いられる事前地図を生成する事前地図生成方法であって、前記領域内の対象物の計測位置を示す点群を含む点群地図を生成し、前記領域を複数の大領域に分割した場合の大領域毎に対象物が存在する確率密度を算出し、前記複数の大領域のそれぞれを複数の小領域にさらに分割して前記確率密度を前記小領域毎に離散化し、前記小領域毎に離散化した前記確率密度を示す前記事前地図を生成する。

本開示に係るプログラムは、特定の領域内を移動する移動体の位置を推定するコンピュータが実行するプログラムであって、前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を記憶し、前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成し、前記事前地図および前記現在地図に基づいて、自己の位置を推定する、処理を前記コンピュータに実行させる。

本開示によれば、移動体の位置の推定を短時間で精度よく行うことができる。

本開示の実施の形態に係る位置推定システムの概要図 移動体の移動領域の第１の例の説明図 移動体の移動領域の第２の例の説明図 位置推定システムの全体動作のフローチャート 事前地図の生成手順のフローチャート 事前地図の生成手順における点群地図の説明図 事前地図の生成手順において大領域に分割する処理の説明図 事前地図の生成手順において大領域毎の確率密度を濃淡で示したイメージ図 事前地図の生成手順において小領域に分割する処理の説明図 事前地図の生成手順における離散化の方法の説明図 事前地図の生成手順における離散化の方法の説明図 事前地図の生成手順における事前地図の説明図 事前地図の生成手順における事前地図の説明図 事前地図のデータ構造の説明図 事前地図のデータ構造の追加データ部の説明図 事前地図のデータ構造をデータ部へ格納する説明図 現在地図の生成手順のフローチャート 現在地図の生成手順において移動体がスタート地点にいる様子を示す図 現在地図の生成手順において移動体がスタート地点にいるときの計測点を示す図 現在地図の生成手順において移動体がスタート地点にいるときに小領域に分割する処理の説明図 現在地図の生成手順において移動体がスタート地点にいるときの現在地図を示す図 現在地図の生成手順において移動体が地点Ｐｃにいる様子を示す図（図２に示す軌跡を移動する場合） 現在地図の生成手順において移動体が地点Ｐｃにいる様子を示す図（図３に示す軌跡を移動する場合） 現在地図の生成手順において移動体が地点Ｐｃにいるときの計測点を含む点群地図を示す図（図２に示す軌跡を移動する場合） 現在地図の生成手順において移動体が地点Ｐｃにいるときの計測点を含む点群地図を示す図（図３に示す軌跡を移動する場合） 現在地図の生成手順において移動体が地点Ｐｃにいるときに移動領域を小領域に分割する処理の説明図（図２に示す軌跡を移動する場合） 現在地図の生成手順において移動体が地点Ｐｃにいるときに移動領域を小領域に分割する処理の説明図（図３に示す軌跡を移動する場合） 現在地図の生成手順において移動体が地点Ｐｃにいるときの現在地図の差分を示す図（図２に示す軌跡を移動する場合） 現在地図の生成手順において移動体が地点Ｐｃにいるときの現在地図の差分を示す図（図３に示す軌跡を移動する場合） 現在地図の生成手順における更新済み現在地図を示す図（図２に示す軌跡を移動する場合） 現在地図の生成手順における更新済み現在地図を示す図（図３に示す軌跡を移動する場合） 自己位置推定部による自己位置推定手順のフローチャート 自己位置推定手順における現在地図の切り出しの様子を示す図（図２に示す軌跡を移動する場合） 自己位置推定手順における現在地図の切り出しの様子を示す図（図３に示す軌跡を移動する場合） 自己位置推定手順において切り出した現在地図を示す図（図２に示す軌跡を移動する場合） 自己位置推定手順において切り出した現在地図を示す図（図３に示す軌跡を移動する場合） 自己位置推定手順における候補位置の例を示す図（図２に示す軌跡を移動する場合） 自己位置推定手順における候補位置の例を示す図（図３に示す軌跡を移動する場合） 自己位置推定手順において候補位置で事前地図を切り出す様子を示す図（図２に示す軌跡を移動する場合） 自己位置推定手順において候補位置で事前地図を切り出す様子を示す図（図３に示す軌跡を移動する場合） 自己位置推定手順において切り出した事前地図を示す図（図２に示す軌跡を移動する場合） 自己位置推定手順において切り出した事前地図を示す図（図３に示す軌跡を移動する場合） 自己位置推定手順において切り出した事前地図において、量子化値の総和を求める小領域を説明するための図（図２に示す軌跡を移動する場合） 自己位置推定手順において切り出した事前地図において、量子化値の総和を求める小領域を説明するための図（図３に示す軌跡を移動する場合）

以下、本開示の実施の形態について、図面を参照しながら説明する。なお、図面は、理解しやすくするために、それぞれの構成要素を主体として模式的に示している。

＜位置推定システム１００＞
まず、本開示の実施の形態における位置推定システムを図１に示す。図１は本開示の実施の形態に係る位置推定システム１００の概要図である。

図１に示すように、位置推定システム１００は、事前地図生成装置２００と、移動体３００と、を有する。事前地図生成装置２００は、移動体３００が自己位置を推定しながら移動する際に用いられる、移動領域内の環境を示す情報を含む事前地図を、移動領域を実際に走行することで、事前に生成する。移動領域とは、移動体３００が、自己位置を推定しながら走行することが予定されている領域（所定の広さを有する環境）である。事前地図とは、事前地図生成装置２００によって事前に（移動体３００が移動領域において、自己位置を推定しながらの移動を行う前に）生成される地図である。事前地図は、例えば移動体３００を、移動領域内を一通り走査するように走行させることで作成される。

本実施の形態では、事前地図生成装置２００は移動体３００に搭載されており、移動体３００が移動領域を実際に走行することにより、事前地図が生成される。なお、本開示はこれに限定されず、例えば事前地図生成部を、移動体３００と同等以上の走行能力を有する計測機器に搭載して移動領域を走行させることで、事前地図を生成してもよい。

事前地図生成装置２００は、距離センサ２０と、点群地図生成部２４、オドメトリ計算部２５、事前地図生成部２６と、を有する。

点群地図生成部２４は、距離センサ２０から得られる距離データと、オドメトリ計算部２５で生成されたオドメトリデータに基づいて、移動体３００が走行する領域に存在する物体を点群で示した点群地図を生成する。

距離センサ２０は、移動体３００が移動領域を実際に走行したときに、移動領域内に存在する物体までの距離および物体の方向を計測する。以下の説明において、距離センサ２０が検出した対象物の距離および方向を示すデータを、距離データと記載する。距離センサ２０は、具体的には、電波、光などの電磁波、または超音波などの反射波を受信することで距離データを取得すればよい。または、距離センサ２０は、ステレオカメラにより視差画像を取得して距離データを取得すればよい。

オドメトリ計算部２５は、移動体３００の車輪の回転速度を計測するエンコーダ２１、加速度や各速度を検出するＩＭＵ（Inertial Measurement Unit：慣性計測装置）２２、カメラ２３などから得られた情報に基づいて、オドメトリデータを計算する。オドメトリデータとは、エンコーダ２１、ＩＭＵ２２、またはカメラ２３の少なくともいずれかから得られた情報を基に移動速度、旋回速度を算出し、積分することで事前地図生成装置２００の移動量を表すデータである。

事前地図生成部２６は、点群地図生成部２４で生成された点群地図に基づいて、事前地図を生成する。事前地図生成装置２００において生成された事前地図は、移動体３００が有する事前地図記憶部３９に記憶される。事前地図の詳細については後述する。

移動体３００は、距離センサ３０と、オドメトリ計算部３１と、自己位置推定部３２と、移動体制御部３８と、事前地図記憶部３９と、を有する。

オドメトリ計算部３１は、オドメトリ計算部２５と同様に、図示しないエンコーダなどの情報に基づいて、移動体３００の移動量を計算し、オドメトリデータを生成する。なお、図１に示す例では、事前地図生成装置２００と移動体３００とがそれぞれオドメトリ計算部を有しているが、いずれか一方のみが有していてもよい。この場合、オドメトリ計算部が生成したオドメトリデータ、事前地図生成装置２００と移動体３００のうち、オドメトリデータを必要とする方が利用すればよい。

自己位置推定部３２は、オドメトリ計算部３１で生成されたオドメトリデータと、距離センサ３０から取得した距離データと、事前地図記憶部３９から読み出した事前地図と、に基づいて、移動体３００の自己位置の推定を行う。距離センサ３０は、距離センサ２０と同様のセンサである。

自己位置推定部３２は、候補点移動部３３と、現在地図生成部３４と、現在地図記憶部３５と、地図切出部３６と、スコア計算部３７と、事前地図記憶部３９と、を有する。自己位置推定部３２の詳細については後述する。

移動体制御部３８は、自己位置推定部３２で生成された自己位置推定結果と、距離センサ３０からの距離データと、事前地図記憶部３９から読み出した事前地図に基づいて、移動体３００の走行制御を行う。

＜移動領域＞
移動領域について具体例を挙げて説明する。図２に、移動領域の第１の具体例を示す。また、図３に、移動領域の第１の具体例を示す。図２および図３は、移動領域の上面図である。図２，図３の７００は移動領域を示し、７０１，７０２は移動領域内に存在する物体を示す。物体７０１，７０２は、壁、柱など、移動体３００の移動の障害となりうる物体である。７０３は移動可能領域を示し、移動体３００が移動可能な床面等である。図２，図３はグローバル座標系で描かれた図であり、Ｏｇはグローバル座標系の原点である。

移動領域７００は、移動体３００が、スタート地点からゴール地点までの間を、自己位置を推定しながら自律走行することが計画されている領域である。図２では移動体３００が直線状の軌跡を、図３では移動体３００が曲線状の軌跡を、それぞれ描いて移動する様子が示されている。なお、図２，図３に示す移動体３００の移動する軌跡は一例であり、本開示における移動体３００の移動する軌跡はこれらに限定されない。

＜動作フロー＞
位置推定システム１００の動作フローについて説明する。

［全体動作］
図４は、位置推定システム１００の全体動作のフローチャートである。ステップＳ１において、事前地図生成装置２００は、事前地図を生成する。上記したように、事前地図は、移動体３００が移動領域内において自己位置を推定しながら自律移動する前に、あらかじめ生成される移動領域の地図である。

ステップＳ２において、移動体３００は、事前地図を用いて、移動領域内における自律移動を開始する。

ステップＳ３において、移動体３００は、移動領域内において自律移動しながら、現在地図の生成を行う。現在地図とは、自律移動中の移動体３００がスタート地点から現在の位置に到達するまでに検知した移動領域内の物体の位置を示した地図である。現在地図の詳細については後述する。

ステップＳ４において、移動体３００は、事前地図と現在地図とに基づいて自己位置の推定を行う。

ステップＳ５において、移動体３００は、推定した自己位置に基づいて、ゴール地点に向かって移動する。

図４に示すフローチャートにおいて、ステップＳ３からステップＳ５までの処理は、所定時間毎に繰り返し行われる。すなわち、移動体３００は、自律移動を開始した後、自己位置の推定と移動とを所定の時間周期で繰り返し行う。これにより、移動体３００は、移動領域内における自己位置を精度よく推定しながら、安全に移動を行うことができる。

以下の説明は、図４のフローチャートのステップＳ３からステップＳ５がループすることを前提とする。以下では、ステップＳ３からステップＳ５までの処理を１ループとして、現在行っているループの処理を現在の処理、１回前のループにおける処理を前回の処理、などと記載することがある。

［事前地図の生成方法］
次に、図４のステップＳ１における、事前地図生成装置２００による事前地図５００の生成方法について説明する。

図５は、事前地図５００の生成手順のフローチャートである。図５に示すフローチャートは、移動体３００が図２，図３に示す移動領域７００内を、実際に走行しながら実行される。

ステップＳ１１において、事前地図生成装置２００は、現在の移動体３００の位置から距離センサ２０によって検知された物体までの距離を示す距離データを、オドメトリ計算部２５から、現在の位置までの移動体３００の移動した軌跡を示すオドメトリデータを、それぞれ取得する。

ステップＳ１２において、点群地図生成部２４は、現在位置に係る点群地図４００を生成する。図６に、点群地図４００の一例を示す。点群地図４００は、移動領域７００内において、距離センサ２０によって検出された検出点を含む点群の位置を示した地図である。

図６において、物体に対応する検出点を含む点群４０１が示されている。点群地図４００において、グローバル座標系が設定されている。Ｏｊは点群地図４００の座標原点を示す。

図５の説明に戻る。ステップＳ１３において、事前地図生成部２６は、ステップＳ１２で生成された点群地図を複数の領域に分割する。以下の説明において、ステップＳ１２において分割した領域を、大領域と記載する。図７に、ステップＳ１２で生成された点群地図４００を大領域に分割した様子を示す。

図７の４００＿Ｒ_Ｌは１つの大領域を示す。大領域４００＿Ｒ_Ｌの１辺のサイズをＬ_Ｌとする。大領域の１辺は例えば１ｍ～１．５ｍに設定される。点群地図４００の各点群はいずれかの大領域に必ず属することになる。

ステップＳ１４において、事前地図生成部２６は、１つの大領域に含まれる点群の二次元の確率密度を計算する。確率密度は以下の式（１）で算出する。

式（１）では、二次元の確率密度が、大領域に含まれる点群の位置情報の平均値、分散値などにより計算されている。

１つの大領域内に点群が１２つ未満の場合、その大領域の確率密度は０になる。１つの大領域内に点群が２つ以上、異なる位置に存在する場合、その大領域の確率密度は０より大きい値となる。

図８は、図７において大領域毎の確率密度を濃淡で示したイメージ図である。図８のＤ_Ｐは、確率密度の分布を等高線により表しており、色が濃いほど確率密度の値が大きいことを示している。

ステップＳ１５において、ステップＳ１３で分割した大領域を、さらに複数の小領域に分割する。図９は、点群地図４００を小領域に分割した様子を示す。

図９において、４００＿Ｒ_Ｓは１つの小領域を示す。小領域の１辺のサイズをＬ_Ｓとする。小領域の１辺のサイズＬ_Ｓは、例えば、大領域の１辺のサイズの１／４～１／１０、すなわち０．１ｍ～０．４ｍ程度に設定される。

ステップＳ１６において、大領域４００＿Ｒ_Ｌ毎の点群位置の確率密度を離散化し、小領域毎の確率密度を算出する。図１０および図１１は、各大領域の点群位置の確率密度を小領域で離散化する方法について説明するための図である。

図１０Ａは、１つの大領域４００＿Ｒ_Ｌを拡大した様子を示す。図１０Ｂは、Ｘｊ軸に沿った二次元の確率密度Ｐ_Ｄ＿ｘの分布を示す。図１０Ｃは、Ｙｊ軸に沿った二次元の確率密度Ｐ_Ｄ＿ｙの分布を示す。

図１０Ｂおよび図１０Ｃに示すように、図１０Ａに例示する大領域４００＿Ｒ_Ｌでは、点群の確率密度Ｐ_Ｄ＿ｘおよびＰ_Ｄ＿ｙは、いずれも山型の滑らかな連続量となっている。なお、図１０Ａでは確率密度を色の濃さで示しているが、図１０Ｂおよび図１０Ｃに示すように、大領域４００＿Ｒ_Ｌの周辺部の白い部分でも確率密度は０ではなく、微小な値を有している。

図１１Ａは、図１０Ａに示す大領域４００＿Ｒ_Ｌのうち、１つの小領域４００＿Ｒ_Ｓを拡大した様子を示す図である。Ｄ_Ｓはこの小領域４００＿Ｒ_Ｓの対角線であり、Ｍ_Ｓは小領域４００＿Ｒ_Ｓの中点である。

図１１Ｂは、図１１Ａに示す小領域４００＿Ｒ_Ｓの対角線Ｄ_Ｓ上における確率密度の分布を示す。図１１Ｂに示す例では、小領域４００＿Ｒ_Ｓの対角線の中点位置における確率密度の値は０．８である。

次に、小領域４００＿Ｒ_Ｓの対角線の中点位置における確率密度を、所定のビット数で量子化する。量子化は、下記の式（２）を用いて行う。

式（２）は、確率密度の値に対し量子化を行って整数値へ変換する式である。ｄは量子化後の確率密度の値（整数値）、ｆは量子化前の確率密度の値、ｆ_ｍａｘはあらかじめ定められた上限値である。ｎはビット数である。

例えば、ｆを０．８、ｆ_ｍａｘを１．０、ｎを２４とすると、ｄは１３，４２１，７７２となる。

図１２は、図１０Ａに示す大領域４００＿Ｒ_Ｌに対応する事前地図の一部５００＿Ｒ_Ｌを示す図である。また、図１３は、図９に含まれる全ての小領域４００＿Ｒ_Ｓ毎の量子化した確率密度の値を含む事前地図５００を示す図である。

図５の説明に戻る。ステップＳ１７において、事前地図生成部２６は、ステップＳ１２からステップＳ１６で計算された、現在位置毎の点群地図４００における小領域毎の確率密度と、オドメトリ計算部２５から取得した、移動体３００が移動した軌跡を示すオドメトリデータと、に基づいて、事前地図５００を生成する。

図１４Ａは、事前地図５００のデータ構造８００を示す。図１４Ａにおいて、８０１はファイルヘッダ、８０２は情報ヘッダ、８０３はデータ部、８０４は追加データ部である。

図１４Ｂは、ファイルヘッダ８０１に格納される情報の例を示す。図１４Ｃは、情報ヘッダ８０２に格納される情報の例を示す。情報ヘッダ８０２には、一般的な画像データの情報が格納される。

データ部８０３は、ステップＳ１６で生成された点群地図４００における小領域毎の確率密度の量子化値（図１３参照）を格納する。

データ部８０３に、点群地図４００における小領域毎の確率密度の離散化値を格納する方法は、例えば以下のとおりである。図１５Ａは点群地図４００の小領域毎の量子化値を、データ部８０３に格納する際の順番を説明する図である。また、図１５Ｂは、データ部８０３のデータ構造と格納の順番を示す図である。

図１５Ａに示すように、小領域毎の量子化値は、原点に近い位置からＸｊ軸に沿って、１列ごと下から上に読み取られる。図１５Ｂに示すように、データ部８０３には、読み取られた量子化値が１行毎に左から右に格納される。

ここで、事前地図に採用する画像ファイル形式に合わせて、例えば１行当たりのデータ量を調整することが行われる。本実施の形態では、事前地図にはビットマップ形式を採用するが、ビットマップ形式では１行のデータ量は４の倍数のＢｙｔｅ数にする必要がある。１つの小領域を２４ｂｉｔ＝３Ｂｙｔｅで量子化する場合、点群地図における小領域の数が１１個であるとすると、１行のデータ量は３３Ｂｙｔｅとなる。この場合、ビットマップ形式に合わせて１行のデータ量を４の倍数とするためには、３Ｂｙｔｅ追加して３６Ｂｙｔｅとすればよい。

この場合、データ部８０３は、１列にＭ×２４ｂｉｔ＋αのデータが格納され、これが行方向にＮ個繋がったデータ構造を有する。Ｍは点群地図４００におけるＸｊ方向に沿った小格子の個数、ＮはＹｊ方向に沿った小格子の個数、αは行毎のデータ量を４の倍数に合わせるために加算する値である。

なお、上記の説明では、点群地図を２４ｂｉｔで量子化する例について説明したが、２４ｂｉｔ以外で量子化する場合、図１４Ｂ，図１４Ｃに示すヘッダ情報を変更することで、２４ｂｉｔ以外で量子化することも可能である。

追加データ部８０４は、三次元の点群地図４００＿３Ｄに対応するための追加データを格納する。図１６Ａは、三次元の点群地図４００＿３Ｄのサイズを示す概念図である。三次元の点群地図４００＿３Ｄは、図６に示すＸｊ－Ｙｊ平面の点群地図４００をＺｊ軸方向に積層したものである。ｘｍｉｎ，ｙｍｉｎ，ｚｍｉｎはそれぞれＸｊ軸、Ｙｊ軸、Ｚｊ軸に沿った点群地図４００＿３Ｄにおける距離の最小値、ｘｍａｘ，ｙｍａｘ，ｚｍａｘはそれぞれＸｊ軸、Ｙｊ軸、Ｚｊ軸に沿った点群地図４００＿３Ｄにおける距離の最大値である。

図１６Ｂは、追加データ部８０４に格納される追加データを示す。図１６Ｂに示すように、追加データには、三次元のＸｊ軸、Ｙｊ軸、Ｚｊ軸に沿った距離の最小値、小領域の個数、小領域の１辺のサイズが含まれる。

以上の処理により、事前地図５００を生成することができる。このように確率密度の離散化、量子化をあらかじめ行っておくことにより、移動体３００の自律移動時における自己位置推定処理における負荷低減を実現できる。また、事前地図５００が三次元に対応する場合でもコンパクトなデータ構造で事前地図５００のデータを格納できるので、自己位置推定処理における負荷低減を実現できる。

事前地図生成装置２００によって生成された事前地図５００は、移動体３００の事前地図記憶部３９に記憶される。

［現在地図の生成方法］
次に、図４のステップＳ２における、現在地図６００の生成方法について説明する。

図１７は、現在地図６００の生成手順のフローチャートである。図１７では、一例として、移動体３００が図２および図３に示す移動領域７００において、スタート地点からゴール地点に向かって自律走行する場合の現在地図６００の生成手順を説明する。

移動体３００がスタート地点にいるとき、移動体３００を基準とした座標原点Ｏ_３００と移動領域７００のスタート地点を基準とした座標原点Ｏ_７００とは一致している。

ステップＳ２１において、現在地図生成部３４は、移動体３００がスタート地点にいるか否かを判定する。移動体３００がスタート地点にいる場合、処理はステップＳ２２に進む。そうでない場合、処理はステップＳ２５に進む。

ステップＳ２１において、移動体３００がスタート地点にいると判断された場合、ステップＳ２２からステップＳ２４の処理が実行される。

図１８は、移動体３００がスタート地点にいる場合の移動領域７００を示す。図１８において、ＳＬは距離センサ３０の走査線、Ｏ_３００は移動体３００を基準とした座標原点、Ｏ_７００は移動領域７００のスタート地点を基準とした座標原点を示している。なお、移動体３００を基準とした座標原点Ｏ_３００は移動体３００が移動するのに合わせて移動するが、移動領域７００のスタート地点を基準を基準とした座標原点Ｏ_７００は移動しない。

ステップＳ２２において、現在地図生成部３４は、距離センサ３０から計測点までの距離および距離センサ３０から見た計測点の方向を取得する。計測点は、距離センサ３０の走査線ＳＬと物体７０１，７０２との交点である。図１９は、移動体３００がスタート地点にいる時の計測点Ｐｍを示す図である。

ステップＳ２３において、現在地図生成部３４は、点群地図４００を小領域４００＿Ｒ_Ｓに分割する。図２０は、図１９に示す点群地図４００を小領域４００＿Ｒ_Ｓに分割した様子を示す。ステップＳ２３で分割される小領域４００＿Ｒ_Ｓのサイズは、図５のステップＳ１５で分割される点群地図４００の小領域４００＿Ｒ_Ｓのサイズと同じである。図２０に示すように、計測点Ｐｍは小領域４００＿Ｒ_Ｓのいずれかに属する。

ステップＳ２４において、現在地図生成部３４は、点群地図４００を分割した小領域４００＿Ｒ_Ｓ毎の計測点Ｐｍの有無に基づいて、現在地図６００を生成する。図２１は、移動体３００がスタート地点にある時の現在地図６００を示す。現在地図６００は、点群地図４００を分割した小領域４００＿Ｒ_Ｓのそれぞれと対応する小領域６００＿Ｒ_Ｓを有している。現在地図６００は、小領域４００＿Ｒ_Ｓ内に計測点Ｐｍが入っている場合には対応する小領域６００＿Ｒ_Ｓに「１」の値を、小領域４００＿Ｒ_Ｓ内に計測点Ｐｍが入っていない場合には対応する小領域６００＿Ｒ_Ｓに「０」の値を、それぞれ格納することで生成される。ステップＳ２４の完了後、処理はステップＳ２１に戻る。

ステップＳ２１において、移動体３００がスタート地点にいないと判断された場合、ステップＳ２５からステップＳ２８の処理が実行される。ステップＳ２５からステップＳ２７までの処理では、ステップＳ２４までの処理で生成された現在地図６００からの差分が生成される。なお、以降の説明において、新たに行われるステップＳ２５からステップＳ２８までの処理より前に行われたステップＳ２２からステップＳ２４までの処理、または、新たに行われるステップＳ２５からステップＳ２８までの処理より前に行われたステップＳ２５からステップＳ２８までの処理において生成された現在地図６００を、前回の現在地図と記載することがある。

ステップＳ２５において、現在地図生成部３４は距離センサ３０から距離データを、オドメトリ計算部３１からオドメトリデータをそれぞれ取得する。オドメトリデータは、前回の現在地図６００の生成時点における移動体３００の位置から、現在までの間に移動体３００が移動した移動量を示すデータである。

図２２および図２３は、移動体３００がスタート地点とゴール地点との間の地点Ｐｃにいる場合の移動領域７００を示す図である。図２２は、図２に示す直線状の軌跡を移動体３００が移動する場合に対応しており、図２３は、図３に示す曲線状の軌跡に沿って移動体３００が移動する場合に対応している。

ステップＳ２６において、現在地図生成部３４は点群地図４００を小領域４００＿Ｒ_Ｓに分割する。図２４は、移動体３００が図２１に示す第１地点にいる場合の距離センサ３０の計測点を含む点群地図４００を示す図である。図２４は、移動体３００が図２２に示す地点Ｐｃにいる場合の距離センサ３０の計測点を含む点群地図４００を示す図である。図２５は、移動体３００が図２３に示す地点Ｐｃにいる場合の距離センサ３０の計測点を示す図である。図２６および図２７は、図２３および図２３に示す点群地図４００を小領域４００＿Ｒ_Ｓに分割した様子を示している。ステップＳ２６において分割される小領域４００＿Ｒ_Ｓのサイズは、図５のステップＳ１５で分割される点群地図４００の小領域４００＿Ｒ_Ｓと同じサイズである。

ステップＳ２７において、現在地図生成部３４は、小領域に分割された点群地図４００に基づいて、現在地図の差分６００Ｄを生成する。図２８は、移動体３００が図２２に示す地点Ｐｃにいる時の現在地図の差分６００Ｄを示す図である。図２９は、移動体３００が図２３に示す地点Ｐｃにいる時の現在地図の差分６００Ｄを示す図である。ステップＳ２７における現在地図の差分６００Ｄの生成方法は、ステップＳ２４において説明した方法と同じである。すなわち、現在地図の差分は、小領域４００＿Ｒ_Ｓ内に計測点が入っている場合には対応する小領域に「１」の値を、小領域４００＿Ｒ_Ｓ内に計測点が入っていない場合には対応する小領域に「０」の値を、それぞれ格納することで生成される。

ステップＳ２８において、現在地図生成部３４は、ステップＳ２４において生成した前回の現在地図６００と、ステップＳ２７において生成した現在地図の差分６００Ｄと、に基づいて、現在地図６００を更新する。

更新済み現在地図６００Ｕの生成方法は、例えば以下のとおりである。現在地図生成部３４は、ステップＳ２４において生成した現在地図６００と、ステップＳ２７において生成した現在地図の差分６００Ｄとを、原点Ｏｇが一致するように重ね合わせる。

図３０は、図２１に示す現在地図６００と、図２８に示す現在地図の差分６００Ｄとに基づいて更新された、更新済み現在地図６００Ｕを示す図である。図３１は、図２１に示す現在地図６００と、図２９に示す現在地図の差分６００Ｄとに基づいて更新された、更新済み現在地図６００Ｕを示す図である。図３０および図３１においては、移動体３００がスタート地点から移動することで、移動体３００の座標原点Ｏ_３００も移動している。移動体３００の座標原点Ｏ_３００の位置および向きは、例えば、移動体３００の移動量を示すオドメトリデータや方位センサ（図示せず）による検知結果などに基づいて得られる。

図３０および図３１に示すように、現在地図生成部３４は、ステップＳ２４において生成した現在地図６００と、ステップＳ２７において生成した現在地図の差分６００Ｄとを重ね合わせたとき、同じ位置の小領域６００＿Ｒ_Ｓのいずれかに「１」が格納されている場合、当該小領域６００＿Ｒ_Ｓの値を「１」とし、いずれにも「１」が格納されていない場合、当該小領域６００＿Ｒ_Ｓの値を「０」のままとする。これにより、更新済み現在地図６００Ｕを生成することができる。

ステップＳ２８の完了後、処理はステップＳ２１に戻る。以降の処理では、移動体３００はスタート地点にいないため、処理はステップＳ２５に進み、ステップＳ２７において新たな現在地図の差分を生成し、ステップＳ２８において更新済み現在地図６００Ｕに新たな差分を重ねることにより、新たな更新済み現在地図６００Ｕが生成される。このように、図１７に示す現在地図生成部３４の処理は、所定時間毎に繰り返される。生成された現在地図は、現在地図記憶部３５（図１参照）に記憶される。

［自己位置推定方法］
次に、図４のステップＳ４における、移動体３００の自己位置を推定する方法について説明する。ステップＳ４における処理は、ステップＳ１で生成した事前地図、およびステップＳ３で生成または更新した現在地図を用いて、実際の移動領域内を自律移動する移動体３００が、自己位置を推定する処理である。図４ではステップＳ３の現在地図の生成または更新とステップＳ４の自己位置推定とは、異なるステップで示されているが、実際には移動体３００が自律移動する間に、所定周期で順番に、または並行して行われる。

図３２は、自己位置推定部３２による自己位置推定手順のフローチャートである。以下の説明において、移動体３００は、図２２および図２３に示す移動領域７００において、スタート地点とゴール地点との間の地点Ｐｃにいるとする。

まず、ステップＳ３１において、自己位置推定部３２は、距離センサ３０から距離データを、オドメトリ計算部３１からオドメトリデータを、それぞれ取得する。

ステップＳ３２において、地図切出部３６は、図４のステップＳ３にて生成した現在地図６００を用いて、現在地図における移動体３００を基準とした座標原点Ｏ_３００を含む所定範囲の切り出しを行う。図３３および図３４は、現在地図６００の切り出しの様子を示す図である。図３３に示す現在地図６００は、移動体３００が図２２に示す地点Ｐｃにいるときの更新済み現在地図６００Ｕ（図３０参照）である。図３４に示す現在地図６００は、移動体３００が図２３に示す地点Ｐｃにいるときの更新済み現在地図６００Ｕ（図３１参照）である。

所定範囲は、座標原点Ｏ_３００からＸｖ方向、Ｙｖ方向にそれぞれあらかじめ決められた距離に設定された境界によって規定される範囲である。なお、Ｘｖ方向、Ｙｖ方向は、方位センサなどによって得られた移動体３００の向きに基づいて決定される。

図３３に示す例では、所定範囲の境界は、座標原点Ｏ_３００から＋Ｘｖ方向に距離Ｌ_ｘ１、－Ｘｖ方向に距離Ｌｘ２、＋Ｙｖ方向に距離Ｌ_ｙ１、－Ｙｖ方向に距離Ｌｙ２離れた位置に設定されている。距離Ｌ_ｘ１、距離Ｌ_ｙ１、および距離Ｌ_ｙ２は、距離センサ３０が精度よく距離を計測できる範囲となるように設定されればよく、例えば距離センサ３０の走査線（図１７に示す走査線ＳＬ）の長さの７５％～８５％に設定されることが望ましい。また、それまでの移動体３００の移動により得られた現在地図を反映するため、移動体３００の後ろ側の距離Ｌ_ｘ２は、前側の距離Ｌ_ｘ１より長く設定される。具体的には、距離Ｌ_ｘ２は前側の距離Ｌ_ｘ１の４～６倍に設定されることが望ましい。

切り出した現在地図６００Ｃを図３５および図３６に示す。図３５は、図３３に示す現在地図６００から切り出された現在地図６００Ｃを、図３６は、図３４に示す現在地図６００から切り出された現在地図６００Ｃを、それぞれ示している。なお、図３４に示すように、移動体３００の位置が小領域を構成する格子線の交点上に位置しない場合、図３６に示すように、切り出された現在地図６００Ｃは、移動体３００の位置が交点上に位置するように、再構成される。この際、切り出された現在地図６００Ｃにおける格子線は、図３６に示すように、移動体３００の位置を基準として再設定され、新たな格子線によって構成される各格子が有する値は、位置的に対応する切り出される前の格子が有する値に設定されればよい。

ステップＳ３３において、候補点移動部３３は、オドメトリデータを用いて移動体３００の現在位置の候補位置および姿勢を計算する。オドメトリデータは前回の処理で得られた自己推定位置からの移動体３００の移動量を示すデータである。このため、現在位置の候補位置は、前回の処理で推定された自己位置にオドメトリデータが示す移動量を足し合わせることで算出される。

しかしながら、オドメトリデータは精度がそれほど高くないため、オドメトリデータが示す移動量を前回の処理で推定された自己位置に足し合わせても、精密な現在位置を得ることは困難である。このため、本開示では、オドメトリデータが示す移動量を用いて、いくつかの候補位置を算出している。

図３７および図３８は、候補位置の例を示す図である。図３７および図３８では、移動体３００の３つの候補位置Ｐ１，Ｐ２，Ｐ３が示されている。なお、図３７は、移動体３００が図２２に示す地点Ｐｃにいるときの候補位置の例を示しており、図３８は、移動体３００が図２３に示す地点Ｐｃにいるときの候補位置の例を示している。候補位置Ｐ１の移動体３００を基準とした座標の原点をＯ_３００＿Ｐ１、候補位置Ｐ２の移動体３００を基準とした座標の原点をＯ_３００＿Ｐ２、候補位置Ｐ３の移動体３００を基準とした座標の原点をＯ_３００＿Ｐ３とする。

ステップＳ３４において、地図切出部３６は、候補位置毎に、座標原点を基準として、図４のステップＳ１で生成した事前地図５００の切り出しを行う。切り出す範囲の境界設定方法は、ステップＳ３２における現在地図６００の切り出し方法と同じである。図３９は、図３７に示す候補位置Ｐ１で図１３に示す事前地図５００を切り出す様子を示す図であり、図４１は、切り出した事前地図５００Ｃを示す図である。図４０は、図３８に示す候補位置Ｐ１で図１３に示す事前地図５００を切り出す様子を示す図であり、図４２は、切り出した事前地図５００Ｃを示す図である。

ステップＳ３５において、スコア計算部３７は、ステップＳ３２で切り出した現在地図６００およびステップＳ３４で切りだした事前地図５００に基づいて、スコア計算を行う。具体的には、スコア計算部３７は、切り出された現在地図６００における移動体３００の座標原点Ｏ_３００と、切り出された事前地図５００の候補位置毎の座標原点と重ね合わせる。

事前地図５００と、現在地図６００とは、同じ大きさの小領域で分割されており、小領域毎に事前地図５００には確率密度の量子化値が、現在地図６００には物体の有無を示す値（「０」または「１」）が、それぞれ格納されている。スコア計算部３７は、所定の範囲の中で、現在地図６００において物体があるとされる小領域（すなわち値が「１」である小領域）を抽出し、抽出した小領域における事前地図５００の量子化値の総和を計算する。

図４３および図４４は、切り出した事前地図５００において、量子化値の総和を求める小領域を説明するための図である。図４１に示す、切り出した事前地図５００Ｃの小領域のうち、図３５に示す、切り出した現在地図６００Ｃにおいて「１」の値が格納された小領域は、図４３において、太実線で囲まれた小領域で示される。また、図４２に示す、切り出した事前地図５００Ｃの小領域のうち、図３６に示す、切り出した現在地図６００Ｃにおいて「１」の値が格納された小領域は、図４４において、太実線で囲まれた小領域で示される。スコア計算部３７は、これらの小領域に格納された確率密度の量子化値の総和を算出する。すなわち、図４３に示す例では、３１，８７６，７０８となる。また、図４４に示す例では、３０，１９８，９８６となる。このような処理を、全ての候補位置Ｐ１～Ｐ３について行う。

ステップＳ３６において、自己位置推定部３２は、全ての候補位置について計算されたスコアのうち、最もスコアが高い候補位置を、現在の移動体３００の推定位置として出力する。なお、最もスコアが高い候補位置を現在の移動体３００の推定位置とするのではなく、例えば、スコアが比較的高い上位複数個の候補位置の重心点を計算し、当該重心点を移動体３００の自己位置としてもよい。なお、図３７および図３８では、候補位置が３箇所である場合の例が示されているが、本開示では候補位置はより多くてもよいし、２つでもよい。候補位置が多い方が演算量は多くなるものの、自己位置推定精度は向上するため、所望の精度に合わせて候補位置は適宜設定されればよい。

以上説明した処理により、三次元データの処理、および、確率密度の計算など、複雑で処理負荷が大きい処理は、移動体３００が実際に移動を開始するより前に、事前地図生成装置２００によって行われており、その結果として事前地図５００が作成される。移動体３００は、実際に移動しながら自己位置を推定するときには、事前地図５００に基づいて自己位置の推定を行うため、移動体３００の自己位置推定処理に要する負荷が小さくて済む。特に、自己位置推定処理の際に行われる計算は、小領域内の量子化値の可算であるため、演算不可が大きい乗算などを行う必要がなく、処理速度を速くすることができる。このため、移動体３００が高度な計算能力を有していない場合でも、素早く、かつ高精度に自己位置を推定することができる。

本開示は、自律移動ロボットや、自動運転の分野で活用することができる。

１００ 位置推定システム
２００ 事前地図生成装置
３００ 移動体
２０，３０ 距離センサ
２１ エンコーダ
２２ ＩＭＵ
２３ カメラ
２４ 点群地図生成部
２５ オドメトリ計算部
２６ 事前地図生成部
３０ 距離センサ
３１ オドメトリ計算部
３２ 自己位置推定部
３３ 候補点移動部
３４ 現在地図生成部
３５ 現在地図記憶部
３６ 地図切出部
３７ スコア計算部
３８ 移動体制御部
３９ 事前地図記憶部
４００ 点群地図
４０１ 点群
５００ 事前地図
６００ 現在地図
７００ 移動領域
７０１，７０２ 物体
８００ データ構造
８０１ ファイルヘッダ
８０２ 情報ヘッダ
８０３ データ部
８０４ 追加データ部

Claims (7)

1. 特定の領域内を移動する移動体であって、
   前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を記憶する事前地図記憶部と、
   前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成する現在地図生成部と、
   前記事前地図および前記現在地図に基づいて、自己の位置を推定する自己位置推定部と、
   を備える、移動体。
2. 前記自己位置推定部は、前記候補位置毎に、前記現在地図が前記対象物の存在を示す全ての小領域に対応する前記事前地図の小領域を抽出し、抽出した小領域が示す前記確率密度の総和を算出し、当該総和に基づいて前記自己の位置を推定する、
   請求項１に記載の移動体。
3. 特定の領域内を移動する移動体の自己位置推定に用いられる事前地図を生成する事前地図生成装置であって、
   前記領域内の対象物の計測位置を示す点群を含む点群地図を生成する点群地図生成部と、
   前記領域を複数の大領域に分割した場合の大領域毎に対象物が存在する確率密度を算出し、前記複数の大領域のそれぞれを複数の小領域にさらに分割して前記確率密度を前記小領域毎に離散化し、前記小領域毎に離散化した前記確率密度を示す前記事前地図を生成する事前地図生成部と、
   を備える、事前地図生成装置。
4. 特定の領域内を移動する移動体の位置を推定する位置推定システムであって、
   前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を生成する事前地図生成装置と、
   前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成し、前記事前地図および前記現在地図に基づいて、自己の位置を推定する移動体と、
   を備える、位置推定システム。
5. 特定の領域内を移動する移動体の位置推定方法であって、
   前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を記憶し、
   前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成し、
   前記事前地図および前記現在地図に基づいて、自己の位置を推定する、
   位置推定方法。
6. 特定の領域内を移動する移動体の自己位置推定に用いられる事前地図を生成する事前地図生成方法であって、
   前記領域内の対象物の計測位置を示す点群を含む点群地図を生成し、
   前記領域を複数の大領域に分割した場合の大領域毎に対象物が存在する確率密度を算出し、
   前記複数の大領域のそれぞれを複数の小領域にさらに分割して前記確率密度を前記小領域毎に離散化し、
   前記小領域毎に離散化した前記確率密度を示す前記事前地図を生成する、
   事前地図生成方法。
7. 特定の領域内を移動する移動体の位置を推定するコンピュータが実行するプログラムであって、
   前記領域を複数の小領域に分割した場合の小領域毎に対象物が存在する確率密度を示す事前地図を記憶し、
   前記対象物までの距離および方位の計測値に基づいて、前記移動体の現在位置の候補位置を基準として、前記小領域毎の前記対象物の存在の有無を示す現在地図を生成し、
   前記事前地図および前記現在地図に基づいて、自己の位置を推定する、
   処理を前記コンピュータに実行させる、プログラム。

Images