An indoor mobile robot autonomous mapping and path planning method

Abstract

The present invention provides an indoor mobile robot autonomous mapping and path planning method. In this method, an explosion-proof lidar and a strap-down inertial navigation installed on the mobile robot are used to collect indoor environment information. First, the explosion-proof lidar and the strap-down inertial navigation are jointly calibrated through an external parameter calibration tool. Then, the mobile robot is remotely controlled to perform closed-loop uniform motion in the indoor environment according to a preset route. After that, the high-frequency information collected by the strap-down inertial navigation is used to correct the distortion and perform combined filtering on the point cloud data of the lidar. The unscented Kalman filter is used to fuse multi-sensor information for pose estimation. The NDT_OMP point cloud registration algorithm is used to match each frame of point cloud. The plane detection algorithm is used to fit the plane coefficients to add ground detection constraints for mapping. The A* algorithm is used to autonomously plan the global map path of the mobile robot. This method can achieve precise navigation of the mobile robot in the indoor environment, improve the operation efficiency of the indoor mobile robot, and reduce the human resource cost.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robots in indoor environments, and particularly to an autonomous mapping and path planning method for indoor mobile robots. Background Art

[0002] Mobile robots have been widely applied in the service industry, medical field, military field, etc., and have broad application prospects. However, the inventors of the present invention have found through research that many current indoor mapping algorithms are large in volume, and ghosting phenomena occur during mapping in narrow spaces or complex environments, which is not conducive to the path planning of mobile robots. Therefore, it is of great significance to innovatively develop an autonomous mapping and path planning method for mobile robots in indoor environments that can also quickly, accurately, and stably complete multi-sensor fusion in indoor environments. Summary of the Invention

[0003] In view of the technical problem that many existing indoor mapping algorithms are large in volume, and ghosting phenomena occur during mapping in narrow spaces or complex environments, which is not conducive to the path planning of mobile robots, and there is an urgent need for an autonomous mapping and path planning method for mobile robots in indoor environments that can also quickly, accurately, and stably complete multi-sensor fusion in indoor environments, the present invention provides an autonomous mapping and path planning method for indoor mobile robots, which can accurately map indoor environments and quickly, accurately, and stably complete the path planning of mobile robots with multi-sensor fusion in indoor environments.

[0004] To solve the above technical problems, the present invention adopts the following technical solutions:

[0005] An autonomous mapping and path planning method for indoor mobile robots, in which multiple sensors are used. The multiple sensors include an explosion-proof lidar and a strap-down inertial navigation system. The explosion-proof lidar and the strap-down inertial navigation system are installed on a quadruped mobile robot driven by hydraulics. The explosion-proof lidar and the strap-down inertial navigation system are electrically connected to the mobile robot, and the initial pose of the mobile robot in the indoor environment is set. The method includes the following steps:

[0006] S1. Initialization: remotely control the robot to move to the indoor initial point, and establish communication between the mobile robot and the computer;

[0007] S2. The mobile robot remains stationary at the initial position, and joint calibration is performed on the explosion-proof lidar and the strap-down inertial navigation system respectively. Specifically, coordinate system calibration between the explosion-proof lidar and the explosion-proof strap-down inertial navigation system is performed through the lidar-imu extrinsic calibration tool lidar_align.

[0008] S3. The remotely controlled mobile robot moves, starting from the initial point and ending at the initial point in the indoor environment according to the pre-set route, and performs closed-loop uniform motion;

[0009] S4. The mobile robot performs distortion correction processing on the point cloud data collected by the explosion-proof lidar through the high-frequency information collected by the strapdown inertial navigation to improve the quality of the point cloud collected by the indoor lidar, and performs combined filtering in multiple ways including voxel filtering, radius filtering, and conditional filtering on the original signal collected by the explosion-proof lidar to filter out outliers and wild points, and obtains the downsampled point cloud information;

[0010] S5. The mobile robot performs pose estimation by fusing multi-sensor information through unscented Kalman filtering to obtain the pose matrix; among them, the information collected by the strapdown inertial navigation is used as the prediction information, and the information collected by the explosion-proof lidar is used as the observation information;

[0011] S9. The remotely controlled mobile robot ends this navigation and waits for the next instruction.

[0012] S7. The mobile robot fits the plane coefficients of the point cloud data after filtering in step S4 through the plane detection algorithm, adds ground detection constraints for mapping, and improves the mapping accuracy;

[0013] S8. The mobile robot receives the target point information sent by the upper computer and performs autonomous path planning on the global map through the A* algorithm;

[0014] S9. The remotely controlled mobile robot ends this navigation and waits for the next instruction.

[0015] Furthermore, the pose matrix obtained by fusing multi-sensor information for pose estimation in step S5, and the pose matrix between the coordinate system of the explosion-proof lidar itself and the strapdown inertial navigation coordinate system are mainly composed of a rotation matrix and a translation matrix. The lidar-imu extrinsic calibration tool lidar_align is used to calibrate between the coordinate systems of the explosion-proof lidar and the explosion-proof strapdown inertial navigation.

[0016] Furthermore, the specific point cloud distortion correction method and combined filtering algorithm adopted in step S4 are as follows: First, the angular velocity information collected by the strapdown inertial navigation is used to perform distortion correction processing on the point cloud data of the lidar, then conditional filtering is performed on the point cloud data after distortion correction processing to filter out outliers, then voxel filtering is performed, and finally radius filtering processing is performed. The specific steps are as follows:

[0017] S41. First, collect the data information of the strapdown inertial navigation, where the angular velocity is expressed as:

[0018] IMU_Agu(ang_x, ang_y, ang_z)

[0019] Among them, ang_x, ang_y, and ang_z represent the components of the angular velocity in the xyz directions. The coordinates of the corrected laser point cloud are expressed as:

[0020]

[0021] Among them, represents converting the rotation angle into a quaternion. Δt = scan_period * i / n, where i = 1, 2... n, scan_period represents the scanning period of a beam of laser lines, and p(x i , y i , z i ) represents the coordinate representation of each point cloud in the current frame of the lidar coordinate system;

[0022] S42. Secondly, perform downsampling processing on the laser point cloud. Filter out the point clouds that are too close and too far away around the mobile robot through conditional filtering. Calculate a cube that can just enclose all the point clouds in the current frame through voxel filtering. According to the preset resolution, divide the cube into different small cubes. For the points in each small cube, approximate the several points in the small cube with the coordinates of the centroid in the small cube. The calculation formula of the centroid is as follows:

[0023]

[0024]

[0025]

[0026] Among them, a represents the number of point clouds in each small cube;

[0027] Radius filtering pre-sets a filtering radius and a point cloud number threshold. By traversing all the point cloud data, calculate the Euclidean distance from the central point cloud within the given radius. Within the filtering radius, if the number of points with an Euclidean distance less than the filtering radius is less than the point cloud number threshold, they are regarded as outlier points and filtered out. Conversely, if the number of points with an Euclidean distance less than the filtering radius is greater than or equal to the point cloud number threshold, they are considered not to be outlier points and will be used for subsequent mapping and positioning modules.

[0028] Furthermore, in step S5, the mobile robot performs pose estimation by fusing multi-sensor information through unscented Kalman filtering to obtain a pose matrix, which specifically includes the steps:

[0029] S51. Through the state equation and observation equation of the robot system:

[0030] X k+1= f(X k , W k )

[0031] Z k = h(X k , V k )

[0032] where X k+1 represents the relationship between the k-th frame of point cloud and the (k + 1)-th frame of point cloud, and f represents the state equation function of this non-linear system; Z k represents the observation equation of the system, and h represents the observation equation function of this non-linear system; W k and V k are the current noises of the state equation and the observation equation respectively;

[0033] S52. Obtain 2n + 1 Sigma point sets and their weights:

[0034]

[0035] where represents the mean value of the sampling points, C is the covariance matrix of the current state; λ is a scaling parameter used to reduce the total prediction error; n is the number of parameters to be estimated;

[0036] S53. Obtain the (k + 1)-step prediction value from the 2n + 1 Sigma point sets and the state equation:

[0037]

[0038] S54. According to the obtained 2n + 1 prediction results and the weight of each prediction result:

[0039]

[0040] where the subscript m represents the mean value of the (k + 1)-step prediction value, and c represents the covariance of the (k + 1)-step prediction value. After obtaining the weights, substitute them into the following formula to obtain the mean value and covariance matrix of the system state quantity at the (k + 1)-step prediction:

[0041]

[0042]

[0043] where Q represents the prediction noise matrix;

[0044] S55. According to the prediction mean and the covariance matrix P k+1k , use the UT transformation again to generate a new 2n + 1 Sigma point set:

[0045]

[0046] S56. Substitute the Sigma point set into the observation equation to obtain the predicted observation quantity at the k+1 step:

[0047]

[0048] S57. Calculate the mean and covariance matrix of the predicted observation quantity at the k+1 step based on the predicted observation quantity at the k+1 step:

[0049]

[0050]

[0051]

[0052] where R represents the observation noise matrix;

[0053] S58. Calculate the Kalman gain matrix at the k+1 step using the following formula. The Kalman gain matrix is used to adjust the proportional relationship between the predicted value and the observed value:

[0054]

[0055] S59. Update the state of the robot system and covariance C k+1k+1 :

[0056]

[0057]

[0058] where Z k+1 represents the observation quantity at the k+1 step.

[0059] Furthermore, in step S6, the NDT_OMP point cloud registration algorithm is used to match each frame of point cloud, specifically including the steps:

[0060] S61. Fit the given reference point cloud with a normal distribution and calculate the mean and covariance in each grid of the reference point cloud:

[0061]

[0062]

[0063] S62. Assume that the current pose transformation matrix is T l w, calculate the coordinates of the target point cloud in the coordinate system of the previous frame of the reference point cloud, and determine the corresponding normal distribution of the target point cloud in the reference point cloud according to the coordinates. According to the probability calculation method of the normal distribution, calculate the probability that the point satisfies the corresponding normal distribution; among them, the coordinate representation of the target point cloud transformed to the reference point cloud is X i ' = T(X i , P), X i represents the coordinates of the point cloud, represents the transformation parameters from the target point cloud to the reference point cloud, and the objective function is expressed as:

[0064] The optimization problem is usually described as a minimization problem. The Newton algorithm is used to iteratively find the parameters that minimize the function. The optimal parameters are calculated by iteratively solving the equation HΔP = -g, where g is the transposed gradient of score(P), and H is the Hessian matrix of score(P). To prevent symbol confusion, it is denoted as where q = X i '- q i .

[0065] Further, in step S7, the plane coefficients are fitted through a plane detection algorithm to add a ground detection constraint to the mapping, which specifically includes:

[0066] Assume that in the world coordinate system X w Y w Z w , the general equation of the global plane π1 is Ax + By + Cz + D = 0. A plane can be described by four parameters. The lidar coordinates are X l Y l Z l , and the transformation of the lidar in the world coordinate system is R represents the rotation matrix, and T represents the translation matrix. Then, according to the parametric equation of the global plane and the pose of the lidar, the parametric equation of the global plane in the lidar coordinate system can be solved; assume a(x a , y a , z a ) is the three-dimensional representation of a point on the global plane, and the normal vector of the plane is The parametric equation of the plane can be obtained as:

[0067]

[0068] Plane normal vector In the lidar coordinate system, it can be expressed as:

[0069]

[0070] Point a(x a , y a,z a ) is represented in the lidar coordinate system as:

[0071] a'(x' a ,y' a ,z' a ) = R a (x a ,y a ,z a ) + T

[0072] Then the expression of this global plane in the lidar coordinate system is:

[0073] x' n x + y' n y + z' n z - (x' n x' a + y' n y' a + z' n z' a ) = 0

[0074] The plane detection equation fitted by the RANSAC function is:

[0075] x d x + y d x + z d x + D d = 0

[0076] Two plane equations in the lidar coordinate system are obtained in this way. However, due to measurement errors, the coefficients of the two plane equations are different. An error equation needs to be defined to measure the degree of this difference. There are two fitted planes in the lidar coordinate system. The blue plane represents the plane obtained by transforming the globally consistent ground to the radar coordinate system, and the red plane represents the plane fitted in the lidar coordinate system. Their normal vectors correspond to the vectors of the same color. To quantitatively represent this error, a rotation matrix, R x represents rotating the normal vector of the plane of the globally consistent ground in the lidar coordinate system to the x-axis, as shown in the following formula:

[0077]

[0078] This rotation angle is α. Then apply this rotation matrix to the plane equation fitted by the RANSAC algorithm:

[0079]

[0080] This rotation angle is β. This error is represented by the included angle between the two transformed normal vectors;

[0081] α is expressed as α = arctan2(y' d, x' d ), β is expressed as

[0082] Furthermore, the specific steps for the mobile robot to autonomously plan the path for the global map through the A* algorithm in step S8 include:

[0083] S81. Determine whether the starting point or the target point is on the map, whether the starting point or the target point is the same point, and whether the starting point or the target point is an obstacle;

[0084] S82. Add the starting point S to the openlist table, where the openlist stores the grids waiting to be checked;

[0085] S83. Add the grids around the grid S to the openlist table;

[0086] S84. Delete the grid S from the openlist table and put it into the closelist table, where the closelist table is used to store the grids that no longer need to be checked;

[0087] S85. Calculate the total cost value F of each surrounding grid. It is preset that the cost of moving one grid horizontally or vertically is 10, and the cost of moving one grid diagonally is 14. The total cost value F of each grid is equal to the cost G from the starting point to this grid and the cost H from this grid to the target point. The Manhattan distance is used for calculation, ignoring the obstacle information and only calculating horizontal and vertical movements;

[0088] S86. Select the grid Q with the lowest total cost value F from the openlist table, delete it from the openlist table, and add it to the closelist table;

[0089] S87. Calculate the grids around the grid Q, excluding the obstacle grids and the grids already in the closelist table. If the grids around the grid Q are not in the openlist, add them to the openlist list, calculate the total cost value F of these newly added grids, and set their parent node to Q. If the grids around the grid Q are in the openlist, then recalculate the cost G from the starting point to this grid passing through the grid Q, and determine whether it is necessary to update the G value. If it is necessary to update the G value, then it is necessary to update the parent node information and the total cost value F of this grid;

[0090] S88. Loop and execute step S86 and step S87;

[0091] S89. When the target point is in the openlist, it means the path has been found. When there are no grid nodes in the openlist table, it means there is no path between the starting grid and the target grid.

[0092] Compared with the prior art, for the indoor mobile robot autonomous mapping and path planning method provided by the present invention, the mobile robot collects the environmental information of the indoor through the multi-sensors carried, first performs joint calibration on the explosion-proof lidar and the strapdown inertial navigation, then remotely controls the mobile robot to drive a closed-loop route indoors, and tries to maintain a constant speed during the driving process. After that, the collected indoor point cloud data is subjected to distortion correction and fusion filtering to filter out the outlier point clouds and noise point clouds. Then, the improved NDT_OMP point cloud registration algorithm is run to construct the indoor map, the loop detection is used to eliminate the cumulative error of the local mapping, the unscented Kalman filter is used for the pose estimation of the mobile robot, and the A* algorithm is used to autonomously plan the motion path. This method can speed up the operation speed of the computer, improve the mapping and path planning efficiency of the mobile robot, and reduce the human resource cost. Description of the Drawings

[0093] Figure 1 is the flow chart of the indoor mobile robot autonomous mapping and path planning method provided by the present invention.

[0094] Figure 2 is the schematic diagram of the point cloud filtering provided by the present invention.

[0095] Figure 3 is the NDT schematic diagram provided by the present invention.

[0096] Figure 4 is the conversion schematic diagram from the world coordinate system to the lidar coordinate system provided by the present invention.

[0097] Figure 5 is the schematic diagram of two fitting planes in the lidar coordinate system provided by the present invention. Detailed Embodiments

[0098] In order to make the technical means, creative features, achieved purposes and effects of the present invention easy to understand, the present invention will be further described below with reference to specific illustrations.

[0099] Please refer to Figure 1As shown in the figure, the present invention provides an indoor mobile robot autonomous mapping and path planning method. In this method, multiple sensors are adopted. The multiple sensors include an explosion-proof lidar and a strapdown inertial navigation system. The explosion-proof lidar and the strapdown inertial navigation system are installed on a hydraulic-driven quadruped mobile robot. The explosion-proof lidar and the strapdown inertial navigation system are electrically connected to the mobile robot, and the initial pose of the mobile robot in the indoor environment is set. The explosion-proof lidar can be specifically implemented by using an existing lidar with the model number LR-16FIS-C1. The horizontal angular resolution of the explosion-proof lidar is 0.09°. The explosion-proof strapdown inertial navigation system can be specifically implemented by using an existing strapdown inertial navigation system with the model number LPMS-IG1-485. The angular resolution of the strapdown inertial navigation system is 0.01°. The method includes the following steps:

[0100] S1. Initialization: The remote-controlled robot is moved to the indoor initial point, and the mobile robot establishes communication with the computer.

[0101] S2. The mobile robot remains stationary at the initial position, and the explosion-proof lidar and the strapdown inertial navigation system are respectively calibrated jointly. Specifically, the lidar-imu extrinsic calibration tool lidar_align is used to calibrate the coordinate systems between the explosion-proof lidar and the explosion-proof strapdown inertial navigation system.

[0102] S3. The remote-controlled mobile robot moves, and performs a closed-loop uniform motion in the indoor environment along a pre-set route from the initial point to the initial point.

[0103] S4. The mobile robot performs distortion correction processing on the point cloud data collected by the explosion-proof lidar through the high-frequency information collected by the strapdown inertial navigation system to improve the quality of the point cloud collected by the indoor lidar, and performs a combined filtering of multiple methods including voxel filtering, radius filtering, and conditional filtering on the original signal collected by the explosion-proof lidar to filter out outliers and wild points, and obtain the downsampled point cloud information.

[0104] S5. The mobile robot performs pose estimation by fusing multi-sensor information through unscented Kalman filtering to obtain a pose matrix. Among them, the information collected by the strapdown inertial navigation system is used as prediction information, and the information collected by the explosion-proof lidar is used as observation information.

[0105] S6. The mobile robot matches the point cloud data after distortion correction processing in step S4 through the NDT_OMP point cloud registration algorithm for each frame of point cloud to prepare for mapping.

[0106] S7. The mobile robot fits the plane coefficients of the point cloud data after filtering processing in step S4 through a plane detection algorithm to add ground detection constraints to the mapping and improve the mapping accuracy.

[0107] S8. The mobile robot receives the target point information sent by the upper computer and autonomously plans the path on the global map through the A* algorithm;

[0108] S9. The remotely controlled mobile robot ends this navigation and waits for the next instruction.

[0109] As a specific embodiment, the pose matrix obtained by fusing multi-sensor information for pose estimation in step S5, and the pose matrix between the coordinate system of the explosion-proof lidar and the strapdown inertial navigation coordinate system are mainly composed of a rotation matrix and a translation matrix. The lidar-imu extrinsic calibration tool lidar_align is used to calibrate the coordinate systems between the explosion-proof lidar and the explosion-proof strapdown inertial navigation.

[0110] As a specific embodiment, the point cloud distortion correction method and the combined filtering algorithm adopted in step S4 are specifically as follows: First, the angular velocity information collected by the strapdown inertial navigation is used to perform distortion correction processing on the point cloud data of the lidar. Then, conditional filtering is performed on the distorted point cloud data to filter out outliers. Then, voxel filtering is performed, and finally, radius filtering is performed, which specifically includes the following steps:

[0111] S41. First, collect the data information of the strapdown inertial navigation, where the angular velocity is expressed as:

[0112] IMU_Agu(ang_x,ang_y,ang_z)

[0113] where ang_x, ang_y, and ang_z represent the components of the angular velocity in the xyz directions, and the coordinates of the corrected lidar point cloud are expressed as:

[0114]

[0115] where represents converting the rotation angle into a quaternion, Δt = scan_period*i / n, i = 1, 2...n, scan_period represents the scanning period of a beam of laser lines, and p(x i ,y i ,z i ) represents the coordinate representation of each point cloud in the current frame of the lidar coordinate system;

[0116] S42. Secondly, perform downsampling processing on the lidar point cloud, and filter out the point clouds that are too close and too far away around the mobile robot through conditional filtering, such as Figure 2As shown, all the magenta point clouds in the figure will be filtered out in the conditional filtering stage; a cube that can just enclose all the point clouds of the current frame is calculated through voxel filtering. According to the preset resolution, the cube is divided into different small cubes. For the points in each small cube, the coordinates of the centroid in the small cube are used to approximate several points in the small cube. The calculation formula of the centroid is as follows:

[0117]

[0118]

[0119]

[0120] Among them, a represents the number of point clouds in each small cube;

[0121] Radius filtering presets a filtering radius and a point cloud number threshold. By traversing all the point cloud data, the Euclidean distance from the central point cloud within the given radius is calculated. Within the filtering radius, if the number of points with an Euclidean distance less than the filtering radius is less than the point cloud number threshold, they are regarded as outlier points (such as the blue points shown in Figure 2 ) and filtered out. Conversely, if the number of points with an Euclidean distance less than the filtering radius is greater than or equal to the point cloud number threshold, it is considered that they are not outlier points (such as the red points shown in Figure 2 ) and will be used for the subsequent mapping and positioning modules.

[0122] As a specific embodiment, in step S5, the mobile robot performs pose estimation by fusing multi-sensor information through unscented Kalman filtering to obtain a pose matrix, which specifically includes the following steps:

[0123] S51. Through the state equation and observation equation of the robot system:

[0124] X k+1 = f(X k , W k )

[0125] Z k = h(X k , V k )

[0126] Among them, X k+1 represents the relationship between the k-th frame of point cloud and the (k + 1)-th frame of point cloud, and f represents the state equation function of this non-linear system; Z k represents the observation equation of the system, and h represents the observation equation function of this non-linear system; W k and V k are the current noises of the state equation and the observation equation respectively;

[0127] S52. Obtain 2n + 1 Sigma point sets and their weights:

[0128]

[0129] Among them, represents the mean value of the sampling points, C is the covariance matrix of the current state; λ is a scaling ratio parameter used to reduce the total prediction error; n is the number of parameters to be estimated;

[0130] S53. Obtain the predicted value at the k+1 step from the 2n+1 Sigma point sets and the state equation:

[0131]

[0132] S54. According to the obtained 2n+1 prediction results and the weights of each prediction result:

[0133]

[0134] Among them, the subscript m represents the mean value of the predicted value at the k+1 step, and c represents the covariance of the predicted value at the k+1 step. After obtaining the weights, substitute them into the following formula to obtain the predicted mean value and covariance matrix of the system state quantity at the k+1 step:

[0135]

[0136]

[0137] Among them, Q represents the prediction noise matrix;

[0138] S55. According to the predicted mean value and the covariance matrix P k+1k , use the UT transformation again to generate a new set of 2n+1 Sigma points:

[0139]

[0140] S56. Substitute the Sigma point set into the observation equation to obtain the predicted observation quantity at the k+1 step:

[0141]

[0142] S57. According to the predicted observation quantity at the k+1 step, calculate the mean value and covariance matrix of the predicted observation quantity at the k+1 step:

[0143]

[0144]

[0145]

[0146] Among them, R represents the observation noise matrix;

[0147] S58. Calculate the Kalman gain matrix for the (k + 1)-th step, which is the pose matrix. The Kalman gain matrix is used to adjust the proportional relationship between the predicted value and the observed value, as follows:

[0148]

[0149] S59. Finally, update the state of the robot system and covariance C k+1k+1 :

[0150]

[0151]

[0152] where Z k+1 represents the observed value at the (k + 1)-th step.

[0153] As a specific embodiment, the NDT_OMP point cloud registration algorithm used in step S6 is a multi-threaded optimized version of the NDT_OMP algorithm. As shown in Figure 3 , the left side of the figure is a schematic diagram after fitting the given reference point cloud (laser scan at the previous moment) with a normal distribution. In this regard, the matching between each frame of point clouds through the NDT_OMP point cloud registration algorithm in step S6 specifically includes the following steps:

[0154] S61. Fit the given reference point cloud with a normal distribution and calculate the mean and covariance in each grid of the reference point cloud:

[0155]

[0156]

[0157] Figure 3 The blue ellipse in is the normal distribution ellipse corresponding to the Gaussian distribution, and the right side of the figure represents the target point cloud of the current scan;

[0158] S62. Assume that the current pose transformation matrix is T l w , calculate the coordinates of the target point cloud in the coordinate system of the previous frame of reference point cloud, and determine the normal distribution corresponding to the target point cloud in the reference point cloud according to these coordinates. According to the probability calculation method of the normal distribution, calculate the probability that the point satisfies the corresponding normal distribution; where the coordinates of the target point cloud transformed to the reference point cloud are expressed as X i ' = T(X i , P), X i represents the coordinates of the point cloud, represents the transformation parameters from the target point cloud to the reference point cloud, and the objective function is expressed as:

[0159] Optimization problems are usually described as minimization problems. The Newton algorithm is used to iteratively find the parameters that minimize the function. The optimal parameters are calculated by iteratively solving the equation HΔP = -g, where g is the transposed gradient of score(P) and H is the Hessian matrix of score(P). To prevent symbol confusion, it is denoted as where q = X i '-q i .

[0160] As a specific embodiment, in a flat indoor environment, the movement of the mobile robot has no large jitter. Adding a plane detection constraint can reduce the elevation error. Since the number of point clouds is huge, not all point clouds need to participate in the calculation of the plane detection algorithm. To save computing resources, the ground is detected every 10 s. For this, in step S7, the plane coefficients are fitted through the plane detection algorithm to add a ground detection constraint to the mapping, specifically including:

[0161] Assume that in the world coordinate system X w Y w Z w , the general equation of the global plane π1 is Ax + By + Cz + D = 0. A plane can be described by four parameters. The lidar coordinates are X l Y l Z l . The transformation of the lidar in the world coordinate system is R represents the rotation matrix and T represents the translation matrix. As shown in Figure 4 , then according to the parametric equation of the global plane and the pose of the lidar, the parametric equation of the global plane in the lidar coordinate system can be solved. Assume that a(x a ,y a ,z a ) is the three-dimensional representation of a point on this global plane, and the normal vector of this plane is The parametric equation of the plane can be obtained as:

[0162] x n x + y n y + z n z - (x n x a + y n y a + z n z a ) = 0

[0163] The plane normal vector can be expressed in the lidar coordinate system as:

[0164]

[0165] The point a(xa , y a , z a ) is represented in the lidar coordinate system as:

[0166] a'(x' a , y' a , z' a ) = R a (x a , y a , z a ) + T

[0167] Then the expression of this global plane in the lidar coordinate system is:

[0168] x' n x + y' n y + z' n z - (x' n x' a + y' n y' a + z' n z' a ) = 0

[0169] The plane detection equation fitted by the RANSAC function is:

[0170] x d x + y d x + z d x + D d = 0

[0171] Thus, two plane equations in the lidar coordinate system are obtained. However, due to measurement errors, the coefficients of the two plane equations are different. An error equation needs to be defined to measure the degree of this difference. As Figure 5 shown, there are two fitted planes in the lidar coordinate system. The blue plane represents the plane obtained by transforming the globally consistent ground to the lidar coordinate system, and the red plane represents the plane fitted in the lidar coordinate system. The corresponding normal vectors of each are the vectors of the same color. To quantitatively represent this error, a rotation matrix, R x is used to rotate the normal vector of the plane of the globally consistent ground in the lidar coordinate system to the x-axis, as shown in the following equation:

[0172]

[0173] This rotation angle is α. Then, this rotation matrix is applied to the plane equation fitted by the RANSAC algorithm:

[0174]

[0175] This rotation angle is β. This error is represented by the included angle between the two transformed normal vectors;

[0176] α is expressed as α = arctan2(y' d , x' d ), and β is expressed as

[0177] As a specific embodiment, the specific steps of the mobile robot autonomously planning a path for the global map through the A* algorithm in step S8 include:

[0178] S81. Determine whether the initial point or the target point is on the map, whether the initial point or the target point is the same point, and whether the initial point or the target point is an obstacle;

[0179] S82. Add the initial point S to the openlist table, and the openlist stores the grids waiting to be checked;

[0180] S83. Add the grids around the grid S to the openlist table;

[0181] S84. Delete the grid S from the openlist table and put it into the closelist table. The closelist table is used to store the grids that no longer need to be checked;

[0182] S85. Calculate the total cost value F of each surrounding grid. It is preset that the cost of moving one grid horizontally or vertically is 10, and the cost of moving one grid diagonally is 14. The total cost value F of each grid is equal to the cost G from the initial point to this grid and the cost H from this grid to the target point. The Manhattan distance is used for calculation, ignoring the obstacle information, and only calculating horizontal and vertical movements;

[0183] S86. Select the grid Q with the lowest total cost value F from the openlist table, delete it from the openlist table, and add it to the closelist table;

[0184] S87. Calculate the grids around the grid Q, without considering the obstacle grids and the grids already in the closelist table. If the grids around the grid Q are not in the openlist, add them to the openlist list, calculate the total cost value F of these newly added grids, and set their parent node to Q. If the grids around the grid Q are in the openlist, then recalculate the cost G from the initial point to this grid passing through the grid Q, and determine whether it is necessary to update the G value. If it is necessary to update the G value, then it is necessary to update the parent node information and the total cost value F of this grid;

[0185] S88. Loop and execute step S86 and step S87;

[0186] S89. When the target point exists in the open list, it indicates that the path has been found. When there is no grid node in the open list, it means that there is no path between the initial grid and the target grid.

[0187] Working principle of the present invention:

[0188] ① Multi-sensor fusion of mobile robots in indoor environments

[0189] During the process of remotely controlling the mobile robot to walk, start the explosion-proof lidar and strap-down inertial navigation to work, collect information in the indoor environment, and fuse the information of the two sensors in the way of unscented Kalman filtering. Take the data collected by the strap-down inertial navigation as the predicted quantity of the unscented Kalman algorithm, and the data collected by the lidar as the observed quantity of the unscented Kalman algorithm. Use the value measured by the lidar to correct and update the predicted quantity value to obtain a consistent interpretation of the measured target.

[0190] ② Real-time mapping and positioning of mobile robots in indoor environments

[0191] The remotely controlled mobile robot walks in the indoor environment for one circle, so that the driving path of the mobile robot forms a closed loop, and real-time data in the indoor environment is collected through the explosion-proof lidar and strap-down inertial navigation sensors carried during the walking process. Use the indoor laser point cloud preprocessing method to operate to improve the quality of the point cloud, thereby improving the mapping and positioning accuracy, and input the processing results into the improved NDT_OMP algorithm and unscented Kalman filtering module for map construction and path planning of the mobile robot in the indoor environment.

[0192] ③ Path planning of mobile robots in indoor environments

[0193] Send the target point position to the mobile robot through the upper computer. The mobile robot starts the path planning module, plans the path from the initial point to the target point, and sends a motion command to the robot to move to the target point.

[0194] Compared with the prior art, for the indoor mobile robot autonomous mapping and path planning method provided by the present invention, the mobile robot collects the environmental information in the indoor through the multi-sensors carried. First, jointly calibrate the explosion-proof lidar and strap-down inertial navigation, and then remotely control the mobile robot to drive a closed-loop route in the indoor, and try to keep a constant speed during the driving process. After that, perform distortion correction and fusion filtering processing on the collected indoor point cloud data to filter out the outlier point cloud and noise point cloud. Then run the improved NDT_OMP point cloud registration algorithm to construct the indoor map, eliminate the cumulative error of local mapping through loop detection, use unscented Kalman filtering for pose estimation of the mobile robot, and use the A* algorithm to autonomously plan the motion path. This method can speed up the operation speed of the computer, improve the mapping and path planning efficiency of the mobile robot, and reduce the human resource cost.

[0195] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical solutions of the present invention, and they should all be covered within the scope of the claims of the present invention.

Claims

1. An indoor mobile robot autonomous mapping and path planning method, characterized in that In this method, multiple sensors are adopted. The multiple sensors include an explosion-proof lidar and a strap-down inertial navigation system. The explosion-proof lidar and the strap-down inertial navigation system are installed on a hydraulic-driven quadruped mobile robot. The explosion-proof lidar and the strap-down inertial navigation system are connected to the mobile robot, and the initial pose of the mobile robot in the indoor environment is set. The method includes the following steps: S1. Initialization: remotely control the robot to move to the indoor initial point, and establish communication between the mobile robot and the computer. S2. The mobile robot remains stationary at the initial position, and joint calibration is performed on the explosion-proof lidar and the strap-down inertial navigation system respectively. Specifically, coordinate system calibration between the explosion-proof lidar and the explosion-proof strap-down inertial navigation system is carried out through the lidar-imu extrinsic calibration tool lidar_align. S3. Remotely control the mobile robot to move, and perform closed-loop uniform motion in the indoor environment along the pre-set route from the initial point to the initial point. S4. The mobile robot corrects the distortion of the point cloud data of the explosion-proof lidar through the high-frequency information collected by the strap-down inertial navigation system to improve the quality of the point cloud collected by the indoor lidar, and performs a combination of multiple filtering methods including voxel filtering, radius filtering, and conditional filtering on the original signal collected by the explosion-proof lidar to filter out outliers and wild points, and obtain the downsampled point cloud information. S5. The mobile robot performs pose estimation by fusing multi-sensor information through unscented Kalman filtering to obtain a pose matrix. Among them, the information collected by the strap-down inertial navigation system is used as prediction information, and the information collected by the explosion-proof lidar is used as observation information. S6. The mobile robot matches the point cloud data after distortion correction in step S4 through the NDT_OMP point cloud registration algorithm for each frame of point cloud to prepare for mapping. S7. The mobile robot fits the plane coefficients of the point cloud data after filtering in step S4 through a plane detection algorithm to add ground detection constraints to the mapping and improve the mapping accuracy. S8. The mobile robot receives the target point information sent by the upper computer and autonomously plans a path for the global map through the A* algorithm. S9. Remotely control the mobile robot to end this navigation and wait for the next instruction.

2. The indoor mobile robot autonomous mapping and path planning method according to claim 1, characterized in that The pose matrix obtained by fusing multi-sensor information for pose estimation in step S5, and the pose matrix between the coordinate system of the explosion-proof lidar itself and the coordinate system of the strap-down inertial navigation system are composed of a rotation matrix and a translation matrix. Coordinate system calibration between the explosion-proof lidar and the explosion-proof strap-down inertial navigation system is carried out through the lidar-imu extrinsic calibration tool lidar_align.

3. The indoor mobile robot autonomous mapping and path planning method according to claim 1, characterized in that The specific point cloud distortion correction method and combination filtering algorithm adopted in step S4 are as follows: First, the point cloud data of the lidar is corrected for distortion through the angular velocity information collected by the strap-down inertial navigation system. Then, conditional filtering is performed on the point cloud data after distortion correction to filter out outliers. Then, voxel filtering is performed, and finally, radius filtering is performed. Specifically, it includes the following steps: S41. First, collect the data information of the strap-down inertial navigation system, where the angular velocity is expressed as: IMU_Agu(ang_x,ang_y,ang_z) Among them, ang_x, ang_y, and ang_z represent the components of the angular velocity in the xyz directions, and the coordinates of the corrected laser point cloud are expressed as: Among them, represents converting the rotation angle into a quaternion, Δt = scan_period * i / n, i = 1, 2... n, scan_period represents the scanning period of a laser beam, p(x i , y i , z i ) represents the coordinate representation of each point cloud in the current frame LiDAR coordinate system; S42. Secondly, perform downsampling processing on the laser point cloud. Filter out the point clouds that are too close and too far away around the mobile robot through conditional filtering. Calculate a cube that can just include all the point clouds of the current frame through voxel filtering. According to the preset resolution, divide this cube into different small cubes. For the points in each small cube, use the coordinates of the centroid in this small cube to approximate several points in this small cube. The calculation formula of the centroid is as follows: Among them, a represents the number of point clouds in each small cube; Radius filtering presets a filtering radius and a point cloud number threshold. By traversing all the point cloud data, calculate the Euclidean distance from the central point cloud within the given radius. Within the filtering radius, if the number of points with an Euclidean distance less than the filtering radius is less than the point cloud number threshold, they are regarded as outlier points and filtered out. Conversely, if the number of points with an Euclidean distance less than the filtering radius is greater than or equal to the point cloud number threshold, it is considered not an outlier point and will be used for the subsequent mapping and positioning modules.

4. The indoor mobile robot autonomous mapping and path planning method according to claim 1, characterized in that In the step S5, the mobile robot performs pose estimation by fusing multi-sensor information through unscented Kalman filtering to obtain a pose matrix, which specifically includes the steps: S51. Through the state equation and observation equation of the robot system: X k+1 = f(X k , W k ) Z k = h(X k , V k ) Among them, X k+1 represents the relationship between the k-th frame of point cloud and the (k + 1)-th frame of point cloud, and f represents the state equation function of this non-linear system; Z k represents the observation equation of the system, and h represents the observation equation function of this non-linear system; W k and V k are respectively the current noises of the state equation and the observation equation; S52. Obtain 2n + 1 Sigma point sets and their weights: where, represents the mean of the sampling points, C is the covariance matrix of the current state; λ is a scaling parameter used to reduce the total prediction error; n is the number of parameters to be estimated; S53. Obtain the k + 1 step prediction value from the 2n + 1 Sigma point sets and the state equation: S54. According to the obtained 2n + 1 prediction results and the weights of each prediction result: Among them, the subscript m represents the mean value of the k + 1 step prediction value, and c represents the covariance of the k + 1 step prediction value. After obtaining the weights, substitute them into the following formula to obtain the mean value and covariance matrix of the system state quantity at the k + 1 step prediction: Among them, Q represents the prediction noise matrix; S55. According to the predicted mean and covariance matrix P k+1|k , use the UT transformation again to generate a new set of 2n + 1 Sigma points: S56. Substitute the Sigma point set into the observation equation to obtain the predicted observation quantity at the k + 1 step: S57. According to the predicted observation quantity at the k + 1 step, calculate the mean value and covariance matrix of the predicted observation quantity at the k + 1 step: Among them, R represents the observation noise matrix; S58. Calculate the Kalman gain matrix at the k + 1 step using the following formula. The Kalman gain matrix is used to adjust the proportional relationship between the predicted value and the observed value: S59. Update the status of the robot system and covariance C k+1|k+1 : Among them, Z k+1 represents the observation at the (k + 1)-th step.

5. The indoor mobile robot autonomous mapping and path planning method according to claim 1, wherein In the step S6, perform matching between each frame of point clouds through the NDT_OMP point cloud registration algorithm, which specifically includes the steps: S61. Fit the given reference point cloud with a normal distribution and calculate the mean value and covariance in each grid of the reference point cloud: S62. Assume that the current pose transformation matrix is T l w , calculate the coordinates of the target point cloud in the coordinate system of the previous frame of reference point cloud, and determine the corresponding normal distribution of the target point cloud in the reference point cloud according to the coordinates. According to the probability calculation method of the normal distribution, calculate the probability that the point satisfies the corresponding normal distribution; where the coordinate representation of the target point cloud transformed to the reference point cloud is X′ i = T(X i , P), X i represents the coordinates of the point cloud, represents the transformation parameter from the target point cloud to the reference point cloud, and the objective function is expressed as: Optimization problems are usually described as minimization problems. The Newton algorithm is used to iteratively find the parameters that minimize the function. The optimal parameters are calculated by iteratively solving the equation HΔP = -g, where g is the transpose gradient of score(P) and H is the Hessian matrix of score(P). To prevent confusion in notation, it will be denoted as where q = X' i -q i .

6. The indoor mobile robot autonomous mapping and path planning method according to claim 1, characterized in that, In the step S7, fit the plane coefficients through a plane detection algorithm to add a ground detection constraint for mapping, which specifically includes: Assume in the world coordinate system X w Y w Z w that the general equation of the global plane π1 is Ax + By + Cz + D = 0. A plane is described by four parameters. The lidar coordinates are X l Y l Z l . The transformation of the lidar in the world coordinate system is where R represents the rotation matrix and T represents the translation matrix. Then, according to the parametric equation of the global plane and the pose of the lidar, the parametric equation of the global plane in the lidar coordinate system can be solved. Assume a(x a , y a , z a ) is the three-dimensional representation of a point on the global plane, and the normal vector of the plane is The parametric equation of the plane can be obtained as follows: x n x + y n y + z n z - (x n x a + y n y a + z n z a ) = 0 Normal vector of the plane It can be expressed in the lidar coordinate system as follows: Point a(x a , y a , z a ) is represented in the lidar coordinate system as: a′(x′ a ,y′ a ,z′ a ) = R a (x a ,y a ,z a ) + T Then the expression of this global plane in the lidar coordinate system is: x′ n x + y′ n y + z′ n z - (x′ n x′ a + y′ n y′ a + z′ n z′ a ) = 0 The plane detection equation fitted by the RANSAC function is: x d x + y d x + z d x + D d = 0 Two plane equations in the lidar coordinate system are obtained in this way. However, due to measurement errors, the coefficients of the two plane equations are different. An error equation needs to be defined to measure the degree of this difference. There are two fitted planes in the lidar coordinate system. The blue plane represents the plane obtained by transforming the globally consistent ground into the lidar coordinate system, and the red plane represents the plane fitted in the lidar coordinate system. Their normal vectors correspond to the vectors of the same color. To quantitatively represent this error, a rotation matrix, R, needs to be constructed. x It means rotating the normal vector of the plane of the globally consistent ground in the lidar coordinate system to the x-axis, as shown in the following equation: This rotation angle is α, and then apply this rotation matrix to the plane equation fitted by the RANSAC algorithm: This rotation angle is β, and represent this error through the included angle between the two transformed normal vectors; α is expressed as α = arctan2(y′ d , x′ d ), β is expressed as 7. The indoor mobile robot autonomous mapping and path planning method according to claim 1, characterized in that In the step S8, the specific steps for the mobile robot to perform autonomous path planning on the global map through the A* algorithm include: S81. Determine whether the starting point or the target point is on the map, whether the starting point or the target point is the same point, and whether the starting point or the target point is an obstacle; S82. Add the starting point S to the openlist table, where the openlist stores the grids waiting to be checked; S83. Add the grids around grid S to the openlist table; S84. Delete grid S from the openlist table and put it into the closelist table. The closelist table is used to store the grids that no longer need to be checked; S85. Calculate the total cost value F of each surrounding grid. It is preset that the cost of moving one grid horizontally or vertically is 10, and the cost of moving one grid diagonally is 14. The total cost value F of each grid is equal to the cost G from the starting point to this grid and the cost H from this grid to the target point. The Manhattan distance is used for calculation, ignoring the obstacle information and only calculating horizontal and vertical movements; S86. Select the grid Q with the lowest total cost value F from the openlist table, delete it from the openlist table, and add it to the closelist table; S87. Calculate the grids around grid Q, without considering the obstacle grids and the grids already in the closelist table. If the grids around grid Q are not in the openlist, add them to the openlist list, calculate the total cost value F of these newly added grids, and set their parent node as Q. If the grids around grid Q are in the openlist, recalculate the cost value G from the starting point to this grid passing through grid Q, and judge whether it is necessary to update the G value. If it is necessary to update the G value, it is necessary to update the parent node information and the total cost value F of this grid; S88. Loop through steps S86 and S87; S89. When the target point is in the openlist, it means the path has been found. When there are no grid nodes in the openlist table, it means there is no path between the starting grid and the target grid.

Images