A two-dimensional lidar positioning method based on intersection point constraints

Abstract

The present invention discloses a two-dimensional laser radar positioning method based on intersection point constraints, which belongs to the technical field of robot positioning. The method includes acquiring point cloud data of an indoor environment based on a two-dimensional laser radar; extracting straight line segments from the point cloud data using a seed region growing method; classifying the straight line segments based on an adaptive axis selection strategy based on the main extension direction of the line segments, and establishing a straight line matching relationship between adjacent frames based on the intersection information between the straight line segments and the coordinate axes; based on the straight line matching relationship between adjacent frames, minimizing the distance residual from the point to the line according to the Gauss-Newton equation to obtain the robot's posture information. Based on the matching results, the present invention uses the Gauss-Newton method to minimize the distance residual from the point to the line to obtain the robot's posture. The entire method has low computational time and can still maintain high efficiency under high point cloud density. It is suitable for low-power processors and meets the requirements of indoor robots for lightweight and real-time performance.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and in particular relates to a two-dimensional laser radar positioning method based on intersection point constraints. Background Art

[0002] With the rapid development of robotics and autonomous driving technologies, simultaneous localization and mapping (SLAM) technology is crucial. SLAM technology enables robots to determine their position in dynamic environments in real time and simultaneously build a map of their surroundings, which is the foundation for autonomous navigation and task execution.

[0003] In SLAM systems, sensor selection plays a crucial role. Sensor performance directly impacts the accuracy and reliability of SLAM algorithms, and also influences system cost and resource consumption. For example, vision sensors are relatively inexpensive and provide rich texture information, but they have limitations in acquiring depth information and are susceptible to lighting conditions. Inadequate or fluctuating lighting conditions can distort image features, making vision sensors prone to positioning errors. Radar, on the other hand, transmits and receives laser beams to accurately measure the distance to near-ground objects, providing robots with high-precision environmental perception data. Consequently, the application of lidar is becoming increasingly urgent. With the rise of technologies such as service robots and IoT-enabled logistics automation, fast and high-precision positioning solutions in indoor environments have become a key requirement. Three-dimensional lidar, with its multi-dimensional acquisition capabilities, is widely used in fields such as autonomous driving. However, in indoor robotics scenarios, the multi-beam scanning of traditional multi-line lidar requires precise optical layout and demanding mechanical assembly structures, resulting in high production and maintenance costs. Point clouds can reach tens of thousands, necessitating rapidly increasing computing power requirements and requiring high-performance processors. This not only increases hardware costs but also conflicts with the lightweight and low-power design goals of indoor robots. 2D LiDAR uses a single-beam scanning mechanism, significantly reducing production costs. Typical indoor spaces have distinct shapes, and a single 2D scan can capture essential environmental information. This ensures effective navigation while significantly reducing computational effort, enabling 2D SLAM algorithms to run in real time on embedded platforms, providing a viable solution for indoor robotics.

[0004] To achieve precise positioning, an appropriate scan matching strategy is indispensable. Indoor environments are rich in geometric structures such as lines and surfaces. Scan matching methods based on geometric structures can fully utilize these features to improve matching accuracy and stability. However, this method has many limitations. On the one hand, the extraction and matching of geometric structure features is computationally intensive, placing extremely high demands on computing hardware performance. On the other hand, most matching algorithms that rely on geometric structure features tend to use local features for matching, which can easily lead to mismatches in complex environments or areas with similar features, thus affecting positioning accuracy and map construction quality. For example, the Iterative Closest Point (ICP) algorithm in point cloud scan matching directly matches two consecutive frames of laser point clouds, recovering the relative pose transformation information by minimizing the Euclidean distance between the two point clouds. However, its point-by-point matching approach results in high time complexity. Although the PLICP algorithm, a variant of ICP, changes the distance metric from point-to-point to point-to-line, accelerating convergence and improving matching accuracy, the overall computational complexity remains high. Existing feature-based scan matching methods reduce computational overhead by matching feature points extracted from lidar data. They extract Euclidean invariant features from the scan data and use geometric hashing to match two frames of scan data. However, the similarity of local features can cause matching failures. Mathematical feature-based scan matching methods, such as the NDT algorithm, convert discrete 2D points into piecewise continuously differentiable probability densities on a two-dimensional plane, maximizing the likelihood of the target point cloud in the current probability field by optimizing pose parameters. Gaussian mixture models represent point clouds as a mixture of multiple Gaussian distributions and solve pose transformations by minimizing the difference between two Gaussian mixture models (e.g., KL divergence). While these methods can suppress the influence of local noise and outliers through probabilistic modeling and converge to the global optimum more easily than ICP, they are extremely sensitive to grid cell size, and improper settings can severely impact matching accuracy and computational efficiency. In terms of 2D lidar odometry and SLAM algorithms, Fast-SLAM decomposes the SLAM problem into two independent sub-problems: robot pose estimation and map coordinate posterior probability distribution estimation based on particle filters. Particle filtering technology is used to handle nonlinear systems, but a large number of particles are often required to obtain a good estimate, which can easily lead to memory overflow, and particle resampling may cause particle exhaustion. Existing technologies use particle filtering methods based on Fast-SLAM. Each particle represents a possible hypothesis of the robot's pose and an independently maintained grid map. The particle pose is adjusted by optimizing the matching of the current laser scan data with the particle map. It has good real-time and accuracy performance in indoor environments, but significant errors will accumulate over long periods of operation. Karto is the first open source SLAM algorithm based on graph optimization. It uses the current data frame and adjacent data frames to form a local map, and uses a correlation-based scan matching method to calculate the pose by aligning the data frame with the local map.Hector-SLAM does not require odometry information and directly uses the Gauss-Newton method for frame-to-map matching optimization to construct a grid map. However, map errors can affect subsequent matching and mapping performance. Furthermore, the computational complexity of scanning the map is high, requiring the robot to move at low speeds. International researchers have proposed the Cartographer algorithm, which incorporates sensor synchronization, a pose extrapolator, and laser data preprocessing. It introduces the concept of submaps and uses CSM and gradient optimization methods for matching within submaps, improving matching accuracy.

[0005] Current indoor robot SLAM technology has shortcomings in sensor selection and scan matching strategies. There is an urgent need for a method that can effectively improve computational efficiency while ensuring positioning accuracy to meet the application needs of indoor robots under resource-constrained conditions. Summary of the Invention

[0006] The purpose of the present invention is to address the above-mentioned deficiencies in the prior art and provide a two-dimensional lidar positioning method based on intersection point constraints to solve the problem in the existing SLAM algorithm that, although the traditional geometric feature matching method can utilize the rich linear structures in the indoor environment, it has high computational overhead and is prone to mismatching in complex environments.

[0007] In order to achieve the above object, the technical solution adopted by the present invention is:

[0008] A two-dimensional laser radar positioning method based on intersection point constraint comprises the following steps:

[0009] S1. Based on 2D LiDAR, obtain point cloud data of indoor environment;

[0010] S2, extract straight line segments from point cloud data using seed region growing method;

[0011] S3. Classify the straight line segments based on the adaptive axis selection strategy of the main extension direction of the line segments, and establish the straight line matching relationship between adjacent frames according to the intersection information of the straight line segments and the coordinate axes;

[0012] S4. Based on the straight line matching relationship between adjacent frames, the distance residual from the point to the straight line is minimized according to the Gauss-Newton equation to obtain the robot's posture information.

[0013] Furthermore, S2 specifically includes the following sub-steps:

[0014] S21. Select multiple continuous point clouds in the point cloud data, and select seed line segments based on constraints of point-to-point distance and point-to-line distance;

[0015] S22. Perform forward growth and reverse growth on the seed segment to obtain a straight line segment.

[0016] Furthermore, in S21, the constraint on the distance from a point to a line is expressed as:

[0017]

[0018] Where, Indicates the distance from each point cloud on the candidate seed segment to the fitted straight line; are the parameters of the fitted straight line; 、 Indicates the horizontal coordinate of the selected point cloud; Indicates the threshold value;

[0019] The point-to-point distance constraint is specifically expressed as:

[0020]

[0021] in:

[0022]

[0023]

[0024] Where, 、 is the position of the prediction point; is the angle of the current point cloud; Indicates the upper limit of the point-to-point distance.

[0025] Furthermore, S3 specifically includes the following sub-steps:

[0026] S31, using the intersection of the straight line segment and the body coordinate axis as a feature representation of the straight line segment, and determining the attributes of the coordinate axis intersected by the straight line segment based on an adaptive axis selection strategy for the main extension direction of the line segment;

[0027] S32. Establish a straight line matching relationship between adjacent frames based on the distribution of intersection points between the straight line segments and the corresponding coordinate axes on the axes, and then establish a matching relationship between the point cloud and the straight line.

[0028] Furthermore, S31 specifically includes:

[0029] Divide the feature space into two regions;

[0030] like , then the intersection of the straight line segment and the body coordinate on the x-axis is extracted as a feature, and the feature is matched with the x-axis feature set constructed from the previous frame; 、 Indicates the absolute value of the difference between the starting coordinates of the line segment;

[0031] like When , the intersection of the straight line segment and the body coordinate on the y-axis is extracted as a feature, and the feature is matched with the y-axis feature set constructed from the previous frame.

[0032] Furthermore, S3 also includes:

[0033] Based on the distribution of regular structures in the indoor environment, feature enhancement areas are established, and straight line segments belonging to the feature enhancement areas are selected. At the same time, the intersection points of the straight line segments with the x-axis and y-axis of the body coordinates are calculated, thereby constructing a complete intersection library of straight lines and coordinate axes;

[0034] Among them, the feature enhancement area is defined as:

[0035]

[0036] Where, 、 Indicates the boundary of the feature enhancement region.

[0037] Furthermore, S4 specifically includes the following sub-steps:

[0038] S41. Constructing the distance residual from the point cloud to the line based on the matching relationship between the point cloud and the line;

[0039] S42. Minimize the distance residual from point to line through the Gauss-Newton equation to obtain the robot's posture information.

[0040] Furthermore, in S41, the distance residual from the point cloud to the straight line is constructed, including:

[0041] Calculate the perpendicular distance from the point cloud to the line , which is expressed as:

[0042]

[0043] The distance residual from the point cloud to the straight line is .

[0044] Furthermore, S42 specifically includes:

[0045] The Gauss-Newton method is used to optimize the distance residual from the point cloud to the straight line to obtain the objective function , which is expressed as:

[0046]

[0047] in:

[0048]

[0049]

[0050] Where, is the coordinate of the original point cloud, Represents the coordinates of the original point cloud after coordinate transformation; pose , 、 represents the horizontal displacement of the robot, Indicates the yaw angle of the robot;

[0051] Will get the pose increment Transformed into the solution of the least squares problem, expressed as:

[0052]

[0053] According to the extreme value condition, the objective function right Taking the derivative and setting it to 0, we get the minimization problem of the Gauss-Newton equation, and then get the pose increment equation:

[0054]

[0055] Where, Represents the increment of pose during the optimization process; Represents the objective function Position The derivative of the variable; by solving the pose increment equation, the transformed pose between adjacent frames is obtained.

[0056] Furthermore, the objective function Position Derivative of a variable Expressed as:

[0057] .

[0058] The two-dimensional laser radar positioning method based on intersection point constraint provided by the present invention has the following beneficial effects:

[0059] The present invention transforms the high-dimensional point cloud matching problem into low-dimensional intersection matching, omitting the point-by-point search process; proposes a dual-axis intersection feature enhancement mechanism, which dynamically selects the intersection with the coordinate axis as the matching reference to avoid the matching degradation problem caused by the straight line being parallel to a single axis, establishes a feature enhancement area, and records the intersection of the straight line and the dual axes at the same time to ensure that continuous matching can be completed when the trend attributes of the straight line change.

[0060] Based on the matching results, the present invention uses the Gauss-Newton method to minimize the residual distance from the point to the line to obtain the robot's posture. The entire method has low computational time and can still maintain high efficiency under high point cloud density. It is suitable for low-power processors and meets the requirements of indoor robots for lightweight and real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 This is a flow chart of a two-dimensional laser radar positioning method based on intersection point constraints in an embodiment of the present invention.

[0062] Figure 2 Construct an indoor simulation environment map for the Gazebo physical simulator in an embodiment of the present invention.

[0063] Figure 3 Schematic diagram of point cloud data of the external environment in an embodiment of the present invention.

[0064] Figure 4 This is a result diagram of extracting straight line segments using the seed region growing method in an embodiment of the present invention.

[0065] Figure 5 Schematic diagram of an adaptive axis selection strategy based on the main extension direction of a line segment in an embodiment of the present invention; wherein, Figure 5 A in the figure is the result of matching using only the intersection of the straight line and the x-axis; Figure 5 B in the figure represents the distribution relationship between the line segment and the coordinate axis according to the horizontal trend information of the line segment; Figure 5 C in the equation is the result of matching the intersection of the straight line with the x-axis and the y-axis.

[0066] Figure 6 Schematic diagram of setting up a feature enhancement area in an embodiment of the present invention to establish a complete feature library; wherein, Figure 6 A in the figure is the feature enhancement area set to ensure the continuity of feature tracking. When the straight line segment is in area 2, its intersection with the x-axis and y-axis is recorded at the same time; Figure 6 B in the figure indicates that the axial attribute of the straight line segment L2 has changed, but the corresponding axial feature has been pre-stored in frame F1, so that L2 can correctly complete cross-frame matching.

[0067] Figure 7 This is the trajectory positioning result of the simulation experiment in the embodiment of the present invention; wherein, Figure 7 a in the figure is the trajectory result of simulation sequence 1; Figure 7 b in the figure is the trajectory result of simulation sequence 2; Figure 7 c in the figure is the trajectory result of simulation sequence 3; Figure 7 d in is the trajectory result of simulation sequence 4; Figure 7 The e in is the trajectory result of simulation sequence 5; Figure 7 f in is the trajectory result of simulation sequence 6.

[0068] Figure 8 Schematic diagram of a real experimental scenario in an embodiment of the present invention.

[0069] Figure 9 This is the actual experimental trajectory positioning result in the embodiment of the present invention, where: Figure 9a in is the trajectory result of the real sequence 1; Figure 9 b in is the trajectory result of the real sequence 2; Figure 9 c in the figure is the trajectory result of the real sequence 3; Figure 9 The d in is the trajectory result of the real sequence 4.

[0070] Figure 10 is the calculation time result of the odometer module in the embodiment of the present invention, where: Figure 10 a in the figure is the algorithm time consumption under simulation conditions; Figure 10 The b in the figure is the actual algorithm time consumption. DETAILED DESCRIPTION

[0071] The specific embodiments of the present invention are described below to facilitate understanding of the present invention by those skilled in the art. However, it should be clear that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the appended claims, these changes are obvious, and all inventions and creations utilizing the concepts of the present invention are protected.

[0072] The two-dimensional laser radar positioning method based on intersection point constraint of this embodiment addresses the problem that the existing two-dimensional laser SLAM algorithm relies on point-by-point nearest neighbor search or probabilistic grid matching, has high computational complexity, and is difficult to run in real time on resource-constrained embedded devices; this embodiment extracts a straight line from each frame of laser scanning data, constructs an objective function based on the distance from the point to the straight line, and uses the Gauss-Newton optimization method to solve the robot's posture, in which the intersection of the straight line and the coordinate axis is used as the search basis for matching the point cloud with the straight line. The number of straight line segments is much less than the number of point clouds in the laser scanning data, omitting the point-by-point matching in the matching optimization process, which effectively improves the real-time performance of the calculation. Figure 1 , which specifically includes the following:

[0073] S1. Based on 2D LiDAR, obtain point cloud data of indoor environment;

[0074] In some embodiments, a 2D laser sensor is mounted on a robot; assuming uniform motion between two consecutive frames of laser point clouds, a linear interpolation method is used to de-motion-distort the point clouds, thereby obtaining point cloud data of the indoor environment.

[0075] In one embodiment, reference Figure 2 and Figure 3 , build an indoor environment in the Gazebo physics simulator and use a 2D lidar to obtain (raw) point cloud data of the surrounding environment.

[0076] S2. Extract straight line segments from point cloud data using seed region growing method, which specifically includes the following steps:

[0077] S21. In some embodiments, straight line segments are extracted from the scanned point cloud, S consecutive point clouds are selected, and the parameters of the straight line fitted by these points are solved using orthogonal least squares. Then, seed line segments are selected based on the constraints of point-to-point distance and point-to-line distance.

[0078] The initial stage of the seeded region growing algorithm involves determining seed points. A seed segment can be interpreted as a line segment composed of a small number of consecutive point clouds. After acquiring laser data, S consecutive point clouds are selected one by one to evaluate whether they meet the seed segment criteria. If these point clouds meet the preset criteria, a region growing process is performed to identify a complete line segment. If not, the currently selected seed segment is discarded and a new one is selected.

[0079] Among them, the constraint of the distance from the point to the line is expressed as:

[0080]

[0081] Where, Indicates the distance from each point cloud on the candidate seed segment to the fitted straight line; are the parameters of the fitted straight line; 、 Indicates the horizontal coordinate of the selected point cloud; Indicates the threshold value;

[0082] Due to the point-to-point distance constraint, the candidate seed line segment point clouds are often small, so the error may be large. The parameters of the current seed line segment are determined based on the selected S point clouds. Figure 4 In the figure, the red point is the predicted position of the seed point, and the blue point is the actual position of the seed candidate point. When the error of the candidate seed segment is large, the gap between the predicted and actual positions is often large, so this judgment condition needs to be added when judging the seed segment. It is specifically expressed as follows:

[0083]

[0084] in:

[0085]

[0086]

[0087] Where, 、 is the position of the prediction point; is the angle of the current point cloud; Indicates the upper limit of the point-to-point distance.

[0088] S22. Perform forward growth and reverse growth on the seed segment to obtain a straight line segment.

[0089] S3. Classify the straight line segments based on the adaptive axis selection strategy of the main extension direction of the line segments, and establish the straight line matching relationship between adjacent frames based on the intersection information of the straight line segments and the coordinate axes. This specifically includes the following steps:

[0090] S31, using the intersection of the straight line segment and the body coordinate axis as a feature representation of the straight line segment, and determining the attributes of the coordinate axis intersected by the straight line segment based on an adaptive axis selection strategy for the main extension direction of the line segment;

[0091] The intersection of a line segment and the body coordinate axis is used as the feature representation of the line segment, and based on the distribution of these features along the coordinate axis, the correspondence between the line segments in two adjacent frames is established. However, this method has two key flaws. First, when the line feature is parallel or nearly parallel to a specific coordinate axis (such as the x-axis), the lack of valid intersections will lead to a decrease in feature quality. Second, small rotational perturbations between adjacent frames will cause the intersection position to shift significantly along the parallel axis. This geometric sensitivity not only introduces ambiguity in feature matching, but also causes the Jacobian matrix to become ill-conditioned during the optimization process, ultimately leading to cumulative errors in pose estimation.

[0092] Based on this, in a specific embodiment, an adaptive axis selection strategy based on the main extension direction of the line segment is proposed. By analyzing the spatial distribution characteristics of the line segment point coordinates, the feature space is divided into two regions, such as Figure 5 As shown in B;

[0093] Divide the feature space into two regions;

[0094] like , then the intersection of the straight line segment and the body coordinate on the x-axis is extracted as a feature, and the feature is matched with the x-axis feature set constructed from the previous frame; where, 、 Indicates the absolute value of the difference between the starting coordinates of the line segment;

[0095] like When , the intersection of the straight line segment and the body coordinate on the y-axis is extracted as a feature, and the feature is matched with the y-axis feature set constructed from the previous frame.

[0096] S32. Establish a straight line matching relationship between adjacent frames based on the distribution of intersection points between the straight line segments and the corresponding coordinate axes on the axes, and then establish a matching relationship between the point cloud and the straight line.

[0097] In some embodiments, when executing S3, a line may be near a boundary line where the horizontal trend changes. In this case, the coordinate axis to which the line segment belongs may switch between two frames, resulting in degradation of matching information. Therefore, it is necessary to establish a complete library of intersection points between lines and coordinate axes.

[0098] Therefore, based on this, in this embodiment, according to the distribution of regular structures in the indoor environment, a feature enhancement area is established, and the straight line segments belonging to the feature enhancement area are screened. At the same time, the intersection points of the straight line segments with the x-axis and y-axis of the body coordinate are calculated, thereby constructing a complete intersection point library of straight lines and coordinate axes.

[0099] Among them, the feature enhancement area is defined as:

[0100]

[0101] in, 、 Indicates the boundary of the feature enhancement region.

[0102] S4. Based on the straight line matching relationship between adjacent frames, the residual distance from the point to the straight line is minimized according to the Gauss-Newton equation to obtain the robot's posture information, which specifically includes the following steps:

[0103] S41. Constructing the distance residual from the point cloud to the line based on the matching relationship between the point cloud and the line;

[0104] Calculate the perpendicular distance from the point cloud to the line , which is expressed as:

[0105]

[0106] Since the denominator is a fixed constant and can be ignored, the numerator can be directly used as the distance residual from the point cloud to the line.

[0107] S42. Minimize the residual of the distance from the point to the line through the Gauss-Newton equation to obtain the posture information of the robot, which specifically includes the following:

[0108] The residual distance from the point cloud to the straight line is , use the Gauss-Newton method to optimize and solve the objective function , which is expressed as:

[0109]

[0110] in:

[0111]

[0112]

[0113] Where, is the coordinate of the original point cloud, Represents the coordinates of the original point cloud after coordinate transformation; pose , 、 represents the horizontal displacement of the robot, Indicates the robot's yaw angle and posture This is what needs to be solved in this embodiment;

[0114] The goal of this embodiment is to find a pose increment , making Reach the minimum; transform the problem into solving the least squares problem:

[0115]

[0116] in, Represents the objective function Position The derivative of a variable is expressed as:

[0117]

[0118] According to the extreme value condition, the objective function right Taking the derivative and setting it to 0, we get the minimization problem of the Gauss-Newton equation, and then get the pose increment equation:

[0119]

[0120] Where, Indicates the increment of pose during the optimization process.

[0121] By solving the pose increment equation, the transformed pose between adjacent frames is obtained.

[0122] In one embodiment, reference Figure 5 Figure A shows a typical mismatch. In frame F1, the intersection points of the three lines [L1, L2, L3] with the x-axis are denoted as p1, p2, and p3, respectively. In the adjacent frame F2, the intersection points of the lines [L1, L2, L3] with the x-axis are p′1, p′2, and p′3, respectively. Notably, line L1 is nearly parallel to the x-axis, and its features drift significantly due to inter-frame pose changes. Its projected point p′1 forms a pseudo-nearest neighbor correspondence with the original point p2 (shown by the red dashed line), significantly amplifying the difference in the projected distances between the true corresponding points.

[0123] In one embodiment, reference Figure 5 In C, the adaptive axis selection strategy based on the main extension direction of the line segment detects that the main extension direction of line L1 is aligned with the y-axis. Therefore, the algorithm switches to y-axis projection mode, extracts the intersection point on the y-axis as a feature, and correctly matches this point to the y-axis feature set built from the previous frames. This approach effectively alleviates the geometric degradation caused by selecting a single axis for projection.

[0124] In a specific embodiment, when the spatial distribution of the straight line segments approaches the axis selection threshold, the feature representation mode is switched. In frame F1, the straight line L2 satisfies <1, record its intersection with the y-axis. However, in frame F2, due to the change in posture, the algorithm switches to the x-axis projection mode. Figure 5 As can be seen from C in the figure, there is no feature matching L2 in the x-axis feature library of F1. This difference in feature expression caused by the dynamic axis selection mechanism leads to a discontinuous mapping of the feature space across frames. After introducing this feature enhancement region, the line segment space is divided into three types of regions, such as Figure 6 As shown in Figure 1, within the feature enhancement region, a dual-axis intersection synchronization update strategy is used to simultaneously calculate the intersection of each line segment with the x-axis and y-axis. This approach creates a dynamic redundancy in the number of feature points generated in the current frame, thereby constructing an effective feature buffer and ensuring the continuity of geometric feature tracking.

[0125] As shown in Figure 6B, in frame F1, L1 is a longitudinal feature line, and the coordinates of its y-axis intersection are stored in the y-axis feature set. L3 is a transverse feature line, and the coordinates of its intersection with the x-axis are stored in the x-axis feature set. Specifically, a composite feature analysis is performed on L2, and its intersections with both the x-axis and the y-axis are extracted simultaneously. When processing frame F2, the axial properties of L1 and L3 remain unchanged, naturally forming a stable cross-frame match. Although the axial properties of L2 have changed, pre-storing the features of L2 in frame F1 ensures the continuity of L2's cross-frame matching.

[0126] refer to Figure 7 The trajectory result corresponding to the method of the present invention is LIA-LO (a two-dimensional laser radar positioning method based on intersection point constraints). In the simulation scenario, compared with the point-based positioning method ICP (Iterated Closest Pointsrrn), the point-to-line positioning method PLICP (An ICP variant using a point-to-line metric), and the grid-based positioning method Hector-SLAM (A Flexible and Scalable SLAM System with Full 3D Motion Estimation), the method of the present invention has good observation results for both translation and rotation, and is very close to the true value.

[0127] In one embodiment, reference Figure 8 The hardware platform of the real experiment uses a small vehicle as a carrier, equipped with lidar, low-cost edge computers, batteries, Figure 9The trajectory estimation results of the real experiment show that the positioning accuracy of the method proposed in this paper is on par with Hector-SLAM and PLICP.

[0128] The time complexity analysis of LIA-LO includes extracting straight line segments using the seeded region growing method, calculating and matching the intersection points of the straight line segments with the coordinate axes, and calculating the pose transformation using the Gauss-Newton optimization process.

[0129] 1) Select seed points from the point cloud sequence. For each seed point, check whether its neighboring points satisfy the collinearity condition. The time complexity of a single point neighborhood search query is O(1). Assuming that each line segment contains an average of L points, the time required to generate a line segment is O(L). If M line segments are extracted, the total time complexity is O(L ∗ M). Since L ∗ M ≈ N, the total time complexity of extracting line segments is O(N).

[0130] 2) Calculate the intersection of the line segment and the coordinate axis, with a complexity of O(M), and then perform strong feature matching with a complexity of O(M²). Therefore, the total time complexity of feature matching is O(M²);

[0131] 3) The time complexity of a single Gauss-Newton attitude transformation iteration is O(N). Given K iterations, the total time complexity is O(KN). Therefore, the total time complexity of LIA-LO is:

[0132]

[0133] In the structural environment, M is much smaller than N; the calculation time results of the odometer module in the simulation and real experiment are as follows Figure 10 As shown, it can be seen that the method proposed in the present invention has the best computational efficiency compared with Hector-SLAM, PLICP, and ICP.

[0134] Although the specific embodiments of the invention are described in detail in conjunction with the accompanying drawings, this should not be construed as limiting the scope of protection of this patent. Within the scope described by the claims, various modifications and variations that can be made by those skilled in the art without creative work still fall within the scope of protection of this patent.

Claims

1. A two-dimensional laser radar positioning method based on intersection point constraint, characterized in that: The following steps are involved: S1. Based on 2D LiDAR, obtain point cloud data of indoor environment; S2, extract straight line segments from point cloud data using seed region growing method; S3. Classify the straight line segments based on the adaptive axis selection strategy of the main extension direction of the line segments, and establish the straight line matching relationship between adjacent frames according to the intersection information of the straight line segments and the coordinate axes; S4. Based on the straight line matching relationship between adjacent frames, the residual of the distance from the point to the straight line is minimized according to the Gauss-Newton equation to obtain the robot's posture information; The S3 specifically includes the following sub-steps: S31, using the intersection of the straight line segment and the body coordinate axis as a feature representation of the straight line segment, and determining the attributes of the coordinate axis intersected by the straight line segment based on an adaptive axis selection strategy for the main extension direction of the line segment; S32, establishing a straight line matching relationship between adjacent frames based on the distribution of intersection points between the straight line segments and the corresponding coordinate axes on the axes, and then establishing a matching relationship between the point cloud and the straight line; The S31 specifically includes: Divide the feature space into two regions; like , then the intersection of the straight line segment and the body coordinate x-axis is extracted as a feature, and the feature is matched with the x-axis feature set constructed from the previous frame; where, 、 Indicates the absolute value of the difference between the starting coordinates of the line segment; are the parameters of the fitted straight line; like When , the intersection of the straight line segment and the body coordinate on the y-axis is extracted as a feature, and the feature is matched with the y-axis feature set constructed from the previous frame; Said S3 further comprises: Based on the distribution of regular structures in the indoor environment, feature enhancement areas are established, and straight line segments belonging to the feature enhancement areas are selected. At the same time, the intersection points of the straight line segments with the x-axis and y-axis of the body coordinates are calculated, thereby constructing a complete intersection library of straight lines and coordinate axes; Among them, the feature enhancement area is defined as: Where, 、 Indicates the boundary of the feature enhancement region.

2. The two-dimensional laser radar positioning method based on intersection point constraint according to claim 1, characterized in that: The S2 specifically includes the following sub-steps: S21. Select multiple continuous point clouds in the point cloud data, and select seed line segments based on constraints of point-to-point distance and point-to-line distance; S22. Perform forward growth and reverse growth on the seed segment to obtain a straight line segment.

3. The two-dimensional laser radar positioning method based on intersection point constraint according to claim 2, characterized in that: In S21, the constraint of the distance from the point to the line is expressed as: Where, Indicates the distance from each point cloud on the candidate seed segment to the fitted straight line; are the parameters of the fitted straight line; 、 Indicates the horizontal coordinate of the selected point cloud; Indicates the threshold value; The point-to-point distance constraint is specifically expressed as: in: Where, 、 is the position of the prediction point; is the angle of the current point cloud; Indicates the upper limit of the point-to-point distance.

4. The two-dimensional laser radar positioning method based on intersection point constraint according to claim 3 is characterized in that: The S4 specifically includes the following sub-steps: S41. Constructing the distance residual from the point cloud to the line based on the matching relationship between the point cloud and the line; S42. Minimize the distance residual from point to line through the Gauss-Newton equation to obtain the robot's posture information.

5. The two-dimensional laser radar positioning method based on intersection point constraint according to claim 4, characterized in that: In S41, constructing the distance residual from the point cloud to the straight line includes: Calculate the perpendicular distance from the point cloud to the line , which is expressed as: The distance residual from the point cloud to the straight line is .

6. The two-dimensional laser radar positioning method based on intersection point constraint according to claim 5, characterized in that: The S42 specifically includes: The Gauss-Newton method is used to optimize the distance residual from the point cloud to the straight line to obtain the objective function , which is expressed as: in: Where, is the coordinate of the original point cloud, Represents the coordinates of the original point cloud after coordinate transformation; pose , 、 represents the horizontal displacement of the robot, Indicates the yaw angle of the robot; Will get the pose increment Transformed into the solution of the least squares problem, expressed as: According to the extreme value condition, the objective function right Taking the derivative and setting it to 0, we get the minimization problem of the Gauss-Newton equation, and then get the pose increment equation: Where, Represents the increment of pose during the optimization process; Represents the objective function Position The derivative of the variable; by solving the pose increment equation, the transformed pose between adjacent frames is obtained.

7. The two-dimensional laser radar positioning method based on intersection point constraint according to claim 6, characterized in that: Objective function Position Derivative of a variable Expressed as: 。

Images