A high-precision indoor self-positioning method based on laser odometry

Abstract

The present invention discloses a high-precision indoor self-positioning method based on a laser odometer, comprising a mechanically rotating three-dimensional multi-line laser radar mobile platform, a portable computing device, and multiple reflective markers. First, the laser radar device is fixedly mounted on the movable platform, and multiple reflective markers of different shapes are arranged in the indoor scene to achieve long-distance positioning; then, the laser radar is started, and environmental point cloud data is obtained through the portable computing device. Point sets with large reflection intensity values ​​are screened out, and a clustering algorithm is used to distinguish reflective markers of different shapes and filter out outliers; the laser radar position under a single reflective marker is calculated based on the angular relationship between the laser radar coordinate system and the world coordinate system; finally, the sum of the distance values ​​of the reflective marker reference point cloud is normalized, and the final position of the laser radar is estimated by calculating the distance value weight of each reflective marker reference point.

Description

Technical Field

[0001] The present invention relates to the field of indoor self-positioning using LiDAR (Light Detection and Ranging). Specifically, the invention utilizes the characteristic of LiDAR that materials with different reflectivities have different reflection intensity measurement values. By introducing a specially shaped high-reflectivity marker at a reference position, the reflective marker reference point is screened out based on point cloud data measured by the LiDAR, and the position coordinates of the LiDAR are calculated using the angle and distance information of the reference point. This method significantly reduces the time complexity of the traditional LiDAR odometry (LO) algorithm of simultaneous localization and mapping (SLAM) technology, while improving positioning accuracy. Background Art

[0002] In recent years, with the rapid development of robotics and sensor technology, indoor mobile robots have become increasingly important in the robotics field, making research on indoor positioning technology particularly important. As a core technology for the next-generation Internet of Things (IoT), indoor positioning has enormous market demand. Due to the complex and volatile topology and layout of indoor spaces, coupled with the complex electromagnetic environment that affects effective signal transmission, signal propagation suffers from multipath errors and non-line-of-sight errors. These factors significantly reduce the accuracy of outdoor positioning technologies such as satellite positioning and navigation when applied indoors. Indoor positioning systems have undergone decades of development and refinement, encompassing technologies such as Wi-Fi, Bluetooth, ultrasound, radio frequency identification (RFID), and ultra-wideband (UWB). However, these wireless positioning technologies struggle to maintain accurate positioning in complex indoor environments due to the impact of multipath propagation on radio signals. LiDAR, with its advantages of high resolution, strong interference immunity, and immunity to light, has made indoor positioning technology based on LiDAR increasingly a hot research area.

[0003] The use of laser point clouds to solve the localization and mapping problems of three-dimensional scenes in SLAM can be traced back to the LOAM (LiDAR Odometry and Mapping) algorithm. This algorithm uses a novel method to extract line and surface features from laser point clouds to reduce computational complexity. It also pioneered the separation of the motion estimation problem into two independent algorithms. One algorithm performs high-frequency odometry and returns a low-precision motion estimate, while the other runs less frequently for matching and mapping but returns a highly accurate motion estimate. Ultimately, the two data sets are fused into a high-frequency, high-precision motion estimate, achieving a good balance between accuracy and efficiency and high real-time performance. However, its only drawback is the lack of loop closure detection, which can lead to large cumulative drift errors in mapping and position estimation. LeGO-LOAM (Lightweight and Ground-Optimized LiDAR Odometry and Mapping) builds on LOAM's foundation by improving feature extraction with lightweight and ground optimization, and adding loop closure detection. This framework aligns with the established framework of graph-optimized 3D LiDAR SLAM solutions, resulting in a more complete map. HDL_Graph_SLAM, a graph-based SLAM solution based on the Velodyne HDL-32E lidar, can better build a globally consistent map by integrating information from the Global Positioning System (GPS), Inertial Measurement Unit (IMU), and road constraints during back-end optimization. However, adding sensors and loop detection imposes significant computational overhead on the system, and loop detection suffers from perceptual bias and variation, requiring a trade-off between accuracy and recall, impacting the real-time performance of SLAM.

[0004] In addition to mapping, these SLAM algorithms propose global pose estimation by using point cloud registration algorithms to obtain laser odometry for the current frame. However, the sparsity of point clouds and motion interference can significantly reduce the accuracy of laser odometry. Laser odometry accuracy can be improved by fusing state estimation data from the IMU. These methods are mainly divided into two categories: loosely coupled methods, which calculate state estimation data from the lidar and IMU separately, discarding a large amount of irrelevant data in exchange for a smaller computational load. Tightly coupled methods, also known in the field as laser inertial odometry (LIO), use IMU measurements to provide state predictions, and measurement updates are used to correct the predicted state. LIO_mapping (LiDAR Inertial Odometry and Mapping) is the first to propose a tightly coupled LiDAR-IMU fusion method. By jointly optimizing the measurement data from the IMU and LiDAR, it corrects drift errors. Compared to loosely coupled methods, tightly coupled methods significantly improve accuracy. As an update to LeGO-LOAM, LIO-SAM implements tightly coupled laser-inertial odometry based on a graph optimization framework. This type of odometry calculation method based on point cloud registration relies on feature extraction from laser point clouds and motion state estimation, which has high time complexity. Furthermore, the odometry only contains prior motion information, which can easily lead to inaccurate pose estimation.

[0005] Previous work has primarily focused on processing pure geometric information to correct point cloud errors, neglecting intensity information. In fact, intensity information is related to the reflectivity of materials, and the differences in reflectivity between different types of objects are particularly pronounced, making it valuable for positioning and object recognition. Introducing reflective markers into a SLAM system can exploit the LiDAR's ability to easily distinguish highly reflective materials. By fixing the reflective marker's position, the LiDAR's position coordinates can be calculated based on the measured angle and distance information, enabling autonomous positioning of the LiDAR.

[0006] This method calculates position solely through LiDAR acquisition of the angle and distance information of reflective reference points in a three-dimensional scene. It does not incorporate algorithms like odometry that use prior information for a posteriori estimation. This method can correct for the accumulated drift errors caused by inter-frame estimation during motion, thereby replacing loop closure detection, which is computationally complex and inefficient, and is expected to achieve high-precision positioning in indoor scenarios. However, in reality, the laser point cloud of a reflective marker is not an ideal particle model, but rather a collection of scattered points. This requires that reference points be selected based on the LiDAR angular resolution and the shape and size of the reflective marker to avoid positioning errors caused by improper reference point selection. Summary of the Invention

[0007] The purpose of the present invention is to provide a high-precision indoor self-positioning method based on a laser odometry. Compared with the traditional laser odometry method, the present invention utilizes the characteristic of laser radar that it has a higher reflection intensity value for high-reflectivity materials, introduces a reflective marker at the reference position, and then solves the position of the laser radar according to the angle and distance information of the marker; at the same time, considering that the reflective marker is not an ideal particle model in actual application, it is necessary to select the reference point based on the laser radar angular resolution and the shape and size of the reflective marker. Due to the limitation of the laser radar measurement distance and the interference of motion distortion, it is difficult to complete long-distance indoor high-precision positioning in a complex environment with only a single reflective marker. This method introduces N reflective markers of different shapes to complete long-distance positioning, which can significantly reduce the algorithm time complexity of the traditional laser odometry while correcting the accumulated drift error of the odometry.

[0008] The technical solution adopted by the present invention is: a high-precision indoor self-positioning method based on a laser odometry, comprising a mechanically rotating three-dimensional multi-line laser radar mobile platform, a portable computing device, and N reference reflective markers. The laser radar device is fixed on the mobile platform, and the coordinate system with the laser radar as the origin is called the laser radar coordinate system, which corresponds to the local map composed of each frame of point cloud, while the coordinate system with the initial position of the laser radar as the origin is called the world coordinate system, which corresponds to the global map composed of all point clouds. A specially shaped reflective marker R is placed at a reference position in the indoor environment. i (1≤i≤N), which is made of reflective material with high reflectivity and is characterized by a high reflection intensity value, and records R i Coordinate values ​​in the world coordinate system It is characterized by comprising the following steps:

[0009] Step 1: Start the LiDAR, obtain point cloud data through the connected portable computing device and store it in the collection Take out at the same time The reflection intensity value I is normalized and stored in the collection Eliminate the influence of different intensity values ​​I due to distance changes of the same object, so as to be used for the subsequent reflection marker R i The feature extraction is expressed as:

[0010]

[0011] Step 2: Collect the collected data I i Sort from large to small, filter out isolated points with large individual intensity values, record their index values, and get the set

[0012] Step 3: Point cloud collection after processing the i-th frame The maximum intensity value in is divided into M subsets C using clustering algorithm u (1≤u≤M≤N), corresponding to the point cloud of M reflective markers in space (not all reflective markers may appear in a single frame point cloud);

[0013] Step 4: From subset C u Take out the point with the largest reflection intensity value and its original data such as distance and azimuth, and store them in T i In the set C, u Constructing a straight line and set C u These two situations of constituting a plane are handled;

[0014] Step 5: If T i If the collection only contains point clouds on a single line, the location ID of the center point can be obtained by taking the average of the index i , if T i If the collection contains multiple harness point clouds, the ID numbers are averaged and rounded to get the single harness index value ID. mid , then the ID mid The point cloud ID number in the average is obtained by i , and finally the calculated ID i Save to the ref collection (i.e. reflection marker R i corresponding reference point cloud set);

[0015] Step 6: Record the rotation angle of the lidar coordinate system around the z-axis of the world coordinate system According to the index number of the reference point ref, the point cloud collection The incident angle α formed by the incident laser beam and the xoy plane, the angle γ formed by the projection of the reference point ref on the xoy plane and the y axis, and the distance d from the laser radar to ref are obtained in the laser radar coordinate system;

[0016] Step 7: Rotate the laser radar coordinate system around the z-axis of the world coordinate system The incident angle α, horizontal rotation angle γ and distance value d in the laser radar coordinate system are used to calculate the coordinate value of the reference point ref in the laser radar coordinate system. The relationship satisfies:

[0017]

[0018] Step 8: Assume that the coordinate value of the laser radar in the world coordinate system is (x L ,y L ,z L ), according to the conversion relationship between the lidar coordinate system and the world coordinate system, the reference point ref can be converted from the lidar coordinate system to the world coordinate system. The conversion relationship is expressed as:

[0019]

[0020] Step 9: According to the above conversion relationship, the coordinate value (x L ,y L ,z L )for:

[0021]

[0022] in, is the coordinate of the reference point ref in the laser radar coordinate system, and its equivalent relationship is:

[0023]

[0024] Step 10: It can be further converted into matrix form:

[0025]

[0026] Step 11: The above steps obtain the value of a reflective marker R i The laser radar position coordinates are further analyzed for the M subsets C in step 3. u Repeat steps 4 to 10 to obtain the laser radar position coordinates (x L ,y L ,z L );

[0027] Step 12: According to each subset C in step 5 u The corresponding ref, normalizes the sum of its distance values ​​d, and calculates the distance value weight of each reference point to estimate the position of the lidar (x′ L ,y′ L ,z′ L ), whose expression is:

[0028]

[0029] Step 13: Assemble the point cloud By performing the above steps on all frames in the image, the motion trajectory of the lidar in the SLAM process can be obtained, and the high-precision indoor self-positioning of the lidar can be completed. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 This is a layout diagram of the laser radar platform of the present invention in an indoor scene;

[0031] Figure 2 This is the reflection intensity value diagram of the indoor scene of the present invention (including reflection markers);

[0032] Figure 3 This is a diagram showing the conversion relationship between the laser radar coordinate system and the world coordinate system of the present invention;

[0033] Figure 4 This is the laser radar self-positioning error diagram of the present invention and the two SLAM algorithm odometers. Specific implementation plan

[0034] The present invention is further described in detail below with reference to the accompanying drawings.

[0035] The technical solution adopted by the present invention is a high-precision indoor self-positioning method based on a laser odometry, which includes a mechanically rotating three-dimensional multi-line laser radar mobile platform, a portable computing device, and N reference reflective markers. The laser radar device is fixed to the mobile platform. The coordinate system with the laser radar as the origin is called the laser radar coordinate system, which corresponds to the local map composed of each frame of point cloud. The coordinate system with the initial position of the laser radar as the origin is called the world coordinate system, which corresponds to the global map composed of all point clouds. It is characterized by comprising the following steps:

[0036] Step 1: Figure 1 As shown, the laser radar device is fixedly installed on the mobile platform and placed at the starting position. A reflective marker R is selected and fixed in the indoor corridor environment. The reflective marker R can be made of a reflective material with high reflectivity and is characterized by a high reflection intensity value. The coordinate value of R in the world coordinate system (x R ,y R ,z R );

[0037] Step 2: Start the lidar, run the SLAM algorithm, obtain point cloud data through the connected portable computing device and store it in the collection Take out at the same time The reflection intensity value I is stored in the collection So that it can be used for feature extraction of reflective markers later;

[0038] Step 3: In an indoor environment, the atmospheric transmission coefficient and the system transmission coefficient can be considered constants, and the change in I caused by the difference in distance values ​​is not obvious. Therefore, the reflection intensity value I is greatly affected by the reflectivity of the object material. It can be obtained by comparing the reflection intensity value set The reference point ref of the reflective marker is filtered and estimated, and the reflection intensity values ​​of the reflective marker and other objects in the indoor environment are visible Figure 2 ;

[0039] Step 4: According to the index number of the reference point ref, you can find the index in the point cloud collection The incident angle α formed by the incident laser beam and the xoy plane, the angle γ formed by the projection of the reference point ref on the xoy plane and the y axis, and the distance d from the laser radar to ref are obtained in the laser radar coordinate system;

[0040] Step 5: Figure 3 As shown, according to the angle of rotation of the laser radar coordinate system around the z-axis of the world coordinate system The vertical angle α and horizontal rotation angle γ in the laser radar coordinate system and the distance value d are used to calculate the coordinate value (x′) of the reference point ref in the laser radar coordinate system. R ,y′ R ,z′ R ), the relationship satisfies:

[0041]

[0042] Step 6: Assume that the coordinate value of the laser radar in the world coordinate system is (x L ,y L ,z L ), the conversion relationship between the lidar coordinate system and the world coordinate system is shown in Figure 3 , the reference point ref can be converted from the lidar coordinate system to the world coordinate system, and the conversion relationship is expressed as:

[0043]

[0044] Step 7: According to the above conversion relationship, the coordinate value (x L ,y L ,z L )for:

[0045]

[0046] Among them, (x′ R ,y′ R ,z′ R ) is the coordinate of the reference point in the laser radar coordinate system, and its equivalent relationship is:

[0047]

[0048] Step 8: It can be further converted into a matrix form:

[0049]

[0050] Step 9: Point Cloud Collection By performing the above steps on all data frames in the SLAM process, the motion trajectory of the laser radar can be obtained, and high-precision indoor positioning can be completed. Figure 4This is a comparison chart of the autonomous positioning errors of the two SLAM algorithm odometers, LOAM and LeGO-LOAM, and the lidar method of the present invention in the experiment;

[0051] Step 10: The above steps obtain the laser radar position coordinates based on a certain reflective marker R, and then introduce N reflective markers and repeat the above steps;

[0052] Step 11: From step 2, gather Using the clustering algorithm, we can get M subsets C according to the maximum intensity value. u (1≤u≤M≤N), corresponding to the point cloud of M reflective markers, the expression is:

[0053]

[0054] Then we can get the laser radar position coordinates (x L ,y L ,z L );

[0055] Step 12: Obtain the reflection marker point cloud set C from step 3 u The corresponding ref, normalize the sum of its distance value d, calculate the distance value weight of each reflective marker reference point to estimate the position coordinates of the lidar (x′ L ,y′ L ,z′ L ), the expression is:

[0056]

[0057] Finally, the motion trajectory of the lidar can be obtained to achieve high-precision self-positioning indoors.

[0058] The step three comprises the following steps:

[0059] Step 3 (1) Collected collection Frame by Frame I i Sort by order from large to small, and filter out isolated points with large individual intensity values ​​that lead to misjudgment of reflection markers {I i,k},I i,k ∈I i , record the original index value of each point, and get the set The relationship satisfies:

[0060] Among them, k is the isolated point in the i-th frame point cloud I i The index value in ;

[0061] Step 3 (2): Point cloud collection after processing from the i-th frame Take out the point with the largest reflection intensity value and its original data such as distance and azimuth, and store them in T i middle;

[0062] Step 3 (III), T i There are two types of situations for the points in the image. One is to form a straight line with the adjacent points, and the other is to form a plane with the adjacent points. This depends on the size of the selected single reflective marker and the angular resolution of the laser radar. The laser radar scans in a fixed direction (clockwise or counterclockwise). If T i The point in the set is the first case. Simply take the mean of the index to get the position ID of the center point. i , and the relationship satisfies:

[0063]

[0064] Step 3 (4): If T i If the points in the set belong to the second case, then the ID numbers of multiple bundles need to be averaged and rounded to get the ID mid ,satisfy:

[0065]

[0066] Step 3 (5) further ID mid Substitute the point in step 5 (3) for T i Perform calculations;

[0067] Step 3 (6): Finally, the calculated ID i Stored in the ref collection, the expression is:

[0068]

Claims

1. A high-precision indoor self-positioning method based on laser odometry, comprising a mechanically rotating 3D multi-line laser radar mobile platform, a portable computing device, and N reference reflective markers. The laser radar device is fixed to the mobile platform. The coordinate system with the laser radar as the origin is called the laser radar coordinate system, which corresponds to the local map composed of each frame of point cloud. The coordinate system with the laser radar initial position as the origin is called the world coordinate system, which corresponds to the global map composed of all point clouds. Specially shaped reflective markers R are placed at reference positions in the indoor environment. i (1≤i≤N), which is made of reflective material with high reflectivity and is characterized by a high reflection intensity value, and records R i Coordinate values ​​in the world coordinate system It is characterized by The following steps are involved: Step 1: Start the LiDAR, obtain point cloud data through the connected portable computing device and store it in the collection Take out at the same time The reflection intensity value I is normalized and stored in the collection Eliminate the influence of different intensity values ​​I due to distance changes of the same object, so as to be used for the subsequent reflection marker R i The feature extraction is expressed as: Step 2: Collect the collected data I i Sort from large to small, filter out isolated points with large individual intensity values, record their index values, and get the set Step 3: Point cloud collection after processing the i-th frame The maximum intensity value in is divided into M subsets C using clustering algorithm u (1≤u≤M≤N), corresponding to the point cloud of M reflective markers in space; Step 4: From subset C u Take out the original data of the point with the largest reflection intensity value and its distance and azimuth, and store them in T i In the set C, u Constructing a straight line and set C u These two situations of constituting a plane are handled; Step 5: If T i If the collection only contains point clouds on a single line, the location ID of the center point can be obtained by taking the average of the index i , if T i If the collection contains multiple harness point clouds, the ID numbers are averaged and rounded to get the single harness index value ID. mid , then the ID mid The point cloud ID number in the average is obtained by i , and finally the calculated ID i Save to ref collection; Step 6: Record the rotation angle of the lidar coordinate system around the z-axis of the world coordinate system According to the index number of the reference point ref, the point cloud collection The incident angle α formed by the incident laser beam and the xoy plane, the angle γ formed by the projection of the reference point ref on the xoy plane and the y axis, and the distance d from the laser radar to ref are obtained in the laser radar coordinate system; Step 7: Rotate the laser radar coordinate system around the z-axis of the world coordinate system The incident angle α, horizontal rotation angle γ and distance value d in the laser radar coordinate system are used to calculate the coordinate value (x′) of the reference point ref in the laser radar coordinate system. Ri ,y′ Ri ,z′ Ri ), the relationship satisfies: Step 8: Assume that the coordinate value of the laser radar in the world coordinate system is (x L ,y L ,z L ), according to the conversion relationship between the lidar coordinate system and the world coordinate system, the reference point ref can be converted from the lidar coordinate system to the world coordinate system. The conversion relationship is expressed as: Step 9: According to the above conversion relationship, the coordinate value (x L ,y L ,z L )for: Among them, (x′ Ri ,y′ Ri ,z′ Ri ) is the coordinate of the reference point ref in the laser radar coordinate system, and its equivalent relationship is: Step 10: It can be further converted into a matrix form: Step 11: The above steps obtain the value of a reflective marker R i The laser radar position coordinates are further analyzed for the M subsets C in step 3. u Repeat steps 4 to 10 to obtain the laser radar position coordinates (x L ,y L ,z L ); Step 12: According to each subset C in step 5 u The corresponding ref, normalizes the sum of its distance values ​​d, and calculates the distance value weight of each reference point to estimate the position of the lidar (x′ L ,y′ L ,z′ L ), whose expression is: Step 13: Assemble the point cloud By performing the above steps on all frames in the image, the motion trajectory of the lidar in the SLAM process can be obtained, and the high-precision indoor self-positioning of the lidar can be completed.

Images