特許 7032062 点群データ処理装置、移動ロボット、移動ロボットシステム、および点群データ処理方法

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2022-02-28

(45)【発行日】2022-03-08

(54)【発明の名称】点群データ処理装置、移動ロボット、移動ロボットシステム、および点群データ処理方法

(51)【国際特許分類】

   G05D 1/02 20200101AFI20220301BHJP

   B25J 19/02 20060101ALI20220301BHJP

   B25J 5/00 20060101ALI20220301BHJP

【ＦＩ】

G05D1/02 L

B25J19/02

B25J5/00 E

【請求項の数】 8

(21)【出願番号】P 2017110393

(22)【出願日】2017-06-02

(65)【公開番号】P2018206038

(43)【公開日】2018-12-27

【審査請求日】2020-03-27

(73)【特許権者】

【識別番号】000000099

【氏名又は名称】株式会社ＩＨＩ

(73)【特許権者】

【識別番号】500302552

【氏名又は名称】株式会社ＩＨＩエアロスペース

(74)【代理人】

【識別番号】100097515

【弁理士】

【氏名又は名称】堀田 実

(74)【代理人】

【識別番号】100136700

【弁理士】

【氏名又は名称】野村 俊博

(72)【発明者】

【氏名】梅村 篤志

(72)【発明者】

【氏名】矢代 裕之

【審査官】藤崎 詔夫

(56)【参考文献】

【文献】特開２００８－２５０９０６（ＪＰ，Ａ）

【文献】特開２００９－２０５２２６（ＪＰ，Ａ）

【文献】特開２０１３－１０９３２５（ＪＰ，Ａ）

【文献】特開２０１７－０４５４４７（ＪＰ，Ａ）

【文献】特開２０１５－０８７１４９（ＪＰ，Ａ）

【文献】特開２０１２－１８５０１１（ＪＰ，Ａ）

【文献】特開２００９－０９３３０８（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／０２

Ｂ２５Ｊ １９／０２

Ｂ２５Ｊ ５／００

(57)【特許請求の範囲】

【請求項1】

環境計測センサが取得した点群データを処理する点群データ処理装置であって、
前記環境計測センサは、計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わした点群データを取得するものであり、
前記点群データ処理装置は、
前記点群データに対しデータ量削減処理を行うことにより、前記点群データに含まれる前記座標点の数を減らすデータ量削減部を備え、
前記点群データは、近距離点群データと遠距離点群データとを含み、前記設定座標系において、前記近距離点群データは、前記計測位置の側の近距離領域の前記各座標点の位置を示し、前記遠距離点群データは、前記近距離領域よりも前記計測位置から離れている遠距離領域の前記各座標点の位置を示し、
前記データ量削減処理は、前記近距離点群データの座標点密度を第１閾値以下にし、前記遠距離点群データの座標点密度を第２閾値以下にする処理であって、前記第２閾値がゼロより大きく、前記第１閾値が前記第２閾値よりも高い処理であり、
前記点群データ処理装置は、
地図座標系において物体表面の各座標点の位置を表わした地図データを記憶する地図記憶部と、
前記データ量削減処理後の前記近距離点群データを前記地図データに統合することにより、新たな地図データを生成し、当該新たな地図データを前記地図記憶部に記憶させる地図生成部とを備える、点群データ処理装置。

【請求項2】

環境計測センサが取得した点群データを処理する点群データ処理装置であって、
前記環境計測センサは、計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わした点群データを取得するものであり、
前記点群データ処理装置は、
前記点群データに対しデータ量削減処理を行うことにより、前記点群データに含まれる前記座標点の数を減らすデータ量削減部を備え、
前記点群データは、近距離点群データと遠距離点群データとを含み、前記設定座標系において、前記近距離点群データは、前記計測位置の側の近距離領域の前記各座標点の位置を示し、前記遠距離点群データは、前記近距離領域よりも前記計測位置から離れている遠距離領域の前記各座標点の位置を示し、
前記データ量削減処理は、前記近距離点群データの座標点密度を第１閾値以下にし、前記遠距離点群データの座標点密度を第２閾値以下にする処理であって、前記第２閾値がゼロより大きく、前記第１閾値が前記第２閾値よりも高い処理であり、
前記点群データ処理装置は、移動ロボットに設けられ、
前記データ量削減部から前記データ量削減処理後の前記遠距離点群データが入力され、該遠距離点群データに基づいて前記移動ロボットの姿勢と位置を推定する推定部と、
前記データ量削減部から前記データ量削減処理後の前記近距離点群データが入力され、当該近距離点群データに基づいて地図データを生成する地図生成部とを備える、点群データ処理装置。

【請求項3】

環境計測センサが取得した点群データを処理する点群データ処理装置であって、
前記環境計測センサは、計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わした点群データを取得するものであり、
前記点群データ処理装置は、
前記点群データに対しデータ量削減処理を行うことにより、前記点群データに含まれる前記座標点の数を減らすデータ量削減部を備え、
前記点群データは、近距離点群データと遠距離点群データとを含み、前記設定座標系において、前記近距離点群データは、前記計測位置の側の近距離領域の前記各座標点の位置を示し、前記遠距離点群データは、前記近距離領域よりも前記計測位置から離れている遠距離領域の前記各座標点の位置を示し、
前記データ量削減処理は、前記近距離点群データの座標点密度を第１閾値以下にし、前記遠距離点群データの座標点密度を第２閾値以下にする処理であって、前記第２閾値がゼロより大きく、前記第１閾値が前記第２閾値よりも高い処理であり、
前記点群データ処理装置は、移動ロボットに設けられ、
前記データ量削減部から前記遠距離点群データが入力され、該遠距離点群データに基づいて前記移動ロボットの姿勢と位置を推定する推定部と、
前記データ量削減部から少なくとも前記近距離点群データが入力され、前記近距離点群データに基づいて地図データを生成する地図生成部と、を備え、
前記データ量削減部は、前記環境計測センサが繰り返し取得する前記点群データに対し前記データ量削減処理を繰り返し行い、
前記地図生成部は、前記データ量削減部から繰り返し入力される、前記データ量削減処理後の前記近距離点群データに基づいて、前記地図データを繰り返し生成し、
前記地図生成部が生成した最新の前記地図データを記憶する地図記憶部を備え、
前記推定部は、前記地図記憶部に記憶されている前記最新の地図データと前記遠距離点群データとに基づいて、該地図データにおける前記移動ロボットの姿勢と位置を推定する、点群データ処理装置。

【請求項4】

前記データ量削減部は、
前記設定座標系において、前記近距離領域を互いに等しい第１体積のセルに区切り、前記遠距離領域を互いに等しい第２体積のセルに区切り、前記第１体積は、前記第２体積よりも小さく、
前記近距離領域と前記遠距離領域において、各前記セルについて、当該セル内に複数の座標点が存在する場合には、当該複数の座標点を１つの代表座標点に置き換える、請求項１～３のいずれか一項に記載の点群データ処理装置。

【請求項5】

移動ロボットであって、
請求項１～４のいずれか一項に記載の点群データ処理装置と、
前記点群データを取得する前記環境計測センサと、
前記データ量削減処理後の前記近距離点群データと前記遠距離点群データの一方または両方を他の移動ロボットに送信する通信部を備える、移動ロボット。

【請求項6】

複数の移動ロボットを備える移動ロボットシステムであって、
各移動ロボットは、
該移動ロボットの位置である計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わした点群データを取得する環境計測センサと、
他の前記移動ロボットと通信を行う通信部と、を備え、
少なくともいずれかの前記移動ロボットは、
該移動ロボットの前記環境計測センサが取得した前記点群データを処理する請求項１～４のいずれか一項に記載の点群データ処理装置を備え、
該移動ロボットの前記通信部は、前記点群データ処理装置による前記データ量削減処理後の前記近距離点群データと前記遠距離点群データの一方または両方を、他の前記移動ロボットに送信し、
該他の移動ロボットは、送信された前記近距離点群データと前記遠距離点群データの一方または両方に基づいて地図データを生成する地図生成部を備える、移動ロボットシステム。

【請求項7】

環境計測センサが取得した点群データを処理する点群データ処理方法であって、
前記環境計測センサにより、計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わす点群データが取得された場合に、
前記点群データに対しデータ量削減処理を行うことにより、前記点群データに含まれる前記座標点の数を減らし、
前記点群データは、近距離点群データと遠距離点群データとを含み、前記設定座標系において、前記近距離点群データは、前記計測位置の側の近距離領域の前記各座標点の位置を示し、前記遠距離点群データは、前記近距離領域よりも前記計測位置から離れている遠距離領域の前記各座標点の位置を示し、
前記データ量削減処理は、前記近距離点群データの座標点密度を第１閾値以下にし、前記遠距離点群データの座標点密度を第２閾値以下にする処理であって、前記第２閾値がゼロより大きく、前記第１閾値が前記第２閾値よりも高い処理であり、
地図座標系において物体表面の各座標点の位置を表わした地図データが地図記憶部に記憶されており、
前記データ量削減処理後の前記近距離点群データを前記地図データに統合することにより、新たな地図データを生成し、当該新たな地図データを前記地図記憶部に記憶させる、点群データ処理方法。

【請求項8】

環境計測センサが取得した点群データを処理する点群データ処理方法であって、
前記環境計測センサにより、計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わす点群データが取得された場合に、
前記点群データに対しデータ量削減処理を行うことにより、前記点群データに含まれる前記座標点の数を減らし、
前記点群データは、近距離点群データと遠距離点群データとを含み、前記設定座標系において、前記近距離点群データは、前記計測位置の側の近距離領域の前記各座標点の位置を示し、前記遠距離点群データは、前記近距離領域よりも前記計測位置から離れている遠距離領域の前記各座標点の位置を示し、
前記データ量削減処理は、前記近距離点群データの座標点密度を第１閾値以下にし、前記遠距離点群データの座標点密度を第２閾値以下にする処理であって、前記第２閾値がゼロより大きく、前記第１閾値が前記第２閾値よりも高い処理であり、
前記環境計測センサは、移動ロボットに設けられ、
前記データ量削減処理後の前記遠距離点群データに基づいて前記移動ロボットの姿勢と位置を推定し、
前記データ量削減処理後の前記近距離点群データに基づいて地図データを生成する、点群データ処理方法。

【発明の詳細な説明】

【技術分野】

【0001】

本技術は、移動ロボットに設けられた環境計測センサが取得した点群データを処理する点群データ処理装置および点群データ処理方法に関する。また、本発明は、点群データ処理装置を備えた移動ロボット、および複数の移動ロボットを備える移動ロボットシステムに関する。

【背景技術】

【0002】

移動ロボットは、自己の付近の環境を認識するために環境計測センサを備えている。環境計測センサは例えばレーザスキャナである。レーザスキャナは、移動ロボットから各方向へレーザ光を射出することにより、計測範囲（例えば移動ロボットの周囲）の物体表面の各被計測点から反射光を受けて、この被計測点の位置を求める。すなわち、レーザスキャナは、物体表面の各被計測点の位置（座標）を表わした点群データを取得する。

【0003】

このような点群データに基づいて、移動ロボットを自律移動させるための技術として、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）が知られている。ＳＬＡＭでは、移動ロボットの環境計測センサが取得した点群データに基づいて、移動ロボットの姿勢および位置を自ら推定し、同時に、周囲の障害物の箇所を示す地図データを生成する。この技術は、例えば下記の特許文献１に記載されている。

【0004】

複数の移動ロボットを含む移動ロボットシステムでは、複数の点群データを統合する。第１の移動ロボットに搭載した環境計測センサが第１地点からの計測により第１の点群データを取得する。第２移動ロボットに搭載した環境計測センサが第２地点からの計測により第２の点群データを取得する。第１の移動ロボットは、第２の移動ロボットから送信された第２の点群データを受信し、受信した第２の点群データを第１の点群データに統合して地図データを生成する。これにより、広域の地図データを得ることができる。

【0005】

あるいは、１つの移動ロボットが、互いに異なる複数の地点から計測によりそれぞれの点群データを取得した場合に、これら複数の点群データを統合して地図データを生成する。これにより、広域の地図データを得ることができる。

【先行技術文献】

【特許文献】

【0006】

【文献】特開２００９－２０５２２６号公報

【発明の概要】

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

【0007】

ところで、点群データ同士の統合を効率的にあるいは高速で行えるようにするために、点群データの量を削減することが考えられる。例えば、点群データにフィルタ処理を行って、点群データにおける被計測点の密度を、低い値に抑えることが考えられる。しかし、精度のよい地図データを生成するには、高密度な被計測点が適している。すなわち、フィルタ処理により、被計測点の密度が低下してしまうので、精度のよい地図データを生成できなくなってしまう可能性がある。

【0008】

そこで、本技術の目的は、点群データ同士の統合の効率化あるいは高速化のために点群データの量を削減しても、精度のよい地図の生成に適した点群データが得られるようにすることにある。

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

【0009】

上述の目的を達成するため、本技術の一形態に係る点群データ処理装置は、環境計測センサが取得した点群データを処理する。環境計測センサは、計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わした点群データを取得するものである。
点群データ処理装置は、点群データに対しデータ量削減処理を行うことにより、点群データに含まれる座標点の数を減らすデータ量削減部を備える。
点群データは、近距離点群データと遠距離点群データとを含み、設定座標系において、近距離点群データは、計測位置の側の近距離領域の各座標点の位置を示し、遠距離点群データは、近距離領域よりも計測位置から離れている遠距離領域の各座標点の位置を示す。
データ量削減処理は、
（Ａ）近距離点群データの座標点密度を第１閾値以下にし、遠距離点群データの座標点密度を第２閾値以下にする処理であって、第１閾値が第２閾値よりも高い処理であるか、又は
（Ｂ）遠距離点群データの座標点の数を維持しつつ、近距離点群データにおける座標点密度が第１閾値以下になるように近距離点群データの座標点の数を減らす処理である。

【0010】

本技術の一形態に係る移動ロボットは、上述の点群データ処理装置と、点群データを取得する環境計測センサと、データ量削減処理がなされた近距離点群データを他の移動ロボットに送信する通信部を備える。

【0011】

本技術の一形態に係る移動ロボットシステムは、複数の移動ロボットを備える。
各移動ロボットは、
自身の位置である計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わした点群データを取得する環境計測センサと、
他の移動ロボットと通信を行う通信部と、を備える。
少なくともいずれかの移動ロボットは、この移動ロボットの環境計測センサが取得した点群データを処理する上述の点群データ処理装置を備える。
移動ロボットの通信部は、点群データ処理装置によるデータ量削減処理後の近距離点群データを、他の移動ロボットに送信し、この他の移動ロボットは、近距離点群データに基づいて地図データを生成する地図生成部を備える。

【0012】

本技術の一形態に係る点群データ処理方法は、環境計測センサが取得した点群データを処理する。環境計測センサにより、計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置を設定座標系で表わす点群データが取得された場合に、点群データに対しデータ量削減処理を行うことにより、点群データに含まれる座標点の数を減らす。
点群データは、近距離点群データと遠距離点群データとを含み、設定座標系において、近距離点群データは、計測位置の側の近距離領域の各座標点の位置を示し、遠距離点群データは、近距離領域よりも計測位置から離れている遠距離領域の各座標点の位置を示す。
データ量削減処理は、
（Ａ）近距離点群データの座標点密度を第１閾値以下にし、遠距離点群データの座標点密度を第２閾値以下にする処理であって、第１閾値が第２閾値よりも高い処理であるか、又は
（Ｂ）遠距離点群データの座標点の数を維持しつつ、近距離点群データにおける座標点密度が第１閾値以下になるように近距離点群データの座標点の数を減らす処理である。

【発明の効果】

【0013】

上述した本技術によると、取得した点群データは、データ量削減処理により、座標点の数が減らされるので、そのデータ量が削減される。したがって、点群データ同士の統合を効率化あるいは高速化することが可能となる。
また、データ量削減処理では、近距離点群データの座標点密度が第１閾値以下になり、遠距離点群データの座標点密度が第２閾値以下になるように点群データの座標点の数を減らし、第１閾値は第２閾値よりも高い。したがって、データ量削減処理後の近距離点群データの座標点密度を、精度のよい地図の生成に適した値にすることができる。
よって、データ量削減処理により、点群データ同士の統合の効率化あるいは高速化のためにデータ量削減処理を行っても、精度のよい地図の生成に適した点群データが得られる。

【0014】

データ量削減処理により、近距離点群データにおける座標点の密度が第１閾値以下になるように、近距離点群データと遠距離点群データのうち近距離点群データの座標点の数を減らしても、上記の効果が得られる。すなわち、近距離点群データにおける座標点の密度は高い傾向があるので、近距離点群データの座標点の密度を適切な閾値（第１閾値）以下に減らせば、遠距離点群データの座標点の数を減らさなくてもよい。これによっても、点群データ同士の統合の効率化あるいは高速化のためにデータ量削減処理を行いつつ、精度のよい地図の生成に適した点群データが得られる。

【図面の簡単な説明】

【0015】

【図1】本技術の実施形態による移動ロボットシステムを示す。

【図2】本技術の実施形態による移動ロボットの構成を示すブロック図である。

【図3】本技術の実施形態による点群データ処理方法を示すフローチャートである。

【図4】本技術の実施形態による点群データ同士の統合の説明図である。

【図5】本技術の実施形態による点群データ同士の統合の別の説明図である。

【図6】本技術の実施形態による点群データ同士の統合の別の説明図である。

【図7】本技術の実施形態による遠距離点群データに基づく移動ロボットの姿勢推定の説明図である。

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

【0016】

本技術の実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

【0017】

図１は、本技術の実施形態による移動ロボットシステム１を示す。移動ロボットシステム１は、複数の移動ロボット１０を含む。各移動ロボット１０は、車輪を有し、この車輪が地面に接触しながら回転駆動されることにより地上面を走行する車両であってよい。代わりに、各移動ロボット１０は、クローラにより地上を走行する走行装置や、歩行する脚を有するロボットなどの他の移動装置であってもよい。

【0018】

移動ロボット１０には、環境計測センサ３が設けられている。環境計測センサ３は、移動ロボット１０の位置である計測位置から見た計測範囲における物体表面の各被計測点を座標点として、各座標点の位置（座標）を、設定座標系としてのセンサ座標系で表わす点群データを取得する。センサ座標系は、移動ロボット１０に固定された座標系である。本実施形態では、センサ座標系は３次元の座標系であり、センサ座標系の原点は、環境計測センサ３により点群データを取得した時（以下で単に観測時という）における移動ロボット１０の位置となる。図１の例では、計測範囲は、移動ロボット１０の周囲であり、図１において、小さい各白丸は被計測点（座標点）を示す。

【0019】

環境計測センサ３は、計測範囲に対してレーザ光（例えばパルスレーザ光）を走査して、計測範囲内の物体表面の各座標点の３次元位置を計測するレーザスキャナであってよい。レーザスキャナは、例えばＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）またはＬＲＦ（Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）と呼ばれる機器であってよい。この場合、レーザスキャナは、移動ロボット１０における設置箇所から計測範囲へ各射出方向にレーザ光を射出する。これにより、レーザスキャナは、射出方向毎に、レーザ光の反射光に基づいて、レーザ光の反射点となった座標点の位置を検出する。

【0020】

ただし、環境計測センサ３は、計測範囲内に存在する物体における各座標点の位置を計測できるものであればよい。例えば、環境計測センサ３は、計測範囲の画像（例えばステレオ画像）を取得するカメラと、この画像を処理することにより計測範囲内に存在する物体表面における各座標点の３次元位置を取得する画像処理部を有するものであってもよい。

【0021】

環境計測センサ３が取得した点群データには、近距離点群データと遠距離点群データとが含まれている。遠距離点群データは、近距離点群データよりも、上述の計測位置から離れている空間領域のデータである。言い換えると、センサ座標系において、近距離点群データは、計測位置の側の近距離領域の各座標点の位置（座標）を示し、遠距離点群データは、近距離領域よりも計測位置から離れている遠距離領域の各座標点の位置（座標）を示す。例えば、点群データを計測した計測位置からの距離が設定距離以内となる空間領域が近距離領域であり、当該計測位置からの距離が設定距離を超える空間領域が遠距離領域である。ここで、設定距離は、例えば２０ｍ以上５０ｍ以下の値、５０ｍ以上７５ｍ以下の値、７５ｍ以上１００ｍ以下の値、１００ｍ以上１５０ｍ以下の値、または、１５０ｍ以上２００ｍ以下の値であるが、これらに限定されない。

【0022】

点群データにおいて、計測位置（移動ロボット１０）に近いほど座標点の密度が高くなる傾向がある。すなわち、同じ物体を同じ方向から第１および第２の計測位置からそれぞれ環境計測センサ３で計測した場合であって、第１の計測位置が第２の計測位置よりも当該物体に近い場合に、第１の計測位置からの計測で得た点群データにおける当該物体表面の座標点の密度は、第２の計測位置からの計測で得た点群データにおける当該物体表面の座標点の密度よりも高い。したがって、近距離点群データにおける座標点の密度は、通常、遠距離点群データにおける座標点の密度よりも高くなっている。

【0023】

図２は、移動ロボット１０の構成を示すブロック図である。図２において、１つの移動ロボット１０における複数の構成要素を示している。以下において、この移動ロボット１０の構成を説明するが、各移動ロボット１０は同じ構成を有していてよい。移動ロボット１０は、環境計測センサ３と、本技術の実施形態による点群データ処理装置２０と、通信部７とを備える。

【0024】

点群データ処理装置２０は、データ量削減部５と地図記憶部９と推定部１１と地図生成部１３を備える。

【0025】

データ量削減部５は、点群データに対しデータ量削減処理を行うことにより、点群データに含まれる座標点の数を減らす。本実施形態では、データ量削減部５は、近距離点群データの座標点密度が第１閾値以下になり、遠距離点群データの座標点密度が第２閾値以下になるようにデータ量削減処理を行う。第１閾値は第２閾値よりも高い。第１閾値は、第２閾値の２倍以上の値であってよいが、これに限定されず、例えば、第２閾値の１．５倍以上の値、又は第２閾値の３倍以上の値であってもよい。自明ではあるが、点群データの座標点密度を閾値（第１又は第２の閾値）以下にするデータ量削減処理において、この閾値をＮ倍にすると、各座標点間の距離が１／Ｎになるような処理結果が得られることになり、閾値の増大に伴いデータ量の削減効果は低減する。

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

（変更例２）
ステップＳ４において、データ量削減部５は、データ量削減処理後の近距離点群データと遠距離点群データのうち、近距離点群データのみを地図生成部１３に入力してもよい。この場合、ステップＳ６において、地図生成部１３は、この近距離点群データを、地図記憶部９の最新の地図データに統合することにより、新たな地図データを生成する。

【0063】

また、ステップＳ３で、通信部７は、上述では、データ量削減処理後の近距離点群データと遠距離点群データの両方を送信したが、データ量削減処理後の近距離点群データと遠距離点群データの一方のみ（例えば近距離点群データ）を他の移動ロボット１０に送信してもよい。これにより、他の移動ロボット１０では、当該一方を通信部７で受信し、地図生成部１３は、当該一方（例えば近距離点群データ）を、地図記憶部９の最新の地図データに統合することにより、新たな地図データを生成する。

【0064】

（変更例３）
上述のデータ量削減処理は、近距離点群データにおける座標点の密度が第１閾値以下になるように、近距離点群データと遠距離点群データのうち近距離点群データの座標点の数を減らす処理であってもよい。すなわち、データ量削減処理において、遠距離点群データの座標点の数を減らさずに維持するようにしてもよい。

【0065】

（変更例４）
移動ロボットシステム１を構成する複数の移動ロボット１０のうち、いずれかの移動ロボット１０を、地表面に対して移動する機構を有しない固定センサに置き換えてもよい。この固定センサは、設定位置に設置されて点群データを取得して当該点群データを処理できるように構成されている。すなわち、この固定センサは、上述した環境計測センサ３、データ量削減部５、および通信部７を備えるように構成された点群データ処理装置２０であってよい。この点群データ処理装置２０は、地図記憶部９と推定部１１と地図生成部１３を有していなくてもよい。この場合、当該点群データ処理装置２０は、データ量削減部５によりデータ量削減処理が行われた点群データを通信部７から、移動ロボット１０（移動ロボットシステム１を構成する移動ロボット１０）に送信する。これにより、移動ロボット１０において、この点群データに基づいて、上述のステップＳ６により地図データが生成される。

【0066】

（変更例５）
上述の実施形態では、第１閾値は、近距離領域のいずれの部分でも同じ値であり、上述の第２閾値は、遠距離領域のいずれの部分でも同じ値である。ただし、第１閾値は、上述の計測位置から離れるにつれて次第に又は段階的に低くなっていてもよく、第２閾値は、上述の計測位置から離れるにつれて次第に又は段階的に低くなっていてもよい。

【0067】

（変更例６）
移動ロボットシステム１に含まれる複数の移動ロボット１０には、点群データ処理装置２０を有しないが他の点は上述と同じ構成を有する移動ロボット１０が含まれていてもよい。すなわち、本技術による移動ロボットシステム１において、少なくともいずれかの移動ロボット１０は、点群データ処理装置２０を備える。

【符号の説明】

【0068】

１ 移動ロボットシステム、３ 環境計測センサ、５ データ量削減部、７ 通信部、９ 地図記憶部、１０ 移動ロボット、１１ 推定部、１３ 地図生成部、１５ 制御装置、１７ 姿勢センサ、１９ 速度センサ、２０ 点群データ処理装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】