GPS and laser radar continuous positioning method for providing accurate point cloud registration initial value

Abstract

The invention discloses a GPS and laser radar continuous positioning method for providing an accurate point cloud registration initial value, and the method comprises the steps: firstly carrying out the mapping of an unreliable or invalid GPS positioning region through a sensor carried by an unmanned platform, obtaining a point cloud map with global positioning, determining a laser radar positioning effective region, and carrying out the positioning of the laser radar; gPS signals can be received on the area curve of the laser radar positioning effective area and in the curve close to the curve; and when the unmanned platform works, a positioning mode is determined according to existence of the GPS signal and whether the GPS signal is in the laser radar positioning effective area. According to the invention, unmanned platforms such as unmanned vehicles needing to frequently go in and out of a room for operation have more comprehensive positioning capability.

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 fields of unmanned platforms such as unmanned vehicles and robots have been continuously expanded, and their application scenarios have become increasingly diversified. The use of unmanned platforms is no longer limited to a single indoor or outdoor environment. Traditional GPS positioning technology is also applied to outdoor scenes, and it is difficult to play a role indoors or in areas with severe occlusion; while SLAM (Simultaneous Localization and Mapping) technology is difficult to capture effective feature points in open outdoor environments, which easily leads to a decrease in positioning accuracy, but it has good applicability in indoor scenes with rich environmental features. In order to simultaneously meet the positioning needs of indoor and outdoor scenes, multiple positioning methods need to be adopted, which puts forward certain requirements for the positioning continuity of unmanned platforms such as unmanned vehicles when switching between different working scenes. In practice, the laser radar positioning method is highly dependent on the provision of initial registration values. When the positioning is switched, the method of manually providing initial registration values ​​destroys the positioning continuity. While ensuring continuity, the provision of accurate and effective initial value of point cloud registration has also become an urgent problem to be solved. 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 laser radar providing accurate initial value of point cloud registration, comprising:

[0006] Step 1: Use the sensors on the unmanned platform to build a map for areas where GPS positioning is unreliable or invalid, obtain a point cloud map with global positioning, and determine the effective area of ​​the laser radar positioning. The area on the curve of the effective area of ​​the laser radar positioning and the area inside 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 laser radar positioning, including:

[0008] If the unmanned platform is started in an area with reliable GPS signals and is outside the effective area of ​​laser radar 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 in the effective area of ​​laser radar positioning, or when the unmanned platform is moving and enters the effective area of ​​laser radar positioning, the GPS positioning result is used as the initial registration value Q1, and 3D point cloud registration is used for laser radar positioning, and the laser radar positioning information is released;

[0010] 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 sliding window method to obtain the initial registration value Q2, and the lidar positioning is performed using 3D point cloud registration, and the lidar positioning information is released.

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

[0012] Using data from the IMU, lidar, and GPS receiver on the unmanned platform, and adopting LIO-SAM technology with improved GPS as a mandatory factor, we can complete the construction of a point cloud map with global positioning.

[0013] Preferably, in step 1, the determination of the effective area of ​​laser radar positioning is: using 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 the selected convex polygons, and using unscented Kalman filtering to process the GPS positioning and orientation information based on the vehicle kinematic model, and recording the trajectories before and after filtering; using the Fréchet distance to evaluate the similarity of the two trajectories obtained 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 ​​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 triangles and the area of ​​the convex polygon. If the areas are equal, the unmanned platform enters 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; use the initial value Q1 to perform lidar positioning according to the point cloud data, check the Euclidean distance between the lidar positioning and the GPS positioning, if it is greater than the error threshold, the matching result is considered unreliable and re-matching is performed; if it is less than the error threshold, the match is considered successful, enter the lidar matching and positioning stage, and release the lidar positioning information.

[0020] Preferably, in step 2, if the unmanned platform is started in a GPS-free area, the point cloud after feature extraction is initially aligned with the global point cloud map by sliding a window, and the initial alignment 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 result 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;

[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 coordinates in the coordinate system, and this is used as the registration initial value Q2.

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

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

[0026] The reliability check is as follows: storing the most recent positioning results into a cache queue, calculating the distance between two adjacent frames of positioning in the cache queue in real time, and determining whether it is greater than a 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: performing time optimization during the iteration process, specifically: obtaining the positioning time difference based on the timestamps of two adjacent positioning results, dynamically adjusting the voxel grid filter parameters during point cloud registration according to the gradient of multiple data time differences, if the matching time shows an increasing trend, then increasing the grid filter parameters to reduce the number of point clouds involved in the registration to reduce the time pressure; if the matching time shows a decreasing trend, then reducing the grid filter parameters 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 comprises: determining whether the unmanned platform has left the effective area of ​​the laser radar positioning; if so, re-determining the positioning mode according to the presence or absence of a GPS signal and whether it is within the effective area of ​​the laser radar positioning.

[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 prior art. Since the switching area construction takes into account the middle area with GPS signals, the GPS signal can be used as the initial registration value without destroying the continuity of positioning. The initial registration value is accurate, and while ensuring continuity, it can also ensure the accuracy of subsequent laser radars.

[0032] (2) In the case where 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 for starting in an area without GPS information, thereby improving 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, and constructs a high-precision point cloud map by using the LIO-SAM method. This method can combine the information of sensors such as IMU, 3D laser radar, GPS, etc., and use the pre-integration of IMU to complete the compensation of laser radar drift to achieve the construction of a high-precision point cloud map;

[0035] From the perspective of mapping, the intersection of LIO-SAM and traditional methods can provide more stable mapping results when dealing with the relative lack of geometric features; from the perspective of positioning, the combined positioning method can expand the scope of use of unmanned platforms, making them not limited to a single indoor or outdoor scene. Therefore, it has a wider range of uses.

[0036] (5) In a preferred embodiment, if the unmanned platform is started in a GPS-free area, ISS+FPFH is used for feature extraction, SAC-IA algorithm is used for initial registration, and 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 ISS algorithm for accurate identification of key points and FPFH for high descriptiveness and low time complexity of features, and the SAC-IA method uses 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 a reliability judgment is further performed to ensure the accuracy of the positioning after the switch.

[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 laser radar continuous positioning method that provides accurate point cloud registration initial values, which can be applied to any unmanned platform. This embodiment is described by taking an unmanned vehicle as an example.

[0045] The basic idea of ​​the present invention is to first utilize the IMU, multi-line laser radar, 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 laser radar positioning effective area that is slightly larger than the GPS positioning unreliable or invalid area. The area curve of the laser radar 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 according to whether there is a GPS signal and whether it is in the effective area of ​​the laser radar 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 ​​LiDAR positioning, the GPS positioning information will be released;

[0048] ② If the unmanned vehicle is started in an area where the GPS signal is reliable and is located in the effective area of ​​the LiDAR positioning, 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;

[0049] When the unmanned vehicle enters the effective area of ​​the laser radar positioning during its movement, the GPS positioning result is 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 sliding window method to obtain the initial registration value Q2, and the 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 starting, 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] It can be seen that the present invention is based on the characteristics of the laser radar positioning method that relies on relatively accurate initial positioning values. Regardless of the position where the drone is started, a relatively accurate initial registration value can be obtained. When the unmanned vehicle enters the switching area during travel, it does not need to wait at the switching location like the prior art. Since the construction of the switching area takes into account the middle area with GPS signals, the GPS signal can be used as the initial registration value, which will not destroy the continuity of positioning. The initial registration value is accurate, and while ensuring continuity, it can also ensure the accuracy of subsequent laser radars.

[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, and the steps of the method include:

[0054] Step 1: Use the sensors on the unmanned platform to map the areas where GPS positioning is unreliable or invalid, 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 inside 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 invalid.

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

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

[0058] Before constructing the point cloud map, first complete the calibration of the IMU and lidar 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] Among them, R is a 3rd-order square matrix, which describes the rotation of IMU relative to the lidar in the x, y, and z directions. t is a 3×1 matrix, which describes the translation relationship of 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 laser radar is calibrated relative to the GPS receiver, which can be represented by the posture transformation matrix T', and the subsequent laser radar 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, LIO-SAM (Tightly-coupled Lidar InertialOdometry via Smoothing and Mapping) technology is adopted with improved GPS factor as a mandatory factor. As a tightly coupled lidar inertial odometer method, it completes the construction of a map with global positioning information through sensors such as lidar, IMU, and GPS receiver, including point cloud data dedistortion, feature extraction, IMU pre-integration, back-end factor graph optimization and other modules.

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

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

[0065] In this step, in order to ensure the adaptability and diversity of the positioning area coverage, according to the point cloud map constructed by LIO-SAM technology, multiple different convex polygons with an area slightly larger than the point cloud map are selected to cover the mapping area. When selecting polygons, each vertex is selected outside the outline of the point cloud map, and the location is selected to avoid areas without GPS signals.

[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, the vehicle kinematic model is established. The state quantities 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 transfer 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. The position information and heading angle information are selected as the observation quantity.

[0075]

[0076] The observation equation is as follows

[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, 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 Fréchet distance to evaluate the similarity of 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 from the selected multiple different convex polygons as the optimal division area, that is, the effective area of ​​the laser radar 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 curve among all possible isometric mappings. The formula of 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 keeps the distance unchanged. d(f(c1), c2) represents the distance between point c1 on curve C1 mapped to point f(c1) on curve C2 by isometric mapping f and point c2 on curve C2.

[0084] The division of the laser radar 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 smallest signal fluctuations and mutations on this curve, which can ensure the accuracy of the GPS information received when the unmanned vehicle enters the switching area to a certain extent.

[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 laser radar positioning.

[0086] In this step, the method of determining whether there is a GPS signal is to check 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 can be received and the vehicle is in a reliable GPS signal area. If there is no data in the GPS cache queue or the GPS data validity check fails, the vehicle is considered to be in a GPS-free area.

[0087] The method for determining whether the unmanned vehicle is located in the effective area of ​​the laser radar positioning is as follows: connect the position of the unmanned platform with each vertex of the convex polygon of the effective area of ​​the laser radar positioning in sequence to form several triangles; compare the sum of the areas of all triangles and the area of ​​the convex polygon. If the areas are equal, the unmanned platform is located in or has entered the effective area of ​​the laser radar positioning. Compared with the line cross product judgment method, this method has better versatility when dealing with floating-point numbers such as GPS data.

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

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

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

[0091] Step 3: The unmanned vehicle starts in an area with reliable GPS signals and is outside the effective area of ​​the laser radar positioning. At this time, it means 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, according to the judgment result of step 2, if it is not in the effective area of ​​laser radar positioning, the converted GPS UTM coordinates in the MAP map are output, and the data queue related to laser radar positioning is cleared. Return to step 2 to continue to judge and output positioning information.

[0093] Step 4: The unmanned vehicle is started in an area with reliable GPS signals and is located in the effective area of ​​the laser radar positioning, or when the unmanned platform is moving and enters the effective area of ​​the laser radar positioning, the laser radar 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 precise positioning and proceed to step 6.

[0094] In this step, the GPS receiver and the laser radar receive data at the same time, read data from the GPS cache queue and the point cloud data cache queue, and complete data time synchronization according to the timestamp; use the initial registration value Q1 to perform the first match according to the initial pose in the point cloud cache queue; check the Euclidean distance between the laser radar matching positioning result and the GPS positioning. If this distance is greater than the error threshold, the matching result is considered inaccurate and re-matched; if the error is less than the threshold, the match is considered successful, enter the laser radar matching positioning stage, and enter step 6. If multiple matches are unsuccessful, give priority to calibrating the relative position of the radar and GPS or try to change the position of entering the switching area for matching.

[0095] Step 5: The unmanned vehicle starts in an area without GPS. 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 alignment, provide an accurate alignment initial value Q2, and perform 3D point cloud alignment based on the alignment initial value Q2 to obtain precise positioning and enter step 6.

[0096] In this step, in the area without GPS, the method to obtain the initial value of point cloud registration is:

[0097] Step S21: Using a sliding window in 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, and obtain the coordinates in the coordinate system through coordinate transformation according to the position relationship of the corresponding local map in the global map, and use this as the initial value Q2 for precise registration. Then proceed to step six.

[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 previous positioning information, and the previous positioning information is used as the initial value of the next point cloud alignment. The positioning information is continuously acquired in an incremental form, and the reliability check of the positioning information is performed. If the reliability check fails, it is necessary to re-align and return to step two to re-determine the positioning method based on the presence or absence of GPS signals and whether it is within the effective area of ​​the laser radar positioning; if the reliability check passes, the laser radar positioning information is released in real time.

[0102] Among them, the reliability check can be: storing the latest 20 positioning results into a cache queue, calculating the distance between two adjacent frame positioning in the cache queue in real time, and dynamically obtaining the distance threshold according to 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 optimization is performed 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 multiple data time differences. 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 time are weighed.

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

[0105] The positioning effect is verified as follows.

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

[0107] Whether it is in the positioning switching area is determined by the area method, that is, 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 than the cross multiplication method when dealing with floating-point numbers.

[0108] After the boundary crosses the switching line, GPS is used to provide accurate initial registration values, and this initial registration value is brought into the 3D-NDT registration to generate accurate positioning information. Subsequent positioning information is iteratively updated on this basis, and the change in positioning source can be intuitively seen from the visual interface.

[0109] Start the unmanned vehicle in a GPS-free area. At this time, ISS+FPFH (incremental shape and feature histogram) is used to extract point cloud features, and different local maps are obtained by sliding the global map window. The SAC-IA method is used to perform preliminary registration with the current scanned point cloud. The successful registration result is further 3D-NDT registered, and the registration results and scores are continuously stored in the cache queue. The cache queue is implemented in a priority queue with the score as the sorting basis, which can quickly extract the preliminary registration result with the highest score. After obtaining the highest scoring initial registration value, the subsequent positioning information is iteratively updated on this basis. After obtaining stable positioning information, a blue positioning appears in the visualization interface.

[0110] The above specific embodiments only describe the design principle of the present invention. The shapes and names of the components in the description may be different and are not limited. Therefore, those skilled in the art in the field of the present invention may modify or replace the technical solutions recorded in the above embodiments; and these modifications and replacements do not deviate from the creative purpose and technical solutions of the present invention and should all fall within the protection scope of the present invention.

Claims

1. A method for continuous positioning of GPS and laser radar that provides accurate initial value of point cloud registration, characterized in that: include: Step 1: Use the sensors on the unmanned platform to build a map for areas where GPS positioning is unreliable or invalid, obtain a point cloud map with global positioning, and determine the effective area of ​​the laser radar positioning. The area on the curve of the effective area of ​​the laser radar positioning and the area inside the curve close to the curve can receive GPS signals; 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 laser radar positioning, including: If the unmanned platform is started in an area with reliable GPS signals and is outside the effective area of ​​laser radar positioning, the GPS positioning information will be released; If the unmanned platform is started in an area with reliable GPS signals and is located in the effective area of ​​laser radar positioning, or when the unmanned platform is moving and enters the effective area of ​​laser radar positioning, the GPS positioning result is used as the initial registration value Q1, and 3D point cloud registration is used for laser radar positioning, and the laser radar 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 sliding window method to obtain the initial registration value Q2, and the lidar positioning is performed using 3D point cloud registration, and the lidar positioning information is released.

2. The method according to claim 1, characterized in that In step 1, the point cloud map with global positioning is obtained by using the sensor carried by the unmanned platform: Using data from the IMU, lidar, and GPS receiver on the unmanned platform, and adopting LIO-SAM technology with improved GPS as a mandatory factor, we can complete the construction of a point cloud map with global positioning.

3. The method according to claim 1 or 2, characterized in that In step 1, the determination of the effective area of ​​laser radar positioning is as follows: using laser radar to obtain a point cloud map of the area where GPS positioning is unreliable or invalid, and 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 the selected convex polygons, and using unscented Kalman filtering to process the GPS positioning and orientation information based on the vehicle kinematic model, 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 ​​laser radar positioning.

4. The method according to claim 1, characterized in that In step 2, the method for determining whether the unmanned platform is located in or enters the effective area of ​​the laser radar 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 triangles and the area of ​​the convex polygon. If the areas are equal, the unmanned platform enters the laser radar positioning effective area.

5. The method according to claim 1, characterized in that If the unmanned platform is started in a reliable GPS signal area and is outside the effective area of ​​the laser radar 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.

6. The method according to claim 1, characterized in that The GPS positioning result is used as the initial registration value Q1, and 3D point cloud registration is used for laser radar positioning. The laser radar positioning information is released as follows: 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; use the initial value Q1 to perform lidar positioning according to the point cloud data, check the Euclidean distance between the lidar positioning and the GPS positioning, if it is greater than the error threshold, the matching result is considered unreliable and re-matching is performed; if it is less than the error threshold, the match is considered successful, enter the lidar matching and positioning stage, and release the lidar positioning information.

7. The method according to claim 1, characterized in that In step 2, if the unmanned platform is started in a GPS-free area, the point cloud after feature extraction is used to perform preliminary registration with the global point cloud map sliding window method, and the initial registration value Q2 is obtained as: 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 result 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; 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 coordinates in the coordinate system, and this is used as the registration initial value Q2.

8. The method according to claim 1, characterized in that The laser radar positioning using 3D point cloud registration includes: The last positioning information is used as the initial value for the next point cloud registration. The positioning information is continuously acquired in an incremental form, and the reliability check of the positioning information is performed. If the reliability check is passed, the lidar positioning information is released; if it fails, the positioning method is re-determined based on the presence or absence of GPS signals and whether it is within the effective area of ​​the lidar positioning. The reliability check is as follows: storing the most recent positioning results into a cache queue, calculating the distance between two adjacent frames of positioning in the cache queue in real time, and determining whether it is greater than a 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.

9. The method according to claim 1, characterized in that 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, dynamically adjusting the voxel grid filter parameters during point cloud registration according to the gradient of multiple data time differences, if the matching time consumption shows an increasing trend, then increasing the grid filter parameters 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, then reducing the grid filter parameters to retain more environmental features to improve the accuracy of the registration; in this way, the real-time and accuracy of the positioning release time are weighed.

10. The method according to claim 1, characterized in that The method further includes: determining whether the unmanned platform has left the laser radar positioning effective area, and 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