Mobile robot global repositioning method and device based on 3D laser point cloud

Abstract

The invention discloses a mobile robot global repositioning method and device based on 3D laser point clouds, and the method comprises the steps: obtaining a 3D point cloud map of a current environment, and obtaining the 3D laser point clouds in the driving process of a mobile robot; generating a 2D laser point cloud by using the 3D laser point cloud and generating a 2D grid map by using the 3D point cloud map; performing related scanning matching on the 2D laser point cloud and the grid map to obtain an optimal 2D estimated pose; transforming the 3D laser point cloud to be near the optimal 2D estimated pose, and performing registration on the 3D laser point cloud and the 3D point cloud map to obtain an optimal 3D estimated pose; and verifying the currently obtained optimal 2D estimated pose according to the registration result, and if the verification is passed, outputting the optimal 2D and 3D estimated poses as the current estimated pose. The method has the advantages of being simple in implementation method, low in cost, high in positioning efficiency and reliability, high in flexibility and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous navigation of mobile robots, and in particular to a global repositioning method and device for a mobile robot based on 3D (Dimensions) laser point cloud. Background Art

[0002] Real-time positioning is a crucial part of autonomous navigation of mobile robots, which determines whether the robot can accurately perceive its own position and environmental information, so as to effectively complete the navigation task. Although the Simultaneous Localization and Mapping (SLAM) technology can enable the robot to build an environmental map and determine the robot's position relative to the environmental map under the premise of unknown scenes and unknown states, in the traditional SLAM method, the initial position of the robot relative to the existing environmental map is unknown each time the robot is turned on, so the map needs to be rebuilt after each startup, and the environmental map cannot be effectively reused. The mapping process often requires manual assistance, which greatly increases the workload and time cost of mapping. In addition, to complete the relocalization, the robot must move a certain distance to build a sub-map containing sufficient environmental information, and the robot will be at risk of motion collision when running under the premise of initial unknown.

[0003] In order to solve the problem of the robot's initial position positioning relative to the existing map, one solution strategy in the prior art is to assume that the robot's starting point is the same each time, so as to turn the robot's initial position relative to the existing environment map into a known problem. This strategy can bypass the robot's initial positioning problem, but the robot needs to start from the same fixed position each time, which greatly limits the flexibility of the robot's tasks. Another solution strategy is that the robot will request the user to specify an initial position before the navigation begins, but this type of navigation method requires human participation and cannot achieve fully autonomous mobile robot navigation. In addition, some outdoor robots can rely on GPS (Global Positioning System) to obtain the initial position, but GPS cannot provide direction information, which means that the method of obtaining the initial position based on GPS also needs to be combined with other sensors to jointly determine the robot's initial direction, which has a high implementation cost, and it is difficult to ensure that the GPS device works normally in indoor and some outdoor GPS blocking areas. Summary of the invention

[0004] The technical problem to be solved by the present invention is: in view of the technical problems existing in the prior art, the present invention provides a global repositioning method and device for a mobile robot based on 3D laser point cloud, which has a simple implementation method, low cost, high positioning efficiency and reliability.

[0005] In order to solve the above technical problems, the technical solution proposed by the present invention is: A global relocalization method for a mobile robot based on 3D laser point cloud, comprising the following steps: Obtain a 3D point cloud map of the current environment of the mobile robot, and obtain a 3D laser point cloud obtained by a laser radar scanning the surrounding environment of the mobile robot during the driving of the mobile robot, wherein the 3D point cloud map is constructed in advance based on the 3D laser point cloud obtained by the mobile robot traversing the current environment; Using the 3D laser point cloud to generate a 2D laser point cloud and using the 3D point cloud map to generate a 2D grid map; Perform correlation scanning and matching between the 2D laser point cloud and the grid map, and obtain the best 2D estimated pose according to the matching result; Transforming the 3D laser point cloud to a vicinity of the optimal 2D estimated pose, and obtaining the optimal 3D estimated pose by registering the 3D laser point cloud with the 3D point cloud map; The currently obtained best 2D estimated pose is verified according to the registration result between the 3D laser point cloud and the 3D point cloud map. If the verification passes, the best 2D estimated pose and the best 3D estimated pose are output as the current estimated pose.

[0006] Furthermore, the correlative scanning and matching of the 2D laser point cloud and the grid map includes: The 2D laser point cloud is rotated and translated so that the laser points in the 2D laser point cloud fall within the grid map; The probability that the laser point in the 2D laser point cloud hits the grid map is calculated, and the best matching pose between the current 2D laser point cloud and the grid map is searched according to the calculated probability, that is, the best 2D estimated pose is obtained.

[0007] Furthermore, searching for the best matching pose between the current 2D laser point cloud and the grid map according to the calculated probability includes: Generate all possible pose solution sets on the grid map according to different resolutions; Using a branch and bound method to search for the best matching pose in the pose solution set to obtain the best 2D estimated pose; The branch and bound method obtains the pose solution set as the initial candidate pose set and uses the grid map layer to build a multi-resolution map. The candidate poses in the current candidate pose set are first branched and expanded to obtain a candidate pose subset, and then pruned in the candidate pose subset. The branch expansion and pruning operations are recursively performed on each candidate pose subset until a termination condition is met, and finally the pose with the highest score is selected as the best matching pose, that is, the best 2D estimated pose.

[0008] Furthermore, in the process of using the branch and bound method, different sizes of angular step sizes are used on maps with different resolutions, wherein a first step size and a first angular resolution are used on a first resolution map to generate candidate poses, and a second step size and a second angular resolution are used on a second resolution map to generate candidate poses, the first resolution is smaller than the second resolution, the first step size is larger than the second step size, and the first angular resolution is larger than the second angular resolution.

[0009] Furthermore, after obtaining the best 2D estimated pose, it also includes scoring the best 2D pose according to the matching status of the current laser frame and the grid map, and judging the validity of the current pose estimation according to the scoring result, wherein if the score value is greater than a preset threshold, the subsequent alignment between the 3D laser point cloud and the 3D point cloud map is continued, otherwise it is judged that the current pose estimation has failed.

[0010] Furthermore, the best 2D pose is scored according to the following formula:

[0011] Where X represents the best estimated 2D pose, represents the score of pose X in the grid map, h represents the number of laser points that the current laser frame hits the grid map, and n represents the number of laser points that the current laser frame does not hit the grid map.

[0012] Furthermore, the verification of the currently obtained best 2D estimated pose according to the alignment result between the 3D laser point cloud and the 3D point cloud map includes: calculating the 3D pose score according to the point cloud similarity between the 3D laser point cloud and the 3D point cloud map, and if the calculated 3D pose score is greater than a preset threshold, the verification is judged to be passed, otherwise the verification fails.

[0013] Furthermore, converting the 3D laser point cloud into a 2D laser point cloud comprises: Filter each frame of 3D laser point cloud, remove laser points that are not within the preset interception interval, and then project the 3D laser point cloud into a 2D laser point cloud; The 2D laser point cloud is downsampled to remove duplicate laser points to obtain the final 2D laser point cloud.

[0014] A mobile robot global repositioning device based on 3D laser point cloud comprises a processor and a memory, wherein the memory is used to store a computer program, and the processor is used to execute the computer program to perform the above method.

[0015] A computer-readable storage medium storing a computer program, wherein the computer program implements the above method when executed.

[0016] Compared with the prior art, the advantages of the present invention are: 1. The present invention realizes the global repositioning of the mobile robot by integrating 2D and 3D point cloud matching methods, uses 2D laser point cloud and raster map for correlation scanning matching to search for the best 2D pose estimation, uses 3D laser point cloud frame and point cloud map for point cloud matching to obtain the best 3D pose estimation, and uses the point cloud registration result to verify the estimated pose, and uses the pose estimation after verification as the positioning result of repositioning. It can make full use of the 2D point cloud matching method to accelerate the search speed of pose estimation, and use the 3D point cloud matching method to verify the 2D pose search results, which can take into account both the search speed and the credibility of the search results to achieve fast and accurate global repositioning.

[0017] 2. When the initial position of the robot relative to the environmental map is unknown, the present invention can provide a reliable initial position for the robot when it needs to be repositioned, and can realize the reuse of the environmental map by the mobile navigation robot without the need to rebuild the map every time the robot is turned on, thereby reducing the cost of repeated mapping and improving the navigation efficiency of the robot. It can also quickly restore the robot's position when the position is lost during the navigation process, thereby improving the navigation success rate and navigation efficiency.

[0018] 3. In the present invention, the robot can complete the entire repositioning process by keeping still during the repositioning process, and the risk of movement collision of the robot can also be reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a schematic diagram of the implementation flow of the global relocalization method of a mobile robot based on 3D laser point cloud in this embodiment.

[0020] Figure 2 It is a schematic diagram of a flow chart for realizing global repositioning of a mobile robot in a specific application embodiment of the present invention.

[0021] Figure 3 It is a schematic diagram of the grid map obtained in the specific application embodiment of the present invention and the effect of the laser point cloud hitting. DETAILED DESCRIPTION

[0022] The present invention is further described below in conjunction with the accompanying drawings and specific preferred embodiments, but the protection scope of the present invention is not limited thereby.

[0023] For ease of understanding, the relevant technical background of the present invention is first introduced.

[0024] SLAM methods can be divided into two types: 2D SLAM based on LiDAR and 3D SLAM. 2D SLAM uses a single LiDAR to scan on a two-dimensional plane and completes mapping based on the 2D LiDAR point cloud. This type of method can obtain angle and distance information on a two-dimensional coordinate system, but cannot obtain object height information. It is easy to trigger wrong positioning in an environment where there are many local scenes with high similarity. 3D SLAM scans in both horizontal and vertical dimensions through 3D LiDAR, and completes mapping based on the 3D LiDAR point cloud using the point cloud registration method. It can obtain angle and distance information on a three-dimensional coordinate system, thereby obtaining object height information, which can improve positioning accuracy. However, since the point cloud registration method needs to rely on initial values, 3D SLAM must first have a rough initial pose. In the absence of an initial pose, the point cloud registration method cannot be used for positioning. Regardless of whether it is 2D SLAM or 3D SLAM, the initial position of the robot relative to the existing environmental map is unknown each time the robot is turned on. Therefore, the map needs to be rebuilt each time it is turned on. Manual assistance is also required in the map building process, which greatly increases the workload and labor time cost of map building. In addition, to complete repositioning based on SLAM, the robot needs to move a certain distance to build a sub-map containing sufficient environmental information. It is difficult for the robot to complete repositioning in a stationary state.

[0025] When the mobile robot needs to be relocated, the present invention realizes the global relocation of the mobile robot by integrating 2D and 3D point cloud matching. First, the 3D laser point cloud scanned by the lidar and the pre-constructed point cloud map are obtained, and a 2D laser point cloud and a raster map are generated according to the 3D laser point cloud and the point cloud map respectively. The 2D laser point cloud and the raster map are used to perform correlation scanning and matching to quickly search for the best 2D pose estimation. On the basis of the best 2D pose estimation, the 3D laser point cloud frame and the point cloud map are matched to obtain the best 3D pose estimation after registration. At the same time, the registration results of the 3D laser point cloud and the point cloud map are used to verify the estimated best 2D pose. The best 2D pose estimation and the best 3D pose estimation after verification are used as the positioning results of repositioning, which can make full use of the 2D point cloud matching method to accelerate the search speed of pose estimation, and use the 3D point cloud matching method to verify the 2D pose search results, to ensure the accuracy and reliability of the positioning results, so that both the search speed and the credibility of the search results can be taken into account to achieve fast and accurate global repositioning, and a reliable initial pose can be provided for the robot in the initial pose of the unknown robot relative to the environmental map, and the mobile navigation robot can reuse the environmental map without rebuilding the map every time it is turned on, reducing the cost of repeated mapping and improving the navigation efficiency of the robot. The present invention can complete the entire repositioning process by keeping the robot still during the repositioning process, and can also reduce the risk of motion collision of the robot.

[0026] The present invention can be applied to the application scenario of providing an initial posture for a robot when the initial posture of the robot relative to the environment map is unknown, and can also be applied to the application scenario of restoring positioning when the robot cannot estimate its own posture relative to the environment map due to the existence of cumulative positioning errors and the occurrence of the "robot kidnapping problem" during the robot navigation process, so as to help the robot quickly restore positioning to ensure the efficiency and success rate of navigation, and improve the safety and reliability of the navigation process. The robot kidnapping problem refers to the loss of positioning of the robot during the navigation process due to the occlusion of the sensor and the simultaneous movement of the robot.

[0027] like Figure 1 As shown, the steps of the mobile robot global relocalization method based on 3D laser point cloud in this embodiment include: Step S01. Obtain a 3D point cloud map of the current environment of the mobile robot, and obtain a 3D laser point cloud obtained by a laser radar scanning the surrounding environment of the mobile robot during the driving of the mobile robot, wherein the 3D point cloud map is constructed in advance based on the 3D laser point cloud obtained by the mobile robot traversing the current environment.

[0028] In this embodiment, a multi-line laser radar is pre-installed on the controlled mobile robot, and the multi-line laser radar is used to sense the surrounding environment during the driving process of the mobile robot, and a 3D laser point cloud is obtained by scanning. It can be understood that in addition to the multi-line laser radar, sensors such as solid-state laser radar and cameras can also be used.

[0029] When the mobile robot needs to be repositioned, for example, it needs to be repositioned when it is restarted or it needs to be restored due to the "robot kidnapping problem", the 3D laser point cloud data obtained by the real-time scanning of the lidar is obtained respectively, and the point cloud map of the current environment is obtained as input data.

[0030] The point cloud map is pre-constructed based on the 3D laser point cloud obtained by the mobile robot traversing the current environment, and the pre-constructed point cloud map of the current environment can be directly read. For example, when the robot has a navigation need in a new scene for the first time, the mobile robot platform is equipped with a multi-line laser radar sensor to traverse the environment for a week, and the continuous laser point cloud is aligned using the ICP (Iterative Closest Point) method to estimate the robot's motion state, generate a point cloud map, and store the generated point cloud map after filtering and downsampling. When repositioning is required in this environment later, the stored point cloud map can be directly read. Since the point cloud map can be reused, there is no need to rebuild the map every time the machine is turned on, which can reduce a lot of map construction costs and time. By reusing the point cloud map, the robot can also complete the entire repositioning process while remaining stationary, reducing the robot's driving risk.

[0031] This embodiment uses the three-dimensional point cloud in the environment for repositioning, which has stronger perception capabilities than two-dimensional point clouds, and can make full use of the information in the surrounding environment to complete positioning, reduce the probability of erroneous positioning, and improve the success rate of positioning.

[0032] Step S02: Generate a 2D laser point cloud using a 3D laser point cloud and generate a 2D grid map using a 3D point cloud map.

[0033] After the 3D laser point cloud is acquired, a copy of the 3D laser point cloud is copied and converted to generate a 2D laser point cloud. Optionally, converting the 3D laser point cloud to a 2D laser point cloud may include: Step S201. Filter each frame of 3D laser point cloud, remove laser points that are not within a preset interception interval (for example, larger than or smaller than the preset interception interval), and then project the 3D laser point cloud into a 2D laser point cloud; Step S202: downsample the projected 2D laser point cloud to remove duplicate laser points to obtain a final 2D laser point cloud.

[0034] Point cloud map uses a large number of laser points in 3D space to represent the environment map. Optionally, a grid map can be generated by intercepting the 3D point cloud, for example, intercepting the 3D laser points by height, discarding the 3D laser points outside the height range, and then projecting and downsampling the remaining laser points within the height range, and generating a grid map based on the plane coordinates of the laser points in the point cloud. The grid map uses small grids to represent a two-dimensional plane. Each grid has three states: occupied, idle, and unknown. The occupied state indicates that there are obstacles and the robot cannot pass through. The idle state indicates that there are no obstacles and the robot can pass through. The unknown state indicates that the robot has not yet observed the space.

[0035] Step S03: Perform Correlative Scan Matching (CSM) on the 2D laser point cloud and the grid map, and obtain the best 2D estimated pose based on the matching result.

[0036] After obtaining the 2D laser point cloud and the grid map, this embodiment uses correlation scanning matching to quickly search for the best 2D estimated pose.

[0037] Optionally, correlating the 2D laser point cloud with the raster map may include: Step S301. Rotate and translate the 2D laser point cloud so that the laser points in the 2D laser point cloud fall within the grid map; Step S302: Calculate the probability that the laser point in the 2D laser point cloud hits the grid map, and search for the best matching pose between the current 2D laser point cloud and the grid map based on the calculated probability, so as to obtain the best 2D estimated pose.

[0038] Optionally, the probability of the laser hitting the grid map can be calculated as shown in formula (1) to search for the best matching pose between the current laser frame and the existing grid map: (1) Among them, X represents the robot posture, X contains a rotation matrix R and a translation matrix t, n is the number of laser points in a frame of laser point cloud, f ( X ) is the evaluation function. When the laser hits the occupied grid of the grid map, the evaluation function f ( X ) score, otherwise f ( X ) does not score. Indicates i Evaluation function corresponding to the frame laser point cloud.

[0039] As Figure 3 The grid map obtained in the specific application embodiment is shown as an example, where the red arrow and the purple circle represent the estimated position and orientation of the robot. The colored dots other than the red arrow and the purple circle represent a frame of 2D laser point cloud. Different colors in the colored dots represent point clouds formed by different reflection intensities, where purple, red, blue and green represent point clouds formed by different reflection intensities. The black part represents the occupied grid of the grid map, and the gray part represents the idle grid. When the laser point cloud overlaps with the occupied grid, it means that the laser clicks on the grid map evaluation function f(X) to score.

[0040] Preferably, searching for the best matching pose between the current 2D laser point cloud and the grid map according to the calculated probability specifically includes: Generate all possible pose solution sets on the grid map according to different resolutions; Use the branch and bound method to search for the best matching pose in the pose solution set to obtain the best 2D estimated pose; Among them, the branch and bound method is to obtain a set of pose solutions as the initial candidate pose set and use the raster map layer to build a multi-resolution map. First, the candidate poses in the current candidate pose set are branched and expanded to obtain a candidate pose subset, and then pruned in the candidate pose subset. The branch expansion and pruning operations are recursively performed on each candidate pose subset until the termination condition is met. The termination condition can be the size of the search space, the search depth, or the error threshold. Finally, the pose with the highest score is selected as the best matching pose, that is, the best 2D estimated pose.

[0041] The above pruning strategy is to terminate the search on some branches in advance during the search process to reduce the number of nodes that need to be processed, thereby reducing the scale of calculation and improving the search speed. Branch and bound is used to accelerate the CSM matching strategy. By recursively expanding and pruning the candidate pose subsets, the best matching pose can be quickly searched. By searching for the best 2D estimated pose of the robot based on 3D lidar and existing environment maps, combined with the branch and bound accelerated search strategy, the relocation search time can be greatly reduced, and the positioning efficiency can be further improved, thereby achieving rapid global relocation of the robot.

[0042] In a multi-layer map, the map resolutions of different layers are different. Furthermore, in the process of using the branch and bound method, different angular step sizes are used on maps of different resolutions, wherein the first step size and the first angular resolution are used on the first resolution map to generate candidate poses, and the second step size and the second angular resolution are used on the second resolution map to generate candidate poses, the first resolution is smaller than the second resolution, the first step size is larger than the second step size, the first angular resolution is larger than the second angular resolution, and the angular resolution is the minimum discernible angle. That is, a larger step size and angular resolution are used to generate the solution space (candidate poses) on a low-resolution map, and a smaller step size and angular resolution are selected on a high-resolution map to generate candidate poses. By using different search step sizes and angular resolutions for maps of different resolutions, the number of candidate sets can be effectively reduced, thereby further reducing the matching search time and improving the search efficiency.

[0043] Preferably, after obtaining the best 2D estimated pose, the best 2D pose is scored according to the matching status of the current laser frame and the grid map, and the validity of the current pose estimation is judged according to the scoring result, wherein if the score value is greater than a preset threshold, the subsequent alignment between the 3D laser point cloud and the 3D point cloud map is continued, otherwise the current pose estimation is judged to have failed.

[0044] Optionally, the best 2D pose can be scored as follows: (2) Where X represents the best estimated 2D pose, represents the score of pose X in the grid map, h represents the number of laser points in the grid map hit by the current laser frame, and n represents the number of laser points in the grid map that the current laser frame does not hit. Both n and h are non-negative integers.

[0045] Specifically, the estimated 2D pose X can be judged according to formula (2). When the score S of the estimated pose X is less than the threshold, it is considered that the pose estimation fails, and the pose estimation failure is directly output. When the score S of the estimated pose X is greater than the threshold, the next step is entered to perform the point cloud registration verification link.

[0046] Step S04: transform the 3D laser point cloud to the vicinity of the best 2D estimated pose, and obtain the best 3D estimated pose by aligning the 3D laser point cloud with the 3D point cloud map.

[0047] Since 3D point cloud matching needs to rely on the initial pose, this embodiment can be equivalent to providing an initial value for 3D point cloud matching by transforming the 3D laser point cloud to near the best 2D estimated pose, so that the 3D laser point cloud can be subsequently aligned with the 3D point cloud map.

[0048] After searching for the best 2D estimated pose, this embodiment uses a point cloud registration method to verify the correctness of the best estimated pose. First, the 3D point cloud frame is transformed to the vicinity of the 2D best estimated pose, and then the correctness of the 2D best estimated pose is verified by registering the point cloud frame and the point cloud map. Optionally, the ICP point cloud registration method can be used to align the multi-line laser radar point cloud frame and the point cloud map for registration. Of course, other types of point cloud registration methods such as NDT (Normal Distributions Transform) and feature matching can also be used.

[0049] For example, assuming that the point cloud map is M, at the current moment, the multi-line laser radar scans the robot's surrounding environment to generate a 3D point cloud frame F, and the best 2D pose is X. For any laser point in the 3D point cloud frame F, the implementation process of transforming to the best 2D estimated pose is shown in formula (3): (3) in, Represents a laser point in a frame of laser point cloud F The laser point after conversion to the best 2D estimated pose. The best estimated pose X=[x,y,yaw]T, where x and y are plane Cartesian coordinates, and yaw represents the orientation of the robot. X’ The 3D transformation matrix representing the best 2D estimated pose consists of a rotation matrix and a translation vector, expressed in vector form as X'=[x,y,0,yaw,0,0]T.

[0050] Step S05. Verify the best 2D best estimated pose according to the registration result between the 3D laser point cloud and the 3D point cloud map. If the verification passes, output the best 2D estimated pose and the best 3D estimated pose as the current estimated pose.

[0051] This embodiment verifies the positioning result by using the three-dimensional point cloud, so as to ensure the reliability of the positioning result, thereby obtaining a positioning result with a higher confidence level.

[0052] Optionally, verifying the best 2D best estimated pose according to the registration result between the 3D laser point cloud and the 3D point cloud map includes: calculating a 3D pose score according to the point cloud similarity between the 3D laser point cloud and the 3D point cloud map, and judging that the verification is passed if the calculated 3D pose score is greater than a preset threshold, otherwise the verification fails. After transforming the point cloud frame F to the vicinity of the best 2D estimated pose X, the point cloud similarity score Sicp and the best 3D pose estimate can be obtained by registering the point cloud through the ICP method, and the point cloud registration result can be scored using the point cloud similarity score Sicp, and then judging whether the verification is passed based on the scoring result.

[0053] Optionally, the point cloud registration result can be scored according to the following formula: (4) in, represents the 3D pose score, It represents the point cloud similarity score obtained by registering the point cloud frame F and the point cloud map M based on the point cloud registration method ICP. When the 3D pose score S3d is greater than the preset threshold, the verification is passed and the current estimated best 2D estimated pose and 3D pose estimation result are output; when the 3D pose score S3d is greater than the preset threshold, the verification is passed and the best 2D estimated pose and 3D pose estimation result are output; When it is less than the preset threshold, the verification fails and the output pose estimation fails.

[0054] The present invention is further described below by taking a specific application embodiment in which the above method of the present invention is used to construct a positioning system to achieve global positioning of a mobile robot as an example. Figure 2 As shown, the input of this system is a multi-line laser radar, and the output is the initial pose estimation of the robot relative to the environment map. The environment point cloud map is constructed by using the multi-line laser radar, and the 2D laser radar point cloud and the grid map are generated using the 3D laser point cloud and the 3D point cloud map respectively. The 2D best pose is searched and the score of the 2D laser point cloud hitting the grid map under the pose is calculated. If the score is greater than the threshold, a point cloud registration check is performed on the best estimated pose using the 3D laser point cloud and the point cloud map. If the check passes, the best estimated pose is output. If the best estimated pose score is less than the threshold or the check fails, the pose is not output. The specific modules in the system are as follows: The multi-line laser radar module is used to perceive the environment around the robot and describe the environmental information around the robot with 3D laser point cloud.

[0055] The 3D point cloud to 2D point cloud module is used to generate a 2D laser point cloud after intercepting the 3D laser point cloud output by the multi-line laser radar. Specifically, it filters a frame of 3D laser point cloud, removes laser points with height values ​​higher and lower than the interception interval, and then projects the 3D laser point cloud into a 2D laser point cloud.

[0056] The 2D laser point cloud module is used to receive the 2D laser point cloud frame output by the 3D point cloud to 2D point cloud module and downsample the 2D laser point cloud to remove duplicate laser points.

[0057] The simultaneous positioning and mapping module is used to receive the 3D laser point cloud frames output by the multi-line laser radar module, and align the continuous laser point cloud frames using the ICP point cloud registration method to obtain the robot's motion state estimation and generate a point cloud map at the end of mapping. The simultaneous positioning and mapping module only works when the robot is operating for the first time in a new scene to build an environmental point cloud map. After having the point cloud map, the robot does not need to start the simultaneous positioning and mapping module to repeat the mapping when it is subsequently positioned in the scene, and can directly use the point cloud map.

[0058] The point cloud map module is used to filter and downsample the point cloud map generated by the simultaneous positioning and mapping module, and then store the point cloud map locally on the computer.

[0059] The point cloud map to raster map module is used to generate a raster map by intercepting 3D point clouds.

[0060] The raster map module is used to receive the point cloud output by the point cloud map to raster map module, and generate a raster map based on the plane coordinates of the laser points in the point cloud.

[0061] The correlation scan matching module is used to rotate and translate the 2D laser point cloud so that as many laser points as possible in the 2D laser point cloud fall on the grid map, and the best matching pose between the current laser frame and the existing grid map is found by calculating the probability of the laser point hitting the grid map. The branch and bound acceleration module is used to accelerate the search process of correlation scan matching using the branch and bound method. The correlation scan matching module generates all possible pose solution sets on the grid map according to different resolutions, and then transmits the solution space to the branch and bound acceleration search module, which generates an optimal 2D estimated pose.

[0062] The branch and bound acceleration module specifically obtains the output from the relevant scan matching module as the initial candidate pose set and constructs a multi-resolution map using the Sanger map layer. First, a portion of the candidate poses with low scores are filtered out by the minimum score threshold, and then the candidate poses are branched and expanded to obtain a subset of candidate poses with higher resolution. This subset of candidate poses is the search space of the candidate poses, representing a higher resolution and more accurate pose estimation of the robot relative to the map; during the search process, different angular step sizes are selected on maps of different resolutions to reduce the number of candidate sets and improve the search efficiency. Then, a pruning strategy is used in the search space of the candidate poses to reduce the complexity of the search, and the above branch-reduction steps are recursively applied to each subset until the termination condition is met. Finally, after all searches are completed, the pose with the highest score is selected as the best 2D estimated pose.

[0063] The 2D pose scoring module is used to receive the best estimated pose X output by the branch and bound module and score the estimated pose X.

[0064] The pose score judgment module is used to judge the estimated 2D pose X. When the score S of the estimated pose X is less than the threshold, the pose estimation is considered to have failed, and the pose estimation output module directly outputs the pose estimation failure. When the score S of the estimated pose X is greater than the threshold, the next step of point cloud registration verification is entered.

[0065] The point cloud registration module is used to verify the correctness of the best estimated pose using the point cloud registration method. First, the 3D point cloud frame is transformed to the vicinity of the 2D best estimated pose, and then the correctness of the 2D best estimated pose is verified by registering the point cloud frame and the point cloud map.

[0066] The 3D pose scoring module is used to score the results of the point cloud registration module. When the value is greater than the threshold, the estimated pose output module outputs the 2D and 3D pose estimation results. When it is less than the threshold, the verification fails and the estimated pose output module outputs the pose estimation failure.

[0067] The estimated pose output module is used to output the 2D best estimated pose and the 3D best estimated pose. When the 2D pose score and the 3D pose score are greater than their respective set thresholds, the best 2D estimated pose and the best 3D estimated pose are output as the output results of repositioning. When the 2D pose score and the 3D pose score are less than their respective set thresholds, the repositioning fails and the positioning result is not output.

[0068] The above system can realize the rapid and precise repositioning of the mobile robot, taking into account the accuracy and efficiency of positioning, and efficiently provide a reliable initial pose for the mobile navigation robot. The entire positioning process only requires one multi-line laser radar to complete the initial pose estimation and estimated pose verification of the robot, without relying on other sensors and maps. It can not only reduce the implementation cost, but also can be easily embedded in other SLAM methods, which can improve the robustness of positioning.

[0069] This embodiment further provides a computer device, including a processor and a memory, wherein the memory is used to store a computer program, and the processor is used to execute the computer program to perform the above method.

[0070] It is understandable that the above method of this embodiment can be executed by a single device, such as a computer or server, etc., and can also be applied to a distributed scenario and completed by multiple devices in cooperation with each other. In the case of a distributed scenario, one of the multiple devices can only execute one or more steps in the above method of this embodiment, and multiple devices interact to complete the above method. The processor can be implemented in the form of a general-purpose CPU, a microprocessor, an application-specific integrated circuit, or one or more integrated circuits, etc., for executing related programs to implement the above method of this embodiment. The memory can be implemented in the form of a read-only memory ROM, a random access memory RAM, a static storage device, and a dynamic storage device. The memory can store an operating system and other applications. When the above method of this embodiment is implemented by software or firmware, the relevant program code is stored in the memory and called and executed by the processor.

[0071] This embodiment further provides a computer-readable storage medium storing a computer program, and when the computer program is executed by a processor, the above method is implemented.

[0072] Those skilled in the art should understand that the above-mentioned embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application can take the form of a computer program product implemented on one or more computer-readable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the process Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing device to work in a specific way, so that the instructions stored in the computer-readable memory produce a product including an instruction device, which implements the functions specified in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable device provide for implementing the process in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0073] The above is only a preferred embodiment of the present invention, and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment, it is not intended to limit the present invention. Therefore, any simple modification, equivalent change and modification made to the above embodiment according to the technical essence of the present invention without departing from the content of the technical solution of the present invention shall fall within the scope of protection of the technical solution of the present invention.

Claims

1. A global relocalization method for a mobile robot based on 3D laser point cloud, characterized in that the steps include: Obtain a 3D point cloud map of the current environment of the mobile robot, and obtain a 3D laser point cloud obtained by a laser radar scanning the surrounding environment of the mobile robot during the driving of the mobile robot, wherein the 3D point cloud map is constructed in advance based on the 3D laser point cloud obtained by the mobile robot traversing the current environment; Using the 3D laser point cloud to generate a 2D laser point cloud and using the 3D point cloud map to generate a 2D grid map; Perform correlation scanning and matching between the 2D laser point cloud and the grid map, and obtain the best 2D estimated pose according to the matching result; Transforming the 3D laser point cloud to a vicinity of the optimal 2D estimated pose, and obtaining the optimal 3D estimated pose by registering the 3D laser point cloud with the 3D point cloud map; The currently obtained best 2D estimated pose is verified according to the registration result between the 3D laser point cloud and the 3D point cloud map. If the verification passes, the best 2D estimated pose and the best 3D estimated pose are output as the current estimated pose.

2. The global relocalization method of a mobile robot based on 3D laser point cloud according to claim 1, characterized in that: The correlative scanning and matching of the 2D laser point cloud and the grid map comprises: The 2D laser point cloud is rotated and translated so that the laser points in the 2D laser point cloud fall within the grid map; The probability that the laser point in the 2D laser point cloud hits the grid map is calculated, and the best matching pose between the current 2D laser point cloud and the grid map is searched according to the calculated probability, that is, the best 2D estimated pose is obtained.

3. The global repositioning method of a mobile robot based on 3D laser point cloud according to claim 2, characterized in that: The searching for the best matching pose between the current 2D laser point cloud and the grid map according to the calculated probability includes: Generate all possible pose solution sets on the grid map according to different resolutions; Using a branch and bound method to search for the best matching pose in the pose solution set to obtain the best 2D estimated pose; The branch and bound method obtains the pose solution set as the initial candidate pose set and uses the grid map layer to build a multi-resolution map. The candidate poses in the current candidate pose set are first branched and expanded to obtain a candidate pose subset, and then pruned in the candidate pose subset. The branch and pruning operations are recursively performed on each candidate pose subset until a termination condition is met, and finally the pose with the highest score is selected as the best matching pose, that is, the best 2D estimated pose.

4. The global repositioning method of a mobile robot based on 3D laser point cloud according to claim 3, characterized in that: In the process of using the branch and bound method, different sizes of angular steps are used on maps with different resolutions, wherein a first step size and a first angular resolution are used on a first resolution map to generate candidate poses, and a second step size and a second angular resolution are used on a second resolution map to generate candidate poses, the first resolution is smaller than the second resolution, the first step size is larger than the second step size, and the first angular resolution is larger than the second angular resolution.

5. The global relocalization method of a mobile robot based on 3D laser point cloud according to claim 1, characterized in that: After obtaining the best 2D estimated pose, it also includes scoring the best 2D pose according to the matching status of the current laser frame and the grid map, and judging the validity of the current pose estimation according to the scoring result. If the score value is greater than the preset threshold, the subsequent alignment between the 3D laser point cloud and the 3D point cloud map will continue to be performed, otherwise the current pose estimation is judged to have failed.

6. The global relocalization method of a mobile robot based on 3D laser point cloud according to claim 5, characterized in that: The best 2D pose is scored as follows: Where X represents the best estimated 2D pose, represents the score of pose X in the grid map, h represents the number of laser points that the current laser frame hits the grid map, and n represents the number of laser points that the current laser frame does not hit the grid map.

7. The global relocalization method of a mobile robot based on 3D laser point cloud according to any one of claims 1 to 6, characterized in that: The verification of the currently obtained best 2D estimated pose according to the alignment result between the 3D laser point cloud and the 3D point cloud map includes: calculating the 3D pose score according to the point cloud similarity between the 3D laser point cloud and the 3D point cloud map, and if the calculated 3D pose score is greater than a preset threshold, the verification is judged to be passed, otherwise the verification fails.

8. The global relocalization method of a mobile robot based on 3D laser point cloud according to any one of claims 1 to 6, characterized in that: The converting the 3D laser point cloud into a 2D laser point cloud comprises: Filter each frame of 3D laser point cloud, remove laser points that are not within the preset interception interval, and then project the 3D laser point cloud into a 2D laser point cloud; The 2D laser point cloud is downsampled to remove duplicate laser points to obtain the final 2D laser point cloud.

9. A mobile robot global repositioning device based on 3D laser point cloud, comprising a processor and a memory, wherein the memory is used to store a computer program, characterized in that: The processor is configured to execute the computer program to perform the method according to any one of claims 1 to 8.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed, the method according to any one of claims 1 to 8 is implemented.

Images