Methods and Systems for Local Trajectory Replanning of Unmanned Aerial Vehicles

Abstract

This invention relates to a method and system for local trajectory replanning of unmanned aerial vehicles (UAVs). Current inspection UAVs suffer from difficulties in target point selection during replanning, inability to reach preset target points, and a tendency to get stuck in local extrema. This invention introduces a linear gravitational potential field to establish an improved artificial potential field model; obtains the minimum distance between the UAV and the target point, obstacles, and the predetermined trajectory; calculates the gravitational, repulsive, and resultant forces; obtains the UAV's horizontal and vertical movement directions; based on the planning step size, obtains the UAV's next position as a new node; sets the new node as the new current point; updates and iterates the artificial potential field model; detects the UAV's status; and terminates the planning process when the termination condition is met. This invention eliminates the need for UAVs to select local target points during local trajectory replanning, enabling them to accurately reach preset target points when approaching the target location, and effectively preventing UAVs from stalling or wandering within a certain area during inspections, thus possessing high practical value.

Description

Technical Field

[0001] This invention relates to the field of trajectory planning technology, specifically to a method and system for local trajectory replanning of unmanned aerial vehicles (UAVs). Background Technology

[0002] The inspection of the high-speed railway overhead contact system is essential to ensure the good condition and normal operation of critical railway and power facilities. Traditional inspections are typically done manually, with inspectors carrying testing instruments to inspect and maintain the facilities. This process is extremely resource-intensive. The overhead contact system is a crucial component of railway electric locomotive operation, directly supplying power from traction substations. Its quality and operational status directly impact the electrified railway's transport capacity. Furthermore, because the contact system is exposed to the elements without backups, and the load on the line moves and changes along the contact wire with the locomotives, ensuring its proper operation is increasingly vital for maintaining the safe, stable, and efficient operation of trains. The use of robots, especially unmanned aerial vehicles (UAVs), to replace manual inspections is becoming increasingly widespread. Their three-dimensional mobility greatly expands the scope of robotic inspections, making them suitable for various scenarios. UAV patrol technology provides high-resolution images and video data, helping staff better monitor and diagnose operational problems in the high-speed railway overhead contact system, improving maintenance and repair efficiency, reducing the risks of working at heights, and enhancing the safety of maintenance and repair work.

[0003] Artificial potential field methods are commonly used for UAV trajectory planning, but their application in local trajectory planning still faces several challenges. First, UAV trajectory planning suffers from difficulty in target point selection during replanning. When replanning a local trajectory, a local target point needs to be determined, but in practice, the UAV only has local environmental information, and the target point may be within obstacle range, leading to path planning failure. Second, UAV trajectory planning may fail to reach the preset target point. Approaching the target location, the net force acting on the UAV becomes a repulsive force, preventing it from reaching the target. Third, UAV trajectory planning is prone to getting trapped in local extrema. In some situations, the net force acting on the UAV is zero, causing it to stagnate or wander within a certain area.

[0004] Therefore, there is an urgent need to propose a new method for replanning the local trajectory of UAVs to overcome the aforementioned problems existing in the current method of using artificial potential fields for UAV local trajectory planning. Summary of the Invention

[0005] The purpose of this invention is to provide a method and system for local trajectory replanning of unmanned aerial vehicles (UAVs), and to improve the artificial potential field algorithm to solve the problems of difficulty in selecting target points during replanning of inspection UAVs, inability to reach preset target points, and easy getting trapped in local extreme points.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] A method for local trajectory replanning of unmanned aerial vehicles (UAVs), the method comprising:

[0008] An improved artificial potential field model is established by introducing a linear gravitational potential field, and the planning step size is determined.

[0009] The system obtains the distance between the drone and the target point, the distance between the drone and the obstacle, the minimum distance between the drone and the predetermined flight path, calculates the magnitude and direction of the gravitational, repulsive, and resultant forces, and obtains the drone's horizontal running angle and vertical motion direction.

[0010] Based on the planned step size, the next position of the drone is obtained as a new node;

[0011] Set the newly obtained node as the new current point and update the artificial potential field model iteratively.

[0012] The system checks the drone's status to determine if the termination conditions have been met. If the termination conditions are met, the planning process ends; otherwise, it returns to the next update iteration.

[0013] Furthermore, an improved artificial potential field model is established by introducing a linear gravitational potential field, including:

[0014] Establish the gravitational potential field function U at the target point att_goal :

[0015] U att_goal =ζρ(q,q goal )

[0016] in:

[0017] ζ is the gravitational gain coefficient at the target point;

[0018] ρ(q,q goal () represents the distance between the drone and the target point;

[0019] Establish the linear gravitational potential field function U att_line :

[0020]

[0021] in:

[0022] k is the linear gravitational gain coefficient;

[0023] ρ(q,q line () represents the minimum distance from the drone to the preset flight path;

[0024] d line The distance threshold of the linear gravitational potential field;

[0025] Combined with the gravitational potential field function U at the target point att_goal and the gravitational potential field function U att_line To obtain the improved gravitational field function U att :

[0026] U att =U att_goal +U att_line

[0027] Establish an improved repulsive field function U rep :

[0028]

[0029] in:

[0030] η is the repulsive force gain coefficient;

[0031] ρ(q,q0) represents the distance between the drone and the obstacle;

[0032] ρ0 represents the distance affected by the repulsive force;

[0033] k ov This is the relative velocity gain coefficient;

[0034] v or The relative speed between the drone and the obstacle;

[0035] σ is the proportionality coefficient for dynamically adjusting the linear potential force, σ=1-2ρ(q,q0) / ρ0;

[0036] Combined with the improved gravitational field function U att and the improved repulsive field function U rep An improved artificial potential field model was obtained.

[0037] Furthermore, obtaining the minimum distance between the drone and the predetermined flight path includes:

[0038] Calculate the current point P of the drone k The distance d to all characteristic vertices of the predetermined trajectory curve r(u) i ;

[0039] Find the minimum value e k =min(d i If ρ(q,q) is the minimum distance from the UAV to the preset flight path, then ρ(q,q) is the minimum distance from the UAV to the preset flight path. line ) falls on e k curve segments r on both sides of the corresponding point k-1 (u) and r k (u) up;

[0040] curve segment r k-1 (u) and rk (u) Divide it into equal parts along its main direction, and find the distance from each division point to P. k Find the minimum distance among the distances. The minimum distance ρ(q,q) from the UAV to the preset flight path line ) distributed in Within the adjacent interval of the corresponding point;

[0041] Calculate the distance between the dividing points of adjacent intervals. If the distance is less than the calculation precision, then the shortest distance is obtained. If the calculation accuracy is not achieved, then... The adjacent intervals of the corresponding points are then divided again.

[0042] Furthermore, the magnitude and direction of the gravitational, repulsive, and net forces are calculated, including:

[0043] Calculate gravity and its direction based on the improved gravitational field function;

[0044] Based on the improved repulsive field function, the repulsive force and its direction are calculated. The repulsive force consists of four parts:

[0045]

[0046]

[0047]

[0048] F rep3 =kv or e v

[0049]

[0050] in:

[0051] This indicates that the repulsive field function is differentiated with respect to distance and relative velocity;

[0052] F rep1 The repulsive component;

[0053] e q0 The direction vector from the obstacle to the drone;

[0054] F rep2 For gravitational components;

[0055] e qg This is the direction vector of the drone pointing towards the target point;

[0056] F rep3 For velocity potential force;

[0057] e vThis is the direction vector of the relative velocity between the drone and the obstacle;

[0058] F rep4 For linear attraction;

[0059] e ql It is the direction vector opposite to the linear gravitational force;

[0060] Calculate F rep1 F rep2 F rep3 F rep4 The resultant force and its direction.

[0061] Furthermore, based on the planned step size, the next position of the drone is obtained, including:

[0062] Get the current point P of the drone k coordinates (x) k ,y k ,z k );

[0063] Using R as the planning step size, obtain the next position P of the UAV. k+1 coordinates (x) k+1 ,y k+1 ,z k+1 ):

[0064]

[0065] Furthermore, the termination conditions include:

[0066] The distance between the drone and the target point is less than a set threshold;

[0067] The distance between the drone and the preset flight path is less than the set threshold and there are no new sudden obstacles;

[0068] The number of update iterations has reached the preset maximum.

[0069] Furthermore, the method also includes:

[0070] Determine the conditions for introducing dynamic forces, and introduce dynamic forces, including:

[0071] The condition for introducing dynamic forces is that the angle between the directions of repulsive and attractive forces is θ∈(π±β), where β is the threshold of the angle between attractive and repulsive forces;

[0072] Introducing dynamic forces:

[0073]

[0074] in:

[0075] κ is the adjustment coefficient;

[0076] When the drone approaches an obstacle, the dynamic force increases accordingly; when the angle between the repulsive force and the attractive force acting on the drone is ≤θ, the dynamic force is added, otherwise the dynamic force is canceled.

[0077] On the other hand, a local flight path replanning system for unmanned aerial vehicles (UAVs) is provided, the system being used to implement the method, including:

[0078] The model building module is used to introduce the linear gravitational potential field to build an improved artificial potential field model and determine the planning step size;

[0079] The calculation and acquisition module is used to obtain the distance between the UAV and the target point, the distance between the UAV and the obstacle, the minimum distance between the UAV and the predetermined flight path, calculate the magnitude and direction of the gravitational force, repulsive force and resultant force, and obtain the horizontal running angle and vertical motion direction of the UAV.

[0080] The step size update module is used to obtain the next position of the drone based on the planned step size, and use it as a new node;

[0081] The update iteration module is used to set the newly obtained node as the new current point and update the artificial potential field model.

[0082] The detection and judgment module is used to detect the drone's status and determine whether the termination condition has been met. If the termination condition is met, the planning ends; otherwise, it returns to the next update iteration.

[0083] Furthermore, the system also includes a dynamic force introduction module, used to determine the conditions for introducing dynamic forces and to introduce dynamic forces.

[0084] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0085] The method provided by this invention improves the artificial potential field algorithm, enabling UAVs to actively avoid obstacles and smoothly return to their original preset trajectory during flight. By adding a linear gravitational potential field to the gravitational field function, the UAV can return to its original preset trajectory under the attraction of linear gravity after obstacle avoidance, avoiding the difficulty of target point selection during local replanning. By adding a decision factor between the UAV and the target point and incorporating the relative velocity information between the UAV and the obstacle to the repulsive field function, the repulsive function is modified to eliminate linear gravity, thus solving the problem of the UAV being unable to reach the preset target point. Furthermore, by introducing dynamic forces, the problem of the UAV getting stuck in local extreme points and being unable to plan its trajectory is solved. Through the implementation of this method, the UAV no longer needs to determine local target points during local trajectory replanning, can accurately reach the preset target point when approaching the target location, and effectively prevents the UAV from stalling or wandering in one area during inspection, avoiding critical point oscillations. This method has high practical value in the field of UAV inspection. Attached Figure Description

[0086] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other embodiments can be obtained from these drawings without creative effort.

[0087] Figure 1 This is a flowchart of the method of the present invention.

[0088] Figure 2 This is a schematic diagram showing the minimum distance from a point to a segment point of a curve.

[0089] Figure 3 This is a schematic diagram of dividing adjacent curve segments.

[0090] Figure 4 This is a schematic diagram of the drone's trajectory replanning results. Detailed Implementation

[0091] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Preferred embodiments of the invention are shown in the drawings. However, the invention can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a thorough and complete understanding of the disclosure of the invention.

[0092] It should be noted that similar names, symbols, and letters indicate similar items; therefore, once an item is defined in one embodiment, it does not need to be further defined and explained in subsequent embodiments. The terms "comprising" and any variations thereof are intended to cover non-exclusive inclusion, meaning that a process, method, system, product, or apparatus that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0093] Current overhead contact line inspection drones often require operators to control them via radio remote control to fly along power transmission lines and conduct inspections. To avoid collisions, operators must remain close to both the drone and the power lines. However, high-speed rail overhead contact lines are typically long and traverse complex terrain such as mountains, rivers, swamps, and tunnels. Considering high-speed rail safety, inspection personnel should not walk on the rail lines or stay near the tracks for extended periods. Without the rail lines, navigating these complex terrains is extremely difficult. Operators cannot guarantee they will not stray too far from the contact line, cannot accurately observe the distance between the drone and the contact line, and are unable to complete the inspection task or guarantee that the drone will not collide with the contact line. Autonomous drone inspection can solve these problems and is therefore highly feasible.

[0094] Example 1:

[0095] This embodiment relates to a method for local trajectory replanning of an unmanned aerial vehicle (UAV). The steps can be executed in a computer system, such as a set of computer-executable instructions. Although the logical order is shown in the flowchart, in some cases, they can be executed in a different order. Figure 1 The methods include:

[0096] S1: Introduce a linear gravitational potential field to establish an improved artificial potential field model and determine the planning step size. The improved artificial potential field model includes an improved gravitational field function and an improved repulsive field function, specifically including the following steps:

[0097] S101: Establish the gravitational potential field function U at the target point att_goal :

[0098] U att_goal =ζρ(q,q goal )

[0099] in:

[0100] ζ is the gravitational gain coefficient at the target point;

[0101] ρ(q,q goal ) represents the distance between the drone and the target point.

[0102] S102: Establish the linear gravitational potential field function U att_line :

[0103]

[0104] in:

[0105] k is the linear gravitational gain coefficient;

[0106] ρ(q,q line) This is the minimum distance the drone must travel to the preset flight path.

[0107] d line The distance threshold of the linear gravitational potential field.

[0108] S103: Combining the gravitational potential field function U at the target point att_goal and the gravitational potential field function U att_line To obtain the improved gravitational field function U att :

[0109] U att =U att_goal +U att_line

[0110] The improved gravitational field function of this method, in addition to the gravitational field, also introduces the linear gravitational force exerted by the preset flight path on the UAV. This eliminates the need for local target point selection during local replanning, ensuring that the UAV can return to the original planned flight path under the influence of linear gravity after active obstacle avoidance. The improved linear gravitational potential field is piecewise: a quadratic function of the distance from the UAV to the preset flight path is used near the path, while a linear function is used at greater distances, with terms added to ensure continuity at the point of segmentation. Specifically, when the distance from the UAV to the preset flight path is less than a threshold value for the linear gravitational potential field, the function is quadratic and continuously differentiable at the preset flight path. When the distance exceeds the threshold value, the function becomes linear, resulting in a fixed linear gravitational force.

[0111] S104: Establish the improved repulsive field function U rep :

[0112]

[0113] in:

[0114] η is the repulsive force gain coefficient;

[0115] ρ(q,q0) represents the distance between the drone and the obstacle;

[0116] ρ0 represents the distance affected by the repulsive force;

[0117] k ov This is the relative velocity gain coefficient;

[0118] v or The relative speed between the drone and the obstacle;

[0119] σ is the proportionality coefficient for dynamically adjusting the linear potential force, σ=1-2ρ(q,q0) / ρ0, which makes the linear attraction gradually increase as it approaches the obstacle, thus counteracting the linear attraction.

[0120] To address the issue of UAVs failing to reach their target due to a net repulsive force as they approach, this method improves the repulsive field function by incorporating a decision factor ρ between the target point and the UAV. n (q,q goal The distance between the drone and the target point is the nth power of the distance between the drone and the obstacle. The relative speed information between the drone and the obstacle is added to improve its dynamic obstacle avoidance performance. At the same time, the influence of the addition of the linear gravitational potential field in the gravitational field on obstacle avoidance must be considered. The linear gravitational potential field can guide the drone to return to the preset route after avoiding the obstacle, but the force of the linear gravitational potential field will cancel out part of the repulsive force, which has an adverse effect on obstacle avoidance itself. Therefore, the repulsive field function is modified to eliminate the adverse effect of the linear gravitational potential field.

[0121] S105: Combining the improved gravitational field function U att and the improved repulsive field function U rep An improved artificial potential field model was obtained.

[0122] S106: Determine the planning step size R based on the speed and sampling time. In this method, the step size is set to 0.2.

[0123] When building the model, it is necessary to determine the gain coefficients of attraction, repulsion, and linear attraction, determine the distance of influence of repulsion, initialize the parameters based on the current environmental information and the preset flight path, and set the number of iterations to k=0.

[0124] S2: Obtain the distance between the drone and the target point, the distance between the drone and the obstacle, the minimum distance between the drone and the predetermined trajectory, calculate the magnitude and direction of the gravitational, repulsive, and resultant forces, and obtain the drone's horizontal running angle and vertical motion direction.

[0125] S201: Obtain the distance between the drone and the target point.

[0126] S202: Obtain the distance between the drone and obstacles.

[0127] S203: Obtain the minimum distance between the UAV and the predetermined flight path, including:

[0128] S20301: Calculate the current point P of the UAV k The distance d to all characteristic vertices of the predetermined trajectory curve r(u) i .

[0129] The shortest distance from the UAV to the predetermined trajectory is the distance from the point to the irregular curve. This method assumes that the predetermined trajectory curve is a complex B-spline curve and uses the segmentation approximation method for calculation.

[0130] S20302: Find the minimum value e k =min(d iIf ρ(q,q) is the minimum distance from the UAV to the preset flight path, then ρ(q,q) is the minimum distance from the UAV to the preset flight path. line ) falls on e k curve segments r on both sides of the corresponding point k-1 (u) and r k (u) on.

[0131] S20303: Transform curve segment r k-1 (u) and r k (u) Divide it equally along its principal direction, see Figure 2 and Figure 3 We can divide the sample into four equal parts and find the distance from each division point to point P. k Find the minimum distance among the distances. The minimum distance ρ(q,q) from the UAV to the preset flight path line ) distributed in Within the adjacent intervals of the corresponding points.

[0132] S20304: Calculate the distance between the dividing points of adjacent intervals. If it is less than the calculation precision, the shortest distance is finally obtained. If the calculation accuracy is not achieved, then... The adjacent intervals of the corresponding points are then divided again.

[0133] Repeat step S20304 until the required calculation accuracy is achieved. Experiments show that this calculation method achieves high accuracy after three or four divisions, meeting the requirements for calculating the linear potential field. Furthermore, its calculation time is short, satisfying the real-time requirements of local trajectory planning.

[0134] S204: Calculate the magnitude and direction of gravitational, repulsive, and resultant forces, including:

[0135] S20401: Calculate gravity and its direction based on the improved gravitational field function;

[0136] S20402: Based on the improved repulsive field function, calculate the repulsive force and its direction. The repulsive force consists of four parts:

[0137]

[0138]

[0139]

[0140] F rep3 =kv or e v

[0141]

[0142] in:

[0143] This indicates that the repulsive field function is differentiated with respect to distance and relative velocity;

[0144] F rep1 e is the repulsive component. q0 The direction vector pointing from the obstacle to the drone gradually decreases as the distance to the target point decreases;

[0145] F rep2 e is the gravitational component. qg This is the direction vector of the UAV pointing towards the target point, which increases as the distance to the target point decreases;

[0146] F rep3 For the velocity potential force, e v This is the direction vector of the relative velocity between the drone and the obstacle;

[0147] F rep4 For linear attraction, e ql It is the direction vector opposite to the linear gravitational force, in order to counteract the linear gravitational force.

[0148] When a drone is about to reach its target location, the gravitational force it experiences increases with distance, while the repulsive force decreases accordingly, effectively solving the problem of being unable to reach the target location.

[0149] S20403: Calculate F rep1 F rep2 F rep3 F rep4 The resultant force and its direction.

[0150] S205: Based on the model, obtain the horizontal operating angle and vertical movement direction of the UAV.

[0151] S3: Based on the planned step size, obtain the next position of the drone as a new node.

[0152] S301: Obtain the current point P of the drone k coordinates (x) k ,y k ,z k );

[0153] S302: Using R as the planning step size, obtain the next position P of the UAV. k+1 coordinates (x) k+1 ,y k+1 ,z k+1 ):

[0154]

[0155] S4: Set the new node as the new current point, update the artificial potential field model, and update k = k + 1.

[0156] S5: Detect the drone's status and determine if the termination conditions have been met. If the termination conditions are met, end the planning; otherwise, return to the next update iteration. Termination conditions include:

[0157] 1. The distance between the drone and the target point is less than the set threshold;

[0158] 2. The distance between the drone and the preset flight path is less than the set threshold, and there are no new sudden obstacles;

[0159] 3. The number of updates and iterations has reached the preset maximum number.

[0160] Furthermore, this method addresses the problem of UAVs getting trapped in local minima by introducing dynamic forces. First, the conditions for adding dynamic forces are determined. If a dynamic normal force is added when the UAV enters a local extremum, although it can escape the local extremum, it will inevitably lead to an uneven flight path. Therefore, a dynamic force is added when the gravitational and repulsive forces are completely opposite, i.e., the angle between the directions of the repulsive and gravitational forces is θ∈(π±β). β is a threshold angle between the gravitational and repulsive forces. β is added because in reality, it is rare for the gravitational and repulsive forces to be completely opposite. When they are close to being opposite, although the UAV will not completely get trapped in a local extremum, it will still experience significant oscillations. To avoid this situation, β is added to ensure smooth obstacle avoidance. After determining the conditions, a dynamic force is added, with its direction perpendicular to the gravitational force. The dynamic force is:

[0161]

[0162] in:

[0163] κ is an adjustment coefficient, the magnitude of which is related to the aforementioned gravitational gain coefficient and repulsive gain coefficient.

[0164] When the drone approaches an obstacle, the dynamic force increases accordingly, which can effectively prevent the drone from entering a local extreme point or oscillating. When the angle between the repulsive force and the attractive force on the drone is ≤θ, the dynamic force is added; otherwise, the dynamic force is canceled.

[0165] Example 2:

[0166] This embodiment relates to a local trajectory replanning system for unmanned aerial vehicles (UAVs), which can be implemented in hardware or software to perform the method described in Embodiment 1. The system includes:

[0167] The model building module is used to introduce the linear gravitational potential field to establish an improved artificial potential field model and determine the planning step size, which corresponds to the content of step S1 in Example 1.

[0168] The calculation and acquisition module is used to obtain the distance between the UAV and the target point, the distance between the UAV and the obstacle, the minimum distance between the UAV and the predetermined flight path, calculate the magnitude and direction of the gravitational force, repulsive force and resultant force, and obtain the horizontal running angle and vertical motion direction of the UAV, corresponding to the content of step S2 in embodiment 1.

[0169] The step size update module is used to obtain the next position of the UAV according to the planned step size, which is used as the content of step S3 in embodiment 1 corresponding to the new node.

[0170] The update iteration module is used to set the obtained new node as the new current point and update the artificial potential field model, which corresponds to the content of step S4 in Example 1.

[0171] The detection and judgment module is used to detect the status of the drone and determine whether the termination condition has been met. If the termination condition is met, the planning ends; if the termination condition is not met, the process returns to the next update iteration, which corresponds to step S1 in embodiment 1.

[0172] This invention employs an improved artificial potential field algorithm for UAV obstacle avoidance and trajectory replanning. It improves the gravitational field function by incorporating the linear gravitational force of the predetermined trajectory on the UAV. During local replanning, the selection of local target points is eliminated, ensuring that the UAV can return to its original planned route under the influence of linear gravity after active obstacle avoidance. To address the issue of the UAV failing to reach the target point, this solution improves the repulsive field function by adding a decision factor from the target point to the UAV; it also incorporates the relative velocity information between the UAV and obstacles to enhance dynamic obstacle avoidance performance; and considering the impact of linear gravity on obstacle avoidance, it modifies the repulsive field function to eliminate its adverse effects. For the problem of the UAV getting trapped in local minima, this solution addresses it by introducing dynamic forces.

[0173] The above optimizations and improvements enable the UAV to actively avoid obstacles and perform local trajectory planning during flight. By using optimization algorithms, the optimal path of the UAV under the influence of the comprehensive potential field is calculated, ensuring the accuracy and efficiency of flight. Figure 4 The right-hand track is the preset reference track, and the left-hand track is the replanned track. It can be seen that the locally replanned track smoothly avoids the obstacle and returns to the preset reference track after the UAV flies over the obstacle.

[0174] Those skilled in the art will understand that all or part of the functions of the embodiments of the present invention can be implemented by hardware or by computer program. When all or part of the functions in the above embodiments are implemented by computer program, the program can be stored in a computer-readable storage medium, which may include: read-only memory, random access memory, disk, optical disk, hard disk, etc., and the program is executed by a computer to achieve the above functions. For example, the program can be stored in the memory of a device, and when the program in the memory is executed by the processor, all or part of the above functions can be achieved. In addition, when all or part of the functions in the above embodiments are implemented by computer program, the program can also be stored in a storage medium such as a server, another computer, disk, optical disk, flash drive, or portable hard drive, and can be downloaded or copied to the memory of a local device, or the system of the local device can be updated. When the program in the memory is executed by the processor, all or part of the functions in the above embodiments can be achieved.

[0175] The above examples illustrate the present invention only to aid in understanding it and are not intended to limit the scope of the invention. Those skilled in the art can make various simple deductions, modifications, or substitutions based on the principles of this invention.

Claims

1. A method for local trajectory replanning of unmanned aerial vehicles (UAVs), characterized by: The method includes: An improved artificial potential field model is established by introducing a linear gravitational potential field, and the planning step size is determined. The system obtains the distance between the drone and the target point, the distance between the drone and the obstacle, the minimum distance between the drone and the predetermined flight path, calculates the magnitude and direction of the gravitational, repulsive, and resultant forces, and obtains the drone's horizontal running angle and vertical motion direction. Based on the planned step size, the next position of the drone is obtained as a new node; Set the newly obtained node as the new current point and update the artificial potential field model iteratively. Detect the drone's status and determine if the termination condition has been met. If the termination condition has been met, end the planning; otherwise, return to the next update and iteration. Among them, an improved artificial potential field model is established by introducing a linear gravitational potential field, including: Establish the gravitational potential field function U at the target point att_goal : U att_goal =ζρ(q,q goal ) in: ζ is the gravitational gain coefficient at the target point; ρ(q,q goal () represents the distance between the drone and the target point; Establish the linear gravitational potential field function U att_line : in: k is the linear gravitational gain coefficient; ρ(q,q line () represents the minimum distance from the drone to the preset flight path; d line The distance threshold of the linear gravitational potential field; Combined with the gravitational potential field function U at the target point att_goal and the gravitational potential field function U att_line To obtain the improved gravitational field function U att : IN att =U att_goal +U att_line Establish an improved repulsive field function U rep : in: η is the repulsive force gain coefficient; ρ(q,q0) represents the distance between the drone and the obstacle; ρ0 represents the distance affected by the repulsive force; k ov This is the relative velocity gain coefficient; v or The relative speed between the drone and the obstacle; σ is the proportionality coefficient for dynamically adjusting the linear potential force, σ=1-2ρ(q,q0) / ρ0; Combined with the improved gravitational field function U att and the improved repulsive field function U rep An improved artificial potential field model was obtained.

2. The method according to claim 1, characterized in that: Obtain the minimum distance between the drone and the predetermined flight path, including: Calculate the current point P of the drone k The distance d to all characteristic vertices of the predetermined trajectory curve r(u) i ; Find the minimum value e k =min(d i If ρ(q,q) is the minimum distance from the UAV to the preset flight path, then ρ(q,q) is the minimum distance from the UAV to the preset flight path. line ) falls on e k curve segments r on both sides of the corresponding point k-1 (u) and r k (u) up; curve segment r k-1 (u) and r k (u) Divide it into equal parts along its main direction, and find the distance from each division point to P. k Find the minimum distance among the distances. The minimum distance ρ(q,q) from the UAV to the preset flight path line ) distributed in Within the adjacent interval of the corresponding point; Calculate the distance between the dividing points of adjacent intervals. If the distance is less than the calculation precision, then the shortest distance is obtained. If the calculation accuracy is not achieved, then... The adjacent intervals of the corresponding points are then divided again.

3. The method according to claim 2, characterized in that: Calculate the magnitude and direction of gravitational, repulsive, and net forces, including: Calculate gravity and its direction based on the improved gravitational field function; Based on the improved repulsive field function, the repulsive force and its direction are calculated. The repulsive force consists of four parts: F rep3 =kv or yes v in: This indicates that the repulsive field function is differentiated with respect to distance and relative velocity; F rep1 The repulsive component; e q0 The direction vector from the obstacle to the drone; F rep2 For gravitational components; e qg This is the direction vector of the drone pointing towards the target point; F rep3 For velocity potential force; e v This is the direction vector of the relative velocity between the drone and the obstacle; F rep4 For linear attraction; e ql It is the direction vector opposite to the linear gravitational force; Calculate F rep1 F rep2 F rep3 F rep4 The resultant force and its direction.

4. The method according to claim 3, characterized in that: Based on the planned step size, the next position of the drone is obtained, including: Get the current point P of the drone k coordinates (x) k ,y k ,z k ); Using R as the planning step size, obtain the next position P of the UAV. k+1 coordinates (x) k+1 ,y k+1 ,z k+1 ):

5. The method according to claim 4, characterized in that: Termination conditions include: The distance between the drone and the target point is less than a set threshold; The distance between the drone and the preset flight path is less than the set threshold and there are no new sudden obstacles; The number of update iterations has reached the preset maximum.

6. The method according to claim 5, characterized in that: The method further includes: Determine the conditions for introducing dynamic forces, and introduce dynamic forces, including: The condition for introducing dynamic forces is that the angle between the directions of repulsive and attractive forces is θ∈(π±β), where β is the threshold of the angle between attractive and repulsive forces; Introducing dynamic forces: in: κ is the adjustment coefficient; When the drone approaches an obstacle, the dynamic force increases accordingly; when the angle between the repulsive force and the attractive force acting on the drone is ≤θ, the dynamic force is added, otherwise the dynamic force is canceled.

7. A local trajectory replanning system for unmanned aerial vehicles (UAVs), characterized in that: The system is used to implement the method according to any one of claims 1-6, comprising: The model building module is used to introduce the linear gravitational potential field to build an improved artificial potential field model and determine the planning step size; The calculation and acquisition module is used to obtain the distance between the UAV and the target point, the distance between the UAV and the obstacle, the minimum distance between the UAV and the predetermined flight path, calculate the magnitude and direction of the gravitational force, repulsive force and resultant force, and obtain the horizontal running angle and vertical motion direction of the UAV. The step size update module is used to obtain the next position of the drone based on the planned step size, and use it as a new node; The update iteration module is used to set the newly obtained node as the new current point and update the artificial potential field model. The detection and judgment module is used to detect the drone's status and determine whether the termination condition has been met. If the termination condition is met, the planning ends; if the termination condition is not met, it returns to the next update and iteration. Among them, an improved artificial potential field model is established by introducing a linear gravitational potential field, including: Establish the gravitational potential field function U at the target point att_goal : U att_goal =ζρ(q,q goal ) in: ζ is the gravitational gain coefficient at the target point; ρ(q,q goal () represents the distance between the drone and the target point; Establish the linear gravitational potential field function U att_line : in: k is the linear gravitational gain coefficient; ρ(q,q line () represents the minimum distance from the drone to the preset flight path; d line The distance threshold of the linear gravitational potential field; Combined with the gravitational potential field function U at the target point att_goal and the gravitational potential field function U att_line To obtain the improved gravitational field function U att : IN att =U att_goal +U att_line Establish an improved repulsive field function U rep : in: η is the repulsive force gain coefficient; ρ(q,q0) represents the distance between the drone and the obstacle; ρ0 represents the distance affected by the repulsive force; k ov This is the relative velocity gain coefficient; v or The relative speed between the drone and the obstacle; σ is the proportionality coefficient for dynamically adjusting the linear potential force, σ=1-2ρ(q,q0) / ρ0; Combined with the improved gravitational field function U att and the improved repulsive field function U rep An improved artificial potential field model was obtained.

8. The system according to claim 7, characterized in that: The system also includes a dynamic force introduction module, which is used to determine the conditions for introducing dynamic forces and introduce dynamic forces.

Images