JP2010066595A - 環境地図生成装置及び環境地図生成方法 - Google Patents

環境地図生成装置及び環境地図生成方法 Download PDF

Info

Publication number
JP2010066595A
JP2010066595A JP2008233708A JP2008233708A JP2010066595A JP 2010066595 A JP2010066595 A JP 2010066595A JP 2008233708 A JP2008233708 A JP 2008233708A JP 2008233708 A JP2008233708 A JP 2008233708A JP 2010066595 A JP2010066595 A JP 2010066595A
Authority
JP
Japan
Prior art keywords
plane
position data
dimensional position
map generation
environment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
JP2008233708A
Other languages
English (en)
Inventor
Yutaka Takaoka
豊 高岡
Satoshi Kagami
聡 加賀美
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Tokyo NUC
Toyota Motor Corp
Original Assignee
University of Tokyo NUC
Toyota Motor Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Tokyo NUC, Toyota Motor Corp filed Critical University of Tokyo NUC
Priority to JP2008233708A priority Critical patent/JP2010066595A/ja
Publication of JP2010066595A publication Critical patent/JP2010066595A/ja
Pending legal-status Critical Current

Links

Landscapes

  • Instructional Devices (AREA)
  • Processing Or Creating Images (AREA)
  • Image Processing (AREA)
  • Image Analysis (AREA)

Abstract

【課題】数の3次元位置データをより精度良く繋ぎ合わせることにより、より精細な環境地図を生成することを可能とする。
【解決手段】環境地図を生成する環境地図生成装置100は、所定の環境内を視覚的に認識する視覚センサ11と、視覚センサ11による計測結果を用いて、環境に存在する計測対象の位置を示す3次元位置データを生成する3次元位置データ生成部12と、異なる計測位置で計測された複数の3次元位置データを統合することにより、環境に関する環境地図を生成する環境地図生成部13とを備える。環境地図生成部13は、法線が互いに平行となっていない少なくとも3つの平面であって、複数の3次元位置データにおいて対応する平面を有する平面を用いて、複数の3次元位置データを統合する。
【選択図】図1

Description

本発明は、環境地図生成装置、環境地図生成方法に関し、特に移動ロボットが移動するために必要な環境地図を生成する環境地図生成装置及び環境地図生成方法に関する。
移動ロボットは、環境地図を参照することによって移動経路を計画し、当該移動経路に従って移動を行う。例えば、環境地図は、移動ロボットが移動を行う2次元の移動空間を格子状に分割するとともに、格子に囲まれた各々のセルが、移動ロボットにとって移動可能な領域であるか否かを表したグリッド地図として作成される。
移動ロボットには、環境地図生成装置が搭載されている。また、環境地図生成装置には、レーザレンジファインダなどの視覚センサが搭載されている。そして、環境地図生成装置は、視覚センサにより取得される距離データを用いて、外部環境の3次元位置データを生成する。次に、3次元位置データから複数の平面を検出することにより、環境地図を生成する(例えば、特許文献1〜5参照)。
また、環境地図生成装置は、複数の異なる位置で3次元位置データを計測することによって、広範囲の環境地図を生成することができる。具体的には、環境地図生成装置は、当該複数の3次元位置データを計測位置などに基づいて繋ぎ合わせる。これにより、環境地図生成装置は、複数の局所的な3次元位置データが統合された広範囲の環境地図を生成する。
例えば、ICPアルゴリズムを用いて、複数の3次元位置データにおいて、一の3次元位置データにおける測定点と、当該測定点に対応する他の3次元位置データにおける測定点との距離を最小化する。これにより、複数の局所的な3次元位置データを繋ぎ合わせる。
また、環境にマーカ(特徴点)を設け、当該マーカを指標として、複数の3次元位置データを繋ぎ合わせる方法もある。
特許第2813767号公報 特開平9−297019号公報 特開2001−266123号公報 特開2000−221037号公報 特開2003−317081号公報
しかしながら、上述のICPアルゴリズムを用いて、複数の3次元位置データにおいて対応する測定点間の距離を最小化することにより、広範囲の環境地図を生成する方法は、点と点とを合わせる方法である。そのため、3次元位置データとしては精度が高くても、個々の点としては精度が低い場合には、複数の3次元位置データを正確に繋ぐことができない。
また、上述のマーカを用いて、複数の3次元位置データを繋ぎ、広範囲の環境地図を生成する方法では、マーカの位置に誤差があると、正確に環境地図を生成することができない。特に、大規模な3次元位置データの繋ぎ合わせとなると、マーカの位置誤差の影響が大きくなってしまう。
本発明は、上述した知見に基づいてなされたものであって、複数の3次元位置データをより精度良く繋ぎ合わせることにより、より精細な環境地図を生成することが可能な環境地図生成装置及び環境地図生成方法を提供することを目的とする。
本発明の第1の態様は、環境地図を生成する環境地図生成装置であり、視覚センサ、3次元位置データ生成部、環境地図生成部を備える。前記視覚センサは、所定の環境内を視覚的に認識する。前記3次元位置データ生成部は、前記視覚センサによる計測結果を用いて、前記環境に存在する計測対象の位置を示す3次元位置データを生成する。前記環境地図生成部は、異なる計測位置で計測された複数の前記3次元位置データを統合することにより、前記環境に関する環境地図を生成する。さらに、前記環境地図生成部は、法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有する平面を用いて、複数の前記3次元位置データを統合する。
本発明の第1の態様にかかる環境地図生成装置では、環境地図生成部が、法線が互いに平行となっていない少なくとも3つの平面を用いて、複数の3次元位置データを統合する。そのため、従来のICPを用いて点と点とを合わせることにより複数の3次元位置データを統合する場合に比べて、複数の3次元位置データをより精度良く繋ぎ合わせることができる。特に、各平面の法線の推定において、各平面に含まれる個々の測定点のばらつきを吸収することができる。したがって、本発明の第1の態様にかかる環境地図生成装置は、より精細な環境地図を生成することができる。
また、前記環境地図生成部は、初期位置合わせ部を備えることが好ましい。前記初期位置合わせ部は、前記環境内の特徴点であるマーカを用いて、複数の前記3次元位置データ同士の位置を合わせる初期位置合わせを行う。そのため、後の工程において、平面を用いて複数の3次元位置データを統合する際の計算コストを軽減することができる。
さらに、前記環境地図生成部は、平面検出部を備えることが好ましい。前記平面検出部は、プレーンセグメントファインダと呼ばれる手法を用いて、前記3次元位置データから平面を検出する。プレーンセグメントファインダを用いることにより、複数に分割された平面であっても検出することができる。また、検出範囲を狭めることができ計算コストを低減することも可能である。
さらにまた、前記環境地図生成部は、信頼度算出部を備えることが好ましい。前記信頼度算出部は、前記平面検出部によって検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する。
前記信頼度算出部は、前記平面検出部によって検出された平面に属する前記3次元位置データの点の数に基づいて、前記スコアを算出してもよい。
また、前記信頼度算出部は、前記平面検出部によって検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出してもよい。
信頼度算出部によって、個々の平面の信頼度を表すスコアが算出される。そのため、環境地図生成部が複数の3次元位置データを統合する際に用いる平面を当該スコアに基づいて選択することが可能となる。
また、前記環境地図生成部は、剛体変換パラメータ算出部と地図統合部とを備える。前記剛体変換パラメータ算出部は、法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する。また、前記地図統合部は、前記剛体変換パラメータ算出部によって算出された剛体変換パラメータを用いて、複数の前記3次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記3次元位置データを統合する。
剛体変換パラメータ算出部は、剛体変換パラメータを算出するのに平面を利用する。そのため、剛体変換パラメータ算出部は、剛体変換パラメータをより精度良く算出することができる。これにより、地図統合部は、複数の3次元位置データをより精度良く統合することができる。したがって、環境地図生成装置は、より精細な環境地図を生成することができる。
本発明の第2の態様は、環境地図を生成する環境地図生成方法であって、3次元位置データ生成ステップと、環境地図生成ステップとを備える。前記3次元位置データ生成ステップでは、視覚センサによる計測結果を用いて、所定の環境に存在する計測対象の位置を示す3次元位置データを生成する。また、前記環境地図生成ステップでは、異なる計測位置で計測された複数の前記3次元位置データを統合することにより、前記環境に関する環境地図を生成する。さらに、前記環境地図生成ステップでは、法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有する平面を用いて、複数の前記3次元位置データを統合する。
本発明の第2の態様にかかる環境地図生成方法では、環境地図生成ステップにおいて、法線が互いに平行となっていない少なくとも3つの平面を用いて、複数の3次元位置データを統合する。そのため、従来のICPを用いて点と点とを合わせることにより複数の3次元位置データを統合する場合に比べて、複数の3次元位置データをより精度良く繋ぎ合わせることができる。特に、各平面の法線の推定において、各平面に含まれる個々の測定点のばらつきを吸収することができる。したがって、本発明の第2の態様にかかる環境地図生成方法は、より精細な環境地図を生成することができる。
また、前記環境地図生成ステップは、初期位置合わせステップを備えることが好ましい。前記初期位置合わせステップでは、前記環境内の特徴点であるマーカを用いて、複数の前記3次元位置データ同士の位置を合わせる初期位置合わせを行う。そのため、後の工程において、平面を用いて複数の3次元位置データを統合する際の計算コストを軽減することができる。
さらに、前記環境地図生成ステップは、平面検出ステップを備えることが好ましい。前記平面検出ステップでは、プレーンセグメントファインダと呼ばれる手法を用いて、前記3次元位置データから平面を検出する。プレーンセグメントファインダを用いることにより、複数に分割された平面であっても検出することができる。また、検出範囲を狭めることができ計算コストを低減することも可能である。
さらにまた、前記環境地図生成ステップは、信頼度算出ステップを備えることが好ましい。前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する。
前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面に属する前記3次元位置データの点の数に基づいて、前記スコアを算出してもよい。
また、前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出してもよい。
信頼度算出ステップにおいて、個々の平面の信頼度を表すスコアが算出される。そのため、環境地図生成ステップにおいて、複数の3次元位置データを統合する際に用いる平面を当該スコアに基づいて選択することが可能となる。
また、前記環境地図生成ステップは、剛体変換パラメータ算出ステップと地図統合ステップとを備えることが好ましい。前記剛体変換パラメータ算出ステップでは、法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する。また、地図統合ステップでは、前記剛体変換パラメータ算出ステップにおいて算出された剛体変換パラメータを用いて、複数の前記3次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記3次元位置データを統合する。
剛体変換パラメータ算出ステップでは、剛体変換パラメータを算出するのに平面を利用する。そのため、剛体変換パラメータ算出ステップでは、剛体変換パラメータをより精度良く算出することができる。これにより、地図統合ステップでは、複数の3次元位置データをより精度良く統合することができる。したがって、環境地図生成方法は、より精細な環境地図を生成することができる。
本発明により、複数の3次元位置データをより精度良く繋ぎ合わせることにより、より精細な環境地図を生成することができる。
以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。
<発明の実施の形態1>
本実施の形態にかかる環境地図生成装置100は、複数の計測位置において、環境地図生成装置100の外界の距離画像データを取得する。即ち、環境地図生成装置100は、所定の環境内の異なる範囲の距離画像データを複数取得する。以下、一つの計測位置から計測を行う範囲をフレームと称する。次いで、環境地図生成装置100は、当該距離画像データから3次元位置データを生成する。即ち、環境地図生成装置100は、各フレーム毎に3次元位置データを生成する。次いで、環境地図生成装置100は、各フレーム毎に3次元位置データから平面を検出する。そして、環境地図生成装置100は、一のフレームの平面と対応する他のフレームの平面とを利用して、当該一のフレームと他のフレームを繋ぎ合わせる。これにより、環境地図生成装置100は、複数のフレームを繋ぎ合わせ、広範囲の環境地図を生成する。
環境地図生成装置100は、例えば、様々な環境下において動作するロボットや車両などに搭載される。そして、当該ロボットや車両は、環境地図生成装置100により生成された環境地図を参照し、移動経路を計画し、当該移動経路に従って移動を行う。
図1に、環境地図生成装置100の制御系の構成を示す。図1に示すように、環境地図生成装置100は、視覚センサ11、3次元位置データ生成部12、地図生成部13(環境地図生成部)、環境地図記憶部14などを備えている。
視覚センサ11は、レーザレンジファインダ等のアクティブ距離センサを有している。そして、視覚センサ11は、環境地図生成装置100の環境の所定範囲の距離画像データを取得する。視覚センサ11が計測を行う範囲は、視覚センサ11の性能によって規定される。そして、視覚センサ11が計測を行う範囲が、各フレームの大きさを規定する。また、環境地図生成装置100が異なる複数の計測位置において計測を行うことにより、視覚センサ11は、異なる範囲の距離画像データを取得する。
なお、環境地図生成装置100は、視覚センサ11として、CCD(Charge Coupled Device)イメージセンサ又はCMOS(Complementary Metal Oxide Semiconductor)イメージセンサ等の撮像素子を備えた複数のカメラを備えてもよい。そして、視覚センサ11は、これら複数のカメラによって撮影した画像データを用いて距離画像データを生成してもよい。具体的には、視覚センサ11は、複数のカメラによって撮影した画像データから対応点を検出する。次いで、視覚センサ11は、ステレオ視によって対応点の3次元位置を復元すればよい。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。
3次元位置データ生成部12は、距離画像データの座標変換を行って3次元位置データを生成する。なお、3次元位置データとは、距離画像データに含まれる多数の計測点の位置ベクトルを3次元直交座標系で表したデータの集合である。ここで、3次元位置データに含まれる点群の座標は、環境地図生成装置100に固定された装置座標系で表示されていてもよいし、環境に固定されたグローバル座標系で表示されていてもよい。
地図生成部13は、複数の計測位置で得られた局所的な3次元位置データを統合することにより、広範囲の環境地図を生成する。即ち、複数のフレームの3次元位置データを繋ぎ合わせることにより、広範囲の環境地図を生成する。本実施の形態では、環境地図が、3次元地図であるとして説明する。
具体的には、地図生成部13は、法線が互いに平行となっていない少なくとも3つの平面であって、複数のフレームの3次元位置データにおいて対応する平面を有し、且つ、所定値以上のスコア(後述)を有する平面を用いて、複数のフレームの3次元位置データを統合する。
地図生成部13により生成された環境地図は、環境地図記憶部14に格納される。
図2を参照しながら、地図生成部13による環境地図の生成方法を説明する。図2(a)は、環境地図生成装置100が計測を行う環境の一例を示している。図2(a)の環境160は、床面161上に、直方体形状の障害物162と円柱形状の障害物163を有している。そして、環境160は、床面161に相当する平面Aと障害物162の前面に相当する平面B、障害物162の紙面向かって右側の側面に相当する平面C、障害物162の上面に相当する平面D、障害物163の上面に相当する平面Eを有している。
また、図2(a)に示すように、環境地図生成装置100は、一の計測位置Aと、計測位置Aとは異なる他の計測位置Bから、環境160の計測を行う。また、図2(a)において、計測地点A及び計測地点Bは、環境160に対して同じ側となっている。
図2(b)に、計測位置Aから計測を行った場合に得られる3次元地図を示す。また、図2(c)に、計測位置Bから計測を行った場合に得られる3次元地図を示す。図2(a)〜図2(d)において、計測地点A及び計測地点Bから奥行き方向をx軸、計測地点A及び計測地点Bの左右方向をy軸、計測地点A及び計測地点Bの上下方向をz軸とする。
図2(b)に示すように、計測位置Aから計測を行って得られる3次元地図には、床面161に相当する平面Aの一部と、障害物162に相当する平面B、平面C、平面Dとが表わされている。また、図2(c)に示すように、計測位置Bから計測を行って得られる3次元地図には、床面161に相当する平面Aの一部と、障害物162に相当する平面B及び平面Dの一部、平面Cと、障害物162に相当する平面Eとが表わされている。したがって、図2(b)に示す3次元地図と、図2(c)に示す3次元地図とは、一部重複している。
そして、地図生成部13は、重複部分において、図2(b)に示す3次元地図と図2(c)に示す3次元地図とを重ね合わせることにより、広範囲の環境地図を生成する。図2(d)に、図2(b)に示す3次元地図と図2(c)に示す3次元地図とを繋ぎ合わせることにより生成される環境地図を示す。また、図2(d)において、図2(b)の3次元地図と図2(c)の3次元地図とを繋ぎ合わせた部分における図2(b)の3次元地図の境界線を破線で示し、図2(c)の3次元地図の境界線を一点鎖線で示す。
具体的には、地図生成部13は、図2(b)に含まれる平面と、当該平面に対応する図2(c)に含まれる平面とを利用して、図2(b)の3次元地図と図2(c)の3次元地図とを繋ぎ合わせる。
以下、地図生成部13の構成及び環境地図生成方法について、詳細に説明する。
図1に示すように、地図生成部13は、初期位置合わせ部131、平面検出部132、信頼度算出部133、対応平面検出部134、剛体変換パラメータ算出部135、地図統合部136を含む。
初期位置合わせ部131は、マーカを指標として、複数のフレームの3次元位置データの位置合わせを行う。ここで、マーカとは、環境内の特徴点である。そして、初期位置合わせ部131は、一のフレーム内に含まれるマーカと、当該マーカに対応する他のフレーム内に含まれるマーカとの位置合わせを行うことにより、当該2つのフレームの初期位置合わせを行う。このようにして、初期位置合わせ部131は、複数のフレーム内の互いに対応するマーカの位置が合うように、複数のフレームの初期位置合わせを行う。
平面検出部132は、各フレーム毎に、3次元位置データから平面を検出する。また、平面検出部132は、検出した平面に平面IDを付与する。本実施の形態における平面検出には、プレーンセグメントファインダ(Plane Segment Finder(PLS))と呼ばれる手法が用いられる。プレーンセグメントファインダでは、まず、3次元の点群から2点をランダムにサンプリングする。次いで、当該2点のベクトルをクラスタリングすることにより、平面の位置姿勢を求めるアルゴリズムである。
具体的には、プレーンセグメントファインダでは、まず、3次元位置データに対してランダマイズド3次元ハフ変換を行って、最大平面の傾きを検出する。次に、再び、ランダマイズド3次元ハフ変換を用いて、当該最大平面と原点との距離を検出する。そして、当該最大平面に属する点を抽出することにより、平面領域(プレーンセグメント)を検出する。
より具体的には、プレーンセグメントファインダにおいて、平面は、以下の数式(1)により表わされる。
ρ=(xcos(θ)+ysin(θ))cos(φ)+zsin(φ)
・・・・・・(1)
数式(1)において、ρは平面から原点へ下ろした垂線の長さ、θは当該垂線をxy平面に投影した線とx軸とのなす角、φは当該垂線とxy平面とのなす角である。
そして、3次元位置データ内の各測定点(x,y,z)を(ρ,θ,φ)空間に投影し、ピークを検出することにより、平面を検出する。プレーンセグメントファインダでは、複数に分割された平面であっても検出することができる。また、3次元位置データの所定部分の測定点のみを(ρ,θ,φ)空間の部分空間に投影して、検出範囲を狭めることができる。これにより、計算コストを大幅に低減することも可能である。
なお、多数の計測点(3次元位置データ)から平面の方程式を表す平面パラメータ(例えば、法線ベクトル及び法線ベクトルの座標系原点からの距離)を検出するために、ハフ変換法やランダムサンプリング法が従来から用いられている。本実施の形態における平面検出に、従来から知られているこれらの手法を適用してもよい。
信頼度算出部133は、平面検出部132によって検出された各平面の信頼度を評価し、信頼度を表すスコアを算出し、各平面に当該スコアを付与する。
平面の面積が大きいほど角度精度が上がるため、各平面の信頼度は高くなる。そこで、平面のスコアは、検出された平面に属するデータの点数に基づいて評価する。
また、計測方向に対して垂直の角度にある平面は計測精度が高くなる。そこで、平面のスコアは、視覚センサ11の位置と平面位置(平面データの重心)とを結ぶ直線と、当該平面の法線との内積に基づいて評価する。
対応平面検出部134は、ICP(Interactive Closest Point)アルゴリズムを用いて、一のフレームに含まれる平面と対応する他のフレームに含まれる平面を検出する。
図3を用いて、対応平面検出部134による処理を説明する。図3に示すように、対応平面検出部134は、一のフレーム内の平面Aに含まれる点(図3において白抜きの丸で示す)から最も近傍にある他のフレーム内の点(図3においてハッチングが付された丸で示す)に着目する。そして、平面検出部133は、当該他のフレーム内の点が属する平面の平面IDを検出する。そして、対応平面検出部134は、一のフレーム内の平面Aに属する全ての点から最も近傍にある他のフレーム内の点が属する平面の平面IDとして最も多く検出された平面IDを有する平面(図3の例では平面A')を平面Aの対応する平面として検出する。同様に、対応平面検出部134は、一のフレーム内の平面Bに属する全ての点から最も近傍にある他のフレーム内の点が属する平面IDとして最も多く検出された平面IDを有する平面(図3の例では平面B')を平面Bの対応する平面として検出する。
剛体変換パラメータ算出部135は、対応平面検出部134によって検出された対応する平面を用いて剛体変換パラメータを求める。
具体的には、剛体変換パラメータ算出部135は、対応平面検出部134によって他のフレーム内に対応する平面が検出された平面であり、信頼度算出部133によって算出されたスコアが高い平面を少なくとも3つ選択する。また、選択された各平面の法線は互いに平行となっていない。
そして、剛体変換パラメータ算出部135は、一のフレームから選択した少なくとも3つの平面と、他のフレーム内の当該平面と対応する少なくとも3つの平面とをフィッティングする。これにより、剛体変換パラメータを算出する。
ここで、剛体変換パラメータとは、剛体変換式において用いられるパラメータである。剛体変換式は、以下の数式(2)により表わされる。
t−1=R×D+T ・・・・・・(2)
数式(2)において、Rは回転を表わすパラメータであり、Tは並進を表わすパラメータである。説明の簡単のため、剛体変換パラメータ算出部135が図2(b)及び図2(c)において、平面A、平面B、平面Cを選択したとする。また、図4に示すように、図2(b)、図2(c)の何れか一方の平面A、平面B、平面Cがそれぞれ、座標系のxy平面上、xz平面上、zy平面上にあるとする。また、x軸を回動軸とする回動をroll、y軸を回動軸とする回動をpitch、z軸を回動軸とする回動をyawと表わす。
このとき、Rは回転を表わすパラメータであるため、roll、pitch、yawで表わされる。また、Tは並進を表わすパラメータであるため、x、y、zで表わされる。
そして、図2(b)の平面Aと図2(c)の平面Aとをフィッティングすることにより、z、roll、pitchを一意に求めることができる。また、図2(b)の平面Bと図2(c)の平面Bとをフィッティングすることにより、yaw、yを一意に求めることができる。また、図2(b)の平面Cと図2(c)の平面Cとをフィッティングすることにより、xを一意に求めることができる。
したがって、剛体変換パラメータ算出部135は、法線が互いに平行となっていない少なくとも3組の平面(図2(b)の平面Aと図2(c)の平面A、図2(b)の平面Bと図2(c)の平面B、図2(b)の平面Cと図2(c)の平面C)を用いて剛体変換パラメータを算出することができる。
また、剛体変換パラメータ算出部135は、3フレーム以上の3次元位置データにおいて互いに対応する平面を用いて剛体変換パラメータR及びTを算出する場合には、全ての平面において、各平面を表す平面式(例えば、数式(1)の平面式)の一致誤差が最小となるような剛体変換パラメータを算出する。
地図統合部136は、異なる計測位置で得られた複数の3次元位置データを統合することにより、広範囲の環境地図を生成する。即ち、地図統合部136は、複数のフレームの3次元位置データを繋ぎ合わせて、広範囲の環境地図を生成する。例えば、地図統合部136は、図2(b)に示す3次元地図と図2(c)に示す3次元地図とを重ね合わせることにより、広範囲の環境地図(図2(d))を生成する。
具体的には、地図統合部136は、剛体変換パラメータ算出部135によって算出された剛体変換パラメータR及びTと数式(2)を用いて、各フレームの座標系を変換して統一する。これにより、地図統合部136は、複数のフレームを繋ぎ合わせて広範囲の環境地図を生成する。
続いて以下では、環境地図生成装置100の環境地図生成方法について図5を参照して詳細に説明する。図5は、環境地図生成装置100による環境地図の生成手順の具体例を示すフローチャートである。
まず、環境地図生成装置100は、複数の計測地点において計測を行って、複数フレームの距離画像データを取得する。次いで、3次元位置データ生成部12は、複数フレームの距離画像データの座標変換を行って、複数フレームの3次元位置データを生成する(ステップS1;3次元位置データ生成ステップ)。
次に、初期位置合わせ部131は、マーカを指標として、複数のフレームの3次元位置データの位置合わせを行う(ステップS2;初期位置合わせステップ)。
次に、平面検出部132は、各フレーム毎に、3次元位置データから平面を検出する(ステップS3;平面検出ステップ)。また、平面検出部132は、検出した平面に平面IDを付与する。
次に、信頼度算出部133は、平面検出部132によって検出された各平面の信頼度を評価し、スコアを算出し、各平面に当該スコアを付与する(ステップS4;信頼度算出ステップ)。
次に、対応平面検出部134は、ICPアルゴリズムを用いて、一のフレームに含まれる平面と対応する他のフレームに含まれる平面を検出する(ステップS5)。
次に、剛体変換パラメータ算出部135は、一のフレームから少なくとも3つの平面を選択する。剛体変換パラメータ算出部135によって選択される平面は、他のフレーム内に対応する平面が検出されており、スコアが高いものである。また、剛体変換パラメータ算出部135によって選択される各平面の法線は互いに平行となっていない。そして、剛体変換パラメータ算出部135は、一のフレームから選択した少なくとも3つの平面と、他のフレーム内の当該平面と対応する少なくとも3つの平面とをフィッティングする。これにより、剛体変換パラメータを算出する(ステップS6;剛体変換パラメータ算出ステップ)。
次に、地図統合部136は、異なる計測位置で得られた複数フレームの3次元位置データを繋ぎ合わせて広範囲の環境地図を生成する(ステップS7;地図統合ステップ)。具体的には、地図統合部136は、剛体変換パラメータ算出部135によって算出された剛体変換パラメータR及びTと数式(2)を用いて、各フレームの座標系を変換して統一する。これにより、地図統合部136は、複数のフレームを繋ぎ合わせて広範囲の環境地図を生成する。
このようにして、本実施の形態にかかる環境地図生成装置100によって生成された環境地図の一例を図6、図7に示す。
図6、図7は、東大安田講堂周辺の環境地図である。18箇所の計測位置において、合計約80回、計測を行った。また、視覚センサ11の解像度は、0.1°であり、計測範囲は、上下80°、左右360°である。
図6は、東大安田講堂周辺を上側から表す環境地図である。具体的には、図6に示す環境地図は、xy平面上で表され、それぞれの点がz軸方向(高さ方向)の高さについてのデータを保持している。
図7は、安田講堂周辺を表す3次元の環境地図であり、いわゆる俯瞰図である。
以上に説明したように、本実施の形態にかかる環境地図生成装置100においては、地図生成部13が、法線が互いに平行となっていない少なくとも3つの平面を用いて、複数の3次元位置データを統合する。そのため、従来のICPを用いて点と点とを合わせることにより複数の3次元位置データを統合する場合に比べて、複数の3次元位置データをより精度良く繋ぎ合わせることができる。特に、各平面の法線の推定において、各平面に含まれる個々の測定点のばらつきを吸収することができる。したがって、環境地図生成装置100は、より精細な環境地図を生成することができる。
また、初期位置合わせ部131は、環境内の特徴点であるマーカを用いて、複数の3次元位置データ同士の位置を合わせる初期位置合わせを行う。そのため、後の工程において、平面を用いて複数の3次元位置データを統合する際の計算コストを軽減することができる。
さらに、平面検出部132は、プレーンセグメントファインダと呼ばれる手法を用いて、3次元位置データから平面を検出する。プレーンセグメントファインダを用いることにより、複数に分割された平面であっても検出することができる。また、検出範囲を狭めることができ計算コストを低減することも可能である。
さらにまた、信頼度算出部133によって、個々の平面の信頼度を表すスコアが算出される。そのため、環境地図生成部13が複数の3次元位置データを統合する際に用いる平面を当該スコアに基づいて選択することが可能となる。
また、剛体変換パラメータ算出部135は、剛体変換パラメータを算出するのに平面を利用する。そのため、剛体変換パラメータ算出部135は、剛体変換パラメータをより精度良く算出することができる。これにより、地図統合部136は、複数の3次元位置データをより精度良く統合することができる。したがって、環境地図生成装置100は、より精細な環境地図を生成することができる。
なお、環境地図生成装置100は、脚歩行型の移動ロボットに適用してもよい。また、環境地図生成装置100は、車輪走行型などの脚歩行型以外の移動ロボットに適用されてもよい。
また、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
発明の実施の形態1にかかる環境地図生成装置の制御系を示すブロック図である。 発明の実施の形態1にかかる環境地図生成装置が計測する環境の一例及び環境地図の一例を示す図である。 発明の実施の形態1にかかる対応平面検出部による処理を説明する図である。 発明の実施の形態1にかかる剛体変換パラメータ算出部による処理を説明する図である。 発明の実施の形態1にかかる環境地図生成装置の環境地図生成方法を示すフローチャートである。 発明の実施の形態1にかかる環境地図生成装置によって生成される環境地図の一例を示す図である。 発明の実施の形態1にかかる環境地図生成装置によって生成される環境地図の一例を示す図である。
符号の説明
100 環境地図生成装置
11 視覚センサ
12 3次元位置データ生成部
13 地図生成部(環境地図生成部)
131 初期位置合わせ部
132 平面検出部
133 信頼度算出部
134 対応平面検出部
135 剛体変換パラメータ算出部
136 地図統合部

Claims (14)

  1. 環境地図を生成する環境地図生成装置であって、
    所定の環境内を視覚的に認識する視覚センサと、
    前記視覚センサによる計測結果を用いて、前記環境に存在する計測対象の位置を示す3次元位置データを生成する3次元位置データ生成部と、
    異なる計測位置で計測された複数の前記3次元位置データを統合することにより、前記環境に関する環境地図を生成する環境地図生成部とを備え、
    前記環境地図生成部は、法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有する平面を用いて、複数の前記3次元位置データを統合する環境地図生成装置。
  2. 前記環境地図生成部は、
    前記環境内の特徴点であるマーカを用いて、複数の前記3次元位置データ同士の位置を合わせる初期位置合わせ部を備える請求項1に記載の環境地図生成装置。
  3. 前記環境地図生成部は、
    プレーンセグメントファインダと呼ばれる手法を用いて、前記3次元位置データから平面を検出する平面検出部を備える請求項1又は2に記載の環境地図生成装置。
  4. 前記環境地図生成部は、
    前記平面検出部によって検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する信頼度算出部を備える請求項3に記載の環境地図生成装置。
  5. 前記信頼度算出部は、前記平面検出部によって検出された平面に属する前記3次元位置データの点の数に基づいて、前記スコアを算出する請求項4に記載の環境地図生成装置。
  6. 前記信頼度算出部は、前記平面検出部によって検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出する請求項4又は5に記載の環境地図生成装置。
  7. 前記環境地図生成部は、
    法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する剛体変換パラメータ算出部と、
    前記剛体変換パラメータ算出部によって算出された剛体変換パラメータを用いて、複数の前記3次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記3次元位置データを統合する地図統合部とを備える請求項4乃至6の何れか一項に記載の環境地図生成装置。
  8. 環境地図を生成する環境地図生成方法であって、
    視覚センサによる計測結果を用いて、所定の環境内に存在する計測対象の位置を示す3次元位置データを生成する3次元位置データ生成ステップと、
    異なる計測位置で計測された複数の前記3次元位置データを統合することにより、前記環境に関する環境地図を生成する環境地図生成ステップとを備え、
    前記環境地図生成ステップでは、法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有する平面を用いて、複数の前記3次元位置データを統合する環境地図生成方法。
  9. 前記環境地図生成ステップは、
    前記環境内の特徴点であるマーカを用いて、複数の前記3次元位置データ同士の位置を合わせる初期位置合わせステップを備える請求項8に記載の環境地図生成方法。
  10. 前記環境地図生成ステップは、
    プレーンセグメントファインダと呼ばれる手法を用いて、前記3次元位置データから平面を検出する平面検出ステップを備える請求項8又は9に記載の環境地図生成方法。
  11. 前記環境地図生成ステップは、
    前記平面検出ステップにおいて検出された平面の信頼度を評価し、当該信頼度を表すスコアを算出する信頼度算出ステップを備える請求項10に記載の環境地図生成方法。
  12. 前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面に属する前記3次元位置データの点の数に基づいて、前記スコアを算出する請求項11に記載の環境地図生成方法。
  13. 前記信頼度算出ステップでは、前記平面検出ステップにおいて検出された平面の重心位置と前記視覚センサとを結ぶ直線と、当該平面の法線との内積に基づいて、前記スコアを算出する請求項11又は12に記載の環境地図生成方法。
  14. 前記環境地図生成ステップは、
    法線が互いに平行となっていない少なくとも3つの平面であって、複数の前記3次元位置データにおいて対応する平面を有し、且つ、所定値以上の前記スコアを有する平面を用いて、剛体変換パラメータを算出する剛体変換パラメータ算出ステップと、
    前記剛体変換パラメータ算出ステップにおいて算出された剛体変換パラメータを用いて、複数の前記3次元位置データの座標系に剛体変換処理を行うことにより、当該複数の前記3次元位置データを統合する地図統合ステップとを備える請求項11乃至13の何れか一項に記載の環境地図生成方法。
JP2008233708A 2008-09-11 2008-09-11 環境地図生成装置及び環境地図生成方法 Pending JP2010066595A (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2008233708A JP2010066595A (ja) 2008-09-11 2008-09-11 環境地図生成装置及び環境地図生成方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2008233708A JP2010066595A (ja) 2008-09-11 2008-09-11 環境地図生成装置及び環境地図生成方法

Publications (1)

Publication Number Publication Date
JP2010066595A true JP2010066595A (ja) 2010-03-25

Family

ID=42192225

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2008233708A Pending JP2010066595A (ja) 2008-09-11 2008-09-11 環境地図生成装置及び環境地図生成方法

Country Status (1)

Country Link
JP (1) JP2010066595A (ja)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012063866A (ja) * 2010-09-14 2012-03-29 Topcon Corp 点群位置データ処理装置、点群位置データ処理方法、点群位置データ処理システム、および点群位置データ処理プログラム
JP2015515655A (ja) * 2012-06-29 2015-05-28 三菱電機株式会社 データを位置合わせする方法
JP2015164073A (ja) * 2015-05-13 2015-09-10 東芝プラントシステム株式会社 3次元cadデータ作成システムおよび3次元cadデータ作成方法
JP2017166933A (ja) * 2016-03-15 2017-09-21 富士通株式会社 情報処理装置及び情報合成プログラム
WO2019044500A1 (ja) * 2017-09-04 2019-03-07 日本電産株式会社 位置推定システム、および当該位置推定システムを備える移動体
WO2020079814A1 (ja) * 2018-10-18 2020-04-23 富士通株式会社 算出方法、算出プログラムおよび情報処理装置
US11856176B2 (en) 2021-06-01 2023-12-26 Evident Corporation Three-dimensional image display method, three-dimensional image display device, and recording medium

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012063866A (ja) * 2010-09-14 2012-03-29 Topcon Corp 点群位置データ処理装置、点群位置データ処理方法、点群位置データ処理システム、および点群位置データ処理プログラム
JP2015515655A (ja) * 2012-06-29 2015-05-28 三菱電機株式会社 データを位置合わせする方法
JP2015164073A (ja) * 2015-05-13 2015-09-10 東芝プラントシステム株式会社 3次元cadデータ作成システムおよび3次元cadデータ作成方法
JP2017166933A (ja) * 2016-03-15 2017-09-21 富士通株式会社 情報処理装置及び情報合成プログラム
WO2019044500A1 (ja) * 2017-09-04 2019-03-07 日本電産株式会社 位置推定システム、および当該位置推定システムを備える移動体
WO2020079814A1 (ja) * 2018-10-18 2020-04-23 富士通株式会社 算出方法、算出プログラムおよび情報処理装置
JPWO2020079814A1 (ja) * 2018-10-18 2021-09-02 富士通株式会社 算出方法、算出プログラムおよび情報処理装置
US11468580B2 (en) 2018-10-18 2022-10-11 Fujitsu Limited Calculation method, computer-readable recording medium recording calculation program, and information processing apparatus
US11856176B2 (en) 2021-06-01 2023-12-26 Evident Corporation Three-dimensional image display method, three-dimensional image display device, and recording medium

Similar Documents

Publication Publication Date Title
CN112785702B (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
Zhang et al. Intelligent collaborative localization among air-ground robots for industrial environment perception
US10509983B2 (en) Operating device, operating system, operating method, and program therefor
US9171225B2 (en) Device, method, and recording medium for detecting and removing mistracked points in visual odometry systems
JP5992184B2 (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
US20160117824A1 (en) Posture estimation method and robot
JP2009193240A (ja) 移動ロボット及び環境地図の生成方法
JP2006252473A (ja) 障害物検出装置、キャリブレーション装置、キャリブレーション方法およびキャリブレーションプログラム
JP2010066595A (ja) 環境地図生成装置及び環境地図生成方法
JP6584208B2 (ja) 情報処理装置、情報処理方法、プログラム
US11783507B2 (en) Camera calibration apparatus and operating method
CN105324792B (zh) 用于估计移动元件相对于参考方向的角偏差的方法
US20120114174A1 (en) Voxel map generator and method thereof
Lagisetty et al. Object detection and obstacle avoidance for mobile robot using stereo camera
EP3324359B1 (en) Image processing device and image processing method
JP2011008320A (ja) 自律移動体、自己位置推定装置、およびプログラム
JP4132068B2 (ja) 画像処理装置及び三次元計測装置並びに画像処理装置用プログラム
CN112700486B (zh) 对图像中路面车道线的深度进行估计的方法及装置
CN112068152A (zh) 使用3d扫描仪同时进行2d定位和2d地图创建的方法和系统
Hertzberg et al. Experiences in building a visual SLAM system from open source components
Núnez et al. Data Fusion Calibration for a 3D Laser Range Finder and a Camera using Inertial Data.
JP6410231B2 (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
JP6543935B2 (ja) 視差値導出装置、機器制御システム、移動体、ロボット、視差値導出方法、およびプログラム
Gallegos et al. Appearance-based slam relying on a hybrid laser/omnidirectional sensor
JP4935769B2 (ja) 平面領域推定装置及びプログラム