Projectile trajectory prediction method, system and device based on UKF and storage medium

Abstract

The application discloses a kind of UKF-based trajectory prediction method of projectile, system, equipment and storage medium, the method includes: through observation model, current time observation position vector under machine coordinate system is converted into current time global position vector under global coordinate system;Current time global position vector is determined based on the current time global position vector through uncontrolled projectile dynamics model current time global state quantity of projectile;According to current time global state quantity, the current time state estimation vector of projectile is constructed;According to current time state estimation vector and next time observation position vector, next time state estimation vector is calculated through UKF recursive algorithm;According to next time state estimation vector, the trajectory prediction of projectile is realized through projectile dynamics discrete state equation.The present application does not need to rely on prior information of the dynamics parameters such as mass of projectile, solves the numerical instability problem caused by large span of parameter value range through logarithmic space mapping strategy, and improves the parameter estimation accuracy.

Description

Technical Field

[0001] This invention relates to the field of dynamic obstacle avoidance technology for mobile robots, and in particular to a method, system, device, and storage medium for predicting projectile trajectories based on UKF. Background Technology

[0002] In rescue or industrial scenarios, mobile robots may encounter projectiles falling from heights. Real-time prediction of the projectile's trajectory is crucial to provide input for dynamic obstacle avoidance algorithms and ensure the robot's safe operation. Existing projectile trajectory prediction methods are typically based on classical state estimation theory, primarily including techniques such as Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Particle Filter (PPF), which infer the projectile's future trajectory through a dynamic model. Among these, EKF performs state prediction by linearizing the nonlinear system model, offering high processing efficiency. However, the linearization process introduces significant errors, making it difficult to guarantee estimation accuracy in highly nonlinear scenarios. In contrast, the UKF method directly models and infers the nonlinear system through unscented transformation, avoiding the linearization errors of EKF. This results in higher estimation accuracy when dealing with nonlinear systems, and thus it is widely adopted.

[0003] While existing UKF-based projectile trajectory prediction methods have improved the state estimation capabilities of nonlinear systems to some extent, they still face the following challenges in practical applications: Traditional methods typically require prior measurement of dynamic parameters such as the projectile's mass, drag coefficient, and frontal area. However, in complex and unknown environments, these parameters are difficult to measure directly, leading to decreased prediction accuracy. Furthermore, the dynamic parameters of different types of projectiles vary significantly, ranging from 0.1 kg to hundreds of kg in mass, from 0.2 for smooth surfaces to over 1.0 for irregular objects, and from 0.001 m² for small projectiles to several square meters for large objects. These parameters span multiple orders of magnitude, resulting in significantly different dynamic characteristics and increasing the risk of numerical instability, further impacting state estimation performance.

[0004] The above content is only used to help understand the technical solution of the present invention and does not represent an admission that the above content is prior art. Summary of the Invention

[0005] The main objective of this invention is to provide a UKF-based method, system, device, and storage medium for predicting the trajectory of a projectile, aiming to solve the technical problem of how to predict the future motion trajectory of a projectile in a complex and unknown environment.

[0006] To achieve the above objectives, the present invention provides a UKF-based method for predicting projectile trajectories, the UKF-based method comprising:

[0007] Obtain the current observation position vector of the projectile in the machine coordinate system;

[0008] The observation model converts the current observation position vector in the machine coordinate system into the current global position vector in the global coordinate system.

[0009] Based on the current global position vector, the current global state of the projectile is determined by the uncontrolled projectile dynamics model. The current global state includes three-dimensional coordinates, velocity components, and the equivalent drag coefficient in logarithmic space.

[0010] Construct the current state estimation vector of the projectile based on the three-dimensional coordinates, the velocity components, and the equivalent drag coefficient in logarithmic space;

[0011] The next state estimation vector of the projectile is calculated using the UKF recursive algorithm based on the current state estimation vector and the next observation position vector of the projectile.

[0012] The trajectory prediction of the projectile is achieved by using the discrete state equations of projectile dynamics based on the state estimation vector at the next moment.

[0013] Optionally, before the step of converting the current observation position vector in the machine coordinate system to the current global position vector in the global coordinate system using the observation model, the method further includes:

[0014] Determine the planar coordinates and heading angle of the mobile robot in the global coordinate system;

[0015] An observation matrix is ​​constructed based on the heading angle and the state space vector through a coordinate mapping relationship, and an offset vector is constructed based on the plane coordinates and the heading angle.

[0016] An observation model is established based on the observation matrix, the offset vector, and the measurement noise.

[0017] Optionally, before the step of determining the current global state of the projectile based on the current global position vector using the uncontrolled projectile dynamics model, the method further includes:

[0018] Based on the current global position vector, velocity vector, object mass, air resistance magnitude, air density, air resistance coefficient, frontal area, and gravitational acceleration, an uncontrolled projectile dynamic model is constructed using dynamic relationships.

[0019] Optionally, the step of determining the current global state of the projectile based on the current global position vector using an uncontrolled projectile dynamics model includes:

[0020] The three-dimensional coordinates and velocity components are determined based on the global position vector at the current moment.

[0021] Based on the uncontrolled projectile dynamics model, the object's mass, air resistance, air density, air resistance coefficient, frontal area, and gravitational acceleration are integrated into a one-dimensional equivalent drag using a logarithmic mapping strategy to obtain the equivalent drag coefficient in logarithmic space.

[0022] Optionally, the step of calculating the next-time state estimation vector of the projectile using the UKF recursive algorithm based on the current-time state estimation vector and the next-time observed position vector of the projectile includes:

[0023] Multiple sigma points are generated based on the current state estimation vector.

[0024] Multiple sigma points are propagated through the state transition function to obtain multiple predicted sigma points, and the predicted state mean and predicted covariance are calculated based on each predicted sigma point.

[0025] Each predicted sigma point is mapped to the observation space to determine multiple observed sigma points, and the predicted measurement mean and predicted measurement covariance are calculated based on the observed sigma points.

[0026] The next-time state estimation vector of the projectile is calculated using the UKF recursive algorithm based on the observed position vector of the projectile at the next moment, the predicted state mean, the predicted covariance, the predicted measurement mean, and the predicted measurement covariance.

[0027] Optionally, the step of calculating the estimated state vector of the projectile at the next moment using the UKF recursive algorithm based on the observed position vector of the projectile at the next moment, the predicted state mean, the predicted covariance, the predicted measurement mean, and the predicted measurement covariance includes:

[0028] The cross covariance is calculated based on each predicted sigma point, each observed sigma point, the mean of the predicted state, and the mean of the predicted measurement.

[0029] Calculate the Kalman gain based on the predicted measurement covariance and the cross covariance;

[0030] The next-time state estimation vector of the projectile is calculated using the UKF recursive algorithm based on the observed position vector of the projectile at the next moment, the predicted state mean, the predicted measurement mean, and the Kalman gain.

[0031] Furthermore, to achieve the above objectives, this invention also proposes a UKF-based projectile trajectory prediction system, which includes:

[0032] The acquisition module is used to obtain the current observation position vector of the projectile in the machine coordinate system;

[0033] The conversion module is used to convert the current observation position vector in the machine coordinate system into the current global position vector in the global coordinate system through the observation model;

[0034] The calculation module is used to determine the current global state of the projectile based on the current global position vector through the uncontrolled projectile dynamics model. The current global state includes three-dimensional coordinates, velocity components, and the equivalent drag coefficient in logarithmic space.

[0035] The calculation module is also used to construct the current state estimation vector of the projectile based on the three-dimensional coordinates, the velocity components, and the equivalent drag coefficient in the logarithmic space.

[0036] The calculation module is also used to calculate the next-time state estimation vector of the projectile based on the current-time state estimation vector and the next-time observation position vector of the projectile using the UKF recursive algorithm;

[0037] The prediction module is used to predict the trajectory of the projectile based on the next moment state estimation vector through the discrete state equations of projectile dynamics.

[0038] Furthermore, to achieve the above objectives, the present invention also proposes a UKF-based projectile trajectory prediction device, the device comprising: a memory, a processor, and a UKF-based projectile trajectory prediction program stored in the memory and executable on the processor, the UKF-based projectile trajectory prediction program being configured to implement the steps of the UKF-based projectile trajectory prediction method described above.

[0039] Furthermore, to achieve the above objectives, the present invention also proposes a storage medium storing a UKF-based projectile trajectory prediction program, wherein when the UKF-based projectile trajectory prediction program is executed by a processor, it implements the steps of the UKF-based projectile trajectory prediction method described above.

[0040] This invention first obtains the current observed position vector of the projectile in the machine coordinate system. Then, it converts the current observed position vector in the machine coordinate system into the current global position vector in the global coordinate system using an observation model. Next, based on the current global position vector, the current global state variables of the projectile are determined using an uncontrolled projectile dynamics model. The current global state variables include three-dimensional coordinates, velocity components, and the equivalent drag coefficient in logarithmic space. The current state estimation vector of the projectile is constructed based on the three-dimensional coordinates, velocity components, and the equivalent drag coefficient in logarithmic space. Finally, the next state estimation vector of the projectile is calculated using the UKF recursive algorithm based on the current state estimation vector and the next observed position vector of the projectile. The trajectory prediction of the projectile is achieved using the discrete state equations of projectile dynamics based on the next state estimation vector. This invention integrates dynamic parameters such as projectile mass, air drag coefficient, and frontal area into an equivalent drag coefficient, thereby reducing the complexity of parameter estimation. By introducing a logarithmic space mapping strategy, the equivalent drag coefficient is converted to logarithmic space for estimation, solving the numerical instability problem caused by parameter values ​​of different types of projectiles spanning multiple orders of magnitude, and improving the stability and accuracy of the estimation process. At the same time, the equivalent drag coefficient in logarithmic space is used as a component of the extended state vector and jointly estimated with the real-time position and velocity state variables of the projectile. Based on the UKF framework, dynamic updates are performed. Combining the projectile dynamics model and the state variables and equivalent drag coefficients estimated by UKF, the future trajectory of the projectile can be predicted. Attached Figure Description

[0041] Figure 1 This is a schematic diagram of the structure of a UKF-based projectile trajectory prediction device in the hardware operating environment of the embodiment of the present invention.

[0042] Figure 2 This is a flowchart illustrating the first embodiment of the UKF-based projectile trajectory prediction method of the present invention.

[0043] Figure 3 This is a schematic diagram of the sensing information input for the first embodiment of the UKF-based projectile trajectory prediction method of the present invention;

[0044] Figure 4 This is a simulation diagram of the trajectory of a mobile robot in the first embodiment of the UKF-based projectile trajectory prediction method of the present invention.

[0045] Figure 5 This is a structural block diagram of the first embodiment of the UKF-based projectile trajectory prediction system of the present invention.

[0046] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0047] It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the scope of the invention.

[0048] Reference Figure 1 , Figure 1 This is a schematic diagram of the structure of a UKF-based projectile trajectory prediction device for the hardware operating environment involved in the embodiments of the present invention.

[0049] like Figure 1 As shown, the UKF-based projectile trajectory prediction device may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to enable communication between these components. The user interface 1003 may include a display screen or an input unit such as a keyboard; optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wireless-Fidelity (Wi-Fi) interface). The memory 1005 may be high-speed random access memory (RAM) or stable non-volatile memory (NVM), such as a disk storage device. The memory 1005 may also optionally be a storage system independent of the aforementioned processor 1001.

[0050] Those skilled in the art will understand that Figure 1 The structure shown does not constitute a limitation on UKF-based projectile trajectory prediction devices and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0051] like Figure 1 As shown, the memory 1005, which serves as a storage medium, may include an operating system, a network communication module, a user interface module, and a UKF-based projectile trajectory prediction program.

[0052] exist Figure 1In the UKF-based projectile trajectory prediction device shown, the network interface 1004 is mainly used for data communication with the network server; the user interface 1003 is mainly used for data interaction with the user; the processor 1001 and memory 1005 in the UKF-based projectile trajectory prediction device of the present invention can be set in the UKF-based projectile trajectory prediction device. The UKF-based projectile trajectory prediction device calls the UKF-based projectile trajectory prediction program stored in the memory 1005 through the processor 1001 and executes the UKF-based projectile trajectory prediction method provided in the embodiment of the present invention.

[0053] This invention provides a UKF-based method for predicting projectile trajectories, referring to... Figure 2 , Figure 2 This is a flowchart illustrating the first embodiment of the UKF-based projectile trajectory prediction method of the present invention.

[0054] In this embodiment, the UKF-based projectile trajectory prediction method includes the following steps:

[0055] Step S10: Obtain the current observation position vector of the projectile in the machine coordinate system.

[0056] It is easy to understand that the execution subject of this embodiment can be a UKF-based projectile trajectory prediction system with functions such as data processing, network communication and program execution, or other computer devices with similar functions. This embodiment does not limit it.

[0057] In a specific implementation case, a wheeled mobile robot is used as the carrier, and the projectile sensing sensor is mounted on the top of the robot. The sensing input is the three-dimensional coordinates (x, y, y) of the projectile in the robot coordinate system with Gaussian noise. m ,y m ,z m Simultaneously, the two-dimensional planar coordinates X, Y and heading angle ψ of the mobile robot are acquired in real time, such as... Figure 3 As shown, Figure 3 This is a schematic diagram of the sensory information input for the first embodiment of the UKF-based projectile trajectory prediction method of the present invention.

[0058] It should also be noted that the mobile robot starts from the origin at a speed of 3 m / s and travels along an S-shaped trajectory, as shown in the figure. Figure 4 As shown, Figure 4 This is a simulation diagram of a mobile robot trajectory, representing the first embodiment of the UKF-based projectile trajectory prediction method of the present invention.

[0059] Assume we choose a falling rock, a basketball, and a light ball as projectiles. The combined drag coefficients of these three projectiles span several orders of magnitude, with significant differences in parameters. Specific parameters are shown in the table below:

[0060]

[0061] The sensor is mounted on the top of the mobile robot and receives projectile observation data at a frequency of 300Hz. The observed coordinate data contains Gaussian noise. The received projectile observation data is the current observation position vector z of the projectile in the machine coordinate system. k =[x m,k y m,k z m,k ] T x m,k ,y m,k ,z m,k It is the three-dimensional coordinate of the projectile in the robot coordinate system at time step k.

[0062] It should also be noted that the current observation position vector includes the observation position vector of the first frame and the observation position vector of the second frame, i.e., the observation position vectors of the first frame z0 and the second frame z1. The initial velocity can be estimated based on the observation position vectors of the first frame and the second frame.

[0063] Step S20: Convert the current observation position vector in the machine coordinate system into the current global position vector in the global coordinate system using the observation model.

[0064] Furthermore, the planar coordinates X and Y and the heading angle ψ of the mobile robot in the global coordinate system are determined; an observation matrix is ​​constructed based on the heading angle and the state space vector through coordinate mapping relationship, and an offset vector is constructed based on the planar coordinates and the heading angle; an observation model is established based on the observation matrix, the offset vector and the measurement noise.

[0065] Observation model:

[0066] z k =H(x) k )+b k +v k

[0067]

[0068] b k =[-Xcosψ-YsinψXsinψ-Ycosψ0] T

[0069] v k =d m [n x,k n y,k n z,k ] T

[0070] In the formula, H is the observation matrix, x k Let b be a state-space vector. kLet v be the offset vector. k To measure noise, d m n is the distance correlation factor, representing the variation of noise intensity with distance. x,k ,n y,k ,n z,k It is a random noise vector.

[0071] The measurement noise covariance matrix R is defined as follows:

[0072]

[0073] In this experiment, the measurement noise covariance matrix R was set to:

[0074] R = diag(0.1 0.1 0.1)

[0075] In the specific implementation, based on the coordinate observation results of the first frame (i.e., the observation position vector of z0 in the first frame), it is transformed into the global coordinate system as the initial position estimate:

[0076] [x g,0 ,y g,0 ,z g,0 ] T =(H [:,1:3] ) -1 (z0-b0)

[0077] Among them, H [:,1:3] This represents the first 3x3 of the observation matrix H, [x g,0 ,y g,0 ,z g,0 [] represents the global position vector at the current moment in the global coordinate system.

[0078] Step S30: Based on the current global position vector, determine the current global state of the projectile using the uncontrolled projectile dynamics model. The current global state includes three-dimensional coordinates, velocity components, and the equivalent drag coefficient in logarithmic space.

[0079] In this embodiment, when describing the dynamic characteristics of an uncontrolled projectile, a dynamic model needs to be constructed based on the following reasonable assumptions:

[0080] 1) During flight, the air drag coefficient and frontal area remain constant;

[0081] 2) Only gravity and air resistance are considered, while other secondary forces and moments are ignored;

[0082] 3) Gravitational acceleration is constant and its direction is vertically downward;

[0083] 4) The direction of air resistance is opposite to the direction of flight speed;

[0084] 5) Under standard meteorological conditions.

[0085] Based on the above assumptions, an uncontrolled projectile dynamics model is established as follows:

[0086]

[0087] Where r is the position vector of the projectile in the global coordinate system, v is the velocity vector, m is the mass of the object, k is the vertical direction vector in the global coordinate system, and F... d It is the magnitude of air resistance, ρ is the air density, and C is the air resistance. d is the air resistance coefficient, A is the frontal area, and g is the acceleration due to gravity.

[0088] A state-space model (i.e., state-space vector) also needs to be established.

[0089] The projectile motion state needs to be described using three-dimensional position and three-dimensional velocity. Parameters such as projectile mass, air drag coefficient, and frontal area are difficult to obtain directly. These parameters are integrated using a one-dimensional equivalent drag coefficient, and a logarithmic mapping strategy compresses the parameter range to the [-6, -1] interval, improving numerical stability. The following extended state vector is constructed:

[0090]

[0091] Where, x g,k ,y g,k ,z g,k It is the three-dimensional coordinate of the projectile in the global coordinate system at time step k, v xg,k ,v yg,k ,v zg,k It is the velocity component, δ k It is the equivalent drag coefficient defined in logarithmic space, x g,k y g,k z g,k v xg,k v yg,k v zg,k δ k These are the global state variables at time k.

[0092] The discrete system state equations corresponding to the projectile dynamics model (i.e., the discrete state equations of projectile dynamics) are as follows:

[0093] x k+1 =f(x) k )+w k

[0094]

[0095] Among them, the state transition equation f(x) k ) represents state prediction based on a dynamic model.

[0096] The process noise matrix Q is defined as:

[0097]

[0098] In this experiment, the process noise matrix Q is set to:

[0099] Q = diag(0.1 0.1 0.1 1 1 1 1)

[0100] It should also be noted that, compared with existing technologies, this embodiment does not rely on prior information about dynamic parameters such as the mass of the projectile; parameter identification and state estimation can be achieved solely through real-time position observation data. By employing a logarithmic mapping strategy, the numerical instability caused by the large range of parameter values ​​is resolved, improving the accuracy of parameter estimation. This makes it suitable for trajectory prediction of various types of unknown projectiles, enhancing its versatility and practical value.

[0101] The logarithmic mapping strategy is to use δ k The parameter range is compressed to a preset range, such as [-6, -1], to improve numerical stability.

[0102] Furthermore, the method for determining the global state variables of the projectile at the current moment based on the global position vector at the current moment through the uncontrolled projectile dynamics model is to determine the three-dimensional coordinates and velocity components based on the global position vector at the current moment; and to perform one-dimensional equivalent drag integration processing on the object mass, air resistance magnitude, air density, air resistance coefficient, windward area and gravitational acceleration based on the uncontrolled projectile dynamics model according to the logarithmic mapping strategy to obtain the equivalent drag coefficient in logarithmic space.

[0103] Step S40: Construct the current state estimation vector of the projectile based on the three-dimensional coordinates, the velocity components, and the equivalent drag coefficient in logarithmic space.

[0104] In the specific implementation, the initial velocity is estimated based on the coordinate observation results of the first and second frames, the comprehensive drag coefficient is initialized to an intermediate value within a reasonable range, and the initial state estimation vector (i.e., the initial moment state estimation vector) is:

[0105]

[0106] In the initial covariance matrix setting, the position variance is set based on the sensor accuracy, the velocity variance is set based on the initial velocity estimation error, and a large initial value is set for the comprehensive drag coefficient to reflect uncertainty.

[0107] Step S50: Calculate the next-time state estimation vector of the projectile using the UKF recursive algorithm based on the current state estimation vector and the next-time observation position vector of the projectile.

[0108] Further, multiple sigma points are generated based on the current state estimation vector; these multiple sigma points are propagated through a state transition function to obtain multiple predicted sigma points, and the predicted state mean and predicted covariance are calculated based on each predicted sigma point; each predicted sigma point is mapped to the observation space to determine multiple observed sigma points, and the predicted measurement mean and predicted measurement covariance are calculated based on the observed sigma points; the cross covariance is calculated based on each predicted sigma point, each observed sigma point, the predicted state mean, and the predicted measurement mean; the Kalman gain is calculated based on the predicted measurement covariance and the cross covariance; and the next-time state estimation vector of the projectile is calculated using the UKF recursive algorithm based on the observed position vector of the projectile at the next time step, the predicted state mean, the predicted measurement mean, and the Kalman gain.

[0109] In this embodiment, UKF parameters α, κ, and β are set so that the sigma weight calculation result satisfies the condition that the first term is greater than all other terms. In this experiment, UKF parameters α = 0.8, κ = 3, and β = 2 are set. The weight calculation formula is:

[0110]

[0111] Among them, W i m It is the mean weight, W i c λ is the covariance weight, n = 7 is the dimension of the state vector, and λ = α(n + κ) - n.

[0112] In the specific implementation, 2n+1 sigma points are generated based on the current state estimation vector:

[0113]

[0114] in, Represents the scaled covariance matrix Perform square root decomposition on the matrix to obtain the i-th column vector of its lower triangular matrix.

[0115] Propagate the sigma point through the state transition function:

[0116]

[0117] in, It predicts the sigma point.

[0118] Predicted state mean:

[0119]

[0120] Predicting the covariance matrix:

[0121]

[0122] Where Q is the process noise covariance matrix.

[0123] The sigma points are propagated through the observation model, and for each propagated sigma point, the corresponding observed sigma point is calculated:

[0124]

[0125] Predicted measurement mean:

[0126]

[0127] Predictive measurement covariance:

[0128]

[0129] Where R is the measurement noise covariance matrix.

[0130] Calculate the cross covariance matrix:

[0131]

[0132] Calculate the Kalman gain:

[0133] K = P xz S -1

[0134] Updated state estimate:

[0135]

[0136] Update the covariance matrix:

[0137]

[0138] After iterating through the UKF algorithm, the state estimation vector for the next time step can be obtained. Covariance Matrix

[0139] It should also be noted that, after 0.25 seconds of initial estimation, the position state estimation accuracy for different types of projectiles reached [a certain level]. Velocity state estimation accuracy reaches The accuracy of the comprehensive drag coefficient estimation reaches

[0140] Step S60: Based on the next moment state estimation vector, the trajectory of the projectile is predicted using the discrete state equation of projectile dynamics.

[0141] It should also be noted that the current time step is known. k+1 State estimation results To predict the trajectory of the projectile at a future time step n, the following recursive calculations are required:

[0142]

[0143] Here, i is the index of the prediction step size, which starts from 0 and increments to the prediction time step. Each recursive calculation uses the prediction result of the previous step as input, propagates the state expectation through the discrete state equation, and can ultimately predict the position of the projectile at any future time step.

[0144] Experimental results show that, under complex noise conditions, the algorithm of this invention achieves convergence of the comprehensive drag coefficient parameters in an average time of 0.25 seconds (error <5%). Furthermore, UKF effectively suppresses noise interference and enables state estimation and trajectory prediction for different types of projectiles, verifying the applicability of this method.

[0145] In this embodiment, the current observed position vector of the projectile in the machine coordinate system is first obtained. Then, the current observed position vector in the machine coordinate system is converted into the current global position vector in the global coordinate system using an observation model. Next, based on the current global position vector, the current global state variables of the projectile are determined using an uncontrolled projectile dynamics model. The current global state variables include three-dimensional coordinates, velocity components, and the equivalent drag coefficient in logarithmic space. The current state estimation vector of the projectile is constructed based on the three-dimensional coordinates, velocity components, and the equivalent drag coefficient in logarithmic space. Finally, the next state estimation vector of the projectile is calculated using the UKF recursive algorithm based on the current state estimation vector and the next observed position vector. The trajectory of the projectile is predicted using the discrete state equations of projectile dynamics based on the next state estimation vector. This embodiment does not rely on prior information about dynamic parameters such as the projectile's mass; parameter identification and state estimation can be achieved solely through real-time position observation data. Furthermore, the logarithmic mapping strategy solves the numerical instability problem caused by the large range of parameter values, improving parameter estimation accuracy. It is applicable to trajectory prediction of various types of unknown projectiles, enhancing its versatility and practical value.

[0146] Reference Figure 5 , Figure 5 This is a structural block diagram of the first embodiment of the UKF-based projectile trajectory prediction system of the present invention.

[0147] like Figure 5 As shown, the UKF-based projectile trajectory prediction system proposed in this embodiment of the invention includes:

[0148] Module 5001 is used to obtain the current observation position vector of the projectile in the machine coordinate system;

[0149] The conversion module 5002 is used to convert the current observation position vector in the machine coordinate system into the current global position vector in the global coordinate system through the observation model;

[0150] Calculation module 5003 is used to determine the current global state quantity of the projectile based on the current global position vector through the uncontrolled projectile dynamics model. The current global state quantity includes three-dimensional coordinates, velocity components, and equivalent drag coefficient in logarithmic space.

[0151] The calculation module 5003 is also used to construct the current state estimation vector of the projectile based on the three-dimensional coordinates, the velocity components, and the equivalent drag coefficient in the logarithmic space.

[0152] The calculation module 5003 is further configured to calculate the next-time state estimation vector of the projectile using the UKF recursive algorithm based on the current-time state estimation vector and the next-time observation position vector of the projectile.

[0153] The prediction module 5004 is used to predict the trajectory of the projectile based on the next moment state estimation vector through the discrete state equation of projectile dynamics.

[0154] Other embodiments or specific implementations of the UKF-based projectile trajectory prediction system of the present invention can be found in the above-described method embodiments, and will not be repeated here.

[0155] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0156] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0157] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as read-only memory / random access memory, magnetic disk, optical disk) and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, or network device, etc.) to execute the methods described in the various embodiments of the present invention.

[0158] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A UKF-based trajectory prediction method for a projectile, characterized in that, The UKF-based trajectory prediction method of the projectile comprises the following steps: an observation position vector of the projectile at a current time in a machine coordinate system is acquired; the observation position vector of the projectile at the current time in the machine coordinate system is converted into a global position vector at the current time in a global coordinate system through an observation model; a global state quantity of the projectile at the current time is determined based on the global position vector at the current time through an uncontrolled projectile dynamics model, the global state quantity at the current time comprising three-dimensional coordinates, velocity components and an equivalent drag coefficient in a logarithmic space; a state estimation vector of the projectile at the current time is constructed according to the three-dimensional coordinates, the velocity components and the equivalent drag coefficient in the logarithmic space; a state estimation vector of the projectile at a next time is calculated according to the state estimation vector at the current time and a next-time observation position vector of the projectile through a UKF recursive algorithm; trajectory prediction of the projectile is realized through a discrete state equation of projectile dynamics according to the state estimation vector at the next time; the step of determining the global state quantity of the projectile at the current time based on the global position vector at the current time through the uncontrolled projectile dynamics model comprises: three-dimensional coordinates and velocity components are determined according to the global position vector at the current time; an equivalent drag coefficient in a logarithmic space is obtained by performing one-dimensional equivalent drag integration processing on object mass, air resistance, air density, air resistance coefficient, windward area and gravitational acceleration according to a logarithmic mapping strategy based on the uncontrolled projectile dynamics model.

2. The method of claim 1, wherein, Before the step of converting the observation position vector of the projectile at the current time in the machine coordinate system into the global position vector at the current time in the global coordinate system through the observation model, the method further comprises the following steps: a planar coordinate and a heading angle of the mobile robot in the global coordinate system are determined; an observation matrix is constructed through a coordinate mapping relationship according to the heading angle and a state space vector, and an offset vector is constructed according to the planar coordinate and the heading angle; an observation model is established according to the observation matrix, the offset vector and measurement noise.

3. The method of claim 2, wherein, Before the step of determining the global state quantity of the projectile at the current time based on the global position vector at the current time through the uncontrolled projectile dynamics model, the method further comprises the following step: an uncontrolled projectile dynamics model is constructed through a dynamics relationship according to the global position vector at the current time, a velocity vector, object mass, air resistance, air density, air resistance coefficient, windward area and gravitational acceleration.

4. The method of claim 1, wherein, The step of calculating the state estimation vector of the projectile at the next time according to the state estimation vector at the current time and the next-time observation position vector of the projectile through the UKF recursive algorithm comprises the following steps: a plurality of sigma points are generated according to the state estimation vector at the current time; a plurality of predicted sigma points are obtained by propagating the plurality of sigma points through a state transition function, and a predicted state mean value and a predicted covariance are calculated based on each predicted sigma point; each predicted sigma point is mapped to an observation space to determine a plurality of observation sigma points, and a predicted measurement mean value and a predicted measurement covariance are calculated based on the observation sigma points; The next time state estimation vector of the projectile is calculated by UKF recursive algorithm according to the next time observed position vector of the projectile, the predicted state mean, the predicted covariance, the predicted measurement mean and the predicted measurement covariance.

5. The method of claim 4, wherein, The step of calculating the next time state estimation vector of the projectile according to the next time observed position vector of the projectile, the predicted state mean, the predicted measurement mean and the predicted measurement covariance by UKF recursive algorithm comprises: The cross covariance is calculated according to each predicted sigma point, each observed sigma point, the predicted state mean and the predicted measurement mean; The Kalman gain is calculated according to the predicted measurement covariance and the cross covariance; The next time state estimation vector of the projectile is calculated by UKF recursive algorithm according to the next time observed position vector of the projectile, the predicted state mean, the predicted measurement mean and the Kalman gain.

6. A UKF-based projectile trajectory prediction system, characterized by, The UKF-based projectile trajectory prediction system comprises: An acquisition module is configured to acquire a current time observed position vector of a projectile in a machine coordinate system; A conversion module is configured to convert the current time observed position vector in the machine coordinate system into a current time global position vector in a global coordinate system through an observation model; A calculation module is configured to determine a current time global state quantity of the projectile based on the current time global position vector through an uncontrolled projectile dynamics model, wherein the current time global state quantity comprises a three-dimensional coordinate, a velocity component and an equivalent drag coefficient in a logarithmic space; The calculation module is further configured to construct a current time state estimation vector of the projectile according to the three-dimensional coordinate, the velocity component and the equivalent drag coefficient in the logarithmic space; The calculation module is further configured to calculate a next time state estimation vector of the projectile through UKF recursive algorithm according to the current time state estimation vector and a next time observed position vector of the projectile; A prediction module is configured to realize trajectory prediction of the projectile through a projectile dynamics discrete state equation according to the next time state estimation vector. The UKF-based projectile trajectory prediction system is configured to realize the steps of the UKF-based projectile trajectory prediction method according to any one of claims 1 to 5.

7. A UKF-based trajectory prediction device for a projectile, characterized by, The device comprises a memory, a processor and a UKF-based projectile trajectory prediction program stored on the memory and executable on the processor, wherein the UKF-based projectile trajectory prediction program is configured to realize the steps of the UKF-based projectile trajectory prediction method according to any one of claims 1 to 5.

8. A storage medium, characterized by The storage medium stores a UKF-based projectile trajectory prediction program, wherein the UKF-based projectile trajectory prediction program is executable on a processor to realize the steps of the UKF-based projectile trajectory prediction method according to any one of claims 1 to 5.

Images