CN108592911A - 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 - Google Patents

Abstract

本发明公开了一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，属于组合导航领域。本发明利用四旋翼飞行器的动力学模型，与机载传感器相结合，对其角速度、姿态、速度、位置信息进行估计，在本发明中，采用的动力学模型包括升力模型、阻力模型、扭矩模型、横滚力矩模型、俯仰力矩模型，采用的机载传感器包括旋翼转速传感器、磁传感器、卫星导航系统、气压高度计。在该方法中，没有采用目前四旋翼飞行器中常用的惯性传感器如陀螺仪与加速度计，通过四旋翼飞行器的动力学模型输出加速度、角加速度信息，对惯性传感器进行替代。该方法可用于解决惯性传感器失效时的四旋翼飞行器导航问题。

Description

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

技术领域

本发明属于组合导航领域，具体涉及一种四旋翼飞行器的动力学模型/机载传感器组合导航方法。

背景技术

四旋翼飞行器具有体积小、结构简单、可悬停和垂直起降等优点，特别适合在近地面环境(如室内、城区和丛林等)中执行监视、侦察等任务，具有广阔的军事和民用前景。导航系统为四旋翼飞行器提供其飞行控制系统所必须的导航信息，是其完成各种复杂飞行任务的必要保障。

目前四旋翼飞行器常用的传感器包括惯性传感器、GNSS(卫星导航系统)、磁传感器、气压高度计，其中惯性传感器包括陀螺仪与加速度计。受成本、体积所限，四旋翼飞行器中选用的惯性传感器精度、可靠性较低，易受外界温度、振动干扰而产生性能下降，甚至失效。此时，会导致导航系统精度下降，影响飞行安全。目前，尚未有针对惯性传感器失效情况下的四旋翼飞行器导航方法。

发明内容

发明目的：本发明所要解决的技术问题是提供一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，通过四旋翼飞行器的动力学模型，与GNSS、磁传感器、气压高度计相融合，计算四旋翼飞行器的角速度、姿态、速度、位置信息，解决四旋翼飞行器在惯性传感器失效时的导航问题。

技术方案：一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，包括如下步骤：

步骤一：周期读取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时刻机体系相对于导航系的加速度在机体系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时刻四旋翼飞行器的角速度、姿态、速度、位置；

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

其中，为k-1时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量；Δ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时刻的北向位置、东向位置、地向高度；

步骤四：通过卡尔曼滤波器，对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的零矩阵， ，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为单位矩阵。

有益效果：

通过本发明，可以在不使用惯性传感器的条件下，完成四旋翼飞行器角速度、姿态、速度、位置的解算，提高了四旋翼飞行器的导航可靠性。

附图说明

图1为本发明方法的流程示意图；

图2为四旋翼飞行器的参考位置以及采用本发明方法后的滤波估计位置；

图3为采用本发明方法后四旋翼飞行器在航迹飞行状态下的定位误差；

图4为采用本发明方法后四旋翼飞行器在航迹飞行状态下的机体系X、Y、Z轴方向速度误差；

图5为采用本发明方法后四旋翼飞行器在航迹飞行状态下的姿态误差；

图6为采用本发明方法后四旋翼飞行器在航迹飞行状态下的角速度误差。

具体实施方式

本发明方法的流程如图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时刻四旋翼飞行器的角速度、姿态、速度、位置：

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

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

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

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

步骤四：通过卡尔曼滤波器，对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的零矩阵，，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 01],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为单位矩阵。

采用仿真的形式，对使用本发明方法后的四旋翼飞行器导航精度进行验证。其中机载传感器精度设置如下：GPS速度精度为0.1m/s，磁航向精度为0.05°，气压高度计精度为0.1m。

令四旋翼飞行器以1m/s的速度匀速向上爬升至10m高度处，以5m/s的速度沿着矩形航迹飞行，悬停10s。

图2为四旋翼飞行器的参考位置以及采用本发明方法后的滤波估计位置。

图3为采用本发明方法后四旋翼飞行器在航迹飞行状态下的定位误差，可以看出定位精度在3m以内。

图4为采用本发明方法后四旋翼飞行器在航迹飞行状态下的机体系X、Y、Z轴方向的速度误差，可以看出解算精度在0.2m/s以内。

图5为采用本发明方法后四旋翼飞行器在航迹飞行状态下的姿态误差，可以看出解算精度在1.5°以内。

图6为采用本发明方法后四旋翼飞行器在航迹飞行状态下的角速度误差，可以看出解算精度在2.5°/s以内。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (4)

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时刻四旋翼飞行器的角速度、姿态、速度、位置进行校正。

2.根据权利要求1所述的一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，其特征在于，在步骤二中采用如下形式计算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为模型参数，均为常数，通过离线辨识方法获得。

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

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

其中，为k-1时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量；Δ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时刻的北向位置、东向位置、地向高度。

4.根据权利要求1所述的一种四旋翼飞行器的动力学模型/机载传感器组合导航方法，其特征在于，步骤四中通过卡尔曼滤波器，对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的零矩阵，

，

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×4＝[1 0 0 0],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_1×5为1×5的零矩阵，0_1×3为1×3的零矩阵；

式中，Υ_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为单位矩阵。