基于数据融合和串级PID的小型四旋翼无人机高度测量与控制系统

作者：Tomy

摘要：简单介绍了一种基于 Mahony 算法无人机姿态解算算法，并在这个的基础上设计了一
种融合惯导加速度计和气压计的小型无人机高度测量系统， 同时利用该高度测量系统设计了
基于串级 PID 的小型无人机高度控制器。经过实验验证表明，该高度测量系统能够较为准
确的反映无人机的高度数据和高度变化， 同时该高度控制器也能较好的控制无人机高度进行
升降和悬停。


关键词：四旋翼无人机；惯性导航；互补融合；气压计；PID 控制器


1 引言
小型四旋翼无人机是一种结构简单，飞行灵活的无人机系统[1]。其相对于普通固定翼无人机的优势之一在于可以完成悬停动作， 而悬停动作需要依靠可靠的高度测量， 通过相应的控制算法实现。


目前，国内外的无人机高度测量主要通过机载的气压传感器来实现。一般的气压传感器受测量环境的影响，容易产生较大的测量误差。同时受传感器的限制，在数据转换过程中存在不同程度延迟， 对高度的变化的动态响应不够迅速。 飞行器上搭载的惯性测量单元 （IMU） ，能够测量在机体坐标下三个轴的加速度运动情况， 通过姿态矩阵的转换， 能够变换到大地坐标轴下。通过对加速度的积分，得到速度，再次积分得到位移，能够准确的反应无人机系统的运动变化情况。但是在积分过程中不可避免的会累积误差，这样在长时间之后，测量数据就会变得不够可靠[2]。


2 基于数据融合的高度测量


2.1 飞行器姿态坐标变换
定义     为飞行器在机体坐标下的姿态，    为飞行器在大地坐标下的姿态。根据捷联惯性导航， 绕大地坐标三次旋转可以得到大地坐标到机体坐标的变换， 分别用方向余弦矩阵和 四元数[3]表示：
 

反之为绕机体坐标三次旋转可以得到机体坐标到大地坐标的变换    。在进行坐 标变换之前，需要求解出飞行器的姿态。本文设计了基于 Mahony[4]滤波器的飞行器姿态估 计算法，其主要流程为：

1、将重力矩阵    转移到机体机体坐标上，    ；
2、重力与加速度矩阵叉乘得到误差，    ；
3、对误差进行 PI 计算，    ；
4、校正陀螺仪，    ；
5、一阶龙格库塔法更新四元数    ；
6、重复迭代。


完成姿态的计算之后，姿态矩阵被不断的更新，这样就可以用其将机体坐标系下得到的加速度变换到大地坐标下，从而得到对地方向上的加速度：
 

式中：
  ，大地坐标下对地加速度；

 ，机体坐标下加速度测量矩阵。

2.2 气压传感器与加速度计互补融合


根据气压传感器和加速度计的不同特点[5]，本文设计了一种两级互补融合算法，其主要流程如图 1 所示：
 


首先利用气压计微分量与 2.1 节所述求得的对地加速度  进行一阶互补融合得到一个对地速度的估计值 ，然后利用该对地速度的估计值  与气压计原始数据进行一阶互补融合得到最终的高度估计值  。一阶互补融合根据如下式子计算：
 


式中：
 ，当前时刻融合得到的值；
 ，上一时刻融合得到的值；
 ，当前时刻原始数据，本文中分别对应气压计微分值和气压计原始值；
 ，变化量，本文中分别对应加速度值和对地速度估计值；
 ，计算权重；
 ，计算时间间隔；


在实际计算中可以根据加速度计和气压计的可信赖程度调整计算权重，从而使估计值更快的收敛。通过两次互补融合，可以得到一个比较可靠的对地速度的估计值  ，高度估计值  。

3 基于串级PID的无人机高度控制器


根据 2.2 节得到的对地速度的估计值  ，高度估计值  ，可以设计出基于串级 PID 的高度控制器。
PID 控制算法如下：
 

在上式中，
 为比例系数，  为微分系数，  为积分系数，dt 为时间间隔， e(t) 为误差。


根据系统的特点我们设计出串级 PID 控制器。串级 PID 相对于单级 PID 来说，能够使得控制任务细化，使控制系统更趋于稳定[5]。


在 PID 计算中分为内环和外环，其中外环为高度环反馈控制，利用高度估计值  和目标高度值  作为控制输入。内环为速度环反馈控制，利用对地速度的估计值  和外环高度环的输出作为控制输入。整个控制系统流程如图 2 所示：

4 算法验证与实验结果 4.1四旋翼无人机硬件 实验平台采用自行研制的四旋翼无人飞行器，平台实物如图 3、4 所示，其组成结构如图5 所示： 该飞行平台包括了飞行控制系统、飞行地面站系统、图像采集系统等。其中飞行控制系统实时通过IIC数据总线读取陀螺仪\加速度计（MPU6050），气压传感器（MS5611），电子罗盘（HMC5883L）中的数据，通过 Mahony 算法解算得到飞行器的飞行姿态，然后融合加速度计和气压计的数据， 得到飞行器高度数据。 主控芯片通过这些数据实时控制飞行器的高度和姿态，并且将计算得到的 PWM 信号传递给电子调速器（ESC），实现对电机转速的控制。无人机实时的将飞行器上的各种数据回转到地面站上，以便于实时监控和后期分析。整个飞行器姿态解算的频率为 500hz，电子调速器控制电机转速的频率为 400hz，高度计算和高度控制的频率为 40hz。       4.2高度测量实验结果 高度测量实验分为两部分：1、静态高度测量；2、动态高度测量。静态高度测量是通过外力将测量装置放置不同高度，并观测高度测量的结果。其测量结果如图 6 所示：   结果分析：高度估计值相对于气压计原始数据曲线更平滑，而且速度估计值基本能反映高度的变化过程。 由于一阶互补滤波本质上是一个低通滤波器和高通滤波器的组合， 所以作用在气压计原始数据上的低通滤波使得互补得到的数据存在 1s 左右的延迟。 动态高度测试是通过外力使测量装置作峰-峰值为 0.5m 的近似正弦运动，并观测高度测量的结果。其测量结果如图 7 所示：   结果分析：由于实际运动为峰-峰值为 0.5m 的近似正弦运动，所以融合得到的高度和速度基本能反映运动过程。而气压计原始数据虽然也能反映高度的变化过程，但是在峰-峰值上发散太过于严重，与实际不相符。 4.3高度控制实验结果 高度控制实验是在真实飞行状况下，测量起飞到悬停的整个过程高度变化，并与目标高度值作对比。高度控制结果如图 8 所示：   结果分析：无人飞行器能够根据目标高度进行动态跟踪，误差保持在±0.5m 以内。 5 结语 本文采用基于加速度计与气压计融合的无人机高度测量系统能够精确测量无人机的高度，并且能够快速响应无人机的高度变化。同时本文采用串级 PID 控制器对无人机的高度进行控制，能够达到良好的控制效果。由于无人机高度受姿态和环境因素变化较大，所以下一步还将针对干扰及飞行器运动状态下的高度测量与控制进行优化。 参考文献 [1] Mahony R,Kumar V,Corke P.Multirotor Aerial Vehicles Modeling, Estimation and Control ofQuadrotor[J].IEEE Robotics& Automation Magazine,2012,19(3): 20-32 [2]谢勇,陈照海,陈万法.某无人直升机高度测量系统融合方法[J].兵工自动化，2010,29（2）：85-90 [3]邓志红,付梦印,张继伟等.惯性器件与惯性导航[M].科学出版社,2012 [4]Hamel T,Mahony R.Attitude estimation on SO(3)based on direct inertial measurements[C].Proceedings of 2006 IEEE International Conference on Robotics and Automation. Florida,Orlando,2006:2170-2175 [5]李洪辉,裴海龙.无人直升机高度测量的设计与实现[J].自动化与仪表,2012,7 [6]谭广超.四旋翼飞行器姿态控制系统的设计与实现[D].大连理工大学硕士学位论文，2013