A positioning method based on a retroreflective column, a storage medium and an electronic device

Abstract

A positioning method based on retro-reflective prisms, a storage medium and an electronic device, comprising the following steps: obtaining the global coordinates of the retro-reflective prisms; identifying and extracting the retro-reflective prisms within the radar range, and obtaining the coordinates of the retro-reflective prisms in the radar coordinate system; converting the coordinates of the retro-reflective prisms in the laser radar coordinate system to the map coordinate system based on the TF transformation between the radar coordinate system and the map coordinate system; matching the coordinates of the retro-reflective prisms in the map coordinate system with the global coordinates of the retro-reflective prisms; if the number of successful matches is greater than or equal to 2, the matching is successful, the robot pose is calculated using the ICP algorithm, otherwise the robot pose is predicted based on the odometer data; the above technical solution reduces the difficulty of actual deployment of the retro-reflective prisms, can more accurately reflect the relative position relationship between the retro-reflective prisms, reduces the matching error, makes the final pose estimation more accurate, and thus improves the overall positioning accuracy.

Description

Technical Field

[0001] This application relates to the field of navigation technology, specifically to a positioning method, storage medium, and electronic device based on reflective columns. Background Technology

[0002] Reflector-based positioning algorithms are a type of positioning technology based on the characteristics of reflectors, widely used in autonomous driving, robot navigation, and industrial automation. Existing reflector-based positioning algorithms primarily identify reflectors in the environment and utilize the feature information of these reflectors scanned by radar (LiDAR) or other radar systems to perform precise position calculation and localization. Most commercially available technologies using reflectors for assisted localization currently employ triangle matching to identify reflectors around the robot, and then use trilateration algorithms to calculate the robot's pose.

[0003] However, existing reflective column positioning algorithms have the following problems:

[0004] First, there are high requirements for the arrangement of reflective columns; the reflective columns should be arranged in positions that can form stable and easily identifiable triangles, and these positions should be distributed as evenly as possible to reduce matching errors caused by excessively large or small spacing between reflective columns.

[0005] Second, the positioning accuracy is severely affected by the matching effect; the triangle matching method is sensitive to noise in the input data. In practical applications, due to factors such as radar accuracy and environmental interference, the input data often contains a certain amount of noise, which leads to low robustness of the algorithm; at the same time, a large number of similar triangular regions are required in real-world scenarios, and the triangle matching algorithm has a high mismatch rate, which can easily lead to positioning jumps.

[0006] Third, the application scenarios are limited; the trilateration algorithm requires at least three known points that cannot be collinear, which may be difficult to meet in some scenarios, thus limiting the application scope of the trilateration algorithm. Summary of the Invention

[0007] In view of the above problems, this application provides a positioning method based on reflective columns, which solves the problems of existing positioning methods based on reflective columns having high requirements for the arrangement of reflective columns, severe impact of matching effect on positioning accuracy, and limited application scenarios.

[0008] To achieve the above objectives, the inventors provide a positioning method based on reflective columns, which includes the following steps:

[0009] Obtain the global coordinates of the reflector;

[0010] Identify and extract reflective columns within the radar's field of view (FOV), and obtain the coordinates of the reflective columns in the radar coordinate system;

[0011] Based on the TF transformation between the radar coordinate system and the map coordinate system, the coordinates of the reflector column in the lidar coordinate system are transformed to the map coordinate system;

[0012] The coordinates of the reflector in the map coordinate system are matched with the global coordinates of the reflector. If the number of successful matches is greater than or equal to 2, the match is successful and the ICP algorithm is used to calculate the robot pose. Otherwise, the robot pose is predicted based on the odometry data.

[0013] Furthermore, the global coordinates of the reflective column are obtained by traversing and sampling the area where the reflective column is deployed.

[0014] Furthermore, in the step of matching the coordinates of the reflector in the map coordinate system with the global coordinates of the reflector, the nearest neighbor matching algorithm is used to match the coordinates of the reflector in the map coordinate system with the global coordinates of the reflector.

[0015] Furthermore, in the step of calculating the robot pose using the ICP algorithm, the ICP algorithm is an ICP algorithm based on Ceres' least squares method.

[0016] Furthermore, the step of predicting the robot pose based on odometry data includes the following steps:

[0017] The robot pose is determined by finding the closest time to the timestamp in the odometry data.

[0018] Furthermore, after the step of calculating the robot pose using the ICP algorithm, the method also includes performing a positioning jump detection, comparing the robot pose calculated each time with the robot pose calculated in the previous frame or the predicted robot pose, and if the Euclidean distance between the two is greater than a set threshold, it is determined to be a positioning jump, and the predicted robot pose is used to replace the robot pose calculated in the current frame.

[0019] Furthermore, it also includes the numerical value for determining frame loss;

[0020] If the calculation is successful and the calculated pose does not change position compared to the calculated or predicted pose of the previous frame, then the frame loss calculation is reset; otherwise, the frame loss calculation is performed.

[0021] If the calculated value of consecutive frame loss exceeds a preset loss threshold, the localization process will stop.

[0022] Furthermore, prior to the step of identifying and extracting reflective pillars within the radar's field of view (FOV) and obtaining the coordinates of the reflective pillars in the radar coordinate system, the method further includes:

[0023] Determine whether a reflective column has been detected;

[0024] If so, determine whether to perform initialization. If so, identify and extract reflective columns within the radar's FOV range and obtain the coordinates of the reflective columns in the radar coordinate system. Otherwise, perform initialization first, then identify and extract reflective columns within the radar's FOV range and obtain the coordinates of the reflective columns in the radar coordinate system.

[0025] If not, determine whether to perform initialization; if so, predict the robot pose based on odometry data; otherwise, do not perform localization.

[0026] A storage medium storing a computer program that, when executed by a processor, implements the aforementioned positioning method based on reflective columns.

[0027] An electronic device includes a memory and a processor, wherein the memory stores a computer program, and the computer program, when executed by the processor, implements the above-described positioning method based on reflective columns.

[0028] Unlike existing technologies, the above-mentioned technical solution matches the coordinates of the reflective pillars in the map coordinate system with their global coordinates. This eliminates the need to deploy the reflective pillars in easily identifiable triangular locations, reducing the difficulty of actual deployment. It also more accurately reflects the relative positional relationships between the reflective pillars, reducing matching errors caused by environmental interference, radar errors, and other factors, thus improving the accuracy and reliability of the matching. Even in complex and variable environments, such as those with changing lighting or obstructions, it maintains high robustness. Furthermore, the generated matching point pairs are used to calculate the pose using the ICP algorithm. The ICP algorithm iteratively optimizes the distance between the matching point pairs, resulting in a more accurate final pose estimation and thus improving the overall positioning accuracy.

[0029] The above description of the invention is merely an overview of the technical solution of this application. In order to enable those skilled in the art to better understand the technical solution of this application and to implement it based on the description and drawings, and to make the above-mentioned objectives and other objectives, features and advantages of this application easier to understand, the following description is provided in conjunction with the specific embodiments and drawings of this application. Attached Figure Description

[0030] The accompanying drawings are only used to illustrate the principles, implementation methods, applications, features, and effects of specific embodiments of the present invention and other related contents, and should not be considered as limitations on this application.

[0031] In the accompanying drawings of the instruction manual:

[0032] Figure 1 This is a flowchart illustrating the positioning method based on reflective columns as described in a specific implementation. Detailed Implementation

[0033] To illustrate the possible application scenarios, technical principles, implementable specific solutions, and achievable objectives and effects of this application in detail, the following description, in conjunction with the listed specific embodiments and accompanying drawings, provides a detailed explanation. The embodiments described herein are merely illustrative of the technical solutions of this application and are therefore intended to limit the scope of protection of this application.

[0034] In this document, the term "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The term "embodiment" appearing in various places throughout the specification does not necessarily refer to the same embodiment, nor does it specifically limit its independence or connection with other embodiments. In principle, in this application, as long as there are no technical contradictions or conflicts, the technical features mentioned in each embodiment can be combined in any way to form corresponding implementable technical solutions.

[0035] Unless otherwise defined, the technical terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains; the use of related terms herein is merely for the purpose of describing particular embodiments and is not intended to limit this application.

[0036] In the description of this application, the term "and / or" is used to describe the logical relationship between objects, indicating that three relationships can exist. For example, A and / or B means: A exists, B exists, and A and B exist simultaneously. Additionally, the character " / " in this document generally indicates that the preceding and following objects have an "or" logical relationship.

[0037] In this application, terms such as “first” and “second” are used only to distinguish one entity or operation from another, and do not necessarily require or imply any actual quantity, hierarchy or order between these entities or operations.

[0038] Without further limitations, the use of terms such as “comprising,” “including,” “having,” or other similar open-ended expressions in this application is intended to cover non-exclusive inclusion, which does not exclude the presence of additional elements in a process, method, or product that includes the stated elements, such that a process, method, or product that includes a list of elements may include not only those defined elements but also other elements not expressly listed, or elements inherent to such a process, method, or product.

[0039] Similar to the understanding in the Examination Guidelines, in this application, expressions such as "greater than," "less than," and "exceeding" are understood to exclude the stated number; expressions such as "above," "below," and "within" are understood to include the stated number. Furthermore, in the description of the embodiments in this application, "multiple" means two or more (including two), and similar expressions related to "multiple" are also understood in this way, such as "multiple groups" and "multiple times," unless otherwise explicitly specified.

[0040] In the description of the embodiments of this application, the space-related expressions used, such as "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "vertical," "top," "bottom," "inner," "outer," "clockwise," "counterclockwise," "axial," "radial," and "circumferential," indicate the orientation or positional relationship based on the orientation or positional relationship shown in the specific embodiments or drawings. They are only for the purpose of describing the specific embodiments of this application or for the reader's understanding, and do not indicate or imply that the device or component referred to must have a specific position, a specific orientation, or be constructed or operated in a specific orientation. Therefore, they should not be construed as limitations on the embodiments of this application.

[0041] See Figure 1 As shown, a positioning method based on reflective pillars matches the coordinates of the reflective pillars in the map coordinate system with their global coordinates. This eliminates the need to deploy the reflective pillars in easily identifiable triangular locations, reducing the difficulty of actual deployment. Simultaneously, it more accurately reflects the relative positional relationships between reflective pillars, reducing matching errors caused by environmental interference, radar errors, and other factors, thus improving the accuracy and reliability of the matching. Even in complex and variable environments, such as those with changing lighting or obstructions, it maintains high robustness. The generated matching point pairs are then used to calculate the pose using the ICP algorithm. The ICP algorithm iteratively optimizes the distance between the matching point pairs, resulting in a more accurate final pose estimation and improving the overall positioning accuracy. This positioning method can add new reflective pillars or update the position information of existing reflective pillars to adapt to different application scenarios and needs.

[0042] Within the deployment area, several reflective pillars can be distributed in various corners or key locations. This eliminates the need to deploy them in easily identifiable triangular positions, reducing the difficulty of actual deployment. The reflective pillars can be evenly or randomly distributed; even distribution ensures appropriate spacing between them, neither too dense nor too sparse. In some embodiments, a grid distribution method can be used, dividing the application scene into multiple grids and deploying reflective pillars at the intersections or specific locations of each grid, with the spacing between adjacent pillars ranging from 1.5 to 3 meters.

[0043] The following combination Figure 1 This application provides an implementation method for a positioning method based on reflective columns, which includes the following steps:

[0044] Obtain the global coordinates of the reflector;

[0045] Identify and extract reflective columns within the radar's field of view (FOV), and obtain the coordinates of the reflective columns in the radar coordinate system;

[0046] Based on the TF transformation between the radar coordinate system and the map coordinate system, the coordinates of the reflector column in the lidar coordinate system are transformed to the map coordinate system;

[0047] The coordinates of the reflector in the map coordinate system are matched with the global coordinates of the reflector. If the number of successful matches is greater than or equal to 2, the match is successful and the pose is calculated using the ICP algorithm. Otherwise, the pose is predicted based on the odometry data.

[0048] The aforementioned TF transformation between the radar coordinate system and the map coordinate system refers to the pose relationship (position transformation and attitude transformation) in different coordinate systems (radar coordinate system and map coordinate system) at the same point in time. The TF transformation can use transformation matrices to represent the pose relationship in different coordinate systems. The transformation matrices include rotation and translation matrices, representing the attitude transformation and position transformation between coordinate systems, respectively. The coordinates of the reflector in the map coordinate system are obtained by multiplying the coordinates of the reflector in the lidar coordinate system by the transformation matrix.

[0049] The global coordinates of the aforementioned reflective pillars can be obtained by traversing and sampling points within the deployment area. Starting from the starting point of the deployment area, the pillars move step by step along a predetermined traversal path (such as a grid or spiral pattern). At each traversal point, the global coordinates of that point are recorded using radar. Specifically, nearby high-reflectivity points are extracted using radar to obtain the relative pose of the reflective pillars to the lidar; then, the reflective pillar poses are converted to a map coordinate system to obtain the global coordinates of the reflective pillars, which are then stored in a txt file for later use.

[0050] In the above step of matching the coordinates of the reflector in the map coordinate system with its global coordinates, a nearest neighbor matching algorithm can be used. Specifically, this includes the following steps:

[0051] For each reflective column identified by radar, calculate the coordinates of the reflective column in the map coordinate system and the distances between the reflective column and all reflective columns in the global coordinate system.

[0052] If the coordinates of a reflector in the map coordinate system are (x_map, y_map), then the coordinates of a reflector in the global coordinate system are (x_global, y_global).

[0053] The distance between the coordinates of the reflector in the map coordinate system and a corresponding reflector in the global coordinate system can be calculated using the Euclidean distance formula: d = sqrt((x_map - x_global)^2 + (y_map - y_global)^2). For each reflector identified by radar, find the reflector with the smallest distance in the global coordinate system (its nearest neighbor), and record the coordinates of the matching pair and its distance. A distance threshold T can be set. If the distance between the matching pairs is less than T, the match is considered successful; otherwise, the match is considered unsuccessful.

[0054] The above-described ICP (Iterative Closest Point) algorithm is used to calculate the robot pose. This algorithm finds a series of rigid body transformations (rotations and translations) that align two pairs of matching points as closely as possible. Specifically, based on the corresponding point pairs, an optimal rigid body transformation (rotation and translation) is calculated, transforming the coordinates of the reflector in the map coordinate system to the coordinates of its paired reflector in the global coordinate system. This transformation is solved by minimizing the distance error between the two points. Typically, SVD (Singular Value Decomposition), quaternion methods, and Ceres-based least squares methods are used to calculate the optimal rotation matrix and translation vector, thereby accurately locating the robot's pose. The preferred method for calculating robot pose is the ICP algorithm based on Ceres' least squares method. This method uses Ceres' least squares method to calculate the optimal rotation matrix and translation vector. Ceres' least squares method does not rely on singular value decomposition of the matrix, but finds the optimal solution through iterative optimization. It can better handle special cases such as multiple collinear points, making the algorithm more robust when dealing with point cloud data with special geometric features (such as multiple collinear points). This helps to reduce the calculation error in the registration process and improve the registration accuracy.

[0055] The above-mentioned prediction of robot pose based on odometry data refers to finding the robot pose with the closest time based on the timestamp in the odometry data.

[0056] To prevent positioning jumps, after the step of calculating the robot pose using the ICP algorithm, a positioning jump detection process is also included. This involves comparing the calculated robot pose with the robot pose calculated or predicted in the previous frame. If the Euclidean distance between the two is greater than a set threshold, a positioning jump is identified, and the robot pose calculated or predicted in the previous frame is used to replace the currently calculated robot pose. By detecting and processing positioning jumps, sudden positional shifts caused by environmental noise (such as sudden changes in lighting, obstructions, etc.), radar errors, or algorithm instability can be significantly reduced, thereby improving the continuity and stability of robot positioning. Simultaneously, it can effectively reduce unnecessary resource consumption (positioning jumps may lead to unnecessary re-localization or path planning by the robot), improving the overall efficiency of the system.

[0057] To reduce positioning errors, the calculation of frame loss is also included;

[0058] If the calculation is successful and the calculated pose does not change position compared to the calculated or predicted pose of the previous frame, then the frame loss calculation is reset; otherwise, the frame loss calculation is performed.

[0059] If the calculated value of consecutive frame loss exceeds a preset loss threshold, the localization process will stop.

[0060] By monitoring frame loss, the system can promptly detect anomalies in data transmission or processing, thus avoiding the use of erroneous or incomplete data for positioning calculations. This helps reduce positioning errors and improves the overall reliability of the positioning system. When the calculated value of consecutive frame loss is less than a preset loss threshold, the robot's pose can be predicted based on odometry data. This allows the robot to maintain positioning accuracy even when encountering brief data loss or processing delays, enhancing its robustness. When the calculated value of consecutive frame loss exceeds a preset loss threshold, positioning is stopped. This provides a preventative measure to avoid complete positioning failure due to severe data loss or processing malfunctions, protecting the robot and its surrounding environment and preventing potential collisions or damage.

[0061] Before the steps of identifying and extracting reflective columns within the radar's field of view (FOV) and obtaining the coordinates of the reflective columns in the radar coordinate system, it is usually also necessary to determine whether a reflective column has been detected.

[0062] If so, determine whether to perform initialization. If so, identify and extract reflective columns within the radar's FOV range and obtain the coordinates of the reflective columns in the radar coordinate system. Otherwise, perform initialization first, then identify and extract reflective columns within the radar's FOV range and obtain the coordinates of the reflective columns in the radar coordinate system.

[0063] If not, determine whether to perform initialization; if so, predict the robot pose based on odometry data; otherwise, do not perform localization.

[0064] The above initialization records the odometry data of the first frame and the TF transformation between the laser and the map coordinate system. The above prediction of robot pose based on odometry data also refers to finding the robot pose with the closest time based on the timestamp in the odometry data.

[0065] This application also provides a storage medium storing a computer program that, when executed by a processor, implements the above-described positioning method based on reflective columns.

[0066] The computer program involved in the embodiments can be stored in a computer device readable storage medium, which includes, but is not limited to, disks, magnetic tapes, magnetic cards, floppy disks, flash memory, optical disks, optical cards, read-only memory (ROM), random access memory (RAM), erasable programmable ROM (EPROM), and electrically erasable programmable ROM (EEPROM), etc., and also includes other biological, physical, or chemical structures that can achieve the same or equivalent functions as the storage media listed above, such as DNA, RNA, proteins, and other units with information storage capabilities. In specific embodiments, the storage medium involved can be one of the above-mentioned media types, or a combination of the above-mentioned media types. In different embodiments, the computer program involved in the embodiments can be centrally stored in a single medium, or distributed and stored in multiple media. The memory containing the computer device readable storage medium can be non-volatile memory or random access memory. These computer device readable storage media can be built into the device, or can be connected to the device involved in the embodiments as an external device or part of an external device. In some embodiments, the memory having a computer device readable storage medium is deployed locally; in other embodiments, the memory may be deployed remotely from the processor, for example, as a network-attached memory accessed via RF circuitry or an external port and a communication network, wherein the communication network may be the Internet, one or more intranets, a local area network (LAN), a wide area network (WLAN), a storage area network (SAN), or a suitable combination thereof, as long as computer device access to the memory is possible. Furthermore, the computer program involved in the embodiments may be stored in plaintext / ciphertext form, or it may be designed as training data, integrated and recombined through model training and implicitly stored in the parameter states of a deep neural network or other robot learning model.

[0067] This application also provides an electronic device including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, it implements the above-described positioning method based on reflective columns.

[0068] The processor described in the embodiments of this application can be implemented by hardware, firmware, software, or a combination thereof. It can be a circuit, one or more of an application-specific integrated circuit (ASIC), a digital signal processor (DSP), a digital signal processing device (DSPD), a programmable logic device (PLD), a field-programmable gate array (FPGA), a central processing unit (CPU), a controller, a microcontroller, or a microprocessor. It also includes other physical, biological, or chemical structures that can implement the same or equivalent functions as the processors listed above, such as biological neurons, quantum computing units, DNA computing units, etc., so that the processor can execute some or all of the steps in the computer program or method involved in the various embodiments of this application, or any combination of the steps mentioned therein.

[0069] Finally, it should be noted that although the above embodiments have been described in the text and drawings of this application, this should not limit the scope of patent protection of this application. Any technical solutions that are based on the essential concept of this application and utilize the content described in the text and drawings of this application, resulting in equivalent structural or procedural substitutions or modifications, as well as the direct or indirect application of the technical solutions of the above embodiments to other related technical fields, are all included within the scope of patent protection of this application.

Claims

1. A positioning method based on reflective columns, characterized in that, Includes the following steps: Obtain the global coordinates of the reflector; Identify and extract reflective columns within the radar's field of view (FOV), and obtain the coordinates of the reflective columns in the radar coordinate system; Based on the TF transformation between the radar coordinate system and the map coordinate system, the coordinates of the reflector column in the lidar coordinate system are transformed to the map coordinate system; The coordinates of the reflector in the map coordinate system are matched with the global coordinates of the reflector. If the number of successful matches is greater than or equal to 2, the match is successful and the ICP algorithm is used to calculate the robot pose. Otherwise, the robot pose is predicted based on the odometry data. Before the steps of identifying and extracting reflective pillars within the radar's field of view (FOV) and obtaining the coordinates of the reflective pillars in the radar coordinate system, the method further includes: Determine whether a reflective column has been detected; If so, determine whether to perform initialization. If so, identify and extract reflective columns within the radar's FOV range and obtain the coordinates of the reflective columns in the radar coordinate system. Otherwise, perform initialization first, then identify and extract reflective columns within the radar's FOV range and obtain the coordinates of the reflective columns in the radar coordinate system. If not, determine whether to perform initialization; if yes, predict the robot pose based on odometry data; otherwise, do not perform localization. After the step of calculating the robot pose using the ICP algorithm, the method further includes performing a positioning jump detection. The robot pose calculated each time is compared with the robot pose calculated in the previous frame or the predicted robot pose. If the Euclidean distance between the two is greater than a set threshold, it is determined to be a positioning jump, and the predicted robot pose is used to replace the robot pose calculated in the current frame.

2. The positioning method based on reflective pillars according to claim 1, characterized in that, The global coordinates of the reflective pillars are obtained by traversing and sampling the area where the reflective pillars are deployed.

3. The positioning method based on reflective pillars according to claim 1, characterized in that, In the step of matching the coordinates of the reflector in the map coordinate system with the global coordinates of the reflector, the nearest neighbor matching algorithm is used to match the coordinates of the reflector in the map coordinate system with the global coordinates of the reflector.

4. The positioning method based on reflective pillars according to claim 1, characterized in that, In the step of calculating the robot pose using the ICP algorithm, the ICP algorithm is an ICP algorithm based on Ceres' least squares method.

5. The positioning method based on reflective pillars according to claim 1, characterized in that, The step of predicting robot pose based on odometry data includes the following steps: The robot pose is determined by finding the closest time to the timestamp in the odometry data.

6. The positioning method based on reflective pillars according to claim 1, characterized in that, It also includes the numerical values ​​calculated to determine the number of frames lost; If the calculation is successful and the calculated pose does not change position compared to the calculated or predicted pose of the previous frame, then the frame loss calculation is reset; otherwise, the frame loss calculation is performed. If the calculated value of consecutive frame loss exceeds a preset loss threshold, the localization process will stop.

7. A storage medium storing a computer program that, when executed by a processor, implements the positioning method based on reflective posts as described in any one of claims 1-6.

8. An electronic device, characterized in that, It includes a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, it implements the positioning method based on reflective columns as described in any one of claims 1-6.

Images