Surface target motion estimation method

Abstract

The present invention relates to a method for estimating the motion of a surface target, comprising the following steps: a. preprocessing the motion point data of a surface target with low measurement accuracy; b. estimating the motion speed and direction of the surface target. The present invention can accurately estimate the speed and heading of the surface target.

Description

Technical Field

[0001] The invention relates to a method for estimating the motion of a water surface target. Background Art

[0002] Grasping the wide-area situation and safeguarding maritime interests and security are the cornerstones of the development of marine strategy. With the improvement of my country's space-based equipment capabilities, the problems of "seeing far" and "seeing clearly" have been basically solved, but there are still gaps in "seeing clearly and keeping up" and systematic applications. The present invention uses space-based active / passive radars to detect unidentified ships and assist the coast guard in driving them away as the application background. By estimating the target speed and heading, the target intention is grasped.

[0003] Based on the GF-4 satellite data, the literature [1] proposed a method to form a target track using multi-hypothesis tracking [2], and then estimate the target's speed and heading. The main steps of this method include: using multi-structure multi-scale morphological filtering to suppress the background of the ocean background remote sensing image; using adaptive threshold segmentation and self-organizing clustering to obtain candidate targets; using the geostationary satellite staring sequence image to perform multi-target moving neighborhood judgment on the candidate targets according to the target motion characteristics, and eliminate false targets; correlating ship targets and integrating satellite platform data to accurately obtain information such as the position, speed, heading, and motion trajectory of each ship. This method has the advantages of simple algorithm, high target detection rate, low false alarm rate, and good stability. Since the GF-4 satellite adopts an optical imaging system, the motion trajectory of the ship is continuous and smooth on the image plane. In the process of projecting from the image plane to the geographic coordinates, although there will be a certain degree of nonlinear system error, the spatial position relationship between adjacent measurement points is close to the actual situation, and the impact on the speed and heading estimation is relatively small.

[0004] Space-based active / passive radars have been widely used due to their long detection distance and wide coverage. For space-based passive radars, the direction of the incoming wave can be determined by measuring the characteristic parameters of the radiation source signal, and the radiation source positioning and identification are completed. Currently, the commonly used methods are single-satellite reconnaissance positioning method and multi-satellite time difference reconnaissance positioning method [1]. Limited by the antenna size, constellation scale and orbital resources, the positioning accuracy of space-based active / passive radars can reach a maximum of more than ten kilometers, and the target measurement points have large fluctuations. Due to the disorder of the spatial relative positions of adjacent measurement points, the estimated speed often exceeds the actual capability of the ship and the heading estimation is reversed. The method in reference [3] focuses on background suppression, weak target detection under complex backgrounds, and conversion errors from image plane coordinates to geographic coordinates. This method is not suitable for speed and heading estimation under low measurement accuracy.

[0005] Aiming at space-based active / passive radar measurement, the present invention proposes a method for estimating the speed and heading of a ship target under low measurement accuracy, based on the target point track generated by ocean situation and space-time constraints, with the goal of obtaining a smooth track and more accurate heading estimation.

[0006] [1] Yao Libo et al., Ship target tracking based on GF-4 satellite. 2017 Fourth Annual Conference on High Resolution Earth Observation.

[0007] [2] Blackman SS. Multiple hypothesis tracking for multiple target tracking[J]. IEEE Aerospace and Electronic Systems Magazine, 2004, 19(1): 5~18.

[0008] [3] Guo Fucheng et al., Principles of Space Electronic Reconnaissance and Positioning. National Defense Industry Press.

[0009] [4] He You et al., Radar Data Processing and Application. Publishing House of Electronics Industry.

[0010] Generally speaking, the higher the measurement accuracy and data rate, the better the target tracking accuracy, and the more accurate the estimation of speed and heading. Figure 1 From the schematic diagram of high-precision measurement at adjacent moments, it can be seen that the target is at t k-1 The true value and measured value of the position at the moment are x k-1 and (solid point and triangle respectively), the target is at t k The true position and measured value at the moment are x k and As the measurement error increases, the range of the measurement distribution shown in the ellipse area becomes larger, and the measured value deviates from the true value. However, when the measurement error is greater than half the distance between two adjacent points, the spatial relationship of the measured position no longer conforms to the actual situation because the measured value deviates far from the true value, resulting in the northeast velocity being estimated as the southwest velocity, such as Figure 2 Therefore, a series of processing is required on the point track data in advance to restore the relative position relationship of the point track, thereby avoiding the deviation of the estimated heading value from the true value exceeding 90 degrees and the large fluctuation of the speed estimation.

[0011] For speed and heading estimation, we must first establish stable tracking of the target and obtain a smooth track. Generally, a rectangular coordinate system is used to describe the target state equation. Space-based measurements usually use longitude and latitude to describe the position of ship targets, so the geographic coordinate system is the most direct choice. However, the state transition model under the geographic coordinate system is not enough to accurately describe the turning or stationary motion of the target.

[0012] The possible motion forms of ship targets include stationary, linear motion, reciprocating motion, circular motion, etc., so filtering methods are often used. Usually, the state transfer model adopts a uniform speed model, a uniform acceleration model or a coordinated turning model [4]. However, in the case of low measurement accuracy, even if the target is actually moving at a uniform speed, it is easy to be measured and pulled to accelerated motion during the filtering process. To this end, on the one hand, it is necessary to add a state transfer model that is closer to the target motion, and on the other hand, it is necessary to reduce process noise so that the set state transfer model is more trusted rather than the measured value during the process of updating the filter gain. However, this results in a lag in the response of the filter when the target's motion deviates from the set state transfer model. Therefore, it is necessary to select a suitable state transfer model to ensure that it conforms to the motion law of the ship target and avoids the filter being pulled by low-precision measurements. Summary of the invention

[0013] The purpose of the present invention is to provide a method for estimating the motion of a surface target.

[0014] To achieve the above object, the present invention provides a method for estimating the motion of a surface target, comprising the following steps:

[0015] a. Preprocess the motion point data of the surface target;

[0016] b. Fusion weight setting strategy;

[0017] c. Estimate the movement speed and direction of the surface target.

[0018] According to one aspect of the present invention, in step (a), when the distance between two adjacent points is within a fusion threshold, fusion is performed, and the fusion point is the average of the connected points within the fusion threshold;

[0019] If all the continuous points are within the fusion threshold, iterative fusion is performed;

[0020] The sub-satellite point distance or measurement error is used as the basis for calculating the fusion weight.

[0021] According to one aspect of the present invention, step (a) comprises the following steps:

[0022] a1. Sort the input point data according to the measurement time and determine the starting point of the track;

[0023] a2. Assuming that the current sensor measurement accuracy is r, the fusion threshold is set to 2r+V max Δt;

[0024] a3. Find the current time t k-1 The corresponding point x k-1 , and the next moment t k The corresponding point x k , calculate the distance between two adjacent points Δd = ||x k-1 -x k ||; If the measurement at the next moment cannot be found, execute step a8;

[0025] a4. If the distance Δd between two adjacent points exceeds the fusion threshold 2r+V max Δt, then there is no need to process these two points, and jump to step a3; otherwise, execute step a5;

[0026] a5. Calculate the position weights of adjacent moments, which are the normalized w k-1 and w k Since the causes of measurement errors of different sensors are different, the corresponding fusion weight strategy should be used in conjunction with them, as detailed in the next section;

[0027] a6. Perform weighted fusion on two adjacent points to obtain a new point The corresponding measurement time value is The measurement time can also be set as required;

[0028] a7. Repeat steps (a2) to (a6) until there is no measurement point at the next moment;

[0029] a8. The measurement at the next moment cannot be found, and the point trace data preprocessing ends.

[0030] Among them, V is the maximum speed of the ship, which is generally 55.56 kilometers per hour, or 30 knots; △t is the time difference between adjacent points in the time series.

[0031] According to one aspect of the present invention, the method of obtaining the position weight by using the sub-satellite point distance is to make the combined weighted value (w k-1 and w k ) is inversely proportional to the distance to the subsatellite point, and the combined weighted value is normalized to obtain the position weights of the two points. and

[0032] The time corresponding to the new fusion point obtained by weighted fusion is the middle value of the two moments;

[0033] The fused traces will replace (cover) the original traces (the traces involved in the fusion);

[0034] If continuous iterative fusion is performed, the fusion strategy is repeated until the distance between two adjacent points exceeds the fusion threshold;

[0035] Where d is the distance from the current measurement point to the sub-satellite point.

[0036] According to one aspect of the present invention, in step (b), under an interactive multi-model architecture, a set of state variables and state transition models are selected, and a filtering and smoothing method is used to estimate the speed and heading of the surface target.

[0037] For sensors that cannot calculate fusion weights or have no historical statistical data, the fusion point trace can be taken as the average of the connected point traces within the fusion distance threshold.

[0038] In order to obtain better point fusion results, different fusion weight setting strategies can be used according to the positioning mechanism and measurement accuracy of the sensor to accurately restore the relative position relationship between adjacent points, such as using measurement error, measurement covariance matrix, measurement score, sub-satellite distance, etc. as the basis for fusion weight calculation. Among them, the larger the measurement error and measurement covariance matrix, the smaller the measurement score, and the smaller the weight in the fusion process; conversely, the larger the weight. The following focuses on the strategy of using sub-satellite distance as the fusion weight setting, which mainly includes direct method, theoretical method and statistical method, such as Figure 5 shown.

[0039] 1) Direct method

[0040] Step 1: Calculate the subsatellite distance between two adjacent traces;

[0041] Step 2: Calculate the current positioning accuracy estimate according to the interval of the target's distance to the sub-satellite point, and take its reciprocal as the weight of the current point trace and the weight of the next point trace, respectively, denoted as w′ k-1 and w′ k ;

[0042] Step 3: For w′ k-1 and w′ k Normalize and get the final weight w for fusion k-1 and w k .

[0043] 2) Theoretical method

[0044] Step 1: Calculate the theoretical positioning error curve from the zero position of the sub-satellite point to the maximum boundary of the precision guarantee area according to the specific sensor;

[0045] Step 2: Calculate the subsatellite point distance between two adjacent traces;

[0046] Step 3: Based on the positioning error theoretical curve, calculate the positioning error of two adjacent points, and take their reciprocals as the weight of the current point and the weight of the next point, respectively, denoted as w′ k-1 and w′ k ;

[0047] Step 4: For w′ k-1 and w′ k Normalize and get the final weight w for fusion k-1 and w k .

[0048] 3) Statistical method

[0049] Step 1: Perform statistical processing on the historical positioning accuracy of the sensor to obtain a positioning error statistical curve related to the sub-satellite point distance. On this basis, the positioning error statistical curve under different target characteristics can be obtained according to the electromagnetic or optical characteristics of the target;

[0050] Step 2: Calculate the subsatellite point distance between two adjacent traces;

[0051] Step 3: Based on the positioning error theoretical curve, calculate the positioning error of two adjacent points, and take their reciprocals as the weight of the current point and the weight of the next point, respectively, denoted as w′ k-1 and w′ k ;

[0052] Step 4: For w′ k-1 and w′ k Normalize and get the final weight w for fusion k-1 and w k .

[0053] According to one aspect of the present invention, in step (b), under an interactive multi-model architecture, a set of state variables and state transition models are selected, and a filtering and smoothing method is used to estimate the speed and heading of the surface target.

[0054] According to one aspect of the present invention, step (b) comprises the following steps:

[0055] b1. Establish a local northeast celestial coordinate system with the starting point of the track as the origin, and use it as the tracking coordinate system. Then, convert all points on the track from the geographic coordinate system to the local coordinate system;

[0056] b2. Set the transition probabilities: the probability of the stationary model and the probability of the coordinated turning (CT) model;

[0057] b3. Establish the three-axis state equations of the static model and the CT model, where the CT equation introduces the turning angular velocity state variable and establishes the observation equation including the three-axis position measurement;

[0058] The state equation of the cooperative turning model introduces the turning angular velocity state variable to establish the observation equation including the three-axis position measurement;

[0059] b4. Infer the current measurement estimate based on the system state estimate and covariance estimate at the previous moment, and then reinitialize the model. The new initial value is obtained through the Markov operation matrix between different models; the new initial value is obtained through the Markov operation matrix between different models;

[0060] b5. Derive the nonlinear state equation and Jacobian matrix using EKF (Extended Kalman Filter);

[0061] b6. Give the weight ratio of the most suitable tracking model by calculating the similarity between the current model and the current moving target state;

[0062] b7. Obtain an estimation result based on the individual calculation results of each model and the model matching weight, and complete the filtering process;

[0063] b8. Smoothing all traces based on the filtering results, where the smoothing value is constrained by the measurement covariance matrix of the final state and the process noise covariance matrix; smoothing is completed;

[0064] b9. Based on the speed at each moment, the heading is calculated and output; if there is imaging information input that is close in time, the heading can be assisted in determining by identifying the target ship's bow direction, etc. For details, see the speed and heading calculation section.

[0065] b10. Complete the processing of all tracks, and the speed and heading estimation is completed.

[0066] According to one aspect of the present invention, in step (b), the speed v(k) at the kth moment is calculated as:

[0067]

[0068] Among them, v e (k), v n (k) and v u (k) are the velocity components of the three axes in the northeast celestial coordinate system;

[0069] The heading angle is defined as a clockwise angle from north to east, with a range of [0,2π), then the heading Φ(k) at the kth moment is:

[0070]

[0071] According to one aspect of the present invention, the method of assisting in determining the heading in step (b9) is:

[0072] If only one point in the track has corresponding imaging information input, the target ship's bow or wake can be identified if the resolution allows, thereby giving the target heading;

[0073] If multiple points in the track have corresponding imaging information input, the heading estimate is obtained based on the bow direction and wake after static frame image recognition. The track is generated based on the positioning results of multi-frame imaging information, and the speed and heading estimates are output through filtering and smoothing.

[0074] Taking into account the above speed and heading estimates, multiple information is supplemented and integrated to generate the final speed and heading estimates.

[0075] According to one aspect of the present invention, the fusion estimation of speed and heading includes:

[0076] S1. Generate a track based on the positioning results of the imaging information, and output the initial speed and heading estimation by filtering and smoothing;

[0077] S2, identifying the bow of the surface target in each frame, and obtaining the heading of the surface target in the static frame;

[0078] S3, performing weighted fusion on the headings obtained in step (S1) and step (S2), wherein the weighted value is an empirical value or is obtained by using Kalman filter / confidence assessment fusion;

[0079] If the point trace detection time of the imaging information is significantly different from the point trace detection time of the sensor (5 minutes), proceed to the subsequent steps, otherwise directly output the estimated values ​​of the heading and speed;

[0080] S4, fusing the track generated by the low measurement accuracy and the track generated by the imaging information to complete the two track information;

[0081] The fusion processing method is covariance weighted trajectory fusion or adaptive trajectory fusion;

[0082] S5. Filter and smooth the fused track to obtain the estimated values ​​of heading and speed and output them.

[0083] According to the concept of the present invention, a point trace data iterative fusion preprocessing method, a fusion weight setting strategy, and a ship target speed and heading estimation process under low-precision measurement are provided.

[0084] According to a solution of the present invention, a suitable filter architecture and state model transfer set are selected, which can not only meet the demand for accurate characterization of ship target motion, but also avoid the deviation problem caused by using acceleration models under low-precision measurements, and can increase the large amount of calculation caused by the number of models within the tolerance range of engineering applications.

[0085] According to one solution of the present invention, the point track data fusion preprocessing method can effectively restore the relative position relationship of the track points to solve or weaken the problem of large deviation between the estimated heading and speed of slow or stationary targets and the actual values ​​when the sensor positioning accuracy is low, thereby laying the foundation for stable tracking and obtaining a smooth track.

[0086] According to a solution of the present invention, the point track data fusion preprocessing is to restore the spatial relative position relationship of adjacent point tracks by setting a fusion threshold and adopting a recursive fusion strategy, thereby greatly reducing the deviation between the estimated value and the true value of the heading caused by low measurement accuracy, thereby solving the problems of deviation exceeding 90 degrees and large fluctuations in speed estimation.

[0087] The present invention is applicable to situations where other low-measurement-precision sensors estimate deep-level target dynamic information (including position, speed, heading, motion trajectory, etc.), and can obtain a more accurate heading estimate in the presence of other imaging intelligence information confirmation.

[0088] A practical process for estimating the speed and heading of ship targets under low measurement accuracy is proposed. By selecting a suitable filter architecture and state model transfer set, it can not only meet the demand for accurate characterization of ship target motion, but also avoid the deviation problem caused by using acceleration models under low-precision measurement. Moreover, the increase in the amount of calculation caused by increasing the number of models is within the acceptable range of engineering applications.

[0089] According to one solution of the present invention, for data preprocessing with low measurement accuracy, Kalman filtering combined with an interactive multiple model architecture (IMM) is used to estimate speed and heading, thereby optimizing the speed and heading estimation of water targets with low measurement accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] Figure 1 It is a schematic diagram of adjacent positions during high-precision measurement;

[0091] Figure 2 It is a schematic diagram of adjacent positions during low-precision measurement;

[0092] Figure 3 A diagram schematically showing a weighted fusion result of adjacent measurement points according to an embodiment of the present invention;

[0093] Figure 4 A flowchart schematically showing iterative fusion preprocessing of point trace data according to an embodiment of the present invention;

[0094] Figure 5 A flowchart schematically showing an iterative fusion strategy of point trace data according to an embodiment of the present invention;

[0095] Figure 6 A flow chart schematically showing a method for estimating the motion of a surface target according to an embodiment of the present invention;

[0096] Figure 7 A schematic diagram of a low-speed moving target track diagram in the ocean before point-track fusion according to an embodiment of the present invention is shown;

[0097] Figure 8 A track diagram schematically showing point-track fusion according to an embodiment of the present invention;

[0098] Fig. 9 A track diagram schematically showing filtering and smoothing processing according to an embodiment of the present invention. DETAILED DESCRIPTION

[0099] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0100] When describing the embodiments of the present invention, the orientation or positional relationship expressed by the terms "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside" and "outside" are based on the orientation or positional relationship shown in the relevant drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the above terms should not be understood as limiting the present invention.

[0101] The present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments. The embodiments cannot be described one by one here, but the embodiments of the present invention are not therefore limited to the following embodiments.

[0102] The surface target motion estimation method of the present invention is suitable for estimating the speed and heading of surface targets such as ships under low measurement accuracy, and for estimating deep-level target dynamic information (including position, speed, heading, motion trajectory, etc.) by other low measurement accuracy sensors. The method first pre-processes the motion point data of the surface target, and then estimates the motion speed and direction of the surface target to solve the problem of disordered spatial relative position relationship caused by low measurement accuracy and the problem of ship target tracking under low measurement accuracy.

[0103] Among them, when performing point trace data preprocessing, the iterative fusion threshold is first set in combination with the specific situation of the sensor. When the distance between two adjacent point traces is within the fusion threshold (i.e., the distance threshold), fusion is performed; and if continuous point traces (i.e., the phenomenon that continuous point traces need to be fused) are all within the fusion threshold, iterative fusion is performed. Of course, in the absence of a better fusion strategy, the fusion point position can be the mean of the connected point traces within the fusion threshold. In addition, in order to obtain better fusion results, the fusion strategy can be selected according to the positioning mechanism of the sensor, that is, the sub-satellite point distance or measurement error is used as the basis for calculating the fusion weight, so as to restore the relative position relationship between the point traces. When estimating the speed and heading of the surface target, on the basis of the iterative fusion preprocessing of the point trace data, according to the motion characteristics of the ship target, under the interactive multi-model architecture (IMM), the appropriate state variables and state transition model set are selected, and then the filtering and smoothing methods are used to estimate the speed and heading of the surface target.

[0104] See also Figure 3 and Figure 4 The method of point trace data fusion of the present invention is to sort the input point trace data according to the measurement time and determine the starting point of the track. If the measurement accuracy of the current sensor is r, the fusion threshold is set to 2r+V max Δt. Then, determine the current time t k-1 The corresponding point x k-1 and the next time t k The corresponding point x k , calculate the distance between two adjacent points Δd = ||x k-1 -x k ||. If the measurement point at the next moment cannot be found, the point trace data fusion step can be ended directly. If the distance Δd between two adjacent points exceeds the fusion threshold 2r+V max Δt, the two points are not processed and the corresponding points are searched. Otherwise, the subsequent steps are executed. Then, the position weights of adjacent moments are calculated as the normalized w k-1 and w k , and use the corresponding fusion weight strategy. Since the causes of measurement errors of different sensors are different, the value of the position weight can be calculated and normalized based on the sub-satellite point distance, measurement covariance matrix, and scoring strategy. Then, weighted fusion is performed on two adjacent points to obtain the new point The corresponding measurement time value is The measurement time can also be selected according to the needs. Among them, Vmax is the maximum speed of the ship, generally 55.56 kilometers per hour, that is, 30 knots; △t is the time difference between adjacent points in the time series.

[0105] In the present invention, when estimating the speed and heading of a surface target under low-precision measurement, a local northeast celestial coordinate system is first established with the starting point of the track as the origin, and this is used as the tracking coordinate system. Then, all points on the track are converted from the geographic coordinate system to the local coordinate system. Set the conversion probability: the probability of the stationary model and the probability of the collaborative turning (CT) model; establish the three-axis state equations of the stationary model and the CT model, wherein the CT equation introduces the turning angular velocity state variable to establish an observation equation containing three-axis position measurement. Among them, the state equation of the collaborative turning model (i.e., the CT equation) introduces the turning angular velocity state variable to establish an observation equation containing three-axis position measurement.

[0106] The current measurement estimate is inferred based on the system state estimate and covariance estimate at the previous moment, and then the model is reinitialized and calculated. The new initial value is obtained through the Markov operation matrix between different models. EKF is used to derive the nonlinear state equation and Jacobian matrix; the weight ratio of the most suitable tracking model is given by calculating the similarity between the current model and the current moving target state. The final estimation result is obtained based on the calculation results of each model and the model matching weight to complete the filtering process. After that, all point traces are smoothed based on the filtering results. The smoothed value is constrained by the measurement covariance matrix of the final state and the process noise covariance matrix, thereby completing the smoothing process. Based on the speed at each moment, the heading is calculated and output; if there is imaging information input that is close in time, the heading is assisted by identifying the target ship's bow direction and other methods. See the speed and heading calculation section for details. After completing the processing of all tracks, the speed and heading estimation is completed.

[0107] Finally, if Figure 5 As shown, the estimation of speed and heading can be completed after the processing of all tracks is completed.

[0108] When calculating the speed and heading, if there is no other intelligence information to confirm, the formula for calculating the speed v(k) at the kth moment is:

[0109]

[0110] Among them, v e (k), v n (k) and v u (k) are the three-axis velocity components in the ENU coordinate system;

[0111] The heading angle is defined as a clockwise angle from north to east, with a range of [0,2π), then the heading Φ(k) at the kth moment is:

[0112]

[0113] Among them, x E (k) and x N(k) are the eastward and northward velocity components of the target, respectively.

[0114] The auxiliary heading determination method under the condition of confirmation of other imaging intelligence information is as follows: if only one point track in the whole track has corresponding imaging information input, the target ship bow or wake is identified when the resolution allows, so as to give the target heading; if there are multiple points in the whole track with corresponding imaging information input, the heading estimate can be obtained according to the ship bow direction and wake after static frame image recognition. And the track is generated by the positioning results of multi-frame imaging information, and the speed and heading estimates are output through filtering and smoothing; considering the above speed and heading estimates, multiple information is supplemented and fused to generate the final speed and heading estimates. Use multiple means to fuse and estimate the speed and heading. Specifically, the heading estimate is obtained according to the ship bow direction and wake after static frame image recognition. And the track is generated by the positioning results of multi-frame imaging information, and the speed and heading estimates are output through filtering and smoothing.

[0115] Among them, the weighted value is an empirical value or obtained by fusion using methods such as Kalman filtering / confidence assessment. If the point detection time of the imaging information is significantly different from the point detection time of the sensor with low measurement accuracy, proceed to the subsequent steps, otherwise the heading and speed estimation value is directly output. Subsequently, the track generated by the low measurement accuracy and the track generated by the imaging information are fused to complete the information of both parties. The fusion processing method can be covariance weighted trajectory fusion or adaptive trajectory fusion. Finally, the fused track is filtered and smoothed in the above manner to obtain the final heading and speed estimation value and output it.

[0116] The method of the present invention is described in detail below with a specific embodiment:

[0117] This implementation uses about 1 hour of real data of a ship target as input data, with an average speed of 1.3 knots and a positioning accuracy of 5 kilometers, so as to construct a typical low-speed motion or stationary state scenario of the ship target.

[0118] See also Figure 6 and Figure 7 , where the real track and the measured data are both located in the northeast celestial coordinate system where the track starting point is located. It can be seen that due to the low accuracy of the measured data, there is a significant deviation between the measured track and the real track, which is not conducive to the estimation of the heading angle. And the heading angle deviations corresponding to the 4th, 6th, and 7th measurement values ​​exceed 90 degrees, as shown in Table 1:

[0119]

[0120] Table 1. Course speed difference between the real point and the measured point

[0121] In this implementation, the target's latitude and longitude coordinates and timestamp are used as input, and the ENU coordinates are used to describe the track, speed and heading of the surface target as output. The setting of the fusion threshold is affected by the sensor measurement accuracy r, the time difference Δt between two adjacent points, and the average speed V of the target at that moment, and its expression is 2r+VΔt. Then the distance Δd between two adjacent points is calculated = ||x k-1 -x k ||, if Δd>2r+VΔt, the two points are not processed; otherwise, the two adjacent points are merged into one point according to the merging weight value.

[0122] This embodiment adopts the sub-satellite distance as a method for setting the weight value. Specifically, the combined weight value is inversely proportional to the sub-satellite distance, and the combined weight value w corresponding to the two points is k-1 and w k After normalization, the position weight is and The time corresponding to the new point trace obtained by weighted fusion is the middle value of the two moments, and the fused point trace will replace the original point trace. If continuous iterative fusion is performed, the fusion strategy is repeated until the distance between two adjacent points exceeds the fusion threshold; where d is the distance from the target measurement point to the sub-satellite point. As shown in Table 2, the use of the above point trace data fusion method has significantly improved the heading angle and speed estimation, and the angle deviation of more than 90 degrees has been eliminated:

[0123]

[0124] Table 2. Heading and speed differences between real points and pre-processed points

[0125] For sensors that cannot calculate fusion weights or have no historical statistical data, the fusion point trace can be taken as the average of the connected point traces within the fusion distance threshold.

[0126] In order to obtain better point fusion results, different fusion weight setting strategies can be used according to the positioning mechanism and measurement accuracy of the sensor to accurately restore the relative position relationship between adjacent points, such as using measurement error, measurement covariance matrix, measurement score, sub-satellite distance, etc. as the basis for fusion weight calculation. Among them, the larger the measurement error and measurement covariance matrix, the smaller the measurement score, and the smaller the weight in the fusion process; conversely, the larger the value. The strategies for using sub-satellite distance as fusion weight setting mainly include direct method, theoretical method and statistical method. This implementation method uses direct method, such as Figure 5 As shown, including:

[0127] Step 1: Calculate the subsatellite distance between two adjacent traces;

[0128] Step 2: Calculate the current positioning accuracy estimate according to the interval of the target's distance to the sub-satellite point, and take its reciprocal as the weight of the current point trace and the weight of the next point trace, respectively, denoted as w′ k-1 and w′ k ;

[0129] Step 3: For w′ k-1 and w′ k Normalize and get the final weight w for fusion k-1 and w k .

[0130] See also Figure 8 and Fig. 9 ,Due to the existence of measurement wild values, the current track and heading angle still have a certain deviation from the true value, so the track after point track preprocessing will be filtered and smoothed in the future, as shown in Table 3:

[0131]

[0132]

[0133] Table 3. Heading and speed differences between the real point and the smoothed point

[0134] It can be seen that the smoothed track is much closer to the real track, but compared with the point track preprocessing result, the error of the heading and speed estimation value of the last few points increases slightly. This is due to the large positioning error of the last few points and the lack of subsequent point correction and smoothing. In addition, the overall heading and speed deviation value after filtering and smoothing is also more stable.

[0135] The above is only one embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for estimating the motion of a surface target, comprising the following steps: (a) Preprocessing the motion point data of the surface target; (b) Estimate the speed and direction of the surface target; In step (a), when the distance between two adjacent points is within the fusion threshold, fusion is performed, and the fusion point is the average value of the connected points within the fusion threshold; If all the continuous points are within the fusion threshold, iterative fusion is performed; Use the sub-satellite point distance or measurement error as the basis for fusion weight calculation; Step (a) comprises the following steps: (a1) Sort the input point track data according to the measurement time and determine the track starting point; (a2) If the current sensor measurement accuracy is r, then the fusion threshold is set to 2r+V max △t; (a3) Find the current time t k-1 The corresponding point x k-1 and the next time t k The corresponding point x k , calculate the distance between two adjacent points △d = ||x k-1 -x k ||; If the measurement at the next moment cannot be found, execute step a8; (a4) If the distance △d between two adjacent points exceeds the fusion threshold 2r+V max △t, then there is no need to process these two points, and jump to step a3; otherwise, execute step a5; (a5) Calculate the position weights of adjacent moments, which are the normalized w k-1 and w k , and use the corresponding fusion weight strategy; (a6) Perform weighted fusion on two adjacent points to obtain a new point The corresponding measurement time value is (a7) Repeat steps (a2) to (a6) until there is no measurement point at the next moment; (a8) The measurement at the next moment cannot be found, and the point trace data preprocessing ends; Among them, Vmax is the maximum speed of the ship; △t is the time difference between adjacent points in the time series; In step (b), under the interactive multi-model framework, a set of state variables and state transition models are selected, and the speed and heading of the surface target are estimated using filtering and smoothing methods; Step (b) comprises the following steps: (b1) Establish a local northeast celestial coordinate system with the starting point of the track as the origin as the tracking coordinate system, and convert all points on the track from the geographic coordinate system to the local coordinate system; (b2) setting the transition probabilities to the probabilities of the stationary model and the coordinated turning model; (b3) establishing the three-axis state equations of the stationary model and the CT model, wherein the CT equation introduces the turning angular velocity state variable and establishes the observation equation including the three-axis position measurement; (b4) inferring the current measurement estimate based on the system state estimate and covariance estimate at the previous moment, and then reinitializing the model. The new initial value is obtained through the Markov operation matrix between different models. (b5) Use EKF to derive the nonlinear state equation and Jacobian matrix; (b6) giving the weight ratio of the most suitable tracking model at present by calculating the similarity between the current model and the current moving target state; (b7) obtaining an estimation result based on the individual calculation results of each model and the model matching weight, and completing the filtering process; (b8) performing smoothing on all traces based on the filtering result, wherein the smoothing value is constrained by the measurement covariance matrix of the final state and the process noise covariance matrix; the smoothing process ends; (b9) Calculate and output the heading based on the speed at each moment; if there is imaging information input close in time, assist in determining the heading by identifying the target ship's bow direction; (b10) All track processing is completed, and the speed and heading estimation is completed; In step (b), the speed v(k) at the kth moment is calculated as: Among them, v e (k), v n (k) and v u (k) are the velocity components of the three axes in the northeast celestial coordinate system; The heading angle is defined as a clockwise angle from north to east, with a range of [0,2π), then the heading Φ(k) at the kth moment is:

2. The method according to claim 1, characterized in that The method of using the subsatellite distance to obtain the position weight is to make the combined weighted value w corresponding to the two point traces k-1 and w k Inversely proportional to the distance from the subsatellite point, the combined weighted value is normalized to obtain the position weights of the two points. and The time corresponding to the new fusion point obtained by weighted fusion is the middle value of the two moments; The fused traces will replace the original traces; If continuous iterative fusion is performed, the fusion strategy is repeated until the distance between two adjacent points exceeds the fusion threshold; Among them, d is the distance from the current measurement point to the sub-satellite point; For sensors that cannot calculate fusion weights or have no historical statistical data, the fusion point trace is the average of the connected point traces within the fusion distance threshold; Different fusion weight setting strategies are used according to the positioning mechanism and measurement accuracy of the sensor, including using measurement error, measurement covariance matrix, measurement score and sub-satellite distance as the basis for fusion weight calculation; When the sub-satellite distance is used as the strategy for setting the fusion weight, it includes direct method, theoretical method and statistical method; Direct methods include: Calculate the subsatellite point distance between two adjacent traces; The current positioning accuracy estimate is calculated according to the interval of the target distance to the sub-satellite point, and its reciprocal is taken as the weight of the current point trace and the weight of the next point trace, respectively, denoted as w k ' -1 and w k ′; For w k ' -1 and w k ′ is normalized to obtain the final weight w for fusion k-1 and w k ; Theoretical methods include: The theoretical curve of positioning error from the zero position of the sub-satellite point to the maximum boundary of the precision guarantee area is calculated by the sensor; Calculate the subsatellite point distance between two adjacent traces; Based on the positioning error theoretical curve, the positioning error of two adjacent points is calculated, and the reciprocal is taken as the weight of the current point and the weight of the next point, respectively, denoted as w k ' -1 and w k ′; For w k ' -1 and w k ′ is normalized to obtain the final weight w for fusion k-1 and w k ; Statistical methods include: Perform statistical processing on the historical positioning accuracy of the sensor to obtain a positioning error statistical curve related to the sub-satellite point distance. According to the electromagnetic or optical characteristics of the target, obtain the positioning error statistical curve under different target characteristics. Calculate the subsatellite point distance between two adjacent traces; Based on the positioning error theoretical curve, the positioning error of two adjacent points is calculated, and the reciprocal is taken as the weight of the current point and the weight of the next point, respectively, denoted as w k ' -1 and w k ′; For w k ' -1 and w k ′ is normalized to obtain the final weight w for fusion k-1 and w k .

3. The method according to claim 1, characterized in that The method of assisting in determining the heading in step (b9) is: If only one point in the track has corresponding imaging information input, the target ship's bow or wake is identified if the resolution allows, and the target heading is given; If multiple points in the track have corresponding imaging information input, the heading estimate is obtained based on the bow direction and wake after static frame image recognition, and the track is generated based on the positioning results of multiple frames of imaging information, and the speed and heading estimates are output through filtering and smoothing. The above speed and heading estimates are combined with multiple information to generate the final speed and heading estimates.

4. The method according to claim 3, characterized in that The fusion estimation of speed and heading includes: (S1) generating a track based on the positioning result of the imaging information, and outputting an estimate of the initial speed and heading by filtering and smoothing; (S2) identifying the bow of the surface target in each frame, and obtaining the heading of the surface target in the static frame; (S3) performing weighted fusion on the headings obtained in step (S1) and step (S2), wherein the weighted value is an empirical value or is obtained by using Kalman filter / confidence assessment fusion; If the point trace detection time of the imaging information is significantly different from the point trace detection time of the sensor, the subsequent steps are performed, otherwise the estimated values ​​of the heading and speed are directly output; (S4) fusing the track generated by the low measurement accuracy and the track generated by the imaging information to complete the two track information; The fusion processing method is covariance weighted trajectory fusion or adaptive trajectory fusion; (S5) Filtering and smoothing the fused track to obtain and output estimated values ​​of heading and speed.

Images