基于UWB定位的蚁群无人机的设计

张耀金 黄沁颖 李子韩 刘宇健 孔文迪 王月

1研究背景与意义

纵观国际层面，四旋翼无人机已经成为世界范围内研究的标准平台，研究进度十分超前。在军用领域，无人飞行器由于可以代替飞行员执行战场紧急危险任务以及执行超越人类生理极限的任务。在民用领域，无人机在农业植保、环境保护、国土监测、科学考察、气象探测、应急抢险、公共安全等领域有着丰富的需求。

2四轴飞行器控制原理

2.1 PID控制算法

四轴飞行器姿态的控制用到 PID 控制算法，本文为实现飞行器姿态控制算法采用串级 PID 控制方法，其本质是根据期望值和实际值的偏差计算出电机的控制量，PID控制器主要由比例单元、积分单元和微分单元组成。

（1）比例控制算法

比例控制算法利用实际与期望的差值乘增益来影响输出值，其公式为：

（2）积分控制算法

积分控制算法利用实际与期望的差值的累加和乘增益来影响输出值，其公式为：

（3）微分控制算法

微分控制算法利用实际与期望的差值的变化快慢乘增益来影响输出值，其公式为：

根据以上三种算法，可以得到PID控制的表达式为：

2.2四轴飞行器的算法

2.2.1欧拉角

欧拉角是用来唯一地确定定点转动刚体位置的三个一组独立角参量，由章动角θ、进动角ψ和自转角φ组成。本文将yz轴作为参考系的参考轴，并且将xy平面和XY平面的交线称作为N。

2.2.2四元数

與三维旋转矩阵相比，四元数可以更好的给出旋转角度和旋转轴。因此，三维旋转用四元数来表示。若要将一个复数旋转任意角度，只用乘以另一个复数，乘数与实轴的夹角角度为被乘数要旋转的角度。四元数，就是有四个变量的数，其基本公式为：

3四轴飞行器硬件组成

3.1总体设计方案

四轴飞行器由两个MCU共同控制，它的任务是控制四旋翼无人机完成的基础飞行中的各个任务，在飞行控制板上有很多扩展接口，它可以让飞行器对多个环境适用。

3.2主控MCU

主控器是一个系统的核心单元，本文采用的飞行器主控MCU为STM32F411CEU6。

姿态传感器主要由加速度计、陀螺仪、磁力计和气压传感器组成，其中3轴加速度传感器、3轴陀螺仪传感器和3轴磁力计集成于九轴传感器芯片MPU9250。

气压传感器BMP280是BOSCH公司生产的一个高精度低功耗数字气压传感器，四轴飞行器的通信芯片选择NRF51822，并且选择一颗为RFX2401C的功率放大芯片。功率放大芯片RFX2401C是集成电路RF单片机。

4四轴飞行器软件原理

4.1 NRF5822程序框架

NRF51822用来控制四轴飞行器无线通信和管理电源。

esb.c是四轴飞行器的无线驱动代码，system.c是四轴飞行器系统滴答定时器驱动代码，uart.c是飞行器的串口驱动代码最下方的button.c是飞行器的按键驱动代码，pm.c是飞行器的电源管理驱动代码。

无线通信的过程是，遥控器发送的数据包通过radiolink.c发送给飞行器的main.c，之后main.c接受数据包并对其进行解析，若发送对象不是NRF51822就将数据包经过uartlink.c发送给STM32F411。当STM32F411接收到一条来自uartlink.c的数据包时。则返回一条数据包通过uartlink.c给main.c，最后main.c再经过radiolink.c转发给遥控器，通过以上步骤可以发现NRF51822起到了重要的通信桥梁作用。

4.2 STM32F411程序框架

radiolinkTask无线通信任务，将接收NRF51822发送的串口数据并打包为ATKP格式发给atkpRxAnlTask的接收队列中，与此同时回发一帧数据给NRF51822。

usblinkRxTask USB通信接收任务，此任务接收从上位机通过USB虚拟串口的方式发送的数据，将数据包发送给atkpRxAnlTask的接收队列中。

atkpRxAnlTask是接收ATKP数据包并处理，这项任务重要地负责来自上位机和遥控器发送的数据包的处理工作。

stabilizerTask是所有任务最终的实现单元，代表着控制四轴飞行器的平衡飞行。

wifilinkTask代表手机控制任务，此任务通过接收Wifi摄像头模块的数据，对于Wifi摄像头模块相应的通讯协议来将数据解析为相对应的控制指令发送给stabilizerTask。

atkpTxTask代表的是ATKP数据包的发送任务，这个任务是将stabilizerTask中获取到的传感器、PWM输出数据和姿态数据等进行收取发给usblinkTxTask和radiolinkTask，即发送给上位机和遥控器。

usblinkTxTask代表的是usb通信的发送任务，将atkpTxTask要发送的姿态数据和传感器数据等数据包发送给上位机。

4.3姿态解算和PID算法流程图

四轴飞行器有两个相异的控制方法，分别是常用的手动模式和定高模式。手动模式中，四轴飞行器的油门值与设定油门值相等。定高模式中，四轴飞行器的控制油门会被Z轴速度替换，由于本文所设计的四轴飞行器配备了一颗高精度气压计BMP280，通过该模式可以使飞行器在空中定高巡航，对气压传感器数据和垂直地面的速度数据进行叠加获得高度的实际测量值。高度的设定值是对Z轴设定值进行积分得到的，同样将其与实际值做差运算，得到的目标量作为输入进入Z轴高度PID控制器，输出值即为油门变化量，变化量与油门基础值的和为油门实际值。

4.4 Z轴高度环PID

真实油门值由两部分合成，油门的基本参考值确定了四轴飞行器在固定高度飞行时脱离控制的油门大小，它可以为四轴飞行器的空中悬停起很大的帮助，并且对四轴飞行器固定高度的状态有影响。

4.54D空翻算法与基础原理

实现4D空翻的主要秘诀为内环PID。将姿态角度的设定值当做角速度换的设定值，这是控制翻滚的基础。

5实验结果分析与展望

实验所需的飞行器可采取串口方式与匿名科创地面站通信，进而调试参数和PID系数。本文使用两条白色线将四轴飞行器拴在室内黑凳子的扶手处，扶手高度足够高已保证四轴飞行器任何情况下不与凳子接触。另一端栓在固定电池的排针处。

本文提起的四轴飞行器可以随意使用传感器模块并转换不同工作模式，从而让飞行器对多种工作环境都有极好的适应性。本文可以通过多个环境监测的传感器，实时对飞行器周围环境做出监测，并根据不同监测的环境自动转换最适合的工作模式以加强飞行器的实用性。当前市面上现有的小型四轴飞行器机体较轻，导致抗风性很差，容易造成事故，所以，在不损失飞行器飞行稳定性的情况下将四轴飞行器机体重量变小和降低它对环境的危险系数是未来研究的重点和难点。

本文系中国矿业大学（北京）“大学生创新训练项目”《基于UWB定位的蚁群无人机设计》（项目编号： C201902444）的成果。