A method for tracking moving targets by UAV using visual information

Abstract

The present invention belongs to the research field of target tracking and trajectory planning of unmanned aerial vehicles. In order to propose a target motion prediction method based on an adaptive interactive dual model, the present invention uses visual information to realize a method for unmanned aerial vehicles to track moving targets. The steps are as follows: using the airborne depth camera on the unmanned aerial vehicle, on the one hand, the image is transmitted to the deep learning target detection model YOLOv5s for target recognition, and the depth distance information of the target is found according to the recognition result, and then the three-dimensional coordinates of the target in the world coordinate system are obtained through coordinate conversion; then the multi-step prediction method of the target based on the dual model interaction is used to realize the multi-step prediction of the target position, and the position of the target in the future is predicted; on the other hand, the obstacle information is perceived in real time by the depth camera, and the local Euclidean signed distance field ESDF map is constructed according to the depth image information of the depth camera. The present invention is mainly used in the occasion of unmanned aerial vehicle target tracking and trajectory planning.

Description

Technical Field

[0001] The present invention belongs to the research field of target tracking and trajectory planning of unmanned aerial vehicles. Aiming at the tracking and planning task of unmanned aerial vehicles for unknown targets in complex environments, a target tracking method of unmanned aerial vehicles based on visual sensors in unknown complex environments is proposed. Specifically, it relates to a method for unmanned aerial vehicles to track moving targets using visual information. Background Art

[0002] Due to their maneuverability and low cost, drones are gradually being used in various civil and military fields. Quad-rotor drones are now able to complete a variety of perception, control and planning tasks in different application scenarios. In an unknown and complex environment, the task of tracking and planning mobile targets is still challenging. Drones must use limited sensors to identify targets and minimize the impact of obstacles on target occlusion. For safety reasons, drones also need to plan tracking trajectories while safely avoiding obstacles, which requires that trajectory planning has high speed. On the other hand, the trajectory generated by drone planning should be smooth to meet the dynamic constraints of the drone. Based on the above problems, the present invention considers that the task of tracking mobile targets in an unknown and complex environment can be divided into target motion prediction and target tracking planning.

[0003] Target motion prediction is mainly divided into prediction methods based on probability statistics and prediction methods based on deep learning. The prediction method based on probability statistics constructs the corresponding mathematical model through the historical trajectory data of the moving target. Wei Xiqing and others from the Shanghai Institute of Mechanical and Electrical Engineering used the extended Kalman filter method to complete the estimation of the target motion state in the guidance field (Journal: Aerospace Control; Author: Wei Xiqing, Gu Longfei, Li Ruikang, Wang Sheyang; Publication Year: 2017; Article Title: Hypersonic Vehicle Trajectory Tracking and Prediction Based on Singer Model; Pages: 62-66). However, the prediction accuracy of a single Kalman filter is poor. In the prediction method based on deep learning, typical network models include recurrent neural networks and long short-term memory networks. Ji Xuewu and others from Tsinghua University used long short-term memory networks to predict the future longitudinal and lateral trajectories of vehicles on highways (Journal: China Journal of Highway and Transport; Author: Ji Xuewu, Fei Cong, He Xiangkun, Liu Yulong and Liu Yahui; Publication Year: 2019; Article Title: Intention Recognition and Trajectory Prediction for Vehicles Using LSTM Network; Pages: 34-42). However, prediction methods based on deep learning require a large amount of training data for preliminary model training, and the training data of different target objects are different. In addition, such methods often require a large amount of computing resources and are difficult to apply to drones with limited computing resources.

[0004] In the study of target tracking by UAVs, some previous works have developed target tracking algorithms based on visual controllers. Such algorithms use the tracking distance error defined in the image space as feedback and as the input of the UAV control to track the target trajectory. Most of these methods do not consider safety constraints and are not suitable for complex environments. In recent years, some works have proposed more complete target tracking algorithm strategies that take into account obstacle avoidance, dynamic feasibility, and tracking efficiency. T et al. proposed a real-time trajectory generation method that combines model predictive control with the state constraints, dynamic constraints, and collision constraints of the UAV, turning trajectory generation into a constrained optimization problem that can be solved in real time (Journal: IEEE Robotics and Automation Letters; Author: T, Alonso-Mora J, Domahidi A, Rus D and Hilliges O; Publication Year: 2017; Article Title: Real-time motion planning for aerial videography with dynamic obstacle avoidance and viewpoint optimization; Pages: 1696-1703). However, the optimization formulas in this method are all non-convex, which may lead to local minimum problems during implementation. Han Zhichao et al. from Zhejiang University proposed a safe target trajectory tracking planner, which consists of a dynamic search front end with a known target and a spatiotemporal optimal trajectory planning back end. The front-end search designs a heuristic function that takes into account both the current position and the future position of the target. Based on the results of the path search, the back-end algorithm constructs a flight corridor, and then the back-end optimizer generates the optimal spatiotemporal safe trajectory within the flight corridor (Conference: 2021 IEEE International Conference on Robotics and Automation (ICRA2021); Authors: Han Z, Zhang R, Xu C, Pan N and Gao F; Publication Year: 2021; Article Title: Fast-Tracker: A Robust Aerial System for Tracking Agile Target in Cluttered Environments; Pages: 328-334). However, the above-mentioned optimization-based methods generally require the construction of a special form of optimization problem, and the choice of different trajectory representation methods will affect the quality of the planned trajectory. In terms of trajectory representation methods, probabilistic models are also an effective method. Anderson S et al. from the University of Toronto represented the UAV trajectory as a Gaussian process and used a probabilistic inference model to solve it (Conference: 10th Conference on Robotics-Science and Systems (RSS); Authors: Anderson S, Barfoot TD, Tong CH and S; Publication Year: 2015; Article Title: Batch nonlinear continuous-time trajectory estimation as exactly sparse Gaussian process regression; Pages: 221-238). Singh R et al. from Georgia Institute of Technology used the belief propagation algorithm to solve the problem of probabilistic model inference optimization (Journal: IEEE Transactions on Automatic Control; Authors: Singh R, Haasler I, Zhang QS, Karlsson J and Chen YX; Publication Year: 2022; Article Title: Inference With Aggregate Data in Probabilistic Graphical Models: An Optimal Transport Approach; Pages: 4483-4497). The above work shows that the probabilistic inference model can be effectively used to model and solve the problem of UAV trajectory, and can then be applied to the problem of UAV tracking planning of targets. Summary of the invention

[0005] In order to overcome the shortcomings of the prior art, the present invention aims to address the problem of low accuracy of the traditional single model in predicting the motion of unknown targets, and proposes a target motion prediction method based on an adaptive interactive dual model. To this end, the technical solution adopted by the present invention is to use visual information to realize a method for drones to track moving targets, and the steps are as follows: using the onboard depth camera on the drone, on the one hand, the image is transmitted to the deep learning target detection model YOLOv5s for target recognition, and the depth distance information of the target is found based on the recognition result, and then the three-dimensional coordinates of the target in the world coordinate system are obtained through coordinate conversion; then, a multi-step target prediction method based on dual model interaction is used to realize multi-step prediction of the target position, and predict the position of the target for a period of time in the future; on the other hand, the obstacle information is perceived in real time by the depth camera, and a local Euclidean signed distance field ESDF map is constructed based on the depth image information of the depth camera;

[0006] The trajectory planning of the UAV is divided into two parts: front-end path search and back-end optimization. The front-end path search refers to using the current position and predicted path of the target to search for a path with target motion guidance based on the Euclidean signed distance field (ESDF) map information, making the tracking forward-looking. Based on the path points searched by the front-end, the back-end optimization part constructs a UAV trajectory model based on probabilistic reasoning, designs the likelihood function according to the mission requirements, and finally uses the belief propagation algorithm to optimize the probability model to obtain a smooth and safe trajectory.

[0007] The detailed steps of the method are as follows:

[0008] Step 1) Determine the UAV dynamics model;

[0009] The present invention considers the state of the drone x(t) = [p T (t),v T (t),a T (t)], where p T (t), v T (t) and a T (t) represent the three-dimensional position, velocity and acceleration of the UAV respectively, and the third-order particle model is used as the UAV model, and its dynamic equation is as follows:

[0010]

[0011] The input vector of the model is the three-dimensional jerk vector of the drone, that is, the third-order derivative of the position;

[0012] Step 2) determining the target motion mathematical model;

[0013] Discretize the target's motion model, and its state equation is as follows

[0014] x k =A i x k-1 +w k-1 (2)

[0015] Among them A i is the system transfer matrix, w k-1 is the system error, and different motion models have different forms of system transfer matrices;

[0016] When the target moves in a uniform linear motion, the state of the target is defined as Where x and y represent the positions in the x and y directions respectively. Represent the speed in the x direction and y direction respectively. The system transfer matrix A of this model i The specific form is as follows

[0017]

[0018] When the target moves in a circular motion, the state of the target is defined as Where θ represents the yaw angle of the target. The system transfer matrix A of this model i The specific form is as follows

[0019]

[0020] Step 3) Use the multi-step prediction method based on dual model interaction to realize multi-step prediction of the target position

[0021] The measurement formula that defines the target motion model is

[0022]

[0023] Where z is the observation matrix, H is the measurement matrix, v is the noise, and the variance is R. and P k-1|k-1 are the optimal estimation state and covariance matrix of model j at time k-1, is the state predicted by model j using the state of target k-1. According to the probability vector at k-1, the multi-model interaction method will fuse the optimal estimated state and covariance matrix of each model. The fused estimated state and covariance matrix are:

[0024]

[0025] in is the probability coefficient of the model, and its specific calculation form is as follows

[0026]

[0027] where p ij is an element in the probability transfer matrix, representing the probability of the target being transferred from model i to model j. is the correct probability of model j at time k-1;

[0028] Each model will perform state estimation based on the estimated state after fusion, and the likelihood function value of the model It can quantitatively indicate the accuracy of model j in predicting the target state. Its specific form is as follows

[0029]

[0030] where z k is the measured value of the target state at time k, is the measurement error covariance matrix, which is as follows

[0031]

[0032] According to the likelihood function value of each model at time k, the correct probability value of the model will be updated. The correct probability value of model j at time k is Updated to:

[0033]

[0034] According to the model probability vector at time k, the optimal estimated state at time k is obtained by weighted fusion of the optimal estimated states of each model at time k. The optimal estimated state and covariance matrix of the target at time k output by the adaptive interactive multi-model IMM algorithm are as follows:

[0035]

[0036] Define the conversion ratio of the multi-model interaction k-time model as Its specific form is as follows:

[0037]

[0038] Then the model transition probability at the next moment and Updated to:

[0039]

[0040] The algorithm will adaptively adjust the model transfer probability according to the likelihood value of each model at the previous moment. When the likelihood value of model j at time k is greater than the likelihood value of model i, this indicates that the probability of model j being the matching model is greater, and the probability of the target transferring from model i to model j at time k+1 is will increase, and the probability of model j transferring to model i will decrease;

[0041] The motion model of the target in a complex environment is mainly divided into uniform linear motion and circular motion. Therefore, the multi-step prediction method of the target based on the interaction of two models is used to realize the multi-step prediction of the target position. The target motion model will not change in the short future n time steps. The model probability vector u is a time-invariant vector. According to the optimal estimated state of the target at the current time k, the state of the target at time k+1 can be predicted. The calculation expression is as follows

[0042]

[0043] Since the model probability vector is time-invariant, the predicted state of the target at time k+2 can be calculated from the predicted state at time k+1. Definition Then the function f(x k ) is expressed as Then the predicted state of the target at the next n time steps is The calculation formula is as follows

[0044]

[0045] Step 4) Use the depth image information of the depth camera to construct a local Euclidean signed distance field ESDF map

[0046] The drone obtains the depth image information of the environment through the depth camera, and constructs the local Euclidean signed distance field (ESDF) map based on this information. The specific process is as follows: First, the depth image of the environment is captured by the depth camera to obtain the depth data of each point in the scene; then, the obtained depth image is preprocessed, including denoising, filtering and other operations to ensure the accuracy and reliability of the data; then, the preprocessed depth image is converted into three-dimensional point cloud data to represent the position of each point in the scene in three-dimensional space; based on the generated point cloud data, the Euclidean distance from each point to the nearest obstacle is calculated to construct the local Euclidean signed distance field (ESDF) map;

[0047] Step 5) Design a UAV front-end path search method based on target motion guidance;

[0048] According to the target prediction path, define p n is the predicted target position after n time steps, then the target velocity v after n time steps n Can be obtained by p n and p n-1 The difference calculation shows that the state of the target is defined as Then the total heuristic cost function with target motion orientation is as follows:

[0049] f c =g c +h c =C(x sta ,x cur )+C(x cur ,x pre ) (16)

[0050] where g c =C(x sta ,x cur ) represents the cost from the target initial state to the current state, h c =C(x cur ,x pre ) represents the cost from the current state to the predicted state. The cost from state 1 to state 2 can be converted into solving a problem of optimal boundary value between two points, and then the cost value in the heuristic function can be obtained.

[0051] Step 6) Design a UAV trajectory modeling method based on a probabilistic reasoning model;

[0052] The task of trajectory planning is to find an optimal trajectory x that satisfies the UAV dynamics model. *(t), the posterior probability p(x|e) represents the probability distribution of the trajectory when event e occurs, so the task of trajectory planning is converted to finding the drone trajectory that maximizes the posterior probability. If the events are independent, then the task of trajectory planning is to solve the following problem:

[0053]

[0054] The prior probability p(x) represents the natural distribution of the trajectory itself, and the likelihood probability Πp(e i |x) represents the product of the probability of occurrence of each independent event when a given trajectory is given;

[0055] Assuming that the drone trajectory x(t) is a Gaussian process, its specific form is recorded as The UAV model is also a linear steady-state system, so the trajectory that encourages zero input is obtained, and the trajectory is a Gaussian process with a sparse covariance matrix, and its distribution form is as follows:

[0056]

[0057] in and are the state constraints of the initial position and the end position on the trajectory, respectively, i GP (x i-1 ,x i ) represents the correlation constraint between two adjacent states, and the target speed follows the likelihood function as follows:

[0058]

[0059] where d tar =||p i -p tar || represents the distance between the drone and the target, p i is the current position of the drone, p tar is the target position, d set is the set conversion distance, v tar is the current or predicted target speed value, v des is the expected speed of the drone, Σ vel is the covariance matrix, which can represent the weight coefficients of the velocity likelihood function.

[0060] The target speed following likelihood function can improve the tracking effect of the target. tar >d set When d is the distance between the UAV and the target, the UAV is in the target tracking stage. The speed likelihood function makes the UAV speed tend to the set expected speed, and the UAV gradually shortens the distance with the target. tar <dset When the drone is close to the target, it is in the tracking stage of the target;

[0061] In order to make the trajectory of the UAV tracking plan safely avoid obstacles, the obstacle avoidance likelihood function is designed as follows:

[0062]

[0063] where d obs =||p i -p obs || represents the distance between the drone and the obstacle, p i is the current position of the drone, p obs is the position of the obstacle, d θ is the distance threshold set. In order to improve the smoothness of the UAV trajectory, the acceleration of the UAV should be kept at a low level. The designed acceleration minimization likelihood function is:

[0064]

[0065] The posterior distribution of the state can be calculated from the designed prior distribution and likelihood distribution. Let x(τ) be x i and x i+1 For any state between , the specific form of the Gaussian interpolation formula is expressed as:

[0066] x(τ)=Λ(τ)x i +Ψ(τ)x i+1

[0067] Λ(τ)=Φ(τ,t i )-Ψ(τ)Φ Δt

[0068]

[0069] The factor graph consists of variable nodes and factor nodes. In the factor graph, the prior distribution and likelihood distribution are regarded as factor nodes, and the support state is regarded as a variable node. p is the state constraint factor node applied to the first and last states, f GP is the Gaussian prior factor node between adjacent support states, f obs 、f vel and f acc is the likelihood distribution factor node for each support state, f obs (τ), f vel (τ) and f acc (τ) is the likelihood distribution factor node of a certain interpolation state between two adjacent support states, let f intp is the product of the likelihood distribution factors of all interpolated states in the two support states;

[0070] In order to improve the efficiency of the subsequent probability model reasoning process, the factor graph needs to be simplified: Let f i u For each variable node x i is a unary factor, which is the product of the state constraint factor of each variable node and all likelihood factors, f i b is a binary factor between two adjacent variable nodes, which is the product of the Gaussian prior factors of two adjacent variable nodes and the likelihood factors of all interpolation states. The specific form is as follows:

[0071] f i u =f i obs f i vel f i acc

[0072]

[0073] Step 7) Design a probability model optimization solution method based on belief propagation;

[0074] The current prior distribution and likelihood distribution form are not conducive to the subsequent probability model inference process, so the distribution needs to be converted into the following standard form:

[0075]

[0076] Where η is the message vector and Λ is the precision matrix. This standard form can transform the multiplication operation between factors into the addition operation between vectors and matrices, which improves the efficiency of the subsequent reasoning process. In addition, this solution form can more efficiently solve the maximum a posteriori probability MAP state;

[0077] In order to convert the previously defined distribution into a standard form, the present invention considers the likelihood distribution of the state constraints and the support state to be a general Gaussian distribution form, expressed as

[0078]

[0079] where h(x) is any function of state x, such as the target velocity preservation likelihood function, h e is the expected value, such as the target speed. Perform a first-order Taylor expansion of h(x) at x0, and e(x) is converted to

[0080]

[0081] where J0 is the Jacobian matrix obtained by differentiating h(x) at x0, and e0 is a constant. According to the standard form of (34), the message vector and precision matrix of the general Gaussian distribution are:

[0082]

[0083] The Gaussian prior distribution and the likelihood distribution of the interpolation state are binary functions related to the two adjacent support states, and their partial derivatives with respect to the adjacent support states are:

[0084]

[0085] Similarly, according to the standard form of (34), the message vector and precision matrix of the Gaussian prior distribution and the likelihood distribution of the interpolated state are:

[0086]

[0087] The message vector η and the precision matrix Λ are used to represent the standard form of distribution. The message transmission process is divided into three parts: message transmission from factor nodes to variable nodes, message transmission from variable nodes to factor nodes, and belief update on variable nodes.

[0088] definition is the distribution message of the unary factor itself, is the distribution information of the binary factor itself, is the message transmitted from the factor node to the variable node, It is the message transmitted from the variable node to the factor node. The message transmission process represents the transmission and addition of the message vector and the precision matrix.

[0089] Unary factor node f i u It is only related to the variable node to which it is connected. The message it passes to the variable node is its own distribution information. Binary factor node f i b Will accept from x i News Then merge its own distribution message The combined precision matrix and message vector are:

[0090]

[0091] Then for the variable x i+1 Marginalization, get the binary factor node f i b Passed to variable node x i+1 News for

[0092]

[0093] At the same time, the binary factor node f i b It also receives data from x i+1 News And merge its own distribution messages Then marginalize x i Get passed to variable node x i The message is:

[0094]

[0095] Variable node x i will collect the unary factors f i u and binary factors The message passed, and the combined message is sent to the binary factor f i b , similarly collect the binary factors f i b The message passed and the combined message is sent to the binary factor Send to binary factor and f i b The information is:

[0096]

[0097] After receiving the messages from all adjacent factors, the variable node will update its own information as follows:

[0098]

[0099] After obtaining the posterior distribution of the marginal variable nodes, their maximum posterior probability MAP states are directly calculated as:

[0100]

[0101] The characteristics and beneficial effects of the present invention are:

[0102] 1. In view of the uncertain motion of the target in an unknown complex environment, the present invention uses an adaptive interactive multiple model (IMM) algorithm to predict the position of the target at a future moment. The target motion prediction results of different models are integrated to predict the position of the target at a future moment. This method makes up for the poor flexibility of a single prediction model when performing multi-step predictions on moving targets (uniform linear motion and circular motion), and improves the accuracy of multi-step predictions of the target position;

[0103] 2. A front-end search method based on target motion guidance was designed. This method uses the target's future position as the end point of the UAV's front-end path planning, searches for a path with target motion guidance, and reduces the impact of obstacle occlusion on target tracking;

[0104] 3. A UAV trajectory planning method based on a probabilistic reasoning model was designed. Based on the path points planned by the front end, this method uses a Gaussian process to represent the UAV trajectory, and designs prior distributions and likelihood distributions based on state constraints and obstacle avoidance constraints. The method uses probabilistic reasoning to complete trajectory optimization and obtain a safe trajectory with target motion guidance that meets the UAV dynamics constraints. Compared with the method of representing the UAV trajectory with a piecewise polynomial and performing trajectory optimization, the tracking trajectory generated by this method is smoother;

[0105] 4. Combining the target detection method based on deep learning, the target is detected using airborne sensors, and combined with the above-mentioned tracking planning method, a vision-based detection and tracking planning strategy for mobile targets in complex environments is obtained, and its feasibility is verified through simulation and actual flight experiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0106] Figure 1 It is a schematic diagram of the front-end search of the drone with target motion guidance adopted by the present invention;

[0107] Figure 2 It is a block diagram of the target tracking planning strategy based on visual information in a complex environment adopted by the present invention;

[0108] Figure 3 is a diagram of the UAV platform and experimental scene used in the present invention; in the figure, the quad-rotor UAV platform is as shown in FIG. Figure 3 As shown in (a), the experimental scenario is as follows Figure 3 (b) as shown.

[0109] Figure 4 It is the flight trajectory diagram of the UAV in the target tracking experiment;

[0110] Figure 5 It is the speed curve of the UAV in the target tracking experiment;

[0111] Figure 6 It is the acceleration curve of the UAV in the target tracking experiment. DETAILED DESCRIPTION

[0112] In order to overcome the shortcomings of the prior art, the present invention aims to address the problem of low accuracy of the traditional single model in predicting the motion of unknown targets, and proposes a target motion prediction method based on an adaptive interactive dual model. Based on the target motion prediction results, the present invention uses the position of the target at the future moment as the end point of the UAV front-end path planning, and searches for a path with target motion guidance. On this basis, the present invention uses a probabilistic reasoning method to complete trajectory optimization and obtain a smooth and safe UAV tracking trajectory. In addition, the present invention designs a tracking planning strategy based on visual information, combined with a target detection method based on deep learning, to achieve the connection between target detection and tracking planning. The technical solution adopted by the present invention is to use visual information to realize a method for UAV tracking of moving targets, and the steps are as follows:

[0113] Step 1) Determine the UAV dynamics model;

[0114] The present invention considers the state of the drone x(t) = [p T (t),v T (t),a T (t)], where p T (t), v T (t) and a T (t) represent the three-dimensional position, velocity and acceleration of the UAV respectively, and the third-order particle model is used as the UAV model. Its dynamic equation is as follows:

[0115]

[0116] The input vector of the model is the three-dimensional jerk vector of the drone, that is, the third-order derivative of the position.

[0117] Step 2) determining the target motion mathematical model;

[0118] Discretize the target's motion model, and its state equation is as follows

[0119] x k =A i x k-1 +w k-1 (2)

[0120] Among them A i is the system transfer matrix, w k-1 is the system error. The motion of the target in a complex environment cannot be represented by a single simple motion model. Different motion models have different forms of system transfer matrices.

[0121] When the target moves in a uniform linear motion, the state of the target is defined as Where x and y represent the positions in the x and y directions respectively. Represent the speed in the x direction and y direction respectively. The system transfer matrix A of this model i The specific form is as follows

[0122]

[0123] When the target moves in a circular motion, the state of the target is defined as Where θ represents the yaw angle of the target. The system transfer matrix A of this model i The specific form is as follows

[0124]

[0125] Step 3) Use the multi-step prediction method based on dual model interaction to realize multi-step prediction of the target position

[0126] The measurement formula that defines the target motion model is

[0127]

[0128] Where z is the observation matrix, H is the measurement matrix, v is the noise, and the variance is R. and P k-1|k-1 are the optimal estimation state and covariance matrix of model j at time k-1, is the state predicted by model j using the state of target k-1. According to the probability vector at k-1, the multi-model interaction method will fuse the optimal estimated state and covariance matrix of each model. The fused estimated state and covariance matrix are:

[0129]

[0130] in is the probability coefficient of the model, and its specific calculation form is as follows

[0131]

[0132] where p ij is an element in the probability transfer matrix, representing the probability of the target being transferred from model i to model j. is the correct probability of model j at time k-1;

[0133] Each model will perform state estimation based on the estimated state after fusion, and the likelihood function value of the model It can quantitatively indicate the accuracy of model j in predicting the target state. Its specific form is as follows

[0134]

[0135] where z k is the measured value of the target state at time k, is the measurement error covariance matrix, which is as follows

[0136]

[0137] According to the likelihood function value of each model at time k, the correct probability value of the model will be updated. The correct probability value of model j at time k is Updated to:

[0138]

[0139] According to the model probability vector at time k, the optimal estimated state at time k is obtained by weighted fusion of the optimal estimated states of each model at time k. The optimal estimated state and covariance matrix of the target at time k output by the adaptive interactive multi-model IMM algorithm are as follows:

[0140]

[0141] Define the conversion ratio of the multi-model interaction k-time model as Its specific form is as follows:

[0142]

[0143] Then the model transition probability at the next moment and Updated to:

[0144]

[0145] The algorithm will adaptively adjust the model transfer probability according to the likelihood value of each model at the previous moment. When the likelihood value of model j at time k is greater than the likelihood value of model i, this indicates that the probability of model j being the matching model is greater, and the probability of the target transferring from model i to model j at time k+1 is will increase, and the probability of model j transferring to model i will decrease;

[0146] The present invention assumes that the target motion model in a complex environment is mainly divided into uniform linear motion and circular motion. Therefore, a multi-step prediction method based on dual-model interaction is used to achieve multi-step prediction of the target position. Assuming that the target motion model will not change in the next short time n time steps, the model probability vector u is a time-invariant vector. According to the optimal estimated state of the target at the current time k, the state of the target at time k+1 can be predicted. The calculation expression is as follows:

[0147]

[0148] Since the model probability vector is time-invariant, the predicted state of the target at time k+2 can be calculated from the predicted state at time k+1. Definition Then the function f(x k ) is expressed as Then the predicted state of the target at the next n time steps is The calculation formula is as follows

[0149]

[0150] Step 4) Use the depth image information of the depth camera to construct a local Euclidean signed distance field ESDF map

[0151] The drone obtains the depth image information of the environment through the depth camera, and constructs the local Euclidean signed distance field ESDF map based on this information. The specific process is as follows: First, the depth image of the environment is captured by the depth camera to obtain the depth data of each point in the scene. Then, the obtained depth image is preprocessed, including denoising, filtering and other operations to ensure the accuracy and reliability of the data. Then, the preprocessed depth image is converted into three-dimensional point cloud data to represent the position of each point in the scene in three-dimensional space. Based on the generated point cloud data, the Euclidean distance from each point to the nearest obstacle is calculated to construct the local Euclidean signed distance field ESDF map.

[0152] The Euclidean Signed Distance Field (ESDF) map has high real-time performance. By optimizing the algorithm and data processing flow, it can be quickly generated to meet the navigation needs of robots in dynamic environments. In addition, compared with traditional occupancy grid maps, the Euclidean Signed Distance Field (ESDF) map is more efficient and accurate. It uses a depth camera to directly obtain depth information, which reduces the complexity of data preprocessing and improves the efficiency of map generation.

[0153] Step 5) Design a UAV front-end path search method based on target motion guidance;

[0154] According to the target prediction path, the present invention designs a heuristic function with target motion guidance, which makes the UAV front-end path search forward-looking, such as Figure 1 As shown. According to the generated target prediction path, define p n is the predicted target position after n time steps, then the target velocity v after n time steps n Can be obtained by p n and p n-1 The state of the target is defined as The total heuristic cost function with target motion orientation is as follows

[0155] f c =gc +h c =C(x sta ,x cur )+C(x cur ,x pre ) (16)

[0156] where g c =C(x sta ,x cur ) represents the cost from the target initial state to the current state, h c =C(x cur ,x pre ) represents the cost from the current state to the predicted state. The cost from state 1 to state 2 can be converted into solving a two-point boundary value optimal problem, and then the cost value in the heuristic function can be obtained.

[0157] Step 6) Design a UAV trajectory modeling method based on a probabilistic reasoning model;

[0158] The task of trajectory planning is to find an optimal trajectory x that satisfies the UAV dynamics model. * (t). And the trajectory meets different requirements, such as smooth trajectory and safe avoidance of obstacles. The posterior probability p(x|e) represents the probability distribution of the trajectory when event e occurs, so the task of trajectory planning can be converted into finding the drone trajectory that maximizes the posterior probability. However, the design and solution of the posterior probability are more difficult. If the events are independent, then the task of trajectory planning is to solve the following problem:

[0159]

[0160] The prior probability p(x) represents the natural distribution of the trajectory itself, and the likelihood probability Πp(e i |x) represents the product of the probability of occurrence of each independent event for a given trajectory.

[0161] Assuming that the drone trajectory x(t) is a Gaussian process, its specific form is recorded as If the UAV model is also a linear steady-state system, a trajectory that encourages zero input can be obtained, and the trajectory is a Gaussian process with a sparse covariance matrix, and its distribution form is as follows:

[0162]

[0163] in and are the state constraints of the initial position and the end position on the trajectory, respectively, i GP (x i-1 ,x i ) represents the correlation constraint between two adjacent states.

[0164] The Gaussian prior can meet the basic requirements such as smoothness, while the likelihood function can meet the remaining requirements such as safety, speed maintenance and acceleration minimization. In order to improve the tracking effect, the present invention designs the target speed to follow the likelihood function as

[0165]

[0166] where d tar =||p i -p tar || represents the distance between the drone and the target, p i is the current position of the drone, p tar is the target position, d set is the set conversion distance, v tar is the current or predicted target speed value, v des is the expected speed of the drone, Σ vel is the covariance matrix, which can represent the weight coefficients of the velocity likelihood function.

[0167] The target speed following likelihood function can improve the tracking effect of the target. tar >d set When d is the distance between the UAV and the target, the UAV is in the target tracking stage. The speed likelihood function makes the UAV speed tend to the set expected speed, and the UAV gradually shortens the distance with the target. tar <d set When , the distance between the UAV and the target is relatively close. At this time, the UAV is in the target tracking stage. The speed likelihood function makes the UAV speed tend to the target speed, and the UAV can stably follow the target.

[0168] In order to make the UAV track the planned trajectory safely avoid obstacles, the present invention designs an obstacle avoidance likelihood function as follows:

[0169]

[0170] where d obs =||p i -p obs || represents the distance between the drone and the obstacle, p i is the current position of the drone, p obs is the position of the obstacle, d θ is the set distance threshold. In order to improve the smoothness of the UAV trajectory, the acceleration of the UAV should be kept at a low level. The present invention designs an acceleration minimization likelihood function as follows:

[0171]

[0172] The posterior distribution of the state can be calculated from the designed prior distribution and likelihood distribution, but any state on the time-continuous trajectory still requires an effective interpolation method to calculate it. Let x(τ) be x i and x i+1 For any state between

[0173] x(τ)=Λ(τ)x i +Ψ(τ)x i+1

[0174] Λ(τ)=Φ(τ,t i )-Ψ(τ)Φ Δt

[0175]

[0176] The factor graph consists of variable nodes and factor nodes, which can accurately represent the probability model structure of the trajectory. The factor graph form can more efficiently solve the maximum a posteriori probability MAP problem. In the factor graph, the prior distribution and likelihood distribution are regarded as factor nodes, and the support state is regarded as a variable node. p is the state constraint factor node applied to the first and last states. GP is the Gaussian prior factor node between adjacent support states. f obs 、f vel and f acc is the likelihood distribution factor node for each support state. obs (τ), f vel (τ) and f acc (τ) is the likelihood distribution factor node of a certain interpolation state between two adjacent support states. Let f intp is the product of the likelihood distribution factors of all interpolated states in the two support states.

[0177] In order to improve the efficiency of the subsequent probability model reasoning process, the factor graph needs to be simplified. Let f i u For each variable node x i is a unary factor, which is the product of the state constraint factor of each variable node and all likelihood factors, f i b is a binary factor between two adjacent variable nodes, which is the product of the Gaussian prior factors of two adjacent variable nodes and the likelihood factors of all interpolation states. The specific form is as follows

[0178] f i u =f i obs f i vel fi acc

[0179]

[0180] Step 7) Design a probability model optimization solution method based on belief propagation;

[0181] The current prior distribution and likelihood distribution form are not conducive to the subsequent probability model inference process, so the distribution needs to be converted into the following standard form:

[0182]

[0183] Where η is the message vector and Λ is the precision matrix. This standard form can transform the multiplication operation between factors into the addition operation between vectors and matrices, which improves the efficiency of the subsequent reasoning process. In addition, this solution form can more efficiently solve the maximum a posteriori probability MAP state.

[0184] In order to convert the previously defined distribution into a standard form, the present invention considers the likelihood distribution of the state constraints and the support state to be a general Gaussian distribution form, expressed as

[0185]

[0186] where h(x) is any function of state x, such as the target velocity preservation likelihood function, h e is the expected value, such as the target speed. By performing a first-order Taylor expansion of h(x) at x0, e(x) can be converted to

[0187]

[0188] where J0 is the Jacobian matrix obtained by differentiating h(x) at x0, and e0 is a constant. According to the standard form of (34), the message vector and precision matrix of the general Gaussian distribution are

[0189]

[0190] The Gaussian prior distribution and the likelihood distribution of the interpolated state are binary functions related to the two adjacent support states, and their partial derivatives with respect to the adjacent support states are

[0191]

[0192] Similarly, according to the standard form of (34), the message vector and precision matrix of the Gaussian prior distribution and the likelihood distribution of the interpolated state are

[0193]

[0194] The belief propagation algorithm is a marginal inference algorithm. For a chain-structured factor graph, the message passed from the first node to the last node and returned can accurately calculate the marginal posterior distribution. The present invention uses the message vector η and the precision matrix Λ to represent the standard form of distribution. The message transmission process is divided into three parts: message transmission from factor nodes to variable nodes, message transmission from variable nodes to factor nodes, and belief update on variable nodes.

[0195] definition is the distribution message of the unary factor itself, is the distribution information of the binary factor itself, is the message transmitted from the factor node to the variable node, It is the message transmitted from the variable node to the factor node. The message transmission process represents the transmission and addition of the message vector and the precision matrix.

[0196] Unary factor node f i u It is only related to the variable node to which it is connected. The message it passes to the variable node is its own distribution information. Binary factor node f i b Will accept from x i News Then merge its own distribution message The combined precision matrix and message vector are

[0197]

[0198] Then for the variable x i+1 Marginalization, get the binary factor node f i b Passed to variable node x i+1 News for

[0199]

[0200] At the same time, the binary factor node f i b It also receives data from x i+1 News And merge its own distribution messages Then marginalize x i Get passed to variable node x i The message is

[0201]

[0202] Variable node x i will collect the unary factors f iu and binary factors The message passed, and the combined message is sent to the binary factor f i b Similarly, we collect binary factors f i b The message passed and the combined message is sent to the binary factor Send to binary factor and f i b The information is

[0203]

[0204] After receiving the messages from all adjacent factors, the variable node will update its own information to

[0205]

[0206] After obtaining the posterior distribution of the marginal variable nodes, their maximum posterior probability MAP states can be directly calculated as

[0207]

[0208] Step 8) Design a tracking planning strategy based on visual information

[0209] To solve the problem of tracking targets in complex environments, this paper designs a tracking planning strategy based on visual information, such as Figure 2 As shown in the figure. The drone uses an onboard depth camera to transmit images to the deep learning target detection model Yolov5s network for target recognition, and finds the target's depth distance information based on the recognition results, and then obtains the target's three-dimensional coordinates in the world coordinate system through coordinate conversion. Then the target motion prediction module will perform multi-step predictions on the target position and predict the target's position in the future. On the other hand, the drone will perceive obstacle information in real time through the depth camera, and the map construction module will receive the depth image and construct a local Euclidean signed distance field ESDF map based on the depth image information.

[0210] The trajectory planning module of the drone is divided into two parts: front-end search and back-end optimization. The front-end search module receives the current position and predicted path of the target, and searches for a path with target motion guidance based on the map information, making the tracking forward-looking. Based on the path points searched by the front-end, the back-end optimization module builds a drone trajectory model based on probabilistic reasoning, designs the likelihood function according to the mission requirements, and finally uses the belief propagation algorithm to optimize the probabilistic reasoning model to obtain a smooth and safe trajectory.

[0211] The present invention introduces the mechanism of interactive multi-models and proposes a target motion prediction method based on an adaptive interactive dual model. The present invention uses the result of target prediction as the end point of the front-end path planning of the UAV to search for a path with target motion guidance. Based on the path points planned by the front end, the present invention establishes a probabilistic reasoning model and designs a belief propagation solution method to complete trajectory optimization. In addition, the present invention designs a tracking planning strategy based on visual information, combined with a target detection method based on deep learning, to achieve the connection between target detection and tracking planning. The technical solution adopted by the present invention is a target tracking method for UAVs using visual information in a complex environment, and the steps are as follows:

[0212] Step 1) Determine the UAV dynamics model;

[0213] The present invention considers the state of the drone x(t) = [p T (t),v T (t),a T (t)], where p T (t), v T (t) and a T (t) represent the three-dimensional position, velocity and acceleration of the UAV respectively, and the third-order particle model is used as the UAV model. Its dynamic equation is as follows:

[0214]

[0215] The input vector of the model is the three-dimensional jerk vector of the drone, that is, the third-order derivative of the position.

[0216] Step 2) determining the target motion mathematical model;

[0217] Discretize the target's motion model, and its state equation is as follows

[0218] x k =A i x k-1 +w k-1 (2)

[0219] Among them A i is the system transfer matrix, w k-1 is the system error. The motion of the target in a complex environment cannot be represented by a single simple motion model. Different motion models have different forms of system transfer matrices.

[0220] When the target moves in a uniform linear motion, the state of the target is defined as Where x and y represent the positions in the x and y directions respectively. Represent the speed in the x direction and y direction respectively. The system transfer matrix A of this model i The specific form is as follows

[0221]

[0222] When the target moves in a circular motion, the state of the target is defined as Where θ represents the yaw angle of the target. The system transfer matrix A of this model i The specific form is as follows

[0223]

[0224] Step 3) Use the multi-step prediction method based on dual model interaction to realize multi-step prediction of the target position

[0225] The measurement formula that defines the target motion model is

[0226]

[0227] Where z is the observation matrix, H is the measurement matrix, v is the noise, and the variance is R. and P k-1|k-1 are the optimal estimation state and covariance matrix of model j at time k-1, is the state predicted by model j using the state of target k-1. According to the probability vector at k-1, the multi-model interaction method will fuse the optimal estimated state and covariance matrix of each model. The fused estimated state and covariance matrix are:

[0228]

[0229] in is the probability coefficient of the model, and its specific calculation form is as follows

[0230]

[0231] where p ij is an element in the probability transfer matrix, representing the probability of the target being transferred from model i to model j. is the correct probability of model j at time k-1;

[0232] Each model will perform state estimation based on the estimated state after fusion, and the likelihood function value of the model It can quantitatively indicate the accuracy of model j in predicting the target state. Its specific form is as follows

[0233]

[0234] where z k is the measured value of the target state at time k, is the measurement error covariance matrix, which is as follows

[0235]

[0236] According to the likelihood function value of each model at time k, the correct probability value of the model will be updated. The correct probability value of model j at time k is Updated to:

[0237]

[0238] According to the model probability vector at time k, the optimal estimated state at time k is obtained by weighted fusion of the optimal estimated states of each model at time k. The optimal estimated state and covariance matrix of the target at time k output by the adaptive interactive multi-model IMM algorithm are as follows:

[0239]

[0240] Define the conversion ratio of the multi-model interaction k-time model as Its specific form is as follows:

[0241]

[0242] Then the model transition probability at the next moment and Updated to:

[0243]

[0244] The algorithm will adaptively adjust the model transfer probability according to the likelihood value of each model at the previous moment. When the likelihood value of model j at time k is greater than the likelihood value of model i, this indicates that the probability of model j being the matching model is greater, and the probability of the target transferring from model i to model j at time k+1 is will increase, and the probability of model j transferring to model i will decrease;

[0245] The present invention assumes that the target motion model in a complex environment is mainly divided into uniform linear motion and circular motion. Therefore, a multi-step prediction method based on dual-model interaction is used to achieve multi-step prediction of the target position. Assuming that the target motion model will not change in the next short time n time steps, the model probability vector u is a time-invariant vector. According to the optimal estimated state of the target at the current time k, the state of the target at time k+1 can be predicted. The calculation expression is as follows:

[0246]

[0247] Since the model probability vector is time-invariant, the predicted state of the target at time k+2 can be calculated from the predicted state at time k+1. Definition Then the function f(x k ) is expressed as Then the predicted state of the target at the next n time steps is The calculation formula is as follows

[0248]

[0249] Step 4) Use the depth image information of the depth camera to construct a local Euclidean signed distance field ESDF map

[0250] The drone obtains the depth image information of the environment through the depth camera, and constructs the local Euclidean signed distance field ESDF map based on this information. The specific process is as follows: First, the depth image of the environment is captured by the depth camera to obtain the depth data of each point in the scene. Then, the obtained depth image is preprocessed, including denoising, filtering and other operations to ensure the accuracy and reliability of the data. Then, the preprocessed depth image is converted into three-dimensional point cloud data to represent the position of each point in the scene in three-dimensional space. Based on the generated point cloud data, the Euclidean distance from each point to the nearest obstacle is calculated to construct the local Euclidean signed distance field ESDF map.

[0251] The Euclidean Signed Distance Field (ESDF) map has high real-time performance. By optimizing the algorithm and data processing flow, it can be quickly generated to meet the navigation needs of robots in dynamic environments. In addition, compared with traditional occupancy grid maps, the Euclidean Signed Distance Field (ESDF) map is more efficient and accurate. It uses a depth camera to directly obtain depth information, which reduces the complexity of data preprocessing and improves the efficiency of map generation.

[0252] Step 5) Design a UAV front-end path search method based on target motion guidance;

[0253] According to the target prediction path, the present invention designs a heuristic function with target motion guidance, which makes the UAV front-end path search forward-looking, such as Figure 1 As shown. According to the generated target prediction path, define p n is the predicted target position after n time steps, then the target velocity v after n time steps n Can be obtained by p n and p n-1 The state of the target is defined as The total heuristic cost function with target motion orientation is as follows

[0254] f c =g c +h c =C(x sta ,x cur )+C(x cur ,x pre) (16)

[0255] where g c =C(x sta ,x cur ) represents the cost from the target initial state to the current state, h c =C(x cur ,x pre ) represents the cost from the current state to the predicted state. The cost from state 1 to state 2 can be converted into solving a two-point boundary value optimal problem, and then the cost value in the heuristic function can be obtained.

[0256] Step 6) Design a UAV trajectory modeling method based on a probabilistic reasoning model;

[0257] The task of trajectory planning is to find an optimal trajectory x that satisfies the UAV dynamics model. * (t). And the trajectory meets different requirements, such as smooth trajectory and safe avoidance of obstacles. The posterior probability p(x|e) represents the probability distribution of the trajectory when event e occurs, so the task of trajectory planning can be converted into finding the drone trajectory that maximizes the posterior probability. However, the design and solution of the posterior probability are more difficult. If the events are independent, then the task of trajectory planning is to solve the following problem:

[0258]

[0259] The prior probability p(x) represents the natural distribution of the trajectory itself, and the likelihood probability Πp(e i |x) represents the product of the probability of occurrence of each independent event for a given trajectory.

[0260] Assuming that the drone trajectory x(t) is a Gaussian process, its specific form is recorded as If the UAV model is also a linear steady-state system, a trajectory that encourages zero input can be obtained, and the trajectory is a Gaussian process with a sparse covariance matrix, and its distribution form is as follows:

[0261]

[0262] in and are the state constraints of the initial position and the end position on the trajectory, respectively, i GP (x i-1 ,x i ) represents the correlation constraint between two adjacent states.

[0263] The Gaussian prior can meet the basic requirements such as smoothness, while the likelihood function can meet the remaining requirements such as safety, speed maintenance and acceleration minimization. In order to improve the tracking effect, the present invention designs the target speed to follow the likelihood function as

[0264]

[0265] where d tar =||p i -p tar || represents the distance between the drone and the target, p i is the current position of the drone, p tar is the target position, d set is the set conversion distance, v tar is the current or predicted target speed value, v des is the expected speed of the drone, Σ vel is the covariance matrix, which can represent the weight coefficients of the velocity likelihood function.

[0266] The target speed following likelihood function can improve the tracking effect of the target. tar >d set When d is the distance between the UAV and the target, the UAV is in the target tracking stage. The speed likelihood function makes the UAV speed tend to the set expected speed, and the UAV gradually shortens the distance with the target. tar <d set When , the distance between the UAV and the target is relatively close. At this time, the UAV is in the target tracking stage. The speed likelihood function makes the UAV speed tend to the target speed, and the UAV can stably follow the target.

[0267] In order to make the UAV track the planned trajectory safely avoid obstacles, the present invention designs an obstacle avoidance likelihood function as follows:

[0268]

[0269] where d obs =||p i -p obs || represents the distance between the drone and the obstacle, p i is the current position of the drone, p obs is the position of the obstacle, d θ is the set distance threshold. In order to improve the smoothness of the UAV trajectory, the acceleration of the UAV should be kept at a low level. The present invention designs an acceleration minimization likelihood function as follows:

[0270]

[0271] The posterior distribution of the state can be calculated from the designed prior distribution and likelihood distribution, but any state on the time-continuous trajectory still requires an effective interpolation method to calculate it. Let x(τ) be x i and x i+1 For any state between

[0272] x(τ)=Λ(τ)x i +Ψ(τ)x i+1

[0273] Λ(τ)=Φ(τ,t i )-Ψ(τ)Φ Δt

[0274]

[0275] The factor graph consists of variable nodes and factor nodes, which can accurately represent the probability model structure of the trajectory. The factor graph form can more efficiently solve the maximum a posteriori probability MAP problem. In the factor graph, the prior distribution and likelihood distribution are regarded as factor nodes, and the support state is regarded as a variable node. p is the state constraint factor node applied to the first and last states. GP is the Gaussian prior factor node between adjacent support states. f obs 、f vel and f acc is the likelihood distribution factor node for each support state. obs (τ), f vel (τ) and f acc (τ) is the likelihood distribution factor node of a certain interpolation state between two adjacent support states. Let f intp is the product of the likelihood distribution factors of all interpolated states in the two support states.

[0276] In order to improve the efficiency of the subsequent probability model reasoning process, the factor graph needs to be simplified. Let f i u For each variable node x i is a unary factor, which is the product of the state constraint factor of each variable node and all likelihood factors, f i b is a binary factor between two adjacent variable nodes, which is the product of the Gaussian prior factors of two adjacent variable nodes and the likelihood factors of all interpolation states. The specific form is as follows

[0277] f i u =f i obs f i vel fi acc

[0278]

[0279] Step 7) Design a probability model optimization solution method based on belief propagation;

[0280] The current prior distribution and likelihood distribution form are not conducive to the subsequent probability model inference process, so the distribution needs to be converted into the following standard form:

[0281]

[0282] Where η is the message vector and Λ is the precision matrix. This standard form can transform the multiplication operation between factors into the addition operation between vectors and matrices, which improves the efficiency of the subsequent reasoning process. In addition, this solution form can more efficiently solve the maximum a posteriori probability MAP state.

[0283] In order to convert the previously defined distribution into a standard form, the present invention considers the likelihood distribution of the state constraints and the support state to be a general Gaussian distribution form, expressed as

[0284]

[0285] where h(x) is any function of state x, such as the target velocity preservation likelihood function, h e is the expected value, such as the target speed. By performing a first-order Taylor expansion of h(x) at x0, e(x) can be converted to

[0286]

[0287] where J0 is the Jacobian matrix obtained by differentiating h(x) at x0, and e0 is a constant. According to the standard form of (34), the message vector and precision matrix of the general Gaussian distribution are

[0288]

[0289] The Gaussian prior distribution and the likelihood distribution of the interpolated state are binary functions related to the two adjacent support states, and their partial derivatives with respect to the adjacent support states are

[0290]

[0291] Similarly, according to the standard form of (34), the message vector and precision matrix of the Gaussian prior distribution and the likelihood distribution of the interpolated state are

[0292]

[0293] The belief propagation algorithm is a marginal inference algorithm. For a chain-structured factor graph, the message passed from the first node to the last node and returned can accurately calculate the marginal posterior distribution. The present invention uses the message vector η and the precision matrix Λ to represent the standard form of distribution. The message transmission process is divided into three parts: message transmission from factor nodes to variable nodes, message transmission from variable nodes to factor nodes, and belief update on variable nodes.

[0294] definition is the distribution message of the unary factor itself, is the distribution information of the binary factor itself, is the message transmitted from the factor node to the variable node, It is the message transmitted from the variable node to the factor node. The message transmission process represents the transmission and addition of the message vector and the precision matrix.

[0295] Unary factor node f i u It is only related to the variable node to which it is connected. The message it passes to the variable node is its own distribution information. Binary factor node f i b Will accept from x i News Then merge its own distribution message The combined precision matrix and message vector are

[0296]

[0297] Then for the variable x i+1 Marginalization, get the binary factor node f i b Passed to variable node x i+1 News for

[0298]

[0299] At the same time, the binary factor node f i b It also receives data from x i+1 News And merge its own distribution messages Then marginalize x i Get passed to variable node x i The message is

[0300]

[0301] Variable node x i will collect the unary factors f iu and binary factors The message passed, and the combined message is sent to the binary factor f i b Similarly, we collect binary factors f i b The message passed and the combined message is sent to the binary factor Send to binary factor and f i b The information is

[0302]

[0303] After receiving the messages from all adjacent factors, the variable node will update its own information to

[0304]

[0305] After obtaining the posterior distribution of the marginal variable nodes, their maximum posterior probability MAP states can be directly calculated as

[0306]

[0307] Step 8) Design a tracking planning strategy based on visual information

[0308] To solve the problem of tracking targets in complex environments, this paper designs a tracking planning strategy based on visual information, such as Figure 2 As shown in the figure. The drone uses an onboard depth camera to transmit images to the deep learning target detection model Yolov5s network for target recognition, and finds the target's depth distance information based on the recognition results, and then obtains the target's three-dimensional coordinates in the world coordinate system through coordinate conversion. Then the target motion prediction module will perform multi-step predictions on the target position and predict the target's position in the future. On the other hand, the drone will perceive obstacle information in real time through the depth camera, and the map construction module will receive the depth image and construct a local Euclidean signed distance field ESDF map based on the depth image information.

[0309] The trajectory planning module of the drone is divided into two parts: front-end search and back-end optimization. The front-end search module receives the current position and predicted path of the target, and searches for a path with target motion guidance based on the map information, making the tracking forward-looking. Based on the path points searched by the front-end, the back-end optimization module builds a drone trajectory model based on probabilistic reasoning, designs the likelihood function according to the mission requirements, and finally uses the belief propagation algorithm to optimize the probabilistic reasoning model to obtain a smooth and safe trajectory.

[0310] The present invention is described in detail below with reference to specific examples and accompanying drawings.

[0311] 1. Experimental verification and analysis

[0312] (1) Experimental platform and parameter selection

[0313] Quadrotor UAV platform Figure 3 As shown in (a), the drone uses the motion capture system Optitrack for positioning and the depth camera Intel Realsense D435i for visual perception. Figure 3 As shown in (b), several cylindrical obstacles are randomly placed in the scene.

[0314] In the experiment, the position information of the target and obstacles is unknown. The drone perceives local obstacles through the depth image of the depth camera D435i and constructs a local Euclidean signed distance field (ESDF) map based on the depth image information. On the other hand, the drone uses color images and depth images to identify targets and obtain target distances respectively, and obtains the target position through coordinate conversion.

[0315] Set the expected speed v of the drone in tracking state des is 0.5m / s, tracking conversion distance d set 0.9m, maximum acceleration 1m / s 2 , and the other trajectory parameters are set the same as in the simulation. The target UAV is controlled by the remote controller and starts to move about 3.5m after the UAV starts tracking planning. The movement speed v tar For safety reasons, when the distance between the drone and the target is less than the safe distance of 0.8m, the drone will stop suddenly. When the distance between the two is greater than the starting distance of 0.9m, the drone will perform the tracking planning task.

[0316] (2) Experimental results analysis

[0317] The experimental flight results are as follows Figure 4 As shown in the figure, the trajectory of the drone when tracking the target and planning to pass through obstacles. In the tracking stage far away from the target, the distance between the drone and the target gradually decreases. In the tracking stage close to the target, due to the setting of the safety distance and the starting distance, and the planning of the speed following function, the distance between the drone and the target is stable at 0.8-0.9m.

[0318] The speed and acceleration curves of the drone in the experiment are as follows: Figure 5 and Figure 6As shown in the figure, in the tracking phase, the drone can reach the set expected speed of 0.5m / s, and in the tracking phase, the drone can try to maintain a speed close to the target of 0.4m / s. On the other hand, except for the start and emergency stop process, the acceleration curve of the drone is relatively smooth and can be maintained at a low level.

[0319] The experimental results show that the proposed method can complete the target recognition and obstacle perception based on visual information in real time in complex environments, and can complete the tracking planning of the target while safely avoiding obstacles. The speed of the drone can be stabilized near the expected speed at different stages, and the speed and acceleration curves are relatively smooth, with good tracking and tracing effects.

[0320] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed in the present invention should be covered within the protection scope of the present invention.

Claims

1. A method for using visual information to realize unmanned aerial vehicle tracking of moving targets, characterized in that the steps are: As follows: Using the onboard depth camera on the drone, on the one hand, the image is transmitted to the deep learning target detection model YOLOv5s for target recognition, and the depth distance information of the target is found according to the recognition result, and then the three-dimensional coordinates of the target in the world coordinate system are obtained through coordinate conversion; then the multi-step prediction method of the target based on the interaction of the two models is used to realize the multi-step prediction of the target position, and the position of the target in the future is predicted; on the other hand, the obstacle information is perceived in real time by the depth camera, and the local Euclidean signed distance field ESDF map is constructed according to the depth image information of the depth camera; The trajectory planning of the UAV is divided into two parts: front-end path search and back-end optimization. The front-end path search refers to using the current position and predicted path of the target to search for a path with target motion guidance based on the Euclidean signed distance field ESDF map information, so that the tracking is forward-looking; based on the path points searched by the front-end, the back-end optimization part constructs a UAV trajectory model based on probabilistic reasoning, and designs the likelihood function according to the task requirements. Finally, the belief propagation algorithm is used to optimize the probability model to obtain a smooth and safe trajectory; the detailed steps are as follows: Step 1) Determine the UAV dynamics model; The state of the drone is expressed as: x(t) = [p T (t),v T (t),a T (t)], where p T (t), v T (t) and a T (t) represent the three-dimensional position, velocity and acceleration of the UAV respectively, and the third-order particle model is used as the UAV model, and its dynamic equation is as follows: The input vector of the model is the three-dimensional jerk vector of the drone, that is, the third-order derivative of the position; Step 2) determining the target motion mathematical model; Discretize the target's motion model, and its state equation is as follows x k =A i x k-1 +w k-1 (2) Among them A i is the system transfer matrix, w k-1 is the system error, and different motion models have different forms of system transfer matrices; When the target moves in a uniform linear motion, the state of the target is defined as Where x and y represent the positions in the x and y directions respectively. Represent the speed in the x direction and y direction respectively. The system transfer matrix A of this model i The specific form is as follows When the target moves in a circular motion, the state of the target is defined as Where θ represents the yaw angle of the target, and the system transfer matrix A of the model i The specific form is as follows: Step 3) Use the multi-step prediction method based on dual model interaction to realize multi-step prediction of the target position The measurement formula that defines the target motion model is Where z is the observation matrix, H is the measurement matrix, v is the noise, and the variance is R. and P k-1|k-1 are the optimal estimation state and covariance matrix of model j at time k-1, is the state predicted by model j using the state of target k-1. According to the probability vector at k-1, the multi-model interaction method will fuse the optimal estimated state and covariance matrix of each model. The fused estimated state and covariance matrix are: in is the probability coefficient of the model, and its specific calculation form is as follows where p ij is an element in the probability transfer matrix, representing the probability of the target being transferred from model i to model j. is the correct probability of model j at time k-1; Each model will perform state estimation based on the estimated state after fusion, and the likelihood function value of the model It can quantitatively indicate the accuracy of model j in predicting the target state. Its specific form is as follows where z k is the measured value of the target state at time k, is the measurement error covariance matrix, which is as follows According to the likelihood function value of each model at time k, the correct probability value of the model will be updated. The correct probability value of model j at time k is Updated to: According to the model probability vector at time k, the optimal estimated state at time k is obtained by weighted fusion of the optimal estimated states of each model at time k. The optimal estimated state and covariance matrix of the target at time k output by the adaptive interactive multi-model IMM algorithm are as follows: Define the conversion ratio of the multi-model interaction k-time model as Its specific form is as follows: Then the model transition probability at the next moment and Updated to: The algorithm will adaptively adjust the model transfer probability according to the likelihood value of each model at the previous moment. When the likelihood value of model j at time k is greater than the likelihood value of model i, this indicates that the probability of model j being the matching model is greater, and the probability of the target transferring from model i to model j at time k+1 is will increase, and the probability of model j transferring to model i will decrease; The motion model of the target in a complex environment is mainly divided into uniform linear motion and circular motion. Therefore, the multi-step prediction method of the target based on the interaction of two models is used to realize the multi-step prediction of the target position. The target motion model will not change in the short future n time steps. The model probability vector u is a time-invariant vector. According to the optimal estimated state of the target at the current time k, the state of the target at time k+1 can be predicted. The calculation expression is as follows Since the model probability vector is time-invariant, the predicted state of the target at time k+2 can be calculated from the predicted state at time k+1, and the definition is Then the function f(x k ) is expressed as Then the predicted state of the target at the next n time steps is The calculation formula is as follows Step 4) Use the depth image information of the depth camera to construct a local Euclidean signed distance field ESDF map The drone obtains the depth image information of the environment through the depth camera, and constructs the local Euclidean signed distance field (ESDF) map based on this information. The specific process is as follows: First, the depth image of the environment is captured by the depth camera to obtain the depth data of each point in the scene; then, the obtained depth image is preprocessed, including denoising, filtering and other operations to ensure the accuracy and reliability of the data; then, the preprocessed depth image is converted into three-dimensional point cloud data to represent the position of each point in the scene in three-dimensional space; based on the generated point cloud data, the Euclidean distance from each point to the nearest obstacle is calculated to construct the local Euclidean signed distance field (ESDF) map; Step 5) Design a UAV front-end path search method based on target motion guidance; According to the target prediction path, define p n is the predicted target position after n time steps, then the target velocity v after n time steps n Can be obtained by p n and p n-1 The difference calculation shows that the state of the target is defined as Then the total heuristic cost function with target motion orientation is as follows: f c =g c +h c =C(x sta ,x cur )+C(x cur ,x pre ) (16) where g c =C(x sta ,x cur ) represents the cost from the target initial state to the current state, h c =C(x cur ,x pre ) represents the cost from the current state to the predicted state. The cost from state 1 to state 2 can be converted into solving a problem of optimal boundary value between two points, and then the cost value in the heuristic function can be obtained. Step 6) Design a UAV trajectory modeling method based on a probabilistic reasoning model; The task of trajectory planning is to find an optimal trajectory x that satisfies the UAV dynamics model. * (t), the posterior probability p(x|e) represents the probability distribution of the trajectory when event e occurs, so the task of trajectory planning is converted to finding the drone trajectory that maximizes the posterior probability. If the events are independent, then the task of trajectory planning is to solve the following problem: The prior probability p(x) represents the natural distribution of the trajectory itself, and the likelihood probability Πp(e i |x) represents the product of the probability of occurrence of each independent event when a given trajectory is given; Assuming that the drone trajectory x(t) is a Gaussian process, its specific form is recorded as The UAV model is also a linear steady-state system, so the trajectory that encourages zero input is obtained, and the trajectory is a Gaussian process with a sparse covariance matrix, and its distribution form is as follows: in and are the state constraints of the initial position and the end position on the trajectory, respectively, i GP (x i-1 ,x i ) represents the correlation constraint between two adjacent states, and the target speed follows the likelihood function as follows: where d tar =||p i -p tar || represents the distance between the drone and the target, p i is the current position of the drone, p tar is the target position, d set is the set conversion distance, v tar is the current or predicted target speed value, v des is the expected speed of the drone, Σ ve l is the covariance matrix, which can represent the weight coefficient of the velocity likelihood function; The target speed following likelihood function can improve the tracking effect of the target. tar >d set When d is reached, the UAV is far away from the target. At this time, the UAV is in the stage of tracking the target. The speed likelihood function makes the UAV speed tend to the set expected speed. The UAV gradually shortens the distance with the target. tar <d set When the drone is close to the target, it is in the tracking stage of the target; In order to make the trajectory of the UAV tracking plan safely avoid obstacles, the obstacle avoidance likelihood function is designed as follows: where d obs =||p i -p obs || represents the distance between the drone and the obstacle, p i is the current position of the drone, p obs is the position of the obstacle, d θ is the distance threshold set. In order to improve the smoothness of the UAV trajectory, the acceleration of the UAV should be kept at a low level. The designed acceleration minimization likelihood function is: The posterior distribution of the state can be calculated from the designed prior distribution and likelihood distribution. Let x(τ) be x i and x i+1 For any state between , the specific form of the Gaussian interpolation formula is expressed as: x(τ)=Λ(τ)x i +Ψ(t)x i+1 Λ(τ)=Φ(τ,t i )-Ψ(τ)Φ Δt The factor graph consists of variable nodes and factor nodes. In the factor graph, the prior distribution and likelihood distribution are regarded as factor nodes, and the support state is regarded as a variable node. p is the state constraint factor node applied to the first and last states, f GP is the Gaussian prior factor node between adjacent support states, f obs 、f vel and f acc is the likelihood distribution factor node for each support state, f obs (τ), f vel (τ) and f acc (τ) is the likelihood distribution factor node of a certain interpolation state between two adjacent support states, let f intp is the product of the likelihood distribution factors of all interpolated states in the two support states; In order to improve the efficiency of the subsequent probability model reasoning process, the factor graph needs to be simplified: Let f i u For each variable node x i is a unary factor, which is the product of the state constraint factor of each variable node and all likelihood factors, f i b is a binary factor between two adjacent variable nodes, which is the product of the Gaussian prior factors of two adjacent variable nodes and the likelihood factors of all interpolation states. The specific form is as follows: Step 7) Design a probability model optimization solution method based on belief propagation; The current prior distribution and likelihood distribution form are not conducive to the subsequent probability model inference process, so the distribution needs to be converted into the following standard form: Where η is the message vector and Λ is the precision matrix. This standard form can transform the multiplication operation between factors into the addition operation between vectors and matrices, which improves the efficiency of the subsequent reasoning process. In addition, this solution form can more efficiently solve the maximum a posteriori probability MAP state. The likelihood distribution of the state constraints and support states is in the form of a general Gaussian distribution, so the previously defined distribution is converted to the standard form expression: where h(x) is any function of state x, such as the target velocity preservation likelihood function, h e is the expected value, such as the target speed. Perform a first-order Taylor expansion of h(x) at x0 and convert e(x) into where J0 is the Jacobian matrix obtained by differentiating h(x) at x0, e0 is a constant. According to the standard form of (34), the message vector and precision matrix of the general Gaussian distribution are: The Gaussian prior distribution and the likelihood distribution of the interpolation state are binary functions related to the two adjacent support states, and their partial derivatives with respect to the adjacent support states are: Similarly, according to the standard form of (34), the message vector and precision matrix of the Gaussian prior distribution and the likelihood distribution of the interpolated state are: The message vector η and the precision matrix Λ are used to represent the standard form of distribution. The message transmission process is divided into three parts: message transmission from factor nodes to variable nodes, message transmission from variable nodes to factor nodes, and belief update on variable nodes. definition is the distribution message of the unary factor itself, is the distribution information of the binary factor itself, is the message transmitted from factor node to variable node, It is the message transmitted from the variable node to the factor node. The message transmission process represents the transmission and addition of the message vector and the precision matrix. Unary factor node f i u It is only related to the variable node to which it is connected. The message it passes to the variable node is its own distribution information. Binary factor node f i b Will accept from x i News Then merge its own distribution message The combined precision matrix and message vector are: Then for the variable x i+1 Marginalization, get the binary factor node f i b Passed to variable node x i+1 News for At the same time, the binary factor node f i b It also receives data from x i+1 News And merge its own distribution messages Then marginalize x i Get passed to variable node x i The message is: Variable node x i will collect the unary factors f i u and binary factors The message passed, and the combined message is sent to the binary factor f i b , similarly collect the binary factors f i b The message passed and the combined message is sent to the binary factor Send to binary factor and f i b The information is: After receiving the messages from all adjacent factors, the variable node will update its own information as follows: After obtaining the posterior distribution of the marginal variable nodes, their maximum posterior probability MAP states are directly calculated as:

Images