A target tracking method for unmanned vehicle based on extended Kalman filter prediction

Abstract

The present invention discloses an unmanned vehicle target tracking method based on extended Kalman filter prediction. The method comprises the following steps: Step 1: The unmanned vehicle performs a tracking task for the target and obtains navigational state information of the target based on a sensing system or a collaborative intelligent system installed on the unmanned vehicle; Step 2: Establishing a state transition equation and an observation equation for the navigation of the target; Step 3: Predicting the state of the target using an extended Kalman filter algorithm; Step 4: Calculating the desired heading of the unmanned vehicle based on the predicted tracking target state in combination with a pure tracking method; Step 5: Calculating the desired speed of the unmanned vehicle based on the distance between the unmanned vehicle and the target using a PID controller; Step 6: The unmanned vehicle tracks the target based on the desired heading and desired speed. The present invention can achieve the purpose of stable distance tracking of a target vessel.

Description

Technical Field

[0001] The present invention relates to the field of unmanned boat target tracking, and in particular to an unmanned boat target tracking method based on extended Kalman filter prediction. Background Art

[0002] With the development of computer and artificial intelligence technologies, a wide variety of unmanned systems have been developed. Among them, unmanned surface vehicles (USVs) are intelligent surface platforms capable of both remote control and autonomous navigation. Equipped with a variety of functional sensors, they can perform a variety of functions and are widely used in both civilian and military fields. Unmanned surface vehicles (USVs) are often used to replace personnel in maritime missions, and moving target tracking technology has a very important application value. Once an USV can stably track a moving target, it can further perform other tasks such as reconnaissance and surveillance. Due to the randomness and uncertainty of the moving target's navigation state, as well as the objectively complex sea surface conditions, manually controlling the vessel to perform target tracking tasks cannot achieve optimal tracking results throughout the entire process. Therefore, it is of great significance to design a reasonable target tracking strategy to control the USV to autonomously perform target tracking tasks.

[0003] When tracking a physical target, an unmanned vehicle (UAV) must quickly approach the target when it's at a distance. Then, based on changes in the target's position, speed, or heading, the UAV makes real-time decisions and responds, maintaining a stable and continuous presence within a designated area near the target to ensure it remains within range. However, complex and diverse sea conditions and system errors within the UAV's sensors can interfere with the UAV's control and impact target tracking. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide an unmanned boat target tracking method based on extended Kalman filter prediction in response to the defects in the existing technology.

[0005] The technical solution adopted by the present invention to solve its technical problem is:

[0006] The present invention provides an unmanned vehicle target tracking method based on extended Kalman filter prediction, the method comprising the following steps:

[0007] Step 1: The unmanned boat performs the tracking task of the tracking target and obtains the navigation status information of the tracking target based on the perception system or collaborative intelligent system installed on the unmanned boat.

[0008] Step 2: Establish the state transfer equation and observation equation when tracking the target;

[0009] Step 3: Use the extended Kalman filter algorithm to predict the tracking target state;

[0010] Step 4: Calculate the desired heading of the unmanned boat based on the predicted tracking target state and the pure tracking method;

[0011] Step 5: According to the distance between the UAV and the tracking target, the PID controller is used to obtain the expected speed of the UAV;

[0012] Step 6: The unmanned boat tracks the target according to the expected heading and expected speed.

[0013] Furthermore, the collaborative intelligent system in step 1 of the present invention includes: a drone, a roadbed radar and a collaborative ship.

[0014] Furthermore, the method for establishing the state transition equation when tracking the target navigation in step 2 of the present invention is:

[0015] The horizontal and vertical coordinates (x k ,y k ), heading angle θ k , navigation speed v k , acceleration and angular velocity is regarded as the motion state of the tracking target, recorded as The current state of the target X k From the state quantity X at the previous moment k-1 According to the following equation:

[0016]

[0017] Where T is the prediction interval time, considering the Gaussian white noise ω in the process of tracking the target state transition k-1 , define the state transfer equation of the tracking target as:

[0018] X k =f(X k-1 )+ω k-1

[0019] f(X) in X k-1 The Jacobian matrix at is the state transfer matrix, so we can get F(k-1):

[0020]

[0021] Furthermore, the method for establishing the tracking target observation equation in step 2 of the present invention is:

[0022] The measurement equation for tracking the target is as follows:

[0023] Z k =H k X k +v' k

[0024] where Z k To track the target measurement value, v' k is the measurement noise of the perception system itself, H k is the measurement matrix, the matrix form is as follows:

[0025]

[0026] Furthermore, the method in step 3 of the present invention is specifically as follows:

[0027] According to the state transfer equation and observation equation of the tracking target, the current state of the tracking target is used for prior prediction. The equation is as follows:

[0028] X(k|k-1)=F(k-1)X(k-1|k-1)

[0029] The one-step prediction equation for the covariance P(k|k-1) corresponding to the one-step prediction value X(k|k-1) is:

[0030] P(k|k-1)=F(k-1)P(k-1|k-1)F T (k-1)+Q(k-1)

[0031] Where Q(k-1) is the covariance matrix of the process noise, and the final extended Kalman filter posterior prediction value X(k|k) of the tracked target is:

[0032] X(k|k)=X(k|k-1)+K(k)(Z(k)-H(k)X(k|k-1))

[0033] Where K(k) is the extended Kalman filter gain value, R(k) is the covariance matrix of the measurement noise, and the calculation equation of K(k) is:

[0034]

[0035] Finally, update the covariance matrix P(k|k) corresponding to X(k|k):

[0036] P(k|k)=(IK(k)H(k))P(k|k-1)

[0037] Complete the entire extended Kalman filter prediction process.

[0038] Furthermore, the method of step 4 of the present invention is specifically as follows:

[0039] During the final stable tracking process of the target tracking mission, the unmanned boat will always sail within a fan-shaped area behind the target. The best tracking effect can be achieved when it is located at the center of the fan-shaped area. This position is called the optimal tracking position, and its coordinates are calculated as follows:

[0040]

[0041] Where (x p ,y p ) is the optimal tracking position coordinate, (x t ,y t ) is the tracking target coordinate, d p is the set stable tracking distance, θ t To track the target heading angle;

[0042] When the unmanned boat performs the target tracking task, it is optimal when it is at the optimal tracking position. Therefore, the unmanned boat should sail towards this position at every moment. In the process of the unmanned boat approaching the tracking target, because the tracking target state is predicted in multiple steps, each step prediction can get the corresponding optimal tracking position. Combined with the current motion state of the unmanned boat, according to the following formula (, it can be judged that the unmanned boat will be able to sail to the vicinity of the optimal tracking position at a certain moment in the future:

[0043]

[0044] where v s is the current speed of the unmanned boat, (x s ,y s ) is the current coordinate of the unmanned boat, (x pk ,y pk ) is the optimal tracking position coordinate of the tracking target when predicting the kth step, T k is the total time for predicting k steps; once this condition is met, the Kalman filter prediction iteration process ends, and (x pk ,y pk ) as the optimal tracking position coordinate of the unmanned boat at the current moment (x p ,y p );

[0045] According to the current unmanned boat coordinates (x s ,y s ), the expected speed of the unmanned boat is calculated using the pure pursuit method for:

[0046]

[0047] Furthermore, the method in step 5 of the present invention is specifically as follows:

[0048] The expected speed of the unmanned boat is obtained by taking the speed of the tracking target as the benchmark and combining the output of the PID controller:

[0049]

[0050] Among them, u outis the PID controller output, K p , K i , K d They are the coefficients of the proportional link, integral link, and differential link, Δd k , Δd k-1 are the difference between the distance between the unmanned boat and the tracking target and the set tracking distance at the current moment and the previous moment, S i is the cumulative sum of historical distance deviations, v t is the speed of the target ship, v s_expect The expected speed of the unmanned boat.

[0051] The beneficial effects produced by the present invention are:

[0052] Aiming at the target tracking task, the present invention proposes an unmanned boat target tracking strategy based on extended Kalman filter prediction. The extended Kalman filter is used to dynamically predict the future navigation state of the target ship. Combined with the current navigation state of the unmanned boat, the pure tracking method is used to obtain the expected heading of the unmanned boat. According to the distance between the unmanned boat and the target ship, the PID controller is used to obtain the expected speed of the unmanned boat. The control effect of first quickly approaching the tracking target and then stably tracking the target at a distance is achieved.

[0053] 1. The present invention uses a state transfer equation that conforms to the navigation law of the tracking target and adopts an extended Kalman filter algorithm to perform multi-step prediction of the tracking target state.

[0054] 2. During the target tracking process of the unmanned boat of the present invention, when approaching the tracking target, the expected heading of the unmanned boat is calculated based on the state prediction information of the tracking target and the idea of pure tracking algorithm is used to draw on the idea of pure tracking algorithm, so that the unmanned boat can predictably and quickly approach the tracking target, thereby saving time.

[0055] 3. The target tracking algorithm of the present invention can predict the change of the tracking target state, avoid the situation where the tracking target is lost due to a large change in the tracking target state, and make the tracking process more stable. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0057] Figure 1 Schematic diagram of calculating the desired heading of an unmanned boat according to an embodiment of the present invention.

[0058] Figure 2 This is a flow chart of calculating the expected speed of an unmanned boat according to an embodiment of the present invention.

[0059] Figure 3 This is a simulation diagram of an unmanned boat tracking a target moving in a straight line at a uniform speed according to an embodiment of the present invention.

[0060] Figure 4This is a simulation diagram of an unmanned boat tracking a target traveling on a uniform curve according to an embodiment of the present invention.

[0061] Figure 5 This is a simulation diagram of an unmanned boat tracking a target in a complex driving state according to an embodiment of the present invention. DETAILED DESCRIPTION

[0062] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0063] Example 1

[0064] In an embodiment of the present invention, when the unmanned boat itself begins to perform the tracking task of the target ship, the navigation status information of the target ship is obtained based on the perception system installed on the unmanned boat itself or other collaborative intelligent systems such as drones. Based on the collected target navigation status information and the navigation status of the unmanned boat itself, the extended Kalman prediction is used to follow the target motion state, combined with the pure tracking method to plan the desired course of the unmanned boat; based on the distance between the unmanned boat and the tracking target, the PID controller is used to plan the desired speed of the unmanned boat to complete the tracking task of the target ship. Its implementation method and characteristics are:

[0065] (1) Establish the state transfer equation and observation equation when tracking the target

[0066] 1) Tracking target state transfer equation

[0067] The horizontal and vertical coordinates, heading angle, navigation speed, acceleration and angular velocity of the tracking target are regarded as the motion state of the tracking target, which can be recorded as The current state of the target X k The state quantity X at the previous moment can be k-1 According to the following equation:

[0068]

[0069] Where T is the prediction interval time, considering the Gaussian white noise ω in the process of tracking the target state transition k-1 , according to formula (1), the state transfer equation of the tracking target is defined as:

[0070] X k =f(X k-1 )+ω k-1 (2)

[0071] f(X) in X k-1 The Jacobian matrix at is the state transfer matrix, so we can get F(k-1):

[0072]

[0073] 2) Tracking target observation equation

[0074] When the perception system measures the motion state of the tracking target, it can only directly measure the target's position information, heading angle and speed, while the acceleration and angular velocity are calculated based on the historical state information of the tracking target, plus the measurement noise v' of the perception system itself. k , so the measurement equation of the tracking target can be obtained as follows:

[0075] Z k =H k X k +v' k (4)

[0076] where Z k To track the target measurement value, H k is the measurement matrix, the matrix form is as follows:

[0077]

[0078] (2) Using the extended Kalman filter algorithm to predict the tracking target state

[0079] According to the state transfer equation and observation equation of the tracking target, the current state of the tracking target is used for prior prediction. The equation is as follows:

[0080] X(k|k-1)=F(k-1)X(k-1|k-1) (6)

[0081] The one-step prediction equation for the covariance P(k|k-1) corresponding to the one-step prediction value X(k|k-1) is:

[0082] P(k|k-1)=F(k-1)P(k-1|k-1)F T (k-1)+Q(k-1) (7)

[0083] Where Q(k-1) is the covariance matrix of the process noise, and the extended Kalman filter posterior prediction value X(k|k) of the tracked target can be obtained as follows:

[0084] X(k|k)=X(k|k-1)+K(k)(Z(k)-H(k)X(k|k-1)) (8)

[0085] Where K(k) is the extended Kalman filter gain value, R(k) is the covariance matrix of the measurement noise, and the calculation equation of K(k) is:

[0086]

[0087] Finally, the covariance matrix P(k|k) corresponding to X(k|k) is updated to complete the entire extended Kalman filter prediction process.

[0088] P(k|k)=(IK(k)H(k))P(k|k-1) (10)

[0089] This is a complete iterative process of predicting the state of the tracking target at the next moment from the current moment. If this prediction process is repeated, the multi-step prediction state of the tracking target in the future can be obtained.

[0090] The target tracking task in the present invention requires stable distance tracking. At the same time, in order to avoid the situation where the target is lost due to the excessive rotation angle of the unmanned boat itself during the tracking process, there is a constraint range for the relative angle between the unmanned boat and the tracking target. Therefore, the unmanned boat's final tracking effect is always navigating within a fan-shaped area behind the tracking target. When the unmanned boat begins to perform the target tracking task, it may be far away from the initial distance of the tracking target and needs to quickly approach the target. At this stage, the number of steps of predicting the motion state of the tracking target can be increased so that it can predictably approach the tracking target. When the unmanned boat navigates to the ideal fan-shaped tracking area, it only needs to predict the motion state of the tracking target for the next few steps, predict the movement of the tracking target, and then control the unmanned boat to change in advance.

[0091] (3) Calculate the expected heading of the unmanned boat based on the predicted target state combined with the pure tracking method

[0092] In the target tracking task of the present invention, during the final stable tracking process, the unmanned boat will always sail in a fan-shaped area behind the tracking target. The best tracking effect can be achieved at the center of the fan-shaped area. This position is called the optimal tracking position, and its coordinates are calculated as follows:

[0093]

[0094] Where (x p ,y p ) is the optimal tracking position coordinate, (x t ,y t ) is the tracking target coordinate, d p is the set stable tracking distance, θ t To track the target heading angle.

[0095] When the unmanned boat performs the target tracking task, it is optimal when it is traveling at the optimal tracking position. Therefore, the unmanned boat should sail towards this position at every moment. In the process of the unmanned boat approaching the tracking target, because the tracking target state is predicted in multiple steps, each step prediction can obtain the corresponding optimal tracking position. Combined with the current motion state of the unmanned boat, according to formula (12), it can be judged that the unmanned boat will be able to sail to the vicinity of the optimal tracking position at a certain moment in the future.

[0096]

[0097] where v s is the current speed of the unmanned boat, (x s ,y s ) is the current coordinate of the unmanned boat, (x pk ,y pk ) is the optimal tracking position coordinate of the tracking target when predicting the kth step, T k is the total time for predicting k steps. Once this condition is met, the Kalman filter prediction iteration process ends and (x pk ,y pk ) as the optimal tracking position coordinate of the unmanned boat at the current moment (x p ,y p ).

[0098] According to the current unmanned boat coordinates (x s ,y s ), the expected speed of the unmanned boat can be calculated by referring to the idea of pure pursuit method for:

[0099]

[0100] (4) According to the distance between the unmanned boat and the tracking target, the PID controller is used to obtain the expected speed of the unmanned boat.

[0101] When the UAV is not close to the tracking target, the primary goal is to approach the tracking target. Therefore, at this time, the UAV can be controlled to sail at a constant speed higher than the speed of the tracking target. When entering the ideal fan-shaped tracking area behind the tracking target, stable distance tracking is required. The speed change of the tracking target will cause the distance between the UAV and the tracking target to change. Therefore, according to the distance between the UAV and the tracking target, the speed of the tracking target is used as a reference and combined with the output of the PID controller to obtain the expected speed of the UAV:

[0102]

[0103] Among them, u out is the PID controller output, K p , K i , K d They are the coefficients of the proportional link, integral link, and differential link, Δd k , Δd k-1 are the difference between the distance between the unmanned boat and the tracking target and the set tracking distance at the current moment and the previous moment, S i is the cumulative sum of historical distance deviations, v t is the speed of the target ship, v s_expect The expected speed of the unmanned boat.

[0104] Example 2

[0105] The present invention is further described below with reference to specific examples and accompanying drawings.

[0106] The simulation calculation information is as follows: the initial coordinates of the own ship are (0,500), the initial coordinates of the target ship are (2000,0), the initial speed of the target ship is 6 meters / s, the simulation control cycle is 1 second, and the expected tracking effect of the unmanned boat performing the target tracking task is: the distance from the target ship is 400±50 meters; the relative angle constraint range with the tracking target is within 30° of the target ship's heading backward.

[0107] The unmanned boat target tracking method proposed in this invention is used for simulation, and three simulation scenarios are designed:

[0108] Scenario 1: Tracking a target moving in a straight line at a constant speed.

[0109] Scenario 2: Tracking a target traveling along a curve at a constant speed.

[0110] Scenario 3: The target first moves in a straight line, then starts to move in a curve at a certain moment; the speed remains constant at first, then at a certain moment it increases to 0.05m / s 2 The acceleration reaches 10m / s and then maintains a constant speed again.

[0111] The simulation diagrams of scene 1, scene 2 and scene 3 are as follows: Figure 3 、 Figure 4 、 Figure 5 shown.

[0112] It should be understood that those skilled in the art can make improvements or changes based on the above description, and all such improvements and changes should fall within the scope of protection of the appended claims of the present invention.

Claims

1. A method for tracking unmanned vehicle targets based on extended Kalman filter prediction, characterized in that: The method comprises the following steps: Step 1: The unmanned boat performs the tracking task of the tracking target and obtains the navigation status information of the tracking target based on the perception system or collaborative intelligent system installed on the unmanned boat. Step 2: Establish the state transfer equation and observation equation when tracking the target; Step 3: Use the extended Kalman filter algorithm to predict the tracking target state; the method in step 3 is specifically as follows: According to the state transfer equation and observation equation of the tracking target, the current state of the tracking target is used for prior prediction. The equation is as follows: The one-step prediction value The corresponding covariance The one-step prediction equation is: in is the covariance matrix of the process noise, and finally the extended Kalman filter posterior prediction value of the tracking target is obtained for: in is the extended Kalman filter gain value, record is the covariance matrix of the measurement noise, The calculation equation is: Last Updated The corresponding covariance matrix : Complete the entire extended Kalman filter prediction process; Step 4: Calculate the desired heading of the unmanned boat based on the predicted tracking target state and the pure tracking method; Step 5: According to the distance between the UAV and the tracking target, the PID controller is used to obtain the expected speed of the UAV; Step 6: The unmanned boat tracks the target according to the expected heading and expected speed.

2. The unmanned vehicle target tracking method based on extended Kalman filter prediction according to claim 1 is characterized in that: The collaborative intelligent system in step 1 includes: a drone, a road-based radar, and a collaborative ship.

3. The unmanned vehicle target tracking method based on extended Kalman filter prediction according to claim 1, characterized in that: The method for establishing the state transfer equation when tracking the target navigation in step 2 is: The horizontal and vertical coordinates of the tracking target ( ), heading angle , sailing speed , acceleration and angular velocity is regarded as the motion state of the tracking target, recorded as , the current state of the target The state quantity at the previous moment According to the following equation: in To predict the interval time, consider tracking the Gaussian white noise in the target state transition process , define the state transfer equation of the tracking target as: exist The Jacobian matrix at is the state transfer matrix, so we can get : 。 4. The unmanned vehicle target tracking method based on extended Kalman filter prediction according to claim 3 is characterized in that: The method for establishing the tracking target observation equation in step 2 is: The measurement equation for tracking the target is as follows: in To track target measurements, is the measurement noise of the perception system itself, is the measurement matrix, the matrix form is as follows: 。 5. The unmanned vehicle target tracking method based on extended Kalman filter prediction according to claim 1, characterized in that: The method of step 4 is specifically as follows: During the final stable tracking process of the target tracking mission, the unmanned boat will always sail within a fan-shaped area behind the target. The best tracking effect can be achieved when it is located at the center of the fan-shaped area. This position is called the optimal tracking position, and its coordinates are calculated as follows: in is the optimal tracking position coordinate, To track the target coordinates, is the set stable tracking distance, To track the target heading angle; When the unmanned boat performs a target tracking task, it is optimal when it is traveling at the optimal tracking position. Therefore, the unmanned boat should sail towards this position at every moment. In the process of the unmanned boat approaching the tracking target, because the tracking target state is predicted in multiple steps, each step of the prediction can obtain the corresponding optimal tracking position. Combined with the current motion state of the unmanned boat, the following formula can be used to determine whether the unmanned boat will be able to sail to the vicinity of the optimal tracking position at a certain moment in the future: in is the current speed of the unmanned boat, is the current coordinate of the unmanned boat, To track the target k The optimal tracking position coordinates during step prediction, For prediction k The total time of the step; once this condition is met, the Kalman filter prediction iteration process ends. As the best tracking position coordinates of the unmanned boat at the current moment ; According to the current coordinates of the unmanned boat , the expected speed of the unmanned boat is calculated using the pure pursuit method for: 。 6. The unmanned vehicle target tracking method based on extended Kalman filter prediction according to claim 5, characterized in that: The method in step 5 is specifically as follows: The expected speed of the unmanned boat is obtained by taking the speed of the tracking target as the benchmark and combining the output of the PID controller: in, is the PID controller output, are the coefficients of the proportional link, integral link, and differential link respectively, are the difference between the distance between the unmanned boat and the tracking target and the set tracking distance at the current moment and the previous moment, is the cumulative sum of historical distance deviations, is the speed of the target vessel, The expected speed of the unmanned boat.

Images