A reflective marker matching method and a laser radar posture correction method

Abstract

The present invention discloses a reflective marker matching method and a laser radar posture correction method, which can provide a posture correction function according to the pre-information when positioning fails. The method introduces an inertial unit, hereinafter referred to as IMU, to solve the problem of dynamic matching loss caused by the rapid rotation of the mobile device and the reflective marker being too far away during the traditional reflective marker dynamic matching, and can correct the posture by simply inputting the approximate position when positioning fails. The method reduces the complexity of the algorithm, improves the success rate of matching, and improves the stability of the positioning system due to the addition of IMU.

Description

Technical field:

[0001] The present invention relates to the field of positioning technology, and in particular to a reflective marker matching and posture correction method combined with an inertial unit. Background technology:

[0002] With the development of intelligent manufacturing, factory intelligence has become an inevitable trend. Automated Guided Vehicle (AGV) as the main equipment for automated handling has also been more and more widely used. Positioning and navigation technology, as the basis and necessary condition for the self-movement of AGV, is one of the key technologies that must be studied in the development of intelligent mobile devices. Compared with the traditional mobile robot industry (such as the service industry and the entertainment industry), industrial mobile robots have large loads and higher requirements for the safety and stability of the equipment. At present, the more mature industrial vehicle navigation methods on the market are: reflective marker navigation and 2D SLAM navigation. The former is identified by scanning and reflection principles, while 2D SLAM navigation uses single-line laser radar for environmental identification. In situations where high positioning accuracy is required, reflective marker positioning is often more accurate and reliable than laser SLAM.

[0003] The existing reflective marker positioning technology can predict the LiDAR position by combining static matching and dynamic matching. However, when the reflective marker is far away from the LiDAR and the LiDAR rotates at a fast speed, the current dynamic matching technology is prone to matching failures, and the system has poor stability and low availability. Summary of the invention:

[0004] In view of the above shortcomings of the prior art, the purpose of the present invention is to provide a dynamic matching technology and posture correction method when the reflective marker is far away and the laser radar rotates quickly. By adding the inertial unit IMU, the problem of long posture interval time and dynamic matching caused by the low scanning frequency of the laser radar is compensated. The reflective marker positioning system is more stable and more robust.

[0005] This application adopts the following technical solutions to achieve the above effects:

[0006] A reflective marking matching method, the method comprising the following steps:

[0007] S1, perform initial static matching next to the known starting point to obtain the initial posture;

[0008] S2, start IMU time synchronization, use the angle data between radar frames as the angle compensation from the previous posture to the current posture, and dynamically match the reflective marker according to the angle compensation;

[0009] S3. If the dynamic matching fails, static matching is performed based on the pose estimation result of the previous frame. After the matching is successful, S2 is repeated.

[0010] Furthermore, the static matching specifically includes the following steps:

[0011] S11, searching the reflective mark world map for three reflective marks closest to the start point according to the start point number input by the user;

[0012] S12. Calculate the lengths L of the three sides of the triangle formed by the three reflective marks obtained by searching. i , i=1,2,3;

[0013] S13, traverse the three nearest reflective marks scanned by the radar, and for any one of the reflective marks, calculate the distance l to the other two reflective marks i 1 and l i 2 , i=1,2,3;

[0014] S14, if the distance between two reflective cursors is equal to a certain L i is similar, that is, |l i -L i |<matchDist, then record the numbers of the reflective marks at both ends in the world map, where matchDist is the specified edge length matching threshold, which can be changed by the user in the configuration file; if the number of a reflective mark value is greater than once, the coordinates of the reflective mark in the world map can be determined;

[0015] S15, repeat S14 to obtain the world coordinates of the three reflective markers closest to the laser radar;

[0016] S16. Using the known world coordinates and the distance between the current laser radar and the reflective mark, a three-point positioning algorithm is used to calculate the global position of the laser radar in the world coordinate system.

[0017] Furthermore, the matching process specifically includes the following steps:

[0018] S21, start the IMU and synchronize the IMU time with the laser radar time;

[0019] S22, counting the number of IMU messages within the time interval between the time when the laser radar posture was obtained at the last moment and the time when the current radar scans the reflective mark;

[0020] S23, calculating the rotation angle of the laser radar between the two frames by using integration;

[0021] S24, compensating the calculated rotation angle data to the known initial position to obtain new initial posture data;

[0022] S25, using the initial position compensated by IMU in the previous step, the local coordinates of the currently scanned reflective mark under the radar coordinates Convert to world coordinates (X i , Y i );

[0023] S26, the converted world coordinates The coordinates of the pre-stored reflective markers in the world map (X j map , Y j map ),like and Then it is considered that i=j, where matchError is the reflective mark coordinate matching threshold, which can be modified by the user in the configuration file;

[0024] S27. Using the known world coordinates and the distance between the current laser radar and the reflective mark, the global position of the laser radar in the world coordinate system is calculated using a three-point positioning algorithm.

[0025] The present application also provides a method for manually correcting the laser radar posture, which is based on the matching result obtained by the above-mentioned reflective mark matching method, and the method comprises the following steps:

[0026] S10: When the laser radar dynamic matching fails, record the position and posture of the laser radar before the failure.

[0027] S20, calculating the local coordinates of the three reflective markers currently searched and closest to the radar according to the last moment's position and posture;

[0028] S30, using the method of steps S12 to S16, calculating the global coordinates of the laser radar;

[0029] S40: If the positioning information at the last moment cannot be used, manual correction is performed and the approximate position of the laser radar is manually input. Using steps S5 and S6, the global coordinates of the laser radar are corrected, where the approximate position The data recorded by the user when mapping the reflective marks, or recorded at any time during the movement of the LiDAR, is convenient for input during manual correction next time.

[0030] Compared with the prior art, the present invention has the following technical improvements: an IMU is added to record the turning angle of the laser radar through the IMU, and integral interpolation operations are performed on the angular velocity data between two frames of radar time, thereby performing angle compensation for the posture at the previous moment in dynamic matching, which greatly reduces the matching error caused by the low frequency of angle recording and improves the success rate of matching. Description of the drawings:

[0031] Figure 1 A flow chart of a dynamic matching and correction method provided in an embodiment of the present invention;

[0032] Figure 2 This is a schematic diagram of the IMU time synchronization and interpolation calculation principle in an embodiment of the present invention. Specific implementation method:

[0033] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0034] When the present invention utilizes laser radar for positioning, it is necessary to first establish a world map of reflective marks. When establishing the world map of reflective marks, it is first necessary to at least keep the laser radar stable, record at least three reflective marks, and use them as the origin of the world coordinate system, and move the laser radar to establish the world map.

[0035] Example 1

[0036] During the positioning process, the carrier needs to be moved to the vicinity of the origin, or to the vicinity of the starting point with known coordinates for starting. The specific steps are as follows:

[0037] S1, perform initial static matching near the known starting point to give the initial posture;

[0038] S2. After obtaining the initial posture, start IMU time synchronization, use the rotation angle data between radar frames as the angle compensation from the previous posture to the current posture, and use the compensation result to dynamically match the reflective marker.

[0039] S3. If the dynamic matching fails, static matching is performed based on the pose estimation result of the previous frame. After the matching is successful, S2 is repeated.

[0040] Specifically, the method for acquiring the initial pose of the laser radar reflective marker in step S1 is characterized in that the static matching specifically includes the following steps:

[0041] S11, searching the reflective mark world map for three reflective marks closest to the position according to the start point number input by the user;

[0042] S12. Calculate the lengths L of the three sides of the triangle formed by the three reflective marks obtained by searching. i (i=1, 2, 3);

[0043] S13, traverse the reflective marks scanned by the three laser radars, and for any reflective mark, calculate the distance l to the other two reflective marks i 1 and l i 2 , i=1,2,3;

[0044] S14, if l i 1 and a certain L i is similar, that is, |l i -L i |<matchDist, then record the components of l i 1 The reflective markings at both ends of the world map are marked with numbers. i 2 If a number value appears twice, the coordinates of the reflective mark on the world map can be determined. If the match fails, the position of the laser radar can be repeatedly adjusted until the match is successful.

[0045] S15, repeat S14 to obtain the world coordinates of the three reflective markers closest to the laser radar;

[0046] S16. Through the known world coordinates and the distance between the current laser radar and the reflective mark, the global position of the laser radar in the world coordinate system can be calculated using a three-point positioning algorithm.

[0047] After obtaining the initial pose through static matching, it is necessary to use the dynamic matching method combined with IMU to match and locate the scanned reflective markers. The steps are as follows:

[0048] S21, start IMU. Since IMU and LiDAR have different timestamps, it is necessary to synchronize the IMU time with the timeline of LiDAR. After synchronization, there will be multiple IMU angular velocity information between two frames of LiDAR time, such as Figure 2 As shown;

[0049] S22, counting the number of IMU messages within the time interval between the time when the posture was obtained at the last moment and the time when the current radar scans the reflective mark;

[0050] S23. Use integration to calculate the rotation angle of the laser radar between the above two frames. Since there is no pitch and roll motion in general mobile carriers, the corresponding angular velocity can be ignored. The formula for calculating the rotation angle is:

[0051]

[0052] S24, compensating the calculated rotation angle data to the known initial position, that is, Obtain new initial pose data;

[0053] S25, use the initial position compensated by IMU to obtain the radar local coordinates of the currently scanned reflective mark Convert to world coordinates (X i , Y i ), as follows

[0054]

[0055] The local coordinates are the coordinates of the reflective mark relative to the laser radar's own coordinate system, which are obtained by laser radar detection. The detection algorithm is not the focus of this patent, so it is not described in detail in this patent. In the above formula, is the world coordinate of the converted reflective marker, is the world coordinate of the reflective mark at the last moment, is the distance between the reflective marker and the radar at the current moment, α t-1 is the attitude angle of the radar in the world coordinates at the last moment, is the attitude angle of the reflective mark in the radar coordinate system, The compensation angle calculated by IMU;

[0056] S26, the converted world coordinates Pre-stored reflective marker coordinates in the world map like and Then it is considered that i=j.

[0057] S27, similar to S16, by using the known world coordinates and the distance between the current laser radar and the reflective mark, the global position of the laser radar in the world coordinate system can be calculated using a three-point positioning algorithm.

[0058] Example 2

[0059] The present application also provides a method for manually correcting the laser radar posture. The method is based on the matching result obtained by the above-mentioned reflective marker matching method. If the laser radar dynamic matching fails, its posture can be automatically corrected by using the posture at the previous moment. The method steps are as follows:

[0060] S4. When the laser radar dynamic matching fails, record the position of the laser radar before the failure

[0061] S5. Using the recorded position at the last moment, calculate the local coordinates of the three reflective markers currently searched and closest to the radar.

[0062] S6. Calculate the global coordinates of the laser radar using the method of steps S12 to S16.

[0063] S7. If the last moment's positioning information cannot be used (such as the laser radar is seriously offset or there is no last moment's position when starting positioning, etc.), manual correction is required. Manually input the approximate position of the laser radar Using steps S5 and S6, the global coordinates of the laser radar are corrected.

[0064] The method provided in the present application records the turning angle of the laser radar through the IMU, performs integral interpolation operations on the angular velocity data between two frames of radar time, and then performs angle compensation for the posture at the previous moment in dynamic matching, which greatly reduces the matching error caused by the low frequency of angle recording and improves the success rate of matching.

[0065] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A reflective marking matching method, It is characterized in that The method comprises the following steps: S1. Perform initial static matching next to a known starting point to obtain an initial posture. The static matching specifically includes the following steps: S11, searching the reflective mark world map for three reflective marks closest to the start point according to the start point number input by the user; S12. Calculate the lengths L of the three sides of the triangle formed by the three reflective marks obtained by searching. i , i=1,2,3; S13, traverse the three nearest reflective marks scanned by the radar, and for any one of the reflective marks, calculate the distance l to the other two reflective marks i 1 and l i 2 , i=1,2,3; S14. If the distance between two reflective marks is L i is similar, that is, |l i -L i |<matchDist, then record the numbers of the two reflective marks in the world map, where matchDist is the specified edge length matching threshold, which can be changed by the user in the configuration file; if the number of a reflective mark value is greater than once, the coordinates of the reflective mark in the world map can be determined; S15, repeat S14 to obtain the world coordinates of the three reflective markers closest to the laser radar; S16, using the known world coordinates and the distance between the current laser radar and the reflective mark, using a three-point positioning algorithm to calculate the global position of the laser radar in the world coordinate system; S2, start IMU time synchronization, use the angle data between radar frames as the angle compensation from the previous posture to the current posture, and dynamically match the reflective marker according to the angle compensation; S3, if the dynamic matching fails, static matching is performed according to the pose estimation result of the previous frame, and S2 is repeated after the matching is successful; The method further includes a method for manually correcting the laser radar posture, and the method for manually correcting the laser radar posture comprises the following steps: S4. When the laser radar dynamic matching fails, record the position of the laser radar before the failure S5. Calculate the local coordinates of the three reflective markers currently searched and closest to the radar according to the last moment's position and posture; S6. Calculate the global coordinates of the laser radar using the method of steps S12 to S16; S7: If the last moment’s positioning information cannot be used, manual correction is performed and the approximate position of the laser radar is manually input. Using steps S5 and S6, the global coordinates of the laser radar are corrected, where the approximate position The data recorded by the user when mapping the reflective marks, or recorded at any time during the movement of the LiDAR, is convenient for input during manual correction next time.

2. A reflective marking matching method as claimed in claim 1, It is characterized in that The dynamic matching process specifically includes the following steps: S21, start the IMU and synchronize the IMU time with the laser radar time; S22, counting the number of IMU messages within the time interval between the time when the laser radar posture was obtained at the last moment and the time when the current radar scans the reflective mark; S23, calculating the rotation angle of the laser radar between the two frames by using integration; S24, compensating the calculated rotation angle data to the known initial position to obtain new initial posture data; S25, using the initial position compensated by IMU in the previous step, the local coordinates of the currently scanned reflective mark under the radar coordinates Convert to world coordinates (X i , Y i ); S26, the converted world coordinates The coordinates of the pre-stored reflective markers in the world map (X j map , Y j map ),like and Then it is considered that i=j, where matchError is the reflective mark coordinate matching threshold, which can be modified by the user in the configuration file; S27. Using the known world coordinates and the distance between the current laser radar and the reflective mark, the global position of the laser radar in the world coordinate system is calculated using a three-point positioning algorithm.

Images