1. LPMS-ME1 使用简介

本项目所采用的 IMU 是由阿路比提供的 LPMS-ME1。它能实现最高 400Hz 的传输频率和最高 921600 的波特率。在该项目中，它通过 USB 与主机连接，并通过 UART 协议传输数据。根据 LMPS 官方文档，该 IMU 的驱动可以从SiliconLabs官网下载。驱动安装成功后，在计算机的设备管理器中显示为 SiliconLabs CP210x USB to UART Bridge，如图

驱动安装成功后，设备管理器多出一个串口设备

在对其进行开发之前，需要通过上位机软件OpenMAT对其进行设置。在该项目中，我们对其设置如下：

上位机设置界面

注意滤波模式这里设置为陀螺仪+加速度传感器，并没有磁力传感器。因为磁力传感器反应没那么快，加进去以后数据自动校正的速度非常慢，现象上体现为零漂。

2. 串口通信

串口通信主要参考这篇文章。

首先定义一个串口基类。这个类包含了串口的基本属性和变量以及打开关闭操作，具体的参数设置和数据解析需要由子类继承实现。

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

/// <summary>
/// 串口通信
/// </summary>
public abstract class SerialPortItem
{
    #region 公有字段
    /// <summary>
    /// 串口名
    /// </summary>
    public string portName;
    /// <summary>
    /// 无效端口
    /// </summary>
    public List<string> unvalidSerialPort;
    /// <summary>
    /// 波特率
    /// </summary>
    public int BaudRate;
    /// <summary>
    /// 校验位
    /// </summary>
    public Parity Parity;
    /// <summary>
    /// 停止位
    /// </summary>
    public StopBits StopBits;
    /// <summary>
    /// 数据位
    /// </summary>
    public int DataBits;
    /// <summary>
    /// 握手
    /// </summary>
    public Handshake Handshake;
    /// <summary>
    /// 流控
    /// </summary>
    public bool RtsEnable;
    /// <summary>
    /// 数据头标识
    /// </summary>
    public int DATAIDENTI_HEAD;
    /// <summary>
    /// 数据尾部标识
    /// </summary>
    public int DATAIDENTI_END;
    /// <summary>
    /// 数据颈部标识
    /// </summary>
    public int DATAIDENTI_NECK;
    /// <summary>
    /// 指令低八位（后收到）
    /// </summary>
    public int COMMAND_L;
    /// <summary>
    /// 指令高八位（先收到）
    /// </summary>
    public int COMMAND_H;
    /// <summary>
    /// 数据总字节数，分别包括时间戳（4）、原始加速度（4*3）、原始磁场（4*3）、原始陀螺仪（4*3）、角速度（4*3）、四元数（4*4）、欧拉角（4*3）、线性加速度（4*3）
    /// </summary>
    public int DATA_AMOUNT;
    /// <summary>
    /// 数据尾高八位（先收到）
    /// </summary>
    public int DATAIDENTI_END_L;
    /// <summary>
    /// 数据尾低八位（后收到）
    /// </summary>
    public int DATAIDENTI_END_H;
    /// <summary>
    /// 字节转换成四元素的参数常量
    /// </summary>
    public int BYTE2QUA_PARAM;
    /// <summary>
    /// 字节转换成欧拉角速度的参数常量
    /// </summary>
    public int BYTE2EU_PARAM;
    /// <summary>
    /// 串口数据包的长度
    /// </summary>
    public int DATAPACK_LENGTH;
    /// <summary>
    /// 单个数据的长度
    /// </summary>
    public int DATASINGLE_LENGTH;
    #endregion

    # region 保护字段
    /// <summary>
    /// 串口
    /// </summary>
    protected SerialPort serialPort;
    /// <summary>
    /// 读取到串口的缓冲字节数据列表
    /// </summary>
    protected static Queue<byte> QCacheData_ReadSPBytes = new Queue<byte>();
    # endregion
        
    #region 私有字段
    /// <summary>
    /// 串口状态（是否打开）
    /// </summary>
    private bool isOpen;
    // private bool isClose;
    /// <summary>
    /// 收发串口线程
    /// </summary>
    private Thread tReceiveData;
    #endregion

    #region 公有属性
    /// <summary>
    /// 是否开启串口
    /// </summary>
    public bool IsOpen
    {
        get { return isOpen; }
    }
    #endregion

    #region 事件
    /// <summary>
    /// 接到数据(子线程)
    /// </summary>
    public Action<byte[]> OnReceiveData;
    /// <summary>
    /// 错误(子线程)
    /// </summary>
    public Action<string> OnError;
    #endregion

    /// <summary>
    /// 开启串口
    /// </summary>
    public void Open()
    {
        string[] allPorts = SerialPort.GetPortNames();
        if (allPorts == null)
        {
            return;
        }
        serialPort = new SerialPort();
        serialPort.BaudRate = BaudRate;
        serialPort.ReadTimeout = 20;
        serialPort.WriteTimeout = 10;
        int i = 0;
        while (i < allPorts.Length)
        {
            //动态获取串口名称
            this.portName = allPorts[i];
            try
            {
                serialPort.PortName = this.portName;
                Debug.Log(serialPort.PortName);
                //检查无效串口
                if (null == unvalidSerialPort || -1 == unvalidSerialPort.IndexOf(serialPort.PortName))
                {
                    serialPort.Open();
                    Thread.Sleep(10);
                    //如果能读到数据，那就用这个串口
                    if (serialPort.ReadByte() >= 0)
                    {
                        break;
                    }
                    //如果读不到数据，就关闭这个串口，并且列入无效名单，热插拔的时候扫描到这个串口就直接跳过
                    serialPort.Close();
                    unvalidSerialPort.Add(serialPort.PortName);
                }
            }
            catch (Exception ex)
            {
                Debug.LogError(ex.ToString());
                serialPort.Close();
                unvalidSerialPort.Add(serialPort.PortName);
                Debug.Log(unvalidSerialPort.Count);
            }
            i++;
        }
        if (i >= allPorts.Length)
        {
            return;
        }

        isOpen = true;
        //专门开一个线程用来接收数据
        tReceiveData = new Thread(ReceiveData);
        tReceiveData.Start();
        Debug.Log("start" + serialPort.PortName);
    }

    public void Close()
    {
        if (IsOpen == true)
        {
            isOpen = false;
            serialPort.Close();
            serialPort = null;
            tReceiveData.Abort();
        }
    }
    
    //各类传感器的数据解析方式不同，所以写成抽象方法
    public abstract void ReceiveData();
    //各类传感器的初始化方式坑你不同，所以协程抽象方法
    public abstract void InitSerialPortItem();
}

接下来是继承该基类的子类

#define DEBUG

using System.IO.Ports;
using UnityEngine;
using System;
using System.IO;
using System.Linq;
using System.Threading;

public class Gyroscope : SerialPortItem
{
    #region 公有字段
    public bool updated = false;
    public FileStream fs;
    public StreamWriter wr = null;
    public DateTime TimeStart;
    public float LastTime = 0;
    //以下变量顾名思义
    public bool ISChipTimeUpdated;
    public bool IsTimestampUpdated;
    public bool IsTemperatureUpdated;
    public bool IsAccelaraationUpdated;
    public bool IsGyroAngularVelocityUpdated;
    public bool IsAngularVelocityUpdated;
    public bool IsEulerAnglesUpdated;
    public bool IsMagFieldUpdated;
    public bool IsLinAccUpdated;
    public bool IsPortVoltUpdated;
    public bool IsPressureAndAltitudeUpdated;
    public bool IsLocationUpdated;
    public bool IsGPSUpdated;
    public bool IsQuatUpdated;
    public bool IsDataPrepared;
    public const float PI = 3.1415926535897932f;
    public const float g = 9.8f;
    #endregion

    #region 公有属性
    /// <summary>
    /// 本次循环的数据
    /// </summary>
    public IMUData M_CurDataQAEA
    {
        get
        {
            return curDataQuaAndEulerA;
        }
    }
    /// <summary>
    /// 上次循环的数据
    /// </summary>
    public IMUData M_PreDataQAEA
    {
        get
        {
            return previousDataQuaAndEulerA;
        }
    }
    /// <summary>
    /// 单例模式
    /// </summary>
    public static Gyroscope SPInctance
    {
        get
        {
            if (gyroSPItem == null)
            {
                gyroSPItem = new Gyroscope();
            }
            return gyroSPItem;
        }
    }
    #endregion

    #region 保护字段
    protected static IMUData curDataQuaAndEulerA;
    protected static IMUData previousDataQuaAndEulerA;        
    #endregion
        
    #region 私有字段
    private static Gyroscope gyroSPItem;
    private delegate void Convert2DataHandle(byte[] gyroData);
    private delegate void ReceiveDataHandle();
    private Convert2DataHandle Convert2Data;
    private ReceiveDataHandle MyReceiveData;
    #endregion

    override public void InitSerialPortItem()
    {
        curDataQuaAndEulerA = new IMUData();
        curDataQuaAndEulerA.InitValues();
        SPInctance.InitStates();
        switch (HeadPose.Instance.imuModule)
        {
            case IMUModule.LPMS:
                DATAIDENTI_HEAD = 0x3a;
                COMMAND_L = 0x00;
                COMMAND_H = 0x09;
                DATA_AMOUNT = 0x2c;
                DATAIDENTI_END_L = 0x0d;
                DATAIDENTI_END_H = 0x0a;
                SPInctance.BaudRate = 921600;
                DATAPACK_LENGTH = 55;
                Convert2Data = Convert2Data_LPMS;
                MyReceiveData = ReceiveData_LPMS;
                break;
        }
        SPInctance.OnReceiveData += SPInctance.GetBytes;
        SPInctance.Open();
        SPInctance.TimeStart = DateTime.Now;
#if DEBUG
        SPInctance.fs = new FileStream("C:\\Users\\Cheng Yao\\Desktop\\dataorigin.xls", FileMode.Append);
        SPInctance.wr = new StreamWriter(SPInctance.fs);
#endif
    }

    public void GetBytes(byte[] gyroData)
    {
        Convert2Data(gyroData);
#if DEBUG
        wr.WriteLine(/*需要记录的数据*/);
#endif
    }

    /// <summary>
    /// 将一个完整的数据包转换成目标结构体
    /// </summary>
    /// <param name="spReadData"></param>
    /// <returns></returns>
    private void Convert2Data_LPMS(byte[] spReadData)
    {
        IsDataPrepared = false;
        previousDataQuaAndEulerA = curDataQuaAndEulerA;
        //再次判断
        if (DATAIDENTI_HEAD == spReadData[0] && spReadData.Length != DATAPACK_LENGTH)
        {
            Debug.LogWarning("数据包不完整！");
            curDataQuaAndEulerA.IsNaN = true;
            return;
        }
        int i = 7;
        // 时间戳，这个不管怎么设置都有的
        curDataQuaAndEulerA.Timestamp = BitConverter.ToInt32(spReadData, i) * 0.0025f;
        SPInctance.IsTimestampUpdated = true;
        // Debug.Log("时间: " + curDataQuaAndEulerA.Timestamp.ToString());

        //以下几个变量一定要根据上位机的设置读取，否则就乱了
        //角速度 rad/s
        curDataQuaAndEulerA.AngularVelocity[2] = BitConverter.ToSingle(spReadData, i += 4) / PI * 180;
        curDataQuaAndEulerA.AngularVelocity[0] = BitConverter.ToSingle(spReadData, i += 4) / PI * 180;
        curDataQuaAndEulerA.AngularVelocity[1] = BitConverter.ToSingle(spReadData, i += 4) / PI * 180;
        SPInctance.IsAngularVelocityUpdated = true;
        // Debug.Log("角速度: " + curDataQuaAndEulerA.AngularVelocity.ToString());


        //四元数
        curDataQuaAndEulerA.Quat[2] = BitConverter.ToSingle(spReadData, i += 4);
        curDataQuaAndEulerA.Quat[0] = BitConverter.ToSingle(spReadData, i += 4);
        curDataQuaAndEulerA.Quat[1] = BitConverter.ToSingle(spReadData, i += 4);
        curDataQuaAndEulerA.Quat[3] = BitConverter.ToSingle(spReadData, i += 4);
        // Debug.Log("四元数: " + curDataQuaAndEulerA.Quat.ToString());
        SPInctance.IsQuatUpdated = true;

        //线性加速度
        curDataQuaAndEulerA.LinAcc[2] = BitConverter.ToSingle(spReadData, i += 4);
        curDataQuaAndEulerA.LinAcc[0] = BitConverter.ToSingle(spReadData, i += 4);
        curDataQuaAndEulerA.LinAcc[1] = BitConverter.ToSingle(spReadData, i += 4);
        float TimeElapse = (float)(DateTime.Now - SPInctance.TimeStart).TotalMilliseconds / 1000;
        curDataQuaAndEulerA.LinSpeed += curDataQuaAndEulerA.LinAcc * (TimeElapse - SPInctance.LastTime);
        SPInctance.LastTime = TimeElapse;
        // Debug.Log("线速度: " + curDataQuaAndEulerA.LinSpeed.ToString());
        // Debug.Log("加速度: " + curDataQuaAndEulerA.LinAcc.ToString());
        SPInctance.IsLinAccUpdated = true;
        IsDataPrepared = true;
        return;
    }


    public void InitStates()
    {
        IsTimestampUpdated = false;
        IsTemperatureUpdated = false;
        IsAccelaraationUpdated = false;
        IsGyroAngularVelocityUpdated = false;
        IsAngularVelocityUpdated = false;
        IsEulerAnglesUpdated = false;
        IsMagFieldUpdated = false;
        IsPortVoltUpdated = false;
        IsPressureAndAltitudeUpdated = false;
        IsLocationUpdated = false;
        IsGPSUpdated = false;
        IsQuatUpdated = false;
    }

    override public void ReceiveData()
    {
        MyReceiveData();
    }

    private void ReceiveData_LPMS()
    {
        while (serialPort.IsOpen)
        {
            //读取
            try
            {
                if (serialPort.BytesToRead >= DATAPACK_LENGTH)
                {
                    int tempN = serialPort.BytesToRead < DATAPACK_LENGTH * 2 ? serialPort.BytesToRead : DATAPACK_LENGTH * 2;
                    if (0 < tempN)
                    {
                        byte[] tempBufferData = new byte[tempN];
                        int tempReadLength = serialPort.Read(tempBufferData, 0, tempBufferData.Length);
                        tempBufferData.ToList().ForEach(p => QCacheData_ReadSPBytes.Enqueue(p));
                        while (QCacheData_ReadSPBytes.Count > DATAPACK_LENGTH)
                        {
                            byte tempH = QCacheData_ReadSPBytes.Dequeue();
                            //找到头尾并且长度都符合要求的包
                            if (tempH == DATAIDENTI_HEAD && QCacheData_ReadSPBytes.ElementAt(2) == COMMAND_H && QCacheData_ReadSPBytes.ElementAt(4) == DATA_AMOUNT && QCacheData_ReadSPBytes.ElementAt(8 + DATA_AMOUNT) == DATAIDENTI_END_L && QCacheData_ReadSPBytes.ElementAt(9 + DATA_AMOUNT) == DATAIDENTI_END_H && QCacheData_ReadSPBytes.Count > DATAPACK_LENGTH)
                            {
                                //开始组包
                                byte[] tempData = new byte[DATAPACK_LENGTH];
                                tempData[0] = tempH;
                                for (int j = 1; j < DATAPACK_LENGTH; j++)
                                {
                                    tempData[j] = QCacheData_ReadSPBytes.Dequeue();
                                }
                                //组包完成，处理raw数据
                                OnReceiveData(tempData);
                            }
                        }
                    }
                }
            }
            catch (System.IO.IOException e)
            {
                Debug.LogError(e.ToString());
                this.Close();
            }
            serialPort.DiscardInBuffer();
            Thread.Sleep(2);
        }
        serialPort.Close();
    }
}

[Serializable]
public struct IMUData
{
    /// <summary>
    /// 是否为无效数据
    /// </summary>
    public bool IsNaN;
    /// <summary>
    /// 时间：20YY:MM:DD:hh:mm:ss:ms
    /// </summary>
    public float Timestamp;
    public short[] ChipTime;
    public double Temperature;
    public Vector3 Accelaration;
    public Vector3 GyroAngularVelocity;
    public Vector3 AngularVelocity;
    public Vector3 EulerAngles;
    public Vector3 LinAcc;
    public Vector3 LinSpeed;
    public Vector3 MagField;
    public double[] PortVolt;
    public int Pressure;
    public int Latitude;
    public double Altitude;
    public int Longitude;
    public double GPSHeight;
    public double GPSYaw;
    public double GroundVelocity;
    public Quaternion Quat;
    public void InitValues()
    {
        IsNaN = false;
        Timestamp = 0;
        ChipTime = new short[7];
        Temperature = 0;
        Accelaration = Vector3.one;
        GyroAngularVelocity = Vector3.one;
        AngularVelocity = Vector3.one;
        EulerAngles = Vector3.one;
        MagField = Vector3.one;
        LinAcc = Vector3.one;
        LinSpeed = Vector2.zero;
        PortVolt = new double[4];
        Pressure = 0;
        Altitude = 0;
        Longitude = 0;
        Latitude = 0;
        GPSHeight = 0;
        GPSYaw = 0;
        GroundVelocity = 0;
        Quat = new Quaternion(0, 0, 0, 0);
    }
}

上面的代码大部分都很好理解，为了扩展不同数据格式的传感器，这里的数据读取和解析部分都用委托来实现，如果要改用新的传感器，只需把委托相应的数据解析和读取函数分别赋给Convert2Data和MyReceiveData这两个委托变量即可。这里，数据读取函数ReceiveData_LPMS较为复杂，下面用流程图对其进行解释说明。

数据读取流程图

3. 数据调用

数据的调用很简单，直接把 Gyroscope 类的相关变量赋给场景中的 Camera 即可。代码如下：

#define DEBUG
// #undef DEBUG

using UnityEngine;
using System.IO;

public enum IMUModule
{
    LPMS
}

public class HeadPose : MonoBehaviour
{
    public static HeadPose Instance
    {
        get
        {
            if (null == m_Instance)
                m_Instance = FindObjectOfType<HeadPose>();
            return m_Instance;
        }
    }
    [SerializeField]
    public IMUModule imuModule;
    private FileStream fs;
    private StreamWriter wr = null;
    private Vector3 currentSpeed;
    private float originTimeStamp;
    private float originTime;

    private static HeadPose m_Instance;

    void Start()
    {
        currentSpeed = Vector3.zero;
        Gyroscope.SPInctance.InitSerialPortItem();
        if (!Gyroscope.SPInctance.IsOpen)
        {
            Gyroscope.SPInctance.Open();
        }
        if (Gyroscope.SPInctance.IsQuatUpdated)
        {
            this.transform.localRotation = Gyroscope.SPInctance.M_CurDataQAEA.Quat;
            Debug.Log("用四元数更新");
        }
#if DEBUG
        fs = new FileStream("C:\\Users\\Cheng Yao\\Desktop\\data.xls", FileMode.Create);
        wr = new StreamWriter(fs);
#endif
    }

    private void LateUpdate()
    {
        if (!Gyroscope.SPInctance.IsOpen)
        {
            Gyroscope.SPInctance.Open();
        }
        //由于是多线程，所以这里要等整帧数据都传完了才能读，否则有可能前后读到的数据不属于同一次循环
        if (Gyroscope.SPInctance.IsDataPrepared)
        {
            this.transform.localRotation = Gyroscope.SPInctance.M_CurDataQAEA.Quat;
#if DEBUG
            wr.WriteLine(/*需要记录的数据*/);
#endif
            Gyroscope.SPInctance.IsQuatUpdated = false;
            Gyroscope.SPInctance.InitStates();
        }
    }

    private void FixedUpdate()
    {
        if (Time.frameCount % 120 == 0)
        {
            System.GC.Collect();
        }
    }

    private void OnApplicationQuit()
    {
        Gyroscope.SPInctance.Close();
        try
        {
            Gyroscope.SPInctance.wr.Close();
        }
        catch
        {
        }
#if DEBUG
        wr.Close();
#endif
    }
}

这里直接读取 IMU 的数据，看起来是没问题，但是如果我们记录下来这些数据进行分析，就会发现其中的问题。

在 Excel 中把数据化成图像，可以看出这个曲线是不平滑的

在上图中，可以看出这些数据表现得不太正常——两点之间的时间间隔大约是0.02s，对于如此高的帧率，除非我们是非常高频地震动传感器，否则数据不可能如此不平滑（我们也可以记录 Gyroscope 这个类中的数据，可以观察到原始数据其实是很平滑的）。为什么会出现这种情况呢？

这是因为，虽然多线程在宏观上看是并行进行的两个互不干涉的任务，但在微观上看，不同线程在进行计算时，实际上是在先后抢占 CPU 的使用权，各个线程并不一定是轮流使用 CPU 的。因此，在 HeadPose 类每次调用 Update 的时候，其调用的数据是串口上一次读取的数据，而我们并不能知道这个数据从读取到调用间隔了多久有可能，比如，有时是 5ms 前的，有时是 8ms 前的，有时又是 10ms 前的，这就会使原本平滑的数据变得不平滑。如下图所示。

数据延迟示意图

图中，黄色曲线代表 Update 函数调用数据的时间-值关系，蓝色曲线代表串口读取数据的时间-值关系。可以看出，数据在时间上的延迟导致了数据的不平滑。要消除这一延迟，我们可以取两次循环各自的时间戳和数据，通过外插的方式估算出 Update 函数调用的那一时刻的 IMU 数据。因此，代码修改如下。

void Start()
{
    currentSpeed = Vector3.zero;
    Gyroscope.SPInctance.InitSerialPortItem();
    if (!Gyroscope.SPInctance.IsOpen)
    {
        Gyroscope.SPInctance.Open();
    }
    //读到有效数据以后才可以打一个时间戳，如果5秒都读不到，那就不玩了
    while (0 == Gyroscope.SPInctance.M_CurDataQAEA.Timestamp)
    {
        if (Time.realtimeSinceStartup >= 5)
        {
            Debug.LogError("读取数据超时");
            Gyroscope.SPInctance.Close();
            break;
        }
    }
    originTimeStamp = Gyroscope.SPInctance.M_CurDataQAEA.Timestamp;
    originTime = Time.realtimeSinceStartup;

    if (Gyroscope.SPInctance.IsQuatUpdated)
    {
        this.transform.localRotation = Gyroscope.SPInctance.M_CurDataQAEA.Quat;
        Debug.Log("用四元数更新");
    }
#if DEBUG
    fs = new FileStream("C:\\Users\\Cheng Yao\\Desktop\\data.xls", FileMode.Create);
    wr = new StreamWriter(fs);
#endif
    }

private void LateUpdate()
{
    if (!Gyroscope.SPInctance.IsOpen)
    {
        Gyroscope.SPInctance.Open();
    }
    if (Gyroscope.SPInctance.IsDataPrepared)
    {
        float lastTimeStamp = Gyroscope.SPInctance.M_PreDataQAEA.Timestamp - originTimeStamp;
        float curTimeStamp = Gyroscope.SPInctance.M_CurDataQAEA.Timestamp - originTimeStamp;
        float t = (Time.realtimeSinceStartup - originTime - lastTimeStamp) / (curTimeStamp - lastTimeStamp);
        this.transform.localRotation = Quaternion.SlerpUnclamped(Gyroscope.SPInctance.M_PreDataQAEA.Quat, Gyroscope.SPInctance.M_CurDataQAEA.Quat, t);

        Debug.Log(t + ", " + Gyroscope.SPInctance.M_PreDataQAEA.Quat.x + ", " + Gyroscope.SPInctance.M_CurDataQAEA.Quat.x + ", " + this.transform.localRotation.x);
        this.transform.Rotate(0, 0, 0);
#if DEBUG
        wr.WriteLine(/*需要记录的数据*/);
#endif
        Gyroscope.SPInctance.IsQuatUpdated = false;
        Gyroscope.SPInctance.InitStates();
    }
}

经过外插后的数据如下图绿色曲线所示，可以看出数据已经平滑了许多。并且可以看到，绿色曲线整体在蓝色曲线的左边，也就是说，数据的延迟得到了有效的消除。

做了外插后的数据，可以看出平滑了很多，而且延迟也消除了

4. 总结

本文通过 C# 串口通信的方式读取了 LPMS-ME1 这一款 IMU 的数据，使其能够为 XR 头显提供 3DOF。并通过外插方式，在一定程度上消除了数据的延迟，使数据更符合实际应用情况。