基于GPS与AHRS的四轴飞行器悬停算法研究_孙骅

2013年第32卷4月第4期机械科学与技术

Mechanical Science and Technology for Aerospace Engineering April Vol．322013No．

4

基于GPS 与AHRS 的四轴飞行器悬停算法研究

孙

孙

骅

骅

（广州大学机电工程学院，广东510006）

要：通过对国外四轴飞行器控制系统设计思路的研究及其存在问题的分析，就飞行器控制系统

悬停部分的算法进行了研究与探索，并对该系统的设计做了总结与展望。其中电子航向系统摘

(AHRS ) 与全球卫星定位系统(GPS ) 为关键参数获取的传感器，该系统实现了飞行器悬停的全自动外环控制。

词：四轴飞行器; 飞行控制系统; 自动控制; 旋翼; AHRS ; GPS

8728（2013）04-0481-07中图分类号：TP368. 1文献标识码：A 文章编号：1003-关

键

Study on Hover Algorithm for Four Axle Vehicle Based on GPS and AHRS

Sun Hua

（School of Mechanical and Electrical Engineering ，Guangzhou University ，Guangzhou 510006）

Abstract ：Through studying the design ideas of the foreign four-axle vehicle's control system ，a preliminary explo-ration about the hovering algorithm and a brief summary of the system's design were described in this article．AHRS and GPS ，which are sensors for measuring key parameters ，realize the automatic control of vehicle hovering and lay a basis for full self-control flight．

Key words ：four-axle vehicle ；flight control systems ；automatic control ；rotors ；attitude heading reference system ；

global position system

近年来，飞行器不仅广泛用于民用事业和满足

还被利用于进行太空操作和实验等。对国防需求、

飞行器的研究目前主要包括固定翼、旋翼、扑翼这3

种形式。蝶形飞行器属于旋翼的一种，相对于别的旋翼式飞行器来说蝶形飞行器由于能够共享电池、控制电路板等，因此结构更紧凑，能产生更大的升力，而且可以通过反扭矩作用使飞行器扭矩平衡，而不需要专门的反扭矩浆。国内对蝶形飞行器的研究起步较晚。其中四轴飞行器也就属于蝶形飞行器的一种。

四轴飞行器具有4个旋桨（其结构简图见图1所示）。两两相对呈十字交叉结构，桨的转向分正转和反转，这样可抵消模型自身的旋转。4个桨转动时的离心力是分散的，不像直机的桨，只有一个能产生集中的离心力形成陀螺性质的惯性离心力，保

05-06收稿日期：2011-作者简介：孙

骅（1958－），副教授，硕士研究生，研究方向为机械alisasun@gzhu．edu．cn 设计与创新，

持机身平稳使之避免翻到

。四轴飞行器为了能够快

以应对姿态变化引起的飘移，需要高反应速速反应，

I2C 总线接度的电子调速器，因而用高速专用电调，

可达到每秒几百上千次的电机转口传送控制信号，

速变化，在四轴飞行时，姿态时刻能够保持稳定，即

使受到外力突然冲击，依旧保持平稳。

图1四轴飞行器飞行原理

482机械科学与技术第32卷

上述的四轴飞行器内部控制（内环控制）是一套平行系统，可以大大的简化操作者的操作强度与提高四轴飞行器的稳定性。而要实行完全的自动控

尤其是自动悬停，就必须要有第二环的外部控制制，

（即外部智能控制系统）。四轴飞行器的外环控制，是独立于内环控制的额外添加的控制系统，而这要求用更多高质量的传感器采集更多可靠参数。而这对其质量体型细小、动力有限、控制过于灵敏、难以平衡的四轴飞行器来说，是不可能带上沉重而宽大的设备飞行的，只能够尽量减少设备的重量、体积与数量。这样就与其本身结构具体的情况出现矛盾。

在此经过大量的资料翻查以及寻案提问，决定利用体型微小且质量也很小的微型设备，如民用全球卫星定位系统（Global Position System ）、电子航向系统（Attitude Heading Reference System ）和超声波测距仪等定位仪器进行定位数据采集，把采集的数据利用地面处理器（MCU ）处理成与舵机所读信号相符的数据，通过数传模块把数据传输到四轴上，再经过四轴的内部控制环处理后控制四轴飞行器电调，使之做出要求的动作。本文中就如何使用这种方法来处理四轴飞行器定点飞行与悬停的问题进行了研究。11. 1

四轴飞行器的悬停与有地效悬停四轴飞行器的飞行原理

具体四轴飞行器的有地效升限高于无地效升限的原因。一般情况下，四轴飞行器在起飞和着陆过程中的悬停均为有地效悬停2

［1，2］

。

四轴飞行器的控制系统

经过实验和分析，决定利用体型和质量较小的微

［3］

型设备，如民用全球卫星定位系统（Global Position Sys-tem ）、电子航向系统（Attitude Heading Reference Sys-tem ）和超声波测距仪等定位仪器进行定位数据采集，把采集的数据利用地面处理器（MCU ）处理成与舵机所读信号相符的数据，通过数传模块把数据传输到四轴上，再经过四轴的内部控制环处理后控制四轴飞行器电调，使之做出要求的动作。本系统主要处理四轴飞行器的定点飞行与悬停。GPS 进行坐标定位，AHRS 传感器收集姿态及航向数据进行姿态修正，超声波测距仪收集高度数据进行定高，与此同时而最需要的是有一套完善的控制系统算法与之相结合，从而实行完全的自动控制，尤其是自动悬停，故必须要有一套外部智能控制系统。本文中就其问题的解决方法：设计一套能够将GPS 与AHRS 融

［4 7］

。合一起，进行外环控制操作的算法2. 1

控制系统的模块组成

系统硬件模块见图2（即外环飞行控制结构图），

［3］

四轴飞行器飞行原理与直升飞机类似，都属于

垂直起降旋翼飞行器类型。则不同之处为旋翼数量没有尾桨，且都为单层桨，其扭矩的平衡是有4个，

通过各浆转速灵敏的调节来实施。其飞行动作为横滚Roll （左、右平移飞行），俯仰Pitch （前、后平移飞行），航向Yaw （左、右旋转飞行）。1. 2

四轴飞行器的悬停与有地效悬停

在航空领域，悬停是指航空器在一定高度上保

整个控制系统包括电源功能模块、遥控接收模块、内部

AHRS 外部传感模块、GPS 外部传感模控制传感模块、

MCU 及接口与扩展等部分

。电机驱动模块、块、

持空间位置基本不变的飞行状态。悬停是直升旋翼

飞机最主要的飞行特色，也是直升机的最基本科目，因为不管是从事巡航飞行、搬运还是其它任务，绝大

，部分飞行都是“起于悬停、止于悬停”而四轴飞行器也是属于直升旋翼飞机类别，故悬停是四轴飞行

器的重要要动作之一。悬停可分为两类，一种为有另一种为无地效悬停。所谓地效是指地地效悬停，面效应，即当四轴飞行器距离地面比较近时，其旋翼下洗气流会受到地面的影响，地面阻碍造成的流场变化使旋翼消耗的诱导功率（用于加速流经旋翼的空气）减小，也就是说此时四轴飞行器保持悬停所需功率要小于没有地面效应的时候。所以在有地效时，四轴飞行器能在更高的高度上保持悬停，这也是

图2四轴飞行器控制系统构成图

第4期孙骅：基于GPS 与AHRS 的四轴飞行器悬停算法研究

［3］

483

2. 2

控制系统全自动导航理想全过程介绍

理想中的导航可以分为两个过程：1）向目标点

飞行过程；2）到达目标点后的悬停过程。

整个过程除了起飞与降落可以有人为的干预，而在飞行器稳定飞行过程中，如打开自动驾驶模式将全自动控制。

2. 2. 1向目标点飞行过程

1）方向控制：MCU 利用GPS 中的坐标数据输出，算出该时刻所在坐标与目标坐标方位差、以及轨迹方向。MCU 再利用AHRS 中的方向数据输出，算出现时刻的方向。这时候通过对Yaw （Z 轴旋转）的控制，使四轴飞行器方向锁定于轨迹方向。

2）平移控制：当方向锁定后系统将对四轴飞行器加以（X 轴平移）的方向控制，给予正方向动力，使之前进。MCU 利用GPS 中的速度数据输出，算出该时刻飞行速度。通过在程序中设定最高速度（大约不超过10km /h），使之控制在一定速度范围内，以免系统无限制加速导致前倾角过度而坠毁。

3）高度保持：在整个平移系统中，四轴飞行器都会利用超声波测距仪作为定高装置，将该仪器输出的该时刻相对高度数据与程序设定的目标高度作比较，通过MCU 计算出油门差量，使飞行器到达目标高度。

4）悬停跳进：在整个飞行过程中1） 3）步骤会以无限重复执行。但当GPS 探测到四轴飞行器已经到达目标坐标点时，系统将自动取消“向目标的循环程序运行，而自动跳进并运行地飞行过程”“悬停过程”循环程序。2. 2．2

到达目标点后的悬停过程

利用GPS 给定目标坐标，在可修正范围内进行修

图3

传感器数据输出示意图

由于计算机控制是一种采样控制，于是必须对

积分和微分进行离散法处理。离散法处理是以：T k 作为采样序号，作为采样周期，则离散采样时间kT

对应着连续时间t ，用求和的形式代替积分，用增量的形式代替积分。

2

Δu k =K p （Δe k +ie k +Δe k ）=

K p Δe k +K i e k +K d Δ2e k

U k =u k －1+Δu k

（1）

k =0，1，2，…；U k 为第k 次采样式中：k 为采样序号，

时刻的计算机输出值；e k 为第k 次采样时刻输入的偏差值；e k －1为第k －1次采样时刻输入的偏差值；K p 为比例系数；K i 为积分系数；K d 为分析系数；Δe k =e k －e k －1；Δ2e k =Δe k －Δe k －1。

工程实际中，应用最为广泛的调节器控制规律为比例（Proportional ）、积分（Integrating ）、微分（Dif-PID ferentiation ）控制，简称PID 控制，又称PID 调节，

［8］

控制程序框见图4所示。

正。AHRS 实测该时刻的方向角与姿态，再通过较为复杂的三角函数运算出三轴的反方向加速度在俯仰和横滚方向上的分量，具体算法见下分析。（由于篇幅所限，本文只讨论导航系统的悬停部分，而忽略其他

）部分。3

利用AHRS 与GPS 实现四轴飞行器自动悬停系

［8］

统的算法

3. 1利用AHRS 与GPS 实现悬停定位的PID 控制利用GPS 的经度、纬度数据，以及如图3所示，

AHRS 的航向（Yaw ）、横滚（Roll ）、俯仰（Pitch ）等数据，将这些数据经过处理得出偏差值，偏差值通过PID 控制处理后，产生横滚，俯仰，航向这3个初级控制量。然后将3个初级控制量与遥控器的输入量融合后，再与现实姿态作差，从而产生新的偏差值，进行第二次PID 控制处理，得出最终的控制量，来实现悬停

。

图4

PID 控制程序框图

484机械科学与技术第32卷

3. 2

理想控制量的获得

四轴飞行器的悬停：指飞行器在某一地理定点

3. 3

理想控制量的分解实际运用中，并不能够直接得到两位置之差。则

上，离地面一定高度悬空停留飞行。要保持飞行器

就必须要通过传感器采集即时飞行姿态位的悬停，

置等参数，当飞行器实际位置L t 偏离出目标悬停位

置L 0有偏差时，飞行器必与目标产生距离偏差值L 0=L 0－L t ，偏离目标点越远则L 0值越大，故此时飞行器需要一个指向目标悬停位置点的偏移力修正飞行器到目标点。通过对四轴飞行器进行受力分析可知（图5）：这个力的产生可以通过改变飞行器的姿态，使之倾侧与目标悬停位置，倾侧角度为Φ

。

利用GPS 的实测坐标与目标坐标相对比较后，计算其

产生的经度差ΔLon 与纬度差ΔLat （见图7分析）。定义GPS 输出的实测坐标为经度：Lon GPS ，纬度：Lat GPS 。

定义GPS 输出的目标坐标数据为经度：Lon 目标，纬度：Lat 目标。

则经度差为

ΔLon =Lon 目标－Lon GPS 纬度差为

（5）ΔLat =Lat 目标－Lat GPS

对于四轴飞行器的控制，要产生某一个控制量，必须将该控制量分别分解到横滚（左右平移）。控制与俯仰（前后平移）控制上，而四轴飞行器的方向是随机变化的，横滚、俯仰方向不一定与经线、纬线重合。故需要把经度差与ΔLon 纬度差ΔLat 分别都分解到横滚、俯仰方向上去

。

（4）

图5四轴飞行器受力分析图

将Φk 值作为产生合力的输出控制量。

L 作为目标位置与现时刻位置的差值，则输出控制量Φk 与L 满足以下PID 控制关系

k

Φk =K p ·L +K i ·∑L k +K d ·（L k －L k－1）（2）

i =0

则输出控制量Φk 是由传感器中读取参数运算得出的理想控制量，利用该控制量实现：当四轴飞行器初始姿态为水平，受力平衡且没有运动时，对应输出一个能够在理想状态下完成悬停的控制量。而在实际情况中，却会出现很多变化因素。例如当有风等大气运动的情况下：四轴飞行器出现倾侧、漂移时，则理想控制量Φk 与实际的姿态倾侧量Φ之间产生一个新的偏差值Φ偏差k ，这个值与最终输出控制量Φfinalk 满足以下PID 控制关系，即

k

图7

四轴飞行器与经纬度关系图

Φfinalk =Kp ·Φ偏差k +K i ·∑Φ偏差k +

i =0

K d ·（Φ偏差k －Φ偏差k －1）

图6为该状态下PID 控制系统图解

。

（3）

图8偏移量分解图

从图8分析得出：

ΔLon 与ΔLat 在横滚方向上的分解量L roll 为L roll =ΔLoncos θ+ΔLatcos θ（6）

ΔLon 与ΔLat 在俯仰方向上的分解量L pitch 为L pitch =ΔLonsin θ+ΔLatsin θ（7）通过以上分析计算，确定实际坐标与现实坐标

图6PID 控制系统图解

的偏差量在横滚、俯仰方向上的分解量L roll 与L pitch

的具体数值。这两个偏差量就是四轴飞行器在实际飞行中，对横滚、俯仰的控制偏差量。

第4期孙骅：基于GPS 与AHRS 的四轴飞行器悬停算法研究485

3. 3. 1

GPS 横滚控制量算法［8］

GPS 横滚控制量算式

ErrRoll GPS =L pitch =ΔLonsin θ+ΔLatsin θ（8）根据PID 计算GPS 横滚控制量得

Roll GPS =K p ·ErrRoll GPS +

k

该实际偏差量依然需要分别由横滚、俯仰两个方向的控制来实现。于是，以下对实际偏差量的具体算法将同样分为横滚与俯仰两部分。3. 4. 1实际横滚控制量［8］

根据PID 控制算法，计算最终横滚输出控制

（9）

量为

k

K i ·∑IRoll' GPS +Kd ·DRoll GPS

i =0

式中：Roll GPS 为GPS 横滚控制量，取值范围：（－20，

20）；ErrRoll GPS 为GPS 横滚控制偏差量；IRoll GPS 为GPS 积分偏差量。

IRoll GPS =ErrRoll GPS J ITG 范围（－500，500）

（10）

DRoll GPS 为GPS 微分偏差量

DRoll GPS =（ErrRoll GPS －ErrRoll GPSk －1）/JITG

（11）

3. 3. 2

GPS 俯仰控制量算法［8］

根据PID 计算GPS 俯仰控制量得

Pitch GPS =K p ·ErrPitch GPS +

k

Roll 控制=K pQ ΔRoll +K iQ ∑IRoll +K dQ DRoll

i =0

（16）

式中：Roll 控制为横滚最控终制量；K pQ 为比例系数；K iQ

K iQ =K pQ =K pQ ·T /Ti ；K dQ 为微分系数：为积分系数，

K dQ =K pQ D =K pQ ·T d /T。由试验得：K pQ =1. 6，K iQ =0. 4，K dQ =0. 36；ΔRoll 为横滚控制输出偏差量；IRoll 为横滚输出控制积分偏差值；DRoll 为横滚输出控制

微分偏差值；Roll 手动为手动横滚控制量（遥控器输出量）；Roll 为实测横滚角度。3. 4. 2实际俯仰控制量［8］

则根据PID 控制，计算最终俯仰输出控制量为

Pitch 控制=K pQ ·ΔPitch +

k

K i ·∑IPitch GPS +K d ·DPitch GPS

i =0

（12）

式中：Pitch GPS 为GPS 俯仰控制量，取值范围为（－20，20）；K p 为比例系数；K i 为积分系数，K i =K p I =K p ·T /Ti ；K d 为微分系数，K d =K p D =K p ·T d /T。600D 四轴飞行器说明书参数表格得查X-K p =0. 035，K i =0. 01，K d =0. 02。GPS 积分偏差量

IPitch GPS =ErrPitch GPS J ITG 范围：（－500，500）

（13）

DPitch GPS 为GPS 微分偏差量

DPitch GPS =（ErrPitch GPS k －ErrPitch GPS k －1）/JITG

（14）

3. 4

最终输出控制量算法

综上分析所得算法，仅为理想状况下GPS 的控制量。而实际操纵中，则需将此控制量与实际情况

［8］

［2］

K iQ ·∑IPitch +K dQ ·DPitch

i =0

（17）

式中：Pitch 控制为俯仰最终控制量；ΔPitch 为俯仰控

制输出偏差量：ΔPitch =（Pitch 手动+Pitch GPS ）－Pitch 30）；IPitch 为俯仰输出控制积分偏差范围（－30，

50）；DPitch 为俯值：IPitch =Pitch ·J ITG 范围（－50，

仰输出控制微分偏差值：DPitch =Pitch 手动·K QCP ；Pitch 手动为手动俯仰控制量（遥控器输出量）；Pitch 为实测俯仰角度。3. 5

最终输出控制量的稳定航向补充算法综上所述其最终输出控制量的计算，应将整套

［8］

：

ErrPitch GPS 为GPS 俯仰控制偏差量，IPitch GPS 为

四轴飞行器基于AHRS 与GPS 自动悬停算法完成，才能够完成对飞行器两个平移自由度（前后、左右方向平移）与两个旋转自由度（俯仰、横滚动作）的悬停自动控制，并且要求在第3个自由度（Z 轴）没有人工控制下同样能够达到悬停自动控制。由于飞行器航向在这一自由度并没有实现自动控制，故它需要通过人手控制去维持固定航向，故在此特补充关于稳定航向的具体算法。由于它只与现时刻的航

则只与AHRS 中采样的航向量向与目标航向有关，

有关，而与GPS 采样的位置坐标量无关，故与第3. 4

节所提及的变量无关。3. 5. 1

理论算法

当有人工输入的航向控制量H 手动k 时，则需要将H 手动k 与实际的倾斜角度H 实际k 作差，得出最终输出控制量的偏差量H 偏差k ，该偏差量应该满足以下比较式

作比较所产生的偏差量，再由PID 控制算出其最终

控制量来作为最终实际控制量。

现场实际情况控制量为：操控员遥控器的手动输入控制量；飞行器在即时刻的姿态倾斜量。当有人工输入的控制量Φ手动时，则需将GPS 输出的理性控制量Φk 与Φ手动k 相加，得到总控制量。然后再把总控制量与实际的倾斜角度Φ实际k 作差，得出最终输出控制量的偏差量Φ偏差k ，该偏差量应该满足以下比较式。

Φ偏差k =（Φk +Φ手动k ）－Φ实际k

（15）

486机械科学与技术第32卷

H 偏差k =H 手动k －H 实际k

3. 5. 2

具体算法

ΔYaw 计算=Yaw 手动－Yaw

（18）（19）

定义：Yaw 控制为航向控制量；Yaw 为航向控制输出偏差量；IYaw 为航向输出控制积分偏差量；Dyaw 为航向输出控制微分偏差量；Yaw 手动为手动航向控制量（遥控器输出量，实测航向角度Yaw ）。360），航向测量值Yaw 计算实际取值：（0，运算

Yaw 取值为（－180，180）之间，中，故作以下计算调整：

当Yaw 计算＞180时：Yaw =360－Yaw 计算；

当Yaw 计算≤180时：Yaw =360－Yaw 计算；且Yaw

60）；取值范围为（－60，

IYaw =Yaw ·J ITG 范围（－50，50）；DYaw =Yaw 手动·K QCP

则根据PID 计算其最终航向控制量为

k

图9四轴飞行器MQ120

以下是该四轴飞行器参数：

机身尺寸：120cm ˑ 120cm ˑ 30cm 整机重量：1500g 最大起飞重量：2. 5kg

最大任务载荷：800g

电机：4208直流无刷电机KV750桨：1245塑胶正反桨

机架：玻璃纤维+铝合金CNC 脚架：塑胶

电池：2200mAh 3S

控制器：MMC10自动平衡控制系统

NAVI 导航板：NAVI 控制器+GPS 模块+COMPASS 模块

本系统利用Atmega644作为主控芯片，实时采样和处理AHRS 与GPS 的数据并通过无线传回地面站作为监测数据，实验设定采样频率为100Hz ，采样时间为50s ，目标设定为横滚角（Roll ）、俯仰角（Pitch ）和航向角（Yaw ）均为0，悬停坐标经纬度分别为：113. 3630，23. 0446。如图10 图14所示

。

Yaw 控制=K pQ ·ΔYaw +K iQ ·∑IYaw +K dQ ·DYaw

i =0

（20）

K pQ =3. 5；K iQ 为积分系数，K iQ 式中：K pQ 为比例系数，

=1. 0；K dQ 为微分系数，K dQ =0. 15。4

结束语

经过在四轴飞行器MQ120（如图9所示）上利用AHRS 与GPS 自主飞行系统所做的初步研究和设计，实现了四轴飞行器的基本自主悬停。并通过实际飞行实验，确定了以上算法的可行性，能够保证一定经度的悬停

。

图10

横滚角数据

图11俯仰角数据

第4期孙骅：基于GPS 与AHRS

的四轴飞行器悬停算法研究487

图12

航向角数据

图13

经度数据

图14纬度数据

［S ］．广西：桂林飞宇电子科技有限公司，2010

［2］X-600D 四轴飞行器使用说明书［S ］．香港：香港科比特科技

有限公司

［3］Jin Kim H ，Shim D H．A flight control system for aerial robots ：

algorithms and experiments ［A ］．15th IFAC World Congress ，2002on Auto Matic Control ［C ］

［4］张明连．飞行控制系统［M ］．北京：航空工业出版社，1994［5］王坤宇．直升机飞行控制系统［M ］．天津：航空工业出版

1997社，

［6］A．盖索乌·T．直升飞机控制学［M ］．北京：航空工业出版

1998社，

［7］Prouty ，R．W．2003．Helicopter Performance ，Stability ，and

Control ［M ］．Krieger Publishing Company ，Reprint with addi-tion ，original edition ，2003．USA

［8］Frazzoli E ，et al ．Trajectory tracking control design for autono-mous helicopters using a backstepping algorithm ［A ］．In ：Proceedings of the American Control Conference ［C ］，2000：4102 4107

由图10 图14可以看出，利用AHRS 与GPS

自主飞行系统控制四轴飞行器的悬停，可使得四轴飞行器的横滚角（Roll ）和俯仰角（Pitch ）都保持在5ʎ 以内，航向角则能控制在2ʎ 以内。并且经纬度均确保在0. 0001ʎ 范围内。通过对结果分析，可知本实验验证了算法的有效性，达到了预期的效果，以下是实验结论飞行参数：

最大飞行速度为20km /h；最大横滚、俯仰倾角为30ʎ ；最大起飞重量为2100g ；最大飞行天气风速为5m /s；最小自重为1500g ；最大落地速度为700ft /min；最大悬停控制精度直径（最小范围）为5m 。

［参考文献］

［1］飞宇FY －AHRS －1200A 姿态航向参考系统使用说明书