[发明专利]一种四旋翼飞行器动力学模型/机载传感器组合导航方法

权利要求书

1.一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，其特征在于，包括如下步骤：

步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；GPS信息V_NG(k)、V_EG(k)、V_DG(k)，其分别为北向速度、东向速度、地向速度，P_NG(k)、P_EG(k)，其分别为北向位置、东向位置；气压高度计信息h_b(k)；磁传感器信息ψ_m(k)；

步骤二：计算k时刻四旋翼飞行器的加速度、角加速度；在步骤二中采用如下形式计算k时刻四旋翼飞行器的加速度、角加速度：

加速度信息通过下式进行计算：

其中，为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；k_Hx、k_Hy、k_T为模型参数，均为常数，通过离线辨识方法获得；为k-1时刻机体系相对于导航系的线速度在机体系X、Y轴上的分量；

角加速度信息通过下式进行计算：

其中，为k时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，是的微分，即角加速度；k_R1、k_R2、k_P1、k_P2、k_Q为模型参数，均为常数，通过离线辨识方法获得；

步骤三：预测k时刻四旋翼飞行器的角速度、姿态、速度、位置；

步骤四：通过卡尔曼滤波器，对k时刻四旋翼飞行器的角速度、姿态、速度、位置进行校正。

2.根据权利要求1所述的一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，其特征在于，在步骤三中采用如下形式预测k时刻四旋翼飞行器的角速度、姿态、速度、位置：

1)角速度预测采用如下公式：

其中，为k-1时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量；g为重力加速度；ΔT为离散采样周期；

2)姿态预测采用如下公式：

其中，φ(k)、θ(k)、ψ(k)分别为k时刻的横滚角、俯仰角、航向角；φ(k-1)、θ(k-1)、ψ(k-1)分别为k-1时刻的横滚角、俯仰角、航向角；

3)速度预测采用如下公式：

其中，为k时刻机体系相对于导航系的线速度在机体系X、Y、Z轴上的分量；为k-1时刻机体系相对于导航系的线速度在机体系X、Y、Z轴上的分量；

4)位置预测采用如下公式：

其中，p_n(k)、p_e(k)、h(k)分别为k时刻的北向位置、东向位置、地向高度；p_n(k-1)、p_e(k-1)、h(k-1)分别为k-1时刻的北向位置、东向位置、地向高度。

3.根据权利要求2所述的一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，其特征在于，步骤四中通过卡尔曼滤波器，对k时刻四旋翼飞行器的角速度、姿态、速度、位置进行校正：

1)计算一步预测均方误差P(k|k-1)：

P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)^T+G(k-1)W(k-1)G(k-1)^T

式中，I_3×3为3×3的单位矩阵，0_3×3为3×3的零矩阵，φ(k)、θ(k)、ψ(k)为k时刻横滚角、俯仰角、航向角，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，上标T表示转置，P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差，为滤波器k-1时刻的滤波器噪声系数矩阵，W＝[ε_ωx ε_ωy ε_ωz ε_fx ε_fy ε_fz]^T,为k-1时刻状态噪声，ε_ωx、ε_ωy和ε_ωz分别为和的模型噪声，ε_fx、ε_fy和ε_fz分别为和的模型噪声；

2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：

K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+R(k)]^-1

式中，Υ_1×6＝[0 0 0 0 0 1],H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益，为k时刻的量测噪声，diag表示矩阵对角化，其中分别为V_NG、V_EG、V_DG、P_NG、P_EG、h_b、ψ_m的噪声；上标T表示转置，上标-1表示求逆,0_3×6为3×6的零矩阵，0_3×3为3×3的零矩阵，0_1×3为1×3的零矩阵；

3)计算k时扩展卡尔曼滤波器状态估计值

式中，为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤三的预测公式计算得到，Y(k)＝[V_NG(k) V_EG(k)V_DG(k) P_NG(k) P_EG(k) h_b(k) ψ_m(k)]^T为k时刻的量测值，通过步骤一的传感器数据读取获得；

4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：

P(k|k)＝[I-K(k)H(k)]P(k|k-1)

式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。