CN108921870A - 三维空间转弯机动目标的轨迹起始方法及装置 - Google Patents

Abstract

本发明公开了一种用于三维空间转弯机动目标的轨迹起始方法，涉及多传感器信息融合技术领域，通过坐标变换、轨迹探测、状态与误差协方差估计，及坐标反变换，最终探测出观测空间中存在的目标，并获得目标的状态估计。本发明还公开了一种用于三维空间转弯机动目标的轨迹起始装置，有效地解决了三维空间转弯机动目标轨迹难以起始的问题，本发明可用于多目标跟踪领域，具有很强的实用性。

Description

三维空间转弯机动目标的轨迹起始方法及装置

技术领域

本发明属于多传感器信息融合技术领域，尤其涉及三维空间转弯机动目标的轨迹起始方法及装置。

背景技术

随着科学技术的发展，在自动控制以及多传感器信息融合技术领域越来越多的涉及到观测空间中新目标的探测，而其中轨迹起始是该领域中的研究人员重点研究的内容。

轨迹起始的主要任务是利用传感器在多个不同时刻获得的测量探测出现于观测空间中的新目标，并起始其轨迹。目前，常用的轨迹起始方法通常适用于假定目标作匀速或匀加速运动的轨迹起始，但是这些现有的轨迹起始方法并不适用于转弯机动目标的轨迹起始。

因此，如何解决三维空间转弯机动目标的轨迹起始问题是目前需要探索和解决的一个关键技术问题。

发明内容

本发明所要解决的技术问题为提供一种三维空间转弯机动目标的轨迹起始方法及装置，旨在解决三维空间转弯机动目标的轨迹起始的问题。

为解决上述技术问题，本发明提供了一种三维空间转弯机动目标的轨迹起始方法，所述方法包括：

步骤1、分别从三维空间转弯机动目标三个不同时刻的位置测量数据集中各自取出一个位置测量数据，构成位置测量数据组，用所述位置测量数据组中的三个位置测量数据确定坐标变换矩阵，并通过所述坐标变换矩阵将所述三个位置测量数据变换至xoy平面上，得到变换后的三个位置测量数据，用所述坐标变换矩阵对所述三个位置测量数据的误差协方差矩阵进行变换，得到所述三个位置测量数据变换后的误差协方差矩阵；

其具体步骤包括：设所述三维空间转弯机动目标在三个不同时刻的位置测量数据为y_k＝(z_k,t_k)，其中，k＝1,2,3，t_k表示在k时刻的时间， 和分别表示所述三维空间转弯机动目标在k时刻的位置测量数据的x分量、y分量和z分量；

令其中，i＝1,2,3和j＝1,2,3；利用连续的三个不同时刻的位置测量数据y₁＝(z₁,t₁)、y₂＝(z₂,t₂)和y₃＝(z₃,t₃)获得所述三维空间转弯机动目标运动平面与xoy平面的夹角为α，所述三维空间转弯机动目标运动平面与xoy平面的交线和x轴间的夹角为β，所述交线与x轴的交点为[x₀,0,0]^T；

其中，

上标T表示向量或矩阵的转置；

利用所述夹角α和所述夹角β，并取α₁＝α，α₂＝-α，β₁＝β，β₂＝-β，构建坐标变换矩阵F_ij；

其中，i＝1,2和j＝1,2；

利用所述坐标变换矩阵F_ij，以及所述交点[x₀,0,0]^T，分别对位置测量数据z₁、z₂和z₃进行坐标变换得到变换的位置测量数据和

其中，

利用变换的位置测量数据和求取其中i＝1,2和j＝1,2；从s_ij，i＝1,2和j＝1,2中找到最小值对应的索引号，即并取索引号(i₁,j₁)对应的坐标变换矩阵作为所求的变换矩阵F_c，即F_c＝F_i1j1，索引号(i₁,j₁)对应的位置测量数据和分别作为变换后的位置测量数据和即和

设测量z_k的误差协方差矩阵为其中 为误差协方差矩阵R_k的元素值；利用所述变换矩阵F_c对R_k进行变换，得到变换后的误差协方差矩阵其中k＝1,2,3；

步骤2、测试变换后的三个位置测量数据是否同时满足三个预设条件；若能同时满足三个预设条件，用所述变换后的三个位置测量数据形成一试探性轨迹，并转入步骤3；否则转入步骤1取下一组位置测量数据组进行测试；

步骤3、利用所述变换后的三个位置测量数据得到所述三维空间转弯机动目标的转弯率估计；利用所述变换后的三个位置测量数据和所述三维空间转弯机动目标的转弯率估计，得到变换后的三维空间转弯机动目标初始状态估计和误差协方差估计；

步骤4、利用所述的坐标变换矩阵F_c对变换后的三维空间转弯机动目标初始状态估计和误差协方差估计进行反变换，得到所述三维空间转弯机动目标的初始状态估计和误差协方差估计；

步骤5、在一个位置测量数据组的三个位置测量数据的测试完成后，转入步骤1取下一组位置测量数据组进行测试，直至所有的位置测量数据组都进行了测试。

本发明还提供了一种三维空间转弯机动目标的轨迹起始装置，所述装置包括：

变换模块，用于分别从三维空间转弯机动目标三个不同时刻的位置测量数据集中各自取出一个位置测量数据，构成位置测量数据组，用所述位置测量数据组中的三个位置测量数据确定坐标变换矩阵，并通过所述坐标变换矩阵将所述三个位置测量数据变换至xoy平面上，得到变换后的三个位置测量数据，用所述坐标变换矩阵对所述三个位置测量数据的误差协方差矩阵进行变换，得到所述三个位置测量数据变换后的误差协方差矩阵；

其具体用于：设所述三维空间转弯机动目标在三个不同时刻的位置测量数据为y_k＝(z_k,t_k)，其中，k＝1,2,3，t_k表示在k时刻的时间， 和分别表示所述三维空间转弯机动目标在k时刻的位置测量数据的x分量、y分量和z分量；

令其中，i＝1,2,3和j＝1,2,3；利用连续的三个不同时刻的位置测量数据y₁＝(z₁,t₁)、y₂＝(z₂,t₂)和y₃＝(z₃,t₃)获得所述三维空间转弯机动目标运动平面与xoy平面的夹角为α，所述三维空间转弯机动目标运动平面与xoy平面的交线和x轴间的夹角为β，所述交线与x轴的交点为[x₀,0,0]^T；

其中，

上标T表示向量或矩阵的转置；

利用所述夹角α和夹角β，并取α₁＝α，α₂＝-α，β₁＝β，β₂＝-β，构建坐标变换矩阵F_ij；

其中，i＝1,2和j＝1,2；

利用所述坐标变换矩阵F_ij，以及所述交点[x₀,0,0]^T，分别对位置测量数据z₁、z₂和z₃进行坐标变换得到变换的位置测量数据和

其中，

利用变换的位置测量数据和求取其中i＝1,2和j＝1,2；从s_ij，i＝1,2和j＝1,2中找到最小值对应的索引号，即并取索引号(i₁,j₁)对应的坐标变换矩阵作为所求的变换矩阵F_c，即F_c＝F_i1j1，索引号(i₁,j₁)对应的位置测量数据和分别作为变换后的位置测量数据和即和

设测量z_k的误差协方差矩阵为其中 为误差协方差矩阵R_k的元素值；利用所述变换矩阵F_c对R_k进行变换，得到变换后的误差协方差矩阵其中k＝1,2,3；

轨迹探测模块，用于测试变换后的三个位置测量数据是否同时满足三个预设条件；若能同时满足三个预设条件，用所述变换后的三个位置测量数据形成一试探性轨迹，并转入状态与误差协方差估计模块；否则转入变换模块取下一组位置测量数据组进行测试；

状态与误差协方差估计模块，用于利用所述变换后的三个位置测量数据得到所述三维空间转弯机动目标的转弯率估计；利用所述变换后的三个位置测量数据和所述三维空间转弯机动目标的转弯率估计，得到变换后的三维空间转弯机动目标初始状态估计和误差协方差估计；

反变换模块，用于利用所述的坐标变换矩阵F_c对变换后的三维空间转弯机动目标初始状态估计和误差协方差估计进行反变换，得到所述三维空间转弯机动目标的初始状态估计和误差协方差估计。

本发明提供一种用于三维空间转弯机动目标的轨迹起始方法及装置，通过坐标变换、轨迹探测、状态与误差协方差估计，及坐标反变换，最终探测出三维空间中存在的转弯机动目标，并获得三维空间转弯机动目标的状态估计以及误差协方差估计，从而有效地解决了三维空间转弯机动目标轨迹难以起始的问题，本发明可用于多目标跟踪领域，具有很强的实用性。

附图说明

图1为本发明实施例提供的一种用于三维空间转弯机动目标的轨迹起始方法的流程示意图；

图2为本发明实施例提供的一种用于三维空间转弯机动目标的轨迹起始装置的系统示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

作为本发明的第一个实施例，如图1所示，本发明实施例提供的一种用于三维空间转弯机动目标的轨迹起始方法的流程示意图，该方法包括：

步骤101、分别从三维空间转弯机动目标三个不同时刻的位置测量数据集中各自取出一个位置测量数据，构成位置测量数据组，用位置测量数据组中的三个位置测量数据确定坐标变换矩阵，并通过坐标变换矩阵将三个位置测量数据变换至xoy平面上，得到变换后的三个位置测量数据，用坐标变换矩阵对三个位置测量数据的误差协方差矩阵进行变换，得到三个位置测量数据变换后的误差协方差矩阵。

步骤101的主要目的是通过三个不同时刻的位置测量数据，利用坐标变换矩阵，得到变换后的三个位置测量数据，其步骤具体包括：

设三维空间转弯机动目标在三个不同时刻的位置测量数据为y_k＝(z_k,t_k)，其中，k＝1,2,3，t_k表示在k时刻的时间， 和分别表示三维空间转弯机动目标在k时刻的位置测量数据的x分量、y分量和z分量。

令其中，i＝1,2,3和j＝1,2,3；利用连续的三个不同时刻的位置测量数据y₁＝(z₁,t₁)、y₂＝(z₂,t₂)和y₃＝(z₃,t₃)获得三维空间转弯机动目标运动平面与xoy平面的夹角为α，三维空间转弯机动目标运动平面与xoy平面的交线和x轴间的夹角为β，交线与x轴的交点为[x₀,0,0]^T。

其中，

上标T表示向量或矩阵的转置；

利用夹角α和夹角β，并取α₁＝α，α₂＝-α，β₁＝β，β₂＝-β，构建坐标变换矩阵F_ij；

其中，i＝1,2和j＝1,2。

利用坐标变换矩阵F_ij，以及交点[x₀,0,0]^T，分别对位置测量数据z₁、z₂和z₃进行坐标变换得到变换的位置测量数据和

其中，

利用变换的位置测量数据和求取其中i＝1,2和j＝1,2；从s_ij，i＝1,2和j＝1,2中找到最小值对应的索引号，即并取索引号(i₁,j₁)对应的坐标变换矩阵作为所求的变换矩阵F_c，即F_c＝F_i1j1，索引号(i₁,j₁)对应的位置测量数据和分别作为变换后的位置测量数据和即和

设测量z_k的误差协方差矩阵为其中 为误差协方差矩阵R_k的元素值；利用变换矩阵F_c对R_k进行变换，得到变换后的误差协方差矩阵其中k＝1,2,3。

步骤102、测试变换后的三个位置测量数据是否同时满足三个预设条件；若能同时满足三个预设条件，用变换后的三个位置测量数据形成一试探性轨迹，并转入步骤103；否则转入步骤101取下一组位置测量数据组进行测试。

步骤102，主要利用三个预设条件测试来变换后的三个位置测量数据，并将满足三个预设条件的变换后的三个位置测量数据形成试探性轨迹，其具体步骤包括：

以和表示变换后的位置测量数据，测试位置测量数据和是否同时满足下列三个预设条件：

预设条件1：

预设条件2：

预设条件3：

其中，v_min、v_max、a_max和ω_max分别为最小速度、最大速度、最大加速度和最大转弯率，||·||₂表示向量的2范数，b₁、b₂和c是三个与协方差矩阵相关的参数；以V_k表示矩阵的乔里斯基(Cholesky)分解矩阵，即以V_k,1、V_k,2和V_k,3表示分解矩阵V_k的主对角线上的三个元素，其中，k＝1,2,3，则参数b₁、b₂和c分别取为

其中，b₃控制参数，其取值范围为b₃＝1～3。

步骤103、利用变换后的三个位置测量数据得到三维空间转弯机动目标的转弯率估计；利用变换后的三个位置测量数据和三维空间转弯机动目标的转弯率估计，得到变换后的三维空间转弯机动目标初始状态估计和误差协方差估计。其具体步骤包括：

利用已形成试探性轨迹的变换后的三个位置测量数据和得到三维空间转弯机动目标的转弯率估计为

其中，sgn(a)为取变量a的符号，且有

利用已形成试探性轨迹的变换后的三个位置测量数据和形成列向量m_y，得到

用表示r时刻坐标变换后的三维空间转弯机动目标状态估计，其中和分别表示位置的x分量、y分量和z分量，和分别表示速度的x分量和y分量；

基于列向量m_y及转弯率估计获得在r时刻变换后的三维空间转弯机动目标状态估计为

其中，

在r时刻变换后的误差协方差矩阵为

其中，

步骤104、利用的坐标变换矩阵F_c对变换后的三维空间转弯机动目标初始状态估计和误差协方差估计进行反变换，得到三维空间转弯机动目标的初始状态估计和误差协方差估计。其具体步骤包括：

利用变换矩阵F_c对变换后的三维空间转弯机动目标状态估计进行反变换，以获得三维空间转弯机动目标在r时刻的状态估计为利用变换矩阵F_c对变换后的误差协方差矩阵进行反变换，得到目标在r时刻的误差协方差矩阵为P_r

其中，

其中，F_c(i,j)表示矩阵F_c的第i行第j列元素。

步骤105、在一个位置测量数据组的三个位置测量数据的测试完成后，转入步骤101取下一组位置测量数据组进行测试，直至所有的位置测量数据组都进行了测试。

在本发明第一个实施例所提供的一种用于三维空间转弯机动目标的轨迹起始方法，通过坐标变换、轨迹探测、状态与误差协方差估计，及坐标反变换，最终探测出观测空间中存在的目标，并获得目标的状态估计，从而有效地解决了三维空间转弯机动目标轨迹难以起始的问题，可用于多目标跟踪领域，具有很强的实用性。

作为本发明的第二个实施例，请参阅图2，图2为本发明实施例提供的一种用于三维空间转弯机动目标的轨迹起始装置的系统示意图。该装置包括：

变换模块10，用于分别从三维空间转弯机动目标三个不同时刻的位置测量数据集中各自取出一个位置测量数据，构成位置测量数据组，用位置测量数据组中的三个位置测量数据确定坐标变换矩阵，并通过坐标变换矩阵将三个位置测量数据变换至xoy平面上，得到变换后的三个位置测量数据，用坐标变换矩阵对三个位置测量数据的误差协方差矩阵进行变换，得到三个位置测量数据变换后的误差协方差矩阵。

其具体用于：设三维空间转弯机动目标在三个不同时刻的位置测量数据为y_k＝(z_k,t_k)，其中，k＝1,2,3，t_k表示在k时刻的时间， 和分别表示三维空间转弯机动目标在k时刻的位置测量数据的x分量、y分量和z分量。

令其中，i＝1,2,3和j＝1,2,3；利用连续的三个不同时刻的位置测量数据y₁＝(z₁,t₁)、y₂＝(z₂,t₂)和y₃＝(z₃,t₃)获得三维空间转弯机动目标运动平面与xoy平面的夹角为α，三维空间转弯机动目标运动平面与xoy平面的交线和x轴间的夹角为β，交线与x轴的交点为[x₀,0,0]^T；

其中，

上标T表示向量或矩阵的转置。

利用的夹角α和夹角β，并取α₁＝α，α₂＝-α，β₁＝β，β₂＝-β，构建坐标变换矩阵F_ij；

其中，i＝1,2和j＝1,2。

利用坐标变换矩阵F_ij，以及交点[x₀,0,0]^T，分别对位置测量数据z₁、z₂和z₃进行坐标变换得到变换的位置测量数据和

其中，

利用变换的位置测量数据和求取其中i＝1,2和j＝1,2；从s_ij，i＝1,2和j＝1,2中找到最小值对应的索引号，即并取索引号(i₁,j₁)对应的坐标变换矩阵作为所求的变换矩阵F_c，即F_c＝F_i1j1，索引号(i₁,j₁)对应的位置测量数据和分别作为变换后的位置测量数据和即和

设测量z_k的误差协方差矩阵为其中 为误差协方差矩阵R_k的元素值；利用变换矩阵F_c对R_k进行变换，得到变换后的误差协方差矩阵其中k＝1,2,3。

轨迹探测模块10，用于测试变换后的三个位置测量数据是否同时满足三个预设条件；若能同时满足三个预设条件，用变换后的三个位置测量数据形成一试探性轨迹，并转入状态与误差协方差估计模块；否则转入变换模块取下一组位置测量数据组进行测试。轨迹探测模块10具体用于：

以和表示变换后的位置测量数据，测试位置测量数据和是否同时满足下列三个预设条件：

预设条件1：

预设条件2：

预设条件3：

其中，v_min、v_max、a_max和ω_max分别为最小速度、最大速度、最大加速度和最大转弯率，||·||₂表示向量的2范数，b₁、b₂和c是三个与协方差矩阵相关的参数；以V_k表示矩阵的乔里斯基(Cholesky)分解矩阵，即以V_k,1、V_k,2和V_k,3表示分解矩阵V_k的主对角线上的三个元素，其中，k＝1,2,3，则参数b₁、b₂和c分别取为

其中，b₃控制参数，其取值范围为b₃＝1～3。

状态与误差协方差估计模块30，用于利用变换后的三个位置测量数据得到三维空间转弯机动目标的转弯率估计；利用变换后的三个位置测量数据和三维空间转弯机动目标的转弯率估计，得到变换后的三维空间转弯机动目标初始状态估计和误差协方差估计。状态与误差协方差估计模块30具体用于：

利用已形成试探性轨迹的变换后的三个位置测量数据和得到三维空间转弯机动目标的转弯率估计为：

其中，sgn(a)为取变量a的符号，且有

利用已形成试探性轨迹的变换后的三个位置测量数据和形成列向量m_y，得到

用表示r时刻坐标变换后的三维空间转弯机动目标状态估计，其中和分别表示位置的x分量、y分量和z分量，和分别表示速度的x分量和y分量。

基于列向量m_y及转弯率估计获得在r时刻变换后的三维空间转弯机动目标状态估计为

其中，

在r时刻变换后的误差协方差矩阵为：

其中，

反变换模块40，用于利用的坐标变换矩阵F_c对变换后的三维空间转弯机动目标初始状态估计和误差协方差估计进行反变换，得到三维空间转弯机动目标的初始状态估计和误差协方差估计。反变换模块40具体用于：

利用变换矩阵F_c对变换后的三维空间转弯机动目标状态估计进行反变换，以获得三维空间转弯机动目标在r时刻的状态估计为利用变换矩阵F_c对变换后的误差协方差矩阵进行反变换，得到目标在r时刻的误差协方差矩阵为P_r，

其中，

其中，F_c(i,j)表示矩阵F_c的第i行第j列元素。

本发明第二个实施例所提供的一种用于三维空间转弯机动目标的轨迹起始装置，通过坐标变换模块、轨迹探测模块、状态与误差协方差估计模块、坐标反变换模块以及循环装置，最终探测出观测空间中存在的目标，并获得目标的状态估计，从而有效地解决了三维空间转弯机动目标轨迹难以起始的问题，可用于多目标跟踪领域，具有很强的实用性。

作为本发明的第三个实例，其采用了如上述第一个实施例所提供的方法对仿真数据进行了实验。本实施例主要考虑两个在三维空间中作转弯机动的转弯机动目标，目标1的转弯率为ω＝-5°/s，其初始状态为其在t＝0、t＝T和t＝2T时刻位置分别为[-866.8m,969.2m,207.1m]^T，[-324.6m,1224.0m,193.5m]^T和[251.6m,1383.7m,153.4m]^T，目标2的转弯转弯率为ω＝5°/s，其初始状态为[-733.1m,242.5m/s,1203.8m,140.0m/s,-310.6m,0m/s]^T，其在t＝0、t＝T和t＝2T时刻位置分别为[-733.1m,1203.8m,-310.6m]^T，[-274.2m,1523.2m,-323.2m]^T和[123.8m,1914.4m,-360.7m]^T。传感器测量目标位置，其采样周期为T＝2s，位置测量数据为目标位置加上测量噪声，用σ_x表示传感器在x方向上的测量噪声的标准差，σ_y.表示传感器在y方向上的测量噪声的标准差，σ_z.表示传感器在z方向上的测量噪声的标准差，它们的取值为：σ_x＝σ_y＝σ_z＝1m，协方差矩阵中σ_xy、σ_yz和σ_xz的取为σ_xy＝σ_yz＝σ_xz＝0m。传感器在t＝0、t＝T和t＝2T对目标测量，得到3个位置测量数据。两个目标在t＝0s时的状态值如下表1所示：

表1：两个目标在t＝0s时的状态值

为了处理传感器获得的测量数据，设最小速度v_min＝450m/s、最大速度v_max＝200m/s、最大加速度a_max＝3m/s²，最大转弯率ω_max＝12°/s(即ω_max＝0.2094rad/s)，控制参数b₃＝2.5。采用如本发明第一实施例所提供的方法对传感器获得的测量数据进行处理，可以成功地探测出观测空间中的两个目标，并能得到这两个目标在t＝0s时的状态估计，两个目标在t＝0s时的状态估计如下表2所示：

表2：一次实验得到的两个目标在t＝0s时的状态估计

采用本发明第一实施例所提供的方法，进行了100次蒙特·卡罗统计模拟实验，得到上述两个目标在t＝0s时的状态估计下表3所示：

表3：100次蒙特·卡罗统计模拟得到的两个目标在t＝0s时的状态估计

对比表1所示的两目标的状态值，表2和表3的实验结果表明，本发明所提供的三维空间转弯机动目标的轨迹起始方法，可以获得较为精确和可靠的目标状态估计、各目标的状态估计非常接近它们真实的状态值。

以上仅为本发明的较佳实施例而已，并不用以限制发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。

作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。

集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定都是本发明所必须的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其它实施例的相关描述。

以上为对本发明所提供的一种用于三维空间转弯机动目标的轨迹起始方法及装置的描述，对于本领域的技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种用于转弯机动目标的轨迹起始方法，其特征在于，所述方法包括：

步骤1、分别从三维空间转弯机动目标三个不同时刻的位置测量数据集中各自取出一个位置测量数据，构成位置测量数据组，用所述位置测量数据组中的三个位置测量数据确定坐标变换矩阵，并通过所述坐标变换矩阵将所述三个位置测量数据变换至xoy平面上，得到变换后的三个位置测量数据，用所述坐标变换矩阵对所述三个位置测量数据的误差协方差矩阵进行变换，得到所述三个位置测量数据变换后的误差协方差矩阵；

其具体步骤包括：设所述三维空间转弯机动目标在三个不同时刻的位置测量数据为y_k＝(z_k,t_k)，其中，k＝1,2,3，t_k表示在k时刻的时间， 和分别表示所述三维空间转弯机动目标在k时刻的位置测量数据的x分量、y分量和z分量；

令其中，i＝1,2,3和j＝1,2,3；利用连续的三个不同时刻的位置测量数据y₁＝(z₁,t₁)、y₂＝(z₂,t₂)和y₃＝(z₃,t₃)获得所述三维空间转弯机动目标运动平面与xoy平面的夹角为α，所述三维空间转弯机动目标运动平面与xoy平面的交线和x轴间的夹角为β，所述交线与x轴的交点为[x₀,0,0]^T；

其中，

上标T表示向量或矩阵的转置；

利用所述夹角α和所述夹角β，并取α₁＝α，α₂＝-α，β₁＝β，β₂＝-β，构建坐标变换矩阵F_ij；

其中，i＝1,2和j＝1,2；

利用所述坐标变换矩阵F_ij，以及所述交点[x₀,0,0]^T，分别对位置测量数据z₁、z₂和z₃进行坐标变换得到变换的位置测量数据和

其中，

利用变换的位置测量数据和求取其中i＝1,2和j＝1,2；从s_ij，i＝1,2和j＝1,2中找到最小值对应的索引号，即并取索引号(i₁,j₁)对应的坐标变换矩阵作为所求的变换矩阵F_c，即索引号(i₁,j₁)对应的位置测量数据和分别作为变换后的位置测量数据和即和

设测量z_k的误差协方差矩阵为其中 为误差协方差矩阵R_k的元素值；利用所述变换矩阵F_c对R_k进行变换，得到变换后的误差协方差矩阵其中

步骤2、测试变换后的三个位置测量数据是否同时满足三个预设条件；若能同时满足三个预设条件，用所述变换后的三个位置测量数据形成一试探性轨迹，并转入步骤3；否则转入步骤1取下一组位置测量数据组进行测试；

步骤3、利用所述变换后的三个位置测量数据得到所述三维空间转弯机动目标的转弯率估计；利用所述变换后的三个位置测量数据和所述三维空间转弯机动目标的转弯率估计，得到变换后的三维空间转弯机动目标初始状态估计和误差协方差估计；

步骤4、利用所述的坐标变换矩阵F_c对变换后的三维空间转弯机动目标初始状态估计和误差协方差估计进行反变换，得到所述三维空间转弯机动目标的初始状态估计和误差协方差估计；

步骤5、在一个位置测量数据组的三个位置测量数据的测试完成后，转入步骤1取下一组位置测量数据组进行测试，直至所有的位置测量数据组都进行了测试。

2.如权利要求1所述的方法，其特征在于，所述步骤2包括：

以和表示变换后的位置测量数据，测试所述位置测量数据和是否同时满足下列三个预设条件：

预设条件1：

预设条件2：

预设条件3：

其中，v_min、v_max、a_max和ω_max分别为最小速度、最大速度、最大加速度和最大转弯率，||·||₂表示向量的2范数，b₁、b₂和c是三个与协方差矩阵相关的参数；以V_k表示矩阵的乔里斯基(Cholesky)分解矩阵，即以V_k,1、V_k,2和V_k,3表示分解矩阵V_k的主对角线上的三个元素，其中，k＝1,2,3，则参数b₁、b₂和c分别取为

其中，b₃控制参数，其取值范围为b₃＝1～3。

3.如权利要求2所述的方法，其特征在于，所述步骤3包括：

利用已形成试探性轨迹的所述变换后的三个位置测量数据和得到所述三维空间转弯机动目标的转弯率估计为

其中，sgn(a)为取变量a的符号，且有

利用已形成试探性轨迹的所述变换后的三个位置测量数据和形成列向量m_y，得到

用表示r时刻坐标变换后的三维空间转弯机动目标状态估计，其中和分别表示位置的x分量、y分量和z分量，和分别表示速度的x分量和y分量；

基于所述列向量m_y及所述转弯率估计获得在r时刻变换后的三维空间转弯机动目标状态估计为

其中，

在r时刻变换后的误差协方差矩阵为

P_r ^c＝(Φ^TΦ)^-1Φ^TR^cΦ(Φ^TΦ)^-1

其中，

4.如权利要求3所述的方法，其特征在于，所述步骤4包括：

利用所述变换矩阵F_c对变换后的三维空间转弯机动目标状态估计进行反变换，以获得三维空间转弯机动目标在r时刻的状态估计为利用所述变换矩阵F_c对变换后的误差协方差矩阵进行反变换，得到目标在r时刻的误差协方差矩阵为P_r

其中，

其中，F_c(i,j)表示矩阵F_c的第i行第j列元素。

5.三维空间转弯机动目标的轨迹起始装置，其特征在于，所述装置包括：

变换模块，用于分别从三维空间转弯机动目标三个不同时刻的位置测量数据集中各自取出一个位置测量数据，构成位置测量数据组，用所述位置测量数据组中的三个位置测量数据确定坐标变换矩阵，并通过所述坐标变换矩阵将所述三个位置测量数据变换至xoy平面上，得到变换后的三个位置测量数据，用所述坐标变换矩阵对所述三个位置测量数据的误差协方差矩阵进行变换，得到所述三个位置测量数据变换后的误差协方差矩阵；

其具体用于：设所述三维空间转弯机动目标在三个不同时刻的位置测量数据为y_k＝(z_k,t_k)，其中，k＝1,2,3，t_k表示在k时刻的时间，和分别表示所述三维空间转弯机动目标在k时刻的位置测量数据的x分量、y分量和z分量；

令其中，i＝1,2,3和j＝1,2,3；利用连续的三个不同时刻的位置测量数据y₁＝(z₁,t₁)、y₂＝(z₂,t₂)和y₃＝(z₃,t₃)获得所述三维空间转弯机动目标运动平面与xoy平面的夹角为α，所述三维空间转弯机动目标运动平面与xoy平面的交线和x轴间的夹角为β，所述交线与x轴的交点为[x₀,0,0]^T；

其中，

上标T表示向量或矩阵的转置；

利用所述夹角α和夹角β，并取α₁＝α，α₂＝-α，β₁＝β，β₂＝-β，构建坐标变换矩阵F_ij；

其中，i＝1,2和j＝1,2；

利用所述坐标变换矩阵F_ij，以及所述交点[x₀,0,0]^T，分别对位置测量数据z₁、z₂和z₃进行坐标变换得到变换的位置测量数据和

其中，

利用变换的位置测量数据和求取其中i＝1,2和j＝1,2；从s_ij，i＝1,2和j＝1,2中找到最小值对应的索引号，即并取索引号(i₁,j₁)对应的坐标变换矩阵作为所求的变换矩阵F_c，即索引号(i₁,j₁)对应的位置测量数据和分别作为变换后的位置测量数据和即和

设测量z_k的误差协方差矩阵为其中 为误差协方差矩阵R_k的元素值；利用所述变换矩阵F_c对R_k进行变换，得到变换后的误差协方差矩阵其中

轨迹探测模块，用于测试变换后的三个位置测量数据是否同时满足三个预设条件；若能同时满足三个预设条件，用所述变换后的三个位置测量数据形成一试探性轨迹，并转入状态与误差协方差估计模块；否则转入变换模块取下一组位置测量数据组进行测试；

状态与误差协方差估计模块，用于利用所述变换后的三个位置测量数据得到所述三维空间转弯机动目标的转弯率估计；利用所述变换后的三个位置测量数据和所述三维空间转弯机动目标的转弯率估计，得到变换后的三维空间转弯机动目标初始状态估计和误差协方差估计；

反变换模块，用于利用所述的坐标变换矩阵F_c对变换后的三维空间转弯机动目标初始状态估计和误差协方差估计进行反变换，得到所述三维空间转弯机动目标的初始状态估计和误差协方差估计。

6.如权利要求5所述的装置，其特征在于，所述轨迹探测模块具体用于：

以和表示变换后的位置测量数据，测试所述位置测量数据和是否同时满足下列三个预设条件：

预设条件1：

预设条件2：

预设条件3：

其中，v_min、v_max、a_max和ω_max分别为最小速度、最大速度、最大加速度和最大转弯率，||·||₂表示向量的2范数，b₁、b₂和c是三个与协方差矩阵相关的参数；以V_k表示矩阵的乔里斯基(Cholesky)分解矩阵，即以V_k,1、V_k,2和V_k,3表示分解矩阵V_k的主对角线上的三个元素，其中，k＝1,2,3，则参数b₁、b₂和c分别取为

其中，b₃控制参数，其取值范围为b₃＝1～3。

7.如权利要求6所述的装置，其特征在于，所述状态与误差协方差估计模块具体用于：

利用已形成试探性轨迹的所述变换后的三个位置测量数据和得到所述三维空间转弯机动目标的转弯率估计为

其中，sgn(a)为取变量a的符号，且有

利用已形成试探性轨迹的所述变换后的三个位置测量数据和形成列向量m_y，得到

用表示r时刻坐标变换后的三维空间转弯机动目标状态估计，其中和分别表示位置的x分量、y分量和z分量，和分别表示速度的x分量和y分量；

基于所述列向量m_y及所述转弯率估计获得在r时刻变换后的三维空间转弯机动目标状态估计为

其中，

在r时刻变换后的误差协方差矩阵为

P_r ^c＝(Φ^TΦ)^-1Φ^TR^cΦ(Φ^TΦ)^-1

其中，

8.如权利要求7所述的装置，其特征在于，所述反变换模块具体用于：

利用所述变换矩阵F_c对变换后的三维空间转弯机动目标状态估计进行反变换，以获得三维空间转弯机动目标在r时刻的状态估计为利用所述变换矩阵F_c对变换后的误差协方差矩阵进行反变换，得到目标在r时刻的误差协方差矩阵为P_r

其中，

其中，F_c(i,j)表示矩阵F_c的第i行第j列元素。