Kanadeの工作室

電気も機械もあるんだよ！

ジャイロセンサを使った簡単な姿勢推定

制御

機械系 Advent Calendar 2020の記事です

毎度阿鼻叫喚(?)でおなじみのｷﾘｷですが，角運動量とかこねくり回して問題は解けても実際の運動を記述できてる気がしなかった…というのも授業ではオイラー角を使って姿勢を表現できますよ～って説明があっただけなので…

ジャイロセンサを使って姿勢を求めると回転運動のお気持ちが完全に理解できたので記しておきます．

姿勢推定

姿勢の記述方法（回転行列）→姿勢と角速度の関係（微分方程式）→角速度センサから姿勢を推定するプログラム（離散化・差分方程式）

姿勢の表現

ｷﾘｷでは，剛体の無数の質点の運動（を決定するのに必要十分な，二点の運動）として剛体の運動を記述することから始めていますが，ここでは剛体に貼り付いた（固定された）座標系の運動として剛体の運動を記述します．となれば回転の自然な表現はオイラー角ではなく行列になるはずです．オイラー角は「姿勢の表現としては」数学的に不自然で扱いにくいのでここでは使いません．（角速度の表現として使います．オイラー角が自然でないというのは，可換でないということです．またジンバルロックという問題もあります．）（もう一つ回転の表現としてよく使われるものとして，クオータニオンがあります．クオータニオンは複素数の拡張で，複素数が平面の回転を表すのに適しているように，空間の回転をうまく表せます．）

グローバル座標系E(x,y,z)とセンサ座標系S(x',y',z')の変換として姿勢を表現します．

\begin{bmatrix} x'(t)\\y'(t)\\z'(t) \end{bmatrix} =R'(t) \begin{bmatrix} x(t)\\y(t)\\z(t) \end{bmatrix}

x',y',z'の成分表示を縦に並べるだけで回転行列になります．

角速度ベクトル

センサが微小回転したとき，センサ座標系の単位ベクトルの変位は，

\begin{bmatrix} x'(t+dt)-x'(t)\\y'(t+dt)-y'(t)\\z'(t+dt)-z'(t) \end{bmatrix} = \begin{bmatrix} x'(t) \times \omega dt \\y'(t)\times \omega dt \\z'(t)\times \omega dt \end{bmatrix}

角速度ベクトルwとの外積になります．

ジャイロセンサで計測される値(w1',w2',w3')は，角速度をセンサ座標系(x',y',z')で表現したものです． これはそれぞれx',y',z'軸を中心とした角速度，つまりオイラー角の時間微分になっています．じゃあそいつを積分したればええやんけ！とはならないんですね（残当）．一言で言えば微小回転は可換だが回転は非可換であるからです．（Rx(θ1),Ry(θ2),Rz(θ3)を順番を変えてかけたら一致しないが微小角なら一致することからわかります．）

角速度ベクトルの成分を使って，

w = \begin{bmatrix} w'_1\\w'_2\\w'_3 \end{bmatrix} ^T \begin{bmatrix} x'\\y'\\z' \end{bmatrix}

外積を計算してやると，

\begin{bmatrix} x'(t) \times \omega \\y'(t)\times \omega \\z'(t)\times \omega \end{bmatrix} = \begin{bmatrix} 0 & -\omega'_3 & \omega'_2 \\ \omega'_3 & 0 & -\omega'_1 \\ -\omega'_2 & \omega'_1 & 0 \end{bmatrix} \begin{bmatrix} x'(t)\\y'(t)\\z'(t) \end{bmatrix}

もとの式は

\begin{bmatrix} x'(t+dt)-x'(t)\\y'(t+dt)-y'(t)\\z'(t+dt)-z'(t) \end{bmatrix} = \begin{bmatrix} 0 & -\omega'_3 & \omega'_2 \\ \omega'_3 & 0 & -\omega'_1 \\ -\omega'_2 & \omega'_1 & 0 \end{bmatrix} \begin{bmatrix} x'(t)\\y'(t)\\z'(t) \end{bmatrix} dt

これをグローバル座標系で表すと

R'(t+dt) \begin{bmatrix} x\\y\\z \end{bmatrix} = R'(t) \begin{bmatrix} x\\y\\z \end{bmatrix} + \begin{bmatrix} 0 & -\omega'_3 & \omega'_2 \\ \omega'_3 & 0 & -\omega'_1 \\ -\omega'_2 & \omega'_1 & 0 \end{bmatrix} R'(t) \begin{bmatrix} x\\y\\z \end{bmatrix} dt

差分方程式

R'(t+dt) = R'(t)+\omega'R'(t) dt
\omega' = \begin{bmatrix} 0 & -\omega'_3 & \omega'_2 \\ \omega'_3 & 0 & -\omega'_1 \\ -\omega'_2 & \omega'_1 & 0 \end{bmatrix}

これを使ってジャイロセンサから姿勢を計算する ※Rは正規化します

実験

Arduinoでセンサ値をSerialでUnityに渡して表示する 使用したセンサ→秋月

Arduinoのコードは秋月のほぼサンプルです．Arduinoはセンサ値を送るだけで処理はUnity側でしてます．

Unityでシリアルを受け取る方法 → https://rikoubou.hatenablog.com/entry/2018/02/08/174506

周期を安定化

タイマ割り込みでサンプリング周期を安定させてます． これでも±10%の誤差が残ります．

#include<MsTimer.h>
#include<Wire.h>

void setup(){
  Wire.begin();
  Serial.begin(2000000);
  BMX055_Init();
  delay(300);
  MsTimer2::set(10,sense); //sense();を10msごとに実行
  MsTimer2::start();
}

void sense(){
  sei();
  // 略 センサを読んでSerialに送る
}

void loop(){
  // きょむ～
}

※Wireが割り込みを使っているのにMsTimer2が割り込みに飛ぶときに割り込み禁止にしちゃうので割り込み許可をするためにsei();する． 先人のおかげで沼らずにすんだ→ http://kuchem.kyoto-u.ac.jp/kinso/weda/blog/?date=20180107#c

結果

積分誤差が蓄積してます． カルマンフィルタとか使いましょう（） 最近はMadgwickフィルタなんてのがあるらしいですね．

₍ᐢ.ˬ.ᐢ₎ にゃーん