一种基于误差反馈的飞行器惯性加GPS混合高度测量方法

本发明涉及车辆，运动体以及无人飞行器的高精度高度测量领域，具体而言，涉及一种基于误差反馈的飞行器惯性加gps混合高度测量方法的设计方法。

背景技术：

精确的高度信息测量获取对无人飞行器以及飞艇、汽车等在导航与运动控制领域均至关重要，目前高度测量中，大气气压测量的方法测量范围有限，在高空无法使用。而采用惯性技术进行高度测量时，由于加速度到高度的两次积分，导致累积误差随着时间的增加而越来越大。gps测量高度信息的误差不存在时间累积问题，但其末端精度往往存在较大的随机误差。因此如何把多种测量方式得到的高度信息进行融合，吸收每种高度测量的优势，同时又抛弃每种高度测量的劣势，得到最终的多信息融合的组合测量方法，是目前工程应用中十分关心的问题。基于上述背景技术，本发明提出了一种基于误差反馈，多次融合并滤波变换进行高度信息组合融合的方法，具有很高的精度，从而也具有较高的工程应用价值。

需要说明的是，在上述背景技术部分发明的信号仅用于加强对本发明的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信号。

技术实现要素：

本发明的目的在于提供一种基于误差反馈的飞行器惯性加gps混合高度测量方法，进而至少在一定程度上克服由于相关技术的限制和缺陷而导致的单一高度测量精度不高的问题。

根据本发明的一个方面，提供一种基于误差反馈的飞行器惯性加gps混合高度测量方法，包括以下步骤：

步骤s10，在载体上安装sia200mems加速度传感器，并进行积分得到惯性速度与惯性高度；安装gps接收器，测量gps高度；

步骤s20，根据所述的惯性高度与gps高度进行对比求差，得到高度误差信号，然后进行误差非线性变换，得到高度误差非线性变换信号，最后进行非线性校正，得到高度误差非线性校正信号；

步骤s30，针对所述的高度误差非线性校正信号，引入加速度测量信号，生成补偿速度；并与惯性速度进行比较，得到速度误差信号；

步骤s40，根据高度误差信号进行滤波，得到高度误差滤波信号，并与速度误差信号叠加进行补偿，引入惯性速度信号，生成补偿高度信号；

步骤s50，根据补偿高度信号，进行非线性变换滤波，得到补偿高度非线性变换滤波信号；

步骤s60，对gps高度信号进行滤波，得到gps高度滤波信号，根据所述的补偿高度误非线性变换滤波信号与gps高度滤波信号进行比较，得到补偿高度误差信号，然后引入加速度测量信号，生成合成速度信号；

步骤s70，根据所述的合成速度信号与补偿高度误差信号，生成合成高度信号，并与gps滤波高度形成合成高度误差信号，并反馈补偿，对合成高度信号进行修正，得到最终的合成高度信号。

在本发明的一种示例实施例中，在载体上安装sia200mems加速度传感器，并进行积分得到惯性速度与惯性高度；安装gps接收器，测量gps高度包括：

v＝∫adt；

h＝∫vdt；

其中a(n)对应的是时间t＝n*δt时刻采用sia200mems加速度传感器测量的载体加速度信号，简写为a，其中δt为数据的输出采样周期，dt表示对时间信号的积分，v为惯性速度信号，h为惯性高度信号。

在本发明的一种示例实施例中，根据所述的惯性高度与gps高度求差，得到高度误差信号，进行非线性变换，与非线性校正，得到高度误差非线性校正信号包括：

e1(n)＝y1(n)-h(n)。

其中y1(n)为在载体上安装gps接收模块，测量得到的载体gps高度。h为惯性高度信号，e1(n)为高度误差信号，e2(n)为高度误差非线性变换信号，e3(n)为高度误差非线性校正信号，k1、k2与ε1为常值参数，其详细设置见后文案例实施。

在本发明的一种示例实施例中，针对所述的高度误差非线性校正信号，引入加速度测量信号，生成补偿速度；并与惯性速度进行比较，得到速度误差信号包括：

v1(n+1)＝v1(n)+a(n)δt+k3e3(n)δt；

ev(n)＝v1(n)-v(n)；

其中a(n)为加速度测量信号，v1(n)为补偿速度信号，e3(n)为高度误差非线性校正信号。k3为常值参数，其详细设置见后文案例实施。ev(n)为速度误差信号。

在本发明的一种示例实施例中，根据高度误差信号，进行滤波，得到高度误差滤波信号，并与速度误差信号叠加进行补偿，引入惯性速度信号，生成补偿高度信号包括：

y2(n+1)＝y2(n)+v(n)δt+k6ev(n)δt+k7e4(n)δt；

其中e1(n)为高度误差信号，e4(n)为高度误差滤波信号，v(n)为惯性速度信号，ev(n)为速度误差信号，e4(n)为高度误差滤波信号，y2(n)为补偿高度信号，k4、k5、ε2k6、k7为常值参数，其详细设置见后文案例实施。

在本发明的一种示例实施例中，根据补偿高度信号，进行非线性变换滤波，得到补偿高度误差非线性变换滤波信号包括：

其中y2(n)为补偿高度信号，w1(n)为其非线性变换信号，y3(n)为补偿高度非线性变换滤波信号，k8、k9与ε3为常值参数，其详细设置见后文案例实施。

在本发明的一种示例实施例中，对gps高度信号进行滤波，得到gps高度滤波信号，根据所述的补偿高度误非线性变换滤波信号与gps高度滤波信号进行比较，得到补偿高度误差信号，然后引入加速度测量信号，生成合成速度信号包括：

y4(n+1)＝y4(n)+k10(y1(n)-y4(n))；

e5(n)＝y3(n)-y4(n)；

v2(n+1)＝v2(n)+a(n)δt+k11e5(n)δt；

其中y1(n)为gps高度信号，y4(n)为gps高度滤波信号，k10、k11为常值参数，其详细设置见后文案例实施。y3(n)为补偿高度误非线性变换滤波信号，e5(n)为补偿高度误差信号，a(n)为加速度测量信号，v2(n)合成速度信号。

在本发明的一种示例实施例中，根据所述的合成速度信号与补偿高度误差信号，生成合成高度信号，并与gps滤波高度形成合成高度误差信号，并反馈补偿，对合成高度信号进行修正，得到最终的合成高度信号包括：

y5(n+1)＝y5(n)+v2(n)δt+k12e5(n)δt；

e6(n)＝y5(n)-y4(n)；

y6(n+1)＝y6(n)+v2(n)δt+k13e6(n)δt+k14(y4(n)-y6(n))δt；

其中v2(n)为合成速度信号，e5(n)为补偿高度误差信号，y5(n)为合成高度信号，e6(n)为合成高度误差信号，y6(n)为综合高度信号，其中k12、k13、k14为常值参数，其详细设置见后文案例实施。

最终求得的y6(n)即作为整个gps测量高度与惯性测量高度的综合高度输出，其具有良好的精度特性，同时又避免的惯性测量的高度发散的缺点。

有益效果

本发明提供的一种基于误差反馈的飞行器惯性加gps混合高度测量方法，其通过多重高度误差反馈与速度误差反馈与积分以及滤波的方式，使得混合高度测量能够滤除gps测量的大量无规则随机误差，同时又能消除惯性加速度计积分带来的时间累积误差，从而使得两者高度测量的优势能够相互结合互补，实现更佳的混合测量效果。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的一种基于误差反馈的飞行器惯性加gps混合高度测量方法的流程图；

图2是本发明实施例所提供方法的sia200mems加速度传感器实物图；

图3是本发明实施例所提供方法的topgnssgps位置接收器实物图；

图4是本发明实施例所提供方法的载体的加速度信号曲线(单位：米每秒方)；

图5是本发明实施例所提供方法的载体的惯性速度信号曲线(单位：米每秒)；

图6是本发明实施例所提供方法的载体的惯性高度信号曲线(单位：米)；

图7是本发明实施例所提供方法的载体的gps高度信号曲线(单位：米)；

图8是本发明实施例所提供方法的载体高度误差非线性校正信号(无单位)；

图9是本发明实施例所提供方法的载体补偿速度信号曲线(单位：米)；

图10是本发明实施例所提供方法的载体补偿高度信号曲线(单位：米)；

图11是本发明实施例所提供方法的载体补偿高度非线性变换滤波信号(无单位)；

图12是本发明实施例所提供方法的载体合成速度信号曲线(单位：米)；

图13是本发明实施例所提供方法的载体合成高度信号曲线(单位：米)。

具体实施方式

现在将参考附图基础上更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本发明将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。在下面的描述中，提供许多具体细节从而给出对本发明的实施方式的充分理解。然而，本领域技术人员将意识到，可以实践本发明的技术方案而省略所述特定细节中的一个或更多，或者可以采用其它的方法、组元、装置、步骤等。在其它情况下，不详细示出或描述公知技术方案以避免喧宾夺主而使得本发明的各方面变得模糊。

本发明提供了一种基于误差反馈的飞行器惯性加gps混合高度测量方法，其通过安装sia200mems加速度传感器测量载体加速度进行两次积分得到惯性高度，然后通过gps的测量得到载体的gps高度，两者对比得到高度误差信号，再通过非线性变换与校正叠加惯性加速度积分后生成补偿速度与补偿高度信号，再与gps滤波高度对比生成补偿高度误差信号，再次叠加加速度信号积分后生成合成速度与合成高度信号，并引入高度误差信号进行反馈修正，得到最终的高精度合成高度信号，从而使得整个混合高度测量方法具有很高的工程应用价值。

下面，将结合附图对本发明的一种基于误差反馈的飞行器惯性加gps混合高度测量方法进行进一步的解释以及说明。参考图1所示，该一种基于误差反馈的飞行器惯性加gps混合高度测量方法包括以下步骤：

步骤s10，在载体上安装sia200mems加速度传感器，并进行积分得到惯性速度与惯性高度；安装topgnssgps位置接收器，测量载体gps高度；

具体的，首先，在载体上安装sia200mems加速度传感器，其物理尺寸为小封装9*9nm²，其非常小，其物理尺寸如图2所示。功耗小于10mw，工作温度范围为-40度至+125度。加速度量程2g至30g可选，测量分辨率可达到0.00005g。将其安装在载体上，测量载体垂直方向的加速度信号，计作a(n)。其中n＝1,2,3…，对应的是时间t＝n*δt时刻的加速度信号，其中δt为数据的输出采样周期。

其次，对载体加速度测量数据进行积分，得到惯性速度信号，记作v(n)，然后对速度信号进一步积分，得到惯性高度信号，记作h(n)，其积分过程如下所示：

v＝∫adt；h＝∫vdt；

其中dt表示对时间信号积分。

最后，在载体上安装topgnssgps位置接收器，其实物如图3所示，测量载体gps高度，记作y1，然后按照加速度计的数据间隔周期δt，将gps高度数据进行离散化，得到数据y1(n)，其中n＝1,2,3…，对应的是时间t＝n*δt时刻的gps高度测量数据，其中δt为数据的输出采样周期；

步骤s20，根据所述的惯性高度与gps高度进行对比求差，得到高度误差信号，然后进行误差非线性变换，得到高度误差非线性变换信号，最后进行非线性校正，得到高度误差非线性校正信号；

具体的，首先，将所述的惯性高度h(n)与gps高度测量数据y1(n)进行对比，得到高度误差信号，记作e1(n)，其计算方式如下e1(n)＝y1(n)-h(n)。

其次，将高度误差信号进行非线性变换，得到高度误差非线性变换信号，记作e2(n)，其变换方式如下

然后，对高度误差信号进行非线性校正，得到高度误差非线性校正信号，记作e3(n)，其计算方式如下：

其中k1、k2与ε1为常值参数，其详细设置见后文案例实施。

步骤s30，针对所述的高度误差非线性校正信号，引入加速度测量信号，生成补偿速度；并与惯性速度进行比较，得到速度误差信号；

具体的，首先，根据所述的加速度测量信号a(n)，叠加所述的高度误差非线性校正信号，再进行积分，得到补偿速度信号，记作v1(n)，其计算方式如下：

v1(n+1)＝v1(n)+a(n)δt+k3e3(n)δt；

其中k3为常值参数，其详细设置见后文案例实施。

其次，将所述的补偿速度v1(n)与惯性速度v(n)进行对比得到速度误差信号，记作ev(n)，其计算方式如下：

ev(n)＝v1(n)-v(n)；

步骤s40，根据高度误差信号，进行滤波，得到高度误差滤波信号，并与速度误差信号叠加进行补偿，引入惯性速度信号，生成补偿高度信号；

具体的，首先根据所述的高度误差信号e1(n)，进行滤波，得到高度误差滤波信号，记作e4(n)，其滤波方式如下：

其中k4、k5与ε2为常值参数，其详细设置见后文案例实施。

其次，根据惯性速度信号v(n)与速度误差信号ev(n)以及高度误差滤波信号e4(n)进行叠加，然后积分得到补偿高度信号，记作y2(n)，其计算方式如下：

y2(n+1)＝y2(n)+v(n)δt+k6ev(n)δt+k7e4(n)δt；

其中k6、k7为常值参数，其详细设置见后文案例实施。

步骤s50，根据补偿高度信号，进行非线性变换滤波，得到补偿高度非线性变换滤波信号；

具体的，首先针对所述的补偿高度信号y2(n)，进行非线性变换，得到非线性变换信号，记住w1(n)，然后进行非线性滤波，得到补偿高度非线性变换滤波信号，记作y3(n)，其计算方式如下：

其中k8、k9与ε3为常值参数，其详细设置见后文案例实施。

步骤s60，对gps高度信号进行滤波，得到gps高度滤波信号，根据所述的补偿高度误非线性变换滤波信号与gps高度滤波信号进行比较，得到补偿高度误差信号，然后引入加速度测量信号，生成合成速度信号；

具体的，首先，根据所述的gps高度信号y1(n)，进行如下的滤波，生成gps高度滤波信号，记作y4(n)，其计算方式如下：

y4(n+1)＝y4(n)+k10(y1(n)-y4(n))；

其中k10为常值参数，其详细设置见后文案例实施。

其次，根据所述的补偿高度误非线性变换滤波信号y3(n)与gps高度滤波信号y4(n)进行比较，得到补偿高度误差信号，记作e5(n)，其计算方式如下：

e5(n)＝y3(n)-y4(n)；

最后，根据补偿高度误差信号e5(n)叠加加速度测量信号a(n)，生成合成速度信号，记作v2(n)，其计算方式如下：

v2(n+1)＝v2(n)+a(n)δt+k11e5(n)δt；

其中k11为常值参数，其详细设置见后文案例实施。

步骤s70，根据所述的合成速度信号与补偿高度误差信号，生成合成高度信号，并与gps滤波高度形成合成高度误差信号，并反馈补偿，对合成高度信号进行修正，得到最终的合成高度信号。

具体的，首先根据所述的合成速度信号v2(n)，叠加补偿高度误差信号e5(n)，生成合成高度信号，记作y5(n)，其计算方式如下：

y5(n+1)＝y5(n)+v2(n)δt+k12e5(n)δt；

其中k12为常值参数，其详细设置见后文案例实施。

然后对合成高度信号与gps滤波高度对比形成合成高度误差信号，计作e6(n)，其计算方式如下：

e6(n)＝y5(n)-y4(n)；

最后，引入合成高度误差信号，以及自身反馈信号，进行补偿，得到最终的综合高度信号，记作y6(n)，其计算方式如下：

y6(n+1)＝y6(n)+v2(n)δt+k13e6(n)δt+k14(y4(n)-y6(n))δt；

其中k13、k14为常值参数，其详细设置见后文案例实施。

最终求得的y6(n)即作为整个gps测量高度与惯性测量高度的综合高度输出，其具有良好的精度特性，同时又避免的惯性测量的高度发散的缺点。

案例实施与计算机仿真模拟结果分析

在步骤s10中，设置δt＝0.001，在载体上安装sia200mems加速度传感器，测量载体的加速度信号如图4所示，进行积分得到载体的惯性速度信号如图5所示，再积分得到载体的惯性高度信号如图6所示，采用gps测量得到载体的gps高度如图7所示。

在步骤s20中，设置k1＝0.01、k2＝0.02与ε1＝5，得到高度误差非线性校正信号如图8所示。

在步骤s30中，选取k3＝0.01，得到补偿速度信号如图9所示。在步骤s40中，选取k4＝0.01、k5＝0.01与ε2＝0.5，k6＝0.005、k7＝0.002，得到补偿高度信号如图10所示。

在步骤s50中，选取k8＝0.01、k9＝0.015与ε3＝0.5，得到补偿高度非线性变换滤波信号如图11所示。

在步骤s60中，选取k10＝0.05，k11＝0.003，得到合成速度信号如图12所示。

在步骤s70中，选取k12＝0.005，k13＝0.0015、k14＝0.001，得到合成高度信号如图13所示。

本领域技术人员在考虑说明书及实践这类的发明后，将容易想到本发明的其他实施例。本申请旨在涵盖本发明的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本发明未指明的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本发明的真正范围和精神由权利要求指出。