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;
///
/// 串口通信
///
public abstract class SerialPortItem
{
#region 公有字段
///
/// 串口名
///
public string portName;
///
/// 无效端口
///
public List unvalidSerialPort;
///
/// 波特率
///
public int BaudRate;
///
/// 校验位
///
public Parity Parity;
///
/// 停止位
///
public StopBits StopBits;
///
/// 数据位
///
public int DataBits;
///
/// 握手
///
public Handshake Handshake;
///
/// 流控
///
public bool RtsEnable;
///
/// 数据头标识
///
public int DATAIDENTI_HEAD;
///
/// 数据尾部标识
///
public int DATAIDENTI_END;
///
/// 数据颈部标识
///
public int DATAIDENTI_NECK;
///
/// 指令低八位（后收到）
///
public int COMMAND_L;
///
/// 指令高八位（先收到）
///
public int COMMAND_H;
///
/// 数据总字节数，分别包括时间戳（4）、原始加速度（4*3）、原始磁场（4*3）、原始陀螺仪（4*3）、角速度（4*3）、四元数（4*4）、欧拉角（4*3）、线性加速度（4*3）
///
public int DATA_AMOUNT;
///
/// 数据尾高八位（先收到）
///
public int DATAIDENTI_END_L;
///
/// 数据尾低八位（后收到）
///
public int DATAIDENTI_END_H;
///
/// 字节转换成四元素的参数常量
///
public int BYTE2QUA_PARAM;
///
/// 字节转换成欧拉角速度的参数常量
///
public int BYTE2EU_PARAM;
///
/// 串口数据包的长度
///
public int DATAPACK_LENGTH;
///
/// 单个数据的长度
///
public int DATASINGLE_LENGTH;
#endregion
# region 保护字段
///
/// 串口
///
protected SerialPort serialPort;
///
/// 读取到串口的缓冲字节数据列表
///
protected static Queue QCacheData_ReadSPBytes = new Queue();
# endregion
#region 私有字段
///
/// 串口状态（是否打开）
///
private bool isOpen;
// private bool isClose;
///
/// 收发串口线程
///
private Thread tReceiveData;
#endregion
#region 公有属性
///
/// 是否开启串口
///
public bool IsOpen
{
get { return isOpen; }
}
#endregion
#region 事件
///
/// 接到数据(子线程)
///
public Action OnReceiveData;
///
/// 错误(子线程)
///
public Action OnError;
#endregion
///
/// 开启串口
///
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 公有属性
///
/// 本次循环的数据
///
public IMUData M_CurDataQAEA
{
get
{
return curDataQuaAndEulerA;
}
}
///
/// 上次循环的数据
///
public IMUData M_PreDataQAEA
{
get
{
return previousDataQuaAndEulerA;
}
}
///
/// 单例模式
///
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
}
///
/// 将一个完整的数据包转换成目标结构体
///
///
///
/// 是否为无效数据
///
public bool IsNaN;
///
/// 时间：20YY:MM:DD:hh:mm:ss:ms
///
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();
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。并通过外插方式，在一定程度上消除了数据的延迟，使数据更符合实际应用情况。