一种翼伞系统航迹重构的状态估计方法、装置、计算机设备和存储介质

技术领域

本申请涉及翼伞系统技术领域，特别是涉及翼伞系统航迹重构的状态估计方法、装置、设备和介质。

背景技术

翼伞系统是一种典型的非线性、多柔体飞行器，动力学特性十分复杂，建立一个精确可靠的动力学理论模型十分困难，目前对于翼伞系统的认识多由实验手段获得。通过翼伞飞行试验，可以分析系统飞行性能和动力学特性，为参数配置和控制策略的优化与改进提供依据。

根据在飞行试验中获得的数据和运动学关系，可以得到空投系统各状态变量的时间历程，这一过程称为重构输出或航迹重构。航迹重构对理论模型合理性验证以及传感器校准具有重要意义，甚至还可用于飞行异常时的故障诊断；此外，当某些状态量测不准或不可测时，通过航迹重构可以还原出这些状态量的变化情况，代替传感器的测量结果。

现有技术中，航迹重构采用的方法包括：最小二乘估计法、输出误差法以及滤波误差法等。其中，最小二乘估计法假设噪声仅存在于系统输入，方法较为简单，但对于复杂问题适用性较差；输出误差法假设噪声均由测量过程产生，该方法计算耗时，内存占用较大；滤波误差法假设噪声同时存在于系统过程和测量过程中，但算法复杂、计算量大，实际应用较少。

翼伞空投系统是一种复杂的非线性多柔体动力学系统，在伞衣和伞绳上安装传感器十分困难，且由于遮挡、固定、大变形等原因，所测数据可信度较低，飞行攻角等数据难以直接测量得到，若可以重构输出这些状态变量，则有助于更好了解翼伞的飞行特性。

发明内容

基于此，有必要针对上述技术问题，提供一种翼伞系统航迹重构的状态估计方法、装置、设备和介质，能够重构翼伞系统各状态量的时间历程。

一种翼伞系统航迹重构的状态估计方法，包括：

获取翼伞系统的过程方程以及翼伞系统的观测数据，并根据所述观测数据得到翼伞系统的观测方程；分别对所述过程方程和所述观测方程进行泰勒展开，得到线性卡尔曼状态转移方程；

根据所述线性卡尔曼状态转移方程，得到翼伞系统状态的先验估计以及翼伞系统状态的后验估计方程；

根据所述线性卡尔曼状态转移方程、所述先验估计以及所述后验估计方程，得到先验状态误差和后验状态误差；根据所述先验状态误差、所述后验状态误差以及所述后验估计方程，得到卡尔曼增益方程；定义误差协方差矩阵和先验误差协方差矩阵，对所述卡尔曼增益方程进行求解，得到使误差协方差矩阵的迹最小的卡尔曼增益，并得到卡尔曼增益方程的待定系数矩阵；

根据后验估计方程、卡尔曼增益以及待定系数矩阵，得到翼伞系统状态的状态重构结果。

在一个实施例中，获取翼伞系统的过程方程以及翼伞系统的观测数据，并根据所述观测数据得到翼伞系统的观测方程；分别对所述过程方程和所述观测方程进行泰勒展开，得到线性卡尔曼状态转移方程包括：

获取翼伞系统的过程方程：

式中，x

获取翼伞系统的观测数据，并根据观测数据得到翼伞系统的观测方程：

y

式中，y

定义状态误差向量x和观测误差向量y为：

x＝x

y＝y

其中：

y

将过程方程在x

式中，F为f函数的Jacobi矩阵；

将观测方程在x

式中，H为h函数的Jacobi矩阵；

对于过程微分方程，解的形式为：

x

式中，x

根据式(9)和式(10)，得到线性卡尔曼状态转移方程：

x

y

其中：

Φ

式中，y

在一个实施例中，根据所述线性卡尔曼状态转移方程，得到翼伞系统状态的先验估计以及翼伞系统状态的后验估计方程包括：

根据线性卡尔曼状态转移方程，定义翼伞系统状态的先验估计

式中，J

在一个实施例中，根据所述线性卡尔曼状态转移方程、所述先验估计以及所述后验估计方程，得到先验状态误差和后验状态误差包括：

根据所述线性卡尔曼状态转移方程即式(11)、先验估计以及后验估计方程即式(15)，得到先验状态误差

在一个实施例中，根据所述先验状态误差、所述后验状态误差以及所述后验估计方程，得到卡尔曼增益方程包括：

将先验状态误差即式(16)代入后验估计方程即式(15)，得：

再将式(12)代入式(18)，得：

再将式(19)代入后验状态误差即式(17)，得：

根据随机过程的正交性法则，后验状态误差

将式(20)代入式(21)，得：

/>

再将式(12)(令k＝i)代入式(22)，得：

根据随机测量过程噪声、测量噪声与测量过程无关联、正交性准则，由式(23)得：

由于

J

将式(27)代入式(15)，得到卡尔曼增益方程：

式中，K

在一个实施例中，定义误差协方差矩阵和先验误差协方差矩阵，对所述卡尔曼增益方程进行求解，得到使误差协方差矩阵的迹最小的卡尔曼增益包括：

定义误差协方差矩阵P

根据式(20)、式(27)以及式(29)，得：

定义先验误差协方差矩阵

根据式(31)有：

令式(33)中P

式(34)解得：

在一个实施例中，根据后验估计方程、卡尔曼增益以及待定系数矩阵，得到翼伞系统状态的状态重构结果包括：

J

其中

一种翼伞系统航迹重构的状态估计装置，包括：

获取模块，用于获取翼伞系统的过程方程以及翼伞系统的观测数据，并根据所述观测数据得到翼伞系统的观测方程；分别对所述过程方程和所述观测方程进行泰勒展开，得到线性卡尔曼状态转移方程；

估计模块，用于根据所述线性卡尔曼状态转移方程，得到翼伞系统状态的先验估计以及翼伞系统状态的后验估计方程；

计算模块，用于根据所述线性卡尔曼状态转移方程、所述先验估计以及所述后验估计方程，得到先验状态误差和后验状态误差；根据所述先验状态误差、所述后验状态误差以及所述后验估计方程，得到卡尔曼增益方程；定义误差协方差矩阵和先验误差协方差矩阵，对所述卡尔曼增益方程进行求解，得到使误差协方差矩阵的迹最小的卡尔曼增益，并得到卡尔曼增益方程的待定系数矩阵；

重构模块，用于根据后验估计方程、卡尔曼增益以及待定系数矩阵，得到翼伞系统状态的状态重构结果。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

获取翼伞系统的过程方程以及翼伞系统的观测数据，并根据所述观测数据得到翼伞系统的观测方程；分别对所述过程方程和所述观测方程进行泰勒展开，得到线性卡尔曼状态转移方程；

根据所述线性卡尔曼状态转移方程，得到翼伞系统状态的先验估计以及翼伞系统状态的后验估计方程；

根据所述线性卡尔曼状态转移方程、所述先验估计以及所述后验估计方程，得到先验状态误差和后验状态误差；根据所述先验状态误差、所述后验状态误差以及所述后验估计方程，得到卡尔曼增益方程；定义误差协方差矩阵和先验误差协方差矩阵，对所述卡尔曼增益方程进行求解，得到使误差协方差矩阵的迹最小的卡尔曼增益，并得到卡尔曼增益方程的待定系数矩阵；

根据后验估计方程、卡尔曼增益以及待定系数矩阵，得到翼伞系统状态的状态重构结果。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

获取翼伞系统的过程方程以及翼伞系统的观测数据，并根据所述观测数据得到翼伞系统的观测方程；分别对所述过程方程和所述观测方程进行泰勒展开，得到线性卡尔曼状态转移方程；

根据所述线性卡尔曼状态转移方程，得到翼伞系统状态的先验估计以及翼伞系统状态的后验估计方程；

根据所述线性卡尔曼状态转移方程、所述先验估计以及所述后验估计方程，得到先验状态误差和后验状态误差；根据所述先验状态误差、所述后验状态误差以及所述后验估计方程，得到卡尔曼增益方程；定义误差协方差矩阵和先验误差协方差矩阵，对所述卡尔曼增益方程进行求解，得到使误差协方差矩阵的迹最小的卡尔曼增益，并得到卡尔曼增益方程的待定系数矩阵；

根据后验估计方程、卡尔曼增益以及待定系数矩阵，得到翼伞系统状态的状态重构结果。

上述翼伞系统航迹重构的状态估计方法、装置、设备和介质，是一种基于卡尔曼状态估计的翼伞系统航迹重构方法，假设系统和测量过程均有噪声存在，适用于多种航迹重构问题，应用广泛，具体地，在翼伞系统某时刻的真实状态量未知的情况下，利用动力学理论模型(也就是过程方程)得到下一时刻状态量的先验估计值，通过观察或测量手段获得了翼伞飞行数据的测量值，根据测量值采用上述扩展卡尔曼滤波方法估计真实值，并结合动力学理论模型的动力学和运动学关系，重构无法测量的攻角等其他状态量的时间历程，还原出非线性的翼伞系统在某段时间历程内各状态量的动力学状态变化情况，在对于翼伞系统认识不充分、动力学模型不完善的情况下，为模型修正和飞行特性分析提供参考依据。

附图说明

图1为一个实施例中翼伞系统航迹重构的状态估计方法的应用场景图；

图2为一个实施例中翼伞系统航迹重构的状态估计方法的流程示意图；

图3为一个实施例中水平速度重构结果与偏差图，其中，(a)为水平速度重构结果图，(b)为水平速度偏差图；

图4为一个实施例中高度重构结果与偏差图，其中，(a)为高度重构结果图，(b)为高度偏差图；

图5为一个实施例中航向角重构结果与偏差图，其中，(a)为航向角重构结果图，(b)为航向角偏差图；

图6为一个实施例中俯仰角重构结果与偏差图，其中，(a)为俯仰角重构结果图，(b)为俯仰角偏差图；

图7为一个实施例中滚转角重构结果与偏差图，其中，(a)为滚转角重构结果图，(b)为滚转角偏差图；

图8为一个实施例中三维轨迹重构结果与偏差图，其中，(a)为三维轨迹重构结果图，(b)为三维轨迹偏差图；

图9为一个实施例中重构的攻角、侧滑角与升阻比的对比图，其中，(a)为重构的攻角，(b)为重构的侧滑角，(c)为重构的升阻比；

图10为一个实施例中翼伞系统航迹重构的状态估计装置的结构框图；

图11为一个实施例中计算机设备的内部结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明，本申请实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本申请中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多组”的含义是至少两组，例如两组，三组等，除非另有明确具体的限定。

在本申请中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。

另外，本申请各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本申请要求的保护范围之内。

本申请提供的方法，可以应用于如图1所示的应用环境中。其中，终端102通过网络与服务器104进行通信，终端102可以包括但不限于是各种个人计算机、笔记本电脑、智能手机、平板电脑和便携式可穿戴设备，服务器104可以是各类门户网站、工作系统后台对应的服务器等。

本申请提供了一种翼伞系统航迹重构的状态估计方法，如图2所示，在一个实施例中，以该方法应用于图1中的终端为例进行说明，包括：

步骤202，获取翼伞系统的过程方程以及翼伞系统的观测数据，并根据观测数据得到翼伞系统的观测方程；分别对过程方程和观测方程进行泰勒展开，得到线性卡尔曼状态转移方程。

具体地：

获取翼伞系统的过程方程：翼伞系统是典型的非线性连续动态系统，其状态由如下的非线性微分方程描述：

式中，x

过程噪声w定义为：

式中，E为数学期望值，w

在对翼伞系统状态进行实际观察与测量的过程中，获取翼伞系统的观测数据，并根据观测数据得到翼伞系统的观测方程，观测方程也表示为非线性形式：

y

式中，y

观测噪声v定义为：

式中，v

定义状态误差向量x和观测误差向量y为：

x＝x

y＝y

其中：

y

式中，x

当参考航迹与真实航迹足够接近时，二者偏差可以由偏差的泰勒展开的前几项近似代替。

忽略噪声项w，将过程方程也就是式(1)在x

取式(7)前两项，则有过程微分方程：

式中，F为f函数的Jacobi矩阵；

同理，将观测方程也就是式(3)在x

式中，H为h函数的Jacobi矩阵；

对于过程微分方程即式(8)，解的形式为：

x

式中，x

加入噪声项w、v，并根据式(8)、式(9)和式(10)，得到线性卡尔曼状态转移方程：

x

y

其中：

Φ

式中，y

步骤204，根据线性卡尔曼状态转移方程，得到翼伞系统状态的先验估计以及翼伞系统状态的后验估计方程。

具体地：

定义翼伞系统状态的先验估计

定义翼伞系统状态的后验估计

根据线性卡尔曼状态转移方程即式(11)和式(12)，将后验估计结果也就是重构结果表示为先验估计和测量结果的线性组合，得到翼伞系统状态的后验估计方程：

式中，J

步骤206，根据线性卡尔曼状态转移方程、先验估计以及后验估计方程，得到先验状态误差和后验状态误差；根据先验状态误差、后验状态误差以及后验估计方程，得到卡尔曼增益方程；定义误差协方差矩阵和先验误差协方差矩阵，对卡尔曼增益方程进行求解，得到使误差协方差矩阵的迹最小的卡尔曼增益，并得到卡尔曼增益方程的待定系数矩阵。

具体地：

根据线性卡尔曼状态转移方程即式(11)和式(12)、先验估计以及后验估计方程即式(15)，得到先验状态误差

将先验状态误差即式(16)代入后验估计方程即式(15)，得：

再将式(12)代入式(18)，得：

再将式(19)代入后验状态误差即式(17)，得：

根据随机过程的正交性法则，后验状态误差

将式(20)代入式(21)，得：

再将式(12)(令k＝i)代入式(22)，得：

根据随机测量过程噪声也就是观测噪声的定义即式(4)，式(23)的最后一项为零：

因为测量噪声与测量过程无关联，式(24)的最后一项也为零：

根据正交性准则，式(25)的最后一项也为零：

由于

J

将式(27)代入式(15)，整理得到卡尔曼增益方程：

式中，K

为求取K

将式(27)代入式(20)，得：

将式(30)代入式(29)，得：

定义先验误差协方差矩阵

根据式(4)有：

假设v

/>

则式(31)成为：

令式(33)中P

式(34)解得：

由式(35)得到的卡尔曼增益值使得误差协方差矩阵P

由式(27)和(35)得待定系数矩阵J

步骤208，根据后验估计方程、卡尔曼增益以及待定系数矩阵，得到翼伞系统状态的状态重构结果。

具体地：

利用式(15)、(27)和(35)，得到基于测量值y

其中

需要说明，在得到状态重构结果之后，还需要更新误差协方差矩阵，在下一个时刻k+1的状态估计中，将使用前一个时刻所更新的误差协方差矩阵P

在式(35)等号两边右乘

将式(33)展开，根据式(36)，获得测量值后误差协方差矩阵由P

上述翼伞系统航迹重构的状态估计方法，是一种基于卡尔曼状态估计的翼伞系统航迹重构方法，假设系统和测量过程均有噪声存在，适用于多种航迹重构问题，应用广泛，具体地，在翼伞系统某时刻的真实状态量未知的情况下，利用动力学理论模型(也就是过程方程)得到下一时刻状态量的先验估计值，通过观察或测量手段获得了翼伞飞行数据的测量值，根据测量值采用上述扩展卡尔曼滤波方法估计真实值，并结合动力学理论模型的动力学和运动学关系，重构无法测量的攻角等其他状态量的时间历程，还原出非线性的翼伞系统在某段时间历程内各状态量的动力学状态变化情况，在对于翼伞系统认识不充分、动力学模型不完善的情况下，为模型修正和飞行特性分析提供参考依据。本申请对仅适用于线性系统的卡尔曼滤波方法进行改进，使之可以扩展应用到非线性系统，将非线性系统方程线性化，对非线性复杂系统的状态估计结果误差小、效果佳，达到更佳的航迹重构效果。

应该理解的是，虽然图2的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

在一个具体的实施例中，以六自由度翼伞系统动力学模型为例，系统的状态方程由下式描述：

式中，m为翼伞系统的质量矩阵；m

取噪声项均服从均值为0、方差为1的高斯分布，观测矩阵、状态转移矩阵、初始时刻状态量的协方差矩阵均为单位阵I，采用卡尔曼状态估计对翼伞系统在盘旋段的状态变量进行重构，还原出该时间历程内系统状态的变化情况。进行重构的状态量包括高度、水平速度、姿态角(航向角、滚转角、俯仰角)、三维轨迹，模型仿真结果、航迹重构结果、测量结果，模型仿真结果、重构结果与测量值的相对偏差如图3至图8所示。

图3至图8中的结果表明，和理论模型相比，航迹重构结果更加与测量结果的相对偏差显著减小，在测量结果可信度更高的前提下，可认为航迹重构的结果更接近翼伞系统的实际飞行状态。

根据重构结果和动力学关系，还可导出没有直接测量的攻角、侧滑角、升阻比等其他状态变量的时间历程，可作为分析翼伞飞行性能的参考，如图9所示。

根据图9中模型结果与EKF结果(即重构结果)的对比，该翼伞系统在1200m-2200m高空盘旋时，飞行攻角约为20°、侧滑角为-3～-4°、升阻比为2.5左右，符合该型号翼伞的设计工况，且三者均呈周期性波动，波动周期约等于航向角的变化周期(约70s)，也就是翼伞在稳定盘旋飞行时，攻角、侧滑角、升阻比也在稳定波动，这一现象在水平速度和滚转角的测量结果中也可观察到，侧面印证了重构结果的可靠性。

本申请还提供了一种翼伞系统航迹重构的状态估计装置，如图10所示，在一个实施例中，包括：获取模块1002、估计模块1004、计算模块1006和重构模块1008，其中：

获取模块1002，用于获取翼伞系统的过程方程以及翼伞系统的观测数据，并根据所述观测数据得到翼伞系统的观测方程；分别对所述过程方程和所述观测方程进行泰勒展开，得到线性卡尔曼状态转移方程；

估计模块1004，用于根据所述线性卡尔曼状态转移方程，得到翼伞系统状态的先验估计以及翼伞系统状态的后验估计方程；

计算模块1006，用于根据所述线性卡尔曼状态转移方程、所述先验估计以及所述后验估计方程，得到先验状态误差和后验状态误差；根据所述先验状态误差、所述后验状态误差以及所述后验估计方程，得到卡尔曼增益方程；定义误差协方差矩阵和先验误差协方差矩阵，对所述卡尔曼增益方程进行求解，得到使误差协方差矩阵的迹最小的卡尔曼增益，并得到卡尔曼增益方程的待定系数矩阵；

重构模块1008，用于根据后验估计方程、卡尔曼增益以及待定系数矩阵，得到翼伞系统状态的状态重构结果。

关于翼伞系统航迹重构的状态估计装置的具体限定可以参见上文中对于翼伞系统航迹重构的状态估计方法的限定，在此不再赘述。上述装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图11所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种翼伞系统航迹重构的状态估计方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图11中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述实施例中方法的步骤。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中方法的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。