Robot positioning method, system, medium, and apparatus based on retro-reflective posts

Abstract

The application relates to the technical field of robot positioning, and discloses a robot positioning method, system, medium and equipment based on a light column, which comprises the following steps: acquiring a map when a mobile robot moves, configuring the mobile robot with a laser radar and an odometer; combining map and pose transformation data obtained by the odometer to predict the pose of the mobile robot, performing Gaussian distribution sampling on the predicted pose, comparing the matching effect of point clouds obtained by the laser radar and the light column in the map under each sampling particle, and selecting the predicted pose with the best matching effect as the optimal sampling pose; performing matching verification on the matching result, and if the matching is successful, solving the final optimized pose of the mobile robot according to the optimal sampling pose, and if the matching fails, performing Gaussian distribution sampling again. The application can effectively utilize the prior information of the laser radar and the odometer, reduce the false matching, and improve the accuracy and efficiency of finding the real light column.

Description

Technical Field

[0001] This invention relates to the field of robot positioning technology, and in particular to a robot positioning method, system, medium, and device based on reflective columns. Background Technology

[0002] Localization is one of the key technologies for achieving autonomous navigation and path planning in mobile robots. Among existing technologies, reflective column-based localization is a commonly used method for mobile robot localization and has been widely applied in the field. Its principle is to determine the robot's position by identifying and tracking reflective columns using a lidar mounted on the mobile robot.

[0003] Real-time tracking of reflective pillars is fundamental to robot localization during movement, thus requiring precise measurement and matching of their positions. However, traditional reflective pillar tracking methods are often affected by interference from highly reflective objects such as glass and mirrors, leading to inaccurate measurement results. Traditional tracking methods typically search for reflective pillars in a global coordinate map through position tracking, which has several problems: First, it doesn't fully utilize prior information from LiDAR and odometry during the global search; second, when the robot moves at high speeds, the proportion of falsely detected reflective pillars increases, potentially preventing the identification of true reflective pillars; third, the detected true reflective pillars are not validated, especially when interfered with by highly reflective objects like glass and mirrors, reducing matching accuracy and affecting localization results. Summary of the Invention

[0004] Therefore, the technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a robot positioning method, system, medium and device based on reflective pillars, which can effectively utilize the prior information of lidar and odometer, reduce false matching and improve the accuracy and efficiency of finding the real reflective pillars.

[0005] To address the aforementioned technical problems, this invention provides a robot positioning method based on reflective pillars, comprising:

[0006] Acquire a map of the mobile robot as it moves, configure the mobile robot with a LiDAR and an odometer, the LiDAR acquires the real-time point cloud of the mobile robot as it moves, and the odometer acquires the real-time pose change of the mobile robot as it moves.

[0007] The pose of the mobile robot is predicted by combining the map and pose transformation data. The predicted pose of the mobile robot is sampled by Gaussian distribution to obtain multiple Gaussian distribution sampling particles. The matching effect of the point cloud under each sampling particle and the reflective column in the map is compared. The predicted pose of the mobile robot corresponding to the sampling particle with the best matching effect is taken as the optimal sampling pose.

[0008] The matching result of the sampled particle with the best matching effect is checked. If the matching is successful, the final optimized pose of the mobile robot is solved according to the optimal sampling pose. If the matching fails, Gaussian distribution sampling is performed again.

[0009] When solving for the final optimized pose of the mobile robot based on the optimal sampled pose, an objective function for the pose of the mobile robot is constructed and iteratively optimized using the optimal sampled pose of the mobile robot as the initial value. The final optimized pose of the mobile robot is then obtained and used as the localization result of the mobile robot.

[0010] In one embodiment of the present invention, the step of performing Gaussian distribution sampling on the predicted pose of the mobile robot to obtain multiple Gaussian distribution sampling particles specifically involves:

[0011] The predicted pose of the mobile robot is converted into translation and angle. The Gaussian distribution mean is used to select the translation and angle of the current predicted position. Multiple Gaussian distribution sampling particles are randomly selected according to the preset standard deviation of the Gaussian distribution.

[0012] In one embodiment of the present invention, the step of comparing the matching effect of the point cloud under each sampled particle and the reflective pillars in the map, and taking the predicted pose of the mobile robot corresponding to the sampled particle with the best matching effect as the optimal sampled pose, includes:

[0013] Clustering the point cloud under each sampled particle yields multiple point cloud clusters. The center coordinates of the point cloud clusters in the lidar coordinate system are used as the first position coordinates reflecting the reflective column detected by the lidar. For each sampled particle, the first position coordinates are projected onto the map coordinate system to obtain the second position coordinates.

[0014] The center coordinates of the reflective pillars are extracted from the map as the third position coordinates reflecting the reflective pillars. The second position coordinates and the third position coordinates are matched to obtain multiple matching points of the reflective pillars. The matching score of the corresponding sampled particles is calculated based on the matching points of the multiple reflective pillars.

[0015] The sampled particle with the lowest matching score is selected as the final sampled particle. The matching points of the multiple reflective pillars corresponding to the final sampled particle are taken as the final set of reflective pillar matching points. The predicted pose of the mobile robot corresponding to the final sampled particle is taken as the optimal sampling pose.

[0016] In one embodiment of the present invention, the second position coordinates and the third position coordinates are matched to obtain multiple matching points of reflective pillars, and the matching score of the corresponding sampled particles is calculated based on the matching points of the multiple reflective pillars, including:

[0017] For each second position coordinate, calculate the distance between that second position coordinate and all third position coordinates, and select the third position coordinate with the smallest distance that meets the preset threshold as the matching point of the reflector corresponding to that second position coordinate; use the same method to obtain multiple matching points of reflectors corresponding to each second position coordinate;

[0018] The distance between the second and third position coordinates of the matching point of each reflective column is calculated as the matching distance, and the average of all matching distances is used as the matching score of the corresponding sampled particle.

[0019] In one embodiment of the present invention, the matching verification of the matching result under the sampling particle with the best matching effect includes:

[0020] In the final set of reflector matching points, select the two points furthest apart in the third position coordinates. Choose one of these two points as the origin of the coordinate system. Establish a first local coordinate system with the direction of the line connecting these two points as the X-axis and the direction perpendicular to the line as the Y-axis. In the final set of reflector matching points, select the two points furthest apart in the first position coordinates. Choose the point among these two points that corresponds to the origin of the first local coordinate system as the origin of the coordinate system. Establish a second local coordinate system with the direction of the line connecting these two points as the X-axis and the direction perpendicular to the line as the Y-axis.

[0021] Calculate the positions of the final set of reflector matching points in the first and second local coordinate systems, and verify whether the reflector matching points in the final set of reflector matching points are successfully matched.

[0022] In one embodiment of the present invention, calculating the position of the final set of reflective column matching points in the first local coordinate system and the second local coordinate system, and verifying whether the reflective column matching points in the final set of reflective column matching points are successfully matched, includes:

[0023] Calculate the pose transformation matrix of the first local coordinate system relative to the map coordinate system, denoted as . m T local0 Calculate the pose transformation matrix of the second local coordinate system relative to the lidar coordinate system, denoted as . l T local1 ;

[0024] Calculate the position coordinates of all reflector matching points in the final reflector matching point set in the first local coordinate system, and denot them as the first local position coordinates; calculate the position coordinates of the first position coordinates of all reflector matching points in the final reflector matching point set in the second local coordinate system, and denot them as the second local position coordinates.

[0025] Calculate the distance difference between the first local position coordinates and the second local position coordinates:

[0026]

[0027] Where local_dist is the distance difference, dx is the difference along the x-axis, and dy is the difference along the y-axis. The calculation methods for dx and dy are as follows:

[0028]

[0029] Where x represents the coordinates of the first position corresponding to the matching point of the reflector in the final set of reflector matching points in the x-direction of the lidar coordinate system; y represents the coordinates of the first position corresponding to the matching point of the reflector in the final set of reflector matching points in the y-direction of the lidar coordinate system; x m This represents the x-coordinate of the final reflector matching point set in the map coordinate system; y-coordinate represents the x-coordinate of the reflector matching point. m This represents the y-coordinate of the final reflector matching point set in the map coordinate system. -1 This represents the matrix inversion operation;

[0030] The distance difference local_dist is compared with a preset threshold. If the preset threshold is met, the match is successful; otherwise, the match fails.

[0031] In one embodiment of the present invention, the objective function for constructing the pose of the mobile robot is iteratively optimized using the optimal sampled pose of the mobile robot as the initial value to obtain the final optimized pose of the mobile robot, including:

[0032] The objective function for constructing the pose of the mobile robot is:

[0033]

[0034] The pose of the mobile robot is represented as (p x p y ,θ),p x p represents the x-coordinate of the mobile robot in the map coordinate system. y The x-coordinate represents the y-coordinate of the mobile robot in the map coordinate system, and θ represents the rotation angle of the mobile robot in the map coordinate system; q It is the coordinate of the first position of the q-th reflector matching point in the final set of reflector matching points, in the x-direction of the lidar coordinate system, and the y-direction of the first position. q It is the coordinate of the first position of the q-th reflector matching point in the final set of reflector matching points, in the y-direction of the lidar coordinate system, x′. qmIt is the x-coordinate and y′ of the q-th reflector match point in the final reflector match point set in the map coordinate system. qm It represents the y-coordinate of the q-th reflector in the final set of reflector matching points in the map coordinate system; Q represents the number of reflector matching points in the final set of reflector matching points. Represents the square of the L2 norm;

[0035] Using the optimal sampled pose as the initial value, the CERES optimization library is used to iteratively optimize the pose, and the pose of the mobile robot with the minimum objective function is taken as the final optimized pose.

[0036] The present invention also provides a robot positioning system based on reflective pillars, comprising:

[0037] A mobile robot is equipped with a lidar and an odometer. The lidar acquires real-time point cloud data of the mobile robot as it moves, and the odometer acquires real-time pose changes of the mobile robot as it moves.

[0038] The map acquisition module is used to acquire the map as the mobile robot moves.

[0039] The Gaussian distribution sampling module is used to predict the pose of the mobile robot by combining the map and pose transformation data, and to perform Gaussian distribution sampling on the predicted pose of the mobile robot to obtain multiple Gaussian distribution sampling particles.

[0040] The optimal sampling pose calculation module is used to compare the matching effect between the point cloud under each sampling particle and the reflective pillars in the map, and take the predicted pose of the mobile robot corresponding to the sampling particle with the best matching effect as the optimal sampling pose.

[0041] The matching verification module is used to verify the matching results of the sampled particles with the best matching effect. If the matching is successful, the final optimized pose of the mobile robot is solved according to the optimal sampling pose. If the matching fails, Gaussian distribution sampling is performed again.

[0042] The mobile robot localization module is used to solve for the final optimized pose of the mobile robot based on the optimal sampled pose, including: constructing an objective function for the pose of the mobile robot and iteratively optimizing it using the optimal sampled pose of the mobile robot as the initial value, solving for the final optimized pose of the mobile robot, and using the final optimized pose as the localization result of the mobile robot.

[0043] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned robot positioning method based on reflective pillars.

[0044] The present invention also provides a robot positioning device based on reflective pillars, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the robot positioning method based on reflective pillars.

[0045] The technical solution of the present invention has the following advantages compared with the prior art:

[0046] 1. This invention uses prior pose information of mobile robots obtained by odometry, combined with Gaussian particle sampling, to select the pose of the mobile robot with the best matching effect as the optimal sampling pose, thereby expanding the search range of prediction and improving the accuracy of matching.

[0047] 2. This invention reduces interference from highly reflective objects by performing matching verification on the matching results, thereby reducing the occurrence of false matches and improving the accuracy of finding the true reflective column.

[0048] 3. This invention fuses reflective pillars and laser point clouds during the positioning process, eliminating the need to switch positioning algorithms between reflective pillar areas and non-reflective pillar areas, thereby improving the smoothness and consistency of the overall positioning and increasing positioning efficiency. Attached Figure Description

[0049] To make the content of this invention easier to understand, the invention will be further described in detail below with reference to specific embodiments and accompanying drawings, wherein:

[0050] Figure 1 This is a flowchart of the method of the present invention.

[0051] Figure 2 This is an example of a pairing anomaly that occurs when the mobile robot rotates rapidly in an embodiment of the present invention.

[0052] Figure 3 This is an example of a point cloud cluster corresponding to a reflective column matching point in an embodiment of the present invention successfully matching with a paired reflective column.

[0053] Figure 4 This is an example of a failure to match the point cloud cluster corresponding to the reflective column matching point with the paired reflective column in an embodiment of the present invention. Detailed Implementation

[0054] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0055] Example 1

[0056] Reference Figure 1As shown, this invention discloses a robot localization method based on reflective pillars, comprising the following steps:

[0057] S1: Obtain a map of the mobile robot during its movement, and configure the mobile robot with a LiDAR and an odometer. In this embodiment, the odometer can be a wheeled odometer, a laser odometer, etc. The LiDAR acquires a real-time point cloud of the mobile robot during its movement, and the point cloud includes at least distance information, angle information, and intensity information. The odometer acquires the real-time pose changes of the mobile robot during its movement.

[0058] S2: Combining the map and pose transformation data, the predicted pose of the mobile robot is:

[0059] m T b = m T b' × o T b' -1 × o T b ,in, m T b This represents the predicted pose of the mobile robot in the map coordinate system at the current moment. m T b' This represents the pose of the mobile robot in the map coordinate system at the previous moment. o T b' This represents the pose of the mobile robot at the previous moment, as acquired by the odometry. o T b' -1 Representation matrix o T b' Find the reverse. o T b This represents the pose transformation matrix of the robot relative to the odometry coordinate system at the current moment, as acquired by the odometry. Therefore, to obtain the predicted pose of the mobile robot at any given moment, it is necessary to obtain the pose of the mobile robot at its initial moment. In this embodiment, the initial pose of the mobile robot in the map coordinate system is obtained through manual initialization of positioning or by reading the positioning results from the configuration file. The configuration file stores the positioning results at the moment the mobile robot is powered off.

[0060] S3: Gaussian distribution sampling is performed on the predicted pose of the mobile robot to obtain multiple Gaussian distribution sampling particles. The matching effect of the point cloud and the reflective pillars in the map under each sampling particle is compared. The predicted pose of the mobile robot corresponding to the sampling particle with the best matching effect is taken as the optimal sampling pose.

[0061] S3-1: The predicted mobile robot pose is sampled using a Gaussian distribution to obtain multiple Gaussian distribution sampling particles. Specifically, the predicted mobile robot pose is converted into translation and angle. The Gaussian distribution mean is selected based on the translation and angle of the current predicted position. Multiple Gaussian distribution sampling particles are randomly selected according to a preset Gaussian distribution standard deviation. In this embodiment, the translation standard deviation is set to 0.025 meters, and the angle standard deviation is set to 0.18 degrees. The number of Gaussian distribution sampling particles is set to 10, corresponding to 10 predicted poses. In practical applications, the Gaussian distribution standard deviation and the number of Gaussian distribution sampling particles are set according to the noise of the odometer. A larger standard deviation range and a larger number of sampling particles result in higher accuracy for subsequent reflective post recognition and pairing.

[0062] S3-2: Compare the matching effect between the point cloud of each sampled particle and the reflective pillars in the map, and take the predicted pose of the mobile robot corresponding to the sampled particle with the best matching effect as the optimal sampled pose.

[0063] S3-2-1: Cluster the point cloud based on its intensity information to obtain multiple point cloud clusters, and then assign the center coordinates of each cluster to the LiDAR coordinate system. This serves as the first position coordinate reflecting the reflective column detected by the lidar. Let be the first position coordinate of the i-th point cloud cluster, and n represent the number of point cloud clusters obtained by point cloud clustering.

[0064] S3-2-2: For each sampled particle, project the first position coordinates onto the map coordinate system to obtain the second position coordinates of each point cloud cluster in the map coordinate system. The second position coordinates are... The coordinates of the i-th second position are represented by the following method:

[0065]

[0066] in, m T b This represents the predicted pose of the mobile robot in the map coordinate system at the current moment. b T l It refers to the pose of the lidar relative to the coordinate system of the mobile robot. b T l Based on the external parameters acquired by the lidar, in practical applications, the data is generally acquired and stored in advance to form preset data. This represents the x-coordinate value of the second position obtained by projecting the i-th first position coordinate from the lidar coordinate system to the map coordinate system. This represents the y-coordinate value of the second position coordinate of the i-th point cloud cluster, obtained by projecting the first position coordinate of the i-th point cloud cluster from the lidar coordinate system to the map coordinate system. This represents the x-coordinate value of the i-th first position in the lidar coordinate system. This represents the y-coordinate value of the i-th first position coordinate in the lidar coordinate system.

[0067] S3-2-3: The map for the mobile robot's movement is obtained through pre-loading, and the pre-loaded map contains at least the actual position coordinates of the reflective pillars. The center coordinates of the reflective pillars are extracted from the map as the third position coordinates reflecting the reflective pillars. Let be the coordinates of the i-th third position, and m represent the number of reflective pillars obtained from the map.

[0068] S3-2-4: Match the second position coordinates and the third position coordinates to obtain the matching points of multiple reflective pillars, and calculate the matching score of the corresponding sampled particles based on the matching points of the multiple reflective pillars.

[0069] S3-2-4-1: For each second position coordinate, calculate the distance Dist between that second position coordinate and all third position coordinates. ij Dist ij This represents the distance between the i-th second position coordinate and the j-th third position coordinate. In this embodiment, Euclidean distance is used. From the third position coordinates, the one with the smallest distance and a distance that meets a preset threshold is selected as the matching point of the reflector corresponding to the second position coordinate. In this embodiment, the one with the smallest distance and a distance less than the preset threshold d is selected as the matching point of the reflector corresponding to the second position coordinate. The same method is used to obtain the matching points of multiple reflectors corresponding to each second position coordinate.

[0070] S3-2-4-2: Calculate the distance between the second and third position coordinates of the matching point for each reflective column as the matching distance. The average of all matching distances is taken as the matching score of the corresponding sampled particle. That is, the matching score of the sampled particle is:

[0071]

[0072] Among them, Dist i Dist represents the matching score of the i-th particle. min，k This represents the distance between the second and third position coordinates of the matching point of the k-th reflector, which is the distance with the minimum distance and the distance meeting the preset threshold when selecting a matching point. K represents the number of matching points of the obtained reflector.

[0073] S3-2-5: Select the sampling particle with the lowest matching score as the final sampling particle, and use the matching points of multiple reflective pillars corresponding to the final sampling particle as the final reflective pillar matching point set. Use the predicted pose of the mobile robot corresponding to the final sampling particle as the optimal sampling pose. When the mobile robot rotates rapidly, inaccurate odometry angles can cause issues such as… Figure 2 The pairing anomaly shown leads to positioning errors. However, using the particle sampling method of this invention, even if the mobile robot rotates rapidly and the odometry prediction angle error increases, a relatively accurate pairing effect can still be achieved.

[0074] S4: Perform a matching verification on the matching result of the sampled particle with the best matching effect. If the matching is successful, proceed to S5 to solve the final optimized pose of the mobile robot based on the optimal sampling pose. If the matching fails, return to step S3 to re-perform Gaussian distribution sampling.

[0075] S4-1: Select the two points that are farthest apart in the third position coordinate of the final set of reflector matching points, select one of the two reflector matching points as the origin of the coordinate system, and establish the first local coordinate system with the direction of the line connecting the two points as the X-axis direction and the direction perpendicular to the line connecting as the Y-axis direction.

[0076] In the first position coordinates corresponding to the final reflector matching point set (i.e., the point cloud clusters in S1 corresponding to all the third position coordinates in the final reflector matching point set), select the two point cloud clusters that are farthest apart. Select the position coordinates of the point in these two points that corresponds to the origin of the first local coordinate system as the origin of the coordinate system. Establish the second local coordinate system by taking the direction of the line connecting these two points as the X-axis direction and the direction perpendicular to the line connecting as the Y-axis direction.

[0077] S4-2: Calculate the positions of the final set of reflector matching points in the first and second local coordinate systems, and verify whether the reflector matching points in the final set of reflector matching points are successfully matched. Specifically:

[0078] S4-2-1: Calculate the pose transformation matrix of the first local coordinate system relative to the map coordinate system, denoted as... m T local0 .

[0079] S4-2-1-1: Obtain the center coordinates (x, y) of the reflector pillar, which serves as the origin of the first local coordinate system, within the map coordinate system. local0 ,y local0 ), obtain the center coordinates (x, y) of the reflector corresponding to the other third position coordinate selected in step 4-1. v ,y v ).

[0080] S4-2-1-2: Calculate (x) local0 ,y local0 ) and (x v ,y v The angle of the line connecting the two sides is:

[0081]

[0082] S4-2-1-3: Establish the first pose transformation as (x local0 ,y local0 ,yaw0), converting the first pose transformation into a rotation matrix form to obtain m T local0 .

[0083] S4-2-2: Calculate the pose transformation matrix of the second local coordinate system relative to the lidar coordinate system, denoted as... l T local1 .

[0084] S4-2-2-1: Obtain the center coordinates (x, y) of the reflector column, which serves as the origin of the second local coordinate system, within the lidar coordinate system. local1 ,y local1 ), obtain the center coordinates (x, y) of the reflector corresponding to the other first position coordinate selected in step 4-1. u ,y u ).

[0085] S4-2-2-2: Calculate (x) local1 ,y local1 ) and (x u ,y u The angle of the line connecting the two sides is:

[0086]

[0087] S4-2-2-3: Establish the second pose transformation as (x local1 ,y local1 ,yaw1), converting the second pose transformation into a rotation matrix form to obtain l T local1 .

[0088] S4-2-3: Calculate the position coordinates of all reflector matching points in the final reflector matching point set (the reflector matching points in the final reflector matching point set are the third position coordinates, i.e., the position coordinates in the corresponding map coordinate system) in the first local coordinate system, and denot them as the first local position coordinates; calculate the position coordinates of the first position coordinates corresponding to all reflector matching points in the final reflector matching point set (i.e., the point cloud clusters in S1 corresponding to all the third position coordinates in the final reflector matching point set) in the second local coordinate system, and denot them as the second local position coordinates.

[0089] Calculate the distance difference between the first local position coordinates and the second local position coordinates:

[0090]

[0091] Where local_dist is the distance difference, dx is the difference along the x-axis, and dy is the difference along the y-axis. The calculation methods for dx and dy are as follows:

[0092]

[0093] Where x represents the coordinates of the first position corresponding to the matching point of the reflector in the final set of reflector matching points in the x-direction of the lidar coordinate system; y represents the coordinates of the first position corresponding to the matching point of the reflector in the final set of reflector matching points in the y-direction of the lidar coordinate system; x m This represents the x-coordinate of the final reflector matching point set in the map coordinate system; y-coordinate represents the x-coordinate of the reflector matching point. m This represents the y-coordinate of the final reflector matching point set in the map coordinate system. -1 This represents the matrix inversion operation.

[0094] S4-2-4: Compare the distance difference local_dist with a preset threshold. If the preset threshold is met, the corresponding reflective column matching point is successfully matched; otherwise, the matching fails, and the corresponding reflective column matching point is noise. In this embodiment, the preset threshold is relocate_match_tolerance. On surfaces such as glass or mirrors, due to the reflection effect, the laser will also produce high intensity when it hits them. By setting relocate_match_tolerance, these noise points can be filtered out. If the distance difference local_dist is less than relocate_match_tolerance, it means that the point cloud cluster (i.e., the first position coordinate) (x, y) corresponding to the reflective column matching point matches the paired reflective column (x, y). m ,y m Match successful, such as Figure 3As shown; otherwise, the point cloud cluster (x, y) corresponding to the reflected column matching point is noise. Examples of noise points that can be detected using the local coordinate system are shown below. Figure 4 As shown.

[0095] S5: Construct the objective function of the mobile robot's pose and iteratively optimize it using the optimal sampled pose of the mobile robot as the initial value to obtain the final optimized pose of the mobile robot. Use the final optimized pose as the localization result of the mobile robot.

[0096] S5-1: The objective function for constructing the pose of the mobile robot is:

[0097]

[0098] The pose of the mobile robot is represented as (p x p y ,θ),p x p represents the x-coordinate of the mobile robot in the map coordinate system. y The x-coordinate represents the y-coordinate of the mobile robot in the map coordinate system, and θ represents the rotation angle of the mobile robot in the map coordinate system; q It is the coordinate of the first position (i.e., the point cloud cluster) corresponding to the q-th reflector matching point in the final reflector matching point set, in the x-direction coordinate of the lidar coordinate system, and y-direction coordinate of the first position (i.e., the point cloud cluster). q It is the coordinate of the first position of the q-th reflector matching point in the final set of reflector matching points, in the y-direction of the lidar coordinate system, x′. qm It is the x-coordinate and y′ of the q-th reflector matching point (i.e., the third position) in the final reflector matching point set in the map coordinate system. qm It represents the y-coordinate of the q-th reflector in the final set of reflector matching points in the map coordinate system; Q represents the number of reflector matching points in the final set of reflector matching points. This represents the square of the L2 norm.

[0099] S5-2: Using the optimal sampled pose as the initial value, the CERES optimization library is used to iteratively optimize the pose until the objective function is minimized, at which point the robot's pose is taken as the final optimized pose. The localization results of the mobile robot are obtained.

[0100] Example 2

[0101] This invention also discloses a robot positioning system based on reflective pillars, comprising:

[0102] A mobile robot is equipped with a lidar and an odometer. The lidar acquires real-time point cloud data of the mobile robot as it moves, and the odometer acquires real-time pose changes of the mobile robot as it moves.

[0103] The map acquisition module is used to acquire the map as the mobile robot moves.

[0104] The Gaussian distribution sampling module is used to predict the pose of the mobile robot by combining the map and pose transformation data, and to perform Gaussian distribution sampling on the predicted pose of the mobile robot to obtain multiple Gaussian distribution sampling particles.

[0105] The optimal sampling pose calculation module is used to compare the matching effect between the point cloud under each sampling particle and the reflective pillars in the map, and take the predicted pose of the mobile robot corresponding to the sampling particle with the best matching effect as the optimal sampling pose.

[0106] The matching verification module is used to verify the matching results of the sampled particles with the best matching effect. If the matching is successful, the final optimized pose of the mobile robot is solved according to the optimal sampling pose. If the matching fails, Gaussian distribution sampling is performed again.

[0107] The mobile robot localization module is used to continuously update the optimal sampled pose when solving for the final optimized pose of the mobile robot based on the optimal sampled pose, construct the objective function of the mobile robot's pose and solve for the final optimized pose of the mobile robot, and use the final optimized pose as the localization result of the mobile robot.

[0108] Example 3

[0109] The present invention also discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the robot positioning method based on reflective pillars in Embodiment 1.

[0110] Example 4

[0111] The present invention also discloses a device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the robot positioning method based on reflective posts in Embodiment 1.

[0112] This invention uses prior pose information of a mobile robot obtained by odometry, combined with Gaussian particle sampling, to select particles with the smallest average error for tracking and matching reflective pillars, thereby expanding the search range of prediction and improving the accuracy of matching.

[0113] This invention, based on local coordinate system verification, increases the accuracy of real-time tracking of reflective pillars, reduces interference from highly reflective objects, decreases the occurrence of false matches, and improves the accuracy of finding the true reflective pillar.

[0114] In existing technologies, mobile robots commonly use the SLAM algorithm for 2D laser localization. This 2D laser localization employs optimization methods from the Cartographer algorithm, involving the calculation of map matching residuals and odometry prior value residuals. However, this invention, when optimizing the robot's pose, can fuse the 2D laser localization results with the actual localization output. This invention adds reflector residuals and map matching residuals to the optimization equations, fusing reflector and 2D laser localization results in areas with reflectors, and outputting 2D laser localization results in areas without reflectors. This tightly coupled method eliminates the need to define reflector regions for switching, thereby improving the overall smoothness and consistency of localization and increasing localization efficiency.

[0115] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0116] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0117] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0118] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0119] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A robot positioning method based on reflective pillars, characterized in that, include: Acquire a map of the mobile robot as it moves, configure the mobile robot with a LiDAR and an odometer, the LiDAR acquires the real-time point cloud of the mobile robot as it moves, and the odometer acquires the real-time pose change of the mobile robot as it moves. The pose of the mobile robot is predicted by combining the map and pose transformation data. The predicted pose of the mobile robot is sampled by Gaussian distribution to obtain multiple Gaussian distribution sampling particles. The matching effect of the point cloud under each sampling particle and the reflective column in the map is compared. The predicted pose of the mobile robot corresponding to the sampling particle with the best matching effect is taken as the optimal sampling pose. The matching result of the sampled particle with the best matching effect is checked. If the matching is successful, the final optimized pose of the mobile robot is solved according to the optimal sampling pose. If the matching fails, Gaussian distribution sampling is performed again. When solving for the final optimized pose of the mobile robot based on the optimal sampled pose, an objective function for the pose of the mobile robot is constructed and iteratively optimized using the optimal sampled pose of the mobile robot as the initial value. The final optimized pose of the mobile robot is then obtained and used as the localization result of the mobile robot.

2. The robot positioning method based on reflective pillars according to claim 1, characterized in that: The process of sampling the predicted pose of the mobile robot using a Gaussian distribution to obtain multiple Gaussian-distributed sampled particles is as follows: The predicted pose of the mobile robot is converted into translation and angle. The Gaussian distribution mean is used to select the translation and angle of the current predicted position. Multiple Gaussian distribution sampling particles are randomly selected according to the preset standard deviation of the Gaussian distribution.

3. The robot positioning method based on reflective pillars according to claim 1, characterized in that: The step of comparing the matching effect between the point cloud under each sampled particle and the reflective pillars in the map, and taking the predicted pose of the mobile robot corresponding to the sampled particle with the best matching effect as the optimal sampled pose, includes: Clustering the point cloud under each sampled particle yields multiple point cloud clusters. The center coordinates of the point cloud clusters in the lidar coordinate system are used as the first position coordinates reflecting the reflective column detected by the lidar. For each sampled particle, the first position coordinates are projected onto the map coordinate system to obtain the second position coordinates. The center coordinates of the reflective pillars are extracted from the map as the third position coordinates reflecting the reflective pillars. The second position coordinates and the third position coordinates are matched to obtain multiple matching points of the reflective pillars. The matching score of the corresponding sampled particles is calculated based on the matching points of the multiple reflective pillars. The sampled particle with the lowest matching score is selected as the final sampled particle. The matching points of the multiple reflective pillars corresponding to the final sampled particle are taken as the final set of reflective pillar matching points. The predicted pose of the mobile robot corresponding to the final sampled particle is taken as the optimal sampling pose.

4. The robot positioning method based on reflective pillars according to claim 3, characterized in that: The second and third position coordinates are matched to obtain multiple matching points for the reflective pillars. The matching score for the corresponding sampled particle is calculated based on these matching points, including: For each second position coordinate, calculate the distance between that second position coordinate and all third position coordinates, and select the third position coordinate with the smallest distance that meets the preset threshold as the matching point of the reflector corresponding to that second position coordinate; use the same method to obtain multiple matching points of reflectors corresponding to each second position coordinate; The distance between the second and third position coordinates of the matching point of each reflective column is calculated as the matching distance, and the average of all matching distances is used as the matching score of the corresponding sampled particle.

5. The robot positioning method based on reflective pillars according to claim 3, characterized in that: The matching verification of the matching results under the sampled particle with the best matching effect includes: In the final set of reflector matching points, select the two points furthest apart in the third position coordinates. Choose one of these two points as the origin of the coordinate system, and use the direction of the line connecting these two points as the coordinate origin. The axial direction and the direction perpendicular to the connecting line are used as Establish a first local coordinate system along the axis; select the two points furthest apart from the first position coordinates corresponding to the final set of reflector matching points, and choose the point between these two points that corresponds to the origin of the first local coordinate system as the origin of the coordinate system. Use the direction of the line connecting these two points as the coordinate origin. The axial direction and the direction perpendicular to the connecting line are used as Establish a second local coordinate system along the axial direction; Calculate the positions of the final set of reflector matching points in the first and second local coordinate systems, and verify whether the reflector matching points in the final set of reflector matching points are successfully matched.

6. The robot positioning method based on reflective pillars according to claim 5, characterized in that: The calculation of the final set of reflective column matching points in the first and second local coordinate systems, and the verification of whether the reflective column matching points in the final set of reflective column matching points are successfully matched, includes: Calculate the pose transformation matrix of the first local coordinate system relative to the map coordinate system, denoted as . Calculate the pose transformation matrix of the second local coordinate system relative to the lidar coordinate system, denoted as . ; Calculate the position coordinates of all reflector matching points in the final reflector matching point set in the first local coordinate system, and denot them as the first local position coordinates; calculate the position coordinates of the first position coordinates of all reflector matching points in the final reflector matching point set in the second local coordinate system, and denot them as the second local position coordinates. Calculate the distance difference between the first local position coordinates and the second local position coordinates: ， in, This is the distance difference. In order to be in Difference in the axial direction, In order to be in Difference in the axial direction, , The calculation method is as follows: ， in, This represents the coordinates of the first position of the matching point of the reflector in the final set of matching points in the lidar coordinate system. Coordinates of direction; This represents the coordinates of the first position of the matching point of the reflector in the final set of matching points in the lidar coordinate system. Coordinates of direction; This indicates the coordinates of the reflector matching points in the final set of reflector matching points in the map coordinate system. Coordinates of direction; This indicates the coordinates of the reflector matching points in the final set of reflector matching points in the map coordinate system. Directional coordinates This represents the matrix inversion operation; Distance difference The system compares the result with a preset threshold. If the preset threshold is met, the match is successful; otherwise, the match fails.

7. The robot localization method based on reflective pillars according to any one of claims 1-6, characterized in that: The objective function for constructing the pose of the mobile robot is iteratively optimized using the optimal sampled pose of the mobile robot as the initial value, and the final optimized pose of the mobile robot is obtained by solving the problem, including: The objective function for constructing the pose of the mobile robot is: ， The pose of the mobile robot is represented as follows: , Represents the position of the mobile robot in the map coordinate system coordinate, Represents the position of the mobile robot in the map coordinate system coordinate, This indicates the rotation angle of the mobile robot in the map coordinate system; It is the first in the final set of reflector matching points. The first position coordinates corresponding to the matching points of each reflector column are in the lidar coordinate system. Directional coordinates It is the first in the final set of reflector matching points. The first position coordinates corresponding to the matching points of each reflector column are in the lidar coordinate system. Directional coordinates It is the first in the final set of reflector matching points. The matching points of the reflectors are in the map coordinate system. Directional coordinates It is the first in the final set of reflector matching points. The matching points of the reflectors are in the map coordinate system. Coordinates of direction; This indicates the number of reflector matching points in the final set of reflector matching points. Represents the square of the L2 norm; Using the optimal sampled pose as the initial value, the CERES optimization library is used to iteratively optimize the pose, and the pose of the mobile robot with the minimum objective function is taken as the final optimized pose.

8. A robot positioning system based on reflective pillars, characterized in that, include: A mobile robot is equipped with a lidar and an odometer. The lidar acquires real-time point cloud data of the mobile robot as it moves, and the odometer acquires real-time pose changes of the mobile robot as it moves. The map acquisition module is used to acquire the map as the mobile robot moves. The Gaussian distribution sampling module is used to predict the pose of the mobile robot by combining the map and pose transformation data, and to perform Gaussian distribution sampling on the predicted pose of the mobile robot to obtain multiple Gaussian distribution sampling particles. The optimal sampling pose calculation module is used to compare the matching effect between the point cloud under each sampling particle and the reflective pillars in the map, and take the predicted pose of the mobile robot corresponding to the sampling particle with the best matching effect as the optimal sampling pose. The matching verification module is used to verify the matching results of the sampled particles with the best matching effect. If the matching is successful, the final optimized pose of the mobile robot is solved according to the optimal sampling pose. If the matching fails, Gaussian distribution sampling is performed again. The mobile robot localization module is used to solve for the final optimized pose of the mobile robot based on the optimal sampled pose, including: constructing an objective function for the pose of the mobile robot and iteratively optimizing it using the optimal sampled pose of the mobile robot as the initial value, solving for the final optimized pose of the mobile robot, and using the final optimized pose as the localization result of the mobile robot.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When executed by a processor, the computer program implements the robot localization method based on reflective posts as described in any one of claims 1-7.

10. A robot positioning device based on reflective columns, characterized in that: It includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the robot positioning method based on reflective posts as described in any one of claims 1-7.

Images