角速度から姿勢推定を行う際のクォータニオンの積分はどのようにやればいいのでしょうか

ジャイロデータで測定した角速度での姿勢制御のプログラムをpythonで作成しております。
１.クォータニオンの初期値設定
2.クォータニオン微分
３.クォータニオン積分
4.正規化
という流れを現在作成しているのですが、2,3の流れの手法がわからないでおります。

python
1#測定したジャイロデータをwx,wy,wzとする。
2
3
4#４元数の初期値設定
5q0=0
6q1=0
7q2=0
8q3=1
9#初期値から１回分の角速度計算を行うとする。
10#クォータニオンの微分式より
11data=1/2*np.array([[-q1,-q2,-q3],[q0,-q3,q2],[q3,q0,-q1],[-q2,q1,q0]]).dot(np.array([wx[0],wy[0],wz[0]]).T)
12 
13コード

ここで出力されたdataは微分式d/dt[]になっていると思うのですが、4元数クォータニオンの積分はどのように行うべきなのでしょうか?
30hzで測定した場合は、dataのarrayそれぞれに0.2をかけていくとおかしくなっていく気がするのですが。

行動規範の内容に同意します

回答2件

ベストアンサー

q(t+dt) = q(t)q(dt)
= q + dq
= q + 0.5q*w
dqは座標系回転なのでqとw(角速度)の順番にに注意してください。

クォータニオン計算便利ノートによくまとまってます

30hzで測定した場合は、dataのarrayそれぞれに0.2をかけていくとおかしくなっていく気がするのですが。

前から気になっていたんですが、30Hzでなぜ0.2?

投稿2018/06/30 04:40

編集2018/06/30 05:04

ozwk

総合スコア13521

yohehe

2018/06/30 05:21

ozwk様、先日に引き続きありがとうございます。すみませんヘルツについて記載間違えてました、30Hzで測定するとすると1/30ですね。クォータニオン計算便利ノートをもう一度読みます。 q(t+dt) = q(t)*q(dt) = q + dq = q + 0.5*q*w で頭の中で理解できました!全く理解不足ですみません、ありがとうございます。頭の中で混乱しているのが問題だとは思うのですが、 q+dqを行なって進めていった場合に、quarternionは定義上ノルム１となっていると思われるのですが、和については問題ないのでしょうか。

yohehe

2018/06/30 05:25

クォータニオンの積分を行なって、定義上のノルムのクォータニオンに戻す、正規化なのですね。すみません。理解できてきたと思います。

ozwk

2018/06/30 05:49 編集

はい、最後に正規化を行います。ちなみにdqは微小回転であるという前提なので、角速度wが大きい値だとdqが微小ではなくなって、誤差が大きくなります。ちゃんとした方法があった気がするのですが、雑な方法としては、wのノルムが大きくなったら、wを適当に時間分割して細かく足します

yohehe

2018/06/30 05:49

角速度の値が大きいとdqが大きくなることで誤差が大きくなっていくことが数式からも理解できました。 ozwk様ありがとうございます。

yohehe

2018/06/30 05:59

wのノルムについて閾値などを設定し、分割する方法があるのですか。勉強してみます。

行動規範の内容に同意します

クォータニオンも行列による回転行列も本質的には「同じもの」です。クォータニオンは「回転軸のベクトル」と回転角度をパラメーターとして与えますが同じ変換を回転行列でも表現できます。

「クォータニオン」「回転行列」といった用語の違いにとらわれすぎてその本質を押さえないでいるとどう取り扱えばよいかが把握できないのではないでしょうか。

これを理解するには例えば
https://qiita.com/kenjihiranabe/items/945232fbde58fab45681
http://www.mss.co.jp/technology/report/pdf/18-07.pdf

のような解析記事を見てその内容を把握するとよいと思います。こうした解説を読むには幾何計算・線形代数の素養が必要ですがそれさえあれば理解はそれほど難しくはないと思います。

微分と積分

高校数学で学ぶ微積分は初等関数を用いた「代数式の変換」として学ぶと思いますが計算機で「計測値」による計算を行う場合は「代数式の変換としての微積分」ではなく「数値計算的に微積分を行う」ことが多いと思います。それを把握するには微積分の「公式」よりは「微積分とは何か」の本質の知識があればよいです。

例えばセンサーで検出する値はある時刻tにおける、関数G(t)の計測値がわかるだけであり数式が得られるわけではないですよね？数式が未知でもG(t)の具体値から「G(t)の変化の度合い」を計算することはできます。その変化の度合いが要するに「微分」ですよね？そして変化の度合いを累積するのが即ち積分です。我々の道具であるデジタル計算機は離散的なデータを数値として扱うアプローチをとりますので理論的な数式がわかってないような問題の場合はもっぱら数値計算に頼るしかありません。

このあたりのデータ処理の手法は数学の知識および数値計算の知識があると取り組みやすいです。質問者さんはその辺りは充分把握しておられるでしょうか？

例えばf(t)=t^2を例にとりtのインターバルを0.1ごとに計測した値0, 0.01, 0.04, 0.09, ...を用いて数値計算的にf(t)の数値微分とf(t)の数値積分をプログラミングできますか？それがわかればクォータニオンに対しても同様のアプローチで行けると思います。

余談(?)ですが携帯機のジャイロセンサーは「地球の引力」＋「人間（など）が携帯機にかける力（加速度）」といった記事を見ました。もしそうならセンサーの検出値を単純に微分したり積分しても携帯機の軌跡とは一致しないように思います。
ある時刻tでセンサーからの検出値G(t)が得られたとして、静止状態でのt0, G(t0)から連続して得られるG(t)を調べながら初期状態での携帯機の姿勢、引力以外に携帯機にかかった力の変化、携帯機の姿勢および位置の変化をトータルで考えないといけないのではないのかなーと思いました。(自分は携帯機のセンサーを使った本件のようなプログラミングに取り組んだことがないので勘違いしているかも知れません。間違ってたらご容赦を)

投稿2018/06/30 03:51

編集2018/06/30 04:33