为飞行器在机体坐标下的姿态，
为飞行器在大地坐标下的姿态。根据捷联惯性导航， 绕大地坐标三次旋转可以得到大地坐标到机体坐标的变换， 分别用方向余弦矩阵和
四元数[3]表示：
。在进行坐
标变换之前，需要求解出飞行器的姿态。本文设计了基于 Mahony[4]滤波器的飞行器姿态估
计算法，其主要流程为：
转移到机体机体坐标上，
；
；
；
；
；
，大地坐标下对地加速度；
，机体坐标下加速度测量矩阵。
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 |
