WO2022127737A1 - Trajectory planning method and apparatus, trajectory planner, unmanned aerial vehicle, and storage medium - Google Patents

Abstract

A trajectory planning method, a trajectory planning apparatus (500), a trajectory planner (50), an unmanned aerial vehicle (600), and a storage medium. The trajectory planning method comprises: determining global trajectory points of a job route (S110); using a starting point of the global trajectory points as a starting point of a first preset length (S120); screening the global trajectory points for trajectory points covered by the first preset length, determining local trajectory points based on the screened trajectory points, and generating a local trajectory based on the local trajectory points (S130); controlling the unmanned aerial vehicle to move by a second preset length based on the local trajectory (S140); determining whether all the global trajectory points have been screened (S150); if yes, controlling the unmanned aerial vehicle to continue moving along the local trajectory to the last trajectory point (S160); if not, using the next trajectory point of the current position of the unmanned aerial vehicle as a starting point of the first preset length (S170), and returning to the operation of screening the global trajectory points for the trajectory point covered by the first preset length until all global trajectory point are screened, wherein the first preset length is greater than the second preset length. Therefore, the unmanned aerial vehicle can accurately avoid obstacles encountered. The safety of the unmanned aerial vehicle is ensured, and the operation efficiency is improved.

Description

Trajectory planning method, device, trajectory planner, unmanned aerial vehicle and storage medium technical field

Embodiments of the present invention relate to the field of UAV path planning, and in particular, to a trajectory planning method, device, trajectory planner, UAV and storage medium.

Background of the Invention

In recent years, with the popularity of unmanned technology, drones have gradually been used in various fields. Among them, drones can be applied to various operations such as patrol inspection and spraying of route points. During the operation of the UAV, the path planning needs to be carried out in advance to obtain the path trajectory.

However, in the related art, during the actual operation of the UAV based on the planned path trajectory, if it encounters an obstacle, the UAV cannot avoid the obstacle well, which affects the safety of the UAV.

SUMMARY OF THE INVENTION

Embodiments of the present invention provide a trajectory planning method, a device, a trajectory planner, an unmanned aerial vehicle, and a storage medium, which can accurately avoid obstacles when encountering obstacles, ensure the safety of the unmanned aerial vehicle, and improve operation efficiency.

In a first aspect, an embodiment of the present invention provides a trajectory planning method, including:

Determine the global trajectory point of the operation route;

Take the starting point of the global trajectory point as the starting point of the first preset length;

Screening the trajectory points covered by the first preset length in the global trajectory points, determining the local trajectory points based on the screened trajectory points, and generating the local trajectory based on the local trajectory points;

Control the UAV to move the distance of the second preset length based on the local trajectory, take the next trajectory point of the current position of the UAV as the starting point of the first preset length, and return to filter the global trajectory points covered by the first preset length until the global trajectory point screening is completed, wherein the first preset length is greater than the second preset length.

In a second aspect, an embodiment of the present invention further provides a trajectory planning device, including:

The global trajectory point determination module is used to determine the global trajectory point of the operation route;

The starting point determination module is used for taking the starting point of the global trajectory point as the starting point of the first preset length;

The local trajectory generation module is used for screening the trajectory points covered by the first preset length in the global trajectory points, determining the local trajectory points based on the screened trajectory points, and generating the local trajectory based on the local trajectory points;

The control/return module is used to control the UAV to move the distance of the second preset length based on the local trajectory, take the next trajectory point of the current position of the UAV as the starting point of the first preset length, and return to filter in the global trajectory points The step of the track points covered by the first preset length is until the global track point screening is completed, wherein the first preset length is greater than the second preset length.

In a third aspect, an embodiment of the present invention provides a trajectory planner, including: the trajectory planning apparatus provided by the embodiment of the present invention.

In a fourth aspect, an embodiment of the present invention provides an unmanned aerial vehicle, including the trajectory planner provided by the embodiment of the present invention.

In a fifth aspect, an embodiment of the present invention provides a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, the trajectory planning method provided by the embodiment of the present invention is provided.

In a sixth aspect, an embodiment of the present invention provides an unmanned aerial vehicle, including: a processor; and a memory for storing instructions executable by the processor, where the processor is configured to execute the trajectory planning method provided by the embodiment of the present invention.

In the technical solution provided by the embodiment of the present invention, by determining the global trajectory point of the operation route, the starting point of the global trajectory point is used as the starting point of the first preset length, and the trajectory points covered by the first preset length are selected from the global trajectory points, Determine the local trajectory point based on the screened trajectory points, generate a local trajectory, control the UAV to move the second preset length based on the local trajectory, take the next trajectory point of the current position of the UAV as the starting point of the first preset length, and return to The operation of screening the trajectory points covered by the first preset length in the global trajectory points until the global trajectory point selection is completed; wherein, the second preset length is smaller than the first preset length. That is, the local trajectory is generated by the first preset length, the UAV is controlled to move the second preset distance based on the local trajectory, and then the local trajectory is regenerated based on the first preset length, so that in the case of encountering obstacles, it can be carried out in time. Obstacle avoidance processing to achieve precise obstacle avoidance and ensure the safety of UAVs.

Brief Description of Drawings

1a is a flowchart of a trajectory planning method provided by an embodiment of the present invention;

Figure 1b is a schematic diagram of a round-trip operation route;

Figure 1c is a schematic diagram of the operation route in the form of a curve;

1d is a schematic diagram of a working route provided by an embodiment of the present invention;

Figure 1e is a schematic diagram of the route point speed calculation;

Figure 1f is a schematic diagram of the global trajectory point;

1g is a schematic diagram of screening of local trajectory points in the case where there are no obstacles on the trajectory formed by the trajectory points covered by the first preset length according to an embodiment of the present invention;

1h is a schematic diagram of screening of local trajectory points in the case where there are no obstacles on the trajectory formed by the trajectory points covered by the first preset length according to an embodiment of the present invention;

2a is a flowchart of a trajectory planning method provided by another embodiment of the present invention;

Fig. 2b shows that there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is greater than the first preset distance used for obstacle avoidance. Schematic diagram of track point screening;

3a is a flowchart of a trajectory planning method provided by another embodiment of the present invention;

Fig. 3b shows that there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is smaller than the first preset distance used for obstacle avoidance, the local A schematic diagram of the trajectory point screening causing the UAV to fail to return to the original trajectory point;

Fig. 3c shows that there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is smaller than the first preset distance for obstacle avoidance, the local Schematic diagram of trajectory point screening;

4 is a structural block diagram of a trajectory planning device provided by an embodiment of the present invention;

FIG. 5 is a schematic structural diagram of a trajectory planner provided by an embodiment of the present invention.

FIG. 6 shows a block diagram of an unmanned aerial vehicle for executing a trajectory planning method provided by an exemplary embodiment of the present application.

MODES OF IMPLEMENTING THE INVENTION

The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention, but not to limit the present invention. In addition, it should be noted that, for the convenience of description, the drawings only show some but not all structures related to the present invention.

Fig. 1a is a flowchart of a trajectory planning method provided by an embodiment of the present invention. The method can be executed by a trajectory planning device, and the device can be implemented by software and/or hardware. The device can be configured in trajectory planning In the UAV, the trajectory planner can be configured on the UAV. The drone may be a plant protection drone, a surveying and mapping drone, an unmanned vehicle, or an unmanned boat. Optionally, the method provided by the embodiment of the present invention may be applied to a drone operation scenario.

As shown in Figure 1a, the technical solution provided by the embodiment of the present invention includes:

S110: Determine the global trajectory point of the operation route.

In this embodiment of the present invention, global trajectory planning can be performed first, a global trajectory including all route points can be planned, and the position and speed of the UAV at each moment can be preliminarily determined. Then, local trajectory planning is carried out. In the process of local trajectory planning, the trajectory planner uses the planned global trajectory as the basis, and combines the obstacle information obtained by the sensor to perform small-scale and small-scale trajectory planning. It should be noted that the operation route may be a flight route of an aircraft, or a travel route of a vehicle or a ship.

In the embodiment of the present invention, the operation route may be composed of several route points, and the drone needs to pass through these route points during the operation. Wherein, the route point may include position information and speed information, etc., wherein, the speed of the route point may be understood as the speed of the drone at the route point. For example, there are six route points in the work route shown in Figure 1b.

In this embodiment of the present invention, before the drone takes off, or before the drone has taken off but has not started the operation, the trajectory planner can obtain all the data of the operation route in advance, including the number of route points, the Coordinates, the maximum speed of each route point, etc., the trajectory planner can perform global trajectory planning based on the data of the operation route, obstacle maps, etc. Among them, the obstacle map can record geographically inaccessible areas and obstacles for drones. Wherein, the operation route formed by the route points may be a round-trip operation route as shown in FIG. 1b, and may also be in the form of a curve as shown in FIG. 1c. However, the form of the work route is not limited to the above-mentioned form, and may be a form of a 3D work route with a high degree of variation.

In order to simplify the description, it can be assumed that the number of route points of an operation route is 3, the schematic diagram of the operation route can be referred to Fig. 1d, and the horizontal flight of the drone is taken as an example. As shown in Figure 1d, in the process of global trajectory planning, speed allocation needs to be performed, that is, the UAV starts from route point 1 (t=0 at departure), passes through route point 2, and finally arrives at route point 3. It is necessary to determine the total cost The total time T of , in the interval t ∈ (0, T), the position and speed of the UAV need to be determined at each moment. For speed distribution (speed distribution at each moment), it is necessary to set the speed of the drone as it passes each route point. In the related art, when the UAV is flying based on the operation route, the speed of the UAV when passing through each route point is set to 0 m/s. Therefore, the drone needs to stop at each route point and then depart to the next route point, which affects the efficiency of the operation.

In the embodiment of the present invention, in order to improve the operation efficiency, the speed of the drone when passing the route point can be restricted from being 0, so that the movement of the drone is more coherent, and the global trajectory point is finally determined.

In an implementation of the embodiment of the present invention, optionally, determining the global trajectory points of the operation route includes: determining the maximum speed allowed for each route point based on the constraint conditions of the route points in the operation route, and using the maximum speed as the corresponding The speed of the route point; based on the speed and position of each two adjacent route points, determine the position and speed at any time in the road segment between each two adjacent route points, and sample the location of each road segment to obtain the global trajectory point.

Wherein, optionally, determining the maximum allowable speed of each route point based on the constraints of the route points in the operation route includes: determining the maximum speed among the speeds satisfying the following conditions as the maximum speed (the speed of the route points satisfies the following conditions):

The speed at each route point is less than or equal to the maximum speed allowed for the job;

The speed of the first route point and the last route point is 0;

For the speed of a route point, it should start from the speed of the previous route point and reach the full acceleration at the maximum acceleration allowed by the drone, or decelerate at the maximum acceleration allowed by the drone;

For the speed of a route point, it can decelerate to the next route point with the maximum acceleration allowed by the drone, or accelerate to the next route point with the maximum acceleration allowed by the drone;

The forward speed of the waypoint is determined based on the size of the corner.

The speed of the route point is constrained by the above constraint conditions, and the maximum value of the speeds satisfying the above constraint conditions is taken as the speed of the route point, which can improve the work efficiency.

In the embodiment of the present invention, optionally, the forward speed of the route point is determined based on the size of the turning angle, including: determining the forward speed of the route point based on the following formula:

Among them, _vx is the forward speed of the UAV, θ is the size of the turning angle, a ₀ is the acceleration of the UAV, and d is the distance from the route point when the UAV decelerates.

Specifically, taking the situation shown in Figure 1d as an example, if the vector a and the vector b are in the same direction, the UAV seems to be flying through a straight line, and the UAV should not stop at route point 2, and the route is determined according to the size of the corner. Constraints on the velocity of the point do not act as constraints. If the vector a and the vector b are reversed, the speed of the route point 2 needs to be limited to 0, because the speed of the drone needs to be turned 180° at this time. In the case of an included angle between vector a and vector b, it is not necessary to consider the position distribution of the three route points, and the angle formed by them can be squared, as shown in Figure 1e, the angle formed by the front and rear sections is the angle of θ, that is, the size of the rotation angle is θ. At this time, the drone flies along the left side of the angle, and the speed can be decomposed into the vertical speed of v _x and v _y . By the time the drone reaches the vertex, v _y should decrease to zero, while v _x remains constant throughout the drone's flight to and away from the vertex. Suppose the drone starts to decelerate when it is a distance d from the vertex, and then accelerates after passing the vertex. Since the velocity v _y in the y direction at the vertex is 0, then the velocity v _x in the x direction is the velocity allowed to pass through the vertex. It should be noted that the trajectory of the whole process is actually a parabola instead of the trajectory shown in Figure 1e, but it can be considered as the trajectory shown in Figure 1e when d is small.

Among them, it is assumed that the acceleration of the drone is a ₀ , and the time required for the speed in the y direction to decelerate from v _y to 0m/s is t, and t is also the time required for the drone to fly half d _x in the x direction. _vx can be obtained by the following formula:

From the above two formulas, we can get

In the embodiment of the present invention, after the speed of each route point is constrained by the above method, the maximum speed that meets the above constraint conditions is obtained, and the maximum speed is used as the speed of the corresponding route point. The straight-line trajectory planning can be performed on the road segment between every two adjacent route points by the method of trapezoidal speed distribution or the Double S speed distributor. That is, for the road segment formed by two adjacent route points, through the position and speed of the two route points, the time required to fly the road segment can be calculated, so that the position and speed of the UAV at any time in the road segment can be calculated, The position of the UAV is sampled according to the time interval T _s , and the global trajectory point is obtained. The schematic diagram of the global trajectory point can refer to Fig. 1f, the global trajectory point includes the position, speed and corresponding time information of each trajectory point. As shown in Figure 1f, the time interval corresponding to each trajectory point is T _s , therefore, the trajectory points with short distances before and after indicate low velocity, and the trajectory points with large distance indicate high velocity. As shown in Figure 1f, the UAV starts to accelerate from the starting point in the lower left corner. At the beginning, the trajectory points are relatively dense and the speed is small. When it reaches the middle of the first road segment, the trajectory points are relatively sparse and the speed is large; then the UAV decelerates. Reach the top, accelerate again after passing the top, slow down when it reaches the bottom right end point, and finally stop.

In one implementation of the embodiment of the present invention, optionally, a global trajectory may be planned based on the position information of the route points in the operation route and the information in the obstacle map, and the position sampling of the global trajectory may be performed to obtain the global trajectory points. . It should be noted that the manner of determining the global trajectory point is not limited to the above manner, and may also be other manners.

S120: Use the starting point of the global trajectory point as the starting point of the first preset length.

In this embodiment of the present invention, a collision-free and smooth local trajectory can be planned by using global trajectory points.

Specifically, the starting point of the global trajectory point may be used as the starting point of the first preset length. Wherein, the starting point of the global trajectory point may be the first trajectory point at the beginning of the global trajectory point. The first preset length may be set according to actual conditions.

S130: Screen the track points covered by the first preset length from the global track points, determine local track points based on the screened track points, and generate a local track based on the local track points.

In this embodiment of the present invention, the trajectory points covered by the first preset length may include, in the global trajectory, the trajectory points corresponding to the starting point of the first preset length, and the trajectory points located after the starting point of the first preset length and being the same as the first preset length. The distance between the starting points of a preset length is less than or equal to the trajectory points of the first preset length.

In this embodiment of the present invention, in the case where there is no obstacle on the trajectory formed by the trajectory points covered by the first preset length, that is, in the case where there is no obstacle within the distance range covered by the first preset length , the trajectory points covered by the first preset length can be used as local trajectory points, and the local trajectory can be generated based on the local trajectory points.

In this embodiment of the present invention, in the case where there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, that is, in the case where there is an obstacle within the distance range covered by the first preset length, and The distance between the obstacle and the end point of the first preset length is greater than the first preset distance, and the obstacle avoidance trajectory point can be determined based on the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle, and use The obstacle track point replaces the track point in the corresponding global track point, and the unreplaced track point and the obstacle avoidance track point among the track points covered by the first preset length are used as the local track point. Among them, the obstacle information includes the size and position information of the obstacle, and the obstacle information can be determined by the sensor data on the UAV. The first preset distance and the second preset distance may be preset, or may also be determined according to the current speed of the drone. For details of the contents of this part, reference may be made to the introduction of the following embodiments.

In this embodiment of the present invention, in the case where there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, that is, in the case where there is an obstacle within the distance range covered by the first preset length, and The distance between the obstacle and the end point of the first preset length is smaller than the first preset distance, and a third preset length can be added on the basis of the first preset length to obtain a fourth preset length, and the end point of the fourth preset length is the same as The distance of the obstacle is greater than the first preset distance. The obstacle avoidance trajectory point is determined according to the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle, and the obstacle avoidance trajectory point is used to replace the trajectory point in the corresponding global trajectory point; Unreplaced track points and obstacle avoidance track points in the track points covered by the length are used as local track points, and a local track is generated. The content of this part can refer to the introduction of the following embodiments.

S140: Control the drone to move a distance of a second preset length based on the local trajectory.

In one implementation of the embodiment of the present invention, controlling the UAV to move a distance of a second preset length based on the local trajectory includes: determining a control amount of the UAV based on the local trajectory; controlling the UAV along the local trajectory based on the control amount Move a distance of a second preset length. Wherein, the control amount may include information such as acceleration and flight angle. The independent variable of the local trajectory can be time, the velocity at any time can be determined based on the local trajectory, and the acceleration and flight angle can be calculated based on the velocity at any two adjacent moments. In the embodiment of the present invention, the local trajectory is determined, the control amount is determined based on the local trajectory, and the UAV is controlled to move along the local trajectory based on the control amount. Longer global trajectories are processed each time, reducing data processing.

In this embodiment of the present invention, the second preset length is less than the first preset length. Optionally, the second preset length may be less than half of the first preset length. In other examples, the first preset length and the first preset length Two preset lengths can be set as needed.

S150: Determine whether the global trajectory points are screened.

If yes, execute S160, if not, return to S170.

S160: Control the drone to continue to move along the local trajectory to the last trajectory point.

S170: Take the next trajectory point of the current position of the drone as the starting point of the first preset length, and return to S130.

In this embodiment of the present invention, if the first preset length covers the last track point of the global track point, the global track point is screened, and the drone can be controlled to move to the last track point along the last generated local track. If the global trajectory point has not been screened, the next trajectory point of the current position of the drone is used as the starting point of the first preset length, and the process returns to S130.

Wherein, in the case where there is no obstacle on the trajectory formed by the trajectory points covered by the first preset length, the generation of the local trajectory is taken as an example for introduction with reference to the drawings. The trajectory points in Figure 1g are global trajectory points, and "×" represents the position of the UAV. As shown in Figure 1g, L _{min_for_execution} is the first preset length, that is, the length of the generated local trajectory, and L _replan is the second preset length. Set the length, that is, after flying the distance of the second preset length, the local trajectory will be regenerated. As shown in Figure 1g, the UAV is at the starting point of the global trajectory point. In order to generate a local trajectory, it is necessary to filter the trajectory in the global trajectory point. The length of the line connecting the selected trajectory points is determined by L _{min_for_execution} . Among them, the larger L _{min_for_execution} is, the more trajectory points are screened. The local trajectory is generated based on the selected trajectory points, and the control quantity is determined based on the local trajectory. After the UAV moves the distance L _replan along the local trajectory based on the control quantity, the local planning needs to be re-planned. As shown in Figure 1h, after the UAV moves the distance L _replan along the local trajectory based on the control amount, the screening starts from the next trajectory point in front of the current position of the UAV, and the length of the line connecting the selected trajectory points satisfies the L _{min_for_execution} After the length, the local trajectory and the control amount are generated again. After controlling the UAV to move the distance L _replan along the local trajectory based on the control amount, it returns to the next trajectory point in front of the current position of the UAV to start screening until the global trajectory point is screened. . Wherein, when there are obstacles on the trajectory formed by the trajectory points covered by the first preset length, the local trajectory points are determined based on the screened trajectory points, and the method for generating the local trajectory can refer to the introduction of the following embodiments.

In the related art, in the process of performing operations according to the operation route, the drone needs to stop and then fly to the next route point when it reaches a route point. In the process of performing the operation, frequent braking will affect the operation efficiency. In the embodiment of the present invention, the maximum speed allowed for each route point is determined based on the constraint conditions of the route points in the operation route, and the maximum speed is used as the speed of the corresponding route point; Based on the speed and position of each two adjacent route points, speed matching is performed on the road segments between each two adjacent route points, and the position of each road segment is sampled based on the assigned speed to obtain global trajectory points. Local planning can be carried out at the point, so as to avoid the frequent braking of the drone and improve the operation efficiency.

However, in the related art, during the actual flight of the UAV based on the planned path trajectory, if it encounters an obstacle, the UAV cannot avoid the obstacle well, which affects the safety of the UAV.

In the technical solution provided by the embodiment of the present invention, by determining the global trajectory point of the operation route, the starting point of the global trajectory point is used as the starting point of the first preset length, and the trajectory points covered by the first preset length are selected from the global trajectory points, Determine the local trajectory point based on the screened trajectory points, generate a local trajectory, control the UAV to move the second preset length based on the local trajectory, take the next trajectory point of the current position of the UAV as the starting point of the first preset length, and return to The operation of screening the trajectory points covered by the first preset length in the global trajectory points until the global trajectory point selection is completed; wherein, the second preset length is smaller than the first preset length. That is, the local trajectory is generated by the first preset length, the UAV is controlled to move the second preset distance based on the local trajectory, and then the local trajectory is regenerated based on the first preset length. In the case of encountering obstacles, it can be avoided in time. obstacle handling to achieve precise obstacle avoidance and ensure the safety of UAVs.

2a is a flowchart of a trajectory planning method provided by another embodiment of the present invention. In this embodiment, optionally, determining a local trajectory point based on the screened trajectory points includes: if the trajectory points covered by the first preset length form There is an obstacle on the trajectory of , and the distance between the obstacle and the end point of the first preset length is greater than the first preset distance for obstacle avoidance, based on the first preset distance, the second preset distance for obstacle avoidance and The obstacle avoidance trajectory point is determined by the information of the obstacle, and the obstacle avoidance trajectory point is used to replace the trajectory point in the corresponding global trajectory point; the unreplaced trajectory point and the obstacle avoidance trajectory point in the trajectory point covered by the first preset length are used as the local track point.

As shown in Figure 2a, the technical solution provided by the embodiment of the present invention includes:

S210: Determine the global track point of the operation route.

S220: Use the starting point of the global trajectory point as the starting point of the first preset length.

S230: Screen the track points covered by the first preset length from the global track points.

S240: If there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is greater than the first preset distance for obstacle avoidance, based on the first preset The distance, the second preset distance for obstacle avoidance, and the information of the obstacle determine the obstacle avoidance trajectory point, and the obstacle avoidance trajectory point is used to replace the trajectory point in the corresponding global trajectory point.

In this embodiment of the present invention, the first preset distance and the second preset distance may be set as required, or may also be determined based on the current speed of the drone. When the current speed of the drone is larger, the first preset distance and the second preset distance are larger.

In this embodiment of the present invention, there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is greater than the first preset distance used for obstacle avoidance In this case, as shown in FIG. 2b, there is an obstacle 100 on the trajectory formed by the trajectory points covered by the first preset length L _{min_for_execution} , and the distance between the obstacle 100 and the end point of the first preset length is relatively large (the obstacle 100 is located at the end point of the first preset length). The middle position of L _{min_for_execution} ), if the drone moves according to the trajectory point covered by the first preset length, it will hit the obstacle 100. In order to avoid obstacles, the obstacle avoidance trajectory points can be determined based on the first preset distance, the second preset distance and the information of the obstacles. As shown in Fig. 2b, the hollow circle points can be the obstacle avoidance trajectory points, and the obstacle avoidance trajectory points can be Replace the track points in the corresponding global track points (part of track points in the track points covered by the first preset length).

In one implementation of the embodiment of the present invention, optionally, the obstacle avoidance trajectory point is determined based on the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle, and the obstacle avoidance trajectory point is replaced by the obstacle avoidance trajectory point. The trajectory points in the corresponding global trajectory points include: in the global trajectory points, determining the first trajectory point corresponding to the position at the first preset distance behind the obstacle and the position at the second preset distance before the obstacle The corresponding second trajectory point; the obstacle avoidance trajectory point is determined based on the information of the first trajectory point, the information of the second trajectory point and the information of the obstacle; it will be located between the second trajectory point and the first trajectory point in the global trajectory point The trajectory points of are replaced with obstacle avoidance trajectory points.

The information of the first track point includes the speed and position of the first track point, and the information of the second track point includes the position and speed of the second track point. Wherein, the first trajectory point may be a trajectory point at a position of a first preset distance behind the obstacle, or a subsequent trajectory point at a position of the first preset distance behind the obstacle. The second trajectory point may be a trajectory point at a position at a second preset distance before the obstacle, or may also be a previous trajectory point at a position at a second preset distance before the obstacle. Among them, the Hybrid A* algorithm or other related algorithms can be used to determine the obstacle avoidance trajectory point based on the information of the first trajectory point, the information of the second trajectory point and the information of the obstacle (including determining the position and speed of the obstacle avoidance trajectory point) . In the process of determining the obstacle avoidance trajectory point, by considering the speed of the first trajectory point and the second trajectory point, the generated obstacle avoidance trajectory and the original trajectory are smooth in speed, and the operation efficiency is improved. As shown in FIG. 2b, the first trajectory point may be the trajectory point 10, the second trajectory point may be the trajectory point 20, and the obstacle avoidance trajectory point may be used to separate the trajectory point (which may include the trajectory 10) between the trajectory point 10 and the trajectory point 20. and track point 20) to replace the track point between track point 10 and track point 20.

S250: Use unreplaced track points and obstacle avoidance track points in the track points covered by the first preset length as local track points, and generate a local track based on the local track points.

In this embodiment of the present invention, as shown in FIG. 2b , the track points after the track point 10 and the track points before the track point 20 are the track points that are not replaced in the track points covered by the first preset length, and the unreplaced track points are Points and obstacle avoidance trajectory points are used as local trajectory points, and a local trajectory is generated based on the local trajectory points, and a control amount can be generated based on the local trajectory, so that the UAV can be controlled to move along the local trajectory based on the control amount.

S260: Control the UAV to move a distance of a second preset length based on the local trajectory.

S270: Determine whether the global trajectory points are screened.

If yes, execute S280, if not, return to S290.

S280: Control the UAV to continue to move along the local trajectory to the last trajectory point.

S290: Take the next trajectory point of the current position of the drone as the starting point of the first preset length, and return to S230.

In the embodiment of the present invention, the UAV moves a distance of the second preset length based on the local trajectory, and uses the next trajectory point of the current position of the UAV as the starting point of the first preset length. If the local trajectory is regenerated, the last generated local trajectory contains the trajectory points that the UAV has not yet reached, and the trajectory points that the UAV has not reached (may include obstacle avoidance trajectory points) become part of the global trajectory points. Return to the operation of screening the global track points covered by the first preset length until the global track points are screened.

Therefore, by screening the trajectory points covered by the first preset length in the global trajectory points, if there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the obstacle is connected to the end point of the first preset length distance is greater than the first preset distance for obstacle avoidance, determine the obstacle avoidance trajectory point based on the first preset distance, the second preset distance for obstacle avoidance and the information of the obstacle, and use the obstacle avoidance trajectory point to replace The corresponding trajectory points in the global trajectory points; the unreplaced trajectory points and the obstacle avoidance trajectory points in the trajectory points covered by the first preset length are used as the local trajectory points, so that the obstacles can be processed in time and the obstacles can be avoided accurately.

3a is a flowchart of a trajectory planning method provided by another embodiment of the present invention. In this embodiment, optionally, determining a local trajectory point based on the screened trajectory points includes: if the trajectory points covered by the first preset length form There are obstacles on the trajectory of Four preset lengths; wherein, the distance between the end point of the fourth preset length and the obstacle is greater than the first preset distance; the avoidance is determined based on the first preset distance, the second preset distance for obstacle avoidance and the information of the obstacle The obstacle avoidance track point is used to replace the track point in the corresponding global track point; the unreplaced track point and the obstacle avoidance track point in the track point covered by the fourth preset length are used as the local track point.

As shown in Figure 3a, the technical solution provided by the embodiment of the present invention includes:

S310: Determine the global track point of the operation route.

S320: Use the starting point of the global trajectory point as the starting point of the first preset length.

S330: Screen the track points covered by the first preset length from the global track points.

S340: If there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is less than the first preset distance for obstacle avoidance, the first preset On the basis of the length, a third preset length is added to obtain a fourth preset length; wherein, the distance between the end point of the fourth preset length and the obstacle is greater than the first preset distance.

In this embodiment of the present invention, an obstacle exists on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is smaller than the first preset distance used for obstacle avoidance In this case, as shown in FIG. 3b, there is an obstacle 100 on the trajectory formed by the trajectory points covered by the first preset length L _{min_for_execution} , and the distance between the obstacle 100 and the end point of the first preset length is small (the obstacle 100 is close to The end point of the first preset length), the trajectory point after the obstacle 100 is not enough to support the UAV to return to the original trajectory point, so it is necessary to continue to filter the trajectory points along the global trajectory point. As shown in FIG. 3c, a third preset length L _clear may be added to the first preset length to obtain a fourth preset length. Wherein, the distance from the end point of the fourth preset length to the obstacle 100 is greater than the first preset distance, so that the UAV can return to the original trajectory point, and can return while accurately avoiding obstacles. Among them, in Figures 3b and 3c, the hollow circles represent the obstacle avoidance trajectory points, and the solid circles represent the trajectory points in the global trajectory points, which may also be referred to as the original trajectory points.

It should be noted that, since the sensor can identify the distant obstacle and avoid the obstacle in advance, it is impossible for the obstacle to appear at a position close to the starting point of the first preset length.

S350: Determine an obstacle avoidance trajectory point based on the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle, and replace the trajectory point in the corresponding global trajectory point with the obstacle avoidance trajectory point.

In one implementation of the embodiment of the present invention, optionally, the obstacle avoidance trajectory point is determined based on the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle, and the obstacle avoidance trajectory point is replaced by the obstacle avoidance trajectory point. The trajectory points in the corresponding global trajectory points include: in the global trajectory points, determining the first trajectory point corresponding to the position at the first preset distance behind the obstacle and the position at the second preset distance before the obstacle The corresponding second trajectory point; the obstacle avoidance trajectory point is determined based on the information of the first trajectory point, the information of the second trajectory point and the information of the obstacle; it will be located between the second trajectory point and the first trajectory point in the global trajectory point The trajectory points of are replaced with obstacle avoidance trajectory points. For details, reference may be made to the description of the foregoing embodiments.

S360: Use unreplaced track points and obstacle avoidance track points in the track points covered by the fourth preset length as local track points, and generate a local track based on the local track points.

In the embodiment of the present invention, as shown in FIG. 3c , the track points after the track point 30 and the track points before the track point 40 are the track points that are not replaced in the track points covered by the fourth preset length, and the unreplaced track points are Points and obstacle avoidance trajectory points are used as local trajectory points; wherein, for the introduction of generating local trajectories based on local trajectory points, reference may be made to the foregoing embodiments.

S370: Control the UAV to move a distance of a second preset length based on the local trajectory.

S380: Determine whether the global trajectory points are screened.

If yes, execute S390, if not, return to S391.

S390: Control the UAV to continue to move along the local trajectory to the last trajectory point.

S391: Use the next trajectory point of the current position of the drone as the starting point of the first preset length, and return to S330.

Therefore, when there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is less than the first preset distance used for obstacle avoidance, pass On the basis of the first preset length, a third preset length is added to obtain a fourth preset length, and the unreplaced track points and obstacle avoidance track points in the track points covered by the fourth preset length are used as local track points to generate Local trajectory can avoid incomplete obstacle avoidance trajectory generation, and achieve accurate obstacle avoidance while allowing the UAV to return to the global trajectory.

It should be noted that the UAV moves along the global trajectory point under normal circumstances. When the obstacle appears on the global trajectory (in fact, it appears on the local trajectory, the two are basically equivalent in most cases), The drone needs to leave the original trajectory to avoid obstacles, and then return to the original trajectory. In the related art, the UAV needs to be stopped, or the flight controller decelerates the UAV to a certain speed and then the controller controls the UAV to avoid obstacles. Among them, two methods in the related art make the UAV move. The process is not stable, and the change of control of the latter can easily cause the movement of the drone to be unstable. The technical solutions provided by the embodiments of the present invention can be executed by a trajectory planner, which avoids frequent exchange of control rights, increases the integration of planning, and ensures the stability of the movement of the UAV. In addition, the trajectory planner provided by the present invention can also perform the execution of the 3D operation route.

FIG. 4 is a structural block diagram of a trajectory planning apparatus provided by an embodiment of the present invention. As shown in FIG. 4 , the apparatus provided by an embodiment of the present invention includes: a global trajectory point determination module 410, a starting point determination module 420, a local trajectory generation module 430 and Control/return to module 440.

Wherein, the global trajectory point determination module 410 is used to determine the global trajectory point of the operation route;

The starting point determination module 420 is configured to use the starting point of the global trajectory point as the starting point of the first preset length;

The local trajectory generation module 430 is used for screening the trajectory points covered by the first preset length in the global trajectory points, determining the local trajectory points based on the screened trajectory points, and generating the local trajectory based on the local trajectory points;

The control/return module 440 is used to control the UAV to move the distance of the second preset length based on the local trajectory, take the next trajectory point of the current position of the UAV as the starting point of the first preset length, and return to the global trajectory point The operation of screening the trajectory points covered by the first preset length until the global trajectory point screening is completed, wherein the first preset length is greater than the second preset length.

Optionally, determine local trajectory points based on the filtered trajectory points, including:

If there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is greater than the first preset distance for obstacle avoidance, based on the first preset distance, The second preset distance for obstacle avoidance and the information of the obstacle determine the obstacle avoidance trajectory point, and use the obstacle avoidance trajectory point to replace the trajectory point in the corresponding global trajectory point;

Unreplaced track points and obstacle avoidance track points in the track points covered by the first preset length are used as local track points.

Optionally, determine local trajectory points based on the filtered trajectory points, including:

If there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is less than the first preset distance for obstacle avoidance, the On the basis, a third preset length is added to obtain a fourth preset length; wherein, the distance between the end point of the fourth preset length and the obstacle is greater than the first preset distance;

Determine the obstacle avoidance trajectory point based on the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle, and replace the trajectory point in the corresponding global trajectory point with the obstacle avoidance trajectory point;

Unreplaced track points and obstacle avoidance track points in the track points covered by the fourth preset length are used as local track points.

Optionally, the obstacle avoidance trajectory point is determined based on the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle, and the obstacle avoidance trajectory point is used to replace the trajectory point in the corresponding global trajectory point, including: :

In the global trajectory point, a first trajectory point corresponding to a position at a first preset distance behind the obstacle and a second trajectory point corresponding to a position at a second preset distance before the obstacle are determined;

Determine the obstacle avoidance trajectory point based on the information of the first trajectory point, the information of the second trajectory point and the information of the obstacle;

Replace the trajectory points between the second trajectory point and the first trajectory point in the global trajectory points with obstacle avoidance trajectory points.

Optionally, determine local trajectory points based on the filtered trajectory points, including:

If there is no obstacle on the trajectory formed by the trajectory points covered by the first preset length, the selected trajectory points are used as local trajectory points.

Optionally, determine the global trajectory points of the operation route, including:

Determine the maximum speed allowed for each route point based on the constraints of the route points in the operation route, and use the maximum speed as the speed of the route point;

Based on the speed and position of every two adjacent route points, determine the position and speed at any moment in the road segment between each two adjacent route points, and sample the location of each road segment to obtain a global trajectory point.

Optionally, determine the maximum speed allowed for each route point based on the constraints of the route points in the operation route, including:

The maximum speed among the speeds satisfying the following conditions is determined as the maximum speed:

The speed at each route point is less than or equal to the maximum speed allowed for the job;

The speed of the first route point and the last route point is 0;

For the speed of a route point, it should start from the speed of the previous route point and reach the full acceleration at the maximum acceleration allowed by the drone, or decelerate at the maximum acceleration allowed by the drone;

For the speed of a route point, it can decelerate to the next route point with the maximum acceleration allowed by the drone, or accelerate to the next route point with the maximum acceleration allowed by the drone;

The forward speed of the waypoint is determined based on the size of the corner.

Optionally, the forward speed of the route point is determined based on the size of the corner, including:

The forward speed of the route point is determined based on the following formula:

Among them, _vx is the forward speed of the UAV, θ is the size of the turning angle, a ₀ is the acceleration of the UAV, and d is the distance from the route point when the UAV decelerates.

Optionally, control the UAV to move a distance of a second preset length based on the local trajectory, including:

Determine the control amount of the UAV based on the local trajectory;

The UAV is controlled to move a distance of a second preset length along the local trajectory based on the control amount.

The above apparatus can execute the method provided by any embodiment of the present invention, and has corresponding functional modules and beneficial effects for executing the method.

FIG. 5 is a schematic structural diagram of a trajectory planner provided by an embodiment of the present invention. The trajectory planner 50 includes a trajectory planning device 500 provided by an embodiment of the present invention. The trajectory planning device 500 (the trajectory planning device shown in FIG. 4 ) ) to execute the trajectory planning method provided by any of the foregoing embodiments of the present invention.

An embodiment of the present invention further provides an unmanned aerial vehicle, including the trajectory planner provided by the embodiment of the present invention, that is, the planner shown in FIG. 5 .

FIG. 6 shows a block diagram of an unmanned aerial vehicle 600 for executing a trajectory planning method provided by an exemplary embodiment of the present application.

6, the drone 600 includes a processing component 610, which further includes one or more processors, and a memory resource, represented by memory 620, for storing instructions executable by the processing component 610, such as application programs. An application program stored in memory 620 may include one or more modules, each corresponding to a set of instructions. Additionally, the processing component 610 is configured to execute instructions to perform the trajectory planning method described above.

The drone 600 may also include a power supply assembly configured to perform power management of the drone 600, a wired or wireless network interface configured to connect the drone 600 to a network, and an input output (I/O) interface . The drone 600 can be operated based on an operating system stored in the memory 620, such as Windows Server ^™ , Mac OS X ^™ , Unix ^™ , Linux ^™ , FreeBSD ^™ or the like.

An embodiment of the present invention provides a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, implements a trajectory planning method provided by any of the foregoing embodiments of the present invention: for example, it can realize : Determine the global trajectory point of the operation route; take the starting point of the global trajectory point as the starting point of the first preset length; filter the trajectory points covered by the first preset length in the global trajectory points, and determine the local trajectory point based on the selected trajectory points , and generate a local trajectory based on the local trajectory points; control the UAV to move the distance of the second preset length based on the local trajectory, take the next trajectory point of the current position of the UAV as the starting point of the first preset length, and return to the global trajectory The operation of screening the trajectory points covered by the first preset length in the points, until the global trajectory point screening is completed, wherein the first preset length is greater than the second preset length.

Any combination of one or more computer-readable media may be employed. The computer-readable medium may be a computer-readable signal medium or a computer-readable storage medium. The computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus or device, or a combination of any of the above. More specific examples (a non-exhaustive list) of computer readable storage media include: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read only memory (ROM), Erasable programmable read only memory (EPROM or flash memory), optical fiber, portable compact disk read only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above. In this document, a computer-readable storage medium can be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

A computer-readable signal medium may include a propagated data signal in baseband or as part of a carrier wave, with computer-readable program code embodied thereon. Such propagated data signals may take a variety of forms including, but not limited to, electromagnetic signals, optical signals, or any suitable combination of the foregoing. A computer-readable signal medium can also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device .

Program code embodied on a computer readable medium may be transmitted using any suitable medium including, but not limited to, wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.

Computer program code for carrying out operations of the present invention may be written in one or more programming languages, including object-oriented programming languages—such as Java, Smalltalk, C++, but also conventional Procedural programming language - such as "C" language or similar programming language. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer, or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (eg, using an Internet service provider through Internet connection).

Note that the above are only preferred embodiments of the present invention and applied technical principles. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein, and various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the protection scope of the present invention. Therefore, although the present invention has been described in detail through the above embodiments, the present invention is not limited to the above embodiments, and can also include more other equivalent embodiments without departing from the concept of the present invention. The scope is determined by the scope of the appended claims.

Claims (14)

A trajectory planning method, comprising: Determine the global trajectory point of the operation route; Taking the starting point of the global trajectory point as the starting point of the first preset length; Screening the trajectory points covered by the first preset length from the global trajectory points, determining local trajectory points based on the screened trajectory points, and generating a local trajectory based on the local trajectory points; Controlling the UAV to move the distance of the second preset length based on the local trajectory, taking the next trajectory point of the current position of the UAV as the starting point of the first preset length, and returning it to the global trajectory point The step of screening the trajectory points covered by the first preset length until the global trajectory point screening is completed, wherein the first preset length is greater than the second preset length. The trajectory planning method according to claim 1, wherein the determining of the local trajectory points based on the screened trajectory points comprises: If there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is greater than the first preset distance for obstacle avoidance, based on The first preset distance, the second preset distance for obstacle avoidance, and the information of the obstacle determine an obstacle avoidance trajectory point, and use the obstacle avoidance trajectory point to replace the trajectory point in the corresponding global trajectory point; The unreplaced trajectory points and the obstacle avoidance trajectory points among the trajectory points covered by the first preset length are used as the local trajectory points. The trajectory planning method according to claim 1, wherein the determining of the local trajectory points based on the screened trajectory points comprises: If there is an obstacle on the trajectory formed by the trajectory points covered by the first preset length, and the distance between the obstacle and the end point of the first preset length is smaller than the first preset distance for obstacle avoidance, A third preset length is added to the first preset length to obtain a fourth preset length; wherein the distance between the end point of the fourth preset length and the obstacle is greater than the first preset distance ; An obstacle avoidance trajectory point is determined based on the first preset distance, the second preset distance for obstacle avoidance, and the information of the obstacle, and the obstacle avoidance trajectory point is used to replace the trajectory point in the corresponding global trajectory point ; The unreplaced trajectory points and the obstacle avoidance trajectory points among the trajectory points covered by the fourth preset length are used as the local trajectory points. The trajectory planning method according to claim 2 or 3, wherein the obstacle avoidance trajectory point is determined based on the first preset distance, the second preset distance used for obstacle avoidance, and the information of the obstacle , and use the obstacle avoidance trajectory points to replace the trajectory points in the corresponding global trajectory points, including: In the global trajectory point, a first trajectory point corresponding to a position at the first preset distance after the obstacle and a position corresponding to the second preset distance before the obstacle are determined the second track point; Determine the obstacle avoidance trajectory point based on the information of the first trajectory point, the information of the second trajectory point and the information of the obstacle; A trajectory point between the second trajectory point and the first trajectory point among the global trajectory points is replaced with the obstacle avoidance trajectory point. The trajectory planning method according to claim 1, wherein the determining of the local trajectory points based on the screened trajectory points comprises: If there is no obstacle on the trajectory formed by the trajectory points covered by the first preset length, the selected trajectory points are used as the local trajectory points. The trajectory planning method according to any one of claims 1 to 5, wherein the determining of the global trajectory points of the operation route comprises: Determine the maximum speed allowed for each route point based on the constraints of the route points in the operation route, and use the maximum speed as the speed of the route point; Based on the speed and position of every two adjacent route points, determine the position and speed at any moment in the road segment between each two adjacent route points, and sample the location of each road segment to obtain the global trajectory point. The trajectory planning method according to claim 6, wherein the determining the maximum allowable speed of each route point based on the constraints of the route points in the operation route comprises: The maximum value among the speeds satisfying the following conditions is determined as the maximum speed: The speed at each route point is less than or equal to the maximum speed allowed for the job; The speed of the first route point and the last route point is 0; For the speed of a route point, it satisfies that starting from the speed of the previous route point, it is accelerated to the maximum acceleration allowed by the UAV, or decelerated to the maximum allowed acceleration of the UAV; For the speed of one route point, it satisfies the speed of decelerating to the next route point with the maximum acceleration allowed by the drone, or the speed of accelerating to the next route point with the maximum acceleration allowed by the drone; The forward speed of the waypoint is determined based on the size of the corner. The trajectory planning method according to claim 7, wherein the forward speed of the route point is determined based on the size of the turning angle, comprising: The forward speed of the route point is determined based on the formula:

Wherein, _vx is the forward speed of the UAV, θ is the angle of rotation, a ₀ is the acceleration of the UAV, and d is the distance between the UAV and the route point when the UAV decelerates. distance. The trajectory planning method according to any one of claims 1 to 8, wherein the controlling the UAV to move a distance of a second preset length based on the local trajectory, comprising: determining a control amount of the drone based on the local trajectory; The UAV is controlled to move the distance of the second preset length along the local trajectory based on the control amount. A trajectory planning device, comprising: The global trajectory point determination module is used to determine the global trajectory point of the operation route; a starting point determination module, configured to use the starting point of the global trajectory point as the starting point of the first preset length; a local trajectory generation module, configured to screen the trajectory points covered by the first preset length in the global trajectory points, determine local trajectory points based on the screened trajectory points, and generate local trajectory based on the local trajectory points; The control/return module is used to control the UAV to move a distance of a second preset length based on the local trajectory, take the next trajectory point of the current position of the UAV as the starting point of the first preset length, and return The step of screening the trajectory points covered by the first preset length from the global trajectory points until the global trajectory points are screened, wherein the first preset length is greater than the second preset length. A trajectory planner, comprising the trajectory planning device of claim 10 . An unmanned aerial vehicle, comprising the trajectory planner of claim 11 . A computer-readable storage medium on which a computer program is stored, characterized in that, when the program is executed by a processor, the trajectory planning method according to any one of claims 1-9 is implemented. An unmanned aerial vehicle, characterized in that it includes: processor; memory for storing said processor-executable instructions, Wherein, the processor is configured to execute the trajectory planning method according to any one of the preceding claims 1-9.

Images