CN109059925A - 一种航向快速估计方法 - Google Patents

Abstract

本发明属于航空飞行器的导航领域，特别涉及一种旋翼无人机的航向快速估计方法。该方法通过控制无人机进行S弯机动，加快卡尔曼组合导航滤波的航向收敛速度；同时增加滤波器中的协方差矩阵Q的值，从而增加了外部观测量的权重，导致增益矩阵K增大，进一步加快了航向收敛速度，实现一个机动条件下的航向粗对准，最终完成航向快速估计。本发明的优点如下：无人机鲁棒性增强，保证机载航向传感器故障情况下的飞行安全；基于应用广泛的卡尔曼组合导航滤波算法进行修改，算法成熟稳定，简单可靠，无额外运算量；传感器成本低，不需要额外增加其他设备，降低了无人机系统的复杂性；适用性强，适用于多种无人机的快速航向估计，尤其适合旋翼无人机。

Description

一种航向快速估计方法

技术领域

本发明属于航空飞行器的导航领域，特别涉及一种旋翼无人机的航向快速估计方法。

背景技术

无人机航向精度直接关系到导航定位解算精度与目标定位精度，传统航向估计方法主要有磁传感器、GPS测向、地速方向等。磁传感器由于其尺寸、重量、成本、功耗上的优势，使用广泛，但使用前需要严格标定，且飞行过程中易受环境干扰；GPS测向精度较高，且测量值为真北航向，但成本较高，体积较大，不适合小型无人机安装，且差分测向飞行过程中同样易受电磁干扰影响导致测向失败；地速方向算法简单，计算时间短，但精度差，不适用于旋翼无人机，且在无人机存在侧滑情况下有较大偏差。

当无人机机载航向传感器发生故障，航向存在较大偏差时，飞行安全会受到严重影响，针对这种状况完成快速航向估计，是本发明需要解决的问题。

发明内容

为了解决上述问题，本发明的目的是提供一种当无人机机载航向传感器发生故障、航向存在较大偏差时，快速完成航向估计，保障无人机飞行安全，提高导航定位精度和目标定位精度的航向快速估计方法。

本发明包括如下的技术方案：一种航向快速估计方法，其特征在于，该方法通过控制无人机进行S弯机动，加快卡尔曼组合导航滤波的航向收敛速度；同时增加滤波器中的协方差矩阵Q的值，从而增加了外部观测量的权重，导致增益矩阵K增大，进一步加快了航向收敛速度，实现一个机动条件下的航向粗对准，最终完成航向快速估计。

进一步，该方法具体步骤为：

步骤1：无人机根据故障检测发现机载航向传感器故障或长时间无有效航向输入，开始航向快速估计方法；

步骤2：飞控发送姿态控制指令，控制无人机做S弯机动，同时将卡尔曼组合导航滤波中协方差阵Q阵增大为原始值的3-5倍；

步骤3：在无人机做S弯机动过程中，采用根据步骤2调整后协方差阵Q阵数值的卡尔曼组合导航滤波器对所述无人机的航向进行实时校准；

步骤4：当无人机做S弯机动结束后，采用将协方差阵Q阵数值恢复到初始值卡尔曼组合导航滤波器完成对航向的校准，即完成航向快速估计。

进一步，所述步骤3的具体步骤为：

S3.1:k-1时刻，卡尔曼组合导航滤波器通过一步转移矩阵Φ_k,k-1估计系统k时刻的状态估计

S3.2:根据系统协方差阵Q、观测量协方差阵R、k-1时刻系统协方差阵P_k,k-1，计算k时刻系统的增益矩阵K_k，结合当前时刻的系统观测值Z_k完成对进行修正，更新系统协方差阵P_k，继而迭代进行k+1时刻的系统状态估计。

进一步，所述S弯机动为三轴摇摆运动与线运动组合。

本发明相比现有技术有如下优点：

(1)无人机鲁棒性增强，保证机载航向传感器故障情况下的飞行安全；

(2)基于应用广泛的卡尔曼组合导航滤波算法进行修改，算法成熟稳定，简单可靠，无额外运算量；

(3)传感器成本低，不需要额外增加其他设备，降低了无人机系统的复杂性；

(4)适用性强，适用于多种无人机的快速航向估计，尤其适合旋翼无人机。

具体实施方式

下面结合具体实施例对本发明的技术方案做进一步说明。

本发明一种航向快速估计方法，该方法通过控制无人机进行S弯机动，加快卡尔曼组合导航滤波的航向收敛速度；同时增加滤波器中的协方差矩阵Q的值，从而增加了外部观测量的权重，导致增益矩阵K增大，进一步加快了航向收敛速度，实现一个机动条件下的航向粗对准，最终完成航向快速估计。

进一步，该方法具体步骤为：

步骤1：无人机根据故障检测发现机载航向传感器故障或长时间无有效航向输入，开始航向快速估计方法；

步骤2：飞行控制器发送姿态控制指令，控制无人机做S弯机动，同时将卡尔曼组合导航滤波中协方差阵Q阵增大为原始值的3-5倍；

步骤3：在无人机做S弯机动过程中，采用根据步骤2调整后协方差阵Q阵数值的卡尔曼组合导航滤波器对所述无人机的航向进行实时校准；

步骤4：当无人机做S弯机动结束后，采用将协方差阵Q阵数值恢复到初始值卡尔曼组合导航滤波器完成对航向的校准，即完成航向快速估计。

所述步骤3的具体步骤为：

S3.1:k-1时刻，卡尔曼组合导航滤波通过一步转移矩阵Φ_k,k-1估计系统k时刻的状态估计

k-1时刻，卡尔曼组合导航滤波通过一步转移矩阵Φ_k,k-1估计系统k时刻的状态估计

同时根据系统协方差阵Q、观测量协方差阵R、k-1时刻系统协方差阵P_k,k-1，计算k时刻系统的增益矩阵K_k，

P_k＝[I-K_kH_k]P_k,k-1

结合当前时刻的系统观测值Z_k完成对进行修正，更新系统协方差阵P_k，继而迭代进行k+1时刻的系统状态估

S3.2:根据系统协方差阵Q、观测量协方差阵R、k-1时刻系统协方差阵P_k,k-1，计算k时刻系统的增益矩阵K_k，结合当前时刻的系统观测值Z_k完成对进行修正，更新系统协方差阵P_k，继而迭代进行k+1时刻的系统状态估计。

所述S弯机动为三轴摇摆运动与线运动组合。

本发明利用无人机S弯机动为三轴摇摆运动与线运动的组合，机动中滤波器状态变量中的航向误差可观性得到极大提高的特点，控制无人机进行S弯机动，加快卡尔曼组合导航滤波的航向收敛速度；同时增加滤波器中的协方差矩阵Q的值，从而增加了外部观测量的权重，导致增益矩阵K增大，进一步加快了航向收敛速度，实现一个机动条件下的航向粗对准，完成航向快速估计。

Claims (4)

1.一种航向快速估计方法，其特征在于，该方法通过控制无人机进行S弯机动，使卡尔曼组合导航滤波器状态变量中的航向误差可观性得到极大提高，加快卡尔曼组合导航滤波的航向收敛速度；同时增加滤波器中的协方差矩阵Q的每一个数值，从而增加了外部观测量的权重，导致增益矩阵K增大，进一步加快了航向收敛速度，实现一个机动条件下的航向粗对准，最终完成航向快速估计。

2.根据权利要求1所述的方法，其特征在于，该方法具体步骤为：

步骤1：无人机根据故障检测发现机载航向传感器故障或长时间无有效航向输入，开始航向快速估计方法；

步骤2：飞行控制器发送姿态控制指令，控制无人机做S弯机动，同时将卡尔曼组合导航滤波器中协方差阵Q阵增大为原始值的3-5倍；

步骤3：在无人机做S弯机动过程中，采用根据步骤2调整后协方差阵Q阵数值的卡尔曼组合导航滤波器对所述无人机的航向进行实时校准；

步骤4：当无人机做S弯机动结束后，将卡尔曼组合导航滤波器中协方差阵Q阵中的增大后数值恢复到卡尔曼组合导航滤波器的初始设定值，完成对航向的校准，即完成航向快速估计。

3.根据权利要求2所述的方法，其特征在于，所述步骤3的具体步骤为：

S3.1:k-1时刻，卡尔曼组合导航滤波器通过一步转移矩阵Φ_k,k-1估计系统k时刻的状态估计

S3.2:根据系统协方差阵Q、观测量协方差阵R、k-1时刻系统协方差阵P_k,k-1，计算k时刻系统的增益矩阵K_k，结合当前时刻的系统观测值Z_k完成对进行修正，更新系统协方差阵P_k，继而迭代进行k+1时刻的系统状态估计。

4.根据权利要求2所述的方法，其特征在于，所述S弯机动为三轴摇摆运动与线运动组合。