CN104880722A - 无人机的gps速度及位置观测异常值检测方法 - Google Patents

Abstract

本发明提供一种无人机的GPS速度及位置观测异常值检测方法，包括：获取加速度计的传感器数据与GPS定位装置的观测数据融合之后得到的速度估计值和位置估计值；预定时间之后，计算加速度计观测得到的速度变化量和位置变化量，并记录GPS定位装置的速度观测结果和位置观测结果；得到GPS定位装置的速度观测误差和位置观测误差；设定第一阈值和第二阈值，判断GPS定位装置的速度观测结果和位置观测结果是否异常。根据本发明实施例的无人机的GPS速度及位置观测异常值检测方法，可以快速检测GPS速度和位置观测引入的突变值，避免因环境干扰引起的GPS失效导致无人机坠毁事故，提升无人机的飞行安全性。

Description

无人机的GPS速度及位置观测异常值检测方法

技术领域

本发明涉及无人机上GPS应用技术领域，特别涉及一种无人机的GPS速度及位置观测异常值检测方法。

背景技术

无人机因其具有费效比低、部署迅速和零伤亡等特点，被广泛应用于军事和民用领域，可在灾难场景监测、航拍、搜救、基础设施监察等领域发挥重要作用。

无人机在室外场景执行任务时，一般要使用GPS等装置辅助定位，将GPS观测数据与无人机自带的IMU传感器数据进行融合可以得到相对准确的速度和位置估计。但复杂的飞行环境中，无人机很容易受到电磁、复杂气候以及高大建筑物等的干扰和遮挡，无法获得有效的GPS观测数据，容易造成定位数据突变、速度和位置融合估计结果偏差较大，引起无人机的飞行控制算法出现问题导致坠毁等事故。

现在的搭载GPS等定位装置的无人机应用中，传统的方法是提高GPS速度及位置观测数据的可信度门槛，在GPS获取的数据超过足够高的置信度之后，才使用GPS数据。但面临着两个问题，一方面，极大限制了无人机的应用场景；另一方面，GPS数据即使在某一时刻之后可以使用，也无法保证其数据持续有效，必须要在使用过程中随时判断GPS观测数据的可信度。

发明内容

本发明的目的旨在至少解决上述的技术缺陷之一。

为此，本发明的目的在于提出一种无人机的GPS速度及位置观测异常值检测方法。该方法可以提升无人机的飞行安全性。

为了实现上述目的，本发明公开了一种无人机的GPS速度及位置观测异常值检测方法，所述无人机设置有加速度计和GPS定位装置，所述方法包括以下步骤：获取所述加速度计的传感器数据与所述GPS定位装置的观测数据融合之后得到的速度估计值和位置估计值；预定时间之后，计算所述加速度计观测得到的速度变化量和位置变化量，并记录所述GPS定位装置的速度观测结果和位置观测结果；根据所述速度估计值和所述速度观测结果得到所述GPS定位装置的速度观测误差，并根据所述位置估计值和所述位置观测结果得到所述GPS定位装置的位置观测误差；以及判断所述速度变化量与所述速度观测误差的差值的绝对值是否小于第一阈值，如果否，则判定所述GPS定位装置的速度观测结果异常，并判断所述位置变化量与所述位置观测误差的差值的绝对值是否小于第二阈值，如果否，则判定所述GPS定位装置的位置观测结果异常。

根据本发明实施例的无人机的GPS速度及位置观测异常值检测方法，可以快速检测GPS速度和位置观测引入的突变值，避免因环境干扰引起的GPS失效导致无人机坠毁事故，可以大大提升无人机飞行控制精度，提升无人机的飞行安全性。

在本发明的一个实施例中，所述加速度计的传感器数据包括加速度，所述加速度指无人机体坐标系下的加速度包含xyz三个方向上的分量，表示为：

${\stackrel{\→}{a}}_{\mathrm{acc}}=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right].$

在本发明的一个实施例中，所述加速度与北东地坐标系下的加速度之间的转换关系为

${\stackrel{\→}{a}}_{\mathrm{NED}}=\left[\begin{array}{ccc}\cos K_z\cos K_y& \sin K_z\cos K_x+\cos K_z\sin K_y\sin K_x& \sin K_z\sin K_x-\cos K_z\sin K_y\cos K_x\\ -{\sin K}_z{\cos K}_y& \cos K_z\cos K_x+\sin K_z\sin K_y\sin K_x& \cos K_z\sin K_x-\sin K_z\sin K_y\cos K_x\\ \sin K_y& -\cos K_y\sin K_x& \cos K_y\cos K_x\end{array}\right]a_{\mathrm{acc}},$

其中K_x、K_y、K_z分别为无人机与北东地坐标系下xyz三个轴之间的欧拉角。

在本发明的一个实施例中，所述预定时间之后，所述加速度计观测得到的速度变化量指：北东地坐标系下的速度变化量其中，所述其中，所述Δt为所述预定时间。

在本发明的一个实施例中，所述GPS定位装置的速度观测误差为：

$\mathrm{\Δ}{\stackrel{\→}{V}}_{\mathrm{GPS}}={\stackrel{\→}{V}}_{\mathrm{GPS}}-{\stackrel{\→}{V}}_0,$

其中，所述为初始时刻加速度计传感器数据与GPS速度观测融合得到的速度估计值，为所述预定时间后所述GPS定位装置的速度观测结果。

在本发明的一个实施例中，如果所述则判定所述GPS定位装置的速度观测结果正常，否则判定为异常，其中，所述为所述第一阈值。

在本发明的一个实施例中，所述预定时间之后，所述加速度计观测得到的位置变化量指北东地坐标系下的位置变化量其中，

${\stackrel{\→}{S}}_{\mathrm{acc}}=\frac{1}{2}{\stackrel{\→}{a}}_{\mathrm{NED}}\mathrm{\Δ}{t}^2+{\stackrel{\→}{V}}_{\mathrm{NED}}\mathrm{\Δt}$

在本发明的一个实施例中，所述GPS定位装置的位置观测误差为：

$\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{GPS}}={\stackrel{\→}{S}}_{\mathrm{GPS}}-{\stackrel{\→}{S}}_0,$

其中，所述为初始时刻加速度计传感器数据与GPS速度观测融合得到的位置估计值。

在本发明的一个实施例中，当满足所述时，判定所述GPS定位装置的位置观测结果正常，否则判定为异常，其中，所述为所述第二阈值。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中，

图1为根据本发明一个实施例的GPS速度及位置观测异常值检测方法的流程图；

图2为根据本发明一个实施例的GPS速度及位置观测异常值检测方法的GPS速度观测误差示意图；以及

图3为根据本发明一个实施例的GPS速度及位置观测异常值检测方法的GPS位置观测误差示意图。

具体实施方式

下面详细描述本发明的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

以下结合附图描述根据本发明实施例的无人机的GPS速度及位置观测异常值检测方法。

图1为根据本发明一个实施例的无人机的GPS速度及位置观测异常值检测方法的流程图。如图1所示，该方法包括以下步骤：

步骤S101：获取加速度计的传感器数据与GPS定位装置的观测数据融合之后得到的速度估计值和位置估计值。在无人机能够正常使用GPS的观测数据时，记录加速度计与GPS定位装置的GPS速度观测融合之后得到的速度估计值与加速度计与GPS位置观测融合之后得到的位置估计值

步骤S102：预定时间之后，计算加速度计观测得到的速度变化量和位置变化量，并记录GPS定位装置的速度观测结果和位置观测结果。

预定时间(即单位时间间隔)之后，计算加速度计观测得到的速度变化和位置变化。在这里的一个假设是，认为很短的单位时间Δt内，无人机上的加速度计不会发生观测数据的突变和大量漂移，进而以加速度计为基准，计算无人机的速度变化和位置变化。

首先需要获得北东地坐标系下无人机的加速度与无人机机体坐标系下的加速度的转换关系，即：加速度与北东地坐标系下的加速度之间的转换关系为：

$a_{\mathrm{NED}}=\left[\begin{array}{ccc}\cos K_z\cos K_y& \sin K_z\cos K_x+\cos K_z\sin K_y\sin K_x& \sin K_z\sin K_x-\cos K_z\sin K_y\cos K_x\\ -{\sin K}_z{\cos K}_y& \cos K_z\cos K_x+\sin K_z\sin K_y\sin K_x& \cos K_z\sin K_x-\sin K_z\sin K_y\cos K_x\\ \sin K_y& -\cos K_y\sin K_x& \cos K_y\cos K_x\end{array}\right]a_{\mathrm{acc}},$

其中K_x、K_y、K_z分别为无人机与北东地坐标系下xyz三个轴之间的欧拉角。

无人机的加速度计所获得的加速度包含xyz三个方向上的分量，即：加速度计的传感器数据包括加速度，加速度指无人机体坐标系下的加速度包含xyz三个方向上的分量，表示为：

${\stackrel{\→}{a}}_{\mathrm{acc}}=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right].$

由此，可计算在单位时间Δt内，无人机的速度变化为即：预定时间之后，加速度计观测得到的速度变化量指北东地坐标系下的速度变化量其中，其中，Δt为上述的预定时间。

步骤S103：根据速度估计值和速度观测结果得到GPS定位装置的速度观测误差，并根据位置估计值和位置观测结果得到GPS定位装置的位置观测误差。

GPS定位装置的GPS观测数据在可以使用的情况下，单位时间Δt时，GPS的速度观测结果为即：GPS定位装置的速度观测误差为：

$\mathrm{\Δ}{\stackrel{\→}{V}}_{\mathrm{GPS}}={\stackrel{\→}{V}}_{\mathrm{GPS}}-{\stackrel{\→}{V}}_0,$

其中，为初始时刻加速度计传感器数据与GPS速度观测融合得到的速度估计值，为所述预定时间后GPS定位装置的速度观测结果。

步骤S104：判断速度变化量与速度观测误差的差值的绝对值是否小于第一阈值，如果否，则判定GPS定位装置的速度观测结果异常，并判断位置变化量与位置观测误差的差值的绝对值是否小于第二阈值，如果否，则判定GPS定位装置的位置观测结果异常。

如果则判定GPS定位装置的速度观测结果正常，否则判定为异常，其中为第一阈值。具体来说，GPS的速度观测误差与加速度计观测到的无人机速度变化满足如图2所示，设定速度观测误差的阈值为假设GPS得到两个速度观测误差和其中 $|\mathrm{\Δ}{\stackrel{\→}{V}}_{\mathrm{GPS}1}-\mathrm{\Δ}{\stackrel{\→}{V}}_{\mathrm{acc}}|=\left|\mathrm{\Δ}{\stackrel{\→}{V}}_1\right|\≤\left|\mathrm{\Δ}{\stackrel{\→}{V}}_T\right|,$ 则判断第一个速度观测结果没有突变，即不是异常值可以使用。而第二个观测则判断第二个速度观测发生突变，即产生了异常值需要舍弃。

在GPS的位置观测异常值检测过程中，也遵循着类似的步骤。具体地，预定时间之后，加速度计观测得到的位置变化量指北东地坐标系下的位置变化量其中，

${\stackrel{\→}{S}}_{\mathrm{acc}}=\frac{1}{2}{\stackrel{\→}{a}}_{\mathrm{NED}}\mathrm{\Δ}{t}^2+{\stackrel{\→}{V}}_{\mathrm{NED}}\mathrm{\Δt}$

GPS定位装置的位置观测误差为：

$\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{GPS}}={\stackrel{\→}{S}}_{\mathrm{GPS}}-{\stackrel{\→}{S}}_0,$

其中，为初始时刻加速度计传感器数据与GPS速度观测融合得到的位置估计值。

当满足时，判定GPS定位装置的位置观测结果正常，否则判定为异常，其中，为第二阈值。具体来说，GPS的位置观测误差与加速度计观测到的无人机位置变化满足如图3所示，设定位置观测误差的阈值为假设GPS得到两个位置观测误差和其中 $|\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{GPS}1}-\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{acc}}|=\left|\mathrm{\Δ}{\stackrel{\→}{S}}_1\right|\≤\left|\mathrm{\Δ}{\stackrel{\→}{S}}_T\right|,$ 则判断第一个位置观测结果没有突变，即不是异常值可以使用。而第二个观测则判断第二个位置观测发生突变，即产生了异常值需要舍弃。

根据本发明实施例的无人机的GPS速度及位置观测异常值检测方法，可以快速检测GPS速度和位置观测引入的突变值，避免因环境干扰引起的GPS失效导致无人机坠毁事故，可以大大提升无人机飞行控制精度，提升无人机的飞行安全性。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (9)

1.一种无人机的GPS速度及位置观测异常值检测方法，其特征在于，所述无人机设置有加速度计和GPS定位装置，所述方法包括以下步骤：

获取所述加速度计的传感器数据与所述GPS定位装置的观测数据融合之后得到的速度估计值和位置估计值；

预定时间之后，计算所述加速度计观测得到的速度变化量和位置变化量，并记录所述GPS定位装置的速度观测结果和位置观测结果；

根据所述速度估计值和所述速度观测结果得到所述GPS定位装置的速度观测误差，并根据所述位置估计值和所述位置观测结果得到所述GPS定位装置的位置观测误差；以及

判断所述速度变化量与所述速度观测误差的差值的绝对值是否小于第一阈值，如果否，则判定所述GPS定位装置的速度观测结果异常，并判断所述位置变化量与所述位置观测误差的差值的绝对值是否小于第二阈值，如果否，则判定所述GPS定位装置的位置观测结果异常。

2.根据权利要求1所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，所述加速度计的传感器数据包括加速度，所述加速度指无人机体坐标系下的加速度包含xyz三个方向上的分量，表示为：

${\stackrel{\→}{a}}_{\mathrm{acc}}=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right].$

3.根据权利要求2所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，所述加速度与北东地坐标系下的加速度之间的转换关系为：

${\stackrel{\→}{a}}_{\mathrm{NED}}=\left[\begin{array}{ccc}\cos K_z\cos K_y& \sin K_z\cos K_x+\cos K_z\sin K_y\sin K_x& \sin K_z\sin K_x-\cos K_z\sin K_y\cos K_x\\ -\sin K_z\cos K_y& \cos K_z\cos K_x+\sin K_z\sin K_y\sin K_x& \cos K_z\sin K_x-\sin K_z\sin K_y\cos K_x\\ \sin K_y& -\cos K_y\sin K_x& \cos K_y\cos K_x\end{array}\right]a_{\mathrm{acc}},$

其中K_x、K_y、K_z分别为无人机与北东地坐标系下xyz三个轴之间的欧拉角。

4.根据权利要求3所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，所述预定时间之后，所述加速度计观测得到的速度变化量指：北东地坐标系下的速度变化量其中，所述其中，所述Δt为所述预定时间。

5.根据权利要求4所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，所述GPS定位装置的速度观测误差为：

$\mathrm{\Δ}{\stackrel{\→}{V}}_{\mathrm{GPS}}={\stackrel{\→}{V}}_{\mathrm{GPS}}-{\stackrel{\→}{V}}_0,$

其中，所述为初始时刻加速度计传感器数据与GPS速度观测融合得到的速度估计值，为所述预定时间后所述GPS定位装置的速度观测结果。

6.根据权利要求5所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，如果所述则判定所述GPS定位装置的速度观测结果正常，否则判定为异常，其中，所述为所述第一阈值。

7.根据权利要求6所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，所述预定时间之后，所述加速度计观测得到的位置变化量指北东地坐标系下的位置变化量其中，

$\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{acc}}=\frac{1}{2}{\stackrel{\→}{a}}_{\mathrm{NED}}\mathrm{\Δ}{t}^2+{\stackrel{\→}{V}}_{\mathrm{NED}}\mathrm{\Δt}.$

8.根据权利要求7所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，所述GPS定位装置的位置观测误差为：

$\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{GPS}}={\stackrel{\→}{S}}_{\mathrm{GPS}}-{\stackrel{\→}{S}}_0,$

其中，所述为初始时刻加速度计传感器数据与GPS速度观测融合得到的位置估计值。

9.根据权利要求8所述的无人机的GPS速度及位置观测异常值检测方法，其特征在于，当满足所述时，判定所述GPS定位装置的位置观测结果正常，否则判定为异常，其中，所述为所述第二阈值。