CN114862932B - BIM global positioning-based pose correction method and motion distortion correction method - Google Patents

Abstract

The invention relates to a pose correction method and a motion distortion correction method based on BIM global positioning. The pose correction method based on BIM global positioning comprises the steps of dividing a BIM global point cloud map into different three-dimensional grids on the space; counting the number of point clouds in each three-dimensional grid, calculating a multi-dimensional normal distribution parameter of the three-dimensional grid, and transforming the original coordinate of the real-time data of the laser radar into a BIM global point cloud map coordinate system through a transformation matrix T; calculating the probability density of the laser radar conversion points according to the multi-dimensional normal distribution parameters, and establishing an NDT registration function according to the probability density; solving the registration function; releasing a real-time positioning pose of the robot under the BIM global point cloud map; and carrying out ESKF fusion on the laser radar predicted pose and the real-time positioning pose to obtain a corrected pose. The invention utilizes the extracted BIM global point cloud map and the point cloud data of the laser radar real-time frame for registration, and eliminates the pose error calculated by a local sensor.

Description

Pose Correction Method and Motion Distortion Correction Method Based on BIM Global Positioning

technical field

The invention relates to the technical field of indoor robots, in particular to a pose correction method based on BIM global positioning and a motion distortion correction method based on BIM global positioning.

Background technique

Existing indoor robots can use the data collected by the IMU sensor to calculate the pose transformation of the robot, and use the calculated pose transformation to correct the data collected by the radar to achieve the effect of distortion correction. The estimated pose of the robot can be obtained by integrating the IMU sensor. However, during the actual operation of the robot, wheel slippage and IMU drift will cause the estimated pose to be inaccurate. If the laser motion distortion correction of the indoor robot only relies on the IMU And other sensors, lack of global observation data, can not eliminate errors caused by wheel slippage, IMU drift and other phenomena.

Contents of the invention

Based on this, it is necessary to provide a pose correction method based on BIM global positioning and a motion distortion correction method based on BIM global positioning to solve the problem of inaccurate motion distortion correction caused by errors caused by wheel slippage and IMU drift.

To achieve the above object, the present invention adopts the following technical solutions:

Based on the pose correction method of BIM global positioning, the pose correction method comprises the following steps:

Obtaining the BIM global point cloud map, and spatially dividing the BIM global point cloud map into different three-dimensional grids;

，协方差矩阵为Σ

，n表示划分的三维网格数量，k表示n的序号，x表示网格内所有扫描点的位置，T表示由激光雷达坐标系到BIM全局点云地图坐标系的变换矩阵；Count the number of point clouds in each of the three-dimensional grids, and calculate the multi-dimensional normal distribution parameters of the three-dimensional grids, that is, the point cloud mean value and covariance matrix, where the point cloud mean value is q=

, the covariance matrix is Σ

, n represents the number of divided three-dimensional grids, k represents the serial number of n , x represents the position of all scanning points in the grid, T represents the transformation matrix from the lidar coordinate system to the BIM global point cloud map coordinate system;

1] ^T ＝T[x _i ，y _i ，1] ^T ，变换矩阵为T=

，[x _i ,y _i ]为雷达实时数据在原始坐标系下的坐标，[

]经变换矩阵转移到所述BIM全局点云地图坐标系后的坐标，φ表示旋转角度，

表示BIM全局点云地图坐标系下激光雷达坐标系原点到BIM全局点云地图坐标系下x方向上的距离，

表示y方向上的距离；The original coordinates of the real-time data of the lidar are transformed into the BIM global point cloud map coordinate system by the transformation matrix T , and the coordinates located in the BIM global point cloud map are obtained, and the specific transformation is [

1] ^T ＝T[x _i , y _i , 1] ^T , the transformation matrix is T=

, [ _xi ,y _i ] are the coordinates of the real-time radar data in the original coordinate system, [

] is transferred to the coordinates of the BIM global point cloud map coordinate system through the transformation matrix, φ represents the rotation angle,

Indicates the distance from the origin of the lidar coordinate system in the BIM global point cloud map coordinate system to the x direction in the BIM global point cloud map coordinate system,

Indicates the distance in the y direction;

,根据所述概率密度建立NDT配准函数，配准函数为：S(P)=

，即转换点概率密度的累加；Calculate the probability density of the laser radar conversion point according to the multidimensional normal distribution parameter of the three-dimensional grid, and the probability density is: P(x):

, according to the probability density to establish the NDT registration function, the registration function is: S(P)=

, that is, the accumulation of the probability density of the conversion point;

Solve the registration function, find a set of transformation parameters to maximize the registration function, set the iteration threshold and the maximum number of iterations, and end the iteration when the convergence condition is reached;

Publish the real-time positioning pose of the robot under the BIM global point cloud map;

The lidar predicted pose is obtained, and the lidar predicted pose is fused with the real-time positioning pose by ESKF to obtain a corrected pose of the robot.

Further, a kinematic model of the IMU sensor is established in advance, and the predicted pose of the laser radar is estimated through the kinematic model and real-time data collected by the IMU sensor.

Further, the accurate building information model BIM is obtained, feature extraction is performed on the BIM to obtain three-dimensional space feature points, and the three-dimensional space feature points are converted into the BIM global point cloud map.

Further, the feature is component information, which includes size, spatial position, and shape.

Further, an optimization algorithm is used to solve the registration function, and the optimization algorithm includes a Newton iteration algorithm.

The present invention also includes a motion distortion correction method based on BIM global positioning, and the motion distortion correction method includes the following steps:

Obtain the pose of the robot within the laser radar single-frame scanning time, perform linear approximation and interpolation processing on the pose, and obtain a compensation transformation matrix;

Correct the coordinates under the reference coordinate system of all laser points within a single frame time through the compensation transformation matrix to obtain the corrected laser point coordinates, encapsulate the point cloud data of the laser point coordinates, and complete the correction;

It is characterized in that,

Before obtaining the compensation transformation matrix, the pose is corrected, and the pose correction method for the pose adopts the aforementioned pose correction method based on BIM global positioning.

Further, according to the horizontal angle of each point cloud in the lidar ordered point cloud, the relative time of the single point cloud relative to the lidar coordinates at the initial moment of the single frame is calculated, and the pose transformation after the linear approximation transformation is performed according to the relative time The interpolation solution is used to calculate the compensation transformation matrix of each laser point under the laser radar coordinates to obtain the compensation transformation matrix.

Further, obtaining the pose transformation after linear approximation transformation includes the following steps:

Acquire the corrected pose data, align the pose data with the time of the lidar point cloud, perform linear approximation processing on the pose transformation within the single frame point cloud time, and obtain the pose transformation after the linear approximation transformation.

Further, the reference coordinate system O ⁰ XY is established with the lidar position at the initial moment of the single frame as the origin, and the coordinate system established at the remaining moments of the single frame is the lidar coordinate system O ^t XY .

Further, the laser radar point cloud data is obtained, and the laser radar point cloud data is converted into an ordered point cloud according to the radar beam and scanning time, so as to obtain the laser radar ordered point cloud.

The technical scheme provided by the invention has the following beneficial effects:

The prior map required by the robot can be obtained through the building information model. Through the registration of the prior map and the real-time scanning data of the laser radar, the global positioning information of the robot is established to eliminate the pose error calculated by the local sensor, and the pose error calculated by the IMU is corrected. In this way, the effect of correcting lidar motion distortion with higher precision can be achieved.

Description of drawings

Fig. 1 is the flow chart of the pose correction method based on BIM global positioning among the present invention;

Fig. 2 is the flow chart of extracting the global point cloud map from the BIM model in Fig. 1;

FIG. 3 is a schematic diagram of lidar pose registration in FIG. 1;

Fig. 4 is a schematic diagram of BIM positioning data and IMU sensor frequency in Fig. 1;

Fig. 5 is a schematic diagram of global positioning correction pose in Fig. 1;

Fig. 6 is the flowchart of the motion distortion correction method based on BIM global positioning based on Fig. 1;

Fig. 7 is a schematic diagram of the structure of the reference coordinate system in Fig. 6;

FIG. 8 is a block diagram of a general flowchart of a motion distortion correction method based on BIM global positioning based on FIG. 6 .

detailed description

The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

As shown in Figure 1, the present embodiment provides a pose correction method based on BIM global positioning, and the pose correction method includes the following steps:

Obtain the BIM global point cloud map, and spatially divide the BIM global point cloud map into different 3D grids;

，协方差矩阵为Σ

；n表示划分的三维网格数量，k表示n的序号，x表示网格内所有扫描点的位置，T表示由激光雷达坐标系到BIM全局点云地图坐标系的变换矩阵；Count the number of point clouds in each 3D grid, and calculate the multidimensional normal distribution parameters of the 3D grid, that is, the point cloud mean and covariance matrix, where the point cloud mean is q=

, the covariance matrix is Σ

; n represents the number of divided three-dimensional grids, k represents the serial number of n , x represents the position of all scanning points in the grid, T represents the transformation matrix from the lidar coordinate system to the BIM global point cloud map coordinate system;

1] ^T ＝T[x _i ，y _i ，1] ^T ，变换矩阵为T=

；[x _i ,y _i ]为雷达实时数据在原始坐标系下的坐标，[

]经变换矩阵转移到所述BIM全局点云地图坐标系后的坐标，φ表示旋转角度，

表示BIM全局点云地图坐标系下激光雷达坐标系原点到BIM全局点云地图坐标系下x方向上的距离，

表示y方向上的距离；Transform the original coordinates of the real-time data of the lidar into the BIM global point cloud map coordinate system by the transformation matrix T , and obtain the coordinates located in the BIM global point cloud map, and the specific transformation is [

1] ^T ＝T[x _i , y _i , 1] ^T , the transformation matrix is T=

; [x _i , y _i ] are the coordinates of the real-time radar data in the original coordinate system, [

] is transferred to the coordinates of the BIM global point cloud map coordinate system through the transformation matrix, φ represents the rotation angle,

Indicates the distance from the origin of the lidar coordinate system in the BIM global point cloud map coordinate system to the x direction in the BIM global point cloud map coordinate system,

Indicates the distance in the y direction;

,根据概率密度建立NDT配准函数，配准函数为：S(P) =

；Calculate the probability density of the lidar conversion point according to the multidimensional normal distribution parameters of the 3D grid, and the probability density is: P (x):

, according to the probability density to establish the NDT registration function, the registration function is: S(P) =

;

Solve the registration function, find a set of transformation parameters to maximize the registration function, set the iteration threshold and the maximum number of iterations, and end the iteration when the convergence condition is reached;

Publish the real-time positioning pose of the robot under the BIM global point cloud map;

Obtain the predicted pose of the lidar, and perform ESKF fusion of the predicted pose of the lidar and the real-time positioning pose to obtain the corrected pose of the robot.

For the above pose correction method, it is mainly divided into three modules, including the BIM map extraction module, the BIM-based global positioning module, and the global positioning data-based correction and estimation pose module.

For the BIM map extraction module, the specific operation steps are as follows:

S1: Obtain an accurate building information model (BIM). BIM contains various information of buildings. The global positioning module of the robot mainly needs the geometric model in the BIM information to construct a global positioning map;

S2: export the BIM file as an IFC file;

S3: Perform feature extraction on the exported IFC file, according to the component information in the file, that is, size, spatial position, and shape. Extract feature points in three-dimensional space for the construction of walls, doors and windows, floors and slabs and other entities.

S4: Process the extracted three-dimensional space feature points and convert them into the required BIM global point cloud map.

Compared with the two-dimensional grid map, the 3D BIM global point cloud map extracted according to the above steps can provide more information in the real-time pose transformation of the robot. The process of BIM global point cloud map is shown in Figure 2.

For the BIM-based global positioning module, the principle of the BIM-based global positioning module is to register the extracted BIM global point cloud map with the point cloud data of the LiDAR real-time frame. The specific registration method is to convert the BIM global point cloud into a multi-dimensional The normal distribution of variables, and then convert the real-time lidar point cloud into the BIM global point cloud map according to the initial transformation parameters, calculate the probability density of the radar transformation points in the reference system, use the optimizer to solve the transformation parameters of the maximum probability density, and set the iteration Threshold and maximum number of iterations. Output the real-time pose of the robot under the BIM global point cloud map. The specific process is as follows:

S1: Spatially divide the BIM global point cloud map generated by the above modules into different 3D grids.

，协方差矩阵为Σ

，n为划分出的三维网格数量，k表示n的序号，x表示网格内所有扫描点的位置，T表示由激光雷达坐标系到BIM全局点云地图坐标系的变换矩阵；S2: Count the number of point clouds in each grid, and calculate the multidimensional normal distribution parameters of the grid, that is, the point cloud mean and covariance matrix. The point cloud mean is q=

, the covariance matrix is Σ

, n is the number of divided three-dimensional grids, k is the serial number of n , x is the position of all scanning points in the grid, T is the transformation matrix from the lidar coordinate system to the BIM global point cloud map coordinate system;

1] ^T ＝T[x _i ，y _i ，1] ^T ，变换矩阵为T=

，[x _i , y _i ]为雷达实时数据在原始坐标系下的坐标，[

]经变换矩阵转移到所述BIM全局点云地图坐标系后的坐标，φ表示旋转角度，

表示BIM全局点云地图坐标系下激光雷达坐标系原点到BIM全局点云地图坐标系下x方向上的距离，

表示y方向上的距离。S3: Transform the original coordinates of the radar real-time data from the transformation matrix T into the BIM global point cloud map coordinate system, the specific transformation is [

1] ^T ＝T[x _i , y _i , 1] ^T , the transformation matrix is T=

, [ _xi , y _i ] are the coordinates of the real-time radar data in the original coordinate system, [

] is transferred to the coordinates of the BIM global point cloud map coordinate system through the transformation matrix, φ represents the rotation angle,

Indicates the distance from the origin of the lidar coordinate system in the BIM global point cloud map coordinate system to the x direction in the BIM global point cloud map coordinate system,

Indicates the distance in the y direction.

,根据概率密度函数建立NDT配准函数：S(P)=

，即转换点概率密度的累加。其中，[x _i ,y _i ]为雷达实时数据在原始坐标系下的坐标，[

]为雷达数据经变换矩阵转移到BIM点云地图坐标系后的坐标。S4: Calculate the probability density of the radar conversion point according to the normal distribution parameters, the probability density is: P(x):

, establish the NDT registration function according to the probability density function: S(P)=

, which is the accumulation of the transition point probability densities. Among them, [x _i , y _i ] are the coordinates of the real-time radar data in the original coordinate system, [

] are the coordinates after the radar data is transferred to the BIM point cloud map coordinate system through the transformation matrix.

S5: Use the Newton iterative algorithm to solve the registration function, find a set of transformation parameters to maximize the registration function, set the iteration threshold and the maximum number of iterations, and end the iteration when the convergence condition is reached.

S6: Publish the real-time positioning pose of the robot under the global map at a fixed frequency.

Figure 3 is a schematic diagram of lidar registration in the global map. The pose of A is the lidar pose, and the dotted line is the real-time scanning data of the lidar. The time shown in the illustration is the registration process in progress.

For the correction of the estimated pose module based on the global positioning data, in the actual process of using the BIM positioning information to correct the IMU estimated pose, the update frequency of the BIM positioning information is much lower than the frequency of the IMU, as shown in Figure 4. Considering the frequency problem, the error state Karl is selected. Mann filter (ESKF) corrects the estimated pose, as shown in Figure 5, and the specific process is as follows:

S1: Establish the IMU sensor kinematics model.

S2: Infer the predicted pose of the lidar through the kinematic model and the real-time data collected by the IMU.

S3: ESKF fusion is performed with the robot BIM positioning information (real-time positioning pose) as the observed value and predicted pose.

S4: Correct the predicted pose.

This pose correction method can correct and optimize the estimated and predicted pose of the IMU sensor, which is conducive to better correction when the lidar motion is distorted.

As shown in FIG. 6, this embodiment also provides a motion distortion correction method based on BIM global positioning, and the motion distortion correction method includes the following steps:

Obtain the pose of the robot within the single-frame scanning time of the laser radar, perform linear approximation and interpolation processing on the pose, and obtain the compensation transformation matrix;

Correct the coordinates of all laser point reference coordinates in a single frame time through the compensation transformation matrix to obtain the corrected laser point coordinates, encapsulate the point cloud data of the laser point coordinates, and complete the correction;

Before obtaining the compensation transformation matrix, the pose is corrected, and the pose correction method for the pose adopts the aforementioned pose correction method based on BIM global positioning.

For the motion distortion correction method based on BIM global positioning, on the basis of the above-mentioned pose correction method of BIM global positioning, it also includes the point cloud distortion correction module using the estimated pose. The data of each laser point under the same laser beam is obtained by the laser The radar is collected at different times, but the original lidar data is defaulted to be collected by the lidar at the same time for all laser points, and the original data needs to be corrected. Taking Fig. 7 as an example, O ⁰ XY establishes the reference coordinate system based on the radar pose at the initial moment of the single frame, and establishes the radar coordinate system based on the radar pose at the rest of the single frame time. For example, O ^t XY is the radar coordinate system. O ^t A is the data scanned by the lidar at the time of O ^t XY pose; however, because the original point cloud data does not consider the point cloud acquisition cycle time, it is considered that the O ^t A data is collected by the lidar at the time of O ⁰ XY pose data, resulting in motion distortion. The method of correcting point cloud distortion is to transform all laser points within a single frame point cloud time into the reference coordinate system, and then package the point cloud data. The general flow chart is shown in Figure 8, and the specific process is as follows:

S1: Read the lidar point cloud data, convert the radar point cloud into an ordered point cloud according to the radar beam and scanning time.

S2: The reference coordinate system O ⁰ XY is established with the lidar position at the initial moment of each frame as the origin, and the coordinate system established with the rest of the lidar position within a frame is called the lidar coordinate system O ^t XY .

S3: Read the pose data of the corrected pose, align the pose data with the time of the radar point cloud, and perform linear approximation processing on the pose transformation within the single-frame point cloud time.

S4: Calculate the relative time of a single point cloud relative to the lidar coordinates at the initial moment of a single frame according to the horizontal angle of each point cloud in the ordered point cloud, interpolate and solve the pose transformation after linear approximation, and calculate each The compensation transformation matrix of the laser point in the lidar coordinates.

S5: For each laser point, the corrected laser point coordinates can be obtained by multiplying the compensation transformation matrix by the laser point coordinates in the reference coordinate system.

The motion distortion correction method can correct the pose error calculated by the IMU, and can correct the point cloud distortion more accurately.

The technical features of the above-mentioned embodiments can be combined arbitrarily. To make the description concise, all possible combinations of the technical features in the above-mentioned embodiments are not described. However, as long as there is no contradiction in the combination of these technical features, should be considered as within the scope of this specification.

The above-mentioned embodiments only express several implementation modes of the present invention, and the descriptions thereof are relatively specific and detailed, but should not be construed as limiting the patent scope of the invention. It should be pointed out that those skilled in the art can make several modifications and improvements without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent for the present invention should be based on the appended claims.

Claims (10)

1. A BIM global positioning-based pose correction method is used for correcting the predicted pose of an IMU sensor and is characterized by comprising the following steps:

acquiring a BIM global point cloud map, and spatially dividing the BIM global point cloud map into different three-dimensional grids; counting the number of point clouds in each three-dimensional grid, and calculating the multi-dimensional normal distribution parameters of the three-dimensional grids, namely a point cloud mean value and a covariance matrix, wherein the point cloud mean value is

Covariance matrix of ∑

n represents the number of divided three-dimensional grids, k represents the serial number of n, x represents the positions of all scanning points in the grids, T represents transposition, T represents ₁ Representing a transformation matrix from a laser radar coordinate system to a BIM global point cloud map coordinate system;

the original coordinates of the real-time data of the laser radar are transformed by the transformation matrix T ₁ And transforming to the coordinate system of the BIM global point cloud map to obtain the coordinate of the BIM global point cloud map, and specifically transforming to [ x' _i ，y′ _i ，1] ^T ＝T ₁ [x _i ，y _i ，1] ^T Transforming the matrix into

[x _i ,y _i ]Is the coordinate of the ith radar real-time data in the original coordinate system, [ x' _i ，y′ _i ]Transformed matrix T ₁ Coordinates after transferring to the coordinate system of the BIM global point cloud map, phi represents a rotation angle, t _x The distance from the origin of the laser radar coordinate system to the x direction of the abscissa of the BIM global point cloud map coordinate system under the BIM global point cloud map coordinate system is represented, t _y Representing the distance in the y-direction of the respective ordinate;

calculating the probability density of the conversion point of the laser radar according to the three-dimensional grid multi-dimensional normal distribution parameter, wherein the probability density is P (W):

establishing an NDT registration function according to the probability density, wherein the registration function is as follows:

W _i it is indicated that the (i) th switching point,

solving the registration function, searching a group of transformation parameters to maximize the registration function, setting an iteration threshold and the maximum iteration times, and ending the iteration when a convergence condition is reached;

issuing a real-time positioning pose of the robot under the BIM global point cloud map;

and acquiring the predicted pose of the laser radar, and fusing the predicted pose of the laser radar and the real-time positioning pose through an error state Kalman filter to obtain the corrected pose of the robot.

2. The BIM global positioning-based pose correction method according to claim 1, wherein an IMU sensor kinematic model is established in advance, and the lidar predicted pose is inferred through the kinematic model and IMU sensor real-time data acquisition.

3. The BIM global positioning-based pose correction method according to claim 2, characterized in that an accurate building information model BIM is obtained, features of the BIM are extracted to obtain three-dimensional space feature points, and the three-dimensional space feature points are converted into the BIM global point cloud map.

4. The BIM global positioning-based pose correction method according to claim 3, wherein the features are component information comprising size, spatial position and shape.

5. The BIM global positioning-based pose correction method according to claim 4, wherein the registration function is solved by an optimization algorithm, wherein the optimization algorithm comprises a Newton iteration algorithm.

6. The BIM global positioning-based motion distortion correction method comprises the following steps:

acquiring the pose of the robot within the single-frame scanning time of the laser radar, performing linear approximation and interpolation processing on the pose, and acquiring a compensation transformation matrix;

correcting coordinates under all laser point reference coordinate systems within a single frame time through the compensation transformation matrix to obtain corrected laser point coordinates, and packaging point cloud data of the laser point coordinates to finish correction;

it is characterized in that the preparation method is characterized in that,

before the compensation transformation matrix is obtained, correcting the pose, wherein the pose correction method adopts the pose correction method based on BIM global positioning as claimed in any one of claims 1-5.

7. The BIM global positioning-based motion distortion correction method according to claim 6, wherein the relative time of a single point cloud relative to the lidar coordinates at the single frame initial time is calculated according to the horizontal angle of each point cloud in the lidar ordered point cloud, interpolation solution is performed on the linear approximation transformed pose transformation according to the relative time, and the compensation transformation matrix of each laser point under the lidar coordinates is calculated to obtain the compensation transformation matrix.

8. The BIM global positioning-based motion distortion correction method according to claim 7, wherein the step of obtaining the pose transformation through linear approximate transformation comprises the following steps:

and acquiring the corrected pose data, aligning the pose data with the time of the laser radar point cloud, and performing linear approximation processing on pose transformation in the time of single-frame point cloud to obtain the pose transformation subjected to linear approximation transformation.

9. The BIM global positioning-based motion distortion correction method according to claim 8, wherein a reference coordinate system O is established with the lidar position at the single-frame initial time as the origin ⁰ XY, coordinate system established by positions of other moments of single frame is laser radar coordinate system O ^t XY。

10. The BIM global positioning-based motion distortion correction method according to claim 9, wherein laser radar point cloud data is obtained, and the laser radar point cloud data is converted into ordered point cloud according to radar beam and scanning time, so as to obtain the laser radar ordered point cloud.

Images