Multi-sensor fusion park sweeper positioning and environment mapping method

Abstract

The invention discloses a park sweeper positioning and environment mapping method based on multi-sensor fusion. The method comprises the following steps: S1, preprocessing multi-modal data acquired by a plurality of sensors; s2, extracting and fusing semantic features and geometric features in the multi-modal data preprocessed by the plurality of sensors; s3, performing geometric feature extraction and recognition on the point cloud data by using a VoxelNet point cloud processing algorithm; s4, performing pose estimation and track plotting by fusing multi-sensor data; s5, identifying places where the motor sweeper repeatedly passes through through loopback detection, and correcting accumulated errors; and S6, constructing a three-dimensional environment map by fusing the laser radar, the visual features and the IMU data, performing global optimization by applying a map optimization technology, correcting errors, improving map precision, and updating the map in real time. According to the invention, the autonomous navigation capability of the sweeper is enhanced, the sweeping efficiency and safety are improved, and the sweeper is suitable for various complex and dynamic park environments.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving environment perception technology, and in particular to a multi-sensor fusion method for positioning a park sweeper and building an environment map. Background Art

[0002] With the development of intelligent driving technology, park sweepers, as one of the applications of autonomous driving technology, have attracted more and more attention. In modern park and urban environments, sweepers need to be able to autonomously navigate in complex and changing environments and complete cleaning tasks efficiently and accurately. This places high demands on the environmental perception and positioning accuracy of sweepers.

[0003] Traditional road sweepers mostly rely on a single sensor (such as LiDAR or camera) for environmental perception and positioning. However, the limitations of a single sensor make road sweepers susceptible to interference in complex environments, resulting in large positioning errors and even possible navigation failures. For example, in an environment with drastic light changes, the performance of the camera will be greatly reduced; in areas with obvious multipath effects, the ranging accuracy of the LiDAR will be affected. These problems make it impossible for road sweepers to work stably in complex campus environments.

[0004] In order to improve the autonomy and positioning accuracy of the sweeper, multi-sensor fusion technology has gradually been introduced into the design of the sweeper in recent years. By combining the data of multiple sensors such as lidar, camera, IMU, etc., the sweeper can perceive the environment more comprehensively and improve the robustness under different environmental conditions through the complementarity between sensors. In addition, the use of multi-sensor fusion technology for environmental mapping and positioning can also effectively reduce the error caused by a single sensor and enhance the stability and reliability of the system.

[0005] However, existing multi-sensor fusion methods still face some challenges when facing complex dynamic environments. For example, how to strike a balance between real-time performance and accuracy, how to effectively handle the asynchrony of sensor data, and how to achieve accurate map updates and error correction in closed-loop detection are still technical problems that need to be solved. Summary of the invention

[0006] The purpose of the present invention is to provide a multi-sensor fusion park sweeper positioning and environment mapping method, which can effectively improve the positioning accuracy and navigation stability of the sweeper in a complex park environment by introducing graph optimization technology, and meet the needs of efficient cleaning.

[0007] In one aspect of the present invention, the present invention proposes a multi-sensor fusion park sweeper positioning and environment mapping method. According to an embodiment of the present invention, the method comprises the following steps:

[0008] S1. Preprocessing multimodal data collected by multiple sensors, wherein the sensors include a laser radar, a monocular camera, and an inertial measurement unit, and the multimodal data includes point cloud data collected by the laser radar, RGB image data collected by the monocular camera, and acceleration and angular velocity data collected by the inertial measurement unit;

[0009] S2, extract and fuse semantic features and preliminary geometric features from multimodal data preprocessed by multiple sensors;

[0010] S3, use VoxelNet point cloud processing algorithm to extract and identify deep geometric features of point cloud data acquired by LiDAR;

[0011] S4, through the fusion of multi-sensor data, pose estimation and trajectory calculation, first estimate the current pose of the park sweeper, combine the data of the inertial measurement unit, the point cloud data obtained by the lidar and the two-dimensional image data collected by the monocular camera, use the nonlinear optimization algorithm to improve the positioning accuracy, and then perform trajectory calculation based on the kinematic model to predict the future path of the vehicle and guide navigation;

[0012] S5. Loop detection is used to identify locations that the sweeper has repeatedly passed through and to correct the accumulated errors. The system uses the RANSAC algorithm to filter out mismatched points, match the current environmental features with previous data, and retain the most reliable feature matching pairs. After detecting the loop, the vehicle posture is optimized and the map is updated to improve the accuracy of positioning and map construction.

[0013] S6. Build a 3D environment map by integrating data from LiDAR, monocular camera and inertial measurement unit, and apply graph optimization technology for global optimization, correct errors and improve map accuracy, while updating the map in real time.

[0014] In addition, the method for positioning and mapping the environment of a park sweeper using multi-sensor fusion according to the above embodiment of the present invention may also have the following additional technical features:

[0015] In some embodiments of the present invention, in step S1, the preprocessing includes the following steps:

[0016] S101, calibration and time alignment: use Zhang Zhengyou calibration method and lidar-camera extrinsic optimization algorithm for calibration, and achieve time alignment of multiple sensor data through time synchronization hardware or timestamp interpolation algorithm;

[0017] S102, key frame acquisition: using dynamically adjusted time intervals and key frame selection algorithms, dynamically adjusting the data acquisition frequency based on environmental changes or motion states, and dynamically adjusting the acquisition frequency f based on the inter-frame change Δt dynamic It is expressed as:

[0018]

[0019] Where Δt is the time interval between two consecutive key frame acquisitions; scaling factor is the scaling factor, which is defined as the coefficient used to adjust the acquisition frequency;

[0020] S103, point cloud dedistortion and motion compensation: using a time interpolation method to perform dedistortion and motion compensation on the point cloud data;

[0021] S104, posture initialization: combining the inertial measurement unit data and GPS positioning information, and using an adaptive filtering algorithm to optimize the posture estimation.

[0022] In some embodiments of the present invention, in step S103, the time difference method includes the steps of:

[0023] (1) Using each point cloud point p i Timestamp t i The time when the calibration point cloud data was collected;

[0024] (2) Based on the IMU or other positioning sensor data, calculate the sensor's position and posture transformation matrix Τ(t) at each time point during the entire scanning process, where Τ(t) contains the sensor's position and posture information;

[0025] (3) For each point p in the point cloud data i , the sensor pose transformation matrix Τ(t i ), using linear interpolation or higher-order interpolation methods, calculate the time t i The corresponding pose transformation matrix Τ(t i ):

[0026]

[0027] Where t1 is t i The most recent known time point before; t2 is t i The nearest known time point after that; Τ(t1) is the posture transformation matrix corresponding to t1; Τ(t2) is the posture transformation matrix corresponding to t2;

[0028] (4) Point cloud position compensation, using the interpolated pose transformation matrix Τ(t i ), point cloud point p i Convert the local coordinate system during acquisition to a unified global coordinate system to eliminate distortion caused by sensor motion:

[0029] p i '=Τ(t0)×Τ(t i ) -1 ×p i

[0030] In the formula, p i ' is the coordinate of the i-th point in the point cloud data after position compensation; Τ(t0) is the pose transformation matrix at the reference time point t0, which is used to unify the positions of all point cloud points; Τ(t i ) -1 is time point t i The inverse matrix of the pose transformation matrix; p i is the original coordinate of the i-th point in the uncompensated point cloud data;

[0031] (5) Dedistorted point cloud generation: After performing the above compensation on all point cloud points, a dedistorted point cloud dataset is generated.

[0032] In some embodiments of the present invention, step S2 specifically includes the following steps:

[0033] S201, semantic feature and preliminary geometric feature extraction: use the YOLO deep learning model to perform semantic segmentation on the RGB image collected by the monocular camera to extract semantic features, and combine it with the point cloud data obtained by the lidar to extract geometric features;

[0034] S202, feature fusion: By fusing convolutional neural networks, the semantic features of the RGB image and the geometric features of the point cloud data are deeply integrated; at the same time, the acceleration and angular velocity data collected by the inertial measurement unit (IMU) are used to assist in pose estimation and motion compensation. The IMU data does not directly participate in the extraction of geometric or semantic features.

[0035] In some embodiments of the present invention, the fusion convolutional neural network in S202 includes:

[0036] (1) Input layer: RGB image input: Ι∈R H×W×3 , where H is the image height, W is the image width, 3 represents the number of RGB color channels, and R is the real number space; Point cloud input: Among them, f i is related to each point cloud point (x i ,y i ,z i ) corresponding point cloud features, x i ,y i ,z i is the three-dimensional coordinate of the point cloud point, and N is the total number of points in the point cloud;

[0037] (2) Single feature extraction, RGB image feature extraction, through the standard convolutional neural network, extract the high-dimensional features F of the RGB image I :

[0038] F I=CNN(I),

[0039] Where H' is the height of the downsampled image, W' is the width of the downsampled image, C1 is the number of RGB channels, CNN(I) represents the processing of the input RGB image I by the convolutional neural network, and R is the real number space;

[0040] (3) Feature alignment: Point cloud feature mapping, point cloud feature F P Mapped to the image feature space, the point cloud features and image features are fused in the same space:

[0041] F P '=Projection(F P ),

[0042] In the formula, F P ' is the mapped point cloud feature, Projection means projection of the point cloud feature, H' is the height of the image after downsampling, W' is the width of the image after downsampling, C2 is the number of channels; R is the real number space;

[0043] (4) Feature fusion: Fusion convolutional layers to transform the feature image F I And the mapped point cloud feature F P 'Perform feature fusion, and then further extract the fused high-level features through convolution operation:

[0044] F fusion =ReLU(Conv([F I ,F P '])),

[0045] In the formula, F fusion To combine the high-level features of image features and point cloud features, ReLU (·) is a nonlinear activation function used to introduce nonlinear transformations so that the network can learn complex features; Conv (·) represents the convolution operation on the concatenated features. [F I ,F P '] represents the concatenation operation of features, H' is the height of the image after downsampling, W' is the width of the image after downsampling, C3 is the number of channels after fusion, and R is the real number space;

[0046] (5) Deep feature extraction: The subsequent convolutional layer performs deep extraction of fusion features through multiple convolutional layers and nonlinear activation functions, and finally obtains the semantic feature F out :

[0047] F out =Conv n(...ReLU(Conv2(ReLU(Conv1(F fusion )))));

[0048] Where Conv1, Conv2, ..., Conv n Represents the subsequent convolutional layer;

[0049] (6) Output layer: semantic prediction, using a fully connected layer or a 1x1 convolutional layer to transform the deep features F out Mapped to the final semantic label space, the semantic classification results of each pixel or point cloud are obtained:

[0050] S = Softmax(Conv(F out )), S∈R H′×W′×K ;

[0051] Where K is the number of semantic categories.

[0052] In some embodiments of the present invention, in S3, using the VoxelNet point cloud processing algorithm to extract and identify geometric features includes the following steps:

[0053] S301, point cloud voxelization: dividing irregular point cloud data into three-dimensional voxel grids, and assigning each point to a corresponding voxel unit;

[0054] S302, feature aggregation: aggregating geometric features of the point cloud in each voxel unit to generate a feature vector of the voxel;

[0055] S303, 3D convolution extraction: Use 3D convolutional neural network to perform convolution operations on voxel grids to extract local and global geometric features layer by layer;

[0056] S304, information capture and fusion: aggregate the extracted features through global pooling or 1x1 convolutional layer to generate the final feature representation of each voxel;

[0057] S305, output features: The network outputs geometric features or semantic labels of voxels to identify structures and categories in the point cloud. In some embodiments of the present invention, the nonlinear optimization algorithm used in step S4 includes the following steps:

[0058] S401, construct objective function: define posture parameters, set the vehicle posture parameter X to be optimized; objective function: take the residual square sum of all sensors as the optimization target:

[0059]

[0060] In the formula, X * That is, the optimal parameter value to minimize the objective function, each residual termi (X) represents the difference between the sensor measurement value and the predicted value based on the pose parameters; ||Residual i (X)|| 2 is the residual sum of squares; It means solving the optimization problem, that is, finding the parameter X that minimizes the total residual sum of squares. * ;

[0061] S402, calculate Jacobian matrix: perform first-order linearization on the residual function, calculate Jacobian matrix J and residual vector r;

[0062] S403, posture update: by solving ΔX=-(J T J) -1 J T r obtains the pose increment and updates the current pose X k+1 =X k +ΔX, where, X k+1 is the updated pose; X k is the current posture; ΔX is the posture increment;

[0063] S404, iteration and convergence: repeat the above steps until the pose update amount or residual change is less than a preset threshold and the convergence condition is met;

[0064] S405, output optimal posture: after the iteration is completed, the optimal posture parameter X obtained by optimization is output. * , used for precise positioning.

[0065] In some embodiments of the present invention, in step S5, the interior point determination of the RANSAC algorithm includes:

[0066] Input a set of feature point pairs matched between the current frame and the historical frame where p i is the feature point in the current frame, p′ i is the feature point in the corresponding historical frame. For each matching point pair (p i ,p′ i ) Use the assumed model Τ to transform and calculate the transformed point p″ i = Τ(p i ) and the corresponding point p′ i Distance between:

[0067] d i =||p″ i -p′ i ||

[0068] If the distance d i If the value is smaller than the threshold ε, the point pair is considered as an internal point.

[0069] In some embodiments of the present invention, in step S6, the objective function of the graph optimization technique is constructed as follows:

[0070] The laser radar obtains the position X of node i at a certain moment i ; The motion estimation provided by IMU and the relative position constraints obtained by LiDAR result in the edges in the figure representing the relative transformation relationship between adjacent pose nodes T ij ; The relative transformation T of the actual measurement ij With the current estimated pose X i and X j The calculated transformation is subtracted to obtain the residual vector e ij =T ij -h(X i ,X j ), where h(X i ,X j ) is the relative transformation calculated based on the current estimated pose;

[0071] The objective function optimizes the node pose of the entire graph by minimizing the residual sum of squares of all edges:

[0072]

[0073] In the formula, ε represents all the edge sets in the graph, Ω ij is the measured confidence of the edge between node i and node j in the graph.

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

[0075] ① Improve positioning accuracy and stability. Through multi-sensor fusion technology, the present invention can comprehensively utilize the advantages of lidar, visual sensors and IMU, effectively reduce the positioning error caused by a single sensor, and improve the positioning accuracy and stability of the vehicle in complex environments.

[0076] ② Enhanced environmental perception capabilities. Using advanced semantic and geometric feature extraction algorithms, the sweeper can more accurately perceive and understand the surrounding environment. This comprehensive environmental perception capability enables the sweeper to make autonomous decisions in a dynamically changing environment, avoid obstacles, and improve cleaning efficiency.

[0077] ③ Real-time map update and global optimization: Through the map optimization technology, the present invention can update the three-dimensional environment map in real time and correct the accumulated errors to ensure the accuracy and consistency of the map. This enables the sweeper to maintain high-precision navigation and path planning capabilities during long-term operation.

[0078] ④Effective error correction: The combination of loop detection and RANSAC algorithm can effectively identify the places that the sweeper has repeatedly passed by, and timely correct the accumulated errors in positioning and mapping. This function ensures the navigation accuracy of the sweeper in a large area and avoids navigation failure caused by error accumulation.

[0079] ⑤ Adapting to dynamic environments, the adaptive filtering and dynamic key frame selection algorithm of the present invention can flexibly adjust the data processing strategy according to environmental changes and vehicle motion status, ensuring the efficient operation of the sweeper in various complex and dynamic environments.

[0080] ⑥ Improve cleaning efficiency. Since the present invention can provide accurate positioning, real-time map update and optimization, the cleaning vehicle can perform cleaning tasks with higher efficiency, reduce missed cleaning and wrong cleaning, and thus improve the overall cleaning effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 This is an architecture diagram of a multi-sensor fusion park sweeper positioning and environment mapping system in Example 1 of the present invention;

[0082] Figure 2 A schematic diagram of a flow chart of a multi-sensor fusion park sweeper positioning and environment mapping method in Example 2 of the present invention

[0083] Figure 3 This is a diagram of the VoxelNet geometric feature extraction process in Example 2 of the present invention. DETAILED DESCRIPTION

[0084] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0085] Example 1

[0086] like Figure 1 As shown, a multi-sensor fusion park sweeper positioning and environment mapping system includes a perception and data acquisition unit, a signal processing and synchronization unit, a feature analysis and data fusion center, a navigation and posture determination unit, an environment recognition and loop closure unit, and a map generation and accuracy optimization unit.

[0087] Among them, the perception and data acquisition unit is used to collect multimodal data from sensors around the sweeper. Sensors include lidar, monocular camera, IMU (inertial measurement unit), GPS and other sensors, which provide geometric information, semantic information and location information of the environment. At the same time, the collected raw data is transmitted to the "signal processing and synchronization unit" for data preprocessing and synchronization. Specifically, multimodal data includes point cloud data collected by lidar, RGB image data collected by monocular camera, and acceleration and angular velocity data collected by inertial measurement unit.

[0088] The signal processing and synchronization unit is used to pre-process and time-synchronize the data from each sensor. Data from different sensors may have different frequencies and delays. The purpose of this unit is to calibrate the data so that they are aligned in time to ensure that multi-sensor data can be accurately fused. At the same time, the synchronized data is sent to the "Feature Analysis and Data Fusion Center" for feature extraction and multi-sensor data fusion.

[0089] The feature parsing and data fusion center is responsible for extracting and fusing geometric and semantic features from data from different sensors. Geometric features include the three-dimensional environment structure extracted from the lidar point cloud data, and semantic features include the object category identified from the RGB image. This unit generates a more accurate and reliable environment model by fusing this information. At the same time, the processed fused feature data is provided to the "navigation and posture determination unit" and the "environment recognition and loop closure unit" for subsequent navigation and positioning tasks.

[0090] The navigation and posture determination unit estimates the posture (position and direction) of the sweeper based on the fused multi-sensor data and plans the navigation path of the sweeper. By combining the motion data of the IMU and the data of other sensors, the current position information of the sweeper is calculated in real time. At the same time, the fused data from the "Feature Analysis and Data Fusion Center" is used to perform accurate posture calculations, and it is connected with the "Map Generation and Precision Optimization Unit" to ensure that the path planning is adjusted according to the latest environmental map.

[0091] The environment recognition and loop closure unit is used to recognize known features in the environment and detect loops (whether the sweeper returns to a place it has been through before). Through loop detection, the system can reduce the cumulative errors in positioning and mapping, and enhance the accuracy of positioning and mapping. This process confirms the location by comparing it with the environmental features in the known map. At the same time, the environment recognition results are shared with the "navigation and posture determination unit" to help correct the navigation route and posture of the sweeper. The loop detection information is fed back to the "map generation and accuracy optimization unit" for global optimization of the map.

[0092] The map generation and accuracy optimization unit is used to build a three-dimensional environment map based on multi-sensor data, and to globally optimize the map through graph optimization technology to correct map distortion caused by sensor errors and drift. This unit ensures the accuracy of the map by cooperating with the loop detection unit and the navigation unit, and provides updated environmental information for real-time navigation. At the same time, it receives multi-sensor data from the "Feature Analysis and Data Fusion Center" for map construction, and optimizes the map structure using the loop detection results of the "Environmental Identification and Loop Closure Unit". The results of map optimization are also fed back to the "Navigation and Posture Determination Unit" to adjust the navigation path.

[0093] Example 2

[0094] like Figure 2 As shown, a multi-sensor fusion park sweeper positioning and environment mapping method includes the following steps:

[0095] S1. Preprocess the multimodal data collected by multiple sensors, including a laser radar, a monocular camera and an inertial measurement unit. The multimodal data includes point cloud data collected by the laser radar, RGB image data collected by the monocular camera, motion data of the vehicle collected by the inertial measurement unit (including acceleration and angular velocity data), and geographic location information collected by GPS. Specifically, the RGB image is obtained by shooting the environmental scene with a monocular camera, the point cloud data is generated by scanning the environment with a laser radar, the IMU is recorded in real time by the inertial measurement unit, and the GPS data obtains the absolute position information of the vehicle through a GPS receiver. Among them, the monocular camera is installed on the top of the sweeper to provide a wider field of view. This position allows the camera to obtain the environmental image in front of the vehicle, which is convenient for identifying the road ahead, obstacles and other environmental features, and is also convenient for its fusion with the laser radar point cloud data and coordination with the IMU / GPS.

[0096] S101. Calibrate and time-align the sensors. Use Zhang Zhengyou calibration method to accurately calibrate the visual sensor, and use the lidar-camera extrinsic optimization algorithm to improve the calibration accuracy between multiple sensors. To ensure data synchronization, time-align the sensor data through time synchronization hardware or timestamp interpolation algorithm.

[0097] S102, key frame acquisition, using dynamically adjusted time intervals and key frame selection algorithms, dynamically adjusts the acquisition frequency according to environmental changes and vehicle motion status during data acquisition to ensure that the key frames collected have high representativeness and processing efficiency. Dynamically adjust the acquisition frequency based on the inter-frame change Δt:

[0098]

[0099] In the above formula, Δt is the time interval between two consecutive key frame acquisitions; scaling factor is a scaling factor, which is defined as a coefficient used to adjust the acquisition frequency.

[0100] S103, point cloud dedistortion and motion compensation. In order to reduce the point cloud data distortion caused by vehicle motion, the time interpolation method is used to dedistort the point cloud data, and the IMU data is combined to perform real-time motion compensation to ensure the accuracy of the point cloud data.

[0101] The time difference method includes the following:

[0102] (1) Point cloud timestamp calibration: Each point cloud point p i (including the three-dimensional coordinates x i ,y i ,z i ) are all accompanied by a precise timestamp t i , calibrate the time when the point cloud data was collected.

[0103] (2) Sensor motion trajectory estimation: Based on the IMU or other positioning sensor data, the sensor’s position and posture Τ(t) at each time point during the entire scanning process is calculated, where Τ(t) contains the sensor’s position and posture information.

[0104] (3) Time interpolation: For each point p in the point cloud data i , the sensor pose Τ(t i ). Use linear interpolation or higher-order interpolation methods (such as spline interpolation) to calculate the time t i The corresponding pose:

[0105]

[0106] Where t1 is t i The most recent known time point before; t2 is t i The nearest known time point after that; Τ(t1) is the posture transformation matrix corresponding to t1; Τ(t2) is the posture transformation matrix corresponding to t2.

[0107] (4) Point cloud position compensation: Use the interpolated pose transformation matrix Τ(t i ), point cloud point p i Convert from the local coordinate system during acquisition to a unified global coordinate system to eliminate distortion caused by sensor motion:

[0108] p i '=Τ(t0)×Τ(t i ) -1 ×p i

[0109] In the formula, p i ' is the coordinate of the i-th point in the point cloud data after position compensation; Τ(t0) is the pose transformation matrix at the reference time point t0, which is used to unify the positions of all point cloud points; Τ(t i ) -1 is time point t i The inverse matrix of the pose transformation matrix; p i is the original coordinate of the i-th point in the uncompensated point cloud data.

[0110] Finally, the dedistorted point cloud is generated. After the above compensation is performed on all point cloud points, a dedistorted point cloud dataset is generated. Each point p i 'The distortion caused by sensor movement has been eliminated

[0111] S104: Initialization of posture: combining IMU data with GPS positioning information to estimate initial posture. The initial posture is further optimized through adaptive filtering algorithm to improve positioning accuracy.

[0112] S2. Extract and fuse the RGB semantic features collected by the monocular camera and the preliminary geometric features of the point cloud collected by the lidar in the multi-sensor data to improve the accuracy and reliability of the data.

[0113] The details are as follows:

[0114] S201, semantic feature and preliminary geometric feature extraction: Use the YOLO deep learning model to perform semantic segmentation on RGB images, extract semantic features, and combine the point cloud data obtained by LiDAR to extract geometric features to enhance the accuracy of semantic features.

[0115] S202, Feature Fusion: Through the fusion convolutional neural network (F-CNN), the semantic features of the RGB image and the geometric features of the point cloud data are deeply integrated to improve the accuracy and reliability of the semantic information. At the same time, the acceleration and angular velocity data collected by the inertial measurement unit (IMU) are used to assist in pose estimation and motion compensation. The IMU data does not directly participate in the extraction of geometric or semantic features.

[0116] The fused convolutional neural network (F-CNN) includes:

[0117] (1) Input layer: RGB image input: Ι∈R H×W×3 , where H is the image height, W is the image width, and 3 represents the number of RGB color channels. Point cloud input: where f i is related to each point (x i ,y i ,z i ) corresponding point cloud features.

[0118] (2) Single feature extraction: RGB image feature extraction, through the standard convolutional neural network (CNN), extract the high-dimensional features F of the RGB image I :

[0119] F I =CNN(I)

[0120] Where H' is the height of the downsampled image; W' is the width of the downsampled image; C1 is the number of RGB channels; CNN(I) represents the processing of the input RGB image I by the convolutional neural network; R is the real number space.

[0121] (3) Feature alignment: Point cloud feature mapping, point cloud feature F P Mapping to the image feature space (for example, using the method of projecting the point cloud onto the image plane) allows the point cloud features and image features to be fused in the same space:

[0122] F P '=Projection(F P )

[0123]

[0124] In the formula, F P ' is the point cloud feature after mapping; Projection means projection of point cloud feature; H' is the height of the image after downsampling; W' is the width of the image after downsampling; C2 is the number of channels; R is the real number space.

[0125] (4) Feature fusion: Fusion convolutional layers to transform the feature image F I And the mapped point cloud feature F P 'Feature fusion can be performed by pixel-by-pixel addition, splicing, etc., and then the fused high-level features are further extracted through convolution operations:

[0126] F fusion =ReLU(Conv([F I ,F P '])),

[0127] In the formula, F fusion is the high-level feature after fusing the image features and point cloud features; ReLU (·) is a nonlinear activation function used to introduce nonlinear transformation so that the network can learn complex features; Conv (·) represents the convolution operation on the concatenated features; [F I ,F P'] represents the concatenation operation of features; H' is the height of the image after downsampling; W' is the width of the image after downsampling; C3 is the number of channels after fusion; R is the real number space.

[0128] (5) Deep feature extraction: The subsequent convolutional layers perform deep extraction of fusion features through multiple convolutional layers and nonlinear activation functions (such as ReLU), and finally obtain the semantic feature F out :

[0129] F out =Conv n (...ReLU(Conv2(ReLU(Conv1(F fusion )))))

[0130] Among them, Conv1, Conv2, ..., Conv n Represents the subsequent convolutional layer.

[0131] (6) Output layer: semantic prediction, using a fully connected layer or a 1x1 convolutional layer to transform the deep features F out Mapped to the final semantic label space, the semantic classification results of each pixel or point cloud are obtained:

[0132] S = Softmax(Conv(F out ))

[0133] Among them, S∈R H′×W′×K , K is the number of semantic categories.

[0134] S3. Use VoxelNet point cloud processing algorithm to extract and recognize deep geometric features of point cloud data. First, the irregular point cloud data is divided into voxel grids, and then the geometric features in the voxels are extracted through 3D convolutional neural network to process irregular point cloud data, accurately capture spatial information, and improve the recognition accuracy of geometric features.

[0135] like Figure 3 As shown, using the VoxelNet point cloud processing algorithm for geometric feature extraction and recognition includes the following steps:

[0136] S301, point cloud voxelization: divide the irregular point cloud data into a three-dimensional voxel grid, and assign each point to a corresponding voxel unit.

[0137] S302, feature aggregation: Aggregate the geometric features of the point cloud in each voxel unit, usually using a maximum pooling or average pooling method to generate a feature vector of the voxel.

[0138] S303, 3D convolution extraction: Use a 3D convolutional neural network to perform convolution operations on the voxel grid to extract local and global geometric features layer by layer.

[0139] S304, information capture and fusion: Aggregate the extracted features through global pooling or 1x1 convolutional layer to generate the final feature representation of each voxel.

[0140] S305, output features: The network outputs the geometric features or semantic labels of voxels to accurately identify the structure and category in the point cloud

[0141] S4, pose estimation and dead reckoning, by fusing multi-sensor data, first estimate the current pose of the park sweeper, combine IMU data and environmental perception information, use nonlinear optimization algorithms (such as Gauss-Newton method) to iteratively optimize the pose, gradually reduce sensor residuals, and obtain more accurate pose estimation. Based on the differential drive model, establish the vehicle kinematic equation, further perform dead reckoning, predict the vehicle's future path, and guide the sweeper's autonomous navigation.

[0142] The nonlinear optimization algorithm used consists of the following steps:

[0143] S401, construct objective function: define posture parameters, set the vehicle posture parameter X to be optimized, including the vehicle's three-dimensional position and attitude angle. Objective function: take the residual sum of squares of all sensors (IMU, lidar, vision, etc.) as the optimization target:

[0144]

[0145] Residual calculation, each residual term i (X) represents the difference between the sensor measurement and the predicted value based on the pose parameters. For example, for the IMU residual:

[0146] Residual IMU =Z IMU -h IMU (X)

[0147] Among them, Z IMU is the actual measurement value of IMU, h IMU (X) is the predicted measurement value at the current pose X.

[0148] S402, calculate Jacobian matrix: perform first-order linearization on the residual function, and calculate Jacobian matrix J and residual vector r.

[0149] S403, posture update, by solving ΔX = -(J T J) -1 J T r obtains the pose increment and updates the current pose X k+1 =X k +ΔX, where, X k+1 is the updated pose; Xk is the current posture; ΔX is the posture increment.

[0150] S404, iteration and convergence: Repeat the above steps until the pose update amount or residual change is less than the preset threshold and the convergence condition is met.

[0151] S405, output optimal posture: after the iteration is completed, the optimal posture parameter X obtained by optimization is output. * , used for precise positioning.

[0152] S5, loop detection, through loop detection to identify the places that the sweeper passes repeatedly, and correct the accumulated errors. The system uses the RANSAC algorithm to filter out mismatched points, match the current environmental features with previous data, and retain the most reliable feature matching pairs. After detecting the loop, the system will correct the positioning error according to the optimized posture and update the map to improve the accuracy of positioning and map construction.

[0153] The interior point determination of the RANSAC algorithm includes:

[0154] The input is a set of feature point pairs matched between the current frame and the historical frame. where p i is the feature point in the current frame, p i ' is the feature point in the corresponding historical frame. For each matching point pair (p i ,p i ') Transform using the assumed model Τ and calculate the transformed point p i = Τ(p i ) and the corresponding point p i 'The distance between:

[0155] d i =||p″ i -p′ i ||

[0156] If the distance d i If the value is smaller than the threshold ε, the point pair is considered as an internal point.

[0157] Step S6, map construction and optimization, builds a three-dimensional environment map by integrating lidar, visual features and IMU data, and applies graph optimization technology to globally optimize the map, corrects map distortion caused by sensor errors, and improves map accuracy. At the same time, the real-time map update module combines the latest sensor data and optimization results to ensure the accurate navigation of the sweeper in a dynamic environment.

[0158] The three-dimensional environment map can be constructed as follows:

[0159] Voxelization: divide the 3D point cloud data generated by the LiDAR into voxel grids, discretize the point cloud data into small cubes, and each voxel represents an area in space; local map construction: as the sweeper moves, new point cloud data is continuously acquired and integrated into the local map. Each frame of data will update the 3D structure of the local environment; 3D reconstruction: through multiple local map constructions, the 3D structure of the entire environment is formed, and a complete 3D environment map is gradually constructed.

[0160] The objective function of the graph optimization technique can be constructed as:

[0161] The laser radar obtains the node pose X at a certain moment i ; The motion estimation provided by IMU and the relative position constraints obtained by LiDAR result in the edges in the figure representing the relative transformation relationship between adjacent pose nodes T ij ; The relative transformation T of the actual measurement ij With the current estimated pose X i and X j The calculated transformation is subtracted to obtain e ij =T ij -h(X i ,X j ), where h(X i ,X j ) is the relative transformation calculated based on the current estimated pose.

[0162] Then the objective function optimizes the node pose of the entire graph by minimizing the residual sum of squares of all edges:

[0163]

[0164] In the formula, ε represents all the edge sets in the graph, Ω ij is the measured confidence of the edge (relative pose constraint) between nodes i and j in the graph.

[0165] Through the above steps, the multi-sensor fusion positioning and mapping method for the park sweeper of the present invention significantly improves the positioning accuracy and map construction quality of the sweeper by integrating lidar, camera and IMU data, enhances environmental adaptability and operating efficiency, and ensures stable operation and efficient cleaning in complex environments.

[0166] The above contents are merely examples and explanations of the present invention. Those skilled in the art may make various modifications or additions to the specific embodiments described or replace them in a similar manner. As long as they do not deviate from the structure of the present invention or exceed the scope defined by the claims, they shall all fall within the protection scope of the present invention.

Claims

1. A multi-sensor fusion park sweeper positioning and environment mapping method, characterized in that: The following steps are involved: S1. Preprocessing multimodal data collected by multiple sensors, wherein the sensors include a laser radar, a monocular camera, and an inertial measurement unit, and the multimodal data includes point cloud data collected by the laser radar, RGB image data collected by the monocular camera, and acceleration and angular velocity data collected by the inertial measurement unit; S2, extract and fuse semantic features and preliminary geometric features from multimodal data preprocessed by multiple sensors; S3, use VoxelNet point cloud processing algorithm to extract and identify deep geometric features of point cloud data acquired by LiDAR; S4, through the fusion of multi-sensor data, pose estimation and trajectory calculation, first estimate the current pose of the park sweeper, combine the data of the inertial measurement unit, the point cloud data obtained by the lidar and the two-dimensional image data collected by the monocular camera, use the nonlinear optimization algorithm to improve the positioning accuracy, and then perform trajectory calculation based on the kinematic model to predict the future path of the vehicle and guide navigation; S5. Loop detection is used to identify locations that the sweeper has repeatedly passed through and to correct the accumulated errors. The system uses the RANSAC algorithm to filter out mismatched points, match the current environmental features with previous data, and retain the most reliable feature matching pairs. After detecting the loop, the vehicle posture is optimized and the map is updated to improve the accuracy of positioning and map construction. S6. Build a 3D environment map by integrating data from LiDAR, monocular camera and inertial measurement unit, and apply graph optimization technology for global optimization, correct errors and improve map accuracy, while updating the map in real time.

2. A method for positioning and mapping a park sweeper using multi-sensor fusion according to claim 1, characterized in that: In step S1, the preprocessing includes the following steps: S101, calibration and time alignment: use Zhang Zhengyou calibration method and lidar-camera extrinsic optimization algorithm for calibration, and achieve time alignment of multiple sensor data through time synchronization hardware or timestamp interpolation algorithm; S102, key frame acquisition: using dynamically adjusted time intervals and key frame selection algorithms, dynamically adjusting the data acquisition frequency based on environmental changes or motion states, and dynamically adjusting the acquisition frequency f based on the inter-frame change Δt dynamic It is expressed as: Where Δt is the time interval between two consecutive key frame acquisitions; scaling factor is the scaling factor, which is defined as the coefficient used to adjust the acquisition frequency; S103, point cloud dedistortion and motion compensation: using a time interpolation method to perform dedistortion and motion compensation on the point cloud data; S104, posture initialization: combining the inertial measurement unit data and GPS positioning information, and using an adaptive filtering algorithm to optimize the posture estimation.

3. A method for positioning and environment mapping of a park sweeper using multi-sensor fusion according to claim 2, characterized in that: In step S103, the time difference method includes the steps of: (1) Using each point cloud point p i Timestamp t i The time when the calibration point cloud data was collected; (2) Based on the IMU or other positioning sensor data, calculate the sensor's position and posture transformation matrix Τ(t) at each time point during the entire scanning process, where Τ(t) contains the sensor's position and posture information; (3) For each point p in the point cloud data i , the sensor pose transformation matrix Τ(t i ), using linear interpolation or higher-order interpolation methods, calculate the time t i The corresponding pose transformation matrix Τ(t i ): Where t1 is t i The most recent known time point before; t2 is t i The nearest known time point after that; Τ(t1) is the posture transformation matrix corresponding to t1; Τ(t2) is the posture transformation matrix corresponding to t2; (4) Point cloud position compensation, using the interpolated pose transformation matrix Τ(t i ), point cloud point p i Convert the local coordinate system during acquisition to a unified global coordinate system to eliminate distortion caused by sensor motion: p i '=Τ(t0)×Τ(t i ) -1 ×p i In the formula, p i ' is the coordinate of the i-th point in the point cloud data after position compensation; Τ(t0) is the pose transformation matrix at the reference time point t0, which is used to unify the positions of all point cloud points; Τ(t i ) -1 is time point t i The inverse matrix of the pose transformation matrix; p i is the original coordinate of the i-th point in the uncompensated point cloud data; (5) Dedistorted point cloud generation: After performing the above compensation on all point cloud points, a dedistorted point cloud dataset is generated.

4. The method for positioning and environment mapping of a park sweeper using multi-sensor fusion according to claim 1 is characterized in that: The step S2 specifically includes the following steps: S201, semantic feature and preliminary geometric feature extraction: use the YOLO deep learning model to perform semantic segmentation on the RGB image collected by the monocular camera to extract semantic features, and combine it with the point cloud data obtained by the lidar to extract geometric features; S202, feature fusion: By fusing convolutional neural networks, the semantic features of RGB images are deeply integrated with the geometric features of point cloud data; at the same time, the acceleration and angular velocity data collected by the inertial measurement unit are used to assist in pose estimation and motion compensation, and the IMU data is not directly involved in the extraction of geometric or semantic features.

5. The method for positioning and mapping the environment of a park sweeper using multi-sensor fusion according to claim 5 is characterized in that: The fused convolutional neural network in S202 includes: (1) Input layer: RGB image input: Ι∈R H×W×3 , where H is the image height, W is the image width, 3 represents the number of RGB color channels, and R is the real number space; Point cloud input: Among them, f i is related to each point cloud point (x i ,y i ,z i ) corresponding point cloud features, x i ,y i ,z i is the three-dimensional coordinate of the point cloud point, and N is the total number of points in the point cloud; (2) Single feature extraction, RGB image feature extraction, through the standard convolutional neural network, extract the high-dimensional features F of the RGB image I : Where H' is the height of the downsampled image, W' is the width of the downsampled image, C1 is the number of RGB channels, CNN(I) represents the processing of the input RGB image I by the convolutional neural network, and R is the real number space; (3) Feature alignment: Point cloud feature mapping, point cloud feature F P Mapped to the image feature space, the point cloud features and image features are fused in the same space: In the formula, F P ' is the mapped point cloud feature, Projection means projection of the point cloud feature, H' is the height of the image after downsampling, W' is the width of the image after downsampling, C2 is the number of channels; R is the real number space; (4) Feature fusion: Fusion convolutional layers to transform the feature image F I And the mapped point cloud feature F P 'Perform feature fusion, and then further extract the fused high-level features through convolution operation: In the formula, F fusion To combine the high-level features of image features and point cloud features, ReLU (·) is a nonlinear activation function used to introduce nonlinear transformations so that the network can learn complex features; Conv (·) represents the convolution operation on the concatenated features. [F I ,F P '] represents the concatenation operation of features, H' is the height of the image after downsampling, W' is the width of the image after downsampling, C3 is the number of channels after fusion, and R is the real number space; (5) Deep feature extraction: The subsequent convolutional layer performs deep extraction of fusion features through multiple convolutional layers and nonlinear activation functions, and finally obtains the semantic feature F out : F out =Conv n (...ReLU(Conv2(ReLU(Conv1(F fusion ))))); Where Conv1, Conv2, ..., Conv n Represents the subsequent convolutional layer; (6) Output layer: semantic prediction, using a fully connected layer or a 1x1 convolutional layer to transform the deep features F out Mapped to the final semantic label space, the semantic classification results of each pixel or point cloud are obtained: S=Softmax(Conv(F out )),S∈R H′×W′×K ; Where K is the number of semantic categories.

6. The method for positioning and mapping the environment of a park sweeper using multi-sensor fusion according to claim 1 is characterized in that: In S3, using the VoxelNet point cloud processing algorithm to extract and identify geometric features includes the following steps: S301, point cloud voxelization: dividing irregular point cloud data into three-dimensional voxel grids, and assigning each point to a corresponding voxel unit; S302, feature aggregation: aggregating geometric features of the point cloud in each voxel unit to generate a feature vector of the voxel; S303, 3D convolution extraction: Use 3D convolutional neural network to perform convolution operations on voxel grids to extract local and global geometric features layer by layer; S304, information capture and fusion: aggregate the extracted features through global pooling or 1x1 convolutional layer to generate the final feature representation of each voxel; S305, output features: The network outputs the geometric features or semantic labels of voxels to identify the structure and category in the point cloud.

7. The method for positioning and mapping the environment of a park sweeper using multi-sensor fusion according to claim 1 is characterized in that: In step S4, the nonlinear optimization algorithm used includes the following steps: S401, construct objective function: define posture parameters, set the vehicle posture parameter X to be optimized; objective function: take the residual square sum of all sensors as the optimization target: Where, X * That is, the optimal parameter value to minimize the objective function, each residual term i (X) represents the difference between the sensor measurement value and the predicted value based on the pose parameters; ||Residual i (X)|| 2 is the residual sum of squares; It means solving the optimization problem, that is, finding the parameter X that minimizes the total residual sum of squares. * ; S402, calculate Jacobian matrix: perform first-order linearization on the residual function, calculate Jacobian matrix J and residual vector r; S403, posture update: by solving ΔX=-(J T J) -1 J T r obtains the pose increment and updates the current pose X k+1 =X k +ΔX, where, X k+1 is the updated pose; X k is the current posture; ΔX is the posture increment; S404, iteration and convergence: repeat the above steps until the pose update amount or residual change is less than a preset threshold and the convergence condition is met; S405, output optimal posture: after the iteration is completed, the optimal posture parameter X obtained by optimization is output. * , used for precise positioning.

8. The method for positioning and mapping the environment of a park sweeper using multi-sensor fusion according to claim 1 is characterized in that: In step S5, the interior point determination of the RANSAC algorithm includes: Input a set of feature point pairs matched between the current frame and the historical frame where p i is the feature point in the current frame, p′ i is the feature point in the corresponding historical frame. For each matching point pair (p i ,p′ i ) is transformed using the assumed model Τ and the transformed point p is calculated i = Τ(p i ) and the corresponding point p′ i Distance between: d i =||p" i -p′ i || If the distance d i If the value is smaller than the threshold ε, the point pair is considered as an internal point.

9. The method for positioning and mapping the environment of a park sweeper using multi-sensor fusion according to claim 1, characterized in that: In step S6, the objective function of the graph optimization technology is constructed as follows: The laser radar obtains the position X of node i at a certain moment i ; The motion estimation provided by IMU and the relative position constraints obtained by LiDAR result in the edges in the figure representing the relative transformation relationship between adjacent pose nodes T ij ; The relative transformation T of the actual measurement ij With the current estimated pose X i and X j The calculated transformation is subtracted to obtain the residual vector e ij =T ij -h(X i ,X j ), where h(X i ,X j ) is the relative transformation calculated based on the current estimated pose; The objective function optimizes the node pose of the entire graph by minimizing the residual sum of squares of all edges: In the formula, ε represents all the edge sets in the graph, Ω ij is the measured confidence of the edge between node i and node j in the graph.

Images