2/20 State Estimation:状態推定(2)


前回に引き続き, [1]を検討していく.

・2足予測と測定モデル
[2]では, 剛体運動学と, 3軸加速度計と3軸ジャイロスコープからなるIMUの単純なモデルに基づいて, 四足歩行状態の時間発展を記述する連続時間非線形予測モデルが開発された. この予測モデルを以下に示す. この定式化は, \(N\)で示される任意の数の足に適応することができることに留意されたい.
\( \dot r=v\)
\( \ v=a=C^T(\tilde f-b_f-\omega_f)+g\)
\( \dot q=\frac{1}{2}\begin{bmatrix}\tilde \omega -b_f-\omega _{\omega}\\0\end{bmatrix}⊗q\)
\( \dot p_i=C^T \omega _{p,i}\;\;∀i\in \{1,…,N\}\)
\( \dot b_f=\omega _{bf}\)
\( \dot b_{\omega}=\omega_{b\omega}\)
フィルタの状態は次のようになる. \(x=[r,v,q,p_i, b_f,b_{\omega}]\),ここで,\(r\)はIMUの位置(基底にあると仮定), \(v\)は基底速度, \(q\)は世界からボディフレームへの回転を表す4元数である. \(p_i\)は\(i\)番目の足の世界位置, \(b_f\)と\(b_{\omega}\)はそれぞれ加速度計とジャイロスコープのバイアスである. 特記しない限り, \(C\)は\(q\)に対応する回転行列を表す.  生の加速度計とジャイロスコープのデータは\(\tilde f\)と\(\tilde ω\)であり, 追加の熱雑音過程\(\omega_f\)と\(\omega_ω\), および雑音過程\(\omega_{bf}\)と\(\omega_{bω}\)によってパラメータ化されたランダムウォークバイアス(これは IMUセンサーの主なノイズ源を捉えた慣性航法の一般的な単純なモデルである). 最後に, \(\omega _{p, i}\)は, \(i\)番目の足場位置の不確実性を表すノイズプロセスを表す.
確立されたフィルタをヒューマノイドプラットフォームに拡張するために, 足の向きで状態ベクトルを拡張する. 簡潔にするために片足だけを含めて, 新しい状態は次のように定義される. \(x=[r,v,q,p,b_f,b_{\omega},z]\), ここで, \(z\)はワールドからフットフレームへの回転を表す四元数である.[2]でなされた仮定と同様に, 接触している間, 足の向きは一定のままであると仮定する. 少量の回転スリップを許容するために, 予測式は次のように定義される.
$$\dot z=\frac{1}{2}\begin{bmatrix}\omega_z\\0\end{bmatrix}⊗z$$
ここで, \(\omega_z\)は共分散行列\(Q_z\)を持つプロセスノイズである. 足の向きのノイズは, 元のモデルとの一貫性を保つために角速度として適用される.
元のフィルタ更新ステップは, ベースフレーム内で測定されたベースに対するその足の位置を表す各足について1つの測定式を使用して実行された. この測定は, 測定された関節角度と脚の運動学的モデルのみの関数であり, 次式のように書かれる.
$$s_p=C(p-r)+n_p$$
ノイズベクトル\(n_p\)は, エンコーダのノイズと運動学モデルの不確実性の組み合わせを表す. その共分散は, フィルタの主な調整パラメータである. 元のフィルタの定式化に続いて, ここでも測定された関節角度と運動学モデルのみの関数である追加の測定値を導入する. 四元数で表されるベースフレーム内の足の向き
$$s_z={exp}(n_p)⊗q⊗z^{-1}$$
は, 平らな足の接触によって課せられる回転拘束を表す.  雑音項\(n_q\), 式(3)のように指数写像を用いて適用される.  やはり, 雑音項は順運動学の不確実性に依存し, 調整パラメータを構成する.
足が接触を失うと, その測定式は一時的にフィルタから削除される.  さらに, その姿勢は状態から削除される. これは, 足に対応する\(\omega _p\)と\(\omega_z\)の分散を大きな値に設定することでより簡単に実現できる.  これにより, 姿勢の不確実性が急速に高まる.  接触が回復すると, その姿勢と測定値が再び含まれる.  これにより, 足の姿勢が新しい値にリセットされる. これにより, 個別のモデルを必要とせずに接点を切り替えることができる.  空中段階の間, フィルタは予測モデルの統合に帰着する.

・実装の詳細
上記のシステムは連続的で非線形であるため, 実装目的のために離散化および線形化する必要がある. 上記のEKFフレームワークに従って, これには2つの連立方程式が必要である. すなわち, 状態および測定の予測, ならびに予測モデルを通じた状態共分散の伝播および更新ステップにおけるゲインの計算のための離散線形システム.  さらに, 中間ステップで連続線形システムを導く.

・離散非線形モデル
EKFの最初のステップは, 離散化非線形モデルを使用した状態の期待値の伝播である. 小さいタイムステップ\(∆t\)にわたってIMUデータを0次ホールドすると仮定すると, 次のように1次積分スキームを使用して元のシステムを離散化することができる.
\(\hat r^-_{k+1}=\hat r^+_k+\Delta t\hat v^+_k+\frac{\Delta t^2}{2}(\hat C^{+T}_k\hat f_k+g)\)
\(\hat v_{k+1}=\hat v^+_k+\Delta t(\hat C^{+T}_k\hat f_k+g)\)
\(\hat q_{k+1}={exp}(\Delta t\hat{\omega}_k)⊗\hat q^+_k\)
\(\hat p^-_{k+1}=\hat p^+_{i,k}\)
\(\hat b^-_{f,k+1}=\hat b^+_{f,k}\)
\(\hat b^-_{\omega ,k+1}=\hat b^+_{\omega ,k}\)
\(\hat z^-_{k+1}=\hat z^+_{i,k}\)
ここで, \(\hat f_k=\tilde f-\hat b^+_{f,k}\), \(\hat ω_k=\tilde ω- \hat b^+_{ω,k}\)はそれぞれ, 測定された加速度および角速度の期待値を表す. ベース姿勢を表す四元数は, ベースフレーム内で直接測定される微小回転\(Δt\hat ω\) から形成された指数写像を使用して更新されることに留意されたい.  また, 現在のタイムステップでのIMUの加速度を組み込むために, 位置に2次の離散化が使用されている.
測定モデルは単純に
$$\hat s_{p,k}=\hat C^-_k(\hat p^-_k-\hat r^-_k)\hspace{20mm}(14)$$
$$\hat s_{z,k}=\hat q^-_k⊗(\hat z^-_k)^{-1}\hspace{20mm}(15)$$
として表され, 非線形システムの期待される測定値を生成する.

・線形連続モデル
状態推定共分散を伝播して更新ステップを実行するには, 離散化線形化ダイナミクスが必要である.  線形化してから離散化することをお勧めします.  これは多くのテキストで見られるアプローチである.
線形化は, 一次近似を使用して現在の推定値を中心に各状態を展開することによって実行される. この方法では、次の線形化モデルが得られる.
\(\dot \delta_r=\delta_v\)
\(\delta_v=-C^Tf^{\times}\delta\phi-C^T\delta b_f-C^T\omega_f\)
\(\dot \delta_{\phi}=-\omega^{times}\delta\phi-\delta b_{\omega}-\omega_{\omega}\)
\(\dot{\delta}_p=C^T\omega_p\)
\(\dot{\delta}b_f=\omega_{bf}\)
\(\dot{\delta}b_{\omega}=\omega_{b\omega}\)
\(\dot{\delta}\theta=\omega_z\)
ここで, 測定されたIMU量はバイアス補償され, \(v^{\times}\)はベクトル\(v\)に対応する歪対称行列を表す. エラー状態およびプロセスノイズベクトルは, それぞれ\(δx=[δr, δv, δ\phi,δp,δb_f,\delta ω]^T\)および\(\omega=[\omega_f,\omega_{\omega},\omega_p,\omega_{bf},\omega_{b\omega},\omega_z]^T\)と定義される. 線形化されたシステムは, 状態空間形式で\(\dot{\delta}x=F_c\delta x+L_c\omega\)書くことができ,
$$F_c=\left(\begin{array}{ccccccc}0&I&0&0&0&0&0\\
0&0&-C^Tf^{\times}&0&-C^T&0&0\\
0&0&-\omega^{\times}&0&0&-I&0\\
0&0&0&0&0&0&0&\\
0&0&0&0&0&0&0&\\
0&0&0&0&0&0&0&\\
0&0&0&0&0&0&0&\end{array}\right )$$
, \(L_c=diag\{-C^T,-I,C^T,I,I,I\}\)は, それぞれ予測および雑音ヤコビアンである. 簡単にするために, 各プロセスノイズベクトルの共分散行列は等しい要素をもつ対角行列であると仮定する. 連続プロセスノイズ共分散行列は\(Q_c=diag\{Q_f,Q_{\omega},Q_p,Q_{b_f},Q_{b_{\omega}},Q_z\}\)である. 式(14),(15)により定義される測定モデルは次のように線形化される.
$$s_p=-C\delta r+(C(p-r))^x\delta \phi+C\delta p+n_p$$
$$s_z=\delta \phi-C[q⊗z^{-1}]\delta \theta+n_z$$
ここで, \(C[m]\)は, 四元数\(m\)に対応する回転行列を表すために使用される.  このモデルは, \(δy=H_cδx+v\) の形で書くことができ, ここで, \(v=[n_p,n_z]^T\)は測定ノイズベクトルであり,
$$H_c=\begin{pmatrix}-C&0&(C(p-r))^x&C&0&0&0\\
0&0&I&0&0&0&-C[q⊗z^{-1}]\end{pmatrix}$$
は, 測定ヤコビアンである.  測定値は無相関であると仮定され, それゆえ測定ノイズ共分散行列は\(R_c=diag\{R_p,R_z\}\)と定義される. ここで, \(R_p\)と\(R_z\)は, それぞれの対角成分は等しい要素をもつ.

・離散線形モデル
区間\(Δt=t_{k+1}-t_k\) にわたって入力のゼロ次ホールドを仮定すると, 離散化予測ヤコビアンは次式で与えられ、
$$F_k=e^{F_c \delta t}$$
離散化状態共分散行列は次式で与えられる.  
$$Q_{k-1}=\int_{t_{k-1}}^{t_k}e^{F_c(t_k-\tau)}L_cQ_cL^T_ce^{F^T_c(t_k-\tau)}d\tau$$
実際には, これらの式は単純化のために一次でしばしば切り捨てられて, \(F_k≈I+F_cΔt\)および\(Q_k≈F_kL_cQ_cL^T_cF^T_kΔt\)になる. [2]とは対照的に, システムは, 実装を単純化するために, 上記のように雑音項に対してゼロ次ホールドを使用して離散化される.
上記の手順に従って, 離散化された予測と測定のヤコビアンは,
$$F_k=\left(
\begin{array}{ccccccc}I&I\Delta t&0&0&0&0&0\\
0&I&-C^T_kf^{\times}_k\Delta t&0&-C^T_k\Delta t&0&0\\
0&0&I-\omega^{\times}_k\Delta t&0&0&-I\Delta t&0\\
0&0&0&I&0&0&0\\
0&0&0&0&0&I&0\\
0&0&0&0&0&0&I
\end{array}
\right)$$
$$H_k=\begin{pmatrix}-C_k&0&(C_k(p_k-r_k))^{\times}&C_k&0&0&0\\
0&0&I&0&0&0&-C[q_k⊗z^{-1}_k]\end{pmatrix}$$
ここで, すべての量は事前状態ベクトル\(\hat x^-_k \)を使用して計算される. 最後に, 連続測定共分散行列は, \(R_{k-1}≈\frac{R_c}{Δt}\)として離散化される.

以上, ざっくり[2]を読んだ感じ, 状態推定の手法としてはいい感じである.よって,  State EstimatorとしてRotellaらが提案した[2]の手法を採用する. SensorsにはIMUJoint Encodersなどが含まれる.

状態推定後の???から先を考える.
ざっくりとした流れでは, State Estimator→High level controller→Quadratic program solver→Inverse dynamics calculator→Robot→Sensors→State Estimatorとなる.

 

 

 

【参考文献】
[1]https://arxiv.org/pdf/1402.5450.pdf
[2]http://www.roboticsproceedings.org/rss08/p03.pdf