一种基于多站的轨迹被动跟踪方法与流程

1.本发明属于水下目标跟踪技术领域，具体涉及一种基于多站的轨迹被动跟踪方法。


背景技术：

2.无论水下定位系统或是水下无人平台，对目标的“探测”均是首要任务之一。按照己方声纳是否发射声波信号，探测可以分为主动探测与被动探测。主动探测可以获取较多的目标信息，一般有回波方位与回波时延，可直接计算得到目标位置，不足之处在于主动发射声波容易造成自身位置的暴露，这对潜艇、水下无人平台等依靠隐蔽性增加威慑力的装备而言是致命的。被动探测不需要己方发射声波，依靠目标发动机、电机、螺旋桨等产生噪声作为声源进行探测，有利于保持自身隐蔽性，一般可以获取目标的方位信息。但被动探测时一般无法直接估计目标位置，另外，受到复杂的水声信道及海洋环境影响，以目标辐射噪声作为声源的被动探测往往面临着高虚警情况，量测方位中存在较多野值，且探测只关注于目标方位的获取，在多目标时，前后时刻的量测值批次关系无法确知，需要人为地对探测结果进行分析和判断，大大降低无人系统和无人平台的实时性。多基阵量测时，即使人为参与也难以对多基阵量测的目标批次进行融合，从而无法将多基阵所量测的方位转化为目标位置。


技术实现要素：

3.本发明针对上述缺陷，提供一种可以通过多站点的声基阵获取目标不同观测角度，并实时跟踪目标运动轨迹的基于多站的轨迹跟踪算法。
4.本发明提供如下技术方案：一种基于多站的轨迹被动跟踪方法，包括以下步骤：
5.s1：测量所述目标o与第k个站点的声基阵之间连线与测量所述目标与第1个站点的声基阵之间连线的夹角θ1、所述目标o与第k个站点的声基阵之间连线与测量所述目标与第2个站点的声基阵之间连线的夹角θ2，所述k＝3,4,
……
,m
‑
1,m；
6.s2：计算所述目标坐标点(x0，y0)到与第k个站点的声基阵之间的连线的距离平方和f(x0，y0)，并求出所述目标坐标点的估计值(x
′0，y
′0)；
7.s3：用所述目标坐标点的估计值(x
′0，y
′0)相对于第1个站点的声基阵的方位和相对于第2个站点的声基阵的方位作为该站点对所述目标o新的观测方位，再联合其他站点对所述目标o的观测方位原值，分别获得与所述第1个站点的声基阵之间连线的夹角θ
′1和与所述第1个站点的声基阵之间连线的夹角θ
′2；
8.s4：采用所述θ
′1和θ
′2以及所述步骤s2的方法，得到所述目标坐标点的二次估计值(x
″0，y
″0))；
9.s5：采用惯性协同定位跟踪所述目标o的运动轨迹。
10.进一步地，所述步骤2包括以下步骤：
11.s21：计算所述目标坐标点(x0，y0)到与第k个站点的声基阵之间的连线的距离平方
和f(x0，y0)：
[0012][0013]
其中，θ
i
＝90
°
或270
°
(i＝1,2)，a
i
＝1,b
i
＝0,c
i
＝
‑
x
i
；
[0014]
θ
i
＝(i＝1,2)为其他值时，a
i
＝tan(θ
i
),b
i
＝
‑
1,c
i
＝
‑
tan(θ
i
)
·
x
i
；所述x
i
为为所述声基阵在空间内的横轴坐标值；
[0015]
s22：将所述f(x0，y0)分别对x0、y0求偏导，并令其等于0：
[0016][0017]
s23：求出所述目标坐标点(x0，y0)的估计值：
[0018][0019]
进一步地，所述s3步骤包括以下步骤：
[0020]
s31：计算所述目标o的目标坐标点的估计值(x
′0，y
′0)所在空间位置γ0、所述第1站点的声基阵所在空间位置α0和所述第2站点的声基阵所在空间位置β0：
[0021][0022][0023][0024]
其中，所述x
i
为所述声基阵在空间内的横轴坐标值，所述y
i
为所述声基阵在空间内的纵轴坐标值，其中i＝1,2；
[0025]
s32：分别计算所述目标坐标点的估计值(x
′0，y
′0)与所述第i个站点的声基阵之间的欧几里得距离：
[0026][0027]
s33：计算所述目标坐标点的估计值(x
′
0，y
′0)与所述第i个站点的声基阵之间的夹角的余弦值cosθ
′
i
，其中i＝1,2：
[0028][0029][0030]
s34：根据所述s33步骤得到的余弦值cosθ
i
，得到所述与所述第1个站点的声基阵
之间连线的夹角θ
′1和与所述第1个站点的声基阵之间连线的夹角θ
′2[0031][0032][0033]
进一步地，所述s5包括以下步骤：
[0034]
s51：所述目标o向m个站点的声基阵在周期t的时刻n发射位置请求，分别计算所述目标o相对于m个站点的声基阵的位置偏移量，并计算所述目标o相对于自身第n
‑
1时刻的位置偏移量(δx
″0，δy
″0)，其中所述目标o相对于第k个站点的声基阵的位置偏移量为(δx
″
0k
，δy
″
0k
)，进而得到所述目标o相对于第k个站点的距离为d
k
；
[0035]
s52：根据所述s51步骤得到的所述相对于目标o相对于m个站点的声基阵的位置偏移量的加权和计算所述目标o在空间内的位置偏移量(δx0，δy0)；
[0036]
s53：计算所述目标o在空间内的时刻n的位置(x0(n)，y0(n))。
[0037]
进一步地，所述s51步骤中的所述目标o相对于自身第n
‑
1时刻的位置偏移量(δx
″0，δy
″0)的计算公式如下：
[0038][0039][0040]
其中，所述x
″0(n
‑
1)为第n
‑
1时刻所述目标o所在横轴坐标点值，所述x
″0(n
‑
2)为第n
‑
2时刻所述目标o所在横轴坐标点值；所述y
″0(n
‑
1)为第n
‑
1时刻所述目标o所在横轴坐标点值，所述y
″0(n
‑
2)为第n
‑
2时刻所述目标o所在横轴坐标点值；所述t(n
‑
1)为周期t的第n
‑
1时刻，所述t(n
‑
2)为周期t的第n
‑
2时刻。
[0041]
进一步地，所述s52步骤中的所述目标o在空间内的位置偏移量(δx0，δy0)计算公式如下：
[0042]
δx0＝w
o
δx
″0+∑w(d
k
)δx
″
0k
；
[0043]
δy0＝w
o
δy
″0+∑w(d
k
)δy
″
0k
；
[0044]
其中，所述其中w
o
以及w(d
k
)是其所代表的站点与其他站点之间的距离l成反比的权重。
[0045]
进一步地，所述s53步骤中所述目标o在空间内的时刻n的位置(x0(n)，y0(n))的计算公式如下：
[0046]
x0(n)＝[t(n)
‑
t(n
‑
1)]δx0+x
″0(n
‑
1)；
[0047]
y0(n)＝[t(n)
‑
t(n
‑
1)]δy0+y
″0(n
‑
1)。
[0048]
进一步地，所述进而可以得到w
o
可以根据实际情况进行调整，代入到中，就可以得到w(d
k
)，进而得到所述目标o在空间内的时
刻n的位置(x0(n)，y0(n))。
[0049]
本发明的有益效果为：
[0050]
1、本发明针对复杂海洋环境下低信噪比目标定位问题，基于单平台、单基阵水声目标定位技术只能获取自身坐标和水下目标的方位信息，使用在空间上分散布置的多个站点的声基阵能够获取目标不同观测角度，通过多站交叉定位算法可得到探测目标的坐标信息。
[0051]
2、通过与多站点进行实时互动，对目标的坐标信息进行二次定位，对目标位置更进一步估计，用目标第一次估计的位置相对于该站点的方位作为该站点对目标新的观测方位，再联合其他站点对目标的观测方位原值，采用上述方法对目标位置进行二次估计可以更加准确地对目标进行定位和跟踪。
[0052]
3、matlab仿真分析证明该方法可行、有效，可持续进行目标轨迹的跟踪，实现简单。
[0053]
4、本发明提供的基于多站的轨迹跟踪算法，通过计算所述目标o的目标坐标点的估计值(x
′0，y
′0)所在空间位置γ0、所述第1站点的声基阵所在空间位置α0和所述第2站点的声基阵所在空间位置β0，分别计算所述目标坐标点的估计值(x
′0，y
′0)与所述第i个站点的声基阵之间的欧几里得距离和夹角的余弦值cosθ
′
i
，进而得到目标坐标点的二次估计值，快速利用传感器节点能量的需求，提出了通过部署普通的信标节点，以确定传感器定位的误差和精度。本发明提供的多站轨迹跟踪算法考虑了传感器节点不是时间同步的，水中的声速是未知的，通过多个声基站的角度定位，能够补偿时间同步，估计出定位的平均误差，获得了较好的定位精度。
附图说明
[0054]
在下文中将基于实施例并参考附图来对本发明进行更详细的描述。其中：
[0055]
图1为本发明提供的基于多站轨迹跟踪方法的流程图；
[0056]
图2为本发明提供的多站轨迹跟踪方法步骤中的站点分布图；
[0057]
图3为本发明提供的多站轨迹跟踪方法步骤s33中的目标坐标点与第i个声基阵之间夹角示意图；
[0058]
图4为本发明提供的多站轨迹跟踪方法基于3个站点的matlab仿真分析目标运动轨迹图；
[0059]
图5为本发明提供的方法的基于3个站点的matlab仿真分析目标真实轨迹和多站定位估计的目标位置对比图；
[0060]
图6为本发明提供的方法的基于3个站点的matlab仿真分析目标真实轨迹和本发明所采用的方法的目标跟踪目标轨迹对比图；
[0061]
图7为本发明提供的方法的基于3个站点的matlab仿真分析目标真实运动速度和本发明所采用的方法所跟踪到的目标运动速度对比图。
具体实施方式
[0062]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于
本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0063]
如图1所示，为本发明提供一种基于多站的轨迹跟踪算法的流程图，算法包括以下步骤：
[0064]
s1：如图2所示，测量目标o与第k个站点的声基阵之间连线与测量目标与第1个站点的声基阵之间连线的夹角θ1、目标o与第k个站点的声基阵之间连线与测量目标与第2个站点的声基阵之间连线的夹角θ2，k＝3,4,
……
,m
‑
1,m；
[0065]
s21：计算目标坐标点(x0，y0)到与第k个站点的声基阵之间的连线的距离平方和f(x0，y0)：
[0066][0067]
其中，θ
i
＝90
°
或270
°
(i＝1,2)，a
i
＝1,b
i
＝0,c
i
＝
‑
x
i
；
[0068]
θ
i
＝(i＝1,2)为其他值时，a
i
＝tan(θ
i
),b
i
＝
‑
1,c
i
＝
‑
tan(θ
i
)
·
x
i
；x
i
为为声基阵在空间内的横轴坐标值；
[0069]
s22：将f(x0，y0)分别对x0、y0求偏导，并令其等于0：
[0070][0071]
s23：求出目标坐标点(x0，y0)的估计值：
[0072][0073]
s31：计算目标o的目标坐标点的估计值(x
′0，y
′0)所在空间位置γ0、第1站点的声基阵所在空间位置α0和第2站点的声基阵所在空间位置β0：
[0074][0075][0076][0077]
其中，x
i
为声基阵在空间内的横轴坐标值，y
i
为声基阵在空间内的纵轴坐标值，其中i＝1,2；
[0078]
s32：分别计算目标坐标点的估计值(x
′0，y
′0)与第i个站点的声基阵之间的欧几里得距离：
[0079][0080]
s33：如图3所示，计算目标坐标点的估计值(x
′0，y
′0)与第i个站点的声基阵之间的
夹角的余弦值cosθ
′
i
，其中i＝1,2：
[0081][0082][0083]
即采用坐标点值来计算余弦值cosθ
′
i
的计算公式表达如下：
[0084][0085][0086]
s34：根据s33步骤得到的余弦值cosθ
i
，得到与第1个站点的声基阵之间连线的夹角θ
′1和与第1个站点的声基阵之间连线的夹角θ
′2[0087][0088][0089]
s4：采用θ
′1和θ
′2以及步骤s2的方法，得到目标坐标点的二次估计值(x
″0，y
″0)；
[0090]
s5：采用惯性协同定位跟踪目标o的运动轨迹，具体如下：
[0091]
s51：目标o向m个站点的声基阵在周期t的时刻n发射位置请求，分别计算目标o相对于m个站点的声基阵的位置偏移量，并计算目标o相对于自身第n
‑
1时刻的位置偏移量(δx
″0，δy
″0)，其中目标o相对于第k个站点的声基阵的位置偏移量为(δx
″0k，δy
″
0k
)，进而得到目标o相对于第k个站点的距离为d
k
；目标o相对于自身第n
‑
1时刻的位置偏移量(δx
″0，δy
″0)的计算公式如下：
[0092][0093][0094]
其中，x
″0(n
‑
1)为第n
‑
1时刻目标o所在横轴坐标点值，x
″0(n
‑
2)为第n
‑
2时刻目标o所在横轴坐标点值；y
″0(n
‑
1)为第n
‑
1时刻目标o所在横轴坐标点值，y
″0(n
‑
2)为第n
‑
2时刻目标o所在横轴坐标点值；t(n
‑
1)为周期t的第n
‑
1时刻，t(n
‑
2)为周期t的第n
‑
2时刻；
[0095]
s52：根据s51步骤得到的相对于目标o相对于m个站点的声基阵的位置偏移量的加权和计算目标o在空间内的位置偏移量(δx0，δy0)：
[0096]
δx0＝w
o
δ
″0+∑w(d
k
)δ
″
0k
；
[0097]
δy0＝w
o
δy
″0+∑w(d
k
)
△
y
″
0k
；
[0098]
其中，w
o
以及w(d
k
)是其所代表的站点与其他站点之间的距离l成反比的权重，各个站点之间的距离可能不同，此处统一为l，w
o
以及w(d
k
)具有如下关系：w
o
+∑w(d
k
)＝1，
[0099]
进而可以得到w
o
可以根据实际情况进行调整，代入到中，就可以得到w(d
k
)，进而得到目标o在空间内的时刻n的位置(x0(n)，y0(n))。
[0100]
对上述方法采用matlab仿真分析：设定三站点成等边三角形布放，目标进行匀速直线运动，速度6节，采样周期1s，观测方位角分别加入正负10
°
误差(均匀分布)。
[0101]
如图4
‑
7所示，为matlab仿真分析结果，通过对比目标真实运动轨迹及采用本发明方法的目标定位及实时跟踪方法，可以精确地反映出目标的真实空间位置和运动轨迹、运动速度，该基于多站定位的目标轨迹跟踪算法可行、有效，在该仿真条件下，该算法可以准确定位目标的坐标，并进行有效持续的目标轨迹跟踪。
[0102]
虽然已经参考优选实施例对本发明进行了描述，但在不脱离本发明的范围的情况下，可以对其进行各种改进并且可以用等效物替换其中的部件。尤其是，只要不存在结构冲突，各个实施例中所提到的各项技术特征均可以任意方式组合起来。本发明并不局限于文中公开的特定实施例，而是包括落入权利要求的范围内的所有技术方案。