ジャイロセンサのドリフト補正

前回、センサー値をそのままUnity上のオブジェクトの角度プロパティに追加すると、センサー基板が静止状態でも勝手に回転を始めてしまいました。

  ジャイロセンサーのドリフト補正

「ジャイロセンサーは、完全に静止していても継続的にわずかに変動します。」

とのことで、これを ドリフト現象 というようです。

この対策をこころみます。

センサー基板を 静止した状態 のセンサー値を複数回分取得し、その 平均値 を求めてセンサー値から 引き算 することで、ドリフト現象が収まるかについて確認します。

  平均値取得用のUnityコード

Unityでは、ジャイロセンサー値をCOMポートで受ける毎にStillness.csvと名付けたファイルに追記するように一旦コードを変更します。

---

using System.Collections;
using System.Collections.Generic;
using System.IO.Ports;
using System;
using UnityEngine;
using System.IO;
using System.Text;

public class Bluetooth_to_Unity : MonoBehaviour
{
    private SerialPort port;
    private StreamWriter sw;
    private int i;

    // Start is called before the first frame update
    void Start()
    {
        port = new SerialPort("COM7", 9600, Parity.None, 8, StopBits.One);
        Debug.Log("start");
        
        port.NewLine = "\r\n";        
        port.ReadBufferSize = 64;
        port.ReadTimeout = 50;
        port.Open();

        sw = new StreamWriter(@"Stillness.csv", true, Encoding.GetEncoding("Shift_JIS"));
        Debug.Log(sw.ToString() +"\r\n");
        sw.WriteLine("gxRaw, gyRaw, gzRaw");
        i = 0;
    }

    // Update is called once per frame
    void Update()
    {
        if (port.IsOpen)
        {
            try
            {
                String data = port.ReadLine();
                sw.WriteLine(data);
                i++;
                Debug.Log(i.ToString()+"\r\n");
            }
            catch (TimeoutException)
            {
                //sw.WriteLine("0,0,0");
            }            
        }        
    }
}

---

  表計算ソフトで平均値

以上の準備をしてからセンサー基板を固定した状態で 100回程度 のデータを取得します。

100回分のデータが記載された Stillness.csv を表計算ソフトで開いて、xyzそれぞれの平均値を求めます。

この値を毎回の取得データからマイナスすればドリフト補正にとして有効になるでしょうか？

今回は平均値がそれぞれ

x　+41.4

y　-23.4

z　+22.8

になりましたので、これらの値をUnityのコード内でセンサー値から引き算するよう変更します。

 

using System.Collections;
using System.Collections.Generic;
using System.IO.Ports;
using System;
using UnityEngine;


public class Bluetooth_to_Unity : MonoBehaviour
{
    private SerialPort port;
    private float gx, gy, gz;

    // Start is called before the first frame update
    void Start()
    {
        port = new SerialPort("COM7", 9600, Parity.None, 8, StopBits.One);
        Debug.Log("start");

        port.NewLine = "\r\n";
        port.ReadBufferSize = 64;
        port.ReadTimeout = 50;
        port.Open();
    }

    // Update is called once per frame
    void Update()
    {

        if (port.IsOpen)
        {
            try
            {
                String data = port.ReadLine();
                string[] arr = data.Split(',');

                gx = (int.Parse(arr[0]) - 41.4f) / 16.4f; //補正値を引き算
                gy = (int.Parse(arr[1]) + 23.4f) / 16.4f;
                gz = (int.Parse(arr[2]) - 22.8f) / 16.4f;

                Debug.Log(gx.ToString() + ", " + gy.ToString() + ", " + gz.ToString() + "\r\n");
            }
            catch (TimeoutException)
            {                
            }
        }

        Transform myTransform = this.transform;
                
        Vector3 localAngle = myTransform.localEulerAngles;
        localAngle.x = localAngle.x + gx;
        localAngle.y = localAngle.y + gy;
        localAngle.z = localAngle.z + gz;
        myTransform.localEulerAngles = localAngle;

    }
}

 

これを反映した結果がこちら。

かなりドリフト現象がなくなった気がします。

次回以降、センサー基板の回転に合わせてUnity画面上のオブジェクトの向きが変化するようにしたいですし、 回転数を求めたい ですが、今のところどうすればいいのか見当がつかないです。

数学苦手だから、進みが遅くなりそうだなぁ．．．

---