加速度・地磁気センサの値から姿勢(四元数表現)を求める

外乱の無い状態であれば,重力加速度ベクトルと地磁気ベクトルを見ることで,センサの固定された機体座標系の姿勢(基準座標系に対する角度)を求めることができます.

オイラー角の例はよく見かけますが,この記事では前述した二つのセンサ情報から姿勢を表す四元数\( \tilde q_{gm} \)を求めます.

姿勢計算のイメージ

記号・演算


この記事中で使用する記号や演算について記述します.基本的には以前書いた「四元数まとめ資料」と同じ表記になっています.

四元数は変数の上部にチルダを付けて\( \tilde q \in \mathbb{H} \)のように表し,共役はアスタリスクを付けて\( \tilde q^* \)とします.また,ベクトルはボールド体で\( \boldsymbol{a} \in \mathbb{R}^3 \)のように表します.

姿勢計算をする際に出てくる\( \tilde q : \boldsymbol{a} \to \boldsymbol{b} \)は,\( \boldsymbol{b} = {\tilde q} \boldsymbol{a} {\tilde q^*} \)を満たす\( \tilde q \)を求める演算で,

$$ \theta = \arccos \left( \frac{ \boldsymbol{a} \cdot \boldsymbol{b} }{ \| \boldsymbol{a} \| \| \boldsymbol{b} \| } \right), \quad \boldsymbol{n} = \frac{ \boldsymbol{a} \times \boldsymbol{b} }{ \| \boldsymbol{a} \times \boldsymbol{b} \| } $$

とすれば

$$ \tilde q = \cos \frac{\theta}{2} + \boldsymbol{n} \sin \frac{\theta}{2} $$

として求まります.

姿勢計算


機体座標系上における重力加速度ベクトル(センサで計測した値)を\( \boldsymbol{g}_b \),基準座標系上における値を\( \boldsymbol{g}_r \)とすると,対地角\( \tilde q_g \)は以下のようになります.

$$ \tilde q_g : \boldsymbol{g}_b \to \boldsymbol{g}_r $$

基準座標系をENU(x-y-zの順にEast-North-Up)とするなら\( \boldsymbol{g}_r = [0, 0, 1] \)となります.

以上で対地角は求まりました.しかし,\( \boldsymbol{g}_r \)まわりの方位角は保証されないので,地磁気ベクトルを用いて補正角を求めます.

重力加速度の場合は\( \boldsymbol{g}_r \)がある程度正確に分かりますが,地磁気の場合は伏角や偏角があるので\( \boldsymbol{m}_r \)がわかりません.事前に計測しておけばよいといえばそれまでですが,対地角は既に分かっていて地磁気は方位角を求めるために使うのですから,伏角を無視して\( \boldsymbol{m}_r = [*, *, 0] \)のようにしたいところです.

そこで,\( \tilde q_g \)を用いて基準座標系上に持っていくことで伏角の影響を無視します.

$$ \boldsymbol{m}_{b \to r} = \tilde q_g \boldsymbol{m}_b \tilde q_g^* $$

$$ \boldsymbol{m}’_{b \to r} = \boldsymbol{m}_{b \to r} \circ [1, \: 1, \: 0] \quad (\circはアダマール積) $$

以上より,真値に対する\( \tilde q_g \)の補正角\( \tilde q_e \)は

$$ \tilde q_e : \boldsymbol{m}’_{b \to r} \to \boldsymbol{m}_r $$

となり,最終的に姿勢\( \tilde q_{gm} \)が以下のように求まります.

$$ \tilde q_{gm} = \tilde q_e \tilde q_g $$

こうすれば,事前に偏角だけ調べておいて\( \boldsymbol{m}_r = [0.14, 0.99, 0] \)のようにしたり,もしくは方位角は大体合っていれば良いと割り切って\( \boldsymbol{m}_r = [0, 1, 0] \)と指定することができます.

伏角を無視したことで\( \boldsymbol{m}_r \)の指定が楽になるのも良いですが,この方法の最大の利点は,地磁気外乱が加わった際に対地角精度に影響が出ない点にあると思っています.クアッドコプタやヘリのような飛翔体はホバリングのために対地角の推定精度が重要になりますから,この性質は結構嬉しいのではないでしょうか.

応用〜相補フィルタ〜


上記のように求めた\( \tilde q_{gm} \)は外乱の無い状態では正確な姿勢角を表しますが,外乱には非常に弱く,例えばセンサに衝撃が加わると実際の姿勢とはかけ離れた値になってしまいます.そこで,実用的な姿勢推定のために相補フィルタを適用します.

相補フィルタについての詳細は以下のサイトを参照してください.

即戦力モノづくり!エンジニアへの道標:相補フィルタのしくみを解明してみる【加速度・ジャイロセンサ】

上記のサイト中の内容を四元数に置き換えて計算すると,最終的に以下の更新式が得られます.

$$ \tilde q(n) = K \left \{ \tilde q(n – 1) + \frac{T_s}{2} \tilde q(n – 1) \boldsymbol{\omega}_b(n) \right \} + (1 – K) \tilde q_{gm}(n) $$

ただし,\( T_s [\mathrm{s}] \)はサンプリング周期,\( \boldsymbol{\omega}_b [\mathrm{rad/s}] \)は機体座標系上で計測した角速度,\( K \)は\( T_s \)と相補フィルタのカットオフ周波数により決まる係数です.

とてもシンプルな式ですが,実装する際には四元数の符号に注意する必要があります.

一般にLerp(線形補間)の実装は内部で符号選択をしていると思うので,\( \tilde q_0 \)から\( \tilde q_1 \)までの経路を補間する\( \text{Lerp} (\tilde q_0, \tilde q_1; t) \)を利用して以下のようにするのが良いでしょう.

$$ \tilde q(n) = \text{Lerp} \left ( \tilde q(n – 1) + \frac{T_s}{2} \tilde q(n – 1) \boldsymbol{\omega}_b(n), \tilde q_{gm}(n); 1 – K \right ) $$

これは\( \tilde q \)のノルムが1にならないので,最後に正規化するのを忘れずに.

Lerpや符号選択についての詳細は,この記事の上部でも紹介した「四元数まとめ資料」のうち”四元数の補間”の章をご参照ください.

コメントを残す