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

转载自http://www.stmcu.org/module/forum/thread-605855-1-1.html

四轴飞行器是一种特殊结构的无人机（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 为惯性矩阵, M R 为旋转力矩, M 为飞行器所受力矩,由于四轴飞行器是对称的刚体,且其重心与几何中心重合,所以惯性矩阵为对角阵,对角线元素为I x 、I y 、I z  , 考虑沿水平方向的旋转效应,旋转力矩为：

飞行器所受力矩为：

则将式（6)展开为：

定义控制量如下：

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

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

其中J 为雅可比算子。

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

其中： x k  ∈Ｒ n  为系统状态矢量, z k  ∈Ｒ m  为测量量, q k－1  ∈Ｒ n 和r k－1  ∈ Ｒ m 是高斯噪声,且分别q k－1  ~ N（0,Q k－1 ),r k－1  ~N（0,R k－1  ) 分布。初始状态x 0  的均值和协方差分别记为m 0 和P 0  。UKF 算法的根本是无迹变换（Unscented Transform,UT),其过程如下： 先根据系统状态的均值x 和方差矩阵P xx  选择一组样本点x 0  ,…,x i  和相应权值w 0  ,…,w i  ：

其中参数k 是标量的常数。 由样本点x 0  ,…,x i  经过非线性函数得到一组新的样本点z 0  ,…,z i  ,z i  = h（x i  )（i = 0,…,2n)。 用这些带有权值的新样本点计算观测状态的均值和方差

UKF 滤波算法同样包含时间更新与测量更新两个过程： 时间更新：由k － 1 预测值经过上述的无迹变换过程计算k 时刻预测状态 的均值 和方差矩阵

测量更新：计算k 时刻测量量z k  及其均值z k  、方差矩阵P zz,k  及协方差矩阵P xz,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 三个方向的载体位置曲线。图中的真实值为视觉导航系统给出的位置估计。

图2　X、Y、Z 方向位置曲线
Kalman 滤波在对非线性特征的四轴飞行器的位置滤波上存在明显的滤波偏差,相比之下,EKF 和UKF 有较好的滤波效果,三个方向的估计值误差曲线如图５、图6、图7 所示。从X,Y,Z 三个方向的估计误差曲线图上,更能直观地看出三种滤波算法的滤波结果,滤波估计误差的均方差如表1 所示。

图3　X、Y、Z 方向估计误差曲线
Kalman 滤波非线性环境下滤波性能较差, 甚至会出现滤波发散点,而EKF 和UKF 能很好地处理四轴飞行器的非线性特性,从误差均方差上看,UKF 优于EKF,这也是由于EKF是将非线性化的系统线性化造成的偏差,而UKF 是基于样本点的滤波方式,对噪声的适应性更强。

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