2/17 MITの安定化制御則(6)


今回も[1]の内容を見ていく. ページで言うと, 26/47~36/47.
5 State Estimation

コントローラは, 制御ステップごとに, ロボットの完全な状態の高速で待ち時間の短い見積もりを必要とする. この節では, このニーズを満たし, ドリフトが少なく, オンラインでの実行に必要な計算上の制約を満たす状態推定器を紹介する.  [1]のFigure 6は, plannersと状態推定器からコントローラへの信号の流れを示す.  コアフィルタリング手法は[2]に提示されており, Bryらによって導入された推定器から適応された[3].
※Figure 6の説明
システム内の信号の流れを示すブロック図.  フットステッププランナ(セクション3.1)または全身運動プランナ(セクション3.2)は, 所望の軌跡を制御システムに入力する. コントローラ(セクション4)は, 状態推定器(セクション5)と約800 Hzで閉ループで動作する.  ロボットの現在の状態を使用して, LQRソリューションをオンラインで(通常は別のスレッドで)再計算して, nominal歩行軌跡からの偏差に対するシステムの感度を下げることができる.

5.1 Requirement and approach
状態推定に使用できるセンサーは, 骨盤に取り付けられた正確な慣性測定ユニット(IMU), 各関節の関節位置センサー, および外因性センサー:LIDARと視覚, 後者は現在使用されていない.
状態推定器はこれらのセンサを使用して, 回転関節の位置および速度, ならびに「浮動基部」(すなわち骨盤リンク)の姿勢およびねじれの推定値を生成する.  ロボットには関節速度センサがないので, 速度推定値は位置の差とフィルタリングから導き出されなければならない.
さらに, IMUを用いて浮遊基部方位のハイクオリティ測定を容易に達成することができるが, 低ドリフトで高精度位置決めを達成することは依然として重要な課題である. セクション3.1で説明した起伏のある地形を通過できるようにするには, 1ステップあたり1 cm未満の drift rateが必要である.  提案された状態推定器は, 浮動ベース状態推定に対して0.5ミリ秒の待ち時間で333Hzで動作する.
単一のプロセスモデルを使用してロボットの完全な状態を推定するのではなく, 浮動基底とは別に関節の状態を推定することによって問題を分解する.  各関節について単純な一次カルマンフィルタを用いて脚関節位置および速度をフィルタリングする. フィルタリングされた脚関節位置はその後, 浮動基底の状態を推定するために使用される.  拡張カルマンフィルター(EKF)を使用して, 浮動基底状態を推定する.
より長い期間の計画実行をサポートするために, ロボットの環境にローカライズすることでglobal driftを取り除く.  5.5.2節では, LIDARセンサーを使用してロボットの環境の地図に対して位置を特定することでこれを達成する方法について説明する.

5.2 Related work

文献には, 脚付きシステム用に開発されている状態推定量の注目すべき例がいくつかある. Stephens [4]は, ダイナミクスモデルと計画された状態軌跡を用いたヒューマノイドの状態推定を実証した. Xinjilefu ら [5]はこのアプローチを拡張してAtlasロボットに適用した. 彼らは, 多くの自由度を持つヒューマノイドに対して単一の拡張カルマンフィルタ(EKF)を定式化するという計算上の課題を避け, 代わりに骨盤位置と関節力学を別々のフィルタで推定することを提案した.
EKFベースの推定量は, 四足歩行のためにBloesch ら[6]によって提示されている. これは, 慣性測定によって駆動される予測モデルを使用し, 足場測定を使用してフィルタ補正を作成する.  このアプローチは, 足場の位置を状態ベクトルに組み込み(各足の点モデルを使用), システムの一貫性と可観測性を分析した.  このアプローチは, SARCOSヒューマノイドロボットのシミュレートバージョンでの2足歩行にも拡張されている[7].
※SARCOSのヒューマノイドロボット

視覚, LIDARおよび他のセンサーの組み合わせを使用したロボットマッピングはかなり進歩しているが, フィールド操作におけるヒューマノイドプラットフォームでのその使用は, 計算, 待ち時間および感度の理由のために制限されている.  Strasseら[8]による結果の一例は, 実験室環境におけるリアルタイムの単眼視SLAMシステムを用いた閉ループを実証した.
最後に, Hornung ら[9]の研究は, セクション5.5.2に示したアルゴリズムと同様の方法でLIDARセンサを利用してマルチレベル環境内にAldebaran NAOロボットを位置特定した.  ただし, 速度やロボットの高さの推定とは別に, 1次particle filterを使用して推定を行うため, このアプローチでは精度と確率的な一貫性が制限される.

5.3 Signal preprocessing
有用な測定データを得るためにはいくらかの前処理が必要である. 以下の前処理ステップはAtlasロボットに固有のものである.
第一に, IMUが加速度と回転速度の測定を乱す油圧加圧器によって引き起こされた振動を測定することに注意する. 積分の前にこの85 Hzの信号を除去するためにこれらの信号をノッチフィルタリングする. 第二に, 各脚関節の角度は,LVDTを用いてその油圧アクチュエータの行程を測定し, 次いで関節リンケージを介して変換を計算することによって感知される. このマッピングは, 荷重がかかったときのジョイントリンケージの屈曲や, ジョイントの方向が変化したときのバックラッシュを考慮していない. これらの影響を説明するために, Johnsonらによって導入された補正モデル[10]
を使用して関節角度測定値を前処理する. モデルは関節レベルで線形コンプライアンスを次式のように仮定する.
$$q^{preproc}_i=q^{raw}_i-\tau_i/K_i$$

5.4 Process model

5.4.1 State

 

$$\mathbf x_{base}=(^b\omega_b,^b\mathbf v_b,^{\omega}\mathbf R_b,^{\omega}\mathbf p_b)$$

$$\mathbf x_{base}=(^b\mathbf v_b,^{\omega}\mathbf R_b,^{\omega}\mathbf p_b,\mathbf b_{\omega},\mathbf b_a)$$

$$\mathbf R=\hat{\mathbf R}\mathbf R(χ)$$

$$\mathbf e_{base}=\left(\begin{array}{ccccc}^b\tilde{\mathbf v}_b\\χ\\^{\omega}\tilde{\mathbf p}_b\\ \tilde{\mathbf b}_{\omega}\\ \tilde{\mathbf b}_a\end{array}\right)$$

5.4.2 input

角速度と同様に, 加速度\(^b\mathbf a_b\)はIMUによって正確に測定される.  EKFでの測定値(これは一般的なアプローチ)ではなく, \(^b\mathbf ω_b\)と\(^b\mathbf a_b\)の両方をプロセスモデルへの入力として扱う. したがって, EKFの入力ベクトルは次のように書ける.
$$\mathbf u_{base}=\begin{bmatrix}^b\mathbf ω_b\\^b\mathbf a_b\end{bmatrix}$$

5.4.3 Dynamics

state:
\(^b\dot{\mathbf v}_b=-^b\bar{\omega}_b\times ^b\mathbf v_b+^{\omega}\!\mathbf R^T_b\:^{\omega}\mathbf g+^b\! \bar{\mathbf a}_b+\mathbf w_v\\
^{\omega}\dot{\mathbf R}_b=^{\omega}\!\!\mathbf R_b\:^b\hat{\bar{\mathbf {\omega}}}_b\\
^{\omega}\dot{\mathbf p}_b=^{\omega}\!\mathbf R_b\:^b\mathbf v_b+\mathbf w_p\\
\dot{\mathbf b}_{\omega}=\mathbf w_{b_{\omega}}\\
\dot{\mathbf b}_a=\mathbf w_{b_a}\)

$$^b\bar{\mathbf ω}_b=^b\!\mathbf ω_b+\mathbf b_{\omega}$$
$$^b\bar a_b=^b\!\mathbf a_b+\mathbf b_a$$
$$\dot χ=^b\!\mathbf ω_b+\frac{χ\times ^b\!\mathbf ω_b}{2}+\frac{1}{||χ||^2}\bigr [1-\frac{||χ||\sin ||χ||}{2(1-\cos ||χ||)}\bigr ]χ\times(χ\times ^b\!\mathbf ω_b)$$

5.5 Measurement model

5.5.1 Leg kinematics

$$^{\omega}\hat{\mathbf p}_b[k]=^{\omega}\!\mathbf p_f[k-1]+^{\omega}\!\mathbf p_{b/f}[k]$$
$$^{\omega}\hat{\mathbf v}_b[k]=\frac{^{\omega}\mathbf p_b[k]-^{\omega}\!\mathbf p_b[k-1]}{h}$$

5.5.2 LIDAR

5.6 Latency and computation

【参考文献】
[1]https://dspace.mit.edu/openaccess-disseminate/1721.1/110533
[2]https://core.ac.uk/download/pdf/78072765.pdf
[3]http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.675.4981&rep=rep1&type=pdf
[4]https://www.researchgate.net/publication/261416087_State_estimation_for_force-controlled_humanoid_balance_using_simple_models_in_the_presence_of_modeling_error
[5]http://www.cs.cmu.edu/afs/cs/user/sfeng/www/xx_icra14.pdf
[6]http://www.roboticsproceedings.org/rss08/p03.pdf
[7]https://www.researchgate.net/publication/260340237_State_Estimation_for_a_Humanoid_Robot
[8]http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.6329&rep=rep1&type=pdf
[9]https://www.hrl.uni-bonn.de/papers/hornung10iros.pdf
[10]https://www.researchgate.net/publication/308340634_Team_IHMC’s_Lessons_Learned_from_the_DARPA_Robotics_Challenge_Finding_Data_in_the_Rubble