Trajectory planning method, terminal and storage medium based on time-optimal motion primitive

Abstract

The present invention discloses a trajectory planning method, terminal and storage medium based on time-optimal motion primitives, which relates to the field of drone navigation. The method includes determining whether a trajectory switching condition occurs; when the trajectory switching condition occurs, obtaining the current motion speed of the drone; selecting a target trajectory that meets preset conditions from a motion primitive library, and switching from the current trajectory to the target trajectory. The trajectory in the motion primitive library of the present invention is determined by taking the shortest path consumption time as the goal, the terminal speed as zero, and the drone speed and acceleration limit range as constraints. The drone is controlled to move forward along the above trajectory and can reach the destination in the shortest time. In addition, the preset conditions set include: the starting speed is the same as the current motion speed, the direction cost is minimized, and the current trajectory is switched to a trajectory that meets the above preset conditions through obstacle safety detection, so that the destination can be reached at the minimum cost while ensuring safety.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) navigation, and in particular to a trajectory planning method, terminal and storage medium based on time-optimal motion primitives. Background Art

[0002] Drones, including multi-rotor, fixed-wing, and hybrid-wing drones, offer advantages such as high maneuverability, flexibility, simple structure, low cost, high concealment, and high security. Drones play an important role in civilian applications such as express delivery. They are also widely used in the military for surveillance, reconnaissance, and tracking. Drone applications typically occur over vast areas and in complex environments. Therefore, quickly and accurately planning a safe trajectory is a key research area for drone applications.

[0003] Existing online / real-time UAV trajectory planning methods do not consider the time optimality of the trajectory while ensuring safety. Summary of the Invention

[0004] The purpose of the present invention is to provide a trajectory planning method, terminal and storage medium based on time-optimal motion primitives, which can reach the target destination of a drone in the shortest time while ensuring safety.

[0005] To achieve the above object, the present invention provides the following solutions:

[0006] A trajectory planning method based on time-optimal motion primitives, the method comprising:

[0007] Determining whether a track switching condition occurs; the track switching condition includes at least one of satisfying a preset switching time and an obstacle appearing on the current track;

[0008] When the trajectory switching condition occurs, obtain the current movement speed of the drone;

[0009] A target trajectory that meets preset conditions is selected from a motion primitive library, and the current trajectory is switched to the target trajectory; the preset conditions include: the starting speed is the same as the current motion speed, the heading cost is minimized, and obstacle safety detection is passed; the motion primitive library includes multiple trajectories, each of which is composed of a corresponding path and the speed of each discrete point on the corresponding path; each trajectory has a different starting speed, the same starting motion direction, and a terminal speed of zero and the same trajectory length; the speed of each discrete point on the trajectory is determined by the following method: the path consumption time is minimized as the goal, the terminal speed is zero, and the UAV speed and acceleration limit range are constrained.

[0010] Optionally, the orientation cost is determined by an orientation factor, where the orientation factor includes a distance factor, and the distance factor is used to represent the difference between the distance between the end position of the trajectory and the destination and the distance between the initial position of the trajectory and the destination.

[0011] Optionally, the orientation factor includes a direction factor, and the direction factor represents the relationship between the end position of the trajectory and a specified boundary.

[0012] Optionally, before selecting a target trajectory that meets preset conditions from the motion primitive library, the method further includes:

[0013] Collect point clouds of obstacles in front of the drone;

[0014] Based on the obstacle point cloud, obstacle safety detection is performed on each trajectory in the motion primitive library.

[0015] Optionally, also include:

[0016] Collect obstacle point clouds in front of the drone in real time;

[0017] Based on the obstacle point cloud, obstacle safety detection is performed on the current trajectory.

[0018] Optionally, obstacle safety detection methods include:

[0019] Divide the actual space occupied by the trajectory into virtual grids of equal size;

[0020] Determine whether the trajectory intersects with a virtual sphere, and obtain an intersecting virtual grid; the virtual sphere is a three-dimensional sphere with the center of each virtual grid as the sphere center and a preset distance as the radius;

[0021] Determining whether the intersecting virtual grid intersects with the obstacle point cloud in front of the UAV;

[0022] If none of the intersecting virtual grids corresponding to the trajectory intersects with the point cloud of the obstacle in front of the drone, it is determined that the trajectory passes the obstacle safety detection.

[0023] Optionally, the method further includes: constructing the motion primitive library:

[0024] Generate multiple arcs; each of the arcs has the same length and a different radius;

[0025] Different starting angles are selected from a preset angle range, and the arc is controlled to rotate at the starting angle to obtain a path corresponding to the rotated arc; multiple paths constitute the path library.

[0026] Optionally, the step of constructing the motion primitive library further includes:

[0027] Uniformly discretizing the paths in the path library to obtain a plurality of discrete paths, each of which is composed of N discrete points;

[0028] Adding dynamic constraints at the discrete points; the dynamic constraints include the velocity constraint, acceleration constraint and combined velocity constraint of the UAV; the combined velocity constraint is the constraint on the sum of the velocities of the UAV in the three-axis direction in the UAV coordinate system;

[0029] Calculate the speed limit interval for each discrete point based on the terminal speed of the path, the acceleration constraint of the drone, the path length, and the number of discrete points in each path; the terminal speed of the path is 0;

[0030] Calculate the distance between two adjacent discrete points based on the path length and the number of discrete points in the path;

[0031] Calculate the running time of the UAV between the two adjacent discrete points based on the speed limit interval of the discrete points and the distance between the two adjacent discrete points;

[0032] The dynamic parameters with the shortest running time are selected to parameterize the corresponding discrete points to obtain the trajectories corresponding to the paths.

[0033] The present invention also provides a trajectory planning terminal based on time-optimal motion primitives, comprising a memory, a processor, and a computer program stored in the memory and capable of running a trajectory planning method based on time-optimal motion primitives on the processor. When the computer program is executed by the processor, the steps of the above-mentioned trajectory planning method based on time-optimal motion primitives are implemented.

[0034] The present invention also provides a computer-readable storage medium storing a program that can be loaded and executed by a processor to implement the above-mentioned trajectory planning method based on time-optimal motion primitives.

[0035] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0036] The trajectory planning method, terminal, and storage medium based on time-optimal motion primitives provided by the present invention select a target trajectory from a motion primitive library that meets preset conditions when a trajectory switching condition arises, and then switch from the current trajectory to the target trajectory. Because the trajectories in the motion primitive library of this application are determined by minimizing path time, with zero terminal velocity, and the UAV's speed and acceleration limits as constraints, controlling the UAV along the aforementioned trajectory can achieve the shortest possible time to reach the destination. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0038] Figure 1 Flowchart of the trajectory planning method based on time-optimal motion primitives provided by the present invention;

[0039] Figure 2 This is a technical architecture diagram of the trajectory planning method provided by the present invention;

[0040] Figure 3 A path generation diagram provided by the present invention;

[0041] Figure 4 Path parameterization algorithm diagram provided by the present invention;

[0042] Figure 5 A schematic diagram of rapid collision detection provided by the present invention;

[0043] Figure 6 This is a schematic diagram of the collision check time results provided by the present invention;

[0044] Figure 7 A visual comparison chart of the trajectories of different solutions provided by the present invention;

[0045] Figure 8 A comparison chart of the calculation speeds of different solutions provided by the present invention;

[0046] Figure 9 A comparison chart of the comprehensive quality of trajectories of different solutions provided by the present invention. DETAILED DESCRIPTION

[0047] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0048] The purpose of the present invention is to provide a trajectory planning method, terminal and storage medium based on time-optimal motion primitives. By obtaining a path primitive library containing time-optimal trajectories and, when a trajectory switching condition occurs, switching the current trajectory to a target trajectory in the motion primitive library that meets preset conditions, the destination can be reached in the shortest time and the shortest path while ensuring safety.

[0049] Before introducing the specific solutions of the present invention, it should be noted that the potential application industries and typical products or scenarios of the solutions of the present invention include:

[0050] 1. Scheduling and planning safe flight routes for aerial cargo transport over kilometers, hundreds of meters, or even short distances within large-scale city-level or medium-sized township-level environmental maps. Known related industry products include Amazon PrimeAir aerial logistics drones and their supporting planning systems, Meituan delivery drones and their upper-level scheduling systems, and FedEx's express hybrid wing transport system.

[0051] 2. Autonomous real-time online navigation and obstacle avoidance systems for drone aerial tracking, photography, environmental reconstruction, or industrial inspection in known or unknown natural and built environments. Related industry products include DJI's tracking and photography, Skydio's navigation functions for industrial inspection and reconstruction, and Sony's aerial photography.

[0052] Existing UAV trajectory planning methods include motion primitives. These methods generate multiple trajectories and, based on different sampling methods, can find a feasible trajectory within a deterministic timeframe. We summarize the work in this area, which primarily includes methods based on position space, state space, velocity space, and control space.

[0053] Ji Zhang et al. proposed a motion primitive library that samples fixed-length paths offline in position space. This library has fast collision detection, but its paths lack kinematic information, making it difficult to fully utilize the robot's maneuverability.

[0054] Among state-space-based methods, Muller et al. proposed online sampling of different local terminal states (position, velocity, acceleration), combining the starting state with a fixed time period to fit a min-jerk trajectory. However, the trajectory does not necessarily satisfy the robot's kinematic constraints, requiring back-end kinematic feasibility checks. Ryll et al. successfully applied Muller's method to autonomous navigation of quadrotors, which can adjust the sampled local terminal states according to obstacle density, but requires high-frequency replanning to ensure trajectory safety. Bucki et al. proposed a new pyramid-based partitioning method that can speed up collision detection and save computing power, but it results in a more conservative trajectory and poor performance in scenes with dense obstacles.

[0055] In the method based on velocity space sampling, Xuning Yang et al. defined the velocity space a = {v x , v y ,w}, where v xis the velocity along the x-axis, v2 is the velocity along the z-axis, and w is the angular velocity along the z-axis. Different local end states are sampled online in velocity space and combined with the starting state to generate an 8th polynomial trajectory for a fixed time period. The trajectory position information is then integrated. Based on Xuning Yang's method, Collins et al. proposed an adaptive method for sampling end states based on the starting velocity. This method improves the reliability of the trajectory library, but the trajectory selection method requires more computing power.

[0056] In the control space-based method, Florence et al. proposed online sampling of a set of constant control variables, and forward integration of trajectories over a fixed time (constant jerk in the first third and constant acc in the last two thirds) to form a trajectory library, which naturally satisfies the robot's kinematic constraints. However, the trajectory library's coverage of the space varies.

[0057] None of the above methods consider the time optimality of the trajectory. State, velocity, and control space-based methods typically require sampling all motion primitives to check for collisions, resulting in slow collision detection. The present invention considers the kinematic information and time optimality of the trajectory, naturally meeting the robot's kinematic constraints while also facilitating rapid collision detection.

[0058] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0059] Example 1

[0060] like Figure 1 As shown, this embodiment provides a trajectory planning method based on time-optimal motion primitives, including:

[0061] S1. Determine whether a track switching condition occurs; the track switching condition includes at least one of satisfying a preset switching time and an obstacle appearing on the current track.

[0062] S2. When the trajectory switching condition occurs, obtain the current movement speed of the drone.

[0063] S3. Select a target trajectory that meets preset conditions from the motion primitive library and switch from the current trajectory to the target trajectory; the preset conditions include: the starting speed is the same as the current motion speed, the heading cost is minimized, and obstacle safety detection is passed; the motion primitive library includes multiple trajectories, and the trajectory consists of a corresponding path and the speed of each discrete point on the corresponding path; the starting speed of each trajectory is different, the starting motion direction is the same, the terminal speed of each trajectory is zero, and the trajectory length is the same; the speed of each discrete point on the trajectory is determined by the following method: with the shortest path consumption time as the goal, the terminal speed is zero, and the drone speed and acceleration limit range are constrained.

[0064] The technical architecture diagram of this solution can be found in Figure 2 The "sliding point cloud stack - fixed number of point clouds - time-optimal motion primitive library - fast collision detection - trajectory selection - transformation relationship" section in the figure represents the (re)-planning system. Sensors (depth cameras / lidar) and vision / laser inertial navigation state estimation modules serve as input to the (re)-planning system.

[0065] The technical architecture diagram above is mainly divided into four steps: the time-optimal motion primitive library generates Nt time-optimal trajectories offline; the sliding point cloud stack stores the latest Nf frames of point cloud. While ensuring the fidelity of the environment, a random sampling method is further used to sample a fixed number of point clouds. A fast collision check method removes unsafe trajectories from the motion primitive library; the trajectory with the lowest cost is selected based on user-defined requirements and the current speed of the drone; finally, the receding horizon optimization strategy continuously obtains the trajectory with the lowest cost converted to the world system for execution.

[0066] As an optional implementation, the trajectory planning method based on time-optimal motion primitives further includes constructing the motion primitive library.

[0067] It should be noted that the motion primitive library in this solution is generated in an offline manner. Since online calculation is not required, the amount of calculation can be reduced.

[0068] The process of constructing the motion primitive library includes two processes: constructing a path library and parameterizing the path.

[0069] Among them, constructing the path library includes: generating multiple arcs; each of the arcs has the same length and a different radius; selecting different starting angles from a preset angle range, and controlling the arc to rotate at the starting angle to obtain the path corresponding to the rotated arc; multiple paths constitute the path library.

[0070] Specifically, the generation of the path library requires the construction of a cluster of geometric paths (without dynamic information). Figure 3 (a) Here is a specific example to illustrate: set 7 arcs with different radius r = {6; 8; 12; 20; 36; 78; ∞}m (each length is 5m). Then, rotate them with different starting angles θ = {0°; -10°; -20°; 0°; -10°; -20°; \} and 30° to obtain 73 paths (such as Figure 3 (b)).

[0071] The purpose of constructing the path library (73 paths) is to provide input conditions for subsequent path parameterization. Path parameterization is to assign optimal time to a fixed path while satisfying the UAV's dynamic constraints, thereby obtaining the corresponding trajectory (a path that contains dynamic information).

[0072] Optionally, the process of path parameterization includes: uniformly discretizing the paths in the path library to obtain multiple discrete paths; each discrete path consists of N discrete points; adding dynamic constraints to the discrete points; the dynamic constraints include the speed constraint, acceleration constraint and combined speed constraint of the drone; the combined speed constraint is the constraint on the sum of the three-axis speeds of the drone in the drone coordinate system; calculating the speed limit range of each discrete point based on the terminal speed of the path, the acceleration constraint of the drone, the path length and the number of discrete points in each path; the terminal speed of the path is 0; calculating the distance between two adjacent discrete points based on the path length and the number of discrete points in the path; calculating the running time of the drone between two adjacent discrete points based on the speed limit range of the discrete points and the distance between the two adjacent discrete points; selecting the dynamic parameters with the shortest running time to parameterize the corresponding discrete points to obtain the trajectory corresponding to each path.

[0073] The above path parameterization process can be summarized into four steps: path discretization, constraint construction, control set calculation and time-optimal trajectory generation.

[0074] To facilitate understanding by those skilled in the art, the above four steps are briefly summarized as follows: Path discretization: uniformly sample N+1 discrete points along a path according to distance; Constraint construction: add velocity, acceleration, and total velocity constraint formulas (10)(12)(14) at each discrete point; Control set calculation: calculate the velocity from the end point of the path (velocity Known) According to formula (16) (Formula 16 must satisfy the relationship between formulas 2, 10, 12 and 14, corresponding to (16b)(16c)(16d)(16e) respectively), the speed interval at each discrete point is calculated backward (N->0), and the set of all discrete point speed intervals is the control set; Time optimal trajectory generation: From the starting point of the path (speed Known) According to formula (17) (Formula 17 must satisfy the relationship between formulas 10, 12 and 14, the control set, corresponding to (17b)(17c)(17d)(17e) respectively), the speed at each discrete point is calculated forward (0->N); the speed at each discrete point is obtained. Since the path is uniformly discretized (the distance between each discrete point is known), the time t between each discrete point can be calculated, that is, the time parameterization is completed, and the trajectory containing the optimal time allocation is obtained; (Here is an explanation: the path does not contain time information, and the trajectory contains time information). The algorithm flow chart of this part refers to Figure 4 .

[0075] Specifically, path discretization: adopt a high-precision interpolation scheme to uniformly discretize the path q(s)∈R with an interval Δ n (path parameter s:[0,1]→[s0,s N ]) is N+1 discrete points.

[0076] s0, s1...s N-1 , s N . (1)

[0077] Δ=(s N -s0) / N

[0078] Discretization results in: N+1 discrete points (that is, N+1 points are uniformly sampled on a path). The purpose of obtaining discrete points is to add dynamic constraints to the drone at these discrete points.

[0079] In the interval [S i ,S i+1 ], the velocity and acceleration of the ith discrete point are There are the following relationships:

[0080]

[0081] Constraint construction: Add constraints to each discrete point on the path. The general second-order constraint form is as follows:

[0082]

[0083] The differential path q(s) is:

[0084]

[0085] q′ represents the differential with respect to the parameter s, Represents the differential with respect to time.

[0086] Substituting equation (4) into equation (3) yields:

[0087]

[0088] in,

[0089] a(s):=A(q(s))q′(s), (6a)

[0090] b(s):=A(q(s))q"(s)+q′(s)TB(q(s))q′(s), (6b)

[0091] c(s):=f(q(s)), (6c)

[0092]

[0093] It should be noted that: Formulas 4, 5, 7, 8, and 9 are general (universal) constraint expressions about s;

[0094] The logical relationship is: Formulas 4, 5, 7, 8, and 9 are the constraints corresponding to s, and the constraints of velocity, acceleration, and total velocity need to be converted into expressions about s. Therefore, the following are to substitute Formulas 4, 5, 7, 8, and 9 into Formulas 10, 12, and 14 to obtain expressions about s; the results after substitution can be obtained by substituting Formulas 11, 13, and 15 back into Formulas 4, 5, 7, 8, and 9).

[0095] Considering the optimization variables of the LP optimization problem of formulas 16 and 17, (Expression about s). Only when the expression of variables is consistent can the optimization problem be solved. Therefore, constraints 10, 12, and 14 are converted into expressions about s.

[0096] Because in the optimization problem, the optimization variables are set to The linear constraints are constructed as

[0097]

[0098] According to the speed limit of drone [v min ,v max ], acceleration limit [a min ,a max ], and the maximum resultant speed v norm , the corresponding speed constraints are as follows:

[0099]

[0100] Substituting equations (4) and (8) into (10), we have

[0101]

[0102] The corresponding acceleration constraints are as follows:

[0103]

[0104] Substituting equations (4), (5), and (7) into (12), we have

[0105]

[0106] The corresponding resultant velocity constraints are as follows:

[0107]

[0108] Substituting equations (4), (5), and (7) into (14), we have

[0109]

[0110] Control set calculation: According to the reachability analysis, find each discrete point s i The feasible speed square interval at Able to square the speed from the end point on the path The backward (N→0) sequence is calculated. For example, As we know, we can use linear programming problem (LPproblem) to solve

[0111]

[0112] The set of squared velocities at each discrete point is defined as the control set K.

[0113] Time-optimal trajectory generation: Calculate S for each discrete point i The square of the maximum feasible speed

[0114] At each discrete point, The square of the speed from the starting point on the path It is calculated in the forward (0→N) sequence.

[0115] For example, We know that we can use linear programming to solve

[0116]

[0117] Find the average speed on the interval by using all maximum possible speeds The optimal set at time t is further solved. Then, the parameterized path q(s) is used to obtain the time-optimal trajectory q(s(t)).

[0118] The satisfaction error of the interpolation scheme of this method is O(Δ 2 ), which is the same as the number of discrete points N dp It is relevant. You can set N dp = 1000 to balance the efficiency and accuracy of the solution. Seidel algorithm is used to solve equations (16) and (17).

[0119] For each sampled path, set the same terminal speed and change the starting speed To generate a library of motion primitives that has good spatial distribution, considers temporal optimality, and satisfies the UAV's dynamic constraints.

[0120] After the motion primitive library is generated offline, the subsequent trajectory planning needs to switch the target trajectory that meets the preset switching conditions selected from the motion primitive library. Before switching, the trajectory needs to be safety checked.

[0121] As an optional implementation, before S3, it also includes: collecting an obstacle point cloud in front of the drone; and performing obstacle safety detection on each trajectory in the motion primitive library based on the obstacle point cloud.

[0122] For details, please refer to Figure 2 This solution uses sensors (depth cameras / radars) to collect obstacle point clouds in the environment ahead of the drone at a preset frequency. The latest Nf frames of obstacle point clouds are stored in a sliding point cloud stack. While ensuring environmental fidelity (no distortion), a random sampling algorithm is used to obtain a fixed number of point clouds based on the latest n frames of obstacle point clouds. This fixed number of point clouds is used for subsequent trajectory collision detection.

[0123] It should also be noted that the presence of obstacles on the current trajectory mentioned in step S1 is also determined through trajectory collision detection. Therefore, in some embodiments, the trajectory planning method based on time-optimal motion primitives also includes: real-time acquisition of obstacle point clouds in front of the drone; and obstacle safety detection of the current trajectory based on the obstacle point clouds.

[0124] Optionally, the obstacle safety detection includes: dividing the actual occupied space of the trajectory into virtual grids of the same size; judging whether the trajectory intersects with a virtual sphere to obtain an intersecting virtual grid; the virtual sphere is a three-dimensional sphere with the center of each virtual grid as the center and a preset distance as the radius; judging whether the intersecting virtual grid intersects with the obstacle point cloud in front of the UAV; if none of the intersecting virtual grids corresponding to the trajectory intersects with the obstacle point cloud in front of the UAV, it is determined that the trajectory passes the obstacle safety detection.

[0125] To ensure safe navigation of drones in unknown environments, fast collision checking is essential. Typical collision checking methods fuse obstacle point clouds into a grid map or KD-Tree, then sample discrete points on each motion primitive to check for collisions. Existing methods directly sample points on the motion primitives and query whether a collision has occurred at those points to determine whether the motion primitives collide. Therefore, as the sampling resolution of the motion primitives increases, the time required also increases. Consequently, the collision checking time of these methods is non-deterministic, as it depends on the sampling resolution of the motion primitives.

[0126] This embodiment proposes a collision check method with deterministic time. Its time complexity is independent of the sampling resolution of the motion primitive. Specifically, the principle of the collision check method in this embodiment is only related to the number of collected point clouds. The number of collected point clouds is fixed, so the time is deterministic and can ensure that there is a certain safety gap between the offline expansion motion primitive and the obstacle.

[0127] For details on the collision detection principle, please refer to Figure 5 In the figure, ① all motion primitives are constructed offline as a KD-Tree, and the local space of the motion primitives is divided into virtual grids of fixed size. ② The spatial occupancy relationship of each grid relative to the motion primitive can be pre-calculated by querying the KD-Tree within a specific distance d from the center of each grid.

[0128] The role and logical relationship of collision detection is: as long as the online visual sensor obtains the position of the obstacle point cloud, it can determine whether the (motion primitive / path) collides with the obstacle based on the occupancy relationship, that is, the (motion primitive / path) corresponding to the grid where the obstacle point cloud is located will collide.

[0129] Unsafe motion primitives are removed in batches, only by a fixed number of point clouds falling into the corresponding grid. Therefore, the collision check time is only related to the number of point clouds and has nothing to do with the sampling resolution of the motion primitives. The present invention reduces the number of sampled point clouds to a fixed number N pc , so that collision checking can be completed in deterministic time.

[0130] In order to ensure the safety of the drone, it is necessary to maintain a certain safety gap with obstacles. In this embodiment, only the query distance d is d+r inflated (r inflated is a safety gap).

[0131] Obstacle safety detection obtains a safe trajectory. Trajectory selection is performed based on the obtained safe trajectory and user-defined requirements to obtain the optimal trajectory and hand it over to the flight control for execution.

[0132] After excluding unsafe trajectories, the cost function c equation (18) of each trajectory needs to be constructed using user-defined requirements.

[0133] The cost function described here has the same meaning as the orientation cost in step S3.

[0134] As an optional implementation, the orientation cost is determined by an orientation factor, where the orientation factor includes a distance factor, and the distance factor is used to represent the difference between the distance between the end position of the trajectory and the destination and the distance between the initial position of the trajectory and the destination.

[0135] As another optional implementation, the orientation factor includes a direction factor, and the direction factor represents the relationship between the end position of the trajectory and the specified boundary.

[0136] Specifically, the above-mentioned orientation cost can also be calculated by a cost function, which includes a target cost and a boundary cost. The target cost represents the target cost c goal The end position of the trajectory is as close as possible to the global target p goal . Boundary cost c bound The end position of the representative trajectory cannot be outside the specified boundary B.

[0137] c=λ g c goal +λc bound (18a)

[0138] c goal =||p end -p goal |-|p start -p goal || (18b)

[0139]

[0140] Among them, λ g ,λ b is the corresponding weight. start Represents the current position of the drone.

[0141] It should also be noted that, in addition to the above-mentioned target cost and boundary cost, users may also have other desired requirements, which can be achieved by extending the cost function c.

[0142] After constructing the cost function for all safe trajectories, the trajectory with the minimum cost is selected based on the current speed of the drone, which is used as the starting speed of the trajectory.

[0143] Because steps S1-S3 above are a continuous loop, a rolling optimization planning strategy is used with a replanning frequency of 10 Hz. During each replanning, the drone's current state is used as the starting state for the next trajectory, so the trajectory that matches the drone's current velocity is selected. In practice, the trajectory is kept tangential to the drone's current velocity.

[0144] It should also be noted that since the trajectory in the trajectory library is in the velocity system, we need to perform it in the world system, so we need to perform coordinate conversion to convert the trajectory from the velocity system {V} (drone coordinate system) to the world coordinate system {W}. The conversion relationship is:

[0145] x axis =v current / ||vcurrent ||,(19a)

[0146] y axis =x axis ×(0,0,-1) T ,(19b)

[0147] z axis =x axis ×y axis ,(19c)

[0148] R WV =[x axis ,y axis ,z axis ],(19d)

[0149] T EV =[R WV ;p start ].(19e)

[0150] The technical effects of this solution are as follows:

[0151] 1. The planned trajectory is time-optimal and dynamically feasible. Since the trajectories in the motion primitive library of this application are determined by minimizing the path consumption time, with the terminal velocity being zero and the UAV speed and acceleration limit ranges as constraints, the UAV can be controlled to move along the above trajectory to reach the destination in the shortest time.

[0152] 2. Since the motion primitive library is generated offline, this solution can generate high-quality trajectories with minimal online computing power consumption;

[0153] 3. The time complexity is independent of the sampling resolution of the motion primitive, and the trajectory can maintain a safe gap with obstacles by offline dilating the motion primitive.

[0154] Through simulation of this solution, it is verified that the collision check time of the present invention is basically maintained at the level of 3.2-3.6ms, and will not change significantly with the increase of the number of motion primitives. Figure 6 .

[0155] To further illustrate the effect of this solution, the present invention is compared with the existing solution. Figure 7-Figure 9 , Mapless is the solution in [1], EGO-Planner-v2 is the solution in [2], and Proposed is the solution in the present invention. The present invention can achieve the lowest online computing power consumption (single frame environment representation time t env +Calculate the time t for a single trajectory com ) Total generation time (t total ) shortest, total distance (d total)The shortest trajectory.

[0156] Example 2

[0157] This embodiment provides a trajectory planning terminal based on time-optimal motion primitives, including a memory, a processor, and a computer program stored in the memory and capable of running a trajectory planning method based on time-optimal motion primitives on the processor. When the computer program is executed by the processor, the steps of the trajectory planning method based on time-optimal motion primitives in the above-mentioned embodiment 1 are implemented.

[0158] Example 3

[0159] This embodiment provides a computer-readable storage medium storing a program that can be loaded and executed by a processor to implement the trajectory planning method based on the time-optimal motion primitive in the above-mentioned embodiment 1.

[0160] The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. For similar or identical parts between the various embodiments, reference can be made to each other. For the terminals disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple. For relevant parts, refer to the method description.

[0161] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The above examples are only intended to help understand the method and core concept of the present invention. At the same time, those skilled in the art will find that the specific implementation methods and application scopes may vary based on the concept of the present invention. In summary, the contents of this specification should not be construed as limiting the present invention.

Claims

1. A trajectory planning method based on time-optimal motion primitives, characterized in that: The method comprises: Determining whether a track switching condition occurs; the track switching condition includes at least one of satisfying a preset switching time and an obstacle appearing on the current track; When the trajectory switching condition occurs, obtain the current movement speed of the drone; A target trajectory that meets preset conditions is selected from a motion primitive library, and the current trajectory is switched to the target trajectory; the preset conditions include: the starting speed is the same as the current motion speed, the heading cost is minimized, and obstacle safety detection is passed; the motion primitive library includes multiple trajectories, each of which is composed of a corresponding path and the speed of each discrete point on the corresponding path; each trajectory has a different starting speed, the same starting motion direction, and a terminal speed of zero and the same trajectory length; the speed of each discrete point on the trajectory is determined by the following method: the path consumption time is minimized as the goal, the terminal speed is zero, and the UAV speed and acceleration limit range are constrained.

2. The trajectory planning method based on time-optimal motion primitives according to claim 1, characterized in that: The orientation cost is determined by an orientation factor, where the orientation factor includes a distance factor, and the distance factor is used to represent the difference between the distance between the end position of the trajectory and the destination and the distance between the initial position of the trajectory and the destination.

3. The trajectory planning method based on time-optimal motion primitives according to claim 2, characterized in that: The orientation factor includes a direction factor, and the direction factor represents the relationship between the end position of the trajectory and the prescribed boundary.

4. The trajectory planning method based on time-optimal motion primitives according to claim 1, characterized in that: Before selecting a target trajectory that meets preset conditions from the motion primitive library, the method further includes: Collect point clouds of obstacles in front of the drone; Based on the obstacle point cloud, obstacle safety detection is performed on each trajectory in the motion primitive library.

5. The trajectory planning method based on time-optimal motion primitives according to claim 1, characterized in that: Also includes: Collect obstacle point clouds in front of the drone in real time; Based on the obstacle point cloud, obstacle safety detection is performed on the current trajectory.

6. The trajectory planning method based on time-optimal motion primitives according to claim 4 or 5, characterized in that: Obstacle safety detection methods include: Divide the actual space occupied by the trajectory into virtual grids of equal size; Determine whether the trajectory intersects with a virtual sphere, and obtain an intersecting virtual grid; the virtual sphere is a three-dimensional sphere with the center of each virtual grid as the sphere center and a preset distance as the radius; Determining whether the intersecting virtual grid intersects with the obstacle point cloud in front of the UAV; If none of the intersecting virtual grids corresponding to the trajectory intersects with the point cloud of the obstacle in front of the drone, it is determined that the trajectory passes the obstacle safety detection.

7. The trajectory planning method based on time-optimal motion primitives according to claim 1, characterized in that: Also includes: Build the motion primitive library: Generate multiple arcs; each arc has the same length and a different radius; Different starting angles are selected from a preset angle range, and the arc is controlled to rotate at the starting angle to obtain a path corresponding to the rotated arc; multiple paths constitute a path library.

8. The trajectory planning method based on time-optimal motion primitives according to claim 7, characterized in that: The construction of the motion primitive library further includes: Uniformly discretizing the paths in the path library to obtain a plurality of discrete paths, each of which is composed of N discrete points; Adding dynamic constraints at the discrete points; the dynamic constraints include the velocity constraint, acceleration constraint and combined velocity constraint of the UAV; the combined velocity constraint is the constraint on the sum of the velocities of the UAV in the three-axis direction in the UAV coordinate system; Calculate the speed limit interval for each discrete point based on the terminal speed of the path, the acceleration constraint of the drone, the path length, and the number of discrete points in each path; the terminal speed of the path is 0; Calculate the distance between two adjacent discrete points based on the path length and the number of discrete points in the path; Calculate the running time of the UAV between the two adjacent discrete points based on the speed limit interval of the discrete points and the distance between the two adjacent discrete points; The dynamic parameters with the shortest running time are selected to parameterize the corresponding discrete points to obtain the trajectories corresponding to the paths.

9. A trajectory planning terminal based on time-optimal motion primitives, characterized in that: The invention comprises a memory, a processor, and a computer program stored in the memory and capable of running on the processor for a trajectory planning method based on a time-optimal motion primitive, wherein when the computer program is executed by the processor, the steps of the trajectory planning method based on a time-optimal motion primitive are implemented.

10. A computer-readable storage medium, characterized in that The device stores a program that can be loaded and executed by a processor to implement the trajectory planning method based on the time-optimal motion primitive according to any one of claims 1 to 8.

Images