一种无人机高度优化方法及装置与流程

本发明涉及无人机技术领域，尤其涉及一种无人机高度优化方法及装置。

背景技术：

无人驾驶飞机(Unmanned Aerial Vehicle/Drones，简称“无人机”)，是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机，可以分类为无人固定翼机、无人多旋翼飞行器、无人直升机等，广泛应用于航拍、农业、灾难救援、观察野生动物、影视拍摄等领域。

现有的无人机高度定位技术包括：1、通过近低空超声波(或者激光)辅助无人机气压计进行定位，通常测距范围为0.2m-8m，束角约为10度至45度，这种方法受反射物材质的影响，对于许多材质的反射物，测距效果较差，如草地、水面等；此外，存在测量范围较小，不适用于大于测距的场景的缺点。2、通过近低空单目(或者双目)摄像头辅助无人机气压计进行定位，这种方法测距范围受限于摄像头类型，数据计算量大，且受环境光影响较大，当光线过强或光线较弱时，摄像头无法正常使用。

综上所述，现有的无人机高度定位方法需要通过辅助其它设备辅助气压计进行定位，且存在测量范围较低，环境适应性较弱等缺点。

技术实现要素：

本发明提供一种无人机高度优化方法及装置，用以解决现有技术中的无人机高度定位方法需要通过辅助其它设备辅助气压计进行定位，且存在测量范围较低，环境适应性较弱等缺点的问题。

本发明实施例提供一种无人机高度优化方法，包括：

获取当前采样周期无人机的传感器输出的高度数据，将所述高度数据输入跟踪微分器，所述跟踪微分器用于确定所述高度数据的微分信号；

将所述跟踪微分器输出的所述高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值；

根据所述当前采样周期的高度数据及量测噪声值，通过卡尔曼滤波确定当前采样周期的滤波结果数据，将所述当前采样周期的滤波结果数据作为当前采样周期的无人机高度优化值。

较佳地，所述根据当前采样周期的所述高度数据及所述量测噪声值，通过卡尔曼滤波确定当前采样周期的滤波结果数据，包括：

获取上一个采样周期的滤波结果数据及上一个采样周期的校正参数；

根据当前采样周期的量测噪声值及上一个采样周期的校正参数，确定当前采样周期的卡尔曼增益；

根据所述上一个采样周期的滤波结果数据、所述当前采样周期的卡尔曼增益及所述当前采样周期的高度数据，确定当前采样周期的滤波结果数据。

较佳地，所述根据当前采样周期的量测噪声值及上一个采样周期的校正参数，确定当前采样周期的卡尔曼增益之后，还包括：

根据当前采样周期的卡尔曼增益及上一个采样周期的校正参数，确定当前采样周期的校正参数，所述当前采样周期的校正参数用于确定下一个采样周期的卡尔曼增益。

较佳地，所述传感器为气压计。

较佳地，所述卡尔曼滤波为自适应卡尔曼滤波。

本发明实施例还提供一种无人机高度优化装置，包括：

微分模块：用于获取当前采样周期无人机的传感器输出的高度数据，将所述高度数据输入跟踪微分器，所述跟踪微分器用于确定所述高度数据的微分信号；

滤波模块：用于将所述跟踪微分器输出的所述高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值；根据所述当前采样周期的高度数据及量测噪声值，通过卡尔曼滤波确定当前采样周期的滤波结果数据，将所述当前采样周期的滤波结果数据作为当前采样周期的无人机高度优化值。

较佳地，所述滤波模块，具体用于：

获取上一个采样周期的滤波结果数据及上一个采样周期的校正参数；

根据当前采样周期的量测噪声值及上一个采样周期的校正参数，确定当前采样周期的卡尔曼增益；

根据所述上一个采样周期的滤波结果数据、所述当前采样周期的卡尔曼增益及所述当前采样周期的高度数据，确定当前采样周期的滤波结果数据。

较佳地，所述滤波模块，还用于：

根据当前采样周期的卡尔曼增益及上一个采样周期的校正参数，确定当前采样周期的校正参数，所述当前采样周期的校正参数用于确定下一个采样周期的卡尔曼增益。

较佳地，所述卡尔曼滤波为自适应卡尔曼滤波。

较佳地，所述传感器为气压计。

本发明实施例提供的一种无人机高度优化方法及装置，通过获取当前采样周期无人机气压计输出的高度数据，将高度数据输入跟踪微分器；将所述跟踪微分器输出的高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值；并根据所述当前采样周期的高度数据及量测噪声值，通过卡尔曼滤波确定当前采样周期的滤波结果数据。本发明实施例提供的无人机高度优化方法及装置，将跟踪微分器输出的高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值，量测噪声随着每个周期的量测数据的微分信号而变化，在不增加额外的设备的前提下优化了无人机的高度数据，提高了无人机的实时性和适应性，节约了成本。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种无人机高度优化方法流程示意图；

图2为本发明实施例提供的一种无人机定位系统结构图；

图3为本发明实施例提供的一种跟踪微分器原理如图；

图4为本发明实施例提供的一种EKF滤波的流程图；

图5为本发明实施例提供的一种无人机高度优化装置结构示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明实施例提供的无人机高度优化方法，对应的滤波算法为卡尔曼滤波算法，需要说明的是，本发明实施例所指的卡尔曼滤波不限于标准卡尔曼滤波，还包括自适应卡尔曼滤波、EKF(Extended Kalman Filte，扩展卡尔曼滤波)等。本发明实施例提供的滤波算法可应用于对测量的距离、速度、角速度等信号进行滤波，尤其是原始数据噪声较大的信号的应用场景，如超声波测速、激光雷达测速、GPS测度等。

如图1所示，为本发明实施例提供的一种无人机高度优化方法流程示意图，包括：

步骤101：获取当前采样周期无人机的传感器输出的高度数据，将高度数据输入跟踪微分器。

其中，传感器为气压计，气压计用于计算无人机在空中飞行时的高度数据。跟踪微分器用于计算高度数据的微分信号。

需要说明的是，计算高度数据的微分信号的方式有很多种。例如，直接计算，然而由于高度数据本身存在误差，所以直接计算得到的微分信号的精度较差。此外，还可以利用导数滤波器等提取微分信号，但计算量大，且不便于软件编写。而跟踪微分器(Tracking Differentiator，TD)具有一定的滤波能力，可以滤除野值点，从被噪声干扰的信号中提取出比较精确的微分信号，而且跟踪微分器算法简单，计算量较小便于实现，能够满足实时性的需求。

跟踪微分器应用于不连续或带随机噪声的测量信号提取连续信号及微分信号场景。跟踪微分器基于数值积分优于数值微分的原理，将给定信号的微分转化为对一组微分方程的积分，因而跟踪和微分准确性和稳定性较高。需要说明的是，本发明实施例中的跟踪微分器不限于离散跟踪微分器、非线性跟踪微分器、快速跟踪微分器等。

例如，获取当前采样周期无人机气压计输出的高度值，然后将高度值输入跟踪微分器，得到无人机高度值的微分信号。

步骤102：将跟踪微分器输出的高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值。

在卡尔曼滤波中，量测噪声值代表对量测噪声的置信度，如位置、速度、加速度等的测量值往往在任何时候都存在噪声，且噪声信号不能简单的认为是高斯白噪声，因此将量测噪声值设置为定值会不可避免地影响滤波的精度和效果。本发明实施例将跟踪微分器输出的高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值，量测噪声随着每个周期的量测数据的微分信号而变化，对卡尔曼滤波算法进行优化，提高了卡尔曼滤波的精度。

步骤103：根据当前采样周期的高度数据及量测噪声值，通过卡尔曼滤波确定当前采样周期的滤波结果数据。

其中，当前采样周期的滤波结果数据为当前采样周期的无人机高度优化值。

具体地，根据卡尔曼滤波的过程，首先获取上一个采样周期的滤波结果数据及上一个采样周期的校正参数；然后根据当前采样周期的量测噪声值及上一个采样周期的校正参数，确定当前采样周期的卡尔曼增益；最后根据上一个采样周期的滤波结果数据、当前采样周期的卡尔曼增益及当前采样周期的高度数据，确定当前采样周期的滤波结果数据。其中，校正参数为先验预测协方差值。

进一步地，在确定了当前采样周期的卡尔曼增益之后，还需要根据当前采样周期的卡尔曼增益及上一个采样周期的校正参数，确定当前采样周期的校正参数，当前采样周期的校正参数用于确定下一个采样周期的卡尔曼增益。

例如，将当前周期气压计测量的高度数据的微分信号的绝对值作为当前周期的卡尔曼滤波的量测噪声值，以及当前周期的超声波测量的速度值作为EKF的输入，通过卡尔曼滤波算法，得到滤波后的高度优化值，实现过程简单且精度较高。

本发明实施例中，跟踪微分器输出的气压计高度值的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值，量测噪声随着每个周期的量测数据的微分信号而变化，克服了现有技术中将量测噪声简单认为是高斯白噪声而设置为定值导致滤波效果不佳的问题，提高了卡尔曼滤波的精度。此外，解决了无人机机动或有阵风等场景下无人机气压计数据波动较大或者异常时，出现的明显掉高或升高的问题；同时，克服了现有技术中气压计辅助其它传感器进行无人机定位时的测量范围低、环境适应性弱、飞行器高度变化大等缺点，提高无人机定高性能。

本发明实施例提供的无人机高度优化方法不需要增加额外的设备与气压计配合使用来对无人机进行高度定位，只需要气压计输出的高度数据输入跟踪微分器，将跟踪微分器输出的高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值，量测噪声随着每个周期的量测数据的微分信号而变化，因而扩大了无人机的高度测量范围，在不增加额外的设备的前提下优化了无人机的高度数据，提高了无人机的实时性和适应性，节约了成本。

下面针对具体的实施方式对本发明实施例的无人机高度优化方法进行详细说明。

需要说明的是，对于无人机定位系统，为了得到精确的位置信息，一般通过无人机气压计配合其它传感器进行定位。例如，通过近低空超声波(或者激光)辅助气压计进行定位，通常测距范围为0.2m-8m，然而受反射物材质的影响，对于许多材质的反射物，测距效果较差，如草地、水面等；此外，存在测量范围较小，不适用于大于测距的场景的缺点。又例如，通过近低空单目(或者双目)摄像头辅助气压计进行定位，测距范围受限于摄像头类型，且受环境光影响较大，当光线过强或光线较弱时，摄像头无法正常使用。

如图2所示，为本发明实施例提供的一种无人机定位系统结构图，包括：气压计201、跟踪微分器202、EKF 203。首先获取当前采样周期气压计201输出的高度值Baro.height，然后将高度值Baro.height输入跟踪微分器202，通过跟踪微分器202的跟踪微分计算得到无人机高度值的微分信号，将无人机高度值的微分信号的绝对值作为当前周期EKF 203的量测噪声值EKF 203根据气压计201输出的高度值Baro.height及量测噪声值进行扩展卡尔曼滤波计算，得到高精度的结果EKF.height。本发明实施例不需要增加其它设备即可获得高精度的测量结果，不仅节省了设备而且实现过程简单。

具体地，跟踪微分器202的原理如图3所示，输入信号为v(t)，输出两个信号x₁(t)和x₂(t)。其中，x₁(t)能最快地跟踪输入信号v(t)，x₂(t)为x₁(t)的微分，即作为输入信号v(t)的微分。跟踪微分器202的具体算法见公式(1)，即跟踪微分器202的离散差分方程，x_1,k为输入信号的跟踪信号，x_2,k为输入信号的微分信号，h为采用频率，f₁为非线性函数。

其中，f₁＝fhan(x₁-v,x₂,r,h)，fhan由公式(2)给出。x₁和x₂是跟踪微分器的状态变量；r为系统参数，即体现系统跟踪速度的因子；v为被跟踪的输入信号，即高度值Baro.height。

跟踪微分器202根据公式(1)和公式(2)的计算过程，得到输入信号高度值Baro.height的微分信号x_2,k。

进一步地，对于EKF 203，现有的EKF滤波中量测噪声设置为定值，若设置偏大则对递推数据置信度更高，因而量测数据不能及时起作用；若设置偏小则量测数据能够及时器作用，但于气压计201输出高度值容易受环境影响，无人机在机动或有阵风时气压计201输出高度值噪声会很大。因而，当设置为定值时，无人机容易出现突然掉高或者升高的现象。本发明实施例利用跟踪微分器202提取气压计201输出的高度值Baro.height的微分信号x_2,k，将该微分信号的绝对值|x_2,k|作为EKF 203的量测噪声实时更新值，平衡了偏大或者偏小的误差，从而解决了飞机突然掉高或者升高的问题。

本发明实施例提供一种EKF滤波的流程图，如图4所示，包括：

步骤401：EKF初始化。

具体地，包括卡尔曼增益初始化、预测协方差初始化、建立系统状态方程等。

步骤402：计算k-1时刻的先验预测协方差值P_k,k-1，如公式(3)：

其中，Φ_k,k-1为系统的状态转移矩阵，为系统噪声，可根据实验或仿真得到一个最优的值。

步骤403：计算k-1时刻的先验状态预测值如公式(4)：

其中，为k-1时刻EKF的高度估计值。

步骤404：计算k时刻的卡尔曼增益K_k，如公式(5)。

其中，H_k为量测矩阵，根据步骤402计算出的k-1时刻的先验预测协方差值P_k,k-1，以及将k时刻的高度值微分信号的绝对值|x_2,k|代入公式(5)中的得到k时刻的卡尔曼增益K_k。

步骤405：根据k时刻的高度值更新量测误差，如公式(6)：

具体地，获取步骤403计算出的k-1时刻的先验状态预测值代入公式(6)进行计算。其中，为k时刻高度残差，z_k为k时刻气压计输出的高度值。

步骤406：计算k时刻的状态预测，如公式(7)：

具体地，获取步骤404计算出的k时刻的卡尔曼增益K_k，代入公式(7)进行计算。其中，为k时刻EKF滤波的高度值，即EKF输出的k时刻的高度最优值。

步骤407：计算k时刻预测协方差值，如公式(8)：

P_k＝(I-K_kH_k)P_k,k-1 (8)

具体地，首先获取步骤404计算出的k时刻的卡尔曼增益K_k，以及步骤402计算出的k-1时刻的先验预测协方差值P_k,k-1，代入公式(8)进行计算。在计算出k时刻预测协方差值后，将采样周期k+1更新为k，重复步骤402～407。

在EKF滤波时，如果量测信号z_k噪声较小，应当减小量测噪声的数值；如果量测信号z_k噪声较大，应该增大量测噪声的数值。若将量测噪声设置为定值，在量测信号z_k噪声较大时，残差增大，由于量测噪声是定值，则卡尔曼增益K_k会变大，从而导致将误差较大的量测数据引入了状态更新中，使得状态估计精度下降，从而出现突然掉高或者升高的现象。

本发明实施例中，在量测信号z_k噪声较大时，跟踪微分器202输出的高度值微分信号的绝对值|x_2,k|增大，因而量测噪声变大。此时，虽然残差增大，但是量测噪声增大使得卡尔曼增益K_k减小，因而不会将误差较大的量测数据引入状态估计中，使得状态估计保持较高的精度，从而保持高度稳定。反之，如果量测信号z_k噪声较小时，跟踪微分器202输出的高度微分信号的绝对值|x_2,k|减小，从而使量测噪声变小，此时，虽然残差减小，但是量测噪声减小使得卡尔曼增益K_k增大，因而将误差较小的量测数据引入状态估计中，在保持高度稳定的同时提高了状态估计的精度。

本发明实施例中，跟踪微分器输出的气压计高度值的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值，量测噪声随着每个周期的量测数据的微分信号而变化，克服了现有技术中将量测噪声简单认为是高斯白噪声而设置为定值导致滤波效果不佳的问题，提高了卡尔曼滤波的精度。此外，解决了无人机机动或有阵风等场景下无人机气压计数据波动较大或者异常时，出现的明显掉高或升高的问题；同时，克服了现有技术中气压计辅助其它传感器进行无人机定位时的测量范围低、环境适应性弱、飞行器高度变化大等缺点，提高无人机定高性能。本发明实施例提供的无人机高度优化方法无需增加其他的传感器进行定位，具有成本低、适应性强、实时性好、测量范围广的优点。

基于同样的发明构思，本发明实施例还提供一种无人机高度优化装置，如图5所示，为本发明实施例提供的一种无人机高度优化装置结构示意图，包括：

微分模块501：用于获取当前采样周期无人机的传感器输出的高度数据，将所述高度数据输入跟踪微分器，所述跟踪微分器用于确定所述高度数据的微分信号；

滤波模块502：用于将所述跟踪微分器输出的所述高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值；根据所述当前采样周期的高度数据及量测噪声值，通过卡尔曼滤波确定当前采样周期的滤波结果数据，将所述当前采样周期的滤波结果数据作为当前采样周期的无人机高度优化值。

较佳地，所述滤波模块502，具体用于：

获取上一个采样周期的滤波结果数据及上一个采样周期的校正参数；

根据当前采样周期的量测噪声值及上一个采样周期的校正参数，确定当前采样周期的卡尔曼增益；

根据所述上一个采样周期的滤波结果数据、所述当前采样周期的卡尔曼增益及所述当前采样周期的高度数据，确定当前采样周期的滤波结果数据。

较佳地，所述滤波模块502，还用于：

根据当前采样周期的卡尔曼增益及上一个采样周期的校正参数，确定当前采样周期的校正参数，所述当前采样周期的校正参数用于确定下一个采样周期的卡尔曼增益。

较佳地，所述传感器为气压计。

较佳地，所述卡尔曼滤波为自适应卡尔曼滤波。

本发明实施例中，跟踪微分器输出的气压计高度值的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值，量测噪声随着每个周期的量测数据的微分信号而变化，克服了现有技术中将量测噪声简单认为是高斯白噪声而设置为定值导致滤波效果不佳的问题，提高了卡尔曼滤波的精度。此外，解决了无人机机动或有阵风等场景下无人机气压计数据波动较大或者异常时，出现的明显掉高或升高的问题；同时，克服了现有技术中气压计辅助其它传感器进行无人机定位时的测量范围低、环境适应性弱、飞行器高度变化大等缺点，提高无人机定高性能。

本发明实施例提供的无人机高度优化装置不需要增加额外的设备与气压计配合使用来对无人机进行高度定位，只需要气压计输出的高度数据输入跟踪微分器，将跟踪微分器输出的高度数据的微分信号的绝对值作为卡尔曼滤波当前采样周期的量测噪声值，量测噪声随着每个周期的量测数据的微分信号而变化，因而扩大了无人机的高度测量范围，在不增加额外的设备的前提下优化了无人机的高度数据，提高了无人机的实时性和适应性，节约了成本。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的系统。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令系统的制造品，该指令系统实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。