目录
基于扩展卡尔曼滤波(Extended Kalman Filter, EKF)的6自由度(Degrees of Freedom, DOF)无人机全球定位系统(Global Positioning System, GPS)与惯性导航系统(Inertial Navigation System, INS)融合算法,旨在利用GPS提供的绝对定位信息与INS提供的连续、自主的导航信息,互补各自的优缺点,实现无人机高精度、实时的定位与姿态估计。
1.EKF算法描述
EKF是一种广泛应用在非线性系统状态估计中的递归滤波器,通过线性化非线性模型并结合卡尔曼滤波框架,实现对系统状态和误差协方差的在线更新。GPS提供精确的位置、速度和时间信息,但易受遮挡、多径效应等因素影响,导致定位精度降低甚至中断。INS则基于加速度计和陀螺仪测量无人机的运动学参数,具有全自主、连续导航的优点,但长时间运行下会积累误差。EKF-GPS/INS融合算法旨在充分利用两者优势,提高无人机导航精度和鲁棒性。
EKF算法主要包括预测阶段(Prediction Step)和更新阶段(Update Step):
预测阶段
更新阶段
2.融合模型
无人机的高精度导航是实现其自主飞行、精准作业的关键。全球定位系统(Global Positioning System, GPS)与惯性导航系统(Inertial Navigation System, INS)作为两种常用的导航手段,各自具备独特优势。GPS提供绝对、实时的位置和速度信息,但易受到遮挡、多径效应、卫星失锁等因素影响,导致定位精度降低甚至失效。INS则利用加速度计和陀螺仪连续测量无人机的运动信息,实现全自主、连续导航,但长期累积误差较大。为克服单一导航系统的局限性,实现高精度、高稳定性的导航性能,通常采用多传感器融合策略,其中基于扩展卡尔曼滤波(Extended Kalman Filter, EKF)的6自由度(Degrees of Freedom, DOF)无人机GPS-INS融合算法被广泛应用。
对于6DOF无人机GPS-INS融合,系统状态x包含位置、速度、姿态角及其导数等信息,观测z来自GPS接收机(位置、速度)和INS输出(加速度、角速度)。系统动态模型f(⋅)描述无人机运动学特性,观测模型ℎ(⋅)h(⋅)对应GPS和INS数据转换关系。过程噪声wk和观测噪声vk分别反映系统内部不确定性(如传感器噪声、模型误差)和外部干扰(如GPS多径效应)。
3.matlab程序
.........................................................................................
toc
t = 1:(k);
figure(1)
plot(t,P_R(:,1:3),'LineWidth',2)
title('位置的协同作用')
legend('px','py','pz')
grid on
figure(2)
plot(t,P_R(:,4:6),'LineWidth',2)
legend('pxdot','pydot','pzdot')
title('速度的协同作用')
grid on
figure(3)
plot(t,P_R(:,7:10),'LineWidth',2)
title('四元数的协方差')
legend('q0cov','q1cov','q2cov','q3cov')
grid on
figure(8)
plot(t,phi,t,theta,t,psi,t,phi_raw,'b:',t,theta_raw,'g:',t,psi_raw,'r:','LineWidth',2)
title('Phi, Theta, Psi')
legend('phi','theta','psi','phiraw', 'thetaraw', 'psiraw')
grid on
figure(4)
hold on
plot(t,xhatR(:,1:3),t,A(7:9,:),':','LineWidth',2)
title('位置')
legend('px','py','pz','px gps','py gps','pz gps')
grid on
figure(5)
plot(t,xhatR(:,4:6),t,A(10:12,:),':','LineWidth',2)
title('vel x y z')
legend('vx','vy','vz','vx gps','vy gps','vz gps')
grid on
figure(6)
plot(t,xhatR(:,7:10),t,A(13:16,:),':','LineWidth',2)
legend('qo','q1','q2','q3','qodata','q1data','q2data','q3data')
title('四元数')
grid on
figure(9)
plot(t,xhatR(:,11:16),'LineWidth',2)
title('Bias')
legend('bp','bq','br','bfx','bfy','bfz')
grid on
figure(7)
plot(t,OMEGA(:,1:3),'LineWidth',2)
title('OMEGA')
legend('p','q','r')
grid on
figure(10)
plot(t,OMEGA_raw(:,1),t,OMEGA_raw(:,2),t,OMEGA_raw(:,3),'LineWidth',2)
title('OMEGA raw')
legend('p','q','r')
grid on
figure(11)
plot(t,FX(:,1),t,FX(:,2),t,FX(:,3),'LineWidth',2)
title('加速度')
legend('ax','ay','az')
grid on
up4024
4.仿真结果
基于EKF的6DOF无人机GPS-INS融合算法结合了GPS的绝对定位能力和INS的自主连续导航能力,有效提高了无人机导航系统的精度、稳定性和鲁棒性。随着无人机应用领域的不断拓展和技术进步,对导航性能的要求将越来越高,未来的研究应聚焦于高级滤波技术、自适应滤波、多传感器融合以及故障诊断与容错等方面,推动无人机导航技术的持续创新与发展。