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

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

ただいまの
回答率

90.52%

  • Python

    7953questions

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

pythonを使用した加速度とジャイロセンサーによる回転行列について

解決済

回答 1

投稿 編集

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

yohehe

score 19

測定した加速度とジャイロのデータを回転行列で分解するプログラムを作成しております。
動作データと測定データの誤差が大きいのですが、何が課題かわからず困っております。

#取得した加速度データをそれぞれacX,acY,acZとする
取得したジャイロデータをそれぞれradX,radY,radZとする。
#加速度はG表示のため、取得時にm/s*2へ変換

#角速度から角度へ 単位はradに
kakudoX=[]
kakudoY=[]
kakudoZ=[]
for i in range(len(radX)):
    radsecX=radX[i]+radX[i+1]*0.1
    kakudoX.append(radsecX)
    radsecY=radY[i]+radY[i+1]*0.1
    kakudoY.append(radsecY)
    radsecZ=radZ[i]+radZ[i+1]*0.1
    kakudoZ.append(radsecZ)

#acgyのリストを作成
acgy_list=[]
def kaiten(rx,ry,rz):
    #回転行列を定義
    Rx=np.array([[1,0,0],[0,np.cos(rx),-np.sin(rx)],[0,np.sin(rx),np.cos(rx)]])
    Ry=np.array([[np.cos(ry),0,np.sin(ry)],[0,1,0],[-np.sin(ry),0,np.cos(ry)]])
    Rz=np.array([[np.cos(rz),-np.sin(rz),0],[np.sin(rz),np.cos(rz),0],[0,0,1]])
    #基準座標系からみた座標の方向余弦行列.オイラー角z:ヨー角>x:ピッチ角>y:ロール角
    R=Rz.dot(Ry).dot(Rx)
    return R
accelgyro=kaiten(np.radians(x),np.radians(y),np.radians(z)).dot([[acX[i]],[acY[i]],[acZ[i]]])
    acgy_list.append(accelgyro)

#ノイズ除去のためにフィルタリングを行う。
# ローパスフィルターでフィルタリング
#1に近づけるほど平滑化の度合いは大きくなっていく
lowpass_acX=[]
lowpass_acY=[]
lowpass_acZ=[]
for i in range(len(acX)):
    a=0.3
    low=a*dataX[i]+(1-a)*dataX[i]
    lowpass_acX.append(low)
    low=a*dataY[i]+(1-a)*dataY[i]
    lowpass_acY.append(low)
    low=a*dataZ[i]+(1-a)*dataZ[i]
    lowpass_acZ.append(low)
highpass_X=[]
highpass_Y=[]
highpass_Z=[]
#次にハイパスフィルターでフィルタリング
for i in range(len(acX)):
    #ローパスフィルター:現在の値=係数*一つ前の値+(1-係数)センサーの値
    #ハイパスフィルター:センサーの値ーローパスフィルター
    a=0.4
    lowX=a*lowpass_X+rdataX[i]*(1-a)
    highX=rdataX[i]-lowX
    lowY=a*lowpass_Y+rdataY[i]*(1-a)
    highY=rdataY[i]-lowY
    lowZ=a*lowpass_Z+rdataZ[i]*(1-a)
    highZ=rdataZ[i]-lowZ
    highpass_X.append(highX)
    highpass_Y.append(highY)
    highpass_Z.append(highZ)  


コード


以上の方法で加速度とジャイロデータを回転行列として計算しております。このaccel*gyroデータをもとに速度、距離(台形積分法なども試しました)を求めていくと誤差が拡大し困っています。
何が問題かアドバイスをいただけますと幸いです。
よろしくお願いいたします。

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

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

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

    クリップを取り消します

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

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

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

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

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

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

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

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

    質問の評価を下げる

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

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

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

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

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

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

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

    詳細な説明はこちら

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

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

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

質問への追記・修正、ベストアンサー選択の依頼

  • ozwk

    2018/06/29 09:17 編集

    temp_pitch=temp_pitch+motion_Pitch[num]*0.2 <-これは何をしているつもりなんですか?角速度の積分?

    キャンセル

  • yohehe

    2018/06/29 09:45

    回転行列にかけるために角速度rad/secをradianの単位に変更しようとしたのですが。もしかして必要がないのでしょうか。

    キャンセル

回答 1

checkベストアンサー

+3

1
理由は面倒なので説明しませんが、
3次元の回転において、角速度の値を軸ごとに個別に足し合わせて"角度"を求めるのは誤りです。

そもそも3次元の回転は角度で持たずにクォータニオンや回転行列で持ったほうが計算上楽です。
クォータニオンや回転行列を角速度で更新する方法を調べてください。

「無限小回転」「微小回転」というキーワードが役に立つかもしれません。

2
加速度を2回の積分で位置とするのはとんでもなく誤差が大きくなります。
ある条件下では速度を0と推定するというような何らかの制約をつける必要があります。
(例えば、動加速度の大きさが一定以下なら静止しているものとみなすなど)

投稿

編集

  • 回答の評価を上げる

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

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

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

  • 回答の評価を下げる

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

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

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

  • 2018/06/29 10:57

    ozwk様ありがとうございます。そもそも角速度の段階が間違っていたために、徐々に誤差が大きくなっていくという現象だったのですか。ベクトルと同じように考えて速度と同じよう考えず、無限小回転、小回転について調べて一つずつ追記いたします。

    キャンセル

  • 2018/06/29 21:40

    もとの質問から大幅に内容が変わるなら前の質問を残すか質問を別で立ててください

    キャンセル

  • 2018/06/29 21:47

    すみません、そのほうが適切でした。消してしまってます前のプログラムデータを追記いたします。

    キャンセル

  • 2018/06/29 22:14

    ozwk様、文章を修正し、この質問を終了して別の質問にさせていただくようにいたします。ありがとうございます。

    キャンセル

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

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

関連した質問

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

  • Python

    7953questions

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