chakokuのブログ(rev4)

日々のごった煮ブログです

6DoFのLSM6DSLからジャイロ、加速度、温度のセンサーデータを取ってみる

SPIでレジスタが読めるようなので、センサを動かすための最低限の設定だけ行って、ジャイロセンサ、加速度センサ、温度センサの値を読んでみた。以下がその値。ドローンを置いただけなので、Y軸の加速度だけ値が出ている。それ以外は誤差なんだろう。

# gyroscope(x,y,z)    acceleration(x,y,z)   temp
[2, -83, -11]      [171, -203, 16554]  27.96078
[-255, -80, -2]  [155, -192, 16534]  27.92549
[-3, -69, -6]      [166, -215, 16539]  27.92157
[-3, -72, -9]      [168, -213, 16544]  27.96471
[3, -80, -2]       [155, -183, 16547]  27.97647
[-5, -72, -8]     [149, -202, 16553]  27.91765

STEVAL-DRONEをXYZの各軸で回転させると、ロール、ピッチ、ヨーに沿ってセンタの値は変化している。が、、計算に使っていい値域になっているのかは
もう少し仕様書やサンプルコーディングを見ないと分からない。温度はサンプルを調べて実装したのでそれらしい値が出ているが、ジャイロと加速度センサについては、サンプルソースを少し見た限りでは、最大値で割り算とかしている?ようで、、レジスタを読むだけでは多分ダメなんだろう。

絶対値はおかしいとして、どれぐらい動作を検知できているか、ピッチング、ローリング、ヨーイングが発生した時のジャイロセンサーの値(X,Y,Z軸)をグラフ化した。左から、ドローンに対してピッチング、ローリング、ヨーイングさせた時のジャイロセンサの値で、それぞれの操作において、X軸、Y軸、Z軸が最も強く反応している。(グラフ上はY軸、X軸が入れ替わっている。センサの取り付け角度が右回りに90度回転しているため。直進方向がY軸になっているらしい)

加速度センサの計測値は以下

ジャイロセンサーの値は回転速度なので、得られた値を積分することで、回転角が分かる仕組み。ただし、回転するとセンサの軸も傾くので、その分を絶対座標のXYZ軸に戻してから*1積分する必要があるという。。傾いたセンサをXYZ軸の基準として、それを使い続けるとなぜダメなのか?? センサからの情報に基づき回転の計算を終えてから、地球の絶対座標に変換したらダメなんだろうか?? (あるいは、順番が違うだけで結局は同じ計算をしているのか??)

以下は、STマイクロ社の新しいセンサ(LSM9D1)に対して、無理やりAdafruitのドライバを動かしてデータを取ったところ。正規化されているので、何万とかそういった値にはないっていない

0.0 0.0 0.0                               # ジャイロ
-0.01022 0.07406 -0.24108       # 加速度
0.1757765 0.2814561 0.01420262    # 地磁気
21.5                                 # 温度

グラフを見ていると値域はおおよそ-30,000~+30,000の幅で変動している。この±30,000って符号付2バイト整数の値域(-32,768~32,767)と大体合っていて、2バイト整数で表現できる範囲でほぼ振り切っているということだと理解しました。これ以上の加速度を与えると値を表現できずに0x7FFF(32,767)でサチることになるのだろう。

■追記
微積を使って絶対座標で厳密に姿勢を計算しながら制御するのが正統な実装なんだろうけど、ジャイロと加速度センサから得られた値の増減だけに着目して、モータの回転数を上下させるだけでも姿勢制御が可能なのでは?と思えるのだった。ただしフィードバックしすぎて発振しないような鈍感さパラメータは必要と思いますが。例のPID制御につながる話なのかもしれません。

■追記
ジャイロスコープ、角速度の値域は、レジスタ設定の値を最大値として比率で計算するようである(たしかにここは適当に設定したレジスタ値であった)。
AN5040[LSM6DSL: always-on 3D accelerometer and 3D gyroscope]より


■追記
ジャイロスコープ、加速度センサの値域に基づき正規化?したデータは以下(ジャイロはMAX±250dps 、加速度はMAX±2G)。10秒でドローンの機首を真上まで上げて、10秒で水平までおろした。角速度は、9dps (90度/10秒だから)

        gyro(degree/s)   |     accelerometer(G)    | temp
------------------------------------------------------------
-0.299,   0.366, -0.220, | -0.013, -0.046, 1.001,  |31.675,
-0.629,  -1.526,  0.146, | -0.036,  0.008, 1.011,  |31.667,
-2.680,  -10.724, 0.531, |  0.063, -0.013, 1.010,  |31.651,
 2.905,  -6.793, -0.281, |  0.157,  0.004, 0.998,  |31.710,
 0.073,  -6.653,  0.092, |  0.250,  0.003, 0.968,  |31.694,
-0.293,  -8.441,  0.214, |  0.373, -0.014, 0.951,  |31.710,
 0.153, -10.480,  0.970, |  0.426,  0.004, 0.908,  |31.698,
-0.098,  -3.912,  0.397, |  0.534, -0.004, 0.870,  |31.682,
 0.269,  -1.984, -0.348, |  0.568, -0.008, 0.842,  |31.659,
-0.281,  -5.084, -0.525, |  0.592,  0.001, 0.821,  |31.671,
-0.336,  -1.215,  1.483, |  0.642,  0.011, 0.781,  |31.714,
-0.049,  -3.870, -0.140, |  0.660, -0.009, 0.766,  |31.706,
 0.195,  -7.764, -0.610, |  0.714, -0.007, 0.692,  |31.710,
 0.018,  -4.822,  0.275, |  0.765, -0.017, 0.651,  |31.667,
-1.233,  -4.956,  0.208, |  0.806, -0.005, 0.609,  |31.647,
-0.409,  -6.409,  0.439, |  0.851, -0.009, 0.561,  |31.678,
-0.928,  -9.113, -0.623, |  0.882,  0.000, 0.495,  |31.722,
-0.574,  -7.501, -0.922, |  0.938, -0.008, 0.342,  |31.725,
-1.471, -12.836,  0.110, |  0.954,  0.008, 0.223,  |31.698,
 0.568,  -9.778, -0.586, |  0.983, -0.007, 0.100,  |31.702,
						   |
 0.293,  -2.918, -0.360, |  0.999,  0.008, 0.045,  |31.698,
 0.775,  10.694,  1.123, |  0.999,  0.002, 0.102,  |31.741,
						   |
-1.099,   4.126, -1.013, |  0.980,  0.008, 0.200,  |31.694,
-1.135,   9.076,  0.458, |  0.967,  0.007, 0.279,  |31.718,
 1.325,   5.646, -0.592, |  0.930, -0.011, 0.413,  |31.675,
 0.714,   5.219, -0.214, |  0.877, -0.002, 0.460,  |31.729,
-0.055,   6.195,  0.024, |  0.838, -0.023, 0.551,  |31.663,
-0.641,   5.615, -0.464, |  0.800, -0.003, 0.615,  |31.690,
-0.745,   3.235, -0.031, |  0.776,  0.003, 0.657,  |31.753,
 1.147,   6.616, -0.482, |  0.716, -0.013, 0.716,  |31.718,
 0.702,   3.467,  0.452, |  0.681,  0.006, 0.759,  |31.671,
-0.275,   4.535,  0.311, |  0.633, -0.003, 0.792,  |31.698,
 3.851,   6.287, -0.916, |  0.618,  0.003, 0.830,  |31.690,
-0.427,   5.683, -0.385, |  0.489, -0.014, 0.862,  |31.686,
-0.476,   8.466, -0.604, |  0.393,  0.001, 0.913,  |31.698,
 0.195,   1.874, -0.049, |  0.332,  0.008, 0.956,  |31.710,
-0.378,   3.656, -0.366, |  0.280, -0.008, 0.974,  |31.714, 
 0.275,   3.626,  0.116, |  0.231, -0.010, 0.989,  |31.667,
 1.349,   7.526, -0.525, |  0.142, -0.013, 0.996,  |31.718,
-0.244,  10.779, -0.293, |  0.028, -0.017, 1.015,  |31.682,
-0.061,  -0.275, -0.012, |  0.001, -0.002, 1.012,  |31.710,
-0.122,  -0.305, -0.024, | -0.001, -0.002, 1.008,  |31.702,

動作としてはY軸回転(ピッチ)なので、ジャイロセンサはY軸で検知されるはず。一方、ドローンのセンサは90度曲がっているから、、ピッチ動作はX軸で検知されると解釈していたが、上記を見ると、ピッチの検知はY軸の様だ(教科書通りの軸)。だったら冒頭のテストは間違いだったのか??
値は9dpsが出力されるはずだが、手による操作なので、多少ぶれているとして、-10、+10に近い値が出ている。だから、、ジャイロはだいたい正しい値を出していそうだと。一方の加速度センサについて、ドローンを水平においてる時はZ軸に1G、機首を完全に上げた時はX軸に1Gとなっている。斜め45度の時は、X軸とZ軸にGがかかって、x,y,z = 0.7, 0, 0.7である。これは、、直角三角形の底辺が1の場合、斜辺?は0.7というやつだと理解しています。

*1:行列演算