前書き

移動ロボット誘導の文脈でeikonal方程式とFast Marching Methodを調べましたが、納得感のある解説が見つからなかったので自分で整理してみました。オリジナル文献は（多分）次のものです。

J. A. Sethian: A marching level set method for monotonically advancing fronts. in Proceedings of the National Academy of Sciences, 93, pp. 1591–1595 (1996).

上記文献の筆者自身による前刷りが↓で読めます。

Wikipediaも参考にさせて頂きました。

eikonal方程式とは

ある領域$\mathcal{D}$内の任意の位置$\boldsymbol{p}$を出発した点が（与えられた）ゴール領域$\mathcal{G}(\subset\mathcal{D})$内の任意の点に到着するのに要する最短時間を$T(\boldsymbol{p})$とおきます。点は任意の位置において任意の方向に動けますが、その速さ$v(\boldsymbol{p})(>0)$は位置に依存するとします。屈折率が場所により異なる媒質の中を進む光のイメージです。この$T(\boldsymbol{p})$が満たすべき式を考えましょう。

ゴールを$\mathcal{G}$内のある一つの点に定めたとしても、$\boldsymbol{p}$からゴールに至る経路は無数に考えられます。今、その中のある経路$\mathrm{R}$に沿って点が動く状況を考えると、このときに要する時間$T_{\mathrm{R}}(\boldsymbol{p})$は
$$T_{\mathrm{R}}(\boldsymbol{p})=\int_{\mathrm{R}}\frac{\|\mathrm{d}\boldsymbol{p}^{\prime}\|}{v(\boldsymbol{p}^{\prime})}$$
と表せます（$\boldsymbol{p}^{\prime}$は経路上の任意の点）。$\displaystyle T(\boldsymbol{p})=\min_{\mathrm{R}}T_{\mathrm{R}}(\boldsymbol{p})$です。そして明らかに、$\boldsymbol{p}\in\mathcal{G}$ならば$T(\boldsymbol{p})=0$です。

経路$\mathrm{R}$の具体的な形は分かりませんが、ここで次のような思考実験をしてみましょう。点$\boldsymbol{p}$を中心とする半径が無限小$\mathrm{d}l$の円環$\mathcal{B}$を考えます。$\mathcal{B}$上の点$\boldsymbol{p}^{\prime}$をどのように選んでも、$v(\boldsymbol{p}^{\prime})\simeq v(\boldsymbol{p})$ならば$\boldsymbol{p}$から$\boldsymbol{p}^{\prime}$に至る最短時間は$\mathrm{d}l/v(\boldsymbol{p})$ですので、
$$T(\boldsymbol{p})=\min_{\boldsymbol{p}^{\prime}\in\mathcal{B}}\left\{\mathrm{d}l/v(\boldsymbol{p})+T(\boldsymbol{p}^{\prime})\right\}$$
が成り立つはずです。ここで
$$T(\boldsymbol{p}^{\prime})\simeq T(\boldsymbol{p})+\nabla T(\boldsymbol{p})(\boldsymbol{p}^{\prime}-\boldsymbol{p})$$
と近似すれば、上式は
$$\min_{\boldsymbol{p}^{\prime}\in\mathcal{B}}\left\{\mathrm{d}l/v(\boldsymbol{p})+\nabla T(\boldsymbol{p})(\boldsymbol{p}^{\prime}-\boldsymbol{p})\right\}=0$$
と変形できます。中括弧の中で変化するのは$\boldsymbol{p}^{\prime}$だけで、最小となるのは明らかに$\nabla T(\boldsymbol{p})^{\mathrm{T}}$と$\boldsymbol{p}^{\prime}-\boldsymbol{p}$が真逆を向いたときです。そのような$\boldsymbol{p}^{\prime}$は
$$\boldsymbol{p}^{\prime}=\boldsymbol{p}-\mathrm{d}l,\frac{\nabla T(\boldsymbol{p})^{\mathrm{T}}}{\|\nabla T(\boldsymbol{p})\|}$$
と求まります。したがって、
$$\mathrm{d}l/v(\boldsymbol{p})-\mathrm{d}l,\frac{\nabla T(\boldsymbol{p})\nabla T(\boldsymbol{p})^{\mathrm{T}}}{\|\nabla T(\boldsymbol{p})\|}=0\qquad\Leftrightarrow\qquad v(\boldsymbol{p})\|\nabla T(\boldsymbol{p})\|=1$$

が成り立ちます。

以上をまとめた
$$
\begin{align}
v(\boldsymbol{p})\|\nabla T(\boldsymbol{p})\|&=1\qquad(\boldsymbol{p}\in\mathcal{D}\cap\bar{\mathcal{G}})
\\
T(\boldsymbol{p})&=0\qquad(\boldsymbol{p}\in\mathcal{G})
\end{align}
$$
をeikonal方程式と呼びます。

Fast Marching Method

以下は2次元空間に限定し、領域$\mathcal{D}$と$\mathcal{G}$をそれぞれ等間隔格子点の集合$\Lambda_{\mathcal{D}}$と$\Lambda_{\mathcal{G}}$で置き換えて話を進めます。格子点間隔は$x$方向と$y$方向どちらも1としても一般性を失いません。

格子座標$\boldsymbol{p}=<j,i>$における$T(\boldsymbol{p})$および$v(\boldsymbol{p})$の値をそれぞれ$T_{j,i}$および$v_{j,i}$と表すことにしましょう。偏微分を1次差分により近似することにして、eikonal方程式を次のように量子化します。
$$
\begin{align}
T_{j,i}&=0\qquad (<j,i>\in\Lambda_{\mathcal{G}})
\\
\left(T_{j,i}-T_{\mathrm{H}}\right)^{2}+\left(T_{j,i}-T_{\mathrm{V}}\right)^{2}&=f_{j,i}^{2}\qquad (\mbox{otherwise})
\end{align}
$$
ただし、$T_{\mathrm{H}}$は$T_{j-1,i}$か$T_{j+1,i}$のいずれか、$T_{\mathrm{V}}$は$T_{j,i-1}$か$T_{j,i+1}$のいずれかです。また、$f_{j,i}=1/v_{j,i}$とおきました。$T_{\mathrm{H}}$と$T_{\mathrm{V}}$をどのように選ぶかは後ほど説明することにして、後者の式を$T_{j,i}$について解けば
$$
T_{j,i}=\frac{T_{\mathrm{H}}+T_{\mathrm{V}}\pm\sqrt{2f_{j,i}^{2}-(T_{\mathrm{H}}-T_{\mathrm{V}})^{2}}}{2}
$$
となります。これが実数となるための必要条件は
$$
\sqrt{2}f_{j,i}\geq|T_{\mathrm{H}}-T_{\mathrm{V}}|
$$
です。複号の扱いについても後ほど説明します。

今、各格子点に次の三種をとりえるラベル$\lambda_{j,i}$を与えます。

fix ... $T_{j,i}$が確定している
near ... $T_{j,i}$が確定している格子点に隣接している
far ... 上記以外

重要なこととして、$T_{j,i}$の意味を考えれば、最初に確定しているゴール格子点に隣接する格子点の$T_{j,i}$は必ず$0$より大きくなります。同様に考えて、ある$T_{j,i}$を確定する際には、先に確定している隣接格子点の$T_{j,i}$よりも必ず大きくなるようにすべきです。
このことを踏まえて、次のアルゴリズムで$T_{j,i}$を更新していきます。

FastMarchingMethod(Lambda_D,Lambda_G)
{(T_<j,i>,lambda_<j,i>)} <- InitGrid(Lambda_D,Lambda_G)
while is_empty(H)
  <j*,i*> <- DeleteFromHeap(H)
  (T_<j,i>,lambda_<j,i>) <- FixGrid(<j*,i*>,Lambda_D; {lambda_<j,i>},H)
end while
return {T_(j,i)}

ただし、Hはラベルがnearである格子点群を一時保存しておく2分ヒープです。
InitGrid、FixGridは次のように定義します。

InitGrid(Lambda_D,Lambda_G)
for <j,i> in Lambda_D do
  if <j,i> in Lambda_G then
    (T_<j,i>,lambda_<j,i>) <- (0,fix)
  else
    (T_<j,i>,lambda_<j,i>) <- (infty,far)
  end if
end for
H <- {}
for <j,i> in Lambda_D do
  if lambda_<j,i> = fix then
    FixGrid(<j,i>,Lambda_D; {lambda_<j,i>},H)
  end if
end for
return ({(T_<j,i>,lambda_<j,i>)},H)

FixGrid(<j*,i*>,Lambda_D; {lambda_<j,i>},H)
lambda_<j,i> <- fix
for <j',i'> in {<j-1,i>,<j+1,i>,<j,i-1>,<j,i+1>} do
  if <j',i'> in Lambda_D and <j',i'> != fix then
    UpdateGrid(<j,i>;{T_<j,i>})
    if lambda_<j',i'> = far then
      lambda_<j',i'> <- near
      AddToHeap(<j',i'>; H)
    else
      UpHeap(<j',i'>; H)
    end if
  end if
end for

DeleteFromHeap, AddToHeap, UpHeapは標準的なヒープ操作に準じます。

流れの概要は次の通りです。初期化処理として、格子点群$\Lambda_{\mathcal{G}}$に最初にラベルfixと値$T_{j,i}=0$を、残りの点群にラベルfarと暫定値$T_{j,i}=\infty$をそれぞれ割り振り、ラベルがfixである点の隣接点群の$T_{j,i}$値を、後ほど説明するUpdateGridを用いて計算しHに登録します。毎回の反復では、Hから根ノードを取得してその隣接点群を同様に更新し、Hに登録していきます（既にHに登録されていた場合はUpHeap操作を施します）。このように、変化があった格子点の情報のみ用いてHを更新することで、各ステップで$T_{j,i}$値が最小となる格子点が常に根ノードに保存されることになります。Hが空になれば終了です。

UpdateGridを定義するに当たり、考え方から説明していきましょう。格子点群$\Lambda_{\mathcal{G}}$から徐々に確定格子点群を拡大していくことになるので、任意の二つの格子点$<j_1,i_1>$と$<j_2,i_2>$について、$\lambda_{j_1,i_1}=$fixかつ$\lambda_{j_2,i_2}=$nearならば$T_{j_1,i_1}<T_{j_2,i_2}$が言えます。したがってまず、
$$
\begin{align}
T_{\mathrm{H}}&=\min\{T_{j-1,i},T_{j+1,i}\}
\\
T_{\mathrm{V}}&=\min\{T_{j,i-1},T_{j,i+1}\}
\end{align}
$$
と選ばれるべきであり、その上でさらに
$$
T_{j,i}>\min\{T_{\mathrm{H}},T_{\mathrm{V}}\}
$$
を満たすように$T_{j,i}$が選ばれるべきです。もし$T_{\mathrm{H}}>T_{\mathrm{V}}$ならば、満たすべき条件は
$$
T_{j,i}>T_{\mathrm{H}}\quad\Leftrightarrow\quad
\frac{\mp\sqrt{2f_{j,i}^2-(T_{\mathrm{H}}-T_{\mathrm{V}})^{2}}-(T_{\mathrm{H}}-T_{\mathrm{V}})}{2}>0
$$
で、これは根号の前の複号のうち正符号が選ばれ、かつ$f_{j,i}>T_{\mathrm{H}}-T_{\mathrm{V}}$であることと同値です。同様に、もし$T_{\mathrm{H}}<T_{\mathrm{V}}$ならば、根号の前の複号のうち正符号が選ばれ、かつ$f_{j,i}>T_{\mathrm{V}}-T_{\mathrm{H}}$である場合に$T_{j,i}>T_{\mathrm{V}}$が満たされます。これらは
$$
f_{j,i}>|T_{\mathrm{H}}-T_{\mathrm{V}}|
$$
とまとめられます。なお、これが満たされることは$T_{j,i}$が実数となるための十分条件となっています。根号の前の複号はいずれにしても正符号を選ぶことになるので、更新式は
$$
T_{j,i}=\frac{T_{\mathrm{H}}+T_{\mathrm{V}}+\sqrt{2f_{j,i}^{2}-(T_{\mathrm{H}}-T_{\mathrm{V}})^{2}}}{2}
$$
となります。
$f_{j,i}>|T_{\mathrm{H}}-T_{\mathrm{V}}|$が満たされない場合は、$T_{j,i}$の最悪値$f_{j,i}+\min\{T_{\mathrm{H}},T_{\mathrm{V}}\}$を暫定値とすれば良いです。
以上に基づけば、UpdateGridは次のように定義できます。

UpdateGrid(<j,i>;{T_<j,i>})
T_H <- min(T_<j-1,i>,T_<j+1,i>)
T_V <- min(T_<j,i-1>,T_<j,i+1>)
if f_<j,i> > abs(T_H-T_V) then
  T_<j,i> <- (T_H+T_V+sqrt(2*f_<j,i>^2-(T_H-T_V)^2)) / 2
else
  T_<j,i> <- f_<j,i>+min(T_H,T_V)
end if
return T_<j,i>

これがFast Marching Methodの全貌です。

勘の良い方は、Dijkstra法に非常に良く似ていることにお気づきと思います。筆者は当初、$v(\boldsymbol{p})$が不連続なのになぜ$T(\boldsymbol{p})$の偏微分方程式が解けるんだ？と疑問でしたが、これが答えでした。片側からしか解いていかないので、片方向微分可能性さえ満たしていれば良いということです。

高速化の鍵はヒープを使うことです。下記記事もご参照下さい。

Fast Marching Methodを用いた経路計画（誘導）

eikonal方程式では、障害物とそうでない空間を「屈折率の異なる媒体」として表現します。この意味で、得られる経路が障害物を必ず避けることを保証しません。この点は注意が必要です。

例えば障害物のある領域$\mathcal{O}$では速さが著しく低下させられる、と想定して次のようにすれば良いでしょう。
$$
v(\boldsymbol{p})=\begin{cases}
1 & (\boldsymbol{p}\in\mathcal{D}\cap\bar{\mathcal{O}})
\\
1.0\times 10^{-2} & (\boldsymbol{p}\in{\mathcal{O}})
\end{cases}
$$
この下でFast Marching Methodを実行して全格子点における$T_{j,i}$値を得たのち、任意の位置$(x,y)$における$T(x,y)$値を次のように補間によって近似的に得ることにします。
$$
\begin{align}
j&=\lfloor{x}\rfloor
\\
i&=\lfloor{y}\rfloor
\\
T(x,y)&\simeq (j+1−x)(i+1−y)T_{j,i}+(x−j)(i+1−y)T_{j+1,i}+(j+1−x)(y−i)T_{j,i+1}+(x−j)(y−i)T_{j+1,i+1}
\end{align}
$$

さて、$T(x,y)$は「その点から出発し最短経路でゴールまで到達したときに要する時間」ですから、$T(x,y)$の小さくなる方へ辿っていけば、結果的にその軌跡は最短経路になっているはずです。つまり、$T(x,y)$の勾配が進むべき方向を教えてくれます（この意味で、$T(x,y)$はポテンシャル関数の一種であるとも言えます）。上記のように$T(x,y)$の補間式を与えるならば、$(x,y)$における勾配は次式で求まります。
$$
\begin{align}
\frac{\partial T}{\partial x}(x,y)&=(i+1−y)(T_{j+1,i}−T_{j,i})+(y−i)(T_{j+1,i+1}−T_{j,i+1})
\\
\frac{\partial T}{\partial y}(x,y)&=(j+1−x)(T_{j,i+1}-T_{j,i})+(x−j)(T_{j+1,i+1}-T_{j+1,i})
\end{align}
$$
誘導においては勾配の方向だけが意味を持つので、適当なステップ長$l$を用いて
$$
\boldsymbol{d}(x,y)=l\left[\begin{array}{c}
\displaystyle
\frac{\partial T}{\partial x}(x,y)\
\displaystyle
\frac{\partial T}{\partial y}(x,y)
\end{array}\right]
\Big/\left\|
\left[\begin{array}{c}
\displaystyle
\frac{\partial T}{\partial x}(x,y)\
\displaystyle
\frac{\partial T}{\partial y}(x,y)
\end{array}\right]
\right\|
$$
とし、Runge-Kutta法などで$(x,y)$を更新して行けばゴールに辿り着くでしょう。

筆者の実装によって適当な迷路を解かせた例を掲載します。左上がスタート、右下がゴールです。更新にはいわゆる古典的Runge-Kutta法ではなくHeun法を使いました。Euler法（1次解法）だと経路がジャギーになるのですが、Heun法（2次解法）と古典的Runge-Kutta法（4次解法）ではほとんど差がないようです。