JP5745067B2 - Ｖｓｌａｍ最適化のためのシステムおよび方法 - Google Patents

Description

本発明は、一般に、移動型電子装置のナビゲーションに関する。具体的には、本発明は、移動ロボットなどの移動型電子システムに使用することができる位置推定と地図構築技法用のフロントエンドおよびバックエンドの処理に関する。

［関連出願の相互参照］
本出願は、2010年9月24日に出願した米国仮出願第61/404001号の優先権を主張するものであり、その開示内容は、参照によりその全体が本明細書に組み込まれている。本出願は、さらに、SYSTEMS AND METHODS FOR LANDMARK GENERATION FOR VISUAL SIMULTANEOUS LOCALIZATION AND MAPPINGという名称の2003年12月17日に出願した米国特許第7,774,158号公報に関連し、その開示内容は、参照によりその全体が本明細書に組み込まれている。

移動ロボットは、社会において、ますます一般的になっている。これらのロボットは、自動真空掃除機などの様々な形態で具体化することができることが理解されよう。限定されないが、玩具ロボットなどの娯楽用途、空中、深海、低温、放射線、化学暴露、生物学的災害などの人間に不都合な環境における実用性用途、爆発の可能性があるものの処理などの危険作業、崩壊した建物などの狭い空間での作業、清掃などの単純作業の実施などの、移動ロボットの様々な用途を見出すことができる。移動できる従来のロボットは、自動位置推定および/または地図構築機能を含まない。

位置推定技法は、ロボットがその環境に対するその位置を特定する処理を指す。例えば、「純粋な」位置推定システムでは、ロボットは、その環境の地図を提供される。そうした「純粋な」位置推定システムは、手動的技法による地図作成が比較的難しく労力を要する特別な作業なので、不利である。さらに、多くの環境は、静的でない。例えば、室内の家具の再配置は、既存の地図を使用不可能にする可能性がある。その結果、純粋な位置推定システム内の地図は、地図がその環境を正確に表すために、比較的頻繁で高価な更新を受ける。このことは、特に、無人の空中、海中、および地表の航行体に当てはまる可能性がある。

地図構築技法は、ロボットがその環境の地図を構築するプロセスに関する。その環境の地図を自律的に構築し、地図内でそれ自体の位置推定を行うことができるロボットは、有利なことに、比較的高度の自律性を示すことができる。さらに、そうしたロボットは、有利なことに、その環境の変化に適応することができる。地図を構築し、作成地図を使用するこのプロセスは、同時位置推定地図構築(SLAM：Simultaneous Localizatino and Mapping)として知られている。SLAMは、地図の構築(マッピング)および地図の使用(位置推定)に関するが、位置推定に関するプロセスおよび地図構築に関するプロセスは、実際には、システムがSLAMを実行するのに同時に行う必要がないことが理解されよう。例えば、手順を多重的に実施することができる。むしろ、SLAMを実行するために、システムが位置推定も地図構築もできることで十分である。例えば、SLAMシステムは、移動ロボットまたはスマートホンなどの航行体を地図内で位置推定し、さらに地図を更新するために、同じデータを使用することができる。

SLAMプロセスは、通常、ベイズ推定(Bayesian Estimation)などの確率的技法を使用する。ロボットの位置および/またはロボットの地図の様々な仮定などの、動的システムの様々な状態を同時に保持することができる。確率的技法を用いれば、確率密度関数は、システムのこれら様々な状態全体にわたる確率分布を表す。確率密度関数は、「パーティクル(particles)」と呼ばれる有限の数のサンプル点により、またはパラメトリック形式(例えば、ガウス確率分布を表すために平均ベクトルおよび共分散行列を使用する)で近似することができる。

従来のSLAM技法は、比較的多くの欠点を示す。例えば、従来のあるSLAM技法は、レーザー測距器を使用して地図を構築する。そうしたレーザー測距器技法は、正確であるが、人が歩いている環境などの動的環境では比較的信頼できない。それに加えて、レーザー測距器は、比較的高価な装置であり、多くのロボット用途ではコストがかかりすぎる可能性がある。以下の参照文献には、過去のシステムおよび構成要素の全体の概要を提供する。

［参照文献］
[1]M. AgrawalおよびK. Konolige、「Censure:Center surround extremas for realtime feature detection and matching」、ECCV、2008年

[2]Herbert Bay、Andreas Ess、Tinne Tuytelaars、およびLuc Van Gool、「Speeded-up robust features (surf)」、Computer Vision and Image Understanding、110(3)、346〜359頁、2008年

[3]R. O. Castle、G. Klein、およびD. W. Murray、「Video-rate localization in multiple maps for wearable augmented reality」、Proc 12th IEEE Int Symp on Wearable Computers、Pittsburgh PA、9月28日〜10月1日、2008年、15〜22頁、2008年

[4]A. Davison、I. Reid、N. Molton、およびO. Stasse、「Monoslam: Real-time single camera slam」、IEEE Trans. Pattern Anal. Mach. Intell.、29(6): 1052〜1067頁、2007年

[5]Andrew J. Davison、「Real-time simultaneous localisation and mapping with a single camera」、Proc. 9th IEEE International Conference on Computer Vision (ICCV'03)、1403〜1410頁、Nice、France、2003年10月

[6]E. EadeおよびT. Drummond、「Monocular slam as a graph of coalesced observations」、Proc.11th IEEE International Conference on Computer Vision(ICCV'07)、Rio de Janeiro、Brazil、2007年10月

[7]E. EadeおよびT. Drummond、「Unified loop closing and recovery for real time monocular slam」、Proc. British Machine Vision Conference(BM VC '08)、53〜62頁、Leeds、2008年9月、BMVA

[8]M.A. FischlerおよびR.C. Bolles、「Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography」、Communications of the ACM、24(6):381〜395頁、1981年6月

[9]L. Goncalves、E. Di Bernardo、D. Benson、M. Svedman、J. Ostrowski、N. Karlsson、およびP. Pirjanian、「A visual frontend for simultaneous localization and mapping」、Proc. of Int. Conf. on Robotics and Automation(ICRA)、2005年

[10]Giorgio Grisetti、Dario Lodi Rizzini、Cyrill Stachniss、Edwin Olson、およびWolfram Burgard、「Online constraint network optimization for efficient maximum likelihood map learning」、ICRA、1880〜1885頁、Pasadena、California、2008年4月

[11]N. Karlsson、E. di Bernardo、J. Ostrowski、L. Goncalves、P. Pirjanian、およびM.E. Munich、「The vslam algorithm for robust localization and mapping」、Proc. 2005 IEEE International Conference on Robotics and Automation(ICRA '05)、24〜29頁、Barcelona、Spain、2005年4月

[12]G. KleinおよびD. Murray、「Parallel tracking and mapping for small AR workspaces」、Proc. Sixth IEEE and ACM Int'l Symp、Mixed and Augmented Reality、2007年

[13]K. Konolige、J. Bowman、J. D. Chen、P. Mihelich、M. Calonder、V. Lepetit、およびP. Fua、「View-based maps」、Proceedings of Robotics: Science and Systems、Seattle、USA、2009年6月

[14]Kurt Konolige、「Slam via variable reduction from constraint maps」、ICRA、667〜672頁、Barcelona、Spain、2005年4月

[15]Kurt KonoligeおよびMotilal Agrawal、「Frame-frame matching for realtime consistent visual mapping」、Proc. 2007 IEEE International Conference on Robotics and Automation(ICRA '07)、2803〜2810頁、Rome、Italy、2007年4月

[16]David Lowe、「Distinctive image features from scale-invariant keypoints」、International Journal of Computer Vision、60(2):91〜100頁、2004年

[17]J. Matas、O. Chum、M. Urban、およびT. Pajdla、「Robust wide baseline stereo from maximally stable extremal regions」、Proc. British Machine Vision Conference(BMVC '02)、384〜393頁、Cardiff、2002年9月、BMVA

[18]Krystian MikolajczykおよびCordelia Schmid、「A performance evaluation of local descriptors」、IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI)、27(10):1615〜1630頁、2005年

[19]M. Montemerlo、S. Thrun、D. Roller、およびB. Wegbreit、「FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges」、Georg GottlobおよびToby Walsh編集、Proc. Eighteenth International Joint Conference on Artificial Intelligence(IJCAF03)、Acapulco、Mexico、2003年8月、Morgan Kaufmann

[20]David Nister、「An efficient solution to the five-point relative pose problem」、IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI)、26(6):756〜777頁、2004年6月

[21]David NisterおよびHenrik Stewenius、「Scalable recognition with a vocabulary tree」、Proc. IEEE Intl. Conference on Computer Vision and Pattern Recognition (CVPR '06)、2161〜2168頁、New York, NY、USA、2006年6月、IEEE Computer Society

[22]Jonathan R Shewchuk、「An introduction to the conjugate gradient method without the agonizing pain」、Technical report、Pittsburgh, PA、USA、1994年

[23]J. SivicおよびA. Zisserman、「Video Google: A text retrieval approach to object matching in videos」、Proc. 9th IEEE International Conference on Computer Vision(ICCV'03)、1470〜1477頁、Nice、France、2003年10月

[24]S. ThrunおよびM. Montemerlo、「The GraphSLAM algorithm with applications to large-scale mapping of urban structures」、International Journal on Robotics Research、25(5/6) :403〜430頁、2005年

[25]Sebastian ThrunおよびMichael Montemerlo、「The graph slam algorithm with applications to large-scale mapping of urban structures」、The International Journal of Robotics Research、25(5-6):403〜429頁、2006年

[26]Bill Triggs、P. McLauchlan、Richard Hartley、およびA. Fitzgibbon、「Bundle adjustment - a modern synthesis」、B. Triggs、A. Zisserman、およびR. Szeliski編集、Vision Algorithms: Theory and Practice、volume 1883 of Lecture Notes in Computer Science、298〜372頁、Springer- Verlag、2000年

[27]B. Williams、M. Cummins、J. Neira、P. Newman、l. Reid、およびJ. Tardos、「An image-to-map loop closing method for monocular slam」、Proc. Int. Conf. Intelligent Robots and Systems、2008年。掲載予定

ビデオカメラは、その比較的低い価格および高いデータ速度のためにSLAM用の魅力的なセンサである。それらの使用に多大な研究が注がれてきた。

Davisonの[5、4]は、単一のカメラにより小さいスケールで視覚的SLAMを実行するためにカルマンフィルタおよび画像パッチトラッキング(image-patch tracking)を使用する。視覚的特徴点は、ビデオから徐々に選択され、後続のフレームを通してトラッキングされ、すべての測定値は、拡張カルマンフィルタ(EKF)により統合されて共通の状態になる。フィルタ更新のコストは、トラッキングされた特徴点の数と共に二次的に増加し、トラッキングは、不連続な動きまたは低いフレーム速度ではロバストでない。同じプラットホームの後の開発品[27]は、複数の小さい地図をより大きいネットワーク内で接続することができ、トラッキングが失敗したとき、再び画像ベースの位置推定を行う。

Karlssonらの[11、9]は、画像認識システムおよびパーティクルフィルタを使用してSLAMシステムを構築する。その方法は、個々のトラッキングされる特徴点全体にわたるフィルタリングの代わりに、ランドマークと呼ばれる特徴点の集合を構築し、FastSLAM[19]バックエンドフィルタ内にその状態を保持する。パーティクルフィルタは、ランドマークの数が増加するとき、EKFよりも効率的な動作を提供する。認識フロントエンドは、どんなトラッキング仮定にも依存せず、視覚的なループクロージング(loop closure)を自動にする。

Thrunらの[24]は、SLAM状態および観測値を表示するためのグラフベースのフレームワークを提供し、グラフ表示を使用して状態推定値を精細化するための方法を説明する。ロボットの過去のすべての姿勢は、すべての観測値と共にグラフ内に明確に記憶される。グラフの複雑性は、時間と共に徐々に増大し、状態変数は、再帰フィルタ内で長期間合体しない。したがって、反復的精細化により、最終的に、目的関数の非線形性にもかかわらず状態が矛盾のない集束値になる可能性がある。記載されたグラフ精細化は、コンピュータビジョンの構成および運動に通常使用されるバンドル調整法(bundle adjustment method)[26]と構成的には同じである。

Konoligeらの[14]は、過去の姿勢状態を過小評価することにより、そうしたグラフ表示の状態複雑性をどのようにして低減することができるかを示唆する。統計的細分化が記載され、方法が検討されるが、運用用途が提供されていない。

Grisettiらの[10]は、ThrunのグラフフレームワークなどのSLAM拘束ネットワークの漸進的な最適化のための技法を提供する。

Eadeらの[6]は、それぞれが再帰フィルタにより保持された局所地図全体にわたってグラフ構造を構築する。このシステムは、通常の動作中にパッチトラッキングを使用する。再位置推定および復元フレームワーク[7]は、視点不変特徴点および認識方法を使用して局所地図のグラフの頂部上に構築される。状態の統計的推定値全体を精細化するために、グラフが徐々に精細化される。

Kleinらの[12]は、小さい空間内の視覚データのみを使用してトラッキングおよび地図構築をするためのシステムを説明する。キーフレームは、ビデオストリームから選択され、これらのキーフレームの間で対応付けが行われる。すべての対応付けおよびキーフレーム姿勢は、システムの主動作と並行したバンドル調整動作において共に最適化される。各フレームにおいて、画像特徴点は、パッチを使用してトラッキングされ、カメラ姿勢は、グローバル構造の現在の推定値に対して推定される。このシステムは、画像ベースの比較値を使用して再位置推定をサポートするためにCastleらの[3]により拡張される。

Konoligeらの[15]は、SLAMを遂行するためにキーフレームおよびグローバル最適化を使用するKlein'sと同様のシステムを提供する。それに加えて、Konoligeのシステムは、キーフレーム間の複数のトラッキング結果を視覚的オドメトリ測定値内に集約し、視覚的に分離したキーフレーム間のつながりをもたらす。このシステムは、ステレオカメラを有する移動ロボット上で実演される。その際、このシステムは、視覚ベースの認識を使用するために拡張され、トラッキング仮定[13]にはあまり依存しない。

M. AgrawalおよびK. Konolige、「Censure:Center surround extremas for realtime feature detection and matching」、ECCV、2008年 Herbert Bay、Andreas Ess、Tinne Tuytelaars、およびLuc Van Gool、「Speeded-up robust features (surf)」、Computer Vision and Image Understanding、110(3)、346〜359頁、2008年 R. O. Castle、G. Klein、およびD. W. Murray、「Video-rate localization in multiple maps for wearable augmented reality」、Proc 12th IEEE Int Symp on Wearable Computers、Pittsburgh PA、9月28日〜10月1日、2008年、15〜22頁、2008年 A. Davison、I. Reid、N. Molton、およびO. Stasse、「Monoslam: Real-time single camera slam」、IEEE Trans. Pattern Anal. Mach. Intell.、29(6): 1052〜1067頁、2007年 Andrew J. Davison、「Real-time simultaneous localisation and mapping with a single camera」、Proc. 9th IEEE International Conference on Computer Vision(ICCV'03)、1403〜1410頁、Nice、France、2003年10月 E. EadeおよびT. Drummond、「Monocular slam as a graph of coalesced observations」、Proc.11th IEEE International Conference on Computer Vision(ICCV'07)、Rio de Janeiro、Brazil、2007年10月 E. EadeおよびT. Drummond、「Unified loop closing and recovery for real time monocular slam」、Proc. British Machine Vision Conference(BM VC '08)、53〜62頁、Leeds、2008年9月、BMVA M.A. FischlerおよびR.C. Bolles、「Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography」、Communications of the ACM、24(6):381〜395頁、1981年6月 L. Goncalves、E. Di Bernardo、D. Benson、M. Svedman、J. Ostrowski、N. Karlsson、およびP. Pirjanian、「A visual frontend for simultaneous localization and mapping」、Proc. of Int. Conf. on Robotics and Automation(ICRA)、2005年 Giorgio Grisetti、Dario Lodi Rizzini、Cyrill Stachniss、Edwin Olson、およびWolfram Burgard、「Online constraint network optimization for efficient maximum likelihood map learning」、ICRA、1880〜1885頁、Pasadena、California、2008年4月 N. Karlsson、E. di Bernardo、J. Ostrowski、L. Goncalves、P. Pirjanian、およびM.E. Munich、「The vslam algorithm for robust localization and mapping」、Proc. 2005 IEEE International Conference on Robotics and Automation(ICRA '05)、24〜29頁、Barcelona、Spain、2005年4月 G. KleinおよびD. Murray、「Parallel tracking and mapping for small AR workspaces」、Proc. Sixth IEEE and ACM Int'l Symp、Mixed and Augmented Reality、2007年 K. Konolige、J. Bowman、J. D. Chen、P. Mihelich、M. Calonder、V. Lepetit、およびP. Fua、「View-based maps」、Proceedings of Robotics: Science and Systems、Seattle、USA、2009年6月 Kurt Konolige、「Slam via variable reduction from constraint maps」、ICRA、667〜672頁、Barcelona、Spain、2005年4月 Kurt KonoligeおよびMotilal Agrawal、「Frame-frame matching for realtime consistent visual mapping」、Proc. 2007 IEEE International Conference on Robotics and Automation(ICRA '07)、2803〜2810頁、Rome、Italy、2007年4月 David Lowe、「Distinctive image features from scale-invariant keypoints」、International Journal of Computer Vision、60(2):91〜100頁、2004年 J. Matas、O. Chum、M. Urban、およびT. Pajdla、「Robust wide baseline stereo from maximally stable extremal regions」、Proc. British Machine Vision Conference(BMVC '02)、384〜393頁、Cardiff、2002年9月、BMVA Krystian MikolajczykおよびCordelia Schmid、「A performance evaluation of local descriptors」、IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI)、27(10):1615〜1630頁、2005年 M. Montemerlo、S. Thrun、D. Roller、およびB. Wegbreit、「FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges」、Georg GottlobおよびToby Walsh編集、Proc. Eighteenth International Joint Conference on Artificial Intelligence(IJCAF03)、Acapulco、Mexico、2003年8月、Morgan Kaufmann David Nister、「An efficient solution to the five-point relative pose problem」、IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI)、26(6):756〜777頁、2004年6月 David NisterおよびHenrik Stewenius、「Scalable recognition with a vocabulary tree」、Proc. IEEE Intl. Conference on Computer Vision and Pattern Recognition(CVPR '06)、2161〜2168頁、New York, NY、USA、2006年6月、IEEE Computer Society Jonathan R Shewchuk、「An introduction to the conjugate gradient method without the agonizing pain」、Technical report、Pittsburgh, PA、USA、1994年 J. SivicおよびA. Zisserman、「Video Google: A text retrieval approach to object matching in videos」、Proc. 9th IEEE International Conference on Computer Vision(ICCV'03)、1470〜1477頁、Nice、France、2003年10月 S. ThrunおよびM. Montemerlo、「The GraphSLAM algorithm with applications to large-scale mapping of urban structures」、International Journal on Robotics Research、25(5/6) :403〜430頁、2005年 Sebastian ThrunおよびMichael Montemerlo、「The graph slam algorithm with applications to large-scale mapping of urban structures」、The International Journal of Robotics Research、25(5-6):403〜429頁、2006年 Bill Triggs、P. McLauchlan、Richard Hartley、およびA. Fitzgibbon、「Bundle adjustment - a modern synthesis」、B. Triggs、A. Zisserman、およびR. Szeliski編集、Vision Algorithms: Theory and Practice、volume 1883 of Lecture Notes in Computer Science、298〜372頁、Springer- Verlag、2000年 B. Williams、M. Cummins、J. Neira、P. Newman、l. Reid、およびJ. Tardos、「An image-to-map loop closing method for monocular slam」、Proc. Int. Conf. Intelligent Robots and Systems、2008年 David G. Lowe、「Local Feature View Clustering for 3D Object Recognition」、Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition、Kauai、Hawaii(2001年12月) Olivier FaugerasおよびQuang-Tuan Luong、「The Geometry of Multiple Images」、MIT Press(2001年)

当分野における多大な努力にもかかわらず、SLAMグラフをシステム更新および分析をより反映する形式に迅速に再構築するための効率的な方法が未だに必要である。すなわち、所望のレベルの情報内容を保持しながら、SLAM動作の速度を改善するグラフ管理方法が必要である。

いくつかの実施形態は、プロセッサおよびカメラを含むシステム内で位置推定および地図構築を行うための方法を企図し、プロセッサは、複数の姿勢ノードおよび複数のエッジを含むグラフを作成するように構成される。本方法は、グラフ内の姿勢ノードの数が第1の閾値を超える場合にグラフを更新するステップを含み、更新するステップは、i) 2つ以上のインシデントエッジ(incident edge)により関連のマルコフブランケットノードに直接結合する姿勢ノードを特定するステップと、ii) 前記関連のマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップと、iii) 特定された姿勢ノードおよび前記2つ以上のインシデントエッジを除去するステップとを含む。本方法は、グラフ内のエッジの総数が第2の閾値を超える場合、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップを含むこともできる。本方法は、グラフ内に存在する複数のエッジに少なくとも部分的に基づいて残りの姿勢ノードの位置の推定値を更新するステップを含むこともできる。

いくつかの実施形態では、姿勢ノードを特定するステップは、i) グラフ内の各姿勢ノードに関して、姿勢ノードが除去されたときグラフ内に存在するであろうエッジ、およびマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために合成されたインシデントエッジの総数を特定するステップと、ii) グラフから除去される場合にエッジの最小の総数をもたらすであろう姿勢ノードを選択するステップとを含む。いくつかの実施形態では、前記インシデントエッジを合成するステップは、i) マルコフブランケットノードの対の間の相対姿勢の推定値を生成するステップであって、相対姿勢推定値は、2つ以上のインシデントエッジに関連する相対姿勢測定値のベクトル和に部分的に基づいている、ステップと、ii) 2つ以上のインシデントエッジに関連する共分散推定値に基づいて1つの共分散推定値を生成するステップとを含む。いくつかの実施形態では、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップは、i) グラフ内の各エッジの残差値を生成するステップであって、残差値は、グラフ内のエッジにより接続されるノードの相対姿勢と、同じエッジに関連する変換値により与えられる相対姿勢との間の差に少なくとも部分的に基づいている、ステップと、ii) 最小の残差値を有するエッジを特定するステップと、iii) 特定されたエッジをグラフから除去するステップとを含む。いくつかの実施形態では、本システムは、ナビゲーションシステムおよび経路プランナ(path planner)をさらに含む移動ロボットであり、本方法は、i) 軌道を走行するために1組のロボット移動命令を生成するステップと、ii) グラフ内に存在する姿勢ノードの位置の推定値を更新した後、軌道ならびにグラフ内に存在する姿勢ノードおよび複数のランドマーク姿勢ノードの位置の更新された推定値に少なくとも部分的に基づいてロボットをナビゲートするための1組の移動命令を更新するステップとをさらに含む。いくつかの実施形態では、姿勢ノードは、ロボットの姿勢を含み、ランドマークノードは、ロボットの姿勢、1つまたは複数の物体に対応するランドマーク識別子、および1つまたは複数の物体のそれぞれの位置の推定値を含み、エッジは、2つの位置におけるロボットの位置および方位に関する剛体
変換を含む。いくつかの実施形態では、本方法は、軌道を走行しながら少なくとも1つの画像を取得するステップと、1つまたは複数の画像が既知のランドマークまたは新しいランドマークを示すかどうかを特定するステップと、グラフを更新するステップとをさらに含む。これらの実施形態では、グラフを更新するステップは、i) 少なくとも1つの画像が既知のランドマークを示すとき、新しい姿勢ノードを生成するステップと、ii) 少なくとも1つの画像が新しいランドマークを示すとき、新しいランドマーク姿勢ノードを生成するステップと、iii) 新しい姿勢ノードまたは新しいランドマーク姿勢ノードをグラフ内の1つまたは複数の既存のノードに関連付ける少なくとも1つの新しいエッジを生成するステップとを含む。いくつかの実施形態は、それぞれがscale-invariant feature transform(SIFT)特徴点を含む、複数の特徴点を抽出するステップをさらに含む。いくつかの実施形態では、前記関連のマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップは、新しいエッジの平均値と既存のエッジの平均値との重み付き平均値を生成し、新しいエッジの共分散推定値および既存のエッジの共分散推定値の逆行列の和に基づいて新しい共分散推定値を生成することにより、前記新しいエッジの少なくとも1つを既存のエッジと組み合わせるステップをさらに含む。

移動型電子装置は、画像を取得するように構成されたカメラと、複数の姿勢ノードおよびエッジを含むグラフを保持するように構成されたナビゲーションシステムとを含むことをいくつかの実施形態は企図する。これらの実施形態では、ナビゲーションシステムは、グラフ内の姿勢ノードの数が第1の閾値を超える場合にグラフを更新するように構成され、更新は、i) 2つ以上のインシデントエッジにより関連のマルコフブランケットノードに直接結合する姿勢ノードを特定するステップと、ii) 前記関連のマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップと、iii) 特定された姿勢ノードおよび前記2つ以上のインシデントエッジを除去するステップとを含む。本ナビゲーションシステムは、グラフ内のエッジの総数が第2の閾値を超える場合に、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去し、グラフ内に存在する複数のエッジに少なくとも部分的に基づいて残りの姿勢ノードのそれぞれの位置の推定値を更新するように構成することもできる。

いくつかの実施形態では、姿勢ノードを特定するステップは、i) グラフ内の各姿勢ノードに関して、姿勢ノードが除去されたときグラフ内に存在するであろうエッジ、およびブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために合成されたインシデントエッジの総数を特定するステップと、ii) グラフから除去される場合にエッジの最小の総数をもたらすであろう姿勢ノードを選択するステップとを含む。いくつかの実施形態では、前記インシデントエッジを合成するステップは、i) ブランケットノードの対の間の相対姿勢の推定値を生成するステップであって、相対姿勢推定値は、2つ以上のインシデントエッジに関連する相対姿勢測定値のベクトル和である、ステップと、ii) 2つ以上のインシデントエッジに関連する共分散推定値に基づいて1つの共分散推定値を生成するステップとを含む。いくつかの実施形態では、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップは、i) グラフ内の各エッジの残差値を生成するステップであって、残差値は、更新されたグラフ内のエッジにより接続されるノードの相対姿勢と、同じエッジの平均値により与えられる相対姿勢との間の差に少なくとも部分的に基づいている、ステップと、ii) 最小の残差値を有するエッジを特定するステップと、iii) 特定されたエッジをグラフから除去するステップとを含む。いくつかの実施形態では、移動型電子装置は、経路プランナをさらに含む移動ロボットである。これらの実施形態では、本プランナは、i)軌道を走行するために1組のロボット移動命令を生成し、ii) 残りの姿勢ノードの位置の推定値を更新した後、軌道および残りの姿勢ノードの位置の更新された推定値に少なくとも部分的に基づいてロボットをナビゲートするための1組の移動命令を更新するようにさらに構成することができる。いくつかの実施形態では、姿勢ノードは、ロボットの姿勢を含み、ランドマークノードは、ロボットの姿勢、1つまたは複数の物体に対応するランドマーク識別子、および1つまたは複数の物体のそれぞれの位置の推定値を含み、エッジは、2つの位置におけるロボットの位置および方位に関する剛体変換を含む。いくつかの実施形態では、ナビゲーションシステムは、軌道を走行しながら少なくとも1つの画像を取得し、1つまたは複数の画像が既知のランドマークまたは新しいランドマークを示すかどうかを特定し、グラフを更新するようにさらに構成される。グラフを更新するステップは、i) 少なくとも1つの画像が既知のランドマークを示すとき、新しい姿勢ノードを生成するステップと、ii) 少なくとも1つの画像が新しいランドマークを示すとき、新しいランドマークノードを生成するステップと、iii) 新しい姿勢ノードまたは新しいランドマークノードをグラフ内の1つまたは複数の既存のノードに関連付ける少なくとも1つの新しいエッジを生成するステップとを含むことができる。いくつかの実施形態では、ナビゲーションシステムは、それぞれがscale-invariant feature transform(SIFT)特徴点を含む、複数の特徴点を抽出するようにさらに構成される。いくつかの実施形態では、前記関連のブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップは、新しいエッジと既存のエッジとの相対姿勢の推定値を平均し、新しいエッジと既存のエッジとの共分散推定値を平均することにより、前記新しいエッジの少なくとも1つを既存のエッジと組み合わせるステップをさらに含む。

いくつかの実施形態は、移動型システムをナビゲートするための方法を開示し、1つまたは複数のコンピュータシステム上に実装される本方法は、移動型装置内のランドマークを整合させるステップを含む。ランドマークを整合させるステップは、グローバルデータベースから特徴点を検索するステップと、視覚的類似性によりランドマークをランキングするステップと、複数の候補ランドマークを選択するステップと、複数の候補ランドマークのそれぞれに関して、局所データベース内の特徴点を検索するステップと、ロバストな姿勢推定を行うステップと、バンドル調整を行うステップと、観測姿勢および共分散を特定するステップと、整合用ランドマークとして最高の候補を選択するステップとを含むことができる。

いくつかの実施形態は、移動型システムをナビゲートするための方法を企図し、1つまたは複数のコンピュータシステム上に実装される本方法は、移動型装置内にランドマークを生成するステップを含む。ランドマークを生成するステップは、カメラモーションおよびエピポーラ幾何学により正対応整合点を見出すステップと、正対応整合点を使用してカメラモーションを精細化するステップと、十分な正対応点が存在するかどうかを特定するステップと、許容差(tolerance)に達したかどうかを特定するステップと、許容差に達したことの特定に基づいて許容差を調整するステップと、新しいランドマークとして正対応点およびカメラモーションを戻すステップとを含むことができる。

非一時的コンピュータ読取可能媒体は、複数の姿勢ノードおよび複数のエッジを含むグラフを移動型電子装置と通信する1つまたは複数のコンピュータシステムに生成させるように構成された命令を含むことをいくつかの実施形態は企図する。本媒体は、グラフ内の姿勢ノードの数が第1の閾値を超える場合にシステムにグラフを更新させることもでき、更新は、i) 2つ以上のインシデントエッジにより関連のブランケットノードに直接結合する姿勢ノードを特定するステップと、ii) 前記関連のブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップと、iii) 特定された姿勢ノードおよび前記2つ以上のインシデントエッジを除去するステップとを含む。本媒体は、グラフ内のエッジの総数が第2の閾値を超える場合に、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジをシステムに除去させることもできる。本媒体は、グラフ内に存在する複数のエッジに少なくとも部分的に基づいて残りの姿勢ノードのそれぞれの位置の推定値をシステムに更新させることもできる。

いくつかの実施形態では、姿勢ノードを特定するステップは、i) グラフ内の各姿勢ノードに関して、姿勢ノードが除去されたときグラフ内に存在するであろうエッジ、およびブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために合成されたインシデントエッジの総数を特定するステップと、ii) グラフから除去される場合にエッジの最小の総数をもたらすであろう姿勢ノードを選択するステップとを含む。いくつかの実施形態では、前記インシデントエッジを合成するステップは、i) ブランケットノードの対の間の相対姿勢の推定値を生成するステップであって、相対姿勢推定値は、2つ以上のインシデントエッジに関連する相対姿勢測定値のベクトル和である、ステップと、ii) 2つ以上のインシデントエッジに関連する共分散推定値に基づいて1つの共分散推定値を生成するステップとを含む。いくつかの実施形態では、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップは、i) 現在のグラフ平均値を生成するステップと、ii) 各エッジに関する誤差推定値を生成するステップであって、誤差推定値は、グラフ内に存在する複数のエッジのそれぞれの姿勢と現在のグラフ平均値との間の差に少なくとも部分的に基づいている、ステップと、iii) 最低の誤差推定値を有する少なくとも1つのエッジを特定するステップと、iv) 少なくとも1つの特定されたエッジをグラフから除去するステップとを含む。いくつかの実施形態では、本システムは、ナビゲーションシステムおよび経路プランナをさらに含む移動ロボットである。いくつかの実施形態では、本システムは、ナビゲーションシステムおよび経路プランナをさらに含む移動ロボットである。いくつかの実施形態では、本方法は、i) 軌道を走行するために1組のロボット移動命令を生成するステップと、ii) 残りの姿勢ノードの位置の推定値を更新した後、軌道および残りの姿勢ノードの位置の更新された推定値に少なくとも部分的に基づいてロボットをナビゲートするための1組の移動命令を更新するステップとをさらに含む。いくつかの実施形態では、i) 姿勢ノードは、ロボットの姿勢を含み、ii) ランドマークノードは、ロボットの姿勢、1つまたは複数の物体に対応するランドマーク識別子、および1つまたは複数の物体のそれぞれの位置
の推定値を含み、iii) エッジは、2つの位置におけるロボットの位置および方位に関する剛体変換を含む。いくつかの実施形態では、本方法は、軌道を走行しながら少なくとも1つの画像を取得するステップと、1つまたは複数の画像が既知のランドマークまたは新しいランドマークを示すかどうかを特定するステップとをさらに含む。これらの実施形態では、グラフを更新するステップは、i) 少なくとも1つの画像が既知のランドマークを示すとき、新しい姿勢ノードを生成するステップと、ii) 少なくとも1つの画像が新しいランドマークを示すとき、新しいランドマークノードを生成するステップと、iii) 新しい姿勢ノードまたは新しいランドマークノードをグラフ内の1つまたは複数の既存のノードに関連付ける少なくとも1つの新しいエッジを生成するステップとを含むことができる。いくつかの実施形態では、本方法は、それぞれがscale-invariant feature transform(SIFT)特徴点を含む、複数の特徴点を抽出するステップをさらに含む。いくつかの実施形態では、前記関連のブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップは、i)新しいエッジと既存のエッジとの相対姿勢の推定値を平均し、新しいエッジと既存のエッジとの共分散推定値を平均することにより、前記新しいエッジの少なくとも1つを既存のエッジと組み合わせるステップをさらに含む。

移動型電子装置は、画像を取得するように構成されたカメラと、複数の姿勢ノードおよびエッジを含むグラフを保持するように構成されたナビゲーションシステムとを含むことをいくつかの実施形態は企図する。本ナビゲーションシステムは、グラフ内の姿勢ノードの数が第1の閾値を超える場合にグラフを更新するように構成することができる。グラフを更新するステップは、i) 2つ以上のインシデントエッジにより関連のブランケットノードに直接結合する姿勢ノードを特定するステップと、ii) 前記関連のブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップと、iii) 特定された姿勢ノードおよび前記2つ以上のインシデントエッジを除去するステップとを含むことができる。本ナビゲーションシステムは、グラフ内のエッジの総数が第2の閾値を超える場合に、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去し、グラフ内に存在する複数のエッジに少なくとも部分的に基づいて残りの姿勢ノードのそれぞれの位置の推定値を更新するように構成することもできる。

いくつかの実施形態では、姿勢ノードを特定するステップは、i) グラフ内の各姿勢ノードに関して、姿勢ノードが除去されたときグラフ内に存在するであろうエッジ、およびブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために合成されたインシデントエッジの総数を特定するステップと、ii) グラフから除去される場合にエッジの最小の総数をもたらすであろう姿勢ノードを選択するステップとを含む。いくつかの実施形態では、前記インシデントエッジを合成するステップは、i) ブランケットノードの対の間の相対姿勢の推定値を生成するステップであって、相対姿勢推定値は、2つ以上のインシデントエッジに関連する相対姿勢測定値のベクトル和である、ステップと、ii) 2つ以上のインシデントエッジに関連する共分散推定値に基づいて1つの共分散推定値を生成するステップとを含む。いくつかの実施形態では、グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップは、i) 現在のグラフ平均値を生成するステップと、ii) 各エッジに関する誤差推定値を生成するステップであって、誤差推定値は、グラフ内に存在する複数のエッジのそれぞれの姿勢と現在のグラフ平均値との間の差に少なくとも部分的に基づいている、ステップと、iii) 最低の誤差推定値を有する少なくとも1つのエッジを特定するステップと、iv) 少なくとも1つの特定されたエッジをグラフから除去するステップとを含む。いくつかの実施形態では、本システムは、ナビゲーションシステムおよび経路プランナをさらに含む移動ロボットである。いくつかの実施形態では、本システムは、移動ロボットであり、本方法は、i) 軌道を走行するために1組のロボット移動命令を生成するステップと、ii) 残りの姿勢ノードの位置の推定値を更新した後、軌道および残りの姿勢ノードの位置の更新された推定値に少なくとも部分的に基づいてロボットをナビゲートするための1組の移動命令を更新するステップとをさらに含む。いくつかの実施形態では、姿勢ノードは、ロボットの姿勢を含み、ランドマークノードは、ロボットの姿勢、1つまたは複数の物体に対応するランドマーク識別子、および1つまたは複数の物体のそれぞれの位置の推定値を含み、エッジは、2つの位置におけるロボットの位置および方位に関する剛体変換を含む。いくつかの実施形態では、ナビゲーションシステムは、軌道を走行しながら少なくとも1つの画像を取得し、1つまたは複数の画像が既知のランドマークまたは新しいランドマークを示すかどうかを特定するようにさらに構成される。本システムは、i) 少なくとも1つの画像が既知のランドマークを示すとき、新しい姿勢ノードを生成し、ii) 少なくとも1つの画像が新しいランドマークを示すとき、新しいランドマークノードを生成し、iii)新しい姿勢ノードまたは新しいランドマークノードをグラフ内の1つまたは複数の既存のノードに関連付ける少なくとも1つの新しいエッジを生成することによりグラフを更新することもできる。いくつかの実施形態では、ナビゲーションシステムは、それぞれがscale-invariant feature transform(SIFT)特徴点を含む、複数の特徴点を抽出するようにさらに構成される。いくつかの実施形態では、ブランケットノードは、選択されたノードのすぐ近くのノードを含む。いくつかの実施形態では、前記関連のブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップは、i)新しいエッジと既存のエッジとの相対姿勢の推定値を平均し、新しいエッジと既存のエッジとの共分散推定値を平均することにより、前記新しいエッジの少なくとも1つを既存のエッジと組み合わせるステップをさらに含む。

いくつかの実施形態は、移動型システムをナビゲートするための方法を企図し、1つまたは複数のコンピュータシステム上に実装される本方法は、グローバルデータベースから特徴点を検索し、視覚的類似性によりランドマークをランキングし、複数の候補ランドマークを選択することにより、移動型装置内のランドマークを整合させるステップを含む。本方法は、複数の候補ランドマークのそれぞれに関して、局所データベース内の特徴点を検索するステップと、ロバストな姿勢推定を行うステップと、バンドル調整を行うステップと、観測姿勢および共分散を特定するステップと、整合用ランドマークとして最高の候補を選択するステップとを含むこともできる。

いくつかの実施形態は、移動型システムをナビゲートするための方法を企図し、1つまたは複数のコンピュータシステム上に実装される本方法は、カメラモーションおよびエピポーラ幾何学により正対応整合点を見出し、正対応整合点を使用してカメラモーションを精細化し、十分な正対応点が存在するかどうかを特定し、許容差に達したかどうかを特定し、許容差に達したことの特定に基づいて許容差を調整し、新しいランドマークとして正対応点およびカメラモーションを戻すことにより、移動型装置内にランドマークを生成するステップを含む。

ここで、本発明のこれらおよび他の特徴は、以下に簡単に説明する図面を参照して説明する。これらの図面(縮尺は正確でない)および関連説明は、本発明の好ましい実施形態を説明するために提供され、本発明の範囲を限定するものではない。

ロボットの一例を示す図である。 ランドマークデータベース内に記録を作成することに関する、ロボット基準座標系を示す図である。 ランドマークを再訪することに関する、ランドマーク基準座標系およびロボット基準座標系を示す図である。 ΔxおよびΔyの計算を説明するために使用される表記法を示す図である。 Δθの計算を説明するために使用される表記法を示す図である。 VSLAMシステムのシステムアーキテクチャの1つの実施形態を示す図である。 ランドマーク姿勢ノードおよびロボットの他の姿勢ノードを表すSLAMグラフである。 SLAMグラフのノードおよびエッジに関する様々な情報を示す図である。 新しいランドマークを生成するために視覚的フロントエンドにおいて役立つプロセスのフローチャートである。 いくつかの実施形態において説明されるフロントエンド処理の概要を示すフローチャートである。 図7のフローチャートの状態2702、すなわちいくつかの実施形態において実施されるランドマーク整合プロセスに関して詳細に説明するフローチャートである。 図7の状態2705、すなわちランドマーク生成プロセスの1つの実施形態に関して詳細に説明するフローチャートである。 図9のプロセス1616、すなわちランドマーク生成のために3次元構造およびカメラモーションを推定するプロセスに関して詳細に説明するフローチャートである。 いくつかの実施形態において説明するバックエンド処理の概要を示すフローチャートである。 図11のフローチャートの状態2602に関して詳細に説明し、すなわちランドマークをグラフバックエンドに組み込むためのプロセスを説明するフローチャートである。 図11のフローチャートの状態2606に関して詳細に説明し、すなわち観測値をSLAMグラフに組み込むためのプロセスを説明するフローチャートである。 図15〜17に説明する動作のいくつかの後のSLAMグラフの変化した構造を示す図である。 図11のフローチャートの状態2607、すなわちSLAMグラフの複雑性の低減に関して詳細に説明するフローチャートである。 図15のフローチャートの状態2802、すなわちノード選択および除去に関して詳細に説明するフローチャートである。 図16のフローチャートの状態2302、すなわちSLAMグラフから除去すべき姿勢ノードの選択に関して詳細に説明するフローチャートである。 図15のフローチャートの状態2804、すなわちエッジの選択および除去に関して詳細に説明するフローチャートである。 図11のフローチャートの状態2610、すなわちグラフ最適化のための1つの方法に関して詳細に説明するフローチャートである。

［用語集］
以下の用語集は、以下の語句のそれぞれの例を提供する。

姿勢:いくつかの基準座標系におけるロボットの位置および方位などの位置および方位。

ロボット姿勢(グローバルロボット姿勢としても知られている):グローバル基準座標系(global reference frame)におけるロボットの位置および方位。ロボットが床の表面などに沿って、2次元内を走行する設定では、ロボット姿勢は、2次元位置(x,y)および方位(θ)により特徴づけることができる。

相対ロボット姿勢:ランドマーク基準座標系などの別の基準座標系に対するロボットの位置および方位。

グローバル基準座標系:環境に固定された基準座標系。

ランドマーク基準座標系:ランドマークの3D構造が規定された基準座標系。

3D構造:1組の3D特徴点の3D座標。

ランドマーク:ランドマークは、3次元(3D)特徴点(3D features)および固有の識別子の集合を含む。

3D特徴点:例えば、基準座標系における関連の3D座標、およびある位置を見る際に観測できる1つまたは複数の関連の2D特徴点を有する物体の一部分などの観測できる位置。3D特徴点は、様々な2D特徴点を有する1つまたは複数の斜視図から観測することができることが理解されよう。

2D特徴点:ある位置のピクセル、およびその位置のある程度近傍のピクセルに関する画像およびディスクリプタ(descriptor)の位置。

物理的ランドマーク:環境において視覚的に識別可能な1つまたは複数の3D特徴点からなる集合。

ランドマーク姿勢:グローバル基準座標系におけるランドマーク基準座標系の姿勢。

カメラ姿勢:例えばカメラである可能性がある視覚センサの位置に基づくランドマーク基準座標系の相対姿勢。

本発明は、いくつかの好ましい実施形態に関して説明するが、本明細書に記載する利点および特徴のすべてを提供するわけではない実施形態を含む、当業者には明らかな他の実施形態も本発明の範囲内にある。

有利なことに、本発明の実施形態は、1つまたは複数の視覚センサ、および同時位置推定地図構築(SLAM)を処理するための1つまたは複数のデッドレコニングセンサを使用する。SLAMと視覚センサとの組合せは、以後、VSLAMと呼ぶ。そうした視覚的技法は、有利なことに、地図を自律的に生成および更新するために、移動ロボットなどの航行体により使用することができる。1つの実施形態では、VSLAMは、有利なことに、航行体の「アーム」などの航行体の一部分により使用される。レーザー測距器または他の距離ベースの装置もしくはセンサを使用する位置推定地図構築技法と対照的に、これらの視覚的技法は、広範囲の用途で経済的実用性があり、人が移動する環境などの、比較的動的な環境で使用することができる。いくつかの実施形態は、比較的計算効率の良い方式でSLAM情報の代表値を保持し、それにより、比較的安価なマイクロプロセッサベースのコンピュータシステムを使用してソフトウェア内でSLAMプロセスを実行することができる。

移動ロボットは、様々な形態で具体化することができることが当業者には理解されよう。これらの様々な形態では、ロボットは、異なる名称で、例えばロボットが実行する機能で呼ばれる可能性がある。例えば、ロボットは、自動掃除機または自動真空掃除機と呼ばれる可能性がある。1つの実施形態では、移動ロボットは、自律的または半自律的にナビゲートすることができる自己推進物体に相当する。自律型または半自律型移動ロボットの例には、限定されないが、自動床掃除機に使用するための移動ロボット、ヒューマノイドロボット、実験用および研究室用のロボット、物品を配送するためのロボット、閉鎖空間またはアクセスできない空間を調査するためのロボット、娯楽または遊戯用のロボットなどが含まれる。

本明細書に開示するVSLAM技法は、有利なことに、自律型ロボットおよび非自律型ロボットに適用することができる。例えば、VSLAM技法は、爆弾検出用の遠隔制御航行体または他の移動型電子装置などの手動駆動航行体と共に使用することができる。例えば、VSLAM技法は、有利なことに、オペレータが周辺環境をナビゲートするのを助けるために遠隔制御用途で使用することができる。1つの実施形態では、航行体は、航行体の手動制御用のモードおよび航行体の自律制御用の別のモードなどの様々な動作モードを含むことができる。例えば、航行体は、最初の地図構築段階の間に手動駆動することができ、次いで後に、自律制御用に設定することができる。別の実施形態では、VSLAM技法は、ある領域の地図を作成するために監視者により使用される可能性がある。監視者は、例えば人または犬もしくはネズミなどの別の動物に相当する可能性がある。監視者が使用するVSLAMは、環境を観測するために監視者が携帯するビデオカメラ、および変位を測定するためのオドメータ、歩数計、GPSセンサ、慣性センサなどのデッドレコニング装置に結合することができる。監視者が作成する地図は、記憶され、後に監視者または自律型ロボットなどの別のエンティティにより再び使用される可能性がある。監視者による地図の作成と別のエンティティによる地図の使用との間で、視覚センサの差、視覚センサの設置高さの差などを調節するために追加の処理が存在する可能性があることが理解されよう。本明細書に開示する様々な実施形態を使用することができる、他の様々な非自律型システムを容易に認識することができる。例えば、スマートホン、タブレット、および他の移動型装置は、同様に、これらの実施形態を使用することができ、ジャイロ、加速度計、および本明細書に開示する動作を実行するための他のIMUユニットを使用することができる。タブレット、スマートホン、または他の移動型装置は、本明細書に説明する、ナビゲーションシステムおよび/またはプランナとして役立つ可能性があることが認識されよう。いくつかの実施形態では、移動型システムは、例えば、スマートホンがロボット胴体に挿入されるとき、ロボットと連携することができる。

ロボットは、様々な設定に指定することができる。ロボット設定は、通常、少なくとも1つのデッドレコニングセンサおよび少なくとも1つのビデオセンサを含む。デッドレコニングの別の名称は、「ded」レコニングまたは推定レコニングである。デッドレコニングセンサの一例は、光学ホイールエンコーダなどのセンサがホイールの回転を測定する、ホイールオドメータである。ホイールの回転は、走行距離を示すことができ、ホイールの回転の差は、方位の変化を示すことができる。デッドレコニングを用いて、ロボットは、前の位置および方位(姿勢)から進んだコースおよび距離を計算し、この情報を使用して現在の位置および方位(姿勢)を推定することができる。デッドレコニング検知は、比較的短い距離にわたって比較的正確であるが、時間と共にドリフトしやすい。デッドレコニングセンサにより提供される情報は、距離、速度、または加速度のいずれかに相当し、場合に応じて変換することができることが理解されよう。他の形態のデッドレコニングは、歩数計(歩行ロボット用)、慣性測定ユニットからの測定値、光学マウス装置に使用されるセンサなどの光学センサなどを含むことができる。不利なことに、デッドレコニング測定では、ドリフト誤差が累積する可能性がある。ホイールオドメータに関して、ドリフト発生源の例には、校正誤差、ホイール滑りなどが含まれる。これらのドリフト発生源は、距離計算にも方位計算にも影響を及ぼす可能性がある。

視覚センサの一例は、デジタルカメラである。本発明の実施形態は、有利なことに、視覚ベースでランドマークを認識するために視覚センサを使用する。視覚的ランドマークのこれらの観測は、有利なことに、位置のグローバル表示をもたらし、デッドレコニングセンサにおけるドリフトを補正するために使用することができる。レーザー測距器を使用する同時位置推定地図構築(SLAM)と対照的に、本発明の実施形態は、視覚センサおよびデッドレコニングセンサからのデータを使用し、有利なことに、追加コストがほとんど、または全くかかることなく、同時位置推定地図構築(SLAM)を提供することができる。

［例示的なVSLAM式ロボット］
図1は、VSLAMシステムを組み込むことができる移動ロボット100の一例を示す。図示したロボット100は、ランドマークを視覚的に認識するために使用され、その結果、SLAMモジュールがグローバル位置を特定することができる、視覚センサ104を含む。視覚センサ104には、幅広い視覚センサを使用することができる。例えば、視覚センサ104は、CCD撮像素子、CMOS撮像素子、赤外線撮像素子などを有するデジタルカメラに相当する可能性がある。視覚センサ104は、通常のレンズ、または広角レンズ、魚眼レンズ、全方位レンズなどの特殊なレンズを含むことができる。さらに、レンズは、比較的広い視野または複数の視点を提供するために使用することができる、平面鏡、放物面鏡、または円錐鏡などの反射面を含むことができる。別の例では、視覚センサ104は、単一のカメラまたは複数のカメラに相当する可能性がある。1つの実施形態では、VSLAMシステムは、有利なことに、複数のカメラに比べて有利にもコストを低減する、単一のカメラと共に動作するように設定される。

図示したロボット100のモータ110、112は、ロボット100に移動力をもたらすためにホイール114、116に連結する。ホイールの代わりに、またはホイールに加えて、ロボットの他の実施形態は、動き回るために、脚、軌道、ローラ、プロペラなどを使用することができることが当業者には理解されよう。図示した実施形態では、オドメトリとしても知られている、ホイールの回転に関する情報は、制御装置108への入力として提供される。視覚センサ104からの画像データ106も、制御装置108への入力としてロボット100に提供される。1つの実施形態では、VSLAMシステムは、制御装置108内に実装される。図示した実施形態では、制御装置108は、ロボット100の動きを制御するためにモータ110、112結合する。明確にするために、バッテリなどのロボット100用の電源を図1に示していない。

画像データ106に応答して、制御装置108は、ロボット100の動きを制御するモータ110、112に制御信号を提供することができる。例えば、制御装置108は、例えば、前進し、停止し、後退し、旋回し、垂直軸の周りに回転するようにロボットに指示するために制御信号を提供することができる。ロボットが、図1に示す例示的な垂直軸118などの垂直軸の周りに回転するとき、この回転は、「ヨー」と呼ばれる。

制御装置108は、マイクロプロセッサ、メモリなどのハードウェア、ファームウェア、ソフトウェア、ネットワーク通信装置などを含むことができる。1つの実施形態では、制御装置108は、単一ボードコンピュータ、特定用途向集積回路(ASIC)、フィールドプログラマブルゲートアレイ(FPGA)などの専用ハードウェアを使用する。

1つの実施形態では、制御装置108は、ラップトップコンピュータなどの汎用コンピュータと接続することにより、またコンピュータ内で実行するソフトウェアにより実装される。1つの例では、2.4GHzのクロック速度のIntel(登録商標)Pentium(登録商標)4プロセッサを含むラップトップコンピュータは、約1秒でランドマーク作成処理を行うことができ、約0.5秒で視覚測定値を処理することができる。処理時間は、画像解像度、フレーム速度、バス速度などのパラメータに依存している可能性があるが理解されよう。ソフトウェアは、ハードディスクまたは光ディスクなどの有形媒体内に実装される命令を含むことができる。ロボット100用のデータ処理は、ロボット100が自律的であるように、完全にロボット100内で行うことができ、または、データ処理は、ロボット100の外部で部分的に行うことができる。例えば、制御装置108は、データ処理の一部分が行われる無線ネットワークなどのネットワークを介して、データを別のコンピュータに伝送するように設定することができる。

［VSLAM動作の特性］
VSLAMを有するロボットがその環境内を走行するとき、ロボットは、物理的ランドマークを観測することができる。後にさらに詳しく説明するように、これらの物理的ランドマークは、作成されデータベース内に記憶されるランドマークに関係する可能性がある。有利なことに、VSLAM技法は、人工的なナビゲーション用ビーコンが環境内に存在することを必要としない。むしろ、VSLAM技法は、不変で無修正の環境において都合よく使用することができる。しかし、人工的なナビゲーション用ビーコンが環境内に存在するとき、VSLAM技法は、ビーコンおよび/または周辺環境からの特徴点をランドマークとして使用することができることが理解されよう。例えば、ランドマークデータベースでは、ランドマークは、3D特徴点と、3D特徴点が計算される対応する2D特徴点との集合に対応する可能性がある。物理的ランドマークは、例えば壁および壁の一部分に取り付けられた物体などの1つまたは複数の物理的対象物に相当する可能性があることにも留意されたい。これらの物理的ランドマークは、グローバル位置を推定するために使用され、その結果、デッドレコニング測定値のドリフトを後に補正または補償することができる。物理的ランドマークは、通常、グローバル基準座標系における特定の位置および方位に配置され、観測用ロボットは、異なる位置および方位にあることに留意されたい。いくつかの実施形態では、物理的ランドマークの特徴点の位置は、ランドマーク基準座標系を基準とする。その際、ランドマーク自体の位置は、グローバル基準座標系を基準とする。

全体的にVSLAMシステムをロボットに関して説明するが、VSLAMシステムは、様々な装置で使用することもでき、その結果、ロボット姿勢は、装置の姿勢に相当する可能性もあることが理解されよう。ロボットが物理的ランドマークを観測しデータベース内にランドマークを作成するときのロボットの方位(θ)を矢印で示す。1つの実施形態では、グローバル基準座標系において参照される「ランドマーク」の姿勢の初期推定値は、ランドマークを作成する際のロボット姿勢に相当する。新しい物理的ランドマークが観測され、ランドマークが作成されるとき、3D特徴点およびランドマークを視覚的に特定するその対応する2D特徴点の組が記憶される。1つの例では、2D特徴点は、SIFT特徴点に相当する。SIFTの概念は、文献に広範囲に説明された。David G. Lowe、「Local Feature View Clustering for 3D Object Recognition」、Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition、Kauai、Hawaii(2001年12月)参照。SURF、BRIEFなどの、SIFTの変形形態および代替形態が容易に認識されよう。

比較的短い距離では、オドメトリ読取値から得られる測定値などのデッドレコニング測定値は、極めて正確である可能性がある。しかし、校正誤差、ホイール滑りなどにより、これらのデッドレコニング測定値は、距離および/または時間と共にドリフトまたは誤差を累積させる可能性があり、その結果、比較的長い時間の後に計算された位置は、デッドレコニング測定値の誤差が比較的小さいときでも、最初に計算された位置から大幅に変化している可能性がある。例えば、拡張された時間では、ロボットは、環境全体にわたって比較的多くの走行を行い、それにより、ドリフト誤差を累積させる可能性がある。

有利なことに、VSLAM技法は、デッドレコニング測定値のドリフトを全体的または部分的に補償することができ、その結果、ロボットが比較的長い距離にわたって走行した後でも、ロボットのグローバル位置を比較的高い精度で保持することができる。1つの実施形態では、VSLAM技法は、ロボットが比較的長い距離を走行した後でも、視覚測定値の精度を上回る、グローバルロボット姿勢推定値の精度を保持する。図示した例では、使用される視覚センサは、640x480の解像度を有する比較的単純で安価なカラーカメラであり、視覚測定値の精度は、約10センチメートル(cm)に維持された。グレースケールカメラおよび赤外線カメラなどの他の視覚センサを使用することもできることが理解されよう。いくつかの実施形態では、Point Grey Research Inc.、Vancouver、British Columbia、Canadaから入手できるdigiclops(商標)カメラなどの3眼カメラを使用することができる。

有利なことに、双眼または3眼のカメラシステムなどの、立体像を提供するカメラシステムは、ランドマークの3D特徴点を特定し、比較的迅速かつ効率的に3D特徴点に対する変位を推定するために使用することができる。不利なことに、そうしたカメラは、比較的少ない量が生産され、追加の構成要素により、またスケールメリットが比較的少ないことにより、単一の視覚センサカメラと比べて比較的高価である可能性がある。

［視覚測定値の例示］
図2Aおよび2Bは、ロボット502および対応するロボット基準座標系520を示す(縮尺は正確でない)。図示した実施形態では、ロボット基準座標系520は、VSLAMシステムの視覚的位置推定部分により使用される。ロボット基準座標系520におけるゼロベクトルは、ロボット502と共に動く。したがって、ロボット基準座標系520は、グローバルに固定されたゼロベクトルを有するグローバル基準座標系とは対照的に相対基準座標系である。例えば、ロボット基準座標系520におけるゼロベクトルは、ほぼロボット502のカメラに位置する可能性があり、図2Aでは姿勢「A」522により示され、図2Bでは姿勢「B」524により示される。

ロボット502がその環境を走行するとき、ロボット502は、新しい物理的ランドマークを検出し、以前検出したか、または「古い」物理的ランドマークを再訪する。図2Aは、新しいランドマークを「作成」または認識すること、すなわち新たに観測されたランドマークに関するエントリをデータベース内に作成することに関してロボット基準座標系520を示す。新しいランドマークを認識するための視覚的フロントエンドにおける処理または視覚的位置推定処理は、後に図4と関連づけてより詳細に説明する。図2Bは、以前観測され記録されたランドマークを再訪することに関してロボット基準座標系520を示す。ロボット基準座標系520は、ロボット502と共に動き、その結果、ランドマークを作成したときのグローバル基準座標系に対するロボット姿勢に相当する姿勢「A」522と、ランドマークを再訪したときのグローバル基準座標系に対するロボット姿勢に相当する姿勢「B」524とは、図2Bに示すように異なる可能性がある。

ここで図2Aに戻れば、図示した実施形態では、物理的ランドマーク504は、その3D特徴点により特定される。1つの実施形態では、3D特徴点は、2次元(2D)特徴点を三角形に分割し、3焦点テンソル法を使用して構造およびモーション問題を解くことにより抽出される。1つの実施形態では、2D特徴点は、SIFT特徴点である。SIFT特徴点の説明は、上述のLoweに見出すことができる。3焦点テンソル法の説明に関してはOlivier FaugerasおよびQuang-Tuan Luong、「The Geometry of Multiple Images」、MIT Press(2001年)を参照されたい。物理的ランドマーク504は、比較的多くの3D特徴点により特徴づけることができ、物理的ランドマーク504は、1つまたは複数の物理的対象物または物理的対象物の一部分に相当する可能性があることが理解されよう。明確にするために、図2Aに示す物理的ランドマーク504を、3つの3D特徴点、すなわち第1の特徴点506、第2の特徴点508、および第3の特徴点510と共に描く。ロボット502が新しい物理的ランドマークを観測するとき、視覚的フロントエンドは、それぞれの特徴点に対するロボット502の変位または位置を特定する。ランドマークが作成されるとき、ロボット502は、ランドマーク基準座標系の初期推定値としてロボット基準座標系520の現在位置を使用して視覚的特徴点に対する変位を参照することができる。例えば、図2Aに示す例では、矢印r₁、r₂、およびr₃は、ロボット502と、第1の特徴点506、第2の特徴点508、および第3の特徴点510との間のx、y、およびz寸法の変位などの3次元変位をそれぞれ表す。これらのx、y、およびz変位は、ロボット502のロボット基準座標系との比較であり、グローバル基準座標系との比較ではないことに留意されたい。1つの実施形態では、x、y、およびz変位は、前後寸法、左右寸法、および上下寸法の相対変位にそれぞれ相当する。それに加えて、3D特徴点に関する2D画像の座標または位置も記憶される。例えば、視覚センサが640x480のカラーカメラに相当する場合、2D画像の座標は、3D特徴点に対応する1つまたは複数のピクセル位置に相当する。3D特徴点は、通常、空間内の単なる単一点よりも広い場所を占めることが理解されよう。

1つの実施形態では、画像が変位r₁、r₂、およびr₃の計算のために斜視図用に画像が撮影されるときにロボット502が移動する場合、特徴点に対するロボット502の変位は、3つの画像の組の第1の画像が基準となる。しかし、基準として、任意の識別可能な基準座標系を使用することができることが理解されよう。例えば、基準として使用される画像が一貫して選択される限り、画像の組の他の画像も基準として使用することができる。特定の任意の画像に対応しない識別可能な基準座標系も使用することができることにも留意されたい。例えば、図示した実施形態では、3つの画像の組の第1の画像に対応するロボット姿勢は、特定のランドマークに関する局所基準座標系、すなわちランドマーク基準座標系として使用される。

1つの実施形態では、新しい物理的ランドマークに遭遇するとき、視覚的フロントエンド602は、(i) 図3のランドマークデータベース606などのデータベース内のランドマークに関する局所基準座標系における3D特徴点の3D座標と、(ii) 3D特徴点に対応する、第1の画像の2D特徴点などの選択された画像の2D特徴点とを記憶する。1つの実施形態では、新しい物理的ランドマークに遭遇し、その物理的ランドマークが視覚的フロントエンド602により処理されるとき、それに応じて、SLAMモジュール604は、ランドマークが作成されるときに対応するパーティクルに関するロボットの最後の姿勢から各パーティクルに関するデッドレコニングデータにより提供される姿勢の変化より計算される、グローバルロボット姿勢などのランドマーク姿勢の初期推定値を記憶することによりランドマークを「作成」する。

図2Bは、「新しい像」と呼ばれる、初期に観測された物理的ランドマーク504を再訪するロボット502の一例を示す。図2Bでは、ロボット502は、「ランドマーク基準座標系」すなわち新しい姿勢「B」に対応する元の姿勢「A」から変位する。それに応じて、ロボット基準座標系もロボット502と共に動く。ロボット502は、第1の特徴点506、第2の特徴点508、および第3の特徴点510を再び観測する。ロボット502が動き回るとき、物理的ランドマークの特徴点のいくつかは、すべての位置では観測できない可能性があることが理解されよう。ロボット502の視覚的フロントエンド602は、図2Cおよび2Dに示すように、相対姿勢、すなわち新しい姿勢「B」と姿勢「A」との間の差を計算し、SLAMモジュール604またはプレフィルタモジュール622への入力として、1つまたは複数の識別ランドマークに1つまたは複数の相対姿勢を提供する。1つの実施形態では、視覚的フロントエンド602は、第1の特徴点506、第2の特徴点508、および第3の特徴点510の3D特徴点から2D座標上への射影誤差を最小化する、ロボットの相対姿勢を見出すことにより、「A」と示される記憶されたランドマーク基準座標系に対するロボットの相対姿勢を計算する。

ロボット502と特徴点506、508、510との間の点線は、図2Bの上面図の線530で示される画像平面上への特徴点506、508、510の射影を表す。カメラなどの視覚センサの画像平面は、通常、カメラの焦点軸にほぼ垂直であることが理解されよう。線530は、点の射影に関するカメラの視野をほぼ表し、カメラからの特定のどんな距離も示さないことも理解されよう。

新しい像の2D特徴点とランドマークの3D特徴点との間の対応を与えれば、視覚的フロントエンド602は、例えば射影誤差を最小化することにより、相対姿勢を推定することができる。相対姿勢は、(i) ランドマークが作成されデータベース内に記憶される際の姿勢から、(ii) 物理的ランドマークが再観測される際の姿勢への姿勢の変化を示す。そうした相対姿勢を様々な座標形式で表すことができることが理解されよう。例えば、床に沿った相対姿勢の並進運動成分は、直交座標(x,y)により表すことができる。しかし、極座標(ρ,φ)を使用することもできることも理解されよう。図2Cおよび図2Dは、Δx、Δy、およびΔθの「カメラ姿勢」成分としても知られている相対姿勢を図で示す。用語「カメラ姿勢」は単語「カメラ」を含むが、カメラ以外の視覚センサを使用することもできることが理解されよう。相対姿勢は、平坦でない床面、これらの次元におけるロボットおよび/またはカメラモーション、誤認されたランドマーク、環境における物理的ランドマークの変化などの結果である可能性がある、垂直次元、ロール、およびピッチの変化を含む可能性もある。1つの実施形態では、これらの追加次元は、有利なことに、識別ランドマークの妥当性をテストするために使用される。1つの実施形態では、直交座標相対姿勢は、ランドマークに再遭遇する際に視覚的フロントエンドとSLAMモジュールとの間で使用され、極座標「デルタ姿勢」は、デッドレコニングデータにより評価される点間の姿勢の変化を計算する際にSLAMモジュール内で使用される。

1つの実施形態では、ロボットがその環境内を走行するときのデッドレコニングセンサデータに応じたロボット姿勢は、行列内の対応するタイムスタンプと共に記憶される。2つの時点に応じた姿勢の検索により、2つの時点の間で、方位、走行方向、および走行距離の変化を計算することができる。

［VSLAMのシステムアーキテクチャの例］
図3は、VSLAMシステム600のシステムアーキテクチャの1つの実施形態を示す。VSLAMシステム600は、専用ハードウェア、マイクロプロセッサにより実行されるソフトウェア、または専用ハードウェアとソフトウェアとの両方の組合せなどの様々な方法で実装することができることが理解されよう。

VSLAMシステム600への入力値は、1つまたは複数のデッドレコニングセンサ614からの生姿勢データ610を含み、1つもしくは複数のカメラまたは他の視覚センサ616からの視覚データ612も含む。光学ホイールエンコーダなどのデッドレコニングセンサ614は、ドライバまたはハードウェアアブストラクション層などのデッドレコニングインターフェース618を介してVSLAMシステム600と通信することができることが理解されよう。生姿勢データ610は、走行距離、速度、加速度などに相当する可能性があり、使用されるデッドレコニングセンサのタイプに依存する可能性がある。VSLAMシステム600からの出力値は、1つまたは複数の姿勢および地図620を含むことができる。

生姿勢データ610および視覚データ612は、視覚的フロントエンド602への入力値として提供される。視覚的フロントエンド602は、例えば、ランドマークを特定し、ランドマークの3D特徴点を特定し、デルタ姿勢を計算するなど、様々な機能を実行することができる。視覚的フロントエンド602が行うことができる処理の例は、後に図4と関連づけてより詳細に説明する。

視覚的フロントエンド602は、視覚データ612の画像間に走行した近似距離を特定するために生姿勢データ610を使用することができ、次いで、生姿勢データ610は、特徴点に対する変位を評価するために計算に使用される。新しい物理的ランドマークが認識されるとき、対応する記録またはエントリをランドマークデータベース606に追加することができる。新しく認識されたランドマークは、SLAMモジュール604に示すこともできる。例えば、「新しいランドマーク」フラッグを有効にし、「新しいランドマーク」識別子すなわちタグをSLAMモジュールに提供することができ、その結果、SLAMデータベース608およびランドマークデータベース606の適当な記録を整合させることができる。以前認識したランドマークに遭遇するとき、視覚的フロントエンド602は、SLAMモジュール604またはオプションのプレフィルタモジュール622に1つまたは複数の識別子を提供し、1つまたは複数の遭遇ランドマーク、相対姿勢情報(Δx、Δy、およびΔθ)などの相対姿勢情報、および利用可能であればデータ信頼性指標を示すことができる。

オプションのプレフィルタモジュール622は、視覚的フロントエンド602により提供されるデータ信頼性指標を分析する。データ信頼性指標は、視覚的フロントエンド602による物理的ランドマークの特定の信頼性の表示として使用することができる。例えば、プレフィルタモジュール622は、有利なことに、不正確に特定される可能性があり、地図内の他のランドマークに対する外れ値に対応する可能性がある、視覚的フロントエンド602により特定されたランドマーク測定値を特定するために使用することができる。1つの実施形態では、プレフィルタモジュール622は、不正確な可能性がある視覚測定値を特定するとき、識別された視覚的ランドマークデータをSLAMモジュール604まで通過させず、その結果、VSLAMシステム600は、不正確な可能性があるランドマーク測定値を効果的に無視する。SLAMモジュール604へのデータのプレフィルタリングは、有利なことに、SLAMモジュール604により推定される、1つまたは複数の姿勢(位置および方位)および地図620のロバスト性および精度を向上させることができる。

SLAMモジュール604は、1つまたは複数の姿勢および地図620を保持する。1つの実施形態では、SLAMモジュール604は、複数のパーティクルまたは仮定を保持し、各パーティクルは、1つの姿勢および1つの地図と関連がある。

SLAMモジュール604は、デッドレコニングインターフェース618から生姿勢データ610を受け取る。生姿勢データ610の性質は、デッドレコニングセンサ614のタイプおよびデッドレコニングインターフェース618により指定された出力値のタイプに応じて変化する可能性があることが理解されよう。生姿勢データ610の例には、距離測定値、速度測定値、および加速度測定値が含まれる可能性がある。デッドレコニングデータは、SLAMモジュール604により使用され、前の姿勢から走行したコースおよび距離を推定する。SLAMモジュール604が複数の仮定を使用する場合、デッドレコニングデータは、比較的多くの前の姿勢から走行したコースおよび距離を推定するために使用されることが理解されよう。

SLAMモジュール604に対する他の入力値には、視覚的フロントエンド602および/またはオプションのプレフィルタモジュール622からの視覚的位置推定データが含まれる。VSLAMを有するロボットが、ある環境を走行するとき、ロボットは、視覚的ランドマークを観測する。新しい視覚的ランドマークに遭遇するとき、SLAMモジュール604は、SLAMデータベース608内のパーティクルに関するロボットのグローバル基準座標系位置を記憶することができる。例えば、ロボット姿勢は、前の位置、ならびに既知の最後の姿勢から走行したコースおよび距離より推定することができる。

以前作成したランドマークを観測するとき、SLAMモジュール604は、視覚的フロントエンド602またはオプションのプレフィルタモジュール622から、観測されたランドマークに対するΔx、Δy、およびΔθなどの相対姿勢情報の新しい推定値を提供される。SLAMモジュール604は、姿勢情報の変化を使用し、保持された1つまたは複数の姿勢および地図620を更新する。したがって、視覚的に観測されたランドマークは、有利なことに、デッドレコニング測定値のドリフトを補償することができる。

図示した構成では、ランドマークは、ランドマークタグすなわち識別子I、ランドマーク姿勢推定値S、および例えば共分散行列(covariance matrix) Cなどの不確かさの指標と関連がある。3D特徴点などのランドマークの視覚的特徴または画像を記述する情報は、ランドマークデータベース606などの、視覚的フロントエンド602と結合するデータの集合内に記憶することができる。SLAMデータベース608などの、SLAMモジュール604に関するデータの集合では、ランドマークタグIを特定するために、相互参照値またはデータベース記録識別子を使用することができる。

ランドマーク姿勢Sは、ロボットがランドマークを「作成」し、ランドマークを地図に追加するときのロボット自体の姿勢に相当することに留意されたい。1つの実施形態では、ロボットがランドマークを再観測するとき、ランドマーク姿勢Sを更新することもできる。図示した構成では、ランドマーク姿勢Sは、グローバル基準に対するx寸法x、グローバル基準に対するy寸法y、およびグローバル基準座標系に対するロボット方位θの内容を含む3x1列ベクトルに対応する。前に述べたように、仮定上の姿勢および対応する地図は、有利なことに、複数パーティクルまたは複数仮定SLAMシステムのパーティクルの間で変化する可能性がある。

共分散行列Cは、ランドマーク姿勢Sの不確かさを表す。記号

は、パーティクルmに対するランドマークkに関する共分散行列を示すために本明細書に使用する。1つの実施形態では、共分散行列

は、カルマンフィルタにより更新される。

データベースは、アドレス指定可能な記憶媒体上に実装することができ、様々な異なるタイプのアドレス指定可能な記憶媒体を使用して実装することができることが当業者には理解されよう。例えば、ランドマークデータベース606および/またはSLAMデータベース608は、単一の装置内に完全に含むことができるか、またはネットワーク内のいくつかの装置、コンピュータ、またはサーバ全体にわたって広がる可能性がある。ランドマークデータベース606および/またはSLAMデータベース608は、メモリチップ、ハードディスク、光ドライブなどの装置内に実装することができる。図示したデータ構成は、リレーショナルデータベースの形式を有するが、データベースは、例えば、オブジェクト指向データベース、階層データベース、ライトウェイトディレクトリアクセスプロトコル(LDAP)ディレクトリ、オブジェクト指向リレーショナルデータベースなどとすることもできることを当業者は理解するであろう。データベースは、任意のデータベース標準に従うことができるか、または標準的でない民間の仕様に従うこともできる。データベースは、例えば、Oracle Corporationから入手可能なOracle(登録商標)、Microsoft CorporationのSQL ServerおよびAccess、Sybase,IncorporatedのSybase(登録商標)などの任意の数の市販のデータベース製品を使用して実装することもできる。

図示したデータ構成は、リレーショナルデータベース管理システム(RDBMS)を使用する。RDBMSでは、データは、テーブルの形式で記憶される。概念的には、テーブル内のデータは、列および行に配置されるフィールド内に記憶される。各フィールドは、1つの項目の情報を含む。テーブル内の各列は、その列名により特定され、SIFT特徴点の値などの1つのタイプの情報を含む。

［データベースの管理］
パーティクルに関する様々な地図を保持するデータベースのサイズは、ランドマークが地図内に累積されるにつれて、時間と共に増大する可能性があることが当業者には理解されよう。本発明の1つの実施形態は、データベースを管理するための技法も含む。

ランドマークデータベース606および/またはSLAMデータベース608は、多様な設定のVSLAMプロセスの効率的な動作を提供し、VSLAMプロセスに使用されるメモリの量を管理するために管理される可能性がある。データベースを効率的に管理する1つの方法は、環境内にはもはや存在しないと考えられているか、または他に、信頼できない、不良、または他の任意の不要な方法とみなされる可能性がある、データベースからのランドマークを除去することである。

例えば、物理的ランドマークが環境から消え、その結果、対応するランドマークがデータベースから除去されるという評価は、観測されることが予想される姿勢または姿勢の近傍において物理的ランドマークを繰り返し観測しないことに基づいている可能性がある。

別の例では、外れ値に繰り返し対応する測定値、すなわち比較的極端な測定値は、信頼できないものとみなされる可能性もあり、データベースから除去することができる。例えば、長時間にわたるランドマークの測定値が、繰り返し矛盾するか、または信頼できないと別途示されたとき、ランドマークは、信頼できないか、または不良であるとみなされる可能性がある。繰り返し矛盾する測定値の範囲の一例は、約5〜10個の矛盾する測定値である。他の適当な値は、当業者により容易に決定される。1つの実施形態では、ロボットが約90%などの比較的大きい割合のパーティクルとなることをSLAMサブシステムが予測する場所から比較的遠くに離れてロボットが位置することを測定値が示唆するとき、ランドマークの測定値は矛盾する。1つの実施形態では、新しい視覚測定値を推定値に組み込む前のSLAM予測値が95%信頼度楕円の外部にあるとき、ロボットは、比較的遠くに離れて位置することが特定される。1つの実施形態では、95%信頼度楕円は、(i)その平均値としてのロボット姿勢の視覚的測定推定値と、(ii)その共分散行列としてのC_sensorとを有する。別の実施形態では、SLAMにより推定される姿勢と視覚測定により推定される姿勢との間の差が、所定の閾値を超えるとき、ロボットは、比較的遠くに離れて位置することが特定される可能性がある。屋内環境の所定の閾値に関する適当な値の一例は、約2メートルである。他の値は、当業者により容易に決定される。ランドマークの「繰り返し矛盾する」測定値は、ランドマークが信頼できないことを示す可能性があるが、時々矛盾する測定値は、ランドマークが信頼できないことを示すか、または示さない可能性があり、むしろ、そうした時々矛盾する測定値は、ロボットをある場所から別の場所まで持ち上げたり、動かしたりすることによるなど、ロボットの別の物体との衝突、ロボットの「誘拐」などの結果である可能性があることに留意されたい。1つの実施形態では、そうした時々矛盾する測定値は、データベースからのランドマークの削除をもたらさない。

別の例では、ランドマークは、例えば、屋内環境の1平方メートル当り約5〜10個のランドマークなど、地図のいくつかの部分におけるランドマークの密度が比較的高くなることが特定されるとき、望ましくないとみなされる可能性がある。ランドマークの密度は、環境によって大幅に変化する可能性があり、それに応じて、「高い」密度に対する適当な閾値は、同様に変化し、当業者により容易に決定されることが理解されよう。地図の極めて密度の高い部分のランドマークのいくつかを選択的に除去することにより、メモリを他のタスクのために空けることができる。

1つの実施形態では、ランドマークデータベースが比較的大きいサイズまで増大するとき、メモリ管理技法を使用することができる。通常、ハードディスクなどの大容量記憶装置は、ランダムアクセスメモリ(RAM)などの固体メモリ装置と比べて比較的低速である。逆に言えば、大容量記憶装置は、通常、固体メモリ装置よりも大幅に大きい記憶容量を有する。あるいは、例えばフラッシュメモリまたはEEPROM装置などの固体メモリ装置は、不揮発性の形式でランドマークデータベースを記憶するために使用することができる。メモリ使用量は、RAMなどの比較的高速のメモリ内にランドマークデータベース全体の比較的小さい部分のみを一度に保持することにより効率的に管理することができる。例えば、少数の初期ランドマーク測定値およびランドマークデータベースとの比較値は、通常、地図構築される環境でロボットが動作している可能性がある場所をほぼ示すことができる。例えば、家、オフィス、または病院の床全体を環境として地図構築することができ、数回の初期測定の後、VSLAMシステム600は、家の特定の部屋、オフィスの1階、病院の特定の棟などにロボットが居ることを特定することができる。

メモリ資源の消費を低減するために、ロボットの近似位置の特定に少なくとも部分的に応答して、次いで、VSLAMシステム600は、データベースの関連部分を含むRAM内のデータベースの比較的小さい部分集合を保持することができ、以前使用した他のメモリ資源は、システムに再び解放することができる。例えば、データベースのロードされた部分集合とうまく整合することなく、比較的長い時間が過ぎた場合、ロボットが環境の別の部分まで動かされたか、または動いたかを特定するために、地図全体を再び一時的にロードすることができる。例えば、ロボットは、自律的に走行したか、または持ち上げられ、新しい位置まで動かされた可能性がある。

1つの実施形態では、RAMなどの比較的高速のメモリ内に保持される地図の部分集合は、地図の複数のランドマークのランダムに選択された部分集合に少なくとも一時的に対応することができる。別の実施形態では、比較的高速のメモリ内に保持される地図の部分集合は、部分集合内に残るランドマークの密度が地図を通して比較的均一に分布するように選択される部分集合に少なくとも一時的に対応することができる。これらの技法は、有利なことに、例えば、比較的限られたメモリ資源および/または計算資源を有する移動ロボットが1つまたは複数の地図内でそれ自体の位置推定を行うのを助けるために使用することができる。

1つの実施形態では、VSLAMシステムは、有利なことに、新しいランドマークをデータベースに追加することを中断する。1つの例では、VSLAMシステムは、ランドマーク密度が1平方メートル当り5〜10個のランドマークなどの所定の閾値を超えたという判定に少なくとも部分的に応答して、環境の一部分または環境全体においてランドマーク作成を中断する。例えば、データベースが環境の一部分で比較的高いランドマーク密度を示し、環境の別の部分で比較的低いランドマーク密度を示すとき、新しいランドマークのデータベースへの追加は、データベース内の比較的高いランドマーク密度に対応する環境の一部分では無効になる可能性があり、新しいランドマークの追加は、比較的低いランドマーク密度に対応する環境の一部分では有効になる可能性がある。

1つの実施形態では、VSLAMシステムは、ランドマーク作成減衰速度、すなわち一定の時間にわたり新しいランドマークがだんだん特定されなくなるという判定に少なくとも部分的に応答して、新しいランドマークをデータベースに追加することを中断する。ランドマーク作成減衰速度は、環境の一部分または環境全体に適用することができる。例えば、比較的一定の照明状態のもとでの比較的静的な環境では、ランドマークが作成される速度は、通常、多くのランドマークが作成される前では、最初に最も高い。ランドマークの作成、すなわちランドマークのデータベースへの追加により部分的に領域の地図が構築された後、視覚的フロントエンドは、あまり頻繁にランドマークを作成しようとしなくなる。1つの実施形態では、作成速度は、1メートルの走行当りに作成されるランドマークの数に相当する。環境の所与の部分における作成速度が閾値(例えば、(i)10メートル毎に1つのランドマークなどの所定の値、または(ii)環境の関連部分を通る第1の経路の間に得られる速度(単位走行距離当り)の5%などの初期作成速度のパーセンテージに対応する可能性がある)未満まで低下するとき、ランドマーク作成は、環境のその部分で一時的に中断される可能性がある。

別の実施形態では、VSLAMシステムは、視覚測定値と作成ランドマークとの比率に少なくとも部分的に応答して環境全体または環境の一部分で、新しいランドマークをデータベースに追加することを中断する。別の実施形態では、ランドマークの追加を中断することは、地図構築の30分後などの経過時間、またはメモリの75%がランドマークデータベースのために使用されてきたときなどのメモリ使用量に少なくとも部分的に応答して誘発される可能性もある。

［SLAMグラフ構造］
図4は、本実施形態のいくつかが、ランドマークおよび本明細書ではSLAMグラフと呼ぶグラフ構造におけるロボットの他の姿勢を表す方法の例を提供する。本明細書に用語を使用するとき、姿勢ノード（pose node）は、「通常の」姿勢ノード1802または「ランドマーク」姿勢ノード1804のいずれかに分類することができる。ランドマーク姿勢ノード1804は、ある姿勢ノードをそのノードに関連するランドマークに分類する。対照的に、「通常の」姿勢ノード1802は、ロボットの姿勢のみを示す。図4に示すように、複数の姿勢ノード1802、1804は、複数のエッジ1806により接続される。過去の観測値をより最適に表すために実時間でSLAMグラフを調整することは、より効率的なナビゲーションを可能にする。

SLAMグラフ内の各姿勢ノード1802は、「通常」または「ランドマーク」の姿勢ノードのいずれにせよ、特定の時間ステップの演算においてロボットの姿勢推定値をエンコードする。各有向エッジ1806は、ソースグラフノードとデスティネーショングラフノードとの間の座標系の変換の推定値をエンコードする。SLAMグラフは、演算中、すなわち実時間で徐々に構築および改善することができる。

図5は、いくつかの実施形態におけるSLAMグラフ内のノードおよびエッジに関する情報をより詳細に示す。各エッジ1810は、固有識別子1820、そのソースノードの識別子1822、およびそのデスティネーションノードの識別子1824を含む。本発明の1つの実施形態では、ソースからデスティネーションへの変換は、座標変換パラメータに関する変換の平均値1826および共分散1828により表されるガウス確率ベクトルとしてエンコードされる。他の実施形態では、ガウス関数と異なる分布を使用することができ、この分布は、サンプルパーティクルにより、またはパラメトリック形式で表すことができる。他の実施形態では、決定論的公式を使用することができ、エッジデータ構造内に平均値のみが記憶される。姿勢ノードまたはランドマークノードの内容を1808により詳細に示す。各ノードは、固有識別子1812、ロボット姿勢推定値1816、およびSLAMグラフ内のノードから生じるエッジを列挙するエッジ識別子のリスト1818を有する。ノードがランドマークノードであれば、そのノードは、そのノードに関連するランドマークの固有識別子1814をエンコードすることもできる。エッジ変換と同様に、姿勢推定値は、確率変数または決定論的なパラメータとして表すこともできる。確率変数の場合には、分布を表すために、パラメトリックサンプルまたは数的サンプルのいずれかを使用することができる。1つの実施形態では、ガウス分布が使用され、パラメータは、平均ベクトルおよび共分散行列である。固有識別子1812、1820のそれぞれを介してノードおよびエッジにアクセスすることができる。図5に示す構造は、例として提供しているにすぎず、複数の代わりの表示法が容易に認識されよう。

［ランドマーク作成(新しいランドマーク)のための視覚的フロントエンド処理］
図6は、環境内の新しい物理的ランドマークを認識し、データベースの1つまたは複数の地図内に対応するランドマークを作成するとき、視覚的フロントエンドに使用することができるプロセスを全体的に示すフローチャートである。新しいランドマークを作成する演算は、ランドマークの「生成」と呼ぶこともできる。図示したプロセスは、発明の技術的思想および範囲から逸脱することなく様々な方法で変更することができることが当業者には理解されよう。例えば、別の実施形態では、図示したプロセスの様々な部分は、組み合わされ、交互に再配置され、除去されるなどする可能性がある。それに加えて、プロセスは、汎用コンピュータ内で実行するソフトウェア、マイクロプロセッサにより実行されるファームウェア、専用ハードウェアなどにより、様々な方法で実行することができることに留意されたい。

プロセスは、分析のために少なくとも2つの画像のグループを取り込む状態1002で始まる。例えば、双眼もしくは3眼のカメラなどの複数の画像を有する視覚センサ、または単一のカメラなどの単一の撮像素子を有する視覚センサにより、画像を提供することができる。単一のカメラからの画像が使用されるとき、プロセスは、適当に離間した画像を選択することができる。図示した実施形態では、ロボットは、単一の前方監視カメラを装備され、関連の画像を撮るために前方に走行する。視覚センサ用の他の構成も可能である。他の例では、視覚センサは、全体的に上方を指すカメラ、横方向監視カメラ、または前方監視、上方監視、および/もしくは横方向監視の間の位置に対応する可能性がある。ここで、単一の前方監視カメラを有する図示した実施形態に戻れば、1つの例では、3つの画像は、少なくとも約10センチメートル(cm)離れた分離距離で選択される。分離距離のための適当な距離は、環境に応じて広い範囲で変化する可能性があることが理解されよう。例えば、動作環境が、屋外環境などの比較的広範囲の環境に相当する場合、画像間の分離のための適当な距離は、特徴点に関する視点を得るために、より高くなる可能性がある。1つの実施形態では、分離距離は、ロボットの動きを妨害する可能性がある障害物の近接度の推定値に応じて適切に変化する可能性がある。1つの実施形態では、ロボットが単一の前方監視カメラを装備している場合、ロボットは、画像を撮りながら、前方にほぼ直線状に移動する。ロボットが画像を撮る間、多少の回転を許容することができるが、回転は、ランドマークの特徴点がもはやカメラの視野に入らなくなるほど極端であるべきでない。プロセスは、状態1002からオプションの状態1004に進む。

状態1004および判断ブロック1006は、ロボットの構成に応じて任意選択とすることができる。ロボットが、3眼カメラなどの複数の撮像素子を有する視覚センサを装備する場合、状態1004および判断ブロック1006をスキップすることができ、視覚センサ間の間隔をメモリ内の記憶パラメータから検索することができる。スキップされたとき、プロセスは、状態1002から状態1010に進む。

単一のカメラが視覚センサとして使用され、ロボットが様々な視点から様々な画像を撮るために移動するとき、プロセスは、状態1004において画像間の実際の距離を取り込み、判断ブロック1006においてモーションをチェックする。1つの実施形態では、これらの距離は、画像が撮られた時間に対応するデッドレコニングデータを監視することにより決定される。プロセスは、状態1004から判断ブロック1006に進む。

判断ブロック1006では、「ベースライン」と呼ばれる、画像間に走行した距離をテストする。例えば、画像間のベースラインの量は、所定の値と比較される可能性がある。所定の値は、極めて広い範囲で変化する可能性があることが理解されよう。家またはアパートの内部などの屋内環境では、所定の値に関する適当な値は、約10センチメートルとなる可能性がある。当然、適当な値は、環境に依存する可能性があり、他の適当な値は、当業者には容易に決定されよう。グループ内の画像の1つまたは複数の間でロボットの移動が十分でないとき、プロセスは、状態1008に進み、ランドマークを作成しない。そうでない場合、プロセスは、判断ブロックから状態1010に進む。

状態1010では、プロセスは、グループ内の画像に共通の2D特徴点を特定するために選択画像を分析する。観測可能な特徴点の数は、環境に応じて変化する。適当な特徴点の抽出は、文献に広範囲に記載された。SIFT特徴点は、そうした2D特徴点の1つの例である。例えば、David G. Lowe、「Local Feature View Clustering for 3D Object Recognition」、Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition、Kauai、Hawaii(2001年12月)を参照されたい。特徴点の計算に使用される他の情報には、内在のカメラ校正パラメータおよび外在のカメラパラメータが含まれる可能性があることに留意されたい。内在のカメラ校正パラメータの例には、光学中心、歪みパラメータ、および焦点距離が含まれる。外在のカメラ校正パラメータの例には、カメラ基準座標系と局所基準座標系との間のカメラ剛体変換(rigid transformation)が含まれる。プロセスは、状態1010から判断ブロック1012に進む。

判断ブロック1012では、プロセスは、ランドマークの信頼できる特定のために、3つの画像などのグループ内の画像に共通の十分な特徴点が特定されたかどうかを判定する。例えば、プロセスは、特徴点がグループ内の画像に共通する所定の数の特徴点よりも少ないと判定するとき、今後のランドマークを確実に特定するために検出される十分な特徴点が存在しないと判定する可能性がある。この場合、プロセスは、判断ブロック1012から状態1008に進む可能性があり、ランドマークを「作成」しない。所定の数の特徴点に関する適当な値は、極めて広い範囲で変化する可能性があり、視覚的特徴点を特定するのに使用される方法に依存する可能性があるが理解されよう。1つの実施形態では、所定の数の特徴点は、ランドマーク作成のための判断ブロック1012において、画像を既に記憶されたランドマークと比較するのに使用される所定の値よりも大きい。

1つの実施形態では、SIFT特徴点が使用される場合、所定の数の特徴点に関するサンプル値の例は、約10である。他の適当な値は、当業者により容易に決定される。1つの実施形態では、VSLAMシステム600は、所定の値をユーザ設定可能にするように構成することができる。グループ内の画像に共通する十分な特徴点が特定されたとき、プロセスは、判断ブロック1012から状態1014に進む。

状態1014では、プロセスは、状態1010で特定された共通の特徴点に対する3D局所基準座標系位置または変位を計算する。1つの実施形態では、3D局所基準座標系位置は、ロボットの視覚センサに対する特徴点の近似3D位置(x,y,z)に相当する。ロボットが移動する際に単一の視覚センサから複数の画像が撮影される場合、3D局所基準座標系位置は、グループ内の第1の画像などのグループ内の画像の1つをロボットが撮影するときのロボットの位置と比較することができる。1つの例では、3D位置の計算は、3焦点テンソル法を使用して構造およびモーション問題を解くことにより解決される。特徴点は、点よりも大きい空間を占有することができ、その結果、対応する3D位置は、比較的近似的である可能性があることが理解されよう。プロセスは、状態1014から判断ブロック1016に進む。

判断ブロック1016では、ランドマークを確実に認識するのに十分な、状態1014で解かれた特徴点に関する3D局所基準座標系位置が存在したかどうかを判定する。場合により、プロセスは、特定の特徴点に関する3D局所基準座標系位置の解を見出すことができず、その結果、対応する変位情報を有する3D特徴点の数が、そうした初期検出特徴点の数と異なる可能性があることが理解されよう。例えば、判断ブロック1016では、プロセスは、ランドマークの特徴点に関して解かれた3D局所基準座標系位置の数を所定の数と比較することができる。1つの実施形態では、SIFT特徴点が使用される場合、プロセスは、10個以上のそうした特徴点が解かれたとき、比較的信頼できる認識を行うために解かれた3D局所基準座標系位置を有する十分な数の特徴点をランドマークが有すると判定する。他の適当な値は、当業者により容易に決定される。ランドマークが確実に認識されるように特定されたとき、プロセスは、判断ブロック1016から状態1018に進む。そうでない場合、プロセスは、判断ブロック1016から状態1008に進み、ランドマークを「作成」しない。

状態1018では、プロセスは、特徴点、3D位置、および任意選択的に基準として使用する画像の特徴点に対応する近似2D画像位置を識別可能に記憶する。例えば、特徴点の3D位置および2D画像位置は、特徴点テーブル内の記録物に記憶することができる。作成される各ランドマークは、番号付きの固有識別子などの固有の参照値を有するべきであり、さらに、参照値を有する各特徴点は、例えば、ランドマークIDなどのランドマークの参照値と、特徴点IDなどの特徴点の参照値との組合せにより識別可能とすべきであることが理解されよう。プロセスは、状態1018から状態1020に進む。

状態1020では、プロセスは、VSLAMのSLAM部分に関するプロセスなどの他のプロセスに対して新しいランドマークが作成されたという表示を提供する。例えば、この表示は、ソフトウェア呼出し、ハードウェアまたはソフトウェアの中断などにおけるパラメータまたはフラグとして提供することができる。この表示には、新しく作成されたランドマークのランドマーク識別子が伴う可能性もある。

［様々な実施形態の概要］
本実施形態は、SLAMベースの移動型プラットホームナビゲーションシステムのフロントエンドおよびバックエンドに対する様々な改善を開示する。「フロントエンド」は、ランドマークを生成するか、またはグラフ内のランドマークを認識することに関わる演算を指すが、「バックエンド」は、SLAMグラフの維持および保守に関わる演算を指すことが容易に理解される。「フロントエンド」演算と「バックエンド」演算との間の区別は、概念上の利便性のためにすぎず、いくつかの演算は、この人工的な区別を越えて起こる可能性があることが容易に理解される。フロントエンドに関して、本実施形態は、グローバルデータベースが特徴点の局所データベースと併せて探索される、新規の特徴点整合方法を開示する。これらの実施形態は、特に図8と関連させて説明する。それに加えて、さらにフロントエンドに関して、いくつかの実施形態は、ロバストな整合推定ランドマーク作成プロセスを開示する。これらの実施形態は、特に図10と関連させて説明する。バックエンドに関して、いくつかの実施形態は、SLAMベースのシステムのメモリ使用量および演算効率を改善するために、SLAMグラフの最適化および管理の改善のための方法を企図する。これらの実施形態は、特に図15〜18と関連させて説明する。フロントエンドおよびバックエンドに対するこれら特定の改善を特定したが、改善されたSLAMベースのナビゲーションにつながる、本明細書に開示する多くの追加の概念も見出されよう。

以下の定義は、上述の用語集が以下の用語に関する一般的な意味の特定の例を提供するとき、用語集を考慮して考えるべきである。本開示では、「移動型システム」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、ロボット、携帯型電子装置、航行体の付属品などの、移動することができる任意の電子システムを含む。そうしたシステムは、ロボットの例のように、それ自体の制御のもとで移動することができるが、他のシステムは、携帯型装置の例などのようにユーザが移動させることができる。同様に、本開示では、「軌道」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、物体が走行すべき任意の経路を含む。例えば、ロボットは、地域全体にわたる2つの中間地点の間の経路として軌道を使用することができる。同様に、本開示では、「姿勢ノード」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、移動型システムの姿勢情報を記憶するのに十分な任意のデータ構造を含む。「ランドマークノード」は、ランドマークに関する情報を含む、内部識別子または外部識別子のいずれかにより指定される姿勢ノードとして理解される。同様に、本開示では、「ランドマーク」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、システムが続いて特定することができる、地域内で見出される任意の物体、特徴点、または態様を含む。同様に、本開示では、「閾値」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、演算を行うべきかどうかを判定するための基準として使用することができる任意の数値または条件を含む。同様に、本開示では、「姿勢ノードの特定」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、複数の姿勢ノードからある姿勢ノードを選択するのに使用される任意の演算を含む。同様に、本開示では、「姿勢ノードの除去」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、グラフがもはや姿勢ノードを参照しないようにグラフを修正するか、またはグラフがもはや姿勢ノードを参照しないように1つまたは複数の演算を開始させる任意の演算を含む。同様に、本開示では、「姿勢ノードの挿入」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、グラフが姿勢ノードを参照するようにグラフを修正するか、または姿勢ノードを参照するグラフをもたらす1つまたは複数の演算を開始させる任意の演算を含む。

同様に、本開示では、「カメラ」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、画像を取得することができる任意の装置を含む。同様に、本開示では、「ナビゲーションシステム」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、本明細書に開示する演算のいくつかを実行するように構成された、任意のソフトウェア、ハードウェア、またはファームウェアモジュールを含む。例えば、SLAMグラフを保持し、図16〜18に示す演算の1つまたは複数を実行するためのソフトウェア命令を含む、ラップトップまたは任意の移動型コンピュータシステムは、ナビゲーションシステムを含むであろう。同様に、本開示では、「プランナ」は、広義の用語であり、当業者にはその通常および通例の意味が与えられるべきであり(それは、特別または専用の意味に限定されるべきでない)、限定されないが、移動型電子システムを第1の位置から第2の位置に移動させるために、サーボ、モータ、またはピストンなどの様々なアクチュエータを動作させるように構成された、任意のソフトウェア、ハードウェア、またはファームウェアモジュールを含む。

［SLAMフロントエンド処理］
視覚的フロントエンド処理の改善に関する実施形態に関して、本実施形態のいくつかは、特徴点整合、構造、および移動推定の新規機能を有するランドマーク作成モジュールを企図する。このモジュールは、図6に関連させて上述したランドマーク作成プロセスを実行するための新規の基準値を含む。本実施形態は、同様に、図6のフローチャートの一部分またはすべてに従うことができる。

本実施形態のランドマーク作成プロセスでは、ランドマーク作成プロセスは、(以上のいくつかの実施形態に説明した3つの異なる画像とは対照的に)入力値1002として2つの異なる状態像のみを必要とする可能性がある。フレーム間整合手順1010は、デッドレコニングセンサが利用可能なとき、デッドレコニングセンサからの推定値により改善される、特徴点外観もエピポーラ幾何拘束(epipolar geometry constraints)も使用することができる。構造推定1014は、エピポーラ幾何拘束および視覚的測定のみを適用する最適化を使用して実行され、関連の共分散行列を有する相対姿勢推定値を得ることができる。妥当性チェック1016は、一定の数の解かれた特徴点だけでなく、構造推定演算の強度条件および/または数値的条件に関連している可能性がある、移動方向の推定における統計的確かさの程度も必要とする可能性がある。1016におけるこのより強いチェックは、前の実施形態において1006におけるベースラインチェックを緩和させ、ランドマーク作成に関するより広い範囲の存続可能な画像の対を許容する。これらのプロセスのそれぞれを以下により詳細に説明する。

図7は、新しいランドマークを作成するか、またはいくつかの実施形態で実装された過去のランドマークを特定するための視覚的フロントエンドプロセスの全体の概要を示す。本プロセスは、カメラからの画像などの画像を取り込むことにより状態2701で始まる可能性がある。状態2702では、システムは、ランドマークを整合させようとすることができる。1つの実施形態では、これは、取込画像から抽出した1組の視覚的特徴点を、すべての生成ランドマークからのデータベースの視覚的特徴点と整合することにより実行される。判断ブロック2703では、システムは、少なくとも1つのランドマークが取込画像と整合したかどうかを判定することができる。少なくとも1つの整合が存在すれば、システムは終了する可能性がある(2706)。整合が全くなければ、システムは、代わりに、状態2704に進み、画像をランドマーク作成に関する候補画像のリストに追加することができる。次いで、システムは、状態2705に進み、累積した画像のリストからランドマーク作成をしようとすることができる。

［ランドマーク認識］
図8は、いくつかの実施形態において実施されるランドマーク認識プロセスを表す。特に、図8のフローチャートは、図7のフローチャートの状態2702、すなわちいくつかの実施形態において実施されるランドマーク整合プロセスに関して詳細に説明する。このプロセスは、演算中に各取得画像に関して実行することができる。これらの実施形態では、特徴点は、粗密戦略を使用して整合することができる。これは、見出される整合特徴点の数を最大化しようとしながら処理労力を低減する。

プロセスは、システムが候補ランドマークを見出すためにグローバルデータベースからの問合せ画像内の特徴点を検索する、状態2002で始まる。以前特定されたすべてのランドマークの特徴点を含むデータベースを含むグローバルデータベースが理解されよう。そうしたデータベースは、説明される演算を実行する移動型装置上に記憶されるか、またはサーバ上など、離間させて配置される可能性がある。したがって、グローバルデータベース内の特徴点を検索することは、問合せ画像の特徴点と視覚的に類似する、グローバルデータベース内の特徴点を見出すことを意味する。視覚的類似性は、いくつかのよく知られた方法により判定することができる。例えば、SIFTディスクリプタを上述のように使用することができる。その際、これらの特徴点は、状態2004において視覚的類似性によりランキングを構築するために使用することができる。このランキングから、状態2006において、有望なランドマーク候補を選択することができる。

その際、局所データベースを使用して各候補ランドマーク内の特徴点の組に対して、より十分な粒子整合(granular matching)が実行される。ここで、現在のランドマークの特徴点のうちから検索するために参照すべき「局所」データベースが理解されよう。その際、システムは、すべての候補にわたって反復し、2008〜2014において各候補との局所整合を実行し、観測値を得る可能性がある。局所整合は、現在の候補ランドマークの特徴点のみを含むデータベース内の問合せ画像の特徴点を検索するステップを含むことができる。特に、それぞれの反復において、システムは、状態2008で特徴点を検索し、状態2010でロバストな姿勢推定を行い、状態2012でバンドル調整を行い、状態2014で姿勢観測値および共分散を得ることができる。画像特徴点位置およびそれらの特徴点の3D構造を与えられた姿勢を推定する多くの方法が存在することが認識されよう。これらのいくつかは、図2Cおよび2Dに関して上述した。バンドル調整の説明は、Bill Triggs、P. McLauchlan、Richard Hartley、およびA. Fitzgibbon、「Bundle adjustment - a modern synthesis」、B. Triggs、A. Zisserman、およびR. Szeliski編集、Vision Algorithms: Theory and Practice、volume 1883 of Lecture Notes in Computer Science、298〜372頁、Springer- Verlag、2000年に見出すことができる。

共分散は、相対姿勢の推定値の精度であることがさらに認識されよう。

うまく認識されたランドマークは、ランドマーク座標系と問合せ画像との間の相対カメラ姿勢の平均値および共分散からなる状態2014において観測を通して判定される。図8に示すように、システムは、いくつかのルートにより、この状態に到達できない可能性がある。例えば、状態2008では、データベース内に類似した特徴点がなければ、特徴点の検索はできないであろう。状態2010では、ロバストな姿勢推定は、姿勢を見出すことができない可能性がある。同様に、状態2012では、バンドル調整は、再投影誤差が極めて大きいとき、失敗する可能性がある。

［ロバストなフレーム間整合を介したランドマーク作成］
図9は、図7の状態2705、すなわちランドマーク作成プロセスの1つの実施形態に関して詳細に説明するフローチャートである。一般的に言えば、この手順は、最初に、推定上(候補)の対応点を構築し、次いで、幾何拘束を使用して、これらの対応点を正(正しい)対応点および異常(不正な)対応点に分割する。対応点には、ある画像内の特徴点および他の画像内の対応する特徴点からなる1対の整合用特徴点が含まれる。同様に、幾何拘束には、カメラ投影モデルの点の構造に関する拘束が含まれる可能性がある。プロセスは、一時的に局所的な画像の対を選択することにより状態1602で始まる可能性がある(1600)。単一のカメラがあれば、この対は、連続的な、またはほぼ連続的な画像を含むことができる。立体映像システムがあれば、これは、立体画像の対を含むことができる。この画像の対内のフレームは、本明細書では、それぞれフレームAおよびBと呼ぶ。

次いで、システムは、判断ブロック1604に進み、推定値差(differential estimate)が利用可能かどうかを特定することができる。推定値差には、オドメトリまたはジャイロにより提供されるモーション推定値などの、ある瞬間から次の瞬間までのモーションのモーション推定値が含まれる。判断ブロック1604において、特徴点ディスクリプタのみを使用するか、または他のセンサにより供給される任意のデッドレコニングロボットモーション推定値を利用することにより、推定上の対応点を生成することができる。本明細書に使用する用語「推定上の対応点」は、候補対応点の初期原案リストを指す。特徴点ディスクリプタのみが使用される場合、フレームB内の各特徴点は、最初に、特徴点ディスクリプタ空間1608内の距離に応じてフレームA内の特徴点と対になる可能性がある。近似最近傍探索(ANN)法は、状態1608において対応する対を相互に特定するために使用することができる。特徴点のどちらの組も十分小さい濃度を有するとき、正確な最近傍探索を決定することができる。多い組では、正確な最近傍探索の計算コストが高い可能性があるとき、近似最近傍探索を使用することができる。しかし、少ない組では、これは問題にならない可能性がある。1組の推定上の対応点を考慮すれば、外れ値を除去するためにエピポーラ幾何拘束を反復的に適用することができる。カメラモーションの先行値が提供されないとき、この手順の開始点は、1610においてランダムサンプルコンセンサス(RANSAC)および5点アルゴリズムを使用して計算することができる。カメラの相対姿勢および点の構造を初期化するために使用することができる代わりのアルゴリズムが容易に認識されよう。5点アルゴリズムは、David Nister、「An efficient solution to the five-point relative pose problem」、IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI)、26(6):756〜777頁、2004年6月により詳細に説明されている。これは、(縮尺のない)カメラモーションの推定値および1組の正対応点をもたらす。先行するデッドレコニング推定値が利用可能であるとき、上述の近似カメラモーションは、反復のために十分な開始点とすることができる。

その代わりに、判断ブロック1604においてデッドレコニングの形式として2つの画像間のモーション推定値が取得されるとき、モーション推定値は、推定上の対応点の探索を拘束する可能性があり、状態1606においてカメラモーションの推定値を提供する。モーション推定値は、最初に、既知の外在のカメラパラメータを使用してロボット座標系からカメラ座標系に変換することができる。その際、フレームA内の各特徴点は、カメラモーションの推定値を使用してフレームB内のその推定エピポーラ線に投影することができる。プラスの深度(depth)と一致する位置を有する、エピポーラ線近傍の特徴点のみをフレームA内の特徴点に対する候補対応点とみなす必要がある可能性がある。このエピポーラ拘束も満たすディスクリプタ空間内の最近傍の特徴点を推定上の対応点として取得する。

この時点で、システムは、状態1612に達し、以下のように最良整合特徴点を反復的に探索する。

1. 1612:誤差閾値因子

は、所望の最終許容閾値rの倍数として選択される。

2. 1614:現在のモーション推定値により与えられるエピポーラ線の閾値距離内でプラスの深度を有する、すべての推定上の対応点は、正対応点(inlier)と表示される。縮尺率sを有する特徴点の閾値距離sは、

により与えられ、より大きい縮尺率の特徴点と関連する、より大きい位置の不確かさを調整する。sは、特徴点の縮尺特性である。SIFTに使用されるガウス検出器(Gaussian detector)の差などの、異なる縮尺率で特徴点を検出する特徴点検出器では、特徴点は、どれくらいの大きさの画像をカバーするかを説明する縮尺率を有する。一般に、より大きい縮尺率の特徴点は、より大きい位置の不確かさを有する可能性がある。

3. 1616:モーション推定値は、正対応点の現在の組全体にわたる非線形最大尤度推定値により精細化される。

4. 1618:閾値よりも少ない正対応点が見出されるとき、プロセスは、エラーの信号を発する。

5. 1620:

が目標閾値rに達したとき、反復は、正常に完了した。

6. 1622:閾値因子

は、乗法的に低下し、プロセスは、ステップ2から

となるまで反復する。

この方法は、計算コストを低減するために標準的なM-estimator法(反復加重最小二乗法)の代わりに使用することができる。計算の予算が許せば、その代わりに、Tukey加重関数と共にM-estimator法を使用することができる。そうした場合、共分散において非ゼロ重みを有するそれらの対応点を正対応点として取得することができる。

システムは、最後に、終了する前に、ランドマークを形成するための構成要素としての状態1624で正対応点およびカメラモーションを戻すか、または状態1626でエラーを戻す。

［構造推定］
図10は、図9のプロセス1616、すなわちランドマーク生成のために3次元構造およびカメラモーションを推定するプロセスに関して詳細に説明するフローチャートである。プロセスは、状態1702で開始し(1701)、そのとき、ロバストなフレーム間整合からの特徴点対応点およびカメラモーションの入力値が入力値として取り込まれる。状態1704において、各特徴点は、推定カメラモーションを使用する三角測量によりフレームA内のその線に沿って深度を割り当てられる。深度は、いくつかの方法でパラメータ化することができるが、深度パラメータ化逆行列は、線形性を保つのに有利である。

次いで、システムは、状態1706に進み、バンドル調整を行うことにより、構造およびモーションを反復的に最適化することができる。バンドル調整は、構造およびカメラモーションに関する結合推定値をもたらすために再投影目的関数全体にわたって実行することができる。縮尺率は、特徴点対応点に拘束されない可能性があるので、一定の縮尺率の変位にわたり最適化が行われる(カメラ並進運動は、拘束されて単位ベクトルとなる)。通常、パラメータがその最大尤度値に収束する前に、標準的な最適化技法の2または3回の反復のみが必要である。

バンドル調整の後、判断ブロック1708で、三角形に分割されてマイナスの深度になる任意の特徴点に関して構造推定値をチェックする。そうした特徴点が存在するとき、そうした特徴点は、1710においてランドマークから除去され、バンドル調整が反復される。マイナスの深度を有する点がこれ以上存在しないとき、システムは、状態1712に進み、終了する前に推定に使用される2つの画像の間のデッドレコニングオドメトリに基づいてランドマークに縮尺率を割り当てる。

［統計的条件付け基準］
いくつかの実施形態では、推定の条件付けを評価するために、カメラモーションに関する共分散は、バンドル調整(図10の状態1706)中に計算され、検討される可能性がある。特に、移動方向に垂直な不確かさは、抽出され、所定の閾値と比較される可能性がある。不確かさが所定の閾値を超えれば、ランドマーク作成を中断することができる。このチェックは、図6の状態1016の解かれた特徴点に関する閾値を向上させることができる。

［SLAMバックエンド処理］
図11は、いくつかの実施形態において説明するバックエンド処理の概要を示すフローチャートである。特に、図11は、SLAMバックエンドがグラフィックモードで地図を記憶し、グラフ内に記憶されるロボット姿勢を推定する、全体的なプロセスフローを示す。プロセスは、フロントエンドの結果を受け取ることにより、状態2601で始まる。次いで、システムは、判断ブロック2603に進み、通常の姿勢ノードを状態2604で作成すべきか、またはランドマークノードを作成すべきかを判定する。その際、システムは、2605において、生成された姿勢ノードを移動エッジ付きの先行ノードに結合する。移動エッジは、変換情報がデッドレコニングタイプのセンサから得られるエッジと定義される。

次いで、この時点で、システムは、ランドマークが観測されるかどうかを判定することができる(2608)。ランドマークが観測されないとき、システムは、状態2607に直接進み、グラフの複雑性を低減することができる。複雑性が低減されると、システムは、終了する前に状態2610に進み、グラフを最適化することができる。

その代わりに、判断ブロック2608においてランドマークが観測されたとき、システムは、最初に状態2606において姿勢ノードを観測エッジ付きのランドマークノードに結合することができる。観測エッジは、変換情報がランドマークの観測から得られるエッジと定義される。次いで、判断ブロック2622において、システムは、先行ノードがランドマークノードであるかどうかを判定する。ノードがランドマークノードであれば、システムは、状態2623に進み、モーションと観測エッジとを結合する(2623)。ランドマークの追加(2602)、観測値の追加(2606)、およびグラフ最適化(2610)を以下により詳細に説明する。次いで、システムは、状態2607および2610に進み、上述のように、グラフの複雑性を低減し、グラフを最適化することができる。

［新しいランドマーク処理］
図12は、図11のフローチャートの状態2602に関して詳細に説明し、すなわちランドマークをグラフバックエンドに組み込むためのプロセスを説明するフローチャートである。プロセスは、状態1902において新しいランドマークの表示を受け取ることにより始まる。次いで、システムは、状態1904において新しいランドマーク姿勢に先行するノードを特定する。次いで、状態1906において、そのノードと新しいランドマークの姿勢との間の漸進的なモーション推定値を構築する。次いで、新しいランドマークノードは、状態1908において作成され、構築されたモーションおよび先行ノードと一致する姿勢により初期化される。次いで、ランドマークノードと現在のノードとの間のすべてのモーション推定値は、1910において構築することができる。最後に、状態1912において、構築されたモーション推定値を使用して、新しいランドマークノードと、先行および後続のノードとの間で、グラフエッジが作成される。

［ランドマーク認識処理］
図8のプロセスが1組の観測値をもたらすとき、観測値をエッジとしてSLAMグラフに追加することができる。図13は、図11のフローチャートの状態2606に関して詳細に説明し、すなわち観測値をSLAMグラフに組み込むためのプロセスを説明するフローチャートである。特に、システムが1組の観測値を受け取るとき、現在の姿勢のグラフ内に姿勢ノードを作成することができる。次いで、状態2104において、各観測値のグラフ内にエッジが作成され、観測されたランドマークに関連するランドマークノードを、新しく作成された姿勢ノードに結合する。各エッジによりエンコードされる変換推定値は、図8に示すように、対応する観測値の平均値および共分散からなる。

［SLAMグラフ低減の概要］
一般に、SLAMグラフは、ランドマークが作成されるか、または観測され、新しいノードおよびエッジがグラフに追加されるごとに増大する。これは、ロボットが境界のある空間内にあっても当てはまる。その空間を占有するランドマークが繰り返し継続的に観測されるとき、グラフは増大し続け、複雑性は時間と共に増大する。記憶装置の要求およびグラフ最適化コストがグラフの複雑性と共に増大するので、これらのコストを抑制するために、いくつかの実施形態は、グラフの複雑性を抑制するための方法を企図する。

いくつかの実施形態では、ランドマークノードの空間密度は、視覚的フロントエンドにより自動的に抑制することができ(既存のランドマークが姿勢の一定の半径内で認識されるとき)、その結果、一定の空間領域内の演算は、限定された数のランドマークノードを示す。他方、姿勢ノードは、他のノードに関する拘束条件をエンコードするためのデータ構造を除いて、後続の演算では直接には有用でない過去のロボット姿勢を表す。姿勢ノードの数は、ランドマークの数と共にではなく、観測値の数と共に増大する。グラフの複雑性は、姿勢ノードおよび選択されたエッジを除去することにより抑制され、ランドマークの数が線形である、したがって空間の量が線形であるグラフの複雑性を探索し続けることができる。

図14は、過小評価および低減手順の一例を示す。グラフ4800Aは、除去のために選択されたノードn_rを含む元のグラフを表す。グラフ4800Bに示すように、ノードn_rおよびそのすべてのインシデントエッジは、過小評価プロセスの一部分として削除のためにマークされる。点線で示す新しいエッジが導入され、削除のためにマークされたエッジからの情報は、残りのノードに結合する新しいエッジおよび既存のエッジに伝送される。グラフ4800Cでは、中央のノードは、そのインシデントエッジと共に除去され、システムは、除去プロセスの一部として削除されたエッジを埋め合わせるために新しいエッジを導入することによりグラフを再構築する。

［グラフ低減］
図15は、グラフの複雑性の低減の概要を示す。最初に、姿勢ノードの数は、ランドマークノードの数の線形関数により区分される(2801、2802)。ノードの選択および除去の詳細を図16に与える。次いで、平均値程度のノードが区分される(2803、2804、2805)。エッジの選択および除去の詳細を図18に与える。

図15のプロセスは、全体的に3つの段階を含む。第1に、ノードは、除去のために特定され、除去され(2801、2802)、第2に、除去されたノードに入るエッジにより以前表示された情報を保持するためにエッジが追加され(図示せず)、最後に、第2段階で生成された可能性があるエッジを含む、余分なエッジを除去するためにグラフ全体にわたってエッジを削減する(2803、2804、2805)。これら3つの段階のそれぞれを以下により詳細に説明する。

［グラフ低減-姿勢ノード過小評価］
グラフが、ロボットの過去の姿勢に関するガウスマルコフ確率場(GMRF)を表すとき、GMRF状態の対応する姿勢を過小評価することにより、統計的に一貫した形式で姿勢ノードを除去することができる。グラフは、システムのマルコフ性を直接エンコードし、ノードは、それが直接結合していないすべてのノードと条件付きで独立している。したがって、ノードの状態の過小評価は、マルコフブランケットのノード(グラフ内の1つのホップ、すなわちインシデントエッジ内のノードのすべて)のみを含む。さらに、ガウス関数の周辺分布もガウス関数であるので、除去した結果のグラフは、残りの変数全体にわたって修正ガウス分布を正確にエンコードする。

図16は、図15のフローチャートの状態2802、すなわちノード選択および除去に関して詳細に説明するフローチャートである。特に、図16は、SLAMグラフ内のノードの数を低減するための姿勢ノード過小評価手順を示す。一般的に言えば、プロセスは、状態2302で始まり、そのとき、システムは、グラフから除去すべき姿勢ノードを選択する。次いで、システムは、ノードにつながるインシデントエッジの対のそれぞれを通して反復することができる。インシデントエッジの各対に関して、システムは、状態2304でエッジ逆元(edge reversal)を使用してエッジ方向を揃えることができる。次いで、システムは、状態2306で端点ノードに接続すべきエッジを合成することができる。次いで、システムは、状態2308で新しいエッジをグラフに組み合わせることができる。インシデントエッジのすべての対を考慮した後、システムは、状態2310で選択されたノードおよびインシデントエッジを削除することができる。

過小評価によるノードの除去は、除去されたノードに結合したノードのすべての対の間で対どうしの拘束条件をもたらす。そうした対の間の拘束条件(エッジ)が既に存在するとき、新しい拘束条件は、そのガウス関数の乗算により既存の拘束条件と組み合わされる。ノード過小評価手順を規定するために、エッジに関するいくつかの演算が必要である。

［エッジ逆元］
エッジeは、それぞれ適当なリー群およびリー代数における平均値および共分散(μ,Σ)により与えられる、その2つの端点ノード間の未確定(uncertain)の剛体変換を表す。リー群の共役演算子は、リー代数の成分を変換の右接空間(tangent space)から左接空間に動かすことができる。したがって、グラフ内の反対方向を向くが、同じ変換拘束条件をエンコードする、エッジ逆元e^-1は、以下の式により与えられる。
e^-1=(μ^-1,Adj[μ^-1]・Σ・Adj[μ^-1]^T)
式1

［エッジ合成］
ノードaからノードbへのエッジe₀=(μ₀,Σ₀)、およびノードbからノードcへのエッジe₁=(μ₁,Σ₁)が与えられれば、2つのエッジは、カルマンフィルタモーション更新などの未確定の変換を合成することにより、aからcへの1つのエッジに合成することができる。
e₁・e₀=(μ₁・μ₀,Σ₁+Adj[μ₁]・Σ₀・Adj[μ₁]^T)
式2

［エッジ組合せ］
2つの同じノードを同じ方向に結合する2つのエッジe₀=(μ₀,Σ₀)およびe₁=(μ₁,Σ₁)が与えられれば、それらの拘束条件は、関連のガウス分布関数を乗算することにより組み合わされ、結果としてのガウス関数を得ることができる。接空間から変換多様体へ(リー代数からリー群)の指数関数マップが非線形であるので、平均をとるための組合せ手順は、反復的である。組合せ共分散Σ_cは、2つのエッジの情報行列を加えることにより計算される。

組合せ平均値の初期推定値を第1のエッジの平均値とする。

その際、組合せ変換は、2つの変換の間の情報加重平均をとり、リー群に対する補正量の指数をとることにより更新される。

この更新は、収束するまで反復し(通常、3または4回の反復)、k回の反復後、組合せエッジを得る。

インシデントエッジE_r={e₀,…,e_m}と共に、過小評価2302により除去されるべきノードn_rを考慮する。そうしたエッジの各対(e_i,e_j)が合成される(2304)。

得られた合成エッジ(2306)は、n_rでない2つのインシデントノード間のグラフに追加される。そうしたエッジが既に存在するとき、エッジは組み合わされる(新しく合成されたエッジは、それを既存のエッジと組み合わせる前に逆元をとる必要がある可能性がある)(2308)。

最後に、すべてのインシデントエッジE_rは、ノードn_rと共にグラフから削除される(2310)。図16に戻れば、グラフ低減の目的は、グラフノードおよびエッジの濃度を制限することであるので、過小評価する姿勢ノードは、状態2302における姿勢ノードの除去により作成されるエッジの数に応じて選択される。この数は、マルコフブランケットノードの結合されていない対の数からノードの度数(インシデントエッジの数により定義される)を減算することにより決定することができる。グラフエッジの数の最小の増加を伴うノードが過小評価される。マルコフブランケットが既に十分結合しているノードでは、この増加量は、しばしばマイナスであり、グラフエッジの総数は、減少する。このプロセスは、図16のフローチャートの状態2302、すなわちSLAMグラフから除去すべき姿勢ノードの選択に関して詳細に説明するフローチャートである図17に説明される。2401では、既存のすべての姿勢ノード全体にわたる反復が行われる。2402では、各姿勢ノードに関して、そのノードの除去によりグラフに追加されたエッジの数が計算される。2403および2404において反復中、現在の最高の除去候補ノードが記憶される。最高の候補は、その除去によりエッジの数の増加が最小となるものである。反復の最後に2406において、全体の最高の候補が選択される。

［エッジ除去］
ノード過小評価手順は、常に、グラフノードの数を1つだけ減少させ、エッジの数をできる限り減少させようとするが、ノードの度数を抑制できない可能性がある。すべての姿勢ノードを過小評価することにより、ランドマークの数の二次的なエッジ濃度を有する、ランドマークノード全体にわたる完全に結合したグラフがもたらされる。

グラフのエッジの複雑性を制限するために、演算中にエッジを発見的に削減することができる。グラフからエッジを除去することは、観測または測定が全く行われなかったかのように、エッジにより表示された情報を棄却することと等しい。

1つの実施形態のエッジ削減手順は、一定の所定の限度を超える度数のノードのリストを保持する。このリストは、グラフにエッジが追加されるときにのみ変更する必要があり、ランドマーク観測中またはノード除去イベント中に生じる。次いで、ノードの度数が限度を超えなくなるまで、リスト内の各ノードからエッジが除去される。

図18は、図15のフローチャートの状態2804、すなわちエッジの選択および除去に関して詳細に説明するフローチャートである。このプロセスは、最も大きいエッジ度数(すなわち、インシデントエッジの最大数)を有する、SLAMグラフ内の姿勢ノードまたはその一部分が選択される、状態3402で始まることができる。次いで、これらのインシデントエッジのそれぞれに関して、プロセスは、インシデントエッジのどれが最高の除去候補であるかを特定するために複数の演算を実行することができる。システムは、状態3404でエッジの除去がグラフを切断するかどうかを判定する。除去がグラフを2つの結合していない構成要素に分割しないであろうとき、エッジが除去用とみなされるにすぎない。プロセスは、状態3406で検討中のエッジがこれまで検討された最も小さい残差を有するかどうか判定する。ここで、エッジの残差は、最適化プロセスからの残差を指す。これは、そのエッジが与えた拘束条件が現在の解にどれほど適合するかどうか(または、現在の解がエッジにどれほど適合するかどうか)を示す。残差に基づいて、グラフの現在の状態と最も一致し、したがって、除去がグラフ最適化に最も影響を与えそうにないエッジを特定することができる。状態3404または3406のいずれかの条件を満たさないとき、システムは、次のエッジで反復する。そうでない場合、エッジは、状態3408で最高の候補であると記録される。プロセスが完了すると、状態3410において最高の候補エッジが削除される。

［グラフ最適化］
図19は、図11のフローチャートの状態2610、すなわちグラフ最適化のための1つの方法に関して詳細に説明するフローチャートである。SLAMグラフは、SLAM推定問題に対応するGMRFを柔軟に表示する。プロセスは、状態2202において、(ノードによりエンコードされた)パラメータ推定値のマイナスの対数尤度を計算することにより始まる。この値は、以下の形式で説明されるエッジの残差全体にわたって合計することにより計算することができる。

［グラフのマイナス対数尤度計算］
E={e_i}によりエッジの組が与えられる。エッジeに関して、s(e)およびd(e)により、それぞれソースノード姿勢およびデスティネーションノード姿勢が与えられる。エッジ拘束条件の平均値をμ(e)により示し、共分散をΣ(e)により示す。その際、グラフのマイナス対数尤度-L(一定のオフセットまで)は、以下の式により残差ν_iに関して与えられる。

ノード姿勢推定値がエッジ内にエンコードされた拘束条件をより十分に満たすとき、マイナス対数尤度-Lは、より低くなる。グラフ最適化は、ノードパラメータの関数としてマイナス対数尤度を最小化することによりGMRFパラメータの尤度を増加させる。

次いで、システムは、状態2204、2206、および2208において低減した残差を生成する最適化を反復的に求めることができる。それぞれのそうした更新の後、残差(またはその変化)は、状態2204において計算することができ、判断ブロック2206において残差が低下しなかったとき、状態2208において、ノード更新値が戻され、更新値発見法を調整することができる。残差チェックと並行したそうした漸進的な姿勢調整の反復適用は、グラフの誤差を最小化し、したがって、すべてのランドマーク観測値およびモーション推定値の尤度を最大化する。状態2204に関する適当なグラフ最適化実施形態の例には、共役勾配法、確率的緩和法、ガウス・シーデル緩和法、コレスキー分解法、剛体サブグラフ調整法(rigid subgraph adjustment)、多値緩和法(multi-level relaxation)、または他の様々な確立された方法のいずれかが含まれる。さらに、漸進的な非線形最適化のための任意の一般的方法をグラフに十分に適用することができることが容易に認識されよう。

本明細書に開示した実施形態と関連させて説明した方法またはアルゴリズムのステップは、ハードウェア、プロセッサにより実行されるソフトウェアモジュール、または両者の組合せにおいて直接具体化することができる。ソフトウェアモジュールは、RAMメモリ、フラッシュメモリ、ROMメモリ、EPROMメモリ、EEPROMメモリ、レジスタ、ハードディスク、リムーバブルディスク、CD-ROM、または当分野で知られている他の任意の形式の記憶媒体内に存在する可能性がある。例示的な記憶媒体は、プロセッサに結合する可能性があり、その結果、プロセッサは、記憶媒体から情報を読み出し、記憶媒体に情報を書き込むことができる。別の方法では、記憶媒体は、プロセッサと統合することができる。プロセッサおよび記憶媒体は、ASIC内に存在する可能性がある。ASICは、ユーザ端末内に存在する可能性がある。別の方法では、プロセッサおよび記憶媒体は、ユーザ端末内の別個の構成要素として存在する可能性がある。

上述のプロセスのすべては、1つもしくは複数の汎用コンピュータまたは専用コンピュータ、あるいは1つもしくは複数の汎用プロセッサまたは専用プロセッサにより実行されるソフトウェアコードモジュール内で具体化され、このソフトウェアコードモジュールを介して十分に自動化される可能性がある。コードモジュールは、任意のタイプのコンピュータ読取可能媒体、または他のコンピュータ記憶装置、または記憶装置の集合に記憶される可能性がある。あるいは、本方法のいくつかまたはすべては、専用コンピュータハードウェア内に具体化することができる。

本明細書に説明した方法およびタスクのすべては、コンピュータシステムにより実行され、十分に自動化される可能性がある。いくつかの場合には、コンピュータシステムには、説明した機能を実行するためにネットワークを通して通信および相互運用する、複数の異なるコンピュータまたは計算装置(例えば、物理サーバ、ワークステーション、ストレージアレイなど)が含まれる可能性がある。そうした各計算装置は、通常、メモリまたは他の非一過性コンピュータ読取可能記憶媒体に記憶されたプログラム命令またはモジュールを実行する1つのプロセッサ(または複数のプロセッサ、または回路、または回路の集合、モジュールなど)を含む。本明細書に説明した様々な機能をそうしたプログラム命令内に具体化することができるが、代わりに、開示した機能のいくつかまたはすべてをコンピュータシステムの特定用途向回路(ASICまたはFPGAなど)内に実装することができる。コンピュータシステムが複数の計算装置を含む場合、これらの装置は、必須ではないが、同じ場所に配置される可能性がある。開示した方法およびタスクの結果は、固体メモリチップおよび/または磁気ディスクなどの物理記憶装置を異なる状態に変換することにより持続的に記憶することができる。

108 制御装置
110 モータ
112 モータ
504 物理的ランドマーク
506 特徴点
508 特徴点
510 特徴点
602 視覚的フロントエンド
606 ランドマークデータベース
608 SLAMデータベース
610 生姿勢
612 視覚データ
614 デッドレコニングセンサ
616 視覚センサ
618 デッドレコニングインターフェース
620 姿勢、地図
622 プレフィルタ
1802 姿勢ノード
1804 ランドマークノード
1806 エッジ
1808 ノード
1810 エッジ
1812 ノードID
1814 ランドマークID
1816 ロボット姿勢
1818 エッジ識別子
1820 エッジID
1822 ソースノードID
1824 デスティネーションノードID
1826 変換
1828 変換の共分散

Claims (18)

1. プロセッサおよびカメラを備えたシステム内で位置推定および地図構築を行うための方法であって、前記プロセッサは、複数の姿勢ノードおよび複数のエッジを含むグラフを生成するように構成され、本方法は、
   前記グラフ内の姿勢ノードの数が第1の閾値を超える場合に前記グラフを更新するステップであって、
   i) 2つ以上のインシデントエッジと関連付けられたマルコフブランケットノードに直接結合する姿勢ノードを特定するステップ、
   ii) 前記関連付けられたマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップ、ならびに
   iii) 前記特定された姿勢ノードおよび前記2つ以上のインシデントエッジを除去するステップ
   を含む、ステップと、
   前記グラフ内のエッジの総数が第2の閾値を超える場合に、前記グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップと、
   前記グラフ内に存在する前記複数のエッジに少なくとも部分的に基づいて残りの姿勢ノードの位置の推定値を更新するステップと、
   を含むことを特徴とする方法。
2. 前記姿勢ノードを特定するステップは、
   i) 前記グラフ内の各姿勢ノードに関して、前記姿勢ノードが除去されたとき前記グラフ内に存在するであろうエッジ、およびマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために合成されたインシデントエッジの総数を特定するステップと、
   ii) 前記グラフから除去される場合にエッジの最小の総数をもたらすであろう前記姿勢ノードを選択するステップと、
   を含むことを特徴とする請求項1に記載の方法。
3. 前記インシデントエッジを合成するステップは、
   i) マルコフブランケットノードの対の間の相対姿勢の推定値を生成するステップであって、前記相対姿勢の推定値は、2つ以上のインシデントエッジに関連する相対姿勢の測定値のベクトル和に部分的に基づいている、ステップと、
   ii) 2つ以上のインシデントエッジに関連する共分散推定値に基づいて1つの共分散推定値を生成するステップと、
   を含むことを特徴とする請求項1に記載の方法。
4. 前記グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップは、
   i) 前記グラフ内の各エッジの残差値を生成するステップであって、前記残差値は、前記グラフ内の前記エッジにより接続される前記ノードの相対姿勢と、同じエッジに関連する変換値により与えられる相対姿勢との間の差に少なくとも部分的に基づいている、ステップと、
   ii) 最小の残差値を有するエッジを特定するステップと、
   iii) 前記特定されたエッジを前記グラフから除去するステップと、
   を含むことを特徴とする請求項1に記載の方法。
5. 前記システムは、ナビゲーションシステムおよび経路プランナをさらに含む移動ロボットであり、請求項1に記載の方法は、
   i) 軌道を走行するために1組のロボット移動命令を生成するステップと、
   ii) 前記グラフ内に存在する前記姿勢ノードの前記位置の前記推定値を更新した後、前記軌道ならびに前記グラフ内に存在する前記姿勢ノードおよび複数のランドマーク姿勢ノードの前記位置の更新された推定値に少なくとも部分的に基づいて前記ロボットをナビゲートするための前記1組の移動命令を更新するステップと、
   をさらに含むことを特徴とする方法。
6. i) 姿勢ノードは、前記ロボットの姿勢を含み、
   ii) ランドマークノードは、前記ロボットの姿勢、1つまたは複数の物体に対応するランドマーク識別子、および前記1つまたは複数の物体のそれぞれの位置の推定値を含み、
   iii) エッジは、2つの位置における前記ロボットの位置および方位に関する剛体変換を含むことを特徴とする請求項5に記載の方法。
7. 前記軌道を走行しながら少なくとも1つの画像を取得するステップと、
   前記1つまたは複数の画像が既知のランドマークまたは新しいランドマークを示すかどうかを特定するステップと、
   前記グラフを更新するステップであって、
   i) 前記少なくとも1つの画像が既知のランドマークを示すとき、新しい姿勢ノードを生成するステップ、
   ii) 前記少なくとも1つの画像が新しいランドマークを示すとき、新しいランドマーク姿勢ノードを生成するステップ、および
   iii) 前記新しい姿勢ノードまたは新しいランドマーク姿勢ノードを前記グラフ内の1つまたは複数の既存のノードに関連付ける少なくとも1つの新しいエッジを生成するステップを含む、ステップと、
   をさらに含むことを特徴とする請求項5に記載の方法。
8. それぞれがscale-invariant feature transform(SIFT)特徴点を含む、複数の特徴点を抽出するステップをさらに含むことを特徴とする請求項7に記載の方法。
9. 前記関連付けられたマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップは、
   i) 前記新しいエッジの平均値と既存のエッジの平均値との重み付き平均値を生成し、前記新しいエッジの共分散推定値および前記既存のエッジの共分散推定値の逆行列の和に基づいて新しい共分散推定値を生成することにより、前記新しいエッジの少なくとも1つを既存のエッジと組み合わせるステップ
   をさらに含むことを特徴とする請求項1に記載の方法。
10. 移動型電子装置であって、
    画像を取得するように構成されたカメラと、
    複数の姿勢ノードおよびエッジを含むグラフを保持するように構成されたナビゲーションシステムと、
    を備え、
    前記ナビゲーションシステムは、
    前記グラフ内の姿勢ノードの数が第1の閾値を超える場合に前記グラフを更新し、前記更新は、
    i) 2つ以上のインシデントエッジにより関連付けられたマルコフブランケットノードに直接結合する姿勢ノードを特定するステップ、
    ii) 前記関連付けられたマルコフブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップ、ならびに
    iii) 前記特定された姿勢ノードおよび前記2つ以上のインシデントエッジを除去するステップを含み、
    前記グラフ内のエッジの総数が第2の閾値を超える場合に、前記グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去し、
    前記グラフ内に存在する前記複数のエッジに少なくとも部分的に基づいて残りの姿勢ノードのそれぞれの位置の推定値を更新するように構成されることを特徴とする移動型電子装置。
11. 前記姿勢ノードを特定するステップは、
    i) 前記グラフ内の各姿勢ノードに関して、前記姿勢ノードが除去されたとき前記グラフ内に存在するであろうエッジ、およびブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために合成されたインシデントエッジの総数を特定するステップと、
    ii) 前記グラフから除去される場合にエッジの最小の総数をもたらすであろう前記姿勢ノードを選択するステップと、
    を含むことを特徴とする請求項10に記載の移動型電子装置。
12. 前記インシデントエッジを合成するステップは、
    i) ブランケットノードの対の間の相対姿勢の推定値を生成するステップであって、前記相対姿勢推定値は、2つ以上のインシデントエッジに関連する相対姿勢測定値のベクトル和である、ステップと、
    ii) 前記2つ以上のインシデントエッジに関連する共分散推定値に基づいて1つの共分散推定値を生成するステップと、
    を含むことを特徴とする請求項10に記載の移動型電子装置。
13. 前記グラフ内に存在する前記複数のエッジの少なくとも1つのエッジを除去するステップは、
    i) 前記グラフ内の各エッジの残差値を生成するステップであって、前記残差値は、前記更新されたグラフ内の前記エッジにより接続される前記ノードの相対姿勢と、同じエッジの平均値により与えられる相対姿勢との間の差に少なくとも部分的に基づいている、ステップと、
    ii) 最小の残差値を有するエッジを特定するステップと、
    iii) 前記特定されたエッジを前記グラフから除去するステップと、
    を含むことを特徴とする請求項10に記載の移動型電子装置。
14. 前記移動型電子装置は、経路プランナをさらに含む移動ロボットであり、前記プランナは、
    i) 軌道を走行するために1組のロボット移動命令を生成し、
    ii) 前記残りの姿勢ノードの位置の推定値を更新した後、前記軌道および前記残りの姿勢ノードの位置の更新された推定値に少なくとも部分的に基づいて前記ロボットをナビゲートするための前記1組の移動命令を更新するようにさらに構成されることを特徴とする請求項13に記載の移動型電子装置。
15. i) 姿勢ノードは、前記ロボットの姿勢を含み、
    ii) ランドマークノードは、前記ロボットの姿勢、1つまたは複数の物体に対応するランドマーク識別子、および前記1つまたは複数の物体のそれぞれの位置の推定値を含み、
    iii) エッジは、2つの位置における前記ロボットの位置および方位に関する剛体変換を含むことを特徴とする請求項14に記載の移動型電子装置。
16. 前記ナビゲーションシステムは、
    前記軌道を走行しながら少なくとも1つの画像を取得し、
    前記1つまたは複数の画像が既知のランドマークまたは新しいランドマークを示すかどうかを特定し、
    前記グラフを、
    i) 前記少なくとも1つの画像が既知のランドマークを示すとき、新しい姿勢ノードを生成し、
    ii) 前記少なくとも1つの画像が新しいランドマークを示すとき、新しいランドマークノードを生成し、
    iii) 前記新しい姿勢ノードまたは新しいランドマークノードを前記グラフ内の1つまたは複数の既存のノードに関連付ける少なくとも1つの新しいエッジを生成することにより更新する
    ようにさらに構成されることを特徴とする請求項14に記載の移動型電子装置。
17. 前記ナビゲーションシステムは、それぞれがscale-invariant feature transform(SIFT)特徴点を含む、複数の特徴点を抽出するようにさらに構成されることを特徴とする請求項16に記載の移動型電子装置。
18. 前記関連づけられたブランケットノードの対の間に1つまたは複数の新しいエッジを生成するために前記インシデントエッジを合成するステップは、
    i) 前記新しいエッジと既存のエッジとの相対姿勢の推定値を平均し、前記新しいエッジと既存のエッジとの共分散推定値を平均することにより、前記新しいエッジの少なくとも1つを既存のエッジと組み合わせるステップをさらに含むことを特徴とする請求項10に記載の移動型電子装置。

Images