CN117631689A - Unmanned aerial vehicle obstacle avoidance flight method - Google Patents
Unmanned aerial vehicle obstacle avoidance flight method Download PDFInfo
- Publication number
- CN117631689A CN117631689A CN202410100873.3A CN202410100873A CN117631689A CN 117631689 A CN117631689 A CN 117631689A CN 202410100873 A CN202410100873 A CN 202410100873A CN 117631689 A CN117631689 A CN 117631689A
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- acceleration
- ultrasonic
- vector
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 25
- 239000013598 vector Substances 0.000 claims abstract description 103
- 230000001133 acceleration Effects 0.000 claims abstract description 82
- 239000002245 particle Substances 0.000 claims abstract description 44
- 230000009191 jumping Effects 0.000 claims description 12
- 238000013459 approach Methods 0.000 claims description 3
- 230000005540 biological transmission Effects 0.000 claims description 3
- 230000009471 action Effects 0.000 abstract description 5
- 238000004422 calculation algorithm Methods 0.000 abstract description 3
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 abstract description 2
- 230000009286 beneficial effect Effects 0.000 description 8
- 238000001514 detection method Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 235000008694 Humulus lupulus Nutrition 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 125000004122 cyclic group Chemical group 0.000 description 2
- 238000005265 energy consumption Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000010801 machine learning Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000016776 visual perception Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an unmanned aerial vehicle obstacle avoidance flight method, which relates to the field of flight control and comprises the following steps: detecting the neighborhood environment of the unmanned aerial vehicle in real time, and detecting and calculating to obtain the speed vector of the unmanned aerial vehicle; detecting multiple obstacles and calculating respective approximation speed vectors of the multiple obstacles; calculating an anti-collision acceleration vector through a particle swarm algorithm; the unmanned aerial vehicle is driven to adjust the pose according to the anti-collision acceleration vector so as to avoid multiple obstacles. The invention can detect obstacles in different directions simultaneously, sense the motion states of the multiple obstacles, then iteratively calculate and optimize the anti-collision acceleration solution according to the motion states of the multiple obstacles, and then make a preferential evasion action. The active collision of multidirectional obstacles such as birds can be effectively avoided, and the real complex environment is dealt with.
Description
Technical Field
The invention relates to the field of flight control, in particular to an obstacle avoidance flight method of an unmanned aerial vehicle.
Background
With the rapid development of unmanned aerial vehicle technology, the unmanned aerial vehicle is widely applied to the fields of military, agriculture, logistics and the like. However, due to the small size and high flying speed of the unmanned plane, the traditional manual control mode cannot meet the flying requirement in the complex environment. Therefore, how to realize autonomous obstacle avoidance flight of the unmanned aerial vehicle becomes important content of current research.
At present, an unmanned aerial vehicle obstacle avoidance flight technology generally depends on a three-dimensional elevation map, a visual perception system, an infrared sensing system or an ultrasonic perception system, focuses on obstacle avoidance flight route planning research on a middle-distance navigation layer, cannot avoid active collision of moving objects such as birds and the like, and cannot avoid simultaneous collision of different obstacles in multiple directions. Therefore, how to cope with the real complex environment is still a disadvantage of the prior art.
Disclosure of Invention
Aiming at the defects in the prior art, the unmanned aerial vehicle obstacle avoidance flight method provided by the invention solves the problems that the existing unmanned aerial vehicle obstacle avoidance flight technology cannot avoid active collision of moving objects such as birds, cannot avoid simultaneous collision of different obstacles in multiple directions and cannot cope with real complex environments.
In order to achieve the aim of the invention, the invention adopts the following technical scheme:
an unmanned aerial vehicle obstacle avoidance flying method comprises the following steps:
s1, detecting the neighborhood environment of the unmanned aerial vehicle in real time through six-way ultrasonic equipment, and detecting and calculating the speed vector of the unmanned aerial vehicle through an inertial sensor;
s2, judging whether the six-way ultrasonic equipment receives echo signals to detect multiple obstacles, and calculating respective approximation speed vectors of the multiple obstacles;
s3, calculating an anti-collision acceleration vector through a particle swarm model according to the speed vector of the unmanned aerial vehicle and the respective approaching speed vectors of the multiple obstacles;
s4, driving the unmanned aerial vehicle to adjust the pose according to the anti-collision acceleration vector so as to avoid multiple obstacles.
The beneficial effects of the invention are as follows: the invention can detect obstacles in different directions at the same time, sense the motion states of the obstacles, then iteratively calculate and optimize anti-collision acceleration solutions according to the motion states of the obstacles, and then make a preferential evasion action. The active collision of multidirectional obstacles such as birds can be effectively avoided, and the real complex environment is dealt with.
Further, the six-direction ultrasonic equipment is six groups of ultrasonic transceiver elements which are arranged on the body of the unmanned aerial vehicle and respectively face to six directions of front, back, upper, lower, left and right; each group of ultrasonic transceiver elements comprises a transmitting end and a receiving end.
The beneficial effects of the above-mentioned further scheme are: the invention can monitor different obstacles simultaneously in all directions.
Further, the transmitting ends of each group of ultrasonic transceiver elements transmit ultrasonic waves with the power shown in the following formula:
；
；
wherein T is the emission period, s (n ∙ T) is the power of ultrasonic waves in the nth emission period, sin (∙) is a sine function, pi is the circumferential rate, f (M) is the mth preset ultrasonic frequency, and% is the remainder operation, and M is the number of preset ultrasonic frequency values.
The beneficial effects of the above-mentioned further scheme are: compared with visual image detection, the ultrasonic detection has the advantages of small operation amount, hardware resource consumption saving and power consumption saving, but is not easy to deal with complex environments, especially artificial ultrasonic interference, in order to deal with complex environments, and also in order to prevent malicious interference and damage of human factors, a cyclic frequency hopping detection mode of a plurality of frequency ultrasonic waves is set, a transmitting end hops to send out fixed frequency points, and a receiving end carries out corresponding identification action so as to prevent acoustic waves in the natural world or acoustic waves with artificial malicious interference from being used as echo signals.
Further, the inertial sensor is a triaxial acceleration sensor.
Further, the method for detecting and calculating the speed vector of the unmanned aerial vehicle by the S1 through the inertial sensor comprises the following steps:
a1, detecting the acceleration of the unmanned aerial vehicle in the x-axis direction, the acceleration of the y-axis direction and the acceleration of the z-axis direction in a three-dimensional space Cartesian coordinate system through a three-axis acceleration sensor;
a2, calculating to obtain a speed vector of the unmanned aerial vehicle according to the following formula:
；
wherein v is U (k) Is the velocity vector of the unmanned aerial vehicle at the moment k, v U,x (k) For the x-axis direction speed of the unmanned aerial vehicle at the moment k, v U,y (k) For the y-axis direction speed of the unmanned aerial vehicle at the k moment, v U,z (k) For the z-axis direction speed of the unmanned aerial vehicle at the moment k, a U,x (i) For the acceleration of the unmanned plane in the x-axis direction at the moment i, a U,y (i) For the acceleration of the unmanned plane in the y-axis direction at the moment i, a U,z (i) The acceleration of the unmanned plane in the z-axis direction at the moment i is obtained.
The beneficial effects of the above-mentioned further scheme are: and the motion state of the unmanned aerial vehicle in the three-dimensional space is efficiently detected and calculated in real time through the three-axis acceleration sensor and discrete accumulation operation.
Further, the step S2 includes the following sub-steps:
s21, detecting whether the receiving end of each ultrasonic receiving and transmitting element receives ultrasonic waves in each transmitting period of the transmitting end of each ultrasonic receiving and transmitting element, if so, jumping to the step S22, and if not, continuing to detect the next period;
s22, judging whether the ultrasonic wave is an echo signal, if so, detecting an obstacle in the direction of the ultrasonic receiving and transmitting element in the corresponding direction, and jumping to the step S23, and if not, jumping to the step S21;
s23, calculating a first distance of the obstacle of the S22 by the following formula:
；
wherein d 1 A first distance v of the obstacle s Is sound velocity, t 1 The time interval between the receiving end of the ultrasonic receiving and transmitting element receiving the ultrasonic wave and the transmitting end transmitting the ultrasonic wave for detecting the obstacle;
s24, acquiring a second distance of the obstacle in a next transmission period by using the ultrasonic transceiver element which detects the obstacle in the S22;
s25, calculating the approximation speed of the obstacle of the S22 according to the following formula:
；
wherein v is ob Is the approach speed of the obstacle, d 2 A second distance being an obstacle;
s26, according to the direction of the ultrasonic transceiver element detecting the obstacle of the S22, direction information is given to the approximation speed of the S25, and an approximation speed vector of the obstacle of the S22 is obtained;
and S27, summarizing obstacles and respective approaching speed vectors of ultrasonic receiving and transmitting elements with other orientations in the same period.
The beneficial effects of the above-mentioned further scheme are: a speed vector calculation method of the multidirectional multi-obstacle based on ultrasonic ranging is constructed, and the relative speed of the obstacle towards the unmanned aerial vehicle is accurately calculated.
Further, in S22, if the nth of the transmitting ends of the ultrasonic transceiver elements 0 And (3) periodically, judging the ultrasonic signal received by the receiving end as an echo signal if the ultrasonic signal meets the following formula:
；
；
wherein f recv For the frequency of the received ultrasonic signal, f (m 0 ) Is the mth 0 A preset ultrasonic frequency f δ Is the frequency offset threshold.
The beneficial effects of the above-mentioned further scheme are: because the moving object has Doppler frequency shift effect, the method sets a frequency offset threshold value, moderately relaxes the echo signal discrimination condition of the receiving end on the frequency hopping ultrasonic wave of the transmitting end, does not leak echo signals, and effectively avoids environment and human interference.
Further, the step S3 includes the following sub-steps:
s31, constructing a particle swarm model, initializing the number of particles contained in the particle swarm to be N, setting the speed vector of each particle to be the speed vector of the unmanned aerial vehicle, and randomly setting the acceleration vector of each particle, wherein N is a positive integer;
s32, deducing the collision condition of each particle with multiple obstacles collided with respective approaching speed vectors after adjusting the speed vectors of the particles with respective accelerating vectors;
s34, selecting N with the longest avoidance time and successful avoidance time 1 Particles, N 1 The acceleration vector of particles which are smaller than the positive integer of N and are successfully avoided is recorded;
s35, judging whether the number of acceleration vectors of all recorded obstacle avoidance success is larger than N 2 If yes, go to step S37, if no, go to step S36, N 2 Is a positive integer;
s36, selecting the N selected in the S35 2 Randomly copying the acceleration vectors of the individual particles to each particle, randomly fine-adjusting the acceleration vectors of each particle, and jumping to the step S32;
s37, selecting a vector with the minimum two norms in the acceleration vector with successful obstacle avoidance in the record as an anti-collision acceleration vector.
The beneficial effects of the above-mentioned further scheme are: through machine learning, an improved particle swarm model is used for simulating the avoidance effect of the unmanned aerial vehicle on multiple obstacles by using different acceleration vectors, multiple groups of optimal acceleration vector solutions are reserved in each round of simulation and copied to each particle, fine tuning is carried out on the next round of iteration, and after the acceleration vector solutions of multiple avoidable obstacles are obtained, the solution which is minimum in energy consumption and is the solution which is easy to drive and realize is found out.
Further, the method of S4 is as follows: taking the anti-collision acceleration vector as an acceleration target of the unmanned aerial vehicle, and iterating the power driving vector of the unmanned aerial vehicle through the following steps:
；
；
wherein u (k) is a power driving vector of the unmanned aerial vehicle at time k, P out For the outer PI loop proportionality coefficient, I out For the integral coefficient of the outer PI loop, f (k) is the output quantity of the inner PI loop at the moment k, f (k-1) is the output quantity of the inner PI loop at the moment k-1, u (k-1) is the power driving vector of the unmanned aerial vehicle at the moment k-1, and P in Is the proportional coefficient of the inner PI loop, I in The integral coefficient of the inner PI loop is ϵ (k) is an error vector of an acceleration target of the unmanned aerial vehicle and the acceleration of the unmanned aerial vehicle at the moment k, and ϵ (k-1) is an error vector of the acceleration target of the unmanned aerial vehicle and the acceleration of the unmanned aerial vehicle at the moment k-1.
The beneficial effects of the above-mentioned further scheme are: the differential D coefficient of the traditional PID algorithm is used for reducing overshoot, overcoming oscillation, accelerating the response speed of the system and reducing the adjustment time, thereby improving the dynamic performance of the system, but the coefficient is not easy to configure, so that the system is unstable and high-frequency noise is easy to introduce. Especially, for the urgent obstacle avoidance of unmanned aerial vehicle, often make unmanned aerial vehicle disorder self-destruction. The invention designs PI double-ring discrete adjustment self-adaptive control, which can realize quick dynamic reaction and is safe and stable.
Drawings
Fig. 1 is a flowchart of an obstacle avoidance flight method of an unmanned aerial vehicle according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention is provided to facilitate understanding of the present invention by those skilled in the art, but it should be understood that the present invention is not limited to the scope of the embodiments, and all the inventions which make use of the inventive concept are protected by the spirit and scope of the present invention as defined and defined in the appended claims to those skilled in the art.
As shown in fig. 1, in one embodiment of the present invention, a method for unmanned aerial vehicle obstacle avoidance flight includes:
s1, detecting the unmanned aerial vehicle neighborhood environment in real time through six-way ultrasonic equipment, and detecting and calculating the speed vector of the unmanned aerial vehicle through an inertial sensor.
The six-way ultrasonic equipment of the embodiment is six groups of ultrasonic transceiver elements which are arranged on the body of the unmanned aerial vehicle and respectively face to six directions of front, back, upper, lower, left and right; each group of ultrasonic transceiver elements comprises a transmitting end and a receiving end. Different obstacles can be monitored simultaneously in all directions.
The transmitting ends of each group of ultrasonic receiving and transmitting elements transmit ultrasonic waves with the power shown in the following formula:
；
；
wherein T is the emission period, s (n ∙ T) is the power of ultrasonic waves in the nth emission period, sin (∙) is a sine function, pi is the circumferential rate, f (M) is the mth preset ultrasonic frequency, and% is the remainder operation, and M is the number of preset ultrasonic frequency values.
Compared with visual image detection, the ultrasonic detection has the advantages of small operation amount, hardware resource consumption saving and power consumption saving, but is not easy to deal with complex environments, especially artificial ultrasonic interference, in order to deal with complex environments, and also in order to prevent malicious interference and damage of human factors, a cyclic frequency hopping detection mode of a plurality of frequency ultrasonic waves is set, a transmitting end hops to send out fixed frequency points, and a receiving end carries out corresponding identification action so as to prevent acoustic waves in the natural world or acoustic waves with artificial malicious interference from being used as echo signals.
The method for detecting and calculating the speed vector of the unmanned aerial vehicle through the inertial sensor in the step S1 comprises the following steps:
a1, detecting the acceleration of the unmanned aerial vehicle in the x-axis direction, the acceleration of the y-axis direction and the acceleration of the z-axis direction in a three-dimensional space Cartesian coordinate system through a three-axis acceleration sensor;
a2, calculating to obtain a speed vector of the unmanned aerial vehicle according to the following formula:
；
wherein v is U (k) Is the velocity vector of the unmanned aerial vehicle at the moment k, v U,x (k) For the x-axis direction speed of the unmanned aerial vehicle at the moment k, v U,y (k) For the y-axis direction speed of the unmanned aerial vehicle at the k moment, v U,z (k) For the z-axis direction speed of the unmanned aerial vehicle at the moment k, a U,x (i) For the acceleration of the unmanned plane in the x-axis direction at the moment i, a U,y (i) For the acceleration of the unmanned plane in the y-axis direction at the moment i, a U,z (i) The acceleration of the unmanned plane in the z-axis direction at the moment i is obtained.
According to the embodiment, the motion state of the unmanned aerial vehicle in the three-dimensional space is efficiently detected and calculated in real time through the three-axis acceleration sensor and discrete accumulation operation.
S2, judging whether the six-way ultrasonic equipment receives echo signals or not to detect multiple obstacles, and calculating respective approximation speed vectors of the multiple obstacles.
Step S2 comprises the following sub-steps:
s21, detecting whether the receiving end of each ultrasonic receiving and transmitting element receives ultrasonic waves in each transmitting period of the transmitting end of each ultrasonic receiving and transmitting element, if so, jumping to the step S22, and if not, continuing to detect the next period;
s22, judging whether the ultrasonic wave is an echo signal, if so, detecting an obstacle in the direction of the ultrasonic receiving and transmitting element in the corresponding direction, and jumping to the step S23, and if not, jumping to the step S21;
s23, calculating a first distance of the obstacle of the S22 by the following formula:
；
wherein d 1 A first distance v of the obstacle s Is sound velocity, t 1 The time interval between the receiving end of the ultrasonic receiving and transmitting element receiving the ultrasonic wave and the transmitting end transmitting the ultrasonic wave for detecting the obstacle;
s24, acquiring a second distance of the obstacle in a next transmission period by using the ultrasonic transceiver element which detects the obstacle in the S22;
s25, calculating the approximation speed of the obstacle of the S22 according to the following formula:
；
wherein v is ob Is the approach speed of the obstacle, d 2 A second distance being an obstacle;
s26, according to the detected orientation of the ultrasonic transceiver element of the obstacle of S22, direction information is given to the approximation speed of S25, so as to obtain an approximation speed vector of the obstacle of S22, for example, the direction of one obstacle relative to the unmanned aerial vehicle motion is the negative direction of the x-axis in a three-dimensional space Cartesian coordinate system, and the speed is 1m/S, and then the approximation speed vector is:
m/s；
and S27, summarizing obstacles and respective approaching speed vectors of ultrasonic receiving and transmitting elements with other orientations in the same period.
The embodiment constructs a multidirectional multi-obstacle speed vector calculation method based on ultrasonic ranging, and accurately calculates the relative speed of the obstacle towards the unmanned aerial vehicle.
In step S22, if the nth of the transmitting ends of the ultrasonic transceiver elements 0 And (3) periodically, judging the ultrasonic signal received by the receiving end as an echo signal if the ultrasonic signal meets the following formula:
；
；
wherein f recv For the frequency of the received ultrasonic signal, f (m 0 ) Is the mth 0 A preset ultrasonic frequency f δ Is the frequency offset threshold.
Because the moving object has Doppler frequency shift effect, the method sets a frequency offset threshold value, moderately relaxes the echo signal discrimination condition of the receiving end on the frequency hopping ultrasonic wave of the transmitting end, does not leak echo signals, and effectively avoids environment and human interference.
S3, calculating an anti-collision acceleration vector through a particle swarm model according to the speed vector of the unmanned aerial vehicle and the respective approaching speed vector of the multiple obstacles, wherein the anti-collision acceleration vector comprises the following steps of:
s31, constructing a particle swarm model, initializing the number of particles contained in the particle swarm to be N, setting the speed vector of each particle to be the speed vector of the unmanned aerial vehicle, and randomly setting the acceleration vector of each particle, wherein N is a positive integer;
s32, deducing the collision condition of each particle with multiple obstacles collided with respective approaching speed vectors after adjusting the speed vectors of the particles with respective accelerating vectors;
s34, selecting N with the longest avoidance time and successful avoidance time 1 Particles, N 1 The acceleration vector of particles which are smaller than the positive integer of N and are successfully avoided is recorded;
s35, judging whether the number of acceleration vectors of all recorded obstacle avoidance success is larger than N 2 If yes, go to step S37, if no, go to step S36, N 2 Is a positive integer;
s36, selecting the N selected in the S35 2 Randomly copying the acceleration vectors of the individual particles to each particle, randomly fine-adjusting the acceleration vectors of each particle, and jumping to the step S32;
s37, selecting a vector with the minimum two norms in the acceleration vector with successful obstacle avoidance in the record as an anti-collision acceleration vector.
And S3, simulating the avoidance effect of the unmanned aerial vehicle on multiple obstacles by using different acceleration vectors by using an improved particle swarm model through machine learning, copying multiple groups of better acceleration vector solutions to each particle by each round of simulation, fine tuning, iterating for the next round, and optimizing the solution which has the minimum energy consumption and is the solution which is easy to drive and realize after obtaining the acceleration vector solutions of the multiple avoidable obstacles.
S4, driving the unmanned aerial vehicle to adjust the pose according to the anti-collision acceleration vector so as to avoid multiple obstacles. In this embodiment, the anti-collision acceleration vector is used as the acceleration target of the unmanned aerial vehicle, and the power driving vector of the unmanned aerial vehicle is iterated through:
；
；
wherein u (k) is a power driving vector of the unmanned aerial vehicle at time k, P out For the outer PI loop proportionality coefficient, I out For the integral coefficient of the outer PI loop, f (k) is the output quantity of the inner PI loop at the moment k, f (k-1) is the output quantity of the inner PI loop at the moment k-1, u (k-1) is the power driving vector of the unmanned aerial vehicle at the moment k-1, and P in Is the proportional coefficient of the inner PI loop, I in The integral coefficient of the inner PI loop is ϵ (k) is an error vector of an acceleration target of the unmanned aerial vehicle and the acceleration of the unmanned aerial vehicle at the moment k, and ϵ (k-1) is an error vector of the acceleration target of the unmanned aerial vehicle and the acceleration of the unmanned aerial vehicle at the moment k-1.
The differential D coefficient of the traditional PID algorithm is used for reducing overshoot, overcoming oscillation, accelerating the response speed of the system and reducing the adjustment time, thereby improving the dynamic performance of the system, but the coefficient is not easy to configure, so that the system is unstable and high-frequency noise is easy to introduce. Especially, for the urgent obstacle avoidance of unmanned aerial vehicle, often make unmanned aerial vehicle disorder self-destruction. The invention designs PI double-ring discrete adjustment self-adaptive control, which can realize quick dynamic reaction and is safe and stable.
In summary, the invention can detect obstacles in different directions at the same time, sense the motion states of the obstacles, then iteratively find out the anti-collision acceleration solution according to the motion states of the obstacles, and then make a practical evading action. The active collision of multidirectional obstacles such as birds can be effectively avoided, and the real complex environment is dealt with.
The principles and embodiments of the present invention have been described in detail with reference to specific examples, which are provided to facilitate understanding of the method and core ideas of the present invention; meanwhile, as those skilled in the art will have variations in the specific embodiments and application scope in accordance with the ideas of the present invention, the present description should not be construed as limiting the present invention in view of the above.
Those of ordinary skill in the art will recognize that the embodiments described herein are for the purpose of aiding the reader in understanding the principles of the present invention and should be understood that the scope of the invention is not limited to such specific statements and embodiments. Those of ordinary skill in the art can make various other specific modifications and combinations from the teachings of the present disclosure without departing from the spirit thereof, and such modifications and combinations remain within the scope of the present disclosure.
Claims (9)
1. The unmanned aerial vehicle obstacle avoidance flying method is characterized by comprising the following steps of:
s1, detecting the neighborhood environment of the unmanned aerial vehicle in real time through six-way ultrasonic equipment, and detecting and calculating the speed vector of the unmanned aerial vehicle through an inertial sensor;
s2, judging whether the six-way ultrasonic equipment receives echo signals to detect multiple obstacles, and calculating respective approximation speed vectors of the multiple obstacles;
s3, calculating an anti-collision acceleration vector through a particle swarm model according to the speed vector of the unmanned aerial vehicle and the respective approaching speed vectors of the multiple obstacles;
s4, driving the unmanned aerial vehicle to adjust the pose according to the anti-collision acceleration vector so as to avoid multiple obstacles.
2. The unmanned aerial vehicle obstacle avoidance flight method of claim 1, wherein the six-way ultrasonic device is six groups of ultrasonic transceiver elements respectively oriented in six directions of front, rear, up, down, left and right mounted on a fuselage of the unmanned aerial vehicle; each group of ultrasonic transceiver elements comprises a transmitting end and a receiving end.
3. The unmanned aerial vehicle obstacle avoidance flight method of claim 2, wherein the transmitting ends of each group of ultrasonic transceiver elements transmit ultrasonic waves with power represented by the following formula:
；
；
wherein T is the emission period, s (n ∙ T) is the power of ultrasonic waves in the nth emission period, sin (∙) is a sine function, pi is the circumferential rate, f (M) is the mth preset ultrasonic frequency, and% is the remainder operation, and M is the number of preset ultrasonic frequency values.
4. The unmanned aerial vehicle obstacle avoidance flight method of claim 3, wherein the inertial sensor is a three-axis acceleration sensor.
5. The unmanned aerial vehicle obstacle avoidance flight method of claim 4, wherein the method of S1 detecting and calculating the speed vector of the unmanned aerial vehicle by the inertial sensor comprises the steps of:
a1, detecting the acceleration of the unmanned aerial vehicle in the x-axis direction, the acceleration of the y-axis direction and the acceleration of the z-axis direction in a three-dimensional space Cartesian coordinate system through a three-axis acceleration sensor;
a2, calculating to obtain a speed vector of the unmanned aerial vehicle according to the following formula:
；
wherein v is U (k) Is the velocity vector of the unmanned aerial vehicle at the moment k, v U,x (k) For the x-axis direction speed of the unmanned aerial vehicle at the moment k, v U,y (k) For the y-axis direction speed of the unmanned aerial vehicle at the k moment, v U,z (k) For the z-axis direction speed of the unmanned aerial vehicle at the moment k, a U,x (i) For the acceleration of the unmanned plane in the x-axis direction at the moment i, a U,y (i) For the acceleration of the unmanned plane in the y-axis direction at the moment i, a U,z (i) The acceleration of the unmanned plane in the z-axis direction at the moment i is obtained.
6. The unmanned aerial vehicle obstacle avoidance flight method of claim 5, wherein S2 comprises the substeps of:
s21, detecting whether the receiving end of each ultrasonic receiving and transmitting element receives ultrasonic waves in each transmitting period of the transmitting end of each ultrasonic receiving and transmitting element, if so, jumping to the step S22, and if not, continuing to detect the next period;
s22, judging whether the ultrasonic wave is an echo signal, if so, detecting an obstacle in the direction of the ultrasonic receiving and transmitting element in the corresponding direction, and jumping to the step S23, and if not, jumping to the step S21;
s23, calculating a first distance of the obstacle of the S22 by the following formula:
；
wherein d 1 A first distance v of the obstacle s Is sound velocity, t 1 The time interval between the receiving end of the ultrasonic receiving and transmitting element receiving the ultrasonic wave and the transmitting end transmitting the ultrasonic wave for detecting the obstacle;
s24, acquiring a second distance of the obstacle in a next transmission period by using the ultrasonic transceiver element which detects the obstacle in the S22;
s25, calculating the approximation speed of the obstacle of the S22 according to the following formula:
；
wherein v is ob Is the approach speed of the obstacle, d 2 A second distance being an obstacle;
s26, according to the direction of the ultrasonic transceiver element detecting the obstacle of the S22, direction information is given to the approximation speed of the S25, and an approximation speed vector of the obstacle of the S22 is obtained;
and S27, summarizing obstacles and respective approaching speed vectors of ultrasonic receiving and transmitting elements with other orientations in the same period.
7. The unmanned aerial vehicle obstacle avoidance flight method of claim 6 wherein in S22, if at the nth of the transmitting ends of each ultrasonic transceiver element 0 And (3) periodically, judging the ultrasonic signal received by the receiving end as an echo signal if the ultrasonic signal meets the following formula:
；
；
wherein f recv For the frequency of the received ultrasonic signal, f (m 0 ) Is the mth 0 A preset ultrasonic frequency f δ Is the frequency offset threshold.
8. The unmanned aerial vehicle obstacle avoidance flight method of claim 7, wherein S3 comprises the substeps of:
s31, constructing a particle swarm model, initializing the number of particles contained in the particle swarm to be N, setting the speed vector of each particle to be the speed vector of the unmanned aerial vehicle, and randomly setting the acceleration vector of each particle, wherein N is a positive integer;
s32, deducing the collision condition of each particle with multiple obstacles collided with respective approaching speed vectors after adjusting the speed vectors of the particles with respective accelerating vectors;
s34, selecting N with the longest avoidance time and successful avoidance time 1 Particles, N 1 The acceleration vector of particles which are smaller than the positive integer of N and are successfully avoided is recorded;
s35, judging whether the number of acceleration vectors of all recorded obstacle avoidance success is larger than N 2 If yes, go to step S37, if no, go to step S36, N 2 Is a positive integer;
s36, selecting the N selected in the S35 2 Randomly copying the acceleration vectors of the individual particles to each particle, randomly fine-adjusting the acceleration vectors of each particle, and jumping to the step S32;
s37, selecting a vector with the minimum two norms in the acceleration vector with successful obstacle avoidance in the record as an anti-collision acceleration vector.
9. The unmanned aerial vehicle obstacle avoidance flight method of claim 8, wherein the method of S4 is: taking the anti-collision acceleration vector as an acceleration target of the unmanned aerial vehicle, and iterating the power driving vector of the unmanned aerial vehicle through the following steps:
；
；
wherein u (k) is a power driving vector of the unmanned aerial vehicle at time k, P out For the outer PI loop proportionality coefficient, I out For the integral coefficient of the outer PI loop, f (k) is the output quantity of the inner PI loop at the moment k, f (k-1) is the output quantity of the inner PI loop at the moment k-1, u (k-1) is the power driving vector of the unmanned aerial vehicle at the moment k-1, and P in Is the proportional coefficient of the inner PI loop, I in Is the integral coefficient of an inner PI loop, ϵ (k) is the acceleration target of the unmanned aerial vehicle and the acceleration of the unmanned aerial vehicle at the moment kϵ (k-1) is the error vector of the acceleration target of the unmanned aerial vehicle and the acceleration at the moment of the unmanned aerial vehicle k-1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410100873.3A CN117631689B (en) | 2024-01-25 | 2024-01-25 | Unmanned aerial vehicle obstacle avoidance flight method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410100873.3A CN117631689B (en) | 2024-01-25 | 2024-01-25 | Unmanned aerial vehicle obstacle avoidance flight method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117631689A true CN117631689A (en) | 2024-03-01 |
| CN117631689B CN117631689B (en) | 2024-04-16 |
Family
ID=90025494
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410100873.3A Active CN117631689B (en) | 2024-01-25 | 2024-01-25 | Unmanned aerial vehicle obstacle avoidance flight method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117631689B (en) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105717504A (en) * | 2015-08-11 | 2016-06-29 | 王宗博 | Unmanned aerial vehicle 360-degree electronic scanning obstacle avoidance radar |
| CN106020204A (en) * | 2016-07-21 | 2016-10-12 | 触景无限科技（北京）有限公司 | Obstacle detection device, robot and obstacle avoidance system |
| CN109765914A (en) * | 2019-03-12 | 2019-05-17 | 哈尔滨工程大学 | A kind of unmanned surface vehicle collision prevention method based on sliding window population |
| CN109947129A (en) * | 2019-03-29 | 2019-06-28 | 华南理工大学 | Rotor wing unmanned aerial vehicle paths planning method based on Dijkstra and improvement particle swarm algorithm |
| KR102115294B1 (en) * | 2020-02-28 | 2020-06-02 | 주식회사 파블로항공 | Collision Avoidance for UAV |
| CN112306090A (en) * | 2020-10-26 | 2021-02-02 | 中国人民解放军军事科学院国防科技创新研究院 | Unmanned aerial vehicle instant rapid obstacle avoidance method based on relative displacement and velocity vector synthesis |
| CN113655810A (en) * | 2021-08-20 | 2021-11-16 | 上海微电机研究所(中国电子科技集团公司第二十一研究所) | Unmanned aerial vehicle obstacle avoidance method and system based on speed potential field |
| CN115494866A (en) * | 2022-09-22 | 2022-12-20 | 珠海安擎科技有限公司 | Multi-unmanned aerial vehicle global and local path intelligent planning method and system |
| CN116382318A (en) * | 2023-04-12 | 2023-07-04 | 广州昂宝电子有限公司 | Unmanned aerial vehicle and obstacle avoidance device thereof |
| CN117369507A (en) * | 2023-10-31 | 2024-01-09 | 河海大学 | Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm |
-
2024
- 2024-01-25 CN CN202410100873.3A patent/CN117631689B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105717504A (en) * | 2015-08-11 | 2016-06-29 | 王宗博 | Unmanned aerial vehicle 360-degree electronic scanning obstacle avoidance radar |
| CN106020204A (en) * | 2016-07-21 | 2016-10-12 | 触景无限科技（北京）有限公司 | Obstacle detection device, robot and obstacle avoidance system |
| CN109765914A (en) * | 2019-03-12 | 2019-05-17 | 哈尔滨工程大学 | A kind of unmanned surface vehicle collision prevention method based on sliding window population |
| CN109947129A (en) * | 2019-03-29 | 2019-06-28 | 华南理工大学 | Rotor wing unmanned aerial vehicle paths planning method based on Dijkstra and improvement particle swarm algorithm |
| KR102115294B1 (en) * | 2020-02-28 | 2020-06-02 | 주식회사 파블로항공 | Collision Avoidance for UAV |
| CN112306090A (en) * | 2020-10-26 | 2021-02-02 | 中国人民解放军军事科学院国防科技创新研究院 | Unmanned aerial vehicle instant rapid obstacle avoidance method based on relative displacement and velocity vector synthesis |
| CN113655810A (en) * | 2021-08-20 | 2021-11-16 | 上海微电机研究所(中国电子科技集团公司第二十一研究所) | Unmanned aerial vehicle obstacle avoidance method and system based on speed potential field |
| CN115494866A (en) * | 2022-09-22 | 2022-12-20 | 珠海安擎科技有限公司 | Multi-unmanned aerial vehicle global and local path intelligent planning method and system |
| CN116382318A (en) * | 2023-04-12 | 2023-07-04 | 广州昂宝电子有限公司 | Unmanned aerial vehicle and obstacle avoidance device thereof |
| CN117369507A (en) * | 2023-10-31 | 2024-01-09 | 河海大学 | Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117631689B (en) | 2024-04-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3521158B1 (en) | Flight control device, unmanned aerial vehicle, flight control method, and computer program | |
| Liu et al. | High speed navigation for quadrotors with limited onboard sensing | |
| CN111123963B (en) | Unknown environment autonomous navigation system and method based on reinforcement learning | |
| CN107562048B (en) | Dynamic obstacle avoidance control method based on laser radar | |
| CN110764531B (en) | Unmanned aerial vehicle formation flying obstacle avoidance method based on laser radar and artificial potential field method | |
| US20180350086A1 (en) | System And Method Of Dynamically Filtering Depth Estimates To Generate A Volumetric Map Of A Three-Dimensional Environment Having An Adjustable Maximum Depth | |
| WO2017221859A1 (en) | Moving body, moving body control system, moving body control method, interface device, and recording medium having program recorded thereon | |
| CN110262555B (en) | Real-time obstacle avoidance control method for unmanned aerial vehicle in continuous obstacle environment | |
| CN111381600A (en) | UUV path planning method based on particle swarm optimization | |
| WO2021003983A1 (en) | Return guide method, return guide apparatus, mobile robot, and storage medium | |
| US11886198B2 (en) | Systems and methods for detecting blind spots for robots | |
| Chen et al. | Obstacle avoidance strategy for quadrotor UAV based on improved particle swarm optimization algorithm | |
| Wu et al. | Vision-based target detection and tracking system for a quadcopter | |
| Ahmed et al. | An energy efficient IoD static and dynamic collision avoidance approach based on gradient optimization | |
| Park et al. | Indoor path planning for an unmanned aerial vehicle via curriculum learning | |
| US11029709B1 (en) | Adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data | |
| Ahmad et al. | End-to-end probabilistic depth perception and 3d obstacle avoidance using pomdp | |
| CN117631689B (en) | Unmanned aerial vehicle obstacle avoidance flight method | |
| Masmitja et al. | A reinforcement learning path planning approach for range-only underwater target localization with autonomous vehicles | |
| WO2022126396A1 (en) | State information determination method and apparatus, and platform and computer-readable storage medium | |
| Karahan et al. | Path planning and collision avoidance with artificial intelligence for a quadrotor UAV | |
| Yang et al. | Optimization of dynamic obstacle avoidance path of multirotor UAV based on ant colony algorithm | |
| Figetakis et al. | UAV path planning using on-board ultrasound transducer arrays and edge support | |
| Ciftler et al. | Indoor uav navigation to a rayleigh fading source using q-learning | |
| CN114721435A (en) | Soft constraint-based defense method for unmanned aerial vehicle cluster and related equipment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |