はじめに
この記事では、私が考案した（のは事実ですが、似たような先行研究がありました。それについては記事の最後で触れています。）姿勢推定フィルタについて説明し、相補フィルタやEKFとの比較を行います。
相補フィルタやEKFによる姿勢推定では四元数そのものに係数を掛けて足したり引いたりすることで補正を行いますが、本記事中で紹介するフィルタは、角速度を積分する際に補正のための角速度を別途加えることで間接的に補正を行います。
上述した特徴により、相補フィルタと比較して外乱の影響を受けにくくすることができました。
このフィルタのコンセプトは、「EKFより簡単に、相補フィルタより外乱に強く」です。
※追記（2022-03-03）：本記事における外乱とは、主に加速度外乱のことを指します。また、その中でも、一定の動加速度が長時間加わる状況よりは、振動や衝撃により一時的に加速度計測値が乱れるような状況を想定しています。
記号・演算子
本記事中で使用する記号ついてまとめます。 以前書いた「四元数まとめ資料」と同じ表記となっています。
まず、四元数は上にチルダをつけて$( \tilde q (\in \mathbb{H}) $)と表記し、ベクトルはボールド体で$( \boldsymbol{q} (\in \mathbb{R}^3) $)と表記します。
また、式中にサンセリフ体の$( \mathsf{V} $)が出てきますが、これは四元数特有の演算子[2]で
$[ \mathsf{V} \tilde q = \boldsymbol{q} $]
が成り立ちます。
アルゴリズム
角速度を積分することで得られる姿勢角は諸々の要因により徐々に真値からズレていくわけですが、ここで現在の姿勢から真の姿勢に到達するための角速度を求めて、それを積分の式に加えてやればいい感じに補正できるのではないか、というのが基本的な考え方になります。
実際には真の姿勢はわかりませんから、センサで計測した加速度 $( \boldsymbol{a}_\mathrm{b} $) と地磁気 $( \boldsymbol{m}_\mathrm{b} $) から求めた姿勢$( \tilde q_\mathrm{gm} $)を基準姿勢として扱います。 $( \tilde q_\mathrm{gm} $)を求める方法については、文献[3]を参照してください。 このようにして求めた姿勢は高周波のノイズや外乱を含むため、LPFを通します。
次に、基準姿勢に到達するための角速度を求めます。推定した四元数を$( \tilde q $)とすると、$( \tilde q $)から基準となる$( \tilde q_\mathrm{gm} $)に $( \alpha \; (> 0) $) 秒で到達するための角速度$( \boldsymbol{\omega}_\mathrm{c} $)は、角速度の積分の式より
$[ \begin{align*} \tilde q_\mathrm{gm} &= \tilde q + \frac{\alpha}{2} \tilde q \boldsymbol{ \omega }_\mathrm{c} \\ \boldsymbol{\omega}_\mathrm{c} &= \frac{2}{\alpha} \tilde q^{-1} (\tilde q_\mathrm{gm} – \tilde q) \\ &= \frac{2}{\alpha} (\tilde q^{-1} \tilde q_\mathrm{gm} – 1) \end{align*} $]となります。しかし、上式は左辺がベクトルで右辺が四元数となっており辻褄が合いません。そもそも、ここで用いた積分の式は$( \tilde q $)と$( \tilde q_\mathrm{gm} $)が十分近い場合に成り立つ近似式でした。つまり、この条件のもとでは
$[ \tilde q^{-1} \tilde q_\mathrm{gm} \approx 1 $]
となるため、実部を0とみなして
$[ \boldsymbol{\omega}_\mathrm{c} := \frac{2}{\alpha} \mathsf{V} ( \tilde q^{-1} \tilde q_\mathrm{gm} ) $]とすることができます。これで両辺の辻褄が合いました。
上記のように求めた$( \boldsymbol{\omega}_\mathrm{c} $)を積分の式にフィードバックすることで、補正を行います。
$[ \tilde q [\mathrm{n}] = \tilde q[\mathrm{n}-1] + \frac{T_s}{2} \tilde q[\mathrm{n}-1] ( \boldsymbol{\omega}_\mathrm{b} [\mathrm{n}] + \boldsymbol{\omega}_\mathrm{c} [\mathrm{n-1}] + \beta \sum T_s \boldsymbol{\omega}_\mathrm{c} [\mathrm{n-1}] ) $]ただし、$( \mathrm{n} $)は計算ステップ、$( T_s $)はサンプリング周期です。
実は、最初にシミュレーションを組んだ段階では $( \boldsymbol{\omega}_b + \boldsymbol{\omega}_\mathrm{c} $) のみでしたが、角速度バイアスがある場合は基準姿勢に収束しないことがわかったので積分項 $( \beta \sum T_s \boldsymbol{\omega}_\mathrm{c} $) を追加しました。ここで、 $( \beta \; (\geq 0) $) は積分係数です。
アルゴリズム全体をブロック線図にまとめると以下のようになります。
結構ゴチャついてしまったので、見易さのためにPredictとCorrectという二つのブロックに分けています。積分器を後退オイラー法[4]としていますが、他の方法で積分しても構いません。
最終的に、このフィルタの設計パラメータはLPFのカットオフ周波数と$( \alpha, \beta $)の計3つとなりました。ただし、LPFのカットオフ周波数はセンサやセンサを取り付ける物体の有効周波数帯域を考慮して決定しますから、実質的に調整するのは$( \alpha, \beta $)の2つです。
シミュレーション（相補フィルタ、EKFとの比較）
ノイズと角速度バイアスが加わった場合でも正しく姿勢推定できるかどうか、シミュレーションを通して確認します。比較対象として、相補フィルタとEKFのシミュレーションも行いました。EKFの実装は文献[5]を参考にしています。
実際のセンサでは角速度バイアスは少しづつ変動することが知られていますが[5]、その変動はかなり小さいようなのでここでは一定値として扱います。
シミュレーション条件は以下の通りです。
- サンプリング周期：0.02 s
- 角速度計測値に加わるノイズ分散：0.0001
- 加速度 〃 ：0.01
- 地磁気 〃 ：0.01
- 角速度バイアス[x, y, z]：[-0.02, 0.01, 0.05] rad/s
- LPFのカットオフ周波数：10 Hz（相補フィルタも同様）
- $( \alpha = 1.0 $)
- $( \beta = 0.2 $)
以下に各フィルタのシミュレーション結果を示します。
“Omega Feedback Filter”が本記事で説明したフィルタです（以降、OmegaFFと表記）。角速度バイアスの推定値は、OmegaFFとEKF共に真値に収束していることが確認できます。
相補フィルタでは角速度バイアスの推定を行っていないものの、今回のシミュレーションでは姿勢変化がゆっくりなのとバイアスも小さいので推定結果にそこまで大きな誤差は生じていません。
また、OmegaFFの姿勢推定値は相補フィルタのそれに比べると滑らか（加速度・地磁気計測値に加わったノイズの影響をあまり受けていない）であることがわかります。これは角速度を積分する式自体がローパス特性を持っているためと考えられます。相補フィルタでこの結果を出すには極端にカットオフ周波数を下げる必要がありますが、そうすると位相遅れが大きくなってしまうため、OmegaFFのこの特性はかなり有用です。
使用したプログラム：https://github.com/HamaguRe/space-denpa_omega_feedback_filter
実験（相補フィルタ、EKFとの比較）
マイコン上に実装してシミュレーション通りの性能が出るかを確認します。ちゃんとした実験装置が無いので、手で動かして加速度外乱を加えた場合の推定結果を比較します。
フィルタの設計パラメータとサンプリング周期はシミュレーションの場合と同じです。また、3つのフィルタは全て同じ計測値を使用して計算を行っています。
左から順に、OmegaFF, 相補フィルタ, EKFで推定した姿勢を表しています。相補フィルタの姿勢推定値はセンサを振った際に大きく暴れていますが、OmegaFFとEKFはあまり影響を受けておらず、両者とも同じような挙動であることが確認できます。
地磁気センサの校正を行っていないので、OmegaFFとEKFではヨー軸の回転が加わった後に少しづつ動くシーンがあります（わかりにくいですが、32〜36秒など）。
また、静止時にヨー角の推定値がかなりズレているように見えるシーンがありますが、これは表示の仕方の問題で、数値で見るとほぼ同じ値であることが確認できます。ただし、前述したように地磁気センサの校正を行っていないので、それに起因するズレも当然あります。
OmegaFFのパラメータ設定による挙動として、大まかに以下のような傾向が見られました。
- $( \alpha $)小：外乱に敏感になる。
- $( \alpha $)大：外乱の影響を受けにくくなるが、（起動時など推定誤差が大きい場合に）基準姿勢に収束するまでに時間がかかる。
- $( \beta $)小：角速度バイアスが存在する場合、基準姿勢に収束するまで時間がかかる。
- $( \beta $)大：角速度バイアスの推定は早くなるが、積分項が推定値に悪影響を与えることがある。
追記（2022-03-31）：マイコン上での計算時間
STM32F303K8T6上で実行した際に各フィルタが計算に要する時間を簡易的に計測してみました。変数は単精度浮動小数点数で保持し、FPUを使用して演算を行っています。
F303K8T6の場合、デフォルトの動作周波数は8MHzで、内部発振器を用いた場合の最高動作周波数は64MHzなので両方の動作周波数で動かした場合の計測結果を示します。
| フィルタ | 8MHz動作時の計算時間 | 64MHz動作時の計算時間 |
|---|---|---|
| Omega Feedback Filter | 340 us | 59 us |
| Complementary Filter | 296 us | 44 us |
| Extended Kalman Filter | 2.5 ms | 356 us |
相補フィルタが一番速い結果となりましたが、OmegaFFと比べると性能差の割にはそこまで速くないですね。
EKFも思ったより速いですが、U-D分解フィルタでの実装となっていたり、Rが対角行列であることを利用して逆行列の計算を省いていたりするので、EKFの式をそのまま実装した場合よりも高速化できているかもしれません。実装の詳細はシミュレーションで使用したプログラムを参照してください。
おわりに
本記事中で紹介したOmegaFFは、補正用の角速度をフィードバックするという形で間接的に補正を行うことで、相補フィルタよりも外乱の影響を受けにくくすることができました。また、EKFと比較するとかなりシンプルな構造でありながら、今回の実験条件においては同程度の推定性能を達成することができました。
EKFでは数値計算の誤差が蓄積して問題を生じることがありますが[6]、OmegaFFでは単純な構造故にそのような問題が発生しないという点で優位性があるといえます。
この記事を書きながら念のため似たような先行研究が無いかと調べていたら、文献[7]を見つけました。世界中で誰かしら同じことを思いついているだろうと思ってはいましたが、まさか日本語の論文で見つかるとは…。まあ論文として発表されている手法に近いことを自力で考えられたと思えば嬉しいですね。
深く読み込んだわけではありませんが、文献[7]の手法とこの記事で紹介した手法では、四元数を直接補正するのでは無く、角速度を積分の式にフィードバックして補正するという点が類似しています。本記事中でいうところの $( \tilde q_\mathrm{gm} $) や $( \boldsymbol{\omega}_\mathrm{c} $) の求め方は異なりますが、恐らく求めているものは同じでしょう。
文献[7]では大域的漸近収束性の証明や外乱検知についても書かれているので、興味があれば目を通してみてください。
参考文献
- 宇宙電波実験室, “四元数まとめ資料を書いた”, (参照 2022-03-01).
- 堀 源一郎, “ハミルトンと四元数 −人・数の体系・応用−”, 海鳴社, 2007.
- 宇宙電波実験室, “加速度・地磁気センサの値から姿勢（四元数表現）を求める”, (参照 2022-03-01).
- MathWorks Help Center, “Discrete-Time Integrator”, (参照 2022-03-01).
- 鈴木 智, 田原 誠, 中澤 大輔, 野波 健蔵, “動加速度環境下における姿勢推定アルゴリズムの研究”, 日本ロボット学会誌, Vol. 26, No. 6, 2008.
- 片山 徹，”応用カルマンフィルタ”，朝倉書店, 1983.
- 大和 秀彰, 古田 貴之, 富山 健, ”外乱に対して頑強で大域的漸近安定なジャイロバイアス誤差の推定・除去による三次元姿勢推定”, 日本機械学会論文集(C編), Vol. 77, No. 776, 2011.