一种基于ilopf的四旋翼姿态估计方法
【技术领域】
[0001] 本发明属于四旋翼飞行器姿态角速率的测量技术领域，具体涉及一种基于IL0PF 的四旋翼姿态估计方法。
【背景技术】
[0002] 四旋翼飞行器是典型的多耦合、非线性欠驱动系统，近年来，因其飞行条件低，物 理易实现等特点，成为控制领域研究的新热点。多国研究机构为其进行数学建模、控制器设 计和飞行样机制作。为了实现飞行器的有效控制，需要准确描述飞行器的飞行姿态。加速 度在动态过程中由于受到高频振动的影响，无法单独解算出空间的姿态角，因此，四旋翼飞 行器的姿态信息由三轴加速度和三轴角速率经由信息融合算法获得，陀螺仪在测量过程中 由于受到扰动和自身量测噪声影响，角速率数据会产生高频波动，直接使用这样的数据进 行融合获得的姿态信息无法用于控制系统。常用的抗干扰方法有卡尔曼滤波算法、四元数 融合技术及其他机械抗抖动措施，其中卡尔曼滤波器或互补滤波器不能很好得对四旋翼飞 行器的非线性环节进行估计，四元数算法能解决姿态角解算中出现奇点的问题，不过不能 很好得抵抗噪声，机械措施设计难度大，实现成本高。
【发明内容】
[0003] 本发明目的之一在于提供一种实施简单、滤波效率尚、实时性尚、精度尚的基于 IL0PF的四旋翼姿态估计方法。
[0004] 本发明提供的一种基于IL0PF的四旋翼姿态估计方法，包括如下步骤：
[0005] 步骤S1 :初始化粒子集，根据需要估计的状态的值域范围，随机生成N个采样点， 组成粒子集；
[0006] 步骤S2 :利用EKF方法对粒子集进行更新，将得到的近似后验密度作为重要性密 度函数，最后产生新的粒子集；
[0007] 步骤S3 :对所述步骤S2中产生的新的粒子集进行权值更新；
[0008] 步骤S4 :依据区分阈值将权值更新后的粒子集划分为复制组和抛弃组，并分别归 一化权值；
[0009] 步骤S5 :采用轮盘赌方法对归一化权值后的复制组进行重采样，并得出本次估计 的结果。
[0010] 进一步的，所述四旋翼姿态估计方法还包括步骤S6 :采用轮盘赌方法分别取所述 步骤S4中的复制组和抛弃组中的一个采样点，利用下式组成一个新的采样点，
[0011] xn=Xa+randn(xa-xs)，其中randn为随机数；
[0012] 循环N次，产生N个采样点作为下一次估计使用的粒子集。
[0013] 本发明的有益效果在于，本发明解决了基本粒子滤波算法中重要性权值偏差较大 和高频率重采样策略带来的粒子退化和多样性匮乏等问题；通过引入简化的线性优化策 略，提高了粒子的多样性，满足了实时性要求，提高了算法效率，通过快速的滤波处理，能够 有效抑制扰动。
【附图说明】
[0014] 图1所示为本发明基于IL0PF的四旋翼姿态估计方法流程图。
【具体实施方式】
[0015] 下文将结合具体实施例详细描述本发明。应当注意的是，下述实施例中描述的技 术特征或者技术特征的组合不应当被认为是孤立的，它们可以被相互组合从而达到更好的 技术效果。
[0016] 如图1所示，本发明提供的一种基于改进线性优化粒子滤波算法（Improved LinearOptimizationofParticleFilter，简称IL0PF)的四旋翼姿态估计方法包括如下步 骤：
[0017] 步骤S1 :初始化粒子集，根据需要估计的状态的值域范围，随机生成N个采样点， 组成粒子集。
[0018] 步骤S2 :利用扩展卡尔曼滤波（ExtendedKalmanFilter，简称EKF)方法对粒子 集进行更新，将得到的近似后验密度作为重要性密度函数，最后产生新的粒子集。
[0019] EKF方法是在每一时刻按照式（1)对后验概率进行近似，
[0020]
(1)
[0021 ] 其中美为k时刻的状态估计值;> 为k时刻的估计方差。
[0022] 用EKF方法对粒子集进行更新，将最后得到的近似后验密度作为重要性密度函 数，即1 ，由此产生新的粒子集。
[0023] 步骤S3 :对所述步骤S2中产生的新的粒子集进行权值更新。
[0024] 步骤S4 :依据区分阈值将权值更新后的粒子集划分为复制组和抛弃组，并分别归 一化权值。
[0025] 在需要重复采集某个采样点时，通过将采样点和被抛弃的采样点进行合适的线性 组合而产生一个新的采样点，线性组合的方式为：
[0026] xn=xa+L(xa-xs) (2)
[0027] 其中，xn是通过组合方式产生的新采样点，xa为被重复选择的采样点；xs是被抛 弃的采样点，L= (Nw) 1/m，N为粒子个数，m为采样空间维度，w为任意采样点邻域空间内采 样点的分布概率，划分粒子为抛弃组和复制组的阈值为：
[0028]
[0029] 参考上述线性优化过程，提出一种增大随机度的改进组合方式，主要思想是：在某 一组估计进行中，依区分阈值《 将粒子集划分为复制组和抛弃组，并分别归一化权值。
[0030] 步骤S5 :采用轮盘赌方法对归一化权值后的复制组进行重采样，并得出本次估计 的结果；
[0031] 所述四旋翼姿态估计方法还包括步骤S6 :采用轮盘赌方法分别取所述步骤S4中 的复制组和抛弃组中的一个采样点，利用下式组成一个新的采样点，
[0032] xn= x a+randn (x a-xs) (4)
[0033] 其中randn为随机数；如此循环N次，产生N个采样点作为下一次估计使用的粒子 集，然后转至步骤S2进行重新估计。
[0034] 粒子滤波经过经典的重采样过程后，粒子的多样性降低，线性优化方法将候选粒 子划分为抛弃组与复制组，重采样过程完成后，将抛弃组和复制组的粒子进行线性组合，形 成新的粒子集，进入下一次估计。这种方法的优点在于，使用新的粒子代替单一重采样后的 粒子，降低了粒子集中粒子的重复率，而且用这种方法产生的粒子分布更接近真实的后验 概率分布。本发明提出的改进线性优化粒子滤波算法在上述思想基础上进行了适当调整， 针对四旋翼飞行器的姿态角速率估计问题，以降低算法复杂度和提高实时性为目的。
[0035] 本发明解决了基本粒子滤波算法中重要性权值偏差较大和高频率重采样策略带 来的粒子退化和多样性匮乏等问题；通过引入简化的线性优化策略，提高了粒子的多样性， 满足了实时性要求，提高了算法效率，通过快速的滤波处理，能够有效抑制扰动。
[0036] 本文虽然已经给出了本发明的一些实施例，但是本领域的技术人员应当理解，在 不脱离本发明精神的情况下，可以对本文的实施例进行改变。上述实施例只是示例性的，不 应以本文的实施例作为本发明权利范围的限定。
【主权项】
1. 一种基于ILOPF的四旋翼姿态估计方法，其特征在于，包括如下步骤： 步骤Sl :初始化粒子集：根据需要估计的状态的值域范围，随机生成N个采样点，组成 粒子集； 步骤S2 :利用EKF方法对粒子集进行更新，将得到的近似后验密度作为重要性密度函 数，最后产生新的粒子集； 步骤S3 :对所述步骤S2中产生的新的粒子集进行权值更新； 步骤S4 :依据区分阈值将权值更新后的粒子集划分为复制组和抛弃组，并分别归一化 权值； 步骤S5 :采用轮盘赌方法对归一化权值后的复制组进行重采样，并得出本次估计的结 果。2. 如权利要求1所述的一种基于ILOPF的四旋翼姿态估计方法，其特征在于，所述四旋 翼姿态估计方法还包括步骤S6 :采用轮盘赌方法分别取所述步骤S4中的复制组和抛弃组 中的一个采样点，利用下式组成一个新的采样点， xn= X a+randn(xa-xs)，其中 randn 为随机数； 循环N次，产生N个采样点作为下一次估计使用的粒子集。
【专利摘要】本发明提供了一种基于ILOPF的四旋翼姿态估计方法，包括如下步骤：步骤S1：初始化粒子集，根据需要估计的状态的值域范围，随机生成N个采样点，组成粒子集；步骤S2：利用EKF方法对粒子集进行更新，将得到的近似后验密度作为重要性密度函数，最后产生新的粒子集；步骤S3：对所述步骤S2中产生的新的粒子集进行权值更新；步骤S4：依据区分阈值将权值更新后的粒子集划分为复制组和抛弃组，并分别归一化权值；步骤S5：采用轮盘赌方法对归一化权值后的复制组进行重采样，并得出本次估计的结果。本发明提高了粒子的多样性，满足了实时性要求，提高了算法效率，通过快速的滤波处理，能够有效抑制扰动。
【IPC分类】G01C21/20
【公开号】CN105021197
【申请号】CN201510366148
【发明人】黎远松, 彭龑, 刘小芳, 梁金明
【申请人】四川理工学院
【公开日】2015年11月4日
【申请日】2015年6月26日