CN108693372B - 一种四旋翼飞行器的航向轴角速度估计方法 - Google Patents

Abstract

本发明公开了一种四旋翼飞行器的航向轴角速度估计方法，首先，采集多组传感器数据，通过最小二乘法辨识扭矩模型参数，取平均作为模型参数估计的初值；其次，周期读取k时刻四旋翼飞行器机载传感器信息，并计算k时刻的扭矩模型；然后，预测k时刻四旋翼飞行器的航向轴角速度、扭矩模型的一阶马尔可夫、扭矩模型参数及四元数；最后，通过扩展卡尔曼滤波器，对k时刻四旋翼飞行器的航向轴角速度、四元数进行校正。该方法无需增加外部设备，可以在缺少航向轴角速度传感器时，通过扭矩模型辅助获得航向角速度估计。

Description

一种四旋翼飞行器的航向轴角速度估计方法

技术领域

本发明涉及一种四旋翼飞行器的航向轴角速度估计方法，具体涉及一种基于四旋翼飞行器扭矩模型及磁传感器融合的航向轴角速度估计方法。

背景技术

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

目前四旋翼飞行器常用的导航方案包含惯性传感器、卫星导航系统、磁传感器以及气压高度计。其中惯性传感器与磁传感器组成航姿系统，可提供姿态信息，惯性传感器包括加速度计和陀螺仪，陀螺仪是进行姿态解算的基础。受成本、体积所限，四旋翼飞行器中选用的惯性传感器精度、可靠性较低，易受外界温度、振动干扰而产生性能下降，甚至失效。当航向轴陀螺在使用过程中出现故障时，将无法获得航向轴角速度信息，通过使用扭矩模型可以在缺少航向轴角速度量测传感器时，实现对航向轴角速度的实时估计，保证航姿的正确解算。

发明内容

发明目的：本发明提供一种基于四旋翼飞行器扭矩模型及磁传感器融合的航向轴角速度估计方法，通过采用四旋翼飞行器的航向扭矩模型，与其机载传感器相结合，解决四旋翼飞行器在缺少航向轴角速度量测时，航向轴角速度实时估计问题。

技术方案：本发明所述的一种四旋翼飞行器的航向轴角速度估计方法，包括以下步骤：

(1)采集多组传感器数据，通过最小二乘法辨识扭矩模型参数；

(2)周期读取k时刻四旋翼飞行器机载传感器信息；

(3)计算k时刻的扭矩模型；

(4)预测k时刻四旋翼飞行器的航向轴角速度、扭矩模型的一阶马尔可夫、扭矩模型参数及四元数；

(5)通过扩展卡尔曼滤波器，对k时刻四旋翼飞行器的航向轴角速度、扭矩模型的一阶马尔可夫、扭矩模型参数及四元数进行校正，得到航向轴角速度的实时估计。

所述步骤(1)包括以下步骤：

(11)采集一组传感器数据，主要包括旋翼转速传感器信息、陀螺信息；

(12)建立四旋翼飞行器扭矩模型参数辨识的最小二乘方程：

f＝k_z0x₀+k_z1x₁+k_z2x₂

其中，k_z0、k_z1、k_z2为模型参数；

(13)建立四旋翼扭矩模型：

其中，ω_mz为机体系相对于导航系的角速度在机体系z轴上的分量的模型，ω₁、ω₂、ω₃、ω₄分别为四个旋翼的转速；

(14)采集多组实验数据，获得多组拟合结果，取均值作为扭矩模型参数预测的初值。

步骤(2)所述的机载传感器信息主要包括旋翼转速传感器信息、磁传感器信息、陀螺信息及加速度信息等。

步骤(3)所述的k时刻的扭矩模型可通过以下公式获得：

其中，ε_mz(k-1)为k-1时刻的扭矩模型的一阶马尔科夫过程。

步骤(4)所述的k时刻四旋翼飞行器的航向轴角速度、扭矩模型的一阶马尔可夫、扭矩模型参数及四元数可通过以下公式获得：

k_z0(k)＝k_z0(k-1)

k_z1(k)＝k_z1(k-1)

k_z2(k)＝k_z2(k-1)

其中，ΔT为离散采样周期，T_mz为扭矩模型一阶马尔可夫过程相关时间，q₀(k)、q₁(k)、q₂(k)、q₃(k)为k时刻的四元数，

分别为k时刻机体系相对于导航系的角速度在机体系x、y轴上的分量。

所述步骤(5)包括以下步骤：

(51)计算一步预测均方误差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

其中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，上标T表示的转置，P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差，G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵，W(k-1)为k-1时刻状态噪声；

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

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

其中，H(k)为k时刻量测矩阵，R(k)为k时刻的量测噪声，diag表示矩阵对角化，上标T表示转置，上标-1表示求逆；

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

其中，

为k时刻状态量的估计值，

为k-1到k时刻的状态变量一步预测值，

为量测量的一步预测；

(54)通过状态估计结果可以直接获得航向轴角速度；

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

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

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

有益效果：与现有技术相比，本发明的有益效果：1、本发明利用扩展卡尔曼滤波器，使用最小二乘估计出的扭矩参数的均值作为滤波器扭矩参数状态量初值，将四旋翼飞行器的扭矩模型与磁航向角量测信息相融合，实现在缺少航向轴角速度量测信息时，对航向轴角速度的实时估计；2、该方法无需增加任何外部设备，具有成本低、零载重、自主性强的优点。

附图说明

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

图2为采用本发明后四旋翼飞行器航向轴角速度估计结果。

具体实施方式

下面结合附图对本发明作进一步详细描述。

图1为本发明的流程图，具体过程如下：

1、通过最小二乘法辨识扭矩模型参数

(1)采集一组传感器数据，主要包括旋翼转速传感器信息、陀螺信息；

(2)建立四旋翼飞行器扭矩模型参数辨识的最小二乘方程：

f＝k_z0x₀+k_z1x₁+k_z2x₂；

(3)建立四旋翼飞行器的扭矩模型为：

其中，ω_mz为机体系相对于导航系角速度在机体系下的表示，在z轴上的分量，k_z0、k_z1、k_z2、为扭矩模型参数；

(4)在最小二乘拟合中，

x₀＝1，x₁＝ω₁ ²+ω₂ ²-ω₃ ²-ω₄ ²，

通过实验，读取z轴陀螺数据获得的四旋翼飞行器z轴角速度数据，通过求微分获得函数f的数据，通过四旋翼飞行器的旋翼转速数据获得变量x₁的数据，通过四旋翼飞行器的z轴角速度和旋翼转速数据获得变量x₂的数据，通过最小二乘拟合得到扭矩模型中的参数，k_z0、k_z1、k_z2；

(5)采集多组实验数据，重复上述过程，获得多组扭矩模型参数拟合结果，取均值作为扭矩模型参数预测的初值。

2、周期读取k时刻四旋翼飞行器机载传感器信息，包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；磁传感器信息ψ_m(k)；陀螺信息

其分别为为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；加速度信息

其分别为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量。

3、计算k时刻的扭矩模型

采用如下公式计算k时刻的扭矩模型：

其中，ε_zm(k-1)为k-1时刻的扭矩模型的一阶马尔科夫过程。

4、预测k时刻四旋翼飞行器的航向轴角速度，扭矩模型的一阶马尔可夫，扭矩模型参数，四元数

k_z0(k)＝k_z0(k-1)

k_z1(k)＝k_z1(k-1)

k_z2(k)＝k_z2(k-1)

其中，ΔT为离散采样周期，T_mz为扭矩模型一阶马尔可夫过程相关时间，q₀(k)、q₁(k)、q₂(k)、q₃(k)为k时刻的四元数。

5、通过扩展卡尔曼滤波器，对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式中，

为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，上标T表示的转置，P(k-1|k-1) 为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差，为滤波器k-1时刻的滤波器噪声系数矩阵，I_1×1为1×1的单位 向量，

0_4×1为4×1的零矩阵，I_4×4为4×4的单位矩阵，0_4×3为4×3 的零矩阵，0_4×4为4×4的零矩阵，

W(k-1)＝[ε_mz(k-1)ε_mar(k-1)ε_kz0(k-1)ε_kz1(k-1)ε_kz2(k-1)ε_ωx(k-1)ε_ωy(k-1)ε_ωz(k-1)]^T，为k-1时刻状态噪声，ε_mz(k-1)为k-1时刻建模白噪声，ε_mar(k-1)为k-1时刻扭矩模型的一阶马尔可夫过程白噪声，ε_kz0(k-1)、ε_kz1(k-1)、ε_kz2(k-1)为k-1时刻扭矩模型参数白噪声，ε_ωx(k-1)、ε_ωy(k-1)、ε_ωz(k-1)为k-1时刻机体系相对于导航系的角速度白噪声在机体系x、y、z轴上的分量。

(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

其中，

H(k)为k时刻量测矩阵，0_3×5为3×5的零矩阵，0_1×5为1×5的零矩阵，

，g为重力加速度，K(k)为k时刻的滤波增益，

为k时刻的量测噪声，diag表示矩阵对角化，上标T表示转置，上标-1表示求逆。

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

其中，

为k时刻状态量的估计值，

为k-1到k时刻的状态变量一步预测值，使用步骤四的预测公式计算得到，为k时刻的量测值，通过步骤1的传感器数据读取 获得；

为量测量的一步预测。

(4)通过状态估计结果可以直接获得航向轴角速度。

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

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

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

采用半物理仿真的形式，通过实际采集飞行数据，然后进行仿真处理，对使用本方法后的四旋翼无人机的航向轴角速度估计结果进行验证。

令四旋翼无人机做带有航向轴角速度的航向偏转运动，试验时间为5分钟。

图2为采用本发明后四旋翼飞行器航向轴角速度估计结果，可以看出所提出的模型里可以对航向角轴速度进行估计，估计误差在15°/s以内。

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

Claims (2)

1.一种四旋翼飞行器的航向轴角速度估计方法，其特征在于，包括以下步骤：

(1)采集多组传感器数据，通过最小二乘法辨识扭矩模型参数；

(2)周期读取k时刻四旋翼飞行器机载传感器信息；

(3)计算k时刻的扭矩模型；

(4)预测k时刻四旋翼飞行器的航向轴角速度、扭矩模型的一阶马尔可夫、扭矩模型参数及四元数；

(5)通过扩展卡尔曼滤波器，对k时刻四旋翼飞行器的航向轴角速度、扭矩模型的一阶马尔可夫、扭矩模型参数及四元数进行校正，得到航向轴角速度的实时估计；

所述步骤(1)包括以下步骤：

(11)采集一组传感器数据，主要包括旋翼转速传感器信息、陀螺信息；

(12)建立四旋翼飞行器扭矩模型参数辨识的最小二乘方程：

f＝k_z0x₀+k_z1x₁+k_z2x₂

其中，k_z0、k_z1、k_z2为模型参数；

(13)建立四旋翼扭矩模型：

其中，ω_mz为机体系相对于导航系的角速度在机体系z轴上的分量的模型，ω₁、ω₂、ω₃、ω₄分别为四个旋翼的转速；

(14)采集多组实验数据，获得多组拟合结果，取均值作为扭矩模型参数预测的初值；

步骤(3)所述的k时刻的扭矩模型可通过以下公式获得：

其中，ε_mz(k-1)为k-1时刻的扭矩模型的一阶马尔科夫过程；

步骤(4)所述的k时刻四旋翼飞行器的航向轴角速度、扭矩模型的一阶马尔可夫、扭矩模型参数及四元数可通过以下公式获得：

k_z0(k)＝k_z0(k-1)

k_z1(k)＝k_z1(k-1)

k_z2(k)＝k_z2(k-1)

其中，ΔT为离散采样周期，T_mz为扭矩模型一阶马尔可夫过程相关时间，q₀(k)、q₁(k)、q₂(k)、q₃(k)为k时刻的四元数，

分别为k时刻机体系相对于导航系的角速度在机体系x、y轴上的分量；

所述步骤(5)包括以下步骤：

(51)计算一步预测均方误差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

其中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，上标T表示的转置，P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差，G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵，W(k-1)为k-1时刻状态噪声；

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

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

其中，H(k)为k时刻量测矩阵，R(k)为k时刻的量测噪声，diag表示矩阵对角化，上标T表示转置，上标-1表示求逆；

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

其中，

为k时刻状态量的估计值，

为k-1到k时刻的状态变量一步预测值，

为量测量的一步预测；

(54)通过状态估计结果可以直接获得航向轴角速度；

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

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

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

2.根据权利要求1所述的一种四旋翼飞行器的航向轴角速度估计方法，其特征在于，步骤(2)所述的机载传感器信息主要包括旋翼转速传感器信息、磁传感器信息、陀螺信息及加速度信息。

Images