变载荷下六旋翼无人机高度控制*

龙 文,陈 洋*,吴怀宇,程 磊,陈鹏震,王正熙

(1.武汉科技大学信息科学与工程学院,武汉 430081;2.武汉科技大学教育部冶金自动化与检测技术工程研究中心,武汉 430081)



变载荷下六旋翼无人机高度控制*

龙 文1,2,陈 洋1,2*,吴怀宇1,2,程 磊1,2,陈鹏震1,2,王正熙1,2

(1.武汉科技大学信息科学与工程学院,武汉 430081;2.武汉科技大学教育部冶金自动化与检测技术工程研究中心,武汉 430081)

六旋翼无人机应用于植保领域时通常需要定高飞行,单一机载微传感器无法长时间准确的跟踪六旋翼无人机的高度,且当负载发生变化时无人机的高度控制会出现不稳定现象。针对上述问题,本文采用了一种基于气压计和加速度计卡尔曼滤波融合的高度测量系统,并设计了一个双闭环PID控制加前馈控制的高度控制器,最终获得了无人机飞行高度的精确估计与稳定控制。实验结果表明,经过卡尔曼滤波可以得到准确、平稳的高度值和速度值。提出的高度控制器在变载荷情况下也能很好的控制无人机的高度。

六旋翼无人机;变载荷;前馈控制;卡尔曼滤波

近年来,多旋翼无人机逐渐成为航空领域学者们的研究热点,多旋翼无人机是一种具有垂直起降与空中悬停等特殊飞行能力的多旋翼无人飞行器。在飞行过程中,通过改变旋翼的转速,可以改变各种飞行姿态。正因为多旋翼无人机具有垂直起降、空中定点悬停、可以实现六自由度的飞行等独特的飞行能力,使其在军事领域和民用领域得到了广泛的应用[1],包括军事侦察,农业植保,电力巡检,测绘,航拍等。

无人机在执行某些特定的任务过程中质量有可能会发生变化,如农业植保无人机在工作过程中随着农药的喷洒,质量会不断的减小,但是为了更好的保证农药的喷洒质量,不发生漏喷重喷现象,必须保证无人机能与作物保持固定的高度。如果高度控制系统不能感知自身质量的变化便会导致高度控制不稳定,响应偏慢等现象。由此可见针对此类六旋翼无人机,设计控制系统过程中考虑到无人机的质量变化显得尤为重要。旋翼无人机的高度控制系统设计主要包括两个方面,分别是高度测量系统设计和高度控制器设计。高度估计算法方面目前公开的算法主要有互补滤波[2],卡尔曼滤波[3-5]。文献[2]中采用加速度计与气压计进行互补滤波融合来估计高度,具有迭代简单、适合工程实践等优点。文献[3]中采用卡尔曼滤波的方法,利用加速度计和气压计,超声波3种传感器来进行高度的估计,相比互补滤波,卡尔曼滤波具有收敛速度快,精度高等优点。使用最普遍的高度测量传感器是GPS测量系统,目前有许多关于GPS数据定高的融合算法,比如融合GPS/INS数据实现无人机的精确导航[6]、组合气压高速传感器、全球定位系统(GPS)和捷联式线加速度计,建立一种具有较高精度的无人直升机高度测量系统[7]、根据飞行器运动方程,推导出了高度和飞行状态之间的关系,并由所得的飞行状态,利用 Kalman 滤波方法得到高度[8]。这些方法测高效果很好,但是GPS的使用条件苛刻,通常室内或者有高大建筑物遮挡时无法使用。气压计可以克服这一困难,但是单纯的气压器测量的高度存在很大的噪声,而且测量出来的高度很容易受到环境的影响。另外在高度和垂直速度的测量中,气压式高度表有较大的惯性,因而瞬时高度和垂直速度的精度会受到影响。而惯性系统测量高度的误差是以指数形式增长的,因此,用气压式高度表的信息对惯性高度系统进行阻尼,可得到品质较好又不随时间发散的组合高度系统[9]。利用气压计与加速度计卡尔曼滤波得到高度信息,然后采用现代控制理论中的LQI控制对无人机的高度进行控制。这样做在无风和有风的情况下都能获得较理想的定高效果,但是在无人机质量发生变化以及电池消耗较大的情况时,高度控制效果大大降低[10]。

综上所述,基于组合高度测量系统的思想和针对质量变化的六旋翼无人机高度控制响应速度偏慢,控制效果偏差的问题,本文采用一种利用加速度计和气压计进行卡尔曼滤波融合得到垂直方向高度值,速度值和加速度静态偏差的高度测量系统以及双闭环PID控制加前馈控制的高度控制器。实验结果表明数据融合算法能输出准确的高度和速度信息。在双闭环PID控制器的基础上加上前馈控制,利用卡尔曼滤波器估计机体质量,采用牛顿第二定律计算一个自适应升力作为前馈控制量,能加快控制器响应速度,当无人机负载发生变化时也能达到很好的控制效果。

1 高度测量系统设计

定义导航坐标系(n系)和机体坐标系(b系),如图1所示。

图1 导航坐标系与机体坐标系的关系

图1中,导航坐标系采用东北天(ENU)右手坐标系,其坐标原点O位于运载体的质心;机体坐标系是固连于无人机机体的参考坐标系[11]。

加速度计可以测得无人机在机体坐标系的三轴加速度,但是高度测量是相对导航坐标系,所以需要利用方向余弦矩阵将机体坐标系下的加速度转换到导航坐标系中,得到相对大地的垂直方向加速度。计算公式如式(1):

(1)

(2)

注:式中cθ,cφ,cγ,sθ,sφ,sγ分别表示cosθ,cosφ,cosγ,sinθ,sinφ,sinγ。

anz表示导航坐标系中机体垂直方向加速度;那么机体相对大地的加速度为a=anz-g。

用z(k)表示在k时刻气压计的观测高度,则有观测模型:

z(k)=Hx(k)+μ(k)

(3)

定义系统的状态变量为x(k)=[h(k)v(k)bias(k)]T,分别表示无人机高度,垂直方向速度,以及加速度计Z轴上的偏差。由式(3)可得,观测矩阵H=[1 0 0],又由匀加速运动方程可得系统的状态方程为:

(4)

即系统的状态空间模型为:

x(k+1)=Ax(k)+Bu(k)+w(k)

(5)

(6)

式中:P(k)表示k时刻的协方差,K(k)为系统k时刻的卡尔曼增益。

至此,通过时间更新过程和观测更新过程进行不断的预测-修正来递推出一个最优的高度值和速度值,每次更新得到的加速度计偏差实时修正加速度Z轴上的偏差。高度卡尔曼滤波原理框图[12]如图2所示。

图2 卡尔曼滤波原理框图

2 高度控制器设计

高度控制器由双闭环串级PID反馈控制加前馈控制组成,双闭环串级PID有两个环,外环为高度环,内环为垂直方向速度环,前馈控制通过卡尔曼滤波器估算当前机体质量,然后利用牛顿第二定律计算出一个自适应升力直接补偿给控制器输出,这样做可以提高控制器的响应速度,同时能适应无人机负载的变化。控制系统结构如图3所示。

图3 高度控制器结构图

2.1 反馈控制

反馈控制的外环表达式,如式(7)所示:

(7)

内环的表达式,如式(8)所示:

(8)

2.2 前馈控制

前馈控制部分由卡尔曼滤波器估计出机体的质量,然后利用牛顿第二定律计算出一个自适应的升力。卡尔曼滤波器以无人机质量的倒数为状态变量,由牛顿第二定律F=ma可得1/m=a/F,故以a/F作为观测变量,F为油门量即无人机升力,建立状态空间模型如式(9)、式(10)所示。

x(k+1)=Ax(k)+w(k)

(9)

z(k)=x(k)+v(k)

(10)

由于状态变量为一维变量,且认为A为1所以状态估计方程认为无人机质量一直保持不变,测量方程通过无人机在导航坐标系中的加速度除以油门量得到一个观测的质量倒数,去修正预测方程预测的质量倒数。具体卡尔曼滤波递推过程如图2所示。通过卡尔曼滤波器可以实时的估计无人机的质量,然后利用式(11)可得无人机的升力,即为前馈控制输出的控制量。

(11)

综上所示,当无人机在tT时刻切换到定高模式时,系统默认以该时刻估算出来的高度为期望高度hr,当无人机受到扰动,高度有所上升或下降时,就会与期望高度产生一个高度差,对高度差做比例运算,输出一个期望的垂直方向上的速度vr,对期望速度进行限幅,本文限制幅度为8 m/s,期望的垂直方向上的速度与估计的当前时刻速度产生一个速度差,对速度差做PID运算,积分运算限幅,微分运算需要经过截止频率为5 Hz的一阶惯性低通滤波器(LPF),输出值为反馈控制的控制量,同时前馈控制通过卡尔曼滤波估计机体质量通过牛顿第二定律计算出一个自适应升力作为前馈控制量来提高无人机的响应速度,保证无人机在负载发生变化时也能快速的稳定无人机高度。

考虑到实际情况能在定高模式下推动油门控制无人机上升下降,我们给油门值设定了一个死区,在定高模式下遥控器油门的给定满足一个分段函数,如式(12)所示:

(12)

即在油门通道输出PWM脉宽计数为1 400～1 600范围内,且为定高模式时以当前高度作为外环期望高度。当定高模式下油门值输出PWM值不在1 400～1 600范围内时,直接由遥控器的给定作为反馈控制内环的速度给定,跳过外环部分,限制最大爬升速度为8m/s。低于1 400时,限制最大下降速度为4m/s。做这样的处理是为了在定高模式下死区内为高度控制将无人机稳定在一个高度。需要改变高度时,当油门量超过死区,油门量就会作为内环给定速度,跳过外环,让无人机以一定速度爬升或者下降到期望的高度,油门回到死区继续控制无人机稳定在当前的高度。在许多开源飞控中没有采用这种单环和双环的切换,直接将遥控器输出作为速度,然后积分出来高度,作为高度的给定,这样做虽然也可以做到在定高模式中控制无人机高度,但是积分会导致延时产生,导致控制手感偏差。

3 实验与结果

本文搭建了相应的实验平台,飞行控制单元主要由主控模块、惯性测量及气压计模块、GPS测量模块、无线通讯模块和电机驱动模块构成。其中,主控模块用于协同整个系统的运行控制;惯性测量模块用于测量飞行器三轴线速度和角速度,气压计与GPS模块主要用于跟踪三维空间位置信息;无线通讯模块用于无人机与上位机软件之间的通信;电机驱动模块则利用PWM驱动直流电机。飞行控制单元的主控模块采用Cortex-M3内核的32 位单片机STM32F103。该芯片提供I2C接口、SPI接口和PWM输出接口分别用于连接惯性测量模块、无线通讯模块和电机驱动组模块。本文实验在六旋翼植保无人机平台中进行了实验验证,该实验的实物效果图,如图4所示。

图4 六旋翼植保无人机实物效果图

图5 静态与动态高度测试图

3.1 高度测量系统实验

首先进行卡尔曼滤波状态估计的测试实验,将六旋翼无人机平放在地面上取一组数据测试静态效果,然后将无人机起飞到一定高度下降,通过上位机分别读取气压计和数据融合算法得出的高度值和垂直方向速度,从而测试卡尔曼滤波估计高度值和速度值得静态跟踪性能和动态跟踪性能,如图5、图6所示。

图6 静态与动态速度测试图

通过多次测试可以得出,单一的气压计测得的高度值噪声方差约为0.04,而经过本文数据融合算法得到的高度值噪声的方差约为0.000 4。由图5和图6可以明显看出卡尔曼滤波算法能有效融合加速度计与气压计的数据得到一个平稳、准确的高度值和速度值,解决了单一加速度计计算高度容易产生累计误差,单一气压计计算高度噪声过大的问题,充分证明了本文采用的数据融合算法可靠性。

3.2 高度控制系统实验

验证完数据融合算法的可靠性,下面进行高度控制系统测试,首先验证加入了前馈控制的高度控制器,安装好六旋翼无人机后给水箱装满水起飞,当无人机飞到一定高度后切换到定高模式,开始通过水泵喷洒水箱里面的水并向前飞行,通过上位机观察随着机身质量的减小,在定高状态下高度的变化。然后相同的条件下测试只有双闭环PID控制器时,定高模式下,随着质量的减小,高度的变化曲线,实验结果如图7所示曲线。

图7 变载荷条件下定高测试图

通过图形可以看出,针对变载荷下多旋翼无人机,加入了前馈控制的高度控制器得到的高度保持的效果明显比只有双闭环PID的控制效果响应速度更快,定高效果更平稳。通过卡尔曼滤波也能精准的跟踪机体质量的变化。经过多组实验结果证明,基于本文的高度控制算法,可以实现六旋翼植保无人机在质量不断变化的条件下保持高度方向上0.2m的控制精度。

4 结束语

六旋翼无人机进行高度控制的过程中,由于高度估计存在偏差以及负载质量的变化,容易导致高度控制不稳定的现象。针对上述问题,本文在状态估计方面采用卡尔曼滤波估计无人机的高度值、垂直方向上的速度以及加速度Z轴上的偏差等参数。在高度控制器设计方面,引入卡尔曼滤波来实时估计无人机的质量,进而根据牛顿第二定律计算自适应升力作为前馈控制量,叠加到双闭环PID的反馈控制器中。最后,将所采用的方法应用于六旋翼植保无人机高度估计与高度控制中,经过多次实际飞行测试结果表明:①利用卡尔曼滤波状态估计方法,对加速度计与气压计数据进行融合,能有效的抑制气压计的随机噪声,使无人机能够精准的跟踪其高度和速度变化。实验数据表明利用绝对精度为0.4m气压计与加速度计进行数据融合可以得到精度为0.03m的垂直方向上高度信息。②实现了六旋翼无人机在质量发生变化时依然保证高度平稳,在传统双闭环PID控制的基础上加入前馈控制使其控制效果得到明显的改善,实验数据表明采用本文的高度控制算法,能在变载荷的情况下得到0.2m的控制精度。

[1] Sa R C,Barreto G A,De Araujo A L C,et al. Design and Construction of a Quadrotor-Type Unmanned Aerial Vehicle:Preliminary Results[C]//2012 Workshop on Engineering Applications(WEA). IEEE,2012:1-6.

[2] 方家豪,叶欣,董伟,等. 基于加速度信息修正的四旋翼位置估计算法研究[J]. 传感技术学报,2016,29(11):1684-1691.

[3] 赵世荣. 基于嵌入式的四旋翼飞行器姿态控制设计[D]. 广州:中山大学,2016.

[4] Mammarella M,Campa G,Napolitano M R,et al. Machine Vision/GPS Integration Using EKF for the UAV Aerial Refueling Problem[J]. IEEE Transactions on Systems Man and Cybernetics Part C,2008,38(6):791-801.

[5] Oh S M,Johnson E. Development of UAV Navigation System Based on Unscented Kalman Filter[C]//AIAA Guidance,Navigation,and Control Conference and Exhibit. 2006:524-533.

[6] 李洪辉,裴海龙. 无人直升机高度测量的设计与实现[J]. 自动化与仪表,2012,27(7):11-15.

[7] 谢勇,陈照海,陈万法. 某无人直升机高度测量系统融合方法[J]. 兵工自动化,2010(2):85-86.

[8] 胡永红. 小型飞行器高度定位数据融合方法[J]. 传感器与微系统,2004,22(6):24-26.

[9] 王新龙. 惯性导航基础[M]. 西北工业大学出版社,2013.

[10] 王伟,周勇,王峰,等. 基于气压高度计的多旋翼飞行器高度控制[J]. 控制工程,2011,18(4):614-617.

[11] 王立,章政,孙平. 一种自适应互补滤波姿态估计算法[J]. 控制工程,2015,22(5):881-886.

[12] Wang L,Zhang Z,Sun P. Quaternion-Based Kalman Filter for AHRS Using an Adaptive-Step Gradient Descent Algorithm[J]. International Journal of Advanced Robotic Systems,2015,12:131.

龙 文(1993-),男,湖北仙桃人,硕士研究生,研究方向为多旋翼无人机状态估计与控制,963626536@qq.com;

陈 洋(1980-),男,博士,副教授,公开发表学术论文30余篇。主要研究方向为移动机器人建模,规划与控制,人机交互与机器学习,chenyag@wust.edu.cn。

Altitude Control of Six Rotor UAVs with Variable Load*

LONG Wen1,2,CHEN Yang1,2*,WU Huaiyu1,2,CHENG Lei1,2,CHEN Pengzheng1,2,WANGZhengxi1,2

(1.School of Information Science and Engineering,Wuhan University of Science and Technology,Wuhan 430081,China;2.Engineering Research Center for Metallurgical Automation and Detecting Technology of Ministry of Education,Wuhan University of Science and Technology,Wuhan 430081,China)

Six-rotor UAVs usually require constant altitude flight when applied in plant protection,as we know the UAV altitude could not be tracked by a single airborne micro-sensor and that the altitude-hold would be unstable in the case of loads of the UAVs are changed. Aiming to solve these problems,a novel system based on barometer and accelerometer Kalman filtering fusion is proposed for altitude measuring,and also a controller based on double-closed-loop PID and feedforward controller is designed for altitude-hold. As a result,we obtain the accurate altitude-estimation and the stable control of the UAVs. Results of the experiment show thatthe value of altitude and velocity could be more accurate and steady by Kalman filtering. And the altitude-controller has a good performance under the condition of changing load.

six-rotor UAVs;Variable Load;feedforward;Kalman filter

项目来源:国家自然科学基金项目(61573263,61203331);湖北省科技支撑计划项目(2015BAA018)

2016-11-24 修改日期:2017-03-10

TP242

A

1004-1699(2017)07-1125-06

C:7230

10.3969/j.issn.1004-1699.2017.07.026