特開 2024-121391 自己位置推定装置

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

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

(11)【公開番号】P2024121391

(43)【公開日】2024-09-06

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

(51)【国際特許分類】

   G01S 17/89 20200101AFI20240830BHJP

   G05D 1/43 20240101ALI20240830BHJP

【ＦＩ】

G01S17/89

G05D1/02 L

【審査請求】未請求

【請求項の数】4

【出願形態】ＯＬ

(21)【出願番号】P 2023028469

(22)【出願日】2023-02-27

(71)【出願人】

【識別番号】000003218

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

(74)【代理人】

【識別番号】100088155

【弁理士】

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

(74)【代理人】

【識別番号】100113435

【弁理士】

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

(74)【代理人】

【識別番号】100124062

【弁理士】

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

(74)【代理人】

【識別番号】100148013

【弁理士】

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

(72)【発明者】

【氏名】坂元 優太

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

5H301

5J084

【Ｆターム（参考）】

5H301AA01

5H301BB05

5H301BB07

5H301CC03

5H301CC06

5H301CC10

5H301DD01

5H301DD07

5H301DD15

5H301GG08

5J084AA04

5J084AA05

5J084AA10

5J084AC02

5J084BA03

5J084CA31

5J084CA65

5J084EA01

(57)【要約】

【課題】移動体の自己位置の推定精度を向上させることができる自己位置推定装置を提供する。
【解決手段】自己位置推定装置１は、移動体２の周囲にレーザＬを照射し、レーザＬの反射光を受光することにより、移動体２の周囲に存在する物体６までの距離を検出し、点群データＤを出力するレーザセンサ３と、レーザセンサ３の点群データＤを複数の検知領域Ｒに分割する検知領域設定部１２と、検知領域Ｒ毎に点群データＤの点群Ｐの分散値を算出する分散値算出部１３と、点群Ｐの分散値に基づいて、検知領域Ｒがノイズを含んでいるかどうかを判定するノイズ判定部１４と、検知領域Ｒがノイズを含んでいると判定されたときに、検知領域Ｒの点群Ｐが除去された補正点群データＤｃを生成する点群データ補正部１５と、補正点群データＤｃに基づいて、移動体２の自己位置を推定する自己位置推定部１６とを備える。
【選択図】図１

【特許請求の範囲】

【請求項1】

移動体の自己位置の推定を行う自己位置推定装置において、
前記移動体の周囲にレーザを照射し、レーザの反射光を受光することにより、前記移動体の周囲に存在する物体までの距離を検出し、検出データとしての点群データを出力するレーザ距離検出部と、
前記レーザ距離検出部の点群データを複数の検知領域に分割する検知領域設定部と、
前記検知領域設定部により設定された前記検知領域毎に前記点群データの点群の分布状態を検知する点群分布検知部と、
前記点群分布検知部により検知された前記点群の分布状態に基づいて、前記検知領域がノイズを含んでいるかどうかを判定するノイズ判定部と、
前記ノイズ判定部により前記検知領域が前記ノイズを含んでいると判定されたときに、前記ノイズを含んでいる前記検知領域の点群が除去された補正点群データを生成する点群データ補正部と、
前記点群データ補正部により生成された前記補正点群データに基づいて、前記移動体の自己位置を推定する自己位置推定部とを備える自己位置推定装置。

【請求項2】

前記点群分布検知部は、前記検知領域毎に前記点群データの点群の分散値を算出し、
前記ノイズ判定部は、前記点群の分散値が予め決められた規定値以上であるときに、前記検知領域が前記ノイズを含んでいると判定する請求項１記載の自己位置推定装置。

【請求項3】

前記検知領域設定部は、前記検知領域として、第１領域と、前記第１領域よりも面積が小さい第２領域とを設定し、
前記点群分布検知部は、前記第１領域毎に前記点群データの点群の分布状態を検知し、その後前記第２領域毎に前記点群データの点群の分布状態を検知し、
前記ノイズ判定部は、前記点群の分布状態に基づいて、前記第１領域が前記ノイズを含んでいるかどうかを判定し、その後前記点群の分布状態に基づいて、前記第２領域が前記ノイズを含んでいるかどうかを判定し、
前記点群データ補正部は、前記ノイズ判定部により前記第２領域が前記ノイズを含んでいると判定されたときに、前記ノイズを含んでいる前記第２領域の点群が除去された前記補正点群データを生成する請求項１または２記載の自己位置推定装置。

【請求項4】

前記点群分布検知部は、前記ノイズ判定部により前記第１領域が前記ノイズを含んでいないと判定されたときは、前記ノイズを含んでいない前記第１領域内における前記第２領域毎の前記点群の分布状態の検知を行わない請求項３記載の自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

従来の自己位置推定装置としては、例えば特許文献１に記載されている技術が知られている。特許文献１に記載の自己位置推定装置は、ＬｉＤＡＲにより周囲の環境情報を取得すると共に、カメラにより周囲の画像情報を取得し、環境情報または画像情報にノイズ障害物が含まれるか否かを判定する。そして、自己位置推定装置は、ノイズ障害物が含まれていたときは、環境情報からノイズ測定点を除去して加工環境情報を取得し、加工環境情報と地図情報とを比較照合して自己位置を算出し、ノイズ障害物が含まれていないときは、環境情報と地図情報とを比較照合して自己位置を算出する。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０２１－１３１７１３号公報

【発明の概要】

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

【0004】

しかしながら、上記従来技術においては、障害物等の実態のある検出データは除去可能であるが、レーザの乱反射等によるノイズデータは除去することができない。ノイズデータを用いて自己位置が算出されると、自己位置推定の精度の低下を招いてしまう。

【0005】

本発明の目的は、移動体の自己位置の推定精度を向上させることができる自己位置推定装置を提供することである。

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

【0006】

本発明の一態様は、移動体の自己位置の推定を行う自己位置推定装置において、移動体の周囲にレーザを照射し、レーザの反射光を受光することにより、移動体の周囲に存在する物体までの距離を検出し、検出データとしての点群データを出力するレーザ距離検出部と、レーザ距離検出部の点群データを複数の検知領域に分割する検知領域設定部と、検知領域設定部により設定された検知領域毎に点群データの点群の分布状態を検知する点群分布検知部と、点群分布検知部により検知された点群の分布状態に基づいて、検知領域がノイズを含んでいるかどうかを判定するノイズ判定部と、ノイズ判定部により検知領域がノイズを含んでいると判定されたときに、ノイズを含んでいる検知領域の点群が除去された補正点群データを生成する点群データ補正部と、点群データ補正部により生成された補正点群データに基づいて、移動体の自己位置を推定する自己位置推定部とを備える。

【0007】

このような自己位置推定装置においては、レーザ距離検出部によって移動体の周囲に存在する物体までの距離が検出され、点群データが出力される。そして、レーザ距離検出部の点群データが複数の検知領域に分割され、検知領域毎に点群データの点群の分布状態が検知される。そして、点群の分布状態に基づいて、検知領域がノイズを含んでいるかどうかが判定され、検知領域がノイズを含んでいると判定されたときは、当該検知領域の点群が除去された補正点群データが生成される。そして、補正点群データに基づいて移動体の自己位置が推定される。ここで、レーザの乱反射等によるノイズがレーザ距離検出部に入力されると、レーザ距離検出部により検出される距離が定まらず、点群データの点群が広範囲に分布するという特徴がある。そこで、点群データの検知領域毎に点群の分布状態を検知し、点群の分布状態から検知領域がノイズを含んでいると判定されたときは、当該検知領域の点群を除去することにより、レーザの乱反射等によるノイズの影響を除外して、移動体の自己位置の推定が行われることとなる。これにより、移動体の自己位置の推定精度が向上する。

【0008】

点群分布検知部は、検知領域毎に点群データの点群の分散値を算出し、ノイズ判定部は、点群の分散値が予め決められた規定値以上であるときに、検知領域がノイズを含んでいると判定してもよい。

【0009】

このような構成では、点群データの点群の分散値を算出することにより、点群の分布状態が正確に検知されるため、検知領域がノイズを含んでいるかどうかを精度良く判定することができる。

【0010】

検知領域設定部は、検知領域として、第１領域と、第１領域よりも面積が小さい第２領域とを設定し、点群分布検知部は、第１領域毎に点群データの点群の分布状態を検知し、その後第２領域毎に点群データの点群の分布状態を検知し、ノイズ判定部は、点群の分布状態に基づいて、第１領域がノイズを含んでいるかどうかを判定し、その後点群の分布状態に基づいて、第２領域がノイズを含んでいるかどうかを判定し、点群データ補正部は、ノイズ判定部により第２領域がノイズを含んでいると判定されたときに、ノイズを含んでいる第２領域の点群が除去された補正点群データを生成してもよい。

【0011】

このような構成では、第１領域と第１領域よりも面積が小さい第２領域との２段階によって、点群データの点群の分布状態を検知することにより、検知領域がノイズを含んでいるかどうかを精度良く判定することができる。また、第２領域がノイズを含んでいると判定されたときは、当該第２領域の点群のみを除去することにより、自己位置の推定に使用可能な点群が除去されてしまうことを抑制できる。

【0012】

点群分布検知部は、ノイズ判定部により第１領域がノイズを含んでいないと判定されたときは、ノイズを含んでいない第１領域内における第２領域毎の点群の分布状態の検知を行わなくてもよい。

【0013】

このような構成では、第１領域がノイズを含んでいると判定されたときのみ、当該第１領域内における第２領域毎の点群の分布状態の検知を行い、第１領域がノイズを含んでいないと判定されたときは、当該第１領域内における第２領域毎の点群の分布状態の検知を行わないため、点群の分布状態の検知を実行する回数が減少する。従って、演算処理の簡素化を図ることができる。

【発明の効果】

【0014】

本発明によれば、移動体の自己位置の推定精度を向上させることができる。

【図面の簡単な説明】

【0015】

【図1】本発明の一実施形態に係る自己位置推定装置の構成を示すブロック図である。

【図2】レーザセンサから物体に向けてレーザが照射されたときのレーザセンサの点群データの一例を示す図である。

【図3】レーザセンサに対するノイズが発生する具体例を示す図である。

【図4】レーザセンサにノイズデータが入力されたときのレーザセンサの点群データの一例を示す図である。

【図5】レーザセンサの点群データを複数の検知領域に分割した状態を示す概念図である。

【図6】図１に示されたコントローラにより実行される処理の手順を示すフローチャートである。

【図7】レーザセンサの点群データ及び補正点群データの一例を示す図である。

【図8】点群の分散値が小さい検知領域と点群の分散値が大きい検知領域とを比較して示す概念図である。

【図9】本発明の他の実施形態に係る自己位置推定装置の構成を示すブロック図である。

【図10】レーザセンサの点群データを複数の検知領域に２段階で分割した状態を示す概念図である。

【図11】図９に示されたコントローラにより実行される処理の手順を示すフローチャートである。

【図12】レーザセンサの点群データを複数の検知領域に３段階で分割した状態を示す概念図である。

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

【0016】

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

【0017】

図１は、本発明の一実施形態に係る自己位置推定装置の構成を示すブロック図である。図１において、本実施形態の自己位置推定装置１は、例えばフォークリフトやトーイングトラクタ等の産業車両といった移動体２（図２参照）に搭載されている。自己位置推定装置１は、移動体２の自動運転時に、移動体２の自己位置の推定を行う装置である。

【0018】

自己位置推定装置１は、レーザセンサ３と、地図データメモリ４と、コントローラ５とを備えている。

【0019】

レーザセンサ３は、図２に示されるように、移動体２の周囲にレーザＬを照射し、レーザＬの反射光を受光することにより、移動体２の周囲に存在する物体６までの距離を検出し、検出データとしての点群データを出力するレーザ距離検出部である。物体６は、例えば建物、棚、壁や柱等の構造物である。レーザセンサ３としては、例えば３ＤのＬｉＤＡＲ（Light Detection and Ranging）やレーザレンジファインダ等が使用される。

【0020】

地図データメモリ４は、移動体２が走行するエリアの地図データを記憶する。地図データは、点群で表されている。地図データは、レーザセンサ３を用いて予め生成されている。

【0021】

コントローラ５は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ５は、点群データ取得部１１と、検知領域設定部１２と、分散値算出部１３と、ノイズ判定部１４と、点群データ補正部１５と、自己位置推定部１６とを有している。

【0022】

点群データ取得部１１は、図２に示されるように、レーザセンサ３の点群データＤを取得する。レーザセンサ３により検出された物体６は、点群Ｐで表される。点群Ｐは、レーザセンサ３から照射されるレーザＬの反射点の集まりである。

【0023】

図２（ａ）に示されるように、レーザセンサ３から棚６Ａ及び壁６Ｂに向けて照射されたレーザＬは、棚６Ａ及び壁６Ｂで反射されてレーザセンサ３に戻ってくる。このため、レーザセンサ３により棚６Ａ及び壁６Ｂまでの距離を検出することは可能である。従って、図２（ｂ）に示されるように、棚６Ａ及び壁６Ｂに相当する点群Ｐａ，Ｐｂを含む点群データＤが得られる。

【0024】

ところで、移動体２の周囲では、図３に示されるように、レーザセンサ３に対するノイズが発生することがある。レーザセンサ３に対するノイズとしては、他のレーザセンサ５１からのレーザＬｘ（図３（ａ）参照）、レーザセンサ３から照射されて鏡面５２または水面等で乱反射するレーザＬｘ（図３（ｂ）参照）、レーザセンサ３から照射されて、風により揺れ動く草木等といった形状が不安定な物体５３で乱反射するレーザＬｘ（図３（ｃ）参照）が挙げられる。

【0025】

そのようなノイズが発生することで、レーザセンサ３にノイズデータが入力された場合には、レーザセンサ３により検出される物体６までの距離が定まらない。このため、図４に示されるように、点群データＤに含まれる点群Ｐが広範囲に分布してばらつきやすくなる。そこで、コントローラ５は、検知領域設定部１２、分散値算出部１３、ノイズ判定部１４及び点群データ補正部１５を有している。

【0026】

検知領域設定部１２は、図５に示されるように、点群データ取得部１１により取得されたレーザセンサ３の点群データＤを複数の検知領域Ｒに分割する。つまり、検知領域設定部１２は、レーザセンサ３の点群データＤを分割して複数の検知領域Ｒを設定する。検知領域Ｒは、例えば四角形状を呈している。

【0027】

分散値算出部１３は、検知領域設定部１２により設定された複数の検知領域Ｒ毎に点群データＤの点群Ｐの分散値を算出する。分散値算出部１３は、検知領域Ｒ毎に点群データＤの点群Ｐの分布状態を検知する点群分布検知部を構成している。

【0028】

ノイズ判定部１４は、分散値算出部１３により算出された点群Ｐの分散値に基づいて、検知領域Ｒがノイズを含んでいるかどうかを判定する。つまり、ノイズ判定部１４は、点群Ｐの分布状態に基づいて、検知領域Ｒがノイズを含んでいるかどうかを判定する。ノイズ判定部１４は、点群Ｐの分散値が予め決められた規定値以上であるときに、検知領域Ｒがノイズを含んでいると判定する。

【0029】

点群データ補正部１５は、ノイズ判定部１４により検知領域Ｒがノイズを含んでいると判定されたときに、検知領域Ｒの点群Ｐが除去された補正点群データＤｃ（図７参照）を生成する。

【0030】

自己位置推定部１６は、点群データ取得部１１により取得されたレーザセンサ３の点群データＤまたは点群データ補正部１５により生成された補正点群データＤｃに基づいて、移動体２の自己位置を推定する。自己位置推定部１６は、ＳＬＡＭ（simultaneous localization andmapping）手法を用いて、レーザセンサ３の点群データまたは補正点群データと地図データメモリ４に記憶された地図データとをマッチングさせて、移動体２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを用いて自己位置推定を行う自己位置推定技術である。

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

コントローラ５Ａは、全ての小検知領域Ｒ２について点群Ｐの分散値を算出したと判断したときは、手順Ｓ１２９でノイズが含まれていると判定された全ての小検知領域Ｒ２を除外対象検知領域として特定する（手順Ｓ１３２）。そして、コントローラ５Ａは、除外対象検知領域に含まれる点群Ｐを除去することで補正点群データＤｃ（図７参照）を生成する（手順Ｓ１３３）。

【0063】

続いて、コントローラ５Ａは、上述した実施形態におけるコントローラ５と同様に、上記の手順Ｓ１１０以降を実行し、移動体２の自己位置を推定する。

【0064】

ここで、検知領域設定部１２Ａは、上記の手順Ｓ１２１を実行する。分散値算出部１３Ａは、上記の手順Ｓ１２２，Ｓ１２５～Ｓ１２７を実行する。ノイズ判定部１４Ａは、上記の手順Ｓ１２３，Ｓ１２４，Ｓ１２８～Ｓ１３０を実行する。点群データ補正部１５Ａは、上記の手順Ｓ１３１～Ｓ１３３を実行する。

【0065】

以上のような本実施形態においては、大検知領域Ｒ１と大検知領域Ｒ１よりも面積が小さい小検知領域Ｒ２との２段階によって、点群データの点群Ｐの分散値を算出することにより、検知領域Ｒがノイズを含んでいるかどうかを精度良く判定することができる。また、小検知領域Ｒ２がノイズを含んでいると判定されたときは、当該小検知領域Ｒ２の点群Ｐのみを除去することにより、自己位置の推定に使用可能な点群Ｐが除去されてしまうことを抑制できる。

【0066】

また、本実施形態では、大検知領域Ｒ１がノイズを含んでいると判定されたときのみ、当該大検知領域Ｒ１内における小検知領域Ｒ２毎の点群Ｐの分散値の算出を行い、大検知領域Ｒ１がノイズを含んでいないと判定されたときは、当該大検知領域Ｒ１内における小検知領域Ｒ２毎の点群Ｐの分散値の算出を行わないため、点群Ｐの分散値の算出を実行する回数が減少する。従って、演算処理の簡素化を図ることができる。

【0067】

なお、本実施形態では、大検知領域Ｒ１と小検知領域Ｒ２との２段階によって、点群データＤの点群Ｐの分散値が算出されているが、特にその形態に限られず、ノイズを含んでいる検知領域Ｒを段階的に絞り込んでいけばよい。例えば図１２に示されるように、大検知領域Ｒａと中検知領域Ｒｂと小検知領域Ｒｃとの３段階によって、点群データＤの点群Ｐの分散値を算出してもよい。図１２では、大検知領域Ｒａは、点群データＤを３分割して設定され、中検知領域Ｒｂは、点群データＤを１２分割して設定され、小検知領域Ｒｃは、点群データＤを４８分割して設定されている。

【0068】

以上、本発明の実施形態について幾つか説明してきたが、本発明は上記実施形態には限定されない。例えば上記実施形態では、レーザセンサ３の点群データＤが複数の検知領域Ｒに分割され、検知領域Ｒ毎に点群データＤの点群Ｐの分散値が算出されているが、特にその形態には限られず、検知領域Ｒ毎に点群データＤの点群の分布状態（ばらつき状態）が検知されればよい。

【0069】

また、上記実施形態では、検知領域Ｒが四角形状を呈しているが、検知領域Ｒの形状としては、特に四角形状には限られず、レーザセンサ３の点群データＤが複数の検知領域Ｒに分割されるのであれば、三角形状や六角形状等であってもよい。

【0070】

また、上記実施形態では、３Ｄのレーザセンサ３が使用されているが、特にその形態には限られず、２Ｄのレーザセンサ３を使用してもよい。

【符号の説明】

【0071】

１，１Ａ…自己位置推定装置、２…移動体、３…レーザセンサ（レーザ距離検出部）、６…物体、１２，１２Ａ…検知領域設定部、１３，１３Ａ…分散値算出部（点群分布検知部）、１４，１４Ａ…ノイズ判定部、１５，１５Ａ…点群データ補正部、１６…自己位置推定部、Ｄ…点群データ、Ｄｃ…補正点群データ、Ｌ…レーザ、Ｐ…点群、Ｒ…検知領域、Ｒ１…大検知領域（第１領域）、Ｒ２…小検知領域（第２領域）。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】