火星に着陸したローバーに「100m先の岩を調べてこい」と指示を出したとしましょう。地球から火星までの電波の往復には最短でも約8分、最長で約48分かかります。これはつまり、ジョイスティックでリアルタイムにローバーを操縦することが物理的に不可能であることを意味します。目の前に大きな岩が現れても、地球のオペレーターがそれを知るのは数分後、回避指令が届くのはさらに数分後です。その間にローバーは岩に衝突しているかもしれません。
この問題を解決するのが自律ナビゲーションです。ローバー自身がカメラで周囲の地形を認識し、自分の位置を推定し、安全な経路を計画して走行する — つまり、人間の介入なしに「考えて走る」能力を持たせるのです。
自律ナビゲーションの技術を理解すると、以下のような幅広い分野に応用が広がります。
- 惑星探査: NASA Perseverance の AutoNav は自律走行により1日あたりの走行距離を従来の5倍に向上させました
- 自動運転車: 自動車の自動運転で使われるSLAMや経路計画は、惑星探査ローバーの技術がルーツの一つです
- 災害対応ロボット: 福島第一原発のような人間が入れない環境での自律探査
- 倉庫・工場ロボット: 未知の環境でのマッピングと自律移動
本記事の内容
- 通信遅延とローバー自律化の必要性
- ビジュアルオドメトリ（VO）による自己位置推定
- ステレオビジョンによる3D地形マッピング
- SLAM（Simultaneous Localization and Mapping）の基本原理
- 危険地形検知の仕組み
- D/Field Dアルゴリズムによる経路計画
- JPL AutoNavの概要
- Pythonによるグリッドベース SLAM と D* の実装
前提知識
この記事を読む前に、以下の知識があると理解が深まります。
- 線形代数の基礎（行列演算、座標変換）
- 確率の基礎（ベイズの定理）
- グラフ探索アルゴリズム（ダイクストラ法の概念）
なぜ自律ナビゲーションが必要か — 通信遅延の壁
火星との通信事情
地球と火星の間の電波伝搬時間は、両惑星の位置関係によって大きく変動します。光速（約 $3 \times 10^8$ m/s）で伝搬する電波であっても、惑星間距離が膨大であるため、片道の通信遅延は以下のようになります。
$$ \Delta t = \frac{d}{c} $$
ここで $d$ は地球-火星間距離、$c$ は光速です。最接近時（約 $5.5 \times 10^{10}$ m）で片道約3分、最遠時（約 $4.0 \times 10^{11}$ m）で片道約22分です。往復ではそれぞれ約6分、約44分になります。
これだけの遅延があると、地球からの「遠隔操縦」には本質的な限界があります。MER（Mars Exploration Rover）ミッションの Spirit / Opportunity では、地上チームが画像を受信し、経路を計画し、コマンドを送信するという一連の作業に1 sol（火星の1日 ≈ 24時間37分）あたり数十メートルしか走行できませんでした。
自律走行のレベル
ローバーの走行モードは、自律性のレベルによって以下のように分類できます。
レベル1: 完全遠隔操縦（Directed Driving） — 地上から具体的な移動コマンド（前進2m、右旋回15°など）を送信します。安全だが極めて遅く、1 solあたりの走行距離は数十m程度です。
レベル2: ウェイポイント追従（Guarded Driving） — 地上チームが画像から安全な経路を計画し、ウェイポイント列を送信します。ローバーは各ウェイポイント間を直線的に走行しつつ、障害物検知で停止します。
レベル3: 自律ナビゲーション（AutoNav） — ローバーが自ら地形を認識し、安全な経路を計画して走行します。地上からはゴール座標だけを指定すればよく、1 solあたり100m以上の走行が可能になります。
Perseverance ローバーでは、AutoNav の改良版により最大120m/solの自律走行を実現しています。この飛躍的な向上の鍵となるのが、ビジュアルオドメトリ、SLAM、経路計画といった技術です。
では、ローバーはどうやって「自分がどこにいるか」を知るのでしょうか。まずは自己位置推定の基盤となるビジュアルオドメトリから見ていきましょう。
ビジュアルオドメトリ（VO）— カメラ画像からの自己位置推定
車輪オドメトリの限界
ロボットの自己位置推定で最も単純な方法は、車輪の回転数から移動距離を算出する車輪オドメトリ（Wheel Odometry）です。車輪の半径を $r$、回転角度を $\theta$ とすると、移動距離は $s = r\theta$ で計算できます。
しかし火星の地表は砂地や岩場が混在する過酷な環境です。車輪がスリップすると実際の移動距離と車輪の回転量にずれが生じ、推定位置に累積誤差が蓄積されます。Spirit ローバーは柔らかい砂地でスリップ率が90%を超えることもあり、車輪オドメトリだけでは正確な位置推定が不可能でした。
ビジュアルオドメトリの原理
ビジュアルオドメトリ（Visual Odometry, VO）は、連続するカメラ画像のペアから自己運動（ego-motion）を推定する技術です。直感的には「景色の動き方から自分の動きを逆算する」方法です。
電車に乗っているとき、窓の外の景色が後ろに流れていくのを見て「自分が前に進んでいる」と分かりますよね。VOはこれと同じ原理をカメラ画像で実行します。
VOの処理フローは以下の通りです。
ステップ1: 特徴点検出 — 画像中の特徴的な点（コーナー、エッジなど）を検出します。火星の地表では岩のエッジや地表のテクスチャが特徴点になります。代表的なアルゴリズムとしてHarrisコーナー検出やSIFT/SURFがあります。
ステップ2: 特徴点マッチング — 時刻 $t$ と $t+1$ の画像で対応する特徴点のペアを見つけます。特徴量記述子（descriptor）の類似度に基づいてマッチングを行います。
ステップ3: 運動推定 — 対応する特徴点の座標変化から、カメラ（= ローバー）の移動量と回転量を推定します。
ステレオカメラの場合、時刻 $t$ の左右画像から3D点群 $\{P_i^{(t)}\}$ を復元し、時刻 $t+1$ でも同様に $\{P_i^{(t+1)}\}$ を得ます。2つの点群の間の剛体変換 $(\bm{R}, \bm{t})$ を最小二乗法で求めることで、ローバーの移動を推定します。
$$ \bm{P}_i^{(t+1)} = \bm{R} \bm{P}_i^{(t)} + \bm{t} $$
ここで $\bm{R}$ は $3 \times 3$ の回転行列、$\bm{t}$ は $3 \times 1$ の並進ベクトルです。
誤差の最小化
実際のマッチングにはノイズや外れ値（outlier）が含まれるため、全ての対応点が正確に剛体変換に従うわけではありません。そこでRANSAC（Random Sample Consensus）を用いて外れ値を除去した上で、残った inlier に対して最小二乗法を適用します。
誤差関数は以下のように定義されます。
$$ E(\bm{R}, \bm{t}) = \sum_{i \in \text{inliers}} \| \bm{P}_i^{(t+1)} – (\bm{R} \bm{P}_i^{(t)} + \bm{t}) \|^2 $$
この $E$ を最小化する $(\bm{R}, \bm{t})$ が、1ステップ分のローバーの動きの推定値です。各ステップの推定を累積すると、全体の軌跡が得られます。
$$ \bm{T}_{0 \to N} = \bm{T}_{N-1 \to N} \cdot \bm{T}_{N-2 \to N-1} \cdots \bm{T}_{0 \to 1} $$
ただし $\bm{T}_{k \to k+1}$ は同次変換行列 $\begin{pmatrix} \bm{R} & \bm{t} \\ \bm{0}^T & 1 \end{pmatrix}$ です。
VOは車輪オドメトリと比較してスリップの影響を受けにくく、MERミッションでは位置精度を約1%以内（走行距離100mあたり誤差1m以内）に抑えることに成功しました。
VOで自分の位置が分かるようになりました。しかし位置が分かるだけでは不十分です。安全に走行するためには、周囲の地形がどうなっているかを知る必要があります。次にステレオビジョンによる3D地形マッピングを見ていきましょう。
ステレオビジョンによる地形マッピング
ステレオビジョンの原理
人間が両目で奥行きを知覚できるように、ローバーも2台のカメラ（ステレオカメラ）を使って3次元の地形情報を取得します。
ステレオビジョンの基本原理は三角測量です。左右のカメラの間隔（ベースライン）を $b$、焦点距離を $f$ とします。3D空間の点 $P$ が左カメラでは画素位置 $x_L$、右カメラでは $x_R$ に投影されたとき、視差（disparity）$d$ は以下のように定義されます。
$$ d = x_L – x_R $$
この視差から、点 $P$ までの奥行き $Z$ は以下の式で求められます。
$$ \begin{equation} Z = \frac{f \cdot b}{d} \end{equation} $$
この式は、視差 $d$ が大きいほど（= 左右の画像で対象物の位置が大きくずれるほど）対象物が近いことを表しています。手を顔の前に出して片目ずつ交互に見ると、近い手ほど位置が大きくずれて見えますよね。それと同じ原理です。
視差から3D点群へ
視差 $d$ と画像座標 $(x_L, y_L)$ から、3D座標 $(X, Y, Z)$ を復元します。
$$ X = \frac{(x_L – c_x) \cdot Z}{f}, \quad Y = \frac{(y_L – c_y) \cdot Z}{f}, \quad Z = \frac{f \cdot b}{d} $$
ここで $(c_x, c_y)$ は画像の主点（光学中心の画素座標）です。画像の全画素についてこの計算を行うと、周囲の地形の3D点群（point cloud）が得られます。
DEMの生成
得られた3D点群を地表面のグリッドに射影し、各グリッドセルの高さの統計量（平均値、最大値、標準偏差など）を計算すると、DEM（Digital Elevation Model: 数値標高モデル）が生成されます。
DEMは2次元のグリッドマップであり、各セルに「その場所の地表面の高さ」が格納されています。これが後述する危険地形検知や経路計画の入力データになります。
典型的なローバーのDEMの解像度はセルあたり約10〜20cmで、ローバーの前方約10〜20mの範囲をカバーします。Perseverance の NavCam（ナビゲーションカメラ）は、約30m先まで地形を取得できます。
ステレオビジョンにより、ローバーは「目の前の地形がどうなっているか」を3次元的に把握できるようになりました。しかし、毎回の観測は局所的であり、走行しながら全体のマップを構築し、同時に自分の位置も修正していく必要があります。これがSLAMという問題です。
SLAM — 同時自己位置推定と地図構築
SLAMとは何か
部屋の明かりを消した状態で、手探りで家具の配置を把握しながら自分の位置も把握しなければならない状況を想像してください。「地図がなければ自分の位置が分からないが、自分の位置が分からなければ地図も作れない」— この鶏と卵の問題を同時に解くのがSLAM（Simultaneous Localization and Mapping）です。
SLAMは以下の2つのタスクを同時に行います。
- Localization（自己位置推定）: 構築中のマップ上で、ローバーが今どこにいるかを推定する
- Mapping（地図構築）: センサ情報を使って周囲環境のマップを構築・更新する
SLAMの確率的定式化
SLAMの問題を確率論の枠組みで記述しましょう。時刻 $t$ において以下の変数を考えます。
- $\bm{x}_t$: ローバーの姿勢（位置 + 向き）
- $\bm{u}_t$: 制御入力（移動コマンド）
- $\bm{z}_t$: センサ観測（ステレオカメラの画像から得た地形情報）
- $\bm{m}$: マップ（全てのランドマークの位置の集合）
SLAMは、制御入力と観測の履歴が与えられたとき、ローバーの姿勢とマップの同時事後確率を求める問題です。
$$ \begin{equation} p(\bm{x}_{0:t}, \bm{m} \mid \bm{z}_{1:t}, \bm{u}_{1:t}) \end{equation} $$
この事後確率をベイズの定理を用いて逐次的に更新していくのがSLAMの基本的な枠組みです。
予測ステップと更新ステップ
SLAMの逐次推定は、以下の2ステップを繰り返します。
予測ステップ: 制御入力 $\bm{u}_t$ に基づいて、ローバーの姿勢の事前分布を予測します。
$$ p(\bm{x}_t \mid \bm{x}_{t-1}, \bm{u}_t) $$
これは運動モデル（motion model）と呼ばれ、「コマンド $\bm{u}_t$ を実行したらどこに移動するか」を確率的に表現します。車輪のスリップや地形の影響を不確かさとしてモデル化します。
更新ステップ: 新しい観測 $\bm{z}_t$ を得たとき、ベイズの定理で事後分布を更新します。
$$ p(\bm{x}_t, \bm{m} \mid \bm{z}_{1:t}, \bm{u}_{1:t}) \propto p(\bm{z}_t \mid \bm{x}_t, \bm{m}) \cdot p(\bm{x}_t, \bm{m} \mid \bm{z}_{1:t-1}, \bm{u}_{1:t}) $$
$p(\bm{z}_t \mid \bm{x}_t, \bm{m})$ は観測モデル（observation model）であり、「姿勢 $\bm{x}_t$ でマップ $\bm{m}$ の環境を見たとき、どのような観測が得られるか」を記述します。
占有格子マップ（Occupancy Grid Map）
惑星探査ローバーでは、マップ表現として占有格子マップがよく使われます。環境を2次元のグリッドに分割し、各セルに「障害物が存在する確率」を格納します。
セル $(i, j)$ の占有確率を $p(m_{ij} = 1)$ とします。ベイズの定理を用いて、新たな観測 $\bm{z}_t$ を得るたびに各セルの占有確率を更新します。
計算の便宜上、確率の代わりに対数オッズ（log-odds）を使います。
$$ l_{ij} = \log \frac{p(m_{ij} = 1)}{p(m_{ij} = 0)} $$
対数オッズの更新は単純な加算で行えるという大きな利点があります。
$$ \begin{equation} l_{ij}^{(t)} = l_{ij}^{(t-1)} + \log \frac{p(m_{ij} \mid \bm{z}_t, \bm{x}_t)}{1 – p(m_{ij} \mid \bm{z}_t, \bm{x}_t)} – l_0 \end{equation} $$
ここで $l_0 = \log \frac{p(m_{ij} = 1)}{p(m_{ij} = 0)}$ は事前確率の対数オッズ（通常0、つまり事前確率0.5）です。対数オッズから確率への変換は以下の通りです。
$$ p(m_{ij} = 1) = 1 – \frac{1}{1 + \exp(l_{ij})} $$
この更新式の意味を直感的に理解しましょう。あるセルが「障害物あり」と観測されるたびに $l_{ij}$ が増加し、「障害物なし」と観測されるたびに減少します。何度も「障害物あり」と観測されたセルは高い占有確率を持ち、それが本当に障害物であるという確信が強まります。
ローバーSLAMの特殊性
惑星探査ローバーのSLAMには、地上の自動運転車とは異なるいくつかの特徴があります。
GPSが使えない: 火星にはGPS衛星がないため、絶対座標の取得には太陽方位や地上からの電波測距（UHF/X帯）を使います。ただしこれらは精度が粗く、SLAMでの精密な位置推定が不可欠です。
計算資源の制約: ローバーの搭載コンピュータは放射線耐性が求められるため、性能は地上のコンピュータよりはるかに低いです。Perseverance の搭載プロセッサは Qualcomm のRAD750（200MHz）で、スマートフォンの数百分の一の性能です。アルゴリズムは計算効率が極めて重要です。
動的障害物が少ない: 火星には風による砂の移動を除いて動的な障害物がほぼなく、マップが時間的に安定しています。これはSLAMにとって有利な条件です。
SLAMによって地形マップが構築できるようになりました。次の問題は「マップ上のどこが安全で、どこが危険か」を判定することです。危険地形検知の仕組みを見ていきましょう。
危険地形検知 — 岩・斜面・砂地の識別
ハザードの種類
ローバーにとっての「危険地形」は、主に以下の3カテゴリに分類されます。
正の障害物（Positive Obstacles）: 岩や突起など、ローバーの車体より高い障害物です。衝突を避ける必要があります。DEMの高さデータから直接検出できます。
負の障害物（Negative Obstacles）: 崖、クレーター、深い溝などです。ローバーが落下する危険があります。DEMの急激な高さの低下や、ステレオマッチングが失敗する領域（奥が見えない = 崖の可能性）として検出します。
スリップ危険領域: 急斜面や柔らかい砂地など、走行はできるものの車輪のスリップにより位置推定精度が低下したり、スタックしたりする危険がある領域です。
幾何学的ハザード検知
DEMからハザードを検知する基本的な手法は、局所的な幾何学的特徴量の計算です。
ステップ（段差）: 隣接するDEMセル間の高さの差が閾値を超えるかどうかで判定します。
$$ \text{step}_{ij} = \max_{(i’, j’) \in \mathcal{N}(i,j)} |h_{ij} – h_{i’j’}| $$
ここで $\mathcal{N}(i,j)$ はセル $(i,j)$ の近傍、$h_{ij}$ はセルの高さです。
傾斜角（Slope）: DEMの勾配から各セルの地表面の傾斜角を計算します。
勾配ベクトルを $\nabla h = \left(\frac{\partial h}{\partial x}, \frac{\partial h}{\partial y}\right)$ とすると、傾斜角 $\alpha$ は以下で求まります。
$$ \alpha = \arctan\left(\|\nabla h\|\right) = \arctan\left(\sqrt{\left(\frac{\partial h}{\partial x}\right)^2 + \left(\frac{\partial h}{\partial y}\right)^2}\right) $$
MER/MSL ローバーでは傾斜角の閾値として約25°が使われ、これを超える領域は「走行不可」と判定されます。
粗さ（Roughness）: DEMの局所的な高さのばらつきを標準偏差で定量化します。
$$ \sigma_{ij} = \sqrt{\frac{1}{|\mathcal{W}|} \sum_{(i’,j’) \in \mathcal{W}(i,j)} (h_{i’j’} – \bar{h}_{\mathcal{W}})^2} $$
ここで $\mathcal{W}(i,j)$ はセル $(i,j)$ を中心とする窓領域、$\bar{h}_{\mathcal{W}}$ はその窓内の平均高さです。粗さが大きい領域は岩場であり、走行に適しません。
グッドネスマップ
各セルに対して、ステップ、傾斜角、粗さなどの特徴量を組み合わせて走行適性値（goodness）を計算します。これをグッドネスマップ（goodness map）と呼びます。
$$ \begin{equation} g_{ij} = w_1 \cdot f_{\text{step}}(s_{ij}) + w_2 \cdot f_{\text{slope}}(\alpha_{ij}) + w_3 \cdot f_{\text{rough}}(\sigma_{ij}) \end{equation} $$
ここで $f_{\text{step}}, f_{\text{slope}}, f_{\text{rough}}$ は各特徴量を $[0, 1]$ のコスト値に変換する関数（例: シグモイド関数）、$w_1, w_2, w_3$ は重み係数です。$g_{ij}$ が大きいほど走行コストが高い（= 危険）ことを意味します。
このグッドネスマップが経路計画アルゴリズムのコストマップとして使われます。つまり、経路計画アルゴリズムは「走行コストが最小となる経路」を探索することで、自然に危険地形を避ける経路を見つけることができます。
地形のどこが安全で、どこが危険かが分かりました。次は、このコストマップ上でスタートからゴールまでの最適な経路をどう見つけるか — 経路計画アルゴリズムの出番です。
D / Field D アルゴリズムによる経路計画
なぜ D* なのか
経路計画で最も有名なアルゴリズムはダイクストラ法や A* ですが、これらはマップが完全に既知であることを前提としています。しかしローバーの環境では、走行しながら新しい地形情報が次々と得られ、マップが更新されます。マップが更新されるたびにゼロから経路を計算し直すのは非効率です。
D（Dynamic A）は、マップの変化にインクリメンタルに（差分的に）対応できる経路計画アルゴリズムです。最初に計算した経路に対して、新しい障害物が見つかった場合はその周辺の経路だけを効率的に再計算します。
D* Lite の基本原理
ここでは、D の改良版でありJPLのローバーにも実装されている D Lite の原理を解説します。D Lite は A に似た最短経路アルゴリズムですが、以下の2つの特徴があります。
逆方向探索: 通常の A がスタートからゴールへ探索するのに対し、D Lite はゴールからスタートへ向かって探索します。これにより、ローバーが移動してスタート位置が変わっても、ゴールからの探索結果の大部分を再利用できます。
インクリメンタル再計画: マップが変化したとき、影響を受けるノードだけを再計算します。全体を再計算するよりはるかに高速です。
D* Lite では、各ノード $s$ に対して2つの値を管理します。
- $g(s)$: ゴールからノード $s$ までの最短コスト推定値
- $rhs(s)$: 隣接ノードの情報から計算した $g(s)$ の1ステップ先読み値
$rhs(s)$ は以下のように定義されます。
$$ rhs(s) = \begin{cases} 0 & \text{if } s = s_{\text{goal}} \\ \min_{s’ \in \text{Succ}(s)} \left( c(s, s’) + g(s’) \right) & \text{otherwise} \end{cases} $$
ここで $c(s, s’)$ はノード $s$ から $s’$ への移動コスト（グッドネスマップのコスト）、$\text{Succ}(s)$ は $s$ の隣接ノードの集合です。
ノードは以下の2状態をとります。
- 整合（Consistent）: $g(s) = rhs(s)$ — このノードの最短経路推定は最新
- 不整合（Inconsistent）: $g(s) \neq rhs(s)$ — 情報が更新されて再計算が必要
不整合なノードを優先度付きキューに入れ、優先度の高い順に整合状態に戻していくのが D* Lite の基本的な処理です。
優先度キーの計算
D* Lite ではノードの優先度を以下の2要素のタプルで定義します。
$$ \text{key}(s) = \left[ \min(g(s), rhs(s)) + h(s_{\text{start}}, s), \; \min(g(s), rhs(s)) \right] $$
ここで $h(s_{\text{start}}, s)$ はスタートからノード $s$ へのヒューリスティック距離（例: ユークリッド距離）です。キーが小さいノードから優先的に処理されます。
Field D* — 任意方向への補間
標準的な D Lite はグリッド上の8方向（上下左右 + 斜め）にしか移動できません。しかし実際のローバーは任意の方向に移動可能です。Field D** はセル間のコストを線形補間することで、グリッドに縛られない滑らかな経路を生成します。
セルの頂点 $s_1$, $s_2$ の間を補間パラメータ $t \in [0, 1]$ で連続的に移動するコストは以下のように計算します。
$$ \text{cost}(s, t) = c(s, s_1 + t(s_2 – s_1)) + (1-t) \cdot g(s_1) + t \cdot g(s_2) $$
この $t$ を最適化することで、グリッド方向に縛られない「斜め移動」の最適角度を求めます。Perseverance ローバーの AutoNav では Field D* が採用されており、より自然で効率的な経路を生成しています。
これで経路計画の理論が揃いました。では、これらの技術を統合した JPL の AutoNav システムの全体像を見てみましょう。
JPL AutoNav — Perseverance の自律走行システム
AutoNav の概要
AutoNav（Autonomous Navigation）は、JPL（ジェット推進研究所）が開発した惑星探査ローバー向けの自律走行システムです。MER（Spirit/Opportunity）で初めて実装され、MSL（Curiosity）、Mars 2020（Perseverance）と世代を重ねるごとに改良されてきました。
AutoNav の処理パイプラインは以下の通りです。
1. ステレオビジョン: NavCam で前方の地形を撮影し、ステレオマッチングで3D点群を生成します。
2. DEM生成: 3D点群を地表面グリッドに射影し、数値標高モデルを構築します。
3. ハザード検知: DEMから傾斜角、段差、粗さを計算し、グッドネスマップ（走行適性マップ）を生成します。
4. 経路計画: Field D* でグッドネスマップ上の最適経路を計算します。
5. 経路追従: 計画された経路に沿ってローバーを制御します。
6. ビジュアルオドメトリ: 移動後にVOで位置を更新し、次のステップへ。
この一連の処理を1ステップ（約1〜2m走行するごと）に繰り返すことで、ローバーは自律的に走行を続けます。
Perseverance での改良点
Perseverance の AutoNav は従来のシステムから大幅に改良されています。
移動中の撮像（Thinking While Driving）: Curiosity までは「停止 → 撮像 → 計画 → 移動」を繰り返していましたが、Perseverance では移動しながら次の画像を撮影・処理できるようになりました。これにより、実効走行速度が約2倍に向上しています。
拡張されたナビゲーション領域: 改良された NavCam により、より遠方（約30m先）の地形を認識でき、1ステップの走行距離が伸びました。
ENav（Enhanced Navigation）: VFH（Vector Field Histogram）ベースの高速障害物回避を統合し、小さな障害物にも柔軟に対応します。
自律走行の限界と人間の役割
AutoNav は強力な自律走行システムですが、全てを自動で行うわけではありません。
- ゴール設定: どこに向かうかは地上の科学チームが決定します
- 科学的判断: 面白い岩を見つけて分析するかどうかは人間が判断します
- 長距離計画: 大まかなルートは地上チームが衛星画像（HiRISE）から計画します
- 緊急停止: 想定外の状況では安全に停止し、地上からの指示を待ちます
つまり、AutoNav は「戦術レベル」の自律性を担い、「戦略レベル」の判断は人間が行うという役割分担です。
以上で自律ナビゲーションの理論的な全体像をカバーしました。ここからは、Pythonを使ってSLAMとD*アルゴリズムの核心部分を実装し、理論の理解を実践で深めていきましょう。
Pythonで実装する: 占有格子SLAM
占有格子マップの更新
まず、SLAMの核心部分である占有格子マップの逐次更新を実装します。ローバーがセンサで周囲を観測し、対数オッズを使ってマップを更新していく過程を可視化します。
import numpy as np
import matplotlib.pyplot as plt
# --- 占有格子マップの逐次更新シミュレーション ---
# マップサイズ（50x50グリッド）
map_size = 50
# 対数オッズマップ（初期値0 = 占有確率0.5）
log_odds_map = np.zeros((map_size, map_size))
# 対数オッズの上下限（数値安定性のため）
L_MAX = 5.0
L_MIN = -5.0
# センサモデルのパラメータ
l_occ = 0.85 # 障害物ありと観測したときの対数オッズ増分
l_free = -0.40 # 障害物なしと観測したときの対数オッズ減分
def log_odds_to_prob(l):
"""対数オッズを確率に変換"""
return 1.0 - 1.0 / (1.0 + np.exp(l))
def bresenham_line(x0, y0, x1, y1):
"""ブレゼンハムのアルゴリズムで直線上のセルを列挙"""
cells = []
dx = abs(x1 - x0)
dy = abs(y1 - y0)
sx = 1 if x0 < x1 else -1
sy = 1 if y0 < y1 else -1
err = dx - dy
while True:
cells.append((x0, y0))
if x0 == x1 and y0 == y1:
break
e2 = 2 * err
if e2 > -dy:
err -= dy
x0 += sx
if e2 < dx:
err += dx
y0 += sy
return cells
このコードでは、占有格子マップを対数オッズ表現で管理しています。bresenham_line 関数は、ローバーの位置からセンサの検知点まで直線を引き、その経路上のセルを列挙するために使います。センサのビームが通過したセルは「障害物なし」、ビームが到達した先のセルは「障害物あり」として更新します。
次に、仮想的な障害物を配置し、ローバーが複数回観測を行ってマップを構築する過程をシミュレーションします。
# 真の障害物マップ（シミュレーション用）
true_map = np.zeros((map_size, map_size))
# 壁を配置
true_map[10:12, 10:40] = 1 # 横壁
true_map[10:35, 38:40] = 1 # 縦壁
true_map[30:35, 15:25] = 1 # 岩（ブロック）
# 円形クレーター
for i in range(map_size):
for j in range(map_size):
if (i - 40)**2 + (j - 15)**2 < 25:
true_map[i, j] = 1
def simulate_lidar(robot_pos, true_map, n_beams=72, max_range=15):
"""簡易LiDARセンサをシミュレーション"""
rx, ry = robot_pos
angles = np.linspace(0, 2 * np.pi, n_beams, endpoint=False)
observations = []
for angle in angles:
for r in range(1, max_range + 1):
bx = int(rx + r * np.cos(angle))
by = int(ry + r * np.sin(angle))
if bx < 0 or bx >= map_size or by < 0 or by >= map_size:
break
if true_map[bx, by] == 1:
observations.append((rx, ry, bx, by, True))
break
else:
bx = int(rx + max_range * np.cos(angle))
by = int(ry + max_range * np.sin(angle))
bx = np.clip(bx, 0, map_size - 1)
by = np.clip(by, 0, map_size - 1)
observations.append((rx, ry, bx, by, False))
return observations
def update_map(log_odds_map, observations):
"""観測に基づいてマップを更新"""
for (rx, ry, bx, by, hit) in observations:
cells = bresenham_line(rx, ry, bx, by)
# ビーム通過セルは free
for (cx, cy) in cells[:-1]:
log_odds_map[cx, cy] += l_free
# 終端セル
if hit:
log_odds_map[bx, by] += l_occ
else:
log_odds_map[bx, by] += l_free
# クリッピング
log_odds_map[:] = np.clip(log_odds_map, L_MIN, L_MAX)
return log_odds_map
ここでは、レーザーセンサ（LiDAR）の簡易シミュレータを実装しています。実際のローバーはステレオカメラを使いますが、占有格子マップ更新の原理を理解するためにLiDARモデルを使います。各ビームが障害物に当たるかどうかをレイキャスティングで判定し、ブレゼンハムの直線アルゴリズムでビーム経路上のセルを更新します。
# ローバーの移動経路（ウェイポイント）
waypoints = [(5, 25), (15, 25), (20, 10), (25, 30), (35, 8), (45, 25)]
fig, axes = plt.subplots(2, 3, figsize=(15, 10))
axes = axes.flatten()
for idx, (wx, wy) in enumerate(waypoints):
obs = simulate_lidar((wx, wy), true_map)
update_map(log_odds_map, obs)
prob_map = log_odds_to_prob(log_odds_map)
ax = axes[idx]
ax.imshow(prob_map.T, origin='lower', cmap='RdYlGn_r',
vmin=0, vmax=1)
# ローバー位置をプロット
for k in range(idx + 1):
color = 'blue' if k < idx else 'red'
size = 30 if k < idx else 80
ax.scatter(waypoints[k][0], waypoints[k][1],
c=color, s=size, marker='o', zorder=5)
ax.set_title(f'Step {idx+1}: pos=({wx},{wy})', fontsize=10)
ax.set_xlim(0, map_size)
ax.set_ylim(0, map_size)
ax.set_aspect('equal')
plt.suptitle('Occupancy Grid SLAM — Incremental Map Building',
fontsize=14, fontweight='bold')
plt.tight_layout()
plt.savefig('occupancy_grid_slam.png', dpi=150, bbox_inches='tight')
plt.show()
上のグラフでは、ローバーが6つのウェイポイントを順に移動しながら占有格子マップが段階的に構築されていく過程が可視化されています。緑色のセルは「障害物なし」の確信が高い領域、赤色のセルは「障害物あり」の確信が高い領域、黄色は未確定の領域です。
ステップが進むにつれて、以下の特徴が読み取れます。
- 観測を重ねるほどマップの確信度が上がる — 複数回観測されたセルほど、色が濃く（緑 or 赤が濃く）なります。これは対数オッズの加算的な更新が「証拠の蓄積」として機能していることを示しています。
- 未観測領域は黄色（不確実）のまま残る — センサが届かない領域は事前確率（0.5）のままであり、これは「知らない」ことを正しく表現できています。
- 壁や岩の形状が徐々に浮かび上がる — 個々の観測は局所的ですが、異なる位置からの観測を統合することで、障害物の全体像が把握できるようになります。
これが SLAMの「地図構築」の本質です。では次に、構築されたマップ上での経路計画を実装しましょう。
Pythonで実装する: D* Lite 経路計画
D* Lite の実装
構築されたコストマップ上で、D* Lite アルゴリズムによる経路計画を実装します。マップの変化にインクリメンタルに対応できる様子も確認します。
import numpy as np
import matplotlib.pyplot as plt
from heapq import heappush, heappop
class DStarLite:
"""D* Lite 経路計画アルゴリズム"""
def __init__(self, grid, start, goal):
self.grid = grid.copy().astype(float)
self.rows, self.cols = grid.shape
self.start = start
self.goal = goal
self.km = 0 # キー修正値
self.INF = float('inf')
# g値とrhs値の初期化
self.g = np.full((self.rows, self.cols), self.INF)
self.rhs = np.full((self.rows, self.cols), self.INF)
self.rhs[goal] = 0
# 優先度キー
self.open_list = []
self.open_set = set()
self._insert(goal)
def _heuristic(self, s):
"""ヒューリスティック（ユークリッド距離）"""
return np.sqrt((s[0] - self.start[0])**2 +
(s[1] - self.start[1])**2)
def _key(self, s):
"""優先度キーの計算"""
g_val = self.g[s]
rhs_val = self.rhs[s]
min_val = min(g_val, rhs_val)
return (min_val + self._heuristic(s) + self.km, min_val)
def _insert(self, s):
"""ノードをオープンリストに挿入"""
key = self._key(s)
heappush(self.open_list, (key, s))
self.open_set.add(s)
def _neighbors(self, s):
"""8近傍のノードを返す"""
dirs = [(-1, 0), (1, 0), (0, -1), (0, 1),
(-1, -1), (-1, 1), (1, -1), (1, 1)]
result = []
for dr, dc in dirs:
nr, nc = s[0] + dr, s[1] + dc
if 0 <= nr < self.rows and 0 <= nc < self.cols:
result.append((nr, nc))
return result
def _cost(self, s1, s2):
"""ノード間の移動コスト"""
if self.grid[s2] >= 0.7: # 障害物
return self.INF
base = np.sqrt((s1[0]-s2[0])**2 + (s1[1]-s2[1])**2)
# コストマップの値を反映
terrain_cost = 1.0 + 5.0 * self.grid[s2]
return base * terrain_cost
D* Lite クラスの前半部分を定義しました。各ノードに $g$ 値と $rhs$ 値を持たせ、ゴールの $rhs$ を0で初期化しています。コスト関数では、占有確率が0.7以上のセルを通過不可（コスト無限大）とし、それ以外のセルには地形コストを反映させています。
def compute_shortest_path(self):
"""最短経路を計算（メインループ）"""
iterations = 0
max_iter = self.rows * self.cols * 2
while self.open_list and iterations < max_iter:
# 最小キーを取得
top_key = self.open_list[0][0]
start_key = self._key(self.start)
if top_key >= start_key and \
self.rhs[self.start] == self.g[self.start]:
break
key, s = heappop(self.open_list)
self.open_set.discard(s)
iterations += 1
current_key = self._key(s)
if key < current_key:
# キーが古い → 再挿入
self._insert(s)
elif self.g[s] > self.rhs[s]:
# 過大評価 → 整合化
self.g[s] = self.rhs[s]
for nb in self._neighbors(s):
self._update_vertex(nb)
else:
# 過小評価 → リセット
self.g[s] = self.INF
self._update_vertex(s)
for nb in self._neighbors(s):
self._update_vertex(nb)
def _update_vertex(self, s):
"""ノードのrhs値を更新"""
if s != self.goal:
min_rhs = self.INF
for nb in self._neighbors(s):
c = self._cost(s, nb)
if c + self.g[nb] < min_rhs:
min_rhs = c + self.g[nb]
self.rhs[s] = min_rhs
if s in self.open_set:
self.open_set.discard(s)
if self.g[s] != self.rhs[s]:
self._insert(s)
def extract_path(self):
"""計算済みのg値からスタートからゴールへの経路を抽出"""
path = [self.start]
current = self.start
max_steps = self.rows * self.cols
for _ in range(max_steps):
if current == self.goal:
break
best_next = None
best_cost = self.INF
for nb in self._neighbors(current):
c = self._cost(current, nb) + self.g[nb]
if c < best_cost:
best_cost = c
best_next = nb
if best_next is None or best_cost >= self.INF:
break
path.append(best_next)
current = best_next
return path
def update_grid(self, changed_cells):
"""マップ変更時のインクリメンタル再計画"""
for (r, c, new_val) in changed_cells:
self.grid[r, c] = new_val
# 変更されたセルの近傍を更新
for (r, c, _) in changed_cells:
s = (r, c)
self._update_vertex(s)
for nb in self._neighbors(s):
self._update_vertex(nb)
self.compute_shortest_path()
compute_shortest_path が D Lite のメインループです。不整合なノードをキーの小さい順に処理し、$g$ 値と $rhs$ 値を整合させていきます。update_grid メソッドは、マップに変更があった場合に呼び出され、影響のあるノードだけを効率的に再計算します。これがD Liteの「インクリメンタル再計画」の仕組みです。
では、このD* Liteを使って障害物のあるグリッド上で経路を計画し、さらにマップが更新されたときの再計画の様子を可視化してみましょう。
# グリッドマップの作成（障害物付き）
grid_size = 40
grid = np.zeros((grid_size, grid_size))
# 障害物を配置
grid[8:12, 5:30] = 1.0 # 壁1
grid[20:24, 10:35] = 1.0 # 壁2
grid[14:18, 20:24] = 1.0 # 岩
# 緩やかな斜面（コスト中程度）
for i in range(30, 36):
for j in range(5, 20):
grid[i, j] = 0.3
start = (2, 2)
goal = (37, 37)
# D* Lite で経路計画
planner = DStarLite(grid, start, goal)
planner.compute_shortest_path()
path1 = planner.extract_path()
fig, axes = plt.subplots(1, 3, figsize=(18, 6))
# (1) 初期経路
ax = axes[0]
ax.imshow(grid.T, origin='lower', cmap='RdYlGn_r',
vmin=0, vmax=1, alpha=0.7)
if path1:
pr, pc = zip(*path1)
ax.plot(pr, pc, 'b-', linewidth=2, label='Path')
ax.plot(*start, 'go', markersize=12, label='Start')
ax.plot(*goal, 'r*', markersize=15, label='Goal')
ax.set_title('(a) Initial Path', fontsize=12)
ax.legend(loc='upper left', fontsize=9)
ax.set_aspect('equal')
# (2) 新しい障害物が出現 → 再計画
new_obstacles = []
for i in range(12, 20):
for j in range(26, 30):
new_obstacles.append((i, j, 1.0))
planner.update_grid(new_obstacles)
path2 = planner.extract_path()
grid2 = planner.grid.copy()
ax = axes[1]
ax.imshow(grid2.T, origin='lower', cmap='RdYlGn_r',
vmin=0, vmax=1, alpha=0.7)
if path1:
ax.plot(pr, pc, 'b--', linewidth=1, alpha=0.4, label='Old path')
if path2:
pr2, pc2 = zip(*path2)
ax.plot(pr2, pc2, 'b-', linewidth=2, label='Replanned')
# 新障害物を強調
for (r, c, _) in new_obstacles:
ax.plot(r, c, 'rx', markersize=3)
ax.plot(*start, 'go', markersize=12)
ax.plot(*goal, 'r*', markersize=15)
ax.set_title('(b) After New Obstacle (Replan)', fontsize=12)
ax.legend(loc='upper left', fontsize=9)
ax.set_aspect('equal')
# (3) g値のヒートマップ（コスト場）
ax = axes[2]
g_display = planner.g.copy()
g_display[g_display == planner.INF] = np.nan
im = ax.imshow(g_display.T, origin='lower', cmap='viridis')
ax.plot(*start, 'go', markersize=12)
ax.plot(*goal, 'r*', markersize=15)
ax.set_title('(c) Cost-to-Goal (g values)', fontsize=12)
plt.colorbar(im, ax=ax, shrink=0.8)
ax.set_aspect('equal')
plt.suptitle('D* Lite Path Planning with Incremental Replanning',
fontsize=14, fontweight='bold')
plt.tight_layout()
plt.savefig('dstar_lite_planning.png', dpi=150, bbox_inches='tight')
plt.show()
上の3つのパネルから、D* Lite の動作が明確に読み取れます。
- パネル(a) 初期経路: 障害物（赤い領域）を迂回しつつ、緩斜面（オレンジ領域）もなるべく避ける経路が計算されています。A* と同様に、最短距離とコストのバランスをとった経路になっています。
- パネル(b) 再計画後: 新たな障害物（赤い×印）が出現した後、影響を受ける経路部分のみが再計算され、新しい迂回路が生成されています。点線の旧経路と比較すると、新障害物の周囲だけが変更されていることが分かります。これがD* Liteのインクリメンタル再計画の効果です。
- パネル(c) コスト場: ゴールからの到達コスト（$g$ 値）のヒートマップです。ゴール付近は値が小さく（紫色）、障害物の背後はコストが高く（黄色）なっています。経路はこのコスト場の勾配に沿って下っていく形で決定されます。
統合シミュレーション: ローバーの自律走行
最後に、SLAMとD*を統合した簡単な自律走行シミュレーションを実装します。ローバーが未知の環境で地図を構築しながら経路を計画し、新しい障害物を発見したら経路を更新する一連の流れです。
import numpy as np
import matplotlib.pyplot as plt
# 環境設定
env_size = 50
true_env = np.zeros((env_size, env_size))
# 障害物配置（ローバーは最初知らない）
true_env[10:13, 8:35] = 1.0
true_env[22:26, 15:38] = 1.0
true_env[16:20, 28:32] = 1.0
true_env[35:40, 5:15] = 1.0
for i in range(env_size):
for j in range(env_size):
if (i - 42)**2 + (j - 30)**2 < 16:
true_env[i, j] = 1.0
# ローバーの知識（最初は空白）
known_map = np.ones((env_size, env_size)) * 0.5 # 未知 = 0.5
sensor_range = 8
start = (2, 2)
goal = (47, 47)
def sense_environment(pos, true_env, known_map, sensor_range):
"""ローバーのセンサで周囲を観測し known_map を更新"""
rx, ry = pos
changed = []
for dx in range(-sensor_range, sensor_range + 1):
for dy in range(-sensor_range, sensor_range + 1):
nx, ny = rx + dx, ry + dy
if 0 <= nx < env_size and 0 <= ny < env_size:
dist = np.sqrt(dx**2 + dy**2)
if dist <= sensor_range:
old_val = known_map[nx, ny]
new_val = true_env[nx, ny]
if old_val != new_val:
known_map[nx, ny] = new_val
changed.append((nx, ny, new_val))
return changed
# シミュレーション
rover_pos = start
trajectory = [rover_pos]
replan_count = 0
snapshots = []
# 初回観測
sense_environment(rover_pos, true_env, known_map, sensor_range)
# D* Lite で初期経路
planner_grid = known_map.copy()
planner_grid[planner_grid == 0.5] = 0.0 # 未知は通行可として計画
planner_sim = DStarLite(planner_grid, start, goal)
planner_sim.compute_shortest_path()
current_path = planner_sim.extract_path()
max_steps = 500
step = 0
while rover_pos != goal and step < max_steps:
# 現在の経路に沿って1ステップ移動
if len(current_path) > 1:
next_pos = current_path[1]
# 実際に障害物でないか確認
if true_env[next_pos] >= 0.7:
# 障害物！ → 停止して再計画
changed = sense_environment(rover_pos, true_env,
known_map, sensor_range)
planner_sim.update_grid(changed)
current_path = planner_sim.extract_path()
replan_count += 1
else:
rover_pos = next_pos
trajectory.append(rover_pos)
# 新しい位置で観測
changed = sense_environment(rover_pos, true_env,
known_map, sensor_range)
if changed:
planner_sim.update_grid(changed)
current_path = planner_sim.extract_path()
replan_count += 1
else:
current_path = current_path[1:]
else:
break
step += 1
# スナップショット保存
if step in [1, 15, 40, max_steps - 1] or rover_pos == goal:
snapshots.append((step, rover_pos, known_map.copy(),
list(trajectory)))
# スナップショットが足りない場合の対応
if not snapshots:
snapshots.append((step, rover_pos, known_map.copy(),
list(trajectory)))
# 結果の可視化
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
# 左: 真の環境 + ローバー軌跡
ax = axes[0]
ax.imshow(true_env.T, origin='lower', cmap='RdYlGn_r',
vmin=0, vmax=1, alpha=0.6)
if trajectory:
tr, tc = zip(*trajectory)
ax.plot(tr, tc, 'b-', linewidth=1.5, alpha=0.8, label='Trajectory')
ax.plot(*start, 'go', markersize=12, label='Start')
ax.plot(*goal, 'r*', markersize=15, label='Goal')
ax.set_title(f'True Environment + Rover Trajectory\n'
f'Steps: {step}, Replans: {replan_count}', fontsize=11)
ax.legend(fontsize=9)
ax.set_aspect('equal')
# 右: ローバーの知識マップ（最終）
ax = axes[1]
ax.imshow(known_map.T, origin='lower', cmap='RdYlGn_r',
vmin=0, vmax=1, alpha=0.7)
if trajectory:
ax.plot(tr, tc, 'b-', linewidth=1.5, alpha=0.8)
ax.plot(*start, 'go', markersize=12)
ax.plot(*goal, 'r*', markersize=15)
ax.set_title("Rover's Known Map (Final)", fontsize=11)
ax.set_aspect('equal')
plt.suptitle('Autonomous Navigation: SLAM + D* Lite',
fontsize=14, fontweight='bold')
plt.tight_layout()
plt.savefig('autonomous_nav_simulation.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"総ステップ数: {step}")
print(f"再計画回数: {replan_count}")
print(f"軌跡の長さ: {len(trajectory)} ノード")
print(f"ゴール到達: {'成功' if rover_pos == goal else '未到達'}")
この統合シミュレーションでは、ローバーが完全に未知の環境からスタートし、以下のサイクルを繰り返しています。
- センサで周囲を観測して既知マップを更新
- D* Lite で経路を計画（または再計画）
- 経路に沿って1ステップ移動
- 新しい障害物が見つかったら経路を再計画
左パネルの軌跡を見ると、ローバーが障害物を発見するたびに迂回路を選択していることが分かります。直線的な最短経路ではなく、環境を探索しながら適応的に経路を修正しているのが自律ナビゲーションの特徴です。
右パネルの既知マップは、ローバーが通過した経路の周囲だけが確定（緑 or 赤）し、遠方は灰色（未知 = 0.5）のままです。これは実際のローバー運用でも同様で、センサの到達範囲外の地形は衛星画像からの大まかな情報に頼ります。
再計画回数（Replans）が多いほど、ローバーが「想定外の障害物」に多く遭遇したことを意味します。AutoNav の性能は、この再計画を効率的に行えるかどうかにかかっており、D* Lite のインクリメンタル再計画はまさにこの要件に応えるアルゴリズムです。
まとめ
本記事では、惑星探査ローバーの自律ナビゲーションについて、以下の内容を解説しました。
- 通信遅延の壁: 地球-火星間の往復通信に最大44分かかるため、リアルタイム遠隔操縦は不可能であり、ローバー自身が「考えて走る」自律性が必須である
- ビジュアルオドメトリ（VO）: カメラ画像の特徴点マッチングとRANSACにより、車輪スリップに影響されない高精度な自己位置推定を実現する
- ステレオビジョン: 左右カメラの視差から三角測量で3D地形（DEM）を復元し、ローバー前方の地形を3次元的に把握する
- SLAM: 「地図がなければ位置が分からず、位置が分からなければ地図が作れない」という鶏と卵の問題を、ベイズ推定の枠組みで同時に解く。占有格子マップは対数オッズの加算で効率的に更新できる
- 危険地形検知: DEMから段差・傾斜角・粗さを計算し、グッドネスマップとして経路計画に統合する
- D* Lite: マップの変化にインクリメンタルに対応できる経路計画アルゴリズムであり、ゴールからの逆方向探索と $g$/$rhs$ の不整合管理により効率的な再計画を実現する
- AutoNav: これらの技術を統合したJPLの自律走行システムであり、Perseveranceでは「走りながら考える」ことで1日120m以上の自律走行を達成している
自律ナビゲーションは、宇宙探査だけでなく自動運転や災害対応ロボティクスにも共通する中核技術です。次のステップとして、以下のトピックも参考にしてください。
- テラメカニクス — 車輪と土壌の相互作用の力学（ローバーが走行する「地面」の側の物理）
- カルマンフィルタ — SLAMの状態推定で使われるベイズフィルタの代表的手法
- 強化学習と経路計画 — 学習ベースのナビゲーション手法