CN110081878A - 一种多旋翼无人机的姿态及位置确定方法 - Google Patents

Abstract

本发明涉及多传感器融合技术领域，提供一种多旋翼无人机的姿态及位置确定方法。首先利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机的三轴加速度、三轴角速度、三轴磁力值、gps位置、距地面高度数据；然后进行数据预处理；接着利用三轴角速度和三轴加速度、三轴磁力值分别解算多旋翼无人机的姿态，再对上两步解算出的姿态进行扩展卡尔曼滤波融合；再利用gps位置解算多旋翼无人机的水平位置，结合三轴加速度和解算出的姿态，对上一步解算出的水平位置进行扩展卡尔曼滤波融合，利用距地面高度数据解算多旋翼无人机的垂直位置。本发明能够提高多旋翼无人机姿态及位置确定的精度及效率。

Description

一种多旋翼无人机的姿态及位置确定方法

技术领域

本发明涉及多传感器融合技术领域，特别是涉及一种多旋翼无人机的姿态及位置确定方法。

背景技术

随着多旋翼无人机问世以来，多旋翼无人机越来越受到人们的热烈追捧，也在各个方面广泛应用。目前，多旋翼无人机在航拍、农业、植保、微型自拍、快递运输、灾难救援、观察野生动物、监控传染病、测绘、新闻报道、电力巡检、影视拍摄等各个行业都起到了重要的作用，大大地拓展了无人机本身的用途，发达国家也在积极扩展行业应用与发展无人机技术。

多旋翼无人机的广泛应用，也使得无人机要面对更复杂的工作环境。目前无人机工作时的主要控制方法是人的手动遥控和程序的自动控制，手动遥控有安全性风险，易撞击坠毁，且易受到电磁信号干扰。

面对较为复杂的工作环境，无人机的自动控制比手动遥控更为安全可靠，相应地无人机也需要对自身物理状态(位置与姿态)有一个较为精确的估计，以便实现对无人机的精确控制。目前求解无人机位置与姿态的方法主要有卡尔曼滤波和互补滤波两种，求解姿态角融合加速度计与陀螺仪信息，求解位姿用到gps与加速度计以及姿态角。由于gps信息输出频率通常比较低，利用传统的方法融合求得的无人机位置离散化严重，不能满足精确的控制。

发明内容

针对现有技术存在的问题，本发明提供一种多旋翼无人机的姿态及位置确定方法，能够提高多旋翼无人机姿态及位置确定的精度及效率。

本发明的技术方案为：

一种多旋翼无人机的姿态及位置确定方法，所述多旋翼无人机配置有惯性测量单元、gps模块、超声波传感器，所述惯性测量单元包括加速度计、陀螺仪、磁力计，其特征在于，包括下述步骤：

步骤1：进行数据采集：以采样周期T_s，利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机在第k时刻的三轴加速度A_bk＝[a_xk,a_yk,a_zk]^T、三轴角速度Ω_bk＝[w_xk,w_yk,w_zk]^T、三轴磁力值M_bk＝[m_xk,m_yk,m_zk]^T、gps位置[λ_k,φ_k,h_k]^T、距地面高度h_vk；其中，b代表数据是机体坐标系中的，多旋翼无人机的导航坐标系为ENU坐标系，多旋翼无人机的起飞原点为导航坐标系的原点，λ_k、φ_k、h_k分别为WGS-84坐标系中多旋翼无人机在第k时刻的经度、纬度、海拔，k∈{0,1,2,...,K}，K为采样总数；

步骤2：进行数据预处理：对三轴加速度A_k进行低通滤波处理；

步骤3：解算多旋翼无人机的姿态：

步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：

步骤3.1.1：计算多旋翼无人机的偏航角ψ_k、翻滚角θ_k、俯仰角γ_k对应的旋转矩阵分别为

步骤3.1.2：将多旋翼无人机的三次旋转表示为

其中，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的z_n轴旋转角度ψ_k，再绕旋转后的坐标系的y'_n轴旋转角度θ_k，再绕旋转后坐标系的x”_n轴旋转角度γ_k；为导航坐标系下的三轴角速度；

步骤3.1.3：由公式(4)得到欧拉角方程为：

步骤3.1.4：对式(5)进行一阶线性离散化，得到：

步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：

步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为

从而机体坐标系到导航坐标系的坐标转换矩阵为

步骤3.2.2：机体坐标系中的三轴加速度A_bk＝[a_xk,a_yk,a_zk]^T转换到导航坐标系中为A_nk＝[0,0,g]^T，计算得到

由于加速度信息与偏航角ψ_k无关，令ψ_k＝0，得到

其中，g为重力加速度；

步骤3.2.3：机体坐标系中的三轴磁力值M_bk＝[m_xk,m_yk,m_zk]^T转换到导航坐标系中为M_nk＝[m'_xk,m'_yk,m'_zk]^T：

计算得到地磁场水平分量为

计算得出偏航角为

步骤3.3：对步骤3.1和步骤3.2中解算出的姿态进行扩展卡尔曼滤波融合；

步骤3.3.1：构建多旋翼无人机姿态的状态方程和观测方程分别为

X_1k＝f(X_1,k-1)+W_1,k-1 (16)

Z_1k＝H_kX_1k+V_1k (17)

其中，X_1k＝[γ_k,θ_k,ψ_k]^T、Z_1k＝[z_γk,z_θk,z_ψk]^T分别为第k时刻多旋翼无人机的姿态状态向量、姿态测量向量，f(X_1,k-1)表示的关系为式(6)表示的关系，z_γk、z_θk、z_ψk分别为式(10)中的γ_k、式(9)中的θ_k、式(14)中的ψ_k；W_1k＝[δ_γk,δ_θk,δ_ψk]^T、V_1k＝[v_γk,v_θk,v_ψk]^T分别为第一噪声向量、第二噪声向量，均由惯性测量单元标定测量后确定，E[W_1k]＝q_k，Cov(W_1k,W_1k)＝Q_k，E[V_1k]＝r_k，Cov(V_1k,V_1k)＝R_k，Cov(W_1k,V_1k)＝0；对非线性的f(X_1,k-1)用扩展卡尔曼滤波进行线性化，得到

从而多旋翼无人机姿态的状态方程可以表示为

步骤3.3.2：估计第k时刻姿态的状态变量为

步骤3.3.3：计算第k时刻姿态的均方误差为

步骤3.3.4：计算第k时刻姿态的滤波增益为

步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为

将作为多旋翼无人机在第k时刻的最终姿态；

步骤3.3.6：更新均方误差为：

P_1k＝[I-v_1kH_k]P_1,k/k-1 (24)

其中，I为单位矩阵；

步骤4：解算多旋翼无人机的位置：

步骤4.1：利用gps位置数据解算多旋翼无人机的水平位置：

步骤4.1.1：将WGS-84坐标系中的gps位置数据[λ_k,φ_k,h_k]^T转换为ECEF坐标系中的位置数据[x_Ek,y_Ek,z_Ek]^T：

其中，a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；

步骤4.1.2：将ECEF坐标系中的位置数据[x_Ek,y_Ek,z_Ek]^T转换为ENU坐标系中的位置数据[x_k,y_k,z_k]^T：

其中，[x_E0,y_E0,z_E0]^T为导航坐标系的原点在ECEF坐标系中的坐标表示：[λ₀,φ₀,h₀]^T为多旋翼无人机在起飞原点时gps模块输出的gps位置数据；

步骤4.2：结合三轴加速度数据和步骤3中解算出的多旋翼无人机在第k时刻的姿态数据，对步骤4.1中解算出的水平位置进行扩展卡尔曼滤波融合：

步骤4.2.1：构建多旋翼无人机水平位置的状态方程和观测方程分别为

X_2k＝ΦX_2,k-1+Bu_k-1+ΓW_2,k-1 (27)

Z_2k＝HX_2k+V_2k (28)

其中，X_2k＝[S_xk,S_yk,ve_xk,ve_yk]^T、Z_2k＝[S_gxk,S_gyk]^T分别为第k时刻多旋翼无人机的水平位置状态向量、水平位置测量向量，(S_xk,S_yk)、(ve_xk,ve_yk)分别为第k时刻多旋翼无人机在导航坐标系中的水平位置、水平速度，S_gxk、S_gyk分别为式(26)中的x_k、y_k； 为第k时刻多旋翼无人机在导航坐标系中的水平加速度，为中的前两项，中的γ_k、θ_k、ψ_k由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W_2k、V_2k均为2×1维系统噪声向量，W_2k的方差统计特性完全由u_k的统计特性决定，Φ、B、H均为常数矩阵，

步骤4.2.2：估计第k时刻水平位置的状态变量为

步骤4.2.3：计算第k时刻水平位置的均方误差为

其中，P_2,k/k-1不随每次滤波迭代，P均为常数矩阵，

步骤4.2.4：计算第k时刻水平位置的滤波增益为

其中，为常数矩阵，

步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为

将为多旋翼无人机在第k时刻的最终水平位置和水平速度；

步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度h_vk作为多旋翼无人机在第k时刻的的垂直位置S_zk。

所述步骤2中，低通滤波的方法为算术平均滤波法，将三轴加速度的m个连续采样值相加后取算术平均值作为低通滤波后的值：

所述步骤3.2.3中，对式(14)进行修正得到：

本发明的有益效果为：

本发明通过不同数据的融合，得到多旋翼无人机的最优姿态和位置：利用多旋翼无人机的三轴角速度数据和低通滤波后的三轴加速度数据、三轴磁力值数据分别解算多旋翼无人机的姿态，然后对解算出的姿态进行扩展卡尔曼滤波融合，得到多旋翼无人机的最优姿态；并利用gps位置数据解算多旋翼无人机的水平位置，然后结合三轴加速度数据和解算出的姿态数据，对gps位置数据解算出的水平位置进行扩展卡尔曼滤波融合，得到多旋翼无人机的最优水平位置；由超声波传感器输出的距地面高度数据解算出多旋翼无人机的垂直位置，大大提高了多旋翼无人机姿态及位置确定的精度及效率。

附图说明

图1为本发明多旋翼无人机的姿态及位置确定方法的流程图；

图2为实施例中对加速度数据进行低通滤波的效果图；

图3为多旋翼无人机的旋转过程示意图；

图4为WGS-84坐标系的YOZ平面剖面示意图；

图5为WGS-84坐标系的XOY平面剖面示意图；

图6为实施例中未经滤波前直接由gps数据解算出的多旋翼无人机在水平面上的运动轨迹示意图；

图7为实施例中本发明多旋翼无人机的姿态及位置确定方法解算出的多旋翼无人机在水平面上的运动轨迹示意图。

具体实施方式

下面将结合附图和具体实施方式，对本发明作进一步描述。

如图1所示，为本发明多旋翼无人机的姿态及位置确定方法的流程图。本实施例中，所述多旋翼无人机为四旋翼无人机ParrortARDrone2.0。ParrortARDrone2.0是法国的一家无人机厂商生产的无人机产品，主要配置有惯性测量单元IMU、gps模块、超声波传感器、前置相机等，该超声波传感器为定高超声波传感器，所述惯性测量单元包括加速度计、陀螺仪、磁力计。本发明的多旋翼无人机的姿态及位置确定方法，其特征在于，包括下述步骤：

步骤1：进行数据采集：以采样周期T_s，利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机在第k时刻的三轴加速度A_bk＝[a_xk,a_yk,a_zk]^T、三轴角速度Ω_bk＝[w_xk,w_yk,w_zk]^T、三轴磁力值M_bk＝[m_xk,m_yk,m_zk]^T、gps位置[λ_k,φ_k,h_k]^T、距地面高度h_vk；其中，b代表数据是机体坐标系中的，多旋翼无人机的导航坐标系为ENU坐标系，多旋翼无人机的起飞原点为导航坐标系的原点，λ_k、φ_k、h_k分别为WGS-84坐标系中多旋翼无人机在第k时刻的经度、纬度、海拔，k∈{0,1,2,...,K}，K为采样总数。本实施例中，T_s＝0.02s，K＝4000。

其中，由于IMU是固连在无人机机体上的，所以加速度计、陀螺仪、磁力计输出的数据都是在机体坐标系下的。

步骤2：进行数据预处理：对三轴加速度A_k进行低通滤波处理；

无人机飞行时，加速度计的数据输出受到机体振动的影响较大，需要进行低通滤波。本实施例中，低通滤波的方法为算术平均滤波法，将三轴加速度的m＝16个连续采样值相加后取算术平均值作为低通滤波后的值：对多旋翼无人机的y轴加速度数据进行低通滤波的效果图如图2所示。其中，散点是未处理的数据，线条是处理后的数据。可以看出，经过低通滤波后得到的加速度数据曲线比较平滑。

步骤3：解算多旋翼无人机的姿态：

导航坐标系选的是ENU即东-北-天坐标系，其x轴为正东方向、y轴为正北方向、z轴则与重力方向相反向上。无人机的偏航角ψ_k、翻滚角θ_k、俯仰角γ_k都是以导航坐标系为参考系。

步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：

步骤3.1.1：计算多旋翼无人机的偏航角ψ_k、翻滚角θ_k、俯仰角γ_k对应的旋转矩阵分别为

步骤3.1.2：将多旋翼无人机的三次旋转表示为

其中，如图3所示，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的z_n轴旋转角度ψ_k，再绕旋转后的坐标系的y'_n轴旋转角度θ_k，再绕旋转后坐标系的x'_n'轴旋转角度γ_k；为导航坐标系下的三轴角速度；

步骤3.1.3：由公式(4)得到欧拉角方程为：

步骤3.1.4：对式(5)进行一阶线性离散化，得到：

步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：

步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为

从而机体坐标系到导航坐标系的坐标转换矩阵为

无人机在导航坐标系下处于任意姿态并保持静止时，加速度计测量输出三轴加速度A_bk＝[a_xk,a_yk,a_zk]^T。由于重力存在，其加速度计输出值A_bk转换到导航坐标系下后一定为A_nk＝[0,0,g]^T，从而：

步骤3.2.2：机体坐标系中的三轴加速度A_bk＝[a_xk,a_yk,a_zk]^T转换到导航坐标系中为A_nk＝[0,0,g]^T，计算得到

由于加速度信息与偏航角ψ_k无关，令ψ_k＝0，得到

其中，g为重力加速度；

步骤3.2.3：机体坐标系中的三轴磁力值M_bk＝[m_xk,m_yk,m_zk]^T转换到导航坐标系中为M_nk＝[m'_xk,m'_yk,m'_zk]^T：

计算得到地磁场水平分量为

计算得出偏航角为

所述步骤3.2.3中，对式(14)进行修正得到：

步骤3.3：对步骤3.1和步骤3.2中解算出的姿态进行扩展卡尔曼滤波融合；

步骤3.3.1：构建多旋翼无人机姿态的状态方程和观测方程分别为

X_1k＝f(X_1,k-1)+W_1,k-1 (16)

Z_1k＝H_kX_1k+V_1k (17)

其中，X_1k＝[γ_k,θ_k,ψ_k]^T、Z_1k＝[z_γk,z_θk,z_ψk]^T分别为第k时刻多旋翼无人机的姿态状态向量、姿态测量向量，f(X_1,k-1)表示的关系为式(6)表示的关系，z_γk、z_θk、z_ψk分别为式(10)中的γ_k、式(9)中的θ_k、式(14)中的ψ_k；W_1k＝[δ_γk,δ_θk,δ_ψk]^T、V_1k＝[v_γk,v_θk,v_ψk]^T分别为第一噪声向量、第二噪声向量，均由惯性测量单元标定测量后确定，E[W_1k]＝q_k，Cov(W_1k,W_1k)＝Q_k，E[V_1k]＝r_k，Cov(V_1k,V_1k)＝R_k，Cov(W_1k,V_1k)＝0；对非线性的f(X_1,k-1)用扩展卡尔曼滤波进行线性化，得到

从而多旋翼无人机姿态的状态方程可以表示为

其中，无人机姿态信息融合系统为非线性系统，式(16)、式(17)为其离散时间姿态状态模型。

步骤3.3.2：估计第k时刻姿态的状态变量为

步骤3.3.3：计算第k时刻姿态的均方误差为

其中，P_1,k/k-1是经过式(20)后所产生的方差；

步骤3.3.4：计算第k时刻姿态的滤波增益为

其中，滤波增益v_1k代表通过加速度计和磁力计所得到的观测值Z_1k在融合结果中所占的权重。

步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为

将作为多旋翼无人机在第k时刻的最终姿态；

步骤3.3.6：更新均方误差为：

P_1k＝[I-v_1kH_k]P_1,k/k-1 (24)

其中，I为单位矩阵；P_1k是经过式(23)后产生的新方差，为k+1时刻所用。本实施例中，P_1k的初始值为Q_k、R_k均为常数矩阵，

步骤4：解算多旋翼无人机的位置：

由于gps解算出的水平位置较为准确，而垂直误差较大，所以将位置解算分成两个部分：水平位置解算和垂直位置解算。

步骤4.1：利用gps位置数据解算多旋翼无人机的水平位置：

gps模块输出的经纬度和海拔数据是在WGS-84坐标系下的位置表示，需要把它转换到导航坐标系(ENU坐标系)下。这个过程分为两步：首先把WGS-84坐标系下的位置坐标转换到ECEF坐标系下，再将ECEF坐标系下的位置坐标转换到ENU坐标系下。如图4和图5所示，分别为WGS-84坐标系的YOZ平面、XOY平面剖面示意图。ECEF坐标系是三维直角坐标系，其原点与坐标轴与WGS-84坐标系重合。WGS-84坐标系的一些参数如下：

长轴半径：a＝6378137m

扁率：

长半轴a、短半轴b和扁率f之间的关系：

步骤4.1.1：将WGS-84坐标系中的gps位置数据[λ_k,φ_k,h_k]^T转换为ECEF坐标系中的位置数据[x_Ek,y_Ek,z_Ek]^T：

其中，a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；

步骤4.1.2：将ECEF坐标系中的位置数据[x_Ek,y_Ek,z_Ek]^T转换为ENU坐标系中的位置数据[x_k,y_k,z_k]^T：

其中，[x_E0,y_E0,z_E0]^T为导航坐标系的原点在ECEF坐标系中的坐标表示：[λ₀,φ₀,h₀]^T为多旋翼无人机在起飞原点时gps模块输出的gps位置数据；

步骤4.2：结合三轴加速度数据和步骤3中解算出的多旋翼无人机在第k时刻的姿态数据，对步骤4.1中解算出的水平位置进行扩展卡尔曼滤波融合：

步骤4.2.1：构建多旋翼无人机水平位置的状态方程和观测方程分别为

X_2k＝ΦX_2,k-1+Bu_k-1+ΓW_2,k-1 (27)

Z_2k＝HX_2k+V_2k (28)

其中，X_2k＝[S_xk,S_yk,ve_xk,ve_yk]^T、Z_2k＝[S_gxk,S_gyk]^T分别为第k时刻多旋翼无人机的水平位置状态向量、水平位置测量向量，(S_xk,S_yk)、(ve_xk,ve_yk)分别为第k时刻多旋翼无人机在导航坐标系中的水平位置、水平速度，S_gxk、S_gyk分别为式(26)中的x_k、y_k； 为第k时刻多旋翼无人机在导航坐标系中的水平加速度，为中的前两项，中的γ_k、θ_k、ψ_k由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W_2k、V_2k均为2×1维系统噪声向量，W_2k的方差统计特性完全由u_k的统计特性决定，Φ、B、H均为常数矩阵，

其中，无人机水平位置信息融合系统为非线性系统，式(27)、式(28)为其离散时间水平位置状态模型。

步骤4.2.2：估计第k时刻水平位置的状态变量为

步骤4.2.3：计算第k时刻水平位置的均方误差为

其中，P_2,k/k-1不随每次滤波迭代，P均为常数矩阵，

其中，gps的长期静态性能较好而短期响应速度比较差，卡尔曼滤波最终达到的效果是让每次融合的结果方差最小。由于gps响应慢，其解算出来的值易较长时间保持不变，所以在融合过程中更容易占更大的比例，显然这样的结果不是我们想要的。解决方法是让状态一步预测均方误差矩阵P_2,k/k-1不随每次滤波迭代，使其在滤波开始就是确定的常数矩阵P，从而滤波增益也会是一个常数矩阵。Φ、Γ、和H都是常数矩阵，如此便不会使得由状态一步预测得到的结果所起的作用越来越小。

步骤4.2.4：计算第k时刻水平位置的滤波增益为

其中，为常数矩阵，

本实施例中，

步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为

将为多旋翼无人机在第k时刻的最终水平位置和水平速度；

如图6所示，为本实施例中未经滤波前直接由gps数据解算出的多旋翼无人机在水平面上的运动轨迹；如图7所示，为本实施例中本发明多旋翼无人机的姿态及位置确定方法解算出的多旋翼无人机在水平面上的运动轨迹。

步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度h_vk作为多旋翼无人机在第k时刻的的垂直位置S_zk。

本实施例中，ParrortARDrone2.0自带一个定高的超声波传感器，超声波传感器输出的数据直接是无人机距离地面高度，精度为1毫米，有效范围在0.5米到12米，从而可以直接作为最终的垂直位置。

显然，上述实施例仅仅是本发明的一部分实施例，而不是全部的实施例。上述实施例仅用于解释本发明，并不构成对本发明保护范围的限定。基于上述实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，也即凡在本申请的精神和原理之内所作的所有修改、等同替换和改进等，均落在本发明要求的保护范围内。

Claims (3)

1.一种多旋翼无人机的姿态及位置确定方法，所述多旋翼无人机配置有惯性测量单元、gps模块、超声波传感器，所述惯性测量单元包括加速度计、陀螺仪、磁力计，其特征在于，包括下述步骤：

步骤1：进行数据采集：以采样周期T_s，利用加速度计、陀螺仪、磁力计、gps模块、超声波传感器分别采集多旋翼无人机在第k时刻的三轴加速度A_bk＝[a_xk,a_yk,a_zk]^T、三轴角速度Ω_bk＝[w_xk,w_yk,w_zk]^T、三轴磁力值M_bk＝[m_xk,m_yk,m_zk]^T、gps位置[λ_k,φ_k,h_k]^T、距地面高度h_vk；其中，b代表数据是机体坐标系中的，多旋翼无人机的导航坐标系为ENU坐标系，多旋翼无人机的起飞原点为导航坐标系的原点，λ_k、φ_k、h_k分别为WGS-84坐标系中多旋翼无人机在第k时刻的经度、纬度、海拔，k∈{0,1,2,...,K}，K为采样总数；

步骤2：进行数据预处理：对三轴加速度A_k进行低通滤波处理；

步骤3：解算多旋翼无人机的姿态：

步骤3.1：利用三轴角速度数据解算多旋翼无人机的姿态：

步骤3.1.1：计算多旋翼无人机的偏航角ψ_k、翻滚角θ_k、俯仰角γ_k对应的旋转矩阵分别为

步骤3.1.2：将多旋翼无人机的三次旋转表示为

其中，三次旋转为：多旋翼无人机从初始位置开始，绕导航坐标系的z_n轴旋转角度ψ_k，再绕旋转后的坐标系的y'_n轴旋转角度θ_k，再绕旋转后坐标系的x″_n轴旋转角度γ_k；为导航坐标系下的三轴角速度；

步骤3.1.3：由公式(4)得到欧拉角方程为：

步骤3.1.4：对式(5)进行一阶线性离散化，得到：

步骤3.2：利用低通滤波后的三轴加速度数据和三轴磁力值数据解算多旋翼无人机的姿态：

步骤3.2.1：计算导航坐标系到机体坐标系的坐标转换矩阵为

从而机体坐标系到导航坐标系的坐标转换矩阵为

步骤3.2.2：机体坐标系中的三轴加速度A_bk＝[a_xk,a_yk,a_zk]^T转换到导航坐标系中为A_nk＝[0,0,g]^T，计算得到

由于加速度信息与偏航角ψ_k无关，令ψ_k＝0，得到

其中，g为重力加速度；

步骤3.2.3：机体坐标系中的三轴磁力值M_bk＝[m_xk,m_yk,m_zk]^T转换到导航坐标系中为M_nk＝[m'_xk,m'_yk,m'_zk]^T：

计算得到地磁场水平分量为

计算得出偏航角为

步骤3.3：对步骤3.1和步骤3.2中解算出的姿态进行扩展卡尔曼滤波融合；

步骤3.3.1：构建多旋翼无人机姿态的状态方程和观测方程分别为

X_1k＝f(X_1,k-1)+W_1,k-1 (16)

Z_1k＝H_kX_1k+V_1k (17)

其中，X_1k＝[γ_k,θ_k,ψ_k]^T、Z_1k＝[z_γk,z_θk,z_ψk]^T分别为第k时刻多旋翼无人机的姿态状态向量、姿态测量向量，f(X_1,k-1)表示的关系为式(6)表示的关系，z_γk、z_θk、z_ψk分别为式(10)中的γ_k、式(9)中的θ_k、式(14)中的ψ_k；W_1k＝[δ_γk,δ_θk,δ_ψk]^T、V_1k＝[v_γk,v_θk,v_ψk]^T分别为第一噪声向量、第二噪声向量，均由惯性测量单元标定测量后确定，E[W_1k]＝q_k，Cov(W_1k,W_1k)＝Q_k，E[V_1k]＝r_k，Cov(V_1k,V_1k)＝R_k，Cov(W_1k,V_1k)＝0；对非线性的f(X_1,k-1)用扩展卡尔曼滤波进行线性化，得到

从而多旋翼无人机姿态的状态方程可以表示为

步骤3.3.2：估计第k时刻姿态的状态变量为

步骤3.3.3：计算第k时刻姿态的均方误差为

步骤3.3.4：计算第k时刻姿态的滤波增益为

步骤3.3.5：用第k时刻姿态的观测量对第k时刻姿态的估计量进行修正，得到第k时刻姿态的最优估计值为

将作为多旋翼无人机在第k时刻的最终姿态；

步骤3.3.6：更新均方误差为：

P_1k＝[I-v_1kH_k]P_1,k/k-1 (24)

其中，I为单位矩阵；

步骤4：解算多旋翼无人机的位置：

步骤4.1：利用gps位置数据解算多旋翼无人机的水平位置：

步骤4.1.1：将WGS-84坐标系中的gps位置数据[λ_k,φ_k,h_k]^T转换为ECEF坐标系中的位置数据[x_Ek,y_Ek,z_Ek]^T：

其中，a为WGS-84坐标系中的长轴半径，e为WGS-84坐标系的固定参数；

步骤4.1.2：将ECEF坐标系中的位置数据[x_Ek,y_Ek,z_Ek]^T转换为ENU坐标系中的位置数据[x_k,y_k,z_k]^T：

其中，[x_E0,y_E0,z_E0]^T为导航坐标系的原点在ECEF坐标系中的坐标表示：[λ₀,φ₀,h₀]^T为多旋翼无人机在起飞原点时gps模块输出的gps位置数据；

步骤4.2：结合三轴加速度数据和步骤3中解算出的多旋翼无人机在第k时刻的姿态数据，对步骤4.1中解算出的水平位置进行扩展卡尔曼滤波融合：

步骤4.2.1：构建多旋翼无人机水平位置的状态方程和观测方程分别为

X_2k＝ΦX_2,k-1+Bu_k-1+ΓW_2,k-1 (27)

Z_2k＝HX_2k+V_2k (28)

其中，X_2k＝[S_xk,S_yk,ve_xk,ve_yk]^T、Z_2k＝[S_gxk,S_gyk]^T分别为第k时刻多旋翼无人机的水平位置状态向量、水平位置测量向量，(S_xk,S_yk)、(ve_xk,ve_yk)分别为第k时刻多旋翼无人机在导航坐标系中的水平位置、水平速度，S_gxk、S_gyk分别为式(26)中的x_k、y_k； 为第k时刻多旋翼无人机在导航坐标系中的水平加速度，为中的前两项，中的γ_k、θ_k、ψ_k由式(23)计算得出；Γ为噪声驱动矩阵，Γ＝B，W_2k、V_2k均为2×1维系统噪声向量，W_2k的方差统计特性完全由u_k的统计特性决定，Φ、B、H均为常数矩阵，

步骤4.2.2：估计第k时刻水平位置的状态变量为

步骤4.2.3：计算第k时刻水平位置的均方误差为

其中，P_2,k/k-1不随每次滤波迭代，P均为常数矩阵，

步骤4.2.4：计算第k时刻水平位置的滤波增益为

其中，为常数矩阵，

步骤4.2.5：用第k时刻水平位置的观测量对第k时刻水平位置的估计量进行修正，得到第k时刻的最优估计值为

将为多旋翼无人机在第k时刻的最终水平位置和水平速度；

步骤4.3：利用距地面高度数据解算多旋翼无人机的垂直位置：将距地面高度h_vk作为多旋翼无人机在第k时刻的的垂直位置S_zk。

2.根据权利要求1所述的多旋翼无人机的姿态及位置确定方法，其特征在于，所述步骤2中，低通滤波的方法为算术平均滤波法，将三轴加速度的m个连续采样值相加后取算术平均值作为低通滤波后的值：

3.根据权利要求1所述的多旋翼无人机的姿态及位置确定方法，其特征在于，所述步骤3.2.3中，对式(14)进行修正得到：