[发明专利]一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法

权利要求书

1.一种四旋翼飞行器横滚轴角速度及俯仰轴角速度估计方法，其特征在于，包括以下步骤：

(1)采集多组传感器数据，通过最小二乘法辨识横滚力矩模型参数、俯仰力矩模型参数；

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

(3)计算k时刻的横滚轴角加速度、俯仰轴角加速度；

(4)预测k时刻四旋翼飞行器的横滚轴角速度、俯仰轴角速度、横滚力矩模型的一阶马尔可夫、俯仰力矩模型的一阶马尔可夫、横滚力矩模型参数、俯仰力矩模型参数、四元数；

(5)通过扩展卡尔曼滤波器，对k时刻四旋翼飞行器的横滚轴角速度、俯仰轴角速度、四元数进行校正，得到横滚轴角速度、俯仰轴角速度的估计结果；

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

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

(12)通过建立最小二乘方程辨识横滚力矩模型参数：

f＝k_x0x₀+k_x1x₁+k_x2x₂

其中，ω_mx机体系相对于导航系的角速度在机体系x轴上的分量，k_x0、k_x1、k_x2为模型参数；

(13)通过建立最小二乘方程辨识俯仰力矩模型参数：

f_y＝k_y0y₀+k_y1y₁+k_y2y₂；

其中，ω_my机体系相对于导航系的角速度在机体系y轴上的分量的模型，k_y0、k_y1、k_y2为模型参数；

(14)采集多组实验数据，获得多组拟合结果，作为横滚力矩模型及俯仰力矩模型参数估计的初值；

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

(31)横滚轴角加速度：

(32)俯仰轴角加速度：

其中，ε_mx(k-1)为k-1时刻的横滚力矩模型的一阶马尔科夫过程，ε_my(k-1)为k-1时刻的俯仰力矩模型的一阶马尔科夫过程；

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

k_x0(k)＝k_x0(k-1)

k_x1(k)＝k_x1(k-1)

k_x2(k)＝k_x2(k-1)

k_y0(k)＝k_y0(k-1)

k_y1(k)＝k_y1(k-1)

k_y2(k)＝k_y2(k-1)

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

所述步骤(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

其中，K(k)为k时刻的滤波增益，H(k)为k时刻量测矩阵，R(k)为k时刻的量测噪声,上标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)所述的机载传感器信息主要包括旋翼转速传感器信息、磁传感器信息、陀螺信息、加速度信息及线速度信息。