WO2015037222A1

WO2015037222A1 - 三次元物体認識装置、三次元物体認識方法、及び移動体

Info

Publication number: WO2015037222A1
Application number: PCT/JP2014/004618
Authority: WO
Inventors: 和仁田中
Original assignee: トヨタ自動車株式会社
Priority date: 2013-09-11
Filing date: 2014-09-09
Publication date: 2015-03-19
Also published as: US9633438B2; JP2015055974A; DE112014004163B4; US20160063710A1; KR20150135505A; KR101971317B1; DE112014004163T5; JP6032163B2

Abstract

　本発明に係る三次元物体認識装置は、全方位について周辺の物体を測定し、測定結果として物体の位置を特定することができる位置情報を生成する全方位センサと、周辺の物体のうち、一定の測定範囲内の物体を測定し、測定結果として物体の三次元形状を特定することができる三次元形状情報を生成する三次元測定装置と、物体が三次元測定装置の測定範囲内にある場合、三次元測定装置が生成した三次元形状情報に基づいて、物体の三次元形状として認識する形状を更新する制御部を備える。制御部は、物体が三次元測定装置の測定範囲外に出た場合であっても、全方位センサが生成した位置情報に基づいて物体を追跡する。

Description

三次元物体認識装置、三次元物体認識方法、及び移動体

　本発明は、三次元物体認識装置、三次元物体認識方法、及び移動体に関する。

　特許文献１には、カメラの撮影画像中の自車前方の一定範囲内の歩行者を精度よく迅速に認識することを目的とした画像認識装置が搭載された車両が開示されている。この画像認識装置は、レーザレーダの探査により自車前方の所定の歩行者検出可能領域以遠に検出される物標に識別情報ＩＤを付与する。これにより、車両や路側物の電柱等のように、歩行者でなく、レーザ光の吸収が少ない物標に識別情報ＩＤを付与するようにしている。そして、この画像認識装置は、カメラの歩行者検出可能領域の識別情報ＩＤが付いていない物標についてのみパターンマッチングの画像認識処理を施して歩行者を認識するようにしている。これにより、画像認識処理の負荷が少なくなり、精度良く迅速に歩行者検出可能領域内の歩行者を認識できると述べられている。

特開２００９－２３７８９８号公報

　本願出願人は、上述したように周辺の物体を認識する技術において、以下に説明する課題を見出した。以下、その課題について説明する。なお、以下に説明する内容は、本願出願人が新たに検討した内容であって、従来技術を説明するものではない。ここでは、センサによって周辺の障害物（物体）を認識しながら自律移動する移動体について考える。

　まず、移動体が障害物を回避して自律移動する手法として、単純には、周辺の障害物の三次元形状は考慮せずに、障害物の有無及び位置のみを考慮して、障害物を回避して移動経路を計画する手法が考えられる。しかしながら、この手法では、障害物の位置を追跡して認識できていても、移動体がオーバーハングな障害物等に接触してしまう可能性がある。通常、障害物は、その種類によって三次元的特徴が異なり、例えば、椅子の足のように特定部が突出しているものもあるからである。

　よって、移動体が障害物に接触しないように自律移動するためには、障害物の三次元形状を認識しながら、障害物の三次元形状を考慮して、移動体が障害物と接触しないように移動経路を計画して移動する必要がある。

　このような要件を満たす手法として、周辺の障害物の三次元形状を計測するセンサを移動体に搭載し、そのセンサによって障害物の三次元形状を計測する手法が考えられる。しかしながら、三次元形状を計測するセンサの多くは、その計測範囲が限られている。そのため、センサの死角に入ってしまった障害物は計測することができなくなってしまう。

　この問題に対する対策として、障害物がセンサの死角に入った位置を記憶するようにしたとしても、その位置を参考に移動経路を計画することはできるが、その後、障害物が移動したか否かは測定することができない。そのため、その位置の近くを移動経路として計画してしまった場合には、その位置から移動した障害物と接触してしまう可能性がある。それに対して、その位置を大きく迂回するように移動経路を計画するようにしてしまうと、実際には移動していなくなっている障害物を大きく迂回するような移動経路を計画してしまい、移動の効率を大きく損なってしまう可能性がある。

　また、別の対策として、計測範囲が制限されず、全方位について周辺の障害物の三次元形状を計測することができるセンサを搭載することも考えられる。しかしながら、センサの精度・計測範囲とコスト・搭載性はトレードオフの関係にある。すなわち、全方位について周辺の障害物の三次元形状を計測できるようにするためには、そのような機能を有する高価なセンサを搭載する、又は、一定の計測範囲で三次元形状を計測するセンサを全方位を計測可能となるように多数搭載する必要があり、非常に高コストとなってしまうという問題がある。

　本発明は、上述した知見に基づいてなされたものであって、コストを抑えつつ、周辺の物体の三次元形状を広範囲で把握することができる三次元物体認識装置、三次元物体認識方法、及び移動体を提供することを目的とする。

　本発明の第１の態様に係る三次元物体認識装置は、全方位について周辺の物体を測定し、測定結果として前記物体の位置を特定することができる位置情報を生成する全方位センサと、前記周辺の物体のうち、一定の測定範囲内の物体を測定し、測定結果として前記物体の三次元形状を特定することができる三次元形状情報を生成する三次元測定装置と、前記物体が前記三次元測定装置の測定範囲内にある場合、前記三次元測定装置が生成した三次元形状情報に基づいて、前記物体の三次元形状として認識する形状を更新する制御部と、を備え、前記制御部は、前記物体が前記三次元測定装置の測定範囲外に出た場合であっても、前記全方位センサが生成した位置情報に基づいて前記物体を追跡するものである。

　本発明の第２の態様に係る三次元物体認識方法は、三次元測定装置によって、周辺の物体のうち、前記三次元測定装置の一定の測定範囲内における物体について三次元形状を測定し、測定結果に基づいて前記三次元測定装置の測定範囲内にある物体の三次元形状として認識する形状を更新するステップと、全方位センサによって、全方位について周辺の物体の位置を測定し、測定結果に基づいて前記三次元測定装置の測定範囲外に出た物体を追跡するステップと、を備えたものである。

　本発明の第３の態様に係る移動体は、全方位について周辺の物体を測定し、測定結果として前記物体の位置を特定することができる位置情報を生成する全方位センサと、前記周辺の物体のうち、一定の測定範囲内の物体を測定し、測定結果として前記物体の三次元形状を特定することができる三次元形状情報を生成する三次元測定装置と、前記物体が前記三次元測定装置の測定範囲内にある場合、前記三次元測定装置が生成した三次元形状情報に基づいて、前記物体の三次元形状として認識する形状を更新するとともに、前記認識している物体の三次元形状に基づいて、前記物体と接触しないように移動経路を計画し、計画した移動経路を移動するように制御する制御部と、を備え、前記制御部は、前記物体が前記三次元測定装置の測定範囲外に出た場合であっても、前記全方位センサが生成した位置情報に基づいて前記物体を追跡するものである。

　上述した本発明の各態様によれば、コストを抑えつつ、周辺の物体の三次元形状を広範囲で把握することができる三次元物体認識装置、三次元物体認識方法、及び移動体を提供することができる。

実施の形態に係る自律移動体の外部構成図である。実施の形態に係る自律移動体による物体の測定状況の一例を示す図である。実施の形態に係る自律移動体の内部構成図である。実施の形態に係る自律移動体の処理を示すフローチャートである。図４のステップＳ１３の三次元形状情報付与処理を示すフローチャートである。実施の形態に係る自律移動体が扱うデータの流れを示す図である。障害物位置と三次元形状との対応付けの一例を示す図である。三次元距離センサの計測範囲内における障害物の三次元形状を蓄積する一例を示す図である。三次元距離センサの計測範囲外における障害物の三次元形状を生成する一例を示す図である。実施の形態の効果を説明するための図である。実施の形態の効果を説明するための図である。

　以下に図面を参照しながら、本発明の好適な実施の形態について説明する。以下の実施の形態に示す具体的な数値等は、発明の理解を容易とするための例示にすぎず、特に断る場合を除き、それに限定されるものではない。また、以下の記載及び図面では、説明の明確化のため、当業者にとって自明な事項等については、適宜、省略及び簡略化がなされている。

＜発明の実施の形態＞
　まず、本実施の形態に係る自律移動体１について説明する。図１及び図２を参照して、本実施の形態に係る自律移動体１の外部構成について説明する。図１は、本実施の形態に係る自律移動体１の外部構成図である。図２は、実施の形態に係る自律移動体１による物体の測定状況の一例を示す図である。

　自律移動体１は、三次元距離センサ２、全方位センサ３、本体４、車輪５ａ、５ｂを有している。自律移動体１は、その周辺の障害物（物体）を認識し、障害物を回避しつつ自律的に移動する。

　三次元距離センサ２は、図２に示すように、自律移動体１の周辺の障害物のうち、一定の測定範囲内の障害物を測定し、測定結果としてそれらの障害物の測定面（三次元距離センサ２に対向する面）における三次元形状を特定することができる三次元形状情報を生成する。三次元距離センサ２は、自律移動体１の前方の障害物を計測することができるように、本体４の前部に配設されている。

　より具体的には、三次元距離センサ２は、例えば、その計測範囲内における各方位毎に障害物までの距離を測定するセンサである。すなわち、三次元距離センサ２は、三次元形状情報として、その計測範囲内の障害物の測定面における各点までの距離を示す距離情報を生成するセンサである。なお、三次元距離センサは、光学式、超音波式、及びレーザ光線式等の各種方式のうち、任意の方式のものを使用するようにしてよい。

　このように、三次元距離センサ２は、一定の測定範囲内の障害物のみの三次元形状しか計測することができないが、全方位について障害物の三次元形状までをも計測可能とするセンサと比較して安価である。

　ここで、障害物を測定し、障害物の三次元形状を特定することができる三次元形状情報を生成する装置（三次元測定装置）は、三次元距離センサ２に限定されない。例えば、三次元測定装置として、三次元距離センサ２に代えてカメラ（撮像装置）を備えるようにしてもよい。そして、カメラによって、一定の範囲の障害物を撮像し、撮像した範囲の画像を示す画像情報を三次元形状情報として生成するようにしてもよい。すなわち、その内容から計測範囲における障害物の三次元形状を特定することができる情報が得られるのであれば、三次元形状情報の形式及び三次元形状情報を生成する装置は、任意のものを採用することができる。つまり、三次元形状情報は、距離情報のように距離として障害物の三次元形状を示す情報であってもよく、画像情報のように、その内容を解析することで障害物の三次元形状を導出することができる情報であってもよい。

　全方位センサ３は、図２に示すように、全方位について自律移動体１の周辺の障害物を測定し、測定結果としてそれらの障害物の位置を特定することができる障害物位置情報を生成するセンサである。全方位センサ３は、自律移動体１の全方位を計測することができるように、本体４の上部に配設されている。

　全方位センサ３は、例えば、全方位について自律移動体１の周辺の障害物までの距離を計測し、全方位について各障害物までの距離を示す距離情報を障害物位置情報として生成するセンサとしてよい。この場合、全方位センサ３は、全方位について障害物までの距離を測定することができるが、三次元形状までは測定することができない安価な距離センサとする。また、全方位センサ３は、全方位について自律移動体１の周辺の障害物を撮像し、撮像した全方位の画像を示す画像情報を障害物位置情報として生成するセンサであってもよい。すなわち、その内容から全方位における障害物のそれぞれの位置を特定することができる情報が得られるのであれば、障害物位置情報の形式及び全方位センサは、任意のものを採用することができる。つまり、障害物位置情報は、距離情報のように障害物までの距離を示す情報であってもよく、画像情報のように、その内容を解析することで障害物までの距離を導出することができる情報であってもよい。

　このように、全方位センサ３は、安価で周辺の障害物の三次元形状までは計測することはできないが、広範囲における障害物の位置を計測することが可能である。また、このように、全方位センサ３は、障害物の三次元形状までは計測することはできないが、水平方向における測定範囲は、三次元距離センサ２よりも広範囲となる。

　本体４は、その下部に車輪５ａ、５ｂが設けられている。自律移動体１は、車輪５ａ、５ｂを駆動することで移動する。

　上述した構成により、自律移動体１は、三次元距離センサ２及び全方位センサ３の測定結果を利用して、自律移動体１の周辺の障害物を避けて自律的に移動する。また、本実施の形態に係る自律移動体１は、三次元距離センサ２と全方位センサ３を組み合わせることで、三次元形状を計測する三次元距離センサ２の計測範囲が限られていたとしても、周辺の障害物の三次元形状を広範囲で把握することを可能とする。

　続いて、図３を参照して、本実施の形態に係る自律移動体１の内部構成について説明する。図３は、本実施の形態に係る自律移動体１の内部構成図である。

　自律移動体１は、制御部１０及びアクチュエータ１４ａ、１４ｂを有する。

　制御部１０は、自律移動体１を統括的に制御する。制御部１０は、ＣＰＵ（Central Processing Unit）及び記憶部を有しており（図示せず）、そのＣＰＵが記憶部に格納されたプログラムを実行することで、制御部１０としての各種処理を実現する。すなわち、このプログラムは、制御部１０が有するＣＰＵに制御部１０としての各種処理を実行させるためのコードを含んでいる。ここで、記憶部は、上記プログラムや、その他の制御部１０が使用する各種情報が格納される。記憶部は、上記プログラムや各種情報を格納可能な記憶装置の少なくとも１つを含む。記憶装置として、例えば、メモリ及びハードディスク等の少なくとも１つを任意に使用してよい。制御部１０は、三次元障害物位置保存部１１、経路計画部１２、及び移動体駆動制御部１３として機能する。

　三次元障害物位置保存部１１は、全方位センサ３から出力された障害物位置情報に基づいて、自律移動体１の周辺の障害物及びその位置を認識し、認識した障害物のそれぞれに対して識別情報を付与する。ここで、識別情報は、障害物のそれぞれを一意に特定することができる情報である。例えば、識別情報として、一意な整数を割り当てるようにしてもよい。また、三次元障害物位置保存部１１は、障害物位置情報によって特定される障害物のそれぞれの位置に基づいて、認識した障害物のそれぞれの現在位置を追跡する。これにより、障害物が三次元距離センサ２の計測範囲内にあるか否かに関わらず、障害物の現在位置を追跡することが可能となっている。

　また、三次元障害物位置保存部１１は、全方位センサ３から出力された障害物位置情報に基づいて特定した障害物の位置が三次元距離センサ２の計測範囲内である場合、その障害物の位置を示す障害物位置情報と、三次元距離センサ２から出力された三次元形状情報に基づいて特定したその障害物の三次元形状を示す三次元形状情報を対応付けて記憶部に保存する。すなわち、障害物毎に、障害物位置情報と三次元形状情報が対応付けられて記憶部に保存される。この際に、全方位センサ３から出力された障害物位置情報の形式と、記憶部に保存される障害物位置情報の形式は、同一であっても異なっていてもよい。三次元距離センサ２から出力された三次元形状情報の形式と、記憶部に保存される情報の形式についても同様である。つまり、三次元距離センサ２及び全方位センサ３から出力された障害物位置情報及び三次元形状情報から、障害物毎の情報を切り出す際には、切り出した情報がそのまま保存されるようにしてもよく、画像情報から位置や三次元形状を特定する場合のように情報の形式が変換されて保存されるようにしてもよい。

　以降も、三次元障害物位置保存部１１は、障害物の位置が三次元距離センサ２の計測範囲内にある間は、その障害物の三次元形状情報を、三次元距離センサ２から出力された三次元形状情報に基づいて特定した最新の三次元形状を示すように置き換えることで更新していく。

　また、三次元障害物位置保存部１１は、障害物毎に、上記の最新の三次元形状を示す三次元形状情報とは別に、それまでに認識した三次元形状の全てを蓄積した三次元形状を示す三次元形状蓄積情報も記憶部に保存する。三次元障害物位置保存部１１は、障害物の三次元形状を障害物位置を基準として蓄積していく。これにより、障害物が人間のように動作するものであり、障害物の形状が変化している場合に、障害物が動きうる範囲を、その三次元形状として順次補完していくことができる。

　また、三次元障害物位置保存部１１は、障害物の位置が三次元距離センサ２の計測範囲外に出た場合、記憶部に保存していたその障害物の三次元形状蓄積情報が示す蓄積した三次元形状に基づいて、その障害物が動きうる範囲と推定される三次元形状を、所定の算出方法に従って算出する。算出方法については、後に詳述する。そして、三次元障害物位置保存部１１は、算出した三次元形状を示す三次元形状情報を、その障害物の最新の三次元形状を示す三次元形状情報として保存する。このように、障害物が三次元距離センサ２の計測範囲外に出てしまった場合には、その障害物の三次元形状として、その障害物が動きうる範囲と推定される三次元形状まで拡大した形状を認識することで自律移動体１と障害物とが接触しないようにする。

　さらに、三次元障害物位置保存部１１は、再び障害物の位置が三次元距離センサ２の計測範囲内に戻ってきた場合には、三次元距離センサ２から出力された三次元形状情報に基づいたその障害物の最新の三次元形状を示す三次元形状情報の更新を再開する。

　このように、障害物が三次元距離センサ２の計測範囲外に出てしまった場合であっても、全方位センサ３の計測結果に基づいてその障害物を追跡し、その障害物の三次元形状を継続的に把握することを可能としている。

　経路計画部１２は、記憶部に格納された障害物位置情報及び三次元形状情報に基づいて、自律移動体１が障害物と接触しないように自律移動体１が移動する経路を計画する。移動体駆動制御部１３は、経路計画部１２が計画した経路を自律移動体１が移動するようにアクチュエータ１４ａ、１４ｂへの駆動指令値を計算する。そして、移動体駆動制御部１３は、計算した駆動指令値をアクチュエータ１４ａ、１４ｂに出力する。

　アクチュエータ１４ａ、１４ｂは、自律移動体１の車輪５ａ、５ｂを駆動する。アクチュエータ１４ａは、移動体駆動制御部１３からの駆動指令値に応じた回転速度で車輪５ａを回転させる。アクチュエータ１４ｂは、移動体駆動制御部１３からの駆動指令値に応じた回転速度で車輪５ｂを回転させる。

　車輪５ａ及び車輪５ｂは、上述した本体４の下部に取り付けられた車輪に該当する。このように、移動体駆動制御部１３が、アクチュエータ１４ａ、１４ｂを制御して、車輪５ａ、５ｂを回転させることで、自律移動体１が障害物と接触しないように計画された移動経路を移動する。

　続いて、図４～図９を参照して、本実施の形態に係る自律移動体１の処理について説明する。図４は、本実施の形態に係る自律移動体１の処理を示すフローチャートである。図５は、図４のステップＳ１３の三次元形状情報付与処理を示すフローチャートである。図６は、実施の形態に係る自律移動体１が扱うデータの流れを示す図である。図７は、障害物位置と三次元形状との対応付けの一例を示す図である。図８は、三次元距離センサの計測範囲内における障害物の三次元形状を蓄積する一例を示す図である。図９は、三次元距離センサの計測範囲外における障害物の三次元形状を生成する一例を示す図である。なお、図４及び図５におけるＳ１～Ｓ５は、図６におけるＳ１～Ｓ５と対応する。また、図５におけるＳ２、Ｓ３は、図７及び図８と対応し、図５におけるＳ４、Ｓ５は、図９と対応する。

　まず、図４、図６、及び図７を参照して、本実施の形態に係る自律移動体１の処理について説明する。

　まず、自律移動体１が自律移動を開始したものとする（Ｓ１０）。例えば、ユーザによって設定された目的地までの自律移動を開始する。しかしながら、自律移動体１は、目的地が設定されるものに限られず、任意の目的で移動・停止を繰り返すものであってもよい。

　自律移動体１は、三次元距離センサ２及び全方位センサ３によって情報を取得する（Ｓ１１）。具体的には、上述したように、三次元距離センサ２は、自律移動体１の周辺の障害物のうち、一定の測定範囲内の障害物を測定し、三次元形状情報を生成して三次元障害物位置保存部１１に出力する。また、上述したように、全方位センサ３は、全方位について自律移動体１の周辺の障害物を測定し、障害物位置情報を生成して三次元障害物位置保存部１１に出力する。三次元障害物位置保存部１１は、三次元距離センサ２から出力された三次元形状情報と、全方位センサ３から出力された障害物位置情報を取得する。

　自律移動体１は、障害物の識別・追跡を行う（Ｓ１２）。具体的には、三次元障害物位置保存部１１は、全方位センサ３から出力された障害物位置情報に基づいて、周辺の障害物及びその位置を認識し、障害物の識別・追跡を行う。三次元障害物位置保存部１１は、初めて認識する障害物については、識別情報を生成して付与する。そして、三次元障害物位置保存部１１は、生成した識別情報と、認識した障害物の位置を示す障害物位置情報とを対応付けて記憶部に格納する（Ｓ１）。また、三次元障害物位置保存部１１は、既に認識済みの障害物については、その位置の追跡を行う。すなわち、三次元障害物位置保存部１１は、障害物の位置が変化している場合には、記憶部に格納されたその障害物の障害物位置情報が示す位置を、現在位置（全方位センサ３から出力された障害物位置情報に基づいて特定した障害物の位置）に更新する（Ｓ１）。

　ここで、障害物の位置を追跡する際に、全方位センサ３からの障害物位置情報から特定される障害物と、認識済みの障害物とが同一か否かの判定は、どのような手法によって実施してもよい。例えば、記憶部に格納された障害物位置情報のうち、ある障害物の障害物位置情報が示す位置が、全方位センサ３からの障害物位置情報から特定されるある障害物の位置とが最も近い場合、それらの障害物が同一の障害物であると判定するようにしてもよい。

　自律移動体１は、認識した障害物に対して、その障害物の三次元形状情報を付与する（Ｓ１３）。すなわち、初めて認識した障害物に対しては、その三次元形状情報が新規に付与され、認識済みの障害物に対しては、その三次元形状情報を新たに認識した三次元形状を示すように置き換えることで、自律移動体１が障害物の三次元形状として認識する形状を更新する（Ｓ３）。これにより、図７に示すように、識別した障害物について、識別情報と、三次元形状情報と、障害物位置情報とが紐付けられて記憶部に保存される。なお、このステップＳ１３における処理については、図５を参照して詳細に後述する。

　自律移動体１は、自律移動体１が移動する経路を計画し、計画した経路を移動するように車輪５ａ、５ｂを駆動する駆動指令値を計算し、車輪５ａ、５ｂを駆動する（Ｓ１４）。具体的には、経路計画部１２は、三次元障害物位置保存部１１によって記憶部に格納された各障害物の障害物位置情報及び三次元形状情報に基づいて、自律移動体１が障害物に接触しないように移動する経路を計画する。具体的には、例えば、記憶部に予め自律移動体１の三次元形状を示す情報を格納しておき、その情報に基づいて自律移動体１が移動した際に自律移動体１の三次元形状と各障害物の三次元形状と重複しない経路を導出するようにすればよい。

　移動体駆動制御部１３は、経路計画部１２が計画した経路を自律移動体１が移動するようにアクチュエータ１４ａ、１４ｂへの駆動指令値し、アクチュエータ１４ａ、１４ｂを駆動する。これによって、車輪５ａ、５ｂが駆動され、経路計画部１２が計画した経路を自律移動体１が移動する。

　自律移動体１は、自律移動体１が自律移動のゴールに到達したか否かを判定する（Ｓ１５）。例えば、自律移動体１は、上述のユーザによって設定された目的地に到達した場合に、ゴールに到達したと判定する。

　自律移動体１が自律移動のゴールに到達したと判定した場合（Ｓ１５：Ｙｅｓ）、自律移動体１は、自律移動を終了する。自律移動体１が自律移動のゴールに到達していないと判定した場合（Ｓ１５：Ｎｏ）、自律移動体１は、ステップＳ１１に戻る。

　続いて、図５及び図６～図９を参照して、本実施の形態に係る自律移動体１の三次元形状情報付与処理について説明する。図５は、図６のステップＳ１３を詳細に説明するフローチャートとなる。

　三次元障害物位置保存部１１は、識別した障害物が三次元距離センサ２の計測範囲内にあるか否かを判定する（Ｓ２０）。

　識別した障害物が三次元距離センサ２の計測範囲内にあると判定した場合（Ｓ２０：範囲内）、三次元障害物位置保存部１１は、三次元距離センサ２の計測結果から、その障害物の三次元形状を取得し、蓄積する（Ｓ２１）。具体的には、三次元障害物位置保存部１１は、三次元距離センサ２から出力された三次元形状情報から、その障害物を含む一定範囲の三次元形状情報を切り出し、三次元形状蓄積情報に対して蓄積する（Ｓ２）。

　これにより、障害物が三次元距離センサ２の計測範囲内にいる間は、三次元形状情報が一定周期毎に蓄積されていくことになる。具体的には、図８に示すように、障害物の三次元形状として、既に測定した三次元形状が蓄積されている場合には、その障害物の位置を中心として新たに取得した三次元形状を重ね合せることで、その障害物の三次元形状として更新する。すなわち、障害物が動きうる範囲となる三次元形状を、計測によって新たに判明した形状を補完することで更新する。これによれば、例えば、障害物が人間のように動作するものである場合に、その姿勢変化範囲（腕や足の動作する範囲）を記録していくことができる。

　三次元障害物位置保存部１１は、三次元距離センサ２の計測結果から、その障害物の三次元形状を障害物位置と対応付けて保存する（Ｓ２２）。具体的には、三次元障害物位置保存部１１は、切り出した三次元形状情報を、最新の三次元形状を示す三次元形状情報として、識別した障害物の障害物位置情報と対応付けて記憶部に保存する。ここで、上述したように、記憶部に保存する三次元形状情報は、切り出した三次元形状情報の形式そのものに限られず、任意の形式変換が行われていてもよい。これにより、図７に示すように、識別した障害物について、識別情報と、三次元形状情報と、障害物位置情報とが紐付けられて記憶部に保存される（Ｓ３）。

　ここで、識別された障害物が、三次元距離センサ２と全方位センサ３との共通計測範囲内にある間は、上述したように、その障害物の障害物位置情報及び三次元形状情報を更新しながら、その障害物の位置の追跡を行うが、障害物が三次元距離センサ２の計測範囲外に外れた場合には、障害物の位置の追跡を行うことができるが、三次元形状の計測・更新は行うことができなくなる。

　そこで、識別した障害物が三次元距離センサ２の計測範囲内にない（計測範囲外にある）と判定した場合（Ｓ２０：範囲外）、三次元障害物位置保存部１１は、その障害物の三次元形状として認識した形状に基づいて、障害物の姿勢変化範囲（障害物の三次元形状が変化する範囲）を計算する（Ｓ２３）。具体的には、図９に示すように、三次元障害物位置保存部１１は、障害物について蓄積された三次元形状をその障害物の位置から鉛直上方に伸びた軸を中心として回転させた三次元形状を計算する（Ｓ４）。すなわち、三次元形状蓄積情報が示す蓄積した三次元形状を障害物の位置の軸を中心として回転させた三次元形状を計算する。三次元障害物位置保存部１１は、計算した三次元形状を、その三次元形状を示す三次元形状情報に変換して、その計測範囲外に出た障害物の最新の三次元形状を示す三次元形状情報として、その障害物の障害物位置情報と対応付けて記憶部に保存する（Ｓ２４、Ｓ２５、Ｓ５）。この三次元形状情報は、その障害物が再び三次元距離センサ２の計測範囲内に入るまで記憶部に保存し続ける。そして、三次元障害物位置保存部１１は、障害物が再び三次元距離センサ２の計測範囲内に入ったときには、元の三次元形状情報に戻す。または、元の三次元形状情報に強制的には戻さず、上記のステップＳ２２における処理によって三次元形状情報を更新するようにしてもよい。

　ここで、三次元形状の回転軸として、蓄積した三次元形状における任意の位置を定めるようにしてよい。例えば、蓄積した三次元形状の回転軸として、その三次元形状の重心を定めるようにしてもよい。この場合、蓄積した三次元形状の重心と障害物の位置が一致するように三次元形状を蓄積していくことで、障害物の位置と回転軸とを一致させるようにする。また、例えば、蓄積した三次元形状の回転軸として、その三次元形状の所定の特徴的部分の中心位置を定めるようにしてもよい。例えば、障害物が人間である場合、頭の中心位置を回転軸として定めるようにしてもよい。この場合も、蓄積した三次元形状の特徴的部分の中心位置と障害物の位置が一致するように三次元形状を蓄積していくことで、障害物の位置と回転軸とを一致させるようにする。

　蓄積した三次元形状を回転された三次元形状は、障害物が自身の姿勢変化（例えば回転運動）によって動きうる範囲を推定した三次元形状となる。例えば、障害物が人間である場合、三次元距離センサ２によって片方の腕を上げた三次元形状を計測できていたとしても、その人間が三次元距離センサ２の計測範囲外に出てから、他方の腕を上げてしまったとき、又は、１８０度反転してその片方の腕を上げてしまったときには、自律移動体１はその腕の突出を三次元形状として認識していないため、自律移動体１が人間と接触してしまう可能性がある。それに対して、認識している障害物の三次元形状を回転された三次元形状を、障害物の三次元形状として認識しているため、このようなケースであっても、接触を回避することが可能となる。

　このように、自律移動体１が三次元距離センサ２の死角（計測範囲外）の方向に進みだすような経路を計画した場合に、上記のようにして計算・保存した三次元形状情報を用いることで、経路計画において、大まかにではあるが三次元形状を考慮した干渉判定（接触判定）を行うことができる。そのため、信頼性、走破性の高い自律移動が可能となる。

　なお、以上の説明では、障害物が三次元距離センサ２の計測範囲外に出た場合には、姿勢変化範囲を含む三次元形状を推定して、その障害物の三次元形状として認識するようにしているが、三次元形状の推定を行わず、最新の三次元形状もしくは蓄積した三次元形状をそのまま使用するようにしてもよい。そのようにしても、全方位センサ３によって障害物を追跡し、その障害物の三次元形状を考慮した干渉判定を行うことができる。そのため、周辺の障害物の三次元形状を広範囲で把握することができる。しかしながら、好ましくは、上述したように、姿勢変化範囲を含む三次元形状を推定して、その障害物の三次元形状として認識するようにすることで、その障害物が動作して三次元形状が変化するものであったとしても、その障害物との接触を回避することができる。

　以上に説明したように、本実施の形態に係る自律移動体１では、制御部１０は、障害物（物体）が三次元距離センサ２（三次元測定装置）の測定範囲内にある場合、三次元距離センサ２が生成した三次元形状情報に基づいて、障害物の三次元形状として認識する形状を更新するようにしている。また、制御部１０は、障害物が三次元距離センサ２の測定範囲外に出た場合であっても、全方位センサ３が生成した障害物位置情報に基づいて障害物を追跡するようにしている。

　すなわち、三次元距離センサ２に対して、安価で三次元形状まで計測することはできないが、広範囲の障害物の位置を計測することができる全方位センサ３を組み合わせることで、安価とするために計測範囲は狭いが高い精度で三次元形状を計測できる三次元距離センサ２の死角を補うことができるようにしている。したがって、本実施の形態によれば、コストを抑えつつ、周辺の障害物を三次元形状を広範囲で継続的に把握することができる。

　例えば、図１０に示すように、三次元形状を考慮せずに、障害物の有無及び位置のみを考慮して、障害物を回避して移動経路を計画する場合には、オーバーハングな障害物等に接触してしまう可能性や、自律移動体が通過できる場合であっても通過できないと判定して大きく迂回してしまう可能性がある。しかしながら、本実施の形態によれば、図１１に示すように、三次元距離センサ２の死角の障害物の近くを移動する場合であっても、その障害物の三次元形状を追跡把握しているため、自律移動体１が通過できるか否かを高い精度で判定することができる。例えば、自律移動体１がロボットアームを有しており、ロボットアームが突出している場合であっても、障害物の三次元形状を考慮して、ロボットアームと障害物とが接触しないように適切かつ移動効率の良い移動経路を計画することができる。

　また、本実施の形態に係る自律移動体１では、制御部１０は、三次元距離センサ２の測定範囲外に出た障害物については、障害物の三次元形状として得られている形状に基づいて障害物が変化する範囲を含む三次元形状を推定し、推定した三次元形状を三次元距離センサ２の測定範囲外に出た物体の三次元形状として認識するようにしている。

　これによれば、障害物が動作し、その三次元形状が変化するものであっても、その障害物のとり得る三次元形状を、より適切に把握することができる。例えば、上述したように、障害物が三次元距離センサ２の計測範囲外に出た後に、その三次元形状を変化させた場合に対応することができる。よって、障害物を回避しながら移動する自律移動体１において好適に障害物の三次元形状を認識することができる。

　なお、本実施の形態では、蓄積した三次元形状を回転させることで障害物の姿勢変化範囲の三次元形状を推定するようにしているが、これに限られない。例えば、別途の三次元形状を蓄積は行わないようにし、障害物の最新の三次元形状を障害物の位置を中心として回転させることで障害物の姿勢変化範囲の三次元形状を推定するようにしてもよい。しかしながら、好ましくは、蓄積した三次元形状を回転させることで障害物の姿勢変化範囲の三次元形状を推定するようにすることで、障害物の姿勢変化範囲をもれなく含んだ三次元形状を推定することができるため、より障害物との接触を回避することができる。

＜発明の他の実施の形態＞
　上記の実施の形態では、障害物の姿勢変化範囲が未知であると仮定して、障害物の三次元形状を計測結果に基づいて随時更新するとともに、２つのセンサ（三次元距離センサ２、全方位センサ３）の共通計測範囲外に出た障害物については、その三次元形状を回転させることで姿勢変化範囲を推定し、利用するようにしていた。

　しかしながら、識別された障害物が２つのセンサのうち、少なくとも１つのセンサの計測結果を解析することで、カテゴライズし、その障害物のカテゴリーに応じて詳細な姿勢変化範囲の三次元形状を指定するようにしてもよい。

　すなわち、センサの計測結果（最新の三次元形状もしくは蓄積した三次元形状）から特徴的な計測パターンが得られ、障害物があるカテゴリーであると判定できる場合には、予め用意したそのカテゴリーの障害物の姿勢変化範囲の三次元形状を、その障害物の姿勢変化範囲の三次元形状として利用するようにしてもよい。

　例えば、センサの計測結果から障害物が人間であるとカテゴライズされた場合には、予め記憶部に格納しておいた人間の姿勢変化範囲の三次元形状を示す三次元形状情報を、その障害物の最新の三次元形状を示す三次元形状情報として記憶部に保存するようにしてもよい。具体的には、各種カテゴリーについて、そのカテゴリーの障害物が有する計測パターンを示す計測パターン情報と、そのカテゴリーの障害物の三次元形状情報を予め記憶部に格納しておく。三次元障害物位置保存部１１は、三次元距離センサ２及び全方位センサ３の計測結果（障害物位置、最新の三次元形状、及び蓄積した三次元形状）の少なくとも１つにおいて、記憶部に格納された計測パターン情報が示す計測パターンと一致又は類似する計測パターンが検出された場合、その計測パターン情報に対応する三次元形状情報を、その障害物の三次元形状を示す三次元形状情報として特定する。

　このようにすることで、障害物の三次元形状を、より適切に認識することができる。また、それにより、経路計画において、より正確な三次元接触判定を行うことができ、より信頼性、走破性の高い自律移動が可能となる。

　なお、本発明は上記の実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

　上記実施の形態では、最新の三次元形状として、三次元距離センサ２によって最新に取得している三次元形状を認識するようにしているが、これに限られない。例えば、蓄積した三次元形状を、最新の三次元形状として認識するようにしてもよい。しかしながら、好ましくは、上記実施の形態のように、最新の三次元形状として、三次元距離センサ２によって最新に取得している三次元形状を認識するようにするとよい。そのようにすることで、三次元距離センサ２の測定範囲内に障害物がある場合には、その障害物について実際に見えている三次元形状のみを考慮して経路設計を行うことができる。そのため、自律移動体１が余分な遠回りをすることなく、移動効率の良い移動経路を計画することができる。

　また、上記の三次元距離センサ２、全方位センサ３、及び三次元障害物位置保存部１１（制御部１０）を含んで三次元形状認識装置を構成することも可能である。そして、その三次元形状認識装置は、自律移動体１に限られず、その三次元形状認識装置によって認識した周辺の障害物（物体）の三次元形状を用いて任意の制御を行う各種装置に適用することができる。

　この出願は、２０１３年９月１１日に出願された日本出願特願２０１３－１８８２６７を基礎とする優先権を主張し、その開示の全てをここに取り込む。

１　自律移動体
２　三次元距離センサ
３　全方位センサ
４　本体
５ａ、５ｂ　車輪
１０　制御部
１１　三次元障害物位置保存部
１２　経路計画部
１３　移動体駆動制御部
１４ａ、１４ｂ　アクチュエータ

Claims

　全方位について周辺の物体を測定し、測定結果として前記物体の位置を特定することができる位置情報を生成する全方位センサと、
　前記周辺の物体のうち、一定の測定範囲内の物体を測定し、測定結果として前記物体の三次元形状を特定することができる三次元形状情報を生成する三次元測定装置と、
　前記物体が前記三次元測定装置の測定範囲内にある場合、前記三次元測定装置が生成した三次元形状情報に基づいて、前記物体の三次元形状として認識する形状を更新する制御部と、を備え、
　前記制御部は、前記物体が前記三次元測定装置の測定範囲外に出た場合であっても、前記全方位センサが生成した位置情報に基づいて前記物体を追跡する、
　三次元物体認識装置。
　前記制御部は、前記三次元測定装置の測定範囲外に出た物体については、前記物体の三次元形状として認識した形状に基づいて前記物体が変化する範囲を含む三次元形状を推定し、推定した三次元形状を前記三次元測定装置の測定範囲外に出た物体の三次元形状として認識する、
　請求項１に記載の三次元物体認識装置。
　前記制御部は、前記物体が変化する範囲を含む形状として、前記物体の形状として認識した三次元形状を回転させた形状を算出する、
　請求項２に記載の三次元物体認識装置。
　前記物体の形状として認識した三次元形状は、前記三次元測定装置が生成した三次元形状情報に基づいて特定した前記物体の三次元形状を蓄積した形状である、
　請求項２又は３に記載の三次元物体認識装置。
　前記三次元物体認識装置は、さらに、前記物体の三次元形状として認識する形状を示す三次元形状情報と、前記物体の位置を示す位置情報とが対応付けて格納される記憶部を備え、
　前記制御部は、前記物体の三次元形状として認識する形状の更新として、前記三次元測定装置が生成した三次元形状情報に基づいて前記記憶部に格納された三次元形状情報が示す三次元形状を更新するとともに、前記物体の追跡として、前記全方位センサが生成した位置情報に基づいて前記記憶部に格納された位置情報が示す前記物体の位置を更新する、
　請求項１乃至４のいずれか１項に記載の三次元物体認識装置。
　三次元測定装置によって、周辺の物体のうち、前記三次元測定装置の一定の測定範囲内における物体について三次元形状を測定し、測定結果に基づいて前記三次元測定装置の測定範囲内にある物体の三次元形状として認識する形状を更新するステップと、
　全方位センサによって、全方位について周辺の物体の位置を測定し、測定結果に基づいて前記三次元測定装置の測定範囲外に出た物体を追跡するステップと、
　を備えた三次元物体認識方法。
　全方位について周辺の物体を測定し、測定結果として前記物体の位置を特定することができる位置情報を生成する全方位センサと、
　前記周辺の物体のうち、一定の測定範囲内の物体を測定し、測定結果として前記物体の三次元形状を特定することができる三次元形状情報を生成する三次元測定装置と、
　前記物体が前記三次元測定装置の測定範囲内にある場合、前記三次元測定装置が生成した三次元形状情報に基づいて、前記物体の三次元形状として認識する形状を更新するとともに、前記認識している物体の三次元形状に基づいて、前記物体と接触しないように移動経路を計画し、計画した移動経路を移動するように制御する制御部と、を備え、
　前記制御部は、前記物体が前記三次元測定装置の測定範囲外に出た場合であっても、前記全方位センサが生成した位置情報に基づいて前記物体を追跡する、
　移動体。