CN108594849B - Unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar - Google Patents
Unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar Download PDFInfo
- Publication number
- CN108594849B CN108594849B CN201810319791.2A CN201810319791A CN108594849B CN 108594849 B CN108594849 B CN 108594849B CN 201810319791 A CN201810319791 A CN 201810319791A CN 108594849 B CN108594849 B CN 108594849B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- obstacle
- millimeter wave
- distance
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 230000004888 barrier function Effects 0.000 claims abstract description 37
- 238000001514 detection method Methods 0.000 claims abstract description 21
- 238000012545 processing Methods 0.000 claims description 12
- 238000012790 confirmation Methods 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 5
- 238000012216 screening Methods 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 3
- 238000007619 statistical method Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 4
- 238000005507 spraying Methods 0.000 description 4
- 238000001914 filtration Methods 0.000 description 3
- 238000012544 monitoring process Methods 0.000 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 description 2
- 230000001276 controlling effect Effects 0.000 description 2
- 230000002596 correlated effect Effects 0.000 description 2
- 230000000875 corresponding effect Effects 0.000 description 2
- 239000003814 drug Substances 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000000575 pesticide Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Radar Systems Or Details Thereof (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention is suitable for the field of improvement of automatic control technology, and provides an unmanned aerial vehicle obstacle avoidance method based on millimeter wave radars, which is characterized in that a plurality of millimeter wave radars are used for detection, signals are converted into position coordinates of obstacles and input into a well-established local map, and the positions of the obstacles are determined and decelerated in advance through a probability statistical method of map data; and replanning the obstacle avoidance route, and controlling the plant protection unmanned aerial vehicle to avoid the obstacle and return to the air route for continuous operation according to the route. Utilize radar feedback to detect the barrier, establish and keep away the barrier route, effectual flying speed has improved has relieved artificial control and has avoided the barrier, has improved the operating efficiency, has increased the work efficiency of operation at night.
Description
Technical Field
The invention belongs to the field of improvement of automation control technology, and particularly relates to an unmanned aerial vehicle obstacle avoidance method based on a millimeter wave radar.
Background
With the rapid development of agricultural automation technology, plant protection unmanned aerial vehicles have been widely used in the field of pesticide spraying of crops. Plant protection unmanned aerial vehicle's actual farmland operation environment is very complicated, and wire pole, tree, people, animal, earth potential difference etc. all can become unmanned aerial vehicle flight in-process barrier, especially common wire pole in the farmland, have improved the risk that unmanned aerial vehicle explodes the machine. In order to avoid the obstacles, the user is required to intentionally avoid the obstacles in the map when planning the route, which easily causes map blocking, increases workload, and reduces the working efficiency of the unmanned aerial vehicle. Especially, under the condition that the position of the obstacle is unknown, the air route cannot be accurately and reliably planned, so that the unmanned aerial vehicle cannot work.
In the prior art, an autonomous obstacle avoidance method based on an image is to hover and avoid an obstacle when a camera identifies the obstacle. The scheme can not completely overcome the influence of low-light flight at night on camera imaging, and can only reduce the speed flight, so that the night operation efficiency is reduced. The autonomous obstacle avoidance method based on the radar is suspended when meeting obstacles, autonomous flight cannot be continued, manual operation and control are needed to avoid the obstacles, and the operation efficiency is greatly reduced.
Disclosure of Invention
The invention aims to provide an unmanned aerial vehicle obstacle avoidance method based on a millimeter wave radar, and aims to solve the problem.
The invention is realized in the way, an unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar comprises the following steps:
s1, setting an unmanned aerial vehicle operation area through the ground station and automatically generating an operation route by a user;
s2, obtaining distance information and angle information of the obstacle and the unmanned aerial vehicle in automatic flight operation in the current environment through a millimeter wave radar, and establishing a local grid map by taking the unmanned aerial vehicle as a center;
s3, calculating the relative position of the obstacle and the unmanned aerial vehicle according to the acquired distance and angle information, and calculating the absolute coordinate of the obstacle according to the flight coordinate provided by the RTK module;
s4, mapping absolute coordinates of the obstacle obtained according to the real-time position of the unmanned aerial vehicle on a local grid map with the real-time unmanned aerial vehicle as the center;
s5, spirally searching whether the coordinate point of the obstacle is larger than a preset value or not by taking the unmanned aerial vehicle as a center, if so, placing the obstacle in a candidate area and executing the next step; such as less than a preset value. The information is deleted;
s6, judging whether the current barrier influences the flight of the current air route according to the set operation air route of the unmanned aerial vehicle, if so, calculating the position information between the unmanned aerial vehicle and the barrier according to the direction of the air route to generate obstacle avoidance line navigation and executing the next step, and if not, continuing the navigation work;
and S7, judging whether the unmanned aerial vehicle avoids the current obstacle, if so, returning to the original working route to continue working and executing the step S5, and if not, continuing the navigation and executing the step S7.
The further technical scheme of the invention is as follows: the step S4 further includes the following steps:
and S41, judging whether each piece of data information fed back by the millimeter wave radar is real and effective, if so, mapping the effective data on the grid map, and if not, not processing the current data.
The further technical scheme of the invention is as follows: the step S5 further includes the following steps:
and S51, screening out the obstacle having the largest influence on the unmanned air route according to the priority index of the obstacle.
The further technical scheme of the invention is as follows: in the step S2, a plurality of millimeter wave radar combinations are adopted for detection, wide beam millimeter wave radars are installed on the left and right sides of the unmanned aerial vehicle in an inclined manner at a certain angle to cover the regions on the two sides, and a narrow beam millimeter wave radar is installed in front of the unmanned aerial vehicle to detect the high risk region in front of the unmanned aerial vehicle.
The further technical scheme of the invention is as follows: the narrow-beam millimeter wave radar carries out remote detection and indicates the unmanned aerial vehicle to decelerate, and the wide-beam millimeter wave radar is responsible for data redundancy detection of a high-order area and accurate positioning of remote detection and close-range obstacles of two side areas for the overlapping areas on the two sides.
The further technical scheme of the invention is as follows: the deceleration strategy of the wide-beam millimeter wave radar is as follows: judge that the barrier is located the safe distance of the unmanned aerial vehicle left and right sides within, if within safe distance, then control unmanned aerial vehicle to slow down to horizontal distance equals safe distance department and hovers, if outside safe distance, then judge whether vertical distance is greater than suspicious distance threshold value, if vertical distance is less than suspicious distance threshold value, then unmanned aerial vehicle begins to slow down to less speed of setting for and continues the flight apart from barrier certain distance department, resume former operation speed to continue the operation after crossing suspicious barrier until unmanned aerial vehicle, if vertical distance is greater than suspicious distance threshold value, then unmanned aerial vehicle does not respond, continue the flight with the operating speed.
The further technical scheme of the invention is as follows: the narrow-beam millimeter wave radar deceleration strategy is as follows: when the obstacle is located in the beam range, when the obstacle is detected, the distance between the unmanned aerial vehicle and the obstacle is calculated, the unmanned aerial vehicle starts to decelerate at a preset deceleration distance, and the unmanned aerial vehicle keeps flying at a constant speed after being decelerated to a preset speed. When the wide-beam millimeter wave radar detects an obstacle and decelerates to move to a set range, original obstacle information is immediately cleared and secondary confirmation is started, if the obstacle is not detected before timing is finished, the original operation speed is recovered to continue operation, and if the obstacle is detected after the secondary confirmation, an obstacle avoiding route is automatically planned to start obstacle avoidance. When the narrow beam radar and the wide beam radar detect obstacles at different positions at the same time, the deceleration priority is as follows: the short-distance obstacle monitored by the wide-beam radar is larger than the long-distance obstacle monitored by the narrow-beam radar is larger than the short-distance obstacle monitored by the narrow-beam radar.
The further technical scheme of the invention is as follows: the calculation formula of the absolute coordinates of the obstacle in step S3 is:
xo＝xp+dr·cos(psi+θr+α)+dp·cos(psi)
yo＝yp+dr·sin(psi+θr+α)+dp·sin(psi)
wherein (x)o，yo) Is the coordinate of the obstacle, (x)p，yp) Coordinates are made for the position of the unmanned aerial vehicle; drThe distance of the obstacle output by the millimeter wave radar; psi is the unmanned plane heading angle, θrThe obstacle angle is output by the millimeter wave radar; alpha is a radar mounting angle; d is a radical ofpThe distance between the millimeter wave radar center and the unmanned aerial vehicle center.
The further technical scheme of the invention is as follows: the deceleration priority in S6 is short-range obstacle monitored by the wide-beam radar > long-range obstacle monitored by the narrow-beam radar > long-range obstacle monitored by the wide-beam radar > short-range obstacle monitored by the narrow-beam radar.
The further technical scheme of the invention is as follows: the obstacle avoidance line in the step S6 is generated according to the horizontal distance and the vertical distance from the obstacle to the unmanned aerial vehicle; and if the obstacle avoidance line exceeds the operation area in the direction of the preferential obstacle avoidance line, trying to generate an obstacle avoidance line in the other side direction.
The invention has the beneficial effects that: utilize radar feedback to detect the barrier, establish and keep away the barrier route, effectual flying speed has improved has relieved artificial control and has avoided the barrier, has improved the operating efficiency, has increased the work efficiency of operation at night.
Drawings
Fig. 1 is a flowchart of an unmanned aerial vehicle obstacle avoidance method based on a millimeter wave radar according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of a local grid map according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of generating a conventional automatic obstacle avoidance route according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of generating an automatic obstacle avoidance route when an obstacle exists near a boundary of a course of departure provided by the embodiment of the present invention.
Fig. 5 is a schematic diagram of generating an automatic obstacle avoidance route when an obstacle exists near a boundary of a working route according to an embodiment of the present invention.
Detailed Description
As shown in fig. 1, the millimeter wave radar-based unmanned aerial vehicle obstacle avoidance method provided by the present invention is detailed as follows:
step S1, a user sets an unmanned aerial vehicle operation area through a ground station and automatically generates an operation route; the user carries out the operation regional setting to unmanned aerial vehicle in the ground station of ground setting, utilizes plane coordinate system to set up in unmanned aerial vehicle control system and carry out work in certain region to according to the coordinate value in region and the automatic flight line that generates in the scope that unmanned aerial vehicle covered, wherein the radar adopts the millimeter wave radar.
Step S2, obtaining distance information and angle information of the obstacle and the unmanned aerial vehicle in automatic flight operation in the current environment through the millimeter wave radar and establishing a local grid map by taking the unmanned aerial vehicle as a center; unmanned aerial vehicle is in flight operation, installs the radar in its dead ahead and both sides and sending out the detection sound wave incessantly, meets distance and angle information between barrier and the unmanned aerial vehicle in obtaining the current environment through the detection sound wave of feedback, still can use unmanned aerial vehicle as the real-time local grid map of establishing of center at the time in-process, as shown in fig. 2.
The combination of a plurality of millimeter wave radars is adopted for detection, the wide beam millimeter wave radars are installed on the left side and the right side of the unmanned aerial vehicle in a tilting mode at a certain angle to cover the areas on the two sides, and the narrow beam millimeter wave radars are installed in the front of the unmanned aerial vehicle to detect the high risk area in front of the unmanned aerial vehicle.
The narrow-beam millimeter wave radar carries out remote detection and indicates the unmanned aerial vehicle to decelerate, and the wide-beam millimeter wave radar is responsible for data redundancy detection of a high-order area and accurate positioning of remote detection and close-range obstacles of two side areas for the overlapping areas on the two sides.
The deceleration strategy of the wide-beam millimeter wave radar is as follows: judge that the barrier is located the safe distance of the unmanned aerial vehicle left and right sides within, if within safe distance, then control unmanned aerial vehicle to slow down to horizontal distance equals safe distance department and hovers, if outside safe distance, then judge whether vertical distance is greater than suspicious distance threshold value, if vertical distance is less than suspicious distance threshold value, then unmanned aerial vehicle begins to slow down to less speed of setting for and continues the flight apart from barrier certain distance department, resume former operation speed to continue the operation after crossing suspicious barrier until unmanned aerial vehicle, if vertical distance is greater than suspicious distance threshold value, then unmanned aerial vehicle does not respond, continue the flight with the operating speed.
The narrow-beam millimeter wave radar deceleration strategy is as follows: when the obstacle is located in the beam range, when the obstacle is detected, the distance between the unmanned aerial vehicle and the obstacle is calculated, the unmanned aerial vehicle starts to decelerate at a preset deceleration distance, and the unmanned aerial vehicle keeps flying at a constant speed after being decelerated to a preset speed. When the wide-beam millimeter wave radar detects an obstacle and decelerates to move to a set range, original obstacle information is immediately cleared and secondary confirmation is started, if the obstacle is not detected before timing is finished, the original operation speed is recovered to continue operation, and if the obstacle is detected after the secondary confirmation, an obstacle avoiding route is automatically planned to start obstacle avoidance. When the narrow beam radar and the wide beam radar detect obstacles at different positions at the same time, the deceleration priority is as follows: the short-distance obstacle monitored by the wide-beam radar is larger than the long-distance obstacle monitored by the narrow-beam radar is larger than the short-distance obstacle monitored by the narrow-beam radar.
Step S3, calculating the relative position of the obstacle and the unmanned aerial vehicle according to the acquired distance and angle information, and calculating the absolute coordinate of the obstacle according to the flight coordinate provided by the RTK module; in the flying process, the control system calculates the relative position relation between the obstacle and the unmanned aerial vehicle according to information fed back by the radar, and calculates the absolute coordinate of the obstacle according to the flying coordinate converted from longitude and latitude information given by the satellite positioning module, wherein the calculation formula of the absolute coordinate of the obstacle is as follows:
xo＝xp+dr·cos(psi+θr+α)+dp·cos(psi)
yo＝yp+dr·sin(psi+θr+α)+dp·sin(psi)
wherein (x)o，yo) Is the coordinate of the obstacle, (x)p，yp) Coordinates are made for the position of the unmanned aerial vehicle; drThe distance of the obstacle output by the millimeter wave radar; psi is the unmanned plane heading angle, θrThe obstacle angle is output by the millimeter wave radar; alpha is a radar mounting angle; dpThe distance between the millimeter wave radar center and the unmanned aerial vehicle center.
Step S4, absolute coordinates of the obstacle obtained according to the real-time position of the unmanned aerial vehicle are mapped on a local grid map with the real-time unmanned aerial vehicle as the center; and judging whether each piece of data information fed back by the millimeter wave radar is real and effective, if so, mapping the effective data on the grid map, and if not, not performing any processing on the current data. And judging whether each datum of the millimeter wave radar is real and effective or not, and inputting the effective datum into the grid map. And calculating the coordinates of the grid map according to the position of the unmanned aerial vehicle in real time, and updating map data.
Step S5, spirally searching whether the coordinate point of the obstacle is larger than a preset value or not by taking the unmanned aerial vehicle as a center, if so, placing the obstacle in a candidate area and executing the next step; such as less than a preset value. The information is deleted; the unmanned aerial vehicle is used as the center, the map is scanned outwards in a spiral mode, the obstacles in the map are placed in the obstacle selecting area, and the obstacles which have the largest influence on the air route of the unmanned aerial vehicle are screened out according to the obstacle priority indexes. The screening priority is that the vertical distance is smaller than the safe vertical distance and the barrier with the closest linear distance is larger than the barrier with the vertical distance smaller than the safe vertical distance and the barrier with the farther linear distance, the barrier with the vertical distance slightly larger than the safe vertical distance is larger than the barrier with the vertical distance far larger than the safe vertical distance
Step S6, judging whether the current barrier influences the flight of the current air route according to the set operation air route of the unmanned aerial vehicle, if so, calculating the position information between the unmanned aerial vehicle and the barrier according to the direction of the air route to generate obstacle avoidance route navigation and executing the next step, and if not, continuing the navigation work; the obstacle avoidance line is generated according to the horizontal distance and the vertical distance from the obstacle to the unmanned aerial vehicle; and if the obstacle avoidance line exceeds the operation area in the direction of the preferential obstacle avoidance line, trying to generate an obstacle avoidance line in the other side direction.
And step S7, judging whether the unmanned aerial vehicle avoids the current obstacle, if so, returning to the original working route to continue working and executing step S5, and if not, continuing the navigation and executing step S7. And judging whether the obstacle influences the flight of the current air route by combining the planned operation air route of the plant protection unmanned aerial vehicle, calculating the vertical distance and the horizontal distance between the unmanned aerial vehicle and the obstacle according to the direction of the air route, and generating an obstacle avoidance line to avoid the obstacle autonomously. And when the unmanned aerial vehicle avoids the barrier and returns to the air route, the unmanned aerial vehicle continues to operate along the original air route, so that the phenomenon of missed spraying is prevented. Specific examples of obstacle avoidance routes are shown in fig. 3 to 5.
And detecting by adopting a plurality of millimeter wave radar combinations. The wide-beam millimeter wave radar is obliquely installed at the left side and the right side of the unmanned aerial vehicle at a certain angle and is used for covering the areas at the left side and the right side of the unmanned aerial vehicle. Simultaneously, install narrow beam millimeter wave radar in unmanned aerial vehicle the place ahead for detect the high risk area in unmanned aerial vehicle the place ahead.
The narrow-beam millimeter wave radar right in front is mainly used for remote detection and indicating the unmanned aerial vehicle to decelerate; and the overlapping regions of the wide-beam millimeter wave radars on the left side and the right side are responsible for data redundancy detection of high-risk regions. Meanwhile, the unmanned aerial vehicle remote detection system can be respectively responsible for remote detection and close-range obstacle accurate positioning of the left area and the right area of the unmanned aerial vehicle.
Wherein the wide-beam radar decelerates using the following strategy:
if the obstacle is located within the safe distance of the left side and the right side of the unmanned aerial vehicle, namely the vertical distance between the obstacle and the current air route is smaller than the safe distance, the unmanned aerial vehicle is controlled to decelerate to the position where the horizontal distance is equal to the safe distance to hover.
If the barrier is located outside the safe distance of the two sides of the unmanned aerial vehicle, and the vertical distance is smaller than the threshold value of the suspicious distance, the unmanned aerial vehicle starts to decelerate to a smaller set speed at a certain distance from the barrier to continue flying until the unmanned aerial vehicle recovers the original operation speed to continue working after crossing the suspicious barrier.
If the obstacles are positioned on two sides of the unmanned aerial vehicle, and the vertical distance is greater than the threshold value of the suspicious distance, the unmanned aerial vehicle does not respond, and the unmanned aerial vehicle continues flying at the operation speed.
The narrow beam radar adopts the following strategy to decelerate:
if the obstacle is located within the beam range, the obstacle is by default located directly in front of the flight path. When the obstacle is detected, the distance between the airplane and the obstacle is calculated, the airplane starts to decelerate at the set deceleration distance until the speed is reduced to the set speed, and the airplane flies at a constant speed. And if the wide-beam radar detects the obstacle and enters a secondary detection link or passes the obstacle, recovering the original operation speed to continue the operation.
And the obstacle avoidance system performs deceleration operation according to the priority of the obstacles detected by each radar. Preferably, the priority can be set as: the method comprises the steps of wide-beam radar monitoring short-distance obstacles, narrow-beam radar monitoring long-distance obstacles and narrow-beam monitoring short-distance obstacles.
An RTK positioning and orientation system is used for measuring the real-time position and heading of the unmanned aerial vehicle.
As shown in fig. 2, the obstacle coordinate calculation formula is:
xo＝xp+dr·cos(psi+θr+α)+dp·cos(psi)；yo＝yp+dr·sin(psi+θr+α)+dp·sin(psi)；
said (x)o，yo) Is the coordinate of the obstacle, (x)p，yp) Coordinates are made for the position of the unmanned aerial vehicle; drThe distance of the obstacle output by the millimeter wave radar; psi is the unmanned plane heading angle, θrThe obstacle angle is output by the millimeter wave radar; alpha is a radar mounting angle; dpThe distance between the millimeter wave radar center and the unmanned aerial vehicle center.
Unmanned aerial vehicle brakes to behind the barrier the place ahead, carries out the barrier secondary and detects.
If the obstacle is detected by the millimeter wave radar again, the unmanned aerial vehicle avoids the obstacle according to the preset obstacle avoidance logic.
And if the obstacle is not detected by the millimeter wave radar, the unmanned aerial vehicle recovers the original operation speed to continue operation.
The steering engine is used for driving the millimeter wave radar to rotate in the pitching direction of the unmanned aerial vehicle, the steering engine adjusts the angle of the radar in real time according to the pitching angle information of the unmanned aerial vehicle, and the radar is guaranteed to be always in a certain angle with the horizontal plane.
And measuring a real-time inclination angle of the millimeter wave radar by using the inertial navigation sensor, feeding back the real-time inclination angle to the radar data processing module, and controlling the steering engine to rotate. When unmanned aerial vehicle was in the acceleration and deceleration flight state, its pitch angle was the negative, and this moment is used to lead the sensor and record this pitch angle and pass through PWM signal control steering wheel reverse direction and rotate, and the influence of compensation pitch angle to the radar, the radar is surveyed towards the unmanned aerial vehicle dead ahead region all the time, has effectively avoided ground signal's interference, has improved millimeter wave radar's signal stability.
The radar data processing module is used for acquiring and processing radar data, processing radar inclination angle data, outputting steering engine signals and communicating with flight control data.
And the forward radar and the left and right radars respectively establish independent grid maps. Wherein the radar of left and right sides shares a two-dimensional map, and this map is with the square region of unmanned aerial vehicle as the center, and the coordinate of every grid of this map adopts the mode of getting integer to calculate and obtains, and the data in the map need not to be according to the frequent update of unmanned aerial vehicle direction of flight and unmanned aerial vehicle small-range motion. Meanwhile, the mobility of data and lower data operation cost are considered; the map corresponding to the forward radar is a one-dimensional grid, the direction of the map is consistent with the current air route, and the initial grid of the map is the result of the unmanned aerial vehicle after the coordinate is rounded. The data in the map does not need to be updated frequently according to the small-range movement of the unmanned aerial vehicle, and the map data can be cleared after the air route is switched.
The obstacles in the grid map correspond to a probability value, and the probability value is positively correlated with the frequency of detecting the position by the radar; and if the probability value of a certain grid is greater than a set threshold value, marking the position of the obstacle in the map. Preferably, the third power of the count value of each grid is selected as a probability value, so that the probability value of the grid increases exponentially with the increase of the count value, and the probability representing the existence of an obstacle in the grid is higher.
Obstacle speed v output by millimeter wave radaroAnd unmanned aerial vehicle velocity vpThe data matching mode is used for filtering out irrelevant false alarm data, and specifically, the following method can be adopted for filtering out invalid data:
if: v. ofo≤vp+ Δ v, and vo≥vp- Δ v, and voWhen the data is less than 0, the radar data is matched with the speed of the unmanned aerial vehicle, and the data can be input into a grid map;
if: v. ofo＞vp+ Δ v, or vo＜vp- Δ v, or voWhen being more than or equal to 0, the radar data is not matched with the speed of the unmanned aerial vehicle, the data is filtered, and the grid map cannot be input.
Where Δ v is a parameter related to the speed error range.
The map grid search adopts a spiral near-to-far search mode with an unmanned aerial vehicle as a center. The following are examples of searches: and searching by taking the grid where the unmanned aerial vehicle is as the first grid, and searching a grid on the right side of the area when the grid searching is finished and no barrier exists. And when the grid searching is finished and no obstacle exists, searching the upper grid of the area. And when the grid searching is finished and no obstacle exists, searching two grids on the left side of the area. And when the search in the grid is completed and no obstacle exists, searching three grids at the lower side of the area. And repeating the step of traversing the grid map in the counterclockwise direction, and when the grid where the obstacle is located is met, taking the center coordinate of the grid as the position of the obstacle and placing the obstacle into the alternative area. And stopping searching until the number of the obstacles in the candidate area is larger than the maximum number of the obstacles.
The obstacle avoidance path is generated according to the horizontal distance and the vertical distance from the obstacle to the unmanned aerial vehicle, and the obstacle is avoided at a specified safe distance. Preferably, the direction of the first line of the obstacle avoidance line is opposite to the direction of the obstacle, so that the phenomenon that the overlong obstacle avoidance line reduces the operation efficiency is avoided.
Meanwhile, an obstacle avoidance route generated by the unmanned aerial vehicle is limited by an operation area, and the obstacle avoidance route is not allowed to exceed the operation area. If the obstacle avoidance line crosses the operation area in the preferred direction, the obstacle avoidance system tries to generate an obstacle avoidance line in the other direction, and the path search is prevented from failing. For the condition that the obstacle is at the flight path boundary, the obstacle avoidance system selectively skips part of the flight path to directly enter the next flight path for continuous operation, and effectively avoids the dangerous area where the obstacle is located.
Keep away barrier in-process unmanned aerial vehicle and stop the operation, prevent to spill the medicine repeatedly. After the unmanned aerial vehicle avoids the barrier and returns to the original routing, the operation system is started to continue operation, and the spraying leakage is prevented.
After the unmanned aerial vehicle generates an effective obstacle avoidance route, if other obstacles are monitored by the millimeter wave radar in the flying process along the obstacle avoidance route, the unmanned aerial vehicle is controlled to hover. At the moment, the ground station pops up alarm information to inform the user.
And after the unmanned aerial vehicle brakes to the front of the obstacle, detecting the obstacle again for the second time. If the obstacle is detected by the millimeter wave radar again, the unmanned aerial vehicle avoids the obstacle according to a preset obstacle avoidance logic; if the obstacle is not detected by the millimeter wave radar, the unmanned aerial vehicle continues to operate according to the original operation speed.
The steering engine is used for driving the millimeter wave radar to rotate in the pitching direction of the unmanned aerial vehicle, the steering engine adjusts the angle of the radar in real time according to the pitching angle information of the unmanned aerial vehicle, and the radar is guaranteed to be always in a certain angle with the horizontal plane.
And measuring a real-time inclination angle of the millimeter wave radar by using the inertial navigation sensor, feeding back the real-time inclination angle to the radar data processing module, and controlling the steering engine to rotate.
The radar data processing module is used for acquiring and processing radar data, processing radar inclination angle data, outputting steering engine signals and communicating with flight control data.
And the forward radar and the left and right radars respectively establish independent grid maps. The two radars on the left side and the right side share one two-dimensional grid map, the map is a square area with the unmanned aerial vehicle as the center, the coordinates of each grid of the map are obtained through calculation in an integer mode, and data in the map do not need to be updated frequently according to the flight direction of the unmanned aerial vehicle and small-range movement of the unmanned aerial vehicle. The map corresponding to the forward radar is a one-dimensional grid map, the direction of the map is consistent with the current route, and the initial grid of the map is the result of the unmanned aerial vehicle after the coordinates are rounded. The data in the map does not need to be updated frequently according to the small-range movement of the unmanned aerial vehicle, and the data in the map can be cleared after the air route is switched every time.
The obstacles in the grid map correspond to a probability value, and the probability value is positively correlated with the frequency of detecting the position by the radar; and if the probability value of a certain grid is greater than a set threshold value, marking the position of the obstacle in the map.
And filtering irrelevant false alarm data by adopting a mode of matching the obstacle speed information output by the millimeter wave radar with the unmanned aerial vehicle speed data.
The map grid search adopts a spiral near-to-far search mode with an unmanned aerial vehicle as a center.
The obstacle avoidance path is generated according to the horizontal distance and the vertical distance from the obstacle to the unmanned aerial vehicle, and the obstacle is avoided at a specified safe distance.
The obstacle avoidance route generated by the unmanned aerial vehicle is limited by the operation area, and the obstacle avoidance route is not allowed to exceed the operation area.
Keep away barrier in-process unmanned aerial vehicle and stop the operation, prevent to spill medicine repeatedly. After the unmanned aerial vehicle avoids the barrier and returns to the original routing, the operation system is started to continue operation, and the spraying leakage is prevented.
After the unmanned aerial vehicle generates an effective obstacle avoidance route, if other obstacles are monitored by the millimeter wave radar in the flying process along the obstacle avoidance route, the unmanned aerial vehicle is controlled to hover.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents and improvements made within the spirit and principle of the present invention are intended to be included within the scope of the present invention.
Claims (7)
1. An unmanned aerial vehicle obstacle avoidance method based on a millimeter wave radar is characterized by comprising the following steps:
s1, setting an unmanned aerial vehicle operation area through the ground station and automatically generating an operation route by a user;
s2, obtaining distance information and angle information of the obstacle and the unmanned aerial vehicle in automatic flight operation in the current environment through a millimeter wave radar, and establishing a local grid map by taking the unmanned aerial vehicle as a center;
s3, calculating the relative position of the obstacle and the unmanned aerial vehicle according to the acquired distance and angle information, and calculating the absolute coordinate of the obstacle according to the flight coordinate given by the RTK module;
s4, mapping absolute coordinates of the obstacle obtained according to the real-time position of the unmanned aerial vehicle on a local grid map with the real-time unmanned aerial vehicle as the center;
s5, spirally searching whether the coordinate point of the obstacle is larger than a preset value or not by taking the unmanned aerial vehicle as a center, if so, placing the obstacle in a candidate area and executing the next step; if the current value is less than the preset value, no processing is carried out;
s6, judging whether the current obstacle affects the flight of the current air route according to the established operation air route of the unmanned aerial vehicle, if so, reducing the speed in advance, calculating the position information between the unmanned aerial vehicle and the obstacle according to the direction of the air route to generate obstacle avoidance route navigation, executing the next step, and if not, continuing the navigation work;
s7, judging whether the unmanned aerial vehicle avoids the current obstacle, if so, returning to the original operation route to continue working and executing the step S5, and if not, continuing navigating and executing the step S7;
in the step S2, a plurality of millimeter wave radar combinations are adopted for detection, wide-beam millimeter wave radars are installed on the left side and the right side of the unmanned aerial vehicle in a tilted manner at a certain angle to cover low risk areas on the two sides of the unmanned aerial vehicle and high risk areas in front of the unmanned aerial vehicle, and narrow-beam millimeter wave radars are installed right in front of the unmanned aerial vehicle to detect the high risk areas in front of the unmanned aerial vehicle;
the step S4 further includes the following steps:
s41, judging whether each piece of data information fed back by the millimeter wave radar is real and effective, if so, mapping the effective data on a grid map, and if not, not processing the current data;
the calculation formula of the absolute coordinates of the obstacle in step S3 is:
xo＝xp+dr·cos(psi+θr+α)+dp·cos(psi)
yo＝yp+dr·sin(psi+θr+α)+dp·sin(psi)
wherein (x)o,yo) Is the coordinate of the obstacle, (x)p,yp) Coordinates are made for the position of the unmanned aerial vehicle; d is a radical ofrThe distance of the obstacle output by the millimeter wave radar; psi is the unmanned plane heading angle, θrThe obstacle angle is output by the millimeter wave radar; alpha is a radar mounting angle; dpThe distance between the millimeter wave radar center and the unmanned aerial vehicle center.
2. The obstacle avoidance method for the unmanned aerial vehicle based on the millimeter wave radar as claimed in claim 1, wherein the step S5 further comprises the following steps:
s51, screening out the obstacle having the largest influence on the unmanned aerial vehicle air route according to the priority index of the obstacle.
3. The unmanned aerial vehicle obstacle avoidance method based on the millimeter wave radar as claimed in claim 2, wherein the narrow beam millimeter wave radar performs remote detection in a front high risk area and instructs the unmanned aerial vehicle to decelerate, and the wide beam millimeter wave radar is responsible for overlapping areas on two sides: data redundancy detection of high risk areas right ahead, remote detection of low risk areas on two sides, and accurate detection of short-distance obstacles ahead.
4. The unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar of claim 3, wherein the deceleration strategy of the wide-beam millimeter wave radar is as follows: judge whether the barrier is located the safe distance of the unmanned aerial vehicle left and right sides within, if within safe distance, then control unmanned aerial vehicle to slow down to horizontal distance equals safe distance department and hovers, if outside safe distance, then judge whether vertical distance is greater than suspicious distance threshold value, if vertical distance is less than suspicious distance threshold value, then unmanned aerial vehicle begins to slow down to less speed of setting for and continues the flight apart from barrier certain distance department, resume former operation speed to continue the operation after crossing suspicious barrier until unmanned aerial vehicle, if vertical distance is greater than suspicious distance threshold value, then unmanned aerial vehicle does not respond, continue the flight with former operation speed.
5. The unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar of claim 4, wherein the narrow beam millimeter wave radar deceleration strategy is: when the obstacle is detected in the beam range, calculating the distance between the unmanned aerial vehicle and the obstacle, starting deceleration at a set deceleration distance, and keeping constant-speed flight after the deceleration is reduced to a preset speed; when the wide-beam millimeter wave radar detects an obstacle and moves to a set range in a decelerating manner, the original obstacle information is immediately cleared and secondary confirmation is started, if the obstacle is not detected before timing is finished, the original operation speed is restored to continue operation, and if the obstacle is detected after the secondary confirmation, an obstacle avoidance route is automatically planned to start obstacle avoidance.
6. The obstacle avoidance method for unmanned aerial vehicles based on millimeter wave radar as claimed in claim 5, wherein the deceleration priority in step S6 is short-distance obstacle monitored by wide beam radar > long-distance obstacle monitored by narrow beam radar > long-distance obstacle monitored by wide beam radar > short-distance obstacle monitored by narrow beam.
7. The obstacle avoidance method for the unmanned aerial vehicle based on the millimeter wave radar as claimed in claim 6, wherein the obstacle avoidance line in the step S6 is generated according to a horizontal distance and a vertical distance from the obstacle to the unmanned aerial vehicle; and if the obstacle avoidance line exceeds the operation area in the direction of the preferential obstacle avoidance line, trying to generate an obstacle avoidance line in the other side direction.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810319791.2A CN108594849B (en) | 2018-04-10 | 2018-04-10 | Unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810319791.2A CN108594849B (en) | 2018-04-10 | 2018-04-10 | Unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108594849A CN108594849A (en) | 2018-09-28 |
| CN108594849B true CN108594849B (en) | 2022-05-13 |
Family
ID=63621549
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810319791.2A Active CN108594849B (en) | 2018-04-10 | 2018-04-10 | Unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108594849B (en) |
Families Citing this family (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108062109B (en) * | 2017-12-13 | 2020-09-11 | 天津萨瑞德科技有限公司 | Unmanned aerial vehicle obstacle avoidance method |
| CN109298720A (en) * | 2018-09-30 | 2019-02-01 | 鲁东大学 | A kind of plant protection drone flight course planning method |
| CN110291480A (en) * | 2018-10-30 | 2019-09-27 | 深圳市大疆创新科技有限公司 | A kind of unmanned plane test method, equipment and storage medium |
| CN110869872A (en) * | 2018-11-30 | 2020-03-06 | 深圳市大疆创新科技有限公司 | Method and device for accurately determining obstacle and computer-readable storage medium |
| CN109709554B (en) * | 2018-12-13 | 2021-01-19 | 广州极飞科技有限公司 | Work device, and control method and device thereof |
| CN109407703A (en) * | 2018-12-13 | 2019-03-01 | 广州极飞科技有限公司 | Unmanned plane and its control method and device |
| CN109633620B (en) * | 2018-12-13 | 2021-02-19 | 广州极飞科技有限公司 | Target object identification method and device, and working equipment |
| CN109633652A (en) * | 2019-01-14 | 2019-04-16 | 长沙莫之比智能科技有限公司 | Robot obstacle-avoiding system and its application method based on millimetre-wave radar |
| CN109799823B (en) * | 2019-01-29 | 2022-01-18 | 珠海云洲智能科技股份有限公司 | Ocean measurement method, electronic equipment and storage medium |
| CN109901614A (en) * | 2019-01-30 | 2019-06-18 | 沈阳无距科技有限公司 | Track line planing method, device, storage medium and electronic equipment |
| CN110907945A (en) * | 2019-10-28 | 2020-03-24 | 广西电网有限责任公司电力科学研究院 | Positioning method considering indoor and outdoor flight of unmanned aerial vehicle |
| WO2021087718A1 (en) * | 2019-11-05 | 2021-05-14 | 深圳市大疆创新科技有限公司 | Unmanned aerial vehicle control method and apparatus, device, and storage medium |
| WO2021087782A1 (en) * | 2019-11-05 | 2021-05-14 | 深圳市大疆创新科技有限公司 | Obstacle detection method and system, ground end device, and autonomous mobile platform |
| CN113093176B (en) * | 2019-12-23 | 2022-05-17 | 北京三快在线科技有限公司 | Linear obstacle detection method, linear obstacle detection device, electronic apparatus, and storage medium |
| CN111880532B (en) * | 2020-07-13 | 2022-03-18 | 珠海格力电器股份有限公司 | Autonomous mobile device, method, apparatus, device, and storage medium thereof |
| CN112415519A (en) * | 2020-11-03 | 2021-02-26 | 北京京东乾石科技有限公司 | Obstacle detection method, device and system for aircraft |
| CN112859059A (en) * | 2021-03-26 | 2021-05-28 | 江西商思伏沌科技有限公司 | Target detection and tracking system and method |
| CN113655805A (en) * | 2021-05-26 | 2021-11-16 | 陕西长岭电子科技有限责任公司 | Graph search driving mode based on extended spiral line |
| CN113534157A (en) * | 2021-06-07 | 2021-10-22 | 北方天途航空技术发展（北京）有限公司 | Multi-target radar detection method and system based on stack filtering |
| CN114162321A (en) * | 2021-11-24 | 2022-03-11 | 普宙科技(深圳)有限公司 | Unmanned aerial vehicle keeps away barrier structure |
| CN114527783B (en) * | 2021-12-17 | 2024-10-15 | 浙江华飞智能科技有限公司 | Unmanned aerial vehicle control method, device, equipment and computer storage medium |
| CN114648882B (en) * | 2022-02-09 | 2023-05-09 | 上海欧菲智能车联科技有限公司 | Parking space detection method and device |
| CN116076259A (en) * | 2023-03-27 | 2023-05-09 | 国网山东省电力公司费县供电公司 | Tree obstacle laser cleaning method and device for double-wire telegraph pole |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103224026A (en) * | 2012-12-05 | 2013-07-31 | 福建省电力有限公司 | Special-purpose unmanned helicopter obstacle-avoidance system for mountain-area electrical network routing inspection and work flow thereof |
| CN104503463A (en) * | 2014-12-25 | 2015-04-08 | 中国人民解放军总参谋部第六十研究所 | Active obstacle avoidance flight control method for unmanned helicopter |
| KR101668643B1 (en) * | 2015-06-23 | 2016-10-24 | 유콘시스템 주식회사 | Spherical unmanned aerial vehicle with apparatus of view angle |
| CN106527473A (en) * | 2016-10-27 | 2017-03-22 | 上海航天控制技术研究所 | Obstacle-avoidance landing method on lunar surface |
| CN107272741A (en) * | 2017-08-04 | 2017-10-20 | 北京理工大学 | A kind of unmanned plane automatic obstacle-avoiding method based on maximum inscribed circle center of circle mobile vector |
-
2018
- 2018-04-10 CN CN201810319791.2A patent/CN108594849B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103224026A (en) * | 2012-12-05 | 2013-07-31 | 福建省电力有限公司 | Special-purpose unmanned helicopter obstacle-avoidance system for mountain-area electrical network routing inspection and work flow thereof |
| CN104503463A (en) * | 2014-12-25 | 2015-04-08 | 中国人民解放军总参谋部第六十研究所 | Active obstacle avoidance flight control method for unmanned helicopter |
| KR101668643B1 (en) * | 2015-06-23 | 2016-10-24 | 유콘시스템 주식회사 | Spherical unmanned aerial vehicle with apparatus of view angle |
| CN106527473A (en) * | 2016-10-27 | 2017-03-22 | 上海航天控制技术研究所 | Obstacle-avoidance landing method on lunar surface |
| CN107272741A (en) * | 2017-08-04 | 2017-10-20 | 北京理工大学 | A kind of unmanned plane automatic obstacle-avoiding method based on maximum inscribed circle center of circle mobile vector |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108594849A (en) | 2018-09-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108594849B (en) | Unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar | |
| EP3119178B1 (en) | Method and system for navigating an agricultural vehicle on a land area | |
| CN110262518B (en) | Vehicle navigation method, system and medium based on track topological map and obstacle avoidance | |
| CN108007452B (en) | Method and device for updating environment map according to obstacle and robot | |
| CN106843223B (en) | Intelligent obstacle avoidance AGV trolley system and obstacle avoidance method | |
| EP2169503B1 (en) | Multi-vehicle high intensity perception | |
| EP2169498B1 (en) | Vehicle with high integrity perception system | |
| US8818567B2 (en) | High integrity perception for machine localization and safeguarding | |
| US8478493B2 (en) | High integrity perception program | |
| US20180032042A1 (en) | System And Method Of Dynamically Controlling Parameters For Processing Sensor Output Data | |
| US20190329764A1 (en) | Work vehicle | |
| CN109307869B (en) | Device and lighting arrangement for increasing the field of view of a lidar detector | |
| CN112799426A (en) | Unmanned aerial vehicle navigation control system and method based on big data analysis | |
| CN105549616A (en) | Multi-shaft unmanned aerial vehicle barrier avoiding system based on laser array, and barrier avoiding method thereof | |
| WO2019019147A1 (en) | Auto-exploration control of a robotic vehicle | |
| JP7195543B2 (en) | harvester | |
| US11987245B2 (en) | Method for controlling vehicle and vehicle control device | |
| EP3761136B1 (en) | Control device, mobile body, and program | |
| CN115933754A (en) | Electric power inspection unmanned aerial vehicle obstacle avoidance method based on millimeter wave radar and binocular vision | |
| CN109521780B (en) | Control system and control method for remote control work vehicle | |
| US20210026373A1 (en) | Vehicle platooning using surface-penetrating radar systems | |
| CN110645981B (en) | Unmanned ship navigation system and method for cleaning pile foundation type waterborne photovoltaic module | |
| WO2022202801A1 (en) | Autonomous travel system, work vehicle, and recording medium on which autonomous travel program is recorded | |
| US20220026555A1 (en) | Systems and methods for a radar system using sectional three-dimensional beamforming | |
| Contributors et al. | D3. 1-State of the Art of Automated Driving technologies |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20230310 Address after: 200433 room 1001-100, No. 323, Guoding Road, Yangpu District, Shanghai Patentee after: Yunban Technology Co.,Ltd. Address before: 518000 floor 3, building 14, Dayuan Industrial Zone, Xili Pingshan 1st Road, Nanshan District, Shenzhen, Guangdong Patentee before: SHENZHEN EFFICIEN TECH Co.,Ltd. |