Time optimal trajectory planning method for intercepting moving target by unmanned aerial vehicle

Through time node pre-allocation and two-stage optimization methods, combined with inner point method and polynomial interpolation, the problem of large calculation load in the time optimal trajectory planning of the drone intercepting motion target is solved, and efficient and accurate interception trajectory planning is achieved, improving the Intercept efficiency and robustness.

CN118938980APending Publication Date: 2024-11-12ROBOTICS RESEARCH CENTER OF YUYAO CITY +1
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202410977805.5
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-22
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

In the planning of the time optimal trajectory of the drone intercepting motion targets, the large calculation load makes it difficult to balance the interception time optimization and planning efficiency, and it is difficult to achieve efficient and accurate interception.

Method used

By using time node pre-allocation and two-stage optimization methods, the trajectory optimization preheating model with full dynamics and minimized control quantity is constructed, combined with the inner point method to solve, the time-optimal interception trajectory is optimized, and the third-order polynomial interpolation is performed. Continuity, ultimately construct a predictive control model to realize motion control of the drone.

Benefits of technology

It improves the efficiency and accuracy of drone interception, suppresses the local optimal solution of nonlinear optimization problems, realizes the interception trajectory with optimal time planning in real time, and improves the robustness of trajectory optimization.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938980A_ABST
    Figure CN118938980A_ABST
Patent Text Reader

Abstract

The invention discloses a time optimal trajectory planning method for intercepting a moving target by an unmanned aerial vehicle. The time optimal trajectory planning method comprises the steps of unmanned aerial vehicle self-positioning, target detection positioning, target trajectory prediction, unmanned aerial vehicle full-dynamics trajectory optimization preheating solution, final trajectory optimization and nonlinear model prediction control. In rear-end trajectory optimization, a constrained optimization problem including unmanned aerial vehicle full dynamics is constructed, a numerical optimization method is used for pre-solving an optimization problem close to a time-optimal interception trajectory, and finally, the time-optimal interception trajectory is solved by taking a pre-solution as an initial value. And the unmanned aerial vehicle control quantity is obtained through a nonlinear model prediction control method and is input into an unmanned aerial vehicle controller, and time optimal interception of the moving target is completed. The target motion trail can be predicted, the target interception time and speed of the unmanned aerial vehicle are optimized, interception is more efficient, the application expansibility is high, and therefore the problems that an existing interception method is low in interception speed and low in success rate or depends on an indoor motion capture system are solved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to a time-optimal trajectory planning method for a unmanned aerial vehicle to intercept a moving target. Background Art

[0002] At present, drone interception technology has a wide range of applications in the field of security. With the popularization of drone technology, effective interception technology is needed to ensure safety. Due to the small size and high flexibility of fully autonomous drones, a proposed solution is to use drones with onboard sensors for effective interception. During the interception process, the drone first needs to determine its own position and the target position through onboard sensors, and predict the movement of the target based on this, and plan the optimal interception trajectory, including the time and location of the interception. At the same time, in order to minimize the risk, it is often hoped that the interception can be completed as quickly as possible. In general, an efficient and robust time-optimal drone interception trajectory planning method plays a decisive role in drone interception of moving targets, which can help drones track targets and accurately intercept in the shortest time, thereby improving the efficiency and success rate of interception. However, the current technical solution will bring huge computational load due to the time node allocation problem generated by the time-optimal optimization problem, which makes it difficult to balance the optimality of interception time and planning efficiency. Summary of the invention

[0003] In order to solve the shortcomings of the prior art, the present invention adopts the following technical solutions to achieve efficient and accurate optimization of the interception trajectory with the best time through time node pre-allocation and two-stage optimization, so as to achieve the purpose of faster and more accurate interception of the target:

[0004] A time-optimal trajectory planning method for a UAV to intercept a moving target comprises the following steps:

[0005] Step S1: Obtain the drone's own status information;

[0006] Step S2: Obtain the motion state information of the target to be intercepted within a period of time and predict its motion trajectory;

[0007] Step S3: Based on the UAV's own state information, execution constraints, state constraints, and the predicted motion trajectory of the target to be intercepted, a trajectory optimization preheating model of the UAV's full dynamics of intercepting the target and minimizing the control amount is constructed to obtain the trajectory optimized UAV state and input trajectory, actuator constraints, state constraints, predicted target motion trajectory, and the initially selected discrete time step;

[0008] Step S4: Based on the obtained trajectory-optimized UAV state and input trajectory, actuator constraints, state constraints, predicted target motion trajectory, and the initially selected discrete time step, a time-optimal trajectory optimization model that minimizes the interception time is constructed to obtain a discretized time-optimal reference trajectory;

[0009] Step S5: Continuate the discrete time optimal reference trajectory by using a third-order polynomial interpolation to obtain a continuous reference trajectory with optimal time.

[0010] Step S6: Based on the drone's own state information and the time-optimal continuous reference trajectory, a predictive control model for minimizing the error of the drone to the reference trajectory is constructed to obtain the drone state and control input for execution by the drone's motion controller.

[0011] Furthermore, in step S1, the UAV performs VINS-Mono monocular visual inertial state estimation based on the collected image data, its own acceleration and angular velocity data to obtain the UAV's position and posture information.

[0012] Furthermore, in step S2, image data of the target to be intercepted is obtained, a bounding box of the target to be intercepted is obtained through YOLO-v4-tiny target detection, the bounding box is input into a pre-trained key point extraction network YOLO-v8 to obtain a set of key points, and the PNP problem is solved by an EPNP algorithm with a RANSCA random sampling consensus method according to the three-dimensional position coordinates of the key points relative to the target body coordinate system, and the motion state information of the target to be intercepted is calculated;

[0013] The input of the target motion trajectory prediction is the target's motion state information sequence in the previous period of time. The predicted target motion trajectory is output through inference using a pre-trained long short-term memory (LSTM) network.

[0014] Furthermore, in step S3, the trajectory optimization preheating problem established by the trajectory optimization preheating model is as follows:

[0015]

[0016] stx lb ≤x k ≤x ub (2)

[0017] u lb ≤u k ≤u ub (3)

[0018] x0=x init (4)

[0019] Among them, λ1, λ2, and λ3 represent three weight coefficients, and R represents the weight of different control inputs of the UAV, which is used to weigh the importance of the three. T represents the interception position of the drone, T=Ndt0 represents the interception time, N represents the total number of segments into which the entire time optimal interception trajectory is divided, dt0 represents the time interval between each small segment of the trajectory, Indicates the predicted location of the target at time T, x k represents the state variable of the kth discrete node drone, u k represents the input of the kth discrete node drone, represents the square of the L2 norm, f(·) represents the full kinematic equation of the drone, x lb 、x ub They represent the lower and upper bounds of the drone state, u lb 、u ub represents the lower and upper bounds of the drone input, x init represents the initial state variable of the UAV; Formula (1) represents the interception target of the UAV, and the motion trajectory satisfies the penalty function sum of the full dynamics of the UAV and the minimized control amount, aiming to appropriately relax the final nonlinear optimization problem and obtain a good quality initial solution; Formula (2) represents the UAV state constraint, which means that the position, speed, angular velocity, and attitude cannot be greater than the maximum value set by the user; Formula (3) represents the actuator constraint, which means that the thrust cannot be greater than the maximum value set by the user; Formula (4) represents the initial state;

[0020] The solution of trajectory optimization warm-up problem is solved using interior point method.

[0021] Furthermore, the x k =[p k v k q k ω k ] T ,k=0,1,…,N, where p k 、v k ,q k ,ω k They represent the position, velocity, angular velocity, and attitude of the kth discrete point UAV respectively.

[0022] Furthermore, the solution to the trajectory optimization warm-up problem is to use the evenly distributed points of the line connecting the initial position to the position of the target at time T predicted in step S2 and zero speed, zero Euler angle, zero angular velocity, and zero actuator input as the initial values ​​of the optimization, and to obtain the trajectory optimized drone state and input trajectory, actuator constraints, state constraints, predicted target motion trajectory, and the initially selected discrete time step.

[0023] Furthermore, in step S4, the time optimal trajectory optimization problem established by the time optimal trajectory optimization model is as follows:

[0024]

[0025] x lb ≤x k ≤x ub (8)

[0026] u lb ≤u k ≤u ub (9)

[0027] x0=x init (10)

[0028] Wherein, T = Ndt represents the interception time, δ represents the allowable error to ensure that the target is successfully intercepted; Formula (5) represents minimizing the interception time to obtain the target interception trajectory that satisfies the time optimization; Formula (6) represents the final target position constraint to ensure that the target is successfully intercepted; Formula (7) represents the nonlinear full dynamic constraint of the UAV; Formula (8) represents the state constraint of the UAV, which means that the position, velocity, angular velocity, and attitude cannot be greater than the maximum value set by the user; Formula (9) represents the actuator constraint, which means that the thrust cannot be greater than the maximum value set by the user; Formula (10) represents the initial state of the UAV;

[0029] The time-optimal trajectory optimization problem is solved by the interior point method.

[0030] Furthermore, the time optimal trajectory optimization problem is solved, and the initial trajectory obtained in step S3 and the time interval dt0 of each small trajectory segment are used as initial values ​​of numerical optimization to obtain a discretized time optimal reference trajectory.

[0031] Furthermore, in step S6, the nonlinear model predictive control problem established by the predictive control model is as follows:

[0032]

[0033] x k+1 =f(x k ,u k ,dt) (12)

[0034] x lb ≤x k ≤x ub (13)

[0035] u lb ≤u k ≤u ub (14)

[0036] x0=x init (15)

[0037] Among them, p k represents the position of the UAV at the kth time node, p ref,k represents the reference trajectory position of the kth time node obtained by step S5, H represents the step length of the backward vision in the nonlinear model predictive control; Formula (11) represents minimizing the tracking error of the UAV for the reference trajectory; Formula (12) represents the full dynamic model constraint of the UAV; Formula (13) represents the state constraint of the UAV, which means that the position, speed, angular velocity, and attitude cannot be greater than the maximum value set by the user; Formula (14) represents the actuator constraint, which means that the thrust cannot be greater than the maximum value set by the user; Formula (15) represents the initial state;

[0038] The nonlinear model predictive control problem is solved using the interior point method.

[0039] Furthermore, the nonlinear model predictive control problem is solved, and the initial state variables and control inputs obtained in step S4 are used as initial values ​​for numerical optimization to obtain the drone state and input quantities, which are output to the drone motion controller for execution.

[0040] The advantages and beneficial effects of the present invention are:

[0041] (1) The present invention can optimize the time and position of a UAV to intercept a moving target, making the interception more efficient and flexible.

[0042] (2) The present invention pre-solves the warm-up problem of time-optimal interception trajectory optimization through the interior point method, obtains a high-quality initial solution for the final time-optimal interception trajectory planning, improves the robustness of trajectory optimization, and effectively prevents the nonlinear optimization problem from falling into a poor local optimal solution.

[0043] (3) The present invention effectively solves the problem of large computational load caused by considering the time optimal trajectory planning problem through time pre-allocation, and can plan the time optimal interception trajectory in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a method flow chart of an embodiment of the present invention.

[0045] Figure 2 It is a flow chart of target detection and positioning in an embodiment of the present invention. DETAILED DESCRIPTION

[0046] The specific implementation of the present invention is described in detail below in conjunction with the accompanying drawings. It should be understood that the specific implementation described here is only used to illustrate and explain the present invention, and is not used to limit the present invention.

[0047] like Figure 1 As shown, the present invention discloses a time optimal trajectory planning method for a UAV to intercept a moving target, the steps comprising:

[0048] Step S1: Sensor data acquisition and self-positioning of the drone based on the acquired sensor data to obtain drone status information. Sensor data acquisition specifically uses the visual sensor to collect RGB image sequences in real time, and the IMU to collect acceleration and angular velocity data in real time; drone self-positioning is to obtain the drone's six-degree-of-freedom state information, including position and attitude, by inputting the RGB image sequence collected by the visual sensor and the acceleration and angular velocity data collected by the IMU, using the VINS-Mono monocular visual inertial state estimator;

[0049] Step S2: Target detection and positioning and target trajectory prediction.

[0050] like Figure 2 As shown in the figure, the input of target detection and positioning is an RGB image containing the target to be intercepted. The bounding box of the target to be intercepted is obtained by YOLO-v4-tiny detection running on the onboard computer of the drone. Then the obtained bounding box is input into the pre-trained key point extraction network YOLO-v8 to obtain several key points. According to the three-dimensional position coordinates of several key points predetermined on the target to be detected relative to the target body coordinate system, the PNP problem is solved by the EPNP algorithm with the RANSCA random sampling consensus method, and the six-degree-of-freedom state information of the target to be intercepted, including position and posture, is output;

[0051] The input of the target trajectory prediction is the target motion state information sequence obtained by step S2 in the previous δt time, including but not limited to position and speed. Through the pre-trained LSTM network inference, the predicted target motion trajectory is output;

[0052] Step S3: Construct and solve the trajectory optimization warm-up problem. The input is the initial trajectory, actuator constraints, state constraints, predicted target motion trajectory, and the initially selected discrete time step. The optimization warm-up problem is as follows:

[0053]

[0054] stx lb ≤x k ≤x ub (2)

[0055] u lb ≤u k ≤u ub (3)

[0056] x0=x init (4)

[0057] Among them, λ i ,i=0,1,2 represents the weight coefficient of the three-part penalty function, which is used to weigh the importance of the three. R represents the weight of different control inputs of the drone. T represents the interception position of the drone, where T = Ndt0, represents the interception time, where N represents the total number of segments into which the entire time optimal interception trajectory is divided, and dt0 represents the time interval between each small segment of the trajectory. represents the position of the target at time T predicted by S3, x k represents the state variable of the kth discrete node drone, x k =[p k v k q k ω k ] T ,k=0,1,…,N,u k represents the input of the kth discrete node drone, represents the square of the L2 norm, f(·) represents the full dynamics equation of the drone, x lb ,x ub ,u lb ,u ub denote the lower and upper bounds of the drone state and input, respectively, x init Represents the initial state variable of the drone. Formula (1) indicates that the drone intercepts the target, and the motion trajectory satisfies the penalty function sum of the drone's full dynamics and minimizes the control amount, aiming to appropriately relax the final nonlinear optimization problem and obtain a good quality initial solution; Formula (2) represents the drone state constraint, which means that the position, speed, angular velocity, and attitude cannot be greater than the maximum value set by the user; Formula (3) represents the actuator constraint, which means that the thrust cannot be greater than the maximum value set by the user; Formula (4) represents the initial state.

[0058] The trajectory optimization warm-up problem is solved. The nonlinear optimization problem is solved using the interior point method, with uniformly distributed points on the line connecting the initial position to the position of the target at time T predicted by step S2 and zero speed, zero Euler angle, zero angular velocity, and zero actuator input as the initial values ​​of the numerical optimization.

[0059] Step S4: Construct the time-optimal trajectory optimization problem and solve it. The input is the state and input trajectory obtained in S3, the actuator constraints, the state constraints, the predicted target motion trajectory, and the initially selected discrete time step. The optimization problem is as follows:

[0060]

[0061] x lb ≤x k ≤x ub (8)

[0062] u lb ≤u k ≤u ub (9)

[0063] x0=x init (10)

[0064] Where, T = Ndt, represents the interception time, δ represents the allowable error to ensure that the target is successfully intercepted, and the other variables are the same as described in the above steps. Formula (5) represents minimizing the interception time to obtain the target interception trajectory that satisfies the time optimization; Formula (6) represents the final target position constraint to ensure that the target is successfully intercepted; Formula (7) represents the nonlinear full dynamic constraint of the UAV; Formula (8) represents the UAV state constraint, which means that the position, speed, angular velocity, and attitude cannot be greater than the maximum value set by the user; Formula (9) represents the actuator constraint, which means that the thrust cannot be greater than the maximum value set by the user; Formula (10) represents the initial state of the UAV.

[0065] Solving the time-optimal trajectory optimization problem. The above nonlinear optimization problem is solved by the interior point method, using the initial trajectory and dt0 calculated in step S3 as the initial values ​​of the numerical optimization.

[0066] Step S5: Continuation of discrete trajectories. The input is the discretized time-optimal reference trajectory obtained in step S4, and the time-optimal continuous reference trajectory is output through third-order polynomial interpolation.

[0067] Step S6: Construct the nonlinear model predictive control problem. Take the drone's own state obtained in step S1 as input, and the time-optimal third-order polynomial trajectory obtained in step S5 as the reference trajectory, construct the optimization problem, and output the drone control quantity. The optimization problem is as follows:

[0068]

[0069] stx k+1 =f(x k ,u k ,dt) (12)

[0070] x lb ≤x k ≤x ub (13)

[0071] u lb ≤u k ≤u ub (14)

[0072] x0=x init (15)

[0073] Among them, p krepresents the position of the UAV at the kth time node, p ref,k represents the reference trajectory position of the kth time node obtained by step S5, H represents the step length of the backward vision in the nonlinear model predictive control, and the other variables are as described in the above steps. Formula (11) represents the minimization of the tracking error of the drone for the reference trajectory; Formula (12) represents the full dynamic model constraint of the drone; Formula (13) represents the state constraint of the drone, which means that the position, speed, angular velocity, and attitude cannot be greater than the maximum value set by the user; Formula (14) represents the actuator constraint, which means that the thrust cannot be greater than the maximum value set by the user; Formula (15) represents the initial state.

[0074] Solving the nonlinear model predictive control problem: The nonlinear model predictive control problem is solved using the interior point method, and the initial state variables and control inputs calculated in step S4 are used as initial values ​​for numerical optimization.

[0075] Finally, the drone state and input quantity obtained in step S6 are output to the drone motion controller for execution.

[0076] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A time-optimal trajectory planning method for UAV to intercept moving targets, characterized in that The steps include: Step S1: Obtain the drone's own status information; Step S2: Obtain the motion state information of the target to be intercepted within a period of time and predict its motion trajectory; Step S3: Based on the UAV's own state information, execution constraints, state constraints, and the predicted motion trajectory of the target to be intercepted, a trajectory optimization preheating model of the UAV's full dynamics of intercepting the target and minimizing the control amount is constructed to obtain the trajectory optimized UAV state and input trajectory, actuator constraints, state constraints, predicted target motion trajectory, and the initially selected discrete time step; Step S4: Based on the obtained trajectory-optimized UAV state and input trajectory, actuator constraints, state constraints, predicted target motion trajectory, and the initially selected discrete time step, a time-optimal trajectory optimization model that minimizes the interception time is constructed to obtain a discretized time-optimal reference trajectory; Step S5: Continuate the discrete time optimal reference trajectory to obtain the time optimal continuous reference trajectory; Step S6: Based on the UAV's own state information and the time-optimal continuous reference trajectory, a nonlinear model predictive control problem for minimizing the error of the UAV to the reference trajectory is constructed to obtain the UAV state and control input for execution by the UAV's motion controller.

2. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 1, characterized in that: In step S1, the drone performs visual inertial state estimation based on the collected image data, its own acceleration and angular velocity data to obtain the drone's position and posture information.

3. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 1, characterized in that: In the step S2, image data of the target to be intercepted is obtained, a bounding box of the target to be intercepted is obtained through target detection, the bounding box is input into a pre-trained key point extraction network to obtain a set of key points, and the motion state information of the target to be intercepted is calculated according to the three-dimensional position coordinates of the key points relative to the target body coordinate system; The input of the target motion trajectory prediction is the target's motion state information sequence in the previous period of time. The predicted target motion trajectory is output through inference using a pre-trained long short-term memory (LSTM) network.

4. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 1, characterized in that: In step S3, the trajectory optimization preheating problem established by the trajectory optimization preheating model is as follows: s.t.x lb ≤x k ≤x ub in lb in k in ub x0=x init Among them, λ1, λ2, and λ3 represent three weight coefficients, R represents the weight of different control inputs of the UAV, and p T represents the interception position of the drone, T=Ndt0 represents the interception time, N represents the total number of segments into which the entire time optimal interception trajectory is divided, dt0 represents the time interval between each small segment of the trajectory, Indicates the predicted location of the target at time T, x k represents the state variable of the kth discrete node drone, u k represents the input of the kth discrete node drone, represents the square of the L2 norm, f(·) represents the full kinematic equation of the drone, x lb 、x ub They represent the lower and upper bounds of the drone state, u lb 、u ub represents the lower and upper bounds of the drone input, x init Represents the initial state variables of the drone; The solution of trajectory optimization warm-up problem is solved using interior point method.

5. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 4, characterized in that: The x k =[p k v k q k ω k ] T ,k=0,1,…,N, where p k 、v k ,q k ,ω k They represent the position, velocity, angular velocity, and attitude of the kth discrete point UAV respectively.

6. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 4, characterized in that: The solution to the trajectory optimization warm-up problem is to use the evenly distributed points of the line connecting the initial position to the position of the target at time T predicted in step S2 and zero speed, zero Euler angle, zero angular velocity, and zero actuator input as the initial values ​​of the optimization, and solve the trajectory optimized drone state and input trajectory, actuator constraints, state constraints, predicted target motion trajectory, and the initially selected discrete time step.

7. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 4, characterized in that: In step S4, the time optimal trajectory optimization problem established by the time optimal trajectory optimization model is as follows: x ki+1 =f(x ki ,u ki ,dt i ) x lb ≤x k ≤u ub in lb in k in ub x0=x init Where, T = Ndt represents the interception time, and δ represents the allowable error to ensure that the target is successfully intercepted; The time-optimal trajectory optimization problem is solved by the interior point method.

8. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 7, characterized in that: The time optimal trajectory optimization problem is solved by using the initial trajectory obtained in step S3 and the time interval dt0 of each small trajectory segment as initial values ​​for numerical optimization to obtain a discretized time optimal reference trajectory.

9. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 7, characterized in that: In step S6, the nonlinear model predictive control problem established by the predictive control model is as follows: s.t.x k+1 =f(x k ,u k ,dt) x lb ≤x k ≤x ub in lb in k in ub x0=x init Among them, p k represents the position of the UAV at the kth time node, p ref,k represents the reference trajectory position at the kth time node, H represents the step length of the backward vision in the nonlinear model predictive control; The nonlinear model predictive control problem is solved using the interior point method.

10. The time-optimal trajectory planning method for an unmanned aerial vehicle to intercept a moving target according to claim 7, characterized in that: The nonlinear model predictive control problem is solved, and the initial state variables and control inputs obtained in step S4 are used as initial values ​​for numerical optimization to obtain the drone state and input quantities, which are output to the drone motion controller for execution.

Citation Information

Cited By

  • Maneuvering target interception track generation method and system considering passing point constraint

    CN121806981A