提出了一种改进的Sage-Husa自适应扩展Kalman滤波算法, 用于保证多旋翼无人机在噪声统计特性未知且时变、振动为主要扰动源、姿态角高动态变化等飞行条件下飞行姿态角解算的精度与稳定性。该算法采用微机电系统陀螺仪实时动态解算的姿态角方差估计系统噪声方差; 并采用自适应滤波算法在线估计量测噪声方差, 从而保证滤波的精度与稳定性; 同时引入滤波器收敛性判据, 结合强跟踪Kalman滤波算法来抑制滤波发散。飞行实验与分析表明: 改进算法解算的俯仰角与横滚角均方根误差分别为1.722°和1.182°, 明显优于常规的Sage-Husa自适应滤波算法。实验还显示: 改进的算法自适应能力强、实时性好、精度高、运行可靠, 能够满足多旋翼无人机自主飞行的需要, 若对参数进行适当修改, 还可应用于其它动态性能要求较高的导航信息测量系统中。

An improved Sage-Husa adaptive extended Kalman filter algorithm is proposed to ensure the precision and stability of calculating attitude angles of a multi-rotor Unmanned Aerial Vehicle(UAV) under the actual flight conditions, such as unknown and time-varied noise statistical properties, main disturbance source in vibration and attitude angles high dynamically changed. The algorithm uses attitude angle variance estimated by a gyroscope in real time to estimate system noise variance and only adopts an adaptive filter algorithm to estimate measurement noise variance on-line to ensure the precision and stability of filtering. Meanwhile, it introduces the criterion of filter convergence to restrain the divergence of Kalman filter through combining with a strong tracking Kalman filter algorithm. A flight experiment and corresponding analysis show that the root-mean-square errors of the pinch and roll angles estimated by the improved algorithm are 1.722° and 1.182°, obviously better than that of the conventional Sage-Husa adaptive Kalman filter algorithm. It concludes that the improved algorithm has strong adaptive ability, good real-time performance, high precision and reliable operation. It meets the need of multi-rotor UAV autonomous flight and can be applied to other navigation information measuring systems with high dynamic performance requirements if the parameters are modified appropriately.