Mobile robot positioning method based on BIM

Abstract

The invention relates to the technical field of robot positioning, in particular to a BIM (Building Information Modeling)-based mobile robot positioning method, which comprises the following steps of: S1, determining an initial pose of a mobile robot in a building room at an initial moment; s2, obtaining a BIM generation type point cloud map of the building according to the three-dimensional BIM model of the building; s3, segmenting the point cloud data of the BIM generated point cloud map to obtain a plurality of voxels with the same size, recording a point cloud set in each voxel as a point cloud set X, and calculating a point cloud number mean value q and a covariance matrix C in the point cloud set X; s4, collecting an indoor LiDAR point cloud data set of the building at the current moment by using a laser sensor on the mobile robot, and carrying out downsampling processing on the LiDAR point cloud data set to obtain a downsampling point cloud set; and S5, according to the initial pose of the mobile robot, carrying out point cloud matching on the descending sampling point cloud set and the BIM generation type point cloud map through a 3D-NDT algorithm to obtain an optimal pose transformation parameter P of the descending sampling point cloud set relative to the BIM generation type point cloud map, and according to the initial pose and the optimal pose transformation parameter P, carrying out point cloud matching on the descending sampling point cloud set and the BIM generation type point cloud map. And calculating to obtain the indoor pose of the mobile robot in the building at the current moment. According to the positioning method, the mobile robot can be quickly and accurately positioned.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and particularly relates to a mobile robot positioning method based on BIM. Background Art

[0002] In recent years, with the rapid development of robot and unmanned driving technologies, numerous application scenarios have emerged, such as cleaning robots, inspection unmanned vehicles, and package delivery drones. Autonomous positioning is the basis for mobile robot mapping and navigation, and the key lies in being able to determine its specific position in a known map in a complex environment [1]. On-vehicle lidar provides the hardware basis for mobile robot mapping and positioning. For relatively simple indoor environments, 2D lidar can meet the autonomous positioning requirements of mobile robots. However, for relatively complex indoor and outdoor environments, 2D lidar cannot meet the requirements, and 3D lidar needs to be used to achieve mobile robot positioning. The number of point clouds scanned by 3D lidar is huge, and the computing power of the processor of mobile robots is limited. It is time-consuming to perform point cloud registration and positioning on its point cloud data and the real map, and it cannot meet the scene positioning requirements of mobile robots for rapid response. In addition, traditional sensor-based mobile robot positioning technologies usually rely on high-precision maps, which are usually drawn through specialized surveying and mapping technologies, and the process involves high labor costs and consumption of computing resources. In the construction industry, Building Information Modeling (BIM) can comprehensively describe environmental information and has high precision and consistency. Therefore, exploring lightweight mobile robot positioning operations and further improving the accuracy and registration speed of real-time positioning is of great practical significance for the autonomous positioning of mobile robots to adapt to different scenarios. Summary of the Invention

[0003] Aiming at the problems and deficiencies existing in the prior art, the purpose of this patent is to provide a mobile robot positioning method based on BIM.

[0004] To achieve the invention purpose, the technical solution adopted by the present invention is as follows:

[0005] A mobile robot positioning method based on BIM includes the following steps:

[0006] S1: Determine the initial pose of the mobile robot in the building interior at the initial moment;

[0007] S2: According to the 3D BIM model of the building, convert the 3D BIM model into a BIM point cloud model to obtain the BIM generative point cloud map of the building;

[0008] S3: Segment the point cloud data of the BIM generative point cloud map to obtain a plurality of voxels of the same size, and the point cloud set in each voxel is denoted as point cloud set X, X = {x1, x2, …, x n}, where n is the total number of point clouds inside the voxel, calculate the average value q of the number of point clouds in the point cloud set X and the covariance matrix C;

[0009] S4: Use the laser sensor mounted on the mobile robot to collect the LiDAR point cloud data of the building interior at the current moment, and obtain the LiDAR point cloud data set Y of the building at the current moment t , Y t = {y1, y2,...., y n} of the LiDAR point cloud data set Y t is downsampled to obtain the downsampled point cloud set Y' t ;

[0010] S5: According to the initial pose of the mobile robot in the building interior obtained in step S1, the downsampled point cloud set Y' t is point cloud matched with the BIM generative point cloud map through the 3D-NDT algorithm to obtain the best pose transformation parameter P of the downsampled point cloud set Y' t relative to the BIM generative point cloud map. According to the initial pose and the best pose transformation parameter P, calculate the pose of the mobile robot in the building interior at the current moment.

[0011] According to the above mobile robot positioning method, preferably, in step S4, the specific operation of downsampling the LiDAR point cloud data set Y t is as follows:

[0012] S41: Divide the point cloud data of the LiDAR point cloud data set Y t into n voxels of the same size, calculate the centroid of each voxel to obtain the centroid point of each voxel, and form a point cloud set N with the centroid points of all voxels;

[0013] S42: Use the nearest neighbor search method to find the original point closest to each centroid point in the point cloud set N in the LiDAR point cloud data set Y t , and delete the point cloud other than the original point in the LiDAR point cloud data set Y t to obtain the downsampled point cloud set Y' t .

[0014] According to the above mobile robot positioning method, preferably, in step S41, the centroid point of each voxel is (x g , y g , z g ), (x g , y g , z g ) can be expressed as follows:

[0015]

[0016] g = N / V

[0017] where g represents the density of the point cloud set N, x i represents the i-th value of the point cloud set N on the x-axis, y i represents the i-th value of the point cloud set N on the y-axis, z i represents the i-th value of the point cloud set N on the z-axis, N represents the number of point clouds in each voxel, and V represents the volume of the LiDAR point cloud dataset Y t of the volume.

[0018] According to the above mobile robot positioning method, preferably, the specific operation of step S5 is as follows:

[0019] S51: Transform the downsampled point cloud set Y′ t according to the initial pose transformation parameter P0 to obtain the transformed point cloud set Y″ t where the initial pose transformation parameter P0 is the initial pose of the mobile robot; project the point cloud set Y″ t onto the point cloud set X of the BIM generative point cloud map, calculate the probability distribution of each point cloud in the point cloud set Y″ t in the point cloud set X, and calculate the probability product S(p) of the point cloud set Y″ t in the point cloud set X according to the probability distribution.

[0020] S52: Use the iterative optimization algorithm (Newton iterative algorithm) to adjust the pose transformation parameter to make the probability product S(p) maximum or reach the preset number of iterations, end the iteration, and output the pose transformation parameter when the probability product S(p) is maximum or reaches the preset number of iterations, that is, the optimal pose transformation parameter P. Calculate the pose of the mobile robot in the building interior at the current moment according to the initial pose and the optimal pose transformation parameter P.

[0021] According to the above mobile robot positioning method, preferably, in step S51, the calculation formula for the probability distribution of each point cloud in the point cloud set Y″ t in the point cloud set X is as follows:

[0022]

[0023] In the formula, P(x) represents the probability distribution, x i represents the point in the point cloud set X = {x1, x2…x i …x n}, q is the mean value of the number of point clouds in the point cloud set X, and C is the covariance matrix.

[0024] According to the above mobile robot positioning method, preferably, in step S51, the point cloud set Y″ is calculated according to the probability distribution. t The calculation formula of the probability product S(p) of the probability that the laser point cloud data point in the voxel of the point cloud set Y″ in the point cloud set X is as follows:

[0025]

[0026] where K represents the k-th position point among all n data points in the voxel unit, q is the average value of the number of point clouds in the point cloud set X, C is the covariance matrix, and y′ i represents the laser point cloud data point in the voxel of the point cloud set Y″ t .

[0027] According to the above mobile robot positioning method, preferably, in step S52, the iterative optimization algorithm is the Newton iterative algorithm.

[0028] According to the above mobile robot positioning method, preferably, in step S1, the trilateration method is used to determine the initial pose of the mobile robot in the building interior at the initial moment.

[0029] According to the above mobile robot positioning method, preferably, the specific operation of using the trilateration method to determine the initial pose of the mobile robot in the building interior at the initial moment is as follows:

[0030] S11: Set n calibration columns C in the building interior i , i = 1, 2, …, n, n≥3, and the position coordinates of the center of the calibration column C i are ([[]] G x i , G y i );

[0031] S12: Use the laser sensor mounted on the mobile robot to detect the distance from the mobile robot to the center of the calibration column C i . Respectively, with the center of each calibration column C i as the center and the distance from the laser sensor detected by the laser sensor to the center of the calibration column C i as the radius to draw circles, and n circles are obtained; due to the detection error of the laser sensor, the n circles intersect at a region (as shown in Figure 4 b), which is denoted as the intersection region; use the least squares method to calculate each position coordinate point in the intersection region, and find the position coordinate point closest to the theoretical position coordinate, that is, the actual position coordinate of the mobile robot; among them, the theoretical position coordinate is the condition that the laser sensor has no detection error, and circles are drawn with the distance from the laser sensor detected by the laser sensor to the center of the calibration column C i as the radius, and the n circles intersect at a point (as shown inFigure 4 As shown in a), it is denoted as the intersection point, and the position coordinates of the intersection point are the theoretical position coordinates of the mobile robot in the global coordinate system (by calculating all the position coordinate points in the intersection area through the least squares method, the detection error of the laser sensor of the mobile robot can be reduced, making the actual position coordinates obtained by the least squares method closer to the theoretical position coordinates, and improving the accuracy and precision of the subsequent positioning of the mobile robot);

[0032] S13: According to the position coordinates of the center of the calibration column C i ( G x i , G y i ) and the actual position coordinates of the mobile robot, calculate the azimuth angle G θ i , and according to the azimuth angle G θ i , calculate the direction angle G θ of the mobile robot in the global coordinate system, and obtain the initial pose G R = G x, G y, G θ] of the mobile robot in the global coordinate system;

[0033] Among them, the calculation formula of the G θ i is as follows:

[0034]

[0035] In the formula, represents the included angle of the center of the i-th calibration column in the laser sensor coordinate system, G x represents the abscissa of the actual position coordinates of the mobile robot, G y represents the ordinate of the actual position coordinates of the mobile robot;

[0036] G The calculation formula of θ is as follows:

[0037]

[0038] In the formula, n represents the number of calibration columns arranged in the building.

[0039] According to the above mobile robot positioning method, preferably, in step S12, the calculation method of the theoretical position coordinates (i.e., the position coordinates of the intersection point) is specifically:

[0040] According to the position coordinates of the intersection point, the position coordinates of the center of the calibration column C i and the distance from the mobile robot to the calibration column C iDistance ρ from the center Ci Construct a system of equations, solve the system of equations, and obtain the position coordinates of the intersection point;

[0041] The system of equations is as follows:

[0042]

[0043] where, G x is the abscissa of the position coordinates of the intersection point, G y is the ordinate of the intersection point, and n is the number of calibration columns.

[0044] According to the above mobile robot positioning method, preferably, in step S12, the specific operation of calculating each position coordinate point in the intersection area by using the least squares method is:

[0045] According to the formula ( G x, G y) T =(A T A) -1 A T b calculate each position coordinate point in the intersection area to obtain the actual position coordinates of the mobile robot;

[0046] where,

[0047]

[0048] T is the transpose symbol, and A T represents the transpose of A.

[0049] According to the above mobile robot positioning method, preferably, the specific operation in step S2 is;

[0050] S21: Obtain data according to the actual building scene and convert it into a three-dimensional BIM model by using Revit software;

[0051] S21: Obtain the three-dimensional BIM model of the building and export the three-dimensional BIM model of the building as an IFC format file;

[0052] S22: Extract the geometric information of the building from the IFC format file of the BIM model, trim and process the extracted geometric information, and then convert the IFC format file of the BIM model into an obj format file by using IfcConvert software;

[0053] S23: Use CloudCompare software to convert the file saved as the obj format into a pcd format file, that is, obtain the BIM generative point cloud map of the building.

[0054] According to the above mobile robot positioning method, preferably, in step S21, the method for obtaining the building three-dimensional BIM model is as follows: obtaining building data according to the actual building scene, and using Revit software to convert the obtained building data into a three-dimensional BIM model.

[0055] According to the above mobile robot positioning method, preferably, in step S22, the geometric information includes geometric features of structures such as walls, floors, and ceilings.

[0056] According to the above mobile robot positioning method, preferably, in step S24, in order to adapt to the scan matching positioning of the mobile robot and ensure the accuracy and precision of positioning, translation and rotation transformations can be performed on the generated BIM generative point cloud map.

[0057] Compared with the prior art, the positive and beneficial effects achieved by the present invention are as follows:

[0058] (1) Before matching the LiDAR point cloud dataset collected by the mobile robot with the building BIM generative point cloud map in the present invention, it is necessary to perform downsampling on the LiDAR point cloud dataset collected by the mobile robot (that is, first divide the BIM generative point cloud into multiple voxels of the same size, then calculate the center of each voxel, use the center of gravity points of all voxels as a new point cloud set, and then use the nearest neighbor search method to find the original points of each center of gravity point in the new point cloud set in the LiDAR point cloud dataset, and then delete the point cloud other than the original points in the LiDAR point cloud dataset, completing the downsampling of the LiDAR point cloud dataset), and then match the downsampled LiDAR point cloud dataset with the building BIM generative point cloud map; the downsampling operation of the present invention greatly reduces the number of point clouds on the premise of retaining the characteristics of the original point cloud of the LiDAR point cloud dataset, and greatly improves the registration speed and registration accuracy of the point cloud; it solves the technical drawback that when using the 3D-NDT matching algorithm to match two point cloud sets in the prior art, when dividing the point cloud set into multiple voxels of equal size, the center of gravity of the point cloud in the voxel will be directly used as the downsampled point of the current voxel, which is prone to errors.

[0059] (2) When positioning an indoor mobile robot in an existing building, the initial pose of the mobile robot is usually not determined. Instead, the LiDAR point cloud data set obtained by the robot's laser sensor is directly matched with the building BIM-generated point cloud map. Since the initial pose of the mobile robot is unknown, it is more likely to fall into a local extreme value and cannot converge when performing point cloud registration between the LiDAR point cloud data set and the building BIM-generated point cloud map. The positioning method of the present invention places the mobile robot indoors and first obtains the initial pose of the mobile robot in the building through the trilateration method. Then, the initial pose is used as the initial pose transformation parameter when matching the LiDAR point cloud data set with the building BIM-generated point cloud map. Subsequently, the transformation parameter is adjusted through the Newton iteration algorithm to find the optimal pose transformation parameter. Then, the pose of the mobile robot at the current moment is calculated based on the initial pose and the optimal pose transformation parameter. Therefore, the positioning method of the present invention first obtains the initial pose of the mobile robot, which can avoid the situation of falling into local extreme values and unable to converge during the registration process, and improves the real-time positioning accuracy and timeliness of the mobile robot.

[0060] (3) The positioning method of the present invention can not only achieve the rapid positioning of the mobile robot in an indoor environment with a global positioning system signal, but also can achieve the precise positioning of the robot without a signal in an indoor environment lacking a global positioning system signal.

[0061] (4) The positioning method of the present invention combines the BIM-generated point cloud map and the LiDAR point cloud. And since the BIM point cloud map is derived from the modeling of the original building, it can save the mapping process necessary for traditional mobile robot positioning.

[0062] (5) Compared with the traditional mobile robot positioning method, the BIM-based mobile robot positioning method of the present invention does not require additional map drawing, and combines the trilateration algorithm to estimate the initial pose of the mobile robot, thereby reducing the dependence of the NDT scan matching on the initial pose accuracy and effectively improving the efficiency and reliability of positioning.

[0063] (6) When using the trilateration method to determine the initial pose of the mobile robot, due to the measurement error of the laser sensor, these circles do not intersect at a single point but intersect in a region. In order to minimize the error between the initial position coordinates of the mobile robot and the theoretical coordinates, the present invention uses the least squares method to calculate each position coordinate point in the intersection region and finds the position coordinate point closest to the theoretical position coordinate from the intersection region, that is, the actual position coordinates of the mobile robot. This positioning method greatly improves the accuracy of determining the initial position of the mobile robot and improves the accuracy and precision of subsequent mobile robot positioning. Description of the Drawings

[0064] Figure 1 Schematic flowchart of the BIM-based mobile robot positioning method of the present invention;

[0065] Figure 2 Schematic flowchart of using the trilateration method in the present invention to determine the initial pose of the mobile robot in the building interior at the initial moment;

[0066] Figure 3 Schematic diagram of the mobile robot in the present invention;

[0067] Figure 4 Schematic diagram of the theoretical position coordinates and the actual position coordinates in the theoretical case when using the trilateration method in the present invention to determine the initial pose of the mobile robot in the building interior at the initial moment;

[0068] Figure 5 Schematic flowchart of constructing the BIM generative point cloud map of the building according to the three-dimensional BIM model of the building in the present invention. Detailed implementation manners

[0069] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application. Apparently, the described embodiments are some, but not all, of the embodiments of the present application. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application without creative efforts shall fall within the scope of protection of the present application.

[0070] The embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0071] Embodiment 1:

[0072] A BIM-based mobile robot positioning method (as shown in Figure 1 ) includes the following steps:

[0073] S1: Determine the initial pose of the mobile robot in the building interior at the initial moment;

[0074] S2: According to the three-dimensional BIM model of the building, convert the three-dimensional BIM model into a BIM point cloud model to obtain the BIM generative point cloud map of the building;

[0075] S3: Segment the point cloud data of the BIM generative point cloud map to obtain a plurality of voxels of the same size. The point cloud set in each voxel is denoted as point cloud set X, X = {x1, x2,..., x n}, where n is the total number of point clouds in the voxel, and calculate the mean value q of the number of point clouds in point cloud set X and the covariance matrix C;

[0076] S4: Use the laser sensor mounted on the mobile robot to collect the LiDAR point cloud data of the building interior at the current moment, and obtain the LiDAR point cloud data set Y of the building at the current moment. t , Y t = {y1, y2,...., y n}}, perform downsampling on the LiDAR point cloud data set Y t to obtain the downsampled point cloud set Y'. t ;

[0077] S5: According to the initial pose of the mobile robot in the building interior obtained in step S1, perform point cloud matching on the downsampled point cloud set Y' t and the BIM generative point cloud map through the 3D-NDT algorithm to obtain the best pose transformation parameter P of the downsampled point cloud set Y' t relative to the BIM generative point cloud map. According to the initial pose and the best pose transformation parameter P, calculate the pose of the mobile robot in the building interior at the current moment.

[0078] As a preferred implementation manner, in step S1, the trilateration method is used to determine the initial pose of the mobile robot in the building interior at the initial moment. Preferably, the specific steps (as Figure 2 shown) for using the trilateration method to determine the initial pose of the mobile robot in the building interior at the initial moment are:

[0079] S11: Set n calibration columns C i (the calibration columns are preferably reflective columns) in the building interior, i = 1, 2,..., n, n ≥ 3, and the position coordinates of the center of the calibration column C i are ( G x i , G y i );

[0080] S12: Use the laser sensor mounted on the mobile robot (as Figure 3 shown) to detect the distance from the mobile robot to the center of the calibration column C i . Respectively, with the center of each calibration column C i as the center and the distance from the laser sensor detected by the laser sensor to the center of the calibration column C i as the radius, draw n circles. Due to the detection error of the laser sensor, the n circles intersect at a region, denoted as the intersection region (as Figure 4as shown in b); Using the least squares method to calculate each position coordinate point in the intersection area, find the position coordinate point closest to the theoretical position coordinate, which is the actual position coordinate of the mobile robot; where the theoretical position coordinate is obtained by assuming that there is no detection error in the laser sensor, and taking the distance from the laser sensor detected by the laser sensor to the calibration column C i as the radius to draw a circle, and the n circles obtained intersect at a point (as Figure 4 shown in a), denoted as the intersection point, and the position coordinate of the intersection point is the theoretical position coordinate of the mobile robot in the global coordinate system (by calculating all position coordinate points in the intersection area using the least squares method, the detection error of the laser sensor of the mobile robot can be reduced, making the actual position coordinate obtained by the least squares method closer to the theoretical position coordinate, and improving the accuracy and accuracy of the subsequent positioning of the mobile robot);

[0081] S13: According to the position coordinate of the center of the calibration column C i ( G x i , G y i ) and the actual position coordinate of the mobile robot, calculate the azimuth angle G θ i , and according to the azimuth angle G θ i , calculate the direction angle G θ of the mobile robot in the global coordinate system, and obtain the initial pose of the mobile robot in the global coordinate system G R = G x, G y, G θ];

[0082] Among them, the formula for calculating the G θ i is as follows:

[0083]

[0084] In the formula, represents the included angle of the center of the i-th calibration column in the laser sensor coordinate system, G x represents the abscissa of the actual position coordinate of the mobile robot, G y represents the ordinate of the actual position coordinate of the mobile robot;

[0085] G The formula for calculating θ is as follows:

[0086]

[0087] Wherein, n represents the number of calibrated columns arranged in the building.

[0088] Further, in step S12, the calculation method of the theoretical position coordinates (i.e., the intersection point position coordinates) is specifically as follows:

[0089] According to the position coordinates of the intersection point, the position coordinates of the center of the calibrated column C i and the distance from the mobile robot to the center of the calibrated column C i a system of equations is constructed, and the system of equations is solved to obtain the position coordinates of the intersection point; The system of equations is as follows:

[0090] The system of equations is as follows:

[0091]

[0092] Wherein, G x is the abscissa of the intersection point position coordinates, G y is the ordinate of the intersection point, and n is the number of calibrated columns.

[0093] In step S12, the specific operation of calculating each position coordinate point in the intersection area by using the least square method is as follows:

[0095] According to the formula ( G x, G y) T = (A T A) -1 A T b, each position coordinate point in the intersection area is calculated to obtain the actual initial position coordinates of the mobile robot;

[0096] Wherein,

[0097]

[0098] T is the transpose symbol, and A T represents the transpose of A.

[0099] As a preferred implementation manner, the specific operation steps of step S2 (as Figure 5 shown) are:

[0100] S21: Obtain data according to the actual building scene and convert it into a three-dimensional BIM model by using Revit software;

[0101] S21: Obtain the three-dimensional BIM model of the building and export the three-dimensional BIM model of the building as an IFC format file;

[0102] S22: Extract the geometric information of the building from the IFC format file of the BIM model. The geometric information includes the geometric features of structures such as walls, floors, and ceilings, and trim and process the extracted geometric information. Then, use the IfcConvert software to convert the IFC format file of the BIM model into an obj format file;

[0103] S23: Use the CloudCompare software to convert the file saved in the obj format into a pcd format file, and thus obtain the BIM-generated point cloud map of the building.

[0104] As a preferred implementation manner, in step S4, for the LiDAR point cloud dataset Y t The specific operation of performing downsampling processing is as follows:

[0105] S41: Divide the point cloud data of the LiDAR point cloud dataset Y t into n voxels of the same size, calculate the centroid of each voxel to obtain the centroid point of each voxel, and form a point cloud set N with the centroid points of all voxels. Further, in step S41, the centroid point of each voxel is (x g , y g , z g ), and (x g , y g , z g ) can be expressed as follows:

[0106]

[0107] g = N / V

[0108] where g represents the density of the point cloud set N, x i represents the i-th value of the point cloud set N on the x-axis, y i represents the i-th value of the point cloud set N on the y-axis, z i represents the i-th value of the point cloud set N on the z-axis, N represents the number of point clouds in each voxel, and V represents the volume of the LiDAR point cloud dataset Y t .

[0109] S42: Use the nearest neighbor search method to find the original point closest to each centroid point in the point cloud set N in the LiDAR point cloud dataset Y t , and delete the point clouds in the LiDAR point cloud dataset Y t except the original points to obtain a downsampled point cloud set Y' t .

[0110] As a preferred implementation manner, the specific operation of step S5 is as follows:

[0111] S51: Transform the downsampled point cloud set Y′ according to the initial pose transformation parameter P0 t to obtain the transformed point cloud set Y″ t wherein, the initial pose transformation parameter P0 is the initial pose of the mobile robot; project the point cloud set Y″ t into the point cloud set X of the BIM generative point cloud map, calculate the probability distribution of each point cloud in the point cloud set Y″ t in the point cloud set X, and calculate the probability product S(p) of the point cloud set Y″ t in the point cloud set X according to the probability distribution;

[0112] S52: Use an iterative optimization algorithm to adjust the pose transformation parameter to maximize the probability product S(p) or reach a preset number of iterations, end the iteration, and output the pose transformation parameter when the probability product S(p) is maximized or reaches the preset number of iterations, that is, obtain the optimal pose transformation parameter P. Calculate the pose of the mobile robot in the building interior at the current moment according to the initial pose and the optimal pose transformation parameter P. Among them, the iterative optimization algorithm is preferably the Newton iterative algorithm.

[0113] Further preferably, in step S51, the calculation formula for the probability distribution of each point cloud in the point cloud set Y″ t in the point cloud set X is as follows:

[0114]

[0115] wherein, P(x) represents the probability distribution, x i represents the point in the point cloud set X = {x1, x2…x i …x n}, q is the average value of the number of point clouds in the point cloud set X, and C is the covariance matrix.

[0116] In step S51, the calculation formula for the probability product S(p) of the point cloud set Y″ t in the point cloud set X is as follows:

[0117]

[0118] wherein, K represents the kth position point among all n data points in the voxel unit, q is the average value of the number of point clouds in the point cloud set X, C is the covariance matrix, and y′ i represents the laser point cloud data point in the voxel of the point cloud set Y″ t .

[0119] Finally, it should be noted that the above embodiments are only preferred embodiments of the present invention, and are not intended to limit the present invention in other forms. Any person skilled in the relevant art may make changes or modifications by using the above technical content as inspiration. These equivalent embodiments with equivalent changes. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the technical concept of the present invention still fall within the protection scope of the claims of the present invention.

Claims

1. A mobile robot positioning method based on BIM, characterized in that: The following steps are involved: S1: Determine the initial position of the mobile robot in the building at the initial moment; S2: According to the three-dimensional BIM model of the building, convert the three-dimensional BIM model into a BIM point cloud model to obtain a BIM generated point cloud map of the building; S3: Segment the point cloud data of the BIM generated point cloud map to obtain a plurality of voxels of the same size. The point cloud set in each voxel is recorded as point cloud set X, where X = {x1, x2, ..., x n }, n is the total number of point clouds in the voxel, and the mean q and covariance matrix C of the point cloud set X are calculated; S4: Using the laser sensor carried by the mobile robot to collect the LiDAR point cloud data inside the building at the current moment, to obtain the LiDAR point cloud data set Y of the building at the current moment t , Y t ={y1,y2,....,y n }, for the LiDAR point cloud dataset Y t Perform downsampling processing to obtain the downsampling point cloud Y′ t ; S5: According to the initial position of the mobile robot in the building obtained in step S1, the downsampled point cloud is gathered into Y′ t The point cloud is matched with the BIM generated point cloud map through the 3D-NDT algorithm to obtain the downsampled point cloud set Y′ t Relative to the optimal posture transformation parameter P of the BIM generated point cloud map, the posture of the mobile robot in the building interior at the current moment is calculated according to the initial posture and the optimal posture transformation parameter P.

2. The mobile robot positioning method according to claim 1, characterized in that: In step S4, the LiDAR point cloud dataset Y t The specific operations for downsampling are: S41: The LiDAR point cloud dataset Y t The point cloud data is divided into n voxels of the same size, the center of gravity of each voxel is calculated, the center of gravity of each voxel is obtained, and the center of gravity points of all voxels are combined into a point cloud set N; S42: Using the nearest neighbor search method in the LiDAR point cloud dataset Y t Find the original point closest to each centroid point in the point cloud set N, and convert the LiDAR point cloud dataset Y t The point cloud except the original point is deleted to obtain the downsampled point cloud set Y′ t .

3. The mobile robot positioning method according to claim 1 or 2, characterized in that: The specific operations of step S5 are: S51: Using the initial posture of the mobile robot in the building as the initial posture transformation parameter P0 to transform the downsampled point cloud Y′ t Transform and obtain the transformed point cloud set Y″ t ; Set the point cloud set Y″ t Project it into the point cloud set X of the BIM generated point cloud map and calculate the point cloud set Y″ t The probability distribution of each point cloud in the point cloud set X is calculated according to the probability distribution of the point cloud set Y″ t The probability product S(p) in the point cloud set X; S52: Use an iterative optimization algorithm to adjust the posture transformation parameters so that the probability product S(p) is maximized or reaches a preset number of iterations, end the iteration, and output the posture transformation parameters when the probability product S(p) is maximized or reaches a preset number of iterations, that is, the optimal posture transformation parameter P is obtained. According to the initial posture and the optimal posture transformation parameter P, the posture of the mobile robot in the building room at the current moment is calculated.

4. The mobile robot positioning method according to claim 3, characterized in that: In step S51, the point cloud set Y″ is calculated t The calculation formula for the probability distribution of each point cloud in the point cloud set X is as follows: Where P(x) represents the probability distribution, x i Represents a point cloud set X = {x1, x2…x i …x n }, q is the mean number of point clouds in the point cloud set X, and C is the covariance matrix.

5. The mobile robot positioning method according to claim 4, characterized in that: In step S51, the point cloud set Y″ is calculated according to the probability distribution t The calculation formula of the probability product S(p) in the point cloud set X is as follows: Among them, K represents the kth position point among all n data points in the voxel unit, q is the mean number of point clouds in the point cloud set X, C is the covariance matrix, and y′ i Represents point cloud set Y″ t Laser point cloud data points in voxels.

6. The mobile robot positioning method according to claim 5, characterized in that: In step S6, the iterative optimization algorithm is a Newton iterative algorithm.

7. The mobile robot positioning method according to any one of claims 1 to 6, characterized in that: In step S1, the three-sided positioning method is used to determine the initial position of the mobile robot in the building interior at the initial moment.

8. The mobile robot positioning method according to claim 7, characterized in that: The specific operation of using the three-sided positioning method to determine the initial position of the mobile robot in the building interior at the initial moment is: S11: n calibration columns C are set in the building i , i=1,2,…,n,n≥3, calibration column C i The position coordinates of the center are ( G x i , G y i ); S12: Use the laser sensor on the mobile robot to detect the mobile robot to the calibration column C i The distance between the centers is measured at each calibration column C i The center is the center of the circle, and the laser sensor detected by the laser sensor is the calibration column C i Draw a circle with the distance from the center as the radius to obtain n circles; due to the detection error of the laser sensor, the n circles intersect in an area, which is recorded as the intersection area; the least squares method is used to calculate each position coordinate point in the intersection area, and the position coordinate point closest to the theoretical position coordinate is found, that is, the actual position coordinate of the mobile robot is obtained; wherein, the theoretical position coordinate is the distance from the laser sensor to the calibration column C detected by the laser sensor under the condition that the laser sensor has no detection error i Draw a circle with the distance from the center as the radius, and the obtained n circles intersect at one point, which is recorded as the intersection point. The position coordinates of the intersection point are the theoretical position coordinates of the mobile robot in the global coordinate system; S13: According to the calibration column C i The position coordinates of the center ( G x i , G y i ) and the actual position coordinates of the mobile robot, and calculate the azimuth G θ i , according to the azimuth G θ i , calculate the orientation angle of the mobile robot in the global coordinate system G θ, get the initial position of the mobile robot in the global coordinate system G R=[ G x, G y, G θ]; Among them, the G θ i The calculation formula is as follows: In the formula, represents the angle between the center of the i-th calibration column and the laser sensor coordinate system, G x represents the horizontal coordinate of the actual position coordinate in the global coordinate system of the mobile robot, G y represents the ordinate of the actual position coordinate in the global coordinate system of the mobile robot; G The calculation formula of θ is as follows: Where n represents the number of calibration columns arranged in the building.

9. The mobile robot positioning method according to claim 8, characterized in that: In step S12, the calculation method of the theoretical position coordinates is: According to the position coordinates of the intersection point, calibration column C i Center position coordinates, move the robot to the calibration column C i Distance from center Constructing a set of equations, solving the set of equations, and obtaining the position coordinates of the intersection point; The system of equations is as follows: in, G x is the horizontal coordinate of the intersection point, G y is the ordinate of the intersection point, and n is the number of calibration columns.

10. The mobile robot positioning method according to claim 9, characterized in that: In step S12, the specific operation of calculating each position coordinate point in the intersection area using the least square method is: According to the formula ( G x, G y) T =(A T A) -1 A T b. Calculate each position coordinate point in the intersection area to obtain the actual initial position coordinates of the mobile robot; in, T is the transposition symbol, A T represents the transpose of A.

Images