CN-122017878-A - Picture construction positioning method and device based on solid-state laser radar

CN122017878ACN 122017878 ACN122017878 ACN 122017878ACN-122017878-A

Abstract

The application relates to the field of laser radar map building and positioning, in particular to a map building and positioning method and device based on a solid-state laser radar. The method comprises the steps of carrying out pose prediction based on measurement data of an inertial measurement unit to obtain an initial pose of the laser radar, carrying out distortion correction on original point cloud data acquired by the laser radar, carrying out neighbor matching and plane fitting by combining a multi-resolution surface element map, carrying out positioning degradation detection based on plane characteristics to output degradation grades, carrying out grading response according to the degradation grades to obtain a final pose of the laser radar, and updating the multi-resolution surface element map. The method can adapt to the degradation conditions of different scenes and effectively improve the accuracy and reliability of laser radar map construction and positioning.

Inventors

YANG XIUYI
LIU XIAOQIANG

Assignees

昆山岚拓智能机器人有限公司

Dates

Publication Date: 20260512
Application Date: 20260311

Claims (10)

1. The map building and positioning method based on the solid-state laser radar is characterized by comprising the following steps of: Performing pose prediction based on measurement data of an inertial measurement unit to obtain an IMU pose prediction result of the inertial measurement unit, and combining external parameters of a laser radar to obtain an initial pose of the laser radar; Performing distortion correction on the original point cloud data acquired by the laser radar according to the initial pose to obtain corrected point cloud data, performing neighbor matching by combining the constructed multi-resolution face element map, screening to obtain an effective neighbor matching result, and performing plane fitting; Performing positioning degradation detection based on the plane characteristics obtained by the plane fitting, and outputting degradation levels of the current scene, wherein the degradation levels comprise non-degradation, slight degradation and severe degradation; and executing hierarchical response according to the degradation level to obtain a final pose of the laser radar, and updating the multi-resolution bin map according to the final pose, wherein the non-degraded scene adopts a first positioning mode for pose optimization by combining the effective neighbor matching result, the slightly degraded scene adopts a second positioning mode, and the severely degraded scene adopts a third positioning mode.
2. The mapping and positioning method based on the solid-state laser radar according to claim 1, wherein the pose prediction is performed based on measurement data of an inertial measurement unit to obtain an IMU pose prediction result of the inertial measurement unit, and the initial pose of the laser radar is obtained by combining external parameters of the laser radar, and specifically comprises the following steps: Acquiring measurement data of an inertial measurement unit in a sliding window, wherein the measurement data comprises angular velocity data and acceleration data of the inertial measurement unit, preprocessing the measurement data, and then performing integral operation to obtain pose increment at each moment in the sliding window, constructing a continuous motion track by combining a B spline curve on a plum cluster of the sliding window, and obtaining an IMU pose prediction result of the inertial measurement unit according to the continuous motion track; and obtaining external parameter data of the laser radar, carrying out pose conversion on the IMU pose prediction result according to the external parameter data, and calculating to obtain the initial pose of the laser radar under a global coordinate system.
3. The mapping and positioning method based on the solid-state laser radar according to claim 2, wherein the performing distortion correction on the original point cloud data acquired by the laser radar according to the initial pose to obtain corrected point cloud data specifically comprises the following steps: extracting original point cloud data acquired by the laser radar, and recording a time stamp for acquiring each original point cloud and an original coordinate under a laser radar coordinate system; based on the initial pose of the laser radar, combining a time stamp of each original point cloud with a sampling frequency of the laser radar, and obtaining a laser radar instantaneous pose corresponding to each original point cloud at the acquisition time through pose interpolation, wherein the laser radar instantaneous pose comprises an instantaneous rotation matrix and an instantaneous position vector; and converting each original point cloud from the laser radar coordinate system to a global coordinate system to obtain correction point cloud data.
4. The mapping and positioning method based on solid-state lidar according to claim 2, wherein the step of performing neighbor matching in combination with a preset multi-resolution bin map and screening to obtain an effective neighbor matching result specifically comprises the following steps: After the correction point cloud data are obtained, determining the space range of each correction point cloud in the correction point cloud data in the constructed multi-resolution bin map; the multi-resolution surface element map is constructed by dividing a three-dimensional space into a plurality of grids of multiple levels, each effective grid stores a surface element, and the surface element at least comprises a position vector and a normal vector which are obtained by the statistics of the correction point cloud in the current effective grid; Invoking a hash index mechanism of the multi-resolution bin map, positioning the effective grid of a corresponding level according to the space range of the correction point cloud, and searching a neighbor bin and an associated point cloud set of the current correction point cloud in the positioned effective grid; Judging whether the distance between the adjacent surface element and the correction point cloud is smaller than a set distance threshold value or not, and whether the number of the associated point clouds meets a preset matching requirement or not, and screening to obtain an effective adjacent matching result which meets the distance threshold value and the matching requirement simultaneously.
5. The mapping and positioning method based on solid-state lidar according to claim 4, wherein the plane fitting process based on the valid neighbor matching result comprises the steps of: Solving a covariance matrix by adopting a least square method for the association point cloud set corresponding to the effective neighbor matching result obtained by screening, obtaining a plane normal vector and a plane reference point through eigenvalue decomposition, obtaining a fitting plane and constructing a complete plane equation; Calculating the flatness of the fitting plane and the vertical distance between each effective associated point cloud and the fitting plane, marking the fitting plane which does not meet the preset filtering condition and corresponds to the vertical distance as an invalid plane, and filtering to obtain an effective plane.
6. The mapping and positioning method based on solid-state lidar according to claim 5, wherein the positioning degradation detection is performed based on the plane characteristics obtained by the plane fitting, specifically comprising the following steps: Sequentially executing multi-dimensional degradation detection including normal direction detection, feature point density detection, hessian matrix singular degree detection and semantic consistency detection; The normal direction detection comprises the steps of collecting the plane normal vectors of the plane characteristics corresponding to all the effective planes, and analyzing the distribution condition of the plane normal vectors to obtain a normal vector distribution result; The feature point density detection comprises the steps of counting the number of effective associated point clouds in a unit volume through the effective grid of the multi-resolution bin map; the Heisen matrix singular degree detection comprises the steps of constructing a Heisen matrix for point-plane residual errors after plane fitting, and calculating the ratio of the minimum eigenvalue to the maximum eigenvalue of the Heisen matrix; The semantic consistency detection comprises the steps of carrying out semantic labeling on the correction point clouds through a lightweight semantic segmentation model to obtain corresponding semantic categories, and counting the duty ratio of the correction point clouds corresponding to a single semantic category; if all the degradation conditions are not satisfied, judging the scene as a non-degradation scene, if any one of the degradation conditions is satisfied, judging the scene as a light degradation scene, and if more than 2 degradation conditions are satisfied, judging the scene as a heavy degradation scene.
7. The mapping and positioning method based on solid-state lidar of claim 6, wherein the non-degenerate scene adopts a first positioning mode for pose optimization by combining the effective neighbor matching result, and specifically comprises the following steps: converting the correction point cloud data into a self-adaptive multi-resolution sparse surface element consistent with the multi-resolution surface element map format, wherein the sparse surface element comprises a central position vector and a normal vector which are obtained by the statistics of the correction point cloud; Based on the self-adaptive multi-resolution sparse surface element, respectively constructing geometric error constraint matched by the correction point cloud of two continuous frames and prior error constraint matched by the current frame and the global map; combining the angular velocity data and the acceleration data of the inertial measurement unit to construct an IMU physical constraint comprising an acceleration error and an angular velocity error; and taking control points of the continuous motion track in continuous time as optimization variables, minimizing the weighted sum of all constraint loss functions, and carrying out iterative updating to obtain an optimal continuous motion track, and interpolating from the optimal continuous motion track to obtain the final pose of the laser radar.
8. The method for mapping and positioning based on solid-state lidar according to claim 6, wherein the second positioning mode is executed if the determination is made as a slightly degraded scene, specifically comprising the steps of: Constructing a state estimator taking the inertial measurement unit as a main body, wherein a state vector at least comprises a position, a speed, a gesture, an accelerometer zero offset and a gyroscope zero offset; performing state recursion and covariance prediction based on the measurement data to obtain a priori state and a priori covariance matrix only according to IMU prediction; Taking the pose obtained by the correction point cloud matching as an external observation value, constructing an observation equation and carrying out linearization treatment; Dynamically adjusting an observation noise covariance matrix based on the semantic dimension detection result; calculating a Kalman gain, and completing updating of a state and a covariance matrix according to the Kalman gain, the prior state and the external observation value to obtain a posterior state and a posterior covariance matrix fused with laser information; And extracting position and posture parameters from the updated posterior state to serve as the final pose of the laser radar.
9. The solid-state lidar-based mapping and positioning method of claim 6, wherein if the determination is made as to the severe degradation scene, a third positioning mode is performed, and the third positioning mode is used for obtaining the final pose according to the IMU pose prediction result.
10. The drawing construction positioning device based on the solid-state laser radar is characterized by comprising the following modules: The pose prediction module is used for predicting the pose based on the measurement data of the inertial measurement unit to obtain an IMU pose prediction result of the inertial measurement unit, and combining the external parameters of the laser radar to obtain the initial pose of the laser radar; The neighbor matching and plane fitting module is used for carrying out distortion correction on the original point cloud data acquired by the laser radar according to the initial pose to obtain corrected point cloud data, carrying out neighbor matching by combining the constructed multi-resolution face element map, screening to obtain an effective neighbor matching result and carrying out plane fitting; The degradation detection module is used for carrying out positioning degradation detection based on the plane characteristics obtained by the plane fitting and outputting degradation levels of the current scene, wherein the degradation levels comprise non-degradation, slight degradation and severe degradation; The pose updating module is used for executing hierarchical response according to the degradation level to obtain the final pose of the laser radar, and updating the multi-resolution bin map according to the final pose, wherein the non-degraded scene adopts a first positioning mode for pose optimization by combining the effective neighbor matching result, the slightly degraded scene adopts a second positioning mode, and the severely degraded scene adopts a third positioning mode.

Description

Picture construction positioning method and device based on solid-state laser radar Technical Field The application relates to the field of laser radar map building and positioning, in particular to a map building and positioning method and device based on a solid-state laser radar. Background Along with the evolution of the laser radar technology from a mechanical rotary optical array to an electronic control light beam (such as an optical phased array and a Flash technology), the research, development and production cost of the laser radar technology is remarkably reduced, and the laser radar technology is widely applied to the fields of robots, unmanned aerial vehicles, unmanned driving, surveying and mapping and the like and becomes a core sensor for equipment positioning perception navigation. The positioning and mapping method based on the laser radar is also optimized continuously, for example, in the prior art, the schemes of positioning and rectifying the vehicle-mounted laser radar through the true value of the coordinates of the target points on the tunnel site, constructing feature set positioning based on the environment identifier, predicting pose auxiliary point cloud noise point removal and grid map matching, establishing a likelihood domain optimization model to realize accurate positioning of the bumpy scene and the like are all improved in the specific scene. However, the existing laser radar positioning method is mainly aimed at the mechanical laser radar design with larger field angle, and positioning degradation problems still easily occur in scenes lacking multi-directional geometric constraints such as tunnels, long corridor and the like. More importantly, the solid-state laser radar can only scan limited geometric features when facing a single plane scene such as a wall surface in a short distance due to a small field angle, so that geometric constraint is seriously insufficient, and further, the optimized hessian matrix is caused to have singular values, and finally, positioning failure is caused. The prior art does not provide an effective solution for the solid-state laser radar positioning degradation, namely a scheme which simply depends on a laser radar directly fails in a degradation scene, and a scheme which partially fuses the IMU lacks an accurate degradation detection mechanism, or fuses the failure caused by the pollution of the IMU result by error data of the laser radar, or blindly switches to the IMU positioning to cause accuracy drift. The defect severely limits the application of the solid-state laser radar in common scenes such as indoor hallways, canyons, surrounding walls and the like, and becomes a core bottleneck for restricting the large-scale popularization of the solid-state laser radar. Therefore, a mapping and positioning method capable of accurately detecting the positioning degradation of the solid-state laser radar and guaranteeing the positioning continuity and accuracy through an adaptive strategy is needed. Disclosure of Invention In a first aspect, the present application provides a mapping and positioning method based on a solid-state laser radar, which adopts the following technical scheme: a mapping and positioning method based on solid-state laser radar comprises the following steps: Performing pose prediction based on measurement data of an inertial measurement unit to obtain an IMU pose prediction result of the inertial measurement unit, and combining external parameters of a laser radar to obtain an initial pose of the laser radar; Performing distortion correction on the original point cloud data acquired by the laser radar according to the initial pose to obtain corrected point cloud data, performing neighbor matching by combining the constructed multi-resolution face element map, screening to obtain an effective neighbor matching result, and performing plane fitting; Performing positioning degradation detection based on the plane characteristics obtained by the plane fitting, and outputting degradation levels of the current scene, wherein the degradation levels comprise non-degradation, slight degradation and severe degradation; and executing hierarchical response according to the degradation level to obtain a final pose of the laser radar, and updating the multi-resolution bin map according to the final pose, wherein the non-degraded scene adopts a first positioning mode for pose optimization by combining the effective neighbor matching result, the slightly degraded scene adopts a second positioning mode, and the severely degraded scene adopts a third positioning mode. By adopting the technical scheme, the initial pose of the laser radar can be obtained based on the measurement data of the inertial measurement unit, the distortion correction of the original point cloud data and the combination of the multi-resolution bin map are carried out on the neighbor matching and the plane fitting, the positioning degradation detection is carried out