A continuous positioning method using GPS and LiDAR that provides accurate initial values ​​for point cloud registration

Abstract

This invention discloses a method for continuous GPS and laser radar positioning that provides precise initial values ​​for point cloud registration. The method first uses sensors onboard an unmanned platform to map areas where GPS positioning is unreliable or ineffective, obtaining a point cloud map with global positioning. The method then determines a laser radar positioning effective area, where the area on and within the curve near the effective curve of the laser radar positioning effective area can receive GPS signals. When the unmanned platform is operating, the positioning mode is determined based on the presence or absence of GPS signals and whether the platform is within the effective laser radar positioning area. The invention enables unmanned platforms, such as unmanned vehicles that frequently enter and exit indoor and outdoor operations, to have more comprehensive positioning capabilities.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned platform positioning, and in particular to a GPS and laser radar continuous positioning method that provides accurate point cloud registration initial values. Background Art

[0002] In recent years, the application areas of unmanned platforms, such as autonomous vehicles and robots, have continued to expand, and their application scenarios have become increasingly diverse. The use of unmanned platforms is no longer limited to indoor or outdoor environments. Traditional GPS positioning technology, while applicable in both outdoor and indoor scenarios, struggles to function effectively indoors or in heavily obscured areas. Simultaneous Localization and Mapping (SLAM) technology, however, struggles to capture valid feature points in open outdoor environments, resulting in reduced positioning accuracy. However, it performs well in indoor scenarios with richer environmental features. To address positioning requirements in both indoor and outdoor scenarios, multiple positioning methods are required. This places certain demands on positioning continuity when unmanned platforms, such as autonomous vehicles, switch between different operating scenarios. In practice, LiDAR positioning relies heavily on providing initial registration values. Manually providing these initial values ​​can disrupt positioning continuity when switching between different locations. Providing accurate and effective initial registration values ​​for point clouds, while ensuring continuity, has become a pressing issue. Summary of the Invention

[0003] In view of this, according to the characteristics of the laser radar point cloud registration and positioning method that it is more dependent on the initial registration value and the need for positioning continuity, the present invention provides a GPS and laser radar continuous positioning method that provides accurate point cloud registration initial values, which can enable unmanned platforms such as unmanned vehicles that need to frequently enter and exit indoor and outdoor operations to have more comprehensive positioning capabilities.

[0004] In order to solve the above technical problems, the present invention is implemented as follows.

[0005] A method for continuous positioning of GPS and lidar that provides accurate initial values ​​for point cloud registration, comprising:

[0006] Step 1: Use the sensors on the unmanned platform to map areas where GPS positioning is unreliable or ineffective, obtain a point cloud map with global positioning, and determine the effective area of ​​LiDAR positioning. The area on the curve of the effective area of ​​LiDAR positioning and the area within the curve close to the curve can receive GPS signals.

[0007] Step 2: When the unmanned platform is working, the positioning method is determined based on whether there is a GPS signal and whether it is within the effective area of ​​the LiDAR positioning. Specifically, it includes:

[0008] If the unmanned platform is started in an area with reliable GPS signals and is outside the effective area of ​​the LiDAR positioning, the GPS positioning information will be released;

[0009] If the unmanned platform is started in an area with reliable GPS signals and is located within the effective area of ​​the LiDAR positioning, or if the unmanned platform enters the effective area of ​​the LiDAR positioning during its movement, the GPS positioning result is used as the initial registration value Q1, and 3D point cloud registration is used for LiDAR positioning, and the LiDAR positioning information is released;

[0010] If the unmanned platform is started in an area without GPS, the point cloud after feature extraction is initially aligned with the global point cloud map using a sliding window method to obtain the initial alignment value Q2. 3D point cloud alignment is then used for lidar positioning, and lidar positioning information is released.

[0011] Preferably, in step 1, the method of obtaining a point cloud map with global positioning using sensors carried by the unmanned platform is:

[0012] Utilizing data from the IMU, lidar, and GPS receiver carried by the unmanned platform, and adopting LIO-SAM technology with improved GPS as a mandatory factor, point cloud maps with global positioning are constructed.

[0013] Preferably, in step 1, the determination of the effective area of ​​the laser radar positioning is: using the laser radar to obtain a point cloud map of the area where GPS positioning is unreliable or invalid, and at the same time using the GPS configured on the unmanned platform to obtain GPS positioning and orientation information; selecting multiple different convex polygons with an area larger than the point cloud map; allowing the unmanned platform to travel on the edges of each selected convex polygon, and according to the vehicle kinematic model, using the unscented Kalman filter to process the GPS positioning and orientation information, and recording the trajectories before and after filtering; using the Fréchet distance to evaluate the similarity of the two trajectories before and after filtering, and selecting the convex polygon with the greatest similarity among the multiple selected different convex polygons as the effective area of ​​the laser radar positioning.

[0014] Preferably, in step 2, the method for determining whether the unmanned platform is located in or enters the laser radar positioning effective area is:

[0015] Connect the unmanned platform position and each vertex of the convex polygon of the laser radar positioning effective area in sequence to form several triangles; compare the sum of the areas of all the triangles and the area of ​​the convex polygon. If the areas are equal, the unmanned platform has entered the laser radar positioning effective area.

[0016] Preferably, if the unmanned platform is started in a GPS signal reliable area and is outside the laser radar positioning effective area, the GPS positioning information released is:

[0017] If the unmanned platform is started in an area with reliable GPS signals and is outside the effective area of ​​the lidar positioning, the unmanned platform converts the longitude and latitude information calculated by the GPS receiver into UTM coordinates and outputs them, and clears the data queue related to the lidar positioning.

[0018] Preferably, the GPS positioning result is used as the initial registration value Q1, 3D point cloud registration is used for laser radar positioning, and the laser radar positioning information is released as follows:

[0019] The GPS receiver and lidar receive data simultaneously, read data from the GPS cache queue and the point cloud data cache queue, and synchronize the time. Using the initial registration value Q1, the lidar is positioned according to the point cloud data. The Euclidean distance between the lidar positioning and the GPS positioning is checked. If it is greater than the error threshold, the matching result is considered unreliable and the matching is repeated. If it is less than the error threshold, the match is considered successful, the lidar matching and positioning phase is entered, and the lidar positioning information is released.

[0020] Preferably, in step 2, if the unmanned platform is started in a GPS-free area, a preliminary registration is performed using the feature-extracted point cloud and the global point cloud map sliding window method, and the initial registration value Q2 is obtained as:

[0021] Step S21: Using a sliding window on the global point cloud map to obtain different local point cloud maps as target point clouds, and using the intrinsic incremental shape and feature histogram ISS+FPFH technology to complete feature extraction of the point cloud to be matched and the target point cloud;

[0022] Step S22: Use the Sampling Consistency Initial Registration (SAC-IA) algorithm to complete the preliminary registration of positioning under different local point cloud maps, and use the preliminary registration results as the initial value of NDT registration for registration, and store the corresponding maximum likelihood objective function value and the registration results in the cache queue;

[0023] Step S23: Obtain the positioning result with the highest objective function value in the cache queue as the registration result. According to the positional relationship of the corresponding local point cloud map in the global point cloud map, the registration result is transformed into the coordinate system coordinate, and this is used as the registration initial value Q2.

[0024] Preferably, the laser radar positioning using 3D point cloud registration includes:

[0025] The previous positioning information is used as the initial value for the next point cloud registration. Positioning information is continuously acquired in an incremental form and the reliability of the positioning information is checked. If the reliability check is passed, the lidar positioning information is released; if not, the positioning method is re-determined based on the presence of GPS signals and whether it is within the lidar positioning effective area.

[0026] The reliability check is as follows: the most recent positioning results are stored in a cache queue, the distance between two adjacent frames of positioning in the cache queue is calculated in real time, and it is determined whether it is greater than the distance threshold. If so, it is considered that the laser radar positioning at this time has undergone large fluctuations and the reliability check fails; otherwise, the reliability check passes.

[0027] Preferably, the use of 3D point cloud registration for lidar positioning further includes: optimizing the time consumption during the iteration process, specifically: obtaining the positioning time difference based on the timestamps of two adjacent positioning results, and dynamically adjusting the voxel grid filter parameters during point cloud registration according to the gradient of the time difference of multiple data. If the matching time consumption shows an increasing trend, the grid filter parameters are increased to reduce the number of point clouds involved in the registration to reduce the time pressure; if the matching time consumption shows a decreasing trend, the grid filter parameters are reduced to retain more environmental features to improve the accuracy of the registration; in this way, the real-time and accuracy of the positioning release are weighed.

[0028] Preferably, the method further includes: determining whether the unmanned platform has left the laser radar positioning effective area; if so, re-determining the positioning mode based on the presence or absence of GPS signals and whether it is within the laser radar positioning effective area.

[0029] Preferably, the laser radar positioning using 3D point cloud registration adopts a 3D-NDT point cloud registration method.

[0030] Beneficial effects:

[0031] (1) Regardless of where the UAV is started, a relatively accurate initial registration value can be obtained. When the UAV enters the switching area during its movement, it does not need to wait at the switching location as in the existing technology. Since the switching area construction takes into account the intermediate area with GPS signals, the GPS signal can be used as the initial registration value without destroying the continuity of positioning. The accuracy of the initial registration value can also ensure the accuracy of subsequent lidar while ensuring continuity.

[0032] (2) In order to solve the problem that the unmanned vehicle cannot obtain GPS as the initial registration value when starting, the point cloud map sliding window, feature extraction and preliminary registration are used to provide the initial registration value when starting in the area without GPS information, thereby improving the positioning accuracy and speed.

[0033] (3) The present invention constructs multiple convex deformations on the periphery of the map, uses the Fréchet distance to evaluate and delineate the effective area of ​​the lidar positioning to ensure the accuracy of the switching curve positioning, and ensures that the GPS layer has the ability to provide accurate point cloud matching initial values.

[0034] (4) The present invention constructs a high-precision map in advance by using the LIO-SAM method to construct a high-precision point cloud map. This method can combine information from sensors such as IMU, 3D laser radar, and GPS, and use the pre-integration of the IMU to compensate for the laser radar drift to achieve the construction of a high-precision point cloud map.

[0035] From a mapping perspective, the LIO-SAM approach, when combined with traditional methods, provides more stable mapping results when geometric features are relatively lacking. From a positioning perspective, the combined positioning approach can expand the scope of use of unmanned platforms beyond just single indoor or outdoor scenarios, thus offering wider applicability.

[0036] (5) In a preferred embodiment, if the unmanned platform is started in a GPS-free area, ISS+FPFH is used for feature extraction, and the SAC-IA algorithm is used for initial registration. The preliminary registration result is used as the initial value of NDT registration for registration, and then the registration result is selected based on the maximum likelihood objective function value. This method is often used for scene model reconstruction and fitting of real objects and point clouds. It is applied here to utilize the advantages of the ISS algorithm to accurately identify key points and the high descriptiveness and low time complexity of FPFH for features, as well as the SAC-IA method to use sampling consistency to find the initial transformation and perform nonlinear local optimization, which can provide more accurate initial registration values ​​with higher efficiency.

[0037] (6) In a preferred embodiment, the positioning is switched from GPS to lidar, and further reliability judgment is performed to ensure the accuracy of positioning after switching.

[0038] (7) In a preferred embodiment, when performing 3D point cloud registration by laser radar positioning, further reliability checks and time optimization are performed to improve the accuracy of laser radar positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a flow chart of the present invention.

[0040] Figure 2 The process of building a point cloud map.

[0041] Figure 3 A point cloud map of the target area is constructed.

[0042] Figure 4 This is the calibration process of LiDAR and GPS.

[0043] Figure 5 The positioning switching effect after entering the designated area: (a) is the GPS positioning before entering the switching area; (b) is the lidar positioning after entering the switching area. DETAILED DESCRIPTION

[0044] The present invention provides a GPS and LiDAR continuous positioning method that provides accurate initial values ​​for point cloud registration, which can be applied to any unmanned platform. This embodiment is described using an unmanned vehicle as an example.

[0045] The basic idea of ​​the present invention is to first utilize the IMU, multi-line lidar, GPS receiver and other sensors carried by the unmanned vehicle, and use map construction technology such as SLAM to complete the construction of the area where GPS positioning is unreliable or invalid, and then determine a lidar positioning effective area that is slightly larger than the area where GPS positioning is unreliable or invalid. The area on the curve of the lidar positioning effective area and the area inside the curve close to the curve can receive GPS signals.

[0046] When the unmanned platform is working, the positioning method is determined based on whether there is a GPS signal and whether it is within the effective area of ​​the lidar positioning. There are mainly three situations:

[0047] ① If the unmanned vehicle is started in an area with reliable GPS signals and is outside the effective area of ​​the lidar positioning, it will release GPS positioning information;

[0048] ② If the unmanned vehicle is started in an area with reliable GPS signals and is located in the effective area of ​​the LiDAR positioning, the GPS positioning result is used as the initial registration value Q1, 3D point cloud registration is used for LiDAR positioning, and the LiDAR positioning information is released;

[0049] When the unmanned vehicle enters the effective area of ​​the laser radar positioning during its movement, the GPS positioning result is also used as the initial registration value Q1, and 3D point cloud registration is used for laser radar positioning.

[0050] ③ If the unmanned platform is started in an area without GPS, the point cloud after feature extraction is used to perform preliminary registration with the global point cloud map using a sliding window method to obtain the initial registration value Q2. 3D point cloud registration is used for lidar positioning, and the lidar positioning information is released.

[0051] Secondly, in view of the situation that the unmanned vehicle cannot obtain GPS as the initial value for registration when it starts, the present invention uses ISS and FPFH to extract the features of the point cloud to be registered, and uses the SAC-IA method to complete the preliminary pose estimation based on the regional sliding window on the global point cloud map to provide a relatively accurate initial value for registration.

[0052] As can be seen, the present invention addresses the fact that LiDAR positioning relies on relatively accurate initial positioning values. Regardless of the drone's launch location, a relatively accurate initial registration value can be obtained. When the unmanned vehicle enters a handover zone during travel, it no longer needs to wait at the handover location as in the prior art. Because the handover zone construction incorporates an intermediate zone with GPS signals, the GPS signal can be used as the initial registration value, without disrupting positioning continuity. This accurate initial registration value, while ensuring continuity, also guarantees the accuracy of subsequent LiDAR measurements.

[0053] The implementation process of the present invention is described in detail below. Figure 1 A flow chart of a method for continuous positioning of GPS and laser radar with precise point cloud registration initial values ​​according to the present invention is shown. The method comprises the following steps:

[0054] Step 1: Use the sensors on the unmanned platform to map areas where GPS positioning is unreliable or ineffective, obtain a point cloud map with global positioning, and determine the effective area of ​​the lidar positioning. The area on the curve of the lidar positioning effective area and the area within the curve close to the curve can receive GPS signals.

[0055] In this embodiment, a GPS receiver configured on an unmanned platform is used to obtain GPS positioning and orientation information, and a laser radar is used to obtain a point cloud map of areas where GPS positioning is unreliable or ineffective.

[0056] The signal received by the GPS receiver is resolved into longitude and latitude information, which is then converted into UTM (Universal Transverse Mercator Grid System) coordinates. A global coordinate zero point is set in the area to be positioned to simplify the coordinate data information.

[0057] When using lidar to obtain point cloud maps of areas where GPS positioning is unreliable or ineffective, the present invention utilizes sensors such as the onboard IMU and lidar, and uses technologies such as SLAM to complete the construction of accurate point cloud maps with global positioning.

[0058] Before building a point cloud map, the IMU and lidar calibration work is first completed according to the installation position of each sensor to obtain the pose transformation matrix T between the two, including the position relationship and yaw angle relationship between the two.

[0059]

[0060] Where R is a 3rd-order square matrix that describes the rotation of the IMU relative to the lidar in the x, y, and z directions. t is a 3×1 matrix that describes the translation relationship of the IMU relative to the lidar in the x, y, and z directions.

[0061] After the calibration work is completed, the unmanned vehicle is moved in the target area to complete the mapping work, such as Figure 2 、 Figure 3 As shown. Secondly, according to the installation position of each sensor, the calibration of the lidar relative to the GPS receiver is completed, which can be represented by the posture transformation matrix T', and the subsequent lidar positioning information is mapped to the global positioning information, as shown in Figure 4 shown.

[0062] In order to ensure the accuracy of lidar positioning when determining the effective area of ​​lidar positioning, in a preferred embodiment, the LIO-SAM (Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping) technology is adopted, in which the improved GPS factor is a mandatory factor. As a tightly coupled lidar inertial odometry method, it completes the construction of a map with global positioning information through sensors such as lidar, IMU, and GPS receiver, including modules such as point cloud data dedistortion, feature extraction, IMU pre-integration, and back-end factor graph optimization.

[0063] After obtaining a point cloud map of the area where GPS positioning is unreliable or ineffective, use the following steps to determine the effective area for LiDAR positioning:

[0064] Step S11: Select multiple different convex polygons whose areas are larger than the point cloud map.

[0065] In this step, to ensure adaptability and diversity in the positioning area, multiple convex polygons slightly larger than the point cloud map are selected from the point cloud map constructed using LIO-SAM technology to cover the mapping area. When selecting polygons, each vertex is placed outside the point cloud map outline, and the selection avoids areas without GPS signal.

[0066] Step S12: Let the unmanned vehicle drive on the edge of the selected convex polygon, use the unscented Kalman filter to process the GPS positioning and orientation information according to the vehicle kinematic model, and record the trajectory before and after filtering.

[0067] Taking the constant turn rate and velocity amplitude model (CTRV) as an example, a vehicle kinematic model is established. The state variables are selected as follows:

[0068] Among them, p x With p y is the position coordinate in the global coordinate system, v is the velocity, φ is the heading angle, is the angular velocity.

[0069] The discrete-time system has the following state transition equation without considering noise.

[0070]

[0071] The prediction equation considering noise can be expressed as

[0072] x k+1 =f(x k , v k )

[0073] where v k is the process noise, k is the kth iteration, and Δt is the interval sampling time.

[0074] The unmanned vehicle is equipped with two radars for positioning and orientation, so it can observe the current orientation information. Position information and heading angle information are selected as observation quantities.

[0075]

[0076] There is the following observation equation

[0077] z k =h(x k )+ω k

[0078] where h(x k ) is the matrix for obtaining observations from the state vector, ω k is the observation noise.

[0079] According to the established state space equation and observation equation, and based on the prediction and update formula of UKF, the sigma point is selected for traceless transformation to complete the prediction and update of the state quantity.

[0080] Step S13: Use the Fréchet distance to evaluate the similarity between the two trajectories before and after filtering as a basis for measuring the fluctuation amplitude of the GPS signal. The smaller the distance, the greater the similarity. Select the convex polygon with the greatest similarity among the multiple selected different convex polygons as the optimal division area, that is, the effective area of ​​the lidar positioning.

[0081] Fréchet distance is a measure of the similarity between two curves. It is the minimum of the maximum distance between any points on the curves in all possible isometric mappings. The formula for Fréchet distance can be expressed as:

[0082]

[0083] Where f: C1 → C2 represents an isometric mapping from curve C1 to curve C2, that is, a mapping that preserves distance. d(f(c1), c2) represents the distance between point c1 on curve C1, mapped to point f(c1) on curve C2 via the isometric mapping f, and point c2 on curve C2.

[0084] The division of the lidar positioning effective area obtained in this step can ensure that the GPS layer has the ability to provide accurate point cloud matching initial values. The switching curve found using the Fréchet distance has the highest similarity of motion trajectories before and after filtering. The GPS has the least signal fluctuations and mutations on this curve, which can, to a certain extent, ensure the accuracy of the GPS information received when the unmanned vehicle enters the switching area.

[0085] Step 2: When the unmanned vehicle is actually working, the positioning method is determined based on whether there is a GPS signal and whether it is within the effective area of ​​the lidar positioning.

[0086] In this step, the presence of a GPS signal is determined by checking the data in the GPS cache queue. If the RTK fixed positioning solution and orientation can be output, it is considered that a valid GPS signal is being received and the vehicle is in a reliable GPS signal area. If no data is present in the GPS cache queue or the GPS data validity check fails, the vehicle is considered to be in a GPS-denied area.

[0087] The method for determining whether the unmanned vehicle is within the LiDAR positioning area is to connect the unmanned vehicle's location with the vertices of the convex polygon of the LiDAR positioning area to form a number of triangles. The sum of the areas of all the triangles is compared with the area of ​​the convex polygon. If the areas are equal, the unmanned vehicle is within or has entered the LiDAR positioning area. This method is more versatile when dealing with floating-point numbers such as GPS data, compared to the cross product method.

[0088] If the unmanned vehicle is started in an area with reliable GPS signals and is outside the effective area of ​​the lidar positioning, proceed to step 3.

[0089] If the unmanned vehicle is started in an area with reliable GPS signals and is located within the effective area of ​​the LiDAR positioning, or if the unmanned platform enters the effective area of ​​the LiDAR positioning during its movement, proceed to step 4.

[0090] If the unmanned vehicle is started in an area without GPS, proceed to step five.

[0091] Step 3: The unmanned vehicle starts in an area with reliable GPS signals and is outside the effective area of ​​the lidar positioning. At this time, it indicates that the GPS signal is good, and the GPS positioning result is directly used as the positioning information for release, and it is in the GPS positioning stage.

[0092] In this step, if the result of step 2 indicates that the location is not within the effective LiDAR positioning area, the converted GPS UTM coordinates in the MAP are output and the LiDAR positioning data queue is cleared. The process returns to step 2 to continue the determination and output of positioning information.

[0093] In step 4, the unmanned vehicle starts in an area with reliable GPS signals and is located within the effective area of ​​the lidar positioning, or when the unmanned platform enters the effective area of ​​the lidar positioning during its movement, lidar positioning needs to be enabled. The unmanned platform uses the GPS positioning result as the initial registration value Q1, and performs 3D point cloud registration based on the initial registration value Q1 to obtain accurate positioning, and then proceeds to step 6.

[0094] In this step, the GPS receiver and lidar receive data simultaneously, read data from the GPS cache queue and the point cloud data cache queue, and complete data time synchronization based on the timestamp. Using the initial registration value Q1, the first match is performed based on the initial matching pose in the point cloud cache queue. The Euclidean distance between the lidar matching positioning result and the GPS positioning is checked. If this distance is greater than the error threshold, the matching result is considered inaccurate and a new match is performed. If the error is less than the threshold, the match is considered successful, and the lidar matching and positioning phase is entered, and then step 6 is entered. If multiple matches are unsuccessful, the relative position of the lidar and GPS is calibrated first, or the position of the switch area is changed to match again.

[0095] In step 5, the unmanned vehicle starts in a GPS-free area. At this time, the unmanned vehicle needs to use the point cloud after feature extraction and the global point cloud map sliding window method to perform preliminary registration, provide an accurate registration initial value Q2, and perform 3D point cloud registration based on the registration initial value Q2 to obtain precise positioning and enter step 6.

[0096] In this step, in areas without GPS, the method for obtaining the initial value of point cloud registration is:

[0097] Step S21: Using a sliding window on the global point cloud map to obtain different local point cloud maps as target point clouds, and using the ISS+FPFH method to complete feature extraction of the point cloud to be matched and the target point cloud;

[0098] Step S22: Use the SAC-IA method to complete the preliminary positioning registration under different local maps, and use the preliminary registration results as the initial value of NDT registration for registration, and store the corresponding maximum likelihood objective function value and the registration result in the cache queue;

[0099] Step S23: Obtain the positioning result with the highest objective function score in the cache queue as the registration result. Based on the positional relationship of the corresponding local map in the global map, the registration result is transformed into coordinates in the coordinate system, and this is used as the initial value Q2 for precise registration. Then proceed to step 6.

[0100] Step 6: Perform 3D-NDT point cloud registration and publish lidar positioning information.

[0101] In this step, the positioning information obtained in step four or step five is used as the last positioning information, and the last positioning information is used as the initial value for the next point cloud alignment. Positioning information is continuously acquired in an incremental form, and a reliability check of the positioning information is performed. If the reliability check fails, re-alignment is required and the process returns to step two to re-determine the positioning method based on the presence or absence of GPS signals and whether the position is within the effective area of ​​the lidar positioning. If the reliability check passes, the lidar positioning information is released in real time.

[0102] Among them, the reliability check can be: storing the latest 20 positioning results in a cache queue, calculating the distance between two adjacent frame positioning in the cache queue in real time, and dynamically obtaining the distance threshold based on the current running speed. If the adjacent positioning distance is greater than the distance threshold, it is considered that the laser radar positioning at this time has a large fluctuation and fails the reliability check.

[0103] In a preferred embodiment, time consumption is optimized during the iterative process of lidar positioning, specifically: the positioning time difference is obtained based on the timestamps of two adjacent positioning results, and the voxel grid filter parameters during point cloud registration are dynamically adjusted according to the gradient of the time difference of multiple data. If the matching time consumption shows an increasing trend, the grid filter parameters are increased to reduce the number of point clouds involved in the registration to alleviate the time consumption pressure; if the matching time consumption shows a decreasing trend, the grid filter parameters are reduced to retain more environmental features to improve the accuracy of the registration; in this way, the real-time and accuracy of the positioning release are weighed.

[0104] During the execution of the above operations, it is further determined whether the unmanned platform has left the laser radar positioning effective area. If it has left, return to step 2 and re-determine the positioning method based on the presence or absence of GPS signals and whether it is within the laser radar positioning effective area.

[0105] The following is the verification of the positioning effect.

[0106] Start the unmanned vehicle in an open, unobstructed area. After obtaining a stable GPS signal, the positioning arrow will turn green. Drive from the GPS area to the LiDAR positioning area. Figure 5 As shown, green and blue represent GPS positioning and lidar positioning respectively.

[0107] The area method is used to determine whether the position is in the switching area. This method determines whether the sum of the areas of the triangles formed by the current position and the adjacent vertices of the convex polygon is equal to the area of ​​the convex polygon. This method is more applicable to floating-point numbers than the cross product method.

[0108] After crossing the switching line, GPS is used to provide accurate initial registration values, which are then incorporated into 3D-NDT registration to generate precise positioning information. Subsequent positioning information is iteratively updated based on this, and changes in the positioning source can be intuitively seen in the visualization interface.

[0109] The unmanned vehicle is launched into a GPS-denied area. Point cloud features are extracted using ISS+FPFH (Incremental Shape and Feature Histogram) and various local maps are obtained using a global map sliding window. Preliminary registration with the current scanned point cloud is performed using the SAC-IA method. Successful registrations are then further processed for 3D-NDT registration. Registration results and scores are continuously stored in a cache queue. This cache queue is implemented as a priority queue based on score, enabling the fastest extraction of the highest-scoring preliminary registration result. After obtaining the highest-scoring initial registration value, subsequent positioning information is iteratively updated based on this initial value. Once stable positioning information is achieved, a blue location appears in the visualization interface.

[0110] The above specific embodiments merely illustrate the design principles of the present invention. The shapes and names of the components described herein may vary and are not limiting. Therefore, those skilled in the art may modify or substitute equivalents for the technical solutions described in the above embodiments. Such modifications and substitutions, without departing from the inventive spirit and technical solutions of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A method for continuous positioning of GPS and laser radar that provides accurate initial values ​​for point cloud registration, characterized in that: include: Step 1: Use the sensors carried by the unmanned platform to map the area where GPS positioning is unreliable or invalid, obtain a point cloud map with global positioning, and determine the effective area of ​​laser radar positioning, where the area on the curve of the laser radar positioning effective area and the area within the curve close to the curve can receive GPS signals; wherein, the determination of the effective area of ​​laser radar positioning is as follows: use laser radar to obtain a point cloud map of the area where GPS positioning is unreliable or invalid, and use the GPS configured on the unmanned platform to obtain GPS positioning and orientation information; select multiple different convex polygons with an area larger than the point cloud map; let the unmanned platform drive on the edges of each selected convex polygon, use unscented Kalman filtering to process the GPS positioning and orientation information according to the vehicle kinematic model, and record the trajectories before and after filtering; use Fréchet distance to evaluate the similarity of the two trajectories before and after filtering, and select the convex polygon with the greatest similarity among the multiple selected different convex polygons as the effective area of ​​laser radar positioning; Step 2: When the unmanned platform is working, the positioning method is determined based on whether there is a GPS signal and whether it is within the effective area of ​​the LiDAR positioning. Specifically, it includes: If the unmanned platform is started in an area with reliable GPS signals and is outside the effective area of ​​the LiDAR positioning, the GPS positioning information will be released; If the unmanned platform is started in an area with reliable GPS signals and is located within the effective area of ​​the LiDAR positioning, or if the unmanned platform enters the effective area of ​​the LiDAR positioning during its movement, the GPS positioning result is used as the initial registration value Q1, and 3D point cloud registration is used for LiDAR positioning, and the LiDAR positioning information is released; If the unmanned platform is started in an area without GPS, the point cloud after feature extraction is used to perform preliminary registration with the global point cloud map using a sliding window method to obtain the initial registration value Q2. 3D point cloud registration is used for lidar positioning, and lidar positioning information is released. Among them, the method of obtaining the initial registration value Q2 is: Step S21: Using a sliding window on the global point cloud map to obtain different local point cloud maps as target point clouds, and using the intrinsic incremental shape and feature histogram ISS+FPFH technology to complete feature extraction of the point cloud to be matched and the target point cloud; Step S22: Use the Sampling Consistency Initial Registration (SAC-IA) algorithm to complete the preliminary registration of positioning under different local point cloud maps, and use the preliminary registration results as the initial value of NDT registration for registration, and store the corresponding maximum likelihood objective function value and the registration results in the cache queue; Step S23: Obtain the positioning result with the highest objective function value in the cache queue as the registration result. According to the positional relationship of the corresponding local point cloud map in the global point cloud map, the registration result is transformed into the coordinate system coordinate, and this is used as the registration initial value Q2.

2. The method according to claim 1, wherein In step 1, the point cloud map with global positioning is obtained by using the sensors carried by the unmanned platform: Utilizing data from the IMU, lidar, and GPS receiver carried by the unmanned platform, and adopting LIO-SAM technology with improved GPS as a mandatory factor, point cloud maps with global positioning are constructed.

3. The method according to claim 1, wherein In step 2, the method for determining whether the unmanned platform is located in or enters the effective area of ​​the lidar positioning is as follows: Connect the unmanned platform position and each vertex of the convex polygon of the laser radar positioning effective area in sequence to form several triangles; compare the sum of the areas of all the triangles and the area of ​​the convex polygon. If the areas are equal, the unmanned platform has entered the laser radar positioning effective area.

4. The method according to claim 1, wherein If the unmanned platform is started in an area with reliable GPS signals and is outside the effective area of ​​the LiDAR positioning, the GPS positioning information released is: If the unmanned platform is started in an area with reliable GPS signals and is outside the effective area of ​​the lidar positioning, the unmanned platform converts the longitude and latitude information calculated by the GPS receiver into UTM coordinates and outputs them, and clears the data queue related to the lidar positioning.

5. The method according to claim 1, wherein The GPS positioning result is used as the initial registration value Q1, and 3D point cloud registration is used for lidar positioning. The published lidar positioning information is: The GPS receiver and lidar receive data simultaneously, read data from the GPS cache queue and the point cloud data cache queue, and synchronize the time. Using the initial registration value Q1, the lidar is positioned according to the point cloud data. The Euclidean distance between the lidar positioning and the GPS positioning is checked. If it is greater than the error threshold, the matching result is considered unreliable and the matching is repeated. If it is less than the error threshold, the match is considered successful, the lidar matching and positioning phase is entered, and the lidar positioning information is released.

6. The method according to claim 1, wherein The laser radar positioning using 3D point cloud registration includes: The previous positioning information is used as the initial value for the next point cloud registration. Positioning information is continuously acquired in an incremental form and the reliability of the positioning information is checked. If the reliability check is passed, the lidar positioning information is released; if not, the positioning method is re-determined based on the presence of GPS signals and whether it is within the lidar positioning effective area. The reliability check is as follows: the most recent positioning results are stored in a cache queue, the distance between two adjacent frames of positioning in the cache queue is calculated in real time, and it is determined whether it is greater than the distance threshold. If so, it is considered that the laser radar positioning at this time has undergone large fluctuations and the reliability check fails; otherwise, the reliability check passes.

7. The method according to claim 1, wherein Using 3D point cloud registration for lidar positioning further includes: optimizing the time consumption during the iteration process, specifically: obtaining the positioning time difference based on the timestamps of two adjacent positioning results, and dynamically adjusting the voxel grid filter parameters during point cloud registration based on the gradient of the time difference of multiple data. If the matching time shows an increasing trend, the grid filter parameters are increased to reduce the number of point clouds involved in the registration to reduce the time pressure; if the matching time shows a decreasing trend, the grid filter parameters are reduced to retain more environmental features to improve the accuracy of the registration; in this way, the real-time and accuracy of the positioning release are weighed.

8. The method according to claim 1, wherein The method further includes: determining whether the unmanned platform has left the laser radar positioning effective area; if so, re-determining the positioning mode based on the presence or absence of a GPS signal and whether it is within the laser radar positioning effective area.

Images