Lane 3D reconstruction system based on rotating radar and IMU

Abstract

The present invention discloses a three-dimensional reconstruction system and method of a lane based on a rotating radar and an IMU. The system includes a mobile carrier machine, a track wheel, a wheel speed meter, an inertial measurement system IMU, a rotating radar, an industrial computer and a power supply. The wheel speed meter is installed on the track wheel, the inertial measurement system IMU, the rotating radar and the industrial computer are installed on the mobile carrier machine, and the wheel speed meter, the inertial measurement system IMU and the rotating radar are electrically connected to the industrial computer respectively. The three-dimensional reconstruction method of the lane based on the above system assigns point cloud confidence and fuses point clouds based on confidence to obtain a more accurate lane scene point cloud. The lane point cloud is surface reconstructed, so that ground monitoring personnel can observe the lane environment more intuitively.

Description

Technical Field

[0001] The present invention relates to the technical field of three-dimensional space stereo modeling, and in particular to a lane three-dimensional reconstruction system and method based on a rotating radar and an IMU. Background Art

[0002] Three-dimensional spatial modeling technology is suitable for underground mine excavation in the coal mining industry, subway tunnels and other scenes. The existing three-dimensional reconstruction methods of underground environments such as tunnels use several sensors such as lidar, IMU, GPS, etc., most of which are non-online methods, and the created three-dimensional models and posture accuracy are low.

[0003] The non-online 3D reconstruction of tunnels cannot provide environmental information for unmanned or unmanned tunneling operations in coal mines. At the same time, the low-precision 3D reconstruction method will provide incorrect environmental information for tunneling, which will eventually lead to large tunnel forming errors and even accuracy that does not meet engineering requirements. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a lane three-dimensional reconstruction system and method based on a rotating radar and an IMU in view of the above-mentioned deficiencies in the prior art.

[0005] In order to solve the above technical problems, the technical solution adopted by the present invention is: a lane 3D reconstruction system based on rotating radar and IMU, the system comprising:

[0006] Mobile carrier machine, track wheels, wheel speed meter, inertial measurement system IMU, rotating radar, industrial computer and power supply;

[0007] The wheel speed meter is installed on the track wheel, and the inertial measurement system IMU, the rotating radar and the industrial computer are installed on the mobile carrier machine;

[0008] The wheel speed meter, inertial measurement system IMU, and rotating radar are electrically connected to the industrial control machine respectively;

[0009] The power supply is used to supply power to the wheel speed meter, the inertial measurement system IMU, the rotating radar and the industrial computer.

[0010] The industrial computer includes a processor and a memory; a computer program is stored in the memory, and when the computer program is executed by the processor, three-dimensional reconstruction of the lane is achieved.

[0011] The rotating radar includes a laser radar, a rotating gimbal and a rotating axis; the rotating axis is perpendicular to the ground and rotates at a constant speed; the laser radar is fixedly connected to the rotating axis, and the scanning plane of the laser radar is perpendicular to the ground; the fixedly connected laser radar and the rotating axis rotate together with the rotating gimbal.

[0012] On the other hand, the present invention also provides a method for three-dimensional reconstruction using the above-mentioned lane three-dimensional reconstruction system based on rotating radar and IMU, comprising the following steps:

[0013] Step 1: The 3D lane reconstruction system based on the rotating radar and IMU establishes a coordinate system and calibrates the rotating radar. The process is as follows:

[0014] Step 1.1: Establish the mobile carrier coordinate system T according to the mobile carrier machine E ;

[0015] Step 1.2: Establish the IMU coordinate system T according to the inertial measurement system IMU I ;

[0016] Step 1.3: Establish the rotating radar coordinate system T according to the rotating radar L ;

[0017] Step 1.4: Establish the world coordinate system T W , and initialize the coordinate system T with the moving carrier E coincide;

[0018] Step 1.5: Establish world coordinate system 2T W2 , and initialize with the IMU coordinate system T I coincide;

[0019] Step 1.6: The LiDAR collects two consecutive frames of point cloud and uses the Calidar Calibration method to obtain the calibration matrix from the LiDAR coordinate system to the gimbal coordinate system.

[0020] Step 2: Statically obtain the 3D point cloud data collected by the rotating radar, assign a confidence level to each point in the point cloud, and remove points with zero confidence level. The process is as follows:

[0021] Step 2.1: When the mobile carrier machine is stationary, a laser radar is used to obtain static two-dimensional point cloud data of the lane and the rotation angle of the rotating pan-tilt platform;

[0022] Step 2.2: According to the static two-dimensional point cloud data, the rotation angle of the pan-tilt platform and the calibration matrix obtained in step 1.6, obtain the three-dimensional point cloud data in the rotating radar coordinate system;

[0023] Step 2.3: Obtain the three-dimensional point cloud data in the mobile carrier coordinate system according to the three-dimensional point cloud data in the rotating radar coordinate system and the coordinate transformation matrix between the rotating radar coordinate system and the mobile carrier coordinate system;

[0024] Step 2.4: Test the radar accuracy at different distances, obtain the change of radar accuracy at different distances, and set the confidence for each point in the point cloud. The process is as follows:

[0025] Step 2.4.1: Get the distance from each ranging point in the lidar point cloud data to the radar;

[0026] Step 2.4.2: Test the radar accuracy at different distances and obtain the change of radar accuracy at different distances;

[0027] Step 2.4.3: According to the accuracy test results, set a confidence level in the range of 0 to 1 for each point in the point cloud. The larger the value, the higher the confidence level of the point cloud.

[0028] Step 2.5: Determine whether the confidence of the point in the point cloud is zero. If it is zero, remove the point.

[0029] Step 3: Through filtering, remove outliers, remove the mobile carrier machine body point cloud and voxelize the 3D point cloud data. The process is as follows:

[0030] Step 3.1: Use statistical filters to remove outliers from the three-dimensional point cloud data to obtain the point cloud data after removing outliers. The process is as follows:

[0031] Step 3.1.1: Traverse the point cloud and calculate the average distance between each point and its nearest k neighbor points;

[0032] Step 3.1.2: Calculate the mean μ and standard deviation σ of all average distances, then the distance threshold d max Represented as d max =μ+a×σ, where a is the proportionality coefficient;

[0033] Step 3.1.3: Traverse the point cloud again and remove points whose average distance to k neighbor points is greater than d. max point;

[0034] Step 3.2: Conditionally filter the three-dimensional point cloud data using a conditional filter to obtain point cloud data without the mobile carrier machine body. The process is as follows:

[0035] Step 3.2.1: Setting conditional filtering parameters according to the size of the mobile carrier machine and the position of the mobile carrier machine in the mobile carrier coordinate system;

[0036] Step 3.2.2: Remove all point clouds in the cuboid where the mobile carrier machine is located according to the conditional filtering parameters, and obtain three-dimensional point cloud data after removing the point cloud of the mobile carrier machine body in the mobile carrier coordinate system;

[0037] Step 3.3: Use the voxel grid filter to filter and downsample the point cloud data to obtain voxelized three-dimensional point cloud data. The process is as follows:

[0038] Step 3.3.1: Set the grid size and divide the 3D point cloud data into multiple grids, and calculate the centroid of each grid;

[0039] Step 3.3.2: Replace all points in the corresponding grid with the centroid data to obtain voxelized three-dimensional point cloud data in the moving carrier coordinate system.

[0040] Step 4: Obtain the inertial data of the IMU and the speed data of the wheel speed meter, and obtain the displacement data of the mobile carrier machine through Kalman algorithm fusion. The process is as follows:

[0041] Step 4.1: Obtain the inertial data of the inertial measurement system IMU, including the three-axis acceleration data of the inertial measurement system IMU in the IMU coordinate system Quaternion data of IMU rotation angle in world coordinate system 2

[0042] Step 4.2: According to the quaternion data of the IMU rotation angle in the world coordinate system 2, obtain the corresponding rotation matrix R I,W2 ;

[0043] Step 4.3: According to the three-axis acceleration data of the IMU in the IMU coordinate system, obtain the acceleration of the IMU in the world coordinate system 2:

[0044]

[0045] Step 4.4: Obtain the speed data of the two wheel speed meters and obtain the speed data of the mobile carrier machine in the mobile carrier coordinate system through kinematic analysis.

[0046]

[0047] in, is the x-axis speed, is the y-axis speed, is the angular velocity around the z-axis; Step 4.5: Apply the kinematic principle of rigid body plane motion and use the base point method to obtain the velocity data of the mobile carrier machine Obtain the velocity data of the IMU in the mobile carrier coordinate system Among them, the velocity in the z direction is zero;

[0048] Step 4.6: According to the velocity data of IMU in the mobile carrier coordinate system Get the velocity of the IMU in world coordinate system 2:

[0049]

[0050] Among them, R I,e The rotation matrix from the mobile carrier coordinate system to the IMU coordinate system;

[0051] Step 4.7: Use the velocity of the IMU in world coordinate system 2 and the IMU acceleration measured by the IMU as the observation values ​​of the Kalman filter to optimally estimate the system state and estimate the displacement, velocity, and acceleration of the IMU in world coordinate system 2;

[0052] Step 4.8: Based on the quaternion data of the acceleration and rotation angle of the IMU in the world coordinate system 2, obtain the pose matrix of the IMU in the world coordinate system 2, and then perform coordinate transformation to obtain the pose matrix of the mobile carrier machine in the world coordinate system.

[0053] Step 5: Align the two frames of point cloud data to obtain high-precision point cloud pose. The process is as follows:

[0054] Step 5.1: When the rotating radar starts to acquire the point cloud, the machine pose matrix of the mobile carrier in the world coordinate system is stored at the same time;

[0055] Step 5.2: Obtain the mobile carrier machine pose matrix corresponding to the current frame and the previous frame point cloud, and at the same time obtain the mobile carrier machine pose transformation matrix corresponding to the current frame and the previous frame point cloud;

[0056] Step 5.3: Use the mobile carrier machine pose transformation matrix as the initial value, perform ICP registration on the current frame and the previous frame point cloud, and obtain a more accurate mobile carrier machine pose change matrix;

[0057] Step 5.4: using the mobile carrier machine pose transformation matrix to correct the current frame mobile carrier machine pose matrix to obtain a higher precision mobile carrier machine pose matrix;

[0058] Step 5.5: According to the mobile carrier machine pose matrix obtained in step 5.4, the current frame three-dimensional point cloud data is converted from the mobile carrier coordinate system to the world coordinate system, and finally a single frame of three-dimensional point cloud data in the world coordinate system is obtained.

[0059] Step 6: Remove the working surface from the point cloud data of the historical frame;

[0060] Step 7: Perform point cloud fusion based on confidence to obtain a 3D tunnel model. The process is as follows:

[0061] Step 7.1: Calculate the index of each point in the point cloud according to the voxel where each point in the three-dimensional point cloud data in the world coordinate system is located;

[0062] Step 7.2: Put the indexes of all points of the first frame point cloud into the index container;

[0063] Step 7.3: When a new point cloud is input, determine in turn whether the index of each point in the point cloud already exists in the index container;

[0064] If it does not exist, put the index of this point into the index container, keep this point in the overall point cloud, and then continue to determine the next point;

[0065] If it exists, keep the point with greater confidence and discard the point with smaller confidence. If the confidence is the same, keep the midpoint of the two points and continue to judge the next point.

[0066] Step 7.4: After all points in the point cloud are judged, continue to wait for the next frame of point cloud input to dynamically remove the index in the index container, and only retain the index of the voxel that may be affected;

[0067] Step 7.5: Reconstruct the surface of the entire point cloud and obtain the three-dimensional reconstruction model of the tunnel online.

[0068] The beneficial effects of adopting the above technical solution are:

[0069] 1. A rotating radar is designed in the system provided by the present invention to realize the detection of three-dimensional scenes without blind spots;

[0070] 2. The method provided by the present invention obtains the three-dimensional scene point cloud statically, avoiding the point cloud distortion caused by motion;

[0071] 3. The method provided by the present invention proposes the concept of confidence, and obtains a more accurate point cloud of the tunnel scene through reasonable confidence assignment and confidence-based point cloud fusion;

[0072] 4. The method provided by the present invention uses a Kalman filter algorithm to process the mobile carrier machine acceleration and wheel speed meter acquired by the IMU and collect the mobile carrier machine speed to obtain the mobile carrier machine displacement, and combines the mobile carrier machine rotation angle acquired by the IMU to obtain the mobile carrier machine posture information;

[0073] 5. The method provided by the present invention calculates the working surface of the tunnel according to the posture of the mobile carrier machine and the accessible space of the tunnel excavated by the mobile carrier machine when the last frame of point cloud data was collected, and removes the working surface in the last frame of point cloud data by conditional filtering, thereby avoiding that the excavated tunnel wall is still retained in the tunnel point cloud;

[0074] 6. The method provided by the present invention reconstructs the surface of the tunnel point cloud, allowing ground monitoring personnel to observe the tunnel environment more intuitively.

[0075] 7. The present invention is adapted to the underground geological environment of coal mines and can realize online, efficient, safe and high-precision three-dimensional reconstruction of tunnels, which accelerates the process of intelligentization of my country's coal mines and promotes the industrialization of my country's mining industry. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1It is a structural diagram of a lane 3D reconstruction system based on a rotating radar and an IMU in an embodiment of the present invention;

[0077] Figure 2 It is a flow chart of a method for performing three-dimensional reconstruction of a lane using a three-dimensional reconstruction system of a lane based on a rotating radar and an IMU in an embodiment of the present invention;

[0078] Figure 3 A schematic diagram of establishing a coordinate system in a lane 3D reconstruction system in an embodiment of the present invention;

[0079] Figure 4 The data processing flow chart in the embodiment of the present invention. DETAILED DESCRIPTION

[0080] The specific implementation of the present invention is further described in detail below in conjunction with the accompanying drawings and examples. The following examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0081] like Figure 1 As shown, the lane 3D reconstruction system based on rotating radar and IMU in this embodiment is described as follows.

[0082] The system includes: a mobile carrier machine 7, a track wheel 1, a wheel speed meter 2, an inertial measurement system IMU 4, a rotating radar 5, an industrial computer 6 and a power supply 3;

[0083] The wheel speed meter 2 is installed on the track wheel 1, and the inertial measurement system IMU 4, the rotating radar 5 and the industrial computer 6 are installed on the mobile carrier machine 7;

[0084] The wheel speed meter 2, the inertial measurement system IMU 4, and the rotation radar 5 are electrically connected to the industrial computer 6 respectively;

[0085] The power supply is used to supply power to the wheel speed meter 2 , the inertial measurement system IMU 4 , the rotating radar 5 and the industrial computer 6 .

[0086] The industrial computer 6 includes a processor and a memory; a computer program is stored in the memory, and when the computer program is executed by the processor, three-dimensional reconstruction of the lane is achieved.

[0087] The rotating radar 5 includes a laser radar, a rotating gimbal and a rotating axis; the rotating axis is perpendicular to the ground and rotates at a constant speed; the laser radar is fixedly connected to the rotating axis, and the scanning plane of the laser radar is perpendicular to the ground; the fixedly connected laser radar and the rotating axis rotate together with the rotating gimbal.

[0088] In this embodiment, the mobile carrier machine 7 is a tunneling machine, and two wheel speed meters 2 are symmetrically installed on the crawler wheels 1 on both sides of the tunneling machine.

[0089] The wheel speed meter 2 is used to collect the speed data of the track wheel 1 and send the speed data to the memory of the industrial computer 6;

[0090] The inertial measurement system IMU4 is used to collect inertial data of the mobile carrier machine and send the inertial data to the memory of the industrial computer 6;

[0091] The rotating radar 5 is used to collect three-dimensional point cloud data and send the three-dimensional point cloud data to the memory of the industrial computer 6;

[0092] The memory receives the collected data and stores the data. When the processor is working, it calls the computer program and data in the memory at the same time and processes the data to obtain a three-dimensional reconstruction model of the lane.

[0093] On the other hand, the present invention also provides a method for three-dimensional reconstruction using the above-mentioned lane three-dimensional reconstruction system based on rotating radar and IMU, the process of which is as follows: Figure 2 As shown, the following steps are included:

[0094] Step 1: The 3D lane reconstruction system based on the rotating radar and IMU establishes a coordinate system and calibrates the rotating radar. The process is as follows:

[0095] Step 1.1: Establish the mobile carrier coordinate system T according to the mobile carrier machine E ,like Figure 3 As shown;

[0096] Step 1.2: Establish the IMU coordinate system T according to the inertial measurement system IMU I ,like Figure 3 As shown;

[0097] Step 1.3: Establish the rotating radar coordinate system T according to the rotating radar L ,like Figure 3 As shown;

[0098] Step 1.4: Establish the world coordinate system T W ,like Figure 3 As shown, the coordinate system T of the mobile carrier is initialized. E coincide;

[0099] Step 1.5: Establish world coordinate system 2T W2 ,like Figure 3 As shown, and initialized with the IMU coordinate system T I coincide;

[0100] Step 1.6: The LiDAR collects two consecutive frames of point cloud and uses the Calidar Calibration method to obtain the calibration matrix from the LiDAR coordinate system to the gimbal coordinate system.

[0101] Step 2: Statically obtain the 3D point cloud data collected by the rotating radar, assign a confidence level to each point in the point cloud, and remove points with zero confidence level. The process is as follows:

[0102] Step 2.1: When the mobile carrier machine is stationary, a laser radar is used to obtain static two-dimensional point cloud data of the lane and the rotation angle of the rotating pan-tilt platform;

[0103] Step 2.2: According to the static two-dimensional point cloud data, the rotation angle of the rotating gimbal and the calibration matrix obtained in step 1.6, obtain the three-dimensional point cloud data in the rotating radar coordinate system;

[0104] Step 2.3: Obtain the three-dimensional point cloud data in the mobile carrier coordinate system according to the three-dimensional point cloud data in the rotating radar coordinate system and the coordinate transformation matrix between the rotating radar coordinate system and the mobile carrier coordinate system;

[0105] Step 2.4: Test the radar accuracy at different distances, obtain the change of radar accuracy at different distances, and set the confidence for each point in the point cloud. The process is as follows:

[0106] Step 2.4.1: Get the distance from each ranging point in the lidar point cloud data to the radar;

[0107] Step 2.4.2: Test the radar accuracy at different distances and obtain the change of radar accuracy at different distances;

[0108] Step 2.4.3: According to the accuracy test results, set a confidence level in the range of 0 to 1 for each point in the point cloud. The larger the value, the higher the confidence level of the point cloud.

[0109] Step 2.5: Determine whether the confidence of the point in the point cloud is zero. If it is zero, remove the point.

[0110] Step 3: Through filtering, remove outliers, remove the mobile carrier machine body point cloud and voxelize the 3D point cloud data. The process is as follows:

[0111] Step 3.1: Use statistical filters to remove outliers from the three-dimensional point cloud data to obtain the point cloud data after removing outliers. The process is as follows:

[0112] Step 3.1.1: Traverse the point cloud and calculate the average distance between each point and its nearest k neighbor points;

[0113] Step 3.1.2: Calculate the mean μ and standard deviation σ of all average distances, then the distance threshold d max Represented as d max =μ+a×σ, where a is the proportionality coefficient;

[0114] Step 3.1.3: Traverse the point cloud again and remove points whose average distance to k neighbors is greater than d. max point;

[0115] Step 3.2: Conditionally filter the three-dimensional point cloud data using a conditional filter to obtain point cloud data without the mobile carrier machine body. The process is as follows:

[0116] Step 3.2.1: Setting conditional filtering parameters according to the size of the mobile carrier machine and the position of the mobile carrier machine in the mobile carrier coordinate system;

[0117] Step 3.2.2: Remove all point clouds in the cuboid where the mobile carrier machine is located according to the conditional filtering parameters, and obtain three-dimensional point cloud data after removing the point cloud of the mobile carrier machine body in the mobile carrier coordinate system;

[0118] Step 3.3: Use the voxel grid filter to filter and downsample the point cloud data to obtain voxelized three-dimensional point cloud data. The process is as follows:

[0119] Step 3.3.1: Set the grid size and divide the 3D point cloud data into multiple grids, and calculate the centroid of each grid;

[0120] Step 3.3.2: Replace all points in the corresponding grid with the centroid data to obtain voxelized three-dimensional point cloud data in the moving carrier coordinate system.

[0121] Step 4: Obtain the inertial data of the IMU and the speed data of the wheel speed meter, and obtain the displacement data of the mobile carrier machine through Kalman algorithm fusion. The process is as follows:

[0122] Step 4.1: Obtain the inertial data of the inertial measurement system IMU, including the three-axis acceleration data of the inertial measurement system IMU in the IMU coordinate system Quaternion data of IMU rotation angle in world coordinate system 2

[0123] Step 4.2: According to the quaternion data of the IMU rotation angle in the world coordinate system 2, obtain the corresponding rotation matrix R I,W2 ;

[0124] Step 4.3: According to the three-axis acceleration data of the IMU in the IMU coordinate system, obtain the acceleration of the IMU in the world coordinate system 2:

[0125]

[0126] Step 4.4: Obtain the speed data of the two wheel speed meters and obtain the speed data of the mobile carrier machine in the mobile carrier coordinate system through kinematic analysis.

[0127]

[0128] in, is the x-axis speed, is the y-axis speed, is the angular velocity around the z-axis; Step 4.5: Apply the kinematic principle of rigid body plane motion and use the base point method to obtain the velocity data of the mobile carrier machine Obtain the velocity data of the IMU in the mobile carrier coordinate system Among them, the velocity in the z direction is zero;

[0129] Step 4.6: According to the velocity data of IMU in the mobile carrier coordinate system Get the velocity of the IMU in world coordinate system 2:

[0130]

[0131] Among them, R I,e The rotation matrix from the mobile carrier coordinate system to the IMU coordinate system;

[0132] Step 4.7: Use the velocity of the IMU in world coordinate system 2 and the IMU acceleration measured by the IMU as the observation values ​​of the Kalman filter to optimally estimate the system state and estimate the displacement, velocity, and acceleration of the IMU in world coordinate system 2;

[0133] Step 4.8: Based on the quaternion data of the acceleration and rotation angle of the IMU in the world coordinate system 2, obtain the pose matrix of the IMU in the world coordinate system 2, and then perform coordinate transformation to obtain the pose matrix of the mobile carrier machine in the world coordinate system.

[0134] Step 5: Align the two frames of point cloud data to obtain high-precision point cloud pose. The process is as follows:

[0135] Step 5.1: When the rotating radar starts to acquire the point cloud, the machine pose matrix of the mobile carrier in the world coordinate system is stored at the same time;

[0136] Step 5.2: Obtain the mobile carrier machine pose matrix corresponding to the current frame and the previous frame point cloud, and at the same time obtain the mobile carrier machine pose transformation matrix corresponding to the current frame and the previous frame point cloud;

[0137] Step 5.3: Use the mobile carrier machine pose transformation matrix as the initial value, perform ICP registration on the current frame and the previous frame point cloud, and obtain a more accurate mobile carrier machine pose change matrix;

[0138] Step 5.4: using the mobile carrier machine pose transformation matrix to correct the current frame mobile carrier machine pose matrix to obtain a higher precision mobile carrier machine pose matrix;

[0139] Step 5.5: According to the mobile carrier machine pose matrix obtained in step 5.4, the current frame three-dimensional point cloud data is converted from the mobile carrier coordinate system to the world coordinate system, and finally a single frame of three-dimensional point cloud data in the world coordinate system is obtained.

[0140] Step 6: Remove the working surface from the point cloud data of the historical frame. The process is as follows:

[0141] Step 6.1: According to the position of the mobile carrier machine corresponding to the point cloud of the current frame, find the accessible space of the mobile carrier machine digging the tunnel at this time;

[0142] Step 6.2: Find the maximum range that the mobile carrier machine can process at the current position, and find the maximum range that the mobile carrier machine can affect the historical frame point cloud;

[0143] Step 6.3: Use conditional filtering to remove all points within this range in the historical frame point cloud in the world coordinate system.

[0144] Step 7: Perform point cloud fusion based on confidence to obtain a 3D tunnel model. The process is as follows:

[0145] Step 7.1: Calculate the index of each point in the point cloud according to the voxel where each point in the three-dimensional point cloud data in the world coordinate system is located;

[0146] Step 7.2: Put the indexes of all points of the first frame point cloud into the index container;

[0147] Step 7.3: When a new point cloud is input, determine in turn whether the index of each point in the point cloud already exists in the index container;

[0148] If it does not exist, put the index of this point into the index container, keep this point in the overall point cloud, and then continue to determine the next point;

[0149] If it exists, keep the point with greater confidence and discard the point with smaller confidence. If the confidence is the same, keep the midpoint of the two points and continue to judge the next point.

[0150] Step 7.4: After all points in the point cloud are judged, continue to wait for the next frame of point cloud input to dynamically remove the index in the index container, and only retain the index of the voxel that may be affected;

[0151] Step 7.5: Reconstruct the surface of the entire point cloud and obtain the three-dimensional reconstruction model of the tunnel online.

[0152] In this embodiment, the specific data processing flow of the method is as follows Figure 4 shown.

Claims

1. A lane 3D reconstruction system based on rotating radar and IMU, characterized in that: The system includes a mobile carrier machine (7), a track wheel (1), a wheel speed meter (2), an inertial measurement system IMU (4), a rotating radar (5), an industrial computer (6) and a power supply (3); The wheel speed meter (2) is installed on the track wheel (1), and the inertial measurement system IMU (4), the rotating radar (5) and the industrial computer (6) are installed on the mobile carrier machine (7); The wheel speed meter (2), the inertial measurement system IMU (4), and the rotating radar (5) are electrically connected to the industrial control computer (6) respectively; The power supply is used to supply power to the wheel speed meter (2), the inertial measurement system IMU (4), the rotating radar (5) and the industrial computer (6); The lane 3D reconstruction system based on rotating radar and IMU uses the following methods to perform lane 3D reconstruction: Step 1: Establish a coordinate system for the lane 3D reconstruction system based on the rotating radar and IMU, and calibrate the rotating radar; Step 1.1: Establish the mobile carrier coordinate system T according to the mobile carrier machine E ; Step 1.2: Establish the IMU coordinate system T according to the inertial measurement system IMU I ; Step 1.3: Establish the rotating radar coordinate system T according to the rotating radar L ; Step 1.4: Establish the world coordinate system T W , and initialize the coordinate system T with the moving carrier E coincide; Step 1.5: Establish world coordinate system 2T W2 , and initialize with the IMU coordinate system T I coincide; Step 1.6: The laser radar collects two consecutive frames of point cloud, and uses the Calidar Calibration method to obtain the calibration matrix from the laser radar coordinate system to the gimbal coordinate system; Step 2: statically obtain the 3D point cloud data collected by the rotating radar, assign a confidence level to each point in the point cloud, and remove points with zero confidence level; Step 3: Through filtering, the three-dimensional point cloud data is subjected to outlier removal, point cloud removal of the mobile carrier machine body, and voxelization operations; Step 4: Obtain the inertial data of the IMU and the speed data of the wheel speed meter, and obtain the displacement data of the mobile carrier machine through Kalman algorithm fusion; Step 4.1: Obtain the inertial data of the inertial measurement system IMU, including the three-axis acceleration data of the inertial measurement system IMU in the IMU coordinate system Quaternion data of IMU rotation angle in world coordinate system 2 Step 4.2: According to the quaternion data of the IMU rotation angle in the world coordinate system 2, obtain the corresponding rotation matrix R I,W2 ; Step 4.3: According to the three-axis acceleration data of the IMU in the IMU coordinate system, obtain the acceleration of the IMU in the world coordinate system 2: Step 4.4: Obtain the speed data of the two wheel speed meters and obtain the speed data of the mobile carrier machine in the mobile carrier coordinate system through kinematic analysis. in, is the x-axis speed, is the y-axis speed, is the angular velocity around the z axis; Step 4.5: Apply the kinematics of rigid body planar motion and use the base point method to calculate the velocity data of the mobile carrier machine. Obtain the velocity data of the IMU in the mobile carrier coordinate system Among them, the velocity in the z direction is zero; Step 4.6: According to the velocity data of IMU in the mobile carrier coordinate system Get the velocity of the IMU in world coordinate system 2: Among them, R I,e The rotation matrix from the mobile carrier coordinate system to the IMU coordinate system; Step 4.7: Use the velocity of the IMU in world coordinate system 2 and the IMU acceleration measured by the IMU as the observation values ​​of the Kalman filter to optimally estimate the system state and estimate the displacement, velocity, and acceleration of the IMU in world coordinate system 2; Step 4.8: According to the quaternion data of the acceleration and rotation angle of the IMU in the world coordinate system 2, the pose matrix of the IMU in the world coordinate system 2 is obtained, and then the coordinate transformation is performed to obtain the pose matrix of the mobile carrier machine in the world coordinate system; Step 5: Align the point cloud data of the previous and next two frames to obtain high-precision point cloud pose; Step 6: Remove the working surface from the point cloud data of the historical frame; Step 7: Perform point cloud fusion based on confidence to obtain a 3D tunnel model.

2. The lane 3D reconstruction system based on rotating radar and IMU according to claim 1, characterized in that: The industrial computer (6) comprises a processor and a memory; a computer program is stored in the memory, and when the computer program is executed by the processor, three-dimensional reconstruction of the lane is achieved.

3. The lane 3D reconstruction system based on rotating radar and IMU according to claim 1, characterized in that: The rotating radar (5) comprises a laser radar, a rotating platform and a rotating shaft; the rotating shaft is perpendicular to the ground and rotates at a constant speed; the laser radar is fixedly connected to the rotating shaft, and the scanning plane of the laser radar is perpendicular to the ground; the fixedly connected laser radar and the rotating shaft rotate together with the rotating platform.

4. The lane 3D reconstruction system based on rotating radar and IMU according to claim 1, characterized in that: The process of step 2 is as follows: Step 2.1: When the mobile carrier machine is stationary, a laser radar is used to obtain static two-dimensional point cloud data of the lane and the rotation angle of the rotating pan-tilt platform; Step 2.2: According to the static two-dimensional point cloud data, the rotation angle of the pan-tilt platform and the calibration matrix obtained in step 1.6, obtain the three-dimensional point cloud data in the rotating radar coordinate system; Step 2.3: Obtain the three-dimensional point cloud data in the mobile carrier coordinate system according to the three-dimensional point cloud data in the rotating radar coordinate system and the coordinate transformation matrix between the rotating radar coordinate system and the mobile carrier coordinate system; Step 2.4: Test the radar accuracy at different distances, obtain the change of radar accuracy at different distances, and set the confidence for each point in the point cloud. The process is as follows: Step 2.4.1: Get the distance from each ranging point in the lidar point cloud data to the radar; Step 2.4.2: Test the radar accuracy at different distances and obtain the change of radar accuracy at different distances; Step 2.4.3: According to the accuracy test results, set a confidence level in the range of 0 to 1 for each point in the point cloud. The larger the value, the higher the confidence level of the point cloud. Step 2.5: Determine whether the confidence of the point in the point cloud is zero. If it is zero, remove the point.

5. The lane 3D reconstruction system based on rotating radar and IMU according to claim 1, characterized in that: The process of step 3 is as follows: Step 3.1: Use statistical filters to remove outliers from the three-dimensional point cloud data to obtain the point cloud data after removing outliers. The process is as follows: Step 3.1.1: Traverse the point cloud and calculate the average distance between each point and its nearest k neighbor points; Step 3.1.2: Calculate the mean μ and standard deviation σ of all average distances, then the distance threshold d max Represented as d max =μ+a×σ, where a is the proportionality coefficient; Step 3.1.3: Traverse the point cloud again and remove points whose average distance to k neighbors is greater than d. max point; Step 3.2: Conditionally filter the three-dimensional point cloud data using a conditional filter to obtain point cloud data without the mobile carrier machine body. The process is as follows: Step 3.2.1: Setting conditional filtering parameters according to the size of the mobile carrier machine and the position of the mobile carrier machine in the mobile carrier coordinate system; Step 3.2.2: Remove all point clouds in the cuboid where the mobile carrier machine is located according to the conditional filtering parameters, and obtain three-dimensional point cloud data after removing the point cloud of the mobile carrier machine body in the mobile carrier coordinate system; Step 3.3: Use the voxel grid filter to filter and downsample the point cloud data to obtain voxelized three-dimensional point cloud data. The process is as follows: Step 3.3.1: Set the grid size and divide the 3D point cloud data into multiple grids, and calculate the centroid of each grid; Step 3.3.2: Replace all points in the corresponding grid with the centroid data to obtain voxelized three-dimensional point cloud data in the moving carrier coordinate system.

6. The lane 3D reconstruction system based on rotating radar and IMU according to claim 1, characterized in that: The process of step 5 is as follows: Step 5.1: When the rotating radar starts to acquire the point cloud, the machine pose matrix of the mobile carrier in the world coordinate system is stored at the same time; Step 5.2: Obtain the mobile carrier machine pose matrix corresponding to the current frame and the previous frame point cloud, and at the same time obtain the mobile carrier machine pose transformation matrix corresponding to the current frame and the previous frame point cloud; Step 5.3: Use the mobile carrier machine pose transformation matrix as the initial value, perform ICP registration on the current frame and the previous frame point cloud, and obtain a more accurate mobile carrier machine pose change matrix; Step 5.4: using the mobile carrier machine pose transformation matrix to correct the current frame mobile carrier machine pose matrix to obtain a higher precision mobile carrier machine pose matrix; Step 5.5: According to the mobile carrier machine pose matrix obtained in step 5.4, the current frame three-dimensional point cloud data is converted from the mobile carrier coordinate system to the world coordinate system, and finally a single frame of three-dimensional point cloud data in the world coordinate system is obtained.

7. The lane 3D reconstruction system based on rotating radar and IMU according to claim 1, characterized in that: The process of step 7 is as follows: Step 7.1: Calculate the index of each point in the point cloud according to the voxel where each point in the three-dimensional point cloud data in the world coordinate system is located; Step 7.2: Put the indexes of all points of the first frame point cloud into the index container; Step 7.3: When a new point cloud is input, determine in turn whether the index of each point in the point cloud already exists in the index container; If it does not exist, put the index of this point into the index container, keep this point in the overall point cloud, and then continue to determine the next point; If it exists, keep the point with greater confidence and discard the point with smaller confidence. If the confidence is the same, keep the midpoint of the two points and continue to judge the next point. Step 7.4: After all points in the point cloud are judged, continue to wait for the next frame of point cloud input to dynamically remove the index in the index container, and only retain the index of the voxel that may be affected; Step 7.5: Reconstruct the surface of the entire point cloud and obtain the three-dimensional reconstruction model of the tunnel online.

Images