Mobile robot positioning method based on multi-sensor fusion

Abstract

The application discloses a mobile robot positioning method based on multi-sensor fusion, which comprises the following steps: loading a high-precision point cloud map, pre-processing multi-sensor data, initializing the initial position and posture of the mobile robot according to the pre-processed multi-sensor data, moving the local point cloud sub-map obtained by the laser radar to the corresponding position of the global high-precision point cloud map according to the initialized position and posture of the mobile robot, accurately positioning by the laser radar, matching the current real-time obtained laser point cloud data with the sub-map in the global high-precision point cloud map, converting the current coordinates into global positioning coordinates, manufacturing the current laser point cloud data into a scanning context descriptor according to the global positioning coordinates, storing the scanning context descriptor and the corresponding global coordinates into a descriptor database, obtaining the matching score of the current mobile robot coordinates, and updating if the matching score is high, or repositioning the position and posture of the mobile robot. The application can realize high-precision real-time positioning of the mobile robot.

Description

Technical Field

[0001] This invention relates to the field of mobile robot positioning technology, and in particular to a mobile robot positioning method based on multi-sensor fusion. Background Technology

[0002] A key challenge for mobile robots performing autonomous movement in complex environments is achieving high-precision real-time positioning. Given existing prior maps, there are currently three main methods: first, referencing automotive positioning methods, using Global Navigation Satellite Systems (GNSS); second, using mobile robot odometry and inertial measurement units (IMUs) to estimate the robot's position and attitude relative to its initial point; and third, using a single sensor, such as lidar, for positioning based on prior maps.

[0003] All three approaches have shortcomings. The first approach, using GNSS positioning, suffers from limitations. Mobile robots operate in different environments than cars, typically indoors or outdoors with many obstructions. In these scenarios, the GNSS system may lose positioning accuracy or fail to obtain positioning information due to insufficient satellite reception, resulting in inaccurate positioning. Furthermore, GNSS positioning accuracy is generally low and cannot meet positioning requirements. The second approach, using wheeled odometry and IMU for pose estimation, only achieves positioning on flat ground and within a small area. On large areas and uneven ground, the positioning error quickly accumulates due to error buildup. The third approach, using LiDAR for mobile robot positioning, has become increasingly common in recent years. Commonly used methods include Iterative Closest Point (ICP) and Normal Distribution Transform (NDT) point cloud matching. However, the ICP algorithm is highly sensitive to initial values ​​and prone to local extrema during matching. The NDT algorithm is less sensitive to initial values ​​but has lower positioning accuracy. Meanwhile, the third approach requires specifying the current location of the mobile robot before matching and positioning can be performed. Furthermore, if the matching fails, the accumulated error will also cause the matching to fail, and it cannot solve the problem of mobile robot hijacking.

[0004] CN114413894A discloses a mobile robot localization method based on multi-sensor fusion. It uses dual GPS to calculate the current coordinates and orientation as the initial position and attitude for LiDAR point cloud matching. During robot operation, the LiDAR point cloud performs precise localization, while the robot's wheel odometry and IMU are used for motion estimation. If the current LiDAR point cloud localization result is unsatisfactory, the wheel odometry and IMU localization result is used. However, in situations with object occlusion or indoor environments, the mobile robot cannot receive GPS signals. When the LiDAR point cloud localization result fails, the wheel odometry and IMU gradually deviate from the correct localization result due to tire slippage caused by uneven ground, failing to achieve optimal results. Furthermore, due to accumulated errors during the localization process, the localization error increases significantly.

[0005] Analysis reveals that current positioning methods largely rely on single sensors, leading to positioning failures when a sensor malfunctions. Existing multi-sensor fusion positioning methods depend solely on point cloud matching results from LiDAR, typically using NDT or ICP point cloud matching algorithms. If point cloud matching fails, the accumulated errors cause the mobile robot's positioning to gradually deviate from its actual location until complete positioning failure occurs. Summary of the Invention

[0006] The purpose of this invention is to solve the problem of high-precision real-time positioning of mobile robots in urban or indoor environments. It provides a high-precision mobile robot positioning method based on multi-sensor fusion that can improve the positioning of mobile robots on existing maps. This method can effectively achieve high-precision real-time positioning of mobile robots in urban or indoor environments, and can update the accurate position in a timely manner when the mobile robot is initialized in an unfamiliar environment or when a mobile robot kidnapping problem occurs.

[0007] The technical solution adopted to achieve the purpose of this invention is:

[0008] A mobile robot localization method based on multi-sensor fusion includes:

[0009] S1: Load the high-precision point cloud map, preprocess the data of lidar, GNSS and IMU, and obtain the preprocessed lidar point cloud data, IMU data and GNSS data;

[0010] S2: Based on the lidar point cloud data, GNS, and IMU data, the initial position and attitude of the mobile robot are obtained through localization initialization. The local point cloud sub-map acquired by the lidar is moved to the corresponding position on the global high-precision point cloud map according to the initial position and attitude of the mobile robot.

[0011] S3: Use LiDAR for precise positioning, match the currently obtained real-time LiDAR point cloud data with the sub-map in the global high-precision point cloud map, and convert the current coordinates into global positioning coordinates based on the relative pose rotation matrix R and relative translation matrix t of the source point cloud and the target point cloud obtained by the improved GICP LiDAR point cloud matching method.

[0012] S4: Based on the global positioning coordinates, the current laser point cloud data is made into a scanning context descriptor, and the scanning context descriptor and the corresponding global coordinates are stored in the descriptor database.

[0013] S5: Obtain the current mobile robot coordinate matching score. Based on the matching score, determine whether the current mobile robot position and attitude data need to be updated. If the matching score is higher than or equal to the threshold, then update; otherwise, determine if a localization failure or mobile robot hijacking problem has occurred, and repeat S2 to S5 to perform mobile robot position and attitude relocalization until the localization task ends.

[0014] In step S1, loading the high-precision point cloud map includes loading an existing high-precision laser point cloud map within the positioning space and a map descriptor database generated when constructing the high-precision point cloud map. The map descriptor database stores real-time updated scan context descriptor data. The scan context descriptor data is generated at the position of each keyframe using a multi-line laser radar during the generation of the high-precision point cloud map.

[0015] In step S1, the data preprocessing for the lidar, GNSS, and IMU includes: aligning the lidar and IMU timestamps with the GNSS timestamps to achieve time synchronization, using the GNSS system timestamp as a reference; calibrating the internal parameters of the IMU and the external parameters of the IMU and the multi-line lidar; removing motion distortion from the lidar point cloud data; and converting the longitude, latitude, and altitude received by the GNSS system into coordinate information relative to the map origin in the ENU coordinate system of the high-precision point cloud map.

[0016] In step S2, the initial position and attitude are obtained by performing positioning initialization based on lidar point cloud data, GNSS GNS, and IMU data. This includes determining whether the current number of satellites received by the GNSS system is greater than or equal to a preset number. If so, the positioning initialization is performed using GNSS system information to obtain the current approximate position.

[0017] Otherwise, use the point cloud information obtained by the current multi-line lidar to create a scanning context descriptor, compare the scanning context descriptor with the high-precision point cloud map descriptor database, search for the most similar descriptor in the high-precision point cloud map descriptor database, find the current position, and obtain the current rough position;

[0018] After obtaining the current rough position, the IMU data is read to obtain the current heading angle of the mobile robot. This is used as the initial value of the heading angle of the multi-line LiDAR point cloud data. An improved point cloud matching method is used to obtain the current precise position and attitude, and the localization initialization ends.

[0019] The step of using GNSS system information for positioning initialization to obtain the initial position and attitude of the mobile robot includes: reading the current GNSS system timestamp, longitude, latitude, and altitude, and the coordinate information of the current longitude, latitude, and altitude relative to the origin of the high-precision point cloud map; acquiring the current multi-line lidar point cloud information to construct a point cloud sub-map; moving the point cloud sub-map to the corresponding position on the high-precision point cloud map according to the coordinate information acquired by GNSS; and moving the current multi-line lidar data to the corresponding coordinate position on the high-precision point cloud map to obtain the current coarse position. Specifically, this includes:

[0020] Obtain GNSS system information as S GNSS =[GpsTime, Lattitude, Longitude, Altitude], retrieves the state S of the IMU module. IMU =[x roll x pitc x yaw a x a y a z ω x ω y ω z [GpsTime represents the current timestamp of the GNSS system, Lattitude represents the current latitude of the GNSS system, Longitude represents the current longitude of the GNSS system, Altitude module represents the current altitude, x] roll The x value represents the roll angle of the IMU module. pitc The x value represents the pitch angle of the IMU module. yaw This represents the heading angle value of the IMU module, a x This represents the x-axis acceleration count value of the IMU module, a y This represents the y-axis acceleration count value of the IMU module, a z This represents the z-axis acceleration count value of the IMU module, ω. x ω represents the x-axis gyroscope count value of the IMU module. y ω represents the y-axis gyroscope count value of the IMU module. z This represents the z-axis gyroscope count value of the IMU module;

[0021] The longitude, latitude, and altitude obtained from the GNSS system are converted into coordinates in the ENU coordinate system under the current high-precision point cloud map to obtain the initial position information of the mobile robot. Based on the roll angle, pitch angle, and heading angle obtained from the IMU, the initial position and attitude of the mobile robot are finally obtained.

[0022] The step of creating a scanning context descriptor using point cloud information acquired by the current multi-line lidar includes:

[0023] First, the pre-processed multi-line lidar point cloud Projected onto a horizontal plane, and then using the multi-line lidar as the center, the XY plane is uniformly divided into N parts along the radial and circumferential directions of the center. r Each ring and N s Each sector contains multiple laser point clouds that fall into different regions; let the maximum radius of the point cloud region projected onto the XY plane be L. max The radial clearance between the rings is L. max / N r The circumferential spacing is 2π / N s ,but:

[0024]

[0025] Where i represents the index of the specified point cloud projected onto the i-th ring, j represents the index of the specified point cloud projected onto the j-th sector, and N r The index N represents the maximum number of rings. s Index representing the maximum number of sectors. This represents the point cloud of the region block projected onto the i-th ring and the j-th sector;

[0026] Map the point cloud in the region block to a real number. This is used to represent the characteristics of the region / block. Where z(·) represents the Z-axis coordinate of the point;

[0027] The result of the mapping is represented in an N r ×N s From the matrix, the scan context descriptor is obtained; this scan context descriptor is used for feature extraction by dividing the region into fan-shaped areas, and the expression is as follows:

[0028]

[0029] Where i represents the index of the specified point cloud projected onto the i-th ring, j represents the index of the specified point cloud projected onto the j-th sector, and N r The index N represents the maximum number of rings. s Index representing the maximum number of sectors. This represents the point cloud of the region block projected onto the i-th ring and the j-th sector;

[0030] Since the orientation of a mobile robot is not fixed each time it reaches the same location, the observation angle of the multi-line lidar is different each time. Therefore, the sector descriptors obtained from the multi-line lidar point cloud for the same location are different. Remapping these sector descriptors ensures rotation invariance of the point cloud descriptors, which is achieved by introducing cyclic bonds.

[0031] Each row of the sector descriptor is converted into a real number using a ring encoding function, i.e. To represent the ring feature in this way, since the duty cycle of each ring is independent of the viewing angle, using the duty cycle as a feature, we have: The mapping result is represented in a 1×N s From the vector, we obtain the ring bond k, whose expression is as follows:

[0032]

[0033] Where k is the ring key of the scan context descriptor, ψ(r i Let be the duty cycle of the i-th ring. The ring key ensures rotational invariance at different yaw angles, allowing the mobile robot to retrieve whether it has reached the same location through the ring key.

[0034] The scanning context descriptor is compared with a high-precision point cloud map descriptor database. The database is used to search for the most similar descriptor to locate the current position, including:

[0035] The high-precision point cloud map descriptor database is read, and a KD-tree search is used to search within the database to find the descriptor most similar to the current Scan Context descriptor. Specifically, to locate the current position: first, the ring key k is placed into the KD-tree; then, a nearest neighbor search is used to determine the K candidate objects in the descriptor; finally, I is defined. q and I c For the descriptors corresponding to the source point cloud and the candidate point cloud, similarity matching is performed using... and Let represent the column vectors of the source point cloud and the candidate points. The distance between the source point cloud and the candidate points is calculated using cosine similarity, and finally normalized. The expression is:

[0036]

[0037] Exhaustively enumerate all possible column shifts for the sector descriptor to find the minimum distance; the expression is:

[0038]

[0039]

[0040]

[0041] Where D(I) q I c ) represents the minimum difference value among different angles at the same location, n * c* represents the angle corresponding to the minimum difference value, and c* represents the position number corresponding to the highest matching degree in the candidate point cloud sequence. Descriptor I representing the source point cloud q The descriptor after right shifting n columns, where C represents the set of candidate point cloud sequences, [N] s ] represents the set of moves made by the sector descriptor column:

[0042] Using the above formula, the scanning context descriptor created from the current multi-line lidar point cloud data is searched against the high-precision point cloud map descriptor database to find the coordinates (x, y, z) of the most similar descriptor relative to the origin of the high-precision point cloud map. The current 3D lidar point cloud information is then used to construct a point cloud sub-map. The 3D lidar point cloud is moved to the coordinates (x, y, z) using PCL library functions. Finally, based on the position coordinates obtained from GNSS, the current multi-line lidar data is moved to the corresponding coordinate position on the high-precision point cloud map, thus finding the current location.

[0043] In step S3, the precise positioning using lidar includes:

[0044] Receive the point cloud information from the current lidar. Using the mobile robot's current position as the origin, a local map is segmented from the high-precision point cloud map. Based on the improved GICP matching method, point cloud information is used. Match the source point cloud with the local map to obtain the relative pose rotation matrix R and relative translation matrix t between the source point cloud and the target point cloud;

[0045] Wherein, the relative pose rotation matrix R represents the rotation matrix from the position and attitude of the mobile robot obtained in the previous moment to the change in the position and attitude of the mobile robot in the current moment, and the relative translation matrix t represents the displacement matrix from the position and attitude of the mobile robot obtained in the previous moment to the change in the position and attitude of the mobile robot in the current moment.

[0046] Based on the relative pose rotation matrix R and the relative translation matrix t, the initial position coordinates of the mobile robot are calculated. Based on the coordinates at time t-1, the initial position coordinates of the robot are then calculated. Calculate the coordinates of the moving robot at time t, x t y t , z t Let x represent the position coordinates of the moving robot at time t. t-1 y t-1 , z t-1This represents the position coordinates of the moving robot at time t-1;

[0047] Using the known initial position coordinates of the mobile robot, the position coordinates of the mobile robot are updated at each time step to obtain the position coordinate information of the mobile robot in the high-precision point cloud map.

[0048] In step S3, the improved laser point cloud matching method includes: first performing coarse point cloud matching on the laser point cloud data using a normal distribution transformation algorithm, and then performing fine matching on the generalized iterative nearest point after the coarse matching is completed;

[0049] In step S3, matching the currently acquired real-time laser point cloud data with the sub-maps in the global high-precision point cloud map includes:

[0050] The current multi-line LiDAR point cloud data is used as the source point cloud for point cloud matching. The high-precision point cloud map is segmented, with the current coordinates of the mobile robot as the segmentation origin. A positive area is divided with a preset segmentation size, and the area divided by the high-precision point cloud map is used as the target point cloud for point cloud matching.

[0051] Based on the improved laser point cloud matching method, the source point cloud and the target point cloud are first coarsely matched using the NDT algorithm. The result of the coarse matching is the relative pose matrix R′ and t′ between the source point cloud and the target point cloud. The relative pose matrix R′ and t′ calculated by the coarse matching are used as the pose prediction value T′. The source point cloud and the target point cloud are then matched using the gicp algorithm. The pose prediction value T′ is used as the pre-input pose for point cloud matching, and the final pose matrix R and t are calculated.

[0052] In step S5, obtaining the current mobile robot coordinate score includes obtaining the matching score between the 3D laser point cloud and the local map. If the matching score exceeds the set threshold, the 3D point cloud data of the multi-line LiDAR is updated to the corresponding position using the currently obtained relative pose rotation matrix R and relative translation matrix t, thereby updating the current position and attitude of the mobile robot. Otherwise, the process returns to step S2, and S2 to S5 are executed in a loop. The improved laser point cloud matching method is used to update the obtained relative pose rotation matrix R and relative translation matrix t, and then the position and attitude of the mobile robot are relocalized.

[0053] The mobile robot localization method based on multi-sensor fusion of the present invention uses a GNSS system, an IMU module and a multi-line lidar module, combined with an improved GICP point cloud matching algorithm and a Scan Context descriptor algorithm. When the mobile robot encounters problems such as mobile robot kidnapping, signal loss or point cloud matching failure, it can achieve timely and effective relocation, ensuring the real-time performance and effectiveness of mobile robot localization.

[0054] This invention combines the advantages of multiple sensors and their multi-data sources, utilizing a complementary strategy among sensors to ensure accurate positioning of the mobile robot in both indoor and outdoor environments. The positioning method of this invention is unaffected by environmental changes, can switch between indoor and outdoor environments, is unaffected by changes in lighting conditions, and provides excellent positioning performance. Attached Figure Description

[0055] Figure 1 This is a flowchart of a mobile robot positioning method according to an embodiment of the present invention;

[0056] Figure 2 This is a flowchart of multi-sensor data processing according to an embodiment of the present invention;

[0057] Figure 3 This is a flowchart of the positioning initialization process according to an embodiment of the present invention;

[0058] Figure 4 This is a flowchart of the mobile robot positioning process according to an embodiment of the present invention;

[0059] Figure 5 The image shows the effect of the multi-sensor localization method of this invention running on the KITTI dataset 00. Detailed Implementation

[0060] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0061] like Figure 1 As shown, the mobile robot localization method based on multi-sensor fusion according to an embodiment of the present invention includes the following steps:

[0062] Step S1: Load the high-precision point cloud map, preprocess the data from the lidar, GNSS, and IMU to obtain the processed lidar point cloud data, IMU data, and GNSS data.

[0063] In this step, loading the high-precision point cloud map specifically involves loading the laser point cloud map and map descriptor database within the positioning space, obtaining the longitude, latitude, and altitude information of the origin of the current high-precision laser point cloud map, and converting this information into origin information in the positioning coordinate system. The high-precision point cloud map is generated based on high-precision map coordinates, and there is no point cloud ghosting within it. This means the high-precision point cloud map can serve as a true reference for positioning, and the positioning result using the high-precision point cloud map as a reference is the same as the result obtained in the real environment.

[0064] The map descriptor database consists of scan context descriptors generated at the location of each keyframe using a multi-line LiDAR during the generation of a high-precision point cloud map. These scan context descriptors are stored in the map descriptor database.

[0065] In this application, a multi-sensor approach is used, employing Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and multi-line lidar. Therefore, preprocessing of the lidar, GNSS, and IMU data is necessary beforehand. Figure 2 As shown. In practical applications, these three types of sensors can be used, including but not limited to, to make the data received by multiple sensors more reliable in actual use through data preprocessing.

[0066] During sensor data preprocessing, the data acquired by the multi-line lidar module and the IMU module are first time-aligned based on the GNSS timestamp. Since each sensor has its own time source, even if the timestamps of each sensor are aligned initially, timestamp misalignment will still occur after a period of time. Therefore, time alignment first requires unifying the time source. Because the clock of the GNSS system can be calibrated by the atomic clock on the satellite, this embodiment uses the GNSS system as a hardware trigger for the entire multi-sensor fusion process to perform hardware alignment. After hardware alignment, interpolation is used to complete the time alignment of the sensor data. Specifically, the timestamp of the multi-line lidar data acquisition is used as the time point for data interpolation. Taking the linear velocity in the IMU data as an example, the interpolation calculation is performed using the following formula.

[0067]

[0068] The formula indicates that the IMU module receives one frame of multi-line lidar data at time i and time i+1, and the timestamp of the multi-line lidar data is t. x , t i Represents the timestamp at time i, v i Let t represent the velocity along the x-axis at time i. i+1 v represents the timestamp at time i+1. i+1 Let t represent the velocity along the x-axis at time i+1. x v represents the timestamp at time x. x This represents the interpolation result to be obtained at time x. As shown in the formula above, v x This formula represents the IMU's velocity along the x-axis when receiving multi-line lidar data. It can be used to process data from other sensors to obtain the corresponding final data.

[0069] In this step, preprocessing of multiple sensors is performed. Besides time alignment as mentioned above, sensor calibration is also required, including internal parameter calibration of the IMU module and external parameter calibration of the multi-line LiDAR and IMU module. The reason for calibrating the IMU module's internal parameters is to account for manufacturing errors and vehicle body installation errors; the IMU data needs to be calibrated internally before it can be used. Specifically, the `imu_utils` tool can be used for calibration, and the final data will be used to correct the data acquired by the IMU.

[0070] The external parameters of the multi-line lidar and IMU module can be obtained in the following ways:

[0071] A conversion matrix is ​​set between the multi-line LiDAR and the IMU module. This conversion matrix is ​​related to the relative installation positions of the multi-line LiDAR module and the IMU module. The point cloud data acquired by the multi-line LiDAR is then processed. Multiplying the result by the initial transformation matrix yields a new point cloud dataset. Find The distance of each point p′i from the original point cloud data Corresponding point p i The distance is calculated, and the transformation matrix that minimizes the distance is obtained through iterative optimization, which serves as the final external parameter matrix for the multi-line lidar and IMU module.

[0072] This step, after preprocessing the multi-sensor calibration, also includes converting the longitude, latitude, and altitude received by the GNSS system into coordinate information relative to the map origin in the ENU coordinate system of a high-precision point cloud map. This processing step is specifically as follows:

[0073] The system reads the longitude, latitude, and altitude currently received by the GNSS system, obtains the longitude, latitude, and altitude of the origin of the high-precision point cloud map loaded in step 1, and uses the longitude, latitude, and altitude of the high-precision point cloud map as the origin of the ENU coordinate system. It then converts the longitude, latitude, and altitude currently received by the GNSS system into x, y, and z values ​​relative to the origin. The ENU coordinate system, also called the Northeast-Heaven coordinate system, has its x-axis pointing east, y-axis pointing north, and z-axis pointing to the zenith, using a three-dimensional Cartesian coordinate system to describe the current portion of the Earth's surface.

[0074] This step involves preprocessing the multiple sensors, including distortion correction of the point cloud acquired by the multi-line LiDAR. Because the position of each point changes as the multi-line LiDAR rotates throughout the robot's movement, distortion occurs in the point cloud.

[0075] During distortion correction, the time it takes for the mobile robot's LiDAR to complete one rotation, the mobile robot's current speed, and angular velocity are first obtained using the following formula:

[0076]

[0077] The point p in the point cloud can be obtained using the above formula. i timestamp t i The motion of the multi-line lidar network within the current scanning cycle is calculated using interpolation. The motion of the current point relative to the scanning endpoint is calculated, and the coordinates of the current point are multiplied by a transformation matrix to obtain the point cloud of the multi-line lidar network in the current frame. Point cloud after distortion removal

[0078] Where, n i Point p i The index of the point, n start n represents the index of the first point in the point cloud. end t represents the index of the last point in the point cloud. period This indicates the total time of the current point cloud.

[0079] Step S2: Initialize the positioning based on the lidar point cloud data, GNSS, GNS, and IMU data to obtain the initial position and attitude. Move the local point cloud sub-map acquired by the lidar to the corresponding position on the global high-precision point cloud map according to the initial position and attitude.

[0080] In this step, positioning initialization is performed based on multi-sensor data. This involves using data acquired from GNSS, IMU, and multi-line LiDAR to initialize the pose of the mobile robot. The result of pose initialization is to obtain the current position and attitude of the mobile robot relative to the origin of the high-precision point cloud map, with the coordinate system being the ENU coordinate system of the high-precision point cloud map.

[0081] The specific process for obtaining the initial position and attitude of the mobile robot through localization initialization based on lidar point cloud data, GNS, and IMU data is as follows: Figure 3 As shown, it specifically includes:

[0082] First, determine if the GNSS system is available. If the GNSS system is available, read the current GNSS system's timestamp, longitude, latitude, and altitude, and their coordinates relative to the origin of the high-precision point cloud map. Obtain the current multi-line lidar point cloud information to construct a point cloud sub-map. Move the point cloud sub-map to the corresponding position on the high-precision point cloud map based on the coordinate values ​​obtained from the GNSS, thus obtaining the current approximate position. If the GNSS system is unavailable, obtain the current point cloud data from the multi-line lidar. Use this data to create a Scan Context descriptor. Read the high-precision point cloud map descriptor database and use a KD search tree to search for the descriptor most similar to the current Scan Context descriptor. Locate the current position and obtain the current approximate position.

[0083] After obtaining the current rough position using the two methods described above, the IMU data is read to obtain the current heading angle of the mobile robot. This heading angle is used as the initial value of the heading angle of the multi-line LiDAR point cloud data. An improved point cloud matching method is then used to obtain the current precise position and attitude. After the entire data acquisition is completed, the initialization of the entire mobile robot system is finished.

[0084] In determining whether a GNSS system is usable, the number of satellites currently received by the GNSS system can be used as a reference. If the number of satellites received is less than 5, the mobile robot is considered to be in an environment with many obstructions or indoors. In this case, the data received by the GNSS system is of low reliability and cannot be used for pose initialization. Conversely, if the number of satellites received is 5 or more, the mobile robot is considered to be in a relatively open environment. In this case, the data received by the GNSS system is of high reliability and can be used as the basis for system initialization.

[0085] In step 2, when the GNSS system is available, the current GNSS system's timestamp, longitude, latitude, and altitude are read, along with the coordinates of the current longitude, latitude, and altitude relative to the origin of the high-precision point cloud map. The result is the current mobile robot's coordinates (x, y, z) relative to the origin of the high-precision point cloud map. The current multi-line LiDAR point cloud information is then used to construct a point cloud sub-map, and pcl::PointCloud is used. <pcl::pointxyzi>The data type represents the current multi-line laser point cloud data. Will Using the pcl library function to move to the coordinates (x, y, z), the current multi-line lidar data is moved to the corresponding coordinate position on the high-precision point cloud map based on the position coordinate information obtained from GNSS, thus obtaining the current rough position.

[0086] In step 2, when the GNSS system is unavailable, point cloud data acquired by a multi-line lidar is used. Create a ScanContext descriptor, read the high-precision point cloud map descriptor database loaded in step 1, use a KD search tree to search in the high-precision point cloud map descriptor database to find the descriptor most similar to the current Scan Context descriptor, find the current position, and obtain the current rough position.

[0087] The current GNSS module receives 5 or more satellites. When initializing using GNSS module information, the GNSS module information and IMU module status are obtained as follows:

[0088] Obtain GNSS module information as S GNSS =[GpsTime,Lattitude,Longitude,Altitude],

[0089] Get the status S of the IMU module IMU =[x roll x pitch x yaw a x a y a z ω x ω y ω z ].

[0090] Where GpsTime represents the timestamp of the GNSS module at the current moment, Lattitude represents the latitude of the GNSS module at the current moment, Longitude represents the longitude of the GNSS module at the current moment, and Altitude represents the altitude of the GNSS module at the current moment. roll The x value represents the roll angle of the IMU module. pitc The x value represents the pitch angle of the IMU module. yaw This represents the heading angle value of the IMU module, a x This represents the x-axis acceleration count value of the IMU module, a y This represents the y-axis acceleration count value of the IMU module, a z This represents the z-axis acceleration count value of the IMU module, ω. x ω represents the x-axis gyroscope count value of the IMU module. y ω represents the y-axis gyroscope count value of the IMU module. z This represents the z-axis gyroscope count value of the IMU module. Based on the longitude, latitude, and altitude obtained from GNSS, these values ​​are converted to coordinates in the ENU coordinate system under the current high-precision point cloud map to obtain the initial position information of the mobile robot. Based on the roll angle, pitch angle, and yaw angle obtained from the IMU, the current initial position and attitude of the mobile robot are obtained.

[0091] When the number of GNSS satellites received is less than five, a ScanContext descriptor is created using the point cloud information obtained by the current multi-line lidar. The descriptor is compared with the database created by the high-precision point cloud map, and the most similar keyframe is searched to obtain the initial position and attitude of the mobile robot.

[0092] Finally, based on the initial position and attitude obtained by the above two methods, the point cloud sub-map currently acquired by the LiDAR is moved to the corresponding position on the high-precision point cloud map.

[0093] Point cloud data acquired using multi-line lidar. The specific steps for creating a Scan Context descriptor include:

[0094] First, the pre-processed multi-line lidar point cloud Projected onto a horizontal plane, and then using the multi-line lidar as the center, the XY plane is uniformly divided into N parts along the radial and circumferential directions of the center. r Each ring and N s Each sector contains multiple laser point clouds, which fall into different regions. Let the maximum radius of the point cloud region projected onto the XY plane be L. max Therefore, the radial clearance between the rings is calculated as L. max / N r The circumferential spacing is 2π / N s The point cloud formula can be expressed using the parameters calculated above, as follows:

[0095]

[0096] Where i represents the index of the specified point cloud projected onto the i-th ring, j represents the index of the specified point cloud projected onto the j-th sector, and N r The index N represents the maximum number of rings. s Index representing the maximum number of sectors. This represents the point cloud of the region block projected onto the i-th ring and the j-th sector;

[0097] Then the point cloud in the region block is mapped to a real number. This is used to represent the characteristics of the region. The maximum height information of the point cloud can well represent environmental features and requires little computation; therefore, the mapping relationship is as follows: Where z(·) represents the Z-axis coordinate of the point.

[0098] Finally, the result of the mapping is represented in an N r ×N s From the matrix, the Scan Context descriptor is obtained. This descriptor performs feature extraction using a sector-shaped region division, as shown in the following expression:

[0099]

[0100] Where i represents the index of the specified point cloud projected onto the i-th ring, j represents the index of the specified point cloud projected onto the j-th sector, and N r The index N represents the maximum number of rings. s Index representing the maximum number of sectors. This represents the point cloud of the region block projected onto the i-th ring and the j-th sector.

[0101] Since the orientation of the mobile robot when it reaches the same location is not fixed, the observation angle of the multi-line lidar is different each time. The sector descriptors obtained from the point cloud of the multi-line lidar are different for the same location. The sector descriptors are remapped to ensure the rotation invariance of the point cloud descriptors. That is, a loop key is introduced so that the mobile robot can use the loop key to retrieve whether it has reached the same location.

[0102] Specifically, each row of the sector descriptor is converted into a real number using a ring encoding function. This is used to represent the ring feature. Since the duty cycle of each ring is independent of the viewing angle, we use it as a feature here, then: The mapping result is represented in a 1×N s From the vector, we obtain the ring bond k, whose expression is as follows:

[0103]

[0104] Where k is the ring key of the Scan Context descriptor, ψ(r i Let be the duty cycle of the i-th ring. This ring key ensures rotational invariance at different yaw angles, allowing the mobile robot to retrieve whether it has reached the same location via the ring key.

[0105] Specifically, the process involves reading the high-precision point cloud map descriptor database and using a KD search tree to search for the descriptor most similar to the current Scan Context descriptor. This process identifies the current location and includes:

[0106] First, the ring key is placed into a KD-tree, and nearest neighbor search is used to determine the K candidate objects in the descriptor. Then, I is defined. q and I c Similarity matching is performed on the descriptors corresponding to the source and candidate point clouds. Considering that vertical column comparison can better handle dynamic features, therefore... and Let represent the column vectors of the two vectors. The distance between them is calculated using cosine similarity, and finally normalized. The expression is:

[0107]

[0108] Considering that the column order of the sector descriptor may not correspond when viewed from different angles at the same location, we exhaustively enumerate all possible column shifts of the sector descriptor to find the minimum distance, which is expressed as:

[0109]

[0110]

[0111]

[0112] Where D(I) q I c ) represents the minimum difference value among different angles at the same location, n * c represents the angle corresponding to obtaining the minimum difference value. * This represents the position number corresponding to the highest matching degree in the candidate point cloud sequence. Descriptor I representing the source point cloud q The descriptor after right shifting n columns, where C represents the set of candidate point cloud sequences, [N] s [] represents the set of number of times the sector descriptor column has moved.

[0113] Using the above table format, the Scan Context descriptor created from the current multi-line LiDAR point cloud data can be searched against the high-precision point cloud map descriptor database to find the coordinates (x, y, z) of the most similar descriptor relative to the origin of the high-precision point cloud map. Similar to the steps above, the current multi-line LiDAR point cloud information is used to construct a point cloud sub-map. The pcl library function is used to move the data to the coordinates (x, y, z), that is, to move the current multi-line lidar data to the corresponding coordinate position on the high-precision point cloud map based on the position coordinate information obtained from GNSS.

[0114] Step S3: Use LiDAR for precise positioning, match the currently obtained real-time LiDAR point cloud data with the sub-map in the global high-precision point cloud map, use the improved LiDAR point cloud matching method to obtain the relative pose matrix R and t, and convert the current coordinates into global positioning coordinates based on the pose matrix.

[0115] The improved laser point cloud matching method first performs coarse matching of the laser point cloud data using the Normal Distribution Transform (NDT) algorithm, and then performs fine matching using the Generalized Iterative Closest Point (GICP) algorithm after the coarse matching is completed.

[0116] In step 3, precise positioning is achieved using LiDAR, and the currently acquired real-time LiDAR point cloud data is matched with sub-maps in the global high-precision point cloud map. This includes: receiving the current LiDAR point cloud information. Using the mobile robot's current position as the origin, a local map is segmented from the high-precision point cloud map, and the point cloud information is used. Matching with the local map uses an improved GICP point cloud matching method, specifically including:

[0117] like Figure 4 As shown, firstly, the current multi-line lidar point cloud data is... As the source point cloud for point cloud matching, the high-precision point cloud map is segmented. The current coordinates of the mobile robot are used as the segmentation origin, and a positive area is divided with a segmentation size of 50 meters. The area divided by the high-precision point cloud map is used as the target point cloud for point cloud matching.

[0118] Then, based on the improved GICP laser point cloud matching method, the Source point cloud and the Target point cloud are first coarsely matched using the NDT algorithm. The result of the coarse matching is the relative pose matrix R′ and t′ between the Source point cloud and the Target point cloud. The relative pose matrix R′ and t′ calculated by the coarse matching are used as the pose prediction value T′. The GICP algorithm is then used to match the Source point cloud and the Target point cloud. The pose prediction value T′ is used as the pre-input pose for point cloud matching, and the final pose matrix R and t are calculated.

[0119] An improved point cloud matching algorithm combining the NDT and gicp algorithms reduces matching time and improves matching accuracy. Figure 5 The image shows the localization effect of using the improved point cloud matching algorithm on the KITTI dataset.

[0120] Where R represents the rotation matrix of the change in position and attitude of the mobile robot from the previous moment to the current moment, and t represents the displacement matrix of the change in position and attitude of the mobile robot from the previous moment to the current moment.

[0121] After obtaining the relative pose matrix R and t, it can be used to calculate the position of the mobile robot relative to the initial coordinates. Based on the known coordinates at time t-1, the formula is used. Calculate the coordinates of the moving robot at time t, where x t y t , z t Let x represent the robot's position coordinates at time t. t-1 y t-1 , z t-1 This represents the robot's position coordinates at the previous time step, i.e., time t-1.

[0122] In this way, by using the known initialized robot coordinates and updating the robot's position coordinates at each moment, the coordinate information of the current position of the mobile robot in the high-precision point cloud map can be obtained.

[0123] It should be noted that in step S4, based on the global positioning coordinates, the current laser point cloud data is used to create a scanning context descriptor. Specifically, in step S4, the three-dimensional laser point cloud currently obtained by the multi-line lidar is used... Generate a Scan Context descriptor, where p i =(x i y i , z i ) represents the coordinates of point i in the point cloud. The generated Scan Context descriptor I is stored in the sub-database. Its creation process is the same as the method described in step 2, and will not be repeated here.

[0124] Step 5: Obtain the current mobile robot coordinate matching score to determine if the current positioning is reliable. If the matching score is high, the positioning is considered reliable, and the current mobile robot position and attitude need to be updated. If the matching score is low, the positioning is considered unreliable, indicating a positioning failure or mobile robot hijacking problem, requiring mobile robot position and attitude relocalization to update the mobile robot's current true position. Specifically, as in step S5, obtain the 3D laser point cloud. If the matching score S obtained from matching the local map exceeds or equals a set threshold, the relative pose matrix R and t obtained using the improved laser point cloud matching method in step S3 are used as the current pose change matrix to update the robot's current position and attitude. If the local map matching score S is lower than the set threshold, the robot relocalization function is executed.

[0125] Specifically, updating the mobile robot's current location includes:

[0126] The improved point cloud matching algorithm from step 3 is used to match the obtained pose matrices R and t, and the multi-line LiDAR point cloud data is then processed. Update to the corresponding location to update the current position of the mobile robot.

[0127] When determining the matching score, the pcl library function can be used to determine the matching rate between the current laser point cloud and the map. If the matching rate is high, it proves that the point cloud matching is successful and the positioning result is reliable. The pose matrix R and t obtained from the point cloud matching result are used as the final result of the current positioning.

[0128] If the matching rate is low, the point cloud matching fails, and the position and attitude of the mobile robot need to be relocalized. The relocalization process is returned to the execution of the relocalization process, which is the same as the position and attitude initialization of the mobile robot in step 2. This includes: checking the current GNSS signal; when the number of GNSS received satellites is less than the predetermined number, using the point cloud information obtained by the current multi-line lidar to create a scanning context descriptor; comparing the scanning context descriptor with the database created by the high-precision point cloud map; searching for the most similar keyframe; and obtaining the initial position and attitude of the mobile robot.

[0129] The ring feature r generated from the scanning context descriptor of the current multi-line LiDAR point cloud is used to create the current multi-line LiDAR point cloud. i With bond feature k i Place the data into a KD tree and use nearest neighbor search to find the scan context descriptor in the database that is most similar to the current scan context descriptor. Find the location of the most similar scan context descriptor and match the 3D laser point cloud acquired by the multi-line LiDAR with the corresponding location on the high-precision point cloud map to obtain the accurate position and attitude after matching. Iteratively check whether the robot's current position and attitude match the high-precision point cloud map completely until the localization task ends.

[0130] For details regarding the repositioning process, please refer to the relevant description in step 2, which will not be repeated here.

[0131] Experimental results show that the positioning method of the present invention provides good positioning accuracy for mobile robots. In the event of problems such as mobile robot hijacking, signal loss, or point cloud matching failure, the mobile robot can be repositioned to its original location in a short period of time.

[0132] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. It will be apparent to those skilled in the art that the present invention is not limited to the details of the above exemplary embodiments, and that the present invention can be implemented in other specific forms without departing from the spirit or basic features of the present invention.

[0133] Therefore, the embodiments should be regarded as exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, it is intended that all variations falling within the meaning and scope of the equivalents of the claims be included within the invention.

[0134] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.< / pcl::pointxyzi>

Claims

1. A mobile robot localization method based on multi-sensor fusion, characterized in that, Including the following steps: S1: Load the high-precision point cloud map, preprocess the data of lidar, GNSS and IMU, and obtain the preprocessed lidar point cloud data, IMU data and GNSS data; S2: Based on the lidar point cloud data, GNSS data, and IMU data, perform localization initialization to obtain the initial position and attitude of the mobile robot, and move the local point cloud sub-map acquired by the lidar to the corresponding position on the global high-precision point cloud map according to the initial position and attitude of the mobile robot. S3: Using LiDAR for precise positioning, the currently acquired real-time LiDAR point cloud data is matched with sub-maps in the global high-precision point cloud map. The relative pose rotation matrix between the source and target point clouds is obtained based on the improved GICP LiDAR point cloud matching method. and relative translation matrix Convert the current coordinates to global positioning coordinates; S4: Based on the global positioning coordinates, the current laser point cloud data is made into a scanning context descriptor, and the scanning context descriptor and the corresponding global coordinates are stored in the descriptor database. S5: Obtain the current mobile robot coordinate matching score. Based on the matching score, determine whether the current mobile robot position and attitude data need to be updated. If the matching score is higher than the threshold, update it; otherwise, determine if a localization failure or mobile robot hijacking problem has occurred. Repeat S2 to S5 to perform mobile robot position and attitude relocalization until the localization task ends. In step S3, the improved GICP laser point cloud matching method first performs coarse matching of the laser point cloud data using a normal distribution transformation algorithm. After the coarse matching, a generalized iterative nearest-neighbor fine matching is performed, including: first, coarse matching of the source point cloud and the target point cloud using the NDT algorithm. The result of the coarse matching is the relative pose matrix between the source point cloud and the target point cloud. and The relative pose matrix calculated by coarse matching and As pose prediction value The numerical values ​​are used to match the source point cloud and the target point cloud using the GICP algorithm, and the pose prediction values ​​are used. The final relative pose torque matrix is ​​calculated using the pre-input pose for point cloud matching. and relative translation matrix .

2. The mobile robot localization method based on multi-sensor fusion according to claim 1, characterized in that, In step S1, loading the high-precision point cloud map includes loading an existing high-precision laser point cloud map and a map descriptor database generated when constructing the high-precision point cloud map. The map descriptor database stores real-time updated scan context descriptor data. The scan context descriptor data is generated at the position of each key frame using a multi-line laser radar during the generation of the high-precision point cloud map.

3. The mobile robot localization method based on multi-sensor fusion according to claim 2, characterized in that, In step S1, the data preprocessing for the lidar, GNSS, and IMU includes: aligning the lidar and IMU timestamps with the GNSS timestamps to achieve time synchronization, using the GNSS system timestamp as a reference; calibrating the internal parameters of the IMU and the external parameters of the IMU and the multi-line lidar; removing motion distortion from the lidar point cloud data; and converting the longitude, latitude, and altitude received by the GNSS system into coordinate information relative to the map origin in the ENU coordinate system of the high-precision point cloud map.

4. The mobile robot localization method based on multi-sensor fusion according to claim 3, characterized in that, In step S2, the initial position and attitude are obtained by performing positioning initialization based on lidar point cloud data, GNSS data, and IMU data. This includes determining whether the current number of satellites received by the GNSS system is greater than or equal to a preset number. If so, the positioning initialization is performed using GNSS system information to obtain the current approximate position. Otherwise, use the point cloud information obtained by the current multi-line lidar to create a scanning context descriptor, compare the scanning context descriptor with the high-precision point cloud map descriptor database, search for the most similar descriptor in the high-precision point cloud map descriptor database, find the current position, and obtain the current rough position; Once the current approximate position is obtained, the IMU data is read, the current heading angle of the mobile robot is obtained, and this is used as the initial value of the heading angle of the multi-line LiDAR point cloud data. The current precise position is obtained using an improved point cloud matching method, and the localization initialization is complete.

5. The mobile robot localization method based on multi-sensor fusion according to claim 4, characterized in that, The initialization of positioning using GNSS system information to obtain the current coarse location includes: reading the current GNSS system timestamp, longitude, latitude, and altitude, and the coordinate information of the current longitude, latitude, and altitude relative to the origin of the high-precision point cloud map; acquiring the current multi-line lidar point cloud information to construct a point cloud sub-map; moving the point cloud sub-map to the corresponding position on the high-precision point cloud map according to the coordinate information acquired by GNSS; and moving the current multi-line lidar data to the corresponding coordinate position on the high-precision point cloud map to obtain the current coarse location. Specifically, this includes: Obtaining GNSS system information Get the status of the IMU module , This represents the timestamp of the GNSS system at the current moment. This indicates the latitude of the GNSS system at the current moment. This indicates the longitude of the GNSS system at the current time. The current height of the module. This indicates the roll angle value of the IMU module. This indicates the pitch angle value of the IMU module. This indicates the heading angle value of the IMU module. Indicates the IMU module Axis acceleration count value, Indicates the IMU module Axis acceleration count value, Indicates the IMU module Axis acceleration count value, Indicates the IMU module gyroscope count value Indicates the IMU module gyroscope count value Indicates the IMU module gyroscope count value; The longitude, latitude, and altitude obtained from the GNSS system are converted into coordinates in the ENU coordinate system under the current high-precision point cloud map to obtain the approximate initial position of the mobile robot.

6. The mobile robot localization method based on multi-sensor fusion according to claim 5, characterized in that, The step of creating a scanning context descriptor using point cloud information acquired by the current multi-line lidar includes: First, the pre-processed multi-line lidar point cloud Projected onto a horizontal plane, and then using a multi-line lidar as the center, along the radial and circumferential directions respectively... The plane is uniformly divided into Each ring and Each sector, with multiple laser point clouds correspondingly falling into different region blocks; assuming The maximum radius of the point cloud region in the planar projection is The radial gap between the rings is Circumferential interval is ,but: ， in, Indicates the projection of the specified point cloud to the first... Index of a ring, Indicates the projection of the specified point cloud to the first... Index of each sector The index representing the maximum number of rings. Index representing the maximum number of sectors. Indicates projection to the first Ring and the first Point cloud of region blocks in a sector; Map the point cloud in the region block to a real number. This is used to represent the characteristics of the region / block. ,in, Point Axis coordinates; The result of the mapping is represented in From the matrix, the scan context descriptor is obtained; this scan context descriptor is used for feature extraction by dividing the region into fan-shaped areas, and the expression is as follows: ， in, Indicates the projection of the specified point cloud to the first... Index of a ring, Indicates the projection of the specified point cloud to the first... Index of each sector The index representing the maximum number of rings. Index representing the maximum number of sectors. Indicates projection to the first Ring and the first Point cloud of region blocks in a sector; Since the orientation of a mobile robot is not fixed each time it reaches the same location, the observation angle of the multi-line lidar is different each time. Therefore, the sector descriptors obtained from the multi-line lidar point cloud for the same location are different. Remapping these sector descriptors ensures rotation invariance of the point cloud descriptors, which is achieved by introducing cyclic bonds. Each row of the sector descriptor is converted into a real number using a ring encoding function, i.e. This is used to represent the ring feature. Since the duty cycle of each ring is independent of the viewing angle, using the duty cycle as a feature, we have: The result of the mapping is represented in a From the vector, we obtain the ring bond. Its expression is as follows: ， in, To scan the ring keys of the context descriptor, For the first The duty cycle of the ring and the ring key ensure rotational invariance at different yaw angles, allowing the mobile robot to retrieve whether it has reached the same location through the ring key.

7. The mobile robot localization method based on multi-sensor fusion according to claim 6, characterized in that, The scanning context descriptor is compared with the high-precision point cloud map descriptor database. The database is searched for the most similar descriptor to locate the current position, including: Read the high-precision point cloud map descriptor database, and use a KD search tree to search within the database for the descriptor most similar to the current Scan Context descriptor. Find the current location specifically by: first, [the process involves] [adding the ring key]. Insert into a KD-tree and use nearest neighbor search to determine the descriptor. Candidate objects, then define and For the descriptors corresponding to the source point cloud and the candidate point cloud, similarity matching is performed using... and Let represent the column vectors of the source point cloud and candidate points. The distance between the source point cloud and candidate points is calculated using cosine similarity, and finally normalized. The expression is: ， Exhaustively enumerate all possible column shifts for the sector descriptor to find the minimum distance; the expression is: ， ， ， in This represents the minimum difference value among different angles at the same location. This represents the angle corresponding to obtaining the minimum difference value. This represents the position number corresponding to the highest matching degree in the candidate point cloud sequence. Descriptor representing the source point cloud Move right The descriptor following the column, This represents the set of candidate point cloud sequences. A set representing the number of times the sector descriptor column has moved; Using the above expression, the scanning context descriptor created from the current multi-line LiDAR point cloud data is searched against the high-precision point cloud map descriptor database to find the coordinates of the most similar descriptor's location relative to the origin of the high-precision point cloud map. Find the current location.

8. The mobile robot localization method based on multi-sensor fusion according to claim 7, characterized in that, In step S3, the precise positioning using lidar includes: Receive the point cloud information from the current lidar. Using the mobile robot's current position as the origin, a local map is segmented from the high-precision point cloud map. Based on the improved GICP matching method, point cloud information is used. By matching with the local map, the relative pose rotation matrix between the source point cloud and the target point cloud is obtained. and relative translation matrix ; Among them, the relative pose rotation matrix The rotation matrix and the relative translation matrix represent the changes in the robot's position and orientation from the previous moment to the current moment. This represents the displacement matrix representing the change in the position and attitude of the mobile robot from the previous moment to the current moment. Based on relative pose rotation matrix and relative translation matrix Calculate the initial position coordinates of the mobile robot, based on the coordinates at time t-1, using... calculate Move the robot's coordinates constantly. express t The robot's position coordinates are constantly being moved. express t The position coordinates of the moving robot at time -1; Using the known initial position coordinates of the mobile robot, the position coordinates of the mobile robot are updated at each time step to obtain the position coordinate information of the mobile robot in the high-precision point cloud map.

9. The mobile robot localization method based on multi-sensor fusion according to claim 8, characterized in that, In step S3, matching the currently acquired real-time laser point cloud data with the sub-maps in the global high-precision point cloud map includes: The current multi-line LiDAR point cloud data is used as the source point cloud for point cloud matching. The high-precision point cloud map is segmented, with the current coordinates of the mobile robot as the segmentation origin. A positive area is divided with a preset segmentation size, and the area divided by the high-precision point cloud map is used as the target point cloud for point cloud matching.

10. The mobile robot localization method based on multi-sensor fusion according to claim 9, characterized in that, In step S5, obtaining the current mobile robot coordinate score includes obtaining the matching score between the 3D laser point cloud and the local map. If the matching score exceeds a set threshold, the currently obtained relative pose rotation matrix is ​​used. and relative translation matrix The 3D point cloud data of the multi-line LiDAR is updated to the corresponding position, thus updating the current position and attitude of the mobile robot. Otherwise, return to step S2, and repeat steps S2 through S5, using the improved laser point cloud matching method to update the relative pose rotation matrix. and relative translation matrix Then, the position and attitude of the mobile robot are repositioned.

Images