[发明专利]一种应用于四旋翼机器人平台的非线性滤波算法

说明书

技术领域

本发明属于四旋翼机器人技术领域，具体涉及一种应用于四旋翼机器人平台的非线性滤波算法。

背景技术

近年来，随着微机电系统、新型材料(玻纤、碳纤等)以及传感技术的蓬勃发展，飞行机器人取得了飞速的发展和应用，伴随着其负载能力、续航能力的提高，飞行机器人正不断开拓功能，延伸到各个应用领域。

四旋翼机器人伴随着其快速的普及，逐步成为飞行机器人领域的标准研究平台。一般四旋翼飞行器直观上来看是由4个独立的螺旋桨通过十字框架结构相连，位于机身前方的螺旋桨为1号螺旋桨，奇数序的螺旋桨按逆时针方向旋转，偶数序的按顺时针方向旋转，机器人的定位、启动、升降或是航向均是由这四个螺旋桨协同工作实现。因此，如何结合机器人的当前姿态，对各螺旋桨的转速进行实时控制，是实现四旋翼飞行器机体稳定可控的关键问题。

四旋翼飞行器是典型的欠驱动系统，输入量只有各螺旋桨的拉力，输出量则包括了机身的速度、加速度、姿态(倾斜角度)、水平位移以及高度等等。在飞行器的姿态解算中，依靠的最重要部件就是惯性器件——加速度计和陀螺仪。加速度计具有良好的低频特性，可以测量低速的静态加速度；陀螺仪具有较好的高频特性，可以测量高速旋转运动。所以，要想实现对飞行器的精准控制，就要结合先进的滤波技术，得到精确的姿态解算结果，这是飞行器运行的最关键问题。

发明内容

根据以上现有技术的不足，本发明所要解决的技术问题是提出一种应用于四旋翼机器人平台的非线性滤波算法，得到精确的姿态解算结果。

为了解决上述技术问题，本发明采用的技术方案为：一种应用于四旋翼机器人平台的非线性滤波算法，其特征在于,包括如下步骤：步骤一：利用姿态敏感器的多次测量输出多个测量结果，作为服从初始概率密度p(x₀)的N个粒子，记为x₀(i)，i＝1,2,…,N；

步骤二：系统状态转移概率密度中抽取更新粒子，x_k(i)～p(x_k(i)|x_k-1(i))；

步骤三：更新并归一化粒子权重ω_k(i)，计算状态估计值x_k和方差P_k；

步骤四：从步骤三中的ω_k(i)和x_k(i)中进行重采样得到

步骤五：由更新状态估计值和方差P_k。

作为一种优选实施方式：所述步骤三中，状态估计值x_k和方差P_k的具体求解方式为：

所述步骤四中，具体求解方式为：

其中，K_h(·)为核密度函数，h为核带宽，为正数，n是维数，由Epanechnikov核函数抽取样本e_k(i)，计算所述步骤五中，状态估计值和方差P_k为：

本发明有益效果是:在系统干扰不是很大的情况下，本发明滤波算法在姿态解算中更好的解决了粒子多样性缺失呈现的贫化问题，且在运算速度上与SIR保持相当，从误差图上来看，RPF的姿态解算精度比SIR有较为显著的提升，且并没有牺牲系统的实时性。

附图说明

下面对本说明书附图所表达的内容及图中的标记作简要说明：

图1是本发明具体实施方式的RPF和SIR滤波实验100节点下对比及误差对比图。

图2是本发明具体实施方式的RPF和SIR滤波实验5000节点下对比及误差对比图。

具体实施方式

下面通过对实施例的描述，本发明的具体实施方式如所涉及的各构件的形状、构造、各部分之间的相互位置及连接关系、各部分的作用及工作原理、制造工艺及操作使用方法等，作进一步详细的说明，以帮助本领域技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

常用的姿态描述方法包括了欧拉角法、方向余弦阵法、罗德里格参数法、四元数法等。欧拉角法是将机体的运动转化为三次定轴转动，分别为绕z轴转Ψ，绕y轴转θ，绕x轴转φ，从而建立欧拉角微分方程，结合tait-bryan公式，p、q、r为三个坐标轴方向的角速度，则可以得到如下解：

从解可以看到，由于其自身缺陷，无法覆盖整个欧式空间，当时，无解，陷入万向节死锁。

方向余弦阵可以做到与姿态变换的一一对应，基于李代数中的反对称矩阵计算，可建立其微分方程公式：