gggggraziegrazie

graizegrazieさんのやったこと、学んだことを記録する雑記帳です

ヒューリスティックアルゴリズム

ヒューリスティックアルゴリズムと聞くことがあるが、何を意味するのかずっとわからないままだった。そのため今回ヒューリスティックアルゴリズムが何を指すのか調べて見た。その結果、

だとわかった。つまりヒューリスティックアルゴリズムとは、経験則から導き出した・考え出したソフトウェアの処理手順・方式である。

フィッシャーの情報量(情報行列)

(シャノンの)情報量

 情報量といった場合、シャノンの情報量を指すことが多い。そもそも情報量とは、ある事象が起こった時、その事象の発生がどれだけ珍しいかを表す量である。事情が珍しい程、値は大きくなる。実生活においても、珍しい出来事が起こった場合、そのインパクトは大きい。そのため、直感的にも納得しやすい定義に感じる。
 情報量には「自己情報量」と「平均情報量」があり、それぞれ

  • 自己情報量:

 ある事象xについての情報量で、下式(1)で表現される。
 {I(x) \  = \  -log_2(P(x)) \tag{1} }
 ここで{P(x)}は事象の発生確率を表すものとする。

  • 平均情報量:

 ある確率分布についての情報量で、下式(2)で表現される。エントロピーとも言われる。
 {\displaystyle H(P) \  = \  - \sum_{x \in \Omega} P(x) log_2(P(x)) \tag{2} }

フィッシャー情報量(情報行列)

 一方フィッシャー情報量とは、シャノンが定義した情報量とは異なり、尤度に注目した情報量である。対数尤度関数の二階の導関数の値の絶対値で表し、値が大きいほど情報量が高い(珍しい)となる。事象が1変量の場合は、"情報量"と呼称する。事象が多変量の場合は、"情報行列"となる。

Probabilistic Robotics - Chap.10 Simultaneous Localization and Mapping

10.1 Introduction

SLAM問題は、環境地図へのアクセスが出来ない且つ自分の位置もわからない時に発生する
ただしこの時、全ての観測ベクトル{z_{1:t}}と制御ベクトル{u_{1:t}}は既知とする。

SLAM問題にはオンラインSLAMとフルSLAMの2種類ある。

  • オンラインSLAM:ロボットのある瞬間の姿勢についての事後分布を求める

 事後分布
{p(x_t, m | z_{1:t}, u_{1:t}) \tag{10.1}}
の推定には、時刻{t}のみ考慮する。多くの場合、オンラインSLAMのアルゴリズムはインクリメンタルである。

  • フルSLAM:ロボットの移動経路ついての事後分布を求める

 SLAM開始時点から現時点までの姿勢全てを考慮した事後分布を求める。SLAM開始時点から現時点までの姿勢の列は、ロボットが現在地に到達するまでにとった姿勢の集合を意味する。つまり、事後分布
{p(x_{1:t}, m | z_{1:t}, u_{1:t}) \tag{10.2}}
は、ロボットの移動経路の事後分布を意味する。

SLAMの重要な特徴の2つ目は、推定問題の本質の部分
 地図上の物体の位置やロボット自身の姿勢変数を維持する必要がある

EKF Localizationと同様に、SLAMを連続で行うには一致変数を使うとよい。一致変数を導入することで、{式(10.1)、(10.2)}は、下記のようになる。
{(10.1) \rightarrow p(x_t, m, c_t | z_{1:t}, u_{1:t}) \tag{10.4}}
{(10.2) \rightarrow p(x_{1:t}, m , c_{1:t}| z_{1:t}, u_{1:t}) \tag{10.5}}

ただしフルSLAMを続けるのは下記2点のため、演算量・メモリ使用量が膨大になり難しい。
1)変数(Ex. {x_{1:t}})の次元が高次になる
2)一致変数の次元が膨大になる(時間と共に指数的に大きくなるので、実際には近似を用いる必要がある)

10.2 SLAM with Extended Kalman Filters

10.2.1 Setup and Assumptions

本章では、ランドマークと特徴量の一致がわかっていると仮定。
EKF SLAMはオンラインSLMAの一種で、最尤データの関連付けを行う。
EKF SLAMはfeature mapを使用しており、map中のランドマーク数は少ない(1000以下)ことが多い。
EKFを使う場合、対象(ランドマーク)の特徴がハッキリしているほど良い結果を残す。
EKF SLAMでは、ロボットの運動・知覚にガウスノイズを仮定する。

10.2.2 SLAM with Known Correspondence

一致がわかった状態のSLAMは、SLAMの連続的な部分しか言わない
EKF Localizationとの重要な違いの1つは、EKF SLAMは経路上の全てのランドマークの座標を推定する点
結合状態ベクトル{y_t = \left( \begin{matrix} x_t \\ m \end{matrix} \right) \\
= ( x \  y \  \theta \  m_{1, x} \  m_{1, y} \  s_1 \  ... \  m_{N,x} \  m_{N,y} \  s_N )^T
}
ここで、{s_i}はランドマークのIDである。
EKF SLAMは、オンライン事後分布{p(y_t | z_{1:t}, u_{1:t}}を求める

SLAMで求める事後分布の重要な特徴は、ロボットの姿勢に関する情報を得ると、その情報は以前に観測したランドマークへも波及することである。
つまりロボットの姿勢推定結果が改善されると、以前観測したランドマークの位置の確からしさも改善される。

10.3 EKF SLAM with Unknown Correspondences

10.3.1 The General EKF SLAM Algorithm

前章ではランドマークと特徴量の一致がわかっていると仮定したが、本章ではその仮定を取り去った一般EKF SLAMを取り上げる。
一般EKF SLAMでは、インクリメンタルな最尤推定器が一致を決める。
一般EKF SLAMでは、一致が未知のため、入力から一致変数{c_t}が消える
その代わり、その時のランドマーク数{N_{t-1}}を入力する
運動モデルの更新手順は10.2と同じだが、観測モデルの更新手順は異なる
新規ランドマークは、既知のランドマークとのマハラノビス距離が閾値を超えた場合に生成する
最終的に一般EKF SLAMは、新しいランドマーク数{N_t}と、その時の平均{\mu_t}、共分散{\Sigma_t}を出力する

10.3.3 Feature Selection and Map Management

実際にEKF SLAMをロバストにするには、地図管理のための技術が追加で必要になる。
それらの技術はガウスノイズは非現実的であり、ノイズ分布の最後は嘘の観測結果が求まることに関係する。
嘘の観測結果により、嘘のランドマークが求まる。
そうした外れ値の除外する上で最も簡単な方法は、仮のランドマークリストを作ることである。
検出したランドマークは、まずは仮のリストに登録する。
このリスト中のランドマークは、ロボットの姿勢のadjustには使わない。
リスト中のランドマークが継続的に観測され、不確かさが小さくなると、通常の地図に組み込む。
この手法は、物理的に存在するランドマークは残しつつも、地図中のランドマーク数を減らす傾向がある。

問題の救済には、下記2つの手法を用いることが多い。

  • 空間整理

ランドマーク間の距離が遠ざかると、ランドマークを取り違える可能性が減る。
またランドマークが増えすぎると、取り違える可能性が高まる。
一方ランドマークが少なすぎると、取り違える可能性が高まるだけでなく、ロボットの自己位置推定が困難になる。

  • 記号

物体は固有の色や寸法などを持っている。これらを使って識別すると便利。

EKF SLAMの重要な制限事項は、正確なランドマークを選択する必要がある点である。
EKF SLAMは、通常大量のセンサ情報を破棄する(使わずに垂れ流す)ため、情報の欠除を招く。
これにより、地図上の特徴量が{10^6}個以上でないと、EKF SLAMは使えない。
比較的低次元のマップは、データの関連付けが困難な傾向がある。
インクリメンタルな最尤データ関連付けは、大量の特徴量を持った密な地図で動作するが、特徴量が少ない地図では不安定化する傾向がある。
しかしEKFは、密な地図では地図更新が複雑になるため、疎な地図を求めている。

論文紹介:Information-based Active SLAM via Topological Feature Graphs

どんなもの

 Topological Feature Graph(TFG)を使ったActive SLAMの提案。占有格子地図(Occupancy Grid Map)に比べ、地図のスケールの精度や長時間実行時のドリフトが少ない。

先行研究と比べてどこがすごい?

 特徴量ベースなトポロジカルマップでActive SLAMを可能にしたこと。

Active SLAM

 未知領域に対し、ロボット自身がパスプランニングして行うSLAM

技術の手法やキモはどこ?

 

どうやって有効と検証?

 シミュレーション上で、変数の数やCPUのアイドル時間、位置・姿勢の誤差を比較した。姿勢の誤差を除き、全て占有格子地図を使ったActive SLAM(nearest-frontier exploration algorithm)よりもよい性能を示した。

議論はある?

 比較対象が、占有格子地図(メトリックマップタイプの地図)のSLAMであるため、TFGと同じトポロジカルマップのActive SLAMと比べる必要がある。

次に読むべき論文?

 今は思いつかず

Probabilistic Robotics - Chap.9 Occupancy Grid Mapping

9.1 Introduction

 今まではロボットが動作を開始する前に、環境地図が既知であることを仮定していた。ただこれは現実世界では稀なケースである。そこでロボットが自律的に環境地図を知る必要がある。ただし、環境地図の獲得には、下記2つの理由から困難な課題の1つである。

  • 地図の候補空間は巨大である
  • 地図を得ることは"鶏と卵"の問題がある

困難度合いを決める要因には、下記4つがある。

  1. 地図のサイズ(地図を作る上で対象となる空間の大きさ)
  2. 認識・動作におけるノイズの程度
  3. 認識の曖昧さ(特徴量が少ない、似たような光景が広がっているなど)
  4. 周回路(ループクロージャの問題)

 本章では、ロボットの姿勢は既知と仮定({mappig \  with \  known \  pose}問題)。Grid Mappingは、ロボットの姿勢が既知という仮定の元、ノイズを含む不確かなセンサデータから環境地図を生成する技術である。地図中の各Gridは、{occupied}{free}のどちらかの値を取る。どちらの値を取るかは、近似的に実装する事後確率で表現する。
Occupancy Grid Mapは、SLAMやパスプランニングに使うことは少ない。

9.2 The Occupancy Grid Mapping Algorithm

環境地図に関する事後確率は
{
p(m | z_{1:t}, x_{1:t}) \tag{9.1}
}
で表される。ここで{x_{1:t}}は、計測開始後から現在までのロボットの経路(各時刻でのロボットの姿勢)を、{z_{1:t}}は計測開始後から現在までのセンサデータを、それぞれ表す。
なおOccupancy Grid Mapは一般的に2次元の地図を表すが、何層にも重ねることで3次元の地図を形成することもできる。ただし計算コストが膨大なため、通常Occupancy Grid Mapで3次元地図は構成しない。

環境地図{m}は、沢山のセルで構成されることから、
{
m \  = \  \{m_i\}
}
とできる。このとき{m_i \in \{0, \  1\}}{p(m_i \  = \  1) \  = \  p(m_i)}とする。

式(9.1)の問題は、次元数である。もしも地図が10,000個のセルで構成される場合、地図の次元は{2^{10000}}となる。そのため、地図がセルの集合体だと考えると、計算量が膨大となる。

そこで標準的なOccupancy Grid Mapは、各セルに注目して事後確率を計算する。つまり{p(m_i | z_{1:t}, x_{1:t})}を計算し、
{
\displaystyle p(m | z_{1:t}, x_{1:t}) = \prod_i p(m_i | z_{1:t}, x_{1:t})
}
のように統合することで、環境地図の事後確率を求める。

Occupancy Grid Mappingでは、各グリッドの状態を表すのに下記式(9.5)で表される対数オッズを使う。
{
\displaystyle l_{t, i} \  = \  log \frac{p(m_i | z_{1:t}, x_{1:t})}{1 \  - \  p(m_i | z_{1:t}, x_{1:t})} \tag{9.5}
}
対数オッズを使うメリットは、事後確率が計算結果が不安定になる0、1の近傍を取らなくなることである。

{
1: \  Algorithm \  occupancy\_grid\_mapping(\{l_{t-1, i}\}, x_t, z_t): \\
2: \  \quad  for\  all \  cells \  m_i \  do \\
3: \  \qquad if \  \  m_i \  \  in \  perceptual \  field \  of \  z_t \  then \\
4: \  \qquad  \quad l_{t, i} = l_{t-1, i} + inverse\_sensor\_model(m_i, x_t, z_t) - l_0 \\
5: \  \qquad else \\
6: \  \qquad  \quad l_{t,i} = l_{t-1, i}\\
7: \  \qquad endif \\
8: \  \quad  endfor \\
9: \  \quad return \{l_{t,i}\}
}

なお式中の{l_0}は、対数オッズで表現されるOccupancy Grid Mapの事前分布で、
{
\displaystyle l_0 = log \frac{p(m_i \  = \  1)}{p(m_i \  = \  0)} \  = \  log \frac{p(m_i)}{1 \  - \  p(m_i)}
}
とする。ここで、{p(m_i) \  = \  p(m_i \  = \  1)}であり、
{
\displaystyle inverse\_sensor\_model(m_i, x_t, z_t) \  = \  log \frac{p(m_i | z_t, x_t)}{1 - p(m_i | z_t, x_t)}
}
である。

なお、Occupancy Grid Mapを作成する時は、ロボット自身が専有する領域(ロボットの身体)を考慮することは大切である。特に通行人が多いとき等には有用である。

9.3 Learning Inverse Measurement Models

9.3.1 Inverting the Measurement Model

Occupancy Grid Mapは、各セルの逆観測モデル{p(m_i | x, z)}が必要である。
Table9.2ではとりあえずの逆観測モデルを定義した。
ここでいう逆観測モデルは、複数のマップを用意し、それぞれのマップの{m^k_i \  = \  m_i}を満たすマップを使って逆観測モデルを近似する。つまり、
{
\displaystyle p(m_i | x, z) \  = \  \eta \sum_{m:m(i)=m_i} p(z | x, m) p(m)
}
となる。上記の式は計算不可能のため、ロジスティクス回帰やニューラルネットワーク等の教師付き学習アルゴリズムを使って近似的に求める。

Probabilistic Robotics - Chap.6 Robot Perception

1.Introduction
6章に記載されているのはEnvironment Measurement Model(環境計測モデル)
確率ロボティクスではセンサデータの計測に伴うノイズもモデル化する

{ \displaystyle
p(z_t | x_t, m) \\
z_t:時刻tでのセンサデータ\\
x_t:時刻tでのロボットの位置\\
m:環境地図
}

確率ロボティクスでは、センサモデルの不正確さに対し、統計的手法を用いて対応する

{ \displaystyle
z_t = \{z^1_t, ..., z^K_t\}
z^k_t:センサAが時刻tにおいて計測した個々のデータ(LRFならば角度\thetaにおける距離)
}

{ \displaystyle
p(z_t | x_t, m) = \prod_{ k = 0 }^K p(z^k_t | x_t, m)
}
この式は、{p^k_t}がそれぞれ独立していることを仮定しているが、理想状態でしかそれはありえない。

6.2 Maps

地図{m}はオブジェクトのリストを表す
{\displaystyle
m = \{m_1, ... , m_N\}
}

地図には2種類ある

  • 特徴ベース:オブジェクトの位置は地図中に含まれている。形状情報のみ特徴として持つ。そのため、ロボット用のマップとしてはこちらの形式がよく使われる。
  • 位置ベース:{m}の要素は環境中に存在するオブジェクトの位置を表す。各オブジェクトの位置をデータとして保つ必要があるので、データサイズが大きい。空きスペースに関しての情報も保つ必要がある、つまり全セルの情報を保つ必要がある。

6.3 Beam Models of Range Finders

Range Finderは最もよく使われるセンサ

6.3.1 The Basic Measurement Algorithm

ここで紹介するモデルは4つのエラーを考慮する

  1. 分解能によって生じる誤差

正しい距離は計測できているものの、分解能の制約により誤差が乗る場合がある。
正しい距離を{z^{k*}_t}とする
本計測誤差は、平均{z^{k*}_t}、分散{\sigma_{hit}}で表される狭いガウス分布でモデル化できる。
{\sigma_{hit}}は本ノイズモデル固有のパラメータ

f:id:graziegrazie:20170405202747j:plain

  1. 予想外の物体

環境地図に元々含まれていない物体は、元々の地図に記載していたよりも短い距離で見つかることある
尤度は物体との距離に応じて減少していくことから、指数分布で表される。
この指数分布はモデル固有のパラメータ{\lambda_{short}}を使って表せる。

{
\begin{eqnarray}
p_{short}(z^k_t|x_t,m)
 =
  \begin{cases}
    \eta \lambda_{short} e^{-\lambda_{short} z^k_t} if 0 \gt z^k_t \gt z^{k*}_t \\
    0                                               otherwise
  \end{cases}
\end{eqnarray}
}

指数分布は自らの位置から本来の対象の位置までの尤度を表すため、累積確率は

  1. 計測失敗

計測可能レンジの超過
つまり計測値は最大値を示す

  1. ランダム計測

ごくたまに起きる説明の使いない計測結果

それぞれのエラーの密度関数は、それぞれ重みを掛けて合体することで、本モデルの確率密度関数を表現できる

Limitation of the Beam Model

Beamモデルには2つの主な欠点がある
滑らかさがない
 椅子を検知したら椅子の足それぞれマッピングするかもしれない
 ロボットの位置が変化したら、beamの座標変換の計算量が大変なことになる
滑らかさの欠如は2つの問題を招く
 beliefの近似が正しい状態を見失いやすい
 山登り法は局所最大値に陥りやすい
計算量が膨大

4 Liklihood Fields for Range Finders

6.3の制限を解決する尤度フィールドモデルについて述べる。
確率計算などはしないが、実際に使ってみると良い結果が求まる、アドホックアルゴリズム

キーとなるアイデアは、{z_t}の端点をグローバルマップ上に投影すること。
これにより、グローバルマップとロボットのローカルマップの位置関係がわかる。
投影される端点の座標は、
{
\left(
 \begin{array}{ccc}
  x_{z^k_t} \\
  y_{z^k_t}
 \end{array}
\right)
 = 
\left(
 \begin{array}{ccc}
  x \\
  y
 \end{array}
\right)
 +
\left(
 \begin{array}{ccc}
  cos \theta & -sin \theta \\
  sin \theta &  cos \theta
 \end{array}
\right)
\left(
 \begin{array}{ccc}
  x_{k,sens} \\
  y_{k,sens}
 \end{array}
\right)
 + z^k_t
\left(
 \begin{array}{ccc}
  cos \left(\theta + {\theta}_{k,sens} \right) \\
  sin \left(\theta + {\theta}_{k,sens} \right)
 \end{array}
\right)
\tag{6.32}
}
として表される。
このとき、{
x_t = 
\left(
 \begin{array}{ccc}
  x & y & \theta
 \end{array}
\right)^T
}はロボットの座標、
{
\left(
 \begin{array}{ccc}
  x_{k,sens} &  y_{k,sens}
 \end{array}
\right)^T
}
はセンサ検出データの端点
をそれぞれ示す。

Beam Modelと同様に、こちらは3種類のノイズを定義

  • 計測ノイズ

 計測過程で起こる、ガウス分布でモデル化されるノイズ

  • 計測失敗

 センサの最大計測可能距離の時を指す。その時、値が正しいかは不明。

  • 説明の付かない計測結果

 ランダムに発生するノイズ

5 Correlation-Based Measurement Models

文献でよく見かけるmap matchingを使うモデル。
map matchingは、連続するスキャンデータをローカルマップへ落とし込む
つまりmap matchingは、グローバルマップを{m}、ローカルマップを{m_{local}}とした時、尤度{p\left(m_{local} | x_t, m \right)}が高い場所を探すことを指す

尤度は、
{
p\left(m_{local} | x_t , m \right)
 =
max \left\{ {\rho}_{m , m_{local} , x_t} , 0 \right\}
}
で表され、グローバルマップとローカルマップの相関係数{{\rho}_{m , m_{local} , x_t}}と平均地図{\overline{m}}はそれぞれ、
{
 \rho_{m , m_{local} , x_t}
 = 
\frac
 {
  \sum_{x, y} \left(m_{x, y} - \overline{m} \right)
              \left( m_{x, y, local} \left( x_t \right) - \overline{m} \right)
 }
 {
  \sqrt
   {\sum_{x, y} \left(m_{x, y} - \overline{m} \right)^2
    \sum_{x, y} \left( m_{x, y, local} \left( x_t \right) - \overline{m} \right)^2
   }
 }
}
{
\overline{m}
 = 
\frac{1}{2N}
\sum_{x, y} \left( m_{x, y} + m_{x, y, local} \right)
}
尤度フィールドに対するmap matchingの利点は、相関係数に応じてフリースペースを決めること。これにより、センサの計測範囲外のデータも考慮してマップを作ることができる
一方map matchingの欠点は、物理的に明確な説明ができないこと

Feature-Based Measurement Model

今まではセンサの生値をベースとした話
これは一歩進んだ、センサデータから求めた特徴量をベースとした話
Featureベースの利点は、特徴量を低次元化することで計算量の削減が可能な点

ロボットの世界では、環境中の特徴量はランドマークと呼ぶことが一般的
特徴量を求める時に一般的なのは、センサがセンサを基準としたランドマークとの距離と方角を計測できるとする仮定
加えて、特徴量抽出器はサイン(本書では数値で表されるとする)を出力する可能性がある

特徴量ベクトル{f}は、距離{r}と方角{\phi}、サイン{s}それぞれを使って
{
f \left( z_t \right) = \left\{ f^1_t, f^2_t, ... \right\}
 = 
\left\{
 \left(
  \begin{array}{ccc}
   r^1_t    \\
   \phi^1_t \\
   s^1_t
  \end{array}
 \right)
 ,
 \left(
  \begin{array}{ccc}
   r^2_t    \\
   \phi^2_t \\
   s^2_t
  \end{array}
 \right)
 ,...
\right\}
}

確率ロボティクスのアルゴリズムでは下記の条件付き確率を仮定する
{ \displaystyle
 p \left( f \left( z_t \right) | x_t, m\right) = \prod_i p(r^i_t, \phi^i_t, s^i_t | x_t, m)
}

特徴量ベース地図に向けたセンサモデルの改定
特徴量の地図上での位置を{m_i}とし、ロボットの座標を{ \left( \begin{array}{ccc} x \ \  y \ \  \theta \end{array}\right)^T }とすると、特徴量の位置は
{
 \left(
  \begin{array}{ccc}
   r^i_t    \\
   \phi^i_t \\
   s^i_t
  \end{array}
 \right)
 = 
 \left(
  \begin{array}{ccc}
   \sqrt{ \left( m_{j, x} - x \right)^2 + \left(m_{j, y} - y \right)^2 } \\
   atan2 \left( m_{j, y} - y, m_{j, x} - x \right) - \theta              \\
   s_j
  \end{array}
 \right)
 +
 \left(
  \begin{array}{ccc}
   \epsilon_{\sigma^2_r}    \\
   \epsilon_{\sigma^2_\phi} \\
   \epsilon_{\sigma^2_s}
  \end{array}
 \right)
}
ここで{\epsilon_{\sigma^2_r}と\epsilon_{\sigma^2_r}, \epsilon_{\sigma^2_r}}は、平均0で、分散がそれぞれ{\sigma_r, \sigma_\phi, \sigma_s}のガウシアン誤差変数

距離・角度センサの主要な問題点は、データの関連付けである。
これを解決するために{f^i_t}{m_j}の間の一致変数{c}を導入する
{c_i = j \leq N}の時、{i}番目の特徴量は{j}番目のランドマークと一致する
{N}はランドマークの総量

Sampling Poses

今までは特徴量の確からしさについて議論してきた
一方で、特徴量と地図情報からロボットの座標を推定したい時もある
そのためには、事前確率{p \left( x_t | c^i_t, m \right)}が必要である。
あり得ないが事前確率を均一分布とするとロボットの座標の尤度は
{
 p \left( x_t | f^i_t, c^i_t, m \right) = \eta \  p \left( f^i_t | c^i_t, x_t, m \right) p \left( x_t | c^i_t, m \right) \\
 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \  = \eta \  p \left( f^i_t | c^i_t, x_t, m \right)
}