一种基于集员滤波的无人艇目标轨迹预测方法

1.本发明涉及无人艇控制技术领域，特别是一种基于集员滤波的无人艇目标轨迹预测方法。


背景技术：

2.随着大量移动设备的产生，目标轨迹预测被广泛应用于智能交通、航空航天等领域中。目标轨迹预测问题通常被分为数据驱动和行为驱动两种方法，不同移动目标的应用环境对轨迹预测方法的建模的影响是不同的，地面移动目标的轨迹预测通常可以通过基于行为驱动的移动目标轨迹预测方法建立动力学模型来实现短期的轨迹预测，但动力学模型缺乏可解释性。基于数据驱动的目标预测方法有：基于概率统计的移动目标轨迹预测方法、基于神经网络的移动目标轨迹预测方法等，但是传统的目标预测方法已经无法从大量的历史数据中学习到轨迹的结构特征和移动目标的行为特征，因此基于深度学习的目标预测方法得益于人工智能的发展可以有效学学习到轨迹的结构特征和目标的行为特征，但是同样存在着可解释性地、实施性差等问题。除此之外，目标预测问题还存在预测精度以及目标之间的交互关系以及环境变化对目标的运动轨迹影响较大等问题。


技术实现要素：

3.针对上述缺陷，本发明的目的在于提出一种基于集员滤波的无人艇目标轨迹预测方法。
4.为达此目的，本发明采用以下技术方案：一种基于集员滤波的无人艇目标轨迹预测方法，包括以下步骤：
5.步骤s1：获取目标的历史轨迹，通过目标的当前位置以及上一时刻的位置获取目标当前的轨迹运动特征；
6.步骤s2：构建无向图结构，将环境中造成影响的个体作为所述无向图结构的顶点的集合，并根据个体之间的距离对邻接矩阵进行改进；
7.对改进后的邻接矩阵输入至两层卷积网络进行叠加；
8.将轨迹运动特征作为卷积网络的输入特征，得到轨迹交互特征；
9.步骤s3：将所述轨迹交互特征、轨迹运动特征与噪声进行叠加，得出目标在当前的轨迹解码特征；
10.使用解码器将当前的轨迹解码特征解码得到下一时刻的轨迹解码特征；
11.通过所述下一时刻的轨迹解码特征计算获取目标在下一时刻的位置；
12.步骤s4：获取无人艇的当前位置，并将所述目标的下一时刻的位置作为所述无人艇下一时刻的预测位置并使用集员滤波对无人艇的预测位置进行修正，构建出无人艇的运动轨迹，驱使无人艇朝向目标移动；
13.步骤s5：重复步骤s1～s4，直到无人艇到达目标处。
14.优选的，所述步骤s1的具体过程如下：
15.步骤s11：通过目标的当前位置以及上一时刻的位置获取得到目标的相对运动位置；
16.步骤s12：根据所述相对运动位置以及权重函数获取得到运动特征向量，其中所述运动特征向量的获取公式如下：
17.si(t)＝φ(δxi(t),w1)，其中φ为相对位置编码函数，δxi(t)为目标的相对运动位置，δxi(t)＝(xi(t)-xi(t-1)),(yi(t)-yi(t-1))，其中(xi(t)，yi(t))为当前目标的位置，(xi(t-1)，yi(t-1))为目标上一时刻的位置，t表示时间，w1为权重；
18.步骤s13：将所述运动特征向量输入第一编码器中，获取得到所述轨迹运动特征，其中所述轨迹运动特征的获取公式如下：
19.f
i1
(t)＝f
lstm-1
(f
i1
(t-1),si(t),w2)，其中f
lstm-1
为第一编码器，f
i1
(t-1)为第一编码器在t-1时刻的隐藏状态，w2表示第一编码器的编码权重，si(t)表示目标的运动特征向量。
20.优选的，所述步骤s2中所述无向图结构如下所示：
21.g
t
＝(v
t
,e
t
)，其中v
it
∈v
t
，v
it
表示环境中的顶点，i∈{1,2....n}，表示环境中的顶点，i∈{1,2....n}，表示第i个顶点与第j个顶点的之间的交互关系，j∈{1,2....n}，当时，代表第i个顶点与第j个顶点之间存在交互关系，若时，代表第i个顶点与第j个顶点之间不存在交互关系；
22.所述步骤s2具体步骤如下：
23.步骤s21：根据各个顶点之间的位置关系重新分配顶点之间的权重；
24.其中顶点之间位置权重关系分配公式如下：
25.其中xi(t)表是第i个顶点在t时刻的位置，xj(t)是第j个顶点在t时刻的位置；
26.步骤s22：根据所述无向图结构中各个重新分配权重后的顶点之间不同距离的交互关系改进邻接矩阵；
27.其中所述邻接矩阵如下表示：其中表示重新分配权重后的邻接矩阵，表示规范化对称邻接矩阵，a表示未重新分配权重的邻接矩阵，k为中心顶点额外权重系数，i为单位矩阵，d为的度矩阵；
28.步骤s23：将邻接矩阵输入至两层卷积网络进行叠加，获取得到目标的轨迹的输出特征；
29.其中输出特征的获取公式如下：
30.其中σ为激活函数，分别为第一层卷积网络的权重以及第二层卷积网络的权重，为重新分配权重后的邻接矩阵，f
i1
(t)表示轨迹运动特征；
31.步骤s24：将所述输出特征输入至第二编码器中，获取得到轨迹交互特征，其中所述轨迹交互特征的获取公式如下：
32.其中f
lstm-2
为第二编码器，f
i2
(t-1)为第二编码
器在t-1时刻的隐藏状态，w3表示第二编码器的编码权重，表示目标的轨迹的输出特征。
33.优选的，步骤s3中所述轨迹解码特征的具体获取公式如下：
34.di(t)＝f
i1
(t)||f
i2
(t)||z
i,k
，其中di(t)表示t时刻目标的轨迹解码特征，f
i1
(t)表示轨迹运动特征，f
i2
(t)表示轨迹交互特征，z
i,k
表示有界系统噪声，z
i,k
满足其中是已知正定的对称矩阵；
35.所述下一时刻的轨迹解码特征的获取公式如下：
36.f
i3
(t+1)＝f
lstm-3
(di(t),si(t),wd)，其中f
i3
(t+1)表示下一时刻的轨迹解码特征，f
lstm-3
为解码器，di(t)为第t时刻的目标的轨迹解码特征，wd表示解码器的编码权重，si(t)表示目标的运动特征向量；
37.所述目标在下一时刻的位置的获取公式如下：
38.其中δ为线性层。
39.优选的，所述步骤s4中获取无人艇的当前位置包括以下步骤：
40.步骤s41：构建无人艇在水面上匀速行驶的模型；
41.步骤s42：通过融合先验数据以及测量数据对模型进行测量，获取无人艇的测量值；
42.步骤s43：基于集员滤波对无人艇下一时刻的预测位置进行修正。
43.优选的，所述步骤s41中所述模型如下表示：
44.x(k+1)＝a(k)x(k)+ω(k)，其中x(k+1)为k+1时刻的模型，a(k)是k时刻的时变的系数矩阵，ω(k)表示k时刻的未知但有界的系统噪声，其中ω(k)满足ω
t
(k)s
k-1
ω(k)≤1，表示噪声在未知但是有界的椭球域内，其中sk椭球域的范围大小。
45.优选的，所述步骤s42中对模型进行测量的公式如下：
46.z(k)＝c(k)x(k)+v(k)；其中c(k)为k时刻的时变的系数矩阵，v(k)代表未知但有界的测量噪声，其中v(k)满足v
t
(k)r
k-1
v(k)≤1，表示噪声在未知但是有界的椭球域内，其中rk椭球域的范围大小。
47.优选的，所述步骤s43包括如下步骤：
48.根据所述模型以及基于图卷积神经网络建立目标轨迹的预测位置建立集员滤波器，使用集员滤波器获取模型下一时刻的预测位置，模型下一时刻的预测位置的表达式如下：
49.a(k)是k时刻的时变的系数矩阵，为k时刻的模型的预测位置，z(k)为无人艇的测量值，c(k)为k时刻的时变的系数矩阵，kk为集员滤波器参数；
50.获取集员滤波器误差，给定在k时刻集员滤波器误差中的椭圆限定矩阵pk，根据数学归纳法调整k+1时刻的椭圆限定矩阵p
k+1
，保持使得模型在每一个状态下都在预估的水面区域内；
51.其中获取集员滤波器误差的公式如下：
52.在初始时刻时，滤波器误差与椭圆限定矩阵满足如下关系，e0为初始时刻的集员滤波器误差。
53.优选的，当时，通过迭代算法，优化椭圆限定矩阵p
k+1
的迹，以求得集员滤波器参数kk。
54.上述技术方案中的一个技术方案具有如下优点或有益效果：在本发明中首先通过无线传感器节点的检测目标来记录下目标的运动轨迹，获取到目标的当前位置，通过目标的当前位置来预测目标的下一时刻的位置，以目标下一时刻的位置来作为无人艇下一时刻的位置，来驱使无人艇朝向所述目标移动靠近，在此过程中，时间是连续的，目标的位置在不断变化，故需要不停的获取目标的下一时刻的位置来对无人艇的下一时刻位置进行更新，确保无人艇能够精确的到达目标处，在此过程中，本发明还会对无人艇的位置进行修正，确保无人艇下一时刻的位置的范围准确性。保证无人艇能够准确的趋向移动至目标处。
附图说明
55.图1是本发明的一个实施例的流程图。
具体实施方式
56.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
57.在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
58.此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
59.在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
60.如图1所示，一种基于集员滤波的无人艇目标轨迹预测方法包括以下步骤：
61.步骤s1：获取目标的历史轨迹，通过目标的当前位置以及上一时刻的位置获取目标当前的轨迹运动特征；
62.步骤s2：构建无向图结构，将环境中造成影响的个体作为所述无向图结构的顶点的集合，并根据个体之间的距离对邻接矩阵进行改进；
63.对改进后的邻接矩阵输入至两层卷积网络进行叠加；
64.将轨迹运动特征作为卷积网络的输入特征，得到轨迹交互特征；
65.步骤s3：将所述轨迹交互特征、轨迹运动特征与噪声进行叠加，得出目标在当前的轨迹解码特征；
66.使用解码器将当前的轨迹解码特征解码得到下一时刻的轨迹解码特征；
67.通过所述下一时刻的轨迹解码特征计算获取目标在下一时刻的位置；
68.步骤s4：获取无人艇的当前位置，并将所述目标的下一时刻位置作为所述无人艇下一时刻的预测位置并使用集员滤波对无人艇的预测位置进行修正，构建出无人艇的运动轨迹，驱使无人艇朝向目标移动；
69.步骤s5：重复步骤s1～s4，直到无人艇到达目标处。
70.在本发明中首先通过无线传感器节点的检测目标来记录下目标的运动轨迹，获取到目标的当前位置，通过目标的当前位置来预测目标的下一时刻的位置，以目标下一时刻的位置来作为无人艇下一时刻的位置，来驱使无人艇朝向所述目标移动靠近，在此过程中，时间是连续的，目标的位置在不断变化，故需要不停的获取目标的下一时刻的位置来对无人艇的下一时刻位置进行更新，确保无人艇能够精确的到达目标处，在此过程中，本发明还会对无人艇的位置进行修正，确保无人艇下一时刻的位置的范围准确性。保证无人艇能够准确的趋向移动至目标处。
71.在发明的步骤s4中构建出无人艇的运动轨迹，可采用现有的技术来实现，当知道了无人艇的当前位置以及需要移动的位置(目标的下一时刻的位置)后，即可直接策划出直线的路径，然后通过如使用如公开号为cn110906934b的避障技术，对路径上障碍进行规避，以保证无人艇能够顺利到达目标处，对于路径的策划不属于本发明所保护的内容，故不作过多的介绍。
72.优选的，所述步骤s1的具体过程如下：
73.步骤s11：通过目标的当前位置以及上一时刻的位置获取得到目标的相对运动位置；
74.在本发明中相对运动位置为上一时刻的位置与当前时刻的位置之差。
75.步骤s12：根据所述相对运动位置以及权重函数获取得到运动特征向量，其中所述运动特征向量的获取公式如下：
76.si(t)＝φ(δxi(t),w1)，其中φ为相对位置编码函数，δxi(t)为目标的相对运动位置，δxi(t)＝(xi(t)-xi(t-1)),(yi(t)-yi(t-1))，其中(xi(t)，yi(t))为当前目标的位置，(xi(t-1)，yi(t-1))为目标上一时刻的位置，t表示时间，w1为权重；
77.步骤s13：将所述运动特征向量输入第一编码器中，获取得到所述轨迹运动特征，其中所述轨迹运动特征的获取公式如下：
78.f
i1
(t)＝f
lstm-1
(f
i1
(t-1),si(t),w2)，其中f
lstm-1
为第一编码器，f
i1
(t-1)为第一编码器在t-1时刻的隐藏状态，w2表示第一编码器的编码权重，si(t)表示目标的运动特征向量。
79.优选的，所述步骤s2中所述无向图结构如下所示：
80.g
t
＝(v
t
,e
t
)，其中v
it
∈v
t
，v
it
表示环境中的顶点，i∈{1,2....n}，表示环境中的顶点，i∈{1,2....n}，表示第i个顶点与第j个顶点的之间的交互关系，j∈{1,2....n}，当时，代表第i个顶点与第j个顶点之间存在交互关系，若时，代表第i个顶点与第j个顶点之间不存在交互关系；
81.所述步骤s2具体步骤如下：
82.步骤s21：根据各个顶点之间的位置关系重新分配顶点之间的权重；
83.其中顶点之间位置权重关系分配公式如下：
84.其中xi(t)表是第i个顶点在t时刻的位置，xj(t)是第j个顶点在t时刻的位置；
85.步骤s22：根据所述无向图结构中各个重新分配权重后的顶点之间不同距离的交互关系改进邻接矩阵；
86.其中所述邻接矩阵如下表示：其中表示重新分配权重后的邻接矩阵，表示规范化对称邻接矩阵，a表示未重新分配权重的邻接矩阵，k为中心顶点额外权重系数，i为单位矩阵，d为的度矩阵；
87.步骤s23：将邻接矩阵输入至两层卷积网络进行叠加，获取得到目标的轨迹的输出特征；
88.其中输出特征的获取公式如下：
89.其中σ为激活函数，分别为第一层卷积网络的权重以及第二层卷积网络的权重，为重新分配权重后的邻接矩阵，f
i1
(t)表示轨迹运动特征；
90.步骤s24：将所述输出特征输入至第二编码器中，获取得到轨迹交互特征，其中所述轨迹交互特征的获取公式如下：
91.其中f
lstm-2
为第二编码器，f
i2
(t-1)为第二编码器在t-1时刻的隐藏状态，w3表示第二编码器的编码权重，表示目标的轨迹的输出特征。
92.在目标下一时刻的位置轨迹预测任务中各个个体与其他个体之间都存在着相互影响与制约的关系，例如：当目标受到环境(风，水流速度等)作用时，目标的移动将会与其他个体产生相互作用，导致了目标的位置发生偏移。为此本发明中将环境中能够对对无人艇造成影响的客体以及目标作为无向图结构中的顶点，分析出顶点之间的交互关系，从而对目标的位置进行修正，以确保目标位置的准确性。
93.优选的，步骤s3中所述轨迹解码特征的具体获取公式如下：
94.di(t)＝f
i1
(t)||f
i2
(t)||z
i,k
，其中di(t)表示t时刻目标的轨迹解码特征，f
i1
(t)表示轨迹运动特征，f
i2
(t)表示轨迹交互特征，z
i,k
表示有界系统噪声，z
i,k
满足其中是已知正定的对称矩阵；
95.所述下一时刻的轨迹解码特征的获取公式如下：
96.f
i3
(t+1)＝f
lstm-3
(di(t),si(t),wd)，其中f
i3
(t+1)表示下一时刻的轨迹解码特征，f
lstm-3
为解码器，di(t)为第t时刻的目标的轨迹解码特征，wd表示解码器的编码权重，si(t)表示目标的运动特征向量；
97.所述目标在下一时刻的位置的获取公式如下：
98.其中δ为线性层。
99.在步骤s3中通过设置区域噪声范围z
i,k
去模拟实际环境中的不确定噪声，将轨迹运动特征、轨迹交互特征与噪声进行叠加，叠加后得到的轨迹解码特征，所述轨迹解码特征
是叠加了预设的噪声，使得所述轨迹解码特征更加趋近于正式环境，确保了目标的下一时刻的位置的准确性。
100.本发明将基于图卷积神经网络的预测方法应用到无人艇的目标预测过程中，在此过程中考虑到“交互”关系的影响，充分考虑了水面波动而引起目标之间的影响，同时加入了区域有界噪声，提高了获取目标的下一时刻的位置方法的鲁棒性和容错率。
101.优选的，所述步骤s4中获取无人艇的当前位置包括以下步骤：
102.步骤s41：构建无人艇在水面上匀速行驶的模型；
103.步骤s42：通过融合先验数据以及测量数据对模型进行测量，获取无人艇的测量值；
104.步骤s43：基于集员滤波对无人艇下一时刻的预测位置进行修正。
105.建立无人艇的模型以及将目标的下一时刻的位置作为集员滤波对无人艇下一时刻的位置的估计值，并设计相对应的集员滤波器以修正预测位置，这种方法克服了基于深度学习的目标预测方法中实施性低的缺点，不仅对运动轨迹进行了预测，与此同时基于集员滤波器进行了修正，使得目标的下一时刻位置位置在一个固定的椭圆范围内，而不是仅仅针对一个固定点，充分考虑了目标随环境因素(例如：风、水流速度)而改变的移动性，从而使得更加符合实际情况，应用性更加广泛。
106.优选的，所述步骤s41中所述模型如下表示：
107.x(k+1)＝a(k)x(k)+ω(k)，其中x(k+1)为k+1时刻的模型，a(k)是k时刻的时变的系数矩阵，ω(k)表示k时刻的未知但有界的系统噪声，其中ω(k)满足ω
t
(k)s
k-1
ω(k)≤1，表示噪声在未知但是有界的椭球域内，其中sk椭球域的范围大小。
108.优选的，所述步骤s42中对模型进行测量的公式如下：
109.z(k)＝c(k)x(k)+v(k)；其中c(k)为k时刻的时变的系数矩阵即先验数据，v(k)代表未知但有界的测量噪声即测量数据，其中v(k)满足v
t
(k)r
k-1
v(k)≤1，表示噪声在未知但是有界的椭球域内，其中rk椭球域的范围大小。
110.优选的，所述步骤s43包括如下步骤：
111.根据所述模型以及基于图卷积神经网络建立目标轨迹的预测位置建立集员滤波器，使用集员滤波器获取模型下一时刻的预测位置，模型下一时刻的预测位置的表达式如下：
112.a(k)是k时刻的时变的系数矩阵，为k时刻的模型的预测位置，z(k)为无人艇的测量值，c(k)为k时刻的时变的系数矩阵，kk为集员滤波器参数；
113.在本发明中的取值为将目标的下一时刻的位置作为无人艇在k时刻的位置可以对无人艇在下一时刻的位置进行修正，使得获取到的位置更加准确。
114.获取集员滤波器误差，给定在k时刻集员滤波器误差中的椭圆限定矩阵pk，根据数学归纳法调整k+1时刻的椭圆限定矩阵p
k+1
，保持使得模型在每一个状态下都在预估的水面区域内；
115.其中获取集员滤波器误差的公式如下：
116.在初始时刻时，滤波器误差与椭圆限定矩阵满足如下关系，e0为初始时刻的集员滤波器误差。
117.优选的，当时，通过迭代算法，优化椭圆限定矩阵p
k+1
的迹，以求得集员滤波器参数kk。
118.优化椭圆限定矩阵p
k+1
，再通过迭代算法以求得集员滤波器参数kk，根据集员滤波器设计的目标预测位置因为有修正而更加准确，同时确保了无人艇下一时刻的目标轨迹区域。其中迭代算法可采用现有算法公式，如迭代高斯-塞德尔迭代算法、共轭迭代法、变尺度迭代法等等，在此不做过多的解释说明。
119.在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
120.尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。