王宏昊， 陈明， 张坤

　　（哈尔滨理工大学 测控技术与通信工程学院， 黑龙江 哈尔滨 150080）

　　摘要：设计了一款基于嵌入式处理器STM32的四旋翼飞行器，利用低成本传感器测量加速度和机体的角速率，提出一种结合算法，从而实现飞行器姿态的精确测量。采用四元数法描述飞行器的姿态，在测量过程中结合互补滤波算法进行测量数据的矫正；同时针对出现的随机噪声干扰，采用卡尔曼滤波算法，实现姿态的准确测量，最终在实际的飞行器平台上得到了验证。

　　关键词：四旋翼；互补滤波；卡尔曼滤波；姿态控制

0引言

　　近年来，无人飞行器逐渐成为人们研究的热点。无人飞行器的种类繁多，其中四旋翼无人飞行器是一个重要的研究方向［1］。与其他无人飞行器相比，四旋翼飞行器可以实现垂直起降、定点悬停，具有体积小、机动性能好等优势，尤其适用于灾害搜救、航空拍摄和特殊环境的巡视侦查等方面［2］。随着无人飞行器在民用领域的发展，四旋翼飞行器更以其成本低廉的优点得到了广泛的应用。

　　飞行器的位姿估计是实现UAV自主能力飞行的基础。由于微机电系统（Micro Electro Mechanical System，MEMS）具有成本低廉、体积小、功耗低的优点，MEMS被广泛应用于无人机的惯性导航领域。然而，低成本的传感器具有严重的时变漂移，因此需要采用有效的算法实时估计传感器的漂移，抑制姿态误差的累计。至今，国内外已经有很多学者在这方面做了很多工作，参考文献［3］使用卡尔曼滤波算法对加速度传感器数据和陀螺仪数据进行融合，实时地改变测量噪声协方差的值，避免了动态噪声对加速度传感器的影响，提高了飞行器姿态测量的精度；参考文献［4］提出了基于四元数的姿态估计方法，并采用了低功耗的传感器。本文设计了一款低成本的四旋翼飞行器，结合四元数与互补滤波与卡尔曼滤波算法，实现了对飞行器位姿的有效与精确估计，并在实际中得到了验证。

1硬件设计

　　为了设计体积小、重量轻、功耗低的姿态航向参考系统，本文选择集成度高的嵌入式处理器STM32F103作为导航核心处理器，实现姿态和航向解算以及输出。使用集成加速度计和陀螺仪的MPU6050测量加速度和机体的角速率。加速度计比较敏感、变化速度快,在低动态下可以解算得到比较准确的姿态角；而在高速状态下，加速度计在飞行过程中釆集到的数据带有大量的噪声(主要是由震动产生的)，解算传来的姿态角也就有了较大的误差。在加速度较大的情况下，加速度计解算的姿态角变得不可靠，需要使用陀螺仪测量的角速率积分来修正姿态陀螺仪传感器输出变化缓慢,但是时间长了有较大的累积误差,而且还有温度漂移。利用陀螺仪进行物体姿态检测需要考虑到累计误差的消除［5］。因此在四轴飞行器的飞行姿态控制系统中,必须将陀螺仪和加速度计的数据通过数据滤波算法进行融合和滤除噪声干扰,以此来得到正确的姿态数据。

2飞行器姿态估计算法

　　2.1四元数法

　　四旋翼无人飞行器姿态解算是将飞行器上惯性单元的输出实时转换成飞行器的姿态，即飞行器的机体坐标系(xB,yB,zB)相对于导航坐标系(XE,YE,ZE)的角位置。

　　欧拉角是飞行器的3个姿态角，即俯仰角（pitch）、横滚角（roll）、偏航角（yaw）。根据欧拉旋转定律，可用3次旋转使得机体坐标系与导航坐标系重合，每一次旋转都是以导航标系的x、y、z轴中的一个坐标轴来转动，转过的角就是欧拉角，每次旋转后坐标关系可由一旋转矩阵来表示，即方向余弦矩阵：

　　式中φ、、θ分别代表偏航角、横滚角、俯仰角。为避免欧拉角在表示姿态时可能出现的奇异问题，四元数在飞行器的姿态表示方面得到了广泛的应用。设描述四旋翼飞行器姿态的四元数为：

　　导航坐标系与机体坐标系之间的坐标关系可用方向余弦矩阵表示，其四元数形式为：

　　导航坐标系到机体坐标系的旋转过程中坐标系始终保持直角坐标系，所以CBE为正交矩阵，即CBE=(CBE)－1=(CBE)T。可得飞行器的姿态角为：

　　将四元数代入可得到:

　　四元数的微分方程为=12Ωbnbq，即:

　　式中wx、wy、wz为机体坐标系下的角速度。在已知初始四元数的情况下，通过三轴陀螺仪测量的3个轴的角速度就可以实时更新四元数的值，进而更新姿态角获得姿态信息［6］。

　　2.2互补滤波补偿算法

　　陀螺仪存在积分误差，所以解算出来的姿态角也会出现偏差。为了解决这一问题，引入互补滤波算法，利用加速度计来修正陀螺仪的误差［7］。设加速度计测出来的重力向量为ax、ay、az，陀螺仪积分后的姿态推算出来的重力向量为vx、vy、vz，则有:

　　vx=2(q1q3+q2q4)

　　vy=2(q2q3－q0q1)

　　vz=(q20－q21－q22+q23)（7）

　　从而陀螺仪积分后的姿态结合加速度计数据得到的姿态误差为:

　　ex=(ayvz－azvy)

　　ey=(azvx－axvz)

　　ez=(axvy－azvx)（8）

　　此姿态误差与陀螺仪积分误差成正比，使用互补滤波算法来修正陀螺仪角速度积分误差：

　　xint=exint+ex*ki

　　yint=eyint+ey*ki

　　zint=ezint+ez*ki （9）

　　其中xint、yint、zint是对陀螺仪的纠正量的积分项，ki为积分系数。

　　x=ωx+kpex+exint

　　y=ωy+kpey+eyint

　　z=ωz+kpez+ezint （10）

　　其中x、y、z为陀螺仪修正后的输出角速度，kp为比例系数。

　　2.3基于姿态角的卡尔曼滤波算法

　　在实际测试中，上位机接收到姿态角的数据仍然存在少许的噪声干扰，为了滤除噪声的干扰，引入卡尔曼滤波算法［8］。设姿态角为α，对其离散化并构造状态方程和观测方程：

　　αk=Aαk－1+Wk－1

　　Zk=Hkαk+Vk（11）

　　算法步骤如下。

　　（1）状态一步预测。通过第k－1时刻的α值，预测第k时刻的α值：

　　αk|k－1=αk

　　（2）一步预测均方误差。根据第k－1时刻的系统误差估计第k时刻的系统预测误差Pk k-1 ：

　　Pk|k－1=Pk－1+Q

　　（3）滤波增益计算：

　　Kk=Pk|k－1/(Pk|k－1+R)

　　（4）状态更新，计算系统最优估算值：

　　αk=αk－1+K(Zk－αk－1)

　　（5）滤波均方误差更新。在卡尔曼滤波运算中，要实现对均方误差的更新，以便下一刻计算卡尔曼增益，更新方程如下：

　　Pk=(1－Kk)Pk|k－1

3软件设计思路

　　在STM32和MPU6050等传感器上电后，程序首先进行系统的初始化，在初始化中配置MCU以及传感器，设定卡尔曼滤波器参数，然后获取传感器数据，传感器的数据经过换算得到陀螺仪和加速度计的测量值。利用加速度的测量值修正陀螺仪的输出，再通过修正后的角速度更新四元数，进而解算得到姿态角。由于仍然有噪声的干扰，因此采用卡尔曼滤波算法对姿态角进行滤波，从而得到正确的姿态角［9］。具体软件流程如图1所示。

4实验结果与分析

　　在现有的四旋翼飞行器平台上，运用上述算法进行实验。初始时刻，将四旋翼置于水平，然后改变四旋翼的航向，获取俯仰角、滚转角、偏航角的数据，通过蓝牙传送到上位机，得到3个姿态角的输出，如图2~图4所示。

　　其中，a、b、c分别表示加速度计解算出来的姿态角输出、四元数法解算出来的姿态角输出和卡尔曼滤波后的姿态角输出。

5结论

　　本文设计了由低成本传感器组成的四旋翼飞行器，针对传感器测量数据误差的复杂性与累积性，通过使用四元数法描述姿态，避免了用欧拉角描述姿态的奇异值问题，同时引入互补滤波与卡尔曼滤波算法，从而更加精确地获得飞行器的实时精确姿态信息，并在实际的观测中得到了验证，结果十分理想，为有效地控制飞行器平台姿态并最终实现无人飞行打下了良好的基础。

参考文献

［1］ KUMAR V, MICHAEL N. Opportunities and challenges with autonomous micro aerial vehicles［J］． The International Journal of Robotics Research, 2012, 31(11) : 12791291．

［2］ BOUABDALLAH S. Design and control of quadrotors with application to autonomous flyin［D］. Lausanne: Ecole Polytechnique Federale De Lausanne, 2007.

［3］ Wang Shaohua, Yang Ying. Quadrotor aircraft attitude estimation and control based on Kalman filter［C］. Proceedings of the 31st Chinese Control Conference, 2012,30(9)：56345639.

［4］ Chao Haiyang, COMMPMANS C, Di Long, et al. A comparative evaluation of lowcost IMUs for unmanned autonomous systems［C］. IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, 2010:211216.

［5］ LAM  Q M, STAMATAKOS N, WOODRUFF C, et al. Gyro modeling and estimation of its random noise sources［C］. AIAA Guidance, Navigation, and Control Conference and Exhibit, Austin, Texas, 2003.

［6］ KRAJNIK T, VONASEK V, FISER D, et al. Ardrone as a platform for robotic research and education［A］. OBDRZALEK D， GOTTSCHEBER A. Communications in Computer and Information Science［C］. Springer, 2011, 161： 172186.

［7］ BRISTEAU P J, CALLOU F, VISSIERE D, et al. The navigation and control technology inside the ar.drone micro uav［C］. Proceedings of the 18th IFAC World Congress, Milan, Italy, AugustSeptember 2011, 2011，18：14771484.

［8］ Wu Xingming, Duan Li, Chen Weihai. A Kalman filter approach based on random drift data of fiber optic gyro［C］. 2011 6th IEEE Conference on Industrial Electronics and Applications (ICIEA), 2011:19331937.

［9］ 刘晓杰，赵晓辉，顾海军. 微小型四旋翼无人机实时嵌入式控制系统设计与实现［J］.电子技术应用，2009，35（5）：3637.

［10］ 杨萌，雷建和，胡廷轩，等. 基于STM32的四旋翼飞行器控制系统设计［J］.微型机与应用，2015,34（12）：7679.