基于四旋翼机器人平台的非线性滤波算法研究*

陈 俊, 强 俊

(1.安徽机电职业技术学院 电气工程系，安徽 芜湖 241000；2.安徽工程大学 计算机与信息学院，安徽 芜湖 241000)

基于四旋翼机器人平台的非线性滤波算法研究*

陈 俊1, 强 俊2

(1.安徽机电职业技术学院 电气工程系，安徽 芜湖 241000；2.安徽工程大学 计算机与信息学院，安徽 芜湖 241000)

以四旋翼机器人姿态控制为研究目标，用四元数进行姿态描述，非线性滤波算法为主要研究对象，基于Matlab仿真平台，对四旋翼机器人姿态控制进行设计，并以非线性滤波算法的优化为重点，结合常用的滤波算法进行比较，实验结果表明：RPF更适用于机器人的姿态角度更新计算。

四旋翼；Matlab仿真；四元数；非线性滤波；RPF

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

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

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

1 常用的姿态描述方法及其分析

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

(1)

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

(2)

(3)

这里，ω为旋转轴单位矢量，θ为转轴方向上转过的角度。此方法计算量过大，对于有高实时性要求的飞行机器人来说，不适用，并且，在转向θ比较小的情况下，会使得ω产生较大的误差。

四元数法相对来说，具有计算量较小且可实现全姿态工作的优势。四元数是由四个元构成，其基本结构为Q=q0+q1i+q2j+q3k

(4)

(5)

由于机身是运动的，所以用四元数描述对象姿态时，Q本身也是动态的，所以可建立Q对时间的微分方程：

(6)

陀螺的测量误差会随时间累积放大，工作时间越长，产生漂移越大，使得精度降低，故而需要借助其他姿态测量传感器进行修正，所以，姿态确定系统基本都是以陀螺和其他敏感器组合而成。由于姿态敏感器的输出有测量误差，且不一定是载体姿态参数，所以需要先进的滤波算法处理测量数据。如何使用先进的滤波算法，克服不确定因素对姿态估计的影响，从而实现对姿态变换的精确描述，是本文的主要任务。

2 非线性滤波算法

滤波理论从本质上来说就是利用系统观测信号，估计状态概率分布函数。目前应用最广泛的卡尔曼滤波是以最小均方误差为准则，建立的一个线性滤波器。然而，实际应用中通常是非线性状态，在此前提下研究姿态确定系统的滤波问题。贝叶斯最优估计给出了非线性函数滤波最优解，但其系统模型具有非线性特性，算法中的加权积分部分后验概率密度函数难以求得，因而在实际应用中难有作为[4]。

基于此，不少贝叶斯逼近次优滤波算法被相继提出，主要可分为两类：基于系统状态分布为高斯分布的高斯非线性滤波和基于蒙特卡洛采样逼近思想的非高斯滤波，这其中，以扩展卡尔曼滤波(EKF)、粒子滤波(PF)、差值滤波等最具代表性。由于四旋翼飞行器属于非线性非高斯系统，粒子滤波(PF)就是利用蒙特卡洛采样直接逼近系统的状态后验概率密度函数，这从一定程度上实现了非线性非高斯系统的全局近似最优估计。我们对此算法做着重分析研究。

2.1 蒙特卡洛采样思想

2.2 序贯重要性重采样算法(SIR)

依据贝叶斯重要性采样，考虑到其递推的特点，为避免每次新的观测值都重新采样计算样本权值，序贯重要性采样(SIS)算法被提出，其基本思想是从一个重要性概率密度中抽取一组带有权值的样本，以用于代表系统对象的后验概率密度函数，最后通过这组样本的加权求和做状态估计值。

SIS的问题在于，这组样本在经过迭代计算后会产生退化问题，具体表现为大部分样本数据(粒子)的权值极小至可忽略，致使整体样本权值的方差逐步增大，导致大量的运算做了“无用功”，我们称之为样本退化现象。基于此，序贯重要性重采样算法(SIR)应运而生。它针对原有样本集合粒子的离散分布，采取随机选取复制的方式，以“权值较高粒子获得较大概率复制”为原则，避免了退化问题，然而，这种方式使得许多权值较小的粒子失去了被复制的机会，造成粒子多样性的丢失，从而使得滤波精度下降。所以，粒子滤波的重点在于：在合理的重要性概率密度选取下，不至于使粒子多样性丢失的重采样的优化。

2.3 粒子滤波算法的优化

如前文所述，SIR是系统状态在离散近似分布下，依据粒子的权值大小进行复制更新，使得粒子中原本处于“少数派”的高权重粒子得到了复制，而低权重的粒子得到了“忽略”，从而抑制了粒子多样性。基于此，引入Musso提出的正则化粒子滤波(RPF)用于姿态估计。该算法与SIR最大的不同在于，它将在系统状态的连续近似分布下进行采样，从而有效解决了重采样导致的粒子贫化问题。

RPF的基本思路是：

(1) 初始化。产生服从初始概率密度p(x0)的N个粒子，记为x0(i)，i=1,2,…,N。

(3) 量测更新。更新并归一化粒子权重ωk(i)，计算状态估计值xk和方差Pk：

(7)

(8)

(9)

(10)

(5) 状态更新。

(11)

(12)

RPF算法在一定程度上较好解决了粒子贫化问题，虽然增加了核密度函数采样，但计算量并未明显增大，在系统干扰不大的情况下，可提高姿态解算精度[5]。

3 仿真实验

基于Matlab R2015b版本仿真平台，考虑到本文采用了粒子滤波的重采样和正则化方法，同时抑制了粒子退化和多样性贫化问题，所以粒子数目可有效降低，初始粒子总数N为500[6]。

对陀螺仪以500 hz采样频率采集10 s，假定噪声分布为非高斯情况，设置系统初始状态x=0.1，仿真运行时间为5 000个节点，初始过程噪声方差Q=10，初始测量噪声方差R=5，后验误差初值P=5。仿真实验结果如图1和图2所示。为了显示清楚，特将100个节点下的仿真结果如图1所示。

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

图1 RPF和SIR滤波实验对比及误差对比图(100节点)Fig.1 The error comparison of RPF and SIR filtering experiment (100 nodes)

图2 RPF和SIR滤波实验对比及误差对比图(5000节点)Fig.2 The error comparison of RPF and SIR filtering experiment (5000 nodes)

4 结束语

以四旋翼飞行机器人的姿态解算为对象，分析了姿态解算的可实施方法，以飞行器的姿态确定系统为切入点，提出了将RPF融入姿态解算，并与SIR算法作比较，较好地解决了粒子退化和多样性贫化问题，对四旋翼飞行机器人姿态解算问题提供了较有价值的解决途径。

[1] CORKE P I,ROBOTICS.Vision & Control:Fundamental Algorithms in MATLAB[M].Berlin:Springer-Verlag,2011

[2] KITAGAWA G.Monte Carlo Filter and Smoother for Non-Gaussian Nonlinear State Space Models[J].Journal of Computational & Graphical Statistics,2012,5(1):1-25

[3] MELLINGER D, KUMAR V.Minimum Snap Trajectory Generation and Control for Quadrotors[C]∥Robotics and Automation(ICRA).Shanghai,China,2011

[4] 乔相伟.基于四元数非线性滤波的飞行器姿态确定算法研究[D].哈尔滨：哈尔滨工程大学,2011

QIAO X W.Attitude Determination Algorithm Based on Quaternion Nonlinear Filter Foe Spacecraft[D].Harbin:Harbin Engineering University,2011

[5] 曹蓓.粒子滤波改进算法及其应用研究[D].西安：中国科学院,2012

CAO B.Research on Improved Algorithms and Applications Based on Particle Filter[D].Xi’an:University of Chinese Academy of Sciences,2012

[6] 陈俊.智能算法在足球机器人定向运动中的应用[J].重庆工商大学学报(自然科学版),2013,30(3):46-49

CHEN J.Application of Intelligence Algorithm to Oriented Movement of Soccer Robot[J].Chongqing Technology and Business University(Natural Science Edition),2013,30(3):46-49

责任编辑：李翠薇

Research on Nonlinear Filtering Algorithm Based on Quadrotors

CHEN Jun1, QIANG Jun2

(1.Anhui Mechanical College of Mechanic and Electrical Engineering, Anhui Wuhu 241000, China; 2. School of Computer and Information Science, Anhui Polytechnic University, Anhui Wuhu 241000, China)

This paper takes the attitude control of quadrotors as the research object, takes quaternion as pose description, uses nonlinear filtering algorithm as the main research target, based on Matlab simulation platform, makes the attitude control design for the quadrotors, takes the optimization of nonlinear filtering algorithm as the emphasis, and makes comparison by combining usual filtering algorithms.The experimental results show that RPF is more applicable to attitude angel renew calculation.

quadrotors; Matlab simulation; quaternion; nonlinear filtering; RPF

10.16055/j.issn.1672-058X.2017.0003.014

2016-08-20；

2016-10-20. * 基金项目：安徽高校自然科学研究重点项目(KJ2015A353)；安徽高校自然科学研究重点项目(KJ2016A802)；安徽省高等教育提升计划省级自然科学研究一般项目(TSKJ2015B16)；安徽工程大学计算机应用重点实验室开放基金(JSJKF201501).

陈俊(1982-)，男，安徽芜湖人，副教授，硕士研究生，从事智能控制和自动检测研究.

TP181

A

1672-058X(2017)03-0077-05