阅读说明：本技术 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 (Unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large-mobility condition ) 是由 沈锋 卢旺 徐定杰 周威 赵雨晴 于 2018-08-02 设计创作，主要内容包括：本发明提供了一种基于低成本MEMS传感器和单天线GPS的应用于无人机在大机动条件下的姿态估计算法。该算法将GPS测得的速度值差分,得到的结果对IMU测得的线性加速度进行补偿,采用互补滤波融合算法,再利用校正后的加速度信息修正陀螺仪的漂移,从而得到当前姿态角的准确估计,并在无人机采用BTT控制飞行过程中实时利用GPS信息对偏航角进行校正,输出融合并校正的最佳姿态信息。(The invention provides an attitude estimation algorithm based on a low-cost MEMS sensor and a single-antenna GPS and applied to an unmanned aerial vehicle under a large maneuvering condition. The algorithm differentiates speed values measured by a GPS, the obtained result compensates linear acceleration measured by an IMU, a complementary filtering fusion algorithm is adopted, corrected acceleration information is used for correcting drift of a gyroscope, so that accurate estimation of the current attitude angle is obtained, the GPS information is used for correcting the yaw angle in real time in the process that the unmanned aerial vehicle controls flight by adopting the BTT, and the fused and corrected optimal attitude information is output.)

一种基于单天线GPS和IMU的大机动条件下无人机姿态估计 方法

技术领域

本发明属于无人机导航技术领域，具体涉及一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法。

背景技术

无人驾驶飞机(UnmannedAerialVehicle)又简称无人机，是一种利用无线电远程遥控及机载的程序控制器操纵的不载人飞机。其最早出现于20世纪20年代，当时被用于作为军事训练中的靶机，此后经过近百年的不断发展，逐渐转向于侦查、攻击等各种多用途领域。由于其相对于载人飞机来说具有成本低、生存能力强、无人员伤亡风险、使用方便等优点，所以不止能在军事上发挥重要作用，在民用领域也具有广阔的应用前景。

姿态估计是无人机实现姿态控制的前提，是导航系统不可分割的一个重要组成部分，直接影响无人机的生存能力。传统的无人机姿态控制中，姿态角可以由陀螺仪积分得出，也可以通过加速度传感器测量重力加速度在载体系三轴上的向量分解坐标推导得出。其中陀螺仪解算姿态角高频动态响应特性良好，其输出能够迅速响应姿态角的变化；无高频噪声干扰，输出值平滑；陀螺仪的输出不受外部加速度的干扰，在载体处于剧烈震动的情况下仍能维持稳定的输出。但其也具有一些缺陷:如陀螺仪的零点会随温度及其它外部环境因素的改变而产生漂移、对陀螺仪输出值的积分会产生累计误差，长时间运行后解算误差较大。而利用加速度计解算姿态角低频特性良好，静态输出稳定，没有漂移和累积误差；姿态解算计算量小，在没有外部加速度时(仅有重力加速度)的情况下，能够迅速得出准确的瞬时姿态角数据。其具有如下缺陷:有高频噪声干扰、高频动态特性较差，其输出不能迅速响应姿态角的快速变化、容易受到外部加速度的干扰，如果存在除重力加速度之外的加速度，将无法得出准确的姿态角。

发明内容

现有无人机的姿态估计算法，在大机动条件下，加速度计测量值之中包含有害的线性加速度可能会大大的超出重力加速度，从而为算法的稳定性和准确性带来不好的影响。另外，地磁矢量十分微弱，如果无人机工作环境中存在较强的磁干扰，甚至会使有用信号淹没在噪声中。本发明引入了GPS测量的速度信息，对加速度计进行补偿，使无人机在机动条件下仍然可以获得准确的姿态估计值，并在运动时利用GPS信息对偏航角进行校正。针对现有技术不足，本发明提出了一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法。

本发明的技术方案是：

步骤1：设定初始姿态四元数，并对传感器数据进行处理。对陀螺仪和加速度计的输出数据进行低通滤波，并对GPS测量值进行差分后得到线性加速度来补偿加速度计数据；

步骤2：根据姿态四元数构造转换矩阵，同时利用加速度计数据计算参考姿态四元数误差量；

步骤3：设计PI控制器完成加速度计对陀螺仪漂移的补偿；

步骤4：对补偿后的陀螺仪数据积分，之后将GPS的得到的偏航角测量值转化为姿态四元数，与互补滤波得到的四元数进行融合，最终转化为对应的无人机姿态角。

2.根据权利要求1所述的基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，其特征在于，步骤1包括以下步骤：

步骤1.1：选取姿态四元数作为状态向量：

取初始值为x＝[1,0,0,0]；

步骤1.2：利用一阶低通滤波器G(s)＝a/(s+a)对陀螺仪和加速度计传感器数据进行预处理，消除振动产生的传感器干扰。利用三阶最优差分器对GPS速度测量值进行差分得到线性运动加速度a_gps＝(0.0971d₁+0.1662d₂+0.1853d₃)/2T_gps。

3.根据权利要求1所述的基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，其特征在于，步骤2包括以下步骤：

步骤2.1：将加速度计数据进行归一化

a_acc＝[a_x/norm,a_y/norm,a_z/norm]

构造姿态四元数转换矩阵

使用GPS差分结果补偿加速度计测量值，利用四元数求解所得到的旋转矩阵

求解算出估计值

并对参考加速度矢量进行归一化

a＝[a_xgps/norm,a_ygps/norm,(a_zgps+g)/norm]

步骤2.2：叉乘得到参考四元数误差向量

[e_x e_y e_z]^T＝[v_x v_y v_z]^T×[a_ccx a_ccy a_ccz]^T

4.根据权利要求1所述的基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，其特征在于，步骤3包括以下步骤：

步骤3.1：展开得到计算误差，利用PI修正陀螺仪

5.根据权利要求1所述的基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，其特征在于，步骤4包括以下步骤：

步骤4.1：利用修正后的陀螺仪的数据更新四元数，方法采用龙格库塔积分

gyro＝[g_x g_y g_z]

gyro＝gyro+δ

q₀＝q₀+(-q₁*g_x-q₂*g_y-q₃*g_z)*ΔT

q₁＝q₁+(q₀*g_z+q₂*g_z-q₃*g_y)*ΔT

q₂＝q₂+(q₀*g_y-q₁*g_z+q₃*g_x)*ΔT

q₃＝q₃+(q₀*g_z+q₁*g_y-q₂*g_x)*ΔT

步骤4.2：将GPS测得的偏航角ψ_gps参考值融入四元数中

q₀＝cos(φ)cos(θ)cos(ψ_gps)+sin(φ)sin(θ)sin(ψ_gps)；

q₁＝sin(φ)cos(θ)cos(ψ_gps)-cos(φ)sin(θ)sin(ψ_gps)；

q₂＝cos(φ)sin(θ)cos(ψ_gps)+sin(φ)cos(θ)sin(ψ_gps)；

q₃＝cos(φ)cos(θ)sin(ψ_gps)-sin(φ)sin(θ)cos(ψ_gps)；

步骤4.3：四元数归一化后，转化为姿态角

roll＝arctan(2*(q₀*q₁+q₂*q₃)/(1-2(q₁*q₁+q₂*q₂)))

pitch＝arcsin(2*(q₀*q₂-q₁*q₃))

yaw＝arctan(2*(q₀*q₃+q₂*q₁)/(2(q₁*q₁+q₀*q₀)-1))

本发明的有益效果：

(1)本发明提出了一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，本发明引入了GPS测量的速度信息，差分后对加速度计进行补偿，使无人机在机动条件下仍然可以获得准确的姿态估计值；

(2)本发明利用GPS信息测得的速度矢量与正北方向的夹角代替偏航角，弥补了磁强计在磁干扰环境下的劣势；

(3)本发明对使用GPS数据校正后的IMU输出数据应用互补滤波进行数据融合的处理，得到最终的姿态导航信息，算法运算速度快；

(4)本发明对硬件要求低，通过软件编程可以对一系列的算法加以实现，成本较低。

附图说明

图1为本发明的基于单天线GPS和IMU的大机动条件下无人机姿态估计方法整体框图。

图2为本发明的PI控制器原理图。

图3为坐标系示意图。

具体实施方式

以下结合附图对本发明进行详细的说明。

一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，如图1所示，包括以下步骤：

步骤1：设定初始姿态四元数，并对传感器数据进行处理。对陀螺仪和加速度计的输出数据进行低通滤波，并对GPS测量值进行差分得到线性加速度补偿加速度计数据；

步骤2：根据姿态四元数构造转换矩阵，同时利用加速度计数据计算参考姿态四元数误差量，坐标系定义如图3所示；

步骤3：设计PI控制器完成加速度计对陀螺仪漂移的补偿，PI控制器的原理如图2所示；

步骤4：对补偿后的陀螺仪数据积分，之后将GPS的得到的偏航角测量值转化为姿态四元数，与互补滤波得到的四元数进行融合，最终转化为对应的无人机姿态角。

步骤1具体包括以下步骤：

步骤1.1：选取姿态四元数作为状态向量：

取初始值为x＝[1,0,0,0]；

步骤1.2：利用一阶低通滤波器G(s)＝a/(s+a)对陀螺仪和加速度计传感器数据进行预处理，消除振动产生的传感器干扰。利用三阶最优差分器对GPS速度测量值进行差分得到线性运动加速度a_gps＝(0.0971d₁+0.1662d₂+0.1853d₃)/2T_gps。

步骤2具体包括以下步骤：

步骤2.1：将加速度计数据进行归一化

a_acc＝[a_x/norm,a_y/norm,a_z/norm]

构造姿态四元数转换矩阵

使用GPS差分结果补偿加速度计测量值，利用四元数求解所得到的旋转矩阵

求解算出估计值

并对参考加速度矢量进行归一化

a＝[a_xgps/norm,a_ygps/norm,(a_zgps+g)/norm]

步骤2.2：叉乘得到参考四元数误差向量

[e_x e_y e_z]^T＝[v_x v_y v_z]^T×[a_ccx a_ccy a_ccz]^T

步骤3具体包括以下步骤：

步骤3.1：展开得到计算误差，利用PI修正陀螺仪

步骤4具体包括以下步骤：

步骤4.1：利用修正后的陀螺仪的数据更新四元数，方法采用龙格库塔积分

gyro＝[g_x g_y g_z]

gyro＝gyro+δ

q₀＝q₀+(-q₁*g_x-q₂*g_y-q₃*g_z)*ΔT

q₁＝q₁+(q₀*g_z+q₂*g_z-q₃*g_y)*ΔT

q₂＝q₂+(q₀*g_y-q₁*g_z+q₃*g_x)*ΔT

q₃＝q₃+(q₀*g_z+q₁*g_y-q₂*g_x)*ΔT

步骤4.2：将GPS测得的偏航角ψ_gps参考值融入四元数中

q₀＝cos(φ)cos(θ)cos(ψ_gps)+sin(φ)sin(θ)sin(ψ_gps)；

q₁＝sin(φ)cos(θ)cos(ψ_gps)-cos(φ)sin(θ)sin(ψ_gps)；

q₂＝cos(φ)sin(θ)cos(ψ_gps)+sin(φ)cos(θ)sin(ψ_gps)；

q₃＝cos(φ)cos(θ)sin(ψ_gps)-sin(φ)sin(θ)cos(ψ_gps)；

步骤4.3：四元数归一化后，转化为姿态角

roll＝arctan(2*(q₀*q₁+q₂*q₃)/(1-2(q₁*q₁+q₂*q₂)))

pitch＝arcsin(2*(q₀*q₂-q₁*q₃))

yaw＝arctan(2*(q₀*q₃+q₂*q₁)/(2(q₁*q₁+q₀*q₀)-1))

本发明的有益效果如下：

(1)本发明提出了一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，本发明引入了GPS测量的速度信息，差分后对加速度计进行补偿，使无人机在机动条件下仍然可以获得准确的姿态估计值；

(2)本发明利用GPS信息测得的速度矢量与正北方向的夹角代替偏航角，弥补了磁强计在磁干扰环境下的劣势；

(3)本发明对使用GPS数据校正后的IMU输出数据应用互补滤波进行数据融合的处理，得到最终的姿态导航信息，算法运算速度快；

(4)本发明对硬件要求低，通过软件编程可以对一系列的算法加以实现，成本较低。