一种载体航迹角计算方法及系统与流程

1.本发明属于航迹角算法技术领域，具体涉及一种载体航迹角计算方法及系统。


背景技术：

2.航迹角是水面舰船、水下航行器等水中载体运动控制的重要参数，但在载体实际航行过程中，由于受到洋流作用的影响，载体实际航行轨迹所形成航迹角并不等于航向角，此外载体受到波浪、风力因素的影响，姿态、速度和位置等控制参数均会出现周期性的往复运动，这些运动虽然是真实存在的，但对于控制系统来说相当于高频扰动，完全响应会降低载体航程，且由于干扰的随机性，也不利于载体的控制稳定。因此，滤除这些干扰，从惯导系统运动轨迹中准确的提取载体实际航迹曲线，对提高水中载体作业效能的至关重要。
3.emd(empirical mode decomposition，经验模态分解)是一种新的处理非平稳信号的方法，无须预先设定任何基函数，相比于其他的时频分析方法更好地反映了信号的物理意义，既适用于线性、平稳信号的分析，也适用于非线性、非平稳信号的分析。
4.对载体包含高频扰动、噪声的运动轨迹进行emd分解，能够提取出载体实际的受控运动轨迹趋势分量，从中计算出的航迹角能够真实反应载体运动方向，将其作为控制参数，相比于原始的航向角或未经提取的原始轨迹航迹角，能够抑制高频扰动对载体的干扰，显著减小频繁施控带来额外能量消耗，提高系统控制品质。


技术实现要素：

5.针对载体实际航行轨迹中包含高频扰动、噪声等技术问题，本发明提供了一种载体航迹角计算方法及系统，对载体运动轨迹进行emd分解，提取出载体实际的受控运动轨迹趋势分量，计算出的航迹角能够真实反应载体运动方向。
6.为了解决上述技术问题，本发明提供了一种载体航迹角计算方法，包括如下步骤
7.统计载体航行轨迹方程，转换为空间正交的参数方程，获得以时间为参变量的不同正交方向的轨迹；
8.对每个方向的轨迹设置时间窗，采用镜像扩展方法预测未来一定时间内的轨迹数据；
9.在时间窗和镜像扩展区间内，对每个方向的轨迹分别进行emd分解，获得平均趋势分量；
10.根据平均趋势分量计算载体航迹角。
11.进一步地，所述镜像扩展方法为以当前时刻的位置为中心进行中心镜像扩展；所述镜像扩展区间小于时间窗。
12.进一步地，每个方向的轨迹设置的时间窗相同。
13.进一步地，所述emd分解包括如下步骤：
14.计算轨迹的局部极大值和局部极小值；
15.根据轨迹局部极大值和局部极小值分别计算上、下包络曲线；
16.计算轨迹上、下包络曲线均值；
17.计算轨迹固有模态函数；
18.判断固有模态函数是否满足固有模态函数条件，若满足，将固有模态函数记作固有模态，计算将轨迹与固有模态的差作为轨迹分量，开始循环迭代计算，否则，将轨迹与固有模态函数的差作为轨迹分量，开始循环迭代计算，直至满足固有模态函数条件；
19.循环迭代计算直至获得轨迹的平均趋势分量。
20.进一步地，所述的计算轨迹的局部极大值和极小值方法具体包括如下步骤
21.s101、计算xi(t)各个时刻的差分输出δxi(tj)＝xi(t
j+1
)-xi(tj)，xi(t)表示轨迹t时刻第i个分量值，tj和t
j+1
表示前后相邻两个时刻；
22.s102、若δxi(tj)＝0或者sign(δxi(tj))≠sign(δxi(t
j+1
))，则xi(tj)为极值点，sign()为符号函数；
23.s103、极大值点与极小值点相互交替出现，区分所有局部极大值和极小值。进一步地，所述固有模态函数为
24.hi(t)＝xi(t)-mi(t)
25.其中，xi(t)表示轨迹t时刻第i个分量值，mi(t)表示上包络曲线和下包络曲线的均值；
26.所述固有模态函数条件包括
27.在计算区间内，固有模态函数局部极大值与局部极小值数目之和必须与过零点的数目相等或至多相差一个；
28.对固有模态函数进行积分，积分值小于门限值。
29.进一步地，所述门限值小于固有模态函数局部极大值的5％。
30.进一步地，所述计算载体航迹角为
[0031][0032]
其中，x(ti)、y(ti)分别为东向轨迹、北向轨迹，ti和t
i-1
表示前后相邻两个时刻，δsn为前后相邻时刻北向位移，δse为前后相邻时刻东向位移。
[0033]
进一步地，所述载体航迹角计算方法用于水面舰船或水下航行器的航迹角提取。
[0034]
本发明还提供了一种载体航迹角计算系统，包括
[0035]
载体轨迹采集模块，用于统计载体航行轨迹方程，转换为空间正交的参数方程，获得以时间为参变量的不同正交方向的轨迹；
[0036]
加窗模块，用于对不同方向的轨迹按照时间窗划分；
[0037]
镜像扩展模块，用于采用镜像扩展方法预测未来一定时间内的轨迹数据；
[0038]
emd模块，用于对轨迹进行emd分解，获得平均趋势分量；
[0039]
航迹角输出模块，用于根据平均趋势分量计算载体航迹角。
[0040]
本发明与现有技术相比的有益效果：
[0041]
本发明提供的基于emd分解的载体航迹角计算方法，对载体运动轨迹进行emd分解，提取出载体实际的受控运动轨迹趋势分量，能够抑制高频扰动对载体的干扰，显著减小频繁施控带来额外能量消耗，提高系统控制品质；同时通过镜像扩展预测航迹数据，减小边缘数据不稳定现象，保证计算的实时性，获得实时结果。
[0042]
本发明提供的载体航迹角提取方法，主要用于消除水面舰船、水下航行器等水中载体在航行时，外界干扰带来航迹角高频波动，为载体控制系统提供更加符合要求的控制输入参数。
附图说明
[0043]
所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0044]
图1为本发明具体实施例提供的镜像扩展示意图；
[0045]
图2为本发明具体实施例提供的符合要求的固有模态函数曲线示意图；
[0046]
图3为本发明具体实施例提供的经过emd分解平滑后的航迹曲线。
具体实施方式
[0047]
下面结合附图和实施例对本发明进行详细阐述。
[0048]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合实施例中的附图，对本发明实施例中的技术方案进行清楚完整地描述。显然，描述的实施例是本发明的一部分实施例，而不是全部。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动而获得的所有其他实施例，都属于本发明保护的范围。
[0049]
本发明提供的一种基于emd分解的载体航迹角计算方法，包括如下步骤
[0050]
统计载体航行轨迹方程，转换为空间正交的参数方程，获得以时间为参变量的不同正交方向的轨迹；
[0051]
对每个方向的轨迹设置时间窗，采用镜像扩展方法预测未来一定时间内的轨迹数据；
[0052]
在时间窗和镜像扩展区间内，对每个方向的轨迹分别进行emd分解，获得平均趋势分量；
[0053]
根据平均趋势分量计算载体航迹角。
[0054]
本发明载体航迹角计算方法，通过对载体运动轨迹进行emd分解，提取出载体实际的受控运动轨迹趋势分量，能够抑制高频扰动对载体的干扰，显著减小频繁施控带来额外能量消耗，提高系统控制品质；同时通过镜像扩展预测航迹数据，减小边缘数据不稳定现象，保证计算的实时性，获得实时结果。
[0055]
优选的，每个方向轨迹设置的时间窗一致，便于简化计算。
[0056]
优选的，所述镜像扩展方法为以当前时刻的位置为中心进行中心镜像扩展，对未来一段时间内的轨迹进行预测。优选的，由于短时间内目标航向不会发生剧烈变化，设定镜像扩展区间小于时间窗，该配置可以保证计算的准确性。
[0057]
进一步地，所述emd分解包括如下步骤：
[0058]
计算轨迹的局部极大值和极小值；
[0059]
根据轨迹局部极大值和极小值计算上、下包络曲线；
[0060]
计算轨迹上、下包络曲线均值；
[0061]
计算轨迹固有模态函数；
[0062]
判断固有模态函数是否满足固有模态函数条件，若满足，将固有模态函数记作固有模态，计算将轨迹与固有模态的差作为轨迹分量，开始循环迭代计算，否则，将轨迹与固有模态函数的差作为轨迹分量，开始循环迭代计算，直至满足固有模态函数条件；
[0063]
循环迭代计算直至获得轨迹的平均趋势分量。
[0064]
本发明提供的基于emd分解的载体航迹角计算方法，适用于水面舰船、水下航行器等水中载体的航迹角提取。
[0065]
本发明还提供的一种基于emd分解的载体航迹角计算系统，包括
[0066]
载体轨迹采集模块，用于统计载体航行轨迹方程，转换为空间正交的参数方程，获得以时间为参变量的不同正交方向的轨迹；
[0067]
加窗模块，用于对不同方向的轨迹按照时间窗划分；
[0068]
镜像扩展模块，用于采用镜像扩展方法预测未来一定时间内的轨迹数据；
[0069]
emd模块，用于对轨迹进行emd分解，获得平均趋势分量；
[0070]
航迹角输出模块，用于根据平均趋势分量计算载体航迹角。
[0071]
下面以载体水面航行为例，对本发明的技术方案进行详细描述。
[0072]
s1、载体在水面航行时，轨迹可以描述为二维平面方程d(x,y,t)＝0，其中x代表东向位移，y代表北向位移，t为时间。
[0073]
s2、轨迹d(x,y,t)＝0可以表示为空间正交的参数方程，获得以时间为参变量的不同正交方向轨迹x(t)和y(t)。
[0074]
s3、分别对x(t)和y(t)进行emd分解，其中t∈[0,tk]，以x(t)为例：
[0075]
s31、对数据x(t)加窗(即设置时间窗)，确定时间段[t
k-t
,tk]，tk是当前时刻，t为时间窗宽度，t
t-t
为一个时间窗之前的时刻；
[0076]
s32、令xi(t)＝x(t),i＝0，开始迭代计算。
[0077]
i表示第i次迭代，xi(t)表示轨迹t时刻第i个分量值。
[0078]
s33、实时计算时，考虑到水中载体在绝大部分时间内，目标航向不会发生剧烈变化，因此，可以通过镜像扩展的方式对未来时刻的数据进行预测，以减小边缘数据不稳定现象。如图1所示，通过中心镜像扩展的方式对未来时刻的数据进行预测。
[0079]
具体的，以tk时刻x(t)为中心进行中心镜像扩展，计算方法如下
[0080]
x(t
k+n
)＝2x(tk)-x(t
k-n
),n≤m
ꢀꢀꢀ
(1)
[0081]
其中[t
k-m
,t
k+m
]为镜像区域。镜像扩展区间m＜t，n表示采集点。
[0082]
s34、在时间窗和镜像扩展区间[t
k-t
,t
k+m
]内，计算xi(t)迭代过程中的各局部极大值和极小值，具体过程如下
[0083]
s101、计算xi(t)各个时刻的差分输出δxi(tj)＝xi(t
j+1
)-xi(tj)，其中tj∈[t
k-t
,t
k+m
]，xi(t)表示轨迹t时刻第i个分量值，tj和t
j+1
表示前后相邻两个时刻；
[0084]
s102、若δxi(tj)＝0或者sign(δxi(tj))≠sign(δxi(t
j+1
))，则xi(tj)为极值点，sign()为符号函数；
[0085]
s103、极大值点与极小值点相互交替出现，因此首先判断第一个极值点的类型，如果为极大值，则奇数位的极值点为极大值，偶数位的极值为极小值；反之同理，奇数位的极值为极小值，偶数位的极值为极大值。
[0086]
s34、计算包络，采用分段三次曲线拟合方法得到上包络曲线、下包络曲线；
[0087]
s35、计算上包络曲线e
ui
(t)和下包络曲线e
li
(t)的均值mi(t)＝(e
ui
(t)+e
li
(t))/2
[0088]
s36、计算固有模态函数hi(t)＝xi(t)-mi(t)
[0089]
s37、判断hi(t)是否满足固有模态函数条件，即：
[0090]
(1)在[t
k-t
,t
k+m
]范围内，hi(t)局部极大值与局部极小值数目之和必须与过零点的数目相等或至多相差一个；
[0091]
(2)对hi(t)进行积分，判断积分值小于一定的门限值作为判断条件，优选的，设定门限值小于固有模态函数局部极大值的5％，符合要求的固有模态函数曲线如图2所示。
[0092]
s38、如果hi(t)满足固有模态函数条件，则hi(t)称为第i阶固有模态，令x
i+1
(t)＝xi(t)-hi(t)，从步骤s34开始循环迭代计算；如hi(t)不满足固有模态函数条件，则执行步骤s39。
[0093]
s39、令x
ij
(t)＝xi(t)-hi(t)，重复执行步骤s34到s38，直到h
ij
(t)＝x
ij
(t)-m
ij
(t)满足固有模态函数条件，令x
i+1
(t)＝xi(t)-h
ij
(t)，从步骤s34开始循环迭代计算；
[0094]
s40、经过p次循环后，x(t)可以表示为如下形式
[0095][0096]
其中，r(t)称为平均趋势分量，当r(t)表现为常数或单调函数时，迭代计算结束，p为迭代次数。
[0097]
r(t)代表了x(t)中的非振荡部分分量，当x(t)为载体运动轨迹曲线时，r(t)代表消除了高频干扰后的平稳轨迹曲线。一般去除一至二阶imf(固有模态函数)分量后，即可获得较为理想的平稳轨迹曲线，如图3所示。
[0098]
4、计算r(t)的航迹角
[0099][0100]
其中，ti和t
i-1
表示前后相邻两个时刻，δsn为前后相邻时刻北向位移，δse为前后相邻时刻东向位移。
[0101]
航迹角对比如图3所示，可以看出，经过emd分解后的得到的航迹角波动显著减小。
[0102]
本发明设计了一种载体航迹角提取方法，对载体运动轨迹进行emd分解，提取出载体实际的受控运动轨迹趋势分量，能够抑制高频扰动对载体的干扰，显著减小频繁施控带来额外能量消耗，提高系统控制品质；同时通过镜像扩展预测航迹数据，减小边缘数据不稳定现象，保证计算的实时性，获得实时结果。
[0103]
如上针对一种实施例描述和/或示出的特征可以以相同或类似的方式在一个或更多个其它实施例中使用，和/或与其它实施例中的特征相结合或替代其它实施例中的特征使用。
[0104]
应该强调，术语“包括/包含”在本文使用时指特征、整件、步骤或组件的存在，但并不排除一个或更多个其它特征、整件、步骤、组件或其组合的存在或附加。
[0105]
这些实施例的许多特征和优点根据该详细描述是清楚的，因此所附权利要求旨在覆盖这些实施例的落入其真实精神和范围内的所有这些特征和优点。此外，由于本领域的
技术人员容易想到很多修改和改变，因此不是要将本发明的实施例限于所例示和描述的精确结构和操作，而是可以涵盖落入其范围内的所有合适修改和等同物。
[0106]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
[0107]
本发明未详细说明部分为本领域技术人员公知技术。

技术特征：
1.一种载体航迹角计算方法，其特征在于，包括如下步骤统计载体航行轨迹方程，转换为空间正交的参数方程，获得以时间为参变量的不同正交方向的轨迹；对每个方向的轨迹设置时间窗，采用镜像扩展方法预测未来一定时间内的轨迹数据；在时间窗和镜像扩展区间内，对每个方向的轨迹分别进行emd分解，获得平均趋势分量；根据平均趋势分量计算载体航迹角。2.根据权利要求1所述的载体航迹角计算方法，其特征在于，所述镜像扩展方法为以当前时刻的位置为中心进行中心镜像扩展；所述镜像扩展区间小于时间窗。3.根据权利要求1所述的载体航迹角计算方法，其特征在于，每个方向的轨迹设置的时间窗相同。4.根据权利要求1所述的载体航迹角计算方法，其特征在于，所述emd分解包括如下步骤：计算轨迹的局部极大值和局部极小值；根据轨迹局部极大值和局部极小值分别计算上、下包络曲线；计算轨迹上、下包络曲线均值；计算轨迹固有模态函数；判断固有模态函数是否满足固有模态函数条件，若满足，将固有模态函数记作固有模态，计算将轨迹与固有模态的差作为轨迹分量，开始循环迭代计算，否则，将轨迹与固有模态函数的差作为轨迹分量，开始循环迭代计算，直至满足固有模态函数条件；循环迭代计算直至获得轨迹的平均趋势分量。5.根据权利要求4所述的载体航迹角计算方法，其特征在于，所述的计算轨迹的局部极大值和极小值方法具体包括如下步骤s101、计算x
i
(t)各个时刻的差分输出δx
i
(t
j
)＝x
i
(t
j+1
)-x
i
(t
j
)，x
i
(t)表示轨迹t时刻第i个分量值，t
j
和t
j+1
表示前后相邻两个时刻；s102、若δx
i
(t
j
)＝0或者sign(δx
i
(t
j
))≠sign(δx
i
(t
j+1
))，则x
i
(t
j
)为极值点，sign()为符号函数；s103、极大值点与极小值点相互交替出现，区分所有局部极大值和极小值。6.根据权利要求4所述的载体航迹角计算方法，其特征在于，所述固有模态函数为h
i
(t)＝x
i
(t)-m
i
(t)其中，x
i
(t)表示轨迹t时刻第i个分量值，m
i
(t)表示上包络曲线和下包络曲线的均值；所述固有模态函数条件包括在计算区间内，固有模态函数局部极大值与局部极小值数目之和必须与过零点的数目相等或至多相差一个；对固有模态函数进行积分，积分值小于门限值。7.根据权利要求6所述的载体航迹角计算方法，其特征在于，所述门限值小于固有模态函数局部极大值的5％。8.根据权利要求6所述的载体航迹角计算方法，其特征在于，所述计算载体航迹角为
其中，x(t
i
)、y(t
i
)分别为东向轨迹、北向轨迹，t
i
和t
i-1
表示前后相邻两个时刻，δs
n
为前后相邻时刻北向位移，δs
e
为前后相邻时刻东向位移。9.根据权利要求1～9中任一项所述的载体航迹角计算方法，其特征在于，所述载体航迹角计算方法用于水面舰船或水下航行器的航迹角提取。10.一种载体航迹角计算系统，其特征在于，包括载体轨迹采集模块，用于统计载体航行轨迹方程，转换为空间正交的参数方程，获得以时间为参变量的不同正交方向的轨迹；加窗模块，用于对不同方向的轨迹按照时间窗划分；镜像扩展模块，用于采用镜像扩展方法预测未来一定时间内的轨迹数据；emd模块，用于对轨迹进行emd分解，获得平均趋势分量；航迹角输出模块，用于根据平均趋势分量计算载体航迹角。

技术总结
本发明提供了一种载体航迹角计算方法及系统，包括统计载体航行轨迹方程，转换为空间正交的参数方程，获得以时间为参变量的不同正交方向的轨迹；对每个方向的轨迹设置时间窗，采用镜像扩展方法预测未来一定时间内的轨迹数据；在时间窗和镜像扩展区间内，对每个方向的轨迹分别进行EMD分解，获得平均趋势分量；根据平均趋势分量计算载体航迹角。本发明对载体运动轨迹进行EMD分解，能够抑制高频扰动对载体的干扰，显著减小频繁施控带来额外能量消耗，提高系统控制品质。提高系统控制品质。提高系统控制品质。


技术研发人员：周祖洋 庄广琛 郭玉胜 宋雅兰 邓继权
受保护的技术使用者：北京自动化控制设备研究所
技术研发日：2023.06.19
技术公布日：2023/10/11