🚀

MPU9250で取得した角速度を使ってUnity上で姿勢推定をする

2023/09/09に公開

Unity

姿勢推定

mpu9250

tech

を丁寧に書き直した記事だと思ってください。

実行環境

Unity：2021.3.16f

角速度の取得

この情報は調べれば色々あるので省略、自分はこの記事のやり方でやっています。

Unityに角速度の記録が書いてあるファイルを置く

Assets/Resources/下にxxx.csvといったファイルを置いてください。
8~10列目に角速度の値が入っているものとして以下説明します。

ソースコード

list_csv_v3

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

public class list_csv_v3 : MonoBehaviour
{
    TextAsset csvFile;
    List<string[]> csvDatas = new List<string[]>();

    public string load_data = "cansat2020contest";
    public float time_speed = 10f;
    public int gyr_list_index = 4+3;

    private static int max_gyr;
    private static float dt = 0.1f;

    private Quaternion to_qua;
    private Quaternion standard;

    private static float t;
    private static int q_counter = 1;
    private float x_value;
    private float y_value;
    private float z_value;
    Vector3 angle;

    void Start()
    {
        // about gyr
        csvFile = Resources.Load(load_data) as TextAsset;
        StringReader reader = new StringReader(csvFile.text);
        while (reader.Peek() != -1)
        {
            string line = reader.ReadLine();
            csvDatas.Add(line.Split(','));
        }
        max_gyr = csvDatas.Count;
        Debug.Log(csvDatas[0][0]+","+ csvDatas[0][1]+","+ csvDatas[0][2] + ","+csvDatas[0][3] + "," + csvDatas[0][4] + "," + csvDatas[0][5] + "," + csvDatas[0][6] + "," + csvDatas[0][7] + "," + csvDatas[0][8] + "," + csvDatas[0][9]);
    }

    // Update is called once per frame
    void FixedUpdate()
    {
        GyroValue();
    }

    private void GyroValue()
    {
        t += Time.deltaTime;
        if (t > (1 / time_speed) && q_counter < max_gyr - 1)
        {
            t = 0;
            x_value = float.Parse(csvDatas[q_counter + 1][gyr_list_index]);
            y_value = float.Parse(csvDatas[q_counter + 1][gyr_list_index + 1]);
            z_value = float.Parse(csvDatas[q_counter + 1][gyr_list_index + 2]);
            angle = new Vector3(y_value, z_value, -1.0f * x_value);
            to_qua = this.transform.rotation * Quaternion.Euler(angle * dt);
            standard = this.transform.rotation;
            
            q_counter++;
            Debug.Log(q_counter);
        }
        if (q_counter == max_gyr - 1)
        {
            Debug.Log("EOF");
        }

        this.transform.rotation = Quaternion.Slerp(standard, to_qua, t * time_speed);
    }
}

一番コアな部分としては取得した角速度をQuaternion.Eulerに突っ込んでるだけです。
そして、Quaternion.Slerp(standard, to_qua, t * time_speed);で現在の角度から変化した分だけ回転します。

public string load_data = "cansat2020contest";の部分ではファイル名を指定しています。

dtには元データの取得時間の間隔を入れてください。多分もっと早いのが普通なのかな。