四轴飞行器是一种特殊结构的无人机(Unmanned AerialVehicle,UAV),可以只通过调节四个旋翼的转速,实现对飞行姿态的控制,它可为海上、废墟等不适合人员进入的环境,提供侦察、救援、绘图等服务。UAV 多数使用GPS 或惯性导航系统,然而,在GPS 信号不足或室内环境下,惯性导航系统误差又随时间积累,所以本身不能长时间地提供高精度的导航,需要辅助导航 。视觉导航方法,即由机载的视觉传感器获得周围环境的图像信息,然后通过图像算法及相机的位置标定解算出载体的位置,有更大的发展空间,它成本低、质量轻,且易于实现。 3 f% K. U8 q, w) f' _$ J2 O8 s- n 视觉导航方法主要集中分为两类:一类是以视觉伺服为主的导航方法,它基于视觉传感器,直接利用图像特征控制飞行器到指定位置,通常有两个控制环,外环控制实时特征与目标特征间的误差,估计飞行器的理想速度,然后,通过内环控制电机转速到理想速度,最后,使特征的图像匹配到目标位置。另一类是视觉辅助导航方法,即融合视觉传感器和惯性传感器给出载体位置,融合的方法多见Kalman 滤波方法。 本文基于四轴飞行器的数学模型,研究扩展卡尔曼滤波( Extended Kalman Filter, EKF) 和不敏卡尔曼滤波(Unscented Kalman Filter,UKF)两种非线性滤波算法在四轴飞行器视觉辅助导航方法中的应用情况,并对比Kalman 方法,UKF 和EKF 算法融合四轴飞行器多传感器信息的效果。( \6 @$ H, z, n0 p8 \ 1 四轴飞行器数学模型 四轴飞行器的动力学模型满足如下假设:四轴飞行器是均匀对称的刚体结构;惯性系的原点与飞行器的几何中心一致;飞行器所受阻力及重力与飞行高度无关,保持不变;四轴各向拉力与电机的转速成正比。四轴飞行器的结构模型如图1 所示。 图1 四轴飞行器的数学模型示意图2 X2 M& _/ \1 A$ V) l: g . |9 o) U" {1 x6 f& C 2 非线性滤波算法 2.1 扩展卡尔曼滤波6 e% @6 J! |0 ]% Q; _) e# ~4 k EKF 算法利用泰勒展开的一次项来对非线性方程作线性化处理, 再结合经典的卡尔曼滤波进行滤波估计。EKF算法简单,易于实现。 设系统的状态方程为:其中: x 为系统状态矢量, z 为系统观测量; w(k),v(k) 为不相关零均值的高斯噪声; Q(k),R(k) 分别表示其方差矩阵。φ(x),r(x) 表示泰勒展开算子,即线性化过程。 测量更新: 1 ^0 n" S3 `. K0 J( v, a 2.2 不敏卡尔曼滤波(UKF) 不敏卡尔曼滤波是基于采样方法的近似法,用带有权值的样本集来近似目标的状态后验概率密度,采用的是确定的样本点,因而避免由线性化而导致的误差。设系统的状态方程为:其中: xk ∈Rn 为系统状态矢量, zk ∈Rm 为测量量, qk-1 ∈Rn和rk-1 ∈ Rm是高斯噪声,且分别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 : 3 仿真模型建立 ! g# P% w" J) s; i 四轴飞行器状态矢量, 四轴飞行器的速度和欧拉角由惯性导航单元(InertialMeasurement Unit, IMU)测量及解算。根据四轴飞行器的运动学方程,建立如式(25)所示的状态方程: 4 仿真结果 根据四轴飞行器的数学模型及各滤波算法的递推公式,使用Matlab 对系统进行动态仿真。四轴飞行器的空间状态初始值设为:三个方向的位置初值x(0) = 20 m, y(0) =30 m, z(0) =2 m, 三个方向线速度、角速度和角加速初值为零。图2(a) ~ (c)分别为X,Y,Z 三个方向的载体位置曲线。图中的真实值为视觉导航系统给出的位置估计。 Kalman 滤波在对非线性特征的四轴飞行器的位置滤波上存在明显的滤波偏差,相比之下,EKF 和UKF 有较好的滤波效果,三个方向的估计值误差曲线如图5、图6、图7 所示。从X,Y,Z 三个方向的估计误差曲线图上,更能直观地看出三种滤波算法的滤波结果,滤波估计误差的均方差如表1 所示。 Kalman 滤波非线性环境下滤波性能较差, 甚至会出现滤波发散点,而EKF 和UKF 能很好地处理四轴飞行器的非线性特性,从误差均方差上看,UKF 优于EKF,这也是由于EKF是将非线性化的系统线性化造成的偏差,而UKF 是基于样本点的滤波方式,对噪声的适应性更强。# y0 J& f' T2 [$ ~ 5 结语 5 ]+ O8 z2 n% W! ]' ^ 本文研究EKF 和UKF 算法在四轴飞行器视觉组合导航中的应用,针对四轴飞行器的非线性特点,传统的Kalman 滤波方法存在一定的的误差,而EKF 和UKF 在融合视觉位置信息时具有明显的优势,且UKF 比EKF 更适应于四轴飞行器的组合导航系统。$ I, _* a- |6 w7 p0 I7 g& p & ^' f! ]# Z9 g/ G# Q6 C7 N8 P $ o8 T. g/ p5 @( t0 } |
谢谢楼主分享 |
谢楼主~~mark |
扭 |
谢谢楼主分享,只能默默的留名备用,还没有达到玩这个的水平 |
非常感谢分享,希望楼主能继续出一个系列 |
默默留名,楼主好厉害 |
感谢分享, 请问是否有UKF的执行代码? |
都是理论,没有实际的算法 |