CN103837151B - 一种四旋翼飞行器的气动模型辅助导航方法 - Google Patents

Abstract

本发明公开了一种四旋翼飞行器的气动模型辅助导航方法，属于组合导航与自主导航领域。本发明利用四旋翼飞行器的气动模型，与其机载传感器相结合，对其速度、位置信息进行估计。在本发明中，采用的气动模型参数包括旋翼半径、空气密度、旋翼实度、升力斜率、阻力系数、旋翼安装角，采用的机载传感器信息包括旋翼转速、加速度、角速度、姿态、高度。该方法无需增加任何外部设备，可以提高四旋翼飞行器在无GNSS（卫星导航系统）飞行时的速度、位置估计精度，具有成本低、零载重、自主性强等特点。

Description

一种四旋翼飞行器的气动模型辅助导航方法

技术领域

本发明属于组合导航与自主导航领域，具体涉及一种四旋翼飞行器的气动模型辅助导航方法。

背景技术

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

受四旋翼飞行器体积、载重、成本所限，其通常选用低成本、小型化的低精度机载导航传感器。目前四旋翼飞行器常用的导航方案包含惯性传感器、卫星导航系统、磁传感器以及气压高度计：其中惯性传感器与磁传感器组成航姿系统，可提供姿态信息；惯性传感器与卫星导航系统进行信息融合，可提供速度与位置信息；气压高度计用以修正高度信息。

由于卫星导航需要接收外界无线电信号，在一些特殊情况下会受到人为或自然干扰（例如室内飞行时），此时其速度、位置信息仅由惯性传感器解算得到。当四旋翼飞行器的卫星导航系统不可用时，惯性传感器在解算速度、位置信息时采用推算算法，且四旋翼飞行器采用的惯性传感器精度通常较低，致使其误差随时间快速发散，难以保证其定位需求。

发明内容

本发明所要解决的技术问题是：提供一种四旋翼飞行器的气动模型辅助导航方法，通过采用气动模型参数作为辅助参数，与其机载传感器相结合，计算四旋翼飞行器机体系下的速度，进一步计算导航系下的速度，提高了计算精度，解决了四旋翼飞行器在卫星导航系统不可用时的自主测速、定位不精确的问题。

本发明为解决上述技术问题采用以下技术方案：

一种四旋翼飞行器的气动模型辅助导航方法，包括如下步骤：

步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，并计算k时刻四旋翼飞行器机体系下的Z轴的速度然后，构建卡尔曼滤波器计算的在线补偿参数，计算补偿后的机体系下的Z轴的速度

步骤二：按照如下公式计算k时刻四旋翼飞行器机体系下的速度V^b _k：

其中，分别为k时刻四旋翼飞行器速度在机体系下的X轴分量及Y轴分量；按照如下公式计算： 其中F_bxk、F_byk为k时刻机体系下四旋翼飞行器的水平方向气动力矢量F^b _k在X、Y轴的分量，k_a2vk为k时刻四旋翼飞行器的第一气动参数；F^b _k按照如下公式计算：

其中m为四旋翼飞行器质量，f^b _k为k时刻三轴加速度的矢量表示，为四旋翼飞行器k时刻导航系到机体系的坐标转换矩阵，为k时刻地球自转角速度矢量在导航系下的投影，为k时刻导航系相对于地球系的角速度矢量在导航系下的投影，Vⁿ _k-1为四旋翼飞行器k-1时刻导航系下的速度；

k_a2vk按照如下公式计算：

$k_{a2\mathrm{vk}}=-A(v_{1k}+{\tilde{V}}_{\mathrm{nbzk}}^b)C_1-({\mathrm{\ω}}_{1k}+{\mathrm{\ω}}_{2k}+{\mathrm{\ω}}_{3k}+{\mathrm{\ω}}_{4k})\frac{\mathrm{AR}}{4}{C}_2,$

其中，v_1k为四旋翼飞行器k时刻的诱导速度，C₁为第二气动参数，C₂为第三气动参数，ω_1k、ω_2k、ω_3k、ω_4k分别为四个旋翼的转速，A为旋翼旋转面积，R为旋翼的半径；C₁、C₂分别按下式计算：

C₁=ρσaθ_s、其中，ρ为空气密度，σ为旋翼实度，a为升力斜率，θ_s为旋翼安装角，为阻力系数；

步骤三：计算k时刻四旋翼飞行器导航系下的速度其中为的转置矩阵；

步骤四：计算k时刻四旋翼飞行器的经度λ_k、纬度L_k，L_k-1为k-1时刻纬度，为Vⁿ _k的Y轴分量，ΔT为采样时间，R_e为地球半径，h_bk为四旋翼飞行器的高度；λ_k-1为k-1时刻经度，为Vⁿ _k的X轴分量。

采用如下公式计算k时刻四旋翼飞行器机体系下的Z轴的速度

其中为k-1时刻的四旋翼飞行器机体系下Z轴速度，为a^b _k在Z轴的分量；a^b _k为k时刻四旋翼飞行器机体系下的加速度，采用如下公式计算：

${a^b}_k={f^b}_k-C_{\mathrm{nk}}^b({2\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)\×{V^n}_{k-1}-{\mathrm{\ω}}_{\mathrm{nbk}}^b\×{V^b}_{k-1}+C_{\mathrm{nk}}^b{g^n}_k$

其中，为k时刻机体系相对于导航系的角速度矢量在机体系下的投影；V^b _k-1为k-1时刻机体系下的速度；gⁿ _k为k时刻导航系下的地球重力加速度矢量。

计算卡尔曼滤波器对的在线补偿参数采用如下方法：

（a）计算k时刻卡尔曼滤波器一步预测均方误差P_k|k-1

其中，P_k-1为k-1时刻的状态估计均方差，F_k,k-1为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，按照下式计算：

$F_{k,k-1}=\left[\begin{array}{cc}0& 0\\ -\cos \left({\mathrm{\θ}}_k\right)\cos \left({\mathrm{\φ}}_k\right)& 0\end{array}\right],$ 其中θ_k、φ_k分别为俯仰角、横滚角；

为F_k,k-1的转置矩阵；G_k,k-1为滤波器噪声矩阵，按照下式计算：

$G_{k,k-1}=\left[\begin{array}{ccc}0& 0& 1\\ \sin \left({\mathrm{\φ}}_k\right)& -\cos \left({\mathrm{\θ}}_k\right)\sin \left({\mathrm{\φ}}_k\right)& 0\end{array}\right];$ 为G_k,k-1的转置矩阵；Q_k-1为k-1时刻的滤波器噪声方差矩阵，按照下式计算：

其中为机体系下的X轴速度误差，为机体系下的Y轴速度误差，机体系下Z轴加速度误差；

(b)计算k时刻卡尔曼滤波器滤波增益K_k

其中H_k为k时刻的量测矩阵，其值为H_k=[01]，为H_k的转置矩阵；R_k为k时刻的量测噪声，其值为R_k=δh_b，δh_b为气压高度计输出高度的噪声方差；

(c)计算k时刻卡尔曼滤波器估计均方误差P_k|k

P_k|k=(I-K_kH_k)P_k|k-1，其中I为单位矩阵；

(d)计算k时刻卡尔曼滤波器状态估计值X_k|k，并得到

令其中δh_k为四旋翼飞行器高度误差，X_k|k=K_kZ_k；其中Z_k为k时刻的量测向量，其值为Z_k=h_dk-h_bk，其中h_dk为k时刻一步预测的高度，按照下式计算：

$h_{\mathrm{dk}}=h_{k-1}{\tilde{V}}_{\mathrm{nbzk}}^n\mathrm{\ΔT},$

其中h_k-1为k-1时刻的高度，为k时刻一步预测导航系下的速度的Z轴分量，按照下式计算：

$\widetilde{V_k^n}=C_{\mathrm{bk}}^n\widetilde{V_k^b},$

其中 为四旋翼飞行器k-1时刻速度在机体系X轴上的投影，为四旋翼飞行器k-1时刻速度在机体系Y轴上的投影。

采用如下公式计算补偿后的机体系下的Z轴的速度

${\tilde{V}}_{\mathrm{nbzk}}^b=V_{\mathrm{nbzk}}^b-{\mathrm{\δV}}_{\mathrm{nbzk}}^b.$

与现有技术相比，本发明具有以下有益效果：

1、本发明利用四旋翼飞行器的气动模型，结合四旋翼飞行器机载传感器，根据四旋翼飞行器气动模型与导航参数的关系，对其速度、位置进行估计，可有效提高四旋翼飞行器在无GNSS环境下飞行时的测速、定位精度。

2、该方法无需增加任何外部设备，具有成本低、零载重、自主性强等优点，是一种较好的四旋翼飞行器自主导航方法。

附图说明

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

图2为采用本发明方法后四旋翼飞行器悬停状态下的定位误差。

图3为采用本发明方法后四旋翼飞行器悬停状态下的东向速度误差。

图4为采用本发明方法后四旋翼飞行器悬停状态下的北向速度误差。

图5为四旋翼飞行器飞行航迹以及采用本发明方法后的定位结果。

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

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

具体实施方式

本发明方法的流程如图1所示，其具体步骤如下：

步骤一：读取机载传感器数据，以周期ΔT读取k时刻四旋翼飞行器航姿系统输出数据，包括横滚角φ_k、俯仰角θ_k、航向角ψ_k，三轴加速度计的输出三轴陀螺仪的输出气压高度计的输出h_bk，四个旋翼的转速分别为ω_1k、ω_2k、ω_3k、ω_4k；

步骤二：利用三轴加速度计的输出，计算四旋翼飞行器机体系下的Z轴的速度同时，利用气压高度计的输出，构建卡尔曼滤波器对进行在线补偿，得到补偿后的机体系下的Z轴的速度具体步骤如下：

(a)由三轴加速度计的输出推算四旋翼飞行器的天向速度与高度

以a^b _k表示k时刻四旋翼飞行器机体系下的加速度，根据加速度计敏感到的比力的物理量概念，推算得 ${a^b}_k={f^b}_k-C_{\mathrm{nk}}^b({2\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)\×{V^n}_{k-1}-{\mathrm{\ω}}_{\mathrm{nbk}}^b\×{V^b}_{k-1}+C_{\mathrm{nk}}^b{g^n}_k,$ 其中为步骤一中采集得到的k时刻三轴加速度计输出的矢量形式；为四旋翼飞行器k时刻导航系到机体系的坐标转换矩阵；V^b _k-1为机体系下k-1时刻的速度；为k时刻地球自转角速度矢量在导航系下的投影；为k时刻导航系相对于地球系的角速度矢量在导航系下的投影；Vⁿ _k-1为导航系下k-1时刻的速度；为k时刻机体系相对于导航系的角速度矢量在机体系下的投影；为k时刻导航系下的地球重力加速度矢量，其值为gⁿ _k=[00-g]，g为地球重力加速度；

计算k时刻的四旋翼飞行器机体系下Z轴速度，为其中为k-1时刻的四旋翼飞行器机体系下Z轴速度，为a^b _k在Z轴的分量，ΔT为采样时间；

(b)计算k时刻卡尔曼滤波器一步预测均方误差P_k|k-1

其中，P_k-1为k-1时刻的状态估计均方差，F_k,k-1为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，按照下式计算：

$F_{k,k-1}=\left[\begin{array}{cc}0& 0\\ -\cos \left({\mathrm{\θ}}_k\right)\cos \left({\mathrm{\φ}}_k\right)& 0\end{array}\right],$ 其中θ_k、φ_k分别为俯仰角、横滚角，由步骤一得到；

为F_k,k-1的转置矩阵；G_k,k-1为滤波器噪声矩阵，按照下式计算：

$G_{k,k-1}=\left[\begin{array}{ccc}0& 0& 1\\ \sin \left({\mathrm{\θ}}_k\right)& -\cos \left({\mathrm{\θ}}_k\right)\sin \left({\mathrm{\φ}}_k\right)& 0\end{array}\right]\£\»$ 为G_k,k-1的转置矩阵；Q_k-1为k-1时刻的滤波器噪声方差矩阵，按照下式计算：

其中为机体系下的X轴速度误差，为机体系下的Y轴速度误差，机体系下Z轴加速度误差；

(c)计算k时刻卡尔曼滤波器滤波增益K_k

其中H_k为k时刻的量测矩阵，其值为H_k=[01]，为H_k的转置矩阵；R_k为k时刻的量测噪声，其值为R_k=δh_b，δh_b为气压高度计输出高度的噪声方差；

(d)计算k时刻卡尔曼滤波器估计均方误差P_k|k

P_k|k=(I-K_kH_k)P_k|k-1，其中I为单位矩阵；

(e)计算k时刻卡尔曼滤波器状态估计值X_k|k，并得到

令其中δh_k为四旋翼飞行器高度误差，X_k|k=K_kZ_k；其中Z_k为k时刻的量测向量，其值为Z_k=h_dk-h_bk，其中h_dk为k时刻一步预测的高度，按照下式计算：

$h_{\mathrm{dk}}=h_{k-1}+{\tilde{V}}_{\mathrm{nbzk}}^n\mathrm{\ΔT},$

其中h_k-1为k-1时刻的高度，为k时刻一步预测导航系下的速度的Z轴分量，按照下式计算：

${\tilde{V}}_k^n=C_{\mathrm{bk}}^n{\tilde{V}}_k^b,$

其中 为四旋翼飞行器k-1时刻速度在机体系X轴上的投影，为四旋翼飞行器k-1时刻速度在机体系Y轴上的投影。

(f)计算k时刻四旋翼飞行器机体系下补偿后的Z轴速度

根据步骤(e)中估计的对步骤(a)中计算的进行补偿，得到

步骤三：计算k时刻四旋翼飞行器机体系下的速度V^b _k，具体步骤如下：

（a）计算k时刻机体系下四旋翼飞行器的水平方向气动力矢量方程F^b _k

F_bxk、F_byk、F_bzk为F^b _k在X、Y、Z三轴分量，其中m为四旋翼飞行器质量；

（b）计算k时刻第一气动参数k_a2vk

计算四旋翼悬停时每个旋翼提供的升力F_H，其值为F_H=mg/4，其中g为地球重力加速度；计算k时刻的水平空速V_0k， 为k时刻机体系下四旋翼飞行器的X轴速度，可用替代，为k时刻机体系下四旋翼飞行器的Y轴速度，可用替代；计算旋翼旋转面积A，其值为A=πR²，其中R为旋翼的半径；计算k时刻的诱导速度v_1k，其值为 $v_{1k}=\sqrt{-\frac{V_{0k}^2}{2}+\sqrt{{\left(\frac{V_{0k}^2}{2}\right)}^2+{\left(\frac{F_H}{2\mathrm{\ρA}}\right)}^2}},$ 其中ρ为空气密度；计算第二气动参数C₁，其值为C₁=ρσaθ_s，其中σ为旋翼实度，a为升力斜率，θ_s为旋翼安装角；计算第三气动参数C₂，其值为其中为阻力系数；计算k_a2vk，其值为 $k_{a2\mathrm{vk}}=-A(v_{1k}+{\tilde{V}}_{\mathrm{nbzk}}^b)C_1-({\mathrm{\ω}}_{1k}+{\mathrm{\ω}}_{2k}+{\mathrm{\ω}}_{3k}+{\mathrm{\ω}}_{4k})\frac{\mathrm{AR}}{4}{C}_2,$ 其中由步骤二得到，ω_1k、ω_2k、ω_3k、ω_4k由步骤一得到；

（c）计算k时刻四旋翼飞行器速度在机体系下的X轴分量其值为计算k时刻四旋翼飞行器速度在机体系下的Y轴分量其值为其中F_bxk、F_byk由步骤(a)得到，k_a2vk由步骤(b)得到；则其中由步骤二得到；

步骤四：计算k时刻四旋翼飞行器导航系下的速度其中为的转置矩阵，V^b _k由步骤三得到；

步骤五：计算k时刻四旋翼飞行器的经度λ_k、纬度L_k，L_k-1为k-1时刻纬度，为Vⁿ _k的Y轴分量，Vⁿ _k由步骤四得到，ΔT为采样时间，R_e为地球半径，h_bk为k时刻气压高度计输出高度；λ_k-1为k-1时刻经度，为Vⁿ _k的X轴分量。

实施例：

采用仿真的形式，对使用本发明方法后的四旋翼飞行器定位精度进行验证。其中机载传感器精度设置如下：陀螺零偏稳定性为10deg/h，deg/h为度/小时，加速度计零偏稳定性为10-4g，航姿系统的姿态精度为1deg，气压高度计误差为1m。四旋翼飞行器气动模型参数误差设为5%。

令四旋翼飞行器悬停5分钟。图2为四旋翼飞行器在悬停状态下5分钟内采用本发明方法后的定位误差，可以看出定位精度在0.1m以内。采用现有方法（即纯惯性导航方法），其定位误差在100m以上，本方法精度提高了3个数量级以上。

图3、图4分别为四旋翼飞行器在悬停状态下5分钟内东向、北向速度误差，速度误差整体在0.001m/s以内。速度误差在1～10s时较大，是由于四旋翼飞行器在该时段未达到悬停稳定状态，存在一定机动。采用现有方法，东向、北向速度误差在1m/s以上，本方法精度提高了3个数量级以上。

令四旋翼飞行器按照某航迹飞行。图5为四旋翼飞行器飞行航迹以及采用采用本发明方法后的定位结果。

图6为在图5航迹中本方法的定位误差，可以看出定位精度在0.2m以内。采用现有方法，其定位误差在500m以上，本方法精度提高了3个数量级以上。

图7为在图5航迹中本方法的速度误差，可以看出速度误差在0.05m/s以内。采用现有方法，东向、北向速度误差在5m/s以上，本方法精度提高了2个数量级以上。

Claims (4)

1.一种四旋翼飞行器的气动模型辅助导航方法，其特征在于，包括如下步骤：

步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，并计算k时刻四旋翼飞行器机体系下Z轴的速度然后，构建卡尔曼滤波器计算的在线补偿参数，计算补偿后的机体系下Z轴的速度

步骤二：按照如下公式计算k时刻四旋翼飞行器机体系下的速度V^b _k：

其中，分别为k时刻四旋翼飞行器速度在机体系下的X轴分量及Y轴分量；按照如下公式计算：其中F_bxk、F_byk为k时刻机体系下四旋翼飞行器的水平方向气动力矢量F^b _k在X、Y轴的分量，k_a2vk为k时刻四旋翼飞行器的第一气动参数；F^b _k按照如下公式计算：

其中m为四旋翼飞行器质量，f^b _k为k时刻三轴加速度的矢量表示，为四旋翼飞行器k时刻导航系到机体系的坐标转换矩阵，为k时刻地球自转角速度矢量在导航系下的投影，为k时刻导航系相对于地球系的角速度矢量在导航系下的投影，Vⁿ _k-1为四旋翼飞行器k-1时刻导航系下的速度；

k_a2vk按照如下公式计算：

$k_{a2vk}=-A(v_{1k}+{\tilde{V}}_{nbzk}^b)C_1-({\mathrm{\ω}}_{1k}+{\mathrm{\ω}}_{2k}+{\mathrm{\ω}}_{3k}+{\mathrm{\ω}}_{4k})\frac{AR}{4}{C}_2,$

其中，v_1k为四旋翼飞行器k时刻的诱导速度，C₁为第二气动参数，C₂为第三气动参数，ω_1k、ω_2k、ω_3k、ω_4k分别为四个旋翼的转速，A为旋翼旋转面积，R为旋翼的半径；C₁、C₂分别按下式计算：

C₁＝ρσaθ_s、其中，ρ为空气密度，σ为旋翼实度，a为升力斜率，θ_s为旋翼安装角，为阻力系数；

步骤三：计算k时刻四旋翼飞行器导航系下的速度Vⁿ _k，其中为的转置矩阵；

步骤四：计算k时刻四旋翼飞行器的经度λ_k、纬度L_k，L_k-1为k-1时刻纬度，为Vⁿ _k的Y轴分量，ΔT为采样时间，R_e为地球半径，h_bk为四旋翼飞行器的高度；λ_k-1为k-1时刻经度，为Vⁿ _k的X轴分量。

2.根据权利要求1所述的四旋翼飞行器的气动模型辅助导航方法，其特征在于，采用如下公式计算k时刻四旋翼飞行器机体系下的Z轴的速度

其中为k-1时刻的四旋翼飞行器机体系下Z轴速度，为a^b _k在Z轴的分量；a^b _k为k时刻四旋翼飞行器机体系下的加速度，采用如下公式计算：

${a^b}_k={f^b}_k-C_{nk}^b(2{\mathrm{\ω}}_{iek}^n+{\mathrm{\ω}}_{enk}^n)\×{V^n}_{k-1}-{\mathrm{\ω}}_{nbk}^b\×{V^b}_{k-1}+C_{nk}^b{g^n}_k$

其中，为k时刻机体系相对于导航系的角速度矢量在机体系下的投影；V^b _k-1为k-1时刻机体系下的速度；gⁿ _k为k时刻导航系下的地球重力加速度矢量。

3.根据权利要求1所述的四旋翼飞行器的气动模型辅助导航方法，其特征在于，计算卡尔曼滤波器对的在线补偿参数采用如下方法：

(a)计算k时刻卡尔曼滤波器一步预测均方误差P_k|k-1

其中，P_k-1为k-1时刻的状态估计均方差，F_k,k-1为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，按照下式计算：

其中θ_k、φ_k分别为俯仰角、横滚角；

为F_k,k-1的转置矩阵；G_k,k-1为滤波器噪声矩阵，按照下式计算：

为G_k,k-1的转置矩阵；Q_k-1为k-1时刻的滤波器噪声方差矩阵，按照下式计算：

其中为机体系下的X轴速度误差，为机体系下的Y轴速度误差，为机体系下Z轴加速度误差；

(b)计算k时刻卡尔曼滤波器滤波增益K_k

其中H_k为k时刻的量测矩阵，其值为H_k＝[0 1]，为H_k的转置矩阵；R_k为k时刻的量测噪声，其值为R_k＝δh_b，δh_b为气压高度计输出高度的噪声方差；

(c)计算k时刻卡尔曼滤波器估计均方误差P_k|k

P_k|k＝(I-K_kH_k)P_k|k-1，其中I为单位矩阵；

(d)计算k时刻卡尔曼滤波器状态估计值X_k|k，并得到

令其中δh_k为四旋翼飞行器高度误差，X_k|k＝K_kZ_k；其中Z_k为k时刻的量测向量，其值为Z_k＝h_dk-h_bk，其中h_dk为k时刻一步预测的高度，按照下式计算：

$h_{dk}=h_{k-1}+{\tilde{V}}_{nbzk}^n\mathrm{\Δ}T,$

其中h_k-1为k-1时刻四旋翼飞行器的高度，为k时刻一步预测导航系下的速度的Z轴分量，按照下式计算：

${\tilde{V}}_k^n=C_{bk}^n{\tilde{V}}_k^b,$

其中 为四旋翼飞行器k-1时刻速度在机体系X轴上的投影，为四旋翼飞行器k-1时刻速度在机体系Y轴上的投影。

4.根据权利要求1所述的四旋翼飞行器的气动模型辅助导航方法，其特征在于，采用如下公式计算补偿后的机体系下的Z轴的速度

${\tilde{V}}_{nbzk}^b=V_{nbzk}^b-{\mathrm{\δV}}_{nbzk}^b.$