九轴无线姿态传感器（LPMS-B）检测分析及应用

黄悦峰，王 榜，张启鹏，朱婉莹，李 标

（广西制造系统与先进制造技术重点实验室广西大学机械工程学院，广西 南宁530004）

信息时代的关键在于信息获取，信息获取关键在于传感器。姿态信息获取、姿态检测具有广泛和重要的应用范围，在无人机的惯性测量系统、车辆船舶倾斜角的测量、机器人运动的测量等都有着重要的应用。目前，在姿态测量组合方面，比较成熟的有加速度计、陀螺仪和磁强计组合，现有的惯性姿态测量系统主要采用这种组合方式。随着新材料，新原理，新技术的不断出现，姿态测量组合逐渐向小型化、低成本、高精度的方向发展。国外对于MEMS（MEMS，Micro-Electro-Mechanical System）的研究较早，20世纪80年代在加拿大、韩国、日本以及澳大利亚等国家都开展了深入的研究，此外，一些高校和企业投入大量的精力物力开展 MEMS 研究[1，2，3]，都取得了不错的研究成果。许多高技术公司，例如：美国J.F.Lehman&Company公司[3]的最新产品 SiNAV型组合导航系统采用MIMU/GPS紧耦合方案，定位误差小于10 m，速度误差小于0.1 m/s，可耐受20 000 g的冲击，陀螺测量范围可达±14 000（°）/s.德国 Litef公司[2]B-290 硅加速度计，量程为10 g，标度因数稳定性为3×10-4，偏置稳定性为250×10-6g，已经与光纤陀螺组合成IMU.国内对于姿态传感器检测技术的发展起步较晚，和发达国家相比存在较大差距，但一些高校和相关研究部门也取得了很大进步。特别是清华大学、北京航空航天大学、哈尔滨工业大学和哈尔滨工程大学以及中国时代电子集团[3]等都对姿态传感器检测相关技术进行了大量研究，而且取得显著研究成果。

LPMS-B作为姿态传感器当中的一种，可以测量三个全局坐标轴的360°方向。它的测量是以数字方式进行的，并以定向四元数或欧拉角的形式传输到数据分析系统。而欧拉角是描述物体取向的传统方式，四元数允许定向测量而不会遇到所谓的万向节锁定，通过使用四元向量来表示围绕所有轴的取向而不受奇点的限制[4]。LPMS-B应用范围广泛，其中小型运动传感器的应用领域之一就是人体运动损伤康复测量，步态周期分析，手术技能训练和评估等。该传感器还可以应用于虚拟现实，导航，机器人或者是测量车辆动态性能。本文首先对九轴无线姿态传感器（LPMS-B）的基本功能进行介绍，然后对其性能进行测试分析，最后把LPMS-B应用于机器人末端姿态测试。试验结果表明，对于单轴转动，改进的四元数互补滤波数据融合算法更能精确检测机器人的末端姿态，对于多轴联动，由于各轴的相互影响，使得各算法之间的差异不明显。

1　LPMS-B的概述

1.1　LPMS-B的基本参数

目前市场上主要有两种不同的LPMS-B传感器封装，分别命名为LPMS-B标准版和OEM版。其实物图如图1所示，主要参数如表1所示。

图1　LPMS-B实物图

表1　LPMS-B（标准版）和LPMS-B（OEM版）主要参数表

1.2　LPMS-B的滤波

来自三个MEMS传感器的数据通过扩展互补卡尔曼滤波器（LP-Filter）进行组合，以计算方位数据，如四元数和欧拉角。为了使滤波器正常工作，需要以适当的方式设置其参数。首先，需要选择滤波器的模式，可以通过LpmsControl软件或固件命令来设置，滤波模式分为：陀螺仪（Gyr）、陀螺仪+加速度计（Gyr+Acc）、陀螺仪+加速度计+磁强计（Gyr+Acc+Mag）、加速度计+磁强计（Acc+Mag）。各功能如表2所示。

表2　LPMS-B滤波模式

1.3　LPMS-B的校准方法

LPMS-B的校准包括陀螺仪、加速度计和磁力计这三个传感器的校准。当传感器静止时，陀螺仪的输出数据应该在零附近，来自陀螺仪传感器的原始数据具有一定值的恒定偏差。要校准这个偏差，需要通过固件命令或使用LpmsControl软件触发陀螺仪校准程序，在校准的过程中，陀螺仪阈值也将被调整。在磁力计校准过程中，要确定几个参数：磁力计偏置和X，Y和Z轴上的增益；地磁场矢量的长度和方向。由于地球磁场受电源线、金属等电磁噪声源的影响，会使得磁场变得偏心和变形，因此在准期间，计算去中心和变形的量以及磁场矢量的平均长度。加速度计相对于LPMS-B外壳的错位是由错位矩阵来表示的，LpmsControl软件不仅可以对错位矩阵进行校准，还可以评估加速度计的偏移量。

1.4　LpmsControl的介绍

LpmsControl应用程序可以控制LPMS-B传感器的各方面。其功能有：通过一个蓝牙接收器可以连接7个传感器、调整传感器参数（传感器范围等）、重置方向和参考矢量、启动陀螺仪和磁力计校准、以线形图或3D立方体的形式实时显示采集的数据、将传感器的数据记录到CSV数据文件中。上位机应用程序窗口，其中包括采用频率设置，频率范围为5.75 Hz～133 Hz；陀螺仪、加速度计以及磁强计的变化范围分别为：250 dps～2 000 dps、2 G～16 G 以及 130 uT～810 uT.LPMS-B上位机界面如图2所示。

图2　LpmsControl应用程序窗口

2　LPMS-B的测试分析

为了验证LpmsControl软件获取传感器采集得到数据的正确性，需要对传感器随机采集到的数据进行验证，也就是验证基于四元数求得的转换矩阵T和基于欧拉角求得的转换矩阵C是否近似相等。验证结果如图3所示（取两转换矩阵其中一列数值作差得到的结果作图）。可以看出该传感器获得的数据是有效的。

图3　转换矩阵T与C的误差

2.1　陀螺仪的数据测试与处理

一般陀螺仪在静态采集数据时，随时间的推移会产生较大的漂移和累积误差，这种情况严重影响了其测量精度，但是在动态时，产生的漂移和累积误差会相对减小。在这里将采用对照实验，分别测量静态和动态时陀螺仪yaw角的变化，一组是将LPMS-B传感器至于水平桌面上，z轴向上，采用陀螺仪（Gyr Only）滤波模式，采样频率 100 Hz，时间为 60 s.另一组则把LPMS-B传感器至于转椅上，z轴向上，测量过程中转椅转动一定角度，采用陀螺仪（Gyr Only）滤波模式，采样频率100 Hz，时间为60 s.图4和图5分别是静态和动态下的测量结果。从图4的趋势来看，随着时间的推移，漂移会越来越严重，累积误差则逐渐变大。由图5可以看出，陀螺仪动态测量时依旧存在着漂移，但漂移程度有所减小。这说明动态性较能好，但是单独采用陀螺仪进行数据测量，还是会存在较大的误差。

图4　陀螺仪滤波模式下静态测量航向角

图5　陀螺仪滤波模式下动态测量航向角

2.2　加速度计的数据测试与处理

加速度计可以通过计算其倾角来检测传感器安装是否水平，这对加速计采集数据的准确性有着重要的影响作用。加速度计获得的三轴加速度分别是ax，ay，az，三轴加速度矢量和为a→,可得：

对矢量a→归一化

■

由式（2）得到当前时刻的归一化重力方向矢量R→acc.

由归一化矢量方向可得到重力矢量同坐标轴的夹角 θx，θy，θz如图 6 所示。

图6　重力矢量转换坐标

3　机器人末端姿态测试的应用

实验检测末端姿态精度的机器人名称为OTC机器人，实验平台采用的传感器类型为九轴无线姿态传感器，采用（Gyr+Acc+Mag）滤波模式。现场实验图如图7所示。

图7　现场试验

本实验将从机器人的单轴转动和多轴联动采集数据，然后通过更新的四元数算法、卡尔曼滤波数据融合算法和互补滤波数据融合算法进行姿态解算，对解算的结果进行分析和比较。

3.1　机器人单轴转动实验

实验将对欧拉角进行测量，分别试验5次。

（1）测横滚角：机器人末端执行器绕传感器X轴匀速旋转，通过示教盒将其旋转角度为20°左右。

（2）测俯仰角：机器人末端执行器绕传感器Y轴匀速旋转，通过示教盒将其旋转角度为20°左右。

（3）测航向角：机器人末端执行器绕传感器Z轴匀速旋转，通过示教盒将其旋转角度为20°左右。

对于机器人单轴转动采集到的数据，利用改进的高斯牛顿算法、改进的四元数卡尔曼滤波数据融合算法和改进的四元数互补滤波数据融合算法进行解算，结果如图8、图9、图10所示。由于机器人运动平稳，无出现较为明显的抖动现象，欧拉角曲线变化平滑，能很好的检测出机器人末端姿态精度，从图中可以看出基于改进的四元数互补滤波数据融合算法更接近于实际的测量曲线。

图8　单轴转动三种算法比较（roll角）

图9　单轴转动三种算法比较（pitch角）

图10　单轴转动三种算法比较（yaw角）

3.2　机器人多轴联动实验

（1）测横滚角和俯仰角：传感器坐标系和机器人基座标系重合，多轴联动使横滚角和俯仰角转动角度均为 20°（航向角为 20°）。

（2）测量航向角：传感器坐标系和机器人基座标系重合，多轴联动使航向角转动角度为20°（横滚角和航向角均为20°）。

对于机器人多轴联动采集到的数据，利用改进的高斯牛顿算法、改进的四元数卡尔曼滤波数据融合算法和改进的四元数互补滤波数据融合算法进行解算，结果如图11、图12、图13所示。从图中可以看出，对于多轴联动，由于多个轴间的相互影响，出现较为明显的抖动现象，使得欧拉角的累积误差增加，欧拉角曲线变化出现较大波动，使得各算法之间的效果不明显。

图11　多轴联动三种算法比较（roll角）

图12　多轴联动三种算法比较（pitch角）

图13　多轴联动三种算法比较（yaw角）

3.3　三种算法的比较分析

如表3为对单轴转动和多轴联动三种算法的比较，单轴转动时，采用改进的四元数互补滤波数据融合算法解算末端横滚角、俯仰角和航向角的均方根误差值分别为 0.145°、0.180°和 0.195°；多轴联动时，采用改进的四元数互补滤波数据融合算法解算末端横滚角、俯仰角和航向角的均方根误差值分别为0.343°、0.239°和 0.610°.这说明，对于单轴转动时，由于运动平稳，更够更好的检测机器人的末端姿态，而对于多轴联动，由于俯仰角受到的干扰程度比其他横滚角、航向角干扰小，所以精度较高为0.239°，而航向角受到的干扰最大，均方根误差值最大为0.610°.

表3　三种算法比较

4　结束语

通过三种算法分别对九轴无线姿态传感器（LPMS-B）进行检测，试验结果表明，对于单轴转动，由于运动平稳，基于改进的四元数互补滤波数据融合算法效果更好，更能精确检测机器人的末端姿态，但是对于多轴联动，由于多个轴间的相互影响，出现较为明显的抖动现象，使得欧拉角的累积误差增加，欧拉角曲线变化出现较大波动，使得各算法之间的效果不明显。在研究的过程中，也存在着一些不足，机器人多轴联动时，由于机器人各个轴相互影响产生累积误差，使得采用LPMS-B进行精度检测时出现较大的误差，在姿态解算过程如何采用误差补偿提高姿态检测精度这一问题没能解决。

参考文献：

[1]李小朋.微惯性组合的抗磁干扰和温度补偿方法研究[D].上海：上海交通大学，2011.

[2]姚成凯.基于嵌入式的MEMS加速度传感器测控仿真系统[D].哈尔滨：哈尔滨工程大学，2008.

[3]张 英.MEMS航天惯导产品及技术发展简介[J].航天标准化，2010（3）：65-68.

[4]盛庆轩.MIMU/磁强计航姿参考系统研究[D].长沙：国防科学技术大学，2009.