曲仕茹, 马志强

(西北工业大学 自动化学院, 陕西 西安 710072)

改进的粒子滤波在四旋翼姿态估计中的应用

曲仕茹, 马志强

(西北工业大学 自动化学院, 陕西 西安 710072)

针对四旋翼飞行器非线性模型的姿态估计问题,提出了一种基于重要密度函数优选的改进粒子滤波(IDOPF)姿态估计算法。该算法通过扩大重要密度函数的覆盖范围,主动从众多重要密度函数中选择更优初始化粒子群。结合动力学模型和基于反步法控制器的相关控制数据,进行了IDOPF粒子滤波算法在四旋翼飞行器姿态估计中的仿真实验。与EKF算法相比,该算法具有更高的估计收敛速度和估计精度,避免了不稳定滤波,改善了滤波效果,验证了IDOPF算法在四旋翼姿态估计的可行性和有效性。

粒子滤波; 扩展卡尔曼滤波; 四旋翼姿态估计

0 引言

四旋翼飞行器因其结构紧凑、启动条件少和实现成本低等特点,成为近年来航空学术领域的研究热点。四旋翼飞行器是一种可垂直起降、定点悬停的非共轴旋桨飞行器,通过调整四个独立桨叶的转速,实现飞行器姿态的控制。目前已有澳洲卧龙岗大学的Mckerrow对其进行了动力学建模,Bouabdallash等设计了Backstepping,LQ和PID等多种方法的增稳反馈控制器[1-3]。为了使反馈控制器准确有效地控制飞行器平稳飞行,需要了解飞行器的姿态。由于受到各种噪声的影响,一般传感器的敏感值与真实值之间存在一定的偏差,为解决这类问题,常用扩展卡尔曼滤波算法(EKF)及Sigma点卡尔曼滤波算法对观测值进行估计[4-5],达到减小偏差的目的。姿态估计中常用的EKF对四旋翼非线性系统进行线性截断,以实现非线性问题线性化。由于EKF算法忽略了非线性函数的高阶导数项,在姿态估计中会出现一定的偏差,并且在局部线性化不成立或线性化数值结果不良的时候产生不稳定的滤波。

本文提出了一种基于重要密度函数优选的粒子滤波(IDOPF)算法进行姿态估计。通过建立四旋翼飞行器姿态模型并进行仿真,以验证方法的可行性和有效性。

1 粒子滤波基本理论

设非线性系统模型为:

(1)

式中,wk和vk分别为系统噪声和测量噪声,两者相互独立,一般认为是白噪声;uk为系统的控制输入。求解后验概率分布p(xk|y1,k)是实现最优非线性滤波的关键,其中y1,k为观测向量。根据贝叶斯估计理论,在x0,k给定,则yk于k时刻与以前的测量相互独立时,获得后验概率表达式为:

(2)

对于非线性、非高斯模型,消去式(2)中间参量和其他位置参量所涉及的计算是困难的,无法获得完整的解析式来描述此概率密度函数,而求取近似解需要计算多重积分,这些算法上的限制导致求解难以实现。

为解决上述问题,引入用于贝叶斯计算的蒙特卡罗方法,该方法通过合理分散估计量有效地完成求和及积分运算,经过整合,成为本文描述的粒子滤波算法。粒子滤波又称序贯蒙特卡罗方法,是一种基于蒙特卡罗方法和递推贝叶斯估计的统计滤波方法,理论上能够估计任意形式的概率分布,可解决任意非线性系统的估计问题。

由于一般情况中后验概率分布p(xk|y1,k)未知,需要引入重要采样概率函数q(xk|y1,k),利用该函数进行重要性采样,由贝叶斯递推公式推导可得权值函数:

(3)

利用主要采样概率函数,结合式(2)能够获得粒子的权值,进而获得状态的后验概率密度。然而利用该递推公式获得的权值会出现粒子退化现象,有效地改善方法是引入重采样步骤或增加初始粒子数量。

2 四旋翼飞行器姿态模型

四旋翼飞行器是典型的欠驱动系统,通过4个旋转桨叶调节六个自由度的姿态。研究中简化了动力学模型,认为四旋翼飞行器的运动为六自由度的刚体运动,为围绕三个轴向的转动(偏航、俯仰和滚转)和重心沿三个轴向的线运动(进退、侧飞和升降)。

机体运动选择体坐标系进行描述,坐标原点即为机体重心,规定机体俯仰抬头时为正。结合文献[6]对四旋翼飞行器的动力学分析,可以获得动力学关系。体坐标系下三轴转动的角速度和欧拉角速度的关系为:

(4)

机体坐标系-地面坐标系的旋转矩阵为:

(5)

式中,cψ表示cosψ;sψ表示sinψ;其余类似。

地坐标系加速度与合外力关系为:

(6)

式中,Fx,Fy,Fz为体坐标系下的合外力F在三个轴向上的分量;Vxyz为体坐标系中的飞行速度方向;Vxy为体坐标系Oxy平面内的飞行速度方向;C为空气摩擦系数。

本文的四旋翼飞行器研究限于室内悬停及慢飞情况,姿态变化并不剧烈,所以作以下假设:旋翼中心和机体质心始终在一个水平面上;忽略空气扰动及空气摩擦阻力;认为欧拉角速度和体坐标系角速度一致;不考虑旋翼挥舞,认为它只受到与其转速的平方成正比的升力和反扭力矩作用;认为旋翼质量很轻,不考虑其转动惯量矩[7];为了提取数学模型,可以直接将旋翼转速当作系统动力学模型的输入。在上述假设的基础上,获得动力学模型为:

(7)

(8)

式中,θ,φ,ψ分别为俯仰角、滚转角和偏航角;Ui(i=1,2,3,4)为四个旋翼的运动控制量;Ω为四个旋翼的转速;其他测量获得的物理常量如表1所示。

表1 物理常量Table 1 Physical quantity

3 基于IDOPF的姿态估计算法

基本粒子波(PF)对重要密度函数的依赖性很强,重要密度函数选择不当会直接导致粒子枯竭,从而滤波发散,失去估计能力。重要密度函数的选取难度与抽取目标概率样本相同,从量测值中获得概率样本的方法在大噪声情况下受到限制。IDOPF是一种基于PF思想的改进滤波算法,其基本思想是:扩大重要密度函数的覆盖范围,主动从众多重要密度函数中选择更优的初始化粒子群。衡量粒子群优劣的主要方法是考察粒子退化程度,通过统计大权值粒子重采样的次数来反映该项指标。一般情况下,粒子退化程度越严重,大权值粒子的重采样次数越多;但当初始化中心值偏离后验概率过多时,会直接导致粒子枯竭,此时重采样次数反而减少,所以优选需要在中心值与重采样次数符合正比例关系的范围内进行。为了达到粒子中心值的选取精度,可以采用迭代本算法。

本文以四旋翼飞行器的姿态估计为例,详述IDOPF算法步骤。为了避免粒子滤波算法的状态高维数引起的计算量过大、难收敛等问题,设计中将姿态划分为三个通道,每个通道依次进行滤波计算。俯仰角θ通道IDOPF算法步骤为:

(1)给出俯仰角θ通道的状态模型:

(9)

观测方程式(9)是认为欧拉角速度和体坐标系角速度一致而近似得到的。其中:

(10)

(11)

(12)

其他通道的估计过程与上述过程类似。

4 数值仿真

为了验证算法的有效性,本文基于文献[6]提供的物理数据,采用matlab/simulink设计了基于反步法的四旋翼飞行器姿态仿真控制器,获得了可靠的控制数据及姿态角度信息,并采用IDOPF算法对含有噪声的姿态信息进行估计。仿真实验中,传感器采样周期为20 ms;三轴姿态角速度初始真实值为(1,1,1);角速度调整目标值为(0,0,0);三轴姿态角初始真实值为(1,1,1);姿态角调整目标值为(0,0,0);初始姿态方差为(1 rad)2;初始姿态速度方差为(0.1 rad/s)2;初始姿态速度误差为0.3 rad/s;过程方差为(0.3 rad)2;粒子数为500;重要密度函数中心值精度为1。

仿真初始阶段需要对重要密度函数进行优选,根据初始化粒子群的退化程度选择更近似后验概率的重要密度函数,结果如图1所示。

图1 粒子中心值分布与重采样结果Fig.1 Center value distribution and resampling results of particles

中心值选取[1,15]中的整数,经过初始阶段的重采样,[1,10]区间的大权值粒子复制次数数值变化基本符合正比例关系,累计误差也随着重要密度函数的中心值偏离后验概率分布而增加;[11,15]区间的复制次数明显偏低、且不满足正比例关系,这是由于中心值偏离后验概率分布过大(偏离倍数为10倍),从而粒子枯竭而造成的。初始化粒子群用于俯仰角θ通道的滤波估计得到图2所示的一组曲线,重要密度函数中心值选取在[1,10]上的粒子群能够实现状态的估计。

图2 不同重要密度函数粒子群的估计曲线Fig.2 Estimation of particle swarm of different important density function

由图1和图2可知,随着中心值偏离真实后验密度概率函数程度越大,IDOPF粒子退化程度越严重,状态估计的收敛速度越慢、估计精度越低,粒子枯竭的样本基本无法用于滤波估计。

经过重要密度函数优选的粒子群可用于后继的状态估计中,实验着重考察了IDOPF算法在四旋翼飞行器姿态估计中的效果,与同等条件下的扩展卡尔曼滤波EKF算法进行比对,数值仿真结果如图3、图4所示。

通过图3可以发现,在初始有误差情况下,EKF和IDOPF估计算法均能较快调整到观测值附近。在测量值变化剧烈的区域中,IDOPF曲线相比EKF更平滑。由于受到噪声的影响,EKF算法容易出现误估计的情况,而IDOPF算法能够稳定地保持在观测值附近。IDOPF算法和EKF都能根据观测值对真实值进行有效估计,但当局部线性化数值结果不良时,EKF算法滤波并不稳定。

图3 角速度估计跟踪曲线Fig.3 Tracking of angular rate estimation

图4 算法估计误差Fig.4 Algorithm estimation error

由图4可以发现,当数值波动较大时,IDOPF算法估计误差保持在±0.1 rad/s,而EKF算法的误差保持在±0.3 rad/s;在数值波动较小时,两种算法均保持在很低的误差范围内。在误差收敛速度方面,在0.5 s时,IDOPF算法误差收敛到±0.1 rad/s,EKF算法超过±0.1 rad/s,两种算法均存在跟踪误差;相比较而言,IDOPF算法误差小于EKF、波动更小,估计曲线更接近真实姿态曲线。

5 结束语

本文提出将PF算法进行改进获得IDOPF算法,并运用到四旋翼飞行器姿态估计中,解决了由于噪声造成的传感器敏感值与真实值偏差导致姿态控制发散的问题,同时也避免了由于重要密度函数选取不当而造成的粒子枯竭及过早的粒子退化问题。相比EKF算法,IDOPF有效提高了估计精度并降低了姿态估计收敛的时间,使得控制器响应更迅速准确。不足之处是相比于EKF算法,IDOPF需要初始化更多的粒子群,并且后继的滤波过程计算量大,相同水平的硬件平台IDOFP运行速度较低。为解决上述问题,可以考虑利用硬线逻辑电路实现该算法,从而提高应用效率,并可将其运用到各种自动航行器的实时姿态估计中。

[1] Salih A L,Moghavvemi M,Mohamed H A F,et al.Modeling and PID controller design for a quad-rotor unmanned air vehicle[C]//2010 IEEE International Conference on Automation Quality and Testing Robotics (AQTR).2010:1-5.

[2] Bouabdallah S,Siegwart R.Backstepping and sliding-mode techniques applied to an indoor micro quad-rotor[C]//2005 IEEE International Conference on Robotics and Automation.Spain,2005:2259-2264.

[3] Mistler V,Benallegue A,M’Sirdi N K.Exact linearization and noninteracting control of a 4 rotors helicopter via dynamic feedback[C]//10th IEEE International Workshop on Robot and Human Interactive Communication. Paris,2001:586-593.

[4] Einicke G A,White L B.Robust extended Kalman filtering[J].IEEE Transactions on Signal Processing,1999,47(9):2596-2599.

[5] Van Der Merwe R,Wan E A,Julier S.Sigma-point Kalman filters for nonlinear estimation and sensor-fusion—applications to integrated navigation[R].AIAA-2004-5120,2004.

[6] Ryll M,Bulthoff H H,Giordano P R.Modeling and control of a quadrotor UAV with tilting propellers[C]//2012 IEEE International Conference on Robotics and Automation (ICRA).2012:4606-4613.

[7] 聂博文.微小型四旋翼无人直升机建模及控制方法研究[D].长沙:国防科学技术大学,2006.

Applicationoftheimprovedparticlefiltertoquad-rotoraircraft’sattitudeestimation

QU Shi-ru, MA Zhi-qiang

(College of Automation, Northwestern Polytechnical University, Xi’an 710072, China)

This paper presents an estimation method based on the important density function optimization particle filter (IDOPF) in order to solve the problem of quad-rotor aircraft’s attitude estimation. The IDOPF which depends on the important density function select severity according to expand the scope of important density estimation function, and the more optimization particles is acquired by important density functions. The experiments of simulation were carrying out on the control dataset of dynamic model and backstepping controller. Eventually, the results of experiments show that, compared with EKF, the IDOPF has higher estimation convergence speed and higher accuracy, which avoids the unstable filtering, improves the filtering effect and verifies that IDOPF is feasible and effective in the quad-rotor attitude estimation.

particle filter; EKF; quad-rotor aircraft’s attitude estimation

V249.1

A

1002-0853(2013)05-0458-04

2013-01-19;

2013-05-14; < class="emphasis_bold">网络出版时间

时间:2013-08-21 16:13

航天科技创新基金资助(CASC201104)；航空科学基金资助(2012ZC53043)

曲仕茹(1963-)，女，山东青岛人，教授，博士，研究方向为交通运输规划与管理；马志强(1987-)，男，黑龙江哈尔滨人，硕士研究生，研究方向为无人飞行器姿态控制。

(编辑：李怡)