Rustで自律移動ロボットのシミュレータ

最近 [Rust](https://www.rust-lang.org/ja) が流行っていますね。ご多分に漏れず私も始めてみたのですが、[Hello, World!](https://doc.rust-jp.rs/book-ja/ch01-02-hello-world.html) をクリアした後、何を作るとRustっぽいのか悩みました。GUIが得意な言語でもないですし。
そこで今回は、[Numpyで書いた差動二輪型自律移動ロボットのシミュレータ](https://github.com/nmatsui/robot_simulator) を、Rustで再実装してみました。自律移動ロボットは、自己位置推定や局所経路探索のために行列演算を高速に繰り返すので、演算性能に優れたRustの題材としては良さげな気がします。ソースコードは [robot_simulator_rust](https://github.com/nmatsui/robot_simulator_rust) です。ではいってみよう！

# シミュレートする状況
あるルールに従って移動するターゲットを、[差動二輪型自律移動ロボット](http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/wheelrobot.html)が追いかけます。ただしロボットはタイヤのスリップや路面の細かな段差のために、指定した入力どおりには移動できず、移動後のロボットの姿勢（$x$座標、$y$座標、及びロボットの向き）には僅かな誤差が含まれるものとします。

![差動二輪型自律移動ロボット](http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/fig/wheelph1.gif)
> 出典： 東北学院大学 工学部 機械知能工学科 RDE 熊谷正朗 車輪移動ロボット http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/wheelrobot.html

またロボットには360度カメラが搭載されており、外部に設置されているマーカーまでの距離と方向を、マーカーの正確な$x$座標と$y$座標と対応付けて観測することができます。ただしロボットが観測したマーカーまでの距離と方向には、やはり僅かな誤差が含まれてしまうものとします。

このような状況下で、与えられたルールに従ってロボットが自律移動できるように、[拡張カルマンフィルタ](https://en.wikipedia.org/wiki/Extended_Kalman_filter)を用いた自己位置推定と [Dynamic Window Approach](https://qiita.com/MENDY/items/16343a00d37d14234437)もどきを用いた局所経路計画を実装します。

なお今回は計算を簡単にするために、各誤差は特定の傾向を持たない白色ガウス雑音と仮定しています。

# シミュレーション結果
以下に３つのルールに対してシミュレートした結果を図示します（図中に描かれる線などの意味は、次表を参考）。

|線や四角形|意味|
|:--|:--|
|黒のマーク|指定されたルールに従って移動するターゲット|
|黒の線|理想の軌跡|
|灰色の正方形|観測されるマーカー|
|緑の線|ロボットが観測したマーカーの方向と距離|
|赤のマーク|ロボットが推定した自身の姿勢（位置と向き）|
|赤の線|ロボットが推定した自己位置の軌跡|
|青の線|実際にロボットが移動した軌跡（ロボット自身は観測しえない）|
> 円と線分で構成されたマークは、円の中心がロボットの位置、中心から外周に向かう線分がロボットの向きを表しています。

これらのルールは、[Agent trait](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/agent.rs#L128-L178)を実装した構造体の[`get_ideal`メソッド](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/agent.rs#L169-L177)にて定義していますので、構造体を作成し新たなルールを自分で追加することもできます。

```rust:src/agent/circular_agent.rs
impl Agent for CircularAgent {
  /// Get the ideal pose that moves on the circumference at a constant angular velocity
  ///
  /// ## Arguments
  /// * `_` - current pose of robot which is defined as nalgebra::Vector3::new(x, y, theta)
  ///     *  Since the current pose is not used in this function, the argument name is defind as `_`
  /// * `t` - elapsed time (sec) from the start of this simulation
  ///
  /// ## Returns
  /// ideal pose of robot which is defined as nalgebra::Vector3::new(x, y, theta)
  fn get_ideal(&self, _: &na::Vector3<f64>, t: f64) ->  na::Vector3<f64> {
    let angle = INPUT_OMEGA * t;
    let x = angle.cos();
    let y = angle.sin();
    let theta = utils::normalize_angle(angle + PI / 2.0);

    na::Vector3::new(x, y, theta)
  }
```


## ルール１：円を描く
一定の角速度で円を描いて移動するターゲットを追いかけます。
![circular_simulation](https://raw.githubusercontent.com/nmatsui/robot_simulator_rust/main/docs/images/circular_simulation.gif)

## ルール2：正方形を描く
一定の距離進んだ後に反時計回りで90度回転、を繰り返すことで正方形の外周上を移動するターゲットを追いかけます。
![square_simulation](https://raw.githubusercontent.com/nmatsui/robot_simulator_rust/main/docs/images/square_simulation.gif)

* 本来の差動二輪型自律移動ロボットであれば、その場で旋回（超信地旋回）する際にここまで左右にズレたりしませんが、それはシミュレータの設定が甘いということで・・・

## ルール3：waypointを追いかける
ロボットが指定された姿勢（位置と向き）に大体一致すると、次の姿勢へと瞬間移動するターゲット（waypoint）を追いかけます。
![waypoints_simulation](https://raw.githubusercontent.com/nmatsui/robot_simulator_rust/main/docs/images/waypoints_simulation.gif)

# 環境
||バージョン|
|:--|:--|
|Rust|1.51.0|
|OS|macOS Big Sur 11.5.1|

シミュレータの可視化部分をRustで書くのは大変そうだったので、 Python 3.9上の別プロセスで動作する [plotter](https://github.com/nmatsui/robot_simulator/blob/main/plotter.py) を実装しています。シミュレータ本体のRust部分とは [ZeroMQ](https://zeromq.org/) で通信しています。

# 動作方法
## plotterの起動
1. pyenvを用い、tck-tkをincludeしてpython 3.9をインストール

    ```bash
    $ brew install tcl-tk
    $ env \
      PATH="$(brew --prefix tcl-tk)/bin:$PATH" \
      LDFLAGS="-L$(brew --prefix tcl-tk)/lib" \
      CPPFLAGS="-I$(brew --prefix tcl-tk)/include" \
      PKG_CONFIG_PATH="$(brew --prefix tcl-tk)/lib/pkgconfig" \
      CFLAGS="-I$(brew --prefix tcl-tk)/include" \
      PYTHON_CONFIGURE_OPTS="--with-tcltk-includes='-I$(brew --prefix tcl-tk)/include' --with-tcltk-libs='-L$(brew --prefix tcl-tk)/lib -ltcl8.6 -ltk8.6'" \
      pyenv install 3.9.5
    ```
1. リポジトリをclone

    ```bash
    $ git clone https://github.com/nmatsui/robot_simulator.git
    $ cd robot_simulator/
    ```
1. pipenvを用いてライブラリをインストール

    ```bash
    pipenv install
    ```
1. plotterを起動

    ```bash
    $ pipenv run plotter
    ```

## シミュレータの起動
1. リポジトリをclone

    ```bash
    $ git clone https://github.com/nmatsui/robot_simulator_rust.git
    $ cd robot_simulator_rust/
    ```
1. releaseバイナリをビルド

    ```bash
    $ cargo build --release
    ```
1. シミュレータを起動（引数は **circular**, **square**, **waypoints** のどれか）

    ```
    $ ./target/release/robot_simulator_rust circular
    ```

# シミュレータの詳細
## ロボットの運動状態のモデル化
ある地点 $(x, y)$ で $x$軸 からの角度 $\theta$ を向いたロボットに対して、ある入力 $u$ （並進速度 $v$ [m/s] 角速度 $\\omega$ [rad/s]） を与えると、 下の図のようにプログラムの1回分の計算サイクル後にロボットは次の姿勢まで移動します。この間、 $v$ と $\\omega$ は変化しません。

![robot](https://raw.githubusercontent.com/nmatsui/robot_simulator_rust/main/docs/images/robot.png)

この運動を、以下のようにモデル化します。

```math
\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1},u_{k}) + w_{k}
```

ここで $\\hat{x}\_{k-1|k-1}$は 移動を開始する前のロボットの姿勢 $(x\_{k-1}, y\_{k-1}, \\theta\_{k-1})$ 、 $u\_{k}$ はロボットを移動させるために与えた入力 $(v, \\omega)$ であり、 $f$ は $\\hat{x}$ と $u$ の何らかの非線形な関数、 $w\_{k}$ はロボットが移動すると誤差が含まれてしまうことを表す共分散行列が $Q$ で平均が0の白色ガウス雑音です。これらを用いて、移動後のロボットの姿勢 $\\hat{x}\_{k|k-1}$を算出することができます。

ここで $f$ を立式したいのですが、移動を開始する前のロボットの姿勢 $\\hat{x}\_{k-1|k-1}$ に、入力された速度 $u\_{k}$ をプログラムの計算サイクル $\\varDelta{t}$ で積分したものを加えれば、次の姿勢 $\\hat{x}\_{k|k-1}$ になります。この関係から $f$ を立式できますが、真面目に積分するのがちょっと面倒です。そこで今回は **プログラムの計算サイクル $\\varDelta{t}$ における回転角 $\\varDelta{\\theta}$は十分に小さい** と仮定して、本来円弧を描くロボットの移動軌跡を次のように直線で近似して $f$ を立式します。

![state_model](https://raw.githubusercontent.com/nmatsui/robot_simulator_rust/main/docs/images/state_model.png)

この図より、 $\\hat{x}\_{k|k-1}$ と $\\hat{x}\_{k-1|k-1}$ の関係を次のように表すことができます。

```math
x_{k} = x_{k-1}+\varDelta{L}\cos(\theta_{k-1}+\frac{1}{2}\varDelta{\theta})
```
```math
y_{k} = y_{k-1}+\varDelta{L}\sin(\theta_{k-1}+\frac{1}{2}\varDelta{\theta})
```
```math
\theta_{k} = \theta_{k-1}+\varDelta{\theta}
```
また $\\varDelta{L}$ と $\\varDelta{\\theta}$ は、 $\\varDelta{t}$ と $u$ を用いて次のように書けます。

```math
\varDelta{L} = \it{v}\varDelta{t}
```
```math
\varDelta{\theta} = \it{\omega}\varDelta{t}
```

これらから、 $f$ は $\\hat{x}\_{k-1|k-1}$ と $u$ を用いて次のように表すことができます。

```math
\it_{f}
=
\left(
  \begin{array}{c}x_{k-1}\\y_{k-1}\\\theta_{k-1}\end{array}
\right)
+
\left(
  \begin{array}{cc}\cos(\theta_{k-1}+\frac{1}{2}\omega\varDelta{t})\varDelta{t}&0\\\sin(\theta_{k-1}+\frac{1}{2}\omega\varDelta{t})\varDelta{t}&0\\0&\varDelta{t}\end{array}
\right)
\left(
  \begin{array}{c}\it{v}\\\it{\omega}\end{array}
\right)
```

このモデル（状態方程式）を実装したのが、`models::robot`モジュールの[`ideal_move`関数](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/models/robot.rs#L20-37)です。色々とややこしいことを考えてたわりに、関数はシンプルです。

```rust:src/models/robot.rs
/// Calculate the state equation of a simulated robot
///
/// ## Arguments
/// * `current` - the current pose of the simulated robot(x, y, theta)
/// * `input` - the input vector(linear velocity, angular velocity)
/// * `delta` - time delta to next tick
///
/// ## Returns
/// The pose(x, y, theta) of the simulated robot at the next tick
pub fn ideal_move(current: &na::Vector3<f64>, input: &na::Vector2<f64>, delta: f64) -> na::Vector3<f64> {
  let a = current[2] + input[1] * delta / 2.0;
  let m = na::Matrix3x2::new(a.cos() * delta, 0.0,
                             a.sin() * delta, 0.0,
                             0.0,             delta);
  let mut next = current + m * input;
  next[2] = utils::normalize_angle(next[2]);
  next
}
```

## カメラによるマーカー観測のモデル化
ロボットに据え付けられたカメラでマーカーを観測すると、カメラの視界内にマーカーが写ります。ここで使うマーカーは、例えば[ArUcoマーカー](https://docs.opencv.org/4.5.2/d5/dae/tutorial_aruco_detection.html)のような、マーカーの見え方によってカメラからマーカーまでの距離とカメラ座標系におけるマーカーの位置と方向を推定できるものとします。

![singlemarkersdetection](https://docs.opencv.org/4.5.2/singlemarkersdetection.jpg)
![singlemarkersaxes](https://docs.opencv.org/4.5.2/singlemarkersaxes.jpg)
> 出典： OpenCV 4.5.2 Detection of ArUco Markers 　https://docs.opencv.org/4.5.2/d5/dae/tutorial_aruco_detection.html

ただし実際のカメラとArUcoマーカーでは３次元空間内でのマーカー姿勢を算出しますが、今回考えているロボットは $(x, y)$ の２次元平面上を移動します。そこでマーカーは $(x, y)$ 平面上に存在し、カメラはその平面上のマーカーまでの距離 $d\_{m}$ [m] と、カメラ正面からの角度 $\\phi\_{m}$ [rad] が観測できるものとします。また、マーカーのIDとロボット座標系における位置を対応付けておき、観測したマーカーの絶対的な位置 $(x\_{m}, y\_{m})$ が得られるものとします。

![robot](https://raw.githubusercontent.com/nmatsui/robot_simulator_rust/main/docs/images/camera.png)

このマーカーの観測を、以下のようにモデル化します。

```math
z_{k} = h(\hat{x}_{k|k-1}) + v_{k}
```
ここで $\\hat{x}\_{k|k-1}$ はマーカーを観測した時点でのロボットの姿勢 $(x\_{k-1}, y\_{k-1}, \\theta\_{k-1})$ 、 $z\_{k}$ はロボットが観測したマーカーの情報 $(d\_{m}, \\phi\_{m})$ であり、 $h$ は $\\hat{x}$ に関する非線形な関数、 $v\_{k}$ はロボット観測したマーカーの位置と角度には誤差が含まれてしまうことを表す共分散行列が $R$ で平均が0の白色ガウス雑音です。

$h$ はシンプルに、幾何学的な関係から立式することができます。

![observation_model](https://raw.githubusercontent.com/nmatsui/robot_simulator_rust/main/docs/images/observation_model.png)

```math
d_{m} = \sqrt{(x_{m}-x_{k})^2 + (y_{m}-y_{k})^2}
```
```math
\phi_{m} = tan^{-1}\left(\frac{y_{m}-y_{k}}{x_{m}-x_{k}}\right) - \theta_{k}
```
このモデル（観測方程式）を実装したのが、 `models::camera` モジュールの [`observe` 関数](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/models/camera.rs#L7-L19)です。そのまんまですね。

```rust:src/models/camera.rs
/// Calculate the observation equation of a camera
///
/// ## Arguments
/// * `landmark` - the observed landmark
/// * `current` - the current pose of the simulated robot(x, y, theta)
///
/// ## Returns
/// An observed vector(distance, angle)
pub fn observe(landmark: &Point, current: &na::Vector3<f64>) -> na::Vector2<f64> {
  let dist = (na::Vector2::new(landmark.x, landmark.y) - current.fixed_rows::<2>(0)).norm_squared().sqrt();
  let angle = (landmark.y - current[1]).atan2(landmark.x - current[0]) - current[2];
  na::Vector2::new(dist, angle)
}
```
※ Nargeblraの[Matrix::norm_squared](https://docs.rs/nalgebra/0.17.2/nalgebra/base/struct.Matrix.html#method.norm_squared)は、２ベクトル間のL2ノルムの二乗を求めるメソッドです。計算速度を向上させるためにわざわざ二乗したままの結果を返すのに、後から平方してダイナシにしてしまうという（笑

## 拡張カルマンフィルタを用いた自己位置推定
上述したモデルを用いて、ロボットの自己位置を推定します。

### 予測ステップ
$f$ は非線形のため、線形近似するためにヤコビアン $F\_{k-1}$ を計算しておきます（[`calc_f` 関数](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/models/robot.rs#L39-54)）。

```math
F_{k-1}
=
\left.\frac{\partial f(\hat{x},u)}{\partial \hat{x}}\right|_{x=\hat{x}_{k-1|k-1},u=u_{k}}
=
\left(
  \begin{array}{ccc}
    \frac{\partial x_{k}}{\partial x_{k-1}}&\frac{\partial x_{k}}{\partial y_{k-1}}&\frac{\partial x_{k}}{\partial \theta_{k-1}}\\
    \frac{\partial y_{k}}{\partial x_{k-1}}&\frac{\partial y_{k}}{\partial y_{k-1}}&\frac{\partial y_{k}}{\partial \theta_{k-1}}\\
    \frac{\partial \theta_{k}}{\partial x_{k-1}}&\frac{\partial \theta_{k}}{\partial y_{k-1}}&\frac{\partial \theta_{k}}{\partial \theta_{k-1}}
  \end{array}
\right)
=
\left(
  \begin{array}{ccc}
    1&0&-\it{v}\varDelta{t}\sin(\theta_{k-1}+\frac{1}{2}\omega\varDelta{t})\\
    0&1&\it{v}\varDelta{t}\cos(\theta_{k-1}+\frac{1}{2}\omega\varDelta{t})\\
    0&0&1
  \end{array}
\right)
```

```rust:src/models/robot.rs
/// Calculate the jacobian of the state equation
///
/// ## Arguments
/// * `current` - the current pose of the simulated robot(x, y, theta)
/// * `input` - the input vector(linear velocity, angular velocity)
/// * `delta` - time delta to next tick
///
/// ## Returns
/// The jacobian of the state equation
pub fn calc_f(current: &na::Vector3<f64>, input: &na::Vector2<f64>, delta: f64) -> na::Matrix3<f64> {
  let a = current[2] + input[1] * delta / 2.0;

  na::Matrix3::new(1.0, 0.0, -1.0 * a.sin() * delta * input[0],
                   0.0, 1.0,  1.0 * a.cos() * delta * input[0],
                   0.0, 0.0,  1.0)
}
```
まずは $f$ を用いて、事前状態を計算します。

```math
\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1},u_{k}) = \left(
  \begin{array}{c}x_{k-1}\\y_{k-1}\\\theta_{k-1}\end{array}
\right)
+
\left(
  \begin{array}{cc}\cos(\theta_{k-1}+\frac{1}{2}\omega\varDelta{t})\varDelta{t}&0\\\sin(\theta_{k-1}+\frac{1}{2}\omega\varDelta{t})\varDelta{t}&0\\0&\varDelta{t}\end{array}
\right)
\left(
  \begin{array}{c}\it{v}\\\it{\omega}\end{array}
\right)
```
次に次式で事前誤差共分散行列 $P\_{k|k-1}$ を得ます。ここで $Q$ は $w\_{k}$ の共分散行列です。今回は簡単にするため、 $Q$ は時間変化せず一定としています。

```math
P_{k|k-1} = F_{k-1}P_{k-1|k-1}F_{k-1}^{T} + Q
```

rustで実装するとこんな感じです（`filters::kalman_filter::EKF`構造体の[`predict`関連関数](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/filters/kalman_filter.rs#L91-L108)）。言われたとおりに実装しているだけですね。

```rust:src/filters/kalman_filter.rs
  /// **\[private\]** Calculate the "predict step"
  ///
  /// ## Arguments
  /// * `xhat` - the current estimated pose(x, y, theta)
  /// * `p` - the current covariance matrix
  /// * `q` - the covariance matrix of process noise
  /// * `input` - the current input vector(linear velocity, angular velocity)
  /// * `delta` - time delta
  ///
  /// ## Returns
  /// Tuple of (predicted pose(x, y, theta), predicted covariance matrix)
  fn predict(xhat: &na::Vector3<f64>, p: &na::Matrix3<f64>, q: &na::Matrix3<f64>, input: &na::Vector2<f64>, delta: f64)
    -> (na::Vector3<f64>, na::Matrix3<f64>) {
    let a_priori_x = robot::ideal_move(xhat, input, delta);
    let f = robot::calc_f(xhat, input, delta);
    let a_priori_p = f * p * f.transpose() + q;
    (a_priori_x, a_priori_p)
  }
```

### 更新ステップ
同様に $h$ も非線形のため、線形近似するためにヤコビアン $H\_{k}$ を計算しておきます（[`calc_h` 関数](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/models/camera.rs#L21-34)）。

```math
H_{k}
=
\left.\frac{\partial h(\hat{x})}{\partial \hat{x}}\right|_{x=\hat{x}_{k|k-1}}
=
\left(
  \begin{array}{ccc}
    \frac{\partial d_{m}}{\partial x_{k}}&\frac{\partial d_{m}}{\partial y_{k-1}}&\frac{\partial d_{m}}{\partial \theta_{k-1}}\\
    \frac{\partial \phi_{m}}{\partial x_{k-1}}&\frac{\partial \phi_{m}}{\partial y_{k-1}}&\frac{\partial \phi_{m}}{\partial \theta_{k-1}}\\
  \end{array}
\right)
=
\left(
  \begin{array}{ccc}
    -\frac{x_{m}-x_{k}}{\sqrt{(x_{m}-x_{k})^2+(y_{m}-y_{k})^2}}&-\frac{y_{m}-y_{k}}{\sqrt{(x_{m}-x_{k})^2+(y_{m}-y_{k})^2}}&0\\
    \frac{y_{m}-y_{k}}{(x_{m}-x_{k})^2+(y_{m}-y_{k})^2}&-\frac{x_{m}-x_{k}}{(x_{m}-x_{k})^2+(y_{m}-y_{k})^2}&-1
  \end{array}
\right)
```
```rust:src/models/camera.rs
/// Calculate the jacobian of the observation equation
///
/// ## Arguments
/// * `landmark` - the observed landmark
/// * `current` - the current pose of the simulated robot(x, y, theta)
///
/// ## Returns
/// The jacobian of the observation equation
pub fn calc_h(landmark: &Point, current: &na::Vector3<f64>) -> na::Matrix2x3<f64> {
  let q = (landmark.x - current[0]).powf(2.0) + (landmark.y - current[1]).powf(2.0);

  na::Matrix2x3::new((current[0] - landmark.x)/q.sqrt(), (current[1] - landmark.y)/q.sqrt(),  0.0,
                     (landmark.y - current[1])/q,        (current[0] - landmark.x)/q,        -1.0)
}
```
$h$ を用いて、実際に観測したデータと予測した位置から観測したら得られるはずのデータとの誤差（観測残差）を計算します。

```math
\hat{y}_{k} = z_{k} - h(\hat{x}_{k|k-1})
=
\left(
  \begin{array}{c}d_{m}\\\phi_{m}\end{array}
\right)
-
\left(
  \begin{array}{c}\sqrt{(x_{m}-x_{k})^2 + (y_{m}-y_{k})^2}\\tan^{-1}\left(\frac{y_{m}-y_{k}}{x_{m}-x_{k}}\right) - \theta_{k}\end{array}
\right)
```

$H\_{k}$ と事前誤差共分散行列 $P\_{k|k-1}$ を用いて観測残差の共分散を計算し、カルマンゲイン $K\_{k}$ を得ます。ここで $R$ は $v\_{k}$ の共分散行列ですが、 $Q\_{k}$ と同様に時間変化せず一定としています。

```math
S_{k} = H_{k}P_{k|k-1}H_{k}^{T} + R
```
```math
K_{k} = P_{k|k-1}H_{k}^{T}S_{k}^{-1}
```

カルマンゲイン $K\_{k}$ と観測残差 $\\hat{y}\_{k}$ を用いて、予測した状態 $\\hat{x}\_{k|k-1}$ を更新します。

```math
\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_{k}\hat{y}_{k}
```
最後に誤差共分散行列を更新し、次回のステップに備えます。

```math
P_{k|k} = (I-K_{k}H_{k})P_{k|k-1}
```
rustで実装するとこんな感じです（`filters::kalman_filter::EKF` 構造体の [`update` 関連関数](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/filters/kalman_filter.rs#L110-L130)）。

```rust:src/filters/kalman_filter.rs
  /// **\[private\]** Calculate the "update step"
  ///
  /// ## Arguments
  /// * `r` - the covariance matrix of observation noise
  /// * `a_priori_x` - the predicted pose(x, y, theta)
  /// * `a_priori_p` - the predicted covariance matrix
  /// * `observed` - observed landmark
  ///
  /// ## Returns
  /// * Tuple of (estimated pose(x, y, theta), covariance matrix, kalman gain)
  fn update(r: &na::Matrix2<f64>, a_priori_x: &na::Vector3<f64>, a_priori_p: &na::Matrix3<f64>, observed: &Observed)
    -> (na::Vector3<f64>, na::Matrix3<f64>, na::Matrix3x2<f64>) {
    let yhat = na::Vector2::new(observed.distance, observed.angle) - camera::observe(&observed.landmark, a_priori_x);
    let h = camera::calc_h(&observed.landmark, a_priori_x);
    let s = h * a_priori_p * h.transpose() + r;
    let k = a_priori_p * h.transpose() * s.try_inverse().unwrap();
    let xhat = a_priori_x + k * yhat;
    let p = (na::Matrix3::identity() - k * h) * a_priori_p;
    (xhat, p, k)
  }
}
```
観測したマーカーの数だけ更新ステップを繰り返すことで、拡張カルマンフィルタの１サイクルが完了します。このサイクルを延々と繰り返すことで、ロボットの現在位置を逐次的に推定することが可能となります。

```rust:src/filters/kalman_filter.rs
  pub fn step(&mut self) -> ... {
...
    let (mut xhat, mut p) = EKF::predict(&self.xhat, &self.p, &self.q, &input, delta);
    let mut k: na::Matrix3x2<f64> = na::Matrix3x2::zeros();
    for observed in self.agent.noisy_observe() {
      let (updated_xhat, updated_p, updated_k) = EKF::update(&self.r, &xhat, &p, observed);
      xhat = updated_xhat;
      p = updated_p;
      k = updated_k;
    }

    self.xhat = xhat;
    self.p = p;
    self.t = t;
...
  }
```

## Dynamic Window Approachもどきを用いた局所経路計画
拡張カルマンフィルタを用いて自己位置が推定できたので、次はターゲットを追いかけるために最適な入力を計算します。ロボットの運動モデルを元に最適な入力を計算するアルゴリズムは種々提案されていますが、今回は実装がシンプルなDynamic Window Approachもどきを実装します（障害物回避を実装していないので、**もどき** なのです）。

### サンプリングする速度空間（Dynamic Window）の探索
まずはロボットが実際に動作可能な速度の範囲を求めます。ロボットのスペック（最大並進加速度・最大角加速度、最大並進速度・最小並進速度、最大角速度・最小角速度）と現在のロボットの並進速度と角速度から、次の計算サイクルで実際に動作可能な並進速度・角速度の最小値と最大値を計算します。

ここで求めた実現可能な並進速度・角速度の範囲を、事前に決めておいたサンプリングレートで区切り、次の計算サイクルで実行可能な並進速度・角速度をリスト化します。

本来のDWAであれば、障害物に近づきすぎる（あるいは衝突する）速度の組み合わせは取り除くのですが、今回のロボットは障害物を検知するセンサを積んでいない（という設定）のため、この障害物回避処理は割愛しています。

```rust:src/planners/dwa_wo_obstacle.rs
/// **\[private\]** Get the sampled values of linear and angular velocities in the next Dynamic Window
///
/// ## Arguments
/// * `max_accelarations` - the tuple of (maximum linear accelaration, maximum angular accelaration)
/// * `linear_velocities` - the tuple of (maximum linear velocity, minimum linear velocity)
/// * `angular_velocities` - the tuple of (maximum angular velocity, minimum angular velocity)
/// * `current_input` - the current input vector(linear velocity, angular velocity) of this simulated robot
/// * `delta` - time delta to next tick
///
/// ## Returns
/// The tuple of sampled values (Vec of sampled linear velocities, Vec of sampled angular velocities)
fn get_window(max_accelarations: (f64, f64), linear_velocities: (f64, f64), angular_velocities: (f64, f64), current_input: &na::Vector2<f64>, delta: f64) -> (Vec<f64>, Vec<f64>) {
  let delta_v = max_accelarations.0 * delta;
  let delta_omega = max_accelarations.1 * delta;

  let min_v = linear_velocities.1.max(current_input[0] - delta_v);
  let max_v = linear_velocities.0.min(current_input[0] + delta_v);
  let min_omega = angular_velocities.1.max(current_input[1] - delta_omega);
  let max_omega = angular_velocities.0.min(current_input[1] + delta_omega);

  (
    utils::step_by_float(min_v, max_v, V_RESOLUTION),
    utils::step_by_float(min_omega, max_omega, OMEGA_RESOLUTION),
  )
}
```

### サンプリングされた並進速度・角速度で移動した場合の姿勢を評価
サンプリングした並進速度・角速度でロボットを移動させたと場合に、計算サイクル後にロボットがどのような姿勢 $(x, y, \\theta)$ を取るかを、ロボットの運動モデルを用いて予測します（実際にロボットを移動させた場合、不可避の誤差が乗りますが、それは見なかったことにします）。

```rust:src/planners/dwa_wo_obstacle.rs
...
  for (v, omega) in itertools::iproduct!(v_range, omega_range) {
    let input = na::Vector2::new(v, omega);
    let next = robot::ideal_move(current, &input, delta);
...
  }
```
サンプリングされた角並進速度・角速度によって予測した各姿勢を評価して、「最もイイカンジ」な姿勢を選びます。DWAの原論文では、３つの評価関数を組み合わせて評価すると提案されています。

```math
G(v, \omega) = \sigma(\alpha \cdot heading(v, \omega) + \beta \cdot dist(v, \omega) + \gamma \cdot velocity(v, \omega))
```

#### heading
原論文では、予測した姿勢でのロボットの向きと、その地点からゴールへと引いた線分の角度を180度から引いた値とされています。今回の実装では都合上、評価関数を最大化するのではなく最小化する方向で最適化したいので、180度から引かずそのままの値を使います。

```rust:src/planners/dwa_wo_obstacle.rs
/// **\[private\]** Evaluate whether the simulated robot are heading toward the goal
///
/// ## Arguments
/// * `next` -　the ideal pose(x, y, theta) moved by the input vector being considered
/// * `destination` - the goal pose(x, y, theta)
///
/// ## Returns
/// The angle between the direction of the robot moved by the input vector considered and the position of the goal
fn eval_heading(next: &na::Vector3<f64>, destination: &na::Vector3<f64>) -> f64 {
  let angle = (destination[1] - next[1]).atan2(destination[0] - next[0]);
  utils::normalize_angle(angle - next[2]).abs()
}
```

#### dist
原論文では、最も近い障害物への距離とされています。今回の実装では障害物はキニシナイことになっているので、この項は無視します。

#### velocity
原論文では、シンプルに並進速度です。並進速度が高いほど、がんばってゴールに向かっているということですね。今回の実装では最小化する方向で最適化するので、ロボットの最大並進速度からその並進速度を引くことにします。

```rust:src/planners/dwa_wo_obstacle.rs
/// **\[private\]** Evaluate the simulated robot's speed toward the goal
///
/// ## Arguments
/// * `input` - the input vector(linear velocity, angular velocity) being considered
///
/// ## Returns
/// The value subtracting input linear velocity from maximum linear velocity
fn eval_velocity(input: &na::Vector2<f64>) -> f64 {
  robot::MAX_V - input[0]
}
```

#### distanceとtheta
残念ながら、正方形を描くルールでの各頂点近傍での動作のように、ロボットの並進速度が微小でその場で超信地旋回するような状況では、原論文で提案されている $heading$ と $velocity$ だけでは上手く適切な入力を選び出すことができませんでした。そこで今回の実装では、 $distance$ と $theta$ という２つの評価関数を追加しています。

$distance$ はシンプルに、予測したロボットの位置からゴールの位置までのユークリッド距離です。ゴールに近いほうが良い、という価値観ですね（この項を入れるために、最小化する方向で最適化しています）。

```rust:src/planners/dwa_wo_obstacle.rs
/// **\[private\]** Evaluate the distance between the simulated robot and the goal
///
/// ## Arguments
/// * `next` -　the ideal pose(x, y, theta) moved by the input vector being considered
/// * `destination` - the goal pose(x, y, theta)
///
/// ## Returns
/// The distance between the position of the robot moved by the input vector considered and the position of the goal
fn eval_distance(next: &na::Vector3<f64>, destination: &na::Vector3<f64>) -> f64 {
  (next.fixed_rows::<2>(0) - destination.fixed_rows::<2>(0)).norm_squared().sqrt()
}
```
また　 $theta$ もシンプルで、予測した姿勢でのロボットの向きとゴールの向きの差です。ゴールとして指定されている方向に向いている方が良い、ということですね。

```rust:src/planners/dwa_wo_obstacle.rs
/// **\[private\]** Evaluate whether the direction of the simulated robot and the direction of the goal match
///
/// ## Arguments
/// * `next` -　the ideal pose(x, y, theta) moved by the input vector being considered
/// * `destination` - the goal pose(x, y, theta)
///
/// ## Returns
/// The absolute value between the direction of the robot moved by the input vector considered and the direction of the goal
fn eval_theta(next: &na::Vector3<f64>, destination: &na::Vector3<f64>) -> f64 {
  utils::normalize_angle(next[2] - destination[2]).abs()
}
```

### サンプリングされた並進速度・角速度から最適な組み合わせを抽出
上記の評価関数から得られるの生の値はスケールがバラバラですので、 $[0, 1]$ の区間に正規化し、それぞれの評価関数に重みを付けて足し合わせ、最小となる並進速度・角速度の組み合わせを選び出します。

```math
G^{\prime}(v, \omega) = \alpha \cdot heading(v, \omega) + \gamma \cdot velocity(v, \omega) + \delta \cdot distance(v, \omega) + \epsilon \cdot theta(v, \omega))
```

各評価関数の結果を正規化するために、並進速度と角速度の組み合わせごとに計算した各評価値を一旦vecにpushし、各評価関数ごとに正規化した後に改めて、重み付けした正規化済評価値を足し合わせて評価値を計算しています。このあたり、ループが２回回っていて気持ち悪いのですが、なにか良い手はないものか・・・

```rust:src/planners/dwa_wo_obstacle.rs
/// Get the input vector (linear velocity, angular velocity) of next tick based on the goal of next tick and the current pose and input vector of the simulated robot
///
/// ## Arguments
/// * `agent` - the agent instance of this simulated robot
/// * `current` - the current pose(x, y, theta) of this simulated robot
/// * `destination` - the goal pose(x, y, theta)
/// * `current_input` - the current input vector(linear velocity, angular velocity) of this simulated robot
/// * `delta` - time delta to next tick
///
/// ## Returns
/// * The input vector(linear velocity, angular velocity) of next tick
pub fn get_input(agent: &Box<dyn Agent>, current: &na::Vector3<f64>, destination: &na::Vector3<f64>,
                 current_input: &na::Vector2<f64>, delta: f64) -> na::Vector2<f64> {
...
  for (v, omega) in itertools::iproduct!(v_range, omega_range) {
    let input = na::Vector2::new(v, omega);
    input_vec.push(input);

    let next = robot::ideal_move(current, &input, delta);
    heading_vec.push(eval_heading(&next, destination));
    velocity_vec.push(eval_velocity(&input));
    distance_vec.push(eval_distance(&next, destination));
    theta_vec.push(eval_theta(&next, destination));
  }

  let heading_vec = utils::normalize_min_max(heading_vec);
  let velocity_vec = utils::normalize_min_max(velocity_vec);
  let distance_vec = utils::normalize_min_max(distance_vec);
  let theta_vec = utils::normalize_min_max(theta_vec);

  let min_idx = itertools::izip!(&heading_vec, &velocity_vec, &distance_vec, &theta_vec)
                .map(|(heading, velocity, distance, theta)|
                  if (current.fixed_rows::<2>(0) - destination.fixed_rows::<2>(0)).norm_squared() < DISTANCE_SQUARED_THRESHOLD {
                    NEAR_ERROR_ANGLE_GAIN * heading + NEAR_VELOCITY_GAIN * velocity + NEAR_DISTANCE_GAIN * distance + NEAR_THETA_GAIN * theta
                  } else {
                    FAR_ERROR_ANGLE_GAIN * heading + FAR_VELOCITY_GAIN * velocity + FAR_DISTANCE_GAIN * distance + FAR_THETA_GAIN * theta
                  }
                )
                .enumerate()
                .min_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal))
                .map(|(idx, _)| idx)
                .unwrap();

  input_vec[min_idx]
}
```

このDWAもどきによって、推定された現在位置からターゲットへ向かう最適な入力（並進速度・角速度）が得られます。ただ実際に動作させようとすると、サンプリングレートや各評価関数の重みをチューニングしなければなりません。これはトライアンドエラーが必要で、結構面倒です（上記にあるように、今回の実装ではロボットがターゲット至近に居る場合とそうでない場合で、評価関数の重みを切り替えてたりします）。
DWAは数学的な背景に裏打ちされたものではないので、仕方ないところかもしれませんね。

# 雑感1
[Nalgebra](https://docs.rs/nalgebra/)便利ですね。普通の行列演算のような演算子を用いてプログラム化できますし。ただ偉大な[Numpy](https://numpy.org/)を使いこなせている方にとっては、[ndarray](https://docs.rs/ndarray/)の方がスムーズに移行できるのかもしれません。私は修行が足りないので、評価できませんが・・・

# 雑感2
最初にPython Numpyで書いたときには、以下のような普通のオブジェクト指向的な方針で実装しました。

1. 抽象クラスの [Agent](https://github.com/nmatsui/robot_simulator/blob/main/src/agent.py#L8) を準備
1. ロボットの具体的な挙動を [CircularAgent](https://github.com/nmatsui/robot_simulator/blob/main/circular_agent.py#L14) のような具象クラスで定義
1. 自己位置推定や局所経路探索では、具体的な実装を意識せず、Agent型として扱う

このオブジェクト指向的な実装をそのままRustへ移植しようとしたところ、なんだかんだで非常に苦労しました。
最終的には [手続き的マクロ](https://github.com/nmatsui/robot_simulator_rust/blob/main/agent_derive/src/lib.rs) も併用した Factory Method 的な実装で、[CircularAgent](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/agent/circular_agent.rs#L17) のような具体的な実装を隠蔽し [Agent](https://github.com/nmatsui/robot_simulator_rust/blob/main/src/agent.rs#131) のtrait objectで抽象的に操作できるようにしましたが、これが正しい道だったかと言われると微妙な気がします。無駄に複雑で。
まだまだ Rust way が身についてないですね。

# 参考資料
* [Rust] Nalgebra入門(ndarrayとの比較付き) https://qiita.com/ttttkkkkk31525/items/613156f9007c84de1ede
* 東北学院大学 工学部 機械知能工学科 RDE 熊谷正朗 車輪移動ロボット http://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/wheelrobot.html
* Extended Kalman Filter https://en.wikipedia.org/wiki/Extended_Kalman_filter
* ロボティクスにおける経路計画(Path planning and Motion planning)技術の概要 https://myenigma.hatenablog.com/entry/2017/07/23/095511
* base_local_planner http://wiki.ros.org/base_local_planner
* DWA（Dynamic Window Approach）についてまとめてみた https://qiita.com/MENDY/items/16343a00d37d14234437
* D.Fox; W.Burgard; S.Thrun, The dynamic window approach to collision avoidance, IEEE Robotics & Automation Magazine ( Volume: 4, Issue: 1, Mar 1997)
* その他いっぱいありますがメモってない・・・