CN106774409A - 一种无人机的半自主仿地飞行系统及其控制方法 - Google Patents

Abstract

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

Description

一种无人机的半自主仿地飞行系统及其控制方法

技术领域

本发明涉及无人机自动飞行控制系统，尤其是一种无人机的半自主仿地飞行系统及其控制方法。

背景技术

无人机的高度控制是无人机领域的一个难点，尤其是近地表仿地飞行。一般的无人机高度控制通过气压计和加速度计融合计算无人机当前高度。这种方法的问题有：

1、气压计通过获取当前位置气压值计算当前位置的海拔高度，但由于无人机的机动造成无人机周围空气气压变化，获取的高度可能不准确

2、在近地面飞行时，无人机与地表形成的风场影响对气压计数据存在较大干扰

3、无法感知地表高度变化，跟随地形飞行。

目前无人机仿地飞行的主流方案是采用超声波传感器，通过超声波反馈的无人机相对地面的数据直接修正无人机高度控制的方法。它在一定程度上可以解决无人机近地表仿地飞行的问题，但是超声波传感器更新率慢，易受干扰，噪声较大，由于没有融合气压计和加速度计对超声波数据进行滤波，在较为复杂地表下（如玉米地，麦地，灌木丛等）高度控制不稳定，无法在较大载荷的工业无人机中应用。

发明内容

本发明为了克服现有技术方案的不足，提供了一种适应复杂地表环境的高精度高可靠性半自主仿地飞行的技术方案。

为了实现上述目的，本发明的技术方案为：

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

一种无人机的半自主仿地飞行控制方法，其特征在于包括如下步骤：

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

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

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

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

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

本发明与现有技术的有益效果体现在：

1、本发明可通过地表类型和飞行任务选择合适的高度传感器，无人机控制会自动识别高度传感器类型匹配对应的滤波算法，实现最佳的高度控制性能

2、本发明通过观测高度数据噪声，自动识别地表类型，并融合加速度计数据对高度数据滤波，实现获取稳定准确的相对地表高度数据

3、本发明可适应各种复杂地表，在复杂地表下依然后较好的近地表仿地飞行效果

4、在自动仿地飞行过程中，操作人员可干预调整飞行高度，结束干预后无人机可按调整后高度继续自主飞行，可方便实际操作和较好的处理紧急情况。

附图说明

图1为本发明的硬件结构图；

图2为本发明的卡尔曼滤波算法原理图；

图3为本发明的高度修正控制器算法原理图；

具体实施方式

下面结合附图与实施例对本发明作进一步的说明。

一种无人机的半自主仿地飞行系统，如图1所示，包括GPS定位模块、惯性测量模块、气压计模块、雷达高度计、电机驱动器、桨叶动力结构以及飞行控制器，

GPS定位模块通过UART接口与飞行控制器连接，惯性测量模块通过SPI接口与飞行控制器连接，雷达高度计通过UART接口与飞行控制器连接，电机驱动器通过PWM接口与飞行控制器连接，电机驱动器的动力输出轴与桨叶动力结构连接，飞行控制器获取GPS定位模块、惯性测量模块以及雷达高度计的数据进行融合算法的计算，获取无人机位置、姿态、高度数据，从而控制电机驱动器，使无人机在半自主模式下沿航线进行仿地飞行。

其中，半自主模式是指：地面站完成航线规划后，将飞行航线通过数据链路传输到无人机，无人机可按规划的航线自动飞行，同时无人机的飞行高度可由无人机操作人员通过遥控器控制调整，在操作人员不再操作时无人机可按照当前相对地表高度继续沿航线飞行。

GPS定位模块用于获取无人机的位置坐标，计算无人机当前速度，使无人机可以在半自主模式下的沿航线飞行；

惯性测量模块用于获取无人机的加速度和角速度，并计算无人机的当前欧拉角，从而控制无人机稳定飞行；

气压计模块用于获取无人机附近的气压数据；

雷达高度计包括微波雷达高度传感器、红外雷达高度传感器两种种，用于测量无人机相对地表的高度。

飞行控制器是无人机的控制核心部件，获取所有外部传感器的数据进行数据融合，并根据控制算法计算执行机构的控制量控制无人机稳定工作。本发明的高精度高可靠半自主仿地飞行算法即运行在无人机飞行控制器中。

一种无人机的半自主仿地飞行控制方法，步骤如下：

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

具体为，雷达高度计包括微波雷达高度传感器、红外雷达高度传感器两种，无人机操作人员根据表1选择合适的高度传感器。

表1 不同地表对应的适用的高度传感器列表

通过选择合适的雷达高度计，从而获取良好的高度测量性能。

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

步骤3，飞行控制器读取惯性测量模块和气压计模块的数据，通过融合算法得到无人机的相对高度（相对起飞位置高度）；

具体为：

步骤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^-3·m^-1为大气温度梯度；g₀ =9.80665m·s^-2为平均重力加速度；

取参考基准为起飞位置，p_s、T_s为起飞位置的表面压力和温度，则h_c为无人机的气压相对高度（相对起飞位置）。

步骤3.2，通过融合算法计算无人机的当前相对高度（相对于起飞位置的高度）；

融合算法流程如下：

步骤3.2.1,构造量测序列v_k；

量测序列v_k由加速度计、气压计的测量数据构成：

，

其中，a_z为无人机垂直方向加速度测量值；

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

量测矩阵C_k为：

，

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

，

量测方程为：

，

，

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

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

状态预测方程：

，

状态估计方程：

，

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

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

，

步骤3.3，高度修正，如图3所示，具体为：

计算雷达噪声值，飞行控制器读取雷达高度计的数据，计算雷达原始数据Data（t）与雷达低通滤波器输出数据的差值的平方为雷达数据的噪声值Noise（t）

式中：Noise（t）：雷达噪声；Data（t）: 雷达原始数据；F():低通滤波器；dt为数据更新周期；

通过判断雷达数据的噪声水平判断当前地表类型，根据雷达噪声值调整高度修正器的PID参数，使高度修正平滑稳定。

如表2所示，常见的地表有：平地、低杆农作物地表、高杆农作物地表、树林等。

地表类型	雷达反馈噪声值（单位：cm）
		平地	小于10
低杆农作物（水稻、小麦）	小于30
		高杆农作物（玉米）	小于100
树林（果树林）	大于100

表2 地表与雷达噪声对应关系表

其中高度修正的过程具体为：

1、由航线指令或者操纵者遥控指令给出仿地控制的高度指令；

2、雷达输出相对地表高度信息，经过地表检测算法得到高度修正PID控制器PID参数；

3、雷达数据与仿地高度指令数据经过高度修正PID控制器得到无人机的相对高度的控制指令；

4、由气压计与加速度计经过融合算法得到的相对高度作为反馈，经过高度控制PID得到高度控制量通过电机驱动器控制无人机高度。

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

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

步骤5.1，通过测绘获得飞行区域坐标后地面站根据坐标数据规划无人机航线，将航线通过数据链路上传到无人机；

步骤5.2，无人机起飞后由操作人员通过遥控切换到半自主模式，无人机将按照航线自动飞行；

步骤5.3，在自主飞行中无人机操作人员通过遥控器改变无人机相对地表的飞行高度，无人机操作人员不再改变飞行高度时无人机将沿着修改后的高度自主飞行。

以上所述实施方式仅表达了本发明的一种实施方式，但并不能因此而理解为对本发明范围的限制。应当指出，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (10)

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

2.根据权利要求1所述的一种无人机的半自主仿地飞行系统，其特征在于：惯性测量模块通过SPI接口与飞行控制器连接，雷达高度计通过UART接口与飞行控制器连接，电机驱动器通过PWM接口与飞行控制器连接，电机驱动器的动力输出轴与桨叶动力结构连接。

3.根据权利要求1所述的一种无人机的半自主仿地飞行系统，其特征在于：半自主模式是指，地面站完成航线规划后，将飞行航线通过数据链路传输到无人机，无人机按规划的航线自动飞行，同时无人机的飞行高度由无人机操作人员通过遥控器控制调整，在操作人员不再操作时无人机可按照当前相对地表高度继续沿航线飞行。

4.根据权利要求1所述的一种无人机的半自主仿地飞行系统，其特征在于：

GPS定位模块用于获取无人机的位置坐标，计算无人机当前速度；

惯性测量模块用于获取无人机的加速度和角速度，计算无人机的当前欧拉角；

气压计模块用于获取无人机附近的气压数据；

雷达高度计包括微波雷达高度传感器、红外雷达高度传感器两种，用于测量无人机相对地表的高度。

5.一种用于权利要求1所述的无人机的半自主仿地飞行系统的控制方法，其特征在于包括如下步骤：

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

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

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

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

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

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

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

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

步骤3.3，高度修正。

7.根据权利要求6所述的控制方法，其特征在于步骤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^-3·m^-1为大气温度梯度；g₀ =9.80665m·s^-2为平均重力加速度；取参考基准为起飞位置，p_s、T_s为起飞位置的表面压力和温度，则h_c为无人机的气压相对高度。

8.根据权利要求6所述的控制方法，其特征在于步骤3.2具体为：

步骤3.2.1,构造量测序列v_k；

量测序列v_k由加速度计、气压计的测量数据构成：

，

其中，a_z为无人机垂直方向加速度测量值；

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

量测矩阵C_k为：

，

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

，

量测方程为：

，

，

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

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

状态预测方程：

，

状态估计方程：

，

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

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

，

。

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

计算雷达噪声值，飞行控制器读取雷达高度计的数据，计算雷达原始数据Data（t）与雷达低通滤波器输出数据的差值的平方为雷达数据的噪声值Noise（t），

，

式中：Noise（t）：雷达噪声；Data（t）: 雷达原始数据；F():低通滤波器；dt为数据更新周期；

通过判断雷达数据的噪声水平判断当前地表类型，根据雷达噪声值调整高度修正器的PID参数，使高度修正平滑稳定。

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

步骤5.1，通过测绘获得飞行区域坐标后地面站根据坐标数据规划无人机航线，将航线通过数据链路上传到无人机；

步骤5.2，无人机起飞后由操作人员通过遥控切换到半自主模式，无人机将按照航线自动飞行；

步骤5.3，在自主飞行中无人机操作人员通过遥控器改变无人机相对地表的飞行高度，无人机操作人员不再改变飞行高度时无人机将沿着修改后的高度自主飞行。