Quad-rotor unmanned aerial vehicle obstacle avoidance method based on digital twin system

Abstract

The invention discloses a four-rotor unmanned aerial vehicle obstacle avoidance method based on a digital twin system. The method comprises the following steps: step 1, establishing an unmanned aerial vehicle digital twin system; 2, designing a feedforward-feedback control strategy based on model predictive control in a physical space; 3, designing and deploying an off-line trajectory planning framework in the virtual space; according to the invention, on the basis of establishing the unmanned aerial vehicle digital twin system, through the design of the trajectory planning framework and the feedforward-feedback control strategy, the unmanned aerial vehicle trajectory planning quality and the motion control precision are improved.

Description

Technical field:

[0001] The present invention belongs to the technical field of obstacle avoidance for quad-rotor unmanned aerial vehicles, and in particular relates to an obstacle avoidance method for quad-rotor unmanned aerial vehicles based on a digital twin system. Background technology:

[0002] With the development of the low-altitude economy, quadcopter drones are widely used in a variety of scenarios, such as agricultural plant protection, commercial distribution, military operations, and disaster relief. In actual flight missions, drones are required to have autonomous flight capabilities, that is, to simultaneously achieve motion control, trajectory planning, and obstacle perception tasks. In real-world scenarios, autonomous flight of drones faces problems such as heavy computing burden, high hardware cost, and severe environmental interference.

[0003] Digital twin technology is usually used in the field of industrial production to establish a high-precision twin of the physical links in the actual production process, so as to remotely control and monitor the status of the industrial process. Designing a digital twin system for drones can transfer the high computing burden required for real-time perception and planning in drone obstacle avoidance to the virtual space by leveraging the rich computing resources of the virtual space, and reduce the hardware cost required for drones through high-precision mapping relationships.

[0004] Obstacle avoidance flight of drones is divided into two tasks, namely trajectory planning and motion control. In the obstacle avoidance task of drones, the trajectory planning algorithm can output a collision-free trajectory, but due to the real-time and lightweight requirements in actual application scenarios, the planning results are usually not optimal. The mainstream trajectory planning framework uses a depth camera to perceive obstacles and make real-time planning decisions. This vision-based planning framework is easily affected by light in actual tasks and cannot guarantee the stability of obstacle avoidance flight.

[0005] Completing high-quality obstacle avoidance tasks requires precise motion control. Model predictive control and other methods are usually used to control drones. MPC is suitable for processing multiple digital inputs and outputs in drone systems and various constraints, such as safety restrictions and dynamic constraints. Therefore, MPC is applied to physical drones and virtual drones to enable them to accurately track trajectories. However, MPC has the limitation of difficult solution.

[0006] In order to address these limitations, a quadrotor UAV obstacle avoidance method based on digital twin system is proposed to improve the trajectory planning quality and control accuracy in obstacle avoidance tasks. Summary of the invention:

[0007] The purpose of the present invention is to design a quadrotor drone obstacle avoidance method based on a digital twin system for the obstacle avoidance scenario of a quadrotor drone. On the basis of establishing a drone digital twin system, the trajectory planning quality and motion control accuracy of the drone are improved through the design of a trajectory planning framework and a feedforward-feedback control strategy.

[0008] The technical solution of the present invention is: a quad-rotor drone obstacle avoidance method based on a digital twin system, comprising the following steps:

[0009] Step 1: Establish a drone digital twin system;

[0010] Step 2: Design a feedforward-feedback control strategy based on model predictive control in physical space;

[0011] Step 3: Design and deploy an offline trajectory planning framework in the virtual space.

[0012] Furthermore, the process of establishing the UAV digital twin system is as follows:

[0013] The digital twin system is designed from four dimensions: physical space, virtual space, application services, and data processing;

[0014] Digital twins map physical space with high fidelity in a simulation environment, creating a virtual space that is highly similar to the physical space in terms of mathematical and physical properties, thus achieving twin mapping from physical space to virtual space.

[0015] The application service module provides trajectory planning, trajectory visualization and evaluation for the physical drone to the specified target point based on the virtual-real mapping relationship;

[0016] The data processing module stores and forwards the optimal feasible trajectory information and feedforward control quantity obtained from the virtual UAV offline process to the physical UAV.

[0017] Furthermore, the feedforward-feedback control strategy based on model predictive control is designed in the physical space, specifically as follows:

[0018] First, the virtual drone is modeled and analyzed according to Newton's second law and Euler's equation. The dynamic model of the virtual drone is as follows:

[0019]

[0020] Among them, x v (t), y v (t), z v (t) is the position of the virtual drone in space; p v ,q v 、r v I is the angular velocity of the virtual drone rotating around the coordinate axes x, y, and z of the ground coordinate system; vx ,I vy ,I vz is the moment of inertia of the virtual drone rotating around its axis; m v is the mass of the virtual drone; g is the acceleration due to gravity; U v1 , Uv2 , U v3 , U v4 is the control quantity input;

[0021] The quadrotor model is divided into a position loop system and an attitude loop system. The attitude loop system is implemented using a cascade PID controller, which receives the required thrust, roll, and pitch commands and calculates the control commands for the four motors. The desired roll and pitch angles of the attitude controller are expressed as:

[0022]

[0023] in, and θ dv (t) is the desired roll and pitch angle, u vx (t) and u vy (t) represents the control input in the X and Y directions respectively; taking the X direction as an example for analysis, the tracking error is expressed as follows:

[0024]

[0025] Among them, x r (t) is the reference position, x v (t) is the real-time position, e v1 (t) is the position tracking error, e v2 (t) is the velocity tracking error; the tracking error system is expressed as follows:

[0026]

[0027] In addition, let the tracking error vector be e vx (t) = [e v1 (t),e v2 (t)] T , the state vector in the X direction is With h = 0.02s as the sampling period, the tracking error system of the virtual drone after discretization in the X direction is as follows:

[0028]

[0029] where e vx (t k ) and u vx (t k ) represent the virtual UAV at sampling time t k Tracking error and control input; Similarly, the discrete tracking error system of the physical drone in the X direction is as follows:

[0030]

[0031] where e px (tk ) and u px (t k ) represent the physical UAV at sampling time t k The tracking error and control input of the virtual UAV are obtained. After obtaining the tracking error system of the virtual UAV, a model predictive controller is designed for the system, and the terminal controller is designed as follows:

[0032] u vx (t k )=κ1e v1 (t k )+κ2e v2 (t k )

[0033] Where K = [κ1, κ2] is the feedback weight coefficient, giving a circular terminal set:

[0034] Ω:={e vx (t k )|‖e vx (t k )‖ 2 ≤r 2}

[0035] Where r is the radius of the terminal set, let T = nh be the prediction time domain, where N represents the prediction step length; for the tracking error system of the virtual drone, the cost function is given as:

[0036]

[0037] in is the state cost function, is the terminal state function; P, Q, R are three positive definite matrices; for τ∈[t k ,t k +T], the optimization problem is designed as:

[0038]

[0039] stε vx (t k |t k )=ε vx (t k )

[0040]

[0041] e vx (t k +T|t k )∈Ω

[0042] Optimal cost function J * (t k )for:

[0043]

[0044] After solving the optimization problem, only the first control variable is applied in the first sampling period, and the process is repeated in the next sampling period to achieve rolling optimization. The model predictive controller design of the physical drone is consistent with that of the virtual drone. where u v (t),u p (t) are the control inputs of the virtual drone and the physical drone respectively; u v (t) is stored in the data storage unit, and the feedback control input u p (t) is obtained online by the model predictive controller of the physical UAV; the feedforward control input is expressed as:

[0045] u f (t) = δ v u v (t)

[0046] where δ v Denotes the feedforward gain, let δ p is the feedback gain, if there is δ v +δ p =1, the feedforward-feedback control input is normalized; the control output of the virtual UAV MPC is used as the feedforward input of the physical UAV, and a feedforward-feedback control strategy based on the digital twin is constructed to provide prior knowledge for the MPC to improve the solution speed.

[0047] Furthermore, an offline trajectory planning framework is designed and deployed in the virtual space, as follows:

[0048] Repeated planning in virtual space and selection of the best feasible trajectory;

[0049] EGO-Planner is deployed on the virtual UAV to generate multiple reference trajectories for the virtual UAV, and MPC is used to track these reference trajectories. The tracking results are multiple feasible trajectories of the virtual UAV in the offline process;

[0050] For the trajectory that passes through the obstacle area, the evaluation function is selected as:

[0051] G(PT i )=αGoal(PT i )+βObs(PT i )+γVel(PT i )

[0052] Among them, α, β, and γ are weight coefficients, Goal(PTi), Obs(PTi), and Vel(PTi) are the goal evaluation function, obstacle evaluation function, and speed evaluation function of PTi, respectively; the evaluation function is used to select a faster collision-free speed toward the endpoint at the current time t; and the optimal feasible trajectory is selected as the reference trajectory of the physical UAV.

[0053] Beneficial Effects

[0054] Compared with the prior art, the present invention has the following advantages:

[0055] 1. Through the design of a digital twin system for a quad-rotor UAV, the present invention can improve the obstacle avoidance flight performance of the UAV from two aspects: trajectory planning and motion control, and reduce the onboard computing burden and sensor cost of the UAV.

[0056] 2. The present invention transfers the computing burden to the virtual space through the high-precision mapping of the digital twin system, so that the perception and planning tasks are performed in the virtual space, reducing the interference of light in the real scene on the planning results and improving the stability of the trajectory planning results.

[0057] 3. The present invention replaces the initial controller by combining EGO-Planner with MPC to generate high-quality collision-free feasible trajectories in the virtual space; an offline trajectory planning framework is designed in the digital twin system to obtain the optimal feasible trajectory, thereby achieving optimal obstacle avoidance performance.

[0058] 4. The present invention fully utilizes the prior knowledge provided by the virtual space in the digital twin system through the design of a feedforward-feedback control strategy, and uses the control input of the virtual drone as the feedforward input of the physical drone, thereby improving the control accuracy and shortening the MPC solution time. Description of the drawings:

[0059] Figure 1 It is a schematic diagram of the obstacle avoidance method of the quad-rotor drone of the present invention;

[0060] Figure 2 This is a structural diagram of the UAV digital twin system of the present invention;

[0061] Figure 3 It is a structural diagram of the trajectory planning framework of the present invention;

[0062] Figure 4 It is a block diagram of the feedforward-feedback control strategy of the present invention;

[0063] Figure 5 The coordinate system of the drone used in the present invention;

[0064] Figure 6 It is the trajectory planning result diagram of the present invention;

[0065] Figure 7 It is a feedforward-feedback control result diagram of the present invention;

[0066] Figure 8 This is an example process diagram of the UAV digital twin system of the present invention;

[0067] Fig. 9 is a feedforward-feedback control input diagram of the present invention;

[0068] Fig.10 This is a trajectory tracking error result diagram of the present invention. Specific implementation method:

[0069] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0070] The purpose of this invention is to solve the obstacle avoidance scenarios of quad-rotor drones, such as Figure 1 As shown in the figure, a digital twin system is designed to improve the obstacle avoidance performance of the UAV, and a trajectory planning framework and feedforward-feedback control strategy are proposed based on the system, which reduces the hardware cost of the UAV's autonomous flight while achieving better planning and control performance.

[0071] exist Figure 2 In the paper, the digital twin system is designed from four dimensions: physical space, virtual space, application services and data processing. The digital twin performs high-fidelity mapping of the physical space in the simulation environment, creates a virtual space that is highly similar to the physical space in mathematical and physical properties, and realizes the twin mapping from physical space to virtual space. With its powerful computing power, the virtual space can simulate and optimize the trajectory planning process, thereby transferring the computing burden in the physical space to the virtual space. In the virtual space, the trajectory planning algorithm is designed as a repeatable offline process to generate multiple collision-free feasible trajectories, and selects the optimal feasible trajectory as the planning result based on the trajectory evaluation index. The physical space, as the object of the digital twin, includes drones and obstacle environments. Environmental information is obtained through three-dimensional reconstruction and other methods, and a high-precision mapping is constructed in the virtual space. A twin is constructed for the drone in the physical space, and the corresponding mechanical structure is imported into the simulation environment to maintain the same physical parameters to ensure the accuracy of the twin. The application service module is a C++ project built in the ROS system, which provides application services for physical drones based on the virtual-real mapping relationship. The trajectory planning of the specified target point and the visualization and evaluation of multiple feasible trajectories are respectively performed. Combined with the data processing module, the communication between the virtual space and the physical space is established through the ROS topic, and the optimal feasible trajectory information and feedforward control quantity obtained from the offline process of the virtual drone are stored and forwarded to the physical drone. Based on the above four dimensions, a digital twin system based on drones is constructed.

[0072] Depend on Figure 3The UAV coordinate system shown in the figure establishes and discretizes the dynamic model of the virtual UAV, as follows:

[0073] According to Newton's second law and Euler's equation, the virtual drone is modeled and analyzed. The dynamic model of the virtual drone is as follows:

[0074]

[0075] Among them, x v (t), y v (t), z v (t) is the position of the virtual drone in space; p v ,q v 、r v I is the angular velocity of the virtual drone rotating around the coordinate axes x, y, and z of the ground coordinate system; vx ,I vy ,I vz is the moment of inertia of the virtual drone rotating around its axis; m v is the mass of the virtual drone; g is the acceleration due to gravity; U v1 , U v2 , U v3 , U v4 is the control quantity input;

[0076] The quadrotor model is divided into a position loop system and an attitude loop system. The attitude loop system is implemented using a cascade PID controller, which receives the required thrust, roll, and pitch commands and calculates the control commands for the four motors. The desired roll and pitch angles of the attitude controller are expressed as:

[0077]

[0078] in, and θ dv (t) is the desired roll and pitch angle, u vx (t) and u vy (t) represents the control input in the X and Y directions respectively; taking the X direction as an example for analysis, the tracking error is expressed as follows:

[0079]

[0080] Among them, x r (t) is the reference position, x v (t) is the real-time position, e v1 (t) is the position tracking error, e v2 (t) is the velocity tracking error; the tracking error system is expressed as follows:

[0081]

[0082] In addition, let the tracking error vector be e vx (t) = [e v1 (t),e v2 (t)] T , the state vector in the X direction is With h = 0.02s as the sampling period, the tracking error system of the virtual drone after discretization in the X direction is as follows:

[0083]

[0084] where e vx (t k ) and u vx (t k ) represent the virtual UAV at sampling time t k Tracking error and control input; Similarly, the discrete tracking error system of the physical drone in the X direction is as follows:

[0085]

[0086] where e px (t k ) and u px (t k ) represent the physical UAV at sampling time t k Tracking error and control input;

[0087] After obtaining the tracking error system of the virtual UAV, a model predictive controller is designed for the system, and the terminal controller is designed as follows:

[0088] u vx (t k )=κ1e v1 (t k )+κ2e v2 (t k )

[0089] Where K = [κ1, κ2] is the feedback weight coefficient, giving a circular terminal set:

[0090] Ω:={e vx (t k )|‖e vx (t k )‖ 2 ≤r 2}

[0091] Where r is the radius of the terminal set, let T = nh be the prediction time domain, where N represents the prediction step length. For the tracking error system of the virtual drone, the cost function is given as:

[0092]

[0093] in is the state cost function, is the terminal state function; P, Q, R are three positive definite matrices; for τ∈[t k ,t k +T], the optimization problem is designed as:

[0094]

[0095] stε vx (t k |t k )=ε vx (t k )

[0096]

[0097] e vx (t k +T|t k )∈Ω

[0098] Optimal cost function J * (t k )for:

[0099]

[0100] After solving the optimization problem, the first control variable is only applied in the first sampling period, and the process is repeated at the beginning of the next sampling period to achieve rolling optimization. Based on the proposed digital twin system, the same controller is designed for the physical drone and the virtual drone, and Figure 4 The feedforward-feedback control strategy shown in the figure is used to improve the control effect; where u v (t),u p (t) are the control inputs of the virtual drone and the physical drone respectively; u v (t) is stored in the data storage unit, and the feedback control input u p (t) is obtained online by the model predictive controller of the physical UAV; the feedforward control input is expressed as:

[0101] u f (t) = δ v u v (t)

[0102] where δ v Denotes the feedforward gain, let δ p is the feedback gain, if there is δ v +δ p=1, the feedforward-feedback control input is normalized. That is, in the offline process, the optimal feasible trajectory is selected as the reference trajectory of the physical drone, and the control input of the virtual drone is used as the feedforward input of the physical drone. The feedforward-feedback control strategy is obtained from the high-fidelity mapping relationship of the digital twin system, which reduces the complexity and difficulty of mathematical modeling and improves the control performance.

[0103] like Figure 5 As shown in the figure, a trajectory planning framework based on a digital system is designed and applied to a physical UAV, multiple feasible trajectories are obtained in the virtual space and the optimal feasible trajectory is selected; EGO-Planner is deployed on the virtual UAV to generate multiple reference trajectories for the virtual UAV, and these reference trajectories are tracked using MPC. The g tracking results are multiple feasible trajectories of the virtual UAV in the offline process; among these feasible trajectories, there are trajectories that pass through the obstacle area and those that bypass the obstacle area;

[0104] For the trajectory that passes through the obstacle area, the evaluation function is selected as:

[0105] G(PT i )=αGoal(PT i )+βObs(PT i )+γVel(PT i )

[0106] Among them, α, β, and γ are weight coefficients, Goal(PTi), Obs(PTi), and Vel(PTi) are the goal evaluation function, obstacle evaluation function, and speed evaluation function of PTi, respectively; the evaluation function is used to select a faster collision-free speed toward the endpoint at the current time t; and the optimal feasible trajectory is selected as the reference trajectory of the physical UAV.

[0107] Example

[0108] The present invention is based on a four-rotor unmanned aerial vehicle and other equipment as a collision avoidance experimental platform, which is mainly composed of a four-rotor unmanned aerial vehicle, a remote controller, a PC host computer, a wireless router, obstacles and a pair of wireless data transmission devices.

[0109] The digital twin system is deployed on a PC host computer, and the communication between the twin system and the physical drone is established through a wireless LAN, and the obstacle position, size and other information are mapped with high precision in the virtual space. The digital twin system obtains the optimal feasible trajectory as the reference trajectory of the physical drone, and obtains the corresponding feedforward input to perform obstacle avoidance flight of the physical drone. In the digital twin system, based on the start and end points and obstacle information, the virtual drone performs multiple repeated trajectory planning and uses MPC to track and control the trajectory to obtain multiple feasible trajectories. The application service module evaluates these trajectories and obtains the following: Figure 6The trajectory planning results are shown.

[0110] In the obstacle environment, the flight altitude is set to 1.5 meters, and a physical drone is used to complete the collision avoidance experiment to verify the effectiveness of the proposed feedforward-feedback control strategy. RTK positioning is used to obtain precise positioning information, and a coordinate system is established with the initial position of the quadcopter as the origin, the east direction as the X-axis, and the north direction as the Y-axis. After the quadcopter is taken off using the remote control, the flight mode is switched from the fixed-point mode to the offboard mode. The quadcopter flies from the initial position to the target position (9m, 0m) and receives the optimal feasible trajectory information and feedforward control input from the digital twin system. Finally, Figure 7 The collision avoidance trajectory tracking diagram shown in Figure 2 shows the operation process of the digital twin system in the example. Figure 8 shown. Fig. 9 is the position error of the physical drone during trajectory tracking. It can be seen that the feedforward-feedback control strategy proposed in the present invention is effective and can maintain high-precision trajectory tracking control. The feedforward-feedback control input is Fig.10 As shown, it can be seen that the digital twin system designed by the present invention can obtain high-quality trajectory planning results by keeping the control input within the constraint range, and the proposed feedforward-feedback control strategy can improve the control accuracy and realize the transfer of computational burden.

Claims

1. A quadrotor drone obstacle avoidance method based on a digital twin system, comprising the following steps: Step 1: Establish a drone digital twin system; Step 2: Design a feedforward-feedback control strategy based on model predictive control in physical space; Step 3: Design and deploy an offline trajectory planning framework in the virtual space.

2. The quadrotor drone obstacle avoidance method based on a digital twin system according to claim 1, characterized in that: The process of establishing the UAV digital twin system is as follows: The digital twin system is designed from four dimensions: physical space, virtual space, application services, and data processing; Digital twins map physical space with high fidelity in a simulation environment, creating a virtual space that is highly similar to the physical space in terms of mathematical and physical properties, thus achieving twin mapping from physical space to virtual space. The application service module provides trajectory planning, trajectory visualization and evaluation for the physical drone to the specified target point based on the virtual-real mapping relationship; The data processing module stores and forwards the optimal feasible trajectory information and feedforward control quantity obtained from the virtual UAV offline process to the physical UAV.

3. The quadrotor drone obstacle avoidance method based on a digital twin system according to claim 2, characterized in that: The feedforward-feedback control strategy based on model predictive control is designed in the physical space, and is specifically as follows: First, the virtual drone is modeled and analyzed according to Newton's second law and Euler's equation. The dynamic model of the virtual drone is as follows: Among them, x v (t), y v (t), z v (t) is the position of the virtual drone in space; p v ,q v 、r v I is the angular velocity of the virtual drone rotating around the coordinate axes x, y, and z of the ground coordinate system; vx ,I vy ,I vz is the moment of inertia of the virtual drone rotating around its axis; m v is the mass of the virtual drone; g is the acceleration due to gravity; U v1 , U v2 , U v3 , U v4 is the control quantity input; The quadrotor model is divided into a position loop system and an attitude loop system. The attitude loop system is implemented using a cascade PID controller, which receives the required thrust, roll, and pitch commands and calculates the control commands for the four motors. The desired roll and pitch angles of the attitude controller are expressed as: in, and θ dv (t) is the desired roll and pitch angle, u vx (t) and u vy (t) represents the control input in the X and Y directions respectively; taking the X direction as an example for analysis, the tracking error is expressed as follows: Among them, x r (t) is the reference position, x v (t) is the real-time position, e v1 (t) is the position tracking error, e v2 (t) is the velocity tracking error; the tracking error system is expressed as follows: In addition, let the tracking error vector be e vx (t) = [e v1 (t),e v2 (t)] T , the state vector in the X direction is With h = 0.02s as the sampling period, the tracking error system of the virtual drone after discretization in the X direction is as follows: where e vx (t k ) and u vx (t k ) represent the virtual UAV at sampling time t k Tracking error and control input; Similarly, the discrete tracking error system of the physical drone in the X direction is as follows: where e px (t k ) and u px (t k ) represent the physical UAV at sampling time t k The tracking error and control input of the virtual UAV are obtained. After obtaining the tracking error system of the virtual UAV, a model predictive controller is designed for the system, and the terminal controller is designed as follows: u vx (t k )=κ1e v1 (t k )+κ2e v2 (t k ) Where K = [κ1,κ2] is the feedback weight coefficient, giving a circular terminal set: Ω:={e vx (t k )|‖e vx (t k )‖ 2 ≤r 2 } Where r is the radius of the terminal set, let T = nh be the prediction time domain, where N represents the prediction step length; for the tracking error system of the virtual drone, the cost function is given as: in is the state cost function, is the terminal state function; P, Q, R are three positive definite matrices; for τ∈[t k ,t k +T], the optimization problem is designed as: s.t.ε vx (t k |t k )=ε vx (t k ) e vx (t k +T|t k )∈Ω Optimal cost function J * (t k )for: After solving the optimization problem, only the first control variable is applied in the first sampling period, and the process is repeated in the next sampling period to achieve rolling optimization. The model predictive controller design of the physical drone is consistent with that of the virtual drone. where u v (t),u p (t) are the control inputs of the virtual drone and the physical drone respectively; u v (t) is stored in the data storage unit, and the feedback control input u p (t) is obtained online by the model predictive controller of the physical UAV; the feedforward control input is expressed as: u f (t)=δ v u v (t) where δ v Denotes the feedforward gain, let δ p is the feedback gain, if there is δ v +δ p =1, the feedforward-feedback control input is normalized; the control output of the virtual UAV MPC is used as the feedforward input of the physical UAV, and a feedforward-feedback control strategy based on the digital twin is constructed to provide prior knowledge for the MPC to improve the solution speed.

4. The quad-rotor drone obstacle avoidance method based on a digital twin system according to claim 3 is characterized in that: Design and deploy an offline trajectory planning framework in the virtual space as follows: Repeated planning in virtual space and selection of the best feasible trajectory; EGO-Planner is deployed on the virtual UAV to generate multiple reference trajectories for the virtual UAV, and MPC is used to track these reference trajectories. The tracking results are multiple feasible trajectories of the virtual UAV in the offline process; For the trajectory that passes through the obstacle area, the evaluation function is selected as: G(PT i )=αGoal(PT i )+βObs(PT i )+γVel(PT i ) Among them, α, β, and γ are weight coefficients, Goal(PTi), Obs(PTi), and Vel(PTi) are the goal evaluation function, obstacle evaluation function, and speed evaluation function of PTi, respectively; the evaluation function is used to select a faster collision-free speed toward the endpoint at the current time t; and the optimal feasible trajectory is selected as the reference trajectory of the physical UAV.

Images