技术领域
[0001] 本发明属于雷达信号处理领域，具体是一种基于非线性优化的两坐标雷达双基定位跟踪方法，用于计算缺失的雷达俯仰角量测，提高定位跟踪结果的精度。
相关背景技术
[0002] 基于牛顿迭代法的非线性优化算法，是一种用于求解非线性方程组或优化问题的重要方法。它源于牛顿‑拉夫森法(Newton‑Raphson Method)，最初用于求解单变量或多变量方程的根，后来被扩展到非线性优化领域。
[0003] 在接近最优解时，牛顿法具有二次收敛性，即每次迭代的误差与上次误差的平方成正比。该方法可以处理高维度优化问题，它依赖于梯度和Hessian矩阵，能够利用目标函数的几何信息。
[0004] 扩展卡尔曼滤波(Extended Kalman Filter,EKF)是非线性系统中最常用的一种状态估计方法，常被用于多传感器数据融合和动态系统的状态估计问题。EKF可以看作是标准卡尔曼滤波(KF)的非线性扩展，主要通过对非线性系统进行线性化处理，使得卡尔曼滤波方法能够应用于非线性场景。
具体实施方式
[0084] 下面将结合附图和实施例对本发明作进一步的详细说明。
[0085] 本发明基于非线性优化理论，提出了一种两坐标雷达双基定位跟踪方法，基于方位角和距离的目标三维位置估计问题，如图1所示，具体步骤如下：
[0086] 步骤一、针对两传感器A、B，对同一个目标进行测量的场景，得到目标在雷达球坐标系下的二维坐标；
[0087] X1＝(R1,θ1)
[0088] X2＝(R2,θ2)
[0089] X1为传感器A测量的目标的二维坐标，X2为传感器B测量的目标的二维坐标；(R1,θ1)表示目标相对于传感器A阵面中心的径向距离和方位角；(R2,θ2)表示目标相对于传感器B阵面中心的径向距离和方位角；
[0090] 步骤二、设置两传感器对目标的虚拟俯仰角分别为Φ1和Φ2，根据雷达球坐标系和雷达直角坐标系中的转换关系，得到目标在雷达直角坐标系下的虚拟三维坐标；
[0091] 传感器A测量的目标的虚拟三维坐标为：
[0092] x1＝R1cos(Φ1)cos(θ1)
[0093] y1＝R1cos(Φ1)sin(θ1)
[0094] z1＝R1 sin(Φ1)
[0095] 传感器B测量的目标的虚拟三维坐标为：
[0096] x2＝R2cos(Φ2)cos(θ2)
[0097] y2＝R2cos(Φ2)sin(θ2)
[0098] z2＝R2 sin(Φ2)
[0099] 步骤三、将目标的虚拟三维位置转化到WGS‑84坐标系下，计算两传感器对目标的俯仰角Φ1和Φ2的关系等式；
[0100] 首先，将雷达直角坐标系下的目标位置转化为WGS‑84坐标系下，转化后的目标位置为：
[0101]
[0102] 其中，(Ex1,Ey1,Ez1)表示传感器A在地心点坐标系下的位置，(Ex2,Ey2,Ez2)表示传感器B在地心点坐标系下的位置，(L1,B1)表示传感器A在地面点坐标系下的位置，(L2,B2)表示传感器B在地面点坐标系下的位置，(x1,y1,z1)表示目标在传感器A的雷达直角坐标系的位置坐标，(x2,y2,z2)表示目标在传感器B的雷达直角坐标系的位置坐标；
[0103] 然后，将目标转化在WGS‑84坐标系下的虚拟三维位置，进行化简得到两个俯仰角量测值Φ1和Φ2的关系等式：
[0104]
[0105] 其中，各中间变量值计算分别如下：
[0106] ak＝‑Rkcos(θk)sin(Lk)；bk＝‑Rk sin(θk)cos(Lk)sin(Bk)；ck＝Rk cos(Lk)cos(Bk)；
[0107] dk＝Rk cos(θk)cos(Lk)；Ek＝Rk sin(θk)sin(Lk)sin(Bk)；fk＝Rk sin(Lk)cos(Bk)；
[0108] Gk＝Rk sin(θk)cos(Bk)；Jk＝Rk sin(Bk)；Mk＝Exk；Nk＝Eyk；Tk＝Ezk；
[0109] 其中，k＝1,2分别表示传感器A和B；
[0110] 步骤四、结合两传感器对目标的俯仰角Φ1和Φ2的关系等式，计算目标在两个传感器的坐标位置的差值，将该差值的平方和最小化作为目标函数；
[0111] 目标函数构建如下：
[0112]
[0113] 其中，各参数计算如下：Kk＝sinΦk； Ak＝ak+bk；Dk＝dk+Ek；
[0114] 因为Φ1和Φ2的取值范围为 所以cosΦ1和cosΦ2都取正值。
[0115] 步骤五、基于牛顿迭代法对目标函数进行非线性优化，求解最优俯仰角Φ1和Φ2；
[0116] 具体步骤如下：
[0117] 步骤501、设置初始变量Kl＝[K1,K2]＝[0.1,0.1]，梯度初始值Hessian矩阵Bl初始近似值为单位矩阵I；
[0118] 步骤502、使用Hessian矩阵近似值Bl和梯度gl计算搜索方向
[0119] l初始值为0；
[0120] 步骤503、在搜索方向pl上进行线搜索，找到最优步长αl，得到该搜索方向上的目标函数F(Kl+αlpl)最小值。
[0121] 步骤504、利用最优步长αl在搜索方向上更新变量Kl+1＝Kl+αlpl；
[0122] 步骤505、利用更新的变量Kl+1计算新的梯度：
[0123] 步骤506、计算相邻两轮迭代中梯度差量yl＝gl+1‑gl和变量差量sl＝Kl+1‑Kl；
[0124] 步骤507、使用BFGS算法，结合梯度差量和变量差量更新Hessian矩阵近似值Bl+1，用于下一步迭代：
[0125]
[0126] 步骤508、当梯度的范数‖gl+1‖或者变量的变化量‖Kl+1‑Kl‖小于设定阈值，反解目标函数最小值，得到最优解Φ1＝arcsin(K1)和Φ2＝arcsin(K2)并返回；否则，l自增1，返回步骤503继续迭代。
[0127] 步骤六、将最优俯仰角Φ1和Φ2融入到传感器的量测数据(R,θ,Φ)，并利用卡尔曼滤波得到目标在WGS‑84坐标系下的三维位置；
[0128] 具体步骤为：
[0129] 步骤601、设置目标在WGS‑84坐标系下的初始位置估计 和状态估计协方差矩阵P0；
[0130] 步骤602、基于k‑1时刻目标位置的状态向量和估计协方差矩阵，分别预测k时刻的目标位置状态向量 和估计协方差矩阵Pk∣k‑1：
[0131]
[0132] Pk∣k‑1＝FPk‑1∣k‑1FT+Qk；
[0133] k初始值取1，Qk为过程噪声协方差矩阵，F是状态转移矩阵。
[0134] 步骤603、在k时刻分别融合两个传感器的量测信息，进行目标位置状态向量 和协方差矩阵Pk∣k的更新；
[0135] 先融合传感器A的量测信息进行目标预测位置的更新：
[0136] 首先，计算状态的非线性观测模型的雅可比矩阵:
[0137] h1(x)为传感器A的观测矩阵；
[0138] 然后，利用雅可比矩阵计算卡尔曼增益：
[0139]
[0140] R1为传感器A的测量噪声协方差矩阵；
[0141] 利用卡尔曼增益更新目标位置的状态向量和协方差矩阵：
[0142]
[0143] z'1＝(R1,θ1,Φ1)为传感器A的量测数据；I是单位矩阵；
[0144] 利用传感器A的量测信息进行目标预测位置的更新后，继续融合传感器B的量测信息进行目标预测位置的更新：
[0145] 首先，计算状态的非线性观测模型的雅可比矩阵:
[0146] h2(x)为传感器B的观测矩阵；
[0147] 然后，利用雅可比矩阵计算卡尔曼增益:
[0148]
[0149] R2为传感器B的测量噪声协方差矩阵；
[0150] 最后，利用卡尔曼增益更新目标位置的状态向量和协方差矩阵：
[0151]
[0152] z'2＝(R2,θ2,Φ2)为传感器B的量测数据；
[0153] 步骤604、判断k时刻是否为最终时刻，如果是，则输出k时刻的目标在WGS‑84坐标系下的三维位置，结束；否则k自增1，返回步骤602，对下一时刻的量测数据进行融合处理；
[0154] 步骤605、所有时刻的量测数据融合结果，作为目标的三维位置随时间的变化轨迹，即得到目标的定位跟踪。
[0155] 实施例：
[0156] 步骤一、假设俯仰角量测，计算目标在WGS‑84坐标系的位置；
[0157] 两坐标双基雷达系统由传感器A和传感器B对空中目标进行跟踪，其中目标和雷达均处于如图2所示的移动状态，两个传感器得到量测信息为雷达惯性坐标系下的径向距离和方位角，即R和θ。由于量测噪声的存在，两个传感器对径向距离和方位角的量测会产生偏差ΔR和Δθ。
[0158] 在地心点坐标系下，传感器的位置分别表示为(Ex1,Ey1,Ez1)和(Ex2,Ey2,Ez2)，在地面点坐标系下传感器的位置分别为(L1,B1,H1)和(L2,B2,H2)。
[0159] 两个传感器仅能测得目标相对于传感器阵面中心的径向距离和方位角，传感器A、B获得的雷达球坐标系下量测信息可表示为(R1,θ1)和(R2,θ2)，俯仰角Φ1和Φ2为未知量，目标在传感器A的雷达直角坐标系中可表示为(x1,y1,z1)，同理目标在传感器B的雷达直角坐标系中可表示为(x2,y2,z2)。
[0160] 目标在WGS‑84地心点坐标系下的位置坐标为
[0161]
[0162] 步骤二、联立目标位置等式，建立目标函数。
[0163] 雷达球坐标系和雷达直角坐标系中的转换关系为
[0164] x＝Rcos(Φ)cos(θ) (2)
[0165] y＝Rcos(Φ)sin(θ) (3)
[0166] z＝Rsin(Φ) (4)
[0167] 其中，由于Φ1Φ2均为未知量，因此(x1,y1,z1)和(x2,y2,z2)也为未知量。
[0168] 将式(1)展开并化简可得
[0169]
[0170] 其中，
[0171] ak＝‑Rk cos(θk)sin(Lk) (6)
[0172] bk＝‑Rk sin(θk)cos(Lk)sin(Bk) (7)
[0173] ck＝Rk cos(Lk)cos(Bk) (8)
[0174] dk＝Rk cos(θk)cos(Lk) (9)
[0175] Ek＝Rk sin(θk)sin(Lk)sin(Bk) (10)
[0176] fk＝Rk sin(Lk)cos(Bk) (11)
[0177] Gk＝Rk sin(θk)cos(Bk) (12)
[0178] Jk＝Rk sin(Bk) (13)
[0179] Mk＝Exk (14)
[0180] Nk＝Eyk (15)
[0181] Tk＝Ezk (16)
[0182] 其中，k取不同值表示不同的传感器。
[0183] 式(2)可表示为如下等式
[0184] a1cos(Φ1)+b1cos(Φ1)+c1sin(Φ1)+M1＝a2cos(Φ2)+b2cos(Φ2)+c2sin(Φ2)+M2(17)
[0185] d1cos(Φ2)+E1cos(Φ1)+f1sin(Φ1)+N1＝d2cos(Φ2)+E2cos(Φ2)+f2sin(Φ2)+N2 (18)
[0186] G1cos(Φ1)+J1sin(Φ1)+T1＝G2cos(Φ2)+J2sin(Φ2)+T2 (19)
[0187] 通过构造目标函数将定位问题转化为最小化两个雷达分别推导的目标位置的差异。通过求解两部雷达推导出的目标位置坐标的差值，通过最小化差值的平方和，找到使得式(7)最接近成立的Φ1和Φ2的值。
[0188] 目标函数构建如下：
[0189]
[0190] 其中，
[0191] Kk＝sinΦk (21)
[0192]
[0193] Ak＝ak+bk (23)
[0194] Dk＝dk+Ek (24)
[0195] 因为Φ1和Φ2的取值范围为 所以cosΦ1和cosΦ2都取正值。
[0196] 步骤三、基于牛顿迭代法进行非线性优化，计算俯仰角量测信息；
[0197] 如图3所示，通过牛顿迭代法求解非线性优化问题，得到俯仰角的局部最优值：首先，定义目标函数，设置初始猜测并计算初始梯度和初始Hessian矩阵，之后计算搜索方向，在搜索方向上进行线搜索，找到最优步长，更新变量值，计算新的梯度，计算梯度差量，更新Hessian矩阵，检查收敛条件，当梯度的范数或者变量的变化量小于设定阈值，算法收敛，停止迭代；否则，返回继续迭代，优化算法收敛后，返回最优解。
[0198] 具体过程为：
[0199] (1)初始化
[0200] 初始点：K0＝[0.1,0.1]
[0201] 初始梯度：
[0202] 初始Hessian矩阵近似：B0＝I
[0203] (2)计算搜索方向
[0204] 使用初始Hessian矩阵近似值Bl和梯度gl计算搜索方向
[0205] l初始值为0；
[0206] (3)线搜索
[0207] 在搜索方向pl上进行线搜索，找到最优步长αl，使得目标函数F(Kl+αlpl)取得最小值。
[0208] (4)更新变量
[0209] 利用最优步长αl在搜索方向上更新变量Kl+1＝Kl+αlpl；
[0210] (5)计算新梯度
[0211] 计算新的梯度：
[0212] (6)计算差量
[0213] 计算梯度差量yl＝gl+1‑gl和变量差量sl＝Kl+1‑Kl
[0214] (7)更新Hessian矩阵近似
[0215] 使用BFGS更新公式更新Hessian矩阵近似Bl+1：
[0216]
[0217] (8)：检查收敛条件
[0218] 当梯度的范数‖gl+1‖或者变量的变化量‖Kl+1‑Kl‖小于设定阈值，算法收敛，得到最优解Φ1＝arcsin(K1)和Φ2＝arcsin(K2)并返回停止迭代；否则，l自增1，返回步骤503继续迭代。
[0219] (9)运行优化算法
[0220] 配置完成后，使用MATLAB运行优化算法，逐步逼近目标函数的最小值。
[0221] 步骤四、利用卡尔曼滤波(EKF)融合两个雷达的量测数据(R,θ,Φ)，得到目标在WGS‑84坐标系下的位置；
[0222] 如图4所示，融合滤波具体步骤如下：
[0223] (1)初始化：设置初始位置估计 和状态估计协方差矩阵P0；
[0224] (2)预测步骤：
[0225] 预测位置状态：
[0226] 预测协方差：Pk∣k‑1＝FPk‑1∣k‑1FT+Qk
[0227] (3)更新步骤：
[0228] 通过分别融合雷达A和B的量测信息进行目标预测位置的更新：
[0229] (4)输出：
[0230] 返回当前时刻融合后的目标位置估计 和协方差矩阵Pk∣k。
[0231] (5)循环执行：
[0232] 返回步骤2，对下一时刻的量测数据进行融合处理，直至所有时间点的量测数据融合结束。
