2/19 State Estimation:状態推定(1)


とりあえず, 2/12に即興で作ったやつをHumとする.


\(\mathbf T_{i,d}\)は各足の所望の配置
\(i=(r,l)\):rはright右足, lはleft左足
dはdesiredの略
\(\mathbf v_{d,i}\)は各足の所望の空間的速度
\(\ddot{\mathbf θ}^u_d\)は所望の上体関節加速度(uはupper bodyのu).
\(\mathbf r_{G,d}\)は所望のCoM位置.
\(\mathbf k_d\)は所望のCoMの角運動量

Sensorsがロボットから得るデータsensor dataが入る” State Estimator”を追加する.

State Estimatorがやることはstate estimation(状態推定)である. ではどうやって状態を推定するか? 2014年にRotellaらが提案した[1]はどうだろう. なかなかよさげである. [1]を引用した最近の論文を示す[2, 3, 4, 5, 6, 7, 8, 9, 10, 11].
————————————————————-
[1]を詳しく見ていく.
状態推定のためにEKFを使う. [12]で紹介されたフィルタを基にしている.
・推定フレームワーク
カルマンフィルタは, 推定値の不確実性を特定する対応するcovariance matrix(共分散行列)Pと共に状態ベクトルxの推定値を提供する. フィルタには 1)予測ステップにおいてシステムを通して推定値を伝搬させて先験的推定値\(\hat{x}_k^-\)および\(P^-_k\)を生成し, 2)更新ステップにおける測定値を用いて推定値を事後推定値\(\hat x^+_k\)および\(P^+_k\)に更新する. しかしながら, 標準カルマンフィルタは線形システムにおける状態推定にのみ適用可能である. 次のような連続時間の非線形システムがあると仮定する.
$$\dot x=f(x,\omega)$$
$$y=h(x,v)$$
ここで, fは予測モデル, hは測定モデル, \(\omega\)と\( v\)はノイズである. 現在の推定値を中心にこれらのモデルを単純に線形化し, カルマンフィルターの式を適用することができる.  これは拡張カルマンフィルターとして知られている.  最適性と収束性は保証されないが, これは非線形推定の一般的な方法である. その単純さと低い計算コストのために, 粒子フィルタやUKF(Unscented Kalman Filter)のような代替物よりもEKFを選んだ.  しかしながら, 提示されたフレームワークおよび可観測性分析は, これらすべてのフィルタに当てはまる.

・回転量の扱い
単位四元数は, その理論的および計算上の利点のために, 元のフィルタの基本方位を表すために選択される. ただし, 四元数はSO(3)の最小ではない表現であるため, EKFで回転量を処理するときは特に注意が必要.
四元数と三次元座標変換
まず, exponential map(指数写像)
$${exp}(\omega)=\begin{pmatrix}\sin (\frac{||\omega ||}{2})\frac{\omega}{|| \omega ||}\\ \cos (\frac{|| \omega ||}{2})\end{pmatrix}\hspace{20mm}(3)$$
は, 単位ベクトルω/ ||ω||について||ω||の大きさの増分回転が与えられたとき, 時間kとk + 1で四元数を関連付けるのに使われる . すまわち,
$$q_{k+1}={exp}(\omega )⊗q_k$$
ここで、⊗は四元数の乗算を表す.  大まかに言って, 指数写像は回転量の「加算」に使用される. 式(3)の最初の要素は四元数のベクトル部分であり, 2番目の要素はスカラ部分である.
四元数環には, 2つの自己矛盾のない規約がある. そのような問題を避けるために, [13]の四元数の慣習を使う.
EKF状態ベクトルでは, 四元数は, それに対応する3次元誤差回転ベクトルφ∈so(3)によって表される. 四元数によって表される方位の共分散は, この最小表現に関して定義される.
更新ステップ中に, 四元数値の測定値に対応する革新ベクトル\(e\)が, 実際の測定四元数\(s\)と予想される測定四元数\(z\)との間の差から抽出された三次元回転ベクトルとして計算される. すなわち,
$$e={log}(s⊗z^{-1})$$
ここで, log()は, SO(3)の要素をso(3)の対応する要素(指数写像の逆数)にマッピングする対数を表す.  上記の演算は減算の概念を回転量に拡張する.
最後に, 上で計算したような革新ベクトルを使用して, 更新ステップ中に状態補正ベクトル\(Δx\)が\(Δx=Ke\)として計算される. ここで, \(K\)はカルマンゲインである.  すべての非回転状態は単純に\(\hat x_k^+=\hat x^-_k+Δx\)として更新されるが, 四元数状態は次のように式(3)を使用して更新される.
$$\hat q^+_k={exp}(Δ\phi )⊗\hat q^-_k$$
四元数や状態推定におけるその使用の詳細は[13].

 

【参考文献】
[1]https://arxiv.org/pdf/1402.5450.pdf
[2]https://www.researchgate.net/publication/328249073_A_Study_on_Low-Drift_State_Estimation_for_Humanoid_Locomotion_Using_LiDAR_and_Kinematic-Inertial_Data_Fusion
[3]http://comoros.cs.columbia.edu/IROS_2018_proceedings/media/files/1881.pdf
[4]https://arxiv.org/pdf/1709.07472.pdf
[5]https://www.researchgate.net/profile/Stylianos_Piperakis3/publication/326194869_Nonlinear_State_Estimation_for_Humanoid_Robot_Walking/links/5b55baa20f7e9b240ffe0479/Nonlinear-State-Estimation-for-Humanoid-Robot-Walking.pdf
[6]https://hal.archives-ouvertes.fr/hal-01142399/file/mifsud2015iros_final.pdf
[7]https://www.research.ed.ac.uk/portal/files/36374583/nobiliICRA17_1.pdf
[8]https://hal.archives-ouvertes.fr/hal-01574819/document
[9]https://homes.cs.washington.edu/~todorov/papers/LowreyHumanoids16.pdf
[10]https://dhkim0821.github.io/papers/humanoid_nao.pdf
[11]http://users.ics.forth.gr/~spiperakis/Piperakis_Humanoids2016_Final.pdf
[12]http://www.roboticsproceedings.org/rss08/p03.pdf
[13]http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
[14]
[15]
[16]
[17]
[18]
[19]
[20]