Unmanned aerial vehicle perception obstacle avoidance method, system, device and medium

Abstract

The invention discloses an unmanned aerial vehicle perception obstacle avoidance method, system and device, and a medium, and relates to the technical field of unmanned aerial vehicle autonomous perception and obstacle avoidance, and the method comprises the steps: obtaining obstacle information in a three-dimensional space around an unmanned aerial vehicle; using an improved three-dimensional vector field histogram algorithm to predict the motion trail of a dynamic obstacle outside the field of view; the improved three-dimensional vector field histogram algorithm comprises a memory strategy and trajectory prediction; the memory strategy is used for combining a new image and an old image acquired by the unmanned aerial vehicle to obtain combined information; the track prediction is used for sensing and predicting obstacles outside the visual field of the unmanned aerial vehicle; combining the prediction information within a certain time with the combination information of the previous time step through a memory strategy, and predicting the motion trail of the dynamic obstacle outside the visual field; according to a prediction result, the unmanned aerial vehicle performs obstacle avoidance on dynamic obstacles outside the visual field; therefore, quick and accurate obstacle avoidance decision is realized, and the safety and reliability of the unmanned aerial vehicle are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous perception and obstacle avoidance for unmanned aerial vehicles, and particularly to a method, system, device and medium for unmanned aerial vehicle perception and obstacle avoidance. Background Art

[0002] In the current era background, the rapid development of unmanned aerial vehicle technology is constantly expanding its applications in key fields such as military reconnaissance, civilian surveillance, and logistics transportation. In particular, the autonomous flight and efficient obstacle avoidance capabilities of unmanned aerial vehicles have received high attention in the industry in complex environments. Facing challenges such as high algorithm complexity, demanding hardware requirements, and insufficient recognition and processing of dynamic obstacles outside the field of view, there is an urgent need to explore and develop innovative technologies and methods to break through the existing limitations, thereby promoting the further development of unmanned aerial vehicle technology and the expansion of its application scope.

[0003] In an unmanned aerial vehicle perception and obstacle avoidance system, a three-dimensional histogram can be used for environmental modeling and path planning. This method uses spatial segmentation technology to divide the environment around the unmanned aerial vehicle into small cubes (voxels). Each voxel corresponds to a unit of the three-dimensional histogram, and the environmental data collected by the visual sensor is used to fill these voxels. When an obstacle is detected in a voxel, it is marked as an occupied state. By combining the states of all voxels, a complete three-dimensional histogram is formed, representing the obstacle situation of the environment around the unmanned aerial vehicle. The A* algorithm is used to calculate the safe flight path of the unmanned aerial vehicle based on the voxels marked as free in the three-dimensional histogram. As the unmanned aerial vehicle continues to fly, the three-dimensional histogram of the environment is updated in real time, and a safe flight path is continuously planned. According to the planned path, the unmanned aerial vehicle control system navigates the flight according to the path to ensure avoiding obstacles. This navigation framework enables the unmanned aerial vehicle to effectively avoid static obstacles in the environment.

[0004] However, the three-dimensional histogram of moving obstacles in the complex wild environment changes over time, and it is difficult to establish the histogram of obstacles outside the field of view, resulting in difficulty in predicting the movement trajectory of obstacles, and thus it is impossible to avoid dynamic obstacles and obstacles outside the field of view. Summary of the Invention

[0005] Aiming at the deficiency of the prior art in that it is difficult to predict the movement trajectory of dynamic obstacles or obstacles outside the field of view, the present invention proposes a method, system, device and medium for unmanned aerial vehicle perception and obstacle avoidance. Through an improved obstacle avoidance algorithm, it realizes the effective perception and prediction of dynamic obstacles outside the field of view, as well as precise obstacle avoidance control, thus solving the problems existing in the prior art.

[0006] A method for unmanned aerial vehicle perception and obstacle avoidance includes the following steps:

[0007] Obtain obstacle information in the three-dimensional space around the drone and generate original point cloud data;

[0008] Perform object detection on the obstacles within the field of view of the drone, track the detected target obstacles, and obtain the pixel coordinates of the target obstacles; convert the pixel coordinates of the target obstacles into corresponding three-dimensional world coordinates; according to the three-dimensional world coordinates corresponding to the target obstacles, use the least squares method to fit the three-dimensional point cloud trajectory of the target obstacles; through a memory strategy, merge the three-dimensional point cloud trajectory of the target obstacles within a set time with the original point cloud data to predict the three-dimensional point cloud trajectory data of the target obstacles' movement in the next time step; convert the predicted three-dimensional point cloud trajectory data into a vector field to generate a three-dimensional histogram;

[0009] Based on the three-dimensional histogram, the drone selects the flight path direction with the fewest obstacles or no obstacles as the obstacle avoidance path of the drone.

[0010] Furthermore, based on the three-dimensional histogram, the drone selects the flight path direction with the fewest obstacles or no obstacles at all as the obstacle avoidance path, including the following steps:

[0011] Based on the three-dimensional histogram, obtain the obstacle density in the local area of the three-dimensional space;

[0012] Evaluate the obstacle situation in each flight direction of the drone according to the obstacle density, and then select the direction with the fewest obstacles or no obstacles at all as the candidate direction;

[0013] Use the selected candidate direction to generate candidate directions again at a certain distance to obtain a path search tree;

[0014] Use the A* search algorithm to evaluate and plan the path search tree to obtain the optimal obstacle avoidance path.

[0015] Furthermore, the specific steps of using the A* search algorithm to evaluate and plan the path search tree are as follows:

[0016] Use the A* search algorithm to evaluate the path search tree, which is expressed as:

[0017] f(n) = g(n) + h(n);

[0018] Among them, f(n) is the total cost of the current node n; g(n) is the cost from the current node n to the target point; h(n) is the cost from the starting point to the current node n;

[0019] The cost g(n) from the current node n to the target point includes the target cost, yaw cost, path smoothness cost, and smoothness cost, which is expressed as:

[0020]

[0021] Among them, n is the current node; g(n) is the total cost of the node; is a discount factor less than 1; k target is the target weight parameter; g target is the target cost; k yaw is the yaw weight parameter; g yaw is the yaw cost; k path is the path smoothing weight parameter; g path is the path smoothing cost; k tree is the tree smoothing weight parameter; g tree is the tree smoothing cost.

[0022] Furthermore, the cost function is used to select an obstacle-free area or the least obstacle area on the three-dimensional histogram as the candidate direction, where the bright area in the three-dimensional histogram represents the obstacle area, and the dark area represents the obstacle-free area or the least obstacle area; the total cost c total of the cost function includes the target cost c goal and the smoothing cost c smooth and is expressed as:

[0023] c goal =Δ yaw (g, p)+k up ·Δ pitch_up (g, p)+k down ·Δ pitch_down (g, p);

[0024] c smooth =Δ yaw (p old , p)+Δ pitch (p old , p);

[0025] c total =k goal ·c goal +k smooth ·c smooth ;

[0026] Among them, g is the target position direction; p is the candidate direction projected onto the three dimensions at the current time step; p old is the candidate direction projected onto the three dimensions at the previous time step; k goal , k smooth are the weight factors corresponding to different costs; c is the corresponding cost; Δ is the angle difference, Δ yaw is the yaw angle difference, Δ pitch_up Δ pitch_down is the pitch angle difference, k up , k down are the corresponding weight factors.

[0027] Further, according to the binocular stereo vision principle, the pixel coordinates (u, v) of the target obstacle are converted into the corresponding three-dimensional world coordinates (x w , y w , z w ), and the coordinate transformation formula between its world coordinate system and pixel coordinate system is:

[0028]

[0029] where z c is the depth value of the three-dimensional coordinate point in the camera coordinate system, that is, the distance from the camera to this point;

[0030] represents the conversion relationship from the image coordinate system to the pixel coordinate system, [u 0 , v 0 represents the origin of the image coordinate system, represents the conversion relationship from millimeters to the unit of the pixel coordinate system, represents the conversion relationship from the camera coordinate system to the image coordinate system obtained according to perspective projection, and f represents the camera focal length; represents the external camera parameter matrix, represents the rotation matrix for rotating from the world coordinate system to the camera coordinate system; [t x , t y , t z represents the translation vector, describing the position of the camera in the world coordinate system;

[0031] For a pair of pixel coordinates (u 1 , v 1 ), (u 2 , v 2 ) of the center points of the obstacles in the left and right images of the binocular camera in the pixel coordinate system and the world coordinates (x w , y w , z w ) in the world coordinate system are expressed as:

[0032]

[0033] where p is the element in the projection matrix P from the world coordinates to the pixel coordinate system;

[0034] It is derived that:

[0035]

[0036] The world coordinates (x w , y w , z w ) corresponding to the two pixel coordinates are obtained by solving.

[0037] Furthermore, a binocular vision sensor is used to obtain obstacle information in the three-dimensional space around the UAV.

[0038] The present invention further includes a UAV perception and obstacle avoidance system, comprising:

[0039] An acquisition module, configured to acquire obstacle information in the three-dimensional space around the UAV and generate original point cloud data;

[0040] A three-dimensional histogram generation unit, configured to perform target detection on obstacles within the field of view of the UAV, perform target tracking on the detected target obstacles to obtain the pixel coordinates of the target obstacles; convert the pixel coordinates of the target obstacles into corresponding three-dimensional world coordinates; according to the three-dimensional world coordinates corresponding to the target obstacles, use the least squares method to fit the three-dimensional point cloud trajectory of the target obstacles; merge the three-dimensional point cloud trajectory of the target obstacles within a set time with the original point cloud data through a memory strategy to predict the three-dimensional point cloud trajectory data of the target obstacles in the next time step; convert the predicted three-dimensional point cloud trajectory data into a vector field to generate a three-dimensional histogram;

[0041] An obstacle avoidance path selection unit, configured to, based on the three-dimensional histogram, the UAV selects the flight path direction with the fewest obstacles or no obstacles as the obstacle avoidance path of the UAV.

[0042] The present invention further includes a UAV perception and obstacle avoidance computer device, comprising: a memory, a processor, and a computer program stored in the memory, and when the processor executes the computer program, the steps of the UAV perception and obstacle avoidance method are implemented.

[0043] The present invention further includes a readable storage medium, the readable storage medium stores a computer program, the computer program includes program instructions, and when the program instructions are executed by a processor, the steps of the UAV perception and obstacle avoidance method are executed.

[0044] The present invention provides a UAV perception and obstacle avoidance method, system, device and medium, having the following beneficial effects:

[0045] The present invention performs target detection on obstacles in a three-dimensional space, obtains a pair of pixel coordinates of the detected target obstacle after target tracking, converts the pixel coordinates into corresponding three-dimensional world coordinates, enhances the spatial perception and decision-making ability of the unmanned aerial vehicle (UAV) in a complex environment by providing three-dimensional space information, fits the three-dimensional point cloud trajectory of the target obstacle using the least squares method according to the corresponding three-dimensional world coordinates of the target obstacle, enables the UAV to more accurately identify the trajectories of the obstacles and their relative speeds and distances from the UAV, and then obtains the obstacle avoidance path of the UAV based on the generated three-dimensional histogram, thereby realizing fast and accurate obstacle avoidance decision-making. This method not only improves the safety and reliability of the UAV but also opens up new possibilities for the application of the UAV in unknown and dynamic environments. Description of the Drawings

[0046] Figure 1 It is the overall flow block diagram of the three-dimensional vector field histogram algorithm in the embodiment of the present invention;

[0047] Figure 2 It is the overall flow chart of omnidirectional autonomous perception and obstacle avoidance of the UAV based on binocular vision in the embodiment of the present invention;

[0048] Figure 3 It is the principle flow chart of the memory strategy in the embodiment of the present invention;

[0049] Figure 4 It is the improved principle flow chart of the memory strategy in the embodiment of the present invention;

[0050] Figure 5 It is the histogram and cost map in the embodiment of the present invention;

[0051] Figure 6 It is the omnidirectional autonomous perception flow chart in the embodiment of the present invention;

[0052] Figure 7 It is the relationship diagram of four different coordinate systems in the embodiment of the present invention. Detailed Embodiment

[0053] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments.

[0054] The present invention proposes a method for a drone to sense and avoid obstacles; by using a binocular vision sensor to obtain three-dimensional spatial information, precise perception of the surrounding environment is achieved; through an improved three-dimensional vector field histogram algorithm, the problems of detection and avoidance of traditional obstacle avoidance algorithms when facing dynamic or out-of-field-of-view obstacles are solved; combined with deep learning technology, moving objects within the field of view are detected, tracked, and their motion trajectories are predicted to achieve dynamic obstacle avoidance; by comprehensively considering the perception information and motion prediction results, a fast and accurate obstacle avoidance decision is achieved through an omnidirectional obstacle avoidance algorithm; as Figure 7 shown, the specific steps are as follows:

[0055] S1. First, obtain depth data. According to the principle of similar triangles in the ideal two-dimensional mathematical model of binocular stereo vision, we have:

[0056]

[0057] Extended to the three-dimensional case, if the Y-axis is perpendicular to the paper and points outwards, then we have:

[0058]

[0059] After arrangement, the depth formula is obtained as follows:

[0060]

[0061] Among them, b is the baseline length; f is the focal length; (x l , y l ) are the pixel coordinates of the midpoint of the left camera; (x r , y r ) are the pixel coordinates of the corresponding point in the right camera; d is the disparity; (x, y, z) are the world coordinates of the point, which are unknowns.

[0062] S2. Sense the research object: The perception of specific dynamic obstacles is achieved through deep learning. Select pedestrians moving in a uniform straight line as the specific research object and specific research conditions, and use the object detection algorithm based on YOLOv3 for object detection and the object tracking algorithm based on DeepSORT for object tracking, aiming to sense and extract the research object.

[0063] S3. Transform the object coordinates: The coordinate transformation formula between the world coordinate system and the pixel coordinate system is as follows:

[0064]

[0065] For a pair of pixel coordinates (u 1 , v 1 ) of the center point of the object in the left and right images of the binocular camera in the pixel coordinate system, (u 2 , v 2) and the world coordinates (x w , y w , z w ) in the world coordinate system under the world coordinate system have the following formula:

[0066]

[0067] Among them, p is an element in the projection matrix P from the world coordinates to the pixel coordinate system. After sorting, the derivation result can be obtained as follows. Solving it can obtain the world coordinates (x w , y w , z w ) corresponding to the two pixel coordinates.

[0068]

[0069] S4. Predict the motion trajectory and superimpose the predicted point cloud. The prediction of the target motion trajectory is considered to be realized by using the least squares method. The least squares method uses the method of minimizing the sum of the squares of the deviations to find the optimal function of the data to correspond to the problem, and can be used to solve the general law of the data and the fitting of the curve from a pile of related data. In addition, in some practical problems of optimal design, minimizing the energy or maximizing the entropy can also be expressed by minimizing the least squares formula. The scheme considers fitting a three-dimensional straight line by using the least squares method, which is mainly manifested as finding a straight line to minimize the sum of the Euclidean distances between each known point and this straight line, that is, minimizing the sum of the squares of the errors from the points to the straight line. Through the point cloud superimposition link, various information (such as position information and speed information) of the pedestrians moving in a uniform straight line is given by the continuously updated three-dimensional point cloud trajectory, providing correct information for the subsequent obstacle avoidance task when the pedestrians go out of the field of vision. The newly added point cloud has the same status and function as the original point cloud and both have the depth information of the obstacle, and it is changing in real time. The real-time superimposed point cloud represents the predicted future motion state of the research object through the motion state of the research object when it is in the field of vision. This algorithm effectively makes up for the defect of the original UAV obstacle avoidance algorithm based on the three-dimensional vector field histogram in avoiding dynamic obstacles outside the field of vision

[0070] Among them, the specific process of the improved algorithm of the three-dimensional vector field histogram method is as follows: First, the camera obtains an image from the world. The image is converted into a point cloud. After preprocessing, a new point cloud with new information is obtained. The memory strategy combines and considers the new point cloud with new information and the old point cloud with old information, which is called the final point cloud and is used for the subsequent obstacle avoidance process. The subsequent obstacle avoidance process specifically includes generating a histogram, determining candidate directions, path planning, generating waypoints, maneuvering obstacle avoidance, etc. At the same time, the final point cloud obtained by memory and combination can be used for visualization. The visualization process can be carried out simultaneously and independently with the main process. The three-dimensional vector field histogram method uses the histogram method as the core strategy for obstacle avoidance. Its memory strategy can record obstacles within a certain time and a certain range around in the form of static world coordinates. However, the ordinary obstacle avoidance algorithm of the unmanned aerial vehicle based on the three-dimensional vector field histogram method will have problems such as detection information loss and ineffective obstacle avoidance when a moving object moves out of the field of view. A core function of the obstacle avoidance algorithm of the unmanned aerial vehicle based on the three-dimensional vector field histogram method is the memory strategy. Its memory strategy can remember obstacles within a certain time and a certain range around in the world coordinate system. For all obstacles within the field of view, the memory strategy is updated in real time and no memory is generated. For obstacles outside the field of view, once a dynamic or static obstacle moves out of the field of view through relative movement, the algorithm will retain its world coordinates at the final moment for a memory duration. In its memory process, the newly generated images and point clouds of the camera are new information and participate in obstacle avoidance in the form of a histogram. The improvement idea is to divide obstacle avoidance into two parts: ordinary obstacle avoidance and dynamic prediction outside the field of view, and perform corresponding obstacle avoidance for different situations.

[0071] The histogram is the basis for generating a cost map and determining candidate directions. The bright area is the obstacle area, and the dark area is the area with no or few obstacles. The three-dimensional vector field histogram algorithm uses a cost function (also called a cost function) to determine candidate directions on the histogram and generate a cost map. The cost function for determining candidate directions consists of two parts, and its formula is as follows:

[0072] c goal =Δ yaw (g,p)+k up ·Δ pitch_up (g,p)+k down ·Δ pitch_down (g,p);

[0073] c smooth =Δ yaw (p old ,p)+Δ pitch (p old ,p);

[0074] c total =k goal ·c goal +k smooth ·csmooth ;

[0075] where g is the target position direction; p is the candidate direction projected into three dimensions at the current time step; p old is the candidate direction projected into three dimensions at the previous time step; is the weight factor corresponding to different costs; c is the corresponding different costs, and the total cost c total is composed of the target cost c goal and the smoothing cost c smooth in two parts; Δ is the angle difference, divided into the yaw angle difference Δ yaw and the pitch angle difference Δ pitch .

[0076] The improved design lies in the trajectory prediction of dynamic obstacles outside the field of view. The UAV vision-based obstacle avoidance algorithm based on three-dimensional vector field histogram can effectively avoid static or dynamic objects within the field of view and static objects outside the field of view, but this algorithm cannot effectively avoid dynamic obstacles outside the field of view. It can only record their static positions at the last moment within the field of view, leaving much room for improvement. The premise of improving this algorithm is to deconstruct it first; its specific process is that the camera first obtains images from the world, the images are converted into point clouds, and after preprocessing, new point clouds with new information are obtained. At the same time, through the perception and prediction process of dynamic obstacles outside the field of view, specifically, after target detection, target tracking is carried out. A pair of pixel coordinates of the left and right object center points in the pixel coordinate system can be obtained in the left and right views. According to the principle of binocular stereo vision, combined with the internal and external camera parameters obtained by camera calibration, the coordinate transformation of this pixel coordinate can be realized, and the three-dimensional world coordinates corresponding to the same dynamic obstacle object can be obtained. Inputting into the trajectory fitting and prediction process based on the least squares method can obtain the three-dimensional motion trajectory of the object. Through the point cloud generation and superposition process, predicted point clouds are generated and superimposed with the original obstacle avoidance point clouds. The memory strategy combines and considers the new point clouds with new information and the old point clouds with old information, which is called the final point cloud. Finally, they are used for the construction of the vector field, that is, converting the obtained three-dimensional space point cloud data into a vector field. Each vector not only represents the position of the obstacle but also reflects the size and shape of the obstacle; then the generation of the histogram is carried out. Based on the vector field, a three-dimensional histogram is generated. In this histogram, each bin (in the three-dimensional vector field histogram, a "bin" is a three-dimensional unit used to quantify and represent the obstacle density in a certain local area of space) represents the obstacle density in a spatial area and can be regarded as the probability of the obstacle. Then, based on the generated histogram, "candidate directions" are selected. The candidate directions usually refer to the flight path directions that the UAV may choose. These directions are obtained based on the analysis of the obstacle avoidance algorithm. The obstacle situation in each direction is evaluated according to the obstacle density, and the direction with the fewest obstacles or no obstacles at all is selected as the flight path. After the candidate directions are selected, the candidate directions are repeatedly generated at a certain distance again for the obtained candidate directions, and a path search tree can be obtained. After the path tree is obtained, the path tree is evaluated and planned using the path evaluation method of the A* algorithm. Since the path evaluation of the A* algorithm for the paths in the path tree is realized by traversing each path one by one, the path evaluation is also called path search, and its ultimate goal is to achieve path planning. Then the waypoint information obtained from the path planning is sent to the flight control for waypoint flight control, thus realizing the overall obstacle avoidance process.

[0077] The improved obstacle avoidance algorithm has a certain ability to avoid dynamic obstacles outside the field of view, achieving omnidirectional autonomous perception and obstacle avoidance under certain conditions. Based on the original ordinary obstacle avoidance algorithm, the principle flow chart of the improved memory strategy adds a part of the prediction process. The principle of memory generation remains unchanged, where the old information and new information are combined to form combined information with both old and new information. After the next time step, the combined information becomes old information. The combined information still serves as the input for other obstacle avoidance links. The innovation of the method adopted lies in that after perceiving and predicting the objects within the field of view, the predicted information is combined with the old information, enabling the obstacle avoidance algorithm to have an estimation of the future of specific moving objects, rather than incorrectly retaining the static position information at its last appearance moment. The fusion of the ordinary obstacle avoidance method for unmanned aerial vehicles based on the three-dimensional vector field histogram method and the obstacle avoidance method for dynamic obstacles outside the field of view is completed.

[0078] In the obstacle avoidance algorithm based on the three-dimensional vector field histogram method, candidate directions are regenerated at a certain distance for the obtained candidate directions to obtain a path search tree. After obtaining the path tree, the A* algorithm's path evaluation method is used to evaluate and plan the path tree. Since the A* algorithm evaluates the paths in the path tree by traversing them one by one, the path evaluation is also called path search, and its ultimate goal is to achieve path planning. Select the optimal path after evaluation as the final path to obtain the optimal path. The evaluation cost formula of the A* algorithm is:

[0079] f(n) = g(n) + h(n);

[0080] Among them, f(n) is the total cost of the current node n; g(n) is the cost from the current node n to the target point; h(n) is the cost from the starting point to the current node n. The cost or price g(n) from the current point n to the target point is composed of the weighted sum of four parts: the target cost, the yaw cost, the path smoothing cost, and the smoothing cost. In the three-dimensional vector field histogram algorithm, the selection formula for the target cost function g(n) of the node n is as follows:

[0081]

[0082] Among them, n is the current node; c n is the total node cost; λ is a discount factor less than 1; k target is the target weight parameter; c target is the target cost; k yaw is the yaw weight parameter; c yaw is the yaw cost; k path is the path smoothing weight parameter; c path is the path smoothing cost; k tree is the tree smoothing weight parameter; c tree is the tree smoothing cost.

[0083] As Figures 1 to 5 shown, the present invention deconstructs the ordinary obstacle avoidance algorithm of an unmanned aerial vehicle (UAV) based on the three-dimensional vector field histogram method, and then concludes that there are problems of detection loss, information error, and inability to avoid when the obstacle avoidance algorithm faces a moving obstacle moving out of the field of view. This problem is dissected and analyzed, and it is found that the obstacle avoidance problem when the obstacle moves out of the field of view stems from the limitation of the camera field of view and the limitation of the ordinary obstacle avoidance algorithm of the UAV based on the three-dimensional vector field histogram method, and an innovative solution can be used to make up for it to a certain extent. Therefore, the present invention gives corresponding solutions and solution processes for the problem of predicting and avoiding dynamic obstacles outside the field of view. The process is to sense and utilize the motion state of a specific dynamic obstacle when it is within the field of view, predict its motion state outside the field of view and in the future, define the concept of omnidirectional autonomous sensing and obstacle avoidance of the UAV, and adopt a solution combining sensing, prediction, and avoidance for dynamic obstacles outside the field of view of the UAV. Finally, the present invention completes the integration of the ordinary obstacle avoidance method of the UAV based on the three-dimensional vector field histogram method and the method of predicting and avoiding dynamic obstacles outside the field of view, where Figure 5 (a) of Figure 5 is a histogram,

[0084] As Figure 6 shown, by analyzing the principles of the ordinary obstacle avoidance algorithm and the algorithm for predicting and avoiding dynamic obstacles outside the field of view, the present invention gives the combination idea and combination process of these two methods and algorithms. After deep learning, taking pedestrians as specific research objects, the present invention proposes the specific implementation process of the omnidirectional autonomous sensing and obstacle avoidance algorithm of the UAV, including using object detection and object tracking based on deep learning to sense the motion state of the research object when it is within the field of view, and using the binocular vision principle to transform the pixel coordinates of the research object into world coordinates, as Figure 7 shown. The transformation from the world coordinate system to the camera coordinate system is a common operation that transforms points defined in the "world coordinate system" into the "camera coordinate system". This transformation is a rigid body transformation that includes a rotation and a translation, but does not include scaling or distortion. A rigid body transformation can be described by a rotation matrix (R) and a translation vector (T). In three-dimensional space, if we have the coordinates of a point (P w ) in the world coordinate system, then its coordinates (P c ) in the camera coordinate system can be calculated through the following transformation:

[0085] P c = R·P w + T;

[0086] The transformation from the "camera coordinate system" to the "image coordinate system" involves perspective projection, which is a process of mapping points in three-dimensional space onto a two-dimensional image plane. In this process, the camera's internal parameters play a crucial role. The internal parameters include the focal length, the optical center (image center), and other parameters that may affect image distortion. In the camera coordinate system, a point P c =(X c , Y c , Z c ) can be transformed into the image coordinate system through perspective projection. The transformation formula is (the internal parameter matrix of the camera is usually denoted as (K)):

[0087] [x′y′w′]=K·[X c Y c Z c ;

[0088] After this transformation, the obtained coordinates (x′, y′, w′) need to be converted from homogeneous coordinates to Cartesian coordinates, and the final image coordinates (x, y) are:

[0089]

[0090] The transformation from the "image coordinate system" to the "pixel coordinate system" involves affine transformation, which is a process of mapping coordinates in the real world to the discrete pixel grid of a digital image. This transformation maintains the linear relationship between points, including keeping the straightness of original straight lines, the parallelism of parallel lines, and the constancy of ratios.

[0091]

[0092] Among them, (x, y) are points in the original image coordinate system, (x′, y′) are points in the transformed pixel coordinate system, and (a, b, c, d, e, f) in the matrix are the parameters of the affine transformation.

[0093] The least squares method is used for trajectory fitting to predict the future motion state of the research object, generating a predicted point cloud with the future motion information of the object and using it in the subsequent obstacle avoidance process, realizing the ordinary obstacle avoidance algorithm of the UAV based on the three-dimensional vector field histogram method and the predicted obstacle avoidance algorithm for dynamic obstacles outside the field of view.

[0094] Through the improvement of the ordinary obstacle avoidance algorithm of the drone based on the three-dimensional vector field histogram algorithm, this invention provides conditions for the implementation of the subsequent dynamic obstacle prediction and avoidance algorithm and the omnidirectional obstacle avoidance algorithm outside the field of view. The omnidirectional obstacle avoidance method is divided into three stages: perception, prediction, and obstacle avoidance. First is perception, that is, autonomous detection and tracking to sense and obtain the research object; second is prediction, that is, using the past data of the object when it is within the field of view to predict its motion trajectory in the world coordinate system; finally is obstacle avoidance, that is, using the predicted trajectory for obstacle avoidance. The ultimate goal is that under the condition that the research object moves in a uniform straight line, the drone extracts the motion information of the object in the pixel coordinate system when it is within the field of view through the camera, predicts its future motion information in the world coordinate system, and realizes omnidirectional obstacle avoidance under certain conditions.

[0095] Based on the same inventive concept, this invention also proposes a drone perception and obstacle avoidance system, including:

[0096] An acquisition module, used to acquire obstacle information in the three-dimensional space around the drone and generate original point cloud data.

[0097] A three-dimensional histogram generation unit, used to perform target detection on the obstacles within the field of view of the drone, perform target tracking on the detected target obstacles to obtain the pixel coordinates of the target obstacles; convert the pixel coordinates of the target obstacles into corresponding three-dimensional world coordinates; according to the three-dimensional world coordinates corresponding to the target obstacles, use the least squares method to fit the three-dimensional point cloud trajectory of the target obstacles; through a memory strategy, merge the three-dimensional point cloud trajectory of the target obstacles within a set time with the original point cloud data to predict the three-dimensional point cloud trajectory data of the target obstacles' motion in the next time step; convert the predicted three-dimensional point cloud trajectory data into a vector field to generate a three-dimensional histogram.

[0098] An obstacle avoidance path selection unit, used to based on the three-dimensional histogram, the drone selects the flight path direction with the fewest or no obstacles as the obstacle avoidance path of the drone.

[0099] This invention also proposes a computer device for drone perception and obstacle avoidance, including: a memory, a processor, and a computer program stored in the memory. When the processor executes the computer program, it realizes the steps of the drone perception and obstacle avoidance method.

[0100] This invention also proposes a readable storage medium, which stores a computer program. The computer program includes program instructions. When the program instructions are executed by the processor, they are used to execute the steps of the drone perception and obstacle avoidance method.

[0101] The above are only the preferred specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, making equivalent substitutions or changes should be covered within the protection scope of the present invention.

Claims

1. A method for sensing and avoiding obstacles of a drone, characterized in that: The following steps are involved: Obtain obstacle information in the three-dimensional space around the drone and generate raw point cloud data; Perform target detection on obstacles within the drone's field of view, track the detected target obstacles, and obtain the pixel coordinates of the target obstacles; The pixel coordinates of the target obstacle are converted into corresponding three-dimensional world coordinates; the three-dimensional point cloud trajectory of the target obstacle is fitted using the least squares method according to the three-dimensional world coordinates corresponding to the target obstacle; the three-dimensional point cloud trajectory of the target obstacle within the set time is merged with the original point cloud data through the memory strategy, and the three-dimensional point cloud trajectory data of the target obstacle in the next time step is predicted; the predicted three-dimensional point cloud trajectory data is converted into a vector field to generate a three-dimensional histogram; Based on the 3D histogram, the UAV selects the flight path direction with the least or no obstacles as the obstacle avoidance path of the UAV.

2. The method for sensing and avoiding obstacles of a drone according to claim 1, characterized in that: Based on the three-dimensional histogram, the drone selects a flight path direction with the least obstacles or no obstacles at all as an obstacle avoidance path, including the following steps: Obtain the obstacle density of the local area in the three-dimensional space based on the three-dimensional histogram; According to the obstacle density, the obstacle situation in each flight direction of the UAV is evaluated, and then the direction with the least or no obstacles is selected as the candidate direction; Using the selected candidate directions, candidate directions are generated again at a certain distance to obtain a path search tree; The A* search algorithm is used to evaluate and plan the path search tree to obtain the optimal obstacle avoidance path.

3. The method for sensing and avoiding obstacles of a drone according to claim 2, characterized in that: The A* search algorithm is used to evaluate and plan the path search tree, specifically including the following steps: The path search tree is evaluated using the A* search algorithm, which is expressed as: f(n)=g(n)+h(n); Among them, f(n) is the total cost of the current node n; g(n) is the cost from the current node n to the target point; h(n) is the cost from the starting point to the current node n; The cost g(n) from the current node n to the target point includes the target cost, the deviation cost, the path smoothing cost, and the smoothing cost, which is expressed as: Where n is the current node; g(n) is the total cost of the node; is a discount factor less than 1; k target is the target weight parameter; g target is the target cost; k yaw is the yaw weight parameter; g yaw is the yaw cost; k path is the path smoothing weight parameter; g path is the path smoothing cost; k tree is the tree smoothing weight parameter; g tree Smooths the cost for the tree.

4. The method for sensing and avoiding obstacles of a drone according to claim 1, characterized in that: A cost function is used to select an obstacle-free area or an area with the least obstacles as a candidate direction on a three-dimensional histogram, wherein a bright area in the three-dimensional histogram represents an obstacle area, and a dark area represents an obstacle-free area or an area with the least obstacles; the total cost c of the cost function is total Including the target cost c goal and the smoothing cost c smooth Two parts, expressed as: c goal HΔ yaw (g,p)+k up ·Δ pitch_up (g,p)+k down ·Δ pitch_down (g,p) c smooth =D yaw (p old ,p)+D pitch (p old ,p); c total =k goal ·c goal +k smooth ·c smooth ; Among them, g is the target position direction; p is the candidate direction projected to three dimensions at the current time step; p old is the candidate direction projected into three dimensions at the previous time step; k goal , k smooth is the weight factor corresponding to different costs; c is the corresponding different costs; Δ is the angle difference, Δ yaw is the yaw angle difference, Δ pitch_up Δ pitch_down is the pitch angle difference, k up , k down is the corresponding weight factor.

5. The method for sensing and avoiding obstacles of a drone according to claim 1, characterized in that: According to the principle of binocular stereo vision, the pixel coordinates (u, v) of the target obstacle are converted into the corresponding three-dimensional world coordinates (x w ,y w ,z w ), the coordinate transformation formula between the world coordinate system and the pixel coordinate system is: where z c It is the depth value of the three-dimensional coordinate point in the camera coordinate system, that is, the distance from the camera to the point; Represents the transformation relationship from the image coordinate system to the pixel coordinate system, [u0,v0] represents the origin of the image coordinate system, Indicates the conversion relationship between millimeters and pixel coordinate system units. represents the transformation relationship from the camera coordinate system to the image coordinate system based on perspective projection, and f represents the focal length of the camera; represents the camera extrinsic matrix, Represents the rotation matrix, which is used to rotate from the world coordinate system to the camera coordinate system; [t x ,t y ,t z ] represents the translation vector, which describes the position of the camera in the world coordinate system; For the center point of the obstacle in the left and right images of the binocular camera, a pair of pixel coordinates (u1, v1), (u2, v2) in the pixel coordinate system and the world coordinates (x w ,y w ,z w ) is expressed as: Where p is the element in the projection matrix P from the world coordinate system to the pixel coordinate system; It is deduced that: Solve to get the world coordinates (x w ,y w ,z w ).

6. The method for sensing and avoiding obstacles of a drone according to claim 1, characterized in that: A binocular vision sensor is used to obtain obstacle information in the three-dimensional space around the drone.

7. A drone perception and obstacle avoidance system, characterized in that: include: The acquisition module is used to obtain obstacle information in the three-dimensional space around the drone and generate raw point cloud data; A three-dimensional histogram generation unit is used to detect obstacles within the field of view of the drone, track the detected target obstacles, and obtain the pixel coordinates of the target obstacles; The pixel coordinates of the target obstacle are converted into corresponding three-dimensional world coordinates; the three-dimensional point cloud trajectory of the target obstacle is fitted using the least squares method according to the three-dimensional world coordinates corresponding to the target obstacle; the three-dimensional point cloud trajectory of the target obstacle within the set time is merged with the original point cloud data through the memory strategy, and the three-dimensional point cloud trajectory data of the target obstacle in the next time step is predicted; the predicted three-dimensional point cloud trajectory data is converted into a vector field to generate a three-dimensional histogram; The obstacle avoidance path selection unit is used for the UAV to select a flight path direction with the least obstacles or no obstacles as the obstacle avoidance path of the UAV based on the three-dimensional histogram.

8. A computer device for drone perception and obstacle avoidance, characterized in that: include: A memory, a processor, and a computer program stored in the memory, wherein the processor implements the steps of the drone perception and obstacle avoidance method according to any one of claims 1 to 6 when executing the computer program.

9. A readable storage medium, characterized in that: The readable storage medium stores a computer program, which includes program instructions. When the program instructions are executed by a processor, they are used to execute the steps of the drone perception and obstacle avoidance method described in any one of claims 1 to 6.

Images