特開 2022-111524 自己位置推定装置及び方法

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022111524

(43)【公開日】2022-08-01

(54)【発明の名称】自己位置推定装置及び方法

(51)【国際特許分類】

   G05D 1/02 20200101AFI20220725BHJP

【ＦＩ】

G05D1/02 L

【審査請求】未請求

【請求項の数】5

【出願形態】ＯＬ

(21)【出願番号】P 2021006998

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

(71)【出願人】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】100088155

【弁理士】

【氏名又は名称】長谷川 芳樹

(74)【代理人】

【識別番号】100113435

【弁理士】

【氏名又は名称】黒木 義樹

(74)【代理人】

【識別番号】100124062

【弁理士】

【氏名又は名称】三上 敬史

(74)【代理人】

【識別番号】100148013

【弁理士】

【氏名又は名称】中山 浩光

(72)【発明者】

【氏名】古室 達也

【テーマコード（参考）】

5H301

【Ｆターム（参考）】

5H301BB07

5H301CC03

5H301CC06

5H301GG08

(57)【要約】

【課題】３次元のレーザ検出部を使用する場合でも、計算コストを削減することができる自己位置推定装置及び方法を提供する。
【解決手段】自己位置推定装置１０は、移動体２の周囲に３次元のレーザを照射し、レーザの反射光を受光することで、移動体２の周囲に存在する物体７を検出して３次元点群データを取得するレーザセンサ３と、移動体２が走行するエリアのスコアマップを含むマップデータを記憶する記憶部４と、物体７の検出データとマップデータとに基づいて、移動体２の自己位置を推定する自己位置推定部１１と、物体７の検出データとマップデータとに基づいて、自己位置推定の信頼度を算出する信頼度算出部１４と、レーザセンサ３により取得された３次元点群データを２次元化する変換部１３とを備え、信頼度算出部１４は、検出データとして、変換部１３により得られた２次元点群データを使用すると共に、マップデータとしてスコアマップを使用する。
【選択図】図１

【特許請求の範囲】

【請求項1】

移動体の周囲に３次元のレーザを照射し、レーザの反射光を受光することで、前記移動体の周囲に存在する物体を検出して３次元点群データを取得するレーザ検出部と、
前記移動体が走行するエリアの２次元マップを含むマップデータを記憶する記憶部と、
前記移動体の周囲に存在する物体の検出データと前記記憶部に記憶された前記マップデータとに基づいて、前記移動体の自己位置を推定する自己位置推定部と、
前記移動体の周囲に位置する物体の検出データと前記記憶部に記憶された前記マップデータとに基づいて、前記自己位置推定部による前記移動体の自己位置推定の信頼度を算出する信頼度算出部と、
前記レーザ検出部により取得された３次元点群データを２次元化する変換部とを備え、
前記自己位置推定部及び前記信頼度算出部の少なくとも一方は、前記移動体の周囲に存在する物体の検出データとして、前記変換部により得られた２次元点群データを使用すると共に、前記マップデータとして前記２次元マップを使用する自己位置推定装置。

【請求項2】

前記マップデータは、前記移動体が走行するエリアの３次元マップを更に含み、
前記自己位置推定部は、前記レーザ検出部により取得された３次元点群データと前記３次元マップとに基づいて、前記移動体の自己位置を推定し、
前記信頼度算出部は、前記２次元点群データと前記２次元マップとに基づいて、前記自己位置推定の信頼度を算出する請求項１記載の自己位置推定装置。

【請求項3】

前記変換部は、前記レーザ検出部により取得された３次元点群データを２次元グリッドに落とし込んでフラグを立てることにより、前記３次元点群データを２次元化する請求項１または２記載の自己位置推定装置。

【請求項4】

前記移動体の走行状態に応じて、照合精度が異なる複数の前記２次元マップから使用する２次元マップを選択するマップ選択部を更に備え、
前記自己位置推定部及び前記信頼度算出部の少なくとも一方は、前記マップデータとして前記マップ選択部により選択された２次元マップを使用する請求項１～３の何れか一項記載の自己位置推定装置。

【請求項5】

移動体の周囲に３次元のレーザを照射し、レーザの反射光を受光することで、前記移動体の周囲に存在する物体を検出して３次元点群データを取得するレーザ検出部を用いて、前記移動体の自己位置を推定する自己位置推定方法において、
前記移動体が走行するエリアの２次元マップを含むマップデータを作成するマップデータ作成工程と、
前記移動体の周囲に存在する物体の検出データと前記マップデータ作成工程において作成された前記マップデータとに基づいて、前記移動体の自己位置を推定する自己位置推定工程と、
前記移動体の周囲に位置する物体の検出データと前記マップデータ作成工程において作成された前記マップデータとに基づいて、前記自己位置推定工程における前記移動体の自己位置推定の信頼度を算出する信頼度算出工程と、
前記レーザ検出部により取得された３次元点群データを２次元化する変換工程とを含み、
前記自己位置推定工程及び前記信頼度算出工程の少なくとも一方では、前記移動体の周囲に存在する物体の検出データとして、前記変換工程において得られた２次元点群データを使用すると共に、前記マップデータとして前記２次元マップを使用する自己位置推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定装置及び方法に関する。

【背景技術】

【0002】

従来の自己位置推定装置としては、例えば特許文献１に記載されているような技術が知られている。特許文献１に記載の自己位置推定装置は、車両の周囲の障害物の有無及び障害物までの距離を計測するＬＲＦ（レーザレンジファインダ）と、車両が自律走行を行う際に必要となる制御パラメータ及び地図等の情報を記憶する記憶部と、車両を設定された走行経路に追従して走行するように制御する車両制御部と、記憶部に記憶された地図とＬＲＦにより計測された計測データとを照合して、車両の現在位置及び姿勢を推定する位置姿勢推定部と、位置姿勢推定部による推定結果の信頼性を評価する信頼性評価部と、信頼性評価部による評価の結果が信頼性が低いことを示す場合に、車両制御部において用いられる制御パラメータ及び制御入力を変更する制御変更部とを備えている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１５－３６８４０号公報

【発明の概要】

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

【0004】

上記従来技術においては、地図ランドマーク間を結ぶ直線と地図上にプロットされた計測ランドマーク間を結ぶ直線との傾きの差に基づいて、位置姿勢推定部による推定結果の信頼性が評価される。しかし、車両の自己位置の推定精度を上げるために、ＬＲＦ等のレーザ検出部として３次元のセンサを使用した場合には、レーザ検出部の検出データである点群データは無数に存在するため、計測ランドマーク間を結ぶ直線を推定することが困難である。従って、自己位置推定の信頼性の評価の計算量が増えるため、計算コストが増大してしまう。

【0005】

本発明の目的は、３次元のレーザ検出部を使用する場合でも、計算コストを削減することができる自己位置推定装置及び方法を提供することである。

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

【0006】

本発明の一態様に係る自己位置推定装置は、移動体の周囲に３次元のレーザを照射し、レーザの反射光を受光することで、移動体の周囲に存在する物体を検出して３次元点群データを取得するレーザ検出部と、移動体が走行するエリアの２次元マップを含むマップデータを記憶する記憶部と、移動体の周囲に存在する物体の検出データと記憶部に記憶されたマップデータとに基づいて、移動体の自己位置を推定する自己位置推定部と、移動体の周囲に位置する物体の検出データと記憶部に記憶されたマップデータとに基づいて、自己位置推定部による移動体の自己位置推定の信頼度を算出する信頼度算出部と、レーザ検出部により取得された３次元点群データを２次元化する変換部とを備え、自己位置推定部及び信頼度算出部の少なくとも一方は、移動体の周囲に存在する物体の検出データとして、変換部により得られた２次元点群データを使用すると共に、マップデータとして２次元マップを使用する。

【0007】

このような自己位置推定装置においては、レーザ検出部によって移動体の周囲に存在する物体が検出されて３次元点群データが取得される。そして、自己位置推定部によって移動体の自己位置が推定されると共に、信頼度算出部によって自己位置推定の信頼度が算出される。移動体の自己位置の推定及び自己位置推定の信頼度の算出は、移動体の周囲に存在する物体の検出データと記憶部に記憶されたマップデータとに基づいて行われる。また、変換部によって３次元点群データが２次元化される。ここで、自己位置推定部及び信頼度算出部の少なくとも一方は、移動体の周囲に存在する物体の検出データとして、変換部により得られた２次元点群データを使用すると共に、マップデータとして２次元マップを使用する。このため、移動体の自己位置の推定及び自己位置推定の信頼度の算出の少なくとも一方の計算量が低減される。これにより、３次元のレーザ検出部を使用する場合でも、計算コストが削減される。

【0008】

マップデータは、移動体が走行するエリアの３次元マップを更に含み、自己位置推定部は、レーザ検出部により取得された３次元点群データと３次元マップとに基づいて、移動体の自己位置を推定し、信頼度算出部は、２次元点群データと２次元マップとに基づいて、自己位置推定の信頼度を算出してもよい。

【0009】

このような構成では、２次元点群データと２次元マップとに基づいて自己位置推定の信頼度が算出されるため、自己位置推定の信頼度算出の計算量が低減される。従って、自己位置推定の信頼度算出の計算コストが削減される。また、３次元点群データと３次元マップとに基づいて移動体の自己位置が推定されるため、自己位置の推定精度が向上する。

【0010】

変換部は、レーザ検出部により取得された３次元点群データを２次元グリッドに落とし込んでフラグを立てることにより、３次元点群データを２次元化してもよい。

【0011】

このような構成では、３次元点群データが物体の高さ方向に増えると、その分だけ使用される３次元点群データが増えることになる。また、レーザが路面及び天井で反射されることで取得された３次元点群データは、ノイズとして除外される。このため、レーザが建屋の壁等で反射されることで取得された３次元点群データが、主として使用されることになる。以上により、移動体の自己位置の推定精度及び自己位置推定の信頼性の少なくとも一方が高くなる。

【0012】

自己位置推定装置は、移動体の走行状態に応じて、照合精度が異なる複数の２次元マップから使用する２次元マップを選択するマップ選択部を更に備え、自己位置推定部及び信頼度算出部の少なくとも一方は、マップデータとしてマップ選択部により選択された２次元マップを使用してもよい。

【0013】

このような構成では、移動体の自己位置の推定精度及び自己位置推定の信頼度の算出精度の少なくとも一方を、移動体の走行状態に応じて適宜変更することができる。

【0014】

本発明の他の態様は、移動体の周囲に３次元のレーザを照射し、レーザの反射光を受光することで、移動体の周囲に存在する物体を検出して３次元点群データを取得するレーザ検出部を用いて、移動体の自己位置を推定する自己位置推定方法において、移動体が走行するエリアの２次元マップを含むマップデータを作成するマップデータ作成工程と、移動体の周囲に存在する物体の検出データとマップデータ作成工程において作成されたマップデータとに基づいて、移動体の自己位置を推定する自己位置推定工程と、移動体の周囲に位置する物体の検出データとマップデータ作成工程において作成されたマップデータとに基づいて、自己位置推定工程における移動体の自己位置推定の信頼度を算出する信頼度算出工程と、レーザ検出部により取得された３次元点群データを２次元化する変換工程とを含み、自己位置推定工程及び信頼度算出工程の少なくとも一方では、移動体の周囲に存在する物体の検出データとして、変換工程において得られた２次元点群データを使用すると共に、マップデータとして２次元マップを使用する。

【0015】

このような自己位置推定方法においては、レーザ検出部によって移動体の周囲に存在する物体が検出されて３次元点群データが取得される。そして、自己位置推定工程において移動体の自己位置が推定されると共に、信頼度算出工程において自己位置推定の信頼度が算出される。移動体の自己位置の推定及び自己位置推定の信頼度の算出は、移動体の周囲に存在する物体の検出データと記憶部に記憶されたマップデータとに基づいて行われる。また、変換工程において３次元点群データが２次元化される。ここで、自己位置推定工程及び信頼度算出工程の少なくとも一方では、移動体の周囲に存在する物体の検出データとして、変換工程において得られた２次元点群データを使用すると共に、マップデータとして２次元マップを使用する。このため、移動体の自己位置の推定及び自己位置推定の信頼度の算出の少なくとも一方の計算量が低減される。これにより、３次元のレーザ検出部を使用する場合でも、計算コストが削減される。

【発明の効果】

【0016】

本発明によれば、３次元のレーザ検出部を使用する場合でも、計算コストを削減することができる。

【図面の簡単な説明】

【0017】

【図1】本発明の一実施形態に係る自己位置推定装置を具備した走行制御装置を示す概略構成図である。

【図2】レーザセンサにより取得された３次元点群データを模式的に示す概念図である。

【図3】スコアマップの一例を示す平面図である。

【図4】図２に示された３次元点群データを２次元グリッドに落とし込む様子を示す概念図である。

【図5】図２に示された３次元点群データが２次元化された状態を示す平面図である。

【図6】２次元点群データとスコアマップとを比較して、自己位置推定の信頼度を算出する様子を示す平面図である。

【図7】スコアマップを含むマップデータを作成する際に使用されるスコアマップ作成システムを示す概略構成図である。

【図8】ユーザがパソコンを使用してスコアマップを作成する工程を示すフローチャートである。

【図9】本発明の他の実施形態に係る自己位置推定装置を具備した走行制御装置を示す概略構成図である。

【図10】マップ選択部により実行される選択処理の手順を示すフローチャートである。

【図11】ユーザがパソコンを使用してスコアマップを作成する工程の変形例を示すフローチャートである。

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

【0018】

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。

【0019】

図１は、本発明の一実施形態に係る自己位置推定装置を具備した走行制御装置を示す概略構成図である。図１において、走行制御装置１は、フォークリフト等の移動体２（図２参照）に搭載されている。走行制御装置１は、移動体２を自律走行させる装置である。走行制御装置１は、３次元用のレーザセンサ３と、記憶部４と、コントローラ５と、駆動部６とを備えている。

【0020】

レーザセンサ３は、図２に示されるように、移動体２の周囲に３次元のレーザを照射し、レーザの反射光を受光することで、移動体２の周囲に存在する物体７を検出して３次元点群データを取得するレーザ検出部である。物体７は、例えば建屋の壁や柱等である。３次元点群データは、レーザの反射点であるスキャン点Ｓの集まりのデータである。レーザセンサ３としては、例えばレーザレンジファインダが用いられる。

【0021】

記憶部４は、移動体２が走行するエリアのマップ（地図）データを記憶する。記憶部４は、照合マップ記憶部８と、スコアマップ記憶部９とを有している。照合マップ記憶部８は、移動体２が走行するエリアの３次元マップを自己位置推定用の照合マップとして記憶する。スコアマップ記憶部９は、移動体２が走行するエリアの２次元マップをスコアマップとして記憶する。

【0022】

スコアマップは、図３に示されるように、２次元グリッド上に「０」及び「１」の２値画像で表されている。スコアマップにおいて、高さが設定値Ｚ以上である物体７（後述）に相当する点群データＡは、「１」である。なお、スコアマップを含むマップデータの作成方法については、後で詳述する。

【0023】

コントローラ５は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ５は、自己位置推定部１１と、走行制御部１２と、変換部１３と、信頼度算出部１４とを有している。ここで、レーザセンサ３、記憶部４、コントローラ５の自己位置推定部１１、変換部１３及び信頼度算出部１４は、本実施形態の自己位置推定装置１０を構成している。

【0024】

自己位置推定部１１は、レーザセンサ３により取得された３次元点群データと照合マップ記憶部８に記憶された照合マップとに基づいて、移動体２の自己位置を推定する自己位置推定工程を実施する。３次元点群データは、移動体２の周囲に存在する物体７の検出データである。３次元マップは、記憶部４に記憶されたマップデータの一つである。移動体２の自己位置は、移動体２の２次元座標位置及び向き（姿勢）で表される。

【0025】

自己位置推定部１１は、例えばレーザＳＬＡＭ（simultaneous localization andmapping）手法を用いて、移動体２の自己位置の推定演算を行う。ＳＬＡＭは、センサデータ及びマップデータを使って自己位置推定を行う自己位置推定技術である。自己位置推定部１１は、３次元点群データと照合マップとをマッチングさせて、移動体２の自己位置の推定演算を行う。

【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】

２…移動体、３…レーザセンサ（レーザ検出部）、４…記憶部、７…物体、１０…自己位置推定装置、１１…自己位置推定部、１３…変換部、１４…信頼度算出部、３２…マップ選択部。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】