[发明专利]一种无人机的半自主仿地飞行系统及其控制方法

权利要求书

1.一种用于无人机的半自主仿地飞行系统的控制方法，无人机的半自主仿地飞行系统包括GPS定位模块、惯性测量模块、气压计模块、雷达高度计、电机驱动器、桨叶动力结构以及飞行控制器，其特征在于：飞行控制器获取GPS定位模块、惯性测量模块以及雷达高度计的数据进行融合算法的计算，获取无人机位置、姿态、高度数据，从而控制电机驱动器，使无人机在半自主模式下沿航线进行仿地飞行,

其特征在于包括如下步骤：

步骤1，无人机操作人员选择雷达高度计；

步骤2，飞行控制器根据雷达设备ID在线识别接入的雷达高度计，根据检测到雷达高度计类型，自动设置输入低通滤波器的默认截止频率和数据更新频率；

步骤3，飞行控制器读取惯性测量模块和气压计模块的数据，通过融合算法得到无人机的相对高度，步骤3具体为：

步骤3.1，计算当前气压相对高度；

步骤3.2，通过融合算法计算无人机的当前相对高度，步骤3.2具体为：

步骤3.2.1,构造量测序列υ_k；

量测序列υ_k由加速度计、气压计的测量数据构成，

其中，h_c为测量点相对参考基准高度,a_z为无人机垂直方向加速度测量值；

步骤3.2.2，构造量测矩阵C_k；

量测矩阵C_k为：

步骤3.2.3，构造高度融合状态方程，

x_k+1＝A_kx_k+Γ_kξ_k，

量测方程为：

υ_k＝C_kx_k+η_k，

式中：x_k为状态向量，h为相对高度，v为垂直方向速度，a为垂直方向加速度，b为高度测量偏差,A_k是系统的状态方程，Γ_k是系统噪声矩阵，η_kξ_k均是系统噪声，C_k量测矩阵，υ_k为量测序列；

步骤3.2.4，构造卡尔曼滤波算法估计高度的状态方程，具体为：

状态预测方程：

状态估计方程：

G_k为滤波器增益，计算方程为：

P_k,k-1为误差协方差矩阵，估计方程为：

P_k,k＝(I-G_kC_k)P_k,k-1；

步骤3.3，高度修正；

步骤4，飞行控制器通过雷达高度计修正后的高度数据对无人机进行高度控制，实现无人机的跟随地表起伏的变化飞行；

步骤5，无人机通过地面站规划航线实现无人机半自主或自主飞行。

2.根据权利要求1所述的控制方法，其特征在于步骤3.1具体为：根据气压计模块的气压数据通过气压模型计算当前位置的气压相对高度；

气压模型的近似计算公式为：

其中：h_c为测量点相对参考基准高度；p_s、T_s为参考基准的表面压力和温度；h_s为参考点的大地高程；h_b为测量点的大地高程；p_b为测量点表面压力；R＝287.1J·Kg^-1·K^-1为气体常数；K_T＝6.5×10^-3K·m^-1为大气温度梯度；g₀＝9.80665m·s^-2为平均重力加速度；取参考基准为起飞位置，p_s、T_s则为起飞位置的表面压力和温度。