teratail
回答率
85.48%
質問するログイン新規登録
トップに関する質問pythonを使用した加速度とジャイロセンサーによる回転行列について

編集履歴

質問編集履歴

5

追記した内容を別の個別の内容の質問に移動するために修正いたしました。

2018/06/29 13:13

投稿

yohehe
yohehe

スコア48

test CHANGED
@@ -1 +1 @@
1
- pythonを使用した加速度とジャイロセンサーによる姿勢推定　python3.におけるQuarternionの扱い方について
1
+ pythonを使用した加速度とジャイロセンサーによる回転行列について
test CHANGED
@@ -1,155 +1,141 @@
1
- アドバイスをいただきまして、内容を大幅に変更いたしました。
1
+ 測定した加速度とジャイロのデータを回転行列で分解するプログラムを作成しております。
2
2
 
3
- 加速度とジャイロの測定したデータから位置推定するためにクォータニオンを用いて推定する方法を検討しております。
3
+ 動作データと測定データの誤差が大きいのですが、何が課題かわからず困っております。
4
4
 
5
- python3.6でQuarternionを作成するメソッドがなく、自分で組み立てていくしかないのでしょうか。
6
-
7
- ネットを色々と調べましたが、
5
+ ```python
8
-
9
- pip install Quarternion および pip install pyquaternion
10
-
11
- は使用できない。ディープラーニングなども用いたいため、可能であればバージョンはこのままで行きたいと考えていたのですが。
12
6
 
13
7
 
14
8
 
15
- 今現在は以下の流れでプログラムを作成しております。
9
+ #取得した加速度データをそれぞれacX,acY,acZとする
16
10
 
17
- ```python
11
+ 取得したジャイロデータをそれぞれradX,radY,radZとする。
18
12
 
19
- import numpy as np
13
+ #加速度はG表示のため、取得時にm/s*2へ変換
20
14
 
21
15
 
22
16
 
23
- #初期位置を設定する。
17
+ #角速度から角度へ　単位はradに
24
18
 
25
- shokichi=np.array(1,0,0,0)
19
+ kakudoX=[]
26
20
 
27
- #クォータニオンでは
21
+ kakudoY=[]
28
22
 
29
- X=[qw,qx,qy,qz].T
23
+ kakudoZ=[]
30
24
 
25
+ for i in range(len(radX)):
26
+
31
- #でその時間時間ごとでの位置をクォータニオン表記が可能。
27
+ radsecX=radX[i]+radX[i+1]*0.1
28
+
29
+ kakudoX.append(radsecX)
30
+
31
+ radsecY=radY[i]+radY[i+1]*0.1
32
+
33
+ kakudoY.append(radsecY)
34
+
35
+ radsecZ=radZ[i]+radZ[i+1]*0.1
36
+
37
+ kakudoZ.append(radsecZ)
32
38
 
33
39
 
34
40
 
35
- #30hzとするとdt=0.2となると思われる。
41
+ #acgyのリストを作成
36
42
 
37
- #センサーから得られる角速度をgx,gy,gzとすると
43
+ acgy_list=[]
38
44
 
39
- #Xn Xn+1を求める式は　Xn+1=Xn * quarternion(1,wx*dt/2,gy*dt/2,gz*dt/2)
45
+ def kaiten(rx,ry,rz):
40
46
 
41
- #Xn+1=X2,XnをX,30HZで測定するとdt=0.2
47
+ #回転行列を定義
42
48
 
43
- Quint=np.array([[1,-gx*dt/2,-gy*dt/2,-gz*dt/2],
49
+ Rx=np.array([[1,0,0],[0,np.cos(rx),-np.sin(rx)],[0,np.sin(rx),np.cos(rx)]])
44
50
 
45
- [gy*dt/2,1,gz*dt/2,-gy*dt/2],
51
+ Ry=np.array([[np.cos(ry),0,np.sin(ry)],[0,1,0],[-np.sin(ry),0,np.cos(ry)]])
46
52
 
47
- [gy*dt/2,-gz*dt/2,1,gx*dt/2],
53
+ Rz=np.array([[np.cos(rz),-np.sin(rz),0],[np.sin(rz),np.cos(rz),0],[0,0,1]])
48
54
 
49
- [gz*dt/2,gy*dt/2,-gx*dt/2,1]])
55
+ #基準座標系からみた座標の方向余弦行列.オイラー角z:ヨー角>x：ピッチ角>y：ロール角
50
56
 
51
- #for文などでXn+1=Xn*Quintを行い、arrayを作成していく
57
+ R=Rz.dot(Ry).dot(Rx)
52
58
 
59
+ return R
60
+
61
+ accelgyro=kaiten(np.radians(x),np.radians(y),np.radians(z)).dot([[acX[i]],[acY[i]],[acZ[i]]])
62
+
63
+ acgy_list.append(accelgyro)
64
+
65
+
66
+
67
+ #ノイズ除去のためにフィルタリングを行う。
68
+
69
+ # ローパスフィルターでフィルタリング
70
+
71
+ #1に近づけるほど平滑化の度合いは大きくなっていく
72
+
73
+ lowpass_acX=[]
74
+
75
+ lowpass_acY=[]
76
+
77
+ lowpass_acZ=[]
78
+
79
+ for i in range(len(acX)):
80
+
81
+ a=0.3
82
+
83
+ low=a*dataX[i]+(1-a)*dataX[i]
84
+
85
+ lowpass_acX.append(low)
86
+
87
+ low=a*dataY[i]+(1-a)*dataY[i]
88
+
89
+ lowpass_acY.append(low)
90
+
91
+ low=a*dataZ[i]+(1-a)*dataZ[i]
92
+
93
+ lowpass_acZ.append(low)
94
+
95
+ highpass_X=[]
96
+
97
+ highpass_Y=[]
98
+
99
+ highpass_Z=[]
100
+
101
+ #次にハイパスフィルターでフィルタリング
102
+
103
+ for i in range(len(acX)):
104
+
105
+ #ローパスフィルター：現在の値＝係数*一つ前の値＋(1-係数)センサーの値
106
+
53
- #クォータニオン表記を固定座標系のオイラー角に戻す。
107
+ #ハイパスフィルター：センサーの値ーローパスフィルター
108
+
109
+ a=0.4
110
+
111
+ lowX=a*lowpass_X+rdataX[i]*(1-a)
112
+
113
+ highX=rdataX[i]-lowX
114
+
115
+ lowY=a*lowpass_Y+rdataY[i]*(1-a)
116
+
117
+ highY=rdataY[i]-lowY
118
+
119
+ lowZ=a*lowpass_Z+rdataZ[i]*(1-a)
120
+
121
+ highZ=rdataZ[i]-lowZ
122
+
123
+ highpass_X.append(highX)
124
+
125
+ highpass_Y.append(highY)
126
+
127
+ highpass_Z.append(highZ)
128
+
129
+
130
+
131
+
54
132
 
55
133
  コード
56
134
 
57
135
  ```
58
136
 
59
- 流れがあっているかはわかりません。　
137
+ 以上の方法で加速度とジャイロデータを回転行列として計算しております。このaccel*gyroデータをもとに速度、距離(台形積分法なども試しました)を求めていくと誤差が拡大し困っています。
60
138
 
139
+ 何が問題かアドバイスをいただけますと幸いです。
61
140
 
62
-
63
- quartenionのコードを見つけたのですが、
64
-
65
-
66
-
67
- ```python
68
-
69
- コード
70
-
71
- import numpy as np
72
-
73
- from math import pi, sin, cos
74
-
75
-
76
-
77
- class Quaternion(object):
78
-
79
- def __init__(self, t, v):
80
-
81
- self.t = float(t)
82
-
83
- self.v = array(map(float,v))
84
-
85
- def __pos__(p): # +p
86
-
87
- return p
88
-
89
- def __neg__(p): # -p
90
-
91
- return Quaternion(-p.t, -p.v)
92
-
93
- def __add__(p, q): # p+q
94
-
95
- return Quaternion(p.t + q.t, p.v + q.v)
96
-
97
- def __sub__(p, q): # p-q
98
-
99
- return Quaternion(p.t - q.t, p.v - q.v)
100
-
101
- def __mul__(p, q): # p*q
102
-
103
- return Quaternion(p.t*q.t - dot(p.v, q.v),
104
-
105
- p.t*q.v + q.t*p.v + cross(p.v, q.v)
106
-
107
- )
108
-
109
- def inverse(p): # p*p_ = 1
110
-
111
- # 逆４元数
112
-
113
- n = p.abs2()
141
+ よろしくお願いいたします。
114
-
115
- p_= p.conj()
116
-
117
- return Quaternion(p_.t / n, p_.v / n)
118
-
119
- def abs2(p):
120
-
121
- return p.t**2 + sum(p.v**2)
122
-
123
- def __abs__(p):
124
-
125
- return p.abs2()**.5
126
-
127
- def conj(p):
128
-
129
- # 共役４元数
130
-
131
- return Quaternion(p.t, -p.v)
132
-
133
- def __repr__(p):
134
-
135
- return "(%f; %f, %f, %f)" % (p.t, p.x, p.y, p.z)
136
-
137
- @property
138
-
139
- def x(self): return self.v[0]
140
-
141
- @property
142
-
143
- def y(self): return self.v[1]
144
-
145
- @property
146
-
147
- def z(self): return self.v[2]
148
-
149
-
150
-
151
- ```
152
-
153
- 以上のクォータニオンのクラスデータファイルを見つけたのですが、
154
-
155
- np.array(0,0,0)を引数などに代入するとエラーとなってしまいます。

4

文章修正

2018/06/29 13:13

投稿

yohehe
yohehe

スコア48

test CHANGED
File without changes
test CHANGED
@@ -58,24 +58,98 @@
58
58
 
59
59
  流れがあっているかはわかりません。　
60
60
 
61
- 以下の方法でのクォータニオンの計算式が書いてあったのですが、他のプログラミング言語が全くわからず難渋しております。
62
61
 
63
- 4元数同士の掛け算の方法
64
62
 
65
- A = (a; U)
63
+ quartenionのコードを見つけたのですが、
66
64
 
67
- B = (b; V)
68
65
 
69
- AB = (ab - U・V; aV + bU + U×V)
70
66
 
71
- P = (0; x, y, z)
67
+ ```python
72
68
 
73
- Q = (cos(θ/2); α sin(θ/2), β sin(θ/2), γ sin(θ/2))
69
+ コード
74
70
 
75
- R = (cos(θ/2); -α sin(θ/2), -β sin(θ/2), -γ sin(θ/2))
71
+ import numpy as np
76
72
 
77
- ichi=R*P*Q
73
+ from math import pi, sin, cos
78
74
 
79
- 以上のソースコードなども調べてみたのですが、pythonにいかにおとしてやるかがわかっておりません。
80
75
 
76
+
77
+ class Quaternion(object):
78
+
79
+ def __init__(self, t, v):
80
+
81
+ self.t = float(t)
82
+
83
+ self.v = array(map(float,v))
84
+
85
+ def __pos__(p): # +p
86
+
87
+ return p
88
+
89
+ def __neg__(p): # -p
90
+
91
+ return Quaternion(-p.t, -p.v)
92
+
93
+ def __add__(p, q): # p+q
94
+
95
+ return Quaternion(p.t + q.t, p.v + q.v)
96
+
97
+ def __sub__(p, q): # p-q
98
+
99
+ return Quaternion(p.t - q.t, p.v - q.v)
100
+
101
+ def __mul__(p, q): # p*q
102
+
103
+ return Quaternion(p.t*q.t - dot(p.v, q.v),
104
+
105
+ p.t*q.v + q.t*p.v + cross(p.v, q.v)
106
+
107
+ )
108
+
109
+ def inverse(p): # p*p_ = 1
110
+
111
+ # 逆４元数
112
+
113
+ n = p.abs2()
114
+
115
+ p_= p.conj()
116
+
117
+ return Quaternion(p_.t / n, p_.v / n)
118
+
119
+ def abs2(p):
120
+
121
+ return p.t**2 + sum(p.v**2)
122
+
123
+ def __abs__(p):
124
+
125
+ return p.abs2()**.5
126
+
127
+ def conj(p):
128
+
129
+ # 共役４元数
130
+
131
+ return Quaternion(p.t, -p.v)
132
+
133
+ def __repr__(p):
134
+
135
+ return "(%f; %f, %f, %f)" % (p.t, p.x, p.y, p.z)
136
+
81
- また追記いたします。
137
+ @property
138
+
139
+ def x(self): return self.v[0]
140
+
141
+ @property
142
+
143
+ def y(self): return self.v[1]
144
+
145
+ @property
146
+
147
+ def z(self): return self.v[2]
148
+
149
+
150
+
151
+ ```
152
+
153
+ 以上のクォータニオンのクラスデータファイルを見つけたのですが、
154
+
155
+ np.array(0,0,0)を引数などに代入するとエラーとなってしまいます。

3

文章修正

2018/06/29 11:47

投稿

yohehe
yohehe

スコア48

test CHANGED
File without changes
test CHANGED
@@ -1,4 +1,4 @@
1
- 内容を大幅に変更いたしました。アドバイスをいただきまして
1
+ アドバイスをいただきまして、内容を大幅に変更いたしました。
2
2
 
3
3
  加速度とジャイロの測定したデータから位置推定するためにクォータニオンを用いて推定する方法を検討しております。
4
4
 
@@ -56,7 +56,17 @@
56
56
 
57
57
  ```
58
58
 
59
+ 流れがあっているかはわかりません。　
59
60
 
61
+ 以下の方法でのクォータニオンの計算式が書いてあったのですが、他のプログラミング言語が全くわからず難渋しております。
62
+
63
+ 4元数同士の掛け算の方法
64
+
65
+ A = (a; U)
66
+
67
+ B = (b; V)
68
+
69
+ AB = (ab - U・V; aV + bU + U×V)
60
70
 
61
71
  P = (0; x, y, z)
62
72
 

2

アドバイスをもとに大幅修正いたしました。

2018/06/29 11:08

投稿

yohehe
yohehe

スコア48

test CHANGED
@@ -1 +1 @@
1
- 加速度とジャイロセンサーの回転行列における分解とフィルタリングについて
1
+ pythonを使用した加速度とジャイロセンサーによる姿勢推定　python3.におけるQuarternionの扱い方について
test CHANGED
@@ -1,133 +1,71 @@
1
- 質問ばかりで申し訳ないのですが、質問させてください。
1
+ 内容を大幅に変更いたしました。アドバイスをいただきまして
2
2
 
3
- センサーを使用して、加速度とジャイロセンサーで軌道を測定しております。
3
+ 加速度とジャイロの測定したデータから位置推定するためにクォータニオンを用いて推定する方法を検討しております。
4
+
5
+ python3.6でQuarternionを作成するメソッドがなく、自分で組み立てていくしかないのでしょうか。
6
+
7
+ ネットを色々と調べましたが、
8
+
9
+ pip install Quarternion および pip install pyquaternion
10
+
11
+ は使用できない。ディープラーニングなども用いたいため、可能であればバージョンはこのままで行きたいと考えていたのですが。
4
12
 
5
13
 
6
14
 
7
- 動作と測定した値のずれを修正したいと思っております。
15
+ 今現在は以下の流れでプログラムを作成しております。
16
+
17
+ ```python
18
+
19
+ import numpy as np
8
20
 
9
21
 
10
22
 
23
+ #初期位置を設定する。
24
+
25
+ shokichi=np.array(1,0,0,0)
26
+
27
+ #クォータニオンでは
28
+
11
- 基本は以下のコードになると思うのですが、
29
+ X=[qw,qx,qy,qz].T
30
+
31
+ #でその時間時間ごとでの位置をクォータニオン表記が可能。
12
32
 
13
33
 
14
34
 
35
+ #30hzとするとdt=0.2となると思われる。
36
+
37
+ #センサーから得られる角速度をgx,gy,gzとすると
38
+
39
+ #Xn Xn+1を求める式は　Xn+1=Xn * quarternion(1,wx*dt/2,gy*dt/2,gz*dt/2)
40
+
41
+ #Xn+1=X2,XnをX,30HZで測定するとdt=0.2
42
+
43
+ Quint=np.array([[1,-gx*dt/2,-gy*dt/2,-gz*dt/2],
44
+
45
+ [gy*dt/2,1,gz*dt/2,-gy*dt/2],
46
+
47
+ [gy*dt/2,-gz*dt/2,1,gx*dt/2],
48
+
49
+ [gz*dt/2,gy*dt/2,-gx*dt/2,1]])
50
+
51
+ #for文などでXn+1=Xn*Quintを行い、arrayを作成していく
52
+
53
+ #クォータニオン表記を固定座標系のオイラー角に戻す。
54
+
55
+ コード
56
+
15
- ```python
57
+ ```
16
58
 
17
59
 
18
60
 
19
- #各加速度をaccel_x,accel_y,accel_z(単位G)
61
+ P = (0; x, y, z)
20
62
 
21
- #角速度をPitch,Roll,Yawとする(単位rad/sec)
63
+ Q = (cos(θ/2); α sin(θ/2), β sin(θ/2), γ sin(θ/2))
22
64
 
65
+ R = (cos(θ/2); -α sin(θ/2), -β sin(θ/2), -γ sin(θ/2))
23
66
 
67
+ ichi=R*P*Q
24
68
 
25
- #rad/secをradianにしていく
69
+ 以上のソースコードなども調べてみたのですが、pythonにいかにおとしてやるかがわかっておりません。
26
70
 
27
- Pitch=[]
28
-
29
- Roll=[]
30
-
31
- Yaw=[]
32
-
33
- temp_pitch=0
34
-
35
- temp_roll=0
36
-
37
- temp_yaw=0
38
-
39
- #30hzで測定したとすると
40
-
41
- for num in range(len(motion_Yaw)):
42
-
43
- temp_pitch=temp_pitch+motion_Pitch[num]*0.2
44
-
45
- Pitch.append(temp_pitch)
46
-
47
- temp_roll=temp_roll+motion_Roll[num]*0.2
48
-
49
- Roll.append(temp_roll)
50
-
51
- temp_yaw=temp_yaw+motion_Yaw[num]*0.2
52
-
53
- Yaw.append(temp_yaw)
54
-
55
- #回転行列を定義　　
56
-
57
- def kaiten(rx,ry,rz):　
58
-
59
- Rx=np.array([[1,0,0],[0,np.cos(rx),-np.sin(rx)],[0,np.sin(rx),np.cos(rx)]])
60
-
61
- Ry=np.array([[np.cos(ry),0,np.sin(ry)],[0,1,0],[-np.sin(ry),0,np.cos(ry)]])
62
-
63
- Rz=np.array([[np.cos(rz),-np.sin(rz),0],[np.sin(rz),np.cos(rz),0],[0,0,1]])
64
-
65
- R=Rz.dot(Ry).dot(Rx)
66
-
67
- return R
68
-
69
- #加速度を回転行列で分解する
70
-
71
- acgy_data=[]
72
-
73
- accelgyro=kaiten(Pitch[num],Roll[num],Yaw[num]).dot([[accel_X[num]],[accel_Y[num]],[accel_Z[num]]])
74
-
75
- acgy_data.append(accelgyro)
76
-
77
- ```
78
-
79
- 以上で加速度を分解しているのですが、この値をもとに速度計算、距離計算(台形積分法なども試しました)などを行なっていくと、全く思った値にはなりません。
80
-
81
- 様々な手法を調べフィルタリングなどを行うべきと考え、ローパス、ハイパスフィルタリングなどを調べました
82
-
83
- 少々省略しておりますが、
71
+ また追記いたします。
84
-
85
- ```python
86
-
87
- for i in range(len(N)):
88
-
89
- #ローパスフィルター：現在の値＝係数*一つ前の値＋(1-係数)センサーの値
90
-
91
- #ハイパスフィルター：センサーの値ーローパスフィルター
92
-
93
- a=0.4
94
-
95
- lowX=a*lowpass_X+rdataX[i]*(1-a)
96
-
97
- highX=rdataX[i]-lowX
98
-
99
- lowY=a*lowpass_Y+rdataY[i]*(1-a)
100
-
101
- highY=rdataY[i]-lowY
102
-
103
- lowZ=a*lowpass_Z+rdataZ[i]*(1-a)
104
-
105
- highZ=rdataZ[i]-lowZ
106
-
107
- high_X.append(highX)
108
-
109
- high_Y.append(highY)
110
-
111
- high_Z.append(highZ)
112
-
113
- for i in range(len(N)):
114
-
115
- high_X=dataX[i]-lowpass_acX[i]
116
-
117
- highpass_acX.append(high_X)
118
-
119
- high_Y=dataY[i]-lowpass_acY[i]
120
-
121
- highpass_acY.append(high_Y)
122
-
123
- high_Z=dataZ[i]-lowpass_acZ[i]
124
-
125
- highpass_acZ.append(high_Z)
126
-
127
- ```
128
-
129
- 以上のコードでのフィルタリングを行なってもあまり変わらずです。カルマンフィルタリングなども調べましたがフィルタリング以前の問題なのではないかと考えております。
130
-
131
- どこが間違っているか、課題なのかもわからずにおります。
132
-
133
- わかる方がいらっしゃいましたらアドバイスをいただけないでしょうか。すみませんがよろしくお願いいたします。

1

初心者のマークを追加。文章修正

2018/06/29 11:04

投稿

yohehe
yohehe

スコア48

test CHANGED
File without changes
test CHANGED
@@ -76,9 +76,9 @@
76
76
 
77
77
  ```
78
78
 
79
- 以上で加速度を分解しているのですが、この値をもとに計算などを行なっていくと、全く思った値にはなりません。
79
+ 以上で加速度を分解しているのですが、この値をもとに速度計算、距離計算(台形積分法なども試しました)などを行なっていくと、全く思った値にはなりません。
80
80
 
81
- フィルタリングなどを行うべきと考え、ローパス、ハイパスフィルタリングなどを調べました
81
+ 様々な手法を調べフィルタリングなどを行うべきと考え、ローパス、ハイパスフィルタリングなどを調べました
82
82
 
83
83
  少々省略しておりますが、
84
84