嵌入式系统中无人机低空悬停控制程序设计

陈云飞 陈兴武 许超

（福建工程学院电子电气与物理学院 福建省福州市 350118）

小型无人机的研制与应用范围突飞猛进，以其拥有良好的灵活性和廉价等特性受到人们的广泛关注[1]。特别是在2015年无人机元年之后，中国无人机市场蓬勃发展带动了安防、军用、农业和物流产业，无人机在无人驾驶的条件下进行复杂的空中飞行任务和各类负载任务。在全国抗击新型冠状病毒期间，无人机可代替人类完成巡逻和室外消毒等危险且繁琐的工作，无人机的作用功不可没，因此在此基础上提高无人机的稳定性和可靠性具有非同寻常的意义。

飞行控制系统是一个六自由度欠驱动系统[2]，具有多变量非线性和强耦合的特性。以姿态控制为无人机控制的核心，在大部分环境中使用PID 控制算法拥有较好的控制效果。因此研究适宜多旋翼无人机飞行控制算法十分重要。

民用无人机中大部分的使用场景是航拍，国内的无人机厂商有近一半产品是航拍无人机厂商[3]。在人工智能来临之际，通过搭载带有云台的摄像头，航拍无人机可以在高空拍摄照片和视频，包括旅游航拍、三维建模以及视频录制。拥有较强的稳定性能、续航能力是航拍无人机的基本要求，目前民用航拍无人机已经得到了广泛的应用。

1 软硬件设计方案

1.1 硬件方案

国外开源飞控中主要有APM，Pixhawk， FlyingF4 等飞控[3]。其中基于Pixhawk 硬件的arducopter 无人机自动控制系统代码结构好、便于开发，是无人机开发者学习的首选。

本设计采用ST 公司的STM32F427VIT6 作为主控芯片其拥有较多的接口以及较高的主频受到人们的广泛使用。该32-bit Cortex-M4 内核芯片比较强大，具有128KB SRAM、 512KB FLASH、5 个SPI 接口、3 个 IIC 接口、1 个 USB 接口、以及 36 个通用IO 口。

基于Pixhawk 飞行控制器设计原理[5]，使用内部集成了3 轴磁力传感器，3 轴加速度传感器和3 轴陀螺仪传感器的MPU6050 芯片测量四旋翼无人机的姿态数据。通过外设接口，结合卡尔曼滤波算法对其数据进行精准的解算，从而对无人机的运动状态进行信息采集。无人机及传感器硬件连接图如图1所示。

图1：无人机及传感器硬件连接图

1.2 构建开发环境

在搭建环境过程中，使用Windows10 操作系统，CPU 为I7-9700F，显卡为RTX2060 SUPER，内存为16GB，分配100GB 存储空间从而保证虚拟机的流畅运行。并搭建虚拟机，具体配置和版本如表1所示。

表1：硬件配置

在环境搭建过程中，首先通过在Windows10 系统下使用VMware Workstation Pro 软件运行Ubuntu16.04 版本的虚拟机。在terminal（终端）从github 的arducopter 开源项目上（https://github.com/arducopter/arducopter）下载飞控程序。接着在Ubuntu 系统下的sublime 对已有的程序进行裁剪，编写。最后对程序进行编译，并烧录到无人机飞控中。

2 飞控程序设计

2.1 源码与软件初始化

Arducopter 源码由各种飞行器的主要控制代码：如旋翼机，固定翼，智能车；共享的库文件：包括姿态解算的库函数，相关控制算法（PID）的函数；硬件抽象层（方便arducopter 移植不同的硬件平台）；示例代码和文件说明和扩展接口的硬件函数库组成。

在分析arducopter 代码要从arducopter.cpp 和setup()函数着手理解。在arducopter.cpp 文件下使用loop()函数的设计框架既准确又高效。整体设计思路：首先用计时器定时触发测量；其次全部测量过程都靠中断推进；最后在main()函数里不断检查测量是否完毕。在setup()函数内部通过调用init_arducopter()函数，该函数做了一系列的初始化，以此来保障无人机在启动时可以通过自检。当setup()函数完成自检功能之后，在上位机也就是地面站系统中会提示“Ready to fly”字样。此时四旋翼无人机进入随时可以遥控起飞的状态。

2.2 程序框架与算法

常见的无人机传感器有姿态传感器，气压高度传感器和定位模块（GPS）组成[6]。使用到气压传感器MS5611 和姿态传感器MPU6050。传感器驱动程序在后端收集原始姿态数据并转换为标准单位的数据，保存到驱动器的缓存区当中，发送给扩展卡尔曼滤波（EKF）模块。

arducopter 控制的流程首先经过loop()函数后，卡尔曼滤波（EKF）模块处理传感器，加速度，陀螺仪，指南针等信息。再通过执行模式程序、控制程序、位置控制程序、姿态控制程序，最后将PWM 信号发送到硬件抽象层的电机和舵机等设备。

卡尔曼滤波（Kalman filtering）是利用线性状态方程，通过测算和比对传感器发送的姿态数据，对无人机姿态进行有效估计的算法。简单的说卡尔曼滤波的算法是对传感器状态更新（数据取加权平均）并不断进行时间更新（迭代）。主要应用于减少传感器噪声对飞行器实际飞行状态的干扰。

在常见的控制系统中，通过比例、积分和微分进行控制的PID控制器是当前应用最常见且可靠的自动控制器。PID 控制具有适用面广，公式原理简单，易于实现，各控制参数较为独立等优点。PID 控制的公式如下所示。

串级PID 控制通过测量角度和角速度，在内环使用角速度环，外环使用角度环。程序设计中AC_PosControl 类可以把XY 轴的位置P 将位置误差转换为目标速度，速度PID 信号将速度误差转换为所需的加速度信号，然后将其转换为所需的倾斜角度，最后将其发送到姿态控制库并被调用。也就是可以在无人机还没有产生较大位置偏差和角度偏差时，就及时的纠正无人机的姿态，使飞行器的稳定性更高，适应能力更强。用目标角度与实际角度得出的姿态偏差量作为输入，通过比例调节器将目标转化为实际的角速度，接着将误差通过PID 控制求和发送到电机，从而达到调节无人机姿态。使用PID 控制算法，可以先调节P 控制器再组成PI 调节器，最后组成PID 调节器。在各环节也可按照角度环PID，角速度环PID，Z轴高度环PID 控制无人机的飞行姿态，有顺序的进行调节，使无人机可以稳定的悬停。

图3：无人机悬停控制框图

Arducopter 飞行姿态控制程序结构如下（附图如图2所示）：

图2：arducopter 姿态控制流程图

（1）调用“Flight-mode.cpp”中“update_flight_mode()”函数，检查载具的种类是否属于copter（旋翼机）。

（2）调用“Mode_stabilize.cpp”中“run()”函数，将传感器采集到的信息转化为横滚，俯仰和偏航角度等参数。

（3） 调用“AC_attitudeControl.cpp” 中“input_euler_anger_rool_pitch_rate_yaw()”和“set_ throttle.out()”函数，计算姿态误差并转换为横滚、俯仰、偏航旋转速率并计算转速误差并转换为电机给定信号。

（4）调用“AP_ MotorsMatix.cpp”中的“output()”函数，将整体的电机信号发送给各个电机。

（5）调用“SRV_ Channels.cpp”中的“push()”函数，将信号按照比例缩放后发送。

（6）调用“AP_HAL:RCOutput.cpp”中的“push()”函数，将飞控发出的控制信号转换成PWM 波的形式发送给电调，电调控制电机旋转。

程序的循环执行，通过控制飞行姿态从而控制无人机在空中悬停。

基于ardupilot 控制无人机悬停有两种方式分别是Loiter 模式和Poshold 模式，两种方式差别很大，Loiter 模式控制无人机的运动加速度来实现位置控制；PosHold 模式和Loiter 模式类似，都是保持当前高度、位置和航向，不同的是PosHold 模式的摇杆直接控制飞行器偏转的角度。以Loitor 模式进行说明，无人机供电启动后首先进行初始化进行自检程序，其次当需要控制无人机位置时需要获取GPS 和气压计MS5611 的传感器信息，当需要控制高度时需要获取陀螺仪MPU6050 和气压计MS5611 的传感器信息并进行卡尔曼滤波，之后飞控处理器获取到起飞前的位置高度信息并获取到遥控器的期望值，通过与实际值对比，通过串级PID 控制从而及时的纠正无人机的姿态，最后将PWM 信号输出到电调模块，控制电机转速从而使飞行器的稳定性更高，适应能力更强。

3 测试与实验

硬件平台使用Pixhawk2.4.8 飞控模块，烧录优化后的程序，搭配X360 碳纤维机架、好盈30A 电调4 个、格式4S 3300mah 锂离子电池、2212 电机及配套的9045 电动螺旋桨，验证程序设计的合理性。

首先使用micro-USB 数据线将飞控与电脑相连接，在安装飞控相关组件和STM32 虚拟串口的程序后，电脑设备管理器中可以找到COM5 端口，此时可以进行地面站的软件测试。

QGroundControl 地面站可以为无人机提供完整的飞行控制和任务计划，其代码的开源性，方便了开发者的使用。利用此软件在本课题中可以监控飞控的传感器数据并进行程序的烧录。

通过QGroundControl 中的监控按钮MAVlink Inspector，可以检查各个传感器的当前姿态，并绘制出折线图。当调整YAW 轴时，YAW 和YAWSPEED 变化较为明显，且YAWSPEED 大小决定了YAW 轴曲线的变化速率。同样MS5611 气压传感器快速地将飞控从高处放到低处再返回到高处，也可以看到地面站上出现一条V 形曲线。使用MPU6000 六轴陀螺仪对陀螺仪参数进行检测。通过对x 轴进行旋转，可以得到xgyro 在图像上有较大的波动，而ygyro和zgyro 的变化幅度较小。同样测试加速度传感器，在短时间内迅速改变x 轴加速度，即可在图像上明显地观测到绿色折线有较大波动。在测试飞行过程中，无人机悬停姿态良好，没有异常抖动。在给予一个较大的俯仰舵量时，依然可以保持理想的飞行姿态和路线，实现了无人机低空悬停。

4 总结与展望

本文研究的是嵌入式系统中无人机低空悬停控制程序设计，基于已有的开源平台对低空悬停的无人机进行特定的内核裁剪和程序设计。包括采集无人机多种传感器信号并进行数据融合，实现无人机角速度、加速度、航向相对于地面高度的获取。解算出的俯仰横滚及横向角度信息通过嵌入式系统的飞控处理，使用串级PID 控制算法控制无人机实现无人机低空悬停。