特開 2024-150980 レーザ検出処理装置

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

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

(11)【公開番号】P2024150980

(43)【公開日】2024-10-24

(54)【発明の名称】レーザ検出処理装置

(51)【国際特許分類】

   G01S 17/86 20200101AFI20241017BHJP

   G05D 1/43 20240101ALI20241017BHJP

   G01S 17/89 20200101ALI20241017BHJP

   G01B 11/24 20060101ALI20241017BHJP

   G01C 3/06 20060101ALI20241017BHJP

【ＦＩ】

G01S17/86

G05D1/02 L

G01S17/89

G01B11/24 Z

G01C3/06 120Q

G01C3/06 140

【審査請求】未請求

【請求項の数】5

【出願形態】ＯＬ

(21)【出願番号】P 2023064066

(22)【出願日】2023-04-11

(71)【出願人】

【識別番号】000003218

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

(71)【出願人】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(74)【代理人】

【識別番号】100088155

【弁理士】

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

(74)【代理人】

【識別番号】100113435

【弁理士】

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

(74)【代理人】

【識別番号】100124062

【弁理士】

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

(74)【代理人】

【識別番号】100148013

【弁理士】

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

(72)【発明者】

【氏名】服部 晋悟

(72)【発明者】

【氏名】田中 稔

(72)【発明者】

【氏名】津坂 祐司

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

2F065

2F112

5H301

5J084

【Ｆターム（参考）】

2F065AA04

2F065AA06

2F065AA53

2F065CC14

2F065DD19

2F065FF12

2F065FF32

2F065FF41

2F065FF67

2F065GG04

2F065QQ23

2F065QQ27

2F065QQ31

2F065UU05

2F112AD01

2F112BA05

2F112CA08

2F112CA12

2F112DA15

2F112DA25

2F112EA05

2F112FA03

2F112FA21

2F112FA35

2F112FA45

2F112GA01

5H301AA01

5H301AA10

5H301BB05

5H301BB07

5H301CC03

5H301CC06

5H301DD01

5H301DD07

5H301DD15

5H301FF11

5H301GG08

5H301GG14

5H301GG16

5H301GG19

5J084AA04

5J084AA05

5J084AC02

5J084BA03

5J084BA39

5J084BA40

5J084BA49

5J084EA11

(57)【要約】

【課題】点群データを適切な周期で適切なデータ量だけ出力することができるレーザ検出処理装置を提供する。
【解決手段】レーザ検出処理装置２０は、移動体２の周囲にレーザを照射し、レーザの反射光を受光することにより、移動体２の周囲に存在する物体を検出し、検出データとしての点群データを出力するレーザセンサ３と、移動体２の走行状態を検知し、移動体２の自己位置を推定する補正用自己位置推定部１２と、移動体２の自己位置とレーザセンサ３の点群データの時刻情報とに基づいて点群データを補正し、補正点群データを生成する点群補正部１３と、補正点群データの出力周期と周期毎の補正点群データの出力データ量とを決定する周期・データ量決定部１４と、出力周期及び出力データ量に応じて補正点群データを出力する点群データ出力部１５とを備える。
【選択図】図１

【特許請求の範囲】

【請求項1】

移動体の周囲にレーザを照射し、レーザの反射光を受光することにより、前記移動体の周囲に存在する物体を検出し、検出データとしての点群データを出力するレーザセンサと、
前記移動体の走行状態を検知し、前記移動体の自己位置を推定する自己位置推定部と、
前記自己位置推定部により推定された前記移動体の自己位置と前記レーザセンサの前記点群データの時刻情報とに基づいて前記点群データを補正し、補正点群データを生成する点群補正部と、
前記点群補正部により生成された前記補正点群データの出力周期と周期毎の前記補正点群データの出力データ量とを決定する周期・データ量決定部と、
前記周期・データ量決定部により決定された前記出力周期及び前記出力データ量に応じて前記補正点群データを出力する点群データ出力部とを備えるレーザ検出処理装置。

【請求項2】

前記点群データ出力部は、前記補正点群データの一部を前周期と重複して出力する請求項１記載のレーザ検出処理装置。

【請求項3】

前記点群データ出力部は、前記補正点群データの一部を出力しない請求項１記載のレーザ検出処理装置。

【請求項4】

前記点群データ出力部は、前記自己位置推定部により前記移動体の走行状態が不安定であることが検知されたときは、前記移動体の走行状態が不安定である時点の前記補正点群データを出力せずに、前記移動体の走行状態が不安定でない時間帯の前記補正点群データを出力する請求項１記載のレーザ検出処理装置。

【請求項5】

前記点群データ出力部は、予め決められた基準時刻から現時刻までの経過時間を前記補正点群データと共に出力する請求項１記載のレーザ検出処理装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、レーザ検出処理装置に関する。

【背景技術】

【0002】

従来のレーザ検出処理装置としては、例えば特許文献１に記載されているように、ＬｉＤＡＲ等のレーザセンサの点群データを取得し、レーザセンサの点群データと地図点群とのスキャンマッチングを行うことで、車両の自己位置を推定する技術が知られている。

【先行技術文献】

【特許文献】

【0003】

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

【発明の概要】

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

【0004】

ところで、車両が移動すると、レーザセンサの観測位置がずれるため、レーザセンサにより検出された点群の位置もずれる。このため、車両の自己位置の推定精度が低下する。これに対し、例えば車両の車速及び角速度に基づいて車両の自己位置を推定し、その推定結果を用いてレーザセンサの点群データを点毎に補正することで、レーザセンサの点群データに基づいた車両の自己位置の推定精度を向上させることができる。ここで、自己位置推定とは、作業エリア内での車両位置・姿勢を推定することのみならず、パレット等の荷役対象物との相対関係を推定すること等を含む。

【0005】

しかし、レーザセンサの点群データは、レーザセンサにより検出された点群をそのまま出力しただけのデータである。このため、点群データの出力周期及び出力データ量は、レーザセンサにより決まっている。点群データの適切な出力周期及び出力データ量は必要とする機能によって異なるが、従来では点群データの出力周期及び出力データ量が１機能分しか設定されなかった。

【0006】

本発明の目的は、点群データを適切な周期で適切なデータ量だけ出力することができるレーザ検出処理装置を提供することである。

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

【0007】

（１）本発明の一態様に係るレーザ検出処理装置は、移動体の周囲にレーザを照射し、レーザの反射光を受光することにより、移動体の周囲に存在する物体を検出し、検出データとしての点群データを出力するレーザセンサと、移動体の走行状態を検知し、移動体の自己位置を推定する自己位置推定部と、自己位置推定部により推定された移動体の自己位置とレーザセンサの点群データの時刻情報とに基づいてレーザセンサの点群データを補正し、補正点群データを生成する点群補正部と、点群補正部により生成された補正点群データの出力周期と周期毎の補正点群データの出力データ量とを決定する周期・データ量決定部と、周期・データ量決定部により決定された出力周期及び出力データ量に応じて補正点群データを出力する点群データ出力部とを備える。

【0008】

このようなレーザ検出処理装置においては、レーザセンサの点群データが出力されると共に、移動体の走行状態が検知され、移動体の自己位置が推定される。そして、移動体の自己位置とレーザセンサの点群データの時刻情報とに基づいて点群データが補正され、補正点群データが生成される。そして、補正点群データの出力周期と周期毎の補正点群データの出力データ量とが決定され、その出力周期及び出力データ量に応じて補正点群データが出力される。ここで、補正点群データの出力周期と周期毎の補正点群データの出力データ量とを必要とする機能に応じて決定することにより、補正点群データを適切な周期で適切なデータ量だけ出力することができる。

【0009】

（２）上記の（１）において、点群データ出力部は、補正点群データの一部を前周期と重複して出力してもよい。このような構成では、補正点群データの出力周期を高くしても、周期毎の補正点群データの必要な出力データ量が確保される。従って、高周期多データ量の補正点群データを必要とする機能に対応可能となる。

【0010】

（３）上記の（１）において、点群データ出力部は、補正点群データの一部を出力しなくてもよい。このような構成では、補正点群データの出力周期を低くしても、不要な点群データを出力しなくて済む。従って、低周期少データ量の補正点群データを必要とする機能に対応可能となる。

【0011】

（４）上記の（１）～（３）の何れかにおいて、点群データ出力部は、自己位置推定部により移動体の走行状態が不安定であることが検知されたときは、移動体の走行状態が不安定である時点の補正点群データを出力せずに、移動体の走行状態が不安定でない時間帯の補正点群データを出力してもよい。このような構成では、移動体の走行状態が不安定である時点の補正点群データが出力されなくても、その分移動体の走行状態が不安定でない時間帯の補正点群データが出力されるため、補正点群データの出力データ量の減少が抑制される。

【0012】

（５）上記の（１）～（４）の何れかにおいて、点群データ出力部は、予め決められた基準時刻から現時刻までの経過時間を補正点群データと共に出力してもよい。このような構成では、補正点群データを用いる自己位置推定処理等の後処理を別のコンピュータで行う場合でも、別のコンピュータにおいて現時刻から経過時間前の時刻に変換することで、異なるコンピュータ間で時刻が同期するため、後処理を正確な時間で行うことができる。

【発明の効果】

【0013】

本発明によれば、点群データを適切な周期で適切なデータ量だけ出力することができる。

【図面の簡単な説明】

【0014】

【図1】本発明の第１実施形態に係るレーザ検出処理装置を備えた自己位置推定装置の構成を示すブロック図である。

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

【図3】レーザセンサの点群データが時刻情報により補正される概念を示す図である。

【図4】図１に示された点群データ出力部により実行される点群データ出力処理の手順を示すフローチャートである。

【図5】点群データ出力部から補正点群データが高周期で出力される場合における周期毎の補正点群データの出力データ量を示す図である。

【図6】点群データ出力部から補正点群データが低周期で出力される場合における周期毎の補正点群データの出力データ量を示す図である。

【図7】比較例において、点群データ出力部から補正点群データが高周期で出力される場合における周期毎の補正点群データの出力データ量を示す図である。

【図8】比較例において、点群データ出力部から補正点群データが低周期で出力される場合における周期毎の補正点群データの出力データ量を示す図である。

【図9】本発明の第２実施形態に係るレーザ検出処理装置を備えた自己位置推定装置の構成を示すブロック図である。

【図10】図９に示された点群データ出力部により実行される点群データ出力処理の手順を示すフローチャートである。

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

【0015】

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

【0016】

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

【0017】

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

【0018】

レーザセンサ３は、図２に示されるように、移動体２の周囲にレーザＬを照射し、レーザＬの反射光を受光することにより、移動体２の周囲に存在する物体８を検出し、検出データとしての点群データＤを出力する。レーザセンサ３は、移動体２から物体８までの距離または物体８の形状等を検出する。物体８は、例えば建物、棚、壁や柱等の構造物、あるいは移動体２が回避すべき障害物、移動体２が接近すべき設備、移動体２が運搬すべき荷物やパレット等である。

【0019】

レーザセンサ３としては、例えば２Ｄまたは３ＤのＬｉＤＡＲ（Light Detection and Ranging）やレーザレンジファインダ等が使用される。レーザセンサ３は、３６０度全周を走査して検出可能な回転型レーザセンサである。レーザセンサ３は、検出データとしての点群データＤを出力する。

【0020】

オドメトリセンサ４は、移動体２の速度及び角速度を検出するセンサである。オドメトリセンサ４としては、例えば移動体２の各車輪２ａに取り付けられたエンコーダ、ＩＭＵ（Inertial Measurement Unit）やＧＰＳ等が使用される。

【0021】

入力器５は、例えばユーザが点群データの処理に関する情報等を入力する機器である。点群データの処理に関する情報は、後述する点群データの出力周期及び周期毎の点群データの出力データ量に関する情報を含んでいる。入力器５としては、例えばカーナビゲーションやタッチパネル等が使用される。

【0022】

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

【0023】

コントローラ１０は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ１０は、点群データ取得部１１と、補正用自己位置推定部１２と、点群補正部１３と、周期・データ量決定部１４と、点群データ出力部１５と、メイン自己位置推定部１６とを有している。

【0024】

ここで、レーザセンサ３、オドメトリセンサ４、入力器５、コントローラ１０の点群データ取得部１１、補正用自己位置推定部１２、点群補正部１３、周期・データ量決定部１４及び点群データ出力部１５は、本実施形態のレーザ検出処理装置２０を構成している。レーザ検出処理装置２０は、レーザにより移動体２の周囲に存在する物体８を検出し、検出データである点群データを処理する装置である。

【0025】

点群データ取得部１１は、図２に示されるように、レーザセンサ３の点群データＤをレーザセンサ３の点群データＤの時刻情報と共に取得する。レーザセンサ３により検出された物体８は、点群Ｐで表される。点群Ｐは、レーザセンサ３から照射されるレーザＬの反射点の集まりである。点群データＤの時刻情報は、レーザセンサ３からコントローラ１０に点群データＤが出力されたときのコントローラ１０の時刻の情報である。言い換えると、点群データＤの時刻情報は、コントローラ１０において点群データＤが取得されたときの時刻の情報である。時刻情報としては、現時刻でもよいし、適当なインデックス時刻でもよい。

【0026】

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

【0027】

補正用自己位置推定部１２は、オドメトリセンサ４の検出値に基づいて、単位時間当たりの移動体２の移動量及び移動方向を算出することにより、移動体２の走行状態を検知し、移動体２の走行状態に基づいて移動体２の自己位置を補正用自己位置として推定する。補正用自己位置推定部１２は、オドメトリセンサ４と協働して、移動体２の走行状態を検知し、移動体２の自己位置を推定する自己位置推定部を構成する。

【0028】

点群補正部１３は、補正用自己位置推定部１２により推定された移動体２の自己位置とレーザセンサ３の点群データの時刻情報とに基づいて、点群データ取得部１１により取得されたレーザセンサ３の点群データを補正し、補正点群データを生成する。補正点群データは、レーザセンサ３の時刻情報を含んでいる。

【0029】

例えば図３に示されるように、点群データ取得開始位置Ａから点群データ取得終了位置Ｂまでの間に取得された全点群が、点群データ取得終了位置Ｂの時刻で見える点群に補正される。つまり、点群データ取得終了位置Ｂの時刻が基準時刻である。任意位置Ｃの時刻は、点群データ取得終了位置Ｂと任意位置Ｃとの角度差と、点群データ取得周期（例えば２０Ｈｚ）とから算出される。任意位置Ｃは、単位時間当たりの移動体２の移動量及び移動方向を用いて、点群データ取得終了位置Ｂとの時間差分だけ補正される。これにより、移動体２の移動によるレーザセンサ３の点群データの位置ずれ（歪み）が補正されることとなる。

【0030】

周期・データ量決定部１４は、入力器５の入力情報に応じて、点群補正部１３により取得された補正点群データの出力周期と周期毎の補正点群データの出力データ量とを決定する。周期毎の補正点群データの出力データ量は、１周期に出力される補正点群データのデータ量である。

【0031】

例えば、パレットやトラック等の検知を行う場合には、多くの点群データを用いて高速に自己位置の推定を行う必要があるため、補正点群データの出力周期が高周期に設定されると共に、周期毎の補正点群データの出力データ量が多データ量に設定される。一方、高速に自己位置の推定を行う必要がない場合は、補正点群データの出力周期が低周期に設定されると共に、通信がひっ迫しないように周期毎の補正点群データの出力データ量が少データ量に設定される。

【0032】

点群データ出力部１５は、周期・データ量決定部１４により決定された出力周期及び出力データ量に応じて、補正点群データをメイン自己位置推定部１６に出力する。

【0033】

図４は、点群データ出力部１５により実行される点群データ出力処理の手順を示すフローチャートである。本処理は、例えば移動体２の自動運転が開始されると実行される。

【0034】

図４において、点群データ出力部１５は、まず点群補正部１３により生成された補正点群データを取得する（手順Ｓ１０１）。続いて、点群データ出力部１５は、取得された補正点群データから、周期・データ量決定部１４により決定された出力周期及び出力データ量に応じた点群を抽出する（手順Ｓ１０２）。

【0035】

点群データ出力部１５は、図５に示されるように、補正点群データＤｃを高周期で出力する場合は、一部の点群Ｐが前周期と重複するように点群Ｐを抽出する。つまり、抽出される点群Ｐの一部は、前周期に抽出された点群Ｐと同一である。図５では、半分の点群Ｐが前周期と重複するように点群Ｐが抽出される。

【0036】

点群データ出力部１５は、図６に示されるように、補正点群データＤｃを低周期で出力する場合は、補正点群データＤｃの一部の点群Ｐを抽出しない。図６では、補正点群データＤｃの半分の点群Ｐが抽出されない。

【0037】

続いて、点群データ出力部１５は、補正用自己位置推定部１２により検知された移動体２の走行状態の情報を取得する（手順Ｓ１０３）。そして、点群データ出力部１５は、移動体２の走行状態の情報に基づいて、手順Ｓ１０２で抽出された点群が移動体２の安定走行時の点群であるかどうかを判断する（手順Ｓ１０４）。移動体２の動きが激しい場合は、移動体２の角速度が大きく、移動体２の走行状態が不安定となる。

【0038】

点群データ出力部１５は、抽出された点群が移動体２の安定走行時の点群であると判断したときは、手順Ｓ１０２で抽出された点群データを指定された周期でメイン自己位置推定部１６に出力し（手順Ｓ１０５）、上記の手順Ｓ１０１を再度実行する。

【0039】

点群データ出力部１５は、抽出された点群が移動体２の安定走行時の点群でないと判断したときは、手順Ｓ１０２で抽出された点群よりも過去の点群を抽出する（手順Ｓ１０６）。過去の点群は、移動体２の安定走行時の点群である。なお、過去の点群が取得された時刻は、補正点群データに含まれる時刻情報から分かる。そして、点群データ出力部１５は、手順Ｓ１０６で抽出された点群データを指定された周期でメイン自己位置推定部１６に出力し（手順Ｓ１０５）、上記の手順Ｓ１０１を再度実行する。

【0040】

以上により、点群データ出力部１５は、図５に示されるように、補正点群データＤｃを高周期で出力する場合は、補正点群データＤｃの一部を前周期と重複して出力することとなる。点群データ出力部１５は、図６に示されるように、補正点群データＤｃを低周期で出力する場合は、補正点群データＤｃの一部を出力しないこととなる。

【0041】

また、点群データ出力部１５は、補正用自己位置推定部１２により移動体２の走行状態が不安定であることが検知されたときは、移動体２の走行状態が不安定である時点の補正点群データＤｃを出力せずに、移動体２の走行状態が不安定でない時間帯の補正点群データＤｃを出力することとなる。

【0042】

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

【0043】

図７及び図８は、比較例における補正点群データの出力周期及び出力データ量を示す図である。比較例では、補正点群データが点群データ出力部１５からメイン自己位置推定部１６にシーケンシャルに出力される。

【0044】

図７に示されるように、補正点群データＤｃを高周期で出力する場合は、補正点群データＤｃの出力データ量が減少する。このため、高速で移動体２の自己位置の推定を行う際には、補正点群データの点群Ｐの量が足りず、自己位置の推定精度の低下につながる。

【0045】

一方、図８に示されるように、補正点群データＤｃを低周期で出力する場合は、補正点群データＤｃの出力データ量が増加する。従って、不要な点群Ｐが出力されるため、通信のひっ迫につながる。なお、図７及び図８に示されるように、補正点群データＤｃの出力周期が１／２になると、補正点群データＤｃの出力データ量が２倍となる。

【0046】

これに対し、本実施形態では、図５に示されるように、補正点群データＤｃを高周期で出力する場合は、補正点群データＤｃの一部が前周期と重複して出力されるため、補正点群データＤｃの出力データ量が確保される。図５では、図７に示される比較例と比べて、補正点群データＤｃの出力データ量が２倍となる。

【0047】

図６に示されるように、補正点群データＤｃを低周期で出力する場合は、補正点群データＤｃの一部が出力されないため、補正点群データＤｃの出力データ量の増加が抑えられ、必要な補正点群データＤｃのみが出力される。図６では、図８に示される比較例と比べて、補正点群データＤｃの出力データ量が１／２となる。

【0048】

以上のように本実施形態においては、レーザセンサ３の点群データが出力されると共に、移動体２の走行状態が検知され、移動体２の自己位置が推定される。そして、移動体２の自己位置とレーザセンサ３の点群データの時刻情報とに基づいてレーザセンサ３の点群データが補正され、補正点群データが生成される。そして、補正点群データの出力周期と周期毎の補正点群データの出力データ量とが決定され、その出力周期及び出力データ量に応じて補正点群データが出力される。ここで、補正点群データの出力周期と周期毎の補正点群データの出力データ量とを必要とする機能に応じて決定することにより、補正点群データを適切な周期で適切なデータ量だけ出力することができる。その結果、移動体２の自己位置の推定精度を向上させることができる。

【0049】

また、本実施形態では、補正点群データの一部が前周期と重複して出力される。このため、補正点群データの出力周期を高くしても、周期毎の補正点群データの必要な出力データ量が確保される。従って、高周期多データ量の補正点群データを必要とする機能に対応可能となる。

【0050】

また、本実施形態では、補正点群データの一部が出力されない。このため、補正点群データの出力周期を低くしても、不要な点群データを出力しなくて済む。従って、低周期少データ量の補正点群データを必要とする機能に対応可能となる。

【0051】

また、本実施形態では、移動体２の走行状態が不安定であることが検知されたときは、移動体２の走行状態が不安定である時点の補正点群データが出力されずに、移動体２の走行状態が不安定でない時間帯の補正点群データが出力される。このように移動体２の走行状態が不安定である時点の補正点群データが出力されなくても、その分移動体２の走行状態が不安定でない時間帯の補正点群データが出力されるため、補正点群データの出力データ量の減少が抑制される。

【0052】

図９は、本発明の第２実施形態に係るレーザ検出処理装置を備えた自己位置推定装置の構成を示すブロック図である。図９において、自己位置推定装置１Ａは、上記のレーザセンサ３と、上記のオドメトリセンサ４と、上記の入力器５と、点群処理用コントローラ２１と、上記の地図データメモリ６と、自己位置推定用コントローラ２２とを備えている。レーザセンサ３、オドメトリセンサ４、入力器５及び点群処理用コントローラ２１は、本実施形態のレーザ検出処理装置２０Ａを構成している。

【0053】

点群処理用コントローラ２１は、上記の点群データ取得部１１と、上記の補正用自己位置推定部１２と、上記の点群補正部１３と、上記の周期・データ量決定部１４と、点群データ出力部１５Ａとを有している。

【0054】

点群データ出力部１５Ａは、周期・データ量決定部１４により決定された出力周期及び出力データ量に応じて、補正点群データを自己位置推定用コントローラ２２に出力する。点群データ出力部１５Ａは、予め決められた基準時刻から現時刻までの経過時間を補正点群データと共に自己位置推定用コントローラ２２に出力する。

【0055】

図１０は、点群データ出力部１５Ａにより実行される点群データ出力処理の手順を示すフローチャートであり、図４に対応している。図１０において、点群データ出力部１５Ａは、上記の第１実施形態における点群データ出力部１５と同様に、手順Ｓ１０１～Ｓ１０６を実行する。

【0056】

点群データ出力部１５Ａは、手順Ｓ１０５を実行した後、基準時刻から現時刻までの経過時間を相対時刻として出力する（手順Ｓ１０７）。基準時刻は、例えば点群処理用コントローラ２１における本処理の実行を開始した時点の時刻である。点群データ出力部１５Ａは、手順Ｓ１０７を実行した後、上記の手順Ｓ１０１を再度実行する。

【0057】

図９に戻り、自己位置推定用コントローラ２２は、時刻変換部２５と、上記のメイン自己位置推定部１６とを有している。

【0058】

時刻変換部２５は、点群データ出力部１５Ａから出力された相対時刻を自己位置推定用コントローラ２２での時刻に変換する。具体的には、時刻変換部２５は、自己位置推定用コントローラ２２での現時刻から上記経過時間前の時刻に変換する。このとき、点群処理用コントローラ２１から自己位置推定用コントローラ２２への通信時間分については、例えば経過時間に含められる。これにより、点群処理用コントローラ２１の時刻と自己位置推定用コントローラ２２の時刻とが同期するようになる。

【0059】

メイン自己位置推定部１６は、時刻変換部２５により変換された時刻において、点群データ出力部１５Ａにより出力された補正点群データと地図データメモリ６に記憶された地図データとに基づいて、移動体２の自己位置を推定する。

【0060】

このような本実施形態においては、予め決められた基準時刻から現時刻までの経過時間が補正点群データと共に出力される。このため、補正点群データを用いる自己位置推定処理を点群処理用コントローラ２１とは別の自己位置推定用コントローラ２２で行う場合でも、自己位置推定用コントローラ２２において現時刻から経過時間前の時刻に変換することで、点群処理用コントローラ２１と自己位置推定用コントローラ２２との間で時刻が同期するため、自己位置推定処理を正確な時間で行うことができる。従って、ローカルエリアで時刻を同期させるＰＴＰ（時刻同期プロトコル）の機能がない安価なコントローラを使用することができる。

【0061】

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、オドメトリセンサ４の検出値を用いて移動体２の自己位置が推定され、移動体２の自己位置とレーザセンサ３の点群データの時刻情報とに基づいて、レーザセンサ３の点群データが補正されているが、特にそのような形態には限られない。例えば、カルマンフィルタ（オンラインＳＬＡＭ）またはカルマンスムーザー・graph-based ＳＬＡＭ（オフラインＳＬＡＭ）を用いて移動体２の自己位置を推定してもよい。

【0062】

また、上記実施形態では、入力器５の入力情報に応じて、補正点群データの出力周期と周期毎の補正点群データの出力データ量とが決定されているが、特にそのような形態には限られない。例えば、カメラ等により移動体２の周囲環境を検知し、移動体２の周囲環境に応じて、補正点群データの出力周期と周期毎の補正点群データの出力データ量とを決定してもよい。

【0063】

また、上記実施形態のレーザ検出処理装置は、移動体２の自動運転時に、移動体２の自己位置の推定を行う自己位置推定装置に適用されているが、本発明は、特に自己位置推定装置には限られず、点群データを用いて処理を行う装置であれば、移動体２の自動運転及び手動運転に関わらず適用可能である。例えば、自動フォークリフトによる荷役作業の際、ＬｉＤＡＲ等を用いて、自動フォークリフトの走行を停めることなく荷役対象となるパレットの位置及び姿勢を推定する装置に本発明を適用する場合には、パレットの位置及び姿勢の推定精度を向上させることができる。

【符号の説明】

【0064】

２…移動体、３…レーザセンサ、４…オドメトリセンサ（自己位置推定部）、８…物体、１２…補正用自己位置推定部（自己位置推定部）、１３…点群補正部、１４…周期・データ量決定部、１５，１５Ａ…点群データ出力部、２０，２０Ａ…レーザ検出処理装置、Ｄ…点群データ、Ｄｃ…補正点群データ、Ｌ…レーザ、Ｐ…点群。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】