こんにちは。
最近センサから値を読み取り、なるほどと思ったことがあったのでここに書き共有すると同時に備忘録として活用しようと思います。
角速度→角度の積分
IMUセンサのジャイロから計測される角速度を単純に積分したい時があります。
積分誤差などが入ってしまってもとりあえず大丈夫な時。
そんな時は以下の式が使えます。
\begin{align}
\theta(k+1) = \theta(k)+(dt ・\omega(k))\tag{1}
\end{align}
上記の式をpythonで書くと以下のようになります。
Angle= Angle+dt*Omega
#Angle(k+1) = Angle(k)+dt*Omega(k) インデックスを付け加えるとこうなる微分方程式
\begin{align}
\frac{dx}{dt}=-2x
\end{align}
があったとします。このxの時間変化を知りたいときにオイラー法は便利です。
オイラーを用いると、次のように近似できます。
\begin{align}
\frac{dx}{dt} \approx\frac{x(t+\Delta T)-x(t)}{\Delta T}=-2x(t)\\
tをk\Delta Tとしてk=(0,1,2…)と離散化すると\\
\frac{x(k+1)-x(k)}{\Delta T}=-2x(k)\\
上記の式は次のように展開できます。\\
x(k+1)=x(k)+\Delta T(-2x(k))
\end{align}
角速度から角度を求めた式(1)は上の-２がない場合といえる。
例えば, 加速度センサから得られる加速度を積分して速度にしたいときは、
\begin{gather}
\frac{v(k+1)-v(k)}{\Delta t}\approx a(k)\\
v(k+1) = v(k)+\Delta t a(k)
\end{gather}
バイアスを考慮する
上の式でも最初のほうは問題なく角度を推定できます。
しかし下の図のように時間とともに積分誤差が蓄積されてしまいます。
このバイアスを取り除くこともできます。
\begin{align}
\begin{bmatrix}
bias_{rol}(k+1)\\
bias_{pit}(k+1)\\
bias_{yaw}(k+1)\\
\end{bmatrix}
=
\begin{bmatrix}
bias_{rol}(k)\\
bias_{pit}(k)\\
bias_{yaw}(k)\\
\end{bmatrix}
+
I_{3}\Delta T
\end{align}
と考えてこれを同時に計算し、求めたバイアスを角速度から引いて使うとバイアスを消すことができます。下の図がバイアスを引いたジャイロで積分したものです。バイアスの初期値は0.1ずつ与えていますが、これはセンサによって変わってきます。
調べるとバイアスのダイナミクスもあるようですがここでは考えていません。
コメント