Multi-source sensor obstacle identification and obstacle avoidance method for low-altitude logistics

Abstract

The invention provides a multi-source sensor obstacle recognition and obstacle avoidance method for low-altitude logistics, and relates to the technical field of low-altitude logistics. The method comprises the following steps: S1, synchronously acquiring data through a laser radar and a binocular camera, respectively performing distortion correction, exposure control and depth map generation on binocular vision data, and performing segmentation denoising and Kalman-particle hybrid filtering denoising on point cloud data; s2, adopting a YOLOv5s network to extract a barrier feature vector; s3, carrying out obstacle fusion association; S4, fusing historical features and laser radar coordinates based on an LSTM model, and predicting a future trajectory of a dynamic obstacle; and S5, generating an obstacle avoidance path in combination with an A * algorithm and an artificial potential field method, and correcting the obstacle avoidance path in real time. The problems of missing detection or misjudgment of low-texture obstacles, sensor data fault, dynamic obstacle prediction deviation and path oscillation are solved, and the obstacle recognition precision and the obstacle avoidance reliability are improved. The method is high in real-time performance and adapts to low-altitude logistics complex scenes.

Description

Technical Field

[0001] This invention relates to the field of low-altitude logistics technology, and in particular to a multi-source sensor obstacle identification and avoidance method for low-altitude logistics. Background Technology

[0002] Low-altitude logistics, as an efficient last-mile delivery method, needs to cope with complex scenarios such as urban buildings, dynamic obstacles (such as birds and other aircraft), and obstacles made of special materials (such as glass curtain walls and power lines). However, existing technologies using multi-source sensors such as lidar and binocular vision for obstacle avoidance mainly have the following technical problems: First, for obstacles such as glass curtain walls and transparent plastic sheds, which are easily missed by binocular vision and easily misjudged by lidar, false detection of low-texture / special material obstacles is possible; Second, during low-altitude flight, binocular vision may experience data failure due to sudden entry into shadow areas (such as the boundary between warehouse corridors and open areas), and lidar may also experience point cloud gaps due to local dust obstruction, leading to data discontinuity and interruption of obstacle avoidance decisions. Summary of the Invention

[0003] The purpose of this invention is to address the shortcomings of existing technologies by proposing a multi-source sensor obstacle identification and avoidance method for low-altitude logistics.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] A multi-source sensor obstacle recognition and avoidance method for low-altitude logistics, characterized by the following steps:

[0006] S1: Sensor data acquisition and preprocessing;

[0007] Multiple sensors simultaneously collect data, and the collected data is preprocessed;

[0008] The sensors include LiDAR and binocular cameras;

[0009] S11: Simultaneous data acquisition from multiple sensors;

[0010] The lidar collects point cloud data of obstacles in the environment at a fixed frequency; the left and right cameras of the binocular camera capture images simultaneously and output RGB images.

[0011] S12: Binocular vision data preprocessing;

[0012] The pixel coordinates of the RGB image are corrected based on the camera's intrinsic parameters to eliminate distortion. The corrected image is then used for exposure control to output a brightness-corrected image with stable brightness. Finally, the brightness-corrected image is filtered to obtain a depth map.

[0013] S13: Point cloud data denoising and filtering;

[0014] After performing point cloud segmentation and initial denoising on the point cloud data, several obstacle clusters are obtained from the initially filtered point cloud clusters. Kalman-particle hybrid filtering is used to denoise each obstacle cluster to obtain a denoised point cloud.

[0015] S2: Lightweight feature extraction;

[0016] A YOLOv5s pre-trained model is used to quickly locate obstacles from the depth map and output feature vectors.

[0017] The feature vector includes the bounding box coordinates of the target, the class probability, and the confidence level;

[0018] S3: Obstacle fusion association;

[0019] After standardizing the denoised point cloud and bounding box coordinates of each obstacle cluster, IoU matching is performed to associate the denoised point cloud of the obstacle cluster with the bounding box.

[0020] S4: Dynamic obstacle trajectory prediction;

[0021] Based on the LSTM model, a denoised point cloud of obstacle clustering associated with the feature vectors output by dynamic obstacles and the bounding boxes of the feature vectors is fused to predict the future trajectory of dynamic obstacles.

[0022] S5: Obstacle avoidance path planning;

[0023] Based on denoised point clouds, and combining the A* algorithm with the artificial potential field method, a smooth path is generated to avoid static and dynamic obstacles.

[0024] Furthermore, step S12 includes:

[0025] S121: Correct the pixel coordinates of the RGB image based on the distortion coefficients in the camera intrinsic parameters to obtain the corrected image;

[0026] S122: Exposure control outputs a brightness-corrected image with stable brightness;

[0027] The corrected image is obtained, converted to grayscale, and the grayscale value of each pixel is obtained. The average brightness of the image is calculated. Low light threshold and high light threshold are preset. The calculated average brightness is compared with the low light threshold and high light threshold. If the average brightness is less than the low light threshold, the exposure time is increased; otherwise, the exposure time is decreased. The dark current reference value is obtained by using the preset target brightness, the original exposure time and dark environment test. The updated exposure time is calculated. The corrected image is a brightness corrected image with stable brightness output through exposure.

[0028] S123: Filtering to obtain a depth map;

[0029] The disparity of each pixel in the obtained brightness-corrected image is calculated using a stereo matching algorithm, and then converted into a depth value to generate a depth map corresponding to the image resolution.

[0030] Furthermore, step S13 includes:

[0031] S131: Point cloud segmentation and initial denoising;

[0032] First, perform pass-through filtering: read point cloud data, define the filtering object, set the filtering axis to the z-axis, and set the ground threshold; retain points in the point cloud data whose z-axis coordinates are greater than the ground threshold;

[0033] Then, distance filtering is performed, a radar detection threshold is set, the coordinates of each point in the retained point cloud data are obtained, the distance of each point from the origin is calculated, points with a distance greater than the radar detection threshold are removed, each frame of point cloud data is processed, and the point cloud after preliminary filtering is obtained.

[0034] S132: Obstacle clustering;

[0035] Using the Euclidean clustering algorithm with a clustering distance threshold of 0.3m, the initially filtered point cloud is divided into multiple independent clusters, outputting a cluster set {C1, C2, ..., C}. n};

[0036] S133: Kalman-Particle Hybrid Filtering for Noise Reduction;

[0037] Filtering is performed separately for the point cloud of each obstacle cluster. First, the point cloud position is predicted by Kalman filtering, and then the particle filtering correction is activated based on the residual. Finally, the denoised point cloud is output.

[0038] Step 1: Initialize key parameters;

[0039] Initialize the sampling interval, state transition matrix, control matrix, noise standard deviation, particle number, and residual threshold;

[0040] Step 2: Kalman filter prediction;

[0041] State initialization: An obstacle cluster C in the first frame i The predicted value of the point cloud is set as the obstacle clustering C of this frame. i The average coordinates of the point cloud; C i For the clustering of the i-th obstacle in the cluster set;

[0042] State prediction: Clustering of obstacles C in frame t i The point cloud, combined with the predicted value from the previous frame Calculate the current predicted value based on the drone's speed u(t):

[0043] Where F is the state transition matrix; the UAV velocity u(t) is provided by the UAV IMU; and B is the control matrix.

[0044] Step 3: Calculate the residual and determine whether particle filtering is triggered;

[0045] Calculate obstacle clustering C in the current frame i The average coordinates of the point cloud are calculated, and the distance between the average coordinates and the current predicted value is used to obtain the residual. When the residual is greater than the residual threshold, particle filtering is triggered; otherwise, the Kalman prediction value is directly output as the denoised point.

[0046] Step 4: Particle filter correction;

[0047] First, based on the predicted value The mean is 0.05m, and the noise standard deviation is 0.05m, generating 200 Gaussian distributed particles.

[0048] Next, the distance d between each generated particle and the average coordinate is calculated. i ; Assign weights to each particle according to the Gaussian function; Normalize the weights assigned to each particle to obtain normalized weights;

[0049] Then, low-weight particles are eliminated, and high-weight particles are retained to obtain resampled particles;

[0050] Finally, the resampled particles are fused to update the denoised point cloud;

[0051] Obtain the coordinates of all resampled particles, and use the weighted average of the resampled particles as the coordinates of the denoised point cloud to update the denoised point cloud of obstacle clustering.

[0052] Iterate through the point clouds of all obstacle clusters, update the denoised point clouds of all obstacle clusters, obtain the denoised point cloud of the current frame, and iterate through all frames to obtain the denoised point cloud.

[0053] Furthermore, step S2 includes:

[0054] S21: Train the YOLOv5s network to obtain the YOLOv5s pre-trained model. Use the YOLOv5s pre-trained model and convert it to inference mode. The preset input image size is 640×640 pixels. Set the confidence threshold and non-maximum suppression threshold through the configuration file.

[0055] S22: Preprocess the input image; scale and normalize the obtained depth map image, and convert it to the model input format;

[0056] S23: Input the preprocessed input image into the model, and generate feature maps of three scales: 80×80, 40×40, and 20×20 through convolutional layers, residual blocks, and CSP structure; for the feature maps of the three scales, the detection head predicts the position offset of each preset anchor box through convolutional layers; and outputs the confidence score and class probability of the target's existence through the sigmoid function; S24: First, filter candidate boxes with confidence scores below the threshold, and then remove redundant boxes with overlap scores above the threshold through non-maximum suppression, and output the bounding box coordinates (x, y, w, h), class probability p, and confidence score conf of the target;

[0057] S25: Output feature vector, which transforms the output detection results into a standardized feature vector F = (x, y, w, h, pcls, conf).

[0058] Further, step S4:

[0059] S41: LSTM Model Construction and Training;

[0060] Construct an LSTM layer: input dimension is 6, hidden layer dimension is 128, number of layers is 1, tanh activation function is used, and the hidden state h at each step is preserved. k This is used to capture the time dependency of obstacle motion;

[0061] Constructing a fully connected layer: Mapping the LSTM output to 3D; using a linear activation function;

[0062] Configure model structure parameters: set the time step to 10; set the prediction step size K to 15;

[0063] S412: Model training;

[0064] Collect dynamic obstacle data in low-altitude logistics scenarios; a total of 100,000 frames, each frame labeled with real-world coordinates, divided into training set, validation set and test set in a 7:2:1 ratio; train and validate the constructed LSTM model to obtain the training model;

[0065] S42: Trajectory prediction inference;

[0066] S421: Obtain input data;

[0067] Extract the state vector sequence of historical time steps; extract the feature vectors of the 10 frames before the current time to form a state vector sequence;

[0068] S422: Trajectory Prediction;

[0069] 1) LSTM prediction obtains preliminary predicted coordinates; the state vector sequence of historical time steps is input into the pre-trained LSTM model to obtain preliminary predicted coordinates for the next K steps;

[0070] 2) Anomaly filtering;

[0071] Based on the coordinate range after denoising by the lidar, outliers in the predicted coordinates that exceed the coordinate range are filtered out and replaced with the coordinates corrected in the previous step.

[0072] 3) Output the prediction results;

[0073] Output the corrected predicted coordinates for the next K steps; the output format is P = (P1, P2, ..., P...). K ).

[0074] Furthermore, step S412 specifically includes:

[0075] First, feature vectors are extracted from each frame of the training set in step S2;

[0076] Next, based on the category probability p, the obstacle category is determined, the obstacle clusters associated with the bounding boxes identified as dynamic obstacles are extracted, and the denoised point clouds within the smallest bounding box of the obstacle cluster are selected as real-world coordinates.

[0077] Then, the feature vectors of the past 10 frames in the training set are used as input, and the real-world coordinates of the next 15 frames are used as labels; the number of training epochs is set to 50; and the training is validated using a validation set. Training stops when the validation set loss does not decrease for 5 consecutive epochs, and the pre-trained fully connected layer weights W are saved. fc and fully connected layer bias b fc ; Obtain the training model.

[0078] Furthermore, step S5 includes:

[0079] S51: 3D raster environment modeling;

[0080] S511: Establish the grid coordinate system;

[0081] A coordinate system is established with the UAV's current position S as the origin, the X-axis along the initial heading, the Y-axis perpendicular to the heading, and the Z-axis vertically upward; the mapping relationship between the raster index and the world coordinates is 1.2:2.

[0082] S512: Static obstacle marker;

[0083] Traverse the denoised point cloud in step S133, set the grid value corresponding to each static obstacle point to 1, and set the rest of the grids to 0; 1 means impassable, and 0 means passable.

[0084] S513: Dynamic obstacle marking;

[0085] Based on the predicted trajectory P, the predicted P for each frame is calculated through the mapping relationship. K The corresponding grid; these grids are set at the corresponding timestamp t kThe internal marker is 1.

[0086] S514: Output a 3D raster map with a time dimension;

[0087] S52: A* algorithm for global path search;

[0088] S521: Obtain node attributes;

[0089] Each grid is a node; node attributes include coordinates and cost function f(n);

[0090] The coordinates refer to the world coordinates of the center of the grid; let the coordinates of node n be (X... n ,Y n Z n );;

[0091] Cost function: f(n) = g(n) + 1.2 × h(n), where g(n) is the actual distance from the starting point S to node n, and the cumulative distance along the searched path; h(n) is the straight-line distance from node n to the target point.

[0092] S522: OpenList and CloseList Management;

[0093] OpenList stores nodes to be explored, initially containing only the starting point S; CloseList stores explored nodes, initially empty.

[0094] S523: Path search iteration;

[0095] Select the node n with the smallest f(n) from the OpenList. min Move to CloseList; if n min For the target point G, backtrack the parent node relationships of the nodes in CloseList to generate a global path and terminate the search; otherwise, traverse n min Calculate the cost function for each of the 8 adjacent grids. If an adjacent point is in the ClosedList or marked as impassable (1), skip it. If an adjacent point is not in the OpenList and its calculated cost function is smaller, update the adjacent point's g(n), h(n), and f(n), and set n... min Set it as the parent node and add it to OpenList; repeat the process until the target node G is found; output the global path;

[0096] S53: Local path optimization;

[0097] S531: Calculation of attractive potential field;

[0098] For each node in the global path; let n be... i (Xni ,Y ni Z ni ); Calculate the target point G(X) G ,Y G Z G The resultant force of attraction on the nodes;

[0099] According to the formula

[0100] S532: Calculation of repulsive potential field;

[0101] For node n i For all obstacles within a 3m radius, obtain the static point cloud and dynamic predicted trajectory points marked as 1 within the range; calculate the o-to-n pair for each obstacle. i The repulsive force;

[0102] Where d0 is the radius of influence of the obstacle, set at 3m; d (ni,o) For node n i The Euclidean distance to obstacle o, k1 is the repulsive potential coefficient, set to 5.0; n i -o is the vector pointing the node in the opposite direction to the obstacle;

[0103] S533: Total Resultant Force and Path Adjustment;

[0104] Compute node n i Total force F 总 =F 吸引 +F 排斥 Adjust the node position along the direction of the resultant force:

[0105] Where ΔS is the grid size, initially 0.5m, and ||F 总 || represents the modulus of the total force; n i For node n i After adjustment, check the new node n. i 'Is it passable? If not, decrease ΔS and readjust.'

[0106] S534: Mobility constraint check;

[0107] Calculate the angle θ between the lines connecting adjacent nodes after adjustment. If θ > 15°, then insert a transition node n. 过渡 Such that the included angles of adjacent angles are all ≤15°;

[0108] S535: Output the locally optimized smooth path [n'0, n'1, ..., n'] m Each node contains adjusted world coordinates and a timestamp.

[0109] Furthermore, step S5 also includes:

[0110] S54: Real-time path correction;

[0111] S541: Dynamic obstacle location update;

[0112] Every 0.1 seconds, compare the latest predicted trajectory in step S424 with the original trajectory. If the deviation is >0.3m, repeat steps S531-S534 to update the raster map and path.

[0113] S542: Emergency obstacle avoidance triggered;

[0114] If the lidar detects that the real-time distance between an obstacle and the drone is less than 3m, it immediately increases the repulsive potential coefficient k1 to 10.0, forcing the path to adjust away from the obstacle; deflection angle = δ * 15 * (3 / real-time distance);

[0115] S543: Update the output obstacle avoidance path.

[0116] Furthermore, step S123 specifically includes:

[0117] The SGBM algorithm is used, with a matching window size of 9×9 pixels. For each pixel in the left image, the surrounding rectangular area is taken as the matching window. On the same row of the right image, using the pixels in the left image as a reference, the window slides within a preset disparity range, and the SSD of the corresponding window in the left and right corrected images is calculated. The position with the smallest SSD is taken as the matching point, and the disparity d with the smallest SSD is taken as the disparity of that pixel, resulting in a disparity map D. The disparity range is set to 0 to 128 pixels. The disparity of each pixel in the disparity map is obtained, and pixels with a disparity of 0 or a disparity exceeding 128 are removed. If the disparity difference between adjacent pixels exceeds a threshold, it is judged as noise and replaced with the mean of the surrounding effective disparities. After filtering, a new disparity map is formed. The depth value of each pixel in the new disparity map is then calculated using the formula Z = fB / d, where f is the focal length, B is the baseline distance obtained through pre-calibrated intrinsic and extrinsic parameters, and d is the disparity of the pixel. All depth values ​​are calculated to obtain the depth map.

[0118] Furthermore, step S3 specifically includes:

[0119] S31: Standardize the point cloud for obstacle clustering;

[0120] Obtain obstacle clustering C i Obstacle clustering C is calculated. i minimum bounding box i box ;

[0121] S32: Standardize bounding box coordinates;

[0122] Obtain the bounding box coordinates (x, y, w, h) output by the YOLOv5s pre-trained model; calculate the left boundary pixel coordinates u1, right boundary pixel coordinates u2, upper boundary pixel coordinates v1, and lower boundary pixel coordinates v2 of the bounding box respectively;

[0123] The depth of all pixels within the bounding box is obtained from the depth map, and the average depth is calculated.

[0124] Convert the bounding box to a 3D detection box to obtain the 3D detection box: V box =(X vmin ,Y vmin Z vmin ,X vmax ,Y vmax Z vmax );

[0125] S33: IoU matching associated obstacles;

[0126] Based on obstacle clustering C i minimum bounding box i box With the 3D detection box V box The intersection-union ratio (IoU) is used for matching and association. The IoU is compared with a preset threshold. If it is greater than the threshold, the obstacles are clustered into C. i Associate the target obstacle with the bounding box identified in the YOLOv5s output; otherwise, cluster the obstacle into C. i If the target obstacle does not match the bounding box identified in the YOLOv5s output, iterate through other obstacle clusters to match and associate the bounding boxes, and then associate the obstacle clusters with the bounding boxes; iterate through all frames and group the obstacles into clusters C. i All target obstacles identified in the bounding boxes output by YOLOv5s are matched and associated.

[0127] Compared with the prior art, the beneficial effects of the present invention are as follows: (1) The present invention retains texture details by using binocular vision adaptive exposure control (stabilizing the image brightness at the target brightness of 128), and combines it with lidar Kalman-particle hybrid filtering to complete the obstacle outline. Then, it accurately extracts feature vectors through the YOLOv5s network (confidence threshold 0.5, NMS threshold 0.45), thereby achieving effective identification of obstacles of special materials, improving the recognition accuracy, preventing the problem of binocular vision missing detection and lidar misjudgment caused by low-texture obstacles such as glass curtain walls and transparent plastic sheds, and avoiding the collision risk caused by obstacle misjudgment and missed judgment. It is suitable for identification of complex low-altitude logistics scenarios. (2) The present invention completes the depth information by using binocular vision data interpolation and completes the missing point cloud by combining lidar particle filtering, ensuring that the data is not interrupted when the sensor fails locally. At the same time, through feature extraction, trajectory prediction, and path planning, it continuously outputs decisions based on continuous data in a hierarchical recursive manner, avoiding the problem of binocular vision failing due to entering the shadow area and lidar having missing point clouds due to dust obstruction during low-altitude flight, and ensuring the continuous effectiveness of the obstacle avoidance system during flight. (3) This invention uses an LSTM model, taking 10 frames of historical feature vectors and LiDAR coordinates as input, and captures motion time dependencies through LSTM (hidden layer dimension 128), reserving sufficient reaction time for obstacle avoidance path planning, avoiding lag in obstacle avoidance response, and improving the success rate of dynamic obstacle avoidance. (4) This invention plans the global path through the A* algorithm, combined with the artificial potential field method to optimize the local path, and sets a maximum deflection angle of 15° and a grid size of 0.5m to ensure that the path is smooth and adaptable to the minimum turning radius of the UAV, and updates and corrects it in real time to avoid path oscillation and improve flight stability. (5) This invention uses the YOLOv5s lightweight model for feature extraction, combined with hierarchical recursive decision-making (first screening suspected obstacle areas and then deep processing) to reduce invalid calculations, reduce single-frame data processing time, and achieve high real-time performance. Attached Figure Description

[0128] Figure 1 This is a flowchart illustrating the steps of the multi-source sensor obstacle identification and avoidance method for low-altitude logistics according to the present invention. Detailed Implementation

[0129] To provide a further understanding of the purpose, structure, features, and functions of the present invention, detailed descriptions are provided below with reference to specific embodiments.

[0130] A multi-source sensor-based obstacle recognition and avoidance method for low-altitude logistics includes the following steps:

[0131] S1: Sensor data acquisition and preprocessing;

[0132] Multiple sensors simultaneously collect data, and the collected data is preprocessed;

[0133] The sensors include a lidar and a binocular camera; a high-precision clock module (accuracy ±1ppm) is deployed on the drone side. Both the lidar and the binocular camera are connected to the clock module via a hardware synchronization line, and each frame of data is automatically equipped with a timestamp accurate to milliseconds when it is generated.

[0134] S11: Simultaneous data acquisition from multiple sensors;

[0135] LiDAR data acquisition: LiDAR is used to acquire point cloud data of obstacles in the environment at a fixed frequency (10Hz), including the three-dimensional coordinates (x, y, z) and reflection intensity of each point. The output format is point cloud frames; the point cloud data includes timestamps and point sets.

[0136] Binocular vision data acquisition: Images are captured synchronously by the left and right cameras of a binocular camera, and RGB images are output; the output RGB images have a fixed resolution; and calibrated intrinsic and extrinsic parameters are also included.

[0137] The binocular camera's acquisition frequency is synchronized with that of the LiDAR. For the acquired LiDAR point cloud frames and binocular visual image frames, a linear interpolation method is used to compensate for the time difference between frames, based on the timestamp: if the timestamp of the LiDAR frame is t1 and the timestamp of the binocular image frame is t2, and |t1-t2|<0.05s, then the frame with the later timestamp is aligned to the frame with the earlier timestamp; if |t1-t2|>0.05s, then the asynchronous data set is discarded, ensuring that the time deviation of each set of fused data is ≤0.05s.

[0138] Fixed standby frequency and format ensure that the timestamps of the collected point cloud data and binocular vision data are aligned, avoiding positional deviations caused by time differences.

[0139] In advance, intrinsic and extrinsic parameters are obtained through camera calibration. The intrinsic parameters include the focal length f of each of the left and right cameras. L =(f Lx ,f Ly ) and f R =(f Rx ,f Ry (pixel unit), distortion coefficient (used to correct lens distortion), principal point coordinates (u0, v0) (image center pixel position); extrinsic parameters include the relative pose of the two cameras (rotation matrix R and translation vector T), baseline distance B; baseline distance B refers to the distance between the optical centers of the left and right cameras.

[0140] S12: Binocular vision data preprocessing;

[0141] The pixel coordinates of the RGB image are corrected based on the camera's intrinsic parameters to eliminate distortion. The corrected image is then used for exposure control to output a brightness-corrected image with stable brightness. Finally, the brightness-corrected image is filtered to obtain a depth map.

[0142] S121: Correct the pixel coordinates of the RGB image based on the distortion coefficients in the camera intrinsic parameters to obtain the corrected image;

[0143] S122: Exposure control outputs a brightness-corrected image with stable brightness;

[0144] Obtain the corrected image, convert it to grayscale, and obtain the grayscale value I(i,j) of each pixel; calculate the average brightness of the image: Where w is the width of the corrected image; h is the length of the corrected image;

[0145] Preset low light threshold T low and high light threshold T high Compare the calculated average image brightness L with the low-light threshold T. low and high light threshold T high Size of L; if L <T low If the exposure time is sufficient, increase the exposure time; otherwise, decrease the exposure time.

[0146] Preset target brightness T 目标 The value is 128; the original exposure time is t. 原 Dark current reference value L was obtained from dark environment testing. 暗 If the value is 10, then the updated exposure time is t = t_0. 原 *(T 目标 -L 暗 ) / (LL dark). The corrected image outputs a brightness-stabilized image through exposure.

[0147] S123: Filtering to obtain a depth map;

[0148] The disparity of each pixel in the obtained brightness-corrected image is calculated using a stereo matching algorithm, then converted into a depth value to generate a depth map corresponding to the image resolution. The SGBM algorithm is used, with a matching window size of 9×9 pixels. For each pixel in the left image, a rectangular region surrounding it is taken as the matching window. In the same row of the right image, using the pixels in the left image as a reference, the window slides within a preset disparity range, calculating the SSD (sum of squared differences) of corresponding windows in the left and right corrected images. The position with the smallest SSD is taken as the matching point. The smaller d is used as the disparity of the pixel to obtain the disparity map D; the disparity range is set to 0 to 128 pixels; the disparity of each pixel in the disparity map is obtained, and pixels with a disparity of 0 or a disparity exceeding 128 are removed. If the disparity difference between adjacent pixels exceeds a threshold (e.g., 5 pixels), it is judged as noise and replaced with the average of the surrounding effective disparities. After filtering, a new disparity map is formed. Then, the depth value of each pixel in the new disparity map is calculated using the formula Z = fB / d, where f is the focal length of the left camera, specifically the average of the horizontal and vertical focal lengths of the left camera. B is the baseline distance, obtained through calibrated intrinsic and extrinsic parameters, and d is the disparity of a pixel. The depth map is obtained by calculating all depth values.

[0149] To eliminate coordinate deviations caused by lens distortion, the image brightness is stabilized at the target brightness of 128 by adjusting the exposure time, avoiding the loss of texture details due to brightness fluctuations (such as blurred glass curtain wall textures in low light or overexposure of reflections in bright light). This facilitates subsequent analysis, and the depth map obtained through filtering ensures that the depth map accurately reflects the actual distance between the obstacle and the drone, avoiding near-obstacle misjudgment or far-obstacle misjudgment due to depth errors.

[0150] S13: Point cloud data denoising and filtering;

[0151] S131: Point cloud segmentation and initial denoising;

[0152] First, perform pass-through filtering: read point cloud data, define the filtering object, set the filtering axis to the z-axis, and set the ground threshold; retain points in the point cloud data whose z-axis coordinates are greater than the ground threshold;

[0153] Then, range filtering is performed, a radar detection threshold is set, and the coordinates (xi, yi, zi) of each point in the retained point cloud data are obtained; the distance of each point from the origin is calculated: Points longer than the radar detection threshold are removed, and each frame of point cloud data is processed to obtain a pre-filtered point cloud. Through dual filtering, only point cloud data of effective obstacle areas are retained, reducing the data processing load.

[0154] S132: Obstacle clustering;

[0155] Using the Euclidean clustering algorithm with a cluster distance threshold of 0.3m, the initially filtered point cloud is divided into multiple independent clusters (each cluster corresponds to one obstacle), and the output cluster set {C1, C2, ..., C} is calculated. n}

[0156] First, obtain the point cloud after preliminary filtering, extract the 3D coordinates of each point, and convert the point cloud data into an N×3 matrix format; N is the number of points in the preliminary filtered point cloud; the pcl::KdTreeFLANN function from the PCL library is used. <pcl::pointxyz>The class takes a point cloud matrix as input, constructs a KD tree, and builds a point cloud spatial index, with the index ranging from 0 to N-1.

[0157] Next, the markers and containers are initialized. A boolean array `visited` is created, initially all values ​​are false, to mark whether a point has been assigned to a cluster. A cluster set container is created to store the final obstacle cluster set. Each point in the initially filtered point cloud is traversed. If the boolean array `visited` is false, the point has not been visited. Then, using this point as the seed point, a connected component search is initiated to generate a new cluster.

[0158] Create a queue, add the index of the currently unvisited point to the queue, set the boolean array visited for that point to true, and create a temporary vector, adding the coordinates of the point to the temporary vector;

[0159] When the queue is not empty, the following steps are executed in a loop: take the index q of the point at the head of the queue, search for neighboring points with a radius of 0.3m through the KD tree to obtain the index set, traverse the neighboring points, if the boolean array visited of the neighboring points is false, the neighboring point has not been visited, modify the boolean array visited of the neighboring points to true, mark it as visited, add the index of the neighboring point and the neighboring point to the queue and temporary vector respectively, and continue to expand the connected region; if the neighboring point has been visited, skip the point.

[0160] When the queue is empty, all points in the current connected region have been collected. The temporary vector is added to the cluster set container to complete the clustering of an obstacle.

[0161] Continue iterating through the next unvisited point, repeating the connected component search to generate a cluster, until the boolean array `visited` of all points in the initially filtered point cloud is true, forming a cluster set {C1, C2, ..., C} in the cluster set container. n }

[0162] S133: Kalman-Particle Hybrid Filtering for Noise Reduction;

[0163] For each obstacle cluster C i Filtering is performed on the point cloud separately, C i For the i-th obstacle in the cluster set, the point cloud position is first predicted by Kalman filtering, then the particle filter correction is activated based on the residual, and finally all the denoised clusters are merged to output the denoised point cloud.

[0164] Step 1: Initialize key parameters;

[0165] The initial sampling interval is Δt (sampling frequency 10Hz, therefore 0.1s), and the state transition matrix is... The control matrix B is a 3×3 identity matrix. The noise standard deviation is σ: 0.05m; the number of particles N is 200; and the residual threshold 3σ: 0.15m.

[0166] Step 2: Kalman filter prediction;

[0167] State initialization: An obstacle cluster C in the first frame i Predicted values ​​of point clouds Let the obstacle clustering of this frame be C. i The average coordinates of the point cloud;

[0168] State prediction: Clustering of obstacles C in frame t i The point cloud, combined with the predicted value from the previous frame Calculate the current predicted value based on the drone's speed u(t):

[0169] Where F is the state transition matrix; the UAV velocity u(t) is provided by the UAV IMU; u(t) = [v x ,v y ,v z ] T The unit is meters per second (m / s), where v x ,v y ,v z denoted as , and respectively as the instantaneous velocities of the UAV in the x, y, and z directions; B is the control matrix.

[0170] Step 3: Calculate the residual and determine whether particle filtering is triggered;

[0171] Calculate obstacle clustering C in the current frame i The average coordinates P(t) of the point cloud are calculated; the residual ||P(t) - P(t|t-1)|| is calculated, which is the distance between the average coordinates and the current predicted value; if the residual > 3σ, particle filtering is triggered; otherwise, the Kalman prediction value is directly output as the denoised point.

[0172] Step 4: Particle filter correction;

[0173] First, based on the predicted value Let the mean be σ = 0.05m and the standard deviation be 0.05m, and generate 200 Gaussian-distributed particles; let the coordinates of the particles be (x...). i ,y i ,z i );

[0174] Next, the distance d between each generated particle and the average coordinate P(t) is calculated. i According to the Gaussian function Assign weights to each particle; normalize the weights assigned to each particle to ensure that the sum of the weights of all particles is 1, thus obtaining the normalized weights. Where w i is the weight of the particle; N is the number of particles, 200.

[0175] Then, low-weight particles are eliminated, and high-weight particles are retained to obtain resampled particles; for example, particles with normalized weights less than 0.0001 are eliminated.

[0176] Finally, the resampled particles are fused to update the denoised point cloud;

[0177] Obtain the coordinates P of all resampled particles i ′(t), using the weighted average of the resampled particles as the denoised point cloud coordinates. Where n is the number of resampled particles; P i ′(t) represents the coordinates of the resampled particle; w i ' represents the weight normalization of resampled particles. in, is the original normalized weight of the particles after resampling; n is the number of resampled particles.

[0178] The process iterates through the point clouds of all obstacle clusters, updating the denoised point clouds for each cluster to obtain the denoised point cloud for the current frame. This process is repeated for all frames. Kalman-particle hybrid filtering is used for denoising to address the issues of dust and fog causing discrete noise points (such as single points deviating from clusters) in the LiDAR point cloud during low-altitude flight, or uneven point cloud density due to local occlusion. Small-amplitude random noise is filtered using Kalman filtering, followed by particle filtering with weighted averaging to calculate the denoised point cloud, addressing large-amplitude noise or missing point cloud issues. When dust causes localized point cloud loss, particle fusion is used to complete the effective point cloud, ensuring the completeness of obstacle outlines.

[0179] S2: Lightweight feature extraction;

[0180] The YOLOv5s network is used to quickly locate obstacles from binocular images and output feature vectors.

[0181] S21: Select the YOLOv5s network and train it using at least 5000 depth maps containing different static and dynamic obstacles, annotating the detection boxes and obstacle categories, etc., to obtain a YOLOv5s pre-trained model (lightweight version, the backbone network is a simplified version of CSPDarknet53, with about 7.5M parameters, suitable for edge computing devices); load the YOLOv5s pre-trained model and convert it to inference mode, with the preset input image size of 640×640 pixels, and set the confidence threshold (0.5) and non-maximum suppression (NMS) threshold (0.45) through the configuration file.

[0182] S22: Preprocess the input image; perform scaling and normalization on the obtained depth map image, and convert it into the model input format;

[0183] S23: Input the preprocessed input image into the model. The backbone network of YOLOv5s (a lightweight version of CSPDarknet53) generates feature maps of three scales: 80×80, 40×40, and 20×20 through convolutional layers, residual blocks, and CSP (cross-stage local connectivity) structures. For the feature maps of the three scales, the detection head predicts the position offset of each preset anchor box through convolutional layers. The confidence level of the target's existence and the class probability are output through the sigmoid function.

[0184] S24: First, filter candidate boxes with confidence scores below the threshold, then remove redundant boxes with overlap (IoU) scores above the threshold using non-maximum suppression (NMS), and output the bounding box coordinates (x, y, w, h), class probability p, and confidence score conf of the target; (x, y) are the coordinates of the center point of the bounding box, w is the width of the bounding box, and h is the height of the bounding box.

[0185] S25: Output feature vector, which transforms the output detection results into a standardized feature vector F = (x, y, w, h, pcls, conf).

[0186] This application only uses the YOLOv5s pre-trained model and does not make any technical innovations in the model training and algorithm. The training and algorithm working principle of the YOLOv5s pre-trained model will not be described in detail here.

[0187] S3: Obstacle fusion association;

[0188] After standardizing the point cloud and bounding box coordinates of each obstacle cluster, IoU matching is performed to associate the point cloud and bounding box of the obstacle cluster.

[0189] S31: Standardize the point cloud for obstacle clustering;

[0190] Obtain obstacle clustering C i Calculate obstacle clustering C i The smallest bounding box;

[0191] Obstacle clustering C i The denoised point cloud is obtained by taking the minimum and maximum values ​​of the X, Y, and Z axes, respectively. imax Y imax Z imax X imin Y imin Z imin Obstacle clustering C i The smallest bounding box is V ibox (X imin ,Y imin Z imin ,X imax ,Y imax Z imax );

[0192] S32: Standardize bounding box coordinates;

[0193] Obtain the bounding box coordinates (x, y, w, h) output by the YOLOv5s pre-trained model; calculate the left boundary pixel coordinates u1, right boundary pixel coordinates u2, upper boundary pixel coordinates v1, and lower boundary pixel coordinates v2 of the bounding box respectively;

[0194] The left and right boundary pixel coordinates u1, right and upper boundary pixel coordinates v1 and lower boundary pixel coordinates v2 are obtained using the formulas u1 = xw / 2, u2 = x + w / 2, v1 = yh / 2 and v2 = y + h / 2, respectively.

[0195] The depth of all pixels within the bounding box is obtained from the depth map, and the average depth is calculated.

[0196] Convert the bounding box into a 3D detection box according to the formula:

[0197] The minimum and maximum values ​​of the bounding box along the X, Y, and Z axes of the machine are calculated respectively, forming the three-dimensional detection box V. box =(X vmin ,Y vmin Z vmin ,X vmax ,Y vmax Z vmax ); where u0 is the horizontal pixel coordinate of the principal point (u0, v0), v0 is the vertical pixel coordinate of the principal point (u0, v0), and f x It is the focal length of a binocular camera in the horizontal direction. f Lx It is the focal length of the left camera in the horizontal direction, f Rx It is the focal length of the right camera in the horizontal direction, f. y It is the focal length of a binocular camera in the vertical direction. f Ly It is the focal length of the left camera in the vertical direction, f. Ry is the focal length of the right camera in the vertical direction; w is the width in the bounding box coordinates, and h is the height in the bounding box coordinates.

[0198] S33: IoU matching associated obstacles;

[0199] Based on obstacle clustering C i minimum bounding box i box With the 3D detection box V box The intersection-union ratio is used to match associations;

[0200] First, a threshold for the Intersection over Union (IoU) is preset; in this embodiment, it is 0.3.

[0201] Next, calculate obstacle clustering C. i minimum bounding box i box With the 3D detection box V box The intersection volume V1;

[0202] According to V1 = max(0,X) imax -X vmin )×max(0,Y imax -Y vmin )×max(0,Z imax -Z vmin The intersection volume is calculated.

[0203] Then calculate obstacle clustering C. i The smallest bounding box V ibox With the 3D detection box V box The union volume V2; obstacle-based clustering C i The smallest bounding box V ibox With the 3D detection box V box Calculate the volume V3 of the minimum bounding box and the volume V4 of the 3D detection box, respectively;

[0204] V3=(X imax -X imin )×(Y imax -Y imin )×(Z imax -Z imin );

[0205] V4 = (X vmax -X vmin )×(Y vmax -Y vmin )×(Z vmax -Z vmin );

[0206] The union volume V2 of the two can be calculated using the formula V2 = V3 + V4 - V1.

[0207] Finally, the crossover-union ratio was calculated. Compare the Intersection over Union (IoU) with a preset threshold. If the IoU is greater than the threshold, then cluster the obstacles into C groups. i Associate the target obstacle with the bounding box identified in the YOLOv5s output; otherwise, cluster the obstacle into C. i If the target obstacle does not match the bounding box identified in the YOLOv5s output, iterate through other obstacle clusters to match and associate the bounding boxes, and then associate the obstacle clusters with the bounding boxes; iterate through all frames and group the obstacles into clusters C. i All target obstacles identified in the bounding boxes output by YOLOv5s are matched and associated.

[0208] By associating obstacle fusion data with the bounding boxes output from the binocular data, the obstacles in the obstacle cluster are linked to avoid duplicate or missed calculations of the same obstacle. The binocular vision data is also linked with the upcoming radar point cloud, providing a basis for subsequent targeted dynamic obstacle trajectory prediction and enhancing the reliability of dynamic trajectory prediction. S4: Dynamic Obstacle Trajectory Prediction;

[0209] Based on the LSTM model, a denoised point cloud of obstacle clusters is generated by fusing the feature vectors output by dynamic obstacles with the bounding boxes associated with the feature vectors, and the future trajectory of dynamic obstacles is predicted; dynamic obstacles include birds, drones, etc.

[0210] S41: LSTM Model Construction and Training;

[0211] S411: STM model construction;

[0212] Construct an LSTM layer: Input dimension is 6 (state vector dimension), hidden layer dimension is 128, number of layers is 1, tanh activation function is used, and the hidden state h at each step is preserved. k It is used to capture the time dependence of obstacle movement (such as uniform speed, variable speed, turning and other movement patterns);

[0213] Construct a fully connected layer: Map the LSTM output to 3D (corresponding to future coordinates (X,Y,Z), and use a linear activation function.

[0214] Configure model structure parameters: set the time step to 10; set the prediction step size K to 15;

[0215] S412: Model training;

[0216] We collected 100,000 frames of dynamic obstacle data (including birds, drones, etc.) in low-altitude logistics scenarios. Each frame was labeled with real-world coordinates and divided into training, validation, and test sets in a 7:2:1 ratio. We then trained and validated the constructed LSTM model to obtain the training model.

[0217] Specifically, firstly, feature vectors are extracted from each frame of the training set via step S2.

[0218] Next, based on the category probability p, the obstacle category is determined (a preset probability threshold is used to determine whether the category probability exceeds the probability threshold). The obstacle clusters associated with the bounding boxes identified as dynamic obstacles are extracted, and the denoised point clouds within the smallest bounding box of the obstacle cluster are selected as real-world coordinates.

[0219] Then, the feature vectors of the past 10 frames in the training set are used as input, and the real-world coordinates of the next 15 frames are used as labels; the number of training epochs is set to 50; and the training is validated using a validation set. Training stops when the validation set loss does not decrease for 5 consecutive epochs, and the pre-trained fully connected layer weights W are saved. fc and fully connected layer bias b fc ; Obtain the training model.

[0220] S42: Trajectory prediction inference;

[0221] S421: Obtain input data;

[0222] Extract the state vector sequence of historical time steps; extract the feature vectors of the 10 frames before the current time to form the state vector sequence: F1, F2, F3, ..., F t The default value for t is 10, which corresponds to 10 frames of data within 1 second.

[0223] Extract the 3D coordinate sequence of the historical time-step denoised point cloud: P1, P2, P3, ..., P t As the real-world coordinates for each frame, the 3D world coordinates of a dynamic obstacle corresponding to each frame are P. k =(X k ,Y k Z k ).

[0224] S422: Trajectory Prediction;

[0225] 1) LSTM prediction obtains preliminary predicted coordinates; the state vector sequence of historical time steps is input into the pre-trained LSTM model, and the predicted coordinates for the next K steps are calculated using the following formula: K = (1, 2, ..., k, ..., 15);

[0226] h k =LSTM(F k ,h k-1 );

[0227] P LSTM (t+k)=W fc *h t +b fc ;where h k-1 The hidden state of the previous step, k is the specific step number, h is the hidden state of the previous step. t This refers to the hidden state after processing the historical state vector of frame t.

[0228] 2) Anomaly filtering;

[0229] Based on the coordinate range after denoising by the lidar (X,Y∈[-50,50]m, Z∈[0,30]m), outliers in the predicted coordinates that exceed this range are filtered out and replaced with the coordinates corrected in the previous step to ensure the rationality of the predicted trajectory.

[0230] 3) Output the prediction results;

[0231] Output the corrected predicted coordinates P1-P for the next K steps. K The output format is P = (P1, P2, ..., P...). K ).

[0232] S5: Obstacle avoidance path planning;

[0233] By combining the A* algorithm with the artificial potential field method, a smooth path is generated that avoids both static and dynamic obstacles.

[0234] S51: 3D raster environment modeling;

[0235] S511: Establish the grid coordinate system;

[0236] Establish a coordinate system with the UAV's current position S as the origin, the X-axis along the initial heading, the Y-axis perpendicular to the heading, and the Z-axis vertically upward; set the grid size to 0.5m*0.5m*0.5m.

[0237] The mapping relationship between raster index and world coordinates is 1.2:2; that is, world coordinates X = 1.2m, corresponding to raster index X... 格 The value is 2.

[0238] S512: Static obstacle marker;

[0239] Traverse the denoised point cloud in step S133, set the grid value corresponding to each static obstacle point to 1 (impassable), and set the remaining grids to 0 (passable).

[0240] S513: Dynamic obstacle marking;

[0241] Based on the predicted trajectory P, the predicted P for each frame is calculated through the mapping relationship. K The corresponding grid; these grids are set at the corresponding timestamp t k The inner mark is 1 (not passable).

[0242] S514: Output a 3D raster map (x,y,z,t) with a time dimension;

[0243] S52: A* algorithm for global path search;

[0244] Based on a grid map, the algorithm searches for the globally optimal path from the starting point S to the target point G, avoiding both static obstacles and dynamic obstacles within the next 1.5 seconds. The A* algorithm is a heuristic search algorithm used to find the shortest path from the starting point to the target point in a graph or grid. It efficiently finds the optimal path by comprehensively considering the cost of the paths already traversed and the estimated cost to the target.

[0245] S521: Obtain node attributes;

[0246] Each grid is a node; node attributes include coordinates and cost function.

[0247] The coordinates refer to the world coordinates of the center of the grid; let the coordinates of node n be (X... n ,Y n Z n );

[0248] Cost function: f(n) = g(n) + 1.2 × h(n), where g(n) is the actual distance from the starting point S to node n, and the cumulative distance along the searched path, in meters;

[0249] h(n) is the straight-line distance from node n to the target point;

[0250] S522: OpenList and CloseList Management;

[0251] OpenList stores nodes to be explored, initially containing only the starting point S; CloseList stores explored nodes, initially empty.

[0252] S523: Path search iteration;

[0253] Select the node n with the smallest f(n) from the OpenList. min Move to CloseList; if n min For the target point G, backtrack the parent node relationships of the nodes in CloseList to generate a global path and terminate the search; otherwise, traverse n min Given 8 adjacent grid cells (up, down, left, right, front, back, and diagonal in 3D space), calculate the cost function for each adjacent node. If an adjacent node is in the ClosedList or marked as 1 (impassable), skip it. If an adjacent node is not in the OpenList and its calculated cost function is smaller, update the adjacent node's g(n), h(n), and f(n). min Set it as its parent node and add it to OpenList; repeat until the target node G is found. Output the global path: [n0, n1, ..., n m (n0 = S, n) m =G), each node contains world coordinates and a corresponding timestamp.

[0254] S53: Local path optimization;

[0255] Artificial potential field method avoids short-term dynamic obstacles in real time.

[0256] S531: Calculation of attractive potential field;

[0257] For each node in the global path; let n be... i (X ni ,Y ni Z ni ); Calculate the target point G(X) G ,Y G Z G The resultant attraction force on the nodes;

[0258] According to the formula

[0259] S532: Calculation of repulsive potential field;

[0260] For node n i For all obstacles within a 3m radius, obtain the static point cloud and dynamic predicted trajectory points marked as 1 within the range; calculate the o-to-n pair for each obstacle. i The repulsive force.

[0261] Where d0 is the radius of influence of the obstacle, set at 3m; d (ni,o) For node n i The Euclidean distance to obstacle o, k1 is the repulsive potential coefficient, set to 5.0; n i -o is the vector pointing the node in the opposite direction to the obstacle;

[0262] S533: Total Resultant Force and Path Adjustment;

[0263] Compute node n i Total force F 总 =F 吸引 +F 排斥 Adjust the node position along the direction of the resultant force:

[0264] Where ΔS is the grid size, initially 0.5m, and ||F 总 || represents the modulus of the total force; n i For node n i After adjustment, check the new node n. i 'Is it passable? If not, decrease ΔS and readjust.'

[0265] S534: Mobility constraint check;

[0266] Calculate the angle θ between the lines connecting adjacent nodes after adjustment. If θ > 15°, then insert a transition node n. 过渡 The included angles between adjacent angles are all ≤15°.

[0267] S535: Output the locally optimized smooth path [n'0, n'1, ..., n'] m Each node contains adjusted world coordinates, timestamps, etc.

[0268] S54: Real-time path correction;

[0269] S541: Dynamic obstacle location update;

[0270] Every 0.1s (LiDAR sampling interval Δt = 0.1s), compare the latest predicted trajectory in step S324 with the original trajectory. If the deviation is >0.3m, then repeat steps S431-S434 to update the grid map and path.

[0271] S542: Emergency obstacle avoidance triggered;

[0272] If the lidar detects that the real-time distance between an obstacle and the drone is less than 3m, it immediately increases the repulsive potential coefficient k1 to 10.0, forcing the path to adjust away from the obstacle; the deflection angle = δ * 15 * (3 / real-time distance). The closer the distance, the greater the deflection.

[0273] S543: Update the output obstacle avoidance path.

[0274] By correcting the route, the system adapts to dynamic obstacle position deviations in real time, adjusts the relative position of the path and obstacles, avoids collision risks, and responds quickly to sudden close-range obstacles for emergency avoidance.

[0275] The present invention has been described in the above-described embodiments; however, these embodiments are merely examples for implementing the present invention. It must be noted that the disclosed embodiments do not limit the scope of the present invention. Conversely, any modifications and refinements made without departing from the spirit and scope of the present invention are within the scope of patent protection of the present invention.< / pcl::pointxyz>

Claims

1. A multi-source sensor obstacle recognition and avoidance method for low-altitude logistics, characterized in that: Includes the following steps: S1: Sensor data acquisition and preprocessing; Multiple sensors simultaneously collect data, and the collected data is preprocessed; The sensors include LiDAR and binocular cameras; S11: Simultaneous data acquisition from multiple sensors; The lidar collects point cloud data of obstacles in the environment at a fixed frequency; the left and right cameras of the binocular camera capture images simultaneously and output RGB images. S12: Binocular vision data preprocessing; The pixel coordinates of the RGB image are corrected based on the camera's intrinsic parameters to eliminate distortion. The corrected image is then used for exposure control to output a brightness-corrected image with stable brightness. Finally, the brightness-corrected image is filtered to obtain a depth map. S13: Point cloud data denoising and filtering; After performing point cloud segmentation and initial denoising on the point cloud data, several obstacle clusters are obtained from the initially filtered point cloud clusters. Kalman-particle hybrid filtering is used to denoise each obstacle cluster to obtain a denoised point cloud. S2: Lightweight feature extraction; A YOLOv5s pre-trained model is used to quickly locate obstacles from the depth map and output feature vectors. The feature vector includes the bounding box coordinates of the target, the class probability, and the confidence level; S3: Obstacle fusion association; After standardizing the denoised point cloud and bounding box coordinates of each obstacle cluster, IoU matching is performed to associate the denoised point cloud of the obstacle cluster with the bounding box. S4: Dynamic obstacle trajectory prediction; Based on the LSTM model, a denoised point cloud of obstacle clustering associated with the feature vectors output by dynamic obstacles and the bounding boxes of the feature vectors is fused to predict the future trajectory of dynamic obstacles. S5: Obstacle avoidance path planning; Based on denoised point clouds, and combining the A* algorithm with the artificial potential field method, a smooth path is generated to avoid static and dynamic obstacles.

2. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 1, characterized in that: Step S12 includes: S121: Correct the pixel coordinates of the RGB image based on the distortion coefficients in the camera intrinsic parameters to obtain the corrected image; S122: Exposure control outputs a brightness-corrected image with stable brightness; The corrected image is obtained, converted to grayscale, and the grayscale value of each pixel is obtained. The average brightness of the image is calculated. Low light threshold and high light threshold are preset. The calculated average brightness is compared with the low light threshold and high light threshold. If the average brightness is less than the low light threshold, the exposure time is increased; otherwise, the exposure time is decreased. The dark current reference value is obtained by using the preset target brightness, the original exposure time and dark environment test. The updated exposure time is calculated. The corrected image is a brightness correction image with stable brightness output through exposure. S123: Filtering to obtain a depth map; The disparity of each pixel in the obtained brightness-corrected image is calculated using a stereo matching algorithm, and then converted into a depth value to generate a depth map corresponding to the image resolution.

3. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 1, characterized in that: Step S13 includes: S131: Point cloud segmentation and initial denoising; First, perform pass-through filtering: read point cloud data, define the filtering object, set the filtering axis to the z-axis, and set the ground threshold; retain points in the point cloud data whose z-axis coordinates are greater than the ground threshold; Then, distance filtering is performed, a radar detection threshold is set, the coordinates of each point in the retained point cloud data are obtained, the distance of each point from the origin is calculated, points with a distance greater than the radar detection threshold are removed, each frame of point cloud data is processed, and the point cloud after preliminary filtering is obtained. S132: Obstacle clustering; Using the Euclidean clustering algorithm and setting a clustering distance threshold, the initially filtered point cloud is divided into multiple independent clusters, outputting a cluster set {C1, C2, ..., C}. n }; S133: Kalman-Particle Hybrid Filtering for Noise Reduction; Filtering is performed separately for the point cloud of each obstacle cluster. First, the point cloud position is predicted by Kalman filtering, and then the particle filtering correction is activated based on the residual. Finally, the denoised point cloud is output. Step 1: Initialize key parameters; Initialize the sampling interval, state transition matrix, control matrix, noise standard deviation, particle number, and residual threshold; Step 2: Kalman filter prediction; State initialization: An obstacle cluster C in the first frame i The predicted value of the point cloud is set as the obstacle clustering C of this frame. i The average coordinates of the point cloud; C i For the clustering of the i-th obstacle in the cluster set; State prediction: Clustering of obstacles C in frame t i The point cloud, combined with the predicted value from the previous frame Calculate the current predicted value based on the drone's speed u(t): Where F is the state transition matrix; the UAV velocity u(t) is provided by the UAV IMU; and B is the control matrix. Step 3: Calculate the residual and determine whether particle filtering is triggered; Calculate obstacle clustering C in the current frame i The average coordinates of the point cloud are calculated, and the distance between the average coordinates and the current predicted value is used to obtain the residual. When the residual is greater than the residual threshold, particle filtering is triggered; otherwise, the Kalman prediction value is directly output as the denoised point. Step 4: Particle filter correction; First, based on the predicted value The mean is 0.05m, and the noise standard deviation is 0.05m, generating 200 Gaussian distributed particles. Next, the distance d between each generated particle and the average coordinate is calculated. i ; Assign weights to each particle according to the Gaussian function; Normalize the weights assigned to each particle to obtain normalized weights; Then, low-weight particles are eliminated, and high-weight particles are retained to obtain resampled particles; Finally, the resampled particles are fused to update the denoised point cloud; Obtain the coordinates of all resampled particles, and use the weighted average of the resampled particles as the coordinates of the denoised point cloud to update the denoised point cloud of obstacle clustering. Iterate through the point clouds of all obstacle clusters, update the denoised point clouds of all obstacle clusters, obtain the denoised point cloud of the current frame, and iterate through all frames to obtain the denoised point cloud.

4. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 1, characterized in that: Step S2 includes: S21: Train the YOLOv5s network to obtain the YOLOv5s pre-trained model. Use the YOLOv5s pre-trained model and convert it to inference mode. The preset input image size is 640×640 pixels. Set the confidence threshold and non-maximum suppression threshold through the configuration file. S22: Preprocess the input image; scale and normalize the obtained depth map image, and convert it to the model input format; S23: Input the preprocessed input image into the model, and generate feature maps of three scales: 80×80, 40×40, and 20×20 through convolutional layers, residual blocks, and CSP structure; for the feature maps of the three scales, the detection head predicts the position offset of each preset anchor box through convolutional layers; and outputs the confidence score and class probability of the target's existence through the sigmoid function; S24: First, filter candidate boxes with confidence scores below the threshold, and then remove redundant boxes with overlap scores above the threshold through non-maximum suppression, and output the bounding box coordinates (x, y, w, h), class probability p, and confidence score conf of the target; S25: Output feature vector, which transforms the output detection results into a standardized feature vector F = (x, y, w, h, pcls, conf).

5. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 1, characterized in that: Step S4: S41: LSTM Model Construction and Training; Construct an LSTM layer: input dimension is 6, hidden layer dimension is 128, number of layers is 1, tanh activation function is used, and the hidden state h at each step is preserved. k This is used to capture the time dependency of obstacle motion; Constructing a fully connected layer: Mapping the LSTM output to 3D; using a linear activation function; Configure model structure parameters: set the time step to 10; set the prediction step size K to 15; S412: Model training; Collect dynamic obstacle data in low-altitude logistics scenarios; a total of 100,000 frames, each frame labeled with real-world coordinates, divided into training set, validation set and test set in a 7:2:1 ratio; train and validate the constructed LSTM model to obtain the training model; S42: Trajectory prediction inference; S421: Obtain input data; Extract the state vector sequence of historical time steps; extract the feature vectors of the 10 frames before the current time to form a state vector sequence; S422: Trajectory Prediction; 1) LSTM prediction obtains preliminary predicted coordinates; the state vector sequence of historical time steps is input into the pre-trained LSTM model to obtain preliminary predicted coordinates for the next K steps; 2) Anomaly filtering; Based on the coordinate range after denoising by the lidar, outliers in the predicted coordinates that exceed the coordinate range are filtered out and replaced with the coordinates corrected in the previous step. 3) Output the prediction results; Output the corrected predicted coordinates for the next K steps; The output format is P = (P1, P2, ..., P...). K ).

6. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 5, characterized in that: Step S412 is as follows: First, feature vectors are extracted from each frame of the training set in step S2; Next, based on the category probability p, the obstacle category is determined, and the obstacle clusters associated with the bounding boxes identified as dynamic obstacles are extracted. The denoised point cloud in the obstacle cluster is then selected as the real-world coordinates. Then, the feature vectors of the past 10 frames in the training set are used as input, and the real-world coordinates of the next 15 frames are used as labels; the number of training epochs is set to 50; and the training is validated using a validation set. Training stops when the validation set loss does not decrease for 5 consecutive epochs, and the pre-trained fully connected layer weights W are saved. fc and fully connected layer bias b fc ; Obtain the training model.

7. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 1, characterized in that: Step S5 includes: S51: 3D raster environment modeling; S511: Establish the grid coordinate system; A coordinate system is established with the UAV's current position S as the origin, the X-axis along the initial heading, the Y-axis perpendicular to the heading, and the Z-axis vertically upward; the mapping relationship between the raster index and the world coordinates is 1.2:

2. S512: Static obstacle marker; Traverse the denoised point cloud in step S133, set the grid value corresponding to each static obstacle point to 1, and set the rest of the grids to 0; 1 means impassable, 0 means passable. S513: Dynamic obstacle marking; Based on the predicted trajectory P, the predicted P for each frame is calculated through the mapping relationship. K The corresponding grid; these grids are set at the corresponding timestamp t k The internal marker is 1. S514: Output a 3D raster map with a time dimension; S52: A* algorithm for global path search; S521: Obtain node attributes; Each grid is a node; node attributes include coordinates and cost function f(n); The coordinates refer to the world coordinates of the center of the grid; let the coordinates of node n be (X... n ,Y n Z n Cost function: f(n) = g(n) + 1.2 × h(n), where g(n) is the actual distance from the starting point S to node n, and the cumulative distance along the searched path; h(n) is the straight-line distance from node n to the target point. S522: OpenList and CloseList Management; OpenList stores nodes to be explored, initially containing only the starting point S; CloseList stores explored nodes, initially empty. S523: Path search iteration; Select the node n with the smallest f(n) from the OpenList. min Move to CloseList; if n min For the target point G, backtrack the parent node relationships of the nodes in CloseList to generate a global path and terminate the search; otherwise, traverse n min Calculate the cost function for each of the 8 adjacent grids. If an adjacent point is in the ClosedList or marked as impassable (1), skip it. If an adjacent point is not in the OpenList and its calculated cost function is smaller, update the adjacent point's g(n), h(n), and f(n), and set n... min Set it as the parent node and add it to the OpenList; Repeat the process until the target point G is found; output the global path. S53: Local path optimization; S531: Calculation of attractive potential field; For each node in the global path; let n be... i (X ni ,Y ni Z ni ); Calculate the target point G(X) G ,Y G Z G The resultant force of attraction on the nodes; According to the formula S532: Calculation of repulsive potential field; For node n i For all obstacles within a 3m radius, obtain the static point cloud and dynamic predicted trajectory points marked as 1 within the range; calculate the o-to-n pair for each obstacle. i The repulsive force; Where d0 is the radius of influence of the obstacle, set at 3m; d (ni,o) For node n i The Euclidean distance to obstacle o, k1 is the repulsive potential coefficient, set to 5.0; n i -o is the vector pointing the node in the opposite direction to the obstacle; S533: Total Resultant Force and Path Adjustment; Compute node n i Total force F 总 =F 吸引 +F 排斥 Adjust the node position along the direction of the resultant force: Where ΔS is the grid size, initially 0.5m, and ||F 总 || represents the modulus of the total force; n i For node n i After adjustment, check the new node n. i 'Is it passable? If not, decrease ΔS and readjust.' S534: Mobility constraint check; Calculate the angle θ between the lines connecting adjacent nodes after adjustment. If θ > 15°, then insert a transition node n. 过渡 Such that the included angles of adjacent angles are all ≤15°; S535: Output the locally optimized smooth path [n'0, n'1, ..., n'] m Each node contains adjusted world coordinates and a timestamp.

8. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 7, characterized in that: Step S5 also includes: S54: Real-time path correction; S541: Dynamic obstacle location update; Every 0.1 seconds, compare the latest predicted trajectory in step S424 with the original trajectory. If the deviation is >0.3m, repeat steps S531-S534 to update the raster map and path. S542: Emergency obstacle avoidance triggered; If the lidar detects that the real-time distance between an obstacle and the drone is less than 3m, it immediately increases the repulsive potential coefficient k1 to 10.0, forcing the path to adjust away from the obstacle; deflection angle = δ * 15 * (3 / real-time distance); S543: Update the output obstacle avoidance path.

9. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 2, characterized in that: Step S123 is as follows: The SGBM algorithm is used, with a matching window size of 9×9 pixels. For each pixel in the left image, the surrounding rectangular area is taken as the matching window. On the same row of the right image, using the pixels in the left image as a reference, the window slides within a preset disparity range, and the SSD of the corresponding window in the left and right corrected images is calculated. The position with the smallest SSD is taken as the matching point, and the disparity d with the smallest SSD is taken as the disparity of that pixel, resulting in a disparity map D. The disparity range is set to 0 to 128 pixels. The disparity of each pixel in the disparity map is obtained, and pixels with a disparity of 0 or a disparity exceeding 128 are removed. If the disparity difference between adjacent pixels exceeds a threshold, it is judged as noise and replaced with the mean of the surrounding effective disparities. After filtering, a new disparity map is formed. The depth value of each pixel in the new disparity map is then calculated using the formula Z = fB / d, where f is the focal length, B is the baseline distance obtained through pre-calibrated intrinsic and extrinsic parameters, and d is the disparity of the pixel. All depth values ​​are calculated to obtain the depth map.

10. The multi-source sensor obstacle recognition and avoidance method for low-altitude logistics as described in claim 2, characterized in that: Step S3 specifically includes: S31: Standardize the point cloud for obstacle clustering; Obtain obstacle clustering C i Obstacle clustering C is calculated. i minimum bounding box i box ; S32: Standardize bounding box coordinates; Obtain the bounding box coordinates (x, y, w, h) output by the YOLOv5s pre-trained model; calculate the left boundary pixel coordinates u1, right boundary pixel coordinates u2, upper boundary pixel coordinates v1, and lower boundary pixel coordinates v2 of the bounding box respectively; The depth of all pixels within the bounding box is obtained from the depth map, and the average depth is calculated. Convert the bounding box to a 3D detection box to obtain the 3D detection box: V box =(X vmin ,Y vmin Z vmin ,X vmax ,Y vmax Z vmax ); S33: IoU matching associated obstacles; Based on obstacle clustering C i minimum bounding box i box With the 3D detection box V box The intersection-union ratio (IoU) is used for matching and association. The IoU is compared with a preset threshold. If it is greater than the threshold, the obstacles are clustered into C. i Associate the target obstacle with the bounding box identified in the YOLOv5s output; otherwise, cluster the obstacle into C. i If the target obstacle does not match the bounding box identified in the YOLOv5s output, iterate through other obstacle clusters to match and associate the bounding boxes, and then associate the obstacle clusters with the bounding boxes; iterate through all frames and group the obstacles into clusters C. i All target obstacles identified in the bounding boxes output by YOLOv5s are matched and associated.

Images