An unmanned aerial vehicle trajectory planning method for large maneuvering target tracking in an obstacle environment
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- NORTH CHINA ELECTRIC POWER UNIV
- Filing Date
- 2026-03-24
- Publication Date
- 2026-06-12
Smart Images
Figure CN122195090A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of unmanned aerial vehicle (UAV) target tracking technology, and in particular to a UAV trajectory planning method for tracking highly maneuverable targets in obstacle environments. Background Technology
[0002] Drones are widely used in aerial photography, target tracking, and security patrols. Autonomous target tracking requires the use of onboard sensors to detect targets and obstacles in real time and generate safe flight trajectories.
[0003] Existing moving target tracking methods are mainly divided into two categories: one is the visual servoing-based control method, which directly generates control commands through image deviation, but does not fully consider obstacles, resulting in collision risks; the other is the method that integrates environmental perception and trajectory planning, which achieves tracking in obstacle scenarios by constructing an environmental model and introducing target motion prediction. However, the latter still has the following problems when tracking highly maneuvering targets in obstacle environments: path planning only relies on the target's current position or short-term trajectory, resulting in response lag when the target performs large maneuvers, which can easily lead to target loss; using a fixed tracking distance makes the line of sight easily obstructed by obstacles; and failing to consider the visibility of obstacles in the direction of movement leads to perception blind spots and a high risk of collision. Summary of the Invention
[0004] To overcome the shortcomings of existing technologies, the purpose of this invention is to provide a UAV trajectory planning method for tracking large maneuvering targets in obstacle environments. By constructing a hierarchical framework of "target motion prediction - path planning front end - trajectory optimization back end", integrating target history and prediction information as well as visibility enhancement mechanisms, this invention solves the problems of slow response, easy obstruction of line of sight and low visibility of obstacles in existing technologies, and achieves high success rate and low risk robust tracking of large maneuvering targets in obstacle environments.
[0005] To achieve the above objectives, the present invention provides the following solution: A UAV trajectory planning method for tracking highly maneuverable targets in obstacle-prone environments includes: A target motion prediction model is constructed based on the historical position data of the target observed in real time by airborne sensors. The future trajectory of the target is parametrically represented by Bézier curves. The maximum velocity and maximum acceleration of the target are constrained by the convex hull property of the Bézier curves. An objective function is established that integrates residual terms and second-order regularization terms. A time weighting coefficient is introduced to distinguish the credibility of observations at different times. The target motion prediction problem is transformed into a constrained quadratic programming problem and solved by OOQP to obtain the smooth predicted trajectory of the target in the future. Based on hybrid A The algorithm framework constructs a heuristic search method, designs a heuristic function that integrates the current state and the predicted state of the target, introduces the difference between the current observed velocity and the predicted velocity of the target into the velocity compensation mechanism, and dynamically adjusts the tracking distance threshold to generate discrete and dynamically feasible path nodes as the initial guide for backend trajectory optimization. The tracking trajectory of the UAV is parametrically expressed using four-dimensional B-spline curves. Based on this, a constraint system consisting of target visibility constraints, obstacle visibility constraints, UAV kinematic constraints, and trajectory collision-free constraints is constructed. The optimization objective of the trajectory optimization problem is constructed based on the penalty function method, and the L-BFGS algorithm is used to solve it, resulting in a continuous and smooth tracking trajectory that satisfies multiple constraints.
[0006] Preferably, a Bézier curve is used to parameterize the future trajectory of the target, and the convex hull property of the Bézier curve is used to apply constraints on the target's maximum velocity and maximum acceleration to the control points, including: Establish the parameterized equations for the nth-order Bézier curve; The future trajectory of the target is parameterized using a set of control points and an nth-order Bernstein polynomial basis. By utilizing the convex hull property of Bézier curves, the target maximum velocity and target maximum acceleration constraints are applied to the control points.
[0007] Preferably, an objective function integrating residual terms and second-order regularization terms is established, and a time-weighted coefficient is introduced to distinguish the reliability of observations at different times. This transforms the target motion prediction problem into a constrained quadratic programming problem, which is then solved using OOQP, including: When new target observations are obtained, a new target prediction trajectory is generated by fitting historical observation positions. The residual term is constructed to ensure that the predicted target trajectory is close to the historical observation position; The second-order regularization term is constructed to ensure that the predicted trajectory of the target is smooth and physically reasonable; The time-weighted coefficients and weight terms are introduced to adjust the trade-off between fitting history and maintaining smoothness; The target motion prediction problem is transformed into a constrained quadratic programming problem and solved using OOQP to obtain the smooth predicted trajectory of the target over a future period of time.
[0008] Preferably, a heuristic function is designed to integrate the current state of the target with the predicted state of the target, including: Determine the target's current state and the target's predicted state, wherein the target's current state includes the target's current position and the target's current observed velocity, and the target's predicted state includes the predicted position and the target's predicted velocity; When the path planning reaches time τ, the target prediction state is taken from the state of the target prediction trajectory S(t) at time τ; The desired state is determined by a weighted combination of the current state of the target and the predicted state of the target. Calculate the shortest distance between the current state of the UAV and the desired state, obtained by solving the optimal boundary value problem and satisfying the dynamic constraints, and construct a time penalty term; The heuristic function is obtained based on the shortest distance and the time penalty term.
[0009] Preferably, the difference between the target's current observed velocity and the target's predicted velocity is incorporated into the velocity compensation mechanism, including: While keeping the direction of the predicted velocity of the target unchanged, calculate the difference between the magnitude of the predicted velocity of the target and the current observed velocity of the target; Calculate the directional deviation angle between the current speed and the predicted speed; The error gain coefficient is determined based on the directional deviation angle. Based on the difference in speed magnitude, the maximum speed observed during flight, and the error gain coefficient, the amplitude of the target predicted speed is dynamically adjusted to obtain the target predicted speed after speed compensation.
[0010] Preferably, dynamically adjusting the tracking distance threshold to generate discrete, dynamically feasible path nodes includes: Motion trajectory primitives are constructed based on discretized velocity and acceleration control inputs; During the search process, the expansion of each node corresponds to a set of discrete sampled values of the control input, and candidate trajectories are generated through numerical integration; In the initial stage where only the root node exists, the tracking distance threshold is relaxed to expand the node search range; Safe and dynamically feasible trajectories are selected from the voxel mesh to generate the discrete, dynamically feasible path nodes.
[0011] Preferably, the process of constructing the target visibility constraint includes: The environmental complexity is assessed based on the number of voxels occupied within the camera's field of view, and the adaptive adjustment distance is determined based on the environmental complexity. The smooth transition parameters are determined by the Sigmoid function, and an adaptive tracking distance constraint is constructed based on the adaptive adjustment distance and the smooth transition parameters. Set constraints on the tracking angle so that the central axis of the camera's field of view faces the tracking target, thus constructing the tracking angle constraint; The camera field of view is divided using an ellipsoid sequence, and the distance from the center of the ellipsoid to the nearest obstacle is obtained by constructing an ESDF. The distance from the center of the ellipsoid to the nearest obstacle is compared with the overall equivalent distance from the center of the i-th ellipsoid to the surface to construct a line-of-sight occlusion constraint; The adaptive tracking distance constraint, the tracking angle constraint, and the line-of-sight occlusion avoidance constraint are collectively used as the target visibility constraint.
[0012] Preferably, the process of constructing the obstacle visibility constraint includes: The angle between the drone's line of sight and its own speed is defined as the heading angle deviation; The horizontal half-field of view of the UAV is used as the upper limit of the heading angle deviation; The obstacle visibility constraint is constructed by constraining the heading angle deviation to be within the horizontal half-field of view of the UAV.
[0013] Preferably, the process of constructing the UAV kinematic constraints and collision-free trajectory constraints includes: By utilizing the convex hull property of B-splines, the continuous constraints of the UAV's own velocity, acceleration and its rate of change, yaw rate, angular acceleration and its rate of change are transformed into constraints on the B-spline control points, thus constructing the UAV's kinematic constraints. Apply a hard constraint to each B-spline control point, wherein the hard constraint is that the distance between the B-spline control point and the obstacle is not less than a safety threshold. Based on the convex hull property of B-spline curves, the distance between all B-spline control points and obstacles is not less than the safety threshold, thus constructing a collision-free trajectory constraint.
[0014] Preferably, the optimization objective of the trajectory optimization problem is constructed based on the penalty function approach, and solved using the L-BFGS algorithm, including: Based on the target visibility constraint, the obstacle visibility constraint, the UAV kinematic constraints, and the trajectory collision-free constraint, the optimized target is constructed; The optimization objective is represented as a weighted combination of tracking distance cost, tracking angle cost, line-of-sight occlusion cost, obstacle visibility cost, UAV motion cost, and obstacle avoidance cost; The L-BFGS algorithm is used to solve the optimization objective to obtain the continuous smooth tracking trajectory.
[0015] The present invention discloses the following beneficial effects: This invention addresses the problems of existing technologies for tracking highly maneuvering targets in obstacle-prone environments, such as path planning relying solely on the target's current position or short-term trajectory, leading to response lag; fixed tracking distances causing line-of-sight obstruction; and failure to consider obstacle visibility along the direction of movement, resulting in blind spots and increased collision risk. It employs a combined approach of target motion prediction, heuristic path planning, and trajectory optimization. First, a smooth predicted trajectory satisfying the target's maximum velocity and maximum acceleration constraints is obtained based on historical target position data. Then, the target's current state and predicted state are jointly introduced into a heuristic search, dynamically adjusted using a velocity compensation mechanism and tracking distance threshold to generate discrete, dynamically feasible path nodes. Finally, based on four-dimensional B-spline curves, trajectory optimization is completed under the combined influence of target visibility constraints, obstacle visibility constraints, UAV kinematic constraints, and collision-free trajectory constraints. This ensures that the generated tracking trajectory maintains good continuity, foresight, and collision avoidance feasibility even when the target undergoes significant maneuvers, while mitigating target loss and collision risks caused by line-of-sight obstruction and the unseen nature of obstacles in the forward direction. Attached Figure Description
[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0017] Figure 1 A flowchart of the method provided in an embodiment of the present invention; Figure 2 This is an overall framework diagram provided for an embodiment of the present invention; Figure 3 The hybrid A-based embodiment of the present invention provides The tracking path planning flowchart; Figure 4 This is a schematic diagram of the tracking effect without velocity compensation provided in an embodiment of the present invention; Figure 5 This is a schematic diagram of the tracking effect after applying velocity compensation, provided in an embodiment of the present invention. Figure 6 This is a diagram of the backend framework for optimizing drone tracking trajectories provided in an embodiment of the present invention. Figure 7 This is a schematic diagram illustrating the tracking effect of a UAV without adaptive tracking distance constraints, provided in an embodiment of the present invention. Figure 8 A schematic diagram illustrating the tracking effect of an unmanned aerial vehicle (UAV) with an adaptive tracking distance constraint applied, provided in an embodiment of the present invention. Figure 9This is a schematic diagram of the angular velocity change curve of the target velocity direction angle during the tracking process provided in an embodiment of the present invention; Figure 10 This is a schematic diagram illustrating the tracking effect without applied obstacle visibility constraints, provided by an embodiment of the present invention. Figure 11 This is a schematic diagram illustrating the tracking effect after applying obstacle visibility constraints, provided in an embodiment of the present invention. Figure 12 This is a schematic diagram illustrating the tracking effect of vis-planner provided in an embodiment of the present invention; Figure 13 This is a schematic diagram illustrating the tracking effect of the fast-tracker provided in an embodiment of the present invention; Figure 14 This is a schematic diagram illustrating the tracking effect of the method provided in an embodiment of the present invention; Figure 15 This is a schematic diagram of the angular velocity change curve of the target velocity direction angle during the tracking process provided in an embodiment of the present invention; Figure 16 This is a schematic diagram illustrating the tracking effect of vis-planner provided in an embodiment of the present invention; Figure 17 This is a schematic diagram illustrating the tracking effect of the fast-tracker provided in an embodiment of the present invention; Figure 18 This is a schematic diagram illustrating the tracking effect of the method provided in an embodiment of the present invention; Figure 19 This is a schematic diagram illustrating the effect of the vis-planner tracking process provided in an embodiment of the present invention; Figure 20 This is a schematic diagram illustrating the effect of the fast-tracker tracking process provided in an embodiment of the present invention; Figure 21 This is a schematic diagram illustrating the effect of the method during the tracking process provided in this embodiment of the invention; Figure 22 The diagram shows the UAV command and response curves during the tracking process provided in this embodiment of the invention. Detailed Implementation
[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0019] The purpose of this invention is to provide a UAV trajectory planning method for tracking large maneuvering targets in obstacle environments. This method can alleviate the problems of response lag, line-of-sight obstruction, and missed detection of forward obstacles in tracking large maneuvering targets. It can also take into account the continuity of target tracking, trajectory smoothness, and flight safety in obstacle environments.
[0020] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0021] Figure 1 The method flowchart provided in the embodiments of the present invention is as follows: Figure 1 As shown, this invention provides a UAV trajectory planning method for tracking large maneuvering targets in obstacle environments, including: Step 100: Construct a target motion prediction model based on the target's historical position data observed in real time by airborne sensors. Use Bézier curves to parameterize the target's future trajectory and apply the maximum target velocity and maximum target acceleration constraints to the control points using the convex hull property of Bézier curves. Establish an objective function that integrates residual terms and second-order regularization terms. Introduce time weighting coefficients to distinguish the credibility of observations at different times. Transform the target motion prediction problem into a constrained quadratic programming problem and solve it using OOQP to obtain the smooth predicted trajectory of the target over a period of time in the future. Step 200: Based on Hybrid A The algorithm framework constructs a heuristic search method, designs a heuristic function that integrates the current state and the predicted state of the target, introduces the difference between the current observed velocity and the predicted velocity of the target into the velocity compensation mechanism, and dynamically adjusts the tracking distance threshold to generate discrete and dynamically feasible path nodes as the initial guide for backend trajectory optimization. Step 300: The UAV tracking trajectory is parameterized using a four-dimensional B-spline curve. Based on this, a constraint system is constructed consisting of target visibility constraints, obstacle visibility constraints, UAV kinematic constraints, and trajectory collision-free constraints. The optimization objective of the trajectory optimization problem is constructed based on the penalty function method, and the L-BFGS algorithm is used to solve it, resulting in a continuous and smooth tracking trajectory that satisfies multiple constraints.
[0022] This embodiment discloses a UAV trajectory planning method for tracking large maneuvering targets in obstacle environments. Its core lies in constructing a hierarchical planning framework of "target motion prediction – path planning front-end – trajectory optimization back-end." By integrating target history and prediction information, as well as a visibility enhancement mechanism, robust tracking of large maneuvering targets in obstacle environments is achieved. The overall framework diagram of this invention is shown below. Figure 2 As shown, the specific process is as follows: Step 1: Predicting target motion considering dynamic constraints The goal of this phase is to predict the target's trajectory over a future period based on historical observation data, providing prior information for subsequent path planning and trajectory optimization. The specific implementation process is as follows: Step 1.1: Target motion trajectory parameterization This invention addresses the problem of predicting the trajectory of highly maneuvering targets by using Bézier curves to describe the target's motion trajectory, thereby satisfying the constraints of continuity and feasibility. The parameterized equation of the Bézier curve is:
[0023] in, It is the set of control points of the Bézier curve; yes Bernstein polynomial basis of order 1.
[0024] Using the convex hull property of Bézier curves, the following constraints are imposed on the control points to account for the velocity and acceleration boundaries of a highly maneuvering target:
[0025] in, and These are the target's maximum speed and maximum acceleration, respectively.
[0026] Step 1.2: Construct the objective function and solve for the predicted trajectory Assuming the target's accurate location information is Furthermore, its trajectory can be approximated by a smooth curve. When new target observations are obtained, a new target prediction trajectory is generated by fitting historical observation positions. The objective function for curve fitting is shown in the following equation:
[0027] The first term is the residual term, which ensures that the predicted trajectory is close to the historical observation position; the second term is the second-order regularization term, which ensures that the trajectory is smooth and physically reasonable. The length of the historical observation queue; These are time-weighted coefficients used to differentiate the reliability of observations from those at different times; This is a weighting term used to adjust the trade-off between fitting history and maintaining smoothness.
[0028] Based on the objective function constructed above, the target motion prediction problem can be transformed into a constrained quadratic programming problem. Then, by using OOQP to solve it, the predicted trajectory of the target over a period of time in the future can be obtained, providing a reference for subsequent tracking trajectory planning.
[0029] Step 2: Front-end of heuristic path planning that integrates goal prediction This stage is based on hybrid A. The algorithm framework generates a discrete, dynamically feasible sequence of path nodes by designing a heuristic function that integrates the target's historical and predicted states, as well as a velocity compensation mechanism, to serve as the initial guide for backend trajectory optimization.
[0030] Step 2.1: Mix A Algorithm search space construction Based on hybrid A The algorithm's path planning process is as follows: Figure 3 As shown, the algorithm constructs motion trajectory primitives based on discretized control inputs such as velocity and acceleration. During the search process, the expansion of each node corresponds to a set of discrete sampled values of the control inputs. Candidate trajectories are generated through numerical integration, and finally, safe and dynamically feasible trajectories are selected from the voxel mesh diagram.
[0031] This invention employs a dynamic adjustment method for the tracking distance threshold during the search process. In the initial stage, when only the root node is present, the tracking threshold is relaxed to expand the number of nodes, preventing tracking failures due to insufficient search range in the initial phase. Simultaneously, a heuristic function integrates historical and real-time target trajectories, prioritizing the exploration of the space near the target's future trajectory, thus mitigating the lag in UAV tracking response during high-maneuverability targets.
[0032] Step 2.2: Heuristic search strategy integrating target prediction and velocity compensation Mixed A The search efficiency of the algorithm is highly dependent on the heuristic function. The heuristic function designed in this invention... as follows:
[0033] in, This is the current status of the drone. With the expected state The shortest distance between the two sides obtained by solving the optimal boundary value problem and satisfying the dynamic constraints; This is a time-based penalty item; Estimate the total time for the trajectory; Current time. Expected state. The calculation formula is as follows:
[0034] in, As weight; The current state of the target. The target's current position, The current observed velocity of the target; Predict the state for the target. To predict the location, Predict the target speed. During the path planning process... At that time, predict the state Taken from the target predicted trajectory exist The state at any given moment, i.e. .
[0035] When a drone tracks a target, if the target experiences a sudden change in speed, such as a large maneuver, directly using the target's predicted speed corresponding to the predicted trajectory is insufficient. Unable to adapt to sudden speed changes, resulting in drone tracking lag, such as Figure 4 As shown in the figure For different times. In order to mitigate the UAV response lag during large target maneuvers, while maintaining the predicted speed. Assuming the direction remains unchanged, based on the currently observed target velocity and The difference between them is used to dynamically adjust the amplitude of the predicted velocity for velocity compensation, such as... Figure 5 As shown, the drone can respond and track the target in a timely manner. The calculation method for speed compensation is shown in the following formula:
[0036] in, The difference between the predicted velocity of the target and the current observed velocity; This represents the maximum speed observed during the flight. The error gain coefficient is expressed as follows:
[0037] in, , Let be the gain coefficient, satisfying ; This represents the directional deviation angle between the current speed and the predicted speed.
[0038] Using the above method, it is possible to achieve the result based on the direction deviation angle. Dynamically adjust prediction speed : (1) When At that time, the target was in an accelerating state, as As the velocity decreases, the predicted velocity of the target tends to align with the direction of the current observed velocity. Furthermore, its value is constantly increasing, which drives the increase in the predicted speed amplitude, thereby enabling the UAV to respond to the target's acceleration trend in advance and reduce tracking delay.
[0039] (2) When At that time, the target was in a deceleration state, and as As the velocity continues to increase, the deviation between the predicted velocity and the currently observed velocity direction intensifies. Furthermore, the value is continuously decreasing, reducing the predicted velocity amplitude, preventing the drone from deviating from the actual movement due to excessive tracking of historical trajectories, and improving the tracking robustness in scenarios with large target maneuvers.
[0040] Step 3: Trajectory Optimization Backend for Obstacle Visibility Enhancement Trajectory optimization framework such as Figure 6 As shown, firstly, trajectory parameterization is achieved using four-dimensional B-splines to reduce constraint complexity by leveraging their convex hull properties; secondly, visibility constraints are introduced and, together with traditional trajectory optimization constraints, constitute a multi-dimensional constraint system; finally, an optimization model is constructed based on a penalty function and solved using the L-BFGS algorithm to generate the optimal trajectory that satisfies all constraints.
[0041] Step 3.1: Trajectory parameterization based on four-dimensional B-splines The trajectory is parameterized using a four-dimensional B-spline curve, and the trajectory position... With B-spline control points The relationship between them can be represented as:
[0042] Step 3.2: Constructing Target Visibility Constraints Target visibility constraints include three aspects: adaptive tracking distance constraints, tracking angle constraints, and line-of-sight occlusion avoidance constraints.
[0043] (1) Adaptive tracking distance constraint To reduce the probability of drones' line of sight being obstructed in complex obstacle environments, the environmental complexity is assessed based on the number of obstacles, and the drone's tracking distance is then adaptively adjusted.
[0044] The adaptive tracking distance constraints are as follows:
[0045] in, and These represent the distances between the drone and the target, respectively. The vertical and horizontal components; and These represent the minimum and maximum vertical distances between the drone and the target, respectively; these are set values. and These represent the minimum and maximum horizontal distances between the UAV and the target, respectively, and are adaptively adjusted according to environmental complexity. The calculation formulas are as follows:
[0046] in, and These represent the minimum and maximum horizontal distances between the drone and the target, respectively, which satisfy the following relationship; For smooth transition parameters; To adaptively adjust the distance.
[0047]
[0048] in, This is the gain coefficient.
[0049] When environmental complexity is low, it is desirable to maintain a relatively long tracking distance for better environmental observation; when environmental complexity is high, it is desirable to maintain a relatively short tracking distance to ensure stable tracking of the target. Adaptive distance adjustment. The calculation formula is shown below.
[0050]
[0051] in, It represents the number of voxels occupied within the field of view, characterizing the complexity of the environment; Within a certain period of time Maximum value.
[0052] Smooth transition parameters The calculation formula is shown below. When hour, Approaching 0, the drone's target distance is maintained at arrive Between; when hour, Approaching 1, the drone target distance is maintained at arrive Between; when hour, Smooth transitions to avoid abrupt parameter changes.
[0053]
[0054] in, This is the steepness coefficient of the Sigmoid curve; This is the critical coefficient for environmental complexity, and its value is determined through statistical experiments.
[0055] When the target bypasses the obstacle, if the drone does not use adaptive tracking distance conditions, such as Figure 7 As shown, because the drone cannot actively adjust the tracking distance, its line of sight is obstructed by obstacles; if the drone adopts adaptive tracking distance constraints, such as Figure 8 As shown, the drone dynamically reduces its tracking distance when detecting obstacles, thus better maintaining tracking of the target.
[0056] (2) Tracking angle constraint To maintain camera observation of the target, it is desirable that the central axis of the camera's field of view is oriented towards the tracked target. This invention addresses the tracking angle... Set the following constraints:
[0057] in, The maximum allowable tracking angle error; To ensure the UAV's desired tracking angle is pointed towards the target by the camera's central axis, the calculation method is as follows:
[0058] in, , ; and These represent the position vectors of the target and the UAV in the world coordinate system, respectively.
[0059] (3) Avoid obstructing or restricting the line of sight The constraint of avoiding obstruction of sight lines can be expressed by the following formula:
[0060] in, This is the distance from the center of the ellipsoid to the nearest obstacle, which can be obtained by constructing an ESDF; For the first i The global equivalent distance from the center of an ellipsoid to its surface is expressed as follows.
[0061]
[0062] in, , , ellipsoids The three half-axis. For example... Figure 9 As shown, the triangular pyramid represents the robot's field of view (FOV), which is divided into a series of inscribed ellipsoidal regions. The desired ellipsoid contains only the target and no obstacles; the center of the ellipsoid... The three semi-axles are determined by the following formula:
[0063]
[0064]
[0065]
[0066] in, To divide along the direction of sight The normalization ratio of the segment; Indicates the first Segmented ellipsoid; and These are the horizontal field of view and the vertical field of view, respectively.
[0067] Step 3.3: Constructing Obstacle Visibility Constraints Obstacle visibility refers to whether a drone can reliably observe obstacles in its own direction of movement, and its constraints are defined as follows.
[0068]
[0069] in, This is the heading angle deviation, which is the angle between the UAV's line of sight and its own speed; The speed of the drone.
[0070] like Figure 10 and Figure 11 As shown, when the heading angle deviation Larger than the horizontal half-field of view of the drone At times, the drone cannot observe obstacles in its direction of movement, which may lead to a collision; while when Restricted to Within this range, drones can ensure visibility of obstacles in the direction of movement.
[0071] Step 3.4: Construction of UAV kinematic constraints Utilizing the convex hull property of B-splines, this invention measures the speed of the UAV itself. acceleration and its rate of change yaw rate angular acceleration and its rate of change The continuous constraints are transformed into constraints on its control vertices, as follows:
[0072]
[0073]
[0074] in, and It represents the maximum value of velocity and acceleration; and It represents the maximum values of angular velocity and angular acceleration; , It is the maximum value of the rate of change of acceleration and the rate of change of angular acceleration.
[0075] Step 3.5: Constructing Collision-Free Trajectory Constraints To ensure that the generated trajectory is collision-free, this invention provides each control point... Apply the following hard constraints:
[0076] in, This is the safety threshold. Due to the convex hull property of the B-spline curve, the distance between all control points and the obstacle is no less than... This ensures that the entire trajectory is collision-free.
[0077] Step 3.6: Trajectory Optimization Model Construction and Solution Based on the constraint system defined above, the optimization objective of the trajectory optimization problem constructed in this invention using the penalty function approach is as follows:
[0078] in, It is the total cost. It is the cost of tracking distance. It is the cost of tracking angle. It's the price of avoiding obstructed vision. It is the cost of obstructing visibility. It is the cost of drone operations. It is the cost of obstacle avoidance; It is a weighted vector, and each component The weights correspond to different sub-costs. The sub-costs are introduced below in turn.
[0079] (1) Tracking distance cost The definition is as follows:
[0080] in, For the penalty function; , These represent the horizontal and vertical distances between the drone and the target it is tracking, respectively.
[0081] (2) Tracking angle cost It can be represented as:
[0082] in, To ensure that the camera's central axis points towards the target at the desired tracking angle of the drone; The maximum allowable tracking angle error.
[0083] (3) Avoid the cost of shading It can be represented as:
[0084] in, It can be obtained by constructing an ESDF; It is the number of ellipsoids; For the first The combined distance from the center of an ellipsoid to its surface.
[0085] (4) Cost of visibility of obstacles It can be represented as
[0086] in, This is half the camera's field of view. The speed of the drone.
[0087] (5) Cost of drone operation It consists of the following four components:
[0088] in, The cost is speed and acceleration; This comes at the cost of angular velocity and angular acceleration. This comes at the cost of the rate of change of acceleration. This represents the cost of the rate of change of angular acceleration. The expression for the cost is:
[0089]
[0090]
[0091]
[0092] in, , , , These are the maximum values of velocity, acceleration, angular velocity, and angular acceleration, respectively. The rate of change of acceleration; denoted as the rate of change of angular acceleration.
[0093] (6) Cost of obstacle avoidance It can be represented as:
[0094] in, This is a safety threshold; ESDF distance of the control point.
[0095] Based on the completed trajectory optimization model, the L-BFGS algorithm is used to solve the above model to obtain the desired trajectory result.
[0096] Example 1: Comparison of tracking failure probability when UAV line of sight is obstructed This embodiment randomly generates 100 cylindrical and circular obstacles on the map. Under the condition that the target drone moves along the same preset path, the tracking performance of the method of this invention, vis-planner, and fast-tracker are compared. Figures 12 to 14 As shown.
[0097] Figure 12 and Figure 13 In the case of a tracking drone based on vis-planner and fast-tracker, when passing through a narrow area, although the target drone remains within the FOV of the tracking drone, its line of sight is obstructed by obstacles, resulting in tracking failure. Figure 14 The adaptive tracking distance adjustment method of the present invention can actively adjust the tracking distance according to the complexity of the current environment, avoid obstruction of the line of sight, and thus achieve successful tracking.
[0098] To further compare the tracking failure probability caused by line-of-sight occlusion under different tracking methods, 10 maps were generated for scenarios with 50, 100, and 150 obstacles. Twenty different target drone flight paths were randomly generated on each map, ensuring the paths were outside the obstacles. The percentage of tracking failures in 200 tracking attempts was calculated. The statistical results of the probability of tracking failure due to line-of-sight occlusion are shown in Table 1 below: Table 1 Comparison of Tracking Failure Probabilities
[0099] Experimental results show that in an environment with 50 obstacles, the tracking failure probability of the method of this invention is similar to the other two methods. This is because the perceived environmental complexity is low, resulting in a larger tracking distance, which is close to the set tracking distance value. However, as the number of obstacles increases, the method of this invention can adaptively reduce the tracking distance according to the environmental complexity, thereby reducing the tracking failure probability. In an environment with 150 obstacles, the tracking failure probability is reduced by approximately 7.5% and 4% compared to Fast Tracker and Vis-Planner, respectively.
[0100] Example 2: Simulation of tracking large maneuvering targets in an obstacle environment This embodiment compares the tracking performance of the present invention's method, vis-planner, and fast-tracker along a preset target drone's motion path, and simultaneously plots the target's motion during the tracking process. Figure 15 As shown, the response curve of the UAV is as follows: Figures 16 to 22 As shown.
[0101] Figure 15The curve showing the angular velocity change of the target UAV's velocity direction angle is displayed. Excluding the instantaneous state when the tracking starts and the target moves from a stationary state, the maximum angular velocity during the target's motion is 1.63 rad / s.
[0102] Figure 16 , Figure 17 , Figure 18 This demonstrates a comparison of tracking performance under a global path. When the target drone executes a movement close to 180... During high maneuvers, Figure 16 and Figure 17 The tracking drone based on vis-planner and fast-tracker experienced response lag and ultimately lost the target; Figure 18 The tracking drone based on the method of this invention can quickly respond to the target's turning trend and generate an effective tracking trajectory. To facilitate further analysis of tracking details, Figure 19 , Figure 20 , Figure 21 The tracking effect was captured at a specific moment during the tracking process. Figure 19 and Figure 20 In the image, the red trajectory marked by the black circle represents the high-frequency replanning trajectory caused by obstacles obstructing the line of sight of the tracking drone based on vis-planner and fast-tracker. Furthermore... Figure 19 In the middle, at this time, the tracking drone's heading angle deviation The obstacle was too large, and it failed to detect the obstacle within the black box in front of its own movement, posing a risk of collision; in contrast, Figure 21 The tracking drone based on the method of this invention can not only quickly shorten the tracking distance according to the complexity of the environment and avoid line-of-sight obstruction, but also limit the visibility of obstacles by applying them. This ensures visibility of obstacles in the drone's direction of movement and reduces the risk of collision.
[0103] Figure 22 The command output and actual response curves for the UAV tracking the target are shown. The total test duration was 47.6 seconds, with 4761 data points collected at an average sampling frequency of 100 Hz. Figure 22 It can be seen that the drone's response curve is relatively smooth, without continuous or divergent oscillations, and the delay is also small, indicating that the command signals given by the motion planning can be well tracked by the drone.
[0104] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.
[0105] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.
Claims
1. A UAV trajectory planning method for tracking large maneuvering targets in obstacle environments, characterized in that, include: A target motion prediction model is constructed based on the historical position data of the target observed in real time by airborne sensors. The future trajectory of the target is parametrically represented by Bézier curves. The maximum velocity and maximum acceleration of the target are constrained by the convex hull property of the Bézier curves. An objective function is established that integrates residual terms and second-order regularization terms. A time weighting coefficient is introduced to distinguish the credibility of observations at different times. The target motion prediction problem is transformed into a constrained quadratic programming problem and solved by OOQP to obtain the smooth predicted trajectory of the target in the future. Based on hybrid A The algorithm framework constructs a heuristic search method, designs a heuristic function that integrates the current state and the predicted state of the target, introduces the difference between the current observed velocity and the predicted velocity of the target into the velocity compensation mechanism, and dynamically adjusts the tracking distance threshold to generate discrete and dynamically feasible path nodes as the initial guide for backend trajectory optimization. The tracking trajectory of the UAV is parametrically expressed using four-dimensional B-spline curves. Based on this, a constraint system consisting of target visibility constraints, obstacle visibility constraints, UAV kinematic constraints, and trajectory collision-free constraints is constructed. The optimization objective of the trajectory optimization problem is constructed based on the penalty function method, and the L-BFGS algorithm is used to solve it, resulting in a continuous and smooth tracking trajectory that satisfies multiple constraints.
2. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, The future trajectory of the target is parametrically represented using Bézier curves, and the convex hull property of the Bézier curves is used to impose constraints on the control points regarding the target's maximum velocity and maximum acceleration, including: Establish the parameterized equations for the nth-order Bézier curve; The future trajectory of the target is parameterized using a set of control points and an nth-order Bernstein polynomial basis. By utilizing the convex hull property of the Bézier curve, the target maximum velocity and target maximum acceleration constraints are applied to the control point.
3. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, An objective function integrating residual and second-order regularization terms is established, and a time-weighted coefficient is introduced to distinguish the reliability of observations at different times. The target motion prediction problem is transformed into a constrained quadratic programming problem, which is then solved using OOQP, including: When new target observations are obtained, a new target prediction trajectory is generated by fitting historical observation positions. The residual term is constructed to ensure that the predicted trajectory of the target is close to the historical observation position; The second-order regularization term is constructed to ensure that the predicted trajectory of the target is smooth and physically reasonable; The time-weighted coefficients and weight terms are introduced to adjust the trade-off between fitting history and maintaining smoothness; The target motion prediction problem is transformed into a constrained quadratic programming problem and solved using OOQP to obtain the smooth predicted trajectory of the target over a future period of time.
4. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, Design a heuristic function that integrates the current state and the predicted state of the target, including: Determine the target's current state and the target's predicted state, wherein the target's current state includes the target's current position and the target's current observed velocity, and the target's predicted state includes the predicted position and the target's predicted velocity; When the path planning reaches time τ, the target prediction state is taken from the state of the target prediction trajectory S(t) at time τ; The desired state is determined by a weighted combination of the current state of the target and the predicted state of the target. Calculate the shortest distance between the current state of the UAV and the desired state, obtained by solving the optimal boundary value problem and satisfying the dynamic constraints, and construct a time penalty term; The heuristic function is obtained based on the shortest distance and the time penalty term.
5. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, The difference between the target's current observed velocity and its predicted velocity is incorporated into the velocity compensation mechanism, including: While keeping the direction of the predicted velocity of the target unchanged, calculate the difference between the magnitude of the predicted velocity of the target and the current observed velocity of the target; Calculate the directional deviation angle between the current speed and the predicted speed; The error gain coefficient is determined based on the directional deviation angle. Based on the difference in speed magnitude, the maximum speed observed during flight, and the error gain coefficient, the amplitude of the target predicted speed is dynamically adjusted to obtain the target predicted speed after speed compensation.
6. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, Dynamically adjust the tracking distance threshold to generate discrete, dynamically feasible path nodes, including: Motion trajectory primitives are constructed based on discretized velocity and acceleration control inputs; During the search process, the expansion of each node corresponds to a set of discrete sampled values of the control input, and candidate trajectories are generated through numerical integration; In the initial stage where only the root node exists, the tracking distance threshold is relaxed to expand the node search range; Safe and dynamically feasible trajectories are selected from the voxel mesh to generate the discrete, dynamically feasible path nodes.
7. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, The process of constructing the target visibility constraint includes: The environmental complexity is assessed based on the number of voxels occupied within the camera's field of view, and the adaptive adjustment distance is determined based on the environmental complexity. The smooth transition parameters are determined by the Sigmoid function, and an adaptive tracking distance constraint is constructed based on the adaptive adjustment distance and the smooth transition parameters. Set constraints on the tracking angle so that the central axis of the camera's field of view faces the tracking target, thus constructing the tracking angle constraint; The camera field of view is divided using an ellipsoid sequence, and the distance from the center of the ellipsoid to the nearest obstacle is obtained by constructing an ESDF. The distance from the center of the ellipsoid to the nearest obstacle is compared with the overall equivalent distance from the center of the i-th ellipsoid to the surface to construct a line-of-sight occlusion constraint; The adaptive tracking distance constraint, the tracking angle constraint, and the line-of-sight occlusion avoidance constraint are collectively used as the target visibility constraint.
8. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, The process of constructing the obstacle visibility constraints includes: The angle between the drone's line of sight and its own speed is defined as the heading angle deviation; The horizontal half-field of view of the UAV is used as the upper limit of the heading angle deviation; The obstacle visibility constraint is constructed by constraining the heading angle deviation to be within the horizontal half-field of view of the UAV.
9. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, The process of constructing the UAV kinematic constraints and collision-free trajectory constraints includes: By utilizing the convex hull property of B-splines, the continuous constraints of the UAV's own velocity, acceleration and its rate of change, yaw rate, angular acceleration and its rate of change are transformed into constraints on the B-spline control points, thus constructing the UAV's kinematic constraints. Apply a hard constraint to each B-spline control point, wherein the hard constraint is that the distance between the B-spline control point and the obstacle is not less than a safety threshold. Based on the convex hull property of B-spline curves, the distance between all B-spline control points and obstacles is not less than the safety threshold, thus constructing a collision-free trajectory constraint.
10. The UAV trajectory planning method for tracking large maneuvering targets in obstacle environments according to claim 1, characterized in that, The optimization objective of the trajectory optimization problem is constructed based on the penalty function approach, and solved using the L-BFGS algorithm, including: Based on the target visibility constraint, the obstacle visibility constraint, the UAV kinematic constraints, and the trajectory collision-free constraint, the optimized target is constructed; The optimization objective is represented as a weighted combination of tracking distance cost, tracking angle cost, line-of-sight occlusion cost, obstacle visibility cost, UAV motion cost, and obstacle avoidance cost; The L-BFGS algorithm is used to solve the optimization objective to obtain the continuous smooth tracking trajectory.