JP6162526B2 - ３次元環境復元装置 - Google Patents

Description

本発明は、３次元環境復元装置に関する。

近年、特定の空間内を移動してタスクを実行するロボットの開発が行われている。ロボットが移動してタスクを実行するためには、周囲の環境を認識し、障害物との衝突を防ぐ必要がある。

ロボットから周囲の環境を把握するために、例えば距離センサが用いられる。しかしながら、距離センサの限られた計測範囲では限られた環境しか認識できない。

特許文献１には、異なる時刻や異なる視点で計測した計測情報を、逐次環境マップに統合する時分割多段マップ（ＴＣＣＭ：Time-series Composite Cuboid Map）を用い、周囲の環境を認識する３次元環境復元装置について開示されている。

また非特許文献１には、環境を計測した距離点群を立方体で近似して表現するＯｃｔｏＭａｐに関する技術が開示されている。

特開２０１２−１０３１４４号公報

Kai M. Wurm 他４名、「OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation for Robotic Systems」、[online]、２０１０年５月、 [平成２５年７月９日検索]、インターネット＜ＵＲＬ：http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf＞

特許文献１に開示された発明では、環境を計測した距離点群を直方体で近似し、近接する直方体を結合して１つの物体とすることにより、データ量の削減を行う。図９は、特許文献１に開示された方法により、距離点群を直方体で近似し、近接した直方体を結合した状態を示した図である。また非特許文献１に開示された技術では、立方体状に隣接する４つの隣接立方体から、１段大きい１つの立方体を生成するよう結合することでデータ量の削減を行う。しかしながらこれらの方法では、ｚ軸に対して斜めの面や、ｚ軸に対して垂直な床や机面等の面については、３次元環境を復元する際に、隣接する直方体や立方体を統合することができず、データ量の削減の効率が低下していた。そのため、周囲の環境を正確に表現しつつ、データ量を抑えることが可能な環境復元方法が望まれていた。

本発明に係る３次元環境復元装置は、周囲の環境までの距離情報を有する距離点群を取得する距離点群取得部と、前記距離点群取得部により取得された距離点群の中から、分散が最も大きい方向に座標軸を設定する結合座標系設定部と、前記結合座標系設定部により設定された座標軸に基づいて前記距離点群の座標変換を行う距離点群座標変換部と、前記結合座標系設定部により設定された座標軸をグリッド化するグリッド化処理部と、前記距離点群座標変換部により変換された距離点群が、前記グリッド化処理部により生成されたグリッドを占有する状態に応じて、前記グリッドに占有直方体を生成するグリッド占有直方体生成部と、前記グリッド占有直方体生成部により生成された複数の占有直方体が互いに隣接する場合に、前記複数の占有直方体を統合する隣接直方体結合処理部と、を備える。
これにより、環境内の物体が並ぶ方向と平行な方向に新たに座標軸を設定し、物体を示す隣接する直方体同士を結合させて１物体として認識することができる。

データ量の削減の効率が上昇する。

実施の形態１にかかる３次元環境復元装置の構成を示すブロック図である。 実施の形態１にかかる占有直方体の統合を示す図である。 実施の形態１にかかる３次元環境復元装置の動作フローを示すフローチャートである。 実施の形態１にかかる結合座標系を決定する状態を示した図である。 実施の形態１にかかる主平面の検出を用いて結合座標系を決定する状態を示した図である。 実施の形態１にかかる結合座標系に基づく占有直方体を生成した状態を示した図である。 実施の形態２にかかる３次元環境復元装置の構成を示すブロック図である。 実施の形態２にかかる３次元環境復元装置の動作フローを示すフローチャートである。 関連する占有直方体の生成状態を示す図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１は、３次元環境復元装置１の構成を示すブロック図である。

３次元環境復元装置１は、距離点群取得部１１と、結合座標系設定部１２と、距離点群座標変換部１３と、グリッド化処理部１４と、グリッド占有直方体生成部１５と、隣接直方体結合処理部１６を備える。以下では、３次元環境復元装置１はロボットに搭載されているものとして説明する。

距離点群取得部１１は、ステレオカメラや、ＬＲＦ（レーザレンジファインダ）、Microsoft Kinect(登録商標)等の距離測定手段を有する。距離点群取得部１１は、距離測定手段により取得された、ロボットから障害物までの距離情報を、距離点群として取得する。この距離点群はそれぞれ、所定のｘ、ｙ、ｚ座標系における位置を示す座標情報を有している。距離点群取得部１１は、取得した距離点群のデータを、結合座標系設定部１２と距離点群座標変換部１３に出力する。なお典型的には、距離測定手段はロボットに搭載して使用するものを用いるが、環境内に固定して設置し、ロボットと障害物との相対的な距離を測定するものであっても良い。

結合座標系設定部１２は、結合座標系を設定するための演算を行う。例えば、結合座標系設定部１２は、距離点群取得部１１から入力された距離点群をｘ、ｙ、ｚ座標系上にプロットし、主成分分析を行う。これにより結合座標系設定部１２は、分散値が最も大きい方向をＺ軸とし、第２、第３固有ベクトル方向をそれぞれＸ軸、Ｙ軸方向とする結合座標系を設定する。結合座標系設定部１２は、設定した結合座標系の情報を、距離点群座標変換部１３とグリッド化処理部１４に出力する。

距離点群座標変換部１３は、結合座標系設定部１２により設定された結合座標系における、距離点群の座標を算出する。言い換えると、距離点群座標変換部１３は、距離点群取得部１１により取得された距離点群について、ｘ、ｙ、ｚ座標系の座標から、結合座標系設定部１２により設定されたＸ、Ｙ、Ｚ座標系の値となるよう、座標変換を行う。距離点群座標変換部１３は、変換した距離点群の座標情報を、グリッド占有直方体生成部１５に出力する。

グリッド化処理部１４は、結合座標系設定部１２で設定された結合座標系を、グリッド上に区切る処理を行う。グリッド化処理部１４は、Ｘ、Ｙ、Ｚ軸方向のうち少なくとも２方向を一定幅で区切る。典型的にはグリッド化処理部１４は、Ｘ、Ｙ軸方向を一定幅で区切る。
グリッド化処理部１４は、区切ったグリッドの情報をグリッド占有直方体生成部１５に出力する。

グリッド占有直方体生成部１５は、グリッド化処理部１４で区切られたグリッド内に位置する距離点群を評価し、距離点群がグリッドを占有する場合に、該グリッドに占有直方体を生成する。例えば、グリッド占有直方体生成部１５は、グリッド内にあらかじめ設定した閾値以上の数の距離点群がある場合に、該グリッド位置にグリッドの大きさの占有直方体を生成する。グリッド占有直方体生成部１５は、生成した占有直方体の情報を隣接直方体結合処理部１６に出力する。

隣接直方体結合処理部１６は、グリッド占有直方体生成部１５から入力された占有直方体について、隣接しているグリッドに直方体がある場合に、１つの直方体となるように結合する。図２は、隣接する２つの直方体を１つに結合した状態を示す図である。なお隣接直方体結合処理部１６は、占有直方体が隣接していない場合であっても、複数の直方体間距離があらかじめ設定した閾値以下であれば、１つの物体とみなし、結合することとしても良い。

次に、３次元環境復元装置１の動作について説明する。図３は、３次元環境復元装置１の動作フローを示すフローチャートである。

距離点群取得部１１は、ロボットの周囲の環境を、センサにより取得する（ステップＳ１）。具体的には、距離点群取得部１１は、ロボットの周囲の複数の点について距離情報を取得することにより、距離点群を取得する。例えば距離点群は、所定のｘ、ｙ、ｚ座標系における座標（ｘ_１、ｙ_１、ｚ_１）（ｘ_２、ｙ_２、ｚ_２）・・・（ｘ_ｎ、ｙ_ｎ、ｚ_ｎ）に位置している。

結合座標系設定部１２は、距離点群取得部１１が取得した距離点群に対し主成分分析を行い、結合計算に適した座標系を算出する（ステップＳ２）。図４は、結合座標系設定部１２が、主成分分析を用いて結合座標系を決定する様子を示した図である。なお、結合座標系設定部１２は、Ｚ軸方向に直方体を結合するため、分散値が最も大きい第１固有ベクトル方向をＺ軸とし、第２、第３固有ベクトル方向をそれぞれＸ軸、Ｙ軸方向とする結合座標系を設定するのが望ましい。

なお、結合座標系設定部１２は、主平面検出と主成分分析を用いて算出した結合座標系を設定しても良い。図５は、結合座標系設定部１２が、主平面検出と主成分分析を用いて結合座標系を決定する様子を示した図である。図５において、黒丸は主平面を検出した時に主平面に属する距離点群であり、白丸は平面に属しない距離点群である。具体的には、まず結合座標系設定部１２は、入力された距離点群から主平面を検出する。主平面の検出方法には、例えばＲＡＮＳＡＣ(Random sample Consensus)アルゴリズムにより、平面パラメータを最適化する方法を用いる。その後、結合座標系設定部１２は、主平面に属する距離点群に対して主成分分析を行い、分散値が最も大きい第１ベクトル方向をＺ軸方向とする。また結合座標系設定部１２は、第２、第３固有ベクトル方向をそれぞれＸ軸、Ｙ軸方向とする。

距離点群座標変換部１３は、距離点群取得部１１が取得した距離点群の座標変換を行う（ステップＳ３）。例えば距離点群座標変換部１３は、距離点群取得部１１で設定した座標系における距離点群の座標（ｘ_１、ｙ_１、ｚ_１）（ｘ_２、ｙ_２、ｚ_２）・・・（ｘ_ｎ、ｙ_ｎ、ｚ_ｎ）をそれぞれ、結合座標系設定部１２で設定した座標系における座標（Ｘ_１、Ｙ_１、Ｚ_１）（Ｘ_２、Ｙ_２、Ｚ_２）・・・（Ｘ_ｎ、Ｙ_ｎ、Ｚ_ｎ）に変換する。

グリッド化処理部１４は、結合座標系設定部１２で設定した統合座標系について、グリッド化処理を行う（ステップＳ４）。

グリッド占有直方体生成部１５は、グリッド化処理部１４でグリッド化された結合座標系に含まれる距離点群の数が閾値以上のグリッド位置に、占有直方体を生成する（ステップＳ５）。図６は、グリッド占有直方体生成部１５が、占有直方体を生成する様子を示す図である。

隣接直方体結合処理部１６は、グリッド占有直方体生成部１５で作成された２つ以上の直方体が、隣接するグリッドに存在している場合に、１つの直方体となるように結合する（ステップＳ６）。典型的には、その後ステップＳ１に戻り、ロボットの移動後の位置において同様の処理を繰り返す。

これにより、環境内の物体が並ぶ方向と平行な方向に新たに座標軸を設定し、物体を示す隣接する直方体同士を結合させて１物体として認識することができる。したがって、認識する障害物のデータ量の削減効率を向上させることができる。
またデータ量の削減効率が向上することから、ロボットの処理能力に余裕が発生するため、より広い範囲の環境認識が可能となる。

実施の形態２．
図７は、３次元環境復元装置２の構成を示すブロック図である。以下では、実施の形態１に示した３次元環境復元装置１の構成物品と同一の機能を奏する構成物品については、同一の符号を付し、説明を省略する。

３次元環境復元装置２は、距離点群取得部１１と、結合座標系設定部１２と、距離点群座標変換部１３と、グリッド化処理部１４と、グリッド占有直方体生成部１５と、隣接直方体結合処理部１６と、占有直方体記憶部２１と、距離点群変換部２２を備える。

占有直方体記憶部２１は、グリッド占有直方体生成部１５により生成された占有直方体の情報を記憶する。例えば占有直方体記憶部２１は、メモリ、ハードディスクを有し、データベースを用いることができる。具体的には、占有直方体記憶部２１は、グリッド占有直方体生成部１５により生成された、過去数フレームの距離点群から算出された占有直方体の情報を記憶する。なお、占有直方体記憶部２１において記憶するフレーム数は、任意に設定できることが望ましい。占有直方体記憶部２１は、占有直方体の位置とサイズの情報に加え、占有直方体に包含された距離点群の平均位置と、占有直方体に包含された距離点群の数を記憶する。占有直方体記憶部２１は、記憶した占有直方体の情報、及び占有直方体ごとの距離点群の情報を、距離点群変換部２２に出力する。

距離点群変換部２２は、占有直方体記憶部２１に記憶された各占有直方体について、占有直方体に包含された距離点群の平均位置に、記憶された距離点群の数だけ距離点群が存在するという情報に変換する。距離点群変換部２２は、変換後の距離点群の情報を、結合座標系設定部１２に出力する。

次に、３次元環境復元装置２の動作について説明する。図８は、３次元環境復元装置２の動作フローを示すフローチャートである。

距離点群取得部１１は、ロボットの周囲の環境を、センサにより取得する（ステップＳ１１）。

結合座標系設定部１２は、距離点群取得部１１が取得した距離点群と、距離点群変換部２２が記憶している距離点群に対し、結合計算に適した座標系を算出する（ステップＳ１２）。典型的には、結合座標系設定部１２は、距離点群取得部１１が取得した距離点群と、距離点群変換部２２が記憶している過去数フレームの距離点群の両方を、３次元空間内にプロットする。その後、結合座標系設定部１２は、３次元空間内にプロットされた距離点群について主成分分析を行い、分散値が最も大きい第１固有ベクトル方向をＺ軸とし、第２、第３固有ベクトル方向をそれぞれＸ軸、Ｙ軸方向とする結合座標系を設定する。

距離点群座標変換部１３は、距離点群取得部１１が取得した距離点群、及び距離点群変換部２２が記憶している距離点群の座標変換を行う（ステップＳ１３）。

グリッド化処理部１４は、結合座標系設定部１２で設定した座標系について、グリッド化処理を行う（ステップＳ１４）。

グリッド占有直方体生成部１５は、グリッド化処理部１４でグリッド化された結合座標系に含まれる距離点群の数が閾値以上のグリッド位置に、占有直方体を生成する（ステップＳ１５）。

隣接直方体結合処理部１６は、グリッド占有直方体生成部１５で作成された２つ以上の直方体が、隣接するグリッドに存在している場合に、１つの直方体となるように結合する（ステップＳ１６）。

占有直方体記憶部２１は、グリッド占有直方体生成部１５により生成された占有直方体の情報を記憶する（ステップＳ１７）。

距離点群変換部２２は、占有直方体記憶部２１に記憶された各占有直方体について、占有直方体に包含された距離点群の平均位置に、記憶された距離点群の数だけ距離点群が存在するという情報に変換する（ステップＳ１８）。その後ステップＳ１１に戻り、ロボットが移動した後の、次フレームについて処理を行う。このとき典型的には、距離点群変換部２２は、変換した距離点群の情報を、結合座標系設定部１２および距離点群座標変換部１３に出力する。

なお、上記では１つの距離点群取得部１１により距離点群を取得し、過去数フレーム分の距離点群データを用いるものとして説明したが、複数の距離点群取得部１１により多視点からの距離点群を用いることができる。言い換えると、占有直方体記憶部２１では、多視点からの過去数フレーム分の情報を記憶しておくことにより、多視点の過去の数フレームの距離点群の情報を利用することができる。これにより３次元環境復元装置２は、多視点からそれぞれ過去に取得した距離点群の情報を利用して、結合座標系を設定し、占有直方体を作成して占有直方体を統合することができる。

これにより、過去に取得した距離点群の情報を用いて、結合座標系を設定することができる。より具体的には、ロボットが移動するとともに最適な結合座標系は変化するが、過去の結合座標系で生成した占有直方体を一旦距離点群に戻し、現時刻に適した結合座標系を設定し直すことができる。
複数のフレームの情報、及び多視点の情報を統合できることから、未観測領域を減らすことができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、グリッド占有直方体生成部１５で生成するものは、直方体ではなく、円柱やカプセル状等の形状であってもよい。ただし、円柱やカプセル状の場合、隣接するグリッド間でも隙間が生じるため、直方体であることが望ましい。上記では、ロボットに３次元環境復元装置を搭載されているものとして説明したが、３次元環境復元装置の一部又は全部がロボットの外部に設置され、無線等によりロボットと情報の送受信を行うこととしてもよい。

１ ３次元環境復元装置
２ ３次元環境復元装置
１１ 距離点群取得部
１２ 結合座標系設定部
１３ 距離点群座標変換部
１４ グリッド化処理部
１５ グリッド占有直方体生成部
１６ 隣接直方体結合処理部
２１ 占有直方体記憶部
２２ 距離点群変換部

Claims (1)

1. 周囲の環境までの距離情報を有する距離点群を取得する距離点群取得部と、
   前記距離点群取得部により取得された距離点群の中から、分散が最も大きい方向に座標軸を設定する結合座標系設定部と、
   前記結合座標系設定部により設定された座標軸に基づいて前記距離点群の座標変換を行う距離点群座標変換部と、
   前記結合座標系設定部により設定された座標軸のうち、分散が最も大きい方向の座標軸に直交する２つの固有ベクトル方向にそれぞれ定められた座標軸方向を一定幅で区切ってグリッド化するグリッド化処理部と、
   前記距離点群座標変換部により変換された距離点群が、前記グリッド化処理部により生成されたグリッドを占有する状態に応じて、前記グリッドに占有直方体を生成するグリッド占有直方体生成部と、
   前記グリッド占有直方体生成部により生成された複数の占有直方体が、分散が最も大きい方向の座標軸に沿って互いに隣接する場合に、前記複数の占有直方体を統合して一つの占有直方体にする隣接直方体結合処理部と、
   を備える、３次元環境復元装置。

Images