3D laser SLAM fusion magnetic stripe inspection robot navigation positioning method

Abstract

This invention provides a 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method, belonging to the field of navigation and positioning technology. The method includes: initializing the robot by acquiring an initial pose and presetting a rectangular boundary for a dynamically changing region; determining in real time whether the current pose is within the dynamically changing region; if the current pose is outside the dynamically changing region, registering the current frame of laser data with a pre-constructed point cloud map to obtain a first positioning pose; if the current pose is within the dynamically changing region, performing laser fusion magnetic stripe positioning: acquiring the initial 3D positioning pose and initial magnetic stripe data; determining the initial vertical deviation distance; acquiring the real-time 3D positioning pose and real-time magnetic stripe data; determining the real-time vertical deviation distance; and correcting the pose output by the laser odometry through forced constraints to determine the final fused pose. According to this invention, interference in dynamically changing regions can be better addressed, achieving stable and reliable navigation and positioning.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and in particular to a navigation and positioning method for a 3D laser SLAM fusion magnetic stripe inspection robot. Background Technology

[0002] Current technologies, while employing 3D LiDAR SLAM for localization, do not consider the impact of local dynamic environments on the positioning results. In other words, the inspection robot's navigation and positioning are unstable in local dynamic areas.

[0003] The information disclosed in the background section of this application is intended only to enhance the understanding of the general background of this application and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention

[0004] This invention provides a navigation and positioning method for a 3D laser SLAM fusion magnetic stripe inspection robot, which can solve the technical problem of unstable navigation and positioning of inspection robots in local dynamic areas.

[0005] According to the present invention, a 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method is provided, comprising: initializing the robot's initial pose in a global map and presetting a rectangular boundary of at least one dynamically changing region; during the robot's inspection process, determining in real time whether the current pose is located within the dynamically changing region; if the current pose is outside the dynamically changing region, registering the current frame of laser data with a pre-constructed point cloud map to obtain a first positioning pose; if the current pose is within the dynamically changing region, performing laser fusion magnetic stripe positioning: acquiring the initial 3D positioning pose and initial magnetic stripe data in the global coordinate system when the robot first detects the magnetic stripe signal; determining the initial vertical deviation distance based on the initial 3D positioning pose and the initial magnetic stripe data; during continuous operation within the dynamically changing region, acquiring the real-time 3D positioning pose and real-time magnetic stripe data in real time; determining the real-time vertical deviation distance based on the real-time 3D positioning pose and the real-time magnetic stripe data; correcting the pose output by the laser odometry by forcibly constraining the geometric consistency between the real-time vertical deviation distance and the initial vertical deviation distance, and determining the final fused pose.

[0006] Further, determining the initial vertical deviation distance based on the initial 3D positioning pose and the initial magnetic stripe data includes: obtaining the initial center coordinates of the 3D LiDAR center in the global coordinate system at the initial moment based on the initial 3D positioning pose; obtaining the initial front magnetic intersection coordinates and initial rear magnetic intersection coordinates of the intersection points of the magnetic sensor and the magnetic stripe in the global coordinate system at the initial moment based on the initial magnetic stripe data; obtaining the initial magnetic stripe direction angle, wherein the initial magnetic stripe direction angle is the angle between the magnetic stripe direction at the initial moment and the X-axis of the global coordinate system; and determining the initial vertical deviation distance based on the initial center coordinates, the initial front magnetic intersection coordinates, the initial rear magnetic intersection coordinates, and the initial magnetic stripe direction angle.

[0007] Further, based on the initial center coordinates, the initial front magnetic intersection coordinates, the initial rear magnetic intersection coordinates, and the initial magnetic strip direction angle, the initial vertical deviation distance is determined, including: according to the formula: Determine the initial vertical offset distance ,in, As the initial center coordinates, These are the initial pre-magnetic intersection coordinates. These are the initial and subsequent magnetic intersection coordinates. The initial magnetic stripe orientation angle.

[0008] Further, determining the real-time vertical deviation distance based on the real-time 3D positioning pose and the real-time magnetic stripe data includes: obtaining the real-time center coordinates of the 3D LiDAR center in the global coordinate system based on the real-time 3D positioning pose; obtaining the real-time front magnetic intersection coordinates and the real-time rear magnetic intersection coordinates of the intersection points of the front and rear magnetic sensors with the magnetic stripe in the global coordinate system based on the real-time magnetic stripe data; obtaining the real-time magnetic stripe direction angle, wherein the real-time magnetic stripe direction angle is the angle between the real-time magnetic stripe direction and the X-axis of the global coordinate system; and determining the real-time vertical deviation distance based on the real-time center coordinates, the real-time front magnetic intersection coordinates, the real-time rear magnetic intersection coordinates, and the real-time magnetic stripe direction angle.

[0009] Further, based on the real-time center coordinates, the real-time front magnetic intersection coordinates, the real-time rear magnetic intersection coordinates, and the real-time magnetic stripe direction angle, the real-time vertical deviation distance is determined, including: according to the formula: Determine the real-time vertical deviation ,in, For real-time center coordinates, For real-time pre-magnetic intersection coordinates, For real-time post-magnetic intersection coordinates, t represents the real-time magnetic stripe orientation angle, and t represents the current time.

[0010] Furthermore, by forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, the pose output by the laser odometry is corrected to determine the final fused pose. This includes: obtaining the initial center coordinates of the 3D LiDAR center in the global coordinate system based on the initial 3D positioning pose; obtaining the real-time magnetic stripe direction angle and the real-time velocity of the robot along the tangent direction of the magnetic stripe; determining the fused coordinates based on the initial center coordinates, the real-time magnetic stripe direction angle, the real-time velocity, and the initial vertical deviation; obtaining the initial robot heading angle based on the initial 3D positioning pose; obtaining the initial robot yaw angle by subtracting the initial magnetic stripe direction angle from the initial robot heading angle; determining the fused direction angle by adding the value of the real-time magnetic stripe direction angle to the initial robot yaw angle; and determining the final fused pose based on the fused coordinates and the fused direction angle.

[0011] Further, based on the initial center coordinates, the real-time magnetic stripe direction angle, the real-time velocity, and the initial vertical deviation, the fused coordinates are determined, including: according to the formula: Determine fusion coordinates ,in, As the initial center coordinates, for Real-time speed at any moment for Real-time magnetic stripe orientation angle at any given moment. This is the real-time magnetic stripe orientation angle. This is the initial vertical offset.

[0012] According to the present invention, a 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning system is provided, comprising:

[0013] The initialization module is used to obtain the robot's initial pose in the global map for initialization, and to preset the rectangular boundary of at least one dynamically changing region.

[0014] The judgment module is used to determine in real time whether the current pose is located within the dynamically changing area during the robot's inspection process;

[0015] The first positioning pose module is used to register the current frame of laser data with a pre-built point cloud map to obtain the first positioning pose if the current pose is outside the dynamically changing area.

[0016] An execution module is configured to perform laser fusion magnetic stripe positioning if the current pose is located within the dynamically changing region.

[0017] The acquisition module is used to acquire the initial 3D positioning pose in the global coordinate system and the initial magnetic strip data when the robot first detects the magnetic strip signal.

[0018] The initial vertical offset module is used to determine the initial vertical offset based on the initial 3D positioning pose and the initial magnetic strip data.

[0019] The real-time acquisition module is used to acquire real-time 3D positioning pose and real-time magnetic strip data during continuous operation within the dynamically changing area.

[0020] The real-time vertical offset module is used to determine the real-time vertical offset based on the real-time 3D positioning pose and the real-time magnetic strip data.

[0021] The final pose fusion module is used to correct the pose output by the laser odometry by forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, and to determine the final pose fusion.

[0022] Technical Effects: According to the present invention, by pre-setting a dynamically changing area and adopting different positioning strategies, dynamic environmental changes can be effectively addressed. Outside the dynamically changing area, positioning is achieved by registering laser data with a pre-built point cloud map, improving positioning accuracy in conventional environments. Within the dynamically changing area, laser-fused magnetic strip positioning is employed. By forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, the pose output by the laser odometry is corrected, better addressing interference in the dynamically changing area and achieving stable and reliable navigation and positioning performance. When determining the initial vertical deviation, it can be determined by the vertical distance from the radar center to the magnetic strip path, correcting the component of the laser odometry perpendicular to the magnetic strip, thereby suppressing lateral drift and reducing lateral errors in the dynamically changing area. When determining the real-time vertical offset, the vertical distance from the real-time radar center to the current magnetic strip path can be used. This allows for adaptation to minor unevenness in the magnetic strip layout. Even when the robot travels along a curved magnetic strip path, the real-time deviation of the radar center relative to the current magnetic strip segment can be accurately calculated, elevating the magnetic strip information from a "static line" to a "dynamic path constraint," thus improving the adaptability of fusion positioning in complex magnetic strip layouts. When determining the fusion coordinates, factors such as the robot's initial position, speed and direction changes during movement, and the initial vertical offset can be used. Real-time speed and real-time magnetic strip orientation angle reflect the robot's dynamic motion information. By reducing the accumulated lateral error through the initial vertical offset, the robot's actual position in the global coordinate system can be more accurately reflected, thereby improving the positioning accuracy and reliability in dynamic environments.

[0023] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only, and are not intended to limit the invention. Other features and aspects of the invention will become clearer from the following detailed description of exemplary embodiments with reference to the accompanying drawings. Attached Figure Description

[0024] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other embodiments can be obtained based on these drawings without creative effort.

[0025] Figure 1 An exemplary flowchart of a 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to an embodiment of the present invention is shown.

[0026] Figure 2 An exemplary flowchart for calculating the initial vertical offset distance according to an embodiment of the present invention is shown;

[0027] Figure 3 An exemplary flowchart for calculating the real-time vertical deviation distance according to an embodiment of the present invention is shown;

[0028] Figure 4 An exemplary flowchart illustrating the calculation of the final pose fusion according to an embodiment of the present invention is shown;

[0029] Figure 5 A block diagram of a 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning system according to an embodiment of the present invention is shown as an example. Detailed Implementation

[0030] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0031] The technical solution of the present invention will be described in detail below with reference to specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments.

[0032] Figure 1An exemplary flowchart of a 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to an embodiment of the present invention is shown. The method includes: Step S1, initializing the robot's initial pose in a global map and presetting at least one rectangular boundary of a dynamically changing region; Step S2, during the robot's inspection process, determining in real time whether the current pose is located within the dynamically changing region; Step S3, if the current pose is outside the dynamically changing region, registering the current frame of laser data with a pre-constructed point cloud map to obtain a first positioning pose; Step S4, if the current pose is within the dynamically changing region, performing laser fusion magnetic stripe inspection. Positioning: Step S5, acquire the initial 3D positioning pose and initial magnetic stripe data in the global coordinate system when the robot first detects the magnetic stripe signal; Step S6, determine the initial vertical deviation distance based on the initial 3D positioning pose and the initial magnetic stripe data; Step S7, acquire the real-time 3D positioning pose and real-time magnetic stripe data in real time during continuous operation within the dynamically changing area; Step S8, determine the real-time vertical deviation distance based on the real-time 3D positioning pose and the real-time magnetic stripe data; Step S9, correct the pose output by the laser odometry by forcibly constraining the geometric consistency between the real-time vertical deviation distance and the initial vertical deviation distance, and determine the fused final pose.

[0033] The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to embodiments of the present invention can effectively cope with dynamic environmental changes by pre-setting dynamic change areas and adopting different positioning strategies. Outside the dynamic change area, positioning is achieved by registering laser data with a pre-built point cloud map, which improves the positioning accuracy in normal environments. Within the dynamic change area, laser fusion magnetic stripe positioning is adopted. The pose output by the laser odometry is corrected by forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, which can better cope with the interference that occurs in the dynamic change area and achieve stable and reliable navigation and positioning performance.

[0034] According to one embodiment of the present invention, in step S1, a complete and accurate global map is constructed, including magnetic stripe position information. A pose initialization calculation process is performed based on a frame of laser data collected by the lidar and the global map (a pre-constructed point cloud map). For example, iterative nearest-point matching algorithms are used to obtain the robot's current pose on the global map. It should be noted that robot initialization generally needs to be performed in areas with rich environmental features and should not be performed in areas with significant dynamic changes in the environment. After successful initialization, the normal pose calculation process can begin. Dynamically changing areas in the environment are pre-set; for inspection robots operating on a 2D plane, a rectangular area can be used. The rectangular boundary information of the pre-set dynamic changing area, including boundary coordinates, area name, and dynamic change type, is stored in the database of the robot's navigation and positioning system so that it can be quickly and accurately queried and retrieved during robot inspection.

[0035] According to an embodiment of the present invention, in step S2, the lidar provides the robot's position coordinates in the global coordinate system (the coordinate system corresponding to the SLAM map, which is determined when the robot first builds the map) (obtained from the current pose), and compares the position coordinates with the coordinates in the dynamically changing area to determine whether the robot is within the range covered by the dynamically changing area.

[0036] According to an embodiment of the present invention, in step S3, if the current pose is outside the dynamic change area, that is, the robot has not entered the dynamic change area, the current frame of laser data is registered with the pre-constructed point cloud map, that is, matched and aligned. For example, it is implemented by using a matching algorithm such as iterative nearest point. The current frame of laser data is aligned with the point cloud map to obtain its pose relative to the global map, that is, the first positioning pose.

[0037] According to one embodiment of the present invention, in step S4, if the current pose is located in the dynamically changing region, laser fusion magnetic strip positioning is performed, that is, the laser positioning mode is switched to the laser odometry mode, and the magnetic strip data is further fused based on the pose calculated by the laser odometry to obtain the final robot pose (fusion final pose).

[0038] According to one embodiment of the present invention, in step S5, two magnetic stripe sensors are arranged on the robot, respectively at the front and rear ends of the robot. The magnetic stripe sensors should be installed at a suitable position on the bottom of the robot to accurately and stably detect the magnetic stripe signals laid on the ground. If the current pose is in a dynamically changing area, laser fusion magnetic stripe positioning is performed, the magnetic stripe sensors are turned on, and they are put into normal working condition. At this time, the magnetic stripe sensors record the moment when the magnetic stripe signal is detected, and store the relevant information at that moment (e.g., the lateral deviation distance between the center point of the magnetic stripe sensor and the center line of the magnetic stripe in the local coordinate system of the magnetic stripe) as the initial magnetic stripe data. When the robot detects the magnetic stripe signal for the first time, the pose information obtained by the laser odometry in the global coordinate system is the initial 3D positioning pose, which includes the robot's position coordinates (x, y) and initial robot heading angle in the global coordinate system.

[0039] According to an embodiment of the present invention, in step S6, an initial vertical deviation distance is determined based on the initial 3D positioning pose and the initial magnetic strip data.

[0040] Figure 2 An exemplary flowchart for calculating the initial vertical offset distance according to an embodiment of the present invention is shown.

[0041] According to an embodiment of the present invention, step S6 includes: step S61, obtaining the initial center coordinates of the 3D LiDAR center in the global coordinate system at the initial moment based on the initial 3D positioning pose; step S62, obtaining the initial front magnetic intersection coordinates and initial rear magnetic intersection coordinates of the intersection points of the magnetic sensor and the magnetic strip in the global coordinate system at the initial moment based on the initial magnetic strip data; step S63, obtaining the initial magnetic strip direction angle, wherein the initial magnetic strip direction angle is the angle between the magnetic strip direction at the initial moment and the X-axis of the global coordinate system; step S64, determining the initial vertical deviation distance based on the initial center coordinates, the initial front magnetic intersection coordinates, the initial rear magnetic intersection coordinates, and the initial magnetic strip direction angle.

[0042] According to one embodiment of the present invention, based on the initial 3D positioning pose (where the laser odometry is accurate at the initial moment), the initial center coordinates of the 3D laser radar center in the global coordinate system can be obtained, representing the initial position of the laser radar in global space at the initial moment. Using the known initial 3D positioning pose and the magnetic strip sensor installation position, the coordinates of the front and rear magnetic sensor centers in the global coordinate system can be calculated. By determining the lateral deviation distance between the magnetic strip sensor center point and the magnetic strip centerline in the local magnetic strip coordinate system (with the initial center coordinates as the origin and the x-axis aligned with the magnetic strip direction), the intersection point of a straight line drawn laterally along the robot from the current front and rear magnetic sensor center positions with the magnetic strip is determined. For example, if the lateral deviation distance indicates that the sensor center is 0.05 meters to the left, then the intersection point on the magnetic strip is 0.05 meters to the right of the sensor center position. Thus, the coordinates of the intersection points of the front and rear magnetic sensors with the magnetic strip in the global coordinate system at the initial moment are obtained, i.e., the initial front magnetic intersection coordinates and the initial rear magnetic intersection coordinates. The initial magnetic strip direction angle is the angle between the initial magnetic strip direction and the X-axis of the global coordinate system, i.e., ,in, These are the initial pre-magnetic intersection coordinates. These are the initial and subsequent magnetic intersection coordinates. Let be the initial magnetic stripe orientation angle. In a short time, the magnetic stripe is a straight line segment. In a two-dimensional plane scene (assuming that the extension of the magnetic stripe and the robot's movement are mainly in the two-dimensional plane, ignoring the influence of the height direction), at the initial moment (the moment when the robot first detects the magnetic stripe signal), the vertical deviation of the 3D LiDAR center relative to the magnetic stripe path is the initial vertical deviation. The initial vertical deviation reflects the degree of vertical offset of the 3D LiDAR center relative to the magnetic stripe at the initial moment.

[0043] According to an embodiment of the present invention, determining the initial vertical deviation distance based on the initial center coordinates, the initial front magnetic intersection coordinates, the initial rear magnetic intersection coordinates, and the initial magnetic strip direction angle includes: determining the initial vertical deviation distance according to formula (1). , (1), in, As the initial center coordinates, These are the initial pre-magnetic intersection coordinates. These are the initial and subsequent magnetic intersection coordinates. The initial magnetic stripe orientation angle.

[0044] According to an embodiment of the present invention, in formula (1), The coordinates of the midpoint of the magnetic strip formed by the magnetic intersection points before and after the initial moment in the global coordinate system are given, i.e., the initial center point coordinates, representing the positional characteristics of the magnetic strip. The difference between the initial center Y-coordinate and the magnetic strip center point Y-coordinate represents the relative positional relationship between the lidar center and the magnetic strip center in the Y-axis direction at the initial moment. The difference between the X-coordinate of the initial center coordinate and the X-coordinate of the magnetic strip center point represents the relative positional relationship between the lidar center and the magnetic strip center in the X-axis direction at the initial moment. and The cosine and sine values ​​of the initial magnetic stripe direction angle are used to convert the coordinate difference to a component perpendicular to the magnetic stripe direction. To multiply the coordinate difference in the Y-axis direction by The difference in coordinates along the X-axis multiplied by Subtracting the two results gives the vertical distance from the initial center coordinates to the line where the magnetic strip is located, i.e., the initial vertical deviation distance. This indicates that under ideal conditions (no lateral slippage), the geometric relationship should remain constant, i.e., the vertical distance from the radar center to the magnetic strip path.

[0045] In this way, the initial vertical deviation can be determined by the vertical distance from the radar center to the magnetic strip path, which can correct the component of the laser odometry in the direction perpendicular to the magnetic strip, thereby suppressing lateral drift and reducing lateral error in the dynamically changing region.

[0046] According to one embodiment of the present invention, in step S7, when the robot runs continuously in the dynamically changing area, it acquires real-time 3D positioning pose through a laser odometer and real-time magnetic strip data through a magnetic strip sensor.

[0047] According to an embodiment of the present invention, in step S8, the real-time vertical deviation distance is determined based on the real-time 3D positioning pose and the real-time magnetic strip data.

[0048] Figure 3 An exemplary flowchart illustrating the calculation of real-time vertical deviation distance according to an embodiment of the present invention is shown.

[0049] According to an embodiment of the present invention, step S8 includes: step S81, obtaining the real-time center coordinates of the 3D LiDAR center in the global coordinate system based on the real-time 3D positioning pose; step S82, obtaining the real-time front magnetic intersection coordinates and the real-time rear magnetic intersection coordinates of the intersection points of the front and rear magnetic sensors with the magnetic strip in the global coordinate system based on the real-time magnetic strip data; step S83, obtaining the real-time magnetic strip direction angle, wherein the real-time magnetic strip direction angle is the angle between the real-time magnetic strip direction and the X-axis of the global coordinate system; step S84, determining the real-time vertical deviation distance based on the real-time center coordinates, the real-time front magnetic intersection coordinates, the real-time rear magnetic intersection coordinates, and the real-time magnetic strip direction angle.

[0050] According to one embodiment of the present invention, based on the real-time 3D positioning pose (laser odometry has errors in dynamically changing areas), the real-time center coordinates of the 3D lidar center in the global coordinate system at the current moment can be obtained, representing the position of the lidar in global space at the current moment. Using the known real-time 3D positioning pose and the installation position of the magnetic stripe sensor, the coordinates of the front and rear magnetic sensor centers in the global coordinate system can be calculated. By determining the lateral deviation distance between the magnetic stripe sensor center point and the magnetic stripe centerline in the local coordinate system of the magnetic stripe, the intersection point of a straight line drawn laterally along the robot from the current position of the front and rear magnetic sensor centers with the magnetic stripe is determined. For example, if the lateral deviation distance indicates that the sensor center is 0.05 meters to the left, then the intersection point on the magnetic stripe is 0.05 meters to the right of the sensor center position. Thus, the coordinates of the intersection points of the front and rear magnetic sensors with the magnetic stripe in the global coordinate system at the current moment are obtained, i.e., the real-time front magnetic stripe intersection coordinates and the real-time rear magnetic stripe intersection coordinates. The real-time magnetic stripe direction angle is the angle between the current magnetic stripe direction and the X-axis of the global coordinate system, i.e., ,in, For real-time pre-magnetic intersection coordinates, For real-time post-magnetic intersection coordinates, This represents the real-time magnetic stripe orientation angle. The real-time vertical deviation of the 3D LiDAR center relative to the magnetic stripe path at the current moment reflects the degree of vertical offset of the 3D LiDAR center relative to the magnetic stripe at that moment.

[0051] According to an embodiment of the present invention, determining the real-time vertical deviation distance based on the real-time center coordinates, the real-time front magnetic intersection coordinates, the real-time rear magnetic intersection coordinates, and the real-time magnetic stripe orientation angle includes: determining the real-time vertical deviation distance according to formula (2). , (2), in, For real-time center coordinates, For real-time pre-magnetic intersection coordinates, For real-time post-magnetic intersection coordinates, t represents the real-time magnetic stripe orientation angle, and t represents the current time.

[0052] According to an embodiment of the present invention, in formula (2), The coordinates of the midpoint of the magnetic strip formed by the magnetic intersection points before and after the current moment in the global coordinate system are given, i.e., the real-time center point coordinates, representing the positional characteristics of the magnetic strip. The difference between the Y-coordinate of the real-time center coordinate and the Y-coordinate of the magnetic strip center point represents the relative position of the lidar center and the magnetic strip center along the Y-axis at the current moment. The difference between the X-coordinate of the real-time center coordinate and the X-coordinate of the magnetic strip center point represents the relative position of the lidar center and the magnetic strip center along the X-axis at the current moment. and These are the cosine and sine values ​​of the real-time magnetic stripe direction angle, used to convert the coordinate difference into a component perpendicular to the magnetic stripe direction. To multiply the coordinate difference in the Y-axis direction by The difference in coordinates along the X-axis multiplied by Subtracting the two values ​​gives the vertical distance from the real-time center coordinates to the current magnetic stripe's straight line, which is the real-time vertical deviation distance, representing the vertical distance from the radar center to the magnetic stripe path under lateral sliding conditions.

[0053] In this way, the real-time vertical deviation can be determined by the vertical distance from the radar center to the current magnetic strip path. This can adapt to the slight unevenness of the magnetic strip laying. When the robot travels along the curved magnetic strip path, the real-time deviation of the radar center relative to the current magnetic strip segment can also be accurately calculated. This transforms the magnetic strip information from a "static line" to a "dynamic path constraint", improving the adaptability of fusion positioning under complex magnetic strip layouts.

[0054] According to an embodiment of the present invention, in step S9, the cumulative error of the laser odometry along the vertical direction of the magnetic strip is corrected by forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, that is, by forcing the real-time vertical deviation and the initial vertical deviation to be highly consistent in value through algorithm or physical constraints, and the final fusion pose is determined.

[0055] Figure 4 A flowchart illustrating the computation of the final pose fusion according to an embodiment of the present invention is shown.

[0056] According to an embodiment of the present invention, step S9 includes: step S91, obtaining the initial center coordinates of the 3D LiDAR center in the global coordinate system at the initial moment based on the initial 3D positioning pose; step S92, obtaining the real-time magnetic stripe direction angle and the real-time velocity of the robot along the tangent direction of the magnetic stripe; step S93, determining the fused coordinates based on the initial center coordinates, the real-time magnetic stripe direction angle, the real-time velocity, and the initial vertical deviation distance; step S94, obtaining the initial robot heading angle based on the initial 3D positioning pose; step S95, obtaining the initial robot yaw angle by subtracting the initial magnetic stripe direction angle from the initial robot heading angle; step S96, determining the fused direction angle by adding the value of the real-time magnetic stripe direction angle to the initial robot yaw angle; and step S97, determining the final fused pose based on the fused coordinates and the fused direction angle.

[0057] According to one embodiment of the present invention, the robot's actual motion speed along the tangent direction of the magnetic strip is measured using its own speed sensor, i.e., the real-time speed. Based on the robot's motion characteristics, the real-time speed is integrated along the magnetic strip direction, and the coordinates are corrected by combining the initial vertical deviation along the direction perpendicular to the magnetic strip, thus obtaining fused coordinates. The fused coordinates can more accurately describe the robot's actual position in the global coordinate system. From the initial 3D positioning pose, the initial robot heading angle is obtained. By subtracting the initial magnetic strip direction angle from the initial robot heading angle, the initial robot yaw angle is obtained. This initial robot yaw angle describes the degree of deviation of the robot relative to the magnetic strip direction at the initial moment, reflecting the relative relationship between the robot's initial position and the magnetic strip direction. The yaw angle is then corrected. Adding the initial robot yaw angle to the real-time magnetic strip direction angle yields the fused direction angle, i.e., the robot's actual motion direction relative to the global coordinate system at the current moment. The fused coordinates and the fused direction angle are combined to form the fused final pose. The fused final pose contains the robot's position and direction information in the global coordinate system, and can comprehensively and accurately describe the robot's state at the current moment.

[0058] According to an embodiment of the present invention, determining the fusion coordinates based on the initial center coordinates, the real-time magnetic stripe direction angle, the real-time velocity, and the initial vertical deviation distance includes: determining the fusion coordinates according to formula (3). , (3), in, As the initial center coordinates, for Real-time speed at any moment for Real-time magnetic stripe orientation angle at any given moment. This is the real-time magnetic stripe orientation angle. This is the initial vertical offset.

[0059] According to one embodiment of the present invention, in formula (3), The initial center coordinates serve as the starting reference point for the entire coordinate calculation. Integrating the component of the real-time velocity along the X-axis in the global coordinate system from the initial time 0 to the current time t represents the robot's displacement increment along the X-axis. Similarly, The integral of the real-time velocity component along the Y-axis in the global coordinate system from initial time 0 to current time t represents the robot's displacement increment along the Y-axis. Since the initial vertical offset represents the robot's initial vertical position deviation relative to the magnetic strip, it needs to be decomposed based on the magnetic strip orientation angle at the current time to adjust the coordinates to the correct position. In the x-coordinate direction, the correction amount generated by the vertical offset is... The negative sign indicates that the correction direction is opposite to the direction of the coordinate deviation caused by the vertical deviation. In the y-coordinate direction, the correction amount caused by the vertical deviation is... . The x-coordinate of the initial center coordinates Based on this, the robot's displacement increment in the X-axis direction is added, and the correction amount in the X-axis direction due to the initial vertical deviation is subtracted to finally obtain the fused coordinate x-coordinate. , The y-coordinate of the initial center coordinates Based on this, the robot's displacement increment in the Y-axis direction is added, along with the correction amount in the Y-axis direction due to the initial vertical offset, to obtain the fused coordinate's y-coordinate. That is, the fused coordinates are .

[0060] In this way, the fused coordinates can be determined by factors such as the robot's initial position, speed and direction changes during movement, and initial vertical deviation. Real-time speed and real-time magnetic stripe orientation angle reflect the robot's dynamic motion information. By reducing the accumulated error in the lateral direction through the initial vertical deviation, the robot's actual position in the global coordinate system can be more accurately reflected, thereby improving the positioning accuracy and reliability in dynamic environments.

[0061] The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to embodiments of the present invention can effectively cope with dynamic environmental changes by pre-setting dynamic change areas and adopting different positioning strategies. Outside the dynamic change area, positioning is achieved by registering laser data with a pre-built point cloud map, which improves positioning accuracy in normal environments. Within the dynamic change area, laser fusion magnetic stripe positioning is used. By forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, the pose output by the laser odometry is corrected, which can better cope with interference in the dynamic change area and achieve stable and reliable navigation and positioning performance. When determining the initial vertical deviation, the initial vertical deviation can be determined by the vertical distance from the radar center to the magnetic stripe path, which can correct the component of the laser odometry in the direction perpendicular to the magnetic stripe, thereby suppressing lateral drift and reducing lateral errors in the dynamic change area. When determining the real-time vertical offset, the vertical distance from the real-time radar center to the current magnetic strip path can be used. This allows for adaptation to minor unevenness in the magnetic strip layout. Even when the robot travels along a curved magnetic strip path, the real-time deviation of the radar center relative to the current magnetic strip segment can be accurately calculated, elevating the magnetic strip information from a "static line" to a "dynamic path constraint," thus improving the adaptability of fusion positioning in complex magnetic strip layouts. When determining the fusion coordinates, factors such as the robot's initial position, speed and direction changes during movement, and the initial vertical offset can be used. Real-time speed and real-time magnetic strip orientation angle reflect the robot's dynamic motion information. By reducing the accumulated lateral error through the initial vertical offset, the robot's actual position in the global coordinate system can be more accurately reflected, thereby improving the positioning accuracy and reliability in dynamic environments.

[0062] Figure 5 An exemplary block diagram of a 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning system according to an embodiment of the present invention is shown, the system comprising:

[0063] The initialization module is used to obtain the robot's initial pose in the global map for initialization, and to preset the rectangular boundary of at least one dynamically changing region.

[0064] The judgment module is used to determine in real time whether the current pose is located within the dynamically changing area during the robot's inspection process;

[0065] The first positioning pose module is used to register the current frame of laser data with a pre-built point cloud map to obtain the first positioning pose if the current pose is outside the dynamically changing area.

[0066] An execution module is configured to perform laser fusion magnetic stripe positioning if the current pose is located within the dynamically changing region.

[0067] The acquisition module is used to acquire the initial 3D positioning pose in the global coordinate system and the initial magnetic strip data when the robot first detects the magnetic strip signal.

[0068] The initial vertical offset module is used to determine the initial vertical offset based on the initial 3D positioning pose and the initial magnetic strip data.

[0069] The real-time acquisition module is used to acquire real-time 3D positioning pose and real-time magnetic strip data during continuous operation within the dynamically changing area.

[0070] The real-time vertical offset module is used to determine the real-time vertical offset based on the real-time 3D positioning pose and the real-time magnetic strip data.

[0071] The final pose fusion module is used to correct the pose output by the laser odometry by forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, and to determine the final pose fusion.

[0072] This invention can be a method, apparatus, system, and / or computer program product. The computer program product may include a computer-readable storage medium having computer-readable program instructions loaded thereon for performing various aspects of the invention.

[0073] Those skilled in the art should understand that the embodiments of the present invention described above and shown in the accompanying drawings are merely examples and do not limit the present invention. The objectives of the present invention have been fully and effectively achieved. The functions and structural principles of the present invention have been demonstrated and explained in the embodiments, and any variations or modifications may be made to the implementation of the present invention without departing from the stated principles.

Claims

1. A navigation and positioning method for a 3D laser SLAM fusion magnetic stripe inspection robot, characterized in that, include: The robot's initial pose in the global map is obtained for initialization, and at least one rectangular boundary of a dynamically changing region is preset. During the robot's inspection process, it is determined in real time whether the current pose is within the dynamically changing area; If the current pose is outside the dynamically changing area, the current frame of laser data is registered with the pre-constructed point cloud map to obtain the first positioning pose; If the current pose is within the dynamically changing region, then laser fusion magnetic strip positioning is performed; When the robot first detects the magnetic stripe signal, it acquires the initial 3D positioning pose in the global coordinate system and the initial magnetic stripe data. Based on the initial 3D positioning pose and the initial magnetic strip data, determine the initial vertical deviation distance; During continuous operation within the dynamically changing region, real-time 3D positioning pose and real-time magnetic strip data are acquired. Based on the real-time 3D positioning pose and the real-time magnetic strip data, the real-time vertical deviation distance is determined; By forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, the pose output by the laser odometry is corrected, and the final fused pose is determined.

2. The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to claim 1, characterized in that, Based on the initial 3D positioning pose and the initial magnetic strip data, the initial vertical deviation distance is determined, including: Based on the initial 3D positioning pose, obtain the initial center coordinates of the 3D lidar center in the global coordinate system at the initial moment; Based on the initial magnetic strip data, obtain the initial front magnetic intersection coordinates and the initial back magnetic intersection coordinates of the intersection point between the magnetic sensor and the magnetic strip in the global coordinate system at the initial moment. Obtain the initial magnetic stripe orientation angle, wherein the initial magnetic stripe orientation angle is the angle between the magnetic stripe direction and the X-axis of the global coordinate system at the initial moment; The initial vertical deviation distance is determined based on the initial center coordinates, the initial front magnetic intersection coordinates, the initial rear magnetic intersection coordinates, and the initial magnetic strip direction angle.

3. The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to claim 2, characterized in that, The initial vertical deviation distance is determined based on the initial center coordinates, the initial front magnetic intersection coordinates, the initial rear magnetic intersection coordinates, and the initial magnetic strip direction angle, including: according to the formula: ， Determine the initial vertical offset distance ,in, As the initial center coordinates, These are the initial pre-magnetic intersection coordinates. These are the initial and subsequent magnetic intersection coordinates. The initial magnetic stripe orientation angle.

4. The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to claim 1, characterized in that, Based on the real-time 3D positioning pose and the real-time magnetic strip data, the real-time vertical deviation distance is determined, including: Based on the real-time 3D positioning pose, obtain the real-time center coordinates of the 3D LiDAR center in the global coordinate system; Based on the real-time magnetic strip data, obtain the real-time front magnetic intersection coordinates and the real-time rear magnetic intersection coordinates of the intersection points of the front and rear magnetic sensors with the magnetic strip in the global coordinate system. Obtain the real-time magnetic stripe direction angle, wherein the real-time magnetic stripe direction angle is the angle between the real-time magnetic stripe direction and the X-axis of the global coordinate system; The real-time vertical deviation distance is determined based on the real-time center coordinates, the real-time front magnetic intersection coordinates, the real-time rear magnetic intersection coordinates, and the real-time magnetic strip direction angle.

5. The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to claim 4, characterized in that, The real-time vertical deviation distance is determined based on the real-time center coordinates, the real-time front magnetic intersection coordinates, the real-time rear magnetic intersection coordinates, and the real-time magnetic stripe direction angle, including: according to the formula: ， Determine the real-time vertical deviation ,in, For real-time center coordinates, For real-time pre-magnetic intersection coordinates, For real-time post-magnetic intersection coordinates, t represents the real-time magnetic stripe orientation angle, and t represents the current time.

6. The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to claim 1, characterized in that, By forcibly constraining the geometric consistency between the real-time vertical offset and the initial vertical offset, the pose output by the laser odometry is corrected to determine the final fused pose, including: Based on the initial 3D positioning pose, obtain the initial center coordinates of the 3D lidar center in the global coordinate system at the initial moment; The real-time magnetic stripe orientation angle and the real-time velocity of the robot along the tangent direction of the magnetic stripe are obtained; the fused coordinates are determined based on the initial center coordinates, the real-time magnetic stripe orientation angle, the real-time velocity, and the initial vertical deviation. Based on the initial 3D positioning pose, obtain the initial robot heading angle; The initial robot yaw angle is obtained by subtracting the initial magnetic stripe direction angle from the initial robot heading angle; The initial robot yaw angle is added to the real-time magnetic stripe direction angle to determine the fused direction angle; The final fusion pose is determined based on the fusion coordinates and the fusion direction angle.

7. The 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning method according to claim 6, characterized in that, Determining the fused coordinates based on the initial center coordinates, the real-time magnetic stripe orientation angle, the real-time velocity, and the initial vertical deviation distance includes: According to the formula: ， Determine fusion coordinates ,in, As the initial center coordinates, for Real-time speed at any moment for Real-time magnetic stripe orientation angle at any given moment. This is the real-time magnetic stripe orientation angle. This is the initial vertical offset.

8. A 3D laser SLAM fusion magnetic stripe inspection robot navigation and positioning system, used to perform the method as described in any one of claims 1-7, characterized in that, include: The initialization module is used to obtain the robot's initial pose in the global map for initialization, and to preset the rectangular boundary of at least one dynamically changing region. The judgment module is used to determine in real time whether the current pose is located within the dynamically changing area during the robot's inspection process; The first positioning pose module is used to register the current frame of laser data with a pre-built point cloud map to obtain the first positioning pose if the current pose is outside the dynamically changing area. An execution module is configured to perform laser fusion magnetic stripe positioning if the current pose is located within the dynamically changing region. The acquisition module is used to acquire the initial 3D positioning pose in the global coordinate system and the initial magnetic strip data when the robot first detects the magnetic strip signal. The initial vertical offset module is used to determine the initial vertical offset based on the initial 3D positioning pose and the initial magnetic strip data. The real-time acquisition module is used to acquire real-time 3D positioning pose and real-time magnetic strip data during continuous operation within the dynamically changing area. The real-time vertical offset module is used to determine the real-time vertical offset based on the real-time 3D positioning pose and the real-time magnetic strip data. The final pose fusion module is used to correct the pose output by the laser odometry by forcibly constraining the geometric consistency between the real-time vertical deviation and the initial vertical deviation, and to determine the final pose fusion.

Images