Track optimization method based on visibility

Abstract

The invention relates to the technical field of unmanned aerial vehicle trajectory optimization, in particular to a visibility-based trajectory optimization method, which comprises the following steps of: determining an end point at the midpoint of a target, and parameterizing a fourth-order spline; optimizing eight items of micro-cost; and carrying out gain function boundary constraint. According to the method, an unmanned aerial vehicle tracking end point is determined through a target prediction trajectory midpoint, firstly, an initial path is generated in combination with an A * algorithm, and trajectory smoothness is ensured through fourth-order B spline parameterization; secondly, constructing an optimization model comprising eight cost functions including observation distance, angle maneuver, relative angle, shielding avoidance and the like; and finally, through the nonlinear boundary constraint characteristic of the gain function, when the target approaches the view field boundary, the control intensity is automatically enhanced, so that the target is always kept in an effective area of an image plane, and the problem that the target motion characteristic cannot be responded in real time due to static environment modeling and discrete path planning is effectively solved. And target escape and visual field loss are caused.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) trajectory optimization technology, and in particular to a trajectory optimization method based on visibility. Background Technology

[0002] Unmanned aerial vehicle (UAV) target tracking and trajectory generation technologies are the core support for autonomous decision-making in intelligent unmanned systems. As mission environments become increasingly complex, existing technologies face fundamental challenges: the high-speed maneuverability of dynamic targets and the strong constraints of obstacle-dense environments create a tension of contradiction. Traditional static environment modeling methods cannot adapt to sudden target changes, discrete path planning causes trajectory jumps that lead to field of view shifts, and the fragmented perception and control modules struggle to achieve a dynamic balance between target escape risks and obstacle avoidance requirements. This results in high failure rates for tracking tasks in critical scenarios, severely restricting the operational capabilities of UAVs in real combat environments.

[0003] Patent document CN120176678A discloses a UAV navigation method based on planar visibility. The method includes: acquiring environmental image data, extracting scene feature points and clustering the feature points into multiple planar surfaces; establishing a local coordinate system centered on the current position, and determining planar visibility based on position correlation and direction correlation; selecting guard nodes for path search, constructing a connection graph and searching for paths; optimizing the trajectory of the path; and planning the yaw angle according to the distribution characteristics of the visible surfaces to generate UAV flight control commands.

[0004] Therefore, the UAV navigation method based on planar visibility has the following problems: its planar visibility model is only constructed for static environments, and the planar surface formed by feature point clustering cannot perceive the dynamic target motion characteristics, resulting in the failure of visibility guarantee in target escape scenarios; path search depends on discrete guard nodes, and the generated connection graph is difficult to express continuous motion trajectory, causing frequent jitter in the tracking process; yaw angle planning and trajectory optimization are executed separately, and no collaborative mechanism for motion control and view constraint is established, making it impossible to respond to target pose changes in real time; visibility determination is based on a static coordinate system and lacks dynamic modeling of the relative motion between the target and the UAV, resulting in the inability to achieve occlusion avoidance and target locking synchronously. Summary of the Invention

[0005] To address this, the present invention provides a visibility-based trajectory optimization method, which overcomes the problem in the prior art where static environment modeling and discrete path planning cannot respond to target motion characteristics in real time, thus causing target escape and loss of vision, by using dynamic target motion prediction and multi-constraint collaborative optimization mechanisms.

[0006] To achieve the above objectives, the present invention provides a visibility-based trajectory optimization method, comprising: Step S1: Determine the endpoint of the UAV trajectory based on the midpoint of the line connecting the starting point and the endpoint of the target predicted trajectory. Use the A* algorithm to generate the initial tracking trajectory. Parameterize the initial trajectory using a fourth-order B-spline curve to generate a control point sequence. Step S2: Construct a differentiable cost function model that includes the observation distance cost function, angle maneuver cost function, relative angle cost function, occlusion avoidance cost function, target pointing cost function, time optimization cost function, smoothness cost function, and dynamic feasibility cost function, and optimize the control point sequence to obtain the optimized trajectory; Step S3: Construct an output constraint control system based on the gain function to convert the optimized trajectory into control commands under camera field of view boundary constraints; The gain function is configured such that when the target pixel coordinates approach the field of view boundary, the function value increases non-linearly to enhance the constraint strength and ensure that the target pixel coordinates converge within the image plane boundary range.

[0007] Furthermore, the calculation of the trajectory point positions of the fourth-order B-spline curve in step S1 satisfies:

[0008] , where c i Here, Mi, 4(t) represents the control point, 4(t) represents the fourth-order B-spline basis function, p(t) represents the position of the trajectory point, and c represents the position of the control point. i-3 c i-2 c i-1 All of these are continuous control points in the fourth-order B-spline curve that contribute to the position p(t) of the trajectory point.

[0009] Further, the gain function in step S3 is defined as: ,in For the field of view boundary, Let w1(x) be the pixel coordinate, w1(y) be the gain function in the x-direction, and w1(y) be the gain function in the y-direction.

[0010] Furthermore, the calculation formula for the observation distance cost function in step S2 is: ,in, The optimal observation distance coefficient is determined by the visual sensor. It is the distance offset vector to the target observed by the camera, and N is the number of trajectory points in the trajectory that participate in the calculation of the observation distance cost. ||2 is the L2 norm of the distance offset vector from the target, od min od max These are the lower and upper limits of the acceptable observation distance, respectively, J OD This is the observation distance cost function.

[0011] Furthermore, the formula for calculating the angle maneuver cost function in step S2 is: Where the subscripts x and y indicate direction, σ xk Let σ be the attitude angle value of the k-th trajectory point in the x-direction. xk-1 Let σ be the attitude angle value of the (k-1)th trajectory point in the x-direction. yk Let σ be the attitude angle value of the k-th trajectory point in the y-direction. yk-1 J represents the attitude angle value of the (k-1)th trajectory point in the x-direction. AM Let v be the cost function for angular maneuvering. xk-1 Let v be the velocity component of the UAV's attitude change along the x-direction at the (k-1)th trajectory point. yk-1 Let be the velocity component of the attitude change of the UAV along the y direction at the (k-1)th trajectory point.

[0012] Furthermore, the formula for calculating the relative angle cost function in step S2 is: , where J OS V is the relative angle cost function. xk For the k-th trajectory point, the velocity component of the target in the x-direction, V yk When the target reaches the k-th trajectory point, the velocity component J in the y-direction is... OS Let cosα be the relative angular cost function. HF Let cosα be the cosine of the horizontal field of view. VF p is the cosine of the vertical field of view. max ( ) represents the maximum permissible relative angle.

[0013] Furthermore, the occlusion avoidance cost function in step S2 is calculated as follows: , where l xi It is the lateral distance of the obstacle from the tracking axis, l yi It is the longitudinal distance of the obstacle along the tracking axis, γ(m) yk,i γ(m) is the shortest longitudinal distance between the obstacle and the tracking field of view. xk,i J is the shortest lateral distance between the obstacle and the tracking field of view. Minimizing this ratio keeps the system away from the obstacle. OC The occlusion avoidance cost function is defined by M, where M is the number of trajectory points.

[0014] Furthermore, the dynamic feasibility cost function calculation formula in step S2 is as follows: J DFThis is the dynamic feasibility cost function, where subscript θ represents the pitch angle, subscript θmax represents the maximum pitch angle, subscript Ψ represents the yaw angle, and subscript Ψmax represents the maximum yaw angle. k v represents the linear velocity of the drone at the k-th trajectory point. k,max a is the maximum allowable linear velocity value of the system. k a represents the magnitude of the linear acceleration of the drone at the k-th trajectory point. k,max This is the maximum linear acceleration that the drone can withstand.

[0015] Furthermore, in step S3, the output constraint control system is implemented using a Lyapunov function, which is: ; Its derivative satisfies the convergence condition: When ξ∉Ω, where, >0 is the convergence rate parameter, Ω is the bounded stable region, -γ is the negative definite lower bound of the derivative of the Lyapunov function, and γ is a positive convergence rate parameter used to ensure that when the system state deviates from the stable region Ω, the Lyapunov function decreases at a rate of at least γ.

[0016] Furthermore, the target-oriented cost function calculation formula in step S2 is: , where θ k Let θ be the pitch angle of the UAV at the k-th trajectory point. k，d Let Ψ be the expected pitch angle of the UAV at the k-th trajectory point. k Let Ψ be the yaw angle of the UAV at the k-th trajectory point. k，d Let J be the expected yaw angle of the UAV at the k-th trajectory point. AT The target is the cost function.

[0017] Compared with existing technologies, the advantages of this invention are as follows: First, it determines the UAV tracking endpoint by predicting the midpoint of the target trajectory, generates the initial path using the A* algorithm, and ensures trajectory smoothness using fourth-order B-spline parameterization. Second, it constructs an optimization model that includes eight cost functions, such as observation distance, angle maneuver, relative angle, and occlusion avoidance. The observation distance cost maintains the UAV within the optimal observation range, the angle maneuver cost ensures shooting stability by minimizing attitude angle changes, the relative angle constraint ensures the target does not deviate from the center of the field of view, and the occlusion avoidance cost actively avoids obstacles. Finally, through the nonlinear boundary constraint characteristics of the gain function, the control strength is automatically enhanced when the target approaches the field of view boundary, ensuring the target remains within the effective area of ​​the image plane. In terms of anti-occlusion design, a dynamic safety cone is established in the direction of motion based on the obstacle distance ratio, ensuring that obstacles are always outside the edge of the field of view. The observation distance constraint provides an initial feasible region for angle optimization. The angle optimization result is fed back to adjust the distance maintenance threshold, while the occlusion avoidance mechanism corrects the parameters of both in real time. Finally, through the boundary enhancement characteristics of the gain function, the multi-target optimization result is transformed into a strict field of view boundary constraint, achieving a synergistic improvement in tracking stability and trajectory feasibility. This effectively solves the problem of target escape and loss of field of view caused by the inability to respond to target motion characteristics in real time due to static environment modeling and discrete path planning.

[0018] Furthermore, by constructing the trajectory using a combination of control points and basis functions of a specific order, both trajectory smoothness and precise control of flight attitude are ensured. The spacing between control points can be dynamically adjusted according to environmental complexity, automatically increasing density in complex areas and sparsely distributing them in simpler areas. The special mathematical properties of the basis functions ensure that the generated trajectory always remains within the safe range established by the control points, avoiding the risk of sudden boundary breaches. This parameterized method works closely with subsequent optimization modules; changes in the position of the control points affect key parameters such as flight speed and acceleration in real time through the mathematical relationships of the basis functions. The final generated trajectory maintains smooth flight while strictly meeting physical limitations such as the aircraft's speed limit and maximum tilt angle, significantly improving computational efficiency and enabling rapid response to environmental changes while maintaining stable flight quality.

[0019] Furthermore, a designed gain function enables precise target locking control of the UAV. As the target approaches the field of view boundary, the function value exhibits a non-linear, rapid increase, creating a strong "virtual potential field" effect near the boundary. This allows the control system's response intensity to automatically adjust according to the target's deviation. The dynamic interaction between the field of view boundary parameters and real-time pixel coordinates constructs an adaptive constraint mechanism—maintaining gentle control in the central region to avoid jitter, and strengthening the constraint in the boundary region to prevent target loss. This design ensures the target is always stably controlled within the effective area of ​​the image plane, while avoiding the abrupt changes in control commands caused by traditional hard boundary truncation. By adjusting the boundary parameter values, it can flexibly adapt to camera equipment with different focal lengths, reducing the target loss rate to less than 1 / 5 of traditional methods while ensuring system stability, achieving the optimal balance between control accuracy and system robustness.

[0020] Furthermore, by optimizing the observation distance cost function, the UAV maintains the optimal observation position for the target. Its technical advantages are reflected in the dynamic coordination between the optimal observation distance coefficient and the real-time offset vector, constructing an adaptive distance adjustment mechanism. When the UAV approaches the minimum observation distance, the cost function generates a strong repulsive effect, preventing image distortion caused by the target being too close; when approaching the maximum observation distance, it generates an attractive effect, preventing the target from leaving the effective recognition range. The directional sensitivity of the offset vector ensures that the UAV always maintains an advantageous observation position for the target, while the dynamic weighting characteristics of the distance coefficient automatically adjust the optimal observation distance according to the target size. This allows the UAV to maintain good target imaging clarity even during complex maneuvers, while controlling the target's escape probability, forming a triple adaptive adjustment system of distance-azimuth-target characteristics.

[0021] Furthermore, an innovative design of the angle maneuver cost function enables intelligent optimization control of the UAV's flight attitude. This function establishes a dual-axis linkage attitude stability control mechanism by separately evaluating angular changes in the yaw and pitch directions. When the UAV's attitude changes rapidly, the cost function generates a corresponding suppression effect, effectively smoothing the flight trajectory; while when the attitude adjustment is gradual, it maintains a low constraint force to ensure tracking response speed. This design allows the UAV to coordinate horizontal turning and vertical pitch movements, avoiding severe camera viewpoint shake while maintaining good target tracking capability. By dynamically adjusting the control intensity of the two axes, the system can flexibly respond to changes in target motion while ensuring stable captured images, significantly improving overall tracking performance and achieving the best balance between flight stability and maneuver responsiveness.

[0022] Furthermore, an innovative design of the relative angle cost function enables the UAV to intelligently predict and optimize the target's motion direction. This function constructs a proactive heading adjustment mechanism by calculating the angle between the target's motion vector and the UAV's field of view center axis in real time. When the target's motion direction deviates from the center of the field of view, the cost function value increases non-linearly with the deviation angle, driving the UAV to adjust its heading in advance and form an active tracking posture. This design ensures that the UAV always keeps the target's motion trend within the safe range of the field of view (the angle between the target's motion vector and the center axis is always less than a threshold), effectively avoiding the field of view edge loss problem caused by traditional passive tracking. Through dynamic feedback of the target's motion vector, the system can intelligently balance tracking response speed and flight stability, maintaining a smooth view transition even when the target suddenly turns, significantly improving tracking robustness in complex maneuvering scenarios.

[0023] Furthermore, an innovative design of the occlusion avoidance cost function enables intelligent obstacle avoidance and field-of-view maintenance for UAVs in complex environments. This function constructs a safe flight corridor in three-dimensional space by establishing a dynamic proportional relationship between obstacle distance and field of view. When an obstacle approaches the tracking axis, the inverse effect of the distance ratio significantly increases the cost value, driving the UAV to automatically adjust its course. When the obstacle is located at the edge of the field of view, it produces a moderate adjustment effect, achieving a balance between obstacle avoidance and target tracking. This allows the UAV to intelligently select the optimal avoidance path based on the spatial distribution characteristics of obstacles—maintaining sufficient obstacle spacing to ensure flight safety while maximizing the effective imaging area of ​​the target within the field of view. Through real-time interactive calculation of obstacle distance parameters, an adaptive spatial perception and path planning mechanism is formed, maintaining continuous target locking capability even in dense obstacle environments, while avoiding the field-of-view loss problem common in traditional obstacle avoidance algorithms.

[0024] Furthermore, the innovative design of a dynamic feasibility cost function enables dynamic optimization control of the UAV's flight trajectory. This function constructs a multi-dimensional motion constraint evaluation system by comprehensively considering the sum of the squares of angular velocity, angular acceleration, and linear acceleration. When the UAV's motion state approaches its dynamic limits, the cost function generates a gradient-increasing suppression effect, guiding the trajectory towards the feasible region; while within the safe range, it maintains a low constraint strength, ensuring the flexibility of trajectory planning and ensuring that the generated trajectory strictly follows the physical performance boundaries of the aircraft. Simultaneously, through the coupling effect of various motion parameters, a dynamic balance is established between the rate of attitude change and displacement acceleration—automatically smoothing angular velocity changes during high-speed maneuvers and optimizing acceleration distribution during straight-line flight. Through the synergistic optimization of the three sets of motion parameters, the optimal match between flight stability and maneuverability is achieved, avoiding control instability caused by overshoot and fully utilizing the platform's dynamic potential, significantly improving trajectory tracking accuracy under complex maneuvers.

[0025] Furthermore, an adaptive boundary protection mechanism was established by constructing the energy function and setting the convergence conditions. When the system state deviates from the stable region, the negative definite derivative characteristic produces an asymptotic convergence effect, driving the state variables to return to the safe range at a controllable rate; while when approaching the boundary, the nonlinear gain effect automatically strengthens, forming a flexible constraint barrier. This design enables the UAV to intelligently adjust the control intensity during target tracking—maintaining smooth following in the central region of the field of view and strengthening the stabilizing effect near the boundary, avoiding the abrupt changes in control commands caused by traditional hard truncation, and ensuring that the target is always within the effective observation range. By adjusting the convergence rate parameter, the response speed and stability can be flexibly balanced, achieving a seamless transition from rapid tracking to precise maintenance. The dynamic interaction between the state variables and the stable region forms a closed-loop control system with anti-interference capabilities, significantly improving the tracking reliability in complex environments.

[0026] Furthermore, an innovative design of the target pointing cost function enables the UAV to maintain the optimal observation angle for the target. This function constructs an adaptive view adjustment mechanism by comparing the deviation between the current orientation angle and the desired pointing angle in real time. When the UAV deviates from the ideal observation angle, a square relationship generates a gradient-increasing correction force to ensure that the aircraft always faces the optimal observation orientation of the target; while approaching the desired angle, it switches to a fine-tuning mode to avoid view jitter caused by over-correction, enabling the camera's line of sight to intelligently track target features—providing sufficient steering torque when the target is moving rapidly, and maintaining smooth view fine-tuning during the stable tracking phase. Through the dynamic setting of the desired angle, the optimal observation angle can be automatically selected according to the target type (e.g., a top-down angle for vehicle targets, and a level view for personnel targets), while controlling the difference between the current angle and the desired angle within the effective field of view. Attached Figure Description

[0027] Figure 1 This is a flowchart of the visibility-based trajectory optimization method in this embodiment; Figure 2 This is a flowchart illustrating the target tracking based on visibility in this embodiment. Detailed Implementation

[0028] To make the objectives and advantages of the present invention clearer, the present invention will be further described below with reference to embodiments; it should be understood that the specific embodiments described herein are merely for explaining the present invention and are not intended to limit the present invention.

[0029] Preferred embodiments of the present invention will now be described with reference to the accompanying drawings. Those skilled in the art should understand that these embodiments are merely illustrative of the technical principles of the present invention and are not intended to limit the scope of protection of the present invention.

[0030] Please see Figure 1As shown, it is a flowchart of the visibility-based trajectory optimization method in this embodiment; This embodiment provides a visibility-based trajectory optimization method, including: Step S1: Determine the endpoint of the UAV trajectory based on the midpoint of the line connecting the starting point and the endpoint of the target predicted trajectory. Use the A* algorithm to generate the initial tracking trajectory. Parameterize the initial trajectory using a fourth-order B-spline curve to generate a control point sequence. Step S2: Construct a differentiable cost function model that includes the observation distance cost function, angle maneuver cost function, relative angle cost function, occlusion avoidance cost function, target pointing cost function, time optimization cost function, smoothness cost function, and dynamic feasibility cost function, and optimize the control point sequence to obtain the optimized trajectory; Step S3: Construct an output constraint control system based on the gain function to convert the optimized trajectory into control commands under camera field of view boundary constraints; The gain function is configured such that when the target pixel coordinates approach the field of view boundary, the function value increases non-linearly to enhance the constraint strength and ensure that the target pixel coordinates converge within the image plane boundary range.

[0031] Please continue reading. Figure 2 As shown, this is a flowchart of the target tracking based on visibility in this embodiment; Target tracking based on visibility is achieved by first generating the initial tracking trajectory of the UAV using the A* algorithm, considering topological equivalence. Given the need for the aircraft to maintain an appropriate distance from the target, the midpoint of the line connecting the start and end points of the predicted target trajectory is selected as the endpoint of the initial tracking trajectory. B-splines are then used to parameterize the trajectory and generate a sequence of control points. The visibility requirements are mainly threefold: 1) Observation distance (DO): It is necessary to maintain the observation range and prevent the target from escaping laterally; 2) Observation angle (AO): It is necessary to keep pointing at the target and reduce the angle of maneuvering; 3) Obstacle occlusion (OE): It is necessary to avoid obstruction of the field of view and avoid system collisions.

[0032] The UAV tracking endpoint is determined by identifying the midpoint of the predicted target trajectory. An initial path is generated using the A* algorithm, and fourth-order B-spline parameterization ensures trajectory smoothness. Secondly, an optimization model is constructed, incorporating eight cost functions: observation distance, angular maneuvering, relative angle, and occlusion avoidance. The observation distance cost maintains the UAV within its optimal observation range; the angular maneuvering cost ensures shooting stability by minimizing attitude angle changes; the relative angle constraint ensures the target does not deviate from the center of the field of view; and the occlusion avoidance cost actively avoids obstacles. Finally, through the nonlinear boundary constraint characteristics of the gain function, the control strength is automatically increased when the target approaches the field of view boundary, ensuring the target remains within the effective area of ​​the image plane. In terms of anti-occlusion design, a dynamic safety cone is established in the direction of motion based on the obstacle distance ratio, ensuring that obstacles are always outside the edge of the field of view. The observation distance constraint provides an initial feasible region for angle optimization. The angle optimization result is fed back to adjust the distance maintenance threshold, while the occlusion avoidance mechanism corrects the parameters of both in real time. Finally, through the boundary enhancement characteristics of the gain function, the multi-target optimization result is transformed into a strict field of view boundary constraint, achieving a synergistic improvement in tracking stability and trajectory feasibility. This effectively solves the problem of target escape and loss of field of view caused by the inability to respond to target motion characteristics in real time due to static environment modeling and discrete path planning.

[0033] Specifically, the calculation of the trajectory point positions of the fourth-order B-spline curve in step S1 satisfies:

[0034] , where c i Here, Mi, 4(t) represents the control point, 4(t) represents the fourth-order B-spline basis function, p(t) represents the position of the trajectory point, and c represents the position of the control point. i-3 c i-2 c i-1 All of these are continuous control points in the fourth-order B-spline curve that contribute to the position p(t) of the trajectory point.

[0035] By constructing a trajectory using a combination of control points and basis functions of a specific order, both trajectory smoothness and precise control of flight attitude are ensured. The spacing between control points can be dynamically adjusted according to environmental complexity, automatically increasing density in complex areas and sparsely distributing them in simpler areas. The special mathematical properties of the basis functions ensure that the generated trajectory always remains within the safe range established by the control points, avoiding the risk of sudden boundary breaches. This parameterized method works closely with subsequent optimization modules; changes in the position of the control points affect key parameters such as flight speed and acceleration in real time through the mathematical relationships of the basis functions. The final generated trajectory maintains smooth flight while strictly meeting physical limitations such as the aircraft's speed limit and maximum tilt angle, significantly improving computational efficiency and enabling rapid response to environmental changes while maintaining stable flight quality.

[0036] Specifically, the gain function in step S3 is defined as: ,in For the field of view boundary, Let w1(x) be the pixel coordinate, w1(y) be the gain function in the x-direction, and w1(y) be the gain function in the y-direction.

[0037] A designed gain function enables precise target locking control of the UAV. As the target approaches the field of view boundary, the function value exhibits a non-linear, rapid increase, creating a strong "virtual potential field" effect near the boundary. This allows the control system's response intensity to automatically adjust according to the target's deviation. The dynamic interaction between the field of view boundary parameters and real-time pixel coordinates constructs an adaptive constraint mechanism—maintaining gentle control in the central region to avoid jitter, and strengthening the constraint in the boundary region to prevent target loss. This design ensures the target is always stably controlled within the effective area of ​​the image plane, while avoiding the abrupt changes in control commands caused by traditional hard boundary truncation. By adjusting the boundary parameter values, it can flexibly adapt to camera equipment with different focal lengths. While ensuring system stability, it reduces the target loss rate to less than 1 / 5 of traditional methods, achieving the optimal balance between control precision and system robustness.

[0038] Specifically, the calculation formula for the observation distance cost function in step S2 is: ,in, The optimal observation distance coefficient is determined by the visual sensor. It is the distance offset vector to the target observed by the camera, and N is the number of trajectory points in the trajectory that participate in the calculation of the observation distance cost. ||2 is the L2 norm of the distance offset vector from the target, od min od max These are the lower and upper limits of the acceptable observation distance, respectively, and JOD is the observation distance cost function.

[0039] By optimizing the observation distance cost function, the UAV maintains the optimal observation position for the target. Its technical advantages are reflected in the dynamic coordination between the optimal observation distance coefficient and the real-time offset vector, constructing an adaptive distance adjustment mechanism. When the UAV approaches the minimum observation distance, the cost function generates a strong repulsive effect, preventing image distortion caused by the target being too close; when approaching the maximum observation distance, it generates an attractive effect, preventing the target from leaving the effective recognition range. The directional sensitivity of the offset vector ensures that the UAV always maintains an advantageous observation position for the target (lateral offset less than 30% of the field of view), while the dynamic weighting characteristics of the distance coefficient automatically adjust the optimal observation distance according to the target size (increasing the distance by 20% for large targets and decreasing the distance by 15% for small targets). This allows the UAV to maintain good target imaging clarity during complex maneuvers while controlling the target's escape probability, forming a triple adaptive adjustment system of distance-azimuth-target characteristics.

[0040] Specifically, the formula for calculating the angle maneuver cost function in step S2 is: Where the subscripts x and y indicate direction, σ xk Let σ be the attitude angle value of the k-th trajectory point in the x-direction. xk-1 Let σ be the attitude angle value of the (k-1)th trajectory point in the x-direction. yk Let σ be the attitude angle value of the k-th trajectory point in the y-direction. yk-1 J represents the attitude angle value of the (k-1)th trajectory point in the x-direction. AM Let v be the cost function for angular maneuvering. xk-1 Let v be the velocity component of the UAV's attitude change along the x-direction at the (k-1)th trajectory point. yk-1 Let be the velocity component of the attitude change of the UAV along the y direction at the (k-1)th trajectory point.

[0041] Intelligent optimization control of UAV flight attitude was achieved through an innovative design of an angular maneuver cost function. This function establishes a dual-axis linkage attitude stability control mechanism by separately evaluating angular changes in the yaw and pitch directions. When the UAV's attitude changes rapidly, the cost function generates a corresponding suppression effect, effectively smoothing the flight trajectory; while when the attitude adjustment is gradual, it maintains a low constraint force to ensure tracking response speed. This design enables the UAV to coordinate horizontal turning and vertical pitch movements, avoiding severe camera viewpoint shake while maintaining good target tracking capability. By dynamically adjusting the control intensity of the two axes, the system can flexibly respond to changes in target motion while ensuring stable captured images, significantly improving overall tracking performance and achieving the best balance between flight stability and maneuver responsiveness.

[0042] Specifically, the formula for calculating the relative angle cost function in step S2 is: , where J OS V is the relative angle cost function. xk For the k-th trajectory point, the velocity component of the target in the x-direction, V yk When the target reaches the k-th trajectory point, the velocity component J in the y-direction is... OS Let cosα be the relative angular cost function. HF Let cosα be the cosine of the horizontal field of view. VF p is the cosine of the vertical field of view. max ( ) represents the maximum permissible relative angle.

[0043] An innovative design of the relative angle cost function enables the UAV to intelligently predict and optimize the target's motion direction. This function constructs a proactive heading adjustment mechanism by calculating the angle between the target's motion vector and the UAV's field-of-view center axis in real time. When the target's motion direction deviates from the center of the field of view, the cost function value increases non-linearly with the deviation angle, driving the UAV to adjust its heading in advance and form an active tracking posture. This design ensures that the UAV always keeps the target's motion trend within the safe range of the field of view (the angle between the target's motion vector and the center axis is always less than a threshold), effectively avoiding the field-of-view edge loss problem caused by traditional passive tracking. Through dynamic feedback of the target's motion vector, the system can intelligently balance tracking response speed and flight stability, maintaining a smooth view transition even when the target suddenly turns, significantly improving tracking robustness in complex maneuvering scenarios.

[0044] Specifically, the occlusion avoidance cost function calculation formula in step S2 is: , where l xi It is the lateral distance of the obstacle from the tracking axis, l yi It is the longitudinal distance of the obstacle along the tracking axis, γ(m) yk,i γ(m) is the shortest longitudinal distance between the obstacle and the tracking field of view. xk,i J is the shortest lateral distance between the obstacle and the tracking field of view. Minimizing this ratio keeps the system away from the obstacle. OC The occlusion avoidance cost function is defined by M, where M is the number of trajectory points.

[0045] An innovative design of an occlusion-avoidance cost function enables intelligent obstacle avoidance and field-of-view maintenance for UAVs in complex environments. This function constructs a safe flight corridor in three-dimensional space by establishing a dynamic proportional relationship between obstacle distance and field of view. When an obstacle approaches the tracking axis, the inverse effect of the distance ratio significantly increases the cost value, driving the UAV to automatically adjust its course. When the obstacle is located at the edge of the field of view, it produces a moderate adjustment effect, achieving a balance between obstacle avoidance and target tracking. This allows the UAV to intelligently select the optimal avoidance path based on the spatial distribution characteristics of obstacles—maintaining sufficient obstacle spacing to ensure flight safety while maximizing the effective imaging area of ​​the target within the field of view. Through real-time interactive calculation of obstacle distance parameters, an adaptive spatial perception and path planning mechanism is formed, maintaining continuous target locking capability even in dense obstacle environments, while avoiding the field-of-view loss problem common in traditional obstacle avoidance algorithms.

[0046] Specifically, the dynamic feasibility cost function calculation formula in step S2 is: J DF This is the dynamic feasibility cost function, where subscript θ represents the pitch angle, subscript θmax represents the maximum pitch angle, subscript Ψ represents the yaw angle, and subscript Ψmax represents the maximum yaw angle. k v represents the linear velocity of the drone at the k-th trajectory point. k,max a is the maximum allowable linear velocity value of the system. k a represents the magnitude of the linear acceleration of the drone at the k-th trajectory point. k,max This is the maximum linear acceleration that the drone can withstand.

[0047] The innovative design of a dynamic feasibility cost function enables dynamic optimization control of UAV flight trajectories. This function constructs a multi-dimensional motion constraint evaluation system by comprehensively considering the sum of the squares of angular velocity, angular acceleration, and linear acceleration. When the UAV's motion state approaches its dynamic limits, the cost function generates a gradient-increasing suppression effect, guiding the trajectory towards the feasible region; while within the safe range, it maintains a low constraint strength, ensuring the flexibility of trajectory planning and ensuring that the generated trajectory strictly adheres to the physical performance boundaries of the aircraft. Simultaneously, through the coupling effect of various motion parameters, a dynamic balance is established between the rate of attitude change and displacement acceleration—automatically smoothing angular velocity changes during high-speed maneuvers and optimizing acceleration distribution during straight-line flight. Through the synergistic optimization of the three sets of motion parameters, the optimal match between flight stability and maneuverability is achieved, avoiding control instability caused by overshoot while fully utilizing the platform's dynamic potential and significantly improving trajectory tracking accuracy under complex maneuvers.

[0048] Specifically, in step S3, the output constraint control system is implemented using a Lyapunov function, which is: ; Its derivative satisfies the convergence condition: When ξ∉Ω, where, >0 is the convergence rate parameter, Ω is the bounded stable region, -γ is the negative definite lower bound of the derivative of the Lyapunov function, and γ is a positive convergence rate parameter used to ensure that when the system state deviates from the stable region Ω, the Lyapunov function decreases at a rate of at least γ.

[0049] By constructing the energy function and setting the convergence conditions, an adaptive boundary protection mechanism was established. When the system state deviates from the stable region, the negative definite derivative characteristic produces an asymptotic convergence effect, driving the state variables to return to the safe range at a controllable rate. Conversely, as the system approaches the boundary, the nonlinear gain effect automatically strengthens, forming a flexible constraint barrier. This design enables the UAV to intelligently adjust the control intensity during target tracking—maintaining smooth following in the center of the field of view and strengthening stabilization near the boundary. This avoids abrupt changes in control commands caused by traditional hard truncation and ensures that the target remains within the effective observation range. By adjusting the convergence rate parameter, response speed and stability can be flexibly balanced, achieving a seamless transition from rapid tracking to precise maintenance. The dynamic interaction between the state variables and the stable region forms a closed-loop control system with anti-interference capabilities, significantly improving tracking reliability in complex environments.

[0050] Specifically, the target-oriented cost function calculation formula in step S2 is: , where θ k Let θ be the pitch angle of the UAV at the k-th trajectory point. k，d Let Ψ be the expected pitch angle of the UAV at the k-th trajectory point. k Let Ψ be the yaw angle of the UAV at the k-th trajectory point. k，d Let J be the expected yaw angle of the UAV at the k-th trajectory point. AT The target is the cost function.

[0051] An innovative design of the target pointing cost function enables the UAV to maintain the optimal observation angle for the target. This function constructs an adaptive view adjustment mechanism by comparing the deviation between the current orientation angle and the desired pointing angle in real time. When the UAV deviates from the ideal observation angle, a square relationship generates a gradient-increasing correction force to ensure that the aircraft always faces the target's optimal observation orientation; while approaching the desired angle, it switches to a fine-tuning mode to avoid view jitter caused by over-correction, enabling the camera's line of sight to intelligently track target features—providing sufficient steering torque when the target is moving rapidly, and maintaining smooth view fine-tuning during stable tracking. Through the dynamic setting of the desired angle, the optimal observation angle can be automatically selected according to the target type (e.g., a top-down angle for vehicle targets, and a level view for personnel targets), while controlling the difference between the current angle and the desired angle within the effective field of view.

[0052] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A trajectory optimization method based on visibility, characterized in that, include: Step S1: Determine the endpoint of the UAV trajectory based on the midpoint of the line connecting the starting point and the endpoint of the target predicted trajectory. Use the A* algorithm to generate the initial tracking trajectory. Parameterize the initial trajectory using a fourth-order B-spline curve to generate a control point sequence. Step S2: Construct a differentiable cost function model that includes the observation distance cost function, angle maneuver cost function, relative angle cost function, occlusion avoidance cost function, target pointing cost function, time optimization cost function, smoothness cost function, and dynamic feasibility cost function, and optimize the control point sequence to obtain the optimized trajectory; Step S3: Construct an output constraint control system based on the gain function to convert the optimized trajectory into control commands under camera field of view boundary constraints; The gain function is configured such that when the target pixel coordinates approach the field of view boundary, the function value increases non-linearly to enhance the constraint strength and ensure that the target pixel coordinates converge within the image plane boundary range.

2. The visibility-based trajectory optimization method according to claim 1, characterized in that, The calculation of the trajectory point positions of the fourth-order B-spline curve in step S1 satisfies: ， Among them, c i It is a control point, M i，4 (t) is the fourth-order B-spline basis function, p(t) is the position of the trajectory point, and c i-3 c i-2 c i-1 All of these are continuous control points in the fourth-order B-spline curve that contribute to the position p(t) of the trajectory point.

3. The visibility-based trajectory optimization method according to claim 2, characterized in that, The gain function in step S3 is defined as follows: ,in For the field of view boundary, Let w1(x) be the pixel coordinate, w1(y) be the gain function in the x-direction, and w1(y) be the gain function in the y-direction.

4. The visibility-based trajectory optimization method according to claim 3, characterized in that, The calculation formula for the observation distance cost function in step S2 is: ,in, The optimal observation distance coefficient is determined by the visual sensor. It is the distance offset vector to the target observed by the camera, and N is the number of trajectory points in the trajectory that participate in the calculation of the observation distance cost. ||2 is the L2 norm of the distance offset vector from the target, od min od max These are the lower and upper limits of the acceptable observation distance, respectively, J OD This is the observation distance cost function.

5. The visibility-based trajectory optimization method according to claim 4, characterized in that, The formula for calculating the angle maneuver cost function in step S2 is: Where the subscripts x and y indicate direction, σ xk Let σ be the attitude angle value of the k-th trajectory point in the x-direction. xk-1 Let σ be the attitude angle value of the (k-1)th trajectory point in the x-direction. yk Let σy be the attitude angle value of the k-th trajectory point in the y-direction. k-1 J represents the attitude angle value of the (k-1)th trajectory point in the x-direction. AM Let v be the cost function for angular maneuvering. xk-1 Let v be the velocity component of the UAV's attitude change along the x-direction at the (k-1)th trajectory point. yk-1 Let be the velocity component of the attitude change of the UAV along the y direction at the (k-1)th trajectory point.

6. The visibility-based trajectory optimization method according to claim 5, characterized in that, The formula for calculating the relative angle cost function in step S2 is: , where J OS Let v be the relative angular cost function. xk For the k-th trajectory point, the velocity component of the target in the x-direction, v yk For the k-th trajectory point, the velocity component of the target in the y-direction, J OS Let cosα be the relative angular cost function. HF Let cosα be the cosine of the horizontal field of view. VF p is the cosine of the vertical field of view. max ( ) represents the maximum permissible relative angle.

7. The visibility-based trajectory optimization method according to claim 6, characterized in that, The occlusion avoidance cost function in step S2 is calculated as follows: , where l xi It is the lateral distance of the obstacle from the tracking axis, l yi It is the longitudinal distance of the obstacle along the tracking axis, γ(m) yk,i γ(m) is the shortest longitudinal distance between the obstacle and the tracking field of view. xk,i J is the shortest lateral distance between the obstacle and the tracking field of view. Minimizing this ratio keeps the system away from the obstacle. OC The occlusion avoidance cost function is defined by M, where M is the number of trajectory points.

8. The visibility-based trajectory optimization method according to claim 7, characterized in that, The dynamic feasibility cost function calculation formula in step S2 is: ,in, and J represents angular velocity, angular acceleration, and acceleration, respectively. DF This is the dynamic feasibility cost function, where subscript θ represents the pitch angle, subscript θmax represents the maximum pitch angle, subscript Ψ represents the yaw angle, and subscript Ψmax represents the maximum yaw angle. k v represents the linear velocity of the drone at the k-th trajectory point. k,max a is the maximum allowable linear velocity value of the system. k a represents the magnitude of the linear acceleration of the drone at the k-th trajectory point. k,max This is the maximum linear acceleration that the drone can withstand.

9. The visibility-based trajectory optimization method according to claim 8, characterized in that, In step S3, the output constraint control system is implemented using a Lyapunov function, which is: ； Its derivative satisfies the convergence condition: ≤-γ, when ξ∉Ω, where, Here, Ω represents the bounded stable region, -γ is the negative definite lower bound of the derivative of the Lyapunov function, and γ is a positive convergence rate parameter used to ensure that when the system state deviates from the stable region Ω, the Lyapunov function decreases at a rate of at least γ.

10. The visibility-based trajectory optimization method according to claim 9, characterized in that, The target-oriented cost function in step S2 is calculated as follows: , where θ k Let θ be the pitch angle of the UAV at the k-th trajectory point. k，d Let Ψ be the expected pitch angle of the UAV at the k-th trajectory point. k Let Ψ be the yaw angle of the UAV at the k-th trajectory point. k，d Let J be the expected yaw angle of the UAV at the k-th trajectory point. AT The target is the cost function.

Images