Laser SLAM-based map real-time construction and positioning system and positioning method

Abstract

The invention discloses a map real-time construction and positioning system and method based on laser SLAM. The system comprises a multi-source information sensing platform, a laser radar data format processing module, a sensor parameter calibration module, a map construction and optimization module, a point cloud data processing module and a repositioning module. The multi-source information sensing platform integrates an IMU, a speedometer and the like, the IMU measures the acceleration and angular velocity of equipment to provide attitude information, the speedometer calculates relative displacement, and the IMU and the speedometer are input into a data preprocessing unit together with laser radar data; the point cloud data processing module performs time synchronization and filtering processing to provide an accurate data basis for subsequent map construction; the map construction and optimization module optimizes loopback detection and finely processes topographic relief areas, all the modules work cooperatively, underground complex factors are fully considered, and the problem of error accumulation caused by signal interference and topographic relief is effectively solved.

Description

Technical Field

[0001] The invention relates to a real-time map construction and positioning system, and in particular to a real-time map construction and positioning system and a positioning method based on laser SLAM. Background Art

[0002] In underground mining environments, traditional surveying and mapping methods such as total station measurement have problems such as complex operation, low efficiency, and difficulty in updating maps in real time. At the same time, vision-based positioning methods are affected by harsh conditions such as low light and dust underground, and the positioning effect is poor. Although laser SLAM technology has brought new ideas to mine surveying and positioning, it still faces many challenges.

[0003] When the existing laser SLAM technology is applied underground in mines, it does not fully consider the complex environmental factors underground, such as signal interference and large terrain undulations, which leads to error accumulation in the map construction process and the accuracy of the constructed map is difficult to meet actual needs. Moreover, in terms of positioning, the positioning accuracy is low due to sensor errors and algorithm limitations, and it cannot provide reliable support for the precise operation of mining equipment and the safe navigation of personnel. Summary of the invention

[0004] In view of the problems existing in the above-mentioned prior art, the present invention provides a real-time map construction and positioning system and positioning method based on laser SLAM, which have the advantages of strong real-time map construction, high positioning accuracy and adaptability to complex environments. It solves the problem that when the existing laser SLAM technology is applied underground in mines, due to insufficient consideration of complex underground environmental factors such as signal interference and large terrain undulations, errors are easily accumulated during the map construction process, and the accuracy of the constructed map is difficult to meet actual needs; moreover, in terms of positioning, the positioning accuracy is low due to the influence of sensor errors and algorithm limitations, and reliable support cannot be provided for the precise operation of mining equipment and the safe navigation of personnel.

[0005] In order to achieve the above-mentioned purpose, the technical solution adopted by the present invention is: a real-time map construction and positioning system based on laser SLAM, including a multi-source information perception platform, a laser radar data format processing module, a sensor parameter calibration module, a map construction and optimization module, a point cloud data processing module and a repositioning module;

[0006] The multi-source information perception platform is connected to the laser radar data format processing module and the sensor parameter calibration module respectively. The laser radar data format processing module and the sensor parameter calibration module are both connected to the map construction and optimization module. The map construction and optimization module is connected to the point cloud data processing module. The point cloud data processing module is connected to the relocation module.

[0007] The multi-source information perception platform includes a motion control layer and a data perception layer. The motion control layer includes a main control chip microcomputer and a robot. The main control chip microcomputer and the robot are connected. The data perception layer includes a laser radar, an IMU, a global positioning system and a high-performance computing platform. The laser radar, the IMU, and the global positioning system are connected to the high-performance computing platform. The high-performance computing platform stores and processes data, and the motion control layer and the data perception layer are used to transmit the processed data to the user end through a 4G module.

[0008] The lidar data format processing module configures the lidar software and adds conversion nodes to convert the data format; the sensor parameter calibration module calibrates the internal and external parameters of the lidar and IMU; the map construction and optimization module uses multiple factors to build an optimized map based on the LIO-SAM framework; the point cloud data processing module uses specific filtering to remove noise and downsampling; the repositioning module uses the NDT algorithm to achieve repositioning.

[0009] Furthermore, the main control chip microcontroller in the motion control layer is an STM32F103RCT6 main control chip microcontroller, and the laser radar in the data perception layer is a 16-line laser radar.

[0010] A real-time positioning method for a map based on laser SLAM comprises the following steps:

[0011] H1. Data acquisition and preprocessing: The motion control layer of the multi-source information perception platform controls the robot to move underground in the mine. The laser radar, IMU and global positioning system receivers of the data perception layer synchronously collect data. The laser radar data is converted into XYZIRT format by the laser radar data format processing module and then transmitted to the high-performance computing platform. At the same time, the IMU data is preprocessed to remove outliers and wild points.

[0012] H2. Sensor parameter calibration execution: Use the sensor parameter calibration module to calibrate the external and internal parameters of the lidar and IMU. The external parameter calibration collects non-planar motion data from the multi-source information perception platform, and determines the conversion matrix between the lidar and the IMU through timestamp matching, coordinate transformation calculation and iterative optimization. The internal parameter calibration records 2 hours of IMU data, uses the imu_utils and code_utils tools to analyze the Allan variance, and determines the accelerometer and gyroscope error parameters.

[0013] H3, Map construction and optimization operation: Based on the calibrated sensor data, the map construction and optimization module uses the LIO-SAM framework to build a point cloud map. The IMU pre-integration factor calculates the relative motion between adjacent key frames, the lidar odometer factor extracts feature points to build a local map and scans and matches, the GPS factor corrects long-term navigation drift, and the loop detection factor searches for similar state matches and adds closed-loop factors to optimize the map. During the map construction process, the map accuracy is monitored in real time, and the factor weights are adjusted according to the preset thresholds.

[0014] H4. Point cloud data processing flow: The point cloud data processing module first uses statistical robust filtering to remove noise points from the constructed point cloud map, sets the number of neighboring points to 50, calculates the average distance and filters out outliers, and then uses voxel grid filtering to downsample, sets the volume of the three-dimensional voxel cube to 1*1*1m, and uses the center of gravity to approximate points, reducing the amount of data and keeping the map shape basically unchanged. The processed data is used for subsequent relocation;

[0015] H5. Repositioning implementation steps: During the movement of the robot, the repositioning module manually adds the initial posture, operates in Rviz and assigns values ​​after posture transformation and coordinate system conversion, and then uses the NDT algorithm to transform the current scanning point cloud from the lidar coordinate system to the base_link coordinate system, sets the initial posture estimation for registration, calculates the point cloud transformation parameters to achieve registration with the processed point cloud map, obtains the positioning transformation matrix and converts it to ROS type for release, thereby achieving precise positioning and real-time navigation of the robot in the map. During the repositioning process, the positioning accuracy is re-evaluated at certain intervals, and if the accuracy is lower than the threshold, the repositioning operation is re-executed.

[0016] Furthermore, when configuring the laser radar software rslidar_sdk in step H1, the laser radar data format processing module needs to configure the lidar_type in the config.yaml file to RS128, change the laser radar data point cloud format from XYZI to XYZIRT format in the CmakeLists.txt file, and configure the IP address to 192.168.1.102 and the subnet mask to 255.255.255.0 to achieve communication between the laser radar and the high-performance computing platform. At the same time, the added rs_to_velodyne conversion node directly converts the laser radar point cloud format into the velodyne radar point cloud format.

[0017] Furthermore, when the sensor parameter calibration module in step H2 performs external parameter calibration on the lidar and IMU, the multi-source information perception platform that collects data collects data containing a large amount of translation and rotation in a non-planar motion manner, matches the coordinate transformation of each point in each frame of the lidar with the IMU through timestamp matching, and uses the posture information combined with the transformation matrix to splice the lidar frames into a point cloud, calculates the total distance between the point and the nearest neighbor point, and iterates and optimizes to minimize the distance.

[0018] Furthermore, in step H2, the sensor parameter calibration module performs internal parameter calibration on the lidar and IMU using the imu_utils and code_utils tools. The IMU data is recorded for 2 hours, and only the IMU topic is recorded. The Allan variance analysis determines that the accelerometer X-axis white noise is 1.34793x10 2 , the zero bias value is 1.09231×10 4 ; The Y-axis white noise is 1.30486x10 2 , the zero bias value is 3.30663x10 4 ; Z-axis white noise is 1.88501x10 2 , the zero bias value is 2.73953x10 4 , the average white noise is 1.51260x10 2 , the zero bias value is 2.37949x10 4 ; The white noise of the gyroscope X axis is 5.19175x10 5 , the zero bias value is 5.19175×10 -5 ; The white noise on the Y axis is 8.73988×10 5 , the zero bias value is 8.73988x10 -5 ; Z-axis white noise is 5.86245x10 5 , the zero bias value is 5.86245x10 -5 , the average white noise is 6.59803×10 5 , the zero bias value is 6.59803x10 -5 .

[0019] Furthermore, in the map construction and optimization module in step H3, the IMU pre-integration factor calculates the relative velocity Δv between adjacent key frames ij , position Δp ij and rotation ΔR ij The formulas are:

[0020]

[0021] where v i 、v j are the speeds at time i and j respectively, p i 、pj are the positions at time i and j respectively, R i , R j are the rotations at time i and j respectively, g is the acceleration due to gravity, Δv ij is the time interval between time i and j.

[0022] Furthermore, in the map construction and optimization module in step H3, when the laser radar odometer factor extracts feature points to construct a local map, n sub-key frames and the nearest key frame are extracted by the sliding window method for estimation, and the associated sub-key frame set {F i-n , ..., F i}After transformation relation {T i-n , ..., T i}Convert to the world coordinate system and merge into a voxel map in is the edge feature voxel map, is the plane feature voxel map, and the converted edge features are Indicates that the plane feature is Indicates that the relative pose between the current key frame and the previous key frame is calculated by scanning matching This involves the calculation of the corresponding distances between edge features and plane features and the minimization of the target error function.

[0023] Furthermore, when the point cloud data processing module in step H4 uses statistically based robust filtering to remove noise points, the number of neighboring points of the query point is set to 50, and the average distance from each point in the point cloud to its 50 nearest neighboring points is calculated. If the distance of a point is larger than the average distance of one standard deviation, it is determined to be an outlier and filtered out; when voxel grid filtering is used for point cloud downsampling, the volume of the three-dimensional voxel cube is set to 1*1*1m, and the center of gravity is used to approximate all points in each three-dimensional cube, so as to reduce the number of point clouds and keep the map shape basically unchanged, and the processed data is used for subsequent repositioning.

[0024] Furthermore, in step H5, when the repositioning module manually adds the initial pose, after adding the initial pose of the robot in the Rviz visualization tool, the initial pose state is published through the topic, the topic is subscribed to in the constructor, and the initial pose is obtained from Rviz. If the pose is in the "map" coordinate system, the value is directly assigned; if not, the tf transformation between the pose coordinate system and the "map" coordinate system is obtained, and the pose is converted to the "map" coordinate system and then assigned. The NDT algorithm is used for alignment and positioning, and the current scanning point cloud is converted from the lidar coordinate system to the base_link coordinate system. The initial pose estimation is set, the positioning transformation matrix is ​​obtained, and it is converted to ROS type for publication.

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

[0026] 1. The multi-source information perception platform integrates IMU, odometer, etc. The IMU measures the acceleration and angular velocity of the equipment to provide attitude information, and the odometer calculates the relative displacement. The two are input into the data preprocessing unit together with the lidar data; the point cloud data processing module performs time synchronization and filtering processing to provide an accurate data basis for subsequent map construction; the improved map construction and optimization module optimizes loop detection and finely processes the terrain undulating areas. The modules work together to fully consider the complex factors underground, effectively solve the error accumulation problems caused by signal interference and terrain undulations, so that the accuracy of the constructed map meets actual needs and provides accurate map information for mine production and personnel navigation.

[0027] 2. The sensor parameter calibration module is used to calibrate the internal and external parameters of the lidar and IMU to ensure the accuracy of the sensor data; the map construction and optimization module uses a variety of factors (including IMU pre-integration factor, lidar odometer factor, GPS factor and loop detection factor) based on the LIO-SAM framework to build and optimize the point cloud map, thereby realizing high-precision real-time positioning of the robot in complex underground environments; the point cloud data processing module uses statistical robust filtering and voxel grid filtering technology to reduce noise and downsample, further improving the efficiency and accuracy of repositioning, enabling the system to achieve rapid response while maintaining high precision, overcoming the problem of inaccurate positioning caused by sensor errors and algorithm limitations in traditional methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 A block diagram of the connection between the real-time map construction and positioning system of the present invention;

[0029] Figure 2 It is a schematic diagram of the structure framework of the multi-source information perception platform of the present invention;

[0030] Figure 3 It is a schematic diagram of the relationship model of the three-dimensional coordinates in two coordinate systems of the present invention;

[0031] Figure 4 It is a schematic diagram of the overall framework of the LIO-SAM factor graph of the present invention;

[0032] Figure 5 It is a local point cloud map of the mine before and after the robust filtering of the present invention;

[0033] Figure 6 It is a local point cloud map of the mine before and after the voxel grid filtering of the present invention;

[0034] Figure 7 This is a relocation flow chart based on the mine point cloud map of the present invention. DETAILED DESCRIPTION

[0035] The present invention will be further described below in conjunction with the accompanying drawings.

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

[0037] The present invention provides a real-time map construction and positioning system based on laser SLAM, such as Figure 1 As shown, it includes a multi-source information perception platform, a lidar data format processing module, a sensor parameter calibration module, a map construction and optimization module, a point cloud data processing module and a repositioning module; the multi-source information perception platform is connected to the lidar data format processing module and the sensor parameter calibration module respectively, the lidar data format processing module and the sensor parameter calibration module are both connected to the map construction and optimization module, the map construction and optimization module is connected to the point cloud data processing module, and the point cloud data processing module is connected to the repositioning module.

[0038] The multi-source information perception platform includes a motion control layer and a data perception layer. The motion control layer includes a remote control module, a main control chip microcontroller, an encoder, a motor and a robot. The remote control module, the encoder, the motor and the robot are all connected to the main control chip microcontroller. The motion control layer uses a specific main control chip microcontroller to control the robot movement; the data perception layer includes a laser radar (LiDAR), an IMU (inertial measurement unit), a global positioning system (GPS) and a high-performance computing platform (NVIDIA AGX). The laser radar, the IMU and the global positioning system are connected to the high-performance computing platform, and the data is stored and processed by the high-performance computing platform; and the motion control layer and the data perception layer are used to transmit the processed data to the user end through the 4G module.

[0039] The lidar data format processing module configures the lidar software and adds conversion nodes to convert the data format; the sensor parameter calibration module calibrates the internal and external parameters of the lidar and IMU; the map construction and optimization module uses multiple factors to build an optimized map based on the LIO-SAM framework; the point cloud data processing module uses specific filtering to remove noise and downsampling; the repositioning module uses the NDT algorithm to achieve repositioning.

[0040] The multi-source information perception platform includes a motion control layer and a data perception layer, and the ROS operating system is used as the software development environment; the lidar point cloud format is processed and the data format is converted.

[0041] The main control chip MCU in the motion control layer is the STM32F103RCT6 main control chip MCU, which has a Cortex-M3 core, a main frequency of 72MHz, a Flash capacity of 256KB, a motor and driver board power supply voltage range of 6V-28V, and has a motor interface, IMU interface, USB interface, WIFI interface, display interface, and reserved IO interface for external sensor expansion functions; the motion control layer mainly uses the STM32F103RCT6 main control chip MCU to realize communication between serial ports, thereby realizing the movement of the robot.

[0042] The main hardware of the data perception layer includes 16-line laser radar, IMU and global positioning system receiver. The laser radar is connected to the radar algorithm box and connected to the high-performance computing platform through a network cable; the IMU is directly connected to the high-performance computing platform through a USB interface; the global positioning system receiver is connected to the high-performance computing platform through an AGX expansion board; the data perception layer mainly collects mining environment data through hardware such as laser radar, IMU, and global positioning system receivers, and uses a high-performance computing platform to store and process data; the user end communicates with the multi-source information perception platform through a 4G module; and the global positioning system is used as an accuracy verification device. Specifically:

[0043] The laser radar in the data perception layer is a 16-line laser radar, which is produced by RoboSense. It has 16 lines, a measuring distance of 20cm-150m, a vertical field of view of 30°, a horizontal field of view of 360°, a vertical resolution of 2°, a horizontal resolution of 0.4°, an accuracy of -3cm-+3cm, a wavelength of 905nm, an output point of 320,000pts / s, and a rotation speed range of 300-1200rpm. The 16-line laser radar generates 16 groups of channels through a pair of laser transmitters and laser receivers. The 16 groups of channels rotate 360° along the central axis to measure the three-dimensional information of the mining environment. The point cloud data volume of the laser radar is large and scattered.

[0044] The IMU in the data perception layer adopts the CH110 model launched by Chaohe Electronics. The maximum range of the internally integrated three-axis gyroscope is ±500° / s, the maximum range of the three-axis accelerometer is +8G, the maximum range of the three-axis magnetic sensor is 800mG, the maximum output frequency is 400HZ, and the working power supply is 5V. It has the characteristics of low cost, high performance and small size. It integrates sensors such as three-axis gyroscope, three-axis accelerometer, and three-axis magnetic field, and has a certain resistance to magnetic field interference.

[0045] The receiver of the global positioning system in the data perception layer is an RTK-GPS model, which can provide centimeter-level positioning accuracy, power consumption is 5W, power input range is 10-32VDC, operating temperature is between -40℃-+80℃, and data update rate is greater than 1HZ; the receiver of the global positioning system communicates with the outside through the serial port, is equipped with an antenna interface and a power input port, and is suitable for low-cost, low-power field positioning, timing and other applications.

[0046] The data perception layer uses the Robot Operating System (ROS); ROS is a framework for writing robot software running on the Linux system; Figure 2 As shown, the system architecture of ROS is divided into three levels:

[0047] OS layer: ROS is a secondary operating system on the Linux system;

[0048] Middle layer: The ROS middle layer mainly realizes communication through NodeletAPI and UDPROS / TCPROS;

[0049] Application layer: Master is the manager running on the ROS application layer, maintaining the normal operation of the entire system.

[0050] Embodiment 1, a real-time positioning method for a map based on laser SLAM, comprising the following steps:

[0051] H1. Data acquisition and preprocessing: The motion control layer of the multi-source information perception platform controls the robot to move underground in the mine. The laser radar, IMU and global positioning system receivers of the data perception layer synchronously collect data. The laser radar data is converted into XYZIRT format by the laser radar data format processing module and then transmitted to the high-performance computing platform. At the same time, the IMU data is preprocessed to remove outliers and wild points.

[0052] The present invention uses the LIO-SAM framework to realize the construction of the point cloud map of the mine environment. Since the present invention uses the RS-LiDAR-16 model laser radar, when the data is used to construct the point cloud map through the LIO-SAM framework, it will check whether the laser radar has a timestamp and ring. Therefore, it is necessary to add the ring and timestamp to change the laser radar data format to:

[0053] XYZIRT (x, y, z, intensity, ring, timestamp) format. When configuring the lidar software rslidar_sdk, the lidar data format processing module needs to configure lidar_type in the config.yaml file to RS128, change the lidar data point cloud format from XYZI to XYZIRT format in the CmakeLists.txt file, and configure the IP address to 192.168.1.102 and the subnet mask to 255.255.255.0 to achieve communication between the lidar and the high-performance computing platform. At the same time, the added rs_to_velodyne conversion node converts the lidar point cloud format directly into the velodyne radar point cloud format.

[0054] H2. Sensor parameter calibration execution: The sensor parameter calibration module is used to calibrate the external and internal parameters of the lidar and IMU. The external parameter calibration collects non-planar motion data from the multi-source information perception platform, and determines the conversion matrix between the lidar and the IMU through timestamp matching, coordinate transformation calculation and iterative optimization. The internal parameter calibration records 2 hours of IMU data, uses the imu_utils and code_utils tools to analyze the Allan variance, and determines the accelerometer and gyroscope error parameters.

[0055] The sensor parameter calibration module performs external parameter calibration on the lidar and IMU:

[0056] Assume that in space, the laser radar and IMU measure a set of landmarks respectively. Their three-dimensional coordinates are represented differently in the laser radar and IMU coordinate systems. By calculating the coordinate transformation matrix between the two sensors, the two sensors can uniformly represent the motion state. It is necessary to jointly calibrate the sensors to obtain the relationship model of the three-dimensional coordinates in the two coordinate systems. Figure 3 shown.

[0057] like Figure 3 As shown, the coordinate system (O1, X1, Y1, Z1) is recorded as P, and the coordinate system (O2, X2, Y2, Z2) is recorded as Q. Suppose there is a point in the three-dimensional space, the coordinates in the Q coordinate system are (x2, y2, z2), and the coordinates in the P coordinate system are (x1, y1, z1). Let N be the transformation matrix between the two coordinates, which includes the translation matrix T and the rotation matrix R, as shown in the following formula: (x1, y1, z1) T =N T (x2, y2, z2) T ;

[0058] The external parameter calibration of LiDAR and IMU is the process of finding the transformation matrix. The three-dimensional coordinate transformation matrix is ​​shown in the following formula, where R is the geometric transformation, T is the translation transformation, 0 is the projection-free transformation, and 1 is the proportional transformation:

[0059] The laser radar and IMU external parameter calibration process is as follows:

[0060] Step 1: Collect lidar data and IMU data at the same time. The multi-source information perception platform collects data in non-planar motion, and the data packet contains a large amount of translation and rotation;

[0061] Step 2: Read the data of LiDAR and IMU from the collected data packets;

[0062] Step 3: Match the coordinate transformation of each point in each frame of the lidar and the IMU through the timestamp;

[0063] Step 4: Use the transformation matrix obtained by combining the pose information to stitch each frame of the LiDAR into a point cloud;

[0064] Step 5: Calculate the sum of the distances between each point and its nearest neighbor. In optimization, the process is to iterate continuously to find the coordinate transformation that minimizes this distance.

[0065] From the above process, it can be seen that the laser radar and IMU external parameter calibration is to convert the laser radar data to the IMU coordinate system according to the transformation matrix between the two coordinate systems, generate a point cloud, and use the kdtree method to find the data points of the nearest neighbor points of the current scanned data and the next scanned data and calculate the distance value until the total distance is minimized. It can be considered to be a complete match and the calculated external parameter result is correct. That is, the optimization idea is to convert the data collected in the laser radar coordinate system to the IMU coordinate system according to the transformation matrix, so that the data points of the two scans before and after overlap as much as possible to complete the sensor calibration; the rotation matrix information is:

[0066] [-0.975156,-0.221516,-0.000975888,0.221498,-0.975118,0.00911853,-0.002971510.00867584,0.999958];

[0067] The translation matrix information is: [0.00290376, -0.00715269, 0.0395843].

[0068] The sensor parameter calibration module performs internal parameter calibration on the IMU:

[0069] The CH110 IMU model used in the present invention is one of the important sensors for acquiring mining data; the IMU internal parameter is a key factor affecting the autonomous positioning of the multi-source information perception platform in a mining environment and the construction of a high-precision point cloud map.

[0070] The actual acceleration value of the IMU inertial navigation is shown in formula 1.1.

[0071]

[0072] The actual angular velocity value is shown in formula 1.2,

[0073] w m =w+b g +n g , Formula 1.2;

[0074] Where g is the acceleration due to gravity, b is a , b g is a zero-biased random walk, and n g , n a is Gaussian white noise; after the IMU is started, the rotation matrix R between the coordinate systems needs to be determined, and two deviations need to be estimated in real time. By modeling the IMU error, the IMU data is corrected through the built model to improve the data accuracy of the IMU, and the angular velocity of the gyroscope is integrated to obtain the angle of the IMU output, and the velocity of the accelerometer is integrated to obtain the velocity of the IMU output; the essence of IMU calibration is to calibrate the parameters of the installation error, scale coefficient error, and zero bias of the gyroscope and accelerometer.

[0075] When the sensor parameter calibration module performs internal parameter calibration on the lidar and IMU, it uses the imu_utils and code_utils tools, and adopts the Allan variance analysis idea to analyze Gaussian white noise and zero-bias instability noise. The Allan variance can quantitatively separate various error coefficients and identify various types of inertial device errors. The solution of the Allan variance is shown in formula 1.3.

[0076]

[0077] Where N is the total number of data points collected, t is the sampling time of IMU, n is the product of the number of data points collected and the sampling time, and n is the number of data points in each copy. is the amount of data at time i;

[0078] code_utils needs to install Ceres nonlinear optimization library globally. For an optimization problem in internal parameter calibration, the problem is first modeled, and then a suitable optimization method is used to solve it. Finally, derivative calculation and gradient descent are used to find the optimal solution. During the compilation process, since imu_utils depends on code_utils, first install code_utils in the created workspace src directory for compilation, and then install imu_utils for compilation. When recording IMU data, the IMU cannot shake throughout the whole process. To record 2 hours of IMU data, only the topic of the IMU needs to be recorded. The white noise of the accelerometer X-axis is determined to be 1.34793x10 through Allan variance analysis. 2 , the zero bias value is 1.09231×10 4 ; The Y-axis white noise is 1.30486x10 2 , the zero bias value is 3.30663x10 4 ; The white noise on the Z axis is 1.88501×10 2 , the zero bias value is 2.73953x10 4 , the average white noise is 1.51260x10 2 , the zero bias value is 2.37949x10 4 ; The white noise of the gyroscope X axis is 5.19175x10 5 , the zero bias value is 5.19175×10 -5 ; The white noise on the Y axis is 8.73988×10 5 , the zero bias value is 8.73988x10 -5 ; Z-axis white noise is 5.86245x10 5 , the zero bias value is 5.86245x10 -5 , the average white noise is 6.59803×10 5 , the zero bias value is 6.59803x10 -5 .

[0079] H3. Map building and optimization operations: Based on the calibrated sensor data, the map building and optimization module uses the LIO-SAM framework to build a point cloud map. LIO-SAM is a tightly coupled lidar inertial odometer framework based on factor graph optimization, which includes lidar odometer factors, IMU pre-integration factors, GPS factors, and loop detection factors. The overall framework of the LIO-SAM factor graph is as follows: Figure 4As shown; the IMU pre-integration factor calculates the relative motion between adjacent key frames, the lidar odometer factor extracts feature points to build a local map and scans and matches, the GPS factor corrects long-term navigation drift, the loop detection factor searches for similar state matches and adds a closed-loop factor to optimize the map. During the map construction process, the map accuracy is monitored in real time and the factor weights are adjusted according to the preset threshold.

[0080] IMU pre-integration factor:

[0081] The IMU pre-integration method is used to calculate the relative velocity Δv between it and jt ij , position Δp ij and rotation ΔR ij , the formulas are shown in Formula 1.4, Formula 1.5 and Formula 1.6 respectively,

[0082]

[0083] Where v i 、v j are the speeds at time i and j respectively, p i 、p j are the positions at time i and j respectively, R i , R j are the rotations at time i and j respectively, g is the acceleration due to gravity, Δv ij is the time interval between time i and j.

[0084] LiDAR odometry factor:

[0085] The feature extraction of LiDAR odometer is to extract edge and plane features through the curvature of local area points; assuming and They are the plane features and edge features extracted from the laser radar scanning point cloud at time t; F i It is a lidar point cloud frame composed of all features extracted at time t The steps to construct the lidar odometry factor are as follows:

[0086] Step 1: Create a voxel map of the sub-keyframe;

[0087] The LIO-SAM framework uses a sliding window method to construct a point cloud map by extracting n sub-keyframes and the nearest keyframe for estimation; the associated sub-keyframe set {F i-n , ..., F i}Through the transformation relation {T i-n , ..., T i} is converted to the world coordinate system, and the converted sub-keyframes are merged into a voxel map M i As shown in formula 1.7,

[0088]

[0089] In the formula is the edge feature voxel map, as shown in formula 1.8,

[0090]

[0091] In the formula is a plane feature voxel map, as shown in formula 1.9,

[0092]

[0093] The edge features are transformed to the world coordinate system using Indicates that the plane feature is express.

[0094] Step 2: Scan and match;

[0095] Lidar keyframe Convert from the robot coordinate system to the world coordinate system, is the set of converted key frames; the robot motion state predicted by IMU Get the initial transformation relationship; for each frame and The edge features and plane features in the voxel map and Find the corresponding edge in .

[0096] Step 3: relative transformation;

[0097] Assume that the corresponding distance between edge features and edges is as shown in formula 1.10,

[0098]

[0099] The corresponding distance between the plane feature and the plane is shown in formula 1.11,

[0100]

[0101] Where kuvw is the feature index number corresponding to the plane feature or edge feature, and is a voxel map The corresponding point on the edge line is Is an edge keyframe Mid-edge features, and is a voxel map The corresponding plane in Is an edge keyframe The plane features in ; then through dek and d pk Minimize the target error function to calculate the relative pose ΔT between the current key frame and the previous key frame i，i+1 , as shown in formula 1.12,

[0102]

[0103] Where T i and T i+1 Represent the poses of the frames at time i and time i+1 respectively.

[0104] GPS Factor:

[0105] The LIO-SAM framework adopts a closed loop detection method based on Euclidean distance. When a new state is added to the factor graph, it first searches in the factor graph to find a state close to the state in the Euclidean space. The sub-keyframe {F 3-m ,…,F3,…,F 3+m} and keyframe F i+1 To match, in the world coordinate system, the sub-keyframes and keyframes need to be transformed and then added to the factor graph as a closed-loop factor.

[0106] H4. Point cloud data processing flow: The point cloud data processing module first uses statistically based robust filtering to remove noise points from the constructed point cloud map, sets the number of neighboring points to 50, calculates the average distance and filters out outliers, and then uses voxel grid filtering to downsample, sets the volume of the three-dimensional voxel cube to 1*1*1m, and uses the center of gravity approximation to replace the point, reducing the amount of data and keeping the map shape basically unchanged. The processed data is used for subsequent repositioning.

[0107] The point cloud map constructed by the LIO-SAM framework inevitably has noise points. Removing the noise points can improve the accuracy of repositioning. At the same time, point cloud downsampling reduces the number of point clouds while keeping the point cloud shape basically unchanged, which reduces the time and complexity of the algorithm's real-time registration and improves the efficiency of registration. Based on the prior map, the correct initial posture of the multi-source information perception platform is determined, the NDT algorithm is executed, the scan frame is matched with the map, and the repositioning is completed.

[0108] Point cloud data processing:

[0109] There are some "noise points" in the point cloud map constructed by the LIO-SAM framework in the mining environment; as the scanning radius of the 16-line lidar laser beam in the mining environment continues to expand, the data points are sparsely distributed at a distance from the scanning platform, and the density of the scanned data points will be greatly reduced. These sparsely distributed points will affect the accuracy of repositioning, and the noise points need to be filtered out before repositioning; since the prior map is a global map of the mining environment and the number of point clouds is large, voxel grid filtering is used to reduce the number of point clouds. While maintaining the original point cloud morphology, the execution efficiency of the repositioning algorithm is improved. Therefore, this embodiment uses statistically based robust filtering to remove some sparse discrete points far away from the object being measured, that is, outliers, and uses voxel grid filtering to downsample the point cloud.

[0110] (1) Statistical robust filtering:

[0111] The measurement noise is removed by using statistical robust filtering; the method for realizing the local point cloud map of the mine before and after the robust filtering includes the following steps:

[0112] Step 1: Traverse the point cloud, assuming that the average distance satisfies the Gaussian distribution, and calculate the average distance from each point in the point cloud to the k nearest neighboring points of the point;

[0113] Step 2: Define the distance threshold based on the variance and the global distance average;

[0114] Step 3: Traverse the point cloud again. When the distance between the k nearest neighbor points of a point and the point is greater than the distance threshold, the point is defined as an outlier and filtered out.

[0115] The local point cloud map of the mine before and after the statistical robust filtering, such as Figure 5 As shown; the blue (left side) is the original point cloud, and the green (right side) is the point cloud after statistical robust filtering; the present invention adopts C++ language to write the core code of the statistical robust filter, and uses the template class in the PCL point cloud library to create the filter object; the present invention sets the number of neighboring points of the query point during statistics to 50. If the distance of a point is larger than the set average distance of a standard deviation, the point is an outlier that does not meet the calibration, and filtering processing is performed.

[0116] (2) Voxel grid filtering:

[0117] The computational complexity of the NDT registration algorithm is related to the deviation of the initial pose estimation and the density of the input point cloud. The more the initial pose estimation deviates from the true pose, the greater the corresponding computational complexity. The greater the density of the input point cloud, the greater the computational complexity of the registration algorithm. The 16-line RoboSense laser radar used in the present invention returns about 320,000 points per second, and the number of stored point clouds is dense and huge. The construction method of the prior map is incremental, and voxel grid filtering is required to downsample the prior map. Under the premise of keeping the spatial structure information basically unchanged, the number of point clouds is reduced, the time complexity of the algorithm's real-time registration is reduced, and the efficiency of registration is improved.

[0118] Voxel grid filtering divides the point cloud data set into a collection of miniature three-dimensional spatial cubes, that is, creates a three-dimensional voxel grid in the three-dimensional space where the input point cloud is located, and replaces all the points of each three-dimensional cube with the centroid approximation. After using voxel grid filtering, the coordinates of the centroid are shown in formula 1.13.

[0119]

[0120] Where N is all the points in the three-dimensional voxel. In this embodiment, the core code of the voxel grid filter is written in C++ language. The filter is created using the template class in the PCL point cloud library. In order to ensure the real-time performance and accuracy of the matching, the volume of the three-dimensional voxel cube is set to 1*1*1m, and finally the filtering process is performed. The point cloud after voxel grid filtering will be used for NDT registration. The local point cloud map of the mine before and after voxel grid filtering is shown in Figure 6 As shown, the white point cloud (left) is the original point cloud, and the green point cloud (right) is the point cloud left after voxel grid filtering.

[0121] (3)NDT algorithm:

[0122] The normal distribution transform (NDT) algorithm is a technology that relies on real-time lidar measurement data and existing high-precision maps to achieve positioning. It can meet the needs of accurate positioning of multi-source information perception platforms. The positioning problem of autonomous mobile robots in coal mines is to find the corresponding position of the robot on the map. Since the mine paths are long, the longer the robot walks, the greater the deviation of the real-time constructed map. Therefore, it will be easier to locate the robot after having a high-precision map.

[0123] Steps of NDT point cloud registration algorithm:

[0124] Step 1: Establish an NDT model to describe the spatial surface;

[0125] By gridding the matched point cloud space, the space is divided into multiple three-dimensional cells; assuming that the matched point cloud distribution is normally distributed, the discrete point cloud in each cell is represented by a piecewise continuously differentiable probability density function, thereby calculating the probability density function of the normal distribution; the mean vector of the points in each cell is And the covariance matrix ∑ is shown in Formula 1.14 and Formula 1.15,

[0126]

[0127] Where m is the number of point clouds, is the position of the point cloud in each cell; the probability density function of a cell is shown in formula 1.16,

[0128]

[0129] The random variable X satisfies the normal distribution, that is, X~N(μ,σ), and its probability distribution function is shown in formula 1.17.

[0130]

[0131] Where μ is the mean of the normal distribution and 2σ is the variance. By establishing an NDT model to describe the spatial surface, the point cloud is mapped to a smooth surface approximately represented by a series of probability density functions.

[0132] Step 2: construct and solve the objective function;

[0133] NDT registration is to maximize the possibility of the currently scanned point on the 3D map surface; the currently scanned point cloud is rotated, translated and other transformation optimization parameters are performed, p represents the change parameter, X = {x1, x2..., x n} is the point cloud set of the current frame, T(p, x k ) is the spatial transformation function. The posture transformation that maximizes the likelihood function corresponds to the optimal transformation parameter p. Minimizing the negative log-likelihood is equivalent to maximizing the likelihood. The maximum likelihood function is shown in Equation 1.18.

[0134]

[0135] The maximum likelihood estimate of formula 1.18 can be expressed by minimizing the negative log-likelihood -logθ. The parameters are optimized by Newton's method to find the transformation parameter p corresponding to the minimum value of formula 1.19.

[0136]

[0137] H5. Repositioning implementation steps: During the movement of the robot, the repositioning module manually adds the initial posture, operates in Rviz and assigns values ​​after posture transformation and coordinate system conversion, and then uses the NDT algorithm to transform the current scanning point cloud from the lidar coordinate system to the base_link coordinate system, sets the initial posture estimation for registration, calculates the point cloud transformation parameters to achieve registration with the processed point cloud map, obtains the positioning transformation matrix and converts it to ROS type for release, thereby achieving precise positioning and real-time navigation of the robot in the map. During the repositioning process, the positioning accuracy is re-evaluated at certain intervals, and if the accuracy is lower than the threshold, the repositioning operation is re-executed.

[0138] Relocation based on mine point cloud map:

[0139] This embodiment will simplify and efficiently implement NDT registration and positioning on the ROS platform. The repositioning flow chart based on the mine point cloud map is as follows: Figure 7 shown.

[0140] (1) Loading the processed point cloud map. The relocation of the coal mine robot depends on the high-precision point cloud map built offline. The LIO-SAM framework is used to collect mining environment data to form a high-precision point cloud map. The generated global mine map is processed by denoising and downsampling. First, the map needs to be loaded through the map loading node, the point cloud map is loaded into the corresponding directory, the path and topic of the point cloud map are added, and then the point cloud topic is published through the publishing node. In the constructor, the topic and path information in the map are read, and the variable parameters in the point cloud map are initialized. By loading the point cloud map and rotating and translating the map, the transformation of the point cloud is realized.

[0141] (2) Obtaining the initial pose and map initialization The initial pose is the initial position and posture of the multi-source information perception platform. If the initial pose is not accurately determined, the registration and positioning cannot be achieved. In view of the high altitude and poor signal of the mine, this embodiment adopts a manual method to add the initial pose, so that there is no need to use high-precision sensors to obtain the initial pose of the robot, which has the advantages of simplicity, convenience and efficiency. After adding the initial pose of the robot in the Rviz visualization tool, the state of the initial pose is published through a topic. In the constructor, the topic of the initial pose state is subscribed to, and the initialized pose is transformed into the map coordinate system. First, the initial pose is obtained from Rviz, and then the initial pose information is passed to determine whether the input pose is in the "map" coordinate system. If the pose is in the "map" coordinate system, the pose is assigned. If the input pose is not in the "map" coordinate system, the tf transformation between the pose coordinate system and the "map" coordinate system is obtained, and it is converted to the "map" coordinate system, and then the pose is assigned.

[0142] For map initialization, the point cloud map generated by the LIO-SAM framework is used as the target point cloud for NDT algorithm registration. First, the node obtains the topic of the input point cloud, then searches for the minimum change amount and search step size, uses Newton's method to optimize the number of iterations, and finally sets the point cloud as the target point cloud of the NDT algorithm.

[0143] (3) NDT registration and positioning inputs the denoised and downsampled lidar point cloud information, receives the frame_id and timestamp of the lidar point cloud, and converts the point cloud from the lidar coordinate system to the base_link coordinate system; for the lidar used in the present invention, the z-axis distance from the lidar to the base_link is set to 0.1, the lidar point cloud in the base coordinate system is used as the input of the NDT algorithm, the initial pose estimation of the registration is set, the NDT algorithm is used for registration, the positioning transformation matrix is ​​obtained, and the final result of the NDT registration is converted to the ROS type and published.

[0144] In summary, the multi-source information perception platform of the present invention integrates IMU, odometer, etc. The IMU measures the acceleration and angular velocity of the equipment to provide attitude information, and the odometer calculates the relative displacement. The two are input into the data preprocessing unit together with the lidar data; the point cloud data processing module performs time synchronization and filtering processing to provide an accurate data basis for subsequent map construction; the map construction and optimization module optimizes loop detection and finely processes the terrain undulating areas. The modules work together to fully consider the complex factors underground, effectively solve the error accumulation problem caused by signal interference and terrain undulation, so that the accuracy of the constructed map meets the actual needs, and provide accurate map information for mine production and personnel navigation.

[0145] The present invention uses a sensor parameter calibration module to calibrate the internal and external parameters of the laser radar and IMU, thereby ensuring the accuracy of the sensor data; the map construction and optimization module uses a variety of factors (including IMU pre-integration factor, laser radar odometer factor, GPS factor and loop detection factor) based on the LIO-SAM framework to construct and optimize the point cloud map, thereby realizing high-precision real-time positioning of the robot in a complex underground environment; the point cloud data processing module uses statistical robust filtering and voxel grid filtering technology to reduce noise and downsample, further improving the efficiency and accuracy of repositioning, so that the system can achieve rapid response while maintaining high precision, overcoming the problem of inaccurate positioning caused by sensor errors and algorithm limitations in traditional methods, and solving the problem that when the existing laser SLAM technology is applied in underground mines, due to the failure to fully consider the complex underground environmental factors, such as signal interference, large terrain undulations, etc., it is easy to accumulate errors in the map construction process, and the accuracy of the constructed map is difficult to meet actual needs.

[0146] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0147] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any slight modification, equivalent substitution and improvement made to the above embodiment based on the technical essence of the present invention should be included in the protection scope of the technical solution of the present invention.

Claims

1. A real-time map construction and positioning system based on laser SLAM, characterized in that: It includes a multi-source information perception platform, a lidar data format processing module, a sensor parameter calibration module, a map building and optimization module, a point cloud data processing module, and a relocation module; The multi-source information perception platform is connected to the laser radar data format processing module and the sensor parameter calibration module respectively. The laser radar data format processing module and the sensor parameter calibration module are both connected to the map construction and optimization module. The map construction and optimization module is connected to the point cloud data processing module. The point cloud data processing module is connected to the relocation module. The multi-source information perception platform includes a motion control layer and a data perception layer. The motion control layer includes a main control chip microcomputer and a robot. The main control chip microcomputer and the robot are connected. The data perception layer includes a laser radar, an IMU, a global positioning system, and a high-performance computing platform. The laser radar, the IMU, and the global positioning system are connected to the high-performance computing platform. The high-performance computing platform stores and processes data and transmits the data to the user end through a 4G module. The lidar data format processing module configures the lidar software and adds conversion nodes to convert the data format; the sensor parameter calibration module calibrates the internal and external parameters of the lidar and IMU; the map construction and optimization module uses multiple factors to build an optimized map based on the LIO-SAM framework; the point cloud data processing module uses specific filtering to remove noise and downsampling; the repositioning module uses the NDT algorithm to achieve repositioning.

2. According to claim 1, a real-time map construction and positioning system based on laser SLAM is characterized in that: The main control chip single chip microcomputer in the motion control layer is an STM32F103RCT6 main control chip single chip microcomputer, and the laser radar in the data perception layer is a 16-line laser radar.

3. A real-time positioning method for maps based on laser SLAM, characterized in that: The following steps are involved: H1. Data acquisition and preprocessing: The motion control layer of the multi-source information perception platform controls the robot to move underground in the mine. The laser radar, IMU and global positioning system receivers of the data perception layer synchronously collect data. The laser radar data is converted into XYZIRT format by the laser radar data format processing module and then transmitted to the high-performance computing platform. At the same time, the IMU data is preprocessed to remove outliers and wild points. H2. Sensor parameter calibration execution: Use the sensor parameter calibration module to calibrate the external and internal parameters of the lidar and IMU. The external parameter calibration collects non-planar motion data from the multi-source information perception platform, and determines the conversion matrix between the lidar and the IMU through timestamp matching, coordinate transformation calculation and iterative optimization. The internal parameter calibration records 2 hours of IMU data, uses the imu_utils and code_utils tools to analyze the Allan variance, and determines the accelerometer and gyroscope error parameters. H3, Map construction and optimization operation: Based on the calibrated sensor data, the map construction and optimization module uses the LIO-SAM framework to build a point cloud map. The IMU pre-integration factor calculates the relative motion between adjacent key frames, the lidar odometer factor extracts feature points to build a local map and scans and matches, the GPS factor corrects long-term navigation drift, and the loop detection factor searches for similar state matches and adds closed-loop factors to optimize the map. During the map construction process, the map accuracy is monitored in real time, and the factor weights are adjusted according to the preset thresholds. H4. Point cloud data processing flow: The point cloud data processing module first uses statistical robust filtering to remove noise points from the constructed point cloud map, sets the number of neighboring points to 50 to calculate the average distance and filter out outliers, then uses voxel grid filtering to downsample, sets the volume of the three-dimensional voxel cube to 1*1*1m, and uses the center of gravity to approximate points, reducing the amount of data while keeping the map shape unchanged. The processed data is used for subsequent relocation; H5. Repositioning implementation steps: During the movement of the robot, the repositioning module manually adds the initial posture, operates in Rviz and assigns values ​​after posture transformation and coordinate system conversion, and then uses the NDT algorithm to transform the current scanning point cloud from the lidar coordinate system to the base_link coordinate system, sets the initial posture estimation for registration, calculates the point cloud transformation parameters to achieve registration with the processed point cloud map, obtains the positioning transformation matrix and converts it to ROS type for release, thereby achieving precise positioning and real-time navigation of the robot in the map. During the repositioning process, the positioning accuracy is re-evaluated at certain intervals, and if the accuracy is lower than the threshold, the repositioning operation is re-executed.

4. A real-time map construction and positioning system based on laser SLAM according to claim 3, characterized in that: In the step H1, when configuring the laser radar software rslidar_sdk, the laser radar data format processing module configures the lidar_type in the config.yaml file to RS128, changes the laser radar data point cloud format from XYZI to XYZIRT format in the CmakeLists.txt file, and configures the IP address to 192.168.1.102 and the subnet mask to 255.255.255.0 to achieve communication between the laser radar and the high-performance computing platform. At the same time, the added rs_to_velodyne conversion node directly converts the laser radar point cloud format into the velodyne radar point cloud format.

5. The real-time map construction and positioning system based on laser SLAM according to claim 3 is characterized in that: When the sensor parameter calibration module in step H2 performs external parameter calibration on the lidar and IMU, the multi-source information perception platform that collects data collects translation and rotation data in a non-planar motion manner, matches the coordinate transformation of each point in each frame of the lidar with the IMU through timestamps, and uses the posture information combined with the transformation matrix to splice the lidar frames into a point cloud, calculates the total distance between the point and the nearest neighbor point, and iterates and optimizes to minimize the distance.

6. The real-time map construction and positioning system based on laser SLAM according to claim 3 is characterized in that: In step H2, the sensor parameter calibration module performs internal parameter calibration on the lidar and IMU using the imu_utils and code_utils tools. The IMU data is recorded for 2 hours and the accelerometer X-axis white noise is determined to be 1.34793x10 2 , the zero bias value is 1.09231×10 4 ; The Y-axis white noise is 1.30486x10 2 , the zero bias value is 3.30663x10 4 ; The white noise on the Z axis is 1.88501×10 2 , the zero bias value is 2.73953x10 4 , the average white noise is 1.51260x10 2 , the zero bias value is 2.37949x10 4 ; The white noise of the gyroscope X axis is 5.19175x10 5 , the zero bias value is 5.19175×10 -5 ; The white noise on the Y axis is 8.73988×10 5 , the zero bias value is 8.73988x10 -5 ; The white noise on the Z axis is 5.86245x10 5 , the zero bias value is 5.86245x10 -5 , the average white noise is 6.59803×10 5 , the zero bias value is 6.59803x10 -5 .

7. The real-time map construction and positioning system based on laser SLAM according to claim 3 is characterized in that: The map building and optimization module in step H3, the IMU pre-integration factor calculates the relative speed Δv between adjacent key frames ij , position Δp ij and rotation ΔR ij The formulas are: Where V i 、v j are the speeds at time i and j respectively, p i 、p j are the positions at time i and j respectively, R i , R j are the rotations at time i and j respectively, g is the acceleration due to gravity, Δv ij is the time interval between time i and j.

8. The real-time map construction and positioning system based on laser SLAM according to claim 3 is characterized in that: In the map construction and optimization module in step H3, when the laser radar odometer factor extracts feature points to construct a local map, the sliding window method is used to extract n sub-key frames and the nearest key frame for estimation, and the associated sub-key frame set {F i-n , ..., F i }After transformation relation {T i-n , ..., T i }Convert to the world coordinate system and merge into a voxel map in is the edge feature voxel map, is the plane feature voxel map, and the converted edge features are Indicates that the plane feature is Indicates that the relative pose between the current key frame and the previous key frame is calculated by scanning matching This involves the calculation of the corresponding distances between edge features and plane features and the minimization of the target error function.

9. The real-time map construction and positioning system based on laser SLAM according to claim 3, characterized in that: When the point cloud data processing module in step H4 uses statistically based robust filtering to remove noise points, the number of neighboring points of the query point is set to 50, and the average distance from each point in the point cloud to its 50 nearest neighboring points is calculated. If the distance of a point is larger than the average distance of one standard deviation, it is determined to be an outlier and filtered out; when voxel grid filtering is used for point cloud downsampling, the volume of the three-dimensional voxel cube is set to 1*1*1m, and the center of gravity is used to approximate all points in each three-dimensional cube, so as to reduce the number of point clouds and keep the map shape unchanged, and the processed data is used for subsequent repositioning.

10. The real-time map construction and positioning system based on laser SLAM according to claim 3, characterized in that: In the step H5, when the repositioning module manually adds the initial pose, after adding the initial pose of the robot in the Rviz visualization tool, the initial pose state is published through the topic, the topic is subscribed in the constructor, and the initial pose is obtained from Rviz. If the pose is in the "map" coordinate system, the value is directly assigned. If not, the tf transformation between the pose coordinate system and the "map" coordinate system is obtained, and the pose is converted to the "map" coordinate system and then assigned. The NDT algorithm is used for alignment and positioning, and the current scanning point cloud is converted from the lidar coordinate system to the base_link coordinate system. The initial pose estimation is set, and the positioning transformation matrix is ​​obtained and converted to ROS type for publication.

Images