マップ作成方法および装置とそのマップを利用した定位方法
【課題】レンジデータと画像特徴点とを連合させるマップ作成および定位方法を提供し、作成された環境デジタルマップをパーソナルナビゲーション装置（ＰＮＤ）にも提供できるようにする。
【解決手段】マップ作成方法が提供される。環境が走査されて環境障害物の深さ情報を獲得する。環境の画像が捕捉されて画像平面が生成される。環境障害物の深さ情報が画像平面に投影されて、投影位置を獲得する。少なくとも１つの特徴ベクトルが、画像平面中の各投影位置周辺の予め決定された範囲から計算される。環境障害物の深さ情報および特徴ベクトルが合併されて、一時点に対するサブマップが生成される。全時点でのサブマップが連合されてマップを生成する。また、マップを使用する定位方法も提供される。
【解決手段】マップ作成方法が提供される。環境が走査されて環境障害物の深さ情報を獲得する。環境の画像が捕捉されて画像平面が生成される。環境障害物の深さ情報が画像平面に投影されて、投影位置を獲得する。少なくとも１つの特徴ベクトルが、画像平面中の各投影位置周辺の予め決定された範囲から計算される。環境障害物の深さ情報および特徴ベクトルが合併されて、一時点に対するサブマップが生成される。全時点でのサブマップが連合されてマップを生成する。また、マップを使用する定位方法も提供される。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、マップ作成（mapping）および定位（localization）方法に関する。とりわけ、この発明は、レンジデータと画像特徴点とを連合させるマップ作成および定位方法に関する。
【背景技術】
【０００２】
定位ならびにナビゲーション（navigation）技術は、実際の環境中で移動する移動プラットフォーム（mobile platfrom）の位置（poses）を推定するために使用されて、移動プラットフォームの精確かつ安定したナビゲーションを達成する。そのような技術には、２つの主要なタイプが含まれ、１つは、環境デジタルマップを確立し、そのようなマップが移動プラットフォームを定位するための参考情報として採用されるものであり、もう１つは、デジタルマップを事前に作成することなく、移動プラットフォームの移動プロセス期間に同時に定位を実施しデジタルマップを作成するもの（即ち、Simultaneous Localization and Mapping = SLAM技術）である。
【０００３】
デジタルマップを作成するための技術は、先ず移動プラットフォーム上の通常レンジ（general range）装置を介して環境障害物情報（environmental obstacle information）を収集し、移動プラットフォームに関連する空間中における障害物の位置、例えば、図１Ａに示した座標情報を獲得する。そして、障害物の空間的位置情報を収集することにより、図１Ｂに示すように、環境デジタルマップが次第に作成され、そのうち、座標情報が深さ（depth）（輪郭 = profile）情報に転換される。環境デジタルマップが作成された後、移動プラットフォームが移動プロセス期間に通常レンジ装置を介して環境情報を収集するとともに、さらに、その位置を推定するために環境情報をマップと比較する。図２において、Ａが移動プラットフォームの現在位置推定を表し、ドットライン部分が円部分と重なり合っている時、移動プラットフォームの位置推定が現在環境状態に従っていることを表し、ドットライン部分と円部分とが大きく離れている時、移動プラットフォームがその現在の精確な位置を確認できないことを表す。
【０００４】
SLAM技術に従い、環境のマップ作成が事前に実施されないとともに、移動プラットフォームの移動プロセス期間に環境的な特徴または陸標（landmarks）が通常レンジ装置または視覚装置を介して検出されてマップの参考情報として供され、定位を達成する。移動プラットフォームの移動プロセス期間に、環境中の認識可能な特徴が検出され、検出された特徴または陸標位置の信頼性の程度が不明確な分布により表される。そのような技術は、環境特徴および空間中の対応する位置を継続的に検出することによって定位機能を達成する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】欧州特許第1176393号明細書
【非特許文献】
【０００６】
【非特許文献１】"Improving Path Planning and Mapping Based on Stereo Vision and Lidar", 10th International Conference on Control, Automation, Robotics and Vision, 2008 (ICARCV 2008). 17-20 Dec. 2008 Page(s):384 - 389.
【非特許文献２】"Building Local Safety Maps for a Wheelchair Robot using Vision and Lasers", Proceedings of the 3rd Canadian Conference on Computer and Robot Vision (CRV’06). 07-09 June 2006 Page(s):25 - 25. Digital Object Identifier 10.1109/CRV.2006.20.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
移動プラットフォームの上記した従来の定位技術に従い、第１タイプ技術について言えば、精確な定位は、デジタルマップ作成のために使用された同一タイプのレンジ検知装置を通してのみ達成できる。デジタルマップが高精度かつ高価な通常レンジ装置（例えば、一般的に使用されるレーザーレンジファインダー）を介して作成される時、精確な定位および環境中で要求されるナビゲーションを備える移動プラットフォームは、全てそのようなタイプの通常レンジ装置を装備することが求められ、従って、移動プラットフォームのハードウェアコストが大幅に増大するとともに、応用できる移動プラットフォームのタイプが限られてしまう。従って、作成された環境デジタルマップが異なる検知装置を装備した異なる種類の移動プラットフォーム（例えば、スマートホン・パーソナルナビゲーション装置など）に提供されて定位およびナビゲーション機能を履行できるか否かが、そのような技術が広く応用できるか否かのキーポイントである。また、通常レンジ装置および視覚装置を介して履行されるSLAM技術は、環境特徴検知を通してのみ定位機能を達成できる。しかし、実際的な応用において、主要な問題点は、複雑な視覚装置運動モデルを構築しなければならないことであるとともに、環境特徴位置の過大な不確実性が回転により発生しがちであることであり、それは、定位の安定性および精確性に影響を及ぼすことができる。同時に、環境特徴間の対応が確立できないので、移動プラットフォームは、運動プロセス中にどの領域が各２環境特徴間で通過できる領域か判定できず、そのような技術は、実際の環境中を移動する移動プラットフォームのためにナビゲーション機能を達成できない。
【０００８】
欧州特許第1176393号明細書（特許文献１）は、無人自動地下採鉱のための装置を開示しており、そのうち、レーザーレンジファインダーからのレンジデータが先ず収集されて、３次元（three-dimantion = 3D）マップデータが確立される。そして、慣性ナビゲーションシステム（Inertial Navigation System = INS）および視覚センサーが装置を定位することに使われる。その原理は、INSが先ず装置の位置を推定するために使用されるとともに、位置周辺の3Dマップデータが、視覚センサーにより走査された特徴点と比較されて、位置推定のエラーが修正される。そのようなパテントに従って、マップが作成された時、レーザーレンジファインダーからのレンジデータだけが使用されるとともにレンジデータおよび画像データの関連性が確立されない。また、定位期間中、INSが装置の位置を推定するために求められる。
【０００９】
P.Moghadamその他（非特許文献１）およびMurarkaその他（非特許文献２）は、レーザーレンジファインダーおよびステレオビジョン（stereo vision）を連合することにより環境マップを作成する技術を提供する。そのような技術に従って、３Ｄ画像データに基づいて計算された３Ｄ空間深さ情報が２Ｄ平面に投影されるとともに、３Ｄ画像を投影することにより生成された２Ｄマップがレーザーレンジファインダーによって作成された２Ｄマップと連合されて環境マップ作成を完了する。そのような技術に従って、２種類のマップが連合されるものの、それらの対応関係は確立されず、個別のセンサーを介して定位が実施される時、捕捉されたデータおよび作成されたマップ間の比較エラーが大きく、定位精度に影響を及ぼす。
【課題を解決するための手段】
【００１０】
この発明は、マップ作成方法を提供する。このマップ作成方法に従って、環境が走査され環境障害物の深さ情報を獲得する。環境の画像が捕捉されて画像平面が生成される。環境障害物の深さ情報が画像平面に投影されて、多数の投影位置を獲得する。少なくとも１つの特徴ベクトルが画像平面中の各投影位置のために計算される。環境障害物の深さ情報および特徴ベクトルが連合され一時点のためのサブマップが生成される。全時点における前記サブマップを連合して完全なデジタルマップを生成する。
【００１１】
この発明は、定位方法を提供する。先ず、マップが獲得され、そのうち、マップが環境障害物の深さ情報と環境障害物に対応する画像の少なくとも１つの特徴ベクトルとを含む。次に、環境画像が視覚検知装置を介して獲得される。次に、少なくとも１つの画像特徴点が、環境画像から抽出される。次に、定位が画像特徴点およびマップに従って実施される。
【００１２】
この発明は、少なくとも１つのレンジ検知装置と少なくとも１つの視覚検知装置と画像処理装置とを備えるマップ作成装置を提供する。レンジ検知装置が環境障害物の深さ情報を収集するために使用される。視覚検知装置が環境画像を捕捉して画像マップを生成する。画像処理装置がレンジ検知装置および視覚検知装置に連結されるとともに、環境障害物の深さ情報を画像平面に投影して多数の投影点を獲得し、画像平面中の各投影点から少なくとも１つの特徴ベクトルを計算し、かつ環境障害物の深さ情報および特徴ベクトルを連合してマップを生成する。
【発明の効果】
【００１３】
上記に従い、ブロック特徴マップを作成する方法および定位方法が提供される。レンジ検知装置により獲得された環境障害物の深さ情報によりマップを作成するプロセス期間に、視覚検知装置が更に実際の環境に対応する画像のブロック特徴データを獲得する。ブロック特徴マップは、レンジ検知装置により提供されたレンジデータおよび画像のブロック特徴データ間の対応関係に従って作成される。ブロック特徴マップが作成された後、移動プラットフォームの定位およびナビゲーションが視覚検知装置により捕捉された環境特徴を作成されたマップと比較するだけで達成できる。
【図面の簡単な説明】
【００１４】
【図１Ａ】従来のマップ作成方法を説明する概略図である。
【図１Ｂ】従来のマップ作成方法を説明する概略図である。
【図２】従来の定位方法を説明する概略図である。
【図３】この発明の実施形態にかかるマップ作成のために使用されるハードウェア構造を説明する概略図である。
【図４】この発明の実施形態にかかるマップ作成方法を説明するフローチャートである。
【図５Ａ】環境障害物の深さ情報を画像平面に投影する概略図である。
【図５Ｂ】環境障害物の深さ情報を画像平面に投影する概略図である。
【図６】画像特徴位置の決定を説明する概略図である。
【図７】定位のために使用される画像フレームおよび対応するマップ情報を説明する説明図である。
【図８】この発明の実施形態にかかる定位方法を説明するフローチャートである。
【図９】この発明の実施形態にかかる埋め込み式手持ちシステムを説明する概略図である。
【発明を実施するための形態】
【００１５】
以下、この発明を実施するための形態を図面に基づいて説明する。
この発明は、検知融合マップ作成技術を画像定位技術に連合させて革新的な検知融合マップ作成技術を提供し、視覚検知装置の検知情報とレンジ検知装置の検知情報とを融合しマップ作成を確立する。作成されたマップがレンジ検知装置を有する移動プラットフォームに提供されて定位を履行できるだけでなく、低いコストの視覚検知装置を有する移動プラットフォームに提供されて定位およびナビゲーションを履行できる。従って、視覚検知装置だけを介して実施するSLAM技術によるナビゲーションの低い安定性および実行不可能性の問題を解決することができる。
【００１６】
先ず、ブロック特徴マップ作成技術が記述される。
【００１７】
図３は、マップ作成のために使用されるハードウェア構造を説明する概略図である。図３において、マップ作成のために使用されるハードウェア構造は、レンジ検知装置１２と視覚検知装置１４とを備え、移動プラットフォーム１０上に携えることができる。レンジ検知装置１２は、例えば、レーザーレンジファインダーであり、レーザービームで走査を実施することができ、環境障害物２０からのレンジデータを獲得するとともに、環境障害物深さ（輪郭）情報を確立する。視覚検知装置１４は、例えば、デジタルカメラであり、環境の画像データを獲得するために使用される。ハードウェア構造は、更にデータ処理装置を備えることができ、移動プラットフォーム１０上に配備されてマップ作成手続を処理することができる、またはリモートコントロール方式でマップ作成手続を処理することができる。
【００１８】
要するに、マップ作成手続は、例えば、先ずレンジ検知装置１２を介して精確な環境障害物深さ（輪郭）を獲得することである。そして、環境障害物の深さ情報が視覚検知装置１４により捕捉された画像の画像平面に投影される。画像平面上の対応する位置の特徴点が計算されて、環境障害物の深さ及び画像情報間の計量対応（measurement correspondence）を獲得する。
【００１９】
移動プラットフォーム１０が移動する時、レンジ検知装置１２（例えば、レーザーレンジファインダー）により検出された距離計量値および視覚検知装置１４（例えば、通常デジタルカメラ）により捕捉された画像データは、マップ作成に使用される。マップ作成プロセスが完了した後、移動プラットフォーム１０の定位が環境画像特徴と作成されたブロック特徴マップとを比較するだけで達成することができ、そのうち、環境画像特徴は、移動プラットフォーム１０上の視覚検知装置１４により捕捉された環境画像を分析することによって獲得される。
【００２０】
図４は、この発明の実施形態にかかるマップ作成方法を説明するフローチャートである。ステップＳ１００中、システムが初期化され、初期設定（initialization）ステージ期間、視覚検知装置１４（例えば、デジタルカメラ）の校正（calibration）が事前に実施されなければならず、視覚検知装置１４の内部パラメーターを獲得する。ここで、Ｋが内部パラメーターの３×３マトリックスを表す。
【００２１】
例えば、空間中の一点は、内部パラメーターＫの情報に従って視覚検知装置１４により獲得された画像平面上のある位置に投影されることができる。視覚検知装置１４およびレンジ検知装置１２の空間相対関係は、３×３回転マトリックスＲCおよび３×１置換マトリックスＴCによって表すことができる。これらのパラメーターは、下記する数式１に示すように、レンジ検知装置１２により検出された環境障害物深さ情報を画像平面上に投影するために使用される。
ＸＩＭＧ＝Ｋ［ＲC［ＸＳＣＡＮ＿Ｌ］＋ＴC］ （１）
式中、ＸＩＭＧが画像座標情報を表すとともに、ＸＩＭＧ＝（u,v）が画像平面上の位置を表すために使用できる。ＲCおよびＴCは、それぞれ視覚検知装置およびレンジ検知装置の回転ならびに置換関係を表す。Ｋは、視覚検知装置の内部パラメーターを表し、下記マトリックスにより表されことができ、そのうち、（ｆｕ，ｆｖ）が視覚検知装置の焦点距離を表し、（ｃｕ，ｃｖ）が画像中心位置を表し、ｓが画像分布パラメーターを表す等である。
【数１】
【００２２】
次に、ステップＳ１０２中、環境障害物情報の走査マッチング（scan matching）が実施される。レンジ検知装置がある時点においてレーザービームを介して、ある角度範囲の走査を実施して、その時点での環境の障害物深さ情報を獲得する、すなわち、走査を介して障害物の距離情報を獲得した後、空間中の環境障害物の輪郭情報が獲得される。
【００２３】
そして、２つの時点でレンジ検知装置１２により検出された環境の障害物深さ情報が比較されて移動プラットフォーム１０の現在移動量が計算される。移動量を計算するための方法は、下記の数式２により導かれる。
ＸＳＣＡＮ＿Ｌｔ＝ＲｓＸＳＣＡＮ＿Ｌｔ-1＋Ｔｓ （２）
式中、ＸＳＣＡＮ＿Ｌがレンジ検知装置１２座標軸に関する環境障害物の空間位置を表す。ＲｓおよびＴｓがそれぞれ前時点の検知結果に関する現在検知結果の回転量および線形移行量を表す。
【００２４】
次に、ステップＳ１０４中、空間情報および画像情報が対応するように統合される、即ち、レンジ検知装置１２により検出された環境障害物の深さ情報（数式３）が視覚検知装置１４により獲得された画像平面に投影される（数式４）。図５Ａは、ある時点でレンジ検知装置１２により検出された環境障害物の深さ情報（即ち、環境障害物の輪郭）を示す図面である。図５Ｂは、同一時点で視覚検知装置１４により獲得された画像を示す図面である。画像に従って確立された画像平面は、画像座標ＸＩＭＧ＝（u,v）によって表される。図５Ｂ中の点線は、環境障害物の深さ情報を画像平面に投影された結果を表す。即ち、点線は、画像平面上に図５Ａの環境障害物の対応する位置を表す。例えば、図５Ｂ中の位置Ａ&Ｂは、それぞれ図５Ａ中の位置Ａ&Ｂに対応する。
ＸＳＣＡＮ＿Ｌ＝（Ｘｓｃａｎ＿Ｌｘ，Ｙｓｃａｎ＿Ｌｙ，Ｚｓｃａｎ＿Ｌｚ） （３）
ＸＩＭＧ＝（u,v） （４）
【００２５】
次に、ステップＳ１０６中、画像平面上の特徴ベクトル（feature vectors）が計算される。ステップＳ１０４の結果に従って、画像平面上の多数の投影位置が確認された後、投影位置周辺のＭ×Ｍ画素範囲中の画像情報が計算されて、画像特徴位置が抽出される。
【００２６】
図６は、画像特徴位置の決定を説明する概略図である。図６において、画像特徴位置は、コーナーポイント、エッジ、区域極値などにより決定することができるとともに、特徴ベクトルｆｉが画像上で検出された環境特徴を表すために使用できる。適切なｆｉが見つからない時、図６に示すように、上方特徴ベクトルｆｕｉあるいは下方特徴ベクトルｆｄｉが更にサーチされる。
【００２７】
上記特徴ベクトル中、特徴ベクトルｆｉが投影位置に相対して抽出した画像中における距離計量値の特徴ベクトルを表し、（ｆｉ1，ｆｉ2，ｆｉ3，…，ｆｉN）により表すことができる。Ｎが特徴ベクトルの次元（dimension）を表す。
【００２８】
上方特徴ベクトルｆｕｉが投射位置の垂直軸上方の位置から抽出した画像中における距離計量値の特徴ベクトルを表し、（ｆｕｉ1，ｆｕｉ2，ｆｕｉ3，…，ｆｕｉN）により表すことができる。下方特徴ベクトルｆｄｉが投射位置の垂直軸下方の位置から画像中における距離計量値の特徴ベクトルを表し、、（ｆｄｉ1，ｆｄｉ2，ｆｄｉ3，…，ｆｄｉN）により表すことができる。
【００２９】
特徴ベクトルを選択する原則は、主要に示差性（distinctiveness）および角度変化などに対する許容性である。もし計算された環境特徴ベクトルが隣接する特徴ベクトルと高い近似性があるならば、そのようなベクトルは破棄されて、識別困難度が増大することを回避する。
【００３０】
次に、ステップＳ１０８中、移動プラットフォーム１０の位置（pose）が推定される。このステップ中、現時点ｔでの移動プラットフォーム１０の位置および角度情報は、前時点ｔ−１での移動プラットフォーム１０の位置推定およびステップＳ１０２中の比較結果に従って推定できる。時点ｔでの移動プラットフォーム１０の位置の推定結果は、座標ＰＯＳ（ｔ）＝（ｘｔ，ｙｔ，θｔ）で表すことができ、そのうち、（ｘｔ，ｙｔ）が位置を表し、θｔが角度を表す。
【００３１】
ステップＳ１１０中、環境障害物の深さ情報が世界座標システム（world coordinate system）に変換される。即ち、現時点ｔの環境障害物の深さ情報は、下記する数式５に示すように、世界座標システムに変換される。そのうち、ＸＳＣＡＮ＿Ｇｔが世界座標システム中の環境障害物の空間位置を表し、（ｘｉ，ｙｉ，ｚｉ）座標システムにより表すことができ、ｉ＝１〜Ｎであり、かつＮが環境障害物の数量（または、１サイクルの走査点の数量）を表す。
ＸＳＣＡＮ＿Ｇｔ＝（ｓｃａｎ＿Ｇｘ，ｓｃａｎ＿Ｇｙ，ｓｃａｎ＿Ｇｚ） （５）
【００３２】
次に、ステップＳ１１２中、世界座標および画像平面の特徴ベクトルが連合される。ステップＳ１１２中、ステップＳ１０６に従って計算された画像中の環境特徴ベクトルｆｉ，ｆｕｉ，ｆｄｉが環境障害物の深さ情報の世界座標システムと連合されて、時点ｔでのサブマップ情報ＭＡPｔが生成される。サブマップ情報ＭＡPｔは、下記する数式６で表すことができる。
ＭＡPｔ＝（［ＸＳＣＡＮ＿Ｇ１，ｆ１，ｆｕ１，ｆｄ１］，［ＸＳＣＡＮ＿Ｇ２，ｆ２，ｆｕ２，ｆｄ２］，……，［ＸＳＣＡＮ＿ＧＮ，ｆＮ，ｆｕＮ，ｆｄＮ］） （６）
【００３３】
数式６に従って、明白に分かるように、この実施形態の作成されたマップが各時点について環境障害物の深さ情報（世界座標で表される）および画像平面の環境特徴ベクトル情報（深さ情報中の特徴点に対応する特徴ベクトル）を含む。言い換えれば、通常マップに使用される環境障害物の深さ情報に加えて、この実施形態の作成されるマップは、更に画像により呈示される環境特徴ベクトル情報を含む。
【００３４】
次に、ステップＳ１１４中、環境障害物の深さ情報の全てが走査されたか否か決定される。もし「はい」であれば、ステップ１２０が実行されて全てのサブマップが連合されてマップＭＡＰを生成し、それを下記する数式７で表すことができる。
ΣＭＡＰｔ （７）
【００３５】
数式７は、全時点の環境障害物の深さ情報が画像特徴ベクトルに連合された後のマップ結果を表す。
【００３６】
逆に、もしステップＳ１１４に従って走査が完了していないと決定されれば、ステップＳ１１６が実行されて現在マップを更新するとともに、ステップＳ１０２が、環境障害物の走査が完了するまで、繰り返しステップＳ１０２〜Ｓ１１４を実行する。
【００３７】
つまり、この実施形態の作成されたマップは、環境障害物の深さ情報および画像特徴ベクトルを内包して、定位プロセス期間に、定位に使用できるセンサーが多様化される。従って、もしビデオカメラだけが使用されて画像が捕捉されたとしても、定位（localization）は、環境障害物の実際の深さ情報なしに、マップ中の特徴ベクトル情報を介して尚かつ実施でき、マップ作成のために同一タイプの検知装置を使用しなければならない従来の定位技術の不便さを回避することができる。
【００３８】
そして、上記マップ作成方法に従って作成されたマップを使用する定位方法が以下に記述される。
【００３９】
図７は、定位のために使用される画像フレームおよび対応するマップ情報を説明する説明図である。図８は、この発明の実施形態にかかる定位方法を説明するフローチャートである。ここでは、上記したマップ作成方法に従ってマップが既に作成されていると仮定する。
【００４０】
図７と上記した数式６に従って、この実施形態のデジタルマップフォーマットは、空間中の障害物の空間位置ｐｉ（輪郭深さ情報（点線）および図７のマップ中に“○”でマークされた位置）と画像平面上の投影位置の特徴ベクトルｆｉ，ｆｕｉ，ｆｄｉ（図７のフレーム中に“×”でマークされた位置）とを備える。移動プラットフォームが視覚検知装置だけを装備し、かつ視覚検知装置の校正が連合されて内部パラメーターＫが獲得された時、画像特徴点が、捕捉された環境画像特徴点位置ｐｉ（ｕ，ｖ，ｌ）に従って作成されたデジタルマップ中で世界座標位置ｐｗ（ｘ，ｙ，ｚ，ｌ）と比較される。
【００４１】
このようにして、視覚検知装置の外部パラメーターマトリックスＴが更に導かれて移動プラットフォームの位置情報ｃＷ（世界座標）を獲得することができる。
【００４２】
そして、定位方法は、さらに、図８に示すフローチャートを参考にして記述される。図８において、ステップＳ２００中、図７を一例として挙げると、画像フレームｉ中に４つ以上の特徴点があり、かつ画像座標と世界座標とがそれぞれｐｉとｐｗとで表される。画像座標および世界座標間の対応関係は、下記する数式８によって表すことができ、式中、ＲとＴとがそれぞれ回転マトリックスと移行マトリックスとを表す。
【数２】
【００４３】
次に、ステップＳ２０２中、視覚検知装置の内部パラメーターＫにより画像座標中の点ｐｉがカメラ座標中の点ｐｊに変換される。
【００４４】
このステップ中、画像中の座標ＰＩが視覚検知装置の座標システムの位置ＰＪへ投影される。投影方程式は、数式９である。数式１０は、数式９を数式８へ代入することにより獲得できる。
ＰＪ＝Ｋ−１ＰＩ （９）
ＰＪ＝ＰＴｐｗ＝［Ｒ ｔ］ｐｗ＝ＭＰＷ （１０）
【００４５】
次に、ステップＳ２０４中、最小二乗法（least square method）が視覚検知装置の外部パラメーターマトリックスＴを推定することに使用される。ここで、投影マトリックスＰおよび外部パラメーターマトリックスＴがマトリックスＭ（数１０）へ合併されるとともに、かくして外部パラメーターマトリックスＴの解を求めることが線形システムの解を求めることに等しいものとなる。最小二乗法が視覚検知装置の外部パラメーターの解答Ｍ＝［Ｒ ｔ］を獲得するために使用されて、下記する数式１１の最小化を満足させる。また、外部パラメーターマトリックスＴが回転マトリックスＲと置換マトリックスｔとを含み、下記する数式１２により表すことができる。
【数３】
【数４】
【００４６】
ステップＳ２０６中、世界座標システム中の視覚検知装置の位置ｃｗが外部パラメーターマトリックスＴの逆マトリックスを使用することによって獲得できる。視覚検知装置の座標システム中、視覚検知装置の位置は、その座標システムの原点（０，０，０）である。世界座標システム中、視覚検知装置の世界座標位置ｃｗは、外部パラメーターマトリックスＴの逆マトリックス（数式１３）により表すことができる。
【数５】
【００４７】
また、視覚検知装置の角度変化は、隣接する時点で捕捉された２つの連続画像から導き出すことができる。視覚検知装置が原位置（in-situ）回転だけしたと仮定すれば、２つの連続画像が同一特徴点を有する。もし第１時点の特徴点画像座標がｐ１であり、第２時点の特徴点画像座標がｐ２であれば、視覚検知装置の角度変化θは、ｐ１およびｐ２間の距離差と視覚検知装置の焦点距離ｆに従って計算することができる。
【００４８】
上記したステップＳ２００〜Ｓ２０６に従って、定位は、図４のフローチャートに従い作成されたマップに基づいて導き出せる。上記したように、マップ作成期間に、環境障害物（輪郭）の深さ情報および画像特徴点情報がマップ中に統合される。従って、定位期間に、たとえ、もし対応するレンジ検知装置が移動プラットフォームに装備されておらず、視覚検知装置だけが画像特徴点の獲得に使用されても、特徴点が作成されたマップと比較されて精確な定位を達成できる。従って、定位およびナビゲーション機能が低コストの視覚検知装置だけを装備した移動プラットフォームに提供されるため、視覚検知装置だけを介してＳＬＡＭ技術を履行することにより引き起こされるナビゲーションの低い安定性および実行不可能性という問題を解決する。
【００４９】
実施形態の変化例を以下に記述する。上記実施形態中、様々なセンサーが移動プラットフォーム上に採用される。しかし、図９に示すように、マップ作成方法は、埋め込み式手持ち装置にも応用できる。図９は、レンジ検知装置および視覚検知装置を統合した埋め込み式手持ちシステム２００を説明する概略図である。このような構造により、上記した２Ｄマップ作成ならびに移動プラットフォーム定位に加えて、手持ち装置の移動特性が更に統合されて３Ｄブロック特徴マップを構築することができ、埋め込み式手持ち装置を３Ｄパーソナルナビゲーション装置（Personal Navigation Device = PND）として供することができる。
【００５０】
つまり、上記に従って、ブロック特徴マップを作成する方法および定位方法が提供される。レンジ検知装置により獲得された環境障害物の深さ情報によりマップを作成するプロセス期間に、視覚検知装置が更に実際の環境に対応する画像のブロック特徴データを獲得する。ブロック特徴マップは、レンジ検知装置により提供されたレンジデータおよび画像のブロック特徴データ間の対応関係に従って作成される。ブロック特徴マップが作成された後、移動プラットフォームの定位およびナビゲーションが視覚検知装置により補足された環境特徴を作成されたマップと比較するだけで達成できる。
【００５１】
以上のごとく、この発明を実施形態により開示したが、もとより、この発明を限定するためのものではなく、当業者であれば容易に理解できるように、この発明の技術思想の範囲内において、適当な変更ならびに修正が当然なされうるものであるから、その特許権保護の範囲は、特許請求の範囲および、それと均等な領域を基準として定めなければならない。
【符号の説明】
【００５２】
１０ 移動プラットフォーム
１２ レンジ検知装置
１４ 視覚検知装置
２０ 環境障害物
２００ 埋め込み式手持ちシステム（装置）
【技術分野】
【０００１】
この発明は、マップ作成（mapping）および定位（localization）方法に関する。とりわけ、この発明は、レンジデータと画像特徴点とを連合させるマップ作成および定位方法に関する。
【背景技術】
【０００２】
定位ならびにナビゲーション（navigation）技術は、実際の環境中で移動する移動プラットフォーム（mobile platfrom）の位置（poses）を推定するために使用されて、移動プラットフォームの精確かつ安定したナビゲーションを達成する。そのような技術には、２つの主要なタイプが含まれ、１つは、環境デジタルマップを確立し、そのようなマップが移動プラットフォームを定位するための参考情報として採用されるものであり、もう１つは、デジタルマップを事前に作成することなく、移動プラットフォームの移動プロセス期間に同時に定位を実施しデジタルマップを作成するもの（即ち、Simultaneous Localization and Mapping = SLAM技術）である。
【０００３】
デジタルマップを作成するための技術は、先ず移動プラットフォーム上の通常レンジ（general range）装置を介して環境障害物情報（environmental obstacle information）を収集し、移動プラットフォームに関連する空間中における障害物の位置、例えば、図１Ａに示した座標情報を獲得する。そして、障害物の空間的位置情報を収集することにより、図１Ｂに示すように、環境デジタルマップが次第に作成され、そのうち、座標情報が深さ（depth）（輪郭 = profile）情報に転換される。環境デジタルマップが作成された後、移動プラットフォームが移動プロセス期間に通常レンジ装置を介して環境情報を収集するとともに、さらに、その位置を推定するために環境情報をマップと比較する。図２において、Ａが移動プラットフォームの現在位置推定を表し、ドットライン部分が円部分と重なり合っている時、移動プラットフォームの位置推定が現在環境状態に従っていることを表し、ドットライン部分と円部分とが大きく離れている時、移動プラットフォームがその現在の精確な位置を確認できないことを表す。
【０００４】
SLAM技術に従い、環境のマップ作成が事前に実施されないとともに、移動プラットフォームの移動プロセス期間に環境的な特徴または陸標（landmarks）が通常レンジ装置または視覚装置を介して検出されてマップの参考情報として供され、定位を達成する。移動プラットフォームの移動プロセス期間に、環境中の認識可能な特徴が検出され、検出された特徴または陸標位置の信頼性の程度が不明確な分布により表される。そのような技術は、環境特徴および空間中の対応する位置を継続的に検出することによって定位機能を達成する。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】欧州特許第1176393号明細書
【非特許文献】
【０００６】
【非特許文献１】"Improving Path Planning and Mapping Based on Stereo Vision and Lidar", 10th International Conference on Control, Automation, Robotics and Vision, 2008 (ICARCV 2008). 17-20 Dec. 2008 Page(s):384 - 389.
【非特許文献２】"Building Local Safety Maps for a Wheelchair Robot using Vision and Lasers", Proceedings of the 3rd Canadian Conference on Computer and Robot Vision (CRV’06). 07-09 June 2006 Page(s):25 - 25. Digital Object Identifier 10.1109/CRV.2006.20.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
移動プラットフォームの上記した従来の定位技術に従い、第１タイプ技術について言えば、精確な定位は、デジタルマップ作成のために使用された同一タイプのレンジ検知装置を通してのみ達成できる。デジタルマップが高精度かつ高価な通常レンジ装置（例えば、一般的に使用されるレーザーレンジファインダー）を介して作成される時、精確な定位および環境中で要求されるナビゲーションを備える移動プラットフォームは、全てそのようなタイプの通常レンジ装置を装備することが求められ、従って、移動プラットフォームのハードウェアコストが大幅に増大するとともに、応用できる移動プラットフォームのタイプが限られてしまう。従って、作成された環境デジタルマップが異なる検知装置を装備した異なる種類の移動プラットフォーム（例えば、スマートホン・パーソナルナビゲーション装置など）に提供されて定位およびナビゲーション機能を履行できるか否かが、そのような技術が広く応用できるか否かのキーポイントである。また、通常レンジ装置および視覚装置を介して履行されるSLAM技術は、環境特徴検知を通してのみ定位機能を達成できる。しかし、実際的な応用において、主要な問題点は、複雑な視覚装置運動モデルを構築しなければならないことであるとともに、環境特徴位置の過大な不確実性が回転により発生しがちであることであり、それは、定位の安定性および精確性に影響を及ぼすことができる。同時に、環境特徴間の対応が確立できないので、移動プラットフォームは、運動プロセス中にどの領域が各２環境特徴間で通過できる領域か判定できず、そのような技術は、実際の環境中を移動する移動プラットフォームのためにナビゲーション機能を達成できない。
【０００８】
欧州特許第1176393号明細書（特許文献１）は、無人自動地下採鉱のための装置を開示しており、そのうち、レーザーレンジファインダーからのレンジデータが先ず収集されて、３次元（three-dimantion = 3D）マップデータが確立される。そして、慣性ナビゲーションシステム（Inertial Navigation System = INS）および視覚センサーが装置を定位することに使われる。その原理は、INSが先ず装置の位置を推定するために使用されるとともに、位置周辺の3Dマップデータが、視覚センサーにより走査された特徴点と比較されて、位置推定のエラーが修正される。そのようなパテントに従って、マップが作成された時、レーザーレンジファインダーからのレンジデータだけが使用されるとともにレンジデータおよび画像データの関連性が確立されない。また、定位期間中、INSが装置の位置を推定するために求められる。
【０００９】
P.Moghadamその他（非特許文献１）およびMurarkaその他（非特許文献２）は、レーザーレンジファインダーおよびステレオビジョン（stereo vision）を連合することにより環境マップを作成する技術を提供する。そのような技術に従って、３Ｄ画像データに基づいて計算された３Ｄ空間深さ情報が２Ｄ平面に投影されるとともに、３Ｄ画像を投影することにより生成された２Ｄマップがレーザーレンジファインダーによって作成された２Ｄマップと連合されて環境マップ作成を完了する。そのような技術に従って、２種類のマップが連合されるものの、それらの対応関係は確立されず、個別のセンサーを介して定位が実施される時、捕捉されたデータおよび作成されたマップ間の比較エラーが大きく、定位精度に影響を及ぼす。
【課題を解決するための手段】
【００１０】
この発明は、マップ作成方法を提供する。このマップ作成方法に従って、環境が走査され環境障害物の深さ情報を獲得する。環境の画像が捕捉されて画像平面が生成される。環境障害物の深さ情報が画像平面に投影されて、多数の投影位置を獲得する。少なくとも１つの特徴ベクトルが画像平面中の各投影位置のために計算される。環境障害物の深さ情報および特徴ベクトルが連合され一時点のためのサブマップが生成される。全時点における前記サブマップを連合して完全なデジタルマップを生成する。
【００１１】
この発明は、定位方法を提供する。先ず、マップが獲得され、そのうち、マップが環境障害物の深さ情報と環境障害物に対応する画像の少なくとも１つの特徴ベクトルとを含む。次に、環境画像が視覚検知装置を介して獲得される。次に、少なくとも１つの画像特徴点が、環境画像から抽出される。次に、定位が画像特徴点およびマップに従って実施される。
【００１２】
この発明は、少なくとも１つのレンジ検知装置と少なくとも１つの視覚検知装置と画像処理装置とを備えるマップ作成装置を提供する。レンジ検知装置が環境障害物の深さ情報を収集するために使用される。視覚検知装置が環境画像を捕捉して画像マップを生成する。画像処理装置がレンジ検知装置および視覚検知装置に連結されるとともに、環境障害物の深さ情報を画像平面に投影して多数の投影点を獲得し、画像平面中の各投影点から少なくとも１つの特徴ベクトルを計算し、かつ環境障害物の深さ情報および特徴ベクトルを連合してマップを生成する。
【発明の効果】
【００１３】
上記に従い、ブロック特徴マップを作成する方法および定位方法が提供される。レンジ検知装置により獲得された環境障害物の深さ情報によりマップを作成するプロセス期間に、視覚検知装置が更に実際の環境に対応する画像のブロック特徴データを獲得する。ブロック特徴マップは、レンジ検知装置により提供されたレンジデータおよび画像のブロック特徴データ間の対応関係に従って作成される。ブロック特徴マップが作成された後、移動プラットフォームの定位およびナビゲーションが視覚検知装置により捕捉された環境特徴を作成されたマップと比較するだけで達成できる。
【図面の簡単な説明】
【００１４】
【図１Ａ】従来のマップ作成方法を説明する概略図である。
【図１Ｂ】従来のマップ作成方法を説明する概略図である。
【図２】従来の定位方法を説明する概略図である。
【図３】この発明の実施形態にかかるマップ作成のために使用されるハードウェア構造を説明する概略図である。
【図４】この発明の実施形態にかかるマップ作成方法を説明するフローチャートである。
【図５Ａ】環境障害物の深さ情報を画像平面に投影する概略図である。
【図５Ｂ】環境障害物の深さ情報を画像平面に投影する概略図である。
【図６】画像特徴位置の決定を説明する概略図である。
【図７】定位のために使用される画像フレームおよび対応するマップ情報を説明する説明図である。
【図８】この発明の実施形態にかかる定位方法を説明するフローチャートである。
【図９】この発明の実施形態にかかる埋め込み式手持ちシステムを説明する概略図である。
【発明を実施するための形態】
【００１５】
以下、この発明を実施するための形態を図面に基づいて説明する。
この発明は、検知融合マップ作成技術を画像定位技術に連合させて革新的な検知融合マップ作成技術を提供し、視覚検知装置の検知情報とレンジ検知装置の検知情報とを融合しマップ作成を確立する。作成されたマップがレンジ検知装置を有する移動プラットフォームに提供されて定位を履行できるだけでなく、低いコストの視覚検知装置を有する移動プラットフォームに提供されて定位およびナビゲーションを履行できる。従って、視覚検知装置だけを介して実施するSLAM技術によるナビゲーションの低い安定性および実行不可能性の問題を解決することができる。
【００１６】
先ず、ブロック特徴マップ作成技術が記述される。
【００１７】
図３は、マップ作成のために使用されるハードウェア構造を説明する概略図である。図３において、マップ作成のために使用されるハードウェア構造は、レンジ検知装置１２と視覚検知装置１４とを備え、移動プラットフォーム１０上に携えることができる。レンジ検知装置１２は、例えば、レーザーレンジファインダーであり、レーザービームで走査を実施することができ、環境障害物２０からのレンジデータを獲得するとともに、環境障害物深さ（輪郭）情報を確立する。視覚検知装置１４は、例えば、デジタルカメラであり、環境の画像データを獲得するために使用される。ハードウェア構造は、更にデータ処理装置を備えることができ、移動プラットフォーム１０上に配備されてマップ作成手続を処理することができる、またはリモートコントロール方式でマップ作成手続を処理することができる。
【００１８】
要するに、マップ作成手続は、例えば、先ずレンジ検知装置１２を介して精確な環境障害物深さ（輪郭）を獲得することである。そして、環境障害物の深さ情報が視覚検知装置１４により捕捉された画像の画像平面に投影される。画像平面上の対応する位置の特徴点が計算されて、環境障害物の深さ及び画像情報間の計量対応（measurement correspondence）を獲得する。
【００１９】
移動プラットフォーム１０が移動する時、レンジ検知装置１２（例えば、レーザーレンジファインダー）により検出された距離計量値および視覚検知装置１４（例えば、通常デジタルカメラ）により捕捉された画像データは、マップ作成に使用される。マップ作成プロセスが完了した後、移動プラットフォーム１０の定位が環境画像特徴と作成されたブロック特徴マップとを比較するだけで達成することができ、そのうち、環境画像特徴は、移動プラットフォーム１０上の視覚検知装置１４により捕捉された環境画像を分析することによって獲得される。
【００２０】
図４は、この発明の実施形態にかかるマップ作成方法を説明するフローチャートである。ステップＳ１００中、システムが初期化され、初期設定（initialization）ステージ期間、視覚検知装置１４（例えば、デジタルカメラ）の校正（calibration）が事前に実施されなければならず、視覚検知装置１４の内部パラメーターを獲得する。ここで、Ｋが内部パラメーターの３×３マトリックスを表す。
【００２１】
例えば、空間中の一点は、内部パラメーターＫの情報に従って視覚検知装置１４により獲得された画像平面上のある位置に投影されることができる。視覚検知装置１４およびレンジ検知装置１２の空間相対関係は、３×３回転マトリックスＲCおよび３×１置換マトリックスＴCによって表すことができる。これらのパラメーターは、下記する数式１に示すように、レンジ検知装置１２により検出された環境障害物深さ情報を画像平面上に投影するために使用される。
ＸＩＭＧ＝Ｋ［ＲC［ＸＳＣＡＮ＿Ｌ］＋ＴC］ （１）
式中、ＸＩＭＧが画像座標情報を表すとともに、ＸＩＭＧ＝（u,v）が画像平面上の位置を表すために使用できる。ＲCおよびＴCは、それぞれ視覚検知装置およびレンジ検知装置の回転ならびに置換関係を表す。Ｋは、視覚検知装置の内部パラメーターを表し、下記マトリックスにより表されことができ、そのうち、（ｆｕ，ｆｖ）が視覚検知装置の焦点距離を表し、（ｃｕ，ｃｖ）が画像中心位置を表し、ｓが画像分布パラメーターを表す等である。
【数１】
【００２２】
次に、ステップＳ１０２中、環境障害物情報の走査マッチング（scan matching）が実施される。レンジ検知装置がある時点においてレーザービームを介して、ある角度範囲の走査を実施して、その時点での環境の障害物深さ情報を獲得する、すなわち、走査を介して障害物の距離情報を獲得した後、空間中の環境障害物の輪郭情報が獲得される。
【００２３】
そして、２つの時点でレンジ検知装置１２により検出された環境の障害物深さ情報が比較されて移動プラットフォーム１０の現在移動量が計算される。移動量を計算するための方法は、下記の数式２により導かれる。
ＸＳＣＡＮ＿Ｌｔ＝ＲｓＸＳＣＡＮ＿Ｌｔ-1＋Ｔｓ （２）
式中、ＸＳＣＡＮ＿Ｌがレンジ検知装置１２座標軸に関する環境障害物の空間位置を表す。ＲｓおよびＴｓがそれぞれ前時点の検知結果に関する現在検知結果の回転量および線形移行量を表す。
【００２４】
次に、ステップＳ１０４中、空間情報および画像情報が対応するように統合される、即ち、レンジ検知装置１２により検出された環境障害物の深さ情報（数式３）が視覚検知装置１４により獲得された画像平面に投影される（数式４）。図５Ａは、ある時点でレンジ検知装置１２により検出された環境障害物の深さ情報（即ち、環境障害物の輪郭）を示す図面である。図５Ｂは、同一時点で視覚検知装置１４により獲得された画像を示す図面である。画像に従って確立された画像平面は、画像座標ＸＩＭＧ＝（u,v）によって表される。図５Ｂ中の点線は、環境障害物の深さ情報を画像平面に投影された結果を表す。即ち、点線は、画像平面上に図５Ａの環境障害物の対応する位置を表す。例えば、図５Ｂ中の位置Ａ&Ｂは、それぞれ図５Ａ中の位置Ａ&Ｂに対応する。
ＸＳＣＡＮ＿Ｌ＝（Ｘｓｃａｎ＿Ｌｘ，Ｙｓｃａｎ＿Ｌｙ，Ｚｓｃａｎ＿Ｌｚ） （３）
ＸＩＭＧ＝（u,v） （４）
【００２５】
次に、ステップＳ１０６中、画像平面上の特徴ベクトル（feature vectors）が計算される。ステップＳ１０４の結果に従って、画像平面上の多数の投影位置が確認された後、投影位置周辺のＭ×Ｍ画素範囲中の画像情報が計算されて、画像特徴位置が抽出される。
【００２６】
図６は、画像特徴位置の決定を説明する概略図である。図６において、画像特徴位置は、コーナーポイント、エッジ、区域極値などにより決定することができるとともに、特徴ベクトルｆｉが画像上で検出された環境特徴を表すために使用できる。適切なｆｉが見つからない時、図６に示すように、上方特徴ベクトルｆｕｉあるいは下方特徴ベクトルｆｄｉが更にサーチされる。
【００２７】
上記特徴ベクトル中、特徴ベクトルｆｉが投影位置に相対して抽出した画像中における距離計量値の特徴ベクトルを表し、（ｆｉ1，ｆｉ2，ｆｉ3，…，ｆｉN）により表すことができる。Ｎが特徴ベクトルの次元（dimension）を表す。
【００２８】
上方特徴ベクトルｆｕｉが投射位置の垂直軸上方の位置から抽出した画像中における距離計量値の特徴ベクトルを表し、（ｆｕｉ1，ｆｕｉ2，ｆｕｉ3，…，ｆｕｉN）により表すことができる。下方特徴ベクトルｆｄｉが投射位置の垂直軸下方の位置から画像中における距離計量値の特徴ベクトルを表し、、（ｆｄｉ1，ｆｄｉ2，ｆｄｉ3，…，ｆｄｉN）により表すことができる。
【００２９】
特徴ベクトルを選択する原則は、主要に示差性（distinctiveness）および角度変化などに対する許容性である。もし計算された環境特徴ベクトルが隣接する特徴ベクトルと高い近似性があるならば、そのようなベクトルは破棄されて、識別困難度が増大することを回避する。
【００３０】
次に、ステップＳ１０８中、移動プラットフォーム１０の位置（pose）が推定される。このステップ中、現時点ｔでの移動プラットフォーム１０の位置および角度情報は、前時点ｔ−１での移動プラットフォーム１０の位置推定およびステップＳ１０２中の比較結果に従って推定できる。時点ｔでの移動プラットフォーム１０の位置の推定結果は、座標ＰＯＳ（ｔ）＝（ｘｔ，ｙｔ，θｔ）で表すことができ、そのうち、（ｘｔ，ｙｔ）が位置を表し、θｔが角度を表す。
【００３１】
ステップＳ１１０中、環境障害物の深さ情報が世界座標システム（world coordinate system）に変換される。即ち、現時点ｔの環境障害物の深さ情報は、下記する数式５に示すように、世界座標システムに変換される。そのうち、ＸＳＣＡＮ＿Ｇｔが世界座標システム中の環境障害物の空間位置を表し、（ｘｉ，ｙｉ，ｚｉ）座標システムにより表すことができ、ｉ＝１〜Ｎであり、かつＮが環境障害物の数量（または、１サイクルの走査点の数量）を表す。
ＸＳＣＡＮ＿Ｇｔ＝（ｓｃａｎ＿Ｇｘ，ｓｃａｎ＿Ｇｙ，ｓｃａｎ＿Ｇｚ） （５）
【００３２】
次に、ステップＳ１１２中、世界座標および画像平面の特徴ベクトルが連合される。ステップＳ１１２中、ステップＳ１０６に従って計算された画像中の環境特徴ベクトルｆｉ，ｆｕｉ，ｆｄｉが環境障害物の深さ情報の世界座標システムと連合されて、時点ｔでのサブマップ情報ＭＡPｔが生成される。サブマップ情報ＭＡPｔは、下記する数式６で表すことができる。
ＭＡPｔ＝（［ＸＳＣＡＮ＿Ｇ１，ｆ１，ｆｕ１，ｆｄ１］，［ＸＳＣＡＮ＿Ｇ２，ｆ２，ｆｕ２，ｆｄ２］，……，［ＸＳＣＡＮ＿ＧＮ，ｆＮ，ｆｕＮ，ｆｄＮ］） （６）
【００３３】
数式６に従って、明白に分かるように、この実施形態の作成されたマップが各時点について環境障害物の深さ情報（世界座標で表される）および画像平面の環境特徴ベクトル情報（深さ情報中の特徴点に対応する特徴ベクトル）を含む。言い換えれば、通常マップに使用される環境障害物の深さ情報に加えて、この実施形態の作成されるマップは、更に画像により呈示される環境特徴ベクトル情報を含む。
【００３４】
次に、ステップＳ１１４中、環境障害物の深さ情報の全てが走査されたか否か決定される。もし「はい」であれば、ステップ１２０が実行されて全てのサブマップが連合されてマップＭＡＰを生成し、それを下記する数式７で表すことができる。
ΣＭＡＰｔ （７）
【００３５】
数式７は、全時点の環境障害物の深さ情報が画像特徴ベクトルに連合された後のマップ結果を表す。
【００３６】
逆に、もしステップＳ１１４に従って走査が完了していないと決定されれば、ステップＳ１１６が実行されて現在マップを更新するとともに、ステップＳ１０２が、環境障害物の走査が完了するまで、繰り返しステップＳ１０２〜Ｓ１１４を実行する。
【００３７】
つまり、この実施形態の作成されたマップは、環境障害物の深さ情報および画像特徴ベクトルを内包して、定位プロセス期間に、定位に使用できるセンサーが多様化される。従って、もしビデオカメラだけが使用されて画像が捕捉されたとしても、定位（localization）は、環境障害物の実際の深さ情報なしに、マップ中の特徴ベクトル情報を介して尚かつ実施でき、マップ作成のために同一タイプの検知装置を使用しなければならない従来の定位技術の不便さを回避することができる。
【００３８】
そして、上記マップ作成方法に従って作成されたマップを使用する定位方法が以下に記述される。
【００３９】
図７は、定位のために使用される画像フレームおよび対応するマップ情報を説明する説明図である。図８は、この発明の実施形態にかかる定位方法を説明するフローチャートである。ここでは、上記したマップ作成方法に従ってマップが既に作成されていると仮定する。
【００４０】
図７と上記した数式６に従って、この実施形態のデジタルマップフォーマットは、空間中の障害物の空間位置ｐｉ（輪郭深さ情報（点線）および図７のマップ中に“○”でマークされた位置）と画像平面上の投影位置の特徴ベクトルｆｉ，ｆｕｉ，ｆｄｉ（図７のフレーム中に“×”でマークされた位置）とを備える。移動プラットフォームが視覚検知装置だけを装備し、かつ視覚検知装置の校正が連合されて内部パラメーターＫが獲得された時、画像特徴点が、捕捉された環境画像特徴点位置ｐｉ（ｕ，ｖ，ｌ）に従って作成されたデジタルマップ中で世界座標位置ｐｗ（ｘ，ｙ，ｚ，ｌ）と比較される。
【００４１】
このようにして、視覚検知装置の外部パラメーターマトリックスＴが更に導かれて移動プラットフォームの位置情報ｃＷ（世界座標）を獲得することができる。
【００４２】
そして、定位方法は、さらに、図８に示すフローチャートを参考にして記述される。図８において、ステップＳ２００中、図７を一例として挙げると、画像フレームｉ中に４つ以上の特徴点があり、かつ画像座標と世界座標とがそれぞれｐｉとｐｗとで表される。画像座標および世界座標間の対応関係は、下記する数式８によって表すことができ、式中、ＲとＴとがそれぞれ回転マトリックスと移行マトリックスとを表す。
【数２】
【００４３】
次に、ステップＳ２０２中、視覚検知装置の内部パラメーターＫにより画像座標中の点ｐｉがカメラ座標中の点ｐｊに変換される。
【００４４】
このステップ中、画像中の座標ＰＩが視覚検知装置の座標システムの位置ＰＪへ投影される。投影方程式は、数式９である。数式１０は、数式９を数式８へ代入することにより獲得できる。
ＰＪ＝Ｋ−１ＰＩ （９）
ＰＪ＝ＰＴｐｗ＝［Ｒ ｔ］ｐｗ＝ＭＰＷ （１０）
【００４５】
次に、ステップＳ２０４中、最小二乗法（least square method）が視覚検知装置の外部パラメーターマトリックスＴを推定することに使用される。ここで、投影マトリックスＰおよび外部パラメーターマトリックスＴがマトリックスＭ（数１０）へ合併されるとともに、かくして外部パラメーターマトリックスＴの解を求めることが線形システムの解を求めることに等しいものとなる。最小二乗法が視覚検知装置の外部パラメーターの解答Ｍ＝［Ｒ ｔ］を獲得するために使用されて、下記する数式１１の最小化を満足させる。また、外部パラメーターマトリックスＴが回転マトリックスＲと置換マトリックスｔとを含み、下記する数式１２により表すことができる。
【数３】
【数４】
【００４６】
ステップＳ２０６中、世界座標システム中の視覚検知装置の位置ｃｗが外部パラメーターマトリックスＴの逆マトリックスを使用することによって獲得できる。視覚検知装置の座標システム中、視覚検知装置の位置は、その座標システムの原点（０，０，０）である。世界座標システム中、視覚検知装置の世界座標位置ｃｗは、外部パラメーターマトリックスＴの逆マトリックス（数式１３）により表すことができる。
【数５】
【００４７】
また、視覚検知装置の角度変化は、隣接する時点で捕捉された２つの連続画像から導き出すことができる。視覚検知装置が原位置（in-situ）回転だけしたと仮定すれば、２つの連続画像が同一特徴点を有する。もし第１時点の特徴点画像座標がｐ１であり、第２時点の特徴点画像座標がｐ２であれば、視覚検知装置の角度変化θは、ｐ１およびｐ２間の距離差と視覚検知装置の焦点距離ｆに従って計算することができる。
【００４８】
上記したステップＳ２００〜Ｓ２０６に従って、定位は、図４のフローチャートに従い作成されたマップに基づいて導き出せる。上記したように、マップ作成期間に、環境障害物（輪郭）の深さ情報および画像特徴点情報がマップ中に統合される。従って、定位期間に、たとえ、もし対応するレンジ検知装置が移動プラットフォームに装備されておらず、視覚検知装置だけが画像特徴点の獲得に使用されても、特徴点が作成されたマップと比較されて精確な定位を達成できる。従って、定位およびナビゲーション機能が低コストの視覚検知装置だけを装備した移動プラットフォームに提供されるため、視覚検知装置だけを介してＳＬＡＭ技術を履行することにより引き起こされるナビゲーションの低い安定性および実行不可能性という問題を解決する。
【００４９】
実施形態の変化例を以下に記述する。上記実施形態中、様々なセンサーが移動プラットフォーム上に採用される。しかし、図９に示すように、マップ作成方法は、埋め込み式手持ち装置にも応用できる。図９は、レンジ検知装置および視覚検知装置を統合した埋め込み式手持ちシステム２００を説明する概略図である。このような構造により、上記した２Ｄマップ作成ならびに移動プラットフォーム定位に加えて、手持ち装置の移動特性が更に統合されて３Ｄブロック特徴マップを構築することができ、埋め込み式手持ち装置を３Ｄパーソナルナビゲーション装置（Personal Navigation Device = PND）として供することができる。
【００５０】
つまり、上記に従って、ブロック特徴マップを作成する方法および定位方法が提供される。レンジ検知装置により獲得された環境障害物の深さ情報によりマップを作成するプロセス期間に、視覚検知装置が更に実際の環境に対応する画像のブロック特徴データを獲得する。ブロック特徴マップは、レンジ検知装置により提供されたレンジデータおよび画像のブロック特徴データ間の対応関係に従って作成される。ブロック特徴マップが作成された後、移動プラットフォームの定位およびナビゲーションが視覚検知装置により補足された環境特徴を作成されたマップと比較するだけで達成できる。
【００５１】
以上のごとく、この発明を実施形態により開示したが、もとより、この発明を限定するためのものではなく、当業者であれば容易に理解できるように、この発明の技術思想の範囲内において、適当な変更ならびに修正が当然なされうるものであるから、その特許権保護の範囲は、特許請求の範囲および、それと均等な領域を基準として定めなければならない。
【符号の説明】
【００５２】
１０ 移動プラットフォーム
１２ レンジ検知装置
１４ 視覚検知装置
２０ 環境障害物
２００ 埋め込み式手持ちシステム（装置）
【特許請求の範囲】
【請求項１】
環境を走査して環境障害物の深さ情報を獲得することと、
前記環境の画像を捕捉して画像平面を生成することと、
前記環境障害物深さ情報を前記画像平面上に投影して、多数の投影位置を獲得することと、
前記画像平面中の各投影位置周辺の予め決定されたレンジから少なくとも１つの特徴ベクトルを計算することと、
前記環境障害物の深さ情報および各特徴ベクトルを連合して、ある時点に対するサブマップを生成することと、
全時点における前記サブマップを連合してマップを生成することと、を備えるマップ作成方法。
【請求項２】
各投影位置周辺の予め決定されたレンジから前記特徴ベクトルが計算できない時、少なくとも１つの上方特徴ベクトルまたは少なくとも１つの下方特徴ベクトルが、各投影位置の上方または下方の画像データから計算されるものである請求項１記載のマップ作成方法。
【請求項３】
前記上方特徴ベクトルまたは下方特徴ベクトルが、各投影位置の垂直軸における画像平面中の上方または下方画像データに従って計算されるものである請求項２記載のマップ作成方法。
【請求項４】
前記特徴ベクトルが、画像平面上の各投影位置の予め決定されたレンジ中でコーナー点、エッジおよび区域極値に従って決定されるものである請求項１記載のマップ作成方法。
【請求項５】
前記環境障害物の深さ情報および各特徴ベクトルを連合するステップが、
前記環境障害物の深さ情報を世界座標システムへ変換することと、
世界座標システムで表された環境障害物の深さ情報および前記各特徴ベクトルを連合することと、を含む請求項１記載のマップ作成方法。
【請求項６】
前記画像平面中の投影位置に従って特徴ベクトルを計算するステップが、更に、高い相似性を有する特徴ベクトルを破棄するものである請求項１記載のマップ作成方法。
【請求項７】
前記環境を走査するステップが、レーザーレンジファインダーにより実施されるものである請求項１記載のマップ作成方法。
【請求項８】
前記環境の画像を捕捉するステップが、デジタルカメラにより実施されるものである請求項１記載のマップ作成方法。
【請求項９】
画像の環境障害物の深さ情報および前記環境障害物の深さ情報に対応する画像の少なくとも１つの特徴ベクトルを含むマップを獲得することと、
視覚検知装置を使用して環境画像を獲得することと、
前記環境画像から少なくとも１つの画像特徴点を抽出することと、
前記画像特徴点およびマップに従って定位を実施することと、を含む定位方法。
【請求項１０】
前記画像特徴点およびマップに従って定位を実施するステップが、環境画像の画像特徴点の画像座標および世界座標を使用することにより視覚検知装置の世界座標を計算して、定位を履行するものである請求項９記載の定位方法。
【請求項１１】
前記マップを獲得するステップが、
環境を走査して環境障害物の深さ情報を獲得することと、
環境の画像を捕捉して画像平面を生成することと、
前記環境障害物深さ情報を前記画像平面上に投影して、多数の投影位置を獲得することと、
前記画像平面中の各投影位置周辺の予め決定されたレンジから少なくとも１つの特徴ベクトルを計算することと、
前記環境障害物の深さ情報および各特徴ベクトルを連合して、ある時点に対するサブマップを生成することと、
全時点におけるサブマップを連合してマップを生成することと、を備えるものである請求項９記載の定位方法。
【請求項１２】
各投影位置周辺の予め決定されたレンジから特徴ベクトルが計算できない時、少なくとも１つの上方特徴ベクトルまたは少なくとも１つ下方特徴ベクトルが、各投影位置の上方または下方の画像データから計算されるものである請求項１１記載の定位方法。
【請求項１３】
前記上方特徴ベクトルまたは下方特徴ベクトルが、各投影位置の垂直軸における画像平面中の上方または下方画像データに従って計算されるものである請求項１２記載の定位方法。
【請求項１４】
前記特徴ベクトルが、画像平面上の各投影位置の予め決定されたレンジ中でコーナー点、エッジおよび区域極値に従って決定されるものである請求項１１記載の定位方法。
【請求項１５】
前記環境障害物の深さ情報および各特徴ベクトルを連合するステップが、
前記環境障害物の深さ情報を世界座標システムへ変換することと、
世界座標システムで表された環境障害物の深さ情報および前記各特徴ベクトルを連合することと、を含む請求項１１記載の定位方法。
【請求項１６】
環境を走査して環境障害物の深さ情報を生成する、少なくとも１つのレンジ検知装置と、
環境の画像を捕捉して画像平面を生成する、少なくとも１つの視覚検知装置と、
前記レンジ検知装置および視覚検知装置に連結され、前記環境障害物の深さ情報を前記画像平面へ投影して多数の投影位置を獲得し、画像平面中の各投影位置周辺の予め決定されたレンジから少なくとも１つの特徴ベクトルを計算するとともに、前記環境障害物の深さ情報および特徴ベクトルを連合してマップを作成する画像処理装置と、を備えるマップ作成装置。
【請求項１７】
レンジ検知装置が、レーザーレンジファインダーである請求項１６記載のマップ作成装置。
【請求項１８】
視覚検知装置が、デジタルカメラである請求項１６記載のマップ作成装置。
【請求項１９】
前記レンジ検知装置と視覚検知装置と画像処理装置とが、移動プラットフォーム上に配備されるものである請求項１６記載のマップ作成装置。
【請求項２０】
前記レンジ検知装置と視覚検知装置とが、埋め込み式手持ち装置として統合されるものである請求項１６記載のマップ作成装置。
【請求項１】
環境を走査して環境障害物の深さ情報を獲得することと、
前記環境の画像を捕捉して画像平面を生成することと、
前記環境障害物深さ情報を前記画像平面上に投影して、多数の投影位置を獲得することと、
前記画像平面中の各投影位置周辺の予め決定されたレンジから少なくとも１つの特徴ベクトルを計算することと、
前記環境障害物の深さ情報および各特徴ベクトルを連合して、ある時点に対するサブマップを生成することと、
全時点における前記サブマップを連合してマップを生成することと、を備えるマップ作成方法。
【請求項２】
各投影位置周辺の予め決定されたレンジから前記特徴ベクトルが計算できない時、少なくとも１つの上方特徴ベクトルまたは少なくとも１つの下方特徴ベクトルが、各投影位置の上方または下方の画像データから計算されるものである請求項１記載のマップ作成方法。
【請求項３】
前記上方特徴ベクトルまたは下方特徴ベクトルが、各投影位置の垂直軸における画像平面中の上方または下方画像データに従って計算されるものである請求項２記載のマップ作成方法。
【請求項４】
前記特徴ベクトルが、画像平面上の各投影位置の予め決定されたレンジ中でコーナー点、エッジおよび区域極値に従って決定されるものである請求項１記載のマップ作成方法。
【請求項５】
前記環境障害物の深さ情報および各特徴ベクトルを連合するステップが、
前記環境障害物の深さ情報を世界座標システムへ変換することと、
世界座標システムで表された環境障害物の深さ情報および前記各特徴ベクトルを連合することと、を含む請求項１記載のマップ作成方法。
【請求項６】
前記画像平面中の投影位置に従って特徴ベクトルを計算するステップが、更に、高い相似性を有する特徴ベクトルを破棄するものである請求項１記載のマップ作成方法。
【請求項７】
前記環境を走査するステップが、レーザーレンジファインダーにより実施されるものである請求項１記載のマップ作成方法。
【請求項８】
前記環境の画像を捕捉するステップが、デジタルカメラにより実施されるものである請求項１記載のマップ作成方法。
【請求項９】
画像の環境障害物の深さ情報および前記環境障害物の深さ情報に対応する画像の少なくとも１つの特徴ベクトルを含むマップを獲得することと、
視覚検知装置を使用して環境画像を獲得することと、
前記環境画像から少なくとも１つの画像特徴点を抽出することと、
前記画像特徴点およびマップに従って定位を実施することと、を含む定位方法。
【請求項１０】
前記画像特徴点およびマップに従って定位を実施するステップが、環境画像の画像特徴点の画像座標および世界座標を使用することにより視覚検知装置の世界座標を計算して、定位を履行するものである請求項９記載の定位方法。
【請求項１１】
前記マップを獲得するステップが、
環境を走査して環境障害物の深さ情報を獲得することと、
環境の画像を捕捉して画像平面を生成することと、
前記環境障害物深さ情報を前記画像平面上に投影して、多数の投影位置を獲得することと、
前記画像平面中の各投影位置周辺の予め決定されたレンジから少なくとも１つの特徴ベクトルを計算することと、
前記環境障害物の深さ情報および各特徴ベクトルを連合して、ある時点に対するサブマップを生成することと、
全時点におけるサブマップを連合してマップを生成することと、を備えるものである請求項９記載の定位方法。
【請求項１２】
各投影位置周辺の予め決定されたレンジから特徴ベクトルが計算できない時、少なくとも１つの上方特徴ベクトルまたは少なくとも１つ下方特徴ベクトルが、各投影位置の上方または下方の画像データから計算されるものである請求項１１記載の定位方法。
【請求項１３】
前記上方特徴ベクトルまたは下方特徴ベクトルが、各投影位置の垂直軸における画像平面中の上方または下方画像データに従って計算されるものである請求項１２記載の定位方法。
【請求項１４】
前記特徴ベクトルが、画像平面上の各投影位置の予め決定されたレンジ中でコーナー点、エッジおよび区域極値に従って決定されるものである請求項１１記載の定位方法。
【請求項１５】
前記環境障害物の深さ情報および各特徴ベクトルを連合するステップが、
前記環境障害物の深さ情報を世界座標システムへ変換することと、
世界座標システムで表された環境障害物の深さ情報および前記各特徴ベクトルを連合することと、を含む請求項１１記載の定位方法。
【請求項１６】
環境を走査して環境障害物の深さ情報を生成する、少なくとも１つのレンジ検知装置と、
環境の画像を捕捉して画像平面を生成する、少なくとも１つの視覚検知装置と、
前記レンジ検知装置および視覚検知装置に連結され、前記環境障害物の深さ情報を前記画像平面へ投影して多数の投影位置を獲得し、画像平面中の各投影位置周辺の予め決定されたレンジから少なくとも１つの特徴ベクトルを計算するとともに、前記環境障害物の深さ情報および特徴ベクトルを連合してマップを作成する画像処理装置と、を備えるマップ作成装置。
【請求項１７】
レンジ検知装置が、レーザーレンジファインダーである請求項１６記載のマップ作成装置。
【請求項１８】
視覚検知装置が、デジタルカメラである請求項１６記載のマップ作成装置。
【請求項１９】
前記レンジ検知装置と視覚検知装置と画像処理装置とが、移動プラットフォーム上に配備されるものである請求項１６記載のマップ作成装置。
【請求項２０】
前記レンジ検知装置と視覚検知装置とが、埋め込み式手持ち装置として統合されるものである請求項１６記載のマップ作成装置。
【図１Ａ】
【図１Ｂ】
【図２】
【図３】
【図４】
【図５Ａ】
【図５Ｂ】
【図６】
【図７】
【図８】
【図９】
【図１Ｂ】
【図２】
【図３】
【図４】
【図５Ａ】
【図５Ｂ】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２０１１−１１２６４４（Ｐ２０１１−１１２６４４Ａ）
【公開日】平成２３年６月９日（２０１１．６．９）
【国際特許分類】
【外国語出願】
【出願番号】特願２０１０−１４４９８９（Ｐ２０１０−１４４９８９）
【出願日】平成２２年６月２５日（２０１０．６．２５）
【出願人】（３９００２３５８２）財団法人工業技術研究院 (524)
【氏名又は名称原語表記】ＩＮＤＵＳＴＲＩＡＬ ＴＥＣＨＮＯＬＯＧＹ ＲＥＳＥＡＲＣＨ ＩＮＳＴＩＴＵＴＥ
【住所又は居所原語表記】１９５ Ｃｈｕｎｇ Ｈｓｉｎｇ Ｒｄ．，Ｓｅｃ．４，Ｃｈｕｔｕｎｇ，Ｈｓｉｎ−Ｃｈｕ，Ｔａｉｗａｎ Ｒ．Ｏ．Ｃ
【Ｆターム（参考）】
【公開日】平成２３年６月９日（２０１１．６．９）
【国際特許分類】
【出願番号】特願２０１０−１４４９８９（Ｐ２０１０−１４４９８９）
【出願日】平成２２年６月２５日（２０１０．６．２５）
【出願人】（３９００２３５８２）財団法人工業技術研究院 (524)
【氏名又は名称原語表記】ＩＮＤＵＳＴＲＩＡＬ ＴＥＣＨＮＯＬＯＧＹ ＲＥＳＥＡＲＣＨ ＩＮＳＴＩＴＵＴＥ
【住所又は居所原語表記】１９５ Ｃｈｕｎｇ Ｈｓｉｎｇ Ｒｄ．，Ｓｅｃ．４，Ｃｈｕｔｕｎｇ，Ｈｓｉｎ−Ｃｈｕ，Ｔａｉｗａｎ Ｒ．Ｏ．Ｃ
【Ｆターム（参考）】
[ Back to top ]