Two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization

Abstract

The present invention discloses a two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization, which belongs to the field of radar signal processing. Specifically, for a scenario in which two sensors measure the same target, the two-dimensional coordinates of the target in the radar spherical coordinate system are first obtained. Then, virtual pitch angles of the two sensors to the target are set to Φ1 and Φ2 respectively, and the two-dimensional coordinates are converted into virtual three-dimensional coordinates in the radar rectangular coordinate system. The two-dimensional coordinates are further converted into the WGS-84 coordinate system to obtain a relationship equation between the pitch angles Φ1 and Φ2. Based on this, the difference in the coordinate positions of the target in the two sensors is calculated, and the sum of the squares of the differences is minimized as an objective function. The objective function is nonlinearly optimized using the Newton iteration method to solve the optimal pitch angles Φ1 and Φ2. Finally, the optimal pitch angles Φ1 and Φ2 are incorporated into the measurement data (R, θ, Φ) of the sensors, and the three-dimensional position of the target in the WGS-84 coordinate system is obtained using Kalman filtering. The present invention significantly improves the positioning and tracking accuracy.

Description

Technical Field

[0001] The present invention belongs to the field of radar signal processing, and in particular to a two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization, which is used to calculate missing radar pitch angle measurements and improve the accuracy of positioning and tracking results. Background Art

[0002] The Newton-Raphson method is a nonlinear optimization algorithm that is used to solve nonlinear systems of equations or optimization problems. It originated from the Newton-Raphson method and was originally used to solve the roots of single- or multivariable equations. It was later extended to the field of nonlinear optimization.

[0003] When approaching the optimal solution, Newton's method has quadratic convergence, that is, the error of each iteration is proportional to the square of the error of the previous iteration. This method can handle high-dimensional optimization problems. It relies on the gradient and Hessian matrix and can utilize geometric information of the objective function.

[0004] The Extended Kalman Filter (EKF) is the most commonly used state estimation method in nonlinear systems. It is often used for multi-sensor data fusion and state estimation in dynamic systems. The EKF can be considered a nonlinear extension of the standard Kalman Filter (KF). It linearizes nonlinear systems, enabling the Kalman filter to be applied in nonlinear scenarios. Summary of the Invention

[0005] To address the problem of missing radar pitch angle measurements, the present invention proposes a two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization in a three-dimensional state space. The method more accurately calculates the radar's pitch angle measurement information of the target, thereby improving the accuracy of the positioning and tracking results.

[0006] The specific steps are as follows:

[0007] Step 1: For a scenario where two sensors A and B measure the same target, obtain the two-dimensional coordinates of the target in the radar spherical coordinate system;

[0008] X1=(R1,θ1)

[0009] X2=(R2,θ2)

[0010] X1 is the two-dimensional coordinate of the target measured by sensor A, and X2 is the two-dimensional coordinate of the target measured by sensor B; (R1, θ1) represents the radial distance and azimuth of the target relative to the center of the sensor A array; (R2, θ2) represents the radial distance and azimuth of the target relative to the center of the sensor B array;

[0011] Step 2: Set the virtual pitch angles of the two sensors to the target to Φ1 and Φ2 respectively, and obtain the virtual three-dimensional coordinates of the target in the radar rectangular coordinate system based on the conversion relationship between the radar spherical coordinate system and the radar rectangular coordinate system;

[0012] The virtual three-dimensional coordinates of the target measured by sensor A are:

[0013] x1=R1 cos(Φ1)cos(θ1)

[0014] y1=R1 cos(Φ1)sin(θ1)

[0015] z1=R1 sin(Φ1)

[0016] The virtual three-dimensional coordinates of the target measured by sensor B are:

[0017] x2=R2cos(Φ2)cos(θ2)

[0018] y2=R2cos(Φ2)sin(θ2)

[0019] z2=R2 sin(Φ2)

[0020] Step 3: Convert the virtual 3D position of the target to the WGS-84 coordinate system and calculate the relationship between the pitch angles Φ1 and Φ2 of the two sensors to the target.

[0021] First, the target position in the radar rectangular coordinate system is converted to the WGS-84 coordinate system. The converted target position is:

[0022]

[0023] Where (Ex1, Ey1, Ez1) represents the position of sensor A in the geocentric coordinate system, (Ex2, Ey2, Ez2) represents the position of sensor B in the geocentric coordinate system, (L1, B1) represents the position of sensor A in the ground coordinate system, (L2, B2) represents the position of sensor B in the ground coordinate system, (x1, y1, z1) represents the position coordinates of the target in the radar rectangular coordinate system of sensor A, and (x2, y2, z2) represents the position coordinates of the target in the radar rectangular coordinate system of sensor B.

[0024] Then, the target is converted to a virtual three-dimensional position in the WGS-84 coordinate system and simplified to obtain the relationship between the two pitch angle measurements Φ1 and Φ2:

[0025]

[0026] The calculation of each intermediate variable value is as follows:

[0027] ak =-R k cos(θ k )sin(L k );b k =-R k sin(θ k )cos(L k )sin(B k );c k =R k cos(L k )cos(B k );

[0028] d k =R k cos(θ k )cos(L k );E k =R k sin(θ k )sin(L k )sin(B k );f k =R k sin(L k )cos(B k );

[0029] G k =R k sin(θ k )cos(B k );J k =R k sin(B k );M k =Ex k ; N k =Ey k ;T k =Ez k ;

[0030] Where k = 1, 2 represents sensors A and B respectively;

[0031] Step 4: Based on the relationship equation between the pitch angles Φ1 and Φ2 of the two sensors to the target, calculate the difference in the coordinate position of the target on the two sensors, and minimize the sum of the squares of the differences as the objective function;

[0032] The objective function is constructed as follows:

[0033]

[0034] Among them, the parameters are calculated as follows: K k =sinΦ k ; A k =a k +b k ;D k =d k +E k ;

[0035] Because the value range of Φ1 and Φ2 is Therefore, both cosΦ1 and cosΦ2 take positive values.

[0036] Step 5: Perform nonlinear optimization on the objective function based on the Newton iteration method to solve the optimal pitch angles Φ1 and Φ2;

[0037] The specific steps are as follows:

[0038] Step 501: Set the initial variable K l =[K1,K2]=[0.1,0.1], initial gradient value Hessian matrix B l The initial approximation is the identity matrix I;

[0039] Step 502: Use the Hessian matrix approximation B l and the gradient g l Calculate search direction

[0040] The initial value of l is 0;

[0041] Step 503: In the search direction p l Perform a line search on the top to find the optimal step size α l , and obtain the objective function F(K l +α l p l ) minimum value.

[0042] Step 504: Use the optimal step size α l Update the variable K in the search direction l+1 =K l +α l p l ;

[0043] Step 505: Use the updated variable K l+1 Compute the new gradient:

[0044] Step 506: Calculate the gradient difference y between two adjacent iterations l =g l+1 -g l and variable difference s l =K l+1 -K l ;

[0045] Step 507: Use the BFGS algorithm to update the Hessian matrix approximation B by combining the gradient difference and the variable difference. l+1 , used for the next iteration:

[0046]

[0047] Step 508: When the norm of the gradient ‖g l+1 ‖ or the change in the variable ‖K l+1 -K l If ‖ is less than the set threshold, the inverse solution of the objective function is performed to obtain the optimal solution Φ1 = arcsin(K1) and Φ2 = arcsin(K2) and return the result; otherwise, l is incremented by 1 and the process returns to step 503 to continue the iteration.

[0048] Step 6: Incorporate the optimal pitch angles Φ1 and Φ2 into the sensor's measurement data (R, θ, Φ), and use Kalman filtering to obtain the target's three-dimensional position in the WGS-84 coordinate system.

[0049] The specific steps are:

[0050] Step 601: Set initial position estimate and the state estimation covariance matrix P0;

[0051] Step 602: Based on the state vector of the target position at time k-1 and the estimated covariance matrix, the target position state vector at time k is predicted. and the estimated covariance matrix P k∣k-1 :

[0052]

[0053] P k∣k-1 =FP k-1∣k-1 F T +Q k ;

[0054] The initial value of k is 1, Q k is the process noise covariance matrix, and F is the state transfer matrix.

[0055] Step 603: At time k, the measurement information of the two sensors is fused to obtain the target position state vector and the covariance matrix P k∣k Updates;

[0056] First, integrate the measurement information of sensor A to update the target predicted position:

[0057] First, calculate the Jacobian matrix of the nonlinear observation model of the state:

[0058] h1(x) is the measurement matrix of sensor A;

[0059] Then, the Kalman gain is calculated using the Jacobian matrix:

[0060]

[0061] R1 is the measurement noise covariance matrix of sensor A;

[0062] Use the Kalman gain to update the state vector and covariance matrix of the target position:

[0063]

[0064] z'1=(R1,θ1,Φ1) is the measurement data of sensor A; I is the unit matrix;

[0065] After using the measurement information of sensor A to update the target predicted position, continue to fuse the measurement information of sensor B to update the target predicted position:

[0066] First, calculate the Jacobian matrix of the nonlinear observation model of the state:

[0067] h2(x) is the measurement matrix of sensor B;

[0068] Then, the Kalman gain is calculated using the Jacobian matrix:

[0069]

[0070] R2 is the measurement noise covariance matrix of sensor B;

[0071] Finally, the Kalman gain is used to update the state vector and covariance matrix of the target position:

[0072]

[0073] z'2=(R2,θ2,Φ2) is the measurement data of sensor B;

[0074] Step 604: Determine whether time k is the final time. If so, output the three-dimensional position of the target in the WGS-84 coordinate system at time k, and the process ends. Otherwise, k is incremented by 1, and the process returns to step 602 to perform fusion processing on the measurement data at the next time.

[0075] Step 605: The measurement data at all times are fused and used as the trajectory of the target's three-dimensional position changing over time, thereby obtaining the target's positioning and tracking.

[0076] The advantages of the present invention are:

[0077] 1) A two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization. Compared with the traditional cross-positioning method, the present invention comprehensively considers the influence of the reference coordinate system and the earth's curvature on positioning and tracking, and combines EKF fusion filtering to make fuller use of the radar's measurement information, thereby significantly improving the positioning and tracking accuracy.

[0078] 2) A two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization. Compared with the traditional pitch angle solution method, the present invention converts the pitch angle solution into a nonlinear optimization problem, solving the problem that the accurate solution cannot be obtained by analytical methods due to the complexity and nonlinearity of the mathematical model. The local optimal solution is obtained, which meets the practical application of positioning and tracking. Therefore, while calculating the pitch angle measurement information of the target, the system is guaranteed to have good positioning and tracking accuracy for the target.

[0079] 3) A two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization. Compared with traditional single-radar positioning and tracking methods and multi-radar positioning and tracking methods, the present invention overcomes the shortcomings of a single radar that can only locate the target on a two-dimensional plane due to the lack of altitude information, or requires additional speed information or position information to locate the target in three-dimensional space, and the problem that a multi-radar system needs to process and fuse data from multiple radar sites in real time, which increases the system's delay and computational burden. Only the azimuth and distance information of two radars are required to obtain the three-dimensional positioning of the target in real time, which is simpler and more efficient. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 This is a flow chart of the two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization of the present invention;

[0081] Figure 2 This is a schematic diagram of the principle of obtaining target measurement information in radar inertial coordinates according to the present invention;

[0082] Figure 3 This is a flow chart of the present invention for solving nonlinear optimization problems based on the Newton iteration method.

[0083] Figure 4 This is a flow chart of the algorithm for obtaining positioning and tracking results based on EKF fusion filtering in the present invention. DETAILED DESCRIPTION

[0084] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0085] Based on the nonlinear optimization theory, this paper proposes a two-coordinate radar bistatic positioning and tracking method to estimate the three-dimensional position of the target based on azimuth and distance. Figure 1 The specific steps are as follows:

[0086] Step 1: For a scenario where two sensors A and B measure the same target, obtain the two-dimensional coordinates of the target in the radar spherical coordinate system;

[0087] X1=(R1,θ1)

[0088] X2=(R 2, θ2)

[0089] X1 is the two-dimensional coordinate of the target measured by sensor A, and X2 is the two-dimensional coordinate of the target measured by sensor B; (R1, θ1) represents the radial distance and azimuth of the target relative to the center of the sensor A array; (R2, θ2) represents the radial distance and azimuth of the target relative to the center of the sensor B array;

[0090] Step 2: Set the virtual pitch angles of the two sensors to the target to Φ1 and Φ2 respectively, and obtain the virtual three-dimensional coordinates of the target in the radar rectangular coordinate system based on the conversion relationship between the radar spherical coordinate system and the radar rectangular coordinate system;

[0091] The virtual three-dimensional coordinates of the target measured by sensor A are:

[0092] x1=R1cos(Φ1)cos(θ1)

[0093] y1=R1cos(Φ1)sin(θ1)

[0094] z1=R1 sin(Φ1)

[0095] The virtual three-dimensional coordinates of the target measured by sensor B are:

[0096] x2=R2cos(Φ2)cos(θ2)

[0097] y2=R2cos(Φ2)sin(θ2)

[0098] z2=R2 sin(Φ2)

[0099] Step 3: Convert the virtual 3D position of the target to the WGS-84 coordinate system and calculate the relationship between the pitch angles Φ1 and Φ2 of the two sensors to the target.

[0100] First, the target position in the radar rectangular coordinate system is converted to the WGS-84 coordinate system. The converted target position is:

[0101]

[0102] Where (Ex1, Ey1, Ez1) represents the position of sensor A in the geocentric coordinate system, (Ex2, Ey2, Ez2) represents the position of sensor B in the geocentric coordinate system, (L1, B1) represents the position of sensor A in the ground coordinate system, (L2, B2) represents the position of sensor B in the ground coordinate system, (x1, y1, z1) represents the position coordinates of the target in the radar rectangular coordinate system of sensor A, and (x2, y2, z2) represents the position coordinates of the target in the radar rectangular coordinate system of sensor B.

[0103] Then, the target is converted to a virtual three-dimensional position in the WGS-84 coordinate system and simplified to obtain the relationship between the two pitch angle measurements Φ1 and Φ2:

[0104]

[0105] The calculation of each intermediate variable value is as follows:

[0106] a k =-R k cos(θ k )sin(L k );b k =-R k sin(θ k )cos(L k )sin(B k );c k =R k cos(L k )cos(B k );

[0107] d k =R k cos(θ k )cos(L k );E k =R k sin(θ k )sin(L k )sin(B k );f k =R k sin(L k )cos(B k );

[0108] G k =R k sin(θ k )cos(B k );J k =R k sin(B k );Mk =Ex k ; N k =Ey k ;T k =Ez k ;

[0109] Where k = 1, 2 represents sensors A and B respectively;

[0110] Step 4: Based on the relationship equation between the pitch angles Φ1 and Φ2 of the two sensors to the target, calculate the difference in the coordinate position of the target on the two sensors, and minimize the sum of the squares of the differences as the objective function;

[0111] The objective function is constructed as follows:

[0112]

[0113] Among them, the parameters are calculated as follows: K k =sinΦ k ; A k =a k +b k ;D k =d k +E k ;

[0114] Because the value range of Φ1 and Φ2 is Therefore, both cosΦ1 and cosΦ2 take positive values.

[0115] Step 5: Perform nonlinear optimization on the objective function based on the Newton iteration method to solve the optimal pitch angles Φ1 and Φ2;

[0116] The specific steps are as follows:

[0117] Step 501: Set the initial variable K l =[K1,K2]=[0.1,0.1], initial gradient value Hessian matrix B l The initial approximation is the identity matrix I;

[0118] Step 502: Use the Hessian matrix approximation B l and the gradient g l Calculate search direction

[0119] The initial value of l is 0;

[0120] Step 503: In the search direction p l Perform a line search on the top to find the optimal step size α l , and obtain the objective function F(K l+α l p l ) minimum value.

[0121] Step 504: Use the optimal step size α l Update the variable K in the search direction l+1 =K l +α l p l ;

[0122] Step 505: Use the updated variable K l+1 Compute the new gradient:

[0123] Step 506: Calculate the gradient difference y between two adjacent iterations l =g l+1 -g l and variable difference s l =K l+1 -K l ;

[0124] Step 507: Use the BFGS algorithm to update the Hessian matrix approximation B by combining the gradient difference and the variable difference. l+1 , used for the next iteration:

[0125]

[0126] Step 508: When the norm of the gradient ‖g l+1 ‖ or the change in the variable ‖K l+1 -K l If ‖ is less than the set threshold, the inverse solution of the objective function is performed to obtain the optimal solution Φ1 = arcsin(K1) and Φ2 = arcsin(K2) and return the result; otherwise, l is incremented by 1 and the process returns to step 503 to continue the iteration.

[0127] Step 6: Incorporate the optimal pitch angles Φ1 and Φ2 into the sensor's measurement data (R, θ, Φ), and use Kalman filtering to obtain the target's three-dimensional position in the WGS-84 coordinate system.

[0128] The specific steps are:

[0129] Step 601: Set the target's initial position estimate in the WGS-84 coordinate system and the state estimation covariance matrix P0;

[0130] Step 602: Based on the state vector of the target position at time k-1 and the estimated covariance matrix, the target position state vector at time k is predicted. and the estimated covariance matrix P k∣k-1 :

[0131]

[0132] P k∣k-1 =FP k-1∣k-1 F T +Q k ;

[0133] The initial value of k is 1, Q k is the process noise covariance matrix, and F is the state transfer matrix.

[0134] Step 603: At time k, the measurement information of the two sensors is fused to obtain the target position state vector and the covariance matrix P k∣k Updates;

[0135] First, integrate the measurement information of sensor A to update the target predicted position:

[0136] First, calculate the Jacobian matrix of the nonlinear observation model of the state:

[0137] h1(x) is the measurement matrix of sensor A;

[0138] Then, the Kalman gain is calculated using the Jacobian matrix:

[0139]

[0140] R1 is the measurement noise covariance matrix of sensor A;

[0141] Use the Kalman gain to update the state vector and covariance matrix of the target position:

[0142]

[0143] z'1=(R1,θ1,Φ1) is the measurement data of sensor A; I is the unit matrix;

[0144] After using the measurement information of sensor A to update the target predicted position, continue to fuse the measurement information of sensor B to update the target predicted position:

[0145] First, calculate the Jacobian matrix of the nonlinear observation model of the state:

[0146] h2(x) is the measurement matrix of sensor B;

[0147] Then, the Kalman gain is calculated using the Jacobian matrix:

[0148]

[0149] R2 is the measurement noise covariance matrix of sensor B;

[0150] Finally, the Kalman gain is used to update the state vector and covariance matrix of the target position:

[0151]

[0152] z'2=(R2,θ2,Φ2) is the measurement data of sensor B;

[0153] Step 604: Determine whether time k is the final time. If so, output the three-dimensional position of the target in the WGS-84 coordinate system at time k, and the process ends. Otherwise, k is incremented by 1, and the process returns to step 602 to perform fusion processing on the measurement data at the next time.

[0154] Step 605: The measurement data at all times are fused and used as the trajectory of the target's three-dimensional position changing over time, thereby obtaining the target's positioning and tracking.

[0155] Example:

[0156] Step 1: Assuming the pitch angle is measured, calculate the position of the target in the WGS-84 coordinate system;

[0157] The two-coordinate bistatic radar system tracks an aerial target using sensor A and sensor B, where both the target and the radar are in the following positions: Figure 2 In the moving state shown, the two sensors measure radial distance and azimuth in the radar's inertial coordinate system, namely, R and θ. Due to measurement noise, the two sensors' radial distance and azimuth measurements will have deviations of ΔR and Δθ.

[0158] In the geocentric coordinate system, the positions of the sensors are represented as (Ex1, Ey1, Ez1) and (Ex2, Ey2, Ez2), and in the ground coordinate system, the positions of the sensors are (L1, B1, H1) and (L2, B2, H2).

[0159] The two sensors can only measure the radial distance and azimuth of the target relative to the center of the sensor array. The measurement information obtained by sensors A and B in the radar spherical coordinate system can be expressed as (R1, θ1) and (R2, θ2). The pitch angles Φ1 and Φ2 are unknown quantities. The target can be expressed as (x1, y1, z1) in the radar rectangular coordinate system of sensor A. Similarly, the target can be expressed as (x2, y2, z2) in the radar rectangular coordinate system of sensor B.

[0160] The target's position coordinates in the WGS-84 geocentric coordinate system are:

[0161]

[0162] Step 2: Combine the target position equations and establish the objective function.

[0163] The conversion relationship between the radar spherical coordinate system and the radar rectangular coordinate system is:

[0164] x=Rcos(Φ)cos(θ) (2)

[0165] y=Rcos(Φ)sin(θ) (3)

[0166] z=Rsin(Φ) (4)

[0167] Among them, since Φ1Φ2 are both unknown quantities, (x1, y1, z1) and (x2, y2, z2) are also unknown quantities.

[0168] Expanding and simplifying formula (1) yields

[0169]

[0170] in,

[0171] a k =-R k cos(θ k )sin(L k ) (6)

[0172] b k =-R k sin(θ k )cos(L k )sin(B k ) (7)

[0173] c k =R k cos(L k )cos(B k ) (8)

[0174] d k =R k cos(θ k )cos(L k ) (9)

[0175] E k =R k sin(θ k )sin(L k )sin(B k ) (10)

[0176] f k =R k sin(L k )cos(B k ) (11)

[0177] G k =R k sin(θ k )cos(B k ) (12)

[0178] J k =R k sin(B k ) (13)

[0179] M k =Ex k (14)

[0180] N k =Ey k (15)

[0181] T k =Ez k (16)

[0182] Among them, different values ​​of k represent different sensors.

[0183] Formula (2) can be expressed as the following equation

[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] By constructing an objective function, the positioning problem is transformed into minimizing the difference between the target positions derived by the two radars. By solving the difference between the target position coordinates derived by the two radars and minimizing the sum of the squares of the differences, the values ​​of Φ1 and Φ2 that make Equation (7) most likely to hold true are found.

[0188] The objective function is constructed as follows:

[0189]

[0190] in,

[0191] K k =sinΦ k (twenty one)

[0192]

[0193] A k =a k +b k (twenty three)

[0194] D k =d k +E k (twenty four)

[0195] Because the value range of Φ1 and Φ2 is Therefore, both cosΦ1 and cosΦ2 take positive values.

[0196] Step 3: Perform nonlinear optimization based on Newton iteration method to calculate pitch angle measurement information;

[0197] like Figure 3 As shown in the figure, the nonlinear optimization problem is solved by the Newton iteration method to obtain the local optimal value of the pitch angle: first, the objective function is defined, the initial guess is set, and the initial gradient and initial Hessian matrix are calculated. Then, the search direction is calculated, a line search is performed in the search direction, the optimal step size is found, the variable value is updated, the new gradient is calculated, the gradient difference is calculated, the Hessian matrix is ​​updated, and the convergence condition is checked. When the norm of the gradient or the change in the variable is less than the set threshold, the algorithm converges and the iteration stops; otherwise, it returns to continue the iteration. After the optimization algorithm converges, the optimal solution is returned.

[0198] The specific process is:

[0199] (1) Initialization

[0200] Initial point: K0 = [0.1, 0.1]

[0201] Initial gradient:

[0202] Initial Hessian matrix approximation: B0 = I

[0203] (2) Calculate the search direction

[0204] Use the initial Hessian matrix approximation B l and the gradient g l Calculate search direction

[0205] The initial value of l is 0;

[0206] (3) Line search

[0207] In the search direction p l Perform a line search on the top to find the optimal step size α l , so that the objective function F(Kl +α l p l ) to obtain the minimum value.

[0208] (4) Update variables

[0209] Using the optimal step size α l Update the variable K in the search direction l+1 =K l +α l p l ;

[0210] (5) Calculate the new gradient

[0211] Compute the new gradient:

[0212] (6) Calculate the difference

[0213] Calculate the gradient difference y l =g l+1 -g l and variable difference s l =K l+1 -K l

[0214] (7) Update Hessian matrix approximation

[0215] Update the Hessian matrix approximation B using the BFGS update formula l+1 :

[0216]

[0217] (8): Check convergence conditions

[0218] When the norm of the gradient ‖g l+1 ‖ or the change in the variable ‖K l+1 -K l If ‖ is less than the set threshold, the algorithm converges, obtains the optimal solution Φ1 = arcsin(K1) and Φ2 = arcsin(K2), and returns to stop the iteration; otherwise, l is incremented by 1, and returns to step 503 to continue the iteration.

[0219] (9) Run the optimization algorithm

[0220] After the configuration is completed, use MATLAB to run the optimization algorithm to gradually approach the minimum value of the objective function.

[0221] Step 4: Use the EKF to fuse the measurement data (R, θ, Φ) of the two radars to obtain the target position in the WGS-84 coordinate system;

[0222] like Figure 4As shown in Figure 2, the specific steps of fusion filtering are as follows:

[0223] (1) Initialization: Setting the initial position estimate and the state estimation covariance matrix P0;

[0224] (2) Prediction steps:

[0225] Predicted location status:

[0226] Prediction covariance: P k∣k-1 =FP k-1∣k-1 F T +Q k

[0227] (3) Update steps:

[0228] The target predicted position is updated by fusing the measurement information of radars A and B respectively:

[0229] (4) Output:

[0230] Returns the estimated target position after fusion at the current moment and the covariance matrix P k∣k .

[0231] (5) Loop execution:

[0232] Return to step 2 and perform fusion processing on the measurement data at the next moment until the fusion of the measurement data at all time points is completed.

Claims

1. A two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization, characterized in that: The specific steps are as follows: Step 1: For a scenario where two sensors A and B measure the same target, obtain the two-dimensional coordinates of the target in the radar spherical coordinate system; X1=(R 1, θ1) X2=(R 2, θ2) X1 is the two-dimensional coordinate of the target measured by sensor A, and X2 is the two-dimensional coordinate of the target measured by sensor B; (R1, θ1) represents the radial distance and azimuth of the target relative to the center of the sensor A array; (R2, θ2) represents the radial distance and azimuth of the target relative to the center of the sensor B array; Step 2: Set the virtual pitch angles of the two sensors to the target to Φ1 and Φ2 respectively, and obtain the virtual three-dimensional coordinates of the target in the radar rectangular coordinate system based on the conversion relationship between the radar spherical coordinate system and the radar rectangular coordinate system; Step 3: Convert the virtual 3D position of the target to the WGS-84 coordinate system and calculate the relationship between the pitch angles Φ1 and Φ2 of the two sensors to the target. Step 4: Based on the relationship equation between the pitch angles Φ1 and Φ2 of the two sensors to the target, calculate the difference in the coordinate position of the target on the two sensors, and minimize the sum of the squares of the differences as the objective function; The objective function is constructed as follows: Among them, the parameters are calculated as follows: K k =sinΦ k ; A k =a k +b k ;D k =d k +E k ;a k , b k , d k and E k are intermediate variable values; k = 1, 2 represent sensors A and B respectively; because the value range of Φ1 and Φ2 is Therefore, both cosΦ1 and cosΦ2 take positive values; Step 5: Perform nonlinear optimization on the objective function based on the Newton iteration method to solve the optimal pitch angles Φ1 and Φ2; Step 6: Incorporate the optimal pitch angles Φ1 and Φ2 into the sensor's measurement data (R, θ, Φ), and use Kalman filtering to obtain the target's three-dimensional position in the WGS-84 coordinate system.

2. The two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization as claimed in claim 1, characterized in that: In step 2, the virtual three-dimensional coordinates of the target measured by sensor A are: x1=R1cos(Φ1)cos(θ1) y1=R1cos(Φ1)sin(θ1) z1=R1sin(Φ1) The virtual three-dimensional coordinates of the target measured by sensor B are: x2=R2cos(Φ2)cos(θ2) y2=R2cos(Φ2)sin(θ2) z2=R2sin(Φ2) (x1, y1, z1) is the position coordinate of the target in the radar rectangular coordinate system of sensor A, and (x2, y2, z2) is the position coordinate of the target in the radar rectangular coordinate system of sensor B.

3. The two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization as claimed in claim 2, characterized in that: In step 3, first, the target position in the radar rectangular coordinate system is converted to the WGS-84 coordinate system. The converted target position is: Where (Ex1, Ey1, Ez1) represents the position of sensor A in the geocentric coordinate system, (Ex2, Ey2, Ez2) represents the position of sensor B in the geocentric coordinate system, (L1, B1) represents the position of sensor A in the ground coordinate system, and (L2, B2) represents the position of sensor B in the ground coordinate system. Then, the target is converted to a virtual three-dimensional position in the WGS-84 coordinate system and simplified to obtain the relationship between the two pitch angle measurements Φ1 and Φ2: The calculation of each intermediate variable value is as follows: a k =-R k cos(θ k )sin(L k );b k =-R k sin(θ k )cos(L k )sin(B k );c k =R k cos(L k )cos(B k ); d k =R k cos(θ k )cos(L k );E k =R k sin(θ k )sin(L k )sin(B k );f k =R k sin(L k )cos(B k ); G k =R k sin(θ k )cos(B k );J k =R k sin(B k );M k =Ex k (N k =Yes k ;T k =Ez k 。 4. The two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization as claimed in claim 1, characterized in that: The specific steps of step five are as follows: Step 501: Set the initial variable K l =[K1,K2]=[0.1,0.1], initial gradient value g l =▽f([K1,K2]), Hessian matrix B l The initial approximation is the identity matrix I; Step 502: Use the Hessian matrix approximation B l and the gradient g l Calculate search direction The initial value of l is 0; Step 503: In the search direction p l Perform a line search on the top to find the optimal step size α l , and obtain the objective function F(K l +α l p l ) minimum value; Step 504: Use the optimal step size α l Update the variable K in the search direction l+1 =K l +α l p l ; Step 505: Use the updated variable K l+1 Calculate the new gradient: g l+1 =▽F(K l+1 ) Step 506: Calculate the gradient difference y between two adjacent iterations l =g l+1 -g l and variable difference s l =K l+1 -K l ; Step 507: Use the BFGS algorithm to update the Hessian matrix approximation B by combining the gradient difference and the variable difference. l+1 , used for the next iteration: Step 508: When the norm of the gradient || g l+1 ||or the change in the variable||K l+1 -K l || is less than the set threshold, the inverse solution of the objective function minimum value is performed to obtain the optimal solution Φ1 = arcsin(K1) and Φ2 = arcsin(K2) and return; otherwise, l is incremented by 1, and the process returns to step 503 to continue iteration.

5. The two-coordinate radar bistatic positioning and tracking method based on nonlinear optimization as claimed in claim 1, characterized in that: The specific steps of step six are as follows: Step 601: Set initial position estimate and the state estimation covariance matrix P0; Step 602: Based on the state vector of the target position at time k-1 and the estimated covariance matrix, the target position state vector at time k is predicted. and the estimated covariance matrix P k∣k-1 : P k∣k-1 =FP k-1∣k-1 F T +Q k ; The initial value of k is 1, Q k is the process noise covariance matrix, F is the state transfer matrix; Step 603: At time k, the measurement information of the two sensors is fused to obtain the target position state vector and the covariance matrix P k∣k Updates; First, integrate the measurement information of sensor A to update the target predicted position: First, calculate the Jacobian matrix of the nonlinear observation model of the state: h1(x) is the measurement matrix of sensor A; Then, the Kalman gain is calculated using the Jacobian matrix: R1 is the measurement noise covariance matrix of sensor A; Use the Kalman gain to update the state vector and covariance matrix of the target position: P k∣k1 =(I-U1H1)P k∣k-1 z'1=(R1,θ1,Φ1) is the measurement data of sensor A; I is the unit matrix; After using the measurement information of sensor A to update the target predicted position, continue to fuse the measurement information of sensor B to update the target predicted position: First, calculate the Jacobian matrix of the nonlinear observation model of the state: h2(x) is the measurement matrix of sensor B; Then, the Kalman gain is calculated using the Jacobian matrix: R2 is the measurement noise covariance matrix of sensor B; Finally, the Kalman gain is used to update the state vector and covariance matrix of the target position: z'2=(R2,θ2,Φ2) is the measurement data of sensor B; Step 604: Determine whether time k is the final time. If so, output the three-dimensional position of the target in the WGS-84 coordinate system at time k, and the process ends. Otherwise, k is incremented by 1, and the process returns to step 602 to perform fusion processing on the measurement data at the next time. Step 605: The measurement data at all times are fused and used as the trajectory of the target's three-dimensional position changing over time, thereby obtaining the target's positioning and tracking.

Images