A method for synchronous positioning and mapping of mobile robots in underground spaces

Abstract

The present invention discloses a method for synchronous positioning and mapping of a mobile robot in an underground space, which belongs to the fields of mobile robot SLAM, unmanned driving, surveying and mapping, and underground engineering. It adopts a scanning method combining axial motion and circular motion to realize point cloud acquisition, construct a three-dimensional point cloud, and utilizes circular trajectory measurement IMU pre-integration to realize fine measurement of posture. It utilizes UWB base station for global correction to eliminate cumulative errors. Binocular speckle structured light is used to realize point cloud picking in a low-illumination underground environment. Inter-frame point cloud coarse registration is realized by the posture fusion of circular trajectory measurement IMU and UWB base station, and iterative optimization is realized according to the ICP iterative algorithm to improve the accuracy of point cloud registration, and obtain a global space map and robot positioning. The present invention is suitable for three-dimensional mapping and robot positioning of underground narrow and long spaces such as underground caves, large pipe galleries, underground pipe galleries, and mine tunnels, and serves application scenarios such as unmanned construction operations.

Description

Technical Field

[0001] The present invention belongs to the field of mobile robot SLAM, unmanned driving, surveying and mapping, and underground engineering, and specifically relates to a mobile robot synchronous positioning and mapping method for underground space, which can be applied to intelligent construction of pipe galleries, lanes, and pipe gallery environments, and convergence deformation inspection. Background Art

[0002] Simultaneous localization an mapping (SLAM) technology is an important issue in the field of robotics. The SLAM problem can be described as: a robot equipped with sensors moves freely in space, uses the collected sensor information to locate its own position, and incrementally builds a map based on its own positioning, thereby achieving simultaneous positioning and mapping of the robot.

[0003] The underground space has a complex structure and a harsh environment, so intelligent robots are needed to replace humans to work. Environmental perception is a prerequisite for intelligence. The environmental perception of underground space is different from that of the indoor and outdoor environment on the ground. It has the characteristics of low illumination, no GPS signal, and narrow space.

[0004] The existing underground 3D mapping is mainly used for 3D mapping of completed underground pipe corridors to detect whether the pipe corridors are convergent and deformed. The means used are ground 3D laser scanners, total stations, etc. The completed pipe corridors are open and unobstructed, without harsh dust and water mist environments, and the point clouds obtained are sparse and uneven, which cannot meet the 3D mapping of pipe corridors, lanes, pipe corridors, etc. under construction. Summary of the invention

[0005] In order to solve the above technical problems and realize mapping of narrow and cramped underground spaces, the present invention provides a method for synchronous positioning and mapping of a mobile robot in an underground space, which adopts a scanning method combining axial motion and circular motion to realize point cloud acquisition, and constructs a three-dimensional point cloud based on UWB (Ultra Wide Band), IMU (Inertial Measurement Unit), and binocular speckle structured light. IMU pre-integration is used to realize fine measurement of posture, and UWB global correction is used to eliminate cumulative errors. Binocular speckle structured light is used to realize point cloud picking in a low-illuminance underground environment. Inter-frame point cloud coarse alignment is realized through the posture fusion of IMU and UWB, and iterative optimization is realized according to the ICP (Iterative Closest Point) algorithm to improve the point cloud alignment accuracy and obtain a global space map and robot positioning.

[0006] The present invention proposes a speckle structure point cloud acquisition device, which is different from the traditional phase shift or Gray code encoding form. It does not require multiple exposures to achieve point cloud acquisition. The present invention only needs a single shot to achieve point cloud acquisition, and the structural form of the speckle structured light can be adjusted according to the target characteristics of the object to be measured to obtain a high-precision point cloud. There are horizontal and vertical structural lines in the speckle structured light, which facilitates the baseline alignment of the left and right cameras and improves the speed of speckle spot matching. The present invention has the characteristics of wide application scenarios, dense point clouds, high three-dimensional mapping accuracy, and high reliability.

[0007] In order to achieve the above object, the present invention adopts the following technical solution:

[0008] A method for synchronous positioning and mapping of a mobile robot in an underground space comprises the following steps:

[0009] Step 1: Place the underground mobile robot synchronous positioning and mapping system in the tunnel, arrange a UWB base station on the top of the entrance of the tunnel, and establish a world coordinate system;

[0010] Step 2: Initialize the control and processing module, and align the timestamps of the images captured by the circular trajectory measurement IMU, the global measurement UWB base station, the first UWB receiver, the second UWB receiver, and the speckle structured light sensor;

[0011] Step 3: Start circular motion scanning. After scanning one circle along the circular motion, the scanning base moves forward 1 meter along the track, and then starts circular motion scanning again to synchronously collect the point cloud data of the circular trajectory measurement IMU, the second UWB receiver, and the speckle structured light during the motion process;

[0012] Step 4: Perform Kalman filtering fusion on the position and attitude data of the circular trajectory measurement IMU and the second UWB receiver, fit the position and attitude of the speckle structured light sensor in the world coordinate system during the movement, and roughly align the point clouds collected at different times before and after the movement according to the fitted position and attitude to obtain a roughly aligned point cloud;

[0013] Step 5: The point cloud that has completed the rough registration is finely registered between the frame and the sub-image, and between the sub-image and the updated map through the ICP iterative algorithm. The ICP iterative algorithm solves the source point set P = {p i ,i=1,2,...N p} and the target point set Q={q i ,i=1,2,...N q}, so that the distance between point clouds is minimized; where i is the sequence number of the point cloud; N p 、N qare the maximum number of points in the source point set P and the maximum number of points in the target point set Q respectively; by calculating the rotation matrix R and the translation matrix t, the error function E(R,t) is obtained.

[0014] Beneficial effects:

[0015] 1. The present invention realizes the position and posture measurement of the sensor based on IMU and UWB, which can fill the problem that the traditional GPS navigation has no signal and cannot work in the underground space.

[0016] 2. The present invention studies the method of realizing high-precision posture measurement of the end of the moving cantilever based on Kalman filtering, and realizes multi-sensor data fusion based on multi-level Kalman filtering by combining IMU dynamic measurement and UWB static measurement, through UWB global posture correction and IMU local posture fine collection. The global posture of the total station is used to correct the IMU drift problem, and the data missing problem is supplemented by the fine collection integration of the IMU, so as to realize high-precision positioning solution, solve the positioning error problem caused by vibration during the movement, and improve the measurement accuracy.

[0017] 3. The scanning mechanism for point cloud acquisition of the present invention is a hollow mechanism. The scanning mechanism is close to the measured surface and can effectively solve the problem of underground space obstruction.

[0018] 4. The present invention adopts a movement form that combines axis movement and rotation scanning, and the point cloud obtained is denser and more uniform, with a higher degree of refinement. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A schematic diagram of a mobile robot synchronous positioning and mapping system for realizing a mobile robot synchronous positioning and mapping method for an underground space of the present invention;

[0020] Figure 2 This is a schematic diagram of the point cloud acquisition module;

[0021] Figure 3 is a coded structured light pattern;

[0022] Figure 4 A flowchart of the algorithm of the algorithm module;

[0023] Figure 5 It is a three-dimensional point cloud image of the pipeline gallery image obtained according to the present invention.

[0024] Among them, the reference numerals are: mobile robot motion guide rail 1, scanning base 2, first UWB receiver 3, second UWB receiver 4, circular trajectory measurement IMU 5, control and processing module 6, speckle structured light sensor 7, pipe gallery 8, UWB base station 9, left camera 10, right camera 11, laser light source 12, speckle mask 13, zoom lens 14. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0026] The mobile robot synchronous positioning and mapping system for realizing the mobile robot synchronous positioning and mapping method of an underground space of the present invention includes a data acquisition scanning structure, a posture measurement system, a point cloud acquisition module, a control and processing module and an algorithm module.

[0027] The data acquisition and scanning structure includes an axial motion structure and a circular motion structure. The axial motion structure includes but is not limited to a track type, a wheel type, and a quadruped type. The trajectory of the circular motion is a circle, and the circular motion structure is a hollow structure. In empty coal mine tunnels, pipe galleries and other narrow spaces, the wall can be scanned by setting up a laser scanner in the middle. However, during construction work, engineering work devices will be placed inside, and the accumulation of the devices will turn the empty space into a cramped space, which will block the linear propagation of light and thus affect the acquisition and scanning of the wall.

[0028] In order to realize wall scanning in the construction environment, an axial scanning system and a circular scanning system are designed. Since the axial motion and circular motion are independent, in order to avoid interference with each other, in mobile measurement, the axial motion stops after moving a certain distance. After ensuring that the whole is stationary for a period of time, the circular scanning motion starts and remains stationary for a few seconds, and then the next cycle of cyclic measurement is started again.

[0029] like Figure 1 As shown, the data acquisition scanning structure includes a mobile robot motion guide rail 1, a scanning base 2, a first UWB receiver 3 fixed on the scanning base 2, a second UWB receiver 4, a circular trajectory measurement IMU 5 that performs circular motion on the scanning base 2, and a UWB base station 9 fixed on the top of the tunnel 8. The UWB base station 9 on the top of the tunnel 8 constitutes a world coordinate system, the scanning base 2 constitutes a machine coordinate system, and the speckle structured light sensor 7 constitutes a sensor coordinate system.

[0030] The speckle structured light sensor 7 constitutes a point cloud acquisition module, and the acquired data is the three-dimensional position of the target in the local coordinate system of the speckle structured light sensor 7 .

[0031] Specifically, the robot motion guide rail 1 is installed at the bottom of the pipe gallery 8 to support the scanning base 2 and provide a track for the scanning base 2 to move along the axial direction of the pipe gallery 8 .

[0032] Specifically, the scanning base 2 is a ring-shaped structure, which is installed on the robot motion guide rail 1. A first UWB receiver 3 is installed at the bottom of the scanning base 2 to receive the signal of the UWB base station 9, which is used to measure the position and posture of the scanning base 2 in the UWB base station 9 fixed on the top of the tunnel 8, that is, the posture and angle of the scanning base 2 in the world coordinate system. A speckle structured light sensor 7, a circular trajectory measurement IMU 5, a control and processing module 6 and a second UWB receiver 4 are installed on the motion axis on the upper part of the scanning base 2, which together with the motion axis perform a reciprocating circular motion around the central axis of the scanning base 2.

[0033] Specifically, the second UWB receiver 4 is fixedly connected to the speckle structured light sensor 7 to measure the position and posture of the speckle structured light sensor 7 in the UWB base station 9, that is, the posture and angle of the speckle structured light sensor 7 in the world coordinate system.

[0034] Specifically, the circular trajectory measurement IMU 5 is fixedly connected to the speckle structured light sensor 7 and the second UWB receiver 4, and moves together with the speckle structured light sensor 7 moving on the circle. The circular trajectory measurement IMU 5 includes three accelerometers and three angular velocity meters, and can calculate the trajectory position and posture changes of the speckle structured light sensor 7 during the movement by integration.

[0035] Specifically, the control and processing module 6 is fixedly connected with the circular trajectory measurement IMU 5, the speckle structured light sensor 7 and the second UWB receiver 4, so as to control the movement of the scanning base 2 and process the received data from the circular trajectory measurement IMU 5, the speckle structured light sensor 7 and the second UWB receiver 4 in real time.

[0036] Specifically, the pipe gallery 8 is a target measurement object, and the position coordinates of the inner wall space points of the pipe gallery 8 in the world coordinate system need to be obtained.

[0037] Specifically, the UWB base station 9 is a measurement reference, fixed on the top of the tunnel wall, and the UWB base station 9 forms a world coordinate system. Ultimately, all point cloud coordinates must be unified into the world coordinate system formed by the UWB base station 9.

[0038] The posture measurement system realizes posture measurement, including posture measurement of the scanning base 2 and posture measurement of the speckle structured light sensor 7 .

[0039] The position and posture measurement of the scanning base 2 is achieved through a measurement unit composed of an ultra-wideband UWB base station 9 and a first UWB receiver 3 on the scanning base 2, and the position and posture of the machine coordinate system (and the coordinate system where the scanning base 2 is located) in the world coordinate system is obtained through spatial coordinate conversion.

[0040] The position and posture measurement of the speckle structured light sensor 7 is achieved through the UWB base station 9, the second UWB receiver 4, and the circular trajectory measurement IMU 5. The circular trajectory measurement IMU 5 includes a 3-axis accelerometer and a 3-axis gyroscope, which can measure the acceleration of the three axes and the angular velocity around the three axes of the sensor coordinate system fixed to the circular trajectory measurement IMU 5 during the movement.

[0041] The posture during the motion is obtained through global measurement of the UWB base station 9 and the second UWB receiver 4 and circular motion trajectory measurement IMU 5. First, the angular acceleration obtained by the gyroscope and the acceleration obtained by the acceleration are filtered and denoised. The angle measured by the gyroscope is fused with the angle measured by the UWB base station 9 and the second UWB receiver 4 through Kalman filtering to obtain a more accurate angular position. Then, the acceleration value in the sensor coordinate system after filtering is converted into the acceleration value in the world coordinate system. The gravity value is deducted from the vertical direction to obtain the motion acceleration in each direction. The angular velocity obtained by the gyroscope filter is multiplied by the circular motion diameter to obtain the linear velocity of the three circular motions. The displacement calculation formula S=S0+vt+at 2 / 2 calculates the predicted sensor motion position value, where S, S 0、 , v, a, t are the displacement value, initial displacement value, initial velocity, average acceleration within time t and motion time of the speckle structured light sensor 7 on the three axes of the world coordinate system respectively. The position of the sensor in the circular motion process is measured by the second UWB receiver 4, and each discrete measurement point is fitted into a curve to obtain the change of the motion trajectory of the sensor. So far, the position of the speckle structured light sensor 7 is obtained by two methods, and then the motion position of the speckle structured light sensor 7 calculated by the displacement calculation formula and the motion position of the speckle structured light sensor 7 calculated by the second UWB receiver 4 are combined, and the two motion position value data are fused through Kalman filtering to obtain a more accurate sensor position.

[0042] The point cloud acquisition module is an active binocular speckle structure, which can be used for measurement in underground low-light environments. The baseline distance of the speckle structured light sensor 7 can be adjusted according to the accuracy requirements of the center working distance of the measured object to meet the high-precision measurement requirements of different scenes. The special coded speckle structured light sensor 7 is characterized in that the focal length of the speckle light is adjustable. In addition to the scattered spots, the projected speckle structured light can also be Figure 3 As shown, there are also horizontal and vertical coding lines, namely the first horizontal coding line, the second horizontal coding line, the third horizontal coding line and the first vertical coding line, the second vertical coding line, and the third vertical coding line, which divide the normal plane in the projection direction into 16 regions of different sizes. The lines are used to align the images horizontally or vertically when the left and right cameras are shooting, so as to facilitate the matching of the speckle patterns in the later stage, reduce the amount of calculation, and improve the matching efficiency.

[0043] like Figure 2 The figure shows the components of the point cloud acquisition module, including a left camera 10, a right camera 11, a laser light source 12, a speckle mask 13, and a zoom lens 14. The laser light source 12, the speckle mask 13, and the zoom lens 14 constitute a speckle projector, which realizes the function of projecting speckles onto the surface of the object to be measured. The laser light source 12 provides illumination, the speckle mask 13 provides a pattern, and the zoom lens 14 can change the size of the pattern on the speckle mask 13 on the measured surface, thereby adapting to the surfaces of the objects to be measured of different sizes. The left camera 10 and the right camera 11 constitute a binocular photography system, which images the speckle projected on the object to be measured. Due to the difference in the spatial position of the camera, the speckle on the surface of the object to be measured will be imaged differently in the left and right cameras. By comparing the imaging photos of the left and right cameras, according to the object-image relationship, the different image points of the same object point in the left and right cameras are found, and the parallax is calculated, and the spatial three-dimensional position information of the object point can be calculated. The adjustable distance between the left and right cameras is to meet the measurement resolution requirements of objects of different scales. Figure 2 The arrows in indicate that the distance between the left camera 10 and the right camera 11 is adjustable.

[0044] The control and processing module 6 is connected to the speckle structured light sensor 7 via a serial port, and the control and processing module 6 controls the movement of each moving part and the data collection and time stamp alignment of each sensor. Specifically, the control and processing module 6 is an MPU.

[0045] like Figure 4 As shown, the algorithm module realizes the acceleration and angular velocity measurement of the circular trajectory measurement IMU 5, the position measurement of the first UWB receiver 3, the second UWB receiver 4 and the UWB base station 9, obtains the fused position and posture of the speckle structured light sensor 7 in the world coordinate system, transforms the point cloud data measured in the coordinate system of the speckle structured light sensor 7 through the position and posture of the speckle structured light sensor 7 in the world coordinate system, transforms the spatial position of the point cloud data into the world coordinate system, obtains a sub-graph of the point cloud, and then updates the position of the speckle structured light sensor 7, continuously updates the output, aligns the new single-frame point cloud to the point cloud data of the sub-graph, and finally obtains a global map.

[0046] A method for synchronous positioning and mapping of a mobile robot in an underground space of the present invention comprises the following steps:

[0047] Step 1: Arrange the position of the mobile robot synchronous positioning and mapping system in the underground space. Place the mobile robot synchronous positioning and mapping system in the underground space in the tunnel 8, arrange a UWB base station 9 on the top of the entrance of the tunnel 8, and establish a world coordinate system.

[0048] Step 2: Initialize the control and processing module 6, and align the timestamps of the images captured by the circular trajectory measurement IMU 5, the global measurement UWB base station 9, the first UWB receiver 3, the second UWB receiver 4, and the speckle structured light sensor 7.

[0049] Step 3, start circular motion scanning. After one round of circular motion scanning, the scanning base 2 moves forward 1 meter along the track, and starts circular motion scanning again to synchronously collect the point cloud data of the circular trajectory measurement IMU 5, the second UWB receiver 4 and the speckle structured light during the motion process. The update frequency of the circular trajectory measurement IMU 5 and the second UWB receiver 4 is set to 200 Hz, and the update frequency of the speckle structured light is 10 Hz.

[0050] Step 4: Kalman filter fusion is performed on the position and attitude data of the circular trajectory measurement IMU 5 and the second UWB receiver 4 to fit the position and attitude of the speckle structured light sensor 7 in the world coordinate system during the movement, and the point clouds collected at different times before and after the movement are roughly aligned according to the fitted position and attitude to obtain a roughly aligned point cloud.

[0051] Step 5: The ICP iterative algorithm is used to perform fine registration of frames and sub-images, and fine registration of sub-images and updated maps on the point clouds that have completed the rough registration. The ICP iterative algorithm solves the transformation between the source point set P = {pi, i = 1, 2, ... Np} and the target point set Q = {qi, i = 1, 2, ... Nq} to minimize the distance between point clouds. Where i is the sequence number of the point cloud; Np and Nq are the maximum number of points in the source point set P and the maximum number of points in the target point set Q respectively; the error function E (R, t) is obtained by calculating the rotation matrix R and the translation matrix t. The smaller the value, the better.

[0052] Specifically, step 4 includes the conversion of the spatial coordinate system, and the method is as follows:

[0053] The coordinate system conversion order of the narrow space wall coordinates is: sensor coordinate system - machine coordinate system - world coordinate system. The following is an illustration of the conversion from the sensor coordinate system to the machine coordinate system.

[0054] In the three-dimensional measurement of the pipeline corridor, there are multiple coordinate systems, such as Figure 1 As shown, the coordinate system fixedly connected to the scanning base 2 is the machine coordinate system, the coordinate system fixedly connected to the circular trajectory measurement IMU 5 is the sensor coordinate system, and the coordinate system where the UWB base station 9 is located is the world coordinate system.

[0055] According to Euler's theorem of general motion of rigid bodies, the motion of a point in an absolute coordinate system can be divided into the vector sum of the translation displacement along the base point and the rotation around the base point, that is:

[0056] ;

[0057] in, is the coordinate of the i-th point, the superscript T represents the transpose of the matrix; u i is the position before transformation, u i ' is the position after transformation.

[0058] Combined with the kinematic characteristics, when calculating the rotation matrix R, first consider the rotation around the Z axis in the sensor coordinate system (roll angle α), then the rotation around the Y axis (pitch angle β), and finally the rotation around the X axis (roll angle γ). The expression of the rotation matrix R is:

[0059] ;

[0060] Among them, R α , R β , R γ They are the rotation matrices around the Z, Y, and X axes respectively.

[0061] The pre-integration calculation motion of the circular trajectory measurement IMU 5 and the posture fusion method of the circular trajectory measurement IMU 5 and the second UWB receiver 4 for Kalman filtering are:

[0062] In a stationary state, at different rotation angles of the moving axis of the scanning base 2, the position of the speckle structured light sensor 7 in the world coordinate system and the angle value at the current moment are calibrated by the second UWB receiver 4 of global measurement, and the coordinates and angle relationship of the position at different moments in the machine coordinate system are obtained. According to the converted discrete coordinates, the spatial trajectory equation of the rotation axis at different angles is obtained by fitting the equation.

[0063] In the moving state, the circular trajectory is measured and the time-varying values ​​of IMU 5 in the sensor coordinate system are collected. The original collected signal contains a lot of random noise. It is necessary to perform mean filtering on the original collected signal to improve the signal-to-noise ratio, and integrate the filtered angular velocity to obtain the angle changes at different times.

[0064] The posture of the speckle structured light sensor on the moving axis of the scanning base 2 can be obtained by fitting the angular velocity integration of the circular trajectory measurement IMU 5 and the data of the second UWB receiver 4 measured globally. However, there are errors in the data measured by both, and the errors conform to the Gaussian distribution. In order to obtain higher-precision data, Kalman filter fitting is performed on the data from the two sources. The integral result of the circular trajectory measurement IMU 5 is used as the prediction, and the fitting result of the global measurement UWB base station 9 is used as the state measurement result.

[0065] Initial Angle Calculate the components in two directions according to the gravitational acceleration g:

[0066] ;

[0067] in, , , is the angle value at time k and time k-1, ω k ,ω k-1 is the angular velocity value at time k and time k-1, Δt is the time between time k and time k-1, W is the noise, and the acceleration a of the speckle structured light sensor in the y direction in the sensor coordinate system y , the acceleration in the z direction is a z .

[0068] After the fusion posture of the speckle structured light is obtained by fitting the angular velocity integration of the circular trajectory measurement IMU 5 and the data of the second UWB receiver 4 of the global measurement, the acceleration value of the circular trajectory measurement IMU 5 in the sensor coordinate system needs to be converted to the world coordinate system. The three-dimensional acceleration signal in motion contains the gravity acceleration information. When calculating the motion trajectory of the sensor side, the value of gravity contained in the three-dimensional acceleration information needs to be removed. The formula for solving the sensor trajectory of the speckle structured light sensor 7 according to the circular trajectory measurement IMU 5 is as follows:

[0069] ;

[0070] ;

[0071] Among them, X k , Y k , Z k represents the position of the speckle structured light sensor on each coordinate axis at time k in the world coordinate system, X k-1 , Y k-1 , Z k-1 represents the position of the speckle structured light sensor on each coordinate axis at time k-1 in the world coordinate system, v x k 、v y k 、v z k represents the speed of the speckle structured light sensor on each coordinate axis at time k in the world coordinate system, v x k-1 、v y k-1 、v z k-1 represents the speed of the speckle structured light sensor on each coordinate axis at time k-1 in the world coordinate system, a x k-1 、a y k-1 、a zk-1 represents the acceleration of the speckle structured light sensor on each coordinate axis at time k-1 in the world coordinate system, δt is the time interval for collecting data, and a x k ,ω y k ,ω z k are the x-axis acceleration, y-axis angular velocity, and z-axis angular velocity at time k, is the rotation angle around the x-axis.

[0072] Finally, the calculation result of the circular trajectory measurement IMU 5 is used as the prediction quantity, and the data measurement result of the second UWB receiver 4 of the global measurement is used as the state measurement quantity. The two types of different source data are Kalman data fused to obtain a more accurate position of the cantilever end.

[0073] Specifically, the step 5 includes:

[0074] The essence of the ICP iterative algorithm is to convert it into the optimal solution (R, t) of the error function E (R, t), and its formula is:

[0075] ;

[0076] Among them, p i and q i are the i-th point in the source point set P and the i-th point in the target point set Q. n is the total number of matching points, and “‖” represents the norm of the vector, which refers to the length in the vector space.

[0077] The ICP iterative algorithm includes the following steps:

[0078] Step 5.1 Select a point p from the source point set P i ∈P; i is the serial number;

[0079] Step 5.2 Select point q from the target point set Q i ∈Q, and find the corresponding point p in the source point set P i， p i ∈P;

[0080] Step 5.3 Calculate the rotation matrix R and the translation matrix t so that the error function E (R, t) is minimized;

[0081] Step 5.4 Apply the rotation matrix R and translation matrix t obtained in step 5.3 to point p i Transform and obtain a new source point set P'={p' i =Rp i +t, p i ∈P}; p' i is the point after transformation;

[0082] Step 5.5 Calculate p' in the target point set Q and the source point set P' i The corresponding point q i The average distance d between them is: d=∑‖q i -p' i ‖ 2 / n; where n is the number of matching point pairs;

[0083] Step 5.6 sets a threshold. When the average distance d is less than the threshold, the iteration stops. Otherwise, return to step 5.2 until the threshold condition is met.

[0084] like Figure 5 The three-dimensional point cloud image of the pipeline gallery image obtained according to the present invention is a collection of three-dimensional points in the world coordinate system, wherein the depth of gray represents the distance of the point from the central axis, the darker the color, the closer the point is to the central axis, and the blank part indicates that the point cloud is not collected at this position due to occlusion or other reasons under the current viewing angle.

Claims

1. A method for synchronous positioning and mapping of a mobile robot in an underground space, characterized in that: The steps include: Step 1: Place the mobile robot synchronous positioning and mapping system in the underground space in the tunnel, arrange a UWB base station on the top of the entrance of the tunnel, and establish a world coordinate system; Step 2: Initialize the control and processing module, and align the timestamps of the images captured by the circular trajectory measurement IMU, the global measurement UWB base station, the first UWB receiver, the second UWB receiver, and the speckle structured light sensor; Step 3: Start circular motion scanning. After scanning one circle along the circular motion, the scanning base moves forward 1 meter along the track, and then starts circular motion scanning again to synchronously collect the point cloud data of the circular trajectory measurement IMU, the second UWB receiver, and the speckle structured light during the motion process; Step 4: Kalman filter fusion is performed on the position and attitude data of the circular trajectory measurement IMU and the second UWB receiver to fit the position and attitude of the speckle structured light sensor in the world coordinate system during the movement, and the point clouds collected at different times before and after the movement are roughly aligned according to the fitted position and attitude data to obtain the roughly aligned point clouds; Step 5: The point cloud that has completed the rough registration is finely registered between the frame and the sub-image, and between the sub-image and the updated map through the ICP iterative algorithm. The ICP iterative algorithm solves the source point set P = {p i ,i=1,2,...N p } and the target point set Q={q i ,i=1,2,...N q }, so that the distance between point clouds is minimized; where i is the sequence number of the point cloud; p i is a point in the source point set; q i is a point in the target point set; N p 、N q are the maximum number of points in the source point set P and the maximum number of points in the target point set Q respectively; the error function E(R, t) is obtained by calculating the rotation matrix R and the translation matrix t; the ICP iterative algorithm is the iterative closest point algorithm.

2. A method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 1, characterized in that: In step 1, the mobile robot synchronous positioning and mapping system includes a data acquisition scanning structure, a posture measurement system, a point cloud acquisition module, a control and processing module, and an algorithm module.

3. A method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 2, characterized in that: The data acquisition scanning structure includes a mobile robot motion guide rail, a scanning base, a first UWB receiver fixed on the scanning base, a second UWB receiver, a circular trajectory measurement IMU that performs circular motion on the scanning base, and a UWB base station fixed on the top of the tunnel; the UWB base station on the top of the tunnel constitutes a world coordinate system, and the scanning base constitutes a machine coordinate system; the robot motion guide rail is installed at the bottom of the tunnel, used to support the scanning base and provide a track for the scanning base to move along the axis direction of the tunnel; a first UWB receiver is installed at the lower bottom of the scanning base, used to receive the signal of the UWB base station, and used to measure the position and posture of the scanning base in the UWB base station fixed on the top of the tunnel, that is, the posture and angle of the scanning base in the world coordinate system; a speckle structured light sensor, a circular trajectory measurement IMU, a control and processing module and a second UWB receiver are installed on the motion axis of the upper part of the scanning base, and perform a reciprocating circular motion around the central axis of the scanning base together with the motion axis on the scanning base.

4. A method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 3, characterized in that: The speckle structured light sensor constitutes a point cloud acquisition module; the speckle structured light sensor constitutes a sensor coordinate system; the position and posture measurement of the speckle structured light sensor is realized by a UWB base station, a second UWB receiver, and a circular trajectory measurement IMU; the circular trajectory measurement IMU includes a 3-axis accelerometer and a 3-axis gyroscope, which measure the acceleration of the three axes and the angular velocity around the three axes of the sensor coordinate system fixed to the circular trajectory measurement IMU during movement.

5. A method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 4, characterized in that: The position and attitude data of the motion process are obtained through the global measurement of the UWB base station and the second UWB receiver and the circular motion trajectory measurement IMU. The obtained acceleration is filtered and denoised, and the angle measured by the Kalman filter gyroscope and the angle measured by the UWB base station and the second UWB receiver are fused. Then, the acceleration value in the sensor coordinate system after filtering is converted into the acceleration value in the world coordinate system. The gravity value is deducted in the vertical direction to obtain the acceleration in each direction. The angular velocity obtained by filtering is multiplied by the circular motion diameter to obtain the linear velocity of the three circular motions. The predicted sensor motion position value is calculated by the displacement calculation formula. The position of the speckle structured light sensor in the circular motion process is measured by the second UWB receiver, and each discrete measurement point is fitted into a curve to obtain the motion trajectory change of the speckle structured light sensor, thereby obtaining the position of the speckle structured light sensor. Then, the motion position of the speckle structured light sensor calculated by the displacement calculation formula and the motion position of the speckle structured light sensor calculated by the second UWB receiver are fused through Kalman filtering to obtain a more accurate position of the speckle structured light sensor.

6. The method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 4, characterized in that: The point cloud acquisition module is an active binocular speckle structure, which is used for measuring underground low-light environments.

7. The method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 4, characterized in that: The point cloud acquisition module includes a left camera, a right camera, a laser light source, a speckle mask, a zoom lens, and the laser light source, the speckle mask, and the zoom lens constitute a speckle projector to realize the function of projecting speckles onto the surface of the object to be measured. The laser light source provides illumination, the speckle mask provides a pattern, and the zoom lens changes the size of the pattern on the speckle mask on the surface of the object to be measured, thereby adapting to surfaces of objects of different sizes. The left camera and the right camera constitute a binocular photography system to image the speckles projected on the object to be measured, and by comparing the imaging photos of the left camera and the right camera, different image points of the same object point in the left camera and the right camera are found according to the object-image relationship, and the parallax is calculated, thereby calculating the spatial three-dimensional position information of the object point.

8. The method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 4, characterized in that: The algorithm module realizes the acceleration and angular velocity measurement of the circular trajectory measurement IMU, the position measurement of the first UWB receiver, the second UWB receiver and the UWB base station, obtains the position and posture data of the speckle structured light sensor after fusion in the world coordinate system, transforms the point cloud data measured in the speckle structured light sensor coordinate system through the position and posture data of the speckle structured light sensor in the world coordinate system, transforms the spatial position of the point cloud data into the world coordinate system, obtains a sub-graph of the point cloud, and then updates the position of the speckle structured light sensor, continuously updates the output, aligns the new single-frame point cloud to the point cloud of the sub-graph, and finally obtains a global map.

9. The method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 1, characterized in that: The step 4 includes the conversion of the spatial coordinate system, and the conversion order is: the sensor coordinate system is converted to the machine coordinate system, and then converted to the world coordinate system; the method of Kalman filtering the position and attitude data of the circular trajectory measurement IMU and the second UWB receiver is: In a stationary state, at different rotation angles of the motion axis of the scanning base, the position of the speckle structured light sensor in the world coordinate system and the angle value at the current moment are calibrated by the second UWB receiver of global measurement, and the coordinates and angle relationship of the position at different moments in the machine coordinate system are obtained. According to the converted discrete coordinates, the spatial trajectory equation of the rotation axis at different angles is obtained by fitting the equation; In the motion state, the circular trajectory measurement IMU value changing over time in the sensor coordinate system is collected, the original collected signal is processed by mean filtering, and the filtered angular velocity is integrated to obtain the angle change at different times; The posture of the speckle structured light sensor on the moving axis of the scanning base is obtained by fitting the angular velocity integration of the circular trajectory measurement IMU and the data of the second UWB receiver of global measurement. The integral result of the circular trajectory measurement IMU is used as the prediction, and the fitting result of the global measurement UWB base station is used as the state measurement result. Initial Angle Calculate the components in two directions according to the gravitational acceleration g: ； in, , , is the angle value at time k and time k-1, ω k ,ω k-1 is the angular velocity value at time k and time k-1, Δt is the time between time k and time k-1, W is the noise, a y is the acceleration of the speckle structured light sensor in the y direction in the sensor coordinate system, a z is the acceleration of the speckle structured light sensor in the z direction in the sensor coordinate system; After the fusion posture of the speckle structured light is obtained by fitting the angular velocity integration of the circular trajectory measurement IMU and the data of the second UWB receiver of the global measurement, the acceleration value of the circular trajectory measurement IMU in the sensor coordinate system is converted to the world coordinate system; the formula for solving the sensor trajectory of the speckle structured light sensor according to the circular trajectory measurement IMU is as follows: ； ； Among them, X k , Y k , Z k represents the position of the speckle structured light sensor on each coordinate axis at time k in the world coordinate system, X k-1 , Y k-1 , Z k-1 represents the position of the speckle structured light sensor on each coordinate axis at time k-1 in the world coordinate system, v x k 、v y k 、v z k represents the speed of the speckle structured light sensor on each coordinate axis at time k in the world coordinate system, v x k-1 、v y k-1 、v z k-1 represents the speed of the speckle structured light sensor on each coordinate axis at time k-1 in the world coordinate system, a x k-1 、a y k-1 、a z k-1 represents the acceleration of the speckle structured light sensor on each coordinate axis at time k-1 in the world coordinate system, δt is the time interval for collecting data, and a x k ,ω y k ,ω z k is the x-axis acceleration at time k, the y-axis angular velocity at time k, and the z-axis angular velocity at time k, is the rotation angle around the x-axis; Finally, the calculation result of the circular trajectory measurement IMU is used as the prediction quantity, and the data measurement result of the second UWB receiver of the global measurement is used as the state measurement quantity. The prediction quantity and the state measurement quantity are Kalman data fused to obtain a more accurate position of the cantilever end.

10. The method for synchronous positioning and mapping of a mobile robot in an underground space according to claim 1, characterized in that: The step 5 comprises: The calculation method of the ICP iterative algorithm is to find the optimal solution (R, t) of the error function E (R, t), and its formula is: ； Among them, p i and q i are the i-th point in the source point set P and the i-th point in the target point set Q, respectively. n is the total number of matching points. "‖" represents the norm of the vector, which refers to the length in the vector space. The ICP iterative algorithm includes the following steps: Step 5.1 Select a point p from the source point set P i ∈P; i is the serial number; Step 5.2 Select point q from the target point set Q i ∈Q, and find the corresponding point p in the source point set P i， p i ∈P; Step 5.3 Calculate the rotation matrix R and the translation matrix t so that the error function E (R, t) is minimized; Step 5.4 Apply the rotation matrix R and translation matrix t obtained in step 5.3 to point p i Transform and obtain a new source point set P'={p' i =Rp i +t, p i ∈P}; p' i is the point after transformation; Step 5.5 Calculate p' in the target point set Q and the source point set P' i The corresponding point q i The average distance d between them is: d=∑‖q i -p' i ‖ 2 / n; n is the number of matching point pairs; Step 5.6 sets a threshold. When the average distance d is less than the threshold, the iteration stops. Otherwise, return to step 5.2 until the threshold condition is met.

Images