宋锐++刘宏立++李和平

摘 要：文章介绍了MEMS姿态传感器在控制飞行器中的应用研究。首先介绍了无人控制飞行器系统，以STM32F407VGT6（ARM Cortex-M4）处理器为控制核心，利用MEMS姿态传感器为姿态检测模块，通过PC端应用软件系统规划行驶路径，同时辅以4G无线传输GPS地理位置定位、实现飞行器的地理位置反馈和环境监控，通过对比仿真对照ANP控制器和PID控制器的试验结果，试验表明：ANP控制器比PID控制器在飞行器的应用中使得系统性能更稳定，精度更高。

关键词：MEMS姿态传感器 ANP控制 PID控制器 无人驾驶飞行器

中图分类号：TP334.3 文献标识码：A 文章编号：1674-098X（2016）08（c）-0008-03

开发无人驾驶的微型飞行器，对危险领域进行探测和搜救即可减少伤害的发生。在微型飞行器的研究方面，目前欧美等发达国家远远走在全球的前列，且成功研究了固定翼式、旋翼式和扑翼式各类微型飞行器，我国高校和研究所虽然也开始展开了相关研究，但在救灾减灾方面研究极少。监控室的监控人员事先规划路线通过4G无线传输对飞行器实现实时路径导航和无线控制，当飞行器到达灾后现场上空时，通过所携带的摄像头和环境传感器采集将图像资料和环境参数无线传输给监控室，给现场决策提供帮助。

1 硬件设计

1.1 系统结构

系统硬件主要由姿态解算MEMS器件AHRS（Attitude and Heading Reference System）、控制系统（ARM Cortex-M4处理器、环境监控系统、GPS导航系统、智能相机系统及4G传输系统）构成。

主控制芯片选用STM32F407VGT6，采用ARM32-bitCortex?-M4内核，通过STM32内部的PLL，处理器最高运行主频为168 MHz，较强的DSP处理能力在姿态算法上可以保证较高的精度和较快的响应速度[1]。

环境监控模块主要由温湿度传感器SHT11、PM2.5粉尘传感器SM-PWM-0.1A和石油液化气模块MS6100组成。GPS定位选用U-box PAM-7Q模块，具有U-blox 7引擎的高性能，以业内公认的封装形式提供高灵敏度和行内最短的捕捉时间，其最大更新速率为10 Hz，定位精度2.5 mCEP。智能相机选用Intova SP1 Sport HD 1080p高清摄影相机；4G视频数据无线传输支持图像压缩，保证视频高速传递。

1.2 MEMS姿态解算器

姿态解算MEMS器件AHRS（Attitude and Heading Reference System）由三轴角速度陀螺仪、三轴加速度计、三轴磁阻传感器、气压高度传感器组成。

MPU6050組合了16位A/D三轴陀螺仪和三轴加速度计，为全球首例整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了大量包装空间，产品传输可通过最高至400 kHz的I2C总线。

HMC5883L采用霍尼韦尔各向异性磁阻（AMR）技术，这些各向异性传感器具有在轴向高灵敏度和线性高精度的特点。传感器具有的对正交轴的低灵敏度的固相结构能用于测量地球磁场的方向和大小，其测量范围从毫高斯到8高斯（gauss）。

BMP180是一款高精度、小体积、超低能耗的压力传感器，采用强大的8-pin陶瓷无引线芯片承载（LCC）超薄封装。

各传感器通过I2C接口与STM32F407VGT6连接，同时传感器的数据中断引脚与STM32F407VGT6的IO相连。传感器在完成ADC转换，STM32F407VGT6在第一时间读取最新的数据，快速响应姿态的变化。这样的连接使得控制器拥有最大的主动权，最快地获取各传感器的状态和转换结果。

2 算法研究

飞行器采用四元数加互补滤波作为姿态解算算法，引入高度的自适应神经网络PID作为控制器，实现自适应环境飞行和相机增稳等功能。经测试具有灵活轻盈、延展性、适应性强等特点。

2.1 姿态算法

基于互补滤波的四元素算法[2]，利用加速度测得的重力向量与估计姿态得到重力向量的误差来矫正陀螺仪积分误差，从而利用矫正后的陀螺仪积分得到姿态角等，姿态解算已经在多个研究中进行了详细的论证，提出了多种解算算法，故不做详细介绍。

2.2 四轴飞行器高度自适应神经网络PID算法

四轴飞行器的飞行高度控制就如姿态解算一样重要，如天气因素对于飞行器的飞行高度控制系统的影响是一项具有挑战性的技术难点，提供一种合适的方法使机器人在面对不同的环境下，更加稳定和高效的飞行。

尝试使用一种自适应的方法来控制机器人的高度，可以让机器人在不同的状况下做出适当的操作。控制系统的激励已经在MATLAB仿真中完成，飞行器等价为一个质点，因此牛顿-欧拉公式可以在动力学中直接被使用。四转轴机器人的建模中，四转轴机器人的动力学方程使用牛顿-欧拉方法，总的方程如下：

m为机器人质量；VB线性速度；V为角速度；R为旋转矩阵；G为重力加速度；TB为引擎力总和。

自适应PID控制器控制飞行器的高度，类似于多认知对象神经网络结构中的的神经元构架。每一个控制器得到的是一个用于相乘的比率（W1、W2、W3），而控制器的输出被限制在sigmund函数里，而等式（2）中给出了具体的控制器构架。

在更新（W1、W2、W3）时，算法会将基于预定轨迹之间相差错误值来控制系统的值的最小化值。公式（3）会给出这个系统公式。它采用梯度值由递归方程也就是最陡下降算法（SD）和方程（4）和（5）示出其细节实时更新控制器在每个时间的步长系数。通过使用在线学习算法，一些高错误值（外力干扰）输入而造成系统不稳定，虽然输出值Y是一个在[0，1]区间的值，在一些特定的环境下，改变控制器的输出，可以将其改为几个特定的值。

为了优化ANP算法的参数（W1、W2、W3），执行标准应该最小化，并在每一个时间步中使用公式（4）然后再更新参数。其支持一种再没有延时的情况下快速处理并调节算法的系统。

ANP控制器被用来正在确定的系统中自适应的获得合适的参数来控制飞行器的高度。这种方法优点在于其不需要使用不同的数学方法分析来定义合适的系数，适应新的环境和动态的改变系统。

3 软件设计

软件设计分为上位机软件和飞行器底层下位机软件。上位机主要负责路径规划及监控信息实时显示[5]，下位机软件主要负责飞行器的姿态及正常飞行、GPS数据获取、相机控制、传感器数据采集及4G通信等任务。

上位机内嵌Google Earth显示界面，可以直接运用Google Earth规划路径，通过接收GPS信号，进行实时定位显示并将地理位置数据发送给飞行器修正，使飞行器按照上位机软件规划的路径行驶，同时，上位机实时监控飞行器的姿态和相关传感器的数值，并控制摄像头拍照和云台控制，实现智能判断和显示。

4 仿真调试

4.1 姿态角调试

在完成控制器底层的硬件驱动后，开始姿态角算法的调试。通过4G传输模块将解算出的姿态角数据发回上位机，摇动四轴机体，观察上位机数据曲线与姿态曲线。四轴飞行器姿态参数如偏航角：Yaw，俯仰角：Pitch，翻滚角：Rool，从曲线可以看出在进过一段调整后，曲线变为平行曲线，说明这几个量处于一个相对平衡状态，飞行器处于一个平衡状态。

4.2 整机测试

使用LabVIEW编写的上位机软件开始整机调试系统。编写的上位机主要分为5个模块区域：GPS实时地理位置监控和路径规划区域、四轴飞行器姿态及运行状态检测区域、视频扑捉拍摄区域、飞行器当前环境检测区域、4G信号状况显示区域、软件操作区域等，主要操作步骤：

第一步：开启4G网络，使4G网络正常工作；

第二步：在内嵌的Google Earth（GPS实时地理位置监控和路径规划区域），搜索GPS数据和开始进行路径规划设置，使得GPS功能运行正常；

第三步：在四轴飞行器姿态及运行状态检测区域，开启四轴飞行器，让飞行器能够按照路径规划的指定路径飞行；

到达目的地，在视频扑捉拍摄区域开启图像捕捉，或者在环境监测区域查看传感器数据等。

测试过程中，上位机检测效果良好，特别是数据的实时响应，GPS信号解析可以很好的和Google Earth兼容，路径规划传递实时响应，3D渲染也表现的不错，4G网络传输在较为偏僻的地理位置信号较弱。

5 实验及其结果

为了评估这套控制机器人高度的方法，在Matlab仿真中，要求这种控制算法研究2中状态。其在（W1、W2、W3）系数中首要的值被认为是0，在一个有合适参数的时间自适应系统中。在第一个状态中，控制器（Kp，Kd，Ki）首要得到的被认为是一个随机值而结果要求控制系统的输入是一个频率为：1（rad/sec），大小为1 m的正弦波。在这次研究中有一定的尝试和错误，首先r为0.3，同样这个值也對（W1、W2、W3）3个系数有影响。

为了更好的研究控制器的特性，在输入中加入5 N的干扰的方波信号，宽度为0.5个单位。

但是干扰当干扰超过6 N时会造成系统的不稳定并难以控制。在第二种状态下，系统的反应接近正弦输入但是控制器适当的首要的控制值在遗传算法中得到，而r的学习比例将存在出两种不同状态下有干扰和无干扰的区别。

ANP算法的自适应性可以使其通过函数自行降噪，在这个问题中，遗传算法的PID控制函数是通过比较得到的。就如先前的实验中，研究两种状态下有无干扰的降噪效果。

实验的最后一部分是实验在噪声情况下的3种测试，直接测试其控制飞行器的高度，并追踪器输入的正弦信号路径，噪声采用周期0.01 s，范围[-2，2]的输入干扰。

6 结语

在研究中，ANP控制器被用在确定的系统中自适应地获得合适的参数来控制飞行器的高度。这种方法最重要的优点在于其不需要使用不同的数学方法分析来定义合适的系数、适应新的环境和动态的改变系统。另一方面来说，这种方法不能保证系数是近似合适的。另一个问题就是学习比率的值是一个很重要的挑战，因为低的学习比率会降低接近合适参数的速度而使得系统无法快速地适应环境，而高的学习比率可能会导致系统变化无常。因此ANP的首要获得就是一个很重要的问题，而此次基于问题本身的结果控制函数非常巨大。然而，适应性可以提高控制器去除系统输入噪声的能力。ANP控制器可以控制系统中出现的噪声，而PID控制无法做得这么好。

参考文献

[1] 高宗余，方建军，于丽杰.MEMS传感器随机误差Allan方差分析[J].仪器仪表学报，2011（12）：2863-2868.

[2] 付梦印，邓志红，闫莉萍.Kalman滤波理论及其在导航系统中的应用[M].2版.北京：科学出版社，2010：17-18.

[3] C C Foster，G H Elkaim，UC Santa Cruz. Extension of a two-Step calibration methodology to include nonorthogonal sensor axes[J].IEEE Transactionson Aerospace and Electronic Systems，2008，44（3）：1070-1078.

[4] 赵思浩，陆明泉，冯振明.MEMS惯性器件误差系数的Allan方差分析方法[J].中国科学：物理学力学天文学，2010（5）：672-675.

[5] Yun JaeMu，Ko Jae-Pyung，Lee JangMyung.An inexpensive and accurate absolute po-sition sensor for driving assistance[J]. IEEE Transactionson Instrumentation and Measurement，2008，57（4）：864-873.

[6] 吴德会，赵伟，黄松岭，等.传感器动态建模FLANN方法改进研究[J].仪器仪表学报，2009（2）：362-367.

[7] 黄美发，景晖，匡兵，等.基于拟蒙特卡罗方法的测量不确定度评定[J].仪器仪表学报，2009（1）：120-125.

[8] 涂刚毅，金世俊，祝雪芬，等.基于改进粒子滤波算法的GPS非高斯伪距误差修正[J].电子测量与仪器学报，2009（6）：24-28.