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

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

新規登録して質問してみよう
ただいま回答率
85.35%
Python

Pythonは、コードの読みやすさが特徴的なプログラミング言語の1つです。 強い型付け、動的型付けに対応しており、後方互換性がないバージョン2系とバージョン3系が使用されています。 商用製品の開発にも無料で使用でき、OSだけでなく仮想環境にも対応。Unicodeによる文字列操作をサポートしているため、日本語処理も標準で可能です。

Q&A

解決済

1回答

2720閲覧

姿勢角(クォータニオン)から角速度を求めたい

mah110.matthew

総合スコア6

Python

Pythonは、コードの読みやすさが特徴的なプログラミング言語の1つです。 強い型付け、動的型付けに対応しており、後方互換性がないバージョン2系とバージョン3系が使用されています。 商用製品の開発にも無料で使用でき、OSだけでなく仮想環境にも対応。Unicodeによる文字列操作をサポートしているため、日本語処理も標準で可能です。

0グッド

0クリップ

投稿2020/06/01 05:48

クォータニオンで表す時系列の姿勢角データがあり、それぞれの時刻の角速度を python3 で求めようとしています。
時刻 t の姿勢角を Qt、時刻 t+dt の姿勢角を Qt1 とします。
微小時間 dt の変化量 Qdt は 、
Qt1 = dQ * Qt
dQ = Qt1 * Qt.inverse()
で得ることができます。
"クォータニオン計算便利ノート"を読んで、(23) 式を適応できると考え展開し
dQ = [ [-Qt.x, -Qt.y, -Qt.z],
[ Qt.w, Qt.z, -Qt.y],
[-Qt.z, Qt.w, Qt.x],
[ Qt.y, -Qt.x, Qt.w] ] * [wx,wy wz]
から最小二乗法(numpy.linalg.lstsq)でW= [wx,wy wz] で求めようとしましたが
まったくうまく行っていません。
最小二乗法で求められたW を (23) 式に当てはめて値が一致しません。

角速度を求める手法としてどこが間違っているでしょうか。
気になって試したこととしては、
与えられた角速度から次の時刻の姿勢角を求める手法の場合、
dQが Qt1-Qt としているようですが、これを当てはめても改善はしません。
4x3行列になっているので最小二乗法がだめなのかと、疑似逆行列を試して
みましたが、これも同様でした。

確認に使ったスクリプトを載せます

import numpy as np import quaternion # pip install numy-quaternion Qt=quaternion.from_float_array(np.array([0.66767037843408, -0.174897494350767, 0.0985339568173285, -0.716880876844568])) Qt1=quaternion.from_float_array(np.array([0.667756371112699, -0.174757292439958, 0.0987686757272434, -0.716802668990944])) dQ=quaternion.as_float_array(Qt1*Qt.inverse().normalized()) Gt=np.array([ [-Qt.x, -Qt.y, -Qt.z], [ Qt.w, -Qt.z, Qt.y], [-Qt.z, Qt.w, Qt.x], [ Qt.y, -Qt.x, Qt.w] ]) omg, residuals, rank, s=np.linalg.lstsq(Gt ,dQ, rcond=-1) Et=np.array([ [0.0, -omg[0], -omg[1], -omg[2] ], [omg[0], 0.0, -omg[2], -omg[1] ], [omg[1], omg[2], 0.0, -omg[0] ], [omg[2], -omg[1], omg[0], 0.0 ]]) dQp=np.dot(Et,quaternion.as_float_array(Qt)) print(dQ, '\n', dQp)

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

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

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

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

guest

回答1

0

自己解決

numpy-quaternion に、
quaternion.quaternion_time_series.angular_velocity
という関数がありました。

投稿2020/06/07 23:45

mah110.matthew

総合スコア6

バッドをするには、ログインかつ

こちらの条件を満たす必要があります。

あなたの回答

tips

太字

斜体

打ち消し線

見出し

引用テキストの挿入

コードの挿入

リンクの挿入

リストの挿入

番号リストの挿入

表の挿入

水平線の挿入

プレビュー

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

ただいまの回答率
85.35%

質問をまとめることで
思考を整理して素早く解決

テンプレート機能で
簡単に質問をまとめる

質問する

関連した質問