Trajectory planning method based on dynamic intention of unmanned aerial vehicle in fixed and free mixed airspace
By constructing a rotor drone performance model and grid airspace environment, combining GIS and path planning algorithms, the problem of dynamic track intention changes in the rotor drone in a hybrid airspace environment is solved, and flexible and safe track planning is achieved. Improve the level of low-altitude air traffic management.
Patent Information
- Application Number
- CN202410351291.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-03-26
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2044-03-26
AI Technical Summary
The existing technology is difficult to plan dynamic track intention changes for rotor UAVs in a hybrid environment of structured route/route airspace and free airspace, especially in low-altitude airspace, complex such as structured routes, free airspace and ground obstacles. factor.
By building a rotor drone performance model and grid airspace environment, combining GIS geographic information system, determining unavailable grids in the airspace, and using Dijkstra and A* algorithms to plan the drone's tracks to achieve dynamic intention changes and target locations Trail planning.
It realizes flexible track planning for rotor UAVs in solidified and free hybrid airspace environments, improving the flexibility and safety of low-altitude air traffic management.
Smart Images

Figure CN118295449B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to a trajectory planning method based on the dynamic intention of a rotary-wing unmanned aerial vehicle in a solidified and freely mixed airspace environment, and belongs to the technical fields of air traffic management, airspace planning and management, and unmanned aerial vehicle trajectory planning. Background Art
[0002] In view of the problem that a single UAV performs multiple tasks in the UAV air transportation system, the UAV needs to temporarily change its destination after taking off due to the change of the UAV's mission. Due to the change of the design track intention, the original track of the UAV is no longer suitable for the continued execution of the mission; therefore, a UAV dynamic track planning method that can timely and accurately determine the track under the new target for the UAV after the track intention is changed, and can be compatible with the low-altitude airspace demarcation environment, including structured route airspace, free airspace, ground obstacles and other complex factors, is urgently needed to support the operation of large-scale UAV swarms in the future. Therefore, the method in this paper considers the track planning technology suitable for the dynamic intention change of rotary-wing UAVs in the future solidified / free airspace fusion airspace demarcation mode, so as to adapt to the increasingly diversified and complex airspace environment and air traffic needs of UAVs. Summary of the invention
[0003] The present invention provides a trajectory planning method based on the dynamic intention of a rotary-wing UAV in a mixed environment of solidified and free airspace, which solves the problem of a trajectory planning method considering the dynamic trajectory intention of a rotary-wing UAV in a mixed environment of structured route / route airspace and free airspace.
[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is:
[0005] A trajectory planning method based on the dynamic intention of a rotary-wing UAV in a solidified and free mixed airspace environment, the main steps of which include:
[0006] Step 1: Construction of the performance model of the rotary-wing UAV
[0007] Based on the control method and control model of the rotor UAV and the specific performance of the specific UAV model, the state transfer equation of the UAV model is determined.
[0008] Step 2: Construction of the grid airspace environment for rotary-wing drones
[0009] Step 2.1 Determine the spatial grid granularity
[0010] Based on the basic performance parameters of the rotorcraft UAV in step one and the UAV state transfer equation, determine the length, width, and height dimensions of the UAV unit airspace grid.
[0011] Step 2.2 The airspace cannot be determined by grid
[0012] After determining the basic airspace grid granularity, the airspace is rasterized, and then based on the GIS geographic information system and relevant airspace planning information, the building obstacles, airspace restricted areas / forbidden areas in the drone flight airspace are determined and marked as unavailable airspace grids.
[0013] Step 3: Identify the UAV flight process and determine the exit waypoint
[0014] Step 3.1 Identify the flight process of the drone
[0015] Assume that the waypoints in the original flight plan of the UAV form an ordered list TP, TP = [[x 1 ,y 1 ],[x 2 ,y 2 ],…,[x n ,y n ]], the i-th planned waypoint is denoted as TP i =[x i ,y i ], let the current position of the drone be [x, y], then the segment TP to which the drone belongs m -TP n for:
[0016]
[0017] Step 3.2 UAV flight intention identification
[0018] According to the current UAV operation information, determine the UAV's flight plan process and the next target waypoint, and discuss the following situations:
[0019] Case ①: If the current drone position satisfies the formula (*), the next target waypoint next_point is:
[0020] next_point=Tp (i+1)
[0021] Case ②: If the current drone position and a planned waypoint do not satisfy the formula (*), the next position [x', y'] is predicted based on the current drone position [x, y], and the next target waypoint next_point is:
[0022] next_point=argmin{||(x',y'),(x i+1 ,y i+1 )||,||(x',y'),(x i ,y i )||}
[0023] Step 3.3 Change the track intention when the drone leaves the waypoint determination
[0024] Assume that the final destination of the drone after changing its trajectory is D, D = [x d ,y d ], the set of waypoints in the airspace is P, P = [P 1 ,P 2 ,…,P n ], select the airspace waypoint closest to the destination position D as the exit waypoint P * :
[0025] P * =argmin{||P k -D||},k∈{1,2,...,n}
[0026] Step 4: Track planning from the current waypoint to the exit waypoint of the rotary-wing UAV based on the Dijkstra algorithm
[0027] The Dijkstra algorithm is used to plan the trajectory from the next target waypoint to the departure waypoint of the rotary-wing UAV.
[0028] Step 5: Track planning of the rotorcraft UAV from the waypoint to the final target location based on the A* algorithm
[0029] The A* algorithm is used to plan the trajectory of the rotary-wing UAV from the waypoint to the final target location.
[0030] The beneficial effects of the present invention are: realizing the trajectory planning of rotary-wing UAVs in scenarios such as "single UAV multi-tasking" and "single UAV flight mission change" in solidified airspace, and improving the flexibility and safety level of low-altitude air traffic planning and management. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] Figure 1 Schematic diagram of the control model of the rotor UAV, where (a) is a schematic diagram of the rotor speed of the quadrotor UAV when it is moving forward, (b) is a schematic diagram of the rotor speed of the quadrotor UAV when it is moving backward, (c) is a schematic diagram of the rotor speed of the quadrotor UAV when it is turning right, and (d) is a schematic diagram of the rotor speed of the quadrotor UAV when it is turning left.
[0032] Figure 2 The grid airspace environment design diagram of the rotary-wing UAV, where (a) is the two-dimensional / three-dimensional grid accessibility analysis of the UAV, (b) is the cost function design based on the Euclidean space distance of the accessible grid, and (c) is the UAV accessibility analysis when there is an unavailable grid.
[0033] Figure 3 This is an example diagram of the trajectory planning method based on the dynamic intention of the UAV of the present invention.
[0034] Figure 4This is a flow chart of the trajectory planning method based on the dynamic intention of the UAV of the present invention. DETAILED DESCRIPTION
[0035] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.
[0036] As shown in the figure, the present invention provides a trajectory planning method based on the dynamic intention of a rotary-wing UAV in a solidified and free mixed airspace environment, and the steps include:
[0037] Step 1: Construction of the performance model of the rotary-wing UAV
[0038] The UAV control model is established by taking a quad-rotor UAV as an example. The movement of a quad-rotor UAV can be regarded as a combination of the translation of the center of the UAV and the rotation of the UAV body. The rotation matrix R of the UAV is B :
[0039]
[0040] in, is the roll angle of the UAV, θ is the pitch angle, and ψ is the yaw angle.
[0041] The control of a quadcopter is generally achieved by adjusting the speed of the propellers. Due to the difference in the speed of the propellers, complex flight maneuvers such as rolling, yaw, climbing and descending can be achieved. 1 for:
[0042]
[0043] Where ρ is the atmospheric density, v i is the i-th rotor speed of the UAV, S is the propeller blade area of the UAV, C L is the lift coefficient of the UAV rotor, U 1 Provide lift for drones. i =ω i ·r / 2, r is the length of the drone blade. Based on this, formula (2) can be simplified to :
[0044]
[0045] Where b is a function of ρ, S, and C L The composite lift parameters of the composition. i is the angular velocity of the i-th propeller rotor (r / min, RPM), ω is a vector matrix composed of the four angular velocities of the UAV propeller rotor, ω=[ω 1 ,ω 2 ,ω 3 ,ω4 ] T , the force balance equation of the UAV is as follows:
[0046]
[0047]
[0048] Where m is the mass of the drone, g is the acceleration due to gravity, and k a is the air resistance coefficient, is the acceleration of the drone on the x, y, and z axes, is the speed of the drone on the x, y, and z axes.
[0049] Then the rigid body model of the UAV is established. Since the quadrotor UAV has an approximately centrally symmetrical structure, the inertia matrix I can be defined as the diagonal matrix diag(I x ,I y ,I z ), I x ,I y ,I z It reflects the inertia of the drone rotating along the x, y, and z axes. The rotation torque matrix M of the drone 1 as follows:
[0050]
[0051] Among them U 2 ,U 3 ,U 4 are the rolling moment, pitch moment and yaw moment of the drone, l is the distance from the center of the drone propeller to the geometric center of the drone, k d is the proportional coefficient of the rotor's anti-torque. Therefore, the rotational motion equation of the quadrotor drone is:
[0052]
[0053] Among them, I b is the moment of inertia of the propeller about each shaft axis, is the angular acceleration of the roll angle, pitch angle and yaw angle, are the angular velocities of roll, pitch and yaw.
[0054] Now we get the drone state transfer equation, the drone state S after t seconds t (ω) can be described as:
[0055]
[0056] in, is the initial state of the drone, is the new state of the drone at timestamp t.
[0057] Step 2: Construction of the grid airspace environment for rotary-wing drones
[0058] Step 2.1 Determine the spatial grid granularity
[0059] According to the rotor UAV force balance equation (5) and the UAV state transfer equation (8), the speed of the UAV in the cruising speed x, y, z direction is:
[0060]
[0061] Assuming that the planned trajectory time granularity is T, the flight distance Δx, Δy, Δz of each coordinate component of the rotorcraft UAV at the unit trajectory time granularity is:
[0062]
[0063] Substitute the rated speed of the rotary wing UAV during cruising ω i , pitch angle upper limit, pitch angle lower limit, roll angle upper limit, roll angle lower limit, moment of inertia and other parameters, to obtain the standard grid size of the rotor UAV at the rated rotor speed of each coordinate axis component:
[0064]
[0065] ,θ∈{θ max ,θ min},φ∈{φ max ,φ min},ψ∈{ψ max ,ψ min}
[0066] Check the performance parameters of the drone. Here we take the performance parameters of the DJI Phantom4 rotary wing drone as an example (as shown in the table below) to get the optimal grid granularity. Based on this, the airspace grid environment of the rotary-wing UAV is constructed.
[0067] DJI Phantom 4 Rotor UAV Performance Parameters
[0068]
[0069]
[0070] Step 2.2 The airspace cannot be determined by grid
[0071] Obtain the geographic information data of buildings from the geographic information system (GIS) database, aerial images or other geographic information data sources, parse the acquired building data, extract the location, height and other information of the buildings, and determine the obstacle grid formed by the buildings in the airspace according to their height and location; at the same time, obtain the flight restriction area and restricted area information of the airspace, which can usually also be obtained from the GIS database or the data of the aviation management agency, analyze the data of the flight restriction area and restricted area, determine the boundaries and restrictions of these areas, and mark the flight restriction area and restricted area in the grid. The airspace grid composed of buildings, airspace restriction areas and restricted areas are all marked as unavailable grids.
[0072] Step 3: Identify the UAV flight process and determine the exit waypoint
[0073] Step 3.1 Identify the flight process of the drone
[0074] Assume that the waypoints in the original flight plan of the UAV form an ordered list TP, TP = [[x 1 ,y 1 ],[x 2 ,y 2 ],…,[x n ,y n ]], the i-th planned waypoint is denoted as TP i =[x i ,y i ], let the current position of the drone be [x, y], then the segment TP to which the drone belongs m -TP n for:
[0075]
[0076] This is used as a criterion to determine the flight plan segment to which the drone's current location belongs.
[0077] Step 3.2 UAV flight intention identification
[0078] Based on the UAV flight plan, determine the next target waypoint under the current UAV flight process.
[0079] First, if the current track point and the latest track point in the historical track satisfy formula (12), it means that the aircraft is close to the destination and there is no need to determine the next waypoint.
[0080] (xx -1 ) 2 +(yy -1 ) 2 ≤ε (12)
[0081] Among them, x represents the horizontal coordinate of the current drone position, y represents the vertical coordinate of the current drone position, and x -1,y -1 They represent the horizontal and vertical coordinates of the last waypoint planned by the UAV respectively, and ε is a suitable and small value.
[0082] Further discussion is divided into the following situations:
[0083] Case ①: If the current drone position satisfies formula (12), the next target waypoint next_point is:
[0084] next_point=Tp (i+1) (13)
[0085] Case ②: If the current UAV position and a planned waypoint do not satisfy formula (13), the next position [x', y'] is predicted based on the current UAV position [x, y], and the next target waypoint next_point is:
[0086] next_point=argmin{||(x',y'),(x i+1 ,y i+1 )||,||(x',y'),(x i ,y i )||} (14)
[0087] Step 3.3 Change the track intention when the drone leaves the waypoint determination
[0088] Assume that the final destination of the drone after changing its trajectory is D, D = [x d ,y d ], the set of waypoints in the airspace is P, P = [P 1 ,P 2 ,…,P n ], select the airspace waypoint closest to the destination position D as the exit waypoint P * :
[0089] P * =argmin{||P k -D||},k∈{1,2,...,n} (15)
[0090] Step 4: Track planning from the current waypoint to the exit waypoint of the rotary-wing UAV based on the Dijkstra algorithm
[0091] The set of waypoints in the airspace is denoted as P, P = [P 1 ,P 2 ,…,P n ], the next waypoint position in the current running process of the drone is next_point, and the drone leaves the waypoint is P * Now the UAV plans next_point to leave the waypoint P* The specific algorithm process is as follows:
[0092]
[0093]
[0094] Step 5: Track planning of the rotorcraft UAV from the waypoint to the final target location based on the A* algorithm
[0095] After determining the UAV's departure point, the A* algorithm is used to perform track addressing of the UAV from the departure point to the destination (new track intention), realizing the terminal track planning of the rotor UAV's dynamic intention addressing. The specific algorithm process is as follows:
[0096]
[0097]
Claims
1. A trajectory planning method based on the dynamic intention of UAV in fixed and free mixed airspace, It is characterized in that The following steps are involved: Step 1: Construction of rotorcraft UAV performance model; Step 2: Construction of the grid airspace environment of the rotary-wing UAV; Step 3: Identify the UAV flight process and determine the exit waypoint; Step 4: Track planning from the current waypoint to the exit waypoint of the rotary-wing UAV based on the Dijkstra algorithm; Step 5: Track planning from the rotorcraft UAV’s departure point to the final target location based on the A* algorithm; The force balance equation of the drone is: in, is the roll angle of the drone, θ is the pitch angle, ψ is the yaw angle; m is the mass of the drone, g is the gravitational acceleration, k a is the air resistance coefficient, is the acceleration of the UAV on the x, y, and z axes; b is the composite lift parameter; The state transition equation of the drone after t seconds S t (ω) is described as: in, is the initial state of the drone, is the new state of the drone at timestamp t; I x ,I y ,I z is the inertia of the drone rotating along the x, y, and z axes; U 2 ,U 3 ,U 4 are the rolling moment, pitch moment and yaw moment of the drone; I b is the moment of inertia of the propeller about each shaft axis; are the angular velocities of roll, pitch and yaw; The step 2 is specifically as follows: Step 2.1 Determine the spatial grid granularity Based on the force balance equation of the rotor UAV in step 1 and the UAV state transfer equation, determine the length, width, and height dimensions of the UAV unit airspace grid; Step 2.2 The airspace cannot be determined by grid After determining the basic airspace grid granularity, the airspace is rasterized, and then based on the GIS geographic information system and relevant airspace planning information, the building obstacles, airspace restricted areas / restricted areas in the drone flight airspace are determined and marked as unavailable airspace grids; Standard grid size of rotorcraft UAV at rated rotor speed of each coordinate axis for: ,θ∈{θ max ,i min },φ∈{φ max ,f min },ψ∈{ψ max ,ψ min } Among them, ω i is the angular velocity of the i-th propeller rotor, ω is a vector matrix composed of the four angular velocities of the propeller rotor of the UAV, ω=[ω 1 ,ω 2 ,ω 3 ,ω 4 ] T ; T is the set UAV unit running time; The step three is specifically as follows: Step 3.1 Identify the flight process of the drone Assume that the waypoints in the original flight plan of the UAV form an ordered list TP, TP = [[x 1 ,y 1 ],[x 2 ,y 2 ],…,[x n ,y n ]], the i-th planned waypoint is denoted as TP i =[x i ,y i ], let the current position of the drone be [x, y], then the segment TP to which the drone belongs m -TP n for: Step 3.2 UAV flight intention identification According to the current UAV operation information, determine the UAV's flight plan process and the next target waypoint, and discuss the following situations: Case ①: If the current drone position satisfies formula (11), the next target waypoint next_point is: next_point=Tp (i+1) Case ②: If the current UAV position and a planned waypoint do not satisfy formula (11), the next position [x', y'] is predicted based on the current UAV position [x, y], and the next target waypoint next_point is: next_point=argmin{||(x',y'),(x i+1 ,y i+1 )||,||(x',y'),(x i ,y i )||} Step 3.3 Change the track intention when the drone leaves the waypoint determination Assume that the final destination of the drone after changing its trajectory is D, D = [x d ,y d ], the set of waypoints in the airspace is P, P = [P 1 ,P 2 ,…,P n ], select the airspace waypoint closest to the destination position D as the exit waypoint P * : P * =argmin{||P k -D||},k∈{1,2,...,n} 2. According to claim 1, a trajectory planning method based on the dynamic intention of a UAV in a solidified and free mixed airspace, It is characterized in that The step one specifically includes: determining the state transfer equation of the drone model based on the control mode and control model of the rotor drone and the specific force balance equation of the drone model.
3. According to claim 1, a trajectory planning method based on the dynamic intention of a UAV in a solidified and free mixed airspace, Features: The steps four and five are specifically as follows: first, the optimal network search path planning in the fixed airspace of the rotary-wing UAV based on the Dijkstra algorithm is adopted for the fixed route / route airspace, and then the A* algorithm is adopted to plan the trajectory of the rotary-wing UAV in the free airspace environment from the waypoint to the final destination.
