【倒立振子part12】Atom Matrixでの角度導出

今回は倒立振子の傾き角度を求めるためにAtom Matrixでの角度導出する。

角度導出
実測
- 原因
次回

角度導出

ジャイロセンサ

Atom MatrixにはMPU6886という6軸のジャイロセンサが搭載されている。
https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/docs/datasheet/core/MPU-6886-000193%2Bv1.1_GHIC_en.pdf

6軸とはX,Y,Z軸周りの角速度、角加速度を測定できるセンサのことである。
Atom Matrixでは下記の様に設定されていることを確認した。

軸周りの角度は
・軸周りの角速度を積分
・軸方向の角加速度から三角関数で導出
の2パターンで求められる。
今回は倒立振子に使用するY軸周りについて考えていく。

軸周りの角速度を積分

角度と角速度の関係は下記で求められる。
$\displaystyle{\theta=\int_{t0}^{tf} \omega{dt}}$

軸方向の角加速度から三角関数

Y軸周りの角度はX軸方向の角加速度accXとZ軸方向の角加速度accZのベクトルから三角関数によって求められる。

ここでベクトルの方向を考えるとZ軸方向の角加速度は負の方向になることを考慮すると
$\displaystyle{\theta=\tan^{-1}\left(\frac{g_x}{-g_z}\right)}$
によって求められる。

実測

上記で求めるた２種類の角度の測定方法で実測した結果を下記に示す。

測定結果より
・6秒以降の角速度から求めた角度の変化から分かる通りドリフトが発生している
・角加速度から求めた角度はノイズが多く正確に測定できてない
という問題が発生しているのが分かる。

角度を求める方法の違いによるメリットデメリットを下記にまとめる。

どちらかだけで角度を測定するのは正確に測定できないことが判明した。

原因

角加速度によって角度を求める際、突発的な動きに対して本体の重力gが真下にならないためノイズが乗ると考えられる。
また角速度から角度を求める際の計算式をもっと正確に記載すると各時間における角速度を $\omega_k$ 、初期角度を $\theta_0$ 、データを取り込む一定時間を $\delta{t}$ とするとn回目の取り込んだ時の角度 $\theta_n$ は下式で示される。
$\displaystyle{\theta_n=\theta_0+\sum_{k=1}^n\omega_k\Delta{t}}$
また実際のジャイロセンサには一定のオフセット値が含まれるため実際の測定値は「 $\omega_k+\omega_{offset}$ 」となる。
オフセット値が含まれることを考慮した上で上式を変形すると、
$\displaystyle{ \begin{eqnarray} \theta_n &=& \theta_0+\sum_{k=1}^n(\omega_k+\omega_{offset})\Delta{t}\\ &=& \theta_0+\sum_{k=1}^n\omega_k\Delta{t}+n\cdot\omega_{offset}\cdot\Delta{t} \end{eqnarray}}$
この式の最後の項である「 $n\cdot\omega_{offset}\cdot\Delta{t}$ 」は時間nが大きくなるにつれて増加することが分かる。
これがドリフトが発生している原因となっているが、オフセット誤差はジャイロセンサの特性から完全に0にできないため他の方法によって補完する必要がある。

次回

正確な角度を求めるためにカルマンフィルタを導入する。