質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

ただいまの
回答率

90.52%

  • Python 3.x

    6342questions

    Python 3はPythonプログラミング言語の最新バージョンであり、2008年12月3日にリリースされました。

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

解決済

回答 2

投稿

  • 評価
  • クリップ 0
  • VIEW 326

yohehe

score 19

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

#測定したジャイロデータをwx,wy,wzとする。


#4元数の初期値設定
q0=0
q1=0
q2=0
q3=1
#初期値から1回分の角速度計算を行うとする。
#クォータニオンの微分式より
data=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)

コード


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

  • 気になる質問をクリップする

    クリップした質問は、後からいつでもマイページで確認できます。

    またクリップした質問に回答があった際、通知やメールを受け取ることができます。

    クリップを取り消します

  • 良い質問の評価を上げる

    以下のような質問は評価を上げましょう

    • 質問内容が明確
    • 自分も答えを知りたい
    • 質問者以外のユーザにも役立つ

    評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

    質問の評価を上げたことを取り消します

  • 評価を下げられる数の上限に達しました

    評価を下げることができません

    • 1日5回まで評価を下げられます
    • 1日に1ユーザに対して2回まで評価を下げられます

    質問の評価を下げる

    teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。

    • プログラミングに関係のない質問
    • やってほしいことだけを記載した丸投げの質問
    • 問題・課題が含まれていない質問
    • 意図的に内容が抹消された質問
    • 広告と受け取られるような投稿

    評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。

    質問の評価を下げたことを取り消します

    この機能は開放されていません

    評価を下げる条件を満たしてません

    評価を下げる理由を選択してください

    詳細な説明はこちら

    上記に当てはまらず、質問内容が明確になっていない質問には「情報の追加・修正依頼」機能からコメントをしてください。

    質問の評価を下げる機能の利用条件

    この機能を利用するためには、以下の事項を行う必要があります。

回答 2

checkベストアンサー

+2

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

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


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

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

投稿

編集

  • 回答の評価を上げる

    以下のような回答は評価を上げましょう

    • 正しい回答
    • わかりやすい回答
    • ためになる回答

    評価が高い回答ほどページの上位に表示されます。

  • 回答の評価を下げる

    下記のような回答は推奨されていません。

    • 間違っている回答
    • 質問の回答になっていない投稿
    • スパムや攻撃的な表現を用いた投稿

    評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

  • 2018/06/30 14:21

    ozwk様、先日に引き続きありがとうございます。
    すみませんヘルツについて記載間違えてました、30Hzで測定するとすると1/30ですね。
    クォータニオン計算便利ノートをもう一度読みます。

    q(t+dt) = q(t)*q(dt)
    = q + dq
    = q + 0.5*q*w
    で頭の中で理解できました!全く理解不足ですみません、ありがとうございます。
    頭の中で混乱しているのが問題だとは思うのですが、
    q+dqを行なって進めていった場合に、quarternionは定義上ノルム1となっていると思われるのですが、和については問題ないのでしょうか。

    キャンセル

  • 2018/06/30 14:25

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

    キャンセル

  • 2018/06/30 14:34 編集

    はい、最後に正規化を行います。
    ちなみにdqは微小回転であるという前提なので、
    角速度wが大きい値だとdqが微小ではなくなって、誤差が大きくなります。

    ちゃんとした方法があった気がするのですが、
    雑な方法としては、wのノルムが大きくなったら、wを適当に時間分割して細かく足します

    キャンセル

  • 2018/06/30 14:49

    角速度の値が大きいとdqが大きくなることで誤差が大きくなっていくことが数式からも理解できました。

    ozwk様ありがとうございます。

    キャンセル

  • 2018/06/30 14:59

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

    キャンセル

+2

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

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

これを理解するには例えば
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 14:13

    KSwordOfHaste様、ありがとうございます。
    クォータニオンについて色々調べてみたのですが、複素数のようなarrayをベクトルとして理解しずらいのか、頭で理解していないまま進めていた気がいたします。クォータニオン計算便利ノートをもう一度よんで理解に勤めてまいりたいとおもいます。

    キャンセル

15分調べてもわからないことは、teratailで質問しよう!

  • ただいまの回答率 90.52%
  • 質問をまとめることで、思考を整理して素早く解決
  • テンプレート機能で、簡単に質問をまとめられる

関連した質問

同じタグがついた質問を見る

  • Python 3.x

    6342questions

    Python 3はPythonプログラミング言語の最新バージョンであり、2008年12月3日にリリースされました。