黄伟，田明，冯进良，王菲
（长春理工大学 光电工程学院，长春 130022）
手势交互作为一种自然人机交互方式，是基于多传感器设备捕捉肢体动作，处理后生成控制器可识别的信息，从而实现操作的一种交互方式。该技术集成了传感器技术、模式识别技术和电子信息技术，是现如今人机交互领域的重要基础性研究之一。随着新型体感交互设备的普及与最优估计理论的深入，如何快速准确地捕捉和处理人体手臂姿态信息完成机械臂的控制运动，从而达到实现主从手操作感知协同的目的，已成为自然人机交互领域的关键问题。伴随传感器设备的不断发展，捕捉人体手臂信息进行姿态融合解算从而控制机械臂的设备以及方法也越来越多。有基于图像设备捕捉手臂的姿态信息实现机械臂的运动控制，有基于惯性传感器获取手臂的姿态实现机械臂的运动控制。由于每个传感器都存在着一定的局限性，因此需要根据传感器的不同特性进行多传感器互补融合实现手臂运动的姿态估计。
本文使用MEMS 传感器对人体手臂姿态进行采集估计，由于MEMS 传感器的精度低，噪声高，直接使用采集到的三轴角速度进行姿态估计，随着误差的累积，会导致姿态角的发散，因此采用加速度计、陀螺仪和磁力计进行传感器信息融合，可以有效地控制姿态估计误差。Wu等人[1]提出了基于互补滤波的姿态估计算法，该算法根据不同传感器的特性进行互补，计算量低，但是其滤波系数固定，根据经验判断，未能充分考虑到传感器的噪声因素，所以姿态估计的 精度较低。Hadri、Wu 等人[2-3]在 互 补滤波的基础上，使用PI 控制器对四元数进行更新，大大降低了陀螺仪的累积误差，但是动态响应较差。Filho、吴涛等人[4-5]提出采用卡尔曼滤波进行姿态估计，采用四元数作为状态量，加速度计作为观测量进行状态更新，该算法实时性较好，收敛较快，但是由于卡尔曼滤波模型中的系统过程协方差矩阵和量测过程噪声协方差矩阵的统计特性难以准确获取。
为了获取到更加准确的量测过程噪声协方差矩阵，本文提出一种基于T-S 模糊控制的卡尔曼滤波姿态估计方法，将加速度计数据的方差值作为T-S 模糊控制的输入，得到方差系数ζ，用于表征卡尔曼滤波器中量测噪声协方差的变化，实现量测协方差矩阵的实时调整，结合磁力计陀螺仪数据，得到最优四元数估计，达到姿态解算的目的。
坐标系是姿态解算的关键部分。选取不同的坐标系获取的姿态信息也各不相同，常见的坐标系为导航坐标系和载体坐标系。一般情况下称导航坐标系为n系，通常选择地理坐标系为导航坐标系。地理坐标系任取地球表面一点，以经纬线的切线方向为X、Y轴方向，以重力加速度方向为Z轴方向。在姿态解算中常以东北天、北东地为参考系。本文的导航坐标系选择东北天为参考系，记为OXnYnZn。载体坐标系称为b系，以运载体的质心为原点，将运载体自身结构作为坐标系。以飞机航行为例，以飞机质心为原点，飞机机头航向为X轴，水平向右为Y轴，竖直向上为Z轴，即为常见的右手直角坐标系，记为OXbYbZb。
1.1 姿态角表示方法
常见的姿态解算表示方法有欧拉角法、四元数法、方向余弦矩阵法等[6]。其中，欧拉角法是一种比较直观容易理解的表达方式，用三个参数来表示物体运动姿态，并且在计算量方面要比四元数法和方向余弦矩阵法小得多，不存在正交化处理，但是欧拉角存在万向节锁的现象，不能全方位表示姿态测量，涉及大量三角函数运算，不利于硬件上的运算实施
方向余弦法是采用九个参数表示，避免了欧拉角产生的奇异值问题，但是参数的增加也增加了计算成本，会减弱解算的实时性[7]。
四元数法进行姿态解算，相比方向余弦法的计算量大大减少，而且还避免了欧拉角产生的问题，只需要求解四个微分方程，有效地提高了解算的实时性。四元数作为复数的一种扩展形式，表示方式如下：
式中，i、j、k分别表示相对x，y，z轴的旋转角度，因此可以得到用四元数表示的旋转转换矩阵如下：
式中，ωx、ωy、ωz为利用传感器进行数据融合获取的载体坐标系下的陀螺仪三轴角速度值。
由于人体手臂在摆动运动的过程中对姿态解算的精度和实时性的要求比较高，通过陀螺仪三轴角速度信息，利用一阶龙格库塔法[8]对四元数进行实时更新，得到更新矩阵如下：
对四元数进行单位化，更准确地表示姿态信息，公式如下：
1.2 各表示法的相互转化
欧拉角法、方向余弦法、四元数法都是表征姿态角的一种方式，在实际应用当中，需要将载体坐标系中的表示测量值转换到导航坐标系下进行姿态解算。
对于欧拉角转化方向余弦法，从载体坐标系到导航坐标系的转换表示为三个轴向独立转动的乘积：
其中，C(θ)、C(γ)、C(φ)表示为：
因此可以得到从载体坐标系转换到导航坐标系，旋转变换矩阵如下：
若已知坐标转换矩阵内的每个元素值，可以得到姿态角，表达式如下：
四元数与方向余弦法的转换如式（2）的旋转转换矩阵。由式（11）、式（2）得到欧拉角法与四元数法的相互转换关系，如下：
2.1 互补滤波
互补滤波算法通过不同传感器的噪声特性进行互补，实现姿态的解算[9]。其核心是对不同的传感器进行不同的滤波方法，赋予一定的权重，最后求和得到最终信号。将加速度计数据进行低通滤波，陀螺仪数据经高通滤波后，相加求和得到融合姿态角，互补滤波示意图如图1 所示。
图1 互补滤波示意图
2.2 卡尔曼滤波
卡尔曼滤波作为一种线性最优估计算法[10]，通过“预测-实测-修正”的模式，利用系统的量测更新值消除系统的随机干扰，从而达到优化还原系统的状态。
卡尔曼滤波有两大功能，即为预测和校正，如图2 所示。预测为时间更新方程，校正为量测更新方程。时间更新方程通过判断上一时刻的状态值来估计当前时刻的状态变量先验估计值和协方差先验估计值，而量测方程则是将先验估计和当前测量值计算得到后验估计，达到校正的目的。
图2 卡尔曼滤波更新示意图
（1）系统状态一步预测方程：
（2）估计均方差方程：
式中，Pk为时刻的后验估计协方差。
（3）增益方程：
式中，Kk为计算时刻的卡尔曼增益。
（4）状态估值计算方程：
式中，Zk为量测值。
（5）估计均方误差方程：
3.1 T-S 模糊控制器
模糊控制算法是一种非模型算法[11]。模糊控制算法外表用人类的语言进行描述，通过将其转换为数学函数语言，从而达到控制的目的。由此看来，模糊控制没有一个明确的数学模型，以人的实际经验为依据，实现一种智能算法控制。模糊算法一般分为Mamdani 模型和T-S 模型两种模型。T-S 模糊系统可以看成一个用IF-THEN rules 模糊规则描述的输入输出关系，模糊思维框图如图3 所示。
图3 模糊思维框图
模糊推理思维无须建立准确的数学模型，只需要根据系统输入的模糊信息，建立模糊规则库，通过输入信息和模糊规则库对比判断，从而实现判断辨识。T-S 模糊控制器的系统框图如图4 所示。
图4 模糊控制系统框图
3.2 T-S 模糊卡尔曼滤波实现
模糊控制对于处理不确定问题有着极好的能力，因此本文以T-S 型模糊算法和卡尔曼滤波结合实现姿态解算优化[12]。通过模糊推理对加速度计的数据状态进行判断，实现对卡尔曼滤波器中量测噪声协方差的调整，因此本文主要设计思路是以加速度计的x轴和y轴数据的方差作为T-S 模糊控制的输入，得到方差系数ζ，用于表征卡尔曼滤波器中量测噪声协方差的变化，结合磁力计陀螺仪数据，得到陀螺仪随机漂移最优估计和四元数最优估计，用陀螺仪随机漂移最优估计修正陀螺仪姿态角的漂移发散。图5 为模糊推理卡尔曼滤波原理框图。
图5 模糊推理卡尔曼滤波原理框图
T-S 模糊控制采用二维模糊推理器，将加速度计的x轴和y轴数据的方差δ1、δ2作为推理系统的输入T1、T2，得到推理输出的方差控制系数ζ。本文定义输入方差的模糊集合分为正小PS、正中PM、正大PB 三个域，输出模糊集合分为小S，中M，大L。假设隶属分布如图6 所示。
图6 隶属分布图
可以认为输入方差小于等于0.01 时，认为加速度计和磁力计的量测信息可靠；
当输入方差在0.01 和0.015 之间，认为量测信息相对可靠；
当输入方差大于等于0.027 时，认为量测信息不是相对可靠。建立模糊规则库如表1 所示。
表1 模糊规则库
4.1 卡尔曼滤波设计
本文在ZYNQ7020 上实现卡尔曼滤波，硬件实现上对卡尔曼滤波五个方程式进行简化实现的RTL 视图，如图7 所示为卡尔曼滤波RTL 视图。累计消耗3 075 个LUT、5 216 个寄存器、1 个RAM、54 个DSP 资源。
图7 卡尔曼滤波RTL 视图
4.2 T-S 模糊控制设计
本文在ZYNQ7020 上实现T-S 模糊控制器，如图8 所示为T-S 模糊控制RTL 视图，累积消耗6 956 个LUT、8 064 个寄存器资源。
图8 T-S 模糊控制器RTL 视图
4.3 算法分析
为了检验算法的可行性，本文算法在Matlab上进行验证仿真，选取随机参数进行仿真，仿真数据图如图9 所示，本文算法在针对随机数据进行仿真处理，可以看出本算法整体的快速收敛性较好，尤其在数据波动较大的情况，本文算法依旧可以实现快速收敛稳定。如图10 所示，本文算法实现的估计误差快速收敛的同时在显著减小。
图9 仿真数据
图10 仿真估计方差
在此仿真基础上，本文主要是利用传感器MPU9250 对手臂运动姿态进行测量，获取手臂的加速度、角速度和磁力计数据的原始信息，数据共九组，采取动态数据1 382 组，频率为200 Hz，每采集一次进行一次数据融合解算，使用本文算法对传感器采集手臂运动姿态的原始信息进行姿态解算。
如图11 为互补滤波、卡尔曼滤波以及本文算法三种算法解算出来的横滚角对比示意图，图12 为偏航角对比示意图，图13 为俯仰角对比示意图。可以看出，互补滤波的融合解算效果最差，其次是卡尔曼滤波，响应速度较好，本文算法实现的融合解算滤波效果最好，曲线平滑优于互补滤波和卡尔曼滤波。
图11 横滚角滤波对比示意图
图12 偏航角滤波对比示意图
图13 俯仰角滤波对比示意图
为了验证三个算法的滤波融合解算平稳性，对俯仰角、横滚角、偏航角进行平均误差和均方根误差，如表2 为三种算法估计值与真实值的平均误差和均方根误差数据对比表。可以得出解算融合俯仰角，本文算法平均误差比互补滤波降低了96.94%，比卡尔曼滤波降低了92.52%。本文算法均方根误差比互补滤波降低了97.74%，比卡尔曼滤波降低了89.44%。
表2 三种算法估计值与真实值的平均误差和均方根误差
本文基于模糊控制与卡尔曼滤波的原理，融合二者并在FPGA 内实现采集手臂姿态测量解算算法。该算法使用四元数进行解算姿态，避免了欧拉角存在的奇异点问题，且相比方向余弦矩阵法降低了计算量。实验表明：本文实现的算法有效抑制了姿态角漂移和噪声，通过对加速度计的方差数据进行模糊控制得到较精确的量测噪声协方差的信息，融合解算出更精确的姿态角，且在传感器进行快速抖动的前提下，本文算法可以实现快速跟踪，与单独的互补滤波和卡尔曼滤波相比，提高了解算精度，实时性较好，具有一定的意义。
猜你喜欢协方差卡尔曼滤波模糊控制基于递推更新卡尔曼滤波的磁偶极子目标跟踪北京航空航天大学学报(2017年9期)2017-12-18多元线性模型中回归系数矩阵的可估函数和协方差阵的同时Ｂａｙｅｓ估计及优良性华东师范大学学报（自然科学版）(2017年1期)2017-02-27二维随机变量边缘分布函数的教学探索考试周刊(2016年54期)2016-07-18T-S模糊控制综述与展望西南交通大学学报(2016年3期)2016-06-15基于模糊控制的PLC在温度控制中的应用电子制作(2016年21期)2016-05-17不确定系统改进的鲁棒协方差交叉融合稳态Kalman预报器自动化学报(2016年8期)2016-04-16基于模糊控制的恒压供水系统的研究通信电源技术(2016年5期)2016-03-22基于模糊卡尔曼滤波算法的动力电池SOC估计电源技术(2016年9期)2016-02-27基于扩展卡尔曼滤波的PMSM无位置传感器控制电源技术(2015年1期)2015-08-22用于光伏MPPT中的模糊控制占空比扰动法电源技术(2015年11期)2015-08-22