Help us understand the problem. What is going on with this article?

9軸センサで相補フィルタ使って姿勢推定

More than 1 year has passed since last update.

この記事は高知工科大 Advent Calendar 2018の12日目の記事です。

はじめに

9月末に行われた室内飛行ロボコン自動操縦部門に、回路・制御担当として参加してきました。
この記事ではそれに使った機体の姿勢推定について書きます。

概要

加速度3軸、ジャイロ3軸、地磁気3軸の9軸センサを使って姿勢推定をしました。
姿勢はクォータニオンとして保持します。

だいぶ前の動画ですがこんな感じになります。

クォータニオンについて

クォータニオンを使うと姿勢を1つのスカラーと3つのベクトルで表現することができます。

\boldsymbol{q}=q_0+q_1\boldsymbol{i}+q_2\boldsymbol{j}+q_3\boldsymbol{k}

クォータニオンは回転軸の単位ベクトル$\boldsymbol{n}$と、回転角$\theta$で表すことができ、
クォータニオン$\boldsymbol{q}$は以下の式(1)のようになります。

\boldsymbol{q}=\cos{\frac{\theta}{2}}+\boldsymbol{n}\sin{\frac{\theta}{2}}\tag{1}

つまり、$\boldsymbol{n}=[n_x,n_y,n_z]^{-1}$だとすると

\begin{bmatrix}
q_0 \\
q_1 \\
q_2 \\
q_3
\end{bmatrix}
=
\begin{bmatrix}
\cos{\frac{\theta}{2}} \\
n_x\sin{\frac{\theta}{2}} \\
n_y\sin{\frac{\theta}{2}} \\
n_z\sin{\frac{\theta}{2}} 
\end{bmatrix}

となります。

クォータニオンを用いると、ベクトルの回転を三角関数を用いずに処理できます。
回転後のベクトルを$\boldsymbol{r^{'}}$、回転を表すクォータニオンを$\boldsymbol{q}$とすると以下の式(2)のようになります。

\boldsymbol{r^{'}}=\boldsymbol{q^{-1}}\boldsymbol{r}\boldsymbol{q}\tag{2}

なお、$\boldsymbol{q^{-1}}$は$\boldsymbol{q}$の逆クォータニオンで、$\boldsymbol{q}$の逆回転を表すクォータニオンになります。以下で求められます。

\boldsymbol{q^{-1}}=\cos{\frac{\theta}{2}}-\boldsymbol{n}\sin{\frac{\theta}{2}}

考えてみれば当然ですが、回転軸単位ベクトルの符号を反転させるとそのまま逆回転になります。

さて、式(2)の計算を進めていきます。

\boldsymbol{r^{'}}
=A\boldsymbol{r}

とおくと、行列$A$は以下の3x3行列で表せます。

A=
\begin{bmatrix}
q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2+q_0q_3)        & 2(q_1q_3+q_0q_2) \\
2(q_1q_2-q_0q_3)        & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3+q_0q_1) \\
2(q_1q_3+q_0q_2)        & 2(q_2q_3-q_0q_1)        & q_0^2-q_1^2-q_2^2+q_3^2
\end{bmatrix}

この行列$A$を、DCMや、方向余弦行列といったりします。
以上の基礎知識をもとに、姿勢推定を考えます。

ジャイロのみ姿勢推定

ジャイロセンサから取得できる角速度を$\boldsymbol\omega=[\omega_x,\omega_y,\omega_z]^{-1}$とすると、クォータニオン$\boldsymbol{q}=[q_0,q_1,q_2,q_3]^{-1}$は以下の微分方程式で表せます。

\frac{d}{dt}
\begin{bmatrix}
q_0 \\
q_1 \\
q_2 \\
q_3
\end{bmatrix}
=
\begin{bmatrix}
0        &-\omega_x &-\omega_y &-\omega_z \\
\omega_x & 0        & \omega_z &-\omega_y \\
\omega_y &-\omega_z & 0        & \omega_x \\
\omega_z & \omega_y &-\omega_x & 0
\end{bmatrix}
\begin{bmatrix}
q_0 \\
q_1 \\
q_2 \\
q_3
\end{bmatrix}\tag{3}

これを時間で積分してやると姿勢のクォータニオンが得られます。
しかしながら、ジャイロセンサのみだと、ジャイロセンサの誤差が積分で累積してしまい、ドリフトが発生してしまいます。

ジャイロ+加速度センサで姿勢推定

よって、ジャイロセンサの誤差を加速度センサで補正します。
方法としては、カルマンフィルタが有名ですね。今回は比較的直感的に理解しやすい方法を使った相補フィルタを用います。

考え方としては、もしジャイロセンサに誤差が存在しないと仮定すると、角速度のみで更新したクォータニオンで加速度ベクトルを回転させた場合、そのベクトルは重力加速度のベクトル$\boldsymbol{g}=[0,0,1]^{-1}$に一致するはず。一致しなかった場合、そこから一致させるような回転のクォータニオンをつくり係数をかけ、角速度のみで更新したクォータニオンにかけることで補正するというものです。

文章だとわからないと思うので、ステップごとに図解してみます。

1.ジャイロセンサのみで姿勢推定

まず、ジャイロセンサのみでクォータニオンを更新します。このクォータニオンを$\boldsymbol{q_{\omega}}$とすると、式(3)と同様にクォータニオンを更新できます。

$\boldsymbol{q_{\omega}}$で表される回転は、以下の図のようになります。黒矢印が絶対軸、赤矢印がセンサ軸を表します。
image.png

2.加速度ベクトルを更新したクォータニオンで回転

加速度センサより取得した加速度ベクトル$\boldsymbol{a}=[a_x,a_y,a_z]$を$\boldsymbol{q_{\omega}}$で回転させます。回転後の加速度ベクトルを$\boldsymbol{a^{'}}$とすると、式(2)より以下のようになります。

\boldsymbol{a^{'}}=\boldsymbol{q^{-1}_{\omega}}\boldsymbol{a}\boldsymbol{q_{\omega}}

image.png

紫矢印が重力加速度のベクトル。センサ軸上では$\boldsymbol{a}$ベクトル。

このとき、$\boldsymbol{q_{\omega}}$に誤差が含まれていない場合、回転後の$\boldsymbol{a^{'}}$の成分は、初期状態の$\boldsymbol{a_{0}}=\boldsymbol{g}=[0,0,1]$と同じになるはずです。

3.誤差を補正するクォータニオンをつくる

しかしながら、実際は誤差が含まれているので、成分は一致しません。
これを一致させるような回転を表すクォータニオン$\boldsymbol{q_{a}}$をつくります。

image.png

クォータニオンは回転軸単位ベクトルと回転角でつくることができることは説明しました。

2つのベクトル$\boldsymbol{a^{'}}$と$\boldsymbol{a_0}$の回転軸単位ベクトル$\boldsymbol{n_a}$は、外積を用いることで求められます。

\boldsymbol{n_a}=\frac{\boldsymbol{a^{'}}\times\boldsymbol{a_0}}{|\boldsymbol{a^{'}}\times\boldsymbol{a_0}|}

2つのベクトルの回転角$\theta_a$は、内積を用いることで求められます。

\theta_a=\arccos{(\boldsymbol{a^{'}}\cdot\boldsymbol{a_0})}

この$\theta_a$に、任意係数$\alpha(0<\alpha<1)$をかけたものを使ってクォータニオンをつくります。この$\alpha$が大きければ補正量も大きくなり、小さければ補正量も小さくなります。式(1)より

\boldsymbol{q_a}=\cos{\frac{\alpha\theta_a}{2}}+\boldsymbol{n_a}\sin{\frac{\alpha\theta_a}{2}}

4.クォータニオンを補正する

クォータニオン同士の積はそのまま回転の和になるので、補正後のクォータニオン$\boldsymbol{q^{'}}$は、

\boldsymbol{q^{'}}=\boldsymbol{q_{a}}\boldsymbol{q_{\omega}}

となります。

ジャイロ+加速度+地磁気で姿勢推定

ジャイロ+加速度だけでも誤差は補正できますが、加速度センサではオイラー角で言うヨー方向の誤差が補正できません。(姿勢を推定する指標になる重力加速度がZ軸=ヨー軸を貫いているため)

ここに地磁気センサを加えることで、ヨー方向の誤差も補正できます。
地磁気センサについては、このページが参考になるかもしれません。

やることは加速度センサのときと変わらないです。
注意することとして、加速度の場合は初期ベクトルがZ軸に貫く重力加速度ベクトルだったので$\boldsymbol{a_{0}}=[0,0,1]$でしたが、地磁気の場合は姿勢推定を開始したときの地磁気ベクトル$\boldsymbol{m_{0}}=[m_{x0},m_{y0},m_{z0}]$になります。
この初期ベクトルを方位で言う北のベクトルとし、初期クォータニオンをそれに則った値に設定すると、絶対方位におけるセンサの姿勢が得られます。室内ではあまり意味はないですが、屋外でなんか動かすときは方位わかると便利ですね。

まとめ

いいかんじにセンサフュージョンができた。

Tanba28
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした