基于GPS定位的四旋翼无人机位置跟踪控制

杨 帆，龙 坤

（1.武汉工程大学 电气信息学院，武汉430205；2.湖北省视频图像与高清投影工程技术研究中心，武汉430205）

近年来，随着智能仪器与自动化技术的逐步成熟，四旋翼无人机已经从最初军用领域转向了民用领域，在航拍、测绘、植保、消防等方面得到了越来越多的应用[1]。由于四旋翼无人机结构简单，具有横滚、俯仰、偏航、悬停、垂直等多种飞行模式，可最大程度地满足用户的需求。在四旋翼无人机飞行过程中，控制系统和导航技术则是重中之重，给四旋翼无人机加上GPS 模块后，就可以利用GPS 的定位功能更加灵活地控制四旋翼无人机[2]。本文设计了一种串级PD 控制算法， 将其应用到四旋翼无人机控制系统中，相比于传统PID 控制算法，四旋翼无人机飞行状态更加稳定，通过上位机软件可以实时观察到四旋翼无人机的运动轨迹，真正实现四旋翼无人机轨迹跟踪功能。

1 四旋翼无人机动力学建模

1.1 飞行原理

四旋翼无人机在分类上主要有“+”和“x”两种形式，由于“x”型四旋翼无人机在结构上更加灵活，具有更好的飞行视野，故本文采用的是“x”型结构四旋翼无人机[3]，其简化模型如图1所示。在系统结构上，四旋翼无人机主要由飞控板、无刷直流电机、电调、电池、旋翼等组成，是一个典型的非线性欠驱动系统。沿逆时针方向将4 个旋翼分别编号为4 号，2号，3号，1 号， 其中1 号和2 号沿顺时针方向旋转，3 号和4 号沿逆时针方向旋转，这样可抵消四旋翼无人机做飞行运动时的反扭矩效应以及机身陀螺效应。

图1 四旋翼无人机的结构Fig.1 Structure of quadrocopter drone

1.2 机体坐标系转换

将四旋翼无人机绕x 轴产生的横滚角记为ϕ；绕y 轴产生的俯仰角记为θ； 绕z 轴产生的偏航角记为ψ。为了更加准确地描述四旋翼无人机在空间中的位置以及建模的方便， 先建立地球坐标系E（OE，XE，YE，ZE），将四旋翼无人机轴心点记为坐标原点OE， 以垂直于水平地面向上的方向记为ZE轴方向，再根据坐标系右手原则确定XE，YE轴的方向[4]。以四旋翼无人机轴心点为坐标系原点，建立机体坐标系B（OB，XB，YB，ZB），坐标原点OB即为四旋翼无人机轴心点，以垂直于机身水平面向上的方向记为ZB轴的方向，根据右手原则同样可得到XB，YB轴的方向。两种坐标系示意图如图2所示。

图2 机体坐标系-地球坐标系Fig.2 Body coordinate system-Earth coordinate system

由于在建模过程中一般选用的参考标准是地球坐标系，而实际上得到的是机体坐标系，因此需要将四旋翼无人机机体坐标系转换为地球坐标系，四旋翼无人机绕x，y，z 轴旋转所对应的旋转矩阵分别为

在四旋翼无人机飞行过程中，由于四旋翼无人机的姿态角是根据欧拉角解算得来的，在进行坐标系转换时，必须要保证四旋翼无人机的3 个姿态角和欧拉角相同，即要求无人机按照X-Y-Z 的顺序旋转，这样做便于进行坐标系转换[5]。四旋翼无人机地球坐标系转机体坐标系的矩阵RE-B＝C（ϕ）·C（θ）·C（ψ），由旋转矩阵的正交性可得到四旋翼无人机的地球坐标系矩阵为

1.3 牛顿-欧拉方程的建立

由牛顿第二定律可知四旋翼无人机在x，y，z 轴所受合外力可表示为

4 个电机产生的总的升力U1=F1+F2+F3+F4，由式（1）和式（2）可得到：

四旋翼无人机x，y，z 轴方向产生的力矩分别为

假设无人机进行小角度飞行运动，由欧拉方程M=Jε+ω·Jω 可知[6]，则有：

联立式（4）和式（5）可得：

每个电机产生的升力可表示为

故牛顿-欧拉方程为

式中：CT为系统升力系数；CM为系统扭力系数；d 为电机到轴心的轴距；J 为转动惯量矩阵；ω 为角速度；ε 为角加速度；b 为四旋翼无人机旋翼的拖动系数；为电机转速。

2 四旋翼无人机仿真测试

利用MATLAB/Simulink 搭建出牛顿-欧拉方程模型，以电机转速作为输入对象，四旋翼无人机的3 个欧拉角ϕ，θ，ψ 作为输出对象， 建立的欧拉角模型如图3所示。

图3 牛顿-欧拉方程模型Fig.3 Newton-euler equation model

其中姿态计算模块中的3 个子模块分别表示求ϕ¨，θ¨，ψ¨的过程，由于PD 控制存在比较性，需使得角度在2π 弧度内变化，故fcn 模块表示对3 个欧拉角取2π 的余数，方便后续算法的计算。通过姿态计算得到的欧拉角可反解出四旋翼无人机在x，y，z 轴的加速度，最终得到牛顿-欧拉方程的仿真模型。

2.1 四旋翼无人机PD 控制算法结构

结合PID 控制原理以及给定的四旋翼无人机轨迹，通过位置控制后，输出ϕd，θd，ψd姿态角信号；再通过姿态控制输出转力矩信号给电机，当作用力改变后，电机转速ω˜也随之改变；然后再将转速信号输出给四旋翼无人机数学模型，最后将姿态信号，位置信号分别反馈到姿态控制和位置控制模块[7]，其控制框图如图4所示。

图4 四旋翼无人机控制系统的结构原理Fig.4 Structural principle of quadrocopter drone control system

2.2 系统内外环建模仿真

通过图4可以看出，系统的内环控制为姿态角控制，外环为位置控制，由于PID 控制算法应用广泛，在本控制系统中，采用PID 控制后发现最后得到的系统仿真曲线会失真，即x，y，z 轴方向上的运动轨迹无法趋于稳定[8]，原因在于积分环节I 一定程度上降低了系统的稳定性。采用串级PD 控制可以产生早期的修正信号，增加阻尼程度，有效地增强系统的稳定性。因此，系统内外环控制均采用串级PD 控制，以此来改善系统的稳定性。外环输入为xd，yd，zd，x，y，z，然后依次经过P，D 环节，最后输出得到x，y，z 轴方向上的力Ux，Uy，Uz； 内环输入为ϕd，θd，ψd，ϕ，θ，ψ 由于姿态角在输入时存在较大的波动性，故先通过微分环节D 处理得到早期的待稳定信号，然后在将信号传给比例环节P，最后输出得到F2，F3，F4。建立好的系统仿真模型如图5所示。

图5 系统内外环控制仿真模型Fig.5 Simulation model of inner and outer ring control of system

若给定psai_d 等于0.2，x，y，z 为输入的阶跃响应曲线，且幅值范围在10～15 m 之间，仿真时间为100 s，经过多次调整P，D 参数后最终能够得到稳定的系统位置变化曲线。由于在仿真模型里引入了四旋翼无人机质量m，升力系数CT以及扭力系数CM，根据四旋翼无人机的实物参数，再结合北京航空航天大学所研发的飞行测评网站（www.flyeval.com）就可以算出CT和CM的值分别为1.116×10-5和1.474×10-7[9]。四旋翼无人机位置曲线如图6所示。

由图6可知，x，z 方向上的运动轨迹最终能够达到稳定状态，y 方向上出现的波动在经过一段时间后也能回到原始的位置。采用串级PD 控制算法所建立的仿真模型能够得到较好的四旋翼无人机位置变化曲线， 避免了积分环节所带来的曲线失真，为下一步四旋翼无人机的轨迹仿真提供了有效应对办法。

图6 四旋翼无人机位置曲线Fig.6 Position curve of quadrocopter drone

最后利用旋转矩阵Cbn以及所得到的四旋翼无人机的实际位置x，y，z，通过编写简单的建模程序，将仿真模型的信息传到MATLAB 工作区就可以绘制出四旋翼无人机仿真轨迹图。同时还可以得到四旋翼无人机跟踪轨迹曲线变化图，如图7所示。无人机在经过短暂的上升后最终悬停在10 m 左右的高度，与上述仿真结果一致。四旋翼无人机的轨迹跟踪曲线分别如图8和图9所示。

图7 四旋翼无人机完整航迹Fig.7 Complete track of quadrocopter drone

图8 四旋翼无人机欧拉角曲线仿真Fig.8 Simulation of euler angle curve of quadrocopter drone

图9 四旋翼无人机轨迹跟踪仿真Fig.9 Quadrocopter drone tracking simulation

由图8可知，四旋翼无人机的姿态角始终变化在合理区间内，其中横滚角ϕ 大约在57 s 时趋于稳定，俯仰角θ 大约在51 s 时趋于稳定，偏航角ψ 大约在10 s 时趋于稳定；由图9可知，x，y，z 的实际位置与给定的参考位置曲线基本相符，经过一段时间运行后最终都能达到稳定状态。通过仿真测试进一步证明了串级PD 控制算法的稳定性。

3 四旋翼无人机调试实验

3.1 APM 飞控板参数校正

实验平台的硬件结构主要包括F450 机架、APM2.8 飞控板、好盈20 A 电机、30 A 电调、9450 自锁桨、MC6 遥控器、接收机、电池、UBEC 供电模块、数传模块、M8NGPS 模块以及减震板等[10]。APM2.8飞控板主处理器为Arduino Mega2560 单片机，APM2.8 内置六轴MEMS 传感器MPU6000， 气压计MS-5611，三轴磁力计HMC5883，一般还会配置GPS模块接口，可以实现四旋翼无人机定位悬停功能[11]。其中，MPU6000 整合了三轴陀螺仪和三轴加速度计。MS-5611 通过测量气压得到高度，辅助GPS 定位。HMC5883 通过测量地磁场得到方位，辅助无人机定向[12]。飞控采集并融合多种传感器的数据，计算并校正四旋翼无人机的位姿。

利用USB 接口将Mission Planner 地面站软件与APM 飞控进行连接，通过APM 官网进行固件的下载， 将串级PD 控制算法和传统PID 算法分别以代码的方式导入到APM 板中，比较二者的优劣性。固件下载完成后，接着对APM 板上的罗盘，加速度计，遥控器参数以及飞行模式参数进行校正，罗盘校正主要是检测APM 飞控板内置罗盘的性能，加速度计校正主要是对APM 飞控板方向进行读取，飞行模式校正主要是选择无人机的飞行模式。在本次实验过程中， 设置了3 种飞行模式： 自稳模式Stabilize，悬停模式Loiter，返航模式RTL。当飞控板参数调试完成后，最后对电调进行校正，以便于四旋翼无人机能正常起飞。

3.2 四旋翼无人机室外飞行测试

为验证串级PD 控制算法的稳定性， 实验地点选在武汉工程大学流芳校区土操场，实验当天风速大概为2 m/s，风力等级2 级。预先为无人机规划好航点，并利用数传模块读取航点，飞行时首先将通道5 开关拨到Stabilize 处， 当四旋翼无人机上升到10 m 左右的高度时再切换到Loiter 模式，此时无人机可利用GPS 完成定点， 空中悬停效果如图10所示。然后将遥控器的通道5 开关拨到RTL 模式，当到达最后一个航点时无人机会自动沿原路返回。从上位机软件获取的四旋翼无人机的姿态，位置数据如表1所示。

图10 四旋翼无人机悬停图Fig.10 Hover map of quadrocopter drone

表1 四旋翼无人机位置和姿态记录Tab.1 Quadcopter position and attitude recordings

通过地面站数传模块可以观察到四旋翼无人机欧拉角及其速度变化，如图11所示。传统PID 控制算法得到的实验数据存在较大的波动性，无法使得四旋翼无人机稳定飞行， 而串级PD 控制算法得到的实验数据比较稳定，能使得四旋翼无人机保持稳定飞行。

图11 四旋翼无人机地面站控制Fig.11 Quadrocopter drone ground station controlling

4 结语

本文通过对四旋翼无人机系统结构以及动力学模型的分析，以MATLAB/Simulink 为平台，以串级PD 控制为核心对四旋翼无人机内外环控制进行了建模仿真， 通过理论上的建模仿真实验发现，串级PD 控制算法对四旋翼无人机有较好的控制效果， 使得四旋翼无人机在飞行过程中能保持稳定。最后通过室外实验测试， 在串级PD 控制下， 搭载GPS 模块的四旋翼无人机能够稳定飞行并实现轨迹跟踪功能。由于串级PD 控制始终存在一定的误差，如果采用更稳定的控制算法以及控制单元，可以进一步减小误差，提高四旋翼无人机的使用效率。