以下は、Paul Furgale と Timothy D. Barfoot による論文
“Visual Teach and Repeat for Long-Range Rover Autonomy” (Journal of Field Robotics, 2010) の要約です

---

📌 論文の目的

GPSが利用できない環境（例：火星探査）において、
ステレオカメラのみを用いて長距離自律走行を実現する手法を提案・実証すること。

---

🎯 背景と問題設定

通常の自己位置推定（VO・SLAMなど）は：

• 走行距離が伸びるほど誤差が累積
• グローバル整合性を維持するのが困難
• GPSがない環境では長距離自律が難しい

そこで本研究では：

🔁 Teach-and-Repeat方式（教示走行＋再走行）

を採用。

---

🧠 提案手法の核心

1️⃣ Teachフェーズ（学習走行）

• 人間または別の自律アルゴリズムで一度ルートを走行
• ステレオ画像から特徴点を抽出（SURF）
• 3D特徴を三角測量
• ルートを 小さなサブマップ群（約5m単位） に分割
• 各サブマップは：
  • 3D特徴点群
  • 局所地面平面
  • 参照経路
  • 難易度指標

⚠️ 重要：

• グローバル整合性は求めない
• 局所的にユークリッドであればよい（Brooksの思想）

---

2️⃣ Repeatフェーズ（自律再走行）

• 現在画像とサブマップの特徴をマッチング
• RANSAC＋Gauss-Newtonで6DoF推定
• 局所地面平面へ投影して2D経路追従
• 定期的にVOとマップ局所化を切替（ハイブリッド）

💡 これが本論文の重要ポイント：

VO（相対）＋ マップ局所化（絶対）の交互利用
→ 長距離でも誤差が発散しない

---

🌍 実験結果

実験環境

• カナダ北極圏（火星アナログ環境）
• 都市環境
• 草地
• 傾斜地（最大28°）
• 屋内→屋外の極端な照明変化

成果

• 総走行距離：32 km以上
• 自律率：99.6%
• 最長自律走行：3.2 km
• GPS未使用

これは当時としては極めて大規模な実証。

---

📈 技術的特徴

🔹 マニフォールドマップ

• サブマップをグラフ接続
• グローバル整合を要求しない

🔹 ステレオ使用の利点

• 単眼よりも3D構造推定が安定
• 直接三角測量可能

🔹 難易度推定

• 姿勢変化のRMS値でサブマップ難易度算出
• 走行速度を自動調整

🔹 失敗処理

• 局所化失敗時は近傍サブマップ検索
• Broken submapフラグ処理

---

⚠️ 制限

• 照明変化に弱い（SURFベース）
• 逆向き走行は可能だが視線方向は固定
• 動的障害物回避は未実装
• 学習ルート依存

---

🧩 本研究の意義

この論文は：

• 「完全SLAMではなく、局所的整合で十分」という実証
• 実環境でのマルチkm実験
• Teach-and-Repeatを3D不整地に拡張

という点で、非常に重要。

特にあなたのように：

• VPR
• Step-and-Check
• HOLD/JUMP
• Integrity Monitoring

を研究している場合、

この論文は：

🔑 「VPRを状態監視に使う」という発想の源流

に近い考え方です。

---

🧭 まとめ（超短縮版）

項目	内容
センサ	ステレオカメラのみ
戦略	Teach-and-Repeat
地図	局所サブマップ群
局所化	VO＋マップマッチング
実証	32 km / 99.6%自律
意義	GPSなし長距離自律を実証

---

 

日本語訳

本論文は、ステレオカメラのみをセンサとして用い、長距離ローバーの自律走行を可能にするシステムについて述べるものである。学習フェーズにおいて、ローバーがルートに沿って操縦される間に、システムは重なり合うサブマップから構成されるマニフォールドマップを構築する。その後、ローバーが自律的に同じルートを再走行する際には、このマップが自己位置推定に使用される。局所的なサブマップを用いることで、正確なグローバル再構成を必要とせずに、長距離ルートを忠実に再現することが可能となる。

非平面地形における経路追従は、まず三次元で自己位置推定を行い、その結果を現在のサブマップに対応する局所的な地面平面へ投影し、その平面上で経路追従制御を行うことで実現される。

本システムは、都市環境およびカナダ北極圏の惑星アナログ環境において実験を行った。合計32km以上を走行し、そのうち99.6％は自律走行であった。自律走行距離は45mから3.2kmに及び、いずれもGPSを使用していない。単一の指令サイクルで長距離自律行動を可能にすることから、ビジュアル・ティーチ・アンド・リピート方式は、GPSが利用できない火星サンプルリターンなどの惑星探査用途に適している。

1. はじめに

GPSまたはそれに相当するシステムが利用できない環境では、ローバーの長距離自律航行は極めて困難な問題となる。視覚・慣性・オドメトリなどを組み合わせた相対運動推定システムは近年高精度化している。しかし、その精度がいかに高くても、定期的なグローバル補正が行われない限り、走行距離の増加とともに位置推定誤差は無限に増大してしまう。

本論文では、屋外の非構造化環境において移動ロボットを長距離自律運用するための完全なシステムを提案する。このシステムは、ステレオカメラのみを用いたセンシングと、ティーチ・アンド・リピートという運用戦略によって実現される。

学習フェーズ（teach pass）では、ローバーが目的のルートを走行する（人間が操縦する場合もあれば、外部の自律システムによる場合もある）間に、マッピングシステムが重なり合うサブマップからなるマニフォールドマップを構築する。トポロジカルに接続されたこれらのサブマップは、自律走行フェーズ（repeat pass）において自己位置推定に使用される。

このトポロジカル／メトリックのハイブリッド表現は、正確なグローバル再構成の必要性を軽減すると同時に、純粋な相対運動推定に伴う誤差の無限増大を回避する。また、小さなサブマップ群を用いることで、経路再走行時の計算量を経路長から切り離すことができる。

類似のシステムは、屋内環境（Goedemeら, 2005）、鉱山環境（Marshallら, 2008）、あるいは平面的な屋外環境（Royerら, 2007; Segvićら, 2009）で提案されてきた。しかし本研究は、GPSを用いずに、高度に三次元的な屋外非構造化環境において、数キロメートル規模の自律走行を実現した最初のシステムである。

我々のビジュアル・ティーチ・アンド・リピートシステムは、GPSが利用できない状況でローバーの自律性が求められる多くのシナリオに適用可能である。例えば、火星サンプルリターン支援や、月面着陸地点と居住拠点間の機材輸送の自動化などが想定される。そのため、我々はカナダ北極圏デボン島の火星／月アナログサイトで本システムを試験した。さらに、都市環境、草地、困難な三次元地形、屋内から屋外への極端な照明変化といった条件下でも試験を行った。

本論文では、GPSを使用せずに32km以上の評価走行を行い、そのうち99.6％が自律走行であった結果を報告する。

2. 関連研究

視覚に基づく地図構築に関する初期の論文において、Brooks（1985）はロボット地図構築の基本原理として次の点を示した。

• 世界は本質的に三次元である。したがって、自己位置推定および地図構築もそれを反映すべきである。

• センサには不確実性があるため、生成される地図はグローバルには整合しない可能性がある。しかし、ロボットの自律動作を実現するためには、地図は局所的に整合していれば十分である。

この考えに対処するため、Brooksは自由空間プリミティブの抽象グラフから構成される地図を提案した。同様の概念として、Howard、Sukhatme、Mataric（2006）は、ロボットの地図を高次元空間に埋め込まれたマニフォールドとして表現するマルチエージェントシステムを設計・実装した。

マニフォールドマッピングは、地図が世界を表現する方法を変える。地図は接続された空間の列を定義するという意味でトポロジカルになるが、地図上の空間と実世界の空間は多対一対応を持つ場合がある。このトポロジーは、地図をサブマップのグラフに分割する方法（Bosseら, 2004; Eade & Drummond, 2008; Howardら, 2006; Marshallら, 2008）や、連続的な相対表現を用いる方法（Meiら, 2009; Sibleyら, 2009）によって実現される。

純粋なメトリック地図では不整合を引き起こす累積誤差も、マニフォールド表現の中では問題とならない。その結果、ループ閉合の判断を遅らせることができ（Howardら, 2006）、地図の大きさに関係なく一定時間でループ閉合を行うことも可能である（Sibleyら, 2009）。マニフォールドマッピングは、地図がグローバルに整合していなければならないという制約を取り除くが、自己位置推定に有用であるためには、ロボット周辺の近傍は局所的にユークリッド空間として見えなければならない。

この「局所ユークリッド性」の制約がSLAM問題にどのように現れるかを理解するため、基本的なSLAM方程式の構造を考える。SLAMは確率的に定式化され、時刻kにおける地図mと車両状態vkの同時事後分布

$$p(x_k, m \mid z_{0:k}, u_{0:k}, x_0)$$

p(xk​,m∣z0:k​,u0:k​,x0​)

を推定する問題である（Durrant-Whyte & Bailey, 2006）。

多くの解法では、現在の状態と地図推定のもとでの観測ベクトルzkの尤度

$$p(z_k \mid x_k, m)$$

p(zk​∣xk​,m)

を計算する。この尤度は観測モデルh(·)を用いて

$$z_k = h(x_k, m) + v_k$$

zk​=h(xk​,m)+vk​

と表される。ここでvkは観測ノイズである。

この式の性質が制約の形を決める。多くのナビゲーションセンサはロボットの局所近傍の幾何構造を観測するため、地図が有効であるためには、その近傍がセンサにとってユークリッド空間に見える必要がある。そこからの逸脱はノイズvkの中に隠れる程度に小さくなければならない。これが、Sibleyら（2009）の適応最適化手法や、Marshallら（2008）のサブマップサイズ選択の動機である。この制約が満たされていれば、たとえグローバル再構成が不正確であっても、地図は自己位置推定に利用できる。

---

視覚的ティーチ・アンド・リピート方式のナビゲーションシステムは、この概念に基づいて構築されている。トポロジカルに接続されたキーフレームと、ロボットを同じ視点へ導こうとする制御器を組み合わせるものである。

本研究のレビューでは、カメラベースのシステムに焦点を当てる。Marshallら（2008）およびNewmanら（2002）は、屋内廊下環境で平面レーザ距離センサを用いたティーチ・アンド・リピートシステムを構築した。これらは地下鉱山やオフィスビルのような環境では有効であるが、屋外の非構造化環境では、レーザの測距範囲内に壁が存在する保証はない。一方カメラは、地形構造に依存しない。必要なのは周囲光とテクスチャだけである。広視野角や全方位カメラは、ある視点に特有で比較的安定な大域的幾何形状と外観を取得できる。そのため、過去に訪れた場所の認識に適している。

初期の視覚ティーチ・アンド・リピート研究では、正確なグローバル再構成がなくても経路を再現できることが重要な利点として認識されていた（Baumgartner & Skaar, 1994; Brooks, 1985）。

ティーチ・アンド・リピート方式は、Bonin-Fontら（2008）の分類によれば、地図ベースアプローチの連続体にまたがっている。純粋にメトリック（Baumgartner & Skaar, 1994; Royerら, 2007）、トポロジカル／メトリックの混合（Goedeméら, 2007; Segvićら, 2009）、あるいは純粋にトポロジカル（経路上の位置のみを追跡）（Matsumotoら, 1996; Zhang & Kleeman, 2009）などである。本研究のシステムはトポロジカル／メトリック型であり、自己位置推定は3次元で行い、経路追従は局所平面に射影された空間で行い、経路管理はトポロジカルに処理する。

---

外観ベース手法は、入力画像の大部分を教示時に取得したプロトタイプ画像と比較する。Matsumotoら（1996）は単眼カメラで廊下内を自律航行するシステムを開発した。学習フェーズでは順序付き画像列を取得し、再走行時には中央部分のテンプレートを近傍画像と相関させて操舵角を決定する。

その後の研究（Jonesら, 1997; Payáら, 2007; Zhang & Kleeman, 2009）では効率化や全方位画像の利用が進められた。しかしこれらの外観ベース手法は、カメラ運動が平面上に限定されるという仮定に依存しているため、屋外非構造化環境には適さない。

---

別の研究群では、画像特徴を用いた地図構築とナビゲーションが行われているが、やはり平面運動仮定に依存している。Ohnoら（1996）、Tang & Yuta（2001）、Bekrisら（2006）などがその例である。これらは特徴の方位のみを用いるか、平面上の局所化に限定している。

三次元的な非平面運動に対応するためには、点特徴を用いた三次元自己位置推定が必要である。Royerら（2007）は単眼画像列から大域的再構成を行う手法を提案した。一方、Segvićら（2009）は局所的再構成を多数行う方式を提案した。本研究はこれらに近いが、主センサとしてステレオカメラを用いる点が異なる。

---

ステレオカメラは、各ステレオ画像対の中にメトリック構造を提供するため、再構成を大幅に簡素化できる。Konoligeら（2009）はステレオナビゲーションシステムに単純なティーチ・アンド・リピート機能を組み込んだが、長距離ではグローバル整合性維持のために再局所化が必要である。

本研究では、ステレオビジョンのみを用いて、非平面運動を含む屋外非構造化環境において長距離経路を再現できることを示す。基盤となるのはビジュアルオドメトリ（VO）であり、ステレオ特徴追跡、RANSACによる外れ値除去、反復最適化による姿勢推定を行う。これをマッピングおよび局所化システムへ拡張し、単一の指令サイクルで複数キロメートルの自律走行が可能であることを実証する。

3. システム概要

本節では、我々の**視覚ティーチ・アンド・リピート（visual teach-and-repeat）**システムを詳細に説明する。本システムの主要な処理ブロックは図2に示されている。教示（teach）側と追従（repeat）側の両システムは、校正済みの平行ステレオ視（calibrated, parallel stereo vision）に基づいているため、まず基礎事項と記法を示す。次に、経路学習（route-learning）システムを説明する。最後に、経路追従（route-following）アルゴリズムと、その失敗（failure）への対処について概説する。

本システムで用いる座標系を図3に示す。地図座標系

$F_{ \rightarrow m}$

F→m​ は、すべての3次元推定が行われる座標系である。時刻

$k$

k におけるステレオ対の左カメラに固定された座標系を

$F_{ \rightarrow c_k}$

F→ck​​ と定義する。このときのカメラ姿勢は、

$F_{ \rightarrow c_k}$

F→ck​​ から

$F_{ \rightarrow m}$

F→m​ へベクトルを変換する回転行列

$C_{m,c_k}$

Cm,ck​​ によって表される。同様に、カメラ位置は

$\rho_{c_k,m}^m$

ρck​,mm​ と定義し、これは

$F_{ \rightarrow m}$

F→m​ の原点から

$F_{ \rightarrow c_k}$

F→ck​​ の原点へのベクトル（上付き添字で示す）を、

$F_{ \rightarrow m}$

F→m​ で表現したもの（下付き添字で示す）である。

同様の記法により、カメラ座標系

$F_{ \rightarrow c_k}$

F→ck​​ と車体（ビークル）座標系

$F_{ \rightarrow v_k}$

F→vk​​ の間の回転

$C_{c_k,v_k}$

Cck​,vk​​ と並進

$\rho_{c_k,v_k}^{v_k}$

ρck​,vk​vk​​ を定義する。この変換は静的であると仮定するが、時間変化してもよい。最後に、投影座標系

$F_{ \rightarrow p}$

F→p​ は、経路追従コントローラが必要とする「3次元から2次元への投影」を定義する座標系である。

---

3.1 ステレオ・パイプライン

本プロジェクトおよび他のプロジェクトを実現するために、我々はNVIDIAのCompute Unified Device Architecture（CUDA）ツールキットを用い、グラフィックス処理装置（GPU）上で完全に実装された疎（スパース）ステレオ・パイプラインを開発した。図4に示すように、このパイプラインはSpeeded-Up Robust Features（SURF）アルゴリズム（Bay ら, 2008）に基づいている。カメラは任意にロールできないローバーに搭載されているため、回転不変ではない upright descriptor（姿勢を立てた記述子）を用いる。
（注1：upright descriptorを用いても、制御器が同じ視点へ戻すように走行させれば、軌道自体はヘリコプターや水中機のように真に3次元でもよい。upright descriptorは軌道を制限するのではなく、再走行時にカメラ座標系のロール誤差があると自己位置推定を妨げるだけである。）

異なるシーンや照明条件においても、計算量を一定に保ちつつ頑健な性能を得るために、適応的なしきい値（adaptive thresholds）を用いる。時刻

$k$

k にステレオ・パイプラインから得られる各キーポイント

$j$

j は、画像座標

$y_{k,j}$

yk,j​ と、64次元の記述ベクトル

$d_{k,j}$

dk,j​ を持つ。

ステレオ画像対で得られたキーポイントを扱う際には、視差座標（disparity coordinates）（Demirdjian & Darrell, 2002）を用いる。キーポイント

$y_{k,j}$

yk,j​ は次の成分を持つ：

$$y_{k,j} := \begin{bmatrix} u\\ v\\ d \end{bmatrix}.$$

yk,j​:=​uvd​​.

本研究の定式化では左カメラを基準とするため、

$u$

u と

$v$

v は左画像における水平・垂直のピクセル座標であり、

$d$

d は視差（左画像と右画像の水平位置の差）である。

校正済みの平行ステレオカメラモデルは次のパラメータを持つ：

$c_u, c_v$

cu​,cv​：画像左上を原点とするピクセル座標での水平・垂直の主点

$f_u, f_v$

fu​,fv​：ピクセル単位の水平・垂直焦点距離

$b$

b：カメラ基線長（2つの投影中心間距離）［m］

観測モデル

$h(\cdot)$

h(⋅) は、左カメラ座標系で表された点を視差座標へ射影する非線形関数である。左カメラ座標系での3次元特徴点位置を

$$p_{j,c_k}^{c_k}= \begin{bmatrix} x\\ y\\ z \end{bmatrix}$$

pj,ck​ck​​=​xyz​​

とすると、その点の画像上の位置

$y_{k,j}$

yk,j​ は

$$y_{k,j}=h\!\left(p_{j,c_k}^{c_k}\right) =\frac{1}{z} \begin{bmatrix} f_u & 0 & c_u & 0\\ 0 & f_v & c_v & 0\\ 0 & 0 & 0 & b f_u \end{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1 \end{bmatrix}. \quad (3)$$

yk,j​=h(pj,ck​ck​​)=z1​​fu​00​0fv​0​cu​cv​0​00bfu​​​​xyz1​​.(3)

校正済みステレオカメラを用いているため、式(3)は可逆である。逆観測モデル

$g(\cdot)$

g(⋅) は、ステレオ対で観測された点を三角測量する：

$$p_{j,c_k}^{c_k}=g(y_{k,j}) =\frac{b}{d} \begin{bmatrix} \frac{u-c_u}{f_u}\\ \frac{f_v}{f_u}(v-c_v)\\ 1 \end{bmatrix}. \quad (4)$$

pj,ck​ck​​=g(yk,j​)=db​​fu​u−cu​​fu​fv​​(v−cv​)1​​.(4)

---

3.2 汎用の自己位置推定モジュール

本研究全体を通して、Moravec（1980）により提案され、Matthies（1989）らによって改良されたステレオVO（Visual Odometry）アルゴリズムに基づく汎用自己位置推定モジュールを用いる。その概要を図5に示す。ステレオのキーポイントを特徴データベースに対して追跡し、その追跡結果に外れ値検出を適用し、内点（inlier）として残った追跡のみを用いてカメラの現在姿勢を推定する。

特徴データベースや数値解法のブロックを差し替えることで、ティーチ・アンド・リピートに必要な各動作モード（地図構築、VO、サブマップ選択、自己位置推定）を構成できる。本節は、各モードの詳細を説明する際に参照する。以下では、各ブロックの要件を示す。

特徴データベースは、ロボットが自己位置推定を行うための地図を表す。そのために、以下の情報を提供する：

• $N$N：データベース内の特徴点数

• $q_{i,m}^m$qi,mm​：特徴

  $i$i の位置

  $[x_i\; y_i\; z_i]^T$[xi​yi​zi​]T（

  $F_{\rightarrow m}$F→m​ に対する位置で、かつ

  $F_{\rightarrow m}$F→m​ で表現）

• $v_i$vi​：特徴

  $i$i に対応するSURF記述子

データ対応付け（data association）は、記述子空間における最近傍探索によって行う。これらの記法と3.1節の記法を用いると、図5の最初のブロックの出力は、データベース中の特徴

$i$

i と最新のステレオ対から得られた特徴

$j$

j を対応付ける候補トラックのリストである。

候補トラックは外れ値検出ブロックへ渡される。我々は、固定の計算予算のもとで平均的に最良の内点集合を得られるため、preemptive RANSAC（Nistér, 2005）を実装した。特徴データベースと入力ステレオキーポイントを3次元点群（各特徴は式(4)で三角測量）として扱い、仮説生成にはHorn（1987）の3点クォータニオン法を用いる。preemptive RANSACは、内点となる特徴トラックの集合と、

$F_{\rightarrow m}$

F→m​ におけるカメラ姿勢の粗い推定値を生成する。

最後に、内点トラックは姿勢解法（pose solution）へ渡される。この解法は、各入力キーポイントの視差座標、特徴データベース、RANSACが与えた姿勢推定値、前時刻のカメラ姿勢にアクセスできる。これらを用いて、

$F_{\rightarrow m}$

F→m​ におけるカメラ姿勢を推定する：すなわち、

$F_{\rightarrow m}$

F→m​ に対するカメラの姿勢（回転）

$C_{c_k,m}$

Cck​,m​ と、

$F_{\rightarrow m}$

F→m​ に対する位置（かつ

$F_{\rightarrow m}$

F→m​ で表現された）

$\rho_{c_k,m}^m$

ρck​,mm​ である。

各解法はガウス–ニュートン法による反復最小化に基づくが、動作モードごとに異なる数学的定式化を用いる。

 

3.3 経路教示（Route Teaching）

経路教示の基本的なプロセスは、まず一度ルートを走行しながらステレオ画像を記録し、その後、その画像列を後処理して重なり合う複数のサブマップへと変換することである。後処理の流れは図6に示されている。前段ではマッピングループが地図を逐次構築し、その中でローバーの位置を推定する。一定の間隔で地図を分割し、生データはさらに処理され、再走行（repeat pass）で使用される形式へと変換される。

---

3.3.1 教示走行時の自己位置推定とマッピング

このマッピングループは一見SLAM問題を解いているように見える。しかし、本システムの要件はSLAMとは異なる設計選択を要求する。

各サブマップは局所的に整合していなければならない。また、隣接するサブマップ間の変換は妥当でなければならない。しかし、これらの制約を除けば、地図列全体のグローバル整合性はアルゴリズム性能に影響を与えるべきではない。そのため、本システムはグローバル整合性の達成を目的としない。

図7には、5kmの地図列をGPSと比較した例と、ロボットの視点から見た地図が示されている。経路全体の再構成は大きく不正確であるが、局所的には経路追従に十分な精度を持っている。

---

サブマップの構築方法

サブマップは、3.2節で説明した汎用自己位置推定モジュールの特化版を用いて構築される。

システムは最初のキーポイント集合 ({y_{0,j}, d_{0,j}}) で初期化される。地図座標系 (F_{\rightarrow m}) は (F_{\rightarrow c_0}) と同一に定義される。すべてのキーポイントは式(4)により三角測量され、地図内に配置される。

以降の各フレームでは、入力キーポイントを現在のデータベースと照合し、外れ値除去を行う。内点（inlier）となった特徴トラックを (n) でインデックス付けする。各トラックは、地図中の特徴 (i) と現在のキーポイント (j) を対応付ける。

カメラ姿勢 (C_{c_k,m}) および位置 (\rho_{c_k,m}^m) を推定するため、誤差項 (e_n) を次のように定義する：

[
e_n := y_{k,j} – h!\left(C_{c_k,m}\big(q_{i,m}^m – \rho_{c_k,m}^m\big)\right).
]

時刻 (k) における特徴トラック数を (M_k) とすると、目的関数 (J_k) は次のように定義される：

[
J_k := \frac{1}{2} \sum_{n=1}^{M_k} e_n^T W_n e_n.
\tag{5}
]

ここで (W_n) は (y_{k,j}) の推定観測共分散の逆行列に基づく重み行列である。この式を線形化し、ガウス–ニュートン法により最小化する。

---

地図への特徴追加

追跡できた特徴の割合が閾値 (\tau_f) を下回ると、推定姿勢 ((C_{c_k,m}, \rho_{c_k,m}^m)) が参照経路に追加され、すべてのキーポイントが地図へ追加される。

この閾値を用いることで：

• ロボットが停止しているときに無駄に地図が肥大化することを防ぐ
• 地形の難易度に応じて特徴数を自動調整する

という効果がある。

推定された姿勢を用いて、三角測量されたキーポイントを共通座標系 (F_{\rightarrow m}) に配置する：

[
q_{i,m}^m = C_{c_k,m}^T g(y_{j,k}) + \rho_{c_k,m}^m
= C_{c_k,m}^T p_{j,c_k}^{c_k} + \rho_{c_k,m}^m.
]

本システムでは、各特徴のプロトタイプは「最初に観測されたときの三角測量位置とSURF記述子」に基づいている。

追跡に失敗したキーポイントは新規特徴として地図に追加される。追跡に成功したものはすでに地図に存在するため、新たな観測は破棄される。

---

グローバル整合性を求めない理由

ここにはカメラ姿勢や特徴位置を最適化するのに十分な情報が存在する（全地図に対する最適化やスライディングウィンドウ最適化も可能である）。しかし本システムではグローバルに整合した地図を構築する必要はない。

実験結果からも、この実装で再走行に必要な局所的メトリック自己位置推定には十分であることが示されている。

将来的には高度な再構成技術の評価も考えられるが、動作可能なシステムを構築するために局所バンドル調整は必須ではない。

---

サブマップの分割

姿勢と特徴が地図に追加されるたびに、現在の参照経路長を追跡する。経路長が閾値 (\tau_l) を超えると、サブマップを再走行用にパッケージ化してディスクへ保存する。

このパラメータを変更することで、

• 完全なグローバル再構成
• 単一画像との照合による逐次的ルート表現

の間を滑らかに調整できる。

我々は初期段階で複数の (\tau_l) 値を試した。大きな値（大きなサブマップ）は局所化ドロップアウトに対する頑健性を向上させたが、含まれる特徴数が増えるため計算量が増大した。最終的に (\tau_l = 5m) を採用した。これはロボットを実験に十分な速度で走行させるために必要なフレームレートを維持できる最大値であった。

---

マップ構築の失敗

マップ構築は、地図から最新画像への特徴追跡が最小数に達しない場合に失敗することがある。

原因として：

• フレーム間の移動量が大きすぎる
• 低照度でのモーションブラー
• 強い繰り返しパターンのテクスチャ

などがある。

単一フレームの失敗の場合、アルゴリズムはその画像を保存し、次フレームで再試行する。それでも失敗した場合、現在および将来の画像を過去の画像と関連付けることができなくなる。

その場合：

• 現在のサブマップの末尾が「破損（broken）」であることを示すフラグを立てる
• 既存の地図を破棄する
• 新しい地図を初期フレームから開始する

この「broken map」フラグは再走行フェーズにおいて、アルゴリズムが停止し、次の地図セクションを探すべきであることを示す信号として用いられる。詳細は後述する。

 

3.3.2 教示パスにおけるマップのパッケージ化

地図の分割がトリガされると、現在の参照姿勢集合と特徴集合は、再走行（repeat pass）で使用できる形式にパッケージ化される。

まず、参照姿勢は最小間隔制約 ( \tau_s ) を満たすようにサブサンプリングされる。これにより経路が滑らかになり、経路追従器に適した形式となる。本論文のすべての実験では ( \tau_s = 0.5,m ) を使用している。

この処理では、参照経路として用いる姿勢のみが間引かれ、特徴点は、単一のステレオ対でしか観測されなかったものを除き、すべて地図に保持される。

フレーム間追跡は、マップ構築時が最も有利な条件である。画像間の姿勢変化は小さく、照明も安定している。そのため、教示パス中に追跡できなかった特徴は、再走行時にも観測できない可能性が高い。

---

車両参照経路の計算

サブサンプリングされた参照姿勢は、地図座標系 (F_{\rightarrow m}) におけるカメラ経路を与える。しかし、経路追従器が制御するのはカメラではなく**車両（vehicle）**である。

カメラ座標系と車両座標系の回転 (C_{c_k,v_k}) および並進 ( \rho_{c_k,v_k}^{v_k} ) を用いて、車両の参照経路を求める。

車両の参照位置は：

[
\rho_{v_k,m}^m :=
\rho_{c_k,m}^m – C_{m,c_k} C_{c_k,v_k} \rho_{c_k,v_k}^{v_k}
\tag{6}
]

車両の姿勢は：

[
C_{v_k,m} = C_{c_k,v_k}^T C_{c_k,m}
\tag{7}
]

---

地面平面の推定と射影

三次元から二次元への射影は、現在のサブマップ内の特徴点群に平面をフィッティングすることで定義される。

サブサンプリングされた特徴は外れ値除去を通過しているため、局所領域の合理的な疎再構成を表している。

各特徴 (i) の位置 (q_{i,m}^m) に対して、車両参照姿勢との最小距離 (d_i) を計算する：

[
d_i := \min_k |\rho_{v_k,m}^m – q_{i,m}^m|
]

この距離から重み (w_i) を定義する：

[
w_i =
\begin{cases}
\frac{1}{d_i + \sigma_p} & d_i \le \tau_d \
0 & \text{それ以外}
\end{cases}
\tag{8}
]

この重み付けは、ローバーが実際に通過した経路直下の局所地面平面を適切に捉えることを目的としている。

• ( \tau_d )：車両通過領域外の特徴を除外する閾値
• ( \sigma_p )：重み減衰を制御

本論文では ( \sigma_d = 0.01 )、( \tau_d = 1.5 ) を使用。

---

平面パラメータの推定

平面は単位法線ベクトル (n) とオフセット (b) で表され、

[
n^T x + b = 0
]

を満たす。

重み付き最小二乗問題：

[
J_p = \frac{1}{2} \sum_{i=1}^{N} w_i (n^T q_{i,m}^m + b)^2

• \frac{1}{2} \lambda (n^T n – 1)
  \tag{9}
  ]

ここで ( \lambda ) は単位ベクトル制約のためのラグランジュ乗数。

この最小化は固有値問題：

[
A n = -\lambda n
]

に帰着する。最小固有値に対応する固有ベクトルが (n)。

---

射影フレームの計算

法線ベクトル (n) は射影フレーム (F_{\rightarrow p}) の (xy) 平面法線を表す。

回転行列 (C_{m,p}) はオイラー角 ((\alpha,\beta,\gamma)) により

[
C_{m,p} =
\begin{bmatrix}
c_\alpha c_\beta & s_\alpha c_\beta & -s_\beta \
c_\alpha s_\beta s_\gamma – s_\alpha c_\gamma &

• s_\alpha s_\beta s_\gamma + c_\alpha c_\gamma &
  c_\beta s_\gamma \
  c_\alpha s_\beta c_\gamma + s_\alpha c_\gamma &
• s_\alpha s_\beta c_\gamma – c_\alpha s_\gamma &
  c_\beta c_\gamma
  \end{bmatrix}
  \tag{10}
  ]

制約より：

[
\beta = \sin^{-1}(-n_1)
\tag{11}
]

[
\gamma = \text{atan2}(c_\beta n_2, c_\beta n_3)
\tag{12}
]

最後の角度 ( \alpha ) は自由度が残るため、車両初期座標のx軸が射影平面のxz平面内に入るよう追加制約を与える。

参照経路は射影座標系へ変換：

[
\rho_{v_k,v_0}^m = \rho_{v_k,m}^m + C_{c_0,v_0}\rho_{c_0,v_0}^{v_0}
\tag{13}
]

[
\rho_{v_k,v_0}^p = C_{m,p}^T \rho_{v_k,v_0}^m
\tag{14}
]

[
C_{v_k,p} = C_{v_k,m} C_{m,p}
\tag{15}
]

---

サブマップ難易度スコア

再走行時に速度選択へ使用する難易度スコア (h) を定義。

姿勢変化：

[
\delta C_k = C_{c_{k-1},m} C_{c_k,m}^T
]

これを回転軸 ( \hat{a}_k ) と回転角 ( \omega_k ) に分解。

難易度：

[
h = \sqrt{ \frac{1}{M} \sum_{k=1}^{M} \omega_k^2 }
\tag{16}
]

---

保存される情報

サブマップは以下を含む：

• 射影座標系 (F_{\rightarrow p}) での車両参照経路
• 地面平面射影回転 (C_{p,m})
• 特徴集合 ({q_{i,m}^m, v_i})
• 難易度スコア (h)
• 地図開始／終了の破損フラグ

---

データサイズ

• サブマップ：500KB〜2MB
• 平均 348MB/km
• 外観ベースなら 2.9GB/km
• 全キーポイント保存なら 1.3GB/km

→ 約10倍のストレージ削減。

---

サブマップ重なり

サブマップは50%重複で構築。

古い姿勢と特徴を削除しながら、マッピングループを継続。

教示パス終了後、再走行用のマップデータベースが完成する。

 

3.4 経路再走行

再走行（repeat pass）中、ロボットはサブマップのデータベースを用いて経路を再現する。我々が実装したシステムは、カメラが教示パス時と同じ方向を向いている限り、経路上の任意の地点から開始し、どちらの方向にも経路を再走行することができる。経路追従中の方向切り替えや局所障害物検出は実装していないが、いずれも実現可能であると考えられる（Marshallら, 2008）。本節では、経路追従アルゴリズムの詳細、すなわち局所化、経路管理、および失敗処理について説明する。

---

3.4.1 再走行時の局所化

再走行中には、汎用局所化モジュールの3つの特化版が用いられる：サブマップ選択、局所化、およびVO（Visual Odometry）である。

サブマップ選択は、経路の開始時またはロボットが自己位置を見失ったときに実行される。教示パスで構築されたサブマップの1つをメモリに読み込み、特徴データベースとして使用する。特徴は追跡され、外れ値検出が行われる。十分な数の内点特徴トラック（本論文のすべての実験では9個）が得られた場合、教示フェーズで使用した目的関数（式(5)）を用いてカメラの姿勢を求める。この処理が成功すると、ローバーは経路の再走行を開始し、再走行中は局所化とVOを交互に実行する。

再走行中に局所化とVOを交互に実行することは、本システムが照明変化、シーン変化、および遮蔽に対して頑健であるための重要な戦略の1つである。本プロジェクトの初期バージョンでは、Royerら（2007）やSegvićら（2009）と類似した定式化を使用しており、ロボットの位置推定はすべて地図に対する局所化に基づいており、いかなる形式のデッドレコニングも使用していなかった。この方法は舗装路や都市環境では良好に機能したが、草地や荒れた地形でテストしたところ、照明条件が変化すると容易に失敗した。

我々の局所化モジュールがVO（純粋な相対運動推定手法）に基づいていることを踏まえ、VOと局所化を交互に切り替えるシステムを実装した。VOは困難な環境においてもローバーを経路付近に保つのに十分な精度を持ち、周期的な局所化は長距離経路を再現可能にするための大域的（トポロジカル）精度を維持する。これは、Zhang and Kleeman（2009）が大域補正の間にホイールオドメトリを用いる手法に類似している。

VOは毎フレーム処理する。しかし、現在のハードウェアでは毎フレーム局所化も同時に行うだけの計算資源がない。そのため、整数パラメータ (G) を導入し、(\mathrm{mod}(k,G)=0) のとき（すなわちGフレームごと）にのみ局所化を試みる。本研究では (G=3) を使用した。VOにはMaimone, Cheng, and Matthies（2007）によるフレーム間VO手法を用いている。

局所化はサブマップ選択と類似しているが、VOから得られるローバー位置の事前情報を利用できる点が異なる。サブマップ選択で説明した処理のみを用いる場合、システムは周期的に遠方特徴のみを用いて局所化を行うことになる。この場合、姿勢は非常に良好に推定されるが、ローバーの位置は大きくジャンプする傾向がある。同様の挙動はDiosiら（2007）でも報告されている。

この問題に対処するため、姿勢推定に用いる誤差項に事前情報項を追加する。式(5)の誤差項を (J_{vis}) とする。位置に関する事前誤差項 (J_{pos}) と姿勢に関する事前誤差項 (J_{att}) を加え、最小化する誤差項 (J) を

[
J = J_{vis} + J_{pos} + J_{att}
\tag{17}
]

とする。

VOによって推定された位置と姿勢をそれぞれ (\hat{\rho}{c_k,m}^m) および (\hat{C}{c_k,m}) とし、推定したい位置と姿勢を (\rho_{c_k,m}^m) および (C_{c_k,m}) とする。このとき、

[
J_{pos} :=
\frac{1}{2}
(\hat{\rho}{c_k,m}^m – \rho{c_k,m}^m)^T
W_{pos}
(\hat{\rho}{c_k,m}^m – \rho{c_k,m}^m)
]

となる。

(\hat{C}{c_k,m}) および (C{c_k,m}) をそれぞれヨー・ピッチ・ロールのオイラー角ベクトル (\hat{\alpha}_k) および (\alpha_k) で表すと、姿勢に対する誤差項は

[
J_{att} :=
\frac{1}{2}
(\hat{\alpha}k – \alpha_k)^T
W{att}
(\hat{\alpha}_k – \alpha_k)
]

となる。

重み行列は

[
W_{pos} := \frac{1}{\sigma_{pos}^2} I,\quad
W_{att} := \frac{1}{\sigma_{att}^2} I
\tag{18}
]

とした。ここで (I) は単位行列である。本研究のすべての実験では (\sigma_{pos} = 1.0)、(\sigma_{att} = 0.1) を使用しており、これは非常に弱い事前拘束となる。最後に、式(17)を線形化し、ガウス–ニュートン法で解く。

局所化ブロックの出力は、カメラの位置 (\rho_{c_k,m}^m) と姿勢 (C_{c_k,m}) の推定値である。式(6)、(13)、(14)を用いて、射影座標系における車両位置 (\rho_{v_k,p}^p) を計算する。射影座標系における車両姿勢 (C_{p,v_k}) は式(15)により求め、これをヨー・ピッチ・ロールのオイラー角列に分解する。この列のヨー成分が、射影座標系における車両の方位角 (\theta_k) である。

[
\rho_{v_k,p}^p := [x_k; y_k; z_k]^T
]

と定義すると、二次元ロボット姿勢 (\kappa) は

[
\kappa =
\begin{bmatrix}
x_k\
y_k\
\theta_k
\end{bmatrix}
\tag{19}
]

と表される。

この平面姿勢と射影された参照経路は、Marshallら（2008）で述べられている平面経路追従アルゴリズムの単輪モデル版（unicycle model）へ渡される。

 

3.4.2 再走行時の経路管理（Repeat Pass Route Management）

局所化モジュールの出力は、経路管理システムへと渡される。この経路管理システムは、サブマップの切り替え（map handoff）をトリガし、経路難易度に基づいてロボットの速度を設定し、さらに経路追従システムにエラーがないか監視する役割を持つ。

経路管理器は、現在の参照経路上で最も近い点を追跡する。車両が参照経路の中央に到達すると、サブマップの切り替えがトリガされる。この処理には以下の手順が含まれる：

• 次のサブマップをディスクから読み込む
• 局所化に使用する特徴データベースを更新する
• 経路追従器が使用する参照経路を更新する
• 座標変換 (F_{\rightarrow m}) から (F_{\rightarrow p}) への変換を更新する
• サブマップの難易度に基づいてロボットの速度を設定する

---

3.4.3 再走行時の失敗処理（Repeat Pass Failure Handling）

経路追従の失敗は、最後に成功した局所化以降に走行した距離を監視することで検出される。この距離が閾値 ( \tau_g ) に達すると、ローバーは停止し、システムは失敗からの回復を試みる。

回復のために、システムはオペレータに失敗を通知し、その後、トポロジカルに近傍のサブマップをサブマップ選択モードで探索し、ワイドベースラインマッチングを行う。この再初期化が成功すれば、ローバーは経路を再開する。

探索フェーズ中には、オペレータがローバーを経路上の適切な位置へ再配置することもできる（教示パス時の画像を参照して正しい位置を特定する）。いかなる失敗やローバーの再配置も、後述の実験結果において記録される。

3.3.1節で述べたように、アルゴリズムが地図の「破断（break）」に遭遇した場合、システムは現在のサブマップの終端まで走行し、停止し、次のサブマップをメモリに読み込み、局所化を試みる。これが成功すれば、アルゴリズムはローバーを再始動し、経路の再走行を継続する。これが失敗した場合、ローバーはオペレータに介入を求める信号を出す。その後、アルゴリズムはトポロジカルに近傍の地図を探索し、サブマップ選択が成功するまで試みる。オペレータはその後、ローバーを再配置するか、あるいはVOのみを用いて継続するよう指示することができる。

---

3.5 パラメータ選択（Parameter Choices）

多くのロボティクス応用と同様に、各展開において複数のパラメータを調整する必要がある。我々のティーチ・アンド・リピートシステムのパラメータは、アルゴリズム開発中に調整され、その後、本論文で報告する実験のために固定された。

前述のアルゴリズム説明では、各パラメータ選択の背後にある直感についてできる限り説明してきた。明確さのため、主要なアルゴリズムパラメータを表IIにまとめ、それぞれの機能と、その値を選択する際の直感的理由について記載している。

---

3.6 ハードウェア（Hardware）

本論文で述べた実験は、図10に示す6輪関節式ローバーを用いて実施された。ローバーのモータ制御は2つのマイクロコントローラによって行われた。車両レベルの運動指令および経路追従は、1.2GHzのPentium 4プロセッサと1GBのRAMを搭載した単一の組み込みPCによって処理された。

基本電源は3つのリチウムイオンバッテリーパックであったが、本論文の長距離実験を実施するため、車載電源はHonda製1,000W発電機によって補強された。この発電機はベースおよびすべての車載コンピュータに電力を供給した。

局所化および経路管理を実行するコンピュータは、2.4GHz Intel Core 2 Duoプロセッサ、4GB RAM、およびCUDA 1.1をサポート可能なNVIDIA GeForce 8600M GTグラフィックスカードを搭載したMacBook Proであった。

ステレオカメラは、基線長24cm、視野角70度のPoint Grey Research製Bumblebee XB3であり、地面から約1mの高さに設置され、約20度下向きに取り付けられていた。各ステレオ画像は640×480ピクセル解像度で取得された。

可能な場合には、地上真値評価のためにThales DG-16リアルタイムキネマティック（RTK）GPSユニットを2台使用した。これらのユニットは、円形誤差確率（Circular Error Probable）0.4m（データの50%が真値を中心とするこの半径の円内に入る）と評価されている。

しかしながら、無線リンクは遮蔽に対して頑健ではなかったため、長距離経路や建物付近ではリアルタイム補正を受信することができず、通常のGPSのみが利用可能であった。

ローバーは前進および後退の両方向で経路を追従することが可能であった。これにより、カメラを教示時と同じ方向に向けたまま、どちらの方向にも経路を再走行することが可能となった。

 

4. フィールド試験（FIELD TESTING）

我々は、完全なティーチ・アンド・リピートシステムの能力を評価し、局所化システムの性能を特性評価するために、複数のフィールド試験を実施した。本節では、経路追従試験の結果をまとめる。

初期試験はトロント大学航空宇宙研究所（UTIAS）で実施された。本アルゴリズムは惑星探査への応用が想定されるため、カナダ北極圏デボン島にあるHaughton-Mars Project Research Station（HMP-RS）でも試験を行った（Lee ら, 2007）。HMP-RSは極地砂漠内に位置しており、惑星アナログとして価値の高い多様な地質的特徴を備えている。このため、過去にもローバー試験に使用されてきた（Fong ら, 2007; Fong ら, 2008; Wettergreen ら, 2002; Wettergreen ら, 2005）。さらに、植生がなく、太陽高度が低く、多様な地形を持つことから、惑星探査向け視覚アルゴリズムの試験に理想的な環境である。

---

4.1 経路追従

我々のティーチ・アンド・リピートシステムは、27本の経路に対して試験され、32km以上の自律走行を達成した。本論文で報告する結果は、第3節で述べたアルゴリズムによるものである。開発初期段階の結果は含まれていない。ここで述べるすべての試験は同一のコードおよびパラメータで実施された。

各教示走行（teach pass）は図11に示す命名規則に従って命名されている。UTIASで実施された実験は「u」、デボン島で実施されたものは「d」で示される。経路の教示方法は2通りあった。「h」は人間が操縦したことを示し、「a」は自律走行による教示を示す。自律教示走行は、地形評価および経路計画アルゴリズムの試験中に記録された。地形評価アルゴリズムが完了を通知すると、記録画像から経路が教示され、ローバーは自律的にその経路を戻った。

一部の経路はカメラを前方に向けて教示され、他は後方に向けて教示された（並行実験の要件による）。しかし再走行時には、教示時と同じ方向にカメラが向くよう、ローバーは前進または後退した。

全教示・再走行の統計は付録Bに示すが、ここでは概要を述べる。

学習された経路の長さは47mから約5kmに及んだ。27回の教示走行のうち、21回は失敗なく地図構築に成功した。教示走行の失敗については5.5節で詳述する。

経路の難易度は、傾斜計で測定した車両フレームのピッチおよびロール、ならびにGPSによる相対標高変化で評価した。最も過酷な経路では、118.5mの標高変化、最大28度のピッチおよび22度のロール偏差が記録された。

27本の教示経路に対して60回の再走行が実施された。うち手動介入が必要だったのは4経路のみであった。再走行が完遂されなかったものは4回であった。再走行失敗については5.6節で詳述する。

最長の自律再走行は3.2km（dh-07-23-4963）であった。約2kmの自律走行が2回（dh-07-20-2120）、約1kmの自律走行が10回（uh-05-20-1152、uh-05-21-1170、dh-07-22-1091）行われた。

総走行距離32.919kmのうち、手動操縦はわずか0.128kmであった。これは99.6%の自律率に相当する。介入が必要だった場合、ローバーは経路上で停止し、オペレータに通知した。

---

4.2 大きな3次元運動および極端な照明変化を伴う経路

アルゴリズムの動作限界を検証するため、UTIASにおいてローバーが大きな3次元運動と極端な照明変化を経験する経路を構築した。図12は経路の俯瞰図、車両フレームのピッチおよびロール（傾斜計による測定）、および左カメラの代表画像を示す。

ローバーは屋内試験施設の高架プラットフォームから出発し、斜面を下り、2つの丘を登り、スロープを上り、屋外へ通じる狭い通路を通過した。その後、障害物コースを走破し、道路を横断し、最終的に研究室内に駐車した。

プラットフォームでは最大27度のピッチおよびロールが生じ、低照度の屋内環境から屋外へ移動した。マルチメディア拡張1（付録A）に、この経路を再走行する様子の動画を示す。

この経路は、障害物構築中（uh-07-22-0120）と完成後（uh-07-23-0120）の2回教示された。再走行はそれぞれ7回および5回行われ、すべて成功した。カメラの3次元運動にもかかわらず、全再走行が成功した。

図13は教示走行時の通路（±2m横方向拡張表示）と7回の再走行軌跡を重ねたものである。背景が青色で塗られている区間は、局所化ドロップアウトが発生した区間を示す。

屋内区間の急な丘では、著しいモーションブラーにより局所化およびVOが失敗した。経路終端では、コンピュータや椅子が移動され、シーンの外観が大きく変化していた。

この実験は、局所化とVOの相互作用を強調している。VOが失敗した場合には局所化が補正し、ローバーを正しい経路に維持する。局所化が失敗した場合には、VOが十分に正確であり、ローバーを認識可能な場所まで導く。

この実験は、デボン島でのフィールド試験前に、ティーチ・アンド・リピートシステムが3次元地形で機能することを確認するために実施された。フィールド試験では、多数の3次元経路でアルゴリズムを試験した（最も過酷な経路は表III参照）。すべての場合において、カメラの3次元運動は経路追従の制約要因とはならなかった。

 

5. 評価（EVALUATION）

本節では、我々のアルゴリズムがなぜ機能するのか、その強みと弱点は何かを明らかにするための評価を行う。局所化アルゴリズムの収束特性、および照明条件の変化に対する特性を検討する。また、局所化アルゴリズムが推定した横方向経路追従誤差をGPSで測定した値と比較し、局所化に使用されている特徴がどのようなものであるかを調べる。最後に、アルゴリズムが経験した失敗モードについて検討する。

---

5.1 収束特性（Convergence Properties）

局所化アルゴリズムの収束特性を評価するため、デボン島実験で得られた代表的地形において、三脚に取り付けたカメラを用いて単一のマップを教示した。教示パスを処理した後、カメラをマップ中央の基準位置に設置し、局所化を実行する状態に設定した。その後、この基準位置から摂動を与え、局所化が失敗するまで変位させた。

摂動は以下の4通りで与えた：

• 経路中心からの横方向変位（0.1m刻み）
• 車両フレームのヨー軸回転（5度刻み）
• ピッチ軸回転（5度刻み）
• ロール軸回転（5度刻み）

各刻み位置で200回の局所化を実行した。

図14は、横方向および角度偏差に対する平均インライヤ特徴数を示している。曲線は局所化アルゴリズムが失敗した時点で終了する。この実験は、カメラの基準位置から離れると特徴数が急激に減少することを示している。

この種の曲線はシーン依存であるが、正方向横変位で特徴数の減少が緩やかであったのは、経路右側の顕著な岩による可能性がある。

この実験から、経路中心から±1mの横方向変位、およびヨー・ピッチ・ロールすべてにおいて±20度の角度偏差まで局所化が可能であることが示された。

---

5.2 照明依存性（Lighting Dependence）

照明変化に対するアルゴリズムの特性を示すための実験も実施した。SURF特徴記述アルゴリズムは、記述ベクトルを正規化することでコントラスト変化に対応している。しかし我々の経験では、極端な照明変化下では記述子ベースのマッチングは非常に困難である。

これを示すため、短い経路を教示し、カメラを固定して30秒ごとに画像を取得し局所化を実行した。図15は、時間経過に対するインライヤ特徴数を示している。

教示パスから10時間後、局所化モジュールは十分なインライヤ特徴を見つけられなくなった。これは実験で観察された照明依存性を裏付けている。

特に入射角の低い強い照明は問題となる。同様に、曇天で教示し晴天で再走行する場合（またはその逆）も問題を引き起こす。曇天ではSURFのブロブ検出器は主に表面アルベドに基づいて特徴を検出するが、強い照明下では影の発生によりシーン構造に基づく強いコントラスト領域が形成される。その結果、それぞれ異なる特徴集合が抽出される。

---

5.3 経路追従中の局所化性能（Localization Performance during Path Following）

本節では、経路追従中の局所化性能を評価する。経路全体の再構成はグローバルに整合していない可能性があるが、各小区間では再構成誤差は小さいはずである。そのため、局所化アルゴリズムが推定した横方向経路追従誤差と、GPSで測定された値を比較することができる。

我々のGPS装置はリアルタイム補正のために基地局との見通しが必要であったため、すべての経路でRTK-GPSデータが得られたわけではない。

図16は、450m区間（経路da-07-29-0486）における横方向経路追従誤差を示す。この区間では教示パスおよび再走行パスの両方でRTK-GPSが利用可能であった。背景が網掛けされている部分は、再走行中に局所化が失敗した区間を示す。

この図は本アルゴリズムの2つの重要な特性を示している。

第一に、局所化が成功しているとき、推定された横方向誤差はGPSで測定された値と良好に一致している。差は0.2mを超えておらず、これは本GPSで識別可能な範囲内である。

第二に、アルゴリズムがグローバル局所化に失敗した場合、推定値が発散し、その後局所化が回復すると再収束することが示されている。図16では約360m地点で局所化が約15mにわたりドロップアウトしている。発散速度はVOアルゴリズムの精度に依存する。

我々は横方向誤差1.5m、局所化ドロップアウト40mにおいても回復を確認している。いずれの場合も、局所化が成功すると推定値はグローバル整合性へと引き戻され、長距離経路の忠実な再現が可能となる。

---

5.4 キーポイントおよび特徴の使用（Keypoint and Feature Usage）

本節では、局所化にどのキーポイントや特徴が使用されているかを分析する。そのため、経路uh-05-26-0202の9回の再走行データを使用した。この経路は曇天の正午に教示され、最初の7回の再走行は晴天日に午前7時45分から1時間ごとに行われた。6回目の再走行後に雲がかかり、残りの再走行は別日に行われた。多数の再走行および照明条件の変化により、この経路は特徴使用の分析に適している。

図17(a)は、マップに保存された132,781個の特徴のトラック長（観測回数）のヒストグラムである。ほとんどの特徴は2フレームのみで観測されている。その後、トラック長は急速に減少するが、少数の特徴はさらに多くのフレームで観測されている。長い尾部は図では省略されている。最長のトラック長は102フレームであった。

再走行中、どの特徴が局所化に使用されたかを記録した。図17(b)は、教示パス中のトラック長と再走行時の特徴使用頻度との関係を示す。平均値をプロットすると傾き1の強い線形関係が見られる。これは直感と一致している。すなわち、教示中に長時間観測された特徴は再走行時にも容易に検出される。

どの特徴が局所化に最も寄与するかを調べるため、画像空間での特徴観測をプロットした。図18(a)は画像上部付近への特徴集中を示している。図18(b)の典型画像と比較すると、再走行中に使用される特徴の大半がカメラから遠方、すなわち地平線付近に存在していることが明らかである。

地平線特徴はローバー姿勢補正には有効であるが、ステレオ距離精度は距離とともに低下するため、ローバー位置推定にはあまり適していない。この意味で、本アルゴリズムはグローバル整合性のある姿勢更新を伴うVOのように機能している。

これは今後の改善方向も示唆している。複数フレームで追跡された特徴のみを使用すればサブマップサイズを削減できる可能性がある。これにより特徴対応探索の計算量を削減し、より頻繁な局所化が可能になる。

しかし遠方特徴のみを使用する場合、より正確な特徴位置および共分散推定が必要となるため、教示パス中に多フレーム再構成法を用いることが示唆される。あるいは、Kaess, Ni, and Dellaert (2009) が提案した二段階推定アルゴリズムにより、姿勢推定と位置推定を分離することも可能である。

 

 

5.5 教示パスの失敗（Teach Pass Failures）

表IVに示した教示パスの失敗は、すべて画像間でのカメラの大きな変位が原因であった。時には処理の遅延により、データ記録システムが画像をドロップすることがあった。この問題は、多くの地形では大きな問題とはならなかった。特に、明確な地平線特徴や地面平面外の大きな物体が存在する場合には問題が生じにくかった。

しかし、uh-07-23-4963の長い区間のような平坦で反復的な地形では、短時間の画像欠落であっても教示失敗を引き起こした。図19は、この経路で失敗を引き起こした連続2枚の画像を示している。

教示パス失敗が発生した5経路のうち3経路では、オペレータの介入は不要であった。ローバーは単に破断したマップの終端まで走行し、次のマップを読み込み、再局所化を行い、そのまま継続した。

---

5.6 再走行パスの失敗（Repeat Pass Failures）

表Vは、すべての再走行失敗および未完了経路を示している。再走行失敗には2つの明確な原因があった。

1つ目は、自律的地形評価および経路計画アルゴリズムとの統合に起因するものであり、2つ目はシーン外観の変化であった。

本論文で述べた経路学習アルゴリズムは、方向転換を含む画像列の学習には問題がなかったが、経路が自分自身に折り返すような場合には、我々の経路追従アルゴリズムでは対応が困難であった。開発初期段階で、方向切替機能は実装しないことを決定した。

しかし、いくつかの経路を構築するために用いた自律地形評価および経路計画アルゴリズムは、袋小路から抜け出すために自身の経路を後退することがあった。このような経路の結び目（knot）に直面すると、経路追従器はロボットに大きなUターンを指示し、その結果、経路上の望ましい姿勢に対して180度反転した状態で停止してしまった。

これに対処するため、我々は地形評価走行から得られた運動推定を用いて、経路の結び目を自動的に検出し、それを構成する画像を画像列から削除する前処理ステップを開発した。この前処理は2009年6月初旬の試験（ua-06-04-∗ および ua-06-06-∗）の間に開発された。当時の失敗がこの改良につながり、その後は結び目除去処理は問題なく機能した。

その他の再走行失敗は、主に照明条件の変化によるシーン外観の変化が原因であった。ある時間帯には手動介入が必要だった（uh-05-21-1170）あるいは完了できなかった（dh-07-30-0187）経路が、照明条件が変わると自律的に成功する例もあった。これは5.2節の照明試験結果とよく一致している。

経路uh-05-21-1170は、正午の直射日光下で教示され、6回再走行されたが、夕方遅くや早朝のみ問題が発生した。

経路dh-07-30-0187は、拳大の岩のみで構成された地域にあった。この岩によって生じる複雑な影は、時間変化下ではアルゴリズムにとって困難であった。図20は、この経路の教示パス画像と、失敗した再走行および成功した再走行の画像を示している。

平坦で反復的なテクスチャの領域は、照明変化下で特に困難であった。図19で示したuh-07-23-4963の区間は、部分的に曇りで一部強い直射日光がある条件で教示され、2回の再走行はどちらも曇天で行われた。1回目は前進方向、2回目は後退方向で実施された。

両方とも同じ地形区間の両端で失敗した。図21は、1回目の再走行で停止した地点の画像と、対応する教示パス画像を示している。この画像から、ローバーは局所化なしで50m走行した後でも横方向ずれは0.5m以下であったことが明らかである。

それでも、視点はほぼ同じであったにもかかわらず、反復的なテクスチャ、異なる照明、地平線特徴の欠如、そしてシーン内の固有な3次元物体の不足が問題となった。

この時点で、ローバーを経路上に再配置するか手動操縦で通過させる予定であったが、雨が降り始めた。1時間後、雨は止みそうになかったため、防水シートをローバーにかけ、手動操縦で帰還させた。

前述の通り、2回目の再走行は反対方向から行われた。今回は図19に示した教示パス失敗地点で停止し、再局所化できなかった。我々はロボットにVOのみで継続するよう指示したが、経路上のどこでも再局所化できなかった。

教示時と同様の気象条件下でこの経路を再試験する時間は、試験スケジュール上確保できなかった。

 

6. 議論（DISCUSSION）

本アルゴリズムに対する広範なフィールド試験および評価を通じて、カメラベースの局所化およびマッピング分野全般に適用可能ないくつかの教訓を得た。

第一に最も重要な点として、ステレオカメラのみをセンサとして使用し、SURFアルゴリズムによって視覚ランドマークを検出・記述することで、非構造的な3次元地形における長距離自律ナビゲーションが可能であることを示した。

近年の研究では、スパースバンドル調整（Konoligeら, 2007; Sibleyら, 2009）や、大規模なループに対する最適化（Konolige & Agrawal, 2008; Newmanら, 2009）によって、より高精度なマッピングが可能であることが示されている。しかし、マッピング精度は本アルゴリズムの性能を制限する要因ではなかった。我々は、これらの進展は望ましいものではあるが、堅牢な長距離ナビゲーションシステムを構築する上で必須ではないと考えている。

以下では、本プロジェクトを通じて得られた主要な教訓を述べる。

---

特徴検出・記述パイプラインの限界

SURFアルゴリズムは照明変化への対応に多くの困難を抱えていた。これは特に3次元構造を持つ地形（図20の岩場や草地）で顕著であり、都市環境（コンクリートや建物付近）では問題が比較的少なかった。

SURF記述子は照明変化にある程度不変となるよう正規化されているが、影が画像内に高コントラスト領域を生み出すと、検出器は異なる点集合を返す。

画像前処理（例：Zhang & Kleeman (2009) のパッチ正規化）によって性能向上が期待できる可能性はあるが、3次元地形上の強い影は依然として問題となる。我々はSURFでこれを示したが、この結果はあらゆる画像空間ブロブ検出器に当てはまる。

現行フレームワーク内で照明問題に対処する方法として、異なる照明条件下で同じ経路を複数回学習し、再走行時に「最適な」マップ系列を動的に選択する（マッチングスコアや時間帯に基づく）ことが考えられる。しかし、これは特徴検出・記述パラダイム自体が照明変化に弱いという根本問題を解決するものではない。

---

デッドレコニングの有用性

VOと局所化を交互に使用することは、本アルゴリズムを実用的に機能させるための鍵の一つであった。これは付録Aのマルチメディア資料で明確に示されている。

VOは中程度の外観変化がある領域を乗り越える役割を担い、局所化は長距離にわたる整合性を維持し、VOの失敗を補正する。

VOは非常に有効であったが、カメラに依存しない何らかのデッドレコニングも有用である可能性がある。惑星探査ローバーのように電力および計算資源に制約がある場合、低スリップ環境ではステレオ画像間を車輪オドメトリで補完することができる。ローカルサブマップと車輪オドメトリの組み合わせは、Marshallら (2008) によって地下鉱山ナビゲーションで既に使用されている。

地上応用においては、我々は慣性計測装置（IMU）の使用を推奨する。Corkeら (2007) が述べているように、カメラとIMUは相補的なセンサである。本研究にIMUを導入していれば、教示パス失敗を回避でき、モーションブラーによるVO失敗も補償できたであろう。次の改良版では、IMUをマッピングおよび局所化フレームワークに直接組み込む予定である。

---

マップ更新の重要性

経路周囲の環境が時間とともに変化すると、本システムの性能は低下する。この問題への解決策（persistent mapping や lifelong learning と呼ばれる）は、ロボットが広くサービス用途に展開される前に解決されなければならない。

経路追従中に再マッピングを行うようにシステムを修正することは可能であるが、これは以下のような根本的課題を解決するものではない：

1. 静的要素と動的要素の識別
2. 周期的環境変化（例：日照変化や季節変化）
3. ループ閉合時に分離マップを統合する問題

我々は次世代システムが、Konolige & Bowman (2009) のように、常時オンラインでマッピングおよび局所化を行いながら、既知経路をいつでも再走行可能な構造になると考えている。

---

ループ検出の有用性

本研究の主目的ではなかったが、ループや経路ネットワークを扱う能力があれば、本アルゴリズムの応用範囲は大きく広がる。視覚SLAMにおけるループ検出は活発な研究分野である（Williamsら, 2009）。

FAB-MAP (Cummins & Newman, 2008) のような高速・高精度ループ検出技術と、幾何学的一貫性チェック（Eade & Drummond, 2008）を組み合わせれば、2つの直接的利点が得られる：

1. ループ検出信号により、現在のサブマップ選択機構を完全に置き換えることが可能となる。これにより、経路追従誤差やVO失敗に対する頑健性が向上する。
2. 接続された経路セグメントのグラフ構造を構築できる。これにより、マップ内の任意地点間で経路計画が可能となる。

現行フレームワークでも交差点でサブマップを重ねることは可能である（分岐ごとに1サブマップ）。しかし、経路グラフ上でのナビゲーションは、連続的相対表現（Meiら, 2009; Sibleyら, 2009）を用いる方がより洗練された方法で実現できる。

---

7. 結論（CONCLUSION）

本研究では、ステレオカメラのみをセンサとして用いた長距離ローバーナビゲーションのための完全なアルゴリズムを提示した。

本システムは、小さく重なり合うサブマップの系列から構成されるトポロジカル／メトリック複合マップを生成する。ローバーが経路を進むにつれて、最も近いサブマップがメモリに読み込まれる。

ローバーはVOと局所化を交互に使用し、VOは中程度の外観変化領域を通過するために使用され、局所化は長距離経路の終端でローバーが物理的に同じ場所に到達することを保証する。

本アルゴリズムは、都市環境、極端な地形、屋内から屋外への照明変化、および植生のない高北極圏の惑星アナログ環境で試験された。

総走行距離32.919kmのうち、99.6%が自律走行であった。介入が必要なすべての状況において、ローバーは停止し、オペレータに通知した。