[发明专利]基于时钟偏差和站址误差的时差定位跟踪方法

权利要求书

1.一种基于时钟偏差和站址误差的时差定位跟踪方法，其特征在于，包括如下步骤：

(1)构建时差定位跟踪场景：

构建空间直角坐标系OXYZ中包括时差定位跟踪系统和作匀速直线运动的目标的时差定位跟踪场景，其中，时差定位跟踪系统包括信息采集模块、信息处理模块和跟踪控制模块；所述信息采集模块包括时间戳差值信息采集模块和位置信息采集模块；所述时间戳差值信息采集模块包括四个传感器，其中一个传感器为参考传感器，其余三个传感器为测量传感器，测量传感器相对于参考传感器存在时钟偏差；

(2)初始化参数：

设信息采集模块中传感器{S_i|1≤i≤4}的初始位置为设信息处理模块对目标T的状态l进行估计的初始值为状态预测值为的误差协方差为P_1|1，采样时间间隔为Δt，参考距离为r₀，r₀下传感器{S_i|1≤i≤4}的测距方差为传感器{S_i|1≤i≤4}的站址误差的方差为T与{S_i|1≤i≤4}不碰撞的最短距离为R，T与{S_i|1≤i≤4}在高度维的最短距离为h，设跟踪控制模块记录系统工作的初始时刻为k，最大时刻为K，其中，和分别表示{S_i|1≤i≤4}在X、Y和Z轴方向的坐标值，[·]^T表示转置运算，l＝[u^oT,v^oT]^T，u^o表示T的位置向量，u^o＝[x^o,y^o,z^o]^T，x^o、y^o和z^o分别表示T在X、Y和Z轴方向的坐标值，v^o表示T的速度向量，v^o＝[v_x,v_y,v_z]^T，v_x、v_y和v_z分别表示T在X、Y和Z轴方向的速度值，表示T的位置估计的初始值，和分别表示在X、Y和Z轴方向上的分量，表示T的速度估计的初始值，和分别表示在X、Y和Z轴方向上的分量，F表示T的初始状态转移矩阵，K＞2，并令k＝2，Δt＝1；

(3)信息采集模块采集时间戳差值信息和传感器位置的观测值信息：

(3a)时间戳差值信息采集模块中的每个测量传感器{S_i|2≤i≤4}根据k时刻采集的目标T辐射信号的到达时间戳τ_i,k，与参考传感器{S_i|i＝1}k时刻采集的目标T辐射信号的参考到达时间戳τ_1,k，计算自己与参考传感器{S_i|i＝1}的时间戳差值τ_i1,k，并将τ_i1,k发送至信息处理模块：

τ_i1,k＝τ_i,k-τ_1,k；

(3b)位置信息采集模块采集每个传感器{S_i|1≤i≤4}位置的观测值s_i,k，并将s_i,k发送至信息处理模块：

s_i,k＝[x_i,k,y_i,k,z_i,k]^T

其中，x_i,k、y_i,k和z_i,k分别表示s_i,k在X、Y和Z轴方向上的分量；

(4)信息处理模块基于时钟偏差和站址误差获取目标T的位置信息：

(4a)信息处理模块根据每个测量传感器{S_i|2≤i≤4}与参考传感器{S_i|i＝1}的时间戳差值τ_i1,k，计算目标T和每个测量传感器{S_i|2≤i≤4}与目标T和参考传感器{S_i|i＝1}在k时刻的距离差观测值r_i1,k：

r_i1,k＝cτ_i1,k

其中，c表示信号在空间中的传播速度；

(4b)信息处理模块获取测距偏差的估计值

(4b1)信息处理模块根据距离差观测值r_i1,k构建r_i1,k的模型r_k：

n_k＝[n_21,k,n_31,k,n_41,k]^T

其中，表示的向量形式，表示目标T和每个测量传感器{S_i|2≤i≤4}与目标T和参考传感器{S_i|i＝1}在k时刻的距离差真实值，表示{S_i|1≤i≤4}与T的距离真实值，表示k时刻T的位置的真实值，和分别表示k时刻T在X、Y和Z轴方向的坐标值，表示k时刻{S_i|1≤i≤4}的位置的真实值，和分别表示在X、Y和Z轴方向上的分量，||·||表示求向量的二范数，δ°表示的向量形式，表示由{S_i|2≤i≤4}和{S_i|i＝1}间的时钟偏差{t_i1∣2≤i≤4}引起的测距偏差，n_k表示n_i1,k的向量形式，n_k服从均值为零且协方差矩阵为Q_r,k的高斯分布，n_i1,k表示k时刻r_i1,k的由距离决定的测量噪声，且表示传感器{S_i|1≤i≤4}的测距方差，

(4b2)信息处理模块根据距离差观测值r_i1,k，构建关于k时刻目标T的位置的真实值方程组U：

(4b3)信息处理模块对方程组U进行求解，得到T的位置的真实值的一次估计值并根据计算k时刻传感器{S_i|1≤i≤4}与目标T的距离真实值的估计值再根据计算Q_r,k的估计值

其中，和分别表示在X、Y和Z轴方向上的分量，表示传感器{S_i|1≤i≤4}测距方差的估计值，

(4b4)信息处理模块根据建立最大似然优化函数G_1,k，并对G_1,k进行求解，得到的估计值

(4c)信息处理模块对目标T进行预定位：

(4c1)信息处理模块根据每个传感器{S_i|1≤i≤4}位置的观测值s_i,k，构建s_i,k的模型s_k：

其中，表示k时刻传感器{S_i|1≤i≤4}的位置真实值的向量形式，p表示k时刻Δs_i,k的向量形式，p服从均值为0且协方差矩阵为Q_p的高斯分布，Δs_i,k表示k时刻{S_i|1≤i≤4}的站址误差，Δs_i,k＝[Δx_i,k,Δy_i,k,Δz_i,k]^T，Δx_i,k、Δy_i,k和Δz_i,k分别表示Δs_i,k在X、Y和Z轴方向上的分量，diag[I_1×12]表示以I_1×12为对角线的对角矩阵，I_1×12表示1×12维的全1矩阵；

(4c2)信息处理模块计算补偿测距偏差后的距离观测值并根据和建立最大似然优化函数G_2,k，并对G_2,k进行求解，得到的二次估计值和的估计值其中，和分别表示在X、Y和Z轴方向上的分量，和分别表示在X、Y和Z轴方向上的分量；

(4d)信息处理模块对目标T进行定位：

信息处理模块将作为观测值，利用的误差协方差P_k-1|k-1对k时刻T的状态l_k进行卡尔曼滤波，得到T的状态估计值和的误差协方差P_k|k，并通过取的前三个元素获得k时刻T的位置估计值并将发送至跟踪控制模块；

其中，表示k-1时刻的估计值，和分别表示在X、Y和Z轴方向上的分量，表示k-1时刻v^o的估计值，和分别表示在X、Y和Z轴方向上的分量，表示k时刻的估计值，和分别表示在X、Y和Z轴方向上的分量，表示k时刻v^o的估计值，和分别表示在X、Y和Z轴方向上的分量，[·]_1:3表示取向量的前3个元素操作；

(5)跟踪控制模块获取目标T的时差定位跟踪结果：

跟踪控制模块对信息处理模块发送的的估计值进行存储，并判断k＞K是否成立，若是，将K个时刻的作为对目标T的定位跟踪结果，否则，令k＝k+1，并执行步骤(6)；

(6)信息处理模块计算k时刻传感器{S_i|1≤i≤4}移动的目的位置坐标：

(6a)信息处理模块获得目标T的位置的预测值

其中，表示由k-1时刻的目标状态估计值计算的l_k的预测值，表示k时刻的预测值，和分别表示在X、Y和Z轴方向上的分量，表示k时刻v^o的预测值，和分别表示在X、Y和Z轴方向上的分量；

(6b)信息处理模块计算k时刻传感器{S_i|1≤i≤4}移动的目的位置坐标

(7)跟踪控制模块控制传感器移动：

跟踪控制模块向信息采集模块发送指令，控制时间戳差值信息采集模块中的传感器{S_i|1≤i≤4}移动至并执行步骤(3)。