四轴飞行器组合导航非线性滤波算法-文章-技术应用-消费电子

四轴飞行器组合导航非线性滤波算法

时间:11-18 09:51 阅读:2115次

*温馨提示：点击图片可以放大观看高清大图

简介：本文研究EKF 和UKF 算法在四轴飞行器视觉组合导航中的应用,针对四轴飞行器的非线性特点,传统的Kalman 滤波方法存在一定的的误差,而EKF 和UKF 在融合视觉位置信息时具有明显的优势,且UKF 比EKF 更适应于四轴飞行器的组合导航系统。

四轴飞行器是一种特殊结构的无人机（Unmanned AerialVehicle,UAV),可以只通过调节四个旋翼的转速,实现对飞行姿态的控制,它可为海上、废墟等不适合人员进入的环境,提供侦察、救援、绘图等服务。UAV 多数使用GPS 或惯性导航系统,然而,在GPS 信号不足或室内环境下,惯性导航系统误差又随时间积累,所以本身不能长时间地提供高精度的导航,需要辅助导航。视觉导航方法,即由机载的视觉传感器获得周围环境的图像信息,然后通过图像算法及相机的位置标定解算出载体的位置,有更大的发展空间,它成本低、质量轻,且易于实现。

视觉导航方法主要集中分为两类：一类是以视觉伺服为主的导航方法,它基于视觉传感器,直接利用图像特征控制飞行器到指定位置,通常有两个控制环,外环控制实时特征与目标特征间的误差,估计飞行器的理想速度,然后,通过内环控制电机转速到理想速度,最后,使特征的图像匹配到目标位置。另一类是视觉辅助导航方法,即融合视觉传感器和惯性传感器给出载体位置,融合的方法多见Kalman 滤波方法。

本文基于四轴飞行器的数学模型,研究扩展卡尔曼滤波（ Extended Kalman Filter, EKF) 和不敏卡尔曼滤波（Unscented Kalman Filter,UKF)两种非线性滤波算法在四轴飞行器视觉辅助导航方法中的应用情况,并对比Kalman 方法,UKF 和EKF 算法融合四轴飞行器多传感器信息的效果。

1四轴飞行器数学模型

四轴飞行器的动力学模型满足如下假设：四轴飞行器是均匀对称的刚体结构;惯性系的原点与飞行器的几何中心一致;飞行器所受阻力及重力与飞行高度无关,保持不变;四轴各向拉力与电机的转速成正比。四轴飞行器的结构模型如图1 所示。

四轴飞行器组合导航非线性滤波算法

图1四轴飞行器的数学模型示意图

图1 中涉及到惯性坐标系－ E 系和载体坐标系－ B 系,φ,θ,ψ 分别表示惯性坐标系下载体的欧拉角,定义Θ =[φ θ ψ]T 。则根据坐标系变换原则,载体系到惯性系的变换矩阵R 如式1 所示,其中c 表示余弦函数, s 表示正弦函数。

四轴飞行器组合导航非线性滤波算法

四轴飞行器电机的推力Ｆ与它的转速ω 的平方成正比：

四轴飞行器组合导航非线性滤波算法

其中： i = 1,2,3,4 分别代表4 个电机, k 为推力系数。则四轴飞行器的总推力为：

四轴飞行器组合导航非线性滤波算法

定义P = [X Y Z] T表示载体在惯性系下的位置矢量,根据牛顿第二定律知,载体的运动加速度与所受力成正比：

四轴飞行器组合导航非线性滤波算法

则载体在惯性系下的线运动方程为：

四轴飞行器组合导航非线性滤波算法

定义p,q,r 表示四轴飞行器在载体系中的运动角速度,则欧拉角与运动角速度的关系为：

四轴飞行器组合导航非线性滤波算法

角运动方程为：

四轴飞行器组合导航非线性滤波算法

其中： I 为惯性矩阵, MR为旋转力矩, M 为飞行器所受力矩,由于四轴飞行器是对称的刚体,且其重心与几何中心重合,所以惯性矩阵为对角阵,对角线元素为Ix、Iy、Iz , 考虑沿水平方向的旋转效应,旋转力矩为：

四轴飞行器组合导航非线性滤波算法

飞行器所受力矩为：

四轴飞行器组合导航非线性滤波算法

则将式（6)展开为：

四轴飞行器组合导航非线性滤波算法

定义控制量如下：

四轴飞行器组合导航非线性滤波算法

2非线性滤波算法

2.1扩展卡尔曼滤波

EKF 算法利用泰勒展开的一次项来对非线性方程作线性化处理, 再结合经典的卡尔曼滤波进行滤波估计。EKF算法简单,易于实现。

设系统的状态方程为：

其中： x 为系统状态矢量, z 为系统观测量; w（k),v（k) 为不相关零均值的高斯噪声; Q（k),R（k) 分别表示其方差矩阵。φ（x),r（x) 表示泰勒展开算子,即线性化过程。

测量更新：

其中J 为雅可比算子。

2.2不敏卡尔曼滤波(UKF)

不敏卡尔曼滤波是基于采样方法的近似法,用带有权值的样本集来近似目标的状态后验概率密度,采用的是确定的样本点,因而避免由线性化而导致的误差。设系统的状态方程为：

其中： xk∈Ｒn为系统状态矢量, zk∈Ｒm为测量量, qk－1∈Ｒn和rk－1∈ Ｒm是高斯噪声,且分别qk－1~ N（0,Qk－1),rk－1~N（0,Rk－1) 分布。初始状态x0的均值和协方差分别记为m0和P0。UKF 算法的根本是无迹变换（Unscented Transform,UT),其过程如下：

先根据系统状态的均值x和方差矩阵Pxx选择一组样本点x0,…,xi和相应权值w0,…,wi：

其中参数k 是标量的常数。

由样本点x0,…,xi经过非线性函数得到一组新的样本点z0,…,zi,zi= h（xi)（i = 0,…,2n)。

用这些带有权值的新样本点计算观测状态的均值和方差

UKF 滤波算法同样包含时间更新与测量更新两个过程：

时间更新：由k － 1 预测值经过上述的无迹变换过程计算k 时刻预测状态的均值和方差矩阵

测量更新：计算k 时刻测量量zk及其均值zk、方差矩阵Pzz,k及协方差矩阵Pxz,k。

3仿真模型建立

四轴飞行器状态矢量, 四轴飞行器的速度和欧拉角由惯性导航单元（ＩnertialＭeasurement Unit, ＩＭU)测量及解算。根据四轴飞行器的运动学方程,建立如式（25)所示的状态方程：

4仿真结果

根据四轴飞行器的数学模型及各滤波算法的递推公式,使用Ｍatlab 对系统进行动态仿真。四轴飞行器的空间状态初始值设为：三个方向的位置初值x（0) = 20 m, y（0) =30 m, z（0) =2 m, 三个方向线速度、角速度和角加速初值为零。图2（a) ~ （c)分别为X,Y,Z 三个方向的载体位置曲线。图中的真实值为视觉导航系统给出的位置估计。