Four-quadrant detector spot position detection solving method

Abstract

The present application relates to laser four-quadrant detector signal detection processing technical field, especially four-quadrant detector spot position detection solving method, including optical power total signal filtering and spot center relative position filtering two part processing process;Wherein: in the optical power total signal filtering process, the extended Kalman filter is used to obtain four-quadrant detector optical power estimate and optical power rate of change estimate;In the spot center position filtering process, the four-way electrical signal of four-quadrant detector is decomposed into two parts of the former and the latter, the nonlinear measurement equation of the relative position of the spot center is established, and the relative position of the spot center is estimated;At the same time, for the filtering estimation parameters affected by noise outside the constraint area, the optimal estimation value of the relative position and position rate of the spot center is output after iteration by the probability density function truncation estimation, that is, the constrained iterative extended Kalman filter.

Description

Technical Field

[0001] This invention relates to the field of laser four-quadrant detector signal detection and processing technology, and in particular to a method for detecting and calculating the position of a laser spot in a four-quadrant detector. Background Technology

[0002] Four-quadrant detector laser spot position detection technology is widely used in measurement equipment, space laser communication and laser guidance.

[0003] The four-quadrant detector measures the centroid position of the light spot indirectly. The light emitted by the laser shines on the target, and the detector's optical device receives the reflected light spot from the target. After photoelectric conversion by the detector, four signals are obtained, and the centroid position of the light spot is obtained by algorithm processing.

[0004] To determine the centroid position of the light spot based on the four signals from the detector, several common methods exist, including geometric approximation, database lookup, and polynomial fitting. In the geometric approximation method, the energy distribution of the light spot can be categorized as uniform or Gaussian. Uniform distribution is the most widely used in guidance systems, and the commonly used formula is the sum-difference amplitude ratio formula, which approximates the centroid position of the circular spot. However, in the sum-difference amplitude ratio formula detection method, due to noise and signal-to-noise ratio limitations, optimal detection is difficult to achieve in guidance applications.

[0005] For the database lookup method using matched filters, the position detection accuracy depends on the completeness and size of the constructed data table, as well as the accuracy of linear interpolation. Higher detection accuracy requires more stored data. In actual calibration data table creation and interpolation applications, random noise can also have adverse effects.

[0006] The following problems still exist in the sum-difference ratio formula detection and solution method:

[0007] 1) The accuracy is high when the light spot is located within a small area at the center of the photosensitive surface; the error increases and becomes nonlinear when the light spot is located at the edge of the photosensitive surface. The calculation formula has inherent errors, and it is impossible to accurately measure the position of the light spot throughout the entire process;

[0008] 2) The four measurements of the four-quadrant detector are inevitably affected by photoelectric noise. This noise, through the sum-difference-amplitude formula, introduces random errors in the calculation. The variance is related to the optical field of view, the overall signal-to-noise ratio (SNR), and the spot position. Noise from the four signals is included in the numerator and denominator of the formula. The variance when the spot is located at the edge of the field of view is twice that when it is located at the center. Noise affects the accuracy of position detection, therefore it largely depends on the SNR at the receiver.

[0009] 3) When the signal-to-noise ratio is low, the detection of the edge position of the measurable field of view is significantly affected by noise. If the spot is located at the edge of the measurable field of view, the sum-difference ratio formula value is prone to exceed the saturation value due to noise, i.e., it is outside the constraint range of +1 or -1, making it impossible to complete the position detection of the measurable area. This results in the measurable area that can be detected being smaller than the designed optical field of view. Summary of the Invention

[0010] To address the aforementioned problems, this invention provides a method for calculating the position of a spot in a four-quadrant detector. It decomposes the traditional formula for calculating the centroid of the spot into two parts: a formula for the preceding value and a formula for the following value. It defines an accurate nonlinear measurement relationship between the preceding and following values ​​of the measurement input and the relative position of the spot center. Furthermore, it uses Kalman filtering to minimize the impact of noise on the spot position, improving detection accuracy. Simultaneously, it employs a probability density truncation method based on a normal (Gaussian) distribution in the filtering process to estimate the saturation values ​​of the two input signals within the measurable region due to noise, thereby obtaining the relative position of the circular spot within the measurable region after constraint filtering.

[0011] To achieve the above objectives, the technical solution created by this invention is implemented as follows:

[0012] A method for detecting and calculating the position of a light spot using a four-quadrant detector includes two processing steps: filtering the total optical power signal received by the four-quadrant detector, and filtering the center position of the light spot based on the filtered optical power. The total optical power signal filtering process includes:

[0013] A1: Define the system state vector and system state equation of the four-quadrant detector; determine the measurement equation of the laser power of the four-quadrant detector based on the system state vector;

[0014] A2: Select the full-range guided flight time for parameter identification, obtain the predicted values ​​of the laser power and laser power change rate of the four-quadrant detector, and extend the measurement equation of step A1;

[0015] A3: Based on the measurement equation extended in step A2, the predicted values ​​of laser power and laser power change rate obtained in step A2 are corrected and updated using extended Kalman filtering to obtain the estimated values ​​of optical power and optical power change rate of the four-quadrant detector.

[0016] The filtering process at the center of the light spot includes:

[0017] B1: Determine the discrete state vector of the light spot center; initialize and predict the system state and covariance of the iterative extended Kalman filter based on the discrete state vector;

[0018] B2: Combining the optical power estimate obtained in step A3, the sum-difference ratio formula represented by the four signal values ​​of the four-quadrant detector is decomposed into a pre-value formula and a post-value formula.

[0019] B3: Rewrite the pre-value formula and post-value formula obtained in step B2, and based on the rewritten pre-value formula and post-value formula, use iterative extended Kalman filtering to update and correct the state estimate, and output the optimal state estimate.

[0020] Further, step A1 includes: obtaining the laser power equation of the four-quadrant detector and the corresponding first-order and second-order differential equations based on the receiving principle and parameters of the four-quadrant detector; defining the system state vector and system state equation based on the three equations obtained; linearizing the system state equation to obtain the transition matrix; and then discretizing the transition matrix to obtain the discretized first-order linear transition matrix and the discrete system state vector.

[0021] Furthermore, the laser power equation is:

[0022] ;

[0023] Among them, P t R represents the theoretical optical power of the four-quadrant detector, D represents the receiving optical aperture of the four-quadrant detector, and R represents the theoretical optical power of the four-quadrant detector. M P represents the receiving distance of the four-quadrant detector. L Indicates the peak power of the laser. T represents the target reflectivity. r R represents the combined transmission and reception coefficients of the four-quadrant detector; L σ represents the illumination distance, σ represents the atmospheric attenuation coefficient, and θ represents the angle between the target surface normal and the laser receiving direction.

[0024] By performing two differential operations on the laser power equation, we obtain the first-order and second-order differential equations:

[0025] ;

[0026] ;

[0027] in, P represents the theoretical value of optical power. t The first-order differential value, P represents the theoretical value of optical power. t The second derivative value, t represents the rate of change of receiving distance. go Indicates the remaining flight time for guidance, t go =t0-t n , t nt0 represents the guided flight time, f1 represents the first-order differential function, and f2 represents the second-order differential function.

[0028] Furthermore, the process of obtaining the discretized first-order linear transition matrix and the discrete system state vector in step A1 includes:

[0029] make , Then we have the system state vector. and system state equations ;

[0030] For the system state equation matrix Linearization yields the transition matrix;

[0031] Perform an inverse Laplace transform on the transition matrix, take its first-order linear form, and then discretize it according to the sampling time to obtain the first-order linear transition matrix, simultaneously obtaining the discrete system state vector. Discrete system state estimation vector , This represents the state estimate of the discrete system at time k. This represents the rate of change of the discrete system state estimate at time k.

[0032] Furthermore, step A1 also includes: using the sum of the measured values ​​of the optical power in each quadrant of the four-quadrant detector as the input to the measurement equation;

[0033] The measurement equation is:

[0034] ;

[0035] in, H represents the measured value of the optical power of the four-quadrant detector including noise, H=[0,1] represents the measurement equation transfer matrix, and v represents the measurement noise.

[0036] Furthermore, step A2 includes:

[0037] The entire guided flight time is sampled and discretized using the following formula:

[0038] ;

[0039] Among them, t 0k Let k represent the discretized time of the entire guided flight at time k. The full-range guided flight time t at time k-1 is represented. 0k-1 Zero-mean Gaussian white noise;

[0040] The discrete system state vector and measured values ​​are extended using the following formula:

[0041] ;

[0042] ;

[0043] in, Indicates the measured value The discretized measurement at time k, Φ k-1 This represents the first-order linear transition matrix at time k-1;

[0044] Based on the discrete system state vector at time k-1 The full-range guided flight time t at time k-1 0k-1 The predicted values ​​of laser power and laser power change rate are obtained by moving forward one sampling step:

[0045] ;

[0046] ;

[0047] ;

[0048] ;

[0049] Where, x k-1 This represents the laser power value at time k-1. This represents the rate of change of the laser power value at time k-1. This represents the rate of change of the laser power at time k-1. Indicates the receiving distance R M The discrete value at time k-1, T s Indicates the sampling time, x k|k-1 This represents the predicted value of the laser power. The predicted value representing the rate of change of laser power.

[0050] Furthermore, the update process in step A3 is detailed below:

[0051] For different times k, the discrete system state estimation vector is calculated using the following formula. And estimate the entire guided flight time :

[0052] ;

[0053] in, ;

[0054] Based on the first-order linear transition matrix Φ at time k-1 k-1 State covariance matrix P k-1 and zero-mean Gaussian white noise variance The predicted state covariance matrix P is obtained through the following formula. k|k-1 :

[0055] ;

[0056] This leads to the Kalman gain matrix K. k :

[0057] ;

[0058] Through the Kalman gain array K k and the predicted state covariance matrix P k-1|k It can be calculated by the following formula. k The state covariance matrix P at time t k :

[0059] ;

[0060] Where I represents the identity matrix;

[0061] The data should be updated and corrected according to the following formula:

[0062] ;

[0063] At this point, the estimated optical power of the four-quadrant detector is obtained. Estimated rate of change of optical power .

[0064] Furthermore, in step B1, the relative position x of the light spot center on the target surface of the four-quadrant detector is used. r ,speed and acceleration The state vector at the center of the light spot ;

[0065] The discrete state vector of the light spot center is obtained by the following formula. :

[0066] ;

[0067] Among them, w k-1 Let A represent the process noise at time k-1, and let A represent the state transition matrix.

[0068] .

[0069] Furthermore, in step B2, the sum-difference ratio formula z r for:

[0070] ;

[0071] Where p1, p2, p3, and p4 represent the power of the four signals of the four-quadrant detector; combined with the estimated optical power of the four-quadrant detector, the sum-difference amplitude formula z is... r Decomposition yields the formula for the previous value z. ra The formula for sum and aftervalue z rb :

[0072] ;

[0073] .

[0074] Furthermore, step B3 includes:

[0075] Based on the relative position x of the light spot center r The nonlinear theoretical relationship between the ideal measurement input value of the four-quadrant detector and the relative position of the center of the uniform light spot is obtained by formula g. ra (x r ) and g rb (x r )for:

[0076] ;

[0077] ;

[0078] The previous value formula z ra The formula for sum and aftervalue z rb Rewritten as:

[0079] ;

[0080] ;

[0081] Where, λ t P represents the theoretical value of optical power. t Compared with optical power estimates The ratio of the theoretical value of optical power P t v is the sum of the noise-free theoretical values ​​of the power of the four signals. a v represents the sum of the noise in the first quadrant and the noise in the fourth quadrant of a four-quadrant detector. b This represents the sum of the noise in the second quadrant and the noise in the third quadrant of a four-quadrant detector;

[0082] Select the relative position x of the discretized spot center rk ,speed and acceleration and parameter λ t discrete value λ tk , as the new state vector The data is updated and corrected using the following formula:

[0083] ;

[0084] in, Res represents the updated estimate of the new state vector. ak and Res bk These represent the rewritten discretization pre-value formula z. rak The formula for sum and aftervalue z rbk The corresponding estimated residual in the iterative extended Kalman filter, K ak and K bk The formulas for the previous value and the values ​​in the iterative extended Kalman filter are respectively: rak The formula for sum and aftervalue z rbk The corresponding Kalman gain.

[0085] Furthermore, estimate the residuals Res ak and Res bk The acquisition process includes:

[0086] For the nonlinear theoretical relation formula g ra (x r ) and g rb (x r Discretize the expression to obtain the discrete form g. rak (x rk ) and g rbk (x rk );

[0087] Based on discrete g rak (x rk ) and g rbk (x rk ), thus obtaining the Jacobian matrix of the measurement equation corresponding to the previous value formula. The measurement equation corresponding to the aftervalue formula is the Jacobian matrix. ;

[0088] Using the state estimate, the estimated residual Res in the iterative extended Kalman filter is obtained by the following formula. ak and Res bk :

[0089] ;

[0090] ;

[0091] in, Indicates the relative position x of the light spot center r The estimated value at time k, Indicates the parameter λ t The estimated value at time k;

[0092] Select parameter λ t At time k, the parameter λtk As a random constant:

[0093] ;

[0094] in, This represents zero-mean Gaussian white noise.

[0095] Furthermore, step B3, which uses iterative extended Kalman filtering to update and correct the state estimates, also includes:

[0096] Determine if the relative position of the light spot center is within the constraint range [-1, +1]. If it is, directly perform iterative extended Kalman filtering; otherwise, truncate the Gaussian probability density function of the state estimate and its corresponding covariance, iteratively calculate the state mean and covariance of the truncated probability density function, and assign the truncated state mean to the state estimate.

[0097] Compared with the prior art, the present invention can achieve the following beneficial effects:

[0098] (1) After introducing the total signal filtering of the optical power of the four-quadrant detector, the present invention has only two noise inputs for each measurement formula of the first value and the last value, the four noises of the ratio, difference and amplitude formulas are less affected, and the noise variance of the whole field of view is consistent.

[0099] (2) The present invention uses Kalman filtering of a nonlinear measurement model to minimize the influence of noise on the center position of the light spot, thereby improving the detection accuracy;

[0100] (3) The present invention adopts the probability density truncation method of normal (Gaussian) distribution. The saturation value of two input signals with the center position of the light spot in the measurable region [-1,1] is truncated and estimated due to noise, so as to obtain the position of the center of the light spot in the measurable region, so that the size of the measurable region of the detector is the same as the designed optical field of view. Attached Figure Description

[0101] Figure 1 This is a logic block diagram of the four-quadrant detector spot position detection and calculation method provided in the embodiment of the present invention;

[0102] Figure 2 This is a flowchart of the total optical power signal filtering process provided in an embodiment of the present invention;

[0103] Figure 3 This is a flowchart of the spot center position filtering process provided in an embodiment of the present invention;

[0104] Figure 4 This is an overall logic diagram of the four-quadrant detector spot position detection and calculation method provided in the embodiments of the present invention. Detailed Implementation

[0105] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and do not constitute a limitation thereof. It should be noted that, unless otherwise specified, the embodiments and features described herein can be combined with each other.

[0106] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicating orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on this invention. Furthermore, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, features defined with "first," "second," etc., may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more.

[0107] The invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0108] like Figure 1 As shown, the proposed method for detecting and calculating the spot position of a four-quadrant detector includes two processing steps: filtering the total optical power signal received by the four-quadrant detector and filtering the spot center position based on the filtered optical power. The total optical power signal filtering process is implemented using an extended Kalman filter, constructing a state model describing the changes in laser power received by the four-quadrant detector and predicting state estimates; extending the entire guidance flight time as the state, adaptively identifying it, and completing the extended Kalman filtering of the total optical power signal. The spot center position filtering process is implemented using an iterative extended Kalman filter. The iterative extended Kalman filter receives the measurement parameters from the four-quadrant detector, combines them with the optical power estimate, and defines a nonlinear measurement relationship between the previous and subsequent values ​​of the measurement input and the relative position of the spot center; using the relative position, velocity, and acceleration of the spot center as the state vector in the target tracking model, the iterative extended Kalman filtering is completed.

[0109] like Figure 2 As shown, the total optical power signal filtering process specifically includes the following steps:

[0110] A1: Define the system state vector and system state equation of the four-quadrant detector; determine the measurement equation of the laser power of the four-quadrant detector based on the system state vector.

[0111] In some embodiments, step A1 includes:

[0112] Based on the receiving principle and parameters of the four-quadrant detector, the laser power equation and the corresponding first-order and second-order differential equations of the four-quadrant detector are obtained. The laser power equation is as follows:

[0113] ;

[0114] Among them, P t R represents the theoretical optical power of the four-quadrant detector, D represents the receiving optical aperture of the four-quadrant detector, and R represents the theoretical optical power of the four-quadrant detector. M P represents the receiving distance of the four-quadrant detector. L Indicates the peak power of the laser. T represents the target reflectivity. r R represents the combined transmission and reception coefficients of the four-quadrant detector; L σ represents the illumination distance, σ represents the atmospheric attenuation coefficient, and θ represents the angle between the target surface normal and the laser receiving direction.

[0115] By performing two differential operations on the laser power equation, we obtain the first-order and second-order differential equations:

[0116] ;

[0117] ;

[0118] in, P represents the theoretical value of optical power. t The first-order differential value, P represents the theoretical value of optical power. t The second derivative value, t represents the rate of change of receiving distance. go Indicates the remaining flight time for guidance, t go =t0-t n , t n t0 represents the guided flight time, f1 represents the first-order differential function, and f2 represents the second-order differential function.

[0119] Based on the three equations obtained, a system state vector and system state equations are defined; the system state equations are linearized to obtain a transition matrix; then the transition matrix is ​​discretized to obtain a discretized first-order linear transition matrix and a discrete system state vector. In some embodiments, the above process specifically includes:

[0120] make , Then we have the system state vector. and system state equations For the system state equation matrix Linearization yields the transition matrix, i.e.:

[0121] ;

[0122] Where F represents the transition matrix, for The estimated value of the Kalman filter.

[0123] Perform an inverse Laplace transform on the transition matrix F, and take the first-order linear form Φ(t) of the transition matrix F as follows:

[0124] ;

[0125] Where I represents the identity matrix and t represents time. According to the sampling time T... s Discretizing the first-order linear expression Φ(t) yields the first-order linear transition matrix Φ. k As shown in the following formula:

[0126] .

[0127] In some embodiments, after determining the measurement equation for the laser power of the four-quadrant detector based on the system state vector in step A1, the method further includes using the sum of the measured values ​​of the optical power in each quadrant of the four-quadrant detector as the input to the measurement equation. The measurement equation is:

[0128] ;

[0129] in, H represents the measured value of the optical power of the four-quadrant detector including noise, H=[0,1] represents the measurement equation transfer matrix, and v represents the measurement noise.

[0130] Since the noise of a four-quadrant detector mainly consists of three parts: the detector itself, the post-processing circuitry, and the environment, the noise of a four-quadrant detector mainly includes shot noise, thermal noise, composite noise, and 1 / f Noise, the first two of which are white noise, and the composite noise, which is generated by the combination of shot noise and thermal noise, is also white noise, 1 / f The noise is bandwidth-dependent and should be bandwidth-limited white noise. Due to the diversity and complexity of noise in a four-quadrant detector, this embodiment of the invention assumes the measured noise v is zero-mean Gaussian white noise, v = v1 + v2 + v3 + v4, where v1, v2, v3, and v4 represent the noise in the four signals of the four-quadrant detector. Correspondingly, the noise in the four signals is also zero-mean Gaussian white noise, and the total variance of the four noise channels is R.v This can be obtained through the input standard deviation, and is understandable; the actual four signals p of the four-quadrant detector are... 1n p 2n p 3n and p 4n Represented as: p 1n =p1+v1, p 2n =p² + v², p 3n =p3+v3, p 4n =p4+v4. Four-channel measurements from a four-quadrant detector. , where P t =p1+p2+p3+p4, where p1, p2, p3 and p4 represent the theoretical optical power values ​​of each quadrant in the four-quadrant detector.

[0131] A2: Select the full-range guided flight time for parameter identification to obtain the predicted values ​​of the laser power and laser power change rate of the four-quadrant detector.

[0132] In some embodiments, step A2 specifically includes:

[0133] Choosing the entire guided flight time as a random constant, the entire guided flight time is sampled and discretized using the following formula:

[0134] ;

[0135] in, t 0k This represents the discretized full-range guided flight time at time k. The full-range guided flight time t at time k-1 is represented. 0k-1 Zero-mean Gaussian white noise, in this embodiment of the invention, is zero-mean Gaussian white noise. The variance is pre-set to .

[0136] The discrete system state vector and measured values ​​are extended using the following formula:

[0137] ;

[0138] ;

[0139] in, Indicates the measured value The discretized measurement at time k, Φ k-1 Let represent the first-order linear transition matrix at time k-1.

[0140] Based on the discrete system state vector at time k-1 The full-range guided flight time t at time k-1 0k-1The predicted values ​​of laser power and laser power change rate are obtained by moving forward one sampling step:

[0141] ;

[0142] ;

[0143] ;

[0144] ;

[0145] Where, x k-1 This represents the laser power value at time k-1. This represents the rate of change of the laser power value at time k-1. This represents the rate of change of the laser power at time k-1. Indicates the receiving distance R M The discrete value at time k-1, T s Indicates the sampling time, x k|k-1 This represents the predicted value of the laser power. The predicted value representing the rate of change of laser power.

[0146] A3: Based on the measurement equation extended in step A2, the predicted values ​​of laser power and laser power change rate obtained in step A2 are corrected and updated using extended Kalman filtering to obtain the estimated optical power and estimated optical power change rate of the four-quadrant detector. In some embodiments, step A3 includes:

[0147] For different times k, the discrete system state estimation vector is calculated using the following formula. And estimate the entire guided flight time :

[0148] ;

[0149] in, ;

[0150] Based on the first-order linear transition matrix Φ at time k-1 k-1 State covariance matrix P k-1 and zero-mean Gaussian white noise variance The predicted state covariance matrix P is obtained through the following formula. k|k-1 :

[0151] ;

[0152] This leads to the Kalman gain matrix K. k :

[0153] ;

[0154] Through the Kalman gain array K k and the predicted state covariance matrix P k-1|k It can be calculated by the following formula. k The state covariance matrix P at time t k :

[0155] ;

[0156] The data should be updated and corrected according to the following formula:

[0157] ;

[0158] At this point, the estimated optical power of the four-quadrant detector is obtained. Estimated rate of change of optical power .

[0159] like Figure 3 As shown, the process of filtering the center position of the light spot using an iterative extended Kalman filter includes:

[0160] B1: Determine the discrete state vector of the light spot center; initialize and predict the system state and covariance of the iterative extended Kalman filter based on the discrete state vector. In some embodiments, the process of determining the discrete state vector of the light spot center includes:

[0161] The relative position x of the light spot center on the target surface in the four-quadrant detector r ,speed and acceleration The state vector at the center of the light spot ;

[0162] The discrete state vector of the light spot center is obtained by the following formula. :

[0163] ;

[0164] Among them, w k-1 Let A represent the process noise at time k-1, and let A represent the state transition matrix.

[0165] .

[0166] B2: Combining the optical power estimate obtained in step A3, the sum-difference ratio formula represented by the four signal values ​​of the four-quadrant detector is decomposed into a pre-value formula and a post-value formula.

[0167] In some embodiments, the sum-difference ratio formula z r for:

[0168] ;

[0169] Combining the optical power estimates of the four-quadrant detector, the sum-difference amplitude formula z r The formula for the prevalue obtained by decomposition z ra The formula for sum and aftervalue z rb :

[0170] ;

[0171] .

[0172] B3: Rewrite the pre-value formula and post-value formula obtained in step B2, and based on the rewritten pre-value formula and post-value formula, use iterative extended Kalman filtering to update and correct the state estimate, and output the optimal state estimate.

[0173] In some embodiments, step B3 includes:

[0174] Based on the relative position x of the light spot center r The nonlinear theoretical relationship formula g between the measured ideal input value and the relative position of the uniform spot center is obtained. ra (x r ) and g rb (x r )for:

[0175] ;

[0176] ;

[0177] relative position of the center of the light spot x r The value range is [-1, 1], which is the measurable region, and outside this range is the saturation region.

[0178] Now, considering the impact of noise, the previous value formula z... ra The formula for sum and aftervalue z rb Rewritten as:

[0179] ;

[0180] ;

[0181] Where, λ t P represents the theoretical value of the optical power of the four-quadrant detector. t Compared with optical power estimates The ratio, that is v a v represents the sum of the noise in the first quadrant and the noise in the fourth quadrant of a four-quadrant detector. b This represents the sum of the noise in the second quadrant and the noise in the third quadrant of a four-quadrant detector.

[0182] Select the relative position x of the discretized spot center rk ,speed and acceleration and parameter λ t discrete value λ tk , as the new state vector The data is updated and corrected using an iterative extended Kalman filter, as shown in the following formula:

[0183] ;

[0184] in, Res represents the updated estimate of the new state vector. ak and Res bk These represent the rewritten discretization pre-value formula z. rak The formula for sum and aftervalue z rbk The corresponding estimated residual in the iterative extended Kalman filter, K ak and K bk The formulas for the previous value and the values ​​in the iterative extended Kalman filter are respectively: rak The formula for sum and aftervalue z rbk The corresponding Kalman gain.

[0185] In some embodiments, the residual Res is estimated. ak and Res bk The acquisition process includes:

[0186] For the nonlinear theoretical relation formula g ra (x r ) and g rb (x r Discretize the expression to obtain the discrete form g. rak (x rk ) and g rbk (x rk );

[0187] Based on discrete g rak (x rk ) and g rbk (x rk ), thus obtaining the Jacobian matrix of the measurement equation corresponding to the previous value formula. The measurement equation corresponding to the aftervalue formula is the Jacobian matrix. ;

[0188] Using the state estimate, the estimated residual Res in the iterative extended Kalman filter is obtained by the following formula. ak and Res bk :

[0189] ;

[0190] ;

[0191] in, Indicates the relative position x of the light spot center r The estimated value at time k, Indicates the parameter λ t The estimated value at time k;

[0192] Select parameter λ t At time k, the parameter λ tk As a random constant:

[0193] ;

[0194] in, This represents zero-mean Gaussian white noise, whose variance can be preset to... Expanding the dimension of the state transition matrix A yields the extended state transition matrix. :

[0195] ;

[0196] Application of extended state transition matrix , parameter λ tk Adaptive identification is performed in the iterative extended Kalman filter.

[0197] In some embodiments, step B3, which uses iterative extended Kalman filtering to update and correct the state estimate, further includes:

[0198] Determine whether the relative position of the spot center is within the constraint range [-1, +1]. If it is within the constraint range, perform iterative extended Kalman filtering directly. If it is not within the constraint range, truncate the Gaussian probability density function of the state estimate and its corresponding covariance, calculate the truncated state mean and its covariance, and assign them to the state estimate.

[0199] In this embodiment of the invention, the above process specifically includes:

[0200] The relative position of the light spot center is restricted to the constraint range [-1, +1], and the corresponding mathematical inequality of the constraint condition is:

[0201] ,and ;

[0202] Both of the above equations should be subject to restrictions, where A(1,:) denotes the first row of the state transition matrix A. State estimation The predicted value of the previous sampling step.

[0203] If the relative position of the light spot center is not within the constraint range [-1, +1], probability truncation is performed, i.e., the Gaussian probability density function is truncated. Cut off, Discrete state vector representing the center of the light spot The corresponding estimated state vector, P rk Represents the estimated state vector The covariance; calculate the state mean of the truncated probability density function. and its covariance .

[0204] For the initial covariance P rk 0 Performing normal decomposition yields the eigenvectors M and the diagonal matrix of eigenvalues ​​W, i.e.:

[0205] MWM T =P rk 0 ;

[0206] The normalized orthogonal matrix ρ is obtained by the following formula:

[0207] ;

[0208] Where, ρ T Let represent the transpose of the normalized orthogonal matrix ρ. This allows the normalized transformation z before truncation to be... rk0 The probability distribution follows N(0,1), that is:

[0209] ;

[0210] Based on the estimated state vector Obtain the truncated transformation quantity z rk1 The value of should be between c and d, as shown in the following formula:

[0211] ;

[0212] ;

[0213] Where a = -1, b = +1. The transformation quantity z rk1 Gaussian probability density function and normalized transform z rk0 The same, only truncated by constraints c and d. The transform quantity z conforming to the Gaussian probability density function is obtained by the following equation. rk1 corresponding mean :

[0214] ;

[0215] Where erf represents the error function. The transform quantity z after truncating the Gaussian probability density function.rk1 mean Perform an inverse transformation to obtain the state mean that satisfies the constraints. ,Right now:

[0216] .

[0217] It should be understood that the various forms of processes shown above can be used to reorder, add, or delete steps. For example, the steps described in this invention disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this invention can be achieved, and this is not limited herein.

[0218] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for detecting and calculating the position of a light spot in a four-quadrant detector, characterized in that, The process includes two parts: filtering the total optical power signal of the optical power received by the four-quadrant detector, and filtering the center position of the light spot based on the filtered optical power. The total optical power signal filtering process includes: A1: Define the system state vector and system state equation of the four-quadrant detector; determine the measurement equation of the laser power of the four-quadrant detector based on the system state vector; A2: Select the full-range guided flight time for parameter identification, obtain the predicted values ​​of the laser power and laser power change rate of the four-quadrant detector, and extend the measurement equation of step A1; A3: Based on the measurement equation extended in step A2, the predicted values ​​of laser power and laser power change rate obtained in step A2 are corrected and updated using extended Kalman filtering to obtain the estimated values ​​of optical power and optical power change rate of the four-quadrant detector. The filtering process at the center of the light spot includes: B1: Determine the discrete state vector of the light spot center; initialize and predict the system state and covariance of the iterative extended Kalman filter based on the discrete state vector; B2: Combining the optical power estimate obtained in step A3, the sum-difference ratio formula represented by the four signal values ​​of the four-quadrant detector is decomposed into a pre-value formula and a post-value formula. B3: Rewrite the pre-value formula and post-value formula obtained in step B2, and based on the rewritten pre-value formula and post-value formula, use iterative extended Kalman filtering to update and correct the state estimate, and output the optimal state estimate.

2. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 1, characterized in that, Step A1 includes: Based on the receiving principle and parameters of the four-quadrant detector, the laser power equation of the four-quadrant detector and the corresponding first-order and second-order differential equations are obtained. Based on the three equations obtained, define the system state vector and the system state equation; linearize the system state equation to obtain the transition matrix; then discretize the transition matrix to obtain the discretized first-order linear transition matrix and the discrete system state vector.

3. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 2, characterized in that, The laser power equation is: ； Among them, P t R represents the theoretical optical power of the four-quadrant detector, D represents the receiving optical aperture of the four-quadrant detector, and R represents the theoretical optical power of the four-quadrant detector. M P represents the receiving distance of the four-quadrant detector. L Indicates the peak power of the laser. T represents the target reflectivity. r R represents the combined transmission and reception coefficients of the four-quadrant detector; L σ represents the illumination distance, σ represents the atmospheric attenuation coefficient, and θ represents the angle between the target surface normal and the laser receiving direction. By performing two differential operations on the laser power equation, we obtain the first-order and second-order differential equations: ； ； in, P represents the theoretical value of optical power. t The first-order differential value, P represents the theoretical value of optical power. t The second derivative value, t represents the rate of change of receiving distance. go Indicates the remaining flight time for guidance, t go =t0-t n , t n t0 represents the guided flight time, f1 represents the first-order differential function, and f2 represents the second-order differential function.

4. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 3, characterized in that, The process of obtaining the discretized first-order linear transition matrix and the discrete system state vector in step A1 includes: make , Then we have the system state vector. and system state equations ; For the system state equation matrix Linearization yields the transition matrix; Perform an inverse Laplace transform on the transition matrix, take its first-order linear form, and then discretize it according to the sampling time to obtain the first-order linear transition matrix, simultaneously obtaining the discrete system state vector. Discrete system state estimation vector , This represents the state estimate of the discrete system at time k. This represents the rate of change of the discrete system state estimate at time k.

5. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 4, characterized in that, Step A1 also includes: using the sum of the measured values ​​of the optical power in each quadrant of the four-quadrant detector as the input to the measurement equation; The measurement equation is: ； in, H represents the measured value of the optical power of the four-quadrant detector including noise, H=[0,1] represents the measurement equation transfer matrix, and v represents the measurement noise.

6. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 5, characterized in that, Step A2 includes: The entire guided flight time is sampled and discretized using the following formula: ； Among them, t 0k Let k represent the discretized time of the entire guided flight at time k. The full-range guided flight time t at time k-1 is represented. 0k-1 Zero-mean Gaussian white noise; The discrete system state vector and measured values ​​are extended using the following formula: ； ； in, Indicates the measured value The discretized measurement at time k, Φ k-1 This represents the first-order linear transition matrix at time k-1; Based on the discrete system state vector at time k-1 The full-range guided flight time t at time k-1 0k-1 The predicted values ​​of laser power and laser power change rate are obtained by moving forward one sampling step: ； ； ； ； Where, x k-1 This represents the laser power value at time k-1. This represents the rate of change of the laser power value at time k-1. This represents the rate of change of the laser power at time k-1. Indicates the receiving distance R M The discrete value at time k-1, T s Indicates the sampling time, x k|k-1 This represents the predicted value of the laser power. The predicted value representing the rate of change of laser power.

7. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 6, characterized in that, The update process in step A3 is detailed below: For different times k, the discrete system state estimation vector is calculated using the following formula. And estimate the entire guided flight time : ； in, ; Based on the first-order linear transition matrix Φ at time k-1 k-1 State covariance matrix P k-1 and zero-mean Gaussian white noise variance The predicted state covariance matrix P is obtained through the following formula. k|k-1 : ； This leads to the Kalman gain matrix K. k : ； Through the Kalman gain array K k and the predicted state covariance matrix P k-1|k It can be calculated by the following formula. k The state covariance matrix P at time t k : ； Where I represents the identity matrix; The data should be updated and corrected according to the following formula: ； At this point, the estimated optical power of the four-quadrant detector is obtained. Estimated rate of change of optical power .

8. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 7, characterized in that, In step B1, the relative position x of the light spot center on the target surface of the four-quadrant detector is used. r ,speed and acceleration The state vector at the center of the light spot ; The discrete state vector of the light spot center is obtained by the following formula. : ； Among them, w k-1 Let A represent the process noise at time k-1, and let A represent the state transition matrix. 。 9. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 8, characterized in that, In step B2, the sum-difference ratio formula z r for: ； Where p1, p2, p3, and p4 represent the power of the four signals of the four-quadrant detector; combined with the estimated optical power of the four-quadrant detector, the sum-difference amplitude formula z is... r Decomposition yields the formula for the previous value z. ra The formula for sum and aftervalue z rb : ； 。 10. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 9, characterized in that, Step B3 includes: Based on the relative position x of the light spot center r The nonlinear theoretical relationship between the ideal measurement input value of the four-quadrant detector and the relative position of the center of the uniform light spot is obtained by formula g. ra (x r ) and g rb (x r )for: ； ； The previous value formula z ra The formula for sum and aftervalue z rb Rewritten as: ； ； Where, λ t P represents the theoretical value of optical power. t Compared with optical power estimates The ratio of the theoretical value of optical power P t v is the sum of the noise-free theoretical values ​​of the power of the four signals. a v represents the sum of the noise in the first quadrant and the noise in the fourth quadrant of a four-quadrant detector. b This represents the sum of the noise in the second quadrant and the noise in the third quadrant of a four-quadrant detector; Select the relative position x of the discretized spot center rk ,speed and acceleration and parameter λ t discrete value λ tk , as the new state vector The data is updated and corrected using the following formula: ； in, Res represents the updated estimate of the new state vector. ak and Res bk These represent the rewritten discretization pre-value formula z. rak The formula for sum and aftervalue z rbk The corresponding estimated residual in the iterative extended Kalman filter, K ak and K bk The formulas for the previous value and the values ​​in the iterative extended Kalman filter are respectively: rak The formula for sum and aftervalue z rbk The corresponding Kalman gain.

11. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 10, characterized in that, Estimate residuals Res ak and Res bk The acquisition process includes: For the nonlinear theoretical relation formula g ra (x r ) and g rb (x r Discretize the expression to obtain the discrete form g. rak (x rk ) and g rbk (x rk ); Based on discrete g rak (x rk ) and g rbk (x rk ), thus obtaining the Jacobian matrix of the measurement equation corresponding to the previous value formula. The measurement equation corresponding to the aftervalue formula is the Jacobian matrix. ; Using the state estimate, the estimated residual Res in the iterative extended Kalman filter is obtained by the following formula. ak and Res bk : ； ； in, Indicates the relative position x of the light spot center r The estimated value at time k, Indicates the parameter λ t The estimated value at time k; Select parameter λ t At time k, the parameter λ tk As a random constant: ； in, This represents zero-mean Gaussian white noise; Expand the dimension of the state transition matrix A, and based on the expanded state transition matrix, the parameter λ tk Adaptive identification is performed in the iterative extended Kalman filter.

12. The method for detecting and calculating the position of a light spot in a four-quadrant detector according to claim 1, characterized in that, Step B3 uses iterative extended Kalman filtering to update and correct the state estimate. It also includes: determining whether the relative position of the spot center is within the constraint range [-1, +1]. If it is within the constraint range, iterative extended Kalman filtering is performed directly. If it is not within the constraint range, the Gaussian probability density function of the state estimate and its corresponding covariance is truncated, the state mean and covariance of the truncated probability density function are calculated iteratively, and the truncated state mean is assigned to the state estimate.

Images