基于模糊卡尔曼滤波的姿态估计.pdf

长春理工大学学报（自然科学版）Journal of Changchun University of Science and Technology（Natural Science Edition）Vol.46No.2Apr.2023第46卷第2期2023年4月收稿日期：2022-04-11基金项目：科技部国家重点研发计划项目（KYC-JC-XM-2018-157）作者简介：黄伟（1998-），男，硕士研究生，E-mail：通讯作者：田明（1971-），男，硕士，副教授，硕士生导师，E-mail：黄伟，等：基于模糊卡尔曼滤波的姿态估计基于模糊卡尔曼滤波的姿态估计黄伟，田明，冯进良，王菲（长春理工大学光电工程学院，长春130022）摘要：在人机控制交互领域，通常通过惯性测量单元（IMU）采集手臂姿态原始信息并进行解算融合，从而达到获取人体手臂运动姿态以达到控制机械臂的目的。基于陀螺仪的动态特性和加速度计、磁力计的静态性能，考虑引入卡尔曼滤波进行姿态融合解算，由于卡尔曼滤波模型中的量测过程噪声协方差矩阵的统计特性难以准确获取，提出一种基于 T-S 模糊控制的卡尔曼滤波姿态估计方法，实现量测协方差矩阵的实时调整。基于 ZYNQ7020 搭建实验平台对手臂姿态信息进行融合解算，融合解算后的姿态角平均误差和均方误差在 0.06 和 0.16 以内，相比卡尔曼滤波降低了 92.52%、89.44%，有效提高了姿态角解算精度。关键词：卡尔曼滤波；模糊控制；FPGA；人机交互；IMU中图分类号：TH89文献标志码：A文章编号：1672-9870（2023）02-0120-08Pose Estimation Based on Fuzzy Kalman FilteringHUANG Wei，TIAN Ming，FENG Jinliang，WANG Fei（School of Opto-Electronic Engineering，Changchun University of Science and Technology，Changchun 130022）Abstract：In the field of human-computer control interaction，the original information of the arm posture is usually collectedby the inertial measurement unit（IMU）and the solution and fusion are carried out to achieve the purpose of obtaining thehuman arm movement posture to achieve the purpose of controlling the robotic arm.Based on the dynamic characteristics ofthe gyroscope and the static performance of the accelerometer and magnetometer，considering the introduction of Kalman fil-ter for attitude fusion solution，since the statistical characteristics of the noise covariance matrix of the measurement processin the Kalman filter model are difficult to obtain accurately，a Kalman filter attitude estimation method based on T-S fuzzycontrol is proposed to realize the real-time adjustment of the measurement covariance matrix.Based on ZYNQ7020，anexperimental platform was built to fuse and solve the attitude information of the arm，and the average error of the attitudeangle and mean squared error after the fusion solution were within 0.06 and 0.16，which were reduced by 92.52%and89.44%compared with Kalman filtering，which effectively improved the attitude angle solution accuracy.Key words：Kalman filtering；fuzzy control；FPGA；human-computer interaction；IMU手势交互作为一种自然人机交互方式，是基于多传感器设备捕捉肢体动作，处理后生成控制器可识别的信息，从而实现操作的一种交互方式。该技术集成了传感器技术、模式识别技术和电子信息技术，是现如今人机交互领域的重要基础性研究之一。随着新型体感交互设备的普及与最优估计理论的深入，如何快速准确地捕捉和处理人体手臂姿态信息完成机械臂的控制运动，从而达到实现主从手操作感知协同的目的，已成为自然人机交互领域的关键问题。伴随传感器设备的不断发展，捕捉人体手臂信息进行姿态融合解算从而控制机械臂的设备以及方法也越来越多。有基于图像设备捕捉手臂的姿态信息实现机械臂的运动控制，有基于惯性传感器获取手臂的姿态实现机械臂的运动控制。由于每个传感器都存在着一定的局限性，因此需要根据传感器的不同特性进行多传感器互补融合实现手臂运动的姿态估计。本文使用 MEMS 传感器对人体手臂姿态进行采集估计，由于 MEMS 传感器的精度低，噪声高，直接使用采集到的三轴角速度进行姿态估计，随着误差的累积，会导致姿态角的发散，因此采用加速度计、陀螺仪和磁力计进行传感器信息融合，可以有效地控制姿态估计误差。Wu等人1提出了基于互补滤波的姿态估计算法，该算法根据不同传感器的特性进行互补，计算量低，但是其滤波系数固定，根据经验判断，未能充分考虑到传感器的噪声因素，所以姿态估计的精度较低。Hadri、Wu 等人2-3在互补滤波的基础上，使用 PI 控制器对四元数进行更新，大大降低了陀螺仪的累积误差，但是动态响应较差。Filho、吴涛等人4-5提出采用卡尔曼滤波进行姿态估计，采用四元数作为状态量，加速度计作为观测量进行状态更新，该算法实时性较好，收敛较快，但是由于卡尔曼滤波模型中的系统过程协方差矩阵和量测过程噪声协方差矩阵的统计特性难以准确获取。为了获取到更加准确的量测过程噪声协方差矩阵，本文提出一种基于 T-S 模糊控制的卡尔曼滤波姿态估计方法，将加速度计数据的方差值作为 T-S 模糊控制的输入，得到方差系数，用于表征卡尔曼滤波器中量测噪声协方差的变化，实现量测协方差矩阵的实时调整，结合磁力计陀螺仪数据，得到最优四元数估计，达到姿态解算的目的。1姿态角原理坐标系是姿态解算的关键部分。选取不同的坐标系获取的姿态信息也各不相同，常见的坐标系为导航坐标系和载体坐标系。一般情况下称导航坐标系为n系，通常选择地理坐标系为导航坐标系。地理坐标系任取地球表面一点，以经纬线的切线方向为X、Y轴方向，以重力加速度方向为Z轴方向。在姿态解算中常以东北天、北东地为参考系。本文的导航坐标系选择东北天为参考系，记为OXnYnZn。载体坐标系称为b系，以运载体的质心为原点，将运载体自身结构作为坐标系。以飞机航行为例，以飞机质心为原点，飞机机头航向为X轴，水平向右为Y轴，竖直向上为Z轴，即为常见的右手直角坐标系，记为OXbYbZb。1.1姿态角表示方法常见的姿态解算表示方法有欧拉角法、四元数法、方向余弦矩阵法等6。其中，欧拉角法是一种比较直观容易理解的表达方式，用三个参数来表示物体运动姿态，并且在计算量方面要比四元数法和方向余弦矩阵法小得多，不存在正交化处理，但是欧拉角存在万向节锁的现象，不能全方位表示姿态测量，涉及大量三角函数运算，不利于硬件上的运算实施方向余弦法是采用九个参数表示，避免了欧拉角产生的奇异值问题，但是参数的增加也增加了计算成本，会减弱解算的实时性7。四元数法进行姿态解算，相比方向余弦法的计算量大大减少，而且还避免了欧拉角产生的问题，只需要求解四个微分方程，有效地提高了解算的实时性。四元数作为复数的一种扩展形式，表示方式如下：q=q0+q1ix+q2jy+q3kz（1）式中，i、j、k分别表示相对x，y，z轴的旋转角度，因此可以得到用四元数表示的旋转转换矩阵如下：黄伟，等：基于模糊卡尔曼滤波的姿态估计第2期121长春理工大学学报（自然科学版）2023年Tbn=q20+q21-q22-q232(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q20-q21+q22-q232(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q20-q21-q22+q23（2）四元数的微分方程q.=q，写成矩阵形式如下：q0.q1.q2.q3.=12 0-x-y-zx0z-yy-z0 xzy-x0 q0q1q2q3（3）式中，x、y、z为利用传感器进行数据融合获取的载体坐标系下的陀螺仪三轴角速度值。由于人体手臂在摆动运动的过程中对姿态解算的精度和实时性的要求比较高，通过陀螺仪三轴角速度信息，利用一阶龙格库塔法8对四元数进行实时更新，得到更新矩阵如下：q0q1q2q3t+1=q0q1q2q3t+12 0-x-y-zx0z-yy-z0 xzy-x0 q0q1q2q3t（4）对四元数进行单位化，更准确地表示姿态信息，公式如下：q=qq20+q21+q23+q23（5）1.2各表示法的相互转化欧拉角法、方向余弦法、四元数法都是表征姿态角的一种方式，在实际应用当中，需要将载体坐标系中的表示测量值转换到导航坐标系下进行姿态解算。对于欧拉角转化方向余弦法，从载体坐标系到导航坐标系的转换表示为三个轴向独立转动的乘积：Cbn=C()C()C()（6）其中，C()、C()、C()表示为：C()=()cos0-sin010sin0cos（7）C()=()1000cossin0-sincos（8）C()=()cos-sin0sincos0001（9）因此可以得到从载体坐标系转换到导航坐标系，旋转变换矩阵如下：Cb n=()cos-sin0sincos0001()1000cossin0-sincos()cos0-sin010sin0cos=(coscos-sinsinsin-cossinsincos+sincossin cossin-cossincoscossinsinsin-sincoscoscoscos（10）若已知坐标转换矩阵内的每个元素值，可以得到姿态角，表达式如下：=arctan()-c12c22=arcsin()c32=arctan()-c31c33（11）四元数与方向余弦法的转换如式（2）的旋转转换矩阵。由式（11）、式（2）得到欧拉角法与四元数法的相互转换关系，如下：q0q1q2q3=cos2cos2cos2+sin2sin2sin2cos2cos2sin2-in2sin2cos2cos2sin2cos2+sin2cos2sin2sin2cos2cos2-cos2sin2sin2（12）1222姿态融合算法2.1互补滤波互补滤波算法通过不同传感器的噪声特性进行互补，实现姿态的解算9。其核心是对不同的传感器进行不同的滤波方法，赋予一定的权重，最后求和得到最终信号。将加速度计数据进行低通滤波，陀螺仪数据经高通滤波后，相加求和得到融合姿态角，互补滤波示意图如图 1 所示。图 1互补滤