３次元環境復元装置、その処理方法、及びプログラム
【課題】マップ情報の精度を維持しつつ、マップ情報のメモリ消費量を効果的に低減すること。
【解決手段】３次元環境復元装置は、所定空間内の対象物体の３次元計測を行う計測手段と、計測手段により計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて対象物体が存在する格子領域に直方体が配置され、配置された直方体内に３次元点群データを含むマップ情報を生成するマップ情報生成手段と、マップ情報生成手段により生成されたマップ情報の直方体内の３次元点群データに基づいて、３次元点群データを線分近似した芯線情報を生成する芯線情報生成手段と、を備えている。
【解決手段】３次元環境復元装置は、所定空間内の対象物体の３次元計測を行う計測手段と、計測手段により計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて対象物体が存在する格子領域に直方体が配置され、配置された直方体内に３次元点群データを含むマップ情報を生成するマップ情報生成手段と、マップ情報生成手段により生成されたマップ情報の直方体内の３次元点群データに基づいて、３次元点群データを線分近似した芯線情報を生成する芯線情報生成手段と、を備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて対象物体が存在する格子領域に直方体が配置され、配置された直方体に３次元点群データを含むマップ情報を生成する３次元環境復元装置、その処理方法、及びプログラムに関するものである。
【背景技術】
【０００２】
例えば、ロボットなどが周囲環境に接触しないようにアームを移動させるためには、周囲の３次元環境をマップ化する必要がある。３次元環境を効率的にマップ化する手法として、例えば、多段マップ法やボクセルマップ法を用いた手法が提案されている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特願２０１０−２５２４８号
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記手法において、マップ情報の空間を所定幅毎で区切り、データの離散化を行っている。一般に、各区切りを小さくするとマップ情報の精度が向上するが、マップ情報のメモリ消費量が増加することとなる。一方で、各区切りを大きくするとメモリ消費量の低減に繋がるが、マップ情報の精度が低下することとなる。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、マップ情報の精度を維持しつつ、マップ情報のメモリ消費量を効果的に低減することができる３次元環境復元装置、その処理方法、及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、所定空間内の対象物体の３次元計測を行う計測手段と、前記計測手段により計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成するマップ情報生成手段と、前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する芯線情報生成手段と、を備える、ことを特徴とする３次元環境復元装置である。この一態様によれば、マップ情報の精度を維持しつつ、マップ情報のメモリ消費量を効果的に低減することができる。
【０００７】
この一態様において、前記芯線情報生成手段は、前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データの分布情報を算出し、前記算出した分布情報に基づいて、前記３次元点群データを楕円近似し、前記近似した楕円の長軸を算出することで、前記芯線情報を算出してもよい。これにより、マップ情報の直方体内の３次元点群を線分近似した芯線情報を算出することができる。
【０００８】
この一態様において、前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の対象物体形状を復元する復元手段を更に備えていてもよい。これにより、芯線情報を用いて、所定空間内の対象物体形状を高精度に復元することができる。
【０００９】
この一態様において、前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の平面を検出してもよい。これにより、芯線情報を用いて、所定空間内の平面を高精度かつ高速に検出することができる。
【００１０】
この一態様において、前記復元手段は、前記マップ情報の中から、複数の前記芯線情報を選択して平面を構成し、前記構成した平面の平面方程式に前記マップ情報の他の芯線情報をあてはめ、所定条件を満たすか否かを判断することで、前記平面を構成する前記芯線情報のグループを推定してもよい。これにより、芯線情報を用いて、所定空間内の平面を高精度かつ高速に検出することができる。
【００１１】
この一態様において、前記マップ情報生成手段は、前記計測手段により計測された３次元計測データに基づいて、前記配置された直方体に３次元点群データを含む時分割され多段状で構成された時分割多段マップ情報を生成し、生成した前記時分割多段マップ情報を全て加算した全体の時分割多段マップ情報を生成しており、前記マップ情報生成手段により生成された前記全体の時分割多段マップ情報に、最新の前記時分割多段マップ情報を加算し、最古の前記時分割多段マップ情報を減算することで、前記全体の時分割多段マップ情報及び芯線情報の更新を行う更新手段を更に備えていてもよい。これにより、時分割多段マップ情報を更新すると共に、その芯線情報も更新することができる。
【００１２】
この一態様において、前記更新手段は、前記加算において、前記最新の時分割多段マップ情報の直方体を用いて、過去の前記時分割多段マップの直方体を分割してもよい。これにより、加算処理における整合性を図ることができる。
【００１３】
この一態様において、前記更新手段は、前記減算において、前記全体の時分割多段マップ情報の直方体から前記最古の時分割多段マップ情報を減算することで分割を行うと共に、前記分割されたマップ情報の直方体の分布情報を夫々算出してもよい。これにより、減算処理における整合性を図ることができる。
【００１４】
この一態様において、前記復元手段は、前記芯線情報生成手段により生成された所定領域近傍の前記芯線情報を選択し、前記選択した芯線情報の統計的な特徴量を算出し、前記算出した特徴量と、所定モデルの特徴量と、を比較することで、前記対象物体の形状を検出してもよい。これにより、芯線情報を用いて、所定空間の対象物体の形状を高精度かつ高速に検出することができる。
【００１５】
この一態様において、前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報群の中から、所定モデルの少なくとも一部の曲面を示す所定数式にあてはまる、前記芯線情報を選択し、前記選択した芯線情報を含むマップ情報と、前記所定モデルとを比較し、評価することで、前記対象物体の形状を検出してもよい。これにより、芯線情報を用いて、所定空間の対象物体の形状を高精度かつ高速に検出することができる。
【００１６】
この一態様において、前記マップ情報は、例えば、多段状に構成された多段マップ情報、又はボクセルマップ情報であってもよい。
【００１７】
この一態様において、前記復元手段により復元された前記所定空間の対象物体形状と前記マップ情報とに基づいて、移動体の経路を設定する経路設定手段を更に備えていてもよい。これにより、例えば、所定空間の対象物体の形状を考慮して、移動体の最適な経路を設定することができる。
【００１８】
他方、上記目的を達成するための本発明の一態様は、所定空間内の対象物体の３次元計測を行うステップと、前記計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体に３次元点群データを含むマップ情報を生成するステップと、前記生成されたマップ情報の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成するステップと、を含む、ことを特徴とする３次元環境復元装置の処理方法であってもよい。
【００１９】
また、上記目的を達成するための本発明の一態様は、計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体に３次元点群データを含むマップ情報を生成する処理と、前記生成されたマップ情報の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する処理と、をコンピュータに実行させることを特徴とするプログラムであってもよい。
【発明の効果】
【００２０】
本発明によれば、マップ情報の精度を維持しつつ、マップ情報のメモリ消費量を効果的に低減することができる３次元環境復元装置、その処理方法、及びプログラムを提供することができる。
【図面の簡単な説明】
【００２１】
【図１】本発明の実施の形態１に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。
【図２】マップ情報生成部により生成されるボクセルマップ情報の一例を示す図である。
【図３】（ａ）マップ情報の構成要素における３次元点群データの一例を示す図である。（ｂ）マップ情報の構成要素における芯線情報の一例を示す図である。
【図４】選択した芯線で平面領域が構成可能か否かの判断を説明するための図である。
【図５】本発明の実施の形態１に係る３次元環境復元装置による復元方法の一例を示すフローチャートである。
【図６】本発明の実施の形態２に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。
【図７】本発明の実施の形態２に係る３次元環境復元装置による処理フローの一例を示す概念図である。
【図８】分布情報の加減算を示す図である。
【図９】（ａ）更新部による分割処理を説明するための図である。（ｂ）更新部による分割処理を説明するための図である。
【図１０】（ａ）更新部による分割処理を説明するための図である。（ｂ）更新部による分割処理を説明するための図である。
【発明を実施するための形態】
【００２２】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施の形態１に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。本実施の形態１に係る３次元環境復元装置１０は、所定空間内に存在する任意の対象物体の形状を検出、復元することができるものである。
【００２３】
３次元環境復元装置１０は、所定空間内の対象物体の３次元計測を行う環境認識センサ１と、環境認識センサ１により計測された対象物体の３次元計測データに基づいて、所定空間内の対象物体形状（環境形状）を正確に検出、復元する演算装置２と、を備えている。
【００２４】
環境認識センサ１は、計測手段の一具体例であり、例えば、カメラ、超音波センサ、レーザレンジファインダなどから構成されており、所定空間内の対象物体の３次元計測を行なうことができる。例えば、レーザレンジファインダは、前方に向けてレーザを放射し、そのレーザを左右方向へと走査することで対象物体までの距離データを取得して、３次元計測を行う。環境認識センサ１は、演算装置２に接続されており、計測した３次元計測データを演算装置２に対して出力する。
【００２５】
演算装置２は、環境認識センサ１により計測された３次元計測データに基づいて、所定空間のマップ情報を生成する。なお、演算装置２は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバス等によって相互に接続されている。演算装置２は、環境認識センサ１から出力された３次元計測データを、例えば、ＲＡＭ等を用いて逐次記憶する。
【００２６】
演算装置２は、マップ情報を生成するマップ情報生成部３と、芯線情報を生成する芯線情報生成部４と、所定空間内の対象物体を復元する復元部５と、を有している。
【００２７】
マップ情報生成部３は、マップ情報生成手段の一具体例であり、環境認識センサ１により計測された３次元計測データに基づいて、対象物体の３次元点群データを含む多段マップ情報やボクセルマップ情報などの、マップ情報を生成する。
【００２８】
より具体的には、マップ情報生成部３は、環境認識センサ１により計測された３次元計測データに基づいて対象物体の３次元点群データを生成し、３次元環境の基準平面が格子状に区切られて対象物体が存在する格子領域に直方体（キューボイド（cuboid））が配置され、その配置された直方体内に３次元点群データを含むマップ情報（例えば、図２に示す３次元座標系のボクセルマップ情報）を生成する。
【００２９】
図２に示すように、例えば、ボクセルマップ情報において、環境の基準平面を格子状に区切り、対象物体が存在する領域に対して直方体が割り当てられる。直方体の上面の位置を示す値及び高さを示す値は、任意の実数値を取ることができ、各直方体は、上面の位置Ｔｏｐ及び高さＤｅｐｔｈのデータを保持している。また、マップ情報では、環境内に複数の対象物体が存在する場合において、その配置状況に応じて高さ方向（ｚ方向）に複数の直方体を多段状に設定することができる。
【００３０】
マップ情報生成部３は、芯線情報生成部４に接続されており、生成したマップ情報を芯線情報生成部４に対して出力する。
【００３１】
芯線情報生成部４は、芯線情報生成手段の一具体例であり、マップ情報生成部３により生成されたマップ情報の構成要素（直方体）内の３次元点群データ（図３（ａ））に基づいて、その構成要素に対する、３次元点群を局所的に線分近似した芯線情報を生成する。
【００３２】
芯線情報生成部４は、例えば、生成されたマップ情報の３次元点群データの平均値Ｐｎ及び分散値Σｎなどの分布情報を下記（１）式を用いて算出することで、平均値Ｐｎ及び分散値Σｎの逐次更新することができる。なお、下記（１）式においてＸｎを計測点とする。
平均値Ｐｎ＝ｍｎ／ｎ
分散値Σｎ＝（Ｓｎ＋Ｐｎｍｎ）／ｎ （１）式
ｍｎ＝（ｎ−１）Ｐｎ−１＋Ｘｎ
Ｓｎ＝Ｓｎ−１＋ＸｎＸｎ
【００３３】
そして、芯線情報生成部４は、算出した３次元点群データの平均値Ｐｎ及び分散値Σｎを用いて、３次元点群データを楕円体近似した際の長軸（芯線）を算出し、その中心点及び方向ベクトルを算出することで、その３次元点群データの芯線情報を生成する（図３（ｂ））。芯線情報生成部４は、マップ情報の構成要素毎に、算出した芯線情報を対応させて記憶する。
【００３４】
このように、マップ情報内の対象物体を、芯線情報を用いて表し保持することで、マップ情報の解像度を疑似的に向上させることができる。すなわち、マップ情報の精度を維持しつつ、そのメモリ消費量を効果的に低減することができる。芯線情報生成部４は、復元部５に接続されており、算出したマップ情報の構成要素毎の芯線情報を復元部５に対して出力する。
【００３５】
復元部５は、復元手段の一具体例であり、芯線情報生成部４により生成された芯線情報に基づいて、所定空間内の対象物体形状を復元する。
【００３６】
復元部５は、マップ情報生成部３により生成されたマップ情報の中から、任意に、ｎ個の芯線情報を選択して組を生成し、これら選択した芯線情報で平面領域を構成できるか否かを判断する。
【００３７】
復元部５は、例えば、図４に示すように、３つの芯線情報を選択し（ｎ＝３）、各芯線の始点ｐ１〜ｐ３（直方体内の点群の平均）間を結ぶベクトルａを算出する。復元部５は、算出したベクトルａと、芯線の方向ベクトルｎ１、ｎ２（芯線の固有ベクトル）とで外積を取り、垂直ベクトルｂ１（ｂ１＝ｎ１×ａ）、ｂ２（ｂ２＝ｎ２×ａ）を夫々算出する。復元部５は、選択した芯線に対する垂直ベクトルｂ１、ｂ２の角度が予め設定された所定角度以内となっていれば、その選択した３つの芯線で平面領域が構成可能と判断する。
【００３８】
復元部５は、選択した芯線情報で平面領域が構成可能と判断した場合、その平面領域の平面方程式（ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０）を算出する。
【００３９】
復元部５は、マップ情報の他の芯線情報をその算出した平面方程式にあてはめ、下記所定条件（２）式を満たすか否かを判断することで平面探索を行い、その平面領域を構成する芯線情報のグループ（平面構成要素）を推定し、平面領域を検出する。なお、下記（２）式において、ｂ＝（ａ、ｂ、ｃ）、及びｂｉ＝（ｎｘ、ｎｙ、ｎｚ）（入力芯線の方向ベクトル）とする。また、上記所定角度、下記第１及び第２所定閾値は、例えば、ＲＯＭやＲＡＭなどに予め設定されている。
距離閾値||ａｘｉ＋ｂｙｉ＋ｃｚｉ＋ｄ||＜第１所定閾値
角度閾値|ｃｏｓ−１（ｂ・ｂｉ/|ｂ||ｂｉ|）−π/２|≦第２所定閾値 （２）式
【００４０】
なお、復元部５は、所定領域近傍の３次元点群の芯線のペアを構成し、その構成した芯線のペアに基づいて、同属領域を拡張する所謂region growingアプローチ手法を用いて、上記平面領域の検出を行っても良く、芯線情報を用いた任意の検出方法を適用することができる。
【００４１】
以上のようにして、復元部５は、多段マップ法においては水平面、ボクセルマップ法においては水平面及び垂直面、などの特定の傾きの平面領域に制限されることなく、それ以外の任意の傾きを有する平面領域に対しても、離散化の影響を受けることなく、高精度に検出し、復元することができる。さらに、マップ情報の３次元点群を局所的に線分近似した芯線情報を利用し、情報処理量を低下させることで、平面領域の検出を高精度かつ高速に行うことができる。
【００４２】
次に、本実施の形態１に係る３次元環境復元装置１０の処理方法について、詳細に説明する。図５は、本実施の形態１に係る３次元環境復元装置による復元方法の一例を示すフローチャートである。
【００４３】
環境認識センサ１は、所定空間内の対象物体の３次元計測を行ない（ステップＳ１０１）、計測した３次元計測データを演算装置２のマップ情報生成部３に対して出力する。
【００４４】
マップ情報生成部３は、環境認識センサ１からの３次元計測データに基づいて、対象物体の３次元点群データを生成し、対象物体が存在する格子領域に直方体が配置され、その配置された直方体に３次元点群データを含むマップ情報を生成し（ステップＳ１０２）、生成したマップ情報を芯線情報生成部４に対して出力する。
【００４５】
芯線情報生成部４は、マップ情報生成部３からのマップ情報の直方体内の３次元点群データに基づいて、マップ情報の構成要素（直方体）毎に、３次元点群データの芯線情報を生成し（ステップＳ１０３）、生成した芯線情報を復元部５に対して出力する。
【００４６】
復元部５は、芯線情報生成部４により生成された芯線情報に基づいて、マップ情報の平面領域を検出する（ステップＳ１０４）。
【００４７】
以上、本実施の形態１に係る３次元環境復元装置１０によれば、マップ情報の構成要素毎に、３次元点群を局所的に線分近似した芯線情報を対応させて記憶することで、マップ情報の精度を維持しつつ、そのメモリ消費量を効果的に低減することができる。さらに、芯線情報を利用することで、所定空間内の平面領域の検出を高精度かつ高速に行うことができる。
【００４８】
実施の形態２．
図６は、本発明の実施の形態２に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。本実施の形態２に係る３次元環境復元装置２０は、時系列拡張が行われ、時分割されかつ多段で構成された時分割多段マップ情報を用いて平面領域の検出を行う。なお、時分割多段マップの詳細な説明は、特願２０１０−２５２４８号に詳細に記載されており、これを援用できるものとする。
【００４９】
３次元環境復元装置２０は、環境認識センサ１と、マップ情報生成部３、芯線情報生成部４、復元部５、及び更新部６、を有する演算装置２１と、を備えている。
【００５０】
演算装置２１のマップ情報生成部３は、環境認識センサ１により計測された３次元計測データに基づいて、対象物体が存在する格子領域に直方体が配置され、その配置された直方体に３次元点群データを含む複数の時分割多段マップ情報を生成する。ここで、時分割多段マップ情報の各構成要素の基となった３次元点群データは、構成要素毎に分布情報として保持される。
【００５１】
マップ情報生成部３は、生成した時分割多段マップ情報群を全て加算することで、全体の時分割多段マップ情報（以下、全時分割多段マップ情報）を生成する（図７（１））。なお、この処理は、初回のみ実行される。マップ情報生成部３は、生成した全時分割多段マップ情報を更新部６に対して出力する。
【００５２】
更新部６は、更新手段の一具体例であり、マップ情報生成部３により生成された全時分割多段マップ情報に、最新の時分割多段マップ情報を加算し（図７の（２））、続いて、最古の時分割多段マップ情報を減算する（図７（３））ことで、全時分割多段マップ情報の更新を行う。すなわち、更新部６は、全時分割多段マップ情報の更新毎に、ＦＩＦＯ（First-In-First-Out）のように、全時分割多段マップ情報に、最新の時分割多段マップ情報を加算し、続いて、最古の時分割多段マップ情報を減算することを繰り返す。
【００５３】
このとき、更新部６は、時分割多段マップ情報の３次元点群データの分布情報についても、上記同様に減算及び加算を行い、分布情報の整合性を図る（図８）。例えば、構成要素Ｃ１の内部分布Ｎ１（Ｐ１、Σ１）と、構成要素Ｃ２の内部分布Ｎ２（Ｐ２、Σ２）とを、下記（３）式を用いて加算及び減算することができる。
加算： Ｐ＝（ｍ１＋ｍ２）/（ｎ１＋ｎ２）
Σ＝（Ｓ１＋Ｓ２）/（ｎ１＋ｎ２）−ｍ１ｍ２
減算： Ｐ＝（ｍ１−ｍ２）/（ｎ１−ｎ２） （３）式
Σ＝（Ｓ１−Ｓ２）/（ｎ１−ｎ２）−ｍ１ｍ２
【００５４】
なお、上記加算及び減算は、上述のようにして行うことが可能であるが、その際、１つの直方体が２つに分かれる「分割」について、状況に応じた適切な対応が必要となる。以下、その分割方法について説明する。
【００５５】
更新部６は、全時分割多段マップ情報に対して最新の時分割多段マップ情報を加算する場合（図７の（２））に、最新の時分割多段マップ情報を参照して、過去の全ての時分割多段マップ情報を下記のように変形する。
【００５６】
例えば、更新部６は、最新の時分割多段マップ情報Partmap４の直方体（cuboid）を用いて、時分割多段マップ情報Partmap２、３の直方体を分割する（図９（ａ））。その直方体表現において、図９（ａ）に示すように加算処理により、直方体に分離が生じているが、内部の芯線（点分布）は繋がった状態となる（図９（ｂ））。なお、直方体を所定の比率で分割することなく、便宜的に２分割するだけでもよい。
【００５７】
また、更新部６は、全時分割多段マップ情報の直方体から最古の時分割多段マップ情報の直方体を減算する場合（図７の（３））において、例えば、全時分割多段マップ情報の直方体から最古の時分割多段マップ情報Partmap1の直方体を減算して、全時分割多段マップ情報の直方体を２分割する。このとき、更新部６は、全時分割多段マップ情報の直方体の分布情報（平均Ｐ及び分散Σ）から、最古の時系列多段マップ情報の直方体の分布情報を減算し、さらに、時系列多段マップ情報Partmap3の直方体の分布情報と、時系列多段マップ情報Partmap4の直方体の分布情報と、を加算する。更新部６は、以上の処理を行うことで、時分割多段マップ情報の整合性を図ることができる。
【００５８】
復元部５は、上述のように、更新部６により更新された全時分割多段マップ情報の中から、任意に、ｎ個の芯線情報を選択して組を生成し、これら芯線情報で平面領域を構成できるか否かを判断する。復元部５は、選択した芯線情報で平面領域が構成可能と判断した場合、その平面領域の平面方程式を算出する。復元部５は、全時分割多段マップ情報の他の芯線情報を、算出した平面方程式にあてはめ、上記所定条件（２）式を満たすか否かを判断することで平面探索を行い、その平面領域を構成する芯線情報のグループを推定し、平面領域を検出する。
【００５９】
なお、本実施の形態２に係る３次元環境復元装置２０において、上記実施の形態１に係る３次元環境復元装置１０と同一部分に同一符号を付して、詳細な説明は省略する。
【００６０】
以上、本実施の形態２に係る３次元環境復元装置２０によれば、時分割多段マップ情報において、芯線情報を更新しつつ、その更新した芯線情報を用いて所定空間内の平面領域の検出を高精度かつ高速に行うことができる。
【００６１】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
例えば、上記実施の形態において、復元部５は、所定空間内の平面領域の検出を行っているが、これに限らず、例えば、曲面などの任意形状を検出することができる。
【００６２】
復元部５は、芯線情報生成部４により生成された所定領域近傍（曲面近傍）の芯線情報を選択し、選択した芯線情報のヒストグラム（例えば、Point Feature Histogram）に基づいた特徴量を算出する。そして、復元部５は、算出した特徴量と、予め設定された所定モデルの特徴量と、を比較することで、所定空間内の任意形状を検出する。このように、芯線情報を用いることで、例えば、法線ベクトルを算出する際に、法線ベクトルの存在領域を「芯線と直交する軸周り」に限定できるため、上記算出処理を簡略化することができる。
【００６３】
また、以下のようにして、復元部５は、所定空間内の任意形状を検出してもよい。なお、所定モデルは、例えば、ＣＡＤ（Computer Aided Design）などの３次元メッシュ表現で表面点群（パッチ）の位置及び法線の両方が取れるものとする。
【００６４】
復元部５は、芯線情報生成部４により生成された芯線情報群のうち、予め設定された所定モデルの少なくとも一部の曲面を表す所定数式（例えば、（ｘ−ａ）２＋（ｙ−ｂ）２＋（ｚ−ｃ）２＝０）にあてはまる芯線情報を選択する。ここで、復元部５は、所定モデルの形状が小さい場合、選択した芯線情報の近傍から、別の芯線情報を選択する。また、復元部５は、所定モデルの形状の大きさに応じて、選択した芯線情報の近傍から選択範囲を広げて別の芯線情報を選択してもよい。
【００６５】
次に、復元部５は、上記選択した芯線情報を含むマップ情報と所定モデルとを比較し、その差分を算出し、算出した差分を評価することで、所定空間内の任意形状の検出を行う。
【００６６】
上記実施の形態において、３次元環境復元装置１０、２０は、復元部５により復元された所定空間内の対象物体形状とマップ情報とに基づいて、ロボットなどの移動体の経路を設定する経路設定部を備えていてもよい。経路設定部は、例えば、復元部５により復元された対象物体を最適に回避するような移動体の経路を設定してもよい。
【００６７】
また、上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、演算装置２、２１のマップ情報生成部３、芯線情報生成部４、復元部５、及び更新部６が実行する処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００６８】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００６９】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【産業上の利用可能性】
【００７０】
本発明は、例えば、生活環境で物体操作などを行う支援型ロボットに搭載され、３次元環境の対象物体を復元する３次元環境復元装置に利用可能である。
【符号の説明】
【００７１】
１ 環境認識センサ
２ 演算装置
３ マップ情報生成部
４ 芯線情報生成部
５ 復元部
６ 更新部
１０、２０ ３次元環境復元装置
【技術分野】
【０００１】
本発明は、対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて対象物体が存在する格子領域に直方体が配置され、配置された直方体に３次元点群データを含むマップ情報を生成する３次元環境復元装置、その処理方法、及びプログラムに関するものである。
【背景技術】
【０００２】
例えば、ロボットなどが周囲環境に接触しないようにアームを移動させるためには、周囲の３次元環境をマップ化する必要がある。３次元環境を効率的にマップ化する手法として、例えば、多段マップ法やボクセルマップ法を用いた手法が提案されている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特願２０１０−２５２４８号
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記手法において、マップ情報の空間を所定幅毎で区切り、データの離散化を行っている。一般に、各区切りを小さくするとマップ情報の精度が向上するが、マップ情報のメモリ消費量が増加することとなる。一方で、各区切りを大きくするとメモリ消費量の低減に繋がるが、マップ情報の精度が低下することとなる。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、マップ情報の精度を維持しつつ、マップ情報のメモリ消費量を効果的に低減することができる３次元環境復元装置、その処理方法、及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、所定空間内の対象物体の３次元計測を行う計測手段と、前記計測手段により計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成するマップ情報生成手段と、前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する芯線情報生成手段と、を備える、ことを特徴とする３次元環境復元装置である。この一態様によれば、マップ情報の精度を維持しつつ、マップ情報のメモリ消費量を効果的に低減することができる。
【０００７】
この一態様において、前記芯線情報生成手段は、前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データの分布情報を算出し、前記算出した分布情報に基づいて、前記３次元点群データを楕円近似し、前記近似した楕円の長軸を算出することで、前記芯線情報を算出してもよい。これにより、マップ情報の直方体内の３次元点群を線分近似した芯線情報を算出することができる。
【０００８】
この一態様において、前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の対象物体形状を復元する復元手段を更に備えていてもよい。これにより、芯線情報を用いて、所定空間内の対象物体形状を高精度に復元することができる。
【０００９】
この一態様において、前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の平面を検出してもよい。これにより、芯線情報を用いて、所定空間内の平面を高精度かつ高速に検出することができる。
【００１０】
この一態様において、前記復元手段は、前記マップ情報の中から、複数の前記芯線情報を選択して平面を構成し、前記構成した平面の平面方程式に前記マップ情報の他の芯線情報をあてはめ、所定条件を満たすか否かを判断することで、前記平面を構成する前記芯線情報のグループを推定してもよい。これにより、芯線情報を用いて、所定空間内の平面を高精度かつ高速に検出することができる。
【００１１】
この一態様において、前記マップ情報生成手段は、前記計測手段により計測された３次元計測データに基づいて、前記配置された直方体に３次元点群データを含む時分割され多段状で構成された時分割多段マップ情報を生成し、生成した前記時分割多段マップ情報を全て加算した全体の時分割多段マップ情報を生成しており、前記マップ情報生成手段により生成された前記全体の時分割多段マップ情報に、最新の前記時分割多段マップ情報を加算し、最古の前記時分割多段マップ情報を減算することで、前記全体の時分割多段マップ情報及び芯線情報の更新を行う更新手段を更に備えていてもよい。これにより、時分割多段マップ情報を更新すると共に、その芯線情報も更新することができる。
【００１２】
この一態様において、前記更新手段は、前記加算において、前記最新の時分割多段マップ情報の直方体を用いて、過去の前記時分割多段マップの直方体を分割してもよい。これにより、加算処理における整合性を図ることができる。
【００１３】
この一態様において、前記更新手段は、前記減算において、前記全体の時分割多段マップ情報の直方体から前記最古の時分割多段マップ情報を減算することで分割を行うと共に、前記分割されたマップ情報の直方体の分布情報を夫々算出してもよい。これにより、減算処理における整合性を図ることができる。
【００１４】
この一態様において、前記復元手段は、前記芯線情報生成手段により生成された所定領域近傍の前記芯線情報を選択し、前記選択した芯線情報の統計的な特徴量を算出し、前記算出した特徴量と、所定モデルの特徴量と、を比較することで、前記対象物体の形状を検出してもよい。これにより、芯線情報を用いて、所定空間の対象物体の形状を高精度かつ高速に検出することができる。
【００１５】
この一態様において、前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報群の中から、所定モデルの少なくとも一部の曲面を示す所定数式にあてはまる、前記芯線情報を選択し、前記選択した芯線情報を含むマップ情報と、前記所定モデルとを比較し、評価することで、前記対象物体の形状を検出してもよい。これにより、芯線情報を用いて、所定空間の対象物体の形状を高精度かつ高速に検出することができる。
【００１６】
この一態様において、前記マップ情報は、例えば、多段状に構成された多段マップ情報、又はボクセルマップ情報であってもよい。
【００１７】
この一態様において、前記復元手段により復元された前記所定空間の対象物体形状と前記マップ情報とに基づいて、移動体の経路を設定する経路設定手段を更に備えていてもよい。これにより、例えば、所定空間の対象物体の形状を考慮して、移動体の最適な経路を設定することができる。
【００１８】
他方、上記目的を達成するための本発明の一態様は、所定空間内の対象物体の３次元計測を行うステップと、前記計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体に３次元点群データを含むマップ情報を生成するステップと、前記生成されたマップ情報の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成するステップと、を含む、ことを特徴とする３次元環境復元装置の処理方法であってもよい。
【００１９】
また、上記目的を達成するための本発明の一態様は、計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体に３次元点群データを含むマップ情報を生成する処理と、前記生成されたマップ情報の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する処理と、をコンピュータに実行させることを特徴とするプログラムであってもよい。
【発明の効果】
【００２０】
本発明によれば、マップ情報の精度を維持しつつ、マップ情報のメモリ消費量を効果的に低減することができる３次元環境復元装置、その処理方法、及びプログラムを提供することができる。
【図面の簡単な説明】
【００２１】
【図１】本発明の実施の形態１に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。
【図２】マップ情報生成部により生成されるボクセルマップ情報の一例を示す図である。
【図３】（ａ）マップ情報の構成要素における３次元点群データの一例を示す図である。（ｂ）マップ情報の構成要素における芯線情報の一例を示す図である。
【図４】選択した芯線で平面領域が構成可能か否かの判断を説明するための図である。
【図５】本発明の実施の形態１に係る３次元環境復元装置による復元方法の一例を示すフローチャートである。
【図６】本発明の実施の形態２に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。
【図７】本発明の実施の形態２に係る３次元環境復元装置による処理フローの一例を示す概念図である。
【図８】分布情報の加減算を示す図である。
【図９】（ａ）更新部による分割処理を説明するための図である。（ｂ）更新部による分割処理を説明するための図である。
【図１０】（ａ）更新部による分割処理を説明するための図である。（ｂ）更新部による分割処理を説明するための図である。
【発明を実施するための形態】
【００２２】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施の形態１に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。本実施の形態１に係る３次元環境復元装置１０は、所定空間内に存在する任意の対象物体の形状を検出、復元することができるものである。
【００２３】
３次元環境復元装置１０は、所定空間内の対象物体の３次元計測を行う環境認識センサ１と、環境認識センサ１により計測された対象物体の３次元計測データに基づいて、所定空間内の対象物体形状（環境形状）を正確に検出、復元する演算装置２と、を備えている。
【００２４】
環境認識センサ１は、計測手段の一具体例であり、例えば、カメラ、超音波センサ、レーザレンジファインダなどから構成されており、所定空間内の対象物体の３次元計測を行なうことができる。例えば、レーザレンジファインダは、前方に向けてレーザを放射し、そのレーザを左右方向へと走査することで対象物体までの距離データを取得して、３次元計測を行う。環境認識センサ１は、演算装置２に接続されており、計測した３次元計測データを演算装置２に対して出力する。
【００２５】
演算装置２は、環境認識センサ１により計測された３次元計測データに基づいて、所定空間のマップ情報を生成する。なお、演算装置２は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバス等によって相互に接続されている。演算装置２は、環境認識センサ１から出力された３次元計測データを、例えば、ＲＡＭ等を用いて逐次記憶する。
【００２６】
演算装置２は、マップ情報を生成するマップ情報生成部３と、芯線情報を生成する芯線情報生成部４と、所定空間内の対象物体を復元する復元部５と、を有している。
【００２７】
マップ情報生成部３は、マップ情報生成手段の一具体例であり、環境認識センサ１により計測された３次元計測データに基づいて、対象物体の３次元点群データを含む多段マップ情報やボクセルマップ情報などの、マップ情報を生成する。
【００２８】
より具体的には、マップ情報生成部３は、環境認識センサ１により計測された３次元計測データに基づいて対象物体の３次元点群データを生成し、３次元環境の基準平面が格子状に区切られて対象物体が存在する格子領域に直方体（キューボイド（cuboid））が配置され、その配置された直方体内に３次元点群データを含むマップ情報（例えば、図２に示す３次元座標系のボクセルマップ情報）を生成する。
【００２９】
図２に示すように、例えば、ボクセルマップ情報において、環境の基準平面を格子状に区切り、対象物体が存在する領域に対して直方体が割り当てられる。直方体の上面の位置を示す値及び高さを示す値は、任意の実数値を取ることができ、各直方体は、上面の位置Ｔｏｐ及び高さＤｅｐｔｈのデータを保持している。また、マップ情報では、環境内に複数の対象物体が存在する場合において、その配置状況に応じて高さ方向（ｚ方向）に複数の直方体を多段状に設定することができる。
【００３０】
マップ情報生成部３は、芯線情報生成部４に接続されており、生成したマップ情報を芯線情報生成部４に対して出力する。
【００３１】
芯線情報生成部４は、芯線情報生成手段の一具体例であり、マップ情報生成部３により生成されたマップ情報の構成要素（直方体）内の３次元点群データ（図３（ａ））に基づいて、その構成要素に対する、３次元点群を局所的に線分近似した芯線情報を生成する。
【００３２】
芯線情報生成部４は、例えば、生成されたマップ情報の３次元点群データの平均値Ｐｎ及び分散値Σｎなどの分布情報を下記（１）式を用いて算出することで、平均値Ｐｎ及び分散値Σｎの逐次更新することができる。なお、下記（１）式においてＸｎを計測点とする。
平均値Ｐｎ＝ｍｎ／ｎ
分散値Σｎ＝（Ｓｎ＋Ｐｎｍｎ）／ｎ （１）式
ｍｎ＝（ｎ−１）Ｐｎ−１＋Ｘｎ
Ｓｎ＝Ｓｎ−１＋ＸｎＸｎ
【００３３】
そして、芯線情報生成部４は、算出した３次元点群データの平均値Ｐｎ及び分散値Σｎを用いて、３次元点群データを楕円体近似した際の長軸（芯線）を算出し、その中心点及び方向ベクトルを算出することで、その３次元点群データの芯線情報を生成する（図３（ｂ））。芯線情報生成部４は、マップ情報の構成要素毎に、算出した芯線情報を対応させて記憶する。
【００３４】
このように、マップ情報内の対象物体を、芯線情報を用いて表し保持することで、マップ情報の解像度を疑似的に向上させることができる。すなわち、マップ情報の精度を維持しつつ、そのメモリ消費量を効果的に低減することができる。芯線情報生成部４は、復元部５に接続されており、算出したマップ情報の構成要素毎の芯線情報を復元部５に対して出力する。
【００３５】
復元部５は、復元手段の一具体例であり、芯線情報生成部４により生成された芯線情報に基づいて、所定空間内の対象物体形状を復元する。
【００３６】
復元部５は、マップ情報生成部３により生成されたマップ情報の中から、任意に、ｎ個の芯線情報を選択して組を生成し、これら選択した芯線情報で平面領域を構成できるか否かを判断する。
【００３７】
復元部５は、例えば、図４に示すように、３つの芯線情報を選択し（ｎ＝３）、各芯線の始点ｐ１〜ｐ３（直方体内の点群の平均）間を結ぶベクトルａを算出する。復元部５は、算出したベクトルａと、芯線の方向ベクトルｎ１、ｎ２（芯線の固有ベクトル）とで外積を取り、垂直ベクトルｂ１（ｂ１＝ｎ１×ａ）、ｂ２（ｂ２＝ｎ２×ａ）を夫々算出する。復元部５は、選択した芯線に対する垂直ベクトルｂ１、ｂ２の角度が予め設定された所定角度以内となっていれば、その選択した３つの芯線で平面領域が構成可能と判断する。
【００３８】
復元部５は、選択した芯線情報で平面領域が構成可能と判断した場合、その平面領域の平面方程式（ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０）を算出する。
【００３９】
復元部５は、マップ情報の他の芯線情報をその算出した平面方程式にあてはめ、下記所定条件（２）式を満たすか否かを判断することで平面探索を行い、その平面領域を構成する芯線情報のグループ（平面構成要素）を推定し、平面領域を検出する。なお、下記（２）式において、ｂ＝（ａ、ｂ、ｃ）、及びｂｉ＝（ｎｘ、ｎｙ、ｎｚ）（入力芯線の方向ベクトル）とする。また、上記所定角度、下記第１及び第２所定閾値は、例えば、ＲＯＭやＲＡＭなどに予め設定されている。
距離閾値||ａｘｉ＋ｂｙｉ＋ｃｚｉ＋ｄ||＜第１所定閾値
角度閾値|ｃｏｓ−１（ｂ・ｂｉ/|ｂ||ｂｉ|）−π/２|≦第２所定閾値 （２）式
【００４０】
なお、復元部５は、所定領域近傍の３次元点群の芯線のペアを構成し、その構成した芯線のペアに基づいて、同属領域を拡張する所謂region growingアプローチ手法を用いて、上記平面領域の検出を行っても良く、芯線情報を用いた任意の検出方法を適用することができる。
【００４１】
以上のようにして、復元部５は、多段マップ法においては水平面、ボクセルマップ法においては水平面及び垂直面、などの特定の傾きの平面領域に制限されることなく、それ以外の任意の傾きを有する平面領域に対しても、離散化の影響を受けることなく、高精度に検出し、復元することができる。さらに、マップ情報の３次元点群を局所的に線分近似した芯線情報を利用し、情報処理量を低下させることで、平面領域の検出を高精度かつ高速に行うことができる。
【００４２】
次に、本実施の形態１に係る３次元環境復元装置１０の処理方法について、詳細に説明する。図５は、本実施の形態１に係る３次元環境復元装置による復元方法の一例を示すフローチャートである。
【００４３】
環境認識センサ１は、所定空間内の対象物体の３次元計測を行ない（ステップＳ１０１）、計測した３次元計測データを演算装置２のマップ情報生成部３に対して出力する。
【００４４】
マップ情報生成部３は、環境認識センサ１からの３次元計測データに基づいて、対象物体の３次元点群データを生成し、対象物体が存在する格子領域に直方体が配置され、その配置された直方体に３次元点群データを含むマップ情報を生成し（ステップＳ１０２）、生成したマップ情報を芯線情報生成部４に対して出力する。
【００４５】
芯線情報生成部４は、マップ情報生成部３からのマップ情報の直方体内の３次元点群データに基づいて、マップ情報の構成要素（直方体）毎に、３次元点群データの芯線情報を生成し（ステップＳ１０３）、生成した芯線情報を復元部５に対して出力する。
【００４６】
復元部５は、芯線情報生成部４により生成された芯線情報に基づいて、マップ情報の平面領域を検出する（ステップＳ１０４）。
【００４７】
以上、本実施の形態１に係る３次元環境復元装置１０によれば、マップ情報の構成要素毎に、３次元点群を局所的に線分近似した芯線情報を対応させて記憶することで、マップ情報の精度を維持しつつ、そのメモリ消費量を効果的に低減することができる。さらに、芯線情報を利用することで、所定空間内の平面領域の検出を高精度かつ高速に行うことができる。
【００４８】
実施の形態２．
図６は、本発明の実施の形態２に係る３次元環境復元装置の概略的なシステム構成を示すブロック図である。本実施の形態２に係る３次元環境復元装置２０は、時系列拡張が行われ、時分割されかつ多段で構成された時分割多段マップ情報を用いて平面領域の検出を行う。なお、時分割多段マップの詳細な説明は、特願２０１０−２５２４８号に詳細に記載されており、これを援用できるものとする。
【００４９】
３次元環境復元装置２０は、環境認識センサ１と、マップ情報生成部３、芯線情報生成部４、復元部５、及び更新部６、を有する演算装置２１と、を備えている。
【００５０】
演算装置２１のマップ情報生成部３は、環境認識センサ１により計測された３次元計測データに基づいて、対象物体が存在する格子領域に直方体が配置され、その配置された直方体に３次元点群データを含む複数の時分割多段マップ情報を生成する。ここで、時分割多段マップ情報の各構成要素の基となった３次元点群データは、構成要素毎に分布情報として保持される。
【００５１】
マップ情報生成部３は、生成した時分割多段マップ情報群を全て加算することで、全体の時分割多段マップ情報（以下、全時分割多段マップ情報）を生成する（図７（１））。なお、この処理は、初回のみ実行される。マップ情報生成部３は、生成した全時分割多段マップ情報を更新部６に対して出力する。
【００５２】
更新部６は、更新手段の一具体例であり、マップ情報生成部３により生成された全時分割多段マップ情報に、最新の時分割多段マップ情報を加算し（図７の（２））、続いて、最古の時分割多段マップ情報を減算する（図７（３））ことで、全時分割多段マップ情報の更新を行う。すなわち、更新部６は、全時分割多段マップ情報の更新毎に、ＦＩＦＯ（First-In-First-Out）のように、全時分割多段マップ情報に、最新の時分割多段マップ情報を加算し、続いて、最古の時分割多段マップ情報を減算することを繰り返す。
【００５３】
このとき、更新部６は、時分割多段マップ情報の３次元点群データの分布情報についても、上記同様に減算及び加算を行い、分布情報の整合性を図る（図８）。例えば、構成要素Ｃ１の内部分布Ｎ１（Ｐ１、Σ１）と、構成要素Ｃ２の内部分布Ｎ２（Ｐ２、Σ２）とを、下記（３）式を用いて加算及び減算することができる。
加算： Ｐ＝（ｍ１＋ｍ２）/（ｎ１＋ｎ２）
Σ＝（Ｓ１＋Ｓ２）/（ｎ１＋ｎ２）−ｍ１ｍ２
減算： Ｐ＝（ｍ１−ｍ２）/（ｎ１−ｎ２） （３）式
Σ＝（Ｓ１−Ｓ２）/（ｎ１−ｎ２）−ｍ１ｍ２
【００５４】
なお、上記加算及び減算は、上述のようにして行うことが可能であるが、その際、１つの直方体が２つに分かれる「分割」について、状況に応じた適切な対応が必要となる。以下、その分割方法について説明する。
【００５５】
更新部６は、全時分割多段マップ情報に対して最新の時分割多段マップ情報を加算する場合（図７の（２））に、最新の時分割多段マップ情報を参照して、過去の全ての時分割多段マップ情報を下記のように変形する。
【００５６】
例えば、更新部６は、最新の時分割多段マップ情報Partmap４の直方体（cuboid）を用いて、時分割多段マップ情報Partmap２、３の直方体を分割する（図９（ａ））。その直方体表現において、図９（ａ）に示すように加算処理により、直方体に分離が生じているが、内部の芯線（点分布）は繋がった状態となる（図９（ｂ））。なお、直方体を所定の比率で分割することなく、便宜的に２分割するだけでもよい。
【００５７】
また、更新部６は、全時分割多段マップ情報の直方体から最古の時分割多段マップ情報の直方体を減算する場合（図７の（３））において、例えば、全時分割多段マップ情報の直方体から最古の時分割多段マップ情報Partmap1の直方体を減算して、全時分割多段マップ情報の直方体を２分割する。このとき、更新部６は、全時分割多段マップ情報の直方体の分布情報（平均Ｐ及び分散Σ）から、最古の時系列多段マップ情報の直方体の分布情報を減算し、さらに、時系列多段マップ情報Partmap3の直方体の分布情報と、時系列多段マップ情報Partmap4の直方体の分布情報と、を加算する。更新部６は、以上の処理を行うことで、時分割多段マップ情報の整合性を図ることができる。
【００５８】
復元部５は、上述のように、更新部６により更新された全時分割多段マップ情報の中から、任意に、ｎ個の芯線情報を選択して組を生成し、これら芯線情報で平面領域を構成できるか否かを判断する。復元部５は、選択した芯線情報で平面領域が構成可能と判断した場合、その平面領域の平面方程式を算出する。復元部５は、全時分割多段マップ情報の他の芯線情報を、算出した平面方程式にあてはめ、上記所定条件（２）式を満たすか否かを判断することで平面探索を行い、その平面領域を構成する芯線情報のグループを推定し、平面領域を検出する。
【００５９】
なお、本実施の形態２に係る３次元環境復元装置２０において、上記実施の形態１に係る３次元環境復元装置１０と同一部分に同一符号を付して、詳細な説明は省略する。
【００６０】
以上、本実施の形態２に係る３次元環境復元装置２０によれば、時分割多段マップ情報において、芯線情報を更新しつつ、その更新した芯線情報を用いて所定空間内の平面領域の検出を高精度かつ高速に行うことができる。
【００６１】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
例えば、上記実施の形態において、復元部５は、所定空間内の平面領域の検出を行っているが、これに限らず、例えば、曲面などの任意形状を検出することができる。
【００６２】
復元部５は、芯線情報生成部４により生成された所定領域近傍（曲面近傍）の芯線情報を選択し、選択した芯線情報のヒストグラム（例えば、Point Feature Histogram）に基づいた特徴量を算出する。そして、復元部５は、算出した特徴量と、予め設定された所定モデルの特徴量と、を比較することで、所定空間内の任意形状を検出する。このように、芯線情報を用いることで、例えば、法線ベクトルを算出する際に、法線ベクトルの存在領域を「芯線と直交する軸周り」に限定できるため、上記算出処理を簡略化することができる。
【００６３】
また、以下のようにして、復元部５は、所定空間内の任意形状を検出してもよい。なお、所定モデルは、例えば、ＣＡＤ（Computer Aided Design）などの３次元メッシュ表現で表面点群（パッチ）の位置及び法線の両方が取れるものとする。
【００６４】
復元部５は、芯線情報生成部４により生成された芯線情報群のうち、予め設定された所定モデルの少なくとも一部の曲面を表す所定数式（例えば、（ｘ−ａ）２＋（ｙ−ｂ）２＋（ｚ−ｃ）２＝０）にあてはまる芯線情報を選択する。ここで、復元部５は、所定モデルの形状が小さい場合、選択した芯線情報の近傍から、別の芯線情報を選択する。また、復元部５は、所定モデルの形状の大きさに応じて、選択した芯線情報の近傍から選択範囲を広げて別の芯線情報を選択してもよい。
【００６５】
次に、復元部５は、上記選択した芯線情報を含むマップ情報と所定モデルとを比較し、その差分を算出し、算出した差分を評価することで、所定空間内の任意形状の検出を行う。
【００６６】
上記実施の形態において、３次元環境復元装置１０、２０は、復元部５により復元された所定空間内の対象物体形状とマップ情報とに基づいて、ロボットなどの移動体の経路を設定する経路設定部を備えていてもよい。経路設定部は、例えば、復元部５により復元された対象物体を最適に回避するような移動体の経路を設定してもよい。
【００６７】
また、上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、演算装置２、２１のマップ情報生成部３、芯線情報生成部４、復元部５、及び更新部６が実行する処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００６８】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００６９】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【産業上の利用可能性】
【００７０】
本発明は、例えば、生活環境で物体操作などを行う支援型ロボットに搭載され、３次元環境の対象物体を復元する３次元環境復元装置に利用可能である。
【符号の説明】
【００７１】
１ 環境認識センサ
２ 演算装置
３ マップ情報生成部
４ 芯線情報生成部
５ 復元部
６ 更新部
１０、２０ ３次元環境復元装置
【特許請求の範囲】
【請求項１】
所定空間内の対象物体の３次元計測を行う計測手段と、
前記計測手段により計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成するマップ情報生成手段と、
前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する芯線情報生成手段と、
を備える、ことを特徴とする３次元環境復元装置。
【請求項２】
請求項１記載の３次元環境復元装置であって、
前記芯線情報生成手段は、前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データの分布情報を算出し、前記算出した分布情報に基づいて、前記３次元点群データを楕円近似し、前記近似した楕円の長軸を算出することで、前記芯線情報を算出する、ことを特徴とする３次元環境復元装置。
【請求項３】
請求項１又は２記載の３次元環境復元装置であって、
前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の対象物体形状を復元する復元手段を更に備える、ことを特徴とする３次元環境復元装置。
【請求項４】
請求項３記載の３次元環境復元装置であって、
前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の平面を検出する、ことを特徴とする３次元環境復元装置。
【請求項５】
請求項３又は４記載の３次元環境復元装置であって、
前記復元手段は、前記マップ情報の中から、複数の前記芯線情報を選択して平面を構成し、前記構成した平面の平面方程式に前記マップ情報の他の芯線情報をあてはめ、所定条件を満たすか否かを判断することで、前記平面を構成する前記芯線情報のグループを推定する、ことを特徴とする３次元環境復元装置。
【請求項６】
請求項１乃至５のうちいずれか１項記載の３次元環境復元装置であって、
前記マップ情報生成手段は、前記計測手段により計測された３次元計測データに基づいて、前記配置された直方体に３次元点群データを含む時分割され多段状で構成された時分割多段マップ情報を生成し、生成した前記時分割多段マップ情報を全て加算した全体の時分割多段マップ情報を生成しており、
前記マップ情報生成手段により生成された前記全体の時分割多段マップ情報に、最新の前記時分割多段マップ情報を加算し、最古の前記時分割多段マップ情報を減算することで、前記全体の時分割多段マップ情報及び芯線情報の更新を行う更新手段を更に備える、ことを特徴とする３次元環境復元装置。
【請求項７】
請求項６記載の３次元環境復元装置であって、
前記更新手段は、前記加算において、前記最新の時分割多段マップ情報の直方体を用いて、過去の前記時分割多段マップの直方体を分割する、ことを特徴とする３次元環境復元装置。
【請求項８】
請求項６又は７記載の３次元環境復元装置であって、
前記更新手段は、前記減算において、前記全体の時分割多段マップ情報の直方体から前記最古の時分割多段マップ情報を減算することで分割を行うと共に、前記分割されたマップ情報の直方体の分布情報を夫々算出する、ことを特徴とする３次元環境復元装置。
【請求項９】
請求項３乃至８のうちいずれか１項記載の３次元環境復元装置であって、
前記復元手段は、
前記芯線情報生成手段により生成された所定領域近傍の前記芯線情報を選択し、前記選択した芯線情報の統計的な特徴量を算出し、前記算出した特徴量と、所定モデルの特徴量と、を比較することで、前記対象物体の形状を検出する、ことを特徴とする３次元環境復元装置。
【請求項１０】
請求項３乃至８のうちいずれか１項記載の３次元環境復元装置であって、
前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報群の中から、所定モデルの少なくとも一部の曲面を示す所定数式にあてはまる、前記芯線情報を選択し、
前記選択した芯線情報を含むマップ情報と、前記所定モデルとを比較し、評価することで、前記対象物体の形状を検出する、ことを特徴とする３次元環境復元装置。
【請求項１１】
請求項１乃至１０のうちいずれか１項記載の３次元環境復元装置であって、
前記マップ情報は、多段状に構成された多段マップ情報、又はボクセルマップ情報である、ことを特徴とする３次元環境復元装置。
【請求項１２】
請求項３乃至１１のうちいずれか１項記載の３次元環境復元装置であって、
前記復元手段により復元された前記所定空間の対象物体形状と前記マップ情報とに基づいて、移動体の経路を設定する経路設定手段を更に備える、ことを特徴とする３次元環境復元装置。
【請求項１３】
所定空間内の対象物体の３次元計測を行うステップと、
前記計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成するステップと、
前記生成されたマップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成するステップと、
を含む、ことを特徴とする３次元環境復元装置の処理方法。
【請求項１４】
計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成する処理と、
前記生成されたマップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する処理と、
をコンピュータに実行させることを特徴とするプログラム。
【請求項１】
所定空間内の対象物体の３次元計測を行う計測手段と、
前記計測手段により計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成するマップ情報生成手段と、
前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する芯線情報生成手段と、
を備える、ことを特徴とする３次元環境復元装置。
【請求項２】
請求項１記載の３次元環境復元装置であって、
前記芯線情報生成手段は、前記マップ情報生成手段により生成された前記マップ情報の直方体内の３次元点群データの分布情報を算出し、前記算出した分布情報に基づいて、前記３次元点群データを楕円近似し、前記近似した楕円の長軸を算出することで、前記芯線情報を算出する、ことを特徴とする３次元環境復元装置。
【請求項３】
請求項１又は２記載の３次元環境復元装置であって、
前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の対象物体形状を復元する復元手段を更に備える、ことを特徴とする３次元環境復元装置。
【請求項４】
請求項３記載の３次元環境復元装置であって、
前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報に基づいて、前記所定空間内の平面を検出する、ことを特徴とする３次元環境復元装置。
【請求項５】
請求項３又は４記載の３次元環境復元装置であって、
前記復元手段は、前記マップ情報の中から、複数の前記芯線情報を選択して平面を構成し、前記構成した平面の平面方程式に前記マップ情報の他の芯線情報をあてはめ、所定条件を満たすか否かを判断することで、前記平面を構成する前記芯線情報のグループを推定する、ことを特徴とする３次元環境復元装置。
【請求項６】
請求項１乃至５のうちいずれか１項記載の３次元環境復元装置であって、
前記マップ情報生成手段は、前記計測手段により計測された３次元計測データに基づいて、前記配置された直方体に３次元点群データを含む時分割され多段状で構成された時分割多段マップ情報を生成し、生成した前記時分割多段マップ情報を全て加算した全体の時分割多段マップ情報を生成しており、
前記マップ情報生成手段により生成された前記全体の時分割多段マップ情報に、最新の前記時分割多段マップ情報を加算し、最古の前記時分割多段マップ情報を減算することで、前記全体の時分割多段マップ情報及び芯線情報の更新を行う更新手段を更に備える、ことを特徴とする３次元環境復元装置。
【請求項７】
請求項６記載の３次元環境復元装置であって、
前記更新手段は、前記加算において、前記最新の時分割多段マップ情報の直方体を用いて、過去の前記時分割多段マップの直方体を分割する、ことを特徴とする３次元環境復元装置。
【請求項８】
請求項６又は７記載の３次元環境復元装置であって、
前記更新手段は、前記減算において、前記全体の時分割多段マップ情報の直方体から前記最古の時分割多段マップ情報を減算することで分割を行うと共に、前記分割されたマップ情報の直方体の分布情報を夫々算出する、ことを特徴とする３次元環境復元装置。
【請求項９】
請求項３乃至８のうちいずれか１項記載の３次元環境復元装置であって、
前記復元手段は、
前記芯線情報生成手段により生成された所定領域近傍の前記芯線情報を選択し、前記選択した芯線情報の統計的な特徴量を算出し、前記算出した特徴量と、所定モデルの特徴量と、を比較することで、前記対象物体の形状を検出する、ことを特徴とする３次元環境復元装置。
【請求項１０】
請求項３乃至８のうちいずれか１項記載の３次元環境復元装置であって、
前記復元手段は、前記芯線情報生成手段により生成された前記芯線情報群の中から、所定モデルの少なくとも一部の曲面を示す所定数式にあてはまる、前記芯線情報を選択し、
前記選択した芯線情報を含むマップ情報と、前記所定モデルとを比較し、評価することで、前記対象物体の形状を検出する、ことを特徴とする３次元環境復元装置。
【請求項１１】
請求項１乃至１０のうちいずれか１項記載の３次元環境復元装置であって、
前記マップ情報は、多段状に構成された多段マップ情報、又はボクセルマップ情報である、ことを特徴とする３次元環境復元装置。
【請求項１２】
請求項３乃至１１のうちいずれか１項記載の３次元環境復元装置であって、
前記復元手段により復元された前記所定空間の対象物体形状と前記マップ情報とに基づいて、移動体の経路を設定する経路設定手段を更に備える、ことを特徴とする３次元環境復元装置。
【請求項１３】
所定空間内の対象物体の３次元計測を行うステップと、
前記計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成するステップと、
前記生成されたマップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成するステップと、
を含む、ことを特徴とする３次元環境復元装置の処理方法。
【請求項１４】
計測された対象物体の３次元計測データに基づいて、３次元環境が格子領域に区切られて前記対象物体が存在する格子領域に直方体が配置され、前記配置された直方体内に３次元点群データを含むマップ情報を生成する処理と、
前記生成されたマップ情報の直方体内の３次元点群データに基づいて、前記３次元点群データを線分近似した芯線情報を生成する処理と、
をコンピュータに実行させることを特徴とするプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１２−１８５５３４（Ｐ２０１２−１８５５３４Ａ）
【公開日】平成２４年９月２７日（２０１２．９．２７）
【国際特許分類】
【出願番号】特願２０１１−４６２７９（Ｐ２０１１−４６２７９）
【出願日】平成２３年３月３日（２０１１．３．３）
【出願人】（５０４１３７９１２）国立大学法人 東京大学 (1,942)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年９月２７日（２０１２．９．２７）
【国際特許分類】
【出願日】平成２３年３月３日（２０１１．３．３）
【出願人】（５０４１３７９１２）国立大学法人 東京大学 (1,942)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]