A trajectory planning method for unmanned aerial vehicle obstacle avoidance

Abstract

The present invention relates to the technical field of obstacle avoidance and trajectory planning for unmanned aerial vehicles, and specifically refers to a trajectory planning method for obstacle avoidance for unmanned aerial vehicles, including: using a three-dimensional Gauss-Markov model to generate a random motion trajectory of an unmanned aerial vehicle; using a two-dimensional Gauss-Markov model to determine the flight attitude of the unmanned aerial vehicle; judging whether to perform an obstacle avoidance operation / enter a boundary obstacle avoidance mode, when the unmanned aerial vehicle performs an obstacle avoidance operation, based on a repulsive field, a new heading angle and pitch angle are calculated, and the random motion trajectory and flight attitude of the unmanned aerial vehicle are adjusted; when the unmanned aerial vehicle enters a boundary avoidance mode, a new heading angle of the current unmanned aerial vehicle is obtained according to the two-dimensional position coordinates of the center of the environment and the two-dimensional position coordinates of the current unmanned aerial vehicle, and the random motion trajectory of the unmanned aerial vehicle is adjusted; until the preset flight time is reached, the final flight trajectory and flight attitude of the unmanned aerial vehicle are obtained. The present invention improves the flexibility and efficiency of unmanned aerial vehicles in performing tasks, and ensures that unmanned aerial vehicles can perform flight tasks safely and effectively in a port environment.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle obstacle avoidance and trajectory planning, and in particular to a trajectory planning method for unmanned aerial vehicle obstacle avoidance. Background Art

[0002] The importance of UAV trajectory planning in channel modeling of UAV communication is self-evident; in the field of channel modeling of UAV communication, fixed path trajectory planning or linear trajectory planning of UAV is often used to describe the trajectory of UAV. These two methods are feasible, but lack universal applicability and representativeness; in order to solve the above problems and better simulate the real flight trajectory of UAV, the use of random mobile models for UAV trajectory planning has gradually become a trend; however, with the increasing application of random mobile models in channel modeling, how to achieve the smoothness and flexibility of UAV flight paths has become a widely considered issue.

[0003] At present, the commonly used obstacle avoidance methods for drones include: path planning-based methods, visual SLAM (Simultaneous Localization and Mapping)-based obstacle avoidance methods, and laser radar-based obstacle avoidance methods. Path planning-based methods include global path planning methods and local path planning methods. Among them, for global path planning methods, such as A* algorithm, Dijkstra algorithm, etc., it is necessary to model the entire environment and calculate a global optimal path from the starting point to the end point. This calculation complexity is high. The local path planning method also has such a problem and may get stuck in solving the local optimal solution; the visual-based obstacle avoidance method needs to capture the environment image through the camera and perform complex image processing and feature extraction, but this method performs poorly in the case of light changes, occlusion, etc., and also has the problem of large amount of calculation; the laser radar-based obstacle avoidance method obtains the point cloud data of the environment through the laser radar, and performs real-time mapping and positioning. Although this method has high accuracy in complex environments, the laser radar equipment is expensive and the data processing is also complex.

[0004] In port scenarios, due to the influence of obstacles such as containers and cranes, as well as the influence of factors such as sea breeze, if the currently commonly used drone obstacle avoidance method is used to design the drone flight trajectory and realize the drone obstacle avoidance function, there are defects such as high computational complexity, high equipment cost, and complex data processing.

[0005] How to design a relatively realistic UAV flight trajectory while realizing the obstacle avoidance function of the UAV is an urgent problem to be solved.

[0006] In summary, the technical problems that the present invention needs to solve are: how to achieve the smoothness and flexibility of the UAV flight path, and how to design a relatively realistic UAV flight trajectory in the port scene and realize the obstacle avoidance function of the UAV. Summary of the invention

[0007] To this end, the technical problem to be solved by the present invention is to overcome the problem that the UAV flight path in the prior art lacks smoothness and flexibility; the UAV flight trajectory designed in the port scene is not realistic enough, deviates greatly from the actual UAV flight trajectory and cannot fully realize the obstacle avoidance function of the UAV.

[0008] In order to solve the above technical problems, the present invention provides a trajectory planning method for unmanned aerial vehicle obstacle avoidance, comprising:

[0009] Based on the three-dimensional Gauss-Markov model, the speed, spin angle, and heading angle of the UAV at each time step are obtained to generate the random motion trajectory of the UAV;

[0010] According to the random motion trajectory of the UAV, the two-dimensional Gauss-Markov model is used to obtain the pitch angle and roll angle of the UAV at each time step, and then determine the flight attitude of the UAV;

[0011] When the drone flies according to the drone's random motion trajectory and flight attitude, the drone uses an ultrasonic sensor to emit ultrasonic signals and receives return signals generated by reflection when the ultrasonic waves touch environmental obstacles / environmental boundaries; the drone system records the ultrasonic duration and detects the return signal; wherein the ultrasonic duration is the duration from the start of the ultrasonic wave emission to the reflection back to the sensor after it touches an obstacle;

[0012] If the ultrasonic duration is less than the preset time threshold, there is an obstacle in the preset area of ​​the drone, and the drone performs obstacle avoidance operations; the drone system calculates the distance between the current drone and the obstacle based on the ultrasonic propagation speed and the ultrasonic duration; after calculating the vector from the current drone to the obstacle based on the current drone's three-dimensional position vector and the current obstacle's three-dimensional coordinate vector, the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation is obtained by combining the distance between the current drone and the obstacle and the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation; after obtaining the current drone's direction vector based on the current drone's heading angle and pitch angle, the new direction vector of the current drone is obtained by combining the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation, and then the four-quadrant inverse tangent function is used to calculate the new heading angle and pitch angle of the current drone, and the Gauss-Markov model is used to adjust the drone's random motion trajectory and flight attitude;

[0013] If the UAV system detects that the energy of the return signal decreases, and the distance between the UAV and the environmental boundary is less than or equal to the preset boundary distance threshold, there is an environmental boundary in front of the UAV, and the UAV automatically enters the boundary avoidance mode. The UAV system obtains the new heading angle of the current UAV based on the two-dimensional position coordinates of the center of the environment and the two-dimensional position coordinates of the current UAV; at the same time, after the UAV enters the boundary avoidance mode, the avoidance counter starts counting. If the value of the avoidance counter exceeds the preset boundary avoidance duration, the UAV exits the boundary avoidance mode, and the UAV system resets the avoidance counter, and based on the new heading angle of the current UAV, uses the three-dimensional Gauss-Markov model to adjust the random motion trajectory of the UAV;

[0014] Continue to determine whether the drone is close to environmental obstacles / boundaries, whether to perform obstacle avoidance operations / enter boundary obstacle avoidance mode, until the preset flight time is reached, obtain the final flight trajectory and flight attitude of the drone, and complete the trajectory planning of the entire flight process of the drone in the current environmental scenario.

[0015] Preferably, the three-dimensional Gauss-Markov model includes:

[0016] The UAV speed at the previous time step, the expected mean speed, and the random disturbance term corresponding to the UAV speed are used to obtain the UAV speed at the current time step. The expression is:

[0017]

[0018] The spin angle of the drone at the previous time step, the expected mean spin angle, and the random disturbance term corresponding to the spin angle of the drone are used to obtain the spin angle of the drone at the current time step. The expression is:

[0019]

[0020] The heading angle of the UAV in the previous time step, the expected mean of the heading angle, and the random disturbance term corresponding to the heading angle of the UAV are used to obtain the heading angle of the UAV in the current time step. The expression is:

[0021]

[0022] Among them, T (t i ) represents the UAV speed at the i-th time step; ξ T (t i ) represents the spin angle of the i-th time step; γ T (t i ) represents the heading angle of the UAV at the i-th time step; t i represents the moment corresponding to the i-th time step; υ T (t0) represents the speed of the UAV at the initial moment; ξT (t0) represents the UAV rotation angle at the initial moment; γ T (t0) represents the heading angle of the UAV at the initial moment; t0 represents the initial moment; i represents the sequence number of the time step, i∈[0,N], N represents the total sequence number of the time step; represents the expected mean of the drone’s speed; represents the expected mean value of the UAV's spin angle; μ γT represents the expected mean of the heading angle of the UAV; represents the autoregressive coefficient corresponding to the UAV speed at the i-th time step; represents the autoregressive coefficient corresponding to the UAV spin angle at the i-th time step; represents the autoregressive coefficient corresponding to the heading angle of the UAV at the i-th time step; L j represents the random disturbance term corresponding to the UAV velocity at the jth time step; M j N represents the random disturbance term corresponding to the unmanned spin angle at the jth time step; j Represents the random disturbance term corresponding to the heading angle of the UAV at the j-th time step; j represents the sequence number of the time step, j∈[0,i-1].

[0023] Preferably, the two-dimensional Gauss-Markov model comprises:

[0024]

[0025] Among them, ψ T (t i ) represents the pitch angle of the UAV at the i-th time step; θ T (t i ) represents the rolling angle of the UAV at the i-th time step; t i Indicates the moment corresponding to the i-th time step; i represents the sequence number of the time step, i∈[0,N], N represents the total sequence number of the time step; t0 represents the initial moment; ψ T (t0) represents the pitch angle of the drone at the initial moment; θ T (t0) represents the rolling angle of the UAV at the initial moment; represents the expected mean value of the pitch angle of the drone; represents the expected mean of the UAV’s rolling angle; Represents the autoregressive coefficient corresponding to the pitch angle of the UAV at the i-th time step; represents the autoregressive coefficient corresponding to the UAV roll angle at the i-th time step; X j Y represents the random disturbance term corresponding to the pitch angle of the UAV at the jth time step; j Represents the random disturbance term corresponding to the UAV roll angle at the jth time step; j represents the sequence number of the time step, j∈[0,i-1].

[0026] Preferably, the UAV system calculates the distance between the current UAV and the obstacle according to the ultrasonic propagation speed and ultrasonic duration, and the expression is:

[0027]

[0028] Among them, d o Indicates the distance between the current drone and the obstacle; v s Indicates the propagation speed of ultrasonic wave; t m Indicates the duration of ultrasound.

[0029] Preferably, after calculating the vector from the current drone to the obstacle based on the current drone 3D position vector and the current obstacle 3D coordinate vector, the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation is obtained by combining the distance between the current drone and the obstacle and the accumulated repulsive force vector of the obstacle in the previous obstacle avoidance operation, including:

[0030] According to the current UAV 3D position vector and the current obstacle 3D coordinate vector, the vector from the current UAV to the obstacle is calculated. The expression is:

[0031]

[0032] Among them, V j Represents the vector from the current drone to the obstacle; O j =[O jx O jy O jz ] T Represents the three-dimensional coordinate vector of the current obstacle; [xyz] T Represents the current three-dimensional position vector of the drone;

[0033] According to the vector from the current drone to the obstacle, the distance between the current drone and the obstacle, and the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation, the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation is obtained, and its expression is:

[0034]

[0035] Among them, F new Indicates the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation; F re Indicates the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation and the cumulative repulsive force vector of the obstacle in the first obstacle avoidance operation F re =[0 0 0] T .

[0036] Preferably, after obtaining the current drone direction vector based on the current drone heading angle and pitch angle, obtaining the new direction vector of the current drone in combination with the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation includes:

[0037] Based on the current UAV heading angle and pitch angle, the current UAV direction vector is obtained, and its expression is:

[0038]

[0039] Among them, D now represents the direction vector of the current drone; γ represents the heading angle of the current drone; ψ represents the pitch angle of the current drone;

[0040] Based on the current drone direction vector and the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation, the new direction vector of the current drone is obtained, and its expression is:

[0041] D new =D now +F new

[0042] Among them, D new Indicates the new direction vector of the current drone; F new Indicates the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation.

[0043] Preferably, based on the new direction vector of the current drone, the four-quadrant inverse tangent function is used to calculate the new heading angle and pitch angle of the current drone, and the expressions thereof are:

[0044]

[0045] Among them, γ new Indicates the new heading angle of the current UAV; ψ new Indicates the new pitch angle of the current drone; D new,x Indicates the x-coordinate of the new direction vector of the current drone; D new,y Indicates the y coordinate of the new direction vector of the current drone; D new,z Indicates the z coordinate of the current drone's new direction vector.

[0046] Preferably, the UAV system obtains a new heading angle of the UAV according to the two-dimensional position coordinates of the center of the environment and the two-dimensional position coordinates of the current UAV, and the expression is:

[0047]

[0048] Among them, γ i represents the new heading angle of the UAV at the i-th time step; L and W represent the length and width of the environment scene respectively; (x i-1 ,yi-1 ) represents the two-dimensional position coordinates of the UAV at the (i-1)th time step.

[0049] Preferably, if the ultrasonic duration is greater than a preset time threshold, there are no environmental obstacles near the drone, and the drone continues to fly according to the current drone random motion trajectory and flight attitude.

[0050] Preferably, if the UAV system detects that the energy of the return signal decreases, and the UAV system detects that the distance between the UAV and the environmental boundary is greater than a preset boundary distance threshold, there is an environmental boundary in front of the UAV, and the UAV continues to fly according to the current random motion trajectory and flight attitude of the UAV; if the UAV system detects that the energy of the return signal does not decrease, there is no environmental boundary in front of the UAV, and the UAV continues to fly according to the current random motion trajectory and flight attitude of the UAV.

[0051] The above technical solution of the present invention has the following beneficial effects compared with the prior art:

[0052] The trajectory planning method for obstacle avoidance of unmanned aerial vehicles described in the present invention simulates the movement path of the unmanned aerial vehicle through the combination of Gaussian distribution and Markov chain, so that the movement trajectory of the unmanned aerial vehicle conforms to the movement mode in the real world to a certain extent. The Gauss-Markov random movement model can not only generate a smooth and continuous movement trajectory that conforms to the actual situation, but also adapt to complex environments and changeable task requirements, thereby improving the flexibility and efficiency of the unmanned aerial vehicle in performing tasks; the obstacle avoidance operation is designed, and the repulsive force that needs to be generated at this time is calculated according to the position of the obstacle and its position with the unmanned aerial vehicle, so that the unmanned aerial vehicle can avoid the obstacle; in complex environmental scenes, the designed obstacle avoidance operation based on repulsion can It can avoid obstacles through local information of obstacles, and the calculation complexity is low; in addition, the obstacle avoidance operation designed by the present invention adjusts the movement direction of the UAV through repulsion, avoids the action changes of UAV clustering, and makes the flight of the UAV more stable; a boundary avoidance mode is designed to avoid UAV collision or crossing the preset boundary, and at the same time, the UAV heading angle is updated in combination with the center coordinates of the environmental scene and the position coordinates of the UAV, thereby updating the flight trajectory of the UAV, so that the UAV can reasonably avoid the boundary. The boundary avoidance mechanism and the obstacle avoidance operation mechanism together constitute an important part of the automatic obstacle avoidance and path planning of the UAV, ensuring that the UAV can safely and effectively perform flight missions in the port environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to make the content of the present invention more clearly understood, the present invention is further described in detail below according to specific embodiments of the present invention in conjunction with the accompanying drawings, wherein:

[0054] Figure 1 It is a flow chart of a trajectory planning method for obstacle avoidance of a UAV provided by the present invention;

[0055] Figure 2 This is a schematic diagram of the UAV flight trajectory;

[0056] Figure 3 is the graph of the pitch angle and roll angle of the drone changing with time; Figure 3 The upper middle picture shows the pitch angle of the drone changing over time; Figure 3 The lower middle figure shows the change of the UAV's rolling angle over time. DETAILED DESCRIPTION

[0057] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention.

[0058] Reference Figure 1 As shown, Figure 1 The present invention provides a flow chart of a trajectory planning method for unmanned aerial vehicle obstacle avoidance; specifically, it includes:

[0059] S1: Set initial parameters, including: drone parameters, environmental parameters;

[0060] The drone parameters include: drone flight time, time step, drone initial speed, drone initial spin angle, drone initial heading angle, drone flight angle, and drone flight altitude limit;

[0061] Among them, setting the drone parameters includes: setting the drone flight time to 80 seconds; randomly generating the drone initial speed and flight angle; the formulas for the drone initial spin angle ξ0 and the drone initial heading angle γ0 are: ξ0=2π*R ξ -π,γ0=2π*R γ -π, R ξ and R γ are two random numbers between 0 and 1 that satisfy uniform distribution; in order to ensure the safety of the drone during flight and the stability of communication quality, the flight altitude of the drone is limited to 5 to 15 meters;

[0062] The environmental parameters include: the length and width of the port are both set to 100 meters, and obstacles of different sizes and heights are generated, and the threshold of the distance to the obstacles is set to 2 meters.

[0063] S2: Generate random motion trajectories and flight attitudes of drones using the Gauss-Markov model, including:

[0064] S21: Based on the three-dimensional Gauss-Markov model, the speed, spin angle, and heading angle of the drone at each time step are obtained to generate the random motion trajectory of the drone;

[0065] The three-dimensional Gauss-Markov model is:

[0066] Set the autoregressive coefficient of the drone speed, and use the drone speed of the previous time step, the expected mean speed, and the random disturbance term corresponding to the drone speed to obtain the drone speed of the current time step. The expression is:

[0067]

[0068] Set the autoregressive coefficient of the drone spin angle, and use the drone spin angle of the previous time step, the expected mean spin angle, and the random disturbance term corresponding to the drone spin angle to obtain the drone spin angle of the current time step. The expression is:

[0069]

[0070] Set the autoregressive coefficient of the UAV heading angle, and use the UAV heading angle of the previous time step, the expected mean of the heading angle, and the random disturbance term corresponding to the UAV heading angle to obtain the UAV heading angle of the current time step. The expression is:

[0071]

[0072] Among them, T (t i ) represents the UAV speed at the i-th time step; ξ T (t i ) represents the spin angle of the i-th time step; γ T (t i ) represents the heading angle of the UAV at the i-th time step; t i represents the moment corresponding to the i-th time step; υ T (t0) represents the speed of the UAV at the initial moment; ξ T (t0) represents the UAV rotation angle at the initial moment; γ T (t0) represents the heading angle of the UAV at the initial moment; t0 represents the initial moment; i represents the sequence number of the time step, i∈[0,N], N represents the total sequence number of the time step; represents the expected mean of the drone’s speed; represents the expected mean of the UAV’s spin angle; represents the expected mean of the heading angle of the UAV; represents the autoregressive coefficient corresponding to the UAV speed at the i-th time step; represents the autoregressive coefficient corresponding to the UAV spin angle at the i-th time step; represents the autoregressive coefficient corresponding to the heading angle of the UAV at the i-th time step; L j represents the random disturbance term corresponding to the UAV velocity at the jth time step; Mj N represents the random disturbance term corresponding to the unmanned spin angle at the jth time step; j represents the random disturbance term corresponding to the heading angle of the UAV at the jth time step; j represents the sequence number of the time step, j∈[0,i-1];

[0073] Among them, the expected mean provides a benchmark, so that the parameters of the drone can still fluctuate within a reasonable range when subjected to random disturbances; by setting the autoregressive coefficients of the drone's speed, spin angle, and heading angle and It is used to adjust the randomness and time correlation of variables. Specifically, the autoregressive coefficient determines the correlation between the value of the current time step and the value of the previous time step. By adjusting the autoregressive coefficient, the smoothness and volatility of the variable can be controlled. The present invention sets each autoregressive coefficient to 0.9 to improve the time correlation, so that the UAV maintains a stable flight; an independent random disturbance term is introduced, namely L j 、M j and N j Three independent and uncorrelated Gaussian distribution random variables make the speed and angle of the drone at each time step not only depend on the value and mean of the previous time step, but also be affected by random disturbances; when the autoregressive coefficient is 0.9, small random fluctuations will still be introduced at each time step to act as the influence of factors such as wind resistance, thereby simulating a more realistic flight trajectory;

[0074] In short, in S21, the entire time series is obtained from the UAV flight time and time step; the autoregressive coefficients of the speed, spin angle and heading angle are set, and the speed, spin angle and heading angle in the entire time series are generated through a loop; each new state is determined by the previous state, the expected value and a random disturbance term to generate a random motion trajectory of the UAV.

[0075] S22: According to the random motion trajectory of the UAV, the two-dimensional Gauss-Markov model is used to obtain the pitch angle and roll angle of the UAV at each time step, and then determine the flight attitude of the UAV;

[0076] The two-dimensional Gauss-Markov model is:

[0077]

[0078] Among them, ψ T (t i ) represents the pitch angle of the UAV at the i-th time step; θ T (t i ) represents the rolling angle of the UAV at the i-th time step; t iIndicates the moment corresponding to the i-th time step; i represents the sequence number of the time step, i∈[0,N], N represents the total sequence number of the time step; t0 represents the initial moment; ψ T (t0) represents the pitch angle of the drone at the initial moment; θ T (t0) represents the rolling angle of the UAV at the initial moment; represents the expected mean value of the pitch angle of the drone; represents the expected mean of the UAV’s rolling angle; Represents the autoregressive coefficient corresponding to the pitch angle of the UAV at the i-th time step; represents the autoregressive coefficient corresponding to the UAV roll angle at the i-th time step; X j Y represents the random disturbance term corresponding to the pitch angle of the UAV at the jth time step; j represents the random disturbance term corresponding to the UAV roll angle at the jth time step; j represents the sequence number of the time step, j∈[0,i-1];

[0079] After planning the flight trajectory of the drone, its flight attitude, namely the spin angle and heading angle, is simulated during the flight. The flight attitude of the drone is described by a two-dimensional Gauss-Markov random move model. The expected mean of the pitch angle and roll angle of the drone is calculated. and Set it to 0 so that its pitch angle and roll angle fluctuate around 0, indicating that the drone will maintain its original heading without external interference; set the autoregressive coefficient of the drone's pitch angle and roll angle and is 0.9, which makes the UAV's flight attitude maintain a high correlation under the condition of certain randomness, which is in line with the actual situation; j and Y j Two independent and uncorrelated Gaussian distribution random variables are used as random disturbance terms, so that the pitch angle and roll angle of the drone at each time step do not only depend on the angle and mean of the previous time step; similar to the idea of ​​planning the flight trajectory of the drone, the influence of factors such as wind resistance is taken into account, so as to simulate a drone posture that is more in line with reality;

[0080] In S22, the parameters of the UAV flight attitude are defined, including the autoregressive coefficients of the pitch angle and the roll angle, and the pitch angle and the roll angle in the entire time series are generated through a loop;

[0081] In summary, after setting the initial parameters, the flight duration and range of the UAV are determined, and the flight trajectory and attitude of the UAV are designed in S2; in view of the influence of wind factors in the port scene, the flight trajectory and angle of the UAV may be disturbed at some times. Therefore, a three-dimensional Gauss-Markov random mobile model is used, and the adjustment coefficient is set. A new state is generated through the value of the previous time step, the expected value and a random disturbance term to form a random motion trajectory of the UAV; after the trajectory of the UAV is formed, the flight attitude of the UAV is designed. The changes in these angles not only depend on the trajectory of the UAV, but also are affected by the mechanical model, flight control logic and aerodynamic parameters of the UAV. Therefore, a two-dimensional Gauss-Markov mobile model is used to simulate the flight attitude through continuous random distribution.

[0082] S3: When the drone is flying according to the random motion trajectory and flight attitude, the ultrasonic sensor is used to determine the distance between the drone and obstacles and boundaries, so as to determine whether to perform obstacle avoidance and boundary avoidance operations, including:

[0083] S31: using the ultrasonic sensor carried by the drone to transmit an ultrasonic signal, and receiving a return signal generated by reflection when the ultrasonic wave contacts an environmental obstacle / environmental boundary; using the drone system to record the ultrasonic duration, and detecting the return signal; wherein the ultrasonic duration is the duration from the start of the ultrasonic wave transmission to the reflection back to the sensor after contacting an obstacle;

[0084] S32: If the ultrasound duration t m Less than the preset time threshold t s , that is, t m <t s , then there is an obstacle in the preset area of ​​the drone, and the drone performs obstacle avoidance operation, that is, enters S4;

[0085] If the ultrasound duration is longer than the preset time threshold, t m >t s , there are no environmental obstacles in the preset area of ​​the drone, and the drone continues to fly according to the current random motion trajectory and flight attitude of the drone, so there is no need to calculate the distance d o ;

[0086] S33: If the UAV system detects that the energy of the return signal decreases, and the distance between the UAV and the environmental boundary is less than or equal to the preset boundary distance threshold, there is an environmental boundary in front of the UAV, and the UAV automatically enters the boundary avoidance mode, that is, enters S5;

[0087] If the UAV system detects that the energy of the return signal decreases, and the UAV system detects that the distance between the UAV and the environmental boundary is greater than the preset boundary distance threshold, there is an environmental boundary in front of the UAV, and the UAV continues to fly according to the current UAV random motion trajectory and flight attitude; if the UAV system detects that the energy of the return signal does not decrease, there is no environmental boundary in front of the UAV, and the UAV continues to fly according to the current UAV random motion trajectory and flight attitude;

[0088] Among them, in the process of boundary detection of drones, the boundary processing method is similar to the principle of obstacle detection; in order to facilitate detection, the boundary of the port scene is assumed to be a special material, which can not only absorb part of the ultrasonic energy (similar to sound-absorbing materials), but also reflect part of the ultrasonic wave; in this case, when the ultrasonic sensor of the drone transmits a signal and receives a return signal, the system can judge whether it is close to the boundary by analyzing the energy change of the return signal; if the energy of the returned ultrasonic signal is detected to drop significantly, it means that the drone may have approached the boundary;

[0089] Furthermore, the system will compare the detected current distance with the preset boundary distance threshold. If the detected distance is within this threshold range, the drone will automatically enter the boundary avoidance mode to avoid collision or crossing the predetermined boundary;

[0090] The boundary detection mechanism and obstacle detection together constitute an important part of the UAV's automatic obstacle avoidance and path planning, ensuring that the UAV can perform flight missions safely and effectively in the port environment;

[0091] In summary, in S3, due to the good directionality of ultrasound, the ultrasonic sensor carried by the drone can be used to transmit ultrasound. When the ultrasound touches an obstacle, it will be reflected. According to this principle, the time difference of its reflection from the obstacle is measured to calculate the distance. Therefore, specifically, the distance of the drone relative to the obstacle and the boundary is detected according to the ultrasonic sensor module carried by the drone, and the timing starts from the ultrasonic sensor transmitting the ultrasonic signal until the transmitting signal is received. The distance of the drone relative to the obstacle and the boundary is calculated based on this, and the given threshold is compared to determine whether the drone needs to avoid obstacles or stay away from the boundary; wherein, the speed of sound and the maximum detection distance of the sensor need to be set in advance, and then the ultrasonic signal is propagated to obtain the signal reception time; the distance between the drone and the obstacle or boundary is calculated based on the time. If the signal is not received for a long time, it is determined that there are no obstacles and boundaries nearby.

[0092] S4: When obstacle avoidance is required, new heading angle and pitch angle are calculated based on the repulsive field to avoid collision with obstacles;

[0093] According to S32, it is determined that the UAV needs to perform obstacle avoidance operation. The present invention adopts an obstacle avoidance strategy based on a repulsive field to avoid obstacles near the UAV. The distance d between the UAV and the surrounding obstacles is calculated. o , it is determined that repulsion needs to be generated at this moment to make the drone avoid obstacles; the magnitude and direction of the repulsion are inversely proportional to the distance of the obstacle, that is, the closer the distance, the greater the repulsion, and the direction points away from the obstacle;

[0094] Before performing obstacle avoidance operations, obtain the current drone parameters, namely the heading angle and pitch angle of the drone and its location, namely the three-dimensional vector, to provide a data basis;

[0095] The obstacle avoidance operation includes:

[0096] The UAV system calculates the distance between the current UAV and the obstacle based on the ultrasonic propagation speed and ultrasonic duration. The expression is:

[0097]

[0098] Among them, d o Indicates the distance between the current drone and the obstacle; v s Indicates the propagation speed of ultrasonic wave; t m Indicates the duration of ultrasound; sets the ultrasonic propagation speed to 343m / s;

[0099] According to the current UAV 3D position vector and the current obstacle 3D coordinate vector, the vector from the current UAV to the obstacle is calculated. The expression is:

[0100]

[0101] Among them, V j Represents the vector from the current drone to the obstacle; O j =[O jx O jy O jz ] T Represents the three-dimensional coordinate vector of the current obstacle; [xyz] T Represents the current three-dimensional position vector of the drone; for each obstacle, an ultrasonic sensor is used to detect the vector and distance between the drone and the obstacle, and the obstacle coordinate is recorded as O j =[O jx O jy O jz ] T ;

[0102] According to the vector from the current drone to the obstacle, the distance between the current drone and the obstacle, and the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation, the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation is obtained, and its expression is:

[0103]

[0104] Among them, F new Indicates the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation; F re Indicates the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation and the cumulative repulsive force vector of the obstacle in the first obstacle avoidance operation F re =[0 0 0] T ; That is, when an obstacle avoidance operation is required, the repulsion force can be calculated according to the inverse relationship and accumulated into the repulsion force vector to obtain the accumulated repulsion force vector of the obstacle in the current obstacle avoidance operation;

[0105] Based on the current UAV heading angle and pitch angle, the current UAV direction vector is obtained, and its expression is:

[0106]

[0107] Among them, D now represents the direction vector of the current drone; γ represents the heading angle of the current drone; ψ represents the pitch angle of the current drone;

[0108] Based on the current drone direction vector and the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation, the new direction vector of the current drone is obtained, and its expression is:

[0109] D new =D now +F new

[0110] Among them, D new Indicates the new direction vector of the current drone; F new Indicates the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation;

[0111] In order to correctly process the angles of different quadrants and obtain more accurate heading and pitch angles of the drone, the new heading and pitch angles are calculated using the four-quadrant inverse tangent function atan2 based on the calculated direction vector. Specifically, based on the new direction vector of the current drone, the four-quadrant inverse tangent function is used to calculate the new heading and pitch angles of the current drone. The expressions are:

[0112]

[0113] Among them, γ new Indicates the new heading angle of the current UAV; ψ new Indicates the new pitch angle of the current drone; D new,x Indicates the x-coordinate of the new direction vector of the current drone; D new,y Indicates the y coordinate of the new direction vector of the current drone; Dnew,z Indicates the z coordinate of the current drone's new direction vector;

[0114] Based on the new heading angle and pitch angle of the current UAV, the Gauss-Markov model is used to adjust the random motion trajectory and flight attitude of the UAV;

[0115] In summary, when the ultrasonic sensor module determines that the drone needs to avoid obstacles, S4 is executed to implement the drone's obstacle avoidance strategy based on the repulsive field. According to the current distance between the drone and the obstacle, the repulsive force generated by the obstacle is calculated, and the total repulsive force vector is added to the current flight direction vector of the drone, that is, the repulsive force generated by the obstacle is simulated to change the direction of the drone. Combined with the current vector of the drone and the influence of the repulsive force, a new drone motion vector is obtained, thereby changing the flight trajectory of the drone, thereby achieving drone obstacle avoidance;

[0116] The obstacle avoidance operation based on repulsive field design has unique advantages. The repulsive field method does not rely on the specific model of the obstacle. It only needs to know the position and distance of the obstacle to calculate the repulsion, which is very flexible when dealing with complex and changing environments. The repulsive field method mainly relies on local environmental information for obstacle avoidance, and does not require global modeling of the entire environment, so the computational complexity is low. In addition, the repulsive field method adjusts the movement direction of the UAV through gradual repulsion, avoiding drastic changes in movement and making the flight of the UAV more stable.

[0117] S5: When the drone automatically enters the boundary avoidance mode, the avoidance counter starts counting until the value of the avoidance counter exceeds the duration, and the drone exits the boundary avoidance mode to ensure that the drone stays away from the boundary;

[0118] The boundary avoidance mode includes:

[0119] The UAV system obtains the new heading angle of the UAV according to the two-dimensional position coordinates of the environment center and the two-dimensional position coordinates of the current UAV. The expression is:

[0120]

[0121] Among them, γ i represents the new heading angle of the UAV at the i-th time step; L and W represent the length and width of the environment scene respectively; (L / 2, W / 2) represents the two-dimensional position coordinates of the center of the environment; (x i-1 ,y i-1 ) represents the two-dimensional position coordinates of the UAV at the (i-1)th time step;

[0122] At the same time, after the drone enters the boundary avoidance mode, the avoidance counter starts counting. If the value of the avoidance counter exceeds the preset boundary avoidance duration, the drone exits the boundary avoidance mode, the drone system resets the avoidance counter, and adjusts the drone's random motion trajectory based on the current drone's new heading angle using the three-dimensional Gauss-Markov model;

[0123] The duration T of boundary avoidance is set according to the environment. BA =20, when the drone enters the boundary avoidance mode, set the avoidance counter to C BA , initialized to 0, and start counting; if the drone maintains the boundary obstacle avoidance mode, the value of the avoidance counter increases by 1, that is, C BA =C BA +1; if the value of the avoidance counter exceeds the preset boundary avoidance duration, that is, C BA >T BA , then exit the boundary avoidance mode and reset the avoidance counter, waiting for the next boundary avoidance instruction;

[0124] In summary, when the ultrasonic sensor determines that the drone needs to stay away from the boundary, S5 is executed to put the drone into boundary avoidance mode. The heading angle is adjusted based on the vector pointing to the center of the environment (the two-dimensional position coordinates of the center of the environment) and the current vector of the drone (the current two-dimensional position coordinates of the drone), and the flight direction of the drone is changed. At the same time, the avoidance counter starts counting until the value of the avoidance counter exceeds the duration, ensuring that the drone stays away from the boundary.

[0125] S6: Continue to determine whether the drone is close to an environmental obstacle / boundary, and whether to perform obstacle avoidance operations / enter boundary obstacle avoidance mode, until the preset flight time is reached, obtain the final flight trajectory of the drone and the flight attitude corresponding to each position of the drone in the flight trajectory, and complete the trajectory planning of the entire flight process of the drone in the current environmental scenario;

[0126] Among them, according to the update of the data such as the position and angle of the drone during the flight of the drone, the final flight trajectory of the drone and the flight attitude corresponding to each position of the drone in the flight trajectory can be obtained;

[0127] The update formula of the drone's three-dimensional position is:

[0128] x i =x i-1 +υ i ·t step ·cos(ξ i )·cos(γ i )

[0129] y i =y i-1 +υ i·t step ·cos(ξ i )·sin(γ i )

[0130] z i =z i-1 +υ i ·t step ·sin(ξ i )

[0131] Among them, [x i ,y i ,z i ] T represents the three-dimensional position vector of the UAV at the i-th time step; [x i-1 ,t i-1 ,z i-1 ] T represents the 3D position vector of the UAV at the (i-1)th time step; t step represents the time step; ξ i represents the UAV rotation angle at the i-th time step; γ i represents the heading angle of the UAV at the i-th time step;

[0132] Based on the flight trajectory and flight attitude of the UAV obtained in the above steps, the trajectory planning of the UAV in the entire flight process in the port scenario is finally completed.

[0133] Based on the above S1-S6 process, we get Figure 2 The flight trajectory of the drone and Figure 3 The graph of the pitch angle and roll angle of the drone changing with time shows that the present invention can generate a flight trajectory that is more in line with reality. The pitch angle and roll angle both fluctuate around 0, indicating that the drone will maintain its original heading while considering the influence of factors such as wind resistance, thereby simulating a drone posture that is more in line with reality.

[0134] Obviously, the above embodiments are merely examples for the purpose of clear explanation and are not intended to limit the implementation methods. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the implementation methods here. The obvious changes or modifications derived therefrom are still within the scope of protection of the present invention.

Claims

1. A trajectory planning method for unmanned aerial vehicle obstacle avoidance, characterized in that: include: Based on the three-dimensional Gauss-Markov model, the speed, spin angle, and heading angle of the UAV at each time step are obtained to generate the random motion trajectory of the UAV; According to the random motion trajectory of the UAV, the two-dimensional Gauss-Markov model is used to obtain the pitch angle and roll angle of the UAV at each time step, and then determine the flight attitude of the UAV; When the drone flies according to the drone's random motion trajectory and flight attitude, the drone uses an ultrasonic sensor to emit ultrasonic signals and receives return signals generated by reflection when the ultrasonic waves touch environmental obstacles / environmental boundaries; the drone system records the ultrasonic duration and detects the return signal; wherein the ultrasonic duration is the duration from the start of the ultrasonic wave emission to the reflection back to the sensor after it touches an obstacle; If the ultrasonic duration is less than the preset time threshold, there is an obstacle in the preset area of ​​the drone, and the drone performs obstacle avoidance operations; the drone system calculates the distance between the current drone and the obstacle based on the ultrasonic propagation speed and the ultrasonic duration; after calculating the vector from the current drone to the obstacle based on the current drone's three-dimensional position vector and the current obstacle's three-dimensional coordinate vector, the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation is obtained by combining the distance between the current drone and the obstacle and the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation; after obtaining the current drone's direction vector based on the current drone's heading angle and pitch angle, the new direction vector of the current drone is obtained by combining the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation, and then the four-quadrant inverse tangent function is used to calculate the new heading angle and pitch angle of the current drone, and the Gauss-Markov model is used to adjust the drone's random motion trajectory and flight attitude; If the UAV system detects that the energy of the return signal decreases, and the distance between the UAV and the environmental boundary is less than or equal to the preset boundary distance threshold, there is an environmental boundary in front of the UAV, and the UAV automatically enters the boundary avoidance mode. The UAV system obtains the new heading angle of the current UAV based on the two-dimensional position coordinates of the center of the environment and the two-dimensional position coordinates of the current UAV; at the same time, after the UAV enters the boundary avoidance mode, the avoidance counter starts counting. If the value of the avoidance counter exceeds the preset boundary avoidance duration, the UAV exits the boundary avoidance mode, and the UAV system resets the avoidance counter, and based on the new heading angle of the current UAV, uses the three-dimensional Gauss-Markov model to adjust the random motion trajectory of the UAV; Continue to determine whether the drone is close to environmental obstacles / boundaries, whether to perform obstacle avoidance operations / enter boundary obstacle avoidance mode, until the preset flight time is reached, obtain the final flight trajectory and flight attitude of the drone, and complete the trajectory planning of the entire flight process of the drone in the current environmental scenario.

2. The method for trajectory planning of unmanned aerial vehicle obstacle avoidance according to claim 1, characterized in that: The three-dimensional Gauss-Markov model includes: The UAV speed at the previous time step, the expected mean speed, and the random disturbance term corresponding to the UAV speed are used to obtain the UAV speed at the current time step. The expression is: The spin angle of the drone at the previous time step, the expected mean spin angle, and the random disturbance term corresponding to the spin angle of the drone are used to obtain the spin angle of the drone at the current time step. The expression is: The heading angle of the UAV in the previous time step, the expected mean of the heading angle, and the random disturbance term corresponding to the heading angle of the UAV are used to obtain the heading angle of the UAV in the current time step. The expression is: Among them, v T (t i ) represents the UAV speed at the i-th time step; ξ T (t i ) represents the spin angle of the i-th time step; γ T (t i ) represents the heading angle of the UAV at the i-th time step; t i represents the moment corresponding to the i-th time step; v T (t0) represents the speed of the UAV at the initial moment; ξ T (t0) represents the UAV rotation angle at the initial moment; γ T (t0) represents the heading angle of the drone at the initial moment; t0 represents the initial moment; i represents the sequence number of the time step, i∈[0,N], N represents the total sequence number of the time step; represents the expected mean of the drone’s speed; represents the expected mean of the UAV’s spin angle; represents the expected mean of the heading angle of the UAV; represents the autoregressive coefficient corresponding to the UAV speed at the i-th time step; represents the autoregressive coefficient corresponding to the UAV spin angle at the i-th time step; represents the autoregressive coefficient corresponding to the heading angle of the UAV at the i-th time step; L j represents the random disturbance term corresponding to the UAV velocity at the jth time step; M j N represents the random disturbance term corresponding to the unmanned spin angle at the jth time step; j Represents the random disturbance term corresponding to the heading angle of the UAV at the j-th time step; j represents the sequence number of the time step, j∈[0,i-1].

3. The trajectory planning method for obstacle avoidance of a UAV according to claim 1, characterized in that: The two-dimensional Gauss-Markov model includes: Among them, ψ T (t i ) represents the pitch angle of the UAV at the i-th time step; θ T (t i ) represents the rolling angle of the UAV at the i-th time step; t i represents the time corresponding to the i-th time step; i represents the serial number of the time step, i∈[0,N], N represents the total serial number of the time step; t0 represents the initial time; ψ T (t0) represents the pitch angle of the drone at the initial moment; θ T (t0) represents the rolling angle of the UAV at the initial moment; represents the expected mean value of the pitch angle of the drone; represents the expected mean of the UAV’s rolling angle; Represents the autoregressive coefficient corresponding to the pitch angle of the UAV at the i-th time step; represents the autoregressive coefficient corresponding to the UAV roll angle at the i-th time step; X j Y represents the random disturbance term corresponding to the pitch angle of the UAV at the jth time step; j Represents the random disturbance term corresponding to the UAV roll angle at the jth time step; j represents the sequence number of the time step, j∈[0,i-1].

4. The method for trajectory planning of unmanned aerial vehicle obstacle avoidance according to claim 1, characterized in that: The UAV system calculates the distance between the current UAV and the obstacle according to the ultrasonic propagation speed and ultrasonic duration, and the expression is: Among them, d o Indicates the distance between the current drone and the obstacle; v S Indicates the propagation speed of ultrasonic wave; t m Indicates the duration of ultrasound.

5. The method for trajectory planning of unmanned aerial vehicle obstacle avoidance according to claim 1, characterized in that: After calculating the vector from the current drone to the obstacle based on the current drone 3D position vector and the current obstacle 3D coordinate vector, the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation is obtained by combining the distance between the current drone and the obstacle and the accumulated repulsive force vector of the obstacle in the previous obstacle avoidance operation. The accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation includes: According to the current UAV 3D position vector and the current obstacle 3D coordinate vector, the vector from the current UAV to the obstacle is calculated. The expression is: Among them, V j Represents the vector from the current drone to the obstacle; O j =[O jx O jy O jz ] T Represents the three-dimensional coordinate vector of the current obstacle; [xyz] T Represents the current three-dimensional position vector of the drone; According to the vector from the current drone to the obstacle, the distance between the current drone and the obstacle, and the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation, the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation is obtained, and its expression is: Among them, F new Indicates the cumulative repulsive force vector of the obstacle in the current obstacle avoidance operation; F re Indicates the cumulative repulsive force vector of the obstacle in the previous obstacle avoidance operation and the cumulative repulsive force vector of the obstacle in the first obstacle avoidance operation F re =[0 0 0] T .

6. The method for trajectory planning of a drone obstacle avoidance according to claim 1, characterized in that: After obtaining the current drone direction vector based on the current drone heading angle and pitch angle, the new direction vector of the current drone is obtained by combining the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation: Based on the current UAV heading angle and pitch angle, the current UAV direction vector is obtained, and its expression is: Among them, D now represents the direction vector of the current drone; γ represents the heading angle of the current drone; ψ represents the pitch angle of the current drone; Based on the current drone direction vector and the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation, the new direction vector of the current drone is obtained, and its expression is: D new =D now +F new Among them, D new Indicates the new direction vector of the current drone; F new Indicates the accumulated repulsive force vector of the obstacle in the current obstacle avoidance operation.

7. The method for trajectory planning of unmanned aerial vehicle obstacle avoidance according to claim 1, characterized in that: Based on the new direction vector of the current drone, the four-quadrant inverse tangent function is used to calculate the new heading angle and pitch angle of the current drone. The expressions are: Among them, γ new Indicates the new heading angle of the current UAV; ψ new Indicates the new pitch angle of the current drone; D new,x Indicates the x-coordinate of the new direction vector of the current drone; D new,y Indicates the y coordinate of the new direction vector of the current drone; D new,z Indicates the z coordinate of the current drone's new direction vector.

8. The method for trajectory planning of unmanned aerial vehicle obstacle avoidance according to claim 1, characterized in that: The UAV system obtains the new heading angle of the UAV according to the two-dimensional position coordinates of the center of the environment and the two-dimensional position coordinates of the current UAV, and its expression is: Among them, γ i represents the new heading angle of the UAV at the i-th time step; L and W represent the length and width of the environment scene respectively; (x i-1 ,y i-1 ) represents the two-dimensional position coordinates of the UAV at the (i-1)th time step.

9. The method for trajectory planning of unmanned aerial vehicle obstacle avoidance according to claim 1, characterized in that: If the ultrasonic duration is greater than the preset time threshold, there are no environmental obstacles near the drone, and the drone continues to fly according to the current drone random motion trajectory and flight attitude.

10. The method for trajectory planning of unmanned aerial vehicle obstacle avoidance according to claim 1, characterized in that: If the UAV system detects that the energy of the return signal decreases, and the UAV system detects that the distance between the UAV and the environmental boundary is greater than the preset boundary distance threshold, there is an environmental boundary in front of the UAV, and the UAV continues to fly according to the current random motion trajectory and flight attitude of the UAV; if the UAV system detects that the energy of the return signal does not decrease, there is no environmental boundary in front of the UAV, and the UAV continues to fly according to the current random motion trajectory and flight attitude of the UAV.

Images