Low-altitude dynamic SLAM method based on RGB-D image and storage medium

Abstract

The invention relates to a low-altitude dynamic SLAM method based on an RGB-D image and a storage medium. The low-altitude dynamic SLAM method comprises the following steps: acquiring an environment RGB image and a depth image shot by an RGB-D camera; performing target detection and multi-target tracking on the RGB image to obtain a dynamic target area; extracting all feature points of the RGB image, and removing feature points in the dynamic target area to obtain feature points of a static environment; estimating the pose of the unmanned aerial vehicle in combination with the feature points of the static environment and the depth image, and constructing a static map; performing point cloud processing and surrounding processing on the dynamic target to obtain position information, size information and speed information of the dynamic target; and combining the position information, the size information and the speed information of the dynamic target with the static map to construct an overall environment map. The low-altitude dynamic SLAM method has high-precision positioning, the limitation of a traditional positioning technology in a complex environment is avoided, the positioning precision of the unmanned aerial vehicle in complex environments such as urban low altitude is improved, and reliable guarantee is provided for safe and stable flight of the unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles (UAVs), and relates to the construction of an environmental map of a UAV, in particular to a low-altitude dynamic SLAM method and storage medium based on RGB-D images. Background Art

[0002] With the gradual opening up of the low-altitude airspace management system and the large-scale application of drone technology, urban low-altitude airspace is becoming the core operating space for new business formats such as drone logistics and distribution, emergency rescue, and urban inspections. However, the heterogeneous complexity of urban low-altitude environments, such as dense buildings, high-frequency drone traffic flows, and sudden dynamic obstacles, leads to the following problems in traditional drone environmental perception systems:

[0003] 1. GPS / INS-based navigation systems are subject to the risk of positioning drift in building-blocked environments, resulting in high positioning errors. Furthermore, existing environmental perception methods are mostly designed for outdoor barrier-free environments or environments with sparse obstacles, and their accuracy in processing position information changes of dynamic targets is insufficient.

[0004] 2. Traditional perception methods adopt measures such as not detecting or excluding multiple dynamic targets after detection to improve their own positioning accuracy. However, in urban low-altitude environments with multiple dynamic targets, this perception method is difficult to meet the obstacle avoidance needs of drones in urban low-altitude environments. The perception ability of dynamic targets is poor and cannot provide effective support for subsequent dynamic obstacle avoidance.

[0005] To this end, we propose a low-altitude dynamic SLAM method and storage medium based on RGB-D images to solve the above problems. Summary of the Invention

[0006] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a low-altitude dynamic SLAM method and storage medium based on RGB-D images with high detection accuracy and strong perception capability.

[0007] In order to solve the above problems, the technical solution of the present invention is:

[0008] A low-altitude dynamic SLAM method based on RGB-D images, comprising the following steps:

[0009] Get the environment RGB image and depth image taken by the RGB-D camera;

[0010] Perform target detection and multi-target tracking on RGB images to obtain dynamic target areas;

[0011] Extract all feature points of the RGB image, remove the feature points in the dynamic target area, and obtain the feature points of the static environment;

[0012] Combine the feature points and depth images of the static environment to estimate the drone's pose and build a static map;

[0013] Perform point cloud processing and encirclement processing on dynamic targets to obtain the position information, size information and speed information of dynamic targets;

[0014] The overall environment map is constructed by combining the position information, size information, speed information of dynamic targets and static maps.

[0015] In a further embodiment, target detection and multi-target tracking are performed on the RGB image to obtain a dynamic target area, specifically including:

[0016] Generate target detection dataset by combining RGB images and simulation environment;

[0017] Input the target detection data set into the target detection network for training to obtain the target detection model;

[0018] Output target detection results through the target detection model;

[0019] The target detection results are input into the multi-target tracking algorithm, and the previous and next frames of the target detection results are associated to obtain the dynamic target area.

[0020] In a further embodiment, the multi-target tracking algorithm adopts the BoT-SORT algorithm, and adopts the YoloV11 algorithm as the target detector of the BoT-SORT algorithm to output the semantic information and geometric information of the dynamic target area bounding box.

[0021] In a further embodiment, extracting all feature points of the RGB image, removing feature points in the dynamic target area, and obtaining feature points of the static environment specifically includes:

[0022] Perform global feature point detection on the RGB image and extract all feature points of the scene in the RGB image;

[0023] Combine the semantic information and geometric information of the dynamic target area bounding box to obtain the feature points within the dynamic area;

[0024] The feature points in the dynamic area are eliminated from all the feature points, and the remaining feature points are used as the feature point set of the static environment.

[0025] In a further embodiment, the semantic information and geometric information of the dynamic target region bounding box are combined to obtain feature points within the dynamic region, specifically including:

[0026] Get the imaging plane of the RGB-D camera in the previous frame and the current frame, and select a feature point in the 3D environment;

[0027] Set the feature point, the midpoint of the previous frame imaging plane, and the midpoint of the current frame imaging plane to be coplanar as the epipolar plane;

[0028] Connect the midpoint of the previous frame imaging plane and the midpoint of the current frame imaging plane, and set the intersection of the connecting line with the previous frame imaging plane and the current frame imaging plane as the pole;

[0029] Set the intersection line of the previous frame imaging plane, the current frame imaging plane and the epipolar plane as the epipolar line;

[0030] Obtain the projection position of the feature point on the imaging plane of the previous frame and the projection position of the feature point on the imaging plane of the previous frame and the current frame respectively;

[0031] Obtain the distance between the projection position of the feature point on the current frame imaging plane and the epipolar line, and compare it with the error threshold. If the distance is greater than the error threshold, the feature point is judged to be a feature point in the dynamic target area.

[0032] In a further embodiment, the pose of the UAV is estimated by combining the feature points and depth image of the static environment to construct a static map, which specifically includes:

[0033] Extract feature points from RGB images frame by frame and output target detection results;

[0034] Eliminate the feature points in the dynamic target area within the target detection result, retain the remaining feature points as the feature points of the static environment, and match the static environment feature points of the previous frame and the current frame;

[0035] Obtain depth information through the depth image, and combine the two sets of matched feature points corresponding to the previous frame and the current frame to obtain two sets of 3D feature points;

[0036] Perform Euclidean transformation on the two sets of 3D feature points to obtain the pose estimate of the UAV.

[0037] In a further embodiment, feature point extraction includes Oriented FAST key points and BRIEF descriptors, wherein the Oriented FAST key points are used to characterize corner points in the RGB image, and the BRIEF descriptors are used to characterize key point neighborhood features.

[0038] In a further embodiment, the pose of the UAV is estimated by combining the feature points and depth image of the static environment to construct a static map, which specifically includes:

[0039] Based on the pinhole camera imaging model, the depth image is converted into three-dimensional point cloud data through back-projection transformation;

[0040] Combined with the pose estimation of the UAV, all three-dimensional point data are converted to the global coordinate system, and an octree map is generated as a static map.

[0041] In a further embodiment, point cloud processing and encirclement processing are performed on the dynamic target to obtain the position information, size information and speed information of the dynamic target, specifically including:

[0042] Convert dynamic targets into 3D point cloud representations based on depth information;

[0043] The Euclidean clustering algorithm is used to spatially divide the point cloud representation, and the adjacent points are classified into the same category according to the point distance threshold to form several point cloud clusters;

[0044] Perform geometric encapsulation on several point cloud clusters and use the axis-aligned bounding box algorithm to generate the minimum circumscribed cube for each point cloud cluster;

[0045] By traversing the vertices of the point cloud cluster, the coordinates of the two most representative vertices are selected to define the target space position and obtain the size of the point cloud cluster;

[0046] The speed information of the dynamic target is obtained through the position distance and time difference between the previous frame and the current frame.

[0047] A storage medium stores a computer program, which, when executed by a processor, implements a low-altitude dynamic SLAM method based on RGB-D images.

[0048] Compared with the prior art, the present invention has the following beneficial effects:

[0049] This low-altitude dynamic SLAM method offers high-precision positioning, overcoming the limitations of GPS / INS navigation systems, which suffer from drift and high errors in building-obstructed environments. By acquiring image information from an RGB-D camera, combined with target detection, multi-target tracking algorithms, and feature point extraction and matching techniques, and calculating the drone's attitude using static environmental feature points, this method effectively overcomes the limitations of traditional positioning technology in complex environments, significantly improving the drone's positioning accuracy in complex environments such as low-altitude urban areas, and providing reliable assurance for safe and stable flight.

[0050] 2. This low-altitude dynamic SLAM method features highly efficient dynamic target perception. Unlike traditional perception methods that fail to detect dynamic targets or exclude them after detection, this method accurately captures dynamic target areas and generates three-dimensional point clouds, performs Euclidean clustering, and performs AABB bounding box processing on them, thereby precisely calculating the position and velocity information of dynamic targets. This gives drones powerful perception capabilities when facing multiple dynamic targets in low-altitude urban environments, providing detailed and accurate data support for subsequent dynamic obstacle avoidance. This significantly improves the drone's obstacle avoidance success rate in complex dynamic environments and greatly enhances its safety and adaptability in low-altitude urban environments.

[0051] 3. This low-altitude dynamic SLAM method features comprehensive map construction. By converting depth maps into 3D point clouds to generate point cloud maps and constructing octree maps as static maps, it can fully and accurately integrate dynamic targets with static maps. Furthermore, dynamic targets are processed and represented separately, so that the entire map contains detailed information about the static environment while accurately reflecting the status of dynamic targets. This creates a complete, dynamic, and accurate environmental map system, providing comprehensive and reliable environmental information reference for UAV path planning and mission execution.

[0052] 4. This low-altitude dynamic SLAM method has a wide range of environmental adaptability and fully considers complex factors such as building shielding and numerous dynamic targets in urban low-altitude environments. Compared with existing environmental perception methods designed for barrier-free or sparse obstacle environments in the wild, it can better adapt to the complex and dynamic environment of urban low-altitude environments. It has demonstrated excellent performance in positioning, dynamic target perception, and map construction, laying a solid technical foundation for the widespread application of drones in urban low-altitude environments, and has significant practical value and promotion significance. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 This is a flowchart of a low-altitude dynamic SLAM method based on RGB-D images;

[0054] Figure 2 This is a schematic diagram of a low-altitude dynamic SLAM method based on RGB-D images;

[0055] Figure 3 A schematic diagram of dynamic target perception results of a low-altitude dynamic SLAM method based on RGB-D images;

[0056] Figure 4 This is a schematic diagram of the BoT-SORT algorithm structure of a low-altitude dynamic SLAM method based on RGB-D images;

[0057] Figure 5 Schematic diagram of epipolar geometry for a low-altitude dynamic SLAM method based on RGB-D images;

[0058] Figure 6 Schematic diagram of the epipolar geometry of a dynamic target in a low-altitude dynamic SLAM method based on RGB-D images;

[0059] Figure 7 It is a static environment octree map for low-altitude dynamic SLAM method based on RGB-D images;

[0060] Figure 8 Schematic diagram of a pinhole camera imaging model for a low-altitude dynamic SLAM method based on RGB-D images;

[0061] Figure 9 A schematic diagram of the octree map construction principle of a low-altitude dynamic SLAM method based on RGB-D images;

[0062] Figure 10 Schematic diagram of an axis-aligned bounding box algorithm for a low-altitude dynamic SLAM method based on RGB-D images;

[0063] Figure 11 A schematic diagram of the effect of a low-altitude dynamic SLAM method based on RGB-D images;

[0064] Figure 12 A schematic diagram of the estimated trajectory of a UAV using a low-altitude dynamic SLAM method based on RGB-D images;

[0065] Figure 13 Schematic diagram of dynamic target error of a low-altitude dynamic SLAM method based on RGB-D images;

[0066] Figure 14 Schematic diagram of dynamic target estimation trajectory of a low-altitude dynamic SLAM method based on RGB-D images. DETAILED DESCRIPTION

[0067] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, features defined as "first", "second", etc. may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0068] Example 1:

[0069] A low-altitude dynamic SLAM method based on RGB-D images, such as Figures 1 to 14 As shown, the following steps are included:

[0070] S101. Obtain the environment RGB image and depth image captured by the RGB-D camera:

[0071] An RGB-D camera mounted on a drone captures RGB and depth images of the environment. These images support both real-time sensor and local dataset input. In real-time sensor mode, a real-time data interface based on the ROS robot operating system is constructed, enabling simultaneous multimodal data capture through topic subscriptions. The raw data is converted from ROS messages to OpenCV matrices using the cv_bridge toolkit, generating time-aligned color-depth image pairs for input into the algorithm processing pipeline. In local dataset mode, offline data analysis is performed, and a standardized dataset interface specification is designed. The input dataset requires a configuration file containing timestamps, file paths, and standard sensor parameters. The input data must meet the following specifications: color images must use a 640×480 resolution and 8UC3 encoding format, while depth images must match a 640×480 resolution and 16UC1 encoding format.

[0072] S103: Target detection and multi-target tracking are performed on the RGB image to obtain a dynamic target area:

[0073] Based on the AirSim simulation platform, a low-altitude urban environment simulation scenario was constructed. By collecting environmental data generated by the simulation environment and combining it with publicly available online real-world drone low-altitude flight data and synthetic drone imagery of urban streets, the environmental data was annotated using the LabelImg annotation tool. Ultimately, a target detection dataset for complex low-altitude urban environments was constructed. Preferably, dynamic targets include drones.

[0074] The constructed target detection dataset is input into the target detection network for training to obtain a target detection model suitable for this scenario; the target detection model adopts the YoloV11 (You Only Look Once, YOLO) model. The YoloV11 model mainly consists of four modules: input end, backbone network, neck, and head. It can uniformly scale the input image to a uniform size, extract features, and fuse multi-layer feature information; input an image of any size through the input end, and scale the image to a uniform size of 640×640 pixels and input it into the backbone network; the backbone network is composed of different convolutional blocks to enhance the network's learning ability and extract different feature information; the neck fuses the feature information, increases the receptive field, and makes the model adaptable to images of different resolutions; the head uses three detection heads of large, medium, and small sizes to decouple the neck information and finally output the target detection results.

[0075] The output target detection results contain three elements: target category recognition results, bounding box position coordinates and detection confidence. To improve the accuracy of subsequent processing, only valid detection results with confidence higher than the preset threshold are filtered, such as Figure 3As shown, the output dynamic target is a UAV, and the confidence levels are 0.82, 0.82, and 0.80, respectively. Preferably, the preset confidence threshold is 0.8.

[0076] The output target detection results are input into the multi-target tracking algorithm to achieve the correlation between the previous and next frames of the detection results; Figure 4 As shown in the figure, the multi-target tracking algorithm adopts the BoT-SORT algorithm. This algorithm realizes inter-frame target association by reusing the output results of the target detection module, and comprehensively uses Kalman filter prediction and IoU matching technology to assign a unique ID to each independent target. The algorithm integrates a camera motion compensation module to effectively suppress the tracking interference caused by camera motion. For targets that are lost or newly appearing, the system uniformly clears the ID, that is, assigns 0 to them, providing a data basis for subsequent target motion trajectory analysis.

[0077] The BoT-SORT algorithm is a tracking algorithm based on target detection and uses the YoloV11 algorithm as the target detector, replacing the YOLOX algorithm target detector in the original BoT-SORT algorithm. The BoT-SORT algorithm includes a camera motion compensation module, which significantly improves the accuracy and stability of the algorithm's tracking. Because this tracking algorithm uses the target detection result box to perform IoU matching between the previous and next frames for data association, without motion compensation, the detection boxes of the same target in the previous and next frames may have large deviations, resulting in IoU matching failures, thus affecting the accuracy of the tracking effect. Although the algorithm also uses methods such as Kalman filtering and Re-ID to perform secondary data association and matching, they often increase the computational burden, especially Re-ID, which refers to the problem of re-identifying the same target at different times or different perspectives, mainly based on the similarity calculation of the target's appearance. In theory, Re-ID can significantly improve tracking performance, but it also increases the demand for computing resources. Therefore, the present invention disables the Re-ID function of the BoT-SORT algorithm to ensure more efficient real-time processing.

[0078] S105: Extract all feature points of the RGB image, remove the feature points in the dynamic target area, and obtain the feature points of the static environment:

[0079] Feature points of the entire environment are extracted and the feature points of the dynamic target feature area are removed based on semantic information and geometric information to obtain the feature points of the static environment; the feature points of the static environment include performing global feature point detection on the input RGB image to extract all feature points in the scene; combining the semantic information and geometric information of the dynamic target bounding box output by the target detection YoloV11 network module, the feature points located in the dynamic area are eliminated, and the remaining feature points are retained as the feature set representing the static environment.

[0080] Dynamic area feature points are constrained by epipolar geometry. Epipolar geometry is usually a way to estimate camera motion based on two sets of 2D points, which expresses the geometric relationship between imaging information at two different positions during the motion process, such as Figure 5 shown. Figure 5 Point P in the equation is a point in the three-dimensional environment, O1 and O2 represent the center position of the camera in the previous frame and the current frame respectively, I1 and I2 represent the imaging planes of the camera in the previous frame and the current frame respectively, and p1 and p2 are the imaging points of point P on I1 and I2; at this time, points P, O1, and O2 are coplanar, which is set as the epipolar plane; the line connecting O1 and O2 intersects with planes I1 and I1 at points e1 and e2, which are set as poles; the epipolar plane intersects with imaging planes I1 and I2 at lines l1 and l2, which are set as epipolar lines; the camera motion from the previous frame to the current frame is set to rotation R and translation t. If point P does not move, then it is easy to conclude that point p2 should be on the epipolar line l2 based on the projection relationship, as shown in formula (1):

[0081]

[0082] Where x1 and x2 are the coordinates of points p1 and p2 on the I1 and I2 planes, respectively, and E is the essential matrix, which can be quickly solved using the eight-point method. Formula (1) is an ideal case, where point P is a static point and the essential matrix E is very accurate. In non-ideal or actual situations, there is a certain error, so an error threshold is set as the judgment standard.

[0083] like Figure 6 As shown, when point P is a dynamic point, there will be a large distance between the projection point p2 and the epipolar line l2; point P' in the figure is the position of point P after movement, and p3 and p4 are the imaging points of P' on I1 and I2 respectively; let the coordinates corresponding to points p1 and p2 be shown in formula (2):

[0084] p1=[x1,y1,1],p2=[x2,y2,1] (2)

[0085] Then the epipolar line l2 of the current frame can be calculated by the essential matrix E or the fundamental matrix F, as shown in formula (3):

[0086]

[0087] Where X, Y, Z represent line vectors;

[0088] According to formula (1), the epipolar constraint is expressed by formula (4):

[0089]

[0090] Define point p iThe distance between (i=2,4) and the corresponding epipolar line is the offset distance d, as shown in formula (5):

[0091]

[0092] If point P is a static point, the result of formula (5) is 0, that is, in an ideal situation, point p2 should be on the epipolar line l2. However, in actual situations, affected by various error factors, the distance d2 is often greater than zero, but lower than the error threshold. If point P is a dynamic point, that is, it moves to P' in the current frame, then the projected point should be p4. At this time, the distance d4 calculated based on formula (5) should be greater than 0 and greater than the error threshold. Based on this principle, the distance d4 is compared with the error threshold. If the distance d4 is greater than the error threshold, the feature point is determined to be a dynamic point and is eliminated in the subsequent steps.

[0093] The strategy for removing dynamic target feature points is shown in Table 1:

[0094] Table 1 Elimination strategy

[0095]

[0096]

[0097] S107: Combine the feature points and depth image of the static environment to estimate the drone's position and construct a static map:

[0098] For drone pose estimation:

[0099] First, ORB feature points are extracted from the input RGB image frame by frame. This feature is composed of Oriented FAST keypoints and BRIEF descriptors. Oriented FAST keypoints are the "corner points" in the image, which are highly robust to illumination, simple to calculate, and have good real-time performance. BRIEF descriptors use binary encoding to represent the neighborhood features of keypoints, providing an efficient similarity metric for subsequent cross-frame feature matching.

[0100] Secondly, the target detection results filter out feature points within the dynamic target frame, retaining only static features for matching. The BRIEF descriptor uses binary encoding to represent the neighborhood features of key points. By counting the differences in the binary bits of two descriptors and calculating the Hamming distance, it can efficiently determine feature similarity and determine whether feature points are the same point. Furthermore, combined with the Fast Library for Approximate Nearest Neighbors (FLANN) algorithm, a local matching topology is constructed to narrow the search space, reduce computational complexity, and significantly improve real-time performance.

[0101] Finally, based on the two sets of matched feature points corresponding to the previous and next frames, and by obtaining the depth information through the depth map, we can obtain two sets of 3D feature points and solve the Euclidean transformation R,t by formula (6):

[0102]

[0103] Where i ranges from 1 to n, and p i is the i-th feature point of the current frame, p′ i is the i-th feature point corresponding to the previous frame, R is the rotation matrix, and t is the translation matrix. This problem is solved using the ICP (Iterative Closest Point) algorithm:

[0104] 1. Calculate the centroid position p of the two groups of points i , p′ i , then calculate the centroid coordinate q of each point i , as shown in formula (7):

[0105] q i =p i -p (7)

[0106] 2. Calculate the rotation matrix according to the optimization problem of formula (8):

[0107]

[0108] 3. Calculate t based on the rotation matrix R:

[0109] t*=p-Rp′ (9)

[0110] The pose estimation result of the UAV can be obtained based on the solved rotation matrix R and translation matrix t.

[0111] For building static maps:

[0112] Based on the pinhole camera imaging model, the acquired depth image is converted into 3D point cloud data through back-projection transformation; combined with the drone pose estimation results, all point cloud data are converted to the global coordinate system and organized using the octree data structure; the resulting octree map is used as the static environment reference model of the system, such as Figure 7 shown.

[0113] Pinhole camera imaging model Figure 8 As shown in the figure, O is the optical center of the camera, which is also the pinhole in the pinhole model. Point P in real space is projected through the pinhole O and falls on the physical imaging plane O'-x'-y', with the imaging point being P'. Let the coordinates of point P be [X, Y, Z] T , P' coordinates are [X', Y', Z'] T, and the distance from the physical imaging plane to the pinhole is f (focal length); then according to the relationship between similar triangles, we can get formula (10):

[0114]

[0115] The negative sign indicates that the image is inverted. Formula (10) describes the spatial relationship between the point P and the pixel P' in space, with units of meters. However, since the image is not in real scale but in pixels, sampling and quantization are required on the imaging plane to obtain the pixel coordinates [u, v] of P'. T The pixel coordinate system usually assumes that the origin o' is in the upper left corner of the image, the u axis is facing right and parallel to the x axis, and the v axis is facing downward and parallel to the y axis. The difference between the pixel coordinate system and the imaging plane is a scaling and a translation of the origin. Suppose the pixel coordinate is scaled α times on the u axis and β times on the v axis, and the origin is translated [c x ,c y ] T The relationship between the coordinates of P' and the pixel coordinates is shown in formula (11):

[0116]

[0117] Substituting formula (10) into formula (11) yields formula (12):

[0118]

[0119] Where, the unit of f is meter, the units of α and β are pixels / meter, and f x 、f y 、c x 、c y The unit is pixels.

[0120] Arranging formula (10), formula (11), and formula (12) yields formula (13):

[0121]

[0122] Where matrix K is the intrinsic parameter of the camera. Based on formula (13), each pixel in the depth map can be deduced from its coordinates in three-dimensional space to generate the corresponding three-dimensional point cloud.

[0123] The generated three-dimensional point cloud is based on the camera coordinate system, combined with the drone's own rotation matrix R and translation vector t, and the coordinates P of each point in the world coordinate system are calculated by formula (14): w :

[0124]

[0125] Octree map such as Figure 9 As shown, the octree recursively divides the three-dimensional space into eight sub-regions and organizes these sub-spaces through a hierarchical tree structure. Each node represents a cubic region, and its children correspond to smaller-scale spatial regions until a predetermined accuracy or actual objects in the space are achieved. This recursive partitioning strategy enables the octree to efficiently represent large-scale three-dimensional environments and supports fast access, query, and update of objects in the space. In addition, the octree can dynamically adjust resolution and accuracy, providing higher accuracy in dense areas while reducing unnecessary storage in sparse areas, further improving computational efficiency.

[0126] S109: Perform point cloud processing and encirclement processing on the dynamic target to obtain the position information, size information, and speed information of the dynamic target:

[0127] First, the dynamic target is converted into a three-dimensional point cloud representation based on depth information. Then, the point cloud is spatially divided using the Euclidean clustering algorithm, and neighboring points are classified into the same category based on the point spacing threshold until all classifications are completed or the preset maximum number of clusters is reached. After that, geometric encapsulation is performed, and the axis-aligned bounding box (AABB) algorithm is applied to generate a minimum circumscribed cube for each point cloud cluster. Secondly, the position is represented by traversing the vertices of the point cloud cluster and selecting the coordinates of the two most representative points to define the target spatial position. Finally, the velocity information of the object is obtained by calculating the position distance of the frame and the time difference between the previous and next frames.

[0128] like Figure 10 As shown in the figure, due to the large amount of point cloud data, it is usually not used directly for transmission and obstacle avoidance. In order to reduce the amount of data, the present invention uses the axis-aligned bounding box (AABB) algorithm to calculate the center position and size of its point cloud cluster, including: inputting the point cloud cluster, setting the minimum point min Pt and x / y / z to positive infinity, and setting the maximum point max Pt and x / y / z to negative infinity; for each point Point in the point cloud cluster, if Point.x < min Pt.x, then min Pt.x = Point.x; the steps for obtaining y and z are the same as those for x; after iteration, outputting the AABB bounding box: the minimum point min Pt, the maximum point max Pt.

[0129] Step S111: Build an overall environment map by combining the position information, size information, speed information of the dynamic target and the static map:

[0130] The results of steps S101 to S109 use a dual-modal output architecture of offline data and real-time data to achieve persistent storage and real-time interaction; the offline data evaluation interface automatically generates a structured evaluation file package when the task terminates, saving the drone's six-degree-of-freedom pose sequence and dynamic target motion trajectory (identifier, location information, bounding box size, instantaneous speed); the real-time data publishing interface is based on the ROS distributed communication middleware to achieve the output of three core data streams: pose estimation stream, dynamic target stream and environment map stream.

[0131] The present invention also provides a computer device, comprising a processor component: including at least one processor; a storage component: including a memory; program instructions: a computer program stored in the memory; the processor component is configured to read and execute the program instructions in the storage unit to implement steps S101 to S111.

[0132] The present invention provides a computer program product, including computer-executable instructions. When the instructions are executed on a processor of an electronic device, the electronic device can execute steps S101 to S111.

[0133] The present invention also provides a computer-readable storage medium storing computer program instructions, wherein the storage medium includes: a non-volatile memory (such as a solid-state drive, flash memory) or a temporary storage carrier (such as a memory cache). The computer-readable storage medium stores an independent product, and the product implements steps S101 to S111 in the form of a software functional unit. The technical solution of the present invention is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, server, or network device, etc.) to execute steps S101 to S111. The aforementioned storage medium includes but is not limited to semiconductor memory (such as ROM, RAM, Flash), magnetic storage media (such as hard disk, tape), optical storage media (such as CD, DVD) and other physical storage devices that can carry program codes (such as USB flash drives, mobile hard disks, etc.).

[0134] The present invention also provides a server for an urban low-altitude dynamic SLAM system based on RGB-D, comprising: a computing cluster comprising multiple processor nodes, each node being configured with at least one multi-core processor; a distributed storage system for storing three-dimensional point cloud maps and training data sets as well as required computer programs; a network communication interface configured to receive real-time image streams transmitted by drone terminals; a memory database for caching intermediate data during the implementation of the invention; wherein the computing cluster executes the computer program to implement steps S101 to S111.

[0135] The present invention conducted the following experiments according to steps S101 to S111:

[0136] First, the RGB image and depth image taken by the RGB-D camera on the drone are obtained; the collected image is input into the target detection network model trained by the constructed drone dataset to obtain the result containing the target location information box and confidence; the result is input into the multi-target tracking algorithm to realize the association between the previous and next frames of the detection result, give a unique ID to the same target, and obtain the area of the dynamic target; the feature points of the overall environment input into the RGB image by the camera are extracted, and the feature points in the dynamic box output by the multi-target tracking are removed based on the semantic information and geometric information to obtain the static environment feature points; the ORB feature points of the input RGB image are extracted frame by frame, and the static environment feature points are obtained based on the static environment feature points. The 3D feature points are matched based on the dynamic feature points, and two sets of 3D feature points are obtained by combining the depth image. The solution is then superimposed with the results of each frame to obtain the drone's posture. The depth map is converted into a 3D point cloud based on the camera model to generate a point cloud map. Based on the 3D point cloud and the drone's posture, an octree static map is constructed. A 3D point cloud of a dynamic target is generated, and the 3D point cloud is Euclidean clustered. The AABB bounding box algorithm is used to enclose the point cloud cluster of the target, and two points are used to represent the position of the target. The position distance and time difference of the previous and next frames are calculated to obtain the speed information of the object. A dual-modal output architecture of offline data and real-time data is used to realize persistent storage and real-time interaction of the results. The experimental results are as follows: Figure 11 shown.

[0137] The present invention has good real-time performance and is responsive. As shown in Table 2, in milliseconds (ms), the processing time for each frame is about 50ms, or 20 FPS:

[0138] Table 2 Real-time experimental results

[0139]

[0140] The present invention also has good accuracy in estimating the UAV pose in a dynamic target environment, and uses two indicators to evaluate the accuracy of the UAV pose estimation: absolute translation error ATE trans (Absolute Trajectory Error), relative rotation error RPE rota (Relative Pose Error).

[0141] Absolute translation error ATE transIt is the root mean square error between the estimated position and the true position, reflecting the global positioning performance of the perception system. The smaller the error, the closer the estimated position is to the true value and the better the performance. The unit is meter, also known as the "root mean square error". It can be calculated by formula (15):

[0142]

[0143] Where ATE is the absolute trajectory error, Indicates the motion matrix between the previous and next frames in the real trajectory in the same reference frame, T esti,i is the motion matrix between the previous and next frames estimated by the algorithm in this reference frame, i represents the i-th frame image, and N represents the total number of frames of the image

[0144] Relative rotation error RPE rota It is the root mean square error between the estimated pose change between the previous and next frames and the actual pose change, which reflects the local pose estimation performance of the perception system. The smaller the error, the more accurate the estimated pose and the better the performance. The unit is radian and can be calculated by formula (16):

[0145]

[0146] The present invention is compared with similar algorithms, and experiments are conducted under the same experimental environment and dataset, and the error information is statistically analyzed. The absolute translation error is measured in meters, and the experiment is conducted on the public TUM dataset. The results are shown in Table 3:

[0147] Table 3 Experimental results of absolute translation error

[0148]

[0149] The relative rotation error is in degrees. Experiments were conducted on the self-made dataset sequences 1, 2, 3, 4, and 5 in Table 2 above, as shown in Table 4:

[0150] Table 4 Experimental results of relative rotation error

[0151]

[0152]

[0153] In the table, RMSE is the root mean square error, STD is the standard deviation, and the estimated trajectory of the drone and its true trajectory are projected onto a two-dimensional plane for visualization. The results are as follows: Figure 12 As shown in the figure, the black is the true trajectory, the blue is the estimated trajectory, that is, the position and posture calculated by SLAM, and the red is the error between the estimated trajectory and the true trajectory. The larger the red area, the greater the error.

[0154] The present invention realizes accurate estimation of motion information of multiple dynamic targets in a dynamic environment, and uses the multi-target tracking indicators MOTA and absolute translation error ATE to estimate the motion information of multiple dynamic targets. trans The results are analyzed and the trajectories are visualized. The closer the multi-target tracking index MOTA is to 1, the higher the accuracy of dynamic target tracking is, as shown in formula (17):

[0155]

[0156] Where FP stands for false positives, i.e., a target is detected when it does not exist, and the unit is pieces; FN stands for missed positives, i.e., a target is not detected when it exists, and the unit is pieces; IDS stands for ID Switch, i.e., the number of times the ID of the same target changes during tracking; and GT stands for the number of true targets in the dataset. The MOTA results for sequences 3, 4, and 5 of the self-made dataset in Table 2 are shown in Table 5:

[0157] Table 5 Experimental results of multi-target tracking indicators

[0158]

[0159] The self-made dataset sequence 4 in Table 2 contains 95 frames of images and 6 drones (as dynamic targets). The total flight length of the main drone is 32.5 meters. For each dynamic target in sequence 4, the absolute translation error ATE is calculated. trans , the results are shown in Table 6:

[0160] Table 6 Experimental results of translation error of absolute sequence 4

[0161]

[0162] The present invention also visualizes the error of each dynamic target. After aligning the starting point, the result of target 1 is as follows: Figure 13 As shown, the black line represents the absolute pose error between the estimated pose and the true pose at each time point; the blue line represents the root mean square error of the absolute pose error at all time points; the green line represents the median of the absolute pose error at all time points; the red line represents the average of the absolute pose error at all time points; the purple shaded area represents the standard deviation of the absolute pose error, reflecting the degree of error dispersion. After aligning the starting point, the target trajectory and the corresponding true value are visualized, as shown in Figure 14 As shown in the figure, it shows the estimated trajectories and true trajectories of all dynamic targets in the sequence in the global coordinate system in three-dimensional space. The blue line represents the estimated trajectory of target 1, and the green line represents the true trajectory of target 1. The same is true for other targets. It can be seen from the figure that the estimated trajectory of the target and the true trajectory are well matched overall.

[0163] Those skilled in the art will appreciate that the functional unit / module division in the embodiments of the present invention is for the purpose of convenience of description only, and in actual implementation, the functions may be reallocated to different physical or logical units as needed.

[0164] 1) Flexibility in function implementation: Each unit can be implemented using hardware modules, software modules, or a combination of hardware and software;

[0165] 2) Integration freedom: any functional unit can be implemented independently or integrated with other units;

[0166] 3) Workflow reference: For the specific workflow of each unit, please refer to the corresponding description of the aforementioned method embodiment.

[0167] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A low-altitude dynamic SLAM method based on RGB-D images, characterized in that: The following steps are involved: Get the environment RGB image and depth image taken by the RGB-D camera; Performing target detection and multi-target tracking on the RGB image to obtain a dynamic target area; Extracting all feature points of the RGB image, removing feature points within the dynamic target area, and obtaining feature points of a static environment; Combining the feature points of the static environment and the depth image, the pose of the UAV is estimated to construct a static map; Perform point cloud processing and encirclement processing on dynamic targets to obtain the position information, size information and speed information of the dynamic targets The overall environment map is constructed by combining the position information, size information, speed information of the dynamic target and the static map.

2. The low-altitude dynamic SLAM method based on RGB-D images according to claim 1, wherein Performing target detection and multi-target tracking on the RGB image to obtain a dynamic target area specifically includes: Combining the RGB image and the simulation environment to generate a target detection dataset; Inputting the target detection data set into the target detection network for training to obtain a target detection model; Outputting a target detection result through the target detection model; The target detection result is input into a multi-target tracking algorithm, and the previous and next frames of the target detection result are associated to obtain the dynamic target area.

3. The low-altitude dynamic SLAM method based on RGB-D images according to claim 2, wherein The multi-target tracking algorithm adopts the BoT-SORT algorithm and adopts the YoloV11 algorithm as the target detector of the BoT-SORT algorithm to output the semantic information and geometric information of the dynamic target area bounding box.

4. The low-altitude dynamic SLAM method based on RGB-D images according to claim 3, wherein Extracting all feature points of the RGB image, removing feature points within the dynamic target area, and obtaining feature points of the static environment specifically includes: Performing global feature point detection on the RGB image to extract all feature points of the scene in the RGB image; Combining semantic information and geometric information of the dynamic target region bounding box to obtain feature points within the dynamic region; The feature points in the dynamic area are eliminated from all the feature points, and the remaining feature points are used as the feature point set of the static environment.

5. The low-altitude dynamic SLAM method based on RGB-D images according to claim 4, wherein Combining the semantic information and geometric information of the dynamic target region bounding box to obtain feature points within the dynamic region specifically includes: Obtaining the imaging plane of the RGB-D camera in the previous frame and the current frame, and selecting a feature point in the three-dimensional environment; Setting the feature point, the midpoint of the previous frame imaging plane, and the midpoint of the current frame imaging plane to be coplanar as an epipolar plane; Connecting the midpoint of the previous frame imaging plane and the midpoint of the current frame imaging plane, and setting the intersection of the connecting line, the previous frame imaging plane, and the current frame imaging plane as a pole; Setting the intersection line of the previous frame imaging plane, the current frame imaging plane and the epipolar plane as the epipolar line; Acquire the projection positions of the feature points in the previous frame and the current frame respectively on the imaging plane of the previous frame and the current frame; Obtain a distance between the projection position of the feature point on the current frame imaging plane and the epipolar line, and compare the distance with an error threshold; if the distance is greater than the error threshold, determine that the feature point is a feature point within a dynamic target area.

6. The low-altitude dynamic SLAM method based on RGB-D images according to claim 5, wherein Combining the feature points of the static environment and the depth image, the pose of the drone is estimated to construct a static map, specifically including: Extracting feature points from the RGB image frame by frame and outputting the target detection result; Eliminate the feature points in the dynamic target area in the target detection result, retain the remaining feature points as the feature points of the static environment, and match the static environment feature points of the previous frame and the current frame; Acquire depth information through the depth image, and obtain two sets of 3D feature points by combining the two sets of matched feature points corresponding to the previous frame and the current frame; Performing a Euclidean transformation on the two groups of 3D feature points to obtain an estimated pose of the UAV.

7. The low-altitude dynamic SLAM method based on RGB-D images according to claim 6, wherein The feature point extraction includes Oriented FAST key points and BRIEF descriptors, wherein the Oriented FAST key points are used to characterize corner points in the RGB image, and the BRIEF descriptors are used to characterize key point neighborhood features.

8. The low-altitude dynamic SLAM method based on RGB-D images according to claim 7, wherein Combining the feature points of the static environment and the depth image, the pose of the drone is estimated to construct a static map, specifically including: Based on the imaging model of a pinhole camera, the depth image is converted into three-dimensional point cloud data through back-projection transformation; Combined with the pose estimation of the UAV, all three-dimensional point data are converted into a global coordinate system to generate an octree map as the static map.

9. The low-altitude dynamic SLAM method based on RGB-D images according to claim 8, wherein Performing point cloud processing and encirclement processing on the dynamic target to obtain the position information, size information and speed information of the dynamic target, specifically including: Converting the dynamic target into a three-dimensional point cloud representation based on the depth information; The point cloud representation is spatially divided using a Euclidean clustering algorithm, and adjacent points are classified into the same category based on a point distance threshold to form several point cloud clusters; Geometrically encapsulating a number of point cloud clusters, and using an axis-aligned bounding box algorithm to generate a minimum circumscribed cube for each of the point cloud clusters; By traversing the vertices of the point cloud cluster, selecting the coordinates of the two most representative vertices to define the target space position, and obtaining the size information of the point cloud cluster; The speed information of the dynamic target is obtained through the position distance and time difference between the previous frame and the current frame.

10. A storage medium, characterized in that: A computer program is stored, and when the computer program is executed by a processor, the low-altitude dynamic SLAM method based on RGB-D images as described in any one of claims 1 to 9 is implemented.

Images