一种应用于四旋翼机器人平台的非线性滤波算法制造技术

本发明专利技术公开了一种应用于四旋翼机器人平台的非线性滤波算法，包括如下步骤：利用姿态敏感器的多次测量输出多个测量结果，作为服从初始概率密度p(x0)的N个粒子，记为x0(i)，i＝1,2,…,N；系统状态转移概率密度中抽取更新粒子，步骤三：更新并归一化粒子权重ωk(i)，计算状态估计值xk和方差Pk；从步骤三中的ωk(i)和xk(i)中进行重采样得到

A nonlinear filtering algorithm applied to four rotor robot platform

The invention discloses a nonlinear filtering algorithm was applied to the four rotor robot platform, which comprises the following steps: using the attitude sensor measuring output multiple measurements, as to the initial probability density p (x0) N particles, denoted as x0 (I), I = 1,2,... , N; extraction system update particle, the probability density of state transition step three: update and normalized particle weight Omega K (I), the calculation of state estimation and variance of XK Pk; the third step from K (I) and XK (I) were resampled

【技术实现步骤摘要】
一种应用于四旋翼机器人平台的非线性滤波算法
本专利技术属于四旋翼机器人
，具体涉及一种应用于四旋翼机器人平台的非线性滤波算法。
技术介绍
近年来，随着微机电系统、新型材料(玻纤、碳纤等)以及传感技术的蓬勃发展，飞行机器人取得了飞速的发展和应用，伴随着其负载能力、续航能力的提高，飞行机器人正不断开拓功能，延伸到各个应用领域。四旋翼机器人伴随着其快速的普及，逐步成为飞行机器人领域的标准研究平台。一般四旋翼飞行器直观上来看是由4个独立的螺旋桨通过十字框架结构相连，位于机身前方的螺旋桨为1号螺旋桨，奇数序的螺旋桨按逆时针方向旋转，偶数序的按顺时针方向旋转，机器人的定位、启动、升降或是航向均是由这四个螺旋桨协同工作实现。因此，如何结合机器人的当前姿态，对各螺旋桨的转速进行实时控制，是实现四旋翼飞行器机体稳定可控的关键问题。四旋翼飞行器是典型的欠驱动系统，输入量只有各螺旋桨的拉力，输出量则包括了机身的速度、加速度、姿态(倾斜角度)、水平位移以及高度等等。在飞行器的姿态解算中，依靠的最重要部件就是惯性器件——加速度计和陀螺仪。加速度计具有良好的低频特性，可以测量低速的静态加速度；陀螺仪具有较好的高频特性，可以测量高速旋转运动。所以，要想实现对飞行器的精准控制，就要结合先进的滤波技术，得到精确的姿态解算结果，这是飞行器运行的最关键问题。
技术实现思路
根据以上现有技术的不足，本专利技术所要解决的技术问题是提出一种应用于四旋翼机器人平台的非线性滤波算法，得到精确的姿态解算结果。为了解决上述技术问题，本专利技术采用的技术方案为：一种应用于四旋翼机器人平台的非线性滤波算法，其特征在于,包括如下步骤：步骤一：利用姿态敏感器的多次测量输出多个测量结果，作为服从初始概率密度p(x0)的N个粒子，记为x0(i)，i＝1,2,…,N；步骤二：系统状态转移概率密度中抽取更新粒子，xk(i)～p(xk(i)|xk-1(i))；步骤三：更新并归一化粒子权重ωk(i)，计算状态估计值xk和方差Pk；步骤四：从步骤三中的ωk(i)和xk(i)中进行重采样得到步骤五：由更新状态估计值和方差Pk。作为一种优选实施方式：所述步骤三中，状态估计值xk和方差Pk的具体求解方式为：所述步骤四中，具体求解方式为：其中，Kh(·)为核密度函数，h为核带宽，为正数，n是维数，由Epanechnikov核函数抽取样本ek(i)，计算所述步骤五中，状态估计值和方差Pk为：本专利技术有益效果是:在系统干扰不是很大的情况下，本专利技术滤波算法在姿态解算中更好的解决了粒子多样性缺失呈现的贫化问题，且在运算速度上与SIR保持相当，从误差图上来看，RPF的姿态解算精度比SIR有较为显著的提升，且并没有牺牲系统的实时性。附图说明下面对本说明书附图所表达的内容及图中的标记作简要说明：图1是本专利技术具体实施方式的RPF和SIR滤波实验100节点下对比及误差对比图。图2是本专利技术具体实施方式的RPF和SIR滤波实验5000节点下对比及误差对比图。具体实施方式下面通过对实施例的描述，本专利技术的具体实施方式如所涉及的各构件的形状、构造、各部分之间的相互位置及连接关系、各部分的作用及工作原理、制造工艺及操作使用方法等，作进一步详细的说明，以帮助本领域技术人员对本专利技术的专利技术构思、技术方案有更完整、准确和深入的理解。常用的姿态描述方法包括了欧拉角法、方向余弦阵法、罗德里格参数法、四元数法等。欧拉角法是将机体的运动转化为三次定轴转动，分别为绕z轴转Ψ，绕y轴转θ，绕x轴转φ，从而建立欧拉角微分方程，结合tait-bryan公式，p、q、r为三个坐标轴方向的角速度，则可以得到如下解：从解可以看到，由于其自身缺陷，无法覆盖整个欧式空间，当时，无解，陷入万向节死锁。方向余弦阵可以做到与姿态变换的一一对应，基于李代数中的反对称矩阵计算，可建立其微分方程公式：R＝e[ωs]tR0＝e[ω]θR0(3)这里，ω为旋转轴单位矢量，θ为转轴方向上转过的角度。此方法计算量过大，对于有高实时性要求的飞行机器人来说，不适用，并且，在转向θ比较小的情况下，会使得ω产生较大的误差。四元数法相对来说，具有计算量较小且可实现全姿态工作的优势。四元数是由四个元构成，其基本结构为：Q＝q0+q1i+q2j+q3k(4)这里，q0～q3为实数，i、j、k为单位向量，且相互正交，虚单位为它既可以看作是四维空间向量，也可以看作是一个超复数。如果四元数Q能够确定，则可以根据公式(5)中的四元数矩阵与方向余弦阵中元素的对应关系，再转换为欧拉角(俯仰角θ、滚动角φ以及偏航角Ψ)，从而解得姿态。所以，相比于直接用方向余弦阵求解，只需求得q0～q3这四个量，计算量显然小得多，工程上更易于实现。由于机身是运动的，所以用四元数描述对象姿态时，Q本身也是动态的，所以可建立Q对时间的微分方程：这里，角速度ω＝[ωxωyωz]T，可由陀螺仪测量处理后得到。而后，通过四阶-龙格库塔法求更新解Q(t+T)。至此，陀螺仪所得数据成为机身姿态更新的关键要素。陀螺的测量误差会随时间累积放大，工作时间越长，会产生漂移越大，使得精度降低，故而需要借助其他姿态测量传感器进行修正，所以，姿态确定系统基本都是以陀螺和其他敏感器组合而成。由于姿态敏感器的输出有测量误差，且不一定是载体姿态参数，所以需要先进的滤波算法处理测量数据。如何使用先进的滤波算法，克服不确定因素对姿态估计的影响，从而实现对姿态变换的精确描述。本专利技术采用正则化粒子滤波用于姿态估计，该算法在系统状态的连续近似分布下进行采样，从而有效解决了重采样导致的粒子贫化问题。正则化粒子滤波的基本思路是：1)初始化。产生服从初始概率密度p(x0)的N个粒子，记为x0(i)，i＝1,2,…,N；2)时间更新。系统状态转移概率密度中抽取更新粒子，xk(i)～p(xk(i)|xk-1(i))；3)量测更新。更新并归一化粒子权重ωk(i)，计算状态估计值xk和方差Pk；4)正则化重采样。从以上两步中的xk(i)和ωk(i)中进行重采样，依据式(9)重采样得到这里，Kh(·)即为核密度函数，h为核带宽且为正数，n是维数。由Epanechnikov核函数抽取样本ek(i)，计算5)状态更新。RPF算法在一定程度上较好解决了粒子贫化问题，虽然增加了核密度函数采样，但计算量并未明显增大，在系统干扰不大的情况下，可提高姿态解算精度。基于MatlabR2015b版本仿真平台，考虑到本专利技术采用了粒子滤波的重采样和正则化方法，同时抑制了粒子退化和多样性贫化问题，所以粒子数目可有效的降低，初始粒子总数N为500。对陀螺仪以500hz采样频率采集10s，假定噪声分布为非高斯情况，设置系统初始状态x＝0.1，仿真运行时间为5000个节点，初始过程噪声方差Q＝10，初始测量噪声方差R＝5，后验误差初值P＝5。仿真实验结果如图1和图2所示。为了显示清楚，特将100个节点下的仿真结果如图1所示，5000个节点下的仿真结果如图2所示。可以看到，在系统干扰不是很大的情况下，正则化粒子滤波算法在姿态解算中更好的解决了粒子多样性缺失呈现的贫化问题，且在运算速度上与SIR保持相当，从误差图上来看，RPF的姿态解算精度比SIR有较为显著的提升，且并没有牺牲系统的实时性。上面对本本文档来自技高网...

【技术保护点】
一种应用于四旋翼机器人平台的非线性滤波算法，其特征在于,包括如下步骤：步骤一：利用姿态敏感器的多次测量输出多个测量结果，作为服从初始概率密度p(x0)的N个粒子，记为x0(i)，i＝1,2,…,N；步骤二：系统状态转移概率密度中抽取更新粒子，xk(i)～p(xk(i)|xk‑1(i))；步骤三：更新并归一化粒子权重ωk(i)，计算状态估计值xk和方差Pk；步骤四：从步骤三中的ωk(i)和xk(i)中进行重采样得到

【技术特征摘要】
1.一种应用于四旋翼机器人平台的非线性滤波算法，其特征在于,包括如下步骤：步骤一：利用姿态敏感器的多次测量输出多个测量结果，作为服从初始概率密度p(x0)的N个粒子，记为x0(i)，i＝1,2,…,N；步骤二：系统状态转移概率密度中抽取更新粒子，xk(i)～p(xk(i)|xk-1(i))；步骤三：更新并归一化粒子权重ωk(i)，计算状态估计值xk和方差Pk；步骤四：从步骤三中的ωk(i)和xk(i)中进行重采样得到步骤五：由更新状态估计值和方差Pk。2.根...

【专利技术属性】
技术研发人员：陈俊，汪家民，
申请(专利权)人：安徽机电职业技术学院，
类型：发明
国别省市：安徽,34