基于ILOPF的四旋翼姿态估计算法设计与实现*

黎远松，梁金明

（四川理工学院计算机学院，四川 自贡 643000）

基于ILOPF的四旋翼姿态估计算法设计与实现*

黎远松，梁金明

（四川理工学院计算机学院，四川 自贡 643000）

针对四旋翼飞行器姿态角速率的测量伴随随机噪声，直接进行姿态信息融合的结果波动剧烈的问题，提出了一种结合EKF的改进线性优化粒子滤波算法。该方法将EKF融合到重要密度函数中，改进了线性优化过程的结合策略，考虑算法的实时性，对其进行了相应的简化；实现了仿真条件下四旋翼飞行器姿态角速率的估计，统计结果显示，相比EKPF算法，ILOPF的滤波效率提高了20%。在飞行样机的飞行过程中使用ILOPF，表明了其有效性。

粒子滤波，姿态估计，EKF，四旋翼飞行器

0 引言

四旋翼飞行器是典型的多耦合、非线性欠驱动系统，近年来，因其飞行条件低，物理易实现等特点，成为控制领域研究的新热点。多国研究机构为其进行数学建模、控制器设计和飞行样机制作［1］。为了实现飞行器的有效控制，需要准确描述飞行器的飞行姿态。加速度在动态过程中由于受到高频振动的影响，无法单独解算出空间的姿态角，因此，四旋翼飞行器的姿态信息由三轴加速度和三轴角速率经由信息融合算法获得，陀螺仪在测量过程中由于受到扰动和自身量测噪声影响，角速率数据会产生高频波动，直接使用这样的数据进行融合获得的姿态信息无法用于控制系统。常用的抗干扰方法有卡尔曼滤波算法、四元数融合技术及其他机械抗抖动措施，其中卡尔曼滤波器或互补滤波器不能很好地对四旋翼飞行器的非线性环节进行估计，四元数算法能解决姿态角解算中出现奇点的问题，不过不能很好地抵抗噪声，机械措施设计难度大，实现成本高［2-3］。本文结合扩展卡尔曼滤波算法和粒子滤波算法，提出一种适用于四旋翼飞行器角速率估计的简化线性优化粒子滤波算法，该方法比扩展卡尔曼滤波算法更适用于非线性模型，而且简化了粒子滤波过程，提高了算法效率，符合四旋翼飞行器飞行的实时性要求，在数据进行融合前，进行快速的滤波处理，抑制扰动。

简化线性优化粒子滤波算法的思想是：解决基本粒子滤波算法中重要性权值偏差较大的问题，采用高频率的重采样策略，这种方法在工程应用中被证明是有效的［4］；解决高频率重采样策略带来的粒子退化和多样性匮乏问题，引入线性优化策略，提高粒子的多样性；为了满足实时性要求，对线性优化算法进行了简化。

本文首先建立了四旋翼飞行器姿态的基本数学模型，接着研究了四旋翼飞行器姿态角速率估计的改进粒子滤波算法，然后在仿真条件下对文中算法进行试验和评价，最后通过样机飞行数据证明了本文算法的有效性。

1 飞行器模型描述

四旋翼飞行器的机体坐标原点选为机体中心，飞行器姿态使用欧拉角描述，取机头朝向为x轴方向，机体中心指向机体右侧为y轴方向，机体中心垂直水平面指向地心为z轴方向，机体坐标系的选择符合右手定则，绕x轴转动与水平面形成的夹角为滚转角，绕y轴转动与水平面形成的夹角为俯仰角，绕z轴转动与过z轴的水平垂面的夹角为偏航角，分别用φ，θ，ψ表示，角度的正负根据右手定则判断。地面坐标系选择和机体坐标系相同的表示轴系。

结合Paul Pounds等人对四旋翼飞行器的动力学分析［5］，可以获得如下的动力学关系：

体坐标系下三轴转动的角速率同欧拉角速率的关系：

其中,θ，φ，ψ分别为三轴的角度，U为4个旋翼的运动控制量，Ω为4个旋翼的转速，其他为测量获得的物理常量见表1。

根据前述知识，可以根据该四旋翼飞行器模型建立姿态稳定控制器。为了说明问题，本文结合基于反步法的姿态稳定控制器讨论本文中的滤波算法在姿态估计中的性能。

2 改进线性优化粒子滤波算法

粒子滤波经过经典的重采样过程后，粒子的多样性降低［6］，线性优化方法将候选粒子划分为抛弃组与复制组，重采样过程完成后，将抛弃组和复制组的粒子进行线性组合，形成新的粒子集，进入下一次估计。这种方法的优点在于，使用新的粒子代替单一重采样后的粒子，降低了粒子集中粒子的重复率，而且用这种方法产生的粒子分布更接近真实的后验概率分布。本文提出的改进线性优化粒子滤波算法在上述思想基础上进行了适当调整，针对四旋翼飞行器的姿态角速率估计问题，以降低算法复杂度和提高实时性为目的，另外，该方法还能有效提高粒子集的多样性。

2.1 EKF算法

为了设计姿态角速率的粒子滤波算法，需要找到能够比较准确反映姿态角速率概率分布的重要密度函数。由于四旋翼飞行器系统是非线性的，经典卡尔曼滤波算法对其失效，Buay等人针对非线性系统，提出的EKF算法是一种次优滤波方法，其截取非线性系统的泰勒展开式的低阶项，忽略高阶项，将非线性问题线性化［7］。将EKF作用到四旋翼飞行器姿态角速率估计中，得到如图1所示的状态估计曲线，从曲线中可以看出，绝大部分时间里，EKF都比较有效地估计了角速率的状态，局部有所偏差是由于线性化效果不理想导致的［7］。从而得出如下结论，可以通过使用EKF算法获得较为接近真实后验概率分布后验概率密度函数。

在本文算法中，可以用EKF对粒子集进行更新，将最后得到的近似后验密度作为重要性密度函数，即

由此产生新的粒子集，进行权值更新后对粒子集进行重采样，就是所谓的EKPF算法，在此基础上结合改进的线性优化策略，保证运行速率的同时，能够提高算法的精度。

2.2 改进组合方式

经典的线性优化粒子滤波算法的主要思想是：在需要重复采集某个采样点时，通过将采样点和被抛弃的采样点进行合适的线性组合而产生一个新的采样点，线性组合的方式为

式中，xn是通过组合方式产生的新采样点，xa为被重复选择的采样点；xs是被抛弃的采样点，L=（Nw）-1/m，N为粒子个数，m为采样空间维度，w为任意采样点邻域空间内采样点的分布概率，划分粒子为抛弃组和复制组的阈值为：

参考上述线性优化过程，提出一种增大随机度的改进组合方式，主要思想是：在某一组估计进行中，依区分阈值ωThr将粒子集划分为复制组和抛弃组，并分别归一化权值，采用轮盘赌方法对归一化权值后的复制组进行重采样，并得出本次估计的结果，采用轮盘赌方法分别取复制组和抛弃组中的一个采样点，依照式（8）组成一个新的采样点

其中，randn为随机数，用此方法产生N个采样点作为下一次估计使用的粒子集［8］。

2.3 改进的线性优化粒子滤波算法流程

Step1：初始化粒子集，根据需要估计的状态的值域范围，随机生成N个采样点，组成粒子集，本例中，合适的值域范围是［0,1］；

Step2：参考EKPF获得更新后的粒子集，及相应的重要性权值；

Step3：对粒子集进行权值归一化，并将其分别划分为复制组和抛弃组，并分别归一化；

Step4：每次估计均依照改进组合方式过程进行重采样步骤和生成下一组采样点；

Step5：更新状态并返回Step2。

3 数值实验

3.1 仿真实验

仿真算例为上文提到的四旋翼飞行器模型，结合反步法非线性控制系统，获得完整的仿真姿态数据，分别采用EKPF和本文提出的改进粒子滤波算法对四旋翼飞行器的角速率(p，q，r)进行估计。

仿真过程中，传感器采样周期为20 ms，过程噪声方差Q为0.1(rad/s)2，量测噪声方差为0.1(rad/s)2，下页图2所示为使用不同滤波算法对从非平稳姿态调整到平稳姿态过程中的角速率的估计，是四旋翼飞行器姿态调整的重要，并且会反复出现的环节，具备很强的典型性。为了分析算法性能，给出两种算法的均方根误差曲线，均方误差公式为

图3 和图4分别为滚转和俯仰通道的均方根误差曲线，偏航角通道的情况同他们类似，从图3和图4中容易发现，两种算法随着粒子数量的增加，算法的均方根误差减小，本例中，在粒子数为50时，算法的均方根误差基本稳定，相比EKF算法，ILOPF和EKPF的均方根误差更小，而本文的改进算法比EKPF有更小的均方根误差，精度更高。

为了考察本例中两种算法的滤波效率，两种算法的粒子数分别取为100、200和500，运行50次，统计其平均数据，感兴趣的数据为滤波效率、均方根误差和状态估计的平均时间，其中，滤波效率计算为式（10）所示，其中N为算法使用的粒子数目。

结果如表2所示。从表2中的数据可以看出，ILOPF算法和EKPF算法都保有比较稳定的AMSE值，过程噪声增大时，ILOPF算法的AMSE有所降低，说明该算法对噪声的抑制效果比较明显。相同条件下ILOPF算法的AMSE普遍低于EKPF算法，而且更低于EKF算法，说明ILOPF算法的波动小，精度高，从滤波效率观察，容易发现，ILOPF算法的效率比EKPF算法高出20%。研究表2还能发现，ILOPF算法和EKPF算法无法通过增加粒子数目提高滤波效率，所以，针对不同的算例，选择合适的粒子数也对算法的执行效果有一定影响，结合估计时间考虑，两种算法提高估计精度是通过提高算法中使用粒子数和牺牲运算时间获得的，而在粒子数目并非巨大（一般小于500）时，ILOPF算法的各项性能指标都优于EKPF算法，所以在进行一般的状态估计时，优先考虑使用ILOPF算法，而且出于实时性和效率的考虑，建议算法中的粒子数不多于200为宜。

3.2 实验数据

为了进一步验证算法的有效性，将ILOPF算法应用到实验用四旋翼飞行器样机上，该样机使用X650机架，使用STM32F103VET6为主控芯片，使用MPU6050和HMC5883L相结合制作的AHRS，传感器采样周期为20 ms，将ILOPF算法融合到航向姿态解算过程，能够实现飞行器的稳定飞行，飞行器记录仪记录的部分飞行数据绘制成如图5和图6曲线。图5曲线为0 s～25 s内的飞行姿态统计，在这25s内，飞行器姿态从平稳到滚摆并附带一定的偏航角度，最终在25 s左右只进行俯仰操作，从图中容易发现，在由于从MUP6050中获得的角速率信息波动比较大，直接用这种信息进行姿态融合，获得的角度存在很大波动，这种无规律的抖动对控制器的影响很大，控制器使用这样的信息进行输出，机体抖动明显，容易引发系统的高频特性，十分容易倾覆，因此，在进行信息融合前，需要使用滤波算法对角速率进行估计，以平滑并削弱这种波动，从图中曲线容易看出，ILOPF算法有效削弱了量测过程中产生的噪声，姿态曲线比滤波前更加平滑。图6为图5过程中16 s～20 s的细节，从中能够更加清晰地观察到上述情况，在俯仰通道，姿态为趋于0 rad的稳定状态，而由于量测噪声影响，直接融合获得的数据有接近0.05 rad的波动，而经过估计后的数据波动不会超过0.02 rad，有效抑制了噪声，观察其他通道，同样能够获得类似的结果。

4 结束语

本文根据四旋翼飞行器模型的特点，提出了一种改进的线性优化粒子滤波方法。设计的滤波方法过程简单，解决了四旋翼飞行器姿态角速度估计的问题。通过仿真实验，验证了算法对姿态角速率有很好的估计效果，而且均方根误差保持在很低的水平，粒子选择合适的情况下，滤波效率比EKPF算法提高了20%，而且具备很强的实时性。飞行样机记录的飞行数据表明该算法切实可行，可用于一般四旋翼飞行器的姿态角滤波估计。下一步拟结合集群优化算法进一步改进本算法。

［1］Samir B，Andre N，Roland S.PID vs LQ Control Techniques Applied to an Indoor Micro Quadrotor[C]//IEEE，2004：2451-2456.

［2］王帅,魏国.卡尔曼滤波在四旋翼飞行器姿态测量中的应用[J].兵工自动化，2011，30（1）:73-80.

［3］马敏，吴海超.基于四元数自补偿四旋翼飞行器姿态解算[J].制造业自动化，2013，35（23）:18-21.

［4］Crisan D,Doucet A.A Survey of Convergence Results on Particle Filtering Methods for Practitioners[J].Signal Processing, IEEE Transactions on，2002，50（3）：736-746.

［5］Pounds P，Mahony R，Corke P.Modelling and Control of a Large Quadrotor Robot[J].Control Engineering Practice，2010，18（7）：691-699.

［6］Cheng S Y,Zhang J Y.Review on Particle Filters[J].Journal of Astronautics，2008，29（4）：1099-1111.

［7］Garry A E，Langford B W.Robust Extended Kalman Filtering [J].Signal Processing，IEEE Transactions on，1999，47（9）：2596-2599.

［8］Chen J，Yan P，Zhang J Y.Research on Weight Optimal Combination Particle Filter Algorith[J].Computer Engineering and Applications，2009，45（24）：33-35.

［9］Liu Z L，Liu T，Cao J.Fusion Tracking Algorithm Based on Particle Filter[J].Science Technology and Engineering，2009（7）：1751-1753.

［10］Wang L，Xia H P.Application in Target Tracking Based on Tabu Search Particle Filter Algorithm[J].Science Technology and Engineering，2013（6）：1630-1634.

Design and Implementation of ILOPF in Attitude Estimation of Quadrotor

LI Yuan-song，LIANG Jin-ming
（School of Computer，Sichuan University of Science and Engineering，Zigong 643000，China）

For the problem of the volatile fusion result of quadrotor caused by the random noise occurring in the measurement of attitude angular rate.This paper presents an improved linear optimization particle filter with EKF.Firstly,EKF into important density function is integrated; secondly,the combination strategy of linear optimization process is improved,and as the algorithm is real-time,it is simplified according.In the simulation,the algorithm achieves the estimation of attitude angular rate.Statistic shows that compared on EKPF,filtering efficiency of ILOPF increase by 20%. Physics flight with ILOPF demonstrates its effectiveness.

PF，attitude estimation，EKF，quadrotor

TP391

A

1002-0640（2015）09-0167-05

2014-08-15

2014-09-20

四川省教育厅科研基金(13ZA0125)；四川省高校重点实验室开放基金(2014WZY05)；企业信息化与物联网测控技术四川省高校重点实验室项目（2014WZY03）；软件工程专业综合改革基金资助项目（B12201002）

黎远松（1970- ），男，重庆开县人，硕士，副教授。研究方向：智能算法设计等。