CN114879729B - Unmanned aerial vehicle autonomous obstacle avoidance method based on obstacle contour detection algorithm - Google Patents

Abstract

The invention discloses an unmanned aerial vehicle autonomous obstacle avoidance method based on an obstacle contour detection algorithm. Then, thresholding and morphological processing are performed on the converted image. And (3) performing edge detection and contour detection on the processed image, combining the detected result with camera calibration data, and calculating the barycenter coordinates of the obstacle under the world coordinate system, so as to obtain the position information and contour information of the obstacle. And finally, transmitting the information of the obstacle into a D obstacle avoidance algorithm to carry out real-time path solving until the autonomous obstacle avoidance function of the unmanned aerial vehicle is completed. The method disclosed by the invention is high in instantaneity and high in calculation efficiency, and can be popularized to the autonomous obstacle avoidance of the unmanned aerial vehicle under a dynamic obstacle and the autonomous obstacle avoidance of the unmanned aerial vehicle under a real three-dimensional scene based on the method.

Description

An autonomous obstacle avoidance method for UAV based on obstacle contour detection algorithm

Technical Field

The invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to an autonomous obstacle avoidance method for unmanned aerial vehicles.

Background technique

With the rapid development of the drone industry, drone safety has also received widespread attention, especially in unknown environments, where autonomous obstacle avoidance is particularly important. Due to the lack of prior information about unknown environments, drones need to sense and avoid obstacles. Among them, obstacle detection is an important part. The current obstacle detection methods mainly include: ultrasonic-based detection methods, infrared-based detection methods, laser-based detection methods, and machine vision-based detection methods. Among them, the machine vision-based detection method uses a camera to obtain images and processes them through image processing algorithms to obtain information such as the outline, position, and depth of the obstacle. Unlike the first three methods, the machine vision-based detection method obtains richer information.

There are also significant differences in the application of obstacle detection algorithms based on machine vision. Among them, the inter-frame difference method is only applicable to the detection of dynamic obstacles and cannot meet the requirements of real-time detection; the optical flow estimation method requires the prediction of the detection area in advance and cannot detect the complete obstacle; the traditional obstacle contour detection algorithm is suitable for feature obstacle detection, but it has the disadvantages of being greatly affected by noise and poor adaptability. Therefore, designing an obstacle detection algorithm with good adaptability, small computational complexity, and that can meet real-time requirements, and can complete the autonomous obstacle avoidance function of the drone is a technical problem that researchers in this field need to solve.

Summary of the invention

In order to overcome the shortcomings of the prior art, the present invention provides an autonomous obstacle avoidance method for a drone based on an obstacle contour detection algorithm. First, the acquired image is subjected to filtering processing and color space conversion. Then, the converted image is subjected to threshold processing and morphological processing. Edge detection and contour detection are performed on the processed image, and the detection results are combined with the camera calibration data to calculate the center of mass coordinates of the obstacle in the world coordinate system, thereby obtaining the position information and contour information of the obstacle. Finally, the obstacle information is passed into the D* obstacle avoidance algorithm for real-time path solving until the autonomous obstacle avoidance function of the drone is completed. The method of the present invention has high real-time performance and high computational efficiency. Based on this method, it can be extended to autonomous obstacle avoidance of drones under dynamic obstacles and autonomous obstacle avoidance of drones in real three-dimensional scenes.

The technical solution adopted by the present invention to solve the technical problem includes the following steps:

Step 1: Build a drone model;

Step 1-1: The layout structure of the drone model is "X" type, that is, the angle between the forward direction and the adjacent bracket is 45°; assume that the drone model is a rigid body, and the brake of the drone generates force F and torque τ. Assume that the force and torque of the drone at the i-th moment are _Fi and _τi respectively, and the calculation formula is as follows:

Where C _T and C _pow represent the thrust coefficient and power coefficient based on the rotor, respectively, ρ represents the air density, D represents the rotor diameter, ω _max represents the maximum rotation angular velocity, and u _i represents the speed of the motor at the i-th moment;

Step 1-2: Calculate the next motion state of the drone;

Assume that the speed of the drone at the m-1th moment is v _m-1 , the position is p _m-1 , the acceleration is a _m-1 , and the time step is dt. The position p _m and speed v _m at the mth moment are calculated as follows:

Where, p _m is the position of the UAV at the mth moment, and v _m is the speed of the UAV at the mth moment;

Step 2: Use the world coordinate system to determine the starting point and target point of the drone;

Step 3: Determine the UAV path search algorithm;

Select the D* path search algorithm and set its heuristic function to:

f(s)＝h(s)+g(s) (5)

Among them, h(s) represents the cost from the current node to the target point, and g(s) represents the cost from the current node to the starting point;

Assuming that the position coordinates of the current node are (x _s , y _s ), the coordinates of the starting point are (x _start , y _start ), and the coordinates of the target point are ((x _goal , y _goal ), then h(s) and g(s) are expressed as:

Generate a path from the current node to the target node through the D* path search algorithm;

Step 4: Establish an obstacle contour detection algorithm based on color information;

Step 4-1: Obtain environmental images through the drone’s onboard camera;

Step 4-2: The acquired environment image is smoothed by combining Gaussian filtering and median filtering. The calculation formula of Gaussian filtering is:

Among them, G(.) is a two-dimensional Gaussian function, (Δx ² +Δy ² ) represents the sum of the squares of the distances between other pixels in the neighborhood and the central pixel, σ is the standard deviation of the two-dimensional normal distribution, and (Δx, Δy) represents the domain;

Use median filtering to further filter the smoothed filtered image;

Step 4-3: Convert the environment image in RGB space to HSV color space. The calculation formula is as follows:

V＝max(R,G,B) (11)

Among them, R, G, and B represent the values of the three color components in the RGB space, and H, S, and V represent the hue, saturation, and brightness values in the HSV space, respectively;

Step 4-4: Binarize the image and use the Otus algorithm to perform threshold processing on the image. The specific calculation process is as follows:

Step 4-4-1: Calculate the zero-order cumulative moment of the grayscale histogram:

Wherein, histogram _I represents the normalized image grayscale histogram, and histogram _I (k) represents the ratio of pixels with grayscale value equal to k in the image;

Step 4-4-2: Calculate the first-order cumulative moment of the grayscale histogram:

Step 4-4-3: Calculate the grayscale average of the image as a whole:

mean＝oneCuMo(255) (14)

Step 4-4-4: Divide the image into foreground image and background image according to the grayscale characteristics, and calculate the threshold q that can maximize the variance between the foreground image and the background image; the following metric is used to measure the variance:

Step 4-5: Perform morphological processing on the image by first dilating and then corroding;

Step 4-6: Use Canny operator for edge detection and contour detection:

Step 4-6-1: Smoothing the noise in the non-edge area of the image;

Step 4-6-2: Use the Sobel operator to calculate the magnitude and direction of the image gradient;

Step 4-6-3: Go through the pixels one by one to determine whether the current pixel is a point with a maximum gradient value in the gradient direction. If it is a maximum value point, keep the point, otherwise return it to zero;

Step 4-6-4: Use double thresholds to perform threshold processing and obtain edge points;

Step 4-6-5: Fit the edge detection result with the foreground information of the image to approximately obtain the image contour;

Step 4-7: Use Zhang Zhengyou's camera calibration method to determine the intrinsic parameter matrix _M1 and extrinsic parameter matrix _M2 of the airborne camera;

Step 4-8: Solve the coordinates of the center of mass of the obstacle in the world coordinate system;

Step 4-8-1: The calculation formula of the (i+j)-order image moment of the environment image is:

Where x and y represent the horizontal and vertical coordinates of the pixel point, and I(x,y) represents the pixel intensity corresponding to the pixel point with coordinates (x,y);

Step 4-8-2: Calculate the centroid coordinates in the pixel coordinate system using the zero-order image moment M ₀₀ and the first-order image moment (M ₀₁ , M ₁₀ ):

Step 4-8-3: Perform coordinate conversion and convert the centroid coordinates to the world coordinate system:

Where u and v are the coordinates in the pixel coordinate system, ( _XC , _YC , _ZC ) are the coordinates in the camera coordinate system, _fx and _fy represent the physical sizes of the pixels on the x-axis and y-axis respectively, _u0 and _v0 represent the pixel differences between the center pixel coordinates of the image and the pixel coordinates of the image origin in the x-direction and y-direction respectively, and f is the focal length of the camera; R is a 3×3 rotation matrix, that is, the matrix obtained by rotating the coordinate axes when converting from the pixel coordinate system to the world coordinate system; t is the offset vector, and ( _XW , _YW , _ZW ) are the coordinates in the world coordinate system;

Step 5: The drone starts to move from the starting point along the initial path generated in step 3. At the same time, the onboard camera uses the obstacle contour detection algorithm in step 4 for real-time detection. If an unknown obstacle appears on the path, it is determined whether it affects the flight of the drone based on the drone's position information and contour information. If it does, the path search algorithm in step 3 is used to generate a new autonomous obstacle avoidance path from the current point to the target point.

This process is repeated until the drone reaches the target point, and the entire autonomous obstacle avoidance process is completed.

The beneficial effects of the present invention are as follows:

In the present invention, the UAV can use the obstacle detection algorithm to detect and obtain the position and contour of unknown obstacles in real time without the need for the environment to be completely known. By combining with the D* pathfinding algorithm, the goal of autonomous obstacle avoidance of the UAV in an unknown environment can be achieved. The method has high real-time performance and high computational efficiency. Based on this method, it can be extended to autonomous obstacle avoidance of UAVs under dynamic obstacles and autonomous obstacle avoidance of UAVs in real three-dimensional scenes.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a general flow chart of the autonomous obstacle avoidance method for unmanned aerial vehicles based on the obstacle contour detection algorithm of the present invention.

FIG. 2 is a diagram of a UAV simulation model used in AirSim according to an embodiment of the present invention.

FIG. 3 is a top view of a simulation environment constructed in AirSim according to an embodiment of the present invention.

FIG. 4 is a diagram showing the effect of an obstacle contour detection algorithm at a certain moment in an embodiment of the present invention.

FIG. 5 is a diagram showing the path results of the autonomous obstacle avoidance of the UAV according to an embodiment of the present invention.

Detailed ways

The present invention is further described below in conjunction with the accompanying drawings and embodiments.

The present invention provides an autonomous obstacle avoidance method for a UAV based on an improved obstacle contour detection algorithm. First, the acquired image is subjected to filtering processing and color space conversion. Then, the converted image is subjected to threshold processing and morphological processing. Edge detection and contour detection are performed on the processed image, and the detection results are combined with the camera calibration data to calculate the center of mass coordinates of the obstacle in the world coordinate system, thereby obtaining the position information and contour information of the obstacle. Finally, the obstacle information is passed into the D* obstacle avoidance algorithm for real-time path solving until the autonomous obstacle avoidance function of the UAV is completed.

The simulation environment is: Windows 10 operating system, Airsim simulation platform.

The present invention considers a three-dimensional map model, and the coordinate system is a plane coordinate system. Assume that there is a drone with a built-in visual camera, as shown in Figure 2. The size of the constructed simulation map is 100m×100m. Gray obstacles represent obstacles with known positions and contours, and orange and white obstacles represent unknown obstacles. The environment is constructed as shown in Figure 3. Assume that the position coordinates of the drone are (70, 20) and the coordinates of the target point are (70, 80).

As shown in FIG1 , the specific steps of the present invention in the AirSim environment are as follows:

Step 1: Build a drone model;

Step 1-1: The layout structure of the drone model is "X" type, that is, the angle between the forward direction and the adjacent bracket is 45°; assume that the drone model is a rigid body, and the drone can generate force F and torque τ by any number of brakes. Let the force and torque of the drone at the i-th moment be _Fi and _τi respectively, and the calculation formula is as follows:

Where C _T and C _pow represent the thrust coefficient and power coefficient based on the rotor, respectively, ρ represents the air density, D represents the rotor diameter, ω _max represents the maximum rotation angular velocity, and u _i represents the speed of the motor at the i-th moment;

Step 1-2: Calculate the next motion state of the drone;

Assume that the speed of the drone at the m-1th moment is v _m-1 , the position is p _m-1 , the acceleration is a _m-1 , and the time step is dt. The position p _m and speed v _m at the mth moment are calculated as follows:

Where, p _m is the position of the UAV at the mth moment, and v _m is the speed of the UAV at the mth moment;

Step 2: Use the world coordinate system to determine the starting point and target point of the drone;

Step 3: Determine the UAV path search algorithm;

Select the D* path search algorithm and set its heuristic function to:

f(s)＝h(s)+g(s) (5)

Among them, h(s) represents the cost from the current node to the target point, and g(s) represents the cost from the current node to the starting point;

Assuming that the position coordinates of the current node are (x _s , y _s ), the coordinates of the starting point are (x _start , y _start ), and the coordinates of the target point are ((x _goal , y _goal ), then h(s) and g(s) are expressed as:

Generate a path from the current node to the target node through the D* path search algorithm;

Step 4: Establish an obstacle contour detection algorithm based on color information;

Step 4-1: Obtain environmental images through the drone’s onboard camera;

Step 4-2: The acquired environment image is smoothed by combining Gaussian filtering and median filtering. The calculation formula of Gaussian filtering is:

Among them, G(.) is a two-dimensional Gaussian function, (Δx ² +Δy ² ) represents the sum of the squares of the distances between other pixels in the neighborhood and the central pixel, σ is the standard deviation of the two-dimensional normal distribution, and (Δx, Δy) represents the domain;

The median filter is used to further filter the smoothed image, with the aim of eliminating noise while retaining the image contour information to the maximum extent;

Step 4-3: Convert the environment image in RGB space to HSV color space. The calculation formula is as follows:

V＝max(R,G,B) (11)

Among them, R, G, and B represent the values of the three color components in the RGB space, and H, S, and V represent the hue, saturation, and brightness values in the HSV space, respectively;

Step 4-4: Binarize the image and use the Otus algorithm to perform threshold processing on the image. The specific calculation process is as follows:

Step 4-4-1: Calculate the zero-order cumulative moment of the grayscale histogram:

Wherein, histogram _I represents the normalized image grayscale histogram, and histogram _I (k) represents the ratio of pixels with grayscale value equal to k in the image;

Step 4-4-2: Calculate the first-order cumulative moment of the grayscale histogram:

Step 4-4-3: Calculate the grayscale average of the image as a whole:

mean＝oneCuMo(255) (14)

Step 4-4-4: Divide the image into foreground image and background image according to the grayscale characteristics, and calculate the threshold q that can maximize the variance between the foreground image and the background image; the following metric is used to measure the variance:

Step 4-5: Perform morphological processing on the image by first dilating and then corroding;

Step 4-6: Use Canny operator for edge detection and contour detection:

Step 4-6-1: Smoothing the noise in the non-edge area of the image;

Step 4-6-2: Use the Sobel operator to calculate the magnitude and direction of the image gradient;

Step 4-6-3: Go through the pixels one by one to determine whether the current pixel is a point with a maximum gradient value in the gradient direction. If it is a maximum value point, keep the point, otherwise return it to zero;

Step 4-6-4: Use double thresholds to perform threshold processing and obtain edge points;

Step 4-6-5: Fit the edge detection result with the foreground information of the image to approximately obtain the image contour;

Step 4-7: Use Zhang Zhengyou's camera calibration method to determine the intrinsic parameter matrix _M1 and extrinsic parameter matrix _M2 of the airborne camera;

Step 4-8: Solve the coordinates of the center of mass of the obstacle in the world coordinate system;

Step 4-8-1: The calculation formula of the (i+j)-order image moment of the environment image is:

Where x and y represent the horizontal and vertical coordinates of the pixel point, and I(x,y) represents the pixel intensity corresponding to the pixel point with coordinates (x,y);

Step 4-8-2: Calculate the centroid coordinates in the pixel coordinate system using the zero-order image moment M ₀₀ and the first-order image moment (M ₀₁ , M ₁₀ ):

Step 4-8-3: Perform coordinate conversion and convert the centroid coordinates to the world coordinate system:

Wherein, u and v are the coordinates in the pixel coordinate system, ( _XC , _YC , _ZC ) are the coordinates in the camera coordinate system, _fx and _fy represent the physical sizes of the pixels on the x-axis and y-axis respectively, _u0 and _v0 represent the pixel differences between the center pixel coordinates of the image and the pixel coordinates of the image origin in the x-direction and y-direction respectively, and f is the focal length of the camera; R is a 3×3 rotation matrix, that is, the matrix obtained by rotating the coordinate axis when converting from the pixel coordinate system to the world coordinate system; t is the offset vector, ( _XW , _YW , _ZW ) are the coordinates in the world coordinate system, matrix _M1 is the intrinsic parameter matrix of the camera, and matrix _M2 is the extrinsic parameter matrix of the camera, which can be measured by camera calibration;

The drone can obtain the location and contour information of unknown obstacles;

Step 5: The drone starts to move from the starting point along the initial path generated in step 3. At the same time, the onboard camera uses the obstacle contour detection algorithm in step 4 for real-time detection. If an unknown obstacle appears on the path, it is determined whether it affects the flight of the drone based on the drone's position information and contour information. If it does, the path search algorithm in step 3 is used to generate a new autonomous obstacle avoidance path from the current point to the target point.

This process is repeated until the drone reaches the target point, and the entire autonomous obstacle avoidance process is completed.

In summary, the present invention uses the obstacle contour detection algorithm to determine the contour information and position information of the unknown obstacle. FIG4 is a contour result diagram at a certain moment in the obstacle detection process, which can quickly and accurately provide the required unknown obstacle information for the autonomous obstacle avoidance algorithm of the drone. When the drone encounters an unknown obstacle in the process of searching and detecting, it can also generate a path from the current unknown node to the target node. As shown in FIG5, it is a feasible path generated by the drone through the autonomous obstacle avoidance algorithm, which verifies the real-time and feasibility of the algorithm. For autonomous obstacle avoidance of drones, this method is relatively simple and has high real-time robustness, and realizes autonomous obstacle avoidance of drones.

Claims (1)

1. A method for autonomous obstacle avoidance of a UAV based on an obstacle contour detection algorithm, characterized in that it comprises the following steps: Step 1: Build a drone model; Step 1-1: The layout structure of the drone model is "X" type, that is, the angle between the forward direction and the adjacent bracket is 45°; assume that the drone model is a rigid body, and the brake of the drone generates force F and torque τ. Let the force and torque of the drone at the i-th moment be _Fi and _τi respectively, and the calculation formula is as follows: Where C _T and C _pow represent the thrust coefficient and power coefficient based on the rotor, respectively, ρ represents the air density, D represents the rotor diameter, ω _max represents the maximum rotation angular velocity, and u _i represents the speed of the motor at the i-th moment; Step 1-2: Calculate the next motion state of the drone; Assume that the speed of the drone at the m-1th moment is v _m-1 , the position is p _m-1 , the acceleration is a _m-1 , and the time step is dt. The position p _m and speed v _m at the mth moment are calculated as follows: Where, p _m is the position of the UAV at the mth moment, and v _m is the speed of the UAV at the mth moment; Step 2: Use the world coordinate system to determine the starting point and target point of the drone; Step 3: Determine the UAV path search algorithm; Select the D* path search algorithm and set its heuristic function to: f(s)＝h(s)+g(s) (5) Among them, h(s) represents the cost from the current node to the target point, and g(s) represents the cost from the current node to the starting point; Assuming that the position coordinates of the current node are (x _s , y _s ), the coordinates of the starting point are (x _start , y _start ), and the coordinates of the target point are ((x _goal , y _goal ), then h(s) and g(s) are expressed as: Generate a path from the current node to the target node through the D* path search algorithm; Step 4: Establish an obstacle contour detection algorithm based on color information; Step 4-1: Obtain environmental images through the drone’s onboard camera; Step 4-2: The acquired environment image is smoothed by combining Gaussian filtering and median filtering. The calculation formula of Gaussian filtering is: Among them, G(.) is a two-dimensional Gaussian function, (Δx ² +Δy ² ) represents the sum of the squares of the distances between other pixels in the neighborhood and the central pixel, σ is the standard deviation of the two-dimensional normal distribution, and (Δx, Δy) represents the domain; Use median filtering to further filter the smoothed filtered image; Step 4-3: Convert the environment image in RGB space to HSV color space. The calculation formula is as follows: V＝max(R,G,B) (11) Among them, R, G, and B represent the values of the three color components in the RGB space, and H, S, and V represent the hue, saturation, and brightness values in the HSV space, respectively; Step 4-4: Binarize the image and use the Otus algorithm to perform threshold processing on the image. The specific calculation process is as follows: Step 4-4-1: Calculate the zero-order cumulative moment of the grayscale histogram: Wherein, histogram _I represents the normalized image grayscale histogram, and histogram _I (k) represents the ratio of pixels with grayscale value equal to k in the image; Step 4-4-2: Calculate the first-order cumulative moment of the grayscale histogram: Step 4-4-3: Calculate the grayscale average of the image as a whole: mean＝oneCuMo(255) (14) Step 4-4-4: Divide the image into foreground image and background image according to the grayscale characteristics, and calculate the threshold q that can maximize the variance between the foreground image and the background image; the following metric is used to measure the variance: Step 4-5: Perform morphological processing on the image by first dilating and then corroding; Step 4-6: Use Canny operator for edge detection and contour detection: Step 4-6-1: Smoothing the noise in the non-edge area of the image; Step 4-6-2: Use the Sobel operator to calculate the magnitude and direction of the image gradient; Step 4-6-3: Go through the pixels one by one to determine whether the current pixel is a point with a maximum gradient value in the gradient direction. If it is a maximum value point, keep the point, otherwise return it to zero; Step 4-6-4: Use double thresholds to perform threshold processing and obtain edge points; Step 4-6-5: Fit the edge detection result with the foreground information of the image to approximately obtain the image contour; Step 4-7: Use Zhang Zhengyou's camera calibration method to determine the intrinsic parameter matrix _M1 and extrinsic parameter matrix _M2 of the airborne camera; Step 4-8: Solve the coordinates of the center of mass of the obstacle in the world coordinate system; Step 4-8-1: The calculation formula of the (i+j)-order image moment of the environment image is: Where x and y represent the horizontal and vertical coordinates of the pixel point, and I(x,y) represents the pixel intensity corresponding to the pixel point with coordinates (x,y); Step 4-8-2: Calculate the centroid coordinates in the pixel coordinate system using the zero-order image moment M ₀₀ and the first-order image moment (M ₀₁ , M ₁₀ ): Step 4-8-3: Perform coordinate conversion and convert the centroid coordinates to the world coordinate system: Where u and v are the coordinates in the pixel coordinate system, ( _XC , _YC , _ZC ) are the coordinates in the camera coordinate system, _fx and _fy represent the physical sizes of the pixels on the x-axis and y-axis respectively, _u0 and _v0 represent the pixel differences between the center pixel coordinates of the image and the pixel coordinates of the image origin in the x-direction and y-direction respectively, and f is the focal length of the camera; R is a 3×3 rotation matrix, that is, the matrix obtained by rotating the coordinate axes when converting from the pixel coordinate system to the world coordinate system; t is the offset vector, and ( _XW , _YW , _ZW ) are the coordinates in the world coordinate system; Step 5: The drone starts to move from the starting point along the initial path generated in step 3. At the same time, the onboard camera uses the obstacle contour detection algorithm in step 4 for real-time detection. If an unknown obstacle appears on the path, it is determined whether it affects the flight of the drone based on the drone's position information and contour information. If it does, the path search algorithm in step 3 is used to generate a new autonomous obstacle avoidance path from the current point to the target point. This process is repeated until the drone reaches the target point, and the entire autonomous obstacle avoidance process is completed.