Method and system for predicting position of water target, storage medium and electronic equipment
By acquiring the motion data of multiple sensors and using Kalman filtering technology to fuse information, the problem of low prediction accuracy on water targets in the prior art is solved, and higher prediction accuracy and safety are achieved.
Patent Information
- Application Number
- CN202411116601.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-15
- Publication Date
- 2024-12-03
AI Technical Summary
The prior art cannot effectively integrate information from multiple sensors, resulting in low prediction accuracy and poor practicality of target motion status on water.
By obtaining the motion data of multiple sensors on the boat, determining the motion function of the target, and using Kalman filtering technology to filter and fuse the original observations, calculate the target's position prediction value and covariance matrix estimate value to improve prediction accuracy .
It improves the prediction accuracy and accuracy of the movement status of water targets, and enhances the safety of boats avoid obstacles and avoidance when driving unmanned.
Smart Images

Figure CN119063712A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of ship navigation control, and in particular relates to a method, system, storage medium and electronic equipment for predicting the position of a target on water. Background Art
[0002] When patrolling or in combat scenarios, unmanned boats can perform fusion situation analysis on observed water targets to analyze the target's motion state in the future, and at the same time, provide corresponding key information for users to make decisions. However, related technologies cannot fuse information from multiple sensors and cannot predict the target's motion state from multiple dimensions, resulting in low prediction accuracy and poor practicality.
[0003] Therefore, how to propose a method that can improve the target prediction accuracy has become a technical problem that needs to be solved urgently. Summary of the invention
[0004] The present invention provides a method for predicting the position of a target on water, which is used for a boat, and a plurality of different sensors are arranged on the boat, and the method comprises: obtaining motion data of the target observed by the boat; determining the motion function of the target according to the motion data; determining a first position prediction value of the target at the current moment according to the motion function; obtaining original observation values of a plurality of sensors at the current moment; determining a second position prediction value of the target according to the original observation values and the first position prediction value; predicting the position prediction value of the target at the next moment according to the second position prediction value and the motion function; wherein the first position prediction value comprises a position estimation value and a covariance matrix estimation value, and the motion data comprises a velocity and an angular velocity of the target.
[0005] In some technical solutions, optionally, the step of determining the first position prediction value of the target at the current moment according to the motion function includes: determining the first position prediction value at the current moment according to the position prediction value at the previous moment and the motion function; and linearizing the first position prediction value according to the Jacobian matrix.
[0006] In some technical schemes, optionally, the step of determining the second position prediction value of the target based on the original observation value and the first position prediction value includes: using Kalman filtering to filter out interference and noise in the original observation value to obtain a filtered observation value; fusing the filtered observation value and the first position prediction value to obtain the second position prediction value of the target.
[0007] In some technical solutions, optionally, when the motion function of the target is a differentiable function, the differentiable function includes:
[0008]
[0009] Among them, x k represents the predicted position value at time k, xk-1 represents the position prediction value at time k-1, u k represents the system control input at time k, w k represents the external disturbance term, v k represents the observation noise of the system at time k, z k Represents the observed variable, function f represents the calculation of the current position prediction value from the past prediction values, and function h represents filtering out interference and noise in the original observation values.
[0010] In some technical solutions, optionally, acting on x k-1 The expressions of the state transfer matrix and Jacobian matrix are:
[0011]
[0012] Among them, F k Indicates that it acts on x k-1 State transition matrix, u k represents the system control input at time k, H k represents the Jacobian matrix, Represents the predicted value of the target position at time k-1.
[0013] In some technical solutions, optionally, when the motion function of the target is a uniform linear motion function, the motion vector of the target at time k is expressed as:
[0014]
[0015] Among them, x k , They are respectively represented as the position coordinate, velocity, acceleration in the X-axis direction at time k, and y k , They are respectively represented as the position coordinate, velocity, and acceleration in the Y-axis direction at time k, and T is the sampling time interval;
[0016] The linear target motion model equation in the Cartesian coordinate system is:
[0017]
[0018] Among them, X k+1 , X k They are respectively represented as the motion state vector of the target at time k+1 and time k, is represented as the state transfer matrix, Г is represented as the state noise input matrix, W k It is represented as the system state noise, which is set to zero-mean Gaussian white noise.
[0019] In some technical solutions, optionally, when the motion function of the target is a uniform velocity model, the state transfer matrix of the target at time k is It is expressed as:
[0020]
[0021] in, represents the state transfer matrix, T represents the sampling time interval;
[0022] The state noise input matrix Г of the target at time k is expressed as:
[0023]
[0024] Among them, Г represents the state noise input matrix, T represents the sampling time interval;
[0025] The model noise of the target at time k is expressed as:
[0026]
[0027] Among them, W k Denoted as model noise, w x,k It is expressed as the velocity component of the noise on the x-axis at time k, w y,k It is expressed as the velocity component of the noise on the y-axis at time k.
[0028] In some technical solutions, optionally, when the motion function of the target is a uniform acceleration model, the state transfer matrix of the target at time k is It is expressed as:
[0029]
[0030] in, represents the state transfer matrix, T represents the sampling time interval;
[0031] The state noise input matrix Г of the target at time k is expressed as:
[0032]
[0033] Among them, Г represents the state noise input matrix, T represents the sampling time interval;
[0034] The model noise of the target at time k is expressed as:
[0035]
[0036] Among them, W k Denoted as model noise, w x,k It is expressed as the velocity component of the noise on the x-axis at time k, w y,kIt is expressed as the velocity component of the noise on the y-axis at time k.
[0037] In some technical solutions, optionally, when the motion function of the target is a constant speed and rotation speed model, the motion vector of the target at time k is expressed as:
[0038]
[0039] Among them, CTRV X It is represented as x, y, v, w represent the target horizontal coordinate, vertical coordinate, angular velocity, linear velocity, angular acceleration, and linear acceleration, respectively, and T represents the sampling time interval period;
[0040] The equation for the target state transition is:
[0041]
[0042] Among them, x k+1 It is expressed as the predicted value of the position at time k+1, x k It is expressed as the predicted value of the position at time k.
[0043] In some technical solutions, optionally, when the motion function of the target is a constant speed and rotation speed model, the motion vector of the target at time k is expressed as:
[0044]
[0045] Among them, CTRV X It is represented as x, y, v, a, and w represent the target horizontal coordinate, vertical coordinate, angular velocity, linear velocity, angular acceleration, and linear acceleration, respectively, and T represents the sampling time interval;
[0046] The equation for the target state transition is:
[0047]
[0048] in,
[0049]
[0050] x k+1 It is expressed as the predicted value of the position at time k+1, x k It is expressed as the predicted value of the position at time k.
[0051] In some technical solutions, optionally, when the motion function of the target is a constant curvature radius and acceleration model, the position of the target at time k+1 is calculated as follows:
[0052]
[0053] Among them, the left column represents the nonlinear process noise, a a is the heading acceleration, and a ψ is the yaw acceleration, α k Expressed as the target velocity component in the X-axis direction, β k Expressed as the target velocity component in the Y-axis direction, ψ k It is represented as heading angle, and k is represented as the kth moment.
[0054] In some technical solutions, optionally, a method of determining the first position prediction value at the current moment according to the position prediction value at the previous moment and the motion function includes:
[0055]
[0056] The first position prediction value includes the estimated position value of the target at time k and the estimated value of the covariance matrix P of the target at time k k|k-1 , F k Represented as acting on x k-1 The state transfer matrix, B k It is represented as an input-control model acting on the control vector, u k Expressed as the control input variable, Q k is represented as process noise;
[0057] Methods for calculating the second position prediction value of the target include:
[0058]
[0059] The second position prediction value includes the new estimated value of the target at time k. and the new covariance matrix estimate P k|k , It is represented as the measurement margin, S is represented as the measurement margin covariance, K l Expressed as the optimal Kalman gain, S k , K k The specific calculation methods include:
[0060]
[0061] According to a second aspect of the present invention, there is provided a control system for predicting the position of a target on water, which is used for a boat. A plurality of different sensors are arranged on the boat, and the control system comprises: an acquisition module for acquiring motion data of the target observed by the boat; a determination module for determining the motion function of the target based on the motion data; a prediction module for determining a first position prediction value of the target at the current moment based on the motion function; the acquisition module is also used to acquire original observation values of a plurality of sensors at the current moment; the prediction module is also used to determine a second position prediction value of the target based on the original observation values and the first position prediction value; the prediction module is also used to predict the position prediction value of the target at the next moment based on the second position prediction value and the motion function; wherein the first position prediction value comprises a position estimation value and a covariance matrix estimation value, and the motion data comprises a velocity and an angular velocity of the target.
[0062] According to a third aspect of the present invention, there is provided a storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method for predicting the position of an above-water target in the first aspect of the present invention or any possible implementation of the first aspect.
[0063] According to a fourth aspect of the present invention, there is provided an electronic device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of the method for predicting the position of a target on water in the first aspect of the present invention or any possible implementation of the first aspect are implemented.
[0064] The beneficial effects brought by the present invention are as follows:
[0065] The present invention improves the prediction of the motion state of targets on water by boats. By acquiring target information from multiple sensors, the target information can be fused from multiple dimensions and noise can be removed, thereby improving the prediction accuracy of target motion. At the same time, a calibrated prediction value is obtained based on the estimated value at the previous moment combined with the observation value measured by the sensor, and the position of the target at the next moment is predicted based on the calibrated prediction value combined with the motion function, which greatly improves the prediction accuracy of the target. BRIEF DESCRIPTION OF THE DRAWINGS
[0066] Figure 1 A flow chart showing a method for predicting the position of an above-water target according to an embodiment of the present invention;
[0067] Figure 2 A schematic diagram showing the position of a target in a rectangular coordinate system according to an embodiment of the present invention;
[0068] Figure 3 A block diagram showing a control system for predicting the position of an above-water target according to an embodiment of the present invention;
[0069] Figure 4A schematic diagram showing a simulation experiment model of an embodiment of the present invention;
[0070] Figure 5 A block diagram of an electronic device according to an embodiment of the present invention is shown. DETAILED DESCRIPTION
[0071] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0072] like Figure 1 As shown, an embodiment of the present invention provides a method for predicting the position of a target on water, which is used for a boat, and a plurality of different sensors are arranged on the boat, including:
[0073] S101: Acquire motion data of a target observed by a vessel;
[0074] S102: Determine a motion function of the target according to the motion data;
[0075] S103: Determine a first position prediction value of the target at the current moment according to the motion function;
[0076] S104: Obtaining original observation values of multiple sensors at the current moment;
[0077] S105: Determine a second position prediction value of the target according to the original observation value and the first position prediction value;
[0078] S106: Predicting the position prediction value of the target at the next moment according to the second position prediction value and the motion function.
[0079] In this embodiment, the target observed on the water is first obtained, and the motion data of the target is obtained, and then the motion function of the target is determined according to the motion data of the target, and then the first position prediction value of the target at the current moment is determined according to the motion function. Specifically, the calculation method is determined according to the motion function of the target. After that, the original observation values of multiple sensors at the current moment are obtained, and the second position prediction value of the target is determined according to the original observation value and the first position prediction value. The process of determining the second position prediction value is also the process of fusing data. Finally, the position prediction value of the target at the next moment is predicted according to the second position prediction value and the motion function. Generally speaking, the target refers to other boats traveling on the sea. This embodiment can fuse the target information from multiple dimensions and remove noise by fusing the target information of multiple sensors, thereby improving the prediction accuracy of the target motion. At the same time, according to the estimated value at the previous moment combined with the observation value measured by the sensor, a calibrated prediction value is obtained, and the position of the target at the next moment is predicted according to the calibrated prediction value combined with the motion function, which greatly improves the prediction accuracy of the target, that is, the accuracy of the prediction value is improved by calibrating the input value, so that the boat can avoid other boats or obstacles when unmanned, or avoid some special objects, thereby improving the safety of boat automatic driving.
[0080] It is understandable that the prediction method provided in this embodiment adopts Kalman filtering to fuse it, and the data for determining the motion function is also obtained through sensors, and the original observation value is obtained through other sensors. The data from different sensors can be fused together by using Kalman filtering, thereby improving the prediction accuracy. Kalman filtering is a highly efficient recursive filter that can estimate the motion state of the target from a series of incomplete and noisy measurements. Specifically, it is possible to generate an estimate of the unknown variable based on the values of each observation at different times, considering the joint distribution at each time, and therefore it is more accurate than the estimation method based on a single observation. In fact, the above-mentioned Kalman filtering process is more suitable for linear functions, and the Kalman filter is also a basic Kalman filter.
[0081] In some embodiments, optionally, the step of determining the first position prediction value of the target at the current moment according to the motion function includes: determining the first position prediction value at the current moment according to the position prediction value at the previous moment and the motion function; and linearizing the first position prediction value according to the Jacobian matrix.
[0082] In this embodiment, the step of determining the first position prediction value of the target at the current moment according to the motion function includes: first determining the first position prediction value at the current moment according to the position prediction value at the previous moment and the motion function, and then linearizing the first position prediction value according to the Jacobian matrix. In other words, this embodiment is actually a cyclic process, according to the calibrated position prediction value obtained at the previous moment, the data is substituted into the motion function, and the first position prediction value is determined according to the motion function. Since the obtained first position prediction value may be nonlinear, it needs to be linearized to complete the subsequent steps.
[0083] In some embodiments, optionally, the step of determining the second position prediction value of the target based on the original observation value and the first position prediction value includes: using Kalman filtering to filter out interference and noise in the original observation value to obtain a filtered observation value; fusing the filtered observation value and the first position prediction value to obtain the second position prediction value of the target.
[0084] In this embodiment, the data acquired by the sensor contains interference information and is not accurate. In order to improve the accuracy of the acquired data, the Kalman filter is used to filter out the interference and noise in the original observation value to obtain the filtered observation value. That is to say, the original observation value acquired by the sensor contains interference information, and the Kalman filter is used to obtain the filtered observation value, thereby improving the accuracy of the prediction. At the same time, this embodiment also uses the Kalman filter to fuse the filtered observation value and the first position prediction value to obtain the second position prediction value of the target. The accuracy of the second position prediction value is higher than that of the first position prediction value. That is, the first position prediction value is corrected by the filtered observation value, which improves the prediction accuracy of the second position prediction value. By improving the accuracy of the input data, the accuracy of the output data is improved, that is, the accuracy of the target position prediction.
[0085] In some embodiments, optionally, when the motion function of the target is a differentiable function, the differentiable function includes:
[0086]
[0087] Among them, x k represents the predicted position value at time k, x k-1 represents the position prediction value at time k-1, u k represents the system control input at time k, w k represents the external disturbance term, v k represents the observation noise of the system at time k, z k Represents the observed variable, function f represents the calculation of the current position prediction value from the past prediction values, and function h represents filtering out interference and noise in the original observation values.
[0088] In this embodiment, the motion function of the target is a differentiable function. It should be noted that in real life, due to the influence of wind resistance or weather environment, general objects move nonlinearly, that is, the motion function of the target is nonlinear, which is also called a differentiable function. For such a target with changing speed, it is necessary to linearize the motion function of the target. Specifically, the position estimate is linearized according to the Jacobian matrix, and then the linearized function can be used to determine the first position prediction value and the second position prediction value.
[0089] It is understandable that the Kalman filter in this embodiment is an extended Kalman filter, which actually linearizes the information that needs to be weighted through the Jacobian matrix to ensure that the basic Kalman filter can be used for prediction. In many driverless car projects, an extended algorithm of Kalman filtering is required. The Kalman filter algorithm is a state estimation algorithm that combines prediction and measurement update.
[0090] In some embodiments, optionally, acting on x k-1 The expressions of the state transfer matrix and Jacobian matrix are:
[0091]
[0092] Among them, F k Indicates that it acts on x k-1 State transition matrix, u k represents the system control input at time k, H k represents the Jacobian matrix, Represents the predicted value of the target position at time k-1.
[0093] In this embodiment, the Jacobian matrix H k It can correctly transmit or weight the effective part of the observation information, linearize the function through the Jacobian matrix, and predict the nonlinear motion of the target through the basic Kalman filter, thereby improving the prediction accuracy of the target motion state.
[0094] In some embodiments, optionally, when the motion function of the target is a uniform linear motion function, the motion vector of the target at time k is expressed as:
[0095]
[0096] Among them, x k , They are respectively represented as the position coordinate, velocity, acceleration in the X-axis direction at time k, and y k , They are respectively represented as the position coordinate, velocity, and acceleration in the Y-axis direction at time k, and T is the sampling time interval;
[0097] The linear target motion model equation in the Cartesian coordinate system is:
[0098]
[0099] Among them, X k+1 , X k They are respectively represented as the motion state vector of the target at time k+1 and time k, is represented as the state transfer matrix, Г is represented as the state noise input matrix, W k It is represented as the system state noise, which is set to zero-mean Gaussian white noise.
[0100] In this embodiment, the motion function of the target is highly nonlinear, and the performance of the extended Kalman filter will be particularly poor. In this case, an unscented Kalman filter is used, such as Figure 2 As shown in the figure, the position information of the target P(x, y) in the motion coordinate system is displayed. Through the position coordinates, velocity and acceleration of the target on the X-axis and Y-axis at a certain moment, as well as the sampling time interval, a more accurate estimate of the true mean and covariance is obtained. The prediction results can be verified by Monte Carlo sampling or Taylor series expansion of posterior statistics.
[0101] In some embodiments, optionally, when the motion function of the target is a uniform velocity model, the state transfer matrix of the target at time k is It is expressed as:
[0102]
[0103] in, represents the state transfer matrix, T represents the sampling time interval;
[0104] The state noise input matrix Г of the target at time k is expressed as:
[0105]
[0106] Among them, Г represents the state noise input matrix, T represents the sampling time interval;
[0107] The model noise of the target at time k is expressed as:
[0108]
[0109] Among them, W k Denoted as model noise, w x,k It is expressed as the velocity component of the noise on the x-axis at time k, w y,kIt is expressed as the velocity component of the noise on the y-axis at time k.
[0110] In this embodiment, in the uniform linear motion model, the target's motion speed is a constant value, and the state transfer matrix The calculation of the state noise input matrix Г and the model noise is relatively simple and fast, which improves the prediction efficiency.
[0111] In some embodiments, optionally, when the motion function of the target is a uniform acceleration model, the state transfer matrix of the target at time k is It is expressed as:
[0112]
[0113] in, represents the state transfer matrix, T represents the sampling time interval;
[0114] The state noise input matrix Г of the target at time k is expressed as:
[0115]
[0116] Among them, Г represents the state noise input matrix, T represents the sampling time interval;
[0117] The model noise of the target at time k is expressed as:
[0118]
[0119] Among them, W k Denoted as model noise, w x,k It is expressed as the velocity component of the noise on the x-axis at time k, w y,k It is expressed as the velocity component of the noise on the y-axis at time k.
[0120] In this embodiment, the motion function of the target is a uniform acceleration model. Uniformly accelerated linear motion is linear motion in which the target's motion acceleration is a constant. The above calculation formula can also improve the accuracy of predicting the target position.
[0121] In some embodiments, optionally, when the motion function of the target is a constant speed and rotation speed model, the motion vector of the target at time k is expressed as:
[0122]
[0123] Among them, CTRV X It is represented as x, y, v, w represent the target horizontal coordinate, vertical coordinate, angular velocity, linear velocity, angular acceleration, and linear acceleration, respectively, and T represents the sampling time interval period;
[0124] The equation for the target state transition is:
[0125]
[0126] Among them, x k+1 It is expressed as the predicted value of the position at time k+1, x k It is expressed as the predicted value of the position at time k.
[0127] In this embodiment, the motion function of the target is a constant speed and rotation speed model. It is assumed that the target moves at a constant turning rate and speed, that is, the longitudinal acceleration and yaw acceleration are zero. The constant speed and rotation speed can be regarded as a circular arc motion. The above formula can also be used to predict the position information of the target at the next moment.
[0128] In some embodiments, optionally, when the motion function of the target is a constant speed and rotation speed model, the motion vector of the target at time k is expressed as:
[0129]
[0130] Among them, CTRV X It is represented as x, y, v, a, and w represent the target horizontal coordinate, vertical coordinate, angular velocity, linear velocity, angular acceleration, and linear acceleration, respectively, and T represents the sampling time interval;
[0131] The equation for the target state transition is:
[0132]
[0133] in,
[0134]
[0135] x k+1 It is expressed as the predicted value of the position at time k+1, x k It is expressed as the predicted value of the position at time k.
[0136] Derivation process: First, we understand the increments of θ and v very well. The first two items use the results of CTRV combined with the differential method. Assuming that starting from time t, within a very short time dt, we can approximate the CTRA model to the CTRV model (refer to the process of deriving uniform accelerated motion using the differential method). Then, the increments of x and y within the time dt can be expressed as:
[0137]
[0138]
[0139] Simplified, when dt is small, Δx can be approximated as:
[0140]
[0141] In this embodiment, the motion function of the target is a constant speed and rotation speed model. The angle turned in the same time is the same, and it becomes a uniformly accelerated motion in the radial direction. The above formula can also be used to predict the position information of the target at the next moment, thereby improving the accuracy of the prediction.
[0142] In some embodiments, optionally, when the motion function of the target is a constant curvature radius and an acceleration model, the curvature radius of the constant curvature radius and the acceleration model are constants. It can be seen from r=v / ω that when the curvature radius is constant and uniformly accelerated motion is required, the angular velocity must be changing all the time. Therefore, the angle turned in the same time will inevitably increase.
[0143] Assume that the radius of curvature c = 1 / r, then the state variables are as follows:
[0144]
[0145] At this time, the arc length is:
[0146]
[0147] The final result is:
[0148]
[0149] The position of the target at time k+1 is calculated as:
[0150]
[0151] Among them, the left column represents the nonlinear process noise, a a represents the heading acceleration, and a ψ is the yaw acceleration, α k Expressed as the target velocity component in the X-axis direction, β k Expressed as the target velocity component in the Y-axis direction, ψ k It is represented as heading angle, and k is represented as the kth moment.
[0152] In this embodiment, considering that the speed of the surface ship includes the forward speed and the turning speed, the state vector consists of five components: where p x , p y represents the object position, v represents the ship's heading speed, ψ represents the ship's yaw angle, and Represents the yaw speed of the ship. The target information prediction of the boat in the patrol scenario is obtained through the above method.
[0153] In some embodiments, optionally, the method of calculating the first position prediction value of the target includes:
[0154]
[0155] The first position prediction value includes the estimated position value of the target at time k and the estimated value of the covariance matrix P of the target at time k k|k-1 , F k Represented as acting on x k-1 The state transfer matrix, B k It is represented as an input-control model acting on the control vector, u k Expressed as the control input variable, Q k is represented as process noise;
[0156] Methods for calculating the second position prediction value of the target include:
[0157]
[0158] The second position prediction value includes the new estimated value of the target at time k and the new covariance matrix estimate P k|k , It is represented as the measurement margin, S is represented as the measurement margin covariance, K k Expressed as the optimal Kalman gain, S k , K k The specific calculation methods include:
[0159]
[0160] In this embodiment, the first position prediction value and the second position prediction value of the target can be calculated in the above manner. By continuously calculating the prediction values, a more accurate prediction value, that is, the second position prediction value, can be further obtained, thereby improving the prediction accuracy of the boat for the target, and also improving the prediction accuracy of the boat's own movement.
[0161] like Figure 3As shown, according to the second aspect of the present invention, a control system 10 for predicting the position of a target on water is provided, which is used for a boat, and a plurality of different sensors are arranged on the boat. The control system 10 comprises: an acquisition module 11, which is used to acquire the motion data of the target observed by the boat; a determination module 12, which is used to determine the motion function of the target according to the motion data; a prediction module 13, which is used to determine the first position prediction value of the target at the current moment according to the motion function; the acquisition module 11 is also used to acquire the original observation values of the plurality of sensors at the current moment; the prediction module 13 is also used to determine the second position prediction value of the target according to the original observation values and the first position prediction value; the prediction module 13 is also used to predict the position prediction value of the target at the next moment according to the second position prediction value and the motion function; wherein the first position prediction value comprises the position estimation value and the covariance matrix estimation value, and the motion data comprises the velocity and angular velocity of the target. .
[0162] In this embodiment, the control system 10 for predicting the position of the target on the water first obtains the target observed on the water through the acquisition module 11, and obtains the motion data of the target, then the determination module 12 determines the motion function of the target according to the motion data of the target, and the prediction module 13 determines the first position prediction value of the target at the current moment according to the motion function. Specifically, the calculation method is determined according to the motion function of the target. After that, the acquisition module 11 also obtains the original observation values of multiple sensors at the current moment, and the prediction module 13 determines the second position prediction value of the target according to the original observation values and the first position prediction value. The process of determining the second position prediction value is also the process of fusing data. Finally, the prediction module 13 predicts the position prediction value of the target at the next moment according to the second position prediction value and the motion function. Generally speaking, the target refers to other ships and boats traveling on the sea. This embodiment can fuse target information from multiple dimensions and remove noise by fusing target information from multiple sensors, thereby improving the prediction accuracy of target motion. At the same time, a calibrated prediction value is obtained based on the estimated value at the previous moment in combination with the observation value measured by the sensor, and the position of the target at the next moment is predicted based on the calibrated prediction value in combination with the motion function, which greatly improves the prediction accuracy of the target. That is, the accuracy of the prediction value is improved by calibrating the input value, so that the boat can avoid other boats or obstacles or some special objects when it is unmanned, thereby improving the safety of automatic driving of the boat.
[0163] It is understandable that the prediction method provided in this embodiment adopts Kalman filtering to fuse it, and the data for determining the motion function is also obtained through sensors, and the original observation value is obtained through other sensors. The data from different sensors can be fused together by using Kalman filtering, thereby improving the prediction accuracy. Kalman filtering is a highly efficient recursive filter that can estimate the motion state of the target from a series of incomplete and noisy measurements. Specifically, it is possible to generate an estimate of the unknown variable based on the values of each observation at different times, considering the joint distribution at each time, and therefore it is more accurate than the estimation method based on a single observation. In fact, the above-mentioned Kalman filtering process is more suitable for linear functions, and the Kalman filter is also a basic Kalman filter.
[0164] The accuracy rate in this embodiment refers to the number of correct judgments among all the judgments, that is, the positive is judged as positive, and the negative is judged as negative; there are TP+FN+FP+TN in total, so the accuracy rate is: Acc=(TP+TN) / (TP+TN+FN+FP). Among them, TP: True Positive is the number of positives judged as positive True Positive, the judgment is correct and it is judged as positive, that is, the positive prediction is positive. FN: False Negative is the number of positives wrongly judged as negative False Negative, the judgment is wrong and it is judged as negative, that is, the positive is judged as negative. FP: False Positive is the number of negatives wrongly judged as positive False Positive, the judgment is wrong and it is judged as positive, that is, the negative is judged as positive. TN: True Negative is the number of negatives judged as negative True Negative, the judgment is correct and it is judged as negative, that is, the negative is judged as negative.
[0165] In addition, in specific applications, the recognition and prediction algorithms of target continuous actions can also be simulated and evaluated based on convolutional neural networks. Figure 4 As shown in the figure, the simulation model in the experiment is shown. The experiment uses an action data set. Each sample contains multiple frames of time series data, which constitute the continuous action of the target. The input uses a fixed 8 frames as input, and a sparse sampling method is used. Because the time series data is processed, a three-dimensional convolution is used to model the temporal information.
[0166] By adding the dimension of time (continuous frames) to the input of the neural network, the neural network is given the function of action recognition. This embodiment proposes a 3D convolution, which convolves three consecutive frames with a 3D convolution kernel.
[0167] The calculation results are shown in Table 1. According to the calculation results, the recognition and prediction algorithm of the target continuous action based on the convolutional neural network basically meets the index requirements.
[0168] Table 1
[0169] model accuracy imagenet+linear SVM 68.8% iDT+linear SVM 76.2% C3D+linear SVM 82.3% Ours 90.3%
[0170] According to a third aspect of the present invention, there is provided a storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method for predicting the position of an above-water target in the first aspect of the present invention or any possible implementation of the first aspect.
[0171] like Figure 5 As shown, according to the fourth aspect of the present invention, an electronic device 20 is provided, including a memory 21, a processor 22, and a computer program stored in the memory 21 and executable on the processor 22. When the processor 22 executes the program, the steps of the method for predicting the position of a water target in the first aspect of the present invention or any possible implementation of the first aspect are implemented.
[0172] The above are preferred embodiments of the present invention. It should be pointed out that, for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.
Claims
1. A method for predicting the position of a target on water, It is characterized in that For a boat, the boat is provided with a plurality of different sensors, the method comprising: Acquiring motion data of a target observed by the vessel; determining a motion function of the target based on the motion data; Determine a first position prediction value of the target at the current moment according to the motion function; Obtaining original observation values of the plurality of sensors at the current moment; Determine a second position prediction value of the target according to the original observation value and the first position prediction value; Predicting a position prediction value of the target at the next moment according to the second position prediction value and the motion function; The first position prediction value includes a position estimation value and a covariance matrix estimation value, and the motion data includes a speed and an angular speed of the target.
2. The method for predicting the position of a target on water according to claim 1, It is characterized in that The step of determining the first position prediction value of the target at the current moment according to the motion function comprises: Determine the first position prediction value at the current moment according to the position prediction value at the previous moment and the motion function; The first position prediction value is linearized according to a Jacobian matrix.
3. The method for predicting the position of a target on water according to claim 1, It is characterized in that The step of determining the second position prediction value of the target according to the original observation value and the first position prediction value comprises: Using Kalman filtering to filter out interference and noise in the original observation value to obtain a filtered observation value; The filtered observation value and the first position prediction value are fused to obtain a second position prediction value of the target.
4. The method for predicting the position of a target on water according to claim 3, It is characterized in that When the motion function of the target is a differentiable function, the differentiable function includes: Among them, x k represents the predicted position value at time k, x k-1 represents the position prediction value at time k-1, u k represents the system control input at time k, w k represents the external disturbance term, v k represents the observation noise of the system at time k, z k represents the observed variable, function f represents the calculation of the current position prediction value from the past prediction values, and function h represents the filtering of interference and noise in the original observation value; and / or Acting on x k-1 The expressions of the state transfer matrix and Jacobian matrix are: Among them, F k Indicates that it acts on x k-1 State transition matrix, u k represents the system control input at time k, H k represents the Jacobian matrix, Represents the predicted value of the target position at time k-1.
5. The method for predicting the position of a target on water according to claim 1, It is characterized in that When the motion function of the target is a uniform linear motion function, the motion vector of the target at time k is expressed as: Among them, x k , They are respectively represented as the position coordinate, velocity, acceleration in the X-axis direction at time k, and y k , They are respectively represented as the position coordinate, velocity, and acceleration in the Y-axis direction at time k, and T is the sampling time interval; The linear target motion model equation in the Cartesian coordinate system is: Among them, X k+1 , X k They are respectively represented as the motion state vector of the target at time k+1 and time k, is represented as the state transfer matrix, Г is represented as the state noise input matrix, W k Represented as system state noise, set to zero-mean Gaussian white noise; and / or When the motion function of the target is a uniform velocity model, the state transfer matrix of the target at time k is It is expressed as: in, represents the state transfer matrix, T represents the sampling time interval; The state noise input matrix Г of the target at time k is expressed as: Among them, Г represents the state noise input matrix, T represents the sampling time interval; The model noise of the target at time k is expressed as: Among them, W k Denoted as model noise, w x,k It is expressed as the velocity component of the noise on the x-axis at time k, w y,k It is expressed as the velocity component of the noise on the y-axis at time k.
6. The method for predicting the position of a target on water according to claim 1, It is characterized in that When the motion function of the target is a uniform acceleration model, the state transfer matrix of the target at time k is It is expressed as: in, represents the state transfer matrix, T represents the sampling time interval; The state noise input matrix Г of the target at time k is expressed as: Among them, Г represents the state noise input matrix, T represents the sampling time interval; The model noise of the target at time k is expressed as: Among them, W k Denoted as model noise, w x,k It is expressed as the velocity component of the noise on the x-axis at time k, w y,k Expressed as the velocity component of the noise on the y-axis at time k; and / or When the motion function of the target is a constant speed and rotation speed model, the motion vector of the target at time k is expressed as: CTRV X =(x y θ v w) T ; Among them, CTRV X It is expressed as, x, y, θ, v, w represent the target horizontal coordinate, vertical coordinate, angular velocity, linear velocity, angular acceleration, linear acceleration respectively, and T represents the sampling time interval period; The equation for the state transition of the target is: Among them, x k+1 It is expressed as the predicted value of the position at time k+1, x k is represented as the predicted position value at time k; and / or When the motion function of the target is a constant speed and rotation speed model, the motion vector of the target at time k is expressed as: CTRA X =(x y θ v a w) T ; Among them, CTRV X It is expressed as, x, y, θ, v, a, w represent the target horizontal coordinate, vertical coordinate, angular velocity, linear velocity, angular acceleration, linear acceleration respectively, and T represents the sampling time interval; The equation for the state transition of the target is: in, x k+1 It is expressed as the predicted value of the position at time k+1, x k is represented as the predicted position value at time k; and / or When the motion function of the target is a constant curvature radius and acceleration model, the position of the target at time k+1 is calculated as follows: Among them, the left column represents the nonlinear process noise, a a is the heading acceleration, and a ψ is the yaw acceleration, α k Expressed as the target velocity component in the X-axis direction, β k Expressed as the target velocity component in the Y-axis direction, ψ k It is represented as heading angle, and k is represented as the kth moment.
7. The method for predicting the position of a target on water according to any one of claims 2 to 6, It is characterized in that The method of determining the first position prediction value at the current moment according to the position prediction value at the previous moment and the motion function includes: The first position prediction value includes the estimated position value of the target at time k. and the covariance matrix estimate P of the target at time k k|k-1 , F k Represented as acting on x k-1 The state transfer matrix, B k It is represented as an input-control model acting on the control vector, u k Expressed as the control input variable, Q k is represented as process noise; The method of calculating the second position prediction value of the target includes: The second position prediction value includes the new estimated value of the target at time k and the new covariance matrix estimate P k|k , is the measurement margin, S is the measurement margin covariance, K k Expressed as the optimal Kalman gain, S k , K k The specific calculation methods include:
8. A control system for predicting the position of a target on water. It is characterized in that For use in a boat, the boat is provided with a plurality of different sensors, and the control system comprises: An acquisition module, used to acquire motion data of a target observed by the vessel; A determination module, configured to determine a motion function of the target according to the motion data; A prediction module, used to determine a first position prediction value of the target at a current moment according to the motion function; The acquisition module is further used to acquire the original observation values of the multiple sensors at the current moment; The prediction module is used to determine a second position prediction value of the target according to the original observation value and the first position prediction value; The prediction module is further used to predict the position prediction value of the target at the next moment according to the second position prediction value and the motion function; The first position prediction value includes a position estimation value and a covariance matrix estimation value, and the motion data includes a speed and an angular speed of the target.
9. A storage medium having a computer program stored thereon, It is characterized in that When the program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.
10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, It is characterized in that When the processor executes the program, the steps of the method according to any one of claims 1 to 7 are implemented.
