何甘宁，李立欣，高 昂，张会生

（西北工业大学 电子信息学院，陕西 西安 710129）

基于磁传感器的串级PID自主控制四旋翼设计

何甘宁，李立欣，高 昂，张会生

（西北工业大学 电子信息学院，陕西 西安 710129）

为实现四旋翼精确高效的自主飞行，提出一种基于磁传感器的串级PID控制自主飞行系统。该系统硬件平台采用GPS接收机提供经、纬度，磁传感器提供航向角；软件上采用串级PID控制，外环为位置导航级，内环为姿态控制级。首先根据四旋翼起始点位置与预置目标点的关系得到目标航向角和航线，同飞往目标点过程中的偏航角和偏航距作为外环PID控制输入量，得到姿态的参考值，然后经内环PID控制使系统能跟随参考姿态，实时修正航线，使四旋翼以近似直线的轨迹飞往目标点。该系统具有控制精确，计算量小，飞行路径简短，飞行稳定的特点。最终以自行搭建的四旋翼为平台，验证了该自主飞行控制方法的有效性。

磁传感器；串级PID；四旋翼；自主飞行；GPS

四旋翼具有飞行稳定、自由度多等特点，广泛出现在军事、农业和民用等领域，具有很高的科研价值。同时作为一种无人机，自主飞行也是其系统技术的一项研究热点[1]。

当今，世界上对四旋翼的研究主要是针对四旋翼飞行器的自主飞行，基于惯性导航、视觉导航等[5]。文献[3]基于GPS做了四旋翼两方向同时运动的自主飞行方案，其主要优点是无需通过自旋调整航向，但两个方向速度的计算量较大，控制较为复杂。文献[4]同样是基于磁传感器控制四旋翼自主飞行，但并未涉及串级PID控制，同时摘要虽提及航线修正，但文中没有具体说明。

文中设计了基于磁传感器的串级PID控制自主飞行系统。点到点的自主飞行分为两个阶段：自旋调整航向和直线飞行。根据GPS接收机提供的经、纬度和磁传感器提供的航向角，计算出目标航向角和航线，作为外环PID控制量，计算出4个旋翼的转速，再经内环PID控制使四旋翼做自旋运动，调整航向角为绝对航向角。再以航线为基准，实际飞行中目标航向角和偏航距为外环PID控制量，得出姿态参考值，经内环PID控制，实时调整飞行路线，从而以近似直线的路径到达目标点。此系统中，四旋翼只需单方向运动，加入串级PID使控制更加精确稳定，并且飞行路线近似直线，使飞行变得更加高效。

1 系统结构

本次四旋翼硬件平台由以Atmega2560为核心的飞行控制系统、传感器系统、电机控制系统等组成，其结构如图1所示。

图1 硬件系统结构图

飞行控制系统是整个系统的核心，对传感器获取的数据进行处理，解算得到4个电机的转速，然后将此信息传送给电调，控制电机的旋转，从而实现对飞行姿态的控制。

2 自主飞行控制

2.1 数字PID控制器

模拟PID控制器的控制规律如下所示：

数字PID算法总体分为位置型控制算法和增量型控制算法[5]，本文采用的是位置型控制算法。为了实现数字PID算法，需将（1）式变成差分方程。可作如下近似

从而离散化处理得

P参数的作用是减少稳态误差，让飞机快速跟踪输入；D参数的作用是加快系统响应，减小超调量，增加稳定性，防止飞机左右晃动；I参数的作用是可以消除静态误差，防止位置误差的累积[8]。

2.2 自主航向控制

如图2所示，四旋翼从A点到B点自主飞行时，欲将机头朝向目标点B，则需根据A点朝向和A、B两点的位置关系，得到自身需要旋转的角度α，即目标航向角。图中X轴正方向指向地理的正东，Y轴正方向指向地理的正北，点A在坐标轴原点。

图2 四旋翼从A到B飞行

而目标航向角α的确定是四旋翼能自主且准确地到达目标B点的关键，为达到精确控制，采用双闭环PID控制。外环PID控制器为位置导航级PID，根据外围传感器数据，计算得到四旋翼该如何飞行；内环PID控制器为姿态控制级PID，以外环输出和自身闭环回路的姿态反馈作为输入控制电机，使四旋翼按导航级计算得到的飞行计划调整飞行。

四旋翼当前A点的经、纬度及其机头方向θ可以由GPS模块（内置磁传感器）获得，目标B点的经、纬度事先写入四旋翼的DataFlash中。然后由A、B两点的经纬度的差值，得到B点相对A点的坐标，求出绝对航向角β及目标航向角α。然后通过串级PID算法求得对每个电机的调整量，传递给四个电机，从而改变电机转速使四旋翼自旋一个α角度，实现对四旋翼航向角的双级闭环回路精确控制，具体框图如图3所示。

2.3 直线飞行的航向修正

实际飞行中，要使四旋翼以近似直线路径飞行，就必须根据偏航距和当前航向角对飞行路线进行修正。

如图4所示，C点偏离预定航线，α′为当前目标航向角，d是C点的偏航距。将作为外环PID控制器的输入量对航向进行修正，其中α′+d×ξ为偏航修正因子，得到一航向参考值，再由内环PID控制整机姿态，使四旋翼的位置慢慢向预定航线靠拢，从而实现四旋翼直线地飞往目标点。

图3 双级闭环PID控制航向

图4 航向修正示意图

3 系统测试

四旋翼经遥控器切换模式到自主飞行模式后，读取事先存储的目标点的经、纬度和高度信息，然后根据当前自身与目标的位置关系，规划出路径，从而能自主地飞行至目的地。

具体的测试结果如图5所示，（a）中四旋翼处于悬停状态，到图（b）时航向做了一个角度的调整，使其朝向目标点。 （b）（c）（d）图是四旋翼的直线飞行过程，可以看出四旋翼飞行稳定。

图5 路径规划测试

图6是由实际飞行获取的位置数据所描绘的航线图，自主飞行过程中，实际航线与预定航线几乎重合。此外，从图7可以看出，四旋翼的偏航距从水平飞行最开始的35 cm慢慢缩小在-10 cm和10 cm之间，可见利用磁传感器和串级PID可很好地控制四旋翼直线飞行时的航向。但当快到达一个目标点时，会有较大的偏差，可能原因是临近目标点时，利用GPS的位置关系求取目标航向角，角度偏差变大，从而导致航线误差变大；当悬停转向时，四旋翼因外部环境因素使得其位置一直处于变动状态，使得在目标点处有许多不规则的轨迹数据。

图6 实际与预定航线对比

图7 偏航距的收敛

4 结 论

文中首先介绍了系统的硬件平台——四旋翼的系统结构，其具体的组成部分及功能。然后设计了一种基于磁传感器，并利用串级PID的双闭环控制自主飞行方法，最后以自行搭建的四旋翼为平台对自主飞行方法做了验证，结果表明四旋翼有很好地自主飞行能力，而且飞行稳定，实际飞行路线近似直线，缩短了飞行路程，提高了飞行效率。

参考文献：

[1]颜平，王丽丹，李梦柯，等.基于STM32的四旋翼飞行器设计与实现 [J].电子设计工程，2016（2）:187-189.

[2]黄军友.基于PID控制的X模式四旋翼飞行器研究[J].电子设计工程， 2014，22（2）:127-129.

[3]马浩.基于Backstepping的多旋翼无人机自主飞行控制系统研究[D].南京：南京信息工程大学，2014.

[4]钟丽娜，王君浩，王融.基于磁传感器的四旋翼飞行器自主导航设计 [J].重庆理工大学学报:自然科学版， 2013，27（12）:86-90.

[5]冯庆端，裴海龙.串级PID控制在无人机姿态控制的应用[J].微计算机信息， 2009，25（22）:9-10.

[6]贾峰，孙曼.基于STM32的四旋翼飞行姿态串级控制[J].科学技术与工程， 2015，15（6）:220-225.

[7]王璞，邓炜烽.四旋翼飞行器双闭环串级PID姿态控制算法 [J].开封教育学院学报，2015，35（9）:91-93.

[8]吴友谦，裴海龙.基于Dubins曲线的无人直升机轨迹规划[J].计算机工程与设计，2011，32（4）:1426-1429.

[9]杜浩.基于GPS/INS的多旋翼MAVs自主飞行系统研制[D].南京：南京信息工程大学，2012.

[10]张垚，鲜斌，殷强，等.基于ARM处理器的四旋翼无人机自主控制系统研究[J].中国科学技术大学学报， 2012，42（9）:753-760.

[11]叶文伟，刘琨，李中健.基于ARM内核单片机的四旋翼直升机飞行控制系统设计[J].电子设计工程， 2013，21（13）:79-82.

[12]夏伟，邓静.无人直升机自动航线飞行控制律设计与仿真[J].直升机技术，2013（3）:21-25.

[13]杨立本，章卫国，何墉，等.一种自主四旋翼飞行器控制系统结构研究[J].飞行力学，2016，34（1）:59-62.

[14]甄红涛，齐晓慧，夏明旗，等.四旋翼无人直升机飞行控制技术综述 [J].飞行力学，2012，30（4）:295-299.

[15]罗诚.无人机路径规划算法研究[D].上海：复旦大学，2010.

Design of cascade PID autonomous control four-rotor based on magnetic sensor

HE Gan-ning，LI Li-xin，GAO Ang，ZHANG Hui-sheng
（School of Electronics and Information，Northwestern Polytechnical University，Xi’an 710129，China）

In order to realize the autonomous flight of the four-rotor accurately and efficiently，a cascade PID control autonomous flight system based on magnetic sensor is proposed.The hardware platform of the system uses GPS receiver to provide the latitude and longitude，and the magnetic sensor provides the heading angle.In the software， the cascade PID control is adopted， the outer loop is the position navigation stage， and the inner loop is the attitude control stage.First， according to the relationship between the starting point position of the four rotor and the preset target，the target heading angle and route are obtained，with the Yaw angle and yaw pitch in the process flying to the target point as outer loop PID control input.Get the reference value of attitude，and then through the inner loop PID control to make the system can follow the reference attitude， real-time correct the route， so that the four-rotor canfly to the target point in the approximate straight-line trajectory.The system has the characteristics of accurate control， small amount of calculation， short flight path and stable flight.Finally， to verify the effectiveness of the autonomous flight control method，a four-rotor is built.

magnetic sensor；cascade PID； four-rotor； autonomous flight；GPS

V216.8

A

1674－6236（2017）12-0068-04

2016-07-18稿件编号：201607132

陕西省自然科学基础研究计划面上项目（2016JM6062）；中国航天科技集团公司航天科技创新基金资助项目；西北工业大学研究生创意创新种子基金（Z2016106）

何甘宁（1992—），男，浙江金华人，硕士研究生。研究方向：通信与信息系统。