(57)【要約】【課題】上部映像を利用した移動体の位置推定及び地図の生成装置と方法、並びにその装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体を提供する。【解決手段】移動体の位置した環境の上部を撮影して獲得した上部映像でコーナーを表すコーナー点を観測し、観測されたコーナー点からランドマークを生成するランドマーク生成部、及びランドマークから移動体の位置を推定して地図を生成する位置及び地図演算部を備えることを特徴とする移動体の位置推定及び地図の生成装置である。【選択図】図３
2006209770.tif 000002
【特許請求の範囲】
【請求項１】
移動体の位置した環境の上部を撮影して獲得した上部映像で、コーナーを表すコーナー点を観測し、観測されたコーナー点からランドマークを生成するランドマーク生成部と、
前記ランドマークから前記移動体の位置を推定し、かつ地図を生成する位置及び地図演算部と、を備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項２】
前記ランドマーク生成部は、
現在上部映像で、前記コーナー点の座標及び前記コーナー点を互いに区分する映像特徴周辺記述子のうち、少なくとも一つを利用して観測した前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を見つけ、以前上部映像でランドマークでもなくランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録し、前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補をランドマークとして登録し、
前記現在上部映像は、現在獲得された前記上部映像に該当し、前記以前上部映像とは、以前に獲得された前記上部映像に該当することを特徴とする請求項１に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項３】
前記上部映像を利用した移動体の位置推定及び地図の生成装置は、
前記移動体の走行を制御し、前記走行についての情報を出力する走行処理部をさらに備え、
前記位置及び地図演算部は、前記走行処理部から入力した前記走行についての情報と、前記ランドマーク生成部から入力した前記ランドマークとから移動体の位置を推定し、かつ地図を生成し、前記ランドマークについての情報を前記ランドマーク生成部に出力することを特徴とする請求項２に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項４】
前記ランドマーク生成部は、
前記移動体の位置した環境の上部映像を獲得する映像獲得部と、
前記映像獲得部から入力した前記獲得された映像をバッファリングし、バッファリングされた結果を前記以前上部映像として出力する映像バッファと、
前記映像獲得部で現在獲得された前記現在上部映像のコーナー点の座標及び前記映像特徴周辺記述子のうち、少なくとも一つを映像特徴情報として抽出する映像特徴情報抽出部と、
前記以前上部映像での前記ランドマーク及び前記ランドマーク候補を、前記現在上部映像で予測するランドマーク及びランドマーク候補予測部と、
前記予測ランドマーク及びランドマーク候補についての情報と前記映像特徴情報とを比較し、比較された結果により前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測して見つけ、前記以前上部映像で前記ランドマークでもなく、前記ランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録する観測及び登録部と、
前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補を、前記ランドマークとして登録するランドマーク登録部と、を備えることを特徴とする請求項２に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項５】
前記映像獲得部は、
前記移動体の位置した環境の前記上部映像を獲得する上部映像獲得部と、
前記獲得された上部映像の歪曲を補正する歪曲補正部と、を備えることを特徴とする請求項４に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項６】
前記映像獲得部は、
前記移動体の位置した環境の前記上部映像を獲得する上部映像獲得部と、
前記獲得された上部映像を低域通過フィルタリングする低域通過フィルタと、を備えることを特徴とする請求項４に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項７】
前記映像獲得部は、
前記歪曲が補正された結果を低域通過フィルタリングする低域通過フィルタをさらに備えることを特徴とする請求項５に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項８】
前記歪曲補正部は、
前記獲得された上部映像で関心領域を設定し、設定された関心領域のみに対して前記歪曲を補正することを特徴とする請求項５に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項９】
前記映像特徴情報抽出部は、
前記現在上部映像で前記コーナー点を検出し、前記検出されたコーナー点の座標を出力するコーナー点検出部と、
前記検出されたコーナー点の周辺の映像を分析し、前記分析された結果から前記コーナー点別に前記映像特徴周辺記述子を演算する記述子演算部と、を備えることを特徴とする請求項４に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１０】
前記コーナー点検出部は、下記式のＲ値が０より大きく、Ｒ値が局所最大点となる点を前記コーナー点として決定することを特徴とする請求項９に記載の上部映像を利用した移動体の位置推定及び地図の生成装置：
【数１】
2006209770.tif 000026
（ここで、ｔｒは、トレース演算を表し、ｄｅｔは、行列式演算を表し、Ｍは、下記の通りであり、
2006209770.tif 000027
（ｘ，ｙ）は、前記現在上部映像の映像座標系に対する座標を表し、Ｉ’は、前記現在上部映像の輝度強度を表す）。
【請求項１１】
前記ｋは、０.０４であることを特徴とする請求項１０に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１２】
前記記述子演算部で演算される前記映像特徴周辺記述子は、スケール不変特徴変換記述子であることを特徴とする請求項９に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１３】
前記ランドマーク及びランドマーク候補予測部は、
前記以前上部映像で登録された前記ランドマーク及び前記ランドマーク候補の位置の３次元座標を、前記以前上部映像が入力された時の前記移動体の位置及び方位角を利用して、前記以前上部映像の映像座標系を基準に２次元に変換する映像座標変換部と、
前記映像座標変換部から入力した変換された２次元座標を有する前記ランドマーク及び前記ランドマーク候補のうち、前記以前上部映像に属する前記ランドマーク及び前記ランドマーク候補のみを選択する選択部と、
前記映像獲得部から入力した前記現在上部映像と、前記映像バッファから入力した前記以前上部映像との間の変化量を、前記選択された結果について計算し、前記計算された変化量を利用して、前記現在上部映像にある前記ランドマーク及び前記ランドマーク候補を予測する位置予測部と、を備えることを特徴とする請求項４に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１４】
前記位置予測部は、
オプティカルフロートラッカーを利用して、前記現在上部映像にある前記ランドマーク及び前記ランドマーク候補の位置を予測することを特徴とする請求項１３に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１５】
前記観測及び登録部は、
前記コーナー点のそれぞれに対して、前記コーナー点と最も近い予測ランドマークやランドマーク候補を探すランドマーク及びランドマーク候補検索部と、
前記コーナー点のそれぞれに対して、前記コーナー点と前記検出されたランドマークやランドマーク候補との間の距離を計算する第１距離計算部と、
前記第１距離計算部で計算された距離と第１所定距離とを比較する第１比較部と、
前記第１比較部で比較された結果に応答して、前記コーナー点の映像特徴周辺記述子と、前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子との間の距離を計算する第２距離計算部と、
前記第２距離計算部で計算された距離と第２所定距離とを比較する第２比較部と、
前記第２比較部で比較された結果に応答して、前記コーナー点と最も近い位置に前記ランドマークが存在するか、それとも前記ランドマーク候補が存在するかを識別するランドマーク及びランドマーク候補識別部と、
前記識別された結果に応答して、前記ランドマークが観測された回数を表すランドマークフラグをアッパーカウンティングするか、前記ランドマーク候補が観測された回数を表すランドマーク候補フラグをアッパーカウンティングするカウンターと、
前記第１比較部及び前記第２比較部で比較された結果に応答して、前記コーナー点のうち、前記以前上部映像で前記ランドマークでもなく、前記ランドマーク候補でもなかったコーナー点を、前記新たなランドマーク候補として見つけ、検出された前記新たなランドマーク候補についての情報を登録するランドマーク候補初期化部と、を備えることを特徴とする請求項４に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１６】
前記ランドマーク生成部は、
前記登録されたランドマーク候補のうち、互いに重複しているランドマーク候補を除去し、重複していないランドマーク候補等の２次元映像座標から３次元映像座標を推定するランドマーク候補処理部をさらに備え、
前記ランドマーク登録部は、前記重複していないランドマーク候補のうち、前記所定回数以上前記コーナー点として観測されたランドマーク候補を前記ランドマークとして登録することを特徴とする請求項４に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１７】
前記ランドマーク候補処理部は、
前記登録されたランドマーク候補のそれぞれに対して、最も近いランドマーク候補間の最小距離を計算する第３距離計算部と、
前記各最小距離と第３所定距離とを比較する第３比較部と、
前記第３比較部で比較された結果に応答して、前記各ランドマーク候補が重複しているか否かを決定し、決定された結果に相応して、重複しているランドマーク候補を除去するランドマーク候補除去部と、
前記第３比較部で比較された結果に応答して、前記重複していないランドマーク候補の３次元映像座標を推定する座標推定部と、を備えることを特徴とする請求項１６に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１８】
前記位置及び地図演算部は、拡張されたカルマンフィルタを利用して、前記ランドマークから前記移動体の前記位置を推定し、かつ前記地図を生成することを特徴とする請求項１に記載の上部映像を利用した移動体の位置推定及び地図の生成装置。
【請求項１９】
（ａ）移動体の位置した環境の上部を撮影して獲得した上部映像でコーナーを表すコーナー点を観測し、観測されたコーナー点からランドマークを生成するステップと、
（ｂ）前記ランドマークから前記移動体の位置を推定し、かつ地図を生成するステップと、を含むことを特徴とする移動体の位置推定及び地図生成方法。
【請求項２０】
前記（ａ）ステップは、
現在上部映像で前記コーナー点の座標及び前記コーナー点を互い区分させる映像特徴周辺記述子のうち、少なくとも一つを利用して、前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測し、以前上部映像でランドマークでもなくランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録し、前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補をランドマークとして登録し、
前記現在上部映像は、現在獲得された前記上部映像に該当し、前記以前上部映像は、以前に獲得された前記上部映像に該当することを特徴とする請求項１９に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２１】
前記（ａ）ステップは、
前記移動体の位置した環境の上部映像を獲得するステップと、
前記獲得された映像をバッファリングし、バッファリングされた結果を前記以前上部映像として決定するステップと、
前記現在上部映像のコーナー点の座標及び前記映像特徴周辺記述子のうち、少なくとも一つを映像特徴情報として抽出するステップと、
前記以前上部映像での前記ランドマーク及び前記ランドマーク候補を前記現在上部映像で予測するステップと、
前記予測ランドマーク及びランドマーク候補についての情報と前記映像特徴情報とを比較し、比較された結果を利用して、前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測して見つけ、前記以前上部映像で前記ランドマークでもなく、前記ランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録するステップと、
前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補を前記ランドマークとして登録するステップと、を含むことを特徴とする請求項２０に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２２】
前記映像を獲得するステップは、
前記移動体の位置した環境の前記上部映像を獲得するステップと、
前記獲得された上部映像の歪曲を補正するステップと、を含むことを特徴とする請求項２１に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２３】
前記映像を獲得するステップは、
前記移動体の位置した環境の前記上部映像を獲得するステップと、
前記獲得された上部映像を低域通過フィルタリングするステップと、を含むことを特徴とする請求項２１に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２４】
前記映像を獲得するステップは、
前記歪曲が補正された結果を低域通過フィルタリングするステップをさらに含むことを特徴とする請求項２２に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２５】
前記映像特徴情報を抽出するステップは、
前記現在上部映像で前記コーナー点を検出し、前記検出されたコーナー点の座標を求めるステップと、
前記検出されたコーナー点の周辺に映像を分析し、前記分析された結果を利用して、前記コーナー点別に前記映像特徴周辺記述子を求めるステップと、を含むことを特徴とする請求項２１に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２６】
前記ランドマーク及び前記ランドマーク候補の位置を前記現在上部映像で予測するステップは、
前記以前上部映像で登録された前記ランドマーク及び前記ランドマーク候補の位置の３次元座標を、前記以前上部映像が入力された時の前記移動体の位置及び方位角を利用して、前記以前上部映像の映像座標系を基準に２次元に変換するステップと、
前記変換された２次元座標を有する前記ランドマーク及び前記ランドマーク候補のうち、前記以前上部映像に属する前記ランドマーク及び前記ランドマーク候補のみを選択するステップと、
前記現在上部映像と前記以前上部映像との間の変化量を、前記選択された結果について計算し、前記計算された変化量を利用して、前記現在上部映像にある前記ランドマーク及び前記ランドマーク候補を予測するステップと、を含むことを特徴とする請求項２１に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２７】
前記コーナー点を観測して、前記新たなランドマーク候補を登録するステップは、
前記コーナー点のそれぞれに対して、前記コーナー点と最も近い予測ランドマークやランドマーク候補を探すステップと、
前記コーナー点のそれぞれに対して、前記コーナー点と前記検出されたランドマークやランドマーク候補との間の距離が第１所定距離以下であるか否かを判断するステップと、
前記コーナー点と前記検出されたランドマークやランドマーク候補との距離が前記第１所定距離以下であるコーナー点のそれぞれに対して、前記コーナー点の映像特徴周辺記述子と、前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子との間の距離が第２所定距離以下であるか否かを判断するステップと、
前記コーナー点の映像特徴周辺記述子と、前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子との間の距離が前記第２所定距離以下であると判断されれば、前記コーナー点と最も近い位置に前記ランドマークが存在するか、それとも前記ランドマーク候補が存在するかを判断するステップと、
前記ランドマークが存在すると判断されれば、前記ランドマークが観測された回数を表すランドマークフラグをアッパーカウンティングするステップと、
前記ランドマーク候補が存在すると判断されれば、前記ランドマーク候補が観測された回数を表すランドマーク候補フラグをアッパーカウンティングするステップと、
前記検出されたランドマークやランドマーク候補及び前記第１所定距離より遠く位置したコーナー点、または前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子及び前記第２所定距離より遠く位置した前記映像特徴周辺記述子に該当するコーナー点を、前記新たなランドマーク候補として決定し、決定された前記新たなランドマーク候補についての情報を登録するステップと、を含むことを特徴とする請求項２１に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２８】
前記（ａ）ステップは、
前記登録されたランドマーク候補のうち、互いに重複しているランドマーク候補を除去し、重複していないランドマーク候補の２次元映像座標から３次元映像座標を推定するステップをさらに備えることを特徴とする請求項２１に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項２９】
前記映像座標を推定するステップは、
前記登録されたランドマーク候補のそれぞれに対して、最も近いランドマーク候補間の最小距離を求めるステップと、
前記最小距離が第３所定距離以下であるか否かを判断するステップと、
前記第３所定距離以下の最小距離を有するランドマーク候補は互いに重複していると決定し、重複しているランドマーク候補を除去するステップと、
前記第３所定距離より遠く離れた最小距離を有するランドマーク候補は互いに重複していないと決定し、前記重複していないランドマーク候補の３次元映像座標を推定するステップと、を含むことを特徴とする請求項２８に記載の上部映像を利用した移動体の位置推定及び地図生成方法。
【請求項３０】
請求項１９〜２９のいずれかに記載の上部映像を利用した移動体の位置推定及び地図生成方法を実行するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置推定及び地図の生成装置とその方法、並びにその装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体に関する。
【背景技術】
【０００２】
従来、ロボットの地図生成方法は、スキャンマッチング（scan matching）方式を利用して非常に正確な地図が得られるが、地図を獲得するためにレーザーレンジファインダー（laser range finder）のような非常に高価なセンサーを要求するという問題点がある。このような従来の方法の一つが、非特許文献１に発表された論文に開示されている。ここに開示された従来の方法は、レーザーレンジファインダーの他にも、天井映像（ceiling images）を利用して地図を生成する方法を提案しているが、天井映像を全て完全に保存せねばならないため、多くの演算量を要求するだけでなく、ロボットの位置認識及び地図生成を同時に行えないという問題点がある。従来の方法の他の一つが、非特許文献２に発表された論文に開示されている。ここに開示された従来の方法は、主に野外で使用されるように考案された方法であり、地図作成及び位置認識を同時に行えるが、それはまた、高価なレーザーレンジファインダーを利用するという問題点がある。さらに他の従来の方法が、非特許文献３に発表された論文及び特許文献１に開示されている。ここに開示された従来の方法も、ロボットの位置認識及び地図生成を同時に行えないという問題点を有する。
【０００３】
また、従来、ロボットの位置を認識すると共に地図を生成する方法が、非特許文献４に発表された論文に開示されている。ここに、開示された従来の方法は、照明に敏感に影響を受けるという問題点を有する。さらに他の従来の方法が、非特許文献５に発表された論文に開示されている。ここに開示された従来の方法は、２台のカメラを利用せねばならないだけではなく、照明の変化にも敏感であり、位置及び地図を正確に認識して生成できないという問題点を有する。
【特許文献１】米国特許ＵＳ５,７９３,９３４、“Ｍｅｔｈｏｄ  ｆｏｒ  ｔｈｅ  ｏｒｉｅｎｔａｔｉｏｎ、ｒｏｕｔｅｐｌａｎｎｉｎｇ  ａｎｄ  ｃｏｎｔｒｏｌ  ｏｆ  ａｎ  ａｕｔｏｎｏｍｏｕｓ  ｍｏｂｉｌｅ  ｕｎｉｔ”
【非特許文献１】“Ｒｏｂｕｓｔ  Ｍｏｎｔｅ  Ｃａｒｌｏ  Ｌｏｃａｌｉｚａｔｉｏｎ  ｆｏｒ  Ｍｏｂｉｌｅ  Ｒｏｂｏｔｓ”、Ｓ．Ｔｈｒｕｎ、Ｄ．Ｆｏｘ、Ｗ．Ｂｕｒｇａｒｄ及びＦ．Ｄｅｌｌａｅｒｔ、Ａｒｔｉｆｉｃｉａｌ  Ｉｎｔｅｌｌｉｇｅｎｃｅ，Ｖｏｌ．１２８，Ｎｏ．１〜２，２０００、ｐｐ.９９〜１４１
【非特許文献２】“Ｓｉｍｕｌｔａｎｅｏｕｓ  ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  ｍａｐ  ｂｕｉｌｄｉｎｇ  ｕｓｉｎｇ  ｎａｔｕｒａｌ  ｆｅａｔｕｒｅｓ  ａｎｄ  ａｂｓｏｌｕｔｅ  ｉｎｆｏｒｍａｔｉｏｎ”、Ｊｏｓｅ  Ｇｕｉｖａｎｔ、Ｆａｖｉｏ  Ｍａｓｓｏｎ及びＥｄｕａｒｄｏ  Ｎｅｂｏｔ、Ｒｏｂｏｔｉｃｓａｎｄ  Ａｕｔｏｎｏｍｏｕｓ  Ｓｙｓｔｅｍｓ，９８４（２００２）。ｐｐ.１〜１２
【非特許文献３】“Ｖｉｓｕａｌ  Ｃｏｒｒｅｃｔｉｏｎ  ｏｆ  Ｏｒｉｅｎｔａｔｉｏｎ  Ｅｒｒｏｒ  ｆｏｒａ  Ｍｏｂｉｌｅ  Ｒｏｂｏｔ”、Ｓ．Ｌａｎｇ、Ｆ．Ｙｉｌｉ及びＳ．Ｋ．Ｔｓｏ、Ｐｒｏｃ．ｏｆ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆ．ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｒｏｂｏｔｓ  ａｎｄ  Ｓｙｓｔｅｍｓ，１９９９、ｐｐ.１３２８〜１３３３
【非特許文献４】“Ｒｅａｌ−Ｔｉｍｅ  ３Ｄ  ＳＬＡＭ  ｗｉｔｈ  Ｗｉｄｅ−Ａｎｇｌｅ  Ｖｉｓｉｏｎ”、Ａｎｄｒｅｗ  Ｊ．Ｄａｖｉｓｏｎ、Ｙｏｌａｎｄａ  Ｇｏｎｚａｌｅｚ  Ｃｉｄ及びＮｏｂｕｙｕｋｉ  Ｋｉｔａ、Ｐｒｏｃ．ＩＦＡＣ  Ｓｙｍｐｏｓｉｕｍ  ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ａｕｔｏｎｏｍｏｕｓ  Ｖｅｈｉｃｌｅｓ，２００４
【非特許文献５】“Ｍｏｂｉｌｅ  Ｒｏｂｏｔ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｐｉｎｇ  ｗｉｔｈ  Ｕｎｃｅｒｔａｉｎｔｙ  ｕｓｉｎｇ  Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ  Ｖｉｓｕａｌ  Ｌａｎｄｍａｒｋｓ”、Ｓｔｅｐｈｅｎ  Ｓｅ、Ｄａｖｉｄ  Ｌｏｗｅ及びＪｉｍ  Ｌｉｔｔｌｅ、Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｊｏｕｒｎａｌ  ｏｆ  Ｒｏｂｏｔｉｃｓ  Ｒｅｓｅａｒｃｈ，Ｖｏｌｕｍｅ  ２１，Ｎｕｍｂｅｒ  ８，Ａｕｇｕｓｔ  ２００２、ｐｐ.７３５〜７５８
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上述のように、非特許文献１の方法では、天井映像を用いることでより演算量が多くなり、ロボットの位置認識及び地図生成を同時に行えない。また、非特許文献２の方法では、地図作成及び位置認識を同時に行えるが、高価なレーザーレンジファインダーが必要となってしまう。非特許文献３及び特許文献１の方法では、ロボットの位置認識及び地図生成を同時に行えないという問題点を有する。非特許文献４の方法では、地図作成及び位置認識を同時に行えるが、照明に対して敏感に影響を受けて位置及び地図を正確に認識して生成できない。さらに、非特許文献５の方法では、２台のカメラを利用せねばならないだけではなく、照明の変化にも敏感であり、位置及び地図を正確に認識して生成できない。
【０００５】
そこで、本発明が達成しようとする技術的課題は、移動体の位置を正確に認識するとともに地図作成を正確に行うことが可能な移動体の位置推定及び地図の生成装置、その方法、並びにその装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体を提供するところにある。
【課題を解決するための手段】
【０００６】
前記課題を達成するための本願第１発明による上部（upper）映像を利用した移動体の位置推定及び地図の生成装置は、移動体の位置した環境の上部を撮影して獲得した上部映像でコーナーを表すコーナー点を観測し、観測されたコーナー点からランドマークを生成するランドマーク生成部、及び前記ランドマークから前記移動体の位置を推定して地図を生成する位置及び地図演算部から構成されることが好ましい。
【０００７】
コーナー点を観測してランドマークを生成するため、上部映像そのものを保存したり演算することなくランドマークを生成することができ、演算量を減らすことができる。演算量が減少するため、生成したランドマークからの位置の推定及び地図の作成をともに行うことが可能となる。
また、カメラなどを用いて上部映像を取得することでランドマークを生成するため、レーザーレンジファインダーのような高価なカメラが不要である。このとき、上部映像を取得するために、安価の一台のカメラのみを利用すれば良いため、製造コストを低減させることができる。
【０００８】
上述の通り、ランドマークは、上部映像に基づいて自動的に生成される。よって、例えば、赤外線ランドマーク、色パターンのような人工ランドマークなどの別途の付着物によりランドマークを生成する必要がない。このような別途の付着物によるランドマークを用いた場合には、これらのランドマークをセンシングする際に、照明などの影響を受けてランドマークを正確に取得することが出来ない場合がある。しかし、本発明のように、上部映像に基づいてランドマークを生成する場合には、前述のような照明の影響を受けることが無く正確にランドマークを生成でき、正確な位置の推定及び正確な地図作製を行うことができる。
【０００９】
第２発明は、第１発明において、前記ランドマーク生成部は、現在上部映像で、前記コーナー点の座標及び前記コーナー点を互いに区分する映像特徴周辺記述子のうち、少なくとも一つを利用して観測した前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を見つけ、以前上部映像でランドマークでもなくランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録し、前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補をランドマークとして登録し、前記現在上部映像は、現在獲得された前記上部映像に該当し、前記以前上部映像とは、以前に獲得された前記上部映像に該当することを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００１０】
コーナー点や映像特徴周辺記述子を用いてランドマークを生成するため、上部映像そのものを用いる場合よりも大幅に演算量を削減することができる。
ランドマークの候補を新たに登録する時、コーナー点の特徴を表すＳＩＦＴ（Ｓｃａｌｅ  Ｉｎｖａｒｉａｎｔ  Ｆｅａｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ）記述子のような映像特徴周辺記述子を利用する場合には、移動体の位置した室内環境の全体の照度が変化するとしても、映像の輝度変化は一定である。よって、移動体の位置した照明の変化にロバスト性を有するランドマークを見つけて登録できる。これにより移動体の位置認識及び地図作成を正確に行わせることができる。例えば、移動体の平均距離誤差を約６ｃｍ以内に維持し、方位角の平均誤差を約２度以内に維持できる。ここで、ロバスト性とは，外乱や設計誤差などの不確定な変動に対して，システム特性が現状を維持できることを意味する。
【００１１】
第３発明は、第２発明において、前記上部映像を利用した移動体の位置推定及び地図の生成装置は、前記移動体の走行を制御し、前記走行についての情報を出力する走行処理部をさらに備え、前記位置及び地図演算部は、前記走行処理部から入力した前記走行についての情報と、前記ランドマーク生成部から入力した前記ランドマークとから移動体の位置を推定し、かつ地図を生成し、前記ランドマークについての情報を前記ランドマーク生成部に出力することを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００１２】
位置及び地図演算部は、走行処理部からの走行についての情報を取得することで、移動体の位置を正確に取得し、また正確な地図を生成することができる。
第４発明は、第２発明において、前記ランドマーク生成部は、前記移動体の位置した環境の上部映像を獲得する映像獲得部と、前記映像獲得部から入力した前記獲得された映像をバッファリングし、バッファリングされた結果を前記以前上部映像として出力する映像バッファと、前記映像獲得部で現在獲得された前記現在上部映像のコーナー点の座標及び前記映像特徴周辺記述子のうち、少なくとも一つを映像特徴情報として抽出する映像特徴情報抽出部と、前記以前上部映像での前記ランドマーク及び前記ランドマーク候補を、前記現在上部映像で予測するランドマーク及びランドマーク候補予測部と、前記予測ランドマーク及びランドマーク候補についての情報と前記映像特徴情報とを比較し、比較された結果により前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測して見つけ、前記以前上部映像で前記ランドマークでもなく、前記ランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録する観測及び登録部と、前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補を、前記ランドマークとして登録するランドマーク登録部と、を備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００１３】
現在上部映像のコーナー点を随時検出し、そのコーナー点のうち、以前上部映像でランドマーク及びランドマーク候補でもなかったコーナー点を新たなランドマーク候補として登録することで、随時新たなランドマークを生成することができる。
第５発明は、第４発明において、前記映像獲得部は、前記移動体の位置した環境の前記上部映像を獲得する上部映像獲得部と、前記獲得された上部映像の歪曲を補正する歪曲補正部と、を備えることを特徴とする。
【００１４】
第６発明は、第４発明において、前記映像獲得部は、前記移動体の位置した環境の前記上部映像を獲得する上部映像獲得部と、前記獲得された上部映像を低域通過フィルタリングする低域通過フィルタと、を備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
第７発明は、第５発明において、前記映像獲得部は、前記歪曲が補正された結果を低域通過フィルタリングする低域通過フィルタをさらに備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００１５】
第８発明は、第５発明において、前記歪曲補正部は、前記獲得された上部映像で関心領域を設定し、設定された関心領域のみに対して前記歪曲を補正することを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
設定された関心領域のみに対して歪曲を補正することで、演算量を削減することができる。
【００１６】
第９発明は、第４発明において、前記映像特徴情報抽出部は、前記現在上部映像で前記コーナー点を検出し、前記検出されたコーナー点の座標を出力するコーナー点検出部と、前記検出されたコーナー点の周辺に映像を分析し、前記分析された結果から前記コーナー点別に前記映像特徴周辺記述子を演算する記述子演算部と、を備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００１７】
第１０発明は、第９発明において、前記コーナー点検出部は、下記式のＲ値が０より大きく、Ｒ値が局所最大点となる点を前記コーナー点として決定することを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する
【００１８】
【数１】
2006209770.tif 000003
（ここで、ｔｒは、トレース演算を表し、ｄｅｔは、行列式演算を表し、Ｍは、下記の通りであり、
2006209770.tif 000004
（ｘ，ｙ）は、前記現在上部映像の映像座標系に対する座標を表し、Ｉ’は、前記現在上部映像の輝度強度を表す）。
【００１９】
第１１発明は、第１０発明において、前記ｋは、０.０４であることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
第１２発明は、第９発明において、前記記述子演算部で演算される前記映像特徴周辺記述子は、スケール不変特徴変換記述子であることを特徴とする請求項９に記載の上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００２０】
スケール不変特徴変換（ＳＩＦＴ：Ｓｃａｌｅ  Ｉｎｖａｒｉａｎｔ  Ｆｅａｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ）記述子を用いることで、コーナー点の周辺の映像の輝度変化分布などを利用してコーナー点の特徴を表現することができる。
第１３発明は、第４発明において、前記ランドマーク及びランドマーク候補予測部は、前記以前上部映像で登録された前記ランドマーク及び前記ランドマーク候補の位置の３次元座標を、前記以前上部映像が入力された時の前記移動体の位置及び方位角を利用して、前記以前上部映像の映像座標系を基準に２次元に変換する映像座標変換部と、前記映像座標変換部から入力した変換された２次元座標を有する前記ランドマーク及び前記ランドマーク候補のうち、前記以前上部映像に属する前記ランドマーク及び前記ランドマーク候補のみを選択する選択部と、前記映像獲得部から入力した前記現在上部映像と、前記映像バッファから入力した前記以前上部映像との間の変化量を、前記選択された結果について計算し、前記計算された変化量を利用して、前記現在上部映像にある前記ランドマーク及び前記ランドマーク候補を予測する位置予測部と、を備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００２１】
第１４発明は、第１３発明において、前記位置予測部は、オプティカルフロートラッカーを利用して、前記現在上部映像にある前記ランドマーク及び前記ランドマーク候補の位置を予測することを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
オプティカルフロートラックキングと映像特徴周辺記述子とを何れも利用する場合には、従来のＳＬＡＭ技法の問題であるデータ関連問題、すなわち、Ｌａｒｇｅ  ｃｌｏｓｅｄ  ｌｏｏｐ問題を解決でき、ＥＫＦ基盤のＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｔｉｏｎ  Ａｎｄ  Ｍａｐ  Ｂｕｉｌｄｉｎｇ）技法の安定性を確保できる。
【００２２】
第１５発明は、第４発明において、前記観測及び登録部は、前記コーナー点のそれぞれに対して、前記コーナー点と最も近い予測ランドマークやランドマーク候補を探すランドマーク及びランドマーク候補検索部と、前記コーナー点のそれぞれに対して、前記コーナー点と前記検出されたランドマークやランドマーク候補との間の距離を計算する第１距離計算部と、前記第１距離計算部で計算された距離と第１所定距離とを比較する第１比較部と、前記第１比較部で比較された結果に応答して、前記コーナー点の映像特徴周辺記述子と、前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子との間の距離を計算する第２距離計算部と、前記第２距離計算部で計算された距離と第２所定距離とを比較する第２比較部と、前記第２比較部で比較された結果に応答して、前記コーナー点と最も近い位置に前記ランドマークが存在するか、それとも前記ランドマーク候補が存在するかを識別するランドマーク及びランドマーク候補識別部と、前記識別された結果に応答して、前記ランドマークが観測された回数を表すランドマークフラグをアッパーカウンティングするか、前記ランドマーク候補が観測された回数を表すランドマーク候補フラグをアッパーカウンティングするカウンターと、前記第１比較部及び前記第２比較部で比較された結果に応答して、前記コーナー点のうち、前記以前上部映像で前記ランドマークでもなく、前記ランドマーク候補でもなかったコーナー点を、前記新たなランドマーク候補として見つけ、検出された前記新たなランドマーク候補についての情報を登録するランドマーク候補初期化部と、を備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００２３】
第１６発明は、第４発明において、前記ランドマーク生成部は、前記登録されたランドマーク候補のうち、互いに重複しているランドマーク候補を除去し、重複していないランドマーク候補等の２次元映像座標から３次元映像座標を推定するランドマーク候補処理部をさらに備え、前記ランドマーク登録部は、前記重複していないランドマーク候補のうち、前記所定回数以上前記コーナー点として観測されたランドマーク候補を前記ランドマークとして登録することを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００２４】
第１７発明は、第１６発明において、前記ランドマーク候補処理部は、前記登録されたランドマーク候補のそれぞれに対して、最も近いランドマーク候補間の最小距離を計算する第３距離計算部と、前記各最小距離と第３所定距離とを比較する第３比較部と、前記第３比較部で比較された結果に応答して、前記各ランドマーク候補が重複しているか否かを決定し、決定された結果に相応して、重複しているランドマーク候補を除去するランドマーク候補除去部と、前記第３比較部で比較された結果に応答して、前記重複していないランドマーク候補の３次元映像座標を推定する座標推定部と、を備えることを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
【００２５】
第１８発明は、第１発明において、前記位置及び地図演算部は、拡張されたカルマンフィルタを利用して、前記ランドマークから前記移動体の前記位置を推定し、かつ前記地図を生成することを特徴とする上部映像を利用した移動体の位置推定及び地図の生成装置を提供する。
前記他の課題を達成するための本願第１９発明による上部映像を利用した移動体の位置推定及び地図生成方法は、移動体の位置した環境の上部を撮影して獲得した上部映像でコーナーを表すコーナー点を観測し、観測されたコーナー点からランドマークを生成するステップ、及び前記ランドマークから前記移動体の位置を推定して地図を生成するステップからなることが好ましい。
【００２６】
第２０発明は、第１９発明において、前記（ａ）ステップは、現在上部映像で前記コーナー点の座標及び前記コーナー点を互い区分させる映像特徴周辺記述子のうち、少なくとも一つを利用して、前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測し、以前上部映像でランドマークでもなくランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録し、前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補をランドマークとして登録し、前記現在上部映像は、現在獲得された前記上部映像に該当し、前記以前上部映像は、以前に獲得された前記上部映像に該当することを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
【００２７】
第２１発明は、第２０発明において、前記（ａ）ステップは、前記移動体の位置した環境の上部映像を獲得するステップと、前記獲得された映像をバッファリングし、バッファリングされた結果を前記以前上部映像として決定するステップと、前記現在上部映像のコーナー点の座標及び前記映像特徴周辺記述子のうち、少なくとも一つを映像特徴情報として抽出するステップと、前記以前上部映像での前記ランドマーク及び前記ランドマーク候補を前記現在上部映像で予測するステップと、前記予測ランドマーク及びランドマーク候補についての情報と前記映像特徴情報とを比較し、比較された結果を利用して、前記コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測して見つけ、前記以前上部映像で前記ランドマークでもなく、前記ランドマーク候補でもなかった前記コーナー点を新たなランドマーク候補として登録するステップと、前記ランドマーク候補のうち、所定回数以上前記コーナー点として観測されたランドマーク候補を前記ランドマークとして登録するステップと、を含むことを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
【００２８】
第２２発明は、第２１発明において、前記映像を獲得するステップは、前記移動体の位置した環境の前記上部映像を獲得するステップと、前記獲得された上部映像の歪曲を補正するステップと、を含むことを特徴とする記載の上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
第２３発明は、第２１発明において、前記映像を獲得するステップは、前記移動体の位置した環境の前記上部映像を獲得するステップと、前記獲得された上部映像を低域通過フィルタリングするステップと、を含むことを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
【００２９】
第２４発明は、第２２発明において、前記映像を獲得するステップは、前記歪曲が補正された結果を低域通過フィルタリングするステップをさらに含むことを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法。
第２５発明は、第２１発明において、前記映像特徴情報を抽出するステップは、前記現在上部映像で前記コーナー点を検出し、前記検出されたコーナー点の座標を求めるステップと、前記検出されたコーナー点の周辺に映像を分析し、前記分析された結果を利用して、前記コーナー点別に前記映像特徴周辺記述子を求めるステップと、を含むことを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
【００３０】
第２６発明は、第２１発明において、前記ランドマーク及び前記ランドマーク候補の位置を前記現在上部映像で予測するステップは、前記以前上部映像で登録された前記ランドマーク及び前記ランドマーク候補の位置の３次元座標を、前記以前上部映像が入力された時の前記移動体の位置及び方位角を利用して、前記以前上部映像の映像座標系を基準に２次元に変換するステップと、前記変換された２次元座標を有する前記ランドマーク及び前記ランドマーク候補のうち、前記以前上部映像に属する前記ランドマーク及び前記ランドマーク候補のみを選択するステップと、前記現在上部映像と前記以前上部映像との間の変化量を、前記選択された結果について計算し、前記計算された変化量を利用して、前記現在上部映像にある前記ランドマーク及び前記ランドマーク候補を予測するステップと、を含むことを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法。
【００３１】
第２７発明は、第２１発明において、前記コーナー点を観測して、前記新たなランドマーク候補を登録するステップは、前記コーナー点のそれぞれに対して、前記コーナー点と最も近い予測ランドマークやランドマーク候補を探すステップと、前記コーナー点のそれぞれに対して、前記コーナー点と前記検出されたランドマークやランドマーク候補との間の距離が第１所定距離以下であるか否かを判断するステップと、前記コーナー点と前記検出されたランドマークやランドマーク候補との距離が前記第１所定距離以下であるコーナー点のそれぞれに対して、前記コーナー点の映像特徴周辺記述子と、前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子との間の距離が第２所定距離以下であるか否かを判断するステップと、前記コーナー点の映像特徴周辺記述子と、前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子との間の距離が前記第２所定距離以下であると判断されれば、前記コーナー点と最も近い位置に前記ランドマークが存在するか、それとも前記ランドマーク候補が存在するかを判断するステップと、前記ランドマークが存在すると判断されれば、前記ランドマークが観測された回数を表すランドマークフラグをアッパーカウンティングするステップと、前記ランドマーク候補が存在すると判断されれば、前記ランドマーク候補が観測された回数を表すランドマーク候補フラグをアッパーカウンティングするステップと、前記検出されたランドマークやランドマーク候補及び前記第１所定距離より遠く位置したコーナー点、または前記検出されたランドマークやランドマーク候補の映像特徴周辺記述子及び前記第２所定距離より遠く位置した前記映像特徴周辺記述子に該当するコーナー点を、前記新たなランドマーク候補として決定し、決定された前記新たなランドマーク候補についての情報を登録するステップと、を含むことを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
【００３２】
第２８発明は、第２１発明において、前記（ａ）ステップは、前記登録されたランドマーク候補のうち、互いに重複しているランドマーク候補を除去し、重複していないランドマーク候補の２次元映像座標から３次元映像座標を推定するステップをさらに備えることを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
第２９発明は、第２８発明において、前記映像座標を推定するステップは、前記登録されたランドマーク候補のそれぞれに対して、最も近いランドマーク候補間の最小距離を求めるステップと、前記最小距離が第３所定距離以下であるか否かを判断するステップと、前記第３所定距離以下の最小距離を有するランドマーク候補は互いに重複していると決定し、重複しているランドマーク候補を除去するステップと、前記第３所定距離より遠く離れた最小距離を有するランドマーク候補は互いに重複していないと決定し、前記重複していないランドマーク候補の３次元映像座標を推定するステップと、を含むことを特徴とする上部映像を利用した移動体の位置推定及び地図生成方法を提供する。
【００３３】
前記さらに他の課題を達成するためのコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体は、発明１９〜２０のいずれかに記載の上部映像を利用した移動体の位置推定及び地図生成方法を実行するコンピュータプログラムを保存する。
【発明の効果】
【００３４】
本発明によれば、移動体の位置を正確に認識するとともに地図作成を正確に行うことが可能な移動体の位置推定及び地図の生成装置、その方法、並びにその装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体を提供することができる。
【発明を実施するための最良の形態】
【００３５】
以下、添付した図面を参照して、本発明による上部（upper）映像を利用した移動体の位置推定及び地図の生成装置及びその装置の実施形態のそれぞれの構成と動作、並びにその装置と、その実施形態で行われる上部映像を利用した移動体の位置推定及び地図生成の方法とを説明すれば、次の通りである。
図１は、本発明による上部映像を利用して、移動体の位置を推定し、かつ移動体周辺の地図を生成するための装置に係る実施形態のブロック図である。図１に示す装置は、ランドマーク生成部１０、位置及び地図演算部１２及び走行処理部１４から構成される。
【００３６】
図２は、本発明による上部映像を利用した移動体の位置推定及び地図生成方法の実施形態を説明するためのフローチャートである。図２に示すフローチャートは、ランドマーク（landmark）を生成するステップ（ステップ２０）及び位置を推定して地図を生成するステップ（ステップ２２）からなる。
図１に示すランドマーク生成部１０は、上部映像でコーナー点を観測し、観測されたコーナー点から、目印や境界線となるランドマークを生成する（ステップ２０）。この時、ランドマークは、自動的に生成されうる。
【００３７】
本発明によれば、ステップ２０でランドマーク生成部１０は、現在の上部映像から観測されたコーナー点の座標及び映像特徴周辺記述子（Ｌｏｃａｌ  Ｆｅａｔｕｒｅ  Ｄｅｓｃｒｉｐｔｏｒ：以下、ＬＦＤ）のうち、少なくとも一つを利用して、観測されたコーナー点のうち以前の上部映像でランドマークやランドマーク候補であったコーナー点を検出する。そして、現在の上部映像のうち、以前の上部映像でランドマークやランドマーク候補ではなかったコーナー点を新たなランドマーク候補として登録する。さらに、既に登録されたランドマーク候補のうち、所定回数以上コーナー点として観測されたランドマーク候補をランドマークとして登録し、登録されたランドマークを位置及び地図演算部１２に出力する（ステップ２０）。
【００３８】
ここで、コーナー点とは、上部映像においてコーナーを表す点を意味し、ＬＦＤとは、コーナー点の周辺映像を分析した結果を利用して、コーナー点そのものの特徴を表すデータであって、コーナー点を互いに区分する役割を行う。上部映像とは、移動体（図示せず）のある室内環境の上部に対して、移動体に付着された、上部を見渡すカメラ（図示せず）で獲得された映像を意味する。現在上部映像とは、現在、獲得された上部映像を意味し、以前上部映像とは、以前に獲得して、後述される図３に示す映像バッファ３２に保存された映像を意味する。ここで、上部に対して獲得された映像は、移動体の位置した室内環境の天井だけでなく、その天井に隣接した壁面も含みうる。移動体とは、ロボットのように、映像を撮影しつつ移動できる物体を意味する。また、ランドマークとは、現在の上部映像のうち、以前の上部映像でランドマークやランドマーク候補ではなかったコーナー点が、所定回数以上ランドマーク候補として登録されたものである。一方、ランドマーク候補とは、所定回数未満しか登録されていないものである。
【００３９】
図３は、図１に示すランドマーク生成部１０の本発明による好ましい実施形態１０Ａのブロック図であって、映像獲得部３０、映像バッファ３２、ランドマーク及びランドマーク候補予測部３４、映像特徴情報抽出部３６、観測及び登録部３８、ランドマーク候補処理部４０及びランドマーク登録部４２から構成される。
図４は、図２に示すステップ２０についての本発明による実施形態２０Ａを説明するためのフローチャートであって、上部映像を獲得してバッファリングするステップ（ステップ５０、５２）、映像特徴情報を抽出して、以前のランドマーク及びランドマーク候補を、現在上部映像に基づいて予測するステップ（ステップ５４、５６）、新たなランドマーク候補を登録して、以前のランドマーク及びランドマーク候補を管理するステップ（ステップ５８）、及びランドマーク候補を処理して、該当するランドマーク候補をランドマークとして登録するステップ（ステップ６０、６２）からなる。
【００４０】
図３に示す映像獲得部３０は、移動体が位置している環境の上部映像を獲得し、獲得した映像を映像バッファ３２、ランドマーク及びランドマーク候補予測部３４及び映像特徴情報抽出部３６にそれぞれ出力する（ステップ５０）。
図５は、図３に示す映像獲得部３０の本発明による実施形態３０Ａのブロック図であって、上部映像獲得部７０、歪曲補正部７２及び低域通過フィルタ７４から構成される。
【００４１】
図６は、図４に示すステップ５０についての本発明による実施形態５０Ａを説明するためのフローチャートであって、上部映像を獲得するステップ（ステップ８０）。及び映像の歪曲を補正して低域通過フィルタリングを行うステップ（ステップ８２、８４）からなる。
本発明の一実施形態によれば、上部映像獲得部７０は、移動体が位置している環境の上部映像を獲得し、獲得した映像を歪曲補正部７２に出力する（ステップ８０）。
【００４２】
ステップ８０の後に、歪曲補正部７２は、上部映像獲得部７０で獲得された上部映像の歪曲を補正し、歪曲が補正された結果を低域通過フィルタ７４に出力する（ステップ８２）。本発明によれば、演算量を減らすために、歪曲補正部７２は、上部映像獲得部７０で獲得された上部映像で関心領域（ＲＯＩ：Ｒｅｇｉｏｎ  Ｏｆ  Ｉｎｔｅｒｅｓｔ）を設定し、設定されたＲＯＩのみに対して歪曲を補正してもよい。つまり、着目する領域を限定することで、演算量を削減する。
【００４３】
図７Ａは、歪曲補正部７２に入力される映像の一例を示し、図７Ｂは、歪曲補正部７２から出力される映像の一例を示す。
例えば、移動体の上部に装着されうる上部映像獲得部７０が、移動体の位置した環境の上部でさらに広い領域の映像を獲得するために、広角カメラ（図示せず）及び魚眼レンズカメラ（図示せず）で具現される場合、上部映像獲得部７０で獲得される映像は、例えば、図７Ａに示すように歪曲される。したがって、歪曲補正部７２は、図７Ａに示すような歪曲された映像を入力して歪曲を補正し、歪曲が補正された結果である図７Ｂに示す映像を出力する。この時、前記のように演算量を減らすために、歪曲補正部７２は、図７Ｂに示すような限定された領域のＲＯＩ  ９０のみに対して歪曲を補正することもできる。
【００４４】
映像歪曲補正に対しては、‘Ｓ．Ｓｈａｈ’及び‘Ｊ．Ｋ．Ａｇｇａｒｗａｌ’、“Ａ  ｓｉｍｐｌｅ  ｃａｌｉｂｒａｔｉｏｎ  ｐｒｏｃｅｄｕｒｅ  ｆｏｒ  ｆｉｓｈ−ｅｙｅ（ｈｉｇｈ  ｄｉｓｔｏｒｔｉｏｎ）  ｌｅｎｓ  ｃａｍｅｒａ”、ＩＥＥＥ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆ．ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎのｖｏｌ.４，Ｍａｙ  １９９４、ｐｐ.のページ３４２２〜３４２７に掲載された論文に開示されている。
【００４５】
ステップ８２の後に、低域通過フィルタ７４は、歪曲補正部７２で歪曲が補正された結果から高周波成分のノイズを除去するために、歪曲が補正された結果を低域通過フィルタリングし、低域通過フィルタリングされた結果を出力端子ＯＵＴ４を介して出力する（ステップ８４）。このように、低域通過フィルタ７４を利用して映像に含まれた高周波ノイズ成分を除去する場合、映像のスムージング効果を提供できる。このために、低域通過フィルタ７４は、ガウスフィルタ（図示せず）で具現されうる。
【００４６】
例えば、３×３ガウスカーネルの場合、低域通過フィルタ７４は、次の数式（１）のようなフィルタリングされた結果を、出力端子ＯＵＴ４を介して出力する。
【００４７】
【数２】
2006209770.tif 000005
ここで、Ｉ’（ｘ，ｙ）は、低域通過フィルタ７４から出力される映像の空間上の座標（ｘ，ｙ）での輝度強度を意味し、Ｉ（ｘ＋ｉ，ｙ＋ｊ）は、低域通過フィルタ７４に入力される映像の空間上の座標（ｘ＋ｉ，ｙ＋ｊ）での輝度強度を意味し、Ｇ（ｍ，ｎ）＝Ｇmnは、次の数式（２）のように表現される行列Ｇの元素である。
【００４８】
【数３】
2006209770.tif 000006
ここで、Ｇは、例えば、次の数式（３）のように表現されてもよい。
【００４９】
【数４】
2006209770.tif 000007
本発明の他の実施形態によれば、図５に示すところとは違って、映像獲得部３０は、上部映像獲得部７０のみで具現されうる。この場合、図６に示すところとは違って、ステップ５０は、ステップ８０のみで具現される。この時、上部映像獲得部７０で獲得された映像が映像バッファ３２、ランドマーク及びランドマーク候補予測部３４及び映像特徴情報抽出部３６に出力される。
【００５０】
本発明のさらに他の実施形態によれば、図５に示すところとは違って、映像獲得部３０は、上部映像獲得部７０及び歪曲補正部７２のみで具現できる。この場合、図６に示すところとは違って、ステップ５０は、ステップ８０及びステップ８２のみで具現される。この時、歪曲補正部７２で歪曲補正された結果は、映像バッファ３２、ランドマーク及びランドマーク候補予測部３４及び映像特徴情報抽出部３６に出力される。
【００５１】
本発明のさらに他の実施形態によれば、図５に示すところとは違って、映像獲得部３０は、上部映像獲得部７０及び低域通過フィルタ７４のみで具現できる。この場合、図６に示すところとは違って、ステップ５０は、ステップ８０及びステップ８４のみで具現される。この時、低域通過フィルタ７４は、上部映像獲得部７０で獲得された上部映像を低域通過フィルタリングし、低域通過フィルタリングされた結果を出力端子ＯＵＴ４を介して出力する（ステップ８４）。
【００５２】
一方、ステップ５０の後に、映像バッファ３２は、映像獲得部３０で獲得された映像をバッファリングし、バッファリングされた結果を以前上部映像としてランドマーク及びランドマーク候補予測部３４に出力する（ステップ５２）。すなわち、映像バッファ３２は、現在上部映像を受信して記憶すると共に、直前の映像である以前上部映像を出力する。
ステップ５２の後に、映像特徴情報抽出部３６は、映像獲得部３０で現在獲得された現在上部映像のコーナー点の座標及びＬＦＤのうち、少なくとも一つを映像特徴情報として抽出し、抽出された映像特徴情報を観測及び登録部３８に出力する（ステップ５４）。
【００５３】
図８は、図３に示す映像特徴情報抽出部３６の本発明による実施形態３６Ａのブロック図であって、コーナー点検出部１００及び記述子演算部１０２から構成される。
図９は、図４に示すステップＳ５４についての本発明による実施形態５４Ａを説明するためのフローチャートであって、コーナー点を検出するステップ（ステップ１１０）及びＬＦＤを求めるステップ（ステップ１１２）からなる。
【００５４】
コーナー点検出部１００は、入力端子ＩＮ２を介して、映像獲得部３０から入力した現在上部映像でコーナー点を検出し、検出されたコーナー点の座標を記述子演算部１０２に出力する一方、出力端子ＯＵＴ５を介して映像特徴情報として出力する（ステップ１１０）。
図１０Ａ及び図１０Ｂは、コーナー点検出部１００の理解を容易にするための写真であって、図１０Ａは、コーナー点検出部１００に入力される映像の一例を示し、図１０Ｂは、コーナー点検出部１００で検出されたコーナー点を十字架状に示す。
【００５５】
本発明によれば、コーナー点検出部１００は、多様な方法のうち最も性能の良いと知られた、例えば、ハリスコーナー点の検出法によりコーナー点を検出できる。ここで、ハリスコーナー点の検出法については、‘Ｃ．Ｊ．Ｈａｒｒｉｓ’及び‘Ｍ．Ｓｔｅｐｈｅｎｓ’、“Ａ  ｃｏｍｂｉｎｅｄ  ｃｏｒｎｅｒ  ａｎｄ  ｅｄｇｅ  ｄｅｔｅｃｔｏｒ”、Ｉｎ  Ｐｒｏｃ．４ｔｈ  Ａｌｖｅｙ  Ｖｉｓｉｏｎ  Ｃｏｎｆ.，Ｍａｎｃｈｅｓｔｅｒ，１９８８、ｐｐ.１４７〜１５１に開示されている。この場合、例えば、コーナー点検出部１００は、図１０Ａに示すような映像を入力して、図１０Ｂに示すようにコーナー点を検出できる。
【００５６】
例えば、ハリスコーナー点の検出法によれば、コーナー点検出部１００は、次の数式（４）のように表現されるＲ値が０より大きく、Ｒ値が局所最大点となる点をコーナー点として決定できる。
【００５７】
【数５】
2006209770.tif 000008
ここで、ｔｒは、トレース演算を表し、ｄｅｔは、行列式（ｄｅｔｅｒｍｉｎａｎｔ）演算を表し、Ｍは、映像強度の変化の程度をもって、次の数式（５）のように求められる行列である。本発明によれば、ｋは、０.０４となりうる。
【００５８】
【数６】
2006209770.tif 000009
ここで、Ｉ’は、前記の数式（１）で表現される輝度強度を表す。
ステップ１１０の後に、記述子演算部１０２は、コーナー点検出部１００で検出されたコーナー点の周辺の映像を分析する。そして、分析された結果を利用して、コーナー点別にＬＦＤを演算し、演算されたＬＦＤを出力端子ＯＵＴ６を介して映像特徴情報として出力する（ステップ１１２）。本発明によれば、記述子演算部１０２で演算されるＬＦＤは、多様な記述子のうち、好ましくは、スケール不変特徴変換（ＳＩＦＴ：Ｓｃａｌｅ  Ｉｎｖａｒｉａｎｔ  Ｆｅａｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ）記述子を意味することもある。ＳＩＦＴ記述子は、コーナー点の周辺の映像の輝度変化分布などを利用してコーナー点の特徴を表現する。このために、記述子演算部１０２は、入力端子ＩＮ２を介して映像獲得部３０から現在上部映像を入力し、コーナー点検出部１００で検出されたコーナー点の座標を入力し、例えば、１２８バイトからなるＬＦＤを出力する。
【００５９】
‘Ｋ．Ｍｉｋｏｌａｊｃｚｋ’及び‘Ｃ．Ｓｃｈｍｉｄ’、“Ａ  ｐｅｒｆｏｒｍａｎｃｅ  ｅｖａｌｕａｔｉｏｎ  ｏｆ  ｌｏｃａｌ  ｄｅｓｃｒｉｐｔｏｒｓ”、ＩＥＥＥ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｃｏｍｐｕｔｅｒ  Ｖｉｓｉｏｎ  ａｎｄ  Ｐａｔｔｅｒｎ  Ｒｅｃｏｇｎｉｔｉｏｎ，Ｊｕｎｅ  ２００３によれば、前記のＳＩＦＴ記述子は、最も優れていると知られている。また、ＳＩＦＴ記述子の生成方法については、‘Ｄａｖｉｄ  Ｇ．Ｌｏｗｅ’、“Ｄｉｓｔｉｎｃｔｉｖｅ  ｉｍａｇｅ  ｆｅａｔｕｒｅｓ  ｆｒｏｍ  ｓｃａｌｅ−ｉｎｖａｒｉａｎｔ  ｋｅｙｐｏｉｎｔｓ”、Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｊｏｕｒｎａｌ  ｏｆ  Ｃｏｍｐｕｔｅｒ  Ｖｉｓｉｏｎ，６０，２（２００４）、ｐｐ.９１〜１１０に掲載された論文に開示されている。
【００６０】
一方、ステップ５４の後に、ランドマーク及びランドマーク候補予測部３４は、以前上部映像でのランドマーク及びランドマーク候補が、現在上部映像のどの位置にあるかを予測する。そして、現在上部映像で予測ランドマーク及びランドマーク候補の位置を、観測及び登録部３８に出力する（ステップ５６）。このような予測により、図３に示す観測及び登録部３８は、現在上部映像に対して検出されたコーナー点のうち、以前にあらかじめ登録されたランドマーク及びランドマーク候補点であったコーナー点を見つけ得る。
【００６１】
一方、ランドマーク及びランドマーク候補予測部３４では、映像が入力されるごとに予測が行われる。各映像で持続的に追従されたランドマーク候補点の場合、各映像での２次元映像座標軌跡と、映像が入力されるときの移動体の位置及び方位角とを保存する。保存されたそれらの情報は、ランドマーク候補点がランドマークとして登録される時、ランドマークの３次元座標値を推定可能にする。
【００６２】
図３に示すランドマーク及びランドマーク候補予測部３４は、図１に示す位置及び地図演算部１２からランドマーク情報、移動体の位置及び移動体の方位角を入力端子ＩＮ１を介して受信する。そして、ランドマーク登録部４２からランドマーク候補情報を受信し、映像バッファ３２から以前上部映像を受信し、映像獲得部３０から現在上部映像を受信する。ここで、ランドマーク情報は、ランドマークの位置、すなわち、基準座標系でランドマークに対する３次元座標及びランドマークに対するＬＦＤを含む。ランドマーク候補情報は、ランドマーク候補の位置、すなわち、基準座標系でランドマーク候補に対する３次元座標及びランドマーク候補に対するＬＦＤを含む。
【００６３】
図１１は、図３に示すランドマーク及びランドマーク候補予測部３４の本発明による好ましい実施形態３４Ａのブロック図であって、映像座標変換部１３０、選択部１３２及び位置予測部１３４から構成される。
図１２は、図４に示すステップ５６についての本発明による好ましい実施形態５６Ａを説明するためのフローチャートであって、座標を変換するステップ（ステップ１４０）、ランドマーク及びランドマーク候補のうち、以前の映像に属するランドマーク及びランドマーク候補を選択するステップ（ステップ１４２）、及び現在上部映像でのランドマーク及びランドマーク候補の座標、すなわち、位置を予測するステップ（ステップ１４４）からなる。
【００６４】
ステップ５４後に、映像座標変換部１３０は、以前上部映像を基準として以前にあらかじめ登録されたランドマーク及びランドマーク候補情報を、入力端子ＩＮ３を介して受信する。受信したランドマーク及びランドマーク候補情報にそれぞれ含まれる、ランドマーク及びランドマーク候補の位置の３次元座標を、以前上部映像が入力された時の移動体の位置及び移動体の方位角を利用して、以前上部映像の映像座標系を基準に２次元座標に変換する。そして、変換された２次元座標を有するランドマーク及びランドマーク候補情報を選択部１３２に出力する（ステップ１４０）。このために、移動体の位置及び移動体の方位角は、入力端子ＩＮ４を介して映像座標変換部１３０に入力される。
【００６５】
図１３は、映像座標変換部１３０の動作を説明するための座標系を例示的に示す図面である。
図１３に示すように、映像座標系で二つの座標軸ｘv、Ｙv（以下、ｖは、映像座標系を意味する添字である）が存在し、移動体座標系で二つの座標軸Ｘr、Ｙr（以下、ｒは、移動体座標系を意味する添字である）が存在し、基準座標系は三つの座標軸Ｘw、Ｙw、Ｚw（以下、ｗは、基準座標系を意味する添字である）で定義される。ここで、二つの車輪１５４及び１５６を有する移動体は、矢印１５２方向に進むと仮定し、移動体の位置座標は、基準座標系で（ｘ、ｙ、θ）wで表現される。ここで、ｘ、ｙは、位置座標を表し、θは方位角を表す。この時、図１３に示す破線で囲まれた参照符号１５０は、映像獲得部３０で獲得された映像を、移動体が位置した物理的な空間上にカメラの撮像領域を考慮して仮想的に表示したものである。
【００６６】
映像座標変換部１３０の入力端子ＩＮ３を介して入力されるランドマーク及びランドマーク候補情報にそれぞれ含まれるランドマーク及びランドマーク候補の位置は、図１３に示す基準座標系を基準に３次元座標である（ｘ、ｙ、ｚ）wで表示されうる。また、映像座標変換部１３０で変換されたランドマーク及びランドマーク候補の座標のそれぞれは、図１３に示す映像座標系を基準とする２次元座標（ｘ，ｙ）vで表示されうる。ここで、ｖは、映像座標系を意味する添字である。
【００６７】
一方、ステップ１４０の後に、選択部１３２は、映像座標変換部１３０から受信した、変換された２次元座標を有するランドマーク及びランドマーク候補のうち、以前上部映像に属するランドマーク及びランドマーク候補のみを選択し、選択された結果を位置予測部１３４に出力する（ステップ１４２）。
図１４は、図１１に示す選択部１３２の理解を容易にするための図面であって、複数のランドマーク１６０、１６２、１６４、１６６、１６８及び１７０及び複数のランドマーク候補１８０、１８２、１８４及び１８６を表す。本発明の理解を容易にするために、図１４でランドマーク及びランドマーク候補は、記号でもって区分して表示したが、本発明は、必ずしもこれに限定されるものではない。ここで、参照符号１９０及び１９２は、それぞれ以前上部映像１９０及び現在上部映像１９２である。
【００６８】
図１４に示すように、選択部１３２は、変換された２次元座標を有するランドマーク１６０、１６２、１６４、１６６、１６８及び１７０とランドマーク候補１８０、１８２、１８４及び１８６のうち、以前上部映像１９０に属するランドマーク１６８とランドマーク候補１８４及び１８６のみを選択し、選択されたランドマーク１６８とランドマーク候補１８４及び１８６とについての情報を位置予測部１３４に出力する。
【００６９】
ステップ１４２の後に、位置予測部１３４は、映像獲得部３０から入力端子ＩＮ５を介して受信した現在上部映像と、映像バッファ３２から入力端子ＩＮ６を介して受信した以前上部映像との間の変化量を、選択部１３２から受信した選択された結果、すなわち、選択された各ランドマーク及び選択された各ランドマーク候補から計算する。そして、計算された変化量を利用して、現在上部映像にあるランドマーク及びランドマーク候補の座標を予測し、予測結果を出力端子ＯＵＴ７を介して観測及び登録部３８に出力する（第１４４ステップ）。この時、選択部１３２で選択された結果及び位置予測部１３４で予測結果の座標のそれぞれは、映像座標系を基準とする２次元座標である。
【００７０】
図１５は、位置予測部１３４の理解を容易にするための図面であって、以前上部映像１９０を獲得する時の移動体２００と、現在上部映像１９２を獲得する時の移動体２０２とを示す。
図１４及び図１５に示すように、位置予測部１３４は、現在上部映像１９２と以前上部映像１９０との間の変化量を、選択部１３２で選択されたランドマーク１６８と選択されたランドマーク候補１８４及び１８６とのそれぞれについて計算する。そして、計算された変化量を利用して、現在上部映像１９２にあるランドマーク１６８とランドマーク候補１８４及び１８６とのそれぞれの座標を予測し、予測結果を出力端子ＯＵＴ７を介して観測及び登録部３８に出力する。
【００７１】
本発明によれば、位置予測部１３４は、オプティカルフロートラッカーを利用して、現在上部映像にあるランドマーク及びランドマーク候補の位置を予測できる。ここで、オプティカルフローとは、現在上部映像と以前上部映像との間の輝度変化情報を利用して、映像の移動量及び回転量が分かる動きベクトルを意味する。オプティカルフロートラッカーとして広く知られたＬｕｋａｓ−Ｋａｎａｄｅオプティカルフロートラッカーについては、‘Ｌｕｃａｓ，Ｂ．’及び‘Ｋａｎａｄｅ，Ｔ．’、“ＡｎＩｔｅｒａｔｉｖｅ  Ｉｍａｇｅ  Ｒｅｇｉｓｔｒａｔｉｏｎ  Ｔｅｃｈｎｉｑｕｅ  ｗｉｔｈ  ａｎ  Ａｐｐｌｉｃａｔｉｏｎ  ｔｏ  Ｓｔｅｒｅｏ  Ｖｉｓｉｏｎ”、Ｐｒｏｃ．ｏｆ  ７ｔｈ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｊｏｉｎｔ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ａｒｔｉｆｉｃｉａｌ  Ｉｎｔｅｌｌｉｇｅｎｃｅ（ＩＪＣＡＩ）、ｐｐ.６７４〜６７９に掲載された論文に開示されている。
【００７２】
本発明によれば、図４に示すところとは違って、ステップ５４及びステップ５６は、同時に行われてもよく、ステップ５６がステップ５４より先に行われてもよい。
一方、ステップ５６の後に、観測及び登録部３８は、ランドマーク及びランドマーク候補予測部３４で予測ランドマーク及びランドマーク候補についての情報と、映像特徴情報抽出部３６から受信した映像特徴情報とを比較する。比較された結果を利用して、現在上部映像で検索されたコーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測して見つける。そして、以前上部映像でランドマークでもなくランドマーク候補でもなかったコーナー点を新たなランドマーク候補として登録する一方、検出されたコーナー点に該当するランドマークやランドマーク候補についての情報を管理する（ステップ５８）。この時、観測及び登録部３８は、登録された新たなランドマーク候補と管理されたランドマーク候補とについてのランドマーク候補情報を、ランドマーク候補処理部４０に出力する一方、管理されたランドマークについてのランドマーク情報を、出力端子ＯＵＴ２を介して図１に示す位置及び地図演算部１２に出力する。
【００７３】
図１６は、図３に示す観測及び登録部３８の本発明による実施形態３８Ａのブロック図であって、ランドマーク及びランドマーク候補検索部２２０、第１距離計算部２２２及び第２距離計算部２２６、第１比較部２２４及び第２比較部２２８、ランドマーク及びランドマーク候補識別部２３０、カウンター２３２及びランドマーク候補初期化部２３４から構成される。
【００７４】
図１７は、図４に示すステップ５８についての本発明による実施形態５８Ａを説明するためのフローチャートであって、コーナー点のそれぞれに対して最近接の予測ランドマークや予測ランドマーク候補を探すステップ（ステップ２５０）、距離の長さに相応して新たなランドマーク候補を登録するか、またはランドマーク及びランドマーク候補を管理するステップ（ステップ２５２〜ステップ２６２）からなる。
【００７５】
図１８は、図１６に示す観測及び登録部３８Ａの理解を容易にするための例示的な図面である。ランドマーク及びランドマーク候補予測部３４において現在上部映像について予測ランドマーク３００、ランドマーク及びランドマーク候補予測部３４で現在上部映像について予測ランドマーク候補３０６及び３１２、及び映像特徴情報抽出部３６から入力した現在上部映像で検索されたコーナー点３０２、３０４、３０８、３１０及び３１４から構成される。
【００７６】
ランドマーク及びランドマーク候補検索部２２０は、入力端子ＩＮ７を介して映像特徴情報抽出部３６から受信した映像特徴情報に含まれたコーナー点と、入力端子ＩＮ８を介してランドマーク及びランドマーク候補予測部３４で予測ランドマーク及び／またはランドマーク候補とについての情報を受信する。そして、受信したコーナー点のそれぞれについて、予測ランドマークやランドマーク候補のうち、コーナー点と最も近いランドマークやランドマーク候補を探す（ステップ２５０）。
【００７７】
図１８に示すように、ステップ２５０で、ランドマーク及びランドマーク候補検索部２２０は、コーナー点３０２、３０４、３０８、３１０及び３１４のそれぞれについて、コーナー点３０２、３０４、３０８、３１０または３１４と最近接した予測ランドマークや予測ランドマーク候補を探す。すなわち、ランドマーク及びランドマーク候補検索部２２０は、コーナー点３０２と最も近い予測ランドマーク３００を見つけ、コーナー点３０４と最も近い予測ランドマーク候補３０６を見つけ、コーナー点３０８と最も近い予測ランドマーク候補３１２を検出し、コーナー点３１０と最も近い予測ランドマーク候補３１２を見つけ、コーナー点３１４と最も近い予測ランドマーク候補３１２を探す。この時、ランドマーク及びランドマーク候補検索部２２０は、図１８に示すように、コーナー点と最も近い予測ランドマークや予測ランドマーク候補を互いに対としてまとめ、その対についての情報を出力する。すなわち、コーナー点３０２と予測ランドマーク３００とが対２７０となり、コーナー点３０４と予測ランドマーク候補３０６とが対２７８となり、コーナー点３０８と予測ランドマーク候補３１２とが対２７２となり、コーナー点３１０と予測ランドマーク候補３１２とが対２７４となり、コーナー点３１４と予測ランドマーク候補３１２とが対２７６となる。すなわち、各対２７０、２７２、２７４、２７６または２７８に属する元素は、コーナー点及び予測ランドマークまたは予測ランドマーク候補である。
【００７８】
ステップ２５０の後に、コーナー点のそれぞれについて、コーナー点と予測ランドマークや予測ランドマーク候補との間の距離が、第１所定距離以下であるか否かを判断する（ステップ２５２）。このために、第１距離計算部２２２は、ランドマーク及びランドマーク候補検索部２２０から受信したコーナー点のそれぞれに対して、コーナー点と検出されたランドマークやランドマーク候補との間の距離を計算し、計算された距離を第１比較部２２４に出力する。この時、第１比較部２２４は、第１距離計算部２２２で計算された距離と第１所定距離とを比較し、比較された結果を出力する。例えば、第１所定距離は、１５ピクセル距離となりうる。ここで、１ピクセル距離は、一つのピクセルが占める空間上の長さを意味する。
【００７９】
図１８に示すように、第１距離計算部２２２は、ランドマーク及びランドマーク候補検索部２２０から入力した対２７０、２７２、２７４、２７６及び２７８のそれぞれに対して、コーナー点と検出されたランドマークやランドマーク候補間の距離を計算し、計算された距離のそれぞれを第１比較部２２４に出力する。
もし、検出されたランドマークやランドマーク候補との距離が第１所定距離以下である場合、それらのコーナー点のそれぞれに対して、コーナー点のＬＦＤと検出されたランドマークやランドマーク候補のＬＦＤとの間の距離が、第２所定距離以下であるか否かを判断する（ステップ２５４）。このために、第２距離計算部２２６は、第１比較部２２４で比較された結果により、コーナー点と検出されたランドマークやランドマーク候補との距離が第１所定距離以下であると認識されれば、そのコーナー点のＬＦＤと検出されたランドマークやランドマーク候補のＬＦＤとの間のユークリッド距離を計算し、計算された距離を第２比較部２２８に出力する。このために、第２距離計算部２２６は、ＬＦＤをランドマーク及びランドマーク候補検索部２２０から受信する。この時、第２比較部２２８は、第２距離計算部２２６で計算された距離と第２所定距離とを比較し、比較された結果を出力する。例えば、ＬＦＤが１２８個の元素を有するベクトルから構成され、各元素がバイト値を有して、そのバイト値の範囲が０ないし２５５の値を有する場合、第２所定距離は、６３８となりうる。
【００８０】
図１８に示すように、第２距離計算部２２６は、第１比較部２２４で比較された結果により、ランドマーク及びランドマーク候補検索部２２０から受信した対２７０、２７２、２７４、２７６及び２７８のうち、コーナー点と検出されたランドマークや検出されたランドマーク候補との距離が第１所定距離以下であると認識される各対を選別する。そして、第２距離計算部２２６は、選別された対に属するコーナー点のＬＦＤと、選別された対に属する検出されたランドマークやランドマーク候補のＬＦＤと、の間のユークリッド距離を計算し、計算された距離を第２比較部２２８に出力する。
【００８１】
もし、コーナー点のＬＦＤと検出されたランドマークや検出されたランドマーク候補のＬＦＤとの間の距離が第２所定距離以下であると判断されれば、コーナー点と最も近い位置にランドマークが存在するか、それともランドマーク候補が存在するかを判断する（ステップ２５６）。このために、第２比較部２２８で比較された結果により、コーナー点のＬＦＤと検出されたランドマークや検出されたランドマーク候補のＬＦＤとの間の距離が第２所定距離以下であると認識されれば、ランドマーク及びランドマーク候補識別部２３０は、そのコーナー点と最も近い位置にランドマークが存在するか、それともランドマーク候補が存在するかを識別し、識別された結果をカウンター２３２に出力する。
【００８２】
図１８に示すように、ランドマーク及びランドマーク候補識別部２３０は、ランドマーク及びランドマーク候補検索部２２０から受信したコーナー点とランドマークやランドマーク候補との間の距離が第１所定距離以下である対のうち、第２比較部２２８で比較された結果によりＬＦＤ間の距離が第２所定距離以下であると認識される各対について、コーナー点と最も近い位置にランドマークが存在するか、それともランドマーク候補が存在するかを識別し、識別された結果をカウンター２３２に出力する。例えば、図１８に示す対２７０で、コーナー点３０２のＬＦＤとランドマーク３００のＬＦＤとの間の距離が第２所定距離以下であると認識される場合、ランドマーク及びランドマーク候補識別部２３０は、コーナー点３０２と対をなすものがランドマーク３００であると識別する。また、図１８に示す対２７４で、コーナー点３１０のＬＦＤとランドマーク候補３１２のＬＦＤとの間の距離が第２所定距離以下であると認識される場合、ランドマーク及びランドマーク候補識別部２３０は、コーナー点３１０と対をなすものがランドマーク候補３１２であると識別する。
【００８３】
もし、ランドマーク及びランドマーク候補識別部２３０により識別された結果により、コーナー点と最も近い位置に、ランドマーク候補ではなくランドマークが存在することと認識されれば、カウンター２３２は、ランドマークが観測された回数を表すランドマークフラグをアッパーカウンティングする。そして、カウンティングされた結果を出力端子ＯＵＴ８を介して出力する（ステップ２５８）。しかし、ランドマーク及びランドマーク候補識別部２３０からの識別された結果を通じ、コーナー点と最も近い位置にランドマークでなくランドマーク候補が存在することと認識されれば、カウンター２３２は、ランドマーク候補が観測された回数を表すランドマーク候補フラグをアッパーカウンティングし、カウンティングされた結果を出力端子ＯＵＴ８を介して出力する（ステップ２６０）。すなわち、カウンター２３２は、ランドマーク及びランドマーク候補識別部２３０で識別された結果に応答して、ランドマークが観測された回数を表すランドマークフラグをアッパーカウンティングするか、またはランドマーク候補が観測された回数を表すランドマーク候補フラグをアッパーカウンティングし、カウンティングされた結果を出力端子ＯＵＴ８を介して出力する。ここで、カウンター２３２でランドマークフラグをアッパーカウンティングした結果は、ランドマーク情報として出力され、カウンター２３２からランドマーク候補フラグをアッパーカウンティングした結果は、ランドマーク候補情報として出力される。
【００８４】
例えば、図１８に示す対２７０、２７４及び２７８で、図１７に示す第２５２及び第２５４ステップを経たＬＦＤ間の距離が第２所定距離以下であると認識された場合、カウンター２３２は、ランドマーク３００が観測されたため、ランドマーク情報に含まれたランドマークフラグ値をアッパーカウンティングし、二つのランドマーク候補３０６及び３１２も観測されたため、同様にランドマーク候補情報に含まれたそれぞれのランドマーク候補フラグ値をアッパーカウンティングする。
【００８５】
もし、検出されたランドマークやランドマーク候補と第１所定距離より遠くに位置したコーナー点、または検出されたランドマークやランドマーク候補のＬＦＤと第２所定距離より遠くに位置したＬＦＤとを有するコーナー点については、新たなランドマーク候補として決定し、決定された新たなランドマーク候補についての情報を登録する（ステップ２６２）。このために、ランドマーク候補初期化部２３４は、第１比較部２２２で比較された結果により、コーナー点と検出されたランドマークや検出されたランドマーク候補との間の距離が第１所定距離より長いと認識され場合には、該当する各対に含まれるコーナー点を、新たなランドマーク候補として決定する。そして、決定された新たなランドマーク候補についての情報をランドマーク候補情報として登録する。また、ランドマーク候補初期化部２３４は、第２比較部２２６で比較された結果により、ＬＦＤ間の距離が第２所定距離より長いと認識された場合は、該当する各対に含まれるコーナー点を、新たなランドマーク候補として決定し、決定された新たなランドマーク候補についての情報をランドマーク候補情報として登録する。ここで、新たなランドマーク候補として登録されるランドマーク候補情報に含まれるランドマーク候補フラグは、‘０’に初期化される。
【００８６】
例えば、図１８に示す対２７２、２７６及び２７８に含まれたコーナー点３０８、３１４及び３０４が、以前上部映像でランドマークでもなくランドマーク候補でもなかったと認識されれば、コーナー点３０８、３０４及び３１４を新たなランドマーク候補として決定し、決定されたランドマーク候補３０４、３０８及び３１４についてのランドマーク候補情報を登録する。例えば、図１８に示す対２７２に含まれたコーナー点３０８とランドマーク候補３１２との間の距離が第１所定距離より長いか、またはコーナー点３０８のＬＦＤとランドマーク候補３１２のＬＦＤとの間の距離が第２所定距離より長いと認識されれば、コーナー点３０８を新たなランドマーク候補として決定し、決定されたランドマーク候補３０８についてのランドマーク候補情報を登録する。
【００８７】
一方、ステップ５８の後に、ランドマーク候補処理部４０は、観測及び登録部３８で登録されたランドマーク候補のうち、互いに重複しているランドマーク候補を除去する。そして、重複していないランドマーク候補の２次元映像座標から３次元映像座標を推定する。重複していないランドマーク候補のうち、推定された３次元映像座標を有するものをランドマーク登録部４２に出力する（第６０ステップ）。この時、観測及び登録部３８からランドマーク候補処理部４０に入力される、登録されたランドマーク候補のそれぞれの座標は、映像座標系で表現される２次元座標である。
【００８８】
図１９は、図３に示すランドマーク候補処理部４０の本発明による実施形態４０Ａのブロック図であって、第３距離計算部３３０、第３比較部３３２、ランドマーク候補除去部３３４及び座標推定部３３６から構成される。
図２０は、図４に示す第６０ステップについての本発明による実施形態６０Ａを説明するためのフローチャートであって、最小距離と第３所定距離とを比較した結果によって重複しているランドマーク候補を除去するか、または重複していないランドマーク候補の３次元座標を推定するステップ（ステップ３５０、３５６）からなる。
【００８９】
ステップ５８の後に、第３距離計算部３３０は、観測及び登録部３８から入力端子ＩＮ９を介して、登録されたランドマーク候補を受信する。そして、登録されたランドマーク候補のそれぞれに対して、登録されたランドマーク候補と最も近い他の登録されたランドマーク候補との間の最小距離を計算し、計算された最小距離を第３比較部３３２に出力する（ステップ３５０）。
【００９０】
ステップ３５０の後に、最小距離が第３所定距離以下であるか否かを判断する（ステップ３５２）。このために、第３比較部３３２は、第３距離計算部３３０から受信した各最小距離と第３所定距離とを比較し、比較された結果をランドマーク候補除去部３３４に出力する。本発明によれば、第３所定距離は、５ピクセル距離に設定されうる。
ランドマーク候補除去部３３４は、第３比較部３３２で比較された結果により、第３所定距離以下の最小距離を有するランドマーク候補は、互いに重複していると決定する。そして、ランドマーク候補除去部３３４は、入力端子ＩＮ９を介して観測及び登録部３８から受信したランドマーク候補のうち、重複しているランドマーク候補を除去する（ステップ３５４）。
【００９１】
例えば、図１８に示すコーナー点３０８、３０４及び３１４が新たなランドマーク候補として決定され、決定されたランドマーク候補３０８、３０４及び３１４についてのランドマーク候補情報が観測及び登録部３８で登録されたとする。この場合、コーナー点３０８及び３０４の間の最小距離が第３所定距離以下であれば、新たなランドマーク候補３０８及び３０４の一つは除去される。
【００９２】
しかし、第３所定距離より遠く離れた最小距離を有するランドマーク候補は、互いに重複していないと決定する。そして、映像座標系で２次元座標により表現される重複していないランドマーク候補の３次元映像座標を、その２次元座標から、例えば、三角法により推定する（ステップ３５６）。このために、本発明の一実施形態によれば、第３比較部３３２で比較された結果により、第３所定距離より遠く離れた最小距離を有するランドマーク候補は、互い重複していないと認識される。そして、座標推定部３３６は、観測及び登録部３８から入力端子ＩＮ９を介して受信した、重複していないランドマーク候補の３次元映像座標を推定し、推定された３次元座標を有するランドマーク候補を、ランドマーク登録部４２に出力端子ＯＵＴ１０を介して出力する。
【００９３】
または、本発明の他の実施形態によれば、ランドマーク候補除去部３３４が入力端子ＩＮ９を介して観測及び登録部３８から受信したランドマーク候補のうち、除去されていないランドマーク候補のみを座標推定部３３６に出力したとする。この場合、座標推定部３３６は、ランドマーク候補除去部３３４で除去されていないランドマーク候補をランドマーク候補除去部３３４から受信し、この受信した重複していないランドマーク候補の３次元映像座標を推定する。そして、推定された３次元座標を有するランドマーク候補を、ランドマーク登録部４２に出力端子ＯＵＴ１０を介して出力する。
【００９４】
もし、前記のように、図１１に示す位置予測部１３４が、オプティカルフロートラッカーを利用して、現在上部映像にあるランドマーク及びランドマーク候補の位置を予測すれば、図１９に示す座標推定部３３６は、重複していないランドマーク候補の２次元座標の軌跡、移動体の位置及び方位角の軌跡から３次元映像座標をさらに容易に推定できる。
図２１は、図１９に示す座標推定部３３６で三角法を利用して高さ情報を抽出する過程の理解を容易にするための図面であって、基準座標系で位置Ｐ1、Ｐ2及びＰLの座標を示す。
【００９５】
図２１に示すように、移動体が位置Ｐ1に位置する時の移動体の位置及び方位角値は、（ｘR1，ｙR1，θR1）wとして表現され、移動体が位置Ｐ2に位置する時の移動体の位置及び方位角値は（ｘR2，ｙR2，θR2）wとして表現される。一方、位置マークＰLが基準座標系に対して（ｘL，ｙL，θL）wに位置し、このランドマークをＰ1及び位置Ｐ2でそれぞれ上部映像を介して観測すれば、各映像座標系に対して（ｘL1，ｙL1）v及び（ｘL2，ｙL2）v値を求めうる。ここで、映像座標系と基準座標系との関係は、図１３に示した通りである。たとえ、ロボットのような移動体の位置が基準座標系でＰ1からＰ2に変わるとしても、基準座標系でランドマークは、同じ点（ｘL，ｙL，θL）wで表現されるが、映像座標系上では、Ｐ1でのランドマークの座標が、（ｘL1，ｙL1）vで表現され、Ｐ2でのランドマークの座標が、（ｘL2，ｙL2）vで表現される。そして、映像座標系上の点は、ランドマークＰLがロボットカメラの撮像平面に投射された点となる。この時、座標推定部３３６は、移動体が位置Ｐ1から位置Ｐ2に移動した距離θR1及び角度θR2と、各映像座標系に対して求めたランドマークについての観測値（ｘL1，ｙL1）v及び（ｘL2，ｙL2）vとから、三角法により位置ＰLに位置した実際のランドマーク候補の高さを推定できる。
【００９６】
図２２は、映像座標系を移動体座標系に変換する一例を示す図面であり、図２３は、移動体座標系を基準座標系に変換する一例を示す図面である。
図２２及び図２３に示すように、まず、座標推定部３３６は、映像座標系でランドマーク候補の２次元座標を、次の数式（６）のように移動体座標系に変換する。
【００９７】
【数７】
2006209770.tif 000010
【００９８】
【数８】
2006209770.tif 000011
次いで、座標推定部３３６は、移動体座標系でのランドマーク候補の２次元座標を、次の数式（８）のように基準座標系に変換する。
【００９９】
【数９】
2006209770.tif 000012
【０１００】
【数１０】
2006209770.tif 000013
結局、座標推定部３３６は、次の数式（１０）のように、ランドマーク候補の基準座標系での３次元座標を求めうる。
【０１０１】
【数１１】
2006209770.tif 000014
ここで、
ＸLは、ランドマーク候補の基準座標系での３次元座標を表し、次の数式（１１）のように表現され、
ＸRは、ランドマーク候補の映像座標系での２次元座標を表し、次の数式（１２）のように表現され、
Ｔは、転置を表し、
Ｈは、次の数式（１３）のように表現され、座標系変換を表す数式（１４）と、三角法を表す数式（１５）とから誘導されうる。
【０１０２】
【数１２】
2006209770.tif 000015
【０１０３】
【数１３】
2006209770.tif 000016
【０１０４】
【数１４】
2006209770.tif 000017
【０１０５】
【数１５】
2006209770.tif 000018
【０１０６】
【数１６】
2006209770.tif 000019
本発明によれば、図３に示すランドマーク生成部１０Ａは、ランドマーク候補処理部４０を設けなくてもよい。その場合、図４に示すステップ２０Ａは、ステップ６０を含んでいなくてもよい。
【０１０７】
一方、ステップ６０の後に、ランドマーク登録部４２は、ランドマーク候補のうち、所定回数以上コーナー点として観測されたランドマーク候補をランドマークとして新たに登録し、登録されたランドマークを出力端子ＯＵＴ３を介して出力する。逆に、ランドマーク候補のうち、所定回数より少なくコーナー点として観測されたランドマーク候補についてのランドマーク候補情報をランドマーク及びランドマーク候補予測部３４に出力する（ステップ６２）。ここで、所定回数は、例えば、‘５’に設定されうる。ステップ６２で、ランドマーク登録部４２が新たにランドマークを登録する手順を説明すれば、まず、拡張されたカルマンフィルタ（Ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ：ＥＫＦ）の共分散行列の大きさを大きくし、関連ランドマーク位置を共分散行列のサブマトリックスに挿入し、関連ランドマークノイズをノイズ共分散行列に挿入する。
【０１０８】
一方、ステップ２０の後に、位置及び地図演算部１２は、ランドマーク生成部１０から受信したランドマーク情報を利用して、移動体の位置を推定すると共に地図を生成し、推定された位置及び生成された地図を、出力端子ＯＵＴ１を介して出力する。一方、ランドマーク情報と移動体の位置及び方位角となどをランドマーク生成部１０、すなわち、図３に示すランドマーク及びランドマーク候補予測部３４に出力する（ステップ２２）。このために、図１に示す移動体の位置推定及び地図の生成装置は、走行処理部１４をさらに設けうる。
【０１０９】
ここで、走行処理部１４は、移動体の走行を制御し、走行についての情報を位置及び地図演算部１２に出力する。ここで、走行についての情報とは、移動体、例えば、移動ロボットの左右輪に設けられたエンコーダ（図示せず）から獲得したセンサー値、すなわち、ロボット駆動情報となりうる。
この時、位置及び地図演算部１２は、走行処理部１４から受信した走行についての情報と、ランドマーク生成部１０から入力したランドマークとから移動体の位置を推定すると共に、地図を生成して、ランドマークについての情報をランドマーク生成部１０に出力する。
【０１１０】
本発明によれば、位置及び地図演算部１２は、拡張されたカルマンフィルタ（ＥＫＦ）を利用してランドマークから移動する移動体の位置を認識すると共にランドマークについての地図を生成するＳＬＡＭ演算部（図示せず）で具現されうる。
図３に示すランドマーク登録部４２で新たなランドマークを登録する前記の手順と、図１に示す位置及び地図演算部１２とについては、‘Ｍ．Ｗ．Ｍ．Ｇａｍｉｎｉ  Ｄｉｓｓａｎａｙａｋｅ’，‘ＰａｕｌＮｅｗｍａｎ’，‘Ｓｔｅｖｅｎ  Ｃｌａｒｋ’，‘Ｈｕｇｈ  Ｆ．Ｄｕｒｒａｎｔ−Ｗｈｙｔｅ’及び‘Ｍ．Ｃｓｏｒｂａ’、“Ａ  Ｓｏｌｕｔｉｏｎ  ｔｏ  ｔｈｅ  Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐ  Ｂｕｉｌｄｉｎｇ（ＳＬＡＭ）  Ｐｒｏｂｌｅｍ”、ＩＥＥＥ  Ｔｒａｎｓ．ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ．Ｖｏｌ.１７，Ｎｏ.３，Ｊｕｎｅ  ２００１に開示されている。
【０１１１】
以下、位置及び地図演算部１２で行われるＳＬＡＭ更新について、次のように添付された図２４及び図２５を参照して説明する。
ＥＫＦ基盤のＳＬＡＭ技法（自己位置と地図情報の同時推定手法）の場合、一般的な方法論及びＳＬＡＭ技法についての理論的な証明を含んでいるが、本発明で提示された動きモデル、観測モデルなどの一連の方法を含んでいない。すなわち、本発明の図１に示すランドマーク生成部１０を介して、移動体の位置、方位角演算及び地図作成の性能を一般的なＥＫＦ基盤のＳＬＡＭ技法に比べて向上させた。
【０１１２】
図２４は、処理モデルを説明するための図面であって、基準座標系で移動体４００の座標（ｘk，ｙk）を表す。ここで、媒介物処理モデル、すなわち、移動体の位置モデルは、次の数式（１６）のように表現され、ランドマーク処理モデル、すなわち、地図のモデルは、次の数式（１７）のように表現されうる。
【０１１３】
【数１７】
2006209770.tif 000020
ここで、
（ｘk，ｙk，θk）は、基準座標系上で以前上部映像を獲得する時の移動体の位置及び角度を表し、
（ｘk+1，ｙk+1，θk+1）は、基準座標系上で現在上部映像を獲得する時の移動体の位置及び角度を表し、
ｖは、移動体の線速度を表し、ｗは、移動体の角速度を表し、Ｔは、サンプリング時間を表す。
【０１１４】
【数１８】
2006209770.tif 000021
図２５は、観測モデルを説明するための図面であって、ｘ、ｙ及びｚは、基準座標系の軸を表し、ｘv及びｙvは、映像座標系の軸を表し、（ｘj，ｙl，ｚl）は、基準座標系でのランドマークの座標を表し、（ｘR，ｙR及びθR）は、移動体座標系でのランドマークの座標をそれぞれ表す。
【０１１５】
図２５に示すように、映像座標系上で中心点（ｘv，ｙv，ｚv）から点４０２までの距離ｄvは、次の数式（１８）のように表現され、映像座標系上でｘv軸及び点４０２までのベクトルがなす角度ψvは、次の数式（１９）のように表現されうる。
【０１１６】
【数１９】
2006209770.tif 000022
ここで、ｆは、スケールファクターを表し、物理的には、カメラレンズの焦点距離を意味する。
【０１１７】
【数２０】
2006209770.tif 000023
以下、数式（１６）及び数式（１７）で与えられた処理モデル、数式（１８）及び数式（１９）で与えられた観測モデル、‘Ｍ．Ｗ．Ｍ．Ｇａｍｉｎｉ  Ｄｉｓｓａｎａｙａｋｅ’，‘Ｐａｕｌ  Ｎｅｗｍａｎ’，‘Ｓｔｅｖｅｎ  Ｃｌａｒｋ’，‘Ｈｕｇｈ  Ｆ．Ｄｕｒｒａｎｔ−Ｗｈｙｔｅ’及び‘Ｍ．Ｃｓｏｒｂａ’，“Ａ  Ｓｏｌｕｔｉｏｎ  ｔｏ  ｔｈｅ  Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐ  Ｂｕｉｌｄｉｎｇ（ＳＬＡＭ）  Ｐｒｏｂｌｅｍ”，ＩＥＥＥ  Ｔｒａｎｓ．ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ．Ｖｏｌ.１７，Ｎｏ.３，Ｊｕｎｅ  ２００１に開示された方法、及び前記の実施形態を参照すれば、本発明の実施形態は当業者により容易に具現されうる。
【０１１８】
一方、前記の本発明の実施形態は、コンピュータで実行されうるプログラムで作成可能であり、コンピュータで読み取り可能な記録媒体を利用して前記プログラムを動作させる汎用ディジタルコンピューターで具現されうる。前記コンピュータで読み取り可能な記録媒体は、マグネチック保存媒体（例えば、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、フレキシブルディスク、ハードディスク等）、光学的な読み取り媒体（例えば、ＣＤ−ＲＯＭ、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｐｌａｙ）等）及びキャリアウェーブ（例えば、インターネットを介した伝送）のような保存媒体を含む。
【０１１９】
コンピュータで読み取り可能な記録媒体に保存されるコンピュータプログラムは、移動体の位置した周囲の上部を撮影して獲得した上部映像でコーナーを表すコーナー点を観測させ、観測されたコーナー点からランドマークを生成させるステップ及びランドマークから移動体の位置を推定させて地図を生成させるステップを行う。
ランドマークを生成させるステップでは、現在上部映像でコーナーを表すコーナー点の座標及びコーナー点を互いに区分するＬＦＤのうち、少なくとも一つを利用する。そして、コーナー点の座標及び／又はＬＦＤを利用して、コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測させて検出する。以前上部映像でランドマークでもなくランドマーク候補でもなかったコーナー点を、新たなランドマーク候補として登録させ、ランドマーク候補のうち、所定回数以上コーナー点として観測されたランドマーク候補をランドマークとして登録させる。この時、ランドマークを登録させるステップは、移動体の位置した環境の上部映像を獲得させるステップと、獲得された映像をバッファリングさせ、バッファリングされた結果を以前上部映像として決定するステップと、現在上部映像のコーナー点の座標及びＬＦＤのうち、少なくとも一つを映像特徴情報として抽出させるステップと、以前上部映像でのランドマーク及びランドマーク候補を現在上部映像で予測させるステップと、予測ランドマーク及びランドマーク候補についての情報と映像特徴情報とを比較させ、比較された結果を利用して、コーナー点のうち、以前上部映像でランドマークやランドマーク候補であったコーナー点を観測させ、以前上部映像でランドマークでもなくランドマーク候補でもなかったコーナー点を新たなランドマーク候補として登録させるステップと、ランドマーク候補のうち、所定回数以上コーナー点として観測されたランドマーク候補をランドマークとして登録させるステップと、を行う。
【０１２０】
映像を獲得させるステップは、移動体の位置した環境の上部映像を獲得させるステップ、及び獲得された上部映像の歪曲を補正させるステップを行う。映像を獲得させるステップは、歪曲が補正された結果を低域通過フィルタリングさせるステップをさらに行う。または、映像を獲得させるステップは、移動体の位置した周囲の上部映像を獲得させるステップ、及び獲得された上部映像を低域通過フィルタリングさせるステップを行う。映像特徴情報を抽出させるステップは、現在上部映像でコーナー点を検出させ、検出されたコーナー点の座標を求めさせるステップ、及び検出されたコーナー点の周辺の映像を分析させ、分析された結果を利用して、コーナー点別にＬＦＤを求めさせるステップを行う。ランドマーク及びランドマーク候補の位置を現在上部映像で予測させるステップは、以前上部映像で登録されたランドマーク及びランドマーク候補の位置の３次元座標を、前記以前上部映像が入力された時の移動体の位置及び方位角を利用して、以前上部映像の映像座標系を基準に２次元に変換させるステップと、変換された２次元座標を有するランドマーク及びランドマーク候補のうち、以前上部映像に属するランドマーク及びランドマーク候補のみを選択させるステップ、及び現在上部映像と以前上部映像との間の変化量を、選択された結果について計算させ、計算された変化量を利用して、現在上部映像にあるランドマーク及びランドマーク候補を予測させるステップを行う。
【０１２１】
コーナー点を観測して新たなランドマーク候補を登録させるステップは、コーナー点のそれぞれに対して、コーナー点と最も近い予測ランドマークやランドマーク候補を見つけさせるステップと、コーナー点のそれぞれに対して、コーナー点と検出されたランドマークやランドマーク候補との間の距離が第１所定距離以下であるか否かを判断させるステップと、検出されたランドマークやランドマーク候補との距離が第１所定距離以下のコーナー点のそれぞれに対して、コーナー点のＬＦＤと検出されたランドマークやランドマーク候補のＬＦＤとの間の距離が第２所定距離以下であるか否かを判断させるステップと、コーナー点のＬＦＤと検出されたランドマークやランドマーク候補のＬＦＤとの間の距離が第２所定距離以下であると判断されれば、コーナー点と最も近い位置にランドマークが存在するか、それともランドマーク候補が存在するかを判断させるステップと、ランドマークが存在すると判断されれば、ランドマークが観測された回数を表すランドマークフラグをアッパーカウンティングさせるステップと、ランドマーク候補が存在すると判断されれば、ランドマーク候補が観測された回数を表すランドマーク候補フラグをアッパーカウンティングさせるステップ、及び検出されたランドマークやランドマーク候補及び第１所定距離より遠く位置したコーナー点、または検出されたランドマークやランドマーク候補のＬＦＤ及び第２所定距離より遠く位置したＬＦＤに該当するコーナー点を新たなランドマーク候補として決定し、決定された新たなランドマーク候補についての情報を登録させるステップを行う。
【０１２２】
ランドマークを登録するステップは、登録されたランドマーク候補のうち、互いに重複しているランドマーク候補を除去させ、重複していないランドマーク候補の２次元映像座標から３次元映像座標を推定させるステップをさらに行う。映像座標を推定させるステップは、登録されたランドマーク候補のそれぞれに対して、最も近いランドマーク候補間の最小距離を求めさせるステップと、最小距離が第３所定距離以下であるか否かを判断させるステップと、第３所定距離以下の最小距離を有するランドマーク候補は互いに重複していると決定し、重複しているランドマーク候補を除去させるステップ、及び第３所定距離より遠く離れた最小距離を有するランドマーク候補は互いに重複していないと決定し、重複していないランドマーク候補の３次元映像座標を推定させるステップを行う。
【０１２３】
以下、移動体としてロボットを採択しつつ、本発明による上部映像を利用した移動体の位置推定及び地図の生成装置と方法、並びにその装置を制御するコンピュータプログラムを保存するコンピュータで読み取り可能な記録媒体により、ロボットの自身の位置及び方位角をテストした結果を、添付された図面を参照して次の通りに説明する。
まず、ロボットの各位置での距離誤差の和を総サンプル数で除算した結果を距離平均誤差として定義し、ロボットの各位置での方位角度の誤差の和を総サンプル数で除算した結果を方位角の平均誤差として定義する。このような誤差を計算するために、まず、ロボットが学習を始めた初期位置を基準座標系の原点に設定し、その初期位置でのロボットの進行方向を基準座標系のｘ軸方向と一致させる。この時、ロボットの実際の位置及び方位角を（ｘd，ｙd，θd）として表記し、ロボットの測定位置及び方位角を（ｘ，ｙ，θ）として表記する場合、距離誤差ｅdは、次の数式（２０）のように表現され、ｃｍの単位を有し、方位角誤差ｅθは、次の数式（２１）のように表現され、ｄｅｇｒｅｅの単位を有しうる。
【０１２４】
【数２１】
2006209770.tif 000024
【０１２５】
【数２２】
2006209770.tif 000025
一方、ロボットが移動するテスト環境がアパートの居間であるとする時、まず、居間でロボットが移動する経路をあらかじめ設定する。そして、ロボットを経路によって自動的に移動させた後、ロボットの移動時にリアルタイムで居間の地図を作成し、位置を認識させた。この時、ロボットが移動する経路上の決まったポイント、例えば、１５ケ所のポイントにロボットを移動させた後、各ポイントで所定回数、例えば、３回にわたってロボットの位置及び方位角を実測する。この時、ロボット内のプロセッサ上で計算された位置及び方位角と、測定された位置び方位角との間の距離誤差及び方位角誤差を所定数、例えば、４５個ほどサンプルデータとして獲得する。
【０１２６】
図２６は、テストするロボットの外観を示す写真であって、輪駆動部５５０、ロボット５５１、上部カメラ５５２及び天井５５４を示す。ここで、図３に示す映像特徴情報抽出部３６は、天井５５４で複数のコーナー点５６０ないし５７８を検出できる。
図２６に示す輪駆動部５５０は、図１に示す走行処理部１４の役割を行い、上部カメラ５５２は、図３に示す映像獲得部３０に内蔵されうる。
【０１２７】
図２７Ａ及び図２７Ｂは、ロボットが移動する経路を例示的に示す図面であって、図２７Ａは、ロボットが走行するアパートの居間をはじめとして、アパートの平面図を示し、図２７Ｂは、ロボットが走行する経路を例示的に示す図面である。各矢印に対応する数字は、実際ｍｍ単位で表示された距離値を意味する。図２７Ｂに示す経路は、基準座標系で表示される。
【０１２８】
図２７Ａに示すアパートの居間で、ロボットは、図２７Ｂに示す矢印方向の経路によって地図を自動的に学習する。この時、ロボットを図２７Ｂに示す学習開始点（０，０）６００で学習を開始させる。この時、図２７Ｂに示す各ポイントで、基準座標系でロボットの座標値を実測により求め、学習が終わった後、それらのポイントで位置及び方位角の誤差を測定する。
【０１２９】
一方、定規と分度器とを使用して、ロボットの位置及び方位角を測定し、位置及び方位角を測定する順序は、次の通りである。
図２８は、ロボットの外観を示す図面であり、図２９Ａないし図２９Ｄは、ロボットの位置測定を示す写真である。
まず、ロボットをテストポイント、例えば、図２７Ｂに示すポイント６０２ないし６２２のうち、何れか１箇所のポイントに位置させた後、ロボットの輪軸６３４と左右輪の外側面６３０及び６３２とが出合う点を地面方向に投影した時、地面と出合う点を図２９Ａ及び図２９Ｂに示すように印を付ける。この時、ロボットを移動させた後、図２９Ｃに示すように、地面に印を付けられた二つのマークを連結する線分を描いた後、線分の中心点と原点との距離をロボットの距離として測定する。さらに、図２９Ｄに示すように、線分が地面の基準線となす角度をロボットの角度として設定する。
【０１３０】
図３０は、前記の測定方法を利用して、１５個のポイントのそれぞれに対して３回測定して、４５個のサンプルデータを獲得する場合、測定データの一例を示す図面である。
図３１は、図３０から得られた距離誤差に対するＭｉｎｉｔａｂというデータ分析プログラムを利用して実施した、統計的分析結果を説明するための図面であって、最大許容距離誤差（ＵＳＬ、Ｕｐｐｅｒ  Ｓｐｅｃｉｆｉｃａｔｉｏｎ  Ｌｉｍｉｔ）が１０ｃｍである場合、測定された距離誤差についての統計分析結果を表すグラフであって、縦軸は、確率分布を表し、横軸は、距離誤差の範囲を表す。
【０１３１】
図３１からＵＳＬが１０ｃｍである場合、３０個の観測データ数に対して平均距離誤差は５.９０１８ｃｍであり、この時、分散程度を測定するシグマレベルは、全体工程能力に対してＺ．Ｂｅｎｃｈ＋１.５の数式で２.７４シグマレベルであるということが分かる。シグマレベルは、本来、不良率を表す指標であって、６シグマレベルは、１００万個の生産時、３.４個の不良が発生するレベルを意味する。このシックスシグマとは，設計製造やサービス提供に関連するビジネス・プロセス上の欠陥を最小化することにより、業務パフォーマンスを改善するための経営・品質管理手法である．
図３２は、方位角の誤差に対してＭｉｎｉｔａｂというデータ分析プログラムを利用して実施した統計的な分析結果を説明するための図面であって、最大方位角の許容誤差が５度である場合、測定された方位角誤差についての統計分析結果を表すグラフであって、縦軸は、観測された方位角データの確率分布を表し、横軸は、方位角誤差の範囲を表す。
【０１３２】
図３２からＵＳＬが５度である場合、３０個の観測データ数に対して平均距離誤差は１.９４３３３度であり、この時、分散程度を測定するシグマレベルは、全体の工程能力に対してＺ．Ｂｅｎｃｈ＋１.５の数式で３.４３シグマレベルであるということが分かる。ここで、シグマは誤差に対する偏差が良好であることを示すものであり、前記の結果から本発明の方法が極めてロバスト（robust）であることを統計的に示している。
【０１３３】
以上のように、本発明では、コーナー点や映像特徴周辺記述子を用いてランドマークを生成するため、上部映像そのものを保存したり上部映像そのものを用いて演算することなくランドマークを生成することができ、演算量を大幅に減らすことができる。演算量が減少するため、生成したランドマークからの位置の推定及び地図の作成をともに行うことが可能となる。
【０１３４】
また、カメラなどを用いて上部映像を取得することでランドマークを生成するため、レーザーレンジファインダーのような高価なカメラが不要である。このとき、上部映像を取得するために、安価の一台のカメラのみを利用すれば良いため、製造コストを低減させることができる。
また、上述の通り、ランドマークは、上部映像に基づいて自動的に生成される。よって、例えば、赤外線ランドマーク、色パターンのような人工ランドマークなどの別途の付着物によりランドマークを生成する必要がない。このような別途の付着物によるランドマークを用いた場合には、これらのランドマークをセンシングする際に、照明などの影響を受けてランドマークを正確に取得することが出来ない場合がある。しかし、本発明のように、上部映像に基づいてランドマークを生成する場合には、前述のような照明の影響を受けることが無く正確にランドマークを生成でき、正確な位置の推定及び正確な地図作製を行うことができる。
【０１３５】
ランドマークの候補を新たに登録する時、コーナー点の特徴を表すＳＩＦＴ（Ｓｃａｌｅ  Ｉｎｖａｒｉａｎｔ  Ｆｅａｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ）記述子のような映像特徴周辺記述子を利用する場合には、移動体の位置した室内環境の全体の照度が変化するとしても、映像の輝度変化は一定である。よって、移動体の位置した照明の変化にロバスト性を有するランドマークを見つけて登録できる。これにより移動体の位置認識及び地図作成を正確に行わせることができる。例えば、移動体の平均距離誤差を約６ｃｍ以内に維持し、方位角の平均誤差を約２度以内に維持できる。ここで、ロバスト性とは，外乱や設計誤差などの不確定な変動に対して，システム特性が現状を維持できることを意味する。
【０１３６】
さらに、オプティカルフロートラックキングと映像特徴周辺記述子とを何れも利用する場合には、従来のＳＬＡＭ技法の問題であるデータ関連問題、すなわち、Ｌａｒｇｅ  ｃｌｏｓｅｄ  ｌｏｏｐ問題を解決でき、ＥＫＦ基盤のＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｔｉｏｎ  Ａｎｄ  Ｍａｐ  Ｂｕｉｌｄｉｎｇ）技法の安定性を確保できる。
【産業上の利用可能性】
【０１３７】
本発明は、ロボットに関連した技術分野に好適に適用され得る。
【図面の簡単な説明】
【０１３８】
【図１】本発明による上部映像を利用した移動体の位置推定及び地図の生成装置の実施形態のブロック図である。
【図２】本発明による上部映像を利用した移動体の位置推定及び地図生成方法の実施形態を説明するためのフローチャートである。
【図３】図１に示すランドマーク生成部の本発明による好ましい実施形態のブロック図である。
【図４】図２に示す第２０ステップについての本発明による実施形態を説明するためのフローチャートである。
【図５】図３に示す映像獲得部の本発明による実施形態のブロック図である。
【図６】図４に示す第５０ステップについての本発明による実施形態を説明するためのフローチャートである。
【図７Ａ】歪曲補正部に入力される映像の一例を示す写真である。
【図７Ｂ】歪曲補正部から出力される映像の一例を示す写真である。
【図８】図３に示す映像特徴情報抽出部の本発明による実施形態のブロック図である。
【図９】図４に示す第５４ステップについての本発明による実施形態を説明するためのフローチャートである。
【図１０Ａ】コーナー点検出部の理解を容易にするための写真である。
【図１０Ｂ】コーナー点検出部の理解を容易にするための写真である。
【図１１】図３に示すランドマーク及びランドマーク候補予測部の本発明による好ましい実施形態のブロック図である。
【図１２】図４に示す第５６ステップについての本発明による好ましい実施形態を説明するためのフローチャートである。
【図１３】映像座標変換部の動作を説明するための座標系を例示的に示す図面である。
【図１４】図１１に示す選択部の理解を容易にするための図面である。
【図１５】位置予測部の理解を容易にするための図面である。
【図１６】図３に示す観測及び登録部の本発明による実施形態のブロック図である。
【図１７】図４に示す第５８ステップについての本発明による実施形態を説明するためのフローチャートである。
【図１８】図１６に示す観測及び登録部の理解を容易にするための例示的な図面である。
【図１９】図３に示すランドマーク候補処理部の本発明による実施形態のブロック図である。
【図２０】図４に示す第６０ステップについての本発明による実施形態を説明するためのフローチャートである。
【図２１】図１９に示す座標推定部で三角法を利用して、高さ情報を抽出する過程の理解を容易にするための図面である。
【図２２】映像座標系を移動体座標系に変換する一例を示す図面である。
【図２３】移動体座標系を基準座標系に変換する一例を示す図面である。
【図２４】処理モデルを説明するための図面である。
【図２５】観測モデルを説明するための図面である。
【図２６】テストするロボットの外観を示す写真である。
【図２７Ａ】ロボットが移動する経路を例示的に示す図面である。
【図２７Ｂ】ロボットが移動する経路を例示的に示す図面である。
【図２８】ロボットの外観を示す図面である。
【図２９Ａ】ロボットの位置測定を示す写真である。
【図２９Ｂ】ロボットの位置測定を示す写真である。
【図２９Ｃ】ロボットの位置測定を示す写真である。
【図２９Ｄ】ロボットの位置測定を示す写真である。
【図３０】図２９Ａから図２９Ｄの測定方法を利用して、１５個のポイントのそれぞれに対して３回測定して、４５個のサンプルデータを獲得する場合、測定データの一例を示す表である。
【図３１】図３０で得られた距離誤差に対するＭｉｎｉｔａｂというデータ分析プログラムを利用して実施した統計的な分析結果を説明するためのグラフである。
【図３２】前記方位角誤差に対してＭｉｎｉｔａｂというデータ分析プログラムを利用して実施した統計的な分析結果を説明するためのグラフである。
【符号の説明】
【０１３９】
３０  映像獲得部
３２  映像バッファ
３４  ランドマーク及びランドマーク候補予測部
３６  映像特徴情報抽出部
３８  観測及び登録部
４０  ランドマーク候補処理部
４２  ランドマーク登録部
ＩＮ１  入力端子
ＯＵＴ２、ＯＵＴ３  出力端子
【図１】
2006209770.tif 000028
【図２】
2006209770.tif 000029
【図３】
2006209770.tif 000030
【図４】
2006209770.tif 000031
【図５】
2006209770.tif 000032
【図６】
2006209770.tif 000033
【図７Ａ】
2006209770.tif 000034
【図７Ｂ】
2006209770.tif 000035
【図８】
2006209770.tif 000036
【図９】
2006209770.tif 000037
【図１０Ａ】
2006209770.tif 000038
【図１０Ｂ】
2006209770.tif 000039
【図１１】
2006209770.tif 000040
【図１２】
2006209770.tif 000041
【図１３】
2006209770.tif 000042
【図１４】
2006209770.tif 000043
【図１５】
2006209770.tif 000044
【図１６】
2006209770.tif 000045
【図１７】
2006209770.tif 000046
【図１８】
2006209770.tif 000047
【図１９】
2006209770.tif 000048
【図２０】
2006209770.tif 000049
【図２１】
2006209770.tif 000050
【図２２】
2006209770.tif 000051
【図２３】
2006209770.tif 000052
【図２４】
2006209770.tif 000053
【図２５】
2006209770.tif 000054
【図２６】
2006209770.tif 000055
【図２７Ａ】
2006209770.tif 000056
【図２７Ｂ】
2006209770.tif 000057
【図２８】
2006209770.tif 000058
【図２９Ａ】
2006209770.tif 000059
【図２９Ｂ】
2006209770.tif 000060
【図２９Ｃ】
2006209770.tif 000061
【図２９Ｄ】
2006209770.tif 000062
【図３０】
2006209770.tif 000063
【図３１】
2006209770.tif 000064
【図３２】
2006209770.tif 000065