...5芯片为FIR滤波器的硬件支持,应用Xilinx ISE 9.2i软件分析存储器个...
朱科风 周庆华 王广鹏 王乐 林思宇
文章编号： 2095-2163（2018）03-0127-05中图分类号： 文献标志码： A
摘要： 关键词： （School of Physics & Electronic Science， Changsha University of Science & Technology， Changsha 410114， China）
Abstract： The traditional complementary filter algorithm which realizes the data fusion of multi sensors and compensates the integration error of the gyroscope has the advantage of easier calculation. But it also has some defects， such as large data fluctuation and calculating error. In this paper， the differentiation element is introduced to solve the problem of data fluctuation. An improved algorithm is proposed with PID controller to weaken data fluctuation. The experimental results show that the algorithm in this paper has the advantage of less computation and small data fluctuation. It can achieve stable state very quickly and track quadcopter effectively.
Key words：
基金项目：
作者简介：
通讯作者：
收稿日期： 引言
科技的不断发展使得中小型四轴飞行器（以下简称四轴）的续航能力越来越强，四轴的垂直起落、空中悬停、灵活转向等特点也使得其在民用和军事领域都得到了广泛的应用[1]。而四轴的姿态获取是能稳定控制四轴飞行的关键。
四轴的姿态获取指的是获取四轴相对于面的角位置，获取方式主要依赖于惯性测量传感器，其中包括陀螺仪、加速度计以及磁力计。但是能搭载在四轴上的传感器往往精度不高，主要表现在陀螺仪的动态特性会因积分运算而产生累积误差，且误差随时间增大；加速度计与磁力计的静态特性容易受到噪声影响[2]。在硬件条件受限的情况下，对姿态解算算法的研究成为了一个热点，例如：由Madgwick提出的梯度下降法解决了数据融合，避免了地磁补偿前对当地地磁场角度的测量，但是在高速运动情况下偏航角响应存在延迟，会导致姿态解算出错[3]。Mahony互补滤波算法通过加速度计与磁力计测量的姿态数据来补偿陀螺仪的积分误差[4]，运算量小，但精度不高。本文采用经PID控制器改进的互补滤波算法，使姿态解算响应更快，稳定性更强，更有利于四轴的控制。
1姿态描述
为了对四轴的姿态进行描述，需要建立相应的2个不同的坐标系。飞行器的姿态是参考地面来描述的，需要以当地地理位置为参照，以东、北、天方位为X、Y、Z轴建立导航坐标系N[5]。陀螺仪、加速度计等传感器是搭载机体上的，所测得的数据是以机体为参考，以前、右、上方位为x、y、z轴的载体坐标系b来描述。2个坐标系间的变换关系则如图1所示。
图1导航坐标系N与载体坐标系b的变换关系
Fig. 1Transformation relation between coordinate system N and coordinate system b
姿态的数学表示方式通常采用的是欧拉角和四元数[6]。载体相对于导航坐标系的角位置关系可以通过导航坐标系绕坐标轴连续旋转3次来确定：绕导航坐标系Z轴旋转φ角，再绕新坐标系的Y轴旋转θ角，再绕新坐标系的X轴旋转角，实现变换。φ、θ、分別为载体的偏航角、俯仰角、翻滚角，用欧拉角表示的由N系变换到b系的变换矩阵为CbN，数学表述将分别如式（1）、式（2）所示。
CbN=cos φcos θsin φcos θ-sin θ
sin sin θcos φ-sin φcos sin sin θsin φ+cos φcos sin cos θ
sin sin φ+sin θcos φcos sin θsin φcos -sin cos φcos θcos （1）X
Y
Z=CbNx
y
z（2）以欧拉角来表示变换矩阵直观易懂，但是欧拉角在求取姿态时存在奇点，不能用于全姿态解算[7]。为了解决求取姿态时的奇点问题，本文采用四元数法计算求取姿态。四元数法具有计算量小，解算无奇点等优点，能够满足四轴全姿态解算的需求。
四元数由4个元构成，定义q为一个常规四元数向量，数学公式具体如下：q=q0+q1i+q2j+q3k（3）其中，q0、q1、q2、q3为实数，i、j、k为两两正交的向量。当仅是关注N系与b系的角位置关系时，两坐标系之间的变换可以等效为无中间过程的一次等效旋转，这个旋转可以由四元数向量给出表示[8]。矢量v=q1i+q2j+q3k包含了旋转轴和旋转方向的信息，标量u=q0包含了旋转角度的信息。根据四元数的约束条件，只有单位四元数才能表示旋转，将四元数进行单位化，研究推得变换公式如下：q=qq20+q21+q22+q23（4）由N系变换到的b系的变换矩阵用四元数表示为RbN，其计算公式可见如下：
RbN=
1-2q22+q232q1q2+q0q32q1q3-q0q2
2q1q2-q0q31-2q21+q232q2q3+q0q1
2q1q3+q0q22q2q3-q0q11-2q21+q22（5）
由RbN=CbN得出欧拉角与四元数关系，即有如下数学公式：θ=sin-1q0q2-q1q3
=tan-12q2q3+q0q11-2q21+q22
φ=tan-12q1q2+q0q31-2q22+q23（6）2改进型互补滤波算法
2.1传统的互补滤波算法原理
四轴通过陀螺仪、加速度计以及磁力计获得原始的姿态数据。陀螺仪的动态特性好，短时间内测量的数据精度较高，但是由于零漂、温漂的影响随着时间的积累会形成较大的误差；而加速度计与磁力计静态特性好，但动态特性差，容易受高频噪声影响。即在频域上陀螺仪高频特性好，低频特性差；加速度计与磁力计低频特性好，高频特性差。通过互补的高通、低通滤波器融合3个传感器的数据，可以实现较为准确的姿态角解算。
根据传感器的频域特性，将陀螺仪的监测数据通过高通滤波器的设计处理，同时将加速度计与磁力计的数据通过低通滤波器的综合处理，2部分数据相加即得到最终姿态。互补滤波原理可如图2所示。
假设陀螺仪测量运算后的数据为θg=θ+μL，加速度计与磁力计的测量数据为θma=θ+μH。其中，θ为无误差姿态数据，μL为低频噪声，μH为高频噪声。选取一组传递函数H1s=ss+Cs、H2s=Css+Cs满足H1s+H2s=1。并且，H1s为高通滤波器传递函数，H2s为低通滤波器传递函数，Cs的选取满足θs对互补滤波器全通[9]。滤波器的估计量频域响应为：
θ^˙s=θgsH1s+θmasH2s=
θs+H1sμLs+H2sμHs（7）
在实现数据互补的同时为了消除静态误差，选取Cs=Kp+Kis，算法通过PI控制来重点调整滤波器的截止频率与消除静态误差的时间。通过将加速度计与磁力计测得的姿态角θma与滤波器预估的姿态角求差，将差值经过PI调节后补偿陀螺仪的数据得到最终的姿态角，构成一个如图2所示的动态的闭环系统，互补滤波器的估计量即如式（8）所示：
θ^˙=θg+Kpθma-θ^+Ki∫θma-θ^dt（8）
其中，θ^˙为互补滤波器估计的输出量；θ^为反馈量；dt为采样时间。
2.2PID改进型互补滤波算法
互补滤波算法可以达到传感器数据的充分融合，通过运用PI控制器，有效地解决了互补滤波器的低通阻带衰减较慢，受噪声影响大的问题[10- 11]。PI控制器可通过选取合适的Kp、Ki来调整滤波器的截止频率和消除静态误差的时间[12]，但是周期性的误差调整使得姿态数据不断地振动，导致解算的姿态角并不稳定。四轴应对突变的恢复能力较弱，不能迅捷恢复平衡。
为此，针对姿态数据的振动问题，本文设计引入微分环节D，构成PID控制器。PID控制器可以通过微分环节反映误差信号的变化趋势，再据此展开超前调节，抑制数据的振动，并且增强系统的快速性。因此使用PID控制器可以抵消周期调整误差所产生的振动，还可以加快互补滤波算法的响应速度。改进算法的流程如图3所示。
由图3可知，ab、mb分別为加速度计与磁力计的测量值，ab=axayazT、mb=mxmymzT，归一化后得ab^、mb^。vb^表示b系重力加速度，wb^表示b系的地磁强度，分别由N系的重力加速度001T与地磁强度bx0bzT经过旋转矩阵变换得到，旋转变换矩阵则为式（5）的RbN。改进算法通过计算加速度计、磁力计的测量数据与重力加速度、地磁强度的误差e来补偿陀螺仪的数据，补偿后的陀螺仪数据通过一阶龙哥库塔方程对四元数进行更新。各个参数的关系推导可表示如下：e=ab^×vb^+mb^×wb^（9）
经过更新的四元数通过反馈环节实现对重力加速度与地磁强度的变换，再与传感器数据经过叉乘运算求得误差e，因此误差e具有上一周期的陀螺仪数据信息。实验中，四轴姿态信号采样周期为0.01 s，陀螺仪数据在短时间内变化较小，上一周期的陀螺仪数据对下一个周期的陀螺仪数据具有参考意义。因此经过上一周期四元数变换后求得的误差e可以作为下一周期陀螺仪数据的预调量，构成PID控制器的微分部分D。PID控制器的参数Kp、Ki、Kd需要经过反复实验，选取合适的值以满足较为稳定、准确的姿态解算。
3实验验证及结果分析
本文是基于Arduino101开发板构建的四轴硬件平台，Arduino101开发板搭载有BMI160六轴惯性测量单元，包括三轴加速度计与三轴陀螺仪，磁力计采用的是MAG3110。测试数据通过串口传输给PC机，在PC机上完成数据分析，采样频率为100 Hz。
陀螺仪与加速度计的原始数据，则如图4所示。图4是在小幅连续振动的情况下在陀螺仪与加速度计上测得的原始数据。由图4可见，在前3 s连续振动的情况下，陀螺仪数据能实时反映四轴平台的运动状态，动态数据较为敏感，可以实时跟踪四轴平台的姿态变化。而加速度计数据不能切实反映四轴平台的运动状态，动态反应失真。四轴平台回稳后，陀螺仪数据仍有波动，静态条件下不稳定，有较为明显的误差。而加速度计数据静态数据非常准确，变化较小。
将本文的四轴测试平台经过多次实验测试后可知，改进算法的PID参数设为Kp=2.0，Ki=0.005，Kd=0.05时，能获得较为理想的姿态解算结果。将该算法与传统的互补滤波算法、梯度下降算法对四轴的俯仰角（Pitch）与翻滚角（Roll）分别给出观测的效果对比。
运算得到3种算法的解算的姿态角结果，即如图5所示。
图5是将测试平台的俯仰角与翻滚角各自在约10°、3°的位置保持几秒后恢复到水平静止状态所测得的数据。分析后发现：梯度下降法数据波动较大，所需回稳时间较长；互补滤波算法与改进型算法波动幅度较小，回稳速度快，能够更快地趋于稳定。
PID改进型算法是在互补滤波算法的基础上改进得来。为了验证改进算法在抑制振动上的优势，分别在俯仰方向与翻滚方向作连续振动测试，并对2种算法的运算结果进行数据采集，在PC上位机调取相应数据分析，可得结果如图6所示。
图6是分别截取2种姿态角连续10 s的数据。可以看出，改进型互补滤波算法运行解算的姿态数据波动更小，所需趋于平稳的时间更短，而且在回稳后数据较为稳定。相比于互补滤波算法，改进型算法的稳定特性使得四轴更容易控制。
为了测试3种算法的静态状况下的解算准确性，对3种算法在静止水平（俯仰角、翻滚角均为零度）状态下的姿态角各取50组求出平均值，最终得到测量的平均误差可见表1。从表1可以得出改进型互补滤波算法在俯仰角上平均误差相对于梯度下降法和互补滤波算法分别降低了45.4%、52.4%；在翻滚角上，平均误差分别下降了26.3%、62.2%。
结束语
针对互补滤波算法存在的数据波动大等问题，本文提出了四元数PID改进型互补滤波算法。该算法通过四元数描述四轴姿态，避免了欧拉角的奇点问题，实现了全姿态解算，在此基础上引入了PID控制器调节姿态误差，使得解算的姿态角更加稳定。后续又通过仿真实验对比展示了互补滤波算法、梯度下降算法与改进型算法的效果及性能。实验结果表明，运动状态下改进型算法解算出的俯仰角与翻滚角波动较小，且所需稳定时间更短；在水平静止的状态下，改进型算法的解算误差最小。由此可以得知，改进型算法有利于四轴的控制，能使四轴的飞行更加平稳。
参考文献
[1] 万晓凤， 康利平， 余运俊， 等. 互补滤波算法在四旋翼飞行器姿态解算中的应用[J]. 测控技术， 2015，34（2）：8-11.
[2] 陈亮， 杨柳庆， 肖前贵. 基于梯度下降法和互补滤波的航向姿态参考系统[J]. 电子设计工程， 2016，24（24）：38-41，45.
[3] MADGWICK S. An efficient orientation filter for inertial and inertial/magnetic sensor arrays[R]. UK：University of Bristol，2010.
[4] BALDWIN G， MAHONY R， TRUMPF J， et al. Complementary filter design on the Special Euclidean group SE（3）[C]// Proceedings of the European Control Conference. Kos， Greece： IEEE，2007：3763-3770.
[5] 季元扬， 陈跃东， 陈孟元. 基于PID改进型互补滤波[J]. 控制工程， 2017，24（5）：952-957.
[6] 徐云川. 四轴飞行器姿态解算算法设计与仿真[J]. 科技视界， 2016（23）：17-18.
[7] 陈孟元， 谢义建， 陈跃东. 基于四元数改进型互补滤波的MEMS姿态解算[J]. 电子测量与仪器学报， 2015，29（9）：1391-1397.
[8] 秦永元. 惯性导航[M]. 2版. 北京：科学出版社有限责任公司，2014.
[9] MAHONY R， HAMEL T， PFLIMLIN J. Nonlinear complementary filters on the special orthogonal group[J]. IEEE Transactions on Automatic Control， 2008，53（5）：1203-1218.
[10]吕印新， 肖前贵， 胡寿松. 基于四元数互补滤波的无人机姿态解算[J]. 燕山大学学报， 2014，38（2）：175-180.
[11]杜杉杉， 吴昊， 张继文， 等. 一种面向AHRS的改进互补滤波融合算法[J]. 国外电子测量技术， 2015，34（3）：13-18.
[12]陈雷， 卞鸿巍. 基于模糊自适应互补滤波的姿态解算算法[J]. 兵器装备工程学报， 2017，38（4）：183-187.（上接第126页）
[4] Yang Xinshe. Flower pollination algorithm for global optimization[C]//LNCS 7445： Proceedings of the 11th Internation Conference on Unconventional Computation and Natural Computation， Orléan， France， Sep 3-7， 2012. Berlin， Heidelberg： Springer， 2012： 240-249.
[5] Cheng S， Zhang Q， Qin Q. Big data analytics with swarm intelligence[J]. Industrial Management & Data Systems， 2016， 116（4）：646-666.
[6] 馮翔，张进文，虞慧群. 仿生蚊子追踪算法[J]. 计算机学报，2014，37（8）：1794-1808.
[7] 乔莹莹，宋威，马伟. 基于GA优化QPSO算法的文本聚类[J]. 计算机应用研究，2014，31（10）：2912-2915.
[8] Yang X S. A New Meta-heuristic Bat-Inspired Algorithm[J]. Computer Knowledge & Technology， 2010， 284：65-74.
[9] Wang G G， Chu H C E， Mirjalili S. Three-dimensional path planning for UCAV using an improved bat algorithm[J]. Aerospace Science & Technology， 2016， 49：231-238.
[10]Karri C， Jena U. Fast vector quantization using a Bat algorithm for image compression[J]. Engineering Science & Technology An International Journal， 2016， 19（2）：769-781.
[11]姚妮，李红婵. 基于混合蝙蝠算法的多目标柔性作业车间调度问题[J]. 微电子学与计算机，2017，34（3）：25-29，34.
[12]肖辉辉，段艳明. 基于DE算法改进的蝙蝠算法的研究及应用[J]. 计算机仿真，2014，31（1）：272-277.
[13]He X S， Ding W J， Yang X S. Bat algorithm based on simulated annealing and Gaussian perturbations[J]. Neural Computing & Applications， 2014， 25（2）：459-468.
[14]裴宇航，刘景森，李煜. 一种动态调整惯性权重的自适应蝙蝠算法[J]. 计算机科学，2017，44（6）：240-244.
[15]Mirjalili S. SCA： A Sine Cosine Algorithm for solving optimization problems[J]. Knowledge-Based Systems， 2016， 96：120-133.
[16]王文， 王勇， 王晓伟. 采用机动飞行的蝙蝠算法[J]. 计算机应用研究， 2014， 31（10）：2962-2964.
