JP2010044740A - ３次元格子地図作成方法及びこれを用いた自動走行装置の制御方法 - Google Patents

Abstract

【課題】３次元格子地図作成方法及びこれを用いた自動走行装置の制御方法を提供する。
【解決手段】無人車両や移動ロボットの現在位置及び周辺地形を把握するための３次元地図作成において、ステレオ全方位カメラを用いて周辺の全方位映像を獲得し、前記全方位映像を用いて現在の２次元位置を確認し、前記全方位映像の３次元復元を行い、前記２次元位置確認結果と前記３次元復元結果とを結合し、前記ステレオ全方位カメラ周辺の３次元格子地図作成を可能にする。
【選択図】図６

Description

本発明は、３次元格子地図作成方法に係り、特に、ステレオ全方位カメラ（Ｓｔｅｒｅｏ Ｏｍｎｉ−ｄｉｒｅｃｔｉｏｎａｌ Ｃａｍｅｒａ）を用いて周辺の地形を把握し、３次元地図を作成する３次元格子地図作成方法に関する。

全方位カメラは、回転反射鏡、集光レンズ及び撮像素子を用いて全方位を一度に撮影するカメラシステムで、保安施設、監視カメラ、ロボットビジョンなどに応用される。回転反射鏡の形状には双曲面、球面、円錐、複合型などの様々なものがあり、撮像素子としてはＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）またはＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）が使われる。この撮像素子の撮像面に投影される画像（すなわち、全方位映像）は、回転反射鏡から反射されたもので、人間がそのままでは正確に観察できない歪んだ画像となっている。したがって、画像の正確な観察のために、撮像素子の出力に対して外部のマイクロプロセッサーなどを用いてその座標を変換し、新しいパノラマ映像を生成する。

この全方位カメラを用いて得た全方位映像は、全方位カメラ周辺に対する２次元的な情報を提供する。もし複数の全方位カメラを用いて相違なる方向から撮影した複数の全方位映像を利用すると、全方位カメラ周辺に対する３次元的な情報が得られる。複数の全方位カメラで構成される映像装置をステレオ全方位カメラという。このようなステレオ全方位カメラを無人車両や移動ロボットに装着し、該ステレオ全方位カメラで撮影された全方位映像を無人車両または移動ロボットの位置認識及び地図作成に利用してきている。

本発明は、無人車両や移動ロボットの現在位置及び周辺地形を把握するための３次元位置認識及び地図作成において、短時間で正確な３次元位置認識及び地図作成を可能にすることにその目的がある。

本発明の一側面によれば、ステレオ全方位カメラを用いて周辺の全方位映像を獲得し、前記全方位映像を用いて現在の２次元位置を確認し、前記全方位映像の３次元復元を行い、前記２次元位置確認結果と前記３次元復元結果とを結合し、前記ステレオ全方位カメラ周辺の３次元格子地図を作成する３次元格子地図作成方法が提供される。

なお、前記２次元位置確認は、前記全方位映像を用いた２次元位置確認及び地図作成によって行うことができる。

なお、前記２次元位置確認は、前記全方位映像のエピポーラ線上の特徴点を用いて行うことができる。

なお、カラーエッジ（ｃｏｌｏｒ ｅｄｇｅ）抽出によって前記特徴点を獲得することができる。

なお、前記３次元復元結果を用いて格子の占有度合を求めることができる。

なお、前記３次元格子のボクセル（Ｖｏｘｅｌ）の占有カウントが一定数を超過すると、該当のボクセルが占有されたと判断することができる。

本発明の他の側面によれば、ステレオ全方位カメラを用いて周辺の全方位映像を獲得し、前記全方位映像を用いて現在の２次元位置を確認し、前記全方位映像の３次元復元を行い、前記２次元位置確認結果と前記３次元復元結果とを結合し、前記ステレオ全方位カメラ周辺の３次元格子地図を作成し、前記３次元格子地図に基づいて現在位置から目標地点までの走行経路を決定し、前記走行経路に沿って前記目標地点まで走行するように制御する、自動走行装置の制御方法が提供される。

なお、前記２次元位置確認は、前記全方位映像を用いた２次元位置確認及び地図作成によって行うことができる。

なお、前記２次元位置確認は、前記全方位映像のエピポーラ線上の特徴点を用いて行うことができる。

なお、カラーエッジ抽出によって前記特徴点を獲得することができる。

なお、前記３次元復元結果を用いて格子の占有度合を求めることができる。

なお、前記３次元格子のボクセルの占有カウントが一定数を超過すると、該当のボクセルが占有されたと判断することができる。

本発明によれば、無人車両や移動ロボットの現在位置及び周辺地形を把握するための３次元位置認識及び地図作成において、短時間で正確な３次元位置認識及び地図作成が可能になる。

本発明の一実施例によるステレオ全方位カメラの構成を示す図である。 図１に示すステレオ全方位カメラの制御系統を示す図である。 図１に示すステレオ全方位カメラで撮影した映像を示す図である。 図３に示す二つの全方位映像のパノラマ映像及びステレオ整合映像（Ｄｉｓｐａｒｉｔｙ Ｍａｐ）を示す図である。 本発明の一実施例によるステレオ全方位カメラの３次元座標抽出概念を示す図である。 本発明の一実施例による位置確認（Ｌｏｃａｌｉｚａｔｉｏｎ）及び地図作成（Ｍａｐｐｉｎｇ）方法を示すフローチャートである。

以下、本発明に係る３次元格子地図作成方法及び３次元格子地図を用いた自動走行装置の制御方法の好適な実施例を、図１乃至図６を参照して説明する。本発明の一実施例による３次元格子地図作成方法は、無人車両や移動ロボット等において現在位置及び周辺地形の把握による移動経路の決定などに適用可能である。また、保安装置や監視装置のような分野にも適用可能である。

まず、図１は、本発明の一実施例によるステレオ全方位カメラの構成を示す図である。図１に示すように、ステレオ全方位カメラ１００は、互いに同じ方向に向かうように一列に配列された第１全方位カメラ１０２と第２全方位カメラ１０４とから構成される。同図には示さなかったが、第１全方位カメラ１０２と第２全方位カメラ１０４は別途の固定装置によって固定させることが好ましい。

第１全方位カメラ１０２には、カメラモジュール１０２ａと回転反射鏡１０２ｂが相対向して設置される。回転反射鏡１０２ｂは、第１全方位カメラ１０２周辺の３６０゜角度から入射する光を反射してカメラモジュール１０２ａに伝達させる。カメラモジュール１０２ａではＣＣＤ １０２ｃが撮像面を形成し、回転反射鏡１０２ｂから反射されて入射する光を電気信号に変換し、映像信号を生成する。

第２全方位カメラ１０４も同様に、カメラモジュール１０４ａと回転反射鏡１０４ｂとで構成される。第２全方位カメラ１０４のカメラモジュール１０４ａにおいてもＣＣＤ １０４ｃが撮像面を形成し、回転反射鏡１０４ｂから反射されて入射する光を電気エネルギーに変換し、映像信号を生成する。

第１全方位カメラ１０２と第２全方位カメラ１０４のそれぞれは、周辺の３６０゜画角の映像（すなわち、全方位映像）を撮影して映像を生成する。ただし、一つの全方位映像だけでは被写体の２次元情報しかわからず、異なる位置で撮影した二つの全方位映像を用いて被写体の３次元情報を得る。

図２は、図１に示すステレオ全方位カメラの制御系統を示す図である。図２に示すように、制御部２０２の入力側には、ステレオ全方位カメラ１００の第１全方位カメラ１０２と第２全方位カメラ１０４が通信可能に電気的に連結される。制御部２０２は、ステレオ全方位カメラ１００の第１全方位カメラ１０２と第２全方位カメラ１０４のそれぞれで生成された電気信号を映像信号に変換し、この映像信号によって生成される映像からステレオ全方位カメラ１００周辺の３次元地図を生成する。生成された３次元地図はメモリー２０４に保存され、該３次元地図は、ステレオ全方位カメラ１００の位置が変更される度に更新される。もし、ステレオ全方位カメラ１００が無人車両や移動ロボットなどに装着される場合、この３次元地図から無人車両または移動ロボットの現在位置及び周辺の地形を把握して位置制御またはモーション制御を行う。

図３は、図１に示すステレオ全方位カメラで撮影した映像を示す図である。図３に示すように、ステレオ全方位カメラ１００で周辺を撮影すると、第１全方位カメラ１０２によって第１全方位映像３０２が得られ、第２全方位カメラ１０４によって第２全方位映像３０４が得られる。本発明の一実施例では、第１全方位カメラ１０２により得られる第１全方位映像３０２のエピポーラ線（Ｅｐｉｐｏｌａｒ Ｌｉｎｅ）３０６を求め、このエピポーラ線３０６上の特徴点３０８を抽出し、これらの特徴点３０８の２次元座標を求める。これらの特徴点３０８の２次元座標は、３次元格子地図を生成するのに用いられる。本発明の一実施例では、これらの特徴点３０８の抽出にカラーエッジ（Ｃｏｌｏｒ Ｅｄｇｅ）抽出方式が利用される。

図４は、図３に示す二つの全方位映像のパノラマ映像及びステレオ整合映像（Ｄｉｓｐａｒｉｔｙ Ｍａｐ）である。同図で、４０２は、第１全方位映像３０２のパノラマ映像、すなわち第１パノラマ映像であり、４０４は、第２全方位映像３０４のパノラマ映像、すなわち第２パノラマ映像であり、４０６は、第１パノラマ映像４０２と第２パノラマ映像４０４とのステレオ整合映像（Ｄｉｓｐａｒｉｔｙ Ｍａｐ）である。図４に示すステレオ整合映像４０６では、ステレオ全方位カメラ１００の近くに位置する被写体は相対的に明るく現れ、ステレオ全方位カメラ１００の遠くに位置する被写体は相対的に暗く現れる。この明るさ情報（すなわち、映像の深さ（ｄｅｐｔｈ））を用いてステレオ全方位カメラ１００と被写体間の距離を計ることができる。

図５は、本発明の一実施例によるステレオ全方位カメラの３次元座標抽出概念を示す図である。同図で、ｂは第１全方位カメラ１０２と第２全方位カメラ１０４間の距離であるベースライン（Ｂａｓｅ Ｌｉｎｅ）を表し、ｒは回転反射鏡１０２ｂ，１０４ｂの半径を表し、ｐとｐ’は、各全方位映像における特徴点を表し、ｄ’とｄは、第１全方位カメラ１０２と第２全方位カメラ１０４のそれぞれから被写体Ｐまでの距離を表す。１０２ｃと１０４ｃは、図１で既に説明したように、第１全方位カメラ１０２及び第２全方位カメラ１０４のそれぞれの撮像面を表す。

第２全方位カメラ１０４と被写体Ｐ間の距離ｄは、下記の式（１）によって得られる。これと同様にして第１全方位カメラ１０２と被写体間の距離ｄ’も得ることができる。

このような方法でステレオ全方位カメラ１００と被写体Ｐ間の距離ｄを求め、図４のパノラマ映像からステレオ全方位カメラ１００を中心とする被写体Ｐの方位角（ａｚｉｍｕｔｈ）が求められると、下記の式（２）で示される被写体Ｐの３次元座標Ｐ（Ｘ，Ｙ，Ｚ）が得られる。

図６は、本発明の一実施例による位置確認（Ｌｏｃａｌｉｚａｔｉｏｎ）及び地図作成（Ｍａｐｐｉｎｇ）方法を示すフローチャートである。位置確認（Ｌｏｃａｌｉｚａｔｉｏｎ）及び地図作成（Ｍａｐｐｉｎｇ）を同時に行うことを“ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）”と呼ぶ。本発明の一実施例によるＳＬＡＭは、２次元ＳＬＡＭ（６００ａ）と３次元ＳＬＡＭ（６００ｂ）を共に実施する。３次元ＳＬＡＭは、演算量が非常に多いから、２次元ＳＬＡＭと３次元ＳＬＡＭを混用することによって総演算量を減らし、これによって位置確認及び地図作成の速度を高めることができる。すなわち、２次元ＳＬＡＭだけでも必要なデータが得られる場合には２次元ＳＬＡＭのみを行い、３次元ＳＬＡＭが必要な場合に限って３次元ＳＬＡＭを行うと、全てのデータを３次元ＳＬＡＭによって得る場合に比べて演算量が大幅に減少する。位置確認のための演算量が大きく減少するということは、より短時間で必要な位置確認が可能になるということを意味し、これは、ステレオ全方位カメラ１００を装着した自動走行装置（無人車両または移動ロボットなどの装置）がより高速で移動できるということを意味する。

まず、２次元ＳＬＡＭ（６００ａ）で、ステレオ全方位カメラ１００の第１全方位カメラ１０２と第２全方位カメラ１０４のそれぞれを用いて第１全方位映像３０２と第２全方位映像３０４を獲得する（ステップ６０２）。そのうち、下方に位置した第１全方位カメラ１０２を用いて獲得した第１全方位映像３０２のエピポーラ線３０６を獲得し、このエピポーラ線３０６における特徴点３０８を抽出する（ステップ６０４）。２次元ＳＬＡＭではこれら特徴点３０８の２次元座標を獲得するが、このために３次元ＳＬＡＭ（６００ｂ）の３次元位置認識結果を利用する。

３次元ＳＬＡＭ（６００ｂ）において、第１パノラマ映像４０２と第２パノラマ映像４０４とのステレオ整合によってステレオ整合映像（Ｄｉｓｐａｒｉｔｙ Ｍａｐ）４０６を生成する（ステップ６０６）。このステレオ整合映像４０６を通じて、図５で説明した方法を用いてステレオ全方位カメラ１００の周辺映像を３次元復元する（ステップ６０８）。

再び２次元ＳＬＡＭ（６００ａ）を参照すると、ステレオ全方位映像に下記の式（３）、（４）、（５）を適用することによって、抽出された特徴点３０８の２次元座標を獲得する（ステップ６１０）。

上記の式（３）は、２次元ＳＬＡＭのためのもので、（Ｘ_k）は状態ベクトルを表し、Ｕ_kは入力ベクトルを表し、Ｑ_vは入力誤差を表し、Ｚは測定値であり、Ｑ_sは測定誤差である。式（３）の状態ベクトル（Ｘ_k）は、式（４）に示すようなステレオ全方位カメラ１００の位置及び特徴点３０８の２次元座標で構成され、式（５）の測定値はステレオ全方位カメラ１００で測定した特徴点の角度（方位）である。このような式（３）、（４）、（５）を利用すると、エピポーラ線３０６の特徴点３０８の位置とステレオ全方位カメラ１００の位置推定が可能となる。

もし、本発明の一実施例によるステレオ全方位カメラ１００が無人車両や移動ロボットなどに装着されて運用される場合、ステレオ全方位カメラ１００の移動が発生することとなる。したがって、ステレオ全方位カメラ１００の動きを予測し、以前に測定された２次元座標値を新しく補正して更新する（ステップ６１２）。

２次元ＳＬＡＭ（６００ａ）での２次元座標獲得結果と３次元ＳＬＡＭ（６００ｂ）での３次元位置認識結果とを総合することで、ステレオ全方位カメラ１００周辺の３次元格子地図を生成することができる（ステップ６１４）。もし、各ボクセル（Ｖｏｘｅｌ）の占有カウントが一定水準以上であれば、そのボクセルは占有されていると判断され、３次元格子マップの構築が可能である。ステレオ全方位カメラ１００の視野（Ｆｉｅｌｄ ｏｆ Ｖｉｅｗ）内にボクセルが存在する場合（占有または未占有の確認ができる場合）、一定回数で一定占有カウントを超過すると、そのボクセルは占有されたと判断する。

１００ ステレオ全方位カメラ
１０２，１０４ 第１及び第２全方位カメラ
１０２ａ，１０４ａ カメラモジュール
１０２ｂ，１０４ｂ 回転反射鏡
１０２ｃ，１０４ｃ 撮像面（ＣＣＤ）
２０２ 制御部
２０４ メモリー
３０２，３０４ 第１及び第２全方位映像
３０６ エピポーラ線
３０８ 特徴点
４０２，４０４ 第１及び第２パノラマ映像
４０６ ステレオ整合映像

Claims (14)

1. ステレオ全方位カメラを用いて周辺の全方位映像を獲得し、
   前記全方位映像を用いて現在の２次元位置を確認し、
   前記全方位映像の３次元復元を行い、
   前記２次元位置確認結果と前記３次元復元結果とを結合し、前記ステレオ全方位カメラ周辺の３次元格子地図を作成する、３次元格子地図作成方法。
2. 前記２次元位置確認は、前記全方位映像を用いた２次元位置確認及び地図作成によって行われる、請求項１に記載の３次元格子地図作成方法。
3. 前記２次元位置確認は、前記全方位映像のエピポーラ線上の特徴点を用いて行われる、請求項２に記載の３次元格子地図作成方法。
4. カラーエッジ（ｃｏｌｏｒ ｅｄｇｅ）抽出によって前記特徴点を獲得する、請求項３に記載の３次元格子地図作成方法。
5. 前記３次元復元結果を用いて格子の占有度合を求める、請求項１に記載の３次元格子地図作成方法。
6. 前記３次元格子のボクセル（Ｖｏｘｅｌ）の占有カウントが一定数を超過すると、該当のボクセルが占有されたと判断する、請求項５に記載の３次元格子地図作成方法。
7. ステレオ全方位カメラを用いて周辺の全方位映像を獲得し、
   前記全方位映像を用いて現在の２次元位置を確認し、
   前記全方位映像の３次元復元を行い、
   前記２次元位置確認結果と前記３次元復元結果とを結合し、前記ステレオ全方位カメラ周辺の３次元格子地図を作成し、
   前記３次元格子地図に基づいて現在位置から目標地点までの走行経路を決定し、
   前記走行経路に沿って前記目標地点まで走行するように制御する、自動走行装置の制御方法。
8. 前記２次元位置確認は、前記全方位映像を用いた２次元位置確認及び地図作成によって行われる、請求項７に記載の方法。
9. 前記２次元位置確認は、前記全方位映像のエピポーラ線上の特徴点を用いて行われる、請求項８に記載の方法。
10. カラーエッジ抽出によって前記特徴点を獲得する、請求項８に記載の方法。
11. 前記３次元復元結果を用いて格子の占有度合を求める、請求項７に記載の方法。
12. 前記３次元格子のボクセルの占有カウントが一定数を超過すると、該当のボクセルが占有されたと判断する、請求項１１に記載の方法。
13. 前記全方位映像を用いて現在の２次元位置を確認し、前記全方位映像の３次元復元を行い、前記２次元位置確認結果と前記３次元復元結果とを結合し、前記ステレオ全方位カメラ周辺の３次元格子地図を作成するために、ステレオ全方位カメラからの１以上の周辺全方位映像を獲得するためのコントローラ；及び
    前記３次元格子地図に関する走行路に従い前記走行装置の本体を移動するための移動装置
    を含む走行経路に沿って動く走行装置。
14. １以上の全方位映像と前記全方位映像の３次元復元とを用いて現在の２次元位置を作成し；
    前記２次元位置確認結果と前記３次元復元結果とを結合することにより前記３次元格子地図を作成する、３次元格子地図作成方法。