制御

🔗概要

• EKF(Extended Kalman Filter)の立式方法について。
  • カルマンフィルタは、状態量の平均値と分散共分散行列を推定

🔗参考

• 確率ロボティクスのExtended Kalman Filterの欄。
• ジャイロドリフトを含む姿勢推定
• 状態遷移行列の離散化
• カルマンフィルタとオブザーバの説明
• 一次元の場合のわかりやすい立式

🔗手順

• センサ情報を、状態量に対して位置的な役割になりそうなものをz, 速度的な役割になりそうなものをuと振り分ける。
• 観測方程式をたてる
  • 観測方程式のうち、カルマンフィルタで推定したくない観測可能な項を移行したものをzとする。
  • 観測方程式を移行した後、残った推定したい項に含まれる推定したい変数を状態muとする。
  • 観測方程式の関数をh(mu)をする。h(mu)のヤコビアンHを計算する。
• 予測方程式をたてる
  • muのシステム方程式g(mu, u)を立式する。
  • システム方程式を離散化する
  • g(mu, u)のmuに関するヤコビアンである状態遷移行列G, g(mu, u)のuに関するヤコビアンV、M=diag(\alpha u)を定義する。
• カルマンフィルタの予測・観測段を書き下し、必要に応じて単純化する。

🔗用語

状態量: 推定する目的の確率変数のベクトル。$x$で表す。

平均値: この文書の文脈では、状態量の平均。$\mu$で表す。

分散共分散行列: このページの文脈では、状態量の分散共分散行列。$\Sigma$で表す。

制御量: 状態量の速度の観測に関する、確率変数のこと。$u$で表す。

状態遷移関数: ある制御量が新しく入力された時、以前の平均値とその制御量によって、どの平均値に移るかを推定するベクトル関数。$\mu_t = g(\mu_{t-1}, u)$で表す。

予測段: カルマンフィルタは二つのステップに完全に独立に分かれている。その内、制御量を用いて、平均値と分散共分散を更新するステップ。

観測量: センサによる観測に関する、確率変数のこと。$z$で表す。

観測関数: 平均値と何らかモデルを入力にして、理想的な観測を計算するための関数。$h(\mu)$で表す。モデルを明示する場合は、$h(\mu, m)$とすることもある。

観測段: カルマンフィルタは二つのステップに完全に独立に分かれている。その内、観測量を用いて、平均値と分散共分散行列を更新するステップ。

🔗イメージ

状態量: ロボットの位置(x, y, theta)の確率変数

平均値: ロボットの自己位置の中で最も尤もらしい場所。これがいわゆるオドメトリとして扱われる。

分散共分散行列: ロボットの自己位置の不確かさ。例えば、ロボットが「自分は大体x方向に関して標準偏差5cmくらいはずれてるかも…」とか思ってることを表現している。

制御量: エンコーダの情報。確かにこれらはx, y, thetaの速度を司る。

状態遷移関数: オドメトリ更新式。これは、「一つ前のオドメトリとエンコーダ」を入力に、「直後のオドメトリ」を得るベクトル関数である。

予測段: エンコーダによる更新。平均値と分散共分散行列の更新の2本の式に依るが、今までの更新式はまさに平均値の更新にあたる。

観測量: 測距センサによる観測や、カメラや測域センサによるランドマーク検出などによる距離情報や角度情報。

観測関数: オドメトリと測距センサがついている位置から、センサの出力の理想的な出力を計算するための関数。

観測段: 測距、カメラ、測域などによる情報を組みこむ更新。

🔗予測段

予測段では、状態量の速度に関する情報を更新に組みます。

🔗時間の添え字について=

更新前の状態量: $\mu_{t-1}$

更新後の状態量: $\mu_t$

更新に使う制御量: $u_t$

時刻tで得られた制御量によって、時刻t-1の状態量が、時刻tの状態量に更新される、というイメージです。

🔗式

式は以下のとおりです。

G: $\mu_{t-1}$に関するヤコビ行列。$G = \frac{\partial \mu_t}{\partial \mu_{t-1}}$

R: $\mu_t$の分散共分散行列。その1回の予測により、どれくらい状態量の不確かさが増加するのか、を表す。

$\mu_t = g(\mu_{t-1}, u)$

$\Sigma_t = G \Sigma_{t-1} G^t + R$ …(1)

🔗例: 二輪ロボットのオドメトリ

状態量と制御量を以下のように設定します。

$\mu_t = \begin{pmatrix} x_t \\ y_t \\ \theta_t \end{pmatrix}$

$u_t = \begin{pmatrix} v_t \\ \omega_t \end{pmatrix}$

gを資料から読み解きます。

$\begin{pmatrix} x_t \\ y_t \\ \theta_t \end{pmatrix} = \begin{pmatrix} x_{t-1} + v_t \Delta t \cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ y_{t-1} + v_t \Delta t \sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ \theta_{t-1} + \omega \Delta t \end{pmatrix}$

Gを、gを$x_{t-1}$で偏微分することで求めます。

$G = \begin{pmatrix} 1 & 0 & - v_t \Delta t \sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ 0 & 1 & v_t \Delta t \cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ 0 & 0 & 1 \end{pmatrix}$

Rに相当する行列を求めます。

ロボットの場合、一回の予測ステップの不確かさを生みだすのは、制御量に他ならないので、制御量の分散共分散行列Mを仮定し、それをヤコビ行列によって状態量の分散共分散行列に射影します。

制御量による状態量の分散共分散行列は、状態量の制御量に関するヤコビ行列$V = \frac{\partial \mu_t}{\partial u_t}$を用いて、$V M V^t$と表されます。…(2)

$M = \begin{pmatrix} \alpha_1 v_t^2 + \alpha_2 \omega_t^2 & 0 \\ 0 & \alpha_3 v_t^2 + \alpha_4 \omega_t^2 \end{pmatrix}$

$V = \begin{pmatrix} \Delta t cos(\omega_t \Delta t / 2 + \theta_{t-1}) & - v_t \Delta t ^ 2 / 2 sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ \Delta t sin(\omega_t \Delta t / 2 + \theta_{t-1}) & v_t \Delta t ^ 2 / 2 cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ 0 & 1 \end{pmatrix}$

$R = V M V^t$

以上から、予測段は以下のように立式されます。

$\begin{pmatrix} x_t \\ y_t \\ \theta_t \end{pmatrix} = \begin{pmatrix} x_{t-1} + v_t \Delta t \cos(\omega_t \Delta t / 2 + \theta_{t-1}) \\ y_{t-1} + v_t \Delta t \sin(\omega_t \Delta t / 2 + \theta_{t-1}) \\ \theta_{t-1} + \omega_t \Delta t \end{pmatrix}$

$\Sigma_t = G \Sigma_t G^t + V M V^t$…(3)

🔗Tips

🔗分散の線形射影

(1), (2)では以下の事実を利用しています。

線形空間Xのある正規分布が、平均$\mu_X$分散共分散行列を$\Sigma_X$

線形空間Yが存在する。

XからYへの線形変換行列Aが存在する。

Xの正規分布をAによって射影した時の、Yの正規分布は、平均$\mu_Y$分散共分散行列を$\Sigma_Y$とする。

この時、

$\mu_Y = A \mu_X$

$\Sigma_Y = A \Sigma_X A^t$

である。

🔗分散の和

(3)は以下のような事実から本当は明らかです。

関数gが射影しうる分散は、(x, y, theta, v, omega)から来るはずです。

「Tips/分散の線形射影」を用いれば、本当ならばAは5*5の行列になるはずです。

これは「ひとつ前の状態量の分散共分散行列と、今の制御量の分散共分散行列は独立である。」という仮定により分解されています。

つまり、状態の分散共分散と、状態の速度の分散共分散に関係があるわけがない、という信念により、カルマンフィルタのもとの式でもRは別扱いされているのです。

🔗予測段は簡単

予測段は僕でも導出できるくらいに簡単なものです。

カルマンフィルタの真骨頂は観測段にあります。

🔗観測段

観測段では、「状態量にとって意味のある観測」を更新に組みます。

🔗状態量にとって意味のある観測=

まず、「状態量にとって意味のある観測」とは何か、ということについて書きます。

「状態量にとって意味のある観測」とは、「状態量とモデルから、理想的な観測量が計算できる」という意味です。…(4)

例えば、

四角い枠に囲まれたフィールドにロボットがいます。

ある壁に対して、測距センサで距離を測ったとします。

この場合、ロボットのオドメトリと測距センサが付いている位置によって、当然理想的な観測量は計算可能できるはずです。…(5)

🔗時間の添え字について

更新前の状態量: $\mu_{t-1}$

更新後の状態量: $\mu_t$

更新に使う観測量: $z_t$

時刻tで得られた観測量によって、時刻t-1の状態量が、時刻tの状態量に更新される、というイメージです。

🔗式=

以上のことを踏まえて、式を記述します。

H: hの$\mu_{t-1}$に関する偏微分。$\frac{\partial h}{\partial \mu_{t-1}}$

Q: $z_t - h(\mu_{t-1})$の分散共分散行列

$K=\Sigma_{t-1} H^t (H \Sigma_{t-1} H^t + Q)^{-1}$

$\mu_t = \mu_{t-1} + K (z - h(\mu_{t-1}))$

$\Sigma_t = (I - K H) \Sigma_{t-1}$

Kは一般に「カルマンゲイン」と呼ばれています。

式を見れば、これは「状態量からの理想的な観測量と、実際の観測量の差分」を反映させた、フィードバックゲインとなっていることがわかります。

🔗例: 点ランドマークベースの更新=

状態量と実観測量と理想観測量を以下のように設定します。

$\mu = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix}$

$m_x, m_y$: フィールド座標での点ランドマークの実際のx, y。

$z = \begin{pmatrix} r \\ \phi \end{pmatrix}$: 観測された、マシン中心から見た点ランドマークとの距離と角度。

$\hat{z} = h(\mu_{t-1})$: 状態量から算出された理想的な、マシン中心から見た点ランドマークとの距離と角度。

hを立式します。

$q = (m_x - x_{t-1})^2 + (m_y - y_{t-1})^2$

$\hat{z} = h(\mu_{t-1}) = \begin{pmatrix} \sqrt{q} \\ norm(atan2(m_y - y_{t-1}, m_x - x_{t-1}) - \theta_{t-1}) \end{pmatrix}$

ただしnormは、角度を[-pi, pi]に丸める関数。

Hをhを$\mu_{t-1}$で偏微分して求めます。

$H = \frac{\partial h}{\partial \mu_{t-1}} = \begin{pmatrix} -(m_x - x_{t-1}) / \sqrt{q} & -(m_y - y_{t-1}) / \sqrt{q} & 0 \\ (m_y - y_{t-1}) / q & -(m_x - x_{t-1}) / q & 1 \end{pmatrix}$

Qを設定します。$r, \phi$は互いに独立だと仮定して、ランドマークの観測と理想位置とのズレの分散共分散行列は、

$Q = \begin{pmatrix} Q_r & 0 \\ 0 & Q_\theta \end{pmatrix}$

以上より、観測段の更新式は、

$K=\Sigma_{t-1} H^t (H \Sigma_{t-1} H^t + Q)^{-1}$

$\mu_t = \mu_{t-1} + K (z - h(\mu_{t-1}))$

$\Sigma_t = (I - K H) \Sigma_{t-1}$

🔗Tips

🔗状態量にとって意味のある観測の数学的な記述

(4)について、直感的な説明だけにとどめたかったので厳密さに欠くことを書きましたが、実際には、観測量=f(状態量, モデル)、ただし状態量について一階偏微分可能を良く近似するfが存在することが条件となります。

🔗状態量にとって意味のない観測の例

(5)について、意味のない観測の例は下らないのでTipsに。

四角い枠に囲まれたフィールドにロボットがいます。

この時、工学部二号館8階の自動ドアの測距センサが、床もしくは通行人との距離を測ったとします。

この場合、ロボットのオドメトリと自動ドアの測距センサが付いている位置によって、当然自動ドアの理想的な計算量を推定することはできません。

🔗点ランドマークが同時に複数見つかった場合

上記の場合では、点ランドマークが一つしか見つかっていない状況しか想定していません。

もしLRFなどで複数のランドマークが同時に見つかった場合は、「それぞれ別々に」上記の更新を行えばよいです。

つまり、3つランドマークが見つかったのならば、3回カルマンゲインを更新して、3回更新をかけることになります。

🔗Tips

🔗ヤコビ行列=

ベクトル関数のベクトルによる偏微分係数をヤコビ行列と呼びます。

ベクトル関数のベクトルによる偏微分係数の定義を知らない人のために、以下に定義を書きます。

x: n次元実数ベクトル変数

f: n次元実数ベクトルからm次元実数ベクトルへのベクトル関数

$\frac{\partial f}{\partial x} = \begin{pmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{pmatrix}$

🔗観測偏差の分散共分散行列のチューニング方法

🔗基本

• チューニングパラメータは以下である。
• 理想観測と実観測の差分(=観測偏差)の分散共分散行列
• 同定にあたって重要なパラメータは、$\Delta z_t = z_t - \hat{z}_t, \Delta　\mu = K \Delta z_t$である。
  • これらはすぐに確認してplotできるような環境を整えてください。

🔗方法

• $\Delta z_t$をプロットし、その最小値と最大値の半分が1SDになるように分散共分散行列を構成する。
  • 例えば、理想ランドマーク位置までが100mm, 測域センサによる観測が94-108mmだったとする。
  • すると、1SDを(108-94)/2= 14 mmなので、分散は14^2 = 196 mm^2に設定すればよい。
• なぜ半分を1SDにするのか？
  • 本当であれば最大値と最小値を3SD (=99%域)にセットしても良いはずである。
  • 実際のデータはこの観測から大きく外れることが想定される。
  • 中央値がずれる可能性もある、外れ値が出る可能性もある。
• その上、この設定した分散が大きすぎてもパフォーマンスが下がるだけだが、小さすぎると暴走・振動する可能性がある。
  • 基本的にロボットって振動して欲しくないので、安全線として1SDとするのが良いと思う。
  • もっとパフォーマンスをあげようとか思った時に、ちょっとずつ分散共分散行列を下げていけば良い。

🔗初期状態の分散共分散行列

🔗基本

• 大きいと観測による補正がかかりやすくなる

  • これは、自分の位置がよくわからないから、比較的観測を信用することを意味する。
  • 極端に大きいと(Σ=∞)、藁にもすがるように、自分の位置を完全に無視して観測ばかり反映することになる。

• 小さいと観測による補正がかかりにくくなる。

  • これは、自分の位置を信用していて、観測にぶれない自分を持っていることを意味する。
  • 極端に小さいと(Σ=0)、自分が絶対的に正しいと思っているので、どんな観測がきてもその補正量は0となり無視される。

🔗方法

• 補正の信頼性が比較的高い場合

  • 測域など
  • 基本的にはありえないくらいに大きく。
  • 例えば、ロボットの初期位置だったら1SD=±1mとか。
  • 初期位置なんて1cmもずれるわけないが、これくらいに設定しておく。
  • すると、はじめの観測をほぼ信頼して、ロボットの位置が補正される。

• 補正の信頼度が比較的低い場合

  • 測距など
  • ロボットの初期位置の人間の手によるズレのうち、考えられる最小と最大の差の1/2を1SDとするような分散共分散行列にする。
  • 例えば、人間の手でリトライをかけるときに、±(10 mm, 10 mm, 5 mrad)の誤差が考えられるとする。
  • そうしたら、Var = (100 mm^2, 100 mm^2, 25 mrad^2)のように初期状態を設定する。

🔗初期状態を大きくしたら暴走する場合

• 初期状態をいくら大きくしたら暴走するのかを確認する
  • 理論上は、観測関数の線形化がうまく行っていれば、いくら大きくしても暴走することはないはず
  • このような状態になることはほぼありえないので、パラメータチューニングとは別の要因を探すべき
    • 例えば更新にかかる計算時間が遅く、up to dateな更新がかけられていないとか→高速化する
• 1つだけでは不定性のある観測をやたら信頼して更新をかけているとか→2つ見えた時にだけ更新をかける