A method for autonomous tracking trajectory planning of unmanned aerial vehicles

Abstract

The present invention specifically relates to a method for autonomous tracking trajectory planning of an unmanned aerial vehicle, comprising: first, the unmanned aerial vehicle obtains the speed and position of the target at the current moment according to the target recognition result. Then, the motion primitive is expanded based on the position and speed information to predict the position of the target at a certain moment in the future, and the Markov chain state transfer is performed to predict the probability of the target appearing at a certain position. Based on the potential points generated by the prediction results, viewpoints that are conducive to observing the target are generated near all potential points. Then, it is determined whether the target is in the field of view of the unmanned aerial vehicle. If so, the end point of the path planning of the unmanned aerial vehicle is the global optimal observation point; if it has been lost, the viewpoints with the highest cost are traversed in turn to try to recapture the target. Finally, according to the decision result, a multi-constraint joint B-spline trajectory optimization model is constructed, and the yaw angle is used as a separate optimization variable to solve the final optimal tracking trajectory. This scheme can realize the robust and efficient autonomous tracking of unmanned aerial vehicles in complex and dense environments.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a method for autonomous tracking trajectory planning of unmanned aerial vehicles, a storage medium, a computer program product, and unmanned aerial vehicle equipment. Background Art

[0002] Autonomous target tracking by drones refers to the process of using autonomous drone systems to chase and track non-cooperative targets in unknown or complex environments. Autonomous target tracking by drones has a wide range of application scenarios, such as photography, search and rescue, and inspection and supervision.

[0003] In the related art, autonomous target tracking methods for drones generally include tracking methods based on control and based on trajectory planning. Among them, the control-based method mainly relies on visual servoing to solve the control instructions through the deviation of the target position in the pixel plane. However, in complex and dense environments, the occlusion and loss of the target are inevitable. The control-based autonomous tracking method cannot solve the control instructions when the target is lost, which easily leads to tracking failure. In addition, the trajectory planning-based method has poor robustness in complex scenes, resulting in poor quality of the tracking trajectory.

[0004] It should be noted that the information disclosed in the above background technology section is only used to enhance the understanding of the background of the present invention, and therefore may include information that does not constitute the prior art known to ordinary technicians in the field. Summary of the invention

[0005] The present invention provides a method for autonomous tracking trajectory planning of a UAV, a storage medium, a computer program product, and a UAV device, which can effectively improve the robustness of autonomous tracking of the UAV in complex scenarios, thereby overcoming the defects existing in the prior art to a certain extent.

[0006] According to a first aspect of the present invention, a method for autonomous tracking trajectory planning of a drone is provided, the method comprising:

[0007] Acquire current state information of the target; wherein the current state information includes position information and speed information;

[0008] Based on the preset time interval information, the current state information, and the discrete control amount corresponding to the target, position prediction is performed to obtain multiple motion primitives; wherein the motion primitives include: position information, speed information, and acceleration information; the discrete control amount is configured based on the maximum acceleration that the target can achieve;

[0009] Performing Markov state transfer processing on each of the motion primitives to obtain the corresponding occurrence probability of each of the motion primitives, and configuring them as potential points; configuring target position prediction results based on the potential points;

[0010] Determine a mean point according to the position information of each potential point in the target position prediction result, and perform viewpoint sampling based on the mean point to determine the position information and the optimal observation yaw angle corresponding to each viewpoint;

[0011] When it is determined that the target is currently within the field of view of the drone, the viewpoints are screened based on the probability of occurrence of each potential point within the best observation yaw angle range of each viewpoint to determine the global optimal viewpoint;

[0012] The global optimal viewpoint is configured as a planning end point, tracking trajectory planning is performed based on the planning end point to obtain an autonomous tracking trajectory, and a yaw angle of the autonomous tracking trajectory is configured to be oriented toward the mean point.

[0013] In some exemplary embodiments, the position prediction includes at least one round;

[0014] The method further comprises:

[0015] Based on the motion primitives corresponding to the position prediction of the current round, the position prediction of the next round is performed to obtain the motion primitives corresponding to the position prediction of the next round.

[0016] In some exemplary embodiments, the method further comprises:

[0017] When it is determined that the target is currently lost within the field of view of the drone, the mean point corresponding to this round is configured according to the position information of each potential point corresponding to the position prediction in each round, and viewpoint sampling is performed based on the mean point of this round to determine the position information and the optimal observation yaw angle corresponding to each viewpoint in this round;

[0018] Determine the corresponding probability and according to the probability of occurrence of each potential point within the best observation yaw angle range of each viewpoint in this round;

[0019] Determine the capture cost corresponding to each viewpoint by combining the probability and the distance between the viewpoint and the nearest obstacle; and configure the viewpoint corresponding to the maximum capture cost as the capture viewpoint corresponding to this round;

[0020] According to the captured viewpoints corresponding to each round, tracking trajectory planning is performed to obtain an autonomous tracking trajectory; and the yaw angle of the autonomous tracking trajectory is configured according to the optimal observation yaw angle corresponding to each captured viewpoint.

[0021] In some exemplary embodiments, when determining the capture cost corresponding to each viewpoint, the method further includes:

[0022] Configure the distance constraint between viewpoints according to the distance difference between adjacent viewpoints;

[0023] The capture cost corresponding to each viewpoint is determined by combining the distance constraint between viewpoints, the probability sum, and the distance between the viewpoint and the nearest obstacle.

[0024] In some exemplary embodiments, the method further comprises:

[0025] Construct dynamic feasibility constraints based on the maximum speed and maximum acceleration of the UAV;

[0026] Minimize the control points of the high-order derivatives of the B-spline trajectory and construct smoothness constraints;

[0027] Based on the distance between the drone and the obstacle, the obstacle distance safety constraint condition is constructed;

[0028] Based on the mean points corresponding to the motion primitives expanded in each round and the preset tracking distance threshold, a tracking distance constraint condition is constructed;

[0029] At least one of the above constraints is used to configure a target constraint, and based on the target constraint, the autonomous tracking trajectory is optimized using a B-spline curve algorithm.

[0030] In some exemplary embodiments, the target constraints further include: a yaw angle trajectory optimization constraint;

[0031] The yaw angle trajectory optimization constraint conditions include: any one or more of yaw angle dynamics constraint, smoothness constraint, and trajectory direction constraint conditions based on yaw angle configuration.

[0032] In some exemplary embodiments, the method further comprises:

[0033] The potential points are screened based on a preset probability threshold, so as to configure the target location prediction result according to the screened potential points.

[0034] In some exemplary embodiments, performing Markov state transfer processing on each of the motion primitives to obtain the corresponding occurrence probability of each of the motion primitives includes:

[0035] Determine a corresponding obstacle avoidance compensation vector based on the distance between the motion primitive and a plurality of surrounding obstacles;

[0036] The corresponding occurrence probability is determined by combining the state transfer matrix of the Markov chain, the occurrence probability of the motion primitive at the previous moment, and the obstacle avoidance compensation vector.

[0037] In some exemplary embodiments, configuring a mean point according to the position information of each potential point, and performing viewpoint sampling based on the mean point to determine the position information and the optimal observation yaw angle corresponding to each viewpoint includes:

[0038] Calculating a position mean according to the position information of each potential point, and configuring a mean point based on the position mean;

[0039] Taking the mean point as the center and a plurality of radii of different lengths as the circle, sampling is performed at preset intervals on the circumference to obtain the viewpoints and determine the position of each viewpoint;

[0040] The corresponding optimal observation yaw angle is determined according to the viewing angle of the most potential points observed from each viewpoint.

[0041] According to a second aspect of the present invention, there is provided a computer program product having a computer program stored thereon, which implements the above-mentioned method for autonomous tracking trajectory planning of a UAV when the computer program is executed by a processor.

[0042] According to a third aspect of the present invention, there is provided a storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the above-mentioned method for autonomous tracking trajectory planning of a UAV.

[0043] According to a fourth aspect of the present invention, there is provided a drone device, comprising:

[0044] The drone itself; and

[0045] A processor disposed on the drone body, and a memory for storing executable instructions of the processor;

[0046] Wherein, the processor is configured to implement the above-mentioned FPGA-based efficient TM-CFAR implementation method by executing the executable instructions.

[0047] The autonomous tracking trajectory planning method for unmanned aerial vehicles provided by the embodiment of the present invention realizes the prediction of the target position based on the motion primitive expansion method, and combines the Markov chain to predict the probability of the target appearing at a certain position in the future. When making tracking decisions, a series of viewpoints that are conducive to tracking are first generated based on the information gain of the target position prediction result; and when it is determined that the target is in the field of view of the unmanned aerial vehicle, the viewpoints are screened, and then the screened viewpoints are used as the planning end point to complete the planning of the autonomous tracking trajectory based on the viewpoint, thereby reducing the probability of tracking failure; and it can realize that the unmanned aerial vehicle can complete robust and efficient autonomous tracking in complex and dense environments; and realize stable and efficient tracking in complex scenes.

[0048] It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The accompanying drawings herein are incorporated into and constitute a part of the specification, illustrate embodiments consistent with the present invention, and together with the specification are used to explain the principles of the present invention. Obviously, the accompanying drawings described below are only some embodiments of the present invention, and for those of ordinary skill in the art, other accompanying drawings can be obtained based on these accompanying drawings without creative work.

[0050] Figure 1 A schematic diagram schematically illustrates an exemplary embodiment of the present invention, a method for autonomous tracking trajectory planning of a drone;

[0051] Figure 2 A schematic diagram schematically illustrates a process of a method for autonomous tracking trajectory planning of a drone according to an exemplary embodiment of the present invention;

[0052] Figure 3 A schematic diagram schematically illustrates a tracking trajectory of a drone according to an exemplary embodiment of the present invention;

[0053] Figure 4 A schematic diagram schematically illustrates a planning trajectory optimization comparison according to an exemplary embodiment of the present invention. DETAILED DESCRIPTION

[0054] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that the present invention will be more comprehensive and complete and fully convey the concepts of the example embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0055] In addition, the accompanying drawings are only schematic illustrations of the present invention and are not necessarily drawn to scale. The same reference numerals in the figures represent the same or similar parts, and their repeated description will be omitted. Some of the block diagrams shown in the accompanying drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software form, or implemented in one or more hardware modules or integrated circuits, or implemented in different networks and / or processor devices and / or microcontroller devices.

[0056] In the related art, autonomous target tracking of UAVs generally includes the perception, decision-making and trajectory planning of autonomous UAVs to achieve stable tracking in complex scenes. Existing autonomous target tracking methods for UAVs mainly include control-based and trajectory planning-based methods. The control-based method mainly relies on visual servoing to solve the control instructions through the deviation of the target position in the pixel plane. However, in complex and dense environments, the occlusion and loss of targets are inevitable. The control-based autonomous tracking method cannot solve the control instructions when the target is lost, resulting in the failure of the tracking task. In addition, the existing trajectory planning-based method does not consider the problem of how to rediscover the target after the target is lost, and has poor robustness in complex scenes. And the tracking distance problem is not considered at the trajectory planning level; in addition, the time constraint in the tracking process is not considered, resulting in poor quality of the tracking trajectory.

[0057] In view of the shortcomings and deficiencies of the prior art, this example implementation provides a method for autonomous tracking trajectory planning of a drone, which can be applied to autonomous tracking of a drone. Figure 1 As shown, the autonomous tracking trajectory planning method of the UAV may specifically include the following steps:

[0058] Step S11, obtaining current state information of the target; wherein the current state information includes position information and speed information;

[0059] Step S12, performing position prediction based on preset time interval information, the current state information, and discrete control quantities corresponding to the target to obtain a plurality of motion primitives; wherein the motion primitives include: position information, velocity information, and acceleration information; and the discrete control quantities are configured based on the maximum acceleration that the target can achieve;

[0060] Step S13, performing Markov state transfer processing on each of the motion primitives to obtain the corresponding occurrence probability of each of the motion primitives, and configuring them as potential points; configuring the target position prediction result based on the potential points;

[0061] Step S14, determining a mean point according to the position information of each potential point in the target position prediction result, and performing viewpoint sampling based on the mean point to determine the position information and the optimal observation yaw angle corresponding to each viewpoint;

[0062] Step S15, when it is determined that the target is currently within the field of view of the drone, the viewpoints are screened based on the probability of occurrence of each potential point within the optimal observation yaw angle range of each viewpoint to determine the global optimal viewpoint;

[0063] Step S16: configuring the global optimal viewpoint as a planning end point, performing tracking trajectory planning based on the planning end point to obtain an autonomous tracking trajectory, and configuring the yaw angle of the autonomous tracking trajectory to be toward the mean point.

[0064] Below, each step of the method for autonomous tracking trajectory planning of a drone in this example implementation will be described in more detail with reference to the accompanying drawings and embodiments.

[0065] In step S11, the current state information of the target is obtained; wherein the current state information includes position information and speed information.

[0066] Specifically, when the drone is tracking a target in real time, it can collect images of the current field of view through the camera assembly installed on the drone, and perform image recognition on the collected images, so as to use the image recognition results to obtain the real-time position information and speed information of the tracked target. Of course, in other exemplary embodiments, the position information and speed information of the tracked target can also be obtained by other means; for example, by constructing an ESDF (Euclidean Signed Distance Functions) map. This solution does not make special limitations on this.

[0067] For example, the position and velocity state of the target at time t can be expressed as:

[0068]

[0069] in, Represents the coordinates of the target in the coordinate system at time t; Represents the speed of the target in each direction in the coordinate system at time t.

[0070] In step S12, position prediction is performed based on preset time interval information, the current state information, and discrete control quantities corresponding to the target to obtain multiple motion primitives; wherein the motion primitives include: position information, velocity information, and acceleration information; and the discrete control quantities are configured based on the maximum acceleration that the target can achieve.

[0071] For example, when the drone autonomously tracks the current target, it can perform image recognition processing on the collected image data to confirm the type of the target; or, it can determine the type of the target based on pre-configured tracking task data. For each type of target, its corresponding discrete control quantity can be pre-configured. For example, the type of the target can be a pedestrian, a vehicle, an aircraft, etc.

[0072] Specifically, the discrete control quantity can be expressed as:

[0073]

[0074] in, Indicates the maximum acceleration that the target can achieve; n is a positive integer, is the coefficient.

[0075] For example, in the current path planning cycle, after obtaining the position coordinates and speed data corresponding to the target at the current moment, the motion primitives can be expanded in combination with the preset time interval and discrete control quantity to obtain multiple expanded motion primitives to achieve the prediction of the target position. Specifically, the formula for expanding the motion primitives can be expressed as:

[0076]

[0077] in, is a preset time interval. For example, the time interval may be 0.2 seconds, 0.5 seconds, 1 second, or other time lengths.

[0078] After one round of expansion, we can get (2n+1) 3 Each motion primitive includes predicted position information, velocity information, and acceleration information.

[0079] In some exemplary embodiments, the position prediction includes at least one round. The method further includes: performing position prediction for the next round based on the motion primitives corresponding to the position prediction for the current round to obtain the motion primitives corresponding to the position prediction for the next round.

[0080] Specifically, in order to obtain richer prediction information, rounds of motion primitive expansion can be performed. For example, 2, 3, 5, or 7 rounds of motion primitive expansion can be performed. After the first round of motion primitive expansion is completed, these primitives are used as the starting point for a new round of primitive expansion. After m rounds of expansion, (2n+1) 3m primitives.

[0081] For example, considering the speed and acceleration of different types of targets, the motion primitive expansion rounds corresponding to different types of targets can be pre-configured. For example, when determining the type of the current tracking target, configure the corresponding motion primitive expansion rounds. For example, for tracking targets such as pedestrians or non-motor vehicles, due to their low movement speed and achievable acceleration, the number of rounds of motion primitive expansion can be configured to be 5 times; for aircraft and cars, due to their high movement speed and achievable acceleration, the number of rounds of motion primitive expansion can be configured to be 1, 2 or 3. By configuring different motion primitive expansion rounds for different types of targets, the data processing speed can be guaranteed. Avoid reduced data processing efficiency due to too much data; at the same time, avoid inaccurate trajectory planning due to insufficient data.

[0082] In step S13, Markov state transfer processing is performed on each of the motion primitives to obtain the corresponding occurrence probability of each of the motion primitives, and the probability is configured as a potential point; and the target position prediction result is configured based on the potential point.

[0083] Exemplarily, the performing Markov state transfer processing on each of the motion primitives to obtain the corresponding occurrence probability of each of the motion primitives includes:

[0084] Determine a corresponding obstacle avoidance compensation vector based on the distance between the motion primitive and a plurality of surrounding obstacles;

[0085] The corresponding occurrence probability is determined by combining the state transfer matrix of the Markov chain, the occurrence probability of the motion primitive at the previous moment, and the obstacle avoidance compensation vector.

[0086] Specifically, when expanding the motion primitives, each discrete acceleration input can be used as a state of the Markov chain, and the state transition of the Markov chain can be used to calculate the corresponding occurrence probability of each primitive. The formula may include: .

[0087] Among them, p t is the probability vector of the motion primitive; P is the state transfer matrix of the Markov chain, obtained by the Gaussian kernel function; p t-1 is the state of the motion primitive at the previous moment, such as the probability of occurrence. Represents a vector element-wise multiplication operation.

[0088] In addition, p f is the obstacle avoidance compensation vector. Considering that the target's movement will avoid obstacles, the probability of occurrence of the motion primitives near the obstacles is reduced. Therefore, the obstacle avoidance compensation vector is constructed to optimize the probability of occurrence of each motion primitive. Specifically, the formula corresponding to the obstacle avoidance compensation vector includes:

[0089]

[0090] Among them, d i is the distance from each motion primitive to the nearest obstacle; Represents the transposed matrix of the distance vector.

[0091] In some exemplary embodiments, the method further includes: screening each potential point based on a preset probability threshold, so as to configure the target location prediction result according to the screened potential points.

[0092] Specifically, after all motion primitives have completed expansion and Markov state transfer, the pre-configured probability threshold can be used to "prune" motion primitives with too low probability, so that they do not participate in subsequent motion primitive expansion and decision-making, thereby effectively improving data processing effects and improving the accuracy of planned trajectories.

[0093] For example, after obtaining the occurrence probability prediction results corresponding to each motion primitive, the potential points of each selected motion primitive can be configured as follows: ; where x j is the three-dimensional coordinate of each potential point (i.e., motion primitive), p is the probability of occurrence of the target corresponding to the potential point, and i is the number of times the potential point is expanded. Each potential point can be configured as a target position prediction result.

[0094] In step S14, a mean point is determined according to the position information of each potential point in the target position prediction result, and viewpoint sampling is performed based on the mean point to determine the position information and the optimal observation yaw angle corresponding to each viewpoint.

[0095] Exemplarily, the above step S14 may specifically include:

[0096] Step S141, configuring a mean point according to the position information of each potential point, and performing viewpoint sampling based on the mean point to determine the position information and the best observation yaw angle corresponding to each viewpoint, includes:

[0097] Step S142, calculating the position mean according to the position information of each potential point, and configuring the mean point based on the position mean;

[0098] Step S143, taking the mean point as the center and making a circle with multiple radii of different lengths, sampling at preset intervals on the circumference to obtain the viewpoints and determine the position of each viewpoint;

[0099] Step S144, determining the corresponding optimal observation yaw angle according to the viewing angle of the most potential points observed by each viewpoint.

[0100] Specifically, according to the obtained potential points, the corresponding coordinate mean can be calculated according to the corresponding coordinate information , as the mean point. And with the mean point as the center, sample on multiple circles of different radii to obtain the viewpoint VP={x,yaw}; where x represents the position of each viewpoint and yaw is the optimal observation yaw angle of the viewpoint.

[0101] For example, you can configure 3 or 4 circles with different radius sizes, and sample at equal angles on each circle. The number of sampling points is determined by the equal angle interval; for example, you can sample the viewpoints at 30 degree intervals, so that a circle can have 12 sampling points. The best observation yaw angle is from the drone position to the x avg The yaw angle of the direction.

[0102] In step S15, when it is determined that the target is currently within the field of view of the drone, the viewpoints are screened based on the probability of occurrence of each potential point within the optimal observation yaw angle range of each viewpoint to determine the global optimal viewpoint.

[0103] For example, the drone can determine in real time whether the target is within the field of view; for example, by performing image recognition on images collected in real time, and determining whether the target is currently within the field of view based on the image recognition result.

[0104] If it is determined that the target is currently within the field of view of the drone, the method of step S14 described above can be used to calculate the mean point based on all potential points and perform viewpoint sampling. For each viewpoint obtained, the potential points within the viewing angle of the yaw angle can be first determined; then the probability and p can be calculated using the probability of occurrence of each potential point. sum After obtaining the probability and corresponding to each viewpoint, the viewpoints can be sorted according to the value of the probability and, so that the viewpoint with the maximum probability and can be selected as the global optimal viewpoint. The global optimal viewpoint can be used for planning the tracking trajectory.

[0105] Step S16: configure the global viewpoint as a planning end point, perform tracking trajectory planning based on the planning end point to obtain an autonomous tracking trajectory, and configure the yaw angle of the autonomous tracking trajectory to be toward the mean point.

[0106] Specifically, when it is determined that the target is currently within the field of view of the drone, the position average of all motion primitives expanded in each round can be used to determine the mean point, and viewpoint sampling can be performed based on the mean point, and the global optimal viewpoint can be screened out, and the global optimal viewpoint can be used as the planning end point of the tracking trajectory, and the autonomous tracking trajectory can be automatically planned with the planning end point. At the same time, the yaw angle of the autonomous tracking trajectory can be configured to face the mean point.

[0107] For example, the A* search algorithm can be used to search for an autonomous tracking trajectory that avoids collision. Heuristic search algorithm , which can be applied to the field of path optimization; the uniqueness of this algorithm is that it introduces global information when checking each possible node in the shortest path, estimates the distance between the current node and the end point, and uses it as a measure to evaluate the possibility that the node is on the shortest path. The specific calculation process of the A* search algorithm can be implemented using existing technology, and the present disclosure does not specifically limit the specific implementation process of the A* search algorithm.

[0108] For example, the current position of the drone can be configured as the starting point of the autonomous tracking trajectory; first, starting from the starting point, expand to the adjacent nodes, and calculate the heuristic function at the same time, and use the node with the largest heuristic function value as the new starting point, and continue to iterate until the end of the path is searched, that is, the global optimal viewpoint.

[0109] For example, in some embodiments, after completing the viewpoint sampling, it is possible to determine whether the target is still within the field of view. Alternatively, after completing the target position prediction, it is possible to first determine whether the target is currently within the field of view; if it is determined to be, the mean point is calculated using all the potential points expanded in each round, and viewpoint sampling is performed.

[0110] In some exemplary embodiments, the method further comprises:

[0111] Step S21, when it is determined that the target is currently lost within the field of view of the drone, the mean point corresponding to this round is configured according to the position information of each potential point corresponding to the position prediction in each round, and viewpoint sampling is performed based on the mean point of this round to determine the position information and the optimal observation yaw angle corresponding to each viewpoint in this round;

[0112] Step S22, determining the corresponding probability sum according to the occurrence probability of each potential point within the best observation yaw angle range of each viewpoint in this round;

[0113] Step S23, combining the probability and the distance between the viewpoint and the nearest obstacle, determining the capture cost corresponding to each viewpoint; and configuring the viewpoint corresponding to the maximum capture cost as the capture viewpoint corresponding to this round;

[0114] Step S24, performing tracking trajectory planning according to the capture viewpoints corresponding to each round to obtain an autonomous tracking trajectory; and configuring the yaw angle of the autonomous tracking trajectory according to the best observation yaw angle corresponding to each capture viewpoint.

[0115] For example, if the target is determined to be lost within the current field of view of the drone, a recapture mechanism may be triggered, see Figure 3 As shown, the motion primitives expanded in each round are used to calculate the viewpoints corresponding to this round.

[0116] For example, take the current three rounds of motion primitive expansion as an example. After the current target is judged to be lost within the field of view of the drone, the position information of multiple potential points expanded in the first round (i.e., the position information of the motion primitive) can be used to calculate the average value to obtain the mean point of the first round; based on the mean point of the first round, circles with multiple different radii are drawn, and then samples are taken at equal angles on each circle to obtain multiple sampled viewpoints. For the sampled viewpoints, their position information and the corresponding optimal observation yaw angle can be determined. For example, the number of circles can be 2, 3, or 4; the number of sampling points is determined based on equal angle intervals, and can be set to be sampled at intervals of 30 degrees, 45 degrees, or 60 degrees.

[0117] For each viewpoint sampled in the first expansion round, the corresponding probability and can be calculated based on the probability of occurrence of each potential point within the optimal observation yaw angle range, and the viewpoints sampled in the first expansion round are sorted according to the probability and, and the viewpoint with the maximum probability and is selected as the selected viewpoint for the first expansion round. The capture viewpoint is calculated based on the selected viewpoint, and the cost function of the capture viewpoint can be defined as:

[0118]

[0119] Among them, p sum is the probability and probability of occurrence of each potential point under the best observation yaw angle of the viewpoint; c e is the distance from the selected viewpoint to the nearest obstacle; p , e are the weight vectors corresponding to each cost term. Among them, obstacles close to the viewpoint may affect the drone's continuous observation of the target and threaten flight safety. Therefore, the greater the distance between the viewpoint and the obstacle, the higher the quality of the viewpoint.

[0120] In some exemplary embodiments, when determining the capture cost corresponding to each viewpoint, the method further includes: configuring a distance constraint between viewpoints based on the distance difference between adjacent viewpoints; and determining the capture cost corresponding to each viewpoint in combination with the distance constraint between viewpoints, the probability sum, and the distance between the viewpoint and the nearest obstacle.

[0121] Specifically, the cost function for capturing the viewpoint can be defined as:

[0122]

[0123] Among them, λ p , e , d are the weight vectors corresponding to each cost item respectively; is the distance constraint between viewpoints; VP i is the i-th viewpoint, VP i-1 is the adjacent i-1th viewpoint; the distance difference between adjacent viewpoints is calculated according to the coordinates of the viewpoints.

[0124] Setting the distance constraint between viewpoints can be used to prevent the distance between viewpoints from being too large, so as to avoid wasting time during the search process and causing the drone to lag behind the target.

[0125] According to the above method, the potential points corresponding to each expansion round can be calculated at the same time, so as to obtain the capture viewpoints corresponding to each expansion round.

[0126] After determining the capture viewpoints corresponding to each expansion round, the current position of the drone can be configured as the starting point, traversing each capture viewpoint, and using the A* algorithm for path planning. The yaw angle is set to the optimal observation yaw angle corresponding to each capture viewpoint.

[0127] Exemplarily, the method further includes:

[0128] Step S31, constructing dynamic feasibility constraints according to the maximum speed and maximum acceleration of the UAV;

[0129] Step S32, minimizing the control points of the high-order derivatives of the B-spline trajectory and constructing smoothness constraints;

[0130] Step S33, constructing obstacle distance safety constraint conditions based on the distance between the UAV and the obstacle;

[0131] Step S34, constructing a tracking distance constraint condition based on the mean value points corresponding to the motion primitives expanded in each round and a preset tracking distance threshold;

[0132] Step S35 , configuring a target constraint condition using at least one of the above constraints, and optimizing the autonomous tracking trajectory using a B-spline curve algorithm based on the target constraint condition.

[0133] Specifically, after completing the tracking path planning based on the viewpoint-based A* algorithm, considering that the initial trajectory may be too close to the obstacle and lacks dynamic constraints on the drone, the current trajectory can be used as the initial trajectory, and the B-spline curve can be used to optimize the initial trajectory to obtain an optimized autonomous tracking trajectory, thereby making the drone's tracking trajectory more accurate.

[0134] Specifically, the B-spline curve is composed of order p b , N+1 control points {Q0,Q1,…,Q N} and the node vector {t0,t1,…,t N+Pb+1}. Each B-spline curve is composed of a piecewise polynomial with t as a parameter. Since the high-order differentials of B-spline curves are all B-spline curves and have the convex hull property, these two properties ensure that they meet the dynamics and safety constraints, and the high-order derivatives of the trajectory polynomial, i.e., the speed and acceleration of the drone, can be expressed by control points and node vectors as follows:

[0135]

[0136] Among them, V i , A i , J i are the speed, acceleration and jerk corresponding to any coordinate point on the autonomous tracking trajectory of the drone. , which is used for the subsequent penalty term construction.

[0137] We construct the cost function of the drone tracking trajectory, which is specifically expressed as:

[0138]

[0139] Among them, J f Expressed as a dynamic feasibility constraint; J s Expressed as a smoothness constraint; J e Represented as obstacle distance safety constraint; J a Expressed as a tracking distance constraint.

[0140] Specifically, the dynamic feasibility constraint J f It is used to constrain the maximum speed of the drone during autonomous trajectory tracking, and the speed does not exceed the dynamic limit of the drone itself. The formula can be expressed as:

[0141]

[0142] Among them, v m 、a m They are the maximum speed and maximum acceleration that the drone can reach.

[0143] For the smoothness constraint J s Thanks to the convex hull characteristics of the B-spline curve, the smoothness is ensured by minimizing the control points of the high-order derivatives of the B-spline trajectory. The formula can be expressed as:

[0144]

[0145] During the trajectory planning process, control points that are too close to obstacles will receive a "thrust" that pushes them away from the obstacles, thereby preventing the drone from colliding with obstacles or getting too close to obstacles. Based on this, an obstacle distance safety constraint J is constructed. e , the formula can be expressed as:

[0146]

[0147] in, d ( Q i ) is the distance between the control point and the nearest obstacle, d thr The distance threshold for applying "thrust".

[0148] In addition, since the tracking strategy of this scheme configures the viewpoint calculated based on the potential point as the end point of the path planning, the target may be blocked by obstacles. To avoid this problem, a distance cost function is constructed to optimize the tracker trajectory during the tracking process. Figure 4 As shown, Figure 4 (a) is the trajectory before optimization. Figure 4 (b) The optimized trajectory ensures that the optimized trajectory is closer to the target trajectory, thereby preventing the target from being lost. The formula corresponding to the distance cost function can be expressed as:

[0149]

[0150] in, D a ( Q i ) is the distance between the ith control point and the potential point; it can be specifically expressed as:

[0151]

[0152] in, x avg,j is the average position of the potential point j-th expansion; d min is the minimum threshold of the optimal tracking distance, d max is the maximum threshold of the optimal tracking distance. △t is the preset time interval; t i It is expressed as the time corresponding to the ith control point, which is determined based on the time difference between the first control point and the ith control point. j is a positive integer, indicating the jth expansion.

[0153] Based on the one or more constraints mentioned above, the constraint conditions of the B-spline curve can be constructed, and based on the constraint conditions, the planned trajectory can be optimized using the B-spline curve algorithm to obtain an optimized autonomous tracking planned trajectory.

[0154] In some exemplary embodiments, the target constraint condition also includes: a yaw angle trajectory optimization constraint condition; wherein the yaw angle trajectory optimization constraint condition includes: any one or more of a yaw angle dynamic constraint, a smoothness constraint, and a trajectory direction constraint condition based on a yaw angle configuration.

[0155] Specifically, considering that the yaw angle should maintain observation of the target during tracking, the yaw angle trajectory of the drone can be used as a separate optimization variable to construct the corresponding cost function:

[0156]

[0157] Where φ is the yaw angle, and They represent the yaw angle dynamic constraint and the smoothness constraint respectively, and the construction method is the same as the position trajectory constraint. It is a trajectory direction constraint, which is used to constrain the trajectory of the drone to always face the target.

[0158] Specifically, the formula for trajectory direction constraint is expressed as:

[0159]

[0160] in, is the current yaw angle of the drone p at the i-th control point, is the optimal yaw angle of the optimal control point that points to the target target from the i-th control point.

[0161] The method provided by the embodiment of the present invention may include three parts: target motion prediction, tracking / recapture decision and tracking trajectory optimization. Figure 2 As shown in the figure, first, according to the target position and speed given by the target recognition result, the motion primitive is expanded; while the motion primitive is expanded, the Markov chain state transition is performed to predict the probability of the target appearing at a certain position. Secondly, according to the potential points generated by the prediction results, viewpoints that are conducive to observing the target are generated near all potential points. It is judged whether the target is in the current field of view of the drone. If the target is judged to be in the field of view, the end point of the path planning is set to the global optimal viewpoint. Alternatively, if it is not in the field of view, the drone traverses the search viewpoints to complete the recapture of the target and completes the trajectory planning. In addition, the B-spline trajectory optimization model can be constructed by considering factors such as safety, smoothness, and dynamics to solve the optimal tracking trajectory. And considering that yaw is an important factor in maintaining the observation of the target, the yaw angle is used as a separate optimization variable to solve the optimal yaw angle B-spline trajectory. Repeat the above process to complete the target motion prediction, viewpoint generation and tracking / recapture decision-making, and the drone can complete robust and efficient autonomous tracking in a complex and dense environment.

[0162] It should be noted that the above figures are only schematic illustrations of the processes included in the method according to an exemplary embodiment of the present invention, and are not intended to be limiting. It is easy to understand that the processes shown in the above figures do not indicate or limit the time sequence of these processes. In addition, it is also easy to understand that these processes can be performed synchronously or asynchronously, for example, in multiple modules.

[0163] Furthermore, in the embodiment of this example, a drone device is provided, including a drone body and an electronic device arranged on the drone body. For example, the electronic device includes: a memory and a processor.

[0164] For example, the processor is a central processing unit (CPU), which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) or the program loaded from the storage part to the random access memory (RAM). In the RAM, various programs and data required for system operation are also stored. The CPU, ROM, and RAM are connected to each other through a bus. The input / output (I / O) interface is also connected to the bus.

[0165] The drive is also connected to the I / O interface as needed. A removable medium, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., is installed on the drive as needed so that a computer program read therefrom is installed into the storage portion as needed.

[0166] In particular, according to an embodiment of the present invention, the process described below with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present invention includes a computer program product, which includes a computer program carried on a storage medium, and the computer program contains program code for executing the method shown in the flowchart. When the computer program is executed by a central processing unit (CPU), various functions defined in the system of the present application are executed.

[0167] It should be noted that the storage medium shown in the embodiment of the present invention may be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared or semiconductor system, device or device, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM), a flash memory, an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device or any suitable combination of the above. In the present invention, a computer-readable storage medium may be any tangible medium containing or storing a program, which may be used by or in combination with an instruction execution system, device or device. In the present invention, a computer-readable signal medium may include a data signal propagated in a baseband or as part of a carrier wave, which carries a computer-readable program code. 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 above. Computer-readable signal media may also be any storage medium other than computer-readable storage media, which may send, propagate, or transmit programs for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the storage medium may be transmitted using any appropriate medium, including but not limited to: wireless, wired, etc., or any suitable combination of the above.

[0168] The flowchart and block diagram in the accompanying drawings illustrate the possible architecture, functions and operations of the system, method and computer program product according to various embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the above-mentioned module, program segment or a part of the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram or flowchart, and the combination of boxes in the block diagram or flowchart, can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0169] The units involved in the embodiments of the present invention may be implemented by software or hardware, and the units described may also be arranged in a processor. The names of these units do not, in some cases, limit the units themselves.

[0170] It should be noted that, although several modules or units of the equipment for action execution are mentioned in the above detailed description, this division is not mandatory. In fact, according to an embodiment of the present invention, the features and functions of two or more modules or units described above can be embodied in one module or unit. On the contrary, the features and functions of one module or unit described above can be further divided into being embodied by multiple modules or units.

[0171] It should be noted that, as another aspect, the present application also provides a storage medium, which can be included in an electronic device; or it can exist independently without being installed in the electronic device. The above storage medium carries one or more programs, and when the above one or more programs are executed by an electronic device, the electronic device implements the method described in the following embodiments. For example, the electronic device can implement the following Figure 1 The individual steps of the method are shown.

[0172] In one embodiment, the present application provides a computer program product, including a computer program, which implements the steps in the above-mentioned method embodiments when executed by a processor.

[0173] In addition, the above-mentioned figures are only schematic illustrations of the processes included in the method according to an exemplary embodiment of the present invention, and are not intended to be limiting. It is easy to understand that the processes shown in the above-mentioned figures do not indicate or limit the time sequence of these processes. In addition, it is also easy to understand that these processes can be performed synchronously or asynchronously, for example, in multiple modules.

[0174] Other embodiments of the invention will readily occur to those skilled in the art after considering the specification and practicing the invention herein. This application is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art that are not disclosed by the present invention. The specification and examples are to be considered exemplary only, and the true scope and spirit of the invention are indicated by the claims.

[0175] It should be understood that the present invention is not limited to the exact construction that has been described above and shown in the drawings and that various modifications and changes may be made without departing from the scope thereof. The scope of the present invention is limited only by the appended claims.

Claims

1. A method for autonomous tracking trajectory planning of a drone, characterized in that: The method comprises: Acquire current state information of the target; wherein the current state information includes position information and speed information; Based on the preset time interval information, the current state information, and the discrete control amount corresponding to the target, position prediction is performed to obtain multiple motion primitives; wherein the motion primitives include: position information, speed information, and acceleration information; the discrete control amount is configured based on the maximum acceleration that the target can achieve; Performing Markov state transfer processing on each of the motion primitives to obtain the corresponding occurrence probability of each of the motion primitives, and configuring them as potential points; configuring target position prediction results based on the potential points; Determine a mean point according to the position information of each potential point in the target position prediction result, and perform viewpoint sampling based on the mean point to determine the position information and the optimal observation yaw angle corresponding to each viewpoint; When it is determined that the target is currently within the field of view of the drone, the viewpoints are screened based on the probability of occurrence of each potential point within the best observation yaw angle range of each viewpoint to determine the global optimal viewpoint; The global optimal viewpoint is configured as a planning end point, tracking trajectory planning is performed based on the planning end point to obtain an autonomous tracking trajectory, and a yaw angle of the autonomous tracking trajectory is configured to be oriented toward the mean point.

2. The method according to claim 1, characterized in that: Position prediction includes at least one round; The method further comprises: Based on the motion primitives corresponding to the position prediction of the current round, the position prediction of the next round is performed to obtain the motion primitives corresponding to the position prediction of the next round.

3. The method according to claim 1 or 2, characterized in that: The method further comprises: When it is determined that the target is currently lost within the field of view of the drone, the mean point corresponding to this round is configured according to the position information of each potential point corresponding to the position prediction in each round, and viewpoint sampling is performed based on the mean point of this round to determine the position information and the optimal observation yaw angle corresponding to each viewpoint in this round; Determine the corresponding probability and according to the probability of occurrence of each potential point within the best observation yaw angle range of each viewpoint in this round; Determine the capture cost corresponding to each viewpoint by combining the probability and the distance between the viewpoint and the nearest obstacle; and configure the viewpoint corresponding to the maximum capture cost as the capture viewpoint corresponding to this round; According to the captured viewpoints corresponding to each round, tracking trajectory planning is performed to obtain an autonomous tracking trajectory; and the yaw angle of the autonomous tracking trajectory is configured according to the optimal observation yaw angle corresponding to each captured viewpoint.

4. The method according to claim 3, characterized in that When determining the capture cost corresponding to each viewpoint, the method further includes: Configure the distance constraint between viewpoints according to the distance difference between adjacent viewpoints; The capture cost corresponding to each viewpoint is determined by combining the distance constraint between viewpoints, the probability sum, and the distance between the viewpoint and the nearest obstacle.

5. The method according to claim 3, characterized in that: The method further comprises: Construct dynamic feasibility constraints based on the maximum speed and maximum acceleration of the UAV; Minimize the control points of the high-order derivatives of the B-spline trajectory and construct smoothness constraints; Based on the distance between the drone and the obstacle, the obstacle distance safety constraint condition is constructed; Based on the mean points corresponding to the motion primitives expanded in each round and the preset tracking distance threshold, a tracking distance constraint condition is constructed; At least one of the above constraints is used to configure a target constraint, and based on the target constraint, the autonomous tracking trajectory is optimized using a B-spline curve algorithm.

6. The method according to claim 5, characterized in that The target constraints also include: yaw angle trajectory optimization constraints; The yaw angle trajectory optimization constraint conditions include: any one or more of yaw angle dynamics constraint, smoothness constraint, and trajectory direction constraint conditions based on yaw angle configuration.

7. The method according to claim 1 or 2, characterized in that: The method further comprises: The potential points are screened based on a preset probability threshold, so as to configure the target location prediction result according to the screened potential points.

8. The method according to claim 1, characterized in that: The performing Markov state transfer processing on each of the motion primitives to obtain the corresponding occurrence probability of each of the motion primitives includes: Determine a corresponding obstacle avoidance compensation vector based on the distance between the motion primitive and a plurality of surrounding obstacles; The corresponding occurrence probability is determined by combining the state transfer matrix of the Markov chain, the occurrence probability of the motion primitive at the previous moment, and the obstacle avoidance compensation vector.

9. The method according to claim 1, characterized in that: The configuring of the mean point according to the position information of each potential point, and performing viewpoint sampling based on the mean point to determine the position information and the best observation yaw angle corresponding to each viewpoint includes: Calculating a position mean according to the position information of each potential point, and configuring a mean point based on the position mean; Taking the mean point as the center and a plurality of radii of different lengths as the circle, sampling is performed at preset intervals on the circumference to obtain the viewpoints and determine the position of each viewpoint; The corresponding optimal observation yaw angle is determined according to the viewing angle of the most potential points observed from each viewpoint.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the method for autonomous tracking trajectory planning of a drone according to any one of claims 1 to 9 is implemented.

Images