A method of dynamic alignment of coordinate systems in GPS laser fusion SLAM

Abstract

The present invention relates to a method for dynamically aligning coordinate systems in GPS laser fusion SLAM, comprising the steps of: S1, obtaining GPS data and performing a first process, and saving the processed GPS data; S2, obtaining 3D laser point cloud data and performing a second process, obtaining laser odometer data and saving it; S3, synchronizing the processed GPS data and laser odometer data; S4, screening data point pairs; S5, calculating the heading angle based on the data point pairs, thereby realizing the dynamic alignment of coordinate systems in GPS laser fusion SLAM. This solution only needs to provide a single GPS device, does not require a magnetometer, and does not require the user to perform special actions, so as to facilitate seamless access to the laser SLAM algorithm software.

Description

Technical Field

[0001] The invention belongs to the technical field of intelligent robots, and in particular relates to a method for dynamically aligning coordinate systems in GPS laser fusion SLAM. Background Art

[0002] The Global Positioning System (GPS) is an indispensable technology for outdoor positioning. The GPS system includes GPS satellites, ground control stations, data injection stations, monitoring stations, and GPS receivers as user terminals. At least three of the satellites are needed to quickly determine the location and altitude of the user terminal on the earth. Civilian GPS can now achieve a positioning accuracy of about ten meters. The GPS positioning system uses the basic satellite triangulation positioning principle and the GPS receiving device to measure the distance by measuring the transmission time of the radio signal, so as to determine the user's location. However, outdoor robots work in complex dynamic environments, and the GPS multipath effect and signal obstruction problems seriously affect the outdoor robots' use of GPS for real-time positioning.

[0003] SLAM (Simultaneous Localization and Mapping) means that a robot starts from an unknown location in an unknown environment, locates its own position and posture through repeated observation of environmental features during movement, and then constructs an incremental map of the surrounding environment based on its own position, thereby achieving the purpose of simultaneous positioning and mapping. SLAM technology is currently one of the main positioning methods for outdoor robots.

[0004] Outdoor patrol robots usually work in large scenes such as company parks, blocks, squares, and densely populated cities with many dynamic interferences. The single laser SLAM method has the disadvantages of long-term error accumulation and dynamic interference affecting the laser registration accuracy. Compared with GPS positioning, there is no cumulative error, but it is easily affected by signal obstruction and affects the positioning accuracy. Therefore, outdoor robots and unmanned vehicles mostly use GPS / laser fusion SLAM algorithms to improve the robustness and accuracy of outdoor large-scene SLAM.

[0005] The GPS / laser fusion SLAM algorithm will introduce a new key technical point: the alignment technology of GPS coordinate system and robot coordinate system. The accuracy of coordinate system alignment directly affects the accuracy of GPS / laser fusion SLAM. Even if there is a problem of coordinate system misalignment, it may cause the accuracy of GPS / laser fusion SLAM to be lower than the accuracy of single laser SLAM or lower than the accuracy of GPS positioning. At present, the solution to the alignment technology of GPS coordinate system and robot coordinate system is mainly divided into the following solutions:

[0006] 1. Dual-antenna positioning and orientation: By carrying a GPS receiver on the front and back of the robot, using the positioning points given by the two GPSs, the direction angle of the line between the two points (robot heading angle) is calculated through trigonometric functions to complete the alignment of the robot coordinate system to the GPS coordinate system;

[0007] 2. Magnetometer-assisted orientation: A single GPS antenna has a built-in magnetometer to statically calculate the robot's current heading angle in real time. This completes the alignment of the robot coordinate system to the GPS coordinate system;

[0008] 3. Straight-line walking orientation: The robot is equipped with a single GPS antenna, and the initial GPS fixed point is a. By ensuring that the robot is controlled to a certain distance in front of the straight line, the GPS current fixed point is b. Similar to solution 1, two points ab are known, and the direction angle of the line connecting the two points (robot heading angle) is calculated through trigonometric functions to complete the alignment of the robot coordinate system to the GPS coordinate system.

[0009] The above has clearly described the three mainstream technical solutions for aligning the GPS coordinate system with the robot coordinate system. For outdoor robots, the above three solutions all have different disadvantages:

[0010] In Solution 1, the installation length of the dual antennas cannot be too short (>1m), otherwise the directional accuracy may be impaired. Outdoor robots cannot meet this condition, and the dual antennas increase the cost of the robot.

[0011] In solution 2, the magnetometer calculates the heading, but too much metal interference can easily lead to inaccurate orientation, which seriously affects the robustness of the robot product and the user experience of the customer.

[0012] Solution 3 is the closest to the technical solution proposed in the present invention, but Solution 3 requires the robot to walk in a straight line for a section. However, due to installation and motion model problems in actual robot products, it is difficult to ensure that the robot actually walks in a straight line. Summary of the invention

[0013] In order to solve the above technical problems, the present invention provides a method for dynamic alignment of coordinate systems in GPS laser fusion SLAM.

[0014] The present invention is achieved through the following technical solutions:

[0015] A method for dynamic alignment of coordinate systems in GPS laser fusion SLAM, comprising the steps of:

[0016] S1, obtaining GPS data and performing a first process, and saving the processed GPS data;

[0017] S2, acquiring 3D laser point cloud data and performing a second process to obtain and save laser odometer data;

[0018] S3. Synchronize the processed GPS data and the laser odometer data;

[0019] S4. Screen data point pairs;

[0020] S5. Calculate the heading angle based on the data point pairs, thereby realizing the dynamic alignment of the coordinate systems in GPS-Laser fusion SLAM.

[0021] Further, in step S1, the obtaining of GPS data and performing the first processing, and saving the processed GPS data specifically include receiving GPS data through the first callback function, setting the first frame as the origin of the GPS coordinate system, converting subsequent GPS data to the relative position in the GPS coordinate system, and saving the converted data to the cache.

[0022] Further, the received GPS data includes longitude, latitude, altitude, dop value, and timestamp.

[0023] Further, in step S2, the obtaining of 3D laser point cloud data and performing the second processing, and obtaining and saving the laser odometer data specifically include receiving 3D laser point cloud data through the second callback function, calculating the motion trajectory of the robot position in the robot coordinate system via the laser odometry algorithm, and saving it to the cache.

[0024] Further, in step S3, the synchronization processing specifically is to load the GPS cache data and the laser odometer cache data, align the GPS cache data values and the laser odometer cache data values according to the timestamp of each data, and save the aligned data point pairs.

[0025] Further, in step S4, appropriate data point pairs are screened according to the motion distance threshold between the two points of the data point pairs.

[0026] Further, the motion distance threshold is greater than 0.3m.

[0027] Further, the specific calculation steps of the heading angle are as follows:

[0028] a) Calculate the centroid positions p, q of two groups of points p i =[x i y i T q i =[a i b i T :

[0029]

[0030]

[0031] ​​The two sets of points go to the centroid coordinates respectively:

[0032] p i =p i -p,q i =q i -p

[0033] b) Calculate the rotation matrix:

[0034]

[0035] c) Calculate the heading angle:

[0036] Yaw=atan2(R(1,0),R(0,0))

[0037] Among them, x i y i ——Horizontal representation of the robot position i in the GPS coordinate system;

[0038] a i b i ——Horizontal representation of the robot position i in the robot coordinate system.

[0039] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of a method for dynamic alignment of coordinate systems in GPS laser fusion SLAM.

[0040] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps of a method for dynamic alignment of coordinate systems in GPS laser fusion SLAM are implemented.

[0041] Compared with the prior art, the present invention has the following advantages: the present solution only requires a single GPS device, does not require a magnetometer, does not require the user to perform special actions, and is convenient for seamless access to the laser SLAM algorithm software. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The present invention will be further described in detail below with reference to the accompanying drawings;

[0043] Figure 1 It is a software flow chart of the present invention. DETAILED DESCRIPTION

[0044] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0045] In a specific embodiment, the present invention makes two assumptions before proceeding with the algorithm principle:

[0046] a) The SLAM starting position of the outdoor robot is guaranteed to be on the horizontal plane (generally, this is easy to ensure for outdoor robots);

[0047] b) The starting position of the outdoor robot SLAM ensures a good GPS positioning signal;

[0048] Based on assumption a, the problem of aligning the GPS coordinate system with the robot coordinate system degenerates into finding the heading angle deflection between the GPS coordinate system and the robot coordinate system. Therefore, the heading angle deflection between the two coordinate systems can be fitted by representing the robot position in different coordinate systems. The present invention makes full use of the high precision advantage of the laser odometer over short distances, so the representation of the robot position in the robot coordinate system can be accurately obtained; based on assumption b, the accuracy of the short-range GPS is still good, so the representation of the robot position in the GPS coordinate system can be accurately obtained; therefore, through the technical solution proposed in the present invention, it will be possible to obtain high-precision coordinate system alignment accuracy with simple operations. The specific dependence is expressed as follows:

[0049]

[0050] x i y i ——Horizontal representation of the robot position i in the GPS coordinate system (GPS acquisition);

[0051] a i b i ——Horizontal representation of the robot position i in the robot coordinate system (obtained by the laser odometry);

[0052] ——The heading deflection angle of the robot coordinate system relative to the GPS coordinate system (to be determined);

[0053] Assuming that the autonomous mobile navigation robot is equipped with GPS and 3D laser radar equipment, the method of the present invention is as follows: Figure 1 , including the following steps:

[0054] S1, process GPS data and save;

[0055] Receive GPS data (longitude, latitude, altitude, dop value, timestamp) through the callback function, set the first frame as the origin of the local coordinate system XYZ (for convenience, directly referred to as the GPS coordinate system), convert subsequent GPS data to the relative position in the local coordinate system, and save the converted data to the cache.

[0056] S2, processing 3D laser point cloud data;

[0057] The laser point cloud data is received through the callback function, and the motion trajectory of the robot position in the robot coordinate system is calculated by the laser mileage calculation method and saved to the cache.

[0058] S3, synchronize GPS data and laser odometer data;

[0059] Load the GPS cache data and laser odometry cache data, align the GPS data values ​​and laser odometry data according to the timestamp of each data, and save the aligned data point pairs.

[0060] S4, screening data;

[0061] According to the motion distance threshold (>0.3m) between two points, appropriate data point pairs are selected.

[0062] S5, calculating the heading angle;

[0063] Combined with formula (1), the SVD method is used to calculate the heading angle The specific solution steps are as follows:

[0064] a) Calculate two sets of points p i =[x i y i ] T q i =[a i b i ] T The center of mass position pq

[0065]

[0066]

[0067] The two sets of points go to the centroid coordinates

[0068] p i =p i -pq i =q i -p

[0069] b) Calculate the rotation matrix according to the following optimization problem

[0070]

[0071] R——2X2 transformation matrix

[0072] c) Calculate the heading angle

[0073] Yaw=atan2(R(1,0),R(0,0)).

[0074] The present invention also provides a computer-readable storage medium on which a computer program is stored, wherein when the program is executed by a processor, the steps of the coordinate system dynamic alignment method in GPS laser fusion SLAM are implemented.

[0075] The present invention also provides a computer device, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of a method for dynamic alignment of coordinate systems in GPS laser fusion SLAM when executing the program.

[0076] The specific embodiments described above further describe the purpose, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made without departing from the spirit and scope of the present invention also fall within the scope of protection of the present invention.

Claims

1. A method for dynamic alignment of coordinate systems in GPS laser fusion SLAM. It is characterized in that Includes steps: S1, obtaining GPS data and performing a first process, and saving the processed GPS data; S2, acquiring 3D laser point cloud data and performing a second process to obtain and save laser odometer data; S3, synchronously processed GPS data and laser odometer data; S4, screening data point pairs; S5. Calculate the heading angle based on the data point pair, thereby realizing dynamic alignment of the coordinate system in GPS laser fusion SLAM; In step S1, the GPS data is acquired and first processed, and the processed GPS data is saved, specifically including receiving GPS data through a first callback function, setting the first frame as the origin of the GPS coordinate system, converting subsequent GPS data to a relative position in the GPS coordinate system, and saving the converted data to a cache; In step S2, the acquiring of 3D laser point cloud data and performing a second process to obtain and save laser odometer data specifically includes receiving the 3D laser point cloud data through a second callback function, calculating the motion trajectory of the robot position in the robot coordinate system through a laser odometer calculation method and saving it to a cache; In step S3, the synchronization process specifically includes loading the GPS cache data and the laser odometer cache data, aligning the GPS cache data value and the laser odometer cache data value according to the timestamp of each data, and saving the aligned data point pairs; In step S4, the data point pairs are screened according to the motion distance threshold between two points of the data point pairs; The specific calculation steps of the heading angle are as follows: a) Calculate two sets of points p i =[x i y i ] T q i =[a i b i ] T The center of mass position p, q: The two sets of points go to the centroid coordinates respectively: p i = p i -p, q i = q i -p b) Calculate the rotation matrix based on SVD: c) Calculate the heading angle: Yaw=atan2(R(1,0),R(0,0)) Among them, x i y i ——Horizontal representation of the robot position i in the GPS coordinate system; a i b i ——Horizontal representation of the robot position i in the robot coordinate system.

2. The method for dynamic alignment of coordinate systems in GPS laser fusion SLAM according to claim 1, It is characterized in that The received GPS data includes longitude, latitude, altitude, dop value and timestamp.

3. The method for dynamic alignment of coordinate systems in GPS laser fusion SLAM according to claim 1, It is characterized in that The movement distance threshold is greater than 0.3 m.

4. A computer-readable storage medium having a computer program stored thereon, It is characterized in that When the program is executed by a processor, the steps of the method for dynamic alignment of coordinate systems in GPS laser fusion SLAM described in any one of claims 1 to 3 are implemented.

5. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, It is characterized in that When the processor executes the program, the steps of the method for dynamic alignment of coordinate systems in GPS laser fusion SLAM described in any one of claims 1-3 are implemented.

Images