一种多旋翼无人机环境自主监测控制系统及方法.pdf

本发明公开了一种多旋翼无人机环境自主监测控制系统及方法，该系统包括多旋翼无人机、地面通信转换系统和移动终端，多旋翼无人机包括无人机控制系统和无人机动力系统；无人机控制系统用于控制多旋翼无人机的运行状态，包括上升、悬停、避障和下落，在运行过程中对空气中的污染物进行多维度自动监测，并将采集到的数据发送出去；地面通信转换系统用于接收无人机采集到的数据，发送给移动终端，并接收来自移动终端的控制指令，发送给无人机。本发明能在无人机姿态平稳的条件下，保证测量精度，实现多维度、不同污染物的浓度检测。

1.一种多旋翼无人机环境自主监测控制系统，其特征在于，包括多旋翼
无人机、地面通信转换系统和移动终端，多旋翼无人机包括无人机控制系统和
无人机动力系统，其中：
无人机控制系统包括主控模块、姿态控制模块、无线收发模块和污染物测
量模块，用于控制多旋翼无人机的运行状态，包括上升、悬停、避障和下落，
在运行过程中对空气中的污染物进行多维度自动监测，并将采集到的数据发送
出去；
地面通信转换系统用于根据不同的通信方式，包括在线和离线模式，接收
无人机采集到的数据，发送给移动终端，并接收来自移动终端的控制指令，发
送给无人机；
移动终端用于对无人机进行功能设定，并接收和显示无人机的监测数据。
2.根据权利要求1所述的多旋翼无人机环境自主监测控制系统，其特征
在于，无人机控制系统还包括与主控模块相连的图像采集模块、避障模块和导
航模块。
3.根据权利要求1所述的多旋翼无人机环境自主监测控制系统，其特征
在于，地面通信转换模块包括蓝牙模块和无线收发模块。
4.根据权利要求1所述的多旋翼无人机环境自主监测控制系统，其特征
在于，姿态控制模块包括主控MCU、六轴数字传感器和三轴数字磁力计。
5.一种权利要求1所述多旋翼无人机环境自主监测控制系统的控制方法，
其特征在于,包括以下步骤：
S1、移动终端向无人机发送路径指令和控制指令，控制无人机移动至初始
坐标，由无人机姿态估计与姿态控制系统读取传感器的值，进行姿态解算，得
到无人机的横滚、俯仰、航向这些姿态角，进而通过串级PID算法、数字滤波
算法等调整无人机的姿态，消除干扰信号并控制无人机保持平衡；
S2、无人机实时监测当前位置空气中的污染物数据，采集超声波传感器的
值进行高度PID控制，并根据路径指令自动移动至下个位置进行监测，移动过
程中对障碍物进行检测并避障；
S3、无人机将采集到的监测数据通过无线发送给移动终端，完成路径上所
有点的监测后，自动返回。
6.根据权利要求5所述的多旋翼无人机环境自主监测控制方法，其特征
在于，步骤S1中姿态解算的方法具体为：
根据姿态传感器陀螺仪测量的无人机在x、y、z轴方向上的角速度，解算
得到四元数在短时间内的增量，通过积分得到无人机在某一时刻的四元数值，
从而计算出此时刻无人机机体的欧拉角。
7.根据权利要求5所述的多旋翼无人机环境自主监测控制方法，其特征
在于，步骤S1中通过双PID控制调整无人机的姿态的方法具体为：
外环输入为欧拉角角度设定值，并与当前机体姿态解算得到的欧拉角测量
值比较，差值经过外环PID控制器输出内环的目标角速度信号，并与当前姿态
传感器得到的角速度值比较，差值经过内环PID控制器计算，输出为给到电调
的PWM信号，电调输出电流通过旋翼电机控制无人机的机身角度进行飞行，然
后姿态传感器检测下一次机身的角度与角速度值，进行下一次串级PID控制。
其中内、外环PID控制以较短的时间间隔循环。
8.根据权利要求5所述的多旋翼无人机环境自主监测控制方法，其特征
在于，步骤S1中获取无人机坐标的方法具体为：使用卡尔曼滤波器将速度位
置信息与GPS信息融合，得到准确的无人机的导航信息。
9.根据权利要求5所述的多旋翼无人机环境自主监测控制方法，其特征
在于，步骤S2中通过定高控制算法调整无人机的姿态的方法具体为：
控制器输入为高度设定值，并与实际测量高度值比较，差值经过高度PID
控制器，输出为PWM控制信号，与姿态控制输出的PWM信号相加控制旋翼电机，
从而控制无人机机体的高度；然后经过20ms，超声波传感器测量下一次机体
实际高度，进行下一次高度控制；控制算法中，将预先设定高度分300次逐渐
增加共6s传入高度设定值，控制无人机上升的速度，保证其一键起飞的稳定
性；超声波传感器每20ms检测一次无人机高度，高度PID控制器在实际微分
项中加入加速度积分得到的速度，设置一定的系数比，以防止无人机在一键起
飞时的速度偏高，由于积分误差的存在，在到达预设高度后去掉速度融合项。
10.根据权利要求5所述的多旋翼无人机环境自主监测控制方法，其特征
在于，步骤S3中无人机将采集到的监测数据通过无线发送给移动终端的方法
具体包括：
在线状态，移动终端直接利用wifi、gprs，通过向云服务器发送指令实
现对无人机控制，并通过云服务器监控储存无人机地理位置、污染物测量数据、
图像信息，由移动终端通过wifi、gprs无线信号向云服务器发送数据请求读
取并显示数据；
离线状态，利用地面通信转换端作为无人机与移动终端的通信中转站，地
面通信转换端包括无线收发模块和蓝牙模块，移动终端对无人机的控制信息由
蓝牙模块传输至通信转换端内，再由无线收发模块转送至无人机主控芯片；同
样，无人机的位置信息、污染物测量数据和图像采集信息通过通信转换端传回
移动终端，用于后续的分析。

一种多旋翼无人机环境自主监测控制系统及方法

技术领域

本发明涉及无人机和无线传感领域，尤其涉及一种多旋翼无人机环境自主
监测控制系统及方法。

背景技术

近年来，环境问题备受关注,需要对大气中的污染物的进行实时监测并且
采集数据，以便于分析与改善措施的制定等。目前，随着科技的日益发展，利
用无人机来监测环境逐渐普及。无人机的控制和信息采集传送是监测系统的关
键技术之一。现有的环境监测无人机项目大多是使用特定的遥控器进行无线控
制，对于采集到的数据由存储卡进行存储。

采用特定的遥控器进行无人机控制，增加了系统的开发成本且不便于携
带，在高空采集地点不稳定，容易发生偏移，不能对无人机姿态进行完美的控
制，其控制精度与可信度也会受到人为因素的影响。利用存储卡存储无人机采
集到的数据图像信息，并不能在无人机飞行的过程中实时分析环境数据，也就
不便于依照环境情况调整无人机的飞行路线，会增加监测的工作量。

发明内容

本发明要解决的技术问题在于针对现有技术中人工控制无人机采集数据
容易产生误差偏移的缺陷，提供一种能够自动控制无人机进行环境监测的多旋
翼无人机环境自主监测控制系统及方法。

本发明解决其技术问题所采用的技术方案是：

本发明提供一种多旋翼无人机环境自主监测控制系统，包括多旋翼无人
机、地面通信转换系统和移动终端，多旋翼无人机包括无人机控制系统和无人
机动力系统，其中：

无人机控制系统包括主控模块、姿态控制模块、无线收发模块和污染物测
量模块，用于控制多旋翼无人机的运行状态，包括上升、悬停、避障和下落，
在运行过程中对空气中的污染物进行多维度自动监测，并将采集到的数据发送
出去；

地面通信转换系统用于根据不同的通信方式，包括在线和离线模式，接收
无人机采集到的数据，发送给移动终端，并接收来自移动终端的控制指令，发
送给无人机；

移动终端用于对无人机进行功能设定，并接收和显示无人机的监测数据。

进一步地，本发明的无人机控制系统还包括与主控模块相连的图像采集模
块、避障模块和导航模块。

进一步地，本发明的地面通信转换模块包括蓝牙模块和无线收发模块。

进一步地，本发明的姿态控制模块包括主控MCU、六轴数字传感器和三轴
数字磁力计。

本发明提供一种多旋翼无人机环境自主监测控制方法，包括以下步骤：

S1、移动终端向无人机发送路径指令和控制指令，控制无人机移动至初始
坐标，由无人机姿态估计与姿态控制系统读取传感器的值，进行姿态解算，得
到无人机的横滚、俯仰、航向这些姿态角，进而通过串级PID算法、数字滤波
算法等调整无人机的姿态，消除干扰信号并控制无人机保持平衡；

S2、无人机实时监测当前位置空气中的污染物数据，采集超声波传感器的
值进行高度PID控制，并根据路径指令自动移动至下个位置进行监测，移动过
程中对障碍物进行检测并避障；

S3、无人机将采集到的监测数据通过无线发送给移动终端，完成路径上所
有点的监测后，自动返回。

进一步地，本发明的步骤S1中姿态解算的方法具体为：

根据姿态传感器陀螺仪测量的无人机在x、y、z轴方向上的角速度，解算
得到四元数在短时间内的增量，通过积分得到无人机在某一时刻的四元数值，
从而计算出此时刻无人机机体的欧拉角。

进一步地，本发明的步骤S1中通过双PID控制调整无人机的姿态的方法
具体为：

外环输入为欧拉角角度设定值，并与当前机体姿态解算得到的欧拉角测量
值比较，差值经过外环PID控制器输出内环的目标角速度信号，并与当前姿态
传感器得到的角速度值比较，差值经过内环PID控制器计算，输出为给到电调
的PWM信号，电调输出电流通过旋翼电机控制无人机的机身角度进行飞行，然
后姿态传感器检测下一次机身的角度与角速度值，进行下一次串级PID控制。
其中内、外环PID控制以较短的时间间隔循环。

进一步地，本发明的步骤S1中获取无人机坐标的方法具体为：使用卡尔
曼滤波器将速度位置信息与GPS信息融合，得到准确的无人机的导航信息。

进一步地，本发明的步骤S2中通过定高控制算法调整无人机的姿态的方
法具体为：

控制器输入为高度设定值，并与实际测量高度值比较，差值经过高度PID
控制器，输出为PWM控制信号，与姿态控制输出的PWM信号相加控制旋翼电机，
从而控制无人机机体的高度；然后经过20ms，超声波传感器测量下一次机体
实际高度，进行下一次高度控制；控制算法中，将预先设定高度分300次逐渐
增加共6s传入高度设定值，控制无人机上升的速度，保证其一键起飞的稳定
性；超声波传感器每20ms检测一次无人机高度，高度PID控制器在实际微分
项中加入加速度积分得到的速度，设置一定的系数比，以防止无人机在一键起
飞时的速度偏高，由于积分误差的存在，在到达预设高度后去掉速度融合项。

进一步地，本发明的步骤S3中无人机将采集到的监测数据通过无线发送
给移动终端的方法具体包括：

在线状态，移动终端直接利用wifi、gprs，通过向云服务器发送指令实
现对无人机控制，并通过云服务器监控储存无人机地理位置、污染物测量数据、
图像信息，由移动终端通过wifi、gprs无线信号向云服务器发送数据请求读
取并显示数据；

离线状态，利用地面通信转换端作为无人机与移动终端的通信中转站，地
面通信转换端包括无线收发模块和蓝牙模块，移动终端对无人机的控制信息由
蓝牙模块传输至通信转换端内，再由无线收发模块转送至无人机主控芯片；同
样，无人机的位置信息、污染物测量数据和图像采集信息通过通信转换端传回
移动终端，用于后续的分析。

本发明产生的有益效果是：本发明的多旋翼无人机环境自主监测控制系
统，在多旋翼无人机的自主导航与自主悬停性能的基础上，完成某一维度的气
体测量后，控制无人机完成自主上升，到达指定地点悬停，自主测量、避障、
下落等任务，全程无人操作，并且能在无人机姿态平稳的条件下，保证测量精
度，实现多维度、不同污染物的浓度检测，对于监测结果可以通过实时传回移
动终端进行读取显示，便于依照监测环境的情况调整无人机的飞行路线，可以
减少监测的工作量；该系统可以提高无人机的控制精度，实现环境气体的高精
度自动监测。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的多旋翼无人机环境自主监测控制系统的结构图；

图2是本发明实施例的多旋翼无人机环境自主监测控制系统的控制系统
框图；

图3是本发明实施例的多旋翼无人机环境自主监测控制系统的Android
终端的结构框图；

图4是本发明实施例的多旋翼无人机环境自主监测控制系统的姿态控制
的算法示意图；

图5是本发明实施例的多旋翼无人机环境自主监测控制系统的高度控制
的算法示意图；

图中，1-控制系统，10-主控芯片，11-姿态控制系统，12-导航系统，13-
无线收发模块，14-避障模块，15-图像采集模块，16-污染物测量模块，2-地面
通信转换系统，3-Android终端，110-六轴数字传感器，111-三轴数字磁力计，
112-主控MCU，4-无人机动力系统，40-电调，41-无刷电机，42-螺旋桨，43-
电池，30-无人机飞行坐标设定，31-航行轨迹，32-自动归位，33-监控区域图
像，34-空气质量分析，35-云服务器，36-电池电量显示，37-蓝牙。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实
施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅
用以解释本发明，并不用于限定本发明。

如图1所示，本发明实施例的多旋翼无人机环境自主监测控制系统，包括
多旋翼无人机、地面通信转换系统和移动终端，多旋翼无人机包括无人机控制
系统和无人机动力系统，其中：

无人机控制系统包括主控模块、姿态控制模块、无线收发模块和污染物测
量模块，用于控制多旋翼无人机的运行状态，包括上升、悬停、避障和下落，
在运行过程中对空气中的污染物进行多维度自动监测，并将采集到的数据发送
出去；

地面通信转换系统用于根据不同的通信方式，包括在线和离线模式，接收
无人机采集到的数据，发送给移动终端，并接收来自移动终端的控制指令，发
送给无人机；

移动终端用于对无人机进行功能设定，并接收和显示无人机的监测数据。

无人机控制系统还包括与主控模块相连的图像采集模块、避障模块和导航
模块。地面通信转换模块包括蓝牙模块和无线收发模块。姿态控制模块包括主
控MCU、六轴数字传感器和三轴数字磁力计。无人机动力系统包括多组与主控
MCU相连的旋翼模块，每组旋翼模块均包括依次相连的电调、无刷电机和螺旋
桨。

如图2所示，在本发明的另一个具体实施例中，多旋翼无人机环境自主监
测控制系统，由多旋翼无人机，无线通信端和Android终端三大部分组成，多
旋翼无人机主要由控制系统和动力系统两部分组成，控制系统以微处理器为主
控芯片实现整机控制，主要包括姿态控制系统、导航系统、无线收发模块、避
障模块、图像采集模块和大气污染物测量模块；动力系统主要由电调、无刷电
机、螺旋桨和航模电池组成。

无线通信端，按照系统的在线与离线模式，具有两种不同的通信方式，第
一种是在线状态下，Android终端直接利用wifi、gprs，通过向云服务器发送
指令实现对无人机控制，并通过云服务器监控储存无人机地理位置、污染物测
量数据、图像信息，由Android通过所述wifi、gprs无线信号向云服务器发送
数据请求读取并显示数据；第二种是离线状态下，利用地面通信转换端作为无
人机与Android终端的通信中转站，所述地面通信转换端包括无线收发模块和
蓝牙模块，后两者之间有相应的数据转换模块，所述无线收发模块负责无人机
与地面转换端的通信，所述地面转换端内的蓝牙模块与Android终端实现蓝牙
通信，即Android终端对无人机的控制信息由蓝牙模块传输至通信转换端内，
再由无线收发模块转送至无人机主控芯片；同样，无人机的位置信息、污染物
测量数据和图像采集信息通过通信转换端传回Android终端，便于后续的分析。

如图3所示，Andriod终端，能够实现对无人机的坐标设定、自动归位控
制，接收并显示环境监测数据、监控区域图像、航行轨迹以及无人机电池电量，
并根据污染物测量数据分析不同高度的污染程度，测量数据图像以及分析情况
可以同步保存至Android终端SD卡以及云服务器中，所述云服务器，在系统
使用在线模式时，可以实时接收从无人机发来的位置信息、不同高度污染物测
量的数据、图像信息，并存储在云服务器中，Android终端验证用户信息后，
通过无线信号向云服务器发送数据请求信号，云服务器判别请求信号的类型之
后向Android终端发送所需的数据。

如图2所示，姿态控制系统，用六轴数字传感器和三轴数字磁力计相融合，
运用串级PID控制实现多旋翼无人机的平衡控制以及其他所需的计算。多旋
翼无人机使用六轴数字传感器110和三轴数字磁力计111相融合进行姿态估
计，所述六轴数字传感器内部集成三轴加速度计和三轴陀螺仪，能以200Hz
的频率提供所需加速度和陀螺仪数据，三轴数字磁力计能以50Hz的频率提供
所需地磁方向数据，从而得到准确的无人机姿态，便于进行姿态解算。

导航系统12使用现代导航技术。采用新一代低功耗、高精度的GPS模块，
且支持中国北斗导航。多旋翼无人机的速度位置信息通过多旋翼无人机控制系
统的姿态估计器得到。使用卡尔曼滤波器将速度位置信息与GPS信息融合，
可得到准确的无人机的导航信息。然后根据导航信息和无人机设定的飞行信息
对多旋翼无人机进行定位，所述定位信息可以在导航地图上实时显示。

无线收发模块13，对应于系统使用离线模式时启用，可工作于免许可证
的2.4G ISM频段，大功率、高灵敏度、数据传输距离远，且重量只有十几克。
分别安装于多旋翼无人机和地面通信转换端内，用于两者之间的远程通信，多
旋翼无人机的主控系统只需使用SPI接口与该无线模块连接，即可进行数据的
收发。

地面通信转换系统2如图1所示，是系统离线模式下的通信方式，所述地
面通信转换端作为无人机与Android终端的通信中转站，包括无线收发模块和
蓝牙模块。离线模式下Android终端向无人机发送指令的步骤如下：

步骤一：在Android终端设定对无人机的控制信息，由蓝牙模块发送至通
信转换端；

步骤二：由通信转换端内的蓝牙转无线模块将步骤一接收到的信息发送至
无人机的无线收发模块；

步骤三：无人机微处理器主控芯片读取无线收发模块的信息控制无人机。

无人机的监测信息传回Android终端按照上述步骤逆向执行，Android终
端与无人机的通信可以实时进行。

通信转换端在线模式下的通信主要是利用wifi、gprs模块接入internet，
运用云服务器实现Android终端和无人机的互相通信，上述三者在数据传输过
程中以数据帧的形式进行收发数据，所述数据帧的内容包括帧头、功能字、长
度、数据、校验位等。帧头主要是判别数据帧的发送端；功能字主要是发送端
为获得指定数据而特意设定的标志位；长度即数据帧中数据的长度；数据是实
际接收端要处理的数据；校验位主要是为了防止在无线数据收发过程中出现数
据帧丢失而使接收端接收到错误数据，所述校验位通过特定算法，确保了
Android终端、无人机与云服务器之间数据传输的准确性，增强了抗干扰性。

避障模块14，在多旋翼无人机飞行时受到天气条件的影响比较大时启用，
当风力高于3级时，由于风力的影响，无人机会发生飘移的情况，可能导致其
与建筑物发生碰撞。所以需要安装测距系统用来测量无人机距离障碍物的距
离。超声波模块的特点必须符合探测角度足够大、测量距离的范围适中的要求。
由于无人机机体不大，能近距离飞抵线路附近，选用的测试距离为6－10米。
超声波发射、接收以及无人机主控芯片控制三个部分组成了超声波测距模块。
超声波测距模块由主控芯片进行控制并以级联的方式进行联接。在无人机的
左、右、前三个方位上分别安装了超声波测距模块，一旦三个超声波收发模块
同时开启，控制板就会将测得的三个方位的距离信息发送给控制系统，控制系
统随即对三个方位距离信号进行提取分析以判别无人机是否与障碍物过于接
近。

图像采集模块15，是为了记录所监控区域的大气污染信息而使用高清摄
像头来采集大气的图像，同时将图像信息通过地面通信转换系统2或者云服务
器传输到Android终端3，以便检测人员实时了解现场大气的情况与周围环境，
并将多旋翼无人机传来的图像使用Android终端的SD卡或者云服务器进行存
储，方便测量人员的后期检查和处理。

大气污染物测量模块16，通过使用相应的传感器，可以实现对大气污染
物(以PM2.5、PM10为主的颗粒物，以二氧化硫为主的硫氧化合物，以二氧化
氮为主的氮氧化合物等等)和大气中的有害气体进行精准检测。关于传感器的
安装可以选择多个不同功能的传感器也可以只安装单个传感器，实现对大气中
不同类型的污染物进行有选择性的监测。

传感器使用配套的微处理器对数字电路模块检测到的信号进行分析，然后
通过串口与无人机主控芯片相连，输出测得的污染物浓度，通过地面通信转换
系统2或者云服务器传回Android终端进行分析显示。所述传感器要尽可能小
型化、便于维护，降低机体重量和使用成本。

本发明的多旋翼无人机的动力系统4如图2所示，使用电调40、三相无
刷电机41、螺旋桨42和航模电池43。无刷电机由于取消了电刷和换向器组成
的机械接触结构，没有换向火花和机械摩擦，具有效率高、无电磁干扰、寿命
长、运行可靠等优势，使用与无刷电机配套的电调和螺旋桨，可以保证动力系
统的良好性能。多旋翼无人机的电池要求具有大电流放电能力，同时还需体积
小、重量轻，选择航模电池。

Android终端3，是每一个人随身携带的移动产品，可以是装有Android
系统的手机或者PC机，但并不仅限于此。由操作者在所述Android终端进行
无人机的指令发送，并根据所接收信息实时调整无人机的飞行。其主要实现的
功能如图3所示：

Android终端通过无人机飞行坐标设定模块30设置无人机所要监测区域
的经纬度以及空间坐标，可以显示无人机的油门大小以及方向舵相对位置，对
于无人机的飞行速度，飞行时间，飞行高度也会有相应的坐标显示。

航行轨迹31，可以在地图上实时显示无人机所监测区域的航行轨迹，可
以结合监测区域空气质量的分布情况，由无人机飞行坐标设定模块30相应改
变监测位置坐标，实现实时分析，灵活测量。

自动归位模块32可以在无人机监测完毕使无人机自动平稳返回。

监控区域图像模块33内可以查看无人机监测区域的大气图像及其周围环
境。

空气质量分析模块34，可以对蓝牙接收到的大气污染的数据信息进行处
理分析，对比国家空气质量的分析标准，显示所监测区域的空气质量。

云服务器35，用于在线模式下实时储存无人机的航行轨迹、监控区域图
像和污染物测量情况等。用户可以登录查看。

电池电量显示模块36，用来显示多旋翼无人机飞行过程中的电量余额。

蓝牙模块37与通信转换模块2进行连接，用于在离线模式下向无人机发
送控制指令，同时将无人机监测到的图像信息、航行轨迹和大气污染物的数据
信息返回Android控制终端并由SD卡进行存储。

本发明实施例的多旋翼无人机环境自主监测控制方法，包括以下步骤：

S1、移动终端向无人机发送路径指令和控制指令，控制无人机移动至初始
坐标，由无人机姿态估计与姿态控制系统读取传感器的值，进行姿态解算，得
到无人机的横滚、俯仰、航向这些姿态角，进而通过串级PID算法、数字滤波
算法等调整无人机的姿态，并控制无人机保持平衡；

S2、无人机实时监测当前位置空气中的污染物数据，采集超声波传感器的
值进行高度PID控制，并根据路径指令自动移动至下个位置进行监测，移动过
程中对障碍物进行检测并避障；

S3、无人机将采集到的监测数据通过无线发送给移动终端，完成路径上所
有点的监测后，自动返回。

步骤S1中姿态解算的方法具体为：

根据姿态传感器陀螺仪测量的无人机在x、y、z轴方向上的角速度，解算
得到四元数在短时间内的增量，通过积分得到无人机在某一时刻的四元数值，
从而计算出此时刻无人机机体的欧拉角。最终所得欧拉角是用来描述刚体在三
维空间上坐标轴的旋转角度。定义三个旋转角：

横滚角Rol：绕y轴旋转的夹角，逆时针方向为正；

俯仰角Pit：绕x轴旋转的夹角，逆时针方向为正；

航向角Yaw：绕z轴旋转的夹角，逆时针方向为正。

机体坐标系b即可看做由地理坐标系g通过在三个坐标轴上旋转欧拉角而
得到。定义旋转角度γ为无人机上一点A绕机身外一定点O转动，向量OA
为初始位置向量，经过时间t转动角度运动到点B，A与B位于同一平面。

由四元数Q的定义可知：

Q(q0,q1,q2,q3)＝q0+q1i+q2j+q3k (1)

$Q=cos\frac{\mathrm{\γ}}{2}+lsin\frac{\mathrm{\γ}}{2}i+m\sin \frac{\mathrm{\γ}}{2}j+nsin\frac{\mathrm{\γ}}{2}k=cos\frac{\mathrm{\γ}}{2}+u^{g}sin\frac{\mathrm{\γ}}{2}---\left(2\right)$

其中q0、q1、q2、q3为实数，i、j、k是相互正交的单位向量。

对四元数Q求导可得：

$\frac{dQ}{dt}=-\frac{\mathrm{\γ}}{2}sin\frac{\mathrm{\γ}}{2}+u^g\frac{\mathrm{\γ}}{2}cos\frac{\mathrm{\γ}}{2}+sin\frac{\mathrm{\γ}}{2}\frac{{\mathrm{du}}^g}{dt}---\left(3\right)$

然后根据哥氏定理可知：

$\frac{{\mathrm{du}}^g}{dt}=C_b^g\frac{{\mathrm{du}}^b}{dt}+{\mathrm{\ω}}_{gb}^g\×u^g---\left(4\right)$

其中：

$\frac{{\mathrm{du}}^b}{dt}=0,{\mathrm{\ω}}_{gb}^g=\frac{d\mathrm{\γ}}{dt}{u}^g---\left(5\right)$

故：

$\frac{{\mathrm{du}}^g}{dt}=0---\left(6\right)$

所以：

$\frac{dQ}{dt}=-\frac{\mathrm{\γ}}{2}sin\frac{\mathrm{\γ}}{2}+u^g\frac{\mathrm{\γ}}{2}cos\frac{\mathrm{\γ}}{2}---\left(7\right)$

由于：

$\frac{d\mathrm{\γ}}{2dt}{u}^g\⊗Q=\frac{d\mathrm{\γ}}{2dt}{u}^{g}cos\frac{\mathrm{\γ}}{2}-\frac{d\mathrm{\γ}}{2dt}sin\frac{\mathrm{\γ}}{2}---\left(8\right)$

可得：

$\frac{dQ}{dt}=\frac{d\mathrm{\γ}}{2dt}{u}^g\⊗Q=\frac{1}{2}{\mathrm{\ω}}_{gb}^g\⊗Q---\left(9\right)$

由于无人机运载体上姿态传感器陀螺仪测量的角速度即为机体坐标系运
动的角速度。且：

${\mathrm{\ω}}_{gb}^g=Q\⊗{\mathrm{\ω}}_{gb}^b\⊗Q*---\left(10\right)$

可得

$\frac{dQ}{dt}=\frac{1}{2}{\mathrm{\ω}}_{gb}^g\⊗Q=\frac{1}{2}Q\⊗{\mathrm{\ω}}_{gb}^b\⊗Q*\⊗Q=\frac{1}{2}Q\⊗{\mathrm{\ω}}_{gb}^b---\left(11\right)$

由四元数的乘法公式可得：

$\left[\begin{array}{c}\frac{{\mathrm{dq}}_0}{dt}\\ \frac{{\mathrm{dq}}_1}{dt}\\ \frac{{\mathrm{dq}}_2}{dt}\\ \frac{{\mathrm{dq}}_3}{dt}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(12\right)$

故可以根据姿态传感器陀螺仪测量的无人机在x、y、z轴方向上的角速度，
解算得到四元数在短时间内的增量，通过积分得到无人机在某一时刻的四元数
值，四元数值与无人机飞行时改变的欧拉角的关系如下：

$\left[\begin{array}{c}\mathrm{\θ}\\ \mathrm{\β}\\ \mathrm{\α}\end{array}\right]=\left[\begin{array}{c}arctan\frac{2(q_2{q}_3+q_0{q}_1)}{1-2(q_1^2+q_2^2)}\\ arcsin2(q_0{q}_2-q_1{q}_3)\\ \arctan \frac{2(q_1{q}_2+q_0{q}_3)}{1-2(q_2^2+q_3^2)}\end{array}\right]---\left(13\right)$

其中：θ、β、α分别代表无人机的三个旋转角，四元数值与无人机飞
行时的欧拉角关系推导在此不做介绍。

根据式(12)和式(13)可计算出此时刻无人机机体的欧拉角。由此可知，当
得到四旋翼无人机的四元数的值，便可解算得到无人机的横滚、俯仰、航向这
些姿态角，从而控制无人机的运动。

但是由于姿态解算过程中存在积分运算，当陀螺仪检测的角速度信号存在
误差时，随着积分的作用，误差会越来越大，可能导致得到的角度信号失真。
这时就需要一种能长期相信的传感器提供角度信号，这就是加速度计的作用。
机体坐标系b中x、y、z轴方向上所受到的实际加速度与重力加速度在机体坐
标系b中三个轴上投影的加速度相比较，得到二者姿态误差，然后将该误差积
分经过与陀螺仪得到的数据进行互补滤波，纠正陀螺仪的积分误差。从而得到
稳定可信赖的姿态角数据。互补滤波过程中，互补滤波系数可以控制对陀螺仪
与加速度计各自的相信程度，如果完全按照这个姿态误差将陀螺仪的积分误差
补偿，则完全信任加速度计。由于加速度计上噪声的影响，互补滤波系数需根
据实际情况而定。

步骤S1中通过串级PID算法控制调整无人机的姿态的方法具体为：

串级PID控制是对单回路PID控制质量的提升，是四旋翼无人机的姿态控
制的核心。当控制系统存在多个影响因素对输出量有影响时，单回路PID控制
不能很好的达到系统控制要求。串级PID将所有影响因素考虑到控制回路，能
有效实现控制性能，提高系统的动态响应速度和稳定性。

图4为四旋翼无人机的串级PID控制结构图。外环输入为欧拉角角度设定
值，并与当前机体欧拉角测量值比较，差值经过外环PID控制器输出内环的目
标角速度信号，并与当前姿态传感器得到的角速度值比较，差值经过内环PID
控制器计算，输出为给到电调的PWM信号，电调输出电流通过旋翼电机控制无
人机的机身角度进行飞行，然后姿态传感器检测下一次机身的角度与角速度
值，进行下一次串级PID控制。其中内、外环PID控制以较短的时间间隔循环。

步骤S1中通过数字滤波算法调整无人机的姿态的方法具体为：

采用数字滤波算法中的一阶低通滤波，计算量小，适用性强。一阶模拟低
通滤波器为硬件RC滤波，根据电路推导可得其微分方程，则一阶数字低通滤
波用差分方程来表示其微分方程，便可得到其公式为：

Yn＝a×Xn+(1-a)×Yn-1 (14)

其中Yn为本次滤波的输出值；a为滤波系数，其值通常远小于1；Xn为本
次采样输入值；Yn-1为Yn前一次的滤波输出值。

此算法的滤波系数a与截止频率f的关系为：

$f=\frac{a}{2\mathrm{\π}T}---\left(15\right)$

其中π为圆周率，T为输入值的采样间隔时间。

由上式可知，该算法滤波的输出值Yn更倾向于相信上次滤波的输出值Yn-1，而
本次滤波的采样值Xn对本次滤波输出值的影响相对较小，这就在一定程度上
滤除了本次滤波中高频信号的干扰。当输入变量低频变化时，这种滤波算法效
率是相当高的，且滤波效果好。但是，这种滤波算法不能有效滤除高于1/2采
样频率的干扰信号，故这种干扰信号需要采取其他合适的滤波算法。但对于本
系统的应用，一阶低通滤波完全满足控制系统对滤波性能的要求。

步骤S1中获取无人机坐标的方法具体为：使用卡尔曼滤波器将速度位置
信息与GPS信息融合，得到准确的无人机的导航信息。

步骤S2中通过定高控制算法调整无人机的姿态的方法具体为：

图5为四旋翼无人机的高度控制结构图。控制器输入为高度设定值，并与
实际测量高度值比较，差值经过高度PID控制器，输出为PWM控制信号，与姿
态控制输出的PWM信号相加控制旋翼电机，从而控制无人机机体的高度。然后
经过20ms，超声波传感器测量下一次机体实际高度，进行下一次高度控制。
控制算法中，将预先设定高度分300次逐渐增加共6s传入高度设定值，控制
无人机上升的速度，保证其一键起飞的稳定性；超声波传感器每20ms检测一
次无人机高度，高度PID控制器在实际微分项中加入加速度积分得到的速度，
设置一定的系数比，以防止无人机在一键起飞时的速度偏高，由于积分误差的
存在，在到达预设高度后去掉速度融合项。

步骤S2中无人机进行自动移动和监测的方法具体为：

无人机按照预定的路线完成自主上升，到达指定地点悬停、自主测量、避
障、下落任务，以及将返回的测量数据和图像进行分析，并在移动终端显示所
监测区域的图像、飞行器的航行轨迹、航模电池电量余额以及空气质量，相应
的监测信息也会同步发送至云服务器中，在监测完毕可使飞行器自动归位。

步骤S3中无人机将采集到的监测数据通过无线发送给移动终端的方法具
体包括：

在线状态，移动终端直接利用wifi、gprs，通过向云服务器发送指令实
现对无人机控制，并通过云服务器监控储存无人机地理位置、污染物测量数据、
图像信息，由移动终端通过wifi、gprs无线信号向云服务器发送数据请求读
取并显示数据；

离线状态，利用地面通信转换端作为无人机与移动终端的通信中转站，地
面通信转换端包括无线收发模块和蓝牙模块，移动终端对无人机的控制信息由
蓝牙模块传输至通信转换端内，再由无线收发模块转送至无人机主控芯片；同
样，无人机的位置信息、污染物测量数据和图像采集信息通过通信转换端传回
移动终端，用于后续的分析。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进
或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。