ドローンGo! | 何時もの話っ！

オイラー角～ジンバルロック～クォータニオン

オイラー角
ジンバルロック現象、他の問題点
四元数
ロボットの姿勢変換の表現に
簡単な計算例
余談
参考文献
関連記事

オイラー角

オイラー角とは、3次元ユークリッド空間中の2つの直交座標系（デカルト座標系）の関係を表現する方法の一つである。ロボット、ドローンの姿勢（ポーズ、向き）を直観に表現するには、オイラー角を用いることがある。オイラー角またはオイラー回転は3つの角度の組に、3座標軸まわりの回転順序（計12通りもある）で表されるので、オイラー角の変数は4つである。ロボットが姿勢$v$(ローカル座標系$XYZ$)から姿勢$v’$(ローカル座標系$X^{\prime \prime}Y^{\prime \prime}Z^{\prime \prime})$へ変わる際に、例えば$X$軸-$Y’$軸-$Z^{\prime \prime}$軸の順に$XY’Z^{\prime \prime}$系オイラー角 $\alpha,\beta,\gamma$ で3回順次回転することで姿勢の転換を表すことが可能である。【姿勢$v’$の座標 = 回転行列・姿勢$v$の座標】より、姿勢$v$の座標から回転後の姿勢$v’$の座標が求められる。姿勢$v$の座標、姿勢$v’$の座標が分かれば回転行列からオイラー角が求められる場合ある。この場合、積 $ R_{x}( \alpha ) R_{y}( \beta ) R_{z}( \gamma )$ は、$XY’Z^{\prime \prime}$回り順で表したときのオイラー角が $ (\alpha,\beta,\gamma) $ であるような回転を表す回転行列$ R_{xyz}(\alpha,\beta,\gamma) $である。

また、回転軸が固定座標軸か機体座標軸により、オイラー角が以下の2種類ほどある。
・固定座標系の3つの軸を中心とした回転、座標軸は静止しているので、固定角という。
・機体座標系の3つの軸を中心とした回転で、回転中に座標軸がロボットとともに回転するため、オイラー角という。

ロボットやドローンが移動の際、固定座標系を参照できないと、姿勢の転換を表すオイラー角を使うしかない場合がある。

ジンバルロック現象、他の問題点

オイラー角の問題点として、オイラー角を使用すると、オイラー角そのものの定義より、ジンバルロック現象を起こしてしまう場合がある。もともとジンバル装置(静態軸ある)で起きる現象で、航空機(剛体)も起きる。但し、固定角にはジンバルロック現象が起きない【※訂正あり】。上記オイラー角の2番目の回転角度$ \beta = ±π/2 $であれば、1番目回転$ \alpha $と3番目の回転$ \gamma $が同じ回転軸になって、つまり$ X $軸と$ Z^{\prime \prime} $軸が重なることになる。また回転行列より、$ \alpha – \gamma = C$にすると、$ \alpha,\gamma $の解は無限にあることと、1自由度が失われて、3次元空間と2次元空間の違いを考えると、このような3回転はベクトルを平面あるいは錐面に限定されて、すべての3次元ベクトルへ姿勢を変換されず、2自由度にロックされたようにしか考えられぬ現象をジンバルロック現象と呼ばれる。また1ジンバルを1自由度に当てはまると、ジンバルロックといわれるのは分かりやすいだろうか。$ \beta \neq ±π/2 $であれば、例$ \beta = \pi /3 $、$ (\alpha , \pi /3 , \gamma) $の組み合わせであらゆるベクトルに変換可能となる。なお他にジンバルロック現象を起こさないオイラー角$ (\alpha,\beta,\gamma) $に対して、$ (\alpha+π,π−\beta,\gamma+π) $の回転で同じ姿勢になる問題点もある。

よって、以下の条件付きであれば、ジンバルロックが除けて、オイラー角でロボットの姿勢を表すのが一意になる。言い換えれば、回転行列がオイラー角と一対一に対応することになる。

・ $ \beta ∈ (-π/2, π/2), \alpha,\gamma ∈ [-π, π] $

しかし、オイラー角$ (\alpha,\beta,\gamma) $を制限するのは無理の場合、オイラー角から四元数（Quaternion、クォータニオン）の登場となる。

また、オイラー回転の2番目の回転軸が1番目の回転軸 x 3番目の回転軸の外積の方向にある。つまり、2番目の回転軸が1番目と3番目の回転軸と直交する。当然、1番目と3番目の回転軸が必ずしも直交ではないが、ただし1番目と3番目の回転軸の外積$=0$つまり重なるのであれば、ジンバルロックに導く要因となる。

四元数

姿勢あるいは回転の表現には、オイラー角の3回転よりも、単純に回転軸と回転軸まわりの1回転で済む場合、四元数つまり軸回りの回転を表す四元複素数$( w + ix + jy + kz , w, x, y, z $は実数、$ i,j,k $は$XYZ$軸に対応する虚数単位$)$が用いられる。四元数の変数の個数がオイラー角の回転つまり$ [\alpha:Roll,\beta:Pitch,\gamma:Yaw] $の回転順と同様に4つであり、四元数を見るだけでは姿勢転換のイメージが難しいが、回転軸と回転角の表現に変わると一目瞭然になる。よって、一般論として3次元までの空間の回転の表現に必要な変数の数=空間の次元数+1となる。回転行列からオイラー角を求めるのと、逆に回転順とオイラー角から回転行列を求めるのが面倒だが、四元数を用いて回転を表現すると気持ちがすっきりになる。

例えば、以下のように、ベクトル$ v=iv_x + jv_y + kv_z$ で表すロボットの姿勢Vを原点を通る単位ベクトル$ a=ia_x + ja_y + ka_z $を回転軸として、右ねじが進む方向に回転角$θ$だけ回転させて、ベクトル$ v’=iv’_x + jv’_y + kv’_z $で表すロボットの姿勢V’になると、以下に優雅なる表現がある。$$ v’=q v \bar{q} \\ q=cos \frac{θ}{2} + a sin \frac{θ}{2} \\ \bar{q}=cos \frac{θ}{2} – a sin \frac{θ}{2} $$
但し、原点を通る回転軸$a$に合わせ、右手の法則に従う回転角$θ ∈ [-π, π]$、$q$は原点を通る回転軸$a$と回転角$θ$を組合せた回転ベクトルを表す、正規化した四元数(ノルムは$1$)、3次元空間における任意の回転に$q$は必ず且つ唯一に存在する。$q,\bar{q}$は共役または、共軛な複素数、お互いの逆回転でもある。上記計算の結果となる回転後ベクトルの$ v’ $は回転前ベクトルの$ v$と同じく、必ず純複素数となって、つまり実数部はゼロになる。
$ ij=-ji=k, jk=-kj=i, ki=-ik=j $
$ i^2=j^2=k^2=-1, ijk=kk=-1 $
$ ||a||=1, \space ||q||=||\bar{q}||=1 $

また、回転 $q_1$に引き続き、回転 $q_2$を行う場合、次のように書くと良い。
$$ v’=(q_{2}q_{1})v(\bar{q_{1}}\bar{q_{2}}) $$

ロボットの姿勢変換の表現に

$v=(1,0,0)=i$をロボットの初期姿勢として、回転軸$a$回り、$\theta$だけを回転すると、式$v’=q v \bar{q}$を使って姿勢変換後の姿勢$v’$を求めるように、ロボットの姿勢変換に生かせるようになる。また慣性航法などにも利用されている。なお、四元数はあくまで数なので幾何的にどの表現なのか興味深いが、とにかくひとの脳に想像可能な3次元空間ベクトルに混同しないように注意する。$q$はROSで表現すると、$q.x=(a.x)sin \frac{θ}{2} \\
q.y=(a.y)sin \frac{θ}{2} \\
q.z=(a.z)sin \frac{θ}{2} \\
q.w=cos\frac{θ}{2}$となる。

簡単な計算例

例：台車ロボットはZ軸回り180°回転してポーズを求めよう。
回転前ポーズ$v$を機体前面の法線方向ベクトル$v=(1,0,0)=i$に、回転軸$a=(0,0,1)=k$、回転角$\theta=\pi$として、回転ベクトルは$q=cos \frac{θ}{2} + a sin \frac{θ}{2} = k$となる。回転後ポーズは、
$v’ = q v \bar{q} = (k) (i) (-k) = (k) (k) (i) \\= -i = (-1,0,0)$と求められる。
検証として、$z=0$なので複素平面$a+bi$の回転として考えて、$ (1) (i) (i) = (-1, 0, 0) $と分かる。$z=0$を補足すると、四元数の計算結果$(-1,0,0)$と一致することが分かる。

余談

複素平面$a+bi$に実数軸あるのに、なぜ四元数の実数部に対応する座標系の軸はないとか、なぜ三元数は聞いたことはないとかの質問が出るかもしれないが、従来の複素平面$a+bi$から拡張された三元数$a+bi+cj$はどうも都合は悪くて、使い物にならないので、発明者のHamilton氏（英・数学物理学者）は悩んだすえ、新たに四元数に導いたそうだ。以来、三次元空間における姿勢変換の表現に大いに活用されて、四元数そのものの幾何表現は難問。深くまで掘り下げると、矢野忠先生の「四元数の発見」をご一読ください。

【※訂正】固定角にも、第2回転軸回りの回転角度によって、ジンバルロック現象が起きる。

参考文献

・wiki: オイラー角
・wiki: 四元数
・四元数まとめ資料 – 宇宙電波実験室

9軸IMU 6軸／9軸フュージョン低遅延 USB出力補正済み ROS対応
 ROS・Unity・ロボット・ドローン姿勢の回転表現
 SLAM～拡張カルマンフィルタ
 SLAM～Unscentedカルマンフィルタ
 粒子フィルタ
 6軸IMU～拡張カルマンフィルタ
 研究開発用台車型ロボットキット

ロボット・ドローン部品お探しなら

概要

6軸IMU(慣性センサ)＝3軸加速度センサ + 3軸ジャイロセンサに対して拡張カルマンフィルタをかけてセンサ融合$(Sensor Fusion)$またはデータ同化$(Data Assimilation)$を行う。$3$軸加速度センサにてセンサローカル座標系(機体座標系と称す）の$3$軸における重力加速度gの分量が出力され、観測方程式によりオイラー角の$θ,ϕ$が計算できる。$3$軸ジャイロセンサから機体座標系の$3$軸まわりの角速度が出力され、状態方程式によりオイラー角の$ψ,θ,ϕ$が計算できる。また、ランダム誤差(ノイズ)が存在し、そしてドリフトが蓄積していくので、拡張カルマンフィルタの状態空間モデルでの補正を行う。$6$軸$IMU$慣性センサまたは$9$軸$IMU$慣性センサは、ロボット、ドローンなどの位置推定、姿勢推定などに大いに活用されている。本文の固定座標系は地面から見た、Z軸はUP方向の右手の法則に従う座標系とする。機体座標系は移動体にあったIMUセンサの座標系で、最初に機体座標系は固定座標系と重なるとする。固定座標系の$Z, Y, X$軸回りの固定角はヨー、ピッチ、ロールとする。本文の姿勢の表現は$ZY’X^{\prime \prime}$軸回り順のオイラー角$ψ, θ, ϕ$とする。

3軸加速度センサ

オイラー角

$ZY’X^{\prime \prime}$軸回り順として、つまり$Z$軸を第$1$回転軸、回転角度はオイラー角$ψ$、$Y’$軸を第$2$回転軸、回転角度はオイラー角$θ$、$X^{\prime \prime}$軸を第$3$回転軸、回転角度はオイラー角$ϕ$、即ち$R_z(ψ)→R_y(θ)→R_x(ϕ)$と記す場合、機体座標系とオイラー角の対応関係は以下の図に示す。オイラー角は必ず軸まわりの回転順と絡むので、変数は$4$つと考えても良い。本文では、オイラー角の回転方向と回転軸の関係は右手の法則に従う。

回転行列

固定座標系ベクトル$p$は物体座標系とともに、$Z→Y’→X^{\prime \prime}$軸まわり順として、それぞれオイラー角の$ψ, θ, ϕ$の回転移動を行って物体座標系ベクトル$p’$へ変換された場合、固定座標系ベクトル$p$、機体座標系ベクトル$p’$、三次元空間回転行列$R_{zyx}$の関係は、以下の式で表現できる。
$$ p=R_{zyx}p’ \\ R_{zyx} = R_z(ψ)R_y(θ)R_x(ϕ) \\ R_z(ψ) = \begin{bmatrix} cosψ & -sinψ & 0 \\ sinψ & cosψ & 0 \\ 0 & 0 & 1 \end{bmatrix} $$ $$ R_y(θ) = \begin{bmatrix} cosθ & 0 & sinθ \\ 0 & 1 &0 \\ -sinθ & 0 & cosθ \end{bmatrix} $$ $$ R_x(ϕ) = \begin{bmatrix}1 & 0 & 0\\ 0 & cosϕ & -sinϕ \\ 0 & sinϕ & cosϕ \end{bmatrix} $$ 本文の機体座標系(オイラー角)回転行列$R$の計算は右乗として、また固定座標系(固定角)回転行列$R’$の計算は左乗に注意する。$R_{xyz}=R_{zyx}^{\prime},R_{zyx}=R_{xyz}^{\prime} $の関係がある。

観測方程式

$$ p’ = \begin{bmatrix} a_x & a_y & a_z \end{bmatrix}^T \\ p = \begin{bmatrix} 0 & 0 &-g \end{bmatrix}^T $$ ただし、$g$は重力加速度$ (\simeq 9.81m/s^2 ) $、$a_x, a_y, a_z$は重力加速度が機体座標系$X^{\prime \prime}Y^{\prime \prime}Z^{\prime \prime}$軸における成分。
$$ p’ = R^{-1}_{zyx}A = R_{x}^{-1}R_{y}^{-1} R_{z}^{-1} p $$ $$ \small R^{-1}_{zyx} = \begin{bmatrix} CθCψ & CθSψ & -Sθ \\ SϕSθCψ-CϕSψ & SϕSθSψ+CϕCψ & SϕSθ \\ CϕSθCψ+SϕSψ & CϕSθSψ-SϕCψ & CϕCθ \end{bmatrix} $$ ただし、$C = cos, S = sin $とする。
よって、$3$軸加速度センサの観測方程式は以下の式で表す。
$$ \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix} = \begin{bmatrix} gsinθ \\ -gsinϕcosθ \\ -gcosϕcosθ \end{bmatrix} $$

オイラー角$ϕ,θ$

よって、重力加速度$g$の分量により$3$軸加速度計センサのオイラー角が以下の式で求める。
$$\begin{bmatrix} ϕ \\ θ \end{bmatrix} = \begin{bmatrix} tan^{-1} \frac{a_y}{a_z} \\ tan^{-1}\frac{a_x}{\sqrt{a_y^2 + a_z^2}} \end{bmatrix} $$ ただし、ここでジンバルロック現象を避けたいので、$ϕ, θ ≠ ±90°$と限定する。また、ジンバルロック現象について、本サイト記事のオイラー角～ジンバルロック～クォータニオンで説明する。

なお、$ψ$角は、$3$軸加速度センサだけでは推定できないゆえに、$3$軸ジャイロセンサまたはコンパスセンサが必要になる。しかし、センサ融合手法として活用された拡張カルマンフィルタは、複数のセンサとも推定できる角度しか求めないので、$ψ$角は、以下の拡張カルマンフィルタの状態方程式から除外される。$ψ$角の求め方について、角速度$ω_z$の時間積分して単独に求められる。または、$ψ$角が別途観測可能なコンパスセンサを加え、これで9軸IMU = 6軸IMU + 3軸地磁気センサで拡張カルマンフィルタが適用するようになる。

3軸ジャイロセンサ

状態方程式

ジャイロセンサから測定した機体座標系の$X^{\prime \prime}Y^{\prime \prime}Z^{\prime \prime}$軸回りの角速度成分は、$ZY’X^{\prime \prime}$軸回りのオイラー角の時間微分$\dot{ϕ}, \dot{θ}, \dot{ψ} $との対応関係は以下の式で表す。
$$\scriptsize \begin{bmatrix}ω_x\\ω_y\\ω_z\end{bmatrix} = \begin{bmatrix} \dot{ϕ} \\ 0 \\ 0 \end{bmatrix} +R_x^{-1}(ϕ) \begin{bmatrix}0 \\ \dot{ θ}\\ 0 \end{bmatrix} + [R_y(θ)R_x(ϕ)]^{-1}\begin{bmatrix}0 \\ 0 \\ \dot{ψ}\end{bmatrix}$$ よって、回転行列を利用して、機体座標系の角速度から、$ZY’X^{\prime \prime}$軸回りのオイラー角の時間微分は求められる。$$\small\begin{bmatrix}\dot{ϕ}\\\dot{θ}\\\dot{ψ}\end{bmatrix} = \begin{bmatrix}1 & \frac{sinϕ sinθ}{cosθ}& \frac{cosϕ sinθ}{cosθ}\\0&cosϕ&-sinϕ\\0 & \frac{sinϕ}{cosθ} & \frac{cosϕ}{cosθ}\end{bmatrix}\begin{bmatrix}ω_x\\ω_y\\ω_z\end{bmatrix} $$ ただし、$ θ ≠ ±90° $ジンバルロック回避、 $\dot{ϕ}, \dot{θ}, \dot{ψ} $はそれぞれオイラー角の時間微分、$ω_x, ω_y, ω_z$は機体座標系の$X^{\prime \prime}Y^{\prime \prime}Z^{\prime \prime}$軸回りの角速度成分となる。

オイラー角で状態方程式の表現

ジャイロセンサの角速度からオイラー角の計算式は以下となる。$$\begin{bmatrix}ϕ\\θ\end{bmatrix} = \begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}$$ ただし、$ψ$角の積分計算はここで省略する。

誤差の補正

カルマンフィルタおよび本文の及んだ誤差はランダム誤差といい、バイアス誤差はすでにキャリブレーションなどの手法で取り除いたと想定する。加速度センサの精度が悪く、とくに運動中の場合、ただし加速度センサの誤差が蓄積しない。ジャイロセンサの精度が良く、ただしドリフト現象が起きる。ドリフトにより時間とともに誤差がじわじわと蓄積していく。補正手法として、加速度センサにジャイロセンサの出力値を組み合わせて、静的重み係数(経験値)を付ける相補フィルタの他、時系列分析から異なるセンサの出力値に信頼性に関わる動的重み係数を付ける、いわゆるカルマンフィルタ$(Kalman Filter)$の再帰的アルゴリズム(手法)がある。フィルタとはランダム誤差(ノイズ)をフィルタリングする意味合いがある。性能からみればカルマンフィルタのほうは優れるのでよく使われる。

$*$機体座標系で$3$軸加速度センサから傾斜角度と$3$軸ジャイロセンサから回転角度を求めて、静的重み付け(経験値)で加算するセンサ融合の手法がある。出典5-をご参照ください。

カルマンフィルタ

$wikipedia:$ カルマンフィルタ$(Kalman Filter,KF$と略す$)$は、誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルタの一種である。この解釈では理解できたというわけではなく、以下カルマンフィルタ、拡張カルマンフィルタで状態を推定してみる。

状態空間モデル

一般、汎用的な状態方程式、観測方程式は以下のとおり。
$$\begin{eqnarray*}&& x_t = F_{t-1}(x_{t-1}^{true}+ p_{t-1} ) + q_{t-1}\\&& y_t = H_t(x_t^{true}) + r_t\end{eqnarray*}$$ ただし、$F(⋅),H(⋅)$は実装に際して、$3$軸加速度センサ、$3$軸ジャイロセンサに掲載した状態方程式、観測方程式のとおりで利用する。システム誤差、観測誤差$(q_t, r_t)$は期待値が$0$、分散が$(Q_t, R_t)$の独立ガウス分布にする。
$$\begin{eqnarray*}&& q_t ～ N(0,Qt)\\&& r_t ～ N(0,Rt)\\&& E[q_t r_t]=0 \\&& p_{t-1}=x_{t-1}-x_{t-1}^{true}\end{eqnarray*}$$

パラメータ初期化

$$\begin{eqnarray*}&& \hat{x}_0 = E[x_0]\\&& \hat{P_0} = E[(x_0-\hat{x_0})(x_0-\hat{x_0})^T]\end{eqnarray*}$$

状態値、共分散行列予測ステップ（t>0）

$$\begin{eqnarray*}&& \bar{x_t} = F_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = F_{t-1}\hat{P}_{t-1}F_{t-1}^T + Q_{t-1}\end{eqnarray*}$$ ただし、$\bar{x_t}$は予測値、$\bar{P_t}$は事前共分散行列、$Q_{t-1}=E[q_{t-1}q_{t-1}^T]$

カルマンゲイン、状態値、共分散行列更新ステップ

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$ ただし、$K_t$はカルマンゲイン、$\hat{x_t}$は状態推定値、$\hat{P_t}$は事後共分散行列、$R_{t}=E[r_{t}r_{t}^T]$

フローチャート

導出に関わる参考情報として、本サイト記事　カルマンフィルタの導出　に載ってある。

拡張カルマンフィルタ

拡張カルマンフィルタ$(Extended Kalman Filter,EKF$と略す$)$は、非線形フィルタリングである。前述した状態方程式、観測方程式より、以下の状態空間モデルの$f(⋅)$または$H(⋅)$が非線形関数であり、$6$軸$IMU$のオイラー角計算に拡張カルマンフィルタは適用される。テイラー展開より、$2$次微分以降の項目を省略して、非線形である$f(⋅)$の$1$次微分を線形化とし、カルマンフィルタのアルゴリズムが適用可能となる。しかし、$f(⋅)$の微分では$f(⋅)$の一部しか表現できず、この線形化処理($1$次微分)に誤差を大きく招く場合がある。ここでは$H(⋅)$は線形関数$I$であるため、$f(⋅)$のみを線形化する。
$$\begin{eqnarray*}&& \hat{F}_{t-1} =\frac{\partial f_{t-1}(x)}{\partial x}|_{x=\hat{x}_{t-1}}\end{eqnarray*}$$

状態空間モデル

$$\begin{eqnarray*}&& x_t = f_{t-1}(x_{t-1}^{true}+p_{t-1}) + q_{t-1}\\&& y_t = H_t(x_t^{true}) + r_t\end{eqnarray*}$$

パラメータ初期化

$$\begin{eqnarray*}&& \hat{x}_0 = E[x_0]\\&& \hat{P_0} = E[(x_0-\hat{x_0})(x_0-\hat{x_0})^T]\end{eqnarray*}$$

状態値、共分散行列予測ステップ（t>0）

$$\begin{eqnarray*}&& \bar{x_t} = f_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = \hat{F}_{t-1}\hat{P}_{t-1}\hat{F}_{t-1}^T + Q_{t-1}\end{eqnarray*}$$ ただし、$\bar{x_t}$は予測値、$\bar{P_t}$は事前共分散行列、$Q_{t-1}=E[q_{t-1}q_{t-1}^T]$

カルマンゲイン、状態値、共分散行列更新ステップ

フローチャート

拡張カルマンフィルタを6軸IMUへの適用

状態空間モデルは以下のようになる。
$$\scriptsize x_t=f_{t-1}(x_{t-1}^{true}+p_{t-1})+q_{t-1}=\begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}+q_{t-1}$$
$$y_t=H_t(x_t^{true})+r_t=\begin{bmatrix}ϕ\\θ\end{bmatrix}+r_t$$

予測値は以下のように求められる。
$$\bar{x_t}=\begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}\\\scriptsize =\begin{bmatrix}ϕ+Δt(ω_x+ω_y\cdot sinϕ\cdot tanθ+ω_z\cdot cosϕ\cdot tanθ)\\θ+Δt(ω_y\cdot cosϕ-ω_z\cdot sinϕ)\end{bmatrix}$$ ただし、$ω_x, ω_y, ω_z$はジャイロセンサからの観測値。
状態観測値は以下のように求められる。
$$y_t=\begin{bmatrix} ϕ \\ θ \end{bmatrix} = \begin{bmatrix} ϕ \\ θ \end{bmatrix} = \begin{bmatrix} tan^{-1} \frac{a_y}{a_z} \\ tan^{-1}\frac{a_x}{\sqrt{a_y^2 + a_z^2}} \end{bmatrix} $$ ただし、$a_x, a_y, a_z$は加速度センサからの観測値。

$1$次微分線形化(ヤコビアン行列)は以下のように求められる。
$$\hat{F}_t =\frac{\partial f_{t-1}(x)}{\partial x}|_{x=\hat{x}_{t-1}}\\=\begin{bmatrix}\frac{\partial (ϕ+\dot{ϕ}Δt)}{\partial ϕ}&\frac{\partial (ϕ+\dot{ϕ}Δt)}{\partial θ}\\\frac{\partial (θ+\dot{θ}Δt)}{\partial ϕ}&\frac{\partial (θ+\dot{θ}Δt)}{\partial θ}\end{bmatrix}\\\scriptsize=\begin{bmatrix}1+Δt(ω_y\cdot cϕ\cdot tθ-ω_z\cdot sϕ\cdot tθ)&Δt(\frac{ω_y\cdot sϕ}{c^{2}θ} + \frac{ω_z\cdot cϕ}{c^{2}θ})\\-Δt(ω_y\cdot sϕ+ω_z\cdot cϕ)&1\end{bmatrix}$$ ただし、$c = cos, s = sin, t = tan$

拡張カルマンフィルタによる姿勢推定動作の可視化

拡張カルマンフィルタを実装したPythonスクリプトはGitHubで公開した。
imu_ekf Repository@GitHub
実行した結果は以下のようになる。画像をクリックするとYouTubeへジャンプする。

カルマンフィルタの解釈

カルマンフィルタの原理は以下のイメージに示す。

カルマンフィルタでは、状態推定値を予測結果$x_t$（実装例ではジャイロセンサーデータ）と観測データ$y_t$（実装例では加速度センサデータ）の線形結合で合成し，その誤差(共分散)を最小にする推定法だと分かる。これはシステム誤差、観測誤差の数学期待が$0$のガウス分布との前提条件から由来した推定法である。しかし、カルマンフィルタをかけることで、状態推定値は予測結果と観測データの間にあるのは、真値からかなり乖離してしまう場合にあるのか。これはシステム誤差と観測誤差が無相関かつ直交という前提から、勿論真値が推定値と観測値の間にある結論を結ぶ考えである。また、ジャイロセンサーデータと、加速度センサデータとも観測値$y_t$にする方法がある。それぞれのパーフォーマンスの検証は、比較的精確な実験環境(比較用の高精度ジャイロセンサ、加速度センサ、エンコーダ、モータ)はないと、実は容易ではない。というよりも、シミュレーションをかけてカルマンフィルタのアルゴリズムを検証するのが、確実に可能である。

他のフィルタについて

$UKF(Unscented Kalman Filter)$は、非線形の測量方程式$F(⋅), H(⋅)$がテイラー展開より線形化するのではなく、重み付き線形回帰より線形化する手法。$UKF$の精度は拡張カルマンフィルタより高いという実験結果はある。また、線形問題に対しては、カルマンフィルタと効果が同様。$UKF,EKF$が、状態値、観測値ともガウス分布（正規分布）に従うまたは近似する場合限り、論理的に適用可能とされる。

ロボットSLAM～拡張カルマンフィルタ

上述の拡張カルマンフィルタが$IMU$センサへの応用のみならず、同じ理由で、時系列の移動ロボット$SLAM$の$Gmapping$法にも、エンコーダ情報（オドメトリ情報または$odom$情報）とレーザセンサのLidarスキャンマッチング情報をセンサ融合またはセンサフュージョンへ応用される。不整な環境でない限り、$Lidar$スキャンマッチング情報のみを使用した$Hector$法より精度が良いだろう。不整な環境でエンコーダ情報が正確な位置情報に大いに誤差を招くことがあるから、センサ融合にマイナス効果が出ると考えられる。また、高精度、高スキャンレートの$Lidar+IMU$を使用した$Hector$法などの手法はある。もう少し詳細な情報は、本サイト記事 SLAM～拡張カルマンフィルタに載ってある。

参考文献

1-wikipedia: カルマンフィルタ、オイラー角
2-Christopher J. Fisher氏: Analog Device AN-1057、アプリケーション・ノート、加速度センサーによる傾きの検出
3-田島洋氏: マルチボディダイナミクスの基礎―3次元運動方程式の立て方
4-萩原淳一郎氏ら: 基礎からわかる時系列分析　Ｒで実践するカルマンフィルタ・MCMC・粒子フィルタ
5-Bonnie Baker氏: センサ融合を加速度センサやジャイロスコープに適用

粒子フィルタ
 SLAM～Unscentedカルマンフィルタ
 9軸IMU 6軸／9軸フュージョン低遅延 USB出力補正済み ROS対応
 YDLIDAR G4=16m 薄型 ROS対応SLAM LIDAR
点検向け自律移動ロボットRED（薄型・小型）
研究開発用台車型ロボットキット

ロボット・ドローン部品お探しなら

ROS・Unity・ロボット・ドローン姿勢の回転表現

はじめに
オイラー角、回転行列の表現
回転ベクトルの表現
回転ベクトル→オイラー角
オイラー角→回転ベクトル
関連記事

はじめに

姿勢$v$から姿勢$v’$への変換はどう表現するのかは、ロボットや、ドローンの開発、運用に避けられない要素である。本文に出ているソースコードは、$Python$言語を用いる。

オイラー角、回転行列の表現

物体を$X$軸、$Y$軸($Y’$軸)、$Z$軸($Z”$軸)まわりの順にそれぞれオイラー角のロール角$ϕ$、ピッチ角$θ$、ヨー角$ψ$だけ回転させたときに、物体の姿勢の変換は、オイラー角と、もしくはオイラー角の三角関数を用いる以下の回転行列で表す。
$$\small R_{xyz} = \begin{bmatrix} CθCψ & -CθSψ & Sθ \\ SϕSθCψ+CϕSψ & -SϕSθSψ+CϕCψ & -SϕCθ \\-CϕSθCψ+SϕSψ & -CϕSθSψ+SϕCψ & CϕCθ \end{bmatrix} $$ ただし、$C=cos,S=sin$ とする。
しかし、以下のイラストのとおり、オイラー角による姿勢制御の弱点$(Gimbal \space Lock)$があり、例えば$Y’$軸回りを$90°$回転すると、$X’$軸と$Z$軸が同軸となってしまい、以降は姿勢制御(表現)ができなくなる。

このジンバルロックを解消するにはクォータニオンの使命となった。

回転ベクトルの表現

$ROS$では回転ベクトルのクォータニオン(四元数)$q=ix+jy+kz+w$は、

q=(x,y,z,w)

と表す。原点を通す回転軸を表す単位ベクトル$a=(ax,ay,az)$で、この回転軸まわり、角度$θ$だけを回転する場合は、クォータニオンは

(x,y,z,w)=(ax*sin(θ/2), ay*sin(θ/2), az*sin(θ/2)，cos(θ/2))

と表す。

以下の例では、$XY$平面$(z=0)$にて辺長$1m$の正方形に沿って例えば仮に自律移動ロボットに走行させて、動作を確認しよう。勿論$Gazebo$でも確認できる。

$ROS$では、ロボットの位置＆姿勢の表現について、ロボットの中心または、ロボットにある他のポイントの位置は$x,y,z(m)$、姿勢ポーズはクォータニオンで表す。

from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
...
locations['square_vertex_1'] = pose = Pose(Point(1.0,0.0,0.0), Quaternion(0.0,0.0,0.0,1.0))
locations['square_vertex_2'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_3'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_4'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
...

回転ベクトル→オイラー角

geometry_msgs::Quaternion orientation = msg->pose.pose.orientation;    
tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));    
double yaw, pitch, roll;    
mat.getEulerYPR(yaw, pitch, roll);

オイラー角→回転ベクトル

tf::Quaternion q;
q.setRPY(Out_X, Out_Y, Out_Z);
sensor_msgs::Imu imu_data;
imu_data.orientation.x=q[0];
imu_data.orientation.y=q[1];
imu_data.orientation.z=q[2];
imu_data.orientation.w=q[3];

オイラー角～ジンバルロック～クォータニオン
 9軸IMU 6軸／9軸フュージョン低遅延 USB出力補正済み ROS対応
 研究開発用台車型ロボットキット

ロボット・ドローン部品お探しなら

3DR Pixhawk Mini フライトコントローラーキット

概要
主な仕様
開封動画
販売サイト
関連記事

概要

3DR Pixhawk Mini Autopilot & Micro M8N GPS コンパス付き & PM06 PDBボードのフライトコントローラーキットです。3DR Pixhawk MiniオートパイロットはPixhawkの次世代進化バージョンです。元のPixhawkのサイズの約1/3で、より強力なプロセッサとセンサーを備えています。PPX4オープンハードウェアプロジェクトに基づいており、PX4フライトスタック用に最適化されています。

主な仕様

メイン・プロセッサ

・STM32F427 Rev 3

IOプロセッサ

・STM32F103

センサ

・Accel / Gyro / Mag：MPU9250
・Accel / Gyro：ICM20608
・気圧計：MS5611

電圧定格

・パワーモジュール出力：4.1～5.5V
・最大入力電圧：45V（10S LiPo）
・最大電流検出：90A
・USB電源入力：4.1～5.5V
・サーボレール入力：0～10V

サイズ・重量

・38x43x12mm
・15.8g

GPSモジュール

・GNSS受信機：ublox Neo-M8N
・コンパスHMC5983
・重量：22.4g
・サイズ：37x37x12mm

インタフェース

・1×UARTシリアルポート
・Spektrum DSM / DSM2 / DSM-X?衛星対応
・フタバSBUS?対応
・PPM合計信号入力
・I2C
・CAN
・ADC
・内蔵マイクロUSBポート

開封動画

販売サイト

・3DR Pixhawk Mini フライトコントローラーキット

研究開発用台車型ロボットキット

以上

ロボット・ドローン部品お探しなら

YDLIDAR G4=16m 薄型 ROS対応SLAM LIDAR

SLAM LIDAR-YDLIDAR G4 主なスペック
YDLIDAR G4 vs RPLIDAR A2M8 主なスペック比較
移動ロボット、AGVへの活用
タッチスクリーンへの活用
開封動画
SLAMパラメータGithub
YDLIDAR Github
販売ストア

SLAM LIDAR-YDLIDAR G4 主なスペック

・寸法　72mm(直径) x 41mm（厚さ）
・検出方法　360度回転式・三角法
・検出距離　max 16m
・サンプリング周波数 max 9kHz
・回転周波数　max 12Hz
・耐環境光 2kLux
・ROS対応　〇

YDLIDAR G4 vs RPLIDAR A2M8 主なスペック比較

仕様	YDLIDAR G4	RPLIDAR A2M8
メカニズム	360度モータ回転式	360度モータ回転式
計測法	三角法	三角法
ROS対応	〇	〇
寸法	Φ72mm x H41mm	Φ76mm x H41mm
本体重さ	270g	340g
最大起動電流	550mA	1500mA
定格電圧電流	5V450mA	5V500mA
検知距離	0.1～16m	0.15～12m
回転周波数	5Hz～12Hz	5Hz～12Hz
レンジ周波数	4kHz～9kHz	2kHz～8kHz
距離精度	<0.5mm(0.10～2.0m)、距離の1%以下(2.0m～16m)	0.5mm(0.10～1.5m)、距離の1%以下(1.5m～12m)
角度精度	0.3度(回転周波数7Hz)	0.45～1.35度
規模応用例	あり、EAI Robot	あり、Slamtec Robot

移動ロボット、AGVへの活用

タッチスクリーンへの活用

開封動画

SLAMパラメータGithub

YDLidar G4のSLAMパラメータ（ご参考までお願い致します）

YDLIDAR Github

YDLIDAR Github（メーカーアカウント）

販売ストア

令和２年３月再入荷いたしました。どうぞご利用下さい。
YDLIDAR G4 | ROBOT翔-電子部品ストア

ロボット・ドローン部品お探しなら

オイラー角

ジンバルロック現象、他の問題点

四元数

ロボットの姿勢変換の表現に

簡単な計算例

余談

参考文献

関連記事

概要

3軸加速度センサ

オイラー角

回転行列

観測方程式

オイラー角\(ϕ,θ\)

3軸ジャイロセンサ

状態方程式

オイラー角で状態方程式の表現

誤差の補正

カルマンフィルタ

状態空間モデル

パラメータ初期化

状態値、共分散行列予測ステップ（t>0）

カルマンゲイン、状態値、共分散行列更新ステップ

フローチャート

拡張カルマンフィルタ

状態空間モデル

パラメータ初期化

状態値、共分散行列予測ステップ（t>0）

カルマンゲイン、状態値、共分散行列更新ステップ

フローチャート

拡張カルマンフィルタを6軸IMUへの適用

拡張カルマンフィルタによる姿勢推定動作の可視化

カルマンフィルタの解釈

他のフィルタについて

ロボットSLAM～拡張カルマンフィルタ

参考文献

関連記事

はじめに

オイラー角、回転行列の表現

回転ベクトルの表現

回転ベクトル→オイラー角

オイラー角→回転ベクトル

関連記事

概要

主な仕様

メイン・プロセッサ

IOプロセッサ

センサ

電圧定格

サイズ・重量

GPSモジュール

インタフェース

開封動画

販売サイト

関連記事

SLAM LIDAR-YDLIDAR G4 主なスペック

YDLIDAR G4 vs RPLIDAR A2M8 主なスペック比較

移動ロボット、AGVへの活用

タッチスクリーンへの活用

開封動画

SLAMパラメータGithub

YDLIDAR Github

販売ストア