A method for converting relative position to global longitude and latitude based on point cloud positioning

Abstract

The present invention relates to a method for converting relative position to global longitude and latitude based on point cloud positioning. First, a high-precision point cloud map of the environment is established by laser radar, and the longitude and latitude coordinates of GPS are received at the same time to obtain the longitude and latitude trajectory. The two trajectories are matched by NDT to obtain the transformation matrix, and then the ICP is used for precise matching to observe the overlap of the trajectories and obtain the precise transformation matrix, which is the transformation matrix for converting local coordinates to global longitude and latitude coordinates. The obtained transformation matrix completes the conversion of the map coordinate system to the WGS84 coordinate system. The method for converting from relative position to global longitude and latitude can convert the map coordinates obtained by point cloud map matching into longitude and latitude, thereby facilitating the subsequent fusion with satellite positioning system data.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned driving, and relates to a method for converting a relative position based on point cloud positioning into global longitude and latitude, and relates to a method for converting a local position obtained by point cloud positioning into global longitude and latitude. Background Art

[0002] It is crucial to obtain the position of the unmanned vehicle during its driving. Usually, the real-time latitude and longitude of the unmanned vehicle can be obtained through the global satellite positioning system. However, for some obstructed areas, the unmanned vehicle cannot receive satellite signals when passing through, so it needs to calculate the real-time position through dead reckoning. Dead reckoning based on inertial navigation information is often accompanied by cumulative errors, which makes it difficult for unmanned vehicles to achieve autonomous driving in a large area. Another method is to obtain the position of the unmanned vehicle in the map through point cloud matching based on the prior point cloud map. Although this gets rid of the influence of cumulative errors to a certain extent, it can only give the relative position of the unmanned vehicle in the point cloud map.

[0003] From the above analysis process, it can be seen that the traditional pure point cloud positioning can only give the relative position of the unmanned vehicle. This relative position cannot be compared with the longitude and latitude given by the satellite positioning system. Large-scale positioning depends on the accuracy of the map and initialization is prone to errors. The robustness of repositioning cannot be guaranteed during the driving process of the unmanned vehicle. In view of this, this paper invents a method for converting relative position to global longitude and latitude, which can convert the map coordinates obtained by point cloud map matching into longitude and latitude, thereby facilitating the subsequent fusion with satellite positioning system data. Summary of the invention

[0004] Technical issues to be solved

[0005] In order to avoid the shortcomings of the prior art, the present invention proposes a method for converting relative position based on point cloud positioning into global longitude and latitude. In view of the defect that only relative position can be obtained based on point cloud matching, a method for converting map coordinates obtained by point cloud matching into longitude and latitude is proposed by analyzing the transformation relationship between coordinate systems, thereby facilitating subsequent positioning initialization.

[0006] Technical Solution

[0007] A method for converting relative position to global longitude and latitude based on point cloud positioning, characterized by the following steps:

[0008] Step 1: Record environmental radar plus GNSS and IMU data, and build an environmental point cloud map through laser SLAM;

[0009] Step 2: Use coordinate transformation on the point cloud map to transform the map coordinate system into the radar coordinate system, and then use the longitude and latitude at the initial time of map construction as the coordinate origin to convert the input longitude and latitude WGS84 coordinate system into the Northeast Sky navigation coordinate system;

[0010] Step 3: Play back the point cloud data for repositioning and obtain the point cloud map coordinate trajectory and navigation coordinate trajectory;

[0011] Step 4: Convert the transformation between longitude and latitude coordinates and map coordinates into the transformation between navigation coordinate system and radar coordinate system, connect the inertial navigation system on the unmanned vehicle with the radar and the retrograde, and obtain the coordinate system transformation relationship between the two as a constant matrix T, that is, In step 2, three point pairs corresponding to each other in two coordinate systems, namely the navigation coordinate system and the radar coordinate system, are selected from the two trajectories, and the rough transformation matrix origin_T of the two coordinate systems is obtained by SVD decomposition;

[0012] Step 5: Use origin_T as the initial transformation matrix of ndt matching, input the two trajectories obtained in step 2, and calculate the precise transformation matrix between the navigation coordinate system and the radar coordinate system by combining ndt coarse registration with icp fine matching.

[0013] Step 6: Transform the matrix After that, the map coordinates obtained by point cloud matching will be Then transform to the longitude and latitude in the WGS84 coordinate system.

[0014] The laser slam adopts lio_sam or fast_lio.

[0015] Beneficial Effects

[0016] The present invention proposes a method for converting relative position to global longitude and latitude based on point cloud positioning. First, a high-precision point cloud map of the environment is established through a laser radar, and the longitude and latitude coordinates of GPS are received at the same time to obtain the longitude and latitude trajectory. The two trajectories are matched to obtain the transformation matrix through NDT, and then ICP is used for precise matching to observe the overlap of the trajectories and obtain the precise transformation matrix. This matrix is ​​the transformation matrix for converting local coordinates to global longitude and latitude coordinates. The obtained transformation matrix completes the conversion of the map coordinate system to the WGS84 coordinate system. The method of converting relative position to global longitude and latitude can convert the map coordinates obtained by point cloud map matching into longitude and latitude, thereby facilitating the subsequent fusion with satellite positioning system data. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 : Local coordinate trajectory and global coordinate trajectory; the No. 1 trajectory on the left is the local coordinate trajectory obtained by point cloud positioning, and No. 2 is the actual longitude and latitude trajectory. The No. 1 trajectory on the right is also the local longitude and latitude, and No. 2 is the result of the local coordinate trajectory being transformed and fused with the global longitude and latitude trajectory.

[0018] Figure 2 :Algorithm process framework DETAILED DESCRIPTION

[0019] The present invention will now be further described with reference to the embodiments and the accompanying drawings:

[0020] In view of the defect that only relative positions can be obtained based on point cloud matching, the present invention proposes a method that can convert map coordinates obtained by point cloud matching into longitude and latitude by analyzing the transformation relationship between coordinate systems, thereby facilitating subsequent positioning initialization.

[0021] After the point cloud positioning is imported into the map, the NDT transformation can be used to match the scanned frame with the map. However, NDT matching is more dependent on the initial value. The initial value of NDT requires that the two point clouds to be matched have relatively close postures in advance, or there is a transformation matrix that can provide such conditions. For the actual scanned frame matching map, we should not and cannot manually provide the position and orientation of the unmanned vehicle that matches the map in advance. However, due to the inertial navigation equipment carried by the unmanned vehicle, we can obtain the absolute position and orientation information of the unmanned vehicle at this time through GNSS data and IMU data during positioning initialization. Then we only need to convert this absolute information into the map to convert it into a more appropriate NDT matching initial value.

[0022] The NDT initial value is a transformation matrix, and a transformation matrix can be divided into two parts: rotation matrix and translation matrix. The former can be obtained through IMU data, and the latter can be obtained through GNSS data.

[0023] The longitude and latitude data can be obtained through GNSS information. The longitude and latitude need to be converted into map coordinates. First, the WGS84 coordinate system is converted to the Northeast Navigation Coordinate System (ENU). The longitude and latitude at the initial time of map construction are taken as the origin of the coordinate system to establish a local navigation coordinate system. The subsequent longitude and latitude can be converted to coordinates in the local navigation coordinate system, and then the navigation coordinate system is converted to the map coordinate system. In this way, the conversion of longitude and latitude to map coordinates is completed.

[0024] Only after the conversion of longitude and latitude to map coordinates is completed, can the longitude and latitude be received and converted into map coordinates to obtain the translation amount T of the initial position. t .

[0025] N in formula (1-1) t is the coordinates of longitude and latitude in the navigation coordinate system, G f is the transformation relationship between the WGS-84 coordinate system and the local navigation coordinate system (ENU), P(lat0,lon0,alt0) is the longitude and latitude coordinates at the initial positioning time, and the coordinates in the navigation coordinate system (n0,w0,e0) are obtained after conversion.

[0026] N t = G f* P(lat0,lon0,alt0) = (n0,w0,e0) (1-1)

[0027] T t =T f *N t =T f *G f *P(lat0,lon0,alt0)

[0028] =(x0,y0,z0) (1-2)

[0029] In formula (1-2), T t is the map coordinate, T f is the transformation matrix between the map coordinate system and the local navigation coordinate system, N t That is, the coordinates in the navigation coordinate system, which are converted into map coordinates (x0, y0, z0).

[0030] T f *G f The result is used as a matrix to convert longitude and latitude into map coordinates. When we use the collected data to build a map, the laser odometer will also publish map coordinates. At the same time, we also have a series of longitude and latitude coordinates. Due to the frequency of publication, we cannot clearly know whether the map coordinates correspond to the longitude and latitude coordinates one by one. However, if we draw a series of longitude and latitude coordinates and the map coordinates published when building the map as a trajectory, it can be foreseen that these two trajectories will be transformed after a fixed transformation. There will inevitably be a large overlap afterward (the reason why it will not completely overlap is because the SLAM map itself has a certain error, and this accumulated error will cause the end of the trajectory to not overlap). Actually it is T f *G f , and overlap the two trajectories to obtain the transformation matrix The method is to use the NDT algorithm to obtain a more accurate transformation matrix, such as Figure 1 After drawing the trajectory, NDT rough matching and ICP fine matching are used to make the two trajectories overlap to the greatest extent, and finally the transformation matrix is ​​obtained.

[0031] The specific steps are as follows:

[0032] Step 1: First, use the laser radar to create a high-precision point cloud map of the environment, and receive the GPS longitude and latitude coordinates to obtain the longitude and latitude trajectory;

[0033] Record environmental radar plus GNSS and IMU data, and build environmental point cloud map through laser SLAM (such as lio_sam or fast_lio).

[0034] Step 2: Use coordinate transformation on the point cloud map to transform the map coordinate system into the radar coordinate system, and then use the longitude and latitude at the initial time of map construction as the coordinate origin to convert the input longitude and latitude WGS84 coordinate system into the Northeast Sky navigation coordinate system;

[0035] The point cloud is relocated through the NDT algorithm to obtain the local coordinate trajectory of the point cloud;

[0036] Through NDT, the two trajectories are matched to obtain the transformation matrix, and then ICP is used for precise matching. The overlap of the trajectories is observed to obtain the precise transformation matrix, which is the transformation matrix from local coordinates to global longitude and latitude coordinates.

[0037] Step 3: On this basis, replay the point cloud data for repositioning to obtain the point cloud map coordinate trajectory and navigation coordinate trajectory.

[0038] Step 4: Convert the transformation between latitude and longitude coordinates and map coordinates into the transformation between navigation coordinate system and radar coordinate system. In the unmanned vehicle, the inertial navigation and radar are fixedly connected, so the coordinate transformation relationship between the two is also a constant matrix T, that is, In step 2, three corresponding point pairs are selected from the two trajectories in two coordinate systems (navigation coordinate system and radar coordinate system), and the rough transformation matrix origin_T of the two coordinate systems is obtained through SVD decomposition.

[0039] Step 5: Use origin_T as the initial transformation matrix of ndt matching, input the two trajectories obtained in step 2, and calculate the precise transformation matrix between the navigation coordinate system and the radar coordinate system by combining ndt coarse registration with icp fine matching.

[0040] Step 6: Get the transformation matrix Then write it into the program, and the map coordinates obtained by point cloud matching will be Then transform to the longitude and latitude in the WGS84 coordinate system.

Claims

1. A method for converting relative position to global longitude and latitude based on point cloud positioning, characterized in that Here are the steps: Step 1: Record environmental radar plus GNSS and IMU data, and build an environmental point cloud map through laser SLAM; Step 2: Use coordinate transformation on the point cloud map to transform the map coordinate system into the radar coordinate system, and then use the longitude and latitude at the initial moment of map construction as the coordinate origin to convert the input longitude and latitude WGS84 coordinate system into the Northeast Sky navigation coordinate system; Step 3: Play back the point cloud data for repositioning and obtain the point cloud map coordinate trajectory and navigation coordinate trajectory; Step 4: Convert the transformation between longitude and latitude coordinates and map coordinates into the transformation between navigation coordinate system and radar coordinate system, connect the inertial navigation system and radar on the unmanned vehicle, and obtain the coordinate system transformation relationship between the two as a constant matrix T, namely T0 LW ; In step 2, select three point pairs corresponding to each other in two coordinate systems, namely the navigation coordinate system and the radar coordinate system, from the two trajectories, and obtain the rough transformation matrix origin_T of the two coordinate systems through SVD decomposition; Step 5: Use origin_T as the initial transformation matrix of ndt matching, input the two trajectories obtained in step 2, and calculate the precise transformation matrix T0 between the navigation coordinate system and the radar coordinate system by combining the ndt coarse registration with the icp fine matching method. LW ; Step 6: Transform the matrix T0 LW After that, the map coordinates obtained by point cloud matching will be passed through (T0 LW ) -1 Then transform to the longitude and latitude in the WGS84 coordinate system.

2. The method for converting relative position to global longitude and latitude based on point cloud positioning according to claim 1, characterized in that: The laser slam adopts lio_sam or fast_lio.

Images