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

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

ただいまの
回答率

90.51%

  • Python 3.x

    6445questions

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

ジャイロセンサーから測定したquaternionによる姿勢推定を用いた加速度の回転方法について

解決済

回答 1

投稿 編集

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

yohehe

score 19

quaternionを用いた姿勢制御、それを用いた加速度への回転行列定義についてpythonのプログラムを作成使用としております。

ある動作における加速度とジャイロ動作を取得したとする。
30hz毎の測定時において
加速度a=(ax,ay,az)
ジャイロセンサーの値w=(wx,wy,wz)が取得できるとする。
座標系を用いて姿勢制御について検討する。

姿勢制御については、

#quaternionを用いた姿勢推定においては
q=np.array(qx,qy,qz,w)
#quaternion微分値をdqとすると 初期位置を0,0,0として座標系における計算とする。
dq=np.array([0,wz,-wy,wx],[-wz,0,wx,wy],[wy,-wx,0,wz],[-wx,-wy,-wz,0]).dot(q)

#初期位置を四元数を用いて(0,0,0,1)とすると
q_shoki=np.array(0,0,0,1)

#初期位置からの次点でのQuaternion q(t+dt)をq2とすると、
q2=q_shoki+dt

#q2を正規化する。
l=np.sqrt(q2[0]*q2[0]+q2[1]*q2[1]+q[2]*q[2]+q2[3]*q2[3])
q2_seiki=q2/l


#以上を繰り返してクォータニオンによって姿勢制御を求めていく
#角度での表記が必要であればオイラー角へq2をプログラムでオイラー角にする。


以上で座標系における角速度が求められる状態での表記が可能であるということがわかってきました。
この段階で間違えていたらすみませんが、アドバイスください。

このquarternionを用いた姿勢推定に加速度のベクトル成分を導入していくプログラムについてわからずにおります。
今の頭の中での考えでは、

#この姿勢制御に加速度の要素を入れて位置を推定するプログラムを作成していくとすると、


#Quaternionによる姿勢制御から座標系の回転を行う行列を定義
#q2における(q0,q1,q2,q3)とすして回転行列を定義

R=np.array([q0*q0-q1*q1-q2*q2+q3*q3,2(q1*q2+q3*q0),2(q3*q1-q2*q0)],
[2(q1*q2-q3*q0),q2*q2-q3*q3-q1*q1+q0*q0,2(q2*q3+q1*q0)],
[2(q3*q1+q2*q0),2(q2*q3-q1*q0),q3*q3-q1*q1-q2*q2+q0*q0])

#q2における加速度をa2とすると
a2=R.dot(a2)
#となると思われる。すみません、この方法で加速度を導入していくことが間違っているのかわからないでおります。
#以上の考え方が正しかったとすると、n次点での加速度a_nは
a_n=a1+a2+a3+,,,,,,an
#以上で計算することが可能??なのでしょうか。
コード


以上のコードの流れを作成しているのですが、以下の点がわかりません。

  1. 次点次点での姿勢における加速度の回転の導入方法の考え方がそもそもあっておりますでしょうか。
  2. 取得できる加速度データは位置ベクトルと考えると、姿勢制御で得られたqを加速度を位置ベクトルとして処理すべきなのか。色々考えているのですが、わからないでおります。

すみませんが、よろしくお願いいたします。


7/2追記いたしました。

クォータニオン便利ノートより抜粋させていただいております。

![イメージ説明](4b4879968eb5a2490aa553e1130434c1.png)

以上をみて考えたのですが、

クォータニオンの回転を行なったr=(x,y,z)をクォータニオンBで回転させたとする。

r'=Brが成立するのであれば、
Bの逆行列をB_1とすると両辺に左からB_1をかけて
B_1*r'=rが成り立つ。

ここでいうB_1が"加速度におけるセンサー座標系から基準状態に戻すクォータニオンである",との理解はあっていますでしょうか。

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

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

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

    クリップを取り消します

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

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

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

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

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

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

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

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

    質問の評価を下げる

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

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

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

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

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

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

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

    詳細な説明はこちら

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

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

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

回答 1

checkベストアンサー

+1

持っているもの
センサーの座標系での加速度
基準状態の座標系から現在のセンサー座標系への回転クォータニオン

欲しいもの
基準状態の座標系での加速度

やること
現在のセンサー座標系から基準状態に戻すクォータニオンを求めて
センサー座標系での加速度に作用させる

投稿

  • 回答の評価を上げる

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

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

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

  • 回答の評価を下げる

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

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

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

  • 2018/07/01 21:40

    ozwk様、いつもすみません。本当にありがとうございます。
    加速度へのクォータニオンについて頭のなかでの整理ができました。

    センサー座標系から基準系にも戻すクォータニオンとは、
    各次点におけるクォータニオンq(x,y,z,w)における逆quaternion(単純quaternionにおいては逆クォータニオンと共役クォータニオンはイコールだとすると共役でもいい)ということだと自分では考えたのですが、、あってますでしょうか。

    キャンセル

  • 2018/07/01 21:54

    q(t+dt)=q(t)+q(dt)、wを座標系の角速度だとしてセンサー座標系へのクォータニオンを求めるとする。その場合において、
    q(dt)=0.5*q*wとしているとすると、
    qに戻すような 2*w^-1(wの逆行列の意味)が基準系に戻す式が基準座標系に戻すという式になる?とは間違っておりますよね。

    キャンセル

  • 2018/07/01 22:01

    普通に共役で

    キャンセル

  • 2018/07/01 22:03

    ありがとうございます。共役quaternionで進めてみます。

    キャンセル

  • 2018/07/01 23:44

    今すすめているのですが、あと一つ質問させてください。

    加速度に共役クォータニオンを作用をさせるとは、加速度をquaternionの4元数表記にして加速度のquaternionとジャイロから得られているquaternionの積を取るということでよかったですか?

    キャンセル

  • 2018/07/02 07:59 編集

    (ある意味あってるけど)違います
    クォータニオン計算便利ノートに書いてあります

    とりあえずX軸に90度回転させるクォータニオンでも作って適当なベクトル相手に検算すればいいです。

    なんか対象が加速度だから難しく考えているのかもしれませんが
    そもそものやりたいことは、
    「センサー座標系でとれたこのベクトルは、基準座標系でいくつになるんだろう」ってだけです。(ですよね?)
    もし回転行列で扱っていたら回転行列かけて終わる話です

    キャンセル

  • 2018/07/02 20:12

    返信がおそくなり申し訳ありませんが、ozwk様ありがとうございます。
    基準座標系におけるベクトルがいくつになるか測定したいだけです。なかなか理解が追いつかずすみません。確かに加速度のために複雑に考えているのかもしれません。

    クォータニオン計算便利ノートにのクォータニオン表現による変換行列の(ii)だと思われるのでやってみます。ありがとうございます。

    キャンセル

  • 2018/07/02 22:47

    すみません、悩み悩んで追記させていただきました。

    ozwk様、本文に追記したのですが、Bの逆行列が求めるべき基準状態での加速度を求めるために使用するクォータニオンでこれを各次点での加速度にかけていくという判断で大丈夫でしょうか?
    何度も質問すみません。

    キャンセル

  • 2018/07/02 23:11

    用語はともかく大丈夫です

    適当に状況を設定して検算してみてください

    キャンセル

  • 2018/07/02 23:24

    ozwk様、ありがとうございます。プログラムを設定して検算してみます。

    キャンセル

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

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

関連した質問

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

  • Python 3.x

    6445questions

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