Vehicle bottom inspection robot accurate positioning method based on 3D point cloud registration

Abstract

The invention aims to provide an accurate positioning method of a metro vehicle bottom inspection robot based on 3D point cloud registration, and the method comprises the steps: collecting a 3D template point cloud at a designated position of a metro vehicle bottom through a 3D camera at the tail end of the inspection robot, and storing the 3D template point cloud and the coordinates of the 3D template point cloud on an SLAM map in a database. When an inspection robot executes a task and the robot reaches a first template point cloud acquisition position, a 3D task point cloud is acquired by using a terminal 3D camera, the task point cloud and the template point cloud are accurately matched through a point cloud registration algorithm, rotation (R matrix) and translation (T matrix) information is acquired, and thus the prior offset of a map point relative to a vehicle bottom is obtained. In a subsequent task process, a point cloud registration algorithm can be repeatedly called at a subsequent point cloud acquisition map point, and accurate positioning offset correction is continuously provided for the SLAM navigation module in combination with the prior offset, so that robot navigation is more accurate and more robust.

Description

Technical Field

[0001] The present invention relates to a field of precision positioning for an under-vehicle inspection robot based on 3D point cloud registration. Background Art

[0002] Automated subway undercar inspections rely on precise navigation and positioning by inspection robots, and SLAM (Simultaneous Localization and Mapping) technology is central to achieving autonomous navigation. However, subway maintenance tracks (the working environment of undercar inspection robots) are typically long corridors with smooth, symmetrical walls and repetitive textures. This presents a significant challenge for SLAM systems in achieving accurate positioning and mapping. Summary of the Invention

[0003] To address the challenges of the prior art, this paper provides a precise positioning method for an undercar inspection robot based on 3D point cloud registration. The method uses a 3D structured light depth camera at the robot's end to acquire high-precision point cloud data from the subway car undercarriage. Using a point cloud registration algorithm, the robot calculates the rotation (R matrix) and translation (T matrix) of the task point cloud relative to the template point cloud, thereby determining the robot's offset error in the X and Y axes. This error can be used to correct SLAM navigation results at a specific location, further improving the robot's positioning accuracy during inspection tasks.

[0004] The present invention provides a precise positioning method for a vehicle underbody inspection robot based on 3D point cloud registration, comprising the following steps:

[0005] Step 1) Template point cloud acquisition and database construction: Collect high-precision template point clouds and record the robot coordinates at the time of each template point cloud acquisition in the database;

[0006] Step 2) Dynamic registration of task point clouds and initial offset calculation: When the robot performs an inspection task and reaches the physical location corresponding to the first template point cloud, the terminal 3D camera is triggered to collect the 3D task point cloud;

[0007] Step 3) Calculate the prior offset by cloud registration: Call the point cloud registration algorithm to accurately match the task point cloud with the template point cloud in the database, obtain the R matrix and T matrix of the point cloud registration, and calculate the prior offset of the robot in the map. In the subsequent process of the robot inspection task, whenever the robot reaches a new point cloud collection location, it repeats the point cloud collection and point cloud registration to calculate the new R matrix and T matrix.

[0008] Step 4) Dynamic point cloud matching and SLAM correction: Combined with the initial prior offset, the error accumulation of the robot's current position is dynamically calculated during the robot's movement, and the SLAM navigation results are continuously corrected to provide accurate pose updates; by fusing point cloud registration with SLAM pose estimation, the SLAM cumulative error is reduced during long inspection tasks.

[0009] Further improvement, the specific process of template point cloud collection and database construction in step 1) is as follows:

[0010] Step 1.1) Assuming a track map has been constructed, the robot uses fixed-point control technology to reach a precise position, selecting a location where the subway vehicle's underbody texture features are obvious and the component surface has some undulations;

[0011] Step 1.2) Use the 3D structured light depth camera at the end of the inspection robot to collect the 3D template point cloud and record the current robot coordinates. The template point cloud is represented as:

[0012]

[0013] in, is the template point cloud, p i Represents the three-dimensional coordinates of each point;

[0014] The position of each template point cloud in the SLAM map coordinate system is expressed as:

[0015]

[0016] where x i ,y i Respectively represent the coordinates of the i-th template point cloud on the X-axis and Y-axis, θ i Indicates the robot's heading angle.

[0017] Further improvement: in step 1.1), the locations where the texture features of the subway vehicle bottom are relatively obvious and the surface of the components has certain undulations include the bogie center bolt, the brake valve mounting seat, and the bogie.

[0018] Further improvement, the task point cloud in step 2) is represented as:

[0019]

[0020] in, is the template point cloud, q i Represents the three-dimensional coordinates of each point.

[0021] Further improvement, step 3) the point cloud registration calculates the prior offset, the specific process is: using the point cloud registration ICP algorithm to register the task point cloud With template point cloud Perform registration, the goal is to find an optimal rotation matrix and translation vectors So that:

[0022]

[0023] where q' i It is the point cloud data after registration;

[0024] Set the optimization goal to:

[0025]

[0026] The optimal R,T is obtained through iterative optimization, and then the prior offset of the robot relative to the SLAM map coordinate system is calculated:

[0027] ΔX=T x ,ΔY=T y ,Δθ=arctan(R 21 ,R 11 )

[0028] Where: ΔX, ΔY are translation errors, Δθ is the element R of the rotation matrix 21 ,R 11 The yaw angle error is calculated;

[0029] The prior offset Used to represent the SLAM map and the one saved in step 1) The offset of the map point that the robot actually needs to reach is

[0030] Further improvement, the dynamic point cloud matching and SLAM correction process in step 4) is specifically as follows:

[0031] Step 4.1: In the subsequent process of the robot inspection task, whenever the robot reaches the prior offset Corrected new point cloud collection position Repeat point cloud acquisition and point cloud registration to calculate new R, T and obtain the fine-tuned offset.

[0032] Step 4.2: Combined with the initial prior offset, the SLAM navigation pose estimate is corrected to:

[0033]

[0034] Finally, by fusing point cloud registration with SLAM pose estimation, the SLAM cumulative error is reduced in long-term inspection tasks.

[0035] The beneficial effects of the present invention are:

[0036] 1. Utilize 3D point cloud information to enhance environmental perception, improve localization in the vehicle's underbody environment, and reduce the impact of insufficient feature points;

[0037] 2. Accurate pose estimation through point cloud registration can effectively compensate for the cumulative error of SLAM and improve the positioning accuracy of the robot at the task point;

[0038] 3. By integrating point cloud data with SLAM navigation information, high-precision and high-stability robot positioning can be achieved in subway inspection scenarios, ensuring the reliability of inspection tasks.

[0039] 4. The technical solution proposed in this patent can effectively improve the autonomous positioning accuracy of subway inspection robots, reduce the problem of decreased inspection accuracy caused by SLAM errors, and provide a more efficient and safe solution for intelligent inspection under subway vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0041] Figure 1 Execute flowchart for software;

[0042] Figure 2 This is a point cloud visualization of the center bolt of the bogie underbody of a subway vehicle. DETAILED DESCRIPTION

[0043] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0044] The purpose of the present invention is to provide a precise positioning system for a subway vehicle bottom inspection robot based on 3D point cloud registration. The 3D camera at the end of the inspection robot is used to collect 3D template point clouds at designated locations under the subway vehicle, and the 3D template point clouds and their coordinates on the SLAM map are saved in the database. When the inspection robot performs a task, when the robot reaches the first template point cloud collection position, the 3D task point cloud is collected using the end 3D camera, and the task point cloud and the template point cloud are accurately matched through the point cloud registration algorithm to obtain the rotation (R matrix) and translation (T matrix) information, thereby obtaining the prior offset of the map point relative to the bottom of the vehicle. In the subsequent task process, the point cloud registration algorithm can be repeatedly called at the subsequent point cloud collection map points, and the SLAM navigation module can be continuously provided with precise positioning offset correction in combination with the prior offset, so that the robot navigation is more accurate and more robust. The flow chart is as follows Figure 1 shown.

[0045] The technical solution adopted by this invention is a precise positioning system for subway undercar inspection robots based on 3D point cloud registration. The specific implementation is as follows:

[0046] Step 1: Template Point Cloud Collection and Database Construction: During the initial calibration phase, the robot is controlled to move directly below key components (such as bogie center bolts, brake valve mounting bases, and couplers) on the subway car floor. A 3D structured light camera (resolution ≥1mm@1m) is used at the end to collect high-precision template point clouds, with a single frame containing ≥500,000 points. The robot's coordinates at the time of each template point cloud collection are recorded in the database.

[0047] Step 2: When the robot reaches the physical location corresponding to the first template point cloud during its inspection mission, it triggers the terminal 3D camera to capture the 3D mission point cloud. A point cloud registration algorithm is then used to accurately match the mission point cloud with the template point cloud in the database. The rotation matrix (R matrix) and translation matrix (T matrix) for point cloud registration are obtained, and the robot's prior offset in the map is calculated (i.e., the actual error in the SLAM map coordinate system, caused by the offset between each stop of a subway vehicle).

[0048] Step 3: During subsequent inspections, whenever the robot reaches a new point cloud collection location, it repeats point cloud collection and registration, calculating new R and T matrices. Combined with the initial prior offset, the accumulated error of the robot's current position is dynamically calculated as the robot moves, and the SLAM navigation results are continuously corrected to provide accurate pose updates. By integrating point cloud registration with SLAM pose estimation, the accumulated SLAM error is reduced during long inspections, improving the robot's autonomous positioning accuracy in under-vehicle environments.

[0049] The specific process of the present invention is detailed as follows:

[0050] Step 1: Template point cloud collection and database construction:

[0051] Step 1.1: Assuming a track map has been constructed, the robot can achieve precise positioning through fixed-point control technology (under low-speed motion conditions, the robot can achieve high fixed-point control accuracy with high robustness). The robot is primarily selected for locations where the subway vehicle's underbody texture features are obvious and the component surface has certain undulations, such as the bogie center bolt, brake valve mounting seat, and bogie.

[0052] Step 1.2: Use the 3D structured light depth camera at the end of the inspection robot to collect the 3D template point cloud and record the current robot coordinates. Figure 2 The figure below shows a 3D point cloud visualization of the area around the center bolt of the bogie underbody of a subway vehicle. The template point cloud can be represented as:

[0053]

[0054] in, is the template point cloud, p i Represents the three-dimensional coordinates of each point.

[0055] The position of each template point cloud in the SLAM map coordinate system can be expressed as: where x i ,y i Respectively represent the coordinates of the i-th template point cloud on the X-axis and Y-axis, θ i Indicates the robot's heading angle.

[0056] Step 2: Dynamic registration of task point clouds and initial offset calculation

[0057] Step 2.1: When the robot reaches the template point cloud collection position for the first time, it triggers the 3D camera to collect the current task point cloud. The task point cloud can be expressed as:

[0058]

[0059] in, is the template point cloud, q i Represents the three-dimensional coordinates of each point.

[0060] Step 3: Calculate the prior offset by point cloud registration

[0061] The task point cloud is registered using the ICP algorithm (Iterative Closest Point) With template point cloud Perform registration. The goal is to find an optimal rotation matrix and translation vectors So that:

[0062]

[0063] where q' i It is the point cloud data after registration.

[0064] Set the optimization goal to:

[0065]

[0066] The optimal R,T is obtained through iterative optimization, and then the prior offset of the robot relative to the SLAM map coordinate system is calculated:

[0067] ΔX=T x ,ΔY=T y ,Δθ=arctan(R 21 ,R 11 )

[0068] Where: ΔX, ΔY are translation errors, Δθ is the element R of the rotation matrix 21 ,R 11 The yaw angle error is calculated.

[0069] The prior offset Used to represent the SLAM map and the one saved in step 1 That is, These are the coordinates of the map points that the robot actually needs to reach.

[0070] Step 4 Dynamic point cloud matching and SLAM correction

[0071] Step 4.1: In the subsequent process of the robot inspection task, whenever the robot reaches the prior offset Corrected new point cloud collection position Repeat point cloud acquisition and point cloud registration to calculate new R, T and obtain the fine-tuned offset.

[0072] Step 4.2: Combined with the initial prior offset, the SLAM navigation pose estimate is corrected to:

[0073]

[0074] Finally, by integrating point cloud registration and SLAM pose estimation, the SLAM cumulative error is reduced during long-term inspection tasks and the autonomous positioning accuracy of the robot in the vehicle-under-car environment is improved.

[0075] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, the above is only a preferred embodiment of the present invention. Since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited to this. Any technical personnel familiar with this technical field is within the technical scope disclosed by the present invention. For ordinary technical personnel in this technical field, changes or replacements that can be easily thought of should be covered within the protection scope of the present invention without departing from the principle of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A precise positioning method for vehicle underbody inspection robots based on 3D point cloud registration, characterized in that The following steps are involved: Step 1) Template point cloud acquisition and database construction: Collect high-precision template point clouds and record the robot coordinates at the time of each template point cloud acquisition in the database; Step 2) Dynamic registration of task point clouds and initial offset calculation: When the robot performs an inspection task and reaches the physical location corresponding to the first template point cloud, the terminal 3D camera is triggered to collect the 3D task point cloud; Step 3) Calculate the prior offset by cloud registration: Call the point cloud registration algorithm to accurately match the task point cloud with the template point cloud in the database, obtain the R matrix and T matrix of the point cloud registration, and calculate the prior offset of the robot in the map. In the subsequent process of the robot inspection task, whenever the robot reaches a new point cloud collection location, it repeats the point cloud collection and point cloud registration to calculate the new R matrix and T matrix. Step 4) Dynamic point cloud matching and SLAM correction: Combined with the initial prior offset, the error accumulation of the robot's current position is dynamically calculated during the robot's movement, and the SLAM navigation results are continuously corrected to provide accurate pose updates; by fusing point cloud registration with SLAM pose estimation, the SLAM cumulative error is reduced during long inspection tasks.

2. The method for accurately positioning a vehicle underbody inspection robot based on 3D point cloud registration according to claim 1, characterized in that: The specific process of template point cloud collection and database construction in step 1) is as follows: Step 1.1) Assuming a track map has been constructed, the robot uses fixed-point control technology to reach a precise position, selecting a location where the subway vehicle's underbody texture features are obvious and the component surface has some undulations; Step 1.2) Use the 3D structured light depth camera at the end of the inspection robot to collect the 3D template point cloud and record the current robot coordinates. The template point cloud is represented as: in, is the template point cloud, p i Represents the three-dimensional coordinates of each point; The position of each template point cloud in the SLAM map coordinate system is expressed as: where x i ,y i Respectively represent the coordinates of the i-th template point cloud on the X-axis and Y-axis, θ i Indicates the robot's heading angle.

3. The precise positioning method for an under-vehicle inspection robot based on 3D point cloud registration according to claim 2, characterized in that: In step 1.1), the locations where the texture features of the subway vehicle bottom are relatively obvious and the surface of the components has certain undulations include the bogie center bolt, the brake valve mounting seat, and the bogie.

4. The precise positioning method for an under-vehicle inspection robot based on 3D point cloud registration according to claim 1, characterized in that: The task point cloud in step 2) is represented as: in, is the template point cloud, q i Represents the three-dimensional coordinates of each point.

5. The precise positioning method for an under-vehicle inspection robot based on 3D point cloud registration according to claim 2, characterized in that: Step 3) The point cloud registration calculates the prior offset. The specific process is: using the point cloud registration ICP algorithm to register the task point cloud With template point cloud Perform registration, the goal is to find an optimal rotation matrix and translation vectors So that: where q' i It is the point cloud data after registration; Set the optimization goal to: The optimal R,T is obtained through iterative optimization, and then the prior offset of the robot relative to the SLAM map coordinate system is calculated: ΔX=T x ,ΔY=T y ,Δθ=arctan(R 21 ,R 11 ) Where: ΔX, ΔY are translation errors, Δθ is the element R of the rotation matrix 21 ,R 11 The yaw angle error is calculated; The prior offset Used to represent the SLAM map and the one saved in step 1) The offset of the map point that the robot actually needs to reach is 6. The precise positioning method for an under-vehicle inspection robot based on 3D point cloud registration according to claim 1, characterized in that: Step 4) The dynamic point cloud matching and SLAM correction process is specifically as follows: Step 4.1: In the subsequent process of the robot inspection task, whenever the robot reaches the prior offset Corrected new point cloud collection position Repeat point cloud acquisition and point cloud registration to calculate new R, T and obtain the fine-tuned offset. Step 4.2: Combined with the initial prior offset, the SLAM navigation pose estimate is corrected to: Finally, by fusing point cloud registration with SLAM pose estimation, the SLAM cumulative error is reduced in long-term inspection tasks.

Images