Mapping positioning method and device based on solid-state laser radar

Abstract

The invention relates to the field of laser radar mapping positioning, in particular to a mapping positioning method and device based on a solid-state laser radar. The method comprises the steps of performing pose prediction based on measurement data of an inertial measurement unit to obtain an initial pose of a laser radar, performing distortion correction on original point cloud data acquired by the laser radar, performing neighbor matching and plane fitting by combining a multi-resolution surface element map, and performing positioning degradation detection based on plane features to output a degradation level. And performing hierarchical response according to the degradation level to obtain the final pose of the laser radar and updating the multi-resolution surface element map. The invention further comprises a device for realizing the method. The method can adapt to degradation conditions of different scenes, and effectively improves the accuracy and reliability of laser radar mapping positioning.

Description

Technical Field

[0001] This application relates to the field of lidar mapping and positioning, and in particular to a mapping and positioning method and apparatus based on solid-state lidar. Background Technology

[0002] With the evolution of LiDAR technology from mechanical rotating optical arrays to electronically controlled beams (such as optical phased arrays and Flash technology), its R&D and production costs have been significantly reduced. It has been widely applied in robotics, drones, autonomous driving, surveying, and other fields, becoming a core sensor for equipment positioning, perception, and navigation. LiDAR-based positioning and mapping methods have also been continuously optimized. For example, existing technologies such as using the true coordinates of target points in tunnels to achieve vehicle-mounted LiDAR positioning correction, constructing feature sets for positioning based on environmental landmarks, predicting pose to assist in point cloud noise removal and grid map matching, and establishing likelihood domain optimization models to achieve accurate positioning in bumpy scenarios have all improved positioning performance in specific scenarios.

[0003] However, existing lidar positioning methods are mainly designed for mechanical lidar with a large field of view. In scenarios lacking multi-directional geometric constraints, such as tunnels and long corridors, positioning degradation is still prone to occur. More importantly, due to their smaller field of view, solid-state lidar can only scan a limited number of geometric features when facing a single plane such as a wall at close range. This results in insufficient geometric constraints, which in turn leads to singular values ​​in the optimized Hessian matrix, ultimately causing positioning failure.

[0004] Current technologies lack an effective solution to the degradation of solid-state lidar positioning: solutions relying solely on lidar fail directly in degraded scenarios; some solutions that integrate IMU lack accurate degradation detection mechanisms, either causing fusion failure due to erroneous lidar data contaminating the IMU results, or blindly switching to IMU positioning leading to accuracy drift. This deficiency severely limits the application of solid-state lidar in common scenarios such as indoor corridors, canyons, and perimeters of walls, becoming a core bottleneck restricting its large-scale deployment. Therefore, there is an urgent need for a mapping and positioning method that can accurately detect solid-state lidar positioning degradation and ensure positioning continuity and accuracy through adaptive strategies. Summary of the Invention

[0005] Firstly, this application provides a mapping and localization method based on solid-state lidar, employing the technical solution described below: A mapping and localization method based on solid-state lidar includes the following steps: Pose prediction is performed based on the measurement data of the inertial measurement unit (IMU) to obtain the IMU pose prediction result, and the initial pose of the lidar is obtained by combining the extrinsic parameters of the lidar. Based on the initial pose, the original point cloud data collected by the lidar is distorted to obtain rectified point cloud data. Then, the rectified point cloud data is obtained by combining the constructed multi-resolution surface map with nearest neighbor matching. Valid nearest neighbor matching results are then selected and plane fitting is performed. Based on the planar features obtained by the plane fitting, localization degradation detection is performed, and the degradation level of the current scene is output. The degradation level includes non-degradation, mild degradation, and severe degradation. A graded response is executed according to the degradation level to obtain the final pose of the lidar, and the multi-resolution pixel map is updated according to the final pose; wherein, the non-degraded scene adopts a first localization mode that combines the effective nearest neighbor matching results for pose optimization, the mildly degraded scene adopts a second localization mode, and the severely degraded scene adopts a third localization mode.

[0006] By adopting the above technical solution, the initial pose of the lidar can be obtained based on the measurement data of the inertial measurement unit, the distortion of the original point cloud data can be corrected, and the nearest neighbor matching and plane fitting can be performed in combination with the multi-resolution surface map. The localization degradation detection is performed through the planar features and the degradation level is output. The final pose is obtained by performing graded response according to different degradation levels and the multi-resolution surface map is updated, so that the localization module of the solid-state lidar can restart and work normally in the degradation scene.

[0007] Preferably, the pose prediction based on the measurement data of the inertial measurement unit (IMU) is used to obtain the IMU pose prediction result, and the initial pose of the lidar is obtained by combining the extrinsic parameters of the lidar. Specifically, this includes the following steps: The measurement data of the inertial measurement unit within the sliding window is obtained. The measurement data includes the angular velocity data and acceleration data of the inertial measurement unit. After preprocessing the measurement data, the integral operation is performed to obtain the pose increment at each moment within the sliding window. A continuous motion trajectory is constructed by combining the B-spline curve on the Lie group of the sliding window. The IMU pose prediction result of the inertial measurement unit is obtained based on the continuous motion trajectory. The extrinsic parameter data of the lidar is acquired, and the pose prediction result of the IMU is transformed based on the extrinsic parameter data to calculate the initial pose of the lidar in the global coordinate system.

[0008] By adopting the above technical solution, the measurement data of the inertial measurement unit is preprocessed and integrated, and a continuous motion trajectory is constructed by combining it with B-spline curves on the Lie group. This allows for a more accurate prediction of the IMU pose. Then, based on the extrinsic data of the lidar, the pose is transformed, and the initial pose of the lidar in the global coordinate system can be calculated. This provides more reliable initial data for the subsequent mapping and positioning of the lidar, and helps to improve the accuracy of the positioning module restarting normal operation of the solid-state lidar in degraded scenarios.

[0009] Preferably, the step of performing distortion correction on the original point cloud data acquired by the lidar based on the initial pose to obtain corrected point cloud data specifically includes the following steps: Extract the raw point cloud data collected by the lidar, and record the timestamp of each raw point cloud and its original coordinates in the lidar coordinate system. Based on the initial pose of the lidar, combined with the timestamp of each original point cloud and the sampling frequency of the lidar, the instantaneous pose of the lidar corresponding to each original point cloud at the acquisition time is obtained by pose interpolation. The instantaneous pose of the lidar includes an instantaneous rotation matrix and an instantaneous position vector. Each of the original point clouds is transformed from the lidar coordinate system to the global coordinate system to obtain corrected point cloud data.

[0010] By adopting the above technical solution, the instantaneous pose of the original point cloud at the time of acquisition can be obtained by pose interpolation based on the initial pose of the lidar, the timestamp of the original point cloud data, and the sampling frequency. The original point cloud is then transformed from the lidar coordinate system to the global coordinate system, thereby realizing distortion correction of the original point cloud data acquired by the lidar. This provides accurate corrected point cloud data for subsequent operations such as nearest neighbor matching and plane fitting, which helps improve the positioning accuracy of solid-state lidar in degraded scenarios and enables the positioning module to restart and work normally.

[0011] Preferably, the step of performing nearest neighbor matching in conjunction with a preset multi-resolution pixel map to filter out valid nearest neighbor matching results specifically includes the following steps: After obtaining the corrected point cloud data, determine the spatial range of each corrected point cloud in the constructed multi-resolution pixel map; The multi-resolution pixel map is constructed by dividing the three-dimensional space into multiple grids at multiple levels. Each effective grid stores a pixel, and the pixel includes at least the position vector and normal vector obtained by statistical analysis of the corrected point cloud within the current effective grid. The hash indexing mechanism of the multi-resolution pixel map is invoked to locate the corresponding level of the effective grid according to the spatial range of the corrected point cloud, and the nearest neighbor pixels and associated point cloud sets of the current corrected point cloud are searched within the located effective grid. Determine whether the distance between the nearest neighbor element and the corrected point cloud is less than a set distance threshold, and whether the number of associated point clouds meets the preset matching requirements, and filter to obtain valid nearest neighbor matching results that simultaneously meet the distance threshold and the matching requirements.

[0012] By adopting the above technical solution, the spatial range of the corrected point cloud in the multi-resolution pixel map is determined, the effective grid is located using the hash index mechanism, the nearest neighbor pixels and associated point cloud sets are found, and the effective nearest neighbor matching results are filtered out. This can improve the efficiency and accuracy of nearest neighbor matching and provide a more reliable data foundation for subsequent plane fitting and positioning.

[0013] Preferably, the process of performing plane fitting based on effective nearest neighbor matching results includes the following steps: For the set of associated point clouds corresponding to the effective nearest neighbor matching results obtained by screening, the covariance matrix is ​​solved by the least squares method, and the plane normal vector and plane reference point are obtained by eigenvalue decomposition to obtain the fitted plane and construct the complete plane equation. Calculate the flatness of the fitted plane and the vertical distance from each valid associated point cloud to the fitted plane. Mark the fitted planes whose flatness and vertical distance do not meet the preset filtering conditions as invalid planes and filter them to obtain valid planes.

[0014] By adopting the above technical solution, the covariance matrix of the associated point cloud set corresponding to the effective nearest neighbor matching results is solved by the least squares method and eigenvalue decomposition is performed to obtain the fitting plane and the complete plane equation, which facilitates the subsequent use of plane features for localization degradation detection. Calculating the flatness of the fitting plane and the vertical distance from the associated point cloud to the plane and filtering out planes that do not meet the conditions can screen out effective planes, improve the accuracy and reliability of localization, and help to enable the localization module of solid-state lidar to restart and work normally in degraded scenarios.

[0015] Preferably, the localization degradation detection based on the planar features obtained by the plane fitting specifically includes the following steps: The degradation detection is performed sequentially, including normal direction detection, feature point density detection, Hessian matrix singularity detection, and semantic consistency detection. The normal direction detection includes collecting the plane normal vectors of all plane features corresponding to the effective planes, analyzing the distribution of the plane normal vectors to obtain the normal vector distribution result, and determining whether the normal direction degradation condition is met based on the normal vector distribution result. The feature point density detection includes counting the number of valid associated point clouds per unit volume using the effective grid of the multi-resolution pixel map; and determining whether the feature point degradation condition is met based on the number of associated point clouds. The singularity detection of the Hessian matrix includes constructing a Hessian matrix from the point-plane residuals after plane fitting, calculating the ratio of the minimum eigenvalue to the maximum eigenvalue of the Hessian matrix, and determining whether the singularity degradation condition is met based on the ratio. The semantic consistency detection includes: semantically labeling the correction point cloud using a lightweight semantic segmentation model to obtain the corresponding semantic category; statistically analyzing the proportion of the correction point cloud corresponding to a single semantic category; and determining whether the semantic consistency degradation condition is met based on the proportion of each correction point cloud and the distribution result of the normal vector. If none of the degradation conditions are met, it is determined to be a non-degradation scenario; if any of the degradation conditions are met, it is determined to be a mild degradation scenario; if two or more of the degradation conditions are met, it is determined to be a severe degradation scenario.

[0016] By employing the above technical solution, multi-dimensional localization degradation detection is performed on the planar features obtained from plane fitting, including normal direction detection, feature point density detection, Hessian matrix singularity detection, and semantic consistency detection. Normal direction detection determines whether the normal direction degradation condition is met based on the distribution of plane normal vectors; feature point density detection determines whether the feature point degradation condition is met by statistically analyzing the number of effectively associated point clouds per unit volume; Hessian matrix singularity detection determines whether the singularity degradation condition is met by calculating the ratio of Hessian matrix eigenvalues; and semantic consistency detection determines whether the semantic consistency degradation condition is met by semantic annotation and statistically analyzing the proportion of corrected point clouds. Furthermore, based on the satisfaction of different degradation conditions, the current scene is accurately determined as non-degraded, mildly degraded, or severely degraded, so that a graded response can be executed according to the degradation level, ensuring that the localization module of the solid-state LiDAR can restart and function normally in degraded scenes.

[0017] Preferably, the non-degenerate scenario employs a first localization mode that combines the effective nearest neighbor matching results for pose optimization, specifically including the following steps: The corrected point cloud data is converted into an adaptive multi-resolution sparse surface element that is consistent with the multi-resolution surface element map format. The sparse surface element contains the center position vector and normal vector obtained by statistical analysis of the corrected point cloud. Based on the adaptive multi-resolution sparse surface elements, geometric error constraints for matching the corrected point cloud in two consecutive frames and prior error constraints for matching the current frame with the global map are constructed respectively. By combining the angular velocity data and acceleration data of the inertial measurement unit, an IMU physical constraint including acceleration error and angular velocity error is constructed; Using the control points of the continuous motion trajectory in continuous time as optimization variables, the weighted sum of all constraint loss functions is minimized, and the optimal continuous motion trajectory is obtained by iterative update. The final pose of the lidar is obtained by interpolation from the optimal continuous motion trajectory.

[0018] By adopting the above technical solution, the corrected point cloud data can be transformed into adaptive multi-resolution sparse pixels, making the data format compatible with the multi-resolution pixels. Figure 1 This facilitates subsequent processing; it constructs geometric error constraints, prior error constraints, and IMU physical constraints, which can constrain the LiDAR pose from different angles; using the control points of the continuous motion trajectory as optimization variables, it minimizes the weighted sum of the constraint loss function, and iteratively updates the optimal continuous motion trajectory, thereby interpolating to obtain the final pose of the LiDAR, improving the accuracy and stability of solid-state LiDAR positioning in non-degenerate scenarios.

[0019] Preferably, if the scenario is determined to be a mild degradation scenario, the second positioning mode is executed, which specifically includes the following steps: Construct a state estimator with the inertial measurement unit as the backbone, and the state vector includes at least position, velocity, attitude, accelerometer zero bias and gyroscope zero bias; Based on the measurement data, state recursion and covariance prediction are performed to obtain the prior state and prior covariance matrix based solely on IMU prediction. The pose obtained by matching the corrected point cloud is used as the external observation value. The observation equation is constructed and linearized. Based on the results of the semantic dimension detection, the observation noise covariance matrix is ​​dynamically adjusted. Calculate the Kalman gain, and based on the Kalman gain, the prior state, and the external observation, update the state and covariance matrix to obtain the posterior state and posterior covariance matrix that incorporate the laser information. The position and attitude parameters are extracted from the updated posterior state as the final pose of the lidar.

[0020] By adopting the above technical solutions, in mildly degraded scenarios, a state estimator with an inertial measurement unit as the backbone is constructed, and state recursion and covariance prediction are performed to preliminarily estimate the state based on measurement data. The pose obtained by matching the corrected point cloud is used as the external observation value and an observation equation is constructed, which can be combined with point cloud information for localization. Semantically guided adaptive adjustment of observation noise can dynamically adjust the observation noise covariance matrix according to the semantic dimension detection results, thereby improving the localization accuracy. The Kalman gain is calculated and the state and covariance matrix are updated. Finally, the position and attitude parameters are extracted from the updated state vector as the final pose of the lidar, realizing accurate and stable localization of the solid-state lidar in mildly degraded scenarios, enabling the localization module to restart normal operation.

[0021] Preferably, if the scenario is determined to be a severe degradation scenario, a third positioning mode is executed, wherein the third positioning mode is to obtain the final pose based on the IMU pose prediction result.

[0022] By adopting the above technical solution, the final pose can be obtained based on the IMU pose prediction results in severely degraded scenarios, enabling solid-state lidar to still work normally in degraded scenarios.

[0023] Secondly, this application provides a mapping and positioning device based on solid-state lidar, which adopts the following technical solution: A mapping and positioning device based on solid-state lidar includes the following modules: The pose prediction module is used to predict pose based on the measurement data of the inertial measurement unit, obtain the IMU pose prediction result of the inertial measurement unit, and obtain the initial pose of the lidar by combining the extrinsic parameters of the lidar. The nearest neighbor matching and plane fitting module is used to perform distortion correction on the original point cloud data collected by the lidar based on the initial pose to obtain corrected point cloud data, and to perform nearest neighbor matching in combination with the constructed multi-resolution pixel map, filter out the effective nearest neighbor matching results and perform plane fitting. The degradation detection module is used to perform localization degradation detection based on the planar features obtained by the plane fitting, and output the degradation level of the current scene, which includes non-degradation, mild degradation and severe degradation. The pose update module is used to perform a graded response based on the degradation level to obtain the final pose of the lidar, and update the multi-resolution pixel map based on the final pose; wherein, the non-degraded scene adopts a first positioning mode that combines the effective nearest neighbor matching results for pose optimization, the mildly degraded scene adopts a second positioning mode, and the severely degraded scene adopts a third positioning mode.

[0024] By adopting the above technical solution, the pose prediction module obtains the initial pose of the LiDAR based on the measurement data of the inertial measurement unit, which can preliminarily determine the position of the LiDAR; the nearest neighbor matching and plane fitting module performs distortion correction, nearest neighbor matching and plane fitting on the original point cloud data, which can improve the quality of the point cloud data; the degradation detection module performs localization degradation detection and outputs the degradation level, which can accurately judge the degradation status of the current scene; the pose update module performs graded response according to the degradation level to obtain the final pose and update the multi-resolution pixel map, which enables the solid-state LiDAR to achieve accurate mapping and localization in different degradation scenarios, and enables the localization module of the solid-state LiDAR to restart and work normally in degradation scenarios.

[0025] In summary, this application includes at least one of the following beneficial technical effects: (1) This application obtains the initial pose by combining the measurement data of the inertial measurement unit and the external parameters of the lidar, and performs distortion correction and nearest neighbor matching on the original point cloud data, which can improve the accuracy of point cloud data processing; (2) This application performs multi-dimensional localization degradation detection based on planar features obtained by planar fitting, which can accurately determine the degradation level of the current scene; (3) This application implements graded response based on degradation level, which can enable the positioning module of solid-state lidar to restart and work normally in degradation scenarios. Attached Figure Description

[0026] Figure 1 This is a flowchart of the method described in this application; Figure 2 This is a schematic diagram of the device in this application. Detailed Implementation

[0027] This application provides a mapping and positioning method and apparatus based on solid-state lidar. To make the objectives, technical solutions and advantages of this application clearer, the implementation methods of this application will be further described in detail below.

[0028] The following describes in further detail an embodiment of a mapping and positioning method based on solid-state lidar according to the present application, with reference to the accompanying drawings.

[0029] A mapping and localization method based on solid-state lidar, the process is as follows: Figure 1 As shown: S1. Based on the measurement data of the inertial measurement unit (IMU), pose prediction is performed to obtain the IMU pose prediction result. Combined with the extrinsic parameters of the lidar, the initial pose of the lidar is obtained. This specifically includes the following steps: S11. Obtain the measurement data of the inertial measurement unit within the sliding window. The measurement data includes the angular velocity data and acceleration data of the inertial measurement unit. Preprocess the measurement data to lay the foundation for accurate integration. The preprocessing includes noise reduction filtering and initial zero bias compensation.

[0030] The core purpose of preprocessing is to remove noise and outliers to avoid pose prediction errors caused by measurement data contamination.

[0031] Data Acquisition and Synchronization: Acquiring three-axis acceleration data from a solid-state IMU. and triaxial angular velocity Ensure that the timestamps of the IMU data and the LiDAR data are aligned.

[0032] Zero bias calibration: During the device startup phase (stationary state), IMU data is collected for a preset duration (e.g., 10 seconds), and the average values ​​of acceleration and angular velocity are calculated as the zero bias value. Subsequent measurement data are all subtracted from the corresponding zero bias to eliminate the inherent bias of the sensor; Noise suppression: Use sliding window averaging (window size 5-10 data points) or Kalman filtering to filter high-frequency noise and sudden outliers, such as invalid data with acceleration changes exceeding 3g.

[0033] S12. Integrate the preprocessed measurement data to obtain the pose increment at each time step within the sliding window. Initialize the B-spline curve control points Q on the Lie group of the sliding window with the pose increments, and construct... The continuous motion trajectory of the IMU, where k is the B-spline order, specifically... Calculate the index of the interval containing the current time t: ; Where i is the current time period The index of the first control point, It is the start time. It is the length of each interval. This indicates a floor operation, used to determine which interval the current time t falls within.

[0034] Calculate the current time period The standardized time u is used to determine the position within the current time period: ; in, Calculate the offset of t within the current interval; The above steps are used to calculate local parameters in the B-spline basis function to determine the specific location within a certain interval.

[0035] The expression for the B-spline curve on the Lie group is as follows: ; in, It is the initial transformation corresponding to the current control point; Exp is the exponential mapping, used to map Lie algebra elements to Lie groups; It is a B-spline basis function that depends on the standardized time u; These are Lie algebra elements related to the control points.

[0036] The above steps are used to calculate the pose at time t. This takes into account the influence of control points and basis functions.

[0037] Calculate the differences between control points: ; Log is a logarithmic mapping that maps elements of a Lie group to Lie algebras. The above steps are used to define the local properties of the B-spline curve and describe the changes between the control points by calculating the relative transformation between two adjacent control points.

[0038] ; The pose matrix described above combines rotation and translation, where It is the rotating part, which depends on the normalized time. ; It is the translation part.

[0039] The above formula represents the complete rigid body transformation at time t, including rotation and translation.

[0040] By combining the above steps, continuous odometry estimation using B-spline curves on Lie groups is achieved, which allows for smooth and continuous estimation of pose while maintaining temporal continuity, and supports local control to adapt to dynamically changing environments.

[0041] S12. Interpolate the LiDAR point cloud acquisition time from the continuous motion trajectory to obtain the IMU pose corresponding to the acquisition time, and obtain the IMU pose prediction result of the inertial measurement unit, denoted as... ,in Let be a rotation matrix. This is a position vector.

[0042] S13. Using the extrinsic parameter data of the lidar obtained in advance through the hand-eye calibration algorithm, the extrinsic parameter data includes the relative rotation matrix between the IMU and the lidar. and relative translation vector Based on the extrinsic parameter data, pose transformation is performed on the IMU pose prediction results. Based on the rigid body transformation principle, the pose is transformed using the formula... The initial pose of the lidar in the global coordinate system is calculated, i.e. ,in For the rotation matrix of the lidar, This is the position vector of the lidar.

[0043] The preset duration is 8-12 seconds, the window size of the sliding window is 5-10 data points, and the hand-eye calibration algorithm is the Tsai-Lenz algorithm; if there is no relative motion between the IMU and the lidar, the external parameters remain fixed and recalibration is only required after equipment maintenance or changes in installation location.

[0044] S2. Based on the initial pose, the original point cloud data acquired by the lidar is distorted to obtain rectified point cloud data. Nearest neighbor matching is then performed using the constructed multi-resolution pixel map. Valid nearest neighbor matching results are filtered and plane fitting is then performed. The specific steps include the following: S21. Extract the raw point cloud data collected by the lidar, and record the timestamp of each raw point cloud and its original coordinates in the lidar coordinate system.

[0045] Based on the initial pose of the lidar, and combining the timestamp of each raw point cloud acquisition with the lidar sampling frequency, the instantaneous lidar pose of each raw point cloud at the acquisition time is obtained through pose interpolation. The instantaneous lidar pose includes the instantaneous rotation matrix. and instantaneous position vector ; S22. Transform each original point cloud from the lidar coordinate system to the global coordinate system through rigid body transformation.

[0046] The transformation formula is: ,in These are the coordinates of the original point cloud in the global coordinate system. The coordinates of the original point cloud in the lidar coordinate system.

[0047] After performing the above transformation on all point clouds, we obtain corrected point cloud data with motion distortion eliminated, ensuring that all point clouds in the same frame are unified to the same reference base in the global coordinate system, avoiding distortions such as point cloud stretching and offset caused by the movement of the lidar.

[0048] S23. After obtaining the corrected point cloud data, determine the spatial range of each corrected point cloud in the constructed multi-resolution pixel map.

[0049] Multi-resolution pixel maps are constructed by dividing three-dimensional space into multiple grids at multiple levels. That is, the three-dimensional space is divided into multi-scale grids, the coverage of which is twice the range of the LiDAR point cloud, and the grid scale is halved as the division level increases.

[0050] Each valid grid cell stores one face element, which includes at least the position vector and normal vector obtained from the statistical analysis of the corrected point cloud within the current valid grid cell.

[0051] Calculate the mean and variance of the corrected point cloud within each grid cell. Use the mean as the position vector of the cell, and the eigenvector corresponding to the smallest eigenvalue of the variance matrix as the normal vector of the cell. A valid multi-resolution cell is defined as one where the number of point clouds falling into the grid is greater than 10, and the two largest eigenvalues ​​are non-zero.

[0052] S24. Call the hash indexing mechanism of the multi-resolution pixel map, locate the corresponding level grid according to the spatial range of the correction point cloud, and search for the nearest neighbor pixels and related point cloud sets of the current correction point cloud within the located grid to achieve sparse storage and fast indexing of effective pixels.

[0053] In one specific implementation, a polyhedral mesh is preferred for neighborhood lookup to improve search efficiency, and each vertex only needs to search for 8 neighboring vertices.

[0054] When performing nearest neighbor search, the system adaptively selects meshes of different resolutions and their corresponding facets based on the current position of the calibrated point cloud. Low-resolution facet matching is used for geometrically simple scenes (such as walls and roads), while high-resolution facet matching is used for geometrically complex scenes.

[0055] S25. Determine whether the distance between the nearest neighbor pixel and the corrected point cloud is less than the set distance threshold, and whether the number of associated point clouds meets the preset matching requirements, and filter out the effective nearest neighbor matching results that simultaneously meet the distance threshold and matching requirements.

[0056] Retain matching results where the number of nearest neighbors is not less than a preset threshold and the distance between the nearest neighbors and the target point cloud is less than a set distance threshold, and discard invalid matches and outliers.

[0057] In one specific implementation, a threshold for the number of associated point clouds of nearest neighbors and a distance threshold are set. The nearest neighbor matching results obtained in step S33 are then subjected to secondary filtering. Matching features where the number of associated point clouds of nearest neighbor elements meets the standard and the distance between the point cloud and the nearest neighbor element is within the threshold are retained, while invalid matches corresponding to isolated points and distant interference points are removed. The number threshold can be set to no less than 4, and the distance threshold can be set to no more than 5% of the range of the lidar.

[0058] S26. The process of plane fitting based on effective nearest neighbor matching results includes the following steps: S261. For the set of associated point clouds corresponding to the effective nearest neighbor matching results obtained by screening, the covariance matrix of the associated point cloud set is solved by the least squares method, and the plane normal vector and plane reference point are obtained by eigenvalue decomposition. The plane normal vector is the eigenvector corresponding to the smallest eigenvalue of the variance matrix, and the plane reference point is the mean coordinate of the point cloud set. The fitted plane is obtained and the complete plane equation is constructed.

[0059] S262. Verify the effectiveness of the fitted plane: Calculate the flatness of the fitted plane and the vertical distance from each valid associated point cloud to the fitted plane; The flatness of the fitted plane is calculated by the ratio of the largest eigenvalue to the second largest eigenvalue.

[0060] The fitted planes corresponding to flatness and vertical distance that do not meet the preset filtering conditions are marked as invalid planes. Invalid planes with flatness less than a set threshold (e.g., 1.5) or where the distance from the feature point to the plane exceeds the limit are filtered out. Valid plane features that meet the geometric accuracy requirements are retained for subsequent localization degradation detection.

[0061] S3. Based on the planar features obtained by plane fitting, perform localization degradation detection in sequence, including normal direction detection, feature point density detection, Hessian matrix singularity detection, and semantic consistency detection, and output the degradation level of the current scene. The degradation level includes non-degradation, mild degradation, and severe degradation.

[0062] S31, Geometric Dimension: Normal direction detection includes collecting the plane normal vectors of all valid planes corresponding to the plane features, analyzing the distribution of plane normal vectors to obtain the normal vector distribution results, and determining whether the normal direction degradation condition is met based on the normal vector distribution results.

[0063] That is, the variance of the plane normal vector of the statistical effective plane feature is calculated. If the variance is less than the preset normal variance threshold, then the dimension is determined to meet the normal direction degradation condition.

[0064] In another specific implementation, a direction threshold can also be set for the determination. When the plane normal vector is concentrated in a few directions, it is determined that the constraint information provided by the plane features in the current scene is insufficient, that is, the normal direction degradation condition is met.

[0065] S32. Statistical Dimension: Feature point density detection includes counting the number of effective associated point clouds per unit volume using the effective grid of the multi-resolution pixel map, and determining whether the feature point degradation condition is met based on the number of associated point clouds. If the number is less than the preset density threshold, the dimension is determined to meet the feature point degradation condition.

[0066] S33. Optimization Dimension: Hessian matrix singularity detection includes constructing a 6×6 Hessian matrix from the point-plane residuals after plane fitting, calculating the ratio of the minimum eigenvalue to the maximum eigenvalue of the Hessian matrix, and determining whether the singularity degradation condition is met based on the ratio; if the ratio of the minimum eigenvalue to the maximum eigenvalue is less than the preset singularity threshold, then the dimension is determined to meet the singularity degradation condition.

[0067] S34. Semantic Dimension: Semantic consistency detection includes semantic annotation of the corrected point cloud using a lightweight semantic segmentation model to obtain the corresponding semantic categories, including wall, ground, obstacle, corner, etc. The proportion of the corrected point cloud corresponding to each semantic category is counted. Based on the proportion of a single corrected point cloud and the distribution of normal vectors, it is determined whether the semantic consistency degradation condition is met. If the proportion of the corrected point cloud is greater than the preset semantic proportion threshold, and the normal direction distribution corresponding to the semantic category is singular, then the dimension is determined to meet the semantic consistency degradation condition.

[0068] The core of semantic consistency detection is to accurately determine whether the LiDAR is insufficient in effective constraints due to the single scene from two dimensions: semantic scene features and geometric constraint features. Ultimately, it provides a reliable semantic dimension basis for degradation level classification and hierarchical positioning mode selection.

[0069] By calculating the percentage of a single semantic category exceeding a threshold, the semantic level of the scene is used to determine whether the environment is monotonous. In this application, the single semantic category specifically refers to the semantics of large-area planes such as walls and ground. Through the semantic segmentation results of point clouds, it is possible to determine whether the area currently scanned by the LiDAR is a monotonous plane-dominated scene (such as a long corridor, an empty hall, or a straight tunnel).

[0070] The single distribution of normal directions corresponding to semantic categories determines whether there are truly no effective matching constraints from a geometric constraint perspective. It confirms whether a scenario dominated by a single semantic category truly leads to the problem of single geometric constraints, which is the core degradation factor of LiDAR positioning.

[0071] The dimension is determined to be a degradation condition only when both conditions are met simultaneously, which can avoid semantic misjudgment and scene misjudgment and solve the problem of one-sided criteria for pure geometric degradation.

[0072] S35. If none of the degradation conditions are met, it is determined to be a non-degradation scenario; if any degradation condition is met, it is determined to be a mild degradation scenario; if two or more degradation conditions are met, it is determined to be a severe degradation scenario.

[0073] S4. Perform a graded response based on the degradation level to obtain the final pose of the lidar, and update the multi-resolution pixel map based on the final pose.

[0074] S41. In non-degenerate scenarios, the first localization mode is adopted, which combines the effective nearest neighbor matching results to optimize the pose.

[0075] If the current scene is determined to be non-degradable and the LiDAR constraints are sufficient, a B-spline continuous trajectory and multi-constraint fusion optimization is adopted, i.e., LiDAR-driven, with IMU / global map as a supplement, to maximize positioning accuracy. In the above steps, the continuous motion trajectory of the sensor has been modeled with B-spline curves. Next, the total loss is minimized through geometric error constraints (inter-frame + global map) and IMU physical constraints, and finally, accurate pose and local map are output.

[0076] Give B-spline curve Certain constraints are imposed, two of which are related to the corrected point cloud and the global map. For the input point cloud, it is first transformed to global coordinates into adaptive multi-resolution sparse elements. Specifically, this includes the following steps: S411. Convert the corrected point cloud data into an adaptive multi-resolution sparse surface element that is consistent with the multi-resolution surface element map format. The sparse surface element contains the center position vector and normal vector obtained from the statistical analysis of the corrected point cloud.

[0077] Following the same grid division rules as the preset multi-resolution pixel map, adaptive resolution allocation is performed based on the geometric distribution characteristics of the current frame's correction point cloud: high-resolution grids are used for geometrically complex areas such as obstacles and corners, while low-resolution grids are used for geometrically simple areas such as walls and ground. The correction point cloud within each effective grid is statistically analyzed, and the center position vector and normal vector of the pixel are calculated to form the adaptive multi-resolution sparse pixel set of the current frame, ensuring that the pixel set is consistent with the feature format of the global multi-resolution pixel map.

[0078] S412. Based on adaptive multi-resolution sparse surface elements, construct geometric error constraints for matching point clouds between two consecutive frames and prior error constraints for matching the current frame with the global map.

[0079] Calculate the point-to-plane error between two corresponding surface elements along their mean normal direction. and The first loss function constrains the matching relationship between two consecutive frame point clouds, i.e., the geometric error constraint is... ; The second constraint defines the relationship between the current frame and the prior global map, the error between the measurement and the model, and ensures that the current sensor trajectory matches the previously established global map. It defines the loss function between the input polygon and the matched global polygon, i.e., the prior error constraint. ; in, (include ) is the center position of the face element. , (include () is the radar pose at the moment corresponding to the surface element obtained by interpolation from the continuous-time B-spline trajectory.

[0080] S413. Combining the angular velocity and acceleration data of the inertial measurement unit, construct IMU physical constraints that include acceleration error and angular velocity error.

[0081] The pose is constrained over continuous time using the angular velocity and acceleration information from the IMU. ; ; in, , This is the measurement value from the IMU sensor at time t, where g is the acceleration due to gravity. , , It is the interpolation of the continuous-time trajectory at time t. , It's an IMU bias.

[0082] S414. Using the control points of the continuous motion trajectory in continuous time as the optimization variable, minimize the weighted sum of all constraint loss functions, and iteratively update the control points of the continuous motion trajectory to obtain the optimal continuous motion trajectory, that is, ; in, Q is the control point of the continuous time trajectory, and d is the time delay parameter.

[0083] S415. The final pose of the lidar is obtained by interpolation from the optimal continuous motion trajectory.

[0084] After optimization, a local map composed of sparse and dense facets generated based on the optimized pose can be obtained, which can then be integrated into the corresponding global map.

[0085] S42. If the scene is determined to be slightly degraded, the second positioning mode is executed. Since the lidar constraint is insufficient but partially effective, the second positioning mode adopts the IMU backbone adaptive Kalman filtering and semantic adaptive lidar observation, that is, the IMU is dominant and the lidar performs adaptive correction, taking into account both continuity and accuracy. The second localization mode calculates fusion weights based on multi-dimensional degradation detection results, and performs weighted fusion of laser matching results and IMU prediction results to obtain the final pose. Semantic information is used to guide the weight allocation of local regions, and the specific steps include the following: S421. Construct a state estimator with the inertial measurement unit (IMU) as the backbone. The state estimator is an extended Kalman filter (EKF). The nonlinear state equation and observation equation are linearized using a first-order Taylor expansion to adapt to the nonlinear characteristics of IMU measurements. The state vector is a 15-dimensional vector, including position, velocity, attitude, accelerometer bias, and gyroscope bias. That is, the state vector is... ; Where p is position, v is velocity, and q is attitude quaternion. For accelerometer zero bias, This is for zero bias of the gyroscope.

[0086] S422, Prediction Step (IMU Driven): Based on the measurement data, state recursion and covariance prediction are performed to obtain the prior state and the prior covariance matrix, i.e., ; ; in, Let be the prior state estimate at time k, representing the state at time k predicted using only the IMU; For time k-1, the posterior state estimate is given; for the previous time, the final result after fusing the IMU and laser is given. These are the triaxial acceleration and triaxial angular velocity measurements from the IMU. This is the state transition function. The prior covariance matrix; The posterior covariance matrix; Let Jacobian be the state transition matrix. U is the process noise covariance matrix.

[0087] S423, Observation Step (Laser Observation): The pose obtained by matching the corrected point cloud of the lidar is used as the external observation value. The observation equation is constructed and linearized through a first-order Taylor expansion, i.e., ; in, The pose obtained by nearest neighbor matching of the lidar point cloud, i.e., the external observation value. Let v be the observation function and v be the observation noise.

[0088] S424. Adaptive adjustment of observation noise guided by semantics: Dynamically adjust the observation noise covariance matrix R based on the results of semantic dimension detection; ; in, Based on the observation noise covariance matrix, This is a semantic adjustment factor, with a value range of 0.2 to 5.0.

[0089] If the proportion of a single semantic category exceeds the threshold, the observation noise is increased; if the proportion of semantic categories with rich features exceeds the threshold, the observation noise is decreased.

[0090] Semantic modulators It can be continuously adjusted based on semantic proportion: ; in, The percentage of point clouds for single semantic categories such as walls and floors. Enrich the semantic category point cloud proportions for features such as obstacles, corners, and door frames. , This is the preset adjustment coefficient.

[0091] In a specific implementation, if the semantic segmentation results show that the proportion of a single semantic category such as wall or ground exceeds a first preset threshold of 70% to 85%, then =2.0~5.0, increasing the observation noise represents reducing the confidence level of the laser observation; if the semantic segmentation results show that the proportion of feature-rich categories such as obstacles, corners, and door frames exceeds the second preset threshold of 10%~20%, then =0.2~1.0, reducing observation noise, represents improving the confidence level of laser observation.

[0092] S425, Update Step: Calculate the Kalman gain, and update the state and covariance matrix based on the Kalman gain, prior state, and observations to obtain the posterior state and posterior covariance matrix that incorporate laser information.

[0093] Kalman gain: ; Status Update: ; Covariance update: ; in, The Kalman gain matrix; The Jacobian matrix is ​​the observation matrix; I is the identity matrix; Let be the posterior state estimate at time k, and let represent the final result after fusing the IMU and laser at the current time. Let be the posterior covariance at time k, representing the uncertainty in state estimation after laser fusion. The smaller the value, the smaller the Kalman gain at the next time step.

[0094] That is, the final state The sum of the IMU prediction result and the product of the difference between the laser observation and the IMU prediction and their corresponding weights. If the laser and IMU predictions are consistent (residual ≈ 0), no correction is made. If they are inconsistent, the correction amount is determined by the Kalman gain. If K is small (wall scene, single semantic category), the IMU is mainly used. If K is large (obstacle scene, feature-rich semantic category), the laser is mainly used.

[0095] By updating the covariance and fusing laser information, the uncertainty is reduced, and the subtracted part is the information gain brought by laser observation.

[0096] S426. From the updated state vector The position and attitude parameters are extracted and used as the final pose of the LiDAR.

[0097] S43. If the scene is determined to be severely degraded, a third positioning mode is executed, which combines IMU-based positioning with map constraints as a fallback. The third positioning mode obtains the final pose based on the IMU pose prediction results and the boundary constraints of the multi-resolution pixel map. That is, the lidar has no effective constraints, and the lidar is abandoned, with the IMU as the primary source, in order to maximize the continuity of positioning.

[0098] S431. Based on the pose prediction results of the solid-state IMU, extract the effective pixel boundaries in the multi-resolution pixel map as constraints, construct a spatial range constraint box, and optimize the accuracy of the constraint box by combining the semantically labeled wall and ground boundary information.

[0099] S432. Match the IMU pose prediction result with the spatial range constraint box, remove abnormal poses that exceed the constraint box, and perform constraint correction on the IMU pose prediction result to obtain the corrected pose as the final pose of the lidar.

[0100] The above steps construct a spatial range constraint box by using the effective pixel boundaries of the multi-resolution pixel map as constraints, and optimize the accuracy by combining the semantically labeled wall and ground boundary information. The IMU pose prediction results are constrained and corrected, and abnormal poses are eliminated, which can obtain a more accurate final pose in severely degraded scenarios.

[0101] In one specific implementation, to simplify the process, the initial pose of the lidar corresponding to the IMU pose prediction result can also be used as the final pose.

[0102] S44. Update the map based on the final pose obtained from the lidar.

[0103] S441. Transform the corrected point cloud to the global coordinate system according to the final pose, and remove point clouds whose distance from existing surface elements is less than the repetition threshold. S442. For the mesh containing the newly added valid point cloud, recalculate the position vector and normal vector of the surface element, and update the surface element information of the corresponding level. S443. Regularly clean up invalid polygons without point cloud support to maintain the sparsity and effectiveness of the map.

[0104] Based on the same inventive concept described above, this application also discloses a mapping and positioning device based on solid-state lidar, the architecture of which is as follows: Figure 2 As shown, it includes the following modules: The pose prediction module is used to predict pose based on the measurement data of the inertial measurement unit (IMU) to obtain the IMU pose prediction result, and then combine it with the extrinsic parameters of the lidar to obtain the initial pose of the lidar. The nearest neighbor matching and plane fitting module is used to perform distortion correction on the raw point cloud data collected by the lidar based on the initial pose to obtain corrected point cloud data. It then combines the constructed multi-resolution pixel map to perform nearest neighbor matching, filters out the effective nearest neighbor matching results, and performs plane fitting. The degradation detection module is used to perform localization degradation detection based on planar features obtained by plane fitting, and outputs the degradation level of the current scene, which includes non-degradation, mild degradation and severe degradation. The pose update module is used to perform graded responses based on the degradation level to obtain the final pose of the LiDAR and update the multi-resolution pixel map based on the final pose. Among them, the first localization mode is used for non-degradation scenarios, which combines effective nearest neighbor matching results to optimize the pose; the second localization mode is used for mild degradation scenarios; and the third localization mode is used for severe degradation scenarios.

[0105] Those skilled in the art will understand that all or part of the steps of the above embodiments can be implemented by hardware, or by a program instructing related hardware. The program can be stored in a computer-readable storage medium, such as a USB flash drive, a portable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk, and other media capable of storing program code.

[0106] The above description is merely an optional embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A mapping and localization method based on solid-state lidar, characterized in that, Includes the following steps: Pose prediction is performed based on the measurement data of the inertial measurement unit (IMU) to obtain the IMU pose prediction result, and the initial pose of the lidar is obtained by combining the extrinsic parameters of the lidar. Based on the initial pose, the original point cloud data collected by the lidar is distorted to obtain rectified point cloud data. Then, the rectified point cloud data is obtained by combining the constructed multi-resolution surface map with nearest neighbor matching. Valid nearest neighbor matching results are then selected and plane fitting is performed. Based on the planar features obtained by the plane fitting, localization degradation detection is performed, and the degradation level of the current scene is output. The degradation level includes non-degradation, mild degradation, and severe degradation. A graded response is executed according to the degradation level to obtain the final pose of the lidar, and the multi-resolution pixel map is updated according to the final pose; wherein, the non-degraded scene adopts a first localization mode that combines the effective nearest neighbor matching results for pose optimization, the mildly degraded scene adopts a second localization mode, and the severely degraded scene adopts a third localization mode.

2. The mapping and positioning method based on solid-state lidar according to claim 1, characterized in that, The pose prediction based on the measurement data of the inertial measurement unit (IMU) is used to obtain the IMU pose prediction result. Combined with the extrinsic parameters of the lidar, the initial pose of the lidar is obtained. The specific steps include the following: The measurement data of the inertial measurement unit within the sliding window is obtained. The measurement data includes the angular velocity data and acceleration data of the inertial measurement unit. After preprocessing the measurement data, the integral operation is performed to obtain the pose increment at each moment within the sliding window. A continuous motion trajectory is constructed by combining the B-spline curve on the Lie group of the sliding window. The IMU pose prediction result of the inertial measurement unit is obtained based on the continuous motion trajectory. The extrinsic parameter data of the lidar is acquired, and the pose prediction result of the IMU is transformed based on the extrinsic parameter data to calculate the initial pose of the lidar in the global coordinate system.

3. The mapping and positioning method based on solid-state lidar according to claim 2, characterized in that, The step of correcting the distortion of the raw point cloud data acquired by the lidar based on the initial pose to obtain corrected point cloud data specifically includes the following steps: Extract the raw point cloud data collected by the lidar, and record the timestamp of each raw point cloud and its original coordinates in the lidar coordinate system. Based on the initial pose of the lidar, combined with the timestamp of each original point cloud and the sampling frequency of the lidar, the instantaneous pose of the lidar corresponding to each original point cloud at the acquisition time is obtained by pose interpolation. The instantaneous pose of the lidar includes an instantaneous rotation matrix and an instantaneous position vector. Each of the original point clouds is transformed from the lidar coordinate system to the global coordinate system to obtain corrected point cloud data.

4. The mapping and positioning method based on solid-state lidar according to claim 2, characterized in that, The step of combining a preset multi-resolution pixel map to perform nearest neighbor matching and filtering out valid nearest neighbor matching results specifically includes the following steps: After obtaining the corrected point cloud data, determine the spatial range of each corrected point cloud in the constructed multi-resolution pixel map; The multi-resolution pixel map is constructed by dividing the three-dimensional space into multiple grids at multiple levels. Each effective grid stores a pixel, and the pixel includes at least the position vector and normal vector obtained by statistical analysis of the corrected point cloud within the current effective grid. The hash indexing mechanism of the multi-resolution pixel map is invoked to locate the effective grid of the corresponding level according to the spatial range of the correction point cloud, and to search for the nearest neighbor pixels and associated point cloud sets of the current correction point cloud within the located effective grid. Determine whether the distance between the nearest neighbor element and the corrected point cloud is less than a set distance threshold, and whether the number of associated point clouds meets the preset matching requirements, and filter to obtain valid nearest neighbor matching results that simultaneously meet the distance threshold and the matching requirements.

5. The mapping and positioning method based on solid-state lidar according to claim 4, characterized in that, The process of performing plane fitting based on effective nearest neighbor matching results includes the following steps: For the set of associated point clouds corresponding to the effective nearest neighbor matching results obtained by screening, the covariance matrix is ​​solved by the least squares method, and the plane normal vector and plane reference point are obtained by eigenvalue decomposition to obtain the fitted plane and construct the complete plane equation. Calculate the flatness of the fitted plane and the vertical distance from each valid associated point cloud to the fitted plane. Mark the fitted planes whose flatness and vertical distance do not meet the preset filtering conditions as invalid planes and filter them to obtain valid planes.

6. The mapping and positioning method based on solid-state lidar according to claim 5, characterized in that, The localization degradation detection based on the planar features obtained by the plane fitting specifically includes the following steps: The degradation detection is performed sequentially, including normal direction detection, feature point density detection, Hessian matrix singularity detection, and semantic consistency detection. The normal direction detection includes collecting the plane normal vectors of all plane features corresponding to the effective planes, and analyzing the distribution of the plane normal vectors to obtain the normal vector distribution result; Determine whether the normal direction degradation condition is met based on the normal vector distribution results; The feature point density detection includes counting the number of valid associated point clouds per unit volume using the effective grid of the multi-resolution pixel map; The determination of whether the feature point degradation condition is met is based on the number of associated point clouds. The singularity detection of the Hessian matrix includes constructing a Hessian matrix from the point-plane residuals after plane fitting, and calculating the ratio of the minimum eigenvalue to the maximum eigenvalue of the Hessian matrix. Determine whether the singularity degradation condition is met based on the stated ratio; The semantic consistency detection includes: semantically labeling the correction point cloud using a lightweight semantic segmentation model to obtain the corresponding semantic category; statistically analyzing the proportion of the correction point cloud corresponding to a single semantic category; and determining whether the semantic consistency degradation condition is met based on the proportion of each correction point cloud and the distribution result of the normal vector. If none of the degradation conditions are met, it is determined to be a non-degradation scenario; if any of the degradation conditions are met, it is determined to be a mild degradation scenario; if two or more of the degradation conditions are met, it is determined to be a severe degradation scenario.

7. The mapping and positioning method based on solid-state lidar according to claim 6, characterized in that, The non-degenerate scenario employs a first localization mode that combines the effective nearest neighbor matching results for pose optimization, specifically including the following steps: The corrected point cloud data is converted into an adaptive multi-resolution sparse surface element that is consistent with the multi-resolution surface element map format. The sparse surface element contains a center position vector and a normal vector obtained by statistical analysis of the corrected point cloud. Based on the adaptive multi-resolution sparse surface elements, geometric error constraints for matching the corrected point cloud in two consecutive frames and prior error constraints for matching the current frame with the global map are constructed respectively. By combining the angular velocity data and acceleration data of the inertial measurement unit, an IMU physical constraint including acceleration error and angular velocity error is constructed; Using the control points of the continuous motion trajectory in continuous time as optimization variables, the weighted sum of all constraint loss functions is minimized, and the optimal continuous motion trajectory is obtained by iterative update. The final pose of the lidar is obtained by interpolation from the optimal continuous motion trajectory.

8. The mapping and positioning method based on solid-state lidar according to claim 6, characterized in that, If the scenario is determined to be a mild degradation scenario, the second positioning mode is executed, which includes the following steps: Construct a state estimator with the inertial measurement unit as the backbone, and the state vector includes at least position, velocity, attitude, accelerometer zero bias and gyroscope zero bias; Based on the measurement data, state recursion and covariance prediction are performed to obtain the prior state and prior covariance matrix based solely on IMU prediction. The pose obtained by matching the corrected point cloud is used as the external observation value. The observation equation is constructed and linearized. Based on the results of the semantic dimension detection, the observation noise covariance matrix is ​​dynamically adjusted. Calculate the Kalman gain, and based on the Kalman gain, the prior state, and the external observation, update the state and covariance matrix to obtain the posterior state and posterior covariance matrix that incorporate the laser information. The position and attitude parameters are extracted from the updated posterior state as the final pose of the lidar.

9. The mapping and positioning method based on solid-state lidar according to claim 6, characterized in that, If the scenario is determined to be a severe degradation scenario, the third positioning mode is executed, which is to obtain the final pose based on the IMU pose prediction result.

10. A mapping and positioning device based on solid-state lidar, characterized in that, Includes the following modules: The pose prediction module is used to predict pose based on the measurement data of the inertial measurement unit, obtain the IMU pose prediction result of the inertial measurement unit, and obtain the initial pose of the lidar by combining the extrinsic parameters of the lidar. The nearest neighbor matching and plane fitting module is used to perform distortion correction on the original point cloud data collected by the lidar based on the initial pose to obtain corrected point cloud data, and to perform nearest neighbor matching in combination with the constructed multi-resolution pixel map, filter out the effective nearest neighbor matching results and perform plane fitting. The degradation detection module is used to perform localization degradation detection based on the planar features obtained by the plane fitting, and output the degradation level of the current scene, which includes non-degradation, mild degradation and severe degradation. The pose update module is used to perform a graded response based on the degradation level to obtain the final pose of the lidar, and update the multi-resolution pixel map based on the final pose; wherein, the non-degraded scene adopts a first positioning mode that combines the effective nearest neighbor matching results for pose optimization, the mildly degraded scene adopts a second positioning mode, and the severely degraded scene adopts a third positioning mode.

Images