基于DSP的四旋翼无人飞行器自主悬停控制系统及方法

摘要

一种基于DSP的四旋翼无人飞行器自主悬停控制系统及方法，控制系统有分别设置在四个机翼上的四个螺旋桨和控制单元，控制单元是：主控制单元通过无线网络连接地面计算机，接收机与信号转换模块和手动/自动切换芯片依次连接，将接收到的遥控器所发出的信号通过信号转换模块送入手动/自动切换芯片，接收机还通过主控制单元连接手动/自动切换芯片，将接收到的信号通过主控制单元送入手动/自动切换芯片，用于控制驱动单元驱动四个螺旋桨上的直流无刷电机工作。方法有对声纳数据进行卡尔曼滤波处理获得准确的高度数据和驱动器控制。本发明能够有效增强了微小型四旋翼无人飞行器的自主飞行能力和对环境的适应能力，可完成室内环境和室外环境下自主飞行控制。

说明书

技术领域

本发明涉及一种无人飞行器自主悬停控制方法。特别是涉及一种基于DSP的四旋翼无人 飞行器自主悬停控制系统及方法。

背景技术

无人飞行器是指无人驾驶、自主推进，通过无线电遥控或者自身程序控制，利用空气动 力承载飞行并且可回收重复利用的飞行器。它凭借自身的优势和特点，在多个领域获得了广 泛应用。在军事方面，无人飞行器的主要用途包括地面战场侦察、获取敌方情报、近距离控 制巡逻、监视、电子战、地面通信等；在民用方面，无人飞行器的使用范围也相当广泛，例 如航拍和图像传输，巡逻监视和目标跟踪，自然灾害发生之后的搜索和救援，高压线、桥梁、 水坝和地震后路段的检查等。无人飞行器根据其机翼的类型可以分为固定翼无人飞行器和旋 翼式无人飞行器。其中，旋翼式无人飞行器具有广阔的发展前景，因为它具有固定翼无人飞 行器无法比拟的优势：体积小，轻便灵活；结构简单，耗能低；能够自主起飞和降落；适应 各种环境，在障碍物密集环境下的可控性强；保持飞行姿态的能力较强，并且能以多种姿态 进行飞行，包括悬停、前飞、倒飞和侧飞等飞行姿态。近几年，四旋翼无人飞行器已经成为 了控制领域的研究热点（期刊：中国科学技术大学硕士学位论文；著者：庞庆霈；出版时间： 2011年；文章题目：四旋翼飞行器设计与稳定控制研究；页码：15-20）。

目前国内外许多高校和研究机构开展了四旋翼无人飞行器系统控制领域的研究工作，比 较有代表性的成果包括：（1）瑞士洛桑联邦科技学院（EPFL）的自动化系统实验开发了OS4 平台，该平台使用Draganflyer公司的旋翼和十字框架，采用4个Faulhaber的电机，机载传 感器有惯性测量单元（IMU）、声纳传感器和视觉系统，通过地面站计算机作为控制器，完成 飞行控制任务。他们研究的重点在于结构设计和自主飞行控制算法（期刊：长春理工大学硕 士学位论文；著者：郑伟光；出版时间：2010年；文章题目：四旋翼无人飞行器飞行姿态控 制系统研究；页码：10-17）；（2）美国宾夕法尼亚大学的研究小组，搭建的室内四旋翼无人 飞行器机体采用了Ascending Technologies公司的Hummingbird型号本体，有效载荷为0.2kg， 采用了Vicon视觉定位系统，由地面站进行数据采集、运行控制算法和发送控制指令。目前， 该研究小组已在四旋翼无人飞行器室内视觉定位、轨迹规划以及多机协同等方面取得了一定 的研究成果。但是，该平台存在一定的局限，主要是载重小，只能在室内环境下飞行（期刊： IEEE International Conference on Robotics and Automation；著者：S.Shen,N.Michael,V.Kumar； 出版时间：2011年；文章题目：Autonomous Multi-Floor Indoor Navigation with a Computationally  Constrained MAV，页码：20-25）；（3）中国国防科技大学的研究人员自行设计制作了四旋翼 无人飞行器机体和基于单片机作为控制单元的机载控制系统，并在仿真中实现了较好的控制 效果（期刊：中国国防科技大学硕士学位论文；著者：王俊生；出版时间：2007年；文章题 目：四旋翼蝶形飞行器控制系统设计及控制方法研究；页码：12-22）。

发明内容

本发明所要解决的技术问题是，提供一种能够充分发挥机载传感器及自行设计飞行控制 算法作用，有效增强了微小型四旋翼无人飞行器的自主飞行能力和对环境的适应能力，可完 成室内环境和室外环境下自主飞行控制的基于DSP的四旋翼无人飞行器自主悬停控制系统及 方法。

本发明所采用的技术方案是：一种基于DSP的四旋翼无人飞行器自主悬停控制系统，包 括有四旋翼机体、分别设置在四个机翼上的四个螺旋桨以及控制四个螺旋桨旋转的控制单元， 所述的控制单元包括有：主控制单元、地面计算机、接收遥控器信号的接收机、信号转换模 块、手动/自动切换芯片以及用于驱动四个螺旋桨上的直流无刷电机工作的驱动单元，所述的 主控制单元通过无线网络连接地面计算机，所述的接收机与信号转换模块和手动/自动切换芯 片依次连接，将接收到的遥控器所发出的信号通过信号转换模块送入手动/自动切换芯片，用 于控制驱动单元驱动四个螺旋桨上的直流无刷电机工作，所述的接收机还通过主控制单元连 接手动/自动切换芯片，将接收到的信号通过主控制单元送入手动/自动切换芯片，用于控制驱 动单元驱动四个螺旋桨上的直流无刷电机工作。

所述的驱动单元包括有四个电子调速器，所述的四个电子调速器的输入端分别连接手动 /自动切换芯片的输出端，所述四个电子调速器的输出端分别对应连接四个螺旋桨上的直流无 刷电机。

所述的主控制单元包括有机载主控制器，所述的机载主控制器分别连接上位机、机载传 感器和无线数传模块，所述的上位机还连接声纳传感器，其中，所述的机载主控制器分别连 接接收机和手动/自动切换芯片用于控制驱动单元驱动四个螺旋桨上的直流无刷电机工作，所 述的机载主控制器通过无线数传模块和无线连网络接地面计算机，所述的上位机通过无线连 网络接地面计算机。

一种基于DSP的四旋翼无人飞行器自主悬停控制系统的控制方法，包括：对声纳数据进 行卡尔曼滤波处理获得准确的高度数据和驱动器控制，其中，驱动器控制包括如下步骤：

1）系统初始化操作；

2）在系统完成初始化后，各接口开始与外部设备进行通信，其中，串口SCIB与机载传 感器MTi-G连接，读取和验证MTi-G发送过来的姿态位置信息；串口SCIC与无线数传模块 连接，通过无线数传模块的传输功能与地面站进行数据通信；SPI接口与ARM模块进行通信， 获得由ARM模块读取并经过滤波算法处理的声纳数据，并将无人机全部的实时飞行数据快 速地发送至ARM模块；

3）根据各个传感器的数据和飞行轨迹规划指令，对程序中定义的实际飞行位置姿态变 量和期望飞行位置姿态变量进行初始化；

4）进入飞行控制算法部分，使用经典PID算法，结合步骤2）读取到的数据，计算得到 四个直流无刷电机的转速值，直接控制输入量；

5）DSP的PWM模块将电机转速值解算生成具有一定脉宽的PWM信号，输出至高速电 子调速器，驱动电机转动产生相应的转速，使得四旋翼无人飞行器完成自主悬停控制。

所述的对声纳数据进行卡尔曼滤波是应用Kalman基本滤波方程式，有：

P_k/k-1＝ΦP_k-1Φ^T+ΓQΓ^T

P_k＝(I-K_kH_k)P_k/k-1

$K_k=P_k{H}_k^T{R}^{-1}$

其中，为k时刻的状态估计，Q为非负定的系统噪声方差阵，P_k/k-1为一步 预测均方误差，P_k为估计均方误差，R=r,H_k=H，K_k为滤波增益，Z_k为t_k时刻的量测量， Γ＝[0 0 T]^T为系统噪声驱动阵，$\Phi =\left(\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right)$为t_k-1时刻到t_k时刻的一步转移阵，I为 单位矩阵。

步骤4）所述的计算得到四个直流无刷电机的转速值，是采用MATLAB的nlinfit函数来对 转速曲线进行非线性拟合；nlinfit函数是基于最小二乘法对模型的参数进行估计，采用牛顿迭 代算法，拟合结果为

y＝-58441x²+87159x-458741

式中，x为PWM信号占空比，y为电机转速。

本发明的基于DSP的四旋翼无人飞行器自主悬停控制系统及方法，具有如下优点：

1，硬件平台成本低，且配件易于更换。市面上的四旋翼飞行器整机不仅价格昂贵，且配 件的兼容能力很差。本发明采用市面上使用广泛、价格低廉且货源充足的配件自主集成整套 飞行器系统，既可确保飞行器满足基本的性能指标，又可保证多台飞行器在性能上的一致性。

2，充分掌握控制器的架构与算法。商业四旋翼飞行器整机往往封锁底层控制方法，不留 或只留少数接口供用户使用，给二次开发带来不便。本发明的控制器电路及器件连接为全自 主设计，便于升级改进，可使飞行器控制方法更为灵活丰富。

3，采用上下两层控制器架构。这样既保证了底层控制器的实时与高效，又能满足顶层复 杂任务的实施与交互，并有效地节省了开发周期。

4，采用非线性控制算法，有较强的鲁棒性。由于四旋翼无人飞行器的动力学模型为非线 性数学模型，采用非线性控制算法能很好地补偿动力学系统中的非线性特性，可较传统的线 性控制方法获得更好的控制效果。

实验结果表明，本发明具有成本低、结构简单、可扩展性好、控制精度高等优点。

附图说明

图1是本发明基于DSP的四旋翼无人飞行器自主悬停控制系统框图；

图2是基于DSP的四旋翼无人飞行器自主悬停控制系统结构示意图；

图3是本发明的控制流程图；

图4a是声纳传感器滤波前的原始数据；

图4b是声纳传感器滤波后的数据；

图5a是自主悬停时的滚转角（roll）变化曲线图；

图5b是自主悬停时的俯仰角（pitch）变化曲线图；

图5c是自主悬停时的偏航角（yaw）变化曲线图；

图6a是自主悬停时的滚转角速率（gyrx）变化曲线图；

图6b是自主悬停时的俯仰角速率（gyry）变化曲线图；

图6c是自主悬停时的偏航角速率（gyrz）变化曲线图；

图7a是自主悬停时的高度（z）变化曲线图；

图7b是自主悬停时的高度速率（VelocityZ）变化曲线图。

图中

1：四旋翼机体          2：螺旋桨

3：控制单元            31：主控制单元

32：地面站计算机       33：接收遥控器

34：接收机             35：信号转换模块

36：手动/自动切换芯片  37：驱动单元

311：机载主控制器      312：上位机

313：机载传感器        314：无线数传模块

315：声纳传感器        371：电子调速器

具体实施方式

下面结合实施例和附图对本发明的基于DSP的四旋翼无人飞行器自主悬停控制系统及 方法做出详细说明。

如图1、图2所示，本发明的基于DSP的四旋翼无人飞行器自主悬停控制系统，包括有 四旋翼机体1、分别设置在四个机翼上的四个螺旋桨2以及控制四个螺旋桨2旋转的控制单 元3。

1．四旋翼机体1是综合考虑飞行器机体的结构稳固性、负载能力、续航能力、可控性等 特点要求，采用四旋翼无人飞行器。机身直径为450mm，高度为111mm；空机重量为750g（不 含接收机和电池），载重重量为750g，最大安全起飞总重量为1500g；螺旋桨直径为25.5cm， 单个电机提供的最大推力为8N；电源可采用11.1V（3S）2200mAh-5500mAh的锂电池供电， 在起飞重量为1200g时，使用3S3300mAh的锂电池的飞行时间大约为12min；集成了设备承载 板，可方便地安装控制器等机载设备；最多可支持12通道的接收机，飞行至少需要4个通道， 为了便于扩展，本发明使用的是7通道的遥控设备。

作为四旋翼无人飞行器的执行器，直流无刷电机采用电子调速器（ESC）驱动。电子调 速器接收的PWM信号的工作频率设置为430Hz，周期为2.33ms，信号的高电平为2.7V-6V， 有效控制脉宽为1.1ms-1.9ms，即小于1.1ms的脉宽将不能启动无刷电机，大于1.9ms的脉 宽将被视为1.9ms，只有在有效控制脉宽范围内的PWM信号才能调节无刷电机转速。

2．所述的控制单元3包括有：主控制单元31、地面站计算机32、接收遥控器33信号 的接收机34、信号转换模块35、手动/自动切换芯片36以及用于驱动四个螺旋桨2上的直流 无刷电机工作的驱动单元37，所述的主控制单元31通过无线网络连接地面站计算机32，所 述的接收机34与信号转换模块35和手动/自动切换芯片36依次连接，将接收到的遥控器33 所发出的信号通过信号转换模块35送入手动/自动切换芯片36，用于控制驱动单元37驱动四 个螺旋桨2上的直流无刷电机工作，所述的接收机34还通过主控制单元31连接手动/自动切 换芯片36，将接收到的信号通过主控制单元31送入手动/自动切换芯片36，用于控制驱动单 元37驱动四个螺旋桨2上的直流无刷电机工作。

所述的驱动单元37包括有四个电子调速器371，所述的四个电子调速器371的输入端分 别连接手动/自动切换芯片36的输出端，所述四个电子调速器371的输出端分别对应连接四 个螺旋桨2上的直流无刷电机。

所述的主控制单元31包括有机载主控制器311，所述的机载主控制器311分别连接上位 机312、机载传感器313和无线数传模块314，所述的上位机312还连接声纳传感器315，其 中，所述的机载主控制器311分别连接接收机34和手动/自动切换芯片36用于控制驱动单元 37驱动四个螺旋桨2上的直流无刷电机工作，所述的机载主控制器311通过无线数传模块314 和无线连网络接地面站计算机32，所述的上位机312通过无线连网络接地面站计算机32。

1）机载主控制器311，作为四旋翼无人飞行器的核心组成部分机载主控制器311的选型和 设计需要充分考虑四旋翼无人飞行器的特点。因此，本发明中的机载控制芯片应满足以下要 求：供电要求简单、功耗低、体积小、重量轻、运算速度快、外设接口丰富等。本发明选择 TI公司的TMS320F28335作为机载主控制器311。TMS320F28335是一款32位的浮点DSP，主频 为150MHz；集成了快速的中断管理单元，满足实时控制的需求；片上整合了丰富的外设资源， 包括Flash存储器、快速的A/D转换器、串行通信接口、增强捕获单元、PWM模块、CAN模块、 多通道缓冲串口等。

在本发明中，作为机载主控制器311的DSP承担的任务有：运行控制算法程序；通过 RS232串口与机载传感器连接进行数据采集；通过SPI接口与上位控制器模块进行数据通讯； 利用无线数传模块与地面站进行数据通讯；输出PWM信号驱动作为四旋翼无人飞行器驱动 器的无刷直流电机；读取遥控器各通道信号，实现手自动飞行模式的切换。结合四旋翼无人 飞行器的整体结构，参照上述的各功能模块，并考虑DSP最小系统板所必须的电源模块、JTAG 模块、RESET模块、时钟源模块、启动模式模块等外围电路模块，设计制作了基于DSP的机 载主控制器模块。

2）上位机312，本发明使用了一块ARM控制器作为上位机312。主频为720MHz，采用 嵌入式Linux操作系统，存储器包含了512MB的RAM和512MB的Flash，主芯片上有WIFI 和Bluetooth两种无线通信方式，扩展板上集成了USB接口、SD卡插槽、RS232串口、I²C 接口、音频输入输出接口、HDMI视频接口等接口。作为上位机，ARM模块的主要功能是通 过I²C接口与声纳传感器连接，连续地从声纳传感器中读取数据，然后运行数据融合算法将 原始数据处理成四旋翼无人飞行器的高度信息，并通过SPI接口传输至机载主控制器。此外， 机载主控制器能够通过SPI接口向ARM模块发送四旋翼无人飞行器实时的状态信息，这些 状态信息通过WIFI传送至地面站，可实时地监测飞机的飞行状态，也能够保存下来用于实 验后续的数据分析处理。

3）机载传感器313和声纳传感器315，本发明中的四旋翼无人飞行器使用的机载传感器 是荷兰Xsens公司的MTi-G传感器，使用的声纳传感器是Devantech公司的SRF-08声纳传 感器。

MTi-G是一个集成了GPS信号的微型姿态航向参考系统，内部集成了3维加速度计、速度 计、磁力计、气压计、温度计、电子罗盘，可输出3轴姿态、航向、位置和速度等信息，俯仰 角和滚转角精度为0.5°，偏航角精度为1°。MTi-G通过RS232串口与机载主控制器DSP进行数据 通信。

SRF-08声纳传感器采用5V的供电电压，工作电流约为15mA，通过I²C接口与处理器 连接，最小测量距离为3cm，最大测量距离为6m，外形尺寸为40mm×20mm×17mm，重量 为11.3g。声纳传感器通过I²C总线与ARM模块进行数据通信，在通讯过程中，将ARM模 块设为主设备，而将声纳传感器设为从设备。

4）无线数传模块314，无线数传模块在四旋翼无人飞行器实验平台与地面站计算机间建 立了一条快捷方便的数据通道。通过无线数传模块不仅可以实现由四旋翼无人飞行器向地面 站发送飞机的状态信息，而且能够实现通过地面站在线调节四旋翼无人飞行器的飞行控制算 法参数，从而避免了频繁往机载主控制器烧写控制算法程序的麻烦，明显缩短了实验周期。 本发明选用一款半双工微型无线数传模块，分为发射端和接收端，发射端通过RS232串口与 机载主控制器连接，接收端通过USB接口与地面站计算机连接。其功能特点有：发射功率为 500mW；体积为53mm×38mm×10mm（不包括天线接头）；传输距离约为2Km；采用半双 工的通信方式。

本发明的基于DSP的四旋翼无人飞行器自主悬停控制系统的工作过程：

如图1所示，左半部分为四旋翼无人飞行器的手动遥控信号通路，由遥控信号发射机、信 号接收机和信号转换模块三个部分构成。飞行时，遥控信号发射机（即遥控器）向接收机发 送PPM形式的遥控通道信号。接收机通过译码电路将接收到的PPM信号翻译成PWM信号后， 传递至信号转换模块，最终由它转换产生驱动4路高速电子调速器的PWM信号，进而驱动直 流无刷电机转动。

如图1所示，右半部分为四旋翼无人飞行器的自动控制部分。微型姿态航向参考系统 MTi-G可实时测量四旋翼无人飞行器的姿态和位置数据。这些数据通过RS232串口发送至机载 主控制器DSP。声纳传感器数据是通过ARM模块的I²C接口读取，经过滤波处理后，将准确的 高度信息通过SPI接口发送给DSP。然后，DSP运行控制算法，并将控制量转换成能够驱动电 机的PWM信号，输出给高速电子调速器。四旋翼无人飞行器的飞行数据可通过ARM模块的 WIFI模块实现大批量快速地传送至地面站计算机进行显示和存储；也可以借助无线数传模块 实现四旋翼无人飞行器与地面站计算机之间部分数据的传输和控制参数的在线调节。

以上的手动控制信号和自动控制信号是通过CD4053芯片（图1中手动/自动切换芯片） 来进行选通切换的。

本发明的的基于DSP的四旋翼无人飞行器自主悬停控制系统的控制方法，包括：对声纳 数据进行卡尔曼滤波处理获得准确的高度数据和驱动器控制，其中，

1．所述的对声纳数据进行卡尔曼滤波是：

作为四旋翼无人飞行器的高度传感器，虽然SRF-08声纳传感器可测量得到较高精度的高 度数据，但是它在使用中容易受到干扰，测得的原始数据包含的噪声较多，误差较大，无法 直接用于四旋翼无人飞行器的高度控制。因此，本发明在ARM模块中设计了一个卡尔曼滤波 器，来对声纳传感器的原始数据进行滤波处理，可得到较为准确的高度数据。

假设四旋翼无人飞行器沿垂直方向直线运动，t_k时刻的位移、速度、加速度和加加速度 分别为s_k、v_k、a_k和j_k；只对运动物体的位置做观测，观测的采样周期为T；对运动物体的 跟踪者来说，加加速度j_k是随机量，此处取为白噪声，根据运动学方程，系统状态方程为：

X_k＝ΦX_k-1+Γj_k-1  (1)

观测方程为：

Z_k＝s_k+V_k＝H_kX_k+V_k  (2)

其中定义X_k＝[s_k v_k a_k]^T为状态向量；$\Phi =\left(\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right)$为t_k-1时刻到t_k时刻的一步转移阵； Z_k为t_k时刻的量测量；Γ=[0 0 T]^T为系统噪声驱动阵；H_k=[1 0 0]为量测阵，V_k为 量测噪声序列。

以上表达式代表了一个随机线性定常系统的滤波问题，应用Kalman基本滤波方程式， 有：

P_k/k-1＝ΦP_k-1Φ^T+ΓQΓ^T  (4)

P_k＝(I-K_kH_k)P_k/k-1  (5)

$K_k=P_k{H}_k^T{R}^{-1}---\left(6\right)$

其中，为k时刻的状态估计，Q为非负定的系统噪声方差阵，P_k/k-1为一步预测 均方误差，P_k为估计均方误差，R＝r,H_k＝H，K_k为滤波增益，I为单位矩阵。

根据滤波方程，只要给定初值和P₀，根据Z_k，就可以递推计算得到从而得到经 过滤波处理的声纳数据。式（3）和式（6）为滤波计算方程，式（4）和式（5）为增益计算 方程。其中增益计算方程是独立的，而滤波计算方程则依赖于增益计算方程。

图4为滤波前和滤波后声纳数据的对比。从图形中可以看出，声纳数据在滤波前有较多 的数据噪声，最大的噪声误差值达到了4m，误差百分率接近400%；经过滤波，噪声数据基 本被剔除，声纳数据波形光滑，滤波效果较好。

2．驱动器控制

四旋翼无人飞行器驱动器的控制方法主要有开环控制和闭环控制两种方式。开环控制的 优点是控制简单、延时小、速度快，缺点是控制精度低；闭环控制的优点是控制精度高，缺 点是延时大、结构复杂，控制方法主要有PID控制、模糊控制、神经网络控制等等。

根据本发明的控制要求，为简化控制电路设计，本发明选择开环控制作为驱动器的控制 方案。如图3所示，包括如下步骤：

1）系统初始化操作；

当程序开始运行后，首先进行是系统初始化操作，初始化的内容包括：初始化系统的控 制寄存器、锁相环、看门狗以及时钟位默认状态；配置芯片的通用数字I/O为需要的功能引 脚；初始化外设中断向量表；使用特定的函数重新分配中断向量。然后是配置相应的接口模 块的寄存器。

2）在系统完成初始化后，各接口开始与外部设备进行通信。其中，串口SCIB与机载传 感器MTi-G连接，波特率设置与MTi-G一致，为115200bps，负责读取和验证MTi-G发送 过来的姿态位置信息；串口SCIC与无线数传模块连接，波特率设置为19200bps，主要负责 通过无线数传模块的传输功能与地面站计算机进行数据通信，一方面将无人机部分的实时状 态信息发送至地面站计算机进行显示，另一方面可接收地面站计算机发送过来的航迹规划指 令和飞行算法参数调整指令；SPI接口与ARM模块进行通信，主要有两个功能，一个是获得 由ARM模块读取并经过滤波算法处理的声纳数据，另一个是将无人机全部的实时飞行数据 快速地发送至ARM模块，利用ARM模块扩展板上的SD卡进行保存，这种方式的优点是数 据传输速度快、保存的信息量大。

3）控制算法程序安排在串口SCIB中断程序内。当读取完传感器MTi-G单元和声纳传 感器的数据之后，就进入控制算法程序。首先根据各个传感器的数据和飞行轨迹规划指令， 对程序中定义的实际飞行位置姿态变量和期望飞行位置姿态变量进行初始化。

4）在完成各状态变量的初始化和坐标转换后，程序将进入飞行控制算法部分，使用经典 PID(比例-积分-微分)算法，结合之前读取到的数据，计算得到四个直流无刷电机的转速值， 这是直接控制输入量。然后，机载主控制器DSP的PWM模块将电机转速值解算生成具有一定 脉宽的PWM信号，输出至高速电子调速器，驱动电机转动产生相应的转速，使得四旋翼无人 飞行器完成自主悬停控制。

在驱动器的开环控制中，输入量为PWM信号占空比，输出量为实际电机转速，PWM信 号占空比与电机转速的关系可用电机转速曲线来表示。由于电机转速曲线是非线性的，本发 明采用MATLAB的nlinfit函数来对转速曲线进行非线性拟合。nlinfit函数是基于最小二乘法对 模型的参数进行估计，采用牛顿迭代算法。拟合结果为

y＝-58441x²+87159x-458741  (1)

式中，x为PWM信号占空比，y为电机转速。该方法的最大转速拟合误差为130rpm， 基本满足系统要求。

5）DSP的PWM模块将电机转速值解算生成具有一定脉宽的PWM信号，输出至高速电 子调速器，驱动电机转动产生相应的转速，使得四旋翼无人飞行器完成自主悬停控制。

在整个控制系统中，机载传感器MTi-G的数据更新频率为100Hz，它为内环姿态控制 器提供了角度、角速度等信息；串口SCIC与地面站数据通信频率为10Hz；声纳传感器的数 据更新频率为14Hz，为外环位置控制器提供了高度、Z轴线速度；GPS的数据更新频率为4Hz， 为外环位置控制器提供了位置和线速度。因此，内环姿态控制器的控制频率为100Hz，外环 位置控制器的高度控制频率为14Hz，外环位置控制器的水平控制频率为4Hz。

本发明基于搭建的硬件平台和设计的控制算法，进行了四旋翼无人飞行器自主悬停控制 实验，实验的内容包括姿态镇定以及高度控制，目的是要实现滚转角、俯仰角、偏航角和高 度共四个自由度的稳定控制。实验中，期望滚转角和期望俯仰角为0°，期望偏航角为173°，期 望高度为1.1m。

实验过程中，四旋翼无人飞行器的高度（Z轴位移）、Z轴线速度、三维姿态角和三维角 速度如图5a至图7b所示，数据采样周期为10ms，总飞行时间约为380s。

图5a、图5b、图5c为四旋翼无人飞行器的三维姿态角曲线，从图中可以看出，滚转角和 俯仰角的变化范围为-3°至2°，偏航角的变化范围为170°至176°。从图6a、图6b、图6c可以看出， 无人飞行器的三维角速度变化较小，基本保持在±0.4rad/s的范围内。通过分析图5a、图5b、 图5c和图6a、图6b、图6c可知，系统姿态控制器的控制效果较好，为高度控制提供了稳定的 飞行姿态。从图7a、图7b中的高度曲线可以看出无人飞行器高度的变化范围为1m至1.2m，高 度控制的误差为0.1m，垂直方向上的线速度曲线变化范围为-0.3m/s至0.3m/s。从中分析可知， 系统的高度控制基本保持稳定。

实验结果表明，四旋翼无人飞行器较好地实现了自主悬停控制，搭建的硬件平台结构较 为合理，控制算法达到了预期的控制效果。