Radar odometer dynamic point elimination method based on associated visual semantics

Abstract

The invention discloses a radar odometer dynamic point elimination method based on associated visual semanteme, and relates to the field of synchronous positioning and map building.The method comprises the steps that LVI-SAM serves as a basic framework, a visual end adopts YOLOv8 to detect a dynamic object, and dynamic features are extracted and triangularized into three-dimensional visual dynamic points; the radar end receives visual dynamic points and pose information, dynamic semantics are spread to the radar point cloud through an expansion algorithm, a boundary attenuation model is introduced to adjust the expansion range of the visual dynamic point cloud boundary, and static radar point misjudgment is avoided; and finally obtaining a pure static point cloud which is used for feature extraction, matching and pose map optimization of a radar inertial system. The robustness of a complex scene is enhanced through multi-sensor cooperative calibration, scene-based parameter configuration can be flexibly applied to multiple scenes, SLAM mapping'ghosting 'is effectively eliminated, the pose estimation precision is improved, and the requirements of downstream tasks of a robot for map quality and positioning reliability are met.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of simultaneous localization and mapping, and particularly relates to a radar odometry dynamic point removal method based on associated visual semantics. BACKGROUND

[0002] Robots need to obtain their own position and environment information to complete path planning and autonomous navigation tasks. In the absence of GPS, SLAM technology can provide accurate positioning and environment representation. SLAM is divided into visual SLAM and laser radar SLAM according to sensors: visual SLAM includes monocular and binocular types, and mature solutions such as VINS-MONO, but the robustness is poor due to the influence of light, motion blur, etc.; laser radar SLAM obtains high-precision environment information through active ranging, and representative solutions include the LOAM series. In recent years, visual-radar fusion solutions such as LVI-SAM have good effects by combining the advantages of both.

[0003] However, dynamic objects in the environment have a significant impact on various SLAMs: visual SLAM will lose tracking features due to dynamic objects, resulting in positioning drift; although laser SLAM is robust in positioning, dynamic objects will cause "ghosting" in mapping, which will seriously interfere with downstream tasks and reduce the usability of the map.

[0004] Existing dynamic point processing methods have defects: point cloud post-processing algorithms such as PeopleRemover and REMOVERT are prone to misjudgment of static points or missed judgment of dynamic points, and consume resources, making it difficult to integrate into the SLAM framework in real time; visual semantic methods such as DS-SLAM and Dyna-SLAM can accurately identify dynamic objects, but due to the difference in sensing frequency between radar and camera, it is difficult to achieve accurate pose mapping, and LVI-SAM also has similar mismatching problems.

[0005] Therefore, how to accurately remove radar odometry dynamic points in real time within the SLAM framework and improve system performance in dynamic scenes has become a technical problem to be solved. SUMMARY

[0006] In order to make up for the deficiencies of the prior art and solve at least one technical problem proposed in the background art.

[0007] The technical scheme adopted by the present application to solve its technical problems is: the radar odometry dynamic point removal method based on associated visual semantics, comprising the following steps:

[0008] S1: visual end dynamic object identification and feature processing, YOLOv8 is used to detect dynamic objects in the input image, and the image is segmented into dynamic and static regions; dynamic features are extracted from the dynamic region, and static features are extracted from the static region, and the static features participate in the pose estimation and loop detection process of the visual inertial system (VIS);

[0009] S2: Visual dynamic point generation, after the VIS obtains the pose estimation through the radar auxiliary visual odometry module, the triangulation function of the VINS is used to restore the two-dimensional dynamic features extracted in step S1 to three-dimensional visual dynamic points, and the visual dynamic points are sent to the LIS together with the pose of the VINS;

[0010] S3: Visual dynamic point coordinate system mapping, the LIS receives the prior pose provided by the VINS as the initial estimation of scan-to-map matching, and maps the three-dimensional visual dynamic points obtained in step S2 from the camera coordinate system to the radar coordinate system based on the initial estimation;

[0011] S4: Radar point cloud dynamic point elimination, the LIS performs inflation processing on the mapped visual dynamic points using an inflation algorithm, propagates the visual dynamic semantics to the original radar point cloud, adjusts the inflation radius of the visual dynamic points using a boundary attenuation model, eliminates the dynamic points in the original radar point cloud, and obtains a pure point cloud containing only static objects, which is used for subsequent feature extraction, matching and pose graph optimization of the LIS.

[0012] As a preferred technical solution of the present application, the processing process of YOLOv8 in step S1 includes:

[0013] S11: backbone network extracts image features, the backbone network includes five stages, the first three stages generate 1 / 4 resolution, 1 / 8 resolution and 1 / 16 resolution feature maps to adapt to different size target detection, and the last two stages generate 1 / 16 and 1 / 32 resolution feature maps, and use C2f module to replace traditional C3 module, through multi-branch parallel processing, residual connection, feature reuse and adaptive fusion to improve calculation efficiency and relieve gradient disappearance;

[0014] S12: neck network performs feature fusion, receives different resolution and semantic level feature maps output by the backbone network, and transmits high-level semantic information to low-level features from top to bottom through the feature pyramid network, and transmits low-level detail information to high-level features from bottom to top through the path aggregation network;

[0015] S13: the detection head outputs the detection result, processes the fused features, predicts the position, bounding box and category of the dynamic object in the image, filters the low-confidence prediction, and decouples the classification task and the regression task of the detection head to avoid mutual interference.

[0016] As a preferred technical solution of the present application, the generation process of the visual dynamic point in step S2 is specifically: the extracted two-dimensional dynamic features are discretely stored as a set according to different semantic boxes to which they belong , wherein This represents a set of dynamic features from the same dynamic object; triangulation yields a set of three-dimensional visual dynamic points. It is transmitted to LIS along with the VINS pose.

[0017] As a preferred technical solution of this application, the specific implementation of the dilation algorithm in step S4 is as follows: using 0.1m as the standard dilation kernel radius. For each visual dynamic point Calculate the centroid of the corresponding dynamic object's visual dynamic point set. European distance And determine the maximum spacing of the dynamic point set. ; through formula Calculate normalized distance Then, by combining the boundary decay model, the actual expansion kernel radius of each visual dynamic point is obtained. Using visual dynamic points as the center, radar points within the radius of the expansion kernel in the radar point cloud are searched and marked as dynamic points.

[0018] As a preferred technical solution of this application, the expression for the boundary attenuation model is:

[0019]

[0020]

[0021] in, For the decay weight of dynamic points, As the starting point of attenuation, For decay rate, For normalized distance, The standard expansion kernel radius is given.

[0022] As a preferred technical solution of this application, the original radar point cloud in step S4 is represented as follows: ,in For radar points originating from moving objects, The radar points are from a static reference object; the pure static point cloud obtained after removing dynamic points is... .

[0023] As a preferred technical solution of this application, the LVI-SAM consists of LIS and VINS. The LIS is implemented based on LIO-SAM and serves as the backbone of the system's pose estimation and mapping. It assists VINS in completing initialization and associates depth information with VINS. The VINS is implemented based on VINS-MONO and provides the LIS with the initial pose estimation for point cloud registration. After identifying loop closures, it sends the loop closure timestamp to the LIS for processing.

[0024] As a preferred technical solution of this application, when extracting dynamic features from the dynamic region in step S1, the dynamic features are classified and stored according to the category of the dynamic object. Different categories of dynamic features correspond to different semantic labels. When generating visual dynamic points in the subsequent triangulation, the semantic label information is retained and sent to LIS along with the visual dynamic points. When LIS removes dynamic points from the radar point cloud, it can distinguish the radar dynamic points corresponding to different types of dynamic objects according to the semantic labels.

[0025] As a preferred technical solution of this application, the Euclidean distance from a visual dynamic point to the centroid is calculated using the three-dimensional Euclidean distance calculation formula:

[0026]

[0027] Determining the maximum spacing At that time, iterate through the Euclidean distances from all dynamic points in the corresponding dynamic object's visual dynamic point set to the centroid, and select the maximum value as the dynamic point set. Furthermore, it is recalculated in real time when the dynamic point set is updated. To adapt to changes in dynamic point sets.

[0028] As a preferred technical solution of this application, after obtaining a clean static point cloud from LIS, the clean static point cloud is used for line and surface feature extraction. When extracting line and surface features, a region-growing method is used to segment the clean static point cloud. Line and surface features are extracted from the segmented regions respectively. Then, feature matching and pose graph optimization are performed based on the extracted line and surface features to further improve the accuracy of pose estimation.

[0029] The beneficial effects of this invention are as follows:

[0030] 1. This invention leverages YOLOv8's accurate detection capabilities for dynamic objects to extract reliable dynamic features from visual images and transform them into 3D visual dynamic points. A dilation algorithm then transmits the dynamic semantics to the radar point cloud. Simultaneously, a boundary attenuation model is introduced to control the dilation range of the visual dynamic point cloud boundary, preventing static radar points from being mistakenly labeled as dynamic points. This effectively solves the shortcomings of traditional methods, such as "accidental deletion of static points and residual dynamic points," ensuring that SLAM mapping is free of "ghosting" interference and providing high-quality, usable maps for downstream tasks such as path planning.

[0031] 2. This invention optimizes the algorithm architecture for different application scenarios, selects a lightweight YOLOv8 model suitable for the scenario, simplifies redundant calculation steps, and accelerates the expansion calculation and boundary decay model operation in hardware, so that it can still run stably on the embedded platform. Moreover, the processing latency and frame rate meet the standards for real-time robot navigation and can be directly integrated into the SLAM framework. This invention breaks through the limitation of the traditional method that "real-time performance and accuracy cannot be achieved at the same time", and provides real-time and continuous environmental perception support for robot autonomous navigation.

[0032] 3. This invention achieves accurate alignment of visual and radar coordinates through high-precision extrinsic parameter calibration, utilizes static features of the visual inertial system (VIS) to participate in pose estimation, provides reliable prior pose for the radar inertial system (LIS), and then optimizes the line and surface feature extraction and matching process of the LIS based on the clean static point cloud after removing dynamic points, reducing the interference of dynamic objects on feature matching. This enables the SLAM system to maintain high-precision pose estimation in complex scenarios such as sudden changes in lighting, object occlusion, and dense dynamic objects, avoiding robot navigation deviation or collision risks caused by positioning drift.

[0033] 4. This invention employs a scenario-based parameter configuration strategy to flexibly adjust key parameters such as YOLOv8 model parameters, boundary decay model parameters, and expansion kernel radius for different scenarios such as urban roads, park inspections, and indoor warehousing, taking into account the dynamic object characteristics and environmental interference factors. Simultaneously, it adopts a modular design, separating functions such as visual dynamic recognition, coordinate mapping, and dynamic point removal into independent modules. When changing scenarios, only the parameters of the corresponding modules need to be adjusted, without reconstructing the overall algorithm architecture. This significantly reduces deployment costs and timelines in different scenarios and can be widely adapted to diverse application scenarios such as robot autonomous navigation, AGV warehouse handling, and park inspections. Attached Figure Description

[0034] The invention will now be further described with reference to the accompanying drawings.

[0035] Figure 1 This is a YOLOv8 framework diagram of the present invention;

[0036] Figure 2 This is a diagram of the LVI-SAM algorithm framework of the present invention;

[0037] Figure 3 This is a schematic diagram of the algorithm principle framework of this invention;

[0038] Figure 4 This is a diagram of the expansion core boundary decay model of the present invention;

[0039] Figure 5 This invention relates to a visual dynamic point rendering radar dynamic point map;

[0040] Figure 6This is the LVI-SAM and LVI-SAM-Dynamic6-DOF trajectory diagram of the present invention. Detailed Implementation

[0041] To make the technical means, creative features, objectives and effects of this invention easier to understand, the invention will be further described below in conjunction with specific embodiments.

[0042] like Figures 1-6 As shown, this embodiment provides a method for dynamic point removal of radar odometers based on associative visual semantics. Using LVI-SAM as the basic framework, it integrates visual semantics to achieve effective removal of dynamic points from radar odometers. The specific steps are as follows:

[0043] S1: Visual-based dynamic object recognition and feature processing:

[0044] The YOLOv8 model is used to process the images input from the camera. The backbone network of YOLOv8 consists of five stages. The first three stages generate feature maps at 1 / 4, 1 / 8, and 1 / 16 resolutions, respectively, which can adapt to the detection needs of large, medium, and small targets. The last two stages generate feature maps at 1 / 16 and 1 / 32 resolutions, which contain rich semantic information. Furthermore, the backbone network uses the C2f module to replace the traditional C3 module. Through multi-branch parallel processing, residual connection optimization, feature reuse, and adaptive fusion, the computational efficiency is effectively improved and the gradient vanishing problem is alleviated.

[0045] S11: The neck network receives feature maps of different resolutions and semantic levels output by the backbone network. First, it performs top-down feature fusion through the feature pyramid network, passing high-level semantic information to low-level features layer by layer to enhance the semantic expression of low-level features. Then, it performs bottom-up feature aggregation through the path aggregation network, passing low-level detailed information to high-level features layer by layer to supplement the detailed information of high-level features.

[0046] S12: The detection head processes the fused features, decouples the classification and regression tasks to avoid mutual interference, accurately predicts the position, bounding box, and category of dynamic objects in the image, and filters low-confidence prediction results, thereby segmenting the image into dynamic and static regions; extracts dynamic features from dynamic regions and static features from static regions. The static features, according to the function of the visual features in the original architecture of LVI-SAM, participate in the pose estimation and loop closure detection process of the visual inertial system (VIS).

[0047] S13: When extracting dynamic features from dynamic regions, they are classified and stored according to the categories of dynamic objects, such as people, vehicles, and trucks. Different categories of dynamic features correspond to different semantic labels, laying the foundation for distinguishing the radar dynamic points corresponding to different types of dynamic objects in the future.

[0048] S2: Visual dynamic point generation:

[0049] In VIS, the radar-assisted visual odometry module uses depth information provided by LIS to assist in pose estimation, and then uses the VINS triangulation function to process the previously extracted two-dimensional dynamic features; firstly, the two-dimensional dynamic features are discretized and stored as sets according to their respective semantic boxes. ,in This represents a set of dynamic features from the same dynamic object. By triangulation, these two-dimensional dynamic features are restored to three-dimensional visual dynamic points, resulting in a set of three-dimensional visual dynamic points. The visual dynamic points and pose information of VINS are sent together to the radar inertial system LIS, and the semantic label information corresponding to the dynamic features is retained during the transmission process.

[0050] S3: Visual dynamic point coordinate system mapping:

[0051] LIS receives the prior pose provided by VINS, which will serve as the initial estimate for scan-to-map matching. Since the visual dynamic points are generated in the camera coordinate system, while the radar point cloud is in the radar coordinate system, in order to propagate the visual dynamic semantics to the radar point cloud, it is necessary to map the 3D visual dynamic points from the camera coordinate system to the radar coordinate system based on the above initial estimate through a coordinate system transformation matrix. This ensures that the visual dynamic points and the radar point cloud are in the same coordinate system, preparing for subsequent dilation processing and dynamic point removal.

[0052] S4: Dynamic point removal in radar point cloud:

[0053] LIS first acquires the raw radar point cloud, which can be represented as: ,in For radar points originating from moving objects, Radar points originating from a static reference object;

[0054] Then, a dilation algorithm is used to dilate the mapped visual dynamic points, with 0.1m as the standard dilation kernel radius. For each visual dynamic point First, calculate the centroid of the corresponding dynamic point set of the dynamic object. The calculation method is as follows:

[0055]

[0056] in This represents the number of dynamic points in the visual dynamic point set of the dynamic object.

[0057] Then, the Euclidean distance calculation formula in three-dimensional space is used to calculate each visual dynamic point. to the center of mass Euclidean distance:

[0058]

[0059] Iterate through the Euclidean distances from all dynamic points in the visual dynamic point set of the dynamic object to the centroid, and select the maximum value as the maximum spacing of the dynamic point set. Furthermore, it is recalculated in real time when the dynamic point set is updated. To adapt to changes in dynamic point sets;

[0060] Through formula Calculate the normalized distance for each visual dynamic point The attenuation weight for each visual dynamic point is determined by combining the boundary attenuation model. The expression for the boundary decay model is:

[0061]

[0062] Then according to the formula Calculate the actual dilation kernel radius for each visual dynamic point. ;

[0063] Using each visual dynamic point as the center, search for the radius of the expansion kernel in the radar point cloud. The system identifies radar points within the system and marks these radar points as dynamic points. At the same time, the LIS distinguishes the radar dynamic points corresponding to different types of dynamic objects based on the semantic tags carried by the visual dynamic points, further optimizing the targeting of dynamic point removal.

[0064] Finally, by removing the marked dynamic points from the original radar point cloud, a clean point cloud containing only static objects is obtained. Pure static point clouds are used for line and surface feature extraction. A region-growing method is used to segment the pure static point cloud. Line and surface features are extracted from the segmented regions respectively. Then, feature matching and pose graph optimization are performed based on the extracted line and surface features to further improve the accuracy of pose estimation.

[0065] This embodiment is experimentally verified on the KITTI dataset. The sensor configuration of the KITTI dataset includes two 1.4-megapixel PointGrayFlea2 grayscale cameras, two 1.4-megapixel PointGrayFlea2 color cameras, and one 64-line Velodyne rotating LiDAR camera. The data is collected from environments such as Road, City, Residential, and Campus. The scenes include dynamic objects such as People, Cars, and Trucks. The experimental conditions are close to the actual application scenarios.

[0066] The experiment used APEAbsolutePoseERROR to evaluate the method of this invention. APE first aligns the estimated trajectory and the true trajectory in time to ensure that the poses being compared come from the same moment. Then, it unifies the coordinate systems of the estimated trajectory and the true trajectory through translational alignment and rotational alignment. For each timestamp, the Euclidean distance between the two poses is calculated as the translation error, and the angular difference between the two poses is calculated as the rotation error. The trajectory accuracy is evaluated by RMSE root mean square error, Mean mean error, Standard deviation, and Min / Max minimum / maximum error.

[0067] Example 1: Application of Autonomous Navigation for Robots in Urban Road Scenarios

[0068] This embodiment addresses urban road scenarios that include dynamic objects such as vehicles, pedestrians, and non-motorized vehicles, and are subject to interference factors such as changes in lighting and occlusion. It applies a radar odometry dynamic point removal method based on associated visual semantics to a robot's autonomous navigation system. The specific implementation steps are as follows:

[0069] 1. System Hardware Configuration

[0070] Visual sensors: Two 1.4-megapixel PointGrayFlea2 color cameras with a horizontal field of view of 60° and a frame rate of 30fps are mounted on both sides of the robot's head at a baseline distance of 0.3m to acquire visual images of the road scene.

[0071] Radar sensor: A 64-line Velodyne rotating lidar is selected, with a maximum detection range of 100m, a horizontal resolution of 0.2°, and a frame rate of 10Hz. It is installed at the center of the top of the robot to acquire three-dimensional point cloud data of the road environment.

[0072] Inertial Measurement Unit (IMU): Employs a high-precision MEMSMU with a sampling rate of 200Hz, an angular velocity measurement range of ±2000° / s, and an acceleration measurement range of ±16g. It is time-synchronized with radar and cameras to provide initial measurement data of motion status.

[0073] Computing Unit: Equipped with an Intel Core i7-12700K processor, an NVIDIA RTX 3090 graphics card, and 32GB of DDR5 memory, ensuring that the processing latency is ≤100ms for real-time algorithm operation;

[0074] 2. Algorithm parameter configuration

[0075] YOLOv8 model: The YOLOv8-M pre-trained model is selected and fine-tuned on a dataset of 100,000 labeled images on urban roads, covering lighting conditions such as sunny days, cloudy days, and evening. The confidence threshold is set to 0.5 and the IOU threshold is set to 0.3. It is used to detect dynamic objects such as vehicles, pedestrians, and non-motorized vehicles, with a detection speed requirement of ≥25fps.

[0076] Boundary decay model: Set the decay starting point a=0.7, decay rate k=2, and standard expansion kernel radius... The value of the attenuation starting point 'a' is based on the dimensional statistics of dynamic objects on urban roads, such as the average width of a car (1.8m) and the shoulder width of a pedestrian (0.5m), to ensure that static points in the boundary area, such as roadside guardrails and streetlights, are not misjudged.

[0077] Coordinate system mapping parameters: The camera intrinsic parameters focal length, principal point coordinates, distortion coefficients and radar-camera extrinsic parameters rotation matrix R and translation vector T are calibrated using a checkerboard calibration board. The extrinsic parameter calibration error is controlled within 0.01m translation and 0.5° rotation to ensure the mapping accuracy of the visual dynamic point from the camera coordinate system to the radar coordinate system.

[0078] 3. Execution of dynamic point rejection process

[0079] Visual-based dynamic object recognition and feature processing:

[0080] When a camera captures images of urban roads, such as scenes containing cars going straight, pedestrians crossing the road, and electric vehicles parked on the roadside, the YOLOv8 backbone network extracts features through five stages: the first three stages generate feature maps at 1 / 4, 1 / 8, and 1 / 16 resolutions to detect small targets such as pedestrians, medium targets such as electric vehicles, and large targets such as cars; the last two stages generate feature maps at 1 / 16 and 1 / 32 resolutions to enhance the extraction of semantic information of dynamic objects.

[0081] The neck network uses a Feature Pyramid Network (FPN) to pass high-level semantic information, such as the "car" category feature, to lower levels. Then, a Path Aggregation Network (PAN) passes low-level detail information, such as the edges of the car window, to higher levels, achieving multi-scale feature fusion. The detection head decouples classification and regression tasks, outputting the bounding boxes of dynamic objects, such as the coordinates of the car's bounding box. And semantic tags such as "car-white" and "pedestrian-adult";

[0082] ORB dynamic features are extracted from dynamic regions, with ≥500 dynamic feature points extracted per frame. The feature points are required to be evenly distributed in the dynamic object region. ORB static features are extracted from static regions such as road markings and roadside trees, with ≥800 static feature points extracted per frame. Static features are used in pose estimation of the visual inertial system (VIS). Dynamic features are classified and stored according to semantic labels, such as dynamic feature sets for "cars" and dynamic feature sets for "pedestrians".

[0083] Visual dynamic point generation and coordinate system mapping:

[0084] VIS, through a radar-assisted visual odometry module, utilizes depth information provided by radar to optimize visual pose estimation accuracy, requiring translation error ≤0.1m and rotation error ≤0.5°. It then calls the VINS triangulation function to restore two-dimensional dynamic features, such as ORB feature points within the car's bounding box, to three-dimensional visual dynamic points, such as coordinates in the camera coordinate system. And send it to the radar inertial system (LIS) carrying semantic tags;

[0085] LIS receives prior pose, such as translation vector, provided by VINS. The rotation matrix R, based on the extrinsic parameter calibration results, maps the visual dynamic points from the camera coordinate system to the radar coordinate system, as shown in the mapped coordinates. The mapping error is controlled within 0.05m;

[0086] Dynamic point removal and subsequent processing of radar point clouds:

[0087] Calculate the centroid of the visual dynamic point set of the "car" category, as shown in the radar coordinate system. Iterate through the Euclidean distances from each dynamic point in the point set to the centroid. For example, if the distance to a point is 0.8m, determine the maximum spacing. Through formula To obtain the attenuation weight Actual expansion core radius ;

[0088] Centered on this dynamic point, search for radar points within a 0.1m radius in the radar point cloud, such as 10 radar points, and mark them as dynamic points; for visual dynamic points, focus on points 1.1m away from the centroid. ,calculate Actual expansion core radius This reduces misjudgments of static points on roadside guardrails;

[0089] After removing the marked dynamic points, a clean static point cloud is obtained, such as a point cloud containing road surface, guardrail, and street light. Based on this point cloud, LIS extracts line and surface features, extracting ≥300 line features and ≥200 surface features per frame. Feature matching and pose graph optimization are then performed, and the final output is a high-precision pose estimation with an absolute trajectory error (APE) of ≤0.08m, providing reliable localization and environmental perception results for robot autonomous navigation.

[0090] 4. Implementation effect verification

[0091] In a 5km test route in an urban road scenario, including 10 traffic lights, 20 pedestrian crossings, and 50 passing vehicles, the robot's autonomous navigation system using this method was tested.

[0092] The accuracy rate of dynamic point removal is ≥95%, the proportion of false static points is ≤3%, and the proportion of missed dynamic points is ≤2%.

[0093] Pose estimation accuracy: Absolute trajectory error (APE) mean 0.07m, root mean square error (RMSE) 0.09m, which is better than the traditional LVI-SAM "APE mean 0.10m, RMSE 0.12m";

[0094] Real-time performance: The average processing latency is 85ms, which meets the real-time requirement of ≤100ms for robot autonomous navigation, with no obvious stuttering or trajectory drift.

[0095] Example 2: Application of Dynamic Point Removal in Park Inspection Scenarios

[0096] This embodiment addresses a park inspection scenario that includes dynamic objects such as patrol vehicles, pedestrians, and fallen leaves. The scene is relatively small in scale with minimal occlusion but interference from tree shadows. The method is applied to a park inspection robot, and the specific implementation steps are as follows:

[0097] 1. System Hardware Configuration

[0098] Vision sensor: A single 8-megapixel USB 3.0 industrial camera with a 90° field of view and a 20fps frame rate, mounted on the front of the robot, used to capture images of park roads, buildings, and pedestrians;

[0099] Radar sensor: 16-line lidar, maximum detection distance 50m, horizontal resolution 1°, frame rate 15Hz, installed in the middle of the robot, suitable for small-scale environmental perception in the park.

[0100] IMU: Low-cost MEMSIMU with a sampling rate of 100Hz, meeting the measurement requirements for low-speed motion ≤1m / s in the park;

[0101] Computing unit: Embedded processor NVIDIA Jetson AGXXavier, ensuring stable operation of the algorithm on the embedded platform with a processing latency of ≤150ms;

[0102] 2. Algorithm parameter configuration

[0103] YOLOv8 model: The YOLOv8-S lightweight model was selected. The dataset contains 50,000 labeled images in the park scene, covering sunny and cloudy days with fine-tuning. The confidence threshold is 0.4. The detection targets are patrol cars, pedestrians, and piles of fallen leaves. The detection speed is ≥20fps.

[0104] Boundary decay model: decay starting point a=0.65, suitable for small dynamic objects in the park, such as pedestrians and piles of fallen leaves; decay rate k=1.8; standard expansion kernel radius. ;

[0105] Coordinate system mapping: Simplify the external parameter calibration process, adopt checkerboard grid for rapid calibration, and control the external parameter error within 0.03m translation and 1° rotation;

[0106] 3. Core Implementation Steps

[0107] Visual dynamic feature extraction: The camera captures images of the park, such as scenes containing pedestrians walking and patrol cars driving. YOLOv8 detects the bounding boxes of dynamic objects and extracts ≥300 dynamic feature points from the pedestrian area, which are classified and stored according to the labels "pedestrian" and "patrol car". Static features such as building walls and road tiles are used for VIS pose estimation.

[0108] Visual dynamic point mapping and dilation: After outputting the prior pose from VINS, triangulation is performed to generate 3D visual dynamic points, which are then mapped to the radar coordinate system; the centroid of the "pedestrian" dynamic point set is calculated, where the distance from a certain dynamic point to the centroid is 0.5m, and the maximum spacing is... The radius of the expansion core is 0.08m, and the corresponding radar dynamic point is marked.

[0109] Static point cloud application: Pure static point cloud is used for park map updates. The local map is updated every 10 seconds. Based on the optimized pose, the inspection robot completes the positioning and status detection of park equipment such as street lights and fire hydrants without "ghosting" interference.

[0110] 4. Implementation Results

[0111] The accuracy rate of dynamic point removal is ≥93%, and the proportion of misidentified static points such as park benches and shrubs is ≤4%.

[0112] The average pose estimation APE is 0.05m, which meets the requirement of ≤0.1m positioning accuracy for park inspection.

[0113] The embedded platform has a processing latency of 120ms and a battery life of ≥8 hours, making it suitable for long-term inspection tasks in the park.

[0114] Example 3: AGV Navigation Application in Indoor Warehousing Scenarios

[0115] This embodiment addresses an indoor warehousing scenario involving AGVs, handling personnel, shelves, and goods. The space is enclosed, with stable lighting but obstructions from goods. The method is applied to a warehouse AGV navigation system to solve map ghosting and positioning drift problems caused by dynamic objects. The specific implementation steps are as follows:

[0116] 1. System Hardware Configuration

[0117] Vision sensors: Two 1-megapixel RGBD cameras with a depth range of 0.5-10m and a frame rate of 25fps are installed on the front and rear of the AGV to collect visual and depth information of personnel and goods in the warehouse.

[0118] Radar sensor: 2D lidar with a scanning angle of 360°, ranging accuracy of ±3mm, and a frame rate of 20Hz. It is installed at the bottom of the AGV and is suitable for indoor planar environment perception.

[0119] IMU: Industrial-grade IMU, sampling rate 150Hz, anti-vibration interference, suitable for AGV low-speed movement ≤0.5m / s in warehouses;

[0120] Computing unit: Embedded industrial computer with Intel Core i5-1135G7 and 16GB of memory, meeting the requirements of low power consumption and small size for indoor scenarios;

[0121] 2. Algorithm parameter configuration

[0122] YOLOv8 model: YOLOv8-Nano lightweight model, with 30,000 labeled images in the warehouse scenario dataset, covering shelves, AGVs, and personnel fine-tuning, with a confidence threshold of 0.45, the detection targets are AGVs and handling personnel, and the detection speed is ≥30fps;

[0123] Boundary decay model: decay start point Adaptable to narrow spaces between warehouse racks, reducing misjudgments of static points on racks, and decreasing decay rate. Standard expansion kernel radius In indoor scenes, the spacing between dynamic objects should be small to avoid excessive expansion range;

[0124] Coordinate system mapping: In a fixed indoor scene, the camera-radar extrinsic parameter calibration is completed using the Zhang Zhengyou calibration method, with an error of ≤0.02m translation and 0.3° rotation;

[0125] 3. Core Implementation Steps

[0126] Visual dynamic object recognition: RGBD cameras capture warehouse images such as scenes of workers pushing goods between shelves. YOLOv8 detects the dynamic regions of workers and goods, extracting ≥400 dynamic features per frame. Static features such as shelf uprights and ground markings are used in VIS pose estimation. At the same time, the depth information of RGBD cameras is used to help optimize the accuracy of dynamic feature extraction.

[0127] Visual dynamic point processing and mapping: After VINS completes pose estimation and the pose error of the indoor scene is ≤0.03m, it triangulates to generate three-dimensional visual dynamic points, and maps them to the 2D radar coordinate system based on extrinsic parameters, such as the mapped coordinates (2.3m, 1.8m).

[0128] Radar dynamic point culling: Calculating the centroid of the visual dynamic point set of "handlers + goods" as follows The distance from a certain dynamic point to the centroid is 0.3m, and the maximum spacing is... With attenuation weight μ=1 and expansion kernel radius 0.05m, radar points within this range are marked as dynamic points in the 2D radar point cloud, such as 5 points, and then removed.

[0129] AGV navigation application: Pure static point cloud, including shelves and ground markings, is used for AGV scan-to-map matching. The optimized pose estimation absolute trajectory error (APE) average value is ≤0.04m, ensuring that the AGV travels accurately between shelves with a navigation accuracy of ≤0.05m, avoiding collisions with dynamic personnel or goods.

[0130] 4. Implementation Results

[0131] The accuracy rate of dynamic point removal is ≥97%, with no misjudgments of static points such as shelves and floor markings;

[0132] The AGV navigation and positioning accuracy is 0.04m, which meets the precise positioning requirements for handling warehouse goods.

[0133] The algorithm has an average processing latency of 60ms, is suitable for low-speed and stable indoor navigation scenarios for AGVs, and can run continuously for 8 hours without failure.

[0134] I. Basic Experimental Setup

[0135] 1. Experimental dataset and scene matching

[0136] The experiments used both publicly available datasets and self-made labeled datasets corresponding to the scenarios in the three examples to ensure a high degree of consistency between the experimental scenarios and actual application scenarios, as detailed below:

[0137] Urban road scene: The dataset uses KITTIOdometry dataset sequences 05, 07, and 10 and a self-made urban dynamic scene dataset containing a 5km test route, covering sunny, cloudy, and evening lighting conditions, annotating 100,000 frames of images and corresponding radar point clouds, including dynamic objects such as vehicles, pedestrians, and non-motorized vehicles.

[0138] Park inspection scenario: The Campus-LSLAM dataset contains a 3km park route, including dynamic targets such as patrol cars, pedestrians, and piles of fallen leaves, as well as a self-made park annotation dataset of 50,000 frames of images, covering park roads, lawns, and areas around buildings.

[0139] Indoor warehouse scenario: Using the TUMRGB-D dataset sequences fr3 / office, fr3 / long_office_household and a self-made warehouse dynamic dataset of 30,000 frames of images, including AGVs, handling personnel, shelves, goods and other targets, to simulate narrow aisles between shelves and goods obstructing the view;

[0140] 2. Comparison of Algorithms and Evaluation Metrics

[0141] Comparison Algorithms: Traditional LVI-SAM without dynamic point removal, ERASOR radar post-processing dynamic point removal, and Dyna-SLAM visual semantic dynamic point removal were selected as comparison algorithms to ensure the fairness of the experiment;

[0142] Core evaluation indicators:

[0143] Dynamic point removal performance: Dynamic point recall, static point precision, and false positive rate;

[0144] Pose estimation accuracy: Mean of absolute trajectory error (APE), Root mean square error (RMSE), Median;

[0145] Real-time performance: average processing latency per frame, algorithm frame rate (FPS);

[0146] II. Experimental results for urban road scenarios corresponding to Example 1

[0147] 1. Dynamic point rejection performance

[0148] In the KITTI dataset sequence 05 urban road congestion scenario, with a dynamic object density of ≥5 objects / 100m², and on a self-made urban dataset, the test results show that this method performs best in both dynamic point identification and static point protection. Specific data are shown in the table below:

[0149]

[0150] Table 1

[0151] Note: Traditional LVI-SAM does not have a dynamic point removal function, so there is no data on dynamic point recall. This method combines visual semantics and boundary decay model, which reduces the misclassification rate by 78.2% compared with ERASOR and 71.7% compared with Dyna-SLAM, effectively avoiding static points such as roadside guardrails and street lights from being misclassified as dynamic points.

[0152] 2. Pose estimation accuracy

[0153] The APE analysis results for a long-distance urban road in the KITTI sequence 10, including 10 traffic lights and 20 pedestrian crossing points, are shown in the table below. The pose estimation accuracy of our method is significantly better than the comparison algorithm, especially in the vertical direction of the Z-axis and the roll angle.

[0154]

[0155] Table 2

[0156] Note: The mean APE of this method is reduced by 30% compared with the traditional LVI-SAM and by 12.5% ​​compared with Dyna-SLAM; the pose fluctuation amplitude in the Z-axis direction is reduced from ±0.15m of the traditional LVI-SAM to ±0.08m, and the roll angle fluctuation is reduced from ±1.0° to ±0.5°, meeting the high-precision positioning requirements of autonomous navigation for urban road robots with a positioning error ≤0.1m;

[0157] 3. Real-time performance

[0158] On an Intel Core i7-12700K + NVIDIA RTX 3090 hardware platform, the average processing latency of this method is 85ms for a single frame of data image resolution of 1280×720 and radar point cloud of 128,000 points / frame. The algorithm's frame rate is stable at 28FPS, which meets the real-time navigation requirements of urban road robots: processing latency ≤100ms and frame rate ≥25FPS. Specific comparisons are as follows:

[0159]

[0160] Table 3

[0161] Note: ERASOR has the worst real-time performance due to the need for post-processing of radar point clouds; although this method introduces visual semantic segmentation and boundary attenuation calculation, through YOLOv8-M model optimization and GPU acceleration, its real-time performance is better than ERASOR and Dyna-SLAM, and only increases the latency by 25ms compared to the traditional LVI-SAM, which is within an acceptable range.

[0162] III. Experimental Results of Park Inspection Scenario Corresponding to Example 2

[0163] 1. Dynamic point rejection performance

[0164] On the Campus-LSLAM dataset (park roads) with a dynamic object density of 2-3 objects / 100m² and on a self-made park dataset, this method demonstrates excellent recognition performance for small dynamic objects such as patrol cars, pedestrians, and piles of fallen leaves. Specific data is as follows:

[0165]

[0166] Table 4

[0167] Note: This method achieves a recall rate of 91.2% for small dynamic targets such as leaf piles, which is 5.9% higher than Dyna-SLAM's 85.3%; the false positive rate for static targets such as park benches and shrubs is only 5.5%, which is 69.3% lower than ERASOR, thus avoiding "ghosting" in the inspection map.

[0168] 2. Pose estimation accuracy

[0169] On a 3km test route within the self-made park, including areas with lawns, building corners, and tree shadows, the pose estimation accuracy of this method meets the park inspection requirements with a positioning error of ≤0.1m. The APE analysis results are as follows:

[0170]

[0171] Table 5

[0172] Note: The average APE of this method in scenes where visual features are missing in the tree shadow area is only 0.06m, which is 50% lower than the traditional LVI-SAM of 0.12m, demonstrating the robustness of radar and vision fusion.

[0173] 3. Real-time performance of embedded platforms

[0174] Adapting this method to meet the lightweight requirements of a campus inspection robot on the NVIDIA Jetson AGXXavier embedded platform, the single-frame processing latency is 120ms, and the frame rate is 22FPS, satisfying the real-time requirements of embedded scenarios with a processing latency of ≤150ms. Specific comparisons are as follows:

[0175]

[0176] Table 6

[0177] Note: The power consumption of this method on the embedded platform is close to that of the traditional LVI-SAM. The power consumption during a typical 8-hour continuous operation of the park inspection is ≤136Wh, which is suitable for the battery life requirements of the inspection robot.

[0178] IV. Experimental results of the indoor storage scenario corresponding to Example 3

[0179] 1. Dynamic point rejection performance

[0180] In the TUMRGB-D dataset fr3 / long_office_household sequence of indoor long corridors, including scenes of people walking and objects being moved, and in the self-made warehouse dataset of narrow aisles between shelves, with a dynamic object density of 3-4 objects / 50m², this method accurately removes dynamic targets such as AGVs and handling personnel, and has outstanding static point protection capabilities. Specific data are as follows:

[0181]

[0182] Table 7

[0183] Note: This method has a false positive rate of only 1.5% for static targets such as shelf uprights and ground markings, which is 86.1% lower than Dyna-SLAM's 10.8%, completely avoiding the appearance of shelf "ghosts" in the warehouse map; the recall rate for dynamic targets such as AGVs moving at high speeds reaches 98.3%, ensuring that AGVs do not collide with other dynamic objects during navigation;

[0184] 2. Pose estimation accuracy

[0185] On a 500m test route between self-made warehouse racking aisles, including 10 right-angle turns and 5 areas where goods are obstructed, the pose estimation accuracy of this method meets the requirements for precise AGV navigation (≤0.05m). The APE analysis results are as follows:

[0186]

[0187] Table 8

[0188] Note: The average APE of this method is only 0.05m in the scenario where the radar point cloud is missing in the area obscured by goods, which is 54.5% lower than the traditional LVI-SAM of 0.11m; when the AGV travels along the aisle between the shelves, the lateral deviation perpendicular to the travel direction is ≤0.03m to ensure that it does not collide with the shelves.

[0189] 3. Real-time performance and stability

[0190] On an Intel Core i5-1135G7 embedded industrial PC, this method has a single-frame processing latency of 60ms, a frame rate of 30FPS, and runs continuously for 8 hours without failure. It is fully suitable for the low-speed, stable navigation scenario of warehouse AGVs. The specific comparison is as follows:

[0191]

[0192] Table 9

[0193] Note: This method has a lower failure rate than the traditional LVI-SAM because dynamic point removal reduces the number of repositionings caused by pose drift, thus improving the stability of AGV navigation; the processing delay is only 60ms, which will not affect the decision response of AGV at low speeds ≤0.5m / s.

[0194] V. Experimental Conclusions

[0195] Dynamic point removal performance: The dynamic point recall rate of this method is ≥93% in urban roads, park inspection and indoor storage scenarios, the static point accuracy rate is ≥94.5% and the false positive rate is ≤5.5%, which is significantly better than traditional LVI-SAM, ERASOR and Dyna-SLAM. It can effectively avoid false positives of static points and false negatives of dynamic points.

[0196] Pose estimation accuracy: The average APE of this method is ≤0.07m in all three scenarios, which is 30%-50% lower than the traditional LVI-SAM, meeting the high-precision positioning requirements of robots / AGVs in different scenarios.

[0197] Real-time performance and compatibility: This method can run stably on both high-performance and embedded platforms, with an average processing latency of ≤120ms and a frame rate of ≥22FPS. It adapts to the hardware requirements of different scenarios, and has high stability during continuous operation with a failure rate of ≤0.3%, making it valuable for practical applications.

[0198] The terms "front," "back," "left," "right," "top," and "bottom" all refer to the figures in the accompanying drawings. Figure 1 Based on the perspective of the observer, the side of the device facing the observer is defined as the front, the left side of the observer is defined as the left, and so on.

[0199] In the description of this invention, it should be understood that the terms "center", "longitudinal", "lateral", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting the scope of protection of this invention.

[0200] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A dynamic point removal method for radar odometers based on associative visual semantics, characterized in that, Includes the following steps: S1: Visual-side dynamic object recognition and feature processing. YOLOv8 is used to detect dynamic objects in the input image, segmenting the image into dynamic and static regions. Dynamic features are extracted from the dynamic regions, and static features are extracted from the static regions. The static features are used in the pose estimation and loop closure detection process of the visual inertial system (VIS). S2: Visual dynamic point generation. After the VIS obtains the pose estimation through the radar-assisted visual odometry module, it uses the triangulation function of VINS to restore the two-dimensional dynamic features extracted in step S1 into three-dimensional visual dynamic points, and sends the visual dynamic points and the pose of VINS together to the radar inertial system LIS. S3: Visual dynamic point coordinate system mapping. The LIS receives the prior pose provided by the VINS as the initial estimate for scan-to-map matching. Based on this initial estimate, the 3D visual dynamic points obtained in step S2 are mapped from the camera coordinate system to the radar coordinate system. S4: Dynamic point removal from radar point cloud. LIS uses an expansion algorithm to expand the mapped visual dynamic points, propagating the visual dynamic semantics to the original radar point cloud. At the same time, a boundary attenuation model is used to adjust the expansion radius of the visual dynamic points, removing dynamic points from the original radar point cloud and obtaining a pure point cloud containing only static objects, which is used for subsequent LIS feature extraction, matching, and pose graph optimization.

2. The radar odometry dynamic point removal method based on associated visual semantics according to claim 1, characterized in that, The YOLOv8 processing procedure in step S1 includes: S11: The backbone network extracts image features. The backbone network consists of five stages. The first three stages generate feature maps with resolutions of 1 / 4, 1 / 8, and 1 / 16 respectively to adapt to the detection of targets of different sizes. The last two stages generate feature maps with resolutions of 1 / 16 and 1 / 32. The traditional C3 module is replaced by the C2f module. The computational efficiency is improved and gradient vanishing is alleviated through multi-branch parallel processing, residual connection, feature reuse, and adaptive fusion. S12: The neck network performs feature fusion, receives feature maps of different resolutions and semantic levels output by the backbone network, and transmits high-level semantic information to low-level features from top to bottom through the feature pyramid network, and then transmits low-level detailed information to high-level features from bottom to top through the path aggregation network. S13: The detection head outputs the detection results, processes the fused features, predicts the position, bounding box and category of dynamic objects in the image, filters low confidence predictions, and decouples the classification task and regression task to avoid mutual interference.

3. The radar odometry dynamic point removal method based on associated visual semantics according to claim 1, characterized in that, The process of generating visual dynamic points in step S2 is as follows: the extracted two-dimensional dynamic features are discretized and stored as sets according to their respective semantic boxes. ,in Represents a set of dynamic features from the same dynamic object; After triangulation, a three-dimensional visual dynamic point set is obtained. It is transmitted to LIS along with the VINS pose.

4. The radar odometry dynamic point removal method based on associated visual semantics according to claim 1, characterized in that, The specific implementation of the dilation algorithm in step S4 is as follows: using 0.1m as the standard dilation kernel radius. For each visual dynamic point Calculate the centroid of the corresponding dynamic object's visual dynamic point set. European distance And determine the maximum spacing of the dynamic point set. ; through formula Calculate normalized distance Then, by combining the boundary decay model, the actual expansion kernel radius of each visual dynamic point is obtained. Using visual dynamic points as the center, radar points within the radius of the expansion kernel in the radar point cloud are searched and marked as dynamic points.

5. The radar odometer dynamic point removal method based on associated visual semantics according to claim 4, characterized in that, The expression for the boundary decay model is: in, The decay weights for dynamic points, As the starting point of attenuation, For decay rate, For normalized distance, The standard expansion kernel radius is given.

6. The radar odometry dynamic point removal method based on associated visual semantics according to claim 1, characterized in that, In step S4, the original radar point cloud is represented as follows: ,in For radar points originating from moving objects, The radar points are from a static reference object; the pure static point cloud obtained after removing dynamic points is... .

7. The radar odometry dynamic point removal method based on associated visual semantics according to claim 1, characterized in that, The LVI-SAM consists of LIS and VINS. LIS is based on LIO-SAM and serves as the backbone of the system's pose estimation and mapping. It assists VINS in completing initialization and associates depth information with VINS. VINS is implemented based on VINS-MONO. It provides the initial pose estimation for point cloud registration to LIS and sends the loop closure timestamp to LIS after loop closure is identified so that LIS can process the loop closure.

8. The radar odometer dynamic point removal method based on associated visual semantics according to claim 1, characterized in that, In step S1, when extracting dynamic features from the dynamic region, the dynamic objects are classified and stored according to their categories. Different categories of dynamic features correspond to different semantic labels. When generating visual dynamic points through triangulation, the semantic label information is retained and sent to LIS along with the visual dynamic points. When removing dynamic points from the radar point cloud, LIS can distinguish the radar dynamic points corresponding to different types of dynamic objects based on the semantic labels.

9. The radar odometer dynamic point removal method based on associated visual semantics according to claim 4, characterized in that, When calculating the Euclidean distance from a visually dynamic point to the centroid, the three-dimensional Euclidean distance calculation formula is used: Determining the maximum spacing At that time, iterate through the Euclidean distances from all dynamic points in the corresponding dynamic object's visual dynamic point set to the centroid, and select the maximum value as the dynamic point set. Furthermore, it is recalculated in real time when the dynamic point set is updated. To adapt to changes in dynamic point sets.

10. The radar odometry dynamic point removal method based on associated visual semantics according to claim 1, characterized in that, After obtaining a clean static point cloud from LIS, the clean static point cloud is used for line and surface feature extraction. When extracting line and surface features, a region growing method is used to segment the clean static point cloud. Line and surface features are extracted from the segmented regions respectively. Then, feature matching and pose graph optimization are performed based on the extracted line and surface features to further improve the accuracy of pose estimation.

Images